diff --git a/EKF/ekf.h b/EKF/ekf.h index cad5522611..84181828ef 100644 --- a/EKF/ekf.h +++ b/EKF/ekf.h @@ -124,6 +124,9 @@ public: // get the accerometer bias in m/s/s void get_accel_bias(float bias[3]); + // get the gyroscope bias in rad/s + void get_gyro_bias(float bias[3]); + // get GPS check status void get_gps_check_status(uint16_t *_gps_check_fail_status); diff --git a/EKF/ekf_helper.cpp b/EKF/ekf_helper.cpp index 100eaebf35..4336ee2ab4 100644 --- a/EKF/ekf_helper.cpp +++ b/EKF/ekf_helper.cpp @@ -681,6 +681,16 @@ void Ekf::get_accel_bias(float bias[3]) memcpy(bias, temp, 3 * sizeof(float)); } +// get the gyroscope bias in rad/s +void Ekf::get_gyro_bias(float bias[3]) +{ + float temp[3]; + temp[0] = _state.gyro_bias(0) /_dt_ekf_avg; + temp[1] = _state.gyro_bias(1) /_dt_ekf_avg; + temp[2] = _state.gyro_bias(2) /_dt_ekf_avg; + memcpy(bias, temp, 3 * sizeof(float)); +} + // get the diagonal elements of the covariance matrix void Ekf::get_covariances(float *covariances) { diff --git a/EKF/estimator_interface.h b/EKF/estimator_interface.h index 4fc0a5b736..44880a9213 100644 --- a/EKF/estimator_interface.h +++ b/EKF/estimator_interface.h @@ -201,6 +201,7 @@ public: } virtual void get_accel_bias(float bias[3]) = 0; + virtual void get_gyro_bias(float bias[3]) = 0; // get EKF mode status void get_control_mode(uint16_t *val)