4 Commits

Author SHA1 Message Date
lmx
5e587e0527 最新代码 2025-11-13 09:50:42 +08:00
lmx
c88cb70bb1 启动阈值调整 2025-11-12 13:57:59 +08:00
lmx
58ad14691e 修改了六轴配置 2025-11-11 19:31:34 +08:00
lmx
23a71377a2 cun 2025-11-11 09:38:51 +08:00
48 changed files with 172816 additions and 172033 deletions

View File

@ -25,6 +25,7 @@
"dev_manager.h": "c",
"bt_tws.h": "c",
"skiing_tracker.h": "c",
"xtell.h": "c"
"xtell.h": "c",
"debug.h": "c"
}
}

View File

@ -211,6 +211,12 @@ void write_gsensor_data_handle(void)
return ;
}
}
char w_log_buffer_1[100];
char w_log_buffer_2[100];
char w_log_buffer_3[100];
char w_log_buffer_4[100];
char w_log_buffer_5[100];
u8 gravity_sensor_command(u8 w_chip_id, u8 register_address, u8 function_command)
{
// spin_lock(&sensor_iic);
@ -220,6 +226,7 @@ u8 gravity_sensor_command(u8 w_chip_id, u8 register_address, u8 function_command
if (0 == iic_tx_byte(gSensor_info->iic_hdl, w_chip_id)) {
ret = 0;
xlog("\n gsen iic wr err 0\n");
strcpy(&w_log_buffer_1, "gsen iic wr err 0\n");
goto __gcend;
}
@ -228,6 +235,7 @@ u8 gravity_sensor_command(u8 w_chip_id, u8 register_address, u8 function_command
if (0 == iic_tx_byte(gSensor_info->iic_hdl, register_address)) {
ret = 0;
xlog("\n gsen iic wr err 1\n");
strcpy(&w_log_buffer_2, "gsen iic wr err 1\n");
goto __gcend;
}
@ -236,16 +244,24 @@ u8 gravity_sensor_command(u8 w_chip_id, u8 register_address, u8 function_command
if (0 == iic_tx_byte(gSensor_info->iic_hdl, function_command)) {
ret = 0;
xlog("\n gsen iic wr err 2\n");
strcpy(&w_log_buffer_3, "gsen iic wr err 3\n");
goto __gcend;
}
strcpy(&w_log_buffer_4, "gsen iic wr sucess\n");
__gcend:
iic_stop(gSensor_info->iic_hdl);
// spin_unlock(&sensor_iic);
/* os_mutex_post(&SENSOR_IIC_MUTEX); */
return ret;
}
char sen_log_buffer_1[100];
char sen_log_buffer_2[100];
char sen_log_buffer_3[100];
char sen_log_buffer_4[100];
char sen_log_buffer_5[100];
u8 _gravity_sensor_get_ndata(u8 r_chip_id, u8 register_address, u8 *buf, u8 data_len)
{
// xlog("%s",__func__);
@ -257,6 +273,7 @@ u8 _gravity_sensor_get_ndata(u8 r_chip_id, u8 register_address, u8 *buf, u8 data
if (0 == iic_tx_byte(gSensor_info->iic_hdl, r_chip_id - 1)) {
xlog("\n gsen iic rd err 0\n");
read_len = 0;
strcpy(&sen_log_buffer_1, "gsen iic rd err 0\n");
goto __gdend;
}
@ -265,6 +282,7 @@ u8 _gravity_sensor_get_ndata(u8 r_chip_id, u8 register_address, u8 *buf, u8 data
if (0 == iic_tx_byte(gSensor_info->iic_hdl, 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;
}
@ -272,6 +290,7 @@ u8 _gravity_sensor_get_ndata(u8 r_chip_id, u8 register_address, u8 *buf, u8 data
if (0 == iic_tx_byte(gSensor_info->iic_hdl, r_chip_id)) {
xlog("\n gsen iic rd err 2\n");
read_len = 0;
strcpy(&sen_log_buffer_3, "gsen iic rd err 2\n" );
goto __gdend;
}
@ -284,14 +303,15 @@ 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");
__gdend:
iic_stop(gSensor_info->iic_hdl);
delay(gSensor_info->iic_delay);
// spin_unlock(&sensor_iic);
/* os_mutex_post(&SENSOR_IIC_MUTEX); */
// strcpy(&sen_log_buffer_5, "gsen iic rd err\n");
return read_len;
}
void gsensor_io_ctl(u8 cmd, void *arg)

View File

@ -1425,9 +1425,25 @@ void ble_module_enable(u8 en)
#if(TCFG_CHARGE_BOX_ENABLE)
extern u8 get_chgbox_lid_status(void);
#endif
void user_ble_gap_device_set(char* name){ //xtell-set
if(strlen(name) < BT_NAME_LEN_MAX){
strcpy(gap_device_name,name);
//刷新广播
bt_ble_adv_enable(0);
make_set_adv_data();
make_set_rsp_data();
bt_ble_adv_enable(1);
}
}
void bt_ble_init(void)
{
log_info("***** ble_init******\n");
//xtell-set
// extern char xt_ble_new_name[9];
// user_ble_gap_device_set(xt_ble_new_name);
gap_device_name = bt_get_local_name();
gap_device_name_len = strlen(gap_device_name);

View File

@ -520,7 +520,9 @@ const struct hw_iic_config hw_iic_cfg[] = {
{IO_PORTC_02, IO_PORTC_03}, //group c
{IO_PORTA_05, IO_PORTA_06}, //group d
*/
.port = TCFG_HW_I2C0_PORTS,
// .port = TCFG_HW_I2C0_PORTS,
// .port = {IO_PORTC_04,IO_PORTC_05}, // portB: scl、sda
.port = {IO_PORTB_04,IO_PORTB_05}, // portA: scl、sda
.baudrate = TCFG_HW_I2C0_CLK, //IIC通讯波特率
.hdrive = 0, //是否打开IO口强驱
.io_filter = 1, //是否打开滤波器(去纹波)

View File

@ -34,15 +34,18 @@
#define TCFG_SW_I2C0_DAT_PORT IO_PORTA_06 //软件IIC DAT脚选择
#define TCFG_SW_I2C0_DELAY_CNT 10 //IIC延时参数影响通讯时钟频率
/*硬件IIC端口选择
/*硬件IIC端口选择 -- 具体看手册,这里写的不准 -- lmx
SCL SDA
'A': IO_PORT_DP IO_PORT_DM
'B': IO_PORTA_09 IO_PORTA_10
'C': IO_PORTA_07 IO_PORTA_08
'D': IO_PORTA_05 IO_PORTA_06
具体要选择哪个iic口去board_jl701n_demo.c中设置hw_iic_cfg
*/
#define TCFG_HW_I2C0_PORTS 'D'
#define TCFG_HW_I2C0_CLK 100000 //硬件IIC波特率
#define TCFG_HW_I2C0_PORTS 'B'
#define TCFG_HW_I2C0_CLK 4000000 //硬件IIC波特率:100k
//*********************************************************************************//
// 硬件SPI 配置 //
@ -915,7 +918,7 @@ DAC硬件上的连接方式,可选的配置:
#define TCFG_STK8321_EN 0
#define TCFG_IRSENSOR_ENABLE 0
#define TCFG_JSA1221_ENABLE 0
#define TCFG_GSENOR_USER_IIC_TYPE 0 //0:软件IIC 1:硬件IIC
#define TCFG_GSENOR_USER_IIC_TYPE 1 //0:软件IIC 1:硬件IIC
//*********************************************************************************//
// imu-sensor配置 //

View File

@ -202,7 +202,9 @@ void cfg_file_parse(u8 idx)
log_info("read bt name err");
} else if (ret >= LOCAL_NAME_LEN) {
memset(bt_cfg.edr_name, 0x00, LOCAL_NAME_LEN);
memcpy(bt_cfg.edr_name, tmp, LOCAL_NAME_LEN);
// memcpy(bt_cfg.edr_name, tmp, LOCAL_NAME_LEN);
extern char xt_ble_new_name[9];
memcpy(bt_cfg.edr_name, xt_ble_new_name, LOCAL_NAME_LEN);
bt_cfg.edr_name[LOCAL_NAME_LEN - 1] = 0;
} else {
memset(bt_cfg.edr_name, 0x00, LOCAL_NAME_LEN);

View File

@ -1,14 +1,6 @@
/*
动态ZUPT+卡尔曼+巴特沃斯一阶滤波器
针对启动滑雪和停止滑雪,设置不同阈值
启动滑雪和ZUPT更新的陀螺仪方差阈值分开设置
- 启动滑雪的陀螺仪阈值会更宽松一些
原地旋转和ZUPT更新的加速度方差阈值分开设置
- 原地旋转的加速度阈值更宽松
能够从静止状态到变化状态,去根据阈值来判断这个“变化”:进入滑行状态 / 只是原地摆动
- 但是还是不够灵敏
添加了滑雪过程中转弯判断,参数待调
虽然sensor_processing_task是10ms调用一次
但是实际上上一次调用该函数的时间点和下一次调用该函数的时间点会相差40ms
*/
#include "skiing_tracker.h"
@ -32,6 +24,12 @@
// 陀螺仪方差阈值,以允许启动瞬间的正常抖动,但仍能过滤掉混乱的、非滑雪的晃动。
#define SKIING_GYR_VARIANCE_THRESHOLD 15.0f
// --- 滑雪过程 ---
//加速度 模长,低于此值视为 在做匀速运动
#define SKIING_ACC_MAG_THRESHOLD 0.5f
//陀螺仪 模长,高于此值视为 摔倒了
#define FALLEN_GRY_MAG_THRESHOLD 1000.0f //未确定
// --- 原地旋转抖动 ---
// 用于原地旋转判断的加速度方差阈值。此值比ZUPT阈值更宽松
// 以允许原地旋转时身体的正常晃动,但仍能与真实滑行时的剧烈加速度变化区分开。
@ -62,8 +60,30 @@ BLE_KS_send_data_t KS_data;
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 初始化滑雪追踪器
*
@ -76,7 +96,7 @@ void skiing_tracker_init(skiing_tracker_t *tracker)
}
// 使用memset一次性清零整个结构体包括新增的缓冲区
memset(tracker, 0, sizeof(skiing_tracker_t));
tracker->state = SKIING_STATE_STATIC;
tracker->state = STATIC;
}
/**
@ -154,7 +174,7 @@ static float calculate_variance(float buffer[VARIANCE_BUFFER_SIZE][3])
/**
* @brief 状态机更新
*
* @param tracker
* @param tracker 传入同步修改后传出
* @param acc_device_ms2 三轴加速度m/s^2
* @param gyr_dps 三轴陀螺仪dps
*/
@ -162,7 +182,7 @@ static void update_state_machine(skiing_tracker_t *tracker, const float *acc_dev
{
// 缓冲区未填满时,不进行状态判断,默认为静止
if (!tracker->buffer_filled) {
tracker->state = SKIING_STATE_STATIC;
tracker->state = STATIC;
return;
}
@ -171,49 +191,119 @@ static void update_state_machine(skiing_tracker_t *tracker, const float *acc_dev
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]);
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]);
#ifdef XTELL_TEST
debug1.acc_variance =acc_variance;
debug1.gyr_variance =gyr_variance;
debug1.gyr_magnitude=gyr_magnitude;
debug1.acc_magnitude=acc_magnitude - G_ACCELERATION;
debug1.acc_magnitude=fabsf(acc_magnitude - G_ACCELERATION);
#endif
// --- 状态切换逻辑 (按优先级) ---
#if 0
//正在滑雪
if(tracker->state == NO_CONSTANT_SPEED ) {
//摔倒了
if(gyr_magnitude > FALLEN_GRY_MAG_THRESHOLD){
tracker->state = FALLEN;
return;
}
//可能进入了匀速状态
if(gyr_magnitude > SKIING_GYR_MAG_THRESHOLD && acc_magnitude < SKIING_ACC_MAG_THRESHOLD){
tracker->state = CONSTANT_SPEED;
return;
}
//急转弯
if(gyr_magnitude > WHEEL_GYR_MAG_THRESHOLD && acc_variance > WHEEL_ACC_VARIANCE_THRESHOLD){
//TODO可以考虑清掉速度消除积分带来的漂移
tracker->state = WHEEL;
return;
}
}
// 优先级1动态零速更新 (ZUPT) - 最严格和最优先的“刹车”
// --- 状态切换逻辑 (按优先级) ---
// 优先级1静止
if (acc_variance < ZUPT_ACC_VARIANCE_THRESHOLD && gyr_variance < ZUPT_GYR_VARIANCE_THRESHOLD) {
tracker->state = SKIING_STATE_STATIC;
tracker->state = STATIC;
// 速度清零,抑制漂移
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));
return;
}
if(tracker->state == SKIING_STATE_SKIING && gyr_magnitude > WHEEL_GYR_MAG_THRESHOLD && acc_variance > WHEEL_ACC_VARIANCE_THRESHOLD){
//TODO可以考虑清掉速度消除积分带来的漂移
tracker->state = SKIING_STATE_ROTATING;
return;
}
// 优先级2原地旋转 - 特殊的、非滑雪的运动状态
// 条件:角速度很大,同时线性加速度的晃动在一个“中等”范围内。
if (tracker->state == SKIING_STATE_STATIC && gyr_magnitude > ROTATION_GYR_MAG_THRESHOLD && acc_variance < ROTATING_ACC_VARIANCE_THRESHOLD) {
tracker->state = SKIING_STATE_ROTATING;
if (tracker->state == STATIC && gyr_magnitude > ROTATION_GYR_MAG_THRESHOLD && acc_variance < ROTATING_ACC_VARIANCE_THRESHOLD) {
tracker->state = ROTATING;
return;
}
// 优先级3启动滑雪 - “油门”
// 条件:有足够大的线性加速度,同时陀螺仪的抖动在一个“合理”(而非“完全静止”)的范围内。
if (fabsf(acc_magnitude - G_ACCELERATION) > START_SKIING_ACC_THRESHOLD && gyr_variance < SKIING_GYR_VARIANCE_THRESHOLD) {
tracker->state = SKIING_STATE_SKIING;
tracker->state = NO_CONSTANT_SPEED;
return;
}
// 如果不满足任何启动或停止条件,则保持当前状态(滑雪中)
// 如果当前是静止或旋转但没有满足启动条件则状态会保持直到满足ZUPT或旋转条件。
#else
// 无论当前是什么状态,静止总是最高优先级
if (acc_variance < ZUPT_ACC_VARIANCE_THRESHOLD && gyr_variance < ZUPT_GYR_VARIANCE_THRESHOLD) {
tracker->state = STATIC;
// 速度清零,抑制漂移
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));
return;
}
switch (tracker->state) {
case STATIC:
//不break会往下执行判断是否进入非匀速状态
case ROTATING: // 从静止或原地旋转可以启动
if (fabsf(acc_magnitude - G_ACCELERATION) > START_SKIING_ACC_THRESHOLD && gyr_variance < SKIING_GYR_VARIANCE_THRESHOLD) {
tracker->state = NO_CONSTANT_SPEED;
} else if (gyr_magnitude > ROTATION_GYR_MAG_THRESHOLD && acc_variance < ROTATING_ACC_VARIANCE_THRESHOLD) {
tracker->state = ROTATING;
}
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_SKIING_ACC_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;
}
#endif
}
@ -231,6 +321,8 @@ void skiing_tracker_update(skiing_tracker_t *tracker, float *acc_g, float *gyr_d
if (!tracker || !acc_g || !gyr_dps || !angle || dt <= 0) {
return;
}
if(my_skiing_tracker.state == STOP_DETECTION)
return;
// --- 数据预处理和缓冲 ---
float acc_device_ms2[3];
@ -251,8 +343,6 @@ void skiing_tracker_update(skiing_tracker_t *tracker, float *acc_g, float *gyr_d
// --- 更新状态机 ---
update_state_machine(tracker, acc_device_ms2, gyr_dps);
// --- 根据状态进行计算 ---
if (tracker->state == SKIING_STATE_SKIING) {
// 坐标转换 & 移除重力
transform_acc_to_world_frame(acc_device_ms2, angle, tracker->acc_world);
tracker->acc_world[2] -= G_ACCELERATION;
@ -262,11 +352,11 @@ void skiing_tracker_update(skiing_tracker_t *tracker, float *acc_g, float *gyr_d
tracker->acc_world_filtered[i] = HPF_ALPHA * (tracker->acc_world_filtered[i] + tracker->acc_world[i] - tracker->acc_world_unfiltered_prev[i]);
tracker->acc_world_unfiltered_prev[i] = tracker->acc_world[i];
}
// 应用加速度死区,忽略微小抖动和噪声
float acc_horizontal_mag = sqrtf(tracker->acc_world_filtered[0] * tracker->acc_world_filtered[0] +
tracker->acc_world_filtered[1] * tracker->acc_world_filtered[1]);
// --- 根据状态进行计算 ---
if (tracker->state == NO_CONSTANT_SPEED) {
if (acc_horizontal_mag > ACC_DEAD_ZONE_THRESHOLD) {
// 只有当水平加速度足够大时,才进行速度积分
tracker->velocity[0] += tracker->acc_world_filtered[0] * dt;
@ -279,6 +369,20 @@ void skiing_tracker_update(skiing_tracker_t *tracker, float *acc_g, float *gyr_d
#endif
// 如果加速度小于阈值,则不更新速度,相当于速度保持不变(或受下一步的阻尼影响而衰减)
// --- 更新速率和距离 ---
// 只基于水平速度计算速率和距离
tracker->speed = sqrtf(tracker->velocity[0] * tracker->velocity[0] +
tracker->velocity[1] * tracker->velocity[1]);
tracker->distance += tracker->speed * dt;
}else if(tracker->state == CONSTANT_SPEED){ //匀速
#ifdef XTELL_TEST
debug2.acc_magnitude = acc_horizontal_mag;
#endif
//保持上次的速度不变。只更新距离
tracker->distance += tracker->speed * dt;
}else{
// 在静止或旋转状态下,速度已经在状态机内部被清零
// 额外增加速度衰减,模拟摩擦力,进一步抑制漂移
@ -287,11 +391,7 @@ void skiing_tracker_update(skiing_tracker_t *tracker, float *acc_g, float *gyr_d
tracker->velocity[2] = 0; // 垂直速度强制归零
}
// --- 更新速率和距离 ---
// 只基于水平速度计算速率和距离
tracker->speed = sqrtf(tracker->velocity[0] * tracker->velocity[0] +
tracker->velocity[1] * tracker->velocity[1]);
tracker->distance += tracker->speed * dt;
}
@ -304,7 +404,7 @@ void skiing_tracker_update(skiing_tracker_t *tracker, float *acc_g, float *gyr_d
* @return BLE_send_data_t
*/
BLE_send_data_t sensor_processing_task(signed short * acc_data_buf, signed short * gyr_data_buf) {
static skiing_tracker_t my_skiing_tracker;
static int initialized = 0;
static int calibration_done = 0;
@ -316,9 +416,6 @@ BLE_send_data_t sensor_processing_task(signed short * acc_data_buf, signed short
const float delta_time = DELTA_TIME + 0.03f;
BLE_send_data_t BLE_send_data;
if (!initialized) {
skiing_tracker_init(&my_skiing_tracker);
initialized = 1;

View File

@ -4,11 +4,14 @@
#include "../xtell.h"
// 定义滑雪者可能的状态
typedef enum {
SKIING_STATE_STATIC, // 静止或动态稳定
SKIING_STATE_SKIING, // 正在滑雪
SKIING_STATE_ROTATING, // 正在原地旋转 (新增)
SKIING_STATE_FALLEN, // 已摔倒
SKIING_STATE_UNKNOWN // 未知状态
STATIC, // 静止或动态稳定
NO_CONSTANT_SPEED, // 正在滑雪,非匀速
CONSTANT_SPEED, // 正在滑雪,匀速
ROTATING, // 正在原地旋转
WHEEL, // 转弯
FALLEN, // 已摔倒
STOP_DETECTION, // 停止检测
UNKNOWN // 未知状态
} skiing_state_t;
#define VARIANCE_BUFFER_SIZE 5 // 用于计算方差的数据窗口大小 (5个样本 @ 100Hz = 50ms),减小延迟,提高实时性

View File

@ -1,4 +1,4 @@
# 时间间隔
# 时间间隔 -- 软件模拟iic的情况下
目前测试代码如下:

View File

@ -1,117 +1,60 @@
#include "circle_buffer.h"
#include <string.h>
//////////////////////////////////////////////////////////////////////////////////////////////////
//START -- 宏定义
#define ENABLE_XLOG 1
#ifdef xlog
#undef xlog
#endif
#if ENABLE_XLOG
#define xlog(format, ...) printf("[%s] " format, __func__, ##__VA_ARGS__)
#else
#define xlog(format, ...) ((void)0)
#endif
//END -- 宏定义
//////////////////////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////////////////////
//START -- 变量定义
//END -- 变量定义
//////////////////////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////////////////////
//START -- 函数定义
//END -- 函数定义
//////////////////////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////////////////////
//实现
// 初始化环形缓冲区
void circle_buffer_init(circle_buffer_t *cb, u8 *buffer, u16 capacity) {
cb->buffer = buffer;
void circle_buffer_init(circle_buffer_t *cb, void *buffer, u16 capacity, u16 element_size) {
cb->buffer = (u8 *)buffer;
cb->capacity = capacity;
cb->element_size = element_size;
cb->head = 0;
cb->tail = 0;
cb->size = 0;
}
// 向环形缓冲区写入数据
u16 circle_buffer_write(circle_buffer_t *cb, const u8 *data, u16 length) {
if (length > circle_buffer_get_free_space(cb)) {
// 如果剩余空间不足,则只写入能放下的部分
length = circle_buffer_get_free_space(cb);
// 向环形缓冲区写入一个元素
bool circle_buffer_write(circle_buffer_t *cb, const void *element) {
if (circle_buffer_is_full(cb)) {
return false; // 缓冲区已满
}
if (length == 0) {
return 0;
u8 *dest = cb->buffer + (cb->head * cb->element_size);
memcpy(dest, element, cb->element_size);
cb->head = (cb->head + 1) % cb->capacity;
cb->size++;
return true;
}
// 检查是否需要回环
if (cb->head + length > cb->capacity) {
u16 part1_len = cb->capacity - cb->head;
u16 part2_len = length - part1_len;
memcpy(cb->buffer + cb->head, data, part1_len);
memcpy(cb->buffer, data + part1_len, part2_len);
cb->head = part2_len;
} else {
memcpy(cb->buffer + cb->head, data, length);
cb->head += length;
if (cb->head == cb->capacity) {
cb->head = 0;
}
// 从环形缓冲区读取一个元素
bool circle_buffer_read(circle_buffer_t *cb, void *element) {
if (circle_buffer_is_empty(cb)) {
return false; // 缓冲区为空
}
cb->size += length;
return length;
u8 *src = cb->buffer + (cb->tail * cb->element_size);
memcpy(element, src, cb->element_size);
cb->tail = (cb->tail + 1) % cb->capacity;
cb->size--;
return true;
}
// 从环形缓冲区读取数据
u16 circle_buffer_read(circle_buffer_t *cb, u8 *data, u16 length) {
if (length > cb->size) {
// 如果要读取的长度超过了已有的数据,则只读取已有的部分
length = cb->size;
}
if (length == 0) {
return 0;
}
// 检查是否需要回环
if (cb->tail + length > cb->capacity) {
u16 part1_len = cb->capacity - cb->tail;
u16 part2_len = length - part1_len;
memcpy(data, cb->buffer + cb->tail, part1_len);
memcpy(data + part1_len, cb->buffer, part2_len);
cb->tail = part2_len;
} else {
memcpy(data, cb->buffer + cb->tail, length);
cb->tail += length;
if (cb->tail == cb->capacity) {
cb->tail = 0;
}
}
cb->size -= length;
return length;
}
// 获取已用空间的大小
// 获取已用空间的大小(以元素为单位)
u16 circle_buffer_get_size(circle_buffer_t *cb) {
return cb->size;
}
// 获取剩余空间的大小
// 获取剩余空间的大小(以元素为单位)
u16 circle_buffer_get_free_space(circle_buffer_t *cb) {
return cb->capacity - cb->size;
}
// 检查缓冲区是否已满
bool circle_buffer_is_full(circle_buffer_t *cb) {
return cb->size == cb->capacity;
}
// 检查缓冲区是否为空
bool circle_buffer_is_empty(circle_buffer_t *cb) {
return cb->size == 0;
}

View File

@ -6,50 +6,65 @@
// 定义环形缓冲区的结构体
typedef struct {
u8 *buffer; // 缓冲区指针
u16 capacity; // 缓冲区总容量
u16 head; // 头部指针(写入位置
u16 tail; // 部指针(读取位置
u16 size; // 当前已用大小
u16 capacity; // 缓冲区总容量(以元素为单位)
u16 element_size; // 每个元素的大小(以字节为单位
u16 head; // 部指针(写入位置,以元素为单位
u16 tail; // 尾部指针(读取位置,以元素为单位)
u16 size; // 当前已用大小(以元素为单位)
} circle_buffer_t;
/**
* @brief 初始化环形缓冲区
* @param cb 指向环形缓冲区结构体的指针
* @param buffer 外部提供的缓冲区内存
* @param capacity 缓冲区的总容量
* @param capacity 缓冲区的总容量(以元素数量为单位)
* @param element_size 每个元素的大小(字节)
*/
void circle_buffer_init(circle_buffer_t *cb, u8 *buffer, u16 capacity);
void circle_buffer_init(circle_buffer_t *cb, void *buffer, u16 capacity, u16 element_size);
/**
* @brief 向环形缓冲区写入数据
* @brief 向环形缓冲区写入一个元素
* @param cb 指向环形缓冲区结构体的指针
* @param data 要写入的数据的指针
* @param length 要写入的数据的长度
* @return 实际写入的字节数
* @param element 要写入的元素的指针
* @return 成功返回true失败返回false
*/
u16 circle_buffer_write(circle_buffer_t *cb, const u8 *data, u16 length);
bool circle_buffer_write(circle_buffer_t *cb, const void *element);
/**
* @brief 从环形缓冲区读取数据
* @brief 从环形缓冲区读取一个元素
* @param cb 指向环形缓冲区结构体的指针
* @param data 用于存放读取数据的缓冲区的指针
* @param length 想要读取的数据的长度
* @return 实际读取的字节数
* @param element 用于存放读取元素的缓冲区的指针
* @return 成功返回true失败返回false
*/
u16 circle_buffer_read(circle_buffer_t *cb, u8 *data, u16 length);
bool circle_buffer_read(circle_buffer_t *cb, void *element);
/**
* @brief 获取环形缓冲区中已用空间的大小
* @brief 获取环形缓冲区中已用空间的大小(以元素为单位)
* @param cb 指向环形缓冲区结构体的指针
* @return 已用空间的大小
* @return 已用空间的大小(元素数量)
*/
u16 circle_buffer_get_size(circle_buffer_t *cb);
/**
* @brief 获取环形缓冲区中剩余空间的大小
* @brief 获取环形缓冲区中剩余空间的大小(以元素为单位)
* @param cb 指向环形缓冲区结构体的指针
* @return 剩余空间的大小
* @return 剩余空间的大小(元素数量)
*/
u16 circle_buffer_get_free_space(circle_buffer_t *cb);
/**
* @brief 检查缓冲区是否已满
* @param cb 指向环形缓冲区结构体的指针
* @return 如果已满返回true否则返回false
*/
bool circle_buffer_is_full(circle_buffer_t *cb);
/**
* @brief 检查缓冲区是否为空
* @param cb 指向环形缓冲区结构体的指针
* @return 如果为空返回true否则返回false
*/
bool circle_buffer_is_empty(circle_buffer_t *cb);
#endif // CIRCLE_BUFFER_H

View File

@ -17,18 +17,23 @@
#define ZUPT_ACC_VARIANCE_THRESHOLD 0.2f
// 陀螺仪方差阈值
#define ZUPT_GYR_VARIANCE_THRESHOLD 5.0f
// 静止时候的陀螺仪模长
#define ZUPT_GYR_MAG_THRESHOLD 15
// --- --- ---
// --- 启动滑雪阈值 ---
// 加速度模长与重力的差值大于此值,认为开始运动;降低阈值,让“油门”更灵敏,以便能捕捉到真实的慢速启动
#define START_SKIING_ACC_THRESHOLD 0.5f
// 陀螺仪方差阈值,以允许启动瞬间的正常抖动,但仍能过滤掉混乱的、非滑雪的晃动。
#define SKIING_GYR_VARIANCE_THRESHOLD 15.0f
// --- --- ---
// --- 滑雪过程 ---
//加速度 模长,低于此值视为 在做匀速运动
#define SKIING_ACC_MAG_THRESHOLD 0.5f
//陀螺仪 模长,高于此值视为 摔倒了
#define FALLEN_GRY_MAG_THRESHOLD 1000.0f //未确定
#define FALLEN_GRY_MAG_THRESHOLD 2000.0f //未确定
// --- --- ---
// --- 原地旋转抖动 ---
// 用于原地旋转判断的加速度方差阈值。此值比ZUPT阈值更宽松
@ -36,17 +41,36 @@
#define ROTATING_ACC_VARIANCE_THRESHOLD 0.8f
// 旋转/摆动检测阈值:角速度总模长大于此值(度/秒),认为正在进行非滑雪的旋转或摆动
#define ROTATION_GYR_MAG_THRESHOLD 120.0f
// --- --- ---
// --- 滑雪转弯动 ---
// 加速度方差阈值,大于此值,滑雪过程可能发生了急转弯
#define WHEEL_ACC_VARIANCE_THRESHOLD 7.0f
// 旋转/摆动检测阈值:角速度总模长大于此值(度/秒),认为滑雪过程中进行急转弯
#define WHEEL_GYR_MAG_THRESHOLD 220.0f // 150.0f 到 250.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电路而来
// alpha = RC / (RC + dt)参考RC电路而来fc ≈ (1 - alpha) / (2 * π * dt)
#define HPF_ALPHA 0.995
//0.995 0.08 Hz 的信号
// --- --- ---
// 加速度死区阈值 (m/s^2)。低于此阈值的加速度被认为是噪声,不参与积分。
// 设得太高会忽略真实的慢速启动,设得太低则无法有效抑制噪声。
@ -54,9 +78,10 @@
// --- 模拟摩擦力,进行速度衰减 ---
#define SPEED_ATTENUATION 1.0f //暂不模拟
BLE_KS_send_data_t KS_data;
#ifdef XTELL_TEST
BLE_KS_send_data_t KS_data;
debug_t debug1;
debug_t debug2;
#endif
@ -99,6 +124,18 @@ 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;
}
/**
* @brief 将设备坐标系下的加速度转换为世界坐标系
* @param acc_device 设备坐标系下的加速度 [x, y, z]
@ -169,7 +206,17 @@ static float calculate_variance(float buffer[VARIANCE_BUFFER_SIZE][3])
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 状态机更新
@ -191,6 +238,7 @@ static void update_state_machine(skiing_tracker_t *tracker, const float *acc_dev
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]);
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]);
float acc_magnitude_g = acc_magnitude / G_ACCELERATION; // 转换为g单位用于跳跃判断
#ifdef XTELL_TEST
debug1.acc_variance =acc_variance;
@ -199,84 +247,88 @@ static void update_state_machine(skiing_tracker_t *tracker, const float *acc_dev
debug1.acc_magnitude=fabsf(acc_magnitude - G_ACCELERATION);
#endif
#if 0
//正在滑雪
if(tracker->state == NO_CONSTANT_SPEED ) {
//摔倒了
if(gyr_magnitude > FALLEN_GRY_MAG_THRESHOLD){
tracker->state = FALLEN;
return;
// --- 状态机逻辑 (核心修改区域) ---
#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;
}
//可能进入了匀速状态
if(gyr_magnitude > SKIING_GYR_MAG_THRESHOLD && acc_magnitude < SKIING_ACC_MAG_THRESHOLD){
tracker->state = CONSTANT_SPEED;
return;
} else {
tracker->grounded_entry_counter = 0;
}
//急转弯
if(gyr_magnitude > WHEEL_GYR_MAG_THRESHOLD && acc_variance > WHEEL_ACC_VARIANCE_THRESHOLD){
//TODO可以考虑清掉速度消除积分带来的漂移
tracker->state = WHEEL;
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;
}
}
// --- 状态切换逻辑 (按优先级) ---
// 优先级1静止
if (acc_variance < ZUPT_ACC_VARIANCE_THRESHOLD && gyr_variance < ZUPT_GYR_VARIANCE_THRESHOLD) {
tracker->state = STATIC;
// 速度清零,抑制漂移
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));
// 只有在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;
}
// 优先级2原地旋转 - 特殊的、非滑雪的运动状态
// 条件:角速度很大,同时线性加速度的晃动在一个“中等”范围内。
if (tracker->state == STATIC && gyr_magnitude > ROTATION_GYR_MAG_THRESHOLD && acc_variance < ROTATING_ACC_VARIANCE_THRESHOLD) {
tracker->state = ROTATING;
return;
}
// 优先级3启动滑雪 - “油门”
// 条件:有足够大的线性加速度,同时陀螺仪的抖动在一个“合理”(而非“完全静止”)的范围内。
if (fabsf(acc_magnitude - G_ACCELERATION) > START_SKIING_ACC_THRESHOLD && gyr_variance < SKIING_GYR_VARIANCE_THRESHOLD) {
} else {
// 如果在起跳冲击后一段时间内没有失重,说明只是一个颠簸,恢复滑行
// 可以加一个小的超时计数器,这里为了简单先直接恢复
tracker->state = NO_CONSTANT_SPEED;
return;
}
return; // 无论是否切换,都结束本次判断
}
#endif
// 如果不满足任何启动或停止条件,则保持当前状态(滑雪中)
// 如果当前是静止或旋转但没有满足启动条件则状态会保持直到满足ZUPT或旋转条件。
#else
// 无论当前是什么状态,静止总是最高优先级
if (acc_variance < ZUPT_ACC_VARIANCE_THRESHOLD && gyr_variance < ZUPT_GYR_VARIANCE_THRESHOLD) {
// --- 静止判断 ---
if (acc_variance < ZUPT_ACC_VARIANCE_THRESHOLD && gyr_variance < ZUPT_GYR_VARIANCE_THRESHOLD && gyr_magnitude < ZUPT_GYR_MAG_THRESHOLD) {
tracker->state = STATIC;
// 速度清零,抑制漂移
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));
return;
}
// --- 地面状态切换逻辑 ---
switch (tracker->state) {
case LANDING:
tracker->state = STATIC;
break;
case STATIC:
//不break会往下执行判断是否进入非匀速状态
case ROTATING: // 从静止或原地旋转可以启动
if (fabsf(acc_magnitude - G_ACCELERATION) > START_SKIING_ACC_THRESHOLD && gyr_variance < SKIING_GYR_VARIANCE_THRESHOLD) {
if (fabsf(acc_magnitude - G_ACCELERATION) > START_SKIING_ACC_THRESHOLD && gyr_variance > SKIING_GYR_VARIANCE_THRESHOLD) {
tracker->state = NO_CONSTANT_SPEED;
} else if (gyr_magnitude > ROTATION_GYR_MAG_THRESHOLD && acc_variance < ROTATING_ACC_VARIANCE_THRESHOLD) {
tracker->state = ROTATING;
}
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) {
//暂时不考虑摔倒
// 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; //匀速
@ -302,7 +354,7 @@ static void update_state_machine(skiing_tracker_t *tracker, const float *acc_dev
// TODO回到 STATIC
break;
}
#endif
}
@ -342,7 +394,65 @@ void skiing_tracker_update(skiing_tracker_t *tracker, float *acc_g, float *gyr_d
// --- 更新状态机 ---
update_state_machine(tracker, acc_device_ms2, gyr_dps);
#if 1
// --- 根据状态执行不同的计算逻辑 ---
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));
break;
case WHEEL:
case NO_CONSTANT_SPEED:
transform_acc_to_world_frame(acc_device_ms2, angle, tracker->acc_world);
tracker->acc_world[2] -= G_ACCELERATION;
for (int i = 0; i < 3; i++) {
tracker->acc_world_filtered[i] = HPF_ALPHA * (tracker->acc_world_filtered[i] + tracker->acc_world[i] - tracker->acc_world_unfiltered_prev[i]);
tracker->acc_world_unfiltered_prev[i] = tracker->acc_world[i];
}
float acc_horizontal_mag = sqrtf(tracker->acc_world_filtered[0] * tracker->acc_world_filtered[0] +
tracker->acc_world_filtered[1] * tracker->acc_world_filtered[1]);
if (acc_horizontal_mag > ACC_DEAD_ZONE_THRESHOLD) {
tracker->velocity[0] += tracker->acc_world_filtered[0] * dt;
tracker->velocity[1] += tracker->acc_world_filtered[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:
// 速度清零,抑制漂移
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));
break;
case ROTATING:
tracker->speed = 0.0f;
break;
case FALLEN:
// TODO
break;
default:
break;
}
#else
// 坐标转换 & 移除重力
transform_acc_to_world_frame(acc_device_ms2, angle, tracker->acc_world);
tracker->acc_world[2] -= G_ACCELERATION;
@ -356,7 +466,7 @@ void skiing_tracker_update(skiing_tracker_t *tracker, float *acc_g, float *gyr_d
float acc_horizontal_mag = sqrtf(tracker->acc_world_filtered[0] * tracker->acc_world_filtered[0] +
tracker->acc_world_filtered[1] * tracker->acc_world_filtered[1]);
// --- 根据状态进行计算 ---
if (tracker->state == NO_CONSTANT_SPEED) {
if (tracker->state == NO_CONSTANT_SPEED) { //非匀速
if (acc_horizontal_mag > ACC_DEAD_ZONE_THRESHOLD) {
// 只有当水平加速度足够大时,才进行速度积分
tracker->velocity[0] += tracker->acc_world_filtered[0] * dt;
@ -377,12 +487,13 @@ void skiing_tracker_update(skiing_tracker_t *tracker, float *acc_g, float *gyr_d
}else if(tracker->state == CONSTANT_SPEED){ //匀速
#ifdef XTELL_TEST
debug2.acc_magnitude = acc_horizontal_mag;
#endif
//保持上次的速度不变。只更新距离
tracker->distance += tracker->speed * dt;
}else if(tracker->state == WHEEL){ //转弯
//暂时:保持上次的速度不变。只更新距离
tracker->distance += tracker->speed * dt;
}else{
// 在静止或旋转状态下,速度已经在状态机内部被清零
// 额外增加速度衰减,模拟摩擦力,进一步抑制漂移
@ -391,14 +502,14 @@ void skiing_tracker_update(skiing_tracker_t *tracker, float *acc_g, float *gyr_d
tracker->velocity[2] = 0; // 垂直速度强制归零
}
#endif
}
#if 0
/**
* @brief 传感器数据采集与处理任务外部每10ms调用一次如果需要更新时间间隔也需要同步更新宏“ DELTA_TIME ”
*
* 使用到了卡尔曼
* @param acc_data_buf 三轴加速度原始数据
* @param gyr_data_buf 三轴陀螺仪原始数据
* @return BLE_send_data_t
@ -427,14 +538,14 @@ BLE_send_data_t sensor_processing_task(signed short * acc_data_buf, signed short
unsigned char status;
if (!calibration_done) { //第1次启动开启零漂检测
status = SL_SC7U22_Angle_Output(1, combined_raw_data, final_angle_data, 0);
status = Original_SL_SC7U22_Angle_Output(1, combined_raw_data, final_angle_data, 0);
if (status == 1) {
calibration_done = 1;
printf("Sensor calibration successful! Skiing mode is active.\n");
}
} else {
// printf("Calculate the time interval =============== start\n");
status = SL_SC7U22_Angle_Output(0, combined_raw_data, final_angle_data, 0);
status = Original_SL_SC7U22_Angle_Output(0, combined_raw_data, final_angle_data, 0);
}
if (status == 1) {
@ -469,10 +580,11 @@ BLE_send_data_t sensor_processing_task(signed short * acc_data_buf, signed short
BLE_send_data.sensor_state = status;
BLE_send_data.skiing_state = my_skiing_tracker.state;
for (int i = 0; i < 3; i++) {
#ifndef XTELL_TEST
// #ifndef XTELL_TEST
BLE_send_data.acc_original[i] = (int)acc_data_buf[i];
BLE_send_data.gyr_original[i] = (int)gyr_data_buf[i];
#endif
BLE_send_data.Angle[i] = final_angle_data[i];
// #endif
#if KS_BLE
KS_data.acc_KS[i] = (int)(calibrated_acc_g[i] * G_ACCELERATION * 100); //cm/s^s
KS_data.gyr_KS_dps[i] = (int)calibrated_gyr_dps[i];
@ -499,3 +611,64 @@ BLE_send_data_t sensor_processing_task(signed short * acc_data_buf, signed short
}
return BLE_send_data;
}
#else
/**
* @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) {
static int initialized = 0;
static float acc_data_g[3];
static float gyr_data_dps[3];
// 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");
}
// 加速度 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;
// 陀螺仪 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;
}
#endif

View File

@ -4,14 +4,17 @@
#include "../xtell.h"
// 定义滑雪者可能的状态
typedef enum {
STATIC, // 静止或动态稳定
NO_CONSTANT_SPEED, // 正在滑雪,非匀速
CONSTANT_SPEED, // 正在滑雪,匀速
ROTATING, // 正在原地旋转
WHEEL, // 转弯
FALLEN, // 已摔倒
STOP_DETECTION, // 停止检测
UNKNOWN // 未知状态
STATIC, // 静止或动态稳定0
NO_CONSTANT_SPEED, // 正在滑雪,非匀速1
CONSTANT_SPEED, // 正在滑雪,匀速2
ROTATING, // 正在原地旋转
WHEEL, // 转弯3
FALLEN, // 已摔倒4
TAKING_OFF, // 起跳冲击阶段5
IN_AIR, // 空中失重阶段6
LANDING, // 落地冲击阶段7
STOP_DETECTION, // 停止检测8
UNKNOWN // 未知状态9
} skiing_state_t;
#define VARIANCE_BUFFER_SIZE 5 // 用于计算方差的数据窗口大小 (5个样本 @ 100Hz = 50ms),减小延迟,提高实时性
@ -29,6 +32,12 @@ typedef struct {
// 内部计算使用的私有成员
float acc_world[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]; // 加速度数据窗口
@ -42,15 +51,14 @@ typedef struct {
} skiing_tracker_t;
//ble发送的数据
typedef struct __attribute__((packed)){ //该结构体取消内存对齐
typedef struct{ //__attribute__((packed)){ //该结构体取消内存对齐
char sensor_state;
char skiing_state;
int speed_cms; //求出的速度cm/s
int distance_cm; //求出的距离cm
#ifndef XTELL_TEST
int acc_original[3]; //直接读取传感器得到的原始三轴加速度
int gyr_original[3]; //直接读取传感器得到的原始三轴陀螺仪
#endif
short acc_data[3]; //三轴加速度, g
short gyr_data[3]; //三轴陀螺仪, dps
float angle_data[3]; //欧若拉角
}BLE_send_data_t;
typedef struct{
@ -74,12 +82,5 @@ typedef struct{
*/
void skiing_tracker_init(skiing_tracker_t *tracker);
/**
* @brief 传感器数据采集与处理任务外部每10ms调用一次如果需要更新时间间隔也需要同步更新宏“ DELTA_TIME ”
*
* @param acc_data_buf 三轴加速度原始数据
* @param gyr_data_buf 三轴陀螺仪原始数据
* @return BLE_send_data_t
*/
BLE_send_data_t sensor_processing_task(signed short * acc_data_buf, signed short * gyr_data_buf) ;
BLE_send_data_t sensor_processing_task(signed short* acc_data_buf, signed short* gyr_data_buf, float* angle_data) ;
#endif // SKIING_TRACKER_H

View File

@ -15,9 +15,8 @@
#include "bt_profile_cfg.h"
#include "dev_manager/dev_manager.h"
#include "update_loader_download.h"
#include "LIS2DH12.h"
#include "circle_buffer.h"
#include "circle_buffer.h"
#include "./sensor/SC7U22.h"
#include "./buffer/circle_buffer.h"
#include "btstack/avctp_user.h"
#include "calculate/skiing_tracker.h"
#include "xtell.h"
@ -49,14 +48,16 @@ extern void create_process(u16* pid, const char* name, void *priv, void (*func)(
// --- 任务ID ---
static u16 xtell_i2c_test_id;
static u16 collect_data_id;
static u16 send_data_id;
static u16 ble_send_data_id;
static u16 sensor_read_data_id;
static u16 calculate_data_id;
// --- 环形缓冲区 ---
#define SENSOR_DATA_BUFFER_SIZE 512
static u8 sensor_data_buffer[SENSOR_DATA_BUFFER_SIZE];
static circle_buffer_t sensor_cb;
BLE_send_data_t BLE_send_data;
//--- test ---
// 全局变量
u16 gsensor_id=0;
@ -143,7 +144,8 @@ void send_sensor_data_task(void) {
// printf("xtell_ble_send\n");
}
#ifdef XTELL_TEST
#if 0
BLE_send_data_t BLE_send_data;
void test(){
signed short acc_data_buf[3] = {0};
signed short gyr_data_buf[3] = {0};
@ -238,43 +240,232 @@ void test(){
// xlog("end============\n");
}
#else
#endif
void BLE_send_data(){
signed short acc_data_buf[3] = {0};
signed short gyr_data_buf[3] = {0};
signed short acc_gyro_input[6] = {0};
float Angle_output[3] = {0};
SL_SC7U22_RawData_Read(acc_data_buf,gyr_data_buf);
BLE_send_data = sensor_processing_task(acc_data_buf, gyr_data_buf);
u8 data[50];
data[0] = 0xBB;
data[1] = 0xBE;
data[2] = 0x01;
data[3] = sizeof(BLE_send_data_t); //后续包的数据长度
// send_data_to_ble_client(&data,sizeof(BLE_send_data_t)+4);
memcpy(&data[4], &BLE_send_data, sizeof(BLE_send_data_t));
#define SENSOR_DATA_BUFFER_SIZE 100 // 定义缓冲区可以存储100个sensor_data_t元素
static circle_buffer_t sensor_read; // 环形缓冲区管理结构体
typedef struct {
signed short acc_data[3];
signed short gyr_data[3];
float angle[3];
} sensor_data_t;
static sensor_data_t sensor_read_buffer[SENSOR_DATA_BUFFER_SIZE]; // 存放sensor读到的数据
static circle_buffer_t sensor_send; // 环形缓冲区管理结构体
static BLE_send_data_t sensor_send_buffer[SENSOR_DATA_BUFFER_SIZE]; // 存放ble要发送的数据
/**
* @brief //读取传感器的数据放进缓冲区
*
*/
void sensor_read_data(){
// xlog("=======sensor_read_data START\n");
static signed short combined_raw_data[6];
static int initialized = 0;
static int calibration_done = 0;
char status = 0;
if(circle_buffer_is_full(&sensor_read)){
// xlog("sensor_read_data: read buffer full\n");
return;
}
static sensor_data_t tmp;
SL_SC7U22_RawData_Read(tmp.acc_data,tmp.gyr_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));
if (!calibration_done) { //第1次启动开启零漂检测
status = SL_SC7U22_Angle_Output(1, combined_raw_data, tmp.angle, 0);
if (status == 1) {
calibration_done = 1;
printf("Sensor calibration successful! Skiing mode is active.\n");
}
} else {
// printf("Calculate the time interval =============== start\n");
status = SL_SC7U22_Angle_Output(0, combined_raw_data, tmp.angle, 0);
memcpy(tmp.acc_data, &combined_raw_data[0], 3 * sizeof(signed short));
memcpy(tmp.gyr_data, &combined_raw_data[3], 3 * sizeof(signed short));
circle_buffer_write(&sensor_read, &tmp);
}
// xlog("=======sensor_read_data END\n");
}
void calculate_data(){
// xlog("=======start\n");
sensor_data_t tmp;
if(circle_buffer_is_empty(&sensor_read)){
// xlog("sensor_read_buffer: read buffer empty\n");
return;
}
circle_buffer_read(&sensor_read, &tmp);
BLE_send_data_t data_by_calculate = sensor_processing_task(tmp.acc_data, tmp.gyr_data,tmp.angle);
if(circle_buffer_is_full(&sensor_send))
return;
circle_buffer_write(&sensor_send, &data_by_calculate);
// extern void BLE_send_data();
// BLE_send_data();
// xlog("=======end\n");
}
static int count = 0;
if(count >=10){
extern char xt_Check_Flag;
void BLE_send_data(){
if(circle_buffer_is_empty(&sensor_send)){
// xlog("sensor_send_buffer: send buffer empty\n");
return;
}
#ifdef XTELL_TEST
BLE_send_data_t tmp;
circle_buffer_read(&sensor_send, &tmp);
if(count >=50){
int num_chars_written;
count = 0;
send_data_to_ble_client(&data,sizeof(BLE_send_data_t)+4);
char* division = "==========\n";
send_data_to_ble_client(division,strlen(division));
char log_buffer[100]; // 100个字符应该足够了
// extern char iic_read_len;
// extern char iic_write_result;
// num_chars_written = snprintf(log_buffer, sizeof(log_buffer),"SL_SC7U22_Check=0x%d, %d, %d\n", xt_Check_Flag, iic_read_len, iic_write_result);
// send_data_to_ble_client(&log_buffer,strlen(log_buffer));
memset(&log_buffer, 0, 100);
// 使用 snprintf 进行格式化
num_chars_written = snprintf(
log_buffer, // 目标缓冲区
sizeof(log_buffer), // 目标缓冲区的最大容量
"s %d, %dcm/s, %dcm\n", // 格式化字符串
tmp.skiing_state, // 第一个 %d 的参数
tmp.speed_cms, // 第二个 %d 的参数
tmp.distance_cm // 第三个 %d 的参数
);
send_data_to_ble_client(&log_buffer, strlen(log_buffer));
memset(&log_buffer, 0, 100);
num_chars_written = snprintf(
log_buffer,
sizeof(log_buffer),
"Acc:%d, %d, %d\n",
tmp.acc_data[0],tmp.acc_data[1],tmp.acc_data[2]
);
send_data_to_ble_client(&log_buffer,strlen(log_buffer));
memset(&log_buffer, 0, 100);
num_chars_written = snprintf(
log_buffer,
sizeof(log_buffer),
"Gyr:%d, %d, %d\n",
tmp.gyr_data[0],tmp.gyr_data[1],tmp.gyr_data[2]
);
send_data_to_ble_client(&log_buffer,strlen(log_buffer));
memset(&log_buffer, 0, 100);
num_chars_written = snprintf(
log_buffer,
sizeof(log_buffer),
"Angle:%.1f, %.1f, %1.f\n",
tmp.angle_data[0],tmp.angle_data[1],tmp.angle_data[2]
);
send_data_to_ble_client(&log_buffer,strlen(log_buffer));
short acc_mo_cms = sqrtf(tmp.acc_data[0]*tmp.acc_data[0] + tmp.acc_data[1]*tmp.acc_data[1] + tmp.acc_data[2]*tmp.acc_data[2])-900;
memset(&log_buffer, 0, 100);
num_chars_written = snprintf(
log_buffer,
sizeof(log_buffer),
"acc_cm/s^2:%d\n",
acc_mo_cms
);
send_data_to_ble_client(&log_buffer,strlen(log_buffer));
// xlog("s %d, %dcm/s, %dcm\n",tmp.skiing_state, tmp.speed_cms, tmp.distance_cm);
// xlog("Acc:%d, %d, %d\n", tmp.acc_data[0],tmp.acc_data[1],tmp.acc_data[2]);
// xlog("GYR:%d, %d, %d\n", tmp.gyr_data[0],tmp.gyr_data[1],tmp.gyr_data[2]);
}
count++;
memset(&BLE_send_data, 0, sizeof(BLE_send_data_t));
memset(&data, 0, 50);
#else
#endif
}
//iic测试调用的
#if 0
static u16 xt_iic_test_id;
char log_buffer_1[100];
extern char sen_log_buffer_1[100];
extern char sen_log_buffer_2[100];
extern char sen_log_buffer_3[100];
extern char sen_log_buffer_4[100];
extern char sen_log_buffer_5[100];
extern char w_log_buffer_1[100];
extern char w_log_buffer_2[100];
extern char w_log_buffer_3[100];
extern char w_log_buffer_4[100];
extern char w_log_buffer_5[100];
void xt_iic_test(){
char log_buffer[100];
send_data_to_ble_client(&log_buffer_1,strlen(log_buffer_1));
extern char iic_read_len;
extern char iic_write_result;
int num_chars_written = snprintf(log_buffer, sizeof(log_buffer),"SL_SC7U22_Check=0x%d,%d,%d\n", xt_Check_Flag, iic_read_len, iic_write_result);
extern void send_data_to_ble_client(const u8* data, u16 length);
send_data_to_ble_client(&log_buffer,strlen(log_buffer));
if(sen_log_buffer_1 != NULL)
send_data_to_ble_client(&sen_log_buffer_1,strlen(sen_log_buffer_1));
if(sen_log_buffer_2 != NULL)
send_data_to_ble_client(&sen_log_buffer_2,strlen(sen_log_buffer_2));
if(sen_log_buffer_3 != NULL)
send_data_to_ble_client(&sen_log_buffer_3,strlen(sen_log_buffer_3));
if(sen_log_buffer_4 != NULL)
send_data_to_ble_client(&sen_log_buffer_4,strlen(sen_log_buffer_4));
if(sen_log_buffer_5 != NULL)
send_data_to_ble_client(&sen_log_buffer_5,strlen(sen_log_buffer_5));
if(w_log_buffer_1 != NULL)
send_data_to_ble_client(&w_log_buffer_1,strlen(w_log_buffer_1));
if(w_log_buffer_2 != NULL)
send_data_to_ble_client(&w_log_buffer_2,strlen(w_log_buffer_2));
if(w_log_buffer_3 != NULL)
send_data_to_ble_client(&w_log_buffer_3,strlen(w_log_buffer_3));
if(w_log_buffer_4 != NULL)
send_data_to_ble_client(&w_log_buffer_4,strlen(w_log_buffer_4));
if(w_log_buffer_5 != NULL)
send_data_to_ble_client(&w_log_buffer_5,strlen(w_log_buffer_5));
// SL_SC7U22_Config();
}
#endif
void gsensor_test(){
sys_timer_del(gsensor_id);
#if 0
u16 xt_iic_test_id;
char hw_iic_init_result;
void xt_hw_iic_test(){
char log_buffer[100];
extern char iic_read_len;
extern char iic_write_result;
int num_chars_written = snprintf(log_buffer, sizeof(log_buffer),"init result:%d, SL_SC7U22_Check=0x%d,%d,%d\n",hw_iic_init_result, xt_Check_Flag, iic_read_len, iic_write_result);
extern void send_data_to_ble_client(const u8* data, u16 length);
send_data_to_ble_client(&log_buffer,strlen(log_buffer));
}
#endif
void xtell_task_create(void){
@ -283,29 +474,50 @@ void xtell_task_create(void){
// //初始化传感器
// SL_SC7U22_Config();
soft_iic_init(0);
#if TCFG_GSENOR_USER_IIC_TYPE
int ret = hw_iic_init(0);
xlog("init iic result:%d\n", ret); //返回0成功
#else
int ret = soft_iic_init(0);
int num_chars_written = snprintf(log_buffer_1, sizeof(log_buffer_1),"init iic: %d\n", ret);
#endif
gpio_set_direction(IO_PORTE_05,0); //设置PE5 输出模式
gpio_set_pull_up(IO_PORTE_05,1);
gpio_direction_output(IO_PORTE_05,1);
os_time_dly(10);
SL_SC7U22_Config();
// extern u8 LIS2DH12_Config(void);
// LIS2DH12_Config();
xlog("xtell_task_create\n");
// 初始化环形缓冲区
circle_buffer_init(&sensor_cb, sensor_data_buffer, SENSOR_DATA_BUFFER_SIZE);
// circle_buffer_init(&sensor_cb, sensor_data_buffer, SENSOR_DATA_BUFFER_SIZE);
circle_buffer_init(&sensor_read, sensor_read_buffer, SENSOR_DATA_BUFFER_SIZE, sizeof(sensor_data_t));
circle_buffer_init(&sensor_send, sensor_send_buffer, SENSOR_DATA_BUFFER_SIZE, sizeof(BLE_send_data_t));
//初始化滑雪追踪器
// SkiingTracker_Init(&skiing_data);
xlog("SkiingTracker_Init\n");
// create_process(&gsensor_id, "gsensor",NULL, gsensor_test, 1000);
create_process(&test_id, "test",NULL, test, (int)(DELTA_TIME*1000));
// while(1){
// test();
// os_time_dly((int)(DELTA_TIME*1000));
// }
create_process(&sensor_read_data_id, "read",NULL, sensor_read_data, 10);
create_process(&calculate_data_id, "calculate",NULL, calculate_data, 5);
create_process(&ble_send_data_id, "send",NULL, BLE_send_data, 1);
#if 0
hw_iic_init_result = ret;
create_process(&xt_iic_test_id,"iic_test",NULL,xt_hw_iic_test,1000);
#endif
}

View File

@ -4,7 +4,7 @@
#include "os/os_api.h"
#define ENABLE_XLOG 0
// #define ENABLE_XLOG 1
#ifdef xlog
#undef xlog
#endif
@ -63,20 +63,21 @@ static void sl_delay(unsigned char sl_i)
os_time_dly(sl_i);
}
char iic_read_len;
char iic_write_result;
unsigned char SL_SC7U22_Check(void)
{
unsigned char reg_value=0;
xlog("SL_SC7U22_Check\n");
SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x7F, 0x00);//goto 0x00
SL_SC7U22_I2c_Spi_Read(SL_SPI_IIC_INTERFACE, SC7U22_WHO_AM_I, 1, &reg_value);
iic_write_result = SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x7F, 0x00);//goto 0x00
iic_read_len = SL_SC7U22_I2c_Spi_Read(SL_SPI_IIC_INTERFACE, SC7U22_WHO_AM_I, 1, &reg_value);
xlog("0x%x=0x%x\r\n",SC7U22_WHO_AM_I,reg_value);
if(reg_value==0x6A) //设备的id
return 0x01;//SC7U22
else
return 0x00;//通信异常
}
char xt_Check_Flag;
unsigned char SL_SC7U22_Config(void)
{
xlog("SL_SC7U22_Config\n");
@ -102,6 +103,10 @@ unsigned char SL_SC7U22_Config(void)
// Check_Flag=1;//强制初始化
xlog("SL_SC7U22_Check=0x%x\r\n",Check_Flag);
xt_Check_Flag = Check_Flag;
if(Check_Flag==1)
{
Check_Flag= SL_SC7U22_POWER_DOWN();
@ -119,9 +124,18 @@ unsigned char SL_SC7U22_Config(void)
SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x7D, 0x0E);//PWR_CTRL ENABLE ACC+GYR+TEMP
os_time_dly(1);//10ms
SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x40, 0x06);//ACC_CONF 0x07=50Hz 0x06=25Hz
// SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x40, 0x06);//ACC_CONF 0x07=50Hz 0x06=25Hz
// SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x40, 0xA8);//高性能模式连续4个数据平均1次100Hz -- lmx
SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x40, 0xBC);//ACC_CON 高性能模式1600Hz -- lmx
// SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x40, 0xBB);//ACC_CON 高性能模式800Hz -- lmx
SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x41, 0x01);//ACC_RANGE 10±8G 01±4G
SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x42, 0x86);//GYR_CONF 0x87=50Hz 0x86=25Hz
// SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x42, 0x86);//GYR_CONF 0x87=50Hz 0x86=25Hz
// SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x42, 0x8C);//GYR_CONF 1600Hz -- lmx
SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x42, 0xAC);//GYR_CONF 1600Hz -- lmx
// SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x42, 0xAB);//GYR_CONF 800Hz -- lmx
SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x43, 0x00);//GYR_RANGE 2000dps
SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x43, 0x00);//GYR_RANGE 2000dps
SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x04, 0x50);//COM_CFG
@ -927,6 +941,140 @@ unsigned char SL_SC7U22_Angle_Output(unsigned char calibration_en, signed short
return 2; // 校准未完成,返回错误状态
}
// =================================================================================
// 关键参数定义 - 需要根据实际情况进行调整
// ---------------------------------------------------------------------------------
// 滤波系数 α (alpha),决定了对陀螺仪和加速度计的信任程度。
// α 越大,越相信陀螺仪,动态响应越好,但对漂移的修正越慢。
// α 越小,越相信加速度计,对漂移的修正越快,但对运动加速度越敏感。
// 典型值范围0.95 ~ 0.99
#define FILTER_ALPHA 0.98f
// 采样时间间隔 SAMPLE_INTERVAL (单位:秒)
// 必须与此函数的频率严格对应。例如100Hz 对应 0.01f。
#define SAMPLE_INTERVAL 0.01f
// =================================================================================
/**
* @brief 不带卡尔曼的欧若拉数据输出,以及消除零点漂移后的六轴数据
*
* @param calibration_en
* @param acc_gyro_input
* @param Angle_output
* @param yaw_rst
* @return unsigned char
*/
unsigned char Original_SL_SC7U22_Angle_Output(unsigned char calibration_en, signed short *acc_gyro_input, float *Angle_output, unsigned char yaw_rst){
unsigned short acc_gyro_delta[2];
unsigned char sl_i = 0;
float angle_acc[3] = {0};
float gyro_val[3] = {0};
// 如果外部强制使能校准则将标志位置1
if (calibration_en == 0) {
SL_SC7U22_Error_Flag = 1;
}
// =================================================================================
// 步骤 1: 静态校准 (此部分完全保留,不做任何修改)
// ---------------------------------------------------------------------------------
if (SL_SC7U22_Error_Flag == 0) {
// ... (省略与原代码完全相同的部分以节省篇幅) ...
// 计算当前数据与上一帧数据的差值,用于判断是否静止
acc_gyro_delta[0] = 0;
acc_gyro_delta[1] = 0;
for (sl_i = 0; sl_i < 3; sl_i++) {
acc_gyro_delta[0] += SL_GetAbsShort(acc_gyro_input[sl_i] - Temp_Accgyro[sl_i]);
acc_gyro_delta[1] += SL_GetAbsShort(acc_gyro_input[3 + sl_i] - Temp_Accgyro[3 + sl_i]);
}
// 保存当前数据,用于下一帧比较
for (sl_i = 0; sl_i < 6; sl_i++) {
Temp_Accgyro[sl_i] = acc_gyro_input[sl_i];
}
// 判断是否处于静止状态
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)) {
if (SL_SC7U22_Error_cnt < 200) SL_SC7U22_Error_cnt++;
} else {
SL_SC7U22_Error_cnt = 0;
}
// 如果静止时间足够长
if (SL_SC7U22_Error_cnt > 190) {
for (sl_i = 0; sl_i < 6; sl_i++) Sum_Avg_Accgyro[sl_i] += acc_gyro_input[sl_i];
SL_SC7U22_Error_cnt2++;
if (SL_SC7U22_Error_cnt2 > 49) {
SL_SC7U22_Error_Flag = 1;
SL_SC7U22_Error_cnt2 = 0;
SL_SC7U22_Error_cnt = 0;
for (sl_i = 0; sl_i < 6; sl_i++) Sum_Avg_Accgyro[sl_i] = Sum_Avg_Accgyro[sl_i] / 50;
Error_Accgyro[0] = 0 - Sum_Avg_Accgyro[0];
Error_Accgyro[1] = 0 - Sum_Avg_Accgyro[1];
Error_Accgyro[2] = 8192 - Sum_Avg_Accgyro[2];
Error_Accgyro[3] = 0 - Sum_Avg_Accgyro[3];
Error_Accgyro[4] = 0 - Sum_Avg_Accgyro[4];
Error_Accgyro[5] = 0 - Sum_Avg_Accgyro[5];
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;
}
return 0; // 返回0表示正在校准
}
// =================================================================================
// 步骤 2: 姿态解算 (使用一阶互补滤波)
// ---------------------------------------------------------------------------------
if (SL_SC7U22_Error_Flag == 1) { // 确认已经校准完成
// --- 2.1 数据预处理 ---
// 应用零点偏移补偿
for (sl_i = 0; sl_i < 6; sl_i++) {
Temp_Accgyro[sl_i] = acc_gyro_input[sl_i] + Error_Accgyro[sl_i];
}
// --- 2.2 使用加速度计计算姿态角 (Pitch 和 Roll) ---
// 注意这里使用了atan2f来计算Roll角它比atanf更稳定能自动处理所有象限避免了后续的if判断。
float acc_x = (float)Temp_Accgyro[0];
float acc_y = (float)Temp_Accgyro[1];
float acc_z = (float)Temp_Accgyro[2];
// Pitch = arcsin(ax / g)
angle_acc[0] = asinf(acc_x / sqrtf(acc_x*acc_x + acc_y*acc_y + acc_z*acc_z)) * 57.29578f;
// Roll = arctan(ay / az)
angle_acc[1] = atan2f(acc_y, acc_z) * 57.29578f;
// --- 2.3 转换陀螺仪数据单位 ---
// 将陀螺仪原始值LSB转换为角速度度/秒)
gyro_val[0] = Temp_Accgyro[4] * 0.061; // GYR-Y -> Pitch 速率
gyro_val[1] = Temp_Accgyro[3] * 0.061; // GYR-X -> Roll 速率
gyro_val[2] = Temp_Accgyro[5] * 0.061; // GYR-Z -> Yaw 速率
// =================================================================================
// 步骤 2.4: 一阶互补滤波
// ---------------------------------------------------------------------------------
// Pitch 轴融合
Angle_output[0] = FILTER_ALPHA * (Angle_output[0] + gyro_val[0] * SAMPLE_INTERVAL) + (1.0f - FILTER_ALPHA) * angle_acc[0];
// Roll 轴融合
Angle_output[1] = FILTER_ALPHA * (Angle_output[1] + gyro_val[1] * SAMPLE_INTERVAL) + (1.0f - FILTER_ALPHA) * angle_acc[1];
/************** Yaw 轴计算**************/
// Yaw角无法通过加速度计重力来校正因此只能使用陀螺仪进行简单积分。
// 如果需要精确的Yaw角必须引入磁力计进行修正。
if (yaw_rst == 1) {
Angle_output[2] = 0; // 如果有复位信号,则清零
}
// 增加一个简单的阈值,当角速度较小时,认为没有转动,以减少漂移
if (SL_GetAbsShort(Temp_Accgyro[5]) > 8) {
Angle_output[2] += gyro_val[2] * SAMPLE_INTERVAL;
}
return 1; // 返回1表示计算成功
}
return 2; // 校准未完成,返回错误状态
}
unsigned char get_calibration_state(void){
return SL_SC7U22_Error_Flag;
}

View File

@ -130,6 +130,8 @@ unsigned char SL_SC7U22_Angle_Output(unsigned char calibration_en,signed short *
/**output Angle_output[2]: Yaw*******************************/
/**input yaw_rst: reset yaw value***************************/
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 get_calibration_state(void);
/**寄存器宏定义*******************************/
#define SC7U22_WHO_AM_I 0x01

View File

@ -1,7 +1,7 @@
#ifndef XTELL_H
#define XTELL_H
#define KS_BLE 1
// #define KS_BLE 1
#define XTELL_TEST 1

View File

@ -100,6 +100,10 @@ void close_BL(){
close_process(&close_BL_number,__func__);
}
void xtell_set_ble_name(char* name){
}
extern u32 timer_get_ms(void);
void xtell_app_main()

View File

@ -78,7 +78,7 @@ extern u8 init_ok;
extern u8 sniff_out;
unsigned char xtell_bl_state=0; //存放经典蓝牙的连接状态0断开1是连接
u8 bt_newname =0;
unsigned char xt_ble_new_name[9] = "CM-11111";
unsigned char xt_ble_new_name[9] = "CM-22222";
static u16 play_poweron_ok_timer_id = 0;
//
///////////////////////////////////////////////////////////////////////////////////////////////////
@ -313,7 +313,7 @@ static int bt_connction_status_event_handler(struct bt_event *bt)
case BT_STATUS_FIRST_CONNECTED:
xlog("BT_STATUS_CONNECTED\n");
xtell_bl_state = 1; //蓝牙连接成功 置1
if(strcmp(xt_ble_new_name,"CM-1111") != 0){
if(strcmp(xt_ble_new_name,"CM-11111") != 0){
//蓝牙连接成功
bt_newname =1;
u8 temp[5]={0xBB,0xBE,0x02,0x04,0x00};

View File

@ -138,7 +138,7 @@ int hw_iic_init(hw_iic_dev iic)
iic_end_pnd_clr(iic_regs[id]);
iic_start_pnd_clr(iic_regs[id]);
iic_enable(iic_regs[id]);
#if 0
#if 1
printf("info->scl = %d\n", iic_get_scl(iic));
printf("info->sda = %d\n", iic_get_sda(iic));
printf("info->baudrate = %d\n", iic_info_baud(iic));
@ -150,7 +150,7 @@ int hw_iic_init(hw_iic_dev iic)
printf("IIC_CON1 0x%04x\n", iic_regs[id]->CON1);
printf("IIC_BAUD 0x%02x\n", iic_regs[id]->BAUD);
//printf("IIC_BUF %02x\n", iic_regs[id]->BUF);
printf("IOMC1 0x%08x\n", JL_IOMAP->CON1);
// printf("IOMC1 0x%08x\n", JL_IOMAP->CON1);
#endif
return 0;
}

Binary file not shown.

Binary file not shown.

Binary file not shown.

Binary file not shown.

Binary file not shown.

Binary file not shown.

View File

@ -419,15 +419,15 @@ objs/apps/common/device/gSensor/gSensor_manage.c.o
-r=objs/apps/common/device/gSensor/gSensor_manage.c.o,write_gsensor_data_handle,pl
-r=objs/apps/common/device/gSensor/gSensor_manage.c.o,cbuf_write,l
-r=objs/apps/common/device/gSensor/gSensor_manage.c.o,gravity_sensor_command,pl
-r=objs/apps/common/device/gSensor/gSensor_manage.c.o,soft_iic_start,l
-r=objs/apps/common/device/gSensor/gSensor_manage.c.o,soft_iic_tx_byte,l
-r=objs/apps/common/device/gSensor/gSensor_manage.c.o,hw_iic_start,l
-r=objs/apps/common/device/gSensor/gSensor_manage.c.o,hw_iic_tx_byte,l
-r=objs/apps/common/device/gSensor/gSensor_manage.c.o,delay,l
-r=objs/apps/common/device/gSensor/gSensor_manage.c.o,soft_iic_stop,l
-r=objs/apps/common/device/gSensor/gSensor_manage.c.o,hw_iic_stop,l
-r=objs/apps/common/device/gSensor/gSensor_manage.c.o,_gravity_sensor_get_ndata,pl
-r=objs/apps/common/device/gSensor/gSensor_manage.c.o,soft_iic_rx_byte,l
-r=objs/apps/common/device/gSensor/gSensor_manage.c.o,hw_iic_rx_byte,l
-r=objs/apps/common/device/gSensor/gSensor_manage.c.o,gsensor_io_ctl,pl
-r=objs/apps/common/device/gSensor/gSensor_manage.c.o,gravity_sensor_init,pl
-r=objs/apps/common/device/gSensor/gSensor_manage.c.o,soft_iic_init,l
-r=objs/apps/common/device/gSensor/gSensor_manage.c.o,hw_iic_init,l
-r=objs/apps/common/device/gSensor/gSensor_manage.c.o,memcmp,l
-r=objs/apps/common/device/gSensor/gSensor_manage.c.o,strlen,l
-r=objs/apps/common/device/gSensor/gSensor_manage.c.o,gpio_set_pull_up,l
@ -442,11 +442,21 @@ objs/apps/common/device/gSensor/gSensor_manage.c.o
-r=objs/apps/common/device/gSensor/gSensor_manage.c.o,gSensor_hdl,pl
-r=objs/apps/common/device/gSensor/gSensor_manage.c.o,__gSensor_info,pl
-r=objs/apps/common/device/gSensor/gSensor_manage.c.o,read_write_status,pl
-r=objs/apps/common/device/gSensor/gSensor_manage.c.o,w_log_buffer_1,pl
-r=objs/apps/common/device/gSensor/gSensor_manage.c.o,w_log_buffer_2,pl
-r=objs/apps/common/device/gSensor/gSensor_manage.c.o,w_log_buffer_3,pl
-r=objs/apps/common/device/gSensor/gSensor_manage.c.o,w_log_buffer_4,pl
-r=objs/apps/common/device/gSensor/gSensor_manage.c.o,sen_log_buffer_1,pl
-r=objs/apps/common/device/gSensor/gSensor_manage.c.o,sen_log_buffer_2,pl
-r=objs/apps/common/device/gSensor/gSensor_manage.c.o,sen_log_buffer_3,pl
-r=objs/apps/common/device/gSensor/gSensor_manage.c.o,sen_log_buffer_4,pl
-r=objs/apps/common/device/gSensor/gSensor_manage.c.o,sensor_iic_init_status,
-r=objs/apps/common/device/gSensor/gSensor_manage.c.o,gsensor_dev_begin,
-r=objs/apps/common/device/gSensor/gSensor_manage.c.o,gsensor_dev_end,
-r=objs/apps/common/device/gSensor/gSensor_manage.c.o,gsensorlen,
-r=objs/apps/common/device/gSensor/gSensor_manage.c.o,iic_lock,pl
-r=objs/apps/common/device/gSensor/gSensor_manage.c.o,w_log_buffer_5,pl
-r=objs/apps/common/device/gSensor/gSensor_manage.c.o,sen_log_buffer_5,pl
objs/apps/common/device/gSensor/mpu6050.c.o
objs/apps/common/device/gx8002_npu/gx8002_enc/gx8002_enc.c.o
objs/apps/common/device/gx8002_npu/gx8002_npu.c.o
@ -1901,9 +1911,11 @@ objs/apps/common/third_party_profile/jieli/JL_rcsp/bt_trans_data/le_rcsp_adv_mod
-r=objs/apps/common/third_party_profile/jieli/JL_rcsp/bt_trans_data/le_rcsp_adv_module.c.o,bt_ble_init_do,pl
-r=objs/apps/common/third_party_profile/jieli/JL_rcsp/bt_trans_data/le_rcsp_adv_module.c.o,bt_ble_adv_ioctl,pl
-r=objs/apps/common/third_party_profile/jieli/JL_rcsp/bt_trans_data/le_rcsp_adv_module.c.o,ble_module_enable,pl
-r=objs/apps/common/third_party_profile/jieli/JL_rcsp/bt_trans_data/le_rcsp_adv_module.c.o,user_ble_gap_device_set,pl
-r=objs/apps/common/third_party_profile/jieli/JL_rcsp/bt_trans_data/le_rcsp_adv_module.c.o,strlen,l
-r=objs/apps/common/third_party_profile/jieli/JL_rcsp/bt_trans_data/le_rcsp_adv_module.c.o,strcpy,l
-r=objs/apps/common/third_party_profile/jieli/JL_rcsp/bt_trans_data/le_rcsp_adv_module.c.o,bt_ble_init,pl
-r=objs/apps/common/third_party_profile/jieli/JL_rcsp/bt_trans_data/le_rcsp_adv_module.c.o,bt_get_local_name,l
-r=objs/apps/common/third_party_profile/jieli/JL_rcsp/bt_trans_data/le_rcsp_adv_module.c.o,strlen,l
-r=objs/apps/common/third_party_profile/jieli/JL_rcsp/bt_trans_data/le_rcsp_adv_module.c.o,bt_ble_exit,pl
-r=objs/apps/common/third_party_profile/jieli/JL_rcsp/bt_trans_data/le_rcsp_adv_module.c.o,ble_get_server_operation_table,pl
-r=objs/apps/common/third_party_profile/jieli/JL_rcsp/bt_trans_data/le_rcsp_adv_module.c.o,ble_server_send_test_key_num,pl
@ -1932,19 +1944,19 @@ objs/apps/common/third_party_profile/jieli/JL_rcsp/bt_trans_data/le_rcsp_adv_mod
-r=objs/apps/common/third_party_profile/jieli/JL_rcsp/bt_trans_data/le_rcsp_adv_module.c.o,JL_rcsp_auth_reset,l
-r=objs/apps/common/third_party_profile/jieli/JL_rcsp/bt_trans_data/le_rcsp_adv_module.c.o,rcsp_dev_select,l
-r=objs/apps/common/third_party_profile/jieli/JL_rcsp/bt_trans_data/le_rcsp_adv_module.c.o,stop_get_music_timer,l
-r=objs/apps/common/third_party_profile/jieli/JL_rcsp/bt_trans_data/le_rcsp_adv_module.c.o,get_defalut_bt_channel_sel,l
-r=objs/apps/common/third_party_profile/jieli/JL_rcsp/bt_trans_data/le_rcsp_adv_module.c.o,btcon_hash,l
-r=objs/apps/common/third_party_profile/jieli/JL_rcsp/bt_trans_data/le_rcsp_adv_module.c.o,put_buf,l
-r=objs/apps/common/third_party_profile/jieli/JL_rcsp/bt_trans_data/le_rcsp_adv_module.c.o,tws_api_get_role,l
-r=objs/apps/common/third_party_profile/jieli/JL_rcsp/bt_trans_data/le_rcsp_adv_module.c.o,JL_get_cur_bt_channel_sel,l
-r=objs/apps/common/third_party_profile/jieli/JL_rcsp/bt_trans_data/le_rcsp_adv_module.c.o,JL_rcsp_get_auth_flag,l
-r=objs/apps/common/third_party_profile/jieli/JL_rcsp/bt_trans_data/le_rcsp_adv_module.c.o,adv_info_notify,l
-r=objs/apps/common/third_party_profile/jieli/JL_rcsp/bt_trans_data/le_rcsp_adv_module.c.o,get_defalut_bt_channel_sel,l
-r=objs/apps/common/third_party_profile/jieli/JL_rcsp/bt_trans_data/le_rcsp_adv_module.c.o,put_buf,l
-r=objs/apps/common/third_party_profile/jieli/JL_rcsp/bt_trans_data/le_rcsp_adv_module.c.o,JL_rcsp_event_to_user,l
-r=objs/apps/common/third_party_profile/jieli/JL_rcsp/bt_trans_data/le_rcsp_adv_module.c.o,sys_timeout_add,l
-r=objs/apps/common/third_party_profile/jieli/JL_rcsp/bt_trans_data/le_rcsp_adv_module.c.o,update_slave_adv_reopen,l
-r=objs/apps/common/third_party_profile/jieli/JL_rcsp/bt_trans_data/le_rcsp_adv_module.c.o,sys_auto_shut_down_disable,l
-r=objs/apps/common/third_party_profile/jieli/JL_rcsp/bt_trans_data/le_rcsp_adv_module.c.o,get_self_battery_level,l
-r=objs/apps/common/third_party_profile/jieli/JL_rcsp/bt_trans_data/le_rcsp_adv_module.c.o,get_charge_online_flag,l
-r=objs/apps/common/third_party_profile/jieli/JL_rcsp/bt_trans_data/le_rcsp_adv_module.c.o,btcon_hash,l
-r=objs/apps/common/third_party_profile/jieli/JL_rcsp/bt_trans_data/le_rcsp_adv_module.c.o,puts,l
-r=objs/apps/common/third_party_profile/jieli/JL_rcsp/bt_trans_data/le_rcsp_adv_module.c.o,bt_adv_handle,pl
-r=objs/apps/common/third_party_profile/jieli/JL_rcsp/bt_trans_data/le_rcsp_adv_module.c.o,sibling_ver_info,pl
@ -4359,6 +4371,7 @@ objs/apps/earphone/user_cfg.c.o
-r=objs/apps/earphone/user_cfg.c.o,lp_winsize,pl
-r=objs/apps/earphone/user_cfg.c.o,__VERSION_BEGIN,
-r=objs/apps/earphone/user_cfg.c.o,log_tag_const_i_USER_CFG,l
-r=objs/apps/earphone/user_cfg.c.o,xt_ble_new_name,l
-r=objs/apps/earphone/user_cfg.c.o,log_tag_const_d_USER_CFG,l
-r=objs/apps/earphone/user_cfg.c.o,app_var,l
-r=objs/apps/earphone/user_cfg.c.o,status_config,l
@ -5452,6 +5465,7 @@ objs/cpu/br28/iic_hw.c.o
-r=objs/cpu/br28/iic_hw.c.o,hw_iic_suspend,pl
-r=objs/cpu/br28/iic_hw.c.o,hw_iic_resume,pl
-r=objs/cpu/br28/iic_hw.c.o,hw_iic_init,pl
-r=objs/cpu/br28/iic_hw.c.o,printf,l
-r=objs/cpu/br28/iic_hw.c.o,hw_iic_uninit,pl
-r=objs/cpu/br28/iic_hw.c.o,gpio_set_hd,l
-r=objs/cpu/br28/iic_hw.c.o,gpio_set_pull_up,l
@ -6271,6 +6285,7 @@ objs/apps/earphone/xtell_Sensor/xtell_app_main.c.o
-r=objs/apps/earphone/xtell_Sensor/xtell_app_main.c.o,close_BL,pl
-r=objs/apps/earphone/xtell_Sensor/xtell_app_main.c.o,user_send_cmd_prepare,l
-r=objs/apps/earphone/xtell_Sensor/xtell_app_main.c.o,delay_2ms,l
-r=objs/apps/earphone/xtell_Sensor/xtell_app_main.c.o,xtell_set_ble_name,pl
-r=objs/apps/earphone/xtell_Sensor/xtell_app_main.c.o,xtell_app_main,pl
-r=objs/apps/earphone/xtell_Sensor/xtell_app_main.c.o,log_print,l
-r=objs/apps/earphone/xtell_Sensor/xtell_app_main.c.o,timer_get_ms,l
@ -6346,34 +6361,40 @@ objs/apps/earphone/xtell_Sensor/send_data.c.o
-r=objs/apps/earphone/xtell_Sensor/send_data.c.o,ble_send_data,pl
-r=objs/apps/earphone/xtell_Sensor/send_data.c.o,send_data_to_ble_client,l
-r=objs/apps/earphone/xtell_Sensor/send_data.c.o,send_sensor_data_task,pl
-r=objs/apps/earphone/xtell_Sensor/send_data.c.o,test,pl
-r=objs/apps/earphone/xtell_Sensor/send_data.c.o,sensor_read_data,pl
-r=objs/apps/earphone/xtell_Sensor/send_data.c.o,circle_buffer_is_full,l
-r=objs/apps/earphone/xtell_Sensor/send_data.c.o,SL_SC7U22_RawData_Read,l
-r=objs/apps/earphone/xtell_Sensor/send_data.c.o,SL_SC7U22_Angle_Output,l
-r=objs/apps/earphone/xtell_Sensor/send_data.c.o,printf,l
-r=objs/apps/earphone/xtell_Sensor/send_data.c.o,circle_buffer_write,l
-r=objs/apps/earphone/xtell_Sensor/send_data.c.o,calculate_data,pl
-r=objs/apps/earphone/xtell_Sensor/send_data.c.o,circle_buffer_is_empty,l
-r=objs/apps/earphone/xtell_Sensor/send_data.c.o,circle_buffer_read,l
-r=objs/apps/earphone/xtell_Sensor/send_data.c.o,sensor_processing_task,l
-r=objs/apps/earphone/xtell_Sensor/send_data.c.o,BLE_send_data,pl
-r=objs/apps/earphone/xtell_Sensor/send_data.c.o,strlen,l
-r=objs/apps/earphone/xtell_Sensor/send_data.c.o,snprintf,l
-r=objs/apps/earphone/xtell_Sensor/send_data.c.o,printf,l
-r=objs/apps/earphone/xtell_Sensor/send_data.c.o,gsensor_test,pl
-r=objs/apps/earphone/xtell_Sensor/send_data.c.o,sys_timer_del,l
-r=objs/apps/earphone/xtell_Sensor/send_data.c.o,sqrtf,l
-r=objs/apps/earphone/xtell_Sensor/send_data.c.o,xtell_task_create,pl
-r=objs/apps/earphone/xtell_Sensor/send_data.c.o,soft_iic_init,l
-r=objs/apps/earphone/xtell_Sensor/send_data.c.o,hw_iic_init,l
-r=objs/apps/earphone/xtell_Sensor/send_data.c.o,gpio_set_direction,l
-r=objs/apps/earphone/xtell_Sensor/send_data.c.o,gpio_set_pull_up,l
-r=objs/apps/earphone/xtell_Sensor/send_data.c.o,gpio_direction_output,l
-r=objs/apps/earphone/xtell_Sensor/send_data.c.o,os_time_dly,l
-r=objs/apps/earphone/xtell_Sensor/send_data.c.o,SL_SC7U22_Config,l
-r=objs/apps/earphone/xtell_Sensor/send_data.c.o,circle_buffer_init,l
-r=objs/apps/earphone/xtell_Sensor/send_data.c.o,create_process,l
-r=objs/apps/earphone/xtell_Sensor/send_data.c.o,puts,l
-r=objs/apps/earphone/xtell_Sensor/send_data.c.o,gsensor_id,pl
-r=objs/apps/earphone/xtell_Sensor/send_data.c.o,test_id,pl
-r=objs/apps/earphone/xtell_Sensor/send_data.c.o,BLE_send_data,pl
-r=objs/apps/earphone/xtell_Sensor/send_data.c.o,KS_data,l
-r=objs/apps/earphone/xtell_Sensor/send_data.c.o,debug1,l
-r=objs/apps/earphone/xtell_Sensor/send_data.c.o,debug2,l
objs/apps/earphone/xtell_Sensor/buffer/circle_buffer.c.o
-r=objs/apps/earphone/xtell_Sensor/buffer/circle_buffer.c.o,circle_buffer_init,pl
-r=objs/apps/earphone/xtell_Sensor/buffer/circle_buffer.c.o,circle_buffer_write,pl
-r=objs/apps/earphone/xtell_Sensor/buffer/circle_buffer.c.o,circle_buffer_get_free_space,pl
-r=objs/apps/earphone/xtell_Sensor/buffer/circle_buffer.c.o,circle_buffer_is_full,pl
-r=objs/apps/earphone/xtell_Sensor/buffer/circle_buffer.c.o,circle_buffer_read,pl
-r=objs/apps/earphone/xtell_Sensor/buffer/circle_buffer.c.o,circle_buffer_is_empty,pl
-r=objs/apps/earphone/xtell_Sensor/buffer/circle_buffer.c.o,circle_buffer_get_size,pl
-r=objs/apps/earphone/xtell_Sensor/buffer/circle_buffer.c.o,circle_buffer_get_free_space,pl
objs/apps/earphone/xtell_Sensor/sensor/LIS2DH12.c.o
-r=objs/apps/earphone/xtell_Sensor/sensor/LIS2DH12.c.o,LIS2DH12_Check,pl
-r=objs/apps/earphone/xtell_Sensor/sensor/LIS2DH12.c.o,LIS2DH12_read_data,pl
@ -6412,9 +6433,15 @@ objs/apps/earphone/xtell_Sensor/sensor/SC7U22.c.o
-r=objs/apps/earphone/xtell_Sensor/sensor/SC7U22.c.o,SL_SC7U22_Angle_Output,pl
-r=objs/apps/earphone/xtell_Sensor/sensor/SC7U22.c.o,asinf,l
-r=objs/apps/earphone/xtell_Sensor/sensor/SC7U22.c.o,atanf,l
-r=objs/apps/earphone/xtell_Sensor/sensor/SC7U22.c.o,Original_SL_SC7U22_Angle_Output,pl
-r=objs/apps/earphone/xtell_Sensor/sensor/SC7U22.c.o,sqrtf,l
-r=objs/apps/earphone/xtell_Sensor/sensor/SC7U22.c.o,atan2f,l
-r=objs/apps/earphone/xtell_Sensor/sensor/SC7U22.c.o,get_calibration_state,pl
-r=objs/apps/earphone/xtell_Sensor/sensor/SC7U22.c.o,gravity_sensor_command,l
-r=objs/apps/earphone/xtell_Sensor/sensor/SC7U22.c.o,_gravity_sensor_get_ndata,l
-r=objs/apps/earphone/xtell_Sensor/sensor/SC7U22.c.o,iic_write_result,pl
-r=objs/apps/earphone/xtell_Sensor/sensor/SC7U22.c.o,iic_read_len,pl
-r=objs/apps/earphone/xtell_Sensor/sensor/SC7U22.c.o,xt_Check_Flag,pl
-r=objs/apps/earphone/xtell_Sensor/sensor/SC7U22.c.o,angle,pl
-r=objs/apps/earphone/xtell_Sensor/sensor/SC7U22.c.o,angle_dot,pl
-r=objs/apps/earphone/xtell_Sensor/sensor/SC7U22.c.o,angle0,pl
@ -6453,17 +6480,17 @@ objs/apps/earphone/xtell_Sensor/calculate/skiing_tracker.c.o
-r=objs/apps/earphone/xtell_Sensor/calculate/skiing_tracker.c.o,start_detection,pl
-r=objs/apps/earphone/xtell_Sensor/calculate/skiing_tracker.c.o,stop_detection,pl
-r=objs/apps/earphone/xtell_Sensor/calculate/skiing_tracker.c.o,skiing_tracker_init,pl
-r=objs/apps/earphone/xtell_Sensor/calculate/skiing_tracker.c.o,forece_of_friction,pl
-r=objs/apps/earphone/xtell_Sensor/calculate/skiing_tracker.c.o,skiing_tracker_update,pl
-r=objs/apps/earphone/xtell_Sensor/calculate/skiing_tracker.c.o,sqrtf,l
-r=objs/apps/earphone/xtell_Sensor/calculate/skiing_tracker.c.o,sensor_processing_task,pl
-r=objs/apps/earphone/xtell_Sensor/calculate/skiing_tracker.c.o,SL_SC7U22_Angle_Output,l
-r=objs/apps/earphone/xtell_Sensor/calculate/skiing_tracker.c.o,fabsf,l
-r=objs/apps/earphone/xtell_Sensor/calculate/skiing_tracker.c.o,cosf,l
-r=objs/apps/earphone/xtell_Sensor/calculate/skiing_tracker.c.o,sinf,l
-r=objs/apps/earphone/xtell_Sensor/calculate/skiing_tracker.c.o,puts,l
-r=objs/apps/earphone/xtell_Sensor/calculate/skiing_tracker.c.o,debug2,pl
-r=objs/apps/earphone/xtell_Sensor/calculate/skiing_tracker.c.o,KS_data,pl
-r=objs/apps/earphone/xtell_Sensor/calculate/skiing_tracker.c.o,debug1,pl
-r=objs/apps/earphone/xtell_Sensor/calculate/skiing_tracker.c.o,debug2,pl
cpu/br28/liba/cpu.a.llvm.19376.crc16.c
-r=cpu/br28/liba/cpu.a.llvm.19376.crc16.c,__crc16_mutex_init,pl
-r=cpu/br28/liba/cpu.a.llvm.19376.crc16.c,os_mutex_create,l

File diff suppressed because it is too large Load Diff

File diff suppressed because it is too large Load Diff

Binary file not shown.

View File

@ -134,28 +134,11 @@ objs/apps/earphone/xtell_Sensor/send_data.c.o: \
apps/earphone/include\app_charge.h \
apps/common\dev_manager/dev_manager.h \
include_lib/update\update_loader_download.h \
apps/earphone/xtell_Sensor/sensor\LIS2DH12.h \
apps/earphone/xtell_Sensor/./sensor/SC7U22.h \
apps/common/device\gSensor/gSensor_manage.h \
include_lib/driver/cpu/br28\asm/iic_hw.h \
include_lib/driver/cpu/br28\asm/iic_soft.h \
apps/common/third_party_profile/jieli/JL_rcsp/bt_trans_data\le_rcsp_adv_module.h \
apps/common/include\bt_common.h cpu/br28\audio_anc.h \
include_lib/media/media_new/media/cpu/br28\asm/anc.h \
include_lib/media/media_new/media/cpu/br28\asm/audio_adc.h \
include_lib/media/media_new/media\anc_btspp.h \
include_lib/media/media_new/media\anc_uart.h \
apps/common/device\in_ear_detect/in_ear_manage.h \
cpu/br28/audio_anc_mult_scene.h apps/common/icsd/anc\icsd_anc_app.h \
include_lib/media/media_new/media/cpu/br28\asm/dac.h \
include_lib/media/media_new/media/cpu/br28/asm/audio_src.h \
include_lib\media/audio_stream.h \
include_lib/media/media_new\media/audio_base.h \
include_lib/media/media_new/media\audio_resample.h \
include_lib/media/media_new\media/audio_cfifo.h \
apps/common/icsd/anc/icsd_anc.h C:/JL/pi32/pi32v2-include\math.h \
apps/common/icsd/anc/icsd_anc_client_board.h \
cpu/br28/audio_anc_fade_ctr.h \
apps/earphone/xtell_Sensor/buffer\circle_buffer.h \
apps/earphone/xtell_Sensor/./buffer/circle_buffer.h \
include_lib\btstack/avctp_user.h include_lib/btstack/btstack_typedef.h \
apps/earphone/xtell_Sensor/calculate/skiing_tracker.h \
apps/earphone/xtell_Sensor/calculate/../xtell.h

Binary file not shown.

Binary file not shown.

Binary file not shown.