feat: Add rfid feature and .gitignore file
This commit is contained in:
133
apps/earphone/xtell_Sensor/sensor/AK8963.c
Normal file
133
apps/earphone/xtell_Sensor/sensor/AK8963.c
Normal file
@ -0,0 +1,133 @@
|
||||
|
||||
#include "AK8963.h"
|
||||
#include "math.h"
|
||||
#include "os/os_api.h"
|
||||
#include "../xtell.h"
|
||||
#include "printf.h"
|
||||
|
||||
// 用于存放从Fuse ROM读取的磁力计灵敏度校准值
|
||||
static float mag_asa_x = 1.0f;
|
||||
static float mag_asa_y = 1.0f;
|
||||
static float mag_asa_z = 1.0f;
|
||||
|
||||
// 磁力计在16-bit分辨率下的转换因子 (单位: uT/LSB)
|
||||
#define MAG_RAW_TO_UT_FACTOR (4912.0f / 32760.0f)
|
||||
|
||||
/**
|
||||
* @brief 初始化MPU9250的磁力计AK8963
|
||||
* @return 0: 成功, 1: MPU9250连接失败, 2: AK8963连接失败
|
||||
*/
|
||||
u8 MPU9250_Mag_Init(void) {
|
||||
|
||||
u8 temp_data[3];
|
||||
|
||||
// --- 检查 MPU9250 连接并复位 ---
|
||||
_gravity_sensor_get_ndata(MPU9250_ADDR_R, MPU9250_WHO_AM_I, temp_data, 1);
|
||||
if (temp_data[0] != 0x71 && temp_data[0] != 0x73) {
|
||||
printf("MPU9250 comm failed, read ID: 0x%X\n", temp_data[0]);
|
||||
return 1;
|
||||
}
|
||||
printf("MPU9250 get id:0x%X\n", temp_data[0]);
|
||||
|
||||
gravity_sensor_command(MPU9250_ADDR_W, MPU9250_PWR_MGMT_1, 0x80); // 软复位
|
||||
os_time_dly(10); // 等待复位完成
|
||||
|
||||
gravity_sensor_command(MPU9250_ADDR_W, MPU9250_PWR_MGMT_1, 0x01); // 退出睡眠，选择时钟源
|
||||
os_time_dly(2);
|
||||
|
||||
// --- 强制复位 I2C Master 模块并开启旁路 ---
|
||||
|
||||
gravity_sensor_command(MPU9250_ADDR_W, MPU9250_USER_CTRL, 0x20);
|
||||
os_time_dly(1);
|
||||
gravity_sensor_command(MPU9250_ADDR_W, MPU9250_USER_CTRL, 0x00);
|
||||
os_time_dly(1);
|
||||
|
||||
gravity_sensor_command(MPU9250_ADDR_W, MPU9250_INT_PIN_CFG, 0x02);
|
||||
os_time_dly(2);
|
||||
|
||||
// --- 再次验证 AK8963 连接 ---
|
||||
_gravity_sensor_get_ndata(AK8963_ADDR_R, AK8963_WIA, temp_data, 1);
|
||||
if (temp_data[0] != 0x48) {
|
||||
printf("AK8963 comm failed after final attempt, read ID: 0x%X\n", temp_data[0]);
|
||||
return 2;
|
||||
}
|
||||
printf("AK8963 get id: 0x%X\n", temp_data[0]);
|
||||
|
||||
// ------------------ 配置 AK8963 ------------------
|
||||
// Power-down模式
|
||||
gravity_sensor_command(AK8963_ADDR_W, AK8963_CNTL1, 0x00);
|
||||
os_time_dly(1);
|
||||
|
||||
// Fuse ROM access模式
|
||||
gravity_sensor_command(AK8963_ADDR_W, AK8963_CNTL1, 0x0F);
|
||||
os_time_dly(1);
|
||||
_gravity_sensor_get_ndata(AK8963_ADDR_R, AK8963_ASAX, temp_data, 3);
|
||||
|
||||
// 计算校准系数
|
||||
mag_asa_x = (float)(temp_data[0] - 128) / 256.0f + 1.0f;
|
||||
mag_asa_y = (float)(temp_data[1] - 128) / 256.0f + 1.0f;
|
||||
mag_asa_z = (float)(temp_data[2] - 128) / 256.0f + 1.0f;
|
||||
|
||||
// 再次进入Power-down模式
|
||||
gravity_sensor_command(AK8963_ADDR_W, AK8963_CNTL1, 0x00);
|
||||
os_time_dly(1);
|
||||
|
||||
// 设置工作模式：16-bit分辨率，100Hz连续测量模式 (0x16)
|
||||
gravity_sensor_command(AK8963_ADDR_W, AK8963_CNTL1, 0x16);
|
||||
os_time_dly(1);
|
||||
|
||||
printf("AK8963 configured successfully.\n");
|
||||
return 0; // 初始化成功
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* @brief 读取磁力计的三轴原始数据
|
||||
* @param mx, my, mz - 用于存放X, Y, Z轴数据的指针 (int16_t类型)
|
||||
* @return 0: 成功, 1: 数据未就绪, 2: 数据溢出
|
||||
*/
|
||||
u8 MPU9250_Read_Mag_Raw(int16_t *mx, int16_t *my, int16_t *mz) {
|
||||
u8 read_buf[7];
|
||||
|
||||
// 检查数据是否准备好 (使用8位读地址)
|
||||
_gravity_sensor_get_ndata(AK8963_ADDR_R, AK8963_ST1, read_buf, 1);
|
||||
if (!(read_buf[0] & 0x01)) {
|
||||
return 1; // 数据未就绪
|
||||
}
|
||||
|
||||
// 连续读取7个字节 (使用8位读地址)
|
||||
_gravity_sensor_get_ndata(AK8963_ADDR_R, AK8963_HXL, read_buf, 7);
|
||||
|
||||
// 检查数据是否溢出
|
||||
if (read_buf[6] & 0x08) {
|
||||
return 2; // 数据溢出
|
||||
}
|
||||
|
||||
// 组合数据
|
||||
*mx = (int16_t)((read_buf[1] << 8) | read_buf[0]);
|
||||
*my = (int16_t)((read_buf[3] << 8) | read_buf[2]);
|
||||
*mz = (int16_t)((read_buf[5] << 8) | read_buf[4]);
|
||||
|
||||
return 0; // 读取成功
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 读取磁力计的三轴数据，并转换为uT(微特斯拉) (此函数内部逻辑不变)
|
||||
* @param mx, my, mz - 用于存放X, Y, Z轴数据的指针 (float类型)
|
||||
* @return 0: 成功, 1: 数据未就绪, 2: 数据溢出
|
||||
*/
|
||||
u8 MPU9250_Read_Mag_uT(float *mx, float *my, float *mz) {
|
||||
int16_t raw_mx, raw_my, raw_mz;
|
||||
|
||||
u8 status = MPU9250_Read_Mag_Raw(&raw_mx, &raw_my, &raw_mz);
|
||||
if (status != 0) {
|
||||
return status;
|
||||
}
|
||||
|
||||
// 应用灵敏度校准，并转换为uT单位
|
||||
*mx = (float)raw_mx * mag_asa_x * MAG_RAW_TO_UT_FACTOR;
|
||||
*my = (float)raw_my * mag_asa_y * MAG_RAW_TO_UT_FACTOR;
|
||||
*mz = (float)raw_mz * mag_asa_z * MAG_RAW_TO_UT_FACTOR;
|
||||
|
||||
return 0;
|
||||
}
|
||||
46
apps/earphone/xtell_Sensor/sensor/AK8963.h
Normal file
46
apps/earphone/xtell_Sensor/sensor/AK8963.h
Normal file
@ -0,0 +1,46 @@
|
||||
// mpu9250_mag.h
|
||||
|
||||
#ifndef __MPU9250_MAG_H
|
||||
#define __MPU9250_MAG_H
|
||||
|
||||
#include "stdint.h" // 假设你有标准整数类型，u8 对应 uint8_t
|
||||
#include "gSensor/gSensor_manage.h"
|
||||
|
||||
//==================================================================================
|
||||
// MPU9250 和 AK8963 的 I2C 地址 (已转换为8位格式)
|
||||
//==================================================================================
|
||||
// MPU9250的7位地址是 0x68(接地)
|
||||
#define MPU9250_ADDR_7BIT 0x69
|
||||
#define MPU9250_ADDR_W (MPU9250_ADDR_7BIT << 1 | 0) // 8位写地址: 0xD0
|
||||
#define MPU9250_ADDR_R (MPU9250_ADDR_7BIT << 1 | 1) // 8位读地址: 0xD1
|
||||
|
||||
// AK8963磁力计的7位地址是 0x0C
|
||||
#define AK8963_ADDR_7BIT 0x0C
|
||||
#define AK8963_ADDR_W (AK8963_ADDR_7BIT << 1 | 0) // 8位写地址: 0x18
|
||||
#define AK8963_ADDR_R (AK8963_ADDR_7BIT << 1 | 1) // 8位读地址: 0x19
|
||||
|
||||
|
||||
//==================================================================================
|
||||
// MPU9250 相关寄存器 (用于开启旁路模式)
|
||||
//==================================================================================
|
||||
#define MPU9250_WHO_AM_I 0x75
|
||||
#define MPU9250_INT_PIN_CFG 0x37
|
||||
#define MPU9250_USER_CTRL 0x6A
|
||||
#define MPU9250_PWR_MGMT_1 0x6B
|
||||
//==================================================================================
|
||||
// AK8963 磁力计相关寄存器
|
||||
//==================================================================================
|
||||
#define AK8963_WIA 0x00
|
||||
#define AK8963_ST1 0x02
|
||||
#define AK8963_HXL 0x03
|
||||
#define AK8963_ST2 0x09
|
||||
#define AK8963_CNTL1 0x0A
|
||||
#define AK8963_ASAX 0x10
|
||||
|
||||
|
||||
u8 MPU9250_Mag_Init(void);
|
||||
u8 MPU9250_Read_Mag_Raw(int16_t *mx, int16_t *my, int16_t *mz);
|
||||
u8 MPU9250_Read_Mag_uT(float *mx, float *my, float *mz);
|
||||
|
||||
|
||||
#endif // __MPU9250_MAG_H
|
||||
211
apps/earphone/xtell_Sensor/sensor/BMP280.c
Normal file
211
apps/earphone/xtell_Sensor/sensor/BMP280.c
Normal file
@ -0,0 +1,211 @@
|
||||
/*
|
||||
气压计
|
||||
*/
|
||||
#include "BMP280.h"
|
||||
#include <string.h>
|
||||
#include "os/os_api.h"
|
||||
#include "gSensor/gSensor_manage.h"
|
||||
|
||||
/*==================================================================================*/
|
||||
/* BMP280 内部定义 */
|
||||
/*==================================================================================*/
|
||||
|
||||
// 存储校准参数的静态全局变量
|
||||
static uint16_t t1;
|
||||
static int16_t t2, t3;
|
||||
static uint16_t p1;
|
||||
static int16_t p2, p3, p4, p5, p6, p7, p8, p9;
|
||||
static int32_t t_fine;
|
||||
|
||||
/*==================================================================================*/
|
||||
/* 封装的底层I2C读写函数 */
|
||||
/*==================================================================================*/
|
||||
|
||||
/**
|
||||
* @brief 写入单个字节到BMP280寄存器
|
||||
*/
|
||||
static uint8_t bmp280_write_reg(uint8_t reg, uint8_t data) {
|
||||
gravity_sensor_command(BMP_IIC_WRITE_ADDRESS, reg, data);
|
||||
return 0;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 从BMP280读取多个字节
|
||||
*/
|
||||
static uint8_t bmp280_read_regs(uint8_t reg, uint8_t *buf, uint16_t len) {
|
||||
return _gravity_sensor_get_ndata(BMP_IIC_READ_ADDRESS, reg, buf, len);
|
||||
}
|
||||
|
||||
/*==================================================================================*/
|
||||
/* 核心算法 */
|
||||
/*==================================================================================*/
|
||||
|
||||
/**
|
||||
* @brief 温度补偿计算
|
||||
* @param adc_T - 原始温度数据
|
||||
* @return 补偿后的温度值 (单位: °C)
|
||||
*/
|
||||
static float compensate_temperature(int32_t adc_T) {
|
||||
float var1, var2, temperature;
|
||||
|
||||
var1 = (((float)adc_T) / 16384.0f - ((float)t1) / 1024.0f) * ((float)t2);
|
||||
var2 = ((((float)adc_T) / 131072.0f - ((float)t1) / 8192.0f) *
|
||||
(((float)adc_T) / 131072.0f - ((float)t1) / 8192.0f)) *
|
||||
((float)t3);
|
||||
t_fine = (int32_t)(var1 + var2);
|
||||
temperature = (var1 + var2) / 5120.0f;
|
||||
|
||||
if (temperature < -40.0f) return -40.0f;
|
||||
if (temperature > 85.0f) return 85.0f;
|
||||
|
||||
return temperature;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 气压补偿计算
|
||||
* @param adc_P - 原始气压数据
|
||||
* @return 补偿后的气压值 (单位: Pa)
|
||||
*/
|
||||
static float compensate_pressure(int32_t adc_P) {
|
||||
float var1, var2, pressure;
|
||||
|
||||
var1 = ((float)t_fine / 2.0f) - 64000.0f;
|
||||
var2 = var1 * var1 * ((float)p6) / 32768.0f;
|
||||
var2 = var2 + var1 * ((float)p5) * 2.0f;
|
||||
var2 = (var2 / 4.0f) + (((float)p4) * 65536.0f);
|
||||
var1 = (((float)p3) * var1 * var1 / 524288.0f + ((float)p2) * var1) / 524288.0f;
|
||||
var1 = (1.0f + var1 / 32768.0f) * ((float)p1);
|
||||
|
||||
if (var1 == 0.0f) {
|
||||
return 0; // 避免除以零
|
||||
}
|
||||
|
||||
pressure = 1048576.0f - (float)adc_P;
|
||||
pressure = (pressure - (var2 / 4096.0f)) * 6250.0f / var1;
|
||||
var1 = ((float)p9) * pressure * pressure / 2147483648.0f;
|
||||
var2 = pressure * ((float)p8) / 32768.0f;
|
||||
pressure = pressure + (var1 + var2 + ((float)p7)) / 16.0f;
|
||||
|
||||
if (pressure < 30000.0f) return 30000.0f;
|
||||
if (pressure > 110000.0f) return 110000.0f;
|
||||
|
||||
return pressure;
|
||||
}
|
||||
|
||||
/*==================================================================================*/
|
||||
/* 外部接口函数实现 */
|
||||
/*==================================================================================*/
|
||||
|
||||
|
||||
|
||||
|
||||
uint8_t bmp280_init(void) {
|
||||
uint8_t id;
|
||||
uint8_t calib_data[24];
|
||||
|
||||
// 1. 检查芯片ID
|
||||
if (bmp280_read_regs(BMP280_REG_ID, &id, 1) == 0) {
|
||||
printf("bmp280 get id error:%d\n",id );
|
||||
return 1; // I2C读取失败
|
||||
}
|
||||
if (id != 0x58) {
|
||||
printf("bmp280 check diff:%d\n",id );
|
||||
return 1; // ID不匹配
|
||||
}
|
||||
printf("bmp280 get id:0%X\n",id );
|
||||
|
||||
// 2. 软复位
|
||||
bmp280_write_reg(BMP280_REG_RESET, 0xB6);
|
||||
os_time_dly(10); // 等待复位完成
|
||||
|
||||
// 3. 一次性读取所有校准参数
|
||||
if (bmp280_read_regs(BMP280_REG_CALIB_START, calib_data, 24) == 0) {
|
||||
return 2; // 读取校准数据失败
|
||||
}
|
||||
|
||||
// 4. 解析校准参数
|
||||
t1 = (uint16_t)(((uint16_t)calib_data[1] << 8) | calib_data[0]);
|
||||
t2 = (int16_t)(((int16_t)calib_data[3] << 8) | calib_data[2]);
|
||||
t3 = (int16_t)(((int16_t)calib_data[5] << 8) | calib_data[4]);
|
||||
p1 = (uint16_t)(((uint16_t)calib_data[7] << 8) | calib_data[6]);
|
||||
p2 = (int16_t)(((int16_t)calib_data[9] << 8) | calib_data[8]);
|
||||
p3 = (int16_t)(((int16_t)calib_data[11] << 8) | calib_data[10]);
|
||||
p4 = (int16_t)(((int16_t)calib_data[13] << 8) | calib_data[12]);
|
||||
p5 = (int16_t)(((int16_t)calib_data[15] << 8) | calib_data[14]);
|
||||
p6 = (int16_t)(((int16_t)calib_data[17] << 8) | calib_data[16]);
|
||||
p7 = (int16_t)(((int16_t)calib_data[19] << 8) | calib_data[18]);
|
||||
p8 = (int16_t)(((int16_t)calib_data[21] << 8) | calib_data[20]);
|
||||
p9 = (int16_t)(((int16_t)calib_data[23] << 8) | calib_data[22]);
|
||||
|
||||
// 5. 配置传感器 (推荐设置: 正常模式，高精度)
|
||||
// t_standby=0.5ms, filter=16, spi_en=0
|
||||
uint8_t config_reg = (0 << 5) | (4 << 2) | (0 << 0);
|
||||
bmp280_write_reg(BMP280_REG_CONFIG, config_reg);
|
||||
|
||||
// osrs_t=x2, osrs_p=x16, mode=normal
|
||||
uint8_t ctrl_meas_reg = (2 << 5) | (5 << 2) | (3 << 0);
|
||||
bmp280_write_reg(BMP280_REG_CTRL_MEAS, ctrl_meas_reg);
|
||||
|
||||
os_time_dly(10); // 等待配置生效
|
||||
|
||||
printf("bmp280 init success\n");
|
||||
return 0; // 初始化成功
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 获取转换后的温度和压力数据
|
||||
*
|
||||
* @param temperature 传出，温度
|
||||
* @param pressure 传出，压力
|
||||
* @return uint8_t
|
||||
*/
|
||||
uint8_t bmp280_read_data(float *temperature, float *pressure) {
|
||||
uint8_t data[6];
|
||||
int32_t adc_P, adc_T;
|
||||
|
||||
// printf("==========debug1===========\n");
|
||||
// 一次性读取6个字节的温度和气压原始数据
|
||||
if (bmp280_read_regs(BMP280_REG_PRESS_MSB, data, 6) == 0) {
|
||||
printf("bmp280:read data error\n");
|
||||
return 1; // 读取失败
|
||||
}
|
||||
|
||||
// printf("==========debug2===========\n");
|
||||
// 组合原始数据 (20位)
|
||||
adc_P = (int32_t)((((uint32_t)(data[0])) << 12) | (((uint32_t)(data[1])) << 4) | (((uint32_t)(data[2])) >> 4));
|
||||
adc_T = (int32_t)((((uint32_t)(data[3])) << 12) | (((uint32_t)(data[4])) << 4) | (((uint32_t)(data[5])) >> 4));
|
||||
|
||||
// 如果没有数据，直接返回错误 (ADC读数为0x80000是未测量状态)
|
||||
if (adc_T == 0x80000 || adc_P == 0x80000) {
|
||||
*temperature = 0.0f;
|
||||
*pressure = 0.0f;
|
||||
printf("bmp280:no data\n");
|
||||
return 1;
|
||||
}
|
||||
|
||||
// printf("==========debug3===========\n");
|
||||
// 进行补偿计算
|
||||
*temperature = compensate_temperature(adc_T);
|
||||
*pressure = compensate_pressure(adc_P);
|
||||
|
||||
return 0; // 成功
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 获取该气压计的原始adc数据
|
||||
*
|
||||
* @param adc_P 传出，气压
|
||||
* @param adc_T 传出，温度
|
||||
*/
|
||||
void bmp280_read_originanl_data(int* adc_P, int* adc_T){
|
||||
uint8_t data[6];
|
||||
// 一次性读取6个字节的温度和气压原始数据
|
||||
if (bmp280_read_regs(BMP280_REG_PRESS_MSB, data, 6) != 0) {
|
||||
return; // 读取失败
|
||||
}
|
||||
|
||||
// 组合原始数据 (20位)
|
||||
adc_P = (int32_t)((((uint32_t)(data[0])) << 12) | (((uint32_t)(data[1])) << 4) | (((uint32_t)(data[2])) >> 4));
|
||||
adc_T = (int32_t)((((uint32_t)(data[3])) << 12) | (((uint32_t)(data[4])) << 4) | (((uint32_t)(data[5])) >> 4));
|
||||
|
||||
}
|
||||
54
apps/earphone/xtell_Sensor/sensor/BMP280.h
Normal file
54
apps/earphone/xtell_Sensor/sensor/BMP280.h
Normal file
@ -0,0 +1,54 @@
|
||||
#ifndef BMP280_DRIVER_H
|
||||
#define BMP280_DRIVER_H
|
||||
|
||||
#include <stdint.h>
|
||||
|
||||
|
||||
#define BMP_PULL_UP 0 //外部是否接的上拉
|
||||
|
||||
// I2C 从设备地址
|
||||
#if BMP_PULL_UP == 1 //外部接的高
|
||||
#define BMP_IIC_7BIT_ADDRESS 0x76 //7位,外部接高为0x77
|
||||
#define BMP_IIC_WRITE_ADDRESS (BMP_IIC_7BIT_ADDRESS<<1) //8位地址
|
||||
#define BMP_IIC_READ_ADDRESS (BMP_IIC_WRITE_ADDRESS | 0x01)
|
||||
#else
|
||||
#define BMP_IIC_7BIT_ADDRESS 0x77 //7位,外部接低为0x76
|
||||
#define BMP_IIC_WRITE_ADDRESS (BMP_IIC_7BIT_ADDRESS<<1) //8位地址
|
||||
#define BMP_IIC_READ_ADDRESS (BMP_IIC_WRITE_ADDRESS | 0x01)
|
||||
#endif
|
||||
|
||||
|
||||
|
||||
// BMP280 寄存器地址
|
||||
#define BMP280_REG_CALIB_START 0x88
|
||||
#define BMP280_REG_ID 0xD0
|
||||
#define BMP280_REG_RESET 0xE0
|
||||
#define BMP280_REG_STATUS 0xF3
|
||||
#define BMP280_REG_CTRL_MEAS 0xF4
|
||||
#define BMP280_REG_CONFIG 0xF5
|
||||
#define BMP280_REG_PRESS_MSB 0xF7
|
||||
|
||||
/**
|
||||
* @brief 初始化BMP280传感器
|
||||
* @return 0: 成功, 1: 芯片ID错误, 2: 读取校准参数失败
|
||||
* @note 此函数会完成ID检查、软复位、读取校准参数，并设置传感器为连续测量模式。
|
||||
*/
|
||||
uint8_t bmp280_init(void);
|
||||
|
||||
/**
|
||||
* @brief 从BMP280读取温度和气压数据
|
||||
* @param[out] temperature - 指向浮点数变量的指针，用于存储温度值 (单位: °C)
|
||||
* @param[out] pressure - 指向浮点数变量的指针，用于存储气压值 (单位: Pa)
|
||||
* @return 0: 成功, 1: 读取数据失败
|
||||
*/
|
||||
uint8_t bmp280_read_data(float *temperature, float *pressure);
|
||||
|
||||
/**
|
||||
* @brief 获取该气压计的原始adc数据
|
||||
*
|
||||
* @param adc_P 传出，气压
|
||||
* @param adc_T 传出，温度
|
||||
*/
|
||||
void bmp280_read_originanl_data(int* adc_P, int* adc_T);
|
||||
|
||||
#endif // BMP280_DRIVER_H
|
||||
283
apps/earphone/xtell_Sensor/sensor/LIS2DH12.c
Normal file
283
apps/earphone/xtell_Sensor/sensor/LIS2DH12.c
Normal file
@ -0,0 +1,283 @@
|
||||
// LIS2DH12驱动 - 由Kilo Code注释
|
||||
#include "gSensor/gSensor_manage.h"
|
||||
#include "app_config.h"
|
||||
#include "math.h"
|
||||
#include "LIS2DH12.h"
|
||||
#include "colorful_lights/colorful_lights.h"
|
||||
#include <string.h> // 用于 memcpy
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////////////////////////////
|
||||
//START -- 宏定义
|
||||
#define ENABLE_XLOG 1
|
||||
#ifdef xlog
|
||||
#undef xlog
|
||||
#endif
|
||||
#if ENABLE_XLOG
|
||||
#define xlog(format, ...) printf("[%s] " format, __func__, ##__VA_ARGS__)
|
||||
#else
|
||||
#define xlog(format, ...) ((void)0)
|
||||
#endif
|
||||
|
||||
// --- 运动检测核心参数 ---
|
||||
#define SAMPLE_COUNT 6 // 定义静止状态检测所需的样本数量
|
||||
#define THRESHOLD 50.00f // 定义静止状态检测的阈值（三轴数据方差），值越大，对微小抖动的容忍度越高
|
||||
#define LPF_ALPHA 0.95f // 低通滤波系数，越接近1，滤波效果越强，重力估算越平滑
|
||||
#define DEADZONE_MSS 0.2f // 加速度死区阈值 (m/s^2)，低于此值的线性加速度被视为噪声并忽略
|
||||
|
||||
// --- 原有业务逻辑宏定义 ---
|
||||
#define STATIC_MAX_TIME 60*5*5 // 传感器静止最大时间，单位 200ms
|
||||
#define DORMANCY_MAX_TIME 60*5 // 休眠检测时间，单位 200ms
|
||||
|
||||
// --- I2C地址定义 ---
|
||||
#define LIS2DH12_W_ADDR 0x32
|
||||
#define LIS2DH12_R_ADDR 0x33
|
||||
|
||||
// --- IIC 寄存器地址宏定义 ---
|
||||
#define LIS2DH12_WHO_AM_I 0x01 //0F
|
||||
#define LIS2DH12_CTRL_REG1 0x20
|
||||
#define LIS2DH12_CTRL_REG4 0x23
|
||||
#define LIS2DH12_CTRL_REG5 0x24
|
||||
#define LIS2DH12_OUT_X_L 0x28
|
||||
#define LIS2DH12_FIFO_CTRL_REG 0x2E
|
||||
#define LIS2DH12_SRC_REG 0x2F
|
||||
|
||||
//END -- 宏定义
|
||||
//////////////////////////////////////////////////////////////////////////////////////////////////
|
||||
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////////////////////////////
|
||||
//START -- 变量定义
|
||||
|
||||
u8 dormancy_flag = 0; // 休眠标识
|
||||
u8 dormancy_ago_moedl = 0; // 记录休眠前灯效
|
||||
u16 gsensor_static_flag; // 记录传感器静止的时间,单位 200ms
|
||||
axis_info_t current_data[32]; // 用于存储从FIFO读取的原始传感器数据
|
||||
|
||||
//运动数据全局变量
|
||||
static motion_data_t motion_data = {{0.0f, 0.0f, 0.0f}, {0.0f, 0.0f, 0.0f}}; // 存储最终计算出的速度和距离
|
||||
static axis_info_xtell gravity_vector = {0.0f, 0.0f, -GRAVITY_EARTH}; // 存储估算出的重力向量，初始假设Z轴朝下
|
||||
static bool sensor_is_stable = false; // 传感器是否静止的标志
|
||||
static axis_info_xtell linear_accel_global = {0.0f, 0.0f, 0.0f}; // 存储移除重力后的线性加速度，用于日志打印
|
||||
static axis_info_xtell zero_g_offset = {0.0f, 0.0f, 0.0f}; // 存储开机校准测得的零点偏移量
|
||||
|
||||
u8 gsensor_alarm;
|
||||
axis_info_xtell gsensor_xtell; // 存储is_sensor_stable计算出的平均值
|
||||
|
||||
|
||||
|
||||
//END -- 变量定义
|
||||
//////////////////////////////////////////////////////////////////////////////////////////////////
|
||||
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////////////////////////////
|
||||
//START -- 函数定义
|
||||
|
||||
|
||||
|
||||
//END -- 函数定义
|
||||
//////////////////////////////////////////////////////////////////////////////////////////////////
|
||||
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////////////////////////////
|
||||
//实现
|
||||
// --- I2C底层函数封装 ---
|
||||
static u32 SL_MEMS_i2cRead(u8 addr, u8 reg, u8 len, u8 *buf) {
|
||||
return _gravity_sensor_get_ndata(addr, reg, buf, len);
|
||||
}
|
||||
static u8 SL_MEMS_i2cWrite(u8 addr, u8 reg, u8 data) {
|
||||
gravity_sensor_command(addr, reg, data);
|
||||
return 0;
|
||||
}
|
||||
|
||||
// 检查传感器ID，确认设备是否正常连接
|
||||
char LIS2DH12_Check() {
|
||||
u8 reg_value = 0;
|
||||
SL_MEMS_i2cRead(LIS2DH12_R_ADDR, LIS2DH12_WHO_AM_I, 1, ®_value);
|
||||
if (reg_value == 0x6A) { //0x33
|
||||
return 0x01;
|
||||
}
|
||||
return 0x00;
|
||||
}
|
||||
|
||||
// 从传感器FIFO读取一批原始数据
|
||||
void LIS2DH12_read_data(axis_info_t *sl_accel) {
|
||||
u8 fifo_src = 0;
|
||||
u8 samples_available = 0;
|
||||
u8 data[192];
|
||||
s16 raw_x,raw_y,raw_z;
|
||||
|
||||
SL_MEMS_i2cRead(LIS2DH12_R_ADDR, LIS2DH12_SRC_REG, 1, &fifo_src);
|
||||
samples_available = fifo_src & 0x1F;
|
||||
if (samples_available == 0) return;
|
||||
|
||||
SL_MEMS_i2cRead(LIS2DH12_R_ADDR, LIS2DH12_OUT_X_L | 0x80, samples_available * 6, data);
|
||||
|
||||
for (u8 i = 0; i < samples_available; i++) {
|
||||
// 数据处理方式与 +/-8g 普通模式(10位) 匹配
|
||||
raw_x = (int16_t)((data[i * 6 + 1] << 8) | data[i * 6]) >> 6;
|
||||
raw_y = (int16_t)((data[i * 6 + 3] << 8) | data[i * 6 + 2]) >> 6;
|
||||
raw_z = (int16_t)((data[i * 6 + 5] << 8) | data[i * 6 + 4]) >> 6;
|
||||
|
||||
sl_accel[i].x = raw_x;
|
||||
sl_accel[i].y = raw_y;
|
||||
sl_accel[i].z = raw_z;
|
||||
}
|
||||
}
|
||||
|
||||
// 开机校准函数：测量传感器的静态零点偏移
|
||||
void LIS2DH12_calibrate() {
|
||||
xlog("开始传感器校准...\n");
|
||||
axis_info_t cal_data[32];
|
||||
long x_sum = 0, y_sum = 0;
|
||||
const int num_samples = 32;
|
||||
|
||||
delay_2ms(100); // 等待约200ms，让FIFO填满数据
|
||||
|
||||
LIS2DH12_read_data(cal_data);
|
||||
|
||||
for (int i = 0; i < num_samples; i++) {
|
||||
x_sum += cal_data[i].x;
|
||||
y_sum += cal_data[i].y;
|
||||
}
|
||||
|
||||
zero_g_offset.x = (float)x_sum / num_samples;
|
||||
zero_g_offset.y = (float)y_sum / num_samples;
|
||||
zero_g_offset.z = 0; // Z轴主要受重力影响，不进行校准
|
||||
|
||||
xlog("校准完成. X轴偏移: %.2f, Y轴偏移: %.2f\n", zero_g_offset.x, zero_g_offset.y);
|
||||
}
|
||||
|
||||
// 初始化并配置LIS2DH12传感器
|
||||
u8 LIS2DH12_Config(void) {
|
||||
if (LIS2DH12_Check() != 1) {
|
||||
xlog("LIS2DH12 I2C error\n");
|
||||
return -1;
|
||||
}
|
||||
|
||||
// 统一配置: 25Hz采样率, +/-8g量程, 普通模式(10位)
|
||||
SL_MEMS_i2cWrite(LIS2DH12_W_ADDR, LIS2DH12_CTRL_REG1, 0x37); // 25 Hz ODR
|
||||
SL_MEMS_i2cWrite(LIS2DH12_W_ADDR, LIS2DH12_CTRL_REG4, 0x20); // +/-8g, BDU enabled
|
||||
SL_MEMS_i2cWrite(LIS2DH12_W_ADDR, LIS2DH12_CTRL_REG5, 0x40); // 使能FIFO
|
||||
SL_MEMS_i2cWrite(LIS2DH12_W_ADDR, LIS2DH12_FIFO_CTRL_REG, 0x80); // 流模式
|
||||
|
||||
// 执行开机校准
|
||||
LIS2DH12_calibrate();
|
||||
|
||||
xlog("LIS2DH12 I2C success\n");
|
||||
return 0;
|
||||
}
|
||||
|
||||
// 判断传感器是否处于静止状态
|
||||
bool is_sensor_stable(axis_info_t *accel_data, int sample_count) {
|
||||
float mean_x = 0, mean_y = 0, mean_z = 0;
|
||||
float variance_x = 0, variance_y = 0, variance_z = 0;
|
||||
|
||||
if (sample_count <= 1) return true;
|
||||
|
||||
// 1. 计算均值
|
||||
for (int i = 0; i < sample_count; i++) {
|
||||
mean_x += accel_data[i].x;
|
||||
mean_y += accel_data[i].y;
|
||||
mean_z += accel_data[i].z;
|
||||
}
|
||||
mean_x /= sample_count;
|
||||
mean_y /= sample_count;
|
||||
mean_z /= sample_count;
|
||||
|
||||
gsensor_xtell.x = mean_x;
|
||||
gsensor_xtell.y = mean_y;
|
||||
gsensor_xtell.z = mean_z;
|
||||
|
||||
// 2. 计算方差
|
||||
for (int i = 0; i < sample_count; i++) {
|
||||
variance_x += (accel_data[i].x - mean_x) * (accel_data[i].x - mean_x);
|
||||
variance_y += (accel_data[i].y - mean_y) * (accel_data[i].y - mean_y);
|
||||
variance_z += (accel_data[i].z - mean_z) * (accel_data[i].z - mean_z);
|
||||
}
|
||||
variance_x /= (sample_count - 1);
|
||||
variance_y /= (sample_count - 1);
|
||||
variance_z /= (sample_count - 1);
|
||||
|
||||
// 3. 如果方差大于阈值，则认为在运动
|
||||
if (variance_x > THRESHOLD || variance_y > THRESHOLD || variance_z > THRESHOLD) {
|
||||
return false;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
// 获取当前的总加速度（包含重力），单位 m/s^2
|
||||
axis_info_xtell get_current_accel_mss(void) {
|
||||
axis_info_xtell accel_mss;
|
||||
// 灵敏度 @ +/-8g 普通模式 (10-bit) = 12 mg/LSB
|
||||
const float sensitivity_g_per_lsb = 0.012f;
|
||||
|
||||
// 在转换前，先减去校准测得的零点偏移
|
||||
accel_mss.x = ((float)gsensor_xtell.x - zero_g_offset.x) * sensitivity_g_per_lsb * GRAVITY_EARTH;
|
||||
accel_mss.y = ((float)gsensor_xtell.y - zero_g_offset.y) * sensitivity_g_per_lsb * GRAVITY_EARTH;
|
||||
accel_mss.z = (float)gsensor_xtell.z * sensitivity_g_per_lsb * GRAVITY_EARTH;
|
||||
return accel_mss;
|
||||
}
|
||||
|
||||
// 获取计算好的运动数据（速度和距离）
|
||||
void get_motion_data(motion_data_t *data) {
|
||||
if (data) {
|
||||
memcpy(data, &motion_data, sizeof(motion_data_t));
|
||||
}
|
||||
}
|
||||
|
||||
// 获取移除重力后的线性加速度
|
||||
axis_info_xtell get_linear_accel_mss(void) {
|
||||
return linear_accel_global;
|
||||
}
|
||||
|
||||
// 核心计算任务，由定时器周期性调用
|
||||
void xtell_i2c_test() {
|
||||
// 1. 读取一批最新的传感器数据
|
||||
LIS2DH12_read_data(current_data);
|
||||
// 2. 判断传感器当前是否静止
|
||||
sensor_is_stable = is_sensor_stable(current_data, SAMPLE_COUNT);
|
||||
|
||||
// 3. 获取校准和转换后的总加速度 (m/s^2)
|
||||
axis_info_xtell current_accel_mss = get_current_accel_mss();
|
||||
|
||||
// 4. 使用低通滤波器估算重力向量
|
||||
gravity_vector.x = LPF_ALPHA * gravity_vector.x + (1.0f - LPF_ALPHA) * current_accel_mss.x;
|
||||
gravity_vector.y = LPF_ALPHA * gravity_vector.y + (1.0f - LPF_ALPHA) * current_accel_mss.y;
|
||||
gravity_vector.z = LPF_ALPHA * gravity_vector.z + (1.0f - LPF_ALPHA) * current_accel_mss.z;
|
||||
|
||||
// 5. 从总加速度中减去重力，得到线性加速度
|
||||
linear_accel_global.x = current_accel_mss.x - gravity_vector.x;
|
||||
linear_accel_global.y = current_accel_mss.y - gravity_vector.y;
|
||||
linear_accel_global.z = current_accel_mss.z - gravity_vector.z;
|
||||
|
||||
// 6. 应用死区：忽略过小的加速度值（噪声）
|
||||
if (fabsf(linear_accel_global.x) < DEADZONE_MSS) linear_accel_global.x = 0.0f;
|
||||
if (fabsf(linear_accel_global.y) < DEADZONE_MSS) linear_accel_global.y = 0.0f;
|
||||
if (fabsf(linear_accel_global.z) < DEADZONE_MSS) linear_accel_global.z = 0.0f;
|
||||
|
||||
// 7. 积分线性加速度，得到速度
|
||||
motion_data.velocity.x += linear_accel_global.x * SAMPLING_PERIOD_S;
|
||||
motion_data.velocity.y += linear_accel_global.y * SAMPLING_PERIOD_S;
|
||||
motion_data.velocity.z += linear_accel_global.z * SAMPLING_PERIOD_S;
|
||||
|
||||
// 8. 如果传感器静止，重置速度和距离以消除漂移
|
||||
if (sensor_is_stable) {
|
||||
motion_data.velocity.x = 0.0f;
|
||||
motion_data.velocity.y = 0.0f;
|
||||
motion_data.velocity.z = 0.0f;
|
||||
motion_data.distance.x = 0.0f;
|
||||
motion_data.distance.y = 0.0f;
|
||||
motion_data.distance.z = 0.0f;
|
||||
}
|
||||
|
||||
// 9. 积分速度，得到距离
|
||||
motion_data.distance.x += motion_data.velocity.x * SAMPLING_PERIOD_S;
|
||||
motion_data.distance.y += motion_data.velocity.y * SAMPLING_PERIOD_S;
|
||||
motion_data.distance.z += motion_data.velocity.z * SAMPLING_PERIOD_S;
|
||||
|
||||
// 10. 计算并打印总的移动距离（可选，用于调试）
|
||||
float total_distance_magnitude = sqrtf(motion_data.distance.x * motion_data.distance.x +
|
||||
motion_data.distance.y * motion_data.distance.y +
|
||||
motion_data.distance.z * motion_data.distance.z);
|
||||
// xlog("Total distance traveled: %.2f m\n", total_distance_magnitude);
|
||||
}
|
||||
59
apps/earphone/xtell_Sensor/sensor/LIS2DH12.h
Normal file
59
apps/earphone/xtell_Sensor/sensor/LIS2DH12.h
Normal file
@ -0,0 +1,59 @@
|
||||
#ifndef LIS2DH12_H
|
||||
#define LIS2DH12_H
|
||||
#include "gSensor/gSensor_manage.h"
|
||||
#include "le_rcsp_adv_module.h"
|
||||
|
||||
// --- 物理常量定义 ---
|
||||
#define GRAVITY_EARTH 9.80665f // 地球重力加速度 (m/s^2)
|
||||
#define SAMPLING_PERIOD_S 0.2f // 采样周期 (对应于200ms的定时器)
|
||||
|
||||
// --- 数据结构定义 ---
|
||||
|
||||
// 三轴数据结构体 (可用于加速度、速度、距离)
|
||||
typedef struct {
|
||||
float x;
|
||||
float y;
|
||||
float z;
|
||||
} axis_info_xtell;
|
||||
|
||||
// 运动数据结构体，包含速度和距离
|
||||
typedef struct {
|
||||
axis_info_xtell velocity; // 速度 (m/s)
|
||||
axis_info_xtell distance; // 距离 (m)
|
||||
} motion_data_t;
|
||||
|
||||
// --- API 函数声明 ---
|
||||
|
||||
/**
|
||||
* @brief 初始化并配置LIS2DH12传感器
|
||||
* @return 0 表示成功, -1 表示失败
|
||||
*/
|
||||
unsigned char LIS2DH12_Config(void);
|
||||
|
||||
/**
|
||||
* @brief 核心计算任务，应由定时器周期性调用
|
||||
*/
|
||||
void xtell_i2c_test(void);
|
||||
|
||||
|
||||
// --- 数据获取函数声明 ---
|
||||
|
||||
/**
|
||||
* @brief 获取计算好的运动数据（速度和距离）
|
||||
* @param data 指向 motion_data_t 结构体的指针，用于存放结果
|
||||
*/
|
||||
void get_motion_data(motion_data_t *data);
|
||||
|
||||
/**
|
||||
* @brief 获取当前的总加速度（包含重力），单位 m/s^2
|
||||
* @return axis_info_xtell 包含x,y,z轴总加速度的结构体
|
||||
*/
|
||||
axis_info_xtell get_current_accel_mss(void);
|
||||
|
||||
/**
|
||||
* @brief 获取当前移除重力后的线性加速度，单位 m/s^2
|
||||
* @return axis_info_xtell 包含x,y,z轴线性加速度的结构体
|
||||
*/
|
||||
axis_info_xtell get_linear_accel_mss(void);
|
||||
|
||||
#endif
|
||||
259
apps/earphone/xtell_Sensor/sensor/MMC56.c
Normal file
259
apps/earphone/xtell_Sensor/sensor/MMC56.c
Normal file
@ -0,0 +1,259 @@
|
||||
|
||||
#include "MMC56.h"
|
||||
#include "math.h"
|
||||
#include "os/os_api.h"
|
||||
#include "../xtell.h"
|
||||
#include "gSensor/gSensor_manage.h"
|
||||
#include "printf.h"
|
||||
|
||||
#define CALIBRATION_TIME 20000 //校准持续时间 ms
|
||||
#define SAMPLE_INTERVAL 100 //校准采样间隔
|
||||
|
||||
// 用于跟踪当前是否处于连续测量模式
|
||||
static uint8_t g_continuous_mode_enabled = 0;
|
||||
mmc5603nj_cal_data_t cal_data; //校准数据
|
||||
|
||||
static void mmc5603nj_write_reg(uint8_t reg, uint8_t data) {
|
||||
gravity_sensor_command(MMC_IIC_WRITE_ADDRESS, reg, data);
|
||||
}
|
||||
static uint32_t mmc5603nj_read_regs(uint8_t reg, uint8_t *buf, uint8_t len) {
|
||||
return _gravity_sensor_get_ndata(MMC_IIC_READ_ADDRESS, reg, buf, len);
|
||||
}
|
||||
|
||||
// 外部接口函数实现
|
||||
|
||||
uint8_t mmc5603nj_get_pid(void) {
|
||||
uint8_t pid = 0;
|
||||
mmc5603nj_read_regs(MMC_PID, &pid, 1);
|
||||
return pid;
|
||||
}
|
||||
|
||||
int mmc5603nj_init(void) {
|
||||
// ID
|
||||
if ( mmc5603nj_get_pid() != 0x10) {
|
||||
printf("MMC5603NJ init failed: wrong Product ID (read: 0x%X)\n", mmc5603nj_get_pid());
|
||||
// return 0;
|
||||
}
|
||||
|
||||
// 软件复位
|
||||
mmc5603nj_write_reg(MMC_INCTRL1, 0x80); // SW_RESET bit
|
||||
os_time_dly(20); // 等待复位完成
|
||||
|
||||
// 设置20位分辨率 (BW[1:0] = 11)
|
||||
// 同时确保所有轴都使能 (X/Y/Z_inhibit = 0)
|
||||
mmc5603nj_write_reg(MMC_INCTRL1, 0x03);
|
||||
os_time_dly(1);
|
||||
|
||||
// 设置内部控制寄存器2
|
||||
// CMM_EN = 1 (使能连续模式功能)
|
||||
// HPOWER = 1 (高功耗模式，更稳定)
|
||||
mmc5603nj_write_reg(MMC_INCTRL2, 0x90); // 0b10010000
|
||||
|
||||
// 设置自动SET/RESET功能
|
||||
// AUTO_SR_EN = 1
|
||||
mmc5603nj_write_reg(MMC_INCTRL0, 0x20); // 0b00100000
|
||||
|
||||
g_continuous_mode_enabled = 0;
|
||||
printf("MMC5603NJ initialized successfully.\n");
|
||||
|
||||
mmc5603nj_enable_continuous_mode(0x04);
|
||||
|
||||
return 1;
|
||||
}
|
||||
|
||||
void mmc5603nj_start_calibration(void){
|
||||
printf("\n--- Magnetometer Calibration Start ---\n");
|
||||
printf("Slowly rotate the device in all directions (like drawing a 3D '8')...\n");
|
||||
printf("Calibration will last for 20 seconds.\n\n");
|
||||
printf("will start after 5 seconds\n\n");
|
||||
os_time_dly(500);
|
||||
|
||||
// 初始化最大最小值
|
||||
// 使用一个临时变量来读取数据，避免干扰read函数的正常逻辑
|
||||
mmc5603nj_mag_data_t temp_mag_data;
|
||||
// 首次读取以获取初始值
|
||||
mmc5603nj_read_mag_data(&temp_mag_data); // 首次读取不应用校准
|
||||
|
||||
float max_x = temp_mag_data.x;
|
||||
float min_x = temp_mag_data.x;
|
||||
float max_y = temp_mag_data.y;
|
||||
float min_y = temp_mag_data.y;
|
||||
float max_z = temp_mag_data.z;
|
||||
float min_z = temp_mag_data.z;
|
||||
|
||||
uint32_t start_time = os_time_get(); // 假设os_time_get()返回毫秒级时间戳
|
||||
int samples = 0;
|
||||
int over = CALIBRATION_TIME/SAMPLE_INTERVAL;
|
||||
|
||||
while (samples <= over) {
|
||||
// 读取原始磁力计数据
|
||||
mmc5603nj_read_mag_data(&temp_mag_data);
|
||||
|
||||
// 更新最大最小值
|
||||
if (temp_mag_data.x > max_x) max_x = temp_mag_data.x;
|
||||
if (temp_mag_data.x < min_x) min_x = temp_mag_data.x;
|
||||
|
||||
if (temp_mag_data.y > max_y) max_y = temp_mag_data.y;
|
||||
if (temp_mag_data.y < min_y) min_y = temp_mag_data.y;
|
||||
|
||||
if (temp_mag_data.z > max_z) max_z = temp_mag_data.z;
|
||||
if (temp_mag_data.z < min_z) min_z = temp_mag_data.z;
|
||||
|
||||
samples++;
|
||||
os_time_dly(SAMPLE_INTERVAL / 10);
|
||||
}
|
||||
|
||||
// 检查数据范围是否合理，防止传感器未动或故障
|
||||
if ((max_x - min_x < 0.1f) || (max_y - min_y < 0.1f) || (max_z - min_z < 0.1f)) {
|
||||
printf("\n--- Calibration Failed ---\n");
|
||||
printf("Device might not have been rotated enough.\n");
|
||||
printf("X range: %.2f, Y range: %.2f, Z range: %.2f\n", max_x - min_x, max_y - min_y, max_z - min_z);
|
||||
return;
|
||||
}
|
||||
|
||||
// 计算硬磁偏移 (椭球中心)
|
||||
cal_data.offset_x = (max_x + min_x) / 2.0f;
|
||||
cal_data.offset_y = (max_y + min_y) / 2.0f;
|
||||
cal_data.offset_z = (max_z + min_z) / 2.0f;
|
||||
|
||||
printf("\n--- Calibration Complete ---\n");
|
||||
printf("Collected %d samples.\n", samples);
|
||||
printf("Offsets (Gauss):\n");
|
||||
printf(" X: %.4f\n", cal_data.offset_x);
|
||||
printf(" Y: %.4f\n", cal_data.offset_y);
|
||||
printf(" Z: %.4f\n", cal_data.offset_z);
|
||||
printf("Please save these values and apply them in your code.\n\n");
|
||||
}
|
||||
|
||||
|
||||
|
||||
void mmc5603nj_enable_continuous_mode(uint8_t rate) {
|
||||
// 在连续模式下，ODR寄存器必须被设置
|
||||
mmc5603nj_write_reg(MMC_ODR, rate); //要设置频率
|
||||
// mmc5603nj_set_data_rate(0x04);
|
||||
|
||||
// 启用连续模式 (INCTRL2的CMM_EN位已在init中设置)
|
||||
// 只需要设置 INCTRL0 的 CMM_FREQ_EN 位
|
||||
mmc5603nj_write_reg(MMC_INCTRL0, 0xA0); // 0b10100000 (CMM_FREQ_EN=1, AUTO_SR_EN=1)
|
||||
g_continuous_mode_enabled = 1;
|
||||
}
|
||||
|
||||
void mmc5603nj_disable_continuous_mode(void) {
|
||||
// 禁用连续模式
|
||||
mmc5603nj_write_reg(MMC_INCTRL0, 0x20); // 恢复到仅使能 AUTO_SR_EN 的状态
|
||||
g_continuous_mode_enabled = 0;
|
||||
}
|
||||
|
||||
float mmc5603nj_get_temperature(void) {
|
||||
uint8_t status = 0;
|
||||
uint8_t temp_raw = 0;
|
||||
uint8_t timeout = 20;
|
||||
|
||||
// 触发一次温度测量
|
||||
mmc5603nj_write_reg(MMC_INCTRL0, 0x02); // TAKE_MEAS_T
|
||||
|
||||
// 等待测量完成
|
||||
do {
|
||||
os_time_dly(10);
|
||||
mmc5603nj_read_regs(MMC_STATUS1, &status, 1);
|
||||
timeout--;
|
||||
} while ((status & 0x80) == 0 && timeout > 0);
|
||||
|
||||
if (timeout == 0) {
|
||||
printf("Error: Temperature measurement timeout!\n");
|
||||
return -273.15f; // 返回一个绝对零度的错误值
|
||||
}
|
||||
|
||||
mmc5603nj_read_regs(MMC_TOUT, &temp_raw, 1);
|
||||
return ((float)temp_raw * 0.8f) - 75.0f;
|
||||
}
|
||||
|
||||
void mmc5603nj_read_mag_data(mmc5603nj_mag_data_t *mag_data) {
|
||||
uint8_t buffer[9];
|
||||
|
||||
if (g_continuous_mode_enabled) {
|
||||
// 连续模式下，只需检查数据是否就绪
|
||||
uint8_t status = 0;
|
||||
mmc5603nj_read_regs(MMC_STATUS1, &status, 1);
|
||||
if ((status & 0x40) == 0) { // Meas_M_done bit
|
||||
// 数据未就绪，可以选择返回或等待，这里我们直接返回旧数据
|
||||
return;
|
||||
}
|
||||
} else {
|
||||
// 单次测量模式
|
||||
uint8_t status = 0;
|
||||
uint8_t timeout = 20;
|
||||
|
||||
// 触发一次带自动SET/RESET的磁场测量
|
||||
mmc5603nj_write_reg(MMC_INCTRL0, 0x21); // 0b00100001 (TAKE_MEAS_M=1, AUTO_SR_EN=1)
|
||||
|
||||
// 等待测量完成
|
||||
do {
|
||||
os_time_dly(10);
|
||||
mmc5603nj_read_regs(MMC_STATUS1, &status, 1);
|
||||
timeout--;
|
||||
} while ((status & 0x40) == 0 && timeout > 0);
|
||||
|
||||
if (timeout == 0) {
|
||||
printf("Error: Magnetic measurement timeout!\n");
|
||||
mag_data->x = mag_data->y = mag_data->z = 0.0f;
|
||||
return;
|
||||
}
|
||||
}
|
||||
|
||||
// 读取9个字节的原始数据
|
||||
mmc5603nj_read_regs(MMC_XOUT0, buffer, 9);
|
||||
|
||||
// 解析数据 (20位分辨率)
|
||||
int32_t raw_x = ((uint32_t)buffer[0] << 12) | ((uint32_t)buffer[1] << 4) | ((uint32_t)buffer[6] & 0x0F);
|
||||
int32_t raw_y = ((uint32_t)buffer[2] << 12) | ((uint32_t)buffer[3] << 4) | ((uint32_t)buffer[6] >> 4);
|
||||
int32_t raw_z = ((uint32_t)buffer[4] << 12) | ((uint32_t)buffer[5] << 4) | ((uint32_t)buffer[8] & 0x0F);
|
||||
|
||||
// 应用偏置和灵敏度进行转换
|
||||
mag_data->x = ((float)raw_x - 524288.0f) / 16384.0f;
|
||||
mag_data->y = ((float)raw_y - 524288.0f) / 16384.0f;
|
||||
mag_data->z = ((float)raw_z - 524288.0f) / 16384.0f;
|
||||
|
||||
//减去偏移
|
||||
mag_data->x -= cal_data.offset_x;
|
||||
mag_data->y -= cal_data.offset_y;
|
||||
mag_data->z -= cal_data.offset_z;
|
||||
}
|
||||
|
||||
|
||||
void mmc5603nj_read_origin_data(uint8_t *buffer) {
|
||||
|
||||
if (g_continuous_mode_enabled) {
|
||||
// 连续模式下，只需检查数据是否就绪
|
||||
uint8_t status = 0;
|
||||
mmc5603nj_read_regs(MMC_STATUS1, &status, 1);
|
||||
if ((status & 0x40) == 0) { // Meas_M_done bit
|
||||
// 数据未就绪，可以选择返回或等待，这里我们直接返回旧数据
|
||||
return;
|
||||
}
|
||||
} else {
|
||||
// 单次测量模式
|
||||
uint8_t status = 0;
|
||||
uint8_t timeout = 20;
|
||||
|
||||
// 触发一次带自动SET/RESET的磁场测量
|
||||
mmc5603nj_write_reg(MMC_INCTRL0, 0x21); // 0b00100001 (TAKE_MEAS_M=1, AUTO_SR_EN=1)
|
||||
|
||||
// 等待测量完成
|
||||
do {
|
||||
os_time_dly(10);
|
||||
mmc5603nj_read_regs(MMC_STATUS1, &status, 1);
|
||||
timeout--;
|
||||
} while ((status & 0x40) == 0 && timeout > 0);
|
||||
|
||||
if (timeout == 0) {
|
||||
printf("Error: Magnetic measurement timeout!\n");
|
||||
return;
|
||||
}
|
||||
}
|
||||
|
||||
// 读取9个字节的原始数据
|
||||
mmc5603nj_read_regs(MMC_XOUT0, buffer, 9);
|
||||
|
||||
|
||||
}
|
||||
103
apps/earphone/xtell_Sensor/sensor/MMC56.h
Normal file
103
apps/earphone/xtell_Sensor/sensor/MMC56.h
Normal file
@ -0,0 +1,103 @@
|
||||
#ifndef MMC5603NJ_DRIVER_H
|
||||
#define MMC5603NJ_DRIVER_H
|
||||
|
||||
#include <stdint.h>
|
||||
|
||||
|
||||
//该芯片的iic地址是固定的, 没法通过外部上下拉来改变
|
||||
#define BMP_IIC_7BIT_ADDRESS 0x30 //0110000 手册第12页
|
||||
//8位地址:
|
||||
#define MMC_IIC_WRITE_ADDRESS (BMP_IIC_7BIT_ADDRESS <<1) // 0x60 : 01100000
|
||||
#define MMC_IIC_READ_ADDRESS (MMC_IIC_WRITE_ADDRESS | 0x01) // 0x61 : 01100001
|
||||
|
||||
|
||||
|
||||
// 寄存器地址定义 -- 数据手册第6页
|
||||
#define MMC_XOUT0 0x00
|
||||
#define MMC_XOUT1 0x01
|
||||
#define MMC_YOUT0 0x02
|
||||
#define MMC_YOUT1 0x03
|
||||
#define MMC_ZOUT0 0x04
|
||||
#define MMC_ZOUT1 0x05
|
||||
#define MMC_XOUT2 0x06
|
||||
#define MMC_YOUT2 0x07
|
||||
#define MMC_ZOUT2 0x08
|
||||
#define MMC_TOUT 0x09
|
||||
#define MMC_STATUS1 0x18
|
||||
#define MMC_ODR 0x1A
|
||||
#define MMC_INCTRL0 0x1B
|
||||
#define MMC_INCTRL1 0x1C
|
||||
#define MMC_INCTRL2 0x1D
|
||||
#define MMC_ST_X_TH 0x1E
|
||||
#define MMC_ST_Y_TH 0x1F
|
||||
#define MMC_ST_Z_TH 0x20
|
||||
#define MMC_ST_X 0x27
|
||||
#define MMC_ST_Y 0x28
|
||||
#define MMC_ST_Z 0x29
|
||||
#define MMC_PID 0x39
|
||||
|
||||
// 定义一个结构体来存放三轴磁场数据（原始数据）
|
||||
typedef struct {
|
||||
float x;
|
||||
float y;
|
||||
float z;
|
||||
} mmc5603nj_original_data_t;
|
||||
|
||||
|
||||
// 定义一个结构体来存放三轴磁场数据（单位：高斯 Gauss）
|
||||
typedef struct {
|
||||
float x;
|
||||
float y;
|
||||
float z;
|
||||
} mmc5603nj_mag_data_t;
|
||||
|
||||
// 定义一个结构体来存放磁力计的硬磁偏移校准数据
|
||||
typedef struct {
|
||||
float offset_x;
|
||||
float offset_y;
|
||||
float offset_z;
|
||||
} mmc5603nj_cal_data_t;
|
||||
|
||||
/**
|
||||
* @brief 初始化MMC5603NJ传感器
|
||||
* 该函数会对传感器进行软件复位，并检查设备ID。
|
||||
* @return 0 表示成功, -1 表示失败 (设备ID不匹配).
|
||||
*/
|
||||
int mmc5603nj_init(void);
|
||||
|
||||
/**
|
||||
* @brief 设置传感器的数据输出速率 (ODR - Output Data Rate)
|
||||
* @param rate 速率值，具体含义请参考datasheet ODR寄存器说明。
|
||||
*/
|
||||
void mmc5603nj_set_data_rate(uint8_t rate);
|
||||
|
||||
/**
|
||||
* @brief 启用连续测量模式
|
||||
*/
|
||||
void mmc5603nj_enable_continuous_mode(uint8_t rate);
|
||||
|
||||
/**
|
||||
* @brief 禁用连续测量模式
|
||||
*/
|
||||
void mmc5603nj_disable_continuous_mode(void);
|
||||
|
||||
/**
|
||||
* @brief 获取产品ID
|
||||
* @return 产品的ID值，对于MMC5603NJ，应为0x10.
|
||||
*/
|
||||
uint8_t mmc5603nj_get_pid(void);
|
||||
|
||||
/**
|
||||
* @brief 读取传感器的温度
|
||||
* @return 温度值 (单位: 摄氏度 °C).
|
||||
*/
|
||||
float mmc5603nj_get_temperature(void);
|
||||
|
||||
/**
|
||||
* @brief 读取三轴磁场数据
|
||||
* 此函数会根据当前是连续模式还是单次模式来读取数据。
|
||||
* @param mag_data 指向 mmc5603nj_mag_data_t 结构体的指针，用于存放结果。
|
||||
*/
|
||||
void mmc5603nj_read_mag_data(mmc5603nj_mag_data_t *mag_data);
|
||||
|
||||
#endif // MMC5603NJ_DRIVER_H
|
||||
1983
apps/earphone/xtell_Sensor/sensor/SC7U22.c
Normal file
1983
apps/earphone/xtell_Sensor/sensor/SC7U22.c
Normal file
File diff suppressed because it is too large
Load Diff
143
apps/earphone/xtell_Sensor/sensor/SC7U22.h
Normal file
143
apps/earphone/xtell_Sensor/sensor/SC7U22.h
Normal file
@ -0,0 +1,143 @@
|
||||
/**************************************************
|
||||
Copyright (c) 2022 Silan MEMS. All Rights Reserved.
|
||||
@Silan MEMS Sensor Product Line
|
||||
@Code Author:Zhou Min
|
||||
**************************************************/
|
||||
|
||||
#ifndef __SCU722_H__
|
||||
#define __SCU722_H__
|
||||
|
||||
#include "gSensor/gSensor_manage.h"
|
||||
#include "printf.h"
|
||||
#include "MMC56.h"
|
||||
|
||||
//是否使能串口打印调试
|
||||
#define SL_Sensor_Algo_Release_Enable 0x00
|
||||
//是否开启FIFO模式，默认STREAM模式
|
||||
#define SL_SC7U22_FIFO_ENABLE 0x00
|
||||
|
||||
|
||||
/***使用前请根据实际情况配置以下参数******/
|
||||
/**SC7U22的SDO 接地: 0****************/
|
||||
/**SC7U22的SDO 接电源:1****************/
|
||||
#define SL_SC7U22_SDO_VDD_GND 0
|
||||
/*****************************************/
|
||||
/***使用前请根据实际IIC地址配置参数***/
|
||||
/**SC7U22的IIC 接口地址为 7bits: 0****/
|
||||
/**SC7U22的IIC 接口地址为 8bits: 1****/
|
||||
#define SL_SC7U22_IIC_7BITS_8BITS 1
|
||||
/*****************************************/
|
||||
#if SL_SC7U22_SDO_VDD_GND==0
|
||||
#define SL_SC7U22_IIC_7BITS_ADDR 0x18
|
||||
#define SL_SC7U22_IIC_8BITS_WRITE_ADDR 0x30
|
||||
#define SL_SC7U22_IIC_8BITS_READ_ADDR 0x31
|
||||
#else
|
||||
#define SL_SC7U22_IIC_7BITS_ADDR 0x19
|
||||
#define SL_SC7U22_IIC_8BITS_WRITE_ADDR 0x32
|
||||
#define SL_SC7U22_IIC_8BITS_READ_ADDR 0x33
|
||||
#endif
|
||||
#if SL_SC7U22_IIC_7BITS_8BITS==0
|
||||
#define SL_SC7U22_IIC_ADDRESS SL_SC7U22_IIC_7BITS_ADDR
|
||||
#else
|
||||
#define SL_SC7U22_IIC_WRITE_ADDRESS SL_SC7U22_IIC_8BITS_WRITE_ADDR
|
||||
#define SL_SC7U22_IIC_READ_ADDRESS SL_SC7U22_IIC_8BITS_READ_ADDR
|
||||
#endif
|
||||
|
||||
unsigned char SL_SC7U22_I2c_Spi_Write(unsigned char sl_spi_iic, unsigned char reg, unsigned char dat);
|
||||
unsigned char SL_SC7U22_I2c_Spi_Read(unsigned char sl_spi_iic, unsigned char reg, unsigned short len, unsigned char* buf);
|
||||
|
||||
/*************I2C通信检测函数******************/
|
||||
unsigned char SL_SC7U22_Check(void);
|
||||
/*************函数返回值*****************/
|
||||
/**return : 1 IIC通信正常,IC正常**************/
|
||||
/**return : 0 IIC通信异常,IC异常**********/
|
||||
|
||||
/*************传感器初始化函数*******************/
|
||||
unsigned char SL_SC7U22_Config(void);
|
||||
/*************函数返回值*****************/
|
||||
/**return : 1 IIC通信正常,IC正常*************/
|
||||
/**return : 0; IIC通信异常,IC异常*********/
|
||||
|
||||
/*************SC7U22 Sensor Time**************/
|
||||
unsigned int SL_SC7U22_TimeStamp_Read(void);
|
||||
/*************函数返回值*****************/
|
||||
/**return : 内部传感器时间***************/
|
||||
|
||||
#if SL_SC7U22_FIFO_ENABLE ==0x00
|
||||
/******实时读取数据寄存器数据，相当于从400Hz的FIFO中取出数据******/
|
||||
void SL_SC7U22_RawData_Read(signed short* acc_data_buf, signed short* gyr_data_buf);
|
||||
/************* 输入XYZ三轴数据存放的地址*****************/
|
||||
/************* *acc_data_buf: ACC数据***********************/
|
||||
/************* *gyr_data_buf: GYR数据***********************/
|
||||
|
||||
#else
|
||||
/******实时读取数据寄存器FIFO数据******/
|
||||
unsigned short SL_SC7U22_FIFO_Read(signed short* accx_buf, signed short* accy_buf, signed short* accz_buf, signed short* gyrx_buf, signed short* gyry_buf, signed short* gyrz_buf);
|
||||
/*************输入XYZ三轴数据首地址**************************/
|
||||
/*************accx_buf[0]: ACC_X的第一个数据**************/
|
||||
/*************accy_buf[0]: ACC_Y的第一个数据**************/
|
||||
/*************accz_buf[0]: ACC_Z的第一个数据**************/
|
||||
/*************gyrx_buf[0]: GYR_X的第一个数据**************/
|
||||
/*************gyry_buf[0]: GYR_Y的第一个数据**************/
|
||||
/*************gyrz_buf[0]: GYR_Z的第一个数据**************/
|
||||
/****************函数返回值****************************/
|
||||
/**return : len 表示数组长度*************************/
|
||||
#endif
|
||||
|
||||
/*********进入传感器关闭模式*************/
|
||||
unsigned char SL_SC7U22_POWER_DOWN(void);
|
||||
/**0: 关闭模式失败***********************/
|
||||
/**1: 关闭模式成功***********************/
|
||||
|
||||
/*********SC7U22 RESET***************/
|
||||
unsigned char SL_SC7U22_SOFT_RESET(void);
|
||||
/**0: 成功*****************************/
|
||||
/**1: 失败**************************/
|
||||
|
||||
/*************GSensor and GyroSensor开启和关闭函数*********/
|
||||
unsigned char SL_SC7U22_Open_Close_SET(unsigned char acc_enable,unsigned char gyro_enable);
|
||||
/**acc_enable: 0=关闭ACC Sensor; 1=开启ACC Sensor*********/
|
||||
/**gyro_enable: 0=关闭GYRO Sensor; 1=开启GYRO Sensor*******/
|
||||
/**return: 0=设置失败，1=设置成功**************************/
|
||||
|
||||
/*********进入睡眠模式并开启中断函数*************/
|
||||
unsigned char SL_SC7U22_IN_SLEEP_SET(unsigned char acc_odr,unsigned char vth,unsigned char tth,unsigned char int_io);
|
||||
/**acc_odr: 12/25/50**************************************/
|
||||
/**vth: 运动检测，阈值参数****************************/
|
||||
/**tth: 运动检测，持续时间阈值，小于该时间则过滤**********/
|
||||
/**int_io: 1=INT1, 2=INT2*********************************/
|
||||
/**return: 0=设置失败，1=设置成功**************************/
|
||||
|
||||
/*********进入唤醒模式，设置参数并关闭中断函数***********/
|
||||
unsigned char SL_SC7U22_WakeUp_SET(unsigned char odr_mode,unsigned char acc_range,unsigned char acc_hp_en,unsigned short gyro_range,unsigned char gyro_hp_en);
|
||||
/**odr_mode: 25HZ/50Hz/100Hz/200Hz ACC+GYRO***************/
|
||||
/**acc_range: ±2G/±4G/±8G/±16G*****************************/
|
||||
/**acc_hp_en: 0=关闭高性能模式;1=开启*****/
|
||||
/**gyro_range: ±125dps/±250dps/±500dps/±1000dps/±2000dps***/
|
||||
/**gyro_hp_en: 0=关闭高性能模式;1=开启高性能模式; ********/
|
||||
/**return: 0=设置失败，1=设置成功**************************/
|
||||
|
||||
/*********SC7U22 Angle Cauculate***************/
|
||||
unsigned char SL_SC7U22_Angle_Output(unsigned char calibration_en,signed short *acc_gyro_input,float *Angle_output, unsigned char yaw_rst);
|
||||
/**in calibration_en: 1=enable 0=disable***********************/
|
||||
/**in/out acc_gyro_input[0]: ACC-X*****************************/
|
||||
/**in/out acc_gyro_input[1]: ACC-Y*****************************/
|
||||
/**in/out acc_gyro_input[2]: ACC-Z*****************************/
|
||||
/**in/out acc_gyro_input[3]: GYR-X*****************************/
|
||||
/**in/out acc_gyro_input[4]: GYR-Y*****************************/
|
||||
/**in/out acc_gyro_input[5]: GYR-Z*****************************/
|
||||
/**output Angle_output[0]: Pitch*****************************/
|
||||
/**output Angle_output[1]: Roll******************************/
|
||||
/**output Angle_output[2]: Yaw*******************************/
|
||||
/**input yaw_rst: reset yaw value***************************/
|
||||
|
||||
void set_SC7U22_Error_Flag(char flag);
|
||||
unsigned char Original_SL_SC7U22_Angle_Output(unsigned char calibration_en, signed short *acc_gyro_input, float *Angle_output, unsigned char yaw_rst);
|
||||
unsigned char SIX_SL_SC7U22_Angle_Output(unsigned char auto_calib_start, signed short *acc_gyro_input, float *Angle_output, unsigned char yaw_rst);
|
||||
unsigned char Q_SL_SC7U22_Angle_Output(unsigned char calibration_en, signed short *acc_gyro_input, float *Angle_output, const mmc5603nj_mag_data_t *mag_data_input, unsigned char yaw_rst, float *quaternion_output);
|
||||
unsigned char get_calibration_state(void);
|
||||
/**寄存器宏定义*******************************/
|
||||
#define SC7U22_WHO_AM_I 0x01
|
||||
|
||||
|
||||
#endif // __SCU722_H__
|
||||
927
apps/earphone/xtell_Sensor/sensor/SC7U22_Ma.c
Normal file
927
apps/earphone/xtell_Sensor/sensor/SC7U22_Ma.c
Normal file
@ -0,0 +1,927 @@
|
||||
#include "SC7U22.h"
|
||||
#include "math.h"
|
||||
#include "os/os_api.h"
|
||||
|
||||
#if SL_Sensor_Algo_Release_Enable==0x00
|
||||
#include "printf.h"
|
||||
#endif
|
||||
|
||||
|
||||
//I2C SPI选择
|
||||
//#define SL_SC7U22_SPI_EN_I2C_DISABLE 0x00 //需要配合SL_SPI_IIC_INTERFACE使用
|
||||
#define SL_SPI_IIC_INTERFACE 0x01 //需要配合SL_SC7A22H_SPI_EN_I2C_DISABLE 使用
|
||||
//是否使能原始数据高通滤波
|
||||
#define SL_SC7U22_RAWDATA_HPF_ENABLE 0x00
|
||||
//中断默认电平
|
||||
#define SL_SC7U22_INT_DEFAULT_LEVEL 0x01
|
||||
//SDO 是否上拉
|
||||
#define SL_SC7U22_SDO_PullUP_ENABLE 0x01
|
||||
//AOI中断是否唤醒
|
||||
#define SL_SC7U22_AOI_Wake_Up_ENABLE 0x00
|
||||
|
||||
//FIFO_STREAM模式//FIFO_WTM模式
|
||||
//#define SL_SC7U22_FIFO_STREAM_WTM 0x01//0X00=STREAM MODE 0X01=FIFO MODE
|
||||
|
||||
#define SL_SC7U22_IIC_DELAY_US 5
|
||||
|
||||
static u32 SL_MEMS_i2cRead(u8 addr, u8 reg, u8 len, u8 *buf) {
|
||||
return _gravity_sensor_get_ndata(addr, reg, buf, len);
|
||||
}
|
||||
static u8 SL_MEMS_i2cWrite(u8 addr, u8 reg, u8 data) {
|
||||
gravity_sensor_command(addr, reg, data);
|
||||
return 0;
|
||||
}
|
||||
|
||||
unsigned char SL_SC7U22_I2c_Spi_Write(unsigned char sl_spi_iic, unsigned char reg, unsigned char dat)
|
||||
{
|
||||
if (sl_spi_iic == 1) {
|
||||
SL_MEMS_i2cWrite(SL_SC7U22_IIC_8BITS_WRITE_ADDR, reg, dat);
|
||||
return 0;
|
||||
}
|
||||
// SPI not implemented
|
||||
return 1; // 失败
|
||||
}
|
||||
|
||||
unsigned char SL_SC7U22_I2c_Spi_Read(unsigned char sl_spi_iic, unsigned char reg, unsigned short len, unsigned char* buf)
|
||||
{
|
||||
if (sl_spi_iic == 1) {
|
||||
return SL_MEMS_i2cRead(SL_SC7U22_IIC_8BITS_READ_ADDR, reg, len, buf);
|
||||
}
|
||||
// SPI not implemented
|
||||
return 0; // 失败
|
||||
}
|
||||
|
||||
static void sl_delay(unsigned char sl_i)
|
||||
{
|
||||
os_time_dly(sl_i);
|
||||
}
|
||||
|
||||
|
||||
unsigned char SL_SC7U22_Check(void)
|
||||
{
|
||||
unsigned char reg_value=0;
|
||||
|
||||
SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x7F, 0x00);//goto 0x00
|
||||
SL_SC7U22_I2c_Spi_Read(SL_SPI_IIC_INTERFACE, SC7U22_WHO_AM_I, 1, ®_value);
|
||||
#if SL_Sensor_Algo_Release_Enable==0x00
|
||||
printf("0x%x=0x%x\r\n",SC7U22_WHO_AM_I,reg_value);
|
||||
#endif
|
||||
if(reg_value==0x6A)
|
||||
return 0x01;//SC7U22
|
||||
else
|
||||
return 0x00;//通信异常
|
||||
}
|
||||
|
||||
unsigned char SL_SC7U22_Config(void)
|
||||
{
|
||||
unsigned char Check_Flag=0;
|
||||
unsigned char reg_value=0;
|
||||
|
||||
#if SL_SPI_IIC_INTERFACE==0x00 //SPI
|
||||
SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x7F, 0x00);//goto 0x90
|
||||
sl_delay(1);
|
||||
SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x4A, 0x66);
|
||||
sl_delay(1);
|
||||
SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x7F, 0x83);//goto 0x6F
|
||||
sl_delay(1);
|
||||
SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x6F, 0x04);//I2C disable
|
||||
sl_delay(1);
|
||||
SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x7F, 0x00);//goto 0x6F
|
||||
sl_delay(1);
|
||||
SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x4A, 0x00);
|
||||
sl_delay(1);
|
||||
#endif
|
||||
Check_Flag=SL_SC7U22_Check();
|
||||
// Check_Flag= SL_SC7U22_SOFT_RESET();
|
||||
// Check_Flag=1;//强制初始化
|
||||
#if SL_Sensor_Algo_Release_Enable==0x00
|
||||
printf("SL_SC7U22_Check=0x%x\r\n",Check_Flag);
|
||||
#endif
|
||||
if(Check_Flag==1)
|
||||
{
|
||||
Check_Flag= SL_SC7U22_POWER_DOWN();
|
||||
}
|
||||
#if SL_Sensor_Algo_Release_Enable==0x00
|
||||
printf("SL_SC7U22_POWER_DOWN=0x%x\r\n",Check_Flag);
|
||||
#endif
|
||||
if(Check_Flag==1)
|
||||
{
|
||||
Check_Flag= SL_SC7U22_SOFT_RESET();
|
||||
}
|
||||
#if SL_Sensor_Algo_Release_Enable==0x00
|
||||
printf("SL_SC7U22_SOFT_RESET=0x%x\r\n",Check_Flag);
|
||||
#endif
|
||||
if(Check_Flag==1)
|
||||
{
|
||||
SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x7F, 0x00);//goto 0x00
|
||||
os_time_dly(1);//10ms
|
||||
SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x7D, 0x0E);//PWR_CTRL ENABLE ACC+GYR+TEMP
|
||||
os_time_dly(1);//10ms
|
||||
|
||||
SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x40, 0x08);//ACC_CONF 0x08=100Hz
|
||||
SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x41, 0x01);//ACC_RANGE ±4G
|
||||
SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x42, 0x88);//GYR_CONF 0x88=100Hz
|
||||
SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x43, 0x00);//GYR_RANGE 2000dps
|
||||
SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x04, 0x50);//COM_CFG
|
||||
|
||||
#if SL_SC7U22_RAWDATA_HPF_ENABLE ==0x01
|
||||
SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE,0x7F, 0x83);//goto 0x83
|
||||
sl_delay(1);
|
||||
SL_SC7U22_I2c_Spi_Read(SL_SPI_IIC_INTERFACE, 0x26, 1, ®_value);
|
||||
reg_value=reg_value|0xA0;
|
||||
SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x26, reg_value);//HPF_CFG rawdata hpf
|
||||
#endif
|
||||
|
||||
#if SL_SC7U22_AOI_Wake_Up_ENABLE==0x01
|
||||
SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x30, 0x2A);//XYZ-ENABLE
|
||||
SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x32, 0x01);//VTH
|
||||
SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x33, 0x01);//TTH
|
||||
SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x3F, 0x30);//HPF FOR AOI1&AOI2
|
||||
#endif
|
||||
|
||||
#if SL_SC7U22_FIFO_ENABLE==0x01
|
||||
SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x1E,0x1D);//
|
||||
SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x1D,0x00);//
|
||||
SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x1D,0x20);//
|
||||
SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x1C,0x37);//
|
||||
#endif
|
||||
|
||||
|
||||
#if SL_SC7U22_SDO_PullUP_ENABLE ==0x01
|
||||
SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE,0x7F, 0x8C);//goto 0x8C
|
||||
sl_delay(1);
|
||||
SL_SC7U22_I2c_Spi_Read(SL_SPI_IIC_INTERFACE, 0x30, 1, ®_value);
|
||||
reg_value=reg_value&0xFE;//CS PullUP_enable
|
||||
reg_value=reg_value&0xFD;//SDO PullUP_enable
|
||||
SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x30, reg_value);
|
||||
|
||||
SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE,0x7F, 0x00);//goto 0x00
|
||||
os_time_dly(1);
|
||||
SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE,0x7F, 0x00);//goto 0x00
|
||||
os_time_dly(1);
|
||||
#else
|
||||
SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE,0x7F, 0x8C);//goto 0x8C
|
||||
sl_delay(1);
|
||||
SL_SC7U22_I2c_Spi_Read(SL_SPI_IIC_INTERFACE, 0x30, 1, ®_value);
|
||||
reg_value=reg_value&0xFE;//CS PullUP_enable
|
||||
reg_value=reg_value|0x02;//SDO PullUP_disable
|
||||
SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x30, reg_value);
|
||||
|
||||
SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE,0x7F, 0x00);//goto 0x00
|
||||
sl_delay(1);
|
||||
SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE,0x7F, 0x00);//goto 0x00
|
||||
sl_delay(1);
|
||||
#endif
|
||||
|
||||
return 1;
|
||||
}
|
||||
else
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
||||
//读取时间戳
|
||||
unsigned int SL_SC7U22_TimeStamp_Read(void)
|
||||
{
|
||||
unsigned char time_data[3];
|
||||
unsigned int time_stamp;
|
||||
|
||||
SL_SC7U22_I2c_Spi_Read(SL_SPI_IIC_INTERFACE, 0x18, 1, &time_data[0]);
|
||||
SL_SC7U22_I2c_Spi_Read(SL_SPI_IIC_INTERFACE, 0x19, 1, &time_data[1]);
|
||||
SL_SC7U22_I2c_Spi_Read(SL_SPI_IIC_INTERFACE, 0x20, 1, &time_data[2]);
|
||||
|
||||
time_stamp=(unsigned int)(time_data[0]<<16|time_data[1]<<8|time_data[2]);
|
||||
|
||||
return time_stamp;
|
||||
}
|
||||
|
||||
#if SL_SC7U22_FIFO_ENABLE ==0x00
|
||||
//100Hz 10ms read once
|
||||
void SL_SC7U22_RawData_Read(signed short * acc_data_buf,signed short * gyr_data_buf)
|
||||
{
|
||||
unsigned char raw_data[12];
|
||||
unsigned char drdy_satus=0x00;
|
||||
unsigned short drdy_cnt=0;
|
||||
|
||||
while((drdy_satus&0x03)!=0x03)//acc+gyro
|
||||
// while((drdy_satus&0x01)!=0x01)//acc
|
||||
{
|
||||
drdy_satus=0x00;
|
||||
sl_delay(1);
|
||||
SL_SC7U22_I2c_Spi_Read(SL_SPI_IIC_INTERFACE, 0x0B, 1, &drdy_satus);
|
||||
drdy_cnt++;
|
||||
if(drdy_cnt>30000) break;
|
||||
}
|
||||
|
||||
#if SL_Sensor_Algo_Release_Enable==0x00
|
||||
// SL_SC7U22_I2c_Spi_Read(SL_SPI_IIC_INTERFACE, 0x30, 1, &drdy_satus);
|
||||
// printf("RawData:0x40=%x\r\n",drdy_satus);
|
||||
// SL_SC7U22_I2c_Spi_Read(SL_SPI_IIC_INTERFACE, 0x40, 1, &drdy_satus);
|
||||
// printf("RawData:0x40=%x\r\n",drdy_satus);
|
||||
// SL_SC7U22_I2c_Spi_Read(SL_SPI_IIC_INTERFACE, 0x06, 1, &drdy_satus);
|
||||
// printf("RawData:0x06=%x\r\n",drdy_satus);
|
||||
// SL_SC7U22_I2c_Spi_Read(SL_SPI_IIC_INTERFACE, 0x07, 1, &drdy_satus);
|
||||
// printf("RawData:0x07=%x\r\n",drdy_satus);
|
||||
// SL_SC7U22_I2c_Spi_Read(SL_SPI_IIC_INTERFACE, 0x7D, 1, &drdy_satus);
|
||||
// printf("RawData:0x7D=%x\r\n",drdy_satus);
|
||||
// SL_SC7U22_I2c_Spi_Read(SL_SPI_IIC_INTERFACE, 0x31, 1, &drdy_satus);
|
||||
// printf("RawData:0x31=%x\r\n",drdy_satus);
|
||||
// SL_SC7U22_I2c_Spi_Read(SL_SPI_IIC_INTERFACE, 0x02, 1, &drdy_satus);
|
||||
// printf("RawData:0x02=%x\r\n",drdy_satus);
|
||||
// SL_SC7U22_I2c_Spi_Read(SL_SPI_IIC_INTERFACE, 0x03, 1, &drdy_satus);
|
||||
// printf("RawData:0x03=%x\r\n",drdy_satus);
|
||||
#endif
|
||||
|
||||
SL_SC7U22_I2c_Spi_Read(SL_SPI_IIC_INTERFACE, 0x0C, 12, &raw_data[0]);
|
||||
|
||||
acc_data_buf[0] =(signed short)((((unsigned char)raw_data[0])* 256) + ((unsigned char)raw_data[1]));//ACCX-16位
|
||||
acc_data_buf[1] =(signed short)((((unsigned char)raw_data[2])* 256) + ((unsigned char)raw_data[3]));//ACCY-16位
|
||||
acc_data_buf[2] =(signed short)((((unsigned char)raw_data[4])* 256) + ((unsigned char)raw_data[5]));//ACCZ-16位
|
||||
gyr_data_buf[0] =(signed short)((((unsigned char)raw_data[6])* 256) + ((unsigned char)raw_data[7]));//GYRX-16位
|
||||
gyr_data_buf[1] =(signed short)((((unsigned char)raw_data[8])* 256) + ((unsigned char)raw_data[9]));//GYRY-16位
|
||||
gyr_data_buf[2] =(signed short)((((unsigned char)raw_data[10])* 256) + ((unsigned char)raw_data[11]));//GYRZ-16位
|
||||
|
||||
#if SL_Sensor_Algo_Release_Enable==0x00
|
||||
printf("RawData:AX=%d,AY=%d,AZ=%d,GX=%d,GY=%d,GZ=%d\r\n",acc_data_buf[0],acc_data_buf[1],acc_data_buf[2],gyr_data_buf[0],gyr_data_buf[1],gyr_data_buf[2]);
|
||||
#endif
|
||||
|
||||
}
|
||||
#else
|
||||
|
||||
#if SL_Sensor_Algo_Release_Enable==0x00
|
||||
#define SL_SC7U22_WAIT_FIFO_LEN_ENABLE 0x00//0x01
|
||||
#else
|
||||
#define SL_SC7U22_WAIT_FIFO_LEN_ENABLE 0x00
|
||||
#endif
|
||||
unsigned char Acc_FIFO_Num;
|
||||
unsigned char Gyr_FIFO_Num;
|
||||
|
||||
unsigned char SL_SC7U22_FIFO_DATA[1024];
|
||||
|
||||
unsigned short SL_SC7U22_FIFO_Read(signed short *accx_buf,signed short *accy_buf,signed short *accz_buf,signed short *gyrx_buf,signed short *gyry_buf,signed short *gyrz_buf)
|
||||
{
|
||||
int16_t Acc_x = 0, Acc_y = 0, Acc_z = 0;
|
||||
int16_t Gyr_x = 0, Gyr_y = 0, Gyr_z = 0;
|
||||
unsigned char fifo_num1=0;
|
||||
unsigned char fifo_num2=0;
|
||||
unsigned short fifo_num=0;
|
||||
unsigned short fifo_len=0;
|
||||
unsigned short temp = 0;
|
||||
unsigned short i = 0 ;
|
||||
unsigned char header[2];
|
||||
unsigned short j;
|
||||
|
||||
#if SL_Sensor_Algo_Release_Enable==0x00 //user can set to zero
|
||||
#if SL_SC7U22_WAIT_FIFO_LEN_ENABLE==0x00
|
||||
while((fifo_num1&0x20)!=0x20)
|
||||
{
|
||||
sl_delay(200);
|
||||
SL_SC7U22_I2c_Spi_Read(SL_SPI_IIC_INTERFACE, 0x1F,1,&fifo_num1);
|
||||
}
|
||||
SL_SC7U22_I2c_Spi_Read(SL_SPI_IIC_INTERFACE, 0x1F,1,&fifo_num1);
|
||||
SL_SC7U22_I2c_Spi_Read(SL_SPI_IIC_INTERFACE, 0x20,1,&fifo_num2);
|
||||
if((fifo_num1&0x10)==0x10)
|
||||
{
|
||||
fifo_num=2048;
|
||||
}
|
||||
else
|
||||
{
|
||||
fifo_num=(fifo_num1&0x0F)*256+fifo_num2;
|
||||
}
|
||||
#else
|
||||
while(fifo_num2<194)//32
|
||||
{
|
||||
SL_SC7U22_I2c_Spi_Read(SL_SPI_IIC_INTERFACE, 0x1F,1,&fifo_num1);
|
||||
SL_SC7U22_I2c_Spi_Read(SL_SPI_IIC_INTERFACE, 0x20,1,&fifo_num2);
|
||||
sl_delay(20);
|
||||
fifo_wait++;
|
||||
if(fifo_wait>30000) break;
|
||||
}
|
||||
fifo_wait=0;
|
||||
fifo_num=fifo_num2;
|
||||
#endif
|
||||
|
||||
#else
|
||||
SL_SC7U22_I2c_Spi_Read(SL_SPI_IIC_INTERFACE, 0x1F,1,&fifo_num1);
|
||||
SL_SC7U22_I2c_Spi_Read(SL_SPI_IIC_INTERFACE, 0x20,1,&fifo_num2);
|
||||
if((fifo_num1&0x10)==0x10)
|
||||
{
|
||||
fifo_num=2048;
|
||||
}
|
||||
else
|
||||
{
|
||||
fifo_num=(fifo_num1&0x0F)*256+fifo_num2;
|
||||
}
|
||||
#endif
|
||||
|
||||
SL_SC7U22_I2c_Spi_Read(SL_SPI_IIC_INTERFACE, 0x21, fifo_num*2, SL_SC7U22_FIFO_DATA);//读取FIFO数据 BYTE NUM
|
||||
// SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x1D, 0x00);//BY PASS MODE
|
||||
// SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x1D, 0x20);//Stream MODE
|
||||
printf("SC7U22_FIFO_NUM1:%d\n",fifo_num);
|
||||
#if SL_Sensor_Algo_Release_Enable==0x00
|
||||
// printf("0x1F:0x%x 0x20:0x%x\n",fifo_num1,fifo_num2);
|
||||
// printf("SC7U22_FIFO_NUM1:%d\n",fifo_num);
|
||||
#endif
|
||||
fifo_len=0;
|
||||
|
||||
i = 0;
|
||||
Acc_FIFO_Num=0;
|
||||
Gyr_FIFO_Num=0;
|
||||
while(i < fifo_num*2)
|
||||
{
|
||||
//header process 1
|
||||
header[0] = SL_SC7U22_FIFO_DATA[i + 0];
|
||||
header[1] = SL_SC7U22_FIFO_DATA[i + 1];
|
||||
i = i + 2;
|
||||
|
||||
//timestamp process 2
|
||||
if(header[1] & 0x80)
|
||||
{
|
||||
i = i + 4;//every frame include the timestamp, 4 bytes
|
||||
}
|
||||
//acc process 3
|
||||
if(header[0] & 0x04)
|
||||
{
|
||||
accx_buf[Acc_FIFO_Num] = ((s16)(SL_SC7U22_FIFO_DATA[i + 0] * 256 + SL_SC7U22_FIFO_DATA[i + 1])) ;
|
||||
accy_buf[Acc_FIFO_Num] = ((s16)(SL_SC7U22_FIFO_DATA[i + 2] * 256 + SL_SC7U22_FIFO_DATA[i + 3])) ;
|
||||
accz_buf[Acc_FIFO_Num] = ((s16)(SL_SC7U22_FIFO_DATA[i + 4] * 256 + SL_SC7U22_FIFO_DATA[i + 5])) ;
|
||||
printf("AccNum : %d ,Acc_x : %4d, Acc_y : %4d, Acc_z : %4d,\r\n",Acc_FIFO_Num, accx_buf[Acc_FIFO_Num], accy_buf[Acc_FIFO_Num], accz_buf[Acc_FIFO_Num]);
|
||||
i = i + 6;
|
||||
Acc_FIFO_Num++;
|
||||
}
|
||||
//gyro process 3
|
||||
if(header[0] & 0x02)
|
||||
{
|
||||
gyrx_buf[Gyr_FIFO_Num] = ((s16)(SL_SC7U22_FIFO_DATA[i + 0] * 256 + SL_SC7U22_FIFO_DATA[i + 1])) ;
|
||||
gyry_buf[Gyr_FIFO_Num] = ((s16)(SL_SC7U22_FIFO_DATA[i + 2] * 256 + SL_SC7U22_FIFO_DATA[i + 3])) ;
|
||||
gyrz_buf[Gyr_FIFO_Num] = ((s16)(SL_SC7U22_FIFO_DATA[i + 4] * 256 + SL_SC7U22_FIFO_DATA[i + 5])) ;
|
||||
printf("GyrNum : %d, Gyr_x : %4d, Gyr_y : %4d, Gyr_z : %4d,\r\n",Gyr_FIFO_Num, gyrx_buf[Gyr_FIFO_Num], gyry_buf[Gyr_FIFO_Num], gyrz_buf[Gyr_FIFO_Num]);
|
||||
i = i + 6;
|
||||
Gyr_FIFO_Num++;
|
||||
}
|
||||
|
||||
//temperature process 1
|
||||
if(header[0] & 0x01)
|
||||
{
|
||||
i = i + 2;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
return fifo_len;
|
||||
}
|
||||
#endif
|
||||
|
||||
|
||||
unsigned char SL_SC7U22_POWER_DOWN(void)
|
||||
{
|
||||
unsigned char SL_Read_Reg = 0xff;
|
||||
|
||||
SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x7F, 0x00);//goto 0x00
|
||||
sl_delay(20);
|
||||
SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x7D, 0x00);//POWER DOWN
|
||||
sl_delay(200);
|
||||
SL_SC7U22_I2c_Spi_Read(SL_SPI_IIC_INTERFACE, 0x7D, 1,&SL_Read_Reg);
|
||||
if(SL_Read_Reg==0x00) return 1;
|
||||
else return 0;
|
||||
}
|
||||
|
||||
|
||||
unsigned char SL_SC7U22_SOFT_RESET(void)
|
||||
{
|
||||
unsigned char SL_Read_Reg = 0xff;
|
||||
|
||||
SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x7F, 0x00);//goto 0x00
|
||||
os_time_dly(1);
|
||||
#if SL_Sensor_Algo_Release_Enable==0x00
|
||||
SL_SC7U22_I2c_Spi_Read(SL_SPI_IIC_INTERFACE, 0x04, 1,&SL_Read_Reg);
|
||||
printf("SL_SC7U22_SOFT_RESET1 0x04=0x%x\r\n",SL_Read_Reg);
|
||||
SL_Read_Reg = 0xff;
|
||||
#endif
|
||||
SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x04, 0x10);//BOOT
|
||||
#if SL_Sensor_Algo_Release_Enable==0x00
|
||||
#endif
|
||||
SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x4A, 0xA5);//SOFT_RESET
|
||||
SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x4A, 0xA5);//SOFT_RESET
|
||||
os_time_dly(20);
|
||||
SL_SC7U22_I2c_Spi_Read(SL_SPI_IIC_INTERFACE, 0x04, 1,&SL_Read_Reg);
|
||||
#if SL_Sensor_Algo_Release_Enable==0x00
|
||||
printf("SL_SC7U22_SOFT_RESET2 0x08=0x%x\r\n",SL_Read_Reg);
|
||||
#endif
|
||||
if(SL_Read_Reg==0x50) return 1;
|
||||
else return 0;
|
||||
|
||||
}
|
||||
|
||||
|
||||
/****acc_enable ==0 close acc;acc_enable ==1 open acc******/
|
||||
/****gyro_enable==0 close acc;gyro_enable==1 open acc******/
|
||||
unsigned char SL_SC7U22_Open_Close_SET(unsigned char acc_enable,unsigned char gyro_enable)
|
||||
{
|
||||
unsigned char SL_Read_Reg = 0xff;
|
||||
unsigned char SL_Read_Check= 0xff;
|
||||
|
||||
SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x7F, 0x00);//goto 0x00
|
||||
sl_delay(1);
|
||||
SL_SC7U22_I2c_Spi_Read(SL_SPI_IIC_INTERFACE, 0x7D, 1,&SL_Read_Reg);
|
||||
|
||||
if(acc_enable==0)
|
||||
{
|
||||
SL_Read_Reg=SL_Read_Reg&0xFB;//Bit.ACC_EN=0
|
||||
}
|
||||
else if(acc_enable==1)
|
||||
{
|
||||
SL_Read_Reg=SL_Read_Reg|0x04;//Bit.ACC_EN=1
|
||||
}
|
||||
if(gyro_enable==0)
|
||||
{
|
||||
SL_Read_Reg=SL_Read_Reg&0xFD;//Bit.GYR_EN=0
|
||||
}
|
||||
else if(gyro_enable==1)
|
||||
{
|
||||
SL_Read_Reg=SL_Read_Reg|0x02;//Bit.GYR_EN=1
|
||||
}
|
||||
SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x7D, SL_Read_Reg);//PWR_CTRL ENABLE ACC+GYR+TEMP
|
||||
sl_delay(5);//5ms
|
||||
SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x7D, SL_Read_Reg);//PWR_CTRL ENABLE ACC+GYR+TEMP
|
||||
sl_delay(20);//10ms
|
||||
SL_SC7U22_I2c_Spi_Read(SL_SPI_IIC_INTERFACE, 0x7D, 1,&SL_Read_Check);
|
||||
if(SL_Read_Reg!=SL_Read_Check)
|
||||
{
|
||||
#if SL_Sensor_Algo_Release_Enable==0x00
|
||||
printf("SL_Read_Reg=0x%x SL_Read_Check=0x%x\r\n",SL_Read_Reg,SL_Read_Check);
|
||||
#endif
|
||||
return 0;
|
||||
}
|
||||
return 1;
|
||||
}
|
||||
|
||||
|
||||
/*******开启中断******/
|
||||
unsigned char SL_SC7U22_IN_SLEEP_SET(unsigned char acc_odr,unsigned char vth,unsigned char tth,unsigned char int_io)
|
||||
{
|
||||
unsigned char SL_Read_Reg = 0xff;
|
||||
unsigned char SL_Acc_Odr_Reg = 0xff;
|
||||
|
||||
SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x7F, 0x00);//goto 0x00
|
||||
sl_delay(1);
|
||||
if(int_io==1)
|
||||
{
|
||||
SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x06, 0x02);//AOI1-INT1
|
||||
}
|
||||
else if(int_io==2)
|
||||
{
|
||||
SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x08, 0x02);//AOI1-INT2
|
||||
}
|
||||
|
||||
SL_SC7U22_I2c_Spi_Read(SL_SPI_IIC_INTERFACE, 0x04, 1, &SL_Read_Reg);
|
||||
#if SL_SC7U22_INT_DEFAULT_LEVEL ==0x01
|
||||
SL_Read_Reg=SL_Read_Reg|0x04;
|
||||
SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x04, SL_Read_Reg);//defalut high level&& push-pull
|
||||
#else
|
||||
reg_value=reg_value&0xDF;
|
||||
SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x06, SL_Read_Reg);//defalut low level&& push-pull
|
||||
#endif
|
||||
SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x30, 0x2A);//AIO1-Enable
|
||||
SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x32, vth);//VTH
|
||||
SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x33, tth);//TTH
|
||||
|
||||
if(acc_odr==12)
|
||||
{
|
||||
SL_Acc_Odr_Reg=0x05;
|
||||
}
|
||||
else if(acc_odr==25)
|
||||
{
|
||||
SL_Acc_Odr_Reg=0x06;
|
||||
}
|
||||
else if(acc_odr==50)
|
||||
{
|
||||
SL_Acc_Odr_Reg=0x07;
|
||||
}
|
||||
SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x40, SL_Acc_Odr_Reg);//ACC_CONF
|
||||
os_time_dly(1);//5ms
|
||||
SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x7D, 0x04);//acc open and gyro close
|
||||
os_time_dly(1);//5ms
|
||||
SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x7D, 0x04);//acc open and gyro close
|
||||
sl_delay(200);
|
||||
SL_SC7U22_I2c_Spi_Read(SL_SPI_IIC_INTERFACE, 0x7D, 1,&SL_Read_Reg);
|
||||
|
||||
if(SL_Read_Reg!=0x04)
|
||||
{
|
||||
#if SL_Sensor_Algo_Release_Enable==0x00
|
||||
printf("SL_Read_Reg=0x%x 0x04\r\n",SL_Read_Reg);
|
||||
#endif
|
||||
return 0;
|
||||
}
|
||||
return 1;
|
||||
}
|
||||
|
||||
/*******ODR SET:25 50 100 200******************/
|
||||
/*******acc range:2 4 8 16*********************/
|
||||
/*******gyro range:125 250 500 1000 2000*******/
|
||||
/*******acc_hp_en: 0=disable 1=enable**********/
|
||||
/*******gyro_hp_en:0=disable 1=enable**********/
|
||||
unsigned char SL_SC7U22_WakeUp_SET(unsigned char odr_mode,unsigned char acc_range,unsigned char acc_hp_en,unsigned short gyro_range,unsigned char gyro_hp_en)
|
||||
{
|
||||
unsigned char SL_Odr_Reg = 0x00;
|
||||
unsigned char SL_acc_mode_Reg = 0x00;
|
||||
unsigned char SL_gyro_mode_Reg = 0x00;
|
||||
unsigned char SL_acc_range_Reg = 0x00;
|
||||
unsigned char SL_gyro_range_Reg = 0x00;
|
||||
unsigned char SL_Read_Check = 0xff;
|
||||
|
||||
SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x7F, 0x00);//goto 0x00
|
||||
sl_delay(1);
|
||||
SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x7D, 0x06);//PWR_CTRL ENABLE ACC+GYR
|
||||
sl_delay(5);
|
||||
SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x7D, 0x06);//PWR_CTRL ENABLE ACC+GYR
|
||||
sl_delay(200);
|
||||
SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x30, 0x00);//AIO1-disable
|
||||
SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x32, 0xff);//vth
|
||||
SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x33, 0xff);//tth
|
||||
|
||||
if(odr_mode==25)
|
||||
{
|
||||
SL_Odr_Reg=0x06;
|
||||
}
|
||||
else if(odr_mode==50)
|
||||
{
|
||||
SL_Odr_Reg=0x07;
|
||||
}
|
||||
else if(odr_mode==100)
|
||||
{
|
||||
SL_Odr_Reg=0x08;
|
||||
}
|
||||
else if(odr_mode==200)
|
||||
{
|
||||
SL_Odr_Reg=0x09;
|
||||
}
|
||||
if(acc_hp_en==1)
|
||||
SL_acc_mode_Reg=0x80;
|
||||
|
||||
SL_acc_mode_Reg=SL_acc_mode_Reg|SL_Odr_Reg;
|
||||
SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x40, SL_acc_mode_Reg);//ACC_CONF
|
||||
|
||||
if(gyro_hp_en==1)
|
||||
SL_gyro_mode_Reg=0x40;
|
||||
else if(gyro_hp_en==2)
|
||||
SL_gyro_mode_Reg=0x80;
|
||||
else if(gyro_hp_en==3)
|
||||
SL_gyro_mode_Reg=0xC0;
|
||||
|
||||
SL_gyro_mode_Reg=SL_gyro_mode_Reg|SL_Odr_Reg;
|
||||
SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x42, SL_gyro_mode_Reg);//GYR_CONF
|
||||
|
||||
if(acc_range==2)
|
||||
{
|
||||
SL_acc_range_Reg=0x00;
|
||||
}
|
||||
else if(acc_range==4)
|
||||
{
|
||||
SL_acc_range_Reg=0x01;
|
||||
}
|
||||
else if(acc_range==8)
|
||||
{
|
||||
SL_acc_range_Reg=0x02;
|
||||
}
|
||||
else if(acc_range==16)
|
||||
{
|
||||
SL_acc_range_Reg=0x03;
|
||||
}
|
||||
SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x41, SL_acc_range_Reg);//ACC_RANGE
|
||||
|
||||
if(gyro_range==2000)
|
||||
{
|
||||
SL_gyro_range_Reg=0x00;
|
||||
}
|
||||
else if(gyro_range==1000)
|
||||
{
|
||||
SL_gyro_range_Reg=0x01;
|
||||
}
|
||||
else if(gyro_range==500)
|
||||
{
|
||||
SL_gyro_range_Reg=0x02;
|
||||
}
|
||||
else if(gyro_range==250)
|
||||
{
|
||||
SL_gyro_range_Reg=0x03;
|
||||
}
|
||||
else if(gyro_range==125)
|
||||
{
|
||||
SL_gyro_range_Reg=0x04;
|
||||
}
|
||||
SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x43, SL_gyro_range_Reg);//GYR_RANGE 2000dps
|
||||
SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x43, SL_gyro_range_Reg);//GYR_RANGE 2000dps
|
||||
#if SL_Sensor_Algo_Release_Enable==0x00
|
||||
// SL_SC7U22_I2c_Spi_Read(SL_SPI_IIC_INTERFACE, 0x40, 1, &SL_Read_Check);
|
||||
// printf("RawData:0x40=%x\r\n",SL_Read_Check);
|
||||
// SL_SC7U22_I2c_Spi_Read(SL_SPI_IIC_INTERFACE, 0x41, 1, &SL_Read_Check);
|
||||
// printf("RawData:0x41=%x\r\n",SL_Read_Check);
|
||||
// SL_SC7U22_I2c_Spi_Read(SL_SPI_IIC_INTERFACE, 0x42, 1, &SL_Read_Check);
|
||||
// printf("RawData:0x42=%x\r\n",SL_Read_Check);
|
||||
// SL_SC7U22_I2c_Spi_Read(SL_SPI_IIC_INTERFACE, 0x43, 1, &SL_Read_Check);
|
||||
// printf("RawData:0x43=%x\r\n",SL_Read_Check);
|
||||
#endif
|
||||
|
||||
SL_SC7U22_I2c_Spi_Read(SL_SPI_IIC_INTERFACE, 0x43, 1,&SL_Read_Check);
|
||||
if(SL_Read_Check!=SL_gyro_range_Reg)
|
||||
{
|
||||
#if SL_Sensor_Algo_Release_Enable==0x00
|
||||
printf("SL_Read_Check=0x%x SL_gyro_range_Reg=0x%x\r\n",SL_Read_Check,SL_gyro_range_Reg);
|
||||
#endif
|
||||
return 0;
|
||||
}
|
||||
return 1;
|
||||
}
|
||||
|
||||
|
||||
#if SL_SC7U22_FIFO_ENABLE ==0x00
|
||||
// =================================================================================================
|
||||
// Madgwick AHRS 滤波器相关变量和函数
|
||||
// -------------------------------------------------------------------------------------------------
|
||||
|
||||
// 定义常量
|
||||
#define sampleFreq 100.0f // 传感器采样频率 (Hz)，必须与实际的传感器数据更新频率一致
|
||||
#define betaDef 0.1f // 算法的比例增益 beta，影响加速度计修正陀螺仪的权重
|
||||
|
||||
// 全局变量
|
||||
static volatile float beta = betaDef; // 算法增益 beta
|
||||
static volatile float q0 = 1.0f, q1 = 0.0f, q2 = 0.0f, q3 = 0.0f; // 表示姿态的四元数 (w, x, y, z)
|
||||
|
||||
/**
|
||||
* @brief 快速计算 1/sqrt(x)
|
||||
* @param x 输入的浮点数
|
||||
* @return 1/sqrt(x) 的近似值
|
||||
*/
|
||||
static float invSqrt(float x) {
|
||||
float halfx = 0.5f * x;
|
||||
float y = x;
|
||||
long i = *(long*)&y;
|
||||
i = 0x5f3759df - (i>>1);
|
||||
y = *(float*)&i;
|
||||
y = y * (1.5f - (halfx * y * y)); // 牛顿迭代法，提高精度
|
||||
return y;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Madgwick AHRS 姿态更新函数 (IMU版本)
|
||||
* @details 该函数融合了陀螺仪和加速度计的数据，计算出表示设备姿态的四元数。
|
||||
* 1. 使用陀螺仪数据积分，得到一个初步的姿态估计（预测）。
|
||||
* 2. 使用加速度计数据（当设备处于静止或低速运动时，加速度计主要测量重力）来修正这个估计（修正）。
|
||||
* 3. 通过梯度下降法找到一个最优的旋转，使得在当前姿态下，重力向量的方向与加速度计测量的方向最接近。
|
||||
* @param gx, gy, gz 陀螺仪三轴角速度 (单位: rad/s)
|
||||
* @param ax, ay, az 加速度计三轴加速度 (单位: g)
|
||||
*/
|
||||
static void MadgwickAHRSupdateIMU(float gx, float gy, float gz, float ax, float ay, float az) {
|
||||
float recipNorm;
|
||||
float s0, s1, s2, s3;
|
||||
float qDot1, qDot2, qDot3, qDot4;
|
||||
float _2q0, _2q1, _2q2, _2q3, _4q0, _4q1, _4q2 ,_8q1, _8q2, q0q0, q1q1, q2q2, q3q3;
|
||||
float dt = 1.0f / sampleFreq; // 采样时间间隔
|
||||
|
||||
// --- 1. 陀螺仪积分：计算四元数的变化率 ---
|
||||
// 姿态运动学的基本方程，描述了姿态如何随角速度变化。
|
||||
qDot1 = 0.5f * (-q1 * gx - q2 * gy - q3 * gz);
|
||||
qDot2 = 0.5f * (q0 * gx + q2 * gz - q3 * gy);
|
||||
qDot3 = 0.5f * (q0 * gy - q1 * gz + q3 * gx);
|
||||
qDot4 = 0.5f * (q0 * gz + q1 * gy - q2 * gx);
|
||||
|
||||
// --- 2. 加速度计修正 ---
|
||||
// 仅当加速度计读数有效时（即模长不为0）才进行修正，防止计算NaN。
|
||||
if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) {
|
||||
|
||||
// 将加速度计读数归一化，得到单位向量
|
||||
recipNorm = invSqrt(ax * ax + ay * ay + az * az);
|
||||
ax *= recipNorm;
|
||||
ay *= recipNorm;
|
||||
az *= recipNorm;
|
||||
|
||||
// 预先计算一些重复使用的值，提高效率
|
||||
_2q0 = 2.0f * q0;
|
||||
_2q1 = 2.0f * q1;
|
||||
_2q2 = 2.0f * q2;
|
||||
_2q3 = 2.0f * q3;
|
||||
_4q0 = 4.0f * q0;
|
||||
_4q1 = 4.0f * q1;
|
||||
_4q2 = 4.0f * q2;
|
||||
_8q1 = 8.0f * q1;
|
||||
_8q2 = 8.0f * q2;
|
||||
q0q0 = q0 * q0;
|
||||
q1q1 = q1 * q1;
|
||||
q2q2 = q2 * q2;
|
||||
q3q3 = q3 * q3;
|
||||
|
||||
// --- 梯度下降法：计算修正量 ---
|
||||
// 目标函数 f(q, a) = [2(q1q3 - q0q2) - ax, 2(q0q1 + q2q3) - ay, 2(0.5 - q1^2 - q2^2) - az]^T
|
||||
// s0, s1, s2, s3 是目标函数 f 对四元数 q 的雅可比矩阵 J 与 f 的乘积。
|
||||
// 这个结果代表了误差函数的梯度方向，用于修正四元数的变化率。
|
||||
s0 = _4q0 * q2q2 + _2q2 * ax + _4q0 * q1q1 - _2q1 * ay;
|
||||
s1 = _4q1 * q3q3 - _2q3 * ax + 4.0f * q0q0 * q1 - _2q0 * ay - _4q1 + _8q1 * q1q1 + _8q1 * q2q2 + _4q1 * az;
|
||||
s2 = 4.0f * q0q0 * q2 + _2q0 * ax + _4q2 * q3q3 - _2q3 * ay - _4q2 + _8q2 * q1q1 + _8q2 * q2q2 + _4q2 * az;
|
||||
s3 = 4.0f * q1q1 * q3 - _2q1 * ax + 4.0f * q2q2 * q3 - _2q2 * ay;
|
||||
recipNorm = invSqrt(s0 * s0 + s1 * s1 + s2 * s2 + s3 * s3); // 归一化梯度
|
||||
s0 *= recipNorm;
|
||||
s1 *= recipNorm;
|
||||
s2 *= recipNorm;
|
||||
s3 *= recipNorm;
|
||||
|
||||
// --- 应用修正量 ---
|
||||
// 将计算出的修正量（梯度）从陀螺仪积分结果中减去，beta是修正的权重。
|
||||
qDot1 -= beta * s0;
|
||||
qDot2 -= beta * s1;
|
||||
qDot3 -= beta * s2;
|
||||
qDot4 -= beta * s3;
|
||||
}
|
||||
|
||||
// --- 3. 积分：更新四元数 ---
|
||||
// 使用一阶龙格-库塔法（即欧拉法）进行积分，得到新的四元数。
|
||||
q0 += qDot1 * dt;
|
||||
q1 += qDot2 * dt;
|
||||
q2 += qDot3 * dt;
|
||||
q3 += qDot4 * dt;
|
||||
|
||||
// --- 4. 归一化四元数 ---
|
||||
// 保持四元数的模长为1，防止由于计算误差导致的累积漂移。
|
||||
recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
|
||||
q0 *= recipNorm;
|
||||
q1 *= recipNorm;
|
||||
q2 *= recipNorm;
|
||||
q3 *= recipNorm;
|
||||
}
|
||||
// =================================================================================================
|
||||
|
||||
// --- 静态校准相关变量 ---
|
||||
static unsigned char SL_SC7U22_Error_Flag=0;
|
||||
static unsigned char SL_SC7U22_Error_cnt=0;
|
||||
static unsigned char SL_SC7U22_Error_cnt2=0;
|
||||
static signed short Temp_Accgyro[6] ={0};
|
||||
static signed short Error_Accgyro[6]={0};
|
||||
static signed int Sum_Avg_Accgyro[6] ={0};
|
||||
static float yaw_offset = 0.0f;
|
||||
|
||||
static signed short SL_GetAbsShort(signed short v_Val_s16r)
|
||||
{
|
||||
if(v_Val_s16r==(-32768))
|
||||
return 32767;
|
||||
return (v_Val_s16r < 0) ? -v_Val_s16r : v_Val_s16r;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 姿态角解算函数
|
||||
* @details
|
||||
* 该函数主要完成两项工作：
|
||||
* 1. 静态校准：在初始阶段，检测传感器是否处于静止状态。如果是，则计算加速度计和陀螺仪的零点偏移（误差），用于后续的数据补偿。
|
||||
* 2. 姿态解算：使用 Madgwick 滤波器融合经过校准后的加速度计和陀螺仪数据，计算出物体的俯仰角（Pitch）、横滚角（Roll）和偏航角（Yaw）。
|
||||
*
|
||||
* @param calibration_en 传入：外部校准使能标志。如果为0，则强制认为已经校准完成。
|
||||
* @param acc_gyro_input 传入和传出：包含6轴原始数据的数组指针，顺序为 [ACC_X, ACC_Y, ACC_Z, GYR_X, GYR_Y, GYR_Z]。该函数会对其进行原地修改，填充为校准后的数据。
|
||||
* @param Angle_output 传出：滤波后的结果，顺序为 [Pitch, Roll, Yaw]。
|
||||
* @param yaw_rst 传入：Yaw轴重置标志。如果为1，则将Yaw角清零。
|
||||
*
|
||||
* @return
|
||||
* - 0: 正在进行静态校准。
|
||||
* - 1: 姿态角计算成功。
|
||||
* - 2: 校准未完成，无法进行计算。
|
||||
*/
|
||||
unsigned char SL_SC7U22_Angle_Output(unsigned char calibration_en, signed short *acc_gyro_input, float *Angle_output, unsigned char yaw_rst)
|
||||
{
|
||||
unsigned short acc_gyro_delta[2];
|
||||
unsigned char sl_i = 0;
|
||||
|
||||
// 如果外部强制使能校准，则将标志位置1
|
||||
if (calibration_en == 0) {
|
||||
SL_SC7U22_Error_Flag = 1;
|
||||
}
|
||||
|
||||
// =================================================================================
|
||||
// 步骤 1: 静态校准 (与原版逻辑相同)
|
||||
// ---------------------------------------------------------------------------------
|
||||
if (SL_SC7U22_Error_Flag == 0) {
|
||||
// 计算当前数据与上一帧数据的差值，用于判断是否静止
|
||||
acc_gyro_delta[0] = 0;
|
||||
acc_gyro_delta[1] = 0;
|
||||
for (sl_i = 0; sl_i < 3; sl_i++) {
|
||||
acc_gyro_delta[0] += SL_GetAbsShort(acc_gyro_input[sl_i] - Temp_Accgyro[sl_i]);
|
||||
acc_gyro_delta[1] += SL_GetAbsShort(acc_gyro_input[3 + sl_i] - Temp_Accgyro[3 + sl_i]);
|
||||
}
|
||||
// 保存当前数据，用于下一帧比较
|
||||
for (sl_i = 0; sl_i < 6; sl_i++) {
|
||||
Temp_Accgyro[sl_i] = acc_gyro_input[sl_i];
|
||||
}
|
||||
|
||||
// 判断是否处于静止状态：加速度变化量、陀螺仪变化量、各轴加速度值都在一个很小的范围内
|
||||
// 假设1g = 8192 (对应 +/-4g 量程)
|
||||
if ((acc_gyro_delta[0] / 8 < 80) && (acc_gyro_delta[1] < 20) && (SL_GetAbsShort(acc_gyro_input[0]) < 3000) && (SL_GetAbsShort(acc_gyro_input[1]) < 3000) && (SL_GetAbsShort(acc_gyro_input[2] - 8192) < 3000)) { //acc<80mg gyro<20 lsb
|
||||
if (SL_SC7U22_Error_cnt < 200) {
|
||||
SL_SC7U22_Error_cnt++; // 静止计数器累加
|
||||
}
|
||||
} else {
|
||||
SL_SC7U22_Error_cnt = 0; // 如果发生移动，则清空静止计数器
|
||||
}
|
||||
|
||||
// 如果静止时间足够长（这里是190个采样周期，约1.9秒）
|
||||
if (SL_SC7U22_Error_cnt > 190) {
|
||||
// 开始累加50个采样点的数据
|
||||
for (sl_i = 0; sl_i < 6; sl_i++) {
|
||||
Sum_Avg_Accgyro[sl_i] += acc_gyro_input[sl_i];
|
||||
}
|
||||
SL_SC7U22_Error_cnt2++;
|
||||
if (SL_SC7U22_Error_cnt2 > 49) {
|
||||
// 累加满50个点后，计算平均值
|
||||
SL_SC7U22_Error_Flag = 1; // 标记校准完成
|
||||
SL_SC7U22_Error_cnt2 = 0;
|
||||
SL_SC7U22_Error_cnt = 0;
|
||||
for (sl_i = 0; sl_i < 6; sl_i++) {
|
||||
Sum_Avg_Accgyro[sl_i] = Sum_Avg_Accgyro[sl_i] / 50;
|
||||
}
|
||||
|
||||
// 计算零点偏移：理想值 - 实际平均值
|
||||
// 加速度Z轴的理想值是8192，对应1g（假设量程为±4g）
|
||||
Error_Accgyro[0] = 0 - Sum_Avg_Accgyro[0];
|
||||
Error_Accgyro[1] = 0 - Sum_Avg_Accgyro[1];
|
||||
Error_Accgyro[2] = 8192 - Sum_Avg_Accgyro[2];
|
||||
Error_Accgyro[3] = 0 - Sum_Avg_Accgyro[3];
|
||||
Error_Accgyro[4] = 0 - Sum_Avg_Accgyro[4];
|
||||
Error_Accgyro[5] = 0 - Sum_Avg_Accgyro[5];
|
||||
#if SL_Sensor_Algo_Release_Enable == 0x00
|
||||
printf("AVG_Recode AX:%d,AY:%d,AZ:%d,GX:%d,GY:%d,GZ:%d\r\n", Sum_Avg_Accgyro[0], Sum_Avg_Accgyro[1], Sum_Avg_Accgyro[2], Sum_Avg_Accgyro[3], Sum_Avg_Accgyro[4], Sum_Avg_Accgyro[5]);
|
||||
printf("Error_Recode AX:%d,AY:%d,AZ:%d,GX:%d,GY:%d,GZ:%d\r\n", Error_Accgyro[0], Error_Accgyro[1], Error_Accgyro[2], Error_Accgyro[3], Error_Accgyro[4], Error_Accgyro[5]);
|
||||
#endif
|
||||
}
|
||||
} else {
|
||||
// 如果在累加过程中发生移动，则重新开始
|
||||
SL_SC7U22_Error_cnt2 = 0;
|
||||
for (sl_i = 0; sl_i < 6; sl_i++) {
|
||||
Sum_Avg_Accgyro[sl_i] = 0;
|
||||
}
|
||||
}
|
||||
|
||||
return 0; // 返回0，表示正在校准
|
||||
}
|
||||
|
||||
// =================================================================================
|
||||
// 步骤 2: 姿态解算 (Madgwick)
|
||||
// ---------------------------------------------------------------------------------
|
||||
if (SL_SC7U22_Error_Flag == 1) { // 确认已经校准完成
|
||||
// --- 2.1 数据预处理 ---
|
||||
// 应用零点偏移补偿
|
||||
for (sl_i = 0; sl_i < 6; sl_i++) {
|
||||
Temp_Accgyro[sl_i] = acc_gyro_input[sl_i] + Error_Accgyro[sl_i];
|
||||
}
|
||||
|
||||
#if 1 // 将校准后的数据写回输入数组
|
||||
for (sl_i = 0; sl_i < 6; sl_i++) {
|
||||
acc_gyro_input[sl_i] = Temp_Accgyro[sl_i];
|
||||
}
|
||||
#endif
|
||||
// --- 2.2 转换数据单位 ---
|
||||
// 将校准后的传感器原始值 (LSB) 转换为 Madgwick 算法所需的物理单位。
|
||||
// 加速度: LSB -> g (重力加速度)。转换系数 = 量程 / (2^15)。假设 +/-4g 量程, 系数 = 4 / 32768 = 1/8192。
|
||||
float ax = (float)Temp_Accgyro[0] / 8192.0f;
|
||||
float ay = (float)Temp_Accgyro[1] / 8192.0f;
|
||||
float az = (float)Temp_Accgyro[2] / 8192.0f;
|
||||
// 角速度: LSB -> rad/s (弧度/秒)。转换系数 = (量程 * PI) / (180 * 2^15)。
|
||||
// 假设 +/-2000dps 量程, 系数 = (2000 * 3.14159) / (180 * 32768) ≈ 0.001064
|
||||
float gx = (float)Temp_Accgyro[3] * 0.001064f;
|
||||
float gy = (float)Temp_Accgyro[4] * 0.001064f;
|
||||
float gz = (float)Temp_Accgyro[5] * 0.001064f;
|
||||
|
||||
// --- 2.3 调用 Madgwick 更新函数 ---
|
||||
// 将处理好的物理单位数据传入滤波器，更新姿态四元数。
|
||||
MadgwickAHRSupdateIMU(gx, gy, gz, ax, ay, az);
|
||||
|
||||
// --- 2.4 将四元数转换为欧拉角 ---
|
||||
// 欧拉角（Pitch, Roll, Yaw）更直观，便于使用。转换公式如下。
|
||||
// 转换结果单位为度 (乘以 180/PI ≈ 57.29578)。
|
||||
float yaw, pitch, roll;
|
||||
|
||||
// Roll (横滚角，绕x轴旋转)
|
||||
roll = atan2f(2.0f * (q0 * q1 + q2 * q3), 1.0f - 2.0f * (q1 * q1 + q2 * q2)) * 57.29578f;
|
||||
// Pitch (俯仰角，绕y轴旋转)
|
||||
float sinp = 2.0f * (q0 * q2 - q3 * q1);
|
||||
if (fabsf(sinp) >= 1)
|
||||
pitch = copysignf(3.14159265f / 2, sinp) * 57.29578f; // 防止万向节死锁，当sinp接近+/-1时，直接赋+/-90度
|
||||
else
|
||||
pitch = asinf(sinp) * 57.29578f;
|
||||
// Yaw (偏航角，绕z轴旋转)
|
||||
yaw = atan2f(2.0f * (q0 * q3 + q1 * q2), 1.0f - 2.0f * (q2 * q2 + q3 * q3)) * 57.29578f;
|
||||
|
||||
// --- 2.5 处理Yaw轴重置 ---
|
||||
// Yaw角无法通过加速度计校正，会随时间漂移。提供一个重置机制，将当前Yaw角作为新的零点。
|
||||
if (yaw_rst) {
|
||||
yaw_offset = yaw;
|
||||
}
|
||||
|
||||
// --- 2.6 输出最终角度 ---
|
||||
// 将计算出的欧拉角存入输出数组。
|
||||
Angle_output[0] = pitch;
|
||||
Angle_output[1] = roll;
|
||||
Angle_output[2] = yaw - yaw_offset; // 输出减去偏移量的相对Yaw角
|
||||
|
||||
return 1; // 返回1，表示计算成功
|
||||
}
|
||||
|
||||
return 2; // 校准未完成，返回错误状态
|
||||
}
|
||||
#endif
|
||||
|
||||
181
apps/earphone/xtell_Sensor/sensor/WF282A.c
Normal file
181
apps/earphone/xtell_Sensor/sensor/WF282A.c
Normal file
@ -0,0 +1,181 @@
|
||||
/*
|
||||
气压计 - WF282A
|
||||
*/
|
||||
#include "wf282a.h"
|
||||
#include <math.h>
|
||||
#include <stdint.h> // 推荐使用标准类型
|
||||
#include "gSensor/gSensor_manage.h"
|
||||
|
||||
/*==================================================================================*/
|
||||
/* WF282A 内部定义 */
|
||||
/*==================================================================================*/
|
||||
|
||||
// 存储校准系数的静态全局变量
|
||||
static int16_t c0, c1, c01, c11, c20, c21, c30;
|
||||
static int32_t c00, c10;
|
||||
|
||||
/*==================================================================================*/
|
||||
/* 封装的底层I2C读写函数 */
|
||||
/*==================================================================================*/
|
||||
|
||||
/**
|
||||
* @brief 写入单个字节到WF282A寄存器
|
||||
*/
|
||||
static void wf282a_write_reg(uint8_t reg, uint8_t data) {
|
||||
gravity_sensor_command(WF_IIC_WRITE_ADDRESS, reg, data);
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 从WF282A读取多个字节
|
||||
*/
|
||||
static uint32_t wf282a_read_regs(uint8_t reg, uint8_t *buf, uint8_t len) {
|
||||
return _gravity_sensor_get_ndata(WF_IIC_READ_ADDRESS, reg, buf, len);
|
||||
}
|
||||
|
||||
/*==================================================================================*/
|
||||
/* 内部辅助函数 */
|
||||
/*==================================================================================*/
|
||||
|
||||
/**
|
||||
* @brief 从缓冲区中解析所有校准系数
|
||||
* @param buf 包含从寄存器0x10开始读取的18个字节的校准数据
|
||||
*/
|
||||
static void parse_calibration_data(const uint8_t *buf) {
|
||||
// c0 (12-bit)
|
||||
c0 = ((int16_t)buf[0] << 4) | (buf[1] >> 4);
|
||||
if (c0 & (1 << 11)) c0 |= 0xF000;
|
||||
|
||||
// c1 (12-bit)
|
||||
c1 = (((int16_t)buf[1] & 0x0F) << 8) | buf[2];
|
||||
if (c1 & (1 << 11)) c1 |= 0xF000;
|
||||
|
||||
// c00 (20-bit)
|
||||
c00 = ((int32_t)buf[3] << 12) | ((int32_t)buf[4] << 4) | (buf[5] >> 4);
|
||||
if (c00 & (1 << 19)) c00 |= 0xFFF00000;
|
||||
|
||||
// c10 (20-bit)
|
||||
c10 = (((int32_t)buf[5] & 0x0F) << 16) | ((int32_t)buf[6] << 8) | buf[7];
|
||||
if (c10 & (1 << 19)) c10 |= 0xFFF00000;
|
||||
|
||||
// c01, c11, c20, c21, c30 (16-bit)
|
||||
c01 = (int16_t)((uint16_t)buf[8] << 8 | buf[9]);
|
||||
c11 = (int16_t)((uint16_t)buf[10] << 8 | buf[11]);
|
||||
c20 = (int16_t)((uint16_t)buf[12] << 8 | buf[13]);
|
||||
c21 = (int16_t)((uint16_t)buf[14] << 8 | buf[15]);
|
||||
c30 = (int16_t)((uint16_t)buf[16] << 8 | buf[17]);
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 获取原始温度值 (ADC)
|
||||
*/
|
||||
static int32_t Get_Traw() {
|
||||
uint8_t buff[3];
|
||||
int32_t Traw;
|
||||
// 从 MSB 寄存器 WF_TMP_B2 (0x03) 开始连续读取3个字节
|
||||
wf282a_read_regs(WF_TMP_B2, buff, 3);
|
||||
// buff[0] = B2 (MSB), buff[1] = B1, buff[2] = B0 (LSB)
|
||||
Traw = (int32_t)buff[0] << 16 | (int32_t)buff[1] << 8 | (int32_t)buff[2];
|
||||
// 24位二进制补码转32位
|
||||
if (Traw & (1 << 23)) {
|
||||
Traw |= 0xFF000000;
|
||||
}
|
||||
return Traw;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 获取原始气压值 (ADC)
|
||||
*/
|
||||
static int32_t Get_Praw() {
|
||||
uint8_t buff[3];
|
||||
int32_t Praw;
|
||||
// 从 MSB 寄存器 WF_PRS_B2 (0x00) 开始连续读取3个字节
|
||||
wf282a_read_regs(WF_PRS_B2, buff, 3);
|
||||
// buff[0] = B2 (MSB), buff[1] = B1, buff[2] = B0 (LSB)
|
||||
Praw = (int32_t)buff[0] << 16 | (int32_t)buff[1] << 8 | (int32_t)buff[2];
|
||||
// 24位二进制补码转32位
|
||||
if (Praw & (1 << 23)) {
|
||||
Praw |= 0xFF000000;
|
||||
}
|
||||
return Praw;
|
||||
}
|
||||
|
||||
/*==================================================================================*/
|
||||
/* 4. 外部接口函数实现 */
|
||||
/*==================================================================================*/
|
||||
|
||||
uint8_t WF_Init() {
|
||||
uint8_t calib_buf[18];
|
||||
uint8_t check_cfg;
|
||||
|
||||
// 1. 配置传感器工作模式
|
||||
// 推荐配置：压力8次过采样，温度1次过采样，测量速率16Hz
|
||||
wf282a_write_reg(WF_PRS_CFG, (PM_RATE_16 << 4) | PM_PRC_8);
|
||||
wf282a_write_reg(WF_TMP_CFG, (TMP_RATE_16 << 4) | TMP_PRC_1 | TMP_INT_SENSOR);
|
||||
wf282a_write_reg(WF_MEAS_CFG, 0x07); // 启动连续压力和温度测量
|
||||
wf282a_write_reg(WF_CFG_REG, 0x00); // 无中断或FIFO移位配置
|
||||
|
||||
// 2. 一次性读取所有校准系数 (从0x10到0x21，共18字节)
|
||||
if (wf282a_read_regs(COEF_C0, calib_buf, 18) != 0) {
|
||||
return 2; // 读取校准数据失败
|
||||
}
|
||||
parse_calibration_data(calib_buf);
|
||||
|
||||
// 3. 检查配置是否写入成功
|
||||
wf282a_read_regs(WF_MEAS_CFG, &check_cfg, 1);
|
||||
if (check_cfg != 0x07) {
|
||||
return 1; // 错误
|
||||
} else {
|
||||
return 0; // 成功
|
||||
}
|
||||
}
|
||||
|
||||
void WF_Sleep() {
|
||||
wf282a_write_reg(WF_MEAS_CFG, 0x00); // 待机模式
|
||||
}
|
||||
|
||||
void WF_Wakeup() {
|
||||
wf282a_write_reg(WF_MEAS_CFG, 0x07); // 恢复连续测量
|
||||
}
|
||||
|
||||
uint8_t WF_GetID() {
|
||||
uint8_t id;
|
||||
wf282a_read_regs(WF_ID_REG, &id, 1);
|
||||
return id;
|
||||
}
|
||||
|
||||
float WF_Temperature_Calculate() {
|
||||
float Traw_sc;
|
||||
int32_t Traw = Get_Traw();
|
||||
|
||||
Traw_sc = (float)Traw / KT; // 缩放原始温度值
|
||||
return (float)c0 * 0.5f + (float)c1 * Traw_sc;
|
||||
}
|
||||
|
||||
float WF_Pressure_Calculate() {
|
||||
float Traw_sc, Praw_sc, Pcomp;
|
||||
int32_t Traw = Get_Traw();
|
||||
int32_t Praw = Get_Praw();
|
||||
|
||||
Traw_sc = (float)Traw / KT; // 缩放原始温度值
|
||||
Praw_sc = (float)Praw / KP; // 缩放原始压力值
|
||||
|
||||
// 公式: 手册给出
|
||||
Pcomp = (float)c00
|
||||
+ Praw_sc * ((float)c10 + Praw_sc * ((float)c20 + Praw_sc * (float)c30))
|
||||
+ Traw_sc * (float)c01
|
||||
+ Traw_sc * Praw_sc * ((float)c11 + Praw_sc * (float)c21);
|
||||
|
||||
return Pcomp;
|
||||
}
|
||||
|
||||
float WF_Altitude_Calculate() {
|
||||
float pressure_pa = WF_Pressure_Calculate();
|
||||
// 使用标准大气压公式计算海拔
|
||||
// P = P0 * (1 - L*h / T0)^(g*M / (R*L))
|
||||
// 简化公式: h = 44330 * (1 - (P/P0)^(1/5.255))
|
||||
// 1/5.255 ≈ 0.1903
|
||||
if (pressure_pa <= 0) {
|
||||
return 0.0f; // 避免无效计算
|
||||
}
|
||||
return 44330.0f * (1.0f - powf(pressure_pa / 101325.0f, 0.1902949f));
|
||||
}
|
||||
148
apps/earphone/xtell_Sensor/sensor/WF282A.h
Normal file
148
apps/earphone/xtell_Sensor/sensor/WF282A.h
Normal file
@ -0,0 +1,148 @@
|
||||
#ifndef _WF282A_H_
|
||||
#define _WF282A_H_
|
||||
|
||||
#include <stdint.h> // 使用标准整数类型
|
||||
|
||||
// 标定值
|
||||
#define KT 524288.0f
|
||||
#define KP 1572864.0f
|
||||
|
||||
|
||||
#define WF_PULL_UP 1 //外部是否接的上拉
|
||||
|
||||
// I2C 从设备地址
|
||||
#if WF_PULL_UP == 1 //外部接的高
|
||||
#define WF_IIC_7BIT_ADDRESS 0x77 //7位,外部接高为0x77
|
||||
#define WF_IIC_WRITE_ADDRESS (WF_IIC_7BIT_ADDRESS<<1) //8位地址
|
||||
#define WF_IIC_READ_ADDRESS (WF_IIC_WRITE_ADDRESS | 0x01)
|
||||
#else
|
||||
#define WF_IIC_7BIT_ADDRESS 0x76 //7位,外部接低为0x76
|
||||
#define WF_IIC_WRITE_ADDRESS (WF_IIC_7BIT_ADDRESS<<1) //8位地址
|
||||
#define WF_IIC_READ_ADDRESS (WF_IIC_WRITE_ADDRESS | 0x01)
|
||||
#endif
|
||||
|
||||
#define WF_CHIP_ID 0X10
|
||||
|
||||
// 寄存器映射
|
||||
// 压力数据
|
||||
#define WF_PRS_B2 0x00
|
||||
#define WF_PRS_B1 0x01
|
||||
#define WF_PRS_B0 0x02
|
||||
// 温度数据
|
||||
#define WF_TMP_B2 0x03
|
||||
#define WF_TMP_B1 0x04
|
||||
#define WF_TMP_B0 0x05
|
||||
// 配置寄存器
|
||||
#define WF_PRS_CFG 0x06
|
||||
#define WF_TMP_CFG 0x07
|
||||
#define WF_MEAS_CFG 0x08
|
||||
#define WF_CFG_REG 0x09
|
||||
#define WF_INT_STS 0x0A
|
||||
#define WF_FIFO_STS 0x0B
|
||||
#define WF_RESET_REG 0x0C
|
||||
// ID寄存器
|
||||
#define WF_ID_REG 0x0D
|
||||
// 校准系数寄存器
|
||||
#define COEF_C0 0x10
|
||||
#define COEF_C0_C1 0x11
|
||||
#define COEF_C1 0x12
|
||||
#define COEF_C00_H 0x13
|
||||
#define COEF_C00_L 0x14
|
||||
#define COEF_C00_C10 0x15
|
||||
#define COEF_C10_M 0x16
|
||||
#define COEF_C10_L 0x17
|
||||
#define COEF_C01_H 0x18
|
||||
#define COEF_C01_L 0x19
|
||||
#define COEF_C11_H 0x1A
|
||||
#define COEF_C11_L 0x1B
|
||||
#define COEF_C20_H 0x1C
|
||||
#define COEF_C20_L 0x1D
|
||||
#define COEF_C21_H 0x1E
|
||||
#define COEF_C21_L 0x1F
|
||||
#define COEF_C30_H 0x20
|
||||
#define COEF_C30_L 0x21
|
||||
|
||||
// --- 配置宏 ---
|
||||
|
||||
// 压力配置 (PRS_CFG[6:4]) - 测量速率
|
||||
#define PM_RATE_1 0x00 // 1 次/秒
|
||||
#define PM_RATE_2 0x01 // 2 次/秒
|
||||
#define PM_RATE_4 0x02 // 4 次/秒
|
||||
#define PM_RATE_8 0x03 // 8 次/秒
|
||||
#define PM_RATE_16 0x04 // 16 次/秒
|
||||
#define PM_RATE_32 0x05 // 32 次/秒
|
||||
#define PM_RATE_64 0x06 // 64 次/秒
|
||||
#define PM_RATE_128 0x07 // 128 次/秒
|
||||
// 压力配置 (PRS_CFG[3:0]) - 过采样率
|
||||
#define PM_PRC_1 0x00 // 1 次 (单次)
|
||||
#define PM_PRC_2 0x01 // 2 次 (低功耗)
|
||||
#define PM_PRC_4 0x02 // 4 次
|
||||
#define PM_PRC_8 0x03 // 8 次 (标准)
|
||||
#define PM_PRC_16 0x04 // 16 次 (需要移位)
|
||||
#define PM_PRC_32 0x05 // 32 次 (需要移位)
|
||||
#define PM_PRC_64 0x06 // 64 次 (高精度, 需要移位)
|
||||
#define PM_PRC_128 0x07 // 128 次 (需要移位)
|
||||
|
||||
// 温度配置 (TMP_CFG[7]) - 传感器源
|
||||
#define TMP_EXT_SENSOR 0x80 // 使用外部传感器
|
||||
#define TMP_INT_SENSOR 0x00 // 使用内部传感器
|
||||
// 温度配置 (TMP_CFG[6:4]) - 测量速率
|
||||
#define TMP_RATE_1 0x00 // 1 次/秒
|
||||
#define TMP_RATE_2 0x01 // 2 次/秒
|
||||
#define TMP_RATE_4 0x02 // 4 次/秒
|
||||
#define TMP_RATE_8 0x03 // 8 次/秒
|
||||
#define TMP_RATE_16 0x04 // 16 次/秒
|
||||
#define TMP_RATE_32 0x05 // 32 次/秒
|
||||
#define TMP_RATE_64 0x06 // 64 次/秒
|
||||
#define TMP_RATE_128 0x07 // 128 次/秒
|
||||
// 温度配置 (TMP_CFG[3:0]) - 过采样率
|
||||
#define TMP_PRC_1 0x00 // 1 次
|
||||
#define TMP_PRC_2 0x01 // 2 次
|
||||
#define TMP_PRC_4 0x02 // 4 次
|
||||
#define TMP_PRC_8 0x03 // 8 次
|
||||
#define TMP_PRC_16 0x04 // 16 次
|
||||
#define TMP_PRC_32 0x05 // 32 次
|
||||
#define TMP_PRC_64 0x06 // 64 次
|
||||
#define TMP_PRC_128 0x07 // 128 次
|
||||
|
||||
/**
|
||||
* @brief 初始化WF282A传感器
|
||||
* @return 0: 成功, 1: 失败
|
||||
*/
|
||||
uint8_t WF_Init(void);
|
||||
|
||||
/**
|
||||
* @brief 使传感器进入休眠/待机模式
|
||||
*/
|
||||
void WF_Sleep(void);
|
||||
|
||||
/**
|
||||
* @brief 唤醒传感器，开始连续测量
|
||||
*/
|
||||
void WF_Wakeup(void);
|
||||
|
||||
/**
|
||||
* @brief 获取传感器芯片ID
|
||||
* @return 芯片ID (应为 0x10)
|
||||
*/
|
||||
uint8_t WF_GetID(void);
|
||||
|
||||
/**
|
||||
* @brief 计算并返回当前海拔高度
|
||||
* @return 海拔高度 (单位: 米)
|
||||
*/
|
||||
float WF_Altitude_Calculate(void);
|
||||
|
||||
/**
|
||||
* @brief 计算并返回补偿后的压力值
|
||||
* @return 压力 (单位: Pa)
|
||||
*/
|
||||
float WF_Pressure_Calculate(void);
|
||||
|
||||
/**
|
||||
* @brief 计算并返回补偿后的温度值
|
||||
* @return 温度 (单位: °C)
|
||||
*/
|
||||
float WF_Temperature_Calculate(void);
|
||||
|
||||
#endif // _WF282A_H_
|
||||
Reference in New Issue
Block a user