From 9320b5fa8bb0d9db650f23f6b29a20d91aa1e51f Mon Sep 17 00:00:00 2001 From: "taemin.yeom" Date: Wed, 13 Oct 2021 16:57:43 +0900 Subject: [PATCH] Improve correctness in orientation calculation -fix gravity constant with HAL -change reference not to be used in function args to prevent changing values by other threads Change-Id: I39c14d37490803c9712958d6f2dc464f026b07c8 Signed-off-by: taemin.yeom --- .../rotation_vector/fusion_utils/orientation_filter.cpp | 8 ++++---- .../rotation_vector/fusion_utils/orientation_filter.h | 6 +++--- 2 files changed, 7 insertions(+), 7 deletions(-) diff --git a/src/fusion-sensor/rotation_vector/fusion_utils/orientation_filter.cpp b/src/fusion-sensor/rotation_vector/fusion_utils/orientation_filter.cpp index 5df2f088..fb5e6c09 100644 --- a/src/fusion-sensor/rotation_vector/fusion_utils/orientation_filter.cpp +++ b/src/fusion-sensor/rotation_vector/fusion_utils/orientation_filter.cpp @@ -68,7 +68,7 @@ static const float SYMMETRY_TOLERANCE = 1e-10f; * ill-conditioning and div by zeros. * Threshhold: 10% of g, in m/s^2 */ -static const float NOMINAL_GRAVITY = 9.81f; +static const float NOMINAL_GRAVITY = 9.80665f; static const float FREE_FALL_THRESHOLD = 0.1f * (NOMINAL_GRAVITY); /* @@ -308,14 +308,14 @@ bool orientation_filter::checkInitComplete(int what, const vec3_t& d, float dT) return false; } -void orientation_filter::handleGyro(const vec3_t& w, float dT) { +void orientation_filter::handleGyro(const vec3_t w, float dT) { if (!checkInitComplete(GYRO, w, dT)) return; predict(w, dT); } -status_t orientation_filter::handleAcc(const vec3_t& a, float dT) { +status_t orientation_filter::handleAcc(const vec3_t a, float dT) { if (!checkInitComplete(ACC, a, dT)) return BAD_VALUE; @@ -348,7 +348,7 @@ status_t orientation_filter::handleAcc(const vec3_t& a, float dT) { return NO_ERROR; } -status_t orientation_filter::handleMag(const vec3_t& m) { +status_t orientation_filter::handleMag(const vec3_t m) { if (!checkInitComplete(MAG, m)) return BAD_VALUE; diff --git a/src/fusion-sensor/rotation_vector/fusion_utils/orientation_filter.h b/src/fusion-sensor/rotation_vector/fusion_utils/orientation_filter.h index 0ff3ea54..24b2077a 100644 --- a/src/fusion-sensor/rotation_vector/fusion_utils/orientation_filter.h +++ b/src/fusion-sensor/rotation_vector/fusion_utils/orientation_filter.h @@ -64,9 +64,9 @@ class orientation_filter { public: orientation_filter(); void init(int mode = FUSION_9AXIS); - void handleGyro(const vec3_t& w, float dT); - status_t handleAcc(const vec3_t& a, float dT); - status_t handleMag(const vec3_t& m); + void handleGyro(const vec3_t w, float dT); + status_t handleAcc(const vec3_t a, float dT); + status_t handleMag(const vec3_t m); vec4_t getAttitude() const; vec3_t getBias() const; mat33_t getRotationMatrix() const; -- 2.34.1