diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp index 5f8228b1af..018f55afa3 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp @@ -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; } diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp index c5261b30f2..a30edb9f48 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp @@ -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(); } /* diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp index 79f4e0bd6f..08831eb424 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp @@ -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; } }