Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-06-06 16:58:48 +08:00
Code Issues Actions 25 Packages Projects Releases Wiki Activity

[gps] add gps_periodic_check to reset fix after timeout

Browse Source 
- GpsIsLost should eventually be replaced by isLocal/GlobalCoordinateSystemValid from state interface

closes #637
This commit is contained in:
Felix Ruess
2014-02-13 15:37:07 +01:00
parent 7d5c736c9d
commit e7f7192d29
17 changed files with 170 additions and 122 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sw/airborne/firmwares/fixedwing/main_ap.c
+5 -1
View File
@@ -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();
}
sw/airborne/firmwares/rotorcraft/main.c
+1
View File
@@ -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
sw/airborne/modules/ins/ins_xsens.h
+4 -2
View File
@@ -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; \
sw/airborne/modules/ins/ins_xsens700.c
+4 -2
View File
@@ -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));
sw/airborne/subsystems/gps.c
+11
View File
@@ -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;
sw/airborne/subsystems/gps.h
+13 -8
View File
@@ -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
sw/airborne/subsystems/gps/gps_ardrone2.h
+9 -7
View File
@@ -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);
sw/airborne/subsystems/gps/gps_mtk.c
+2 -2
View File
@@ -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;
sw/airborne/subsystems/gps/gps_mtk.h
+27 -24
View File
@@ -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() { \
sw/airborne/subsystems/gps/gps_nmea.h
+17 -15
View File
@@ -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() { \
sw/airborne/subsystems/gps/gps_sim.h
+11 -8
View File
@@ -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 */
sw/airborne/subsystems/gps/gps_sim_nps.h
+11 -7
View File
@@ -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 */
sw/airborne/subsystems/gps/gps_sirf.c
+1 -1
View File
@@ -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;
}
sw/airborne/subsystems/gps/gps_sirf.h
+17 -15
View File
@@ -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() { \
sw/airborne/subsystems/gps/gps_skytraq.h
+4 -2
View File
@@ -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(); \
} \
sw/airborne/subsystems/gps/gps_ubx.h
+21 -19
View File
@@ -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() { \
sw/airborne/subsystems/gps/gps_udp.h
+12 -9
View File
@@ -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; \
} \
}