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_time(0)
  60 {
  61         cvirtual_sensor_config &config = cvirtual_sensor_config::get_instance();
  62
  63         m_name = string(SENSOR_NAME);
  64         m_enable_linear_accel = 0;
  65         register_supported_event(LINEAR_ACCEL_EVENT_RAW_DATA_REPORT_ON_TIME);
  66
  67
  68         if (!config.get(SENSOR_TYPE_LINEAR_ACCEL, ELEMENT_VENDOR, m_vendor)) {
  69                 ERR("[VENDOR] is empty\n");
  70                 throw ENXIO;
  71         }
  72
  73         INFO("m_vendor = %s", m_vendor.c_str());
  74
  75         if (!config.get(SENSOR_TYPE_LINEAR_ACCEL, ELEMENT_RAW_DATA_UNIT, m_raw_data_unit)) {
  76                 ERR("[RAW_DATA_UNIT] is empty\n");
  77                 throw ENXIO;
  78         }
  79
  80         INFO("m_raw_data_unit = %s", m_raw_data_unit.c_str());
  81
  82         if (!config.get(SENSOR_TYPE_LINEAR_ACCEL, ELEMENT_DEFAULT_SAMPLING_TIME, &m_default_sampling_time)) {
  83                 ERR("[DEFAULT_SAMPLING_TIME] is empty\n");
  84                 throw ENXIO;
  85         }
  86
  87         INFO("m_default_sampling_time = %d", m_default_sampling_time);
  88
  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");
  91                 throw ENXIO;
  92         }
  93
  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");
  96                 throw ENXIO;
  97         }
  98
  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]);
 100
 101
 102         if (!config.get(SENSOR_TYPE_LINEAR_ACCEL, ELEMENT_ACCEL_SCALE, &m_accel_scale)) {
 103                 ERR("[ACCEL_SCALE] is empty\n");
 104                 throw ENXIO;
 105         }
 106
 107         INFO("m_accel_scale = %f", m_accel_scale);
 108
 109
 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");
 112                 throw ENXIO;
 113         }
 114
 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]);
 116
 117         m_interval = m_default_sampling_time * MS_TO_US;
 118
 119 }
 120
 121 linear_accel_sensor::~linear_accel_sensor()
 122 {
 123         INFO("linear_accel_sensor is destroyed!\n");
 124 }
 125
 126 bool linear_accel_sensor::init()
 127 {
 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);
 130
 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);
 134                 return false;
 135         }
 136
 137         INFO("%s is created!", sensor_base::get_name());
 138         return true;
 139 }
 140
 141 sensor_type_t linear_accel_sensor::get_type(void)
 142 {
 143         return LINEAR_ACCEL_SENSOR;
 144 }
 145
 146 bool linear_accel_sensor::on_start(void)
 147 {
 148         AUTOLOCK(m_mutex);
 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();
 152
 153         m_accel_sensor->add_client(ACCELEROMETER_RAW_DATA_EVENT);
 154         m_accel_sensor->add_interval((intptr_t)this, (m_interval/MS_TO_US), false);
 155         m_accel_sensor->start();
 156
 157         activate();
 158         return true;
 159 }
 160
 161 bool linear_accel_sensor::on_stop(void)
 162 {
 163         AUTOLOCK(m_mutex);
 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();
 167
 168         m_accel_sensor->delete_client(ACCELEROMETER_RAW_DATA_EVENT);
 169         m_accel_sensor->delete_interval((intptr_t)this, false);
 170         m_accel_sensor->stop();
 171
 172         deactivate();
 173         return true;
 174 }
 175
 176 bool linear_accel_sensor::add_interval(int client_id, unsigned int interval)
 177 {
 178         AUTOLOCK(m_mutex);
 179         m_gravity_sensor->add_interval(client_id, interval, false);
 180         m_accel_sensor->add_interval(client_id, interval, false);
 181
 182         return sensor_base::add_interval(client_id, interval, false);
 183 }
 184
 185 bool linear_accel_sensor::delete_interval(int client_id)
 186 {
 187         AUTOLOCK(m_mutex);
 188         m_gravity_sensor->delete_interval(client_id, false);
 189         m_accel_sensor->delete_interval(client_id, false);
 190
 191         return sensor_base::delete_interval(client_id, false);
 192 }
 193
 194 void linear_accel_sensor::synthesize(const sensor_event_t &event, vector<sensor_event_t> &outs)
 195 {
 196         sensor_event_t lin_accel_event;
 197
 198         unsigned long long diff_time;
 199
 200         if (event.event_type == ACCELEROMETER_RAW_DATA_EVENT) {
 201                 diff_time = event.data.timestamp - m_time;
 202
 203                 if (m_time && (diff_time < m_interval * MIN_DELIVERY_DIFF_FACTOR))
 204                         return;
 205
 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;
 209
 210                 m_accel.m_time_stamp = event.data.timestamp;
 211
 212                 m_enable_linear_accel |= ACCELEROMETER_ENABLED;
 213         }
 214         else if (event.event_type == GRAVITY_EVENT_RAW_DATA_REPORT_ON_TIME) {
 215                 diff_time = event.data.timestamp - m_time;
 216
 217                 if (m_time && (diff_time < m_interval * MIN_DELIVERY_DIFF_FACTOR))
 218                         return;
 219
 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];
 223
 224                 m_gravity.m_time_stamp = event.data.timestamp;
 225
 226                 m_enable_linear_accel |= GRAVITY_ENABLED;
 227         }
 228
 229         if (m_enable_linear_accel == LINEAR_ACCEL_ENABLED) {
 230                 m_enable_linear_accel = 0;
 231
 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);
 242         }
 243
 244         return;
 245 }
 246
 247 int linear_accel_sensor::get_sensor_data(const unsigned int event_type, sensor_data_t &data)
 248 {
 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_RAW_DATA_EVENT, accel_data);
 252
 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;
 256
 257         if (event_type != LINEAR_ACCEL_EVENT_RAW_DATA_REPORT_ON_TIME)
 258                 return -1;
 259
 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;
 266         return 0;
 267 }
 268
 269 bool linear_accel_sensor::get_properties(sensor_properties_s &properties)
 270 {
 271         m_accel_sensor->get_properties(properties);
 272         properties.name = "Linear Acceleration Sensor";
 273         properties.vendor = m_vendor;
 274         properties.resolution = 0.000001;
 275
 276         return true;
 277 }
 278
 279 extern "C" sensor_module* create(void)
 280 {
 281         linear_accel_sensor *sensor;
 282
 283         try {
 284                 sensor = new(std::nothrow) linear_accel_sensor;
 285         } catch (int err) {
 286                 ERR("Failed to create module, err: %d, cause: %s", err, strerror(err));
 287                 return NULL;
 288         }
 289
 290         sensor_module *module = new(std::nothrow) sensor_module;
 291         retvm_if(!module || !sensor, NULL, "Failed to allocate memory");
 292
 293         module->sensors.push_back(sensor);
 294         return module;
 295 }