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

Compare commits

base: lmx:main
Branches Tags
lmx:main lmx:skiing_tmp lmx:lmx_skiing lmx:lmx_rfid lmx:100_rfid lmx:99_skiing lmx:by_lmx lmx:rfid
lmx:ota
...
compare: lmx:ae980789b6834ce7b7f57418cc4d99d6b3955abb
Branches Tags
lmx:skiing_tmp lmx:lmx_skiing lmx:lmx_rfid lmx:100_rfid lmx:99_skiing lmx:by_lmx lmx:rfid lmx:main
lmx:ota

14 Commits
main ... ae980789b6

Author SHA1 Message Date
lmx
ae980789b6 第6版 2025-11-06 19:24:51 +08:00
lmx
ac7299e7ad cun 2025-11-04 19:20:58 +08:00
lmx
6be3cd1070 3 2025-11-04 14:40:55 +08:00
lmx
671730a351 cun 2025-11-03 18:48:15 +08:00
lmx
97e85df2f8 Ma 2025-10-31 18:12:10 +08:00
lmx
8828f24549 编译通过 2025-10-31 17:43:12 +08:00
lmx
830b4637dd SCU722驱动替换更新 2025-10-31 16:58:39 +08:00
lmx
a96264ec36 调整文件 2025-10-31 10:38:17 +08:00
lmx
eb7b89e434 测试完成 2025-10-31 09:40:15 +08:00
lmx
517beaa1a8 蓝牙不开启待查 2025-10-30 18:30:40 +08:00
lmx
1d0eaa037b cun 2025-10-30 17:53:59 +08:00
lmx
a6919c7e43 接受 2025-10-30 16:14:14 +08:00
lmx
c21ac0ab82 cun 2025-10-30 11:33:38 +08:00
lmx
7567ddc088 cun 2025-10-29 16:24:16 +08:00
451 changed files with 187921 additions and 214510 deletions
Expand all files Collapse all files

22
.vscode/settings.json vendored
View File

@ -3,6 +3,26 @@
"board_config.h": "c",
"board_jl701n_demo_cfg.h": "c",
"colorful_lights.h": "c",
"board_jl701n_anc_cfg.h": "c"
"board_jl701n_anc_cfg.h": "c",
"update_loader_download.h": "c",
"iic_soft.h": "c",
"circle_buffer.h": "c",
"default_event_handler.h": "c",
"ui_manage.h": "c",
"charge.h": "c",
"app_main.h": "c",
"app_config.h": "c",
"app_action.h": "c",
"includes.h": "c",
"key_event_deal.h": "c",
"app_umidigi_chargestore.h": "c",
"hci_lmp.h": "c",
"bluetooth.h": "c",
"SCU722.C": "cpp",
"math.h": "c",
"avctp_user.h": "c",
"string.h": "c",
"dev_manager.h": "c",
"bt_tws.h": "c"
}
}

11
Makefile
View File

@ -245,6 +245,10 @@ INCLUDES := \
-Iinclude_lib/media/aispeech/enc/include \
-Icpu/br28/audio_hearing \
-Iinclude_lib/media/cvp \
-Iapps/earphone/xtell_Sensor/buffer \
-Iapps/earphone/xtell_Sensor/sensor \
-Iapps/earphone/xtell_Sensor \
-Iapps/earphone/xtell_Sensor/calculate \
-I$(SYS_INC_DIR) \
@ -609,6 +613,13 @@ c_SRC_FILES := \
cpu/br28/uart_dev.c \
cpu/br28/umidigi_chargestore.c \
apps/common/colorful_lights/colorful_lights.c \
apps/earphone/xtell_Sensor/xtell_app_main.c \
apps/earphone/xtell_Sensor/xtell_handler.c \
apps/earphone/xtell_Sensor/send_data.c \
apps/earphone/xtell_Sensor/buffer/circle_buffer.c \
apps/earphone/xtell_Sensor/sensor/LIS2DH12.c \
apps/earphone/xtell_Sensor/sensor/SC7U22.c \
apps/earphone/xtell_Sensor/calculate/skiing_tracker.c \
# 需要编译的 .S 文件

88
apps/common/device/gSensor/gSensor_manage.c
View File

@ -18,12 +18,24 @@
#include "bt_tws.h"
#endif
#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
spinlock_t iic_lock;
#define LOG_TAG "[GSENSOR]"
#define LOG_ERROR_ENABLE
#define LOG_DEBUG_ENABLE
#define LOG_INFO_ENABLE
#define xlog_ENABLE
/* #define LOG_DUMP_ENABLE */
#define LOG_CLI_ENABLE
#include "debug.h"
@ -38,7 +50,7 @@ extern int gsensorlen;
extern OS_MUTEX SENSOR_IIC_MUTEX;
extern spinlock_t sensor_iic;
// extern spinlock_t sensor_iic;
extern u8 sensor_iic_init_status;
#define BUF_SIZE gsensorlen*3
@ -82,20 +94,20 @@ void gSensor_int_io_detect(void *priv)
u8 int_io_status = 0;
u8 det_result = 0;
int_io_status = gpio_read(platform_data->gSensor_int_io);
//log_info("status %d\n",int_io_status);
//xlog("status %d\n",int_io_status);
gSensor_hdl->gravity_sensor_ctl(GSENSOR_INT_DET, &int_io_status);
if (gSensor_hdl->gravity_sensor_check == NULL) {
return;
}
det_result = gSensor_hdl->gravity_sensor_check();
if (det_result == 1) {
log_info("GSENSOR_EVENT_CLICK\n");
xlog("GSENSOR_EVENT_CLICK\n");
gSensor_event_to_user(KEY_EVENT_CLICK);
} else if (det_result == 2) {
log_info("GSENSOR_EVENT_DOUBLE_CLICK\n");
xlog("GSENSOR_EVENT_DOUBLE_CLICK\n");
gSensor_event_to_user(KEY_EVENT_DOUBLE_CLICK);
} else if (det_result == 3) {
log_info("GSENSOR_EVENT_THREE_CLICK\n");
xlog("GSENSOR_EVENT_THREE_CLICK\n");
gSensor_event_to_user(KEY_EVENT_TRIPLE_CLICK);
}
}
@ -117,7 +129,7 @@ int gSensor_read_data(u8 *buf, u8 buflen)
//
int get_gSensor_data(short *buf)
{
// printf("%s",__func__);
// xlog("%s",__func__);
axis_info_t accel_data[32];
if (!gpio_read(platform_data->gSensor_int_io)) {
gSensor_hdl->gravity_sensor_ctl(READ_GSENSOR_DATA, accel_data);
@ -126,7 +138,7 @@ int get_gSensor_data(short *buf)
buf[i * 2] = accel_data[i].x;
buf[i * 2 + 1] = accel_data[i].y;
buf[i * 2 + 2] = accel_data[i].z;
// printf("cnt:%1d x:%5d y:%5d z:%5d\n", i, accel_data[i].x, accel_data[i].y, accel_data[i].z);
// xlog("cnt:%1d x:%5d y:%5d z:%5d\n", i, accel_data[i].x, accel_data[i].y, accel_data[i].z);
}
@ -144,7 +156,7 @@ int read_gsensor_buf(short *buf)
static u8 wr_lock;
int read_gsensor_nbuf(short *buf, short datalen)
{
// printf("%s",__func__);
// xlog("%s",__func__);
if (data_w_cbuf == NULL) {
return 0;
}
@ -161,7 +173,7 @@ int read_gsensor_nbuf(short *buf, short datalen)
return 0;
}
} else {
printf("%s NOT ONLINE ", __func__);
xlog("%s NOT ONLINE ", __func__);
return 0;
}
}
@ -175,39 +187,39 @@ void write_gsensor_data_handle(void)
if (gSensor_info->init_flag == 1) {
// if (read_write_status == 0) {
// printf("%s ",__func__);
// xlog("%s ",__func__);
// return;
// }
if (!gpio_read(platform_data->gSensor_int_io)) {
gSensor_hdl->gravity_sensor_ctl(READ_GSENSOR_DATA, accel_data);
/*for(int i=0;i<29;i++){
printf("cnt:%1d x:%5d y:%5d z:%5d\n", i, accel_data[i].x, accel_data[i].y, accel_data[i].z);
xlog("cnt:%1d x:%5d y:%5d z:%5d\n", i, accel_data[i].x, accel_data[i].y, accel_data[i].z);
}*/
u8 wlen;
wlen = cbuf_write(data_w_cbuf, accel_data, 2 * 3 * 29);
/* for(int i=0;i<29;i++){ */
/* printf("sour x=%06d y=%06d z=%06d",accel_data[i].x,accel_data[i].y,accel_data[i].z); */
/* xlog("sour x=%06d y=%06d z=%06d",accel_data[i].x,accel_data[i].y,accel_data[i].z); */
/* } */
if (wlen == 0) {
printf("data_w_cbuf_full");
xlog("data_w_cbuf_full");
}
}
} else {
// printf("%s ",__func__);
// xlog("%s ",__func__);
return ;
}
}
u8 gravity_sensor_command(u8 w_chip_id, u8 register_address, u8 function_command)
{
spin_lock(&sensor_iic);
// spin_lock(&sensor_iic);
/* os_mutex_pend(&SENSOR_IIC_MUTEX,0); */
u8 ret = 1;
iic_start(gSensor_info->iic_hdl);
if (0 == iic_tx_byte(gSensor_info->iic_hdl, w_chip_id)) {
ret = 0;
log_e("\n gsen iic wr err 0\n");
xlog("\n gsen iic wr err 0\n");
goto __gcend;
}
@ -215,7 +227,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;
log_e("\n gsen iic wr err 1\n");
xlog("\n gsen iic wr err 1\n");
goto __gcend;
}
@ -223,27 +235,27 @@ 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;
log_e("\n gsen iic wr err 2\n");
xlog("\n gsen iic wr err 2\n");
goto __gcend;
}
__gcend:
iic_stop(gSensor_info->iic_hdl);
spin_unlock(&sensor_iic);
// spin_unlock(&sensor_iic);
/* os_mutex_post(&SENSOR_IIC_MUTEX); */
return ret;
}
u8 _gravity_sensor_get_ndata(u8 r_chip_id, u8 register_address, u8 *buf, u8 data_len)
{
// printf("%s",__func__);
spin_lock(&sensor_iic);
// xlog("%s",__func__);
// spin_lock(&sensor_iic);
/* os_mutex_pend(&SENSOR_IIC_MUTEX,0); */
u8 read_len = 0;
iic_start(gSensor_info->iic_hdl);
if (0 == iic_tx_byte(gSensor_info->iic_hdl, r_chip_id - 1)) {
log_e("\n gsen iic rd err 0\n");
xlog("\n gsen iic rd err 0\n");
read_len = 0;
goto __gdend;
}
@ -251,14 +263,14 @@ 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)) {
log_e("\n gsen iic rd err 1\n");
xlog("\n gsen iic rd err 1\n");
read_len = 0;
goto __gdend;
}
iic_start(gSensor_info->iic_hdl);
if (0 == iic_tx_byte(gSensor_info->iic_hdl, r_chip_id)) {
log_e("\n gsen iic rd err 2\n");
xlog("\n gsen iic rd err 2\n");
read_len = 0;
goto __gdend;
}
@ -277,7 +289,7 @@ __gdend:
iic_stop(gSensor_info->iic_hdl);
delay(gSensor_info->iic_delay);
spin_unlock(&sensor_iic);
// spin_unlock(&sensor_iic);
/* os_mutex_post(&SENSOR_IIC_MUTEX); */
return read_len;
@ -292,7 +304,7 @@ int gravity_sensor_init(void *_data)
if (sensor_iic_init_status == 0) {
spin_lock_init(&sensor_iic);
// spin_lock_init(&sensor_iic);
sensor_iic_init_status = 1;
}
gSensor_info->init_flag = 0;
@ -302,13 +314,13 @@ int gravity_sensor_init(void *_data)
gSensor_info->iic_hdl = platform_data->iic;
retval = iic_init(gSensor_info->iic_hdl);
log_e("\n gravity_sensor_init\n");
xlog("\n gravity_sensor_init\n");
if (retval < 0) {
log_e("\n open iic for gsensor err\n");
xlog("\n open iic for gsensor err\n");
return retval;
} else {
log_info("\n iic open succ\n");
xlog("\n iic open succ\n");
}
retval = -EINVAL;
@ -320,14 +332,14 @@ int gravity_sensor_init(void *_data)
}
if (retval < 0) {
log_e(">>>gSensor_hdl logo err\n");
xlog(">>>gSensor_hdl logo err\n");
return retval;
}
if (gSensor_hdl->gravity_sensor_init()) {
log_e(">>>>gSensor_Int ERROR\n");
xlog(">>>>gSensor_Int ERROR\n");
} else {
log_info(">>>>gSensor_Int SUCC\n");
xlog(">>>>gSensor_Int SUCC\n");
gSensor_info->init_flag = 1;
if (platform_data->gSensor_int_io != -1) {
gpio_set_pull_up(platform_data->gSensor_int_io, 1);
@ -336,7 +348,7 @@ int gravity_sensor_init(void *_data)
gpio_set_die(platform_data->gSensor_int_io, 1);
data_buf = zalloc(BUF_SIZE);
if (data_buf == NULL) {
printf("gsensor_cbuf_error!");
xlog("gsensor_cbuf_error!");
return 0;
}
@ -347,7 +359,7 @@ int gravity_sensor_init(void *_data)
cbuf_init(data_w_cbuf, data_buf, BUF_SIZE);
/* port_edge_wkup_set_callback(write_gsensor_data_handle); */
/* 已改为使用port_edge_wkup_set_callback_by_index,使用时需要重新实现 */
printf("cbuf_init");
xlog("cbuf_init");
// spin_lock_init(&iic_lock);
// sys_s_hi_timer_add(NULL, gSensor_int_io_detect, 10); //10ms
@ -388,7 +400,7 @@ int gsensor_enable(void)
//工作空间
data_buf = zalloc(BUF_SIZE);
if (data_buf == NULL) {
printf("gsensor_cbuf_error!");
xlog("gsensor_cbuf_error!");
return -1;
}
data_w_cbuf = zalloc(sizeof(cbuffer_t));
@ -396,7 +408,7 @@ int gsensor_enable(void)
return -1;
}
cbuf_init(data_w_cbuf, data_buf, BUF_SIZE);
printf("cbuf_init");
xlog("cbuf_init");
//设置参数
valid = 0;
gSensor_hdl->gravity_sensor_ctl(GSENSOR_RESET_INT, &valid);
@ -404,6 +416,6 @@ int gsensor_enable(void)
if (valid == -1) {
return -1;
}
printf("gsensor_reset_succeed\n");
xlog("gsensor_reset_succeed\n");
return 0;
}

27
apps/common/third_party_profile/jieli/JL_rcsp/bt_trans_data/le_rcsp_adv_module.c
View File

@ -588,7 +588,7 @@ static const u8 dueros_dma_uuid_16bit[] = {0x04, 0xFE};
extern u8 *get_chargebox_adv_addr(void);
#endif
static void rcsp_adv_fill_mac_addr(u8 *mac_addr_buf)
void rcsp_adv_fill_mac_addr(u8 *mac_addr_buf)
{
#if (MUTIl_CHARGING_BOX_EN)
u8 *mac_addr = get_chargebox_adv_addr();
@ -1647,3 +1647,28 @@ void send_version_to_sibling(void)
}
#endif
///////////////////////////////////////////////////////////////////////////////////////////////////////
void send_data_to_ble_client(const u8* data, u16 length)
{
// 检查数据长度是否有效
if (length == 0 || length > 512) {
printf("Error: Data length %d is out of range (1-512)\n", length);
return;
}
// 检查缓冲区空间
if (app_send_user_data_check(length)) {
// 发送数据
int ret = app_send_user_data(ATT_CHARACTERISTIC_ae02_01_VALUE_HANDLE, data, length, ATT_OP_NOTIFY);
if (ret == 0) { // 假设 0 表示成功
// printf("Data sent successfully: Length %d\n", length);
} else {
// printf("Failed to send data: Length %d, Error code: %d\n", length, ret);
}
} else {
// printf("Insufficient buffer space to send data: Length %d\n", length);
}
}

19
apps/earphone/app_main.c
View File

