[gps] directly send ABI messages from implementation

and get rid of the on_gps_solution callback in main
paparazzi · Mar 30, 2015 · 6759ea1 · 6759ea1
1 parent 38e0c49
commit 6759ea1
Show file tree

Hide file tree

Showing 33 changed files with 291 additions and 276 deletions.
diff --git a/sw/airborne/arch/sim/sim_gps.c b/sw/airborne/arch/sim/sim_gps.c
@@ -74,7 +74,7 @@ value sim_use_gps_pos(value x, value y, value z, value c, value a, value s, valu
   //gps_verbose_downlink = !launch;
   //gps_downlink();
 
-  gps_available = TRUE;
+  gps_sim_publish();
 
   return Val_unit;
 }

diff --git a/sw/airborne/firmwares/fixedwing/main_ap.c b/sw/airborne/firmwares/fixedwing/main_ap.c
@@ -148,10 +148,6 @@ static inline void on_accel_event(void);
 static inline void on_mag_event(void);
 #endif // USE_IMU
 
-#if USE_GPS
-static inline void on_gps_solution(void);
-#endif
-
 #if defined RADIO_CONTROL || defined RADIO_CONTROL_AUTO1
 static uint8_t  mcu1_ppm_cpt;
 #endif
@@ -688,7 +684,7 @@ void event_task_ap(void)
 #endif
 
 #if USE_GPS
-  GpsEvent(on_gps_solution);
+  GpsEvent();
 #endif /* USE_GPS */
 
 #if USE_BARO_BOARD
@@ -720,21 +716,6 @@ void event_task_ap(void)
 } /* event_task_ap() */
 
 
