Updating orientation sensor after fusion restructuring

- Works in combination with the new fusion sensor
- supports new configuration xml
- cleanup

Change-Id: I0694ff6c2cc5c4ab8d9000d53359eefc39198302

diff --git a/src/orientation/orientation_sensor.cpp b/src/orientation/orientation_sensor.cpp
index d5fe72d..aee027a 100755 (executable)
--- a/src/orientation/orientation_sensor.cpp
+++ b/src/orientation/orientation_sensor.cpp
@@ -35,11 +35,6 @@
 #define SENSOR_NAME "ORIENTATION_SENSOR"
 #define SENSOR_TYPE_ORIENTATION                "ORIENTATION"
 
-#define ACCELEROMETER_ENABLED 0x01
-#define GYROSCOPE_ENABLED 0x02
-#define GEOMAGNETIC_ENABLED 0x04
-#define ORIENTATION_ENABLED 7
-
 #define INITIAL_VALUE -1
 
 #define MS_TO_US 1000
@@ -53,16 +48,6 @@
 #define ELEMENT_VENDOR                                                                                 "VENDOR"
 #define ELEMENT_RAW_DATA_UNIT                                                                  "RAW_DATA_UNIT"
 #define ELEMENT_DEFAULT_SAMPLING_TIME                                                  "DEFAULT_SAMPLING_TIME"
-#define ELEMENT_ACCEL_STATIC_BIAS                                                              "ACCEL_STATIC_BIAS"
-#define ELEMENT_GYRO_STATIC_BIAS                                                               "GYRO_STATIC_BIAS"
-#define ELEMENT_GEOMAGNETIC_STATIC_BIAS                                                        "GEOMAGNETIC_STATIC_BIAS"
-#define ELEMENT_ACCEL_ROTATION_DIRECTION_COMPENSATION                  "ACCEL_ROTATION_DIRECTION_COMPENSATION"
-#define ELEMENT_GYRO_ROTATION_DIRECTION_COMPENSATION                   "GYRO_ROTATION_DIRECTION_COMPENSATION"
-#define ELEMENT_GEOMAGNETIC_ROTATION_DIRECTION_COMPENSATION            "GEOMAGNETIC_ROTATION_DIRECTION_COMPENSATION"
-#define ELEMENT_ACCEL_SCALE                                                                            "ACCEL_SCALE"
-#define ELEMENT_GYRO_SCALE                                                                             "GYRO_SCALE"
-#define ELEMENT_GEOMAGNETIC_SCALE                                                              "GEOMAGNETIC_SCALE"
-#define ELEMENT_MAGNETIC_ALIGNMENT_FACTOR                                              "MAGNETIC_ALIGNMENT_FACTOR"
 #define ELEMENT_PITCH_ROTATION_COMPENSATION                                            "PITCH_ROTATION_COMPENSATION"
 #define ELEMENT_ROLL_ROTATION_COMPENSATION                                             "ROLL_ROTATION_COMPENSATION"
 #define ELEMENT_AZIMUTH_ROTATION_COMPENSATION                                  "AZIMUTH_ROTATION_COMPENSATION"
@@ -107,76 +92,6 @@ orientation_sensor::orientation_sensor()
 
        INFO("m_default_sampling_time = %d", m_default_sampling_time);
 
