From 5f0e943f0fd0667cd31d013cb74c9d69324a2b02 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 7 Nov 2020 17:24:13 +1100 Subject: [PATCH] AP_NavEKF2: use dal reference in EKF backends saves a bit of flash space --- .../AP_NavEKF2/AP_NavEKF2_AirDataFusion.cpp | 2 +- libraries/AP_NavEKF2/AP_NavEKF2_Control.cpp | 12 +++---- .../AP_NavEKF2/AP_NavEKF2_Measurements.cpp | 32 ++++++++--------- libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp | 20 +++++------ .../AP_NavEKF2/AP_NavEKF2_PosVelFusion.cpp | 12 +++---- .../AP_NavEKF2/AP_NavEKF2_VehicleStatus.cpp | 34 +++++++++---------- libraries/AP_NavEKF2/AP_NavEKF2_core.cpp | 29 ++++++++-------- libraries/AP_NavEKF2/AP_NavEKF2_core.h | 1 + 8 files changed, 72 insertions(+), 70 deletions(-) diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_AirDataFusion.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_AirDataFusion.cpp index a33a904a38..92d8dad6d3 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_AirDataFusion.cpp +++ b/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; diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_Control.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_Control.cpp index 8fdb0a99c5..24f163f9a6 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_Control.cpp +++ b/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; diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp index 0eb32e7e58..74ad035f70 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp +++ b/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_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; } } diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_PosVelFusion.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_PosVelFusion.cpp index f16311fe9b..93e6059b30 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_PosVelFusion.cpp +++ b/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) { diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_VehicleStatus.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_VehicleStatus.cpp index 9053152855..88b819fc8d 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_VehicleStatus.cpp +++ b/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); diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_core.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_core.cpp index 5249578c32..2f60e39d6b 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_core.cpp +++ b/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<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; diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_core.h b/libraries/AP_NavEKF2/AP_NavEKF2_core.h index 5eff26e166..63c94c0784 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_core.h +++ b/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;