From 453331489e51e420efa6b32102a7b376a209abe6 Mon Sep 17 00:00:00 2001 From: Ramasamy Date: Fri, 21 Nov 2014 09:27:29 +0530 Subject: [PATCH] Updating Orientation Virtual Sensor Code - Updating Orientation sensor after testing on SDK - Updating based on latest public code - Updating for new XML based configuration file Change-Id: I089d201406e03f283ed6a9bac2b1dbb8241bc219 --- packaging/sensord.spec | 2 +- src/orientation/orientation_sensor.cpp | 230 +++++++++++++++++++++++++-------- src/orientation/orientation_sensor.h | 30 ++++- 3 files changed, 202 insertions(+), 60 deletions(-) diff --git a/packaging/sensord.spec b/packaging/sensord.spec index eb9d4c2..78f5b98 100755 --- a/packaging/sensord.spec +++ b/packaging/sensord.spec @@ -15,7 +15,7 @@ Source2: sensord.socket %define geo_state ON %define pressure_state OFF %define temperature_state OFF -%define orientation_state OFF +%define orientation_state ON %define gravity_state OFF %define linear_accel_state OFF diff --git a/src/orientation/orientation_sensor.cpp b/src/orientation/orientation_sensor.cpp index f4fbaa1..b1222e2 100755 --- a/src/orientation/orientation_sensor.cpp +++ b/src/orientation/orientation_sensor.cpp @@ -30,8 +30,10 @@ #include #include #include +#include #define SENSOR_NAME "ORIENTATION_SENSOR" +#define SENSOR_TYPE_ORIENTATION "ORIENTATION" #define ACCELEROMETER_ENABLED 0x01 #define GYROSCOPE_ENABLED 0x02 @@ -39,26 +41,28 @@ #define ORIENTATION_ENABLED 7 #define INITIAL_VALUE -1 -#define INITIAL_TIME 0 -// Below Defines const variables to be input from sensor config files once code is stabilized -#define SAMPLING_TIME 100 #define MS_TO_US 1000 -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; +#define AZIMUTH_OFFSET 360 + +#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_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" void pre_process_data(sensor_data &data_out, const float *data_in, float *bias, int *sign, float scale) { @@ -73,13 +77,129 @@ orientation_sensor::orientation_sensor() , m_magnetic_sensor(NULL) , m_roll(INITIAL_VALUE) , m_pitch(INITIAL_VALUE) -, m_yaw(INITIAL_VALUE) +, m_azimuth(INITIAL_VALUE) { + cvirtual_sensor_config &config = cvirtual_sensor_config::get_instance(); + m_name = string(SENSOR_NAME); register_supported_event(ORIENTATION_EVENT_RAW_DATA_REPORT_ON_TIME); - m_interval = SAMPLING_TIME * MS_TO_US; m_timestamp = get_timestamp(); m_enable_orientation = 0; + + if (!config.get(SENSOR_TYPE_ORIENTATION, ELEMENT_VENDOR, m_vendor)) { + ERR("[VENDOR] is empty\n"); + throw ENXIO; + } + + INFO("m_vendor = %s", m_vendor.c_str()); + + if (!config.get(SENSOR_TYPE_ORIENTATION, 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_ORIENTATION, 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_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; + } + + INFO("m_azimuth_rotation_compensation = %d", m_azimuth_rotation_compensation); + + if (!config.get(SENSOR_TYPE_ORIENTATION, ELEMENT_PITCH_ROTATION_COMPENSATION, &m_pitch_rotation_compensation)) { + ERR("[PITCH_ROTATION_COMPENSATION] is empty\n"); + throw ENXIO; + } + + INFO("m_pitch_rotation_compensation = %d", m_pitch_rotation_compensation); + + if (!config.get(SENSOR_TYPE_ORIENTATION, ELEMENT_ROLL_ROTATION_COMPENSATION, &m_roll_rotation_compensation)) { + ERR("[ROLL_ROTATION_COMPENSATION] is empty\n"); + throw ENXIO; + } + + INFO("m_roll_rotation_compensation = %d", m_roll_rotation_compensation); + + m_interval = m_default_sampling_time * MS_TO_US; + } orientation_sensor::~orientation_sensor() @@ -181,7 +301,7 @@ void orientation_sensor::synthesize(const sensor_event_t &event, vector= 0) + orientation_event.data.values[0] = euler_orientation.m_ang.m_vec[2]; + else + orientation_event.data.values[0] = euler_orientation.m_ang.m_vec[2] + AZIMUTH_OFFSET; + + push(orientation_event); } return; @@ -257,40 +380,41 @@ int orientation_sensor::get_sensor_data(const unsigned int event_type, sensor_da 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, magnetic_data); - pre_process_data(accel, accel_data.values, bias_accel, sign_accel, scale_accel); - pre_process_data(gyro, gyro_data.values, bias_gyro, sign_gyro, scale_gyro); - pre_process_data(magnetic, magnetic_data.values, bias_magnetic, sign_magnetic, scale_magnetic); + 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; - 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; + m_orientation.m_pitch_phase_compensation = m_pitch_rotation_compensation; + m_orientation.m_roll_phase_compensation = m_roll_rotation_compensation; + m_orientation.m_azimuth_phase_compensation = m_azimuth_rotation_compensation; + m_orientation.m_magnetic_alignment_factor = m_magnetic_alignment_factor; euler_orientation = m_orientation.get_orientation(accel, gyro, magnetic); - data.data_accuracy = SENSOR_ACCURACY_GOOD; - data.data_unit_idx = SENSOR_UNIT_DEGREE; + data.accuracy = SENSOR_ACCURACY_GOOD; data.timestamp = get_timestamp(); - data.values[0] = euler_orientation.m_ang.m_vec[0]; - data.values[1] = euler_orientation.m_ang.m_vec[1]; - data.values[2] = euler_orientation.m_ang.m_vec[2]; - data.values_num = 3; + 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; return 0; } bool orientation_sensor::get_properties(sensor_properties_t &properties) { - properties.sensor_unit_idx = SENSOR_UNIT_DEGREE; - properties.sensor_min_range = -180; - properties.sensor_max_range = 180; - properties.sensor_resolution = 1; + properties.min_range = -180; + properties.max_range = 360; + properties.resolution = 1; - strncpy(properties.sensor_vendor, "Samsung", MAX_KEY_LENGTH); - strncpy(properties.sensor_name, SENSOR_NAME, MAX_KEY_LENGTH); + properties.vendor = "Samsung"; + properties.name = SENSOR_NAME; return true; } diff --git a/src/orientation/orientation_sensor.h b/src/orientation/orientation_sensor.h index f295ccc..ed61891 100755 --- a/src/orientation/orientation_sensor.h +++ b/src/orientation/orientation_sensor.h @@ -20,18 +20,16 @@ #ifndef _ORIENTATION_SENSOR_H_ #define _ORIENTATION_SENSOR_H_ -#include +#include #include #include class orientation_sensor : public virtual_sensor { public: orientation_sensor(); - ~orientation_sensor(); + virtual ~orientation_sensor(); bool init(void); - bool on_start(void); - bool on_stop(void); void synthesize(const sensor_event_t &event, vector &outs); @@ -59,9 +57,29 @@ private: float m_roll; float m_pitch; - float m_yaw; + float m_azimuth; unsigned long long m_timestamp; 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_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; + + bool on_start(void); + bool on_stop(void); }; -#endif /* _ORIENTATION_SENSOR_H_ */ +#endif -- 2.7.4