-       if (!config.get(SENSOR_TYPE_ORIENTATION, ELEMENT_ACCEL_STATIC_BIAS, m_accel_static_bias, 3)) {
-               ERR("[ACCEL_STATIC_BIAS] is empty\n");
-               throw ENXIO;
-       }
-
-       INFO("m_accel_static_bias = (%f, %f, %f)", m_accel_static_bias[0], m_accel_static_bias[1], m_accel_static_bias[2]);
-
-       if (!config.get(SENSOR_TYPE_ORIENTATION, ELEMENT_GYRO_STATIC_BIAS, m_gyro_static_bias,3)) {
-               ERR("[GYRO_STATIC_BIAS] is empty\n");
-               throw ENXIO;
-       }
-
-       INFO("m_gyro_static_bias = (%f, %f, %f)", m_gyro_static_bias[0], m_gyro_static_bias[1], m_gyro_static_bias[2]);
-
-       if (!config.get(SENSOR_TYPE_ORIENTATION, ELEMENT_GEOMAGNETIC_STATIC_BIAS, m_geomagnetic_static_bias, 3)) {
-               ERR("[GEOMAGNETIC_STATIC_BIAS] is empty\n");
-               throw ENXIO;
-       }
-
-       INFO("m_geomagnetic_static_bias = (%f, %f, %f)", m_geomagnetic_static_bias[0], m_geomagnetic_static_bias[1], m_geomagnetic_static_bias[2]);
-
-       if (!config.get(SENSOR_TYPE_ORIENTATION, ELEMENT_ACCEL_ROTATION_DIRECTION_COMPENSATION, m_accel_rotation_direction_compensation, 3)) {
-               ERR("[ACCEL_ROTATION_DIRECTION_COMPENSATION] is empty\n");
-               throw ENXIO;
-       }
-
-       INFO("m_accel_rotation_direction_compensation = (%d, %d, %d)", m_accel_rotation_direction_compensation[0], m_accel_rotation_direction_compensation[1], m_accel_rotation_direction_compensation[2]);
-
-       if (!config.get(SENSOR_TYPE_ORIENTATION, ELEMENT_GYRO_ROTATION_DIRECTION_COMPENSATION, m_gyro_rotation_direction_compensation, 3)) {
-               ERR("[GYRO_ROTATION_DIRECTION_COMPENSATION] is empty\n");
-               throw ENXIO;
-       }
-
-       INFO("m_gyro_rotation_direction_compensation = (%d, %d, %d)", m_gyro_rotation_direction_compensation[0], m_gyro_rotation_direction_compensation[1], m_gyro_rotation_direction_compensation[2]);
-
-       if (!config.get(SENSOR_TYPE_ORIENTATION, ELEMENT_GEOMAGNETIC_ROTATION_DIRECTION_COMPENSATION, m_geomagnetic_rotation_direction_compensation, 3)) {
-               ERR("[GEOMAGNETIC_ROTATION_DIRECTION_COMPENSATION] is empty\n");
-               throw ENXIO;
-       }
-
-       INFO("m_geomagnetic_rotation_direction_compensation = (%d, %d, %d)", m_geomagnetic_rotation_direction_compensation[0], m_geomagnetic_rotation_direction_compensation[1], m_geomagnetic_rotation_direction_compensation[2]);
-
-       if (!config.get(SENSOR_TYPE_ORIENTATION, ELEMENT_ACCEL_SCALE, &m_accel_scale)) {
-               ERR("[ACCEL_SCALE] is empty\n");
-               throw ENXIO;
-       }
-
-       INFO("m_accel_scale = %f", m_accel_scale);
-
-       if (!config.get(SENSOR_TYPE_ORIENTATION, ELEMENT_GYRO_SCALE, &m_gyro_scale)) {
-               ERR("[GYRO_SCALE] is empty\n");
-               throw ENXIO;
-       }
-
-       INFO("m_gyro_scale = %f", m_gyro_scale);
-
-       if (!config.get(SENSOR_TYPE_ORIENTATION, ELEMENT_GEOMAGNETIC_SCALE, &m_geomagnetic_scale)) {
-               ERR("[GEOMAGNETIC_SCALE] is empty\n");
-               throw ENXIO;
-       }
-
-       INFO("m_geomagnetic_scale = %f", m_geomagnetic_scale);
-
-       if (!config.get(SENSOR_TYPE_ORIENTATION, ELEMENT_MAGNETIC_ALIGNMENT_FACTOR, &m_magnetic_alignment_factor)) {
-               ERR("[MAGNETIC_ALIGNMENT_FACTOR] is empty\n");
-               throw ENXIO;
-       }
-
-       INFO("m_magnetic_alignment_factor = %d", m_magnetic_alignment_factor);
-
        if (!config.get(SENSOR_TYPE_ORIENTATION, ELEMENT_AZIMUTH_ROTATION_COMPENSATION, &m_azimuth_rotation_compensation)) {
                ERR("[AZIMUTH_ROTATION_COMPENSATION] is empty\n");
                throw ENXIO;
@@ -199,7 +114,6 @@ orientation_sensor::orientation_sensor()
        INFO("m_roll_rotation_compensation = %d", m_roll_rotation_compensation);
 
        m_interval = m_default_sampling_time * MS_TO_US;
-
 }
 
 orientation_sensor::~orientation_sensor()
@@ -213,13 +127,16 @@ bool orientation_sensor::init(void)
        m_gyro_sensor = sensor_plugin_loader::get_instance().get_sensor(GYROSCOPE_SENSOR);
        m_magnetic_sensor = sensor_plugin_loader::get_instance().get_sensor(GEOMAGNETIC_SENSOR);
 
