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

AP_AHRS: auto-enable EKF3 for misconfiguration

Browse Source 
this fixes a "climb away" in quadplanes when AHRS_EKF_TYPE=2 and
EK2_ENABLE=0. The user has ARMING_CHECK=5390 to disable key arming
checks, and the plane flew on DCM as the configured EKF wasn't
enabled.

This resulted in AHRS::get_origin() returning false as it couldn't ask
EKF2 for the origin (as EKF2 was not enabled or instantiated)

The result was an incorrect calculation for target height during the
descent stage of QRTL, resulting in a climb away until the user took
over
This commit is contained in:
Andrew Tridgell 2022-12-10 11:59:33 +11:00
parent 8bf8d4889a
commit 56c8cffc07
2 changed files with 35 additions and 14 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

46
libraries/AP_AHRS/AP_AHRS.cpp
View File

@ -223,6 +223,14 @@ void AP_AHRS::init()
}
#endif
#if HAL_NAVEKF2_AVAILABLE && HAL_NAVEKF3_AVAILABLE
// a special case to catch users who had AHRS_EKF_TYPE=2 saved and
// updated to a version where EK2_ENABLE=0
if (_ekf_type.get() == 2 && !EKF2.get_enable() && EKF3.get_enable()) {
_ekf_type.set(3);
}
#endif
last_active_ekf_type = (EKFType)_ekf_type.get();
// init backends
@ -2705,29 +2713,21 @@ void AP_AHRS::send_ekf_status_report(GCS_MAVLINK &link) const
}
}
// passes a reference to the location of the inertial navigation origin
// in WGS-84 coordinates
// returns a boolean true when the inertial navigation origin has been set
bool AP_AHRS::get_origin(Location &ret) const
// return origin for a specified EKF type
bool AP_AHRS::get_origin(EKFType type, Location &ret) const
{
switch (ekf_type()) {
switch (type) {
case EKFType::NONE:
return dcm.get_origin(ret);
#if HAL_NAVEKF2_AVAILABLE
case EKFType::TWO:
if (!EKF2.getOriginLLH(ret)) {
return false;
}
return true;
return EKF2.getOriginLLH(ret);
#endif
#if HAL_NAVEKF3_AVAILABLE
case EKFType::THREE:
if (!EKF3.getOriginLLH(ret)) {
return false;
}
return true;
return EKF3.getOriginLLH(ret);
#endif
#if AP_AHRS_SIM_ENABLED
@ -2745,7 +2745,25 @@ bool AP_AHRS::get_origin(Location &ret) const
return AP::externalAHRS().get_origin(ret);
#endif
}
return false;
}
/*
return origin for the configured EKF type. If we are armed and the
configured EKF type cannot return an origin then return origin for
the active EKF type (if available)
This copes with users force arming a plane that is running on DCM as
the EKF has not fully initialised
*/
bool AP_AHRS::get_origin(Location &ret) const
{
if (get_origin(ekf_type(), ret)) {
return true;
}
if (hal.util->get_soft_armed() && get_origin(active_EKF_type(), ret)) {
return true;
}
return false;
}
@ -2754,7 +2772,7 @@ bool AP_AHRS::get_origin(Location &ret) const
// it will return false when no limiting is required
bool AP_AHRS::get_hgt_ctrl_limit(float& limit) const
{
switch (ekf_type()) {
switch (active_EKF_type()) {
case EKFType::NONE:
// We are not using an EKF so no limiting applies
return false;

3
libraries/AP_AHRS/AP_AHRS.h
View File

@ -690,6 +690,9 @@ private:
// update roll_sensor, pitch_sensor and yaw_sensor
void update_cd_values(void);
// return origin for a specified EKF type
bool get_origin(EKFType type, Location &ret) const;
// helper trig variables
float _cos_roll{1.0f};
float _cos_pitch{1.0f};