Updating orientation_filter based on RD-PQ testing

 - Updating orientation filter code based on testing on RD-PQ target
 - Updating orientation_filter class based on configuration to be
   received from user inputs.
 - Updating orientation_sensor test class and code.

Change-Id: Idf41e9fb6ff8ad9d76a40bf0a4e7f17c70d4a7fa

diff --git a/src/sensor_fusion/orientation_filter.cpp b/src/sensor_fusion/orientation_filter.cpp
index 6acc52f..1c09379 100644 (file)
--- a/src/sensor_fusion/orientation_filter.cpp
+++ b/src/sensor_fusion/orientation_filter.cpp
@@ -42,18 +42,6 @@
 #define QWB_CONST      ((2 * (ZIGMA_W * ZIGMA_W)) / TAU_W)
 #define F_CONST                (-1 / TAU_W)
 
-#define BIAS_AX                0.098586
-#define BIAS_AY                0.18385
-#define BIAS_AZ                (10.084 - GRAVITY)
-
-#define BIAS_GX                -5.3539
-#define BIAS_GY                0.24325
-#define BIAS_GZ                2.3391
-
-#define DRIVING_SYSTEM_PHASE_COMPENSATION      -1
-
-#define SCALE_GYRO             575
-
 #define ENABLE_LPF             false
 
 #define M3X3R  3
@@ -95,6 +83,11 @@ orientation_filter<TYPE>::orientation_filter()
        m_state_new = vec1x6;
        m_state_old = vec1x6;
        m_state_error = vec1x6;
+
+       m_pitch_phase_compensation = 1;
+       m_roll_phase_compensation = 1;
+       m_yaw_phase_compensation = 1;
+       m_magnetic_alignment_factor = 1;
 }
 
 template <typename TYPE>
