diff --git a/sw/airborne/gps.c b/sw/airborne/gps.c index cd1a303ba3..f415a1b48b 100644 --- a/sw/airborne/gps.c +++ b/sw/airborne/gps.c @@ -46,6 +46,10 @@ uint16_t last_gps_msg_t; /** cputime of the last gps message */ bool_t gps_verbose_downlink; +#ifdef UGEAR +volatile uint16_t gps_msg_received_counter; +#endif + void gps_downlink( void ) { static uint8_t _4Hz; _4Hz++; if (_4Hz > 3) _4Hz = 0; diff --git a/sw/airborne/gps.h b/sw/airborne/gps.h index db18f3e048..9d1e882229 100644 --- a/sw/airborne/gps.h +++ b/sw/airborne/gps.h @@ -74,6 +74,10 @@ extern volatile uint8_t gps_msg_received; extern bool_t gps_pos_available; extern uint8_t gps_nb_ovrn; +#ifdef UGEAR +extern volatile uint16_t gps_msg_received_counter; +#endif + /** Number of scanned satellites */ extern uint8_t gps_nb_channels; @@ -120,6 +124,15 @@ extern struct svinfo gps_svinfos[GPS_NB_CHANNELS]; gps_downlink(); \ } +#ifdef UGEAR +#define UseGpsPosUgear(_callback) { \ + if (GpsFixValid()) { \ + last_gps_msg_t = cpu_time_sec; \ + _callback(); \ + GpsToggleLed(); \ + } \ + } +#endif #ifdef GPS_CONFIGURE #define GpsParseOrConfigure() { \ diff --git a/sw/airborne/main_ap.c b/sw/airborne/main_ap.c index fe5f0f7edb..5a7c4ec253 100644 --- a/sw/airborne/main_ap.c +++ b/sw/airborne/main_ap.c @@ -738,9 +738,15 @@ void event_task_ap( void ) { parse_ugear_msg(); ugear_msg_received = FALSE; if (gps_pos_available){ - gps_downlink(); + //gps_downlink(); gps_verbose_downlink = !launch; - UseGpsPos(estimator_update_state_gps); +// UseGpsPos(estimator_update_state_gps); + UseGpsPosUgear(estimator_update_state_gps); + gps_msg_received_counter = gps_msg_received_counter+1; + if (gps_msg_received_counter == 25){ + gps_downlink(); + gps_msg_received_counter = 0; + } gps_pos_available = FALSE; } }