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_log.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_info_t sensor_info = {
  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<uint32_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_RDONLY)) < 0) {
  94                 _ERRNO(errno, _E, "accel handle open fail for accel processor");
  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_info_t **sensors)
 115 {
 116         *sensors = &sensor_info;
 117
 118         return 1;
 119 }
 120
 121 bool accel_device::enable(uint32_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(uint32_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(uint32_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::update_value_input_event(void)
 156 {
 157         int accel_raw[3] = {0,};
 158         bool x,y,z;
 159         int read_input_cnt = 0;
 160         const int INPUT_MAX_BEFORE_SYN = 10;
 161         unsigned long long fired_time = 0;
 162         bool syn = false;
 163
 164         x = y = z = false;
 165
 166         struct input_event accel_input;
 167         DBG("accel event detection!");
 168
 169         while ((syn == false) && (read_input_cnt < INPUT_MAX_BEFORE_SYN)) {
 170                 int len = read(m_node_handle, &accel_input, sizeof(accel_input));
 171                 if (len != sizeof(accel_input)) {
 172                         ERR("accel_file read fail, read_len = %d\n",len);
 173                         return false;
 174                 }
 175
 176                 ++read_input_cnt;
 177
 178                 if (accel_input.type == EV_REL) {
 179                         switch (accel_input.code) {
 180                         case REL_X:
 181                                 accel_raw[0] = (int)accel_input.value;
 182                                 x = true;
 183                                 break;
 184                         case REL_Y:
 185                                 accel_raw[1] = (int)accel_input.value;
 186                                 y = true;
 187                                 break;
 188                         case REL_Z:
 189                                 accel_raw[2] = (int)accel_input.value;
 190                                 z = true;
 191                                 break;
 192                         default:
 193                                 ERR("accel_input event[type = %d, code = %d] is unknown.", accel_input.type, accel_input.code);
 194                                 return false;
 195                                 break;
 196                         }
 197                 } else if (accel_input.type == EV_SYN) {
 198                         syn = true;
 199                         fired_time = util::get_timestamp(&accel_input.time);
 200                 } else {
 201                         ERR("accel_input event[type = %d, code = %d] is unknown.", accel_input.type, accel_input.code);
 202                         return false;
 203                 }
 204         }
 205
 206         if (syn == false) {
 207                 ERR("EV_SYN didn't come until %d inputs had come", read_input_cnt);
 208                 return false;
 209         }
 210
 211         if (x)
 212                 m_x =  accel_raw[0];
 213         if (y)
 214                 m_y =  accel_raw[1];
 215         if (z)
 216                 m_z =  accel_raw[2];
 217
 218         m_fired_time = fired_time;
 219
 220         DBG("m_x = %d, m_y = %d, m_z = %d, time = %lluus", m_x, m_y, m_z, m_fired_time);
 221
 222         return true;
 223 }
 224
 225 int accel_device::read_fd(uint32_t **ids)
 226 {
 227         if (!update_value_input_event()) {
 228                 DBG("Failed to update value");
 229                 return false;
 230         }
 231
 232         event_ids.clear();
 233         event_ids.push_back(sensor_info.id);
 234
 235         *ids = &event_ids[0];
 236
 237         return event_ids.size();
 238 }
 239
 240 int accel_device::get_data(uint32_t id, sensor_data_t **data, int *length)
 241 {
 242         int remains = 1;
 243         sensor_data_t *sensor_data;
 244         sensor_data = (sensor_data_t *)malloc(sizeof(sensor_data_t));
 245
 246         sensor_data->accuracy = SENSOR_ACCURACY_GOOD;
 247         sensor_data->timestamp = m_fired_time;
 248         sensor_data->value_count = 3;
 249         sensor_data->values[0] = m_x;
 250         sensor_data->values[1] = m_y;
 251         sensor_data->values[2] = m_z;
 252
 253         raw_to_base(sensor_data);
 254
 255         *data = sensor_data;
 256         *length = sizeof(sensor_data_t);
 257
 258         return --remains;
 259 }
 260
 261 void accel_device::raw_to_base(sensor_data_t *data)
 262 {
 263         data->value_count = 3;
 264         data->values[0] = RAW_DATA_TO_METRE_PER_SECOND_SQUARED_UNIT(data->values[0] * RAW_DATA_UNIT);
 265         data->values[1] = RAW_DATA_TO_METRE_PER_SECOND_SQUARED_UNIT(data->values[1] * RAW_DATA_UNIT);
 266         data->values[2] = RAW_DATA_TO_METRE_PER_SECOND_SQUARED_UNIT(data->values[2] * RAW_DATA_UNIT);
 267 }