4 * Copyright (c) 2015 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>
28 #include <sensor_log.h>
29 #include <tilt_sensor.h>
30 #include <sensor_loader.h>
31 #include <orientation_filter.h>
32 #include <virtual_sensor_config.h>
37 #define SENSOR_NAME "TILT_SENSOR"
38 #define SENSOR_TYPE_TILT "TILT"
40 #define MIN_DELIVERY_DIFF_FACTOR 0.75f
42 #define INITIAL_VALUE -1
46 #define ELEMENT_NAME "NAME"
47 #define ELEMENT_VENDOR "VENDOR"
48 #define ELEMENT_RAW_DATA_UNIT "RAW_DATA_UNIT"
49 #define ELEMENT_DEFAULT_SAMPLING_TIME "DEFAULT_SAMPLING_TIME"
50 #define ELEMENT_PITCH_ROTATION_COMPENSATION "PITCH_ROTATION_COMPENSATION"
51 #define ELEMENT_ROLL_ROTATION_COMPENSATION "ROLL_ROTATION_COMPENSATION"
53 tilt_sensor::tilt_sensor()
54 : m_accel_sensor(NULL)
55 , m_fusion_sensor(NULL)
58 virtual_sensor_config &config = virtual_sensor_config::get_instance();
60 m_name = string(SENSOR_NAME);
61 register_supported_event(TILT_RAW_DATA_EVENT);
63 if (!config.get(SENSOR_TYPE_TILT, ELEMENT_VENDOR, m_vendor)) {
64 _W("[VENDOR] is empty\n");
68 _I("m_vendor = %s", m_vendor.c_str());
70 if (!config.get(SENSOR_TYPE_TILT, ELEMENT_RAW_DATA_UNIT, m_raw_data_unit)) {
71 _W("[RAW_DATA_UNIT] is empty\n");
75 _I("m_raw_data_unit = %s", m_raw_data_unit.c_str());
77 if (!config.get(SENSOR_TYPE_TILT, ELEMENT_DEFAULT_SAMPLING_TIME, &m_default_sampling_time)) {
78 _W("[DEFAULT_SAMPLING_TIME] is empty\n");
82 _I("m_default_sampling_time = %d", m_default_sampling_time);
84 if (!config.get(SENSOR_TYPE_TILT, ELEMENT_PITCH_ROTATION_COMPENSATION, &m_pitch_rotation_compensation)) {
85 _W("[PITCH_ROTATION_COMPENSATION] is empty\n");
89 _I("m_pitch_rotation_compensation = %d", m_pitch_rotation_compensation);
91 if (!config.get(SENSOR_TYPE_TILT, ELEMENT_ROLL_ROTATION_COMPENSATION, &m_roll_rotation_compensation)) {
92 _W("[ROLL_ROTATION_COMPENSATION] is empty\n");
96 _I("m_roll_rotation_compensation = %d", m_roll_rotation_compensation);
98 m_interval = m_default_sampling_time * MS_TO_US;
101 tilt_sensor::~tilt_sensor()
103 _I("tilt_sensor is destroyed!\n");
106 bool tilt_sensor::init(void)
108 m_accel_sensor = sensor_loader::get_instance().get_sensor(ACCELEROMETER_SENSOR);
109 m_fusion_sensor = sensor_loader::get_instance().get_sensor(FUSION_SENSOR);
111 if (!m_accel_sensor || !m_fusion_sensor) {
112 _W("Failed to load sensors, accel: %#x, fusion: %#x",
113 m_accel_sensor, m_fusion_sensor);
117 _I("%s is created!\n", sensor_base::get_name());
122 void tilt_sensor::get_types(vector<sensor_type_t> &types)
124 types.push_back(TILT_SENSOR);
127 bool tilt_sensor::on_start(void)
131 m_accel_sensor->add_client(ACCELEROMETER_RAW_DATA_EVENT);
132 m_accel_sensor->add_interval((intptr_t)this, (m_interval/MS_TO_US), false);
133 m_accel_sensor->start();
135 m_fusion_sensor->register_supported_event(FUSION_EVENT);
136 m_fusion_sensor->register_supported_event(FUSION_TILT_ENABLED);
137 m_fusion_sensor->add_client(FUSION_EVENT);
138 m_fusion_sensor->add_interval((intptr_t)this, (m_interval/MS_TO_US), false);
139 m_fusion_sensor->start();
145 bool tilt_sensor::on_stop(void)
149 m_accel_sensor->delete_client(ACCELEROMETER_RAW_DATA_EVENT);
150 m_accel_sensor->delete_interval((intptr_t)this, false);
151 m_accel_sensor->stop();
153 m_fusion_sensor->delete_client(FUSION_EVENT);
154 m_fusion_sensor->delete_interval((intptr_t)this, false);
155 m_fusion_sensor->unregister_supported_event(FUSION_EVENT);
156 m_fusion_sensor->unregister_supported_event(FUSION_TILT_ENABLED);
157 m_fusion_sensor->stop();
163 bool tilt_sensor::add_interval(int client_id, unsigned int interval)
167 m_accel_sensor->add_interval(client_id, interval, false);
168 m_fusion_sensor->add_interval(client_id, interval, false);
170 return sensor_base::add_interval(client_id, interval, false);
173 bool tilt_sensor::delete_interval(int client_id)
177 m_accel_sensor->delete_interval(client_id, false);
178 m_fusion_sensor->delete_interval(client_id, false);
180 return sensor_base::delete_interval(client_id, false);
183 void tilt_sensor::synthesize(const sensor_event_t &event, vector<sensor_event_t> &outs)
185 sensor_event_t tilt_event;
186 unsigned long long diff_time;
188 if (event.event_type == FUSION_EVENT) {
189 diff_time = event.data.timestamp - m_time;
191 if (m_time && (diff_time < m_interval * MIN_DELIVERY_DIFF_FACTOR))
194 quaternion<float> quat(event.data.values[0], event.data.values[1],
195 event.data.values[2], event.data.values[3]);
197 euler_angles<float> euler = quat2euler(quat);
199 if(m_raw_data_unit == "DEGREES") {
200 euler = rad2deg(euler);
203 euler.m_ang.m_vec[0] *= m_pitch_rotation_compensation;
204 euler.m_ang.m_vec[1] *= m_roll_rotation_compensation;
206 m_time = get_timestamp();
207 tilt_event.sensor_id = get_id();
208 tilt_event.event_type = TILT_RAW_DATA_EVENT;
209 tilt_event.data.accuracy = event.data.accuracy;
210 tilt_event.data.timestamp = m_time;
211 tilt_event.data.value_count = 2;
212 tilt_event.data.values[0] = euler.m_ang.m_vec[0];
213 tilt_event.data.values[1] = euler.m_ang.m_vec[1];
221 int tilt_sensor::get_sensor_data(const unsigned int event_type, sensor_data_t &data)
223 sensor_data_t fusion_data;
225 if (event_type != TILT_RAW_DATA_EVENT)
228 m_fusion_sensor->get_sensor_data(FUSION_TILT_ENABLED, fusion_data);
230 quaternion<float> quat(fusion_data.values[0], fusion_data.values[1],
231 fusion_data.values[2], fusion_data.values[3]);
233 euler_angles<float> euler = quat2euler(quat);
235 if(m_raw_data_unit == "DEGREES") {
236 euler = rad2deg(euler);
239 data.accuracy = fusion_data.accuracy;
240 data.timestamp = get_timestamp();
241 data.value_count = 2;
242 data.values[0] = euler.m_ang.m_vec[0];
243 data.values[1] = euler.m_ang.m_vec[1];
245 data.values[0] *= m_pitch_rotation_compensation;
246 data.values[1] *= m_roll_rotation_compensation;
251 bool tilt_sensor::get_properties(sensor_type_t sensor_type, sensor_properties_s &properties)
253 if(m_raw_data_unit == "DEGREES") {
254 properties.min_range = -180;
255 properties.max_range = 180;
257 properties.min_range = -PI;
258 properties.max_range = PI;
260 properties.resolution = 0.000001;
261 properties.vendor = m_vendor;
262 properties.name = SENSOR_NAME;
263 properties.min_interval = 1;
264 properties.fifo_count = 0;
265 properties.max_batch_count = 0;