From 64813b6b07f32bbee1a5f0743c5ea08458294d13 Mon Sep 17 00:00:00 2001 From: guozhanxin Date: Tue, 12 Feb 2019 10:15:26 +0800 Subject: [PATCH] add first version --- components/drivers/Kconfig | 11 + components/drivers/sensors/SConscript | 9 +- components/drivers/sensors/bmi055_sensor.cpp | 451 ------ components/drivers/sensors/bmi055_sensor.h | 334 ----- components/drivers/sensors/mpu6050_sensor.cpp | 458 ------ components/drivers/sensors/mpu6050_sensor.h | 189 --- components/drivers/sensors/sensor.c | 449 ++++++ components/drivers/sensors/sensor.cpp | 228 --- components/drivers/sensors/sensor.h | 1261 +++-------------- components/drivers/sensors/sensor_test.c | 361 +++++ components/finsh/cmd.c | 1 + include/rtdef.h | 1 + 12 files changed, 988 insertions(+), 2765 deletions(-) delete mode 100644 components/drivers/sensors/bmi055_sensor.cpp delete mode 100644 components/drivers/sensors/bmi055_sensor.h delete mode 100644 components/drivers/sensors/mpu6050_sensor.cpp delete mode 100644 components/drivers/sensors/mpu6050_sensor.h create mode 100644 components/drivers/sensors/sensor.c delete mode 100644 components/drivers/sensors/sensor.cpp create mode 100644 components/drivers/sensors/sensor_test.c diff --git a/components/drivers/Kconfig b/components/drivers/Kconfig index 8f66738260..6246a97160 100755 --- a/components/drivers/Kconfig +++ b/components/drivers/Kconfig @@ -239,6 +239,17 @@ config RT_USING_AUDIO bool "Using Audio device drivers" default n +config RT_USING_SENSOR + bool "Using Sensor device drivers" + select RT_USING_PIN + default n + +if RT_USING_SENSOR + config RT_USING_SENSOR_TEST + bool "Use Sensor test" + default y +endif + menu "Using WiFi" config RT_USING_WIFI bool "Using Wi-Fi framework" diff --git a/components/drivers/sensors/SConscript b/components/drivers/sensors/SConscript index db409cfd2b..e727b14136 100644 --- a/components/drivers/sensors/SConscript +++ b/components/drivers/sensors/SConscript @@ -3,14 +3,11 @@ from building import * cwd = GetCurrentDir() -src = ['sensor.cpp'] +src = ['sensor.c'] CPPPATH = [cwd, cwd + '/../include'] -if GetDepend('SENSOR_USING_MPU6050') and GetDepend('RT_USING_I2C'): - src += ['mpu6050_sensor.cpp']; - -if GetDepend('SENSOR_USING_BMI055') and GetDepend('RT_USING_I2C'): - src += ['bmi055_sensor.cpp'] +if GetDepend('RT_USING_SENSOR_TEST'): + src += ['sensor_test.c']; group = DefineGroup('Sensors', src, depend = ['RT_USING_SENSOR', 'RT_USING_DEVICE'], CPPPATH = CPPPATH) diff --git a/components/drivers/sensors/bmi055_sensor.cpp b/components/drivers/sensors/bmi055_sensor.cpp deleted file mode 100644 index b6e7f99da5..0000000000 --- a/components/drivers/sensors/bmi055_sensor.cpp +++ /dev/null @@ -1,451 +0,0 @@ -/* - * Copyright (c) 2006-2018, RT-Thread Development Team - * - * SPDX-License-Identifier: Apache-2.0 - * - * Change Logs: - * Date Author Notes - * 2015-1-11 RT_learning the first version - */ - -#include -#include -#include -#include "bmi055_sensor.h" - - -const static sensor_t _BMI055_sensor[] = -{ - { - .name = "Accelerometer", - .vendor = "Bosch", - .version = sizeof(sensor_t), - .handle = 0, - .type = SENSOR_TYPE_ACCELEROMETER, - .maxRange = SENSOR_ACCEL_RANGE_16G, - .resolution = 1.0f, - .power = 0.5f, - .minDelay = 10000, - .fifoReservedEventCount = 0, - .fifoMaxEventCount = 64, - }, - { - .name = "Gyroscope", - .vendor = "Bosch", - .version = sizeof(sensor_t), - .handle = 0, - .type = SENSOR_TYPE_GYROSCOPE, - .maxRange = SENSOR_GYRO_RANGE_2000DPS, - .resolution = 1.0f, - .power = 0.5f, - .minDelay = 10000, - .fifoReservedEventCount = 0, - .fifoMaxEventCount = 64, - } -}; - -BMI055::BMI055(int sensor_type, const char* iic_bus, int addr) - : SensorBase(sensor_type) -{ - this->i2c_bus = (struct rt_i2c_bus_device *)rt_device_find(iic_bus); - if (this->i2c_bus == NULL) - { - printf("BMI055: No IIC device:%s\n", iic_bus); - return; - } - - this->i2c_addr = addr; - - /* register to sensor manager */ - SensorManager::registerSensor(this); -} - -int BMI055::read_reg(rt_uint8_t reg, rt_uint8_t *value) -{ - struct rt_i2c_msg msgs[2]; - - msgs[0].addr = this->i2c_addr; - msgs[0].flags = RT_I2C_WR; - msgs[0].buf = ® - msgs[0].len = 1; - - msgs[1].addr = this->i2c_addr; - msgs[1].flags = RT_I2C_RD; /* Read from slave */ - msgs[1].buf = (rt_uint8_t *)value; - msgs[1].len = 1; - - if (rt_i2c_transfer(this->i2c_bus, msgs, 2) == 2) - return RT_EOK; - - return -RT_ERROR; -} - -int BMI055::read_buffer(rt_uint8_t reg, rt_uint8_t* value, rt_size_t size) -{ - struct rt_i2c_msg msgs[2]; - - msgs[0].addr = this->i2c_addr; - msgs[0].flags = RT_I2C_WR; - msgs[0].buf = ® - msgs[0].len = 1; - - msgs[1].addr = this->i2c_addr; - msgs[1].flags = RT_I2C_RD; /* Read from slave */ - msgs[1].buf = (rt_uint8_t *)value; - msgs[1].len = size; - - if (rt_i2c_transfer(this->i2c_bus, msgs, 2) == 2) - return RT_EOK; - - return -RT_ERROR; -} - -int BMI055::write_reg(rt_uint8_t reg, rt_uint8_t value) -{ - struct rt_i2c_msg msgs[2]; - - msgs[0].addr = this->i2c_addr; - msgs[0].flags = RT_I2C_WR; - msgs[0].buf = ® - msgs[0].len = 1; - - msgs[1].addr = this->i2c_addr; - msgs[1].flags = RT_I2C_WR | RT_I2C_NO_START; - msgs[1].buf = (rt_uint8_t *)&value; - msgs[1].len = 1; - - if (rt_i2c_transfer(this->i2c_bus, msgs, 2) == 2) - return RT_EOK; - - return -RT_ERROR; -} - - -BMI055_Accelerometer::BMI055_Accelerometer(const char* iic_name, int addr) - : BMI055(SENSOR_TYPE_ACCELEROMETER, iic_name, addr) -{ - int index; - uint8_t id; - rt_uint8_t value[6] = {0}; - rt_int32_t x, y, z; - SensorConfig config = {SENSOR_MODE_NORMAL, SENSOR_DATARATE_400HZ, SENSOR_ACCEL_RANGE_2G}; - - write_reg(BMI055_BGW_SOFTRESET, 0xB6); /* reset of the sensor P57 */ - write_reg(BMI055_PMU_LPW, 0x00); /* PMU_LPW NORMAL mode P55 */ - write_reg(BMI055_PMU_BW, 0x0A); /* 01010b 31.25 Hz P55 */ - write_reg(BMI055_PMU_RANGE, 0x05); /* 0101b 卤4g range PMU_RANGE set acc +-4g/s P54 */ - - - x_offset = y_offset = z_offset = 0; - x = y = z = 0; - - /* read BMI055 id */ - read_buffer(BMI055_ACC_BGW_CHIPID, &id, 1); /* BGW_CHIPID P47*/ - if (id != BMI055_ACC_BGW_CHIPID_VALUE) - { - printf("Warning: not found BMI055 id: %02x\n", id); - } - - /* get offset */ - for (index = 0; index < 200; index ++) - { - read_buffer(BMI055_ACCD_X_LSB, value, 6); /*ACCD_X_LSB P47 */ - - x += (((rt_int16_t)value[1] << 8) | value[0]); - y += (((rt_int16_t)value[3] << 8) | value[2]); - z += (((rt_int16_t)value[5] << 8) | value[4]); - } - x_offset = x / 200; - y_offset = y / 200; - z_offset = z / 200; - - this->enable = RT_FALSE; - this->sensitivity = SENSOR_ACCEL_SENSITIVITY_2G; - this->config = config; -} - -int -BMI055_Accelerometer::configure(SensorConfig *config) -{ - int range; - uint8_t value; - - if (config == RT_NULL) return -1; - - /* TODO: set datarate */ - - /* get range and calc the sensitivity */ - range = config->range.accel_range; - switch (range) - { - case SENSOR_ACCEL_RANGE_2G: - this->sensitivity = SENSOR_ACCEL_SENSITIVITY_2G; - range = 0x03; //0011b - break; - case SENSOR_ACCEL_RANGE_4G: - this->sensitivity = SENSOR_ACCEL_SENSITIVITY_4G; - range = 0x05; //0101b - break; - case SENSOR_ACCEL_RANGE_8G: - this->sensitivity = SENSOR_ACCEL_SENSITIVITY_8G; - range = 0x01 << 3; //1000b - break; - case SENSOR_ACCEL_RANGE_16G: - this->sensitivity = SENSOR_ACCEL_SENSITIVITY_16G; - range = 0x03 << 2; //1100b - break; - default: - return -1; - } - - /* set range to sensor */ - read_reg(BMI055_PMU_RANGE, &value); /* PMU_RANGE P54 */ - value &= 0xF0; - value |= range; - write_reg(BMI055_PMU_RANGE, value); - - return 0; -} - -int -BMI055_Accelerometer::activate(int enable) -{ - uint8_t value; - - if (enable && this->enable == RT_FALSE) - { - /* enable accelerometer */ - read_reg(BMI055_PMU_LPW, &value); /* P55 */ - value &= ~(0x07 << 7); - write_reg(BMI055_PMU_LPW, value); - } - - if (!enable && this->enable == RT_TRUE) - { - /* disable accelerometer */ - read_reg(BMI055_PMU_LPW, &value); - value &= ~(0x07 << 7); - value |= (0x01 << 7); - write_reg(BMI055_PMU_LPW, value); - } - - if (enable) this->enable = RT_TRUE; - else this->enable = RT_FALSE; - - return 0; -} - -int -BMI055_Accelerometer::poll(sensors_event_t *event) -{ - rt_uint8_t value[6]; - rt_int16_t x, y, z; - - /* parameters check */ - if (event == NULL) return -1; - - /* get event data */ - event->version = sizeof(sensors_event_t); - event->sensor = (int32_t) this; - event->timestamp = rt_tick_get(); - event->type = SENSOR_TYPE_ACCELEROMETER; - - read_buffer(0x02, value, 6); - - /* get raw data */ - x = (((rt_int16_t)value[1] << 8) | value[0]); - y = (((rt_int16_t)value[3] << 8) | value[2]); - z = (((rt_int16_t)value[5] << 8) | value[4]); - - if (config.mode == SENSOR_MODE_RAW) - { - event->raw_acceleration.x = x; - event->raw_acceleration.y = y; - event->raw_acceleration.z = z; - } - else - { - - x -= x_offset; y -= y_offset; z -= z_offset; - event->acceleration.x = x * this->sensitivity * SENSORS_GRAVITY_STANDARD; - event->acceleration.y = y * this->sensitivity * SENSORS_GRAVITY_STANDARD; - event->acceleration.z = z * this->sensitivity * SENSORS_GRAVITY_STANDARD; - } - - return 0; -} - -void -BMI055_Accelerometer::getSensor(sensor_t *sensor) -{ - /* get sensor description */ - if (sensor) - { - memcpy(sensor, &_BMI055_sensor[0], sizeof(sensor_t)); - } -} - -BMI055_Gyroscope::BMI055_Gyroscope(const char* iic_name, int addr) - : BMI055(SENSOR_TYPE_GYROSCOPE, iic_name, addr) -{ - int index; - uint8_t id; - rt_uint8_t value[6]; - rt_int32_t x, y, z; - - /* initialize BMI055 */ - write_reg(BMI055_MODE_LPM1_ADDR, 0x00); /* normal mode */ - write_reg(BMI055_MODE_LPM2_ADDR, 0x80); /* fast powerup */ - write_reg(BMI055_BW_ADDR, 0x03); /* ODR:400Hz Filter Bandwidth:47Hz */ - write_reg(BMI055_RANGE_ADDR, 0x00); /* 2000dps */ - - - x_offset = y_offset = z_offset = 0; - x = y = z = 0; - - /* read BMI055 id */ - read_buffer(BMI055_CHIP_ID_ADDR, &id, 1); - if (id != BMI055_GRRO_CHIP_ID) - { - printf("Warning: not found BMI055 id: %02x\n", id); - } - - /* get offset */ - for (index = 0; index < 200; index ++) - { - read_buffer(BMI055_RATE_X_LSB_ADDR, value, 6); - - x += (((rt_int16_t)value[1] << 8) | value[0]); - y += (((rt_int16_t)value[3] << 8) | value[2]); - z += (((rt_int16_t)value[5] << 8) | value[4]); - } - x_offset = x / 200; - y_offset = y / 200; - z_offset = z / 200; - - this->enable = RT_FALSE; - this->sensitivity = SENSOR_GYRO_SENSITIVITY_250DPS; -} - -int -BMI055_Gyroscope::configure(SensorConfig *config) -{ - int range; - uint8_t value; - - if (config == RT_NULL) return -1; - - /* TODO: set datarate */ - - /* get range and calc the sensitivity */ - range = config->range.gyro_range; - switch (range) - { - //to do add more range e.g 125DPS - //case - case SENSOR_GYRO_RANGE_250DPS: - this->sensitivity = SENSOR_GYRO_SENSITIVITY_250DPS; - range = 0x11; - break; - case SENSOR_GYRO_RANGE_500DPS: - this->sensitivity = SENSOR_GYRO_SENSITIVITY_500DPS; - range = 0x10; - break; - case SENSOR_GYRO_RANGE_1000DPS: - this->sensitivity = SENSOR_GYRO_SENSITIVITY_1000DPS; - range = 0x01; - break; - case SENSOR_GYRO_RANGE_2000DPS: - this->sensitivity = SENSOR_GYRO_SENSITIVITY_2000DPS; - range = 0x00; - break; - default: - return -1; - } - - /* set range to sensor */ - read_reg(BMI055_RANGE_ADDR, &value); - value &= ~0x07; - value |= range; - write_reg(BMI055_RANGE_ADDR, value); - - return 0; -} - -int -BMI055_Gyroscope::activate(int enable) -{ - uint8_t value; - - if (enable && this->enable == RT_FALSE) - { - /* enable gyroscope */ - read_reg(BMI055_MODE_LPM1_ADDR, &value); - value &= ~(0x1010 << 4); //{0; 0} NORMAL mode - write_reg(BMI055_MODE_LPM1_ADDR, value); //P101 NORMAL mode - } - - if (!enable && this->enable == RT_TRUE) - { - /* disable gyroscope */ - read_reg(BMI055_MODE_LPM1_ADDR, &value); - value &= ~(0x01 << 5); //set bit5 deep_suspend 0 - value |= (0x01 << 7); //set bit1 suspend 1 - write_reg(BMI055_MODE_LPM1_ADDR, value); //{1; 0} SUSPEND mode - } - - if (enable) this->enable = RT_TRUE; - else this->enable = RT_FALSE; - - return 0; -} - -int -BMI055_Gyroscope::poll(sensors_event_t *event) -{ - rt_uint8_t value[6]; - rt_int16_t x, y, z; - - /* parameters check */ - if (event == NULL) return -1; - - /* get event data */ - event->version = sizeof(sensors_event_t); - event->sensor = (int32_t) this; - event->timestamp = rt_tick_get(); - event->type = SENSOR_TYPE_GYROSCOPE; - - read_buffer(BMI055_RATE_X_LSB_ADDR, value, 6); - - /* get raw data */ - x = (((rt_int16_t)value[1] << 8) | value[0]); - y = (((rt_int16_t)value[3] << 8) | value[2]); - z = (((rt_int16_t)value[5] << 8) | value[4]); - - - if (config.mode == SENSOR_MODE_RAW) - { - event->raw_gyro.x = x; - event->raw_gyro.y = y; - event->raw_gyro.z = z; - } - else - { - x -= x_offset; y -= y_offset; z -= z_offset; - event->gyro.x = x * this->sensitivity * SENSORS_DPS_TO_RADS; - event->gyro.y = y * this->sensitivity * SENSORS_DPS_TO_RADS; - event->gyro.z = z * this->sensitivity * SENSORS_DPS_TO_RADS; - } - - return 0; -} - -void -BMI055_Gyroscope::getSensor(sensor_t *sensor) -{ - /* get sensor description */ - if (sensor) - { - memcpy(sensor, &_BMI055_sensor[1], sizeof(sensor_t)); - } -} diff --git a/components/drivers/sensors/bmi055_sensor.h b/components/drivers/sensors/bmi055_sensor.h deleted file mode 100644 index 713aa74ad0..0000000000 --- a/components/drivers/sensors/bmi055_sensor.h +++ /dev/null @@ -1,334 +0,0 @@ -/* - * Copyright (c) 2006-2018, RT-Thread Development Team - * - * SPDX-License-Identifier: Apache-2.0 - * - * Change Logs: - * Date Author Notes - * 2015-1-11 RT_learning the first version - */ -#ifndef __BMI055_H__ -#define __BMI055_H__ - -#include - -/**************************************************************************************************/ -/************************************Register map accelerometer************************************/ - -#define BMI055_ACC_I2C_ADDR1 0x18 //SDO is low(GND) -#define BMI055_ACC_I2C_ADDR2 0x19 //SDO is high(VCC) - -#define BMI055_ACC_DEFAULT_ADDRESS BMI055_ACC_I2C_ADDR2 //in the LPC54102 SPM-S - -#define BMI055_ACC_BGW_CHIPID_VALUE 0xFA - -#define BMI055_ACC_BGW_CHIPID 0x00 -/**
-#include -#include - -#include "mpu6050_sensor.h" - - -const static sensor_t _MPU6050_sensor[] = -{ - { - .name = "Accelerometer", - .vendor = "Invensense", - .version = sizeof(sensor_t), - .handle = 0, - .type = SENSOR_TYPE_ACCELEROMETER, - .maxRange = SENSOR_ACCEL_RANGE_16G, - .resolution = 1.0f, - .power = 0.5f, - .minDelay = 10000, - .fifoReservedEventCount = 0, - .fifoMaxEventCount = 64, - }, - { - .name = "Gyroscope", - .vendor = "Invensense", - .version = sizeof(sensor_t), - .handle = 0, - .type = SENSOR_TYPE_GYROSCOPE, - .maxRange = SENSOR_GYRO_RANGE_2000DPS, - .resolution = 1.0f, - .power = 0.5f, - .minDelay = 10000, - .fifoReservedEventCount = 0, - .fifoMaxEventCount = 64, - } -}; - -MPU6050::MPU6050(int sensor_type, const char* iic_bus, int addr) - : SensorBase(sensor_type) -{ - this->i2c_bus = (struct rt_i2c_bus_device *)rt_device_find(iic_bus); - if (this->i2c_bus == NULL) - { - printf("MPU6050: No IIC device:%s\n", iic_bus); - return; - } - - this->i2c_addr = addr; - - /* register to sensor manager */ - SensorManager::registerSensor(this); -} - -int MPU6050::read_reg(rt_uint8_t reg, rt_uint8_t *value) -{ - struct rt_i2c_msg msgs[2]; - - msgs[0].addr = this->i2c_addr; - msgs[0].flags = RT_I2C_WR; - msgs[0].buf = ® - msgs[0].len = 1; - - msgs[1].addr = this->i2c_addr; - msgs[1].flags = RT_I2C_RD; /* Read from slave */ - msgs[1].buf = (rt_uint8_t *)value; - msgs[1].len = 1; - - if (rt_i2c_transfer(this->i2c_bus, msgs, 2) == 2) - return RT_EOK; - - return -RT_ERROR; -} - -int MPU6050::read_buffer(rt_uint8_t reg, rt_uint8_t* value, rt_size_t size) -{ - struct rt_i2c_msg msgs[2]; - - msgs[0].addr = this->i2c_addr; - msgs[0].flags = RT_I2C_WR; - msgs[0].buf = ® - msgs[0].len = 1; - - msgs[1].addr = this->i2c_addr; - msgs[1].flags = RT_I2C_RD; /* Read from slave */ - msgs[1].buf = (rt_uint8_t *)value; - msgs[1].len = size; - - if (rt_i2c_transfer(this->i2c_bus, msgs, 2) == 2) - return RT_EOK; - - return -RT_ERROR; -} - -int MPU6050::write_reg(rt_uint8_t reg, rt_uint8_t value) -{ - struct rt_i2c_msg msgs[2]; - - msgs[0].addr = this->i2c_addr; - msgs[0].flags = RT_I2C_WR; - msgs[0].buf = ® - msgs[0].len = 1; - - msgs[1].addr = this->i2c_addr; - msgs[1].flags = RT_I2C_WR | RT_I2C_NO_START; - msgs[1].buf = (rt_uint8_t *)&value; - msgs[1].len = 1; - - if (rt_i2c_transfer(this->i2c_bus, msgs, 2) == 2) - return RT_EOK; - - return -RT_ERROR; -} - - -MPU6050_Accelerometer::MPU6050_Accelerometer(const char* iic_name, int addr) - : MPU6050(SENSOR_TYPE_ACCELEROMETER, iic_name, addr) -{ - int index; - uint8_t id; - rt_uint8_t value[6] = {0}; - rt_int32_t x, y, z; - SensorConfig config = {SENSOR_MODE_NORMAL, SENSOR_DATARATE_400HZ, SENSOR_ACCEL_RANGE_2G}; - - /* initialize MPU6050 */ - write_reg(MPU6050_PWR_MGMT_1, 0x80); /* reset mpu6050 device */ - write_reg(MPU6050_SMPLRT_DIV, 0x00); /* Sample Rate = Gyroscope Output Rate / (1 + SMPLRT_DIV) */ - write_reg(MPU6050_PWR_MGMT_1, 0x03); /* Wake up device , set device clock Z axis gyroscope */ - write_reg(MPU6050_CONFIG, 0x03); /* set DLPF_CFG 42Hz */ - write_reg(MPU6050_GYRO_CONFIG, 0x18); /* set gyro 2000deg/s */ - write_reg(MPU6050_ACCEL_CONFIG, 0x08); /* set acc +-4g/s */ - - - x_offset = y_offset = z_offset = 0; - x = y = z = 0; - - /* read MPU6050 id */ - read_buffer(MPU6050_WHOAMI, &id, 1); - if (id != MPU6050_ID) - { - printf("Warning: not found MPU6050 id: %02x\n", id); - } - - /* get offset */ - for (index = 0; index < 200; index ++) - { - read_buffer(MPU6050_ACCEL_XOUT_H, value, 6); - - x += (((rt_int16_t)value[0] << 8) | value[1]); - y += (((rt_int16_t)value[2] << 8) | value[3]); - z += (((rt_int16_t)value[4] << 8) | value[5]); - } - x_offset = x / 200; - y_offset = y / 200; - z_offset = z / 200; - - this->enable = RT_FALSE; - this->sensitivity = SENSOR_ACCEL_SENSITIVITY_2G; - this->config = config; -} - -int -MPU6050_Accelerometer::configure(SensorConfig *config) -{ - int range; - uint8_t value; - - if (config == RT_NULL) return -1; - - /* TODO: set datarate */ - - /* get range and calc the sensitivity */ - range = config->range.accel_range; - switch (range) - { - case SENSOR_ACCEL_RANGE_2G: - this->sensitivity = SENSOR_ACCEL_SENSITIVITY_2G; - range = 0; - break; - case SENSOR_ACCEL_RANGE_4G: - this->sensitivity = SENSOR_ACCEL_SENSITIVITY_4G; - range = 0x01 << 2; - break; - case SENSOR_ACCEL_RANGE_8G: - this->sensitivity = SENSOR_ACCEL_SENSITIVITY_8G; - range = 0x02 << 2; - break; - case SENSOR_ACCEL_RANGE_16G: - this->sensitivity = SENSOR_ACCEL_SENSITIVITY_16G; - range = 0x03 << 2; - break; - default: - return -1; - } - - /* set range to sensor */ - read_reg(MPU6050_ACCEL_CONFIG, &value); - value &= ~(0x3 << 2); - value |= range; - write_reg(MPU6050_ACCEL_CONFIG, value); - - return 0; -} - -int -MPU6050_Accelerometer::activate(int enable) -{ - uint8_t value; - - if (enable && this->enable == RT_FALSE) - { - /* enable accelerometer */ - read_reg(MPU6050_PWR_MGMT_2, &value); - value &= ~(0x07 << 2); - write_reg(MPU6050_PWR_MGMT_2, value); - } - - if (!enable && this->enable == RT_TRUE) - { - /* disable accelerometer */ - read_reg(MPU6050_PWR_MGMT_2, &value); - value |= (0x07 << 2); - write_reg(MPU6050_PWR_MGMT_2, value); - } - - if (enable) this->enable = RT_TRUE; - else this->enable = RT_FALSE; - - return 0; -} - -int -MPU6050_Accelerometer::poll(sensors_event_t *event) -{ - rt_uint8_t value[6]; - rt_int16_t x, y, z; - - /* parameters check */ - if (event == NULL) return -1; - - /* get event data */ - event->version = sizeof(sensors_event_t); - event->sensor = (int32_t) this; - event->timestamp = rt_tick_get(); - event->type = SENSOR_TYPE_ACCELEROMETER; - - read_buffer(MPU6050_ACCEL_XOUT_H, value, 6); - - /* get raw data */ - x = (((rt_int16_t)value[0] << 8) | value[1]); - y = (((rt_int16_t)value[2] << 8) | value[3]); - z = (((rt_int16_t)value[4] << 8) | value[5]); - - if (config.mode == SENSOR_MODE_RAW) - { - event->raw_acceleration.x = x; - event->raw_acceleration.y = y; - event->raw_acceleration.z = z; - } - else - { - - x -= x_offset; y -= y_offset; z -= z_offset; - event->acceleration.x = x * this->sensitivity * SENSORS_GRAVITY_STANDARD; - event->acceleration.y = y * this->sensitivity * SENSORS_GRAVITY_STANDARD; - event->acceleration.z = z * this->sensitivity * SENSORS_GRAVITY_STANDARD; - } - - return 0; -} - -void -MPU6050_Accelerometer::getSensor(sensor_t *sensor) -{ - /* get sensor description */ - if (sensor) - { - memcpy(sensor, &_MPU6050_sensor[0], sizeof(sensor_t)); - } -} - -MPU6050_Gyroscope::MPU6050_Gyroscope(const char* iic_name, int addr) - : MPU6050(SENSOR_TYPE_GYROSCOPE, iic_name, addr) -{ - int index; - uint8_t id; - rt_uint8_t value[6]; - rt_int32_t x, y, z; - - /* initialize MPU6050 */ - write_reg(MPU6050_PWR_MGMT_1, 0x80); /* reset mpu6050 device */ - write_reg(MPU6050_SMPLRT_DIV, 0x00); /* Sample Rate = Gyroscope Output Rate / (1 + SMPLRT_DIV) */ - write_reg(MPU6050_PWR_MGMT_1, 0x03); /* Wake up device , set device clock Z axis gyroscope */ - write_reg(MPU6050_CONFIG, 0x03); /* set DLPF_CFG 42Hz */ - write_reg(MPU6050_GYRO_CONFIG, 0x18); /* set gyro 2000deg/s */ - write_reg(MPU6050_ACCEL_CONFIG, 0x08); /* set acc +-4g/s */ - - x_offset = y_offset = z_offset = 0; - x = y = z = 0; - - /* read MPU6050 id */ - read_reg(MPU6050_WHOAMI, &id); - if (id != MPU6050_ID) - { - printf("Warning: not found MPU6050 id: %02x\n", id); - } - - /* get offset */ - for (index = 0; index < 200; index ++) - { - read_buffer(MPU6050_GYRO_XOUT_H, value, 6); - - x += (((rt_int16_t)value[0] << 8) | value[1]); - y += (((rt_int16_t)value[2] << 8) | value[3]); - z += (((rt_int16_t)value[4] << 8) | value[5]); - } - x_offset = x / 200; - y_offset = y / 200; - z_offset = z / 200; - - this->enable = RT_FALSE; - this->sensitivity = SENSOR_GYRO_SENSITIVITY_250DPS; -} - -int -MPU6050_Gyroscope::configure(SensorConfig *config) -{ - int range; - uint8_t value; - - if (config == RT_NULL) return -1; - - /* TODO: set datarate */ - - /* get range and calc the sensitivity */ - range = config->range.gyro_range; - switch (range) - { - case SENSOR_GYRO_RANGE_250DPS: - this->sensitivity = SENSOR_GYRO_SENSITIVITY_250DPS; - range = 0; - break; - case SENSOR_GYRO_RANGE_500DPS: - this->sensitivity = SENSOR_GYRO_SENSITIVITY_500DPS; - range = 0x01 << 2; - break; - case SENSOR_GYRO_RANGE_1000DPS: - this->sensitivity = SENSOR_GYRO_SENSITIVITY_1000DPS; - range = 0x02 << 2; - break; - case SENSOR_GYRO_RANGE_2000DPS: - this->sensitivity = SENSOR_GYRO_SENSITIVITY_2000DPS; - range = 0x03 << 2; - break; - default: - return -1; - } - - /* set range to sensor */ - read_reg(MPU6050_GYRO_CONFIG, &value); - value &= ~(0x3 << 2); - value |= range; - write_reg(MPU6050_GYRO_CONFIG, value); - - return 0; -} - -int -MPU6050_Gyroscope::activate(int enable) -{ - uint8_t value; - - if (enable && this->enable == RT_FALSE) - { - /* enable gyroscope */ - read_reg(MPU6050_PWR_MGMT_1, &value); - value &= ~(0x01 << 4); - write_reg(MPU6050_PWR_MGMT_1, value); - - read_reg(MPU6050_PWR_MGMT_2, &value); - value &= ~(0x07 << 0); - write_reg(MPU6050_PWR_MGMT_2, value); - } - - if (!enable && this->enable == RT_TRUE) - { - /* disable gyroscope */ - read_reg(MPU6050_PWR_MGMT_2, &value); - value |= (0x07 << 0); - write_reg(MPU6050_PWR_MGMT_2, value); - } - - if (enable) this->enable = RT_TRUE; - else this->enable = RT_FALSE; - - return 0; -} - -int -MPU6050_Gyroscope::poll(sensors_event_t *event) -{ - rt_uint8_t value[6]; - rt_int16_t x, y, z; - - /* parameters check */ - if (event == NULL) return -1; - - /* get event data */ - event->version = sizeof(sensors_event_t); - event->sensor = (int32_t) this; - event->timestamp = rt_tick_get(); - event->type = SENSOR_TYPE_GYROSCOPE; - - read_buffer(MPU6050_GYRO_XOUT_H, value, 6); - - /* get raw data */ - x = (((rt_int16_t)value[0] << 8) | value[1]); - y = (((rt_int16_t)value[2] << 8) | value[3]); - z = (((rt_int16_t)value[4] << 8) | value[5]); - - - if (config.mode == SENSOR_MODE_RAW) - { - event->raw_gyro.x = x; - event->raw_gyro.y = y; - event->raw_gyro.z = z; - } - else - { - x -= x_offset; y -= y_offset; z -= z_offset; - event->gyro.x = x * this->sensitivity * SENSORS_DPS_TO_RADS; - event->gyro.y = y * this->sensitivity * SENSORS_DPS_TO_RADS; - event->gyro.z = z * this->sensitivity * SENSORS_DPS_TO_RADS; - } - - return 0; -} - -void -MPU6050_Gyroscope::getSensor(sensor_t *sensor) -{ - /* get sensor description */ - if (sensor) - { - memcpy(sensor, &_MPU6050_sensor[1], sizeof(sensor_t)); - } -} - diff --git a/components/drivers/sensors/mpu6050_sensor.h b/components/drivers/sensors/mpu6050_sensor.h deleted file mode 100644 index 8f70c3e5ed..0000000000 --- a/components/drivers/sensors/mpu6050_sensor.h +++ /dev/null @@ -1,189 +0,0 @@ -/* - * Copyright (c) 2006-2018, RT-Thread Development Team - * - * SPDX-License-Identifier: Apache-2.0 - * - * Change Logs: - * Date Author Notes - * 2014-12-20 Bernard the first version - * 2015-1-11 RT_learning modify the mpu6050 ID - */ - -#ifndef MPU6050_SENSOR_H__ -#define MPU6050_SENSOR_H__ - -#include - -#define MPU6050_ADDRESS_AD0_LOW 0x68 // address pin low (GND), default for InvenSense evaluation board -#define MPU6050_ADDRESS_AD0_HIGH 0x69 // address pin high (VCC) -#define MPU6050_DEFAULT_ADDRESS MPU6050_ADDRESS_AD0_LOW - -#define MPU6050_XG_OFFS_TC 0x00 -#define MPU6050_YG_OFFS_TC 0x01 -#define MPU6050_ZG_OFFS_TC 0x02 -#define MPU6050_X_FINE_GAIN 0x03 -#define MPU6050_Y_FINE_GAIN 0x04 -#define MPU6050_Z_FINE_GAIN 0x05 -#define MPU6050_XA_OFFS_H 0x06 -#define MPU6050_XA_OFFS_L 0x07 -#define MPU6050_YA_OFFS_H 0x08 -#define MPU6050_YA_OFFS_L 0x09 -#define MPU6050_ZA_OFFS_H 0x0A -#define MPU6050_ZA_OFFS_L 0x0B -#define MPU6050_PRODUCT_ID 0x0C -#define MPU6050_SELF_TEST_X 0x0D -#define MPU6050_SELF_TEST_Y 0x0E -#define MPU6050_SELF_TEST_Z 0x0F -#define MPU6050_SELF_TEST_A 0x10 -#define MPU6050_XG_OFFS_USRH 0x13 -#define MPU6050_XG_OFFS_USRL 0x14 -#define MPU6050_YG_OFFS_USRH 0x15 -#define MPU6050_YG_OFFS_USRL 0x16 -#define MPU6050_ZG_OFFS_USRH 0x17 -#define MPU6050_ZG_OFFS_USRL 0x18 -#define MPU6050_SMPLRT_DIV 0x19 -#define MPU6050_CONFIG 0x1A -#define MPU6050_GYRO_CONFIG 0x1B -#define MPU6050_ACCEL_CONFIG 0x1C -#define MPU6050_ACCEL_CONFIG_2 0x1D -#define MPU6050_LP_ACCEL_ODR 0x1E -#define MPU6050_MOT_THR 0x1F -#define MPU6050_FIFO_EN 0x23 -#define MPU6050_I2C_MST_CTRL 0x24 -#define MPU6050_I2C_SLV0_ADDR 0x25 -#define MPU6050_I2C_SLV0_REG 0x26 -#define MPU6050_I2C_SLV0_CTRL 0x27 -#define MPU6050_I2C_SLV1_ADDR 0x28 -#define MPU6050_I2C_SLV1_REG 0x29 -#define MPU6050_I2C_SLV1_CTRL 0x2A -#define MPU6050_I2C_SLV2_ADDR 0x2B -#define MPU6050_I2C_SLV2_REG 0x2C -#define MPU6050_I2C_SLV2_CTRL 0x2D -#define MPU6050_I2C_SLV3_ADDR 0x2E -#define MPU6050_I2C_SLV3_REG 0x2F -#define MPU6050_I2C_SLV3_CTRL 0x30 -#define MPU6050_I2C_SLV4_ADDR 0x31 -#define MPU6050_I2C_SLV4_REG 0x32 -#define MPU6050_I2C_SLV4_DO 0x33 -#define MPU6050_I2C_SLV4_CTRL 0x34 -#define MPU6050_I2C_SLV4_DI 0x35 -#define MPU6050_I2C_MST_STATUS 0x36 -#define MPU6050_INT_PIN_CFG 0x37 -#define MPU6050_INT_ENABLE 0x38 -#define MPU6050_ACCEL_XOUT_H 0x3B -#define MPU6050_ACCEL_XOUT_L 0x3C -#define MPU6050_ACCEL_YOUT_H 0x3D -#define MPU6050_ACCEL_YOUT_L 0x3E -#define MPU6050_ACCEL_ZOUT_H 0x3F -#define MPU6050_ACCEL_ZOUT_L 0x40 -#define MPU6050_TEMP_OUT_H 0x41 -#define MPU6050_TEMP_OUT_L 0x42 -#define MPU6050_GYRO_XOUT_H 0x43 -#define MPU6050_GYRO_XOUT_L 0x44 -#define MPU6050_GYRO_YOUT_H 0x45 -#define MPU6050_GYRO_YOUT_L 0x46 -#define MPU6050_GYRO_ZOUT_H 0x47 -#define MPU6050_GYRO_ZOUT_L 0x48 -#define MPU6050_EXT_SENS_DATA_00 0x49 -#define MPU6050_EXT_SENS_DATA_01 0x4A -#define MPU6050_EXT_SENS_DATA_02 0x4B -#define MPU6050_EXT_SENS_DATA_03 0x4C -#define MPU6050_EXT_SENS_DATA_04 0x4D -#define MPU6050_EXT_SENS_DATA_05 0x4E -#define MPU6050_EXT_SENS_DATA_06 0x4F -#define MPU6050_EXT_SENS_DATA_07 0x50 -#define MPU6050_EXT_SENS_DATA_08 0x51 -#define MPU6050_EXT_SENS_DATA_09 0x52 -#define MPU6050_EXT_SENS_DATA_10 0x53 -#define MPU6050_EXT_SENS_DATA_11 0x54 -#define MPU6050_EXT_SENS_DATA_12 0x55 -#define MPU6050_EXT_SENS_DATA_13 0x56 -#define MPU6050_EXT_SENS_DATA_14 0x57 -#define MPU6050_EXT_SENS_DATA_15 0x58 -#define MPU6050_EXT_SENS_DATA_16 0x59 -#define MPU6050_EXT_SENS_DATA_17 0x5A -#define MPU6050_EXT_SENS_DATA_18 0x5B -#define MPU6050_EXT_SENS_DATA_19 0x5C -#define MPU6050_EXT_SENS_DATA_20 0x5D -#define MPU6050_EXT_SENS_DATA_21 0x5E -#define MPU6050_EXT_SENS_DATA_22 0x5F -#define MPU6050_EXT_SENS_DATA_23 0x60 -#define MPU6050_I2C_SLV0_DO 0x63 -#define MPU6050_I2C_SLV1_DO 0x64 -#define MPU6050_I2C_SLV2_DO 0x65 -#define MPU6050_I2C_SLV3_DO 0x66 -#define MPU6050_I2C_MST_DELAY_CTRL 0x67 -#define MPU6050_SIGNAL_PATH_RESET 0x68 -#define MPU6050_MOT_DETECT_CTRL 0x69 -#define MPU6050_USER_CTRL 0x6A -#define MPU6050_PWR_MGMT_1 0x6B -#define MPU6050_PWR_MGMT_2 0x6C -#define MPU6050_BANK_SEL 0x6D -#define MPU6050_MEM_START_ADDR 0x6E -#define MPU6050_MEM_R_W 0x6F -#define MPU6050_DMP_CFG_1 0x70 -#define MPU6050_DMP_CFG_2 0x71 -#define MPU6050_FIFO_COUNTH 0x72 -#define MPU6050_FIFO_COUNTL 0x73 -#define MPU6050_FIFO_R_W 0x74 -#define MPU6050_WHOAMI 0x75 -#define MPU6050_XA_OFFSET_H 0x77 -#define MPU6050_XA_OFFSET_L 0x78 -#define MPU6050_YA_OFFSET_H 0x7A -#define MPU6050_YA_OFFSET_L 0x7B -#define MPU6050_ZA_OFFSET_H 0x7D -#define MPU6050_ZA_OFFSET_L 0x7E - -#define MPU6050_ID 0x68 - -class MPU6050 :public SensorBase -{ -public: - MPU6050(int sensor_type, const char* iic_bus, int addr); - - int read_reg(rt_uint8_t reg, rt_uint8_t* value); - int write_reg(rt_uint8_t reg, rt_uint8_t value); - int read_buffer(rt_uint8_t reg, rt_uint8_t* value, rt_size_t size); - -private: - struct rt_i2c_bus_device *i2c_bus; - int i2c_addr; -}; - -class MPU6050_Accelerometer:public MPU6050 -{ -public: - MPU6050_Accelerometer(const char* iic_name, int addr); - - virtual int configure(SensorConfig *config); - virtual int activate(int enable); - - virtual int poll(sensors_event_t *event); - virtual void getSensor(sensor_t *sensor); - -private: - rt_int16_t x_offset, y_offset, z_offset; - - rt_bool_t enable; - float sensitivity; -}; - -class MPU6050_Gyroscope:public MPU6050 -{ -public: - MPU6050_Gyroscope(const char* iic_name, int addr); - - virtual int configure(SensorConfig *config); - virtual int activate(int enable); - - virtual int poll(sensors_event_t *event); - virtual void getSensor(sensor_t *sensor); - -private: - rt_int16_t x_offset, y_offset, z_offset; - - rt_bool_t enable; - float sensitivity; -}; - -#endif diff --git a/components/drivers/sensors/sensor.c b/components/drivers/sensors/sensor.c new file mode 100644 index 0000000000..80c9a60ae0 --- /dev/null +++ b/components/drivers/sensors/sensor.c @@ -0,0 +1,449 @@ +/* + * Copyright (c) 2006-2018, RT-Thread Development Team + * + * SPDX-License-Identifier: Apache-2.0 + * + * Change Logs: + * Date Author Notes + * 2019-01-31 flybreak first version + */ + +#include "sensor.h" + +#define DBG_ENABLE +#define DBG_LEVEL DBG_INFO +#define DBG_SECTION_NAME "sensor" +#define DBG_COLOR +#include + +#include + +static char *const sensor_name_str[] = +{ + "none", + "acce_", /* Accelerometer */ + "gyro_", /* Gyroscope */ + "mag_", /* Magnetometer */ + "temp_", /* Temperature */ + "humi_", /* Relative Humidity */ + "baro_", /* Barometer */ + "li_", /* Ambient light */ + "pr_", /* Proximity */ + "hr_", /* Heart Rate */ + "tvoc_", /* TVOC Level */ + "noi_", /* Noise Loudness */ + "step_" /* Step sensor */ +}; + +/* Sensor interrupt correlation function */ +/* + * Sensor interrupt handler function + */ +void rt_sensor_cb(rt_sensor_t sen) +{ + if (sen->parent.rx_indicate == RT_NULL) + { + return; + } + + /* The buffer is not empty. Read the data in the buffer first */ + if (sen->data_len > 0) + { + sen->parent.rx_indicate(&sen->parent, sen->data_len / sizeof(struct rt_sensor_data)); + } + else if (sen->config.mode == RT_SEN_MODE_INT) + { + /* The interrupt mode only produces one data at a time */ + sen->parent.rx_indicate(&sen->parent, 1); + } + else if (sen->config.mode == RT_SEN_MODE_FIFO) + { + sen->parent.rx_indicate(&sen->parent, sen->info.fifo_max); + } +} + +/* ISR for sensor interrupt */ +static void irq_callback(void *args) +{ + rt_sensor_t sensor = args; + rt_uint8_t i; + + if (sensor->module) + { + /* Invoke a callback for all sensors in the module */ + for (i = 0; i < sensor->module->sen_num; i++) + { + rt_sensor_cb(sensor->module->sen[i]); + } + } + else + { + rt_sensor_cb(sensor); + } +} + +/* Sensor interrupt initialization function */ +static rt_err_t rt_sensor_irq_init(rt_sensor_t sensor) +{ + if (sensor->config.irq_pin.pin == RT_PIN_NONE) + { + return -RT_EINVAL; + } + + rt_pin_mode(sensor->config.irq_pin.pin, sensor->config.irq_pin.mode); + + if (sensor->config.irq_pin.mode == PIN_MODE_INPUT_PULLDOWN) + { + rt_pin_attach_irq(sensor->config.irq_pin.pin, PIN_IRQ_MODE_RISING, irq_callback, (void *)sensor); + } + else if (sensor->config.irq_pin.mode == PIN_MODE_INPUT_PULLUP) + { + rt_pin_attach_irq(sensor->config.irq_pin.pin, PIN_IRQ_MODE_FALLING, irq_callback, (void *)sensor); + } + else if (sensor->config.irq_pin.mode == PIN_MODE_INPUT) + { + rt_pin_attach_irq(sensor->config.irq_pin.pin, PIN_IRQ_MODE_RISING_FALLING, irq_callback, (void *)sensor); + } + + rt_pin_irq_enable(sensor->config.irq_pin.pin, RT_TRUE); + + LOG_I("interrupt init success"); + + return 0; +} + +/* Sensor interrupt enable */ +static void rt_sensor_irq_enable(rt_sensor_t sensor) +{ + if (sensor->config.irq_pin.pin != RT_PIN_NONE) + { + rt_pin_irq_enable(sensor->config.irq_pin.pin, RT_TRUE); + } +} + +/* Sensor interrupt disable */ +static void rt_sensor_irq_disable(rt_sensor_t sensor) +{ + if (sensor->config.irq_pin.pin != RT_PIN_NONE) + { + rt_pin_irq_enable(sensor->config.irq_pin.pin, RT_FALSE); + } +} + +/* RT-Thread Device Interface */ + +static rt_err_t rt_sensor_init(rt_device_t dev) +{ + rt_sensor_t sensor = (rt_sensor_t)dev; + RT_ASSERT(dev != RT_NULL); + + if (sensor->module != RT_NULL && sensor->info.fifo_max > 0 && sensor->data_buf == RT_NULL) + { + /* Allocate memory for the sensor buffer */ + sensor->data_buf = rt_malloc(sizeof(struct rt_sensor_data) * sensor->info.fifo_max); + if (sensor->data_buf == RT_NULL) + { + return -RT_ENOMEM; + } + } + + return RT_EOK; +} + +static rt_err_t rt_sensor_open(rt_device_t dev, rt_uint16_t oflag) +{ + rt_sensor_t sensor = (rt_sensor_t)dev; + RT_ASSERT(dev != RT_NULL); + + if (sensor->module) + { + /* take the module mutex */ + rt_mutex_take(sensor->module->lock, RT_WAITING_FOREVER); + } + + if (oflag & RT_DEVICE_FLAG_RDONLY && dev->flag & RT_DEVICE_FLAG_RDONLY) + { + /* If polling mode is supported, configure it to polling mode */ + if (sensor->ops->control(sensor, RT_SEN_CTRL_SET_MODE, (void *)RT_SEN_MODE_POLLING) == RT_EOK) + { + sensor->config.mode = RT_SEN_MODE_POLLING; + } + } + else if (oflag & RT_DEVICE_FLAG_INT_RX && dev->flag & RT_DEVICE_FLAG_INT_RX) + { + /* If interrupt mode is supported, configure it to interrupt mode */ + if (sensor->ops->control(sensor, RT_SEN_CTRL_SET_MODE, (void *)RT_SEN_MODE_INT) == RT_EOK) + { + sensor->config.mode = RT_SEN_MODE_INT; + /* Initialization sensor interrupt */ + rt_sensor_irq_init(sensor); + } + } + else if (oflag & RT_SEN_FLAG_FIFO && dev->flag & RT_SEN_FLAG_FIFO) + { + /* If fifo mode is supported, configure it to fifo mode */ + if (sensor->ops->control(sensor, RT_SEN_CTRL_SET_MODE, (void *)RT_SEN_MODE_FIFO) == RT_EOK) + { + sensor->config.mode = RT_SEN_MODE_FIFO; + /* Initialization sensor interrupt */ + rt_sensor_irq_init(sensor); + } + } + else + { + return -RT_EINVAL; + } + + /* Configure power mode to normal mode */ + if (sensor->ops->control(sensor, RT_SEN_CTRL_SET_POWER, (void *)RT_SEN_POWER_NORMAL) == RT_EOK) + { + sensor->config.power = RT_SEN_POWER_NORMAL; + } + + if (sensor->module) + { + /* release the module mutex */ + rt_mutex_release(sensor->module->lock); + } + + return RT_EOK; +} + +static rt_err_t rt_sensor_close(rt_device_t dev) +{ + rt_sensor_t sensor = (rt_sensor_t)dev; + RT_ASSERT(dev != RT_NULL); + + if (sensor->module) + { + rt_mutex_take(sensor->module->lock, RT_WAITING_FOREVER); + } + + /* Configure power mode to power down mode */ + if (sensor->ops->control(sensor, RT_SEN_CTRL_SET_POWER, (void *)RT_SEN_POWER_DOWN) == RT_EOK) + { + sensor->config.power = RT_SEN_POWER_DOWN; + } + + /* Sensor disable interrupt */ + rt_sensor_irq_disable(sensor); + + if (sensor->module) + { + rt_mutex_release(sensor->module->lock); + } + + return RT_EOK; +} + +static rt_size_t rt_sensor_read(rt_device_t dev, rt_off_t pos, void *buf, rt_size_t len) +{ + rt_sensor_t sensor = (rt_sensor_t)dev; + rt_size_t result = 0; + RT_ASSERT(dev != RT_NULL); + + if (buf == NULL || len == 0) + { + return 0; + } + + if (sensor->module) + { + rt_mutex_take(sensor->module->lock, RT_WAITING_FOREVER); + } + + /* The buffer is not empty. Read the data in the buffer first */ + if (sensor->data_len > 0) + { + if (len > sensor->data_len / sizeof(struct rt_sensor_data)) + { + len = sensor->data_len / sizeof(struct rt_sensor_data); + } + + rt_memcpy(buf, sensor->data_buf, len * sizeof(struct rt_sensor_data)); + + /* Clear the buffer */ + sensor->data_len = 0; + result = len; + } + else + { + /* If the buffer is empty read the data */ + result = sensor->ops->fetch_data(sensor, buf, len); + } + + if (sensor->module) + { + rt_mutex_release(sensor->module->lock); + } + + return result; +} + +static rt_err_t rt_sensor_control(rt_device_t dev, int cmd, void *args) +{ + rt_sensor_t sensor = (rt_sensor_t)dev; + rt_err_t result = RT_EOK; + RT_ASSERT(dev != RT_NULL); + + if (sensor->module) + { + rt_mutex_take(sensor->module->lock, RT_WAITING_FOREVER); + } + + switch (cmd) + { + case RT_SEN_CTRL_GET_ID: + if (args) + { + sensor->ops->control(sensor, RT_SEN_CTRL_GET_ID, args); + } + break; + case RT_SEN_CTRL_GET_INFO: + if (args) + { + rt_memcpy(args, &sensor->info, sizeof(struct rt_sensor_info)); + } + break; + case RT_SEN_CTRL_SET_RANGE: + + /* Configuration measurement range */ + result = sensor->ops->control(sensor, RT_SEN_CTRL_SET_RANGE, args); + if (result == RT_EOK) + { + sensor->config.range = (rt_int32_t)args; + LOG_D("set range %d", sensor->config.range); + } + break; + case RT_SEN_CTRL_SET_ODR: + + /* Configuration data output rate */ + result = sensor->ops->control(sensor, RT_SEN_CTRL_SET_ODR, args); + if (result == RT_EOK) + { + sensor->config.odr = (rt_uint32_t)args & 0xFFFF; + LOG_D("set odr %d", sensor->config.odr); + } + break; + case RT_SEN_CTRL_SET_MODE: + + /* Configuration sensor work mode */ + result = sensor->ops->control(sensor, RT_SEN_CTRL_SET_MODE, args); + if (result == RT_EOK) + { + sensor->config.mode = (rt_uint32_t)args & 0xFF; + LOG_D("set work mode code:", sensor->config.mode); + + if (sensor->config.mode == RT_SEN_MODE_POLLING) + { + rt_sensor_irq_disable(sensor); + } + else if (sensor->config.mode == RT_SEN_MODE_INT || sensor->config.mode == RT_SEN_MODE_FIFO) + { + rt_sensor_irq_enable(sensor); + } + } + break; + case RT_SEN_CTRL_SET_POWER: + + /* Configuration sensor power mode */ + result = sensor->ops->control(sensor, RT_SEN_CTRL_SET_POWER, args); + if (result == RT_EOK) + { + sensor->config.power = (rt_uint32_t)args & 0xFF; + LOG_D("set power mode code:", sensor->config.power); + } + break; + case RT_SEN_CTRL_SELF_TEST: + + /* Device self-test */ + result = sensor->ops->control(sensor, RT_SEN_CTRL_SELF_TEST, args); + break; + default: + return -RT_ERROR; + } + + if (sensor->module) + { + rt_mutex_release(sensor->module->lock); + } + + return result; +} + +#ifdef RT_USING_DEVICE_OPS +const static struct rt_device_ops rt_sensor_ops = +{ + rt_sensor_init, + rt_sensor_open, + rt_sensor_close, + rt_sensor_read, + RT_NULL, + rt_sensor_control +}; +#endif + +/* + * sensor register + */ +int rt_hw_sensor_register(rt_sensor_t sensor, + const char *name, + rt_uint32_t flag, + void *data) +{ + rt_int8_t result; + rt_device_t device; + RT_ASSERT(sensor != RT_NULL); + + char *sensor_name = RT_NULL, *device_name = RT_NULL; + + /* Add a type name for the sensor device */ + sensor_name = sensor_name_str[sensor->info.type]; + device_name = rt_calloc(1, rt_strlen(sensor_name) + 1 + rt_strlen(name)); + if (device_name == RT_NULL) + { + LOG_E("device_name calloc failed!"); + return -RT_ERROR; + } + + rt_memcpy(device_name, sensor_name, rt_strlen(sensor_name) + 1); + strcat(device_name, name); + + if (sensor->module != RT_NULL && sensor->module->lock == RT_NULL) + { + /* Create a mutex lock for the module */ + sensor->module->lock = rt_mutex_create(name, RT_IPC_FLAG_FIFO); + if (sensor->module->lock == RT_NULL) + { + rt_free(device_name); + return -RT_ERROR; + } + } + + device = &sensor->parent; + +#ifdef RT_USING_DEVICE_OPS + device->ops = &rt_sensor_ops; +#else + device->init = rt_sensor_init; + device->open = rt_sensor_open; + device->close = rt_sensor_close; + device->read = rt_sensor_read; + device->write = RT_NULL; + device->control = rt_sensor_control; +#endif + device->type = RT_Device_Class_Sensor; + device->rx_indicate = RT_NULL; + device->tx_complete = RT_NULL; + device->user_data = data; + + result = rt_device_register(device, device_name, flag | RT_DEVICE_FLAG_STANDALONE); + if (result != RT_EOK) + { + LOG_E("rt_sensor register err code: %d", result); + return result; + } + + LOG_I("rt_sensor init success"); + return RT_EOK; +} diff --git a/components/drivers/sensors/sensor.cpp b/components/drivers/sensors/sensor.cpp deleted file mode 100644 index f986510f63..0000000000 --- a/components/drivers/sensors/sensor.cpp +++ /dev/null @@ -1,228 +0,0 @@ -/* - * Copyright (c) 2006-2018, RT-Thread Development Team - * - * SPDX-License-Identifier: Apache-2.0 - * - * Change Logs: - * Date Author Notes - * 2014-08-03 Bernard the first version - */ - -#include -#include - -#include "sensor.h" - -/** - * SensorBase - */ -SensorBase::SensorBase(int type) -{ - memset(&(this->config), 0x0, sizeof(SensorConfig)); - - this->type = type; - this->next = this->prev = NULL; - subscribe(NULL, NULL); -} - -SensorBase::~SensorBase() -{ -} - -int SensorBase::getType(void) -{ - return this->type; -} - -int SensorBase::setConfig(SensorConfig *config) -{ - int result; - - /* configure to the low level sensor */ - result = this->configure(config); - if (result == 0) - { - this->config = *config; - } - - return result; -} - -int SensorBase::getConfig(SensorConfig *config) -{ - *config = this->config; - return 0; -} - -int SensorBase::subscribe(SensorEventHandler_t handler, void *user_data) -{ - this->evtHandler = handler; - this->userData = user_data; - - return 0; -} - -int SensorBase::publish(void) -{ - if (this->evtHandler != NULL) - { - /* invoke subscribed handler */ - (*evtHandler)(this->userData); - } - - return 0; -} - -/** - * Sensor Manager - */ -/* sensors list */ -static SensorBase *sensor_list = NULL; - -SensorManager::SensorManager() -{ -} - -SensorManager::~SensorManager() -{ -} - -int SensorManager::registerSensor(SensorBase *sensor) -{ - RT_ASSERT(sensor != RT_NULL); - - /* add sensor into the list */ - if (sensor_list == NULL) - { - sensor->prev = sensor->next = sensor; - } - else - { - sensor_list->prev->next = sensor; - sensor->prev = sensor_list->prev; - - sensor_list->prev = sensor; - sensor->next = sensor_list; - } - - /* point the sensorList to this sensor */ - sensor_list = sensor; - - return 0; -} - -int SensorManager::unregisterSensor(SensorBase *sensor) -{ - /* disconnect sensor list */ - sensor->next->prev = sensor->prev; - sensor->prev->next = sensor->next; - - /* check the sensorList */ - if (sensor == sensor_list) - { - if (sensor->next == sensor) sensor_list = NULL; /* empty list */ - else sensor_list = sensor->next; - } - - /* re-initialize sensor node */ - sensor->next = sensor->prev = sensor; - - return 0; -} - -SensorBase *SensorManager::getDefaultSensor(int type) -{ - SensorBase *sensor = sensor_list; - - if (sensor == NULL) return NULL; - - do - { - /* find the same type */ - if (sensor->getType() == type) return sensor; - - sensor = sensor->next; - }while (sensor != sensor_list); - - return NULL; -} - -int SensorManager::subscribe(int type, SensorEventHandler_t handler, void *user_data) -{ - SensorBase *sensor; - - sensor = SensorManager::getDefaultSensor(type); - if (sensor != NULL) - { - sensor->subscribe(handler, user_data); - return 0; - } - - return -1; -} - -int SensorManager::sensorEventReady(SensorBase *sensor) -{ - return 0; -} - -int SensorManager::pollSensor(SensorBase *sensor, sensors_event_t *events, int number, int duration) -{ - rt_tick_t tick; - int result, index; - - if (sensor == NULL) return -1; - - tick = rt_tick_get(); - for (index = 0; index < number; index ++) - { - result = sensor->poll(&events[index]); - if (result < 0) break; - - if (rt_tick_get() - tick > duration) break; - } - - return index; -} - -rt_sensor_t rt_sensor_get_default(int type) -{ - return (rt_sensor_t)SensorManager::getDefaultSensor(type); -} - -int rt_sensor_subscribe(rt_sensor_t sensor, SensorEventHandler_t handler, void *user_data) -{ - SensorBase *sensor_base; - if (sensor == NULL) return -1; - - sensor_base = (SensorBase*)sensor; - - return sensor_base->subscribe(handler, user_data); -} - -int rt_sensor_poll(rt_sensor_t sensor, sensors_event_t *event) -{ - SensorBase *sensor_base; - if (sensor == NULL || event == NULL) return -1; - - sensor_base = (SensorBase*)sensor; - return sensor_base->poll(event); -} - -int rt_sensor_configure(rt_sensor_t sensor, SensorConfig *config) -{ - SensorBase *sensor_base; - if (sensor == NULL || config == NULL) return -1; - - sensor_base = (SensorBase*)sensor; - return sensor_base->setConfig(config); -} - -int rt_sensor_activate(rt_sensor_t sensor, int enable) -{ - SensorBase *sensor_base; - if (sensor == NULL) return -1; - - sensor_base = (SensorBase*)sensor; - return sensor_base->activate(enable); -} diff --git a/components/drivers/sensors/sensor.h b/components/drivers/sensors/sensor.h index 916360d34b..15dd79ccdf 100644 --- a/components/drivers/sensors/sensor.h +++ b/components/drivers/sensors/sensor.h @@ -5,1125 +5,188 @@ * * Change Logs: * Date Author Notes - * 2014-08-03 Bernard the first version + * 2019-01-31 flybreak first version */ + +#ifndef __SENSOR_H__ +#define __SENSOR_H__ -/* Modified from: https://github.com/android/platform_hardware_libhardware/blob/master/include/hardware/sensors.h */ - -/* - * Copyright (C) 2012 The Android Open Source Project - * - * 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 SENSORS_H__ -#define SENSORS_H__ - +#include #include -#include -#ifdef __CC_ARM /* skip warning in armcc */ -#pragma anon_unions +#ifdef RT_USING_RTC +#define rt_sen_get_timestamp() time() /* API for the sensor to get the timestamp */ +#else +#define rt_sen_get_timestamp() rt_tick_get() /* API for the sensor to get the timestamp */ #endif -/** - * Handles must be higher than SENSORS_HANDLE_BASE and must be unique. - * A Handle identifies a given sensors. The handle is used to activate - * and/or deactivate sensors. - * In this version of the API there can only be 256 handles. - */ -#define SENSORS_HANDLE_BASE 0 -#define SENSORS_HANDLE_BITS 8 -#define SENSORS_HANDLE_COUNT (1<0 - * ^ - * | - * +-----------+--> y>0 - * | | - * | | - * | | - * | | / z<0 - * | | / - * | | / - * O-----------+/ - * |[] [ ] []/ - * +----------/+ y<0 - * / - * / - * |/ z>0 (toward the sky) - * - * O: Origin (x=0,y=0,z=0) - * - */ - -/* - * Interaction with suspend mode - * - * Unless otherwise noted, an enabled sensor shall not prevent the - * SoC to go into suspend mode. It is the responsibility of applications - * to keep a partial wake-lock should they wish to receive sensor - * events while the screen is off. While in suspend mode, and unless - * otherwise noted (batch mode, sensor particularities, ...), enabled sensors' - * events are lost. - * - * Note that conceptually, the sensor itself is not de-activated while in - * suspend mode -- it's just that the data it returns are lost. As soon as - * the SoC gets out of suspend mode, operations resume as usual. Of course, - * in practice sensors shall be disabled while in suspend mode to - * save power, unless batch mode is active, in which case they must - * continue fill their internal FIFO (see the documentation of batch() to - * learn how suspend interacts with batch mode). - * - * In batch mode, and only when the flag SENSORS_BATCH_WAKE_UPON_FIFO_FULL is - * set and supported, the specified sensor must be able to wake-up the SoC and - * be able to buffer at least 10 seconds worth of the requested sensor events. - * - * There are notable exceptions to this behavior, which are sensor-dependent - * (see sensor types definitions below) - * - * - * The sensor type documentation below specifies the wake-up behavior of - * each sensor: - * wake-up: yes this sensor must wake-up the SoC to deliver events - * wake-up: no this sensor shall not wake-up the SoC, events are dropped - * - */ - -/* - * Sensor type - * - * Each sensor has a type which defines what this sensor measures and how - * measures are reported. All types are defined below. - * - * Device manufacturers (OEMs) can define their own sensor types, for - * their private use by applications or services provided by them. Such - * sensor types are specific to an OEM and can't be exposed in the SDK. - * These types must start at SENSOR_TYPE_DEVICE_PRIVATE_BASE. - */ - -/* - * Base for device manufacturers private sensor types. - * These sensor types can't be exposed in the SDK. - */ -#define SENSOR_TYPE_DEVICE_PRIVATE_BASE 0x10000 - -/* - * Sensor fusion and virtual sensors - * - * Many sensor types are or can be implemented as virtual sensors from - * physical sensors on the device. For instance the rotation vector sensor, - * orientation sensor, step-detector, step-counter, etc... - * - * From the point of view of this API these virtual sensors MUST appear as - * real, individual sensors. It is the responsibility of the driver and HAL - * to make sure this is the case. - * - * In particular, all sensors must be able to function concurrently. - * For example, if defining both an accelerometer and a step counter, - * then both must be able to work concurrently. - */ - -/* - * Trigger modes - * - * Sensors can report events in different ways called trigger modes, - * each sensor type has one and only one trigger mode associated to it. - * Currently there are four trigger modes defined: - * - * continuous: events are reported at a constant rate defined by setDelay(). - * eg: accelerometers, gyroscopes. - * on-change: events are reported only if the sensor's value has changed. - * setDelay() is used to set a lower limit to the reporting - * period (minimum time between two events). - * The HAL must return an event immediately when an on-change - * sensor is activated. - * eg: proximity, light sensors - * one-shot: upon detection of an event, the sensor deactivates itself and - * then sends a single event. Order matters to avoid race - * conditions. No other event is sent until the sensor get - * reactivated. setDelay() is ignored. - * eg: significant motion sensor - * special: see details in the sensor type specification below - * - */ - -/* - * SENSOR_TYPE_META_DATA - * trigger-mode: n/a - * wake-up sensor: n/a - * - * NO SENSOR OF THAT TYPE MUST BE RETURNED (*get_sensors_list)() - * - * SENSOR_TYPE_META_DATA is a special token used to populate the - * sensors_meta_data_event structure. It doesn't correspond to a physical - * sensor. sensors_meta_data_event are special, they exist only inside - * the HAL and are generated spontaneously, as opposed to be related to - * a physical sensor. - * - * sensors_meta_data_event_t.version must be META_DATA_VERSION - * sensors_meta_data_event_t.sensor must be 0 - * sensors_meta_data_event_t.type must be SENSOR_TYPE_META_DATA - * sensors_meta_data_event_t.reserved must be 0 - * sensors_meta_data_event_t.timestamp must be 0 - * - * The payload is a meta_data_event_t, where: - * meta_data_event_t.what can take the following values: - * - * META_DATA_FLUSH_COMPLETE - * This event indicates that a previous (*flush)() call has completed for the sensor - * handle specified in meta_data_event_t.sensor. - * see (*flush)() for more details - * - * All other values for meta_data_event_t.what are reserved and - * must not be used. - * - */ -#define SENSOR_TYPE_META_DATA (0) - -/* - * SENSOR_TYPE_ACCELEROMETER - * trigger-mode: continuous - * wake-up sensor: no - * - * All values are in SI units (m/s^2) and measure the acceleration of the - * device minus the force of gravity. - * - * Acceleration sensors return sensor events for all 3 axes at a constant - * rate defined by setDelay(). - * - * x: Acceleration on the x-axis - * y: Acceleration on the y-axis - * z: Acceleration on the z-axis - * - * Note that the readings from the accelerometer include the acceleration - * due to gravity (which is opposite to the direction of the gravity vector). - * - * Examples: - * The norm of should be close to 0 when in free fall. - * - * When the device lies flat on a table and is pushed on its left side - * toward the right, the x acceleration value is positive. - * - * When the device lies flat on a table, the acceleration value is +9.81, - * which correspond to the acceleration of the device (0 m/s^2) minus the - * force of gravity (-9.81 m/s^2). - * - * When the device lies flat on a table and is pushed toward the sky, the - * acceleration value is greater than +9.81, which correspond to the - * acceleration of the device (+A m/s^2) minus the force of - * gravity (-9.81 m/s^2). - */ -#define SENSOR_TYPE_ACCELEROMETER (1) - -/* - * SENSOR_TYPE_GEOMAGNETIC_FIELD - * trigger-mode: continuous - * wake-up sensor: no - * - * All values are in micro-Tesla (uT) and measure the geomagnetic - * field in the X, Y and Z axis. - * - * Returned values include calibration mechanisms such that the vector is - * aligned with the magnetic declination and heading of the earth's - * geomagnetic field. - * - * Magnetic Field sensors return sensor events for all 3 axes at a constant - * rate defined by setDelay(). - */ -#define SENSOR_TYPE_GEOMAGNETIC_FIELD (2) -#define SENSOR_TYPE_MAGNETIC_FIELD SENSOR_TYPE_GEOMAGNETIC_FIELD - -/* - * SENSOR_TYPE_ORIENTATION - * trigger-mode: continuous - * wake-up sensor: no - * - * All values are angles in degrees. - * - * Orientation sensors return sensor events for all 3 axes at a constant - * rate defined by setDelay(). - * - * azimuth: angle between the magnetic north direction and the Y axis, around - * the Z axis (0<=azimuth<360). - * 0=North, 90=East, 180=South, 270=West - * - * pitch: Rotation around X axis (-180<=pitch<=180), with positive values when - * the z-axis moves toward the y-axis. - * - * roll: Rotation around Y axis (-90<=roll<=90), with positive values when - * the x-axis moves towards the z-axis. - * - * Note: For historical reasons the roll angle is positive in the clockwise - * direction (mathematically speaking, it should be positive in the - * counter-clockwise direction): - * - * Z - * ^ - * (+roll) .--> | - * / | - * | | roll: rotation around Y axis - * X <-------(.) - * Y - * note that +Y == -roll - * - * - * - * Note: This definition is different from yaw, pitch and roll used in aviation - * where the X axis is along the long side of the plane (tail to nose). - */ -#define SENSOR_TYPE_ORIENTATION (3) - -/* - * SENSOR_TYPE_GYROSCOPE - * trigger-mode: continuous - * wake-up sensor: no - * - * All values are in radians/second and measure the rate of rotation - * around the X, Y and Z axis. The coordinate system is the same as is - * used for the acceleration sensor. Rotation is positive in the - * counter-clockwise direction (right-hand rule). That is, an observer - * looking from some positive location on the x, y or z axis at a device - * positioned on the origin would report positive rotation if the device - * appeared to be rotating counter clockwise. Note that this is the - * standard mathematical definition of positive rotation and does not agree - * with the definition of roll given earlier. - * The range should at least be 17.45 rad/s (ie: ~1000 deg/s). - * - * automatic gyro-drift compensation is allowed but not required. - */ -#define SENSOR_TYPE_GYROSCOPE (4) - -/* - * SENSOR_TYPE_LIGHT - * trigger-mode: on-change - * wake-up sensor: no - * - * The light sensor value is returned in SI lux units. - */ -#define SENSOR_TYPE_LIGHT (5) - -/* - * SENSOR_TYPE_PRESSURE - * trigger-mode: continuous - * wake-up sensor: no - * - * The pressure sensor return the athmospheric pressure in hectopascal (hPa) - */ -#define SENSOR_TYPE_PRESSURE (6) - -/* SENSOR_TYPE_TEMPERATURE is deprecated in the HAL */ -#define SENSOR_TYPE_TEMPERATURE (7) - -/* - * SENSOR_TYPE_PROXIMITY - * trigger-mode: on-change - * wake-up sensor: yes - * - * The distance value is measured in centimeters. Note that some proximity - * sensors only support a binary "close" or "far" measurement. In this case, - * the sensor should report its maxRange value in the "far" state and a value - * less than maxRange in the "near" state. - */ -#define SENSOR_TYPE_PROXIMITY (8) - -/* - * SENSOR_TYPE_GRAVITY - * trigger-mode: continuous - * wake-up sensor: no - * - * A gravity output indicates the direction of and magnitude of gravity in - * the devices's coordinates. On Earth, the magnitude is 9.8 m/s^2. - * Units are m/s^2. The coordinate system is the same as is used for the - * acceleration sensor. When the device is at rest, the output of the - * gravity sensor should be identical to that of the accelerometer. - */ -#define SENSOR_TYPE_GRAVITY (9) - -/* - * SENSOR_TYPE_LINEAR_ACCELERATION - * trigger-mode: continuous - * wake-up sensor: no - * - * Indicates the linear acceleration of the device in device coordinates, - * not including gravity. - * - * The output is conceptually: - * output of TYPE_ACCELERATION - output of TYPE_GRAVITY - * - * Readings on all axes should be close to 0 when device lies on a table. - * Units are m/s^2. - * The coordinate system is the same as is used for the acceleration sensor. - */ -#define SENSOR_TYPE_LINEAR_ACCELERATION (10) - -/* - * SENSOR_TYPE_ROTATION_VECTOR - * trigger-mode: continuous - * wake-up sensor: no - * - * The rotation vector symbolizes the orientation of the device relative to the - * East-North-Up coordinates frame. It is usually obtained by integration of - * accelerometer, gyroscope and magnetometer readings. - * - * The East-North-Up coordinate system is defined as a direct orthonormal basis - * where: - * - X points east and is tangential to the ground. - * - Y points north and is tangential to the ground. - * - Z points towards the sky and is perpendicular to the ground. - * - * The orientation of the phone is represented by the rotation necessary to - * align the East-North-Up coordinates with the phone's coordinates. That is, - * applying the rotation to the world frame (X,Y,Z) would align them with the - * phone coordinates (x,y,z). - * - * The rotation can be seen as rotating the phone by an angle theta around - * an axis rot_axis to go from the reference (East-North-Up aligned) device - * orientation to the current device orientation. - * - * The rotation is encoded as the 4 (reordered) components of a unit quaternion: - * sensors_event_t.data[0] = rot_axis.x*sin(theta/2) - * sensors_event_t.data[1] = rot_axis.y*sin(theta/2) - * sensors_event_t.data[2] = rot_axis.z*sin(theta/2) - * sensors_event_t.data[3] = cos(theta/2) - * where - * - rot_axis.x,y,z are the North-East-Up coordinates of a unit length vector - * representing the rotation axis - * - theta is the rotation angle - * - * The quaternion must be of norm 1 (it is a unit quaternion). Failure to ensure - * this will cause erratic client behaviour. - * - * In addition, this sensor reports an estimated heading accuracy. - * sensors_event_t.data[4] = estimated_accuracy (in radians) - * The heading error must be less than estimated_accuracy 95% of the time - * - * This sensor must use a gyroscope and an accelerometer as main orientation - * change input. - * - * This sensor can also include magnetometer input to make up for gyro drift, - * but it cannot be implemented using only a magnetometer. - */ -#define SENSOR_TYPE_ROTATION_VECTOR (11) - -/* - * SENSOR_TYPE_RELATIVE_HUMIDITY - * trigger-mode: on-change - * wake-up sensor: no - * - * A relative humidity sensor measures relative ambient air humidity and - * returns a value in percent. - */ -#define SENSOR_TYPE_RELATIVE_HUMIDITY (12) - -/* - * SENSOR_TYPE_AMBIENT_TEMPERATURE - * trigger-mode: on-change - * wake-up sensor: no - * - * The ambient (room) temperature in degree Celsius. - */ -#define SENSOR_TYPE_AMBIENT_TEMPERATURE (13) - -/* - * SENSOR_TYPE_MAGNETIC_FIELD_UNCALIBRATED - * trigger-mode: continuous - * wake-up sensor: no - * - * Similar to SENSOR_TYPE_MAGNETIC_FIELD, but the hard iron calibration is - * reported separately instead of being included in the measurement. - * Factory calibration and temperature compensation should still be applied to - * the "uncalibrated" measurement. - * Separating away the hard iron calibration estimation allows the system to - * better recover from bad hard iron estimation. - * - * All values are in micro-Tesla (uT) and measure the ambient magnetic - * field in the X, Y and Z axis. Assumptions that the the magnetic field - * is due to the Earth's poles should be avoided. - * - * The uncalibrated_magnetic event contains - * - 3 fields for uncalibrated measurement: x_uncalib, y_uncalib, z_uncalib. - * Each is a component of the measured magnetic field, with soft iron - * and temperature compensation applied, but not hard iron calibration. - * These values should be continuous (no re-calibration should cause a jump). - * - 3 fields for hard iron bias estimates: x_bias, y_bias, z_bias. - * Each field is a component of the estimated hard iron calibration. - * They represent the offsets to apply to the calibrated readings to obtain - * uncalibrated readings (x_uncalib ~= x_calibrated + x_bias) - * These values are expected to jump as soon as the estimate of the hard iron - * changes, and they should be stable the rest of the time. - * - * If this sensor is present, then the corresponding - * SENSOR_TYPE_MAGNETIC_FIELD must be present and both must return the - * same sensor_t::name and sensor_t::vendor. - * - * Minimum filtering should be applied to this sensor. In particular, low pass - * filters should be avoided. - * - * See SENSOR_TYPE_MAGNETIC_FIELD for more information - */ -#define SENSOR_TYPE_MAGNETIC_FIELD_UNCALIBRATED (14) - -/* - * SENSOR_TYPE_GAME_ROTATION_VECTOR - * trigger-mode: continuous - * wake-up sensor: no - * - * Similar to SENSOR_TYPE_ROTATION_VECTOR, but not using the geomagnetic - * field. Therefore the Y axis doesn't point north, but instead to some other - * reference. That reference is allowed to drift by the same order of - * magnitude than the gyroscope drift around the Z axis. - * - * This sensor does not report an estimated heading accuracy: - * sensors_event_t.data[4] is reserved and should be set to 0 - * - * In the ideal case, a phone rotated and returning to the same real-world - * orientation should report the same game rotation vector - * (without using the earth's geomagnetic field). - * - * This sensor must be based on a gyroscope. It cannot be implemented using - * a magnetometer. - * - * see SENSOR_TYPE_ROTATION_VECTOR for more details - */ -#define SENSOR_TYPE_GAME_ROTATION_VECTOR (15) - -/* - * SENSOR_TYPE_GYROSCOPE_UNCALIBRATED - * trigger-mode: continuous - * wake-up sensor: no - * - * All values are in radians/second and measure the rate of rotation - * around the X, Y and Z axis. An estimation of the drift on each axis is - * reported as well. - * - * No gyro-drift compensation shall be performed. - * Factory calibration and temperature compensation should still be applied - * to the rate of rotation (angular speeds). - * - * The coordinate system is the same as is - * used for the acceleration sensor. Rotation is positive in the - * counter-clockwise direction (right-hand rule). That is, an observer - * looking from some positive location on the x, y or z axis at a device - * positioned on the origin would report positive rotation if the device - * appeared to be rotating counter clockwise. Note that this is the - * standard mathematical definition of positive rotation and does not agree - * with the definition of roll given earlier. - * The range should at least be 17.45 rad/s (ie: ~1000 deg/s). - * - * Content of an uncalibrated_gyro event: (units are rad/sec) - * x_uncalib : angular speed (w/o drift compensation) around the X axis - * y_uncalib : angular speed (w/o drift compensation) around the Y axis - * z_uncalib : angular speed (w/o drift compensation) around the Z axis - * x_bias : estimated drift around X axis in rad/s - * y_bias : estimated drift around Y axis in rad/s - * z_bias : estimated drift around Z axis in rad/s - * - * IMPLEMENTATION NOTES: - * - * If the implementation is not able to estimate the drift, then this - * sensor MUST NOT be reported by this HAL. Instead, the regular - * SENSOR_TYPE_GYROSCOPE is used without drift compensation. - * - * If this sensor is present, then the corresponding - * SENSOR_TYPE_GYROSCOPE must be present and both must return the - * same sensor_t::name and sensor_t::vendor. - */ -#define SENSOR_TYPE_GYROSCOPE_UNCALIBRATED (16) - -/* - * SENSOR_TYPE_SIGNIFICANT_MOTION - * trigger-mode: one-shot - * wake-up sensor: yes - * - * A sensor of this type triggers an event each time significant motion - * is detected and automatically disables itself. - * The only allowed value to return is 1.0. - * - * A significant motion is a motion that might lead to a change in the user - * location. - * Examples of such motions are: - * walking, biking, sitting in a moving car, coach or train. - * Examples of situations that should not trigger significant motion: - * - phone in pocket and person is not moving - * - phone is on a table, even if the table shakes a bit due to nearby traffic - * or washing machine - * - * A note on false positive / false negative / power consumption tradeoff - * - The goal of this sensor is to save power. - * - Triggering an event when the user is not moving (false positive) is costly - * in terms of power, so it should be avoided. - * - Not triggering an event when the user is moving (false negative) is - * acceptable as long as it is not done repeatedly. If the user has been - * walking for 10 seconds, not triggering an event within those 10 seconds - * is not acceptable. - * - * IMPORTANT NOTE: this sensor type is very different from other types - * in that it must work when the screen is off without the need of - * holding a partial wake-lock and MUST allow the SoC to go into suspend. - * When significant motion is detected, the sensor must awaken the SoC and - * the event be reported. - * - * If a particular hardware cannot support this mode of operation then this - * sensor type MUST NOT be reported by the HAL. ie: it is not acceptable - * to "emulate" this sensor in the HAL. - * - * The whole point of this sensor type is to save power by keeping the - * SoC in suspend mode when the device is at rest. - * - * When the sensor is not activated, it must also be deactivated in the - * hardware: it must not wake up the SoC anymore, even in case of - * significant motion. - * - * setDelay() has no effect and is ignored. - * Once a "significant motion" event is returned, a sensor of this type - * must disables itself automatically, as if activate(..., 0) had been called. - */ - -#define SENSOR_TYPE_SIGNIFICANT_MOTION (17) - -/* - * SENSOR_TYPE_STEP_DETECTOR - * trigger-mode: special - * wake-up sensor: no - * - * A sensor of this type triggers an event each time a step is taken - * by the user. The only allowed value to return is 1.0 and an event is - * generated for each step. Like with any other event, the timestamp - * indicates when the event (here the step) occurred, this corresponds to when - * the foot hit the ground, generating a high variation in acceleration. - * - * While this sensor operates, it shall not disrupt any other sensors, in - * particular, but not limited to, the accelerometer; which might very well - * be in use as well. - * - * This sensor must be low power. That is, if the step detection cannot be - * done in hardware, this sensor should not be defined. Also, when the - * step detector is activated and the accelerometer is not, only steps should - * trigger interrupts (not accelerometer data). - * - * setDelay() has no impact on this sensor type - */ - -#define SENSOR_TYPE_STEP_DETECTOR (18) - -/* - * SENSOR_TYPE_STEP_COUNTER - * trigger-mode: on-change - * wake-up sensor: no - * - * A sensor of this type returns the number of steps taken by the user since - * the last reboot while activated. The value is returned as a uint64_t and is - * reset to zero only on a system / android reboot. - * - * The timestamp of the event is set to the time when the first step - * for that event was taken. - * See SENSOR_TYPE_STEP_DETECTOR for the signification of the time of a step. - * - * The minimum size of the hardware's internal counter shall be 16 bits - * (this restriction is here to avoid too frequent wake-ups when the - * delay is very large). - * - * IMPORTANT NOTE: this sensor type is different from other types - * in that it must work when the screen is off without the need of - * holding a partial wake-lock and MUST allow the SoC to go into suspend. - * Unlike other sensors, while in suspend mode this sensor must stay active, - * no events are reported during that time but, steps continue to be - * accounted for; an event will be reported as soon as the SoC resumes if - * the timeout has expired. - * - * In other words, when the screen is off and the device allowed to - * go into suspend mode, we don't want to be woken up, regardless of the - * setDelay() value, but the steps shall continue to be counted. - * - * The driver must however ensure that the internal step count never - * overflows. It is allowed in this situation to wake the SoC up so the - * driver can do the counter maintenance. - * - * While this sensor operates, it shall not disrupt any other sensors, in - * particular, but not limited to, the accelerometer; which might very well - * be in use as well. - * - * If a particular hardware cannot support these modes of operation then this - * sensor type MUST NOT be reported by the HAL. ie: it is not acceptable - * to "emulate" this sensor in the HAL. - * - * This sensor must be low power. That is, if the step detection cannot be - * done in hardware, this sensor should not be defined. Also, when the - * step counter is activated and the accelerometer is not, only steps should - * trigger interrupts (not accelerometer data). - * - * The whole point of this sensor type is to save power by keeping the - * SoC in suspend mode when the device is at rest. - */ - -#define SENSOR_TYPE_STEP_COUNTER (19) - -/* - * SENSOR_TYPE_GEOMAGNETIC_ROTATION_VECTOR - * trigger-mode: continuous - * wake-up sensor: no - * - * Similar to SENSOR_TYPE_ROTATION_VECTOR, but using a magnetometer instead - * of using a gyroscope. - * - * This sensor must be based on a magnetometer. It cannot be implemented using - * a gyroscope, and gyroscope input cannot be used by this sensor, as the - * goal of this sensor is to be low power. - * The accelerometer can be (and usually is) used. - * - * Just like SENSOR_TYPE_ROTATION_VECTOR, this sensor reports an estimated - * heading accuracy: - * sensors_event_t.data[4] = estimated_accuracy (in radians) - * The heading error must be less than estimated_accuracy 95% of the time - * - * see SENSOR_TYPE_ROTATION_VECTOR for more details - */ -#define SENSOR_TYPE_GEOMAGNETIC_ROTATION_VECTOR (20) - -/** - * Values returned by the accelerometer in various locations in the universe. - * all values are in SI units (m/s^2) - */ -#define SENSORS_GRAVITY_SUN (275.0f) -#define SENSORS_GRAVITY_MOON (1.6f) -#define SENSORS_GRAVITY_EARTH (9.80665f) -#define SENSORS_GRAVITY_STANDARD (SENSORS_GRAVITY_EARTH) - -/** Maximum magnetic field on Earth's surface */ -#define MAGNETIC_FIELD_EARTH_MAX (60.0f) - -/** Minimum magnetic field on Earth's surface */ -#define MAGNETIC_FIELD_EARTH_MIN (30.0f) - -/** Average sea level pressure is 1013.25 hPa */ -#define SENSORS_PRESSURE_SEALEVELHPA (1013.25F) - -/** Degrees/s to rad/s multiplier */ -#define SENSORS_DPS_TO_RADS (0.017453293F) -/** Gauss to micro-Tesla multiplier */ -#define SENSORS_GAUSS_TO_MICROTESLA (100) - -/** - * status of orientation sensor - */ -#define SENSOR_STATUS_UNRELIABLE 0 -#define SENSOR_STATUS_ACCURACY_LOW 1 -#define SENSOR_STATUS_ACCURACY_MEDIUM 2 -#define SENSOR_STATUS_ACCURACY_HIGH 3 - -/** - * sensor event data - */ -typedef struct +struct rt_sensor_config { + struct rt_sensor_intf intf; /* sensor interface config */ + struct rt_device_pin_mode irq_pin; /* Interrupt pin, The purpose of this pin is to notification read data */ + rt_uint8_t mode; /* sensor work mode */ + rt_uint8_t power; /* sensor power mode */ + rt_uint16_t odr; /* sensor out data rate */ + rt_int32_t range; /* sensor range of measurement */ +}; + +struct rt_sensor_device +{ + struct rt_device parent; /* The standard device */ + + struct rt_sensor_info info; /* The sensor info data */ + struct rt_sensor_config config; /* The sensor config data */ + + void *data_buf; /* The buf of the data received */ + rt_size_t data_len; /* The size of the data received */ + + struct rt_sensor_ops *ops; /* The sensor ops */ + + struct rt_sensor_module *module; /* The sensor module */ +}; +typedef struct rt_sensor_device *rt_sensor_t; + +struct rt_sensor_module +{ + rt_mutex_t lock; /* The module lock */ + + rt_sensor_t sen[RT_SEN_MODULE_MAX]; /* The module contains a list of sensors */ + rt_uint8_t sen_num; /* Number of sensors contained in the module */ +}; + +/* 3-axis Data Type */ +struct sensor_3_axis +{ + rt_int32_t x; + rt_int32_t y; + rt_int32_t z; +}; + +struct rt_sensor_data +{ + rt_uint32_t timestamp; /* The timestamp when the data was received */ + rt_uint8_t type; /* The sensor type of the data */ union { - float v[3]; - struct - { - float x; - float y; - float z; - }; - struct - { - float azimuth; - float pitch; - float roll; - }; - }; - int8_t status; - uint8_t reserved[3]; -} sensors_vec_t; - -/** - * sensor raw vector data - */ -typedef struct -{ - struct - { - int16_t x; - int16_t y; - int16_t z; - }; - - int8_t status; - uint8_t reserved[1]; -} sensors_raw_vec_t; - -/** - * uncalibrated gyroscope and magnetometer event data - */ -typedef struct -{ - union - { - float uncalib[3]; - struct - { - float x_uncalib; - float y_uncalib; - float z_uncalib; - }; - }; - union - { - float bias[3]; - struct - { - float x_bias; - float y_bias; - float z_bias; - }; - }; -} uncalibrated_event_t; - -typedef struct meta_data_event -{ - int32_t what; - int32_t sensor; -} meta_data_event_t; - -/** - * Union of the various types of sensor data - * that can be returned. - */ -typedef struct sensors_event_t -{ - /* must be sizeof(struct sensors_event_t) */ - int32_t version; - - /* sensor identifier */ - int32_t sensor; - - /* sensor type */ - int32_t type; - - /* reserved */ - int32_t reserved0; - - /* time is in nanosecond */ - int64_t timestamp; - - union - { - union - { - float data[16]; - - /* acceleration values are in meter per second per second (m/s^2) */ - sensors_vec_t acceleration; - /* raw acceleration data */ - sensors_raw_vec_t raw_acceleration; - - /* magnetic vector values are in micro-Tesla (uT) */ - sensors_vec_t magnetic; - /* raw magnetic data */ - sensors_raw_vec_t raw_magnetic; - - /* orientation values are in degrees */ - sensors_vec_t orientation; - - /* gyroscope values are in rad/s */ - sensors_vec_t gyro; - /* raw gyroscope data */ - sensors_raw_vec_t raw_gyro; - - /* temperature is in degrees centigrade (Celsius) */ - float temperature; - - /* distance in centimeters */ - float distance; - - /* light in SI lux units */ - float light; - - /* pressure in hectopascal (hPa) */ - float pressure; - - /* relative humidity in percent */ - float relative_humidity; - - /* uncalibrated gyroscope values are in rad/s */ - uncalibrated_event_t uncalibrated_gyro; - - /* uncalibrated magnetometer values are in micro-Teslas */ - uncalibrated_event_t uncalibrated_magnetic; - - /* this is a special event. see SENSOR_TYPE_META_DATA above. - * sensors_meta_data_event_t events are all reported with a type of - * SENSOR_TYPE_META_DATA. The handle is ignored and must be zero. - */ - meta_data_event_t meta_data; - }; - - union - { - uint64_t data[8]; - - /* step-counter */ - uint64_t step_counter; - } u64; - }; - uint32_t reserved1[4]; -} sensors_event_t; - -/* see SENSOR_TYPE_META_DATA */ -typedef sensors_event_t sensors_meta_data_event_t; - -typedef struct sensor_t -{ - /* Name of this sensor. - * All sensors of the same "type" must have a different "name". - */ - const char *name; - - /* vendor of the hardware part */ - const char *vendor; - - /* version of the hardware part + driver. The value of this field - * must increase when the driver is updated in a way that changes the - * output of this sensor. This is important for fused sensors when the - * fusion algorithm is updated. - */ - int version; - - /* handle that identifies this sensors. This handle is used to reference - * this sensor throughout the HAL API. - */ - int handle; - - /* this sensor's type. */ - int type; - - /* maximum range of this sensor's value in SI units */ - float maxRange; - - /* smallest difference between two values reported by this sensor */ - float resolution; - - /* rough estimate of this sensor's power consumption in mA */ - float power; - - /* this value depends on the trigger mode: - * - * continuous: minimum sample period allowed in microseconds - * on-change : 0 - * one-shot :-1 - * special : 0, unless otherwise noted - */ - int32_t minDelay; - - /* number of events reserved for this sensor in the batch mode FIFO. - * If there is a dedicated FIFO for this sensor, then this is the - * size of this FIFO. If the FIFO is shared with other sensors, - * this is the size reserved for that sensor and it can be zero. - */ - uint32_t fifoReservedEventCount; - - /* maximum number of events of this sensor that could be batched. - * This is especially relevant when the FIFO is shared between - * several sensors; this value is then set to the size of that FIFO. - */ - uint32_t fifoMaxEventCount; - - /* reserved fields, must be zero */ - void *reserved[6]; -} sensor_t; - -enum SensorMode -{ - SENSOR_MODE_RAW, - SENSOR_MODE_CALIBRATED, - SENSOR_MODE_NORMAL, + struct sensor_3_axis acce; /* Accelerometer. unit: mG */ + struct sensor_3_axis gyro; /* Gyroscope. unit: mdps */ + struct sensor_3_axis mag; /* Magnetometer. unit: mGauss */ + rt_int32_t temp; /* Temperature. unit: dCelsius */ + rt_int32_t humi; /* Relative humidity. unit: permillage */ + rt_int32_t baro; /* Pressure. unit: pascal (Pa) */ + rt_int32_t light; /* Light. unit: lux */ + rt_int32_t proximity; /* Distance. unit: centimeters */ + rt_int32_t hr; /* Heat rate. unit: HZ */ + rt_int32_t tvoc; /* TVOC. unit: permillage */ + rt_int32_t noise; /* Noise Loudness. unit: HZ */ + rt_uint32_t step; /* Step sensor. unit: 1 */ + }data; }; -enum SensorAccelRange +struct rt_sensor_ops { - SENSOR_ACCEL_RANGE_2G, - SENSOR_ACCEL_RANGE_4G, - SENSOR_ACCEL_RANGE_8G, - SENSOR_ACCEL_RANGE_16G, -}; -#define SENSOR_ACCEL_SENSITIVITY_2G ((float)2/32768) -#define SENSOR_ACCEL_SENSITIVITY_4G ((float)4/32768) -#define SENSOR_ACCEL_SENSITIVITY_8G ((float)8/32768) -#define SENSOR_ACCEL_SENSITIVITY_16G ((float)16/32768) - -enum SensorGyroRange -{ - SENSOR_GYRO_RANGE_250DPS, - SENSOR_GYRO_RANGE_500DPS, - SENSOR_GYRO_RANGE_1000DPS, - SENSOR_GYRO_RANGE_2000DPS, -}; -#define SENSOR_GYRO_SENSITIVITY_250DPS (0.00875F) -#define SENSOR_GYRO_SENSITIVITY_500DPS (0.0175F) -#define SENSOR_GYRO_SENSITIVITY_1000DPS (0.035F) -#define SENSOR_GYRO_SENSITIVITY_2000DPS (0.070F) - -enum SensorDataRate -{ - SENSOR_DATARATE_3200HZ, - SENSOR_DATARATE_1600HZ, - SENSOR_DATARATE_800HZ, - SENSOR_DATARATE_400HZ, - SENSOR_DATARATE_200HZ, - SENSOR_DATARATE_100HZ, - SENSOR_DATARATE_50HZ, - SENSOR_DATARATE_25HZ, - SENSOR_DATARATE_12_5HZ, - SENSOR_DATARATE_6_25HZ, - SENSOR_DATARATE_3_13HZ, - SENSOR_DATARATE_1_56HZ, - SENSOR_DATARATE_0_78HZ, - SENSOR_DATARATE_0_39HZ, - SENSOR_DATARATE_0_20HZ, - SENSOR_DATARATE_0_10HZ, + rt_size_t (*fetch_data)(struct rt_sensor_device *sensor, void *buf, rt_size_t len); + rt_err_t (*control)(struct rt_sensor_device *sensor, int cmd, void *arg); }; -/** - * Sensor Configuration - */ -typedef struct SensorConfig -{ - int mode; - - enum SensorDataRate data_rate; - - union range - { - int range; - enum SensorAccelRange accel_range; - enum SensorGyroRange gyro_range; - } range; -}SensorConfig; - -typedef void (*SensorEventHandler_t)(void *user_data); - -#ifdef __cplusplus -class SensorBase; -class SensorManager; - -/** - * Sensor Base Class - */ -class SensorBase -{ -private: - int type; - -public: - SensorBase(int type); - ~SensorBase(); - - virtual int configure(SensorConfig *config) = 0; - virtual int activate(int enable) = 0; - - virtual int poll(sensors_event_t *events) = 0; - virtual void getSensor(struct sensor_t *sensor) = 0; - - int getType(void); - - int setConfig(SensorConfig *config); - int getConfig(SensorConfig *config); - - int subscribe(SensorEventHandler_t handler, void *user_data); - int publish(void); - -protected: - SensorBase *next; - SensorBase *prev; - - /* sensor configuration */ - SensorConfig config; - - SensorEventHandler_t evtHandler; - void *userData; - - friend class SensorManager; -}; - -/** - * Sensor Manager - */ -class SensorManager -{ -public: - SensorManager(); - ~SensorManager(); - - static int registerSensor(SensorBase *sensor); - static int unregisterSensor(SensorBase *sensor); - - static SensorBase *getDefaultSensor(int type); - static int subscribe(int type, SensorEventHandler_t handler, void *user_data); - - static int sensorEventReady(SensorBase *sensor); - static int pollSensor(SensorBase *sensor, sensors_event_t *events, int number, int duration); -}; -#endif - -/* C programming language APIs */ -/* rt_sensor_t is a C typedef for SensorBase */ -typedef void* rt_sensor_t; - -#ifdef __cplusplus -extern "C" { -#endif - -rt_sensor_t rt_sensor_get_default(int type); - -int rt_sensor_subscribe(rt_sensor_t sensor, SensorEventHandler_t handler, void *user_data); -int rt_sensor_activate (rt_sensor_t sensor, int enable); -int rt_sensor_configure(rt_sensor_t sensor, SensorConfig *config); -int rt_sensor_poll(rt_sensor_t sensor, sensors_event_t *event); - -#ifdef __cplusplus -} -#endif - +int rt_hw_sensor_register(rt_sensor_t sensor, + const char *name, + rt_uint32_t flag, + void *data); #endif diff --git a/components/drivers/sensors/sensor_test.c b/components/drivers/sensors/sensor_test.c new file mode 100644 index 0000000000..0221167e83 --- /dev/null +++ b/components/drivers/sensors/sensor_test.c @@ -0,0 +1,361 @@ +/* + * Copyright (c) 2006-2018, RT-Thread Development Team + * + * SPDX-License-Identifier: Apache-2.0 + * + * Change Logs: + * Date Author Notes + * 2019-01-31 flybreak first version + */ + +#include "sensor.h" + +#define DBG_ENABLE +#define DBG_LEVEL DBG_INFO +#define DBG_SECTION_NAME "sensor.test" +#define DBG_COLOR +#include + +#include +#include + +static rt_sem_t sensor_rx_sem = RT_NULL; + +static void sensor_show_data(rt_size_t num, rt_sensor_t sensor, struct rt_sensor_data *sensor_data) +{ + switch (sensor->info.type) + { + case RT_SEN_CLASS_ACCE: + LOG_I("num:%3d, x:%5d, y:%5d, z:%5d, timestamp:%5d", num, sensor_data->data.acce.x, sensor_data->data.acce.y, sensor_data->data.acce.z, sensor_data->timestamp); + break; + case RT_SEN_CLASS_GYRO: + LOG_I("num:%3d, x:%8d, y:%8d, z:%8d, timestamp:%5d", num, sensor_data->data.gyro.x, sensor_data->data.gyro.y, sensor_data->data.gyro.z, sensor_data->timestamp); + break; + case RT_SEN_CLASS_MAG: + LOG_I("num:%3d, x:%5d, y:%5d, z:%5d, timestamp:%5d", num, sensor_data->data.mag.x, sensor_data->data.mag.y, sensor_data->data.mag.z, sensor_data->timestamp); + break; + case RT_SEN_CLASS_HUMI: + LOG_I("num:%3d, humi:%3d.%d%%, timestamp:%5d", num, sensor_data->data.humi / 10, sensor_data->data.humi % 10, sensor_data->timestamp); + break; + case RT_SEN_CLASS_TEMP: + LOG_I("num:%3d, temp:%3d.%dC, timestamp:%5d", num, sensor_data->data.temp / 10, sensor_data->data.temp % 10, sensor_data->timestamp); + break; + case RT_SEN_CLASS_BARO: + LOG_I("num:%3d, press:%5d, timestamp:%5d", num, sensor_data->data.baro, sensor_data->timestamp); + break; + case RT_SEN_CLASS_STEP: + LOG_I("num:%3d, step:%5d, timestamp:%5d", num, sensor_data->data.step, sensor_data->timestamp); + break; + default: + break; + } +} + +rt_err_t rx_callback(rt_device_t dev, rt_size_t size) +{ + rt_sem_release(sensor_rx_sem); + return 0; +} + +static void sensor_fifo_rx_entry(void *parameter) +{ + rt_device_t dev = parameter; + rt_sensor_t sensor = parameter; + struct rt_sensor_data *data = RT_NULL; + struct rt_sensor_info info; + rt_size_t res, i; + + rt_device_control(dev, RT_SEN_CTRL_GET_INFO, &info); + + data = rt_malloc(sizeof(struct rt_sensor_data) * info.fifo_max); + if (data == RT_NULL) + { + LOG_E("Memory allocation failed!"); + } + + while (1) + { + rt_sem_take(sensor_rx_sem, RT_WAITING_FOREVER); + + res = rt_device_read(dev, 0, data, info.fifo_max); + for (i = 0; i < res; i++) + { + sensor_show_data(i, sensor, &data[i]); + } + } +} + +static void sensor_fifo(int argc, char **argv) +{ + static rt_thread_t tid1 = RT_NULL; + rt_device_t dev = RT_NULL; + rt_sensor_t sensor; + + dev = rt_device_find(argv[1]); + if (dev == RT_NULL) + { + LOG_E("Can't find device:%s", argv[1]); + return; + } + sensor = (rt_sensor_t)dev; + + if (sensor_rx_sem == RT_NULL) + { + sensor_rx_sem = rt_sem_create("sen_rx_sem", 0, RT_IPC_FLAG_FIFO); + } + else + { + LOG_E("The thread is running, please reboot and try again"); + return; + } + + tid1 = rt_thread_create("sen_rx_thread", + sensor_fifo_rx_entry, sensor, + 1024, + 15, 5); + + if (tid1 != RT_NULL) + rt_thread_startup(tid1); + + rt_device_set_rx_indicate(dev, rx_callback); + + if (rt_device_open(dev, RT_SEN_FLAG_FIFO) != RT_EOK) + { + LOG_E("open device failed!"); + return; + } + rt_device_control(dev, RT_SEN_CTRL_SET_ODR, (void *)20); +} +#ifdef FINSH_USING_MSH +MSH_CMD_EXPORT(sensor_fifo, Sensor fifo mode test function); +#endif + +static void sensor_irq_rx_entry(void *parameter) +{ + rt_device_t dev = parameter; + rt_sensor_t sensor = parameter; + struct rt_sensor_data data; + rt_size_t res, i = 0; + + while (1) + { + rt_sem_take(sensor_rx_sem, RT_WAITING_FOREVER); + + res = rt_device_read(dev, 0, &data, 1); + if (res == 1) + { + sensor_show_data(i++, sensor, &data); + } + } +} + +static void sensor_int(int argc, char **argv) +{ + static rt_thread_t tid1 = RT_NULL; + rt_device_t dev = RT_NULL; + rt_sensor_t sensor; + + dev = rt_device_find(argv[1]); + if (dev == RT_NULL) + { + LOG_E("Can't find device:%s", argv[1]); + return; + } + sensor = (rt_sensor_t)dev; + + if (sensor_rx_sem == RT_NULL) + { + sensor_rx_sem = rt_sem_create("sen_rx_sem", 0, RT_IPC_FLAG_FIFO); + } + else + { + LOG_E("The thread is running, please reboot and try again"); + return; + } + + tid1 = rt_thread_create("sen_rx_thread", + sensor_irq_rx_entry, sensor, + 1024, + 15, 5); + + if (tid1 != RT_NULL) + rt_thread_startup(tid1); + + rt_device_set_rx_indicate(dev, rx_callback); + + if (rt_device_open(dev, RT_DEVICE_FLAG_INT_RX) != RT_EOK) + { + LOG_E("open device failed!"); + return; + } + rt_device_control(dev, RT_SEN_CTRL_SET_ODR, (void *)20); +} +#ifdef FINSH_USING_MSH +MSH_CMD_EXPORT(sensor_int, Sensor interrupt mode test function); +#endif + +static void sensor_polling(int argc, char **argv) +{ + uint16_t num = 10; + rt_device_t dev = RT_NULL; + rt_sensor_t sensor; + struct rt_sensor_data data; + rt_size_t res, i; + + dev = rt_device_find(argv[1]); + if (dev == RT_NULL) + { + LOG_E("Can't find device:%s", argv[1]); + return; + } + if (argc > 2) + num = atoi(argv[2]); + + sensor = (rt_sensor_t)dev; + + if (rt_device_open(dev, RT_DEVICE_FLAG_RDWR) != RT_EOK) + { + LOG_E("open device failed!"); + return; + } + rt_device_control(dev, RT_SEN_CTRL_SET_ODR, (void *)100); + + for (i = 0; i < num; i++) + { + res = rt_device_read(dev, 0, &data, 1); + if (res != 1) + { + LOG_E("read data failed!size is %d", res); + } + else + { + sensor_show_data(i, sensor, &data); + } + rt_thread_mdelay(100); + } + rt_device_close(dev); +} +#ifdef FINSH_USING_MSH +MSH_CMD_EXPORT(sensor_polling, Sensor polling mode test function); +#endif + +static void sensor(int argc, char **argv) +{ + static rt_device_t dev = RT_NULL; + struct rt_sensor_data data; + rt_size_t res, i; + + /* If the number of arguments less than 2 */ + if (argc < 2) + { + rt_kprintf("\n"); + rt_kprintf("sensor [OPTION] [PARAM]\n"); + rt_kprintf(" probe Probe sensor by given name\n"); + rt_kprintf(" info Get sensor info\n"); + rt_kprintf(" sr Set range to var\n"); + rt_kprintf(" sm Set work mode to var\n"); + rt_kprintf(" sp Set power mode to var\n"); + rt_kprintf(" sodr Set output date rate to var\n"); + rt_kprintf(" read [num] Read [num] times sensor\n"); + rt_kprintf(" num default 5\n"); + return ; + } + else if (!strcmp(argv[1], "info")) + { + struct rt_sensor_info info; + rt_device_control(dev, RT_SEN_CTRL_GET_INFO, &info); + rt_kprintf("vendor :%d\n", info.vendor); + rt_kprintf("model :%s\n", info.model); + rt_kprintf("unit :%d\n", info.unit); + rt_kprintf("range_max :%d\n", info.range_max); + rt_kprintf("range_min :%d\n", info.range_min); + rt_kprintf("period_min:%d\n", info.period_min); + rt_kprintf("fifo_max :%d\n", info.fifo_max); + } + else if (!strcmp(argv[1], "read")) + { + uint16_t num = 5; + + if (dev == RT_NULL) + { + LOG_W("Please probe sensor device first!"); + return ; + } + if (argc == 3) + { + num = atoi(argv[2]); + } + + for (i = 0; i < num; i++) + { + res = rt_device_read(dev, 0, &data, 1); + if (res != 1) + { + LOG_E("read data failed!size is %d", res); + } + else + { + sensor_show_data(i, (rt_sensor_t)dev, &data); + } + rt_thread_mdelay(100); + } + } + else if (argc == 3) + { + if (!strcmp(argv[1], "probe")) + { + rt_uint8_t reg = 0xFF; + if (dev) + { + rt_device_close(dev); + } + + dev = rt_device_find(argv[2]); + if (dev == RT_NULL) + { + LOG_E("Can't find device:%s", argv[1]); + return; + } + if (rt_device_open(dev, RT_DEVICE_FLAG_RDWR) != RT_EOK) + { + LOG_E("open device failed!"); + return; + } + rt_device_control(dev, RT_SEN_CTRL_GET_ID, ®); + LOG_I("device id: 0x%x!", reg); + + } + else if (dev == RT_NULL) + { + LOG_W("Please probe sensor first!"); + return ; + } + else if (!strcmp(argv[1], "sr")) + { + rt_device_control(dev, RT_SEN_CTRL_SET_RANGE, (void *)atoi(argv[2])); + } + else if (!strcmp(argv[1], "sm")) + { + rt_device_control(dev, RT_SEN_CTRL_SET_MODE, (void *)atoi(argv[2])); + } + else if (!strcmp(argv[1], "sp")) + { + rt_device_control(dev, RT_SEN_CTRL_SET_POWER, (void *)atoi(argv[2])); + } + else if (!strcmp(argv[1], "sodr")) + { + rt_device_control(dev, RT_SEN_CTRL_SET_ODR, (void *)atoi(argv[2])); + } + else + { + LOG_W("Unknown command, please enter 'sensor' get help information!"); + } + } + else + { + LOG_W("Unknown command, please enter 'sensor' get help information!"); + } +} +#ifdef FINSH_USING_MSH +MSH_CMD_EXPORT(sensor, sensor test function); +#endif diff --git a/components/finsh/cmd.c b/components/finsh/cmd.c index 0a30aee809..680d9d01d7 100644 --- a/components/finsh/cmd.c +++ b/components/finsh/cmd.c @@ -800,6 +800,7 @@ static char *const device_type_str[] = "Portal Device", "Timer Device", "Miscellaneous Device", + "Sensor Device", "Unknown" }; diff --git a/include/rtdef.h b/include/rtdef.h index 859278e849..bb11bf88ff 100644 --- a/include/rtdef.h +++ b/include/rtdef.h @@ -846,6 +846,7 @@ enum rt_device_class_type RT_Device_Class_Portal, /**< Portal device */ RT_Device_Class_Timer, /**< Timer device */ RT_Device_Class_Miscellaneous, /**< Miscellaneous device */ + RT_Device_Class_Sensor, /**< Sensor device */ RT_Device_Class_Unknown /**< unknown device */ };