4 * Copyright (c) 2014 Samsung Electronics Co., Ltd.
6 * Licensed under the Apache License, Version 2.0 (the "License");
7 * you may not use this file except in compliance with the License.
8 * You may obtain a copy of the License at
10 * http://www.apache.org/licenses/LICENSE-2.0
12 * Unless required by applicable law or agreed to in writing, software
13 * distributed under the License is distributed on an "AS IS" BASIS,
14 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
15 * See the License for the specific language governing permissions and
16 * limitations under the License.
26 #include <sys/types.h>
29 #include <sf_common.h>
30 #include <orientation_sensor.h>
31 #include <sensor_plugin_loader.h>
32 #include <orientation_filter.h>
33 #include <cvirtual_sensor_config.h>
35 #define SENSOR_NAME "ORIENTATION_SENSOR"
36 #define SENSOR_TYPE_ORIENTATION "ORIENTATION"
38 #define ACCELEROMETER_ENABLED 0x01
39 #define GYROSCOPE_ENABLED 0x02
40 #define GEOMAGNETIC_ENABLED 0x04
41 #define ORIENTATION_ENABLED 7
43 #define INITIAL_VALUE -1
46 #define MIN_DELIVERY_DIFF_FACTOR 0.75f
49 #define AZIMUTH_OFFSET_DEGREES 360
50 #define AZIMUTH_OFFSET_RADIANS (2 * PI)
52 #define ELEMENT_NAME "NAME"
53 #define ELEMENT_VENDOR "VENDOR"
54 #define ELEMENT_RAW_DATA_UNIT "RAW_DATA_UNIT"
55 #define ELEMENT_DEFAULT_SAMPLING_TIME "DEFAULT_SAMPLING_TIME"
56 #define ELEMENT_ACCEL_STATIC_BIAS "ACCEL_STATIC_BIAS"
57 #define ELEMENT_GYRO_STATIC_BIAS "GYRO_STATIC_BIAS"
58 #define ELEMENT_GEOMAGNETIC_STATIC_BIAS "GEOMAGNETIC_STATIC_BIAS"
59 #define ELEMENT_ACCEL_ROTATION_DIRECTION_COMPENSATION "ACCEL_ROTATION_DIRECTION_COMPENSATION"
60 #define ELEMENT_GYRO_ROTATION_DIRECTION_COMPENSATION "GYRO_ROTATION_DIRECTION_COMPENSATION"
61 #define ELEMENT_GEOMAGNETIC_ROTATION_DIRECTION_COMPENSATION "GEOMAGNETIC_ROTATION_DIRECTION_COMPENSATION"
62 #define ELEMENT_ACCEL_SCALE "ACCEL_SCALE"
63 #define ELEMENT_GYRO_SCALE "GYRO_SCALE"
64 #define ELEMENT_GEOMAGNETIC_SCALE "GEOMAGNETIC_SCALE"
65 #define ELEMENT_MAGNETIC_ALIGNMENT_FACTOR "MAGNETIC_ALIGNMENT_FACTOR"
66 #define ELEMENT_PITCH_ROTATION_COMPENSATION "PITCH_ROTATION_COMPENSATION"
67 #define ELEMENT_ROLL_ROTATION_COMPENSATION "ROLL_ROTATION_COMPENSATION"
68 #define ELEMENT_AZIMUTH_ROTATION_COMPENSATION "AZIMUTH_ROTATION_COMPENSATION"
70 void pre_process_data(sensor_data<float> &data_out, const float *data_in, float *bias, int *sign, float scale)
72 data_out.m_data.m_vec[0] = sign[0] * (data_in[0] - bias[0]) / scale;
73 data_out.m_data.m_vec[1] = sign[1] * (data_in[1] - bias[1]) / scale;
74 data_out.m_data.m_vec[2] = sign[2] * (data_in[2] - bias[2]) / scale;
77 orientation_sensor::orientation_sensor()
78 : m_accel_sensor(NULL)
80 , m_magnetic_sensor(NULL)
81 , m_roll(INITIAL_VALUE)
82 , m_pitch(INITIAL_VALUE)
83 , m_azimuth(INITIAL_VALUE)
85 cvirtual_sensor_config &config = cvirtual_sensor_config::get_instance();
87 m_name = string(SENSOR_NAME);
88 register_supported_event(ORIENTATION_EVENT_RAW_DATA_REPORT_ON_TIME);
89 m_timestamp = get_timestamp();
90 m_enable_orientation = 0;
92 if (!config.get(SENSOR_TYPE_ORIENTATION, ELEMENT_VENDOR, m_vendor)) {
93 ERR("[VENDOR] is empty\n");
97 INFO("m_vendor = %s", m_vendor.c_str());
99 if (!config.get(SENSOR_TYPE_ORIENTATION, ELEMENT_RAW_DATA_UNIT, m_raw_data_unit)) {
100 ERR("[RAW_DATA_UNIT] is empty\n");
104 INFO("m_raw_data_unit = %s", m_raw_data_unit.c_str());
106 if (!config.get(SENSOR_TYPE_ORIENTATION, ELEMENT_DEFAULT_SAMPLING_TIME, &m_default_sampling_time)) {
107 ERR("[DEFAULT_SAMPLING_TIME] is empty\n");
111 INFO("m_default_sampling_time = %d", m_default_sampling_time);
113 if (!config.get(SENSOR_TYPE_ORIENTATION, ELEMENT_ACCEL_STATIC_BIAS, m_accel_static_bias, 3)) {
114 ERR("[ACCEL_STATIC_BIAS] is empty\n");
118 INFO("m_accel_static_bias = (%f, %f, %f)", m_accel_static_bias[0], m_accel_static_bias[1], m_accel_static_bias[2]);
120 if (!config.get(SENSOR_TYPE_ORIENTATION, ELEMENT_GYRO_STATIC_BIAS, m_gyro_static_bias,3)) {
121 ERR("[GYRO_STATIC_BIAS] is empty\n");
125 INFO("m_gyro_static_bias = (%f, %f, %f)", m_gyro_static_bias[0], m_gyro_static_bias[1], m_gyro_static_bias[2]);
127 if (!config.get(SENSOR_TYPE_ORIENTATION, ELEMENT_GEOMAGNETIC_STATIC_BIAS, m_geomagnetic_static_bias, 3)) {
128 ERR("[GEOMAGNETIC_STATIC_BIAS] is empty\n");
132 INFO("m_geomagnetic_static_bias = (%f, %f, %f)", m_geomagnetic_static_bias[0], m_geomagnetic_static_bias[1], m_geomagnetic_static_bias[2]);
134 if (!config.get(SENSOR_TYPE_ORIENTATION, ELEMENT_ACCEL_ROTATION_DIRECTION_COMPENSATION, m_accel_rotation_direction_compensation, 3)) {
135 ERR("[ACCEL_ROTATION_DIRECTION_COMPENSATION] is empty\n");
139 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]);
141 if (!config.get(SENSOR_TYPE_ORIENTATION, ELEMENT_GYRO_ROTATION_DIRECTION_COMPENSATION, m_gyro_rotation_direction_compensation, 3)) {
142 ERR("[GYRO_ROTATION_DIRECTION_COMPENSATION] is empty\n");
146 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]);
148 if (!config.get(SENSOR_TYPE_ORIENTATION, ELEMENT_GEOMAGNETIC_ROTATION_DIRECTION_COMPENSATION, m_geomagnetic_rotation_direction_compensation, 3)) {
149 ERR("[GEOMAGNETIC_ROTATION_DIRECTION_COMPENSATION] is empty\n");
153 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]);
155 if (!config.get(SENSOR_TYPE_ORIENTATION, ELEMENT_ACCEL_SCALE, &m_accel_scale)) {
156 ERR("[ACCEL_SCALE] is empty\n");
160 INFO("m_accel_scale = %f", m_accel_scale);
162 if (!config.get(SENSOR_TYPE_ORIENTATION, ELEMENT_GYRO_SCALE, &m_gyro_scale)) {
163 ERR("[GYRO_SCALE] is empty\n");
167 INFO("m_gyro_scale = %f", m_gyro_scale);
169 if (!config.get(SENSOR_TYPE_ORIENTATION, ELEMENT_GEOMAGNETIC_SCALE, &m_geomagnetic_scale)) {
170 ERR("[GEOMAGNETIC_SCALE] is empty\n");
174 INFO("m_geomagnetic_scale = %f", m_geomagnetic_scale);
176 if (!config.get(SENSOR_TYPE_ORIENTATION, ELEMENT_MAGNETIC_ALIGNMENT_FACTOR, &m_magnetic_alignment_factor)) {
177 ERR("[MAGNETIC_ALIGNMENT_FACTOR] is empty\n");
181 INFO("m_magnetic_alignment_factor = %d", m_magnetic_alignment_factor);
183 if (!config.get(SENSOR_TYPE_ORIENTATION, ELEMENT_AZIMUTH_ROTATION_COMPENSATION, &m_azimuth_rotation_compensation)) {
184 ERR("[AZIMUTH_ROTATION_COMPENSATION] is empty\n");
188 INFO("m_azimuth_rotation_compensation = %d", m_azimuth_rotation_compensation);
190 if (!config.get(SENSOR_TYPE_ORIENTATION, ELEMENT_PITCH_ROTATION_COMPENSATION, &m_pitch_rotation_compensation)) {
191 ERR("[PITCH_ROTATION_COMPENSATION] is empty\n");
195 INFO("m_pitch_rotation_compensation = %d", m_pitch_rotation_compensation);
197 if (!config.get(SENSOR_TYPE_ORIENTATION, ELEMENT_ROLL_ROTATION_COMPENSATION, &m_roll_rotation_compensation)) {
198 ERR("[ROLL_ROTATION_COMPENSATION] is empty\n");
202 INFO("m_roll_rotation_compensation = %d", m_roll_rotation_compensation);
204 m_interval = m_default_sampling_time * MS_TO_US;
208 orientation_sensor::~orientation_sensor()
210 INFO("orientation_sensor is destroyed!\n");
213 bool orientation_sensor::init(void)
215 m_accel_sensor = sensor_plugin_loader::get_instance().get_sensor(ACCELEROMETER_SENSOR);
216 m_gyro_sensor = sensor_plugin_loader::get_instance().get_sensor(GYROSCOPE_SENSOR);
217 m_magnetic_sensor = sensor_plugin_loader::get_instance().get_sensor(GEOMAGNETIC_SENSOR);
219 if (!m_accel_sensor || !m_gyro_sensor || !m_magnetic_sensor) {
220 ERR("Failed to load sensors, accel: 0x%x, gyro: 0x%x, mag: 0x%x",
221 m_accel_sensor, m_gyro_sensor, m_magnetic_sensor);
225 INFO("%s is created!", sensor_base::get_name());
229 sensor_type_t orientation_sensor::get_type(void)
231 return ORIENTATION_SENSOR;
234 bool orientation_sensor::on_start(void)
238 m_accel_sensor->add_client(ACCELEROMETER_EVENT_RAW_DATA_REPORT_ON_TIME);
239 m_accel_sensor->add_interval((int)this, (m_interval/MS_TO_US), true);
240 m_accel_sensor->start();
241 m_gyro_sensor->add_client(GYROSCOPE_EVENT_RAW_DATA_REPORT_ON_TIME);
242 m_gyro_sensor->add_interval((int)this, (m_interval/MS_TO_US), true);
243 m_gyro_sensor->start();
244 m_magnetic_sensor->add_client(GEOMAGNETIC_EVENT_RAW_DATA_REPORT_ON_TIME);
245 m_magnetic_sensor->add_interval((int)this, (m_interval/MS_TO_US), true);
246 m_magnetic_sensor->start();
252 bool orientation_sensor::on_stop(void)
256 m_accel_sensor->delete_client(ACCELEROMETER_EVENT_RAW_DATA_REPORT_ON_TIME);
257 m_accel_sensor->delete_interval((int)this, true);
258 m_accel_sensor->stop();
259 m_gyro_sensor->delete_client(GYROSCOPE_EVENT_RAW_DATA_REPORT_ON_TIME);
260 m_gyro_sensor->delete_interval((int)this, true);
261 m_gyro_sensor->stop();
262 m_magnetic_sensor->delete_client(GEOMAGNETIC_EVENT_RAW_DATA_REPORT_ON_TIME);
263 m_magnetic_sensor->delete_interval((int)this, true);
264 m_magnetic_sensor->stop();
270 bool orientation_sensor::add_interval(int client_id, unsigned int interval)
274 m_accel_sensor->add_interval(client_id, interval, true);
275 m_gyro_sensor->add_interval(client_id, interval, true);
276 m_magnetic_sensor->add_interval(client_id, interval, true);
278 return sensor_base::add_interval(client_id, interval, true);
281 bool orientation_sensor::delete_interval(int client_id)
285 m_accel_sensor->delete_interval(client_id, true);
286 m_gyro_sensor->delete_interval(client_id, true);
287 m_magnetic_sensor->delete_interval(client_id, true);
289 return sensor_base::delete_interval(client_id, true);
292 void orientation_sensor::synthesize(const sensor_event_t &event, vector<sensor_event_t> &outs)
294 unsigned long long diff_time;
296 sensor_event_t orientation_event;
297 euler_angles<float> euler_orientation;
298 float azimuth_offset;
300 if (event.event_type == ACCELEROMETER_EVENT_RAW_DATA_REPORT_ON_TIME) {
301 diff_time = event.data.timestamp - m_timestamp;
303 if (m_timestamp && (diff_time < m_interval * MIN_DELIVERY_DIFF_FACTOR))
306 pre_process_data(m_accel, event.data.values, m_accel_static_bias, m_accel_rotation_direction_compensation, m_accel_scale);
308 m_accel.m_time_stamp = event.data.timestamp;
310 m_enable_orientation |= ACCELEROMETER_ENABLED;
312 else if (event.event_type == GYROSCOPE_EVENT_RAW_DATA_REPORT_ON_TIME) {
313 diff_time = event.data.timestamp - m_timestamp;
315 if (m_timestamp && (diff_time < m_interval * MIN_DELIVERY_DIFF_FACTOR))
318 pre_process_data(m_gyro, event.data.values, m_gyro_static_bias, m_gyro_rotation_direction_compensation, m_gyro_scale);
320 m_gyro.m_time_stamp = event.data.timestamp;
322 m_enable_orientation |= GYROSCOPE_ENABLED;
324 else if (event.event_type == GEOMAGNETIC_EVENT_RAW_DATA_REPORT_ON_TIME) {
325 diff_time = event.data.timestamp - m_timestamp;
327 if (m_timestamp && (diff_time < m_interval * MIN_DELIVERY_DIFF_FACTOR))
330 pre_process_data(m_magnetic, event.data.values, m_geomagnetic_static_bias, m_geomagnetic_rotation_direction_compensation, m_geomagnetic_scale);
332 m_magnetic.m_time_stamp = event.data.timestamp;
334 m_enable_orientation |= GEOMAGNETIC_ENABLED;
337 if (m_enable_orientation == ORIENTATION_ENABLED) {
338 m_enable_orientation = 0;
339 m_timestamp = get_timestamp();
341 m_orientation.m_pitch_phase_compensation = m_pitch_rotation_compensation;
342 m_orientation.m_roll_phase_compensation = m_roll_rotation_compensation;
343 m_orientation.m_azimuth_phase_compensation = m_azimuth_rotation_compensation;
344 m_orientation.m_magnetic_alignment_factor = m_magnetic_alignment_factor;
346 euler_orientation = m_orientation.get_orientation(m_accel, m_gyro, m_magnetic);
348 if(m_raw_data_unit == "DEGREES") {
349 euler_orientation = rad2deg(euler_orientation);
350 azimuth_offset = AZIMUTH_OFFSET_DEGREES;
353 azimuth_offset = AZIMUTH_OFFSET_RADIANS;
356 orientation_event.sensor_id = get_id();
357 orientation_event.event_type = ORIENTATION_EVENT_RAW_DATA_REPORT_ON_TIME;
358 orientation_event.data.accuracy = SENSOR_ACCURACY_GOOD;
359 orientation_event.data.timestamp = m_timestamp;
360 orientation_event.data.value_count = 3;
361 orientation_event.data.values[1] = euler_orientation.m_ang.m_vec[0];
362 orientation_event.data.values[2] = euler_orientation.m_ang.m_vec[1];
363 if (euler_orientation.m_ang.m_vec[2] >= 0)
364 orientation_event.data.values[0] = euler_orientation.m_ang.m_vec[2];
366 orientation_event.data.values[0] = euler_orientation.m_ang.m_vec[2] + azimuth_offset;
368 push(orientation_event);
374 int orientation_sensor::get_sensor_data(const unsigned int event_type, sensor_data_t &data)
376 sensor_data<float> accel;
377 sensor_data<float> gyro;
378 sensor_data<float> magnetic;
380 sensor_data_t accel_data;
381 sensor_data_t gyro_data;
382 sensor_data_t magnetic_data;
384 euler_angles<float> euler_orientation;
385 float azimuth_offset;
387 if (event_type != ORIENTATION_EVENT_RAW_DATA_REPORT_ON_TIME)
390 m_accel_sensor->get_sensor_data(ACCELEROMETER_EVENT_RAW_DATA_REPORT_ON_TIME, accel_data);
391 m_gyro_sensor->get_sensor_data(GYROSCOPE_EVENT_RAW_DATA_REPORT_ON_TIME, gyro_data);
392 m_magnetic_sensor->get_sensor_data(GEOMAGNETIC_EVENT_RAW_DATA_REPORT_ON_TIME, magnetic_data);
394 pre_process_data(accel, accel_data.values, m_accel_static_bias, m_accel_rotation_direction_compensation, m_accel_scale);
395 pre_process_data(gyro, gyro_data.values, m_gyro_static_bias, m_gyro_rotation_direction_compensation, m_gyro_scale);
396 pre_process_data(magnetic, magnetic_data.values, m_geomagnetic_static_bias, m_geomagnetic_rotation_direction_compensation, m_geomagnetic_scale);
397 accel.m_time_stamp = accel_data.timestamp;
398 gyro.m_time_stamp = gyro_data.timestamp;
399 magnetic.m_time_stamp = magnetic_data.timestamp;
401 m_orientation.m_pitch_phase_compensation = m_pitch_rotation_compensation;
402 m_orientation.m_roll_phase_compensation = m_roll_rotation_compensation;
403 m_orientation.m_azimuth_phase_compensation = m_azimuth_rotation_compensation;
404 m_orientation.m_magnetic_alignment_factor = m_magnetic_alignment_factor;
406 euler_orientation = m_orientation.get_orientation(accel, gyro, magnetic);
408 if(m_raw_data_unit == "DEGREES") {
409 euler_orientation = rad2deg(euler_orientation);
410 azimuth_offset = AZIMUTH_OFFSET_DEGREES;
413 azimuth_offset = AZIMUTH_OFFSET_RADIANS;
416 data.accuracy = SENSOR_ACCURACY_GOOD;
417 data.timestamp = get_timestamp();
418 data.values[1] = euler_orientation.m_ang.m_vec[0];
419 data.values[2] = euler_orientation.m_ang.m_vec[1];
420 if (euler_orientation.m_ang.m_vec[2] >= 0)
421 data.values[0] = euler_orientation.m_ang.m_vec[2];
423 data.values[0] = euler_orientation.m_ang.m_vec[2] + azimuth_offset;
424 data.value_count = 3;
429 bool orientation_sensor::get_properties(sensor_properties_t &properties)
431 if(m_raw_data_unit == "DEGREES") {
432 properties.min_range = -180;
433 properties.max_range = 360;
436 properties.min_range = -PI;
437 properties.max_range = 2 * PI;
439 properties.resolution = 0.000001;
441 properties.vendor = m_vendor;
442 properties.name = SENSOR_NAME;
447 extern "C" void *create(void)
449 orientation_sensor *inst;
452 inst = new orientation_sensor();
454 ERR("orientation_sensor class create fail , errno : %d , errstr : %s", err, strerror(err));
461 extern "C" void destroy(void *inst)
463 delete (orientation_sensor *)inst;