--- /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 "magnet_device.h"
+
+#define MODEL_NAME "LSM9DS1"
+#define VENDOR "ST Microelectronics"
+
+#define MIN_INTERVAL 10
+#define MAX_BATCH_COUNT 0
+
+#define SENSOR_NAME "SENSOR_MAGNETIC"
+
+#define SENSOR_TYPE_MAGNET "MANGET"
+
+#define INPUT_NAME "magnetometer_sensor"
+#define MAGNET_SENSORHUB_POLL_NODE_NAME "magnet_poll_delay"
+
+#define MAX_ID 0x3
+
+#define I2C_BUS_ADDRESS 1
+#define I2C_SLAVE_ADDRESS 0x1C
+
+#define OFFSET_X_REG_L_M 0x05
+#define OFFSET_X_REG_H_M 0x06
+#define OFFSET_Y_REG_L_M 0x07
+#define OFFSET_Y_REG_H_M 0x08
+#define OFFSET_Z_REG_L_M 0x09
+#define OFFSET_Z_REG_H_M 0x0A
+
+#define CTRL_REG1_M 0x20
+#define CTRL_REG2_M 0x21
+#define CTRL_REG3_M 0x22
+#define CTRL_REG4_M 0x23
+#define CTRL_REG5_M 0x24
+
+#define OUT_X_L_M 0x28
+#define OUT_X_H_M 0x29
+#define OUT_Y_L_M 0x2A
+#define OUT_Y_H_M 0x2B
+#define OUT_Z_L_M 0x2C
+#define OUT_Z_H_M 0x2D
+
+static sensor_info_t sensor_info = {
+ id: 0x1,
+ name: SENSOR_NAME,
+ type: SENSOR_DEVICE_MAGNETOMETER,
+ event_type: (SENSOR_DEVICE_MAGNETOMETER << SENSOR_EVENT_SHIFT) | RAW_DATA_EVENT,
+ model_name: MODEL_NAME,
+ vendor: VENDOR,
+ min_range: -400,
+ max_range: 400,
+ resolution: 14,
+ min_interval: MIN_INTERVAL,
+ max_batch_count: MAX_BATCH_COUNT,
+ wakeup_supported: false
+};
+
+magnet_device::magnet_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 = MAGNET_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, "magnet handle open fail for magnet processor");
+ throw ENXIO;
+ }
+
+ update_value = [=]() {
+ return this->update_value_i2c();
+ };
+
+ _I("magnet_device is created!");
+}
+
+magnet_device::~magnet_device()
+{
+ if (!m_i2c_handle) {
+ peripheral_i2c_write_register_byte(m_i2c_handle, CTRL_REG3_M, 0b00000011);
+
+ peripheral_i2c_close(m_i2c_handle);
+ m_i2c_handle = nullptr;
+ }
+
+ _I("magnet_device is destroyed!");
+}
+int magnet_device::get_poll_fd(void)
+{
+ //return m_i2c_handle->fd;
+ return -1;
+}
+
+int magnet_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 magnet_device::enable(uint32_t id)
+{
+ retvm_if(id == 0 || id > MAX_ID, false, "%s:Invalid ID Received", SENSOR_NAME);
+
+ //turn on the sensor
+ set_fullscale();
+ peripheral_i2c_write_register_byte(m_i2c_handle, CTRL_REG3_M, 0);
+
+ //calibration
+ int16_t bias[3] = {0, 0, 0};
+ int16_t bias_min[3] = {300000, 300000, 300000};
+ int16_t bias_max[3] = {-300000, -300000, -300000};
+
+ uint8_t msb, lsb;
+ for (int i = 0; i < 128; ++i){
+ while (!available_new_value()); // wait for new measured value
+ update_value_i2c();
+
+ if (bias_min[0] > x)
+ bias_min[0] = x;
+ if (bias_max[0] < x)
+ bias_max[0] = x;
+ if (bias_min[1] > y)
+ bias_min[1] = y;
+ if (bias_max[1] < y)
+ bias_max[1] = y;
+ if (bias_min[2] > z)
+ bias_min[2] = z;
+ if (bias_max[2] < z)
+ bias_max[2] = z;
+ }
+
+ bias[0] = (bias_max[0] + bias_min[0]) / 2;
+ bias[1] = (bias_max[1] + bias_min[1]) / 2;
+ bias[2] = (bias_max[2] + bias_min[2]) / 2;
+
+ msb = (bias[0] & 0xFF00) >> 8;
+ lsb = bias[0] & 0xFF;
+ peripheral_i2c_write_register_byte(m_i2c_handle, OFFSET_X_REG_L_M, lsb);
+ peripheral_i2c_write_register_byte(m_i2c_handle, OFFSET_X_REG_H_M, msb);
+
+ msb = (bias[1] & 0xFF00) >> 8;
+ lsb = bias[1] & 0xFF;
+ peripheral_i2c_write_register_byte(m_i2c_handle, OFFSET_Y_REG_L_M, lsb);
+ peripheral_i2c_write_register_byte(m_i2c_handle, OFFSET_Y_REG_H_M, msb);
+
+ msb = (bias[2] & 0xFF00) >> 8;
+ lsb = bias[2] & 0xFF;
+ peripheral_i2c_write_register_byte(m_i2c_handle, OFFSET_Z_REG_L_M, lsb);
+ peripheral_i2c_write_register_byte(m_i2c_handle, OFFSET_Z_REG_H_M, msb);
+
+ _I("Enable magnetometer sensor");
+ return true;
+}
+
+bool magnet_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_REG3_M, 0b00000011);
+ _I("Disable magnetometer sensor");
+ return true;
+}
+
+bool magnet_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 magnet_device::update_value_i2c(void)
+{
+ uint16_t temp[3];
+ peripheral_i2c_read_register_word(m_i2c_handle, OUT_X_L_M, &temp[0]);
+ peripheral_i2c_read_register_word(m_i2c_handle, OUT_Y_L_M, &temp[1]);
+ peripheral_i2c_read_register_word(m_i2c_handle, OUT_Z_L_M, &temp[2]);
+
+ m_x = temp[0];
+ m_y = temp[1];
+ m_z = temp[2];
+
+ return true;
+}
+
+int magnet_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 magnet_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 magnet_device::set_fullscale(void)
+{
+ uint8_t temp;
+
+ //set fullscale ±400uT
+ peripheral_i2c_read_register_byte(m_i2c_handle, CTRL_REG2_M, &temp);
+ temp &= 0b10011111;
+ peripheral_i2c_write_register_byte(m_i2c_handle, CTRL_REG2_M, temp);
+
+ //unit is 0.014uT
+ res = 0.014;
+}
+
+bool magnet_device::available_new_value(void)
+{
+ uint8_t temp;
+ peripheral_i2c_read_register_byte(m_i2c_handle, STATUS_REG_M, &temp);
+
+ if (temp & 0b00001000)
+ return true;
+ return false;
+}
+
+void magnet_device::raw_to_base(sensor_data_t *data)
+{
+ data->values[0] *= res;
+ data->values[1] *= res;
+ data->values[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 _MAGNET_DEVICE_H_
+#define _MAGNET_DEVICE_H_
+
+#include <hal/hal-sensor-types.h>
+#include <string>
+#include <vector>
+#include <functional>
+#include <peripheral_io.h>
+
+class magnet_device : public sensor_device {
+public:
+ magnet_device();
+ virtual ~magnet_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;
+ 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);
+ bool available_new_value(void);
+ void raw_to_base(sensor_data_t *data);
+};
+#endif /* _MAGNET_DEVICE_H_ */
projects / platform / hal / backend / rpi / sensor-rpi.git / commitdiff