diff --git a/libraries/AP_NavEKF/AP_SmallEKF.cpp b/libraries/AP_NavEKF/AP_SmallEKF.cpp index 90abcc2387..57adc40a98 100644 --- a/libraries/AP_NavEKF/AP_SmallEKF.cpp +++ b/libraries/AP_NavEKF/AP_SmallEKF.cpp @@ -894,5 +894,16 @@ void SmallEKF::getDebug(float &tilt, Vector3f &velocity, Vector3f &euler, Vector gyroBias = state.delAngBias / dtIMU; } +// get gyro bias data +void SmallEKF::getGyroBias(Vector3f &gyroBias) const +{ + gyroBias = state.delAngBias / dtIMU; +} + +// get quaternion data +void SmallEKF::getQuat(Quaternion &quat) const +{ + quat = state.quat; +} #endif // HAL_CPU_CLASS diff --git a/libraries/AP_NavEKF/AP_SmallEKF.h b/libraries/AP_NavEKF/AP_SmallEKF.h index 5beb8b26c8..73a6e9932c 100644 --- a/libraries/AP_NavEKF/AP_SmallEKF.h +++ b/libraries/AP_NavEKF/AP_SmallEKF.h @@ -86,6 +86,12 @@ public: // get some debug information void getDebug(float &tilt, Vector3f &velocity, Vector3f &euler, Vector3f &gyroBias) const; + // get gyro bias data + void getGyroBias(Vector3f &gyroBias) const; + + // get quaternion data + void getQuat(Quaternion &quat) const; + static const struct AP_Param::GroupInfo var_info[]; private: