--- /dev/null
+/*
+ * Copyright (c) 2021 Samsung Electronics Co., Ltd.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *
+ */
+
+#include <fcntl.h>
+#include <unistd.h>
+#include <sys/types.h>
+#include <sys/stat.h>
+
+#include <linux/input.h>
+#include <sys/ioctl.h>
+#include <poll.h>
+
+#include <util.h>
+#include <sensor_common.h>
+#include <sensor_log.h>
+
+#include "gyro_device.h"
+
+#define MODEL_NAME "LSM9DS1"
+#define VENDOR "ST Microelectronics"
+
+#define MIN_INTERVAL 10
+#define MAX_BATCH_COUNT 0
+
+#define SENSOR_NAME "SENSOR_GYROSCOPE"
+#define SENSOR_TYPE_ACCEL "GYRO"
+
+#define INPUT_NAME "gyroscope_sensor"
+#define GYRO_SENSORHUB_POLL_NODE_NAME "gyro_poll_delay"
+
+#define MAX_ID 0x3
+
+#define I2C_BUS_ADDRESS 1
+#define I2C_SLAVE_ADDRESS 0x6A
+
+#define CTRL_REG1_G 0x10
+#define CTRL_REG2_G 0x11
+#define CTRL_REG3_G 0x12
+
+#define OUT_X_L_G 0x18
+#define OUT_X_H_G 0x19
+#define OUT_Y_L_G 0x1A
+#define OUT_Y_H_G 0x1B
+#define OUT_Z_L_G 0x1C
+#define OUT_Z_H_G 0x1D
+
+#define CTRL_REG9 0x23
+#define FIFO_CTRL 0x2E
+#define FIFO_SRC 0x2F
+
+static sensor_info_t sensor_info = {
+ id: 0x1,
+ name: SENSOR_NAME,
+ type: SENSOR_DEVICE_GYROSCOPE,
+ event_type: (SENSOR_DEVICE_GYROSCOPE << SENSOR_EVENT_SHIFT) | RAW_DATA_EVENT,
+ model_name: MODEL_NAME,
+ vendor: VENDOR,
+ min_range: -245,
+ max_range: 245,
+ resolution: 0.00875,
+ min_interval: MIN_INTERVAL,
+ max_batch_count: MAX_BATCH_COUNT,
+ wakeup_supported: false
+};
+
+gyro_device::gyro_device()
+: m_node_handle(-1)
+, m_x(-1)
+, m_y(-1)
+, m_z(-1)
+, m_polling_interval(1000)
+, m_fired_time(0)
+, m_sensorhub_controlled(false)
+, m_i2c_handle(nullptr)
+{
+ const std::string sensorhub_interval_node_name = GYRO_SENSORHUB_POLL_NODE_NAME;
+
+ m_method = I2C_METHOD;
+
+ if (peripheral_i2c_open(I2C_BUS_ADDRESS, I2C_SLAVE_ADDRESS, &m_i2c_handle) < 0) {
+ _ERRNO(errno, _E, "gyro handle open fail for gyro processor");
+ throw ENXIO;
+ }
+
+ update_value = [=]() {
+ return this->update_value_i2c();
+ };
+
+ _I("gyro_device is created!");
+}
+
+gyro_device::~gyro_device()
+{
+ if (!m_i2c_handle) {
+ peripheral_i2c_write_register_byte(m_i2c_handle, CTRL_REG1_G, 0);
+
+ peripheral_i2c_close(m_i2c_handle);
+ m_i2c_handle = nullptr;
+ }
+
+ _I("gyro_device is destroyed!");
+}
+int gyro_device::get_poll_fd(void)
+{
+ //return m_i2c_handle->fd;
+ return -1;
+}
+
+int gyro_device::get_sensors(const sensor_info_t **sensors)
+{
+ retvm_if(sensors == NULL || sensors == nullptr, -EINVAL, "%s:NULL interface", SENSOR_NAME);
+ *sensors = &sensor_info;
+
+ return 1;
+}
+
+bool gyro_device::enable(uint32_t id)
+{
+ uint8_t temp;
+ retvm_if(id == 0 || id > MAX_ID, false, "%s:Invalid ID Received", SENSOR_NAME);
+
+ //turn on the sensor
+ set_fullscale();
+ peripheral_i2c_read_register_byte(m_i2c_handle, CTRL_REG1_G, &temp);
+ temp |= 0b00100000;
+ peripheral_i2c_write_register_byte(m_i2c_handle, CTRL_REG1_G, temp);
+
+ //set FIFO
+ peripheral_i2c_read_register_byte(m_i2c_handle, CTRL_REG9, &temp);
+ temp |= 0b00000010;
+ peripheral_i2c_write_register_byte(m_i2c_handle, CTRL_REG9, temp);
+ peripheral_i2c_write_register_byte(m_i2c_handle, FIFO_CTRL, 0b00111111);
+ uint16_t samples = 0;
+ while (samples < 0x1F){
+ peripheral_i2c_read_register_word(m_i2c_handle, FIFO_SRC, &samples);
+ samples &= 0x3F;
+ }
+
+ //calibration
+ for (int i = 0; i < samples; ++i){
+ update_value_i2c();
+ bias[0] += m_x;
+ bias[1] += m_y;
+ bias[2] += m_z;
+ }
+ bias[0] /= samples;
+ bias[1] /= samples;
+ bias[2] /= samples;
+
+ //disable FIFO
+ peripheral_i2c_read_register_byte(m_i2c_handle, CTRL_REG9, &temp);
+ temp &= ~(0b00000010);
+ peripheral_i2c_write_register_byte(m_i2c_handle, CTRL_REG9, temp);
+ peripheral_i2c_write_register_byte(m_i2c_handle, FIFO_CTRL, 0);
+
+ _I("Enable gyroscope sensor");
+ return true;
+}
+
+bool gyro_device::disable(uint32_t id)
+{
+ retvm_if(id == 0 || id > MAX_ID, false, "%s:Invalid ID Received", SENSOR_NAME);
+
+ peripheral_i2c_write_register_byte(m_i2c_handle, CTRL_REG1_G, 0);
+ _I("Disable gyroscope sensor");
+ return true;
+}
+
+bool gyro_device::set_interval(uint32_t id, unsigned long val)
+{
+ unsigned long long polling_interval_ns;
+ retvm_if(id == 0 || id > MAX_ID, false, "%s:Invalid ID Received", SENSOR_NAME);
+
+ /*polling_interval_ns = ((unsigned long long)(val) * 1000llu * 1000llu);
+
+ if (!util::set_node_value(m_interval_node, polling_interval_ns)) {
+ _E("Failed to set polling resource: %s", m_interval_node.c_str());
+ return false;
+ }*/
+
+ _I("Interval is changed from %lu ms to %lu ms", m_polling_interval, val);
+ m_polling_interval = val;
+ return true;
+}
+
+bool gyro_device::update_value_i2c(void)
+{
+ uint16_t temp[3];
+ peripheral_i2c_read_register_word(m_i2c_handle, OUT_X_L_G, &temp[0]);
+ peripheral_i2c_read_register_word(m_i2c_handle, OUT_Y_L_G, &temp[1]);
+ peripheral_i2c_read_register_word(m_i2c_handle, OUT_Z_L_G, &temp[2]);
+
+ m_x = temp[0];
+ m_y = temp[1];
+ m_z = temp[2];
+
+ return true;
+}
+
+int gyro_device::read_fd(uint32_t **ids)
+{
+ retvm_if(ids == NULL || ids == nullptr, -EINVAL, "%s:NULL interface", SENSOR_NAME);
+
+ if (!update_value()) {
+ _D("Failed to update value");
+ return false;
+ }
+
+ event_ids.clear();
+ event_ids.push_back(sensor_info.id);
+
+ *ids = &event_ids[0];
+
+ return event_ids.size();
+}
+
+int gyro_device::get_data(uint32_t id, sensor_data_t **data, int *length)
+{
+ sensor_data_t *sensor_data;
+ retvm_if(data == NULL || data == nullptr, -EINVAL, "%s:NULL data interface", SENSOR_NAME);
+ retvm_if(length == NULL || length == nullptr, -EINVAL, "%s:NULL length interface", SENSOR_NAME);
+ retvm_if(id == 0 || id > MAX_ID, -EINVAL, "%s:Invalid ID Received", SENSOR_NAME);
+
+ sensor_data = new (std::nothrow) sensor_data_t;
+ retvm_if(!sensor_data, -ENOMEM, "Memory allocation failed");
+
+ sensor_data->accuracy = SENSOR_ACCURACY_GOOD;
+ sensor_data->timestamp = m_fired_time;
+ sensor_data->value_count = 3;
+ sensor_data->values[0] = m_x;
+ sensor_data->values[1] = m_y;
+ sensor_data->values[2] = m_z;
+
+ raw_to_base(sensor_data);
+
+ *data = sensor_data;
+ *length = sizeof(sensor_data_t);
+
+ return 0;
+}
+
+void gyro_device::set_fullscale(void)
+{
+ uint8_t temp;
+
+ //set fullscale ±245degree/s
+ peripheral_i2c_read_register_byte(m_i2c_handle, CTRL_REG1_G, &temp);
+ temp &= 0b11100111;
+ peripheral_i2c_write_register_byte(m_i2c_handle, CTRL_REG1_G, temp);
+
+ //unit is 0.00875degree/s
+ res = 0.00875;
+}
+
+void gyro_device::raw_to_base(sensor_data_t *data)
+{
+ data->values[0] = (data->values[0] - bias[0]) * res;
+ data->values[1] = (data->values[1] - bias[1]) * res;
+ data->values[2] = (data->values[2] - bias[2]) * res;
+}
--- /dev/null
+/*
+ * Copyright (c) 2021 Samsung Electronics Co., Ltd.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *
+ */
+
+#ifndef _GYRO_DEVICE_H_
+#define _GYRO_DEVICE_H_
+
+#include <hal/hal-sensor-types.h>
+#include <string>
+#include <vector>
+#include <functional>
+#include <peripheral_io.h>
+
+class gyro_device : public sensor_device {
+public:
+ gyro_device();
+ virtual ~gyro_device();
+
+ int get_poll_fd(void);
+ int get_sensors(const sensor_info_t **sensors);
+
+ bool enable(uint32_t id);
+ bool disable(uint32_t id);
+
+ bool set_interval(uint32_t id, unsigned long val);
+
+ int read_fd(uint32_t **ids);
+ int get_data(uint32_t id, sensor_data_t **data, int *length);
+
+private:
+ int m_node_handle;
+ int m_x;
+ int m_y;
+ int m_z;
+ int bias[3];
+ float res;
+
+ unsigned long m_polling_interval;
+ unsigned long long m_fired_time;
+ bool m_sensorhub_controlled;
+
+ int m_method;
+ std::string m_data_node;
+ std::string m_enable_node;
+ std::string m_interval_node;
+
+ std::function<bool(void)> update_value;
+
+ std::vector<uint32_t> event_ids;
+ peripheral_i2c_h m_i2c_handle;
+
+ bool update_value_i2c(void);
+ void set_fullscale(void);
+ void raw_to_base(sensor_data_t *data);
+};
+#endif /* _GYRO_DEVICE_H_ */