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 <taemin.yeom@samsung.com>

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 5df2f08..fb5e6c0 100644 (file)
--- 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 0ff3ea5..24b2077 100644 (file)
--- 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;