-       if (!m_accel_sensor || !m_gyro_sensor || !m_magnetic_sensor) {
-               ERR("Failed to load sensors,  accel: 0x%x, gyro: 0x%x, mag: 0x%x",
-                       m_accel_sensor, m_gyro_sensor, m_magnetic_sensor);
+       m_fusion_sensor = sensor_plugin_loader::get_instance().get_sensor(FUSION_SENSOR);
+
+       if (!m_accel_sensor || !m_gyro_sensor || !m_magnetic_sensor || !m_fusion_sensor) {
+               ERR("Failed to load sensors,  accel: 0x%x, gyro: 0x%x, mag: 0x%x, fusion: 0x%x",
+                       m_accel_sensor, m_gyro_sensor, m_magnetic_sensor, m_fusion_sensor);
                return false;
        }
 
-       INFO("%s is created!", sensor_base::get_name());
+       INFO("%s is created!\n", sensor_base::get_name());
+
        return true;
 }
 
@@ -242,6 +159,12 @@ bool orientation_sensor::on_start(void)
        m_magnetic_sensor->add_interval((intptr_t)this, (m_interval/MS_TO_US), false);
        m_magnetic_sensor->start();
 
+       m_fusion_sensor->register_supported_event(FUSION_EVENT);
+       m_fusion_sensor->register_supported_event(FUSION_ORIENTATION_ENABLED);
+       m_fusion_sensor->add_client(FUSION_EVENT);
+       m_fusion_sensor->add_interval((intptr_t)this, (m_interval/MS_TO_US), false);
+       m_fusion_sensor->start();
+
        activate();
        return true;
 }
@@ -260,6 +183,12 @@ bool orientation_sensor::on_stop(void)
        m_magnetic_sensor->delete_interval((intptr_t)this, false);
        m_magnetic_sensor->stop();
 
+       m_fusion_sensor->delete_client(FUSION_EVENT);
+       m_fusion_sensor->delete_interval((intptr_t)this, false);
+       m_fusion_sensor->unregister_supported_event(FUSION_EVENT);
+       m_fusion_sensor->unregister_supported_event(FUSION_ORIENTATION_ENABLED);
+       m_fusion_sensor->stop();
+
        deactivate();
        return true;
 }
@@ -272,6 +201,8 @@ bool orientation_sensor::add_interval(int client_id, unsigned int interval)
        m_gyro_sensor->add_interval(client_id, interval, false);
        m_magnetic_sensor->add_interval(client_id, interval, false);
 
+       m_fusion_sensor->add_interval(client_id, interval, false);
+
        return sensor_base::add_interval(client_id, interval, false);
 }
 
@@ -283,68 +214,30 @@ bool orientation_sensor::delete_interval(int client_id)
        m_gyro_sensor->delete_interval(client_id, false);
        m_magnetic_sensor->delete_interval(client_id, false);
 
