Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-09 22:49:53 +08:00
Code Issues Actions 28 Packages Projects Releases Wiki Activity

[gps] add a timeout on gps fix lost (#3105)

Browse Source
This commit is contained in:
Gautier Hattenberger
2023-09-25 21:52:32 +02:00
committed by GitHub
parent 4812be6096
commit f4f0cf2f3b
3 changed files with 41 additions and 14 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sw/airborne/modules/gps/gps.c
+11
View File
@@ -277,6 +277,17 @@ void gps_periodic_check(struct GpsState *gps_s)
#endif
}
bool gps_fix_valid(void)
{
bool gps_3d_timeout_valid = false;
#ifdef GPS_FIX_TIMEOUT
if (get_sys_time_float() - gps_time_since_last_3dfix() < GPS_FIX_TIMEOUT) {
gps_3d_timeout_valid = true;
}
#endif
return (gps.fix >= GPS_FIX_3D || gps_3d_timeout_valid);
}
static abi_event gps_ev;
static void gps_cb(uint8_t sender_id,
sw/airborne/modules/gps/gps.h
+27 -11
View File
@@ -43,11 +43,6 @@ extern "C" {
#define GPS_FIX_DGPS 0x04 ///< DGPS fix
#define GPS_FIX_RTK 0x05 ///< RTK GPS fix
#define GpsFixValid() (gps.fix >= GPS_FIX_3D)
#if USE_GPS
#define GpsIsLost() !GpsFixValid()
#endif
#define GPS_VALID_POS_ECEF_BIT 0
#define GPS_VALID_POS_LLA_BIT 1
#define GPS_VALID_POS_UTM_BIT 2
@@ -177,11 +172,32 @@ extern void gps_inject_data(uint8_t packet_id, uint8_t length, uint8_t *data);
extern void gps_parse_GPS_INJECT(uint8_t *buf);
extern void gps_parse_RTCM_INJECT(uint8_t *buf);
/** GPS timeout in seconds */
/** Check if GPS fix is valid.
*
* If GPS_FIX_TIMEOUT is configured, check for 3D fix with timeout,
* otherwise, returns the last value from GPS module
*/
extern bool gps_fix_valid(void);
// Compatibility macros
#define GpsFixValid() gps_fix_valid()
#if USE_GPS
#define GpsIsLost() !GpsFixValid()
#endif
/** GPS timeout in seconds
* in case of communication loss with GPS module
*/
#ifndef GPS_TIMEOUT
#define GPS_TIMEOUT 2
#endif
/** Periodic GPS check.
* Marks GPS as lost when no GPS message was received for GPS_TIMEOUT seconds
*/
extern void gps_periodic_check(struct GpsState *gps_s);
// FIXME not used, to be removed ?
static inline bool gps_has_been_good(void)
{
static bool gps_had_valid_fix = false;
@@ -191,11 +207,11 @@ static inline bool gps_has_been_good(void)
return gps_had_valid_fix;
}
/** Periodic GPS check.
* Marks GPS as lost when no GPS message was received for GPS_TIMEOUT seconds
*/
extern void gps_periodic_check(struct GpsState *gps_s);
/** Returns the time since last 3D fix in seconds (float) */
static inline float gps_time_since_last_3dfix(void)
{
return (float)(gps.last_3dfix_time + (float)(gps.last_3dfix_ticks) / sys_time.cpu_ticks_per_sec);
}
/*
sw/airborne/modules/gps/gps_ubx.c
+3 -3
View File
@@ -166,7 +166,7 @@ static void gps_ubx_parse_nav_pvt(void)
gps_ubx.state.fix = 4; // dgnss
} else if(gnssFixOK) {
gps_ubx.state.fix = 3; // 3D
} else{
} else {
gps_ubx.state.fix = 0;
}
@@ -397,14 +397,14 @@ static void gps_ubx_parse_nav_relposned(void)
uint8_t gnssFixOK = RTCMgetbitu(&flags, 7, 1);
/* Only save the latest valid relative position */
if(relPosValid) {
if (relPosValid) {
if (diffSoln && carrSoln == 2) {
gps_ubx.state.fix = 5; // rtk
} else if(diffSoln && carrSoln == 1) {
gps_ubx.state.fix = 4; // dgnss
} else if(gnssFixOK) {
gps_ubx.state.fix = 3; // 3D
} else{
} else {
gps_ubx.state.fix = 0;
}