-#if USE_GPS
-static inline void on_gps_solution(void)
-{
-  // current timestamp
-  uint32_t now_ts = get_sys_time_usec();
-
-  AbiSendMsgGPS(1, now_ts, &gps);
-
-#ifdef GPS_TRIGGERED_FUNCTION
-  GPS_TRIGGERED_FUNCTION();
-#endif
-}
-#endif
-
-
 #if USE_IMU
 static inline void on_accel_event(void)
 {

diff --git a/sw/airborne/firmwares/rotorcraft/main.c b/sw/airborne/firmwares/rotorcraft/main.c
@@ -107,7 +107,6 @@ INFO_VALUE("it is recommended to configure in your airframe PERIODIC_FREQUENCY t
 
 static inline void on_gyro_event(void);
 static inline void on_accel_event(void);
-static inline void on_gps_event(void);
 static inline void on_mag_event(void);
 
 
@@ -327,7 +326,7 @@ STATIC_INLINE void main_event(void)
 #endif
 
 #if USE_GPS
-  GpsEvent(on_gps_event);
+  GpsEvent();
 #endif
 
 #if FAILSAFE_GROUND_DETECT || KILL_ON_GROUND_DETECT
@@ -371,20 +370,6 @@ static inline void on_gyro_event( void ) {
 #endif
 }
 
-static inline void on_gps_event(void)
-{
-  // current timestamp
-  uint32_t now_ts = get_sys_time_usec();
-
-  AbiSendMsgGPS(1, now_ts, &gps);
-
-#ifdef USE_VEHICLE_INTERFACE
-  if (gps.fix == GPS_FIX_3D) {
-    vi_notify_gps_available();
-  }
-#endif
-}
-
 static inline void on_mag_event(void)
 {
   imu_scale_mag(&imu);

diff --git a/sw/airborne/modules/ahrs/ahrs_infrared.c b/sw/airborne/modules/ahrs/ahrs_infrared.c
@@ -52,13 +52,26 @@ static float heading;
 #endif
 static abi_event gyro_ev;
 
+#ifndef AHRS_INFRARED_GPS_ID
+#define AHRS_INFRARED_GPS_ID ABI_BROADCAST
+#endif
+static abi_event gps_ev;
+void ahrs_infrared_update_gps(struct GpsState *gps_s);
+
 static void gyro_cb(uint8_t sender_id __attribute__((unused)),
                     uint32_t stamp __attribute__((unused)),
                     struct Int32Rates *gyro)
 {
   stateSetBodyRates_i(gyro);
 }
 
+static void gps_cb(uint8_t sender_id __attribute__((unused)),
+                   uint32_t stamp __attribute__((unused)),
+                   struct GpsState *gps_s)
+{
+  ahrs_infrared_update_gps(gps_s);
+}
+
 
 #if PERIODIC_TELEMETRY
 #include "subsystems/datalink/telemetry.h"
@@ -85,6 +98,7 @@ void ahrs_infrared_init(void)
   heading = 0.;
 
   AbiBindMsgIMU_GYRO_INT32(AHRS_INFRARED_GYRO_ID, &gyro_ev, gyro_cb);
+  AbiBindMsgGPS(AHRS_INFRARED_GPS_ID, &gps_ev, &gps_cb);
 
 #if PERIODIC_TELEMETRY
   register_periodic_telemetry(DefaultPeriodic, "IR_SENSORS", send_infrared);
@@ -93,10 +107,10 @@ void ahrs_infrared_init(void)
 }
 
 
-void ahrs_infrared_update_gps(void)
+void ahrs_infrared_update_gps(struct GpsState *gps_s)
 {
-  float hspeed_mod_f = gps.gspeed / 100.;
-  float course_f = gps.course / 1e7;
+  float hspeed_mod_f = gps_s->gspeed / 100.;
+  float course_f = gps_s->course / 1e7;
 
   // Heading estimator from wind-information, usually computed with -DWIND_INFO
   // wind_north and wind_east initialized to 0, so still correct if not updated

diff --git a/sw/airborne/modules/ahrs/ahrs_infrared.h b/sw/airborne/modules/ahrs/ahrs_infrared.h
@@ -33,8 +33,5 @@
 
 extern void ahrs_infrared_init(void);
 extern void ahrs_infrared_periodic(void);
-extern void ahrs_infrared_update_gps(void);
-
-#define GPS_TRIGGERED_FUNCTION ahrs_infrared_update_gps
 
 #endif /* AHRS_INFRARED_H */
diff --git a/sw/airborne/modules/ins/ins_xsens.c b/sw/airborne/modules/ins/ins_xsens.c
@@ -40,6 +40,7 @@
 #error "USE_GPS needs to be 1 to use the Xsens GPS!"
 #endif
 #include "subsystems/gps.h"
+#include "subsystems/abi.h"
 #include "math/pprz_geodetic_wgs84.h"
 #include "math/pprz_geodetic_float.h"
 #include "subsystems/navigation/common_nav.h" /* needed for nav_utm_zone0 */
@@ -297,7 +298,19 @@ void ins_xsens_update_gps(struct GpsState *gps_s)
 void gps_impl_init(void)
 {
   gps.nb_channels = 0;
-  gps_xsens_msg_available = FALSE;
+}
+
+static void gps_xsens_publish(void)
+{
+  // publish gps data
+  uint32_t now_ts = get_sys_time_usec();
+  gps.last_msg_ticks = sys_time.nb_sec_rem;
+  gps.last_msg_time = sys_time.nb_sec;
+  if (gps.fix == GPS_FIX_3D) {
+    gps.last_3dfix_ticks = sys_time.nb_sec_rem;
+    gps.last_3dfix_time = sys_time.nb_sec;
+  }
+  AbiSendMsgGPS(GPS_XSENS_ID, now_ts, &gps);
 }
 #endif
 
@@ -533,7 +546,7 @@ void parse_ins_msg(void)
       gps.sacc = XSENS_DATA_RAWGPS_sacc(xsens_msg_buf, offset) / 100;
       gps.pdop = 5;  // FIXME Not output by XSens
 
-      gps_xsens_msg_available = TRUE;
+      gps_xsens_publish();
 #endif
       offset += XSENS_DATA_RAWGPS_LENGTH;
     }
@@ -627,7 +640,7 @@ void parse_ins_msg(void)
       gps.hmsl = ins_z * 1000;
       // what about gps.lla_pos.alt and gps.utm_pos.alt ?
 
-      gps_xsens_msg_available = TRUE;
+      gps_xsens_publish();
 #endif
       offset += XSENS_DATA_Position_LENGTH;
     }

diff --git a/sw/airborne/modules/ins/ins_xsens.h b/sw/airborne/modules/ins/ins_xsens.h
@@ -91,19 +91,7 @@ extern void ins_xsens_register(void);
 
 
 #if USE_GPS_XSENS
-extern bool_t gps_xsens_msg_available;
-#define GpsEvent(_sol_available_callback) {         \
-    if (gps_xsens_msg_available) {                  \
-      gps.last_msg_ticks = sys_time.nb_sec_rem;     \
-      gps.last_msg_time = sys_time.nb_sec;          \
-      if (gps.fix == GPS_FIX_3D) {                  \
-        gps.last_3dfix_ticks = sys_time.nb_sec_rem; \
-        gps.last_3dfix_time = sys_time.nb_sec;      \
-      }                                             \
-      _sol_available_callback();                    \
-      gps_xsens_msg_available = FALSE;              \
-    }                                               \
-  }
+#define GpsEvent() {}
 #endif
 
 #endif
diff --git a/sw/airborne/modules/ins/ins_xsens700.c b/sw/airborne/modules/ins/ins_xsens700.c
@@ -42,10 +42,10 @@
 #error "USE_GPS needs to be 1 to use the Xsens GPS!"
 #endif
 #include "subsystems/gps.h"
+#include "subsystems/abi.h"
 #include "math/pprz_geodetic_wgs84.h"
 #include "math/pprz_geodetic_float.h"
 #include "subsystems/navigation/common_nav.h" /* needed for nav_utm_zone0 */
-bool_t gps_xsens_msg_available;
 #endif
 
 // positions
