From 27863743e26f30ce39cfd0609a88ac719c7f4d79 Mon Sep 17 00:00:00 2001 From: Ramasamy Date: Thu, 12 Mar 2015 09:09:30 +0530 Subject: [PATCH] Updating Geomagnetic RV sensor after fusion restructuring - Works in combination with the new fusion sensor - supports new configuration xml - cleanup Change-Id: I60fec833776d3c5d3cbfc76f8fdaf3c7a198a708 --- .../geomagnetic_rv/geomagnetic_rv_sensor.cpp | 158 +++++---------------- .../geomagnetic_rv/geomagnetic_rv_sensor.h | 11 +- 2 files changed, 36 insertions(+), 133 deletions(-) diff --git a/src/rotation_vector/geomagnetic_rv/geomagnetic_rv_sensor.cpp b/src/rotation_vector/geomagnetic_rv/geomagnetic_rv_sensor.cpp index ef56350..381dbe2 100755 --- a/src/rotation_vector/geomagnetic_rv/geomagnetic_rv_sensor.cpp +++ b/src/rotation_vector/geomagnetic_rv/geomagnetic_rv_sensor.cpp @@ -35,9 +35,7 @@ #define SENSOR_NAME "GEOMAGNETIC_RV_SENSOR" #define SENSOR_TYPE_GEOMAGNETIC_RV "GEOMAGNETIC_ROTATION_VECTOR" -#define ACCELEROMETER_ENABLED 0x01 -#define GEOMAGNETIC_ENABLED 0x02 -#define GEOMAGNETIC_RV_ENABLED 3 +#define MIN_DELIVERY_DIFF_FACTOR 0.75f #define INITIAL_VALUE -1 @@ -47,13 +45,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_GEOMAGNETIC_STATIC_BIAS "GEOMAGNETIC_STATIC_BIAS" -#define ELEMENT_ACCEL_ROTATION_DIRECTION_COMPENSATION "ACCEL_ROTATION_DIRECTION_COMPENSATION" -#define ELEMENT_GEOMAGNETIC_ROTATION_DIRECTION_COMPENSATION "GEOMAGNETIC_ROTATION_DIRECTION_COMPENSATION" -#define ELEMENT_ACCEL_SCALE "ACCEL_SCALE" -#define ELEMENT_GEOMAGNETIC_SCALE "GEOMAGNETIC_SCALE" -#define ELEMENT_MAGNETIC_ALIGNMENT_FACTOR "MAGNETIC_ALIGNMENT_FACTOR" void pre_process_data(sensor_data &data_out, const float *data_in, float *bias, int *sign, float scale) { @@ -65,7 +56,6 @@ void pre_process_data(sensor_data &data_out, const float *data_in, float geomagnetic_rv_sensor::geomagnetic_rv_sensor() : m_accel_sensor(NULL) , m_magnetic_sensor(NULL) -, m_accuracy(-1) , m_time(0) { cvirtual_sensor_config &config = cvirtual_sensor_config::get_instance(); @@ -88,57 +78,7 @@ geomagnetic_rv_sensor::geomagnetic_rv_sensor() INFO("m_default_sampling_time = %d", m_default_sampling_time); - if (!config.get(SENSOR_TYPE_GEOMAGNETIC_RV, 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_GEOMAGNETIC_RV, 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_GEOMAGNETIC_RV, 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_GEOMAGNETIC_RV, 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_GEOMAGNETIC_RV, 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_GEOMAGNETIC_RV, 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_GEOMAGNETIC_RV, 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); - m_interval = m_default_sampling_time * MS_TO_US; - } geomagnetic_rv_sensor::~geomagnetic_rv_sensor() @@ -151,9 +91,11 @@ bool geomagnetic_rv_sensor::init() m_accel_sensor = sensor_plugin_loader::get_instance().get_sensor(ACCELEROMETER_SENSOR); m_magnetic_sensor = sensor_plugin_loader::get_instance().get_sensor(GEOMAGNETIC_SENSOR); - if (!m_accel_sensor || !m_magnetic_sensor) { - ERR("Failed to load sensors, accel: 0x%x, mag: 0x%x", - m_accel_sensor, m_magnetic_sensor); + m_fusion_sensor = sensor_plugin_loader::get_instance().get_sensor(FUSION_SENSOR); + + if (!m_accel_sensor || !m_magnetic_sensor || !m_fusion_sensor) { + ERR("Failed to load sensors, accel: 0x%x, mag: 0x%x, fusion: 0x%x", + m_accel_sensor, m_magnetic_sensor, m_fusion_sensor); return false; } @@ -178,6 +120,12 @@ bool geomagnetic_rv_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_GEOMAGNETIC_ROTATION_VECTOR_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; } @@ -193,6 +141,12 @@ bool geomagnetic_rv_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_GEOMAGNETIC_ROTATION_VECTOR_ENABLED); + m_fusion_sensor->stop(); + deactivate(); return true; } @@ -204,6 +158,8 @@ bool geomagnetic_rv_sensor::add_interval(int client_id, unsigned int interval) m_accel_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); } @@ -214,61 +170,33 @@ bool geomagnetic_rv_sensor::delete_interval(int client_id) m_accel_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 geomagnetic_rv_sensor::synthesize(const sensor_event_t& event, vector &outs) +void geomagnetic_rv_sensor::synthesize(const sensor_event_t &event, vector &outs) { - const float MIN_DELIVERY_DIFF_FACTOR = 0.75f; unsigned long long diff_time; sensor_event_t rv_event; - quaternion quaternion_geo_rv; - - 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_geomagnetic_rv |= ACCELEROMETER_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_geomagnetic_rv |= GEOMAGNETIC_ENABLED; - } - - if (m_enable_geomagnetic_rv == GEOMAGNETIC_RV_ENABLED) { - m_enable_geomagnetic_rv = 0; - - m_orientation_filter.m_magnetic_alignment_factor = m_magnetic_alignment_factor; - - m_orientation_filter.get_device_orientation(&m_accel, NULL, &m_magnetic); - - quaternion_geo_rv = m_orientation_filter.m_quat_aid; - m_time = get_timestamp(); rv_event.sensor_id = get_id(); rv_event.event_type = GEOMAGNETIC_RV_RAW_DATA_EVENT; rv_event.data.accuracy = SENSOR_ACCURACY_GOOD; rv_event.data.timestamp = m_time; rv_event.data.value_count = 4; - rv_event.data.values[0] = quaternion_geo_rv.m_quat.m_vec[1]; - rv_event.data.values[1] = quaternion_geo_rv.m_quat.m_vec[2]; - rv_event.data.values[2] = quaternion_geo_rv.m_quat.m_vec[3]; - rv_event.data.values[3] = quaternion_geo_rv.m_quat.m_vec[0]; + rv_event.data.values[0] = event.data.values[1]; + rv_event.data.values[1] = event.data.values[2]; + rv_event.data.values[2] = event.data.values[3]; + rv_event.data.values[3] = event.data.values[0]; push(rv_event); } @@ -278,38 +206,20 @@ void geomagnetic_rv_sensor::synthesize(const sensor_event_t& event, vector accel; - sensor_data magnetic; - - sensor_data_t accel_data; - sensor_data_t magnetic_data; - - quaternion quaternion_geo_rv; + sensor_data_t fusion_data; if (event_type != GEOMAGNETIC_RV_RAW_DATA_EVENT) return -1; - m_accel_sensor->get_sensor_data(ACCELEROMETER_RAW_DATA_EVENT, accel_data); - m_magnetic_sensor->get_sensor_data(GEOMAGNETIC_RAW_DATA_EVENT, magnetic_data); - - pre_process_data(accel, accel_data.values, m_accel_static_bias, m_accel_rotation_direction_compensation, m_accel_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; - magnetic.m_time_stamp = magnetic_data.timestamp; - - m_orientation_filter_poll.m_magnetic_alignment_factor = m_magnetic_alignment_factor; - - m_orientation_filter_poll.get_device_orientation(&m_accel, NULL, &m_magnetic); - - quaternion_geo_rv = m_orientation_filter_poll.m_quat_aid; + m_fusion_sensor->get_sensor_data(FUSION_ORIENTATION_ENABLED, fusion_data); data.accuracy = SENSOR_ACCURACY_GOOD; data.timestamp = get_timestamp(); data.value_count = 4; - data.values[0] = quaternion_geo_rv.m_quat.m_vec[1]; - data.values[1] = quaternion_geo_rv.m_quat.m_vec[2]; - data.values[2] = quaternion_geo_rv.m_quat.m_vec[3]; - data.values[3] = quaternion_geo_rv.m_quat.m_vec[0]; + data.values[0] = data.values[1]; + data.values[1] = data.values[2]; + data.values[2] = data.values[3]; + data.values[3] = data.values[0]; return 0; } diff --git a/src/rotation_vector/geomagnetic_rv/geomagnetic_rv_sensor.h b/src/rotation_vector/geomagnetic_rv/geomagnetic_rv_sensor.h index 8b54705..3cf1999 100755 --- a/src/rotation_vector/geomagnetic_rv/geomagnetic_rv_sensor.h +++ b/src/rotation_vector/geomagnetic_rv/geomagnetic_rv_sensor.h @@ -38,11 +38,12 @@ 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_magnetic_sensor; + sensor_base *m_fusion_sensor; sensor_data m_accel; sensor_data m_magnetic; @@ -54,20 +55,12 @@ private: unsigned int m_enable_geomagnetic_rv; - int m_accuracy; unsigned long long m_time; unsigned int m_interval; string m_vendor; string m_raw_data_unit; int m_default_sampling_time; - float m_accel_static_bias[3]; - float m_geomagnetic_static_bias[3]; - int m_accel_rotation_direction_compensation[3]; - int m_geomagnetic_rotation_direction_compensation[3]; - float m_accel_scale; - float m_geomagnetic_scale; - int m_magnetic_alignment_factor; bool on_start(void); bool on_stop(void); -- 2.7.4