AP_NavEKF2: use dal reference in EKF backends

saves a bit of flash space

libraries/AP_NavEKF2/AP_NavEKF2_AirDataFusion.cpp

@@ -26,7 +26,7 @@ void NavEKF2_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));
     Vector3 SH_TAS;
     float SK_TAS;

libraries/AP_NavEKF2/AP_NavEKF2_Control.cpp

@@ -12,7 +12,7 @@ void NavEKF2_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;
@@ -69,7 +69,7 @@ void NavEKF2_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
@@ -371,7 +371,7 @@ void NavEKF2_core::checkAttitudeAlignmentStatus()
 // return true if we should use the airspeed sensor
 bool NavEKF2_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
@@ -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 AP::dal().get_compass() && AP::dal().get_compass()->use_for_yaw(magSelectIndex) && !allMagSensorsFailed;
+    return dal.get_compass() && dal.get_compass()->use_for_yaw(magSelectIndex) && !allMagSensorsFailed;
 }
 
 /*
@@ -419,7 +419,7 @@ bool NavEKF2_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
@@ -545,7 +545,7 @@ void NavEKF2_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_NavEKF2/AP_NavEKF2_Measurements.cpp

@@ -22,7 +22,7 @@ void NavEKF2_core::readRangeFinder(void)
 
     // 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;
     }
@@ -190,7 +190,7 @@ void NavEKF2_core::writeOptFlowMeas(const uint8_t rawFlowQuality, const Vector2f
 // try changing to another compass
 void NavEKF2_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
@@ -228,12 +228,12 @@ void NavEKF2_core::tryChangeCompass(void)
 // check for new magnetometer data and update store measurements if available
 void NavEKF2_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
@@ -328,7 +328,7 @@ void NavEKF2_core::readMagData()
  */
 void NavEKF2_core::readIMUData()
 {
-    const auto &ins = AP::dal().ins();
+    const auto &ins = dal.ins();
 
     // average IMU sampling rate
     dtIMUavg = ins.get_loop_delta_t();
@@ -472,7 +472,7 @@ void NavEKF2_core::readIMUData()
 // read the delta velocity and corresponding time interval from the IMU
 // return false if data is not available
 bool NavEKF2_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);
@@ -497,7 +497,7 @@ void NavEKF2_core::readGpsData()
 
     // check for new GPS data
     // do not accept data at a faster rate than 14Hz to avoid overflowing the FIFO buffer
-    const auto &gps = AP::dal().gps();
+    const auto &gps = dal.gps();
     if (gps.last_message_time_ms(gps.primary_sensor()) - lastTimeGpsReceived_ms > 70) {
         if (gps.status() >= AP_DAL_GPS::GPS_OK_FIX_3D) {
             // report GPS fix status
@@ -607,7 +607,7 @@ void NavEKF2_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,
@@ -636,7 +636,7 @@ void NavEKF2_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");
         }
     }
 }
