[fixedwing] mandatory ins subsystem

* added ins_gps_passthrough which just uses the gps measurements and passes them along to the state interface * fix ins_xsens subsystem
paparazzi · Sep 19, 2012 · 0072a01 · 0072a01
1 parent e58c77f
commit 0072a01
Show file tree

Hide file tree

Showing 18 changed files with 140 additions and 53 deletions.
diff --git a/conf/airframes/ENAC/fixed-wing/weasel.xml b/conf/airframes/ENAC/fixed-wing/weasel.xml
@@ -37,7 +37,7 @@
     <subsystem name="ahrs"    type="float_dcm">
       <define name="USE_HIGH_ACCEL_FLAG"/>
     </subsystem>
-    <subsystem name="ins" typee="alt_float"/>
+    <subsystem name="ins" type="alt_float"/>
     <subsystem name="control" type="new"/>
     <subsystem name="navigation"/>
     <!-- Sensors -->

diff --git a/conf/airframes/examples/delta_wing_minimal.xml b/conf/airframes/examples/delta_wing_minimal.xml
@@ -20,6 +20,7 @@
     <subsystem name="ahrs" 		type="infrared"/>
     <subsystem name="gps" 		    type="ublox"/>
     <subsystem name="navigation"/>
+    <subsystem name="ins" type="gps_passthrough"/>
   </firmware>
 
   <modules>

diff --git a/conf/airframes/examples/funjet.xml b/conf/airframes/examples/funjet.xml
@@ -30,6 +30,7 @@
     <subsystem name="gps" 		    type="ublox"/>
 
     <subsystem name="navigation"/>
+    <subsystem name="ins" type="gps_passthrough"/>
   </firmware>
 
   <modules>

diff --git a/conf/airframes/examples/funjet_cam.xml b/conf/airframes/examples/funjet_cam.xml
@@ -26,10 +26,10 @@
     <!-- Actuators are automatically chosen according to board-->
     <subsystem name="control"/>
     <!-- Sensors -->
-    <subsystem name="ahrs" 		type="infrared"/>
-    <subsystem name="gps" 		    type="ublox"/>
-
-    <subsystem name="navigation"    type="extra"/>
+    <subsystem name="ahrs" 		  type="infrared"/>
+    <subsystem name="gps" 		  type="ublox"/>
+    <subsystem name="ins"         type="gps_passthrough"/>
+    <subsystem name="navigation"  type="extra"/>
   </firmware>
 
 

diff --git a/conf/airframes/examples/microjet_lisa_m_xsens.xml b/conf/airframes/examples/microjet_lisa_m_xsens.xml
@@ -28,7 +28,6 @@
 
     <subsystem name="control"/>
     <subsystem name="navigation"/>
-    <subsystem name="ins" type="alt_float"/>
   </firmware>
 
   <modules>

diff --git a/conf/airframes/examples/twinjet.xml b/conf/airframes/examples/twinjet.xml
@@ -26,6 +26,7 @@
     </subsystem>
     <subsystem name="gps"           type="ublox"/>
     <subsystem name="navigation"/>
+    <subsystem name="ins" type="gps_passthrough"/>
   </firmware>
 
   <firmware name="setup">

diff --git a/conf/airframes/examples/twinstar.xml b/conf/airframes/examples/twinstar.xml
@@ -21,8 +21,9 @@
     <subsystem name="radio_control" type="ppm"/>
     <subsystem name="telemetry"     type="transparent"/>
     <subsystem name="control"/>
-    <subsystem name="ahrs"      type="infrared"/>
+    <subsystem name="ahrs"          type="infrared"/>
     <subsystem name="gps"           type="ublox_utm"/>
+    <subsystem name="ins"           type="gps_passthrough"/>
     <subsystem name="navigation"/>
   </firmware>
 

diff --git a/conf/airframes/twinjet_overo.xml b/conf/airframes/twinjet_overo.xml
@@ -27,6 +27,7 @@
     <subsystem name="gps"           type="ublox_hitl"/>
     <subsystem name="navigation"/>
     <subsystem name="joystick"/>
+    <subsystem name="ins"           type="gps_passthrough"/>
   </firmware>
 
   <firmware name="setup">

diff --git a/conf/conf.xml.example b/conf/conf.xml.example
@@ -39,7 +39,7 @@
    ac_id="24"
    airframe="airframes/examples/bixler_lisa_m_2.xml"
    radio="radios/cockpitSX.xml"
-   telemetry="telemetry/default_fixedwing.xml"
+   telemetry="telemetry/default_fixedwing_imu.xml"
    flight_plan="flight_plans/versatile.xml"
    settings=" settings/fixedwing_basic.xml settings/control/ctl_basic.xml settings/estimation/ins_neutrals.xml"
    gui_color="blue"

diff --git a/conf/firmwares/subsystems/fixedwing/ins_alt_float.makefile b/conf/firmwares/subsystems/fixedwing/ins_alt_float.makefile
@@ -1,6 +1,5 @@
 # Hey Emacs, this is a -*- makefile -*-
 
