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

AP_NavEKF2: fixed an assumption that _ahrs->get_compass() always works 

if MAG_ENABLE=0 in plane there is no compass
This commit is contained in:
Andrew Tridgell 2016-05-19 14:16:40 +10:00
parent 71e10c9132
commit 4080c6091d
2 changed files with 7 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp
View File

@ -145,6 +145,10 @@ void NavEKF2_core::writeOptFlowMeas(uint8_t &rawFlowQuality, Vector2f &rawFlowRa
// check for new magnetometer data and update store measurements if available
void NavEKF2_core::readMagData()
{
if (!_ahrs->get_compass()) {
allMagSensorsFailed = true;
return;
}
// If we are a vehicle with a sideslip constraint to aid yaw estimation and we have timed out on our last avialable
// magnetometer, then declare the magnetometers as failed for this flight
uint8_t maxCount = _ahrs->get_compass()->get_count();

4
libraries/AP_NavEKF2/AP_NavEKF2_core.cpp
View File

@ -243,7 +243,9 @@ void NavEKF2_core::InitialiseVariables()
posResetNE.zero();
velResetNE.zero();
hgtInnovFiltState = 0.0f;
magSelectIndex = _ahrs->get_compass()->get_primary();
if (_ahrs->get_compass()) {
magSelectIndex = _ahrs->get_compass()->get_primary();
}
imuDataDownSampledNew.delAng.zero();
imuDataDownSampledNew.delVel.zero();
imuDataDownSampledNew.delAngDT = 0.0f;