Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF2: use dal reference in EKF backends 

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

2
libraries/AP_NavEKF2/AP_NavEKF2_AirDataFusion.cpp
View File

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

12
libraries/AP_NavEKF2/AP_NavEKF2_Control.cpp
View File

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

32
libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp
View File

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

20
libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp
View File

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

12
libraries/AP_NavEKF2/AP_NavEKF2_PosVelFusion.cpp
View File

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

34
libraries/AP_NavEKF2/AP_NavEKF2_VehicleStatus.cpp
View File

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

29
libraries/AP_NavEKF2/AP_NavEKF2_core.cpp
View File

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

1
libraries/AP_NavEKF2/AP_NavEKF2_core.h
View File

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