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 <linear_accel_sensor.h>
31 #include <sensor_plugin_loader.h>
32 #include <cvirtual_sensor_config.h>
34 #define SENSOR_NAME "LINEAR_ACCEL_SENSOR"
35 #define SENSOR_TYPE_LINEAR_ACCEL "LINEAR_ACCEL"
37 #define ELEMENT_NAME "NAME"
38 #define ELEMENT_VENDOR "VENDOR"
39 #define ELEMENT_RAW_DATA_UNIT "RAW_DATA_UNIT"
40 #define ELEMENT_DEFAULT_SAMPLING_TIME "DEFAULT_SAMPLING_TIME"
41 #define ELEMENT_ACCEL_STATIC_BIAS "ACCEL_STATIC_BIAS"
42 #define ELEMENT_ACCEL_ROTATION_DIRECTION_COMPENSATION "ACCEL_ROTATION_DIRECTION_COMPENSATION"
43 #define ELEMENT_ACCEL_SCALE "ACCEL_SCALE"
44 #define ELEMENT_LINEAR_ACCEL_SIGN_COMPENSATION "LINEAR_ACCEL_SIGN_COMPENSATION"
46 #define INITIAL_VALUE -1
47 #define GRAVITY 9.80665
50 #define MIN_DELIVERY_DIFF_FACTOR 0.75f
52 #define ACCELEROMETER_ENABLED 0x01
53 #define GRAVITY_ENABLED 0x02
54 #define LINEAR_ACCEL_ENABLED 3
56 linear_accel_sensor::linear_accel_sensor()
57 : m_accel_sensor(NULL)
58 , m_gravity_sensor(NULL)
61 cvirtual_sensor_config &config = cvirtual_sensor_config::get_instance();
63 m_name = string(SENSOR_NAME);
64 m_enable_linear_accel = 0;
65 register_supported_event(LINEAR_ACCEL_EVENT_RAW_DATA_REPORT_ON_TIME);
68 if (!config.get(SENSOR_TYPE_LINEAR_ACCEL, ELEMENT_VENDOR, m_vendor)) {
69 ERR("[VENDOR] is empty\n");
73 INFO("m_vendor = %s", m_vendor.c_str());
75 if (!config.get(SENSOR_TYPE_LINEAR_ACCEL, ELEMENT_RAW_DATA_UNIT, m_raw_data_unit)) {
76 ERR("[RAW_DATA_UNIT] is empty\n");
80 INFO("m_raw_data_unit = %s", m_raw_data_unit.c_str());
82 if (!config.get(SENSOR_TYPE_LINEAR_ACCEL, ELEMENT_DEFAULT_SAMPLING_TIME, &m_default_sampling_time)) {
83 ERR("[DEFAULT_SAMPLING_TIME] is empty\n");
87 INFO("m_default_sampling_time = %d", m_default_sampling_time);
89 if (!config.get(SENSOR_TYPE_LINEAR_ACCEL, ELEMENT_ACCEL_STATIC_BIAS, m_accel_static_bias, 3)) {
90 ERR("[ACCEL_STATIC_BIAS] is empty\n");
94 if (!config.get(SENSOR_TYPE_LINEAR_ACCEL, ELEMENT_ACCEL_ROTATION_DIRECTION_COMPENSATION, m_accel_rotation_direction_compensation, 3)) {
95 ERR("[ACCEL_ROTATION_DIRECTION_COMPENSATION] is empty\n");
99 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]);
102 if (!config.get(SENSOR_TYPE_LINEAR_ACCEL, ELEMENT_ACCEL_SCALE, &m_accel_scale)) {
103 ERR("[ACCEL_SCALE] is empty\n");
107 INFO("m_accel_scale = %f", m_accel_scale);
110 if (!config.get(SENSOR_TYPE_LINEAR_ACCEL, ELEMENT_LINEAR_ACCEL_SIGN_COMPENSATION, m_linear_accel_sign_compensation, 3)) {
111 ERR("[LINEAR_ACCEL_SIGN_COMPENSATION] is empty\n");
115 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]);
117 m_interval = m_default_sampling_time * MS_TO_US;
121 linear_accel_sensor::~linear_accel_sensor()
123 INFO("linear_accel_sensor is destroyed!\n");
126 bool linear_accel_sensor::init()
128 m_gravity_sensor = sensor_plugin_loader::get_instance().get_sensor(GRAVITY_SENSOR);
129 m_accel_sensor = sensor_plugin_loader::get_instance().get_sensor(ACCELEROMETER_SENSOR);
131 if (!m_accel_sensor || !m_gravity_sensor) {
132 ERR("Failed to load sensors, accel: 0x%x, gravity: 0x%x",
133 m_accel_sensor, m_gravity_sensor);
137 INFO("%s is created!", sensor_base::get_name());
141 sensor_type_t linear_accel_sensor::get_type(void)
143 return LINEAR_ACCEL_SENSOR;
146 bool linear_accel_sensor::on_start(void)
149 m_gravity_sensor->add_client(GRAVITY_EVENT_RAW_DATA_REPORT_ON_TIME);
150 m_gravity_sensor->add_interval((intptr_t)this, (m_interval/MS_TO_US), false);
151 m_gravity_sensor->start();
153 m_accel_sensor->add_client(ACCELEROMETER_EVENT_RAW_DATA_REPORT_ON_TIME);
154 m_accel_sensor->add_interval((intptr_t)this, (m_interval/MS_TO_US), false);
155 m_accel_sensor->start();
161 bool linear_accel_sensor::on_stop(void)
164 m_gravity_sensor->delete_client(GRAVITY_EVENT_RAW_DATA_REPORT_ON_TIME);
165 m_gravity_sensor->delete_interval((intptr_t)this, false);
166 m_gravity_sensor->stop();
168 m_accel_sensor->delete_client(ACCELEROMETER_EVENT_RAW_DATA_REPORT_ON_TIME);
169 m_accel_sensor->delete_interval((intptr_t)this, false);
170 m_accel_sensor->stop();
176 bool linear_accel_sensor::add_interval(int client_id, unsigned int interval)
179 m_gravity_sensor->add_interval(client_id, interval, false);
180 m_accel_sensor->add_interval(client_id, interval, false);
182 return sensor_base::add_interval(client_id, interval, false);
185 bool linear_accel_sensor::delete_interval(int client_id)
188 m_gravity_sensor->delete_interval(client_id, false);
189 m_accel_sensor->delete_interval(client_id, false);
191 return sensor_base::delete_interval(client_id, false);
194 void linear_accel_sensor::synthesize(const sensor_event_t &event, vector<sensor_event_t> &outs)
196 sensor_event_t lin_accel_event;
198 unsigned long long diff_time;
200 if (event.event_type == ACCELEROMETER_EVENT_RAW_DATA_REPORT_ON_TIME) {
201 diff_time = event.data.timestamp - m_time;
203 if (m_time && (diff_time < m_interval * MIN_DELIVERY_DIFF_FACTOR))
206 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;
207 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;
208 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;
210 m_accel.m_time_stamp = event.data.timestamp;
212 m_enable_linear_accel |= ACCELEROMETER_ENABLED;
214 else if (event.event_type == GRAVITY_EVENT_RAW_DATA_REPORT_ON_TIME) {
215 diff_time = event.data.timestamp - m_time;
217 if (m_time && (diff_time < m_interval * MIN_DELIVERY_DIFF_FACTOR))
220 m_gravity.m_data.m_vec[0] = event.data.values[0];
221 m_gravity.m_data.m_vec[1] = event.data.values[1];
222 m_gravity.m_data.m_vec[2] = event.data.values[2];
224 m_gravity.m_time_stamp = event.data.timestamp;
226 m_enable_linear_accel |= GRAVITY_ENABLED;
229 if (m_enable_linear_accel == LINEAR_ACCEL_ENABLED) {
230 m_enable_linear_accel = 0;
232 m_time = get_timestamp();
233 lin_accel_event.sensor_id = get_id();
234 lin_accel_event.event_type = LINEAR_ACCEL_EVENT_RAW_DATA_REPORT_ON_TIME;
235 lin_accel_event.data.value_count = 3;
236 lin_accel_event.data.timestamp = m_time;
237 lin_accel_event.data.accuracy = SENSOR_ACCURACY_GOOD;
238 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]);
239 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]);
240 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]);
241 push(lin_accel_event);
247 int linear_accel_sensor::get_sensor_data(const unsigned int event_type, sensor_data_t &data)
249 sensor_data_t gravity_data, accel_data;
250 ((virtual_sensor *)m_gravity_sensor)->get_sensor_data(GRAVITY_EVENT_RAW_DATA_REPORT_ON_TIME, gravity_data);
251 m_accel_sensor->get_sensor_data(ACCELEROMETER_BASE_DATA_SET, accel_data);
253 accel_data.values[0] = m_accel_rotation_direction_compensation[0] * (accel_data.values[0] - m_accel_static_bias[0]) / m_accel_scale;
254 accel_data.values[1] = m_accel_rotation_direction_compensation[1] * (accel_data.values[1] - m_accel_static_bias[1]) / m_accel_scale;
255 accel_data.values[2] = m_accel_rotation_direction_compensation[2] * (accel_data.values[2] - m_accel_static_bias[2]) / m_accel_scale;
257 if (event_type != LINEAR_ACCEL_EVENT_RAW_DATA_REPORT_ON_TIME)
260 data.accuracy = SENSOR_ACCURACY_GOOD;
261 data.timestamp = get_timestamp();
262 data.values[0] = m_linear_accel_sign_compensation[0] * (accel_data.values[0] - gravity_data.values[0]);
263 data.values[1] = m_linear_accel_sign_compensation[1] * (accel_data.values[1] - gravity_data.values[1]);
264 data.values[2] = m_linear_accel_sign_compensation[2] * (accel_data.values[2] - gravity_data.values[2]);
265 data.value_count = 3;
269 bool linear_accel_sensor::get_properties(sensor_properties_s &properties)
271 m_accel_sensor->get_properties(properties);
272 properties.name = "Linear Acceleration Sensor";
273 properties.vendor = m_vendor;
274 properties.resolution = 0.000001;
279 extern "C" sensor_module* create(void)
281 linear_accel_sensor *sensor;
284 sensor = new(std::nothrow) linear_accel_sensor;
286 ERR("Failed to create module, err: %d, cause: %s", err, strerror(err));
290 sensor_module *module = new(std::nothrow) sensor_module;
291 retvm_if(!module || !sensor, NULL, "Failed to allocate memory");
293 module->sensors.push_back(sensor);