From 7f1632d65ba982744c09147c71e56fe88840aa46 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Mon, 9 May 2016 09:33:37 +1000 Subject: [PATCH] ekf2: correct control state message for 3D acc bias --- src/modules/ekf2/ekf2_main.cpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/src/modules/ekf2/ekf2_main.cpp b/src/modules/ekf2/ekf2_main.cpp index dd30bb54e44..e026f1baeb2 100644 --- a/src/modules/ekf2/ekf2_main.cpp +++ b/src/modules/ekf2/ekf2_main.cpp @@ -560,11 +560,11 @@ void Ekf2::task_main() // Acceleration data matrix::Vector 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;