AP_NavEKF3: use dal reference in EKF backends

saves a bit of flash space

libraries/AP_NavEKF3/AP_NavEKF3_AirDataFusion.cpp

@@ -25,7 +25,7 @@ void NavEKF3_core::FuseAirspeed()
     float vd;
     float vwn;
     float vwe;
-    float EAS2TAS = AP::dal().get_EAS2TAS();
+    float EAS2TAS = dal.get_EAS2TAS();
     const float R_TAS = sq(constrain_float(frontend->_easNoise, 0.5f, 5.0f) * constrain_float(EAS2TAS, 0.9f, 10.0f));
     float SH_TAS[3];
     float SK_TAS[2];

libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp

@@ -11,7 +11,7 @@ void NavEKF3_core::controlFilterModes()
 {
     // Determine motor arm status
     prevMotorsArmed = motorsArmed;
-    motorsArmed = AP::dal().get_armed();
+    motorsArmed = dal.get_armed();
     if (motorsArmed && !prevMotorsArmed) {
         // set the time at which we arm to assist with checks
         timeAtArming_ms =  imuSampleTime_ms;
@@ -72,7 +72,7 @@ void NavEKF3_core::setWindMagStateLearningMode()
 
             // set the wind sate variances to the measurement uncertainty
             for (uint8_t index=22; index<=23; index++) {
-                P[index][index] = sq(constrain_float(frontend->_easNoise, 0.5f, 5.0f) * constrain_float(AP::dal().get_EAS2TAS(), 0.9f, 10.0f));
+                P[index][index] = sq(constrain_float(frontend->_easNoise, 0.5f, 5.0f) * constrain_float(dal.get_EAS2TAS(), 0.9f, 10.0f));
             }
         } else {
             // set the variances using a typical wind speed
@@ -425,7 +425,7 @@ void NavEKF3_core::checkAttitudeAlignmentStatus()
 // return true if we should use the airspeed sensor
 bool NavEKF3_core::useAirspeed(void) const
 {
-    return AP::dal().airspeed_sensor_enabled();
+    return dal.airspeed_sensor_enabled();
 }
 
 // return true if we should use the range finder sensor
@@ -479,7 +479,7 @@ bool NavEKF3_core::readyToUseExtNav(void) const
 // return true if we should use the compass
 bool NavEKF3_core::use_compass(void) const
 {
-    const auto *compass  = AP::dal().get_compass();
+    const auto *compass  = dal.get_compass();
     return effectiveMagCal != MagCal::EXTERNAL_YAW &&
            compass &&
            compass->use_for_yaw(magSelectIndex) &&
@@ -503,7 +503,7 @@ bool NavEKF3_core::assume_zero_sideslip(void) const
     // we don't assume zero sideslip for ground vehicles as EKF could
     // be quite sensitive to a rapid spin of the ground vehicle if
     // traction is lost
-    return AP::dal().get_fly_forward() && AP::dal().get_vehicle_class() != AP_DAL::VehicleClass::GROUND;
+    return dal.get_fly_forward() && dal.get_vehicle_class() != AP_DAL::VehicleClass::GROUND;
 }
 
 // set the LLH location of the filters NED origin
@@ -626,7 +626,7 @@ void NavEKF3_core::runYawEstimatorPrediction()
             if (imuDataDelayed.time_ms - tasDataDelayed.time_ms < 5000) {
                 trueAirspeed = tasDataDelayed.tas;
             } else {
-                trueAirspeed = defaultAirSpeed * AP::dal().get_EAS2TAS();
+                trueAirspeed = defaultAirSpeed * dal.get_EAS2TAS();
             }
         } else {
             trueAirspeed = 0.0f;

libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp

@@ -1474,7 +1474,7 @@ bool NavEKF3_core::EKFGSF_resetMainFilterYaw()
         EKFGSF_yaw_reset_ms = imuSampleTime_ms;
         EKFGSF_yaw_reset_count++;
 
-        if (!use_compass() || AP::dal().compass().get_num_enabled() == 0) {
+        if (!use_compass() || dal.compass().get_num_enabled() == 0) {
             GCS_SEND_TEXT(MAV_SEVERITY_INFO, "EKF3 IMU%u yaw aligned using GPS",(unsigned)imu_index);
         } else {
             GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "EKF3 IMU%u emergency yaw reset",(unsigned)imu_index);

libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp

@@ -18,7 +18,7 @@ void NavEKF3_core::readRangeFinder(void)
     uint8_t minIndex;
     // get theoretical correct range when the vehicle is on the ground
     // don't allow range to go below 5cm because this can cause problems with optical flow processing
-    const auto *_rng = AP::dal().rangefinder();
+    const auto *_rng = dal.rangefinder();
     if (_rng == nullptr) {
         return;
     }
@@ -243,7 +243,7 @@ void NavEKF3_core::writeOptFlowMeas(const uint8_t rawFlowQuality, const Vector2f
 // try changing compass, return true if a new compass is found
 void NavEKF3_core::tryChangeCompass(void)
 {
-    const auto &compass = AP::dal().compass();
+    const auto &compass = dal.compass();
     const uint8_t maxCount = compass.get_count();
 
     // search through the list of magnetometers
@@ -280,12 +280,12 @@ void NavEKF3_core::tryChangeCompass(void)
 // check for new magnetometer data and update store measurements if available
 void NavEKF3_core::readMagData()
 {
-    if (!AP::dal().get_compass()) {
+    if (!dal.get_compass()) {
         allMagSensorsFailed = true;
         return;        
     }
 
-    const auto &compass = AP::dal().compass();
+    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
@@ -380,7 +380,7 @@ void NavEKF3_core::readMagData()
  */
 void NavEKF3_core::readIMUData()
 {
-    const auto &ins = AP::dal().ins();
+    const auto &ins = dal.ins();
 
     // calculate an averaged IMU update rate using a spike and lowpass filter combination
     dtIMUavg = 0.02f * constrain_float(ins.get_loop_delta_t(),0.5f * dtIMUavg, 2.0f * dtIMUavg) + 0.98f * dtIMUavg;
@@ -521,7 +521,7 @@ void NavEKF3_core::readIMUData()
 // read the delta velocity and corresponding time interval from the IMU
 // return false if data is not available
 bool NavEKF3_core::readDeltaVelocity(uint8_t ins_index, Vector3f &dVel, float &dVel_dt) {
-    const auto &ins = AP::dal().ins();
+    const auto &ins = dal.ins();
 
     if (ins_index < ins.get_accel_count()) {
         ins.get_delta_velocity(ins_index,dVel);
@@ -540,7 +540,7 @@ void NavEKF3_core::readGpsData()
 {
     // check for new GPS data
     // limit update rate to avoid overflowing the FIFO buffer
-    const auto &gps = AP::dal().gps();
+    const auto &gps = dal.gps();
 
         if (gps.last_message_time_ms(selected_gps) - lastTimeGpsReceived_ms > frontend->sensorIntervalMin_ms) {
             if (gps.status(selected_gps) >= AP_DAL_GPS::GPS_OK_FIX_3D) {
@@ -653,7 +653,7 @@ void NavEKF3_core::readGpsData()
             }
 
             if (gpsGoodToAlign && !have_table_earth_field) {
-                const auto *compass = AP::dal().get_compass();
+                const auto *compass = dal.get_compass();
                 if (compass && compass->have_scale_factor(magSelectIndex) && compass->auto_declination_enabled()) {
                     table_earth_field_ga = AP_Declination::get_earth_field_ga(gpsloc);
                     table_declination = radians(AP_Declination::get_declination(gpsloc.lat*1.0e-7,
@@ -681,7 +681,7 @@ void NavEKF3_core::readGpsData()
 
             // if the GPS has yaw data then input that as well
             float yaw_deg, yaw_accuracy_deg;
-            if (AP::dal().gps().gps_yaw_deg(selected_gps, yaw_deg, yaw_accuracy_deg)) {
+            if (dal.gps().gps_yaw_deg(selected_gps, yaw_deg, yaw_accuracy_deg)) {
                 // GPS modules are rather too optimistic about their
                 // accuracy. Set to min of 5 degrees here to prevent
                 // the user constantly receiving warnings about high
@@ -694,7 +694,7 @@ void NavEKF3_core::readGpsData()
         } else {
             // report GPS fix status
             gpsCheckStatus.bad_fix = true;
-            AP::dal().snprintf(prearm_fail_string, sizeof(prearm_fail_string), "Waiting for 3D fix");
+            dal.snprintf(prearm_fail_string, sizeof(prearm_fail_string), "Waiting for 3D fix");
         }
     }
 }
@@ -702,7 +702,7 @@ void NavEKF3_core::readGpsData()
 // read the delta angle and corresponding time interval from the IMU
 // return false if data is not available
 bool NavEKF3_core::readDeltaAngle(uint8_t ins_index, Vector3f &dAng) {
-    const auto &ins = AP::dal().ins();
+    const auto &ins = dal.ins();
 
     if (ins_index < ins.get_gyro_count()) {
         ins.get_delta_angle(ins_index,dAng);
@@ -721,7 +721,7 @@ void NavEKF3_core::readBaroData()
 {
     // check to see if baro measurement has changed so we know if a new measurement has arrived
     // limit update rate to avoid overflowing the FIFO buffer
-    const auto &baro = AP::dal().baro();
+    const auto &baro = dal.baro();
     if (baro.get_last_update(selected_baro) - lastBaroReceived_ms > frontend->sensorIntervalMin_ms) {
 
         baroDataNew.hgt = baro.get_altitude(selected_baro);
@@ -809,11 +809,11 @@ void NavEKF3_core::readAirSpdData()
     // if airspeed reading is valid and is set by the user to be used and has been updated then
     // we take a new reading, convert from EAS to TAS and set the flag letting other functions
     // know a new measurement is available
-    const auto *airspeed = AP::dal().airspeed();
+    const auto *airspeed = dal.airspeed();
     if (airspeed &&
         airspeed->use(selected_airspeed) &&
         (airspeed->last_update_ms(selected_airspeed) - timeTasReceived_ms) > frontend->sensorIntervalMin_ms) {
-        tasDataNew.tas = airspeed->get_airspeed(selected_airspeed) * AP::dal().get_EAS2TAS();
+        tasDataNew.tas = airspeed->get_airspeed(selected_airspeed) * dal.get_EAS2TAS();
         timeTasReceived_ms = airspeed->last_update_ms(selected_airspeed);
         tasDataNew.time_ms = timeTasReceived_ms - frontend->tasDelay_ms;
 
@@ -839,7 +839,7 @@ void NavEKF3_core::readRngBcnData()
     static_assert(ARRAY_SIZE(rngBcnFusionReport) >= AP_BEACON_MAX_BEACONS, "rngBcnFusionReport should have at least AP_BEACON_MAX_BEACONS elements");
 
     // get the location of the beacon data
-    const AP_DAL_Beacon *beacon = AP::dal().beacon();
+    const AP_DAL_Beacon *beacon = dal.beacon();
 
     // exit immediately if no beacon object
     if (beacon == nullptr) {
@@ -1054,7 +1054,7 @@ void NavEKF3_core::writeExtNavVelData(const Vector3f &vel, float err, uint32_t t
  */
 void NavEKF3_core::update_gps_selection(void)
 {
-    const auto &gps = AP::dal().gps();
+    const auto &gps = dal.gps();
 
     // in normal operation use the primary GPS
     selected_gps = gps.primary_sensor();
@@ -1079,7 +1079,7 @@ void NavEKF3_core::update_gps_selection(void)
  */
 void NavEKF3_core::update_mag_selection(void)
 {
-    const auto *compass = AP::dal().get_compass();
+    const auto *compass = dal.get_compass();
     if (compass == nullptr) {
         return;
     }
@@ -1099,7 +1099,7 @@ void NavEKF3_core::update_mag_selection(void)
  */
 void NavEKF3_core::update_baro_selection(void)
 {
-    auto &baro = AP::dal().baro();
+    auto &baro = dal.baro();
 
     // in normal operation use the primary baro
     selected_baro = baro.get_primary();
@@ -1118,7 +1118,7 @@ void NavEKF3_core::update_baro_selection(void)
  */
 void NavEKF3_core::update_airspeed_selection(void)
 {
-    const auto *arsp = AP::dal().airspeed();
+    const auto *arsp = dal.airspeed();
     if (arsp == nullptr) {
         return;
     }
@@ -1188,7 +1188,7 @@ void NavEKF3_core::getTimingStatistics(struct ekf_timing &_timing)
  */
 void NavEKF3_core::learnInactiveBiases(void)
 {
-    const auto &ins = AP::dal().ins();
+    const auto &ins = dal.ins();
 
     // learn gyro biases
     for (uint8_t i=0; i<INS_MAX_INSTANCES; i++) {
@@ -1264,7 +1264,7 @@ float NavEKF3_core::MagDeclination(void) const
     if (!use_compass()) {
         return 0;
     }
-    return AP::dal().get_compass()->get_declination();
+    return dal.get_compass()->get_declination();
 }
 
 /*
@@ -1289,7 +1289,7 @@ void NavEKF3_core::updateMovementCheck(void)
     const float dtEkfAvgInv = 1.0f / dtEkfAvg;
 
     // get latest bias corrected gyro and accelerometer data
-    const auto &ins = AP::dal().ins();
+    const auto &ins = dal.ins();
     Vector3f gyro = ins.get_gyro(gyro_index_active) - stateStruct.gyro_bias * dtEkfAvgInv;
     Vector3f accel = ins.get_accel(accel_index_active) - stateStruct.accel_bias * dtEkfAvgInv;
 
@@ -1339,7 +1339,7 @@ void NavEKF3_core::updateMovementCheck(void)
         lastMoveCheckLogTime_ms = imuSampleTime_ms;
         const struct log_XKFM pkt{
             LOG_PACKET_HEADER_INIT(LOG_XKFM_MSG),
-            time_us            : AP::dal().micros64(),
+            time_us            : dal.micros64(),
             core               : core_index,
             ongroundnotmoving  : onGroundNotMoving,
             gyro_length_ratio  : gyro_length_ratio,

libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp

@@ -45,7 +45,7 @@ float NavEKF3_core::errorScore() const
         // a better one. This only comes into effect for a forward flight vehicle. A sensitivity factor of 0.3 is added to keep the
         // EKF less sensitive to innovations arising due events like strong gusts of wind, thus, prevent reporting high error scores
         if (assume_zero_sideslip()) {
-            const auto *arsp = AP::dal().airspeed();
+            const auto *arsp = dal.airspeed();
             if (arsp->get_num_sensors() >= 2 && (frontend->_affinity & EKF_AFFINITY_ARSP)) {
                 score = MAX(score, 0.3f * tasTestRatio);
             }
@@ -121,7 +121,7 @@ bool NavEKF3_core::getHeightControlLimit(float &height) const
     // only ask for limiting if we are doing optical flow navigation
     if (frontend->_fusionModeGPS == 3 && (PV_AidingMode == AID_RELATIVE) && flowDataValid) {
         // If are doing optical flow nav, ensure the height above ground is within range finder limits after accounting for vehicle tilt and control errors
-        const auto *_rng = AP::dal().rangefinder();
+        const auto *_rng = dal.rangefinder();
         if (_rng == nullptr) {
             // we really, really shouldn't be here.
             return false;
@@ -142,7 +142,7 @@ bool NavEKF3_core::getHeightControlLimit(float &height) const
 void NavEKF3_core::getEulerAngles(Vector3f &euler) const
 {
     outputDataNew.quat.to_euler(euler.x, euler.y, euler.z);
-    euler = euler - AP::dal().get_trim();
+    euler = euler - dal.get_trim();
 }
 
 // return body axis gyro bias estimates in rad/sec
@@ -175,7 +175,7 @@ void NavEKF3_core::getTiltError(float &ang) const
 void NavEKF3_core::getRotationBodyToNED(Matrix3f &mat) const
 {
     outputDataNew.quat.rotation_matrix(mat);
-    mat = mat * AP::dal().get_rotation_vehicle_body_to_autopilot_body();
+    mat = mat * dal.get_rotation_vehicle_body_to_autopilot_body();
 }
 
 // return the quaternions defining the rotation from NED to XYZ (body) axes
@@ -262,7 +262,7 @@ bool NavEKF3_core::getPosNE(Vector2f &posNE) const
     } else {
         // In constant position mode the EKF position states are at the origin, so we cannot use them as a position estimate
         if(validOrigin) {
-            auto &gps = AP::dal().gps();
+            auto &gps = dal.gps();
             if ((gps.status(selected_gps) >= AP_DAL_GPS::GPS_OK_FIX_2D)) {
                 // If the origin has been set and we have GPS, then return the GPS position relative to the origin
                 const struct Location &gpsloc = gps.location(selected_gps);
@@ -326,7 +326,7 @@ bool NavEKF3_core::getHAGL(float &HAGL) const
 // The getFilterStatus() function provides a more detailed description of data health and must be checked if data is to be used for flight control
 bool NavEKF3_core::getLLH(struct Location &loc) const
 {
-    const auto &gps = AP::dal().gps();
+    const auto &gps = dal.gps();
 
     Location origin;
     float posD;
@@ -429,7 +429,7 @@ 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 (!AP::dal().get_compass()) {
+    if (!dal.get_compass()) {
         return false;
     }
     // compass offsets are valid if we have finalised magnetic field initialisation, magnetic field learning is not prohibited,
@@ -439,12 +439,12 @@ bool NavEKF3_core::getMagOffsets(uint8_t mag_idx, Vector3f &magOffsets) const
     if ((mag_idx == magSelectIndex) &&
             finalInflightMagInit &&
             !inhibitMagStates &&
-            AP::dal().get_compass()->healthy(magSelectIndex) &&
+            dal.get_compass()->healthy(magSelectIndex) &&
             variancesConverged) {
-        magOffsets = AP::dal().get_compass()->get_offsets(magSelectIndex) - stateStruct.body_magfield*1000.0f;
+        magOffsets = dal.get_compass()->get_offsets(magSelectIndex) - stateStruct.body_magfield*1000.0f;
         return true;
     } else {
-        magOffsets = AP::dal().get_compass()->get_offsets(magSelectIndex);
+        magOffsets = dal.get_compass()->get_offsets(magSelectIndex);
         return false;
     }
 }

libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp

@@ -284,7 +284,7 @@ bool NavEKF3_core::resetHeightDatum(void)
     // record the old height estimate
     float oldHgt = -stateStruct.position.z;
     // reset the barometer so that it reads zero at the current height
-    AP::dal().baro().update_calibration();
+    dal.baro().update_calibration();
     // reset the height state
     stateStruct.position.z = 0.0f;
     // adjust the height of the EKF origin so that the origin plus baro height before and after the reset is the same
@@ -299,7 +299,7 @@ bool NavEKF3_core::resetHeightDatum(void)
             // altitude. This ensures the reported AMSL alt from
             // getLLH() is equal to GPS altitude, while also ensuring
             // that the relative alt is zero
-            EKF_origin.alt = AP::dal().gps().location().alt;
+            EKF_origin.alt = dal.gps().location().alt;
         }
         ekfGpsRefHgt = (double)0.01 * (double)EKF_origin.alt;
     }
@@ -322,7 +322,7 @@ void NavEKF3_core::CorrectGPSForAntennaOffset(gps_elements &gps_data) const
     }
     gps_data.corrected = true;
 
-    const Vector3f &posOffsetBody = AP::dal().gps().get_antenna_offset(gps_data.sensor_idx) - accelPosOffset;
+    const Vector3f &posOffsetBody = dal.gps().get_antenna_offset(gps_data.sensor_idx) - accelPosOffset;
     if (posOffsetBody.is_zero()) {
         return;
     }
@@ -349,7 +349,7 @@ void NavEKF3_core::CorrectExtNavForSensorOffset(ext_nav_elements &ext_nav_data)
     ext_nav_data.corrected = true;
 
 #if HAL_VISUALODOM_ENABLED
-    const auto *visual_odom = AP::dal().visualodom();
+    const auto *visual_odom = dal.visualodom();
     if (visual_odom == nullptr) {
         return;
     }
@@ -374,7 +374,7 @@ void NavEKF3_core::CorrectExtNavVelForSensorOffset(ext_nav_vel_elements &ext_nav
     ext_nav_vel_data.corrected = true;
 
 #if HAL_VISUALODOM_ENABLED
-    const auto *visual_odom = AP::dal().visualodom();
+    const auto *visual_odom = dal.visualodom();
     if (visual_odom == nullptr) {
         return;
     }
@@ -943,7 +943,7 @@ void NavEKF3_core::selectHeightForFusion()
     // correct range data for the body frame position offset relative to the IMU
     // the corrected reading is the reading that would have been taken if the sensor was
     // co-located with the IMU
-    const auto *_rng = AP::dal().rangefinder();
+    const auto *_rng = dal.rangefinder();
     if (_rng && rangeDataToFuse) {
         auto *sensor = _rng->get_backend(rangeDataDelayed.sensor_idx);
         if (sensor != nullptr) {

libraries/AP_NavEKF3/AP_NavEKF3_VehicleStatus.cpp

@@ -48,7 +48,7 @@ void NavEKF3_core::calcGpsGoodToAlign(void)
 
     // Check for significant change in GPS position if disarmed which indicates bad GPS
     // This check can only be used when the vehicle is stationary
-    const auto &gps = AP::dal().gps();
+    const auto &gps = dal.gps();
 
     const struct Location &gpsloc = gps.location(preferred_gps); // Current location
     const float posFiltTimeConst = 10.0f; // time constant used to decay position drift
@@ -68,7 +68,7 @@ void NavEKF3_core::calcGpsGoodToAlign(void)
 
     // Report check result as a text string and bitmask
     if (gpsDriftFail) {
-        AP::dal().snprintf(prearm_fail_string,
+        dal.snprintf(prearm_fail_string,
                            sizeof(prearm_fail_string),
                            "GPS drift %.1fm (needs %.1f)", (double)gpsDriftNE, (double)(3.0f*checkScaler));
         gpsCheckStatus.bad_horiz_drift = true;
@@ -99,7 +99,7 @@ void NavEKF3_core::calcGpsGoodToAlign(void)
 
     // Report check result as a text string and bitmask
     if (gpsVertVelFail) {
-        AP::dal().snprintf(prearm_fail_string,
+        dal.snprintf(prearm_fail_string,
                            sizeof(prearm_fail_string),
                            "GPS vertical speed %.2fm/s (needs %.2f)", (double)fabsf(gpsVertVelFilt), (double)(0.3f*checkScaler));
         gpsCheckStatus.bad_vert_vel = true;
@@ -120,7 +120,7 @@ void NavEKF3_core::calcGpsGoodToAlign(void)
 
     // Report check result as a text string and bitmask
     if (gpsHorizVelFail) {
-        AP::dal().snprintf(prearm_fail_string,
+        dal.snprintf(prearm_fail_string,
                            sizeof(prearm_fail_string),
                            "GPS horizontal speed %.2fm/s (needs %.2f)", (double)gpsDriftNE, (double)(0.3f*checkScaler));
         gpsCheckStatus.bad_horiz_vel = true;
@@ -139,7 +139,7 @@ void NavEKF3_core::calcGpsGoodToAlign(void)
 
     // Report check result as a text string and bitmask
     if (hAccFail) {
-        AP::dal().snprintf(prearm_fail_string,
+        dal.snprintf(prearm_fail_string,
                            sizeof(prearm_fail_string),
                            "GPS horiz error %.1fm (needs %.1f)", (double)hAcc, (double)(5.0f*checkScaler));
         gpsCheckStatus.bad_hAcc = true;
@@ -156,7 +156,7 @@ void NavEKF3_core::calcGpsGoodToAlign(void)
     }
     // Report check result as a text string and bitmask
     if (vAccFail) {
-        AP::dal().snprintf(prearm_fail_string,
+        dal.snprintf(prearm_fail_string,
                            sizeof(prearm_fail_string),
                            "GPS vert error %.1fm (needs < %.1f)", (double)vAcc, (double)(7.5f * checkScaler));
         gpsCheckStatus.bad_vAcc = true;
@@ -169,7 +169,7 @@ void NavEKF3_core::calcGpsGoodToAlign(void)
 
     // Report check result as a text string and bitmask
     if (gpsSpdAccFail) {
-        AP::dal().snprintf(prearm_fail_string,
+        dal.snprintf(prearm_fail_string,
                            sizeof(prearm_fail_string),
                            "GPS speed error %.1f (needs < %.1f)", (double)gpsSpdAccuracy, (double)(1.0f*checkScaler));
         gpsCheckStatus.bad_sAcc = true;
@@ -182,7 +182,7 @@ void NavEKF3_core::calcGpsGoodToAlign(void)
 
     // Report check result as a text string and bitmask
     if (hdopFail) {
-        AP::dal().snprintf(prearm_fail_string, sizeof(prearm_fail_string),
+        dal.snprintf(prearm_fail_string, sizeof(prearm_fail_string),
                            "GPS HDOP %.1f (needs 2.5)", (double)(0.01f * gps.get_hdop(preferred_gps)));
         gpsCheckStatus.bad_hdop = true;
     } else {
@@ -194,7 +194,7 @@ void NavEKF3_core::calcGpsGoodToAlign(void)
 
     // Report check result as a text string and bitmask
     if (numSatsFail) {
-        AP::dal().snprintf(prearm_fail_string, sizeof(prearm_fail_string),
+        dal.snprintf(prearm_fail_string, sizeof(prearm_fail_string),
                            "GPS numsats %u (needs 6)", gps.num_sats(preferred_gps));
         gpsCheckStatus.bad_sats = true;
     } else {
@@ -212,7 +212,7 @@ void NavEKF3_core::calcGpsGoodToAlign(void)
 
     // Report check result as a text string and bitmask
     if (yawFail) {
-        AP::dal().snprintf(prearm_fail_string,
+        dal.snprintf(prearm_fail_string,
                            sizeof(prearm_fail_string),
                            "Mag yaw error x=%.1f y=%.1f",
                            (double)magTestRatio.x,
@@ -224,7 +224,7 @@ void NavEKF3_core::calcGpsGoodToAlign(void)
 
     // assume failed first time through and notify user checks have started
     if (lastGpsVelFail_ms == 0) {
-        AP::dal().snprintf(prearm_fail_string, sizeof(prearm_fail_string), "EKF starting GPS checks");
+        dal.snprintf(prearm_fail_string, sizeof(prearm_fail_string), "EKF starting GPS checks");
         lastGpsVelFail_ms = imuSampleTime_ms;
     }
 
@@ -261,7 +261,7 @@ void NavEKF3_core::calcGpsGoodForFlight(void)
 
     // get the receivers reported speed accuracy
     float gpsSpdAccRaw;
-    if (!AP::dal().gps().speed_accuracy(preferred_gps, gpsSpdAccRaw)) {
+    if (!dal.gps().speed_accuracy(preferred_gps, gpsSpdAccRaw)) {
         gpsSpdAccRaw = 0.0f;
     }
 
@@ -321,9 +321,9 @@ void NavEKF3_core::detectFlight()
         bool largeHgtChange = false;
 
         // trigger at 8 m/s airspeed
-        const auto *arsp = AP::dal().airspeed();
+        const auto *arsp = dal.airspeed();
         if (arsp && arsp->healthy(selected_airspeed) && arsp->use(selected_airspeed)) {
-            if (arsp->get_airspeed(selected_airspeed) * AP::dal().get_EAS2TAS() > 10.0f) {
+            if (arsp->get_airspeed(selected_airspeed) * dal.get_EAS2TAS() > 10.0f) {
                 highAirSpd = true;
             }
         }
@@ -374,7 +374,7 @@ void NavEKF3_core::detectFlight()
 
             // If more than 5 seconds since likely_flying was set
             // true, then set inFlight true
-            if (AP::dal().get_time_flying_ms() > 5000) {
+            if (dal.get_time_flying_ms() > 5000) {
                 inFlight = true;
             }
         }
@@ -478,7 +478,7 @@ void NavEKF3_core::detectOptFlowTakeoff(void)
 {
     if (!onGround && !takeOffDetected && (imuSampleTime_ms - timeAtArming_ms) > 1000) {
         // we are no longer confidently on the ground so check the range finder and gyro for signs of takeoff
-        const auto &ins = AP::dal().ins();
+        const auto &ins = dal.ins();
         Vector3f angRateVec;
         Vector3f gyroBias;
         getGyroBias(gyroBias);

libraries/AP_NavEKF3/AP_NavEKF3_core.cpp

@@ -9,7 +9,8 @@
 
 // constructor
 NavEKF3_core::NavEKF3_core(NavEKF3 *_frontend) :
-    frontend(_frontend)
+    frontend(_frontend),
+    dal(AP::dal())
 {
     firstInitTime_ms = 0;
     lastInitFailReport_ms = 0;
@@ -29,8 +30,8 @@ bool NavEKF3_core::setup_core(uint8_t _imu_index, uint8_t _core_index)
      */
 
     // Calculate the expected EKF time step
-    if (AP::dal().ins().get_loop_rate_hz() > 0) {
-        dtEkfAvg = 1.0f / AP::dal().ins().get_loop_rate_hz();
+    if (dal.ins().get_loop_rate_hz() > 0) {
+        dtEkfAvg = 1.0f / dal.ins().get_loop_rate_hz();
         dtEkfAvg = MAX(dtEkfAvg,EKF_TARGET_DT);
     } else {
         return false;
@@ -48,9 +49,9 @@ bool NavEKF3_core::setup_core(uint8_t _imu_index, uint8_t _core_index)
     if (frontend->_fusionModeGPS != 3) {
         // Wait for the configuration of all GPS units to be confirmed. Until this has occurred the GPS driver cannot provide a correct time delay
         float gps_delay_sec = 0;
-        if (!AP::dal().gps().get_lag(selected_gps, gps_delay_sec)) {
+        if (!dal.gps().get_lag(selected_gps, gps_delay_sec)) {
 #ifndef HAL_NO_GCS
-            const uint32_t now = AP::dal().millis();
+            const uint32_t now = dal.millis();
             if (now - lastInitFailReport_ms > 10000) {
                 lastInitFailReport_ms = now;
                 // provide an escalating series of messages
@@ -70,13 +71,13 @@ bool NavEKF3_core::setup_core(uint8_t _imu_index, uint8_t _core_index)
     }
 
     // airspeed sensing can have large delays and should not be included if disabled
-    if (AP::dal().airspeed_sensor_enabled()) {
+    if (dal.airspeed_sensor_enabled()) {
         maxTimeDelay_ms = MAX(maxTimeDelay_ms , frontend->tasDelay_ms);
     }
 
 #if HAL_VISUALODOM_ENABLED
     // include delay from visual odometry if enabled
-    const auto *visual_odom = AP::dal().visualodom();
+    const auto *visual_odom = dal.visualodom();
     if ((visual_odom != nullptr) && visual_odom->enabled()) {
         maxTimeDelay_ms = MAX(maxTimeDelay_ms, MIN(visual_odom->get_delay_ms(), 250));
     }
@@ -157,7 +158,7 @@ bool NavEKF3_core::setup_core(uint8_t _imu_index, uint8_t _core_index)
 
     if ((yawEstimator == nullptr) && (frontend->_gsfRunMask & (1U<<core_index))) {
         // check if there is enough memory to create the EKF-GSF object
-        if (AP::dal().available_memory() < sizeof(EKFGSF_yaw) + 1024) {
+        if (dal.available_memory() < sizeof(EKFGSF_yaw) + 1024) {
             GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "EKF3 IMU%u GSF: not enough memory",(unsigned)imu_index);
             return false;
         }
@@ -182,7 +183,7 @@ bool NavEKF3_core::setup_core(uint8_t _imu_index, uint8_t _core_index)
 void NavEKF3_core::InitialiseVariables()
 {
     // calculate the nominal filter update rate
-    const auto &ins = AP::dal().ins();
+    const auto &ins = dal.ins();
     localFilterTimeStep_ms = (uint8_t)(1000*ins.get_loop_delta_t());
     localFilterTimeStep_ms = MAX(localFilterTimeStep_ms, (uint8_t)EKF_TARGET_DT_MS);
 
@@ -406,7 +407,7 @@ void NavEKF3_core::InitialiseVariables()
     storedExtNavVel.reset();
 
     // initialise pre-arm message
-    AP::dal().snprintf(prearm_fail_string, sizeof(prearm_fail_string), "EKF3 still initialising");
+    dal.snprintf(prearm_fail_string, sizeof(prearm_fail_string), "EKF3 still initialising");
 
     InitialiseVariablesMag();
 
@@ -463,8 +464,8 @@ bool NavEKF3_core::InitialiseFilterBootstrap(void)
     update_sensor_selection();
 
     // If we are a plane and don't have GPS lock then don't initialise
-    if (assume_zero_sideslip() && AP::dal().gps().status(preferred_gps) < AP_DAL_GPS::GPS_OK_FIX_3D) {
-        AP::dal().snprintf(prearm_fail_string,
+    if (assume_zero_sideslip() && dal.gps().status(preferred_gps) < AP_DAL_GPS::GPS_OK_FIX_3D) {
+        dal.snprintf(prearm_fail_string,
                          sizeof(prearm_fail_string),
                          "EKF3 init failure: No GPS lock");
         statesInitialised = false;
@@ -499,7 +500,7 @@ bool NavEKF3_core::InitialiseFilterBootstrap(void)
     Vector3f initAccVec;
 
     // TODO we should average accel readings over several cycles
-    initAccVec = AP::dal().ins().get_accel(accel_index_active);
+    initAccVec = dal.ins().get_accel(accel_index_active);
 
     // normalise the acceleration vector
     float pitch=0, roll=0;
@@ -533,7 +534,7 @@ bool NavEKF3_core::InitialiseFilterBootstrap(void)
     ResetHeight();
 
     // define Earth rotation vector in the NED navigation frame
-    calcEarthRateNED(earthRateNED, AP::dal().get_home().lat);
+    calcEarthRateNED(earthRateNED, dal.get_home().lat);
 
     // initialise the covariance matrix
     CovarianceInit();
@@ -2011,7 +2012,7 @@ void NavEKF3_core::verifyTiltErrorVariance()
 
         const struct log_XKTV msg{
             LOG_PACKET_HEADER_INIT(LOG_XKTV_MSG),
-            time_us      : AP::dal().micros64(),
+            time_us      : dal.micros64(),
             core         : core_index,
             tvs          : tiltErrorVariance,
             tvd          : tiltErrorVarianceAlt,

libraries/AP_NavEKF3/AP_NavEKF3_core.h

@@ -31,6 +31,7 @@
 #include <AP_NavEKF3/AP_NavEKF3_Buffer.h>
 #include <AP_InertialSensor/AP_InertialSensor.h>
 #include <GCS_MAVLink/GCS_MAVLink.h>
+#include <AP_DAL/AP_DAL.h>
 
 #include "AP_NavEKF/EKFGSF_yaw.h"
 
@@ -453,6 +454,7 @@ public:
     
 private:
     EKFGSF_yaw *yawEstimator;
+    AP_DAL &dal;
 
     // Reference to the global EKF frontend for parameters
     class NavEKF3 *frontend;