Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-08 17:08:28 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF2: convert to use AP_DAL for new replay structure

Browse Source 
Co-authored-by: Peter Barker <pbarker@barker.dropbear.id.au>
This commit is contained in:
Andrew Tridgell 2020-11-06 10:30:16 +11:00
parent 97a2c5a576
commit 6be3d19e82
15 changed files with 289 additions and 309 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

137
libraries/AP_NavEKF2/AP_NavEKF2.cpp
View File

@ -426,13 +426,8 @@ const AP_Param::GroupInfo NavEKF2::var_info[] = {
// @Units: m // @Units: m
AP_GROUPINFO("NOAID_M_NSE", 35, NavEKF2, _noaidHorizNoise, 10.0f), AP_GROUPINFO("NOAID_M_NSE", 35, NavEKF2, _noaidHorizNoise, 10.0f),
// @Param: LOG_MASK // 36 was LOG_MASK, used for specifying which IMUs/cores to log
// @DisplayName: EKF sensor logging IMU mask // replay data for
// @Description: This sets the IMU mask of sensors to do full logging for
// @Bitmask: 0:FirstIMU,1:SecondIMU,2:ThirdIMU,3:FourthIMU,4:FifthIMU,5:SixthIMU
// @User: Advanced
// @RebootRequired: True
AP_GROUPINFO("LOG_MASK", 36, NavEKF2, _logging_mask, 1),
// control of magentic yaw angle fusion // control of magentic yaw angle fusion
@ -614,38 +609,15 @@ const AP_Param::GroupInfo NavEKF2::var_info[] = {
NavEKF2::NavEKF2() NavEKF2::NavEKF2()
{ {
AP_Param::setup_object_defaults(this, var_info); AP_Param::setup_object_defaults(this, var_info);
_ahrs = &AP::ahrs();
}
/*
see if we should log some sensor data
*/
void NavEKF2::check_log_write(void)
{
if (!have_ekf_logging()) {
return;
}
if (logging.log_compass) {
AP::logger().Write_Compass(imuSampleTime_us);
logging.log_compass = false;
}
if (logging.log_baro) {
AP::logger().Write_Baro(imuSampleTime_us);
logging.log_baro = false;
}
if (logging.log_imu) {
AP::logger().Write_IMUDT(imuSampleTime_us, _logging_mask.get());
logging.log_imu = false;
}
// this is an example of an ad-hoc log in EKF
// AP::logger().Write("NKA", "TimeUS,X", "Qf", AP_HAL::micros64(), (double)2.4f);
} }
#include <stdio.h>
// Initialise the filter // Initialise the filter
bool NavEKF2::InitialiseFilter(void) bool NavEKF2::InitialiseFilter(void)
{ {
AP::dal().start_frame(AP_DAL::FrameType::InitialiseFilterEKF2);
// Return immediately if there is insufficient memory // Return immediately if there is insufficient memory
if (core_malloc_failed) { if (core_malloc_failed) {
return false; return false;
@ -653,14 +625,14 @@ bool NavEKF2::InitialiseFilter(void)
initFailure = InitFailures::UNKNOWN; initFailure = InitFailures::UNKNOWN;
if (_enable == 0) { if (_enable == 0) {
if (_ahrs->get_ekf_type() == 2) { if (AP::dal().get_ekf_type() == 2) {
initFailure = InitFailures::NO_ENABLE; initFailure = InitFailures::NO_ENABLE;
} }
return false; return false;
} }
const AP_InertialSensor &ins = AP::ins(); const auto &ins = AP::dal().ins();
imuSampleTime_us = AP_HAL::micros64(); imuSampleTime_us = AP::dal().micros64();
// remember expected frame time // remember expected frame time
_frameTimeUsec = 1e6 / ins.get_loop_rate_hz(); _frameTimeUsec = 1e6 / ins.get_loop_rate_hz();
@ -668,12 +640,6 @@ bool NavEKF2::InitialiseFilter(void)
// expected number of IMU frames per prediction // expected number of IMU frames per prediction
_framesPerPrediction = uint8_t((EKF_TARGET_DT / (_frameTimeUsec * 1.0e-6) + 0.5)); _framesPerPrediction = uint8_t((EKF_TARGET_DT / (_frameTimeUsec * 1.0e-6) + 0.5));
// see if we will be doing logging
AP_Logger *logger = AP_Logger::get_singleton();
if (logger != nullptr) {
logging.enabled = logger->log_replay();
}
if (core == nullptr) { if (core == nullptr) {
// don't allow more filters than IMUs // don't allow more filters than IMUs
@ -694,19 +660,19 @@ bool NavEKF2::InitialiseFilter(void)
} }
// check if there is enough memory to create the EKF cores // check if there is enough memory to create the EKF cores
if (hal.util->available_memory() < sizeof(NavEKF2_core)*num_cores + 4096) { if (AP::dal().available_memory() < sizeof(NavEKF2_core)*num_cores + 4096) {
initFailure = InitFailures::NO_MEM; initFailure = InitFailures::NO_MEM;
core_malloc_failed = true; core_malloc_failed = true;
gcs().send_text(MAV_SEVERITY_CRITICAL, "NavEKF2: not enough memory available"); GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "NavEKF2: not enough memory available");
return false; return false;
} }
// try to allocate from CCM RAM, fallback to Normal RAM if not available or full // try to allocate from CCM RAM, fallback to Normal RAM if not available or full
core = (NavEKF2_core*)hal.util->malloc_type(sizeof(NavEKF2_core)*num_cores, AP_HAL::Util::MEM_FAST); core = (NavEKF2_core*)AP::dal().malloc_type(sizeof(NavEKF2_core)*num_cores, AP_DAL::MEM_FAST);
if (core == nullptr) { if (core == nullptr) {
initFailure = InitFailures::NO_MEM; initFailure = InitFailures::NO_MEM;
core_malloc_failed = true; core_malloc_failed = true;
gcs().send_text(MAV_SEVERITY_CRITICAL, "NavEKF2: memory allocation failed"); GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "NavEKF2: memory allocation failed");
return false; return false;
} }
@ -724,7 +690,7 @@ bool NavEKF2::InitialiseFilter(void)
hal.util->free_type(core, sizeof(NavEKF2_core)*num_cores, AP_HAL::Util::MEM_FAST); hal.util->free_type(core, sizeof(NavEKF2_core)*num_cores, AP_HAL::Util::MEM_FAST);
core = nullptr; core = nullptr;
initFailure = InitFailures::NO_SETUP; initFailure = InitFailures::NO_SETUP;
gcs().send_text(MAV_SEVERITY_WARNING, "NavEKF2: core %d setup failed", num_cores); GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "NavEKF2: core %d setup failed", num_cores);
return false; return false;
} }
num_cores++; num_cores++;
@ -750,7 +716,6 @@ bool NavEKF2::InitialiseFilter(void)
memset((void *)&pos_reset_data, 0, sizeof(pos_reset_data)); memset((void *)&pos_reset_data, 0, sizeof(pos_reset_data));
memset(&pos_down_reset_data, 0, sizeof(pos_down_reset_data)); memset(&pos_down_reset_data, 0, sizeof(pos_down_reset_data));
check_log_write();
return ret; return ret;
} }
@ -783,14 +748,14 @@ bool NavEKF2::coreBetterScore(uint8_t new_core, uint8_t current_core) const
// Update Filter States - this should be called whenever new IMU data is available // Update Filter States - this should be called whenever new IMU data is available
void NavEKF2::UpdateFilter(void) void NavEKF2::UpdateFilter(void)
{ {
AP::dal().start_frame(AP_DAL::FrameType::UpdateFilterEKF2);
if (!core) { if (!core) {
return; return;
} }
imuSampleTime_us = AP_HAL::micros64(); imuSampleTime_us = AP::dal().micros64();
const AP_InertialSensor &ins = AP::ins();
bool statePredictEnabled[num_cores]; bool statePredictEnabled[num_cores];
for (uint8_t i=0; i<num_cores; i++) { for (uint8_t i=0; i<num_cores; i++) {
// if we have not overrun by more than 3 IMU frames, and we // if we have not overrun by more than 3 IMU frames, and we
@ -798,7 +763,7 @@ void NavEKF2::UpdateFilter(void)
// loop then suppress the prediction step. This allows // loop then suppress the prediction step. This allows
// multiple EKF instances to cooperate on scheduling // multiple EKF instances to cooperate on scheduling
if (core[i].getFramesSincePredict() < (_framesPerPrediction+3) && if (core[i].getFramesSincePredict() < (_framesPerPrediction+3) &&
(AP_HAL::micros() - ins.get_last_update_usec()) > _frameTimeUsec/3) { (AP::dal().micros64() - imuSampleTime_us) > _frameTimeUsec/3) {
statePredictEnabled[i] = false; statePredictEnabled[i] = false;
} else { } else {
statePredictEnabled[i] = true; statePredictEnabled[i] = true;
@ -842,11 +807,11 @@ void NavEKF2::UpdateFilter(void)
updateLaneSwitchPosResetData(newPrimaryIndex, primary); updateLaneSwitchPosResetData(newPrimaryIndex, primary);
updateLaneSwitchPosDownResetData(newPrimaryIndex, primary); updateLaneSwitchPosDownResetData(newPrimaryIndex, primary);
primary = newPrimaryIndex; primary = newPrimaryIndex;
lastLaneSwitch_ms = AP_HAL::millis(); lastLaneSwitch_ms = AP::dal().millis();
} }
} }
if (primary != 0 && core[0].healthy() && !hal.util->get_soft_armed()) { if (primary != 0 && core[0].healthy() && !AP::dal().get_armed()) {
// when on the ground and disarmed force the first lane. This // when on the ground and disarmed force the first lane. This
// avoids us ending with with a lottery for which IMU is used // avoids us ending with with a lottery for which IMU is used
// in each flight. Otherwise the alignment of the timing of // in each flight. Otherwise the alignment of the timing of
@ -857,8 +822,6 @@ void NavEKF2::UpdateFilter(void)
// performance // performance
primary = 0; primary = 0;
} }
check_log_write();
} }
/* /*
@ -869,7 +832,8 @@ void NavEKF2::UpdateFilter(void)
*/ */
void NavEKF2::checkLaneSwitch(void) void NavEKF2::checkLaneSwitch(void)
{ {
uint32_t now = AP_HAL::millis(); AP::dal().log_event2(AP_DAL::Event2::checkLaneSwitch);
uint32_t now = AP::dal().millis();
if (lastLaneSwitch_ms != 0 && now - lastLaneSwitch_ms < 5000) { if (lastLaneSwitch_ms != 0 && now - lastLaneSwitch_ms < 5000) {
// don't switch twice in 5 seconds // don't switch twice in 5 seconds
return; return;
@ -893,7 +857,7 @@ void NavEKF2::checkLaneSwitch(void)
updateLaneSwitchPosDownResetData(newPrimaryIndex, primary); updateLaneSwitchPosDownResetData(newPrimaryIndex, primary);
primary = newPrimaryIndex; primary = newPrimaryIndex;
lastLaneSwitch_ms = now; lastLaneSwitch_ms = now;
gcs().send_text(MAV_SEVERITY_CRITICAL, "NavEKF2: lane switch %u", primary); GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "NavEKF2: lane switch %u", primary);
} }
} }
@ -910,16 +874,16 @@ bool NavEKF2::healthy(void) const
bool NavEKF2::pre_arm_check(char *failure_msg, uint8_t failure_msg_len) const bool NavEKF2::pre_arm_check(char *failure_msg, uint8_t failure_msg_len) const
{ {
if (!core) { if (!core) {
hal.util->snprintf(failure_msg, failure_msg_len, "no EKF2 cores"); AP::dal().snprintf(failure_msg, failure_msg_len, "no EKF2 cores");
return false; return false;
} }
for (uint8_t i = 0; i < num_cores; i++) { for (uint8_t i = 0; i < num_cores; i++) {
if (!core[i].healthy()) { if (!core[i].healthy()) {
const char *failure = core[primary].prearm_failure_reason(); const char *failure = core[primary].prearm_failure_reason();
if (failure != nullptr) { if (failure != nullptr) {
hal.util->snprintf(failure_msg, failure_msg_len, failure); AP::dal().snprintf(failure_msg, failure_msg_len, failure);
} else { } else {
hal.util->snprintf(failure_msg, failure_msg_len, "EKF2 core %d unhealthy", (int)i); AP::dal().snprintf(failure_msg, failure_msg_len, "EKF2 core %d unhealthy", (int)i);
} }
return false; return false;
} }
@ -1029,6 +993,8 @@ void NavEKF2::getTiltError(int8_t instance, float &ang) const
// reset body axis gyro bias estimates // reset body axis gyro bias estimates
void NavEKF2::resetGyroBias(void) void NavEKF2::resetGyroBias(void)
{ {
AP::dal().log_event2(AP_DAL::Event2::resetGyroBias);
if (core) { if (core) {
for (uint8_t i=0; i<num_cores; i++) { for (uint8_t i=0; i<num_cores; i++) {
core[i].resetGyroBias(); core[i].resetGyroBias();
@ -1043,6 +1009,8 @@ void NavEKF2::resetGyroBias(void)
// If using a range finder for height no reset is performed and it returns false // If using a range finder for height no reset is performed and it returns false
bool NavEKF2::resetHeightDatum(void) bool NavEKF2::resetHeightDatum(void)
{ {
AP::dal().log_event2(AP_DAL::Event2::resetHeightDatum);
bool status = true; bool status = true;
if (core) { if (core) {
for (uint8_t i=0; i<num_cores; i++) { for (uint8_t i=0; i<num_cores; i++) {
@ -1064,12 +1032,26 @@ bool NavEKF2::resetHeightDatum(void)
// Returns 2 if attitude, 3D-velocity, vertical position and relative horizontal position will be provided // Returns 2 if attitude, 3D-velocity, vertical position and relative horizontal position will be provided
uint8_t NavEKF2::setInhibitGPS(void) uint8_t NavEKF2::setInhibitGPS(void)
{ {
AP::dal().log_event2(AP_DAL::Event2::setInhibitGPS);
if (!core) { if (!core) {
return 0; return 0;
} }
return core[primary].setInhibitGPS(); return core[primary].setInhibitGPS();
} }
// Set the argument to true to prevent the EKF using the GPS vertical velocity
// This can be used for situations where GPS velocity errors are causing problems with height accuracy
void NavEKF2::setInhibitGpsVertVelUse(const bool varIn) {
if (varIn) {
AP::dal().log_event2(AP_DAL::Event2::setInhibitGpsVertVelUse);
} else {
AP::dal().log_event2(AP_DAL::Event2::unsetInhibitGpsVertVelUse);
}
inhibitGpsVertVelUse = varIn;
};
// return the horizontal speed limit in m/s set by optical flow sensor limits // return the horizontal speed limit in m/s set by optical flow sensor limits
// return the scale factor to be applied to navigation velocity gains to compensate for increase in velocity noise with height when using optical flow // return the scale factor to be applied to navigation velocity gains to compensate for increase in velocity noise with height when using optical flow
void NavEKF2::getEkfControlLimits(float &ekfGndSpdLimit, float &ekfNavVelGainScaler) const void NavEKF2::getEkfControlLimits(float &ekfGndSpdLimit, float &ekfNavVelGainScaler) const
@ -1179,6 +1161,8 @@ bool NavEKF2::getOriginLLH(int8_t instance, struct Location &loc) const
// Returns false if the filter has rejected the attempt to set the origin // Returns false if the filter has rejected the attempt to set the origin
bool NavEKF2::setOriginLLH(const Location &loc) bool NavEKF2::setOriginLLH(const Location &loc)
{ {
AP::dal().log_SetOriginLLH2(loc);
if (!core) { if (!core) {
return false; return false;
} }
@ -1186,7 +1170,7 @@ bool NavEKF2::setOriginLLH(const Location &loc)
// we don't allow setting of the EKF origin unless we are // we don't allow setting of the EKF origin unless we are
// flying in non-GPS mode. This is to prevent accidental set // flying in non-GPS mode. This is to prevent accidental set
// of EKF origin with invalid position or height // of EKF origin with invalid position or height
gcs().send_text(MAV_SEVERITY_WARNING, "EKF2 refusing set origin"); GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "EKF2 refusing set origin");
return false; return false;
} }
bool ret = false; bool ret = false;
@ -1322,6 +1306,12 @@ bool NavEKF2::getRangeBeaconDebug(int8_t instance, uint8_t &ID, float &rng, floa
// causes the EKF to compensate for expected barometer errors due to ground effect // causes the EKF to compensate for expected barometer errors due to ground effect
void NavEKF2::setTakeoffExpected(bool val) void NavEKF2::setTakeoffExpected(bool val)
{ {
if (val) {
AP::dal().log_event2(AP_DAL::Event2::setTakeoffExpected);
} else {
AP::dal().log_event2(AP_DAL::Event2::unsetTakeoffExpected);
}
if (core) { if (core) {
for (uint8_t i=0; i<num_cores; i++) { for (uint8_t i=0; i<num_cores; i++) {
core[i].setTakeoffExpected(val); core[i].setTakeoffExpected(val);
@ -1333,6 +1323,12 @@ void NavEKF2::setTakeoffExpected(bool val)
// causes the EKF to compensate for expected barometer errors due to ground effect // causes the EKF to compensate for expected barometer errors due to ground effect
void NavEKF2::setTouchdownExpected(bool val) void NavEKF2::setTouchdownExpected(bool val)
{ {
if (val) {
AP::dal().log_event2(AP_DAL::Event2::setTouchdownExpected);
} else {
AP::dal().log_event2(AP_DAL::Event2::unsetTouchdownExpected);
}
if (core) { if (core) {
for (uint8_t i=0; i<num_cores; i++) { for (uint8_t i=0; i<num_cores; i++) {
core[i].setTouchdownExpected(val); core[i].setTouchdownExpected(val);
@ -1346,6 +1342,11 @@ void NavEKF2::setTouchdownExpected(bool val)
// enabled by the combination of EK2_RNG_AID_HGT and EK2_RNG_USE_SPD parameters. // enabled by the combination of EK2_RNG_AID_HGT and EK2_RNG_USE_SPD parameters.
void NavEKF2::setTerrainHgtStable(bool val) void NavEKF2::setTerrainHgtStable(bool val)
{ {
if (val) {
AP::dal().log_event2(AP_DAL::Event2::setTerrainHgtStable);
} else {
AP::dal().log_event2(AP_DAL::Event2::unsetTerrainHgtStable);
}
if (core) { if (core) {
for (uint8_t i=0; i<num_cores; i++) { for (uint8_t i=0; i<num_cores; i++) {
core[i].setTerrainHgtStable(val); core[i].setTerrainHgtStable(val);
@ -1568,6 +1569,8 @@ uint32_t NavEKF2::getLastPosDownReset(float &posDelta)
// update the yaw reset data to capture changes due to a lane switch // update the yaw reset data to capture changes due to a lane switch
void NavEKF2::updateLaneSwitchYawResetData(uint8_t new_primary, uint8_t old_primary) void NavEKF2::updateLaneSwitchYawResetData(uint8_t new_primary, uint8_t old_primary)
{ {
// AP_DAL::log_updateLaneSwitchYawResetData(new_primary, old_primary);
Vector3f eulers_old_primary, eulers_new_primary; Vector3f eulers_old_primary, eulers_new_primary;
float old_yaw_delta; float old_yaw_delta;
@ -1594,6 +1597,8 @@ void NavEKF2::updateLaneSwitchYawResetData(uint8_t new_primary, uint8_t old_prim
// update the position reset data to capture changes due to a lane switch // update the position reset data to capture changes due to a lane switch
void NavEKF2::updateLaneSwitchPosResetData(uint8_t new_primary, uint8_t old_primary) void NavEKF2::updateLaneSwitchPosResetData(uint8_t new_primary, uint8_t old_primary)
{ {
// AP_DAL::log_updateLaneSwitchPosResetData(new_primary, old_primary);
Vector2f pos_old_primary, pos_new_primary, old_pos_delta; Vector2f pos_old_primary, pos_new_primary, old_pos_delta;
// If core position reset data has been consumed reset delta to zero // If core position reset data has been consumed reset delta to zero
@ -1621,6 +1626,8 @@ void NavEKF2::updateLaneSwitchPosResetData(uint8_t new_primary, uint8_t old_prim
// new primary EKF update has been run // new primary EKF update has been run
void NavEKF2::updateLaneSwitchPosDownResetData(uint8_t new_primary, uint8_t old_primary) void NavEKF2::updateLaneSwitchPosDownResetData(uint8_t new_primary, uint8_t old_primary)
{ {
// AP_DAL::log_updateLaneSwitchPosResetData(new_primary, old_primary);
float posDownOldPrimary, posDownNewPrimary, oldPosDownDelta; float posDownOldPrimary, posDownNewPrimary, oldPosDownDelta;
// If core position reset data has been consumed reset delta to zero // If core position reset data has been consumed reset delta to zero
@ -1692,6 +1699,8 @@ bool NavEKF2::isExtNavUsedForYaw() const
void NavEKF2::requestYawReset(void) void NavEKF2::requestYawReset(void)
{ {
AP::dal().log_event2(AP_DAL::Event2::requestYawReset);
for (uint8_t i = 0; i < num_cores; i++) { for (uint8_t i = 0; i < num_cores; i++) {
core[i].EKFGSF_requestYawReset(); core[i].EKFGSF_requestYawReset();
} }
@ -1713,6 +1722,8 @@ bool NavEKF2::getDataEKFGSF(int8_t instance, float &yaw_composite, float &yaw_co
// Writes the default equivalent airspeed in m/s to be used in forward flight if a measured airspeed is required and not available. // Writes the default equivalent airspeed in m/s to be used in forward flight if a measured airspeed is required and not available.
void NavEKF2::writeDefaultAirSpeed(float airspeed) void NavEKF2::writeDefaultAirSpeed(float airspeed)
{ {
AP::dal().log_writeDefaultAirSpeed2(airspeed);
if (core) { if (core) {
for (uint8_t i=0; i<num_cores; i++) { for (uint8_t i=0; i<num_cores; i++) {
core[i].writeDefaultAirSpeed(airspeed); core[i].writeDefaultAirSpeed(airspeed);
@ -1728,6 +1739,8 @@ void NavEKF2::writeDefaultAirSpeed(float airspeed)
*/ */
void NavEKF2::writeExtNavVelData(const Vector3f &vel, float err, uint32_t timeStamp_ms, uint16_t delay_ms) void NavEKF2::writeExtNavVelData(const Vector3f &vel, float err, uint32_t timeStamp_ms, uint16_t delay_ms)
{ {
// AP_DAL::log_writeExtNavVelData(vel, err, timeStamp_ms, delay_ms);
if (core) { if (core) {
for (uint8_t i=0; i<num_cores; i++) { for (uint8_t i=0; i<num_cores; i++) {
core[i].writeExtNavVelData(vel, err, timeStamp_ms, delay_ms); core[i].writeExtNavVelData(vel, err, timeStamp_ms, delay_ms);

22
libraries/AP_NavEKF2/AP_NavEKF2.h
View File

@ -29,7 +29,6 @@
#include <AP_NavEKF/AP_Nav_Common.h> #include <AP_NavEKF/AP_Nav_Common.h>
class NavEKF2_core; class NavEKF2_core;
class AP_AHRS;
class NavEKF2 { class NavEKF2 {
friend class NavEKF2_core; friend class NavEKF2_core;
@ -54,9 +53,6 @@ public:
// Update Filter States - this should be called whenever new IMU data is available // Update Filter States - this should be called whenever new IMU data is available
void UpdateFilter(void); void UpdateFilter(void);
// check if we should write log messages
void check_log_write(void);
// Check basic filter health metrics and return a consolidated health status // Check basic filter health metrics and return a consolidated health status
bool healthy(void) const; bool healthy(void) const;
@ -128,7 +124,7 @@ public:
// Set the argument to true to prevent the EKF using the GPS vertical velocity // Set the argument to true to prevent the EKF using the GPS vertical velocity
// This can be used for situations where GPS velocity errors are causing problems with height accuracy // This can be used for situations where GPS velocity errors are causing problems with height accuracy
void setInhibitGpsVertVelUse(const bool varIn) { inhibitGpsVertVelUse = varIn; }; void setInhibitGpsVertVelUse(const bool varIn);
// return the horizontal speed limit in m/s set by optical flow sensor limits // return the horizontal speed limit in m/s set by optical flow sensor limits
// return the scale factor to be applied to navigation velocity gains to compensate for increase in velocity noise with height when using optical flow // return the scale factor to be applied to navigation velocity gains to compensate for increase in velocity noise with height when using optical flow
@ -319,7 +315,7 @@ public:
void set_enable(bool enable) { _enable.set_enable(enable); } void set_enable(bool enable) { _enable.set_enable(enable); }
// are we doing sensor logging inside the EKF? // are we doing sensor logging inside the EKF?
bool have_ekf_logging(void) const { return logging.enabled && _logging_mask != 0; } bool have_ekf_logging(void) const { return false; }
// get timing statistics structure // get timing statistics structure
void getTimingStatistics(int8_t instance, struct ekf_timing &timing) const; void getTimingStatistics(int8_t instance, struct ekf_timing &timing) const;
@ -381,7 +377,6 @@ private:
uint8_t primary; // current primary core uint8_t primary; // current primary core
NavEKF2_core *core = nullptr; NavEKF2_core *core = nullptr;
bool core_malloc_failed; bool core_malloc_failed;
const AP_AHRS *_ahrs;
uint32_t _frameTimeUsec; // time per IMU frame uint32_t _frameTimeUsec; // time per IMU frame
uint8_t _framesPerPrediction; // expected number of IMU frames per prediction uint8_t _framesPerPrediction; // expected number of IMU frames per prediction
@ -423,7 +418,6 @@ private:
AP_Int8 _imuMask; // Bitmask of IMUs to instantiate EKF2 for AP_Int8 _imuMask; // Bitmask of IMUs to instantiate EKF2 for
AP_Int16 _gpsCheckScaler; // Percentage increase to be applied to GPS pre-flight accuracy and drift thresholds AP_Int16 _gpsCheckScaler; // Percentage increase to be applied to GPS pre-flight accuracy and drift thresholds
AP_Float _noaidHorizNoise; // horizontal position measurement noise assumed when synthesised zero position measurements are used to constrain attitude drift : m AP_Float _noaidHorizNoise; // horizontal position measurement noise assumed when synthesised zero position measurements are used to constrain attitude drift : m
AP_Int8 _logging_mask; // mask of IMUs to log
AP_Float _yawNoise; // magnetic yaw measurement noise : rad AP_Float _yawNoise; // magnetic yaw measurement noise : rad
AP_Int16 _yawInnovGate; // Percentage number of standard deviations applied to magnetic yaw innovation consistency check AP_Int16 _yawInnovGate; // Percentage number of standard deviations applied to magnetic yaw innovation consistency check
AP_Int8 _tauVelPosOutput; // Time constant of output complementary filter : csec (centi-seconds) AP_Int8 _tauVelPosOutput; // Time constant of output complementary filter : csec (centi-seconds)
@ -480,13 +474,6 @@ private:
struct Location common_EKF_origin; struct Location common_EKF_origin;
bool common_origin_valid; bool common_origin_valid;
struct {
bool enabled:1;
bool log_compass:1;
bool log_baro:1;
bool log_imu:1;
} logging;
// time at start of current filter update // time at start of current filter update
uint64_t imuSampleTime_us; uint64_t imuSampleTime_us;
@ -562,8 +549,9 @@ private:
void Log_Write_NKF2(uint8_t core, uint64_t time_us) const; void Log_Write_NKF2(uint8_t core, uint64_t time_us) const;
void Log_Write_NKF3(uint8_t core, uint64_t time_us) const; void Log_Write_NKF3(uint8_t core, uint64_t time_us) const;
void Log_Write_NKF4(uint8_t core, uint64_t time_us) const; void Log_Write_NKF4(uint8_t core, uint64_t time_us) const;
void Log_Write_NKF5(uint64_t time_us) const; void Log_Write_NKF5(uint8_t core, uint64_t time_us) const;
void Log_Write_Quaternion(uint8_t core, uint64_t time_us) const; void Log_Write_Quaternion(uint8_t core, uint64_t time_us) const;
void Log_Write_Beacon(uint64_t time_us) const; void Log_Write_Beacon(uint8_t core, uint64_t time_us) const;
void Log_Write_Timing(uint8_t core, uint64_t time_us) const;
void Log_Write_GSF(uint8_t core, uint64_t time_us) const; void Log_Write_GSF(uint8_t core, uint64_t time_us) const;
}; };

15
libraries/AP_NavEKF2/AP_NavEKF2_AirDataFusion.cpp
View File

@ -2,7 +2,6 @@
#include "AP_NavEKF2.h" #include "AP_NavEKF2.h"
#include "AP_NavEKF2_core.h" #include "AP_NavEKF2_core.h"
#include <AP_AHRS/AP_AHRS.h>
extern const AP_HAL::HAL& hal; extern const AP_HAL::HAL& hal;
@ -21,16 +20,13 @@ extern const AP_HAL::HAL& hal;
*/ */
void NavEKF2_core::FuseAirspeed() void NavEKF2_core::FuseAirspeed()
{ {
// start performance timer
hal.util->perf_begin(_perf_FuseAirspeed);
// declarations // declarations
float vn; float vn;
float ve; float ve;
float vd; float vd;
float vwn; float vwn;
float vwe; float vwe;
float EAS2TAS = _ahrs->get_EAS2TAS(); float EAS2TAS = AP::dal().get_EAS2TAS();
const float R_TAS = sq(constrain_float(frontend->_easNoise, 0.5f, 5.0f) * constrain_float(EAS2TAS, 0.9f, 10.0f)); const float R_TAS = sq(constrain_float(frontend->_easNoise, 0.5f, 5.0f) * constrain_float(EAS2TAS, 0.9f, 10.0f));
Vector3 SH_TAS; Vector3 SH_TAS;
float SK_TAS; float SK_TAS;
@ -178,9 +174,6 @@ void NavEKF2_core::FuseAirspeed()
// force the covariance matrix to be symmetrical and limit the variances to prevent ill-conditioning. // force the covariance matrix to be symmetrical and limit the variances to prevent ill-conditioning.
ForceSymmetry(); ForceSymmetry();
ConstrainVariances(); ConstrainVariances();
// stop performance timer
hal.util->perf_end(_perf_FuseAirspeed);
} }
// select fusion of true airspeed measurements // select fusion of true airspeed measurements
@ -247,9 +240,6 @@ void NavEKF2_core::SelectBetaFusion()
*/ */
void NavEKF2_core::FuseSideslip() void NavEKF2_core::FuseSideslip()
{ {
// start performance timer
hal.util->perf_begin(_perf_FuseSideslip);
// declarations // declarations
float q0; float q0;
float q1; float q1;
@ -425,9 +415,6 @@ void NavEKF2_core::FuseSideslip()
// force the covariance matrix to be symmetrical and limit the variances to prevent ill-conditioning. // force the covariance matrix to be symmetrical and limit the variances to prevent ill-conditioning.
ForceSymmetry(); ForceSymmetry();
ConstrainVariances(); ConstrainVariances();
// stop the performance timer
hal.util->perf_end(_perf_FuseSideslip);
} }
/******************************************************** /********************************************************

33
libraries/AP_NavEKF2/AP_NavEKF2_Control.cpp
View File

@ -2,7 +2,6 @@
#include "AP_NavEKF2.h" #include "AP_NavEKF2.h"
#include "AP_NavEKF2_core.h" #include "AP_NavEKF2_core.h"
#include <AP_AHRS/AP_AHRS.h>
#include <GCS_MAVLink/GCS.h> #include <GCS_MAVLink/GCS.h>
#include <stdio.h> #include <stdio.h>
@ -15,7 +14,7 @@ void NavEKF2_core::controlFilterModes()
{ {
// Determine motor arm status // Determine motor arm status
prevMotorsArmed = motorsArmed; prevMotorsArmed = motorsArmed;
motorsArmed = hal.util->get_soft_armed(); motorsArmed = AP::dal().get_armed();
if (motorsArmed && !prevMotorsArmed) { if (motorsArmed && !prevMotorsArmed) {
// set the time at which we arm to assist with checks // set the time at which we arm to assist with checks
timeAtArming_ms = imuSampleTime_ms; timeAtArming_ms = imuSampleTime_ms;
@ -72,7 +71,7 @@ void NavEKF2_core::setWindMagStateLearningMode()
// set the wind sate variances to the measurement uncertainty // set the wind sate variances to the measurement uncertainty
for (uint8_t index=22; index<=23; index++) { for (uint8_t index=22; index<=23; index++) {
P[index][index] = sq(constrain_float(frontend->_easNoise, 0.5f, 5.0f) * constrain_float(_ahrs->get_EAS2TAS(), 0.9f, 10.0f)); P[index][index] = sq(constrain_float(frontend->_easNoise, 0.5f, 5.0f) * constrain_float(AP::dal().get_EAS2TAS(), 0.9f, 10.0f));
} }
} else { } else {
// set the variances using a typical wind speed // set the variances using a typical wind speed
@ -273,7 +272,7 @@ void NavEKF2_core::setAidingMode()
switch (PV_AidingMode) { switch (PV_AidingMode) {
case AID_NONE: case AID_NONE:
// We have ceased aiding // We have ceased aiding
gcs().send_text(MAV_SEVERITY_WARNING, "EKF2 IMU%u has stopped aiding",(unsigned)imu_index); GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "EKF2 IMU%u has stopped aiding",(unsigned)imu_index);
// When not aiding, estimate orientation & height fusing synthetic constant position and zero velocity measurement to constrain tilt errors // When not aiding, estimate orientation & height fusing synthetic constant position and zero velocity measurement to constrain tilt errors
posTimeout = true; posTimeout = true;
velTimeout = true; velTimeout = true;
@ -292,7 +291,7 @@ void NavEKF2_core::setAidingMode()
case AID_RELATIVE: case AID_RELATIVE:
// We have commenced aiding, but GPS usage has been prohibited so use optical flow only // We have commenced aiding, but GPS usage has been prohibited so use optical flow only
gcs().send_text(MAV_SEVERITY_INFO, "EKF2 IMU%u is using optical flow",(unsigned)imu_index); GCS_SEND_TEXT(MAV_SEVERITY_INFO, "EKF2 IMU%u is using optical flow",(unsigned)imu_index);
posTimeout = true; posTimeout = true;
velTimeout = true; velTimeout = true;
// Reset the last valid flow measurement time // Reset the last valid flow measurement time
@ -307,20 +306,20 @@ void NavEKF2_core::setAidingMode()
bool canUseExtNav = readyToUseExtNav(); bool canUseExtNav = readyToUseExtNav();
// We have commenced aiding and GPS usage is allowed // We have commenced aiding and GPS usage is allowed
if (canUseGPS) { if (canUseGPS) {
gcs().send_text(MAV_SEVERITY_INFO, "EKF2 IMU%u is using GPS",(unsigned)imu_index); GCS_SEND_TEXT(MAV_SEVERITY_INFO, "EKF2 IMU%u is using GPS",(unsigned)imu_index);
} }
posTimeout = false; posTimeout = false;
velTimeout = false; velTimeout = false;
// We have commenced aiding and range beacon usage is allowed // We have commenced aiding and range beacon usage is allowed
if (canUseRangeBeacon) { if (canUseRangeBeacon) {
gcs().send_text(MAV_SEVERITY_INFO, "EKF2 IMU%u is using range beacons",(unsigned)imu_index); GCS_SEND_TEXT(MAV_SEVERITY_INFO, "EKF2 IMU%u is using range beacons",(unsigned)imu_index);
gcs().send_text(MAV_SEVERITY_INFO, "EKF2 IMU%u initial pos NE = %3.1f,%3.1f (m)",(unsigned)imu_index,(double)receiverPos.x,(double)receiverPos.y); GCS_SEND_TEXT(MAV_SEVERITY_INFO, "EKF2 IMU%u initial pos NE = %3.1f,%3.1f (m)",(unsigned)imu_index,(double)receiverPos.x,(double)receiverPos.y);
gcs().send_text(MAV_SEVERITY_INFO, "EKF2 IMU%u initial beacon pos D offset = %3.1f (m)",(unsigned)imu_index,(double)bcnPosOffset); GCS_SEND_TEXT(MAV_SEVERITY_INFO, "EKF2 IMU%u initial beacon pos D offset = %3.1f (m)",(unsigned)imu_index,(double)bcnPosOffset);
} }
// We have commenced aiding and external nav usage is allowed // We have commenced aiding and external nav usage is allowed
if (canUseExtNav) { if (canUseExtNav) {
gcs().send_text(MAV_SEVERITY_INFO, "EKF2 IMU%u is using external nav data",(unsigned)imu_index); GCS_SEND_TEXT(MAV_SEVERITY_INFO, "EKF2 IMU%u is using external nav data",(unsigned)imu_index);
gcs().send_text(MAV_SEVERITY_INFO, "EKF2 IMU%u initial pos NED = %3.1f,%3.1f,%3.1f (m)",(unsigned)imu_index,(double)extNavDataDelayed.pos.x,(double)extNavDataDelayed.pos.y,(double)extNavDataDelayed.pos.z); GCS_SEND_TEXT(MAV_SEVERITY_INFO, "EKF2 IMU%u initial pos NED = %3.1f,%3.1f,%3.1f (m)",(unsigned)imu_index,(double)extNavDataDelayed.pos.x,(double)extNavDataDelayed.pos.y,(double)extNavDataDelayed.pos.z);
//handle yaw reset as special case only if compass is disabled //handle yaw reset as special case only if compass is disabled
if (!use_compass()) { if (!use_compass()) {
extNavYawResetRequest = true; extNavYawResetRequest = true;
@ -356,7 +355,7 @@ void NavEKF2_core::checkAttitudeAlignmentStatus()
tiltErrFilt = alpha*temp + (1.0f-alpha)*tiltErrFilt; tiltErrFilt = alpha*temp + (1.0f-alpha)*tiltErrFilt;
if (tiltErrFilt < 0.005f && !tiltAlignComplete) { if (tiltErrFilt < 0.005f && !tiltAlignComplete) {
tiltAlignComplete = true; tiltAlignComplete = true;
gcs().send_text(MAV_SEVERITY_INFO, "EKF2 IMU%u tilt alignment complete",(unsigned)imu_index); GCS_SEND_TEXT(MAV_SEVERITY_INFO, "EKF2 IMU%u tilt alignment complete",(unsigned)imu_index);
} }
// submit yaw and magnetic field reset requests depending on whether we have compass data // submit yaw and magnetic field reset requests depending on whether we have compass data
@ -374,7 +373,7 @@ void NavEKF2_core::checkAttitudeAlignmentStatus()
// return true if we should use the airspeed sensor // return true if we should use the airspeed sensor
bool NavEKF2_core::useAirspeed(void) const bool NavEKF2_core::useAirspeed(void) const
{ {
return _ahrs->airspeed_sensor_enabled(); return AP::dal().airspeed_sensor_enabled();
} }
// return true if we should use the range finder sensor // return true if we should use the range finder sensor
@ -411,7 +410,7 @@ bool NavEKF2_core::readyToUseExtNav(void) const
// return true if we should use the compass // return true if we should use the compass
bool NavEKF2_core::use_compass(void) const bool NavEKF2_core::use_compass(void) const
{ {
return _ahrs->get_compass() && _ahrs->get_compass()->use_for_yaw(magSelectIndex) && !allMagSensorsFailed; return AP::dal().get_compass() && AP::dal().get_compass()->use_for_yaw(magSelectIndex) && !allMagSensorsFailed;
} }
/* /*
@ -422,7 +421,7 @@ bool NavEKF2_core::assume_zero_sideslip(void) const
// we don't assume zero sideslip for ground vehicles as EKF could // we don't assume zero sideslip for ground vehicles as EKF could
// be quite sensitive to a rapid spin of the ground vehicle if // be quite sensitive to a rapid spin of the ground vehicle if
// traction is lost // traction is lost
return _ahrs->get_fly_forward() && _ahrs->get_vehicle_class() != AHRS_VEHICLE_GROUND; return AP::dal().get_fly_forward() && AP::dal().get_vehicle_class() != AP_DAL::VehicleClass::GROUND;
} }
// set the LLH location of the filters NED origin // set the LLH location of the filters NED origin
@ -451,7 +450,7 @@ void NavEKF2_core::setOrigin(const Location &loc)
// define Earth rotation vector in the NED navigation frame at the origin // define Earth rotation vector in the NED navigation frame at the origin
calcEarthRateNED(earthRateNED, EKF_origin.lat); calcEarthRateNED(earthRateNED, EKF_origin.lat);
validOrigin = true; validOrigin = true;
gcs().send_text(MAV_SEVERITY_INFO, "EKF2 IMU%u origin set",(unsigned)imu_index); GCS_SEND_TEXT(MAV_SEVERITY_INFO, "EKF2 IMU%u origin set",(unsigned)imu_index);
// put origin in frontend as well to ensure it stays in sync between lanes // put origin in frontend as well to ensure it stays in sync between lanes
frontend->common_EKF_origin = EKF_origin; frontend->common_EKF_origin = EKF_origin;
@ -548,7 +547,7 @@ void NavEKF2_core::runYawEstimatorPrediction()
if (imuDataDelayed.time_ms - tasDataDelayed.time_ms < 5000) { if (imuDataDelayed.time_ms - tasDataDelayed.time_ms < 5000) {
trueAirspeed = tasDataDelayed.tas; trueAirspeed = tasDataDelayed.tas;
} else { } else {
trueAirspeed = defaultAirSpeed * AP::ahrs().get_EAS2TAS(); trueAirspeed = defaultAirSpeed * AP::dal().get_EAS2TAS();
} }
} else { } else {
trueAirspeed = 0.0f; trueAirspeed = 0.0f;

93
libraries/AP_NavEKF2/AP_NavEKF2_Logging.cpp
View File

@ -2,6 +2,7 @@
#include <AP_HAL/HAL.h> #include <AP_HAL/HAL.h>
#include <AP_Logger/AP_Logger.h> #include <AP_Logger/AP_Logger.h>
#include <AP_DAL/AP_DAL.h>
void NavEKF2::Log_Write_NKF1(uint8_t _core, uint64_t time_us) const void NavEKF2::Log_Write_NKF1(uint8_t _core, uint64_t time_us) const
{ {
@ -25,7 +26,7 @@ void NavEKF2::Log_Write_NKF1(uint8_t _core, uint64_t time_us) const
const struct log_EKF1 pkt{ const struct log_EKF1 pkt{
LOG_PACKET_HEADER_INIT(LOG_NKF1_MSG), LOG_PACKET_HEADER_INIT(LOG_NKF1_MSG),
time_us : time_us, time_us : time_us,
core : _core, core : DAL_CORE(_core),
roll : (int16_t)(100*degrees(euler.x)), // roll angle (centi-deg, displayed as deg due to format string) roll : (int16_t)(100*degrees(euler.x)), // roll angle (centi-deg, displayed as deg due to format string)
pitch : (int16_t)(100*degrees(euler.y)), // pitch angle (centi-deg, displayed as deg due to format string) pitch : (int16_t)(100*degrees(euler.y)), // pitch angle (centi-deg, displayed as deg due to format string)
yaw : (uint16_t)wrap_360_cd(100*degrees(euler.z)), // yaw angle (centi-deg, displayed as deg due to format string) yaw : (uint16_t)wrap_360_cd(100*degrees(euler.z)), // yaw angle (centi-deg, displayed as deg due to format string)
@ -61,7 +62,7 @@ void NavEKF2::Log_Write_NKF2(uint8_t _core, uint64_t time_us) const
const struct log_NKF2 pkt2{ const struct log_NKF2 pkt2{
LOG_PACKET_HEADER_INIT(LOG_NKF2_MSG), LOG_PACKET_HEADER_INIT(LOG_NKF2_MSG),
time_us : time_us, time_us : time_us,
core : _core, core : DAL_CORE(_core),
AZbias : (int8_t)(100*azbias), AZbias : (int8_t)(100*azbias),
scaleX : (int16_t)(100*gyroScaleFactor.x), scaleX : (int16_t)(100*gyroScaleFactor.x),
scaleY : (int16_t)(100*gyroScaleFactor.y), scaleY : (int16_t)(100*gyroScaleFactor.y),
@ -91,7 +92,7 @@ void NavEKF2::Log_Write_NKF3(uint8_t _core, uint64_t time_us) const
const struct log_NKF3 pkt3{ const struct log_NKF3 pkt3{
LOG_PACKET_HEADER_INIT(LOG_NKF3_MSG), LOG_PACKET_HEADER_INIT(LOG_NKF3_MSG),
time_us : time_us, time_us : time_us,
core : _core, core : DAL_CORE(_core),
innovVN : (int16_t)(100*velInnov.x), innovVN : (int16_t)(100*velInnov.x),
innovVE : (int16_t)(100*velInnov.y), innovVE : (int16_t)(100*velInnov.y),
innovVD : (int16_t)(100*velInnov.z), innovVD : (int16_t)(100*velInnov.z),
@ -134,7 +135,7 @@ void NavEKF2::Log_Write_NKF4(uint8_t _core, uint64_t time_us) const
const struct log_NKF4 pkt4{ const struct log_NKF4 pkt4{
LOG_PACKET_HEADER_INIT(LOG_NKF4_MSG), LOG_PACKET_HEADER_INIT(LOG_NKF4_MSG),
time_us : time_us, time_us : time_us,
core : _core, core : DAL_CORE(_core),
sqrtvarV : (int16_t)(100*velVar), sqrtvarV : (int16_t)(100*velVar),
sqrtvarP : (int16_t)(100*posVar), sqrtvarP : (int16_t)(100*posVar),
sqrtvarH : (int16_t)(100*hgtVar), sqrtvarH : (int16_t)(100*hgtVar),
@ -152,9 +153,14 @@ void NavEKF2::Log_Write_NKF4(uint8_t _core, uint64_t time_us) const
AP::logger().WriteBlock(&pkt4, sizeof(pkt4)); AP::logger().WriteBlock(&pkt4, sizeof(pkt4));
} }
void NavEKF2::Log_Write_NKF5(uint64_t time_us) const void NavEKF2::Log_Write_NKF5(uint8_t _core, uint64_t time_us) const
{ {
// Write fifth EKF packet - take data from the primary instance if (_core != primary) {
// log only primary instance for now
return;
}
// Write fifth EKF packet
float normInnov=0; // normalised innovation variance ratio for optical flow observations fused by the main nav filter float normInnov=0; // normalised innovation variance ratio for optical flow observations fused by the main nav filter
float gndOffset=0; // estimated vertical position of the terrain relative to the nav filter zero datum float gndOffset=0; // estimated vertical position of the terrain relative to the nav filter zero datum
float flowInnovX=0, flowInnovY=0; // optical flow LOS rate vector innovations from the main nav filter float flowInnovX=0, flowInnovY=0; // optical flow LOS rate vector innovations from the main nav filter
@ -164,11 +170,12 @@ void NavEKF2::Log_Write_NKF5(uint64_t time_us) const
float range=0; // measured range float range=0; // measured range
float gndOffsetErr=0; // filter ground offset state error float gndOffsetErr=0; // filter ground offset state error
Vector3f predictorErrors; // output predictor angle, velocity and position tracking error Vector3f predictorErrors; // output predictor angle, velocity and position tracking error
getFlowDebug(-1,normInnov, gndOffset, flowInnovX, flowInnovY, auxFlowInnov, HAGL, rngInnov, range, gndOffsetErr); getFlowDebug(_core, normInnov, gndOffset, flowInnovX, flowInnovY, auxFlowInnov, HAGL, rngInnov, range, gndOffsetErr);
getOutputTrackingError(-1,predictorErrors); getOutputTrackingError(_core, predictorErrors);
const struct log_NKF5 pkt5{ const struct log_NKF5 pkt5{
LOG_PACKET_HEADER_INIT(LOG_NKF5_MSG), LOG_PACKET_HEADER_INIT(LOG_NKF5_MSG),
time_us : time_us, time_us : time_us,
core : DAL_CORE(_core),
normInnov : (uint8_t)(MIN(100*normInnov,255)), normInnov : (uint8_t)(MIN(100*normInnov,255)),
FIX : (int16_t)(1000*flowInnovX), FIX : (int16_t)(1000*flowInnovX),
FIY : (int16_t)(1000*flowInnovY), FIY : (int16_t)(1000*flowInnovY),
@ -193,7 +200,7 @@ void NavEKF2::Log_Write_Quaternion(uint8_t _core, uint64_t time_us) const
const struct log_Quaternion pktq1{ const struct log_Quaternion pktq1{
LOG_PACKET_HEADER_INIT(LOG_NKQ_MSG), LOG_PACKET_HEADER_INIT(LOG_NKQ_MSG),
time_us : time_us, time_us : time_us,
core : _core, core : DAL_CORE(_core),
q1 : quat.q1, q1 : quat.q1,
q2 : quat.q2, q2 : quat.q2,
q3 : quat.q3, q3 : quat.q3,
@ -202,8 +209,13 @@ void NavEKF2::Log_Write_Quaternion(uint8_t _core, uint64_t time_us) const
AP::logger().WriteBlock(&pktq1, sizeof(pktq1)); AP::logger().WriteBlock(&pktq1, sizeof(pktq1));
} }
void NavEKF2::Log_Write_Beacon(uint64_t time_us) const void NavEKF2::Log_Write_Beacon(uint8_t _core, uint64_t time_us) const
{ {
if (_core != primary) {
// log only primary instance for now
return;
}
if (AP::beacon() != nullptr) { if (AP::beacon() != nullptr) {
uint8_t ID; uint8_t ID;
float rng; float rng;
@ -213,11 +225,12 @@ void NavEKF2::Log_Write_Beacon(uint64_t time_us) const
Vector3f beaconPosNED; Vector3f beaconPosNED;
float bcnPosOffsetHigh; float bcnPosOffsetHigh;
float bcnPosOffsetLow; float bcnPosOffsetLow;
if (getRangeBeaconDebug(-1, ID, rng, innov, innovVar, testRatio, beaconPosNED, bcnPosOffsetHigh, bcnPosOffsetLow)) { if (getRangeBeaconDebug(_core, ID, rng, innov, innovVar, testRatio, beaconPosNED, bcnPosOffsetHigh, bcnPosOffsetLow)) {
if (rng > 0.0f) { if (rng > 0.0f) {
struct log_RngBcnDebug pkt10 = { struct log_RngBcnDebug pkt10 = {
LOG_PACKET_HEADER_INIT(LOG_NKF10_MSG), LOG_PACKET_HEADER_INIT(LOG_NKF10_MSG),
time_us : time_us, time_us : time_us,
core : DAL_CORE(_core),
ID : (uint8_t)ID, ID : (uint8_t)ID,
rng : (int16_t)(100*rng), rng : (int16_t)(100*rng),
innov : (int16_t)(100*innov), innov : (int16_t)(100*innov),
@ -238,6 +251,36 @@ void NavEKF2::Log_Write_Beacon(uint64_t time_us) const
} }
} }
void NavEKF2::Log_Write_Timing(uint8_t _core, uint64_t time_us) const
{
// log EKF timing statistics every 5s
static uint32_t lastTimingLogTime_ms = 0;
if (AP::dal().millis() - lastTimingLogTime_ms <= 5000) {
return;
}
lastTimingLogTime_ms = AP::dal().millis();
struct ekf_timing timing;
getTimingStatistics(_core, timing);
const struct log_NKT nkt{
LOG_PACKET_HEADER_INIT(LOG_NKT_MSG),
time_us : time_us,
core : _core,
timing_count : timing.count,
dtIMUavg_min : timing.dtIMUavg_min,
dtIMUavg_max : timing.dtIMUavg_max,
dtEKFavg_min : timing.dtEKFavg_min,
dtEKFavg_max : timing.dtEKFavg_max,
delAngDT_min : timing.delAngDT_min,
delAngDT_max : timing.delAngDT_max,
delVelDT_min : timing.delVelDT_min,
delVelDT_max : timing.delVelDT_max,
};
AP::logger().WriteBlock(&nkt, sizeof(nkt));
}
void NavEKF2::Log_Write() void NavEKF2::Log_Write()
{ {
// only log if enabled // only log if enabled
@ -245,32 +288,26 @@ void NavEKF2::Log_Write()
return; return;
} }
const uint64_t time_us = AP_HAL::micros64(); const uint64_t time_us = AP::dal().micros64();
Log_Write_NKF5(time_us);
// note that several of these functions exit-early if they're not
// attempting to log the primary core.
for (uint8_t i=0; i<activeCores(); i++) { for (uint8_t i=0; i<activeCores(); i++) {
Log_Write_NKF1(i, time_us); Log_Write_NKF1(i, time_us);
Log_Write_NKF2(i, time_us); Log_Write_NKF2(i, time_us);
Log_Write_NKF3(i, time_us); Log_Write_NKF3(i, time_us);
Log_Write_NKF4(i, time_us); Log_Write_NKF4(i, time_us);
Log_Write_NKF5(i, time_us);
Log_Write_Quaternion(i, time_us); Log_Write_Quaternion(i, time_us);
Log_Write_GSF(i, time_us); Log_Write_GSF(i, time_us);
// write range beacon fusion debug packet if the range value is non-zero
Log_Write_Beacon(i, time_us);
Log_Write_Timing(i, time_us);
} }
// write range beacon fusion debug packet if the range value is non-zero AP::dal().start_frame(AP_DAL::FrameType::LogWriteEKF2);
Log_Write_Beacon(time_us);
// log EKF timing statistics every 5s
static uint32_t lastTimingLogTime_ms = 0;
if (AP_HAL::millis() - lastTimingLogTime_ms > 5000) {
lastTimingLogTime_ms = AP_HAL::millis();
struct ekf_timing timing;
for (uint8_t i=0; i<activeCores(); i++) {
getTimingStatistics(i, timing);
Log_EKF_Timing("NKT", i, time_us, timing);
}
}
} }
void NavEKF2::Log_Write_GSF(uint8_t _core, uint64_t time_us) const void NavEKF2::Log_Write_GSF(uint8_t _core, uint64_t time_us) const
@ -306,7 +343,7 @@ void NavEKF2::Log_Write_GSF(uint8_t _core, uint64_t time_us) const
"F-000000000000", "F-000000000000",
"QBffffffffffff", "QBffffffffffff",
time_us, time_us,
_core, DAL_CORE(_core),
yaw_composite, yaw_composite,
sqrtf(MAX(yaw_composite_variance, 0.0f)), sqrtf(MAX(yaw_composite_variance, 0.0f)),
yaw[0], yaw[0],

22
libraries/AP_NavEKF2/AP_NavEKF2_MagFusion.cpp
View File

@ -115,7 +115,7 @@ void NavEKF2_core::controlMagYawReset()
// send initial alignment status to console // send initial alignment status to console
if (!yawAlignComplete) { if (!yawAlignComplete) {
gcs().send_text(MAV_SEVERITY_INFO, "EKF2 IMU%u ext nav yaw alignment complete",(unsigned)imu_index); GCS_SEND_TEXT(MAV_SEVERITY_INFO, "EKF2 IMU%u ext nav yaw alignment complete",(unsigned)imu_index);
} }
// record the reset as complete and also record the in-flight reset as complete to stop further resets when height is gained // record the reset as complete and also record the in-flight reset as complete to stop further resets when height is gained
@ -148,14 +148,14 @@ void NavEKF2_core::controlMagYawReset()
// send initial alignment status to console // send initial alignment status to console
if (!yawAlignComplete) { if (!yawAlignComplete) {
gcs().send_text(MAV_SEVERITY_INFO, "EKF2 IMU%u initial yaw alignment complete",(unsigned)imu_index); GCS_SEND_TEXT(MAV_SEVERITY_INFO, "EKF2 IMU%u initial yaw alignment complete",(unsigned)imu_index);
} }
// send in-flight yaw alignment status to console // send in-flight yaw alignment status to console
if (finalResetRequest) { if (finalResetRequest) {
gcs().send_text(MAV_SEVERITY_INFO, "EKF2 IMU%u in-flight yaw alignment complete",(unsigned)imu_index); GCS_SEND_TEXT(MAV_SEVERITY_INFO, "EKF2 IMU%u in-flight yaw alignment complete",(unsigned)imu_index);
} else if (interimResetRequest) { } else if (interimResetRequest) {
gcs().send_text(MAV_SEVERITY_WARNING, "EKF2 IMU%u ground mag anomaly, yaw re-aligned",(unsigned)imu_index); GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "EKF2 IMU%u ground mag anomaly, yaw re-aligned",(unsigned)imu_index);
} }
// update the yaw reset completed status // update the yaw reset completed status
@ -206,7 +206,7 @@ void NavEKF2_core::realignYawGPS()
ResetPosition(); ResetPosition();
// send yaw alignment information to console // send yaw alignment information to console
gcs().send_text(MAV_SEVERITY_INFO, "EKF2 IMU%u yaw aligned to GPS velocity",(unsigned)imu_index); GCS_SEND_TEXT(MAV_SEVERITY_INFO, "EKF2 IMU%u yaw aligned to GPS velocity",(unsigned)imu_index);
// zero the attitude covariances because the correlations will now be invalid // zero the attitude covariances because the correlations will now be invalid
zeroAttCovOnly(); zeroAttCovOnly();
@ -235,9 +235,6 @@ void NavEKF2_core::realignYawGPS()
// select fusion of magnetometer data // select fusion of magnetometer data
void NavEKF2_core::SelectMagFusion() void NavEKF2_core::SelectMagFusion()
{ {
// start performance timer
hal.util->perf_begin(_perf_FuseMagnetometer);
// clear the flag that lets other processes know that the expensive magnetometer fusion operation has been performed on that time step // clear the flag that lets other processes know that the expensive magnetometer fusion operation has been performed on that time step
// used for load levelling // used for load levelling
magFusePerformed = false; magFusePerformed = false;
@ -293,9 +290,7 @@ void NavEKF2_core::SelectMagFusion()
} }
// fuse the three magnetometer componenents using sequential fusion of each axis // fuse the three magnetometer componenents using sequential fusion of each axis
hal.util->perf_begin(_perf_test[0]);
FuseMagnetometer(); FuseMagnetometer();
hal.util->perf_end(_perf_test[0]);
// zero the test ratio output from the inactive simple magnetometer yaw fusion // zero the test ratio output from the inactive simple magnetometer yaw fusion
yawTestRatio = 0.0f; yawTestRatio = 0.0f;
@ -317,9 +312,6 @@ void NavEKF2_core::SelectMagFusion()
bodyMagFieldVar.y = P[20][20]; bodyMagFieldVar.y = P[20][20];
bodyMagFieldVar.z = P[21][21]; bodyMagFieldVar.z = P[21][21];
} }
// stop performance timer
hal.util->perf_end(_perf_FuseMagnetometer);
} }
/* /*
@ -1186,9 +1178,9 @@ bool NavEKF2_core::EKFGSF_resetMainFilterYaw()
EKFGSF_yaw_reset_count++; EKFGSF_yaw_reset_count++;
if (!use_compass()) { if (!use_compass()) {
gcs().send_text(MAV_SEVERITY_INFO, "EKF2 IMU%u yaw aligned using GPS",(unsigned)imu_index); GCS_SEND_TEXT(MAV_SEVERITY_INFO, "EKF2 IMU%u yaw aligned using GPS",(unsigned)imu_index);
} else { } else {
gcs().send_text(MAV_SEVERITY_WARNING, "EKF2 IMU%u emergency yaw reset",(unsigned)imu_index); GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "EKF2 IMU%u emergency yaw reset",(unsigned)imu_index);
} }
// Fail the magnetomer so it doesn't get used and pull the yaw away from the correct value // Fail the magnetomer so it doesn't get used and pull the yaw away from the correct value

55
libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp
View File

@ -1,14 +1,9 @@
#include <AP_HAL/AP_HAL.h> #include <AP_HAL/AP_HAL.h>
#include "AP_NavEKF2_core.h" #include "AP_NavEKF2_core.h"
#include <AP_AHRS/AP_AHRS.h>
#include <AP_Vehicle/AP_Vehicle.h> #include <AP_Vehicle/AP_Vehicle.h>
#include <GCS_MAVLink/GCS.h> #include <GCS_MAVLink/GCS.h>
#include <AP_RangeFinder/AP_RangeFinder.h> #include <AP_DAL/AP_DAL.h>
#include <AP_RangeFinder/AP_RangeFinder_Backend.h>
#include <AP_GPS/AP_GPS.h>
#include <AP_Baro/AP_Baro.h>
#include <AP_Compass/AP_Compass.h>
extern const AP_HAL::HAL& hal; extern const AP_HAL::HAL& hal;
@ -27,7 +22,7 @@ void NavEKF2_core::readRangeFinder(void)
// get theoretical correct range when the vehicle is on the ground // 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 // don't allow range to go below 5cm because this can cause problems with optical flow processing
const RangeFinder *_rng = AP::rangefinder(); const auto *_rng = AP::dal().rangefinder();
if (_rng == nullptr) { if (_rng == nullptr) {
return; return;
} }
@ -45,11 +40,11 @@ void NavEKF2_core::readRangeFinder(void)
// use data from two range finders if available // use data from two range finders if available
for (uint8_t sensorIndex = 0; sensorIndex <= 1; sensorIndex++) { for (uint8_t sensorIndex = 0; sensorIndex <= 1; sensorIndex++) {
AP_RangeFinder_Backend *sensor = _rng->get_backend(sensorIndex); auto *sensor = _rng->get_backend(sensorIndex);
if (sensor == nullptr) { if (sensor == nullptr) {
continue; continue;
} }
if ((sensor->orientation() == ROTATION_PITCH_270) && (sensor->status() == RangeFinder::Status::Good)) { if ((sensor->orientation() == ROTATION_PITCH_270) && (sensor->status() == AP_DAL_RangeFinder::Status::Good)) {
rngMeasIndex[sensorIndex] ++; rngMeasIndex[sensorIndex] ++;
if (rngMeasIndex[sensorIndex] > 2) { if (rngMeasIndex[sensorIndex] > 2) {
rngMeasIndex[sensorIndex] = 0; rngMeasIndex[sensorIndex] = 0;
@ -195,7 +190,7 @@ void NavEKF2_core::writeOptFlowMeas(const uint8_t rawFlowQuality, const Vector2f
// try changing to another compass // try changing to another compass
void NavEKF2_core::tryChangeCompass(void) void NavEKF2_core::tryChangeCompass(void)
{ {
const Compass &compass = AP::compass(); const auto &compass = AP::dal().compass();
const uint8_t maxCount = compass.get_count(); const uint8_t maxCount = compass.get_count();
// search through the list of magnetometers // search through the list of magnetometers
@ -208,7 +203,7 @@ void NavEKF2_core::tryChangeCompass(void)
// if the magnetometer is allowed to be used for yaw and has a different index, we start using it // if the magnetometer is allowed to be used for yaw and has a different index, we start using it
if (compass.healthy(tempIndex) && compass.use_for_yaw(tempIndex) && tempIndex != magSelectIndex) { if (compass.healthy(tempIndex) && compass.use_for_yaw(tempIndex) && tempIndex != magSelectIndex) {
magSelectIndex = tempIndex; magSelectIndex = tempIndex;
gcs().send_text(MAV_SEVERITY_INFO, "EKF2 IMU%u switching to compass %u",(unsigned)imu_index,magSelectIndex); GCS_SEND_TEXT(MAV_SEVERITY_INFO, "EKF2 IMU%u switching to compass %u",(unsigned)imu_index,magSelectIndex);
// reset the timeout flag and timer // reset the timeout flag and timer
magTimeout = false; magTimeout = false;
lastHealthyMagTime_ms = imuSampleTime_ms; lastHealthyMagTime_ms = imuSampleTime_ms;
@ -233,12 +228,12 @@ void NavEKF2_core::tryChangeCompass(void)
// check for new magnetometer data and update store measurements if available // check for new magnetometer data and update store measurements if available
void NavEKF2_core::readMagData() void NavEKF2_core::readMagData()
{ {
if (!_ahrs->get_compass()) { if (!AP::dal().get_compass()) {
allMagSensorsFailed = true; allMagSensorsFailed = true;
return; return;
} }
const Compass &compass = AP::compass(); const auto &compass = AP::dal().compass();
// If we are a vehicle with a sideslip constraint to aid yaw estimation and we have timed out on our last avialable // 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 // magnetometer, then declare the magnetometers as failed for this flight
@ -277,8 +272,6 @@ void NavEKF2_core::readMagData()
if (use_compass() && if (use_compass() &&
compass.healthy(magSelectIndex) && compass.healthy(magSelectIndex) &&
compass.last_update_usec(magSelectIndex) - lastMagUpdate_us > 70000) { compass.last_update_usec(magSelectIndex) - lastMagUpdate_us > 70000) {
frontend->logging.log_compass = true;
// detect changes to magnetometer offset parameters and reset states // detect changes to magnetometer offset parameters and reset states
Vector3f nowMagOffsets = compass.get_offsets(magSelectIndex); Vector3f nowMagOffsets = compass.get_offsets(magSelectIndex);
@ -335,7 +328,7 @@ void NavEKF2_core::readMagData()
*/ */
void NavEKF2_core::readIMUData() void NavEKF2_core::readIMUData()
{ {
const AP_InertialSensor &ins = AP::ins(); const auto &ins = AP::dal().ins();
// average IMU sampling rate // average IMU sampling rate
dtIMUavg = ins.get_loop_delta_t(); dtIMUavg = ins.get_loop_delta_t();
@ -479,7 +472,7 @@ void NavEKF2_core::readIMUData()
// read the delta velocity and corresponding time interval from the IMU // read the delta velocity and corresponding time interval from the IMU
// return false if data is not available // return false if data is not available
bool NavEKF2_core::readDeltaVelocity(uint8_t ins_index, Vector3f &dVel, float &dVel_dt) { bool NavEKF2_core::readDeltaVelocity(uint8_t ins_index, Vector3f &dVel, float &dVel_dt) {
const AP_InertialSensor &ins = AP::ins(); const auto &ins = AP::dal().ins();
if (ins_index < ins.get_accel_count()) { if (ins_index < ins.get_accel_count()) {
ins.get_delta_velocity(ins_index,dVel); ins.get_delta_velocity(ins_index,dVel);
@ -504,9 +497,9 @@ void NavEKF2_core::readGpsData()
// check for new GPS data // check for new GPS data
// do not accept data at a faster rate than 14Hz to avoid overflowing the FIFO buffer // do not accept data at a faster rate than 14Hz to avoid overflowing the FIFO buffer
const AP_GPS &gps = AP::gps(); const auto &gps = AP::dal().gps();
if (gps.last_message_time_ms() - lastTimeGpsReceived_ms > 70) { if (gps.last_message_time_ms(gps.primary_sensor()) - lastTimeGpsReceived_ms > 70) {
if (gps.status() >= AP_GPS::GPS_OK_FIX_3D) { if (gps.status() >= AP_DAL_GPS::GPS_OK_FIX_3D) {
// report GPS fix status // report GPS fix status
gpsCheckStatus.bad_fix = false; gpsCheckStatus.bad_fix = false;
@ -514,7 +507,7 @@ void NavEKF2_core::readGpsData()
secondLastGpsTime_ms = lastTimeGpsReceived_ms; secondLastGpsTime_ms = lastTimeGpsReceived_ms;
// get current fix time // get current fix time
lastTimeGpsReceived_ms = gps.last_message_time_ms(); lastTimeGpsReceived_ms = gps.last_message_time_ms(gps.primary_sensor());
// estimate when the GPS fix was valid, allowing for GPS processing and other delays // estimate when the GPS fix was valid, allowing for GPS processing and other delays
@ -614,7 +607,7 @@ void NavEKF2_core::readGpsData()
} }
if (gpsGoodToAlign && !have_table_earth_field) { if (gpsGoodToAlign && !have_table_earth_field) {
const Compass *compass = _ahrs->get_compass(); const auto *compass = AP::dal().get_compass();
if (compass && compass->have_scale_factor(magSelectIndex) && compass->auto_declination_enabled()) { if (compass && compass->have_scale_factor(magSelectIndex) && compass->auto_declination_enabled()) {
table_earth_field_ga = AP_Declination::get_earth_field_ga(gpsloc); table_earth_field_ga = AP_Declination::get_earth_field_ga(gpsloc);
table_declination = radians(AP_Declination::get_declination(gpsloc.lat*1.0e-7, table_declination = radians(AP_Declination::get_declination(gpsloc.lat*1.0e-7,
@ -643,7 +636,7 @@ void NavEKF2_core::readGpsData()
} else { } else {
// report GPS fix status // report GPS fix status
gpsCheckStatus.bad_fix = true; gpsCheckStatus.bad_fix = true;
hal.util->snprintf(prearm_fail_string, sizeof(prearm_fail_string), "Waiting for 3D fix"); AP::dal().snprintf(prearm_fail_string, sizeof(prearm_fail_string), "Waiting for 3D fix");
} }
} }
} }
@ -651,11 +644,10 @@ void NavEKF2_core::readGpsData()
// read the delta angle and corresponding time interval from the IMU // read the delta angle and corresponding time interval from the IMU
// return false if data is not available // return false if data is not available
bool NavEKF2_core::readDeltaAngle(uint8_t ins_index, Vector3f &dAng, float &dAng_dt) { bool NavEKF2_core::readDeltaAngle(uint8_t ins_index, Vector3f &dAng, float &dAng_dt) {
const AP_InertialSensor &ins = AP::ins(); const auto &ins = AP::dal().ins();
if (ins_index < ins.get_gyro_count()) { if (ins_index < ins.get_gyro_count()) {
ins.get_delta_angle(ins_index,dAng); ins.get_delta_angle(ins_index,dAng);
frontend->logging.log_imu = true;
dAng_dt = MAX(ins.get_delta_angle_dt(ins_index),1.0e-4f); dAng_dt = MAX(ins.get_delta_angle_dt(ins_index),1.0e-4f);
dAng_dt = MIN(dAng_dt,1.0e-1f); dAng_dt = MIN(dAng_dt,1.0e-1f);
return true; return true;
@ -673,9 +665,8 @@ void NavEKF2_core::readBaroData()
{ {
// check to see if baro measurement has changed so we know if a new measurement has arrived // 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 // do not accept data at a faster rate than 14Hz to avoid overflowing the FIFO buffer
const AP_Baro &baro = AP::baro(); const auto &baro = AP::dal().baro();
if (baro.get_last_update() - lastBaroReceived_ms > 70) { if (baro.get_last_update() - lastBaroReceived_ms > 70) {
frontend->logging.log_baro = true;
baroDataNew.hgt = baro.get_altitude(); baroDataNew.hgt = baro.get_altitude();
@ -762,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 // 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 // we take a new reading, convert from EAS to TAS and set the flag letting other functions
// know a new measurement is available // know a new measurement is available
const AP_Airspeed *aspeed = _ahrs->get_airspeed(); const auto *aspeed = AP::dal().airspeed();
if (aspeed && if (aspeed &&
aspeed->use() && aspeed->use() &&
aspeed->last_update_ms() != timeTasReceived_ms) { aspeed->last_update_ms() != timeTasReceived_ms) {
tasDataNew.tas = aspeed->get_airspeed() * AP::ahrs().get_EAS2TAS(); tasDataNew.tas = aspeed->get_airspeed() * AP::dal().get_EAS2TAS();
timeTasReceived_ms = aspeed->last_update_ms(); timeTasReceived_ms = aspeed->last_update_ms();
tasDataNew.time_ms = timeTasReceived_ms - frontend->tasDelay_ms; tasDataNew.time_ms = timeTasReceived_ms - frontend->tasDelay_ms;
@ -788,7 +779,7 @@ void NavEKF2_core::readAirSpdData()
void NavEKF2_core::readRngBcnData() void NavEKF2_core::readRngBcnData()
{ {
// get the location of the beacon data // get the location of the beacon data
const AP_Beacon *beacon = AP::beacon(); const auto *beacon = AP::dal().beacon();
// exit immediately if no beacon object // exit immediately if no beacon object
if (beacon == nullptr) { if (beacon == nullptr) {
@ -970,7 +961,7 @@ float NavEKF2_core::MagDeclination(void) const
if (!use_compass()) { if (!use_compass()) {
return 0; return 0;
} }
return _ahrs->get_compass()->get_declination(); return AP::dal().get_compass()->get_declination();
} }
/* /*
@ -980,7 +971,7 @@ float NavEKF2_core::MagDeclination(void) const
*/ */
void NavEKF2_core::learnInactiveBiases(void) void NavEKF2_core::learnInactiveBiases(void)
{ {
const AP_InertialSensor &ins = AP::ins(); const auto &ins = AP::dal().ins();
// learn gyro biases // learn gyro biases
for (uint8_t i=0; i<INS_MAX_INSTANCES; i++) { for (uint8_t i=0; i<INS_MAX_INSTANCES; i++) {

11
libraries/AP_NavEKF2/AP_NavEKF2_OptFlowFusion.cpp
View File

@ -26,8 +26,6 @@ void NavEKF2_core::SelectFlowFusion()
optFlowFusionDelayed = false; optFlowFusionDelayed = false;
} }
// start performance timer
hal.util->perf_begin(_perf_FuseOptFlow);
// Perform Data Checks // Perform Data Checks
// Check if the optical flow data is still valid // Check if the optical flow data is still valid
flowDataValid = ((imuSampleTime_ms - flowValidMeaTime_ms) < 1000); flowDataValid = ((imuSampleTime_ms - flowValidMeaTime_ms) < 1000);
@ -61,9 +59,6 @@ void NavEKF2_core::SelectFlowFusion()
// reset flag to indicate that no new flow data is available for fusion // reset flag to indicate that no new flow data is available for fusion
flowDataToFuse = false; flowDataToFuse = false;
} }
// stop the performance timer
hal.util->perf_end(_perf_FuseOptFlow);
} }
/* /*
@ -73,9 +68,6 @@ Equations generated using https://github.com/PX4/ecl/tree/master/EKF/matlab/scri
*/ */
void NavEKF2_core::EstimateTerrainOffset() void NavEKF2_core::EstimateTerrainOffset()
{ {
// start performance timer
hal.util->perf_begin(_perf_TerrainOffset);
// horizontal velocity squared // horizontal velocity squared
float velHorizSq = sq(stateStruct.velocity.x) + sq(stateStruct.velocity.y); float velHorizSq = sq(stateStruct.velocity.x) + sq(stateStruct.velocity.y);
@ -264,9 +256,6 @@ void NavEKF2_core::EstimateTerrainOffset()
} }
} }
} }
// stop the performance timer
hal.util->perf_end(_perf_TerrainOffset);
} }
/* /*

27
libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp
View File

@ -1,9 +1,8 @@
#include <AP_HAL/AP_HAL.h> #include <AP_HAL/AP_HAL.h>
#include "AP_NavEKF2_core.h" #include "AP_NavEKF2_core.h"
#include <AP_DAL/AP_DAL.h>
#include <AP_AHRS/AP_AHRS.h> #include <AP_AHRS/AP_AHRS.h>
#include <AP_GPS/AP_GPS.h>
#include <AP_RangeFinder/AP_RangeFinder.h>
extern const AP_HAL::HAL& hal; extern const AP_HAL::HAL& hal;
@ -100,7 +99,7 @@ bool NavEKF2_core::getHeightControlLimit(float &height) const
// only ask for limiting if we are doing optical flow only navigation // only ask for limiting if we are doing optical flow only navigation
if (frontend->_fusionModeGPS == 3 && (PV_AidingMode == AID_RELATIVE) && flowDataValid) { 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 // 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 RangeFinder *_rng = AP::rangefinder(); const auto *_rng = AP::dal().rangefinder();
if (_rng == nullptr) { if (_rng == nullptr) {
// we really, really shouldn't be here. // we really, really shouldn't be here.
return false; return false;
@ -121,7 +120,7 @@ bool NavEKF2_core::getHeightControlLimit(float &height) const
void NavEKF2_core::getEulerAngles(Vector3f &euler) const void NavEKF2_core::getEulerAngles(Vector3f &euler) const
{ {
outputDataNew.quat.to_euler(euler.x, euler.y, euler.z); outputDataNew.quat.to_euler(euler.x, euler.y, euler.z);
euler = euler - _ahrs->get_trim(); euler = euler - AP::dal().get_trim();
} }
// return body axis gyro bias estimates in rad/sec // return body axis gyro bias estimates in rad/sec
@ -156,7 +155,7 @@ void NavEKF2_core::getTiltError(float &ang) const
void NavEKF2_core::getRotationBodyToNED(Matrix3f &mat) const void NavEKF2_core::getRotationBodyToNED(Matrix3f &mat) const
{ {
outputDataNew.quat.rotation_matrix(mat); outputDataNew.quat.rotation_matrix(mat);
mat = mat * _ahrs->get_rotation_vehicle_body_to_autopilot_body(); mat = mat * AP::dal().get_rotation_vehicle_body_to_autopilot_body();
} }
// return the quaternions defining the rotation from NED to XYZ (body) axes // return the quaternions defining the rotation from NED to XYZ (body) axes
@ -252,9 +251,9 @@ bool NavEKF2_core::getPosNE(Vector2f &posNE) const
} else { } else {
// In constant position mode the EKF position states are at the origin, so we cannot use them as a position estimate // In constant position mode the EKF position states are at the origin, so we cannot use them as a position estimate
if(validOrigin) { if(validOrigin) {
if ((AP::gps().status() >= AP_GPS::GPS_OK_FIX_2D)) { if ((AP::dal().gps().status(AP::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 // If the origin has been set and we have GPS, then return the GPS position relative to the origin
const struct Location &gpsloc = AP::gps().location(); const struct Location &gpsloc = AP::dal().gps().location();
const Vector2f tempPosNE = EKF_origin.get_distance_NE(gpsloc); const Vector2f tempPosNE = EKF_origin.get_distance_NE(gpsloc);
posNE.x = tempPosNE.x; posNE.x = tempPosNE.x;
posNE.y = tempPosNE.y; posNE.y = tempPosNE.y;
@ -316,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 // 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 bool NavEKF2_core::getLLH(struct Location &loc) const
{ {
const AP_GPS &gps = AP::gps(); const auto &gps = AP::dal().gps();
Location origin; Location origin;
float posD; float posD;
@ -336,7 +335,7 @@ bool NavEKF2_core::getLLH(struct Location &loc) const
} else { } else {
// we could be in constant position mode because the vehicle has taken off without GPS, or has lost GPS // we could be in constant position mode because the vehicle has taken off without GPS, or has lost GPS
// in this mode we cannot use the EKF states to estimate position so will return the best available data // in this mode we cannot use the EKF states to estimate position so will return the best available data
if ((gps.status() >= AP_GPS::GPS_OK_FIX_2D)) { if ((gps.status() >= AP_DAL_GPS::GPS_OK_FIX_2D)) {
// we have a GPS position fix to return // we have a GPS position fix to return
const struct Location &gpsloc = gps.location(); const struct Location &gpsloc = gps.location();
loc.lat = gpsloc.lat; loc.lat = gpsloc.lat;
@ -358,7 +357,7 @@ bool NavEKF2_core::getLLH(struct Location &loc) const
} else { } else {
// If no origin has been defined for the EKF, then we cannot use its position states so return a raw // If no origin has been defined for the EKF, then we cannot use its position states so return a raw
// GPS reading if available and return false // GPS reading if available and return false
if ((gps.status() >= AP_GPS::GPS_OK_FIX_3D)) { if ((gps.status() >= AP_DAL_GPS::GPS_OK_FIX_3D)) {
const struct Location &gpsloc = gps.location(); const struct Location &gpsloc = gps.location();
loc = gpsloc; loc = gpsloc;
loc.relative_alt = 0; loc.relative_alt = 0;
@ -414,7 +413,7 @@ void NavEKF2_core::getMagXYZ(Vector3f &magXYZ) const
// return true if offsets are valid // return true if offsets are valid
bool NavEKF2_core::getMagOffsets(uint8_t mag_idx, Vector3f &magOffsets) const bool NavEKF2_core::getMagOffsets(uint8_t mag_idx, Vector3f &magOffsets) const
{ {
if (!_ahrs->get_compass()) { if (!AP::dal().get_compass()) {
return false; return false;
} }
@ -425,12 +424,12 @@ bool NavEKF2_core::getMagOffsets(uint8_t mag_idx, Vector3f &magOffsets) const
if ((mag_idx == magSelectIndex) && if ((mag_idx == magSelectIndex) &&
finalInflightMagInit && finalInflightMagInit &&
!inhibitMagStates && !inhibitMagStates &&
_ahrs->get_compass()->healthy(magSelectIndex) && AP::dal().get_compass()->healthy(magSelectIndex) &&
variancesConverged) { variancesConverged) {
magOffsets = _ahrs->get_compass()->get_offsets(magSelectIndex) - stateStruct.body_magfield*1000.0f; magOffsets = AP::dal().get_compass()->get_offsets(magSelectIndex) - stateStruct.body_magfield*1000.0f;
return true; return true;
} else { } else {
magOffsets = _ahrs->get_compass()->get_offsets(magSelectIndex); magOffsets = AP::dal().get_compass()->get_offsets(magSelectIndex);
return false; return false;
} }
} }

25
libraries/AP_NavEKF2/AP_NavEKF2_PosVelFusion.cpp
View File

@ -2,10 +2,7 @@
#include "AP_NavEKF2.h" #include "AP_NavEKF2.h"
#include "AP_NavEKF2_core.h" #include "AP_NavEKF2_core.h"
#include <AP_RangeFinder/AP_RangeFinder.h> #include <AP_DAL/AP_DAL.h>
#include <AP_RangeFinder/AP_RangeFinder_Backend.h>
#include <AP_GPS/AP_GPS.h>
#include <AP_Baro/AP_Baro.h>
#include <stdio.h> #include <stdio.h>
@ -290,7 +287,7 @@ bool NavEKF2_core::resetHeightDatum(void)
// record the old height estimate // record the old height estimate
float oldHgt = -stateStruct.position.z; float oldHgt = -stateStruct.position.z;
// reset the barometer so that it reads zero at the current height // reset the barometer so that it reads zero at the current height
AP::baro().update_calibration(); AP::dal().baro().update_calibration();
// reset the height state // reset the height state
stateStruct.position.z = 0.0f; 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 // adjust the height of the EKF origin so that the origin plus baro height before and after the reset is the same
@ -306,7 +303,7 @@ bool NavEKF2_core::resetHeightDatum(void)
// altitude. This ensures the reported AMSL alt from // altitude. This ensures the reported AMSL alt from
// getLLH() is equal to GPS altitude, while also ensuring // getLLH() is equal to GPS altitude, while also ensuring
// that the relative alt is zero // that the relative alt is zero
EKF_origin.alt = AP::gps().location().alt; EKF_origin.alt = AP::dal().gps().location().alt;
} }
ekfGpsRefHgt = (double)0.01 * (double)EKF_origin.alt; ekfGpsRefHgt = (double)0.01 * (double)EKF_origin.alt;
} }
@ -322,7 +319,7 @@ bool NavEKF2_core::resetHeightDatum(void)
*/ */
void NavEKF2_core::CorrectGPSForAntennaOffset(gps_elements &gps_data) const void NavEKF2_core::CorrectGPSForAntennaOffset(gps_elements &gps_data) const
{ {
const Vector3f &posOffsetBody = AP::gps().get_antenna_offset(gpsDataDelayed.sensor_idx) - accelPosOffset; const Vector3f &posOffsetBody = AP::dal().gps().get_antenna_offset(gpsDataDelayed.sensor_idx) - accelPosOffset;
if (posOffsetBody.is_zero()) { if (posOffsetBody.is_zero()) {
return; return;
} }
@ -348,7 +345,7 @@ void NavEKF2_core::CorrectGPSForAntennaOffset(gps_elements &gps_data) const
void NavEKF2_core::CorrectExtNavForSensorOffset(Vector3f &ext_position) const void NavEKF2_core::CorrectExtNavForSensorOffset(Vector3f &ext_position) const
{ {
#if HAL_VISUALODOM_ENABLED #if HAL_VISUALODOM_ENABLED
AP_VisualOdom *visual_odom = AP::visualodom(); const auto *visual_odom = AP::dal().visualodom();
if (visual_odom == nullptr) { if (visual_odom == nullptr) {
return; return;
} }
@ -367,7 +364,7 @@ void NavEKF2_core::CorrectExtNavForSensorOffset(Vector3f &ext_position) const
void NavEKF2_core::CorrectExtNavVelForSensorOffset(Vector3f &ext_velocity) const void NavEKF2_core::CorrectExtNavVelForSensorOffset(Vector3f &ext_velocity) const
{ {
#if HAL_VISUALODOM_ENABLED #if HAL_VISUALODOM_ENABLED
AP_VisualOdom *visual_odom = AP::visualodom(); const auto *visual_odom = AP::dal().visualodom();
if (visual_odom == nullptr) { if (visual_odom == nullptr) {
return; return;
} }
@ -568,9 +565,6 @@ void NavEKF2_core::SelectVelPosFusion()
// fuse selected position, velocity and height measurements // fuse selected position, velocity and height measurements
void NavEKF2_core::FuseVelPosNED() void NavEKF2_core::FuseVelPosNED()
{ {
// start performance timer
hal.util->perf_begin(_perf_FuseVelPosNED);
// health is set bad until test passed // health is set bad until test passed
velHealth = false; velHealth = false;
posHealth = false; posHealth = false;
@ -948,9 +942,6 @@ void NavEKF2_core::FuseVelPosNED()
} }
} }
} }
// stop performance timer
hal.util->perf_end(_perf_FuseVelPosNED);
} }
/******************************************************** /********************************************************
@ -968,9 +959,9 @@ void NavEKF2_core::selectHeightForFusion()
// correct range data for the body frame position offset relative to the IMU // 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 // the corrected reading is the reading that would have been taken if the sensor was
// co-located with the IMU // co-located with the IMU
const RangeFinder *_rng = AP::rangefinder(); const auto *_rng = AP::dal().rangefinder();
if (_rng && rangeDataToFuse) { if (_rng && rangeDataToFuse) {
const AP_RangeFinder_Backend *sensor = _rng->get_backend(rangeDataDelayed.sensor_idx); const auto *sensor = _rng->get_backend(rangeDataDelayed.sensor_idx);
if (sensor != nullptr) { if (sensor != nullptr) {
Vector3f posOffsetBody = sensor->get_pos_offset() - accelPosOffset; Vector3f posOffsetBody = sensor->get_pos_offset() - accelPosOffset;
if (!posOffsetBody.is_zero()) { if (!posOffsetBody.is_zero()) {

41
libraries/AP_NavEKF2/AP_NavEKF2_VehicleStatus.cpp
View File

@ -1,10 +1,8 @@
#include <AP_HAL/AP_HAL.h> #include <AP_HAL/AP_HAL.h>
#include "AP_NavEKF2_core.h" #include "AP_NavEKF2_core.h"
#include <AP_AHRS/AP_AHRS.h>
#include <AP_Vehicle/AP_Vehicle.h> #include <AP_Vehicle/AP_Vehicle.h>
#include <GCS_MAVLink/GCS.h> #include <GCS_MAVLink/GCS.h>
#include <AP_GPS/AP_GPS.h>
extern const AP_HAL::HAL& hal; extern const AP_HAL::HAL& hal;
@ -20,7 +18,7 @@ extern const AP_HAL::HAL& hal;
*/ */
void NavEKF2_core::calcGpsGoodToAlign(void) void NavEKF2_core::calcGpsGoodToAlign(void)
{ {
const AP_GPS &gps = AP::gps(); const auto &gps = AP::dal().gps();
if (inFlight && assume_zero_sideslip() && !use_compass()) { if (inFlight && assume_zero_sideslip() && !use_compass()) {
// this is a special case where a plane has launched without magnetometer // this is a special case where a plane has launched without magnetometer
@ -74,7 +72,7 @@ void NavEKF2_core::calcGpsGoodToAlign(void)
// Report check result as a text string and bitmask // Report check result as a text string and bitmask
if (gpsDriftFail) { if (gpsDriftFail) {
hal.util->snprintf(prearm_fail_string, AP::dal().snprintf(prearm_fail_string,
sizeof(prearm_fail_string), sizeof(prearm_fail_string),
"GPS drift %.1fm (needs %.1f)", (double)gpsDriftNE, (double)(3.0f*checkScaler)); "GPS drift %.1fm (needs %.1f)", (double)gpsDriftNE, (double)(3.0f*checkScaler));
gpsCheckStatus.bad_horiz_drift = true; gpsCheckStatus.bad_horiz_drift = true;
@ -95,9 +93,9 @@ void NavEKF2_core::calcGpsGoodToAlign(void)
// if we have a 3D fix with no vertical velocity and // if we have a 3D fix with no vertical velocity and
// EK2_GPS_TYPE=0 then change it to 1. It means the GPS is not // EK2_GPS_TYPE=0 then change it to 1. It means the GPS is not
// capable of giving a vertical velocity // capable of giving a vertical velocity
if (gps.status() >= AP_GPS::GPS_OK_FIX_3D) { if (gps.status() >= AP_DAL_GPS::GPS_OK_FIX_3D) {
frontend->_fusionModeGPS.set(1); frontend->_fusionModeGPS.set(1);
gcs().send_text(MAV_SEVERITY_WARNING, "EK2: Changed EK2_GPS_TYPE to 1"); GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "EK2: Changed EK2_GPS_TYPE to 1");
} }
} else { } else {
gpsVertVelFail = false; gpsVertVelFail = false;
@ -105,7 +103,7 @@ void NavEKF2_core::calcGpsGoodToAlign(void)
// Report check result as a text string and bitmask // Report check result as a text string and bitmask
if (gpsVertVelFail) { if (gpsVertVelFail) {
hal.util->snprintf(prearm_fail_string, AP::dal().snprintf(prearm_fail_string,
sizeof(prearm_fail_string), sizeof(prearm_fail_string),
"GPS vertical speed %.2fm/s (needs %.2f)", (double)fabsf(gpsVertVelFilt), (double)(0.3f*checkScaler)); "GPS vertical speed %.2fm/s (needs %.2f)", (double)fabsf(gpsVertVelFilt), (double)(0.3f*checkScaler));
gpsCheckStatus.bad_vert_vel = true; gpsCheckStatus.bad_vert_vel = true;
@ -126,7 +124,7 @@ void NavEKF2_core::calcGpsGoodToAlign(void)
// Report check result as a text string and bitmask // Report check result as a text string and bitmask
if (gpsHorizVelFail) { if (gpsHorizVelFail) {
hal.util->snprintf(prearm_fail_string, AP::dal().snprintf(prearm_fail_string,
sizeof(prearm_fail_string), sizeof(prearm_fail_string),
"GPS horizontal speed %.2fm/s (needs %.2f)", (double)gpsDriftNE, (double)(0.3f*checkScaler)); "GPS horizontal speed %.2fm/s (needs %.2f)", (double)gpsDriftNE, (double)(0.3f*checkScaler));
gpsCheckStatus.bad_horiz_vel = true; gpsCheckStatus.bad_horiz_vel = true;
@ -145,7 +143,7 @@ void NavEKF2_core::calcGpsGoodToAlign(void)
// Report check result as a text string and bitmask // Report check result as a text string and bitmask
if (hAccFail) { if (hAccFail) {
hal.util->snprintf(prearm_fail_string, AP::dal().snprintf(prearm_fail_string,
sizeof(prearm_fail_string), sizeof(prearm_fail_string),
"GPS horiz error %.1fm (needs %.1f)", (double)hAcc, (double)(5.0f*checkScaler)); "GPS horiz error %.1fm (needs %.1f)", (double)hAcc, (double)(5.0f*checkScaler));
gpsCheckStatus.bad_hAcc = true; gpsCheckStatus.bad_hAcc = true;
@ -161,7 +159,7 @@ void NavEKF2_core::calcGpsGoodToAlign(void)
} }
// Report check result as a text string and bitmask // Report check result as a text string and bitmask
if (vAccFail) { if (vAccFail) {
hal.util->snprintf(prearm_fail_string, AP::dal().snprintf(prearm_fail_string,
sizeof(prearm_fail_string), sizeof(prearm_fail_string),
"GPS vert error %.1fm (needs < %.1f)", (double)vAcc, (double)(7.5f * checkScaler)); "GPS vert error %.1fm (needs < %.1f)", (double)vAcc, (double)(7.5f * checkScaler));
gpsCheckStatus.bad_vAcc = true; gpsCheckStatus.bad_vAcc = true;
@ -174,7 +172,7 @@ void NavEKF2_core::calcGpsGoodToAlign(void)
// Report check result as a text string and bitmask // Report check result as a text string and bitmask
if (gpsSpdAccFail) { if (gpsSpdAccFail) {
hal.util->snprintf(prearm_fail_string, AP::dal().snprintf(prearm_fail_string,
sizeof(prearm_fail_string), sizeof(prearm_fail_string),
"GPS speed error %.1f (needs < %.1f)", (double)gpsSpdAccuracy, (double)(1.0f*checkScaler)); "GPS speed error %.1f (needs < %.1f)", (double)gpsSpdAccuracy, (double)(1.0f*checkScaler));
gpsCheckStatus.bad_sAcc = true; gpsCheckStatus.bad_sAcc = true;
@ -187,7 +185,7 @@ void NavEKF2_core::calcGpsGoodToAlign(void)
// Report check result as a text string and bitmask // Report check result as a text string and bitmask
if (hdopFail) { if (hdopFail) {
hal.util->snprintf(prearm_fail_string, sizeof(prearm_fail_string), AP::dal().snprintf(prearm_fail_string, sizeof(prearm_fail_string),
"GPS HDOP %.1f (needs 2.5)", (double)(0.01f * gps.get_hdop())); "GPS HDOP %.1f (needs 2.5)", (double)(0.01f * gps.get_hdop()));
gpsCheckStatus.bad_hdop = true; gpsCheckStatus.bad_hdop = true;
} else { } else {
@ -199,7 +197,7 @@ void NavEKF2_core::calcGpsGoodToAlign(void)
// Report check result as a text string and bitmask // Report check result as a text string and bitmask
if (numSatsFail) { if (numSatsFail) {
hal.util->snprintf(prearm_fail_string, sizeof(prearm_fail_string), AP::dal().snprintf(prearm_fail_string, sizeof(prearm_fail_string),
"GPS numsats %u (needs 6)", gps.num_sats()); "GPS numsats %u (needs 6)", gps.num_sats());
gpsCheckStatus.bad_sats = true; gpsCheckStatus.bad_sats = true;
} else { } else {
@ -217,7 +215,7 @@ void NavEKF2_core::calcGpsGoodToAlign(void)
// Report check result as a text string and bitmask // Report check result as a text string and bitmask
if (yawFail) { if (yawFail) {
hal.util->snprintf(prearm_fail_string, AP::dal().snprintf(prearm_fail_string,
sizeof(prearm_fail_string), sizeof(prearm_fail_string),
"Mag yaw error x=%.1f y=%.1f", "Mag yaw error x=%.1f y=%.1f",
(double)magTestRatio.x, (double)magTestRatio.x,
@ -229,7 +227,7 @@ void NavEKF2_core::calcGpsGoodToAlign(void)
// assume failed first time through and notify user checks have started // assume failed first time through and notify user checks have started
if (lastGpsVelFail_ms == 0) { if (lastGpsVelFail_ms == 0) {
hal.util->snprintf(prearm_fail_string, sizeof(prearm_fail_string), "EKF starting GPS checks"); AP::dal().snprintf(prearm_fail_string, sizeof(prearm_fail_string), "EKF starting GPS checks");
lastGpsVelFail_ms = imuSampleTime_ms; lastGpsVelFail_ms = imuSampleTime_ms;
} }
@ -266,7 +264,7 @@ void NavEKF2_core::calcGpsGoodForFlight(void)
// get the receivers reported speed accuracy // get the receivers reported speed accuracy
float gpsSpdAccRaw; float gpsSpdAccRaw;
if (!AP::gps().speed_accuracy(gpsSpdAccRaw)) { if (!AP::dal().gps().speed_accuracy(gpsSpdAccRaw)) {
gpsSpdAccRaw = 0.0f; gpsSpdAccRaw = 0.0f;
} }
@ -326,9 +324,9 @@ void NavEKF2_core::detectFlight()
bool largeHgtChange = false; bool largeHgtChange = false;
// trigger at 8 m/s airspeed // trigger at 8 m/s airspeed
if (_ahrs->airspeed_sensor_enabled()) { if (AP::dal().airspeed_sensor_enabled()) {
const AP_Airspeed *airspeed = _ahrs->get_airspeed(); const auto *airspeed = AP::dal().airspeed();
if (airspeed->get_airspeed() * AP::ahrs().get_EAS2TAS() > 10.0f) { if (airspeed->get_airspeed() * AP::dal().get_EAS2TAS() > 10.0f) {
highAirSpd = true; highAirSpd = true;
} }
} }
@ -385,8 +383,7 @@ void NavEKF2_core::detectFlight()
// If more than 5 seconds since likely_flying was set // If more than 5 seconds since likely_flying was set
// true, then set inFlight true // true, then set inFlight true
const AP_Vehicle *vehicle = AP::vehicle(); if (AP::dal().get_time_flying_ms() > 5000) {
if (vehicle->get_time_flying_ms() > 5000) {
inFlight = true; inFlight = true;
} }
} }
@ -481,7 +478,7 @@ void NavEKF2_core::detectOptFlowTakeoff(void)
{ {
if (!onGround && !takeOffDetected && (imuSampleTime_ms - timeAtArming_ms) > 1000) { 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 // we are no longer confidently on the ground so check the range finder and gyro for signs of takeoff
const AP_InertialSensor &ins = AP::ins(); const auto &ins = AP::dal().ins();
Vector3f angRateVec; Vector3f angRateVec;
Vector3f gyroBias; Vector3f gyroBias;
getGyroBias(gyroBias); getGyroBias(gyroBias);

58
libraries/AP_NavEKF2/AP_NavEKF2_core.cpp
View File

@ -2,9 +2,7 @@
#include "AP_NavEKF2.h" #include "AP_NavEKF2.h"
#include "AP_NavEKF2_core.h" #include "AP_NavEKF2_core.h"
#include <AP_AHRS/AP_AHRS.h>
#include <GCS_MAVLink/GCS.h> #include <GCS_MAVLink/GCS.h>
#include <AP_GPS/AP_GPS.h>
extern const AP_HAL::HAL& hal; extern const AP_HAL::HAL& hal;
@ -32,26 +30,8 @@ extern const AP_HAL::HAL& hal;
// constructor // constructor
NavEKF2_core::NavEKF2_core(NavEKF2 *_frontend) : NavEKF2_core::NavEKF2_core(NavEKF2 *_frontend) :
_perf_UpdateFilter(hal.util->perf_alloc(AP_HAL::Util::PC_ELAPSED, "EK2_UpdateFilter")),
_perf_CovariancePrediction(hal.util->perf_alloc(AP_HAL::Util::PC_ELAPSED, "EK2_CovariancePrediction")),
_perf_FuseVelPosNED(hal.util->perf_alloc(AP_HAL::Util::PC_ELAPSED, "EK2_FuseVelPosNED")),
_perf_FuseMagnetometer(hal.util->perf_alloc(AP_HAL::Util::PC_ELAPSED, "EK2_FuseMagnetometer")),
_perf_FuseAirspeed(hal.util->perf_alloc(AP_HAL::Util::PC_ELAPSED, "EK2_FuseAirspeed")),
_perf_FuseSideslip(hal.util->perf_alloc(AP_HAL::Util::PC_ELAPSED, "EK2_FuseSideslip")),
_perf_TerrainOffset(hal.util->perf_alloc(AP_HAL::Util::PC_ELAPSED, "EK2_TerrainOffset")),
_perf_FuseOptFlow(hal.util->perf_alloc(AP_HAL::Util::PC_ELAPSED, "EK2_FuseOptFlow")),
frontend(_frontend) frontend(_frontend)
{ {
_perf_test[0] = hal.util->perf_alloc(AP_HAL::Util::PC_ELAPSED, "EK2_Test0");
_perf_test[1] = hal.util->perf_alloc(AP_HAL::Util::PC_ELAPSED, "EK2_Test1");
_perf_test[2] = hal.util->perf_alloc(AP_HAL::Util::PC_ELAPSED, "EK2_Test2");
_perf_test[3] = hal.util->perf_alloc(AP_HAL::Util::PC_ELAPSED, "EK2_Test3");
_perf_test[4] = hal.util->perf_alloc(AP_HAL::Util::PC_ELAPSED, "EK2_Test4");
_perf_test[5] = hal.util->perf_alloc(AP_HAL::Util::PC_ELAPSED, "EK2_Test5");
_perf_test[6] = hal.util->perf_alloc(AP_HAL::Util::PC_ELAPSED, "EK2_Test6");
_perf_test[7] = hal.util->perf_alloc(AP_HAL::Util::PC_ELAPSED, "EK2_Test7");
_perf_test[8] = hal.util->perf_alloc(AP_HAL::Util::PC_ELAPSED, "EK2_Test8");
_perf_test[9] = hal.util->perf_alloc(AP_HAL::Util::PC_ELAPSED, "EK2_Test9");
} }
// setup this core backend // setup this core backend
@ -61,7 +41,6 @@ bool NavEKF2_core::setup_core(uint8_t _imu_index, uint8_t _core_index)
gyro_index_active = _imu_index; gyro_index_active = _imu_index;
accel_index_active = _imu_index; accel_index_active = _imu_index;
core_index = _core_index; core_index = _core_index;
_ahrs = frontend->_ahrs;
/* /*
the imu_buffer_length needs to cope with a 260ms delay at a the imu_buffer_length needs to cope with a 260ms delay at a
@ -69,7 +48,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 than 100Hz is downsampled. For 50Hz main loop rate we need a
shorter buffer. shorter buffer.
*/ */
if (AP::ins().get_loop_rate_hz() < 100) { if (AP::dal().ins().get_loop_rate_hz() < 100) {
imu_buffer_length = 13; imu_buffer_length = 13;
} else { } else {
// maximum 260 msec delay at 100 Hz fusion rate // maximum 260 msec delay at 100 Hz fusion rate
@ -113,15 +92,15 @@ bool NavEKF2_core::setup_core(uint8_t _imu_index, uint8_t _core_index)
if ((yawEstimator == nullptr) && (frontend->_gsfRunMask & (1U<<core_index))) { if ((yawEstimator == nullptr) && (frontend->_gsfRunMask & (1U<<core_index))) {
// check if there is enough memory to create the EKF-GSF object // check if there is enough memory to create the EKF-GSF object
if (hal.util->available_memory() < sizeof(EKFGSF_yaw) + 1024) { if (AP::dal().available_memory() < sizeof(EKFGSF_yaw) + 1024) {
gcs().send_text(MAV_SEVERITY_CRITICAL, "EKF2 IMU%u GSF: not enough memory",(unsigned)imu_index); GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "EKF2 IMU%u GSF: not enough memory",(unsigned)imu_index);
return false; return false;
} }
// try to instantiate // try to instantiate
yawEstimator = new EKFGSF_yaw(); yawEstimator = new EKFGSF_yaw();
if (yawEstimator == nullptr) { if (yawEstimator == nullptr) {
gcs().send_text(MAV_SEVERITY_CRITICAL, "EKF2 IMU%uGSF: allocation failed",(unsigned)imu_index); GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "EKF2 IMU%uGSF: allocation failed",(unsigned)imu_index);
return false; return false;
} }
} }
@ -138,7 +117,7 @@ bool NavEKF2_core::setup_core(uint8_t _imu_index, uint8_t _core_index)
void NavEKF2_core::InitialiseVariables() void NavEKF2_core::InitialiseVariables()
{ {
// calculate the nominal filter update rate // calculate the nominal filter update rate
const AP_InertialSensor &ins = AP::ins(); const auto &ins = AP::dal().ins();
localFilterTimeStep_ms = (uint8_t)(1000*ins.get_loop_delta_t()); localFilterTimeStep_ms = (uint8_t)(1000*ins.get_loop_delta_t());
localFilterTimeStep_ms = MAX(localFilterTimeStep_ms,10); localFilterTimeStep_ms = MAX(localFilterTimeStep_ms,10);
@ -348,7 +327,7 @@ void NavEKF2_core::InitialiseVariables()
have_table_earth_field = false; have_table_earth_field = false;
// initialise pre-arm message // initialise pre-arm message
hal.util->snprintf(prearm_fail_string, sizeof(prearm_fail_string), "EKF2 still initialising"); AP::dal().snprintf(prearm_fail_string, sizeof(prearm_fail_string), "EKF2 still initialising");
InitialiseVariablesMag(); InitialiseVariablesMag();
@ -394,8 +373,8 @@ void NavEKF2_core::InitialiseVariablesMag()
bool NavEKF2_core::InitialiseFilterBootstrap(void) bool NavEKF2_core::InitialiseFilterBootstrap(void)
{ {
// If we are a plane and don't have GPS lock then don't initialise // If we are a plane and don't have GPS lock then don't initialise
if (assume_zero_sideslip() && AP::gps().status() < AP_GPS::GPS_OK_FIX_3D) { if (assume_zero_sideslip() && AP::dal().gps().status(AP::dal().gps().primary_sensor()) < AP_DAL_GPS::GPS_OK_FIX_3D) {
hal.util->snprintf(prearm_fail_string, AP::dal().snprintf(prearm_fail_string,
sizeof(prearm_fail_string), sizeof(prearm_fail_string),
"EKF2 init failure: No GPS lock"); "EKF2 init failure: No GPS lock");
statesInitialised = false; statesInitialised = false;
@ -416,7 +395,7 @@ bool NavEKF2_core::InitialiseFilterBootstrap(void)
// set re-used variables to zero // set re-used variables to zero
InitialiseVariables(); InitialiseVariables();
const AP_InertialSensor &ins = AP::ins(); const auto &ins = AP::dal().ins();
// Initialise IMU data // Initialise IMU data
dtIMUavg = ins.get_loop_delta_t(); dtIMUavg = ins.get_loop_delta_t();
@ -473,7 +452,7 @@ bool NavEKF2_core::InitialiseFilterBootstrap(void)
ResetHeight(); ResetHeight();
// define Earth rotation vector in the NED navigation frame // define Earth rotation vector in the NED navigation frame
calcEarthRateNED(earthRateNED, _ahrs->get_home().lat); calcEarthRateNED(earthRateNED, AP::dal().get_home().lat);
// initialise the covariance matrix // initialise the covariance matrix
CovarianceInit(); CovarianceInit();
@ -559,9 +538,8 @@ void NavEKF2_core::UpdateFilter(bool predict)
#if ENABLE_EKF_TIMING #if ENABLE_EKF_TIMING
void *istate = hal.scheduler->disable_interrupts_save(); void *istate = hal.scheduler->disable_interrupts_save();
static uint32_t timing_start_us; static uint32_t timing_start_us;
timing_start_us = AP_HAL::micros(); timing_start_us = AP::dal().micros();
#endif #endif
hal.util->perf_begin(_perf_UpdateFilter);
fill_scratch_variables(); fill_scratch_variables();
@ -620,11 +598,10 @@ void NavEKF2_core::UpdateFilter(bool predict)
calcOutputStates(); calcOutputStates();
// stop the timer used for load measurement // stop the timer used for load measurement
hal.util->perf_end(_perf_UpdateFilter);
#if ENABLE_EKF_TIMING #if ENABLE_EKF_TIMING
static uint32_t total_us; static uint32_t total_us;
static uint32_t timing_counter; static uint32_t timing_counter;
total_us += AP_HAL::micros() - timing_start_us; total_us += AP::dal().micros() - timing_start_us;
if (timing_counter++ == 4000) { if (timing_counter++ == 4000) {
hal.console->printf("ekf2 avg %.2f us\n", total_us / float(timing_counter)); hal.console->printf("ekf2 avg %.2f us\n", total_us / float(timing_counter));
total_us = 0; total_us = 0;
@ -641,14 +618,14 @@ void NavEKF2_core::UpdateFilter(bool predict)
it try again. it try again.
*/ */
if (filterStatus.value != 0) { if (filterStatus.value != 0) {
last_filter_ok_ms = AP_HAL::millis(); last_filter_ok_ms = AP::dal().millis();
} }
if (filterStatus.value == 0 && if (filterStatus.value == 0 &&
last_filter_ok_ms != 0 && last_filter_ok_ms != 0 &&
AP_HAL::millis() - last_filter_ok_ms > 5000 && AP::dal().millis() - last_filter_ok_ms > 5000 &&
!hal.util->get_soft_armed()) { !AP::dal().get_armed()) {
// we've been unhealthy for 5 seconds after being healthy, reset the filter // 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); GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "EKF2 IMU%u forced reset",(unsigned)imu_index);
last_filter_ok_ms = 0; last_filter_ok_ms = 0;
statesInitialised = false; statesInitialised = false;
InitialiseFilterBootstrap(); InitialiseFilterBootstrap();
@ -908,7 +885,6 @@ void NavEKF2_core::calcOutputStates()
*/ */
void NavEKF2_core::CovariancePrediction() void NavEKF2_core::CovariancePrediction()
{ {
hal.util->perf_begin(_perf_CovariancePrediction);
float windVelSigma; // wind velocity 1-sigma process noise - m/s float windVelSigma; // wind velocity 1-sigma process noise - m/s
float dAngBiasSigma;// delta angle bias 1-sigma process noise - rad/s float dAngBiasSigma;// delta angle bias 1-sigma process noise - rad/s
float dVelBiasSigma;// delta velocity bias 1-sigma process noise - m/s float dVelBiasSigma;// delta velocity bias 1-sigma process noise - m/s
@ -1432,8 +1408,6 @@ void NavEKF2_core::CovariancePrediction()
// constrain diagonals to prevent ill-conditioning // constrain diagonals to prevent ill-conditioning
ConstrainVariances(); ConstrainVariances();
hal.util->perf_end(_perf_CovariancePrediction);
} }
// zero specified range of rows in the state covariance matrix // zero specified range of rows in the state covariance matrix

18
libraries/AP_NavEKF2/AP_NavEKF2_core.h
View File

@ -24,16 +24,13 @@
#pragma GCC optimize("O2") #pragma GCC optimize("O2")
#endif #endif
#define EK2_DISABLE_INTERRUPTS 0
#include <AP_Common/Location.h> #include <AP_Common/Location.h>
#include <AP_Math/AP_Math.h> #include <AP_Math/AP_Math.h>
#include <AP_Math/vectorN.h> #include <AP_Math/vectorN.h>
#include <AP_NavEKF/AP_NavEKF_core_common.h> #include <AP_NavEKF/AP_NavEKF_core_common.h>
#include <AP_NavEKF2/AP_NavEKF2_Buffer.h> #include <AP_NavEKF2/AP_NavEKF2_Buffer.h>
#include <AP_InertialSensor/AP_InertialSensor.h>
#include <GCS_MAVLink/GCS_MAVLink.h> #include <GCS_MAVLink/GCS_MAVLink.h>
#include <AP_DAL/AP_DAL.h>
#include "AP_NavEKF/EKFGSF_yaw.h" #include "AP_NavEKF/EKFGSF_yaw.h"
@ -427,8 +424,6 @@ private:
typedef uint32_t Vector_u32_50[50]; typedef uint32_t Vector_u32_50[50];
#endif #endif
const AP_AHRS *_ahrs;
// the states are available in two forms, either as a Vector31, or // the states are available in two forms, either as a Vector31, or
// broken down as individual elements. Both are equivalent (same // broken down as individual elements. Both are equivalent (same
// memory) // memory)
@ -1223,17 +1218,6 @@ private:
// string representing last reason for prearm failure // string representing last reason for prearm failure
char prearm_fail_string[41]; char prearm_fail_string[41];
// performance counters
AP_HAL::Util::perf_counter_t _perf_UpdateFilter;
AP_HAL::Util::perf_counter_t _perf_CovariancePrediction;
AP_HAL::Util::perf_counter_t _perf_FuseVelPosNED;
AP_HAL::Util::perf_counter_t _perf_FuseMagnetometer;
AP_HAL::Util::perf_counter_t _perf_FuseAirspeed;
AP_HAL::Util::perf_counter_t _perf_FuseSideslip;
AP_HAL::Util::perf_counter_t _perf_TerrainOffset;
AP_HAL::Util::perf_counter_t _perf_FuseOptFlow;
AP_HAL::Util::perf_counter_t _perf_test[10];
// earth field from WMM tables // earth field from WMM tables
bool have_table_earth_field; // true when we have initialised table_earth_field_ga bool have_table_earth_field; // true when we have initialised table_earth_field_ga
Vector3f table_earth_field_ga; // earth field from WMM tables Vector3f table_earth_field_ga; // earth field from WMM tables

1
libraries/AP_NavEKF2/AP_NavEKF_GyroBias.cpp
View File

@ -3,7 +3,6 @@
#include "AP_NavEKF2.h" #include "AP_NavEKF2.h"
#include "AP_NavEKF2_core.h" #include "AP_NavEKF2_core.h"
#include <AP_AHRS/AP_AHRS.h>
#include <AP_Vehicle/AP_Vehicle.h> #include <AP_Vehicle/AP_Vehicle.h>
#include <stdio.h> #include <stdio.h>

40
libraries/AP_NavEKF2/LogStructure.h Normal file
View File

@ -0,0 +1,40 @@
#pragma once
#include <AP_Logger/LogStructure.h>
#define LOG_IDS_FROM_NAVEKF2 \
LOG_NKT_MSG
struct PACKED log_NKT {
LOG_PACKET_HEADER;
uint64_t time_us;
uint8_t core;
uint32_t timing_count;
float dtIMUavg_min;
float dtIMUavg_max;
float dtEKFavg_min;
float dtEKFavg_max;
float delAngDT_min;
float delAngDT_max;
float delVelDT_min;
float delVelDT_max;
};
// @LoggerMessage: NKT
// @Description: EKF2 timing information
// @Field: TimeUS: Time since system startup
// @Field: C: EKF core this message instance applies to
// @Field: Cnt: count of samples used to create this message
// @Field: IMUMin: smallest IMU sample interval
// @Field: IMUMax: largest IMU sample interval
// @Field: EKFMin: low-passed achieved average time step rate for the EKF (minimum)
// @Field: EKFMax: low-passed achieved average time step rate for the EKF (maximum)
// @Field: AngMin: accumulated measurement time interval for the delta angle (minimum)
// @Field: AngMax: accumulated measurement time interval for the delta angle (maximum)
// @Field: VMin: accumulated measurement time interval for the delta velocity (minimum)
// @Field: VMax: accumulated measurement time interval for the delta velocity (maximum)
#define LOG_STRUCTURE_FROM_NAVEKF2 \
{ LOG_NKT_MSG, sizeof(log_NKT), \
"NKT", "QBIffffffff", "TimeUS,C,Cnt,IMUMin,IMUMax,EKFMin,EKFMax,AngMin,AngMax,VMin,VMax", "s#sssssssss", "F-000000000"},