Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_GPS: fixed vertical velocity with AGRICA support 

and add health check for unicore GPS
This commit is contained in:
Andrew Tridgell 2022-11-28 11:00:24 +11:00
parent 55683c51f5
commit c453ff1ce2
2 changed files with 33 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

30
libraries/AP_GPS/AP_GPS_NMEA.cpp
View File

@ -447,6 +447,9 @@ bool AP_GPS_NMEA::_term_complete()
case _GPS_SENTENCE_AGRICA: {
const auto &ag = _agrica;
_last_AGRICA_ms = now;
_last_vvelocity_ms = now;
_last_vaccuracy_ms = now;
_last_3D_velocity_ms = now;
state.location.lat = ag.lat*1.0e7;
state.location.lng = ag.lng*1.0e7;
state.location.alt = ag.alt*1.0e2;
@ -796,4 +799,31 @@ void AP_GPS_NMEA::send_config(void)
}
}
/*
return health status
*/
bool AP_GPS_NMEA::is_healthy(void) const
{
switch (get_type()) {
#if AP_GPS_NMEA_UNICORE_ENABLED
case AP_GPS::GPS_TYPE_UNICORE_MOVINGBASE_NMEA:
case AP_GPS::GPS_TYPE_UNICORE_NMEA:
// we should be getting AGRICA messages
return _last_AGRICA_ms != 0;
#endif // AP_GPS_NMEA_UNICORE_ENABLED
case AP_GPS::GPS_TYPE_HEMI:
// we should be getting HDR for yaw
return _last_yaw_ms != 0;
case AP_GPS::GPS_TYPE_ALLYSTAR:
// we should get vertical velocity and accuracy from PHD
return _last_vvelocity_ms != 0 && _last_vaccuracy_ms != 0;
default:
break;
}
return true;
}
#endif // AP_GPS_NMEA_ENABLED

3
libraries/AP_GPS/AP_GPS_NMEA.h
View File

@ -66,6 +66,9 @@ public:
const char *name() const override { return "NMEA"; }
// driver specific health, returns true if the driver is healthy
bool is_healthy(void) const override;
private:
/// Coding for the GPS sentences that the parser handles
enum _sentence_types : uint16_t { //there are some more than 10 fields in some sentences , thus we have to increase these value.