src/accel/accel.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 #include <linux/input.h>
  23 #include <util.h>
  24 #include <sensor_logs.h>
  25
  26 #include "accel.h"
  27
  28 #define GRAVITY 9.80665
  29 #define G_TO_MG 1000
  30 #define RAW_DATA_TO_G_UNIT(X) (((float)(X))/((float)G_TO_MG))
  31 #define RAW_DATA_TO_METRE_PER_SECOND_SQUARED_UNIT(X) (GRAVITY * (RAW_DATA_TO_G_UNIT(X)))
  32
  33 #define MIN_RANGE(RES) (-((1 << (RES))/2))
  34 #define MAX_RANGE(RES) (((1 << (RES))/2)-1)
  35
  36 #define MODEL_NAME "K2HH"
  37 #define VENDOR "ST Microelectronics"
  38 #define RESOLUTION 16
  39 #define RAW_DATA_UNIT 0.122
  40 #define MIN_INTERVAL 1
  41 #define MAX_BATCH_COUNT 0
  42
  43 #define SENSORHUB_ACCELEROMETER_ENABLE_BIT 0
  44
  45 static const sensor_handle_t handle = {
  46         id: 0x1,
  47         name: "Accelerometer",
  48         type: SENSOR_DEVICE_ACCELEROMETER,
  49         event_type: (SENSOR_DEVICE_ACCELEROMETER << 16) | 0x0001,
  50         model_name: MODEL_NAME,
  51         vendor: VENDOR,
  52         min_range: MIN_RANGE(RESOLUTION) * RAW_DATA_TO_METRE_PER_SECOND_SQUARED_UNIT(RAW_DATA_UNIT),
  53         max_range: MAX_RANGE(RESOLUTION) * RAW_DATA_TO_METRE_PER_SECOND_SQUARED_UNIT(RAW_DATA_UNIT),
  54         resolution: RAW_DATA_TO_METRE_PER_SECOND_SQUARED_UNIT(RAW_DATA_UNIT),
  55         min_interval: MIN_INTERVAL,
  56         max_batch_count: MAX_BATCH_COUNT,
  57         wakeup_supported: false
  58 };
  59
  60 std::vector<uint16_t> accel_device::event_ids;
  61
  62 accel_device::accel_device()
  63 : m_node_handle(-1)
  64 , m_x(-1)
  65 , m_y(-1)
  66 , m_z(-1)
  67 , m_polling_interval(0)
  68 , m_fired_time(0)
  69 , m_sensorhub_controlled(false)
  70 {
  71         const std::string sensorhub_interval_node_name = "accel_poll_delay";
  72
  73         node_info_query query;
  74         node_info info;
  75
  76         query.sensorhub_controlled = m_sensorhub_controlled = util::is_sensorhub_controlled(sensorhub_interval_node_name);
  77         query.sensor_type = "ACCEL";
  78         query.key = "accelerometer_sensor";
  79         query.iio_enable_node_name = "accel_enable";
  80         query.sensorhub_interval_node_name = sensorhub_interval_node_name;
  81
  82         if (!util::get_node_info(query, info)) {
  83                 ERR("Failed to get node info");
  84                 throw ENXIO;
  85         }
  86
  87         util::show_node_info(info);
  88
  89         m_data_node = info.data_node_path;
  90         m_enable_node = info.enable_node_path;
  91         m_interval_node = info.interval_node_path;
  92
  93         if ((m_node_handle = open(m_data_node.c_str(), O_RDWR)) < 0) {
  94                 ERR("accel handle open fail for accel processor, error:%s\n", strerror(errno));
  95                 throw ENXIO;
  96         }
  97
  98         INFO("accel_device is created!\n");
  99 }
 100
 101 accel_device::~accel_device()
 102 {
 103         close(m_node_handle);
 104         m_node_handle = -1;
 105
 106         INFO("accel_device is destroyed!\n");
 107 }
 108
 109 int accel_device::get_poll_fd()
 110 {
 111         return m_node_handle;
 112 }
 113
 114 int accel_device::get_sensors(const sensor_handle_t **sensors)
 115 {
 116         *sensors = &handle;
 117
 118         return 1;
 119 }
 120
 121 bool accel_device::enable(uint16_t id)
 122 {
 123         util::set_enable_node(m_enable_node, m_sensorhub_controlled, true, SENSORHUB_ACCELEROMETER_ENABLE_BIT);
 124         set_interval(id, m_polling_interval);
 125
 126         m_fired_time = 0;
 127         INFO("Enable accelerometer sensor");
 128         return true;
 129 }
 130
 131 bool accel_device::disable(uint16_t id)
 132 {
 133         util::set_enable_node(m_enable_node, m_sensorhub_controlled, false, SENSORHUB_ACCELEROMETER_ENABLE_BIT);
 134
 135         INFO("Disable accelerometer sensor");
 136         return true;
 137 }
 138
 139 bool accel_device::set_interval(uint16_t id, unsigned long val)
 140 {
 141         unsigned long long polling_interval_ns;
 142
 143         polling_interval_ns = ((unsigned long long)(val) * 1000llu * 1000llu);
 144
 145         if (!util::set_node_value(m_interval_node, polling_interval_ns)) {
 146                 ERR("Failed to set polling resource: %s\n", m_interval_node.c_str());
 147                 return false;
 148         }
 149
 150         INFO("Interval is changed from %dms to %dms]", m_polling_interval, val);
 151         m_polling_interval = val;
 152         return true;
 153 }
 154
 155 bool accel_device::set_batch_latency(uint16_t id, unsigned long val)
 156 {
 157         return false;
 158 }
 159
 160 bool accel_device::set_attribute(uint16_t id, int32_t attribute, int32_t value)
 161 {
 162         return false;
 163 }
 164
 165 bool accel_device::set_attribute_str(uint16_t id, char *attribute, char *value, int value_len)
 166 {
 167         return false;
 168 }
 169
 170 bool accel_device::update_value_input_event(void)
 171 {
 172         int accel_raw[3] = {0,};
 173         bool x,y,z;
 174         int read_input_cnt = 0;
 175         const int INPUT_MAX_BEFORE_SYN = 10;
 176         unsigned long long fired_time = 0;
 177         bool syn = false;
 178
 179         x = y = z = false;
 180
 181         struct input_event accel_input;
 182         DBG("accel event detection!");
 183
 184         while ((syn == false) && (read_input_cnt < INPUT_MAX_BEFORE_SYN)) {
 185                 int len = read(m_node_handle, &accel_input, sizeof(accel_input));
 186                 if (len != sizeof(accel_input)) {
 187                         ERR("accel_file read fail, read_len = %d\n",len);
 188                         return false;
 189                 }
 190
 191                 ++read_input_cnt;
 192
 193                 if (accel_input.type == EV_REL) {
 194                         switch (accel_input.code) {
 195                         case REL_X:
 196                                 accel_raw[0] = (int)accel_input.value;
 197                                 x = true;
 198                                 break;
 199                         case REL_Y:
 200                                 accel_raw[1] = (int)accel_input.value;
 201                                 y = true;
 202                                 break;
 203                         case REL_Z:
 204                                 accel_raw[2] = (int)accel_input.value;
 205                                 z = true;
 206                                 break;
 207                         default:
 208                                 ERR("accel_input event[type = %d, code = %d] is unknown.", accel_input.type, accel_input.code);
 209                                 return false;
 210                                 break;
 211                         }
 212                 } else if (accel_input.type == EV_SYN) {
 213                         syn = true;
 214                         fired_time = util::get_timestamp(&accel_input.time);
 215                 } else {
 216                         ERR("accel_input event[type = %d, code = %d] is unknown.", accel_input.type, accel_input.code);
 217                         return false;
 218                 }
 219         }
 220
 221         if (syn == false) {
 222                 ERR("EV_SYN didn't come until %d inputs had come", read_input_cnt);
 223                 return false;
 224         }
 225
 226         if (x)
 227                 m_x =  accel_raw[0];
 228         if (y)
 229                 m_y =  accel_raw[1];
 230         if (z)
 231                 m_z =  accel_raw[2];
 232
 233         m_fired_time = fired_time;
 234
 235         DBG("m_x = %d, m_y = %d, m_z = %d, time = %lluus", m_x, m_y, m_z, m_fired_time);
 236
 237         return true;
 238 }
 239
 240 int accel_device::read_fd(uint16_t **ids)
 241 {
 242         if (!update_value_input_event()) {
 243                 DBG("Failed to update value");
 244                 return false;
 245         }
 246
 247         event_ids.clear();
 248         event_ids.push_back(handle.id);
 249
 250         *ids = &event_ids[0];
 251
 252         return event_ids.size();
 253 }
 254
 255 int accel_device::get_data(uint16_t id, sensor_data_t **data, int *length)
 256 {
 257         int remains = 1;
 258         sensor_data_t *sensor_data;
 259         sensor_data = (sensor_data_t *)malloc(sizeof(sensor_data_t));
 260
 261         sensor_data->accuracy = SENSOR_ACCURACY_GOOD;
 262         sensor_data->timestamp = m_fired_time;
 263         sensor_data->value_count = 3;
 264         sensor_data->values[0] = m_x;
 265         sensor_data->values[1] = m_y;
 266         sensor_data->values[2] = m_z;
 267
 268         raw_to_base(sensor_data);
 269
 270         *data = sensor_data;
 271         *length = sizeof(sensor_data_t);
 272
 273         return --remains;
 274 }
 275
 276 bool accel_device::flush(uint16_t id)
 277 {
 278         return false;
 279 }
 280
 281 void accel_device::raw_to_base(sensor_data_t *data)
 282 {
 283         data->value_count = 3;
 284         data->values[0] = RAW_DATA_TO_METRE_PER_SECOND_SQUARED_UNIT(data->values[0] * RAW_DATA_UNIT);
 285         data->values[1] = RAW_DATA_TO_METRE_PER_SECOND_SQUARED_UNIT(data->values[1] * RAW_DATA_UNIT);
 286         data->values[2] = RAW_DATA_TO_METRE_PER_SECOND_SQUARED_UNIT(data->values[2] * RAW_DATA_UNIT);
 287 }