Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-22 16:48:29 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF: cope with compass going offline while in flight

Browse Source
This commit is contained in:
Andrew Tridgell 2014-03-02 14:32:10 +11:00
parent 77f91e6250
commit 449d09051e
2 changed files with 14 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
libraries/AP_NavEKF/AP_NavEKF.cpp
View File

@ -363,7 +363,7 @@ void NavEKF::InitialiseFilterDynamic(void)
float magHeading = atan2f(initMagNED.y, initMagNED.x);
// get the magnetic declination
float magDecAng = _ahrs->get_compass()->get_declination();
float magDecAng = use_compass() ? _ahrs->get_compass()->get_declination() : 0;
// calculate yaw angle rel to true north
yaw = magDecAng - magHeading;
@ -439,7 +439,7 @@ void NavEKF::InitialiseFilterBootstrap(void)
float magHeading = atan2f(initMagNED.y, initMagNED.x);
// get the magnetic declination
float magDecAng = _ahrs->get_compass()->get_declination();
float magDecAng = use_compass() ? _ahrs->get_compass()->get_declination() : 0;
// calculate yaw angle rel to true north
yaw = magDecAng - magHeading;
@ -2630,7 +2630,7 @@ void NavEKF::readHgtData()
void NavEKF::readMagData()
{
// scale compass data to improve numerical conditioning
if (_ahrs->get_compass()->last_update != lastMagUpdate) {
if (use_compass() && _ahrs->get_compass()->last_update != lastMagUpdate) {
lastMagUpdate = _ahrs->get_compass()->last_update;
// Body fixed magnetic bias is opposite sign to APM compass offsets
magBias = -_ahrs->get_compass()->get_offsets() * 0.001f;
@ -2800,4 +2800,12 @@ bool NavEKF::static_mode_demanded(void) const
return !_ahrs->get_armed() || !_ahrs->get_correct_centrifugal();
}
/*
see if we should use the compass
*/
bool NavEKF::use_compass(void) const
{
return _ahrs->get_compass() && _ahrs->get_compass()->use_for_yaw();
}
#endif // HAL_CPU_CLASS

3
libraries/AP_NavEKF/AP_NavEKF.h
View File

@ -455,6 +455,9 @@ private:
perf_counter_t _perf_FuseMagnetometer;
perf_counter_t _perf_FuseAirspeed;
#endif
// should we use the compass?
bool use_compass(void) const;
};
#if CONFIG_HAL_BOARD != HAL_BOARD_PX4