@ -236,16 +236,17 @@ static void app_poweron_check(int update)
}
extern u32 timer_get_ms(void);
void app_main()
{
void xtell_app_main();
xtell_app_main();
#if 0
int update = 0;
u32 addr = 0, size = 0;
struct intent it;
log_info("app_main\n");
app_var.start_time = timer_get_ms();
#if (defined(CONFIG_MEDIA_NEW_ENABLE) || (defined(CONFIG_MEDIA_DEVELOP_ENABLE)))
/*解码器*/
audio_enc_init();
@ -312,8 +313,18 @@ void app_main()
set_charge_event_flag(1);
#endif
log_info("app_main\n");
app_var.start_time = timer_get_ms();
void xtell_main(void);
xtell_main();
#endif
}
int __attribute__((weak)) eSystemConfirmStopStatus(void)
{
/* 系统进入在未来时间里，无任务超时唤醒，可根据用户选择系统停止，或者系统定时唤醒(100ms)，或自己指定唤醒时间 */

56
apps/earphone/board/br28/board_jl701n_demo_cfg.h
View File

@ -29,9 +29,9 @@
//*********************************************************************************//
// IIC配置 //
//*********************************************************************************//
/*软件IIC设置*/
#define TCFG_SW_I2C0_CLK_PORT IO_PORTG_07 //软件IIC CLK脚选择
#define TCFG_SW_I2C0_DAT_PORT IO_PORTG_08 //软件IIC DAT脚选择
/*软件IIC设置*/ //xtell
#define TCFG_SW_I2C0_CLK_PORT IO_PORTA_05 //软件IIC CLK脚选择 XTELL
#define TCFG_SW_I2C0_DAT_PORT IO_PORTA_06 //软件IIC DAT脚选择
#define TCFG_SW_I2C0_DELAY_CNT 10 //IIC延时参数，影响通讯时钟频率
/*硬件IIC端口选择
@ -41,16 +41,16 @@
'C': IO_PORTA_07 IO_PORTA_08
'D': IO_PORTA_05 IO_PORTA_06
*/
#define TCFG_HW_I2C0_PORTS 'B'
#define TCFG_HW_I2C0_PORTS 'D'
#define TCFG_HW_I2C0_CLK 100000 //硬件IIC波特率
//*********************************************************************************//
// 硬件SPI 配置 //
//*********************************************************************************//
#define TCFG_HW_SPI1_ENABLE 1
#define TCFG_HW_SPI1_PORT_CLK IO_PORTC_04//IO_PORTA_00
#define TCFG_HW_SPI1_PORT_CLK 0//IO_PORTC_04//IO_PORTA_00 xtellio
#define TCFG_HW_SPI1_PORT_DO IO_PORTC_05//IO_PORTA_01
#define TCFG_HW_SPI1_PORT_DI IO_PORTC_03//IO_PORTA_02
#define TCFG_HW_SPI1_PORT_DI 0//IO_PORTC_03//IO_PORTA_02 xtellio
#define TCFG_HW_SPI1_BAUD 2400000L
#define TCFG_HW_SPI1_MODE SPI_MODE_BIDIR_1BIT
#define TCFG_HW_SPI1_ROLE SPI_ROLE_MASTER
@ -72,9 +72,9 @@
#define TCFG_SD0_DET_IO IO_PORTB_03 //当检测方式为IO检测可用
#define TCFG_SD0_DET_IO_LEVEL 0 //当检测方式为IO检测可用,0：低电平检测到卡。 1：高电平(外部电源)检测到卡。 2：高电平(SD卡电源)检测到卡。
#define TCFG_SD0_CLK (3000000 * 4L) //SD卡时钟频率设置
#define TCFG_SD0_PORT_CMD IO_PORTC_04
#define TCFG_SD0_PORT_CMD NULL//IO_PORTC_04 //xtellio
#define TCFG_SD0_PORT_CLK IO_PORTC_05
#define TCFG_SD0_PORT_DA0 IO_PORTC_03
#define TCFG_SD0_PORT_DA0 0//IO_PORTC_03 //xtellio
#define TCFG_SD0_PORT_DA1 NO_CONFIG_PORT //当选择4线模式时要用
#define TCFG_SD0_PORT_DA2 NO_CONFIG_PORT
#define TCFG_SD0_PORT_DA3 NO_CONFIG_PORT
@ -101,16 +101,16 @@
#define MULT_KEY_ENABLE 1//DISABLE //是否使能组合按键消息, 使能后需要配置组合按键映射表
#define TCFG_KEY_TONE_EN DISABLE // 按键提示音。
#define TCFG_KEY_TONE_EN ENABLE//DISABLE xtell // 按键提示音。
//*********************************************************************************//
// iokey 配置 //
//*********************************************************************************//
#define TCFG_IOKEY_ENABLE ENABLE_THIS_MOUDLE //是否使能IO按键
#define TCFG_IOKEY_ENABLE DISABLE_THIS_MOUDLE//ENABLE_THIS_MOUDLE //是否使能IO按键 xtellio
#define TCFG_IOKEY_POWER_CONNECT_WAY ONE_PORT_TO_LOW //按键一端接低电平一端接IO
#define TCFG_IOKEY_POWER_ONE_PORT IO_PORTB_01 //IO按键端口
#define TCFG_IOKEY_POWER_ONE_PORT NULL//IO_PORTG_05//IO_PORTB_01 //xtellio //IO按键端口
//*********************************************************************************//
// adkey 配置 //
@ -287,7 +287,7 @@
//RDEC2配置
#define TCFG_RDEC2_ECODE1_PORT IO_PORTB_00
#define TCFG_RDEC2_ECODE2_PORT IO_PORTB_01
#define TCFG_RDEC2_ECODE2_PORT NULL//IO_PORTG_05 //IO_PORTB_01 xtellio
#define TCFG_RDEC2_KEY0_VALUE 4
#define TCFG_RDEC2_KEY1_VALUE 5
@ -295,9 +295,11 @@
// Digital Hearing Aid(DHA)辅听耳机配置 //
//*********************************************************************************//
/*辅听功能使能*/
// #define TCFG_AUDIO_HEARING_AID_ENABLE DISABLE_THIS_MOUDLE
#define TCFG_AUDIO_HEARING_AID_ENABLE DISABLE_THIS_MOUDLE
/*听力验配功能*/
#define TCFG_AUDIO_DHA_FITTING_ENABLE DISABLE
#define TCFG_AUDIO_DHA_FITTING_ENABLE DISABLE //DISABLE
/*辅听功能互斥配置*/
#define TCFG_AUDIO_DHA_AND_MUSIC_MUTEX ENABLE //辅听功能和音乐播放互斥(默认互斥，资源有限)
#define TCFG_AUDIO_DHA_AND_CALL_MUTEX ENABLE //辅听功能和通话互斥(默认互斥，资源有限)
@ -365,7 +367,7 @@ DAC硬件上的连接方式,可选的配置：
#define DAC_MODE_H2_DIFF (4) // 高压2档差分模式, 适用于高功率差分PA音箱, 输出幅度 0~5Vpp , VDDIO >= 3.3V
#define DAC_MODE_H2_SINGLE (5) // 高压2档单端模式, 适用于高功率单端PA音箱, 输出幅度 0~2.5Vpp, VDDIO >= 3.3V
*/
#define TCFG_AUDIO_DAC_MODE DAC_MODE_L_DIFF // DAC_MODE_L_DIFF 低压， DAC_MODE_H1_DIFF 高压
#define TCFG_AUDIO_DAC_MODE DAC_MODE_L_DIFF // DAC_MODE_L_DIFF 低压， DAC_MODE_H1_DIFF 高压 xtell
/*预留接口，未使用*/
@ -449,7 +451,7 @@ DAC硬件上的连接方式,可选的配置：
#if (TCFG_AUDIO_ANC_ENABLE || TCFG_AD2DA_LOW_LATENCY_ENABLE || TCFG_AUDIO_HEARING_AID_ENABLE)
#define SYS_VOL_TYPE VOL_TYPE_DIGITAL
#else
#define SYS_VOL_TYPE VOL_TYPE_DIGITAL_HW
#define SYS_VOL_TYPE VOL_TYPE_DIGITAL //xtell
#endif/*TCFG_AUDIO_ANC_ENABLE*/
/*
*通话的时候使用数字音量
@ -628,7 +630,7 @@ DAC硬件上的连接方式,可选的配置：
#define TCFG_AEC_TOOL_ONLINE_ENABLE DISABLE_THIS_MOUDLE
/*麦克风测试和传递函数测试*/
#define TCFG_AUDIO_MIC_DUT_ENABLE DISABLE_THIS_MOUDLE
#define TCFG_AUDIO_MIC_DUT_ENABLE DISABLE_THIS_MOUDLE //xtell
//*********************************************************************************//
// Audio Smart Voice //
@ -671,7 +673,7 @@ DAC硬件上的连接方式,可选的配置：
//*********************************************************************************//
// Spatial Audio Effect 空间音效配置 //
//*********************************************************************************//
#define TCFG_AUDIO_SPATIAL_EFFECT_ENABLE DISABLE_THIS_MOUDLE
#define TCFG_AUDIO_SPATIAL_EFFECT_ENABLE DISABLE_THIS_MOUDLE //xtell
#define TCFG_TWS_SPATIAL_AUDIO_AS_CHANNEL 'L'
/*独立任务里面跑空间音效*/
@ -907,9 +909,9 @@ DAC硬件上的连接方式,可选的配置：
//*********************************************************************************//
// g-sensor配置 //
//*********************************************************************************//
#define TCFG_GSENSOR_ENABLE 0 //gSensor使能
#define TCFG_GSENSOR_ENABLE 0//1 //gSensor使能
#define TCFG_DA230_EN 0
#define TCFG_SC7A20_EN 0
#define TCFG_SC7A20_EN 0 //0 //xtell
#define TCFG_STK8321_EN 0
#define TCFG_IRSENSOR_ENABLE 0
#define TCFG_JSA1221_ENABLE 0
@ -925,7 +927,7 @@ DAC硬件上的连接方式,可选的配置：
#define TCFG_MPU6887P_USER_IIC_TYPE 0 //iic有效:1:硬件iic, 0:软件iic
#define TCFG_MPU6887P_USER_IIC_INDEX 0 //IIC 序号
#define TCFG_MPU6887P_DETECT_IO (-1) //传感器中断io
#define TCFG_MPU6887P_AD0_SELETE_IO IO_PORTC_03 //iic地址选择io
#define TCFG_MPU6887P_AD0_SELETE_IO 0//IO_PORTC_03 //iic地址选择io xtellio
//icm42607p cfg
#define TCFG_ICM42670P_ENABLE 1
#define TCFG_ICM42670P_INTERFACE_TYPE 0 //0:iic, 1:spi
@ -960,14 +962,14 @@ DAC硬件上的连接方式,可选的配置：
#define TCFG_LSM6DSL_DETECT_IO (-1) //传感器中断io
#define TCFG_LSM6DSL_AD0_SELETE_IO (-1) //iic地址选择io
//mpu6050 cfg
#define TCFG_MPU6050_EN 0
#define TCFG_MPU6050_EN 0 //xtell
//qmc5883 cfg
/*
*imu-sensor power manager
*不用独立IO供电，则配置 NO_CONFIG_PORT
*/
#define TCFG_IMU_SENSOR_PWR_PORT IO_PORTG_05
#define TCFG_IMU_SENSOR_PWR_PORT NO_CONFIG_PORT
/*空间音效和传感器的依赖*/
@ -993,15 +995,15 @@ DAC硬件上的连接方式,可选的配置：
//*********************************************************************************//
// 系统配置 //
//*********************************************************************************//
#define TCFG_AUTO_SHUT_DOWN_TIME 180 //没有蓝牙连接自动关机时间
#define TCFG_AUTO_SHUT_DOWN_TIME 0 //180 //没有蓝牙连接自动关机时间，0 表示关闭系统自动关机 xtellbt
#define TCFG_SYS_LVD_EN 1 //电量检测使能
#define TCFG_POWER_ON_NEED_KEY 0 //是否需要按按键开机配置
#define TWFG_APP_POWERON_IGNORE_DEV 0 //上电忽略挂载设备，0时不忽略，非0则n毫秒忽略
#define TWFG_APP_POWERON_IGNORE_DEV 0 //上电忽略挂载设备，0时不忽略，非0则n毫秒忽略
//*********************************************************************************//
// 蓝牙配置 //
//*********************************************************************************//
#define TCFG_USER_TWS_ENABLE 0 //tws功能使能
#define TCFG_USER_TWS_ENABLE 0 //tws功能使能
#define TCFG_USER_BLE_ENABLE 1 //BLE功能使能
#define TCFG_BT_SUPPORT_AAC 1 //AAC格式支持
#define TCFG_BT_SUPPORT_LDAC 0 //LDAC格式支持
@ -1025,7 +1027,7 @@ DAC硬件上的连接方式,可选的配置：
#define TCFG_AUTO_STOP_PAGE_SCAN_TIME 0 //配置一拖二第一台连接后自动关闭PAGE SCAN的时间(单位分钟)
#else
#define TCFG_BD_NUM 1 //连接设备个数配置
#define TCFG_AUTO_STOP_PAGE_SCAN_TIME 0 //配置一拖二第一台连接后自动关闭PAGE SCAN的时间(单位分钟)
#define TCFG_AUTO_STOP_PAGE_SCAN_TIME 0 //配置一拖二第一台连接后自动关闭PAGE SCAN的时间(单位分钟)
#endif
#define BT_INBAND_RINGTONE 1 //是否播放手机自带来电铃声
@ -1123,7 +1125,7 @@ DAC硬件上的连接方式,可选的配置：
//*********************************************************************************//
#if TCFG_USER_BLE_ENABLE
#define DUEROS_DMA_EN 0
#define TRANS_DATA_EN 1
#define TRANS_DATA_EN 0//1 //xtellota
#define BLE_HID_EN 0
#if (DUEROS_DMA_EN)

46
apps/earphone/earphone.c
View File

@ -70,6 +70,9 @@
#include "bt_background.h"
#include "default_event_handler.h"
#define xlog(format, ...) printf("[%s] " format, __func__, ##__VA_ARGS__)
#ifdef CONFIG_BOARD_AISPEECH_VAD_ASR
extern int ais_platform_asr_open(void);
extern void ais_platform_asr_close(void);
@ -130,13 +133,14 @@ void __set_sbc_cap_bitpool(u8 sbc_cap_bitpoola);
static u16 power_mode_timer = 0;
u8 init_ok = 0;
u8 get_bt_init_status(void)
{
return init_ok;
}
static u8 sniff_out = 0;
u8 sniff_out = 0;
u8 get_sniff_out_status()
{
return sniff_out;
@ -146,6 +150,7 @@ void clear_sniff_out_status()
sniff_out = 0;
}
void earphone_change_pwr_mode(int mode, int msec)
{
#if TCFG_POWER_MODE_QUIET_ENABLE
@ -489,6 +494,8 @@ void spp_data_handler(u8 packet_type, u16 ch, u8 *packet, u16 size)
#endif
break;
}
}
extern const char *sdk_version_info_get(void);
@ -2231,7 +2238,7 @@ static void bt_vendor_meta_event_handle(u8 sub_evt, u8 *arg, u8 len)
extern void set_remote_test_flag(u8 own_remote_test);
static int bt_hci_event_handler(struct bt_event *bt)
int bt_hci_event_handler(struct bt_event *bt)
{
//对应原来的蓝牙连接上断开处理函数 ,bt->value=reason
log_info("------------------------bt_hci_event_handler reason %x %x", bt->event, bt->value);
@ -2684,6 +2691,15 @@ static int state_machine(struct application *app, enum app_state state, struct i
bredr_handle_register();
EARPHONE_STATE_INIT();
btstack_init();
u8 mac_data[6];
extern void rcsp_adv_fill_mac_addr(u8 *mac_addr_buf);
rcsp_adv_fill_mac_addr(mac_data); //读取MAC地址
xlog("ble xtell BT mac data:%x:%x:%x:%x:%x:%x",mac_data[0],mac_data[1],mac_data[2],mac_data[3],mac_data[4],mac_data[5]);
xlog("=============bt_function_select_init================\n");
#if TCFG_USER_TWS_ENABLE
tws_profile_init();
sys_key_event_filter_disable();
@ -2760,17 +2776,17 @@ static int state_machine(struct application *app, enum app_state state, struct i
return error;
}
static const struct application_operation app_earphone_ops = {
.state_machine = state_machine,
.event_handler = event_handler,
};
// static const struct application_operation app_earphone_ops = {
// .state_machine = state_machine,
// .event_handler = event_handler,
// };
/*
* 注册earphone模式
*/
REGISTER_APPLICATION(app_earphone) = {
.name = "earphone",
.action = ACTION_EARPHONE_MAIN,
.ops = &app_earphone_ops,
.state = APP_STA_DESTROY,
};
// /*
// * 注册earphone模式
// */
// REGISTER_APPLICATION(app_earphone) = {
// .name = "earphone",
// .action = ACTION_EARPHONE_MAIN,
// .ops = &app_earphone_ops,
// .state = APP_STA_DESTROY,
// };

12
apps/earphone/include/app_config.h
View File

@ -5,10 +5,10 @@
* 系统打印总开关
*/
#define LIB_DEBUG 1
#define LIB_DEBUG 1 //1 //xtelllog 1是打开库打印
#define CONFIG_DEBUG_LIB(x) (x & LIB_DEBUG)
#define CONFIG_DEBUG_ENABLE
#define CONFIG_DEBUG_ENABLE //xtelllog 注释就关闭log
#ifndef CONFIG_DEBUG_ENABLE
//#define CONFIG_DEBUG_LITE_ENABLE //轻量级打印开关, 默认关闭
@ -18,7 +18,7 @@
//*********************************************************************************//
// AI配置 //
//*********************************************************************************//
// #define CONFIG_APP_BT_ENABLE
#define CONFIG_APP_BT_ENABLE
#ifdef CONFIG_APP_BT_ENABLE
#define TRANS_DATA_EN 0
@ -68,8 +68,8 @@
#define OTA_TWS_SAME_TIME_ENABLE 1 //是否支持TWS同步升级
#define OTA_TWS_SAME_TIME_NEW 1 //使用新的tws ota流程
#else
#define OTA_TWS_SAME_TIME_ENABLE 0 //是否支持TWS同步升级
#define OTA_TWS_SAME_TIME_NEW 0 //使用新的tws ota流程
#define OTA_TWS_SAME_TIME_ENABLE 1//0 xtellota //是否支持TWS同步升级
#define OTA_TWS_SAME_TIME_NEW 1//0 //使用新的tws ota流程
#endif //CONFIG_DOUBLE_BANK_ENABLE
#else
#define RCSP_UPDATE_EN 0 //是否支持rcsp升级
@ -203,7 +203,7 @@
/* #undef TCFG_UART0_ENABLE
#define TCFG_UART0_ENABLE DISABLE_THIS_MOUDLE */
dzfghsdfhgsfgh
#endif //(CONFIG_BT_MODE != BT_NORMAL)

219
apps/earphone/xtell_Sensor/A_hide/1/skiing_tracker.c Normal file
View File

@ -0,0 +1,219 @@
/*
静态ZUPT+卡尔曼，效果貌似还行
*/
#include "skiing_tracker.h"
#include "../sensor/SC7U22.h" // 包含传感器驱动头文件以调用姿态解算函数
#include <math.h>
#include <string.h>
#define G_ACCELERATION 9.81f // 重力加速度 (m/s^2)
#define DEG_TO_RAD (3.14159265f / 180.0f)
// --- 状态检测阈值 ---
// 判断是否静止的加速度阈值 (m/s^2)。当加速度的模长减去重力后，小于此值，则认为可能静止。
#define STATIC_ACC_THRESHOLD 1.0f
// 连续多少帧满足静止条件才确认为静止状态
#define STATIC_FRAMES_REQUIRED 50 // 累加，超过这个数加速度仍变化不大，说明近似静止
/**
* @brief 初始化滑雪追踪器
*/
void skiing_tracker_init(skiing_tracker_t *tracker)
{
if (!tracker) {
return;
}
memset(tracker, 0, sizeof(skiing_tracker_t));
tracker->state = SKIING_STATE_STATIC; // 初始状态为静止
}
/**
* @brief 将设备坐标系下的加速度转换为世界坐标系
* @param acc_device 设备坐标系下的加速度 [x, y, z]
* @param angle 姿态角 [pitch, roll, yaw]，单位: 度
* @param acc_world 输出：世界坐标系下的加速度 [x, y, z]
*/
static void transform_acc_to_world_frame(const float *acc_device, const float *angle, float *acc_world)
{
// 修正#1：驱动输出的角度与标准航空定义相反，需要取反才能用于标准旋转矩阵。
float pitch = -angle[0] * DEG_TO_RAD;
float roll = -angle[1] * DEG_TO_RAD;
float cp = cosf(pitch);
float sp = sinf(pitch);
float cr = cosf(roll);
float sr = sinf(roll);
float ax = acc_device[0];
float ay = acc_device[1];
float az = acc_device[2];
// 修正#2：使用经过验证的、正确的身体坐标系到世界坐标系的旋转矩阵 (基于 Y-X 旋转顺序)
// 这个矩阵将设备测量的加速度(ax, ay, az)正确地转换到世界坐标系(acc_world)。
acc_world[0] = cp * ax + sp * sr * ay + sp * cr * az;
acc_world[1] = 0 * ax + cr * ay - sr * az;
acc_world[2] = -sp * ax + cp * sr * ay + cp * cr * az;
}
/**
* @brief 更新滑雪状态机
*/
static void update_state_machine(skiing_tracker_t *tracker, const float *acc_device)
{
// 计算当前加速度的模长
float acc_magnitude = sqrtf(acc_device[0] * acc_device[0] + acc_device[1] * acc_device[1] + acc_device[2] * acc_device[2]);
// 状态判断逻辑
switch (tracker->state) {
case SKIING_STATE_STATIC:
// 如果加速度变化较大，则切换到滑雪状态
if (fabsf(acc_magnitude - G_ACCELERATION) > STATIC_ACC_THRESHOLD * 2.0f) { // 使用一个更大的阈值来启动
tracker->state = SKIING_STATE_SKIING;
tracker->static_frames_count = 0;
}
break;
case SKIING_STATE_SKIING:
// 检测是否进入静止状态 (零速更新 ZUPT)
if (fabsf(acc_magnitude - G_ACCELERATION) < STATIC_ACC_THRESHOLD) {
tracker->static_frames_count++;
if (tracker->static_frames_count >= STATIC_FRAMES_REQUIRED) {
tracker->state = SKIING_STATE_STATIC;
// 进入静止状态，强制将速度清零以消除漂移
memset(tracker->velocity, 0, sizeof(tracker->velocity));
tracker->speed = 0.0f;
}
} else {
// 如果在运动，则重置静止计数器
tracker->static_frames_count = 0;
}
break;
// 可以在此添加摔倒等其他状态的判断
case SKIING_STATE_FALLEN:
// TODO: 添加从摔倒状态恢复的逻辑
break;
default:
tracker->state = SKIING_STATE_UNKNOWN;
break;
}
}
/**
* @brief 主更新函数
*/
void skiing_tracker_update(skiing_tracker_t *tracker, float *acc, float *angle, float dt)
{
if (!tracker || !acc || !angle || dt <= 0) {
return;
}
// 将原始g单位的加速度转换为 m/s^2
float acc_device_ms2[3];
acc_device_ms2[0] = acc[0] * G_ACCELERATION;
acc_device_ms2[1] = acc[1] * G_ACCELERATION;
acc_device_ms2[2] = acc[2] * G_ACCELERATION;
// 更新状态机
update_state_machine(tracker, acc_device_ms2);
// 只有在滑雪状态下才进行计算
if (tracker->state == SKIING_STATE_SKIING) {
// 坐标系转换: 首先，利用姿态角(Pitch, Roll)将传感器测得的总加速度（运动加速度+重力）
// 从不断变化的“设备坐标系”转换到一个固定的“世界坐标系”。在这个世界坐标系里，Z轴永远垂直于地面指向上方。
// 执行坐标系转换
transform_acc_to_world_frame(acc_device_ms2, angle, tracker->acc_world);
// 转换完成后，重力就变成了一个恒定的、方向沿Z轴向下的矢量。
// 修正#3：经过正确的坐标转换后，静止时重力在世界坐标系Z轴上的分量精确为+g。
// 必须减去这个g，才能得到纯粹的运动加速度。
tracker->acc_world[2] -= G_ACCELERATION;
// 积分计算速度 (v = v0 + a*t)
tracker->velocity[0] += tracker->acc_world[0] * dt;
tracker->velocity[1] += tracker->acc_world[1] * dt;
tracker->velocity[2] += tracker->acc_world[2] * dt; // 垂直方向速度也计算在内
// 计算当前速率
tracker->speed = sqrtf(tracker->velocity[0] * tracker->velocity[0] +
tracker->velocity[1] * tracker->velocity[1] +
tracker->velocity[2] * tracker->velocity[2]);
// 积分计算距离 (d = d0 + v*t)
tracker->distance += tracker->speed * dt;
}
}
// 传感器数据采集与处理任务
void sensor_processing_task(signed short * acc_data_buf,signed short * gyr_data_buf) {
// --- 1. 定义静态变量 ---
static skiing_tracker_t my_skiing_tracker;
static int initialized = 0;
static int calibration_done = 0; // 新增：用于标记一次性静态校准是否完成
static signed short combined_raw_data[6];
static float final_angle_data[3];
static float calibrated_acc_g[3];
// sensor_processing_task的调用频率, dt = 0.001s
const float delta_time = 0.01f;
// --- 2. 初始化 ---
if (!initialized) {
skiing_tracker_init(&my_skiing_tracker);
initialized = 1;
printf("Skiing Tracker Initialized. Waiting for sensor calibration...\n");
}
// --- 3. 数据处理 ---
//合并加速度和陀螺仪数据
memcpy(&combined_raw_data[0], acc_data_buf, 3 * sizeof(signed short));
memcpy(&combined_raw_data[3], gyr_data_buf, 3 * sizeof(signed short));
unsigned char status;
if (get_calibration_state() == 0) { //正在校准
//领票校准
status = 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 {
status = SL_SC7U22_Angle_Output(0, combined_raw_data, final_angle_data, 0);
}
// c. 检查姿态解算是否成功
if (status == 1) {
// 将校准后的加速度数据从 LSB (原始值) 转换为 g (重力单位)
// ±8g量程下，8192 LSB 对应 1g
calibrated_acc_g[0] = (float)combined_raw_data[0] / 8192.0f;
calibrated_acc_g[1] = (float)combined_raw_data[1] / 8192.0f;
calibrated_acc_g[2] = (float)combined_raw_data[2] / 8192.0f;
skiing_tracker_update(&my_skiing_tracker, calibrated_acc_g, final_angle_data, delta_time);
static count = 0;
if(count < 10){
count++;
return;
}else{
count = 0;
}
printf("State: %d, Speed: %.2f m/s, Distance: %.2f m\n",
my_skiing_tracker.state,
my_skiing_tracker.speed,
my_skiing_tracker.distance);
} else if (status == 0) {
// 传感器正在进行静态校准
// printf("Sensor is calibrating...\n");
} else {
// status == 2, 表示校准失败或发生错误
// printf("Angle calculation error or calibration not finished.\n");
}
}

41
apps/earphone/xtell_Sensor/A_hide/1/skiing_tracker.h Normal file
View File

@ -0,0 +1,41 @@
#ifndef SKIING_TRACKER_H
#define SKIING_TRACKER_H
// 定义滑雪者可能的状态
typedef enum {
SKIING_STATE_STATIC, // 静止
SKIING_STATE_SKIING, // 正在滑雪
SKIING_STATE_FALLEN, // 已摔倒
SKIING_STATE_UNKNOWN // 未知状态
} skiing_state_t;
// 追踪器数据结构体
typedef struct {
float velocity[3]; // 当前速度 (x, y, z)，单位: m/s
float distance; // 总滑行距离，单位: m
float speed; // 当前速率 (标量)，单位: m/s
skiing_state_t state; // 当前滑雪状态
// 私有成员，用于内部计算
int static_frames_count; // 用于判断静止状态的帧计数器
float acc_world[3]; // 在世界坐标系下的加速度
} skiing_tracker_t;
/**
* @brief 初始化滑雪追踪器
*
* @param tracker 指向 skiing_tracker_t 结构体的指针
*/
void skiing_tracker_init(skiing_tracker_t *tracker);
/**
* @brief 处理传感器数据并更新滑雪状态
*
* @param tracker 指向 skiing_tracker_t 结构体的指针
* @param acc 校准后的加速度数据 [x, y, z]，单位: g (1g = 9.8m/s^2)
* @param angle 姿态角数据 [pitch, roll, yaw]，单位: 度
* @param dt 采样时间间隔，单位: 秒 (s)
*/
void skiing_tracker_update(skiing_tracker_t *tracker, float *acc, float *angle, float dt);
#endif // SKIING_TRACKER_H

259
apps/earphone/xtell_Sensor/A_hide/2/skiing_tracker.c Normal file
View File

@ -0,0 +1,259 @@
/*
动态ZUPT+卡尔曼
*/
#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)
// --- 算法阈值定义 ---
// 动态零速更新(ZUPT)阈值：加速度方差小于此值，认为处于动态稳定状态
#define ZUPT_ACC_VARIANCE_THRESHOLD 0.05f
// 旋转检测阈值：Z轴角速度大于此值(度/秒)，认为正在原地旋转
#define ROTATION_GYR_Z_THRESHOLD 60.0f
// 启动滑雪阈值：加速度模长与重力的差值大于此值，认为开始运动
#define START_SKIING_ACC_THRESHOLD 1.5f
/**
* @brief 初始化滑雪追踪器
*/
void skiing_tracker_init(skiing_tracker_t *tracker)
{
if (!tracker) {
return;
}
// 使用memset一次性清零整个结构体，包括新增的缓冲区
memset(tracker, 0, sizeof(skiing_tracker_t));
tracker->state = SKIING_STATE_STATIC;
}
/**
* @brief 将设备坐标系下的加速度转换为世界坐标系
* @param acc_device 设备坐标系下的加速度 [x, y, z]
* @param angle 姿态角 [pitch, roll, yaw]，单位: 度
* @param acc_world 输出：世界坐标系下的加速度 [x, y, z]
*/
static void transform_acc_to_world_frame(const float *acc_device, const float *angle, float *acc_world)
{
// 驱动输出的角度与标准航空定义相反，需要取反才能用于标准旋转矩阵。
float pitch = -angle[0] * DEG_TO_RAD;
float roll = -angle[1] * DEG_TO_RAD;
float cp = cosf(pitch);
float sp = sinf(pitch);
float cr = cosf(roll);
float sr = sinf(roll);
float ax = acc_device[0];
float ay = acc_device[1];
float az = acc_device[2];
// 使用经过验证的、正确的身体坐标系到世界坐标系的旋转矩阵 (基于 Y-X 旋转顺序)
// 这个矩阵将设备测量的加速度(ax, ay, az)正确地转换到世界坐标系(acc_world)。
acc_world[0] = cp * ax + sp * sr * ay + sp * cr * az;
acc_world[1] = 0 * ax + cr * ay - sr * az;
acc_world[2] = -sp * ax + cp * sr * ay + cp * cr * az;
}
/**
* @brief 计算缓冲区内三轴数据的方差之和
*/
static float calculate_variance(float buffer[VARIANCE_BUFFER_SIZE][3])
{
float mean[3] = {0};
float variance[3] = {0};
// 1. 计算均值
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;
// 2. 计算方差
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 升级后的状态机，包含旋转检测和动态零速更新
*/
static void update_state_machine(skiing_tracker_t *tracker, const float *acc_device_ms2, const float *gyr_dps)
{
// 缓冲区未填满时，不进行状态判断，默认为静止
if (!tracker->buffer_filled) {
tracker->state = SKIING_STATE_STATIC;
return;
}
// --- 计算关键指标 ---
float acc_variance = calculate_variance(tracker->acc_buffer);
float gyr_z_abs = fabsf(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]);
// --- 状态切换逻辑 (按优先级) ---
// 旋转检测
// 如果Z轴角速度很大，则判断为原地旋转，暂停积分
if (gyr_z_abs > ROTATION_GYR_Z_THRESHOLD) {
tracker->state = SKIING_STATE_ROTATING;
return; // 直接返回，不执行后续判断
}
// 动态零速更新 (ZUPT)
// 如果加速度在窗口期内非常稳定（方差很小），则认为是动态稳定状态，进行零速校正
if (acc_variance < ZUPT_ACC_VARIANCE_THRESHOLD) {
tracker->state = SKIING_STATE_STATIC;
// 速度清零，抑制漂移
memset(tracker->velocity, 0, sizeof(tracker->velocity));
tracker->speed = 0.0f;
return;
}
// 从静止/旋转状态启动
if (tracker->state == SKIING_STATE_STATIC || tracker->state == SKIING_STATE_ROTATING) {
if (fabsf(acc_magnitude - G_ACCELERATION) > START_SKIING_ACC_THRESHOLD) {
tracker->state = SKIING_STATE_SKIING;
return;
}
}
// 滑雪
if (tracker->state != SKIING_STATE_STATIC) {
tracker->state = SKIING_STATE_SKIING;
}
}
/**
* @brief 主更新函数
*/
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;
}
// --- 1. 数据预处理和缓冲 ---
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; // 标记缓冲区已满
}
// --- 2. 更新状态机 ---
update_state_machine(tracker, acc_device_ms2, gyr_dps);
// --- 3. 根据状态进行计算 ---
// 只有在明确的“滑雪”状态下才进行积分
if (tracker->state == SKIING_STATE_SKIING) {
transform_acc_to_world_frame(acc_device_ms2, angle, tracker->acc_world);
tracker->acc_world[2] -= G_ACCELERATION;
tracker->velocity[0] += tracker->acc_world[0] * dt;
tracker->velocity[1] += tracker->acc_world[1] * dt;
tracker->velocity[2] += tracker->acc_world[2] * dt;
}
// 在其他状态下（静止、旋转），速度已经在状态机内部被清零或保持不变
// --- 4. 更新速率和距离 ---
// 速率和距离总是在更新，但在非滑雪状态下，速度为0，所以它们不会增加
tracker->speed = sqrtf(tracker->velocity[0] * tracker->velocity[0] +
tracker->velocity[1] * tracker->velocity[1] +
tracker->velocity[2] * tracker->velocity[2]);
tracker->distance += tracker->speed * dt;
}
// 传感器数据采集与处理任务
void 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;
static signed short combined_raw_data[6];
static float final_angle_data[3];
static float calibrated_acc_g[3];
static float calibrated_gyr_dps[3]; // 新增：用于存放转换后的陀螺仪数据
const float delta_time = 0.01f;
if (!initialized) {
skiing_tracker_init(&my_skiing_tracker);
initialized = 1;
printf("Skiing Tracker Initialized. Waiting for sensor calibration...\n");
}
memcpy(&combined_raw_data[0], acc_data_buf, 3 * sizeof(signed short));
memcpy(&combined_raw_data[3], gyr_data_buf, 3 * sizeof(signed short));
unsigned char status;
if (!calibration_done) {
status = 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 {
status = SL_SC7U22_Angle_Output(0, combined_raw_data, final_angle_data, 0);
}
if (status == 1) {
// 加速度 LSB to g
calibrated_acc_g[0] = (float)combined_raw_data[0] / 8192.0f;
calibrated_acc_g[1] = (float)combined_raw_data[1] / 8192.0f;
calibrated_acc_g[2] = (float)combined_raw_data[2] / 8192.0f;
// 陀螺仪 LSB to dps (度/秒)
// ±2000dps量程下，转换系数约为 0.061
calibrated_gyr_dps[0] = (float)combined_raw_data[3] * 0.061f;
calibrated_gyr_dps[1] = (float)combined_raw_data[4] * 0.061f;
calibrated_gyr_dps[2] = (float)combined_raw_data[5] * 0.061f;
skiing_tracker_update(&my_skiing_tracker, calibrated_acc_g, calibrated_gyr_dps, final_angle_data, delta_time);
// 打印逻辑保持不变
static int count = 0;
if(count < 10){
count++;
return;
} else {
count = 0;
}
printf("State: %d, Speed: %.2f m/s, Distance: %.2f m\n",
my_skiing_tracker.state,
my_skiing_tracker.speed,
my_skiing_tracker.distance);
} else if (status == 0) {
// printf("Sensor is calibrating...\n");
} else {
// printf("Angle calculation error or calibration not finished.\n");
}
}

50
apps/earphone/xtell_Sensor/A_hide/2/skiing_tracker.h Normal file
View File

@ -0,0 +1,50 @@
#ifndef SKIING_TRACKER_H
#define SKIING_TRACKER_H
// 定义滑雪者可能的状态
typedef enum {
SKIING_STATE_STATIC, // 静止或动态稳定
SKIING_STATE_SKIING, // 正在滑雪
SKIING_STATE_ROTATING, // 正在原地旋转 (新增)
SKIING_STATE_FALLEN, // 已摔倒
SKIING_STATE_UNKNOWN // 未知状态
} skiing_state_t;
#define VARIANCE_BUFFER_SIZE 15 // 用于计算方差的数据窗口大小 (15个样本 @ 100Hz = 150ms)
// 追踪器数据结构体
typedef struct {
// 公开数据
float velocity[3]; // 当前速度 (x, y, z)，单位: m/s
float distance; // 总滑行距离，单位: m
float speed; // 当前速率 (标量)，单位: m/s
skiing_state_t state; // 当前滑雪状态
// 内部计算使用的私有成员
float acc_world[3]; // 在世界坐标系下的加速度
// 用于动态零速更新和旋转检测的缓冲区
float acc_buffer[VARIANCE_BUFFER_SIZE][3]; // 加速度数据窗口
float gyr_buffer[VARIANCE_BUFFER_SIZE][3]; // 角速度数据窗口
int buffer_index; // 缓冲区当前索引
int buffer_filled; // 缓冲区是否已填满的标志
} skiing_tracker_t;
/**
* @brief 初始化滑雪追踪器
*
* @param tracker 指向 skiing_tracker_t 结构体的指针
*/
void skiing_tracker_init(skiing_tracker_t *tracker);
/**
* @brief 处理传感器数据并更新滑雪状态
*
* @param tracker 指向 skiing_tracker_t 结构体的指针
* @param acc_g 校准后的加速度数据 [x, y, z]，单位: g (1g = 9.8m/s^2)
* @param gyr_dps 角速度
* @param angle 姿态角数据 [pitch, roll, yaw]，单位: 度
* @param dt 采样时间间隔，单位: 秒 (s)
*/
void skiing_tracker_update(skiing_tracker_t *tracker, float *acc_g, float *gyr_dps, float *angle, float dt);
#endif // SKIING_TRACKER_H

277
apps/earphone/xtell_Sensor/A_hide/3/skiing_tracker.c Normal file
View File

@ -0,0 +1,277 @@
/*
动态ZUPT+卡尔曼
*/
#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)
// --- 算法阈值定义 ---
//两个判断是否静止的必要条件
// 动态零速更新(ZUPT)阈值
// 提高阈值，让“刹车”更灵敏，以便在波浪式前进等慢速漂移时也能触发零速更新
#define ZUPT_ACC_VARIANCE_THRESHOLD 0.2f
// 陀螺仪方差阈值
#define ZUPT_GYR_VARIANCE_THRESHOLD 5.0f
// 旋转/摆动检测阈值：角速度总模长大于此值(度/秒)，认为正在进行非滑雪的旋转或摆动
#define ROTATION_GYR_MAG_THRESHOLD 45.0f
// 启动滑雪阈值：加速度模长与重力的差值大于此值，认为开始运动
// 降低阈值，让“油门”更灵敏，以便能捕捉到真实的慢速启动
#define START_SKIING_ACC_THRESHOLD 0.5f
// 新增：速度阻尼系数，用于模拟摩擦力，抑制漂移
#define VELOCITY_DAMPING_FACTOR 0.98f
/**
* @brief 初始化滑雪追踪器
*/
void skiing_tracker_init(skiing_tracker_t *tracker)
{
if (!tracker) {
return;
}
// 使用memset一次性清零整个结构体，包括新增的缓冲区
memset(tracker, 0, sizeof(skiing_tracker_t));
tracker->state = SKIING_STATE_STATIC;
}
/**
* @brief 将设备坐标系下的加速度转换为世界坐标系
* @param acc_device 设备坐标系下的加速度 [x, y, z]
* @param angle 姿态角 [pitch, roll, yaw]，单位: 度
* @param acc_world 输出：世界坐标系下的加速度 [x, y, z]
*/
static void transform_acc_to_world_frame(const float *acc_device, const float *angle, float *acc_world)
{
// 驱动输出的角度与标准航空定义相反，需要取反才能用于标准旋转矩阵。
float pitch = -angle[0] * DEG_TO_RAD;
float roll = -angle[1] * DEG_TO_RAD;
float cp = cosf(pitch);
float sp = sinf(pitch);
float cr = cosf(roll);
float sr = sinf(roll);
float ax = acc_device[0];
float ay = acc_device[1];
float az = acc_device[2];
// 使用经过验证的、正确的身体坐标系到世界坐标系的旋转矩阵 (基于 Y-X 旋转顺序)
// 这个矩阵将设备测量的加速度(ax, ay, az)正确地转换到世界坐标系(acc_world)。
acc_world[0] = cp * ax + sp * sr * ay + sp * cr * az;
acc_world[1] = 0 * ax + cr * ay - sr * az;
acc_world[2] = -sp * ax + cp * sr * ay + cp * cr * az;
}
/**
* @brief 计算缓冲区内三轴数据的方差之和
*/
static float calculate_variance(float buffer[VARIANCE_BUFFER_SIZE][3])
{
float mean[3] = {0};
float variance[3] = {0};
// 1. 计算均值
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;
// 2. 计算方差
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 升级后的状态机，包含旋转检测和动态零速更新
*/
static void update_state_machine(skiing_tracker_t *tracker, const float *acc_device_ms2, const float *gyr_dps)
{
// 缓冲区未填满时，不进行状态判断，默认为静止
if (!tracker->buffer_filled) {
tracker->state = SKIING_STATE_STATIC;
return;
}
// --- 1. 计算关键指标 ---
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]);
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]);
// --- 2. 状态切换逻辑 (按优先级) ---
// 优先级1：原地旋转/摆动检测 (最终版)
// 增加一个关键前提：只在当前不处于滑雪状态时，才检测原地旋转。
// 这可以防止滑雪过程中的高速转弯被误判为原地旋转。
if (gyr_magnitude > ROTATION_GYR_MAG_THRESHOLD && tracker->state != SKIING_STATE_SKIING) {
tracker->state = SKIING_STATE_ROTATING;
return;
}
// 动态零速更新 (ZUPT)
// 必须同时满足加速度和角速度都稳定，才能判断为“真静止”，以区分匀速运动
if (acc_variance < ZUPT_ACC_VARIANCE_THRESHOLD && gyr_variance < ZUPT_GYR_VARIANCE_THRESHOLD) {
tracker->state = SKIING_STATE_STATIC;
// 速度清零，抑制漂移
memset(tracker->velocity, 0, sizeof(tracker->velocity));
tracker->speed = 0.0f;
return;
}
// 从静止/旋转状态启动
if (tracker->state == SKIING_STATE_STATIC || tracker->state == SKIING_STATE_ROTATING) {
// 最终版启动逻辑：必须同时满足“有足够大的线性加速度”和“旋转稳定”两个条件
if (fabsf(acc_magnitude - G_ACCELERATION) > START_SKIING_ACC_THRESHOLD && gyr_variance < ZUPT_GYR_VARIANCE_THRESHOLD) {
tracker->state = SKIING_STATE_SKIING;
return;
}
}
// 滑雪
if (tracker->state != SKIING_STATE_STATIC) {
tracker->state = SKIING_STATE_SKIING;
}
}
/**
* @brief 主更新函数
*/
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;
}
// --- 1. 数据预处理和缓冲 ---
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; // 标记缓冲区已满
}
// --- 2. 更新状态机 ---
update_state_machine(tracker, acc_device_ms2, gyr_dps);
// --- 3. 根据状态进行计算 ---
// 只有在明确的“滑雪”状态下才进行积分
if (tracker->state == SKIING_STATE_SKIING) {
transform_acc_to_world_frame(acc_device_ms2, angle, tracker->acc_world);
tracker->acc_world[2] -= G_ACCELERATION;
tracker->velocity[0] += tracker->acc_world[0] * dt;
tracker->velocity[1] += tracker->acc_world[1] * dt;
tracker->velocity[2] += tracker->acc_world[2] * dt;
}
// 在其他状态下（静止、旋转），速度已经在状态机内部被清零或保持不变
// --- 4. 引入速度阻尼（软件摩擦力） ---
// 只要不处于滑雪状态，就对速度进行衰减，以对抗漂移和抑制抖动产生的微小速度
if (tracker->state != SKIING_STATE_SKIING) {
tracker->velocity[0] *= VELOCITY_DAMPING_FACTOR;
tracker->velocity[1] *= VELOCITY_DAMPING_FACTOR;
tracker->velocity[2] *= VELOCITY_DAMPING_FACTOR;
}
// --- 5. 更新速率和距离 ---
tracker->speed = sqrtf(tracker->velocity[0] * tracker->velocity[0] +
tracker->velocity[1] * tracker->velocity[1] +
tracker->velocity[2] * tracker->velocity[2]);
tracker->distance += tracker->speed * dt;
}
// 传感器数据采集与处理任务
void 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;
static signed short combined_raw_data[6];
static float final_angle_data[3]; // 计算得到的欧若拉角
static float calibrated_acc_g[3]; // 转换后的加速度计数据
static float calibrated_gyr_dps[3]; // 转换后的陀螺仪数据
const float delta_time = 0.01f;
if (!initialized) {
skiing_tracker_init(&my_skiing_tracker);
initialized = 1;
printf("Skiing Tracker Initialized. Waiting for sensor calibration...\n");
}
memcpy(&combined_raw_data[0], acc_data_buf, 3 * sizeof(signed short));
memcpy(&combined_raw_data[3], gyr_data_buf, 3 * sizeof(signed short));
unsigned char status;
if (!calibration_done) { //第1次启动，开启零漂检测
status = 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 {
status = SL_SC7U22_Angle_Output(0, combined_raw_data, final_angle_data, 0);
}
if (status == 1) {
// 加速度 LSB to g
calibrated_acc_g[0] = (float)combined_raw_data[0] / 8192.0f;
calibrated_acc_g[1] = (float)combined_raw_data[1] / 8192.0f;
calibrated_acc_g[2] = (float)combined_raw_data[2] / 8192.0f;
// 陀螺仪 LSB to dps (度/秒)
// ±2000dps量程下，转换系数约为 0.061
calibrated_gyr_dps[0] = (float)combined_raw_data[3] * 0.061f;
calibrated_gyr_dps[1] = (float)combined_raw_data[4] * 0.061f;
calibrated_gyr_dps[2] = (float)combined_raw_data[5] * 0.061f;
skiing_tracker_update(&my_skiing_tracker, calibrated_acc_g, calibrated_gyr_dps, final_angle_data, delta_time);
// 打印逻辑保持不变
static int count = 0;
if(count < 10){
count++;
return;
} else {
count = 0;
}
printf("State: %d, Speed: %.2f m/s, Distance: %.2f m\n",
my_skiing_tracker.state,
my_skiing_tracker.speed,
my_skiing_tracker.distance);
} else if (status == 0) {
// printf("Sensor is calibrating...\n");
} else {
// printf("Angle calculation error or calibration not finished.\n");
}
}

