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

AP_AHRS: debounce the GPS/compass consistency test 

we will only consider the compass inconsistent with GPS if it is off
for more than 2 seconds
This commit is contained in:
Andrew Tridgell 2013-10-22 13:06:27 +11:00
parent 6f3458eb25
commit d305dd5946
3 changed files with 11 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
libraries/AP_AHRS/AP_AHRS.h
View File

@ -200,7 +200,7 @@ public:
}
// return true if we will use compass for yaw
virtual bool use_compass(void) const { return _compass && _compass->use_for_yaw(); }
virtual bool use_compass(void) { return _compass && _compass->use_for_yaw(); }
// return true if yaw has been initialised
bool yaw_initialised(void) const {

9
libraries/AP_AHRS/AP_AHRS_DCM.cpp
View File

@ -289,7 +289,7 @@ bool AP_AHRS_DCM::have_gps(void) const
}
// return true if we should use the compass for yaw correction
bool AP_AHRS_DCM::use_compass(void) const
bool AP_AHRS_DCM::use_compass(void)
{
if (!_compass || !_compass->use_for_yaw()) {
// no compass available
@ -310,9 +310,14 @@ bool AP_AHRS_DCM::use_compass(void) const
// prevent flyaways with very bad compass offsets
int32_t error = abs(wrap_180_cd(yaw_sensor - _gps->ground_course_cd));
if (error > 4500 && _wind.length() < _gps->ground_speed_cm*0.008f) {
// start using the GPS for heading
if (hal.scheduler->millis() - _last_consistent_heading > 2000) {
// start using the GPS for heading if the compass has been
// inconsistent with the GPS for 2 seconds
return false;
}
} else {
_last_consistent_heading = hal.scheduler->millis();
}
// use the compass
return true;

3
libraries/AP_AHRS/AP_AHRS_DCM.h
View File

@ -71,7 +71,7 @@ public:
// if we have an estimate
bool airspeed_estimate(float *airspeed_ret);
bool use_compass(void) const;
bool use_compass(void);
private:
float _ki;
@ -147,6 +147,7 @@ private:
Vector3f _last_vel;
uint32_t _last_wind_time;
float _last_airspeed;
uint32_t _last_consistent_heading;
// estimated wind in m/s
Vector3f _wind;