Compare commits
2 Commits
054ea8644a
...
9ccf1acda8
| Author | SHA1 | Date | |
|---|---|---|---|
| 9ccf1acda8 | |||
| 2bfdc81991 |
6
Makefile
6
Makefile
@ -250,6 +250,9 @@ INCLUDES := \
|
||||
-Iapps/earphone/xtell_Sensor \
|
||||
-Iapps/earphone/xtell_Sensor/calculate \
|
||||
-Iapps/earphone/xtell_Sensor/ano \
|
||||
-Iapps/earphone/xtell_Sensor/sensor/ \
|
||||
-Iapps/earphone/xtell_Sensor/sensor/ \
|
||||
-Iapps/earphone/xtell_Sensor/sensor/ \
|
||||
-I$(SYS_INC_DIR) \
|
||||
|
||||
|
||||
@ -622,6 +625,9 @@ c_SRC_FILES := \
|
||||
apps/earphone/xtell_Sensor/sensor/SC7U22.c \
|
||||
apps/earphone/xtell_Sensor/calculate/skiing_tracker.c \
|
||||
apps/earphone/xtell_Sensor/ano/ano_protocol.c \
|
||||
apps/earphone/xtell_Sensor/sensor/MMC56.c \
|
||||
apps/earphone/xtell_Sensor/sensor/BMP280.c \
|
||||
apps/earphone/xtell_Sensor/sensor/AK8963.c \
|
||||
|
||||
|
||||
# 需要编译的 .S 文件
|
||||
|
||||
@ -212,6 +212,42 @@ void write_gsensor_data_handle(void)
|
||||
}
|
||||
}
|
||||
|
||||
// 临时的设备扫描诊断函数
|
||||
void i2c_scanner_probe(void)
|
||||
{
|
||||
printf("Starting I2C bus scan...\n");
|
||||
int devices_found = 0;
|
||||
|
||||
// I2C地址范围是 0x08 到 0x77
|
||||
for (uint8_t addr_7bit = 0x00; addr_7bit < 0x7F; addr_7bit++)
|
||||
{
|
||||
// 构建8位的写地址
|
||||
uint8_t write_addr_8bit = (addr_7bit << 1);
|
||||
|
||||
iic_start(gSensor_info->iic_hdl);
|
||||
|
||||
// 尝试发送写地址，并检查返回值
|
||||
// iic_tx_byte 返回 1 表示收到了 ACK
|
||||
if (iic_tx_byte(gSensor_info->iic_hdl, write_addr_8bit))
|
||||
{
|
||||
printf("=====================================================================\n");
|
||||
printf("I2C device found at 7-bit address: 0x%02X\n", addr_7bit);
|
||||
printf("I2C device found at 8-bit address: 0x%02X\n", write_addr_8bit);
|
||||
printf("=====================================================================\n");
|
||||
devices_found++;
|
||||
}
|
||||
|
||||
iic_stop(gSensor_info->iic_hdl);
|
||||
delay(gSensor_info->iic_delay); // 短暂延时
|
||||
}
|
||||
|
||||
if (devices_found == 0) {
|
||||
printf("Scan finished. No I2C devices found.\n");
|
||||
} else {
|
||||
printf("Scan finished. Found %d device(s).\n", devices_found);
|
||||
}
|
||||
}
|
||||
|
||||
char w_log_buffer_1[100];
|
||||
char w_log_buffer_2[100];
|
||||
char w_log_buffer_3[100];
|
||||
@ -222,7 +258,10 @@ u8 gravity_sensor_command(u8 w_chip_id, u8 register_address, u8 function_command
|
||||
// spin_lock(&sensor_iic);
|
||||
/* os_mutex_pend(&SENSOR_IIC_MUTEX,0); */
|
||||
u8 ret = 1;
|
||||
// xlog("iic_start\n");
|
||||
iic_start(gSensor_info->iic_hdl);
|
||||
|
||||
// xlog("iic_tx_byte id\n");
|
||||
if (0 == iic_tx_byte(gSensor_info->iic_hdl, w_chip_id)) {
|
||||
ret = 0;
|
||||
xlog("\n gsen iic wr err 0\n");
|
||||
@ -230,8 +269,10 @@ u8 gravity_sensor_command(u8 w_chip_id, u8 register_address, u8 function_command
|
||||
goto __gcend;
|
||||
}
|
||||
|
||||
// xlog("iic delay\n");
|
||||
delay(gSensor_info->iic_delay);
|
||||
|
||||
// xlog("iic_tx_byte: address\n");
|
||||
if (0 == iic_tx_byte(gSensor_info->iic_hdl, register_address)) {
|
||||
ret = 0;
|
||||
xlog("\n gsen iic wr err 1\n");
|
||||
@ -241,6 +282,7 @@ u8 gravity_sensor_command(u8 w_chip_id, u8 register_address, u8 function_command
|
||||
|
||||
delay(gSensor_info->iic_delay);
|
||||
|
||||
// xlog("iic_tx_byte: command\n");
|
||||
if (0 == iic_tx_byte(gSensor_info->iic_hdl, function_command)) {
|
||||
ret = 0;
|
||||
xlog("\n gsen iic wr err 2\n");
|
||||
@ -249,6 +291,7 @@ u8 gravity_sensor_command(u8 w_chip_id, u8 register_address, u8 function_command
|
||||
}
|
||||
|
||||
strcpy(&w_log_buffer_4, "gsen iic wr sucess\n");
|
||||
// xlog("\n gsen iic wr sucess\n");
|
||||
|
||||
__gcend:
|
||||
iic_stop(gSensor_info->iic_hdl);
|
||||
@ -271,7 +314,7 @@ u8 _gravity_sensor_get_ndata(u8 r_chip_id, u8 register_address, u8 *buf, u8 data
|
||||
|
||||
iic_start(gSensor_info->iic_hdl);
|
||||
if (0 == iic_tx_byte(gSensor_info->iic_hdl, r_chip_id - 1)) {
|
||||
xlog("\n gsen iic rd err 0\n");
|
||||
xlog("I2C NACK on writing ADDR: 0x%X\n", r_chip_id - 1);
|
||||
read_len = 0;
|
||||
strcpy(&sen_log_buffer_1, "gsen iic rd err 0\n");
|
||||
goto __gdend;
|
||||
@ -280,7 +323,8 @@ u8 _gravity_sensor_get_ndata(u8 r_chip_id, u8 register_address, u8 *buf, u8 data
|
||||
|
||||
delay(gSensor_info->iic_delay);
|
||||
if (0 == iic_tx_byte(gSensor_info->iic_hdl, register_address)) {
|
||||
xlog("\n gsen iic rd err 1\n");
|
||||
xlog("I2C NACK on register ADDR: 0x%X\n", register_address);
|
||||
// xlog("\n gsen iic rd err 1\n");
|
||||
read_len = 0;
|
||||
strcpy(&sen_log_buffer_2, "gsen iic rd err 1\n");
|
||||
goto __gdend;
|
||||
@ -304,6 +348,7 @@ u8 _gravity_sensor_get_ndata(u8 r_chip_id, u8 register_address, u8 *buf, u8 data
|
||||
*buf = iic_rx_byte(gSensor_info->iic_hdl, 0);
|
||||
read_len ++;
|
||||
strcpy(&sen_log_buffer_4, "gsen iic rd success\n");
|
||||
// xlog("\n gsen iic rd success\n");
|
||||
|
||||
__gdend:
|
||||
|
||||
|
||||
@ -526,7 +526,7 @@ const struct hw_iic_config hw_iic_cfg[] = {
|
||||
.baudrate = TCFG_HW_I2C0_CLK, //IIC通讯波特率
|
||||
.hdrive = 0, //是否打开IO口强驱
|
||||
.io_filter = 1, //是否打开滤波器（去纹波）
|
||||
.io_pu = 1, //是否打开上拉电阻，如果外部电路没有焊接上拉电阻需要置1
|
||||
.io_pu = 0, //是否打开上拉电阻，如果外部电路没有焊接上拉电阻需要置1
|
||||
},
|
||||
};
|
||||
|
||||
|
||||
651
apps/earphone/xtell_Sensor/A_hide/calculate/skiing_tracker.c
Normal file
651
apps/earphone/xtell_Sensor/A_hide/calculate/skiing_tracker.c
Normal file
@ -0,0 +1,651 @@
|
||||
/*
|
||||
|
||||
*/
|
||||
#include "skiing_tracker.h"
|
||||
#include "../sensor/SC7U22.h"
|
||||
#include <math.h>
|
||||
#include <string.h>
|
||||
|
||||
#define G_ACCELERATION 9.81f
|
||||
#define DEG_TO_RAD (3.14159265f / 180.0f)
|
||||
|
||||
#define ENABLE_XLOG 1
|
||||
#ifdef xlog
|
||||
#undef xlog
|
||||
#endif
|
||||
#if ENABLE_XLOG
|
||||
#define xlog(format, ...) printf("[XT:%s] " format, __func__, ##__VA_ARGS__)
|
||||
#else
|
||||
#define xlog(format, ...) ((void)0)
|
||||
#endif
|
||||
|
||||
// --- 静止检测 ---
|
||||
//两个判断是否静止的必要条件：动态零速更新(ZUPT)阈值
|
||||
// 加速方差阈值，提高阈值，让“刹车”更灵敏，以便在波浪式前进等慢速漂移时也能触发零速更新
|
||||
#define STOP_ACC_VARIANCE_THRESHOLD 0.2f
|
||||
// 陀螺仪方差阈值
|
||||
#define STOP_GYR_VARIANCE_THRESHOLD 5.0f
|
||||
// 静止时候的陀螺仪模长
|
||||
#define STOP_GYR_MAG_THRESHOLD 15
|
||||
// --- --- ---
|
||||
|
||||
// --- 启动滑雪阈值 ---
|
||||
// 加速度模长与重力的差值大于此值，认为开始运动；降低阈值，让“油门”更灵敏，以便能捕捉到真实的慢速启动
|
||||
#define START_ACC_MAG_THRESHOLD 1.0f //0.5、1
|
||||
// 陀螺仪方差阈值，以允许启动瞬间的正常抖动，但仍能过滤掉混乱的、非滑雪的晃动。
|
||||
#define START_GYR_VARIANCE_THRESHOLD 15.0f
|
||||
// --- --- ---
|
||||
|
||||
// --- 滑雪过程 ---
|
||||
//加速度 模长(不含重力），低于此值视为 在做匀速运动
|
||||
#define SKIING_ACC_MAG_THRESHOLD 0.5f
|
||||
//陀螺仪 模长，高于此值视为 摔倒了
|
||||
#define FALLEN_GRY_MAG_THRESHOLD 2000.0f //未确定
|
||||
// --- --- ---
|
||||
|
||||
// --- 原地旋转抖动 ---
|
||||
// 加速度 方差 阈值。此值比 静止检测 阈值更宽松，
|
||||
#define WOBBLE_ACC_VARIANCE_THRESHOLD 0.5f
|
||||
// 加速度 模长 阈值
|
||||
#define WOBBLE_ACC_MAG_THRESHOLD 1.0f
|
||||
// 角速度 总模长 大于此值(度/秒)，认为正在进行非滑雪的旋转或摆动
|
||||
#define ROTATION_GYR_MAG_THRESHOLD 30.0f
|
||||
// --- --- ---
|
||||
|
||||
// --- 滑雪转弯动 ---
|
||||
// 加速度 方差 阈值，大于此值，滑雪过程可能发生了急转弯
|
||||
#define WHEEL_ACC_VARIANCE_THRESHOLD 7.0f
|
||||
// 角速度 总模长 大于此值(度/秒)，认为滑雪过程中进行急转弯
|
||||
#define WHEEL_GYR_MAG_THRESHOLD 500.0f //
|
||||
// --- --- ---
|
||||
|
||||
// --- 跳跃 ---
|
||||
// 加速度模长低于此值(g)，认为进入失重状态(IN_AIR)
|
||||
#define AIRBORNE_ACC_MAG_LOW_THRESHOLD 0.4f
|
||||
// 加速度模长高于此值(g)，认为发生落地冲击(LANDING)
|
||||
#define LANDING_ACC_MAG_HIGH_THRESHOLD 3.5f
|
||||
// 起跳加速度阈值(g)，用于进入TAKING_OFF状态
|
||||
#define TAKEOFF_ACC_MAG_HIGH_THRESHOLD 1.8f
|
||||
// 进入空中状态确认计数：需要连续3个采样点加速度低于阈值才判断为起跳
|
||||
#define AIRBORNE_CONFIRM_COUNT 3
|
||||
// 落地状态确认计数：加速度恢复到1g附近并持续2个采样点(20ms)则认为已落地
|
||||
#define GROUNDED_CONFIRM_COUNT 2
|
||||
// 最大滞空时间(秒)，超过此时间强制认为已落地，防止状态锁死
|
||||
#define MAX_TIME_IN_AIR 12.5f
|
||||
// --- --- ---
|
||||
|
||||
// --- 用于消除积分漂移的滤波器和阈值 ---
|
||||
// 高通滤波器系数 (alpha)。alpha 越接近1，滤除低频(直流偏移)的效果越强，但可能滤掉真实的慢速运动。
|
||||
// alpha = RC / (RC + dt)，参考RC电路而来，fc ≈ (1 - alpha) / (2 * π * dt)
|
||||
#define HPF_ALPHA 0.999f
|
||||
//0.995f： 0.08 Hz 的信号
|
||||
//0.999f： 0.0159 Hz
|
||||
// --- --- ---
|
||||
|
||||
// --- 低通滤波器 ---
|
||||
// 低通滤波器系数 (alpha)。alpha 越小，滤波效果越强（更平滑），但延迟越大。
|
||||
// alpha 推荐范围 0.7 ~ 0.95。可以从 0.85 开始尝试。
|
||||
#define LPF_ALPHA 0.7f
|
||||
|
||||
// 加速度死区阈值 (m/s^2)。低于此阈值的加速度被认为是噪声，不参与积分。
|
||||
// 设得太高会忽略真实的慢速启动，设得太低则无法有效抑制噪声。
|
||||
//参考：0.2f ~ 0.4f
|
||||
#define ACC_DEAD_ZONE_THRESHOLD 0.05f
|
||||
|
||||
// --- 模拟摩擦力，进行速度衰减 ---
|
||||
#define SPEED_ATTENUATION 1.0f //暂不模拟
|
||||
BLE_KS_send_data_t KS_data;
|
||||
static float quaternion_data[4];
|
||||
#ifdef XTELL_TEST
|
||||
|
||||
debug_t debug1;
|
||||
debug_t debug2;
|
||||
#endif
|
||||
|
||||
static skiing_tracker_t my_skiing_tracker;
|
||||
//////////////////////////////////////////////////////////////////////////////////////////////////
|
||||
//实现
|
||||
|
||||
void clear_speed(void){
|
||||
my_skiing_tracker.state = STATIC;
|
||||
memset(my_skiing_tracker.velocity, 0, sizeof(my_skiing_tracker.velocity));
|
||||
my_skiing_tracker.speed = 0;
|
||||
}
|
||||
|
||||
void start_detection(void){
|
||||
my_skiing_tracker.state = STATIC;
|
||||
memset(my_skiing_tracker.velocity, 0, sizeof(my_skiing_tracker.velocity));
|
||||
my_skiing_tracker.distance = 0;
|
||||
my_skiing_tracker.speed = 0;
|
||||
}
|
||||
|
||||
void stop_detection(void){
|
||||
my_skiing_tracker.state = STOP_DETECTION;
|
||||
memset(my_skiing_tracker.velocity, 0, sizeof(my_skiing_tracker.velocity));
|
||||
my_skiing_tracker.speed = 0;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 初始化滑雪追踪器
|
||||
*
|
||||
* @param tracker
|
||||
*/
|
||||
void skiing_tracker_init(skiing_tracker_t *tracker)
|
||||
{
|
||||
if (!tracker) {
|
||||
return;
|
||||
}
|
||||
// 使用memset一次性清零整个结构体，包括新增的缓冲区
|
||||
memset(tracker, 0, sizeof(skiing_tracker_t));
|
||||
tracker->state = STATIC;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 当检测到落地时，计算空中的水平飞行距离并累加到总距离
|
||||
*/
|
||||
static void calculate_air_distance(skiing_tracker_t *tracker) {
|
||||
float horizontal_speed_on_takeoff = sqrtf(
|
||||
tracker->initial_velocity_on_takeoff[0] * tracker->initial_velocity_on_takeoff[0] +
|
||||
tracker->initial_velocity_on_takeoff[1] * tracker->initial_velocity_on_takeoff[1]
|
||||
);
|
||||
float distance_in_air = horizontal_speed_on_takeoff * tracker->time_in_air;
|
||||
tracker->distance += distance_in_air;
|
||||
}
|
||||
|
||||
|
||||
|
||||
/**
|
||||
* @brief 使用四元数直接从设备坐标系的加速度中移除重力分量
|
||||
* @details 这种方法比使用欧拉角更精确、更稳定，且避免了万向节死锁。
|
||||
* @param acc_device 输入：设备坐标系下的原始加速度 [x, y, z], 单位 m/s^2
|
||||
* @param q 输入：表示姿态的四元数 [w, x, y, z]
|
||||
* @param acc_linear_device 输出：设备坐标系下移除重力后的线性加速度 [x, y, z]
|
||||
*/
|
||||
void q_remove_gravity_with_quaternion(const float *acc_device, const float *q, float *acc_linear_device)
|
||||
{
|
||||
// 从四元数计算重力在设备坐标系下的投影
|
||||
// G_device = R_transpose * G_world
|
||||
// G_world = [0, 0, g]
|
||||
// R_transpose 的第三列即为重力投影方向
|
||||
float gx = 2.0f * (q[1] * q[3] - q[0] * q[2]);
|
||||
float gy = 2.0f * (q[0] * q[1] + q[2] * q[3]);
|
||||
float gz = q[0] * q[0] - q[1] * q[1] - q[2] * q[2] + q[3] * q[3];
|
||||
|
||||
// 从原始加速度中减去重力分量
|
||||
acc_linear_device[0] = acc_device[0] - gx * G_ACCELERATION;
|
||||
acc_linear_device[1] = acc_device[1] - gy * G_ACCELERATION;
|
||||
acc_linear_device[2] = acc_device[2] - gz * G_ACCELERATION;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 使用四元数将设备坐标系的线性加速度转换到世界坐标系
|
||||
* @details 同样，此方法比使用欧拉角更优。
|
||||
* @param acc_linear_device 输入：设备坐标系下的线性加速度 [x, y, z]
|
||||
* @param q 输入：表示姿态的四元数 [w, x, y, z]
|
||||
* @param acc_linear_world 输出：世界坐标系下的线性加速度 [x, y, z]
|
||||
*/
|
||||
void q_transform_to_world_with_quaternion(const float *acc_linear_device, const float *q, float *acc_linear_world)
|
||||
{
|
||||
// 这是 R_device_to_world * acc_linear_device 的展开形式
|
||||
acc_linear_world[0] = (1.0f - 2.0f*q[2]*q[2] - 2.0f*q[3]*q[3]) * acc_linear_device[0] +
|
||||
(2.0f*q[1]*q[2] - 2.0f*q[0]*q[3]) * acc_linear_device[1] +
|
||||
(2.0f*q[1]*q[3] + 2.0f*q[0]*q[2]) * acc_linear_device[2];
|
||||
|
||||
acc_linear_world[1] = (2.0f*q[1]*q[2] + 2.0f*q[0]*q[3]) * acc_linear_device[0] +
|
||||
(1.0f - 2.0f*q[1]*q[1] - 2.0f*q[3]*q[3]) * acc_linear_device[1] +
|
||||
(2.0f*q[2]*q[3] - 2.0f*q[0]*q[1]) * acc_linear_device[2];
|
||||
|
||||
acc_linear_world[2] = (2.0f*q[1]*q[3] - 2.0f*q[0]*q[2]) * acc_linear_device[0] +
|
||||
(2.0f*q[2]*q[3] + 2.0f*q[0]*q[1]) * acc_linear_device[1] +
|
||||
(1.0f - 2.0f*q[1]*q[1] - 2.0f*q[2]*q[2]) * acc_linear_device[2];
|
||||
// acc_linear_world[2] -= G_ACCELERATION;
|
||||
}
|
||||
|
||||
|
||||
|
||||
/**
|
||||
* @brief 计算缓冲区内三轴数据的方差之和
|
||||
*
|
||||
* @param buffer 传进来的三轴数据：陀螺仪/加速度
|
||||
* @return float 返回方差和
|
||||
*/
|
||||
static float calculate_variance(float buffer[VARIANCE_BUFFER_SIZE][3])
|
||||
{
|
||||
float mean[3] = {0};
|
||||
float variance[3] = {0};
|
||||
|
||||
// 计算均值
|
||||
for (int i = 0; i < VARIANCE_BUFFER_SIZE; i++) {
|
||||
mean[0] += buffer[i][0];
|
||||
mean[1] += buffer[i][1];
|
||||
mean[2] += buffer[i][2];
|
||||
}
|
||||
mean[0] /= VARIANCE_BUFFER_SIZE;
|
||||
mean[1] /= VARIANCE_BUFFER_SIZE;
|
||||
mean[2] /= VARIANCE_BUFFER_SIZE;
|
||||
|
||||
// 计算方差
|
||||
for (int i = 0; i < VARIANCE_BUFFER_SIZE; i++) {
|
||||
variance[0] += (buffer[i][0] - mean[0]) * (buffer[i][0] - mean[0]);
|
||||
variance[1] += (buffer[i][1] - mean[1]) * (buffer[i][1] - mean[1]);
|
||||
variance[2] += (buffer[i][2] - mean[2]) * (buffer[i][2] - mean[2]);
|
||||
}
|
||||
variance[0] /= VARIANCE_BUFFER_SIZE;
|
||||
variance[1] /= VARIANCE_BUFFER_SIZE;
|
||||
variance[2] /= VARIANCE_BUFFER_SIZE;
|
||||
|
||||
// 返回三轴方差之和，作为一个综合的稳定度指标
|
||||
return variance[0] + variance[1] + variance[2];
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 摩擦力模拟，进行速度衰减
|
||||
*
|
||||
* @param tracker
|
||||
*/
|
||||
void forece_of_friction(skiing_tracker_t *tracker){
|
||||
// 增加速度衰减，模拟摩擦力
|
||||
tracker->velocity[0] *= SPEED_ATTENUATION;
|
||||
tracker->velocity[1] *= SPEED_ATTENUATION;
|
||||
tracker->velocity[2] = 0; // 垂直速度强制归零
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 状态机更新
|
||||
*
|
||||
* @param tracker 传入同步修改后传出
|
||||
* @param acc_device_ms2 三轴加速度，m/s^2
|
||||
* @param gyr_dps 三轴陀螺仪，dps
|
||||
*/
|
||||
static void update_state_machine(skiing_tracker_t *tracker, const float *acc_device_ms2, const float *gyr_dps)
|
||||
{
|
||||
// 缓冲区未填满时，不进行状态判断，默认为静止
|
||||
if (!tracker->buffer_filled) {
|
||||
tracker->state = STATIC;
|
||||
return;
|
||||
}
|
||||
|
||||
// --- 计算关键指标 ---
|
||||
float acc_variance = calculate_variance(tracker->acc_buffer); // 计算加速度方差
|
||||
float gyr_variance = calculate_variance(tracker->gyr_buffer); // 计算陀螺仪方差
|
||||
float gyr_magnitude = sqrtf(gyr_dps[0]*gyr_dps[0] + gyr_dps[1]*gyr_dps[1] + gyr_dps[2]*gyr_dps[2]); //dps
|
||||
float acc_magnitude = sqrtf(acc_device_ms2[0]*acc_device_ms2[0] + acc_device_ms2[1]*acc_device_ms2[1] + acc_device_ms2[2]*acc_device_ms2[2]); //m/s^s
|
||||
float acc_magnitude_g = acc_magnitude / G_ACCELERATION; // 转换为g单位，用于跳跃判断
|
||||
|
||||
#ifdef XTELL_TEST
|
||||
debug1.acc_variance =acc_variance;
|
||||
debug1.gyr_variance =gyr_variance;
|
||||
debug1.gyr_magnitude=gyr_magnitude;
|
||||
debug1.acc_magnitude=fabsf(acc_magnitude - G_ACCELERATION);
|
||||
#endif
|
||||
|
||||
|
||||
|
||||
// --- 状态机逻辑 (核心修改区域) ---
|
||||
|
||||
#if 0 //暂时不考虑空中
|
||||
// 1. 空中/落地状态的后续处理
|
||||
if (tracker->state == IN_AIR) {
|
||||
// A. 检测巨大冲击 -> 落地
|
||||
if (acc_magnitude_g > LANDING_ACC_MAG_HIGH_THRESHOLD) {
|
||||
tracker->state = LANDING;
|
||||
// B. 检测超时 -> 强制落地 (安全机制)
|
||||
} else if (tracker->time_in_air > MAX_TIME_IN_AIR) {
|
||||
tracker->state = LANDING;
|
||||
// C. 检测恢复正常重力 (平缓落地)
|
||||
} else if (acc_magnitude_g > 0.8f && acc_magnitude_g < 1.5f) {
|
||||
tracker->grounded_entry_counter++;
|
||||
if (tracker->grounded_entry_counter >= GROUNDED_CONFIRM_COUNT) {
|
||||
tracker->state = LANDING;
|
||||
}
|
||||
} else {
|
||||
tracker->grounded_entry_counter = 0;
|
||||
}
|
||||
return; // 在空中或刚切换到落地，结束本次状态判断
|
||||
}
|
||||
|
||||
// 2. 严格的 "起跳->空中" 状态转换逻辑
|
||||
// 只有当处于滑行状态时，才去检测起跳意图
|
||||
if (tracker->state == NO_CONSTANT_SPEED || tracker->state == CONSTANT_SPEED || tracker->state == WHEEL) {
|
||||
if (acc_magnitude_g > TAKEOFF_ACC_MAG_HIGH_THRESHOLD) {
|
||||
tracker->state = TAKING_OFF;
|
||||
tracker->airborne_entry_counter = 0; // 准备检测失重
|
||||
return;
|
||||
}
|
||||
}
|
||||
// 只有在TAKING_OFF状态下，才去检测是否进入失重
|
||||
if (tracker->state == TAKING_OFF) {
|
||||
if (acc_magnitude_g < AIRBORNE_ACC_MAG_LOW_THRESHOLD) {
|
||||
tracker->airborne_entry_counter++;
|
||||
if (tracker->airborne_entry_counter >= AIRBORNE_CONFIRM_COUNT) {
|
||||
memcpy(tracker->initial_velocity_on_takeoff, tracker->velocity, sizeof(tracker->velocity));
|
||||
tracker->time_in_air = 0;
|
||||
tracker->state = IN_AIR;
|
||||
tracker->airborne_entry_counter = 0;
|
||||
tracker->grounded_entry_counter = 0;
|
||||
return;
|
||||
}
|
||||
} else {
|
||||
// 如果在起跳冲击后一段时间内没有失重，说明只是一个颠簸，恢复滑行
|
||||
// 可以加一个小的超时计数器，这里为了简单先直接恢复
|
||||
tracker->state = NO_CONSTANT_SPEED;
|
||||
}
|
||||
return; // 无论是否切换，都结束本次判断
|
||||
}
|
||||
#endif
|
||||
|
||||
|
||||
// --- 静止判断 ---
|
||||
if (acc_variance < STOP_ACC_VARIANCE_THRESHOLD && gyr_variance < STOP_GYR_VARIANCE_THRESHOLD && gyr_magnitude < STOP_GYR_MAG_THRESHOLD) {
|
||||
tracker->state = STATIC;
|
||||
return;
|
||||
}
|
||||
|
||||
// --- 地面状态切换逻辑 ---
|
||||
switch (tracker->state) {
|
||||
case LANDING:
|
||||
tracker->state = STATIC;
|
||||
break;
|
||||
case STATIC:
|
||||
// 优先判断是否进入 WOBBLE 状态
|
||||
// 条件：陀螺仪活动剧烈，但整体加速度变化不大（说明是原地转或晃）
|
||||
if (gyr_magnitude > ROTATION_GYR_MAG_THRESHOLD && fabsf(acc_magnitude - G_ACCELERATION) < WOBBLE_ACC_MAG_THRESHOLD) {
|
||||
tracker->state = WOBBLE;
|
||||
}
|
||||
// 只有在陀螺仪和加速度都满足“前进”特征时，才启动
|
||||
else if (gyr_variance > START_GYR_VARIANCE_THRESHOLD && fabsf(acc_magnitude - G_ACCELERATION) > START_ACC_MAG_THRESHOLD) {
|
||||
tracker->state = NO_CONSTANT_SPEED;
|
||||
}
|
||||
break;
|
||||
|
||||
case WOBBLE:
|
||||
// 从 WOBBLE 状态启动的条件应该和从 STATIC 启动一样严格
|
||||
if (gyr_variance < START_GYR_VARIANCE_THRESHOLD * 2 && fabsf(acc_magnitude - G_ACCELERATION) > START_ACC_MAG_THRESHOLD) {
|
||||
tracker->state = NO_CONSTANT_SPEED;
|
||||
}
|
||||
// 如果陀螺仪活动减弱，则可能恢复静止
|
||||
else if (gyr_magnitude < ROTATION_GYR_MAG_THRESHOLD * 0.8f) { // 增加迟滞，避免抖动
|
||||
// 不直接跳回STATIC，而是依赖下一轮的全局静止判断
|
||||
}
|
||||
break;
|
||||
case NO_CONSTANT_SPEED: //非匀速状态
|
||||
//暂时不考虑摔倒
|
||||
// if (gyr_magnitude > FALLEN_GRY_MAG_THRESHOLD) {
|
||||
// tracker->state = FALLEN; //摔倒
|
||||
// } else
|
||||
if (gyr_magnitude > WHEEL_GYR_MAG_THRESHOLD && acc_variance > WHEEL_ACC_VARIANCE_THRESHOLD) {
|
||||
tracker->state = WHEEL; //转弯
|
||||
} else if (fabsf(acc_magnitude - G_ACCELERATION) < SKIING_ACC_MAG_THRESHOLD) {
|
||||
tracker->state = CONSTANT_SPEED; //匀速
|
||||
}
|
||||
break;
|
||||
|
||||
case CONSTANT_SPEED: //匀速状态
|
||||
if (fabsf(acc_magnitude - G_ACCELERATION) > START_ACC_MAG_THRESHOLD) {
|
||||
tracker->state = NO_CONSTANT_SPEED;
|
||||
}
|
||||
//TODO：可以添加进入转弯或摔倒的判断
|
||||
break;
|
||||
|
||||
case WHEEL:
|
||||
// 从转弯状态，检查转弯是否结束
|
||||
// 如果角速度和加速度方差都降下来了，就回到普通滑行状态
|
||||
if (gyr_magnitude < WHEEL_GYR_MAG_THRESHOLD * 0.8f && acc_variance < WHEEL_ACC_VARIANCE_THRESHOLD * 0.8f) { // 乘以一个滞后系数避免抖动
|
||||
tracker->state = NO_CONSTANT_SPEED;
|
||||
}
|
||||
break;
|
||||
|
||||
case FALLEN:
|
||||
// TODO：回到 STATIC
|
||||
break;
|
||||
}
|
||||
|
||||
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* @brief 主更新函数
|
||||
*
|
||||
* @param tracker
|
||||
* @param acc_g 三轴加速度，g
|
||||
* @param gyr_dps 三轴陀螺仪，dps
|
||||
* @param angle 欧若拉角
|
||||
* @param dt 采样时间间隔，会用来积分求速度
|
||||
*/
|
||||
void skiing_tracker_update(skiing_tracker_t *tracker, float *acc_g, float *gyr_dps, float *angle, float dt)
|
||||
{
|
||||
if (!tracker || !acc_g || !gyr_dps || !angle || dt <= 0) {
|
||||
return;
|
||||
}
|
||||
if(my_skiing_tracker.state == STOP_DETECTION)
|
||||
return;
|
||||
|
||||
// --- 数据预处理和缓冲 ---
|
||||
float acc_device_ms2[3];
|
||||
acc_device_ms2[0] = acc_g[0] * G_ACCELERATION;
|
||||
acc_device_ms2[1] = acc_g[1] * G_ACCELERATION;
|
||||
acc_device_ms2[2] = acc_g[2] * G_ACCELERATION;
|
||||
|
||||
// 将最新数据存入缓冲区
|
||||
memcpy(tracker->acc_buffer[tracker->buffer_index], acc_device_ms2, sizeof(acc_device_ms2));
|
||||
memcpy(tracker->gyr_buffer[tracker->buffer_index], gyr_dps, 3 * sizeof(float));
|
||||
|
||||
tracker->buffer_index++;
|
||||
if (tracker->buffer_index >= VARIANCE_BUFFER_SIZE) {
|
||||
tracker->buffer_index = 0;
|
||||
tracker->buffer_filled = 1; // 标记缓冲区已满
|
||||
}
|
||||
|
||||
// --- 更新状态机 ---
|
||||
update_state_machine(tracker, acc_device_ms2, gyr_dps);
|
||||
|
||||
// --- 根据状态执行不同的计算逻辑 ---
|
||||
switch (tracker->state) {
|
||||
case TAKING_OFF:
|
||||
tracker->speed = 0.0f;
|
||||
break;
|
||||
case IN_AIR:
|
||||
// 在空中时，只累加滞空时间
|
||||
tracker->time_in_air += dt;
|
||||
break;
|
||||
case LANDING:
|
||||
// 刚落地，计算空中距离
|
||||
calculate_air_distance(tracker);
|
||||
// 清理速度和滤波器状态，为恢复地面追踪做准备
|
||||
memset(tracker->velocity, 0, sizeof(tracker->velocity));
|
||||
tracker->speed = 0;
|
||||
memset(tracker->acc_world_unfiltered_prev, 0, sizeof(tracker->acc_world_unfiltered_prev));
|
||||
memset(tracker->acc_world_filtered, 0, sizeof(tracker->acc_world_filtered));
|
||||
memset(tracker->acc_world_lpf, 0, sizeof(tracker->acc_world_lpf)); // 清理新增的LPF状态
|
||||
break;
|
||||
case WHEEL:
|
||||
case NO_CONSTANT_SPEED:
|
||||
float linear_acc_device[3];
|
||||
float linear_acc_world[3];
|
||||
// 在设备坐标系下，移除重力，得到线性加速度
|
||||
q_remove_gravity_with_quaternion(acc_device_ms2, quaternion_data, linear_acc_device);
|
||||
|
||||
// 将设备坐标系下的线性加速度，旋转到世界坐标系
|
||||
q_transform_to_world_with_quaternion(linear_acc_device, quaternion_data, linear_acc_world);
|
||||
// 将最终用于积分的加速度存入 tracker 结构体
|
||||
memcpy(tracker->acc_no_g, linear_acc_world, sizeof(linear_acc_world));
|
||||
|
||||
float acc_world_temp[3]; // 临时变量存储当前周期的加速度
|
||||
for (int i = 0; i < 2; i++) { // 只处理水平方向的 x 和 y 轴
|
||||
|
||||
// --- 核心修改：颠倒滤波器顺序为 HPF -> LPF ---
|
||||
|
||||
// 1. 高通滤波 (HPF) 先行: 消除因姿态误差导致的重力泄漏（直流偏置）
|
||||
// HPF的瞬态响应会产生尖峰，这是正常的。
|
||||
tracker->acc_world_filtered[i] = HPF_ALPHA * (tracker->acc_world_filtered[i] + tracker->acc_no_g[i] - tracker->acc_world_unfiltered_prev[i]);
|
||||
tracker->acc_world_unfiltered_prev[i] = tracker->acc_no_g[i];
|
||||
|
||||
// 2. 低通滤波 (LPF) 殿后: 平滑掉HPF产生的尖峰和传感器自身的高频振动噪声。
|
||||
// 这里使用 tracker->acc_world_filtered[i] 作为LPF的输入。
|
||||
tracker->acc_world_lpf[i] = (1.0f - LPF_ALPHA) * tracker->acc_world_filtered[i] + LPF_ALPHA * tracker->acc_world_lpf[i];
|
||||
|
||||
// 将最终处理完的加速度值存入临时变量
|
||||
acc_world_temp[i] = tracker->acc_world_lpf[i];
|
||||
}
|
||||
|
||||
// 计算处理后加速度的水平模长
|
||||
float acc_horizontal_mag = sqrtf(acc_world_temp[0] * acc_world_temp[0] +
|
||||
acc_world_temp[1] * acc_world_temp[1]);
|
||||
#if XTELL_TEST
|
||||
debug2.acc_magnitude = acc_horizontal_mag;
|
||||
#endif
|
||||
// 应用死区，并积分
|
||||
if (acc_horizontal_mag > ACC_DEAD_ZONE_THRESHOLD) {
|
||||
tracker->velocity[0] += acc_world_temp[0] * dt;
|
||||
tracker->velocity[1] += acc_world_temp[1] * dt;
|
||||
}
|
||||
|
||||
// 更新速度和距离
|
||||
tracker->speed = sqrtf(tracker->velocity[0] * tracker->velocity[0] +
|
||||
tracker->velocity[1] * tracker->velocity[1]);
|
||||
tracker->distance += tracker->speed * dt;
|
||||
break;
|
||||
case CONSTANT_SPEED:
|
||||
//保持上次的速度不变。只更新距离
|
||||
tracker->distance += tracker->speed * dt;
|
||||
break;
|
||||
case STATIC:
|
||||
case WOBBLE:
|
||||
// 速度清零，抑制漂移
|
||||
memset(tracker->velocity, 0, sizeof(tracker->velocity));
|
||||
tracker->speed = 0.0f;
|
||||
memset(tracker->acc_world_unfiltered_prev, 0, sizeof(tracker->acc_world_unfiltered_prev));
|
||||
memset(tracker->acc_world_filtered, 0, sizeof(tracker->acc_world_filtered));
|
||||
memset(tracker->acc_world_lpf, 0, sizeof(tracker->acc_world_lpf)); // 清理新增的LPF状态
|
||||
#if XTELL_TEST
|
||||
debug2.acc_magnitude = 0;
|
||||
#endif
|
||||
break;
|
||||
case FALLEN:
|
||||
// TODO
|
||||
break;
|
||||
default:
|
||||
break;
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
#if 1
|
||||
float linear_acc_device[3];
|
||||
float linear_acc_world[3];
|
||||
float tmp_world_acc[3];
|
||||
// 在设备坐标系下，移除重力，得到线性加速度
|
||||
q_remove_gravity_with_quaternion(acc_device_ms2, quaternion_data, linear_acc_device);
|
||||
|
||||
// 将设备坐标系下的线性加速度，旋转到世界坐标系
|
||||
q_transform_to_world_with_quaternion(linear_acc_device, quaternion_data, tmp_world_acc);
|
||||
|
||||
|
||||
float all_world_mag = sqrtf(tmp_world_acc[0] * tmp_world_acc[0] +
|
||||
tmp_world_acc[1] * tmp_world_acc[1] +
|
||||
tmp_world_acc[2] * tmp_world_acc[2]);
|
||||
|
||||
static int count = 0;
|
||||
if(count > 100){
|
||||
xlog("===original(g): x %.2f, y %.2f, z %.2f===\n",acc_g[0],acc_g[1],acc_g[2]);
|
||||
xlog("===world(m/s^2) no g: x %.2f, y %.2f, z %.2f, all %.2f===\n",tmp_world_acc[0],tmp_world_acc[1],tmp_world_acc[2],all_world_mag); //去掉重力加速度
|
||||
xlog("===gyr(dps) : x %.2f, y %.2f, z %.2f===\n",gyr_dps[0],gyr_dps[1],gyr_dps[2]); //angle
|
||||
xlog("===angle : x %.2f, y %.2f, z %.2f,===\n",angle[0],angle[1],angle[2]);
|
||||
count = 0;
|
||||
|
||||
}
|
||||
count++;
|
||||
#endif
|
||||
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
/**
|
||||
* @brief 滑雪数据计算
|
||||
*
|
||||
* @param acc_data_buf 传入的三轴加速度数据
|
||||
* @param gyr_data_buf 传入的三轴陀螺仪数据
|
||||
* @param angle_data 传入的欧若拉角数据
|
||||
* @return BLE_send_data_t 要发送给蓝牙的数据
|
||||
*/
|
||||
BLE_send_data_t sensor_processing_task(signed short* acc_data_buf, signed short* gyr_data_buf, float* angle_data, float* quaternion) {
|
||||
|
||||
static int initialized = 0;
|
||||
static float acc_data_g[3];
|
||||
static float gyr_data_dps[3];
|
||||
|
||||
if(quaternion != NULL){
|
||||
memcpy(quaternion_data, quaternion, 4 * sizeof(float));
|
||||
}
|
||||
|
||||
// const float delta_time = DELTA_TIME+0.01f;
|
||||
// const float delta_time = DELTA_TIME + 0.005f;
|
||||
const float delta_time = DELTA_TIME;
|
||||
BLE_send_data_t BLE_send_data;
|
||||
|
||||
if (!initialized) {
|
||||
skiing_tracker_init(&my_skiing_tracker);
|
||||
initialized = 1;
|
||||
printf("Skiing Tracker Initialized. Waiting for sensor calibration...\n");
|
||||
}
|
||||
|
||||
|
||||
#if ACC_RANGE==2
|
||||
// 加速度 LSB to g
|
||||
acc_data_g[0] = (float)acc_data_buf[0] / 16384.0f;
|
||||
acc_data_g[1] = (float)acc_data_buf[1] / 16384.0f;
|
||||
acc_data_g[2] = (float)acc_data_buf[2] / 16384.0f;
|
||||
#endif
|
||||
|
||||
#if ACC_RANGE==4
|
||||
// 加速度 LSB to g
|
||||
acc_data_g[0] = (float)acc_data_buf[0] / 8192.0f;
|
||||
acc_data_g[1] = (float)acc_data_buf[1] / 8192.0f;
|
||||
acc_data_g[2] = (float)acc_data_buf[2] / 8192.0f;
|
||||
#endif
|
||||
|
||||
#if ACC_RANGE==8
|
||||
//±8g 4096
|
||||
acc_data_g[0] = (float)acc_data_buf[0] / 4096.0f; //ax
|
||||
acc_data_g[1] = (float)acc_data_buf[1] / 4096.0f; //ay
|
||||
acc_data_g[2] = (float)acc_data_buf[2] / 4096.0f; //az
|
||||
#endif
|
||||
|
||||
#if ACC_RANGE==16
|
||||
//±16g 2048
|
||||
acc_data_g[0] = (float)acc_data_buf[0] / 2048.0f; //ax
|
||||
acc_data_g[1] = (float)acc_data_buf[1] / 2048.0f; //ay
|
||||
acc_data_g[2] = (float)acc_data_buf[2] / 2048.0f; //az
|
||||
#endif
|
||||
|
||||
// 陀螺仪 LSB to dps (度/秒)
|
||||
// ±2000dps量程下，转换系数约为 0.061
|
||||
gyr_data_dps[0] = (float)gyr_data_buf[0] * 0.061f;
|
||||
gyr_data_dps[1] = (float)gyr_data_buf[1] * 0.061f;
|
||||
gyr_data_dps[2] = (float)gyr_data_buf[2] * 0.061f;
|
||||
|
||||
skiing_tracker_update(&my_skiing_tracker, acc_data_g, gyr_data_dps, angle_data, delta_time);
|
||||
|
||||
BLE_send_data.skiing_state = my_skiing_tracker.state;
|
||||
for (int i = 0; i < 3; i++) {
|
||||
#ifdef XTELL_TEST
|
||||
BLE_send_data.acc_data[i] = (short)(acc_data_g[i] * 9.8f) * 100; //cm/^s2
|
||||
BLE_send_data.gyr_data[i] = (short)gyr_data_dps[i]; //dps
|
||||
BLE_send_data.angle_data[i] = angle_data[i];
|
||||
#else
|
||||
BLE_send_data.acc_data[i] = (short)acc_data_buf[i]; //原始adc数据
|
||||
BLE_send_data.gyr_data[i] = (short)gyr_data_buf[i]; //原始adc数据
|
||||
BLE_send_data.angle_data[i] = angle_data[i];
|
||||
#endif
|
||||
}
|
||||
BLE_send_data.speed_cms = (int)(my_skiing_tracker.speed * 100);
|
||||
BLE_send_data.distance_cm = (int)(my_skiing_tracker.distance * 100);
|
||||
// printf("Calculate the time interval =============== end\n");
|
||||
|
||||
return BLE_send_data;
|
||||
}
|
||||
|
||||
88
apps/earphone/xtell_Sensor/A_hide/calculate/skiing_tracker.h
Normal file
88
apps/earphone/xtell_Sensor/A_hide/calculate/skiing_tracker.h
Normal file
@ -0,0 +1,88 @@
|
||||
#ifndef SKIING_TRACKER_H
|
||||
#define SKIING_TRACKER_H
|
||||
|
||||
#include "../xtell.h"
|
||||
// 定义滑雪者可能的状态
|
||||
typedef enum {
|
||||
STATIC, // 静止或动态稳定：0
|
||||
NO_CONSTANT_SPEED, // 正在滑雪，非匀速：1
|
||||
CONSTANT_SPEED, // 正在滑雪，匀速：2
|
||||
WOBBLE, // 正在原地旋转：3
|
||||
WHEEL, // 转弯：4
|
||||
FALLEN, // 已摔倒：5
|
||||
TAKING_OFF, // 起跳冲击阶段：6
|
||||
IN_AIR, // 空中失重阶段：7
|
||||
LANDING, // 落地冲击阶段：8
|
||||
STOP_DETECTION, // 停止检测：9
|
||||
UNKNOWN // 未知状态：10
|
||||
} skiing_state_t;
|
||||
|
||||
#define VARIANCE_BUFFER_SIZE 5 // 用于计算方差的数据窗口大小 (5个样本 @ 100Hz = 50ms)，减小延迟，提高实时性
|
||||
#define DELTA_TIME 0.01f
|
||||
|
||||
|
||||
// 追踪器数据结构体
|
||||
typedef struct {
|
||||
// 公开数据
|
||||
float velocity[3]; // 当前速度 (x, y, z)，单位: m/s
|
||||
float distance; // 总滑行距离，单位: m
|
||||
float speed; // 当前速率 (标量)，单位: m/s
|
||||
skiing_state_t state; // 当前滑雪状态
|
||||
|
||||
// 内部计算使用的私有成员
|
||||
float acc_no_g[3]; // 去掉重力分量后的加速度
|
||||
|
||||
// 用于空中距离计算
|
||||
float time_in_air; // 滞空时间计时器
|
||||
float initial_velocity_on_takeoff[3]; // 起跳瞬间的速度向量
|
||||
int airborne_entry_counter; // 进入空中状态的确认计数器
|
||||
int grounded_entry_counter; // 落地确认计数器
|
||||
|
||||
// --- 内部计算使用的私有成员 ---
|
||||
// 用于动态零速更新和旋转检测的缓冲区
|
||||
float acc_buffer[VARIANCE_BUFFER_SIZE][3]; // 加速度数据窗口
|
||||
float gyr_buffer[VARIANCE_BUFFER_SIZE][3]; // 角速度数据窗口
|
||||
int buffer_index; // 缓冲区当前索引
|
||||
int buffer_filled; // 缓冲区是否已填满的标志
|
||||
|
||||
// 用于高通滤波器（巴特沃斯一阶滤波器）的私有成员，以消除加速度的直流偏置
|
||||
float acc_world_filtered[3]; //过滤过的
|
||||
float acc_world_unfiltered_prev[3]; //上一次没过滤的
|
||||
|
||||
float acc_world_lpf[3]; // 经过低通滤波后的世界坐标系加速度
|
||||
} skiing_tracker_t;
|
||||
|
||||
//ble发送的数据
|
||||
typedef struct{ //__attribute__((packed)){ //该结构体取消内存对齐
|
||||
char sensor_state;
|
||||
char skiing_state;
|
||||
int speed_cms; //求出的速度，cm/s
|
||||
int distance_cm; //求出的距离，cm
|
||||
short acc_data[3]; //三轴加速度, g
|
||||
short gyr_data[3]; //三轴陀螺仪, dps
|
||||
float angle_data[3]; //欧若拉角
|
||||
}BLE_send_data_t;
|
||||
|
||||
typedef struct{
|
||||
int acc_KS[3]; //卡尔曼后，LSB转换后的 三轴加速度数据（cm/s^2）
|
||||
int gyr_KS_dps[3]; //卡尔曼后，LSB to dps 三轴陀螺仪数据
|
||||
int angle_KS[3]; //卡尔曼后，计算得到的欧若拉角数据
|
||||
}BLE_KS_send_data_t;
|
||||
|
||||
#ifdef XTELL_TEST
|
||||
typedef struct{
|
||||
float acc_variance; //三轴加速度方差之和
|
||||
float gyr_variance; //三轴陀螺仪方差之和
|
||||
float acc_magnitude; //三轴加速度模长
|
||||
float gyr_magnitude; //三轴陀螺仪模长
|
||||
}debug_t;
|
||||
#endif
|
||||
/**
|
||||
* @brief 初始化滑雪追踪器
|
||||
*
|
||||
* @param tracker 指向 skiing_tracker_t 结构体的指针
|
||||
*/
|
||||
void skiing_tracker_init(skiing_tracker_t *tracker);
|
||||
|
||||
BLE_send_data_t sensor_processing_task(signed short* acc_data_buf, signed short* gyr_data_buf, float* angle_data, float* quaternion);
|
||||
#endif // SKIING_TRACKER_H
|
||||
@ -1,8 +1,12 @@
|
||||
/*
|
||||
发送数据给上位机的,需要将log打印出口关闭
|
||||
*/
|
||||
|
||||
#include "ano_protocol.h"
|
||||
#include "asm/uart_dev.h"
|
||||
#include "app_config.h" // 需要包含这个头文件来获取 TCFG_ONLINE_TX_PORT 等宏定义
|
||||
#include "app_config.h"
|
||||
|
||||
// 定义匿名协议常量
|
||||
// 定义协议常量
|
||||
#define ANO_FRAME_HEADER 0xAA
|
||||
#define ANO_TO_COMPUTER_ADDR 0xFF
|
||||
|
||||
@ -10,7 +14,7 @@
|
||||
static const uart_bus_t *ano_uart_dev = NULL;
|
||||
|
||||
/**
|
||||
* @brief 计算并填充匿名协议的校验和
|
||||
* @brief 计算并填充协议的校验和
|
||||
* @param frame_buffer 指向数据帧缓冲区的指针
|
||||
*/
|
||||
static void ano_calculate_checksum(u8 *frame_buffer) {
|
||||
@ -35,7 +39,7 @@ static void ano_calculate_checksum(u8 *frame_buffer) {
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 初始化用于匿名上位机通信的串口
|
||||
* @brief 初始化用于上位机通信的串口
|
||||
*/
|
||||
int ano_protocol_init(u32 baudrate) {
|
||||
#if TCFG_UART0_ENABLE==0
|
||||
@ -47,7 +51,6 @@ int ano_protocol_init(u32 baudrate) {
|
||||
struct uart_platform_data_t ut_arg = {0};
|
||||
|
||||
// TCFG_ONLINE_TX_PORT 和 TCFG_ONLINE_RX_PORT 通常在 app_config.h 中定义
|
||||
// 请确保您的 app_config.h 中有正确的引脚配置
|
||||
ut_arg.tx_pin = TCFG_ONLINE_TX_PORT;
|
||||
ut_arg.rx_pin = (u8)-1; // -1 表示不使用该引脚，因为我们只发送数据
|
||||
ut_arg.baud = baudrate;
|
||||
|
||||
@ -1,14 +1,11 @@
|
||||
/*
|
||||
|
||||
使用四元数求角度和去掉重力分量
|
||||
*/
|
||||
#include "skiing_tracker.h"
|
||||
#include "../sensor/SC7U22.h"
|
||||
#include <math.h>
|
||||
#include <string.h>
|
||||
|
||||
#define G_ACCELERATION 9.81f
|
||||
#define DEG_TO_RAD (3.14159265f / 180.0f)
|
||||
|
||||
#define ENABLE_XLOG 1
|
||||
#ifdef xlog
|
||||
#undef xlog
|
||||
@ -19,83 +16,13 @@
|
||||
#define xlog(format, ...) ((void)0)
|
||||
#endif
|
||||
|
||||
// --- 静止检测 ---
|
||||
//两个判断是否静止的必要条件：动态零速更新(ZUPT)阈值
|
||||
// 加速方差阈值，提高阈值，让“刹车”更灵敏，以便在波浪式前进等慢速漂移时也能触发零速更新
|
||||
#define STOP_ACC_VARIANCE_THRESHOLD 0.2f
|
||||
// 陀螺仪方差阈值
|
||||
#define STOP_GYR_VARIANCE_THRESHOLD 5.0f
|
||||
// 静止时候的陀螺仪模长
|
||||
#define STOP_GYR_MAG_THRESHOLD 15
|
||||
// --- --- ---
|
||||
|
||||
// --- 启动滑雪阈值 ---
|
||||
// 加速度模长与重力的差值大于此值，认为开始运动；降低阈值，让“油门”更灵敏，以便能捕捉到真实的慢速启动
|
||||
#define START_ACC_MAG_THRESHOLD 1.0f //0.5、1
|
||||
// 陀螺仪方差阈值，以允许启动瞬间的正常抖动，但仍能过滤掉混乱的、非滑雪的晃动。
|
||||
#define START_GYR_VARIANCE_THRESHOLD 15.0f
|
||||
// --- --- ---
|
||||
#define G_ACCELERATION 9.81f
|
||||
#define DEG_TO_RAD (3.14159265f / 180.0f)
|
||||
|
||||
// --- 滑雪过程 ---
|
||||
//加速度 模长(不含重力），低于此值视为 在做匀速运动
|
||||
#define SKIING_ACC_MAG_THRESHOLD 0.5f
|
||||
//陀螺仪 模长，高于此值视为 摔倒了
|
||||
#define FALLEN_GRY_MAG_THRESHOLD 2000.0f //未确定
|
||||
// --- --- ---
|
||||
|
||||
// --- 原地旋转抖动 ---
|
||||
// 加速度 方差 阈值。此值比 静止检测 阈值更宽松，
|
||||
#define WOBBLE_ACC_VARIANCE_THRESHOLD 0.5f
|
||||
// 加速度 模长 阈值
|
||||
#define WOBBLE_ACC_MAG_THRESHOLD 1.0f
|
||||
// 角速度 总模长 大于此值(度/秒)，认为正在进行非滑雪的旋转或摆动
|
||||
#define ROTATION_GYR_MAG_THRESHOLD 30.0f
|
||||
// --- --- ---
|
||||
|
||||
// --- 滑雪转弯动 ---
|
||||
// 加速度 方差 阈值，大于此值，滑雪过程可能发生了急转弯
|
||||
#define WHEEL_ACC_VARIANCE_THRESHOLD 7.0f
|
||||
// 角速度 总模长 大于此值(度/秒)，认为滑雪过程中进行急转弯
|
||||
#define WHEEL_GYR_MAG_THRESHOLD 500.0f //
|
||||
// --- --- ---
|
||||
|
||||
// --- 跳跃 ---
|
||||
// 加速度模长低于此值(g)，认为进入失重状态(IN_AIR)
|
||||
#define AIRBORNE_ACC_MAG_LOW_THRESHOLD 0.4f
|
||||
// 加速度模长高于此值(g)，认为发生落地冲击(LANDING)
|
||||
#define LANDING_ACC_MAG_HIGH_THRESHOLD 3.5f
|
||||
// 起跳加速度阈值(g)，用于进入TAKING_OFF状态
|
||||
#define TAKEOFF_ACC_MAG_HIGH_THRESHOLD 1.8f
|
||||
// 进入空中状态确认计数：需要连续3个采样点加速度低于阈值才判断为起跳
|
||||
#define AIRBORNE_CONFIRM_COUNT 3
|
||||
// 落地状态确认计数：加速度恢复到1g附近并持续2个采样点(20ms)则认为已落地
|
||||
#define GROUNDED_CONFIRM_COUNT 2
|
||||
// 最大滞空时间(秒)，超过此时间强制认为已落地，防止状态锁死
|
||||
#define MAX_TIME_IN_AIR 12.5f
|
||||
// --- --- ---
|
||||
|
||||
// --- 用于消除积分漂移的滤波器和阈值 ---
|
||||
// 高通滤波器系数 (alpha)。alpha 越接近1，滤除低频(直流偏移)的效果越强，但可能滤掉真实的慢速运动。
|
||||
// alpha = RC / (RC + dt)，参考RC电路而来，fc ≈ (1 - alpha) / (2 * π * dt)
|
||||
#define HPF_ALPHA 0.999f
|
||||
//0.995f： 0.08 Hz 的信号
|
||||
//0.999f： 0.0159 Hz
|
||||
// --- --- ---
|
||||
|
||||
// --- 低通滤波器 ---
|
||||
// 低通滤波器系数 (alpha)。alpha 越小，滤波效果越强（更平滑），但延迟越大。
|
||||
// alpha 推荐范围 0.7 ~ 0.95。可以从 0.85 开始尝试。
|
||||
#define LPF_ALPHA 0.7f
|
||||
|
||||
// 加速度死区阈值 (m/s^2)。低于此阈值的加速度被认为是噪声，不参与积分。
|
||||
// 设得太高会忽略真实的慢速启动，设得太低则无法有效抑制噪声。
|
||||
//参考：0.2f ~ 0.4f
|
||||
#define ACC_DEAD_ZONE_THRESHOLD 0.05f
|
||||
|
||||
// --- 模拟摩擦力，进行速度衰减 ---
|
||||
#define SPEED_ATTENUATION 1.0f //暂不模拟
|
||||
BLE_KS_send_data_t KS_data;
|
||||
static float quaternion_data[4];
|
||||
|
||||
#ifdef XTELL_TEST
|
||||
|
||||
debug_t debug1;
|
||||
@ -126,7 +53,7 @@ void stop_detection(void){
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 初始化滑雪追踪器
|
||||
* @brief 初始化
|
||||
*
|
||||
* @param tracker
|
||||
*/
|
||||
@ -140,18 +67,6 @@ void skiing_tracker_init(skiing_tracker_t *tracker)
|
||||
tracker->state = STATIC;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 当检测到落地时，计算空中的水平飞行距离并累加到总距离
|
||||
*/
|
||||
static void calculate_air_distance(skiing_tracker_t *tracker) {
|
||||
float horizontal_speed_on_takeoff = sqrtf(
|
||||
tracker->initial_velocity_on_takeoff[0] * tracker->initial_velocity_on_takeoff[0] +
|
||||
tracker->initial_velocity_on_takeoff[1] * tracker->initial_velocity_on_takeoff[1]
|
||||
);
|
||||
float distance_in_air = horizontal_speed_on_takeoff * tracker->time_in_air;
|
||||
tracker->distance += distance_in_air;
|
||||
}
|
||||
|
||||
|
||||
|
||||
/**
|
||||
@ -178,7 +93,7 @@ void q_remove_gravity_with_quaternion(const float *acc_device, const float *q, f
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 使用四元数将设备坐标系的线性加速度转换到世界坐标系
|
||||
* @brief 使用四元数将设备坐标系的线性加速度转换到世界坐标系,并且移除重力分量
|
||||
* @details 同样，此方法比使用欧拉角更优。
|
||||
* @param acc_linear_device 输入：设备坐标系下的线性加速度 [x, y, z]
|
||||
* @param q 输入：表示姿态的四元数 [w, x, y, z]
|
||||
@ -202,208 +117,6 @@ void q_transform_to_world_with_quaternion(const float *acc_linear_device, const
|
||||
}
|
||||
|
||||
|
||||
|
||||
/**
|
||||
* @brief 计算缓冲区内三轴数据的方差之和
|
||||
*
|
||||
* @param buffer 传进来的三轴数据：陀螺仪/加速度
|
||||
* @return float 返回方差和
|
||||
*/
|
||||
static float calculate_variance(float buffer[VARIANCE_BUFFER_SIZE][3])
|
||||
{
|
||||
float mean[3] = {0};
|
||||
float variance[3] = {0};
|
||||
|
||||
// 计算均值
|
||||
for (int i = 0; i < VARIANCE_BUFFER_SIZE; i++) {
|
||||
mean[0] += buffer[i][0];
|
||||
mean[1] += buffer[i][1];
|
||||
mean[2] += buffer[i][2];
|
||||
}
|
||||
mean[0] /= VARIANCE_BUFFER_SIZE;
|
||||
mean[1] /= VARIANCE_BUFFER_SIZE;
|
||||
mean[2] /= VARIANCE_BUFFER_SIZE;
|
||||
|
||||
// 计算方差
|
||||
for (int i = 0; i < VARIANCE_BUFFER_SIZE; i++) {
|
||||
variance[0] += (buffer[i][0] - mean[0]) * (buffer[i][0] - mean[0]);
|
||||
variance[1] += (buffer[i][1] - mean[1]) * (buffer[i][1] - mean[1]);
|
||||
variance[2] += (buffer[i][2] - mean[2]) * (buffer[i][2] - mean[2]);
|
||||
}
|
||||
variance[0] /= VARIANCE_BUFFER_SIZE;
|
||||
variance[1] /= VARIANCE_BUFFER_SIZE;
|
||||
variance[2] /= VARIANCE_BUFFER_SIZE;
|
||||
|
||||
// 返回三轴方差之和，作为一个综合的稳定度指标
|
||||
return variance[0] + variance[1] + variance[2];
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 摩擦力模拟，进行速度衰减
|
||||
*
|
||||
* @param tracker
|
||||
*/
|
||||
void forece_of_friction(skiing_tracker_t *tracker){
|
||||
// 增加速度衰减，模拟摩擦力
|
||||
tracker->velocity[0] *= SPEED_ATTENUATION;
|
||||
tracker->velocity[1] *= SPEED_ATTENUATION;
|
||||
tracker->velocity[2] = 0; // 垂直速度强制归零
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 状态机更新
|
||||
*
|
||||
* @param tracker 传入同步修改后传出
|
||||
* @param acc_device_ms2 三轴加速度，m/s^2
|
||||
* @param gyr_dps 三轴陀螺仪，dps
|
||||
*/
|
||||
static void update_state_machine(skiing_tracker_t *tracker, const float *acc_device_ms2, const float *gyr_dps)
|
||||
{
|
||||
// 缓冲区未填满时，不进行状态判断，默认为静止
|
||||
if (!tracker->buffer_filled) {
|
||||
tracker->state = STATIC;
|
||||
return;
|
||||
}
|
||||
|
||||
// --- 计算关键指标 ---
|
||||
float acc_variance = calculate_variance(tracker->acc_buffer); // 计算加速度方差
|
||||
float gyr_variance = calculate_variance(tracker->gyr_buffer); // 计算陀螺仪方差
|
||||
float gyr_magnitude = sqrtf(gyr_dps[0]*gyr_dps[0] + gyr_dps[1]*gyr_dps[1] + gyr_dps[2]*gyr_dps[2]); //dps
|
||||
float acc_magnitude = sqrtf(acc_device_ms2[0]*acc_device_ms2[0] + acc_device_ms2[1]*acc_device_ms2[1] + acc_device_ms2[2]*acc_device_ms2[2]); //m/s^s
|
||||
float acc_magnitude_g = acc_magnitude / G_ACCELERATION; // 转换为g单位，用于跳跃判断
|
||||
|
||||
#ifdef XTELL_TEST
|
||||
debug1.acc_variance =acc_variance;
|
||||
debug1.gyr_variance =gyr_variance;
|
||||
debug1.gyr_magnitude=gyr_magnitude;
|
||||
debug1.acc_magnitude=fabsf(acc_magnitude - G_ACCELERATION);
|
||||
#endif
|
||||
|
||||
|
||||
|
||||
// --- 状态机逻辑 (核心修改区域) ---
|
||||
|
||||
#if 0 //暂时不考虑空中
|
||||
// 1. 空中/落地状态的后续处理
|
||||
if (tracker->state == IN_AIR) {
|
||||
// A. 检测巨大冲击 -> 落地
|
||||
if (acc_magnitude_g > LANDING_ACC_MAG_HIGH_THRESHOLD) {
|
||||
tracker->state = LANDING;
|
||||
// B. 检测超时 -> 强制落地 (安全机制)
|
||||
} else if (tracker->time_in_air > MAX_TIME_IN_AIR) {
|
||||
tracker->state = LANDING;
|
||||
// C. 检测恢复正常重力 (平缓落地)
|
||||
} else if (acc_magnitude_g > 0.8f && acc_magnitude_g < 1.5f) {
|
||||
tracker->grounded_entry_counter++;
|
||||
if (tracker->grounded_entry_counter >= GROUNDED_CONFIRM_COUNT) {
|
||||
tracker->state = LANDING;
|
||||
}
|
||||
} else {
|
||||
tracker->grounded_entry_counter = 0;
|
||||
}
|
||||
return; // 在空中或刚切换到落地，结束本次状态判断
|
||||
}
|
||||
|
||||
// 2. 严格的 "起跳->空中" 状态转换逻辑
|
||||
// 只有当处于滑行状态时，才去检测起跳意图
|
||||
if (tracker->state == NO_CONSTANT_SPEED || tracker->state == CONSTANT_SPEED || tracker->state == WHEEL) {
|
||||
if (acc_magnitude_g > TAKEOFF_ACC_MAG_HIGH_THRESHOLD) {
|
||||
tracker->state = TAKING_OFF;
|
||||
tracker->airborne_entry_counter = 0; // 准备检测失重
|
||||
return;
|
||||
}
|
||||
}
|
||||
// 只有在TAKING_OFF状态下，才去检测是否进入失重
|
||||
if (tracker->state == TAKING_OFF) {
|
||||
if (acc_magnitude_g < AIRBORNE_ACC_MAG_LOW_THRESHOLD) {
|
||||
tracker->airborne_entry_counter++;
|
||||
if (tracker->airborne_entry_counter >= AIRBORNE_CONFIRM_COUNT) {
|
||||
memcpy(tracker->initial_velocity_on_takeoff, tracker->velocity, sizeof(tracker->velocity));
|
||||
tracker->time_in_air = 0;
|
||||
tracker->state = IN_AIR;
|
||||
tracker->airborne_entry_counter = 0;
|
||||
tracker->grounded_entry_counter = 0;
|
||||
return;
|
||||
}
|
||||
} else {
|
||||
// 如果在起跳冲击后一段时间内没有失重，说明只是一个颠簸，恢复滑行
|
||||
// 可以加一个小的超时计数器，这里为了简单先直接恢复
|
||||
tracker->state = NO_CONSTANT_SPEED;
|
||||
}
|
||||
return; // 无论是否切换，都结束本次判断
|
||||
}
|
||||
#endif
|
||||
|
||||
|
||||
// --- 静止判断 ---
|
||||
if (acc_variance < STOP_ACC_VARIANCE_THRESHOLD && gyr_variance < STOP_GYR_VARIANCE_THRESHOLD && gyr_magnitude < STOP_GYR_MAG_THRESHOLD) {
|
||||
tracker->state = STATIC;
|
||||
return;
|
||||
}
|
||||
|
||||
// --- 地面状态切换逻辑 ---
|
||||
switch (tracker->state) {
|
||||
case LANDING:
|
||||
tracker->state = STATIC;
|
||||
break;
|
||||
case STATIC:
|
||||
// 优先判断是否进入 WOBBLE 状态
|
||||
// 条件：陀螺仪活动剧烈，但整体加速度变化不大（说明是原地转或晃）
|
||||
if (gyr_magnitude > ROTATION_GYR_MAG_THRESHOLD && fabsf(acc_magnitude - G_ACCELERATION) < WOBBLE_ACC_MAG_THRESHOLD) {
|
||||
tracker->state = WOBBLE;
|
||||
}
|
||||
// 只有在陀螺仪和加速度都满足“前进”特征时，才启动
|
||||
else if (gyr_variance > START_GYR_VARIANCE_THRESHOLD && fabsf(acc_magnitude - G_ACCELERATION) > START_ACC_MAG_THRESHOLD) {
|
||||
tracker->state = NO_CONSTANT_SPEED;
|
||||
}
|
||||
break;
|
||||
|
||||
case WOBBLE:
|
||||
// 从 WOBBLE 状态启动的条件应该和从 STATIC 启动一样严格
|
||||
if (gyr_variance < START_GYR_VARIANCE_THRESHOLD * 2 && fabsf(acc_magnitude - G_ACCELERATION) > START_ACC_MAG_THRESHOLD) {
|
||||
tracker->state = NO_CONSTANT_SPEED;
|
||||
}
|
||||
// 如果陀螺仪活动减弱，则可能恢复静止
|
||||
else if (gyr_magnitude < ROTATION_GYR_MAG_THRESHOLD * 0.8f) { // 增加迟滞，避免抖动
|
||||
// 不直接跳回STATIC，而是依赖下一轮的全局静止判断
|
||||
}
|
||||
break;
|
||||
case NO_CONSTANT_SPEED: //非匀速状态
|
||||
//暂时不考虑摔倒
|
||||
// if (gyr_magnitude > FALLEN_GRY_MAG_THRESHOLD) {
|
||||
// tracker->state = FALLEN; //摔倒
|
||||
// } else
|
||||
if (gyr_magnitude > WHEEL_GYR_MAG_THRESHOLD && acc_variance > WHEEL_ACC_VARIANCE_THRESHOLD) {
|
||||
tracker->state = WHEEL; //转弯
|
||||
} else if (fabsf(acc_magnitude - G_ACCELERATION) < SKIING_ACC_MAG_THRESHOLD) {
|
||||
tracker->state = CONSTANT_SPEED; //匀速
|
||||
}
|
||||
break;
|
||||
|
||||
case CONSTANT_SPEED: //匀速状态
|
||||
if (fabsf(acc_magnitude - G_ACCELERATION) > START_ACC_MAG_THRESHOLD) {
|
||||
tracker->state = NO_CONSTANT_SPEED;
|
||||
}
|
||||
//TODO：可以添加进入转弯或摔倒的判断
|
||||
break;
|
||||
|
||||
case WHEEL:
|
||||
// 从转弯状态，检查转弯是否结束
|
||||
// 如果角速度和加速度方差都降下来了，就回到普通滑行状态
|
||||
if (gyr_magnitude < WHEEL_GYR_MAG_THRESHOLD * 0.8f && acc_variance < WHEEL_ACC_VARIANCE_THRESHOLD * 0.8f) { // 乘以一个滞后系数避免抖动
|
||||
tracker->state = NO_CONSTANT_SPEED;
|
||||
}
|
||||
break;
|
||||
|
||||
case FALLEN:
|
||||
// TODO：回到 STATIC
|
||||
break;
|
||||
}
|
||||
|
||||
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* @brief 主更新函数
|
||||
*
|
||||
@ -427,139 +140,46 @@ void skiing_tracker_update(skiing_tracker_t *tracker, float *acc_g, float *gyr_d
|
||||
acc_device_ms2[1] = acc_g[1] * G_ACCELERATION;
|
||||
acc_device_ms2[2] = acc_g[2] * G_ACCELERATION;
|
||||
|
||||
// 将最新数据存入缓冲区
|
||||
memcpy(tracker->acc_buffer[tracker->buffer_index], acc_device_ms2, sizeof(acc_device_ms2));
|
||||
memcpy(tracker->gyr_buffer[tracker->buffer_index], gyr_dps, 3 * sizeof(float));
|
||||
|
||||
tracker->buffer_index++;
|
||||
if (tracker->buffer_index >= VARIANCE_BUFFER_SIZE) {
|
||||
tracker->buffer_index = 0;
|
||||
tracker->buffer_filled = 1; // 标记缓冲区已满
|
||||
}
|
||||
|
||||
// --- 更新状态机 ---
|
||||
update_state_machine(tracker, acc_device_ms2, gyr_dps);
|
||||
|
||||
// --- 根据状态执行不同的计算逻辑 ---
|
||||
switch (tracker->state) {
|
||||
case TAKING_OFF:
|
||||
tracker->speed = 0.0f;
|
||||
break;
|
||||
case IN_AIR:
|
||||
// 在空中时，只累加滞空时间
|
||||
tracker->time_in_air += dt;
|
||||
break;
|
||||
case LANDING:
|
||||
// 刚落地，计算空中距离
|
||||
calculate_air_distance(tracker);
|
||||
// 清理速度和滤波器状态，为恢复地面追踪做准备
|
||||
memset(tracker->velocity, 0, sizeof(tracker->velocity));
|
||||
tracker->speed = 0;
|
||||
memset(tracker->acc_world_unfiltered_prev, 0, sizeof(tracker->acc_world_unfiltered_prev));
|
||||
memset(tracker->acc_world_filtered, 0, sizeof(tracker->acc_world_filtered));
|
||||
memset(tracker->acc_world_lpf, 0, sizeof(tracker->acc_world_lpf)); // 清理新增的LPF状态
|
||||
break;
|
||||
case WHEEL:
|
||||
case NO_CONSTANT_SPEED:
|
||||
float linear_acc_device[3];
|
||||
float linear_acc_world[3];
|
||||
// 在设备坐标系下，移除重力，得到线性加速度
|
||||
q_remove_gravity_with_quaternion(acc_device_ms2, quaternion_data, linear_acc_device);
|
||||
|
||||
// 将设备坐标系下的线性加速度，旋转到世界坐标系
|
||||
q_transform_to_world_with_quaternion(linear_acc_device, quaternion_data, linear_acc_world);
|
||||
// 将最终用于积分的加速度存入 tracker 结构体
|
||||
memcpy(tracker->acc_no_g, linear_acc_world, sizeof(linear_acc_world));
|
||||
|
||||
float acc_world_temp[3]; // 临时变量存储当前周期的加速度
|
||||
for (int i = 0; i < 2; i++) { // 只处理水平方向的 x 和 y 轴
|
||||
|
||||
// --- 核心修改：颠倒滤波器顺序为 HPF -> LPF ---
|
||||
|
||||
// 1. 高通滤波 (HPF) 先行: 消除因姿态误差导致的重力泄漏（直流偏置）
|
||||
// HPF的瞬态响应会产生尖峰，这是正常的。
|
||||
tracker->acc_world_filtered[i] = HPF_ALPHA * (tracker->acc_world_filtered[i] + tracker->acc_no_g[i] - tracker->acc_world_unfiltered_prev[i]);
|
||||
tracker->acc_world_unfiltered_prev[i] = tracker->acc_no_g[i];
|
||||
|
||||
// 2. 低通滤波 (LPF) 殿后: 平滑掉HPF产生的尖峰和传感器自身的高频振动噪声。
|
||||
// 这里使用 tracker->acc_world_filtered[i] 作为LPF的输入。
|
||||
tracker->acc_world_lpf[i] = (1.0f - LPF_ALPHA) * tracker->acc_world_filtered[i] + LPF_ALPHA * tracker->acc_world_lpf[i];
|
||||
|
||||
// 将最终处理完的加速度值存入临时变量
|
||||
acc_world_temp[i] = tracker->acc_world_lpf[i];
|
||||
}
|
||||
|
||||
// 计算处理后加速度的水平模长
|
||||
float acc_horizontal_mag = sqrtf(acc_world_temp[0] * acc_world_temp[0] +
|
||||
acc_world_temp[1] * acc_world_temp[1]);
|
||||
#if XTELL_TEST
|
||||
debug2.acc_magnitude = acc_horizontal_mag;
|
||||
#endif
|
||||
// 应用死区，并积分
|
||||
if (acc_horizontal_mag > ACC_DEAD_ZONE_THRESHOLD) {
|
||||
tracker->velocity[0] += acc_world_temp[0] * dt;
|
||||
tracker->velocity[1] += acc_world_temp[1] * dt;
|
||||
}
|
||||
|
||||
// 更新速度和距离
|
||||
tracker->speed = sqrtf(tracker->velocity[0] * tracker->velocity[0] +
|
||||
tracker->velocity[1] * tracker->velocity[1]);
|
||||
tracker->distance += tracker->speed * dt;
|
||||
break;
|
||||
case CONSTANT_SPEED:
|
||||
//保持上次的速度不变。只更新距离
|
||||
tracker->distance += tracker->speed * dt;
|
||||
break;
|
||||
case STATIC:
|
||||
case WOBBLE:
|
||||
// 速度清零，抑制漂移
|
||||
memset(tracker->velocity, 0, sizeof(tracker->velocity));
|
||||
tracker->speed = 0.0f;
|
||||
memset(tracker->acc_world_unfiltered_prev, 0, sizeof(tracker->acc_world_unfiltered_prev));
|
||||
memset(tracker->acc_world_filtered, 0, sizeof(tracker->acc_world_filtered));
|
||||
memset(tracker->acc_world_lpf, 0, sizeof(tracker->acc_world_lpf)); // 清理新增的LPF状态
|
||||
#if XTELL_TEST
|
||||
debug2.acc_magnitude = 0;
|
||||
#endif
|
||||
break;
|
||||
case FALLEN:
|
||||
// TODO
|
||||
break;
|
||||
default:
|
||||
break;
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
#if 1
|
||||
float linear_acc_device[3];
|
||||
float linear_acc_world[3];
|
||||
#if 1 //测试禁止状态下陀螺仪的三轴加速度，去掉重力分量后是否正常
|
||||
float tmp_device_acc[3];
|
||||
float tmp_world_acc[3];
|
||||
// 在设备坐标系下，移除重力，得到线性加速度
|
||||
q_remove_gravity_with_quaternion(acc_device_ms2, quaternion_data, linear_acc_device);
|
||||
// remove_gravity_in_device_frame(acc_device_ms2,angle,tmp_device_acc);
|
||||
// transform_acc_to_world_frame(acc_device_ms2,angle,tmp_world_acc);
|
||||
|
||||
// 将设备坐标系下的线性加速度，旋转到世界坐标系
|
||||
q_transform_to_world_with_quaternion(linear_acc_device, quaternion_data, tmp_world_acc);
|
||||
q_remove_gravity_with_quaternion(acc_device_ms2,quaternion_data,tmp_device_acc);
|
||||
q_transform_to_world_with_quaternion(tmp_device_acc,quaternion_data,tmp_world_acc);
|
||||
|
||||
// 计算处理后加速度的水平模长
|
||||
float all_device_mag = sqrtf(tmp_device_acc[0] * tmp_device_acc[0] +
|
||||
tmp_device_acc[1] * tmp_device_acc[1] +
|
||||
tmp_device_acc[2] * tmp_device_acc[2]);
|
||||
|
||||
|
||||
float all_world_mag = sqrtf(tmp_world_acc[0] * tmp_world_acc[0] +
|
||||
tmp_world_acc[1] * tmp_world_acc[1] +
|
||||
tmp_world_acc[2] * tmp_world_acc[2]);
|
||||
|
||||
|
||||
float gx_proj = 2.0f * (quaternion_data[1] * quaternion_data[3] - quaternion_data[0] * quaternion_data[2]);
|
||||
float gy_proj = 2.0f * (quaternion_data[0] * quaternion_data[1] + quaternion_data[2] * quaternion_data[3]);
|
||||
float gz_proj = quaternion_data[0] * quaternion_data[0] - quaternion_data[1] * quaternion_data[1] - quaternion_data[2] * quaternion_data[2] + quaternion_data[3] * quaternion_data[3];
|
||||
|
||||
|
||||
static int count = 0;
|
||||
if(count > 100){
|
||||
xlog("===original(g): x %.2f, y %.2f, z %.2f===\n",acc_g[0],acc_g[1],acc_g[2]);
|
||||
xlog("===world(m/s^2) no g: x %.2f, y %.2f, z %.2f, all %.2f===\n",tmp_world_acc[0],tmp_world_acc[1],tmp_world_acc[2],all_world_mag); //去掉重力加速度
|
||||
xlog("===device(m/s^2) no g: x %.2f, y %.2f, z %.2f, all %.2f===\n",tmp_device_acc[0],tmp_device_acc[1],tmp_device_acc[2],all_device_mag);
|
||||
xlog("===world(m/s^2) no g: x %.2f, y %.2f, z %.2f, all %.2f===\n",tmp_world_acc[0],tmp_world_acc[1],tmp_world_acc[2],all_world_mag);
|
||||
xlog("===gyr(dps) : x %.2f, y %.2f, z %.2f, all %.2f===\n",gyr_dps[0],gyr_dps[1],gyr_dps[2]); //angle
|
||||
xlog("===angle : x %.2f, y %.2f, z %.2f,===\n",angle[0],angle[1],angle[2]);
|
||||
xlog("GRAVITY VECTOR in device frame: gx=%.2f, gy=%.2f, gz=%.2f\n", gx_proj, gy_proj, gz_proj);
|
||||
extern mmc5603nj_cal_data_t cal_data;
|
||||
xlog("cal_data:X: %.4f, Y: %.4f, Z: %.4f\n", cal_data.offset_x,cal_data.offset_y,cal_data.offset_z);
|
||||
count = 0;
|
||||
|
||||
}
|
||||
count++;
|
||||
|
||||
#endif
|
||||
|
||||
|
||||
}
|
||||
|
||||
|
||||
@ -577,7 +197,6 @@ BLE_send_data_t sensor_processing_task(signed short* acc_data_buf, signed short*
|
||||
static int initialized = 0;
|
||||
static float acc_data_g[3];
|
||||
static float gyr_data_dps[3];
|
||||
|
||||
if(quaternion != NULL){
|
||||
memcpy(quaternion_data, quaternion, 4 * sizeof(float));
|
||||
}
|
||||
@ -630,21 +249,21 @@ BLE_send_data_t sensor_processing_task(signed short* acc_data_buf, signed short*
|
||||
|
||||
skiing_tracker_update(&my_skiing_tracker, acc_data_g, gyr_data_dps, angle_data, delta_time);
|
||||
|
||||
BLE_send_data.skiing_state = my_skiing_tracker.state;
|
||||
for (int i = 0; i < 3; i++) {
|
||||
#ifdef XTELL_TEST
|
||||
BLE_send_data.acc_data[i] = (short)(acc_data_g[i] * 9.8f) * 100; //cm/^s2
|
||||
BLE_send_data.gyr_data[i] = (short)gyr_data_dps[i]; //dps
|
||||
BLE_send_data.angle_data[i] = angle_data[i];
|
||||
#else
|
||||
BLE_send_data.acc_data[i] = (short)acc_data_buf[i]; //原始adc数据
|
||||
BLE_send_data.gyr_data[i] = (short)gyr_data_buf[i]; //原始adc数据
|
||||
BLE_send_data.angle_data[i] = angle_data[i];
|
||||
#endif
|
||||
}
|
||||
BLE_send_data.speed_cms = (int)(my_skiing_tracker.speed * 100);
|
||||
BLE_send_data.distance_cm = (int)(my_skiing_tracker.distance * 100);
|
||||
// printf("Calculate the time interval =============== end\n");
|
||||
// BLE_send_data.skiing_state = my_skiing_tracker.state;
|
||||
// for (int i = 0; i < 3; i++) {
|
||||
// #ifdef XTELL_TEST
|
||||
// BLE_send_data.acc_data[i] = (short)(acc_data_g[i] * 9.8f) * 100; //cm/^s2
|
||||
// BLE_send_data.gyr_data[i] = (short)gyr_data_dps[i]; //dps
|
||||
// BLE_send_data.angle_data[i] = angle_data[i];
|
||||
// #else
|
||||
// BLE_send_data.acc_data[i] = (short)acc_data_buf[i]; //原始adc数据
|
||||
// BLE_send_data.gyr_data[i] = (short)gyr_data_buf[i]; //原始adc数据
|
||||
// BLE_send_data.angle_data[i] = angle_data[i];
|
||||
// #endif
|
||||
// }
|
||||
// BLE_send_data.speed_cms = (int)(my_skiing_tracker.speed * 100);
|
||||
// BLE_send_data.distance_cm = (int)(my_skiing_tracker.distance * 100);
|
||||
// // printf("Calculate the time interval =============== end\n");
|
||||
|
||||
return BLE_send_data;
|
||||
}
|
||||
|
||||
@ -30,7 +30,7 @@ typedef struct {
|
||||
skiing_state_t state; // 当前滑雪状态
|
||||
|
||||
// 内部计算使用的私有成员
|
||||
float acc_no_g[3]; // 去掉重力分量后的加速度
|
||||
float acc_world[3]; // 在世界坐标系下的加速度
|
||||
|
||||
// 用于空中距离计算
|
||||
float time_in_air; // 滞空时间计时器
|
||||
|
||||
@ -21,6 +21,9 @@
|
||||
#include "calculate/skiing_tracker.h"
|
||||
#include "xtell.h"
|
||||
#include "./ano/ano_protocol.h"
|
||||
#include "./sensor/MMC56.h"
|
||||
#include "./sensor/BMP280.h"
|
||||
#include "./sensor/AK8963.h"
|
||||
///////////////////////////////////////////////////////////////////////////////////////////////////
|
||||
//宏定义
|
||||
#define ENABLE_XLOG 1
|
||||
@ -58,6 +61,7 @@ static u16 calculate_data_id;
|
||||
static u8 sensor_data_buffer[SENSOR_DATA_BUFFER_SIZE];
|
||||
static circle_buffer_t sensor_cb;
|
||||
|
||||
static int count = 0;
|
||||
|
||||
//--- test ---
|
||||
// 全局变量
|
||||
@ -217,9 +221,8 @@ void sensor_read_data(){
|
||||
// status = SIX_SL_SC7U22_Angle_Output(1, combined_raw_data, tmp.angle, 0);
|
||||
// status = Original_SL_SC7U22_Angle_Output(1, combined_raw_data, tmp.angle, 0);
|
||||
|
||||
status = Q_SL_SC7U22_Angle_Output(1, combined_raw_data, tmp.angle, 0, tmp.quaternion_output);
|
||||
status = Q_SL_SC7U22_Angle_Output(1, combined_raw_data, tmp.angle,NULL, 0, tmp.quaternion_output);
|
||||
|
||||
int count = 0;
|
||||
if(count > 100){
|
||||
count = 0;
|
||||
char log_buffer[100]; // 100个字符应该足够了
|
||||
@ -238,7 +241,7 @@ void sensor_read_data(){
|
||||
// status = SL_SC7U22_Angle_Output(0, combined_raw_data, tmp.angle, 0);
|
||||
// status = SIX_SL_SC7U22_Angle_Output(0, combined_raw_data, tmp.angle, 0);
|
||||
// status = Original_SL_SC7U22_Angle_Output(0, combined_raw_data, tmp.angle, 0);
|
||||
status = Q_SL_SC7U22_Angle_Output(0, combined_raw_data, tmp.angle, 0, tmp.quaternion_output);
|
||||
status = Q_SL_SC7U22_Angle_Output(0, combined_raw_data, tmp.angle,NULL, 0, tmp.quaternion_output);
|
||||
|
||||
memcpy(tmp.acc_data, &combined_raw_data[0], 3 * sizeof(signed short));
|
||||
memcpy(tmp.gyr_data, &combined_raw_data[3], 3 * sizeof(signed short));
|
||||
@ -293,7 +296,6 @@ void calculate_data(){
|
||||
// xlog("=======end\n");
|
||||
}
|
||||
|
||||
static int count = 0;
|
||||
extern char xt_Check_Flag;
|
||||
void BLE_send_data(){
|
||||
// xlog("=======start\n");
|
||||
@ -465,14 +467,12 @@ void sensor_measure(void){
|
||||
static int calibration_done = 0;
|
||||
char status = 0;
|
||||
|
||||
if(count_test1 >= 100){
|
||||
count_test1 = 0;
|
||||
xlog("count_test1\n");
|
||||
}
|
||||
count_test1++;
|
||||
|
||||
static sensor_data_t tmp;
|
||||
mmc5603nj_mag_data_t mag_data;
|
||||
SL_SC7U22_RawData_Read(tmp.acc_data,tmp.gyr_data);
|
||||
// os_time_dly(1);
|
||||
mmc5603nj_read_mag_data(&mag_data);
|
||||
// xlog("=======sensor_read_data middle 1\n");
|
||||
memcpy(&combined_raw_data[0], tmp.acc_data, 3 * sizeof(signed short));
|
||||
memcpy(&combined_raw_data[3], tmp.gyr_data, 3 * sizeof(signed short));
|
||||
@ -481,15 +481,15 @@ void sensor_measure(void){
|
||||
// status = SL_SC7U22_Angle_Output(1, combined_raw_data, tmp.angle, 0);
|
||||
// status = SIX_SL_SC7U22_Angle_Output(1, combined_raw_data, tmp.angle, 0);
|
||||
// status = Original_SL_SC7U22_Angle_Output(1, combined_raw_data, tmp.angle, 0);
|
||||
status = Q_SL_SC7U22_Angle_Output(1, combined_raw_data, tmp.angle, 0, tmp.quaternion_output);
|
||||
status = Q_SL_SC7U22_Angle_Output(1, combined_raw_data, tmp.angle,&mag_data, 0, tmp.quaternion_output);
|
||||
|
||||
int count = 0;
|
||||
if(count > 100){
|
||||
count = 0;
|
||||
char log_buffer[100];
|
||||
// snprintf( log_buffer, sizeof(log_buffer),"status:%d\n",status);
|
||||
// send_data_to_ble_client(&log_buffer,strlen(log_buffer));
|
||||
xlog("status:%d\n", status);
|
||||
xlog("RawData:AX=%d,AY=%d,AZ=%d,GX=%d,GY=%d,GZ=%d\r\n",combined_raw_data[0],combined_raw_data[1],combined_raw_data[2],combined_raw_data[3],combined_raw_data[4],combined_raw_data[5]);
|
||||
}
|
||||
count++;
|
||||
|
||||
@ -502,14 +502,23 @@ void sensor_measure(void){
|
||||
// status = SL_SC7U22_Angle_Output(0, combined_raw_data, tmp.angle, 0);
|
||||
// status = SIX_SL_SC7U22_Angle_Output(0, combined_raw_data, tmp.angle, 0);
|
||||
// status = Original_SL_SC7U22_Angle_Output(0, combined_raw_data, tmp.angle, 0);
|
||||
status = Q_SL_SC7U22_Angle_Output(0, combined_raw_data, tmp.angle, 0, tmp.quaternion_output);
|
||||
status = Q_SL_SC7U22_Angle_Output(0, combined_raw_data, tmp.angle,&mag_data, 0, tmp.quaternion_output);
|
||||
memcpy(tmp.acc_data, &combined_raw_data[0], 3 * sizeof(signed short));
|
||||
memcpy(tmp.gyr_data, &combined_raw_data[3], 3 * sizeof(signed short));
|
||||
BLE_send_data_t data_by_calculate = sensor_processing_task(tmp.acc_data, tmp.gyr_data,tmp.angle, tmp.quaternion_output);
|
||||
extern void ano_send_attitude_data(float rol, float pit, float yaw, uint8_t fusion_sta) ;
|
||||
ano_send_attitude_data(tmp.angle[0],tmp.angle[1],tmp.angle[2], 1);
|
||||
ano_send_attitude_data(tmp.angle[0],tmp.angle[1],tmp.angle[2], 1);
|
||||
}
|
||||
|
||||
// mmc5603nj_mag_data_t mag_data;
|
||||
// mmc5603nj_read_mag_data(&mag_data);
|
||||
// float temperature = mmc5603nj_get_temperature();
|
||||
// count_test1++;
|
||||
// if(count_test1 >500){
|
||||
// count_test1 =0;
|
||||
// xlog("Mag X: %.4f, Y: %.4f, Z: %.4f Gauss\n", mag_data.x, mag_data.y, mag_data.z);
|
||||
// }
|
||||
|
||||
// xlog("=======sensor_read_data END\n");
|
||||
}
|
||||
|
||||
@ -538,9 +547,22 @@ void xtell_task_create(void){
|
||||
// os_time_dly(10);
|
||||
// delay_2ms(10);
|
||||
|
||||
SL_SC7U22_Config();
|
||||
// extern u8 LIS2DH12_Config(void);
|
||||
// LIS2DH12_Config();
|
||||
|
||||
|
||||
// if(bmp280_init() != 0){
|
||||
// xlog("bmp280 init error\n");
|
||||
// }
|
||||
// float temp, press;
|
||||
// bmp280_read_data(&temp, &press);
|
||||
// xlog("get temp: %d, get press: %d\n",temp, press);
|
||||
|
||||
|
||||
|
||||
// MPU9250_Mag_Init();
|
||||
//iic总线设备扫描
|
||||
// extern void i2c_scanner_probe(void);
|
||||
// i2c_scanner_probe();
|
||||
|
||||
|
||||
xlog("xtell_task_create\n");
|
||||
// 初始化环形缓冲区
|
||||
@ -548,7 +570,7 @@ void xtell_task_create(void){
|
||||
|
||||
|
||||
ano_protocol_init(115200);
|
||||
create_process(&calculate_data_id, "calculate",NULL, sensor_measure, 10);
|
||||
|
||||
|
||||
circle_buffer_init(&sensor_read, sensor_read_buffer, SENSOR_DATA_BUFFER_SIZE, sizeof(sensor_data_t));
|
||||
|
||||
|
||||
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
|
||||
178
apps/earphone/xtell_Sensor/sensor/BMP280.c
Normal file
178
apps/earphone/xtell_Sensor/sensor/BMP280.c
Normal file
@ -0,0 +1,178 @@
|
||||
/*
|
||||
气压计
|
||||
*/
|
||||
#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不匹配
|
||||
}
|
||||
|
||||
// 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); // 等待配置生效
|
||||
|
||||
return 0; // 初始化成功
|
||||
}
|
||||
|
||||
uint8_t bmp280_read_data(float *temperature, float *pressure) {
|
||||
uint8_t data[6];
|
||||
int32_t adc_P, adc_T;
|
||||
|
||||
// 一次性读取6个字节的温度和气压原始数据
|
||||
if (bmp280_read_regs(BMP280_REG_PRESS_MSB, data, 6) != 0) {
|
||||
return 1; // 读取失败
|
||||
}
|
||||
|
||||
// 组合原始数据 (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;
|
||||
return 1;
|
||||
}
|
||||
|
||||
// 进行补偿计算
|
||||
*temperature = compensate_temperature(adc_T);
|
||||
*pressure = compensate_pressure(adc_P);
|
||||
|
||||
return 0; // 成功
|
||||
}
|
||||
46
apps/earphone/xtell_Sensor/sensor/BMP280.h
Normal file
46
apps/earphone/xtell_Sensor/sensor/BMP280.h
Normal file
@ -0,0 +1,46 @@
|
||||
#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 0x77 //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 0x76 //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);
|
||||
|
||||
#endif // BMP280_DRIVER_H
|
||||
221
apps/earphone/xtell_Sensor/sensor/MMC56.c
Normal file
221
apps/earphone/xtell_Sensor/sensor/MMC56.c
Normal file
@ -0,0 +1,221 @@
|
||||
|
||||
#include "MMC56.h"
|
||||
#include "math.h"
|
||||
#include "os/os_api.h"
|
||||
#include "../xtell.h"
|
||||
#include "gSensor/gSensor_manage.h"
|
||||
#include "printf.h"
|
||||
|
||||
// 用于跟踪当前是否处于连续测量模式
|
||||
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() != 0x80) {
|
||||
printf("MMC5603NJ init failed: wrong Product ID (read: 0x%X)\n", mmc5603nj_get_pid());
|
||||
return -1;
|
||||
}
|
||||
|
||||
// 软件复位
|
||||
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);
|
||||
|
||||
|
||||
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);
|
||||
|
||||
// 定义校准时长和采样间隔
|
||||
const uint32_t calibration_duration_ms = 20000; // 20秒
|
||||
const uint32_t sample_interval_ms = 100; // 每100ms采样一次
|
||||
|
||||
// 初始化最大最小值
|
||||
// 使用一个临时变量来读取数据，避免干扰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_duration_ms/sample_interval_ms;
|
||||
|
||||
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_ms / 10); // os_time_dly的参数通常是ticks (1 tick = 10ms)
|
||||
}
|
||||
|
||||
// 检查数据范围是否合理，防止传感器未动或故障
|
||||
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 -1;
|
||||
}
|
||||
|
||||
// 计算硬磁偏移 (椭球中心)
|
||||
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");
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
||||
|
||||
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;
|
||||
}
|
||||
95
apps/earphone/xtell_Sensor/sensor/MMC56.h
Normal file
95
apps/earphone/xtell_Sensor/sensor/MMC56.h
Normal file
@ -0,0 +1,95 @@
|
||||
#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
|
||||
|
||||
// 定义一个结构体来存放三轴磁场数据（单位：高斯 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
|
||||
@ -1,4 +1,6 @@
|
||||
|
||||
/*
|
||||
六轴
|
||||
*/
|
||||
#include "SC7U22.h"
|
||||
#include "math.h"
|
||||
#include "os/os_api.h"
|
||||
@ -68,9 +70,12 @@ char iic_write_result;
|
||||
unsigned char SL_SC7U22_Check(void)
|
||||
{
|
||||
unsigned char reg_value=0;
|
||||
xlog("SL_SC7U22_Check\n");
|
||||
|
||||
iic_write_result = SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x7F, 0x00);//goto 0x00
|
||||
// xlog("SL_SC7U22_Check write: %d\n", iic_write_result);
|
||||
|
||||
iic_read_len = SL_SC7U22_I2c_Spi_Read(SL_SPI_IIC_INTERFACE, SC7U22_WHO_AM_I, 1, ®_value);
|
||||
// xlog("SL_SC7U22_Check read : %d\n", iic_write_result);
|
||||
xlog("0x%x=0x%x\r\n",SC7U22_WHO_AM_I,reg_value);
|
||||
if(reg_value==0x6A) //设备的id
|
||||
return 0x01;//SC7U22
|
||||
@ -1007,10 +1012,6 @@ unsigned char SL_SC7U22_Angle_Output(unsigned char calibration_en, signed short
|
||||
/**
|
||||
* @brief 姿态角解算函数 (基于一阶互补滤波)
|
||||
* @details
|
||||
* 该函数主要完成两项工作：
|
||||
* 1. 静态校准：在初始阶段，检测传感器是否处于静止状态。如果是，则计算加速度计和陀螺仪的零点偏移（误差），用于后续的数据补偿。
|
||||
* 2. 姿态解算：使用一阶互补滤波器融合经过校准后的加速度计和陀螺仪数据，计算出物体的俯仰角（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]。
|
||||
@ -1172,27 +1173,40 @@ unsigned char get_calibration_state(void){
|
||||
|
||||
|
||||
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////
|
||||
// 如果没有定义 PI，请取消下面的注释
|
||||
// #define PI 3.14159265358979323846f
|
||||
|
||||
// =================================================================================================
|
||||
// Mahony AHRS (Attitude and Heading Reference System) 相关变量定义
|
||||
// Mahony滤波器是一种高效的互补滤波器，它使用四元数来表示姿态，从而避免了万向节死锁问题。
|
||||
// 它通过一个PI控制器来校正陀ar螺仪的积分漂移。
|
||||
// 通过一个PI控制器来校正陀ar螺仪的积分漂移。
|
||||
// -------------------------------------------------------------------------------------------------
|
||||
// --- 滤波器参数 ---
|
||||
// Kp: 比例增益，决定了加速度计数据校正陀螺仪的权重。值越大，对加速度计的响应越快，但对运动加速度更敏感。
|
||||
// Ki: 积分增益，决定了用于校正陀螺仪静态漂移的权重。
|
||||
// Q_dt: 采样时间间隔（单位：秒），这里是10ms (0.01s)，对应100Hz的采样率。
|
||||
#define HAVE_MAG 1
|
||||
#if HAVE_MAG == 0
|
||||
// -- 无地磁 --
|
||||
const float Kp = 2.0f;
|
||||
const float Ki = 0.005f;
|
||||
const float Q_dt = 0.01f;
|
||||
#else
|
||||
// -- 有地磁 --
|
||||
const float Kp = 0.3f;
|
||||
const float Ki = 0.001f;
|
||||
const float Q_dt = 0.01f;
|
||||
#endif
|
||||
|
||||
// --- 状态变量 ---
|
||||
// 四元数 (Quaternion)，表示当前的姿态。初始化为 (1, 0, 0, 0)，代表初始姿态为水平。
|
||||
static float q0 = 1.0f, q1 = 0.0f, q2 = 0.0f, q3 = 0.0f;
|
||||
// 陀螺仪积分误差项，用于补偿静态漂移
|
||||
static float exInt = 0.0f, eyInt = 0.0f, ezInt = 0.0f;
|
||||
|
||||
// 磁力计校准相关的变量
|
||||
float Error_Mag_f[3] = {0.0f, 0.0f, 0.0f};
|
||||
double Sum_Avg_Mag_f[3] = {0.0, 0.0, 0.0}; // 使用double避免累加过程中的精度损失
|
||||
// 临时存储校准后数据的数组
|
||||
signed short Temp_AccGyro[6] = {0};
|
||||
float Temp_Mag[3] = {0.0f, 0.0f, 0.0f};
|
||||
// =================================================================================================
|
||||
|
||||
|
||||
@ -1204,20 +1218,242 @@ static float exInt = 0.0f, eyInt = 0.0f, ezInt = 0.0f;
|
||||
* 该函数主要完成两项工作：
|
||||
* 1. 静态校准：在初始阶段，检测传感器是否处于静止状态。如果是，则计算加速度计和陀螺仪的零点偏移（误差），用于后续的数据补偿。
|
||||
* 2. 姿态解算：使用基于四元数的Mahony互补滤波器融合经过校准后的加速度计和陀螺仪数据，计算出物体的俯仰角（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 mag_data_input 传入：指向包含三轴磁力计数据的结构体指针。数据单位应为高斯(Gauss)。已经8面校准过的数据
|
||||
* @param yaw_rst 传入：Yaw轴重置标志。如果为1，则将整个姿态滤波器状态重置。
|
||||
*
|
||||
* @param quaternion_output 传出, 四元数,用于后续重力分量的去除计算
|
||||
* @return
|
||||
* - 0: 正在进行静态校准。
|
||||
* - 1: 姿态角计算成功。
|
||||
* - 2: 校准未完成，无法进行计算。
|
||||
*/
|
||||
unsigned char Q_SL_SC7U22_Angle_Output(unsigned char calibration_en, signed short *acc_gyro_input, float *Angle_output, unsigned char yaw_rst, float *quaternion_output)
|
||||
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)
|
||||
{
|
||||
#if 1 //有地磁置1
|
||||
unsigned char sl_i = 0;
|
||||
// 如果外部强制禁用校准，则将标志位置1
|
||||
if (calibration_en == 0) {
|
||||
SL_SC7U22_Error_Flag = 1;
|
||||
}
|
||||
|
||||
// ====================== 坐标对齐 ======================
|
||||
mmc5603nj_mag_data_t mag_data_input;
|
||||
|
||||
mag_data_input.x = - _mag_data_input->x;
|
||||
mag_data_input.y = - _mag_data_input->y;
|
||||
mag_data_input.z = _mag_data_input->z;
|
||||
// ================================================================
|
||||
|
||||
|
||||
// =================================================================================
|
||||
// 静态校准
|
||||
// ---------------------------------------------------------------------------------
|
||||
if (SL_SC7U22_Error_Flag == 0) {
|
||||
unsigned short acc_gyro_delta[2];
|
||||
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];
|
||||
}
|
||||
#if (ACC_RANGE == 2)
|
||||
if ((acc_gyro_delta[0] / 8 < 160) && (acc_gyro_delta[1] < 40) && (SL_GetAbsShort(acc_gyro_input[0]) < 3000) && (SL_GetAbsShort(acc_gyro_input[1]) < 3000) && (SL_GetAbsShort(acc_gyro_input[2] - 16384) < 3000)) {
|
||||
#elif (ACC_RANGE == 4)
|
||||
if ((acc_gyro_delta[0] / 8 < 160) && (acc_gyro_delta[1] < 40) && (SL_GetAbsShort(acc_gyro_input[0]) < 3000) && (SL_GetAbsShort(acc_gyro_input[1]) < 3000) && (SL_GetAbsShort(acc_gyro_input[2] - 8192) < 3000)) {
|
||||
#elif (ACC_RANGE == 8)
|
||||
if ((acc_gyro_delta[0] / 8 < 160) && (acc_gyro_delta[1] < 40) && (SL_GetAbsShort(acc_gyro_input[0]) < 3000) && (SL_GetAbsShort(acc_gyro_input[1]) < 3000) && (SL_GetAbsShort(acc_gyro_input[2] - 4096) < 3000)) {
|
||||
#elif (ACC_RANGE == 16)
|
||||
if ((acc_gyro_delta[0] / 8 < 160) && (acc_gyro_delta[1] < 40) && (SL_GetAbsShort(acc_gyro_input[0]) < 3000) && (SL_GetAbsShort(acc_gyro_input[1]) < 3000) && (SL_GetAbsShort(acc_gyro_input[2] - 2048) < 3000)) {
|
||||
#endif
|
||||
if (SL_SC7U22_Error_cnt < 200) SL_SC7U22_Error_cnt++;
|
||||
} else {
|
||||
SL_SC7U22_Error_cnt = 0;
|
||||
}
|
||||
if (SL_SC7U22_Error_cnt > 190) {
|
||||
//累加6轴数据
|
||||
for (sl_i = 0; sl_i < 6; sl_i++) Sum_Avg_Accgyro[sl_i] += acc_gyro_input[sl_i];
|
||||
|
||||
Sum_Avg_Mag_f[0] += mag_data_input.x;
|
||||
Sum_Avg_Mag_f[1] += mag_data_input.y;
|
||||
Sum_Avg_Mag_f[2] += mag_data_input.z;
|
||||
|
||||
SL_SC7U22_Error_cnt2++;
|
||||
if (SL_SC7U22_Error_cnt2 > 49) {
|
||||
SL_SC7U22_Error_Flag = 1;
|
||||
SL_SC7U22_Error_cnt2 = 0;
|
||||
SL_SC7U22_Error_cnt = 0;
|
||||
//6轴偏置
|
||||
for (sl_i = 0; sl_i < 6; sl_i++) Sum_Avg_Accgyro[sl_i] = Sum_Avg_Accgyro[sl_i] / 50;
|
||||
Error_Accgyro[0] = 0 - Sum_Avg_Accgyro[0];
|
||||
Error_Accgyro[1] = 0 - Sum_Avg_Accgyro[1];
|
||||
#if ACC_RANGE==2
|
||||
Error_Accgyro[2] = 16384 - Sum_Avg_Accgyro[2];
|
||||
#elif ACC_RANGE==4
|
||||
Error_Accgyro[2] = 8192 - Sum_Avg_Accgyro[2];
|
||||
#elif ACC_RANGE==8
|
||||
Error_Accgyro[2] = 4096 - Sum_Avg_Accgyro[2];
|
||||
#elif ACC_RANGE==16
|
||||
Error_Accgyro[2] = 2048 - Sum_Avg_Accgyro[2];
|
||||
#endif
|
||||
Error_Accgyro[3] = 0 - Sum_Avg_Accgyro[3];
|
||||
Error_Accgyro[4] = 0 - Sum_Avg_Accgyro[4];
|
||||
Error_Accgyro[5] = 0 - Sum_Avg_Accgyro[5];
|
||||
// //磁力计偏置 -- 不在这弄,在磁力计初始化的时候开始
|
||||
// Sum_Avg_Mag_f[0] /= 50.0;
|
||||
// Sum_Avg_Mag_f[1] /= 50.0;
|
||||
// Sum_Avg_Mag_f[2] /= 50.0;
|
||||
// Error_Mag_f[0] = 0.0f - (float)Sum_Avg_Mag_f[0];
|
||||
// Error_Mag_f[1] = 0.0f - (float)Sum_Avg_Mag_f[1];
|
||||
// Error_Mag_f[2] = 0.0f - (float)Sum_Avg_Mag_f[2];
|
||||
// xlog("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]);
|
||||
// xlog("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]);
|
||||
}
|
||||
} else {
|
||||
SL_SC7U22_Error_cnt2 = 0;
|
||||
for (sl_i = 0; sl_i < 6; sl_i++) Sum_Avg_Accgyro[sl_i] = 0;
|
||||
// Sum_Avg_Mag_f[0] = 0.0;
|
||||
// Sum_Avg_Mag_f[1] = 0.0;
|
||||
// Sum_Avg_Mag_f[2] = 0.0;
|
||||
}
|
||||
return 0; // 返回0，表示正在校准
|
||||
}
|
||||
|
||||
// =================================================================================
|
||||
// 姿态解算 (Mahony AHRS)
|
||||
// ---------------------------------------------------------------------------------
|
||||
if (SL_SC7U22_Error_Flag == 1) { // 确认已经校准完成
|
||||
|
||||
// --- Yaw轴/姿态重置 ---
|
||||
// 注意：重置yaw会重置整个姿态滤波器，使设备回到初始水平姿态
|
||||
if (yaw_rst == 1) {
|
||||
q0 = 1.0f; q1 = 0.0f; q2 = 0.0f; q3 = 0.0f;
|
||||
exInt = 0.0f; eyInt = 0.0f; ezInt = 0.0f;
|
||||
}
|
||||
|
||||
// --- 数据预处理 ---
|
||||
// 应用零点偏移补偿
|
||||
for (sl_i = 0; sl_i < 6; sl_i++) {
|
||||
Temp_Accgyro[sl_i] = acc_gyro_input[sl_i] + Error_Accgyro[sl_i];
|
||||
}
|
||||
|
||||
// Temp_Mag[0] = mag_data_input.x + Error_Mag_f[0];
|
||||
// Temp_Mag[1] = mag_data_input.y + Error_Mag_f[1];
|
||||
// Temp_Mag[2] = mag_data_input.z + Error_Mag_f[2];
|
||||
Temp_Mag[0] = mag_data_input.x;
|
||||
Temp_Mag[1] = mag_data_input.y;
|
||||
Temp_Mag[2] = mag_data_input.z;
|
||||
|
||||
// 将校准后的数据写回输入数组
|
||||
#if 1
|
||||
for (sl_i = 0; sl_i < 6; sl_i++) {
|
||||
acc_gyro_input[sl_i] = Temp_Accgyro[sl_i];
|
||||
}
|
||||
#endif
|
||||
|
||||
// 获取校准后的数据
|
||||
float ax = (float)Temp_Accgyro[0];
|
||||
float ay = (float)Temp_Accgyro[1];
|
||||
float az = (float)Temp_Accgyro[2];
|
||||
// 将陀螺仪数据从 LSB 转换为弧度/秒 (rad/s)
|
||||
// 转换系数 0.061 ≈ 2000dps / 32768 LSB; PI/180 ≈ 0.01745
|
||||
float gx = (float)Temp_Accgyro[3] * 0.061f * 0.0174533f; // Roll rate
|
||||
float gy = (float)Temp_Accgyro[4] * 0.061f * 0.0174533f; // Pitch rate
|
||||
float gz = (float)Temp_Accgyro[5] * 0.061f * 0.0174533f; // Yaw rate
|
||||
float mx = Temp_Mag[0];
|
||||
float my = Temp_Mag[1];
|
||||
float mz = Temp_Mag[2];
|
||||
|
||||
// --- Mahony算法核心 ---
|
||||
float norm;
|
||||
float q0q0 = q0 * q0;
|
||||
float q0q1 = q0 * q1;
|
||||
float q0q2 = q0 * q2;
|
||||
float q0q3 = q0 * q3;
|
||||
float q1q1 = q1 * q1;
|
||||
float q1q2 = q1 * q2;
|
||||
float q1q3 = q1 * q3;
|
||||
float q2q2 = q2 * q2;
|
||||
float q2q3 = q2 * q3;
|
||||
float q3q3 = q3 * q3;
|
||||
float hx, hy, bx, bz;
|
||||
float vx, vy, vz, wx, wy, wz;
|
||||
float ex, ey, ez;
|
||||
|
||||
// 归一化加速度计测量值，得到单位重力向量
|
||||
norm = sqrtf(ax * ax + ay * ay + az * az);
|
||||
if (norm > 0.0f) { ax /= norm; ay /= norm; az /= norm; }
|
||||
else { return 1; }
|
||||
|
||||
norm = sqrtf(mx * mx + my * my + mz * mz);
|
||||
if (norm > 0.0f) { mx /= norm; my /= norm; mz /= norm; }
|
||||
|
||||
// 根据当前姿态(四元数)估计重力向量的方向
|
||||
vx = 2.0f * (q1q3 - q0q2);
|
||||
vy = 2.0f * (q0q1 + q2q3);
|
||||
vz = q0q0 - q1q1 - q2q2 + q3q3;
|
||||
|
||||
// 计算磁场误差 (倾斜补偿)
|
||||
hx = 2.0f * mx * (0.5f - q2q2 - q3q3) + 2.0f * my * (q1q2 - q0q3) + 2.0f * mz * (q1q3 + q0q2);
|
||||
hy = 2.0f * mx * (q1q2 + q0q3) + 2.0f * my * (0.5f - q1q1 - q3q3) + 2.0f * mz * (q2q3 - q0q1);
|
||||
bx = sqrtf(hx * hx + hy * hy);
|
||||
bz = 2.0f * mx * (q1q3 - q0q2) + 2.0f * my * (q2q3 + q0q1) + 2.0f * mz * (0.5f - q1q1 - q2q2);
|
||||
wx = 2.0f * bx * (0.5f - q2q2 - q3q3) + 2.0f * bz * (q1q3 - q0q2);
|
||||
wy = 2.0f * bx * (q1q2 - q0q3) + 2.0f * bz * (q0q1 + q2q3);
|
||||
wz = 2.0f * bx * (q1q3 + q0q2) + 2.0f * bz * (0.5f - q1q1 - q2q2);
|
||||
|
||||
// 合并重力和磁场误差
|
||||
ex = (ay * vz - az * vy) + (my * wz - mz * wy);
|
||||
ey = (az * vx - ax * vz) + (mz * wx - mx * wz);
|
||||
ez = (ax * vy - ay * vx) + (mx * wy - my * wx);
|
||||
|
||||
// PI控制器
|
||||
if (Ki > 0.0f) {
|
||||
exInt += ex * Ki * Q_dt;
|
||||
eyInt += ey * Ki * Q_dt;
|
||||
ezInt += ez * Ki * Q_dt;
|
||||
}
|
||||
gx += Kp * ex + exInt;
|
||||
gy += Kp * ey + eyInt;
|
||||
gz += Kp * ez + ezInt;
|
||||
|
||||
// 使用校正后的角速度更新四元数 (一阶毕卡法)
|
||||
q0 += (-q1 * gx - q2 * gy - q3 * gz) * 0.5f * Q_dt;
|
||||
q1 += ( q0 * gx + q2 * gz - q3 * gy) * 0.5f * Q_dt;
|
||||
q2 += ( q0 * gy - q1 * gz + q3 * gx) * 0.5f * Q_dt;
|
||||
q3 += ( q0 * gz + q1 * gy - q2 * gx) * 0.5f * Q_dt;
|
||||
|
||||
// 归一化四元数，保持其单位长度
|
||||
norm = sqrtf(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
|
||||
q0 /= norm; q1 /= norm; q2 /= norm; q3 /= norm;
|
||||
|
||||
// --- 将四元数转换为欧拉角 (Pitch, Roll, Yaw) ---
|
||||
// Pitch (绕Y轴旋转)
|
||||
Angle_output[0] = asinf(-2.0f * (q1 * q3 - q0 * q2)) * 57.29578f;
|
||||
// Roll (绕X轴旋转)
|
||||
Angle_output[1] = atan2f(2.0f * (q0 * q1 + q2 * q3), q0q0 - q1q1 - q2q2 + q3q3) * 57.29578f;
|
||||
// Yaw (绕Z轴旋转)
|
||||
Angle_output[2] = atan2f(2.0f * (q1 * q2 + q0 * q3), q0q0 + q1q1 - q2q2 - q3q3) * 57.29578f;
|
||||
|
||||
//将四元数传出去
|
||||
if (quaternion_output != NULL) {
|
||||
quaternion_output[0] = q0; // w
|
||||
quaternion_output[1] = q1; // x
|
||||
quaternion_output[2] = q2; // y
|
||||
quaternion_output[3] = q3; // z
|
||||
}
|
||||
return 1; // 返回1，表示计算成功
|
||||
}
|
||||
|
||||
return 2; // 校准未完成，返回错误状态
|
||||
|
||||
#else
|
||||
unsigned char sl_i = 0;
|
||||
|
||||
// 如果外部强制禁用校准，则将标志位置1
|
||||
@ -1250,6 +1486,7 @@ unsigned char Q_SL_SC7U22_Angle_Output(unsigned char calibration_en, signed shor
|
||||
#endif
|
||||
if (SL_SC7U22_Error_cnt < 200) SL_SC7U22_Error_cnt++;
|
||||
} else {
|
||||
// printf("error: The calibration process has undergone a shift.\n");
|
||||
SL_SC7U22_Error_cnt = 0;
|
||||
}
|
||||
if (SL_SC7U22_Error_cnt > 190) {
|
||||
@ -1390,6 +1627,7 @@ unsigned char Q_SL_SC7U22_Angle_Output(unsigned char calibration_en, signed shor
|
||||
}
|
||||
|
||||
return 2; // 校准未完成，返回错误状态
|
||||
#endif
|
||||
}
|
||||
|
||||
#endif
|
||||
|
||||
@ -9,6 +9,7 @@ Copyright (c) 2022 Silan MEMS. All Rights Reserved.
|
||||
|
||||
#include "gSensor/gSensor_manage.h"
|
||||
#include "printf.h"
|
||||
#include "MMC56.h"
|
||||
|
||||
//是否使能串口打印调试
|
||||
#define SL_Sensor_Algo_Release_Enable 0x00
|
||||
@ -132,7 +133,7 @@ unsigned char SL_SC7U22_Angle_Output(unsigned char calibration_en,signed short *
|
||||
|
||||
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, unsigned char yaw_rst, float *quaternion_output);
|
||||
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
|
||||
|
||||
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_
|
||||
@ -45,6 +45,10 @@
|
||||
#include "default_event_handler.h"
|
||||
#include "debug.h"
|
||||
#include "system/event.h"
|
||||
#include "./ano/ano_protocol.h"
|
||||
#include "./sensor/MMC56.h"
|
||||
#include "./sensor/BMP280.h"
|
||||
#include "./sensor/AK8963.h"
|
||||
///////////////////////////////////////////////////////////////////////////////////////////////////
|
||||
//宏定义
|
||||
#define LOG_TAG_CONST EARPHONE
|
||||
@ -199,18 +203,40 @@ void le_user_app_event_handler(struct sys_event* event){
|
||||
if(event->u.app.buffer[2] == 0x01){ //后面的数据长度 1
|
||||
switch (event->u.app.buffer[3]){
|
||||
case 0x01:
|
||||
extern void start_detection(void);
|
||||
start_detection();
|
||||
char* send_tmp = "start_detection\n";
|
||||
char* send_start = "will start after 5 seconds\n";
|
||||
send_data_to_ble_client(send_start,strlen(send_start));
|
||||
|
||||
if (mmc5603nj_init() != 0) {
|
||||
xlog("MMC5603NJ initialization failed!\n");
|
||||
char* send_error = "calibration error\n";
|
||||
send_data_to_ble_client(send_error,strlen(send_error));
|
||||
}
|
||||
xlog("MMC5603NJ PID: 0x%02X\n", mmc5603nj_get_pid());
|
||||
char* send_tmp = "8th calibration completed\n";
|
||||
send_data_to_ble_client(send_tmp,strlen(send_tmp));
|
||||
break;
|
||||
case 0x02:
|
||||
extern void create_process(u16* pid,char* name, void *priv, void (*func)(void *priv), u32 msec);
|
||||
extern void sensor_measure(void);
|
||||
static int test_id;
|
||||
SL_SC7U22_Config();
|
||||
create_process(&test_id, "test",NULL, sensor_measure, 10);
|
||||
send_tmp = "start_detection\n";
|
||||
send_data_to_ble_client(send_tmp,strlen(send_tmp));
|
||||
break;
|
||||
case 0x03:
|
||||
extern void start_detection(void);
|
||||
start_detection();
|
||||
send_tmp = "start_detection\n";
|
||||
send_data_to_ble_client(send_tmp,strlen(send_tmp));
|
||||
break;
|
||||
case 0x04:
|
||||
extern void stop_detection(void);
|
||||
stop_detection();
|
||||
send_tmp = "stop_detection\n";
|
||||
send_data_to_ble_client(send_tmp,strlen(send_tmp));
|
||||
break;
|
||||
case 0x03:
|
||||
case 0x05:
|
||||
extern void clear_speed(void);
|
||||
clear_speed();
|
||||
send_tmp = "Reset speed and distances to zero\n";
|
||||
|
||||
@ -188,12 +188,18 @@ void hw_iic_stop(hw_iic_dev iic)
|
||||
|
||||
u8 hw_iic_tx_byte(hw_iic_dev iic, u8 byte)
|
||||
{
|
||||
// printf("====debug1=======\n");
|
||||
u8 id = iic_get_id(iic);
|
||||
// printf("====debug2=======\n");
|
||||
iic_dir_out(iic_regs[id]);
|
||||
// printf("====debug3=======\n");
|
||||
iic_buf_reg(iic_regs[id]) = byte;
|
||||
// printf("====debug4=======\n");
|
||||
iic_cfg_done(iic_regs[id]);
|
||||
// printf("====debug5=======\n");
|
||||
/* putchar('a'); */
|
||||
while (!iic_pnd(iic_regs[id]));
|
||||
// printf("====debug6=======\n");
|
||||
iic_pnd_clr(iic_regs[id]);
|
||||
/* putchar('b'); */
|
||||
return iic_send_is_ack(iic_regs[id]);
|
||||
|
||||
Reference in New Issue
Block a user