50
apps/earphone/xtell_Sensor/A_hide/3/skiing_tracker.h Normal file
View File

@ -0,0 +1,50 @@
#ifndef SKIING_TRACKER_H
#define SKIING_TRACKER_H
// 定义滑雪者可能的状态
typedef enum {
SKIING_STATE_STATIC, // 静止或动态稳定
SKIING_STATE_SKIING, // 正在滑雪
SKIING_STATE_ROTATING, // 正在原地旋转 (新增)
SKIING_STATE_FALLEN, // 已摔倒
SKIING_STATE_UNKNOWN // 未知状态
} skiing_state_t;
#define VARIANCE_BUFFER_SIZE 15 // 用于计算方差的数据窗口大小 (15个样本 @ 100Hz = 150ms)
// 追踪器数据结构体
typedef struct {
// 公开数据
float velocity[3]; // 当前速度 (x, y, z)，单位: m/s
float distance; // 总滑行距离，单位: m
float speed; // 当前速率 (标量)，单位: m/s
skiing_state_t state; // 当前滑雪状态
// 内部计算使用的私有成员
float acc_world[3]; // 在世界坐标系下的加速度
// 用于动态零速更新和旋转检测的缓冲区
float acc_buffer[VARIANCE_BUFFER_SIZE][3]; // 加速度数据窗口
float gyr_buffer[VARIANCE_BUFFER_SIZE][3]; // 角速度数据窗口
int buffer_index; // 缓冲区当前索引
int buffer_filled; // 缓冲区是否已填满的标志
} skiing_tracker_t;
/**
* @brief 初始化滑雪追踪器
*
* @param tracker 指向 skiing_tracker_t 结构体的指针
*/
void skiing_tracker_init(skiing_tracker_t *tracker);
/**
* @brief 处理传感器数据并更新滑雪状态
*
* @param tracker 指向 skiing_tracker_t 结构体的指针
* @param acc_g 校准后的加速度数据 [x, y, z]，单位: g (1g = 9.8m/s^2)
* @param gyr_dps 角速度
* @param angle 姿态角数据 [pitch, roll, yaw]，单位: 度
* @param dt 采样时间间隔，单位: 秒 (s)
*/
void skiing_tracker_update(skiing_tracker_t *tracker, float *acc_g, float *gyr_dps, float *angle, float dt);
#endif // SKIING_TRACKER_H

465
apps/earphone/xtell_Sensor/A_hide/4/skiing_tracker.c Normal file
View File

@ -0,0 +1,465 @@
/**
* 效果不行，对原地转动太灵敏了
*/
#include "skiing_tracker.h"
#include <math.h>
#include <string.h>
// --- 核心算法参数 ---
#define COMPLEMENTARY_FILTER_ALPHA 0.96f // 互补滤波器alpha值 (降低alpha, 增加对加速度计的信任, 更好地抑制陀螺仪漂移)
#define ACC_SENSITIVITY 8192.0f // 加速度计灵敏度 (LSB/g), 对应 ±4g 量程
#define GYRO_SENSITIVITY 16.4f // 陀螺仪灵敏度 (LSB/dps), 对应 ±2000dps 量程
#define SKI_DT_FIXED 0.01f // 固定时间间隔 10ms (100Hz)
// --- 状态机和阈值 ---
#define SKI_STATIC_THRESHOLD_ACC 0.3f // 静止检测加速度阈值 (m/s^2) - 提高以忽略更多噪声
#define SKI_STATIC_THRESHOLD_GYRO 3.0f // 静止检测角速度阈值 (dps) - 更严格
#define SKI_MOVEMENT_THRESHOLD 1.2f // 运动启动阈值 (m/s^2) - 显著提高，需要更明确的运动意图
#define SKI_WINDOW_SIZE 10 // 滑动窗口大小 (当前未使用)
#define SKI_CALIBRATION_SAMPLES 100 // 校准采样点数 (恢复到100，确保校准精度)
// 滑雪数据结构
typedef struct {
// 状态相关
SkiState_t current_state;
uint32_t state_duration; // 状态持续时间(ms)
uint8_t static_counter; // 静态计数器
// --- 核心运动数据 ---
// 姿态数据 (Roll, Pitch)
float attitude[2];
// 世界坐标系下的加速度
float world_acc[3];
// 世界坐标系下的速度
float velocity[3];
// 水平面速度和总距离
float horizontal_speed;
float total_horizontal_distance;
// 历史数据 (用于滤波)
float acc_history[SKI_WINDOW_SIZE][3];
float gyro_history[SKI_WINDOW_SIZE][3];
uint8_t history_index;
// 时间相关
float dt; // 时间间隔
// 运动检测
float movement_score; // 运动评分
float turning_score; // 转弯评分
// 校准数据 (用于陀螺仪零偏)
float gyro_bias[3];
float acc_bias[3];
float calibration_sum[6];
uint32_t calibration_count;
uint8_t calibrated;
// 状态标志和计数器
uint32_t update_count;
float velocity_decay_factor;
uint8_t is_moving;
} SkiData_t;
static SkiData_t ski_data = {0};
// --- 内部核心函数声明 ---
// 校准
static void Ski_CalibrateSensor(float *acc, float *gyro); // 校准传感器
// 姿态解算
static void Ski_UpdateAttitude(float *acc, float *gyro); // 更新姿态
// 坐标系转换
static void Ski_TransformToWorldFrame(float *acc, float *world_acc); // 转换到世界坐标系
// 速度与距离积分
static void Ski_UpdateVelocityAndDistance(void); // 更新速度和距离
// --- 辅助函数声明 ---
static float Ski_CalculateMovementScore(void); // 计算运动评分
static float Ski_CalculateTurningScore(float *gyro);
static void Ski_UpdateStateMachine(void);
static void Ski_MovingAverageFilter(float *new_acc, float *new_gyro, float *acc_filtered, float *gyro_filtered);
// 修改初始化函数
void Ski_Init(void)
{
memset(&ski_data, 0, sizeof(SkiData_t));
ski_data.current_state = SKI_STATE_STATIC;
// 初始化参数
ski_data.velocity_decay_factor = 0.8f; // 速度衰减因子
ski_data.is_moving = 0;
ski_data.update_count = 0;
ski_data.dt = SKI_DT_FIXED;
}
// 滑动窗口滤波
static void Ski_MovingAverageFilter(float *new_acc, float *new_gyro,
float *acc_filtered, float *gyro_filtered)
{
// 更新历史数据
ski_data.acc_history[ski_data.history_index][0] = new_acc[0];
ski_data.acc_history[ski_data.history_index][1] = new_acc[1];
ski_data.acc_history[ski_data.history_index][2] = new_acc[2];
ski_data.gyro_history[ski_data.history_index][0] = new_gyro[0];
ski_data.gyro_history[ski_data.history_index][1] = new_gyro[1];
ski_data.gyro_history[ski_data.history_index][2] = new_gyro[2];
ski_data.history_index = (ski_data.history_index + 1) % SKI_WINDOW_SIZE;
// 计算平均值
memset(acc_filtered, 0, 3 * sizeof(float));
memset(gyro_filtered, 0, 3 * sizeof(float));
for (int i = 0; i < SKI_WINDOW_SIZE; i++) {
acc_filtered[0] += ski_data.acc_history[i][0];
acc_filtered[1] += ski_data.acc_history[i][1];
acc_filtered[2] += ski_data.acc_history[i][2];
gyro_filtered[0] += ski_data.gyro_history[i][0];
gyro_filtered[1] += ski_data.gyro_history[i][1];
gyro_filtered[2] += ski_data.gyro_history[i][2];
}
acc_filtered[0] /= SKI_WINDOW_SIZE;
acc_filtered[1] /= SKI_WINDOW_SIZE;
acc_filtered[2] /= SKI_WINDOW_SIZE;
gyro_filtered[0] /= SKI_WINDOW_SIZE;
gyro_filtered[1] /= SKI_WINDOW_SIZE;
gyro_filtered[2] /= SKI_WINDOW_SIZE;
}
// =================================================================================
// 核心更新函数 - 已重构
// =================================================================================
void Ski_UpdateData(int16_t acc_x, int16_t acc_y, int16_t acc_z,
int16_t gyro_x, int16_t gyro_y, int16_t gyro_z)
{
ski_data.update_count++;
// 1. LSB原始数据转换为物理单位 (g 和 度/秒)
float acc[3] = { (float)acc_x / ACC_SENSITIVITY, (float)acc_y / ACC_SENSITIVITY, (float)acc_z / ACC_SENSITIVITY };
float gyro[3] = { (float)gyro_x / GYRO_SENSITIVITY, (float)gyro_y / GYRO_SENSITIVITY, (float)gyro_z / GYRO_SENSITIVITY };
// 2. 传感器校准 (移除零偏)
if (!ski_data.calibrated) {
Ski_CalibrateSensor(acc, gyro);
return; // 等待校准完成
}
acc[0] -= ski_data.acc_bias[0];
acc[1] -= ski_data.acc_bias[1];
acc[2] -= ski_data.acc_bias[2];
gyro[0] -= ski_data.gyro_bias[0];
gyro[1] -= ski_data.gyro_bias[1];
gyro[2] -= ski_data.gyro_bias[2];
// 3. 姿态解算 (互补滤波器)
Ski_UpdateAttitude(acc, gyro);
// 4. 将身体坐标系的加速度转换为世界坐标系
Ski_TransformToWorldFrame(acc, ski_data.world_acc);
// 5. 在世界坐标系中移除重力 (重力永远是 [0, 0, 1]g)
ski_data.world_acc[2] -= 1.0f;
// 6. 对世界坐标系下的加速度进行滤波 (可选，但推荐)
// 注意: 如果需要，可在此处重用滑动平均滤波器，但暂时我们直接使用原始世界坐标系加速度
// 7. 基于世界坐标系的数据更新状态机
Ski_UpdateStateMachine();
// 8. 基于世界坐标系的数据更新速度和距离
Ski_UpdateVelocityAndDistance();
}
// 计算转弯评分
static float Ski_CalculateTurningScore(float *gyro)
{
// 主要考虑Z轴旋转(偏航)和X轴旋转(俯仰)
float turning_score = fabsf(gyro[2]) * 0.6f + fabsf(gyro[0]) * 0.4f;
return turning_score;
}
// =================================================================================
// 新增核心算法函数
// =================================================================================
/**
* @brief 在静止状态下计算传感器零偏
*/
static void Ski_CalibrateSensor(float *acc, float *gyro)
{
float acc_mag = sqrtf(acc[0]*acc[0] + acc[1]*acc[1] + acc[2]*acc[2]);
float gyro_mag = sqrtf(gyro[0]*gyro[0] + gyro[1]*gyro[1] + gyro[2]*gyro[2]);
// 放宽静止判断条件，使其更容易满足
if (fabsf(acc_mag - 1.0f) < 0.1f && gyro_mag < SKI_STATIC_THRESHOLD_GYRO) {
ski_data.calibration_sum[0] += acc[0];
ski_data.calibration_sum[1] += acc[1];
ski_data.calibration_sum[2] += acc[2];
ski_data.calibration_sum[3] += gyro[0];
ski_data.calibration_sum[4] += gyro[1];
ski_data.calibration_sum[5] += gyro[2];
ski_data.calibration_count++;
if (ski_data.calibration_count >= SKI_CALIBRATION_SAMPLES) {
// 计算加速度计零偏
ski_data.acc_bias[0] = ski_data.calibration_sum[0] / ski_data.calibration_count;
ski_data.acc_bias[1] = ski_data.calibration_sum[1] / ski_data.calibration_count;
// Z轴的零偏是相对于1g的, 需要找到重力轴
float gravity_mag = sqrtf(powf(ski_data.calibration_sum[0] / ski_data.calibration_count, 2) +
powf(ski_data.calibration_sum[1] / ski_data.calibration_count, 2) +
powf(ski_data.calibration_sum[2] / ski_data.calibration_count, 2));
ski_data.acc_bias[2] = ski_data.calibration_sum[2] / ski_data.calibration_count - gravity_mag;
// 计算陀螺仪零偏
ski_data.gyro_bias[0] = ski_data.calibration_sum[3] / ski_data.calibration_count;
ski_data.gyro_bias[1] = ski_data.calibration_sum[4] / ski_data.calibration_count;
ski_data.gyro_bias[2] = ski_data.calibration_sum[5] / ski_data.calibration_count;
ski_data.calibrated = 1;
printf("传感器校准完成!\n");
}
} else {
// 如果检测到移动，则重置校准计数
ski_data.calibration_count = 0;
memset(ski_data.calibration_sum, 0, sizeof(ski_data.calibration_sum));
// 增加调试打印，方便定位问题
// printf("Calibration reset. Acc mag: %.3f, Gyro mag: %.3f\n", acc_mag, gyro_mag);
}
}
/**
* @brief 使用互补滤波器更新姿态角(Roll, Pitch)
*/
static void Ski_UpdateAttitude(float *acc, float *gyro)
{
float dt = ski_data.dt;
float alpha = COMPLEMENTARY_FILTER_ALPHA;
// 从加速度计计算角度 (单位: 度)
float roll_acc = atan2f(acc[1], acc[2]) * 180.0f / M_PI;
float pitch_acc = atan2f(-acc[0], sqrtf(acc[1] * acc[1] + acc[2] * acc[2])) * 180.0f / M_PI;
// 陀螺仪积分预测角度
// 注意: 这里的gyro单位是 度/秒
float roll_gyro = ski_data.attitude[0] + gyro[0] * dt;
float pitch_gyro = ski_data.attitude[1] + gyro[1] * dt;
// 互补滤波融合
ski_data.attitude[0] = alpha * roll_gyro + (1.0f - alpha) * roll_acc;
ski_data.attitude[1] = alpha * pitch_gyro + (1.0f - alpha) * pitch_acc;
}
/**
* @brief 将身体坐标系的加速度转换为世界坐标系
*/
static void Ski_TransformToWorldFrame(float *acc, float *world_acc)
{
// 将姿态角转换为弧度
float roll = ski_data.attitude[0] * M_PI / 180.0f;
float pitch = ski_data.attitude[1] * M_PI / 180.0f;
float cos_roll = cosf(roll);
float sin_roll = sinf(roll);
float cos_pitch = cosf(pitch);
float sin_pitch = sinf(pitch);
// 通过旋转矩阵将身体坐标系的加速度(acc)转换到世界坐标系(world_acc)
// (简化版，假设偏航角yaw=0)
world_acc[0] = acc[0] * cos_pitch + acc[2] * sin_pitch;
world_acc[1] = acc[0] * sin_pitch * sin_roll + acc[1] * cos_roll - acc[2] * cos_pitch * sin_roll;
world_acc[2] = -acc[0] * sin_pitch * cos_roll + acc[1] * sin_roll + acc[2] * cos_pitch * cos_roll;
}
// 改进的运动评分计算
static float Ski_CalculateMovementScore(void)
{
// 在世界坐标系下计算运动评分，只考虑水平加速度
float horizontal_acc_mag = sqrtf(ski_data.world_acc[0]*ski_data.world_acc[0] + ski_data.world_acc[1]*ski_data.world_acc[1]);
return horizontal_acc_mag * 9.81f; // 从 g 转换为 m/s^2
}
// 改进的状态机 - 添加更严格的转换条件
/**
* @brief 基于世界坐标系下的加速度更新状态机
*/
static void Ski_UpdateStateMachine(void)
{
ski_data.state_duration++;
ski_data.movement_score = Ski_CalculateMovementScore();
switch (ski_data.current_state) {
case SKI_STATE_STATIC:
if (ski_data.movement_score > SKI_MOVEMENT_THRESHOLD) {
ski_data.static_counter++;
if (ski_data.static_counter > 5) { // 需要连续几帧的运动才切换
ski_data.current_state = SKI_STATE_MOVING;
ski_data.is_moving = 1;
ski_data.state_duration = 0;
ski_data.static_counter = 0;
}
} else {
ski_data.static_counter = 0;
}
break;
case SKI_STATE_MOVING:
// 使用更低的阈值来检测停止，形成迟滞效应，防止状态抖动
if (ski_data.movement_score < SKI_STATIC_THRESHOLD_ACC) {
ski_data.static_counter++;
if (ski_data.static_counter > 8) { // 减少进入静止状态的等待时间
ski_data.current_state = SKI_STATE_STATIC;
ski_data.is_moving = 0;
ski_data.state_duration = 0;
ski_data.static_counter = 0;
// 停止时强制将速度清零，以防止漂移
memset(ski_data.velocity, 0, sizeof(ski_data.velocity));
}
} else {
ski_data.static_counter = 0;
}
// TODO: 如果需要，可以增加转弯状态的检测
break;
// 如果需要，可以增加其他状态 (例如 TURNING, STOPPING)
default:
ski_data.current_state = SKI_STATE_STATIC;
break;
}
}
// 改进的速度和距离更新 - 添加零速度更新(ZUPT)
/**
* @brief 在世界坐标系下更新速度和距离
*/
static void Ski_UpdateVelocityAndDistance(void)
{
// 将世界坐标系下的加速度从 g 转换为 m/s^2
float acc_ms2[3];
acc_ms2[0] = ski_data.world_acc[0] * 9.81f;
acc_ms2[1] = ski_data.world_acc[1] * 9.81f;
acc_ms2[2] = ski_data.world_acc[2] * 9.81f;
// 是否进行积分的决定由状态机控制
if (ski_data.is_moving) {
// 速度积分
ski_data.velocity[0] += acc_ms2[0] * ski_data.dt;
ski_data.velocity[1] += acc_ms2[1] * ski_data.dt;
// 如果需要，也可以对垂直速度进行积分
// ski_data.velocity[2] += acc_ms2[2] * ski_data.dt;
// 通过累加每一步的位移来计算总距离
float distance_step = sqrtf(
(ski_data.velocity[0] * ski_data.dt) * (ski_data.velocity[0] * ski_data.dt) +
(ski_data.velocity[1] * ski_data.dt) * (ski_data.velocity[1] * ski_data.dt)
);
ski_data.total_horizontal_distance += distance_step;
} else {
// 当静止时，对速度进行衰减
ski_data.velocity[0] *= ski_data.velocity_decay_factor;
ski_data.velocity[1] *= ski_data.velocity_decay_factor;
ski_data.velocity[2] *= ski_data.velocity_decay_factor;
}
// 更新当前水平速度
ski_data.horizontal_speed = sqrtf(ski_data.velocity[0]*ski_data.velocity[0] + ski_data.velocity[1]*ski_data.velocity[1]);
}
// 获取当前速度 (m/s)
float Ski_GetSpeed(void)
{
return ski_data.horizontal_speed;
}
// 获取总移动距离 (m)
float Ski_GetDistance(void)
{
return ski_data.total_horizontal_distance;
}
// 获取当前状态
SkiState_t Ski_GetState(void)
{
return ski_data.current_state;
}
// 重置距离计数
void Ski_ResetDistance(void)
{
ski_data.total_horizontal_distance = 0;
// 速度也应该一起重置
memset(ski_data.velocity, 0, sizeof(ski_data.velocity));
ski_data.horizontal_speed = 0;
}
// 获取校准状态
uint8_t Ski_IsCalibrated(void)
{
return ski_data.calibrated;
}
// 手动设置重力向量
// 这个函数不再需要，将被姿态估计算法取代
/*
void Ski_SetGravityVector(float gx, float gy, float gz)
{
...
}
*/
// 添加调试信息函数
void Ski_PrintDebugInfo(void)
{
printf("State: %d, MoveScore: %.2f, Speed: %.3f, Dist: %.3f, Roll: %.2f, Pitch: %.2f\n",
ski_data.current_state, ski_data.movement_score, ski_data.horizontal_speed, ski_data.total_horizontal_distance,
ski_data.attitude[0], ski_data.attitude[1]);
}
void sensor_processing_task(void){
static int first_init = 0;
if(!first_init){
Ski_Init();
first_init = 1;
}
static signed short acc_data[3], gyr_data[3];
static signed short combined_raw_data[6];
static float final_angle_data[3];
// 读取传感器数据
SL_SC7U22_RawData_Read(acc_data, gyr_data);
memcpy(&combined_raw_data[0], acc_data, 3 * sizeof(signed short));
memcpy(&combined_raw_data[3], gyr_data, 3 * sizeof(signed short));
// 校准逻辑已内置于Ski_UpdateData中，不再需要外部检查
// if(get_calibration_state() == 0){ ... }
// 更新滑雪数据
Ski_UpdateData(combined_raw_data[0], combined_raw_data[1], combined_raw_data[2],
combined_raw_data[3], combined_raw_data[4], combined_raw_data[5]);
static int count = 0;
if(count < 10){
count++;
return;
}else{
count = 0;
}
// 打印结果和调试信息
float speed = Ski_GetSpeed();
float distance = Ski_GetDistance();
SkiState_t state = Ski_GetState();
// printf("Speed: %.3f m/s, Distance: %.3f m, State: %d\n", speed, distance, state);
Ski_PrintDebugInfo();
}

40
apps/earphone/xtell_Sensor/A_hide/4/skiing_tracker.h Normal file
View File

@ -0,0 +1,40 @@
#ifndef SKI_SPEED_DISTANCE_H
#define SKI_SPEED_DISTANCE_H
#include <stdint.h>
// 滑雪状态定义
typedef enum {
SKI_STATE_STATIC = 0, // 静止状态
SKI_STATE_STARTING, // 启动状态
SKI_STATE_MOVING, // 运动状态
SKI_STATE_TURNING, // 转弯状态
SKI_STATE_STOPPING // 停止状态
} SkiState_t;
// 初始化滑雪模块
void Ski_Init(void);
// 更新滑雪数据
void Ski_UpdateData(int16_t acc_x, int16_t acc_y, int16_t acc_z,
int16_t gyro_x, int16_t gyro_y, int16_t gyro_z);
// 获取当前速度 (m/s)
float Ski_GetSpeed(void);
// 获取总移动距离 (m)
float Ski_GetDistance(void);
// 获取当前状态
SkiState_t Ski_GetState(void);
// 重置距离计数
void Ski_ResetDistance(void);
// 获取校准状态
uint8_t Ski_IsCalibrated(void);
// 手动设置重力向量（可选）
void Ski_SetGravityVector(float gx, float gy, float gz);
#endif

311
apps/earphone/xtell_Sensor/A_hide/5/skiing_tracker.c Normal file
View File

