diff --git a/sw/airborne/firmwares/fixedwing/main_ap.c b/sw/airborne/firmwares/fixedwing/main_ap.c index fc79f5ee11..ea5eeac23a 100644 --- a/sw/airborne/firmwares/fixedwing/main_ap.c +++ b/sw/airborne/firmwares/fixedwing/main_ap.c @@ -455,7 +455,7 @@ void reporting_task( void ) { #ifdef FAILSAFE_DELAY_WITHOUT_GPS -#define GpsTimeoutError (sys_time.nb_sec - gps.last_fix_time > FAILSAFE_DELAY_WITHOUT_GPS) +#define GpsTimeoutError (sys_time.nb_sec - gps.last_3dfix_time > FAILSAFE_DELAY_WITHOUT_GPS) #endif /** @@ -596,6 +596,10 @@ void sensors_task( void ) { baro_periodic(); #endif +#if USE_GPS + gps_periodic_check(); +#endif + ins_periodic(); } diff --git a/sw/airborne/firmwares/rotorcraft/main.c b/sw/airborne/firmwares/rotorcraft/main.c index 20ebb3aa75..30410067b4 100644 --- a/sw/airborne/firmwares/rotorcraft/main.c +++ b/sw/airborne/firmwares/rotorcraft/main.c @@ -254,6 +254,7 @@ STATIC_INLINE void failsafe_check( void ) { #endif #if USE_GPS + gps_periodic_check(); if (autopilot_mode == AP_MODE_NAV && autopilot_motors_on && #if NO_GPS_LOST_WITH_RC_VALID diff --git a/sw/airborne/modules/ins/ins_xsens.h b/sw/airborne/modules/ins/ins_xsens.h index d64befa4c7..379706205f 100644 --- a/sw/airborne/modules/ins/ins_xsens.h +++ b/sw/airborne/modules/ins/ins_xsens.h @@ -89,9 +89,11 @@ extern struct ImuXsens imu_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_fix_ticks = sys_time.nb_sec_rem; \ - gps.last_fix_time = sys_time.nb_sec; \ + 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; \ diff --git a/sw/airborne/modules/ins/ins_xsens700.c b/sw/airborne/modules/ins/ins_xsens700.c index 18c03c446b..70354bbea5 100644 --- a/sw/airborne/modules/ins/ins_xsens700.c +++ b/sw/airborne/modules/ins/ins_xsens700.c @@ -424,7 +424,8 @@ void parse_ins_msg( void ) { #if USE_GPS_XSENS gps.nb_channels = XSENS_XDI_GpsSvInfo_nch(xsens_msg_buf+offset); - gps.last_fix_time = sys_time.nb_sec; + gps.last_3dfix_ticks = sys_time.nb_sec_rem; + gps.last_3dfix_time = sys_time.nb_sec; uint8_t i; // Do not write outside buffer @@ -463,7 +464,8 @@ void parse_ins_msg( void ) { #ifdef GPS_LED LED_TOGGLE(GPS_LED); #endif - gps.last_fix_time = sys_time.nb_sec; + gps.last_3dfix_ticks = sys_time.nb_sec_rem; + gps.last_3dfix_time = sys_time.nb_sec; gps.week = 0; // FIXME lla_f.lat = RadOfDeg(XSENS_DATA_LatLon_lat(xsens_msg_buf,offset)); lla_f.lon = RadOfDeg(XSENS_DATA_LatLon_lon(xsens_msg_buf,offset)); diff --git a/sw/airborne/subsystems/gps.c b/sw/airborne/subsystems/gps.c index 29f2cf5f5d..62164f6430 100644 --- a/sw/airborne/subsystems/gps.c +++ b/sw/airborne/subsystems/gps.c @@ -101,6 +101,11 @@ void gps_init(void) { gps.week = 0; gps.tow = 0; gps.cacc = 0; + + gps.last_3dfix_ticks = 0; + gps.last_3dfix_time = 0; + gps.last_msg_ticks = 0; + gps.last_msg_time = 0; #ifdef GPS_LED LED_OFF(GPS_LED); #endif @@ -116,6 +121,12 @@ void gps_init(void) { #endif } +void gps_periodic_check(void) { + if (sys_time.nb_sec - gps.last_msg_time > GPS_TIMEOUT) { + gps.fix = GPS_FIX_NONE; + } +} + uint32_t gps_tow_from_sys_ticks(uint32_t sys_ticks) { uint32_t clock_delta; diff --git a/sw/airborne/subsystems/gps.h b/sw/airborne/subsystems/gps.h index 2254f03d3c..007c337ffd 100644 --- a/sw/airborne/subsystems/gps.h +++ b/sw/airborne/subsystems/gps.h @@ -82,8 +82,10 @@ struct GpsState { uint8_t nb_channels; ///< Number of scanned satellites struct SVinfo svinfos[GPS_NB_CHANNELS]; ///< holds information from the Space Vehicles (Satellites) - uint32_t last_fix_ticks; ///< cpu time ticks at last valid fix - uint32_t last_fix_time; ///< cpu time in sec at last valid fix + uint32_t last_3dfix_ticks; ///< cpu time ticks at last valid 3D fix + uint32_t last_3dfix_time; ///< cpu time in sec at last valid 3D fix + uint32_t last_msg_ticks; ///< cpu time ticks at last received GPS message + uint32_t last_msg_time; ///< cpu time in sec at last received GPS message uint16_t reset; ///< hotstart, warmstart, coldstart }; @@ -104,21 +106,24 @@ extern void gps_init(void); extern void gps_impl_init(void); -/* mark GPS as lost when no valid 3D fix was received for GPS_TIMEOUT secs */ +/** GPS timeout in seconds */ #ifndef GPS_TIMEOUT -#define GPS_TIMEOUT 5 +#define GPS_TIMEOUT 2 #endif inline bool_t GpsIsLost(void); inline bool_t GpsIsLost(void) { - if (sys_time.nb_sec - gps.last_fix_time > GPS_TIMEOUT) { - gps.fix = GPS_FIX_NONE; - return TRUE; + if (gps.fix == GPS_FIX_3D) { + return FALSE; } - return FALSE; + return TRUE; } +/** Periodic GPS check. + * Marks GPS as lost when no GPS message was received for GPS_TIMEOUT seconds + */ +extern void gps_periodic_check(void); /** * GPS Reset diff --git a/sw/airborne/subsystems/gps/gps_ardrone2.h b/sw/airborne/subsystems/gps/gps_ardrone2.h index 2995d0f853..ac27e6eefc 100644 --- a/sw/airborne/subsystems/gps/gps_ardrone2.h +++ b/sw/airborne/subsystems/gps/gps_ardrone2.h @@ -36,15 +36,17 @@ extern bool_t gps_ardrone2_available; /* * The GPS event */ -#define GpsEvent(_sol_available_callback) { \ +#define GpsEvent(_sol_available_callback) { \ if (gps_ardrone2_available) { \ - if (gps.fix == GPS_FIX_3D) { \ - gps.last_fix_ticks = sys_time.nb_sec_rem; \ - gps.last_fix_time = sys_time.nb_sec; \ - } \ - _sol_available_callback(); \ + 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; \ - } \ + } \ } void gps_ardrone2_parse(navdata_gps_t *navdata_gps); diff --git a/sw/airborne/subsystems/gps/gps_mtk.c b/sw/airborne/subsystems/gps/gps_mtk.c index 00b160b341..5b3886804f 100644 --- a/sw/airborne/subsystems/gps/gps_mtk.c +++ b/sw/airborne/subsystems/gps/gps_mtk.c @@ -167,7 +167,7 @@ void gps_mtk_read_message(void) { gps.lla_pos.lat = RadOfDeg(MTK_DIY14_NAV_LAT(gps_mtk.msg_buf))*10; gps.lla_pos.lon = RadOfDeg(MTK_DIY14_NAV_LON(gps_mtk.msg_buf))*10; // FIXME: with MTK you do not receive vertical speed - if (sys_time.nb_sec - gps.last_fix_time < 2) { + if (sys_time.nb_sec - gps.last_3dfix_time < 2) { gps.ned_vel.z = ((gps.hmsl - MTK_DIY14_NAV_HEIGHT(gps_mtk.msg_buf)*10)*OUTPUT_RATE)/10; } else gps.ned_vel.z = 0; @@ -231,7 +231,7 @@ void gps_mtk_read_message(void) { gps.lla_pos.lat = RadOfDeg(MTK_DIY16_NAV_LAT(gps_mtk.msg_buf))*10; gps.lla_pos.lon = RadOfDeg(MTK_DIY16_NAV_LON(gps_mtk.msg_buf))*10; // FIXME: with MTK you do not receive vertical speed - if (sys_time.nb_sec - gps.last_fix_time < 2) { + if (sys_time.nb_sec - gps.last_3dfix_time < 2) { gps.ned_vel.z = ((gps.hmsl - MTK_DIY16_NAV_HEIGHT(gps_mtk.msg_buf)*10)*OUTPUT_RATE)/10; } else gps.ned_vel.z = 0; diff --git a/sw/airborne/subsystems/gps/gps_mtk.h b/sw/airborne/subsystems/gps/gps_mtk.h index 571dce14cf..887778cad5 100644 --- a/sw/airborne/subsystems/gps/gps_mtk.h +++ b/sw/airborne/subsystems/gps/gps_mtk.h @@ -70,30 +70,33 @@ extern bool_t gps_configuring; #define GpsConfigure() {} #endif -#define GpsEvent(_sol_available_callback) { \ - if (GpsBuffer()) { \ - ReadGpsBuffer(); \ - GpsConfigure(); \ - } \ - if (gps_mtk.msg_available) { \ - gps_mtk_read_message(); \ - if (gps_mtk.msg_class == MTK_DIY14_ID && \ - gps_mtk.msg_id == MTK_DIY14_NAV_ID) { \ - if (gps.fix == GPS_FIX_3D) { \ - gps.last_fix_time = sys_time.nb_sec; \ - } \ - _sol_available_callback(); \ - } \ - if (gps_mtk.msg_class == MTK_DIY16_ID && \ - gps_mtk.msg_id == MTK_DIY16_NAV_ID) { \ - if (gps.fix == GPS_FIX_3D) { \ - gps.last_fix_ticks = sys_time.nb_sec_rem; \ - gps.last_fix_time = sys_time.nb_sec; \ - } \ - _sol_available_callback(); \ - } \ - gps_mtk.msg_available = FALSE; \ - } \ +#define GpsEvent(_sol_available_callback) { \ + if (GpsBuffer()) { \ + ReadGpsBuffer(); \ + GpsConfigure(); \ + } \ + if (gps_mtk.msg_available) { \ + gps.last_msg_ticks = sys_time.nb_sec_rem; \ + gps.last_msg_time = sys_time.nb_sec; \ + gps_mtk_read_message(); \ + if (gps_mtk.msg_class == MTK_DIY14_ID && \ + gps_mtk.msg_id == MTK_DIY14_NAV_ID) { \ + 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(); \ + } \ + if (gps_mtk.msg_class == MTK_DIY16_ID && \ + gps_mtk.msg_id == MTK_DIY16_NAV_ID) { \ + 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_mtk.msg_available = FALSE; \ + } \ } #define ReadGpsBuffer() { \ diff --git a/sw/airborne/subsystems/gps/gps_nmea.h b/sw/airborne/subsystems/gps/gps_nmea.h index 950e5ff56d..0772e68b90 100644 --- a/sw/airborne/subsystems/gps/gps_nmea.h +++ b/sw/airborne/subsystems/gps/gps_nmea.h @@ -61,21 +61,23 @@ extern struct GpsNmea gps_nmea; #define GpsBuffer() GpsLink(ChAvailable()) -#define GpsEvent(_sol_available_callback) { \ - if (GpsBuffer()) { \ - ReadGpsBuffer(); \ - } \ - if (gps_nmea.msg_available) { \ - nmea_parse_msg(); \ - if (gps_nmea.pos_available) { \ - if (gps.fix == GPS_FIX_3D) { \ - gps.last_fix_ticks = sys_time.nb_sec_rem; \ - gps.last_fix_time = sys_time.nb_sec; \ - } \ - _sol_available_callback(); \ - } \ - gps_nmea.msg_available = FALSE; \ - } \ +#define GpsEvent(_sol_available_callback) { \ + if (GpsBuffer()) { \ + ReadGpsBuffer(); \ + } \ + if (gps_nmea.msg_available) { \ + gps.last_msg_ticks = sys_time.nb_sec_rem; \ + gps.last_msg_time = sys_time.nb_sec; \ + nmea_parse_msg(); \ + if (gps_nmea.pos_available) { \ + 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_nmea.msg_available = FALSE; \ + } \ } #define ReadGpsBuffer() { \ diff --git a/sw/airborne/subsystems/gps/gps_sim.h b/sw/airborne/subsystems/gps/gps_sim.h index 4f84766cc5..8a5ab7ea11 100644 --- a/sw/airborne/subsystems/gps/gps_sim.h +++ b/sw/airborne/subsystems/gps/gps_sim.h @@ -11,14 +11,17 @@ extern bool_t gps_available; extern void gps_impl_init(void); -#define GpsEvent(_sol_available_callback) { \ - if (gps_available) { \ - if (gps.fix == GPS_FIX_3D) { \ - gps.last_fix_time = sys_time.nb_sec; \ - } \ - _sol_available_callback(); \ - gps_available = FALSE; \ - } \ +#define GpsEvent(_sol_available_callback) { \ + if (gps_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_available = FALSE; \ + } \ } #endif /* GPS_SIM_H */ diff --git a/sw/airborne/subsystems/gps/gps_sim_nps.h b/sw/airborne/subsystems/gps/gps_sim_nps.h index 9c327590a2..3bf1216932 100644 --- a/sw/airborne/subsystems/gps/gps_sim_nps.h +++ b/sw/airborne/subsystems/gps/gps_sim_nps.h @@ -12,13 +12,17 @@ extern void gps_feed_value(); extern void gps_impl_init(); -#define GpsEvent(_sol_available_callback) { \ - if (gps_available) { \ - if (gps.fix == GPS_FIX_3D) \ - gps.last_fix_time = sys_time.nb_sec; \ - _sol_available_callback(); \ - gps_available = FALSE; \ - } \ +#define GpsEvent(_sol_available_callback) { \ + if (gps_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_available = FALSE; \ + } \ } #endif /* GPS_SIM_NPS_H */ diff --git a/sw/airborne/subsystems/gps/gps_sirf.c b/sw/airborne/subsystems/gps/gps_sirf.c index 17650f8ef3..96d0e165b1 100644 --- a/sw/airborne/subsystems/gps/gps_sirf.c +++ b/sw/airborne/subsystems/gps/gps_sirf.c @@ -159,7 +159,7 @@ void sirf_parse_2(void) { printf("GPS %i %i %i %i\n", ticks, (sys_time.nb_sec - start_time), ticks2, (sys_time.nb_sec - start_time2)); #endif } - else if(sys_time.nb_sec - gps.last_fix_time > 10) { + else if(sys_time.nb_sec - gps.last_3dfix_time > 10) { start_time = sys_time.nb_sec; ticks = 0; } diff --git a/sw/airborne/subsystems/gps/gps_sirf.h b/sw/airborne/subsystems/gps/gps_sirf.h index b5ae016992..02d9fde3f5 100644 --- a/sw/airborne/subsystems/gps/gps_sirf.h +++ b/sw/airborne/subsystems/gps/gps_sirf.h @@ -133,21 +133,23 @@ struct sirf_msg_41 { #define GpsBuffer() GpsLink(ChAvailable()) -#define GpsEvent(_sol_available_callback) { \ - if (GpsBuffer()) { \ - ReadGpsBuffer(); \ - } \ - if (gps_sirf.msg_available) { \ - sirf_parse_msg(); \ - if (gps_sirf.pos_available) { \ - if (gps.fix == GPS_FIX_3D) { \ - gps.last_fix_ticks = sys_time.nb_sec_rem; \ - gps.last_fix_time = sys_time.nb_sec; \ - } \ - _sol_available_callback(); \ - } \ - gps_sirf.msg_available = FALSE; \ - } \ +#define GpsEvent(_sol_available_callback) { \ + if (GpsBuffer()) { \ + ReadGpsBuffer(); \ + } \ + if (gps_sirf.msg_available) { \ + gps.last_msg_ticks = sys_time.nb_sec_rem; \ + gps.last_msg_time = sys_time.nb_sec; \ + sirf_parse_msg(); \ + if (gps_sirf.pos_available) { \ + 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_sirf.msg_available = FALSE; \ + } \ } #define ReadGpsBuffer() { \ diff --git a/sw/airborne/subsystems/gps/gps_skytraq.h b/sw/airborne/subsystems/gps/gps_skytraq.h index a9ab5762e4..ee95b8e233 100644 --- a/sw/airborne/subsystems/gps/gps_skytraq.h +++ b/sw/airborne/subsystems/gps/gps_skytraq.h @@ -69,11 +69,13 @@ extern struct GpsSkytraq gps_skytraq; ReadGpsBuffer(); \ } \ if (gps_skytraq.msg_available) { \ + gps.last_msg_ticks = sys_time.nb_sec_rem; \ + gps.last_msg_time = sys_time.nb_sec; \ gps_skytraq_read_message(); \ if (gps_skytraq.msg_id == SKYTRAQ_ID_NAVIGATION_DATA) { \ if (gps.fix == GPS_FIX_3D) { \ - gps.last_fix_ticks = sys_time.nb_sec_rem; \ - gps.last_fix_time = sys_time.nb_sec; \ + gps.last_3dfix_ticks = sys_time.nb_sec_rem; \ + gps.last_3dfix_time = sys_time.nb_sec; \ } \ _sol_available_callback(); \ } \ diff --git a/sw/airborne/subsystems/gps/gps_ubx.h b/sw/airborne/subsystems/gps/gps_ubx.h index 1c1c08839e..7fc6aeedb6 100644 --- a/sw/airborne/subsystems/gps/gps_ubx.h +++ b/sw/airborne/subsystems/gps/gps_ubx.h @@ -100,25 +100,27 @@ extern struct GpsUbxRaw gps_ubx_raw; * All position/speed messages are sent in one shot and VELNED is the last one on fixedwing * For rotorcraft, only SOL message is needed for pos/speed data */ -#define GpsEvent(_sol_available_callback) { \ - if (GpsBuffer()) { \ - ReadGpsBuffer(); \ - } \ - if (gps_ubx.msg_available) { \ - gps_ubx_read_message(); \ - gps_ubx_ucenter_event(); \ - if (gps_ubx.msg_class == UBX_NAV_ID && \ - (gps_ubx.msg_id == UBX_NAV_VELNED_ID || \ - (gps_ubx.msg_id == UBX_NAV_SOL_ID && \ - gps_ubx.have_velned == 0))) { \ - if (gps.fix == GPS_FIX_3D) { \ - gps.last_fix_ticks = sys_time.nb_sec_rem; \ - gps.last_fix_time = sys_time.nb_sec; \ - } \ - _sol_available_callback(); \ - } \ - gps_ubx.msg_available = FALSE; \ - } \ +#define GpsEvent(_sol_available_callback) { \ + if (GpsBuffer()) { \ + ReadGpsBuffer(); \ + } \ + if (gps_ubx.msg_available) { \ + gps.last_msg_ticks = sys_time.nb_sec_rem; \ + gps.last_msg_time = sys_time.nb_sec; \ + gps_ubx_read_message(); \ + gps_ubx_ucenter_event(); \ + if (gps_ubx.msg_class == UBX_NAV_ID && \ + (gps_ubx.msg_id == UBX_NAV_VELNED_ID || \ + (gps_ubx.msg_id == UBX_NAV_SOL_ID && \ + gps_ubx.have_velned == 0))) { \ + 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_ubx.msg_available = FALSE; \ + } \ } #define ReadGpsBuffer() { \ diff --git a/sw/airborne/subsystems/gps/gps_udp.h b/sw/airborne/subsystems/gps/gps_udp.h index 04e4aeb3d8..ae47c3135e 100644 --- a/sw/airborne/subsystems/gps/gps_udp.h +++ b/sw/airborne/subsystems/gps/gps_udp.h @@ -9,15 +9,18 @@ extern bool_t gps_available; extern void gps_parse(void); -#define GpsEvent(_sol_available_callback) { \ - gps_parse(); \ - if (gps_available) { \ - if (gps.fix == GPS_FIX_3D) { \ - gps.last_fix_time = sys_time.nb_sec; \ - } \ - _sol_available_callback(); \ - gps_available = FALSE; \ - } \ +#define GpsEvent(_sol_available_callback) { \ + gps_parse(); \ + if (gps_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_available = FALSE; \ + } \ }