From b0b78e974bbe90837a55930b0d879a8299ded09b Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Wed, 1 Apr 2020 10:08:54 +0900 Subject: [PATCH] AP_AHRS: add get_quaternion --- libraries/AP_AHRS/AP_AHRS.h | 2 ++ libraries/AP_AHRS/AP_AHRS_DCM.cpp | 7 +++++++ libraries/AP_AHRS/AP_AHRS_DCM.h | 3 +++ libraries/AP_AHRS/AP_AHRS_NavEKF.cpp | 30 ++++++++++++++++++++++++++++ libraries/AP_AHRS/AP_AHRS_NavEKF.h | 3 +++ 5 files changed, 45 insertions(+) diff --git a/libraries/AP_AHRS/AP_AHRS.h b/libraries/AP_AHRS/AP_AHRS.h index c8e69e0459..89f98d2ed2 100644 --- a/libraries/AP_AHRS/AP_AHRS.h +++ b/libraries/AP_AHRS/AP_AHRS.h @@ -392,6 +392,8 @@ public: return _sin_yaw; } + // return the quaternion defining the rotation from NED to XYZ (body) axes + virtual bool get_quaternion(Quaternion &quat) const WARN_IF_UNUSED = 0; // return secondary attitude solution if available, as eulers in radians virtual bool get_secondary_attitude(Vector3f &eulers) const WARN_IF_UNUSED { diff --git a/libraries/AP_AHRS/AP_AHRS_DCM.cpp b/libraries/AP_AHRS/AP_AHRS_DCM.cpp index 7d2c9e1150..fe94e93fb0 100644 --- a/libraries/AP_AHRS/AP_AHRS_DCM.cpp +++ b/libraries/AP_AHRS/AP_AHRS_DCM.cpp @@ -473,6 +473,13 @@ bool AP_AHRS_DCM::use_compass(void) return true; } +// return the quaternion defining the rotation from NED to XYZ (body) axes +bool AP_AHRS_DCM::get_quaternion(Quaternion &quat) const +{ + quat.from_rotation_matrix(_dcm_matrix); + return true; +} + // yaw drift correction using the compass or GPS // this function prodoces the _omega_yaw_P vector, and also // contributes to the _omega_I.z long term yaw drift estimate diff --git a/libraries/AP_AHRS/AP_AHRS_DCM.h b/libraries/AP_AHRS/AP_AHRS_DCM.h index 2d995c4c4a..bb20df4b71 100644 --- a/libraries/AP_AHRS/AP_AHRS_DCM.h +++ b/libraries/AP_AHRS/AP_AHRS_DCM.h @@ -96,6 +96,9 @@ public: bool use_compass() override; + // return the quaternion defining the rotation from NED to XYZ (body) axes + bool get_quaternion(Quaternion &quat) const override WARN_IF_UNUSED; + bool set_home(const Location &loc) override WARN_IF_UNUSED; void estimate_wind(void); diff --git a/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp b/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp index a67a3bcd4f..0a7c2f7006 100644 --- a/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp +++ b/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp @@ -545,6 +545,36 @@ bool AP_AHRS_NavEKF::use_compass(void) return AP_AHRS_DCM::use_compass(); } +// return the quaternion defining the rotation from NED to XYZ (body) axes +bool AP_AHRS_NavEKF::get_quaternion(Quaternion &quat) const +{ + switch (active_EKF_type()) { + case EKFType::NONE: + return AP_AHRS_DCM::get_quaternion(quat); +#if HAL_NAVEKF2_AVAILABLE + case EKFType::TWO: + EKF2.getQuaternion(-1, quat); + return _ekf2_started; +#endif +#if HAL_NAVEKF3_AVAILABLE + case EKFType::THREE: + EKF3.getQuaternion(-1, quat); + return _ekf3_started; +#endif +#if CONFIG_HAL_BOARD == HAL_BOARD_SITL + case EKFType::SITL: + if (_sitl) { + const struct SITL::sitl_fdm &fdm = _sitl->state; + quat = fdm.quaternion; + return true; + } else { + return false; + } +#endif + } + // since there is no default case above, this is unreachable + return false; +} // return secondary attitude solution if available, as eulers in radians bool AP_AHRS_NavEKF::get_secondary_attitude(Vector3f &eulers) const diff --git a/libraries/AP_AHRS/AP_AHRS_NavEKF.h b/libraries/AP_AHRS/AP_AHRS_NavEKF.h index 7101f1ca8c..4682e1d02f 100644 --- a/libraries/AP_AHRS/AP_AHRS_NavEKF.h +++ b/libraries/AP_AHRS/AP_AHRS_NavEKF.h @@ -115,6 +115,9 @@ public: } #endif + // return the quaternion defining the rotation from NED to XYZ (body) axes + bool get_quaternion(Quaternion &quat) const override WARN_IF_UNUSED; + // return secondary attitude solution if available, as eulers in radians bool get_secondary_attitude(Vector3f &eulers) const override;