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

AP_NavEKF3: stop using AHRS as conduit for Compass pointer

This commit is contained in:
Peter Barker 2021-07-26 10:32:12 +10:00 committed by Peter Barker
parent dec4ba86b6
commit c34284b6eb
3 changed files with 19 additions and 20 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp
View File

@ -567,9 +567,8 @@ bool NavEKF3_core::use_compass(void) const
return false;
}
const auto *compass = dal.get_compass();
return compass &&
compass->use_for_yaw(magSelectIndex) &&
const auto &compass = dal.compass();
return compass.use_for_yaw(magSelectIndex) &&
!allMagSensorsFailed;
}

24
libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp
View File

@ -283,13 +283,13 @@ void NavEKF3_core::tryChangeCompass(void)
// check for new magnetometer data and update store measurements if available
void NavEKF3_core::readMagData()
{
if (!dal.get_compass()) {
const auto &compass = dal.compass();
if (!compass.available()) {
allMagSensorsFailed = true;
return;
}
const auto &compass = dal.compass();
// 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
const uint8_t maxCount = compass.get_count();
@ -678,8 +678,9 @@ void NavEKF3_core::readGpsData()
}
if (gpsGoodToAlign && !have_table_earth_field) {
const auto *compass = dal.get_compass();
if (compass && compass->have_scale_factor(magSelectIndex) && compass->auto_declination_enabled()) {
const auto &compass = dal.compass();
if (compass.have_scale_factor(magSelectIndex) &&
compass.auto_declination_enabled()) {
getEarthFieldTable(gpsloc);
if (frontend->_mag_ef_limit > 0) {
// initialise earth field from tables
@ -1139,15 +1140,12 @@ void NavEKF3_core::update_gps_selection(void)
*/
void NavEKF3_core::update_mag_selection(void)
{
const auto *compass = dal.get_compass();
if (compass == nullptr) {
return;
}
const auto &compass = dal.compass();
if (frontend->_affinity & EKF_AFFINITY_MAG) {
if (core_index < compass->get_count() &&
compass->healthy(core_index) &&
compass->use_for_yaw(core_index)) {
if (core_index < compass.get_count() &&
compass.healthy(core_index) &&
compass.use_for_yaw(core_index)) {
// use core_index compass if it is healthy
magSelectIndex = core_index;
}
@ -1322,7 +1320,7 @@ ftype NavEKF3_core::MagDeclination(void) const
if (!use_compass()) {
return 0;
}
return dal.get_compass()->get_declination();
return dal.compass().get_declination();
}
/*

10
libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp
View File

@ -377,9 +377,11 @@ void NavEKF3_core::getMagXYZ(Vector3f &magXYZ) const
// return true if offsets are valid
bool NavEKF3_core::getMagOffsets(uint8_t mag_idx, Vector3f &magOffsets) const
{
if (!dal.get_compass()) {
const auto &compass = dal.compass();
if (!compass.available()) {
return false;
}
// compass offsets are valid if we have finalised magnetic field initialisation, magnetic field learning is not prohibited,
// primary compass is valid and state variances have converged
const float maxMagVar = 5E-6f;
@ -387,12 +389,12 @@ bool NavEKF3_core::getMagOffsets(uint8_t mag_idx, Vector3f &magOffsets) const
if ((mag_idx == magSelectIndex) &&
finalInflightMagInit &&
!inhibitMagStates &&
dal.get_compass()->healthy(magSelectIndex) &&
compass.healthy(magSelectIndex) &&
variancesConverged) {
magOffsets = dal.get_compass()->get_offsets(magSelectIndex) - stateStruct.body_magfield.tofloat()*1000.0;
magOffsets = compass.get_offsets(magSelectIndex) - stateStruct.body_magfield.tofloat()*1000.0;
return true;
} else {
magOffsets = dal.get_compass()->get_offsets(magSelectIndex);
magOffsets = compass.get_offsets(magSelectIndex);
return false;
}
}