@ -0,0 +1,311 @@
/*
动态ZUPT+卡尔曼
多了加速度死区、摩擦力速度衰减、高通滤波
原地摆动产生的速度、距离变化还是没法消除
水平移动、斜坡移动效果貌似还行
*/
#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)
// --- 算法阈值定义 ---
//两个判断是否静止的必要条件
// 动态零速更新(ZUPT)阈值
// 提高阈值，让“刹车”更灵敏，以便在波浪式前进等慢速漂移时也能触发零速更新
#define ZUPT_ACC_VARIANCE_THRESHOLD 0.2f
// 陀螺仪方差阈值
#define ZUPT_GYR_VARIANCE_THRESHOLD 5.0f
// 旋转/摆动检测阈值：角速度总模长大于此值(度/秒)，认为正在进行非滑雪的旋转或摆动 -- 没法完全消除
#define ROTATION_GYR_MAG_THRESHOLD 45.0f
// 启动滑雪阈值：加速度模长与重力的差值大于此值，认为开始运动
// 降低阈值，让“油门”更灵敏，以便能捕捉到真实的慢速启动
#define START_SKIING_ACC_THRESHOLD 0.5f
// --- 用于消除积分漂移的滤波器和阈值 ---
// 高通滤波器系数 (alpha)。alpha 越接近1，滤除低频(直流偏移)的效果越强，但可能滤掉真实的慢速运动。
// alpha = RC / (RC + dt)，
#define HPF_ALPHA 0.95f
// 加速度死区阈值 (m/s^2)。低于此阈值的加速度被认为是噪声，不参与积分。
// 设得太高会忽略真实的慢速启动，设得太低则无法有效抑制噪声。
#define ACC_DEAD_ZONE_THRESHOLD 0.1f
#define SPEED_ATTENUATION 0.98f //模拟摩擦力，进行速度衰减
/**
* @brief 初始化滑雪追踪器
*/
void skiing_tracker_init(skiing_tracker_t *tracker)
{
if (!tracker) {
return;
}
// 使用memset一次性清零整个结构体，包括新增的缓冲区
memset(tracker, 0, sizeof(skiing_tracker_t));
tracker->state = SKIING_STATE_STATIC;
}
/**
* @brief 将设备坐标系下的加速度转换为世界坐标系
* @param acc_device 设备坐标系下的加速度 [x, y, z]
* @param angle 姿态角 [pitch, roll, yaw]，单位: 度
* @param acc_world 输出：世界坐标系下的加速度 [x, y, z]
*/
static void transform_acc_to_world_frame(const float *acc_device, const float *angle, float *acc_world)
{
// 驱动输出的角度与标准航空定义相反，需要取反才能用于标准旋转矩阵。
float pitch = -angle[0] * DEG_TO_RAD;
float roll = -angle[1] * DEG_TO_RAD;
// TODO: 当引入三轴磁力计后，这里的 yaw 应由磁力计和陀螺仪融合解算得出，以解决航向漂移问题。
// 目前 yaw 暂时不参与计算，因为仅靠加速度计和陀螺仪无法获得准确的绝对航向角。
// float yaw = -angle[2] * DEG_TO_RAD;
float cp = cosf(pitch);
float sp = sinf(pitch);
float cr = cosf(roll);
float sr = sinf(roll);
float ax = acc_device[0];
float ay = acc_device[1];
float az = acc_device[2];
// 使用经过验证的、正确的身体坐标系到世界坐标系的旋转矩阵 (基于 Y-X 旋转顺序)
// 这个矩阵将设备测量的加速度(ax, ay, az)正确地转换到世界坐标系(acc_world)。
// 注意：这里没有使用yaw，主要关心的是坡面上的运动，绝对航向暂时不影响速度和距离的计算。
// TODO
acc_world[0] = cp * ax + sp * sr * ay + sp * cr * az;
acc_world[1] = 0 * ax + cr * ay - sr * az;
acc_world[2] = -sp * ax + cp * sr * ay + cp * cr * az;
}
/**
* @brief 计算缓冲区内三轴数据的方差之和
*/
static float calculate_variance(float buffer[VARIANCE_BUFFER_SIZE][3])
{
float mean[3] = {0};
float variance[3] = {0};
// 1. 计算均值
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;
// 2. 计算方差
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 状态机更新
*/
static void update_state_machine(skiing_tracker_t *tracker, const float *acc_device_ms2, const float *gyr_dps)
{
// 缓冲区未填满时，不进行状态判断，默认为静止
if (!tracker->buffer_filled) {
tracker->state = SKIING_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]);
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]);
// --- 状态切换逻辑---
// 原地旋转/摆动检测
// 增加一个关键前提：只在当前不处于滑雪状态时，才检测原地旋转。
// 这可以防止滑雪过程中的高速转弯被误判为原地旋转。
// 暂时没办法完全消除
if (gyr_magnitude > ROTATION_GYR_MAG_THRESHOLD && tracker->state != SKIING_STATE_SKIING) {
tracker->state = SKIING_STATE_ROTATING;
return;
}
// 动态零速更新 (ZUPT)
// 必须同时满足加速度和角速度都稳定，才能判断为“真静止”，以区分匀速运动
if (acc_variance < ZUPT_ACC_VARIANCE_THRESHOLD && gyr_variance < ZUPT_GYR_VARIANCE_THRESHOLD) {
tracker->state = SKIING_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_STATIC || tracker->state == SKIING_STATE_ROTATING) {
// 最终版启动逻辑：必须同时满足“有足够大的线性加速度”和“旋转稳定”两个条件
if (fabsf(acc_magnitude - G_ACCELERATION) > START_SKIING_ACC_THRESHOLD && gyr_variance < ZUPT_GYR_VARIANCE_THRESHOLD) {
tracker->state = SKIING_STATE_SKIING;
return;
}
}
// 滑雪
if (tracker->state != SKIING_STATE_STATIC) {
tracker->state = SKIING_STATE_SKIING;
}
}
/**
* @brief 主更新函数
*/
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;
}
// --- 数据预处理和缓冲 ---
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);
// --- 根据状态进行计算 ---
if (tracker->state == SKIING_STATE_SKIING) {
// 坐标转换 & 移除重力
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->velocity[2] += tracker->acc_world_filtered[2] * dt;
}
// 如果加速度小于阈值，则不更新速度，相当于速度保持不变（或受下一步的阻尼影响而衰减）
} else {
// 在静止或旋转状态下，速度已经在状态机内部被清零
// 额外增加速度衰减，模拟摩擦力，进一步抑制漂移
tracker->velocity[0] *= SPEED_ATTENUATION;
tracker->velocity[1] *= SPEED_ATTENUATION;
tracker->velocity[2] = 0; // 垂直速度强制归零
}
// --- 更新速率和距离 ---
// 只基于水平速度计算速率和距离
tracker->speed = sqrtf(tracker->velocity[0] * tracker->velocity[0] +
tracker->velocity[1] * tracker->velocity[1]);
tracker->distance += tracker->speed * dt;
}
// 传感器数据采集与处理任务
void 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;
static signed short combined_raw_data[6];
static float final_angle_data[3]; // 计算得到的欧若拉角
static float calibrated_acc_g[3]; // 转换后的加速度计数据
static float calibrated_gyr_dps[3]; // 转换后的陀螺仪数据
const float delta_time = 0.01f;
if (!initialized) {
skiing_tracker_init(&my_skiing_tracker);
initialized = 1;
printf("Skiing Tracker Initialized. Waiting for sensor calibration...\n");
}
memcpy(&combined_raw_data[0], acc_data_buf, 3 * sizeof(signed short));
memcpy(&combined_raw_data[3], gyr_data_buf, 3 * sizeof(signed short));
unsigned char status;
if (!calibration_done) { //第1次启动，开启零漂检测
status = 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 {
status = SL_SC7U22_Angle_Output(0, combined_raw_data, final_angle_data, 0);
}
if (status == 1) {
// 加速度 LSB to g
calibrated_acc_g[0] = (float)combined_raw_data[0] / 8192.0f;
calibrated_acc_g[1] = (float)combined_raw_data[1] / 8192.0f;
calibrated_acc_g[2] = (float)combined_raw_data[2] / 8192.0f;
// 陀螺仪 LSB to dps (度/秒)
// ±2000dps量程下，转换系数约为 0.061
calibrated_gyr_dps[0] = (float)combined_raw_data[3] * 0.061f;
calibrated_gyr_dps[1] = (float)combined_raw_data[4] * 0.061f;
calibrated_gyr_dps[2] = (float)combined_raw_data[5] * 0.061f;
skiing_tracker_update(&my_skiing_tracker, calibrated_acc_g, calibrated_gyr_dps, final_angle_data, delta_time);
// 打印逻辑保持不变
static int count = 0;
if(count < 10){
count++;
return;
} else {
count = 0;
}
printf("State: %d, Speed: %.2f m/s, Distance: %.2f m\n",
my_skiing_tracker.state,
my_skiing_tracker.speed,
my_skiing_tracker.distance);
} else if (status == 0) {
// printf("Sensor is calibrating...\n");
} else {
// printf("Angle calculation error or calibration not finished.\n");
}
}

55
apps/earphone/xtell_Sensor/A_hide/5/skiing_tracker.h Normal file
View File

@ -0,0 +1,55 @@
#ifndef SKIING_TRACKER_H
#define SKIING_TRACKER_H
// 定义滑雪者可能的状态
typedef enum {
SKIING_STATE_STATIC, // 静止或动态稳定
SKIING_STATE_SKIING, // 正在滑雪
SKIING_STATE_ROTATING, // 正在原地旋转 (新增)
SKIING_STATE_FALLEN, // 已摔倒
SKIING_STATE_UNKNOWN // 未知状态
} skiing_state_t;
#define VARIANCE_BUFFER_SIZE 15 // 用于计算方差的数据窗口大小 (15个样本 @ 100Hz = 150ms)
// 追踪器数据结构体
typedef struct {
// 公开数据
float velocity[3]; // 当前速度 (x, y, z)，单位: m/s
float distance; // 总滑行距离，单位: m
float speed; // 当前速率 (标量)，单位: m/s
skiing_state_t state; // 当前滑雪状态
// 内部计算使用的私有成员
float acc_world[3]; // 在世界坐标系下的加速度
// --- 内部计算使用的私有成员 ---
// 用于动态零速更新和旋转检测的缓冲区
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];
} skiing_tracker_t;
/**
* @brief 初始化滑雪追踪器
*
* @param tracker 指向 skiing_tracker_t 结构体的指针
*/
void skiing_tracker_init(skiing_tracker_t *tracker);
/**
* @brief 处理传感器数据并更新滑雪状态
*
* @param tracker 指向 skiing_tracker_t 结构体的指针
* @param acc_g 校准后的加速度数据 [x, y, z]，单位: g (1g = 9.8m/s^2)
* @param gyr_dps 角速度
* @param angle 姿态角数据 [pitch, roll, yaw]，单位: 度
* @param dt 采样时间间隔，单位: 秒 (s)
*/
void skiing_tracker_update(skiing_tracker_t *tracker, float *acc_g, float *gyr_dps, float *angle, float dt);
#endif // SKIING_TRACKER_H

374
apps/earphone/xtell_Sensor/A_hide/6/skiing_tracker.c Normal file
View File

@ -0,0 +1,374 @@
/*
动态ZUPT+卡尔曼+巴特沃斯一阶滤波器
针对启动滑雪和停止滑雪，设置不同阈值
启动滑雪和ZUPT更新的陀螺仪方差阈值分开设置
- 启动滑雪的陀螺仪阈值会更宽松一些
原地旋转和ZUPT更新的加速度方差阈值分开设置
- 原地旋转的加速度阈值更宽松
能够从静止状态到变化状态，去根据阈值来判断这个“变化”：进入滑行状态 / 只是原地摆动
- 但是还是不够灵敏
*/
#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)
// --- 算法阈值定义 ---
//两个判断是否静止的必要条件：动态零速更新(ZUPT)阈值
// 加速方差阈值，提高阈值，让“刹车”更灵敏，以便在波浪式前进等慢速漂移时也能触发零速更新
#define ZUPT_ACC_VARIANCE_THRESHOLD 0.2f
// 陀螺仪方差阈值
#define ZUPT_GYR_VARIANCE_THRESHOLD 5.0f
// 用于原地旋转判断的加速度方差阈值。此值比ZUPT阈值更宽松，
// 以允许原地旋转时身体的正常晃动，但仍能与真实滑行时的剧烈加速度变化区分开。
#define ROTATING_ACC_VARIANCE_THRESHOLD 0.8f
// 用于启动滑雪判断的陀螺仪方差阈值。此值比ZUPT阈值更宽松，
// 以允许启动瞬间的正常抖动，但仍能过滤掉混乱的、非滑雪的晃动。
#define SKIING_GYR_VARIANCE_THRESHOLD 15.0f
// 旋转/摆动检测阈值：角速度总模长大于此值(度/秒)，认为正在进行非滑雪的旋转或摆动
#define ROTATION_GYR_MAG_THRESHOLD 45.0f
// 启动滑雪阈值：加速度模长与重力的差值大于此值，认为开始运动
// 降低阈值，让“油门”更灵敏，以便能捕捉到真实的慢速启动
#define START_SKIING_ACC_THRESHOLD 0.5f
// --- 用于消除积分漂移的滤波器和阈值 ---
// 高通滤波器系数 (alpha)。alpha 越接近1，滤除低频(直流偏移)的效果越强，但可能滤掉真实的慢速运动。
// alpha = RC / (RC + dt)，
#define HPF_ALPHA 0.95f // 换算大概就是衰减频率低于约 0.84 Hz 的信号
// 任何比“大约1秒钟变化一次”还要慢的运动，其加速度信号也会被部分衰减。
// 而滑雪时的快速转弯、加减速等动作，其频率远高于 0.84 Hz，它们的信号会被保留下来。
// 加速度死区阈值 (m/s^2)。低于此阈值的加速度被认为是噪声，不参与积分。
// 设得太高会忽略真实的慢速启动，设得太低则无法有效抑制噪声。
#define ACC_DEAD_ZONE_THRESHOLD 0.15f
// --- 模拟摩擦力，进行速度衰减 ---
#define SPEED_ATTENUATION 0.98f
//////////////////////////////////////////////////////////////////////////////////////////////////
//实现
/**
* @brief 初始化滑雪追踪器
*
* @param tracker
*/
void skiing_tracker_init(skiing_tracker_t *tracker)
{
if (!tracker) {
return;
}
// 使用memset一次性清零整个结构体，包括新增的缓冲区
memset(tracker, 0, sizeof(skiing_tracker_t));
tracker->state = SKIING_STATE_STATIC;
}
/**
* @brief 将设备坐标系下的加速度转换为世界坐标系
* @param acc_device 设备坐标系下的加速度 [x, y, z]
* @param angle 姿态角 [pitch, roll, yaw]，单位: 度
* @param acc_world 输出：世界坐标系下的加速度 [x, y, z]
*/
static void transform_acc_to_world_frame(const float *acc_device, const float *angle, float *acc_world)
{
// 驱动输出的角度与标准航空定义相反，需要取反才能用于标准旋转矩阵。
float pitch = -angle[0] * DEG_TO_RAD;
float roll = -angle[1] * DEG_TO_RAD;
// TODO: 当引入三轴磁力计后，这里的 yaw 应由磁力计和陀螺仪融合解算得出，以解决航向漂移问题。
// 目前 yaw 暂时不参与计算，因为仅靠加速度计和陀螺仪无法获得准确的绝对航向角。
// float yaw = -angle[2] * DEG_TO_RAD;
float cp = cosf(pitch);
float sp = sinf(pitch);
float cr = cosf(roll);
float sr = sinf(roll);
float ax = acc_device[0];
float ay = acc_device[1];
float az = acc_device[2];
// 使用经过验证的、正确的身体坐标系到世界坐标系的旋转矩阵 (基于 Y-X 旋转顺序)
// 这个矩阵将设备测量的加速度(ax, ay, az)正确地转换到世界坐标系(acc_world)。
// 注意：这里没有使用yaw，主要关心的是坡面上的运动，绝对航向暂时不影响速度和距离的计算。
// TODO
acc_world[0] = cp * ax + sp * sr * ay + sp * cr * az;
acc_world[1] = 0 * ax + cr * ay - sr * az;
acc_world[2] = -sp * ax + cp * sr * ay + cp * cr * az;
}
/**
* @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
* @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 = SKIING_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]);
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]);
// --- 状态切换逻辑 (按优先级) ---
// 优先级1：动态零速更新 (ZUPT) - 最严格和最优先的“刹车”
if (acc_variance < ZUPT_ACC_VARIANCE_THRESHOLD && gyr_variance < ZUPT_GYR_VARIANCE_THRESHOLD) {
tracker->state = SKIING_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;
}
// 优先级2：原地旋转 - 特殊的、非滑雪的运动状态
// 条件：角速度很大，同时线性加速度的晃动在一个“中等”范围内。
if (gyr_magnitude > ROTATION_GYR_MAG_THRESHOLD && acc_variance < ROTATING_ACC_VARIANCE_THRESHOLD) {
tracker->state = SKIING_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;
return;
}
// 如果不满足任何启动或停止条件，则保持当前状态（滑雪中）
// 如果当前是静止或旋转，但没有满足启动条件，则状态会保持，直到满足ZUPT或旋转条件。
}
/**
* @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;
}
// --- 数据预处理和缓冲 ---
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);
// --- 根据状态进行计算 ---
if (tracker->state == SKIING_STATE_SKIING) {
// 坐标转换 & 移除重力
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->velocity[2] += tracker->acc_world_filtered[2] * dt;
}
// 如果加速度小于阈值，则不更新速度，相当于速度保持不变（或受下一步的阻尼影响而衰减）
} else {
// 在静止或旋转状态下，速度已经在状态机内部被清零
// 额外增加速度衰减，模拟摩擦力，进一步抑制漂移
tracker->velocity[0] *= SPEED_ATTENUATION;
tracker->velocity[1] *= SPEED_ATTENUATION;
tracker->velocity[2] = 0; // 垂直速度强制归零
}
// --- 更新速率和距离 ---
// 只基于水平速度计算速率和距离
tracker->speed = sqrtf(tracker->velocity[0] * tracker->velocity[0] +
tracker->velocity[1] * tracker->velocity[1]);
tracker->distance += tracker->speed * dt;
}
/**
* @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) {
static skiing_tracker_t my_skiing_tracker;
static int initialized = 0;
static int calibration_done = 0;
static signed short combined_raw_data[6];
static float final_angle_data[3]; // 计算得到的欧若拉角
static float calibrated_acc_g[3]; // 转换后的加速度计数据
static float calibrated_gyr_dps[3]; // 转换后的陀螺仪数据
const float delta_time = DELTA_TIME;
BLE_send_data_t BLE_send_data;
BLE_KS_send_data_t KS_data;
if (!initialized) {
skiing_tracker_init(&my_skiing_tracker);
initialized = 1;
printf("Skiing Tracker Initialized. Waiting for sensor calibration...\n");
}
memcpy(&combined_raw_data[0], acc_data_buf, 3 * sizeof(signed short));
memcpy(&combined_raw_data[3], gyr_data_buf, 3 * sizeof(signed short));
unsigned char status;
if (!calibration_done) { //第1次启动，开启零漂检测
status = 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 {
status = SL_SC7U22_Angle_Output(0, combined_raw_data, final_angle_data, 0);
}
if (status == 1) {
// 加速度 LSB to g
calibrated_acc_g[0] = (float)combined_raw_data[0] / 8192.0f;
calibrated_acc_g[1] = (float)combined_raw_data[1] / 8192.0f;
calibrated_acc_g[2] = (float)combined_raw_data[2] / 8192.0f;
// 陀螺仪 LSB to dps (度/秒)
// ±2000dps量程下，转换系数约为 0.061
calibrated_gyr_dps[0] = (float)combined_raw_data[3] * 0.061f;
calibrated_gyr_dps[1] = (float)combined_raw_data[4] * 0.061f;
calibrated_gyr_dps[2] = (float)combined_raw_data[5] * 0.061f;
skiing_tracker_update(&my_skiing_tracker, calibrated_acc_g, calibrated_gyr_dps, final_angle_data, delta_time);
// 打印逻辑保持不变
// static int count = 0;
// if(count >= 10){
// printf("State: %d, Speed: %.2f m/s, Distance: %.2f m\n",
// my_skiing_tracker.state,
// my_skiing_tracker.speed,
// my_skiing_tracker.distance);
// count = 0;
// } else {
// count++;
// }
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
BLE_send_data.acc_original[i] = (int)acc_data_buf[i];
BLE_send_data.gyr_original[i] = (int)gyr_data_buf[i];
#endif
#if KS_BLE
KS_data.acc_KS_g[i] = (int)calibrated_acc_g[i];
KS_data.gyr_KS_dps[i] = (int)calibrated_gyr_dps[i];
KS_data.angle_KS[i] = (int)final_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);
} else if (status == 0) {
memset(&BLE_send_data, 0, sizeof(BLE_send_data_t));
BLE_send_data.sensor_state = status;
#if KS_BLE
memset(&KS_data, 0, sizeof(BLE_send_data_t));
#endif
// printf("Sensor is calibrating...\n");
} else {
memset(&BLE_send_data, 0, sizeof(BLE_send_data_t));
BLE_send_data.sensor_state = status;
#if KS_BLE
memset(&KS_data, 0, sizeof(BLE_send_data_t));
#endif
// printf("Angle calculation error or calibration not finished.\n");
}
return BLE_send_data;
}

74
apps/earphone/xtell_Sensor/A_hide/6/skiing_tracker.h Normal file
View File

@ -0,0 +1,74 @@
#ifndef SKIING_TRACKER_H
#define SKIING_TRACKER_H
#include "../xtell.h"
// 定义滑雪者可能的状态
typedef enum {
SKIING_STATE_STATIC, // 静止或动态稳定
SKIING_STATE_SKIING, // 正在滑雪
SKIING_STATE_ROTATING, // 正在原地旋转 (新增)
SKIING_STATE_FALLEN, // 已摔倒
SKIING_STATE_UNKNOWN // 未知状态
} 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_world[3]; // 在世界坐标系下的加速度
// --- 内部计算使用的私有成员 ---
// 用于动态零速更新和旋转检测的缓冲区
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]; //上一次没过滤的
} skiing_tracker_t;
//ble发送的数据
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
}BLE_send_data_t;
typedef struct{
int acc_KS_g[3]; //卡尔曼后，LSB to g 三轴加速度数据
int gyr_KS_dps[3]; //卡尔曼后，LSB to dps 三轴陀螺仪数据
int angle_KS[3]; //卡尔曼后，计算得到的欧若拉角数据
}BLE_KS_send_data_t;
/**
* @brief 初始化滑雪追踪器
*
* @param tracker 指向 skiing_tracker_t 结构体的指针
*/
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) ;
#endif // SKIING_TRACKER_H

117
apps/earphone/xtell_Sensor/buffer/circle_buffer.c Normal file
View File

@ -0,0 +1,117 @@
#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;
cb->capacity = capacity;
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);
}
if (length == 0) {
return 0;
}
// 检查是否需要回环
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;
}
}
cb->size += length;
return length;
}
// 从环形缓冲区读取数据
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;
}

55
apps/earphone/xtell_Sensor/buffer/circle_buffer.h Normal file
View File

@ -0,0 +1,55 @@
#ifndef CIRCLE_BUFFER_H
#define CIRCLE_BUFFER_H
#include "system/includes.h"
// 定义环形缓冲区的结构体
typedef struct {
u8 *buffer; // 缓冲区指针
u16 capacity; // 缓冲区总容量
u16 head; // 头部指针（写入位置）
u16 tail; // 尾部指针（读取位置）
u16 size; // 当前已用大小
} circle_buffer_t;
/**
* @brief 初始化环形缓冲区
* @param cb 指向环形缓冲区结构体的指针
* @param buffer 外部提供的缓冲区内存
* @param capacity 缓冲区的总容量
*/
void circle_buffer_init(circle_buffer_t *cb, u8 *buffer, u16 capacity);
/**
* @brief 向环形缓冲区写入数据
* @param cb 指向环形缓冲区结构体的指针
* @param data 要写入的数据的指针
* @param length 要写入的数据的长度
* @return 实际写入的字节数
*/
u16 circle_buffer_write(circle_buffer_t *cb, const u8 *data, u16 length);
/**
* @brief 从环形缓冲区读取数据
* @param cb 指向环形缓冲区结构体的指针
* @param data 用于存放读取数据的缓冲区的指针
* @param length 想要读取的数据的长度
* @return 实际读取的字节数
*/
u16 circle_buffer_read(circle_buffer_t *cb, u8 *data, u16 length);
/**
* @brief 获取环形缓冲区中已用空间的大小
* @param cb 指向环形缓冲区结构体的指针
* @return 已用空间的大小
*/
u16 circle_buffer_get_size(circle_buffer_t *cb);
/**
* @brief 获取环形缓冲区中剩余空间的大小
* @param cb 指向环形缓冲区结构体的指针
* @return 剩余空间的大小
*/
u16 circle_buffer_get_free_space(circle_buffer_t *cb);
#endif // CIRCLE_BUFFER_H

377
apps/earphone/xtell_Sensor/calculate/skiing_tracker.c Normal file
View File

@ -0,0 +1,377 @@
/*
动态ZUPT+卡尔曼+巴特沃斯一阶滤波器
针对启动滑雪和停止滑雪，设置不同阈值
启动滑雪和ZUPT更新的陀螺仪方差阈值分开设置
- 启动滑雪的陀螺仪阈值会更宽松一些
原地旋转和ZUPT更新的加速度方差阈值分开设置
- 原地旋转的加速度阈值更宽松
能够从静止状态到变化状态，去根据阈值来判断这个“变化”：进入滑行状态 / 只是原地摆动
- 但是还是不够灵敏
*/
#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)
// --- 算法阈值定义 ---
//两个判断是否静止的必要条件：动态零速更新(ZUPT)阈值
// 加速方差阈值，提高阈值，让“刹车”更灵敏，以便在波浪式前进等慢速漂移时也能触发零速更新
#define ZUPT_ACC_VARIANCE_THRESHOLD 0.2f
// 陀螺仪方差阈值
#define ZUPT_GYR_VARIANCE_THRESHOLD 5.0f
// 用于原地旋转判断的加速度方差阈值。此值比ZUPT阈值更宽松，
// 以允许原地旋转时身体的正常晃动，但仍能与真实滑行时的剧烈加速度变化区分开。
#define ROTATING_ACC_VARIANCE_THRESHOLD 0.8f
// 用于启动滑雪判断的陀螺仪方差阈值。此值比ZUPT阈值更宽松，
// 以允许启动瞬间的正常抖动，但仍能过滤掉混乱的、非滑雪的晃动。
#define SKIING_GYR_VARIANCE_THRESHOLD 15.0f
// 旋转/摆动检测阈值：角速度总模长大于此值(度/秒)，认为正在进行非滑雪的旋转或摆动
#define ROTATION_GYR_MAG_THRESHOLD 90.0f //测试记录：45.0f、90.0f
// 启动滑雪阈值：加速度模长与重力的差值大于此值，认为开始运动
// 降低阈值，让“油门”更灵敏，以便能捕捉到真实的慢速启动
#define START_SKIING_ACC_THRESHOLD 0.5f
// --- 用于消除积分漂移的滤波器和阈值 ---
// 高通滤波器系数 (alpha)。alpha 越接近1，滤除低频(直流偏移)的效果越强，但可能滤掉真实的慢速运动。
// alpha = RC / (RC + dt)，
#define HPF_ALPHA 0.95f // 换算大概就是衰减频率低于约 0.84 Hz 的信号
// 任何比“大约1秒钟变化一次”还要慢的运动，其加速度信号也会被部分衰减。
// 而滑雪时的快速转弯、加减速等动作，其频率远高于 0.84 Hz，它们的信号会被保留下来。
// 加速度死区阈值 (m/s^2)。低于此阈值的加速度被认为是噪声，不参与积分。
// 设得太高会忽略真实的慢速启动，设得太低则无法有效抑制噪声。
#define ACC_DEAD_ZONE_THRESHOLD 0.15f
// --- 模拟摩擦力，进行速度衰减 ---
#define SPEED_ATTENUATION 0.98f
//////////////////////////////////////////////////////////////////////////////////////////////////
//实现
/**
* @brief 初始化滑雪追踪器
*
* @param tracker
*/
void skiing_tracker_init(skiing_tracker_t *tracker)
{
if (!tracker) {
return;
}
// 使用memset一次性清零整个结构体，包括新增的缓冲区
memset(tracker, 0, sizeof(skiing_tracker_t));
tracker->state = SKIING_STATE_STATIC;
}
/**
* @brief 将设备坐标系下的加速度转换为世界坐标系
* @param acc_device 设备坐标系下的加速度 [x, y, z]
* @param angle 姿态角 [pitch, roll, yaw]，单位: 度
* @param acc_world 输出：世界坐标系下的加速度 [x, y, z]
*/
static void transform_acc_to_world_frame(const float *acc_device, const float *angle, float *acc_world)
{
// 驱动输出的角度与标准航空定义相反，需要取反才能用于标准旋转矩阵。
float pitch = -angle[0] * DEG_TO_RAD;
float roll = -angle[1] * DEG_TO_RAD;
// TODO: 当引入三轴磁力计后，这里的 yaw 应由磁力计和陀螺仪融合解算得出，以解决航向漂移问题。
// 目前 yaw 暂时不参与计算，因为仅靠加速度计和陀螺仪无法获得准确的绝对航向角。
// float yaw = -angle[2] * DEG_TO_RAD;
float cp = cosf(pitch);
float sp = sinf(pitch);
float cr = cosf(roll);
float sr = sinf(roll);
float ax = acc_device[0];
float ay = acc_device[1];
float az = acc_device[2];
// 使用经过验证的、正确的身体坐标系到世界坐标系的旋转矩阵 (基于 Y-X 旋转顺序)
// 这个矩阵将设备测量的加速度(ax, ay, az)正确地转换到世界坐标系(acc_world)。
// 注意：这里没有使用yaw，主要关心的是坡面上的运动，绝对航向暂时不影响速度和距离的计算。
// TODO
acc_world[0] = cp * ax + sp * sr * ay + sp * cr * az;
acc_world[1] = 0 * ax + cr * ay - sr * az;
acc_world[2] = -sp * ax + cp * sr * ay + cp * cr * az;
}
/**
* @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
* @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 = SKIING_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]);
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]);
// --- 状态切换逻辑 (按优先级) ---
// 优先级1：动态零速更新 (ZUPT) - 最严格和最优先的“刹车”
if (acc_variance < ZUPT_ACC_VARIANCE_THRESHOLD && gyr_variance < ZUPT_GYR_VARIANCE_THRESHOLD) {
tracker->state = SKIING_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;
}
// 优先级2：原地旋转 - 特殊的、非滑雪的运动状态
// 条件：角速度很大，同时线性加速度的晃动在一个“中等”范围内。
if (gyr_magnitude > ROTATION_GYR_MAG_THRESHOLD && acc_variance < ROTATING_ACC_VARIANCE_THRESHOLD) {
tracker->state = SKIING_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;
return;
}
// 如果不满足任何启动或停止条件，则保持当前状态（滑雪中）
// 如果当前是静止或旋转，但没有满足启动条件，则状态会保持，直到满足ZUPT或旋转条件。
}
/**
* @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;
}
// --- 数据预处理和缓冲 ---
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);
// --- 根据状态进行计算 ---
if (tracker->state == SKIING_STATE_SKIING) {
// 坐标转换 & 移除重力
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->velocity[2] += tracker->acc_world_filtered[2] * dt;
}
// 如果加速度小于阈值，则不更新速度，相当于速度保持不变（或受下一步的阻尼影响而衰减）
} else {
// 在静止或旋转状态下，速度已经在状态机内部被清零
// 额外增加速度衰减，模拟摩擦力，进一步抑制漂移
tracker->velocity[0] *= SPEED_ATTENUATION;
tracker->velocity[1] *= SPEED_ATTENUATION;
tracker->velocity[2] = 0; // 垂直速度强制归零
}
// --- 更新速率和距离 ---
// 只基于水平速度计算速率和距离
tracker->speed = sqrtf(tracker->velocity[0] * tracker->velocity[0] +
tracker->velocity[1] * tracker->velocity[1]);
tracker->distance += tracker->speed * dt;
}
/**
* @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) {
static skiing_tracker_t my_skiing_tracker;
static int initialized = 0;
static int calibration_done = 0;
static signed short combined_raw_data[6];
static float final_angle_data[3]; // 计算得到的欧若拉角
static float calibrated_acc_g[3]; // 转换后的加速度计数据
static float calibrated_gyr_dps[3]; // 转换后的陀螺仪数据
const float delta_time = DELTA_TIME;
BLE_send_data_t BLE_send_data;
BLE_KS_send_data_t KS_data;
if (!initialized) {
skiing_tracker_init(&my_skiing_tracker);
initialized = 1;
printf("Skiing Tracker Initialized. Waiting for sensor calibration...\n");
}
memcpy(&combined_raw_data[0], acc_data_buf, 3 * sizeof(signed short));
memcpy(&combined_raw_data[3], gyr_data_buf, 3 * sizeof(signed short));
unsigned char status;
if (!calibration_done) { //第1次启动，开启零漂检测
status = 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);
}
if (status == 1) {
// 加速度 LSB to g
calibrated_acc_g[0] = (float)combined_raw_data[0] / 8192.0f;
calibrated_acc_g[1] = (float)combined_raw_data[1] / 8192.0f;
calibrated_acc_g[2] = (float)combined_raw_data[2] / 8192.0f;
// 陀螺仪 LSB to dps (度/秒)
// ±2000dps量程下，转换系数约为 0.061
calibrated_gyr_dps[0] = (float)combined_raw_data[3] * 0.061f;
calibrated_gyr_dps[1] = (float)combined_raw_data[4] * 0.061f;
calibrated_gyr_dps[2] = (float)combined_raw_data[5] * 0.061f;
skiing_tracker_update(&my_skiing_tracker, calibrated_acc_g, calibrated_gyr_dps, final_angle_data, delta_time);
// 打印逻辑保持不变
// static int count = 0;
// if(count >= 10){
// printf("State: %d, Speed: %.2f m/s, Distance: %.2f m\n",
// my_skiing_tracker.state,
// my_skiing_tracker.speed,
// my_skiing_tracker.distance);
// count = 0;
// } else {
// count++;
// }
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
BLE_send_data.acc_original[i] = (int)acc_data_buf[i];
BLE_send_data.gyr_original[i] = (int)gyr_data_buf[i];
#endif
#if KS_BLE
KS_data.acc_KS_g[i] = (int)calibrated_acc_g[i];
KS_data.gyr_KS_dps[i] = (int)calibrated_gyr_dps[i];
KS_data.angle_KS[i] = (int)final_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");
} else if (status == 0) {
memset(&BLE_send_data, 0, sizeof(BLE_send_data_t));
BLE_send_data.sensor_state = status;
#if KS_BLE
memset(&KS_data, 0, sizeof(BLE_send_data_t));
#endif
// printf("Sensor is calibrating...\n");
} else {
memset(&BLE_send_data, 0, sizeof(BLE_send_data_t));
BLE_send_data.sensor_state = status;
#if KS_BLE
memset(&KS_data, 0, sizeof(BLE_send_data_t));
#endif
// printf("Angle calculation error or calibration not finished.\n");
}
return BLE_send_data;
}

74
apps/earphone/xtell_Sensor/calculate/skiing_tracker.h Normal file
View File

@ -0,0 +1,74 @@
#ifndef SKIING_TRACKER_H
#define SKIING_TRACKER_H
#include "../xtell.h"
// 定义滑雪者可能的状态
typedef enum {
SKIING_STATE_STATIC, // 静止或动态稳定
SKIING_STATE_SKIING, // 正在滑雪
SKIING_STATE_ROTATING, // 正在原地旋转 (新增)
SKIING_STATE_FALLEN, // 已摔倒
SKIING_STATE_UNKNOWN // 未知状态
} 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_world[3]; // 在世界坐标系下的加速度
// --- 内部计算使用的私有成员 ---
// 用于动态零速更新和旋转检测的缓冲区
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]; //上一次没过滤的
} skiing_tracker_t;
//ble发送的数据
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
}BLE_send_data_t;
typedef struct{
int acc_KS_g[3]; //卡尔曼后，LSB to g 三轴加速度数据
int gyr_KS_dps[3]; //卡尔曼后，LSB to dps 三轴陀螺仪数据
int angle_KS[3]; //卡尔曼后，计算得到的欧若拉角数据
}BLE_KS_send_data_t;
/**
* @brief 初始化滑雪追踪器
*
* @param tracker 指向 skiing_tracker_t 结构体的指针
*/
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) ;
#endif // SKIING_TRACKER_H

