src/accel/accel_device.cpp

   1 /*
   2  * Copyright (c) 2016 Samsung Electronics Co., Ltd.
   3  *
   4  * Licensed under the Apache License, Version 2.0 (the "License");
   5  * you may not use this file except in compliance with the License.
   6  * You may obtain a copy of the License at
   7  *
   8  * http://www.apache.org/licenses/LICENSE-2.0
   9  *
  10  * Unless required by applicable law or agreed to in writing, software
  11  * distributed under the License is distributed on an "AS IS" BASIS,
  12  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  13  * See the License for the specific language governing permissions and
  14  * limitations under the License.
  15  *
  16  */
  17
  18 #include <fcntl.h>
  19 #include <unistd.h>
  20 #include <sys/types.h>
  21 #include <sys/stat.h>
  22
  23 #include <linux/input.h>
  24 #include <sys/ioctl.h>
  25 #include <poll.h>
  26
  27 #include <macro.h>
  28 #include <util.h>
  29 #include <sensor_common.h>
  30 #include <sensor_log.h>
  31
  32 #include "accel_device.h"
  33
  34 #define MODEL_NAME "ICM20610"
  35 #define VENDOR "Invensense"
  36 #define RESOLUTION 16
  37 #define RAW_DATA_UNIT 0.119
  38 #define MIN_INTERVAL 1
  39 #define MAX_BATCH_COUNT 0
  40
  41 #define SENSOR_NAME "SENSOR_ACCELEROMETER"
  42 #define SENSOR_TYPE_ACCEL               "ACCEL"
  43
  44 #define INPUT_NAME      "accelerometer_sensor"
  45 #define ACCEL_SENSORHUB_POLL_NODE_NAME "accel_poll_delay"
  46
  47 #define GRAVITY 9.80665
  48 #define G_TO_MG 1000
  49 #define RAW_DATA_TO_G_UNIT(X) (((float)(X))/((float)G_TO_MG))
  50 #define RAW_DATA_TO_METRE_PER_SECOND_SQUARED_UNIT(X) (GRAVITY * (RAW_DATA_TO_G_UNIT(X)))
  51
  52 #define MIN_RANGE(RES) (-((1 << (RES))/2))
  53 #define MAX_RANGE(RES) (((1 << (RES))/2)-1)
  54
  55 static sensor_info_t sensor_info = {
  56         id: 0x1,
  57         name: SENSOR_NAME,
  58         type: SENSOR_DEVICE_ACCELEROMETER,
  59         event_type: (SENSOR_DEVICE_ACCELEROMETER << SENSOR_EVENT_SHIFT) | RAW_DATA_EVENT,
  60         model_name: MODEL_NAME,
  61         vendor: VENDOR,
  62         min_range: MIN_RANGE(RESOLUTION) * RAW_DATA_TO_METRE_PER_SECOND_SQUARED_UNIT(RAW_DATA_UNIT),
  63         max_range: MAX_RANGE(RESOLUTION) * RAW_DATA_TO_METRE_PER_SECOND_SQUARED_UNIT(RAW_DATA_UNIT),
  64         resolution: RAW_DATA_TO_METRE_PER_SECOND_SQUARED_UNIT(RAW_DATA_UNIT),
  65         min_interval: MIN_INTERVAL,
  66         max_batch_count: MAX_BATCH_COUNT,
  67         wakeup_supported: false
  68 };
  69
  70 accel_device::accel_device()
  71 : m_node_handle(-1)
  72 , m_x(-1)
  73 , m_y(-1)
  74 , m_z(-1)
  75 , m_polling_interval(1000)
  76 , m_fired_time(0)
  77 , m_sensorhub_controlled(false)
  78 {
  79         const std::string sensorhub_interval_node_name = ACCEL_SENSORHUB_POLL_NODE_NAME;
  80
  81         node_info_query query;
  82         node_info info;
  83
  84         query.sensorhub_controlled = m_sensorhub_controlled = util::is_sensorhub_controlled(sensorhub_interval_node_name);
  85         query.sensor_type = SENSOR_TYPE_ACCEL;
  86         query.key = INPUT_NAME;
  87         query.iio_enable_node_name = "accel_enable";
  88         query.sensorhub_interval_node_name = sensorhub_interval_node_name;
  89
  90         if (!util::get_node_info(query, info)) {
  91                 _E("Failed to get node info");
  92                 throw ENXIO;
  93         }
  94
  95         util::show_node_info(info);
  96
  97         m_method = info.method;
  98         m_data_node = info.data_node_path;
  99         m_enable_node = info.enable_node_path;
 100         m_interval_node = info.interval_node_path;
 101
 102         m_node_handle = open(m_data_node.c_str(), O_RDONLY);
 103
 104         if (m_node_handle < 0) {
 105                 _ERRNO(errno, _E, "accel handle open fail for accel processor");
 106                 throw ENXIO;
 107         }
 108
 109         if (m_method == INPUT_EVENT_METHOD) {
 110                 if (!util::set_monotonic_clock(m_node_handle))
 111                         throw ENXIO;
 112
 113                 update_value = [=]() {
 114                         return this->update_value_input_event();
 115                 };
 116         } else {
 117                 if (!info.buffer_length_node_path.empty())
 118                         util::set_node_value(info.buffer_length_node_path, 600);
 119
 120                 if (!info.buffer_enable_node_path.empty())
 121                         util::set_node_value(info.buffer_enable_node_path, 1);
 122
 123                 update_value = [=]() {
 124                         return this->update_value_iio();
 125                 };
 126         }
 127
 128         _I("accel_device is created!");
 129 }
 130
 131 accel_device::~accel_device()
 132 {
 133         close(m_node_handle);
 134         m_node_handle = -1;
 135
 136         _I("accel_device is destroyed!");
 137 }
 138
 139 int accel_device::get_poll_fd(void)
 140 {
 141         return m_node_handle;
 142 }
 143
 144 int accel_device::get_sensors(const sensor_info_t **sensors)
 145 {
 146         *sensors = &sensor_info;
 147
 148         return 1;
 149 }
 150
 151 bool accel_device::enable(uint32_t id)
 152 {
 153         util::set_enable_node(m_enable_node, m_sensorhub_controlled, true, SENSORHUB_ACCELEROMETER_ENABLE_BIT);
 154         set_interval(id, m_polling_interval);
 155
 156         m_fired_time = 0;
 157         _I("Enable accelerometer sensor");
 158         return true;
 159 }
 160
 161 bool accel_device::disable(uint32_t id)
 162 {
 163         util::set_enable_node(m_enable_node, m_sensorhub_controlled, false, SENSORHUB_ACCELEROMETER_ENABLE_BIT);
 164
 165         _I("Disable accelerometer sensor");
 166         return true;
 167 }
 168
 169 bool accel_device::set_interval(uint32_t id, unsigned long val)
 170 {
 171         unsigned long long polling_interval_ns;
 172
 173         polling_interval_ns = ((unsigned long long)(val) * 1000llu * 1000llu);
 174
 175         if (!util::set_node_value(m_interval_node, polling_interval_ns)) {
 176                 _E("Failed to set polling resource: %s", m_interval_node.c_str());
 177                 return false;
 178         }
 179
 180         _I("Interval is changed from %dms to %dms", m_polling_interval, val);
 181         m_polling_interval = val;
 182         return true;
 183 }
 184
 185 bool accel_device::update_value_input_event(void)
 186 {
 187         int accel_raw[3] = {0, };
 188         bool x, y, z;
 189         int read_input_cnt = 0;
 190         const int INPUT_MAX_BEFORE_SYN = 10;
 191         unsigned long long fired_time = 0;
 192         bool syn = false;
 193
 194         x = y = z = false;
 195
 196         struct input_event accel_input;
 197         _D("accel event detection!");
 198
 199         while ((syn == false) && (read_input_cnt < INPUT_MAX_BEFORE_SYN)) {
 200                 int len = read(m_node_handle, &accel_input, sizeof(accel_input));
 201                 if (len != sizeof(accel_input)) {
 202                         _E("accel_file read fail, read_len = %d", len);
 203                         return false;
 204                 }
 205
 206                 ++read_input_cnt;
 207
 208                 if (accel_input.type == EV_REL) {
 209                         switch (accel_input.code) {
 210                         case REL_X:
 211                                 accel_raw[0] = (int)accel_input.value;
 212                                 x = true;
 213                                 break;
 214                         case REL_Y:
 215                                 accel_raw[1] = (int)accel_input.value;
 216                                 y = true;
 217                                 break;
 218                         case REL_Z:
 219                                 accel_raw[2] = (int)accel_input.value;
 220                                 z = true;
 221                                 break;
 222                         default:
 223                                 _E("accel_input event[type = %d, code = %d] is unknown.", accel_input.type, accel_input.code);
 224                                 return false;
 225                                 break;
 226                         }
 227                 } else if (accel_input.type == EV_SYN) {
 228                         syn = true;
 229                         fired_time = util::get_timestamp(&accel_input.time);
 230                 } else {
 231                         _E("accel_input event[type = %d, code = %d] is unknown.", accel_input.type, accel_input.code);
 232                         return false;
 233                 }
 234         }
 235
 236         if (syn == false) {
 237                 _E("EV_SYN didn't come until %d inputs had come", read_input_cnt);
 238                 return false;
 239         }
 240
 241         if (x)
 242                 m_x =  accel_raw[0];
 243         if (y)
 244                 m_y =  accel_raw[1];
 245         if (z)
 246                 m_z =  accel_raw[2];
 247
 248         m_fired_time = fired_time;
 249
 250         _D("m_x = %d, m_y = %d, m_z = %d, time = %lluus", m_x, m_y, m_z, m_fired_time);
 251
 252         return true;
 253 }
 254
 255 bool accel_device::update_value_iio(void)
 256 {
 257         struct {
 258                 int16_t x;
 259                 int16_t y;
 260                 int16_t z;
 261                 int64_t timestamp;
 262         } __attribute__((packed)) data;
 263
 264         struct pollfd pfd;
 265
 266         pfd.fd = m_node_handle;
 267         pfd.events = POLLIN | POLLERR;
 268         pfd.revents = 0;
 269
 270         int ret = poll(&pfd, 1, -1);
 271
 272         if (ret == -1) {
 273                 _ERRNO(errno, _E, "Failed to poll from m_node_handle:%d", m_node_handle);
 274                 return false;
 275         } else if (!ret) {
 276                 _E("poll timeout m_node_handle:%d", m_node_handle);
 277                 return false;
 278         }
 279
 280         if (pfd.revents & POLLERR) {
 281                 _E("poll exception occurred! m_node_handle:%d", m_node_handle);
 282                 return false;
 283         }
 284
 285         if (!(pfd.revents & POLLIN)) {
 286                 _E("poll nothing to read! m_node_handle:%d, pfd.revents = %d", m_node_handle, pfd.revents);
 287                 return false;
 288         }
 289
 290         int len = read(m_node_handle, &data, sizeof(data));
 291
 292         if (len != sizeof(data)) {
 293                 _E("Failed to read data, m_node_handle:%d read_len:%d", m_node_handle, len);
 294                 return false;
 295         }
 296
 297         m_x = data.x;
 298         m_y = data.y;
 299         m_z = data.z;
 300         m_fired_time = NSEC_TO_MSEC(data.timestamp);
 301
 302         _D("m_x = %d, m_y = %d, m_z = %d, time = %lluus", m_x, m_y, m_z, m_fired_time);
 303
 304         return true;
 305 }
 306
 307 int accel_device::read_fd(uint32_t **ids)
 308 {
 309         if (!update_value()) {
 310                 _D("Failed to update value");
 311                 return false;
 312         }
 313
 314         event_ids.clear();
 315         event_ids.push_back(sensor_info.id);
 316
 317         *ids = &event_ids[0];
 318
 319         return event_ids.size();
 320 }
 321
 322 int accel_device::get_data(uint32_t id, sensor_data_t **data, int *length)
 323 {
 324         sensor_data_t *sensor_data;
 325         sensor_data = (sensor_data_t *)malloc(sizeof(sensor_data_t));
 326         retvm_if(!sensor_data, -ENOMEM, "Memory allocation failed");
 327
 328         sensor_data->accuracy = SENSOR_ACCURACY_GOOD;
 329         sensor_data->timestamp = m_fired_time;
 330         sensor_data->value_count = 3;
 331         sensor_data->values[0] = m_x;
 332         sensor_data->values[1] = m_y;
 333         sensor_data->values[2] = m_z;
 334
 335         raw_to_base(sensor_data);
 336
 337         *data = sensor_data;
 338         *length = sizeof(sensor_data_t);
 339
 340         return 0;
 341 }
 342
 343 void accel_device::raw_to_base(sensor_data_t *data)
 344 {
 345         data->values[0] = RAW_DATA_TO_METRE_PER_SECOND_SQUARED_UNIT(data->values[0] * RAW_DATA_UNIT);
 346         data->values[1] = RAW_DATA_TO_METRE_PER_SECOND_SQUARED_UNIT(data->values[1] * RAW_DATA_UNIT);
 347         data->values[2] = RAW_DATA_TO_METRE_PER_SECOND_SQUARED_UNIT(data->values[2] * RAW_DATA_UNIT);
 348 }