-ap_CFLAGS += -DUSE_INS
 ap_CFLAGS += -DINS_TYPE_H=\"subsystems/ins/ins_alt_float.h\"
 ap_srcs   += $(SRC_FIXEDWING)/subsystems/ins.c
 ap_srcs   += $(SRC_FIXEDWING)/subsystems/ins/ins_alt_float.c
diff --git a/conf/firmwares/subsystems/fixedwing/ins_gps_passthrough.makefile b/conf/firmwares/subsystems/fixedwing/ins_gps_passthrough.makefile
@@ -0,0 +1,4 @@
+
+ap.srcs   += $(SRC_SUBSYSTEMS)/ins/ins_gps_passthrough.c
+
+sim.srcs  += $(SRC_SUBSYSTEMS)/ins/ins_gps_passthrough.c
diff --git a/conf/firmwares/subsystems/fixedwing/ins_xsens.makefile b/conf/firmwares/subsystems/fixedwing/ins_xsens.makefile
@@ -12,9 +12,7 @@
 #########################################
 ## ATTITUDE
 
-ifeq ($(TARGET), ap)
-
-ap.CFLAGS += -DUSE_INS
+ap.CFLAGS += -DUSE_INS_MODULE
 
 # AHRS Results
 ap.CFLAGS += -DINS_TYPE_H=\"modules/ins/ins_xsens.h\"
@@ -34,7 +32,6 @@ ap.srcs   += $(SRC_MODULES)/ins/ins_xsens.c
 ap.CFLAGS += -DAHRS_TRIGGERED_ATTITUDE_LOOP
 
 
-endif
 
 ifeq ($(TARGET), fbw)
 
@@ -45,16 +42,6 @@ fbw.CFLAGS += -DAHRS_TYPE_H=\"modules/ins/ins_xsens.h\"
 endif
 
 
-ifeq ($(TARGET), sim)
-
-sim.CFLAGS += -DAHRS_TYPE_H=\"subsystems/ahrs/ahrs_sim.h\"
-sim.CFLAGS += -DUSE_AHRS -DAHRS_UPDATE_FW_ESTIMATOR
-
-sim.srcs   += $(SRC_SUBSYSTEMS)/ahrs.c
-sim.srcs   += $(SRC_SUBSYSTEMS)/ahrs/ahrs_sim.c
-
-endif
-
 #########################################
 ## GPS
 
@@ -65,6 +52,19 @@ ap.CFLAGS += -DUSE_GPS -DGPS_USE_LATLONG
 ap.CFLAGS += -DGPS_TYPE_H=\"modules/ins/ins_xsens.h\"
 ap.srcs += $(SRC_SUBSYSTEMS)/gps.c
 
+
+#########################################
+## Simulator
+
+sim.CFLAGS += -DAHRS_TYPE_H=\"subsystems/ahrs/ahrs_sim.h\"
+sim.CFLAGS += -DUSE_AHRS -DAHRS_UPDATE_FW_ESTIMATOR
+
+sim.srcs   += $(SRC_SUBSYSTEMS)/ahrs.c
+sim.srcs   += $(SRC_SUBSYSTEMS)/ahrs/ahrs_sim.c
+
+sim.CFLAGS += -DUSE_INS_MODULE -DINS_TYPE_H=\"subsystems/ins/ins_gps_only.h\"
+sim.srcs   += $(SRC_SUBSYSTEMS)/ins/ins_gps_only.c
+
 sim.CFLAGS += -DUSE_GPS -DGPS_USE_LATLONG
 sim.CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_sim.h\"
 sim.srcs += $(SRC_SUBSYSTEMS)/gps/gps_sim.c

diff --git a/conf/tests_conf.xml b/conf/tests_conf.xml
@@ -39,7 +39,7 @@
    ac_id="24"
    airframe="airframes/examples/bixler_lisa_m_2.xml"
    radio="radios/cockpitSX.xml"
-   telemetry="telemetry/default_fixedwing.xml"
+   telemetry="telemetry/default_fixedwing_imu.xml"
    flight_plan="flight_plans/versatile.xml"
    settings=" settings/fixedwing_basic.xml settings/control/ctl_basic.xml settings/estimation/ins_neutrals.xml"
    gui_color="blue"

diff --git a/sw/airborne/firmwares/fixedwing/main_ap.c b/sw/airborne/firmwares/fixedwing/main_ap.c
@@ -56,9 +56,8 @@
 #if USE_BAROMETER
 #include "subsystems/sensors/baro.h"
 #endif
-#if USE_INS
 #include "subsystems/ins.h"
-#endif
+
 
 // autopilot & control
 #include "state.h"
