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

AP_AHRS: auto-switch to EK3/EK2 if selected option disabled

Browse Source 
if we have an EKF backend selected and that backend doesn't exist then
auto-switch to the other backend.

This fixes MatekF405-Wing which has EKF2 disabled and was falling back
to DCM
This commit is contained in:
Andrew Tridgell 2020-04-12 08:26:48 +10:00
parent 685d26c808
commit 5c399fce41
2 changed files with 31 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

26
libraries/AP_AHRS/AP_AHRS_NavEKF.cpp
View File

@ -46,6 +46,24 @@ AP_AHRS_NavEKF::AP_AHRS_NavEKF(uint8_t flags) :
_dcm_matrix.identity(); _dcm_matrix.identity();
} }
// init sets up INS board orientation
void AP_AHRS_NavEKF::init()
{
#if !HAL_NAVEKF2_AVAILABLE && HAL_NAVEKF3_AVAILABLE
if (_ekf_type.get() == 2) {
_ekf_type.set(3);
EKF3.set_enable(true);
}
#elif !HAL_NAVEKF3_AVAILABLE && HAL_NAVEKF2_AVAILABLE
if (_ekf_type.get() == 3) {
_ekf_type.set(2);
EKF2.set_enable(true);
}
#endif
// init parent class
AP_AHRS_DCM::init();
}
// return the smoothed gyro vector corrected for drift // return the smoothed gyro vector corrected for drift
const Vector3f &AP_AHRS_NavEKF::get_gyro(void) const const Vector3f &AP_AHRS_NavEKF::get_gyro(void) const
{ {
@ -351,6 +369,7 @@ void AP_AHRS_NavEKF::update_SITL(void)
} }
#if HAL_NAVEKF3_AVAILABLE
if (_sitl->odom_enable) { if (_sitl->odom_enable) {
// use SITL states to write body frame odometry data at 20Hz // use SITL states to write body frame odometry data at 20Hz
uint32_t timeStamp_ms = AP_HAL::millis(); uint32_t timeStamp_ms = AP_HAL::millis();
@ -372,6 +391,7 @@ void AP_AHRS_NavEKF::update_SITL(void)
EKF3.writeBodyFrameOdom(quality, delPos, delAng, delTime, timeStamp_ms, posOffset); EKF3.writeBodyFrameOdom(quality, delPos, delAng, delTime, timeStamp_ms, posOffset);
} }
} }
#endif // HAL_NAVEKF3_AVAILABLE
} }
#endif // CONFIG_HAL_BOARD #endif // CONFIG_HAL_BOARD
@ -1647,7 +1667,11 @@ bool AP_AHRS_NavEKF::attitudes_consistent(char *failure_msg, const uint8_t failu
// for EKF3. DCM can't use external yaw, so we don't expect it's // for EKF3. DCM can't use external yaw, so we don't expect it's
// yaw to align with EKF3 when EKF3 is using an external yaw // yaw to align with EKF3 when EKF3 is using an external yaw
// source // source
if (ekf_type() != EKFType::THREE || !EKF3.using_external_yaw()) { bool using_external_yaw = false;
#if HAL_NAVEKF3_AVAILABLE
using_external_yaw = ekf_type() == EKFType::THREE && EKF3.using_external_yaw();
#endif
if (!using_external_yaw) {
diff = fabsf(angle_diff.z); diff = fabsf(angle_diff.z);
if (check_yaw && (diff > ATTITUDE_CHECK_THRESH_YAW_RAD)) { if (check_yaw && (diff > ATTITUDE_CHECK_THRESH_YAW_RAD)) {
hal.util->snprintf(failure_msg, failure_msg_len, "DCM Yaw inconsistent by %d deg", (int)degrees(diff)); hal.util->snprintf(failure_msg, failure_msg_len, "DCM Yaw inconsistent by %d deg", (int)degrees(diff));

9
libraries/AP_AHRS/AP_AHRS_NavEKF.h
View File

@ -55,6 +55,9 @@ public:
// Constructor // Constructor
AP_AHRS_NavEKF(uint8_t flags = FLAG_NONE); AP_AHRS_NavEKF(uint8_t flags = FLAG_NONE);
// initialise
void init(void) override;
/* Do not allow copies */ /* Do not allow copies */
AP_AHRS_NavEKF(const AP_AHRS_NavEKF &other) = delete; AP_AHRS_NavEKF(const AP_AHRS_NavEKF &other) = delete;
AP_AHRS_NavEKF &operator=(const AP_AHRS_NavEKF&) = delete; AP_AHRS_NavEKF &operator=(const AP_AHRS_NavEKF&) = delete;
@ -296,12 +299,12 @@ public:
private: private:
enum class EKFType { enum class EKFType {
NONE = 0, NONE = 0
#if HAL_NAVEKF3_AVAILABLE #if HAL_NAVEKF3_AVAILABLE
THREE = 3, ,THREE = 3
#endif #endif
#if HAL_NAVEKF2_AVAILABLE #if HAL_NAVEKF2_AVAILABLE
TWO = 2 ,TWO = 2
#endif #endif
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
,SITL = 10 ,SITL = 10