312
apps/earphone/xtell_Sensor/calculate/tmp.c Normal file
View File

@ -0,0 +1,312 @@
/*
动态ZUPT+卡尔曼
多了加速度死区、摩擦力速度衰减、高通滤波
原地摆动产生的速度、距离变化还是没法消除
水平移动、斜坡移动效果貌似还行
*/
#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)
// --- 算法阈值定义 ---
//两个判断是否静止的必要条件
// 动态零速更新(ZUPT)阈值
// 提高阈值，让“刹车”更灵敏，以便在波浪式前进等慢速漂移时也能触发零速更新
#define ZUPT_ACC_VARIANCE_THRESHOLD 0.2f
// 陀螺仪方差阈值
#define ZUPT_GYR_VARIANCE_THRESHOLD 5.0f
// 旋转/摆动检测阈值：角速度总模长大于此值(度/秒)，认为正在进行非滑雪的旋转或摆动 -- 没法完全消除
#define ROTATION_GYR_MAG_THRESHOLD 45.0f
// 启动滑雪阈值：加速度模长与重力的差值大于此值，认为开始运动
// 降低阈值，让“油门”更灵敏，以便能捕捉到真实的慢速启动
#define START_SKIING_ACC_THRESHOLD 0.5f
// --- 用于消除积分漂移的滤波器和阈值 ---
// 高通滤波器系数 (alpha)。alpha 越接近1，滤除低频(直流偏移)的效果越强，但可能滤掉真实的慢速运动。
// alpha = RC / (RC + dt)，
#define HPF_ALPHA 0.95f
// 加速度死区阈值 (m/s^2)。低于此阈值的加速度被认为是噪声，不参与积分。
// 设得太高会忽略真实的慢速启动，设得太低则无法有效抑制噪声。
#define ACC_DEAD_ZONE_THRESHOLD 0.1f
// --- 模拟摩擦力，进行速度衰减 ---
#define SPEED_ATTENUATION 0.98f
/**
* @brief 初始化滑雪追踪器
*/
void skiing_tracker_init(skiing_tracker_t *tracker)
{
if (!tracker) {
return;
}
// 使用memset一次性清零整个结构体，包括新增的缓冲区
memset(tracker, 0, sizeof(skiing_tracker_t));
tracker->state = SKIING_STATE_STATIC;
}
/**
* @brief 将设备坐标系下的加速度转换为世界坐标系
* @param acc_device 设备坐标系下的加速度 [x, y, z]
* @param angle 姿态角 [pitch, roll, yaw]，单位: 度
* @param acc_world 输出：世界坐标系下的加速度 [x, y, z]
*/
static void transform_acc_to_world_frame(const float *acc_device, const float *angle, float *acc_world)
{
// 驱动输出的角度与标准航空定义相反，需要取反才能用于标准旋转矩阵。
float pitch = -angle[0] * DEG_TO_RAD;
float roll = -angle[1] * DEG_TO_RAD;
// TODO: 当引入三轴磁力计后，这里的 yaw 应由磁力计和陀螺仪融合解算得出，以解决航向漂移问题。
// 目前 yaw 暂时不参与计算，因为仅靠加速度计和陀螺仪无法获得准确的绝对航向角。
// float yaw = -angle[2] * DEG_TO_RAD;
float cp = cosf(pitch);
float sp = sinf(pitch);
float cr = cosf(roll);
float sr = sinf(roll);
float ax = acc_device[0];
float ay = acc_device[1];
float az = acc_device[2];
// 使用经过验证的、正确的身体坐标系到世界坐标系的旋转矩阵 (基于 Y-X 旋转顺序)
// 这个矩阵将设备测量的加速度(ax, ay, az)正确地转换到世界坐标系(acc_world)。
// 注意：这里没有使用yaw，主要关心的是坡面上的运动，绝对航向暂时不影响速度和距离的计算。
// TODO
acc_world[0] = cp * ax + sp * sr * ay + sp * cr * az;
acc_world[1] = 0 * ax + cr * ay - sr * az;
acc_world[2] = -sp * ax + cp * sr * ay + cp * cr * az;
}
/**
* @brief 计算缓冲区内三轴数据的方差之和
*/
static float calculate_variance(float buffer[VARIANCE_BUFFER_SIZE][3])
{
float mean[3] = {0};
float variance[3] = {0};
// 1. 计算均值
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;
// 2. 计算方差
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 状态机更新
*/
static void update_state_machine(skiing_tracker_t *tracker, const float *acc_device_ms2, const float *gyr_dps)
{
// 缓冲区未填满时，不进行状态判断，默认为静止
if (!tracker->buffer_filled) {
tracker->state = SKIING_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]);
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]);
// --- 状态切换逻辑---
// 原地旋转/摆动检测
// 增加一个关键前提：只在当前不处于滑雪状态时，才检测原地旋转。
// 这可以防止滑雪过程中的高速转弯被误判为原地旋转。
// 暂时没办法完全消除
if (gyr_magnitude > ROTATION_GYR_MAG_THRESHOLD && tracker->state != SKIING_STATE_SKIING) {
tracker->state = SKIING_STATE_ROTATING;
return;
}
// 动态零速更新 (ZUPT)
// 必须同时满足加速度和角速度都稳定，才能判断为“真静止”，以区分匀速运动
if (acc_variance < ZUPT_ACC_VARIANCE_THRESHOLD && gyr_variance < ZUPT_GYR_VARIANCE_THRESHOLD) {
tracker->state = SKIING_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_STATIC || tracker->state == SKIING_STATE_ROTATING) {
// 最终版启动逻辑：必须同时满足“有足够大的线性加速度”和“旋转不剧烈”两个条件
// 新增 gyr_magnitude 判断，防止原地旋转产生的离心加速度被误判为启动
if (fabsf(acc_magnitude - G_ACCELERATION) > START_SKIING_ACC_THRESHOLD &&
gyr_variance < ZUPT_GYR_VARIANCE_THRESHOLD &&
gyr_magnitude < ROTATION_GYR_MAG_THRESHOLD) {
tracker->state = SKIING_STATE_SKIING;
return;
}
}
// 最后的 fall-through 逻辑已移除，以修复原地旋转被错误判断为滑雪的bug。
// 如果不满足任何状态切换条件，状态将保持不变，直到ZUPT或启动条件被满足。
}
/**
* @brief 主更新函数
*/
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;
}
// --- 数据预处理和缓冲 ---
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);
// --- 根据状态进行计算 ---
if (tracker->state == SKIING_STATE_SKIING) {
// 坐标转换 & 移除重力
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->velocity[2] += tracker->acc_world_filtered[2] * dt;
}
// 如果加速度小于阈值，则不更新速度，相当于速度保持不变（或受下一步的阻尼影响而衰减）
} else {
// 在静止或旋转状态下，速度已经在状态机内部被清零
// 额外增加速度衰减，模拟摩擦力，进一步抑制漂移
tracker->velocity[0] *= SPEED_ATTENUATION;
tracker->velocity[1] *= SPEED_ATTENUATION;
tracker->velocity[2] = 0; // 垂直速度强制归零
}
// --- 更新速率和距离 ---
// 只基于水平速度计算速率和距离
tracker->speed = sqrtf(tracker->velocity[0] * tracker->velocity[0] +
tracker->velocity[1] * tracker->velocity[1]);
tracker->distance += tracker->speed * dt;
}
// 传感器数据采集与处理任务
void 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;
static signed short combined_raw_data[6];
static float final_angle_data[3]; // 计算得到的欧若拉角
static float calibrated_acc_g[3]; // 转换后的加速度计数据
static float calibrated_gyr_dps[3]; // 转换后的陀螺仪数据
const float delta_time = 0.01f;
if (!initialized) {
skiing_tracker_init(&my_skiing_tracker);
initialized = 1;
printf("Skiing Tracker Initialized. Waiting for sensor calibration...\n");
}
memcpy(&combined_raw_data[0], acc_data_buf, 3 * sizeof(signed short));
memcpy(&combined_raw_data[3], gyr_data_buf, 3 * sizeof(signed short));
unsigned char status;
if (!calibration_done) { //第1次启动，开启零漂检测
status = 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 {
status = SL_SC7U22_Angle_Output(0, combined_raw_data, final_angle_data, 0);
}
if (status == 1) {
// 加速度 LSB to g
calibrated_acc_g[0] = (float)combined_raw_data[0] / 8192.0f;
calibrated_acc_g[1] = (float)combined_raw_data[1] / 8192.0f;
calibrated_acc_g[2] = (float)combined_raw_data[2] / 8192.0f;
// 陀螺仪 LSB to dps (度/秒)
// ±2000dps量程下，转换系数约为 0.061
calibrated_gyr_dps[0] = (float)combined_raw_data[3] * 0.061f;
calibrated_gyr_dps[1] = (float)combined_raw_data[4] * 0.061f;
calibrated_gyr_dps[2] = (float)combined_raw_data[5] * 0.061f;
skiing_tracker_update(&my_skiing_tracker, calibrated_acc_g, calibrated_gyr_dps, final_angle_data, delta_time);
// 打印逻辑保持不变
static int count = 0;
if(count < 10){
count++;
return;
} else {
count = 0;
}
printf("State: %d, Speed: %.2f m/s, Distance: %.2f m\n",
my_skiing_tracker.state,
my_skiing_tracker.speed,
my_skiing_tracker.distance);
} else if (status == 0) {
// printf("Sensor is calibrating...\n");
} else {
// printf("Angle calculation error or calibration not finished.\n");
}
}

37
apps/earphone/xtell_Sensor/example/example.c Normal file
View File

@ -0,0 +1,37 @@
//////////////////////////////////////////////////////////////////////////////////////////////////
//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 -- 函数定义
//////////////////////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////////////////////
//实现

235
apps/earphone/xtell_Sensor/send_data.c Normal file
View File

@ -0,0 +1,235 @@
#include "system/includes.h"
#include "btstack/btstack_task.h"
#include "app_config.h"
#include "app_action.h"
#include "asm/pwm_led.h"
#include "tone_player.h"
#include "ui_manage.h"
#include "gpio.h"
#include "app_main.h"
#include "asm/charge.h"
#include "update.h"
#include "app_power_manage.h"
#include "audio_config.h"
#include "app_charge.h"
#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 "btstack/avctp_user.h"
#include "calculate/skiing_tracker.h"
#include "xtell.h"
///////////////////////////////////////////////////////////////////////////////////////////////////
//宏定义
#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
//
///////////////////////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////////////////////
//START -- 函数定义
void send_data_to_ble_client(const u8* data, u16 length);
extern void create_process(u16* pid, const char* name, void *priv, void (*func)(void *priv), u32 msec);
//END -- 函数定义
//////////////////////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////////////////////
//START -- 变量定义
// --- 任务ID ---
static u16 xtell_i2c_test_id;
static u16 collect_data_id;
static u16 send_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;
u16 test_id=0;
//END -- 变量定义
//////////////////////////////////////////////////////////////////////////////////////////////////
// /**
// * @brief 向匿名上位机发送数据帧
// * @param function_id 功能码 (例如 0x01, 0x03)
// * @param data 指向 int16_t 数据数组的指针 (例如加速度、欧拉角)
// * @param data_len int16_t 数据的个数 (例如发送6轴数据时为6，发送3个欧拉角时为3)
// * @param status_byte 附加的状态字节 (例如 SHOCK_STA 或 FUSION_STA)
// */
// void send_data_anotc(uint8_t function_id, int16_t* data, uint8_t data_len, uint8_t status_byte) {
// // 定义一个足够大的缓冲区来构建数据帧
// // 最大长度(ID 0x01): 1(HEAD)+1(D_ADDR)+1(ID)+1(LEN)+13(DATA)+1(SC)+1(AC) = 19字节
// uint8_t buffer[32];
// uint8_t data_payload_len = data_len * sizeof(int16_t) + sizeof(uint8_t);
// // 1. 填充帧头和地址
// buffer[0] = 0xAA; // 帧头 HEAD
// buffer[1] = 0xFF; // 目标地址 D_ADDR
// // 2. 填充功能码和数据长度
// buffer[2] = function_id;
// buffer[3] = data_payload_len;
// // 3. 填充数据内容 (DATA)
// // 首先使用 memcpy 拷贝主要的 int16_t 数组数据
// // &buffer[4] 是数据区的起始地址
// memcpy(&buffer[4], (uint8_t*)data, data_len * sizeof(int16_t));
// // 然后在数据区末尾填充状态字节
// buffer[4 + data_len * sizeof(int16_t)] = status_byte;
// // 4. 计算校验和 (SC 和 AC)
// uint8_t sum_check = 0;
// uint8_t add_check = 0;
// // SC: 和校验 (从帧头到数据区最后一个字节)
// for (int i = 0; i < 4 + data_payload_len; ++i) {
// sum_check += buffer[i];
// }
// // 将SC填充到缓冲区
// buffer[4 + data_payload_len] = sum_check;
// // AC: 附加校验 (从帧头到SC)
// for (int i = 0; i < 4 + data_payload_len + 1; ++i) {
// add_check += buffer[i];
// }
// // 将AC填充到缓冲区
// buffer[4 + data_payload_len + 1] = add_check;
// // 5. 发送整个数据帧
// uint16_t frame_length = 4 + data_payload_len + 2;
// // Serial_Send_Buffer(buffer, frame_length);
// for (int i = 0; i < frame_length; ++i) {
// // 使用 %c 格式化字符来发送单个字节的原始值
// printf("%c", buffer[i]);
// }
// printf("\n");
// }
void ble_send_data(signed short *acc_gyro_input, float *Angle_output){
char buffer[50]; //一次最多发送50字节
u8 len = 0;
//AA FF 01 六轴数据 EE
//TO DO
send_data_to_ble_client(&buffer,len);
//AA FF 02 欧若拉角数据 EE
// TO DO
send_data_to_ble_client(&buffer,len);
}
// 从环形缓冲区读取数据并发送
void send_sensor_data_task(void) {
// printf("xtell_ble_send\n");
}
void test(){
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));
static int count = 0;
if(count >=10){
count = 0;
#ifdef XTELL_TEST
xlog("BLE_send_data_t:%d\n",sizeof(BLE_send_data_t));
xlog("ACC_X:%d, ACC_Y:%d, ACC_Z:%d, GYR_X:%.d, GYR_Y:%d, GYR_Z:%d",
acc_data_buf[0],acc_data_buf[1],acc_data_buf[2],gyr_data_buf[0],gyr_data_buf[1],gyr_data_buf[2]
);
printf("State: %d, Speed: %d cm/s, Distance: %d cm\n",
BLE_send_data.skiing_state,
BLE_send_data.speed_cms,
BLE_send_data.distance_cm);
char log_buffer[100]; // 100个字符应该足够了
// 使用 snprintf 进行格式化
int num_chars_written = snprintf(
log_buffer, // 目标缓冲区
sizeof(log_buffer), // 目标缓冲区的最大容量
"State: %d, Speed: %d cm/s, Distance: %d cm\n", // 格式化字符串
BLE_send_data.skiing_state, // 第一个 %d 的参数
BLE_send_data.speed_cms, // 第二个 %d 的参数
BLE_send_data.distance_cm // 第三个 %d 的参数
);
send_data_to_ble_client(&log_buffer,strlen(log_buffer));
// xlog("Pitch:%.2f, Roll:%.2f, Yaw:%.2f\n",
// Angle_output[0],Angle_output[1],Angle_output[2]
// );
#else
send_data_to_ble_client(&data,sizeof(BLE_send_data_t)+4);
#endif
}
count++;
memset(&BLE_send_data, 0, sizeof(BLE_send_data_t));
memset(&data, 0, 50);
}
void gsensor_test(){
sys_timer_del(gsensor_id);
}
void xtell_task_create(void){
// int ret = hw_iic_init(0);
// xlog("hw_iic_init result:%d\n",ret);
// //初始化传感器
// SL_SC7U22_Config();
soft_iic_init(0);
gpio_set_direction(IO_PORTE_05,0); //设置PE5 输出模式
gpio_set_pull_up(IO_PORTE_05,1);
gpio_direction_output(IO_PORTE_05,1);
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);
//初始化滑雪追踪器
// 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));
}

283
apps/earphone/xtell_Sensor/sensor/LIS2DH12.c Normal file
View File

@ -0,0 +1,283 @@
// LIS2DH12驱动 - 由Kilo Code注释
#include "gSensor/gSensor_manage.h"
#include "app_config.h"
#include "math.h"
#include "LIS2DH12.h"
#include "colorful_lights/colorful_lights.h"
#include <string.h> // 用于 memcpy
//////////////////////////////////////////////////////////////////////////////////////////////////
//START -- 宏定义
#define ENABLE_XLOG 1
#ifdef xlog
#undef xlog
#endif
#if ENABLE_XLOG
#define xlog(format, ...) printf("[%s] " format, __func__, ##__VA_ARGS__)
#else
#define xlog(format, ...) ((void)0)
#endif
// --- 运动检测核心参数 ---
#define SAMPLE_COUNT 6 // 定义静止状态检测所需的样本数量
#define THRESHOLD 50.00f // 定义静止状态检测的阈值（三轴数据方差），值越大，对微小抖动的容忍度越高
#define LPF_ALPHA 0.95f // 低通滤波系数，越接近1，滤波效果越强，重力估算越平滑
#define DEADZONE_MSS 0.2f // 加速度死区阈值 (m/s^2)，低于此值的线性加速度被视为噪声并忽略
// --- 原有业务逻辑宏定义 ---
#define STATIC_MAX_TIME 60*5*5 // 传感器静止最大时间，单位 200ms
#define DORMANCY_MAX_TIME 60*5 // 休眠检测时间，单位 200ms
// --- I2C地址定义 ---
#define LIS2DH12_W_ADDR 0x32
#define LIS2DH12_R_ADDR 0x33
// --- IIC 寄存器地址宏定义 ---
#define LIS2DH12_WHO_AM_I 0x01 //0F
#define LIS2DH12_CTRL_REG1 0x20
#define LIS2DH12_CTRL_REG4 0x23
#define LIS2DH12_CTRL_REG5 0x24
#define LIS2DH12_OUT_X_L 0x28
#define LIS2DH12_FIFO_CTRL_REG 0x2E
#define LIS2DH12_SRC_REG 0x2F
//END -- 宏定义
//////////////////////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////////////////////
//START -- 变量定义
u8 dormancy_flag = 0; // 休眠标识
u8 dormancy_ago_moedl = 0; // 记录休眠前灯效
u16 gsensor_static_flag; // 记录传感器静止的时间,单位 200ms
axis_info_t current_data[32]; // 用于存储从FIFO读取的原始传感器数据
//运动数据全局变量
static motion_data_t motion_data = {{0.0f, 0.0f, 0.0f}, {0.0f, 0.0f, 0.0f}}; // 存储最终计算出的速度和距离
static axis_info_xtell gravity_vector = {0.0f, 0.0f, -GRAVITY_EARTH}; // 存储估算出的重力向量，初始假设Z轴朝下
static bool sensor_is_stable = false; // 传感器是否静止的标志
static axis_info_xtell linear_accel_global = {0.0f, 0.0f, 0.0f}; // 存储移除重力后的线性加速度，用于日志打印
static axis_info_xtell zero_g_offset = {0.0f, 0.0f, 0.0f}; // 存储开机校准测得的零点偏移量
u8 gsensor_alarm;
axis_info_xtell gsensor_xtell; // 存储is_sensor_stable计算出的平均值
//END -- 变量定义
//////////////////////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////////////////////
//START -- 函数定义
//END -- 函数定义
//////////////////////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////////////////////
//实现
// --- I2C底层函数封装 ---
static u32 SL_MEMS_i2cRead(u8 addr, u8 reg, u8 len, u8 *buf) {
return _gravity_sensor_get_ndata(addr, reg, buf, len);
}
static u8 SL_MEMS_i2cWrite(u8 addr, u8 reg, u8 data) {
gravity_sensor_command(addr, reg, data);
return 0;
}
// 检查传感器ID，确认设备是否正常连接
char LIS2DH12_Check() {
u8 reg_value = 0;
SL_MEMS_i2cRead(LIS2DH12_R_ADDR, LIS2DH12_WHO_AM_I, 1, &reg_value);
if (reg_value == 0x6A) { //0x33
return 0x01;
}
return 0x00;
}
// 从传感器FIFO读取一批原始数据
void LIS2DH12_read_data(axis_info_t *sl_accel) {
u8 fifo_src = 0;
u8 samples_available = 0;
u8 data[192];
s16 raw_x,raw_y,raw_z;
SL_MEMS_i2cRead(LIS2DH12_R_ADDR, LIS2DH12_SRC_REG, 1, &fifo_src);
samples_available = fifo_src & 0x1F;
if (samples_available == 0) return;
SL_MEMS_i2cRead(LIS2DH12_R_ADDR, LIS2DH12_OUT_X_L | 0x80, samples_available * 6, data);
for (u8 i = 0; i < samples_available; i++) {
// 数据处理方式与 +/-8g 普通模式(10位) 匹配
raw_x = (int16_t)((data[i * 6 + 1] << 8) | data[i * 6]) >> 6;
raw_y = (int16_t)((data[i * 6 + 3] << 8) | data[i * 6 + 2]) >> 6;
raw_z = (int16_t)((data[i * 6 + 5] << 8) | data[i * 6 + 4]) >> 6;
sl_accel[i].x = raw_x;
sl_accel[i].y = raw_y;
sl_accel[i].z = raw_z;
}
}
// 开机校准函数：测量传感器的静态零点偏移
void LIS2DH12_calibrate() {
xlog("开始传感器校准...\n");
axis_info_t cal_data[32];
long x_sum = 0, y_sum = 0;
const int num_samples = 32;
delay_2ms(100); // 等待约200ms，让FIFO填满数据
LIS2DH12_read_data(cal_data);
for (int i = 0; i < num_samples; i++) {
x_sum += cal_data[i].x;
y_sum += cal_data[i].y;
}
zero_g_offset.x = (float)x_sum / num_samples;
zero_g_offset.y = (float)y_sum / num_samples;
zero_g_offset.z = 0; // Z轴主要受重力影响，不进行校准
xlog("校准完成. X轴偏移: %.2f, Y轴偏移: %.2f\n", zero_g_offset.x, zero_g_offset.y);
}
// 初始化并配置LIS2DH12传感器
u8 LIS2DH12_Config(void) {
if (LIS2DH12_Check() != 1) {
xlog("LIS2DH12 I2C error\n");
return -1;
}
// 统一配置: 25Hz采样率, +/-8g量程, 普通模式(10位)
SL_MEMS_i2cWrite(LIS2DH12_W_ADDR, LIS2DH12_CTRL_REG1, 0x37); // 25 Hz ODR
SL_MEMS_i2cWrite(LIS2DH12_W_ADDR, LIS2DH12_CTRL_REG4, 0x20); // +/-8g, BDU enabled
SL_MEMS_i2cWrite(LIS2DH12_W_ADDR, LIS2DH12_CTRL_REG5, 0x40); // 使能FIFO
SL_MEMS_i2cWrite(LIS2DH12_W_ADDR, LIS2DH12_FIFO_CTRL_REG, 0x80); // 流模式
// 执行开机校准
LIS2DH12_calibrate();
xlog("LIS2DH12 I2C success\n");
return 0;
}
// 判断传感器是否处于静止状态
bool is_sensor_stable(axis_info_t *accel_data, int sample_count) {
float mean_x = 0, mean_y = 0, mean_z = 0;
float variance_x = 0, variance_y = 0, variance_z = 0;
if (sample_count <= 1) return true;
// 1. 计算均值
for (int i = 0; i < sample_count; i++) {
mean_x += accel_data[i].x;
mean_y += accel_data[i].y;
mean_z += accel_data[i].z;
}
mean_x /= sample_count;
mean_y /= sample_count;
mean_z /= sample_count;
gsensor_xtell.x = mean_x;
gsensor_xtell.y = mean_y;
gsensor_xtell.z = mean_z;
// 2. 计算方差
for (int i = 0; i < sample_count; i++) {
variance_x += (accel_data[i].x - mean_x) * (accel_data[i].x - mean_x);
variance_y += (accel_data[i].y - mean_y) * (accel_data[i].y - mean_y);
variance_z += (accel_data[i].z - mean_z) * (accel_data[i].z - mean_z);
}
variance_x /= (sample_count - 1);
variance_y /= (sample_count - 1);
variance_z /= (sample_count - 1);
// 3. 如果方差大于阈值，则认为在运动
if (variance_x > THRESHOLD || variance_y > THRESHOLD || variance_z > THRESHOLD) {
return false;
}
return true;
}
// 获取当前的总加速度（包含重力），单位 m/s^2
axis_info_xtell get_current_accel_mss(void) {
axis_info_xtell accel_mss;
// 灵敏度 @ +/-8g 普通模式 (10-bit) = 12 mg/LSB
const float sensitivity_g_per_lsb = 0.012f;
// 在转换前，先减去校准测得的零点偏移
accel_mss.x = ((float)gsensor_xtell.x - zero_g_offset.x) * sensitivity_g_per_lsb * GRAVITY_EARTH;
accel_mss.y = ((float)gsensor_xtell.y - zero_g_offset.y) * sensitivity_g_per_lsb * GRAVITY_EARTH;
accel_mss.z = (float)gsensor_xtell.z * sensitivity_g_per_lsb * GRAVITY_EARTH;
return accel_mss;
}
// 获取计算好的运动数据（速度和距离）
void get_motion_data(motion_data_t *data) {
if (data) {
memcpy(data, &motion_data, sizeof(motion_data_t));
}
}
// 获取移除重力后的线性加速度
axis_info_xtell get_linear_accel_mss(void) {
return linear_accel_global;
}
// 核心计算任务，由定时器周期性调用
void xtell_i2c_test() {
// 1. 读取一批最新的传感器数据
LIS2DH12_read_data(current_data);
// 2. 判断传感器当前是否静止
sensor_is_stable = is_sensor_stable(current_data, SAMPLE_COUNT);
// 3. 获取校准和转换后的总加速度 (m/s^2)
axis_info_xtell current_accel_mss = get_current_accel_mss();
// 4. 使用低通滤波器估算重力向量
gravity_vector.x = LPF_ALPHA * gravity_vector.x + (1.0f - LPF_ALPHA) * current_accel_mss.x;
gravity_vector.y = LPF_ALPHA * gravity_vector.y + (1.0f - LPF_ALPHA) * current_accel_mss.y;
gravity_vector.z = LPF_ALPHA * gravity_vector.z + (1.0f - LPF_ALPHA) * current_accel_mss.z;
// 5. 从总加速度中减去重力，得到线性加速度
linear_accel_global.x = current_accel_mss.x - gravity_vector.x;
linear_accel_global.y = current_accel_mss.y - gravity_vector.y;
linear_accel_global.z = current_accel_mss.z - gravity_vector.z;
// 6. 应用死区：忽略过小的加速度值（噪声）
if (fabsf(linear_accel_global.x) < DEADZONE_MSS) linear_accel_global.x = 0.0f;
if (fabsf(linear_accel_global.y) < DEADZONE_MSS) linear_accel_global.y = 0.0f;
if (fabsf(linear_accel_global.z) < DEADZONE_MSS) linear_accel_global.z = 0.0f;
// 7. 积分线性加速度，得到速度
motion_data.velocity.x += linear_accel_global.x * SAMPLING_PERIOD_S;
motion_data.velocity.y += linear_accel_global.y * SAMPLING_PERIOD_S;
motion_data.velocity.z += linear_accel_global.z * SAMPLING_PERIOD_S;
// 8. 如果传感器静止，重置速度和距离以消除漂移
if (sensor_is_stable) {
motion_data.velocity.x = 0.0f;
motion_data.velocity.y = 0.0f;
motion_data.velocity.z = 0.0f;
motion_data.distance.x = 0.0f;
motion_data.distance.y = 0.0f;
motion_data.distance.z = 0.0f;
}
// 9. 积分速度，得到距离
motion_data.distance.x += motion_data.velocity.x * SAMPLING_PERIOD_S;
motion_data.distance.y += motion_data.velocity.y * SAMPLING_PERIOD_S;
motion_data.distance.z += motion_data.velocity.z * SAMPLING_PERIOD_S;
// 10. 计算并打印总的移动距离（可选，用于调试）
float total_distance_magnitude = sqrtf(motion_data.distance.x * motion_data.distance.x +
motion_data.distance.y * motion_data.distance.y +
motion_data.distance.z * motion_data.distance.z);
// xlog("Total distance traveled: %.2f m\n", total_distance_magnitude);
}

