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

AP_AHRS_NavEKF: get_secondary_quat and position may return EKF3 value

Browse Source
This commit is contained in:
Randy Mackay 2020-11-27 11:37:04 +09:00
parent 1b3ad9cd8d
commit 0cc9fd2b5d
2 changed files with 126 additions and 56 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

178
libraries/AP_AHRS/AP_AHRS_NavEKF.cpp
View File

@ -741,51 +741,51 @@ bool AP_AHRS_NavEKF::get_quaternion(Quaternion &quat) const
// return secondary attitude solution if available, as eulers in radians
bool AP_AHRS_NavEKF::get_secondary_attitude(Vector3f &eulers) const
{
switch (active_EKF_type()) {
EKFType secondary_ekf_type;
if (!get_secondary_EKF_type(secondary_ekf_type)) {
return false;
}
switch (secondary_ekf_type) {
case EKFType::NONE:
// EKF is secondary
switch ((EKFType)_ekf_type.get()) {
#if HAL_NAVEKF3_AVAILABLE
case EKFType::THREE:
EKF3.getEulerAngles(-1, eulers);
return _ekf3_started;
#endif
#if HAL_NAVEKF2_AVAILABLE
case EKFType::TWO:
EKF2.getEulerAngles(-1, eulers);
return _ekf2_started;
#endif
#if HAL_EXTERNAL_AHRS_ENABLED
case EKFType::EXTERNAL: {
Quaternion quat;
if (!AP::externalAHRS().get_quaternion(quat)) {
return false;
}
quat.to_euler(eulers.x, eulers.y, eulers.z);
return true;
}
#endif
default:
return false;
}
// DCM is secondary
eulers = _dcm_attitude;
return true;
#if HAL_NAVEKF2_AVAILABLE
case EKFType::TWO:
// EKF2 is secondary
EKF2.getEulerAngles(-1, eulers);
return _ekf2_started;
#endif
#if HAL_NAVEKF3_AVAILABLE
case EKFType::THREE:
// EKF3 is secondary
EKF3.getEulerAngles(-1, eulers);
return _ekf3_started;
#endif
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
case EKFType::SITL:
// SITL is secondary (should never happen)
return false;
#endif
#if HAL_EXTERNAL_AHRS_ENABLED
case EKFType::EXTERNAL:
#endif
// DCM is secondary
eulers = _dcm_attitude;
case EKFType::EXTERNAL: {
// External is secondary
Quaternion quat;
if (!AP::externalAHRS().get_quaternion(quat)) {
return false;
}
quat.to_euler(eulers.x, eulers.y, eulers.z);
return true;
}
#endif
}
// since there is no default case above, this is unreachable
return false;
}
@ -794,36 +794,45 @@ bool AP_AHRS_NavEKF::get_secondary_attitude(Vector3f &eulers) const
// return secondary attitude solution if available, as quaternion
bool AP_AHRS_NavEKF::get_secondary_quaternion(Quaternion &quat) const
{
switch (active_EKF_type()) {
case EKFType::NONE:
// EKF is secondary
#if HAL_NAVEKF2_AVAILABLE
EKF2.getQuaternion(-1, quat);
return _ekf2_started;
#elif HAL_EXTERNAL_AHRS_ENABLED
return AP::externalAHRS().get_quaternion(quat);
#else
EKFType secondary_ekf_type;
if (!get_secondary_EKF_type(secondary_ekf_type)) {
return false;
#endif
}
switch (secondary_ekf_type) {
case EKFType::NONE:
// DCM is secondary
quat.from_rotation_matrix(AP_AHRS_DCM::get_rotation_body_to_ned());
return true;
#if HAL_NAVEKF2_AVAILABLE
case EKFType::TWO:
// EKF2 is secondary
EKF2.getQuaternion(-1, quat);
return _ekf2_started;
#endif
#if HAL_NAVEKF3_AVAILABLE
case EKFType::THREE:
// EKF3 is secondary
EKF3.getQuaternion(-1, quat);
return _ekf3_started;
#endif
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
case EKFType::SITL:
// SITL is secondary (should never happen)
return false;
#endif
#if HAL_EXTERNAL_AHRS_ENABLED
case EKFType::EXTERNAL:
// External is secondary
return AP::externalAHRS().get_quaternion(quat);
#endif
// DCM is secondary
quat.from_rotation_matrix(AP_AHRS_DCM::get_rotation_body_to_ned());
return true;
}
// since there is no default case above, this is unreachable
return false;
}
@ -831,36 +840,45 @@ bool AP_AHRS_NavEKF::get_secondary_quaternion(Quaternion &quat) const
// return secondary position solution if available
bool AP_AHRS_NavEKF::get_secondary_position(struct Location &loc) const
{
switch (active_EKF_type()) {
case EKFType::NONE:
// EKF is secondary
#if HAL_NAVEKF2_AVAILABLE
EKF2.getLLH(loc);
return _ekf2_started;
#elif HAL_EXTERNAL_AHRS_ENABLED
return AP::externalAHRS().get_location(loc);
#else
EKFType secondary_ekf_type;
if (!get_secondary_EKF_type(secondary_ekf_type)) {
return false;
#endif
}
switch (secondary_ekf_type) {
case EKFType::NONE:
// return DCM position
AP_AHRS_DCM::get_position(loc);
return true;
#if HAL_NAVEKF2_AVAILABLE
case EKFType::TWO:
// EKF2 is secondary
EKF2.getLLH(loc);
return _ekf2_started;
#endif
#if HAL_NAVEKF3_AVAILABLE
case EKFType::THREE:
// EKF3 is secondary
EKF3.getLLH(loc);
return _ekf3_started;
#endif
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
case EKFType::SITL:
// SITL is secondary (should never happen)
return false;
#endif
#if HAL_EXTERNAL_AHRS_ENABLED
case EKFType::EXTERNAL:
// External is secondary
return AP::externalAHRS().get_location(loc);
#endif
// return DCM position
AP_AHRS_DCM::get_position(loc);
return true;
}
// since there is no default case above, this is unreachable
return false;
}
@ -1542,6 +1560,54 @@ AP_AHRS_NavEKF::EKFType AP_AHRS_NavEKF::active_EKF_type(void) const
return ret;
}
// get secondary EKF type. returns false if no secondary (i.e. only using DCM)
bool AP_AHRS_NavEKF::get_secondary_EKF_type(EKFType &secondary_ekf_type) const
{
switch (active_EKF_type()) {
case EKFType::NONE:
// EKF2, EKF3 or External is secondary
#if HAL_NAVEKF3_AVAILABLE
if ((EKFType)_ekf_type.get() == EKFType::THREE) {
secondary_ekf_type = EKFType::THREE;
return true;
}
#endif
#if HAL_NAVEKF2_AVAILABLE
if ((EKFType)_ekf_type.get() == EKFType::TWO) {
secondary_ekf_type = EKFType::TWO;
return true;
}
#endif
#if HAL_EXTERNAL_AHRS_ENABLED
if ((EKFType)_ekf_type.get() == EKFType::EXTERNAL) {
secondary_ekf_type = EKFType::EXTERNAL;
return true;
}
#endif
return false;
#if HAL_NAVEKF2_AVAILABLE
case EKFType::TWO:
#endif
#if HAL_NAVEKF3_AVAILABLE
case EKFType::THREE:
#endif
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
case EKFType::SITL:
#endif
#if HAL_EXTERNAL_AHRS_ENABLED
case EKFType::EXTERNAL:
#endif
// DCM is secondary
secondary_ekf_type = EKFType::NONE;
return true;
}
// since there is no default case above, this is unreachable
return false;
}
/*
check if the AHRS subsystem is healthy
*/

4
libraries/AP_AHRS/AP_AHRS_NavEKF.h
View File

@ -348,6 +348,10 @@ private:
};
EKFType active_EKF_type(void) const;
// if successful returns true and sets secondary_ekf_type to None (for DCM), EKF3 or EKF3
// returns false if no secondary (i.e. only using DCM)
bool get_secondary_EKF_type(EKFType &secondary_ekf_type) const;
bool always_use_EKF() const {
return _ekf_flags & FLAG_ALWAYS_USE_EKF;
}