dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
EKF/ekf.h
View File

@ -124,6 +124,9 @@ public:
// get the accerometer bias in m/s/s // get the accerometer bias in m/s/s
void get_accel_bias(float bias[3]); 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 // get GPS check status
void get_gps_check_status(uint16_t *_gps_check_fail_status); void get_gps_check_status(uint16_t *_gps_check_fail_status);

10
EKF/ekf_helper.cpp
View File

@ -681,6 +681,16 @@ void Ekf::get_accel_bias(float bias[3])
memcpy(bias, temp, 3 * sizeof(float)); 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 // get the diagonal elements of the covariance matrix
void Ekf::get_covariances(float *covariances) void Ekf::get_covariances(float *covariances)
{ {

1
EKF/estimator_interface.h
View File

@ -201,6 +201,7 @@ public:
} }
virtual void get_accel_bias(float bias[3]) = 0; virtual void get_accel_bias(float bias[3]) = 0;
virtual void get_gyro_bias(float bias[3]) = 0;
// get EKF mode status // get EKF mode status
void get_control_mode(uint16_t *val) void get_control_mode(uint16_t *val)