src/rotation_vector/rv_raw/rv_raw_sensor_hal.cpp

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