src/linear_accel/linear_accel_sensor.cpp

   1 /*
   2  * sensord
   3  *
   4  * Copyright (c) 2014 Samsung Electronics Co., Ltd.
   5  *
   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
   9  *
  10  * http://www.apache.org/licenses/LICENSE-2.0
  11  *
  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.
  17  *
  18  */
  19
  20 #include <stdio.h>
  21 #include <stdlib.h>
  22 #include <unistd.h>
  23 #include <errno.h>
  24 #include <math.h>
  25 #include <time.h>
  26 #include <sys/types.h>
  27 #include <dlfcn.h>
  28 #include <common.h>
  29 #include <sf_common.h>
  30 #include <linear_accel_sensor.h>
  31 #include <sensor_plugin_loader.h>
  32 #include <cvirtual_sensor_config.h>
  33
  34 #define SENSOR_NAME "LINEAR_ACCEL_SENSOR"
  35 #define SENSOR_TYPE_LINEAR_ACCEL        "LINEAR_ACCEL"
  36
  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"
  45
  46 #define INITIAL_VALUE -1
  47 #define GRAVITY 9.80665
  48
  49 #define MS_TO_US 1000
  50 #define MIN_DELIVERY_DIFF_FACTOR 0.75f
  51
  52 #define ACCELEROMETER_ENABLED 0x01
  53 #define GRAVITY_ENABLED 0x02
  54 #define LINEAR_ACCEL_ENABLED 3
  55
  56 linear_accel_sensor::linear_accel_sensor()
  57 : m_accel_sensor(NULL)
  58 , m_gravity_sensor(NULL)
  59 , m_x(INITIAL_VALUE)
  60 , m_y(INITIAL_VALUE)
  61 , m_z(INITIAL_VALUE)
  62 {
  63         cvirtual_sensor_config &config = cvirtual_sensor_config::get_instance();
  64
  65         m_name = string(SENSOR_NAME);
  66         m_timestamp = get_timestamp();
  67         m_enable_linear_accel = 0;
  68         register_supported_event(LINEAR_ACCEL_EVENT_RAW_DATA_REPORT_ON_TIME);
  69
  70
  71         if (!config.get(SENSOR_TYPE_LINEAR_ACCEL, ELEMENT_VENDOR, m_vendor)) {
  72                 ERR("[VENDOR] is empty\n");
  73                 throw ENXIO;
  74         }
  75
  76         INFO("m_vendor = %s", m_vendor.c_str());
  77
  78         if (!config.get(SENSOR_TYPE_LINEAR_ACCEL, ELEMENT_RAW_DATA_UNIT, m_raw_data_unit)) {
  79                 ERR("[RAW_DATA_UNIT] is empty\n");
  80                 throw ENXIO;
  81         }
  82
  83         INFO("m_raw_data_unit = %s", m_raw_data_unit.c_str());
  84
  85         if (!config.get(SENSOR_TYPE_LINEAR_ACCEL, ELEMENT_DEFAULT_SAMPLING_TIME, &m_default_sampling_time)) {
  86                 ERR("[DEFAULT_SAMPLING_TIME] is empty\n");
  87                 throw ENXIO;
  88         }
  89
  90         INFO("m_default_sampling_time = %d", m_default_sampling_time);
  91
  92         if (!config.get(SENSOR_TYPE_LINEAR_ACCEL, ELEMENT_ACCEL_STATIC_BIAS, m_accel_static_bias, 3)) {
  93                 ERR("[ACCEL_STATIC_BIAS] is empty\n");
  94                 throw ENXIO;
  95         }
  96
  97         if (!config.get(SENSOR_TYPE_LINEAR_ACCEL, ELEMENT_ACCEL_ROTATION_DIRECTION_COMPENSATION, m_accel_rotation_direction_compensation, 3)) {
  98                 ERR("[ACCEL_ROTATION_DIRECTION_COMPENSATION] is empty\n");
  99                 throw ENXIO;
 100         }
 101
 102         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]);
 103
 104
 105         if (!config.get(SENSOR_TYPE_LINEAR_ACCEL, ELEMENT_ACCEL_SCALE, &m_accel_scale)) {
 106                 ERR("[ACCEL_SCALE] is empty\n");
 107                 throw ENXIO;
 108         }
 109
 110         INFO("m_accel_scale = %f", m_accel_scale);
 111
 112
 113         if (!config.get(SENSOR_TYPE_LINEAR_ACCEL, ELEMENT_LINEAR_ACCEL_SIGN_COMPENSATION, m_linear_accel_sign_compensation, 3)) {
 114                 ERR("[LINEAR_ACCEL_SIGN_COMPENSATION] is empty\n");
 115                 throw ENXIO;
 116         }
 117
 118         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]);
 119
 120         m_interval = m_default_sampling_time * MS_TO_US;
 121
 122 }
 123
 124 linear_accel_sensor::~linear_accel_sensor()
 125 {
 126         INFO("linear_accel_sensor is destroyed!\n");
 127 }
 128
 129 bool linear_accel_sensor::init()
 130 {
 131         m_gravity_sensor = sensor_plugin_loader::get_instance().get_sensor(GRAVITY_SENSOR);
 132         m_accel_sensor = sensor_plugin_loader::get_instance().get_sensor(ACCELEROMETER_SENSOR);
 133
 134         if (!m_accel_sensor || !m_gravity_sensor) {
 135                 ERR("Failed to load sensors,  accel: 0x%x, gravity: 0x%x",
 136                         m_accel_sensor, m_gravity_sensor);
 137                 return false;
 138         }
 139
 140         INFO("%s is created!", sensor_base::get_name());
 141         return true;
 142 }
 143
 144 sensor_type_t linear_accel_sensor::get_type(void)
 145 {
 146         return LINEAR_ACCEL_SENSOR;
 147 }
 148
 149 bool linear_accel_sensor::on_start(void)
 150 {
 151         AUTOLOCK(m_mutex);
 152         m_gravity_sensor->add_client(GRAVITY_EVENT_RAW_DATA_REPORT_ON_TIME);
 153         m_gravity_sensor->add_interval((intptr_t)this, (m_interval/MS_TO_US), true);
 154         m_gravity_sensor->start();
 155
 156         m_accel_sensor->add_client(ACCELEROMETER_EVENT_RAW_DATA_REPORT_ON_TIME);
 157         m_accel_sensor->add_interval((intptr_t)this, (m_interval/MS_TO_US), true);
 158         m_accel_sensor->start();
 159
 160         activate();
 161         return true;
 162 }
 163
 164 bool linear_accel_sensor::on_stop(void)
 165 {
 166         AUTOLOCK(m_mutex);
 167         m_gravity_sensor->delete_client(GRAVITY_EVENT_RAW_DATA_REPORT_ON_TIME);
 168         m_gravity_sensor->delete_interval((intptr_t)this, true);
 169         m_gravity_sensor->stop();
 170
 171         m_accel_sensor->delete_client(ACCELEROMETER_EVENT_RAW_DATA_REPORT_ON_TIME);
 172         m_accel_sensor->delete_interval((intptr_t)this, true);
 173         m_accel_sensor->stop();
 174
 175         deactivate();
 176         return true;
 177 }
 178
 179 bool linear_accel_sensor::add_interval(int client_id, unsigned int interval)
 180 {
 181         AUTOLOCK(m_mutex);
 182         m_gravity_sensor->add_interval(client_id, interval, true);
 183         m_accel_sensor->add_interval((intptr_t)this , interval, true);
 184
 185         return sensor_base::add_interval(client_id, interval, true);
 186 }
 187
 188 bool linear_accel_sensor::delete_interval(int client_id)
 189 {
 190         AUTOLOCK(m_mutex);
 191         m_gravity_sensor->delete_interval(client_id, true);
 192         m_accel_sensor->delete_interval(client_id, true);
 193
 194         return sensor_base::delete_interval(client_id, true);
 195 }
 196
 197 void linear_accel_sensor::synthesize(const sensor_event_t &event, vector<sensor_event_t> &outs)
 198 {
 199         sensor_event_t lin_accel_event;
 200
 201         unsigned long long diff_time;
 202
 203         if (event.event_type == ACCELEROMETER_EVENT_RAW_DATA_REPORT_ON_TIME) {
 204                 diff_time = event.data.timestamp - m_timestamp;
 205
 206                 if (m_timestamp && (diff_time < m_interval * MIN_DELIVERY_DIFF_FACTOR))
 207                         return;
 208
 209                 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;
 210                 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;
 211                 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;
 212
 213                 m_accel.m_time_stamp = event.data.timestamp;
 214
 215                 m_enable_linear_accel |= ACCELEROMETER_ENABLED;
 216         }
 217         else if (event.event_type == GRAVITY_EVENT_RAW_DATA_REPORT_ON_TIME) {
 218                 diff_time = event.data.timestamp - m_timestamp;
 219
 220                 if (m_timestamp && (diff_time < m_interval * MIN_DELIVERY_DIFF_FACTOR))
 221                         return;
 222
 223                 m_gravity.m_data.m_vec[0] = event.data.values[0];
 224                 m_gravity.m_data.m_vec[1] = event.data.values[1];
 225                 m_gravity.m_data.m_vec[2] = event.data.values[2];
 226
 227                 m_gravity.m_time_stamp = event.data.timestamp;
 228
 229                 m_enable_linear_accel |= GRAVITY_ENABLED;
 230         }
 231
 232         if (m_enable_linear_accel == LINEAR_ACCEL_ENABLED) {
 233                 m_enable_linear_accel = 0;
 234                 m_timestamp = get_timestamp();
 235
 236                 AUTOLOCK(m_value_mutex);
 237
 238                 lin_accel_event.sensor_id = get_id();
 239                 lin_accel_event.event_type = LINEAR_ACCEL_EVENT_RAW_DATA_REPORT_ON_TIME;
 240                 lin_accel_event.data.value_count = 3;
 241                 lin_accel_event.data.timestamp = m_timestamp;
 242                 lin_accel_event.data.accuracy = SENSOR_ACCURACY_GOOD;
 243                 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]);
 244                 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]);
 245                 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]);
 246                 push(lin_accel_event);
 247         }
 248
 249         return;
 250 }
 251
 252 int linear_accel_sensor::get_sensor_data(const unsigned int event_type, sensor_data_t &data)
 253 {
 254         sensor_data_t gravity_data, accel_data;
 255         ((virtual_sensor *)m_gravity_sensor)->get_sensor_data(GRAVITY_EVENT_RAW_DATA_REPORT_ON_TIME, gravity_data);
 256         m_accel_sensor->get_sensor_data(ACCELEROMETER_BASE_DATA_SET, accel_data);
 257
 258         accel_data.values[0] = m_accel_rotation_direction_compensation[0] * (accel_data.values[0] - m_accel_static_bias[0]) / m_accel_scale;
 259         accel_data.values[1] = m_accel_rotation_direction_compensation[1] * (accel_data.values[1] - m_accel_static_bias[1]) / m_accel_scale;
 260         accel_data.values[2] = m_accel_rotation_direction_compensation[2] * (accel_data.values[2] - m_accel_static_bias[2]) / m_accel_scale;
 261
 262         if (event_type != LINEAR_ACCEL_EVENT_RAW_DATA_REPORT_ON_TIME)
 263                 return -1;
 264
 265         data.accuracy = SENSOR_ACCURACY_GOOD;
 266         data.timestamp = get_timestamp();
 267         data.values[0] = m_linear_accel_sign_compensation[0] * (accel_data.values[0] - gravity_data.values[0]);
 268         data.values[1] = m_linear_accel_sign_compensation[1] * (accel_data.values[1] - gravity_data.values[1]);
 269         data.values[2] = m_linear_accel_sign_compensation[2] * (accel_data.values[2] - gravity_data.values[2]);
 270         data.value_count = 3;
 271         return 0;
 272 }
 273
 274 bool linear_accel_sensor::get_properties(sensor_properties_s &properties)
 275 {
 276         m_accel_sensor->get_properties(properties);
 277         properties.name = "Linear Acceleration Sensor";
 278         properties.vendor = m_vendor;
 279         properties.resolution = 0.000001;
 280
 281         return true;
 282 }
 283
 284 extern "C" void *create(void)
 285 {
 286         linear_accel_sensor *inst;
 287
 288         try {
 289                 inst = new linear_accel_sensor();
 290         } catch (int err) {
 291                 ERR("Failed to create linear_accel_sensor class, errno : %d, errstr : %s", err, strerror(err));
 292                 return NULL;
 293         }
 294
 295         return (void *)inst;
 296 }
 297
 298 extern "C" void destroy(void *inst)
 299 {
 300         delete (linear_accel_sensor *)inst;
 301 }