/*
- * Copyright (c) 2016 Samsung Electronics Co., Ltd.
+ * 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.
#include "accel_device.h"
-#define MODEL_NAME "K2HH"
+#define MODEL_NAME "LSM9DS1"
#define VENDOR "ST Microelectronics"
#define RESOLUTION 16
#define RAW_DATA_UNIT 0.122
#define MAX_ID 0x3
+#define I2C_BUS_ADDRESS 1
+#define I2C_SLAVE_ADDRESS 0x6A
+
+#define CTRL_REG5_XL 0x1F
+#define CTRL_REG6_XL 0x20
+
+#define OUT_X_L_XL 0x28
+#define OUT_X_H_XL 0x29
+#define OUT_Y_L_XL 0x2A
+#define OUT_Y_H_XL 0x2B
+#define OUT_Z_L_XL 0x2C
+#define OUT_Z_H_XL 0x2D
+
static sensor_info_t sensor_info = {
id: 0x1,
name: SENSOR_NAME,
, m_polling_interval(1000)
, m_fired_time(0)
, m_sensorhub_controlled(false)
+, m_i2c_handle(nullptr)
{
const std::string sensorhub_interval_node_name = ACCEL_SENSORHUB_POLL_NODE_NAME;
- node_info_query query;
- node_info info;
-
- query.sensorhub_controlled = m_sensorhub_controlled = util::is_sensorhub_controlled(sensorhub_interval_node_name);
- query.sensor_type = SENSOR_TYPE_ACCEL;
- query.key = INPUT_NAME;
- query.iio_enable_node_name = "accel_enable";
- query.sensorhub_interval_node_name = sensorhub_interval_node_name;
-
- if (!util::get_node_info(query, info)) {
- _E("Failed to get node info");
- throw ENXIO;
- }
+ m_method = I2C_METHOD;
- util::show_node_info(info);
-
- m_method = info.method;
- m_data_node = info.data_node_path;
- m_enable_node = info.enable_node_path;
- m_interval_node = info.interval_node_path;
-
- m_node_handle = open(m_data_node.c_str(), O_RDONLY);
-
- if (m_node_handle < 0) {
+ if (peripheral_i2c_open(I2C_BUS_ADDRESS, I2C_SLAVE_ADDRESS, &m_i2c_handle) < 0) {
_ERRNO(errno, _E, "accel handle open fail for accel processor");
throw ENXIO;
}
- if (m_method == INPUT_EVENT_METHOD) {
- if (!util::set_monotonic_clock(m_node_handle))
- throw ENXIO;
-
- update_value = [=]() {
- return this->update_value_input_event();
- };
- } else {
- if (!info.buffer_length_node_path.empty())
- util::set_node_value(info.buffer_length_node_path, 480);
-
- if (!info.buffer_enable_node_path.empty())
- util::set_node_value(info.buffer_enable_node_path, 1);
-
- update_value = [=]() {
- return this->update_value_iio();
- };
- }
+ peripheral_i2c_write_register_byte(m_i2c_handle, CTRL_REG5_XL, 0b00111000);
+ peripheral_i2c_write_register_byte(m_i2c_handle, CTRL_REG6_XL, 0b01000000);
+ update_value = [=]() {
+ return this->update_value_i2c();
+ };
_I("accel_device is created!");
}
accel_device::~accel_device()
{
- close(m_node_handle);
- m_node_handle = -1;
+ if (!m_i2c_handle) {
+ peripheral_i2c_write_register_byte(m_i2c_handle, CTRL_REG6_XL, 0);
+
+ peripheral_i2c_close(m_i2c_handle);
+ m_i2c_handle = nullptr;
+ }
_I("accel_device is destroyed!");
}
int accel_device::get_poll_fd(void)
{
- return m_node_handle;
+ //return m_i2c_handle->fd;
+ return -1;
}
int accel_device::get_sensors(const sensor_info_t **sensors)
{
retvm_if(id == 0 || id > MAX_ID, false, "%s:Invalid ID Received", SENSOR_NAME);
- util::set_enable_node(m_enable_node, m_sensorhub_controlled, true, SENSORHUB_ACCELEROMETER_ENABLE_BIT);
- set_interval(id, m_polling_interval);
-
- m_fired_time = 0;
+ peripheral_i2c_write_register_byte(m_i2c_handle, CTRL_REG5_XL, 0b00111000);
+ peripheral_i2c_write_register_byte(m_i2c_handle, CTRL_REG6_XL, 0b01000000);
_I("Enable accelerometer sensor");
return true;
}
{
retvm_if(id == 0 || id > MAX_ID, false, "%s:Invalid ID Received", SENSOR_NAME);
- util::set_enable_node(m_enable_node, m_sensorhub_controlled, false, SENSORHUB_ACCELEROMETER_ENABLE_BIT);
-
+ peripheral_i2c_write_register_byte(m_i2c_handle, CTRL_REG6_XL, 0);
_I("Disable accelerometer sensor");
return true;
}
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);
+ /*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 accel_device::update_value_input_event(void)
+bool accel_device::update_value_i2c(void)
{
- int accel_raw[3] = {0, };
- bool x, y, z;
- int read_input_cnt = 0;
- const int INPUT_MAX_BEFORE_SYN = 10;
- unsigned long long fired_time = 0;
- bool syn = false;
-
- x = y = z = false;
-
- struct input_event accel_input;
- _D("accel sensor event detection");
-
- while ((syn == false) && (read_input_cnt < INPUT_MAX_BEFORE_SYN)) {
- int len = read(m_node_handle, &accel_input, sizeof(accel_input));
- if (len != sizeof(accel_input)) {
- _E("accel_file read fail, read_len = %d", len);
- return false;
- }
-
- ++read_input_cnt;
-
- if (accel_input.type == EV_REL) {
- switch (accel_input.code) {
- case REL_X:
- accel_raw[0] = (int)accel_input.value;
- x = true;
- break;
- case REL_Y:
- accel_raw[1] = (int)accel_input.value;
- y = true;
- break;
- case REL_Z:
- accel_raw[2] = (int)accel_input.value;
- z = true;
- break;
- default:
- _E("accel_input event[type = %d, code = %d] is unknown.", accel_input.type, accel_input.code);
- return false;
- break;
- }
- } else if (accel_input.type == EV_SYN) {
- syn = true;
- fired_time = util::get_timestamp(&accel_input.time);
- } else {
- _E("accel_input event[type = %d, code = %d] is unknown.", accel_input.type, accel_input.code);
- return false;
- }
- }
-
- if (syn == false) {
- _E("EV_SYN didn't come until %d inputs had come", read_input_cnt);
- return false;
- }
-
- if (x)
- m_x = accel_raw[0];
- if (y)
- m_y = accel_raw[1];
- if (z)
- m_z = accel_raw[2];
-
- m_fired_time = fired_time;
-
- _D("m_x = %d, m_y = %d, m_z = %d, time = %lluus", m_x, m_y, m_z, m_fired_time);
-
- return true;
-}
-
-bool accel_device::update_value_iio(void)
-{
- struct {
- int16_t x;
- int16_t y;
- int16_t z;
- int64_t timestamp;
- } __attribute__((packed)) data;
-
- struct pollfd pfd;
-
- pfd.fd = m_node_handle;
- pfd.events = POLLIN | POLLERR;
- pfd.revents = 0;
-
- int ret = poll(&pfd, 1, -1);
-
- if (ret == -1) {
- _ERRNO(errno, _E, "Failed to poll from m_node_handle:%d", m_node_handle);
- return false;
- } else if (!ret) {
- _E("poll timeout m_node_handle:%d", m_node_handle);
- return false;
- }
-
- if (pfd.revents & POLLERR) {
- _E("poll exception occurred! m_node_handle:%d", m_node_handle);
- return false;
- }
-
- if (!(pfd.revents & POLLIN)) {
- _E("poll nothing to read! m_node_handle:%d, pfd.revents = %d", m_node_handle, pfd.revents);
- return false;
- }
-
- int len = read(m_node_handle, &data, sizeof(data));
-
- if (len != sizeof(data)) {
- _E("Failed to read data, m_node_handle:%d read_len:%d", m_node_handle, len);
- return false;
- }
-
- m_x = data.x;
- m_y = data.y;
- m_z = data.z;
- m_fired_time = data.timestamp;
-
- _D("m_x = %d, m_y = %d, m_z = %d, time = %lluus", m_x, m_y, m_z, m_fired_time);
-
+ uint8_t temp[6];
+ peripheral_i2c_read_register_byte(m_i2c_handle, OUT_X_L_XL, &temp[0]);
+ peripheral_i2c_read_register_byte(m_i2c_handle, OUT_X_H_XL, &temp[1]);
+ peripheral_i2c_read_register_byte(m_i2c_handle, OUT_Y_L_XL, &temp[2]);
+ peripheral_i2c_read_register_byte(m_i2c_handle, OUT_Y_H_XL, &temp[3]);
+ peripheral_i2c_read_register_byte(m_i2c_handle, OUT_Z_L_XL, &temp[4]);
+ peripheral_i2c_read_register_byte(m_i2c_handle, OUT_Z_H_XL, &temp[5]);
+
+ //high 8bits and low 8bits
+ m_x = (temp[1] << 8) | temp[0];
+ m_y = (temp[3] << 8) | temp[2];
+ m_z = (temp[5] << 8) | temp[4];
+
+ //time stamp
return true;
}
sensor_data->values[1] = m_y;
sensor_data->values[2] = m_z;
- raw_to_base(sensor_data);
+ //raw_to_base(sensor_data);
*data = sensor_data;
*length = sizeof(sensor_data_t);
projects / platform / hal / backend / rpi / sensor-rpi.git / commitdiff