diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_Control.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_Control.cpp index c575b8b5c0..b0c7a8b559 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_Control.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_Control.cpp @@ -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; } /* diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp index b7aeec6440..ad473751fc 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp @@ -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(); } /* diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp index 21bcd3eb8b..aedf27f06d 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp @@ -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; } }