diff --git a/libraries/AP_AHRS/AP_AHRS.h b/libraries/AP_AHRS/AP_AHRS.h index b5127d444b..db7b19c22d 100644 --- a/libraries/AP_AHRS/AP_AHRS.h +++ b/libraries/AP_AHRS/AP_AHRS.h @@ -264,6 +264,12 @@ public: AP_Float _kp; AP_Float gps_gain; + // return secondary attitude solution if available, as eulers in radians + virtual bool get_secondary_attitude(Vector3f &eulers) { return false; } + + // return secondary position solution if available + virtual bool get_secondary_position(struct Location &loc) { return false; } + protected: // settable parameters AP_Float beta; diff --git a/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp b/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp index c513054f0b..115105841a 100644 --- a/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp +++ b/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp @@ -47,6 +47,10 @@ const Vector3f &AP_AHRS_NavEKF::get_gyro_drift(void) const void AP_AHRS_NavEKF::update(void) { AP_AHRS_DCM::update(); + + // keep DCM attitude available for get_secondary_attitude() + _dcm_attitude(roll, pitch, yaw); + if (!ekf_started) { if (start_time_ms == 0) { start_time_ms = hal.scheduler->millis(); @@ -139,4 +143,39 @@ bool AP_AHRS_NavEKF::use_compass(void) return AP_AHRS_DCM::use_compass(); } + +// return secondary attitude solution if available, as eulers in radians +bool AP_AHRS_NavEKF::get_secondary_attitude(Vector3f &eulers) +{ + if (ekf_started && _ekf_use) { + // return DCM attitude + eulers = _dcm_attitude; + return true; + } + if (ekf_started) { + // EKF is secondary + EKF.getEulerAngles(eulers); + return true; + } + // no secondary available + return false; +} + +// return secondary position solution if available +bool AP_AHRS_NavEKF::get_secondary_position(struct Location &loc) +{ + if (ekf_started && _ekf_use) { + // return DCM position + AP_AHRS_DCM::get_position(loc); + return true; + } + if (ekf_started) { + // EKF is secondary + EKF.getLLH(loc); + return true; + } + // no secondary available + return false; +} + #endif // AP_AHRS_NAVEKF_AVAILABLE diff --git a/libraries/AP_AHRS/AP_AHRS_NavEKF.h b/libraries/AP_AHRS/AP_AHRS_NavEKF.h index bbc821083f..e673d1799b 100644 --- a/libraries/AP_AHRS/AP_AHRS_NavEKF.h +++ b/libraries/AP_AHRS/AP_AHRS_NavEKF.h @@ -73,11 +73,18 @@ public: const NavEKF &get_NavEKF(void) const { return EKF; } + // return secondary attitude solution if available, as eulers in radians + bool get_secondary_attitude(Vector3f &eulers); + + // return secondary position solution if available + bool get_secondary_position(struct Location &loc); + private: NavEKF EKF; AP_Baro &_baro; bool ekf_started; Matrix3f _dcm_matrix; + Vector3f _dcm_attitude; const uint16_t startup_delay_ms; uint32_t start_time_ms; };