src/plugins/accel/accel_sensor_hal.cpp

   1 /*
   2  * accel_sensor_hal
   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 #include <fcntl.h>
  20 #include <sys/stat.h>
  21 #include <linux/input.h>
  22 #include <csensor_config.h>
  23 #include <accel_sensor_hal.h>
  24 #include <sys/poll.h>
  25
  26 using std::ifstream;
  27 using std::string;
  28
  29 #define GRAVITY 9.80665
  30 #define G_TO_MG 1000
  31 #define RAW_DATA_TO_G_UNIT(X) (((float)(X))/((float)G_TO_MG))
  32 #define RAW_DATA_TO_METRE_PER_SECOND_SQUARED_UNIT(X) (GRAVITY * (RAW_DATA_TO_G_UNIT(X)))
  33
  34 #define MIN_RANGE(RES) (-((1 << (RES))/2))
  35 #define MAX_RANGE(RES) (((1 << (RES))/2)-1)
  36
  37 #define SENSOR_TYPE_ACCEL               "ACCEL"
  38 #define ELEMENT_NAME                    "NAME"
  39 #define ELEMENT_VENDOR                  "VENDOR"
  40 #define ELEMENT_RAW_DATA_UNIT   "RAW_DATA_UNIT"
  41 #define ELEMENT_RESOLUTION              "RESOLUTION"
  42
  43 #define ATTR_VALUE                              "value"
  44
  45 #define INPUT_NAME      "accelerometer_sensor"
  46 #define ACCEL_SENSORHUB_POLL_NODE_NAME "accel_poll_delay"
  47
  48 accel_sensor_hal::accel_sensor_hal()
  49 : m_x(-1)
  50 , m_y(-1)
  51 , m_z(-1)
  52 , m_node_handle(-1)
  53 , m_polling_interval(POLL_1HZ_MS)
  54 , m_fired_time(0)
  55 {
  56         const string sensorhub_interval_node_name = "accel_poll_delay";
  57         csensor_config &config = csensor_config::get_instance();
  58
  59         node_info_query query;
  60         node_info info;
  61
  62         if (!find_model_id(SENSOR_TYPE_ACCEL, m_model_id)) {
  63                 ERR("Failed to find model id");
  64                 throw ENXIO;
  65         }
  66
  67         query.sensorhub_controlled = m_sensorhub_controlled = is_sensorhub_controlled(sensorhub_interval_node_name);
  68         query.sensor_type = SENSOR_TYPE_ACCEL;
  69         query.key = "accelerometer_sensor";
  70         query.iio_enable_node_name = "accel_enable";
  71         query.sensorhub_interval_node_name = sensorhub_interval_node_name;
  72
  73         if (!get_node_info(query, info)) {
  74                 ERR("Failed to get node info");
  75                 throw ENXIO;
  76         }
  77
  78         show_node_info(info);
  79
  80         m_method = info.method;
  81         m_data_node = info.data_node_path;
  82         m_enable_node = info.enable_node_path;
  83         m_interval_node = info.interval_node_path;
  84
  85         if (!config.get(SENSOR_TYPE_ACCEL, m_model_id, ELEMENT_VENDOR, m_vendor)) {
  86                 ERR("[VENDOR] is empty\n");
  87                 throw ENXIO;
  88         }
  89
  90         INFO("m_vendor = %s", m_vendor.c_str());
  91
  92         if (!config.get(SENSOR_TYPE_ACCEL, m_model_id, ELEMENT_NAME, m_chip_name)) {
  93                 ERR("[NAME] is empty\n");
  94                 throw ENXIO;
  95         }
  96
  97         INFO("m_chip_name = %s\n",m_chip_name.c_str());
  98
  99         long resolution;
 100
 101         if (!config.get(SENSOR_TYPE_ACCEL, m_model_id, ELEMENT_RESOLUTION, resolution)) {
 102                 ERR("[RESOLUTION] is empty\n");
 103                 throw ENXIO;
 104         }
 105
 106         m_resolution = (int)resolution;
 107
 108         INFO("m_resolution = %d\n",m_resolution);
 109
 110         double raw_data_unit;
 111
 112         if (!config.get(SENSOR_TYPE_ACCEL, m_model_id, ELEMENT_RAW_DATA_UNIT, raw_data_unit)) {
 113                 ERR("[RAW_DATA_UNIT] is empty\n");
 114                 throw ENXIO;
 115         }
 116
 117         m_raw_data_unit = (float)(raw_data_unit);
 118         INFO("m_raw_data_unit = %f\n", m_raw_data_unit);
 119
 120         if ((m_node_handle = open(m_data_node.c_str(), O_RDWR)) < 0) {
 121                 ERR("accel handle open fail for accel processor, error:%s\n", strerror(errno));
 122                 throw ENXIO;
 123         }
 124
 125         if (m_method == INPUT_EVENT_METHOD) {
 126                 int clockId = CLOCK_MONOTONIC;
 127                 if (ioctl(m_node_handle, EVIOCSCLOCKID, &clockId) != 0)
 128                         ERR("Fail to set monotonic timestamp for %s", m_data_node.c_str());
 129
 130                 update_value = [=]() {
 131                         return this->update_value_input_event();
 132                 };
 133         } else {
 134                 if (!info.buffer_length_node_path.empty())
 135                         set_node_value(info.buffer_length_node_path, 480);
 136
 137                 if (!info.buffer_enable_node_path.empty())
 138                         set_node_value(info.buffer_enable_node_path, 1);
 139
 140                 update_value = [=]() {
 141                         return this->update_value_iio();
 142                 };
 143         }
 144
 145         INFO("accel_sensor is created!\n");
 146 }
 147
 148 accel_sensor_hal::~accel_sensor_hal()
 149 {
 150         close(m_node_handle);
 151         m_node_handle = -1;
 152
 153         INFO("accel_sensor is destroyed!\n");
 154 }
 155
 156 string accel_sensor_hal::get_model_id(void)
 157 {
 158         return m_model_id;
 159 }
 160
 161 sensor_hal_type_t accel_sensor_hal::get_type(void)
 162 {
 163         return SENSOR_HAL_TYPE_ACCELEROMETER;
 164 }
 165
 166 bool accel_sensor_hal::enable(void)
 167 {
 168         set_enable_node(m_enable_node, m_sensorhub_controlled, true, SENSORHUB_ACCELEROMETER_ENABLE_BIT);
 169         set_interval(m_polling_interval);
 170
 171         m_fired_time = 0;
 172         INFO("Accel sensor real starting");
 173         return true;
 174 }
 175
 176 bool accel_sensor_hal::disable(void)
 177 {
 178         set_enable_node(m_enable_node, m_sensorhub_controlled, false, SENSORHUB_ACCELEROMETER_ENABLE_BIT);
 179
 180         INFO("Accel sensor real stopping");
 181         return true;
 182 }
 183
 184 bool accel_sensor_hal::set_interval(unsigned long val)
 185 {
 186         unsigned long long polling_interval_ns;
 187         polling_interval_ns = ((unsigned long long)(val) * 1000llu * 1000llu);
 188
 189         if (!set_node_value(m_interval_node, polling_interval_ns)) {
 190                 ERR("Failed to set polling resource: %s\n", m_interval_node.c_str());
 191                 return false;
 192         }
 193
 194         INFO("Interval is changed from %dms to %dms]", m_polling_interval, val);
 195         m_polling_interval = val;
 196         return true;
 197 }
 198
 199 bool accel_sensor_hal::update_value_input_event()
 200 {
 201         int accel_raw[3] = {0,};
 202         bool x,y,z;
 203         int read_input_cnt = 0;
 204         const int INPUT_MAX_BEFORE_SYN = 10;
 205         unsigned long long fired_time = 0;
 206         bool syn = false;
 207
 208         x = y = z = false;
 209
 210         struct input_event accel_input;
 211         DBG("accel event detection!");
 212
 213         while ((syn == false) && (read_input_cnt < INPUT_MAX_BEFORE_SYN)) {
 214                 int len = read(m_node_handle, &accel_input, sizeof(accel_input));
 215                 if (len != sizeof(accel_input)) {
 216                         ERR("accel_file read fail, read_len = %d\n",len);
 217                         return false;
 218                 }
 219
 220                 ++read_input_cnt;
 221
 222                 if (accel_input.type == EV_REL) {
 223                         switch (accel_input.code) {
 224                         case REL_X:
 225                                 accel_raw[0] = (int)accel_input.value;
 226                                 x = true;
 227                                 break;
 228                         case REL_Y:
 229                                 accel_raw[1] = (int)accel_input.value;
 230                                 y = true;
 231                                 break;
 232                         case REL_Z:
 233                                 accel_raw[2] = (int)accel_input.value;
 234                                 z = true;
 235                                 break;
 236                         default:
 237                                 ERR("accel_input event[type = %d, code = %d] is unknown.", accel_input.type, accel_input.code);
 238                                 return false;
 239                                 break;
 240                         }
 241                 } else if (accel_input.type == EV_SYN) {
 242                         syn = true;
 243                         fired_time = sensor_hal_base::get_timestamp(&accel_input.time);
 244                 } else {
 245                         ERR("accel_input event[type = %d, code = %d] is unknown.", accel_input.type, accel_input.code);
 246                         return false;
 247                 }
 248         }
 249
 250         if (syn == false) {
 251                 ERR("EV_SYN didn't come until %d inputs had come", read_input_cnt);
 252                 return false;
 253         }
 254
 255         if (x)
 256                 m_x =  accel_raw[0];
 257         if (y)
 258                 m_y =  accel_raw[1];
 259         if (z)
 260                 m_z =  accel_raw[2];
 261
 262         m_fired_time = fired_time;
 263
 264         DBG("m_x = %d, m_y = %d, m_z = %d, time = %lluus", m_x, m_y, m_z, m_fired_time);
 265
 266         return true;
 267 }
 268
 269 bool accel_sensor_hal::update_value_iio()
 270 {
 271         const int READ_LEN = 14;
 272         char data[READ_LEN] = {0,};
 273
 274         struct pollfd pfd;
 275
 276         pfd.fd = m_node_handle;
 277         pfd.events = POLLIN | POLLERR;
 278         pfd.revents = 0;
 279
 280         int ret = poll(&pfd, 1, -1);
 281
 282         if (ret == -1) {
 283                 ERR("poll error:%s m_node_handle:d", strerror(errno), m_node_handle);
 284                 return false;
 285         } else if (!ret) {
 286                 ERR("poll timeout m_node_handle:%d", m_node_handle);
 287                 return false;
 288         }
 289
 290         if (pfd.revents & POLLERR) {
 291                 ERR("poll exception occurred! m_node_handle:%d", m_node_handle);
 292                 return false;
 293         }
 294
 295         if (!(pfd.revents & POLLIN)) {
 296                 ERR("poll nothing to read! m_node_handle:%d, pfd.revents = %d", m_node_handle, pfd.revents);
 297                 return false;
 298         }
 299
 300         int len = read(m_node_handle, data, sizeof(data));
 301
 302         if (len != sizeof(data)) {
 303                 ERR("Failed to read data, m_node_handle:%d read_len:%d", m_node_handle, len);
 304                 return false;
 305         }
 306
 307         short *short_data = (short *)(data);
 308         m_x = *(short_data);
 309         m_y = *((short *)(data + 2));
 310         m_z = *((short *)(data + 4));
 311
 312         m_fired_time = *((long long*)(data + 6));
 313
 314         INFO("m_x = %d, m_y = %d, m_z = %d, time = %lluus", m_x, m_y, m_z, m_fired_time);
 315
 316         return true;
 317 }
 318
 319 bool accel_sensor_hal::is_data_ready(void)
 320 {
 321         bool ret;
 322         ret = update_value();
 323         return ret;
 324 }
 325
 326 int accel_sensor_hal::get_sensor_data(sensor_data_t &data)
 327 {
 328         data.accuracy = SENSOR_ACCURACY_GOOD;
 329         data.timestamp = m_fired_time;
 330         data.value_count = 3;
 331         data.values[0] = m_x;
 332         data.values[1] = m_y;
 333         data.values[2] = m_z;
 334
 335         return 0;
 336 }
 337
 338 bool accel_sensor_hal::get_properties(sensor_properties_s &properties)
 339 {
 340         properties.name = m_chip_name;
 341         properties.vendor = m_vendor;
 342         properties.min_range = MIN_RANGE(m_resolution)* RAW_DATA_TO_METRE_PER_SECOND_SQUARED_UNIT(m_raw_data_unit);
 343         properties.max_range = MAX_RANGE(m_resolution)* RAW_DATA_TO_METRE_PER_SECOND_SQUARED_UNIT(m_raw_data_unit);
 344         properties.min_interval = 1;
 345         properties.resolution = RAW_DATA_TO_METRE_PER_SECOND_SQUARED_UNIT(m_raw_data_unit);
 346         properties.fifo_count = 0;
 347         properties.max_batch_count = 0;
 348         return true;
 349 }