From 8c1123c590082fcfb3d77bcbc1438936ba1c8a7d Mon Sep 17 00:00:00 2001 From: John Yao Date: Thu, 29 Oct 2015 17:22:58 -0400 Subject: [PATCH] zeroing out magnetometer readings before they enter the attitude filter --- .../attitude_estimator_so3/attitude_estimator_so3_main.cpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/modules/attitude_estimator_so3/attitude_estimator_so3_main.cpp b/src/modules/attitude_estimator_so3/attitude_estimator_so3_main.cpp index cf4730c9a10c..23f0d4473b17 100755 --- a/src/modules/attitude_estimator_so3/attitude_estimator_so3_main.cpp +++ b/src/modules/attitude_estimator_so3/attitude_estimator_so3_main.cpp @@ -568,6 +568,9 @@ int attitude_estimator_so3_thread_main(int argc, char *argv[]) continue; } + mag[0] = 0.0; + mag[1] = 0.0; + mag[2] = 0.0; // gyro, acc is rotated by sensors module by SENS_BOARD_ROT param NonlinearSO3AHRSupdate(gyro[0], gyro[1], gyro[2], acc[0], acc[1], acc[2],