diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_AirDataFusion.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_AirDataFusion.cpp index 6e78924a8e..5d31ea4824 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_AirDataFusion.cpp +++ b/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]; diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp index 1c00444228..7384a3ce31 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp +++ b/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; diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp index bda9951065..8b1a815d51 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp +++ b/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); diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp index 3ed74506ed..53314431bd 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp +++ b/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; iget_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, diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp index 746b8fda0e..3a19347496 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp +++ b/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; } } diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp index 23a379e434..bf1432fc29 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp +++ b/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) { diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_VehicleStatus.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_VehicleStatus.cpp index 7cef50178c..5eaea57725 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_VehicleStatus.cpp +++ b/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); diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp index fa1918adcd..4e5e1d9696 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp +++ b/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< #include #include +#include #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;