59
apps/earphone/xtell_Sensor/sensor/LIS2DH12.h Normal file
View File

@ -0,0 +1,59 @@
#ifndef LIS2DH12_H
#define LIS2DH12_H
#include "gSensor/gSensor_manage.h"
#include "le_rcsp_adv_module.h"
// --- 物理常量定义 ---
#define GRAVITY_EARTH 9.80665f // 地球重力加速度 (m/s^2)
#define SAMPLING_PERIOD_S 0.2f // 采样周期 (对应于200ms的定时器)
// --- 数据结构定义 ---
// 三轴数据结构体 (可用于加速度、速度、距离)
typedef struct {
float x;
float y;
float z;
} axis_info_xtell;
// 运动数据结构体，包含速度和距离
typedef struct {
axis_info_xtell velocity; // 速度 (m/s)
axis_info_xtell distance; // 距离 (m)
} motion_data_t;
// --- API 函数声明 ---
/**
* @brief 初始化并配置LIS2DH12传感器
* @return 0 表示成功, -1 表示失败
*/
unsigned char LIS2DH12_Config(void);
/**
* @brief 核心计算任务，应由定时器周期性调用
*/
void xtell_i2c_test(void);
// --- 数据获取函数声明 ---
/**
* @brief 获取计算好的运动数据（速度和距离）
* @param data 指向 motion_data_t 结构体的指针，用于存放结果
*/
void get_motion_data(motion_data_t *data);
/**
* @brief 获取当前的总加速度（包含重力），单位 m/s^2
* @return axis_info_xtell 包含x,y,z轴总加速度的结构体
*/
axis_info_xtell get_current_accel_mss(void);
/**
* @brief 获取当前移除重力后的线性加速度，单位 m/s^2
* @return axis_info_xtell 包含x,y,z轴线性加速度的结构体
*/
axis_info_xtell get_linear_accel_mss(void);
#endif

1005
apps/earphone/xtell_Sensor/sensor/SC7U22.c Normal file
View File

File diff suppressed because it is too large Load Diff

138
apps/earphone/xtell_Sensor/sensor/SC7U22.h Normal file
View File

@ -0,0 +1,138 @@
/**************************************************
Copyright (c) 2022 Silan MEMS. All Rights Reserved.
@Silan MEMS Sensor Product Line
@Code Author:Zhou Min
**************************************************/
#ifndef __SCU722_H__
#define __SCU722_H__
#include "gSensor/gSensor_manage.h"
#include "printf.h"
//是否使能串口打印调试
#define SL_Sensor_Algo_Release_Enable 0x00
//是否开启FIFO模式，默认STREAM模式
#define SL_SC7U22_FIFO_ENABLE 0x00
/***使用前请根据实际情况配置以下参数******/
/**SC7U22的SDO 接地: 0****************/
/**SC7U22的SDO 接电源:1****************/
#define SL_SC7U22_SDO_VDD_GND 1
/*****************************************/
/***使用前请根据实际IIC地址配置参数***/
/**SC7U22的IIC 接口地址为 7bits: 0****/
/**SC7U22的IIC 接口地址为 8bits: 1****/
#define SL_SC7U22_IIC_7BITS_8BITS 0
/*****************************************/
#if SL_SC7U22_SDO_VDD_GND==0
#define SL_SC7U22_IIC_7BITS_ADDR 0x18
#define SL_SC7U22_IIC_8BITS_WRITE_ADDR 0x30
#define SL_SC7U22_IIC_8BITS_READ_ADDR 0x31
#else
#define SL_SC7U22_IIC_7BITS_ADDR 0x19
#define SL_SC7U22_IIC_8BITS_WRITE_ADDR 0x32
#define SL_SC7U22_IIC_8BITS_READ_ADDR 0x33
#endif
#if SL_SC7U22_IIC_7BITS_8BITS==0
#define SL_SC7U22_IIC_ADDRESS SL_SC7U22_IIC_7BITS_ADDR
#else
#define SL_SC7U22_IIC_WRITE_ADDRESS SL_SC7U22_IIC_8BITS_WRITE_ADDR
#define SL_SC7U22_IIC_READ_ADDRESS SL_SC7U22_IIC_8BITS_READ_ADDR
#endif
unsigned char SL_SC7U22_I2c_Spi_Write(unsigned char sl_spi_iic, unsigned char reg, unsigned char dat);
unsigned char SL_SC7U22_I2c_Spi_Read(unsigned char sl_spi_iic, unsigned char reg, unsigned short len, unsigned char* buf);
/*************I2C通信检测函数******************/
unsigned char SL_SC7U22_Check(void);
/*************函数返回值*****************/
/**return : 1 IIC通信正常,IC正常**************/
/**return : 0 IIC通信异常,IC异常**********/
/*************传感器初始化函数*******************/
unsigned char SL_SC7U22_Config(void);
/*************函数返回值*****************/
/**return : 1 IIC通信正常,IC正常*************/
/**return : 0; IIC通信异常,IC异常*********/
/*************SC7U22 Sensor Time**************/
unsigned int SL_SC7U22_TimeStamp_Read(void);
/*************函数返回值*****************/
/**return : 内部传感器时间***************/
#if SL_SC7U22_FIFO_ENABLE ==0x00
/******实时读取数据寄存器数据，相当于从400Hz的FIFO中取出数据******/
void SL_SC7U22_RawData_Read(signed short* acc_data_buf, signed short* gyr_data_buf);
/************* 输入XYZ三轴数据存放的地址*****************/
/************* *acc_data_buf: ACC数据***********************/
/************* *gyr_data_buf: GYR数据***********************/
#else
/******实时读取数据寄存器FIFO数据******/
unsigned short SL_SC7U22_FIFO_Read(signed short* accx_buf, signed short* accy_buf, signed short* accz_buf, signed short* gyrx_buf, signed short* gyry_buf, signed short* gyrz_buf);
/*************输入XYZ三轴数据首地址**************************/
/*************accx_buf[0]: ACC_X的第一个数据**************/
/*************accy_buf[0]: ACC_Y的第一个数据**************/
/*************accz_buf[0]: ACC_Z的第一个数据**************/
/*************gyrx_buf[0]: GYR_X的第一个数据**************/
/*************gyry_buf[0]: GYR_Y的第一个数据**************/
/*************gyrz_buf[0]: GYR_Z的第一个数据**************/
/****************函数返回值****************************/
/**return : len 表示数组长度*************************/
#endif
/*********进入传感器关闭模式*************/
unsigned char SL_SC7U22_POWER_DOWN(void);
/**0: 关闭模式失败***********************/
/**1: 关闭模式成功***********************/
/*********SC7U22 RESET***************/
unsigned char SL_SC7U22_SOFT_RESET(void);
/**0: 成功*****************************/
/**1: 失败**************************/
/*************GSensor and GyroSensor开启和关闭函数*********/
unsigned char SL_SC7U22_Open_Close_SET(unsigned char acc_enable,unsigned char gyro_enable);
/**acc_enable: 0=关闭ACC Sensor; 1=开启ACC Sensor*********/
/**gyro_enable: 0=关闭GYRO Sensor; 1=开启GYRO Sensor*******/
/**return: 0=设置失败，1=设置成功**************************/
/*********进入睡眠模式并开启中断函数*************/
unsigned char SL_SC7U22_IN_SLEEP_SET(unsigned char acc_odr,unsigned char vth,unsigned char tth,unsigned char int_io);
/**acc_odr: 12/25/50**************************************/
/**vth: 运动检测，阈值参数****************************/
/**tth: 运动检测，持续时间阈值，小于该时间则过滤**********/
/**int_io: 1=INT1, 2=INT2*********************************/
/**return: 0=设置失败，1=设置成功**************************/
/*********进入唤醒模式，设置参数并关闭中断函数***********/
unsigned char SL_SC7U22_WakeUp_SET(unsigned char odr_mode,unsigned char acc_range,unsigned char acc_hp_en,unsigned short gyro_range,unsigned char gyro_hp_en);
/**odr_mode: 25HZ/50Hz/100Hz/200Hz ACC+GYRO***************/
/**acc_range: ±2G/±4G/±8G/±16G*****************************/
/**acc_hp_en: 0=关闭高性能模式;1=开启*****/
/**gyro_range: ±125dps/±250dps/±500dps/±1000dps/±2000dps***/
/**gyro_hp_en: 0=关闭高性能模式;1=开启高性能模式; ********/
/**return: 0=设置失败，1=设置成功**************************/
/*********SC7U22 Angle Cauculate***************/
unsigned char SL_SC7U22_Angle_Output(unsigned char calibration_en,signed short *acc_gyro_input,float *Angle_output, unsigned char yaw_rst);
/**in calibration_en: 1=enable 0=disable***********************/
/**in/out acc_gyro_input[0]: ACC-X*****************************/
/**in/out acc_gyro_input[1]: ACC-Y*****************************/
/**in/out acc_gyro_input[2]: ACC-Z*****************************/
/**in/out acc_gyro_input[3]: GYR-X*****************************/
/**in/out acc_gyro_input[4]: GYR-Y*****************************/
/**in/out acc_gyro_input[5]: GYR-Z*****************************/
/**output Angle_output[0]: Pitch*****************************/
/**output Angle_output[1]: Roll******************************/
/**output Angle_output[2]: Yaw*******************************/
/**input yaw_rst: reset yaw value***************************/
unsigned char get_calibration_state(void);
/**寄存器宏定义*******************************/
#define SC7U22_WHO_AM_I 0x01
#endif // __SCU722_H__

996
apps/earphone/xtell_Sensor/sensor/SC7U22_Ma.c Normal file
View File

