src/plugins/accel/accel_sensor_device.cpp

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