lmx/99_7018_lmx
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

first

Browse Source
This commit is contained in:
lmx
2025-10-29 13:10:02 +08:00
commit 49a07fa419
2284 changed files with 642060 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

821
cpu/br28/spatial_effect/spatial_effect_imu.c Normal file
View File

@ -0,0 +1,821 @@
/*****************************************************************
>
>file name : spatial_imu_data.c
>create time : Mon 03 Jan 2022 12:21:19 PM CST
*****************************************************************/
#include "typedef.h"
#include "app_config.h"
#include "gSensor/mpu6050.h"
#include "gSensor/gSensor_manage.h"
#include "spatial_effect_imu.h"
#include "imu_sensor/imuSensor_manage.h"
#include "imu_sensor/mpu6887/mpu6887p.h"
#include "imu_sensor/qmi8658/qmi8658c.h"
#include "imu_sensor/icm_42670p/icm_42670p.h"
#define MOTION_SENSOR_FIFO_ENABLE 1
struct space_data_context {
u8 first_data;
u8 init_state;
u16 timeout;
u16 timer;
#if (TCFG_SENSOR_DATA_EXPORT_ENABLE == SENSOR_DATA_EXPORT_USE_UART)
u8 cbuf[1024];
cbuffer_t data_cbuf;
#endif /*TCFG_SENSOR_DATA_EXPORT_ENABLE*/
};
extern int aec_uart_open(u8 nch, u16 single_size);
extern int aec_uart_fill(u8 ch, void *buf, u16 size);
extern void aec_uart_write(void);
extern int aec_uart_close(void);
void imu_sensor_power_ctl(u32 gpio, u8 value);
static s16 mpu6050_read_data(addr)
{
u8 high;
u8 low;
_gravity_sensor_get_ndata(I2C_ADDR_MPU6050_R, addr, &high, 1);
_gravity_sensor_get_ndata(I2C_ADDR_MPU6050_R, addr + 1, &low, 1);
return (high << 8) | low;
}
static int mpu6050_fifo_read_data(void *data, int len)
{
int remain_len = len;
int rlen = 0;
int total_rlen = 0;
do {
rlen = _gravity_sensor_get_ndata(I2C_ADDR_MPU6050_R, MPU6050_RA_FIFO_R_W, data, remain_len > 254 ? 254 : remain_len);
remain_len -= rlen;
total_rlen += rlen;
} while (remain_len > 0);
return total_rlen;
}
static int swap_data_endian(s16 *data, int len)
{
#define SWAP_16(x) ((((x)&0xFF00) >> 8) | (((x)&0x00FF) << 8))
len >>= 1;
for (int i = 0; i < len; i++) {
data[i] = SWAP_16(data[i]);
}
return len << 1;
}
/*
+------------------------------------------------+
| 陀螺仪满量程与LSB灵敏度对应表 |
+--------+--------------------+------------------+
| FS_SEL | Full Scale Range | LSB Sensitivity |
+--------+--------------------+------------------+
| 0 | ±250°/s | 131 LSB/°/s |
+--------+--------------------+------------------+
| 1 | ±500°/s | 65.5 LSB/°/s |
+--------+--------------------+------------------+
| 2 | ±1000°/s | 32.8 LSB/°/s |
+--------+--------------------+------------------+
| 3 | ±2000°/s | 16.4 LSB/°/s |
+--------+--------------------+------------------+
+------------------------------------------------+
| 加速计满量程与LSB灵敏度对应表 |
+--------+--------------------+------------------+
| FS_SEL | Full Scale Range | LSB Sensitivity |
+--------+--------------------+------------------+
| 0 | ±2g | 16384 LSB/g |
+--------+--------------------+------------------+
| 1 | ±4g | 8192 LSB/g |
+--------+--------------------+------------------+
| 2 | ±8g | 4096 LSB/g |
+--------+--------------------+------------------+
| 3 | ±16g | 2048 LSB/g |
+--------+--------------------+------------------+
*/
static void space_motion_detect_timer(void *arg)
{
#if MOTION_SENSOR_FIFO_ENABLE
s16 data_len = mpu6050_read_data(MPU6050_RA_FIFO_COUNTH);
s16 *data = NULL;
int i = 0;
if (data_len) {
data = (s16 *)malloc(data_len);
mpu6050_fifo_read_data(data, data_len);
swap_data_endian(data, data_len);
printf("-- data_len : %d --\n", data_len);
data_len /= sizeof(*data);
s16 *print_data = data;
for (i = 0; i < data_len; i += 3) {
printf("[%d, %d, %d]\n", data[i], data[i + 1], data[i + 2]);
}
free(data);
}
#else
s16 gyro_xout, gyro_yout, gyro_zout;
s16 accel_xout, accel_yout, accel_zout;
gyro_xout = mpu6050_read_data(MPU6050_RA_GYRO_XOUT_H);
gyro_yout = mpu6050_read_data(MPU6050_RA_GYRO_YOUT_H);
gyro_zout = mpu6050_read_data(MPU6050_RA_GYRO_ZOUT_H);
accel_xout = mpu6050_read_data(MPU6050_RA_ACCEL_XOUT_H);
accel_yout = mpu6050_read_data(MPU6050_RA_ACCEL_YOUT_H);
accel_zout = mpu6050_read_data(MPU6050_RA_ACCEL_ZOUT_H);
printf("x : %d, y : %d, z : %d\n accel, x : %d, y : %d, z : %d\n", gyro_xout, gyro_yout, gyro_zout, accel_xout, accel_yout, accel_zout);
#endif
}
int __mpu6050_space_motion_data_read(void *sensor, void *data, int len)
{
struct space_data_context *ctx = (struct space_data_context *)sensor;
#if 1
if (!ctx->init_state) {
return 0;
}
s16 data_len = mpu6050_read_data(MPU6050_RA_FIFO_COUNTH);
if (data_len < 12) {
return 0;
}
if (data_len > 1000) {
printf("gsensor fifo full : %d\n", data_len);
gravity_sensor_command(I2C_ADDR_MPU6050_W, MPU6050_RA_USER_CTRL, 0x44);
}
data_len = (data_len / 12) * 12;
data_len = mpu6050_fifo_read_data(data, data_len);
if (ctx->first_data) {
ctx->first_data = 0;
return 0;
}
swap_data_endian((s16 *)data, data_len);
return data_len;
#else
int *rand_data = (int *)data;
rand_data[0] = 0x12345678;
rand_data[1] = 0x23456789;
rand_data[2] = 0x3456789A;
rand_data[3] = 0x456789AB;
rand_data[4] = 0x56789ABC;
rand_data[5] = 0x6789ABCD;
rand_data[6] = 0x789ABCDE;
rand_data[7] = 0x89ABCDEF;
return 32;
#endif
}
static void space_motion_sensor_init(void *arg)
{
struct space_data_context *ctx = (struct space_data_context *)arg;
sys_timeout_del(ctx->timeout);
ctx->timeout = 0;
gravity_sensor_command(I2C_ADDR_MPU6050_W, MPU6050_RA_PWR_MGMT_1, 0x4); //关闭温度传感器
gravity_sensor_command(I2C_ADDR_MPU6050_W, MPU6050_RA_INT_ENABLE, 0x0); //关闭中断
#if 1
gravity_sensor_command(I2C_ADDR_MPU6050_W, MPU6050_RA_SMPLRT_DIV, 7); //设置采样率, 采样率=陀螺仪输出速率/(1+SMPLRT_DIV)
#if MOTION_SENSOR_FIFO_ENABLE
gravity_sensor_command(I2C_ADDR_MPU6050_W, MPU6050_RA_USER_CTRL, 0x44);
gravity_sensor_command(I2C_ADDR_MPU6050_W, MPU6050_RA_FIFO_EN, 0x78); // 陀螺仪xyz fifo使能，加速计fifo使能
#endif
gravity_sensor_command(I2C_ADDR_MPU6050_W, MPU6050_RA_CONFIG, 0x6); // 1kHz
gravity_sensor_command(I2C_ADDR_MPU6050_W, MPU6050_RA_GYRO_CONFIG, 0x18); // ±2000°/s
gravity_sensor_command(I2C_ADDR_MPU6050_W, MPU6050_RA_ACCEL_CONFIG, 0x19); // ±2g
#endif
ctx->init_state = 1;
}
static void space_motion_sensor_reset(void)
{
gravity_sensor_command(I2C_ADDR_MPU6050_W, MPU6050_RA_PWR_MGMT_1, 0x80); // 复位
}
void *__mpu6050_space_motion_detect_open(void)
{
u8 id = 0;
_gravity_sensor_get_ndata(I2C_ADDR_MPU6050_R, MPU6050_RA_WHO_AM_I, &id, 1);
if (id != MPU_ADDR) {
return NULL;
}
struct space_data_context *ctx = (struct space_data_context *)zalloc(sizeof(struct space_data_context));
if (!ctx) {
return NULL;
}
space_motion_sensor_reset();
ctx->timeout = sys_timeout_add((void *)ctx, space_motion_sensor_init, 100);
ctx->first_data = 1;
/*ctx->timer = sys_timer_add(NULL, space_motion_detect_timer, 100); */
return ctx;
}
void __mpu6050_space_motion_detect_close(void *sensor)
{
struct space_data_context *ctx = (struct space_data_context *)sensor;
#if 1
if (ctx->timeout) {
sys_timeout_del(ctx->timeout);
}
gravity_sensor_command(I2C_ADDR_MPU6050_W, MPU6050_RA_USER_CTRL, 0);
gravity_sensor_command(I2C_ADDR_MPU6050_W, MPU6050_RA_FIFO_EN, 0); // 陀螺仪xyz fifo使能，加速计fifo使能
/*gravity_sensor_command(I2C_ADDR_MPU6050_W, MPU6050_RA_PWR_MGMT_1, BIT(6));*/
#endif
if (ctx) {
if (ctx->timer) {
sys_timer_del(ctx->timer);
ctx->timer = 0;
}
free(ctx);
}
}
#if TCFG_ICM42670P_ENABLE
int __icm42670p_space_motion_data_read(void *sensor, void *data, int len)
{
struct space_data_context *ctx = (struct space_data_context *)sensor;
#if 1
if (!ctx->init_state) {
return 0;
}
u8 sensor_name[20] = "icm42670p";
int data_len = imu_sensor_io_ctl(sensor_name, IMU_GET_SENSOR_READ_FIFO, (u8 *)data);
if (data_len < 12) {
return 0;
}
//丢掉第一次读到的数据
if (ctx->first_data) {
ctx->first_data = 0;
return 0;
}
return data_len;
#else
int *rand_data = (int *)data;
rand_data[0] = 0x12345678;
rand_data[1] = 0x23456789;
rand_data[2] = 0x3456789A;
rand_data[3] = 0x456789AB;
rand_data[4] = 0x56789ABC;
rand_data[5] = 0x6789ABCD;
rand_data[6] = 0x789ABCDE;
rand_data[7] = 0x89ABCDEF;
return 32;
#endif
}
void *__icm42670p_space_motion_detect_open(void)
{
u8 sensor_name[20] = "icm42670p";
u8 arg_data;
//唤醒MPU
imu_sensor_io_ctl(sensor_name, IMU_SENSOR_WAKEUP, &arg_data);
//检查传感器id
int ret = imu_sensor_io_ctl(sensor_name, IMU_SENSOR_SEARCH, &arg_data);
printf("ret : %d\n", ret);
if (ret != 0) {
return NULL;
}
struct space_data_context *ctx = (struct space_data_context *)zalloc(sizeof(struct space_data_context));
if (!ctx) {
return NULL;
}
ctx->first_data = 1;
ctx->init_state = 1;
printf(" __space_motion_detect_open success");
return ctx;
}
void __icm42670p_space_motion_detect_close(void *sensor)
{
struct space_data_context *ctx = (struct space_data_context *)sensor;
u8 sensor_name[20] = "icm42670p";
u8 arg_data;
#if 1
if (ctx->timeout) {
sys_timeout_del(ctx->timeout);
}
#endif
if (ctx) {
//MPU进入睡眠
imu_sensor_io_ctl(sensor_name, IMU_SENSOR_SLEEP, NULL);
if (ctx->timer) {
sys_timer_del(ctx->timer);
ctx->timer = 0;
}
free(ctx);
}
}
#endif /*TCFG_ICM42670P_ENABLE*/
#if TCFG_MPU6887P_ENABLE
/*剔除mpu6887p fifo数据中的温度数据*/
/*数据格式:acc + temp + gyro*/
static int fifo_data_delete_temp(u8 *buf, int len)
{
int data_len = len;
len = len / 14; //数据帧数
buf = buf + 6;//温度数据起始位置
int tmp = 2;
//把每帧温度数据后面的12字节的加速度和陀螺仪依次往前移，覆盖温度数据
for (int i = 0; i < len; i++) {
memcpy(buf, buf + tmp, 12);
tmp = tmp + 2;
buf = buf + 12;
}
return (data_len - (len << 1));
}
int __mpu6887p_space_motion_data_read(void *sensor, void *data, int len)
{
struct space_data_context *ctx = (struct space_data_context *)sensor;
#if 1
if (!ctx->init_state) {
return 0;
}
int data_len = mpu6887p_read_fifo_data((u8 *)data);
if (data_len < 14) {
return 0;
}
/* if (data_len > 1000) { */
/* printf("gsensor fifo full : %d\n", data_len); */
/* gravity_sensor_command(I2C_ADDR_MPU6050_W, MPU6050_RA_USER_CTRL, 0x44); */
/* } */
//丢掉第一次读到的数据
if (ctx->first_data) {
ctx->first_data = 0;
return 0;
}
//交换s16数据的高低位
swap_data_endian((s16 *)data, data_len);
//剔除温度数据
data_len = fifo_data_delete_temp((u8 *)data, data_len);
return data_len;
#else
int *rand_data = (int *)data;
rand_data[0] = 0x12345678;
rand_data[1] = 0x23456789;
rand_data[2] = 0x3456789A;
rand_data[3] = 0x456789AB;
rand_data[4] = 0x56789ABC;
rand_data[5] = 0x6789ABCD;
rand_data[6] = 0x789ABCDE;
rand_data[7] = 0x89ABCDEF;
return 32;
#endif
}
void *__mpu6887p_space_motion_detect_open(void)
{
u8 name_test[20] = "mpu6887p";
u8 arg_data;
//唤醒MPU
mpu6887p_set_sleep_enabled(0);// 0:唤醒MPU, 1:disable
//判断mpu6887p是否已经初始化了
u8 status_temp = mpu6887p_read_status();
printf("status:0x%x", status_temp);
if ((status_temp & 0x01) == 0) {
//如果还没有初始化，重新初始化
imu_sensor_io_ctl(name_test, IMU_SENSOR_ENABLE, &arg_data);
}
//检查传感器id
int ret = imu_sensor_io_ctl(name_test, IMU_SENSOR_SEARCH, &arg_data);
printf("ret : %d\n", ret);
if (ret != 0) {
return NULL;
}
struct space_data_context *ctx = (struct space_data_context *)zalloc(sizeof(struct space_data_context));
if (!ctx) {
return NULL;
}
//关闭温度传感器
mpu6887p_disable_temp_Sensor(1);
//设置fifo使能
mpu6887p_config_fifo(300, 1, 1, 1);
ctx->first_data = 1;
ctx->init_state = 1;
printf(" __space_motion_detect_open success");
return ctx;
}
void __mpu6887p_space_motion_detect_close(void *sensor)
{
struct space_data_context *ctx = (struct space_data_context *)sensor;
#if 1
if (ctx->timeout) {
sys_timeout_del(ctx->timeout);
}
#endif
if (ctx) {
//MPU进入睡眠
mpu6887p_set_sleep_enabled(1);// 0:唤醒MPU, 1:disable
if (ctx->timer) {
sys_timer_del(ctx->timer);
ctx->timer = 0;
}
free(ctx);
}
}
#endif /*TCFG_MPU6887P_ENABLE*/
#if TCFG_QMI8658_ENABLE
int __qmi8658_space_motion_data_read(void *sensor, void *data, int len)
{
struct space_data_context *ctx = (struct space_data_context *)sensor;
#if 1
if (!ctx->init_state) {
return 0;
}
u8 name_test[20] = "qmi8658";
int data_len = imu_sensor_io_ctl(name_test, IMU_GET_SENSOR_READ_FIFO, (u8 *)data);
if (data_len < 12) {
return 0;
}
//丢掉第一次读到的数据
if (ctx->first_data) {
ctx->first_data = 0;
return 0;
}
return data_len;
#else
int *rand_data = (int *)data;
rand_data[0] = 0x12345678;
rand_data[1] = 0x23456789;
rand_data[2] = 0x3456789A;
rand_data[3] = 0x456789AB;
rand_data[4] = 0x56789ABC;
rand_data[5] = 0x6789ABCD;
rand_data[6] = 0x789ABCDE;
rand_data[7] = 0x89ABCDEF;
return 32;
#endif
}
void *__qmi8658_space_motion_detect_open(void)
{
u8 name_test[20] = "qmi8658";
u8 arg_data;
//唤醒MPU
imu_sensor_io_ctl(name_test, IMU_SENSOR_WAKEUP, &arg_data);
//检查传感器id
int ret = imu_sensor_io_ctl(name_test, IMU_SENSOR_SEARCH, &arg_data);
printf("ret : %d\n", ret);
if (ret != 0) {
return NULL;
}
struct space_data_context *ctx = (struct space_data_context *)zalloc(sizeof(struct space_data_context));
if (!ctx) {
return NULL;
}
//设置fifo使能
/* u8 mytmp[4]; */
/* mytmp[0] = 128; */
/* mytmp[1] = Qmi8658_Fifo_128; */
/* mytmp[2] = Qmi8658_Fifo_Stream; */
/* mytmp[3] = QMI8658_FORMAT_12_BYTES; */
/* imu_sensor_io_ctl(name_test, IMU_SET_SENSOR_FIFO_CONFIG, &mytmp); */
ctx->first_data = 1;
ctx->init_state = 1;
printf(" __space_motion_detect_open success");
return ctx;
}
void __qmi8658_space_motion_detect_close(void *sensor)
{
struct space_data_context *ctx = (struct space_data_context *)sensor;
u8 name_test[20] = "qmi8658";
u8 arg_data;
#if 1
if (ctx->timeout) {
sys_timeout_del(ctx->timeout);
}
#endif
if (ctx) {
//MPU进入睡眠
imu_sensor_io_ctl("qmi8658", IMU_SENSOR_SLEEP, NULL);
if (ctx->timer) {
sys_timer_del(ctx->timer);
ctx->timer = 0;
}
free(ctx);
}
}
#endif /*TCFG_QMI8658_ENABLE*/
#if TCFG_LSM6DSL_ENABLE
/*交换lsm6dsl陀螺仪和加速度数据的位置*/
/*交换前数据格式:gyro + acc*/
static int swap_gyro_acc_data(s16 *buf, int len)
{
int frames = len / 12; //数据帧数
s16 tmp[3];
for (int i = 0; i < frames; i++) {
memcpy(tmp, buf, 6);
memcpy(buf, buf + 3, 6);
memcpy(buf + 3, tmp, 6);
buf = buf + 6;
}
return len;
}
int __lsm6dsl_space_motion_data_read(void *sensor, void *data, int len)
{
struct space_data_context *ctx = (struct space_data_context *)sensor;
#if 1
if (!ctx->init_state) {
return 0;
}
u8 sensor_name[20] = "lsm6dsl";
int data_len = imu_sensor_io_ctl(sensor_name, IMU_GET_SENSOR_READ_FIFO, (u8 *)data);
if (data_len < 12) {
return 0;
}
//丢掉第一次读到的数据
if (ctx->first_data) {
ctx->first_data = 0;
return 0;
}
//交换陀螺仪和加速度的数据位置
swap_gyro_acc_data(data, len);
return data_len;
#else
int *rand_data = (int *)data;
rand_data[0] = 0x12345678;
rand_data[1] = 0x23456789;
rand_data[2] = 0x3456789A;
rand_data[3] = 0x456789AB;
rand_data[4] = 0x56789ABC;
rand_data[5] = 0x6789ABCD;
rand_data[6] = 0x789ABCDE;
rand_data[7] = 0x89ABCDEF;
return 32;
#endif
}
void *__lsm6dsl_space_motion_detect_open(void)
{
u8 sensor_name[20] = "lsm6dsl";
u8 arg_data;
//唤醒MPU
imu_sensor_io_ctl(sensor_name, IMU_SENSOR_WAKEUP, &arg_data);
//检查传感器id
int ret = imu_sensor_io_ctl(sensor_name, IMU_SENSOR_SEARCH, &arg_data);
printf("ret : %d\n", ret);
if (ret != 0) {
return NULL;
}
struct space_data_context *ctx = (struct space_data_context *)zalloc(sizeof(struct space_data_context));
if (!ctx) {
return NULL;
}
//设置fifo使能
/* u8 mytmp[4]; */
/* mytmp[0] = 128; */
/* mytmp[1] = Qmi8658_Fifo_128; */
/* mytmp[2] = Qmi8658_Fifo_Stream; */
/* mytmp[3] = QMI8658_FORMAT_12_BYTES; */
/* imu_sensor_io_ctl(sensor_name, IMU_SET_SENSOR_FIFO_CONFIG, &mytmp); */
ctx->first_data = 1;
ctx->init_state = 1;
printf(" __space_motion_detect_open success");
return ctx;
}
void __lsm6dsl_space_motion_detect_close(void *sensor)
{
struct space_data_context *ctx = (struct space_data_context *)sensor;
u8 sensor_name[20] = "lsm6dsl";
u8 arg_data;
#if 1
if (ctx->timeout) {
sys_timeout_del(ctx->timeout);
}
#endif
if (ctx) {
//MPU进入睡眠
imu_sensor_io_ctl(sensor_name, IMU_SENSOR_SLEEP, NULL);
if (ctx->timer) {
sys_timer_del(ctx->timer);
ctx->timer = 0;
}
free(ctx);
}
}
#endif /*TCFG_LSM6DSL_ENABLE*/
int space_motion_data_read(void *sensor, void *data, int len)
{
struct space_data_context *ctx = (struct space_data_context *)sensor;
if (!ctx) {
return 0;
}
int data_len = 0;
#if TCFG_ICM42670P_ENABLE
data_len = __icm42670p_space_motion_data_read(sensor, data, len);
#elif TCFG_LSM6DSL_ENABLE
data_len = __lsm6dsl_space_motion_data_read(sensor, data, len);
#elif TCFG_MPU6887P_ENABLE
data_len = __mpu6887p_space_motion_data_read(sensor, data, len);
#elif TCFG_QMI8658_ENABLE
data_len = __qmi8658_space_motion_data_read(sensor, data, len);
#elif TCFG_MPU6050_EN
data_len = __mpu6050_space_motion_data_read(sensor, data, len);
#endif
#if (TCFG_SENSOR_DATA_EXPORT_ENABLE == SENSOR_DATA_EXPORT_USE_UART)
int wlen = cbuf_write(&ctx->data_cbuf, data, data_len);
if (cbuf_get_data_size(&ctx->data_cbuf) >= 360) {
u8 tmp_buf[512];
cbuf_read(&ctx->data_cbuf, tmp_buf, 360);
aec_uart_fill(0, tmp_buf, 360);
aec_uart_write();
putchar('|');
}
#elif (TCFG_SENSOR_DATA_EXPORT_ENABLE == SENSOR_DATA_EXPORT_USE_SPP)
extern int audio_data_export_run(u8 ch, u8 * data, int len);
audio_data_export_run(0, data, data_len);
#endif /*TCFG_SENSOR_DATA_EXPORT_ENABLE*/
return data_len;
}
void *space_motion_detect_open(void)
{
struct space_data_context *ctx = NULL;
/*打开传感器电源*/
imu_sensor_power_ctl(TCFG_IMU_SENSOR_PWR_PORT, 1);
os_time_dly(1);
#if TCFG_ICM42670P_ENABLE
ctx = __icm42670p_space_motion_detect_open();
#elif TCFG_LSM6DSL_ENABLE
ctx = __lsm6dsl_space_motion_detect_open();
#elif TCFG_MPU6887P_ENABLE
ctx = __mpu6887p_space_motion_detect_open();
#elif TCFG_QMI8658_ENABLE
ctx = __qmi8658_space_motion_detect_open();
#elif TCFG_MPU6050_EN
ctx = __mpu6050_space_motion_detect_open();
#endif
if (ctx) {
#if (TCFG_SENSOR_DATA_EXPORT_ENABLE == SENSOR_DATA_EXPORT_USE_UART)
cbuf_init(&ctx->data_cbuf, ctx->cbuf, sizeof(ctx->cbuf));
aec_uart_open(1, 360);
#endif /*TCFG_SENSOR_DATA_EXPORT_ENABLE*/
}
return ctx;
}
void space_motion_detect_close(void *sensor)
{
if (!sensor) {
return;
}
#if TCFG_ICM42670P_ENABLE
__icm42670p_space_motion_detect_close(sensor);
#elif TCFG_LSM6DSL_ENABLE
__lsm6dsl_space_motion_detect_close(sensor);
#elif TCFG_MPU6887P_ENABLE
__mpu6887p_space_motion_detect_close(sensor);
#elif TCFG_QMI8658_ENABLE
__qmi8658_space_motion_detect_close(sensor);
#elif TCFG_MPU6050_EN
__mpu6050_space_motion_detect_close(sensor);
#endif
/*关闭传感器电源*/
/* imu_sensor_power_ctl(TCFG_IMU_SENSOR_PWR_PORT, 0); */
#if (TCFG_SENSOR_DATA_EXPORT_ENABLE == SENSOR_DATA_EXPORT_USE_UART)
aec_uart_close();
#endif /*TCFG_SENSOR_DATA_EXPORT_ENABLE*/
}
void imu_sensor_ad0_selete(u32 gpio, u8 value)
{
#if (TCFG_MPU6887P_AD0_SELETE_IO != NO_CONFIG_PORT) || \
(TCFG_QMI8658_AD0_SELETE_IO != NO_CONFIG_PORT)
gpio_set_die(gpio, 1);
gpio_set_pull_up(gpio, 0);
gpio_set_pull_down(gpio, 0);
gpio_direction_output(gpio, value);
#endif
}
void imu_sensor_power_ctl(u32 gpio, u8 value)
{
#if (TCFG_IMU_SENSOR_PWR_PORT != NO_CONFIG_PORT)
gpio_set_die(gpio, 1);
gpio_set_pull_up(gpio, 0);
gpio_set_pull_down(gpio, 0);
gpio_direction_output(gpio, value);
#endif
}
void imu_sensor_test(void)
{
#if 0
#include "imu_sensor/imuSensor_manage.h"
#include "imu_sensor/mpu6887/mpu6887p.h"
#include "imu_sensor/lsm6dsl/lsm6dsl.h"
/* u8 name_test[20] = "mpu6887p"; */
/* u8 name_test[20] = "lsm6dsl"; */
u8 name_test[20] = "icm42670p";
int arg_data = 0;
int ret = 0;
//获取传感器状态
ret = imu_sensor_io_ctl(name_test, IMU_GET_SENSOR_STATUS, &arg_data);
printf("sta : %d\n", arg_data);
if ((arg_data & 0x01) == 0) {
//如果还没有初始化，重新初始化
imu_sensor_io_ctl(name_test, IMU_SENSOR_ENABLE, &arg_data);
}
//检查传感器id
ret = imu_sensor_io_ctl(name_test, IMU_SENSOR_SEARCH, &arg_data);
extern int swap_data_endian(s16 * data, int len);
extern int fifo_data_delete_temp(u8 * buf, int len);
//关闭温度传感器
arg_data = 1;
/* ret = imu_sensor_io_ctl(name_test, IMU_SET_SENSOR_TEMP_DISABLE, &arg_data); */
//设置fifo使能
u8 mytmp[5];
mytmp[0] = (1000 & 0xff);
mytmp[1] = (1000 >> 8) & 0xff;
mytmp[2] = 1;
mytmp[3] = 1;
mytmp[4] = 1;
/* ret = imu_sensor_io_ctl(name_test, IMU_SET_SENSOR_FIFO_CONFIG, &mytmp); */
s16 s_data[1024];
int data_len = 0;
//读取六轴数据
printf("lsm6dsl_read_raw_acc_gyro_xyz");
/* data_len = imu_sensor_io_ctl(name_test, IMU_SENSOR_READ_DATA, s_data); */
//d读取fifo数据
u8 cnt = 10;
while (cnt--) {
printf("lsm6dsl_read_fifo");
data_len = imu_sensor_io_ctl(name_test, IMU_GET_SENSOR_READ_FIFO, s_data);
printf("data_len: %d", data_len);
if (data_len >= 12) {
/* swap_data_endian((s16 *)s_data, data_len); */
/* data_len = fifo_data_delete_temp((u8 *)s_data, data_len); */
for (int i = 0; i < data_len / 6; i = i + 6) {
printf("%d, %d, %d, %d, %d, %d\n", s_data[i], s_data[i + 1], s_data[i + 2], s_data[i + 3], s_data[i + 4], s_data[i + 5]);
}
}
os_time_dly(50);//500ms
}
#endif
}