Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-06-06 06:43:21 +08:00
Code Issues Actions 30 Packages Projects Releases Wiki Activity

ekf2: correct control state message for 3D acc bias

Browse Source
This commit is contained in:
Paul Riseborough
2016-05-09 09:33:37 +10:00
committed by Lorenz Meier
parent 6ef2e4c9b2
commit 7f1632d65b
1 changed files with 5 additions and 5 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/ekf2_main.cpp
+5 -5
View File
@@ -560,11 +560,11 @@ void Ekf2::task_main()
// Acceleration data
matrix::Vector<float, 3> acceleration = {&sensors.accelerometer_m_s2[0]};
float accel_bias = 0.0f;
_ekf.get_accel_bias(&accel_bias);
ctrl_state.x_acc = acceleration(0);
ctrl_state.y_acc = acceleration(1);
ctrl_state.z_acc = acceleration(2) - accel_bias;
float accel_bias[3];
_ekf.get_accel_bias(accel_bias);
ctrl_state.x_acc = acceleration(0) - accel_bias[0];
ctrl_state.y_acc = acceleration(1) - accel_bias[1];
ctrl_state.z_acc = acceleration(2) - accel_bias[2];
// compute lowpass filtered horizontal acceleration
acceleration = R_to_body.transpose() * acceleration;