4 * Copyright (c) 2014 Samsung Electronics Co., Ltd.
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
10 * http://www.apache.org/licenses/LICENSE-2.0
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.
22 #include <linux/input.h>
23 #include <csensor_config.h>
24 #include <gyro_sensor_hal.h>
25 #include <sys/ioctl.h>
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))
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"
42 #define ATTR_VALUE "value"
44 gyro_sensor_hal::gyro_sensor_hal()
49 , m_polling_interval(POLL_1HZ_MS)
52 const string sensorhub_interval_node_name = "gyro_poll_delay";
53 csensor_config &config = csensor_config::get_instance();
55 node_info_query query;
58 if (!find_model_id(SENSOR_TYPE_GYRO, m_model_id)) {
59 ERR("Failed to find model id");
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;
70 if (!get_node_info(query, info)) {
71 ERR("Failed to get node info");
77 m_data_node = info.data_node_path;
78 m_enable_node = info.enable_node_path;
79 m_interval_node = info.interval_node_path;
81 if (!config.get(SENSOR_TYPE_GYRO, m_model_id, ELEMENT_VENDOR, m_vendor)) {
82 ERR("[VENDOR] is empty\n");
86 INFO("m_vendor = %s", m_vendor.c_str());
88 if (!config.get(SENSOR_TYPE_GYRO, m_model_id, ELEMENT_NAME, m_chip_name)) {
89 ERR("[NAME] is empty\n");
93 INFO("m_chip_name = %s\n",m_chip_name.c_str());
97 if (!config.get(SENSOR_TYPE_GYRO, m_model_id, ELEMENT_RESOLUTION, resolution)) {
98 ERR("[RESOLUTION] is empty\n");
102 m_resolution = (int)resolution;
104 double raw_data_unit;
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");
111 m_raw_data_unit = (float)(raw_data_unit);
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));
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());
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");
127 gyro_sensor_hal::~gyro_sensor_hal()
129 close(m_node_handle);
132 INFO("gyro_sensor is destroyed!\n");
135 string gyro_sensor_hal::get_model_id(void)
140 sensor_hal_type_t gyro_sensor_hal::get_type(void)
142 return SENSOR_HAL_TYPE_GYROSCOPE;
145 bool gyro_sensor_hal::enable(void)
149 set_enable_node(m_enable_node, m_sensorhub_controlled, true, SENSORHUB_GYROSCOPE_ENABLE_BIT);
150 set_interval(m_polling_interval);
153 INFO("Gyro sensor real starting");
157 bool gyro_sensor_hal::disable(void)
161 set_enable_node(m_enable_node, m_sensorhub_controlled, false, SENSORHUB_GYROSCOPE_ENABLE_BIT);
163 INFO("Gyro sensor real stopping");
168 bool gyro_sensor_hal::set_interval(unsigned long val)
170 unsigned long long polling_interval_ns;
174 polling_interval_ns = ((unsigned long long)(val) * 1000llu * 1000llu);
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());
181 INFO("Interval is changed from %dms to %dms]", m_polling_interval, val);
182 m_polling_interval = val;
186 bool gyro_sensor_hal::update_value(void)
188 int gyro_raw[3] = {0,};
190 int read_input_cnt = 0;
191 const int INPUT_MAX_BEFORE_SYN = 10;
192 unsigned long long fired_time = 0;
197 struct input_event gyro_input;
198 DBG("gyro event detection!");
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));
209 if (gyro_input.type == EV_REL) {
210 switch (gyro_input.code) {
212 gyro_raw[0] = (int)gyro_input.value;
216 gyro_raw[1] = (int)gyro_input.value;
220 gyro_raw[2] = (int)gyro_input.value;
224 ERR("gyro_input event[type = %d, code = %d] is unknown.", gyro_input.type, gyro_input.code);
228 } else if (gyro_input.type == EV_SYN) {
230 fired_time = sensor_hal_base::get_timestamp(&gyro_input.time);
232 ERR("gyro_input event[type = %d, code = %d] is unknown.", gyro_input.type, gyro_input.code);
237 AUTOLOCK(m_value_mutex);
246 m_fired_time = fired_time;
248 DBG("m_x = %d, m_y = %d, m_z = %d, time = %lluus", m_x, m_y, m_z, m_fired_time);
253 bool gyro_sensor_hal::is_data_ready(void)
256 ret = update_value();
260 int gyro_sensor_hal::get_sensor_data(sensor_data_t &data)
262 AUTOLOCK(m_value_mutex);
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;
274 bool gyro_sensor_hal::get_properties(sensor_properties_s &properties)
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;