@ -0,0 +1,996 @@
#include "SC7U22.h"
#include "math.h"
#include "os/os_api.h"
#if SL_Sensor_Algo_Release_Enable==0x00
#include "printf.h"
#endif
//I2C SPI选择
//#define SL_SC7U22_SPI_EN_I2C_DISABLE 0x00 //需要配合SL_SPI_IIC_INTERFACE使用
#define SL_SPI_IIC_INTERFACE 0x01 //需要配合SL_SC7A22H_SPI_EN_I2C_DISABLE 使用
//是否使能原始数据高通滤波
#define SL_SC7U22_RAWDATA_HPF_ENABLE 0x00
//中断默认电平
#define SL_SC7U22_INT_DEFAULT_LEVEL 0x01
//SDO 是否上拉
#define SL_SC7U22_SDO_PullUP_ENABLE 0x01
//AOI中断是否唤醒
#define SL_SC7U22_AOI_Wake_Up_ENABLE 0x00
//FIFO_STREAM模式//FIFO_WTM模式
//#define SL_SC7U22_FIFO_STREAM_WTM 0x01//0X00=STREAM MODE 0X01=FIFO MODE
#define SL_SC7U22_IIC_DELAY_US 5
static u32 SL_MEMS_i2cRead(u8 addr, u8 reg, u8 len, u8 *buf) {
return _gravity_sensor_get_ndata(addr, reg, buf, len);
}
static u8 SL_MEMS_i2cWrite(u8 addr, u8 reg, u8 data) {
gravity_sensor_command(addr, reg, data);
return 0;
}
unsigned char SL_SC7U22_I2c_Spi_Write(unsigned char sl_spi_iic, unsigned char reg, unsigned char dat)
{
if (sl_spi_iic == 1) {
SL_MEMS_i2cWrite(SL_SC7U22_IIC_8BITS_WRITE_ADDR, reg, dat);
return 0;
}
// SPI not implemented
return 1; // 失败
}
unsigned char SL_SC7U22_I2c_Spi_Read(unsigned char sl_spi_iic, unsigned char reg, unsigned short len, unsigned char* buf)
{
if (sl_spi_iic == 1) {
return SL_MEMS_i2cRead(SL_SC7U22_IIC_8BITS_READ_ADDR, reg, len, buf);
}
// SPI not implemented
return 0; // 失败
}
static void sl_delay(unsigned char sl_i)
{
os_time_dly(sl_i);
}
unsigned char SL_SC7U22_Check(void)
{
unsigned char reg_value=0;
SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x7F, 0x00);//goto 0x00
SL_SC7U22_I2c_Spi_Read(SL_SPI_IIC_INTERFACE, SC7U22_WHO_AM_I, 1, &reg_value);
#if SL_Sensor_Algo_Release_Enable==0x00
printf("0x%x=0x%x\r\n",SC7U22_WHO_AM_I,reg_value);
#endif
if(reg_value==0x6A)
return 0x01;//SC7U22
else
return 0x00;//通信异常
}
unsigned char SL_SC7U22_Config(void)
{
unsigned char Check_Flag=0;
unsigned char reg_value=0;
#if SL_SPI_IIC_INTERFACE==0x00 //SPI
SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x7F, 0x00);//goto 0x90
sl_delay(1);
SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x4A, 0x66);
sl_delay(1);
SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x7F, 0x83);//goto 0x6F
sl_delay(1);
SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x6F, 0x04);//I2C disable
sl_delay(1);
SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x7F, 0x00);//goto 0x6F
sl_delay(1);
SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x4A, 0x00);
sl_delay(1);
#endif
Check_Flag=SL_SC7U22_Check();
// Check_Flag= SL_SC7U22_SOFT_RESET();
// Check_Flag=1;//强制初始化
#if SL_Sensor_Algo_Release_Enable==0x00
printf("SL_SC7U22_Check=0x%x\r\n",Check_Flag);
#endif
if(Check_Flag==1)
{
Check_Flag= SL_SC7U22_POWER_DOWN();
}
#if SL_Sensor_Algo_Release_Enable==0x00
printf("SL_SC7U22_POWER_DOWN=0x%x\r\n",Check_Flag);
#endif
if(Check_Flag==1)
{
Check_Flag= SL_SC7U22_SOFT_RESET();
}
#if SL_Sensor_Algo_Release_Enable==0x00
printf("SL_SC7U22_SOFT_RESET=0x%x\r\n",Check_Flag);
#endif
if(Check_Flag==1)
{
SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x7F, 0x00);//goto 0x00
os_time_dly(1);//10ms
SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x7D, 0x0E);//PWR_CTRL ENABLE ACC+GYR+TEMP
os_time_dly(1);//10ms
SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x40, 0x08);//ACC_CONF 0x08=100Hz
SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x41, 0x01);//ACC_RANGE ±4G
SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x42, 0x88);//GYR_CONF 0x88=100Hz
SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x43, 0x00);//GYR_RANGE 2000dps
SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x04, 0x50);//COM_CFG
#if SL_SC7U22_RAWDATA_HPF_ENABLE ==0x01
SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE,0x7F, 0x83);//goto 0x83
sl_delay(1);
SL_SC7U22_I2c_Spi_Read(SL_SPI_IIC_INTERFACE, 0x26, 1, &reg_value);
reg_value=reg_value|0xA0;
SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x26, reg_value);//HPF_CFG rawdata hpf
#endif
#if SL_SC7U22_AOI_Wake_Up_ENABLE==0x01
SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x30, 0x2A);//XYZ-ENABLE
SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x32, 0x01);//VTH
SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x33, 0x01);//TTH
SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x3F, 0x30);//HPF FOR AOI1&AOI2
#endif
#if SL_SC7U22_FIFO_ENABLE==0x01
SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x1E,0x1D);//
SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x1D,0x00);//
SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x1D,0x20);//
SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x1C,0x37);//
#endif
#if SL_SC7U22_SDO_PullUP_ENABLE ==0x01
SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE,0x7F, 0x8C);//goto 0x8C
sl_delay(1);
SL_SC7U22_I2c_Spi_Read(SL_SPI_IIC_INTERFACE, 0x30, 1, &reg_value);
reg_value=reg_value&0xFE;//CS PullUP_enable
reg_value=reg_value&0xFD;//SDO PullUP_enable
SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x30, reg_value);
SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE,0x7F, 0x00);//goto 0x00
os_time_dly(1);
SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE,0x7F, 0x00);//goto 0x00
os_time_dly(1);
#else
SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE,0x7F, 0x8C);//goto 0x8C
sl_delay(1);
SL_SC7U22_I2c_Spi_Read(SL_SPI_IIC_INTERFACE, 0x30, 1, &reg_value);
reg_value=reg_value&0xFE;//CS PullUP_enable
reg_value=reg_value|0x02;//SDO PullUP_disable
SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x30, reg_value);
SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE,0x7F, 0x00);//goto 0x00
sl_delay(1);
SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE,0x7F, 0x00);//goto 0x00
sl_delay(1);
#endif
return 1;
}
else
return 0;
}
//读取时间戳
unsigned int SL_SC7U22_TimeStamp_Read(void)
{
unsigned char time_data[3];
unsigned int time_stamp;
SL_SC7U22_I2c_Spi_Read(SL_SPI_IIC_INTERFACE, 0x18, 1, &time_data[0]);
SL_SC7U22_I2c_Spi_Read(SL_SPI_IIC_INTERFACE, 0x19, 1, &time_data[1]);
SL_SC7U22_I2c_Spi_Read(SL_SPI_IIC_INTERFACE, 0x20, 1, &time_data[2]);
time_stamp=(unsigned int)(time_data[0]<<16|time_data[1]<<8|time_data[2]);
return time_stamp;
}
#if SL_SC7U22_FIFO_ENABLE ==0x00
//100Hz 10ms read once
void SL_SC7U22_RawData_Read(signed short * acc_data_buf,signed short * gyr_data_buf)
{
unsigned char raw_data[12];
unsigned char drdy_satus=0x00;
unsigned short drdy_cnt=0;
while((drdy_satus&0x03)!=0x03)//acc+gyro
// while((drdy_satus&0x01)!=0x01)//acc
{
drdy_satus=0x00;
sl_delay(1);
SL_SC7U22_I2c_Spi_Read(SL_SPI_IIC_INTERFACE, 0x0B, 1, &drdy_satus);
drdy_cnt++;
if(drdy_cnt>30000) break;
}
#if SL_Sensor_Algo_Release_Enable==0x00
// SL_SC7U22_I2c_Spi_Read(SL_SPI_IIC_INTERFACE, 0x30, 1, &drdy_satus);
// printf("RawData:0x40=%x\r\n",drdy_satus);
// SL_SC7U22_I2c_Spi_Read(SL_SPI_IIC_INTERFACE, 0x40, 1, &drdy_satus);
// printf("RawData:0x40=%x\r\n",drdy_satus);
// SL_SC7U22_I2c_Spi_Read(SL_SPI_IIC_INTERFACE, 0x06, 1, &drdy_satus);
// printf("RawData:0x06=%x\r\n",drdy_satus);
// SL_SC7U22_I2c_Spi_Read(SL_SPI_IIC_INTERFACE, 0x07, 1, &drdy_satus);
// printf("RawData:0x07=%x\r\n",drdy_satus);
// SL_SC7U22_I2c_Spi_Read(SL_SPI_IIC_INTERFACE, 0x7D, 1, &drdy_satus);
// printf("RawData:0x7D=%x\r\n",drdy_satus);
// SL_SC7U22_I2c_Spi_Read(SL_SPI_IIC_INTERFACE, 0x31, 1, &drdy_satus);
// printf("RawData:0x31=%x\r\n",drdy_satus);
// SL_SC7U22_I2c_Spi_Read(SL_SPI_IIC_INTERFACE, 0x02, 1, &drdy_satus);
// printf("RawData:0x02=%x\r\n",drdy_satus);
// SL_SC7U22_I2c_Spi_Read(SL_SPI_IIC_INTERFACE, 0x03, 1, &drdy_satus);
// printf("RawData:0x03=%x\r\n",drdy_satus);
#endif
SL_SC7U22_I2c_Spi_Read(SL_SPI_IIC_INTERFACE, 0x0C, 12, &raw_data[0]);
acc_data_buf[0] =(signed short)((((unsigned char)raw_data[0])* 256) + ((unsigned char)raw_data[1]));//ACCX-16位
acc_data_buf[1] =(signed short)((((unsigned char)raw_data[2])* 256) + ((unsigned char)raw_data[3]));//ACCY-16位
acc_data_buf[2] =(signed short)((((unsigned char)raw_data[4])* 256) + ((unsigned char)raw_data[5]));//ACCZ-16位
gyr_data_buf[0] =(signed short)((((unsigned char)raw_data[6])* 256) + ((unsigned char)raw_data[7]));//GYRX-16位
gyr_data_buf[1] =(signed short)((((unsigned char)raw_data[8])* 256) + ((unsigned char)raw_data[9]));//GYRY-16位
gyr_data_buf[2] =(signed short)((((unsigned char)raw_data[10])* 256) + ((unsigned char)raw_data[11]));//GYRZ-16位
#if SL_Sensor_Algo_Release_Enable==0x00
printf("RawData:AX=%d,AY=%d,AZ=%d,GX=%d,GY=%d,GZ=%d\r\n",acc_data_buf[0],acc_data_buf[1],acc_data_buf[2],gyr_data_buf[0],gyr_data_buf[1],gyr_data_buf[2]);
#endif
}
#else
#if SL_Sensor_Algo_Release_Enable==0x00
#define SL_SC7U22_WAIT_FIFO_LEN_ENABLE 0x00//0x01
#else
#define SL_SC7U22_WAIT_FIFO_LEN_ENABLE 0x00
#endif
unsigned char Acc_FIFO_Num;
unsigned char Gyr_FIFO_Num;
unsigned char SL_SC7U22_FIFO_DATA[1024];
unsigned short SL_SC7U22_FIFO_Read(signed short *accx_buf,signed short *accy_buf,signed short *accz_buf,signed short *gyrx_buf,signed short *gyry_buf,signed short *gyrz_buf)
{
int16_t Acc_x = 0, Acc_y = 0, Acc_z = 0;
int16_t Gyr_x = 0, Gyr_y = 0, Gyr_z = 0;
unsigned char fifo_num1=0;
unsigned char fifo_num2=0;
unsigned short fifo_num=0;
unsigned short fifo_len=0;
unsigned short temp = 0;
unsigned short i = 0 ;
unsigned char header[2];
unsigned short j;
#if SL_Sensor_Algo_Release_Enable==0x00 //user can set to zero
#if SL_SC7U22_WAIT_FIFO_LEN_ENABLE==0x00
while((fifo_num1&0x20)!=0x20)
{
sl_delay(200);
SL_SC7U22_I2c_Spi_Read(SL_SPI_IIC_INTERFACE, 0x1F,1,&fifo_num1);
}
SL_SC7U22_I2c_Spi_Read(SL_SPI_IIC_INTERFACE, 0x1F,1,&fifo_num1);
SL_SC7U22_I2c_Spi_Read(SL_SPI_IIC_INTERFACE, 0x20,1,&fifo_num2);
if((fifo_num1&0x10)==0x10)
{
fifo_num=2048;
}
else
{
fifo_num=(fifo_num1&0x0F)*256+fifo_num2;
}
#else
while(fifo_num2<194)//32
{
SL_SC7U22_I2c_Spi_Read(SL_SPI_IIC_INTERFACE, 0x1F,1,&fifo_num1);
SL_SC7U22_I2c_Spi_Read(SL_SPI_IIC_INTERFACE, 0x20,1,&fifo_num2);
sl_delay(20);
fifo_wait++;
if(fifo_wait>30000) break;
}
fifo_wait=0;
fifo_num=fifo_num2;
#endif
#else
SL_SC7U22_I2c_Spi_Read(SL_SPI_IIC_INTERFACE, 0x1F,1,&fifo_num1);
SL_SC7U22_I2c_Spi_Read(SL_SPI_IIC_INTERFACE, 0x20,1,&fifo_num2);
if((fifo_num1&0x10)==0x10)
{
fifo_num=2048;
}
else
{
fifo_num=(fifo_num1&0x0F)*256+fifo_num2;
}
#endif
SL_SC7U22_I2c_Spi_Read(SL_SPI_IIC_INTERFACE, 0x21, fifo_num*2, SL_SC7U22_FIFO_DATA);//读取FIFO数据 BYTE NUM
// SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x1D, 0x00);//BY PASS MODE
// SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x1D, 0x20);//Stream MODE
printf("SC7U22_FIFO_NUM1:%d\n",fifo_num);
#if SL_Sensor_Algo_Release_Enable==0x00
// printf("0x1F:0x%x 0x20:0x%x\n",fifo_num1,fifo_num2);
// printf("SC7U22_FIFO_NUM1:%d\n",fifo_num);
#endif
fifo_len=0;
i = 0;
Acc_FIFO_Num=0;
Gyr_FIFO_Num=0;
while(i < fifo_num*2)
{
//header process 1
header[0] = SL_SC7U22_FIFO_DATA[i + 0];
header[1] = SL_SC7U22_FIFO_DATA[i + 1];
i = i + 2;
//timestamp process 2
if(header[1] & 0x80)
{
i = i + 4;//every frame include the timestamp, 4 bytes
}
//acc process 3
if(header[0] & 0x04)
{
accx_buf[Acc_FIFO_Num] = ((s16)(SL_SC7U22_FIFO_DATA[i + 0] * 256 + SL_SC7U22_FIFO_DATA[i + 1])) ;
accy_buf[Acc_FIFO_Num] = ((s16)(SL_SC7U22_FIFO_DATA[i + 2] * 256 + SL_SC7U22_FIFO_DATA[i + 3])) ;
accz_buf[Acc_FIFO_Num] = ((s16)(SL_SC7U22_FIFO_DATA[i + 4] * 256 + SL_SC7U22_FIFO_DATA[i + 5])) ;
printf("AccNum : %d ,Acc_x : %4d, Acc_y : %4d, Acc_z : %4d,\r\n",Acc_FIFO_Num, accx_buf[Acc_FIFO_Num], accy_buf[Acc_FIFO_Num], accz_buf[Acc_FIFO_Num]);
i = i + 6;
Acc_FIFO_Num++;
}
//gyro process 3
if(header[0] & 0x02)
{
gyrx_buf[Gyr_FIFO_Num] = ((s16)(SL_SC7U22_FIFO_DATA[i + 0] * 256 + SL_SC7U22_FIFO_DATA[i + 1])) ;
gyry_buf[Gyr_FIFO_Num] = ((s16)(SL_SC7U22_FIFO_DATA[i + 2] * 256 + SL_SC7U22_FIFO_DATA[i + 3])) ;
gyrz_buf[Gyr_FIFO_Num] = ((s16)(SL_SC7U22_FIFO_DATA[i + 4] * 256 + SL_SC7U22_FIFO_DATA[i + 5])) ;
printf("GyrNum : %d, Gyr_x : %4d, Gyr_y : %4d, Gyr_z : %4d,\r\n",Gyr_FIFO_Num, gyrx_buf[Gyr_FIFO_Num], gyry_buf[Gyr_FIFO_Num], gyrz_buf[Gyr_FIFO_Num]);
i = i + 6;
Gyr_FIFO_Num++;
}
//temperature process 1
if(header[0] & 0x01)
{
i = i + 2;
}
}
return fifo_len;
}
#endif
unsigned char SL_SC7U22_POWER_DOWN(void)
{
unsigned char SL_Read_Reg = 0xff;
SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x7F, 0x00);//goto 0x00
sl_delay(20);
SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x7D, 0x00);//POWER DOWN
sl_delay(200);
SL_SC7U22_I2c_Spi_Read(SL_SPI_IIC_INTERFACE, 0x7D, 1,&SL_Read_Reg);
if(SL_Read_Reg==0x00) return 1;
else return 0;
}
unsigned char SL_SC7U22_SOFT_RESET(void)
{
unsigned char SL_Read_Reg = 0xff;
SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x7F, 0x00);//goto 0x00
os_time_dly(1);
#if SL_Sensor_Algo_Release_Enable==0x00
SL_SC7U22_I2c_Spi_Read(SL_SPI_IIC_INTERFACE, 0x04, 1,&SL_Read_Reg);
printf("SL_SC7U22_SOFT_RESET1 0x04=0x%x\r\n",SL_Read_Reg);
SL_Read_Reg = 0xff;
#endif
SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x04, 0x10);//BOOT
#if SL_Sensor_Algo_Release_Enable==0x00
#endif
SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x4A, 0xA5);//SOFT_RESET
SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x4A, 0xA5);//SOFT_RESET
os_time_dly(20);
SL_SC7U22_I2c_Spi_Read(SL_SPI_IIC_INTERFACE, 0x04, 1,&SL_Read_Reg);
#if SL_Sensor_Algo_Release_Enable==0x00
printf("SL_SC7U22_SOFT_RESET2 0x08=0x%x\r\n",SL_Read_Reg);
#endif
if(SL_Read_Reg==0x50) return 1;
else return 0;
}
/****acc_enable ==0 close acc;acc_enable ==1 open acc******/
/****gyro_enable==0 close acc;gyro_enable==1 open acc******/
unsigned char SL_SC7U22_Open_Close_SET(unsigned char acc_enable,unsigned char gyro_enable)
{
unsigned char SL_Read_Reg = 0xff;
unsigned char SL_Read_Check= 0xff;
SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x7F, 0x00);//goto 0x00
sl_delay(1);
SL_SC7U22_I2c_Spi_Read(SL_SPI_IIC_INTERFACE, 0x7D, 1,&SL_Read_Reg);
if(acc_enable==0)
{
SL_Read_Reg=SL_Read_Reg&0xFB;//Bit.ACC_EN=0
}
else if(acc_enable==1)
{
SL_Read_Reg=SL_Read_Reg|0x04;//Bit.ACC_EN=1
}
if(gyro_enable==0)
{
SL_Read_Reg=SL_Read_Reg&0xFD;//Bit.GYR_EN=0
}
else if(gyro_enable==1)
{
SL_Read_Reg=SL_Read_Reg|0x02;//Bit.GYR_EN=1
}
SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x7D, SL_Read_Reg);//PWR_CTRL ENABLE ACC+GYR+TEMP
sl_delay(5);//5ms
SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x7D, SL_Read_Reg);//PWR_CTRL ENABLE ACC+GYR+TEMP
sl_delay(20);//10ms
SL_SC7U22_I2c_Spi_Read(SL_SPI_IIC_INTERFACE, 0x7D, 1,&SL_Read_Check);
if(SL_Read_Reg!=SL_Read_Check)
{
#if SL_Sensor_Algo_Release_Enable==0x00
printf("SL_Read_Reg=0x%x SL_Read_Check=0x%x\r\n",SL_Read_Reg,SL_Read_Check);
#endif
return 0;
}
return 1;
}
/*******开启中断******/
unsigned char SL_SC7U22_IN_SLEEP_SET(unsigned char acc_odr,unsigned char vth,unsigned char tth,unsigned char int_io)
{
unsigned char SL_Read_Reg = 0xff;
unsigned char SL_Acc_Odr_Reg = 0xff;
SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x7F, 0x00);//goto 0x00
sl_delay(1);
if(int_io==1)
{
SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x06, 0x02);//AOI1-INT1
}
else if(int_io==2)
{
SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x08, 0x02);//AOI1-INT2
}
SL_SC7U22_I2c_Spi_Read(SL_SPI_IIC_INTERFACE, 0x04, 1, &SL_Read_Reg);
#if SL_SC7U22_INT_DEFAULT_LEVEL ==0x01
SL_Read_Reg=SL_Read_Reg|0x04;
SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x04, SL_Read_Reg);//defalut high level&& push-pull
#else
reg_value=reg_value&0xDF;
SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x06, SL_Read_Reg);//defalut low level&& push-pull
#endif
SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x30, 0x2A);//AIO1-Enable
SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x32, vth);//VTH
SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x33, tth);//TTH
if(acc_odr==12)
{
SL_Acc_Odr_Reg=0x05;
}
else if(acc_odr==25)
{
SL_Acc_Odr_Reg=0x06;
}
else if(acc_odr==50)
{
SL_Acc_Odr_Reg=0x07;
}
SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x40, SL_Acc_Odr_Reg);//ACC_CONF
os_time_dly(1);//5ms
SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x7D, 0x04);//acc open and gyro close
os_time_dly(1);//5ms
SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x7D, 0x04);//acc open and gyro close
sl_delay(200);
SL_SC7U22_I2c_Spi_Read(SL_SPI_IIC_INTERFACE, 0x7D, 1,&SL_Read_Reg);
if(SL_Read_Reg!=0x04)
{
#if SL_Sensor_Algo_Release_Enable==0x00
printf("SL_Read_Reg=0x%x 0x04\r\n",SL_Read_Reg);
#endif
return 0;
}
return 1;
}
/*******ODR SET:25 50 100 200******************/
/*******acc range:2 4 8 16*********************/
/*******gyro range:125 250 500 1000 2000*******/
/*******acc_hp_en: 0=disable 1=enable**********/
/*******gyro_hp_en:0=disable 1=enable**********/
unsigned char SL_SC7U22_WakeUp_SET(unsigned char odr_mode,unsigned char acc_range,unsigned char acc_hp_en,unsigned short gyro_range,unsigned char gyro_hp_en)
{
unsigned char SL_Odr_Reg = 0x00;
unsigned char SL_acc_mode_Reg = 0x00;
unsigned char SL_gyro_mode_Reg = 0x00;
unsigned char SL_acc_range_Reg = 0x00;
unsigned char SL_gyro_range_Reg = 0x00;
unsigned char SL_Read_Check = 0xff;
SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x7F, 0x00);//goto 0x00
sl_delay(1);
SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x7D, 0x06);//PWR_CTRL ENABLE ACC+GYR
sl_delay(5);
SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x7D, 0x06);//PWR_CTRL ENABLE ACC+GYR
sl_delay(200);
SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x30, 0x00);//AIO1-disable
SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x32, 0xff);//vth
SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x33, 0xff);//tth
if(odr_mode==25)
{
SL_Odr_Reg=0x06;
}
else if(odr_mode==50)
{
SL_Odr_Reg=0x07;
}
else if(odr_mode==100)
{
SL_Odr_Reg=0x08;
}
else if(odr_mode==200)
{
SL_Odr_Reg=0x09;
}
if(acc_hp_en==1)
SL_acc_mode_Reg=0x80;
SL_acc_mode_Reg=SL_acc_mode_Reg|SL_Odr_Reg;
SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x40, SL_acc_mode_Reg);//ACC_CONF
if(gyro_hp_en==1)
SL_gyro_mode_Reg=0x40;
else if(gyro_hp_en==2)
SL_gyro_mode_Reg=0x80;
else if(gyro_hp_en==3)
SL_gyro_mode_Reg=0xC0;
SL_gyro_mode_Reg=SL_gyro_mode_Reg|SL_Odr_Reg;
SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x42, SL_gyro_mode_Reg);//GYR_CONF
if(acc_range==2)
{
SL_acc_range_Reg=0x00;
}
else if(acc_range==4)
{
SL_acc_range_Reg=0x01;
}
else if(acc_range==8)
{
SL_acc_range_Reg=0x02;
}
else if(acc_range==16)
{
SL_acc_range_Reg=0x03;
}
SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x41, SL_acc_range_Reg);//ACC_RANGE
if(gyro_range==2000)
{
SL_gyro_range_Reg=0x00;
}
else if(gyro_range==1000)
{
SL_gyro_range_Reg=0x01;
}
else if(gyro_range==500)
{
SL_gyro_range_Reg=0x02;
}
else if(gyro_range==250)
{
SL_gyro_range_Reg=0x03;
}
else if(gyro_range==125)
{
SL_gyro_range_Reg=0x04;
}
SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x43, SL_gyro_range_Reg);//GYR_RANGE 2000dps
SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x43, SL_gyro_range_Reg);//GYR_RANGE 2000dps
#if SL_Sensor_Algo_Release_Enable==0x00
// SL_SC7U22_I2c_Spi_Read(SL_SPI_IIC_INTERFACE, 0x40, 1, &SL_Read_Check);
// printf("RawData:0x40=%x\r\n",SL_Read_Check);
// SL_SC7U22_I2c_Spi_Read(SL_SPI_IIC_INTERFACE, 0x41, 1, &SL_Read_Check);
// printf("RawData:0x41=%x\r\n",SL_Read_Check);
// SL_SC7U22_I2c_Spi_Read(SL_SPI_IIC_INTERFACE, 0x42, 1, &SL_Read_Check);
// printf("RawData:0x42=%x\r\n",SL_Read_Check);
// SL_SC7U22_I2c_Spi_Read(SL_SPI_IIC_INTERFACE, 0x43, 1, &SL_Read_Check);
// printf("RawData:0x43=%x\r\n",SL_Read_Check);
#endif
SL_SC7U22_I2c_Spi_Read(SL_SPI_IIC_INTERFACE, 0x43, 1,&SL_Read_Check);
if(SL_Read_Check!=SL_gyro_range_Reg)
{
#if SL_Sensor_Algo_Release_Enable==0x00
printf("SL_Read_Check=0x%x SL_gyro_range_Reg=0x%x\r\n",SL_Read_Check,SL_gyro_range_Reg);
#endif
return 0;
}
return 1;
}
#if SL_SC7U22_FIFO_ENABLE ==0x00
// =================================================================================================
// Madgwick AHRS 滤波器相关变量和函数
// -------------------------------------------------------------------------------------------------
// 定义常量
#define sampleFreq 100.0f // 传感器采样频率 (Hz)，必须与实际的传感器数据更新频率一致
#define betaDef 0.1f // 算法的比例增益 beta，影响加速度计修正陀螺仪的权重
// 全局变量
static volatile float beta = betaDef; // 算法增益 beta
static volatile float q0 = 1.0f, q1 = 0.0f, q2 = 0.0f, q3 = 0.0f; // 表示姿态的四元数 (w, x, y, z)
/**
* @brief 快速计算 1/sqrt(x)
* @param x 输入的浮点数
* @return 1/sqrt(x) 的近似值
*/
static float invSqrt(float x) {
float halfx = 0.5f * x;
float y = x;
long i = *(long*)&y;
i = 0x5f3759df - (i>>1);
y = *(float*)&i;
y = y * (1.5f - (halfx * y * y)); // 牛顿迭代法，提高精度
return y;
}
/**
* @brief Madgwick AHRS 姿态更新函数 (IMU版本)
* @details 该函数融合了陀螺仪和加速度计的数据，计算出表示设备姿态的四元数。
* 1. 使用陀螺仪数据积分，得到一个初步的姿态估计（预测）。
* 2. 使用加速度计数据（当设备处于静止或低速运动时，加速度计主要测量重力）来修正这个估计（修正）。
* 3. 通过梯度下降法找到一个最优的旋转，使得在当前姿态下，重力向量的方向与加速度计测量的方向最接近。
* @param gx, gy, gz 陀螺仪三轴角速度 (单位: rad/s)
* @param ax, ay, az 加速度计三轴加速度 (单位: g)
*/
static void MadgwickAHRSupdateIMU(float gx, float gy, float gz, float ax, float ay, float az) {
float recipNorm;
float s0, s1, s2, s3;
float qDot1, qDot2, qDot3, qDot4;
float _2q0, _2q1, _2q2, _2q3, _4q0, _4q1, _4q2 ,_8q1, _8q2, q0q0, q1q1, q2q2, q3q3;
float dt = 1.0f / sampleFreq; // 采样时间间隔
// --- 1. 陀螺仪积分：计算四元数的变化率 ---
// 姿态运动学的基本方程，描述了姿态如何随角速度变化。
qDot1 = 0.5f * (-q1 * gx - q2 * gy - q3 * gz);
qDot2 = 0.5f * (q0 * gx + q2 * gz - q3 * gy);
qDot3 = 0.5f * (q0 * gy - q1 * gz + q3 * gx);
qDot4 = 0.5f * (q0 * gz + q1 * gy - q2 * gx);
// --- 2. 加速度计修正 ---
// 仅当加速度计读数有效时（即模长不为0）才进行修正，防止计算NaN。
if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) {
// 将加速度计读数归一化，得到单位向量
recipNorm = invSqrt(ax * ax + ay * ay + az * az);
ax *= recipNorm;
ay *= recipNorm;
az *= recipNorm;
// 预先计算一些重复使用的值，提高效率
_2q0 = 2.0f * q0;
_2q1 = 2.0f * q1;
_2q2 = 2.0f * q2;
_2q3 = 2.0f * q3;
_4q0 = 4.0f * q0;
_4q1 = 4.0f * q1;
_4q2 = 4.0f * q2;
_8q1 = 8.0f * q1;
_8q2 = 8.0f * q2;
q0q0 = q0 * q0;
q1q1 = q1 * q1;
q2q2 = q2 * q2;
q3q3 = q3 * q3;
// --- 梯度下降法：计算修正量 ---
// 目标函数 f(q, a) = [2(q1q3 - q0q2) - ax, 2(q0q1 + q2q3) - ay, 2(0.5 - q1^2 - q2^2) - az]^T
// s0, s1, s2, s3 是目标函数 f 对四元数 q 的雅可比矩阵 J 与 f 的乘积。
// 这个结果代表了误差函数的梯度方向，用于修正四元数的变化率。
s0 = _4q0 * q2q2 + _2q2 * ax + _4q0 * q1q1 - _2q1 * ay;
s1 = _4q1 * q3q3 - _2q3 * ax + 4.0f * q0q0 * q1 - _2q0 * ay - _4q1 + _8q1 * q1q1 + _8q1 * q2q2 + _4q1 * az;
s2 = 4.0f * q0q0 * q2 + _2q0 * ax + _4q2 * q3q3 - _2q3 * ay - _4q2 + _8q2 * q1q1 + _8q2 * q2q2 + _4q2 * az;
s3 = 4.0f * q1q1 * q3 - _2q1 * ax + 4.0f * q2q2 * q3 - _2q2 * ay;
recipNorm = invSqrt(s0 * s0 + s1 * s1 + s2 * s2 + s3 * s3); // 归一化梯度
s0 *= recipNorm;
s1 *= recipNorm;
s2 *= recipNorm;
s3 *= recipNorm;
// --- 应用修正量 ---
// 将计算出的修正量（梯度）从陀螺仪积分结果中减去，beta是修正的权重。
qDot1 -= beta * s0;
qDot2 -= beta * s1;
qDot3 -= beta * s2;
qDot4 -= beta * s3;
}
// --- 3. 积分：更新四元数 ---
// 使用一阶龙格-库塔法（即欧拉法）进行积分，得到新的四元数。
q0 += qDot1 * dt;
q1 += qDot2 * dt;
q2 += qDot3 * dt;
q3 += qDot4 * dt;
// --- 4. 归一化四元数 ---
// 保持四元数的模长为1，防止由于计算误差导致的累积漂移。
recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
q0 *= recipNorm;
q1 *= recipNorm;
q2 *= recipNorm;
q3 *= recipNorm;
}
// =================================================================================================
// --- 静态校准相关变量 ---
static unsigned char SL_SC7U22_Error_Flag=0;
static unsigned char SL_SC7U22_Error_cnt=0;
static unsigned char SL_SC7U22_Error_cnt2=0;
static signed short Temp_Accgyro[6] ={0};
static signed short Error_Accgyro[6]={0};
static signed int Sum_Avg_Accgyro[6] ={0};
static float yaw_offset = 0.0f;
static signed short SL_GetAbsShort(signed short v_Val_s16r)
{
if(v_Val_s16r==(-32768))
return 32767;
return (v_Val_s16r < 0) ? -v_Val_s16r : v_Val_s16r;
}
/**
* @brief 姿态角解算函数
* @details
* 该函数主要完成两项工作：
* 1. 静态校准：在初始阶段，检测传感器是否处于静止状态。如果是，则计算加速度计和陀螺仪的零点偏移（误差），用于后续的数据补偿。
* 2. 姿态解算：使用 Madgwick 滤波器融合经过校准后的加速度计和陀螺仪数据，计算出物体的俯仰角（Pitch）、横滚角（Roll）和偏航角（Yaw）。
*
* @param calibration_en 传入：外部校准使能标志。如果为0，则强制认为已经校准完成。
* @param acc_gyro_input 传入和传出：包含6轴原始数据的数组指针，顺序为 [ACC_X, ACC_Y, ACC_Z, GYR_X, GYR_Y, GYR_Z]。该函数会对其进行原地修改，填充为校准后的数据。
* @param Angle_output 传出：滤波后的结果，顺序为 [Pitch, Roll, Yaw]。
* @param yaw_rst 传入：Yaw轴重置标志。如果为1，则将Yaw角清零。
*
* @return
* - 0: 正在进行静态校准。
* - 1: 姿态角计算成功。
* - 2: 校准未完成，无法进行计算。
*/
unsigned char SL_SC7U22_Angle_Output(unsigned char calibration_en, signed short *acc_gyro_input, float *Angle_output, unsigned char yaw_rst)
{
unsigned short acc_gyro_delta[2];
unsigned char sl_i = 0;
// 如果外部强制使能校准，则将标志位置1
if (calibration_en == 0) {
SL_SC7U22_Error_Flag = 1;
}
// =================================================================================
// 步骤 1: 静态校准 (与原版逻辑相同)
// ---------------------------------------------------------------------------------
if (SL_SC7U22_Error_Flag == 0) {
// 计算当前数据与上一帧数据的差值，用于判断是否静止
acc_gyro_delta[0] = 0;
acc_gyro_delta[1] = 0;
for (sl_i = 0; sl_i < 3; sl_i++) {
acc_gyro_delta[0] += SL_GetAbsShort(acc_gyro_input[sl_i] - Temp_Accgyro[sl_i]);
acc_gyro_delta[1] += SL_GetAbsShort(acc_gyro_input[3 + sl_i] - Temp_Accgyro[3 + sl_i]);
}
// 保存当前数据，用于下一帧比较
for (sl_i = 0; sl_i < 6; sl_i++) {
Temp_Accgyro[sl_i] = acc_gyro_input[sl_i];
}
// 判断是否处于静止状态：加速度变化量、陀螺仪变化量、各轴加速度值都在一个很小的范围内
// 假设1g = 8192 (对应 +/-4g 量程)
if ((acc_gyro_delta[0] / 8 < 80) && (acc_gyro_delta[1] < 20) && (SL_GetAbsShort(acc_gyro_input[0]) < 3000) && (SL_GetAbsShort(acc_gyro_input[1]) < 3000) && (SL_GetAbsShort(acc_gyro_input[2] - 8192) < 3000)) { //acc<80mg gyro<20 lsb
if (SL_SC7U22_Error_cnt < 200) {
SL_SC7U22_Error_cnt++; // 静止计数器累加
}
} else {
SL_SC7U22_Error_cnt = 0; // 如果发生移动，则清空静止计数器
}
// 如果静止时间足够长（这里是190个采样周期，约1.9秒）
if (SL_SC7U22_Error_cnt > 190) {
// 开始累加50个采样点的数据
for (sl_i = 0; sl_i < 6; sl_i++) {
Sum_Avg_Accgyro[sl_i] += acc_gyro_input[sl_i];
}
SL_SC7U22_Error_cnt2++;
if (SL_SC7U22_Error_cnt2 > 49) {
// 累加满50个点后，计算平均值
SL_SC7U22_Error_Flag = 1; // 标记校准完成
SL_SC7U22_Error_cnt2 = 0;
SL_SC7U22_Error_cnt = 0;
for (sl_i = 0; sl_i < 6; sl_i++) {
Sum_Avg_Accgyro[sl_i] = Sum_Avg_Accgyro[sl_i] / 50;
}
// 计算零点偏移：理想值 - 实际平均值
// 加速度Z轴的理想值是8192，对应1g（假设量程为±4g）
Error_Accgyro[0] = 0 - Sum_Avg_Accgyro[0];
Error_Accgyro[1] = 0 - Sum_Avg_Accgyro[1];
Error_Accgyro[2] = 8192 - Sum_Avg_Accgyro[2];
Error_Accgyro[3] = 0 - Sum_Avg_Accgyro[3];
Error_Accgyro[4] = 0 - Sum_Avg_Accgyro[4];
Error_Accgyro[5] = 0 - Sum_Avg_Accgyro[5];
#if SL_Sensor_Algo_Release_Enable == 0x00
printf("AVG_Recode AX:%d,AY:%d,AZ:%d,GX:%d,GY:%d,GZ:%d\r\n", Sum_Avg_Accgyro[0], Sum_Avg_Accgyro[1], Sum_Avg_Accgyro[2], Sum_Avg_Accgyro[3], Sum_Avg_Accgyro[4], Sum_Avg_Accgyro[5]);
printf("Error_Recode AX:%d,AY:%d,AZ:%d,GX:%d,GY:%d,GZ:%d\r\n", Error_Accgyro[0], Error_Accgyro[1], Error_Accgyro[2], Error_Accgyro[3], Error_Accgyro[4], Error_Accgyro[5]);
#endif
}
} else {
// 如果在累加过程中发生移动，则重新开始
SL_SC7U22_Error_cnt2 = 0;
for (sl_i = 0; sl_i < 6; sl_i++) {
Sum_Avg_Accgyro[sl_i] = 0;
}
}
return 0; // 返回0，表示正在校准
}
// =================================================================================
// 步骤 2: 姿态解算 (Madgwick)
// ---------------------------------------------------------------------------------
if (SL_SC7U22_Error_Flag == 1) { // 确认已经校准完成
// --- 2.1 数据预处理 ---
// 应用零点偏移补偿
for (sl_i = 0; sl_i < 6; sl_i++) {
Temp_Accgyro[sl_i] = acc_gyro_input[sl_i] + Error_Accgyro[sl_i];
}
#if 1 // 将校准后的数据写回输入数组
for (sl_i = 0; sl_i < 6; sl_i++) {
acc_gyro_input[sl_i] = Temp_Accgyro[sl_i];
}
#endif
// --- 2.2 转换数据单位 ---
// 将校准后的传感器原始值 (LSB) 转换为 Madgwick 算法所需的物理单位。
// 加速度: LSB -> g (重力加速度)。转换系数 = 量程 / (2^15)。假设 +/-4g 量程, 系数 = 4 / 32768 = 1/8192。
float ax = (float)Temp_Accgyro[0] / 8192.0f;
float ay = (float)Temp_Accgyro[1] / 8192.0f;
float az = (float)Temp_Accgyro[2] / 8192.0f;
// 角速度: LSB -> rad/s (弧度/秒)。转换系数 = (量程 * PI) / (180 * 2^15)。
// 假设 +/-2000dps 量程, 系数 = (2000 * 3.14159) / (180 * 32768) ≈ 0.001064
float gx = (float)Temp_Accgyro[3] * 0.001064f;
float gy = (float)Temp_Accgyro[4] * 0.001064f;
float gz = (float)Temp_Accgyro[5] * 0.001064f;
// --- 2.3 调用 Madgwick 更新函数 ---
// 将处理好的物理单位数据传入滤波器，更新姿态四元数。
MadgwickAHRSupdateIMU(gx, gy, gz, ax, ay, az);
// --- 2.4 将四元数转换为欧拉角 ---
// 欧拉角（Pitch, Roll, Yaw）更直观，便于使用。转换公式如下。
// 转换结果单位为度 (乘以 180/PI ≈ 57.29578)。
float yaw, pitch, roll;
// Roll (横滚角，绕x轴旋转)
roll = atan2f(2.0f * (q0 * q1 + q2 * q3), 1.0f - 2.0f * (q1 * q1 + q2 * q2)) * 57.29578f;
// Pitch (俯仰角，绕y轴旋转)
float sinp = 2.0f * (q0 * q2 - q3 * q1);
if (fabsf(sinp) >= 1)
pitch = copysignf(3.14159265f / 2, sinp) * 57.29578f; // 防止万向节死锁，当sinp接近+/-1时，直接赋+/-90度
else
pitch = asinf(sinp) * 57.29578f;
// Yaw (偏航角，绕z轴旋转)
yaw = atan2f(2.0f * (q0 * q3 + q1 * q2), 1.0f - 2.0f * (q2 * q2 + q3 * q3)) * 57.29578f;
// --- 2.5 处理Yaw轴重置 ---
// Yaw角无法通过加速度计校正，会随时间漂移。提供一个重置机制，将当前Yaw角作为新的零点。
if (yaw_rst) {
yaw_offset = yaw;
}
// --- 2.6 输出最终角度 ---
// 将计算出的欧拉角存入输出数组。
Angle_output[0] = pitch;
Angle_output[1] = roll;
Angle_output[2] = yaw - yaw_offset; // 输出减去偏移量的相对Yaw角
return 1; // 返回1，表示计算成功
}
return 2; // 校准未完成，返回错误状态
}
#endif
/*
//-----------------------------------------
调用示例-by_lmx：
//1.
// 定义用于存放传感器数据的缓冲区
static signed short acc_raw_data[3]; // [0]: acc_x, [1]: acc_y, [2]: acc_z
static signed short gyr_raw_data[3]; // [0]: gyr_x, [1]: gyr_y, [2]: gyr_z
static signed short combined_raw_data[6]; // 用于合并 acc 和 gyr 数据
static float final_angle_data[3]; // [0]: Pitch, [1]: Roll, [2]: Yaw
// 传感器数据处理任务
void sensor_processing_task(void *priv)
{
while (1) {
// 4. 周期性调用读取函数，获取原始数据
SL_SC7U22_RawData_Read(acc_raw_data, gyr_raw_data);
// 5. 合并加速度和角速度数据到一个数组中
// SL_SC7U22_Angle_Output 函数需要一个包含6个元素的数组作为输入
memcpy(&combined_raw_data[0], acc_raw_data, sizeof(acc_raw_data));
memcpy(&combined_raw_data[3], gyr_raw_data, sizeof(gyr_raw_data));
// 6. 调用姿态解算函数
// 参数: (校准使能, 输入的6轴数据, 输出的角度数据, Yaw轴复位标志)
// calibration_en = 1: 让函数内部自动管理校准过程
// yaw_rst = 0: 不复位Yaw角
unsigned char status = SL_SC7U22_Angle_Output(1, combined_raw_data, final_angle_data, 0);
// 7. 检查函数返回的状态
if (status == 1) {
// 计算成功，final_angle_data 中的数据有效
printf("Pitch: %.2f, Roll: %.2f, Yaw: %.2f\n", final_angle_data[0], final_angle_data[1], final_angle_data[2]);
} else if (status == 0) {
// 传感器正在进行静态校准，此时角度数据可能不准确
printf("Sensor is calibrating...\n");
} else {
// status == 2, 表示校准未完成或发生错误
printf("Angle calculation error or calibration not finished.\n");
}
// 延时一段时间，例如10ms (对应100Hz)
os_time_dly(1);
}
}
// 应用程序主入口或初始化函数
void app_main()
{
// ... 其他初始化代码 ...
// 2. 调用初始化函数来配置SCU722传感器
unsigned char init_success = SL_SC7U22_Config();
if (init_success) {
printf("SCU722 初始化成功!\n");
// 3. 创建一个任务来周期性地读取和处理数据
task_create(sensor_processing_task, NULL, "sensor_task");
} else {
printf("SCU722 初始化失败!\n");
}
// ...
}
//-----------------------------------------
*/

8
apps/earphone/xtell_Sensor/xtell.h Normal file
View File

@ -0,0 +1,8 @@
#ifndef XTELL_H
#define XTELL_H
#define KS_BLE 0
#define XTELL_TEST 1
#endif

172
apps/earphone/xtell_Sensor/xtell_app_main.c Normal file
View File

@ -0,0 +1,172 @@
#include "system/includes.h"
/*#include "btcontroller_config.h"*/
#include "btstack/btstack_task.h"
#include "app_config.h"
#include "app_action.h"
#include "asm/pwm_led.h"
#include "tone_player.h"
#include "gpio.h"
#include "app_main.h"
#include "asm/charge.h"
#include "update.h"
#include "app_power_manage.h"
#include "app_charge.h"
#include "bt_profile_cfg.h"
#include "dev_manager/dev_manager.h"
#include "update_loader_download.h"
#include "avctp_user.h"
#include "debug.h"
///////////////////////////////////////////////////////////////////////////////////////////////////
//宏定义
#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
#define LOG_TAG_CONST APP
#define LOG_TAG "[APP]"
#define LOG_ERROR_ENABLE
#define LOG_DEBUG_ENABLE
#define LOG_INFO_ENABLE
/* #define LOG_DUMP_ENABLE */
#define LOG_CLI_ENABLE
//
///////////////////////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////////////////////
//变量
extern APP_VAR app_var;
u16 close_BL_number=0;
//
//////////////////////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////////////////////
//函数定义
extern void timer_2ms_handler();
extern void app_var_init(void);
void app_earphone_play_voice_file(const char *name);
void clr_wdt(void);
extern void check_power_on_key(void);
extern int cpu_reset_by_soft();
extern int audio_dec_init();
extern int audio_enc_init();
//
//////////////////////////////////////////////////////////////////////////////////////////////////
/*充电拔出,CPU软件复位, 不检测按键，直接开机*/
static void app_poweron_check(int update)
{
if (!update && cpu_reset_by_soft()) {
app_var.play_poweron_tone = 0;
return;
}
}
void create_process(u16* pid,char* name, void *priv, void (*func)(void *priv), u32 msec){
xlog("1 name=%s, pid =%d\n",name,*pid);
if (*pid != 0) return;
*pid = sys_timer_add(priv, func, msec);
xlog("2 name=%s, pid =%d\n",name,*pid);
}
void close_process(u16* pid,char* name){
xlog("name=%s,pid =%d\n",name,*pid);
if (*pid == 0) return;
sys_timer_del(*pid);
*pid = 0;
}
void close_BL(){
/**开机默认关闭 经典蓝牙 */
// close_BL_flag++;
xlog("xtell Classic Bluetooth off\n");
user_send_cmd_prepare(USER_CTRL_DISCONNECTION_HCI, 0, NULL); //断开此时经典蓝牙的连接，经典蓝牙还是可以被发现
delay_2ms(50);
user_send_cmd_prepare(USER_CTRL_WRITE_SCAN_DISABLE, 0, NULL); //关闭蓝牙可发现,已连接时不能操作
delay_2ms(50);
user_send_cmd_prepare(USER_CTRL_WRITE_CONN_DISABLE, 0, NULL); //关闭蓝牙可连接，
// sys_timer_del(close_BL_number); //删除定时器任务
close_process(&close_BL_number,__func__);
}
extern u32 timer_get_ms(void);
void xtell_app_main()
{
int update = 0;
u32 addr = 0, size = 0;
struct intent it;
xlog("==============xtell_app_main start================\n");
log_info("app_main\n");
app_var.start_time = timer_get_ms();
if (!UPDATE_SUPPORT_DEV_IS_NULL()) {
update = update_result_deal();
}
app_var_init();
if (get_charge_online_flag()) {
#if(TCFG_SYS_LVD_EN == 1)
vbat_check_init();
#endif
xlog("==============idle================\n");
init_intent(&it);
it.name = "idle";
it.action = ACTION_IDLE_MAIN;
start_app(&it);
} else {
xlog("==============handler start================\n");
check_power_on_voltage();
app_poweron_check(update);
init_intent(&it);
it.name = "handler";
it.action = ACTION_EARPHONE_MAIN;
start_app(&it);
xlog("==============handler end================\n");
}
//////////////////////////////////////////////////
//开机必须延时关闭经典蓝牙，不然底层代码会再次把蓝牙 打开
create_process(&close_BL_number, "close_BL",NULL, close_BL, 3000);
u8 mac_data[6];
extern void rcsp_adv_fill_mac_addr(u8 *mac_addr_buf);
rcsp_adv_fill_mac_addr(mac_data); //读取MAC地址
xlog("xtell BT mac data:%x:%x:%x:%x:%x:%x",mac_data[0],mac_data[1],mac_data[2],mac_data[3],mac_data[4],mac_data[5]);
user_send_cmd_prepare(USER_CTRL_WRITE_SCAN_ENABLE, 0, NULL); //打开蓝牙可发现,已连接时不能操作
delay_2ms(50);
user_send_cmd_prepare(USER_CTRL_WRITE_CONN_ENABLE, 0, NULL); //打开蓝牙可连接
delay_2ms(50);
extern void xtell_task_create(void);
xtell_task_create();
xlog("==============xtell_app_end================\n");
}