@@ -110,16 +103,8 @@ inline void orientation_filter<TYPE>::filter_sensor_data(const sensor_data<TYPE>
        const TYPE iir_b[] = {0.98, 0};
        const TYPE iir_a[] = {1.0000000, 0.02};
 
-       TYPE a_bias[] = {BIAS_AX, BIAS_AY, BIAS_AZ};
-       TYPE g_bias[] = {BIAS_GX, BIAS_GY, BIAS_GZ};
-
        vect<TYPE> acc_data(V1x3S);
        vect<TYPE> gyr_data(V1x3S);
-       vect<TYPE> acc_bias(V1x3S, a_bias);
-       vect<TYPE> gyr_bias(V1x3S, g_bias);
-
-       acc_data = accel.m_data - acc_bias;
-       gyr_data = (gyro.m_data - gyr_bias) / (TYPE) SCALE_GYRO;
 
        m_filt_accel[0] = m_filt_accel[1];
        m_filt_gyro[0] = m_filt_gyro[1];
@@ -127,14 +112,14 @@ inline void orientation_filter<TYPE>::filter_sensor_data(const sensor_data<TYPE>
 
        if (ENABLE_LPF)
        {
-               m_filt_accel[1].m_data = acc_data * iir_b[0] - m_filt_accel[0].m_data * iir_a[1];
-               m_filt_gyro[1].m_data = gyr_data * iir_b[0] - m_filt_gyro[0].m_data * iir_a[1];
+               m_filt_accel[1].m_data = accel.m_data * iir_b[0] - m_filt_accel[0].m_data * iir_a[1];
+               m_filt_gyro[1].m_data = gyro.m_data * iir_b[0] - m_filt_gyro[0].m_data * iir_a[1];
                m_filt_magnetic[1].m_data = magnetic.m_data * iir_b[0] - m_filt_magnetic[0].m_data * iir_a[1];
        }
        else
        {
-               m_filt_accel[1].m_data = acc_data;
-               m_filt_gyro[1].m_data = gyr_data;
+               m_filt_accel[1].m_data = accel.m_data;
+               m_filt_gyro[1].m_data = gyro.m_data;
                m_filt_magnetic[1].m_data = magnetic.m_data;
        }
 
@@ -142,9 +127,6 @@ inline void orientation_filter<TYPE>::filter_sensor_data(const sensor_data<TYPE>
        m_filt_gyro[1].m_time_stamp = accel.m_time_stamp;
        m_filt_magnetic[1].m_time_stamp = accel.m_time_stamp;
 
-       normalize(m_filt_accel[1]);
-       normalize(m_filt_magnetic[1]);
-
        m_filt_gyro[1].m_data = m_filt_gyro[1].m_data - m_bias_correction;
 }
 
@@ -152,7 +134,7 @@ template <typename TYPE>
 inline void orientation_filter<TYPE>::orientation_triad_algorithm()
 {
        TYPE arr_acc_e[V1x3S] = {0.0, 0.0, 1.0};
-       TYPE arr_mag_e[V1x3S] = {0.0, -1.0, 0.0};
+       TYPE arr_mag_e[V1x3S] = {0.0, m_magnetic_alignment_factor, 0.0};
 
        vect<TYPE> acc_e(V1x3S, arr_acc_e);
        vect<TYPE> mag_e(V1x3S, arr_mag_e);
@@ -261,7 +243,9 @@ inline void orientation_filter<TYPE>::time_update()
 
        orientation = quat2euler(m_quat_driv);
 
-       m_orientation = orientation.m_ang * (TYPE) DRIVING_SYSTEM_PHASE_COMPENSATION;
+       m_orientation.m_ang.m_vec[0] = orientation.m_ang.m_vec[0] * m_roll_phase_compensation;
+       m_orientation.m_ang.m_vec[1] = orientation.m_ang.m_vec[1] * m_pitch_phase_compensation;
+       m_orientation.m_ang.m_vec[2] = orientation.m_ang.m_vec[2] * m_yaw_phase_compensation;
 
        m_rot_matrix = quat2rot_mat(m_quat_driv);
 

diff --git a/src/sensor_fusion/orientation_filter.h b/src/sensor_fusion/orientation_filter.h
index de07af9..3f97569 100644 (file)
--- a/src/sensor_fusion/orientation_filter.h
+++ b/src/sensor_fusion/orientation_filter.h
@@ -53,6 +53,11 @@ public:
        rotation_matrix<TYPE> m_rot_matrix;
        euler_angles<TYPE> m_orientation;
 
+       int m_pitch_phase_compensation;
+       int m_roll_phase_compensation;
+       int m_yaw_phase_compensation;
+       int m_magnetic_alignment_factor;
+
        orientation_filter();
        ~orientation_filter();
 

diff --git a/src/sensor_fusion/standalone/orientation_sensor.cpp b/src/sensor_fusion/standalone/orientation_sensor.cpp
index b86919d..26412c2 100644 (file)
--- a/src/sensor_fusion/standalone/orientation_sensor.cpp
+++ b/src/sensor_fusion/standalone/orientation_sensor.cpp
@@ -19,16 +19,64 @@
 
 #ifdef _ORIENTATION_SENSOR_H
 
-euler_angles<float> orientation_sensor::get_orientation(const sensor_data<float> accel,
-               const sensor_data<float> gyro, const sensor_data<float> magnetic)
+float bias_accel[] = {0.098586, 0.18385, (10.084 - GRAVITY)};
+float bias_gyro[] = {-5.3539, 0.24325, 2.3391};
+float bias_magnetic[] = {0, 0, 0};
+int sign_accel[] = {+1, +1, +1};
+int sign_gyro[] = {+1, +1, +1};
+int sign_magnetic[] = {+1, +1, +1};
+float scale_accel = 1;
+float scale_gyro = 575;
+float scale_magnetic = 1;
+
+int pitch_phase_compensation = -1;
+int roll_phase_compensation = -1;
+int yaw_phase_compensation = -1;
+int magnetic_alignment_factor = -1;
+
+void pre_process_data(sensor_data<float> &data_out, sensor_data<float> &data_in, float *bias, int *sign, float scale)
+{
+       data_out.m_data.m_vec[0] = sign[0] * (data_in.m_data.m_vec[0] - bias[0]) / scale;
+       data_out.m_data.m_vec[1] = sign[1] * (data_in.m_data.m_vec[1] - bias[1]) / scale;
+       data_out.m_data.m_vec[2] = sign[2] * (data_in.m_data.m_vec[2] - bias[2]) / scale;
+
+       data_out.m_time_stamp = data_in.m_time_stamp;
+}
+
+euler_angles<float> orientation_sensor::get_orientation(sensor_data<float> accel_data,
+               sensor_data<float> gyro_data, sensor_data<float> magnetic_data)
 {
-       return orien_filter.get_orientation(accel, gyro, magnetic);
+
+       pre_process_data(accel_data, accel_data, bias_accel, sign_accel, scale_accel);
+       normalize(accel_data);
+       pre_process_data(gyro_data, gyro_data, bias_gyro, sign_gyro, scale_gyro);
+       pre_process_data(magnetic_data, magnetic_data, bias_magnetic, sign_magnetic, scale_magnetic);
+       normalize(magnetic_data);
+
+       orien_filter.m_pitch_phase_compensation = pitch_phase_compensation;
+       orien_filter.m_roll_phase_compensation = roll_phase_compensation;
+       orien_filter.m_yaw_phase_compensation = yaw_phase_compensation;
+       orien_filter.m_magnetic_alignment_factor = magnetic_alignment_factor;
+
+       return orien_filter.get_orientation(accel_data, gyro_data, magnetic_data);
 }
 
-rotation_matrix<float> orientation_sensor::get_rotation_matrix(const sensor_data<float> accel,
-               const sensor_data<float> gyro, const sensor_data<float> magnetic)
+rotation_matrix<float> orientation_sensor::get_rotation_matrix(sensor_data<float> accel_data,
+               sensor_data<float> gyro_data, sensor_data<float> magnetic_data)
 {
-       return orien_filter.get_rotation_matrix(accel, gyro, magnetic);
+
+       pre_process_data(accel_data, accel_data, bias_accel, sign_accel, scale_accel);
+       normalize(accel_data);
+       pre_process_data(gyro_data, gyro_data, bias_gyro, sign_gyro, scale_gyro);
+       pre_process_data(magnetic_data, magnetic_data, bias_magnetic, sign_magnetic, scale_magnetic);
+       normalize(magnetic_data);
+
+       orien_filter.m_pitch_phase_compensation = pitch_phase_compensation;
+       orien_filter.m_roll_phase_compensation = roll_phase_compensation;
+       orien_filter.m_yaw_phase_compensation = yaw_phase_compensation;
+       orien_filter.m_magnetic_alignment_factor = magnetic_alignment_factor;
+
+       return orien_filter.get_rotation_matrix(accel_data, gyro_data, magnetic_data);
 }
 
 #endif

diff --git a/src/sensor_fusion/standalone/orientation_sensor.h b/src/sensor_fusion/standalone/orientation_sensor.h
index 487d67a..e670309 100644 (file)
--- a/src/sensor_fusion/standalone/orientation_sensor.h
+++ b/src/sensor_fusion/standalone/orientation_sensor.h
@@ -27,10 +27,10 @@ class orientation_sensor
 public:
        orientation_filter<float> orien_filter;
 
-       euler_angles<float> get_orientation(const sensor_data<float> accel,
-                       const sensor_data<float> gyro, const sensor_data<float> magnetic);
-       rotation_matrix<float> get_rotation_matrix(const sensor_data<float> accel,
-                       const sensor_data<float> gyro, const sensor_data<float> magnetic);
+       euler_angles<float> get_orientation(sensor_data<float> accel,
+                       sensor_data<float> gyro, sensor_data<float> magnetic);
+       rotation_matrix<float> get_rotation_matrix(sensor_data<float> accel,
+                       sensor_data<float> gyro, sensor_data<float> magnetic);
 };
 
 #include "orientation_sensor.cpp"