added method to get gyro bias

Signed-off-by: Roman Bapst <roman@px4.io>
This commit is contained in:
Roman Bapst 2016-06-21 13:51:04 +02:00
parent 74a77b45b2
commit bec1a6831e
3 changed files with 14 additions and 0 deletions

View File

@ -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);

View File

@ -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)
{

View File

@ -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)