@@ -644,7 +644,7 @@ void NavEKF2_core::readGpsData()
 // read the delta angle and corresponding time interval from the IMU
 // return false if data is not available
 bool NavEKF2_core::readDeltaAngle(uint8_t ins_index, Vector3f &dAng, float &dAng_dt) {
-    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);
@@ -665,7 +665,7 @@ void NavEKF2_core::readBaroData()
 {
     // check to see if baro measurement has changed so we know if a new measurement has arrived
     // do not accept data at a faster rate than 14Hz to avoid overflowing the FIFO buffer
-    const auto &baro = AP::dal().baro();
+    const auto &baro = dal.baro();
     if (baro.get_last_update() - lastBaroReceived_ms > 70) {
 
         baroDataNew.hgt = baro.get_altitude();
@@ -753,11 +753,11 @@ void NavEKF2_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 *aspeed = AP::dal().airspeed();
+    const auto *aspeed = dal.airspeed();
     if (aspeed &&
             aspeed->use() &&
             aspeed->last_update_ms() != timeTasReceived_ms) {
-        tasDataNew.tas = aspeed->get_airspeed() * AP::dal().get_EAS2TAS();
+        tasDataNew.tas = aspeed->get_airspeed() * dal.get_EAS2TAS();
         timeTasReceived_ms = aspeed->last_update_ms();
         tasDataNew.time_ms = timeTasReceived_ms - frontend->tasDelay_ms;
 
@@ -779,7 +779,7 @@ void NavEKF2_core::readAirSpdData()
 void NavEKF2_core::readRngBcnData()
 {
     // get the location of the beacon data
-    const auto *beacon = AP::dal().beacon();
+    const auto *beacon = dal.beacon();
 
     // exit immediately if no beacon object
     if (beacon == nullptr) {
@@ -961,7 +961,7 @@ float NavEKF2_core::MagDeclination(void) const
     if (!use_compass()) {
         return 0;
     }
-    return AP::dal().get_compass()->get_declination();
+    return dal.get_compass()->get_declination();
 }
 
 /*
@@ -971,7 +971,7 @@ float NavEKF2_core::MagDeclination(void) const
  */
 void NavEKF2_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++) {

libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp

@@ -99,7 +99,7 @@ bool NavEKF2_core::getHeightControlLimit(float &height) const
     // only ask for limiting if we are doing optical flow only 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;
@@ -120,7 +120,7 @@ bool NavEKF2_core::getHeightControlLimit(float &height) const
 void NavEKF2_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
@@ -155,7 +155,7 @@ void NavEKF2_core::getTiltError(float &ang) const
 void NavEKF2_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
@@ -251,9 +251,9 @@ bool NavEKF2_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) {
-            if ((AP::dal().gps().status(AP::dal().gps().primary_sensor()) >= AP_DAL_GPS::GPS_OK_FIX_2D)) {
+            if ((dal.gps().status(dal.gps().primary_sensor()) >= 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 = AP::dal().gps().location();
+                const struct Location &gpsloc = dal.gps().location();
                 const Vector2f tempPosNE = EKF_origin.get_distance_NE(gpsloc);
                 posNE.x = tempPosNE.x;
                 posNE.y = tempPosNE.y;
@@ -315,7 +315,7 @@ bool NavEKF2_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 NavEKF2_core::getLLH(struct Location &loc) const
 {
-    const auto &gps = AP::dal().gps();
+    const auto &gps = dal.gps();
     Location origin;
     float posD;
 
@@ -413,7 +413,7 @@ 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 (!AP::dal().get_compass()) {
+    if (!dal.get_compass()) {
         return false;
     }
 
@@ -424,12 +424,12 @@ bool NavEKF2_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_NavEKF2/AP_NavEKF2_PosVelFusion.cpp

@@ -285,7 +285,7 @@ bool NavEKF2_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
@@ -301,7 +301,7 @@ bool NavEKF2_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;
     }
@@ -317,7 +317,7 @@ bool NavEKF2_core::resetHeightDatum(void)
 */
 void NavEKF2_core::CorrectGPSForAntennaOffset(gps_elements &gps_data) const
 {
-    const Vector3f &posOffsetBody = AP::dal().gps().get_antenna_offset(gpsDataDelayed.sensor_idx) - accelPosOffset;
+    const Vector3f &posOffsetBody = dal.gps().get_antenna_offset(gpsDataDelayed.sensor_idx) - accelPosOffset;
     if (posOffsetBody.is_zero()) {
         return;
     }
@@ -343,7 +343,7 @@ void NavEKF2_core::CorrectGPSForAntennaOffset(gps_elements &gps_data) const
 void NavEKF2_core::CorrectExtNavForSensorOffset(Vector3f &ext_position) const
 {
 #if HAL_VISUALODOM_ENABLED
-    const auto *visual_odom = AP::dal().visualodom();
+    const auto *visual_odom = dal.visualodom();
     if (visual_odom == nullptr) {
         return;
     }
@@ -362,7 +362,7 @@ void NavEKF2_core::CorrectExtNavForSensorOffset(Vector3f &ext_position) const
 void NavEKF2_core::CorrectExtNavVelForSensorOffset(Vector3f &ext_velocity) const
 {
 #if HAL_VISUALODOM_ENABLED
-    const auto *visual_odom = AP::dal().visualodom();
+    const auto *visual_odom = dal.visualodom();
     if (visual_odom == nullptr) {
         return;
     }
@@ -957,7 +957,7 @@ void NavEKF2_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) {
         const auto *sensor = _rng->get_backend(rangeDataDelayed.sensor_idx);
         if (sensor != nullptr) {

libraries/AP_NavEKF2/AP_NavEKF2_VehicleStatus.cpp

@@ -18,7 +18,7 @@ extern const AP_HAL::HAL& hal;
 */
 void NavEKF2_core::calcGpsGoodToAlign(void)
 {
-    const auto &gps = AP::dal().gps();
+    const auto &gps = dal.gps();
 
     if (inFlight && assume_zero_sideslip() && !use_compass()) {
         // this is a special case where a plane has launched without magnetometer
@@ -72,7 +72,7 @@ void NavEKF2_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;
@@ -103,7 +103,7 @@ void NavEKF2_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;
@@ -124,7 +124,7 @@ void NavEKF2_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;
@@ -143,7 +143,7 @@ void NavEKF2_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;
@@ -159,7 +159,7 @@ void NavEKF2_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;
@@ -172,7 +172,7 @@ void NavEKF2_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;
@@ -185,7 +185,7 @@ void NavEKF2_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()));
         gpsCheckStatus.bad_hdop = true;
     } else {
@@ -197,7 +197,7 @@ void NavEKF2_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());
         gpsCheckStatus.bad_sats = true;
     } else {
@@ -215,7 +215,7 @@ void NavEKF2_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,
@@ -227,7 +227,7 @@ void NavEKF2_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;
     }
 
@@ -264,7 +264,7 @@ void NavEKF2_core::calcGpsGoodForFlight(void)
 
     // get the receivers reported speed accuracy
     float gpsSpdAccRaw;
-    if (!AP::dal().gps().speed_accuracy(gpsSpdAccRaw)) {
+    if (!dal.gps().speed_accuracy(gpsSpdAccRaw)) {
         gpsSpdAccRaw = 0.0f;
     }
 
@@ -324,9 +324,9 @@ void NavEKF2_core::detectFlight()
         bool largeHgtChange = false;
 
         // trigger at 8 m/s airspeed
-        if (AP::dal().airspeed_sensor_enabled()) {
-            const auto *airspeed = AP::dal().airspeed();
-            if (airspeed->get_airspeed() * AP::dal().get_EAS2TAS() > 10.0f) {
+        if (dal.airspeed_sensor_enabled()) {
+            const auto *airspeed = dal.airspeed();
+            if (airspeed->get_airspeed() * dal.get_EAS2TAS() > 10.0f) {
                 highAirSpd = true;
             }
         }
@@ -383,7 +383,7 @@ void NavEKF2_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 NavEKF2_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_NavEKF2/AP_NavEKF2_core.cpp

@@ -30,7 +30,8 @@ extern const AP_HAL::HAL& hal;
 
 // constructor
 NavEKF2_core::NavEKF2_core(NavEKF2 *_frontend) :
-    frontend(_frontend)
+    frontend(_frontend),
+    dal(AP::dal())
 {
 }
 
@@ -48,7 +49,7 @@ bool NavEKF2_core::setup_core(uint8_t _imu_index, uint8_t _core_index)
       than 100Hz is downsampled. For 50Hz main loop rate we need a
       shorter buffer.
      */
-    if (AP::dal().ins().get_loop_rate_hz() < 100) {
+    if (dal.ins().get_loop_rate_hz() < 100) {
         imu_buffer_length = 13;
     } else {
         // maximum 260 msec delay at 100 Hz fusion rate
@@ -92,7 +93,7 @@ bool NavEKF2_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, "EKF2 IMU%u GSF: not enough memory",(unsigned)imu_index);
             return false;
         }
@@ -117,7 +118,7 @@ bool NavEKF2_core::setup_core(uint8_t _imu_index, uint8_t _core_index)
 void NavEKF2_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,10);
 
@@ -327,7 +328,7 @@ void NavEKF2_core::InitialiseVariables()
     have_table_earth_field = false;
 
     // initialise pre-arm message
-    AP::dal().snprintf(prearm_fail_string, sizeof(prearm_fail_string), "EKF2 still initialising");
+    dal.snprintf(prearm_fail_string, sizeof(prearm_fail_string), "EKF2 still initialising");
 
     InitialiseVariablesMag();
 
@@ -373,8 +374,8 @@ void NavEKF2_core::InitialiseVariablesMag()
 bool NavEKF2_core::InitialiseFilterBootstrap(void)
 {
     // If we are a plane and don't have GPS lock then don't initialise
-    if (assume_zero_sideslip() && AP::dal().gps().status(AP::dal().gps().primary_sensor()) < AP_DAL_GPS::GPS_OK_FIX_3D) {
-        AP::dal().snprintf(prearm_fail_string,
+    if (assume_zero_sideslip() && dal.gps().status(dal.gps().primary_sensor()) < AP_DAL_GPS::GPS_OK_FIX_3D) {
+        dal.snprintf(prearm_fail_string,
                            sizeof(prearm_fail_string),
                            "EKF2 init failure: No GPS lock");
         statesInitialised = false;
@@ -395,7 +396,7 @@ bool NavEKF2_core::InitialiseFilterBootstrap(void)
     // set re-used variables to zero
     InitialiseVariables();
 
-    const auto &ins = AP::dal().ins();
+    const auto &ins = dal.ins();
 
     // Initialise IMU data
     dtIMUavg = ins.get_loop_delta_t();
@@ -452,7 +453,7 @@ bool NavEKF2_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();
@@ -538,7 +539,7 @@ void NavEKF2_core::UpdateFilter(bool predict)
 #if ENABLE_EKF_TIMING
     void *istate = hal.scheduler->disable_interrupts_save();
     static uint32_t timing_start_us;
-    timing_start_us = AP::dal().micros();
+    timing_start_us = dal.micros();
 #endif
 
     fill_scratch_variables();
@@ -601,7 +602,7 @@ void NavEKF2_core::UpdateFilter(bool predict)
 #if ENABLE_EKF_TIMING
     static uint32_t total_us;
     static uint32_t timing_counter;
-    total_us += AP::dal().micros() - timing_start_us;
+    total_us += dal.micros() - timing_start_us;
     if (timing_counter++ == 4000) {
         hal.console->printf("ekf2 avg %.2f us\n", total_us / float(timing_counter));
         total_us = 0;
@@ -618,12 +619,12 @@ void NavEKF2_core::UpdateFilter(bool predict)
       it try again.
      */
     if (filterStatus.value != 0) {
-        last_filter_ok_ms = AP::dal().millis();
+        last_filter_ok_ms = dal.millis();
     }
     if (filterStatus.value == 0 &&
         last_filter_ok_ms != 0 &&
-        AP::dal().millis() - last_filter_ok_ms > 5000 &&
-        !AP::dal().get_armed()) {
+        dal.millis() - last_filter_ok_ms > 5000 &&
+        !dal.get_armed()) {
         // we've been unhealthy for 5 seconds after being healthy, reset the filter
         GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "EKF2 IMU%u forced reset",(unsigned)imu_index);
         last_filter_ok_ms = 0;

libraries/AP_NavEKF2/AP_NavEKF2_core.h

@@ -368,6 +368,7 @@ public:
     
 private:
     EKFGSF_yaw *yawEstimator;
+    AP_DAL &dal;
 
     // Reference to the global EKF frontend for parameters
     class NavEKF2 *frontend;