aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_NavEKF3: use dal reference in EKF backends

Browse Source 
saves a bit of flash space
This commit is contained in:
Andrew Tridgell 2020-11-07 17:24:13 +11:00
parent 5f0e943f0f
commit 9b81c5a1e0
9 changed files with 81 additions and 78 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
libraries/AP_NavEKF3/AP_NavEKF3_AirDataFusion.cpp
View File

@ -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];

12
libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp
View File

@ -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;

2
libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp
View File

@ -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);

46
libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp
View File

@ -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,

20
libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp
View File

@ -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;
}
}

12
libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp
View File

@ -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) {

32
libraries/AP_NavEKF3/AP_NavEKF3_VehicleStatus.cpp
View File

@ -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);

31
libraries/AP_NavEKF3/AP_NavEKF3_core.cpp
View File

@ -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,

2
libraries/AP_NavEKF3/AP_NavEKF3_core.h
View File

@ -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;