src/gyro/gyro_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 "gyro_device.h"
  33
  34 #define MODEL_NAME "ICM20610"
  35 #define VENDOR "Invensense"
  36 #define RESOLUTION 16
  37 #define RAW_DATA_UNIT 61.04
  38 #define MIN_INTERVAL 5
  39 #define MAX_BATCH_COUNT 30
  40
  41 #define SENSOR_NAME "SENSOR_GYROSCOPE"
  42 #define SENSOR_TYPE_GYRO                "GYRO"
  43
  44 #define INPUT_NAME      "gyro_sensor"
  45 #define GYRO_SENSORHUB_POLL_NODE_NAME "gyro_poll_delay"
  46
  47 #define DPS_TO_MDPS 1000
  48 #define RAW_DATA_TO_DPS_UNIT(X) ((float)(X)/((float)DPS_TO_MDPS))
  49 #define MIN_RANGE(RES) (-((1 << (RES))/2))
  50 #define MAX_RANGE(RES) (((1 << (RES))/2)-1)
  51
  52 static sensor_info_t sensor_info = {
  53         id: 0x1,
  54         name: SENSOR_NAME,
  55         type: SENSOR_DEVICE_GYROSCOPE,
  56         event_type: (SENSOR_DEVICE_GYROSCOPE << SENSOR_EVENT_SHIFT) | RAW_DATA_EVENT,
  57         model_name: MODEL_NAME,
  58         vendor: VENDOR,
  59         min_range: MIN_RANGE(RESOLUTION) * RAW_DATA_TO_DPS_UNIT(RAW_DATA_UNIT),
  60         max_range: MAX_RANGE(RESOLUTION) * RAW_DATA_TO_DPS_UNIT(RAW_DATA_UNIT),
  61         resolution: RAW_DATA_TO_DPS_UNIT(RAW_DATA_UNIT),
  62         min_interval: MIN_INTERVAL,
  63         max_batch_count: MAX_BATCH_COUNT,
  64         wakeup_supported: false
  65 };
  66
  67 gyro_device::gyro_device()
  68 : m_node_handle(-1)
  69 , m_x(-1)
  70 , m_y(-1)
  71 , m_z(-1)
  72 , m_polling_interval(1000)
  73 , m_fired_time(0)
  74 , m_sensorhub_controlled(false)
  75 {
  76         const std::string sensorhub_interval_node_name = GYRO_SENSORHUB_POLL_NODE_NAME;
  77
  78         node_info_query query;
  79         node_info info;
  80
  81         query.sensorhub_controlled = m_sensorhub_controlled = util::is_sensorhub_controlled(sensorhub_interval_node_name);
  82         query.sensor_type = SENSOR_TYPE_GYRO;
  83         query.key = INPUT_NAME;
  84         query.iio_enable_node_name = "gyro_enable";
  85         query.sensorhub_interval_node_name = sensorhub_interval_node_name;
  86
  87         if (!util::get_node_info(query, info)) {
  88                 _E("Failed to get node info");
  89                 throw ENXIO;
  90         }
  91
  92         util::show_node_info(info);
  93
  94         m_method = info.method;
  95         m_data_node = info.data_node_path;
  96         m_enable_node = info.enable_node_path;
  97         m_interval_node = info.interval_node_path;
  98
  99         m_node_handle = open(m_data_node.c_str(), O_RDONLY);
 100
 101         if (m_node_handle < 0) {
 102                 _ERRNO(errno, _E, "Failed to open gyro handle");
 103                 throw ENXIO;
 104         }
 105
 106         if (m_method == INPUT_EVENT_METHOD) {
 107                 if (!util::set_monotonic_clock(m_node_handle))
 108                         throw ENXIO;
 109
 110                 update_value = [=]() {
 111                         return this->update_value_input_event();
 112                 };
 113         } else {
 114                 if (!info.buffer_length_node_path.empty())
 115                         util::set_node_value(info.buffer_length_node_path, 600);
 116
 117                 if (!info.buffer_enable_node_path.empty())
 118                         util::set_node_value(info.buffer_enable_node_path, 1);
 119
 120                 update_value = [=]() {
 121                         return this->update_value_iio();
 122                 };
 123         }
 124
 125         _I("gyro_device is created!");
 126 }
 127
 128 gyro_device::~gyro_device()
 129 {
 130         close(m_node_handle);
 131         m_node_handle = -1;
 132
 133         _I("gyro_device is destroyed!");
 134 }
 135
 136 int gyro_device::get_poll_fd(void)
 137 {
 138         return m_node_handle;
 139 }
 140
 141 int gyro_device::get_sensors(const sensor_info_t **sensors)
 142 {
 143         *sensors = &sensor_info;
 144
 145         return 1;
 146 }
 147
 148 bool gyro_device::enable(uint32_t id)
 149 {
 150         util::set_enable_node(m_enable_node, m_sensorhub_controlled, true, SENSORHUB_GYROSCOPE_ENABLE_BIT);
 151         set_interval(id, m_polling_interval);
 152
 153         m_fired_time = 0;
 154         _I("Enable gyroscope sensor");
 155         return true;
 156 }
 157
 158 bool gyro_device::disable(uint32_t id)
 159 {
 160         util::set_enable_node(m_enable_node, m_sensorhub_controlled, false, SENSORHUB_GYROSCOPE_ENABLE_BIT);
 161
 162         _I("Disable gyroscope sensor");
 163         return true;
 164 }
 165
 166 bool gyro_device::set_interval(uint32_t id, unsigned long val)
 167 {
 168         unsigned long long polling_interval_ns;
 169
 170         polling_interval_ns = ((unsigned long long)(val) * 1000llu * 1000llu);
 171
 172         if (!util::set_node_value(m_interval_node, polling_interval_ns)) {
 173                 _E("Failed to set polling resource: %s", m_interval_node.c_str());
 174                 return false;
 175         }
 176
 177         _I("Interval is changed from %dms to %dms", m_polling_interval, val);
 178         m_polling_interval = val;
 179         return true;
 180 }
 181
 182 bool gyro_device::update_value_input_event(void)
 183 {
 184         int gyro_raw[3] = {0, };
 185         bool x, y, z;
 186         int read_input_cnt = 0;
 187         const int INPUT_MAX_BEFORE_SYN = 10;
 188         unsigned long long fired_time = 0;
 189         bool syn = false;
 190
 191         x = y = z = false;
 192
 193         struct input_event gyro_input;
 194         _D("gyro event detection!");
 195
 196         while ((syn == false) && (read_input_cnt < INPUT_MAX_BEFORE_SYN)) {
 197                 int len = read(m_node_handle, &gyro_input, sizeof(gyro_input));
 198                 if (len != sizeof(gyro_input)) {
 199                         _E("gyro_file read fail, read_len = %d", len);
 200                         return false;
 201                 }
 202
 203                 ++read_input_cnt;
 204
 205                 if (gyro_input.type == EV_REL) {
 206                         switch (gyro_input.code) {
 207                         case REL_RX:
 208                                 gyro_raw[0] = (int)gyro_input.value;
 209                                 x = true;
 210                                 break;
 211                         case REL_RY:
 212                                 gyro_raw[1] = (int)gyro_input.value;
 213                                 y = true;
 214                                 break;
 215                         case REL_RZ:
 216                                 gyro_raw[2] = (int)gyro_input.value;
 217                                 z = true;
 218                                 break;
 219                         default:
 220                                 _E("gyro_input event[type = %d, code = %d] is unknown.", gyro_input.type, gyro_input.code);
 221                                 return false;
 222                                 break;
 223                         }
 224                 } else if (gyro_input.type == EV_SYN) {
 225                         syn = true;
 226                         fired_time = util::get_timestamp(&gyro_input.time);
 227                 } else {
 228                         _E("gyro_input event[type = %d, code = %d] is unknown.", gyro_input.type, gyro_input.code);
 229                         return false;
 230                 }
 231         }
 232
 233         if (syn == false) {
 234                 _E("EV_SYN didn't come until %d inputs had come", read_input_cnt);
 235                 return false;
 236         }
 237
 238         if (x)
 239                 m_x =  gyro_raw[0];
 240         if (y)
 241                 m_y =  gyro_raw[1];
 242         if (z)
 243                 m_z =  gyro_raw[2];
 244
 245         m_fired_time = fired_time;
 246
 247         _D("m_x = %d, m_y = %d, m_z = %d, time = %lluus", m_x, m_y, m_z, m_fired_time);
 248
 249         return true;
 250 }
 251
 252 bool gyro_device::update_value_iio(void)
 253 {
 254         struct {
 255                 int32_t x;
 256                 int32_t y;
 257                 int32_t z;
 258                 int64_t timestamp;
 259         } __attribute__((packed)) data;
 260
 261         struct pollfd pfd;
 262
 263         pfd.fd = m_node_handle;
 264         pfd.events = POLLIN | POLLERR;
 265         pfd.revents = 0;
 266
 267         int ret = poll(&pfd, 1, -1);
 268
 269         if (ret == -1) {
 270                 _ERRNO(errno, _E, "Failed to poll from m_node_handle:%d", m_node_handle);
 271                 return false;
 272         } else if (!ret) {
 273                 _E("poll timeout m_node_handle:%d", m_node_handle);
 274                 return false;
 275         }
 276
 277         if (pfd.revents & POLLERR) {
 278                 _E("poll exception occurred! m_node_handle:%d", m_node_handle);
 279                 return false;
 280         }
 281
 282         if (!(pfd.revents & POLLIN)) {
 283                 _E("poll nothing to read! m_node_handle:%d, pfd.revents = %d", m_node_handle, pfd.revents);
 284                 return false;
 285         }
 286
 287         int len = read(m_node_handle, &data, sizeof(data));
 288
 289         if (len != sizeof(data)) {
 290                 _E("Failed to read data, m_node_handle:%d read_len:%d", m_node_handle, len);
 291                 return false;
 292         }
 293
 294         m_x = data.x;
 295         m_y = data.y;
 296         m_z = data.z;
 297         m_fired_time = NSEC_TO_MSEC(data.timestamp);
 298
 299         _D("m_x = %d, m_y = %d, m_z = %d, time = %lluus", m_x, m_y, m_z, m_fired_time);
 300
 301         return true;
 302 }
 303
 304 int gyro_device::read_fd(uint32_t **ids)
 305 {
 306         if (!update_value()) {
 307                 _D("Failed to update value");
 308                 return false;
 309         }
 310
 311         event_ids.clear();
 312         event_ids.push_back(sensor_info.id);
 313
 314         *ids = &event_ids[0];
 315
 316         return event_ids.size();
 317 }
 318
 319 int gyro_device::get_data(uint32_t id, sensor_data_t **data, int *length)
 320 {
 321         sensor_data_t *sensor_data;
 322         sensor_data = (sensor_data_t *)malloc(sizeof(sensor_data_t));
 323         retvm_if(!sensor_data, -ENOMEM, "Memory allocation failed");
 324
 325         sensor_data->accuracy = SENSOR_ACCURACY_GOOD;
 326         sensor_data->timestamp = m_fired_time;
 327         sensor_data->value_count = 3;
 328         sensor_data->values[0] = m_x;
 329         sensor_data->values[1] = m_y;
 330         sensor_data->values[2] = m_z;
 331
 332         raw_to_base(sensor_data);
 333
 334         *data = sensor_data;
 335         *length = sizeof(sensor_data_t);
 336
 337         return 0;
 338 }
 339
 340 void gyro_device::raw_to_base(sensor_data_t *data)
 341 {
 342         data->values[0] = data->values[0] * RAW_DATA_TO_DPS_UNIT(RAW_DATA_UNIT);
 343         data->values[1] = data->values[1] * RAW_DATA_TO_DPS_UNIT(RAW_DATA_UNIT);
 344         data->values[2] = data->values[2] * RAW_DATA_TO_DPS_UNIT(RAW_DATA_UNIT);
 345 }