mirror of https://github.com/ArduPilot/ardupilot
AP_NavEKF2: convert to use AP_DAL for new replay structure
Co-authored-by: Peter Barker <pbarker@barker.dropbear.id.au>
This commit is contained in:
parent
97a2c5a576
commit
6be3d19e82
|
@ -426,13 +426,8 @@ const AP_Param::GroupInfo NavEKF2::var_info[] = {
|
|||
// @Units: m
|
||||
AP_GROUPINFO("NOAID_M_NSE", 35, NavEKF2, _noaidHorizNoise, 10.0f),
|
||||
|
||||
// @Param: LOG_MASK
|
||||
// @DisplayName: EKF sensor logging IMU mask
|
||||
// @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),
|
||||
// 36 was LOG_MASK, used for specifying which IMUs/cores to log
|
||||
// replay data for
|
||||
|
||||
// control of magentic yaw angle fusion
|
||||
|
||||
|
@ -614,38 +609,15 @@ const AP_Param::GroupInfo NavEKF2::var_info[] = {
|
|||
NavEKF2::NavEKF2()
|
||||
{
|
||||
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
|
||||
bool NavEKF2::InitialiseFilter(void)
|
||||
{
|
||||
AP::dal().start_frame(AP_DAL::FrameType::InitialiseFilterEKF2);
|
||||
|
||||
// Return immediately if there is insufficient memory
|
||||
if (core_malloc_failed) {
|
||||
return false;
|
||||
|
@ -653,14 +625,14 @@ bool NavEKF2::InitialiseFilter(void)
|
|||
|
||||
initFailure = InitFailures::UNKNOWN;
|
||||
if (_enable == 0) {
|
||||
if (_ahrs->get_ekf_type() == 2) {
|
||||
if (AP::dal().get_ekf_type() == 2) {
|
||||
initFailure = InitFailures::NO_ENABLE;
|
||||
}
|
||||
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
|
||||
_frameTimeUsec = 1e6 / ins.get_loop_rate_hz();
|
||||
|
@ -668,12 +640,6 @@ bool NavEKF2::InitialiseFilter(void)
|
|||
// expected number of IMU frames per prediction
|
||||
_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) {
|
||||
|
||||
// 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
|
||||
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;
|
||||
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;
|
||||
}
|
||||
|
||||
// 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) {
|
||||
initFailure = InitFailures::NO_MEM;
|
||||
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;
|
||||
}
|
||||
|
||||
|
@ -724,7 +690,7 @@ bool NavEKF2::InitialiseFilter(void)
|
|||
hal.util->free_type(core, sizeof(NavEKF2_core)*num_cores, AP_HAL::Util::MEM_FAST);
|
||||
core = nullptr;
|
||||
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;
|
||||
}
|
||||
num_cores++;
|
||||
|
@ -750,7 +716,6 @@ bool NavEKF2::InitialiseFilter(void)
|
|||
memset((void *)&pos_reset_data, 0, sizeof(pos_reset_data));
|
||||
memset(&pos_down_reset_data, 0, sizeof(pos_down_reset_data));
|
||||
|
||||
check_log_write();
|
||||
return ret;
|
||||
}
|
||||
|
||||
|
@ -783,13 +748,13 @@ 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
|
||||
void NavEKF2::UpdateFilter(void)
|
||||
{
|
||||
AP::dal().start_frame(AP_DAL::FrameType::UpdateFilterEKF2);
|
||||
|
||||
if (!core) {
|
||||
return;
|
||||
}
|
||||
|
||||
imuSampleTime_us = AP_HAL::micros64();
|
||||
|
||||
const AP_InertialSensor &ins = AP::ins();
|
||||
imuSampleTime_us = AP::dal().micros64();
|
||||
|
||||
bool statePredictEnabled[num_cores];
|
||||
for (uint8_t i=0; i<num_cores; i++) {
|
||||
|
@ -798,7 +763,7 @@ void NavEKF2::UpdateFilter(void)
|
|||
// loop then suppress the prediction step. This allows
|
||||
// multiple EKF instances to cooperate on scheduling
|
||||
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;
|
||||
} else {
|
||||
statePredictEnabled[i] = true;
|
||||
|
@ -842,11 +807,11 @@ void NavEKF2::UpdateFilter(void)
|
|||
updateLaneSwitchPosResetData(newPrimaryIndex, primary);
|
||||
updateLaneSwitchPosDownResetData(newPrimaryIndex, primary);
|
||||
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
|
||||
// avoids us ending with with a lottery for which IMU is used
|
||||
// in each flight. Otherwise the alignment of the timing of
|
||||
|
@ -857,8 +822,6 @@ void NavEKF2::UpdateFilter(void)
|
|||
// performance
|
||||
primary = 0;
|
||||
}
|
||||
|
||||
check_log_write();
|
||||
}
|
||||
|
||||
/*
|
||||
|
@ -869,7 +832,8 @@ void NavEKF2::UpdateFilter(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) {
|
||||
// don't switch twice in 5 seconds
|
||||
return;
|
||||
|
@ -893,7 +857,7 @@ void NavEKF2::checkLaneSwitch(void)
|
|||
updateLaneSwitchPosDownResetData(newPrimaryIndex, primary);
|
||||
primary = newPrimaryIndex;
|
||||
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
|
||||
{
|
||||
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;
|
||||
}
|
||||
for (uint8_t i = 0; i < num_cores; i++) {
|
||||
if (!core[i].healthy()) {
|
||||
const char *failure = core[primary].prearm_failure_reason();
|
||||
if (failure != nullptr) {
|
||||
hal.util->snprintf(failure_msg, failure_msg_len, failure);
|
||||
AP::dal().snprintf(failure_msg, failure_msg_len, failure);
|
||||
} 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;
|
||||
}
|
||||
|
@ -1029,6 +993,8 @@ void NavEKF2::getTiltError(int8_t instance, float &ang) const
|
|||
// reset body axis gyro bias estimates
|
||||
void NavEKF2::resetGyroBias(void)
|
||||
{
|
||||
AP::dal().log_event2(AP_DAL::Event2::resetGyroBias);
|
||||
|
||||
if (core) {
|
||||
for (uint8_t i=0; i<num_cores; i++) {
|
||||
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
|
||||
bool NavEKF2::resetHeightDatum(void)
|
||||
{
|
||||
AP::dal().log_event2(AP_DAL::Event2::resetHeightDatum);
|
||||
|
||||
bool status = true;
|
||||
if (core) {
|
||||
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
|
||||
uint8_t NavEKF2::setInhibitGPS(void)
|
||||
{
|
||||
AP::dal().log_event2(AP_DAL::Event2::setInhibitGPS);
|
||||
|
||||
if (!core) {
|
||||
return 0;
|
||||
}
|
||||
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 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
|
||||
|
@ -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
|
||||
bool NavEKF2::setOriginLLH(const Location &loc)
|
||||
{
|
||||
AP::dal().log_SetOriginLLH2(loc);
|
||||
|
||||
if (!core) {
|
||||
return false;
|
||||
}
|
||||
|
@ -1186,7 +1170,7 @@ bool NavEKF2::setOriginLLH(const Location &loc)
|
|||
// we don't allow setting of the EKF origin unless we are
|
||||
// flying in non-GPS mode. This is to prevent accidental set
|
||||
// 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;
|
||||
}
|
||||
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
|
||||
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) {
|
||||
for (uint8_t i=0; i<num_cores; i++) {
|
||||
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
|
||||
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) {
|
||||
for (uint8_t i=0; i<num_cores; i++) {
|
||||
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.
|
||||
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) {
|
||||
for (uint8_t i=0; i<num_cores; i++) {
|
||||
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
|
||||
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;
|
||||
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
|
||||
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;
|
||||
|
||||
// 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
|
||||
void NavEKF2::updateLaneSwitchPosDownResetData(uint8_t new_primary, uint8_t old_primary)
|
||||
{
|
||||
// AP_DAL::log_updateLaneSwitchPosResetData(new_primary, old_primary);
|
||||
|
||||
float posDownOldPrimary, posDownNewPrimary, oldPosDownDelta;
|
||||
|
||||
// If core position reset data has been consumed reset delta to zero
|
||||
|
@ -1692,6 +1699,8 @@ bool NavEKF2::isExtNavUsedForYaw() const
|
|||
|
||||
void NavEKF2::requestYawReset(void)
|
||||
{
|
||||
AP::dal().log_event2(AP_DAL::Event2::requestYawReset);
|
||||
|
||||
for (uint8_t i = 0; i < num_cores; i++) {
|
||||
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.
|
||||
void NavEKF2::writeDefaultAirSpeed(float airspeed)
|
||||
{
|
||||
AP::dal().log_writeDefaultAirSpeed2(airspeed);
|
||||
|
||||
if (core) {
|
||||
for (uint8_t i=0; i<num_cores; i++) {
|
||||
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)
|
||||
{
|
||||
// AP_DAL::log_writeExtNavVelData(vel, err, timeStamp_ms, delay_ms);
|
||||
|
||||
if (core) {
|
||||
for (uint8_t i=0; i<num_cores; i++) {
|
||||
core[i].writeExtNavVelData(vel, err, timeStamp_ms, delay_ms);
|
||||
|
|
|
@ -29,7 +29,6 @@
|
|||
#include <AP_NavEKF/AP_Nav_Common.h>
|
||||
|
||||
class NavEKF2_core;
|
||||
class AP_AHRS;
|
||||
|
||||
class NavEKF2 {
|
||||
friend class NavEKF2_core;
|
||||
|
@ -54,9 +53,6 @@ public:
|
|||
// Update Filter States - this should be called whenever new IMU data is available
|
||||
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
|
||||
bool healthy(void) const;
|
||||
|
||||
|
@ -128,7 +124,7 @@ public:
|
|||
|
||||
// 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 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 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); }
|
||||
|
||||
// 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
|
||||
void getTimingStatistics(int8_t instance, struct ekf_timing &timing) const;
|
||||
|
@ -381,7 +377,6 @@ private:
|
|||
uint8_t primary; // current primary core
|
||||
NavEKF2_core *core = nullptr;
|
||||
bool core_malloc_failed;
|
||||
const AP_AHRS *_ahrs;
|
||||
|
||||
uint32_t _frameTimeUsec; // time per IMU frame
|
||||
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_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_Int8 _logging_mask; // mask of IMUs to log
|
||||
AP_Float _yawNoise; // magnetic yaw measurement noise : rad
|
||||
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)
|
||||
|
@ -480,13 +474,6 @@ private:
|
|||
struct Location common_EKF_origin;
|
||||
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
|
||||
uint64_t imuSampleTime_us;
|
||||
|
||||
|
@ -562,8 +549,9 @@ private:
|
|||
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_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_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;
|
||||
};
|
||||
|
|
|
@ -2,7 +2,6 @@
|
|||
|
||||
#include "AP_NavEKF2.h"
|
||||
#include "AP_NavEKF2_core.h"
|
||||
#include <AP_AHRS/AP_AHRS.h>
|
||||
|
||||
extern const AP_HAL::HAL& hal;
|
||||
|
||||
|
@ -21,16 +20,13 @@ extern const AP_HAL::HAL& hal;
|
|||
*/
|
||||
void NavEKF2_core::FuseAirspeed()
|
||||
{
|
||||
// start performance timer
|
||||
hal.util->perf_begin(_perf_FuseAirspeed);
|
||||
|
||||
// declarations
|
||||
float vn;
|
||||
float ve;
|
||||
float vd;
|
||||
float vwn;
|
||||
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));
|
||||
Vector3 SH_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.
|
||||
ForceSymmetry();
|
||||
ConstrainVariances();
|
||||
|
||||
// stop performance timer
|
||||
hal.util->perf_end(_perf_FuseAirspeed);
|
||||
}
|
||||
|
||||
// select fusion of true airspeed measurements
|
||||
|
@ -247,9 +240,6 @@ void NavEKF2_core::SelectBetaFusion()
|
|||
*/
|
||||
void NavEKF2_core::FuseSideslip()
|
||||
{
|
||||
// start performance timer
|
||||
hal.util->perf_begin(_perf_FuseSideslip);
|
||||
|
||||
// declarations
|
||||
float q0;
|
||||
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.
|
||||
ForceSymmetry();
|
||||
ConstrainVariances();
|
||||
|
||||
// stop the performance timer
|
||||
hal.util->perf_end(_perf_FuseSideslip);
|
||||
}
|
||||
|
||||
/********************************************************
|
||||
|
|
|
@ -2,7 +2,6 @@
|
|||
|
||||
#include "AP_NavEKF2.h"
|
||||
#include "AP_NavEKF2_core.h"
|
||||
#include <AP_AHRS/AP_AHRS.h>
|
||||
#include <GCS_MAVLink/GCS.h>
|
||||
|
||||
#include <stdio.h>
|
||||
|
@ -15,7 +14,7 @@ void NavEKF2_core::controlFilterModes()
|
|||
{
|
||||
// Determine motor arm status
|
||||
prevMotorsArmed = motorsArmed;
|
||||
motorsArmed = hal.util->get_soft_armed();
|
||||
motorsArmed = AP::dal().get_armed();
|
||||
if (motorsArmed && !prevMotorsArmed) {
|
||||
// set the time at which we arm to assist with checks
|
||||
timeAtArming_ms = imuSampleTime_ms;
|
||||
|
@ -72,7 +71,7 @@ void NavEKF2_core::setWindMagStateLearningMode()
|
|||
|
||||
// set the wind sate variances to the measurement uncertainty
|
||||
for (uint8_t index=22; index<=23; index++) {
|
||||
P[index][index] = sq(constrain_float(frontend->_easNoise, 0.5f, 5.0f) * constrain_float(_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 {
|
||||
// set the variances using a typical wind speed
|
||||
|
@ -273,7 +272,7 @@ void NavEKF2_core::setAidingMode()
|
|||
switch (PV_AidingMode) {
|
||||
case AID_NONE:
|
||||
// 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
|
||||
posTimeout = true;
|
||||
velTimeout = true;
|
||||
|
@ -292,7 +291,7 @@ void NavEKF2_core::setAidingMode()
|
|||
|
||||
case AID_RELATIVE:
|
||||
// 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;
|
||||
velTimeout = true;
|
||||
// Reset the last valid flow measurement time
|
||||
|
@ -307,20 +306,20 @@ void NavEKF2_core::setAidingMode()
|
|||
bool canUseExtNav = readyToUseExtNav();
|
||||
// We have commenced aiding and GPS usage is allowed
|
||||
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;
|
||||
velTimeout = false;
|
||||
// We have commenced aiding and range beacon usage is allowed
|
||||
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 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 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 beacon pos D offset = %3.1f (m)",(unsigned)imu_index,(double)bcnPosOffset);
|
||||
}
|
||||
// We have commenced aiding and external nav usage is allowed
|
||||
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 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 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);
|
||||
//handle yaw reset as special case only if compass is disabled
|
||||
if (!use_compass()) {
|
||||
extNavYawResetRequest = true;
|
||||
|
@ -356,7 +355,7 @@ void NavEKF2_core::checkAttitudeAlignmentStatus()
|
|||
tiltErrFilt = alpha*temp + (1.0f-alpha)*tiltErrFilt;
|
||||
if (tiltErrFilt < 0.005f && !tiltAlignComplete) {
|
||||
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
|
||||
|
@ -374,7 +373,7 @@ void NavEKF2_core::checkAttitudeAlignmentStatus()
|
|||
// return true if we should use the airspeed sensor
|
||||
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
|
||||
|
@ -411,7 +410,7 @@ bool NavEKF2_core::readyToUseExtNav(void) const
|
|||
// return true if we should use the compass
|
||||
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
|
||||
// be quite sensitive to a rapid spin of the ground vehicle if
|
||||
// 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
|
||||
|
@ -451,7 +450,7 @@ void NavEKF2_core::setOrigin(const Location &loc)
|
|||
// define Earth rotation vector in the NED navigation frame at the origin
|
||||
calcEarthRateNED(earthRateNED, EKF_origin.lat);
|
||||
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
|
||||
frontend->common_EKF_origin = EKF_origin;
|
||||
|
@ -548,7 +547,7 @@ void NavEKF2_core::runYawEstimatorPrediction()
|
|||
if (imuDataDelayed.time_ms - tasDataDelayed.time_ms < 5000) {
|
||||
trueAirspeed = tasDataDelayed.tas;
|
||||
} else {
|
||||
trueAirspeed = defaultAirSpeed * AP::ahrs().get_EAS2TAS();
|
||||
trueAirspeed = defaultAirSpeed * AP::dal().get_EAS2TAS();
|
||||
}
|
||||
} else {
|
||||
trueAirspeed = 0.0f;
|
||||
|
|
|
@ -2,6 +2,7 @@
|
|||
|
||||
#include <AP_HAL/HAL.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
|
||||
{
|
||||
|
@ -25,7 +26,7 @@ void NavEKF2::Log_Write_NKF1(uint8_t _core, uint64_t time_us) const
|
|||
const struct log_EKF1 pkt{
|
||||
LOG_PACKET_HEADER_INIT(LOG_NKF1_MSG),
|
||||
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)
|
||||
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)
|
||||
|
@ -61,7 +62,7 @@ void NavEKF2::Log_Write_NKF2(uint8_t _core, uint64_t time_us) const
|
|||
const struct log_NKF2 pkt2{
|
||||
LOG_PACKET_HEADER_INIT(LOG_NKF2_MSG),
|
||||
time_us : time_us,
|
||||
core : _core,
|
||||
core : DAL_CORE(_core),
|
||||
AZbias : (int8_t)(100*azbias),
|
||||
scaleX : (int16_t)(100*gyroScaleFactor.x),
|
||||
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{
|
||||
LOG_PACKET_HEADER_INIT(LOG_NKF3_MSG),
|
||||
time_us : time_us,
|
||||
core : _core,
|
||||
core : DAL_CORE(_core),
|
||||
innovVN : (int16_t)(100*velInnov.x),
|
||||
innovVE : (int16_t)(100*velInnov.y),
|
||||
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{
|
||||
LOG_PACKET_HEADER_INIT(LOG_NKF4_MSG),
|
||||
time_us : time_us,
|
||||
core : _core,
|
||||
core : DAL_CORE(_core),
|
||||
sqrtvarV : (int16_t)(100*velVar),
|
||||
sqrtvarP : (int16_t)(100*posVar),
|
||||
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));
|
||||
}
|
||||
|
||||
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 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
|
||||
|
@ -164,11 +170,12 @@ void NavEKF2::Log_Write_NKF5(uint64_t time_us) const
|
|||
float range=0; // measured range
|
||||
float gndOffsetErr=0; // filter ground offset state error
|
||||
Vector3f predictorErrors; // output predictor angle, velocity and position tracking error
|
||||
getFlowDebug(-1,normInnov, gndOffset, flowInnovX, flowInnovY, auxFlowInnov, HAGL, rngInnov, range, gndOffsetErr);
|
||||
getOutputTrackingError(-1,predictorErrors);
|
||||
getFlowDebug(_core, normInnov, gndOffset, flowInnovX, flowInnovY, auxFlowInnov, HAGL, rngInnov, range, gndOffsetErr);
|
||||
getOutputTrackingError(_core, predictorErrors);
|
||||
const struct log_NKF5 pkt5{
|
||||
LOG_PACKET_HEADER_INIT(LOG_NKF5_MSG),
|
||||
time_us : time_us,
|
||||
core : DAL_CORE(_core),
|
||||
normInnov : (uint8_t)(MIN(100*normInnov,255)),
|
||||
FIX : (int16_t)(1000*flowInnovX),
|
||||
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{
|
||||
LOG_PACKET_HEADER_INIT(LOG_NKQ_MSG),
|
||||
time_us : time_us,
|
||||
core : _core,
|
||||
core : DAL_CORE(_core),
|
||||
q1 : quat.q1,
|
||||
q2 : quat.q2,
|
||||
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));
|
||||
}
|
||||
|
||||
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) {
|
||||
uint8_t ID;
|
||||
float rng;
|
||||
|
@ -213,11 +225,12 @@ void NavEKF2::Log_Write_Beacon(uint64_t time_us) const
|
|||
Vector3f beaconPosNED;
|
||||
float bcnPosOffsetHigh;
|
||||
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) {
|
||||
struct log_RngBcnDebug pkt10 = {
|
||||
LOG_PACKET_HEADER_INIT(LOG_NKF10_MSG),
|
||||
time_us : time_us,
|
||||
core : DAL_CORE(_core),
|
||||
ID : (uint8_t)ID,
|
||||
rng : (int16_t)(100*rng),
|
||||
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()
|
||||
{
|
||||
// only log if enabled
|
||||
|
@ -245,32 +288,26 @@ void NavEKF2::Log_Write()
|
|||
return;
|
||||
}
|
||||
|
||||
const uint64_t time_us = AP_HAL::micros64();
|
||||
|
||||
Log_Write_NKF5(time_us);
|
||||
const uint64_t time_us = AP::dal().micros64();
|
||||
|
||||
// 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++) {
|
||||
Log_Write_NKF1(i, time_us);
|
||||
Log_Write_NKF2(i, time_us);
|
||||
Log_Write_NKF3(i, time_us);
|
||||
Log_Write_NKF4(i, time_us);
|
||||
Log_Write_NKF5(i, time_us);
|
||||
Log_Write_Quaternion(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(time_us);
|
||||
Log_Write_Beacon(i, 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);
|
||||
}
|
||||
Log_Write_Timing(i, time_us);
|
||||
}
|
||||
|
||||
AP::dal().start_frame(AP_DAL::FrameType::LogWriteEKF2);
|
||||
}
|
||||
|
||||
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",
|
||||
"QBffffffffffff",
|
||||
time_us,
|
||||
_core,
|
||||
DAL_CORE(_core),
|
||||
yaw_composite,
|
||||
sqrtf(MAX(yaw_composite_variance, 0.0f)),
|
||||
yaw[0],
|
||||
|
|
|
@ -115,7 +115,7 @@ void NavEKF2_core::controlMagYawReset()
|
|||
|
||||
// send initial alignment status to console
|
||||
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
|
||||
|
@ -148,14 +148,14 @@ void NavEKF2_core::controlMagYawReset()
|
|||
|
||||
// send initial alignment status to console
|
||||
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
|
||||
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) {
|
||||
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
|
||||
|
@ -206,7 +206,7 @@ void NavEKF2_core::realignYawGPS()
|
|||
ResetPosition();
|
||||
|
||||
// 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
|
||||
zeroAttCovOnly();
|
||||
|
@ -235,9 +235,6 @@ void NavEKF2_core::realignYawGPS()
|
|||
// select fusion of magnetometer data
|
||||
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
|
||||
// used for load levelling
|
||||
magFusePerformed = false;
|
||||
|
@ -293,9 +290,7 @@ void NavEKF2_core::SelectMagFusion()
|
|||
}
|
||||
|
||||
// fuse the three magnetometer componenents using sequential fusion of each axis
|
||||
hal.util->perf_begin(_perf_test[0]);
|
||||
FuseMagnetometer();
|
||||
hal.util->perf_end(_perf_test[0]);
|
||||
|
||||
// zero the test ratio output from the inactive simple magnetometer yaw fusion
|
||||
yawTestRatio = 0.0f;
|
||||
|
@ -317,9 +312,6 @@ void NavEKF2_core::SelectMagFusion()
|
|||
bodyMagFieldVar.y = P[20][20];
|
||||
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++;
|
||||
|
||||
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 {
|
||||
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
|
||||
|
|
|
@ -1,14 +1,9 @@
|
|||
#include <AP_HAL/AP_HAL.h>
|
||||
|
||||
#include "AP_NavEKF2_core.h"
|
||||
#include <AP_AHRS/AP_AHRS.h>
|
||||
#include <AP_Vehicle/AP_Vehicle.h>
|
||||
#include <GCS_MAVLink/GCS.h>
|
||||
#include <AP_RangeFinder/AP_RangeFinder.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>
|
||||
#include <AP_DAL/AP_DAL.h>
|
||||
|
||||
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
|
||||
// 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) {
|
||||
return;
|
||||
}
|
||||
|
@ -45,11 +40,11 @@ void NavEKF2_core::readRangeFinder(void)
|
|||
// use data from two range finders if available
|
||||
|
||||
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) {
|
||||
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] ++;
|
||||
if (rngMeasIndex[sensorIndex] > 2) {
|
||||
rngMeasIndex[sensorIndex] = 0;
|
||||
|
@ -195,7 +190,7 @@ void NavEKF2_core::writeOptFlowMeas(const uint8_t rawFlowQuality, const Vector2f
|
|||
// try changing to another compass
|
||||
void NavEKF2_core::tryChangeCompass(void)
|
||||
{
|
||||
const Compass &compass = AP::compass();
|
||||
const auto &compass = AP::dal().compass();
|
||||
const uint8_t maxCount = compass.get_count();
|
||||
|
||||
// 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 (compass.healthy(tempIndex) && compass.use_for_yaw(tempIndex) && tempIndex != magSelectIndex) {
|
||||
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
|
||||
magTimeout = false;
|
||||
lastHealthyMagTime_ms = imuSampleTime_ms;
|
||||
|
@ -233,12 +228,12 @@ void NavEKF2_core::tryChangeCompass(void)
|
|||
// check for new magnetometer data and update store measurements if available
|
||||
void NavEKF2_core::readMagData()
|
||||
{
|
||||
if (!_ahrs->get_compass()) {
|
||||
if (!AP::dal().get_compass()) {
|
||||
allMagSensorsFailed = true;
|
||||
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
|
||||
// magnetometer, then declare the magnetometers as failed for this flight
|
||||
|
@ -277,8 +272,6 @@ void NavEKF2_core::readMagData()
|
|||
if (use_compass() &&
|
||||
compass.healthy(magSelectIndex) &&
|
||||
compass.last_update_usec(magSelectIndex) - lastMagUpdate_us > 70000) {
|
||||
frontend->logging.log_compass = true;
|
||||
|
||||
|
||||
// detect changes to magnetometer offset parameters and reset states
|
||||
Vector3f nowMagOffsets = compass.get_offsets(magSelectIndex);
|
||||
|
@ -335,7 +328,7 @@ void NavEKF2_core::readMagData()
|
|||
*/
|
||||
void NavEKF2_core::readIMUData()
|
||||
{
|
||||
const AP_InertialSensor &ins = AP::ins();
|
||||
const auto &ins = AP::dal().ins();
|
||||
|
||||
// average IMU sampling rate
|
||||
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
|
||||
// return false if data is not available
|
||||
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()) {
|
||||
ins.get_delta_velocity(ins_index,dVel);
|
||||
|
@ -504,9 +497,9 @@ void NavEKF2_core::readGpsData()
|
|||
|
||||
// check for new GPS data
|
||||
// do not accept data at a faster rate than 14Hz to avoid overflowing the FIFO buffer
|
||||
const AP_GPS &gps = AP::gps();
|
||||
if (gps.last_message_time_ms() - lastTimeGpsReceived_ms > 70) {
|
||||
if (gps.status() >= AP_GPS::GPS_OK_FIX_3D) {
|
||||
const auto &gps = AP::dal().gps();
|
||||
if (gps.last_message_time_ms(gps.primary_sensor()) - lastTimeGpsReceived_ms > 70) {
|
||||
if (gps.status() >= AP_DAL_GPS::GPS_OK_FIX_3D) {
|
||||
// report GPS fix status
|
||||
gpsCheckStatus.bad_fix = false;
|
||||
|
||||
|
@ -514,7 +507,7 @@ void NavEKF2_core::readGpsData()
|
|||
secondLastGpsTime_ms = lastTimeGpsReceived_ms;
|
||||
|
||||
// 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
|
||||
|
@ -614,7 +607,7 @@ void NavEKF2_core::readGpsData()
|
|||
}
|
||||
|
||||
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()) {
|
||||
table_earth_field_ga = AP_Declination::get_earth_field_ga(gpsloc);
|
||||
table_declination = radians(AP_Declination::get_declination(gpsloc.lat*1.0e-7,
|
||||
|
@ -643,7 +636,7 @@ void NavEKF2_core::readGpsData()
|
|||
} else {
|
||||
// report GPS fix status
|
||||
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
|
||||
// return false if data is not available
|
||||
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()) {
|
||||
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 = MIN(dAng_dt,1.0e-1f);
|
||||
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
|
||||
// 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) {
|
||||
frontend->logging.log_baro = true;
|
||||
|
||||
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
|
||||
// we take a new reading, convert from EAS to TAS and set the flag letting other functions
|
||||
// know a new measurement is available
|
||||
const AP_Airspeed *aspeed = _ahrs->get_airspeed();
|
||||
const auto *aspeed = AP::dal().airspeed();
|
||||
if (aspeed &&
|
||||
aspeed->use() &&
|
||||
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();
|
||||
tasDataNew.time_ms = timeTasReceived_ms - frontend->tasDelay_ms;
|
||||
|
||||
|
@ -788,7 +779,7 @@ void NavEKF2_core::readAirSpdData()
|
|||
void NavEKF2_core::readRngBcnData()
|
||||
{
|
||||
// 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
|
||||
if (beacon == nullptr) {
|
||||
|
@ -970,7 +961,7 @@ float NavEKF2_core::MagDeclination(void) const
|
|||
if (!use_compass()) {
|
||||
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)
|
||||
{
|
||||
const AP_InertialSensor &ins = AP::ins();
|
||||
const auto &ins = AP::dal().ins();
|
||||
|
||||
// learn gyro biases
|
||||
for (uint8_t i=0; i<INS_MAX_INSTANCES; i++) {
|
||||
|
|
|
@ -26,8 +26,6 @@ void NavEKF2_core::SelectFlowFusion()
|
|||
optFlowFusionDelayed = false;
|
||||
}
|
||||
|
||||
// start performance timer
|
||||
hal.util->perf_begin(_perf_FuseOptFlow);
|
||||
// Perform Data Checks
|
||||
// Check if the optical flow data is still valid
|
||||
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
|
||||
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()
|
||||
{
|
||||
// start performance timer
|
||||
hal.util->perf_begin(_perf_TerrainOffset);
|
||||
|
||||
// horizontal velocity squared
|
||||
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);
|
||||
}
|
||||
|
||||
/*
|
||||
|
|
|
@ -1,9 +1,8 @@
|
|||
#include <AP_HAL/AP_HAL.h>
|
||||
|
||||
#include "AP_NavEKF2_core.h"
|
||||
#include <AP_DAL/AP_DAL.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;
|
||||
|
||||
|
@ -100,7 +99,7 @@ bool NavEKF2_core::getHeightControlLimit(float &height) const
|
|||
// only ask for limiting if we are doing optical flow only navigation
|
||||
if (frontend->_fusionModeGPS == 3 && (PV_AidingMode == AID_RELATIVE) && flowDataValid) {
|
||||
// If are doing optical flow nav, ensure the height above ground is within range finder limits after accounting for vehicle tilt and control errors
|
||||
const RangeFinder *_rng = AP::rangefinder();
|
||||
const auto *_rng = AP::dal().rangefinder();
|
||||
if (_rng == nullptr) {
|
||||
// we really, really shouldn't be here.
|
||||
return false;
|
||||
|
@ -121,7 +120,7 @@ bool NavEKF2_core::getHeightControlLimit(float &height) const
|
|||
void NavEKF2_core::getEulerAngles(Vector3f &euler) const
|
||||
{
|
||||
outputDataNew.quat.to_euler(euler.x, euler.y, euler.z);
|
||||
euler = euler - _ahrs->get_trim();
|
||||
euler = euler - AP::dal().get_trim();
|
||||
}
|
||||
|
||||
// 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
|
||||
{
|
||||
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
|
||||
|
@ -252,9 +251,9 @@ bool NavEKF2_core::getPosNE(Vector2f &posNE) const
|
|||
} else {
|
||||
// In constant position mode the EKF position states are at the origin, so we cannot use them as a position estimate
|
||||
if(validOrigin) {
|
||||
if ((AP::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
|
||||
const struct Location &gpsloc = AP::gps().location();
|
||||
const struct Location &gpsloc = AP::dal().gps().location();
|
||||
const Vector2f tempPosNE = EKF_origin.get_distance_NE(gpsloc);
|
||||
posNE.x = tempPosNE.x;
|
||||
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
|
||||
bool NavEKF2_core::getLLH(struct Location &loc) const
|
||||
{
|
||||
const AP_GPS &gps = AP::gps();
|
||||
const auto &gps = AP::dal().gps();
|
||||
Location origin;
|
||||
float posD;
|
||||
|
||||
|
@ -336,7 +335,7 @@ bool NavEKF2_core::getLLH(struct Location &loc) const
|
|||
} else {
|
||||
// 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
|
||||
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
|
||||
const struct Location &gpsloc = gps.location();
|
||||
loc.lat = gpsloc.lat;
|
||||
|
@ -358,7 +357,7 @@ bool NavEKF2_core::getLLH(struct Location &loc) const
|
|||
} else {
|
||||
// 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
|
||||
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();
|
||||
loc = gpsloc;
|
||||
loc.relative_alt = 0;
|
||||
|
@ -414,7 +413,7 @@ void NavEKF2_core::getMagXYZ(Vector3f &magXYZ) const
|
|||
// return true if offsets are valid
|
||||
bool NavEKF2_core::getMagOffsets(uint8_t mag_idx, Vector3f &magOffsets) const
|
||||
{
|
||||
if (!_ahrs->get_compass()) {
|
||||
if (!AP::dal().get_compass()) {
|
||||
return false;
|
||||
}
|
||||
|
||||
|
@ -425,12 +424,12 @@ bool NavEKF2_core::getMagOffsets(uint8_t mag_idx, Vector3f &magOffsets) const
|
|||
if ((mag_idx == magSelectIndex) &&
|
||||
finalInflightMagInit &&
|
||||
!inhibitMagStates &&
|
||||
_ahrs->get_compass()->healthy(magSelectIndex) &&
|
||||
AP::dal().get_compass()->healthy(magSelectIndex) &&
|
||||
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;
|
||||
} else {
|
||||
magOffsets = _ahrs->get_compass()->get_offsets(magSelectIndex);
|
||||
magOffsets = AP::dal().get_compass()->get_offsets(magSelectIndex);
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
|
|
@ -2,10 +2,7 @@
|
|||
|
||||
#include "AP_NavEKF2.h"
|
||||
#include "AP_NavEKF2_core.h"
|
||||
#include <AP_RangeFinder/AP_RangeFinder.h>
|
||||
#include <AP_RangeFinder/AP_RangeFinder_Backend.h>
|
||||
#include <AP_GPS/AP_GPS.h>
|
||||
#include <AP_Baro/AP_Baro.h>
|
||||
#include <AP_DAL/AP_DAL.h>
|
||||
|
||||
#include <stdio.h>
|
||||
|
||||
|
@ -290,7 +287,7 @@ bool NavEKF2_core::resetHeightDatum(void)
|
|||
// record the old height estimate
|
||||
float oldHgt = -stateStruct.position.z;
|
||||
// reset the barometer so that it reads zero at the current height
|
||||
AP::baro().update_calibration();
|
||||
AP::dal().baro().update_calibration();
|
||||
// reset the height state
|
||||
stateStruct.position.z = 0.0f;
|
||||
// adjust the height of the EKF origin so that the origin plus baro height before and after the reset is the same
|
||||
|
@ -306,7 +303,7 @@ bool NavEKF2_core::resetHeightDatum(void)
|
|||
// altitude. This ensures the reported AMSL alt from
|
||||
// getLLH() is equal to GPS altitude, while also ensuring
|
||||
// that the relative alt is zero
|
||||
EKF_origin.alt = AP::gps().location().alt;
|
||||
EKF_origin.alt = AP::dal().gps().location().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
|
||||
{
|
||||
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()) {
|
||||
return;
|
||||
}
|
||||
|
@ -348,7 +345,7 @@ void NavEKF2_core::CorrectGPSForAntennaOffset(gps_elements &gps_data) const
|
|||
void NavEKF2_core::CorrectExtNavForSensorOffset(Vector3f &ext_position) const
|
||||
{
|
||||
#if HAL_VISUALODOM_ENABLED
|
||||
AP_VisualOdom *visual_odom = AP::visualodom();
|
||||
const auto *visual_odom = AP::dal().visualodom();
|
||||
if (visual_odom == nullptr) {
|
||||
return;
|
||||
}
|
||||
|
@ -367,7 +364,7 @@ void NavEKF2_core::CorrectExtNavForSensorOffset(Vector3f &ext_position) const
|
|||
void NavEKF2_core::CorrectExtNavVelForSensorOffset(Vector3f &ext_velocity) const
|
||||
{
|
||||
#if HAL_VISUALODOM_ENABLED
|
||||
AP_VisualOdom *visual_odom = AP::visualodom();
|
||||
const auto *visual_odom = AP::dal().visualodom();
|
||||
if (visual_odom == nullptr) {
|
||||
return;
|
||||
}
|
||||
|
@ -568,9 +565,6 @@ void NavEKF2_core::SelectVelPosFusion()
|
|||
// fuse selected position, velocity and height measurements
|
||||
void NavEKF2_core::FuseVelPosNED()
|
||||
{
|
||||
// start performance timer
|
||||
hal.util->perf_begin(_perf_FuseVelPosNED);
|
||||
|
||||
// health is set bad until test passed
|
||||
velHealth = 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
|
||||
// the corrected reading is the reading that would have been taken if the sensor was
|
||||
// co-located with the IMU
|
||||
const RangeFinder *_rng = AP::rangefinder();
|
||||
const auto *_rng = AP::dal().rangefinder();
|
||||
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) {
|
||||
Vector3f posOffsetBody = sensor->get_pos_offset() - accelPosOffset;
|
||||
if (!posOffsetBody.is_zero()) {
|
||||
|
|
|
@ -1,10 +1,8 @@
|
|||
#include <AP_HAL/AP_HAL.h>
|
||||
|
||||
#include "AP_NavEKF2_core.h"
|
||||
#include <AP_AHRS/AP_AHRS.h>
|
||||
#include <AP_Vehicle/AP_Vehicle.h>
|
||||
#include <GCS_MAVLink/GCS.h>
|
||||
#include <AP_GPS/AP_GPS.h>
|
||||
|
||||
extern const AP_HAL::HAL& hal;
|
||||
|
||||
|
@ -20,7 +18,7 @@ extern const AP_HAL::HAL& hal;
|
|||
*/
|
||||
void NavEKF2_core::calcGpsGoodToAlign(void)
|
||||
{
|
||||
const AP_GPS &gps = AP::gps();
|
||||
const auto &gps = AP::dal().gps();
|
||||
|
||||
if (inFlight && assume_zero_sideslip() && !use_compass()) {
|
||||
// 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
|
||||
if (gpsDriftFail) {
|
||||
hal.util->snprintf(prearm_fail_string,
|
||||
AP::dal().snprintf(prearm_fail_string,
|
||||
sizeof(prearm_fail_string),
|
||||
"GPS drift %.1fm (needs %.1f)", (double)gpsDriftNE, (double)(3.0f*checkScaler));
|
||||
gpsCheckStatus.bad_horiz_drift = true;
|
||||
|
@ -95,9 +93,9 @@ void NavEKF2_core::calcGpsGoodToAlign(void)
|
|||
// 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
|
||||
// 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);
|
||||
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 {
|
||||
gpsVertVelFail = false;
|
||||
|
@ -105,7 +103,7 @@ void NavEKF2_core::calcGpsGoodToAlign(void)
|
|||
|
||||
// Report check result as a text string and bitmask
|
||||
if (gpsVertVelFail) {
|
||||
hal.util->snprintf(prearm_fail_string,
|
||||
AP::dal().snprintf(prearm_fail_string,
|
||||
sizeof(prearm_fail_string),
|
||||
"GPS vertical speed %.2fm/s (needs %.2f)", (double)fabsf(gpsVertVelFilt), (double)(0.3f*checkScaler));
|
||||
gpsCheckStatus.bad_vert_vel = true;
|
||||
|
@ -126,7 +124,7 @@ void NavEKF2_core::calcGpsGoodToAlign(void)
|
|||
|
||||
// Report check result as a text string and bitmask
|
||||
if (gpsHorizVelFail) {
|
||||
hal.util->snprintf(prearm_fail_string,
|
||||
AP::dal().snprintf(prearm_fail_string,
|
||||
sizeof(prearm_fail_string),
|
||||
"GPS horizontal speed %.2fm/s (needs %.2f)", (double)gpsDriftNE, (double)(0.3f*checkScaler));
|
||||
gpsCheckStatus.bad_horiz_vel = true;
|
||||
|
@ -145,7 +143,7 @@ void NavEKF2_core::calcGpsGoodToAlign(void)
|
|||
|
||||
// Report check result as a text string and bitmask
|
||||
if (hAccFail) {
|
||||
hal.util->snprintf(prearm_fail_string,
|
||||
AP::dal().snprintf(prearm_fail_string,
|
||||
sizeof(prearm_fail_string),
|
||||
"GPS horiz error %.1fm (needs %.1f)", (double)hAcc, (double)(5.0f*checkScaler));
|
||||
gpsCheckStatus.bad_hAcc = true;
|
||||
|
@ -161,7 +159,7 @@ void NavEKF2_core::calcGpsGoodToAlign(void)
|
|||
}
|
||||
// Report check result as a text string and bitmask
|
||||
if (vAccFail) {
|
||||
hal.util->snprintf(prearm_fail_string,
|
||||
AP::dal().snprintf(prearm_fail_string,
|
||||
sizeof(prearm_fail_string),
|
||||
"GPS vert error %.1fm (needs < %.1f)", (double)vAcc, (double)(7.5f * checkScaler));
|
||||
gpsCheckStatus.bad_vAcc = true;
|
||||
|
@ -174,7 +172,7 @@ void NavEKF2_core::calcGpsGoodToAlign(void)
|
|||
|
||||
// Report check result as a text string and bitmask
|
||||
if (gpsSpdAccFail) {
|
||||
hal.util->snprintf(prearm_fail_string,
|
||||
AP::dal().snprintf(prearm_fail_string,
|
||||
sizeof(prearm_fail_string),
|
||||
"GPS speed error %.1f (needs < %.1f)", (double)gpsSpdAccuracy, (double)(1.0f*checkScaler));
|
||||
gpsCheckStatus.bad_sAcc = true;
|
||||
|
@ -187,7 +185,7 @@ void NavEKF2_core::calcGpsGoodToAlign(void)
|
|||
|
||||
// Report check result as a text string and bitmask
|
||||
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()));
|
||||
gpsCheckStatus.bad_hdop = true;
|
||||
} else {
|
||||
|
@ -199,7 +197,7 @@ void NavEKF2_core::calcGpsGoodToAlign(void)
|
|||
|
||||
// Report check result as a text string and bitmask
|
||||
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());
|
||||
gpsCheckStatus.bad_sats = true;
|
||||
} else {
|
||||
|
@ -217,7 +215,7 @@ void NavEKF2_core::calcGpsGoodToAlign(void)
|
|||
|
||||
// Report check result as a text string and bitmask
|
||||
if (yawFail) {
|
||||
hal.util->snprintf(prearm_fail_string,
|
||||
AP::dal().snprintf(prearm_fail_string,
|
||||
sizeof(prearm_fail_string),
|
||||
"Mag yaw error x=%.1f y=%.1f",
|
||||
(double)magTestRatio.x,
|
||||
|
@ -229,7 +227,7 @@ void NavEKF2_core::calcGpsGoodToAlign(void)
|
|||
|
||||
// assume failed first time through and notify user checks have started
|
||||
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;
|
||||
}
|
||||
|
||||
|
@ -266,7 +264,7 @@ void NavEKF2_core::calcGpsGoodForFlight(void)
|
|||
|
||||
// get the receivers reported speed accuracy
|
||||
float gpsSpdAccRaw;
|
||||
if (!AP::gps().speed_accuracy(gpsSpdAccRaw)) {
|
||||
if (!AP::dal().gps().speed_accuracy(gpsSpdAccRaw)) {
|
||||
gpsSpdAccRaw = 0.0f;
|
||||
}
|
||||
|
||||
|
@ -326,9 +324,9 @@ void NavEKF2_core::detectFlight()
|
|||
bool largeHgtChange = false;
|
||||
|
||||
// trigger at 8 m/s airspeed
|
||||
if (_ahrs->airspeed_sensor_enabled()) {
|
||||
const AP_Airspeed *airspeed = _ahrs->get_airspeed();
|
||||
if (airspeed->get_airspeed() * AP::ahrs().get_EAS2TAS() > 10.0f) {
|
||||
if (AP::dal().airspeed_sensor_enabled()) {
|
||||
const auto *airspeed = AP::dal().airspeed();
|
||||
if (airspeed->get_airspeed() * AP::dal().get_EAS2TAS() > 10.0f) {
|
||||
highAirSpd = true;
|
||||
}
|
||||
}
|
||||
|
@ -385,8 +383,7 @@ void NavEKF2_core::detectFlight()
|
|||
|
||||
// If more than 5 seconds since likely_flying was set
|
||||
// true, then set inFlight true
|
||||
const AP_Vehicle *vehicle = AP::vehicle();
|
||||
if (vehicle->get_time_flying_ms() > 5000) {
|
||||
if (AP::dal().get_time_flying_ms() > 5000) {
|
||||
inFlight = true;
|
||||
}
|
||||
}
|
||||
|
@ -481,7 +478,7 @@ void NavEKF2_core::detectOptFlowTakeoff(void)
|
|||
{
|
||||
if (!onGround && !takeOffDetected && (imuSampleTime_ms - timeAtArming_ms) > 1000) {
|
||||
// we are no longer confidently on the ground so check the range finder and gyro for signs of takeoff
|
||||
const AP_InertialSensor &ins = AP::ins();
|
||||
const auto &ins = AP::dal().ins();
|
||||
Vector3f angRateVec;
|
||||
Vector3f gyroBias;
|
||||
getGyroBias(gyroBias);
|
||||
|
|
|
@ -2,9 +2,7 @@
|
|||
|
||||
#include "AP_NavEKF2.h"
|
||||
#include "AP_NavEKF2_core.h"
|
||||
#include <AP_AHRS/AP_AHRS.h>
|
||||
#include <GCS_MAVLink/GCS.h>
|
||||
#include <AP_GPS/AP_GPS.h>
|
||||
|
||||
extern const AP_HAL::HAL& hal;
|
||||
|
||||
|
@ -32,26 +30,8 @@ extern const AP_HAL::HAL& hal;
|
|||
|
||||
// constructor
|
||||
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)
|
||||
{
|
||||
_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
|
||||
|
@ -61,7 +41,6 @@ bool NavEKF2_core::setup_core(uint8_t _imu_index, uint8_t _core_index)
|
|||
gyro_index_active = _imu_index;
|
||||
accel_index_active = _imu_index;
|
||||
core_index = _core_index;
|
||||
_ahrs = frontend->_ahrs;
|
||||
|
||||
/*
|
||||
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
|
||||
shorter buffer.
|
||||
*/
|
||||
if (AP::ins().get_loop_rate_hz() < 100) {
|
||||
if (AP::dal().ins().get_loop_rate_hz() < 100) {
|
||||
imu_buffer_length = 13;
|
||||
} else {
|
||||
// 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))) {
|
||||
// check if there is enough memory to create the EKF-GSF object
|
||||
if (hal.util->available_memory() < sizeof(EKFGSF_yaw) + 1024) {
|
||||
gcs().send_text(MAV_SEVERITY_CRITICAL, "EKF2 IMU%u GSF: not enough memory",(unsigned)imu_index);
|
||||
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);
|
||||
return false;
|
||||
}
|
||||
|
||||
// try to instantiate
|
||||
yawEstimator = new EKFGSF_yaw();
|
||||
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;
|
||||
}
|
||||
}
|
||||
|
@ -138,7 +117,7 @@ bool NavEKF2_core::setup_core(uint8_t _imu_index, uint8_t _core_index)
|
|||
void NavEKF2_core::InitialiseVariables()
|
||||
{
|
||||
// calculate the nominal filter update rate
|
||||
const AP_InertialSensor &ins = AP::ins();
|
||||
const auto &ins = AP::dal().ins();
|
||||
localFilterTimeStep_ms = (uint8_t)(1000*ins.get_loop_delta_t());
|
||||
localFilterTimeStep_ms = MAX(localFilterTimeStep_ms,10);
|
||||
|
||||
|
@ -348,7 +327,7 @@ void NavEKF2_core::InitialiseVariables()
|
|||
have_table_earth_field = false;
|
||||
|
||||
// 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();
|
||||
|
||||
|
@ -394,8 +373,8 @@ void NavEKF2_core::InitialiseVariablesMag()
|
|||
bool NavEKF2_core::InitialiseFilterBootstrap(void)
|
||||
{
|
||||
// If we are a plane and don't have GPS lock then don't initialise
|
||||
if (assume_zero_sideslip() && AP::gps().status() < AP_GPS::GPS_OK_FIX_3D) {
|
||||
hal.util->snprintf(prearm_fail_string,
|
||||
if (assume_zero_sideslip() && AP::dal().gps().status(AP::dal().gps().primary_sensor()) < AP_DAL_GPS::GPS_OK_FIX_3D) {
|
||||
AP::dal().snprintf(prearm_fail_string,
|
||||
sizeof(prearm_fail_string),
|
||||
"EKF2 init failure: No GPS lock");
|
||||
statesInitialised = false;
|
||||
|
@ -416,7 +395,7 @@ bool NavEKF2_core::InitialiseFilterBootstrap(void)
|
|||
// set re-used variables to zero
|
||||
InitialiseVariables();
|
||||
|
||||
const AP_InertialSensor &ins = AP::ins();
|
||||
const auto &ins = AP::dal().ins();
|
||||
|
||||
// Initialise IMU data
|
||||
dtIMUavg = ins.get_loop_delta_t();
|
||||
|
@ -473,7 +452,7 @@ bool NavEKF2_core::InitialiseFilterBootstrap(void)
|
|||
ResetHeight();
|
||||
|
||||
// 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
|
||||
CovarianceInit();
|
||||
|
@ -559,9 +538,8 @@ void NavEKF2_core::UpdateFilter(bool predict)
|
|||
#if ENABLE_EKF_TIMING
|
||||
void *istate = hal.scheduler->disable_interrupts_save();
|
||||
static uint32_t timing_start_us;
|
||||
timing_start_us = AP_HAL::micros();
|
||||
timing_start_us = AP::dal().micros();
|
||||
#endif
|
||||
hal.util->perf_begin(_perf_UpdateFilter);
|
||||
|
||||
fill_scratch_variables();
|
||||
|
||||
|
@ -620,11 +598,10 @@ void NavEKF2_core::UpdateFilter(bool predict)
|
|||
calcOutputStates();
|
||||
|
||||
// stop the timer used for load measurement
|
||||
hal.util->perf_end(_perf_UpdateFilter);
|
||||
#if ENABLE_EKF_TIMING
|
||||
static uint32_t total_us;
|
||||
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) {
|
||||
hal.console->printf("ekf2 avg %.2f us\n", total_us / float(timing_counter));
|
||||
total_us = 0;
|
||||
|
@ -641,14 +618,14 @@ void NavEKF2_core::UpdateFilter(bool predict)
|
|||
it try again.
|
||||
*/
|
||||
if (filterStatus.value != 0) {
|
||||
last_filter_ok_ms = AP_HAL::millis();
|
||||
last_filter_ok_ms = AP::dal().millis();
|
||||
}
|
||||
if (filterStatus.value == 0 &&
|
||||
last_filter_ok_ms != 0 &&
|
||||
AP_HAL::millis() - last_filter_ok_ms > 5000 &&
|
||||
!hal.util->get_soft_armed()) {
|
||||
AP::dal().millis() - last_filter_ok_ms > 5000 &&
|
||||
!AP::dal().get_armed()) {
|
||||
// we've been unhealthy for 5 seconds after being healthy, reset the filter
|
||||
gcs().send_text(MAV_SEVERITY_WARNING, "EKF2 IMU%u forced reset",(unsigned)imu_index);
|
||||
GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "EKF2 IMU%u forced reset",(unsigned)imu_index);
|
||||
last_filter_ok_ms = 0;
|
||||
statesInitialised = false;
|
||||
InitialiseFilterBootstrap();
|
||||
|
@ -908,7 +885,6 @@ void NavEKF2_core::calcOutputStates()
|
|||
*/
|
||||
void NavEKF2_core::CovariancePrediction()
|
||||
{
|
||||
hal.util->perf_begin(_perf_CovariancePrediction);
|
||||
float windVelSigma; // wind velocity 1-sigma process noise - m/s
|
||||
float dAngBiasSigma;// delta angle bias 1-sigma process noise - rad/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
|
||||
ConstrainVariances();
|
||||
|
||||
hal.util->perf_end(_perf_CovariancePrediction);
|
||||
}
|
||||
|
||||
// zero specified range of rows in the state covariance matrix
|
||||
|
|
|
@ -24,16 +24,13 @@
|
|||
#pragma GCC optimize("O2")
|
||||
#endif
|
||||
|
||||
#define EK2_DISABLE_INTERRUPTS 0
|
||||
|
||||
|
||||
#include <AP_Common/Location.h>
|
||||
#include <AP_Math/AP_Math.h>
|
||||
#include <AP_Math/vectorN.h>
|
||||
#include <AP_NavEKF/AP_NavEKF_core_common.h>
|
||||
#include <AP_NavEKF2/AP_NavEKF2_Buffer.h>
|
||||
#include <AP_InertialSensor/AP_InertialSensor.h>
|
||||
#include <GCS_MAVLink/GCS_MAVLink.h>
|
||||
#include <AP_DAL/AP_DAL.h>
|
||||
|
||||
#include "AP_NavEKF/EKFGSF_yaw.h"
|
||||
|
||||
|
@ -427,8 +424,6 @@ private:
|
|||
typedef uint32_t Vector_u32_50[50];
|
||||
#endif
|
||||
|
||||
const AP_AHRS *_ahrs;
|
||||
|
||||
// the states are available in two forms, either as a Vector31, or
|
||||
// broken down as individual elements. Both are equivalent (same
|
||||
// memory)
|
||||
|
@ -1223,17 +1218,6 @@ private:
|
|||
// string representing last reason for prearm failure
|
||||
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
|
||||
bool have_table_earth_field; // true when we have initialised table_earth_field_ga
|
||||
Vector3f table_earth_field_ga; // earth field from WMM tables
|
||||
|
|
|
@ -3,7 +3,6 @@
|
|||
|
||||
#include "AP_NavEKF2.h"
|
||||
#include "AP_NavEKF2_core.h"
|
||||
#include <AP_AHRS/AP_AHRS.h>
|
||||
#include <AP_Vehicle/AP_Vehicle.h>
|
||||
|
||||
#include <stdio.h>
|
||||
|
|
|
@ -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"},
|
||||
|
Loading…
Reference in New Issue