431
apps/earphone/xtell_Sensor/xtell_handler.c Normal file
View File

@ -0,0 +1,431 @@
#include "system/includes.h"
#include "media/includes.h"
#include "tone_player.h"
#include "earphone.h"
#include "app_config.h"
#include "app_action.h"
#include "app_task.h"
#include "btstack/avctp_user.h"
#include "btstack/btstack_task.h"
#include "btctrler/btctrler_task.h"
#include "btstack/frame_queque.h"
#include "user_cfg.h"
// #include "aec_user.h"
#include "classic/hci_lmp.h"
#include "bt_common.h"
#include "bt_ble.h"
#include "bt_tws.h"
#include "pbg_user.h"
#include "btstack/bluetooth.h"
#include "colorful_lights/colorful_lights.h"
#include "app_chargestore.h"
#include "jl_kws/jl_kws_api.h"
#include "asm/charge.h"
#include "app_charge.h"
#include "ui_manage.h"
#include "app_chargestore.h"
#include "app_umidigi_chargestore.h"
#include "app_testbox.h"
#include "app_online_cfg.h"
#include "app_main.h"
#include "app_power_manage.h"
#include "gSensor/gSensor_manage.h"
#include "key_event_deal.h"
#include "classic/tws_api.h"
#include "asm/pwm_led.h"
#include "ir_sensor/ir_manage.h"
#include "in_ear_detect/in_ear_manage.h"
#include "vol_sync.h"
#include "bt_background.h"
#include "default_event_handler.h"
#include "debug.h"
///////////////////////////////////////////////////////////////////////////////////////////////////
//宏定义
#define LOG_TAG_CONST EARPHONE
#define LOG_TAG "[EARPHONE]"
#define LOG_ERROR_ENABLE
#define LOG_DEBUG_ENABLE
#define xlog_ENABLE
#if(USE_DMA_UART_TEST) //使用dm串口测试时不能同时打开
#define MY_SNIFF_EN 0
#else
#define MY_SNIFF_EN 1 //默认打开
#endif
#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
//
///////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////
//变量
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";
static u16 play_poweron_ok_timer_id = 0;
//
///////////////////////////////////////////////////////////////////////////////////////////////////
extern int bt_hci_event_handler(struct bt_event *bt);
///////////////////////////////////////////////////////////////////////////////////////////////////
/*
* 模式状态机, 通过start_app()控制状态切换
*/
/* extern int audio_mic_init(); */
static int state_machine(struct application *app, enum app_state state, struct intent *it){
int error = 0;
static u8 tone_player_err = 0;
xlog("bt_state_machine=%d\n", state);
switch (state) {
case APP_STA_CREATE:
xlog("APP_STA_CREATE\n");
/* set_adjust_conn_dac_check(0); */
break;
case APP_STA_START:
xlog("APP_STA_START\n");
if (!it) {
xlog("APP_STA_START:it none\n");
break;
}
switch (it->action) {
case ACTION_EARPHONE_MAIN:
xlog("ble init\n");
/*
* handler 初始化
*/
clk_set("sys", BT_NORMAL_HZ);
u32 sys_clk = clk_get("sys");
bt_pll_para(TCFG_CLOCK_OSC_HZ, sys_clk, 0, 0);
/* bredr_set_dut_enble(1, 1); */
bt_function_select_init();
bredr_handle_register();
EARPHONE_STATE_INIT();
btstack_init();
sys_auto_shut_down_enable();
bt_sniff_feature_init();
sys_auto_sniff_controle(MY_SNIFF_EN, NULL);
app_var.dev_volume = -1;
break;
case ACTION_A2DP_START: //蓝牙音频传输协议
xlog("ACTION_A2DP_START\n");
break;
case ACTION_BY_KEY_MODE:
xlog("ACTION_BY_KEY_MODE\n");
break;
case ACTION_TONE_PLAY:
xlog("ACTION_TONE_PLAY\n");
// STATUS *p_tone = get_tone_config();
// tone_play_index(p_tone->bt_init_ok, 1);
break;
case ACTION_DO_NOTHING:
xlog("ACTION_DO_NOTHING\n");
break;
}
break;
case APP_STA_PAUSE:
xlog("APP_STA_PAUSE\n");
break;
case APP_STA_RESUME:
xlog("APP_STA_RESUME\n");
//恢复前台运行
sys_auto_shut_down_disable();
sys_key_event_enable();
break;
case APP_STA_STOP:
xlog("APP_STA_STOP\n");
break;
case APP_STA_DESTROY:
xlog("APP_STA_DESTROY\n");
r_printf("APP_STA_DESTROY\n");
if (!app_var.goto_poweroff_flag) {
bt_app_exit(NULL);
}
break;
}
xlog("state machine error\n");
return error;
}
///////////////////////////////////////////////////////////////////////////////////////////////////
//handle
static void play_poweron_ok_timer(void *priv)
{
app_var.wait_timer_do = 0;
log_d("\n-------play_poweron_ok_timer-------\n", priv);
if (is_dac_power_off()) {
#if TCFG_USER_TWS_ENABLE
bt_tws_poweron();
#else
bt_wait_connect_and_phone_connect_switch(0);
#endif
return;
}
app_var.wait_timer_do = sys_timeout_add(priv, play_poweron_ok_timer, 100);
}
static void play_bt_connect_dly(void *priv)
{
app_var.wait_timer_do = 0;
log_d("\n-------play_bt_connect_dly-------\n", priv);
if (!app_var.goto_poweroff_flag) {
STATUS *p_tone = get_tone_config();
tone_play_index(p_tone->bt_connect_ok, 1);
}
}
static int bt_connction_status_event_handler(struct bt_event *bt)
{
STATUS *p_tone = get_tone_config();
u8 *phone_number = NULL;
switch (bt->event) {
case BT_STATUS_INIT_OK:
/*
* 蓝牙初始化完成
*/
xlog("BT_STATUS_INIT_OK\n");
init_ok = 1;
__set_sbc_cap_bitpool(38);
#if (TCFG_USER_BLE_ENABLE)
if (BT_MODE_IS(BT_BQB)) {
ble_bqb_test_thread_init();
} else {
#if !TCFG_WIRELESS_MIC_ENABLE
bt_ble_init();
#endif
}
#endif
bt_init_ok_search_index();
#if TCFG_TEST_BOX_ENABLE
testbox_set_bt_init_ok(1);
#endif
#if ((CONFIG_BT_MODE == BT_BQB)||(CONFIG_BT_MODE == BT_PER))
bt_wait_phone_connect_control(1);
#else
if (is_dac_power_off()) {
bt_wait_connect_and_phone_connect_switch(0);
} else {
app_var.wait_timer_do = sys_timeout_add(NULL, play_poweron_ok_timer, 100);
}
#endif
/*if (app_var.play_poweron_tone) {
tone_play_index(p_tone->power_on, 1);
}*/
break;
case BT_STATUS_SECOND_CONNECTED:
clear_current_poweron_memory_search_index(0);
case BT_STATUS_FIRST_CONNECTED:
xlog("BT_STATUS_CONNECTED\n");
xtell_bl_state = 1; //蓝牙连接成功 置1
if(strcmp(xt_ble_new_name,"CM-1111") != 0){
//蓝牙连接成功
bt_newname =1;
u8 temp[5]={0xBB,0xBE,0x02,0x04,0x00};
temp[4] = xtell_bl_state; //经典蓝牙连接状态
send_data_to_ble_client(&temp,5);
}
earphone_change_pwr_mode(PWR_DCDC15, 3000);
sys_auto_shut_down_disable();
ui_update_status(STATUS_BT_CONN); //单台在此处设置连接状态,对耳的连接状态需要同步，在bt_tws.c中去设置
/* tone_play(TONE_CONN); */
/*os_time_dly(40); // for test*/
xlog("tone status:%d\n", tone_get_status());
if (get_call_status() == BT_CALL_HANGUP) {
if (app_var.phone_dly_discon_time) {
sys_timeout_del(app_var.phone_dly_discon_time);
app_var.phone_dly_discon_time = 0;
} else {
app_var.wait_timer_do = sys_timeout_add(NULL, play_bt_connect_dly, 1600);
/* tone_play_index(p_tone->bt_connect_ok, 1); */
}
}
/*int timeout = 5000 + rand32() % 10000;
sys_timeout_add(NULL, connect_phone_test, timeout);*/
break;
case BT_STATUS_FIRST_DISCONNECT:
case BT_STATUS_SECOND_DISCONNECT:
xlog("BT_STATUS_DISCONNECT\n");
xtell_bl_state = 0; //断开蓝牙 清0
//蓝牙断开连接
if(bt_newname){ //已经改成新蓝牙名字，断开才播报
bt_newname=0;
u8 temp[5]={0xBB,0xBE,0x02,0x04,0x00};
temp[4] = xtell_bl_state; //经典蓝牙连接状态
send_data_to_ble_client(&temp,5);
}
if (app_var.goto_poweroff_flag) {
/*关机不播断开提示音*/
/*关机时不改UI*/
break;
}
// bt_discon_dly_handle(NULL);
break;
//phone status deal
case BT_STATUS_PHONE_INCOME:
break;
case BT_STATUS_PHONE_OUT:
break;
case BT_STATUS_PHONE_ACTIVE:
break;
case BT_STATUS_PHONE_HANGUP:
break;
case BT_STATUS_PHONE_NUMBER:
break;
case BT_STATUS_INBAND_RINGTONE: //铃声
break;
case BT_STATUS_CALL_VOL_CHANGE:
break;
case BT_STATUS_SNIFF_STATE_UPDATE:
xlog(" BT_STATUS_SNIFF_STATE_UPDATE %d\n", bt->value); //0退出SNIFF
if (bt->value == 0) {
sniff_out = 1;
sys_auto_sniff_controle(MY_SNIFF_EN, bt->args);
} else {
sys_auto_sniff_controle(0, bt->args);
}
break;
case BT_STATUS_LAST_CALL_TYPE_CHANGE:
break;
case BT_STATUS_CONN_A2DP_CH:
case BT_STATUS_CONN_HFP_CH:
if ((!is_1t2_connection()) && (get_current_poweron_memory_search_index(NULL))) { //回连下一个device
if (get_esco_coder_busy_flag()) {
clear_current_poweron_memory_search_index(0);
} else {
user_send_cmd_prepare(USER_CTRL_START_CONNECTION, 0, NULL);
}
}
break;
case BT_STATUS_PHONE_MANUFACTURER:
break;
case BT_STATUS_VOICE_RECOGNITION:
break;
case BT_STATUS_AVRCP_INCOME_OPID:
xlog("BT_STATUS_AVRCP_INCOME_OPID:%d\n", bt->value);
break;
default:
xlog(" BT STATUS DEFAULT\n");
break;
}
return 0;
}
static int event_handler(struct application *app, struct sys_event *event)
{
if (SYS_EVENT_REMAP(event)) {
g_printf("****SYS_EVENT_REMAP**** \n");
return 0;
}
switch (event->type) {
case SYS_KEY_EVENT:
break;
case SYS_BT_EVENT:
/*
* 蓝牙事件处理
*/
if ((u32)event->arg == SYS_BT_EVENT_TYPE_CON_STATUS) {
printf("in event_handler:bt_connction_status_event_handler");
bt_connction_status_event_handler(&event->u.bt);
} else if ((u32)event->arg == SYS_BT_EVENT_TYPE_HCI_STATUS) {
bt_hci_event_handler(&event->u.bt);
}
break;
case SYS_DEVICE_EVENT:
/*
* 系统设备事件处理
*/
if ((u32)event->arg == DEVICE_EVENT_FROM_CHARGE) {
} else if ((u32)event->arg == DEVICE_EVENT_FROM_POWER) {
return app_power_event_handler(&event->u.dev);
}
#if TCFG_UMIDIGI_BOX_ENABLE
else if ((u32)event->arg == DEVICE_EVENT_UMIDIGI_CHARGE_STORE) {
app_umidigi_chargestore_event_handler(&event->u.umidigi_chargestore);
}
#endif
#if TCFG_TEST_BOX_ENABLE
else if ((u32)event->arg == DEVICE_EVENT_TEST_BOX) {
app_testbox_event_handler(&event->u.testbox);
}
#endif
break;
default:
return false;
}
SYS_EVENT_HANDLER_SPECIFIC(event);
#ifdef CONFIG_BT_BACKGROUND_ENABLE
if (app) {
default_event_handler(event);
}
#endif
return false;
}
///////////////////////////////////////////////////////////////////////////////////////////////////
static const struct application_operation app_handler_ops = {
.state_machine = state_machine,
.event_handler = event_handler,
};
/*
* 注册earphone模式
*/
REGISTER_APPLICATION(app_handler) = {
.name = "handler",
.action = ACTION_EARPHONE_MAIN,
.ops = &app_handler_ops,
.state = APP_STA_DESTROY,
};

10
cpu/br28/lp_touch_key_tool.c
View File

@ -392,11 +392,11 @@ int lp_touch_key_online_debug_exit(void)
extern u8 testbox_get_key_action_test_flag(void *priv);
extern void eartch_state_update(u8 state);
__attribute__((weak))
u32 user_send_cmd_prepare(USER_CMD_TYPE cmd, u16 param_len, u8 *param)
{
return 0;
}
// __attribute__((weak))
// u32 user_send_cmd_prepare(USER_CMD_TYPE cmd, u16 param_len, u8 *param)
// {
// return 0;
// }
u8 lp_touch_key_testbox_remote_test(u8 ch, u8 event)
{

1
cpu/br28/sdk.ld
View File

@ -15,6 +15,7 @@
lp_signature_set = ABSOLUTE(0x1fd6c);
memmem = ABSOLUTE(0x1fd70);
memcpy = ABSOLUTE(0x1fd74);

1
cpu/br28/sdk_used_list.used
View File

@ -14,6 +14,7 @@
sdfile_vfs_ops
sbc_decoder

1
cpu/br28/tools/AC69.key Normal file
View File

@ -0,0 +1 @@
1cfdc466ec927dbedd4d12447b6bbff1da1bd6ddb6b375ecda1bd6ddb6b375ec950b1206

BIN
cpu/br28/tools/aec.bin
View File

Binary file not shown.

BIN
cpu/br28/tools/app.bin
View File

Binary file not shown.

BIN
cpu/br28/tools/data_code.bin
View File

Binary file not shown.

4
cpu/br28/tools/download.bat
View File

@ -15,12 +15,14 @@
@echo off
Setlocal enabledelayedexpansion
@echo ********************************************************************************
@echo SDK BR28
@echo ********************************************************************************
@echo %date%
set KEY_FILE=-key AC69.key
cd /d %~dp0
@ -59,4 +61,4 @@ copy /b text.bin + data.bin + mov_slot.bin + data_code.bin + aec.bin + aac.bin +
del !bankfiles! common.bin text.bin data.bin bank.bin
copy eq_cfg_hw_less.bin eq_cfg_hw.bin
call download/earphone/download.bat
call download/earphone/download_app_ota.bat

1
cpu/br28/tools/download.c
View File

@ -93,6 +93,7 @@ Setlocal enabledelayedexpansion
@echo SDK BR28
@echo ********************************************************************************
@echo %date%
set KEY_FILE=-key AC69.key
cd /d %~dp0

1
cpu/br28/tools/download/earphone/646-AC690X-7603.key Normal file
View File

@ -0,0 +1 @@
64F3350FE2590FAF79755623B7E159CE83FAD97C34014E5EB2B528F6D6C2DABAB3B7C88C

14
cpu/br28/tools/download/earphone/download.bat
View File

@ -10,14 +10,14 @@ copy ..\..\ota.bin .
copy ..\..\anc_coeff.bin .
copy ..\..\anc_gains.bin .
..\..\isd_download.exe ..\..\isd_config.ini -tonorflash -dev br28 -boot 0x120000 -div8 -wait 300 -uboot ..\..\uboot.boot -app ..\..\app.bin -res ..\..\cfg_tool.bin tone.cfg p11_code.bin ..\..\eq_cfg_hw.bin -uboot_compress -key AC69.key -format all -key AC690X-8029.key
..\..\isd_download.exe ..\..\isd_config.ini -tonorflash -dev br28 -boot 0x120000 -div8 -wait 300 -uboot ..\..\uboot.boot -app ..\..\app.bin -res ..\..\cfg_tool.bin tone.cfg p11_code.bin ..\..\eq_cfg_hw.bin -uboot_compress -key AC69.key -format all -key 646-AC690X-7603.key
@REM..\..\isd_download.exe ..\..\isd_config.ini -tonorflash -dev br34 -boot 0x20000 -div8 -wait 300 -uboot ..\..\uboot.boot -app ..\..\app.bin ..\..\cfg_tool.bin -res tone.cfg kws_command.bin p11_code.bin -uboot_compress
:: -format all
::-reboot 2500
@rem ɾ<><C9BE><EFBFBD><EFBFBD>ʱ<EFBFBD>ļ<EFBFBD>-format all
@rem ɾ<><C9BE><EFBFBD><EFBFBD>ʱ<EFBFBD>ļ<EFBFBD>-format all
if exist *.mp3 del *.mp3
if exist *.PIX del *.PIX
if exist *.TAB del *.TAB
@ -28,7 +28,7 @@ if exist *.sty del *.sty
copy jl_isd.ufw update.ufw
del jl_isd.ufw
@REM <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ļ<EFBFBD><C4BC><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ļ<EFBFBD>
@REM <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ļ<EFBFBD><C4BC><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ļ<EFBFBD>
::ufw_maker.exe -chip AC800X %ADD_KEY% -output config.ufw -res bt_cfg.cfg
::IF EXIST jl_696x.bin del jl_696x.bin
@ -40,10 +40,10 @@ if exist br28loader.bin del br28loader.bin
if exist anc_coeff.bin del anc_coeff.bin
if exist anc_gains.bin del anc_gains.bin
@rem <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>˵<EFBFBD><CBB5>
@rem -format vm //<2F><><EFBFBD><EFBFBD>VM <20><><EFBFBD><EFBFBD>
@rem -format cfg //<2F><><EFBFBD><EFBFBD>BT CFG <20><><EFBFBD><EFBFBD>
@rem -format 0x3f0-2 //<2F><>ʾ<EFBFBD>ӵ<EFBFBD> 0x3f0 <20><> sector <20><>ʼ<EFBFBD><CABC><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> 2 <20><> sector(<28><>һ<EFBFBD><D2BB><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϊ16<31><36><EFBFBD>ƻ<EFBFBD>10<31><30><EFBFBD>ƶ<EFBFBD><C6B6>ɣ<EFBFBD><C9A3>ڶ<EFBFBD><DAB6><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>10<31><30><EFBFBD><EFBFBD>)
@rem <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>˵<EFBFBD><CBB5>
@rem -format vm //<2F><><EFBFBD><EFBFBD>VM <20><><EFBFBD><EFBFBD>
@rem -format cfg //<2F><><EFBFBD><EFBFBD>BT CFG <20><><EFBFBD><EFBFBD>
@rem -format 0x3f0-2 //<2F><>ʾ<EFBFBD>ӵ<EFBFBD> 0x3f0 <20><> sector <20><>ʼ<EFBFBD><CABC><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> 2 <20><> sector(<28><>һ<EFBFBD><D2BB><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϊ16<31><36><EFBFBD>ƻ<EFBFBD>10<31><30><EFBFBD>ƶ<EFBFBD><C6B6>ɣ<EFBFBD><C9A3>ڶ<EFBFBD><DAB6><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>10<31><30><EFBFBD><EFBFBD>)
ping /n 2 127.1>null
IF EXIST null del null

14
cpu/br28/tools/download/earphone/download_app_ota.bat
View File

@ -10,14 +10,14 @@ copy ..\..\ota.bin .
copy ..\..\anc_coeff.bin .
copy ..\..\anc_gains.bin .
..\..\isd_download.exe ..\..\isd_config.ini -tonorflash -dev br28 -boot 0x120000 -div8 -wait 300 -uboot ..\..\uboot.boot -app ..\..\app.bin -res ..\..\cfg_tool.bin tone.cfg p11_code.bin ..\..\eq_cfg_hw.bin -uboot_compress -key AC690X-8029.key
..\..\isd_download.exe ..\..\isd_config.ini -tonorflash -dev br28 -boot 0x120000 -div8 -wait 300 -uboot ..\..\uboot.boot -app ..\..\app.bin -res ..\..\cfg_tool.bin tone.cfg p11_code.bin ..\..\eq_cfg_hw.bin -uboot_compress -key 646-AC690X-7603.key
@REM..\..\isd_download.exe ..\..\isd_config.ini -tonorflash -dev br34 -boot 0x20000 -div8 -wait 300 -uboot ..\..\uboot.boot -app ..\..\app.bin ..\..\cfg_tool.bin -res tone.cfg kws_command.bin p11_code.bin -uboot_compress
:: -format all
::-reboot 2500
@rem ɾ<><C9BE><EFBFBD><EFBFBD>ʱ<EFBFBD>ļ<EFBFBD>-format all
@rem ɾ<><C9BE><EFBFBD><EFBFBD>ʱ<EFBFBD>ļ<EFBFBD>-format all
if exist *.mp3 del *.mp3
if exist *.PIX del *.PIX
if exist *.TAB del *.TAB
@ -28,7 +28,7 @@ if exist *.sty del *.sty
copy jl_isd.ufw update.ufw
del jl_isd.ufw
@REM <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ļ<EFBFBD><C4BC><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ļ<EFBFBD>
@REM <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ļ<EFBFBD><C4BC><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ļ<EFBFBD>
::ufw_maker.exe -chip AC800X %ADD_KEY% -output config.ufw -res bt_cfg.cfg
::IF EXIST jl_696x.bin del jl_696x.bin
@ -40,10 +40,10 @@ if exist br28loader.bin del br28loader.bin
if exist anc_coeff.bin del anc_coeff.bin
if exist anc_gains.bin del anc_gains.bin
@rem <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>˵<EFBFBD><CBB5>
@rem -format vm //<2F><><EFBFBD><EFBFBD>VM <20><><EFBFBD><EFBFBD>
@rem -format cfg //<2F><><EFBFBD><EFBFBD>BT CFG <20><><EFBFBD><EFBFBD>
@rem -format 0x3f0-2 //<2F><>ʾ<EFBFBD>ӵ<EFBFBD> 0x3f0 <20><> sector <20><>ʼ<EFBFBD><CABC><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> 2 <20><> sector(<28><>һ<EFBFBD><D2BB><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϊ16<31><36><EFBFBD>ƻ<EFBFBD>10<31><30><EFBFBD>ƶ<EFBFBD><C6B6>ɣ<EFBFBD><C9A3>ڶ<EFBFBD><DAB6><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>10<31><30><EFBFBD><EFBFBD>)
@rem <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>˵<EFBFBD><CBB5>
@rem -format vm //<2F><><EFBFBD><EFBFBD>VM <20><><EFBFBD><EFBFBD>
@rem -format cfg //<2F><><EFBFBD><EFBFBD>BT CFG <20><><EFBFBD><EFBFBD>
@rem -format 0x3f0-2 //<2F><>ʾ<EFBFBD>ӵ<EFBFBD> 0x3f0 <20><> sector <20><>ʼ<EFBFBD><CABC><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> 2 <20><> sector(<28><>һ<EFBFBD><D2BB><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϊ16<31><36><EFBFBD>ƻ<EFBFBD>10<31><30><EFBFBD>ƶ<EFBFBD><C6B6>ɣ<EFBFBD><C9A3>ڶ<EFBFBD><DAB6><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>10<31><30><EFBFBD><EFBFBD>)
ping /n 2 127.1>null
IF EXIST null del null

BIN
cpu/br28/tools/download/earphone/jl_isd.bin
View File

Binary file not shown.

BIN
cpu/br28/tools/download/earphone/jl_isd.fw
View File

Binary file not shown.

BIN
cpu/br28/tools/download/earphone/update.ufw
View File

Binary file not shown.

BIN
cpu/br28/tools/sdk.elf
View File

Binary file not shown.

BIN
cpu/br28/tools/sdk.elf.0.0.preopt.bc
View File

Binary file not shown.

BIN
cpu/br28/tools/sdk.elf.0.2.internalize.bc
View File

Binary file not shown.

BIN
cpu/br28/tools/sdk.elf.0.4.opt.bc
View File

Binary file not shown.

BIN
cpu/br28/tools/sdk.elf.0.5.precodegen.bc
View File

Binary file not shown.

BIN
cpu/br28/tools/sdk.elf.o
View File

Binary file not shown.

2
cpu/br28/tools/sdk.elf.objs.txt
View File

File diff suppressed because one or more lines are too long

1335
cpu/br28/tools/sdk.elf.resolution.txt
View File

File diff suppressed because it is too large Load Diff

390402
cpu/br28/tools/sdk.lst
View File

File diff suppressed because it is too large Load Diff

1969
cpu/br28/tools/sdk.map
View File

File diff suppressed because it is too large Load Diff

BIN
objs/apps/common/audio/amplitude_statistic.c.o
View File

Binary file not shown.

BIN
objs/apps/common/audio/audio_dvol.c.o
View File

Binary file not shown.

BIN
objs/apps/common/audio/audio_export_demo.c.o
View File

Binary file not shown.

BIN
objs/apps/common/audio/audio_noise_gate.c.o
View File

Binary file not shown.

BIN
objs/apps/common/audio/audio_ns.c.o
View File

Binary file not shown.

BIN
objs/apps/common/audio/audio_plc.c.o
View File

Binary file not shown.

BIN
objs/apps/common/audio/audio_utils.c.o
View File

Binary file not shown.

BIN
objs/apps/common/audio/decode/audio_key_tone.c.o
View File

Binary file not shown.

BIN
objs/apps/common/audio/decode/decode.c.o
View File

Binary file not shown.

BIN
objs/apps/common/audio/demo/audio_demo.c.o
View File

Binary file not shown.

BIN
objs/apps/common/audio/online_debug/aud_data_export.c.o
View File

Binary file not shown.

BIN
objs/apps/common/audio/online_debug/aud_mic_dut.c.o
View File

Binary file not shown.

BIN
objs/apps/common/audio/online_debug/aud_spatial_effect_dut.c.o
View File

Binary file not shown.

BIN
objs/apps/common/audio/online_debug/audio_online_debug.c.o
View File

Binary file not shown.

BIN
objs/apps/common/audio/sine_make.c.o
View File

Binary file not shown.

BIN
objs/apps/common/audio/uartPcmSender.c.o
View File

Binary file not shown.

BIN
objs/apps/common/audio/wm8978/iic.c.o
View File

Binary file not shown.

BIN
objs/apps/common/audio/wm8978/wm8978.c.o
View File

Binary file not shown.

BIN
objs/apps/common/bt_common/bt_test_api.c.o
View File

Binary file not shown.

BIN
objs/apps/common/cJSON/cJSON.c.o
View File

Binary file not shown.

BIN
objs/apps/common/colorful_lights/colorful_lights.c.o
View File

Binary file not shown.

BIN
objs/apps/common/config/app_config.c.o
View File

Binary file not shown.

BIN
objs/apps/common/config/bt_profile_config.c.o
View File

Binary file not shown.

BIN
objs/apps/common/config/ci_transport_uart.c.o
View File

Binary file not shown.

BIN
objs/apps/common/config/new_cfg_tool.c.o
View File

Binary file not shown.

BIN
objs/apps/common/debug/debug.c.o
View File

Binary file not shown.

BIN
objs/apps/common/debug/debug_lite.c.o
View File

Binary file not shown.

BIN
objs/apps/common/dev_manager/dev_manager.c.o
View File

Binary file not shown.

BIN
objs/apps/common/dev_manager/dev_reg.c.o
View File

Binary file not shown.

BIN
objs/apps/common/dev_manager/dev_update.c.o
View File

Binary file not shown.

BIN
objs/apps/common/device/gSensor/SC7A20.c.o
View File

Binary file not shown.

BIN
objs/apps/common/device/gSensor/STK8321.c.o
View File

Binary file not shown.

BIN
objs/apps/common/device/gSensor/da230.c.o
View File

Binary file not shown.

BIN
objs/apps/common/device/gSensor/gSensor_manage.c.o
View File

Binary file not shown.

BIN
objs/apps/common/device/gSensor/mpu6050.c.o
View File

Binary file not shown.

BIN
objs/apps/common/device/gx8002_npu/gx8002_enc/gx8002_enc.c.o
View File

Binary file not shown.

BIN
objs/apps/common/device/gx8002_npu/gx8002_npu.c.o
View File

Binary file not shown.

BIN
objs/apps/common/device/gx8002_npu/gx8002_npu_event_deal.c.o
View File

Binary file not shown.

BIN
objs/apps/common/device/gx8002_npu/gx8002_upgrade/app_upgrade/gx_uart_upgrade_app.c.o
View File

Binary file not shown.

BIN
objs/apps/common/device/gx8002_npu/gx8002_upgrade/app_upgrade/gx_uart_upgrade_tws.c.o
View File

Binary file not shown.

Some files were not shown because too many files have changed in this diff Show More