From c5a3d394f5744ea7f13bde16a3a87cfd09c92952 Mon Sep 17 00:00:00 2001 From: Ramasamy Date: Fri, 21 Nov 2014 09:54:58 +0530 Subject: [PATCH] Updating Linear Acceleration Virtual Sensor Code - Updating Linear Acceleration sensor after testing on SDK - Updating based on latest public code - Updating for new XML based configuration file Change-Id: I1981b0ce9a5a10525d54c3d6977075cd1f70e6d0 --- packaging/sensord.spec | 2 +- src/linear_accel/CMakeLists.txt | 1 + src/linear_accel/linear_accel_sensor.cpp | 179 ++++++++++++++++++++++++++----- src/linear_accel/linear_accel_sensor.h | 38 ++++--- 4 files changed, 175 insertions(+), 45 deletions(-) diff --git a/packaging/sensord.spec b/packaging/sensord.spec index bfa8fa4..a76daab 100755 --- a/packaging/sensord.spec +++ b/packaging/sensord.spec @@ -17,7 +17,7 @@ Source2: sensord.socket %define temperature_state OFF %define orientation_state ON %define gravity_state ON -%define linear_accel_state OFF +%define linear_accel_state ON %define build_test_suite OFF diff --git a/src/linear_accel/CMakeLists.txt b/src/linear_accel/CMakeLists.txt index b158f2b..6601d90 100755 --- a/src/linear_accel/CMakeLists.txt +++ b/src/linear_accel/CMakeLists.txt @@ -10,6 +10,7 @@ SET(SENSOR_NAME linear_accel_sensor) include_directories(${CMAKE_CURRENT_SOURCE_DIR}) include_directories(${CMAKE_SOURCE_DIR}/src/libsensord) +include_directories(${CMAKE_SOURCE_DIR}/src/sensor_fusion) include(FindPkgConfig) pkg_check_modules(rpkgs REQUIRED vconf) diff --git a/src/linear_accel/linear_accel_sensor.cpp b/src/linear_accel/linear_accel_sensor.cpp index 6967da0..794bef0 100755 --- a/src/linear_accel/linear_accel_sensor.cpp +++ b/src/linear_accel/linear_accel_sensor.cpp @@ -25,33 +25,104 @@ #include #include #include -#include #include #include -#include #include #include +#include #define SENSOR_NAME "LINEAR_ACCEL_SENSOR" +#define SENSOR_TYPE_LINEAR_ACCEL "LINEAR_ACCEL" + +#define ELEMENT_NAME "NAME" +#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_ACCEL_ROTATION_DIRECTION_COMPENSATION "ACCEL_ROTATION_DIRECTION_COMPENSATION" +#define ELEMENT_ACCEL_SCALE "ACCEL_SCALE" +#define ELEMENT_LINEAR_ACCEL_SIGN_COMPENSATION "LINEAR_ACCEL_SIGN_COMPENSATION" #define INITIAL_VALUE -1 -#define INITIAL_TIME 0 #define GRAVITY 9.80665 +#define MS_TO_US 1000 + +#define ACCELEROMETER_ENABLED 0x01 +#define GRAVITY_ENABLED 0x02 +#define LINEAR_ACCEL_ENABLED 3 + linear_accel_sensor::linear_accel_sensor() : m_gravity_sensor(NULL) +, m_accel_sensor(NULL) , m_x(INITIAL_VALUE) , m_y(INITIAL_VALUE) , m_z(INITIAL_VALUE) -, m_time(INITIAL_TIME) { + cvirtual_sensor_config &config = cvirtual_sensor_config::get_instance(); + m_name = string(SENSOR_NAME); + m_timestamp = get_timestamp(); + m_enable_linear_accel = 0; register_supported_event(LINEAR_ACCEL_EVENT_RAW_DATA_REPORT_ON_TIME); + + + if (!config.get(SENSOR_TYPE_LINEAR_ACCEL, ELEMENT_VENDOR, m_vendor)) { + ERR("[VENDOR] is empty\n"); + throw ENXIO; + } + + INFO("m_vendor = %s", m_vendor.c_str()); + + if (!config.get(SENSOR_TYPE_LINEAR_ACCEL, ELEMENT_RAW_DATA_UNIT, m_raw_data_unit)) { + ERR("[RAW_DATA_UNIT] is empty\n"); + throw ENXIO; + } + + INFO("m_raw_data_unit = %s", m_raw_data_unit.c_str()); + + if (!config.get(SENSOR_TYPE_LINEAR_ACCEL, ELEMENT_DEFAULT_SAMPLING_TIME, &m_default_sampling_time)) { + ERR("[DEFAULT_SAMPLING_TIME] is empty\n"); + throw ENXIO; + } + + INFO("m_default_sampling_time = %d", m_default_sampling_time); + + if (!config.get(SENSOR_TYPE_LINEAR_ACCEL, ELEMENT_ACCEL_STATIC_BIAS, m_accel_static_bias, 3)) { + ERR("[ACCEL_STATIC_BIAS] is empty\n"); + throw ENXIO; + } + + if (!config.get(SENSOR_TYPE_LINEAR_ACCEL, 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_LINEAR_ACCEL, 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_LINEAR_ACCEL, ELEMENT_LINEAR_ACCEL_SIGN_COMPENSATION, m_linear_accel_sign_compensation, 3)) { + ERR("[LINEAR_ACCEL_SIGN_COMPENSATION] is empty\n"); + throw ENXIO; + } + + INFO("m_linear_accel_sign_compensation = (%d, %d, %d)", m_linear_accel_sign_compensation[0], m_linear_accel_sign_compensation[1], m_linear_accel_sign_compensation[2]); + + m_interval = m_default_sampling_time * MS_TO_US; + } linear_accel_sensor::~linear_accel_sensor() { - INFO("linear_accel_sensor is destroyed!"); + INFO("linear_accel_sensor is destroyed!\n"); } bool linear_accel_sensor::init() @@ -78,7 +149,13 @@ bool linear_accel_sensor::on_start(void) { AUTOLOCK(m_mutex); m_gravity_sensor->add_client(GRAVITY_EVENT_RAW_DATA_REPORT_ON_TIME); + m_gravity_sensor->add_interval((int)this, (m_interval/MS_TO_US), true); m_gravity_sensor->start(); + + m_accel_sensor->add_client(ACCELEROMETER_EVENT_RAW_DATA_REPORT_ON_TIME); + m_accel_sensor->add_interval((int)this, (m_interval/MS_TO_US), true); + m_accel_sensor->start(); + activate(); return true; } @@ -87,7 +164,13 @@ bool linear_accel_sensor::on_stop(void) { AUTOLOCK(m_mutex); m_gravity_sensor->delete_client(GRAVITY_EVENT_RAW_DATA_REPORT_ON_TIME); + m_gravity_sensor->delete_interval((int)this, true); m_gravity_sensor->stop(); + + m_accel_sensor->delete_client(ACCELEROMETER_EVENT_RAW_DATA_REPORT_ON_TIME); + m_accel_sensor->delete_interval((int)this, true); + m_accel_sensor->stop(); + deactivate(); return true; } @@ -96,6 +179,8 @@ bool linear_accel_sensor::add_interval(int client_id, unsigned int interval) { AUTOLOCK(m_mutex); m_gravity_sensor->add_interval(client_id, interval, true); + m_accel_sensor->add_interval((int)this , interval, true); + return sensor_base::add_interval(client_id, interval, true); } @@ -103,27 +188,62 @@ bool linear_accel_sensor::delete_interval(int client_id) { AUTOLOCK(m_mutex); m_gravity_sensor->delete_interval(client_id, true); + m_accel_sensor->delete_interval(client_id, true); + return sensor_base::delete_interval(client_id, true); } void linear_accel_sensor::synthesize(const sensor_event_t &event, vector &outs) { - vector gravity_event; sensor_event_t lin_accel_event; - ((virtual_sensor *)m_gravity_sensor)->synthesize(event, gravity_event); - if (!gravity_event.empty() && event.event_type == ACCELEROMETER_EVENT_RAW_DATA_REPORT_ON_TIME) { + const float MIN_DELIVERY_DIFF_FACTOR = 0.75f; + unsigned long long diff_time; + + if (event.event_type == ACCELEROMETER_EVENT_RAW_DATA_REPORT_ON_TIME) { + diff_time = event.data.timestamp - m_timestamp; + + if (m_timestamp && (diff_time < m_interval * MIN_DELIVERY_DIFF_FACTOR)) + return; + + m_accel.m_data.m_vec[0] = m_accel_rotation_direction_compensation[0] * (event.data.values[0] - m_accel_static_bias[0]) / m_accel_scale; + m_accel.m_data.m_vec[1] = m_accel_rotation_direction_compensation[1] * (event.data.values[1] - m_accel_static_bias[1]) / m_accel_scale; + m_accel.m_data.m_vec[2] = m_accel_rotation_direction_compensation[2] * (event.data.values[2] - m_accel_static_bias[2]) / m_accel_scale; + + m_accel.m_time_stamp = event.data.timestamp; + + m_enable_linear_accel |= ACCELEROMETER_ENABLED; + } + else if (event.event_type == GRAVITY_EVENT_RAW_DATA_REPORT_ON_TIME) { + diff_time = event.data.timestamp - m_timestamp; + + if (m_timestamp && (diff_time < m_interval * MIN_DELIVERY_DIFF_FACTOR)) + return; + + m_gravity.m_data.m_vec[0] = event.data.values[0]; + m_gravity.m_data.m_vec[1] = event.data.values[1]; + m_gravity.m_data.m_vec[2] = event.data.values[2]; + + m_gravity.m_time_stamp = event.data.timestamp; + + m_enable_linear_accel |= GRAVITY_ENABLED; + } + + if (m_enable_linear_accel == LINEAR_ACCEL_ENABLED) { + m_enable_linear_accel = 0; + m_timestamp = get_timestamp(); + AUTOLOCK(m_value_mutex); - lin_accel_event.data.values_num = 3; - lin_accel_event.data.timestamp = gravity_event[0].data.timestamp; + lin_accel_event.sensor_id = get_id(); lin_accel_event.event_type = LINEAR_ACCEL_EVENT_RAW_DATA_REPORT_ON_TIME; - lin_accel_event.data.data_accuracy = SENSOR_ACCURACY_GOOD; - lin_accel_event.data.data_unit_idx = SENSOR_UNIT_METRE_PER_SECOND_SQUARED; - lin_accel_event.data.values[0] = event.data.values[0] - gravity_event[0].data.values[0]; - lin_accel_event.data.values[1] = event.data.values[1] - gravity_event[0].data.values[1]; - lin_accel_event.data.values[2] = event.data.values[2] - gravity_event[0].data.values[2]; - outs.push_back(lin_accel_event); + lin_accel_event.data.value_count = 3; + lin_accel_event.data.timestamp = m_timestamp; + lin_accel_event.data.accuracy = SENSOR_ACCURACY_GOOD; + lin_accel_event.data.values[0] = m_linear_accel_sign_compensation[0] * (m_accel.m_data.m_vec[0] - m_gravity.m_data.m_vec[0]); + lin_accel_event.data.values[1] = m_linear_accel_sign_compensation[1] * (m_accel.m_data.m_vec[1] - m_gravity.m_data.m_vec[1]); + lin_accel_event.data.values[2] = m_linear_accel_sign_compensation[2] * (m_accel.m_data.m_vec[2] - m_gravity.m_data.m_vec[2]); + push(lin_accel_event); } return; @@ -135,27 +255,28 @@ int linear_accel_sensor::get_sensor_data(const unsigned int event_type, sensor_d ((virtual_sensor *)m_gravity_sensor)->get_sensor_data(GRAVITY_EVENT_RAW_DATA_REPORT_ON_TIME, gravity_data); m_accel_sensor->get_sensor_data(ACCELEROMETER_BASE_DATA_SET, accel_data); + accel_data.values[0] = m_accel_rotation_direction_compensation[0] * (accel_data.values[0] - m_accel_static_bias[0]) / m_accel_scale; + accel_data.values[1] = m_accel_rotation_direction_compensation[1] * (accel_data.values[1] - m_accel_static_bias[1]) / m_accel_scale; + accel_data.values[2] = m_accel_rotation_direction_compensation[2] * (accel_data.values[2] - m_accel_static_bias[2]) / m_accel_scale; + if (event_type != LINEAR_ACCEL_EVENT_RAW_DATA_REPORT_ON_TIME) return -1; - data.data_accuracy = SENSOR_ACCURACY_GOOD; - data.data_unit_idx = SENSOR_UNIT_METRE_PER_SECOND_SQUARED; - data.timestamp = gravity_data.timestamp; - data.values[0] = accel_data.values[0] - gravity_data.values[0]; - data.values[1] = accel_data.values[1] - gravity_data.values[1]; - data.values[2] = accel_data.values[2] - gravity_data.values[2]; - data.values_num = 3; + data.accuracy = SENSOR_ACCURACY_GOOD; + data.timestamp = get_timestamp(); + data.values[0] = m_linear_accel_sign_compensation[0] * (accel_data.values[0] - gravity_data.values[0]); + data.values[1] = m_linear_accel_sign_compensation[1] * (accel_data.values[1] - gravity_data.values[1]); + data.values[2] = m_linear_accel_sign_compensation[2] * (accel_data.values[2] - gravity_data.values[2]); + data.value_count = 3; return 0; } -bool linear_accel_sensor::get_properties(const unsigned int type, sensor_properties_t &properties) +bool linear_accel_sensor::get_properties(sensor_properties_t &properties) { - m_gravity_sensor->get_properties(type, properties); - - if (type != LINEAR_ACCEL_EVENT_RAW_DATA_REPORT_ON_TIME) - return true; + m_gravity_sensor->get_properties(properties); + properties.name = "Linear Acceleration Sensor"; + properties.vendor = m_vendor; - strncpy(properties.sensor_name, "Linear Accelerometer Sensor", MAX_KEY_LENGTH); return true; } diff --git a/src/linear_accel/linear_accel_sensor.h b/src/linear_accel/linear_accel_sensor.h index 1616392..273fa84 100755 --- a/src/linear_accel/linear_accel_sensor.h +++ b/src/linear_accel/linear_accel_sensor.h @@ -20,15 +20,11 @@ #ifndef _LINEAR_ACCEL_SENSOR_H_ #define _LINEAR_ACCEL_SENSOR_H_ -#include -#include -#include +#include #include +#include -using std::string; - -class linear_accel_sensor : public virtual_sensor -{ +class linear_accel_sensor : public virtual_sensor { public: linear_accel_sensor(); virtual ~linear_accel_sensor(); @@ -36,27 +32,39 @@ public: bool init(); sensor_type_t get_type(void); - static bool working(void *inst); - - bool on_start(void); - bool on_stop(void); - void synthesize(const sensor_event_t &event, vector &outs); bool add_interval(int client_id, unsigned int interval); bool delete_interval(int client_id); int get_sensor_data(const unsigned int event_type, sensor_data_t &data); - bool get_properties(const unsigned int type, sensor_properties_t &properties); + bool get_properties(sensor_properties_t &properties); private: sensor_base *m_accel_sensor; sensor_base *m_gravity_sensor; cmutex m_value_mutex; + sensor_data m_accel; + sensor_data m_gravity; + float m_x; float m_y; float m_z; - unsigned long long m_time; + unsigned long long m_timestamp; + unsigned int m_interval; + + unsigned int m_enable_linear_accel; + + string m_vendor; + string m_raw_data_unit; + int m_default_sampling_time; + float m_accel_static_bias[3]; + int m_accel_rotation_direction_compensation[3]; + float m_accel_scale; + int m_linear_accel_sign_compensation[3]; + + bool on_start(void); + bool on_stop(void); }; -#endif /*_LINEAR_ACCEL_SENSOR_H_*/ +#endif -- 2.7.4