Updating orientation virtual sensor based on RD-PQ testing

Orientation virtual sensor tested on RD_PQ device based on workaround
on top of the code being submitted here. Workaround involves enabling
accelerometer events for 100ms and then everytime accel event is
received the gyroscope and geomagnetic sensor events are polled and
provided as input to the orientation filter class which computes the
orientation.

Change-Id: Ie30889de3f380422a052d2ba9455586a64ccb640

diff --git a/src/orientation/orientation_sensor.cpp b/src/orientation/orientation_sensor.cpp
index a4743ce..0b051ce 100755 (executable)
--- a/src/orientation/orientation_sensor.cpp
+++ b/src/orientation/orientation_sensor.cpp
@@ -36,18 +36,36 @@
 #define ACCELEROMETER_ENABLED 0x01
 #define GYROSCOPE_ENABLED 0x02
 #define GEOMAGNETIC_ENABLED 0x04
-#define ORIENTATION_ENABLED 0x07
+#define ORIENTATION_ENABLED 7
+
 #define INITIAL_VALUE -1
 #define INITIAL_TIME 0
 
-#define SAMPLING_TIME 0.1
+// Below Defines const variables to be input from sensor config files once code is stabilized
+#define SAMPLING_TIME 100
 #define MS_TO_US 1000
 
-void copy_sensor_data(sensor_data<float> &data_out, sensor_data_t &data_in)
+float bias_accel[] = {0.098586, 0.18385, (10.084 - GRAVITY)};
+float bias_gyro[] = {-5.3539, 0.24325, 2.3391};
+float bias_magnetic[] = {0, -37.6, +37.6};
+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 = 580 * 2;
+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_t &data_in, float *bias, int *sign, float scale)
 {
-       data_out.m_data.m_vec[0] = data_in.values[0];
-       data_out.m_data.m_vec[1] = data_in.values[1];
-       data_out.m_data.m_vec[2] = data_in.values[2];
+       data_out.m_data.m_vec[0] = sign[0] * (data_in.values[0] - bias[0]) / scale;
+       data_out.m_data.m_vec[1] = sign[1] * (data_in.values[1] - bias[1]) / scale;
+       data_out.m_data.m_vec[2] = sign[2] * (data_in.values[2] - bias[2]) / scale;
+
        data_out.m_time_stamp = data_in.timestamp;
 }
 
@@ -61,7 +79,9 @@ orientation_sensor::orientation_sensor()
 {
        m_name = string(SENSOR_NAME);
        register_supported_event(ORIENTATION_EVENT_RAW_DATA_REPORT_ON_TIME);
-       m_timestamp = SAMPLING_TIME * MS_TO_US;
+       m_interval = SAMPLING_TIME * MS_TO_US;
+       m_timestamp = get_timestamp();
+       m_enable_orientation = 0;
 }
 
 orientation_sensor::~orientation_sensor()
