Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-25 10:08:28 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_AHRS: Add accessor function for vibration affected status

Browse Source 
AP_AHRS: fix rebase build error
This commit is contained in:
Paul Riseborough 2021-07-16 16:23:47 +10:00 committed by Randy Mackay
parent 662bda1bbd
commit 6907fa8e88
2 changed files with 26 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

23
libraries/AP_AHRS/AP_AHRS_NavEKF.cpp
View File

@ -2487,6 +2487,29 @@ bool AP_AHRS_NavEKF::get_innovations(Vector3f &velInnov, Vector3f &posInnov, Vec
return false; return false;
} }
// returns true when the state estimates are significantly degraded by vibration
bool AP_AHRS::is_vibration_affected() const
{
switch (ekf_type()) {
#if HAL_NAVEKF3_AVAILABLE
case EKFType::THREE:
return EKF3.isVibrationAffected(-1);
#endif
case EKFType::NONE:
#if HAL_NAVEKF2_AVAILABLE
case EKFType::TWO:
#endif
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
case EKFType::SITL:
#endif
#if HAL_EXTERNAL_AHRS_ENABLED
case EKFType::EXTERNAL:
#endif
return false;
}
return false;
}
// get_variances - provides the innovations normalised using the innovation variance where a value of 0 // get_variances - provides the innovations normalised using the innovation variance where a value of 0
// indicates prefect consistency between the measurement and the EKF solution and a value of of 1 is the maximum // indicates prefect consistency between the measurement and the EKF solution and a value of of 1 is the maximum
// inconsistency that will be accpeted by the filter // inconsistency that will be accpeted by the filter

3
libraries/AP_AHRS/AP_AHRS_NavEKF.h
View File

@ -259,6 +259,9 @@ public:
// An out of range instance (eg -1) returns data for the primary instance // An out of range instance (eg -1) returns data for the primary instance
bool get_innovations(Vector3f &velInnov, Vector3f &posInnov, Vector3f &magInnov, float &tasInnov, float &yawInnov) const override; bool get_innovations(Vector3f &velInnov, Vector3f &posInnov, Vector3f &magInnov, float &tasInnov, float &yawInnov) const override;
// returns true when the state estimates are significantly degraded by vibration
bool is_vibration_affected() const;
// get_variances - provides the innovations normalised using the innovation variance where a value of 0 // get_variances - provides the innovations normalised using the innovation variance where a value of 0
// indicates perfect consistency between the measurement and the EKF solution and a value of of 1 is the maximum // indicates perfect consistency between the measurement and the EKF solution and a value of of 1 is the maximum
// inconsistency that will be accepted by the filter // inconsistency that will be accepted by the filter