[rotorcraft] simple HITL by using ref as gps

conf/firmwares/subsystems/rotorcraft/gps_sim_hitl.makefile

@@ -0,0 +1,15 @@
+# Hey Emacs, this is a -*- makefile -*-
+
+ap.srcs += $(SRC_SUBSYSTEMS)/gps.c
+ap.CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_sim_hitl.h\"
+ap.srcs += $(SRC_SUBSYSTEMS)/gps/gps_sim_hitl.c
+ap.CFLAGS += -DUSE_GPS -DHITL
+
+ifneq ($(GPS_LED),none)
+  ap.CFLAGS += -DGPS_LED=$(GPS_LED)
+endif
+
+nps.CFLAGS += -DUSE_GPS
+nps.srcs += $(SRC_SUBSYSTEMS)/gps.c
+nps.CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_sim_nps.h\"
+nps.srcs += $(SRC_SUBSYSTEMS)/gps/gps_sim_nps.c

sw/airborne/subsystems/actuators/motor_mixing.c

@@ -158,7 +158,11 @@ void motor_mixing_run_spinup(uint32_t counter, uint32_t max_counter)
 
 void motor_mixing_run(bool_t motors_on, bool_t override_on, pprz_t in_cmd[] ) {
   uint8_t i;
+#if !HITL
   if (motors_on) {
+#else
+  if (FALSE) {
+#endif
     int32_t min_cmd = INT32_MAX;
     int32_t max_cmd = INT32_MIN;
     /* do the mixing in float to avoid overflows, implicitly casted back to int32_t */

sw/airborne/subsystems/gps/gps_sim_hitl.c

@@ -0,0 +1,35 @@
+/*
+ * Copyright (C) 2014 Sergey Krukowski <softsr@yahoo.de>
+ *
+ * This file is part of paparazzi.
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING.  If not, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ */
+
+/**
+ * @file subsystems/gps/gps_sim_hitl.c
+ * GPS subsystem simulation from rotorcrafts horizontal/vertical reference system
+ */
+
+#include "subsystems/gps.h"
+
+bool_t gps_available;
+uint32_t gps_sim_hitl_timer;
+
+void gps_impl_init(void) {
+  gps.fix = GPS_FIX_NONE;
+  gps_available = FALSE;
+}

sw/airborne/subsystems/gps/gps_sim_hitl.h

@@ -0,0 +1,82 @@
+/*
+ * Copyright (C) 2014 Sergey Krukowski <softsr@yahoo.de>
+ *
+ * This file is part of paparazzi.
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING.  If not, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ */
+
+/**
+ * @file subsystems/gps/gps_sim_hitl.h
+ * GPS subsystem simulation from rotorcrafts horizontal/vertical reference system
+ */
+
+#ifndef GPS_SIM_HITL_H
+#define GPS_SIM_HITL_H
+
+#include "std.h"
+#include "subsystems/ins.h"
+#include "guidance/guidance_h.h"
+#include "guidance/guidance_v.h"
+
+extern bool_t gps_available;
+extern uint32_t gps_sim_hitl_timer;
+
+#define GpsEvent(_sol_available_callback) {                             \
+    if (SysTimeTimer(gps_sim_hitl_timer) > 100000) {                    \
+      SysTimeTimerStart(gps_sim_hitl_timer);                            \
+      gps.last_msg_ticks = sys_time.nb_sec_rem;                         \
+      gps.last_msg_time = sys_time.nb_sec;                              \
+      if (ins_impl.ltp_initialized) {                                   \
+        struct NedCoor_i ned_coor;                                      \
+        struct NedCoor_i ned_vel;                                       \
+        if (!autopilot_in_flight) {                                     \
+          struct Int32Vect2 zero_vector;                                \
+          INT_VECT2_ZERO(zero_vector);                                  \
+          gh_set_ref(zero_vector, zero_vector, zero_vector);            \
+          INT_VECT2_ZERO(guidance_h_pos_ref);                           \
+          INT_VECT2_ZERO(guidance_h_speed_ref);                         \
+          INT_VECT2_ZERO(guidance_h_accel_ref);                         \
+          gv_set_ref(0, 0, 0);                                          \
+          guidance_v_z_ref = 0;                                         \
+          guidance_v_zd_ref = 0;                                        \
+          guidance_v_zdd_ref = 0;                                       \
+        }                                                               \
+        VECT3_ASSIGN(ned_coor, guidance_h_pos_ref.x * INT32_POS_OF_CM_DEN / INT32_POS_OF_CM_NUM, \
+                     guidance_h_pos_ref.y * INT32_POS_OF_CM_DEN / INT32_POS_OF_CM_NUM, \
+                     guidance_v_z_ref * INT32_POS_OF_CM_DEN / INT32_POS_OF_CM_NUM); \
+        VECT3_ASSIGN(ned_vel, guidance_h_speed_ref.x * INT32_SPEED_OF_CM_S_DEN / INT32_SPEED_OF_CM_S_NUM, \
+                     guidance_h_speed_ref.y * INT32_SPEED_OF_CM_S_DEN / INT32_SPEED_OF_CM_S_NUM, \
+                     guidance_v_zd_ref * INT32_SPEED_OF_CM_S_DEN / INT32_SPEED_OF_CM_S_NUM); \
+        ecef_of_ned_point_i(&gps.ecef_pos, &ins_impl.ltp_def, &ned_coor); \
+        ecef_of_ned_vect_i(&gps.ecef_vel, &ins_impl.ltp_def, &ned_vel); \
+        gps.fix = GPS_FIX_3D;                                           \
+        gps.last_3dfix_ticks = sys_time.nb_sec_rem;                     \
+        gps.last_3dfix_time = sys_time.nb_sec;                          \
+        gps_available = TRUE;                                           \
+      }                                                                 \
+      else {                                                            \
+        struct Int32Vect2 zero_vector;                                  \
+        INT_VECT2_ZERO(zero_vector);                                    \
+        gh_set_ref(zero_vector, zero_vector, zero_vector);              \
+        gv_set_ref(0, 0, 0);                                            \
+      }                                                                 \
+      _sol_available_callback();                                        \
+      gps_available = FALSE;                                            \
+    }                                                                   \
+  }
+
+#endif /* GPS_SIM_HITL_H */