[fbw_gps] GPS in FBW/FTD (e.g. geofencing) (#1876)

* [FBW_GPS] Run GPS driver in FBW, e.g. for geofencing

* [FBW_GPS] convert all GPS modules to run in FBW

conf/airframes/examples/separate_fbw_ap.xml

@@ -202,7 +202,6 @@
 
 
   <modules>
-    <module name="gps" type="ubx_ucenter"/>
 <!--
     <module name="adc_generic">
       <configure name="ADC_CHANNEL_GENERIC1" value="ADC_1" />
@@ -248,6 +247,12 @@
         <configure name="INTERMCU_PORT" value="UART5"/>
       </module>
 
+      <module name="gps" type="ublox">
+        <configure name="GPS_PORT" value="UART1"/>
+      </module>
+      <module name="gps" type="ubx_ucenter"/>
+
+
       <module name="control"/>
       <module name="navigation"/>
     </target>
@@ -272,6 +277,11 @@
     <target name="sim" board="pc">
       <module name="control"/>
       <module name="navigation"/>
+
+      <module name="gps" type="ublox">
+        <configure name="GPS_PORT" value="UART1"/>
+      </module>
+
     </target>
 
     <configure name="FLASH_MODE" value="JTAG"/>
@@ -296,10 +306,6 @@
 
     <!-- Sensors -->
     <module name="imu" type="aspirin_v2.1"/>
-    <module name="gps" type="ublox">
-      <configure name="GPS_PORT" value="UART1"/>
-    </module>
-
     <module name="ahrs" type="float_dcm"/>
     <module name="ins" type="alt_float"/>
 

conf/firmwares/subsystems/rotorcraft/intermcu_uart.makefile

@@ -27,6 +27,12 @@ ifneq ($(RADIO_CONTROL_LED),none)
     $(TARGET).CFLAGS += -DRADIO_CONTROL_LED=$(RADIO_CONTROL_LED)
 endif
 
+ifneq (,$(findstring $(SECONDARY_GPS), imcu))
+	# this is the secondary GPS
+	$(TARGET).CFLAGS += -DGPS_SECONDARY_TYPE_H=\"subsystems/intermcu/intermcu_ap.h\"
+	$(TARGET).CFLAGS += -DSECONDARY_GPS=GPS_IMCU -DIMCU_GPS
+endif
+
   ap.srcs += subsystems/intermcu/intermcu_ap.c
   ap.srcs += pprzlink/src/pprz_transport.c
   $(TARGET).srcs += subsystems/radio_control.c

conf/modules/gps_datalink.xml

@@ -17,23 +17,26 @@
   <periodic fun="gps_datalink_periodic_check()" freq="1." autorun="TRUE"/>
   <datalink message="REMOTE_GPS" fun="gps_datalink_parse_REMOTE_GPS()"/>
   <datalink message="REMOTE_GPS_SMALL" fun="gps_datalink_parse_REMOTE_GPS_SMALL()"/>
-  <makefile target="ap">
+  <makefile target="ap|fbw">
     <file name="gps_datalink.c" dir="subsystems/gps"/>
     <raw>
       ifdef SECONDARY_GPS
       ifneq (,$(findstring $(SECONDARY_GPS), datalink))
       # this is the secondary GPS
-      ap.CFLAGS += -DGPS_SECONDARY_TYPE_H=\"subsystems/gps/gps_datalink.h\"
-      ap.CFLAGS += -DSECONDARY_GPS=GPS_DATALINK
+      $(TARGET).CFLAGS += -DGPS_SECONDARY_TYPE_H=\"subsystems/gps/gps_datalink.h\"
+      $(TARGET).CFLAGS += -DSECONDARY_GPS=GPS_DATALINK
       else
-      ap.CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_datalink.h\"
-      ap.CFLAGS += -DPRIMARY_GPS=GPS_DATALINK
+      $(TARGET).CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_datalink.h\"
+      $(TARGET).CFLAGS += -DPRIMARY_GPS=GPS_DATALINK
       endif
       else
       # plain old single GPS usage
-      ap.CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_datalink.h\"
+      $(TARGET).CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_datalink.h\"
       endif
     </raw>
   </makefile>
+  <makefile target="fbw">
+    <define name="USE_GPS"/>
+  </makefile>
 </module>
 

conf/modules/gps_furuno.xml

@@ -18,7 +18,7 @@
   <init fun="gps_nmea_init()"/>
   <periodic fun="gps_nmea_periodic_check()" freq="1." autorun="TRUE"/>
   <event fun="gps_nmea_event()"/>
-  <makefile target="ap">
+  <makefile target="ap|fbw">
     <configure name="FURUNO_GPS_PORT" default="$(GPS_PORT)" case="upper|lower"/>
     <configure name="FURUNO_GPS_BAUD" default="$(GPS_BAUD)"/>
 
@@ -35,16 +35,19 @@
       ifdef SECONDARY_GPS
       ifneq (,$(findstring $(SECONDARY_GPS), nmea furuno))
       # this is the secondary GPS
-      ap.CFLAGS += -DGPS_SECONDARY_TYPE_H=\"subsystems/gps/gps_nmea.h\"
-      ap.CFLAGS += -DSECONDARY_GPS=GPS_NMEA
+      $(TARGET).CFLAGS += -DGPS_SECONDARY_TYPE_H=\"subsystems/gps/gps_nmea.h\"
+      $(TARGET).CFLAGS += -DSECONDARY_GPS=GPS_NMEA
       else
-      ap.CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_nmea.h\"
-      ap.CFLAGS += -DPRIMARY_GPS=GPS_NMEA
+      $(TARGET).CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_nmea.h\"
+      $(TARGET).CFLAGS += -DPRIMARY_GPS=GPS_NMEA
       endif
       else
       # plain old single GPS usage
-      ap.CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_nmea.h\"
+      $(TARGET).CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_nmea.h\"
       endif
     </raw>
   </makefile>
+  <makefile target="fbw">
+    <define name="USE_GPS"/>
+  </makefile>
 </module>

conf/modules/gps_mediatek_diy.xml

@@ -18,7 +18,7 @@
   <init fun="gps_mtk_init()"/>
   <periodic fun="gps_mtk_periodic_check()" freq="1." autorun="TRUE"/>
   <event fun="gps_mtk_event()"/>
-  <makefile target="ap">
+  <makefile target="ap|fbw">
     <configure name="MTK_GPS_PORT" default="$(GPS_PORT)" case="upper|lower"/>
     <configure name="MTK_GPS_BAUD" default="$(GPS_BAUD)"/>
 
@@ -31,16 +31,19 @@
       ifdef SECONDARY_GPS
       ifneq (,$(findstring $(SECONDARY_GPS), mtk mediatek))
       # this is the secondary GPS
-      ap.CFLAGS += -DGPS_SECONDARY_TYPE_H=\"subsystems/gps/gps_mtk.h\"
-      ap.CFLAGS += -DSECONDARY_GPS=GPS_MTK
+      $(TARGET).CFLAGS += -DGPS_SECONDARY_TYPE_H=\"subsystems/gps/gps_mtk.h\"
+      $(TARGET).CFLAGS += -DSECONDARY_GPS=GPS_MTK
       else
-      ap.CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_mtk.h\"
-      ap.CFLAGS += -DPRIMARY_GPS=GPS_MTK
+      $(TARGET).CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_mtk.h\"
+      $(TARGET).CFLAGS += -DPRIMARY_GPS=GPS_MTK
       endif
       else
       # plain old single GPS usage
-      ap.CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_mtk.h\"
+      $(TARGET).CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_mtk.h\"
       endif
     </raw>
   </makefile>
+  <makefile target="fbw">
+    <define name="USE_GPS"/>
+  </makefile>
 </module>

conf/modules/gps_nmea.xml

@@ -18,7 +18,7 @@
   <init fun="gps_nmea_init()"/>
   <periodic fun="gps_nmea_periodic_check()" freq="1." autorun="TRUE"/>
   <event fun="gps_nmea_event()"/>
-  <makefile target="ap">
+  <makefile target="ap|fbw">
     <configure name="NMEA_GPS_PORT" default="$(GPS_PORT)" case="upper|lower"/>
     <configure name="NMEA_GPS_BAUD" default="$(GPS_BAUD)"/>
 
@@ -31,16 +31,19 @@
       ifdef SECONDARY_GPS
       ifneq (,$(findstring $(SECONDARY_GPS), nmea))
       # this is the secondary GPS
-      ap.CFLAGS += -DGPS_SECONDARY_TYPE_H=\"subsystems/gps/gps_nmea.h\"
-      ap.CFLAGS += -DSECONDARY_GPS=GPS_NMEA
+      $(TARGET).CFLAGS += -DGPS_SECONDARY_TYPE_H=\"subsystems/gps/gps_nmea.h\"
+      $(TARGET).CFLAGS += -DSECONDARY_GPS=GPS_NMEA
       else
-      ap.CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_nmea.h\"
-      ap.CFLAGS += -DPRIMARY_GPS=GPS_NMEA
+      $(TARGET).CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_nmea.h\"
+      $(TARGET).CFLAGS += -DPRIMARY_GPS=GPS_NMEA
       endif
       else
       # plain old single GPS usage
-      ap.CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_nmea.h\"
+      $(TARGET).CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_nmea.h\"
       endif
     </raw>
   </makefile>
+  <makefile target="fbw">
+    <define name="USE_GPS"/>
+  </makefile>
 </module>

conf/modules/gps_piksi.xml

@@ -18,7 +18,7 @@
   <init fun="gps_piksi_init()"/>
   <periodic fun="gps_piksi_periodic_check()" freq="1." autorun="TRUE"/>
   <event fun="gps_piksi_event()"/>
-  <makefile target="ap">
+  <makefile target="ap|fbw">
     <configure name="PIKSI_GPS_PORT" default="$(GPS_PORT)" case="upper|lower"/>
     <configure name="PIKSI_GPS_BAUD" default="B115200"/>
 
@@ -34,16 +34,19 @@
       ifdef SECONDARY_GPS
       ifneq (,$(findstring $(SECONDARY_GPS), piksi))
       # this is the secondary GPS
-      ap.CFLAGS += -DGPS_SECONDARY_TYPE_H=\"subsystems/gps/gps_piksi.h\"
-      ap.CFLAGS += -DSECONDARY_GPS=GPS_PIKSI
+      $(TARGET).CFLAGS += -DGPS_SECONDARY_TYPE_H=\"subsystems/gps/gps_piksi.h\"
+      $(TARGET).CFLAGS += -DSECONDARY_GPS=GPS_PIKSI
       else
-      ap.CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_piksi.h\"
-      ap.CFLAGS += -DPRIMARY_GPS=GPS_PIKSI
+      $(TARGET).CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_piksi.h\"
+      $(TARGET).CFLAGS += -DPRIMARY_GPS=GPS_PIKSI
       endif
       else
       # plain old single GPS usage
-      ap.CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_piksi.h\"
+      $(TARGET).CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_piksi.h\"
       endif
     </raw>
   </makefile>
+  <makefile target="fbw">
+    <define name="USE_GPS"/>
+  </makefile>
 </module>

conf/modules/gps_sirf.xml

@@ -18,7 +18,7 @@
   <init fun="gps_sirf_init()"/>
   <periodic fun="gps_sirf_periodic_check()" freq="1." autorun="TRUE"/>
   <event fun="gps_sirf_event()"/>
-  <makefile target="ap">
+  <makefile target=target="ap|fbw">
     <configure name="SIRF_GPS_PORT" default="$(GPS_PORT)" case="upper|lower"/>
     <configure name="SIRF_GPS_BAUD" default="$(GPS_BAUD)"/>
 
@@ -31,16 +31,19 @@
       ifdef SECONDARY_GPS
       ifneq (,$(findstring $(SECONDARY_GPS), sirf))
       # this is the secondary GPS
-      ap.CFLAGS += -DGPS_SECONDARY_TYPE_H=\"subsystems/gps/gps_sirf.h\"
-      ap.CFLAGS += -DSECONDARY_GPS=GPS_SIRF
+      $(TARGET).CFLAGS += -DGPS_SECONDARY_TYPE_H=\"subsystems/gps/gps_sirf.h\"
+      $(TARGET).CFLAGS += -DSECONDARY_GPS=GPS_SIRF
       else
-      ap.CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_sirf.h\"
-      ap.CFLAGS += -DPRIMARY_GPS=GPS_SIRF
+      $(TARGET).CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_sirf.h\"
+      $(TARGET).CFLAGS += -DPRIMARY_GPS=GPS_SIRF
       endif
       else
       # plain old single GPS usage
-      ap.CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_sirf.h\"
+      $(TARGET).CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_sirf.h\"
       endif
     </raw>
   </makefile>
+  <makefile target="fbw">
+    <define name="USE_GPS"/>
+  </makefile>
 </module>

conf/modules/gps_skytraq.xml

@@ -18,7 +18,7 @@
   <init fun="gps_skytraq_init()"/>
   <periodic fun="gps_skytraq_periodic_check()" freq="1." autorun="TRUE"/>
   <event fun="gps_skytraq_event()"/>
-  <makefile target="ap">
+  <makefile target="ap|fbw">
     <configure name="SKYTRAQ_GPS_PORT" default="$(GPS_PORT)" case="upper|lower"/>
     <configure name="SKYTRAQ_GPS_BAUD" default="$(GPS_BAUD)"/>
 
@@ -31,16 +31,19 @@
       ifdef SECONDARY_GPS
       ifneq (,$(findstring $(SECONDARY_GPS), skytraq))
       # this is the secondary GPS
-      ap.CFLAGS += -DGPS_SECONDARY_TYPE_H=\"subsystems/gps/gps_skytraq.h\"
-      ap.CFLAGS += -DSECONDARY_GPS=GPS_SKYTRAQ
+      $(TARGET).CFLAGS += -DGPS_SECONDARY_TYPE_H=\"subsystems/gps/gps_skytraq.h\"
+      $(TARGET).CFLAGS += -DSECONDARY_GPS=GPS_SKYTRAQ
       else
-      ap.CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_skytraq.h\"
-      ap.CFLAGS += -DPRIMARY_GPS=GPS_SKYTRAQ
+      $(TARGET).CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_skytraq.h\"
+      $(TARGET).CFLAGS += -DPRIMARY_GPS=GPS_SKYTRAQ
       endif
       else
       # plain old single GPS usage
-      ap.CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_skytraq.h\"
+      $(TARGET).CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_skytraq.h\"
       endif
     </raw>
   </makefile>
+  <makefile target="fbw">
+    <define name="USE_GPS"/>
+  </makefile>
 </module>

conf/modules/gps_ublox.xml

@@ -18,7 +18,7 @@
   <init fun="gps_ubx_init()"/>
   <periodic fun="gps_ubx_periodic_check()" freq="1." autorun="TRUE"/>
   <event fun="gps_ubx_event()"/>
-  <makefile target="ap">
+  <makefile target="ap|fbw">
     <configure name="UBX_GPS_PORT" default="$(GPS_PORT)" case="upper|lower"/>
     <configure name="UBX_GPS_BAUD" default="$(GPS_BAUD)"/>
 
@@ -31,17 +31,20 @@
       ifdef SECONDARY_GPS
       ifneq (,$(findstring $(SECONDARY_GPS), ublox))
       # this is the secondary GPS
-      ap.CFLAGS += -DGPS_SECONDARY_TYPE_H=\"subsystems/gps/gps_ubx.h\"
-      ap.CFLAGS += -DSECONDARY_GPS=GPS_UBX
+      $(TARGET).CFLAGS += -DGPS_SECONDARY_TYPE_H=\"subsystems/gps/gps_ubx.h\"
+      $(TARGET).CFLAGS += -DSECONDARY_GPS=GPS_UBX
       else
-      ap.CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_ubx.h\"
-      ap.CFLAGS += -DPRIMARY_GPS=GPS_UBX
+      $(TARGET).CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_ubx.h\"
+      $(TARGET).CFLAGS += -DPRIMARY_GPS=GPS_UBX
       endif
       else
       # plain old single GPS usage
-      ap.CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_ubx.h\"
+      $(TARGET).CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_ubx.h\"
       endif
     </raw>
   </makefile>
+  <makefile target="fbw">
+    <define name="USE_GPS"/>
+  </makefile>
 </module>
 

conf/modules/gps_ubx_i2c.xml

@@ -18,7 +18,7 @@
   <periodic fun="gps_ubx_i2c_periodic()" freq="10."/>
   <event fun="GpsUbxi2cEvent()"/>
 
-  <makefile target="ap">
+  <makefile target="ap|fbw">
     <configure name="GPS_UBX_I2C_DEV" default="i2c1" case="upper|lower"/>
     <define name="USE_$(GPS_UBX_I2C_DEV_UPPER)"/>
     <define name="GPS_UBX_I2C_DEV" value="$(GPS_UBX_I2C_DEV_LOWER)"/>

conf/modules/gps_ubx_ucenter.xml

@@ -40,7 +40,7 @@ Warning: you still need to tell the driver, which paparazzi port you use.
   </header>
   <init fun="gps_ubx_ucenter_init()"/>
   <periodic fun="gps_ubx_ucenter_periodic()" start="gps_ubx_ucenter_init()" freq="4." autorun="TRUE"/>
-  <makefile target="ap">
+  <makefile target="ap|fbw">
     <file name="gps_ubx_ucenter.c"/>
     <define name="GPS_UBX_UCENTER"/>
   </makefile>

sw/airborne/subsystems/abi_sender_ids.h

@@ -178,6 +178,10 @@
 #define GPS_VECTORNAV_ID 13
 #endif
 
+#ifndef GPS_IMCU_ID
+#define GPS_IMCU_ID 14
+#endif
+
 /*
  * IDs of IMU sensors (accel, gyro)
  */

sw/airborne/subsystems/gps.c

@@ -77,7 +77,7 @@ struct GpsState gps;
 struct GpsTimeSync gps_time_sync;
 
 #ifdef SECONDARY_GPS
-static uint8_t current_gps_id = 0;
+static uint8_t current_gps_id = GpsId(PRIMARY_GPS);
 #endif
 
 uint8_t multi_gps_mode;

sw/airborne/subsystems/intermcu/intermcu_ap.c

@@ -47,6 +47,13 @@ uint8_t imcu_msg_buf[128] __attribute__((aligned));  ///< The InterMCU message b
 static struct fbw_status_t fbw_status;
 static inline void intermcu_parse_msg(void (*rc_frame_handler)(void));
 
+#if IMCU_GPS
+#include "std.h"
+#include "subsystems/abi.h"
+#include "subsystems/gps.h"
+static struct GpsState gps_imcu;
+#endif
+
 
 #if PERIODIC_TELEMETRY
 #include "subsystems/datalink/telemetry.h"
@@ -65,6 +72,15 @@ void intermcu_init(void)
 {
   pprz_transport_init(&intermcu.transport);
 
+#if IMCU_GPS
+  gps_imcu.fix = GPS_FIX_NONE;
+  gps_imcu.pdop = 0;
+  gps_imcu.sacc = 0;
+  gps_imcu.pacc = 0;
+  gps_imcu.cacc = 0;
+  gps_imcu.comp_id = GPS_IMCU_ID;
+#endif
+
 #if PERIODIC_TELEMETRY
   register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_FBW_STATUS, send_status);
 #endif
@@ -79,6 +95,10 @@ void intermcu_periodic(void)
   } else {
     intermcu.time_since_last_frame++;
   }
