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

AP_NavEKF2: stop using AHRS as conduit for Compass pointer

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

2
libraries/AP_NavEKF2/AP_NavEKF2_Control.cpp
View File

@ -408,7 +408,7 @@ bool NavEKF2_core::readyToUseExtNav(void) const
// return true if we should use the compass // return true if we should use the compass
bool NavEKF2_core::use_compass(void) const bool NavEKF2_core::use_compass(void) const
{ {
return dal.get_compass() && dal.get_compass()->use_for_yaw(magSelectIndex) && !allMagSensorsFailed; return dal.compass().use_for_yaw(magSelectIndex) && !allMagSensorsFailed;
} }
/* /*

13
libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp
View File

@ -228,13 +228,13 @@ void NavEKF2_core::tryChangeCompass(void)
// check for new magnetometer data and update store measurements if available // check for new magnetometer data and update store measurements if available
void NavEKF2_core::readMagData() void NavEKF2_core::readMagData()
{ {
if (!dal.get_compass()) { const auto &compass = dal.compass();
if (!compass.available()) {
allMagSensorsFailed = true; allMagSensorsFailed = true;
return; 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 // 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 // magnetometer, then declare the magnetometers as failed for this flight
const uint8_t maxCount = compass.get_count(); const uint8_t maxCount = compass.get_count();
@ -627,8 +627,9 @@ void NavEKF2_core::readGpsData()
} }
if (gpsGoodToAlign && !have_table_earth_field) { if (gpsGoodToAlign && !have_table_earth_field) {
const auto *compass = dal.get_compass(); const auto &compass = dal.compass();
if (compass && compass->have_scale_factor(magSelectIndex) && compass->auto_declination_enabled()) { if (compass.have_scale_factor(magSelectIndex) &&
compass.auto_declination_enabled()) {
table_earth_field_ga = AP_Declination::get_earth_field_ga(gpsloc).toftype(); table_earth_field_ga = AP_Declination::get_earth_field_ga(gpsloc).toftype();
table_declination = radians(AP_Declination::get_declination(gpsloc.lat*1.0e-7, table_declination = radians(AP_Declination::get_declination(gpsloc.lat*1.0e-7,
gpsloc.lng*1.0e-7)); gpsloc.lng*1.0e-7));
@ -984,7 +985,7 @@ ftype NavEKF2_core::MagDeclination(void) const
if (!use_compass()) { if (!use_compass()) {
return 0; return 0;
} }
return dal.get_compass()->get_declination(); return dal.compass().get_declination();
} }
/* /*

9
libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp
View File

@ -377,7 +377,8 @@ void NavEKF2_core::getMagXYZ(Vector3f &magXYZ) const
// return true if offsets are valid // return true if offsets are valid
bool NavEKF2_core::getMagOffsets(uint8_t mag_idx, Vector3f &magOffsets) const bool NavEKF2_core::getMagOffsets(uint8_t mag_idx, Vector3f &magOffsets) const
{ {
if (!dal.get_compass()) { const auto &compass = dal.compass();
if (!compass.available()) {
return false; return false;
} }
@ -388,12 +389,12 @@ bool NavEKF2_core::getMagOffsets(uint8_t mag_idx, Vector3f &magOffsets) const
if ((mag_idx == magSelectIndex) && if ((mag_idx == magSelectIndex) &&
finalInflightMagInit && finalInflightMagInit &&
!inhibitMagStates && !inhibitMagStates &&
dal.get_compass()->healthy(magSelectIndex) && compass.healthy(magSelectIndex) &&
variancesConverged) { variancesConverged) {
magOffsets = dal.get_compass()->get_offsets(magSelectIndex) - (stateStruct.body_magfield*1000.0).tofloat(); magOffsets = compass.get_offsets(magSelectIndex) - (stateStruct.body_magfield*1000.0).tofloat();
return true; return true;
} else { } else {
magOffsets = dal.get_compass()->get_offsets(magSelectIndex); magOffsets = compass.get_offsets(magSelectIndex);
return false; return false;
} }
} }