Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 14:38:30 -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
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
void NavEKF2_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();
@ -627,8 +627,9 @@ void NavEKF2_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()) {
table_earth_field_ga = AP_Declination::get_earth_field_ga(gpsloc).toftype();
table_declination = radians(AP_Declination::get_declination(gpsloc.lat*1.0e-7,
gpsloc.lng*1.0e-7));
@ -984,7 +985,7 @@ ftype NavEKF2_core::MagDeclination(void) const
if (!use_compass()) {
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
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;
}
@ -388,12 +389,12 @@ bool NavEKF2_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*1000.0).tofloat();
magOffsets = compass.get_offsets(magSelectIndex) - (stateStruct.body_magfield*1000.0).tofloat();
return true;
} else {
magOffsets = dal.get_compass()->get_offsets(magSelectIndex);
magOffsets = compass.get_offsets(magSelectIndex);
return false;
}
}