@@ -213,7 +213,19 @@ void ins_update_gps(void)
 void gps_impl_init(void)
 {
   gps.nb_channels = 0;
-  gps_xsens_msg_available = FALSE;
+}
+
+static void gps_xsens_publish(void)
+{
+  // publish gps data
+  uint32_t now_ts = get_sys_time_usec();
+  gps.last_msg_ticks = sys_time.nb_sec_rem;
+  gps.last_msg_time = sys_time.nb_sec;
+  if (gps.fix == GPS_FIX_3D) {
+    gps.last_3dfix_ticks = sys_time.nb_sec_rem;
+    gps.last_3dfix_time = sys_time.nb_sec;
+  }
+  AbiSendMsgGPS(GPS_XSENS_ID, now_ts, &gps);
 }
 #endif
 
@@ -485,7 +497,7 @@ void parse_ins_msg(void)
           gps.utm_pos.east = utm_f.east * 100;
           gps.utm_pos.north = utm_f.north * 100;
 
-          gps_xsens_msg_available = TRUE;
+          gps_xsens_publish();
         }
       } else if (code1 == 0xD0) {
         if (code2 == 0x10) {

diff --git a/sw/airborne/subsystems/abi_sender_ids.h b/sw/airborne/subsystems/abi_sender_ids.h
@@ -111,4 +111,51 @@
 #define MAG_HMC58XX_SENDER_ID 2
 #endif
 
+/*
+ * IDs of GPS sensors
+ */
+#ifndef GPS_UBX_ID
+#define GPS_UBX_ID 1
+#endif
+
+#ifndef GPS_NMEA_ID
+#define GPS_NMEA_ID 2
+#endif
+
+#ifndef GPS_SIRF_ID
+#define GPS_SIRF_ID 3
+#endif
+
+#ifndef GPS_SKYTRAQ_ID
+#define GPS_SKYTRAQ_ID 4
+#endif
+
+#ifndef GPS_MTK_ID
+#define GPS_MTK_ID 5
+#endif
+
+#ifndef GPS_PIKSI_ID
+#define GPS_PIKSI_ID 6
+#endif
+
+#ifndef GPS_XSENS_ID
+#define GPS_XSENS_ID 7
+#endif
+
+#ifndef GPS_DATALINK_ID
+#define GPS_DATALINK_ID 8
+#endif
+
+#ifndef GPS_UDP_ID
+#define GPS_UDP_ID 9
+#endif
+
+#ifndef GPS_ARDRONE2_ID
+#define GPS_ARDRONE2_ID 10
+#endif
+
+#ifndef GPS_SIM_ID
+#define GPS_SIM_ID 11
+#endif
+
 #endif /* ABI_SENDER_IDS_H */
diff --git a/sw/airborne/subsystems/gps/gps_ardrone2.c b/sw/airborne/subsystems/gps/gps_ardrone2.c
@@ -31,13 +31,11 @@
 #endif
 
 #include "subsystems/gps.h"
+#include "subsystems/abi.h"
 #include "math/pprz_geodetic_double.h"
 
-bool_t gps_ardrone2_available;
-
 void gps_impl_init(void)
 {
-  gps_ardrone2_available = FALSE;
 }
 
 void gps_ardrone2_parse(navdata_gps_t *navdata_gps)
@@ -76,6 +74,13 @@ void gps_ardrone2_parse(navdata_gps_t *navdata_gps)
     gps.fix = GPS_FIX_NONE;
   }
 
-  // Set that there is a packet
-  gps_ardrone2_available = TRUE;
+  gps.last_msg_ticks = sys_time.nb_sec_rem;
+  gps.last_msg_time = sys_time.nb_sec;
+  if (gps.fix == GPS_FIX_3D) {
+    gps.last_3dfix_ticks = sys_time.nb_sec_rem;
+    gps.last_3dfix_time = sys_time.nb_sec;
+  }
+
+  uint32_t now_ts = get_sys_time_usec();
+  AbiSendMsgGPS(GPS_ARDRONE2_ID, now_ts, &gps);
 }
diff --git a/sw/airborne/subsystems/gps/gps_ardrone2.h b/sw/airborne/subsystems/gps/gps_ardrone2.h
@@ -31,23 +31,11 @@
 #include "boards/ardrone/at_com.h"
 
 #define GPS_NB_CHANNELS 12
-extern bool_t gps_ardrone2_available;
 
 /*
- * The GPS event
+ * The GPS event (dummy)
  */
-#define GpsEvent(_sol_available_callback) {         \
-    if (gps_ardrone2_available) {                   \
-      gps.last_msg_ticks = sys_time.nb_sec_rem;     \
-      gps.last_msg_time = sys_time.nb_sec;          \
-      if (gps.fix == GPS_FIX_3D) {                  \
-        gps.last_3dfix_ticks = sys_time.nb_sec_rem; \
-        gps.last_3dfix_time = sys_time.nb_sec;      \
-      }                                             \
-      _sol_available_callback();                    \
-      gps_ardrone2_available = FALSE;               \
-    }                                               \
-  }
+#define GpsEvent() {}
 
 void gps_ardrone2_parse(navdata_gps_t *navdata_gps);