AP_AHRS: add using_extnav_for_yaw

This commit is contained in:
Randy Mackay 2021-08-18 16:45:01 +09:00 committed by Andrew Tridgell
parent 63e579d738
commit 1c08866342
3 changed files with 32 additions and 1 deletions

View File

@ -3047,6 +3047,31 @@ bool AP_AHRS::using_noncompass_for_yaw(void) const
return false;
}
// check if external nav is providing yaw
bool AP_AHRS::using_extnav_for_yaw(void) const
{
switch (active_EKF_type()) {
#if HAL_NAVEKF2_AVAILABLE
case EKFType::TWO:
return EKF2.isExtNavUsedForYaw();
#endif
case EKFType::NONE:
#if HAL_NAVEKF3_AVAILABLE
case EKFType::THREE:
return EKF3.using_extnav_for_yaw();
#endif
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
case EKFType::SIM:
#endif
#if HAL_EXTERNAL_AHRS_ENABLED
case EKFType::EXTERNAL:
#endif
return false;
}
// since there is no default case above, this is unreachable
return false;
}
// set and save the alt noise parameter value
void AP_AHRS::set_alt_measurement_noise(float noise)
{

View File

@ -329,6 +329,9 @@ public:
// check if non-compass sensor is providing yaw. Allows compass pre-arm checks to be bypassed
bool using_noncompass_for_yaw(void) const override;
// check if external nav is providing yaw
bool using_extnav_for_yaw(void) const override;
// set and save the ALT_M_NSE parameter value
void set_alt_measurement_noise(float noise) override;

View File

@ -92,7 +92,10 @@ public:
// check if non-compass sensor is providing yaw. Allows compass pre-arm checks to be bypassed
virtual bool using_noncompass_for_yaw(void) const { return false; }
// check if external nav is providing yaw
virtual bool using_extnav_for_yaw(void) const { return false; }
// request EKF yaw reset to try and avoid the need for an EKF lane switch or failsafe
virtual void request_yaw_reset(void) {}