src/plugins/gyro/gyro_sensor_hal.cpp

   1 /*
   2  * gyro_sensor_hal
   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 <sys/stat.h>
  21 #include <dirent.h>
  22 #include <linux/input.h>
  23 #include <csensor_config.h>
  24 #include <gyro_sensor_hal.h>
  25 #include <sys/ioctl.h>
  26 #include <fstream>
  27
  28 using std::ifstream;
  29 using std::string;
  30
  31 #define DPS_TO_MDPS 1000
  32 #define MIN_RANGE(RES) (-((1 << (RES))/2))
  33 #define MAX_RANGE(RES) (((1 << (RES))/2)-1)
  34 #define RAW_DATA_TO_DPS_UNIT(X) ((float)(X)/((float)DPS_TO_MDPS))
  35
  36 #define SENSOR_TYPE_GYRO                "GYRO"
  37 #define ELEMENT_NAME                    "NAME"
  38 #define ELEMENT_VENDOR                  "VENDOR"
  39 #define ELEMENT_RAW_DATA_UNIT   "RAW_DATA_UNIT"
  40 #define ELEMENT_RESOLUTION              "RESOLUTION"
  41
  42 #define ATTR_VALUE                              "value"
  43
  44 gyro_sensor_hal::gyro_sensor_hal()
  45 : m_x(-1)
  46 , m_y(-1)
  47 , m_z(-1)
  48 , m_node_handle(-1)
  49 , m_polling_interval(POLL_1HZ_MS)
  50 , m_fired_time(0)
  51 {
  52         const string sensorhub_interval_node_name = "gyro_poll_delay";
  53         csensor_config &config = csensor_config::get_instance();
  54
  55         node_info_query query;
  56         node_info info;
  57
  58         if (!find_model_id(SENSOR_TYPE_GYRO, m_model_id)) {
  59                 ERR("Failed to find model id");
  60                 throw ENXIO;
  61
  62         }
  63
  64         query.sensorhub_controlled = m_sensorhub_controlled = is_sensorhub_controlled(sensorhub_interval_node_name);
  65         query.sensor_type = SENSOR_TYPE_GYRO;
  66         query.key = "gyro_sensor";
  67         query.iio_enable_node_name = "gyro_enable";
  68         query.sensorhub_interval_node_name = sensorhub_interval_node_name;
  69
  70         if (!get_node_info(query, info)) {
  71                 ERR("Failed to get node info");
  72                 throw ENXIO;
  73         }
  74
  75         show_node_info(info);
  76
  77         m_data_node = info.data_node_path;
  78         m_enable_node = info.enable_node_path;
  79         m_interval_node = info.interval_node_path;
  80
  81         if (!config.get(SENSOR_TYPE_GYRO, m_model_id, ELEMENT_VENDOR, m_vendor)) {
  82                 ERR("[VENDOR] is empty\n");
  83                 throw ENXIO;
  84         }
  85
  86         INFO("m_vendor = %s", m_vendor.c_str());
  87
  88         if (!config.get(SENSOR_TYPE_GYRO, m_model_id, ELEMENT_NAME, m_chip_name)) {
  89                 ERR("[NAME] is empty\n");
  90                 throw ENXIO;
  91         }
  92
  93         INFO("m_chip_name = %s\n",m_chip_name.c_str());
  94
  95         long resolution;
  96
  97         if (!config.get(SENSOR_TYPE_GYRO, m_model_id, ELEMENT_RESOLUTION, resolution)) {
  98                 ERR("[RESOLUTION] is empty\n");
  99                 throw ENXIO;
 100         }
 101
 102         m_resolution = (int)resolution;
 103
 104         double raw_data_unit;
 105
 106         if (!config.get(SENSOR_TYPE_GYRO, m_model_id, ELEMENT_RAW_DATA_UNIT, raw_data_unit)) {
 107                 ERR("[RAW_DATA_UNIT] is empty\n");
 108                 throw ENXIO;
 109         }
 110
 111         m_raw_data_unit = (float)(raw_data_unit);
 112
 113         if ((m_node_handle = open(m_data_node.c_str(), O_RDWR)) < 0) {
 114                 ERR("gyro handle open fail for gyro processor, error:%s\n", strerror(errno));
 115                 throw ENXIO;
 116         }
 117
 118         int clockId = CLOCK_MONOTONIC;
 119         if (ioctl(m_node_handle, EVIOCSCLOCKID, &clockId) != 0)
 120                 ERR("Fail to set monotonic timestamp for %s", m_data_node.c_str());
 121
 122         INFO("m_raw_data_unit = %f\n",m_raw_data_unit);
 123         INFO("RAW_DATA_TO_DPS_UNIT(m_raw_data_unit) = [%f]",RAW_DATA_TO_DPS_UNIT(m_raw_data_unit));
 124         INFO("gyro_sensor is created!\n");
 125 }
 126
 127 gyro_sensor_hal::~gyro_sensor_hal()
 128 {
 129         close(m_node_handle);
 130         m_node_handle = -1;
 131
 132         INFO("gyro_sensor is destroyed!\n");
 133 }
 134
 135 string gyro_sensor_hal::get_model_id(void)
 136 {
 137         return m_model_id;
 138 }
 139
 140 sensor_hal_type_t gyro_sensor_hal::get_type(void)
 141 {
 142         return SENSOR_HAL_TYPE_GYROSCOPE;
 143 }
 144
 145 bool gyro_sensor_hal::enable(void)
 146 {
 147         AUTOLOCK(m_mutex);
 148
 149         set_enable_node(m_enable_node, m_sensorhub_controlled, true, SENSORHUB_GYROSCOPE_ENABLE_BIT);
 150         set_interval(m_polling_interval);
 151
 152         m_fired_time = 0;
 153         INFO("Gyro sensor real starting");
 154         return true;
 155 }
 156
 157 bool gyro_sensor_hal::disable(void)
 158 {
 159         AUTOLOCK(m_mutex);
 160
 161         set_enable_node(m_enable_node, m_sensorhub_controlled, false, SENSORHUB_GYROSCOPE_ENABLE_BIT);
 162
 163         INFO("Gyro sensor real stopping");
 164         return true;
 165
 166 }
 167
 168 bool gyro_sensor_hal::set_interval(unsigned long val)
 169 {
 170         unsigned long long polling_interval_ns;
 171
 172         AUTOLOCK(m_mutex);
 173
 174         polling_interval_ns = ((unsigned long long)(val) * 1000llu * 1000llu);
 175
 176         if (!set_node_value(m_interval_node, polling_interval_ns)) {
 177                 ERR("Failed to set polling resource: %s\n", m_interval_node.c_str());
 178                 return false;
 179         }
 180
 181         INFO("Interval is changed from %dms to %dms]", m_polling_interval, val);
 182         m_polling_interval = val;
 183         return true;
 184 }
 185
 186 bool gyro_sensor_hal::update_value(void)
 187 {
 188         int gyro_raw[3] = {0,};
 189         bool x,y,z;
 190         int read_input_cnt = 0;
 191         const int INPUT_MAX_BEFORE_SYN = 10;
 192         unsigned long long fired_time = 0;
 193         bool syn = false;
 194
 195         x = y = z = false;
 196
 197         struct input_event gyro_input;
 198         DBG("gyro event detection!");
 199
 200         while ((syn == false) && (read_input_cnt < INPUT_MAX_BEFORE_SYN)) {
 201                 int len = read(m_node_handle, &gyro_input, sizeof(gyro_input));
 202                 if (len != sizeof(gyro_input)) {
 203                         ERR("gyro_file read fail, read_len = %d\n, %s",len, strerror(errno));
 204                         return false;
 205                 }
 206
 207                 ++read_input_cnt;
 208
 209                 if (gyro_input.type == EV_REL) {
 210                         switch (gyro_input.code) {
 211                                 case REL_RX:
 212                                         gyro_raw[0] = (int)gyro_input.value;
 213                                         x = true;
 214                                         break;
 215                                 case REL_RY:
 216                                         gyro_raw[1] = (int)gyro_input.value;
 217                                         y = true;
 218                                         break;
 219                                 case REL_RZ:
 220                                         gyro_raw[2] = (int)gyro_input.value;
 221                                         z = true;
 222                                         break;
 223                                 default:
 224                                         ERR("gyro_input event[type = %d, code = %d] is unknown.", gyro_input.type, gyro_input.code);
 225                                         return false;
 226                                         break;
 227                         }
 228                 } else if (gyro_input.type == EV_SYN) {
 229                         syn = true;
 230                         fired_time = sensor_hal_base::get_timestamp(&gyro_input.time);
 231                 } else {
 232                         ERR("gyro_input event[type = %d, code = %d] is unknown.", gyro_input.type, gyro_input.code);
 233                         return false;
 234                 }
 235         }
 236
 237         AUTOLOCK(m_value_mutex);
 238
 239         if (x)
 240                 m_x =  gyro_raw[0];
 241         if (y)
 242                 m_y =  gyro_raw[1];
 243         if (z)
 244                 m_z =  gyro_raw[2];
 245
 246         m_fired_time = fired_time;
 247
 248         DBG("m_x = %d, m_y = %d, m_z = %d, time = %lluus", m_x, m_y, m_z, m_fired_time);
 249
 250         return true;
 251 }
 252
 253 bool gyro_sensor_hal::is_data_ready(void)
 254 {
 255         bool ret;
 256         ret = update_value();
 257         return ret;
 258 }
 259
 260 int gyro_sensor_hal::get_sensor_data(sensor_data_t &data)
 261 {
 262         AUTOLOCK(m_value_mutex);
 263
 264         data.accuracy = SENSOR_ACCURACY_GOOD;
 265         data.timestamp = m_fired_time ;
 266         data.value_count = 3;
 267         data.values[0] = m_x;
 268         data.values[1] = m_y;
 269         data.values[2] = m_z;
 270
 271         return 0;
 272 }
 273
 274 bool gyro_sensor_hal::get_properties(sensor_properties_s &properties)
 275 {
 276         properties.name = m_chip_name;
 277         properties.vendor = m_vendor;
 278         properties.min_range = MIN_RANGE(m_resolution)* RAW_DATA_TO_DPS_UNIT(m_raw_data_unit);
 279         properties.max_range = MAX_RANGE(m_resolution)* RAW_DATA_TO_DPS_UNIT(m_raw_data_unit);
 280         properties.min_interval = 1;
 281         properties.resolution = RAW_DATA_TO_DPS_UNIT(m_raw_data_unit);
 282         properties.fifo_count = 0;
 283         properties.max_batch_count = 0;
 284         return true;
 285
 286 }