@@ -184,9 +183,7 @@ void init_ap( void ) {
   baro_init();
 #endif
 
-#if USE_INS
   ins_init();
-#endif
 
   stateInit();
 
@@ -566,9 +563,7 @@ void sensors_task( void ) {
   baro_periodic();
 #endif
 
-#if USE_INS
   ins_periodic();
-#endif
 }
 
 
@@ -670,9 +665,7 @@ void event_task_ap( void ) {
 
 #if USE_GPS
 static inline void on_gps_solution( void ) {
-#if USE_INS
   ins_update_gps();
-#endif
 #if USE_AHRS
   ahrs_update_gps();
 #endif
@@ -774,9 +767,7 @@ static inline void on_mag_event(void)
 #if USE_BAROMETER
 
 static inline void on_baro_abs_event( void ) {
-#if USE_INS
   ins_update_baro();
-#endif
 }
 
 static inline void on_baro_dif_event( void ) {

diff --git a/sw/airborne/modules/ins/ins_module.h b/sw/airborne/modules/ins/ins_module.h
@@ -59,7 +59,7 @@ extern INS_FORMAT ins_mx;
 extern INS_FORMAT ins_my;
 extern INS_FORMAT ins_mz;
 
-#if USE_INS
+#if USE_INS_MODULE
 extern INS_FORMAT ins_roll_neutral;
 extern INS_FORMAT ins_pitch_neutral;
 #endif

diff --git a/sw/airborne/modules/ins/ins_xsens.c b/sw/airborne/modules/ins/ins_xsens.c
@@ -20,7 +20,7 @@
  *
  */
 
-/** @file xsens.c
+/** @file ins_xsens.c
  * Parser for the Xsens protocol.
  */
 
@@ -33,7 +33,6 @@
 #include "generated/airframe.h"
 
 #include "mcu_periph/sys_time.h"
-#include "subsystems/datalink/downlink.h"
 #include "messages.h"
 
 #if USE_GPS_XSENS
@@ -71,7 +70,7 @@ INS_FORMAT ins_mx;
 INS_FORMAT ins_my;
 INS_FORMAT ins_mz;
 
-#if USE_INS
+#if USE_INS_MODULE
 float ins_pitch_neutral;
 float ins_roll_neutral;
 #endif
@@ -207,12 +206,15 @@ uint8_t send_ck;
 
 volatile int xsens_configured = 0;
 
-void ins_init( void ) {
+void xsens_init(void);
+void xsens_periodic(void);
+
+void xsens_init(void) {
 
   xsens_status = UNINIT;
   xsens_configured = 20;
 
-#if USE_INS
+#if USE_INS_MODULE
   ins_pitch_neutral = INS_PITCH_NEUTRAL_DEFAULT;
   ins_roll_neutral = INS_ROLL_NEUTRAL_DEFAULT;
 #endif
@@ -227,25 +229,38 @@ void ins_init( void ) {
 struct ImuXsens imu_xsens;
 
 void imu_impl_init(void) {
-  ins_init();
+  xsens_init();
   imu_xsens.gyro_available = FALSE;
   imu_xsens.accel_available = FALSE;
   imu_xsens.mag_available = FALSE;
 }
 
 void imu_periodic(void) {
-  ins_periodic();
+  xsens_periodic();
 }
 #endif /* USE_IMU */
 
+#if USE_INS_MODULE
+void ins_init(void) {
+  xsens_init();
+}
+
+void ins_periodic(void) {
+  xsens_periodic();
+}
+
+void ins_update_gps(void) {
+}
+#endif
+
 #if USE_GPS_XSENS
 void gps_impl_init(void) {
   gps.nb_channels = 0;
   gps_xsens_msg_available = FALSE;
 }
 #endif
 
-void ins_periodic( void ) {
+void xsens_periodic(void) {
   if (xsens_configured > 0)
     {
       switch (xsens_configured)
@@ -306,11 +321,7 @@ void ins_periodic( void ) {
   RunOnceEvery(100,XSENS_ReqGPSStatus());
 }
 
-void ins_update_gps(void) {
-
-}
-
-#if USE_INS
+#if USE_INS_MODULE
 #include "state.h"
 
 static inline void update_fw_estimator(void) {
@@ -341,11 +352,11 @@ static inline void update_fw_estimator(void) {
   stateSetNedToBodyEulers_f(&att);
   stateSetBodyRates_f(&rates);
 }
-#endif /* USE_INS */
+#endif /* USE_INS_MODULE */
 
 void handle_ins_msg(void) {
 
-#if USE_INS
+#if USE_INS_MODULE
   update_fw_estimator();
 #endif
 
@@ -388,7 +399,7 @@ void handle_ins_msg(void) {
 #endif // USE_GPS_XSENS
 }
 
-void parse_ins_msg( void ) {
+void parse_ins_msg(void) {
   uint8_t offset = 0;
   if (xsens_id == XSENS_ReqOutputModeAck_ID) {
     xsens_output_mode = XSENS_ReqOutputModeAck_mode(xsens_msg_buf);
@@ -633,7 +644,7 @@ void parse_ins_msg( void ) {
 }
 
 
-void parse_ins_buffer( uint8_t c ) {
+void parse_ins_buffer(uint8_t c) {
   ck += c;
   switch (xsens_status) {
   case UNINIT:

diff --git a/sw/airborne/modules/ins/ins_xsens.h b/sw/airborne/modules/ins/ins_xsens.h
@@ -80,7 +80,7 @@ extern struct ImuXsens imu_xsens;
 
 
 /* use Xsens as a full INS solution */
-#if USE_INS
+#if USE_INS_MODULE
 #define InsEvent(_ins_handler) {	\
   InsEventCheckAndHandle(handle_ins_msg()) 			\
 }