+       m_fusion_sensor->delete_interval(client_id, false);
+
        return sensor_base::delete_interval(client_id, false);
 }
 
 void orientation_sensor::synthesize(const sensor_event_t &event, vector<sensor_event_t> &outs)
 {
-       unsigned long long diff_time;
-
        sensor_event_t orientation_event;
-       euler_angles<float> euler_orientation;
+       unsigned long long diff_time;
        float azimuth_offset;
 
-       if (event.event_type == ACCELEROMETER_RAW_DATA_EVENT) {
-               diff_time = event.data.timestamp - m_time;
-
-               if (m_time && (diff_time < m_interval * MIN_DELIVERY_DIFF_FACTOR))
-                       return;
-
-               pre_process_data(m_accel, event.data.values, m_accel_static_bias, m_accel_rotation_direction_compensation, m_accel_scale);
-
-               m_accel.m_time_stamp = event.data.timestamp;
-
-               m_enable_orientation |= ACCELEROMETER_ENABLED;
-       }
-       else if (event.event_type == GYROSCOPE_RAW_DATA_EVENT) {
-               diff_time = event.data.timestamp - m_time;
-
-               if (m_time && (diff_time < m_interval * MIN_DELIVERY_DIFF_FACTOR))
-                       return;
-
-               pre_process_data(m_gyro, event.data.values, m_gyro_static_bias, m_gyro_rotation_direction_compensation, m_gyro_scale);
-
-               m_gyro.m_time_stamp = event.data.timestamp;
-
-               m_enable_orientation |= GYROSCOPE_ENABLED;
-       }
-       else if (event.event_type == GEOMAGNETIC_RAW_DATA_EVENT) {
+       if (event.event_type == FUSION_EVENT) {
                diff_time = event.data.timestamp - m_time;
 
                if (m_time && (diff_time < m_interval * MIN_DELIVERY_DIFF_FACTOR))
                        return;
 
-               pre_process_data(m_magnetic, event.data.values, m_geomagnetic_static_bias, m_geomagnetic_rotation_direction_compensation, m_geomagnetic_scale);
-
-               m_magnetic.m_time_stamp = event.data.timestamp;
-
-               m_enable_orientation |= GEOMAGNETIC_ENABLED;
-       }
-
-       if (m_enable_orientation == ORIENTATION_ENABLED) {
-               m_enable_orientation = 0;
+               quaternion<float> quat(event.data.values[0], event.data.values[1],
+                               event.data.values[2], event.data.values[3]);
 
-               m_orientation_filter.m_pitch_phase_compensation = m_pitch_rotation_compensation;
-               m_orientation_filter.m_roll_phase_compensation = m_roll_rotation_compensation;
-               m_orientation_filter.m_azimuth_phase_compensation = m_azimuth_rotation_compensation;
-               m_orientation_filter.m_magnetic_alignment_factor = m_magnetic_alignment_factor;
-
-               m_orientation_filter.get_device_orientation(&m_accel, &m_gyro, &m_magnetic);
-
-               euler_orientation = m_orientation_filter.m_orientation;
+               euler_angles<float> euler = quat2euler(quat);
 
                if(m_raw_data_unit == "DEGREES") {
-                       euler_orientation = rad2deg(euler_orientation);
+                       euler = rad2deg(euler);
                        azimuth_offset = AZIMUTH_OFFSET_DEGREES;
                }
                else {
@@ -354,15 +247,19 @@ void orientation_sensor::synthesize(const sensor_event_t &event, vector<sensor_e
                m_time = get_timestamp();
                orientation_event.sensor_id = get_id();
                orientation_event.event_type = ORIENTATION_RAW_DATA_EVENT;
-               orientation_event.data.accuracy = SENSOR_ACCURACY_GOOD;
+               orientation_event.data.accuracy = event.data.accuracy;
                orientation_event.data.timestamp = m_time;
                orientation_event.data.value_count = 3;
-               orientation_event.data.values[1] = euler_orientation.m_ang.m_vec[0];
-               orientation_event.data.values[2] = euler_orientation.m_ang.m_vec[1];
-               if (euler_orientation.m_ang.m_vec[2] >= 0)
-                       orientation_event.data.values[0] = euler_orientation.m_ang.m_vec[2];
+               orientation_event.data.values[1] = euler.m_ang.m_vec[0];
+               orientation_event.data.values[2] = euler.m_ang.m_vec[1];
+               if (euler.m_ang.m_vec[2] >= 0)
+                       orientation_event.data.values[0] = euler.m_ang.m_vec[2];
                else
-                       orientation_event.data.values[0] = euler_orientation.m_ang.m_vec[2] + azimuth_offset;
+                       orientation_event.data.values[0] = euler.m_ang.m_vec[2] + azimuth_offset;
+
+               orientation_event.data.values[1] *= m_pitch_rotation_compensation;
+               orientation_event.data.values[2] *= m_roll_rotation_compensation;
+               orientation_event.data.values[0] *= m_azimuth_rotation_compensation;
 
                push(orientation_event);
        }
@@ -372,57 +269,40 @@ void orientation_sensor::synthesize(const sensor_event_t &event, vector<sensor_e
 
 int orientation_sensor::get_sensor_data(const unsigned int event_type, sensor_data_t &data)
 {
-       sensor_data<float> accel;
-       sensor_data<float> gyro;
-       sensor_data<float> magnetic;
-
-       sensor_data_t accel_data;
-       sensor_data_t gyro_data;
-       sensor_data_t magnetic_data;
-
-       euler_angles<float> euler_orientation;
+       sensor_data_t fusion_data;
        float azimuth_offset;
 
        if (event_type != ORIENTATION_RAW_DATA_EVENT)
                return -1;
 
-       m_accel_sensor->get_sensor_data(ACCELEROMETER_RAW_DATA_EVENT, accel_data);
-       m_gyro_sensor->get_sensor_data(GYROSCOPE_RAW_DATA_EVENT, gyro_data);
-       m_magnetic_sensor->get_sensor_data(GEOMAGNETIC_RAW_DATA_EVENT, magnetic_data);
+       m_fusion_sensor->get_sensor_data(FUSION_ORIENTATION_ENABLED, fusion_data);
 
-       pre_process_data(accel, accel_data.values, m_accel_static_bias, m_accel_rotation_direction_compensation, m_accel_scale);
-       pre_process_data(gyro, gyro_data.values, m_gyro_static_bias, m_gyro_rotation_direction_compensation, m_gyro_scale);
-       pre_process_data(magnetic, magnetic_data.values, m_geomagnetic_static_bias, m_geomagnetic_rotation_direction_compensation, m_geomagnetic_scale);
-       accel.m_time_stamp = accel_data.timestamp;
-       gyro.m_time_stamp = gyro_data.timestamp;
-       magnetic.m_time_stamp = magnetic_data.timestamp;
+       quaternion<float> quat(fusion_data.values[0], fusion_data.values[1],
+                       fusion_data.values[2], fusion_data.values[3]);
 
-       m_orientation_filter_poll.m_pitch_phase_compensation = m_pitch_rotation_compensation;
-       m_orientation_filter_poll.m_roll_phase_compensation = m_roll_rotation_compensation;
-       m_orientation_filter_poll.m_azimuth_phase_compensation = m_azimuth_rotation_compensation;
-       m_orientation_filter_poll.m_magnetic_alignment_factor = m_magnetic_alignment_factor;
-
-       m_orientation_filter_poll.get_device_orientation(&m_accel, &m_gyro, &m_magnetic);
-
-       euler_orientation = m_orientation_filter_poll.m_orientation;
+       euler_angles<float> euler = quat2euler(quat);
 
        if(m_raw_data_unit == "DEGREES") {
-               euler_orientation = rad2deg(euler_orientation);
+               euler = rad2deg(euler);
                azimuth_offset = AZIMUTH_OFFSET_DEGREES;
        }
        else {
                azimuth_offset = AZIMUTH_OFFSET_RADIANS;
        }
 
-       data.accuracy = SENSOR_ACCURACY_GOOD;
+       data.accuracy = fusion_data.accuracy;
        data.timestamp = get_timestamp();
-       data.values[1] = euler_orientation.m_ang.m_vec[0];
-       data.values[2] = euler_orientation.m_ang.m_vec[1];
-       if (euler_orientation.m_ang.m_vec[2] >= 0)
-               data.values[0] = euler_orientation.m_ang.m_vec[2];
-       else
-               data.values[0] = euler_orientation.m_ang.m_vec[2] + azimuth_offset;
        data.value_count = 3;
+       data.values[1] = euler.m_ang.m_vec[0];
+       data.values[2] = euler.m_ang.m_vec[1];
+       if (euler.m_ang.m_vec[2] >= 0)
+               data.values[0] = euler.m_ang.m_vec[2];
+       else
+               data.values[0] = euler.m_ang.m_vec[2] + azimuth_offset;
+
+       data.values[1] *= m_pitch_rotation_compensation;
+       data.values[2] *= m_roll_rotation_compensation;
+       data.values[0] *= m_azimuth_rotation_compensation;
 
        return 0;
 }

diff --git a/src/orientation/orientation_sensor.h b/src/orientation/orientation_sensor.h
index baa783b..1128541 100755 (executable)
--- a/src/orientation/orientation_sensor.h
+++ b/src/orientation/orientation_sensor.h
@@ -38,12 +38,13 @@ public:
        bool get_properties(sensor_properties_s &properties);
        sensor_type_t get_type(void);
 
-       int get_sensor_data(const unsigned int data_id, sensor_data_t &data);
+       int get_sensor_data(const unsigned int event_type, sensor_data_t &data);
 
 private:
        sensor_base *m_accel_sensor;
        sensor_base *m_gyro_sensor;
        sensor_base *m_magnetic_sensor;
+       sensor_base *m_fusion_sensor;
 
        sensor_data<float> m_accel;
        sensor_data<float> m_gyro;
@@ -62,16 +63,6 @@ private:
        string m_vendor;
        string m_raw_data_unit;
        int m_default_sampling_time;
-       float m_accel_static_bias[3];
-       float m_gyro_static_bias[3];
-       float m_geomagnetic_static_bias[3];
-       int m_accel_rotation_direction_compensation[3];
-       int m_gyro_rotation_direction_compensation[3];
-       int m_geomagnetic_rotation_direction_compensation[3];
-       float m_accel_scale;
-       float m_gyro_scale;
-       float m_geomagnetic_scale;
-       int m_magnetic_alignment_factor;
        int m_azimuth_rotation_compensation;
        int m_pitch_rotation_compensation;
        int m_roll_rotation_compensation;