From: Ramasamy Date: Fri, 19 Sep 2014 11:24:59 +0000 (+0530) Subject: Updating orientation virtual sensor based on RD-PQ testing X-Git-Tag: submit/tizen/20150113.012540~134^2 X-Git-Url: http://review.tizen.org/git/?a=commitdiff_plain;h=822e75bfee7deb90518c73d844fef1ef94dca6ac;p=platform%2Fcore%2Fsystem%2Fsensord.git 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 --- 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 &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 &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, vectorget_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 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 --- 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 --- a/src/sensor_fusion/orientation_filter.cpp +++ b/src/sensor_fusion/orientation_filter.cpp @@ -134,7 +134,7 @@ template inline void orientation_filter::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 acc_e(V1x3S, arr_acc_e); vect mag_e(V1x3S, arr_mag_e);