+
+#if IMCU_GPS
+  RunOnceEvery(PERIODIC_FREQUENCY, gps_periodic_check(&gps_imcu));
+#endif
 }
 
 /* Enable or disable the communication of the InterMCU */
@@ -151,6 +171,44 @@ static inline void intermcu_parse_msg(void (*rc_frame_handler)(void))
       telemetry_intermcu_on_msg(0, msg, size);
       break;
     }
+#if IMCU_GPS
+    case DL_IMCU_REMOTE_GPS: {
+      uint32_t now_ts = get_sys_time_usec();
+      gps_imcu.ecef_pos.x = DL_IMCU_REMOTE_GPS_ecef_x(imcu_msg_buf);
+      gps_imcu.ecef_pos.y = DL_IMCU_REMOTE_GPS_ecef_y(imcu_msg_buf);
+      gps_imcu.ecef_pos.z = DL_IMCU_REMOTE_GPS_ecef_z(imcu_msg_buf);
+      SetBit(gps_imcu.valid_fields, GPS_VALID_POS_ECEF_BIT);
+
+      gps_imcu.lla_pos.alt = DL_IMCU_REMOTE_GPS_alt(imcu_msg_buf);
+      gps_imcu.hmsl = DL_IMCU_REMOTE_GPS_hmsl(imcu_msg_buf);
+      SetBit(gps_imcu.valid_fields, GPS_VALID_HMSL_BIT);
+
+      gps_imcu.ecef_vel.x = DL_IMCU_REMOTE_GPS_ecef_xd(imcu_msg_buf);
+      gps_imcu.ecef_vel.y = DL_IMCU_REMOTE_GPS_ecef_yd(imcu_msg_buf);
+      gps_imcu.ecef_vel.z = DL_IMCU_REMOTE_GPS_ecef_zd(imcu_msg_buf);
+      SetBit(gps_imcu.valid_fields, GPS_VALID_VEL_ECEF_BIT);
+
+      gps_imcu.course = DL_IMCU_REMOTE_GPS_course(imcu_msg_buf);
+      gps_imcu.gspeed = DL_IMCU_REMOTE_GPS_gspeed(imcu_msg_buf);
+      SetBit(gps_imcu.valid_fields, GPS_VALID_COURSE_BIT);
+
+      gps_imcu.pacc = DL_IMCU_REMOTE_GPS_pacc(imcu_msg_buf);
+      gps_imcu.sacc = DL_IMCU_REMOTE_GPS_sacc(imcu_msg_buf);
+      gps_imcu.num_sv = DL_IMCU_REMOTE_GPS_numsv(imcu_msg_buf);
+      gps_imcu.fix = DL_IMCU_REMOTE_GPS_fix(imcu_msg_buf);
+
+      // set gps msg time
+      gps_imcu.last_msg_ticks = sys_time.nb_sec_rem;
+      gps_imcu.last_msg_time = sys_time.nb_sec;
+
+      if (gps_imcu.fix >= GPS_FIX_3D) {
+        gps_imcu.last_3dfix_ticks = sys_time.nb_sec_rem;
+        gps_imcu.last_3dfix_time = sys_time.nb_sec;
+      }
+
+      AbiSendMsgGPS(GPS_IMCU_ID, now_ts, &gps_imcu);
+      break;
+    }
 
 #endif
 