@@ -159,31 +179,31 @@ void orientation_sensor::synthesize(const sensor_event_t &event, vector<sensor_e
 
        if (event.event_type == ACCELEROMETER_EVENT_RAW_DATA_REPORT_ON_TIME) {
                diff_time = event.data.timestamp - m_timestamp;
-               if (m_timestamp && (diff_time < SAMPLING_TIME * MS_TO_US * MIN_DELIVERY_DIFF_FACTOR))
+
+               if (m_timestamp && (diff_time < m_interval * MIN_DELIVERY_DIFF_FACTOR))
                        return;
 
-               m_accel_sensor->get_sensor_data(ACCELEROMETER_EVENT_RAW_DATA_REPORT_ON_TIME, accel_data);
-               copy_sensor_data(accel, accel_data);
+               pre_process_data(accel, accel_data, bias_accel, sign_accel, scale_accel);
 
                m_enable_orientation |= ACCELEROMETER_ENABLED;
        }
        else if (event.event_type == GYROSCOPE_EVENT_RAW_DATA_REPORT_ON_TIME) {
                diff_time = event.data.timestamp - m_timestamp;
-               if (m_timestamp && (diff_time < SAMPLING_TIME * MS_TO_US * MIN_DELIVERY_DIFF_FACTOR))
+
+               if (m_timestamp && (diff_time < m_interval * MIN_DELIVERY_DIFF_FACTOR))
                        return;
 
-               m_gyro_sensor->get_sensor_data(GYROSCOPE_EVENT_RAW_DATA_REPORT_ON_TIME, gyro_data);
-               copy_sensor_data(gyro, gyro_data);
+               pre_process_data(gyro, gyro_data, bias_gyro, sign_gyro, scale_gyro);
 
                m_enable_orientation |= GYROSCOPE_ENABLED;
        }
        else if (event.event_type == GEOMAGNETIC_EVENT_RAW_DATA_REPORT_ON_TIME) {
                diff_time = event.data.timestamp - m_timestamp;
-               if (m_timestamp && (diff_time < SAMPLING_TIME * MS_TO_US * MIN_DELIVERY_DIFF_FACTOR))
+
+               if (m_timestamp && (diff_time < m_interval * MIN_DELIVERY_DIFF_FACTOR))
                        return;
 
-               m_magnetic_sensor->get_sensor_data(GEOMAGNETIC_EVENT_RAW_DATA_REPORT_ON_TIME, mag_data);
-               copy_sensor_data(magnetic, mag_data);
+               pre_process_data(magnetic, magnetic_data, bias_magnetic, sign_magnetic, scale_magnetic);
 
                m_enable_orientation |= GEOMAGNETIC_ENABLED;
        }
@@ -192,6 +212,11 @@ void orientation_sensor::synthesize(const sensor_event_t &event, vector<sensor_e
                m_enable_orientation = 0;
                m_timestamp = get_timestamp();
 
+               m_orientation.m_pitch_phase_compensation = pitch_phase_compensation;
+               m_orientation.m_roll_phase_compensation = roll_phase_compensation;
+               m_orientation.m_yaw_phase_compensation = yaw_phase_compensation;
+               m_orientation.m_magnetic_alignment_factor = magnetic_alignment_factor;
+
                euler_orientation = m_orientation.get_orientation(accel, gyro, magnetic);
 
                orientation_event.data.data_accuracy = SENSOR_ACCURACY_GOOD;
@@ -202,6 +227,7 @@ void orientation_sensor::synthesize(const sensor_event_t &event, vector<sensor_e
                orientation_event.data.values[0] = euler_orientation.m_ang.m_vec[0];
                orientation_event.data.values[1] = euler_orientation.m_ang.m_vec[1];
                orientation_event.data.values[2] = euler_orientation.m_ang.m_vec[2];
+
                outs.push_back(orientation_event);
        }
 
@@ -216,7 +242,7 @@ int orientation_sensor::get_sensor_data(const unsigned int event_type, sensor_da
 
        sensor_data_t accel_data;
        sensor_data_t gyro_data;
-       sensor_data_t mag_data;
+       sensor_data_t magnetic_data;
 
        euler_angles<float> euler_orientation;
 
@@ -225,11 +251,16 @@ int orientation_sensor::get_sensor_data(const unsigned int event_type, sensor_da
 
        m_accel_sensor->get_sensor_data(ACCELEROMETER_EVENT_RAW_DATA_REPORT_ON_TIME, accel_data);
        m_gyro_sensor->get_sensor_data(GYROSCOPE_EVENT_RAW_DATA_REPORT_ON_TIME, gyro_data);
-       m_magnetic_sensor->get_sensor_data(GEOMAGNETIC_EVENT_RAW_DATA_REPORT_ON_TIME, mag_data);
+       m_magnetic_sensor->get_sensor_data(GEOMAGNETIC_EVENT_RAW_DATA_REPORT_ON_TIME, magnetic_data);
+
+       pre_process_data(accel, accel_data, bias_accel, sign_accel, scale_accel);
+       pre_process_data(gyro, gyro_data, bias_gyro, sign_gyro, scale_gyro);
+       pre_process_data(magnetic, magnetic_data, bias_magnetic, sign_magnetic, scale_magnetic);
 
-       copy_sensor_data(accel, accel_data);
-       copy_sensor_data(gyro, gyro_data);
-       copy_sensor_data(magnetic, mag_data);
+       m_orientation.m_pitch_phase_compensation = pitch_phase_compensation;
+       m_orientation.m_roll_phase_compensation = roll_phase_compensation;
+       m_orientation.m_yaw_phase_compensation = yaw_phase_compensation;
+       m_orientation.m_magnetic_alignment_factor = magnetic_alignment_factor;
 
        euler_orientation = m_orientation.get_orientation(accel, gyro, magnetic);
 
@@ -240,6 +271,7 @@ int orientation_sensor::get_sensor_data(const unsigned int event_type, sensor_da
        data.values[1] = euler_orientation.m_ang.m_vec[1];
        data.values[2] = euler_orientation.m_ang.m_vec[2];
        data.values_num = 3;
+
        return 0;
 }
 
@@ -249,8 +281,10 @@ bool orientation_sensor::get_properties(sensor_properties_t &properties)
        properties.sensor_min_range = -180;
        properties.sensor_max_range = 180;
        properties.sensor_resolution = 1;
+
        strncpy(properties.sensor_vendor, "Samsung", MAX_KEY_LENGTH);
        strncpy(properties.sensor_name, SENSOR_NAME, MAX_KEY_LENGTH);
+
        return true;
 }
 

diff --git a/src/orientation/orientation_sensor.h b/src/orientation/orientation_sensor.h
index 5ed776b..9f289f0 100755 (executable)
--- a/src/orientation/orientation_sensor.h
+++ b/src/orientation/orientation_sensor.h
@@ -55,6 +55,7 @@ private:
        float m_pitch;
        float m_yaw;
        unsigned long long m_timestamp;
+       unsigned int m_interval;
 };
 
 #endif /* _ORIENTATION_SENSOR_H_ */

diff --git a/src/sensor_fusion/orientation_filter.cpp b/src/sensor_fusion/orientation_filter.cpp
index 1c09379..2226cbe 100644 (file)
--- a/src/sensor_fusion/orientation_filter.cpp
+++ b/src/sensor_fusion/orientation_filter.cpp
@@ -134,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, m_magnetic_alignment_factor, 0.0};
+       TYPE arr_mag_e[V1x3S] = {0.0, (TYPE) m_magnetic_alignment_factor, 0.0};
 
        vect<TYPE> acc_e(V1x3S, arr_acc_e);
        vect<TYPE> mag_e(V1x3S, arr_mag_e);