From f56f5842337f3d2d9bf5a429e3d02c5b5b07b9e1 Mon Sep 17 00:00:00 2001 From: dgrat Date: Fri, 3 Jul 2015 19:56:44 +0200 Subject: [PATCH] Compass: Improved field rounding in learning Signed-off-by: Daniel Frenzel --- libraries/AP_Compass/Compass_learn.cpp | 11 +++++++---- 1 file changed, 7 insertions(+), 4 deletions(-) diff --git a/libraries/AP_Compass/Compass_learn.cpp b/libraries/AP_Compass/Compass_learn.cpp index 137a7b15af..1cb4e77b7a 100644 --- a/libraries/AP_Compass/Compass_learn.cpp +++ b/libraries/AP_Compass/Compass_learn.cpp @@ -1,5 +1,6 @@ /// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- #include "Compass.h" +#include // don't allow any axis of the offset to go above 2000 #define COMPASS_OFS_LIMIT 2000 @@ -47,7 +48,9 @@ Compass::learn_offsets(void) for (uint8_t i=0; i<_mag_history_size; i++) { // fill the history buffer with the current mag vector, // with the offset removed - _state[k].mag_history[i] = Vector3i((field.x+0.5f) - ofs.x, (field.y+0.5f) - ofs.y, (field.z+0.5f) - ofs.z); + _state[k].mag_history[i] = Vector3i(roundf(field.x) - ofs.x, + roundf(field.y) - ofs.y, + roundf(field.z) - ofs.z); } _state[k].mag_history_index = 0; } @@ -92,9 +95,9 @@ Compass::learn_offsets(void) } // put the vector in the history - _state[k].mag_history[_state[k].mag_history_index] = Vector3i((field.x+0.5f) - ofs.x, - (field.y+0.5f) - ofs.y, - (field.z+0.5f) - ofs.z); + _state[k].mag_history[_state[k].mag_history_index] = Vector3i(roundf(field.x) - ofs.x, + roundf(field.y) - ofs.y, + roundf(field.z) - ofs.z); _state[k].mag_history_index = (_state[k].mag_history_index + 1) % _mag_history_size; // equation 6 of Bills paper