sw/airborne/subsystems/intermcu/intermcu_fbw.c

@@ -66,10 +66,26 @@ static void intermcu_parse_msg(void (*commands_frame_handler)(void));
 static void checkPx4RebootCommand(unsigned char b);
 #endif
 
+#ifdef USE_GPS
+
+#ifndef IMCU_GPS_ID
+#define IMCU_GPS_ID GPS_MULTI_ID
+#endif
+
+#include "subsystems/abi.h"
+#include "subsystems/gps.h"
+static abi_event gps_ev;
+static void gps_cb(uint8_t sender_id, uint32_t stamp, struct GpsState *gps_s);
+#endif
+
 void intermcu_init(void)
 {
   pprz_transport_init(&intermcu.transport);
 
+#if USE_GPS
+  AbiBindMsgGPS(IMCU_GPS_ID, &gps_ev, gps_cb);
+#endif
+
 #ifdef BOARD_PX4IO
   px4bl_tid = sys_time_register_timer(10.0, NULL);
 #endif
@@ -205,6 +221,33 @@ void InterMcuEvent(void (*frame_handler)(void))
   }
 }
 
+#if USE_GPS
+static void gps_cb(uint8_t sender_id __attribute__((unused)),
+                   uint32_t stamp __attribute__((unused)),
+                   struct GpsState *gps_s) {
+  pprz_msg_send_IMCU_REMOTE_GPS(&(intermcu.transport.trans_tx), intermcu.device, INTERMCU_FBW,
+    &gps_s->ecef_pos.x,
+    &gps_s->ecef_pos.y,
+    &gps_s->ecef_pos.z,
+    &gps_s->lla_pos.alt,
+    &gps_s->hmsl,
+    &gps_s->ecef_vel.x,
+    &gps_s->ecef_vel.y,
+    &gps_s->ecef_vel.z,
+    &gps_s->course,
+    &gps_s->gspeed,
+    &gps_s->pacc,
+    &gps_s->sacc,
+    &gps_s->num_sv,
+    &gps_s->fix);
+}
+
+void gps_periodic_check(struct GpsState *gps_s) {
+  if (sys_time.nb_sec - gps_s->last_msg_time > GPS_TIMEOUT) {
+    gps_s->fix = GPS_FIX_NONE;
+  }
+}
+#endif
 
 /* SOME STUFF FOR PX4IO BOOTLOADER (TODO: move this code) */
 #ifdef BOARD_PX4IO