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

Compare commits

base: lmx:f3710fbb4b00e819e65bff842927cc0b24970ecd
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:46d6aefc9bb15516527c0be648ddad9838f8b12f
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

3 Commits
f3710fbb4b ... 46d6aefc9b

Author SHA1 Message Date
lmx
46d6aefc9b 临时存档 2025-11-24 18:58:32 +08:00
lmx
e19ac5ad00 cun 2025-11-24 16:33:33 +08:00
lmx
eb9de783ed 存档 2025-11-24 13:55:10 +08:00
62 changed files with 3168 additions and 11760 deletions
Expand all files Collapse all files

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

@ -224,6 +224,7 @@ void i2c_scanner_probe(void)
// 构建8位的写地址
uint8_t write_addr_8bit = (addr_7bit << 1);
//传入使用的iic句柄编号
iic_start(gSensor_info->iic_hdl);
// 尝试发送写地址，并检查返回值
@ -237,6 +238,7 @@ void i2c_scanner_probe(void)
devices_found++;
}
//传入使用的iic句柄编号
iic_stop(gSensor_info->iic_hdl);
delay(gSensor_info->iic_delay); // 短暂延时
}

0
apps/earphone/remote_control/RC_app_main.c Normal file
View File

89
apps/earphone/remote_control/RFID/include/CPU_CARD.h Normal file
View File

@ -0,0 +1,89 @@
#ifndef CPU_CARD_H
#define CPU_CARD_H 1
#include "rfid_main.h" // 包含 transmission_struct 的定义
/**
* @brief 存储ATS (Answer to Select) 信息的结构体
*/
struct ATS_STR
{
unsigned char Length; /**< ATS数据长度 */
unsigned char Ats_Data[255]; /**< ATS数据缓冲区 */
};
/**
* @brief 存储PPS (Protocol and Parameter Selection) 信息的结构体
*/
struct PPS_STR
{
unsigned char Length; /**< PPS数据长度 */
unsigned char Pps_Data[1]; /**< PPS数据缓冲区 */
};
/**
* @brief 存储CPU卡通信参数的结构体
*/
struct CPU_CARD_STR
{
unsigned char FSCI; /**< Frame Size for proximity coupling Integer */
unsigned char FSC; /**< Frame Size for proximity coupling (in bytes) */
unsigned char FWI; /**< Frame Waiting time Integer */
unsigned int FWT; /**< Frame Waiting Time (in ms) */
unsigned char SFGI; /**< Start-up Frame Guard time Integer */
unsigned char TA; /**< TA(1) parameter from ATS */
unsigned char TB; /**< TB(1) parameter from ATS */
unsigned char TC; /**< TC(1) parameter from ATS */
unsigned char PCB; /**< Protocol Control Byte */
unsigned char WTXM; /**< Waiting Time eXtension Multiplier */
struct ATS_STR ATS; /**< ATS信息 */
struct PPS_STR PPS; /**< PPS信息 */
};
extern struct CPU_CARD_STR CPU_CARD;
/**
* @brief 解析ATS (Answer to Select) 数据。
* @param ats_len [in] ATS数据的长度。
* @param ats [in] 指向ATS数据的指针。
* @return 操作状态，SUCCESS表示成功。
*/
extern unsigned char Ats_Process( unsigned char ats_len, unsigned char *ats );
/**
* @brief CPU卡事件处理函数（示例）。
* @return 操作状态，SUCCESS表示成功，FAIL表示失败。
*/
extern unsigned char CPU_CARD_EVENT( void );
/**
* @brief 封装了重试逻辑的TPDU传输函数。
* @param tpdu [in, out] 指向传输结构体的指针。
* @return 操作状态。
*/
extern unsigned char CPU_TPDU( transmission_struct *tpdu );
/**
* @brief 发送RATS (Request for Answer to Select) 命令。
* @param ats_len [out] 指向用于存储ATS长度的变量的指针。
* @param ats [out] 指向用于存储ATS数据的缓冲区的指针。
* @return 操作状态，SUCCESS表示成功。
*/
extern unsigned char CPU_Rats( unsigned char *ats_len, unsigned char *ats );
/**
* @brief 发送NAK (Negative Acknowledge) 响应。
* @param tpdu [in, out] 指向传输结构体的指针。
* @return 操作状态。
*/
extern unsigned char CPU_NAK( transmission_struct *tpdu );
/**
* @brief 发送APDU (Application Protocol Data Unit) 命令。
* @param apdu [in, out] 指向传输结构体的指针，包含APDU命令和响应。
* @return 操作状态。
*/
extern unsigned char CPU_APDU( transmission_struct *apdu );
#endif

97
apps/earphone/remote_control/RFID/include/MIFARE.h Normal file
View File

@ -0,0 +1,97 @@
#ifndef _MIFARE_H_
#define _MIFARE_H_
// 定义Mifare认证密钥类型
#define KEY_A_M1 0
#define KEY_B_M1 1
// 声明全局变量
extern unsigned char SECTOR,BLOCK,BLOCK_NUM;
extern unsigned char BLOCK_DATA[16];
extern unsigned char KEY_A[16][6];
extern unsigned char KEY_B[16][6];
/**
* @brief Mifare卡事件处理函数（示例）。
* @return 操作状态，SUCCESS表示成功，FAIL表示失败。
*/
extern unsigned char MIFARE_CARD_EVENT(void);
/**
* @brief 清除Mifare卡加密认证标志。
* @return 无。
*/
extern void Mifare_Clear_Crypto(void);
/**
* @brief 将6字节的Mifare密钥加载到芯片的密钥缓冲区。
* @param mifare_key [in] 指向6字节密钥数组的指针。
* @return 操作状态，SUCCESS表示成功，FAIL表示失败。
*/
unsigned char Mifare_LoadKey(unsigned char *mifare_key);
/**
* @brief 执行Mifare卡的传输（Transfer）命令。
* @param block [in] 块号。
* @return 操作状态，SUCCESS表示成功，FAIL表示失败。
*/
extern unsigned char Mifare_Transfer(unsigned char block);
/**
* @brief 执行Mifare卡的恢复（Restore）命令。
* @param block [in] 块号。
* @return 操作状态，SUCCESS表示成功，FAIL表示失败。
*/
extern unsigned char Mifare_Restore(unsigned char block);
/**
* @brief 将4字节数据格式化为Mifare值块格式并写入指定块。
* @param block [in] 目标块号。
* @param data_buff [in] 指向4字节源数据的指针。
* @return 操作状态，SUCCESS表示成功，FAIL表示失败。
*/
extern unsigned char Mifare_Blockset(unsigned char block,unsigned char *data_buff);
/**
* @brief 对Mifare卡的指定值块执行增值操作。
* @param block [in] 值块的块号。
* @param data_buff [in] 指向4字节增值数据的指针。
* @return 操作状态，SUCCESS表示成功，FAIL表示失败。
*/
extern unsigned char Mifare_Blockinc(unsigned char block,unsigned char *data_buff);
/**
* @brief 对Mifare卡的指定值块执行减值操作。
* @param block [in] 值块的块号。
* @param data_buff [in] 指向4字节减值数据的指针。
* @return 操作状态，SUCCESS表示成功，FAIL表示失败。
*/
extern unsigned char Mifare_Blockdec(unsigned char block,unsigned char *data_buff);
/**
* @brief 向Mifare卡写入一个16字节的数据块。
* @param block [in] 要写入的块号 (0x00 - 0x3F)。
* @param data_buff [in] 指向16字节数据的指针。
* @return 操作状态，SUCCESS表示成功，FAIL表示失败。
*/
extern unsigned char Mifare_Blockwrite(unsigned char block,unsigned char *data_buff);
/**
* @brief 从Mifare卡读取一个16字节的数据块。
* @param block [in] 要读取的块号 (0x00 - 0x3F)。
* @param data_buff [out] 指向16字节缓冲区的指针，用于存储读取的数据。
* @return 操作状态，SUCCESS表示成功，FAIL表示失败。
*/
extern unsigned char Mifare_Blockread(unsigned char block,unsigned char *data_buff);
/**
* @brief 对Mifare Classic卡片的指定扇区进行认证。
* @param key_mode [in] 认证模式，`KEY_A_M1` (0) 表示使用密钥A，`KEY_B_M1` (1) 表示使用密钥B。
* @param sector [in] 要认证的扇区号 (0-15)。
* @param mifare_key [in] 指向6字节认证密钥的指针。
* @param card_uid [in] 指向4字节卡片UID的指针。
* @return 操作状态，SUCCESS表示认证成功，FAIL表示失败。
*/
extern unsigned char Mifare_Auth(unsigned char key_mode,unsigned char sector,unsigned char *mifare_key,unsigned char *card_uid);
#endif

8
apps/earphone/remote_control/RFID/include/NTAG.h Normal file
View File

@ -0,0 +1,8 @@
#ifndef _NTAG_H
#define _NTAG_H
extern unsigned char PAGE_DATA[16];
extern unsigned char NTAG_EVENT(void);
extern unsigned char Read_Page(unsigned char page_num,unsigned char *page_data);
extern unsigned char Write_Page(unsigned char page_num,unsigned char *page_data);
#endif

153
apps/earphone/remote_control/RFID/include/READER.h Normal file
View File

@ -0,0 +1,153 @@
/********************************************************************************************************
* @file READER.h
* @brief RFID 读卡器底层驱动及协议头文件
* @details
* 本文件定义了与RFID芯片交互所需的常量、数据结构和函数原型。
*
* @author Kilo Code
* @date 2025-11-24
* @version 1.0
********************************************************************************************************/
#ifndef _READER_H
#define _READER_H
/********************************************************************************************************
* 常量定义
********************************************************************************************************/
// ISO14443A 命令码
static const unsigned char RF_CMD_REQA = 0x26; /**< 请求命令 */
static const unsigned char RF_CMD_WUPA = 0x52; /**< 唤醒命令 */
static const unsigned char RF_CMD_ANTICOLL[3] = {0x93, 0x95, 0x97}; /**< 防冲突命令，根据级联级别选择 */
static const unsigned char RF_CMD_SELECT[3] = {0x93, 0x95, 0x97}; /**< 选择命令，根据级联级别选择 */
// MIFARE Classic 命令码
static const unsigned char RF_CMD_KEYA = 0x60; /**< 密钥A认证 */
static const unsigned char RF_CMD_KEYB = 0x61; /**< 密钥B认证 */
/********************************************************************************************************
* 卡片信息结构体
********************************************************************************************************/
/**
* @brief 存储ISO/IEC 14443 Type B卡片信息的结构体
*/
struct picc_b_struct
{
unsigned char ATQB[12]; /**< REQB/WUPB的响应 (Answer to Request B) */
unsigned char PUPI[4]; /**< Pseudo-Unique PICC Identifier */
unsigned char APPLICATION_DATA[4]; /**< 应用数据 */
unsigned char PROTOCOL_INF[3]; /**< 协议信息 */
unsigned char CID; /**< 卡片ID (Card Identifier) */
unsigned char Answer_to_HALT[1]; /**< HALT命令的响应 */
unsigned char SN[8]; /**< 序列号 (自定义命令获取) */
};
extern struct picc_b_struct PICC_B;
/**
* @brief 存储ISO/IEC 14443 Type A卡片信息的结构体
*/
struct picc_a_struct
{
unsigned char ATQA[2]; /**< REQA/WUPA的响应 (Answer to Request A) */
unsigned char CASCADE_LEVEL; /**< 当前级联级别 (用于处理多级UID) */
unsigned char UID_Length; /**< UID的长度 (4, 7, or 10 bytes) */
unsigned char UID[15]; /**< 卡片唯一ID (Unique Identifier) */
unsigned char BCC[3]; /**< 块校验字符 (Block Check Character) */
unsigned char SAK[3]; /**< 选择确认 (Select Acknowledge) */
};
extern struct picc_a_struct PICC_A;
/**
* @brief 存储ISO/IEC 15693 (Type V) 卡片信息的结构体
*/
struct picc_v_struct
{
unsigned char UID[8]; /**< 卡片唯一ID (Unique Identifier) */
unsigned char RESPONSE; /**< 命令响应标志 */
unsigned char BLOCK_DATA[4]; /**< 读取或写入的块数据 */
};
extern struct picc_v_struct PICC_V;
/**
* @brief 存储FeliCa (Type F) 卡片信息的结构体
*/
struct picc_f_struct
{
unsigned char UID[8]; /**< 卡片唯一ID (Unique Identifier) */
};
extern struct picc_f_struct PICC_F;
/********************************************************************************************************
* 芯片参数配置
********************************************************************************************************/
// --- Type A 参数 ---
#define GAIN_A 7 // 接收增益 (范围 0~7)
#define HPCF_A 3 // 高通滤波器截止频率 (范围 0~7)
#define AMPLITUDE_A 255 // RF场幅度 (范围 0~255)
// --- Type B 参数 ---
#define GAIN_B 7 // 接收增益
#define HPCF_B 3 // 高通滤波器截止频率
#define AMPLITUDE_B 255 // RF场幅度
#define MODULATION_B 100 // 调制深度 (范围 0~255, 值越小调制越深)
// --- Type V (ISO15693) 参数 ---
#define GAIN_V 7 // 接收增益
#define HPCF_V 4 // 高通滤波器截止频率
#define AMPLITUDE_V 255 // RF场幅度
#define MODULATION_V 10 // 调制深度
// --- Type F (FeliCa) 参数 ---
#define GAIN_F 7 // 接收增益
#define HPCF_F 4 // 高通滤波器截止频率
#define AMPLITUDE_F 255 // RF场幅度
#define MODULATION_F 100 // 调制深度
/********************************************************************************************************
* 函数原型声明
********************************************************************************************************/
// --- 通用函数 ---
extern void ModifyReg(unsigned char reg_address, unsigned char mask, unsigned char set);
extern void Clear_FIFO(void);
extern unsigned char SetCommand(unsigned char command);
extern void SetParity(unsigned char state);
extern void SetTimer(unsigned int timeout);
extern unsigned char SetCW(unsigned char mode);
// --- 协议初始化函数 ---
extern unsigned char ReaderA_Initial(void);
extern unsigned char ReaderB_Initial(void);
extern unsigned char ReaderV_Initial(void);
extern unsigned char ReaderF_Initial(void);
// --- Type A 命令 ---
extern unsigned char ReaderA_Wakeeup(struct picc_a_struct *picc_a);
extern unsigned char ReaderA_Request(struct picc_a_struct *picc_a);
extern unsigned char ReaderA_Anticoll(struct picc_a_struct *picc_a);
extern unsigned char ReaderA_Select(struct picc_a_struct *picc_a);
extern unsigned char ReaderA_CardActivate(struct picc_a_struct *picc_a);
// --- Type B 命令 ---
extern unsigned char ReaderB_Wakeup(struct picc_b_struct *picc_b);
extern unsigned char ReaderB_Request(struct picc_b_struct *picc_b);
extern unsigned char ReaderB_Attrib(struct picc_b_struct *picc_b);
extern unsigned char ReaderB_Halt(struct picc_b_struct *picc_b);
extern unsigned char ReaderB_Get_SN(struct picc_b_struct *picc_b);
// --- Type V (ISO15693) 命令 ---
extern unsigned char ReaderV_Inventory(struct picc_v_struct *picc_v);
extern unsigned char ReaderV_Select(struct picc_v_struct *picc_v);
extern unsigned char ReaderV_ReadSingleBlock(unsigned char block_num, struct picc_v_struct *picc_v);
extern unsigned char ReaderV_WriteSingleBlock(unsigned char block_num, struct picc_v_struct *picc_v);
// --- Type F (FeliCa) 命令 ---
extern unsigned char ReaderF_Inventory(struct picc_f_struct *picc_f);
#endif // _READER_H

478
apps/earphone/remote_control/RFID/include/READER_REG.h Normal file
View File

@ -0,0 +1,478 @@
/*********************************************************************
* *
* Copyright (c) 2010 Shanghai FuDan MicroElectronic Inc, Ltd. *
* All rights reserved. Licensed Software Material. *
* *
* Unauthorized use, duplication, or distribution is strictly *
* prohibited by law. *
* *
**********************************************************************/
#ifndef _READER_REG_H
#define _READER_REG_H
#define REG_COMMAND 0x00 //
#define REG_HOSTCTRL 0x01 //
#define REG_FIFOCONTROL 0x02 //
#define REG_WATERLEVEL 0x03 //
#define REG_FIFOLENGTH 0x04 //
#define REG_FIFODATA 0x05 //
#define REG_IRQ0 0x06 //
#define REG_IRQ1 0x07 //
#define REG_IRQ0EN 0x08 //
#define REG_IRQ1EN 0x09 //
#define REG_ERROR 0x0A //
#define REG_STATUS 0x0B //
#define REG_RXBITCTRL 0x0C //
#define REG_RXCOLL 0x0D //
#define REG_TCONTROL 0x0E //
#define REG_T0CONTROL 0x0F //
#define REG_T0RELOADHI 0x10 //
#define REG_T0RELOADLO 0x11 //
#define REG_T0COUNTERVALHI 0x12 //
#define REG_T0COUNTERVALLO 0x13 //
#define REG_T1CONTROL 0x14 //
#define REG_T1RELOADHI 0x15 //
#define REG_T1RELOADLO 0x16 //
#define REG_T1COUNTERVALHI 0x17 //
#define REG_T1COUNTERVALLO 0x18 //
#define REG_T2CONTROL 0x19 //
#define REG_T2RELOADHI 0x1A //
#define REG_T2RELOADLO 0x1B //
#define REG_T2COUNTERVALHI 0x1C //
#define REG_T2COUNTERVALLO 0x1D //
#define REG_T3CONTROL 0x1E //
#define REG_T3RELOADHI 0x1F //
#define REG_T3RELOADLO 0x20 //
#define REG_T3COUNTERVALHI 0x21 //
#define REG_T3COUNTERVALLO 0x22 //
#define REG_T4CONTROL 0x23 //
#define REG_T4RELOADHI 0x24 //
#define REG_T4RELOADLO 0x25 //
#define REG_T4COUNTERVALHI 0x26 //
#define REG_T4COUNTERVALLO 0x27 //
#define REG_TXMODE 0x28
#define REG_TXAMP 0x29
#define REG_TXCON 0x2A //
#define REG_TXI 0x2B //
#define REG_TXCRCCON 0x2C //
#define REG_RXCRCCON 0x2D //
#define REG_TXDATANUM 0x2E
#define REG_TXMODWIDTH 0x2F //
#define REG_TXSYM10BURSTLEN 0x30 //
#define REG_TXWAITCTRL 0x31 //
#define REG_TXWAITLO 0x32 //
#define REG_FRAMECON 0x33 //
#define REG_RXSOFD 0x34 //
#define REG_RXCTRL 0x35 //
#define REG_RXWAIT 0x36 //
#define REG_RXTHRESHOLD 0x37 //
#define REG_RCV 0x38 //
#define REG_RXANA 0x39 //
#define REG_LPCD_OPTIONS 0x3A //
#define REG_SERIALSPEED 0x3B //
#define REG_LFO_TRIMM 0x3C //
#define REG_CLKOUT_CTRL 0x3D //
#define REG_LPCD_THRESHOLD 0x3E //
#define REG_LPCD_QMIN 0x3F //
#define REG_LPCD_QMAX 0x40
#define REG_LPCD_IMIN 0x41
#define REG_LPCD_RESULT_I 0x42
#define REG_LPCD_RESULT_Q 0x43
#define REG_THNADJ 0x5F
#define REG_THNSET 0x61
#define REG_THNMIN 0x62
#define REG_DSP_CTRL1 0x64
#define REG_MISC 0x75
#define REG_RXTXCON 0x77
#define REG_ERROREXT 0x7E
#define REG_VERSION 0x7F
#define CMD_MASK 0x1F
#define CMD_IDLE 0x00
#define CMD_LPCD 0x01
#define CMD_LOADKEY 0x02
#define CMD_AUTHENT 0x03
#define CMD_RECEIVE 0x05
#define CMD_TRANSMIT 0x06
#define CMD_TRANSCEIVE 0x07
#define CMD_WRITEE2 0x08
#define CMD_WRITEE2PAGE 0x09
#define CMD_READE2 0x0A
#define CMD_LOADREG 0x0C
#define CMD_LOADPROTOCOL 0x0D
#define CMD_LOADKEYE2 0x0E
#define CMD_STOREKEYE2 0x0F
#define CMD_CRCCALC 0x1B
#define CMD_READRNR 0x1C
#define CMD_SOFTRESET 0x1F
/** \name Host-Control Register Contents (0x00)
*/
/*@{*/
#define BIT_STANDBY 0x80U /**< Standby bit; If set, the IC transits to standby mode. */
#define BIT_MODEMOFF 0x40U
/*@{*/
/** \name Host-Control Register Contents (0x01)
*/
/*@{*/
#define BIT_I2CFORCEHS 0x01U
//#define BIT_REGEN 0x80U
//#define BIT_BUSHOST 0x40U
//#define BIT_BUSSAM 0x20U
//#define MASK_SAMINTERFACE 0x0CU
/*@}*/
/** \name FIFO-Control Register Contents (0x02)
*/
/*@{*/
#define BIT_FIFOSIZE 0x80U
#define BIT_HIALERT 0x40U
#define BIT_LOALERT 0x20U
#define BIT_FIFOFLUSH 0x10U
#define BIT_WATERLEVEL_HI 0x04U
#define MASK_FIFOLENGTH_HI 0x03U
/*@}*/
/** \name IRQ0 Register(s) Contents (0x06/0x08)
*/
/*@{*/
#define BIT_SET 0x80U
#define BIT_IRQINV 0x80U
#define BIT_HIALERTIRQ 0x40U
#define BIT_LOALERTIRQ 0x20U
#define BIT_IDLEIRQ 0x10U
#define BIT_TXIRQ 0x08U
#define BIT_RXIRQ 0x04U
#define BIT_ERRIRQ 0x02U
#define BIT_RXSOFIRQ 0x01U
/*@}*/
/** \name IRQ1 Register(s) Contents (0x07/0x09)
*/
/*@{*/
/* #define BIT_SET 0x80U */
#define BIT_IRQPUSHPULL 0x80U
#define BIT_GLOBALIRQ 0x40U
#define BIT_IRQPINEN 0x40U
#define BIT_LPCDIRQ 0x20U
#define BIT_TIMER4IRQ 0x10U
#define BIT_TIMER3IRQ 0x08U
#define BIT_TIMER2IRQ 0x04U
#define BIT_TIMER1IRQ 0x02U
#define BIT_TIMER0IRQ 0x01U
/*@}*/
/** \name Error Register Contents (0x0A)
*/
/*@{*/
#define BIT_CMDEE_ERR 0x80U
#define BIT_FIFOWRERR 0x40U
#define BIT_FIFOOVL 0x20U
#define BIT_MINFRAMEERR 0x10U
#define BIT_NODATAERR 0x08U
#define BIT_COLLDET 0x04U
#define BIT_PROTERR 0x02U
#define BIT_INTEGERR 0x01U
/*@}*/
/** \name Status Register Contents (0x0B)
*/
/*@{*/
#define BIT_CRYPTO1ON 0x20U
#define MASK_COMMSTATE 0x07U
/*@}*/
/** \name Rx-Bit-Control Register Contents (0x0C)
*/
/*@{*/
#define BIT_VALUESAFTERCOLL 0x80U
#define BIT_NOCOLL 0x08U
#define MASK_RXALIGN 0x70U
#define MASK_RXLASTBITS 0x07U
/*@}*/
/** \name Rx-Coll Register Contents (0x0D)
*/
/*@{*/
#define BIT_COLLPOSVALID 0x80U
#define MASK_COLLPOS 0x7FU
/*@}*/
/** \name Timer-Control Register Contents (0x0E)
*/
/*@{*/
#define BIT_T3RUNNING 0x80U
#define BIT_T2RUNNING 0x40U
#define BIT_T1RUNNING 0x20U
#define BIT_T0RUNNING 0x10U
#define BIT_T3STARTSTOPNOW 0x08U
#define BIT_T2STARTSTOPNOW 0x04U
#define BIT_T1STARTSTOPNOW 0x02U
#define BIT_T0STARTSTOPNOW 0x01U
/*@}*/
/** \name T[0-3]-Control Register Contents (0x0F/0x14/0x19/0x1E)
*/
/*@{*/
#define BIT_TSTOP_RX 0x80U /**< Stop timer on receive interrupt. */
#define BIT_TAUTORESTARTED 0x08U /**< Auto-restart timer after underflow. */
#define BIT_TSTART_TX 0x10U /**< Start timer on transmit interrupt. */
//#define BIT_TSTART_LFO 0x20U /**< Use this timer for LFO trimming. */
//#define BIT_TSTART_LFO_UV 0x30U /**< Use this timer for LFO trimming (generate UV at a trimming event). */
#define MASK_TSTART 0x30U /**< Mask for TSTART bits. */
#define VALUE_TCLK_1356_MHZ 0x00U /**< Use 13.56MHz as input clock. */
#define VALUE_TCLK_212_KHZ 0x01U /**< Use 212KHz as input clock. */
#define VALUE_TCLK_T0 0x02U /**< Use timer0 as input clock. */
#define VALUE_TCLK_T1 0x03U /**< Use timer1 as input clock. */
/*@}*/
/** \name T4-Control Register Contents (0x23)
*/
/*@{*/
#define BIT_T4RUNNING 0x80U
#define BIT_T4STARTSTOPNOW 0x40U
#define BIT_T4AUTOTRIMM 0x20U
#define BIT_T4AUTOLPCD 0x10U
#define BIT_T4AUTORESTARTED 0x08U
#define BIT_T4AUTOWAKEUP 0x04U
/*#define MASK_TSTART 0x30U*/
#define VALUE_TCLK_LFO_64_KHZ 0x00U
#define VALUE_TCLK_LFO_8_KHZ 0x01U
#define VALUE_TCLK_LFO_4_KHZ 0x02U
#define VALUE_TCLK_LFO_2_KHZ 0x03U
/*@}*/
/** \name Driver Mode Register Contents (0x28)
*/
/*@{*/
#define BIT_TX2INV 0x80U
#define BIT_TX1INV 0x40U
#define BIT_TXEN 0x08U
#define VALUE_TXCLKMODE_HIGHIMPEDANCE 0x00U
#define VALUE_TXCLKMODE_OUTPULL0 0x01U
#define VALUE_TXCLKMODE_OUTPULL1 0x02U
#define VALUE_TXCLKMODE_RFLOWPULL 0x05U
#define VALUE_TXCLKMODE_RFHIGHPUSH 0x06U
#define VALUE_TXCLKMODE_PUSHPULL 0x07U
#define BIT_RFON 0x04U
#define BIT_TPUSHON 0x02U
#define BIT_TPULLON 0x01U
/*@}*/
/** \name Tx Amplifier Register Contents (0x29)
*/
/*@{*/
#define MASK_CW_AMPLITUDE 0x00U
#define MASK_RESIDUAL_CARRIER 0x1FU
/*@}*/
/** \name Driver Control Register Contents (0x2A)
*/
/*@{*/
#define BIT_CWMAX 0x08U
#define BIT_DRIVERINV 0x04U
#define VALUE_DRIVERSEL_LOW 0x00U
#define VALUE_DRIVERSEL_TXENV 0x01U
#define VALUE_DRIVERSEL_SIGIN 0x02U
/*@}*/
/** \name Tx-/Rx-CRC Control Register Contents (0x2C/0x2D)
*/
/*@{*/
#define BIT_RXFORCECRCWRITE 0x80U
#define BIT_CRCINVERT 0x02U
#define BIT_CRCEN 0x01U
#define MASK_CRCPRESETVAL 0x70U
#define MASK_CRCTYPE 0x0CU
#define MASK_CRCTYPE5 0x00U
#define MASK_CRCTYPE16 0x08U
/*@}*/
/** \name Tx-DataNum Register Contents (0x2E)
*/
/*@{*/
#define BIT_KEEPBITGRID 0x10U
#define BIT_DATAEN 0x08U
#define MASK_TXLASTBITS 0x07U
#define MASK_SYMBOL_SEND 0x08U
/*@}*/
/** \name Tx-Wait Control Register Contents (0x31)
*/
/*@{*/
#define BIT_TXWAIT_START_RX 0x80U
#define BIT_TXWAIT_DBFREQ 0x40U
#define MASK_TXWAITHI 0x38U
#define MASK_TXSTOPBITLEN 0x07U
/*@}*/
/** \name Frame Control Register Contents (0x33)
*/
/*@{*/
#define BIT_TXPARITYEN 0x80U
#define BIT_RXPARITYEN 0x40U
#define VALUE_STOP_SYM3 0x0CU
#define VALUE_STOP_SYM2 0x08U
#define VALUE_STOP_SYM1 0x04U
#define VALUE_START_SYM3 0x03U
#define VALUE_START_SYM2 0x02U
#define VALUE_START_SYM1 0x01U
#define MASK_STARTSYM 0x03U
#define MASK_STOPSYM 0x0CU
/*@}*/
/** \name Rx Control Register Contents (0x35)
*/
/*@{*/
#define BIT_RXALLOWBITS 0x80U
#define BIT_RXMULTIPLE 0x40U
#define BIT_RXEOFTYPE 0x20U
#define BIT_EGT_CHECK 0x10U
#define BIT_EMD_SUPPRESSION 0x08U
#define MASK_RXBAUDRATE 0x07U
/*@}*/
/** \name Rx-Wait Register Contents (0x36)
*/
/*@{*/
#define BIT_RXWAITDBFREQ 0x80U
#define MASK_RXWAIT 0x7FU
/*@}*/
/** \name Rx-Threshold Register Contents (0x37)
*/
/*@{*/
#define MASK_MINLEVEL 0xF0U
#define MASK_MINLEVELP 0x0FU
/*@}*/
/** \name Rx-Receiver Register Contents (0x38)
*/
/*@{*/
#define BIT_RX_SINGLE 0x80U
#define BIT_RX_SHORT_MIX2ADC 0x40U
#define BIT_USE_SMALL_EVAL 0x04U
#define MASK_RX_SIGPRO_IN_SEL 0x30U
#define MASK_COLLLEVEL 0x03U
/*@}*/
/** \name Rx-Analog Register Contents (0x39)
*/
/*@{*/
#define BIT_RX_OC_FUN_ENABLE 0x20U
#define BIT_RX_HP_LOWF 0x10U
#define MASK_VMID_R_SEL 0xC0U
#define MASK_RCV_HPCF 0x0CU
#define MASK_RCV_GAIN 0x03U
/*@}*/
/** \name Serial-Speed Register Contents (0x3B)
*/
/*@{*/
#define MASK_BR_T0 0xE0U
#define MASK_BR_T1 0x1FU
/*@}*/
/** \name LPCD Result(Q) Register Contents (0x43)
*/
/*@{*/
#define BIT_LPCDIRQ_CLR 0x40U
/*@}*/
/** \name Tx-BitMod Register Contents (0x48)
*/
/*@{*/
#define BIT_TXMSBFIRST 0x80U
#define BIT_TXPARITYTYPE 0x20U
#define BIT_TXSTOPBITTYPE 0x08U
#define BIT_TXSTARTBITTYPE 0x02U
#define BIT_TXSTARTBITEN 0x01U
/*@}*/
/** \name Rx-BitMod Register Contents (0x58)
*/
/*@{*/
#define BIT_RXSTOPONINVPAR 0x20U
#define BIT_RXSTOPONLEN 0x10U
#define BIT_RXMSBFIRST 0x08U
#define BIT_RXSTOPBITEN 0x04U
#define BIT_RXPARITYTYPE 0x02U
/*@}*/
/** \name Rx-Mod Register Contents (0x5D)
*/
/*@{*/
#define BIT_PREFILTER 0x20U
#define BIT_RECTFILTER 0x10U
#define BIT_SYNCHIGH 0x08U
#define BIT_CORRINV 0x04U
#define BIT_FSK 0x02U
#define BIT_BPSK 0x01U
/*@}*/
/** \name RxSupCfg Register Contents (0x6E)
*/
/*@{*/
#define BIT_RXNOERR 0x80U
/*@}*/
/** \name RxTxConReg Register Contents (0x77)
*/
/*@{*/
#define BIT_SHMODE 0x08U //<2F>Ϻ<EFBFBD><CFBA>㷨
/*@}*/
/** \name ErrorExtReg Register Contents (0x7E)
*/
/*@{*/
#define PARITY_ERROR 0x08U
#define CRC_ERROR 0x04U
/*@{*/
#define LPCD_OPTION2 0x1DF
//---------------------------------------------------------------
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Э<EFBFBD><D0AD><EFBFBD>
#define RX_TYPEA_106 0
#define RX_TYPEA_212 1
#define RX_TYPEA_424 2
#define RX_TYPEA_848 3
#define RX_TYPEB_106 4
#define RX_TYPEB_212 5
#define RX_TYPEB_424 6
#define RX_TYPEB_848 7
#define RX_TYPEV_26 10
#define RX_TYPEV_53 11
#define RX_FELICA_212 19
#define RX_FELICA_424 20
//<2F><><EFBFBD>巢<EFBFBD><E5B7A2>Э<EFBFBD><D0AD><EFBFBD>
#define TX_TYPEA_106 0
#define TX_TYPEA_212 1
#define TX_TYPEA_424 2
#define TX_TYPEA_848 3
#define TX_TYPEB_106 4
#define TX_TYPEB_212 5
#define TX_TYPEB_424 6
#define TX_TYPEB_848 7
#define TX_TYPEV_26 10
#define TX_TYPEV_53 11
#define TX_FELICA_212 19
#define TX_FELICA_424 20
#endif

82
apps/earphone/remote_control/RFID/include/rfid_main.h Normal file
View File

@ -0,0 +1,82 @@
/********************************************************************************************************
* @file rfid_main.h
* @brief RFID 读卡器应用层主头文件
* @details
* 本文件定义了RFID应用层所需的数据结构、枚举类型和全局函数。
*
* @author Kilo Code
* @date 2025-11-24
* @version 1.0
********************************************************************************************************/
#ifndef _RFID_MAIN_H
#define _RFID_MAIN_H
// 包含项目的基础类型定义，如果您的项目中没有 "system/includes.h"，
// 请替换为包含 stdint.h 或类似的头文件以获取 u8, u16, u32 等类型定义。
#include "system/includes.h"
/**
* @brief 操作状态枚举
*/
typedef enum {
FAIL = 0U,
SUCCESS = !FAIL
} ErrorStatus;
/**
* @brief 功能使能状态枚举
*/
typedef enum {
FUN_DISABLE = 0U,
FUN_ENABLE = !FUN_DISABLE
} FunState;
/**
* @brief 标志位状态枚举
*/
typedef enum {
RESET = 0U,
SET = !RESET
} FlagStatus, ITStatus;
/**
* @brief 通用位宏定义
*/
#define BIT0 (1 << 0)
#define BIT1 (1 << 1)
#define BIT2 (1 << 2)
#define BIT3 (1 << 3)
#define BIT4 (1 << 4)
#define BIT5 (1 << 5)
#define BIT6 (1 << 6)
#define BIT7 (1 << 7)
/**
* @brief 数据传输结构体
* @details 用于在不同函数间传递发送和接收数据缓冲区及其长度信息。
*/
typedef struct
{
unsigned char SendLength; /**< 要发送的数据长度 */
unsigned char *pSendBuffer; /**< 指向发送数据缓冲区的指针 */
unsigned char ReceiveLength; /**< 接收到的数据长度 */
unsigned char *pReceiveBuffer; /**< 指向接收数据缓冲区的指针 */
unsigned int Timeout; /**< 操作超时时间（单位：毫秒） */
} transmission_struct;
/********************************************************************************************************
* 全局函数声明
********************************************************************************************************/
/**
* @brief RFID模块的主任务函数。
* @details
* 这是一个示例性的任务函数，展示了如何初始化RFID芯片并进入一个无限循环来轮询不同类型的卡片。
* 您可以将此函数作为一个独立的任务运行，或者将其中的逻辑集成到您现有的任务调度中。
*/
void rfid_task(void);
#endif // _RFID_MAIN_H

447
apps/earphone/remote_control/RFID/reader/CPU_CARD.c Normal file
View File

@ -0,0 +1,447 @@
#include "../include/READER.h"
#include "../include/CPU_CARD.h"
#include "../include/READER_REG.h"
#include "../include/rfid_main.h"
#include "../rfid_hal.h"
#define FUN_ENABLE_XLOG 1
#ifdef xlog
#undef xlog
#endif
#if FUN_ENABLE_XLOG
#define xlog(format, ...) printf("[XT:%s] " format, __func__, ##__VA_ARGS__)
#else
#define xlog(format, ...) ((void)0)
#endif
struct CPU_CARD_STR CPU_CARD;
// 声明一个静态函数用于TPDU传输，因为它只在本文件内部使用
static unsigned char FM176XX_TPDU(transmission_struct *tpdu);
/**
* @brief CPU卡事件处理函数（示例）。
* @details
* 1. 发送RATS (Request for Answer to Select) 命令以激活卡片并获取ATS (Answer to Select)。
* 2. 解析ATS，获取卡片能力信息（如FSC, FWI等）。
* 3. 发送一系列APDU (Application Protocol Data Unit) 指令与卡片应用进行交互。
* @return 操作状态，SUCCESS表示成功，FAIL表示失败。
*/
unsigned char CPU_CARD_EVENT(void)
{
unsigned char result;
unsigned char SendBuffer[255];
unsigned char ReceiveBuffer[255];
int i;
transmission_struct APDU;
APDU.pSendBuffer = SendBuffer;
APDU.pReceiveBuffer = ReceiveBuffer;
result = CPU_Rats(&CPU_CARD.ATS.Length, CPU_CARD.ATS.Ats_Data);
if (result != SUCCESS)
{
SetCW(FUN_DISABLE);
xlog("-> RATS ERROR!\r\n");
return result;
}
xlog("-> ATS = ");
for(i = 0; i < CPU_CARD.ATS.Length; i++)
xlog("%02X", CPU_CARD.ATS.Ats_Data[i]);
xlog("\r\n");
result = Ats_Process(CPU_CARD.ATS.Length, CPU_CARD.ATS.Ats_Data);
if (result != SUCCESS)
{
SetCW(FUN_DISABLE);
xlog("-> ATS Process ERROR!\r\n");
return result;
}
// 选择主文件(MF)
memcpy(APDU.pSendBuffer, "\x00\xA4\x00\x00\x02\x3F\x00", 7);
APDU.SendLength = 7;
result = CPU_APDU(&APDU);
if (result != SUCCESS)
{
SetCW(FUN_DISABLE);
xlog("-> APDU ERROR!\r\n");
return result;
}
xlog("-> Select MF Response = ");
for(i=0; i<APDU.ReceiveLength; i++)
xlog("%02X", APDU.pReceiveBuffer[i]);
xlog("\r\n");
// ... 此处可以添加更多APDU指令 ...
return result;
}
/**
* @brief 将数据写入芯片的FIFO缓冲区。
* @param length [in] 要写入的数据长度。
* @param buff [in] 指向源数据缓冲区的指针。
* @return 无。
*/
static void Write_FIFO(unsigned char length, unsigned char* buff)
{
unsigned char i;
for(i=0; i<length; i++)
{
SetReg(REG_FIFODATA,buff[i]);
}
}
/**
* @brief 从芯片的FIFO缓冲区读取数据。
* @param length [in] 要读取的数据长度。
* @param buff [out] 指向目标数据缓冲区的指针。
* @return 无。
*/
static void Read_FIFO(unsigned char length, unsigned char* buff)
{
unsigned char i;
for(i=0; i<length; i++)
{
GetReg(REG_FIFODATA,&buff[i]);
}
}
/**
* @brief 执行TPDU (Transmission Protocol Data Unit) 数据交换。
* @param tpdu [in, out] 指向 `transmission_struct` 结构体的指针，包含发送和接收信息。
* @return 操作状态，SUCCESS表示成功，FAIL表示超时或出错。
* @details
* 这是与卡片进行底层数据块交换的核心函数。它负责：
* - 将数据写入FIFO。
* - 启动TRANSCEIVE命令。
* - 等待接收中断或超时。
* - 从FIFO读取响应数据。
* - 检查错误状态。
*/
static unsigned char FM176XX_TPDU(transmission_struct *tpdu)
{
unsigned char irq0, error;
unsigned int i;
SetCommand(CMD_IDLE);
SetReg(REG_TXDATANUM,0x08);
SetReg(REG_IRQ0,0x7F); // 清除IRQ0所有中断标志
SetReg(REG_IRQ1,0x7F); // 清除IRQ1所有中断标志
ModifyReg(REG_FIFOCONTROL,BIT_FIFOFLUSH,FUN_ENABLE); // 清空FIFO
Write_FIFO(tpdu->SendLength,tpdu->pSendBuffer);
ModifyReg(REG_TXCRCCON, BIT_CRCEN,FUN_ENABLE);
ModifyReg(REG_RXCRCCON, BIT_CRCEN,FUN_ENABLE);
// SetTimer(tpdu->Timeout); // 定时器功能可以根据需要启用
SetCommand(CMD_TRANSCEIVE);
// 等待接收完成或超时
for(i = 0; i < tpdu->Timeout; i++)
{
rfid_delay_ms(1);
GetReg(REG_IRQ0,&irq0);
if(irq0 & BIT_RXIRQ) // 检查是否收到数据
{
GetReg(REG_ERROR, &error); // 获取错误状态
error &= (BIT_NODATAERR | BIT_COLLDET | BIT_PROTERR | BIT_INTEGERR);
if(error != 0)
return FAIL; // 接收到错误
GetReg(REG_FIFOLENGTH, &tpdu->ReceiveLength);
if(tpdu->ReceiveLength > 0)
{
Read_FIFO(tpdu->ReceiveLength,tpdu->pReceiveBuffer);
return SUCCESS;
}
}
}
return FAIL; // 超时
}
/**
* @brief 发送RATS (Request for Answer to Select) 命令。
* @param ats_len [out] 指向用于存储ATS长度的变量的指针。
* @param ats [out] 指向用于存储ATS数据的缓冲区的指针。
* @return 操作状态，SUCCESS表示成功。
* @details
* RATS是激活ISO/IEC 14443-4卡片的第一步，用于获取卡片的基本通信参数。
*/
unsigned char CPU_Rats(unsigned char *ats_len, unsigned char *ats)
{
unsigned char result;
unsigned char outbuffer[2], inbuffer[64];
transmission_struct tpdu;
tpdu.pSendBuffer = outbuffer;
tpdu.pReceiveBuffer = inbuffer;
tpdu.pSendBuffer[0] = 0xE0; // RATS命令起始字节
tpdu.pSendBuffer[1] = 0x50; // 参数字节 (FSDI=5, CID=0)
tpdu.SendLength = 2;
tpdu.Timeout = 160; // 超时时间
result = FM176XX_TPDU(&tpdu);
if (result == SUCCESS)
{
*ats_len = tpdu.ReceiveLength;
memcpy(ats, tpdu.pReceiveBuffer, *ats_len);
}
return (result);
}
/**
* @brief 解析ATS (Answer to Select) 数据。
* @param ats_len [in] ATS数据的长度。
* @param ats [in] 指向ATS数据的指针。
* @return 操作状态，SUCCESS表示成功。
* @details
* 此函数从ATS响应中提取关键参数，如FSC (Frame Size for proximity coupling)、
* FWI (Frame Waiting time Integer)等，并存储在全局的 `CPU_CARD` 结构体中。
*/
unsigned char Ats_Process(unsigned char ats_len, unsigned char *ats)
{
unsigned char offset;
if (ats_len < 2) return FAIL;
// 解析FSCI (Frame Size for proximity coupling Integer) -> FSC
CPU_CARD.FSCI = ats[1] & 0x0F;
switch(CPU_CARD.FSCI) {
case 0: CPU_CARD.FSC = 16; break;
case 1: CPU_CARD.FSC = 24; break;
case 2: CPU_CARD.FSC = 32; break;
case 3: CPU_CARD.FSC = 40; break;
case 4: CPU_CARD.FSC = 48; break;
case 5: CPU_CARD.FSC = 64; break;
case 6: CPU_CARD.FSC = 96; break;
case 7: CPU_CARD.FSC = 128; break;
case 8: CPU_CARD.FSC = 256; break;
default: CPU_CARD.FSC = 32; break; // 默认值
}
xlog("-> CPU_CARD.FSC = %d\r\n", CPU_CARD.FSC);
offset = 0;
if (ats[1] & BIT4) // TA(1) present
{
CPU_CARD.TA = ats[2];
offset++;
}
if (ats[1] & BIT5) // TB(1) present
{
CPU_CARD.TB = ats[2 + offset];
CPU_CARD.SFGI = CPU_CARD.TB & 0x0F;
CPU_CARD.FWI = (CPU_CARD.TB >> 4) & 0x0F;
xlog("-> CPU_CARD.SFGI = %02X\r\n", CPU_CARD.SFGI);
xlog("-> CPU_CARD.FWI = %02X\r\n", CPU_CARD.FWI);
// 根据FWI计算FWT (Frame Waiting Time)
unsigned long base_fwt = 256 * 16 / 13560; // (256 * 16 / fc) in ms
CPU_CARD.FWT = base_fwt * (1 << CPU_CARD.FWI);
offset++;
} else {
CPU_CARD.FWT = 160; // 默认FWT
}
if (ats[1] & BIT6) // TC(1) present
{
CPU_CARD.TC = ats[2 + offset];
offset++;
}
CPU_CARD.PCB = 0x02; // PCB初始值为0x02
return SUCCESS;
}
/**
* @brief 发送NAK (Negative Acknowledge) 响应。
* @param tpdu [in, out] 指向传输结构体的指针。
* @return 操作状态。
* @details
* 在TPDU交换中，如果接收到错误的数据块，会发送NAK请求重发。
*/
unsigned char CPU_NAK(transmission_struct *tpdu)
{
unsigned char result, tpdu_send_buffer[1], tpdu_receive_buffer[255];
tpdu->pSendBuffer = tpdu_send_buffer;
tpdu->pReceiveBuffer = tpdu_receive_buffer;
tpdu->pSendBuffer[0] = 0xB0 | CPU_CARD.PCB; // NAK PCB
tpdu->SendLength = 1;
result = FM176XX_TPDU(tpdu);
return (result);
}
/**
* @brief 封装了重试逻辑的TPDU传输函数。
* @param tpdu [in, out] 指向传输结构体的指针。
* @return 操作状态。
* @details
* 此函数调用底层的 `FM176XX_TPDU`，并在失败时进行最多3次重试。
* 它还处理ACK/NAK逻辑，以确保数据的可靠传输。
*/
unsigned char CPU_TPDU(transmission_struct *tpdu)
{
unsigned char result, i, pcb_byte;
transmission_struct nak_tpdu;
result = FM176XX_TPDU(tpdu);
for (i = 0; i < 3; i++)
{
if (result != SUCCESS)
{
result = CPU_NAK(&nak_tpdu);
if(result == SUCCESS && nak_tpdu.ReceiveLength > 0)
{
memcpy(&pcb_byte, nak_tpdu.pReceiveBuffer, 1);
if((pcb_byte & 0xF0) == 0xA0) // R(ACK)
{
xlog("...pcb_byte = %02X\r\n", pcb_byte);
xlog("...CPU_CARD.PCB = %02X\r\n", CPU_CARD.PCB);
if((pcb_byte & 0x01) != (CPU_CARD.PCB & 0x01))
{
result = FM176XX_TPDU(tpdu);
}
else
{
tpdu->pSendBuffer[0] ^= 0x01; // 翻转序列号位
CPU_CARD.PCB = tpdu->pSendBuffer[0] & 0x01;
result = FM176XX_TPDU(tpdu);
}
}
}
}
else
{
break; // 成功则退出循环
}
}
return (result);
}
/**
* @brief 发送APDU (Application Protocol Data Unit) 命令。
* @param apdu [in, out] 指向传输结构体的指针，包含APDU命令和响应。
* @return 操作状态。
* @details
* 此函数处理APDU的块链接（chaining）逻辑。如果APDU长度超过卡片的最大帧大小（FSC），
* 它会自动将APDU分割成多个TPDU块进行传输。
*/
unsigned char CPU_APDU(transmission_struct *apdu)
{
unsigned char result, pcb_byte, i;
unsigned char tpdu_send_buffer[256], tpdu_receive_buffer[256];
unsigned int unsent_length;
transmission_struct tpdu;
tpdu.pSendBuffer = tpdu_send_buffer;
tpdu.pReceiveBuffer = tpdu_receive_buffer;
tpdu.Timeout = CPU_CARD.FWT;
apdu->ReceiveLength = 0;
unsent_length = apdu->SendLength;
// --- 发送阶段 ---
for (i = 0; i < 16; i++) // 最多16个链式块
{
xlog("unsent_length = %d\r\n", unsent_length);
if (unsent_length <= (CPU_CARD.FSC - 1))
{
// 最后一个或唯一的数据块
tpdu.pSendBuffer[0] = CPU_CARD.PCB; // I-Block, no chaining
memcpy(tpdu.pSendBuffer + 1, apdu->pSendBuffer + apdu->SendLength - unsent_length, unsent_length);
tpdu.SendLength = unsent_length + 1;
xlog("--> ");
for(int j=0; j<tpdu.SendLength; j++) xlog("%02X", tpdu.pSendBuffer[j]);
xlog("\r\n");
result = CPU_TPDU(&tpdu);
if ((result != SUCCESS) || (tpdu.ReceiveLength == 0))
return (result);
xlog("<-- ");
for(int j=0; j<tpdu.ReceiveLength; j++) xlog("%02X", tpdu.pReceiveBuffer[j]);
xlog("\r\n");
unsent_length = 0;
break; // 发送完成
}
else
{
// 需要分块传输
tpdu.pSendBuffer[0] = CPU_CARD.PCB | 0x10; // I-Block with chaining
memcpy(tpdu.pSendBuffer + 1, apdu->pSendBuffer + apdu->SendLength - unsent_length, CPU_CARD.FSC - 1);
tpdu.SendLength = CPU_CARD.FSC;
xlog("..--> ");
for(int j=0; j<tpdu.SendLength; j++) xlog("%02X", tpdu.pSendBuffer[j]);
xlog("\r\n");
result = CPU_TPDU(&tpdu);
xlog("<-- ");
for(int j=0; j<tpdu.ReceiveLength; j++) xlog("%02X", tpdu.pReceiveBuffer[j]);
xlog("\r\n");
if ((result != SUCCESS) || (tpdu.ReceiveLength != 1))
return (result);
memcpy(&pcb_byte, tpdu.pReceiveBuffer, 1);
if ((pcb_byte & 0xE0) == 0xA0) // R(ACK) block
{
unsent_length -= (CPU_CARD.FSC - 1);
CPU_CARD.PCB = (pcb_byte & 0x01) ^ 0x01; // 更新序列号
xlog("unsent_length = %d\r\n", unsent_length);
}
else
{
return (FAIL); // 未收到预期的ACK
}
}
}
// --- 接收阶段 ---
for (i = 0; i < 255; i++) // 最多255个链式块
{
if ((result != SUCCESS) || (tpdu.ReceiveLength == 0))
return (FAIL);
memcpy(&pcb_byte, tpdu.pReceiveBuffer, 1);
if ((pcb_byte & 0xC0) == 0x00) // I-Block
{
CPU_CARD.PCB = (pcb_byte & 0x01) ^ 0x01;
memcpy(apdu->pReceiveBuffer + apdu->ReceiveLength, tpdu.pReceiveBuffer + 1, tpdu.ReceiveLength - 1);
apdu->ReceiveLength += (tpdu.ReceiveLength - 1);
if (pcb_byte & 0x10) // 还有后续数据块
{
tpdu.pSendBuffer[0] = 0xA0 | CPU_CARD.PCB; // 发送ACK
tpdu.SendLength = 1;
xlog("...--> ACK = %02X\r\n", tpdu.pSendBuffer[0]);
result = CPU_TPDU(&tpdu);
}
else // 最后一个数据块
{
return SUCCESS;
}
}
else if ((pcb_byte & 0xE0) == 0xE0) // S-Block (WTX)
{
// 回复WTX响应
memcpy(tpdu.pSendBuffer, tpdu.pReceiveBuffer, tpdu.ReceiveLength);
tpdu.SendLength = tpdu.ReceiveLength;
xlog("....--> WTX = ");
for(int j=0; j<tpdu.SendLength; j++) xlog("%02X", tpdu.pSendBuffer[j]);
xlog("\r\n");
result = CPU_TPDU(&tpdu);
}
else
{
return FAIL; // 未知响应
}
}
return (FAIL); // 接收块过多
}

455
apps/earphone/remote_control/RFID/reader/MIFARE.c Normal file
View File

@ -0,0 +1,455 @@
#include "../include/MIFARE.h"
#include "../include/READER.h"
#include "../include/READER_REG.h"
#include "../include/rfid_main.h"
#include "../rfid_hal.h"
#define FUN_ENABLE_XLOG 1
#ifdef xlog
#undef xlog
#endif
#if FUN_ENABLE_XLOG
#define xlog(format, ...) printf("[XT:%s] " format, __func__, ##__VA_ARGS__)
#else
#define xlog(format, ...) ((void)0)
#endif
unsigned char SECTOR,BLOCK,BLOCK_NUM;
unsigned char BLOCK_DATA[16];
unsigned char KEY_A[16][6]=
{{0xFF,0xFF,0xFF,0xFF,0xFF,0xFF},//0
{0xFF,0xFF,0xFF,0xFF,0xFF,0xFF},//1
{0xFF,0xFF,0xFF,0xFF,0xFF,0xFF},//2
{0xFF,0xFF,0xFF,0xFF,0xFF,0xFF},//3
{0xFF,0xFF,0xFF,0xFF,0xFF,0xFF},//4
{0xFF,0xFF,0xFF,0xFF,0xFF,0xFF},//5
{0xFF,0xFF,0xFF,0xFF,0xFF,0xFF},//6
{0xFF,0xFF,0xFF,0xFF,0xFF,0xFF},//7
{0xFF,0xFF,0xFF,0xFF,0xFF,0xFF},//8
{0xFF,0xFF,0xFF,0xFF,0xFF,0xFF},//9
{0xFF,0xFF,0xFF,0xFF,0xFF,0xFF},//10
{0xFF,0xFF,0xFF,0xFF,0xFF,0xFF},//11
{0xFF,0xFF,0xFF,0xFF,0xFF,0xFF},//12
{0xFF,0xFF,0xFF,0xFF,0xFF,0xFF},//13
{0xFF,0xFF,0xFF,0xFF,0xFF,0xFF},//14
{0xFF,0xFF,0xFF,0xFF,0xFF,0xFF}};//15
unsigned char KEY_B[16][6]=
{{0xFF,0xFF,0xFF,0xFF,0xFF,0xFF},//0
{0xFF,0xFF,0xFF,0xFF,0xFF,0xFF},//1
{0xFF,0xFF,0xFF,0xFF,0xFF,0xFF},//2
{0xFF,0xFF,0xFF,0xFF,0xFF,0xFF},//3
{0xFF,0xFF,0xFF,0xFF,0xFF,0xFF},//4
{0xFF,0xFF,0xFF,0xFF,0xFF,0xFF},//5
{0xFF,0xFF,0xFF,0xFF,0xFF,0xFF},//6
{0xFF,0xFF,0xFF,0xFF,0xFF,0xFF},//7
{0xFF,0xFF,0xFF,0xFF,0xFF,0xFF},//8
{0xFF,0xFF,0xFF,0xFF,0xFF,0xFF},//9
{0xFF,0xFF,0xFF,0xFF,0xFF,0xFF},//10
{0xFF,0xFF,0xFF,0xFF,0xFF,0xFF},//11
{0xFF,0xFF,0xFF,0xFF,0xFF,0xFF},//12
{0xFF,0xFF,0xFF,0xFF,0xFF,0xFF},//13
{0xFF,0xFF,0xFF,0xFF,0xFF,0xFF},//14
{0xFF,0xFF,0xFF,0xFF,0xFF,0xFF}};//15
/**
* @brief 清除Mifare卡加密认证标志。
* @details
* 在对Mifare卡进行认证（Authentication）后，芯片内部会设置一个加密标志位（BIT_CRYPTO1ON）。
* 此函数用于清除该标志，以便可以对新的扇区进行认证或执行非加密操作。
* @return 无。
*/
void Mifare_Clear_Crypto(void)
{
ModifyReg(REG_STATUS,BIT_CRYPTO1ON,RESET);
return;
}
/**
* @brief Mifare卡事件处理函数（示例）。
* @details
* 这是一个示例函数，演示了对Mifare Classic卡进行读写操作的完整流程：
* 1. 清除加密状态。
* 2. 对指定的扇区（例如扇区1）使用密钥A进行认证。
* 3. 如果认证成功，则遍历该扇区的数据块（块0到块2）。
* 4. 对每个块先执行写操作，写入16字节的0xFF。
* 5. 然后再执行读操作，将数据读回并打印。
* @return 操作状态，SUCCESS表示成功，FAIL表示失败。
*/
unsigned char MIFARE_CARD_EVENT(void)
{
unsigned char result;
int i;
Mifare_Clear_Crypto();
SECTOR = 1;
//for(SECTOR = 0;SECTOR < 16; SECTOR++)
{
BLOCK_NUM = (SECTOR * 4) + BLOCK;
result = Mifare_Auth(KEY_A_M1,SECTOR,KEY_A[SECTOR],PICC_A.UID);
if(result != SUCCESS)
{
SetCW(FUN_DISABLE);
xlog("-> AUTH ERROR!\r\n");
return result;
}
xlog("-> AUTH SUCCESS!\r\n");
for(BLOCK = 0;BLOCK < 3;BLOCK++)
{
BLOCK_NUM = (SECTOR * 4) + BLOCK;
if(BLOCK_NUM == 0)
BLOCK_NUM = 1;
xlog("-> SECTOR = %02X\r\n",SECTOR);;
xlog("-> BLOCK = %02X\r\n",BLOCK);
xlog("-> BLOCK_NUM = %02X\r\n",BLOCK_NUM);
memcpy(BLOCK_DATA,"\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF",16);
result = Mifare_Blockwrite(BLOCK_NUM,BLOCK_DATA);
if(result != SUCCESS)
{
SetCW(FUN_DISABLE);
xlog("-> WRITE BLOCK ERROR!\r\n");
return result;
}
xlog("-> WRITE BLOCK SUCCESS!\r\n");
result = Mifare_Blockread(BLOCK_NUM,BLOCK_DATA);
if(result != SUCCESS)
{
SetCW(FUN_DISABLE);
xlog("-> READ BLOCK ERROR!\r\n");
return result;
}
xlog("-> READ BLOCK = ");
for(i=0; i<16; i++) xlog("%02X", BLOCK_DATA[i]);
xlog("\r\n");
}
}
SetCW(FUN_DISABLE);
return result;
}
/**
* @brief 将6字节的Mifare密钥加载到芯片的密钥缓冲区。
* @param mifare_key [in] 指向6字节密钥数组的指针。
* @return 操作状态，SUCCESS表示成功，FAIL表示失败。
* @details
* 在执行认证命令（Mifare_Auth）之前，必须先调用此函数将要使用的密钥加载到芯片内部。
*/
unsigned char Mifare_LoadKey(unsigned char *mifare_key)
{
unsigned char reg_data;
SetCommand(CMD_IDLE);
ModifyReg(REG_FIFOCONTROL,BIT_FIFOFLUSH,FUN_ENABLE); //Clear FIFO
SetReg(REG_FIFODATA,mifare_key[0]);
SetReg(REG_FIFODATA,mifare_key[1]);
SetReg(REG_FIFODATA,mifare_key[2]);
SetReg(REG_FIFODATA,mifare_key[3]);
SetReg(REG_FIFODATA,mifare_key[4]);
SetReg(REG_FIFODATA,mifare_key[5]);
SetCommand(CMD_LOADKEY);
rfid_delay_ms(1);
GetReg(REG_COMMAND,&reg_data);
if((reg_data & CMD_MASK) == CMD_IDLE)
return SUCCESS;
else
return FAIL;
}
/**
* @brief 对Mifare Classic卡片的指定扇区进行认证。
* @param key_mode [in] 认证模式，`KEY_A_M1` (0) 表示使用密钥A，`KEY_B_M1` (1) 表示使用密钥B。
* @param sector [in] 要认证的扇区号 (0-15)。
* @param mifare_key [in] 指向6字节认证密钥的指针。
* @param card_uid [in] 指向4字节卡片UID的指针。
* @return 操作状态，SUCCESS表示认证成功，FAIL表示失败。
* @details
* 这是访问Mifare卡数据块之前的必要步骤。认证成功后，芯片会设置加密标志位。
*/
unsigned char Mifare_Auth(unsigned char key_mode,unsigned char sector,unsigned char *mifare_key,unsigned char *card_uid)
{
unsigned char result,reg_data;
result = Mifare_LoadKey(mifare_key);
if (result != SUCCESS)
return result;
SetCommand(CMD_IDLE);
ModifyReg(REG_FIFOCONTROL,BIT_FIFOFLUSH,FUN_ENABLE); //Clear FIFO
if(key_mode == KEY_A_M1)
{
SetReg(REG_FIFODATA,0x60);// 0x60: Key A认证指令
ModifyReg(REG_RXTXCON,BIT_SHMODE,FUN_DISABLE);
}
if(key_mode == KEY_B_M1)
{
SetReg(REG_FIFODATA,0x61);// 0x61: Key B认证指令
ModifyReg(REG_RXTXCON,BIT_SHMODE,FUN_DISABLE);
}
SetReg(REG_FIFODATA,sector * 4);// 认证扇区的块0地址
SetReg(REG_FIFODATA,card_uid[0]);
SetReg(REG_FIFODATA,card_uid[1]);
SetReg(REG_FIFODATA,card_uid[2]);
SetReg(REG_FIFODATA,card_uid[3]);
ModifyReg(REG_TXCRCCON, BIT_CRCEN,FUN_ENABLE);
ModifyReg(REG_RXCRCCON, BIT_CRCEN,FUN_ENABLE);
SetCommand(CMD_AUTHENT);
rfid_delay_ms(5);
GetReg(REG_COMMAND,&reg_data);
if((reg_data & CMD_MASK) == CMD_IDLE)
{
GetReg(REG_STATUS,&reg_data);
if(reg_data & BIT_CRYPTO1ON)// 检查加密标志位以确认认证成功
return SUCCESS;
}
return FAIL;
}
/**
* @brief 将4字节数据格式化为Mifare值块格式并写入指定块。
* @param block [in] 目标块号。
* @param data_buff [in] 指向4字节源数据的指针。
* @return 操作状态，SUCCESS表示成功，FAIL表示失败。
* @details
* Mifare值块有特定的数据格式，包含值、值的反码和地址字节。此函数会自动处理格式转换。
*/
unsigned char Mifare_Blockset(unsigned char block,unsigned char *data_buff)
{
unsigned char block_data[16],result;
// 格式化为值块
block_data[0] = data_buff[3];
block_data[1] = data_buff[2];
block_data[2] = data_buff[1];
block_data[3] = data_buff[0];
block_data[4] = ~data_buff[3];
block_data[5] = ~data_buff[2];
block_data[6] = ~data_buff[1];
block_data[7] = ~data_buff[0];
block_data[8] = data_buff[3];
block_data[9] = data_buff[2];
block_data[10] = data_buff[1];
block_data[11] = data_buff[0];
block_data[12] = block;
block_data[13] = ~block;
block_data[14] = block;
block_data[15] = ~block;
result = Mifare_Blockwrite(block,block_data);
return result;
}
/**
* @brief 从Mifare卡读取一个16字节的数据块。
* @param block [in] 要读取的块号 (0x00 - 0x3F)。
* @param data_buff [out] 指向16字节缓冲区的指针，用于存储读取的数据。
* @return 操作状态，SUCCESS表示成功，FAIL表示失败。
*/
unsigned char Mifare_Blockread(unsigned char block,unsigned char *data_buff)
{
unsigned char reg_data,i;
SetCommand(CMD_IDLE);
SetReg(REG_TXDATANUM,0x08);
SetReg(REG_FIFODATA,0x30);// 0x30: 读块指令
SetReg(REG_FIFODATA,block);// 块地址
ModifyReg(REG_TXCRCCON, BIT_CRCEN,FUN_ENABLE);
ModifyReg(REG_RXCRCCON, BIT_CRCEN,FUN_ENABLE);
SetCommand(CMD_TRANSCEIVE);
rfid_delay_ms(2);
GetReg(REG_FIFOLENGTH,&reg_data);
if (reg_data != 16) // 成功时应返回16字节数据
return FAIL;
GetReg(REG_ERROR,&reg_data);
if(reg_data & 0x07)
return FAIL;
for(i=0;i<16;i++)
{
GetReg (REG_FIFODATA,&data_buff[i]);
}
return SUCCESS;
}
/**
* @brief 向Mifare卡写入一个16字节的数据块。
* @param block [in] 要写入的块号 (0x00 - 0x3F)。
* @param data_buff [in] 指向16字节数据的指针。
* @return 操作状态，SUCCESS表示成功，FAIL表示失败。
*/
unsigned char Mifare_Blockwrite(unsigned char block,unsigned char *data_buff)
{
unsigned char reg_data,i;
SetCommand(CMD_IDLE);
SetReg(REG_TXDATANUM,0x08);
SetReg(REG_FIFODATA,0xA0);// 0xA0: 写块指令
SetReg(REG_FIFODATA,block);// 块地址
ModifyReg(REG_TXCRCCON, BIT_CRCEN,FUN_ENABLE);
ModifyReg(REG_RXCRCCON, BIT_CRCEN,FUN_DISABLE);
SetCommand(CMD_TRANSCEIVE);
rfid_delay_ms(5);
GetReg(REG_FIFOLENGTH,&reg_data);
if (reg_data != 1) // 接收到ACK (0x0A)
return FAIL;
GetReg (REG_FIFODATA,&reg_data);
if(reg_data != 0x0A)
return FAIL;
for(i=0;i<16;i++)
{
SetReg(REG_FIFODATA,data_buff[i]);
}
SetCommand(CMD_TRANSCEIVE);
rfid_delay_ms(5);
GetReg(REG_FIFOLENGTH,&reg_data);
if (reg_data != 1) // 接收到ACK (0x0A)
return FAIL;
GetReg (REG_FIFODATA,&reg_data);
if(reg_data != 0x0A)
return FAIL;
return SUCCESS;
}
/**
* @brief 对Mifare卡的指定值块执行增值操作。
* @param block [in] 值块的块号。
* @param data_buff [in] 指向4字节增值数据的指针。
* @return 操作状态，SUCCESS表示成功，FAIL表示失败。
*/
unsigned char Mifare_Blockinc(unsigned char block,unsigned char *data_buff)
{
unsigned char reg_data,i;
SetCommand(CMD_IDLE);
SetReg(REG_TXDATANUM,0x08);
SetReg(REG_FIFODATA,0xC1);// 0xC1: 增值指令
SetReg(REG_FIFODATA,block);// 块地址
ModifyReg(REG_TXCRCCON, BIT_CRCEN,FUN_ENABLE);
ModifyReg(REG_RXCRCCON, BIT_CRCEN,FUN_DISABLE);
SetCommand(CMD_TRANSCEIVE);
rfid_delay_ms(5);
GetReg(REG_FIFOLENGTH,&reg_data);
if (reg_data != 1)
return FAIL;
GetReg (REG_FIFODATA,&reg_data);
if(reg_data != 0x0A)
return FAIL;
for(i=0;i<4;i++)
{
SetReg(REG_FIFODATA,data_buff[i]);
}
SetCommand(CMD_TRANSCEIVE);
rfid_delay_ms(5);
GetReg(REG_FIFOLENGTH,&reg_data);
if (reg_data != 1)
return FAIL;
GetReg (REG_FIFODATA,&reg_data);
if(reg_data != 0x0A)
return FAIL;
return SUCCESS;
}
/**
* @brief 对Mifare卡的指定值块执行减值操作。
* @param block [in] 值块的块号。
* @param data_buff [in] 指向4字节减值数据的指针。
* @return 操作状态，SUCCESS表示成功，FAIL表示失败。
*/
unsigned char Mifare_Blockdec(unsigned char block,unsigned char *data_buff)
{
unsigned char reg_data,i;
SetCommand(CMD_IDLE);
SetReg(REG_TXDATANUM,0x08);
SetReg(REG_FIFODATA,0xC0);// 0xC0: 减值指令
SetReg(REG_FIFODATA,block);// 块地址
ModifyReg(REG_TXCRCCON, BIT_CRCEN,FUN_ENABLE);
ModifyReg(REG_RXCRCCON, BIT_CRCEN,FUN_DISABLE);
SetCommand(CMD_TRANSCEIVE);
rfid_delay_ms(5);
GetReg(REG_FIFOLENGTH,&reg_data);
if (reg_data != 1)
return FAIL;
GetReg (REG_FIFODATA,&reg_data);
if(reg_data != 0x0A)
return FAIL;
for(i=0;i<4;i++)
{
SetReg(REG_FIFODATA,data_buff[i]);
}
SetCommand(CMD_TRANSCEIVE);
rfid_delay_ms(5);
GetReg(REG_FIFOLENGTH,&reg_data);
if (reg_data != 1)
return FAIL;
GetReg (REG_FIFODATA,&reg_data);
if(reg_data != 0x0A)
return FAIL;
return SUCCESS;
}
/**
* @brief 执行Mifare卡的传输（Transfer）命令。
* @param block [in] 块号。
* @return 操作状态，SUCCESS表示成功，FAIL表示失败。
* @details
* 在对值块进行增/减值操作后，需要调用此函数将结果从内部寄存器写入到块中。
*/
unsigned char Mifare_Transfer(unsigned char block)
{
unsigned char reg_data;
SetCommand(CMD_IDLE);
SetReg(REG_TXDATANUM,0x08);
SetReg(REG_FIFODATA,0xB0);// 0xB0: Transfer指令
SetReg(REG_FIFODATA,block);// 块地址
ModifyReg(REG_TXCRCCON, BIT_CRCEN,FUN_ENABLE);
ModifyReg(REG_RXCRCCON, BIT_CRCEN,FUN_DISABLE);
SetCommand(CMD_TRANSCEIVE);
rfid_delay_ms(5);
GetReg(REG_FIFOLENGTH,&reg_data);
if (reg_data != 1)
return FAIL;
GetReg (REG_FIFODATA,&reg_data);
if(reg_data != 0x0A)
return FAIL;
return SUCCESS;
}
/**
* @brief 执行Mifare卡的恢复（Restore）命令。
* @param block [in] 块号。
* @return 操作状态，SUCCESS表示成功，FAIL表示失败。
* @details
* 此命令用于将一个块的内容从内部寄存器中恢复。
*/
unsigned char Mifare_Restore(unsigned char block)
{
unsigned char reg_data,i;
SetCommand(CMD_IDLE);
SetReg(REG_TXDATANUM,0x08);
SetReg(REG_FIFODATA,0xC2);// 0xC2: Restore指令
SetReg(REG_FIFODATA,block);// 块地址
ModifyReg(REG_TXCRCCON, BIT_CRCEN,FUN_ENABLE);
ModifyReg(REG_RXCRCCON, BIT_CRCEN,FUN_DISABLE);
SetCommand(CMD_TRANSCEIVE);
rfid_delay_ms(5);
GetReg(REG_FIFOLENGTH,&reg_data);
if (reg_data != 1)
return FAIL;
GetReg (REG_FIFODATA,&reg_data);
if(reg_data != 0x0A)
return FAIL;
for(i=0;i<4;i++)
{
SetReg(REG_FIFODATA,0);
}
SetCommand(CMD_TRANSCEIVE);
rfid_delay_ms(5);
GetReg(REG_FIFOLENGTH,&reg_data);
if (reg_data != 1)
return FAIL;
GetReg (REG_FIFODATA,&reg_data);
if(reg_data != 0x0A)
return FAIL;
return SUCCESS;
}

108
apps/earphone/remote_control/RFID/reader/NTAG.c Normal file
View File

@ -0,0 +1,108 @@
#include "../include/READER.h"
#include "../include/NTAG.h"
#include "../include/READER_REG.h"
#include "../include/rfid_main.h"
#include "../rfid_hal.h"
#define FUN_ENABLE_XLOG 1
#ifdef xlog
#undef xlog
#endif
#if FUN_ENABLE_XLOG
#define xlog(format, ...) printf("[XT:%s] " format, __func__, ##__VA_ARGS__)
#else
#define xlog(format, ...) ((void)0)
#endif
unsigned char PAGE_DATA[16];
/**
* @brief NTAG卡事件处理函数（示例）。
* @details
* 这是一个示例函数，演示了对NTAG系列卡片进行读写操作的流程：
* 1. 准备要写入的数据（4字节）。
* 2. 调用 `Write_Page()` 函数将数据写入第8页。
* 3. 调用 `Read_Page()` 函数从第8页读回数据并打印，以进行验证。
* @return 操作状态，SUCCESS表示成功，FAIL表示失败。
*/
unsigned char NTAG_EVENT(void)
{
unsigned char result;
memcpy(PAGE_DATA,"\x01\x02\x03\x04",4);
result = Write_Page(8,PAGE_DATA);
if (result != SUCCESS)
return result;
xlog("PAGE 8 Write OK\r\n");
result = Read_Page(8,PAGE_DATA);
xlog("PAGE 8 = %02X%02X%02X%02X\r\n",PAGE_DATA[0],PAGE_DATA[1],PAGE_DATA[2],PAGE_DATA[3]);
return result;
}
/**
* @brief 从NTAG卡读取一个或多个页的数据。
* @param page_num [in] 要读取的起始页号。
* @param page_data [out] 指向缓冲区的指针，用于存储读取的数据。对于NTAG21x系列，一次最少读取16字节（4页）。
* @return 操作状态，SUCCESS表示成功，FAIL表示失败。
*/
unsigned char Read_Page(unsigned char page_num,unsigned char *page_data)
{
unsigned char reg_data,i;
SetCommand(CMD_IDLE);
Clear_FIFO();
SetReg(REG_FIFODATA,0x30); // 读指令
SetReg(REG_FIFODATA,page_num);
ModifyReg(REG_TXCRCCON, BIT_CRCEN,FUN_ENABLE);
ModifyReg(REG_RXCRCCON, BIT_CRCEN,FUN_ENABLE);
SetCommand(CMD_TRANSCEIVE);
rfid_delay_ms(5);
GetReg(REG_ERROR,&reg_data);
if(reg_data & 0x07)
return FAIL;
GetReg(REG_FIFOLENGTH,&reg_data);
if(reg_data != 16) // NTAG一次读取返回16字节
return FAIL;
for(i=0;i<16;i++)
{
GetReg(REG_FIFODATA,&page_data[i]);
}
return SUCCESS;
}
/**
* @brief 向NTAG卡的一个页（Page）写入4字节数据。
* @param page_num [in] 要写入的页号。
* @param page_data [in] 指向4字节数据的指针。
* @return 操作状态，SUCCESS表示成功，FAIL表示失败。
*/
unsigned char Write_Page(unsigned char page_num,unsigned char *page_data)
{
unsigned char reg_data;
SetCommand(CMD_IDLE);
Clear_FIFO();
SetReg(REG_FIFODATA,0xA2); // 写指令
SetReg(REG_FIFODATA,page_num);
SetReg(REG_FIFODATA,page_data[0]);
SetReg(REG_FIFODATA,page_data[1]);
SetReg(REG_FIFODATA,page_data[2]);
SetReg(REG_FIFODATA,page_data[3]);
ModifyReg(REG_TXCRCCON, BIT_CRCEN,FUN_ENABLE);
ModifyReg(REG_RXCRCCON, BIT_CRCEN,FUN_DISABLE);
SetCommand(CMD_TRANSCEIVE);
rfid_delay_ms(5);
GetReg(REG_FIFOLENGTH,&reg_data);
if(reg_data != 1) // 应该收到一个ACK
return FAIL;
GetReg(REG_FIFODATA,&reg_data);
if(reg_data != 0x0A) // ACK的值为0x0A
return FAIL;
return SUCCESS;
}

773
apps/earphone/remote_control/RFID/reader/READER.c Normal file
View File

@ -0,0 +1,773 @@
/********************************************************************************************************
* @file READER.c
* @brief RFID 读卡器底层驱动及协议实现
* @details
* 本文件实现了与FM176XX系列RFID芯片的底层通信协议。它包含以下功能：
* - 控制芯片进入不同工作模式（如Type A, B, V, F）。
* - 实现各种卡片类型的寻卡、防冲突、选择和数据交换命令。
* - 管理芯片的FIFO、定时器和RF场。
* 所有硬件相关的操作均通过 `rfid_hal.h` 中定义的接口完成。
********************************************************************************************************/
#include "../include/READER.h"
#include "../include/READER_REG.h"
#include "../include/rfid_main.h"
#include "../rfid_hal.h" // 引入硬件抽象层
#define FUN_ENABLE_XLOG 1
#ifdef xlog
#undef xlog
#endif
#if FUN_ENABLE_XLOG
#define xlog(format, ...) printf("[XT:%s] " format, __func__, ##__VA_ARGS__)
#else
#define xlog(format, ...) ((void)0)
#endif
// 定义全局变量以存储不同类型卡片的信息
struct picc_a_struct PICC_A;
struct picc_b_struct PICC_B;
struct picc_v_struct PICC_V;
struct picc_f_struct PICC_F;
/********************************************************************************************************
* 公共接口函数
********************************************************************************************************/
/**
* @brief 修改寄存器的特定位。
* @param reg_address [in] 目标寄存器的地址。
* @param mask [in] 要修改的位的掩码。
* @param set [in] 如果为非0，则将掩码对应的位设置为1；如果为0，则清零。
* @return 无。
* @details
* 这是一个“读-改-写”操作。首先读取寄存器的当前值，然后根据掩码和`set`参数修改它，
* 最后将修改后的值写回寄存器。
*/
void ModifyReg(unsigned char reg_address, unsigned char mask, unsigned char set)
{
unsigned char reg_data;
GetReg(reg_address, &reg_data);
if (set)
{
reg_data |= mask;
}
else
{
reg_data &= ~mask;
}
SetReg(reg_address, reg_data);
}
/**
* @brief 向命令寄存器写入一个命令。
* @param command [in] 要执行的命令代码（如CMD_IDLE, CMD_TRANSCEIVE等）。
* @return 操作状态，SUCCESS表示成功。
*/
unsigned char SetCommand(unsigned char command)
{
return SetReg(REG_COMMAND, CMD_MASK & command);
}
/**
* @brief 设置芯片内部定时器的超时时间。
* @param timeout [in] 超时时间，单位为毫秒(ms)。
* @return 无。
* @details
* 根据输入的超时时间，计算合适的预分频值和重载值，并配置T0和T1定时器。
* 这用于在收发数据时进行超时检测。
*/
void SetTimer(unsigned int timeout)
{
unsigned long prescale = 1;
unsigned long t, fc;
fc = timeout * 13560; // 13.56MHz时钟频率
t = fc;
while (fc > 65535)
{
prescale *= 2;
fc = t / prescale;
if (fc * prescale != t)
fc++;
}
if (prescale > 1)
{
SetReg(REG_T0CONTROL, BIT_TSTOP_RX | BIT_TSTART_TX | BIT_TAUTORESTARTED | VALUE_TCLK_1356_MHZ);
SetReg(REG_T0RELOADHI, (unsigned char)(fc >> 8));
SetReg(REG_T0RELOADLO, (unsigned char)fc);
SetReg(REG_T1CONTROL, BIT_TSTOP_RX | BIT_TSTART_TX | VALUE_TCLK_T0);
SetReg(REG_T1RELOADHI, (unsigned char)(prescale >> 8));
SetReg(REG_T1RELOADLO, (unsigned char)prescale);
}
else
{
SetReg(REG_T1CONTROL, BIT_TSTOP_RX | BIT_TSTART_TX | VALUE_TCLK_1356_MHZ);
SetReg(REG_T1RELOADHI, (unsigned char)(fc >> 8));
SetReg(REG_T1RELOADLO, (unsigned char)fc);
}
}
/**
* @brief 打开或关闭RF场（载波）。
* @param mode [in] FUN_ENABLE表示打开，FUN_DISABLE表示关闭。
* @return 操作状态，SUCCESS表示成功。
*/
unsigned char SetCW(unsigned char mode)
{
if (mode == FUN_ENABLE)
{
ModifyReg(REG_COMMAND, BIT_MODEMOFF, FUN_DISABLE);
ModifyReg(REG_TXMODE, BIT_TPUSHON | BIT_TPULLON, FUN_ENABLE);
}
else
{
ModifyReg(REG_COMMAND, BIT_MODEMOFF, FUN_ENABLE);
ModifyReg(REG_TXMODE, BIT_TPUSHON | BIT_TPULLON, FUN_DISABLE);
}
rfid_delay_ms(5);
return SUCCESS;
}
/**
* @brief 清空芯片内部的FIFO缓冲区。
* @return 无。
*/
void Clear_FIFO(void)
{
unsigned char fifolength;
GetReg(REG_FIFOLENGTH, &fifolength);
if (fifolength != 0)
{
ModifyReg(REG_FIFOCONTROL, BIT_FIFOFLUSH, FUN_ENABLE);
}
}
/**
* @brief 加载指定的通信协议参数到芯片。
* @param p_rx [in] 接收协议代码。
* @param p_tx [in] 发送协议代码。
* @return 操作状态，SUCCESS表示成功，FAIL表示失败。
* @details
* 不同的卡片类型（A, B, V, F）使用不同的通信速率和编码方式，
* 此函数用于将这些协议参数加载到芯片中。
*/
unsigned char LoadProtocol(unsigned char p_rx, unsigned char p_tx)
{
unsigned char reg_data = 0;
SetCommand(CMD_IDLE);
ModifyReg(REG_FIFOCONTROL, BIT_FIFOFLUSH, FUN_ENABLE); // 清空FIFO
SetReg(REG_FIFODATA, p_rx); // 写入接收协议
SetReg(REG_FIFODATA, p_tx); // 写入发送协议
SetCommand(CMD_LOADPROTOCOL);
rfid_delay_ms(2);
GetReg(REG_COMMAND, &reg_data);
if (reg_data != CMD_IDLE)
return FAIL;
return SUCCESS;
}
/**
* @brief 设置发送和接收的奇偶校验位使能状态。
* @param state [in] FUN_ENABLE或FUN_DISABLE。
* @return 无。
*/
void SetParity(unsigned char state)
{
ModifyReg(REG_FRAMECON, BIT_TXPARITYEN | BIT_RXPARITYEN, state);
}
/**
* @brief 初始化读卡器以支持Type A卡片。
* @return 操作状态，SUCCESS表示成功。
*/
unsigned char ReaderA_Initial(void)
{
LoadProtocol(RX_TYPEA_106, TX_TYPEA_106);
ModifyReg(REG_TXMODE, BIT_RFON, FUN_ENABLE); // FORCE 100ask FUN_ENABLE
SetReg(REG_TXAMP, AMPLITUDE_A);
SetReg(REG_TXCON, 0x00);
SetReg(REG_RXANA, (HPCF_A << 3) | GAIN_A);
SetReg(0x5F, 0x08);
SetReg(REG_THNSET, 0xFF);
SetReg(REG_THNMIN, 0xC0);
SetReg(REG_RXTXCON, 0x80);
SetParity(FUN_ENABLE);
SetReg(REG_STATUS, 0); // 清除Crypto1On位
return SUCCESS;
}
/**
* @brief 初始化读卡器以支持Type B卡片。
* @return 操作状态，SUCCESS表示成功。
*/
unsigned char ReaderB_Initial(void)
{
LoadProtocol(RX_TYPEB_106, TX_TYPEB_106);
ModifyReg(REG_TXMODE, BIT_RFON, FUN_DISABLE); // FORCE 100ask FUN_DISABLE
SetReg(REG_TXAMP, AMPLITUDE_B);
SetReg(REG_TXCON, MODULATION_B);
SetReg(REG_RXANA, (HPCF_B << 3) | GAIN_B);
SetReg(0x5F, 0x08);
SetReg(REG_THNSET, 0xFF);
SetReg(REG_THNMIN, 0xC0);
SetReg(REG_RXTXCON, 0x80);
return SUCCESS;
}
/**
* @brief 初始化读卡器以支持Type V (ISO15693) 卡片。
* @return 操作状态，SUCCESS表示成功。
*/
unsigned char ReaderV_Initial(void)
{
LoadProtocol(RX_TYPEV_26, RX_TYPEV_26);
ModifyReg(REG_RXANA, MASK_RCV_GAIN | MASK_RCV_HPCF, FUN_DISABLE);
ModifyReg(REG_RXANA, (HPCF_V << 3) | GAIN_V, FUN_ENABLE);
SetParity(FUN_DISABLE);
SetReg(REG_TXAMP, AMPLITUDE_V);
SetReg(REG_TXCON, MODULATION_V);
SetReg(REG_TXI, 0x06);
SetReg(REG_THNSET, 0xFF);
SetReg(REG_THNMIN, 0x80);
SetReg(REG_THNADJ, 0x08);
SetReg(REG_RXTXCON, 0);
return SUCCESS;
}
/**
* @brief 初始化读卡器以支持Type F (FeliCa) 卡片。
* @return 操作状态，SUCCESS表示成功。
*/
unsigned char ReaderF_Initial(void)
{
ModifyReg(REG_MISC, 0x04, FUN_ENABLE);
LoadProtocol(RX_FELICA_212, TX_FELICA_212);
SetReg(REG_TXAMP, AMPLITUDE_F);
SetReg(REG_TXCON, MODULATION_F);
ModifyReg(REG_RXANA, MASK_RCV_GAIN | MASK_RCV_HPCF, FUN_DISABLE);
ModifyReg(REG_RXANA, (HPCF_F << 3) | GAIN_F, FUN_ENABLE);
SetParity(FUN_DISABLE);
SetReg(REG_THNSET, 0xFF);
SetReg(REG_THNMIN, 0x80);
SetReg(REG_THNADJ, 0x08);
ModifyReg(REG_MISC, 0x04, FUN_DISABLE);
return SUCCESS;
}
/**
* @brief 向Type A卡片发送WUPA（Wake-Up A）命令。
* @param picc_a [out] 指向存储卡片信息的结构体。
* @return 操作状态，SUCCESS表示成功，FAIL表示失败。
*/
unsigned char ReaderA_Wakeeup(struct picc_a_struct *picc_a)
{
unsigned char reg_data;
SetCommand(CMD_IDLE);
SetReg(REG_TXDATANUM, 0x0F);
ModifyReg(REG_FIFOCONTROL, BIT_FIFOFLUSH, FUN_ENABLE);
SetReg(REG_FIFODATA, RF_CMD_WUPA);
ModifyReg(REG_TXCRCCON, BIT_CRCEN, FUN_DISABLE);
ModifyReg(REG_RXCRCCON, BIT_CRCEN, FUN_DISABLE);
SetCommand(CMD_TRANSCEIVE);
rfid_delay_ms(2);
GetReg(REG_FIFOLENGTH, &reg_data);
if (reg_data != 2)
return FAIL;
GetReg(REG_FIFODATA, &picc_a->ATQA[0]);
GetReg(REG_FIFODATA, &picc_a->ATQA[1]);
return SUCCESS;
}
/**
* @brief 向Type A卡片发送REQA（Request A）命令。
* @param picc_a [out] 指向存储卡片信息的结构体。
* @return 操作状态，SUCCESS表示成功，FAIL表示失败。
*/
unsigned char ReaderA_Request(struct picc_a_struct *picc_a)
{
unsigned char reg_data;
SetCommand(CMD_IDLE);
SetReg(REG_TXDATANUM, 0x0F);
ModifyReg(REG_FIFOCONTROL, BIT_FIFOFLUSH, FUN_ENABLE);
SetReg(REG_FIFODATA, RF_CMD_REQA);
ModifyReg(REG_TXCRCCON, BIT_CRCEN, FUN_DISABLE);
ModifyReg(REG_RXCRCCON, BIT_CRCEN, FUN_DISABLE);
SetCommand(CMD_TRANSCEIVE);
rfid_delay_ms(2);
GetReg(REG_FIFOLENGTH, &reg_data);
if (reg_data != 2)
return FAIL;
GetReg(REG_FIFODATA, &picc_a->ATQA[0]);
GetReg(REG_FIFODATA, &picc_a->ATQA[1]);
return SUCCESS;
}
/**
* @brief 执行Type A卡片的防冲突流程。
* @param picc_a [in, out] 指向存储卡片信息的结构体。
* @return 操作状态，SUCCESS表示成功，FAIL表示失败。
*/
unsigned char ReaderA_Anticoll(struct picc_a_struct *picc_a)
{
unsigned char reg_data;
SetCommand(CMD_IDLE);
SetReg(REG_TXDATANUM, 0x08);
ModifyReg(REG_FIFOCONTROL, BIT_FIFOFLUSH, FUN_ENABLE);
SetReg(REG_FIFODATA, RF_CMD_ANTICOLL[picc_a->CASCADE_LEVEL]);
SetReg(REG_FIFODATA, 0x20);
ModifyReg(REG_TXCRCCON, BIT_CRCEN, FUN_DISABLE);
ModifyReg(REG_RXCRCCON, BIT_CRCEN, FUN_DISABLE);
SetCommand(CMD_TRANSCEIVE);
rfid_delay_ms(2);
GetReg(REG_FIFOLENGTH, &reg_data);
if (reg_data != 5)
return FAIL;
GetReg(REG_FIFODATA, &picc_a->UID[picc_a->CASCADE_LEVEL * 4]);
GetReg(REG_FIFODATA, &picc_a->UID[picc_a->CASCADE_LEVEL * 4 + 1]);
GetReg(REG_FIFODATA, &picc_a->UID[picc_a->CASCADE_LEVEL * 4 + 2]);
GetReg(REG_FIFODATA, &picc_a->UID[picc_a->CASCADE_LEVEL * 4 + 3]);
GetReg(REG_FIFODATA, &picc_a->BCC[picc_a->CASCADE_LEVEL]);
if ((picc_a->UID[picc_a->CASCADE_LEVEL * 4] ^ picc_a->UID[picc_a->CASCADE_LEVEL * 4 + 1] ^ picc_a->UID[picc_a->CASCADE_LEVEL * 4 + 2] ^ picc_a->UID[picc_a->CASCADE_LEVEL * 4 + 3]) == picc_a->BCC[picc_a->CASCADE_LEVEL])
return SUCCESS;
return FAIL;
}
/**
* @brief 选择一个已经过防冲突的Type A卡片。
* @param picc_a [in, out] 指向存储卡片信息的结构体。
* @return 操作状态，SUCCESS表示成功，FAIL表示失败。
*/
unsigned char ReaderA_Select(struct picc_a_struct *picc_a)
{
unsigned char reg_data;
SetCommand(CMD_IDLE);
SetReg(REG_TXDATANUM, 0x08);
ModifyReg(REG_FIFOCONTROL, BIT_FIFOFLUSH, FUN_ENABLE);
SetReg(REG_FIFODATA, RF_CMD_ANTICOLL[picc_a->CASCADE_LEVEL]);
SetReg(REG_FIFODATA, 0x70);
SetReg(REG_FIFODATA, picc_a->UID[picc_a->CASCADE_LEVEL * 4]);
SetReg(REG_FIFODATA, picc_a->UID[picc_a->CASCADE_LEVEL * 4 + 1]);
SetReg(REG_FIFODATA, picc_a->UID[picc_a->CASCADE_LEVEL * 4 + 2]);
SetReg(REG_FIFODATA, picc_a->UID[picc_a->CASCADE_LEVEL * 4 + 3]);
SetReg(REG_FIFODATA, picc_a->BCC[picc_a->CASCADE_LEVEL]);
ModifyReg(REG_TXCRCCON, BIT_CRCEN, FUN_ENABLE);
ModifyReg(REG_RXCRCCON, BIT_CRCEN, FUN_ENABLE);
SetCommand(CMD_TRANSCEIVE);
rfid_delay_ms(2);
GetReg(REG_ERROR, &reg_data);
if ((reg_data & 0x0F) != 0)
return FAIL;
GetReg(REG_FIFOLENGTH, &reg_data);
if (reg_data != 1)
return FAIL;
GetReg(REG_FIFODATA, &picc_a->SAK[picc_a->CASCADE_LEVEL]);
return SUCCESS;
}
/**
* @brief 激活Type A卡片（完成REQA, Anticoll, Select全过程）。
* @param picc_a [out] 指向存储卡片信息的结构体。
* @return 操作状态，SUCCESS表示成功，FAIL表示失败。
*/
unsigned char ReaderA_CardActivate(struct picc_a_struct *picc_a)
{
unsigned char result, cascade_level;
result = ReaderA_Request(picc_a);
if (result != SUCCESS)
return result;
if ((picc_a->ATQA[0] & 0xC0) == 0x00) // 单倍UID
{
cascade_level = 1;
picc_a->UID_Length = 4;
}
else if ((picc_a->ATQA[0] & 0xC0) == 0x40) // 双倍UID
{
cascade_level = 2;
picc_a->UID_Length = 7; // 实际是7字节
}
else if ((picc_a->ATQA[0] & 0xC0) == 0x80) // 三倍UID
{
cascade_level = 3;
picc_a->UID_Length = 10; // 实际是10字节
}
else
{
return FAIL; // 未知UID长度
}
for (picc_a->CASCADE_LEVEL = 0; picc_a->CASCADE_LEVEL < cascade_level; picc_a->CASCADE_LEVEL++)
{
result = ReaderA_Anticoll(picc_a);
if (result != SUCCESS)
return result;
result = ReaderA_Select(picc_a);
if (result != SUCCESS)
return result;
}
picc_a->CASCADE_LEVEL--;
return result;
}
/**
* @brief 向Type B卡片发送WUPB（Wake-Up B）命令。
* @param picc_b [out] 指向存储卡片信息的结构体。
* @return 操作状态，SUCCESS表示成功，FAIL表示失败。
*/
unsigned char ReaderB_Wakeup(struct picc_b_struct *picc_b)
{
unsigned char reg_data, i;
SetCommand(CMD_IDLE);
SetReg(REG_TXDATANUM, 0x08);
ModifyReg(REG_FIFOCONTROL, BIT_FIFOFLUSH, FUN_ENABLE);
SetReg(REG_FIFODATA, 0x05); // APf
SetReg(REG_FIFODATA, 0x00); // AFI (00:for all cards)
SetReg(REG_FIFODATA, 0x08); // PARAM(WUPB, Number of slots = 0)
ModifyReg(REG_TXCRCCON, BIT_CRCEN, FUN_ENABLE);
ModifyReg(REG_RXCRCCON, BIT_CRCEN, FUN_ENABLE);
SetCommand(CMD_TRANSCEIVE);
rfid_delay_ms(10);
GetReg(REG_ERROR, &reg_data);
if ((reg_data & 0x0F) != 0)
return FAIL;
GetReg(REG_FIFOLENGTH, &reg_data);
if (reg_data != 12)
return FAIL;
for (i = 0; i < 12; i++)
GetReg(REG_FIFODATA, &picc_b->ATQB[i]);
memcpy(picc_b->PUPI, picc_b->ATQB + 1, 4);
memcpy(picc_b->APPLICATION_DATA, picc_b->ATQB + 6, 4);
memcpy(picc_b->PROTOCOL_INF, picc_b->ATQB + 10, 3);
return SUCCESS;
}
/**
* @brief 向Type B卡片发送REQB（Request B）命令。
* @param picc_b [out] 指向存储卡片信息的结构体。
* @return 操作状态，SUCCESS表示成功，FAIL表示失败。
*/
unsigned char ReaderB_Request(struct picc_b_struct *picc_b)
{
unsigned char reg_data, i;
SetCommand(CMD_IDLE);
SetReg(REG_TXDATANUM, 0x08);
ModifyReg(REG_FIFOCONTROL, BIT_FIFOFLUSH, FUN_ENABLE);
SetReg(REG_FIFODATA, 0x05); // APf
SetReg(REG_FIFODATA, 0x00); // AFI (00:for all cards)
SetReg(REG_FIFODATA, 0x00); // PARAM(REQB, Number of slots = 0)
ModifyReg(REG_TXCRCCON, BIT_CRCEN, FUN_ENABLE);
ModifyReg(REG_RXCRCCON, BIT_CRCEN, FUN_ENABLE);
SetCommand(CMD_TRANSCEIVE);
rfid_delay_ms(10);
GetReg(REG_ERROR, &reg_data);
if ((reg_data & 0x0F) != 0)
return FAIL;
GetReg(REG_FIFOLENGTH, &reg_data);
if (reg_data != 12)
return FAIL;
for (i = 0; i < 12; i++)
GetReg(REG_FIFODATA, &picc_b->ATQB[i]);
memcpy(picc_b->PUPI, picc_b->ATQB + 1, 4);
memcpy(picc_b->APPLICATION_DATA, picc_b->ATQB + 6, 4);
memcpy(picc_b->PROTOCOL_INF, picc_b->ATQB + 10, 3);
return SUCCESS;
}
/**
* @brief 向Type B卡片发送ATTRIB命令，以选择卡片并设置通信参数。
* @param picc_b [in, out] 指向存储卡片信息的结构体。
* @return 操作状态，SUCCESS表示成功，FAIL表示失败。
*/
unsigned char ReaderB_Attrib(struct picc_b_struct *picc_b)
{
unsigned char reg_data;
SetCommand(CMD_IDLE);
SetReg(REG_TXDATANUM, 0x08);
ModifyReg(REG_FIFOCONTROL, BIT_FIFOFLUSH, FUN_ENABLE);
SetReg(REG_FIFODATA, 0x1D);
SetReg(REG_FIFODATA, picc_b->PUPI[0]);
SetReg(REG_FIFODATA, picc_b->PUPI[1]);
SetReg(REG_FIFODATA, picc_b->PUPI[2]);
SetReg(REG_FIFODATA, picc_b->PUPI[3]);
SetReg(REG_FIFODATA, 0x00); // Param1
SetReg(REG_FIFODATA, 0x08); // Param2
SetReg(REG_FIFODATA, 0x01); // COMPATIBLE WITH 14443-4
SetReg(REG_FIFODATA, 0x01); // CID:01
ModifyReg(REG_TXCRCCON, BIT_CRCEN, FUN_ENABLE);
ModifyReg(REG_RXCRCCON, BIT_CRCEN, FUN_ENABLE);
SetCommand(CMD_TRANSCEIVE);
rfid_delay_ms(10);
GetReg(REG_ERROR, &reg_data);
if ((reg_data & 0x0F) != 0)
return FAIL;
GetReg(REG_FIFOLENGTH, &reg_data);
if (reg_data != 1)
return FAIL;
GetReg(REG_FIFODATA, &reg_data);
picc_b->CID = reg_data & 0x0F;
return SUCCESS;
}
/**
* @brief 向Type B卡片发送HALT命令，使其进入休眠状态。
* @param picc_b [in, out] 指向存储卡片信息的结构体。
* @return 操作状态，SUCCESS表示成功，FAIL表示失败。
*/
unsigned char ReaderB_Halt(struct picc_b_struct *picc_b)
{
unsigned char reg_data;
SetCommand(CMD_IDLE);
SetReg(REG_TXDATANUM, 0x08);
ModifyReg(REG_FIFOCONTROL, BIT_FIFOFLUSH, FUN_ENABLE);
SetReg(REG_FIFODATA, 0x50);
SetReg(REG_FIFODATA, picc_b->PUPI[0]);
SetReg(REG_FIFODATA, picc_b->PUPI[1]);
SetReg(REG_FIFODATA, picc_b->PUPI[2]);
SetReg(REG_FIFODATA, picc_b->PUPI[3]);
ModifyReg(REG_TXCRCCON, BIT_CRCEN, FUN_ENABLE);
ModifyReg(REG_RXCRCCON, BIT_CRCEN, FUN_ENABLE);
SetCommand(CMD_TRANSCEIVE);
rfid_delay_ms(10);
GetReg(REG_ERROR, &reg_data);
if ((reg_data & 0x0F) != 0)
return FAIL;
GetReg(REG_FIFOLENGTH, &reg_data);
if (reg_data != 1)
return FAIL;
GetReg(REG_FIFODATA, &reg_data);
*picc_b->Answer_to_HALT = reg_data & 0x0F;
return SUCCESS;
}
/**
* @brief 获取Type B卡片的序列号（这是一个自定义命令，非标准）。
* @param picc_b [out] 指向存储卡片信息的结构体。
* @return 操作状态，SUCCESS表示成功，FAIL表示失败。
*/
unsigned char ReaderB_Get_SN(struct picc_b_struct *picc_b)
{
unsigned char reg_data, i;
SetCommand(CMD_IDLE);
SetReg(REG_TXDATANUM, 0x08);
ModifyReg(REG_FIFOCONTROL, BIT_FIFOFLUSH, FUN_ENABLE);
SetReg(REG_FIFODATA, 0x00);
SetReg(REG_FIFODATA, 0x36);
SetReg(REG_FIFODATA, 0x00);
SetReg(REG_FIFODATA, 0x00);
SetReg(REG_FIFODATA, 0x08);
ModifyReg(REG_TXCRCCON, BIT_CRCEN, FUN_ENABLE);
ModifyReg(REG_RXCRCCON, BIT_CRCEN, FUN_ENABLE);
SetCommand(CMD_TRANSCEIVE);
rfid_delay_ms(10);
GetReg(REG_ERROR, &reg_data);
if ((reg_data & 0x0F) != 0)
return FAIL;
GetReg(REG_FIFOLENGTH, &reg_data);
if (reg_data != 10)
return FAIL;
for (i = 0; i < 8; i++)
GetReg(REG_FIFODATA, &picc_b->SN[i]);
return SUCCESS;
}
/**
* @brief 向Type V (ISO15693) 卡片发送Inventory命令。
* @param picc_v [out] 指向存储卡片信息的结构体。
* @return 操作状态，SUCCESS表示成功，FAIL表示失败。
*/
unsigned char ReaderV_Inventory(struct picc_v_struct *picc_v)
{
unsigned char reg_data, i;
SetCommand(CMD_IDLE);
SetReg(REG_TXDATANUM, 0x08);
ModifyReg(REG_FIFOCONTROL, BIT_FIFOFLUSH, FUN_ENABLE);
SetReg(REG_FIFODATA, 0x26);
SetReg(REG_FIFODATA, 0x01);
SetReg(REG_FIFODATA, 0x00);
ModifyReg(REG_TXCRCCON, BIT_CRCEN, FUN_ENABLE);
ModifyReg(REG_RXCRCCON, BIT_CRCEN, FUN_ENABLE);
SetCommand(CMD_TRANSCEIVE);
rfid_delay_ms(10);
GetReg(REG_ERROR, &reg_data);
if ((reg_data & 0x0F) != 0)
return FAIL;
GetReg(REG_FIFOLENGTH, &reg_data);
if (reg_data != 10)
return FAIL;
GetReg(REG_FIFODATA, &picc_v->RESPONSE);
GetReg(REG_FIFODATA, &reg_data); // DSFID
for (i = 0; i < 8; i++)
{
GetReg(REG_FIFODATA, &picc_v->UID[i]);
}
return SUCCESS;
}
/**
* @brief 选择一个已获取UID的Type V卡片。
* @param picc_v [in, out] 指向存储卡片信息的结构体。
* @return 操作状态，SUCCESS表示成功，FAIL表示失败。
*/
unsigned char ReaderV_Select(struct picc_v_struct *picc_v)
{
unsigned char reg_data;
SetCommand(CMD_IDLE);
SetReg(REG_TXDATANUM, 0x08);
ModifyReg(REG_FIFOCONTROL, BIT_FIFOFLUSH, FUN_ENABLE);
SetReg(REG_FIFODATA, 0x22); // Addressed flag
SetReg(REG_FIFODATA, 0x25); // Select command
SetReg(REG_FIFODATA, picc_v->UID[0]);
SetReg(REG_FIFODATA, picc_v->UID[1]);
SetReg(REG_FIFODATA, picc_v->UID[2]);
SetReg(REG_FIFODATA, picc_v->UID[3]);
SetReg(REG_FIFODATA, picc_v->UID[4]);
SetReg(REG_FIFODATA, picc_v->UID[5]);
SetReg(REG_FIFODATA, picc_v->UID[6]);
SetReg(REG_FIFODATA, picc_v->UID[7]);
ModifyReg(REG_TXCRCCON, BIT_CRCEN, FUN_ENABLE);
ModifyReg(REG_RXCRCCON, BIT_CRCEN, FUN_ENABLE);
SetCommand(CMD_TRANSCEIVE);
rfid_delay_ms(10);
GetReg(REG_ERROR, &reg_data);
if ((reg_data & 0x0F) != 0)
return FAIL;
GetReg(REG_FIFOLENGTH, &reg_data);
if (reg_data != 1)
return FAIL;
GetReg(REG_FIFODATA, &picc_v->RESPONSE);
return SUCCESS;
}
/**
* @brief 读取Type V卡片的单个数据块。
* @param block_num [in] 要读取的块号。
* @param picc_v [out] 指向存储卡片信息的结构体，读取的数据将存入 `BLOCK_DATA`。
* @return 操作状态，SUCCESS表示成功，FAIL表示失败。
*/
unsigned char ReaderV_ReadSingleBlock(unsigned char block_num, struct picc_v_struct *picc_v)
{
unsigned char reg_data, i;
SetCommand(CMD_IDLE);
SetReg(REG_TXDATANUM, 0x08);
ModifyReg(REG_FIFOCONTROL, BIT_FIFOFLUSH, FUN_ENABLE);
SetReg(REG_FIFODATA, 0x02); // Addressed flag
SetReg(REG_FIFODATA, 0x20); // Read Single Block command
SetReg(REG_FIFODATA, block_num);
ModifyReg(REG_TXCRCCON, BIT_CRCEN, FUN_ENABLE);
ModifyReg(REG_RXCRCCON, BIT_CRCEN, FUN_ENABLE);
SetCommand(CMD_TRANSCEIVE);
rfid_delay_ms(10);
GetReg(REG_ERROR, &reg_data);
if ((reg_data & 0x0F) != 0)
return FAIL;
GetReg(REG_FIFOLENGTH, &reg_data);
if (reg_data != 5) // 1 byte response flag + 4 bytes data
return FAIL;
GetReg(REG_FIFODATA, &picc_v->RESPONSE);
for (i = 0; i < 4; i++)
{
GetReg(REG_FIFODATA, &picc_v->BLOCK_DATA[i]);
}
return SUCCESS;
}
/**
* @brief 向Type V卡片的单个数据块写入数据。
* @param block_num [in] 要写入的块号。
* @param picc_v [in] 指向存储卡片信息的结构体，要写入的数据在 `BLOCK_DATA` 中。
* @return 操作状态，SUCCESS表示成功，FAIL表示失败。
*/
unsigned char ReaderV_WriteSingleBlock(unsigned char block_num, struct picc_v_struct *picc_v)
{
unsigned char reg_data;
SetCommand(CMD_IDLE);
SetReg(REG_TXDATANUM, 0x08);
ModifyReg(REG_FIFOCONTROL, BIT_FIFOFLUSH, FUN_ENABLE);
SetReg(REG_FIFODATA, 0x02); // Addressed flag
SetReg(REG_FIFODATA, 0x21); // Write Single Block command
SetReg(REG_FIFODATA, block_num);
SetReg(REG_FIFODATA, picc_v->BLOCK_DATA[0]);
SetReg(REG_FIFODATA, picc_v->BLOCK_DATA[1]);
SetReg(REG_FIFODATA, picc_v->BLOCK_DATA[2]);
SetReg(REG_FIFODATA, picc_v->BLOCK_DATA[3]);
ModifyReg(REG_TXCRCCON, BIT_CRCEN, FUN_ENABLE);
ModifyReg(REG_RXCRCCON, BIT_CRCEN, FUN_ENABLE);
SetCommand(CMD_TRANSCEIVE);
rfid_delay_ms(10);
GetReg(REG_ERROR, &reg_data);
if ((reg_data & 0x0F) != 0)
return FAIL;
GetReg(REG_FIFOLENGTH, &reg_data);
if (reg_data != 1)
return FAIL;
GetReg(REG_FIFODATA, &picc_v->RESPONSE);
return SUCCESS;
}
/**
* @brief 向Type F (FeliCa) 卡片发送Inventory命令。
* @param picc_f [out] 指向存储卡片信息的结构体。
* @return 操作状态，SUCCESS表示成功，FAIL表示失败。
*/
unsigned char ReaderF_Inventory(struct picc_f_struct *picc_f)
{
unsigned char reg_data, i;
SetCommand(CMD_IDLE);
SetReg(REG_TXDATANUM, 0x08);
ModifyReg(REG_FIFOCONTROL, BIT_FIFOFLUSH, FUN_ENABLE);
SetReg(REG_FIFODATA, 0x06);
SetReg(REG_FIFODATA, 0x00);
SetReg(REG_FIFODATA, 0xFF);
SetReg(REG_FIFODATA, 0xFF);
SetReg(REG_FIFODATA, 0x10);
SetReg(REG_FIFODATA, 0x00);
ModifyReg(REG_TXCRCCON, BIT_CRCEN, FUN_ENABLE);
ModifyReg(REG_RXCRCCON, BIT_CRCEN, FUN_ENABLE);
SetCommand(CMD_TRANSCEIVE);
rfid_delay_ms(10);
GetReg(REG_ERROR, &reg_data);
if ((reg_data & 0x0F) != 0)
return FAIL;
GetReg(REG_FIFOLENGTH, &reg_data);
if (reg_data != 18)
return FAIL;
GetReg(REG_FIFODATA, &reg_data); // Length
GetReg(REG_FIFODATA, &reg_data); // Response code
for (i = 0; i < 8; i++)
{
GetReg(REG_FIFODATA, &picc_f->UID[i]);
}
return SUCCESS;
}

303
apps/earphone/remote_control/RFID/rfid_event.c Normal file
View File

@ -0,0 +1,303 @@
/********************************************************************************************************
* @file rfid_main.c
* @brief RFID 读卡器应用层主逻辑文件
********************************************************************************************************/
#include "./include/rfid_main.h"
#include "./include/READER.h"
#include "./include/READER_REG.h"
#include "./include/MIFARE.h"
#include "./include/NTAG.h"
#include "./include/CPU_CARD.h"
#include "./rfid_hal.h"
#define FUN_ENABLE_XLOG 1
#ifdef xlog
#undef xlog
#endif
#if FUN_ENABLE_XLOG
#define xlog(format, ...) printf("[XT:%s] " format, __func__, ##__VA_ARGS__)
#else
#define xlog(format, ...) ((void)0)
#endif
/**
* @brief 处理Type A卡片事件。
* @details
* 该函数执行ISO/IEC 14443 Type A卡片的完整激活流程，包括：
* 1. 初始化读卡器以支持Type A协议。
* 2. 打开RF场。
* 3. 请求（Request）和防冲突（Anticollision），最终激活卡片。
* 4. 根据卡片的SAK（Select Acknowledge）值，判断卡片具体类型（如Mifare, NTAG, CPU卡）并调用相应的处理函数。
* 5. 操作结束后关闭RF场。
* @return 无。
*/
void TYPE_A_EVENT(void)
{
unsigned char result;
int i;
xlog("TYPE_A_EVENT begin\n");
// 初始化读卡器为Type A模式
result = ReaderA_Initial();
if (result != SUCCESS)
{
xlog("INIT_ERROR\r\n");
SetCW(FUN_DISABLE);
return;
}
// 打开RF场（载波）
result = SetCW(FUN_ENABLE);
if (result != SUCCESS)
{
xlog("CW_ERROR\r\n");
SetCW(FUN_DISABLE);
return;
}
// 激活Type A卡片
result = ReaderA_CardActivate(&PICC_A);
if (result != SUCCESS)
{
// xlog("ReaderA_CardActivate_ERROR\r\n");
SetCW(FUN_DISABLE);
return;
}
xlog("************* TYPE A CARD ************* \r\n");
xlog("-> ATQA = %02X%02X\r\n", PICC_A.ATQA[0], PICC_A.ATQA[1]);
if (PICC_A.UID_Length > 0)
{
xlog("-> UID = ");
for (i = 0; i < PICC_A.UID_Length; i++)
{
xlog("%02X", PICC_A.UID[i]);
}
xlog("\r\n");
}
xlog("-> SAK = %02X\r\n", PICC_A.SAK[0]);
// 根据SAK值判断卡片类型
if (PICC_A.SAK[0] == 0x08)
{
xlog("************* Mifare CARD ************* \r\n");
result = MIFARE_CARD_EVENT();
}
else if ((PICC_A.SAK[0] == 0x28) || (PICC_A.SAK[0] == 0x20))
{
xlog("************* CPU CARD ************* \r\n");
result = CPU_CARD_EVENT();
}
else if (PICC_A.SAK[0] == 0x04)
{
xlog("************* NTAG CARD ************* \r\n");
result = NTAG_EVENT();
}
SetCW(FUN_DISABLE); // 关闭RF场
}
/**
* @brief 处理Type B卡片事件。
* @details
* 该函数执行ISO/IEC 14443 Type B卡片的激活流程，包括：
* 1. 初始化读卡器以支持Type B协议。
* 2. 打开RF场。
* 3. 发送REQB/WUPB命令寻卡。
* 4. 发送ATTRIB命令选卡。
* 5. 获取卡片序列号（SN）。
* 6. 操作结束后关闭RF场。
* @return 无。
*/
void TYPE_B_EVENT(void)
{
unsigned char result;
int i;
xlog("TYPE_B_EVENT begin\n");
ReaderB_Initial();
SetCW(FUN_ENABLE);
result = ReaderB_Request(&PICC_B);
if (result != SUCCESS)
{
SetCW(FUN_DISABLE);
return;
}
xlog("************* TYPE B CARD ************* \r\n");
// 打印ATQB信息
xlog("-> ATQB = ");
for(i=0; i<12; i++) xlog("%02X", PICC_B.ATQB[i]);
xlog("\r\n");
result = ReaderB_Attrib(&PICC_B);
if (result != SUCCESS)
{
SetCW(FUN_DISABLE);
return;
}
xlog("-> ATTRIB = %02X\r\n", PICC_B.CID);
result = ReaderB_Get_SN(&PICC_B);
if (result != SUCCESS)
{
SetCW(FUN_DISABLE);
return;
}
xlog("-> SN = ");
for(i=0; i<8; i++) xlog("%02X", PICC_B.SN[i]);
xlog("\r\n");
SetCW(FUN_DISABLE);
}
/**
* @brief 处理Type V (ISO/IEC 15693) 卡片事件。
* @details
* 该函数执行ISO/IEC 15693 Vicinity卡片的交互流程，包括：
* 1. 初始化读卡器以支持15693协议。
* 2. 打开RF场。
* 3. 发送Inventory命令寻卡并获取UID。
* 4. 发送Select命令选择卡片。
* 5. 示例性地对第4块进行写操作，然后再读回校验。
* 6. 操作结束后关闭RF场。
* @return 无。
*/
void TYPE_V_EVENT(void)
{
unsigned char result, i;
xlog("TYPE_V_EVENT begin\n");
ReaderV_Initial();
SetCW(FUN_ENABLE);
result = ReaderV_Inventory(&PICC_V);
if (result != SUCCESS)
{
SetCW(FUN_DISABLE);
xlog("-> ReaderV Inventory ERROR!\r\n");
return;
}
xlog("************* TYPE V CARD ************* \r\n");
xlog("UID=");
for (i = 0; i < 8; i++)
{
xlog("%02X", PICC_V.UID[i]);
}
xlog("\r\n");
result = ReaderV_Select(&PICC_V);
if (result != SUCCESS)
{
SetCW(FUN_DISABLE);
xlog("-> ReaderV Select ERROR!\r\n");
return;
}
// 示例：写单个块
memcpy(PICC_V.BLOCK_DATA, "\x11\x22\x33\x44", 4);
result = ReaderV_WriteSingleBlock(4, &PICC_V);
if (result != SUCCESS)
{
SetCW(FUN_DISABLE);
xlog("-> ReaderV WriteSingleBlock ERROR!\r\n");
return;
}
xlog("WriteSingleBlock SUCCESS\r\n");
// 示例：读单个块
result = ReaderV_ReadSingleBlock(4, &PICC_V);
if (result != SUCCESS)
{
SetCW(FUN_DISABLE);
xlog("-> ReaderV ReadSingleBlock ERROR!\r\n");
return;
}
xlog("BLOCK DATA = %02X%02X%02X%02X \r\n", PICC_V.BLOCK_DATA[0], PICC_V.BLOCK_DATA[1], PICC_V.BLOCK_DATA[2], PICC_V.BLOCK_DATA[3]);
SetCW(FUN_DISABLE);
}
/**
* @brief 处理Type F (FeliCa) 卡片事件。
* @details
* 该函数执行FeliCa卡片的交互流程，包括：
* 1. 初始化读卡器以支持FeliCa协议。
* 2. 打开RF场。
* 3. 发送Inventory命令寻卡并获取UID。
* 4. 后续可以添加与FeliCa卡的数据交换命令。
* 5. 操作结束后关闭RF场。
* @note 当前实现仅包含寻卡部分。
* @return 无。
*/
void TYPE_F_EVENT(void)
{
unsigned char result, i;
xlog("TYPE_F_EVENT begin\n");
ReaderF_Initial();
SetCW(FUN_ENABLE);
result = ReaderF_Inventory(&PICC_F);
if (result != SUCCESS)
{
SetCW(FUN_DISABLE);
return;
}
xlog("************* TYPE F CARD ************* \r\n");
xlog("->TYPE F UID = ");
for(i=0; i<8; i++) xlog("%02X", PICC_F.UID[i]);
xlog("\r\n");
SetCW(FUN_DISABLE);
}
/**
* @brief RFID模块的主任务函数。
* @details
* 利用定时器调用
* @return 无。
*/
void rfid_task_fuc(void)
{
unsigned char result, reg_data;
static u8 first_init = 0;
if(first_init == 0){
first_init = 1;
// rfid_hal_init();
FM176XX_HardInit();
// 2. 复位 FM176XX 芯片
while (1)
{
result = FM176XX_SoftReset();
if (result != SUCCESS)
{
xlog("FM176XX HardReset FAIL\r\n");
rfid_delay_ms(1000); // 延时后重试
}
else
{
xlog("FM176XX HardReset SUCCESS\r\n");
break;
}
}
}
// 3. 读取芯片版本号，确认通信是否正常
GetReg(REG_VERSION, &reg_data);
xlog("REG_VERSION = %02X\r\n", reg_data);
TYPE_A_EVENT();
TYPE_B_EVENT();
TYPE_V_EVENT();
TYPE_F_EVENT();
}

91
apps/earphone/remote_control/RFID/rfid_hal.c Normal file
View File

@ -0,0 +1,91 @@
#include "./rfid_hal.h"
#include "gSensor/gSensor_manage.h"
#include "./include/rfid_main.h"
#include "./include/READER_REG.h"
#define FUN_ENABLE_XLOG 1
#ifdef xlog
#undef xlog
#endif
#if FUN_ENABLE_XLOG
#define xlog(format, ...) printf("[XT:%s] " format, __func__, ##__VA_ARGS__)
#else
#define xlog(format, ...) ((void)0)
#endif
#define INTERFACE_TYPE 0
//////////////////////////////////////////////////////////////////////////////////////////////////
//
#if INTERFACE_TYPE == 0 //iic接口
/*
IF2 IF0 ADDR
0 0 0x28
0 1 0x29
1 0 0x2A
1 1 0x2B
*/
#define FM176_7BIT_ADDR 0x28 //后两位地址由IF2、IF1决定
#define FM176_READ_ADDR (FM176_7BIT_ADDR << 1)
#define FM176_WRITE_ADDR ((FM176_7BIT_ADDR << 1) | 0x01)
unsigned char FM176XX_HardInit(void){
int ret = hw_iic_init(0);
xlog("init iic result:%d\n", ret); //返回0成功
}
/**
* @brief 从FM176XX芯片读取一个字节的寄存器值。
* @param address [in] 目标寄存器的地址。
* @param reg_data [out] 指向用于存储读取数据的字节的指针。
* @return 操作状态，SUCCESS表示成功。
* @details
* 接口：iic
*/
unsigned char GetReg(unsigned char address, unsigned char *reg_data){
if(_gravity_sensor_get_ndata(FM176_READ_ADDR, address, reg_data, 1)){
return SUCCESS;
}else{
return FAIL;
}
}
/**
* @brief 向FM176XX芯片写入一个字节的寄存器值。
* @param address [in] 目标寄存器的地址。
* @param reg_data [in] 要写入的字节数据。
* @return 操作状态，SUCCESS表示成功。
* @details
* 接口：iic
*/
unsigned char SetReg(unsigned char address, unsigned char reg_data){
if(gravity_sensor_command(FM176_WRITE_ADDR, address, reg_data) == 0){
return FAIL;
}else{
return SUCCESS;
}
}
/**
* @brief 软件复位，命令：0x1F
*
* @return unsigned char
*/
unsigned char FM176XX_SoftReset(void){
gravity_sensor_command(FM176_WRITE_ADDR,REG_COMMAND,0x1F);
}
#elif INTERFACE_TYPE == 1 //spi
#elif INTERFACE_TYPE == 2 //uart
#endif
void rfid_delay_ms(unsigned int ms){
os_time_dly(ms/10);
}

76
apps/earphone/remote_control/RFID/rfid_hal.h Normal file
View File

@ -0,0 +1,76 @@
/********************************************************************************************************
* @file rfid_hal.h
* @brief RFID 硬件抽象层 (HAL) 接口定义
* @details
*
********************************************************************************************************/
#ifndef RFID_HAL_H
#define RFID_HAL_H
#include "system/includes.h"
/********************************************************************************************************
* 函数原型声明
********************************************************************************************************/
/**
* @brief 从FM176XX芯片读取一个字节的寄存器值。
* @param address [in] 目标寄存器的地址。
* @param reg_data [out] 指向用于存储读取数据的字节的指针。
* @return 操作状态，SUCCESS表示成功。
* @details
* 接口：uart、iic、spi
* 自行实现
*/
unsigned char GetReg(unsigned char address, unsigned char *reg_data);
/**
* @brief 向FM176XX芯片写入一个字节的寄存器值。
* @param address [in] 目标寄存器的地址。
* @param reg_data [in] 要写入的字节数据。
* @return 操作状态，SUCCESS表示成功。
* @details
* 接口：uart、iic、spi
* 自行实现
*/
unsigned char SetReg(unsigned char address, unsigned char reg_data);
/**
* @brief 接口硬件初始化
*
* @return unsigned char
*/
unsigned char FM176XX_HardInit(void);
/**
* @brief 硬件复位
* 通过控制RST引脚产生一个低电平脉冲来复位芯片。
* 复位后，会读取命令寄存器（REG_COMMAND）的值进行检查，
* 如果值不为0x40，则认为复位失败。
*
* @return unsigned char
*/
unsigned char FM176XX_HardReset(void);
/**
* @brief 软件复位，命令：0x1F
*
* @return unsigned char
*/
unsigned char FM176XX_SoftReset(void);
/**
* @brief 实现一个毫秒级的延时。
* @param ms [in] 要延时的毫秒数。
* @return 无。
* @details
* 一个阻塞式延时函数。
*/
void rfid_delay_ms(unsigned int ms);
#endif // RFID_HAL_H

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

@ -1,219 +0,0 @@
/*
静态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
View File

@ -1,41 +0,0 @@
#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

596
apps/earphone/xtell_Sensor/A_hide/10/skiing_tracker.c
View File

@ -1,596 +0,0 @@
/*
*/
#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 STOP_ACC_VARIANCE_THRESHOLD 0.2f
// 陀螺仪方差阈值
#define STOP_GYR_VARIANCE_THRESHOLD 5.0f
// 静止时候的陀螺仪模长
#define STOP_GYR_MAG_THRESHOLD 15
// --- --- ---
// --- 启动滑雪阈值 ---
// 加速度模长与重力的差值大于此值，认为开始运动；降低阈值，让“油门”更灵敏，以便能捕捉到真实的慢速启动
#define START_ACC_MAG_THRESHOLD 1.0f //0.5、1
// 陀螺仪方差阈值，以允许启动瞬间的正常抖动，但仍能过滤掉混乱的、非滑雪的晃动。
#define START_GYR_VARIANCE_THRESHOLD 15.0f
// --- --- ---
// --- 滑雪过程 ---
//加速度 模长(不含重力），低于此值视为 在做匀速运动
#define SKIING_ACC_MAG_THRESHOLD 0.5f
//陀螺仪 模长，高于此值视为 摔倒了
#define FALLEN_GRY_MAG_THRESHOLD 2000.0f //未确定
// --- --- ---
// --- 原地旋转抖动 ---
// 加速度 方差 阈值。此值比 静止检测 阈值更宽松，
#define WOBBLE_ACC_VARIANCE_THRESHOLD 0.5f
// 加速度 模长 阈值
#define WOBBLE_ACC_MAG_THRESHOLD 1.0f
// 角速度 总模长 大于此值(度/秒)，认为正在进行非滑雪的旋转或摆动
#define ROTATION_GYR_MAG_THRESHOLD 30.0f
// --- --- ---
// --- 滑雪转弯动 ---
// 加速度 方差 阈值，大于此值，滑雪过程可能发生了急转弯
#define WHEEL_ACC_VARIANCE_THRESHOLD 7.0f
// 角速度 总模长 大于此值(度/秒)，认为滑雪过程中进行急转弯
#define WHEEL_GYR_MAG_THRESHOLD 500.0f //
// --- --- ---
// --- 跳跃 ---
// 加速度模长低于此值(g)，认为进入失重状态(IN_AIR)
#define AIRBORNE_ACC_MAG_LOW_THRESHOLD 0.4f
// 加速度模长高于此值(g)，认为发生落地冲击(LANDING)
#define LANDING_ACC_MAG_HIGH_THRESHOLD 3.5f
// 起跳加速度阈值(g)，用于进入TAKING_OFF状态
#define TAKEOFF_ACC_MAG_HIGH_THRESHOLD 1.8f
// 进入空中状态确认计数：需要连续3个采样点加速度低于阈值才判断为起跳
#define AIRBORNE_CONFIRM_COUNT 3
// 落地状态确认计数：加速度恢复到1g附近并持续2个采样点(20ms)则认为已落地
#define GROUNDED_CONFIRM_COUNT 2
// 最大滞空时间(秒)，超过此时间强制认为已落地，防止状态锁死
#define MAX_TIME_IN_AIR 12.5f
// --- --- ---
// --- 用于消除积分漂移的滤波器和阈值 ---
// 高通滤波器系数 (alpha)。alpha 越接近1，滤除低频(直流偏移)的效果越强，但可能滤掉真实的慢速运动。
// alpha = RC / (RC + dt)，参考RC电路而来，fc ≈ (1 - alpha) / (2 * π * dt)
#define HPF_ALPHA 0.999f
//0.995f： 0.08 Hz 的信号
//0.999f： 0.0159 Hz
// --- --- ---
// --- 低通滤波器 ---
// 低通滤波器系数 (alpha)。alpha 越小，滤波效果越强（更平滑），但延迟越大。
// alpha 推荐范围 0.7 ~ 0.95。可以从 0.85 开始尝试。
#define LPF_ALPHA 0.7f
// 加速度死区阈值 (m/s^2)。低于此阈值的加速度被认为是噪声，不参与积分。
// 设得太高会忽略真实的慢速启动，设得太低则无法有效抑制噪声。
//参考：0.2f ~ 0.4f
#define ACC_DEAD_ZONE_THRESHOLD 0.05f
// --- 模拟摩擦力，进行速度衰减 ---
#define SPEED_ATTENUATION 1.0f //暂不模拟
BLE_KS_send_data_t KS_data;
#ifdef XTELL_TEST
debug_t debug1;
debug_t debug2;
#endif
static skiing_tracker_t my_skiing_tracker;
//////////////////////////////////////////////////////////////////////////////////////////////////
//实现
void clear_speed(void){
my_skiing_tracker.state = STATIC;
memset(my_skiing_tracker.velocity, 0, sizeof(my_skiing_tracker.velocity));
my_skiing_tracker.speed = 0;
}
void start_detection(void){
my_skiing_tracker.state = STATIC;
memset(my_skiing_tracker.velocity, 0, sizeof(my_skiing_tracker.velocity));
my_skiing_tracker.distance = 0;
my_skiing_tracker.speed = 0;
}
void stop_detection(void){
my_skiing_tracker.state = STOP_DETECTION;
memset(my_skiing_tracker.velocity, 0, sizeof(my_skiing_tracker.velocity));
my_skiing_tracker.speed = 0;
}
/**
* @brief 初始化滑雪追踪器
*
* @param tracker
*/
void skiing_tracker_init(skiing_tracker_t *tracker)
{
if (!tracker) {
return;
}
// 使用memset一次性清零整个结构体，包括新增的缓冲区
memset(tracker, 0, sizeof(skiing_tracker_t));
tracker->state = STATIC;
}
/**
* @brief 当检测到落地时，计算空中的水平飞行距离并累加到总距离
*/
static void calculate_air_distance(skiing_tracker_t *tracker) {
float horizontal_speed_on_takeoff = sqrtf(
tracker->initial_velocity_on_takeoff[0] * tracker->initial_velocity_on_takeoff[0] +
tracker->initial_velocity_on_takeoff[1] * tracker->initial_velocity_on_takeoff[1]
);
float distance_in_air = horizontal_speed_on_takeoff * tracker->time_in_air;
tracker->distance += distance_in_air;
}
/**
* @brief 在设备坐标系下，从原始加速度数据中移除重力分量
* @param acc_device 输入：设备坐标系下的原始加速度 [x, y, z], 单位 m/s^2
* @param angle 输入：姿态角 [pitch, roll, yaw]，单位: 度
* @param acc_linear_device 输出：设备坐标系下移除重力后的线性加速度 [x, y, z]
*/
void remove_gravity_in_device_frame(const float *acc_device, const float *angle, float *acc_linear_device)
{
float pitch = angle[0] * DEG_TO_RAD; // 绕 Y 轴
float roll = angle[1] * DEG_TO_RAD; // 绕 X 轴
float yaw = angle[2] * DEG_TO_RAD; // 绕 Z 轴
float cp = cosf(pitch);
float sp = sinf(pitch);
float cr = cosf(roll);
float sr = sinf(roll);
float cy = cosf(yaw);
float sy = sinf(yaw);
// 世界坐标系下的重力矢量
const float g_world[3] = {0.0f, 0.0f, G_ACCELERATION};
// 计算旋转矩阵 R 的转置矩阵 R_transpose
// R (Z-Y-X) =
// [ cy*cp, cy*sp*sr - sy*cr, cy*sp*cr + sy*sr]
// [ sy*cp, sy*sp*sr + cy*cr, sy*sp*cr - cy*sr]
// [ -sp, cp*sr, cp*cr ]
//
// R_transpose =
// [ cy*cp, sy*cp, -sp ]
// [ cy*sp*sr - sy*cr, sy*sp*sr + cy*cr, cp*sr ]
// [ cy*sp*cr + sy*sr, sy*sp*cr - cy*sr, cp*cr ]
// 计算重力在设备坐标系下的投影 G_device = R_transpose * G_world
// 由于 G_world 只有 z 分量，计算可以简化
float g_device[3];
g_device[0] = (-sp) * g_world[2];
g_device[1] = (cp * sr) * g_world[2];
g_device[2] = (cp * cr) * g_world[2];
// 从原始设备加速度中减去重力投影
acc_linear_device[0] = acc_device[0] - g_device[0];
acc_linear_device[1] = acc_device[1] - g_device[1];
acc_linear_device[2] = acc_device[2] - g_device[2];
}
/**
* @brief 计算缓冲区内三轴数据的方差之和
*
* @param buffer 传进来的三轴数据：陀螺仪/加速度
* @return float 返回方差和
*/
static float calculate_variance(float buffer[VARIANCE_BUFFER_SIZE][3])
{
float mean[3] = {0};
float variance[3] = {0};
// 计算均值
for (int i = 0; i < VARIANCE_BUFFER_SIZE; i++) {
mean[0] += buffer[i][0];
mean[1] += buffer[i][1];
mean[2] += buffer[i][2];
}
mean[0] /= VARIANCE_BUFFER_SIZE;
mean[1] /= VARIANCE_BUFFER_SIZE;
mean[2] /= VARIANCE_BUFFER_SIZE;
// 计算方差
for (int i = 0; i < VARIANCE_BUFFER_SIZE; i++) {
variance[0] += (buffer[i][0] - mean[0]) * (buffer[i][0] - mean[0]);
variance[1] += (buffer[i][1] - mean[1]) * (buffer[i][1] - mean[1]);
variance[2] += (buffer[i][2] - mean[2]) * (buffer[i][2] - mean[2]);
}
variance[0] /= VARIANCE_BUFFER_SIZE;
variance[1] /= VARIANCE_BUFFER_SIZE;
variance[2] /= VARIANCE_BUFFER_SIZE;
// 返回三轴方差之和，作为一个综合的稳定度指标
return variance[0] + variance[1] + variance[2];
}
/**
* @brief 摩擦力模拟，进行速度衰减
*
* @param tracker
*/
void forece_of_friction(skiing_tracker_t *tracker){
// 增加速度衰减，模拟摩擦力
tracker->velocity[0] *= SPEED_ATTENUATION;
tracker->velocity[1] *= SPEED_ATTENUATION;
tracker->velocity[2] = 0; // 垂直速度强制归零
}
/**
* @brief 状态机更新
*
* @param tracker 传入同步修改后传出
* @param acc_device_ms2 三轴加速度，m/s^2
* @param gyr_dps 三轴陀螺仪，dps
*/
static void update_state_machine(skiing_tracker_t *tracker, const float *acc_device_ms2, const float *gyr_dps)
{
// 缓冲区未填满时，不进行状态判断，默认为静止
if (!tracker->buffer_filled) {
tracker->state = STATIC;
return;
}
// --- 计算关键指标 ---
float acc_variance = calculate_variance(tracker->acc_buffer); // 计算加速度方差
float gyr_variance = calculate_variance(tracker->gyr_buffer); // 计算陀螺仪方差
float gyr_magnitude = sqrtf(gyr_dps[0]*gyr_dps[0] + gyr_dps[1]*gyr_dps[1] + gyr_dps[2]*gyr_dps[2]); //dps
float acc_magnitude = sqrtf(acc_device_ms2[0]*acc_device_ms2[0] + acc_device_ms2[1]*acc_device_ms2[1] + acc_device_ms2[2]*acc_device_ms2[2]); //m/s^s
float acc_magnitude_g = acc_magnitude / G_ACCELERATION; // 转换为g单位，用于跳跃判断
#ifdef XTELL_TEST
debug1.acc_variance =acc_variance;
debug1.gyr_variance =gyr_variance;
debug1.gyr_magnitude=gyr_magnitude;
debug1.acc_magnitude=fabsf(acc_magnitude - G_ACCELERATION);
#endif
// --- 状态机逻辑 (核心修改区域) ---
#if 0 //暂时不考虑空中
// 1. 空中/落地状态的后续处理
if (tracker->state == IN_AIR) {
// A. 检测巨大冲击 -> 落地
if (acc_magnitude_g > LANDING_ACC_MAG_HIGH_THRESHOLD) {
tracker->state = LANDING;
// B. 检测超时 -> 强制落地 (安全机制)
} else if (tracker->time_in_air > MAX_TIME_IN_AIR) {
tracker->state = LANDING;
// C. 检测恢复正常重力 (平缓落地)
} else if (acc_magnitude_g > 0.8f && acc_magnitude_g < 1.5f) {
tracker->grounded_entry_counter++;
if (tracker->grounded_entry_counter >= GROUNDED_CONFIRM_COUNT) {
tracker->state = LANDING;
}
} else {
tracker->grounded_entry_counter = 0;
}
return; // 在空中或刚切换到落地，结束本次状态判断
}
// 2. 严格的 "起跳->空中" 状态转换逻辑
// 只有当处于滑行状态时，才去检测起跳意图
if (tracker->state == NO_CONSTANT_SPEED || tracker->state == CONSTANT_SPEED || tracker->state == WHEEL) {
if (acc_magnitude_g > TAKEOFF_ACC_MAG_HIGH_THRESHOLD) {
tracker->state = TAKING_OFF;
tracker->airborne_entry_counter = 0; // 准备检测失重
return;
}
}
// 只有在TAKING_OFF状态下，才去检测是否进入失重
if (tracker->state == TAKING_OFF) {
if (acc_magnitude_g < AIRBORNE_ACC_MAG_LOW_THRESHOLD) {
tracker->airborne_entry_counter++;
if (tracker->airborne_entry_counter >= AIRBORNE_CONFIRM_COUNT) {
memcpy(tracker->initial_velocity_on_takeoff, tracker->velocity, sizeof(tracker->velocity));
tracker->time_in_air = 0;
tracker->state = IN_AIR;
tracker->airborne_entry_counter = 0;
tracker->grounded_entry_counter = 0;
return;
}
} else {
// 如果在起跳冲击后一段时间内没有失重，说明只是一个颠簸，恢复滑行
// 可以加一个小的超时计数器，这里为了简单先直接恢复
tracker->state = NO_CONSTANT_SPEED;
}
return; // 无论是否切换，都结束本次判断
}
#endif
// --- 静止判断 ---
if (acc_variance < STOP_ACC_VARIANCE_THRESHOLD && gyr_variance < STOP_GYR_VARIANCE_THRESHOLD && gyr_magnitude < STOP_GYR_MAG_THRESHOLD) {
tracker->state = STATIC;
return;
}
// --- 地面状态切换逻辑 ---
switch (tracker->state) {
case LANDING:
tracker->state = STATIC;
break;
case STATIC:
// 优先判断是否进入 WOBBLE 状态
// 条件：陀螺仪活动剧烈，但整体加速度变化不大（说明是原地转或晃）
if (gyr_magnitude > ROTATION_GYR_MAG_THRESHOLD && fabsf(acc_magnitude - G_ACCELERATION) < WOBBLE_ACC_MAG_THRESHOLD) {
tracker->state = WOBBLE;
}
// 只有在陀螺仪和加速度都满足“前进”特征时，才启动
else if (gyr_variance > START_GYR_VARIANCE_THRESHOLD && fabsf(acc_magnitude - G_ACCELERATION) > START_ACC_MAG_THRESHOLD) {
tracker->state = NO_CONSTANT_SPEED;
}
break;
case WOBBLE:
// 从 WOBBLE 状态启动的条件应该和从 STATIC 启动一样严格
if (gyr_variance > START_GYR_VARIANCE_THRESHOLD && fabsf(acc_magnitude - G_ACCELERATION) > START_ACC_MAG_THRESHOLD) {
tracker->state = NO_CONSTANT_SPEED;
}
// 如果陀螺仪活动减弱，则可能恢复静止
else if (gyr_magnitude < ROTATION_GYR_MAG_THRESHOLD * 0.8f) { // 增加迟滞，避免抖动
// 不直接跳回STATIC，而是依赖下一轮的全局静止判断
}
break;
case NO_CONSTANT_SPEED: //非匀速状态
//暂时不考虑摔倒
// if (gyr_magnitude > FALLEN_GRY_MAG_THRESHOLD) {
// tracker->state = FALLEN; //摔倒
// } else
if (gyr_magnitude > WHEEL_GYR_MAG_THRESHOLD && acc_variance > WHEEL_ACC_VARIANCE_THRESHOLD) {
tracker->state = WHEEL; //转弯
} else if (fabsf(acc_magnitude - G_ACCELERATION) < SKIING_ACC_MAG_THRESHOLD) {
tracker->state = CONSTANT_SPEED; //匀速
}
break;
case CONSTANT_SPEED: //匀速状态
if (fabsf(acc_magnitude - G_ACCELERATION) > START_ACC_MAG_THRESHOLD) {
tracker->state = NO_CONSTANT_SPEED;
}
//TODO：可以添加进入转弯或摔倒的判断
break;
case WHEEL:
// 从转弯状态，检查转弯是否结束
// 如果角速度和加速度方差都降下来了，就回到普通滑行状态
if (gyr_magnitude < WHEEL_GYR_MAG_THRESHOLD * 0.8f && acc_variance < WHEEL_ACC_VARIANCE_THRESHOLD * 0.8f) { // 乘以一个滞后系数避免抖动
tracker->state = NO_CONSTANT_SPEED;
}
break;
case FALLEN:
// TODO：回到 STATIC
break;
}
}
/**
* @brief 主更新函数
*
* @param tracker
* @param acc_g 三轴加速度，g
* @param gyr_dps 三轴陀螺仪，dps
* @param angle 欧若拉角
* @param dt 采样时间间隔，会用来积分求速度
*/
void skiing_tracker_update(skiing_tracker_t *tracker, float *acc_g, float *gyr_dps, float *angle, float dt)
{
if (!tracker || !acc_g || !gyr_dps || !angle || dt <= 0) {
return;
}
if(my_skiing_tracker.state == STOP_DETECTION)
return;
// --- 数据预处理和缓冲 ---
float acc_device_ms2[3];
acc_device_ms2[0] = acc_g[0] * G_ACCELERATION;
acc_device_ms2[1] = acc_g[1] * G_ACCELERATION;
acc_device_ms2[2] = acc_g[2] * G_ACCELERATION;
// 将最新数据存入缓冲区
memcpy(tracker->acc_buffer[tracker->buffer_index], acc_device_ms2, sizeof(acc_device_ms2));
memcpy(tracker->gyr_buffer[tracker->buffer_index], gyr_dps, 3 * sizeof(float));
tracker->buffer_index++;
if (tracker->buffer_index >= VARIANCE_BUFFER_SIZE) {
tracker->buffer_index = 0;
tracker->buffer_filled = 1; // 标记缓冲区已满
}
// --- 更新状态机 ---
update_state_machine(tracker, acc_device_ms2, gyr_dps);
// --- 根据状态执行不同的计算逻辑 ---
switch (tracker->state) {
case TAKING_OFF:
tracker->speed = 0.0f;
break;
case IN_AIR:
// 在空中时，只累加滞空时间
tracker->time_in_air += dt;
break;
case LANDING:
// 刚落地，计算空中距离
calculate_air_distance(tracker);
// 清理速度和滤波器状态，为恢复地面追踪做准备
memset(tracker->velocity, 0, sizeof(tracker->velocity));
tracker->speed = 0;
memset(tracker->acc_world_unfiltered_prev, 0, sizeof(tracker->acc_world_unfiltered_prev));
memset(tracker->acc_world_filtered, 0, sizeof(tracker->acc_world_filtered));
memset(tracker->acc_world_lpf, 0, sizeof(tracker->acc_world_lpf)); // 清理新增的LPF状态
break;
case WHEEL:
case NO_CONSTANT_SPEED:
remove_gravity_in_device_frame(acc_device_ms2, angle, tracker->acc_no_g);
float acc_world_temp[3]; // 临时变量存储当前周期的加速度
for (int i = 0; i < 2; i++) { // 只处理水平方向的 x 和 y 轴
// --- 核心修改：颠倒滤波器顺序为 HPF -> LPF ---
// 1. 高通滤波 (HPF) 先行: 消除因姿态误差导致的重力泄漏（直流偏置）
// HPF的瞬态响应会产生尖峰，这是正常的。
tracker->acc_world_filtered[i] = HPF_ALPHA * (tracker->acc_world_filtered[i] + tracker->acc_no_g[i] - tracker->acc_world_unfiltered_prev[i]);
tracker->acc_world_unfiltered_prev[i] = tracker->acc_no_g[i];
// 2. 低通滤波 (LPF) 殿后: 平滑掉HPF产生的尖峰和传感器自身的高频振动噪声。
// 这里使用 tracker->acc_world_filtered[i] 作为LPF的输入。
tracker->acc_world_lpf[i] = (1.0f - LPF_ALPHA) * tracker->acc_world_filtered[i] + LPF_ALPHA * tracker->acc_world_lpf[i];
// 将最终处理完的加速度值存入临时变量
acc_world_temp[i] = tracker->acc_world_lpf[i];
}
// 计算处理后加速度的水平模长
float acc_horizontal_mag = sqrtf(acc_world_temp[0] * acc_world_temp[0] +
acc_world_temp[1] * acc_world_temp[1]);
#if XTELL_TEST
debug2.acc_magnitude = acc_horizontal_mag;
#endif
// 应用死区，并积分
if (acc_horizontal_mag > ACC_DEAD_ZONE_THRESHOLD) {
tracker->velocity[0] += acc_world_temp[0] * dt;
tracker->velocity[1] += acc_world_temp[1] * dt;
}
// 更新速度和距离
tracker->speed = sqrtf(tracker->velocity[0] * tracker->velocity[0] +
tracker->velocity[1] * tracker->velocity[1]);
tracker->distance += tracker->speed * dt;
break;
case CONSTANT_SPEED:
//保持上次的速度不变。只更新距离
tracker->distance += tracker->speed * dt;
break;
case STATIC:
case WOBBLE:
// 速度清零，抑制漂移
memset(tracker->velocity, 0, sizeof(tracker->velocity));
tracker->speed = 0.0f;
memset(tracker->acc_world_unfiltered_prev, 0, sizeof(tracker->acc_world_unfiltered_prev));
memset(tracker->acc_world_filtered, 0, sizeof(tracker->acc_world_filtered));
memset(tracker->acc_world_lpf, 0, sizeof(tracker->acc_world_lpf)); // 清理新增的LPF状态
#if XTELL_TEST
debug2.acc_magnitude = 0;
#endif
break;
case FALLEN:
// TODO
break;
default:
break;
}
}
/**
* @brief 滑雪数据计算
*
* @param acc_data_buf 传入的三轴加速度数据
* @param gyr_data_buf 传入的三轴陀螺仪数据
* @param angle_data 传入的欧若拉角数据
* @return BLE_send_data_t 要发送给蓝牙的数据
*/
BLE_send_data_t sensor_processing_task(signed short* acc_data_buf, signed short* gyr_data_buf, float* angle_data) {
static int initialized = 0;
static float acc_data_g[3];
static float gyr_data_dps[3];
// const float delta_time = DELTA_TIME+0.01f;
// const float delta_time = DELTA_TIME + 0.005f;
const float delta_time = DELTA_TIME;
BLE_send_data_t BLE_send_data;
if (!initialized) {
skiing_tracker_init(&my_skiing_tracker);
initialized = 1;
printf("Skiing Tracker Initialized. Waiting for sensor calibration...\n");
}
#if ACC_RANGE==2
// 加速度 LSB to g
acc_data_g[0] = (float)acc_data_buf[0] / 16384.0f;
acc_data_g[1] = (float)acc_data_buf[1] / 16384.0f;
acc_data_g[2] = (float)acc_data_buf[2] / 16384.0f;
#endif
#if ACC_RANGE==4
// 加速度 LSB to g
acc_data_g[0] = (float)acc_data_buf[0] / 8192.0f;
acc_data_g[1] = (float)acc_data_buf[1] / 8192.0f;
acc_data_g[2] = (float)acc_data_buf[2] / 8192.0f;
#endif
#if ACC_RANGE==8
//±8g 4096
acc_data_g[0] = (float)acc_data_buf[0] / 4096.0f; //ax
acc_data_g[1] = (float)acc_data_buf[1] / 4096.0f; //ay
acc_data_g[2] = (float)acc_data_buf[2] / 4096.0f; //az
#endif
#if ACC_RANGE==16
//±16g 2048
acc_data_g[0] = (float)acc_data_buf[0] / 2048.0f; //ax
acc_data_g[1] = (float)acc_data_buf[1] / 2048.0f; //ay
acc_data_g[2] = (float)acc_data_buf[2] / 2048.0f; //az
#endif
// 陀螺仪 LSB to dps (度/秒)
// ±2000dps量程下，转换系数约为 0.061
gyr_data_dps[0] = (float)gyr_data_buf[0] * 0.061f;
gyr_data_dps[1] = (float)gyr_data_buf[1] * 0.061f;
gyr_data_dps[2] = (float)gyr_data_buf[2] * 0.061f;
skiing_tracker_update(&my_skiing_tracker, acc_data_g, gyr_data_dps, angle_data, delta_time);
BLE_send_data.skiing_state = my_skiing_tracker.state;
for (int i = 0; i < 3; i++) {
#ifdef XTELL_TEST
BLE_send_data.acc_data[i] = (short)(acc_data_g[i] * 9.8f) * 100; //cm/^s2
BLE_send_data.gyr_data[i] = (short)gyr_data_dps[i]; //dps
BLE_send_data.angle_data[i] = angle_data[i];
#else
BLE_send_data.acc_data[i] = (short)acc_data_buf[i]; //原始adc数据
BLE_send_data.gyr_data[i] = (short)gyr_data_buf[i]; //原始adc数据
BLE_send_data.angle_data[i] = angle_data[i];
#endif
}
BLE_send_data.speed_cms = (int)(my_skiing_tracker.speed * 100);
BLE_send_data.distance_cm = (int)(my_skiing_tracker.distance * 100);
// printf("Calculate the time interval =============== end\n");
return BLE_send_data;
}

88
apps/earphone/xtell_Sensor/A_hide/10/skiing_tracker.h
View File

@ -1,88 +0,0 @@
#ifndef SKIING_TRACKER_H
#define SKIING_TRACKER_H
#include "../xtell.h"
// 定义滑雪者可能的状态
typedef enum {
STATIC, // 静止或动态稳定：0
NO_CONSTANT_SPEED, // 正在滑雪，非匀速：1
CONSTANT_SPEED, // 正在滑雪，匀速：2
WOBBLE, // 正在原地旋转：3
WHEEL, // 转弯：4
FALLEN, // 已摔倒：5
TAKING_OFF, // 起跳冲击阶段：6
IN_AIR, // 空中失重阶段：7
LANDING, // 落地冲击阶段：8
STOP_DETECTION, // 停止检测：9
UNKNOWN // 未知状态：10
} skiing_state_t;
#define VARIANCE_BUFFER_SIZE 5 // 用于计算方差的数据窗口大小 (5个样本 @ 100Hz = 50ms)，减小延迟，提高实时性
#define DELTA_TIME 0.01f
// 追踪器数据结构体
typedef struct {
// 公开数据
float velocity[3]; // 当前速度 (x, y, z)，单位: m/s
float distance; // 总滑行距离，单位: m
float speed; // 当前速率 (标量)，单位: m/s
skiing_state_t state; // 当前滑雪状态
// 内部计算使用的私有成员
float acc_no_g[3]; // 去掉重力分量后的加速度
// 用于空中距离计算
float time_in_air; // 滞空时间计时器
float initial_velocity_on_takeoff[3]; // 起跳瞬间的速度向量
int airborne_entry_counter; // 进入空中状态的确认计数器
int grounded_entry_counter; // 落地确认计数器
// --- 内部计算使用的私有成员 ---
// 用于动态零速更新和旋转检测的缓冲区
float acc_buffer[VARIANCE_BUFFER_SIZE][3]; // 加速度数据窗口
float gyr_buffer[VARIANCE_BUFFER_SIZE][3]; // 角速度数据窗口
int buffer_index; // 缓冲区当前索引
int buffer_filled; // 缓冲区是否已填满的标志
// 用于高通滤波器（巴特沃斯一阶滤波器）的私有成员，以消除加速度的直流偏置
float acc_world_filtered[3]; //过滤过的
float acc_world_unfiltered_prev[3]; //上一次没过滤的
float acc_world_lpf[3]; // 经过低通滤波后的世界坐标系加速度
} skiing_tracker_t;
//ble发送的数据
typedef struct{ //__attribute__((packed)){ //该结构体取消内存对齐
char sensor_state;
char skiing_state;
int speed_cms; //求出的速度，cm/s
int distance_cm; //求出的距离，cm
short acc_data[3]; //三轴加速度, g
short gyr_data[3]; //三轴陀螺仪, dps
float angle_data[3]; //欧若拉角
}BLE_send_data_t;
typedef struct{
int acc_KS[3]; //卡尔曼后，LSB转换后的 三轴加速度数据（cm/s^2）
int gyr_KS_dps[3]; //卡尔曼后，LSB to dps 三轴陀螺仪数据
int angle_KS[3]; //卡尔曼后，计算得到的欧若拉角数据
}BLE_KS_send_data_t;
#ifdef XTELL_TEST
typedef struct{
float acc_variance; //三轴加速度方差之和
float gyr_variance; //三轴陀螺仪方差之和
float acc_magnitude; //三轴加速度模长
float gyr_magnitude; //三轴陀螺仪模长
}debug_t;
#endif
/**
* @brief 初始化滑雪追踪器
*
* @param tracker 指向 skiing_tracker_t 结构体的指针
*/
void skiing_tracker_init(skiing_tracker_t *tracker);
BLE_send_data_t sensor_processing_task(signed short* acc_data_buf, signed short* gyr_data_buf, float* angle_data) ;
#endif // SKIING_TRACKER_H

377
apps/earphone/xtell_Sensor/A_hide/11_test/skiing_tracker.c
View File

@ -1,377 +0,0 @@
/*
使用四元数求角度和去掉重力分量
*/
#include "skiing_tracker.h"
#include "../sensor/SC7U22.h"
#include <math.h>
#include <string.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 G_ACCELERATION 9.81f
#define DEG_TO_RAD (3.14159265f / 180.0f)
BLE_KS_send_data_t KS_data;
static float quaternion_data[4];
#ifdef XTELL_TEST
debug_t debug1;
debug_t debug2;
#endif
static skiing_tracker_t my_skiing_tracker;
//////////////////////////////////////////////////////////////////////////////////////////////////
//实现
void clear_speed(void){
my_skiing_tracker.state = STATIC;
memset(my_skiing_tracker.velocity, 0, sizeof(my_skiing_tracker.velocity));
my_skiing_tracker.speed = 0;
}
void start_detection(void){
my_skiing_tracker.state = STATIC;
memset(my_skiing_tracker.velocity, 0, sizeof(my_skiing_tracker.velocity));
my_skiing_tracker.distance = 0;
my_skiing_tracker.speed = 0;
}
void stop_detection(void){
my_skiing_tracker.state = STOP_DETECTION;
memset(my_skiing_tracker.velocity, 0, sizeof(my_skiing_tracker.velocity));
my_skiing_tracker.speed = 0;
}
/**
* @brief 初始化滑雪追踪器
*
* @param tracker
*/
void skiing_tracker_init(skiing_tracker_t *tracker)
{
if (!tracker) {
return;
}
// 使用memset一次性清零整个结构体，包括新增的缓冲区
memset(tracker, 0, sizeof(skiing_tracker_t));
tracker->state = STATIC;
}
/**
* @brief 当检测到落地时，计算空中的水平飞行距离并累加到总距离
*/
static void calculate_air_distance(skiing_tracker_t *tracker) {
float horizontal_speed_on_takeoff = sqrtf(
tracker->initial_velocity_on_takeoff[0] * tracker->initial_velocity_on_takeoff[0] +
tracker->initial_velocity_on_takeoff[1] * tracker->initial_velocity_on_takeoff[1]
);
float distance_in_air = horizontal_speed_on_takeoff * tracker->time_in_air;
tracker->distance += distance_in_air;
}
/**
* @brief 将设备坐标系下的加速度转换为世界坐标系，去掉重力分量
*
* @param acc_device
* @param angle
* @param acc_linear_world
*/
static void transform_acc_to_world_frame(const float *acc_device, const float *angle, float *acc_linear_world)
{
// 1. 将输入的角度从度转换为弧度
// angle[0] -> pitch, angle[1] -> roll, angle[2] -> yaw
float pitch_rad = -angle[0] * (M_PI / 180.0f);
float roll_rad = -angle[1] * (M_PI / 180.0f);
float yaw_rad = -angle[2] * (M_PI / 180.0f);
// 2. 预先计算三角函数值，以提高效率
float c_r = cosf(roll_rad);
float s_r = sinf(roll_rad);
float c_p = cosf(pitch_rad);
float s_p = sinf(pitch_rad);
float c_y = cosf(yaw_rad);
float s_y = sinf(yaw_rad);
// 3. 构建从设备坐标系到世界坐标系的旋转矩阵 R_device_to_world
// 该矩阵基于 Z-Y-X (Yaw-Pitch-Roll) 欧拉角顺序
// R = R_z(yaw) * R_y(pitch) * R_x(roll)
float R[3][3];
R[0][0] = c_y * c_p;
R[0][1] = c_y * s_p * s_r - s_y * c_r;
R[0][2] = c_y * s_p * c_r + s_y * s_r;
R[1][0] = s_y * c_p;
R[1][1] = s_y * s_p * s_r + c_y * c_r;
R[1][2] = s_y * s_p * c_r - c_y * s_r;
R[2][0] = -s_p;
R[2][1] = c_p * s_r;
R[2][2] = c_p * c_r;
// 4. 将设备坐标系的加速度计总读数旋转到世界坐标系
// a_raw_world = R * acc_device
float ax_raw_world = R[0][0] * acc_device[0] + R[0][1] * acc_device[1] + R[0][2] * acc_device[2];
float ay_raw_world = R[1][0] * acc_device[0] + R[1][1] * acc_device[1] + R[1][2] * acc_device[2];
float az_raw_world = R[2][0] * acc_device[0] + R[2][1] * acc_device[1] + R[2][2] * acc_device[2];
// 5. 在世界坐标系中减去重力分量，得到线性加速度
// 假设世界坐标系Z轴垂直向上，重力矢量为 [0, 0, -g]
// 线性加速度 = 总加速度 - 重力加速度
// az_linear = az_raw_world - (-g) = az_raw_world + g (如果Z轴向上)
// az_linear = az_raw_world - (+g) = az_raw_world - g (如果Z轴向下)
// 这里我们采用 Z 轴向上的标准惯性系 (ENU)
acc_linear_world[0] = ax_raw_world;
acc_linear_world[1] = ay_raw_world;
acc_linear_world[2] = az_raw_world - G_ACCELERATION; // Z轴向上，重力为正值，所以减去
}
/**
* @brief 在设备坐标系下，从原始加速度数据中移除重力分量
* @param acc_device 输入：设备坐标系下的原始加速度 [x, y, z], 单位 m/s^2
* @param angle 输入：姿态角 [pitch, roll, yaw]，单位: 度
* @param acc_linear_device 输出：设备坐标系下移除重力后的线性加速度 [x, y, z]
*/
void remove_gravity_in_device_frame(const float *acc_device, const float *angle, float *acc_linear_device)
{
float pitch = -angle[0] * DEG_TO_RAD; // 绕 Y 轴
float roll = -angle[1] * DEG_TO_RAD; // 绕 X 轴
float yaw = -angle[2] * DEG_TO_RAD; // 绕 Z 轴
float cp = cosf(pitch);
float sp = sinf(pitch);
float cr = cosf(roll);
float sr = sinf(roll);
float cy = cosf(yaw);
float sy = sinf(yaw);
// 世界坐标系下的重力矢量
const float g_world[3] = {0.0f, 0.0f, G_ACCELERATION};
// 计算旋转矩阵 R 的转置矩阵 R_transpose
// R (Z-Y-X) =
// [ cy*cp, cy*sp*sr - sy*cr, cy*sp*cr + sy*sr]
// [ sy*cp, sy*sp*sr + cy*cr, sy*sp*cr - cy*sr]
// [ -sp, cp*sr, cp*cr ]
//
// R_transpose =
// [ cy*cp, sy*cp, -sp ]
// [ cy*sp*sr - sy*cr, sy*sp*sr + cy*cr, cp*sr ]
// [ cy*sp*cr + sy*sr, sy*sp*cr - cy*sr, cp*cr ]
// 计算重力在设备坐标系下的投影 G_device = R_transpose * G_world
// 由于 G_world 只有 z 分量，计算可以简化
float g_device[3];
g_device[0] = (-sp) * g_world[2];
g_device[1] = (cp * sr) * g_world[2];
g_device[2] = (cp * cr) * g_world[2];
// 从原始设备加速度中减去重力投影
acc_linear_device[0] = acc_device[0] - g_device[0];
acc_linear_device[1] = acc_device[1] - g_device[1];
acc_linear_device[2] = acc_device[2] - g_device[2];
}
/**
* @brief 使用四元数直接从设备坐标系的加速度中移除重力分量
* @details 这种方法比使用欧拉角更精确、更稳定，且避免了万向节死锁。
* @param acc_device 输入：设备坐标系下的原始加速度 [x, y, z], 单位 m/s^2
* @param q 输入：表示姿态的四元数 [w, x, y, z]
* @param acc_linear_device 输出：设备坐标系下移除重力后的线性加速度 [x, y, z]
*/
void q_remove_gravity_with_quaternion(const float *acc_device, const float *q, float *acc_linear_device)
{
// 从四元数计算重力在设备坐标系下的投影
// G_device = R_transpose * G_world
// G_world = [0, 0, g]
// R_transpose 的第三列即为重力投影方向
float gx = 2.0f * (q[1] * q[3] - q[0] * q[2]);
float gy = 2.0f * (q[0] * q[1] + q[2] * q[3]);
float gz = q[0] * q[0] - q[1] * q[1] - q[2] * q[2] + q[3] * q[3];
// 从原始加速度中减去重力分量
acc_linear_device[0] = acc_device[0] - gx * G_ACCELERATION;
acc_linear_device[1] = acc_device[1] - gy * G_ACCELERATION;
acc_linear_device[2] = acc_device[2] - gz * G_ACCELERATION;
}
/**
* @brief 使用四元数将设备坐标系的线性加速度转换到世界坐标系,并且移除重力分量
* @details 同样，此方法比使用欧拉角更优。
* @param acc_linear_device 输入：设备坐标系下的线性加速度 [x, y, z]
* @param q 输入：表示姿态的四元数 [w, x, y, z]
* @param acc_linear_world 输出：世界坐标系下的线性加速度 [x, y, z]
*/
void q_transform_to_world_with_quaternion(const float *acc_linear_device, const float *q, float *acc_linear_world)
{
// 这是 R_device_to_world * acc_linear_device 的展开形式
acc_linear_world[0] = (1.0f - 2.0f*q[2]*q[2] - 2.0f*q[3]*q[3]) * acc_linear_device[0] +
(2.0f*q[1]*q[2] - 2.0f*q[0]*q[3]) * acc_linear_device[1] +
(2.0f*q[1]*q[3] + 2.0f*q[0]*q[2]) * acc_linear_device[2];
acc_linear_world[1] = (2.0f*q[1]*q[2] + 2.0f*q[0]*q[3]) * acc_linear_device[0] +
(1.0f - 2.0f*q[1]*q[1] - 2.0f*q[3]*q[3]) * acc_linear_device[1] +
(2.0f*q[2]*q[3] - 2.0f*q[0]*q[1]) * acc_linear_device[2];
acc_linear_world[2] = (2.0f*q[1]*q[3] - 2.0f*q[0]*q[2]) * acc_linear_device[0] +
(2.0f*q[2]*q[3] + 2.0f*q[0]*q[1]) * acc_linear_device[1] +
(1.0f - 2.0f*q[1]*q[1] - 2.0f*q[2]*q[2]) * acc_linear_device[2];
acc_linear_world[2] -= G_ACCELERATION;
}
/**
* @brief 主更新函数
*
* @param tracker
* @param acc_g 三轴加速度，g
* @param gyr_dps 三轴陀螺仪，dps
* @param angle 欧若拉角
* @param dt 采样时间间隔，会用来积分求速度
*/
void skiing_tracker_update(skiing_tracker_t *tracker, float *acc_g, float *gyr_dps, float *angle, float dt)
{
if (!tracker || !acc_g || !gyr_dps || !angle || dt <= 0) {
return;
}
if(my_skiing_tracker.state == STOP_DETECTION)
return;
// --- 数据预处理和缓冲 ---
float acc_device_ms2[3];
acc_device_ms2[0] = acc_g[0] * G_ACCELERATION;
acc_device_ms2[1] = acc_g[1] * G_ACCELERATION;
acc_device_ms2[2] = acc_g[2] * G_ACCELERATION;
#if 1 //测试禁止状态下陀螺仪的三轴加速度，去掉重力分量后是否正常
float tmp_device_acc[3];
float tmp_world_acc[3];
// remove_gravity_in_device_frame(acc_device_ms2,angle,tmp_device_acc);
// transform_acc_to_world_frame(acc_device_ms2,angle,tmp_world_acc);
q_remove_gravity_with_quaternion(acc_device_ms2,quaternion_data,tmp_device_acc);
q_transform_to_world_with_quaternion(acc_device_ms2,quaternion_data,tmp_world_acc);
// 计算处理后加速度的水平模长
float all_device_mag = sqrtf(tmp_device_acc[0] * tmp_device_acc[0] +
tmp_device_acc[1] * tmp_device_acc[1] +
tmp_device_acc[2] * tmp_device_acc[2]);
float all_world_mag = sqrtf(tmp_world_acc[0] * tmp_world_acc[0] +
tmp_world_acc[1] * tmp_world_acc[1] +
tmp_world_acc[2] * tmp_world_acc[2]);
static int count = 0;
if(count > 100){
xlog("===original(g): x %.2f, y %.2f, z %.2f===\n",acc_g[0],acc_g[1],acc_g[2]);
xlog("===device(m/s^2) no g: x %.2f, y %.2f, z %.2f, all %.2f===\n",tmp_device_acc[0],tmp_device_acc[1],tmp_device_acc[2],all_device_mag); //去掉重力加速度
xlog("===world(m/s^2) no g: x %.2f, y %.2f, z %.2f, all %.2f===\n",tmp_world_acc[0],tmp_world_acc[1],tmp_world_acc[2],all_world_mag); //去掉重力加速度
xlog("===gyr(dps) : x %.2f, y %.2f, z %.2f, all %.2f===\n",gyr_dps[0],gyr_dps[1],gyr_dps[2]); //angle
xlog("===angle : x %.2f, y %.2f, z %.2f,===\n",angle[0],angle[1],angle[2]);
count = 0;
}
count++;
#endif
}
/**
* @brief 滑雪数据计算
*
* @param acc_data_buf 传入的三轴加速度数据
* @param gyr_data_buf 传入的三轴陀螺仪数据
* @param angle_data 传入的欧若拉角数据
* @return BLE_send_data_t 要发送给蓝牙的数据
*/
BLE_send_data_t sensor_processing_task(signed short* acc_data_buf, signed short* gyr_data_buf, float* angle_data, float* quaternion) {
static int initialized = 0;
static float acc_data_g[3];
static float gyr_data_dps[3];
if(quaternion != NULL){
memcpy(quaternion_data, quaternion, 4 * sizeof(float));
}
// const float delta_time = DELTA_TIME+0.01f;
// const float delta_time = DELTA_TIME + 0.005f;
const float delta_time = DELTA_TIME;
BLE_send_data_t BLE_send_data;
if (!initialized) {
skiing_tracker_init(&my_skiing_tracker);
initialized = 1;
printf("Skiing Tracker Initialized. Waiting for sensor calibration...\n");
}
#if ACC_RANGE==2
// 加速度 LSB to g
acc_data_g[0] = (float)acc_data_buf[0] / 16384.0f;
acc_data_g[1] = (float)acc_data_buf[1] / 16384.0f;
acc_data_g[2] = (float)acc_data_buf[2] / 16384.0f;
#endif
#if ACC_RANGE==4
// 加速度 LSB to g
acc_data_g[0] = (float)acc_data_buf[0] / 8192.0f;
acc_data_g[1] = (float)acc_data_buf[1] / 8192.0f;
acc_data_g[2] = (float)acc_data_buf[2] / 8192.0f;
#endif
#if ACC_RANGE==8
//±8g 4096
acc_data_g[0] = (float)acc_data_buf[0] / 4096.0f; //ax
acc_data_g[1] = (float)acc_data_buf[1] / 4096.0f; //ay
acc_data_g[2] = (float)acc_data_buf[2] / 4096.0f; //az
#endif
#if ACC_RANGE==16
//±16g 2048
acc_data_g[0] = (float)acc_data_buf[0] / 2048.0f; //ax
acc_data_g[1] = (float)acc_data_buf[1] / 2048.0f; //ay
acc_data_g[2] = (float)acc_data_buf[2] / 2048.0f; //az
#endif
// 陀螺仪 LSB to dps (度/秒)
// ±2000dps量程下，转换系数约为 0.061
gyr_data_dps[0] = (float)gyr_data_buf[0] * 0.061f;
gyr_data_dps[1] = (float)gyr_data_buf[1] * 0.061f;
gyr_data_dps[2] = (float)gyr_data_buf[2] * 0.061f;
skiing_tracker_update(&my_skiing_tracker, acc_data_g, gyr_data_dps, angle_data, delta_time);
// BLE_send_data.skiing_state = my_skiing_tracker.state;
// for (int i = 0; i < 3; i++) {
// #ifdef XTELL_TEST
// BLE_send_data.acc_data[i] = (short)(acc_data_g[i] * 9.8f) * 100; //cm/^s2
// BLE_send_data.gyr_data[i] = (short)gyr_data_dps[i]; //dps
// BLE_send_data.angle_data[i] = angle_data[i];
// #else
// BLE_send_data.acc_data[i] = (short)acc_data_buf[i]; //原始adc数据
// BLE_send_data.gyr_data[i] = (short)gyr_data_buf[i]; //原始adc数据
// BLE_send_data.angle_data[i] = angle_data[i];
// #endif
// }
// BLE_send_data.speed_cms = (int)(my_skiing_tracker.speed * 100);
// BLE_send_data.distance_cm = (int)(my_skiing_tracker.distance * 100);
// // printf("Calculate the time interval =============== end\n");
return BLE_send_data;
}

88
apps/earphone/xtell_Sensor/A_hide/11_test/skiing_tracker.h
View File

@ -1,88 +0,0 @@
#ifndef SKIING_TRACKER_H
#define SKIING_TRACKER_H
#include "../xtell.h"
// 定义滑雪者可能的状态
typedef enum {
STATIC, // 静止或动态稳定：0
NO_CONSTANT_SPEED, // 正在滑雪，非匀速：1
CONSTANT_SPEED, // 正在滑雪，匀速：2
WOBBLE, // 正在原地旋转：3
WHEEL, // 转弯：4
FALLEN, // 已摔倒：5
TAKING_OFF, // 起跳冲击阶段：6
IN_AIR, // 空中失重阶段：7
LANDING, // 落地冲击阶段：8
STOP_DETECTION, // 停止检测：9
UNKNOWN // 未知状态：10
} skiing_state_t;
#define VARIANCE_BUFFER_SIZE 5 // 用于计算方差的数据窗口大小 (5个样本 @ 100Hz = 50ms)，减小延迟，提高实时性
#define DELTA_TIME 0.01f
// 追踪器数据结构体
typedef struct {
// 公开数据
float velocity[3]; // 当前速度 (x, y, z)，单位: m/s
float distance; // 总滑行距离，单位: m
float speed; // 当前速率 (标量)，单位: m/s
skiing_state_t state; // 当前滑雪状态
// 内部计算使用的私有成员
float acc_world[3]; // 在世界坐标系下的加速度
// 用于空中距离计算
float time_in_air; // 滞空时间计时器
float initial_velocity_on_takeoff[3]; // 起跳瞬间的速度向量
int airborne_entry_counter; // 进入空中状态的确认计数器
int grounded_entry_counter; // 落地确认计数器
// --- 内部计算使用的私有成员 ---
// 用于动态零速更新和旋转检测的缓冲区
float acc_buffer[VARIANCE_BUFFER_SIZE][3]; // 加速度数据窗口
float gyr_buffer[VARIANCE_BUFFER_SIZE][3]; // 角速度数据窗口
int buffer_index; // 缓冲区当前索引
int buffer_filled; // 缓冲区是否已填满的标志
// 用于高通滤波器（巴特沃斯一阶滤波器）的私有成员，以消除加速度的直流偏置
float acc_world_filtered[3]; //过滤过的
float acc_world_unfiltered_prev[3]; //上一次没过滤的
float acc_world_lpf[3]; // 经过低通滤波后的世界坐标系加速度
} skiing_tracker_t;
//ble发送的数据
typedef struct{ //__attribute__((packed)){ //该结构体取消内存对齐
char sensor_state;
char skiing_state;
int speed_cms; //求出的速度，cm/s
int distance_cm; //求出的距离，cm
short acc_data[3]; //三轴加速度, g
short gyr_data[3]; //三轴陀螺仪, dps
float angle_data[3]; //欧若拉角
}BLE_send_data_t;
typedef struct{
int acc_KS[3]; //卡尔曼后，LSB转换后的 三轴加速度数据（cm/s^2）
int gyr_KS_dps[3]; //卡尔曼后，LSB to dps 三轴陀螺仪数据
int angle_KS[3]; //卡尔曼后，计算得到的欧若拉角数据
}BLE_KS_send_data_t;
#ifdef XTELL_TEST
typedef struct{
float acc_variance; //三轴加速度方差之和
float gyr_variance; //三轴陀螺仪方差之和
float acc_magnitude; //三轴加速度模长
float gyr_magnitude; //三轴陀螺仪模长
}debug_t;
#endif
/**
* @brief 初始化滑雪追踪器
*
* @param tracker 指向 skiing_tracker_t 结构体的指针
*/
void skiing_tracker_init(skiing_tracker_t *tracker);
BLE_send_data_t sensor_processing_task(signed short* acc_data_buf, signed short* gyr_data_buf, float* angle_data, float* quaternion);
#endif // SKIING_TRACKER_H

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

@ -1,259 +0,0 @@
/*
动态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
View File

@ -1,50 +0,0 @@
#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
View File

@ -1,277 +0,0 @@
/*
动态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
View File

@ -1,50 +0,0 @@
#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
View File

@ -1,465 +0,0 @@
/**
* 效果不行，对原地转动太灵敏了
*/
#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
View File

@ -1,40 +0,0 @@
#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
View File

@ -1,311 +0,0 @@
/*
动态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
View File

@ -1,55 +0,0 @@
#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
View File

@ -1,374 +0,0 @@
/*
动态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
View File

@ -1,74 +0,0 @@
#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

276
apps/earphone/xtell_Sensor/A_hide/7/skiing_tracker.c
View File

@ -1,276 +0,0 @@
/*
简化的滑雪追踪器：
- 直接读取六轴数据
- 分离重力分量
- 直接积分求速度和距离
*/
#include "skiing_tracker.h"
#include "../sensor/SC7U22.h"
#include <math.h>
#include <string.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 G_ACCELERATION 9.81f
#define DEG_TO_RAD (3.14159265f / 180.0f)
// 运动检测阈值 (m/s^2)。当水平加速度的模长大于此值时，认为处于运动状态。
#define MOTION_THRESHOLD 0.2f
static skiing_tracker_t my_skiing_tracker;
#ifdef XTELL_TEST
BLE_KS_send_data_t KS_data;
debug_t debug1;
debug_t debug2;
#endif
//////////////////////////////////////////////////////////////////////////////////////////////////
//实现
/**
* @brief 初始化滑雪追踪器
*
* @param tracker
*/
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)
{
// 驱动输出的角度与标准航空定义相反，需要取反才能用于标准旋转矩阵。
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 旋转顺序)
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;
}
void clear_speed(void){
my_skiing_tracker.state = SKIING_STATE_STATIC;
memset(my_skiing_tracker.velocity, 0, sizeof(my_skiing_tracker.velocity));
my_skiing_tracker.speed = 0;
}
void start_detection(void){
my_skiing_tracker.state = SKIING_STATE_STATIC;
memset(my_skiing_tracker.velocity, 0, sizeof(my_skiing_tracker.velocity));
my_skiing_tracker.distance = 0;
my_skiing_tracker.speed = 0;
}
void stop_detection(void){
my_skiing_tracker.state = SKINNG_STOP_DETECTION;
memset(my_skiing_tracker.velocity, 0, sizeof(my_skiing_tracker.velocity));
my_skiing_tracker.speed = 0;
}
/**
* @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 || !angle || dt <= 0) {
return;
}
//停止检测
if(my_skiing_tracker.state == SKINNG_STOP_DETECTION)
return;
// --- 数据预处理 ---
float acc_device_ms2[3];
acc_device_ms2[0] = acc_g[0] * G_ACCELERATION;
acc_device_ms2[1] = acc_g[1] * G_ACCELERATION;
acc_device_ms2[2] = acc_g[2] * G_ACCELERATION;
// --- 坐标转换 & 移除重力 ---
// 1. 将设备坐标系下的总加速度转换到世界坐标系
transform_acc_to_world_frame(acc_device_ms2, angle, tracker->acc_world);
// 2. 在世界坐标系下，减去Z轴的重力分量，得到线性加速度
tracker->acc_world[2] -= G_ACCELERATION;
// --- 运动状态判断与计算 ---
// 计算水平方向的线性加速度模长
float acc_horizontal_mag = sqrtf(tracker->acc_world[0] * tracker->acc_world[0] +
tracker->acc_world[1] * tracker->acc_world[1]);
if (acc_horizontal_mag > MOTION_THRESHOLD) {
// 加速
tracker->state = SKIING_STATE_SKIING;
// 积分计算速度（只考虑水平方向，避免垂直方向漂移）
tracker->velocity[0] += tracker->acc_world[0] * dt;
tracker->velocity[1] += tracker->acc_world[1] * dt;
tracker->velocity[2] = 0; // 垂直速度强制为0
// --- 更新速率和距离 ---
// 只基于水平速度计算速率
tracker->speed = sqrtf(tracker->velocity[0] * tracker->velocity[0] +
tracker->velocity[1] * tracker->velocity[1]);
// 累加距离
tracker->distance += tracker->speed * dt;
return;
} else {
//匀速
tracker->distance += tracker->speed * dt;
return;
}
}
// 获取当前 RTC 时间戳
time_t get_rtc_timestamp(void) {
static void *rtc_hdl = NULL;
struct sys_time rtc_time = {0};
dev_ioctl(rtc_hdl, IOCTL_GET_SYS_TIME, (u32)&rtc_time);
return rtc_sys_time_to_timestamp(&rtc_time);
}
/**
* @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 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]; // 转换后的陀螺仪数据
float delta_time = DELTA_TIME + 0.03f; //观察得到实际上调用间隔会多30ms
BLE_send_data_t BLE_send_data;
// xlog("===========start\n");
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);
// printf("Calculate the time interval =============== end\n");
}
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);
// printf("calibrated_acc_g: %.2f, %.2f, %.2f\n",
// calibrated_acc_g[0],
// calibrated_acc_g[1],
// calibrated_acc_g[2]);
// 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[i] = (int)(calibrated_acc_g[i] * G_ACCELERATION * 100); //cm/s^s
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");
}
// xlog("end++++++++++++++\n");
return BLE_send_data;
}

67
apps/earphone/xtell_Sensor/A_hide/7/skiing_tracker.h
View File

@ -1,67 +0,0 @@
#ifndef SKIING_TRACKER_H
#define SKIING_TRACKER_H
#include "../xtell.h"
// 定义滑雪者可能的状态
typedef enum {
SKIING_STATE_STATIC, // 静止
SKIING_STATE_SKIING, // 正在滑雪
SKINNG_STOP_DETECTION, // 停止检测
} skiing_state_t;
#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]; // 在世界坐标系下的线性加速度
} 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[3]; //卡尔曼后，LSB转换后的 三轴加速度数据（cm/s^2）
int gyr_KS_dps[3]; //卡尔曼后，LSB to dps 三轴陀螺仪数据
int angle_KS[3]; //卡尔曼后，计算得到的欧若拉角数据
}BLE_KS_send_data_t;
#ifdef XTELL_TEST
typedef struct{
float acc_variance; //三轴加速度方差之和
float gyr_variance; //三轴陀螺仪方差之和
float acc_magnitude; //三轴加速度模长
float gyr_magnitude; //三轴陀螺仪模长
}debug_t;
#endif
/**
* @brief 初始化滑雪追踪器
*
* @param tracker 指向 skiing_tracker_t 结构体的指针
*/
void skiing_tracker_init(skiing_tracker_t *tracker);
/**
* @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

501
apps/earphone/xtell_Sensor/A_hide/8/skiing_tracker.c
View File

@ -1,501 +0,0 @@
/*
虽然sensor_processing_task是10ms调用一次
但是实际上上一次调用该函数的时间点和下一次调用该函数的时间点，会相差40ms
*/
#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 ---
//两个判断是否静止的必要条件：动态零速更新(ZUPT)阈值
// 加速方差阈值，提高阈值，让“刹车”更灵敏，以便在波浪式前进等慢速漂移时也能触发零速更新
#define ZUPT_ACC_VARIANCE_THRESHOLD 0.2f
// 陀螺仪方差阈值
#define ZUPT_GYR_VARIANCE_THRESHOLD 5.0f
// --- 启动滑雪阈值 ---
// 加速度模长与重力的差值大于此值，认为开始运动；降低阈值，让“油门”更灵敏，以便能捕捉到真实的慢速启动
#define START_SKIING_ACC_THRESHOLD 0.5f
// 陀螺仪方差阈值，以允许启动瞬间的正常抖动，但仍能过滤掉混乱的、非滑雪的晃动。
#define SKIING_GYR_VARIANCE_THRESHOLD 15.0f
// --- 滑雪过程 ---
//加速度 模长，低于此值视为 在做匀速运动
#define SKIING_ACC_MAG_THRESHOLD 0.5f
//陀螺仪 模长，高于此值视为 摔倒了
#define FALLEN_GRY_MAG_THRESHOLD 1000.0f //未确定
// --- 原地旋转抖动 ---
// 用于原地旋转判断的加速度方差阈值。此值比ZUPT阈值更宽松，
// 以允许原地旋转时身体的正常晃动，但仍能与真实滑行时的剧烈加速度变化区分开。
#define ROTATING_ACC_VARIANCE_THRESHOLD 0.8f
// 旋转/摆动检测阈值：角速度总模长大于此值(度/秒)，认为正在进行非滑雪的旋转或摆动
#define ROTATION_GYR_MAG_THRESHOLD 120.0f
// --- 滑雪转弯动 ---
// 加速度方差阈值，大于此值，滑雪过程可能发生了急转弯
#define WHEEL_ACC_VARIANCE_THRESHOLD 7.0f
// 旋转/摆动检测阈值：角速度总模长大于此值(度/秒)，认为滑雪过程中进行急转弯
#define WHEEL_GYR_MAG_THRESHOLD 220.0f // 150.0f 到 250.0f之间进行调整
// --- 用于消除积分漂移的滤波器和阈值 ---
// 高通滤波器系数 (alpha)。alpha 越接近1，滤除低频(直流偏移)的效果越强，但可能滤掉真实的慢速运动。
// alpha = RC / (RC + dt)，参考RC电路而来
#define HPF_ALPHA 0.995
// 加速度死区阈值 (m/s^2)。低于此阈值的加速度被认为是噪声，不参与积分。
// 设得太高会忽略真实的慢速启动，设得太低则无法有效抑制噪声。
#define ACC_DEAD_ZONE_THRESHOLD 0.05f
// --- 模拟摩擦力，进行速度衰减 ---
#define SPEED_ATTENUATION 1.0f //暂不模拟
#ifdef XTELL_TEST
BLE_KS_send_data_t KS_data;
debug_t debug1;
debug_t debug2;
#endif
static skiing_tracker_t my_skiing_tracker;
//////////////////////////////////////////////////////////////////////////////////////////////////
//实现
void clear_speed(void){
my_skiing_tracker.state = STATIC;
memset(my_skiing_tracker.velocity, 0, sizeof(my_skiing_tracker.velocity));
my_skiing_tracker.speed = 0;
}
void start_detection(void){
my_skiing_tracker.state = STATIC;
memset(my_skiing_tracker.velocity, 0, sizeof(my_skiing_tracker.velocity));
my_skiing_tracker.distance = 0;
my_skiing_tracker.speed = 0;
}
void stop_detection(void){
my_skiing_tracker.state = STOP_DETECTION;
memset(my_skiing_tracker.velocity, 0, sizeof(my_skiing_tracker.velocity));
my_skiing_tracker.speed = 0;
}
/**
* @brief 初始化滑雪追踪器
*
* @param tracker
*/
void skiing_tracker_init(skiing_tracker_t *tracker)
{
if (!tracker) {
return;
}
// 使用memset一次性清零整个结构体，包括新增的缓冲区
memset(tracker, 0, sizeof(skiing_tracker_t));
tracker->state = STATIC;
}
/**
* @brief 将设备坐标系下的加速度转换为世界坐标系
* @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 = 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]);
#ifdef XTELL_TEST
debug1.acc_variance =acc_variance;
debug1.gyr_variance =gyr_variance;
debug1.gyr_magnitude=gyr_magnitude;
debug1.acc_magnitude=fabsf(acc_magnitude - G_ACCELERATION);
#endif
#if 0
//正在滑雪
if(tracker->state == NO_CONSTANT_SPEED ) {
//摔倒了
if(gyr_magnitude > FALLEN_GRY_MAG_THRESHOLD){
tracker->state = FALLEN;
return;
}
//可能进入了匀速状态
if(gyr_magnitude > SKIING_GYR_MAG_THRESHOLD && acc_magnitude < SKIING_ACC_MAG_THRESHOLD){
tracker->state = CONSTANT_SPEED;
return;
}
//急转弯
if(gyr_magnitude > WHEEL_GYR_MAG_THRESHOLD && acc_variance > WHEEL_ACC_VARIANCE_THRESHOLD){
//TODO：可以考虑清掉速度消除积分带来的漂移
tracker->state = WHEEL;
return;
}
}
// --- 状态切换逻辑 (按优先级) ---
// 优先级1：静止
if (acc_variance < ZUPT_ACC_VARIANCE_THRESHOLD && gyr_variance < ZUPT_GYR_VARIANCE_THRESHOLD) {
tracker->state = STATIC;
// 速度清零，抑制漂移
memset(tracker->velocity, 0, sizeof(tracker->velocity));
tracker->speed = 0.0f;
memset(tracker->acc_world_unfiltered_prev, 0, sizeof(tracker->acc_world_unfiltered_prev));
memset(tracker->acc_world_filtered, 0, sizeof(tracker->acc_world_filtered));
return;
}
// 优先级2：原地旋转 - 特殊的、非滑雪的运动状态
// 条件：角速度很大，同时线性加速度的晃动在一个“中等”范围内。
if (tracker->state == STATIC && gyr_magnitude > ROTATION_GYR_MAG_THRESHOLD && acc_variance < ROTATING_ACC_VARIANCE_THRESHOLD) {
tracker->state = ROTATING;
return;
}
// 优先级3：启动滑雪 - “油门”
// 条件：有足够大的线性加速度，同时陀螺仪的抖动在一个“合理”（而非“完全静止”）的范围内。
if (fabsf(acc_magnitude - G_ACCELERATION) > START_SKIING_ACC_THRESHOLD && gyr_variance < SKIING_GYR_VARIANCE_THRESHOLD) {
tracker->state = NO_CONSTANT_SPEED;
return;
}
// 如果不满足任何启动或停止条件，则保持当前状态（滑雪中）
// 如果当前是静止或旋转，但没有满足启动条件，则状态会保持，直到满足ZUPT或旋转条件。
#else
// 无论当前是什么状态，静止总是最高优先级
if (acc_variance < ZUPT_ACC_VARIANCE_THRESHOLD && gyr_variance < ZUPT_GYR_VARIANCE_THRESHOLD) {
tracker->state = STATIC;
// 速度清零，抑制漂移
memset(tracker->velocity, 0, sizeof(tracker->velocity));
tracker->speed = 0.0f;
memset(tracker->acc_world_unfiltered_prev, 0, sizeof(tracker->acc_world_unfiltered_prev));
memset(tracker->acc_world_filtered, 0, sizeof(tracker->acc_world_filtered));
return;
}
switch (tracker->state) {
case STATIC:
//不break，会往下执行，判断是否进入非匀速状态
case ROTATING: // 从静止或原地旋转可以启动
if (fabsf(acc_magnitude - G_ACCELERATION) > START_SKIING_ACC_THRESHOLD && gyr_variance < SKIING_GYR_VARIANCE_THRESHOLD) {
tracker->state = NO_CONSTANT_SPEED;
} else if (gyr_magnitude > ROTATION_GYR_MAG_THRESHOLD && acc_variance < ROTATING_ACC_VARIANCE_THRESHOLD) {
tracker->state = ROTATING;
}
break;
case NO_CONSTANT_SPEED: //非匀速状态
if (gyr_magnitude > FALLEN_GRY_MAG_THRESHOLD) {
tracker->state = FALLEN; //摔倒
} else if (gyr_magnitude > WHEEL_GYR_MAG_THRESHOLD && acc_variance > WHEEL_ACC_VARIANCE_THRESHOLD) {
tracker->state = WHEEL; //转弯
} else if (fabsf(acc_magnitude - G_ACCELERATION) < SKIING_ACC_MAG_THRESHOLD) {
tracker->state = CONSTANT_SPEED; //匀速
}
break;
case CONSTANT_SPEED: //匀速状态
if (fabsf(acc_magnitude - G_ACCELERATION) > START_SKIING_ACC_THRESHOLD) {
tracker->state = NO_CONSTANT_SPEED;
}
//TODO：可以添加进入转弯或摔倒的判断
break;
case WHEEL:
// 从转弯状态，检查转弯是否结束
// 如果角速度和加速度方差都降下来了，就回到普通滑行状态
if (gyr_magnitude < WHEEL_GYR_MAG_THRESHOLD * 0.8f && acc_variance < WHEEL_ACC_VARIANCE_THRESHOLD * 0.8f) { // 乘以一个滞后系数避免抖动
tracker->state = NO_CONSTANT_SPEED;
}
break;
case FALLEN:
// TODO：回到 STATIC
break;
}
#endif
}
/**
* @brief 主更新函数
*
* @param tracker
* @param acc_g 三轴加速度，g
* @param gyr_dps 三轴陀螺仪，dps
* @param angle 欧若拉角
* @param dt 采样时间间隔，会用来积分求速度
*/
void skiing_tracker_update(skiing_tracker_t *tracker, float *acc_g, float *gyr_dps, float *angle, float dt)
{
if (!tracker || !acc_g || !gyr_dps || !angle || dt <= 0) {
return;
}
if(my_skiing_tracker.state == STOP_DETECTION)
return;
// --- 数据预处理和缓冲 ---
float acc_device_ms2[3];
acc_device_ms2[0] = acc_g[0] * G_ACCELERATION;
acc_device_ms2[1] = acc_g[1] * G_ACCELERATION;
acc_device_ms2[2] = acc_g[2] * G_ACCELERATION;
// 将最新数据存入缓冲区
memcpy(tracker->acc_buffer[tracker->buffer_index], acc_device_ms2, sizeof(acc_device_ms2));
memcpy(tracker->gyr_buffer[tracker->buffer_index], gyr_dps, 3 * sizeof(float));
tracker->buffer_index++;
if (tracker->buffer_index >= VARIANCE_BUFFER_SIZE) {
tracker->buffer_index = 0;
tracker->buffer_filled = 1; // 标记缓冲区已满
}
// --- 更新状态机 ---
update_state_machine(tracker, acc_device_ms2, gyr_dps);
// 坐标转换 & 移除重力
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 (tracker->state == NO_CONSTANT_SPEED) {
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;
}
#ifdef XTELL_TEST
debug2.acc_magnitude = acc_horizontal_mag;
#endif
// 如果加速度小于阈值，则不更新速度，相当于速度保持不变（或受下一步的阻尼影响而衰减）
// --- 更新速率和距离 ---
// 只基于水平速度计算速率和距离
tracker->speed = sqrtf(tracker->velocity[0] * tracker->velocity[0] +
tracker->velocity[1] * tracker->velocity[1]);
tracker->distance += tracker->speed * dt;
}else if(tracker->state == CONSTANT_SPEED){ //匀速
#ifdef XTELL_TEST
debug2.acc_magnitude = acc_horizontal_mag;
#endif
//保持上次的速度不变。只更新距离
tracker->distance += tracker->speed * dt;
}else{
// 在静止或旋转状态下，速度已经在状态机内部被清零
// 额外增加速度衰减，模拟摩擦力，进一步抑制漂移
tracker->velocity[0] *= SPEED_ATTENUATION;
tracker->velocity[1] *= SPEED_ATTENUATION;
tracker->velocity[2] = 0; // 垂直速度强制归零
}
}
/**
* @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 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 + 0.03f;
BLE_send_data_t BLE_send_data;
if (!initialized) {
skiing_tracker_init(&my_skiing_tracker);
initialized = 1;
printf("Skiing Tracker Initialized. Waiting for sensor calibration...\n");
}
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);
// printf("calibrated_acc_g: %.2f, %.2f, %.2f\n",
// calibrated_acc_g[0],
// calibrated_acc_g[1],
// calibrated_acc_g[2]);
// 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[i] = (int)(calibrated_acc_g[i] * G_ACCELERATION * 100); //cm/s^s
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;
}

85
apps/earphone/xtell_Sensor/A_hide/8/skiing_tracker.h
View File

@ -1,85 +0,0 @@
#ifndef SKIING_TRACKER_H
#define SKIING_TRACKER_H
#include "../xtell.h"
// 定义滑雪者可能的状态
typedef enum {
STATIC, // 静止或动态稳定
NO_CONSTANT_SPEED, // 正在滑雪，非匀速
CONSTANT_SPEED, // 正在滑雪，匀速
ROTATING, // 正在原地旋转
WHEEL, // 转弯
FALLEN, // 已摔倒
STOP_DETECTION, // 停止检测
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[3]; //卡尔曼后，LSB转换后的 三轴加速度数据（cm/s^2）
int gyr_KS_dps[3]; //卡尔曼后，LSB to dps 三轴陀螺仪数据
int angle_KS[3]; //卡尔曼后，计算得到的欧若拉角数据
}BLE_KS_send_data_t;
#ifdef XTELL_TEST
typedef struct{
float acc_variance; //三轴加速度方差之和
float gyr_variance; //三轴陀螺仪方差之和
float acc_magnitude; //三轴加速度模长
float gyr_magnitude; //三轴陀螺仪模长
}debug_t;
#endif
/**
* @brief 初始化滑雪追踪器
*
* @param tracker 指向 skiing_tracker_t 结构体的指针
*/
void skiing_tracker_init(skiing_tracker_t *tracker);
/**
* @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

583
apps/earphone/xtell_Sensor/A_hide/9/skiing_tracker.c
View File

@ -1,583 +0,0 @@
/*
*/
#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 STOP_ACC_VARIANCE_THRESHOLD 0.2f
// 陀螺仪方差阈值
#define STOP_GYR_VARIANCE_THRESHOLD 5.0f
// 静止时候的陀螺仪模长
#define STOP_GYR_MAG_THRESHOLD 15
// --- --- ---
// --- 启动滑雪阈值 ---
// 加速度模长与重力的差值大于此值，认为开始运动；降低阈值，让“油门”更灵敏，以便能捕捉到真实的慢速启动
#define START_ACC_MAG_THRESHOLD 1.0f //0.5、1
// 陀螺仪方差阈值，以允许启动瞬间的正常抖动，但仍能过滤掉混乱的、非滑雪的晃动。
#define START_GYR_VARIANCE_THRESHOLD 15.0f
// --- --- ---
// --- 滑雪过程 ---
//加速度 模长(不含重力），低于此值视为 在做匀速运动
#define SKIING_ACC_MAG_THRESHOLD 0.5f
//陀螺仪 模长，高于此值视为 摔倒了
#define FALLEN_GRY_MAG_THRESHOLD 2000.0f //未确定
// --- --- ---
// --- 原地旋转抖动 ---
// 加速度 方差 阈值。此值比 静止检测 阈值更宽松，
#define WOBBLE_ACC_VARIANCE_THRESHOLD 0.5f
// 加速度 模长 阈值
#define WOBBLE_ACC_MAG_THRESHOLD 1.0f
// 角速度 总模长 大于此值(度/秒)，认为正在进行非滑雪的旋转或摆动
#define ROTATION_GYR_MAG_THRESHOLD 30.0f
// --- --- ---
// --- 滑雪转弯动 ---
// 加速度 方差 阈值，大于此值，滑雪过程可能发生了急转弯
#define WHEEL_ACC_VARIANCE_THRESHOLD 7.0f
// 角速度 总模长 大于此值(度/秒)，认为滑雪过程中进行急转弯
#define WHEEL_GYR_MAG_THRESHOLD 500.0f //
// --- --- ---
// --- 跳跃 ---
// 加速度模长低于此值(g)，认为进入失重状态(IN_AIR)
#define AIRBORNE_ACC_MAG_LOW_THRESHOLD 0.4f
// 加速度模长高于此值(g)，认为发生落地冲击(LANDING)
#define LANDING_ACC_MAG_HIGH_THRESHOLD 3.5f
// 起跳加速度阈值(g)，用于进入TAKING_OFF状态
#define TAKEOFF_ACC_MAG_HIGH_THRESHOLD 1.8f
// 进入空中状态确认计数：需要连续3个采样点加速度低于阈值才判断为起跳
#define AIRBORNE_CONFIRM_COUNT 3
// 落地状态确认计数：加速度恢复到1g附近并持续2个采样点(20ms)则认为已落地
#define GROUNDED_CONFIRM_COUNT 2
// 最大滞空时间(秒)，超过此时间强制认为已落地，防止状态锁死
#define MAX_TIME_IN_AIR 12.5f
// --- --- ---
// --- 用于消除积分漂移的滤波器和阈值 ---
// 高通滤波器系数 (alpha)。alpha 越接近1，滤除低频(直流偏移)的效果越强，但可能滤掉真实的慢速运动。
// alpha = RC / (RC + dt)，参考RC电路而来，fc ≈ (1 - alpha) / (2 * π * dt)
#define HPF_ALPHA 0.999f
//0.995f： 0.08 Hz 的信号
//0.999f： 0.0159 Hz
// --- --- ---
// --- 低通滤波器 ---
// 低通滤波器系数 (alpha)。alpha 越小，滤波效果越强（更平滑），但延迟越大。
// alpha 推荐范围 0.7 ~ 0.95。可以从 0.85 开始尝试。
#define LPF_ALPHA 0.7f
// 加速度死区阈值 (m/s^2)。低于此阈值的加速度被认为是噪声，不参与积分。
// 设得太高会忽略真实的慢速启动，设得太低则无法有效抑制噪声。
//参考：0.2f ~ 0.4f
#define ACC_DEAD_ZONE_THRESHOLD 0.05f
// --- 模拟摩擦力，进行速度衰减 ---
#define SPEED_ATTENUATION 1.0f //暂不模拟
BLE_KS_send_data_t KS_data;
#ifdef XTELL_TEST
debug_t debug1;
debug_t debug2;
#endif
static skiing_tracker_t my_skiing_tracker;
//////////////////////////////////////////////////////////////////////////////////////////////////
//实现
void clear_speed(void){
my_skiing_tracker.state = STATIC;
memset(my_skiing_tracker.velocity, 0, sizeof(my_skiing_tracker.velocity));
my_skiing_tracker.speed = 0;
}
void start_detection(void){
my_skiing_tracker.state = STATIC;
memset(my_skiing_tracker.velocity, 0, sizeof(my_skiing_tracker.velocity));
my_skiing_tracker.distance = 0;
my_skiing_tracker.speed = 0;
}
void stop_detection(void){
my_skiing_tracker.state = STOP_DETECTION;
memset(my_skiing_tracker.velocity, 0, sizeof(my_skiing_tracker.velocity));
my_skiing_tracker.speed = 0;
}
/**
* @brief 初始化滑雪追踪器
*
* @param tracker
*/
void skiing_tracker_init(skiing_tracker_t *tracker)
{
if (!tracker) {
return;
}
// 使用memset一次性清零整个结构体，包括新增的缓冲区
memset(tracker, 0, sizeof(skiing_tracker_t));
tracker->state = STATIC;
}
/**
* @brief 当检测到落地时，计算空中的水平飞行距离并累加到总距离
*/
static void calculate_air_distance(skiing_tracker_t *tracker) {
float horizontal_speed_on_takeoff = sqrtf(
tracker->initial_velocity_on_takeoff[0] * tracker->initial_velocity_on_takeoff[0] +
tracker->initial_velocity_on_takeoff[1] * tracker->initial_velocity_on_takeoff[1]
);
float distance_in_air = horizontal_speed_on_takeoff * tracker->time_in_air;
tracker->distance += distance_in_air;
}
/**
* @brief 将设备坐标系下的加速度转换为世界坐标系
* @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
*/
void forece_of_friction(skiing_tracker_t *tracker){
// 增加速度衰减，模拟摩擦力
tracker->velocity[0] *= SPEED_ATTENUATION;
tracker->velocity[1] *= SPEED_ATTENUATION;
tracker->velocity[2] = 0; // 垂直速度强制归零
}
/**
* @brief 状态机更新
*
* @param tracker 传入同步修改后传出
* @param acc_device_ms2 三轴加速度，m/s^2
* @param gyr_dps 三轴陀螺仪，dps
*/
static void update_state_machine(skiing_tracker_t *tracker, const float *acc_device_ms2, const float *gyr_dps)
{
// 缓冲区未填满时，不进行状态判断，默认为静止
if (!tracker->buffer_filled) {
tracker->state = STATIC;
return;
}
// --- 计算关键指标 ---
float acc_variance = calculate_variance(tracker->acc_buffer); // 计算加速度方差
float gyr_variance = calculate_variance(tracker->gyr_buffer); // 计算陀螺仪方差
float gyr_magnitude = sqrtf(gyr_dps[0]*gyr_dps[0] + gyr_dps[1]*gyr_dps[1] + gyr_dps[2]*gyr_dps[2]); //dps
float acc_magnitude = sqrtf(acc_device_ms2[0]*acc_device_ms2[0] + acc_device_ms2[1]*acc_device_ms2[1] + acc_device_ms2[2]*acc_device_ms2[2]); //m/s^s
float acc_magnitude_g = acc_magnitude / G_ACCELERATION; // 转换为g单位，用于跳跃判断
#ifdef XTELL_TEST
debug1.acc_variance =acc_variance;
debug1.gyr_variance =gyr_variance;
debug1.gyr_magnitude=gyr_magnitude;
debug1.acc_magnitude=fabsf(acc_magnitude - G_ACCELERATION);
#endif
// --- 状态机逻辑 (核心修改区域) ---
#if 0 //暂时不考虑空中
// 1. 空中/落地状态的后续处理
if (tracker->state == IN_AIR) {
// A. 检测巨大冲击 -> 落地
if (acc_magnitude_g > LANDING_ACC_MAG_HIGH_THRESHOLD) {
tracker->state = LANDING;
// B. 检测超时 -> 强制落地 (安全机制)
} else if (tracker->time_in_air > MAX_TIME_IN_AIR) {
tracker->state = LANDING;
// C. 检测恢复正常重力 (平缓落地)
} else if (acc_magnitude_g > 0.8f && acc_magnitude_g < 1.5f) {
tracker->grounded_entry_counter++;
if (tracker->grounded_entry_counter >= GROUNDED_CONFIRM_COUNT) {
tracker->state = LANDING;
}
} else {
tracker->grounded_entry_counter = 0;
}
return; // 在空中或刚切换到落地，结束本次状态判断
}
// 2. 严格的 "起跳->空中" 状态转换逻辑
// 只有当处于滑行状态时，才去检测起跳意图
if (tracker->state == NO_CONSTANT_SPEED || tracker->state == CONSTANT_SPEED || tracker->state == WHEEL) {
if (acc_magnitude_g > TAKEOFF_ACC_MAG_HIGH_THRESHOLD) {
tracker->state = TAKING_OFF;
tracker->airborne_entry_counter = 0; // 准备检测失重
return;
}
}
// 只有在TAKING_OFF状态下，才去检测是否进入失重
if (tracker->state == TAKING_OFF) {
if (acc_magnitude_g < AIRBORNE_ACC_MAG_LOW_THRESHOLD) {
tracker->airborne_entry_counter++;
if (tracker->airborne_entry_counter >= AIRBORNE_CONFIRM_COUNT) {
memcpy(tracker->initial_velocity_on_takeoff, tracker->velocity, sizeof(tracker->velocity));
tracker->time_in_air = 0;
tracker->state = IN_AIR;
tracker->airborne_entry_counter = 0;
tracker->grounded_entry_counter = 0;
return;
}
} else {
// 如果在起跳冲击后一段时间内没有失重，说明只是一个颠簸，恢复滑行
// 可以加一个小的超时计数器，这里为了简单先直接恢复
tracker->state = NO_CONSTANT_SPEED;
}
return; // 无论是否切换，都结束本次判断
}
#endif
// --- 静止判断 ---
if (acc_variance < STOP_ACC_VARIANCE_THRESHOLD && gyr_variance < STOP_GYR_VARIANCE_THRESHOLD && gyr_magnitude < STOP_GYR_MAG_THRESHOLD) {
tracker->state = STATIC;
return;
}
// --- 地面状态切换逻辑 ---
switch (tracker->state) {
case LANDING:
tracker->state = STATIC;
break;
case STATIC:
// 优先判断是否进入 WOBBLE 状态
// 条件：陀螺仪活动剧烈，但整体加速度变化不大（说明是原地转或晃）
if (gyr_magnitude > ROTATION_GYR_MAG_THRESHOLD && fabsf(acc_magnitude - G_ACCELERATION) < WOBBLE_ACC_MAG_THRESHOLD) {
tracker->state = WOBBLE;
}
// 只有在陀螺仪和加速度都满足“前进”特征时，才启动
else if (gyr_variance > START_GYR_VARIANCE_THRESHOLD && fabsf(acc_magnitude - G_ACCELERATION) > START_ACC_MAG_THRESHOLD) {
tracker->state = NO_CONSTANT_SPEED;
}
break;
case WOBBLE:
// 从 WOBBLE 状态启动的条件应该和从 STATIC 启动一样严格
if (gyr_variance > START_GYR_VARIANCE_THRESHOLD && fabsf(acc_magnitude - G_ACCELERATION) > START_ACC_MAG_THRESHOLD) {
tracker->state = NO_CONSTANT_SPEED;
}
// 如果陀螺仪活动减弱，则可能恢复静止
else if (gyr_magnitude < ROTATION_GYR_MAG_THRESHOLD * 0.8f) { // 增加迟滞，避免抖动
// 不直接跳回STATIC，而是依赖下一轮的全局静止判断
}
break;
case NO_CONSTANT_SPEED: //非匀速状态
//暂时不考虑摔倒
// if (gyr_magnitude > FALLEN_GRY_MAG_THRESHOLD) {
// tracker->state = FALLEN; //摔倒
// } else
if (gyr_magnitude > WHEEL_GYR_MAG_THRESHOLD && acc_variance > WHEEL_ACC_VARIANCE_THRESHOLD) {
tracker->state = WHEEL; //转弯
} else if (fabsf(acc_magnitude - G_ACCELERATION) < SKIING_ACC_MAG_THRESHOLD) {
tracker->state = CONSTANT_SPEED; //匀速
}
break;
case CONSTANT_SPEED: //匀速状态
if (fabsf(acc_magnitude - G_ACCELERATION) > START_ACC_MAG_THRESHOLD) {
tracker->state = NO_CONSTANT_SPEED;
}
//TODO：可以添加进入转弯或摔倒的判断
break;
case WHEEL:
// 从转弯状态，检查转弯是否结束
// 如果角速度和加速度方差都降下来了，就回到普通滑行状态
if (gyr_magnitude < WHEEL_GYR_MAG_THRESHOLD * 0.8f && acc_variance < WHEEL_ACC_VARIANCE_THRESHOLD * 0.8f) { // 乘以一个滞后系数避免抖动
tracker->state = NO_CONSTANT_SPEED;
}
break;
case FALLEN:
// TODO：回到 STATIC
break;
}
}
/**
* @brief 主更新函数
*
* @param tracker
* @param acc_g 三轴加速度，g
* @param gyr_dps 三轴陀螺仪，dps
* @param angle 欧若拉角
* @param dt 采样时间间隔，会用来积分求速度
*/
void skiing_tracker_update(skiing_tracker_t *tracker, float *acc_g, float *gyr_dps, float *angle, float dt)
{
if (!tracker || !acc_g || !gyr_dps || !angle || dt <= 0) {
return;
}
if(my_skiing_tracker.state == STOP_DETECTION)
return;
// --- 数据预处理和缓冲 ---
float acc_device_ms2[3];
acc_device_ms2[0] = acc_g[0] * G_ACCELERATION;
acc_device_ms2[1] = acc_g[1] * G_ACCELERATION;
acc_device_ms2[2] = acc_g[2] * G_ACCELERATION;
// 将最新数据存入缓冲区
memcpy(tracker->acc_buffer[tracker->buffer_index], acc_device_ms2, sizeof(acc_device_ms2));
memcpy(tracker->gyr_buffer[tracker->buffer_index], gyr_dps, 3 * sizeof(float));
tracker->buffer_index++;
if (tracker->buffer_index >= VARIANCE_BUFFER_SIZE) {
tracker->buffer_index = 0;
tracker->buffer_filled = 1; // 标记缓冲区已满
}
// --- 更新状态机 ---
update_state_machine(tracker, acc_device_ms2, gyr_dps);
// --- 根据状态执行不同的计算逻辑 ---
switch (tracker->state) {
case TAKING_OFF:
tracker->speed = 0.0f;
break;
case IN_AIR:
// 在空中时，只累加滞空时间
tracker->time_in_air += dt;
break;
case LANDING:
// 刚落地，计算空中距离
calculate_air_distance(tracker);
// 清理速度和滤波器状态，为恢复地面追踪做准备
memset(tracker->velocity, 0, sizeof(tracker->velocity));
tracker->speed = 0;
memset(tracker->acc_world_unfiltered_prev, 0, sizeof(tracker->acc_world_unfiltered_prev));
memset(tracker->acc_world_filtered, 0, sizeof(tracker->acc_world_filtered));
memset(tracker->acc_world_lpf, 0, sizeof(tracker->acc_world_lpf)); // 清理新增的LPF状态
break;
case WHEEL:
case NO_CONSTANT_SPEED:
transform_acc_to_world_frame(acc_device_ms2, angle, tracker->acc_world);
tracker->acc_world[2] -= G_ACCELERATION;
float acc_world_temp[3]; // 临时变量存储当前周期的加速度
for (int i = 0; i < 2; i++) { // 只处理水平方向的 x 和 y 轴
// --- 核心修改：颠倒滤波器顺序为 HPF -> LPF ---
// 1. 高通滤波 (HPF) 先行: 消除因姿态误差导致的重力泄漏（直流偏置）
// HPF的瞬态响应会产生尖峰，这是正常的。
tracker->acc_world_filtered[i] = HPF_ALPHA * (tracker->acc_world_filtered[i] + tracker->acc_world[i] - tracker->acc_world_unfiltered_prev[i]);
tracker->acc_world_unfiltered_prev[i] = tracker->acc_world[i];
// 2. 低通滤波 (LPF) 殿后: 平滑掉HPF产生的尖峰和传感器自身的高频振动噪声。
// 这里使用 tracker->acc_world_filtered[i] 作为LPF的输入。
tracker->acc_world_lpf[i] = (1.0f - LPF_ALPHA) * tracker->acc_world_filtered[i] + LPF_ALPHA * tracker->acc_world_lpf[i];
// 将最终处理完的加速度值存入临时变量
acc_world_temp[i] = tracker->acc_world_lpf[i];
}
// 计算处理后加速度的水平模长
float acc_horizontal_mag = sqrtf(acc_world_temp[0] * acc_world_temp[0] +
acc_world_temp[1] * acc_world_temp[1]);
#if XTELL_TEST
debug2.acc_magnitude = acc_horizontal_mag;
#endif
// 应用死区，并积分
if (acc_horizontal_mag > ACC_DEAD_ZONE_THRESHOLD) {
tracker->velocity[0] += acc_world_temp[0] * dt;
tracker->velocity[1] += acc_world_temp[1] * dt;
}
// 更新速度和距离
tracker->speed = sqrtf(tracker->velocity[0] * tracker->velocity[0] +
tracker->velocity[1] * tracker->velocity[1]);
tracker->distance += tracker->speed * dt;
break;
case CONSTANT_SPEED:
//保持上次的速度不变。只更新距离
tracker->distance += tracker->speed * dt;
break;
case STATIC:
case WOBBLE:
// 速度清零，抑制漂移
memset(tracker->velocity, 0, sizeof(tracker->velocity));
tracker->speed = 0.0f;
memset(tracker->acc_world_unfiltered_prev, 0, sizeof(tracker->acc_world_unfiltered_prev));
memset(tracker->acc_world_filtered, 0, sizeof(tracker->acc_world_filtered));
memset(tracker->acc_world_lpf, 0, sizeof(tracker->acc_world_lpf)); // 清理新增的LPF状态
#if XTELL_TEST
debug2.acc_magnitude = 0;
#endif
break;
case FALLEN:
// TODO
break;
default:
break;
}
}
/**
* @brief 滑雪数据计算
*
* @param acc_data_buf 传入的三轴加速度数据
* @param gyr_data_buf 传入的三轴陀螺仪数据
* @param angle_data 传入的欧若拉角数据
* @return BLE_send_data_t 要发送给蓝牙的数据
*/
BLE_send_data_t sensor_processing_task(signed short* acc_data_buf, signed short* gyr_data_buf, float* angle_data) {
static int initialized = 0;
static float acc_data_g[3];
static float gyr_data_dps[3];
// const float delta_time = DELTA_TIME+0.01f;
// const float delta_time = DELTA_TIME + 0.005f;
const float delta_time = DELTA_TIME;
BLE_send_data_t BLE_send_data;
if (!initialized) {
skiing_tracker_init(&my_skiing_tracker);
initialized = 1;
printf("Skiing Tracker Initialized. Waiting for sensor calibration...\n");
}
#if ACC_RANGE==2
// 加速度 LSB to g
acc_data_g[0] = (float)acc_data_buf[0] / 16384.0f;
acc_data_g[1] = (float)acc_data_buf[1] / 16384.0f;
acc_data_g[2] = (float)acc_data_buf[2] / 16384.0f;
#endif
#if ACC_RANGE==4
// 加速度 LSB to g
acc_data_g[0] = (float)acc_data_buf[0] / 8192.0f;
acc_data_g[1] = (float)acc_data_buf[1] / 8192.0f;
acc_data_g[2] = (float)acc_data_buf[2] / 8192.0f;
#endif
#if ACC_RANGE==8
//±8g 4096
acc_data_g[0] = (float)acc_data_buf[0] / 4096.0f; //ax
acc_data_g[1] = (float)acc_data_buf[1] / 4096.0f; //ay
acc_data_g[2] = (float)acc_data_buf[2] / 4096.0f; //az
#endif
#if ACC_RANGE==16
//±16g 2048
acc_data_g[0] = (float)acc_data_buf[0] / 2048.0f; //ax
acc_data_g[1] = (float)acc_data_buf[1] / 2048.0f; //ay
acc_data_g[2] = (float)acc_data_buf[2] / 2048.0f; //az
#endif
// 陀螺仪 LSB to dps (度/秒)
// ±2000dps量程下，转换系数约为 0.061
gyr_data_dps[0] = (float)gyr_data_buf[0] * 0.061f;
gyr_data_dps[1] = (float)gyr_data_buf[1] * 0.061f;
gyr_data_dps[2] = (float)gyr_data_buf[2] * 0.061f;
skiing_tracker_update(&my_skiing_tracker, acc_data_g, gyr_data_dps, angle_data, delta_time);
BLE_send_data.skiing_state = my_skiing_tracker.state;
for (int i = 0; i < 3; i++) {
#ifdef XTELL_TEST
BLE_send_data.acc_data[i] = (short)(acc_data_g[i] * 9.8f) * 100; //cm/^s2
BLE_send_data.gyr_data[i] = (short)gyr_data_dps[i]; //dps
BLE_send_data.angle_data[i] = angle_data[i];
#else
BLE_send_data.acc_data[i] = (short)acc_data_buf[i]; //原始adc数据
BLE_send_data.gyr_data[i] = (short)gyr_data_buf[i]; //原始adc数据
BLE_send_data.angle_data[i] = angle_data[i];
#endif
}
BLE_send_data.speed_cms = (int)(my_skiing_tracker.speed * 100);
BLE_send_data.distance_cm = (int)(my_skiing_tracker.distance * 100);
// printf("Calculate the time interval =============== end\n");
return BLE_send_data;
}

88
apps/earphone/xtell_Sensor/A_hide/9/skiing_tracker.h
View File

@ -1,88 +0,0 @@
#ifndef SKIING_TRACKER_H
#define SKIING_TRACKER_H
#include "../xtell.h"
// 定义滑雪者可能的状态
typedef enum {
STATIC, // 静止或动态稳定：0
NO_CONSTANT_SPEED, // 正在滑雪，非匀速：1
CONSTANT_SPEED, // 正在滑雪，匀速：2
WOBBLE, // 正在原地旋转：3
WHEEL, // 转弯：4
FALLEN, // 已摔倒：5
TAKING_OFF, // 起跳冲击阶段：6
IN_AIR, // 空中失重阶段：7
LANDING, // 落地冲击阶段：8
STOP_DETECTION, // 停止检测：9
UNKNOWN // 未知状态：10
} skiing_state_t;
#define VARIANCE_BUFFER_SIZE 5 // 用于计算方差的数据窗口大小 (5个样本 @ 100Hz = 50ms)，减小延迟，提高实时性
#define DELTA_TIME 0.01f
// 追踪器数据结构体
typedef struct {
// 公开数据
float velocity[3]; // 当前速度 (x, y, z)，单位: m/s
float distance; // 总滑行距离，单位: m
float speed; // 当前速率 (标量)，单位: m/s
skiing_state_t state; // 当前滑雪状态
// 内部计算使用的私有成员
float acc_world[3]; // 在世界坐标系下的加速度
// 用于空中距离计算
float time_in_air; // 滞空时间计时器
float initial_velocity_on_takeoff[3]; // 起跳瞬间的速度向量
int airborne_entry_counter; // 进入空中状态的确认计数器
int grounded_entry_counter; // 落地确认计数器
// --- 内部计算使用的私有成员 ---
// 用于动态零速更新和旋转检测的缓冲区
float acc_buffer[VARIANCE_BUFFER_SIZE][3]; // 加速度数据窗口
float gyr_buffer[VARIANCE_BUFFER_SIZE][3]; // 角速度数据窗口
int buffer_index; // 缓冲区当前索引
int buffer_filled; // 缓冲区是否已填满的标志
// 用于高通滤波器（巴特沃斯一阶滤波器）的私有成员，以消除加速度的直流偏置
float acc_world_filtered[3]; //过滤过的
float acc_world_unfiltered_prev[3]; //上一次没过滤的
float acc_world_lpf[3]; // 经过低通滤波后的世界坐标系加速度
} skiing_tracker_t;
//ble发送的数据
typedef struct{ //__attribute__((packed)){ //该结构体取消内存对齐
char sensor_state;
char skiing_state;
int speed_cms; //求出的速度，cm/s
int distance_cm; //求出的距离，cm
short acc_data[3]; //三轴加速度, g
short gyr_data[3]; //三轴陀螺仪, dps
float angle_data[3]; //欧若拉角
}BLE_send_data_t;
typedef struct{
int acc_KS[3]; //卡尔曼后，LSB转换后的 三轴加速度数据（cm/s^2）
int gyr_KS_dps[3]; //卡尔曼后，LSB to dps 三轴陀螺仪数据
int angle_KS[3]; //卡尔曼后，计算得到的欧若拉角数据
}BLE_KS_send_data_t;
#ifdef XTELL_TEST
typedef struct{
float acc_variance; //三轴加速度方差之和
float gyr_variance; //三轴陀螺仪方差之和
float acc_magnitude; //三轴加速度模长
float gyr_magnitude; //三轴陀螺仪模长
}debug_t;
#endif
/**
* @brief 初始化滑雪追踪器
*
* @param tracker 指向 skiing_tracker_t 结构体的指针
*/
void skiing_tracker_init(skiing_tracker_t *tracker);
BLE_send_data_t sensor_processing_task(signed short* acc_data_buf, signed short* gyr_data_buf, float* angle_data) ;
#endif // SKIING_TRACKER_H

651
apps/earphone/xtell_Sensor/A_hide/calculate/skiing_tracker.c
View File

@ -1,651 +0,0 @@
/*
*/
#include "skiing_tracker.h"
#include "../sensor/SC7U22.h"
#include <math.h>
#include <string.h>
#define G_ACCELERATION 9.81f
#define DEG_TO_RAD (3.14159265f / 180.0f)
#define ENABLE_XLOG 1
#ifdef xlog
#undef xlog
#endif
#if ENABLE_XLOG
#define xlog(format, ...) printf("[XT:%s] " format, __func__, ##__VA_ARGS__)
#else
#define xlog(format, ...) ((void)0)
#endif
// --- 静止检测 ---
//两个判断是否静止的必要条件：动态零速更新(ZUPT)阈值
// 加速方差阈值，提高阈值，让“刹车”更灵敏，以便在波浪式前进等慢速漂移时也能触发零速更新
#define STOP_ACC_VARIANCE_THRESHOLD 0.2f
// 陀螺仪方差阈值
#define STOP_GYR_VARIANCE_THRESHOLD 5.0f
// 静止时候的陀螺仪模长
#define STOP_GYR_MAG_THRESHOLD 15
// --- --- ---
// --- 启动滑雪阈值 ---
// 加速度模长与重力的差值大于此值，认为开始运动；降低阈值，让“油门”更灵敏，以便能捕捉到真实的慢速启动
#define START_ACC_MAG_THRESHOLD 1.0f //0.5、1
// 陀螺仪方差阈值，以允许启动瞬间的正常抖动，但仍能过滤掉混乱的、非滑雪的晃动。
#define START_GYR_VARIANCE_THRESHOLD 15.0f
// --- --- ---
// --- 滑雪过程 ---
//加速度 模长(不含重力），低于此值视为 在做匀速运动
#define SKIING_ACC_MAG_THRESHOLD 0.5f
//陀螺仪 模长，高于此值视为 摔倒了
#define FALLEN_GRY_MAG_THRESHOLD 2000.0f //未确定
// --- --- ---
// --- 原地旋转抖动 ---
// 加速度 方差 阈值。此值比 静止检测 阈值更宽松，
#define WOBBLE_ACC_VARIANCE_THRESHOLD 0.5f
// 加速度 模长 阈值
#define WOBBLE_ACC_MAG_THRESHOLD 1.0f
// 角速度 总模长 大于此值(度/秒)，认为正在进行非滑雪的旋转或摆动
#define ROTATION_GYR_MAG_THRESHOLD 30.0f
// --- --- ---
// --- 滑雪转弯动 ---
// 加速度 方差 阈值，大于此值，滑雪过程可能发生了急转弯
#define WHEEL_ACC_VARIANCE_THRESHOLD 7.0f
// 角速度 总模长 大于此值(度/秒)，认为滑雪过程中进行急转弯
#define WHEEL_GYR_MAG_THRESHOLD 500.0f //
// --- --- ---
// --- 跳跃 ---
// 加速度模长低于此值(g)，认为进入失重状态(IN_AIR)
#define AIRBORNE_ACC_MAG_LOW_THRESHOLD 0.4f
// 加速度模长高于此值(g)，认为发生落地冲击(LANDING)
#define LANDING_ACC_MAG_HIGH_THRESHOLD 3.5f
// 起跳加速度阈值(g)，用于进入TAKING_OFF状态
#define TAKEOFF_ACC_MAG_HIGH_THRESHOLD 1.8f
// 进入空中状态确认计数：需要连续3个采样点加速度低于阈值才判断为起跳
#define AIRBORNE_CONFIRM_COUNT 3
// 落地状态确认计数：加速度恢复到1g附近并持续2个采样点(20ms)则认为已落地
#define GROUNDED_CONFIRM_COUNT 2
// 最大滞空时间(秒)，超过此时间强制认为已落地，防止状态锁死
#define MAX_TIME_IN_AIR 12.5f
// --- --- ---
// --- 用于消除积分漂移的滤波器和阈值 ---
// 高通滤波器系数 (alpha)。alpha 越接近1，滤除低频(直流偏移)的效果越强，但可能滤掉真实的慢速运动。
// alpha = RC / (RC + dt)，参考RC电路而来，fc ≈ (1 - alpha) / (2 * π * dt)
#define HPF_ALPHA 0.999f
//0.995f： 0.08 Hz 的信号
//0.999f： 0.0159 Hz
// --- --- ---
// --- 低通滤波器 ---
// 低通滤波器系数 (alpha)。alpha 越小，滤波效果越强（更平滑），但延迟越大。
// alpha 推荐范围 0.7 ~ 0.95。可以从 0.85 开始尝试。
#define LPF_ALPHA 0.7f
// 加速度死区阈值 (m/s^2)。低于此阈值的加速度被认为是噪声，不参与积分。
// 设得太高会忽略真实的慢速启动，设得太低则无法有效抑制噪声。
//参考：0.2f ~ 0.4f
#define ACC_DEAD_ZONE_THRESHOLD 0.05f
// --- 模拟摩擦力，进行速度衰减 ---
#define SPEED_ATTENUATION 1.0f //暂不模拟
BLE_KS_send_data_t KS_data;
static float quaternion_data[4];
#ifdef XTELL_TEST
debug_t debug1;
debug_t debug2;
#endif
static skiing_tracker_t my_skiing_tracker;
//////////////////////////////////////////////////////////////////////////////////////////////////
//实现
void clear_speed(void){
my_skiing_tracker.state = STATIC;
memset(my_skiing_tracker.velocity, 0, sizeof(my_skiing_tracker.velocity));
my_skiing_tracker.speed = 0;
}
void start_detection(void){
my_skiing_tracker.state = STATIC;
memset(my_skiing_tracker.velocity, 0, sizeof(my_skiing_tracker.velocity));
my_skiing_tracker.distance = 0;
my_skiing_tracker.speed = 0;
}
void stop_detection(void){
my_skiing_tracker.state = STOP_DETECTION;
memset(my_skiing_tracker.velocity, 0, sizeof(my_skiing_tracker.velocity));
my_skiing_tracker.speed = 0;
}
/**
* @brief 初始化滑雪追踪器
*
* @param tracker
*/
void skiing_tracker_init(skiing_tracker_t *tracker)
{
if (!tracker) {
return;
}
// 使用memset一次性清零整个结构体，包括新增的缓冲区
memset(tracker, 0, sizeof(skiing_tracker_t));
tracker->state = STATIC;
}
/**
* @brief 当检测到落地时，计算空中的水平飞行距离并累加到总距离
*/
static void calculate_air_distance(skiing_tracker_t *tracker) {
float horizontal_speed_on_takeoff = sqrtf(
tracker->initial_velocity_on_takeoff[0] * tracker->initial_velocity_on_takeoff[0] +
tracker->initial_velocity_on_takeoff[1] * tracker->initial_velocity_on_takeoff[1]
);
float distance_in_air = horizontal_speed_on_takeoff * tracker->time_in_air;
tracker->distance += distance_in_air;
}
/**
* @brief 使用四元数直接从设备坐标系的加速度中移除重力分量
* @details 这种方法比使用欧拉角更精确、更稳定，且避免了万向节死锁。
* @param acc_device 输入：设备坐标系下的原始加速度 [x, y, z], 单位 m/s^2
* @param q 输入：表示姿态的四元数 [w, x, y, z]
* @param acc_linear_device 输出：设备坐标系下移除重力后的线性加速度 [x, y, z]
*/
void q_remove_gravity_with_quaternion(const float *acc_device, const float *q, float *acc_linear_device)
{
// 从四元数计算重力在设备坐标系下的投影
// G_device = R_transpose * G_world
// G_world = [0, 0, g]
// R_transpose 的第三列即为重力投影方向
float gx = 2.0f * (q[1] * q[3] - q[0] * q[2]);
float gy = 2.0f * (q[0] * q[1] + q[2] * q[3]);
float gz = q[0] * q[0] - q[1] * q[1] - q[2] * q[2] + q[3] * q[3];
// 从原始加速度中减去重力分量
acc_linear_device[0] = acc_device[0] - gx * G_ACCELERATION;
acc_linear_device[1] = acc_device[1] - gy * G_ACCELERATION;
acc_linear_device[2] = acc_device[2] - gz * G_ACCELERATION;
}
/**
* @brief 使用四元数将设备坐标系的线性加速度转换到世界坐标系
* @details 同样，此方法比使用欧拉角更优。
* @param acc_linear_device 输入：设备坐标系下的线性加速度 [x, y, z]
* @param q 输入：表示姿态的四元数 [w, x, y, z]
* @param acc_linear_world 输出：世界坐标系下的线性加速度 [x, y, z]
*/
void q_transform_to_world_with_quaternion(const float *acc_linear_device, const float *q, float *acc_linear_world)
{
// 这是 R_device_to_world * acc_linear_device 的展开形式
acc_linear_world[0] = (1.0f - 2.0f*q[2]*q[2] - 2.0f*q[3]*q[3]) * acc_linear_device[0] +
(2.0f*q[1]*q[2] - 2.0f*q[0]*q[3]) * acc_linear_device[1] +
(2.0f*q[1]*q[3] + 2.0f*q[0]*q[2]) * acc_linear_device[2];
acc_linear_world[1] = (2.0f*q[1]*q[2] + 2.0f*q[0]*q[3]) * acc_linear_device[0] +
(1.0f - 2.0f*q[1]*q[1] - 2.0f*q[3]*q[3]) * acc_linear_device[1] +
(2.0f*q[2]*q[3] - 2.0f*q[0]*q[1]) * acc_linear_device[2];
acc_linear_world[2] = (2.0f*q[1]*q[3] - 2.0f*q[0]*q[2]) * acc_linear_device[0] +
(2.0f*q[2]*q[3] + 2.0f*q[0]*q[1]) * acc_linear_device[1] +
(1.0f - 2.0f*q[1]*q[1] - 2.0f*q[2]*q[2]) * acc_linear_device[2];
// acc_linear_world[2] -= G_ACCELERATION;
}
/**
* @brief 计算缓冲区内三轴数据的方差之和
*
* @param buffer 传进来的三轴数据：陀螺仪/加速度
* @return float 返回方差和
*/
static float calculate_variance(float buffer[VARIANCE_BUFFER_SIZE][3])
{
float mean[3] = {0};
float variance[3] = {0};
// 计算均值
for (int i = 0; i < VARIANCE_BUFFER_SIZE; i++) {
mean[0] += buffer[i][0];
mean[1] += buffer[i][1];
mean[2] += buffer[i][2];
}
mean[0] /= VARIANCE_BUFFER_SIZE;
mean[1] /= VARIANCE_BUFFER_SIZE;
mean[2] /= VARIANCE_BUFFER_SIZE;
// 计算方差
for (int i = 0; i < VARIANCE_BUFFER_SIZE; i++) {
variance[0] += (buffer[i][0] - mean[0]) * (buffer[i][0] - mean[0]);
variance[1] += (buffer[i][1] - mean[1]) * (buffer[i][1] - mean[1]);
variance[2] += (buffer[i][2] - mean[2]) * (buffer[i][2] - mean[2]);
}
variance[0] /= VARIANCE_BUFFER_SIZE;
variance[1] /= VARIANCE_BUFFER_SIZE;
variance[2] /= VARIANCE_BUFFER_SIZE;
// 返回三轴方差之和，作为一个综合的稳定度指标
return variance[0] + variance[1] + variance[2];
}
/**
* @brief 摩擦力模拟，进行速度衰减
*
* @param tracker
*/
void forece_of_friction(skiing_tracker_t *tracker){
// 增加速度衰减，模拟摩擦力
tracker->velocity[0] *= SPEED_ATTENUATION;
tracker->velocity[1] *= SPEED_ATTENUATION;
tracker->velocity[2] = 0; // 垂直速度强制归零
}
/**
* @brief 状态机更新
*
* @param tracker 传入同步修改后传出
* @param acc_device_ms2 三轴加速度，m/s^2
* @param gyr_dps 三轴陀螺仪，dps
*/
static void update_state_machine(skiing_tracker_t *tracker, const float *acc_device_ms2, const float *gyr_dps)
{
// 缓冲区未填满时，不进行状态判断，默认为静止
if (!tracker->buffer_filled) {
tracker->state = STATIC;
return;
}
// --- 计算关键指标 ---
float acc_variance = calculate_variance(tracker->acc_buffer); // 计算加速度方差
float gyr_variance = calculate_variance(tracker->gyr_buffer); // 计算陀螺仪方差
float gyr_magnitude = sqrtf(gyr_dps[0]*gyr_dps[0] + gyr_dps[1]*gyr_dps[1] + gyr_dps[2]*gyr_dps[2]); //dps
float acc_magnitude = sqrtf(acc_device_ms2[0]*acc_device_ms2[0] + acc_device_ms2[1]*acc_device_ms2[1] + acc_device_ms2[2]*acc_device_ms2[2]); //m/s^s
float acc_magnitude_g = acc_magnitude / G_ACCELERATION; // 转换为g单位，用于跳跃判断
#ifdef XTELL_TEST
debug1.acc_variance =acc_variance;
debug1.gyr_variance =gyr_variance;
debug1.gyr_magnitude=gyr_magnitude;
debug1.acc_magnitude=fabsf(acc_magnitude - G_ACCELERATION);
#endif
// --- 状态机逻辑 (核心修改区域) ---
#if 0 //暂时不考虑空中
// 1. 空中/落地状态的后续处理
if (tracker->state == IN_AIR) {
// A. 检测巨大冲击 -> 落地
if (acc_magnitude_g > LANDING_ACC_MAG_HIGH_THRESHOLD) {
tracker->state = LANDING;
// B. 检测超时 -> 强制落地 (安全机制)
} else if (tracker->time_in_air > MAX_TIME_IN_AIR) {
tracker->state = LANDING;
// C. 检测恢复正常重力 (平缓落地)
} else if (acc_magnitude_g > 0.8f && acc_magnitude_g < 1.5f) {
tracker->grounded_entry_counter++;
if (tracker->grounded_entry_counter >= GROUNDED_CONFIRM_COUNT) {
tracker->state = LANDING;
}
} else {
tracker->grounded_entry_counter = 0;
}
return; // 在空中或刚切换到落地，结束本次状态判断
}
// 2. 严格的 "起跳->空中" 状态转换逻辑
// 只有当处于滑行状态时，才去检测起跳意图
if (tracker->state == NO_CONSTANT_SPEED || tracker->state == CONSTANT_SPEED || tracker->state == WHEEL) {
if (acc_magnitude_g > TAKEOFF_ACC_MAG_HIGH_THRESHOLD) {
tracker->state = TAKING_OFF;
tracker->airborne_entry_counter = 0; // 准备检测失重
return;
}
}
// 只有在TAKING_OFF状态下，才去检测是否进入失重
if (tracker->state == TAKING_OFF) {
if (acc_magnitude_g < AIRBORNE_ACC_MAG_LOW_THRESHOLD) {
tracker->airborne_entry_counter++;
if (tracker->airborne_entry_counter >= AIRBORNE_CONFIRM_COUNT) {
memcpy(tracker->initial_velocity_on_takeoff, tracker->velocity, sizeof(tracker->velocity));
tracker->time_in_air = 0;
tracker->state = IN_AIR;
tracker->airborne_entry_counter = 0;
tracker->grounded_entry_counter = 0;
return;
}
} else {
// 如果在起跳冲击后一段时间内没有失重，说明只是一个颠簸，恢复滑行
// 可以加一个小的超时计数器，这里为了简单先直接恢复
tracker->state = NO_CONSTANT_SPEED;
}
return; // 无论是否切换，都结束本次判断
}
#endif
// --- 静止判断 ---
if (acc_variance < STOP_ACC_VARIANCE_THRESHOLD && gyr_variance < STOP_GYR_VARIANCE_THRESHOLD && gyr_magnitude < STOP_GYR_MAG_THRESHOLD) {
tracker->state = STATIC;
return;
}
// --- 地面状态切换逻辑 ---
switch (tracker->state) {
case LANDING:
tracker->state = STATIC;
break;
case STATIC:
// 优先判断是否进入 WOBBLE 状态
// 条件：陀螺仪活动剧烈，但整体加速度变化不大（说明是原地转或晃）
if (gyr_magnitude > ROTATION_GYR_MAG_THRESHOLD && fabsf(acc_magnitude - G_ACCELERATION) < WOBBLE_ACC_MAG_THRESHOLD) {
tracker->state = WOBBLE;
}
// 只有在陀螺仪和加速度都满足“前进”特征时，才启动
else if (gyr_variance > START_GYR_VARIANCE_THRESHOLD && fabsf(acc_magnitude - G_ACCELERATION) > START_ACC_MAG_THRESHOLD) {
tracker->state = NO_CONSTANT_SPEED;
}
break;
case WOBBLE:
// 从 WOBBLE 状态启动的条件应该和从 STATIC 启动一样严格
if (gyr_variance < START_GYR_VARIANCE_THRESHOLD * 2 && fabsf(acc_magnitude - G_ACCELERATION) > START_ACC_MAG_THRESHOLD) {
tracker->state = NO_CONSTANT_SPEED;
}
// 如果陀螺仪活动减弱，则可能恢复静止
else if (gyr_magnitude < ROTATION_GYR_MAG_THRESHOLD * 0.8f) { // 增加迟滞，避免抖动
// 不直接跳回STATIC，而是依赖下一轮的全局静止判断
}
break;
case NO_CONSTANT_SPEED: //非匀速状态
//暂时不考虑摔倒
// if (gyr_magnitude > FALLEN_GRY_MAG_THRESHOLD) {
// tracker->state = FALLEN; //摔倒
// } else
if (gyr_magnitude > WHEEL_GYR_MAG_THRESHOLD && acc_variance > WHEEL_ACC_VARIANCE_THRESHOLD) {
tracker->state = WHEEL; //转弯
} else if (fabsf(acc_magnitude - G_ACCELERATION) < SKIING_ACC_MAG_THRESHOLD) {
tracker->state = CONSTANT_SPEED; //匀速
}
break;
case CONSTANT_SPEED: //匀速状态
if (fabsf(acc_magnitude - G_ACCELERATION) > START_ACC_MAG_THRESHOLD) {
tracker->state = NO_CONSTANT_SPEED;
}
//TODO：可以添加进入转弯或摔倒的判断
break;
case WHEEL:
// 从转弯状态，检查转弯是否结束
// 如果角速度和加速度方差都降下来了，就回到普通滑行状态
if (gyr_magnitude < WHEEL_GYR_MAG_THRESHOLD * 0.8f && acc_variance < WHEEL_ACC_VARIANCE_THRESHOLD * 0.8f) { // 乘以一个滞后系数避免抖动
tracker->state = NO_CONSTANT_SPEED;
}
break;
case FALLEN:
// TODO：回到 STATIC
break;
}
}
/**
* @brief 主更新函数
*
* @param tracker
* @param acc_g 三轴加速度，g
* @param gyr_dps 三轴陀螺仪，dps
* @param angle 欧若拉角
* @param dt 采样时间间隔，会用来积分求速度
*/
void skiing_tracker_update(skiing_tracker_t *tracker, float *acc_g, float *gyr_dps, float *angle, float dt)
{
if (!tracker || !acc_g || !gyr_dps || !angle || dt <= 0) {
return;
}
if(my_skiing_tracker.state == STOP_DETECTION)
return;
// --- 数据预处理和缓冲 ---
float acc_device_ms2[3];
acc_device_ms2[0] = acc_g[0] * G_ACCELERATION;
acc_device_ms2[1] = acc_g[1] * G_ACCELERATION;
acc_device_ms2[2] = acc_g[2] * G_ACCELERATION;
// 将最新数据存入缓冲区
memcpy(tracker->acc_buffer[tracker->buffer_index], acc_device_ms2, sizeof(acc_device_ms2));
memcpy(tracker->gyr_buffer[tracker->buffer_index], gyr_dps, 3 * sizeof(float));
tracker->buffer_index++;
if (tracker->buffer_index >= VARIANCE_BUFFER_SIZE) {
tracker->buffer_index = 0;
tracker->buffer_filled = 1; // 标记缓冲区已满
}
// --- 更新状态机 ---
update_state_machine(tracker, acc_device_ms2, gyr_dps);
// --- 根据状态执行不同的计算逻辑 ---
switch (tracker->state) {
case TAKING_OFF:
tracker->speed = 0.0f;
break;
case IN_AIR:
// 在空中时，只累加滞空时间
tracker->time_in_air += dt;
break;
case LANDING:
// 刚落地，计算空中距离
calculate_air_distance(tracker);
// 清理速度和滤波器状态，为恢复地面追踪做准备
memset(tracker->velocity, 0, sizeof(tracker->velocity));
tracker->speed = 0;
memset(tracker->acc_world_unfiltered_prev, 0, sizeof(tracker->acc_world_unfiltered_prev));
memset(tracker->acc_world_filtered, 0, sizeof(tracker->acc_world_filtered));
memset(tracker->acc_world_lpf, 0, sizeof(tracker->acc_world_lpf)); // 清理新增的LPF状态
break;
case WHEEL:
case NO_CONSTANT_SPEED:
float linear_acc_device[3];
float linear_acc_world[3];
// 在设备坐标系下，移除重力，得到线性加速度
q_remove_gravity_with_quaternion(acc_device_ms2, quaternion_data, linear_acc_device);
// 将设备坐标系下的线性加速度，旋转到世界坐标系
q_transform_to_world_with_quaternion(linear_acc_device, quaternion_data, linear_acc_world);
// 将最终用于积分的加速度存入 tracker 结构体
memcpy(tracker->acc_no_g, linear_acc_world, sizeof(linear_acc_world));
float acc_world_temp[3]; // 临时变量存储当前周期的加速度
for (int i = 0; i < 2; i++) { // 只处理水平方向的 x 和 y 轴
// --- 核心修改：颠倒滤波器顺序为 HPF -> LPF ---
// 1. 高通滤波 (HPF) 先行: 消除因姿态误差导致的重力泄漏（直流偏置）
// HPF的瞬态响应会产生尖峰，这是正常的。
tracker->acc_world_filtered[i] = HPF_ALPHA * (tracker->acc_world_filtered[i] + tracker->acc_no_g[i] - tracker->acc_world_unfiltered_prev[i]);
tracker->acc_world_unfiltered_prev[i] = tracker->acc_no_g[i];
// 2. 低通滤波 (LPF) 殿后: 平滑掉HPF产生的尖峰和传感器自身的高频振动噪声。
// 这里使用 tracker->acc_world_filtered[i] 作为LPF的输入。
tracker->acc_world_lpf[i] = (1.0f - LPF_ALPHA) * tracker->acc_world_filtered[i] + LPF_ALPHA * tracker->acc_world_lpf[i];
// 将最终处理完的加速度值存入临时变量
acc_world_temp[i] = tracker->acc_world_lpf[i];
}
// 计算处理后加速度的水平模长
float acc_horizontal_mag = sqrtf(acc_world_temp[0] * acc_world_temp[0] +
acc_world_temp[1] * acc_world_temp[1]);
#if XTELL_TEST
debug2.acc_magnitude = acc_horizontal_mag;
#endif
// 应用死区，并积分
if (acc_horizontal_mag > ACC_DEAD_ZONE_THRESHOLD) {
tracker->velocity[0] += acc_world_temp[0] * dt;
tracker->velocity[1] += acc_world_temp[1] * dt;
}
// 更新速度和距离
tracker->speed = sqrtf(tracker->velocity[0] * tracker->velocity[0] +
tracker->velocity[1] * tracker->velocity[1]);
tracker->distance += tracker->speed * dt;
break;
case CONSTANT_SPEED:
//保持上次的速度不变。只更新距离
tracker->distance += tracker->speed * dt;
break;
case STATIC:
case WOBBLE:
// 速度清零，抑制漂移
memset(tracker->velocity, 0, sizeof(tracker->velocity));
tracker->speed = 0.0f;
memset(tracker->acc_world_unfiltered_prev, 0, sizeof(tracker->acc_world_unfiltered_prev));
memset(tracker->acc_world_filtered, 0, sizeof(tracker->acc_world_filtered));
memset(tracker->acc_world_lpf, 0, sizeof(tracker->acc_world_lpf)); // 清理新增的LPF状态
#if XTELL_TEST
debug2.acc_magnitude = 0;
#endif
break;
case FALLEN:
// TODO
break;
default:
break;
}
#if 1
float linear_acc_device[3];
float linear_acc_world[3];
float tmp_world_acc[3];
// 在设备坐标系下，移除重力，得到线性加速度
q_remove_gravity_with_quaternion(acc_device_ms2, quaternion_data, linear_acc_device);
// 将设备坐标系下的线性加速度，旋转到世界坐标系
q_transform_to_world_with_quaternion(linear_acc_device, quaternion_data, tmp_world_acc);
float all_world_mag = sqrtf(tmp_world_acc[0] * tmp_world_acc[0] +
tmp_world_acc[1] * tmp_world_acc[1] +
tmp_world_acc[2] * tmp_world_acc[2]);
static int count = 0;
if(count > 100){
xlog("===original(g): x %.2f, y %.2f, z %.2f===\n",acc_g[0],acc_g[1],acc_g[2]);
xlog("===world(m/s^2) no g: x %.2f, y %.2f, z %.2f, all %.2f===\n",tmp_world_acc[0],tmp_world_acc[1],tmp_world_acc[2],all_world_mag); //去掉重力加速度
xlog("===gyr(dps) : x %.2f, y %.2f, z %.2f===\n",gyr_dps[0],gyr_dps[1],gyr_dps[2]); //angle
xlog("===angle : x %.2f, y %.2f, z %.2f,===\n",angle[0],angle[1],angle[2]);
count = 0;
}
count++;
#endif
}
/**
* @brief 滑雪数据计算
*
* @param acc_data_buf 传入的三轴加速度数据
* @param gyr_data_buf 传入的三轴陀螺仪数据
* @param angle_data 传入的欧若拉角数据
* @return BLE_send_data_t 要发送给蓝牙的数据
*/
BLE_send_data_t sensor_processing_task(signed short* acc_data_buf, signed short* gyr_data_buf, float* angle_data, float* quaternion) {
static int initialized = 0;
static float acc_data_g[3];
static float gyr_data_dps[3];
if(quaternion != NULL){
memcpy(quaternion_data, quaternion, 4 * sizeof(float));
}
// const float delta_time = DELTA_TIME+0.01f;
// const float delta_time = DELTA_TIME + 0.005f;
const float delta_time = DELTA_TIME;
BLE_send_data_t BLE_send_data;
if (!initialized) {
skiing_tracker_init(&my_skiing_tracker);
initialized = 1;
printf("Skiing Tracker Initialized. Waiting for sensor calibration...\n");
}
#if ACC_RANGE==2
// 加速度 LSB to g
acc_data_g[0] = (float)acc_data_buf[0] / 16384.0f;
acc_data_g[1] = (float)acc_data_buf[1] / 16384.0f;
acc_data_g[2] = (float)acc_data_buf[2] / 16384.0f;
#endif
#if ACC_RANGE==4
// 加速度 LSB to g
acc_data_g[0] = (float)acc_data_buf[0] / 8192.0f;
acc_data_g[1] = (float)acc_data_buf[1] / 8192.0f;
acc_data_g[2] = (float)acc_data_buf[2] / 8192.0f;
#endif
#if ACC_RANGE==8
//±8g 4096
acc_data_g[0] = (float)acc_data_buf[0] / 4096.0f; //ax
acc_data_g[1] = (float)acc_data_buf[1] / 4096.0f; //ay
acc_data_g[2] = (float)acc_data_buf[2] / 4096.0f; //az
#endif
#if ACC_RANGE==16
//±16g 2048
acc_data_g[0] = (float)acc_data_buf[0] / 2048.0f; //ax
acc_data_g[1] = (float)acc_data_buf[1] / 2048.0f; //ay
acc_data_g[2] = (float)acc_data_buf[2] / 2048.0f; //az
#endif
// 陀螺仪 LSB to dps (度/秒)
// ±2000dps量程下，转换系数约为 0.061
gyr_data_dps[0] = (float)gyr_data_buf[0] * 0.061f;
gyr_data_dps[1] = (float)gyr_data_buf[1] * 0.061f;
gyr_data_dps[2] = (float)gyr_data_buf[2] * 0.061f;
skiing_tracker_update(&my_skiing_tracker, acc_data_g, gyr_data_dps, angle_data, delta_time);
BLE_send_data.skiing_state = my_skiing_tracker.state;
for (int i = 0; i < 3; i++) {
#ifdef XTELL_TEST
BLE_send_data.acc_data[i] = (short)(acc_data_g[i] * 9.8f) * 100; //cm/^s2
BLE_send_data.gyr_data[i] = (short)gyr_data_dps[i]; //dps
BLE_send_data.angle_data[i] = angle_data[i];
#else
BLE_send_data.acc_data[i] = (short)acc_data_buf[i]; //原始adc数据
BLE_send_data.gyr_data[i] = (short)gyr_data_buf[i]; //原始adc数据
BLE_send_data.angle_data[i] = angle_data[i];
#endif
}
BLE_send_data.speed_cms = (int)(my_skiing_tracker.speed * 100);
BLE_send_data.distance_cm = (int)(my_skiing_tracker.distance * 100);
// printf("Calculate the time interval =============== end\n");
return BLE_send_data;
}

88
apps/earphone/xtell_Sensor/A_hide/calculate/skiing_tracker.h
View File

@ -1,88 +0,0 @@
#ifndef SKIING_TRACKER_H
#define SKIING_TRACKER_H
#include "../xtell.h"
// 定义滑雪者可能的状态
typedef enum {
STATIC, // 静止或动态稳定：0
NO_CONSTANT_SPEED, // 正在滑雪，非匀速：1
CONSTANT_SPEED, // 正在滑雪，匀速：2
WOBBLE, // 正在原地旋转：3
WHEEL, // 转弯：4
FALLEN, // 已摔倒：5
TAKING_OFF, // 起跳冲击阶段：6
IN_AIR, // 空中失重阶段：7
LANDING, // 落地冲击阶段：8
STOP_DETECTION, // 停止检测：9
UNKNOWN // 未知状态：10
} skiing_state_t;
#define VARIANCE_BUFFER_SIZE 5 // 用于计算方差的数据窗口大小 (5个样本 @ 100Hz = 50ms)，减小延迟，提高实时性
#define DELTA_TIME 0.01f
// 追踪器数据结构体
typedef struct {
// 公开数据
float velocity[3]; // 当前速度 (x, y, z)，单位: m/s
float distance; // 总滑行距离，单位: m
float speed; // 当前速率 (标量)，单位: m/s
skiing_state_t state; // 当前滑雪状态
// 内部计算使用的私有成员
float acc_no_g[3]; // 去掉重力分量后的加速度
// 用于空中距离计算
float time_in_air; // 滞空时间计时器
float initial_velocity_on_takeoff[3]; // 起跳瞬间的速度向量
int airborne_entry_counter; // 进入空中状态的确认计数器
int grounded_entry_counter; // 落地确认计数器
// --- 内部计算使用的私有成员 ---
// 用于动态零速更新和旋转检测的缓冲区
float acc_buffer[VARIANCE_BUFFER_SIZE][3]; // 加速度数据窗口
float gyr_buffer[VARIANCE_BUFFER_SIZE][3]; // 角速度数据窗口
int buffer_index; // 缓冲区当前索引
int buffer_filled; // 缓冲区是否已填满的标志
// 用于高通滤波器（巴特沃斯一阶滤波器）的私有成员，以消除加速度的直流偏置
float acc_world_filtered[3]; //过滤过的
float acc_world_unfiltered_prev[3]; //上一次没过滤的
float acc_world_lpf[3]; // 经过低通滤波后的世界坐标系加速度
} skiing_tracker_t;
//ble发送的数据
typedef struct{ //__attribute__((packed)){ //该结构体取消内存对齐
char sensor_state;
char skiing_state;
int speed_cms; //求出的速度，cm/s
int distance_cm; //求出的距离，cm
short acc_data[3]; //三轴加速度, g
short gyr_data[3]; //三轴陀螺仪, dps
float angle_data[3]; //欧若拉角
}BLE_send_data_t;
typedef struct{
int acc_KS[3]; //卡尔曼后，LSB转换后的 三轴加速度数据（cm/s^2）
int gyr_KS_dps[3]; //卡尔曼后，LSB to dps 三轴陀螺仪数据
int angle_KS[3]; //卡尔曼后，计算得到的欧若拉角数据
}BLE_KS_send_data_t;
#ifdef XTELL_TEST
typedef struct{
float acc_variance; //三轴加速度方差之和
float gyr_variance; //三轴陀螺仪方差之和
float acc_magnitude; //三轴加速度模长
float gyr_magnitude; //三轴陀螺仪模长
}debug_t;
#endif
/**
* @brief 初始化滑雪追踪器
*
* @param tracker 指向 skiing_tracker_t 结构体的指针
*/
void skiing_tracker_init(skiing_tracker_t *tracker);
BLE_send_data_t sensor_processing_task(signed short* acc_data_buf, signed short* gyr_data_buf, float* angle_data, float* quaternion);
#endif // SKIING_TRACKER_H

95
apps/earphone/xtell_Sensor/README.md
View File

@ -1,95 +0,0 @@
# 时间间隔 -- 软件模拟iic的情况下
目前测试代码如下：
```c
create_process(&test_id, "test",NULL, test, (int)(DELTA_TIME*1000));
```
对于test函数的调用时间设置的是10ms调用一次，test代码如下：
```c
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);
//----省略-----
//一些ble数据发送
memset(&BLE_send_data, 0, sizeof(BLE_send_data_t));
memset(&data, 0, 50);
// xlog("end============\n");
}
```
sensor_processing_task当中就进行了计算，包括卡尔曼等，在timer设置成10ms的情况下，实际上test函数（或者是sensor_processing_task函数），距离上次调用到本次调用，实际间隔为40ms
计算距离不能直接采用timers设置的时间间隔作为dt来求距离，实际每次计算求速度的时间应该是40ms
# 11.13
代码主要文件夹：apps\earphone\xtell_Sensor
- apps\earphone\xtell_Sensor\send_data.c，‘ xtell_task_create ’ 函数，传感器计算程序逻辑开始位置，包括传感器读取数据的任务、蓝牙发送任务、速度距离计算任务
- apps\earphone\xtell_Sensor\calculate：目前只有计算传感器去除重力分量的代码
- 问题：存在漂移
- 水平的情况下三轴去掉重力分量，计算出来的结果, 误差有0.0x m/s^s
- original(g): x -0.02, y 0.00, z 1.00
device(m/s^2) no g: x -0.08, y -0.01, z -0.04, all 0.08
- 在板子任意倾斜角度下，去掉各轴重力分量的情况下，误差有0.3x m/s^s
- ===original(g): x -0.20, y -0.85, z 0.41===
- ===device(m/s^2) no g: x 0.06, y 0.31, z -0.10, all 0.33===
- apps\earphone\xtell_Sensor\sensor：传感器驱动，参与编译的为SC7U22.c和SC7U22.h
- apps\earphone\xtell_Sensor\A_hide：速度和距离计算代码
- 最新一版为apps\earphone\xtell_Sensor\A_hide\10\，水平距离测速2、3m误差
- 要使用只需要把代码复制粘贴到calculate文件夹下
# 11.18
去除重力分量后仍有误差：
- 数据对齐？
- 有没有丢数据？
- 重复定位的数据？
- 静态时的角度误差？
定时器1的回调函数（10ms调用一次）**A**：读取传感器数据，放进buff
定时器2的回调函数（5ms调用一次）**B**：读取buff的传感器数据，去除重力分离的计算
- **数据没有对齐**，A 的回调调用计数 > B 的回调调用计数
- **丢数据了**，A 读取传感器数据的回调函数中，打印了buff已满的log
- **重复定位**：移动后回到原先的位置，前后的计算得到的三轴角度相同
- **静态时的角度误差**：1°左右
- 定时器2不进行重力分离计算，只进行计数，也仍然有数据没有对齐和丢数据的情况
将读取传感器数据、去除重力分量计算放到同一个任务下，同步进行
- 数据没有丢失，数据也对齐了
- 在小倾斜的坡面下，去除重力分量后的总的加速度，**小于0.1m/s^2**
- 在大倾斜的坡面下（如旋转超过70°），去除重力分量后的总的加速度，在**0.4m/s^2上下**
- 貌似是角度越大，越接近方向锁，导致角度更容易漂移造错数据错误
采用四元数的方式做去除重力分量的计算：
- 将读取传感器数据、去除重力分量计算放到同一个任务下
- 在小倾斜的坡面下，去除重力分量后的总的加速度，低于**0.04m/s^2**
- 在大倾斜的坡面下（如旋转超过70°），去除重力分量后的总的加速度，在**0.1m/s^2上下**
- 大倾斜角度的误差要靠磁力计来消除（yaw无法通过加速度计来消除偏差）

166
apps/earphone/xtell_Sensor/ano/ano_protocol.c
View File

@ -1,166 +0,0 @@
/*
发送数据给上位机的,需要将log打印出口关闭
*/
#include "ano_protocol.h"
#include "asm/uart_dev.h"
#include "app_config.h"
// 定义协议常量
#define ANO_FRAME_HEADER 0xAA
#define ANO_TO_COMPUTER_ADDR 0xFF
// 用于保存 uart_dev_open 返回的句柄
static const uart_bus_t *ano_uart_dev = NULL;
/**
* @brief 计算并填充协议的校验和
* @param frame_buffer 指向数据帧缓冲区的指针
*/
static void ano_calculate_checksum(u8 *frame_buffer) {
#if TCFG_UART0_ENABLE==0
u8 sum_check = 0;
u8 add_check = 0;
// 数据长度在索引为 3 的位置
u8 data_len = frame_buffer[3];
// 需要计算校验和的总长度 = 帧头(1) + 地址(1) + ID(1) + 长度(1) + 数据(data_len)
u16 checksum_len = 4 + data_len;
for (u16 i = 0; i < checksum_len; i++) {
sum_check += frame_buffer[i];
add_check += sum_check;
}
// 将计算出的校验和填充到帧的末尾
frame_buffer[checksum_len] = sum_check;
frame_buffer[checksum_len + 1] = add_check;
#endif
}
/**
* @brief 初始化用于上位机通信的串口
*/
int ano_protocol_init(u32 baudrate) {
#if TCFG_UART0_ENABLE==0
// 防止重复初始化
if (ano_uart_dev) {
return 0;
}
struct uart_platform_data_t ut_arg = {0};
// TCFG_ONLINE_TX_PORT 和 TCFG_ONLINE_RX_PORT 通常在 app_config.h 中定义
ut_arg.tx_pin = TCFG_ONLINE_TX_PORT;
ut_arg.rx_pin = (u8)-1; // -1 表示不使用该引脚，因为我们只发送数据
ut_arg.baud = baudrate;
// 以下为接收相关配置，由于只发送，全部设为0或NULL
ut_arg.rx_cbuf = NULL;
ut_arg.rx_cbuf_size = 0;
ut_arg.frame_length = 0;
ut_arg.rx_timeout = 0;
ut_arg.isr_cbfun = NULL;
ano_uart_dev = (uart_bus_t *)uart_dev_open(&ut_arg);
if (ano_uart_dev == NULL) {
return -1;
}
#endif
return 0;
}
/**
* @brief 发送惯性传感器数据 (ID: 0x01)
*/
void ano_send_inertial_data(s16 acc_x, s16 acc_y, s16 acc_z,
s16 gyr_x, s16 gyr_y, s16 gyr_z,
u8 shock_sta) {
#if TCFG_UART0_ENABLE==0
if (ano_uart_dev == NULL) {
return; // 如果串口未初始化，则不执行任何操作
}
// 帧总长度 = 4(固定头) + 13(数据) + 2(校验) = 19 字节
u8 frame_buffer[19];
u8 data_idx = 4; // DATA区域从索引4开始
// 1. 填充帧头、地址、ID、长度
frame_buffer[0] = ANO_FRAME_HEADER;
frame_buffer[1] = ANO_TO_COMPUTER_ADDR;
frame_buffer[2] = 0x01; // 功能码 ID
frame_buffer[3] = 13; // 数据长度 LEN
// 2. 填充数据内容 (DATA)，注意小端模式 (低字节在前)
frame_buffer[data_idx++] = (u8)(acc_x & 0xFF);
frame_buffer[data_idx++] = (u8)(acc_x >> 8);
frame_buffer[data_idx++] = (u8)(acc_y & 0xFF);
frame_buffer[data_idx++] = (u8)(acc_y >> 8);
frame_buffer[data_idx++] = (u8)(acc_z & 0xFF);
frame_buffer[data_idx++] = (u8)(acc_z >> 8);
frame_buffer[data_idx++] = (u8)(gyr_x & 0xFF);
frame_buffer[data_idx++] = (u8)(gyr_x >> 8);
frame_buffer[data_idx++] = (u8)(gyr_y & 0xFF);
frame_buffer[data_idx++] = (u8)(gyr_y >> 8);
frame_buffer[data_idx++] = (u8)(gyr_z & 0xFF);
frame_buffer[data_idx++] = (u8)(gyr_z >> 8);
frame_buffer[data_idx++] = shock_sta;
// 3. 计算并填充校验和
ano_calculate_checksum(frame_buffer);
// 4. 通过串口发送整个数据帧
ano_uart_dev->write(frame_buffer, sizeof(frame_buffer));
#endif
}
/**
* @brief 发送飞控姿态数据 (ID: 0x03)
*
* @param rol
* @param pit
* @param yaw
* @param fusion_sta
*/
void ano_send_attitude_data(float rol, float pit, float yaw, u8 fusion_sta) {
#if TCFG_UART0_ENABLE==0
if (ano_uart_dev == NULL) {
return; // 如果串口未初始化，则不执行任何操作
}
// 帧总长度 = 4(固定头) + 7(数据) + 2(校验) = 13 字节
u8 frame_buffer[13];
u8 data_idx = 4; // DATA区域从索引4开始
// 1. 填充帧头、地址、ID、长度
frame_buffer[0] = ANO_FRAME_HEADER;
frame_buffer[1] = ANO_TO_COMPUTER_ADDR;
frame_buffer[2] = 0x03; // 功能码 ID
frame_buffer[3] = 7; // 数据长度 LEN
// 2. 转换浮点数为整数并填充 (DATA)，注意小端模式
s16 rol_int = (s16)(rol * 100);
s16 pit_int = (s16)(pit * 100);
s16 yaw_int = (s16)(yaw * 100);
frame_buffer[data_idx++] = (u8)(rol_int & 0xFF);
frame_buffer[data_idx++] = (u8)(rol_int >> 8);
frame_buffer[data_idx++] = (u8)(pit_int & 0xFF);
frame_buffer[data_idx++] = (u8)(pit_int >> 8);
frame_buffer[data_idx++] = (u8)(yaw_int & 0xFF);
frame_buffer[data_idx++] = (u8)(yaw_int >> 8);
frame_buffer[data_idx++] = fusion_sta;
// 3. 计算并填充校验和
ano_calculate_checksum(frame_buffer);
// 4. 通过串口发送整个数据帧
ano_uart_dev->write(frame_buffer, sizeof(frame_buffer));
#endif
}

37
apps/earphone/xtell_Sensor/ano/ano_protocol.h
View File

@ -1,37 +0,0 @@
#ifndef __ANO_PROTOCOL_H__
#define __ANO_PROTOCOL_H__
#include "system/includes.h"
/**
* @brief 初始化用于上位机通信的串口
*
* @param baudrate 波特率，例如 115200
* @return 0: 成功, -1: 失败
*/
int ano_protocol_init(u32 baudrate);
/**
* @brief 发送惯性传感器数据 (ID: 0x01)
* @param acc_x X轴加速度
* @param acc_y Y轴加速度
* @param acc_z Z轴加速度
* @param gyr_x X轴陀螺仪
* @param gyr_y Y轴陀螺仪
* @param gyr_z Z轴陀螺仪
* @param shock_sta 震动状态
*/
void ano_send_inertial_data(s16 acc_x, s16 acc_y, s16 acc_z,
s16 gyr_x, s16 gyr_y, s16 gyr_z,
u8 shock_sta);
/**
* @brief 发送飞控姿态数据 (ID: 0x03)
* @param rol 横滚角 (单位: 度)
* @param pit 俯仰角 (单位: 度)
* @param yaw 航向角 (单位: 度)
* @param fusion_sta 融合状态
*/
void ano_send_attitude_data(float rol, float pit, float yaw, u8 fusion_sta);
#endif // __ANO_PROTOCOL_H__

60
apps/earphone/xtell_Sensor/buffer/circle_buffer.c
View File

@ -1,60 +0,0 @@
#include "circle_buffer.h"
#include <string.h>
// 初始化环形缓冲区
void circle_buffer_init(circle_buffer_t *cb, void *buffer, u16 capacity, u16 element_size) {
cb->buffer = (u8 *)buffer;
cb->capacity = capacity;
cb->element_size = element_size;
cb->head = 0;
cb->tail = 0;
cb->size = 0;
}
// 向环形缓冲区写入一个元素
bool circle_buffer_write(circle_buffer_t *cb, const void *element) {
if (circle_buffer_is_full(cb)) {
return false; // 缓冲区已满
}
u8 *dest = cb->buffer + (cb->head * cb->element_size);
memcpy(dest, element, cb->element_size);
cb->head = (cb->head + 1) % cb->capacity;
cb->size++;
return true;
}
// 从环形缓冲区读取一个元素
bool circle_buffer_read(circle_buffer_t *cb, void *element) {
if (circle_buffer_is_empty(cb)) {
return false; // 缓冲区为空
}
u8 *src = cb->buffer + (cb->tail * cb->element_size);
memcpy(element, src, cb->element_size);
cb->tail = (cb->tail + 1) % cb->capacity;
cb->size--;
return true;
}
// 获取已用空间的大小（以元素为单位）
u16 circle_buffer_get_size(circle_buffer_t *cb) {
return cb->size;
}
// 获取剩余空间的大小（以元素为单位）
u16 circle_buffer_get_free_space(circle_buffer_t *cb) {
return cb->capacity - cb->size;
}
// 检查缓冲区是否已满
bool circle_buffer_is_full(circle_buffer_t *cb) {
return cb->size == cb->capacity;
}
// 检查缓冲区是否为空
bool circle_buffer_is_empty(circle_buffer_t *cb) {
return cb->size == 0;
}

70
apps/earphone/xtell_Sensor/buffer/circle_buffer.h
View File

@ -1,70 +0,0 @@
#ifndef CIRCLE_BUFFER_H
#define CIRCLE_BUFFER_H
#include "system/includes.h"
// 定义环形缓冲区的结构体
typedef struct {
u8 *buffer; // 缓冲区指针
u16 capacity; // 缓冲区总容量（以元素为单位）
u16 element_size; // 每个元素的大小（以字节为单位）
u16 head; // 头部指针（写入位置，以元素为单位）
u16 tail; // 尾部指针（读取位置，以元素为单位）
u16 size; // 当前已用大小（以元素为单位）
} circle_buffer_t;
/**
* @brief 初始化环形缓冲区
* @param cb 指向环形缓冲区结构体的指针
* @param buffer 外部提供的缓冲区内存
* @param capacity 缓冲区的总容量（以元素数量为单位）
* @param element_size 每个元素的大小（字节）
*/
void circle_buffer_init(circle_buffer_t *cb, void *buffer, u16 capacity, u16 element_size);
/**
* @brief 向环形缓冲区写入一个元素
* @param cb 指向环形缓冲区结构体的指针
* @param element 要写入的元素的指针
* @return 成功返回true，失败返回false
*/
bool circle_buffer_write(circle_buffer_t *cb, const void *element);
/**
* @brief 从环形缓冲区读取一个元素
* @param cb 指向环形缓冲区结构体的指针
* @param element 用于存放读取元素的缓冲区的指针
* @return 成功返回true，失败返回false
*/
bool circle_buffer_read(circle_buffer_t *cb, void *element);
/**
* @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);
/**
* @brief 检查缓冲区是否已满
* @param cb 指向环形缓冲区结构体的指针
* @return 如果已满返回true，否则返回false
*/
bool circle_buffer_is_full(circle_buffer_t *cb);
/**
* @brief 检查缓冲区是否为空
* @param cb 指向环形缓冲区结构体的指针
* @return 如果为空返回true，否则返回false
*/
bool circle_buffer_is_empty(circle_buffer_t *cb);
#endif // CIRCLE_BUFFER_H

634
apps/earphone/xtell_Sensor/calculate/skiing_tracker.c
View File

@ -1,634 +0,0 @@
/*
*/
#include "skiing_tracker.h"
#include "../sensor/SC7U22.h"
#define G_ACCELERATION 9.81f
#define DEG_TO_RAD (3.14159265f / 180.0f)
#define ENABLE_XLOG 1
#ifdef xlog
#undef xlog
#endif
#if ENABLE_XLOG
#define xlog(format, ...) printf("[XT:%s] " format, __func__, ##__VA_ARGS__)
#else
#define xlog(format, ...) ((void)0)
#endif
// --- 静止检测 ---
//两个判断是否静止的必要条件：动态零速更新(ZUPT)阈值
// 加速方差阈值，提高阈值，让“刹车”更灵敏，以便在波浪式前进等慢速漂移时也能触发零速更新
#define STOP_ACC_VARIANCE_THRESHOLD 0.2f
// 陀螺仪方差阈值
#define STOP_GYR_VARIANCE_THRESHOLD 5.0f
// 静止时候的陀螺仪模长
#define STOP_GYR_MAG_THRESHOLD 15
// --- --- ---
// --- 启动滑雪阈值 ---
// 加速度模长与重力的差值大于此值，认为开始运动；降低阈值，让“油门”更灵敏，以便能捕捉到真实的慢速启动
#define START_ACC_MAG_THRESHOLD 1.0f //0.5、1
// 陀螺仪方差阈值，以允许启动瞬间的正常抖动，但仍能过滤掉混乱的、非滑雪的晃动。
#define START_GYR_VARIANCE_THRESHOLD 15.0f
// --- --- ---
// --- 滑雪过程 ---
//加速度 模长(不含重力），低于此值视为 在做匀速运动
#define SKIING_ACC_MAG_THRESHOLD 0.5f
//陀螺仪 模长，高于此值视为 摔倒了
#define FALLEN_GRY_MAG_THRESHOLD 2000.0f //未确定
// --- --- ---
// --- 原地旋转抖动 ---
// 加速度 方差 阈值。此值比 静止检测 阈值更宽松，
#define WOBBLE_ACC_VARIANCE_THRESHOLD 0.5f
// 加速度 模长 阈值
#define WOBBLE_ACC_MAG_THRESHOLD 1.0f
// 角速度 总模长 大于此值(度/秒)，认为正在进行非滑雪的旋转或摆动
#define ROTATION_GYR_MAG_THRESHOLD 30.0f
// --- --- ---
// --- 滑雪转弯动 ---
// 加速度 方差 阈值，大于此值，滑雪过程可能发生了急转弯
#define WHEEL_ACC_VARIANCE_THRESHOLD 7.0f
// 角速度 总模长 大于此值(度/秒)，认为滑雪过程中进行急转弯
#define WHEEL_GYR_MAG_THRESHOLD 500.0f //
// --- --- ---
// --- 跳跃 ---
// 加速度模长低于此值(g)，认为进入失重状态(IN_AIR)
#define AIRBORNE_ACC_MAG_LOW_THRESHOLD 0.4f
// 加速度模长高于此值(g)，认为发生落地冲击(LANDING)
#define LANDING_ACC_MAG_HIGH_THRESHOLD 3.5f
// 起跳加速度阈值(g)，用于进入TAKING_OFF状态
#define TAKEOFF_ACC_MAG_HIGH_THRESHOLD 1.8f
// 进入空中状态确认计数：需要连续3个采样点加速度低于阈值才判断为起跳
#define AIRBORNE_CONFIRM_COUNT 3
// 落地状态确认计数：加速度恢复到1g附近并持续2个采样点(20ms)则认为已落地
#define GROUNDED_CONFIRM_COUNT 2
// 最大滞空时间(秒)，超过此时间强制认为已落地，防止状态锁死
#define MAX_TIME_IN_AIR 12.5f
// --- --- ---
// --- 用于消除积分漂移的滤波器和阈值 ---
// 高通滤波器系数 (alpha)。alpha 越接近1，滤除低频(直流偏移)的效果越强，但可能滤掉真实的慢速运动。
// alpha = RC / (RC + dt)，参考RC电路而来，fc ≈ (1 - alpha) / (2 * π * dt)
#define HPF_ALPHA 0.999f
//0.995f： 0.08 Hz 的信号
//0.999f： 0.0159 Hz
// --- --- ---
// --- 低通滤波器 ---
// 低通滤波器系数 (alpha)。alpha 越小，滤波效果越强（更平滑），但延迟越大。
// alpha 推荐范围 0.7 ~ 0.95。可以从 0.85 开始尝试。
#define LPF_ALPHA 0.7f
// 加速度死区阈值 (m/s^2)。低于此阈值的加速度被认为是噪声，不参与积分。
// 设得太高会忽略真实的慢速启动，设得太低则无法有效抑制噪声。
//参考：0.2f ~ 0.4f
#define ACC_DEAD_ZONE_THRESHOLD 0.05f
// --- 模拟摩擦力，进行速度衰减 ---
#define SPEED_ATTENUATION 1.0f //暂不模拟
BLE_KS_send_data_t KS_data;
static float quaternion_data[4];
#ifdef XTELL_TEST
debug_t debug1;
debug_t debug2;
#endif
static skiing_tracker_t my_skiing_tracker;
//////////////////////////////////////////////////////////////////////////////////////////////////
//实现
void clear_speed(void){
my_skiing_tracker.state = STATIC;
memset(my_skiing_tracker.velocity, 0, sizeof(my_skiing_tracker.velocity));
my_skiing_tracker.speed = 0;
}
void start_detection(void){
my_skiing_tracker.state = STATIC;
memset(my_skiing_tracker.velocity, 0, sizeof(my_skiing_tracker.velocity));
my_skiing_tracker.distance = 0;
my_skiing_tracker.speed = 0;
}
void stop_detection(void){
my_skiing_tracker.state = STOP_DETECTION;
memset(my_skiing_tracker.velocity, 0, sizeof(my_skiing_tracker.velocity));
my_skiing_tracker.speed = 0;
}
/**
* @brief 初始化滑雪追踪器
*
* @param tracker
*/
void skiing_tracker_init(skiing_tracker_t *tracker)
{
if (!tracker) {
return;
}
// 使用memset一次性清零整个结构体，包括新增的缓冲区
memset(tracker, 0, sizeof(skiing_tracker_t));
tracker->state = STATIC;
}
/**
* @brief 当检测到落地时，计算空中的水平飞行距离并累加到总距离
*/
static void calculate_air_distance(skiing_tracker_t *tracker) {
float horizontal_speed_on_takeoff = sqrtf(
tracker->initial_velocity_on_takeoff[0] * tracker->initial_velocity_on_takeoff[0] +
tracker->initial_velocity_on_takeoff[1] * tracker->initial_velocity_on_takeoff[1]
);
float distance_in_air = horizontal_speed_on_takeoff * tracker->time_in_air;
tracker->distance += distance_in_air;
}
/**
* @brief 使用四元数直接从设备坐标系的加速度中移除重力分量
* @details 这种方法比使用欧拉角更精确、更稳定，且避免了万向节死锁。
* @param acc_device 输入：设备坐标系下的原始加速度 [x, y, z], 单位 m/s^2
* @param q 输入：表示姿态的四元数 [w, x, y, z]
* @param acc_linear_device 输出：设备坐标系下移除重力后的线性加速度 [x, y, z]
*/
void q_remove_gravity_with_quaternion(const float *acc_device, const float *q, float *acc_linear_device)
{
// 从四元数计算重力在设备坐标系下的投影
// G_device = R_transpose * G_world
// G_world = [0, 0, g]
// R_transpose 的第三列即为重力投影方向
float gx = 2.0f * (q[1] * q[3] - q[0] * q[2]);
float gy = 2.0f * (q[0] * q[1] + q[2] * q[3]);
float gz = q[0] * q[0] - q[1] * q[1] - q[2] * q[2] + q[3] * q[3];
// 从原始加速度中减去重力分量
acc_linear_device[0] = acc_device[0] - gx * G_ACCELERATION;
acc_linear_device[1] = acc_device[1] - gy * G_ACCELERATION;
acc_linear_device[2] = acc_device[2] - gz * G_ACCELERATION;
}
/**
* @brief 使用四元数将设备坐标系的线性加速度转换到世界坐标系
* @details 同样，此方法比使用欧拉角更优。
* @param acc_linear_device 输入：设备坐标系下的线性加速度 [x, y, z]
* @param q 输入：表示姿态的四元数 [w, x, y, z]
* @param acc_linear_world 输出：世界坐标系下的线性加速度 [x, y, z]
*/
void q_transform_to_world_with_quaternion(const float *acc_linear_device, const float *q, float *acc_linear_world)
{
// 这是 R_device_to_world * acc_linear_device 的展开形式
acc_linear_world[0] = (1.0f - 2.0f*q[2]*q[2] - 2.0f*q[3]*q[3]) * acc_linear_device[0] +
(2.0f*q[1]*q[2] - 2.0f*q[0]*q[3]) * acc_linear_device[1] +
(2.0f*q[1]*q[3] + 2.0f*q[0]*q[2]) * acc_linear_device[2];
acc_linear_world[1] = (2.0f*q[1]*q[2] + 2.0f*q[0]*q[3]) * acc_linear_device[0] +
(1.0f - 2.0f*q[1]*q[1] - 2.0f*q[3]*q[3]) * acc_linear_device[1] +
(2.0f*q[2]*q[3] - 2.0f*q[0]*q[1]) * acc_linear_device[2];
acc_linear_world[2] = (2.0f*q[1]*q[3] - 2.0f*q[0]*q[2]) * acc_linear_device[0] +
(2.0f*q[2]*q[3] + 2.0f*q[0]*q[1]) * acc_linear_device[1] +
(1.0f - 2.0f*q[1]*q[1] - 2.0f*q[2]*q[2]) * acc_linear_device[2];
// acc_linear_world[2] -= G_ACCELERATION;
}
/**
* @brief 计算缓冲区内三轴数据的方差之和
*
* @param buffer 传进来的三轴数据：陀螺仪/加速度
* @return float 返回方差和
*/
static float calculate_variance(float buffer[VARIANCE_BUFFER_SIZE][3])
{
float mean[3] = {0};
float variance[3] = {0};
// 计算均值
for (int i = 0; i < VARIANCE_BUFFER_SIZE; i++) {
mean[0] += buffer[i][0];
mean[1] += buffer[i][1];
mean[2] += buffer[i][2];
}
mean[0] /= VARIANCE_BUFFER_SIZE;
mean[1] /= VARIANCE_BUFFER_SIZE;
mean[2] /= VARIANCE_BUFFER_SIZE;
// 计算方差
for (int i = 0; i < VARIANCE_BUFFER_SIZE; i++) {
variance[0] += (buffer[i][0] - mean[0]) * (buffer[i][0] - mean[0]);
variance[1] += (buffer[i][1] - mean[1]) * (buffer[i][1] - mean[1]);
variance[2] += (buffer[i][2] - mean[2]) * (buffer[i][2] - mean[2]);
}
variance[0] /= VARIANCE_BUFFER_SIZE;
variance[1] /= VARIANCE_BUFFER_SIZE;
variance[2] /= VARIANCE_BUFFER_SIZE;
// 返回三轴方差之和，作为一个综合的稳定度指标
return variance[0] + variance[1] + variance[2];
}
/**
* @brief 摩擦力模拟，进行速度衰减
*
* @param tracker
*/
void forece_of_friction(skiing_tracker_t *tracker){
// 增加速度衰减，模拟摩擦力
tracker->velocity[0] *= SPEED_ATTENUATION;
tracker->velocity[1] *= SPEED_ATTENUATION;
tracker->velocity[2] = 0; // 垂直速度强制归零
}
/**
* @brief 状态机更新
*
* @param tracker 传入同步修改后传出
* @param acc_device_ms2 三轴加速度，m/s^2
* @param gyr_dps 三轴陀螺仪，dps
*/
static void update_state_machine(skiing_tracker_t *tracker, const float *acc_device_ms2, const float *gyr_dps)
{
// 缓冲区未填满时，不进行状态判断，默认为静止
if (!tracker->buffer_filled) {
tracker->state = STATIC;
return;
}
// --- 计算关键指标 ---
float acc_variance = calculate_variance(tracker->acc_buffer); // 计算加速度方差
float gyr_variance = calculate_variance(tracker->gyr_buffer); // 计算陀螺仪方差
float gyr_magnitude = sqrtf(gyr_dps[0]*gyr_dps[0] + gyr_dps[1]*gyr_dps[1] + gyr_dps[2]*gyr_dps[2]); //dps
float acc_magnitude = sqrtf(acc_device_ms2[0]*acc_device_ms2[0] + acc_device_ms2[1]*acc_device_ms2[1] + acc_device_ms2[2]*acc_device_ms2[2]); //m/s^s
float acc_magnitude_g = acc_magnitude / G_ACCELERATION; // 转换为g单位，用于跳跃判断
#ifdef XTELL_TEST
debug1.acc_variance =acc_variance;
debug1.gyr_variance =gyr_variance;
debug1.gyr_magnitude=gyr_magnitude;
debug1.acc_magnitude=fabsf(acc_magnitude - G_ACCELERATION);
#endif
// --- 状态机逻辑 (核心修改区域) ---
#if 0 //暂时不考虑空中
// 1. 空中/落地状态的后续处理
if (tracker->state == IN_AIR) {
// A. 检测巨大冲击 -> 落地
if (acc_magnitude_g > LANDING_ACC_MAG_HIGH_THRESHOLD) {
tracker->state = LANDING;
// B. 检测超时 -> 强制落地 (安全机制)
} else if (tracker->time_in_air > MAX_TIME_IN_AIR) {
tracker->state = LANDING;
// C. 检测恢复正常重力 (平缓落地)
} else if (acc_magnitude_g > 0.8f && acc_magnitude_g < 1.5f) {
tracker->grounded_entry_counter++;
if (tracker->grounded_entry_counter >= GROUNDED_CONFIRM_COUNT) {
tracker->state = LANDING;
}
} else {
tracker->grounded_entry_counter = 0;
}
return; // 在空中或刚切换到落地，结束本次状态判断
}
// 2. 严格的 "起跳->空中" 状态转换逻辑
// 只有当处于滑行状态时，才去检测起跳意图
if (tracker->state == NO_CONSTANT_SPEED || tracker->state == CONSTANT_SPEED || tracker->state == WHEEL) {
if (acc_magnitude_g > TAKEOFF_ACC_MAG_HIGH_THRESHOLD) {
tracker->state = TAKING_OFF;
tracker->airborne_entry_counter = 0; // 准备检测失重
return;
}
}
// 只有在TAKING_OFF状态下，才去检测是否进入失重
if (tracker->state == TAKING_OFF) {
if (acc_magnitude_g < AIRBORNE_ACC_MAG_LOW_THRESHOLD) {
tracker->airborne_entry_counter++;
if (tracker->airborne_entry_counter >= AIRBORNE_CONFIRM_COUNT) {
memcpy(tracker->initial_velocity_on_takeoff, tracker->velocity, sizeof(tracker->velocity));
tracker->time_in_air = 0;
tracker->state = IN_AIR;
tracker->airborne_entry_counter = 0;
tracker->grounded_entry_counter = 0;
return;
}
} else {
// 如果在起跳冲击后一段时间内没有失重，说明只是一个颠簸，恢复滑行
// 可以加一个小的超时计数器，这里为了简单先直接恢复
tracker->state = NO_CONSTANT_SPEED;
}
return; // 无论是否切换，都结束本次判断
}
#endif
// --- 静止判断 ---
if (acc_variance < STOP_ACC_VARIANCE_THRESHOLD && gyr_variance < STOP_GYR_VARIANCE_THRESHOLD && gyr_magnitude < STOP_GYR_MAG_THRESHOLD) {
tracker->state = STATIC;
return;
}
// --- 地面状态切换逻辑 ---
switch (tracker->state) {
case LANDING:
tracker->state = STATIC;
break;
case STATIC:
// 优先判断是否进入 WOBBLE 状态
// 条件：陀螺仪活动剧烈，但整体加速度变化不大（说明是原地转或晃）
if (gyr_magnitude > ROTATION_GYR_MAG_THRESHOLD && fabsf(acc_magnitude - G_ACCELERATION) < WOBBLE_ACC_MAG_THRESHOLD) {
tracker->state = WOBBLE;
}
// 只有在陀螺仪和加速度都满足“前进”特征时，才启动
else if (gyr_variance > START_GYR_VARIANCE_THRESHOLD && fabsf(acc_magnitude - G_ACCELERATION) > START_ACC_MAG_THRESHOLD) {
tracker->state = NO_CONSTANT_SPEED;
}
break;
case WOBBLE:
// 从 WOBBLE 状态启动的条件应该和从 STATIC 启动一样严格
if (gyr_variance < START_GYR_VARIANCE_THRESHOLD * 2 && fabsf(acc_magnitude - G_ACCELERATION) > START_ACC_MAG_THRESHOLD) {
tracker->state = NO_CONSTANT_SPEED;
}
// 如果陀螺仪活动减弱，则可能恢复静止
else if (gyr_magnitude < ROTATION_GYR_MAG_THRESHOLD * 0.8f) { // 增加迟滞，避免抖动
// 不直接跳回STATIC，而是依赖下一轮的全局静止判断
}
break;
case NO_CONSTANT_SPEED: //非匀速状态
//暂时不考虑摔倒
// if (gyr_magnitude > FALLEN_GRY_MAG_THRESHOLD) {
// tracker->state = FALLEN; //摔倒
// } else
if (gyr_magnitude > WHEEL_GYR_MAG_THRESHOLD && acc_variance > WHEEL_ACC_VARIANCE_THRESHOLD) {
tracker->state = WHEEL; //转弯
} else if (fabsf(acc_magnitude - G_ACCELERATION) < SKIING_ACC_MAG_THRESHOLD) {
tracker->state = CONSTANT_SPEED; //匀速
}
break;
case CONSTANT_SPEED: //匀速状态
if (fabsf(acc_magnitude - G_ACCELERATION) > START_ACC_MAG_THRESHOLD) {
tracker->state = NO_CONSTANT_SPEED;
}
//TODO：可以添加进入转弯或摔倒的判断
break;
case WHEEL:
// 从转弯状态，检查转弯是否结束
// 如果角速度和加速度方差都降下来了，就回到普通滑行状态
if (gyr_magnitude < WHEEL_GYR_MAG_THRESHOLD * 0.8f && acc_variance < WHEEL_ACC_VARIANCE_THRESHOLD * 0.8f) { // 乘以一个滞后系数避免抖动
tracker->state = NO_CONSTANT_SPEED;
}
break;
case FALLEN:
// TODO：回到 STATIC
break;
}
}
/**
* @brief 主更新函数
*
* @param tracker
* @param acc_g 三轴加速度，g
* @param gyr_dps 三轴陀螺仪，dps
* @param angle 欧若拉角
* @param dt 采样时间间隔，会用来积分求速度
*/
void skiing_tracker_update(skiing_tracker_t *tracker, float *acc_g, float *gyr_dps, float *angle, float dt)
{
if (!tracker || !acc_g || !gyr_dps || !angle || dt <= 0) {
return;
}
if(my_skiing_tracker.state == STOP_DETECTION)
return;
// --- 数据预处理和缓冲 ---
float acc_device_ms2[3];
acc_device_ms2[0] = acc_g[0] * G_ACCELERATION;
acc_device_ms2[1] = acc_g[1] * G_ACCELERATION;
acc_device_ms2[2] = acc_g[2] * G_ACCELERATION;
// 将最新数据存入缓冲区
memcpy(tracker->acc_buffer[tracker->buffer_index], acc_device_ms2, sizeof(acc_device_ms2));
memcpy(tracker->gyr_buffer[tracker->buffer_index], gyr_dps, 3 * sizeof(float));
tracker->buffer_index++;
if (tracker->buffer_index >= VARIANCE_BUFFER_SIZE) {
tracker->buffer_index = 0;
tracker->buffer_filled = 1; // 标记缓冲区已满
}
// --- 更新状态机 ---
update_state_machine(tracker, acc_device_ms2, gyr_dps);
// --- 根据状态执行不同的计算逻辑 ---
switch (tracker->state) {
case TAKING_OFF:
tracker->speed = 0.0f;
break;
case IN_AIR:
// 在空中时，只累加滞空时间
tracker->time_in_air += dt;
break;
case LANDING:
// 刚落地，计算空中距离
calculate_air_distance(tracker);
// 清理速度和滤波器状态，为恢复地面追踪做准备
memset(tracker->velocity, 0, sizeof(tracker->velocity));
tracker->speed = 0;
memset(tracker->acc_world_unfiltered_prev, 0, sizeof(tracker->acc_world_unfiltered_prev));
memset(tracker->acc_world_filtered, 0, sizeof(tracker->acc_world_filtered));
memset(tracker->acc_world_lpf, 0, sizeof(tracker->acc_world_lpf)); // 清理新增的LPF状态
break;
case WHEEL:
case NO_CONSTANT_SPEED:
float linear_acc_device[3];
float linear_acc_world[3];
// 在设备坐标系下，移除重力，得到线性加速度
q_remove_gravity_with_quaternion(acc_device_ms2, quaternion_data, linear_acc_device);
// 将设备坐标系下的线性加速度，旋转到世界坐标系
q_transform_to_world_with_quaternion(linear_acc_device, quaternion_data, linear_acc_world);
// 将最终用于积分的加速度存入 tracker 结构体
memcpy(tracker->acc_no_g, linear_acc_world, sizeof(linear_acc_world));
float acc_world_temp[3]; // 临时变量存储当前周期的加速度
for (int i = 0; i < 2; i++) { // 只处理水平方向的 x 和 y 轴
// --- 核心修改：颠倒滤波器顺序为 HPF -> LPF ---
// 1. 高通滤波 (HPF) 先行: 消除因姿态误差导致的重力泄漏（直流偏置）
// HPF的瞬态响应会产生尖峰，这是正常的。
tracker->acc_world_filtered[i] = HPF_ALPHA * (tracker->acc_world_filtered[i] + tracker->acc_no_g[i] - tracker->acc_world_unfiltered_prev[i]);
tracker->acc_world_unfiltered_prev[i] = tracker->acc_no_g[i];
// 2. 低通滤波 (LPF) 殿后: 平滑掉HPF产生的尖峰和传感器自身的高频振动噪声。
// 这里使用 tracker->acc_world_filtered[i] 作为LPF的输入。
tracker->acc_world_lpf[i] = (1.0f - LPF_ALPHA) * tracker->acc_world_filtered[i] + LPF_ALPHA * tracker->acc_world_lpf[i];
// 将最终处理完的加速度值存入临时变量
acc_world_temp[i] = tracker->acc_world_lpf[i];
}
// 计算处理后加速度的水平模长
float acc_horizontal_mag = sqrtf(acc_world_temp[0] * acc_world_temp[0] +
acc_world_temp[1] * acc_world_temp[1]);
#if XTELL_TEST
debug2.acc_magnitude = acc_horizontal_mag;
#endif
// 应用死区，并积分
if (acc_horizontal_mag > ACC_DEAD_ZONE_THRESHOLD) {
tracker->velocity[0] += acc_world_temp[0] * dt;
tracker->velocity[1] += acc_world_temp[1] * dt;
}
// 更新速度和距离
tracker->speed = sqrtf(tracker->velocity[0] * tracker->velocity[0] +
tracker->velocity[1] * tracker->velocity[1]);
tracker->distance += tracker->speed * dt;
break;
case CONSTANT_SPEED:
//保持上次的速度不变。只更新距离
tracker->distance += tracker->speed * dt;
break;
case STATIC:
case WOBBLE:
// 速度清零，抑制漂移
memset(tracker->velocity, 0, sizeof(tracker->velocity));
tracker->speed = 0.0f;
memset(tracker->acc_world_unfiltered_prev, 0, sizeof(tracker->acc_world_unfiltered_prev));
memset(tracker->acc_world_filtered, 0, sizeof(tracker->acc_world_filtered));
memset(tracker->acc_world_lpf, 0, sizeof(tracker->acc_world_lpf)); // 清理新增的LPF状态
#if XTELL_TEST
debug2.acc_magnitude = 0;
#endif
break;
case FALLEN:
// TODO
break;
default:
break;
}
#if 1
float linear_acc_device[3];
float linear_acc_world[3];
float tmp_world_acc[3];
// 在设备坐标系下，移除重力，得到线性加速度
q_remove_gravity_with_quaternion(acc_device_ms2, quaternion_data, linear_acc_device);
// 将设备坐标系下的线性加速度，旋转到世界坐标系
q_transform_to_world_with_quaternion(linear_acc_device, quaternion_data, tmp_world_acc);
float all_world_mag = sqrtf(tmp_world_acc[0] * tmp_world_acc[0] +
tmp_world_acc[1] * tmp_world_acc[1] +
tmp_world_acc[2] * tmp_world_acc[2]);
static int count = 0;
if(count > 100){
xlog("===original(g): x %.2f, y %.2f, z %.2f===\n",acc_g[0],acc_g[1],acc_g[2]);
xlog("===world(m/s^2) no g: x %.2f, y %.2f, z %.2f, all %.2f===\n",tmp_world_acc[0],tmp_world_acc[1],tmp_world_acc[2],all_world_mag); //去掉重力加速度
xlog("===gyr(dps) : x %.2f, y %.2f, z %.2f===\n",gyr_dps[0],gyr_dps[1],gyr_dps[2]); //angle
xlog("===angle : x %.2f, y %.2f, z %.2f,===\n",angle[0],angle[1],angle[2]);
count = 0;
}
count++;
#endif
}
/**
* @brief 滑雪数据计算
*
* @param acc_data_buf 传入的三轴加速度数据
* @param gyr_data_buf 传入的三轴陀螺仪数据
* @param angle_data 传入的欧若拉角数据
* @return 速度cm/s
*/
uint16_t sensor_processing_task(signed short* acc_data_buf, signed short* gyr_data_buf, float* angle_data, float* quaternion) {
static int initialized = 0;
static float acc_data_g[3];
static float gyr_data_dps[3];
if(quaternion != NULL){
memcpy(quaternion_data, quaternion, 4 * sizeof(float));
}
// const float delta_time = DELTA_TIME+0.01f;
// const float delta_time = DELTA_TIME + 0.005f;
const float delta_time = DELTA_TIME;
if (!initialized) {
skiing_tracker_init(&my_skiing_tracker);
initialized = 1;
printf("Skiing Tracker Initialized. Waiting for sensor calibration...\n");
}
#if ACC_RANGE==2
// 加速度 LSB to g
acc_data_g[0] = (float)acc_data_buf[0] / 16384.0f;
acc_data_g[1] = (float)acc_data_buf[1] / 16384.0f;
acc_data_g[2] = (float)acc_data_buf[2] / 16384.0f;
#endif
#if ACC_RANGE==4
// 加速度 LSB to g
acc_data_g[0] = (float)acc_data_buf[0] / 8192.0f;
acc_data_g[1] = (float)acc_data_buf[1] / 8192.0f;
acc_data_g[2] = (float)acc_data_buf[2] / 8192.0f;
#endif
#if ACC_RANGE==8
//±8g 4096
acc_data_g[0] = (float)acc_data_buf[0] / 4096.0f; //ax
acc_data_g[1] = (float)acc_data_buf[1] / 4096.0f; //ay
acc_data_g[2] = (float)acc_data_buf[2] / 4096.0f; //az
#endif
#if ACC_RANGE==16
//±16g 2048
acc_data_g[0] = (float)acc_data_buf[0] / 2048.0f; //ax
acc_data_g[1] = (float)acc_data_buf[1] / 2048.0f; //ay
acc_data_g[2] = (float)acc_data_buf[2] / 2048.0f; //az
#endif
// 陀螺仪 LSB to dps (度/秒)
// ±2000dps量程下，转换系数约为 0.061
gyr_data_dps[0] = (float)gyr_data_buf[0] * 0.061f;
gyr_data_dps[1] = (float)gyr_data_buf[1] * 0.061f;
gyr_data_dps[2] = (float)gyr_data_buf[2] * 0.061f;
skiing_tracker_update(&my_skiing_tracker, acc_data_g, gyr_data_dps, angle_data, delta_time);
return (uint16_t)(my_skiing_tracker.speed * 100);
}

83
apps/earphone/xtell_Sensor/calculate/skiing_tracker.h
View File

@ -1,83 +0,0 @@
#ifndef SKIING_TRACKER_H
#define SKIING_TRACKER_H
#include "../xtell.h"
#include <math.h>
#include <string.h>
// 定义滑雪者可能的状态
typedef enum {
STATIC, // 静止或动态稳定：0
NO_CONSTANT_SPEED, // 正在滑雪，非匀速：1
CONSTANT_SPEED, // 正在滑雪，匀速：2
WOBBLE, // 正在原地旋转：3
WHEEL, // 转弯：4
FALLEN, // 已摔倒：5
TAKING_OFF, // 起跳冲击阶段：6
IN_AIR, // 空中失重阶段：7
LANDING, // 落地冲击阶段：8
STOP_DETECTION, // 停止检测：9
UNKNOWN // 未知状态：10
} skiing_state_t;
#define VARIANCE_BUFFER_SIZE 5 // 用于计算方差的数据窗口大小 (5个样本 @ 100Hz = 50ms)，减小延迟，提高实时性
#define DELTA_TIME 0.01f
// 追踪器数据结构体
typedef struct {
// 公开数据
float velocity[3]; // 当前速度 (x, y, z)，单位: m/s
float distance; // 总滑行距离，单位: m
float speed; // 当前速率 (标量)，单位: m/s
skiing_state_t state; // 当前滑雪状态
// 内部计算使用的私有成员
float acc_no_g[3]; // 去掉重力分量后的加速度
// 用于空中距离计算
float time_in_air; // 滞空时间计时器
float initial_velocity_on_takeoff[3]; // 起跳瞬间的速度向量
int airborne_entry_counter; // 进入空中状态的确认计数器
int grounded_entry_counter; // 落地确认计数器
// --- 内部计算使用的私有成员 ---
// 用于动态零速更新和旋转检测的缓冲区
float acc_buffer[VARIANCE_BUFFER_SIZE][3]; // 加速度数据窗口
float gyr_buffer[VARIANCE_BUFFER_SIZE][3]; // 角速度数据窗口
int buffer_index; // 缓冲区当前索引
int buffer_filled; // 缓冲区是否已填满的标志
// 用于高通滤波器（巴特沃斯一阶滤波器）的私有成员，以消除加速度的直流偏置
float acc_world_filtered[3]; //过滤过的
float acc_world_unfiltered_prev[3]; //上一次没过滤的
float acc_world_lpf[3]; // 经过低通滤波后的世界坐标系加速度
} skiing_tracker_t;
typedef struct{
int acc_KS[3]; //卡尔曼后，LSB转换后的 三轴加速度数据（cm/s^2）
int gyr_KS_dps[3]; //卡尔曼后，LSB to dps 三轴陀螺仪数据
int angle_KS[3]; //卡尔曼后，计算得到的欧若拉角数据
}BLE_KS_send_data_t;
#ifdef XTELL_TEST
typedef struct{
float acc_variance; //三轴加速度方差之和
float gyr_variance; //三轴陀螺仪方差之和
float acc_magnitude; //三轴加速度模长
float gyr_magnitude; //三轴陀螺仪模长
}debug_t;
#endif
/**
* @brief 初始化滑雪追踪器
*
* @param tracker 指向 skiing_tracker_t 结构体的指针
*/
void skiing_tracker_init(skiing_tracker_t *tracker);
uint16_t sensor_processing_task(signed short* acc_data_buf, signed short* gyr_data_buf, float* angle_data, float* quaternion);
#endif // SKIING_TRACKER_H

320
apps/earphone/xtell_Sensor/send_data.c
View File

@ -1,320 +0,0 @@
#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 "./sensor/SC7U22.h"
#include "./buffer/circle_buffer.h"
#include "btstack/avctp_user.h"
#include "calculate/skiing_tracker.h"
#include "xtell.h"
#include "./ano/ano_protocol.h"
#include "./sensor/MMC56.h"
#include "./sensor/BMP280.h"
#include "./sensor/AK8963.h"
///////////////////////////////////////////////////////////////////////////////////////////////////
//宏定义
#define ENABLE_XLOG 1
#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 SENSOR_DATA_BUFFER_SIZE 500 // 定义缓冲区可以存储XXX个sensor_data_t元素
//
///////////////////////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////////////////////
//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);
extern void close_process(u16* pid,char* name);
//END -- 函数定义
//////////////////////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////////////////////
//START -- 变量定义
typedef struct {
// -- 六轴 --
signed short SC7U22_data[6];
// -- 磁力计 --
uint8_t mmc5603nj_buffer[9];
// -- 速度 --
uint16_t speed_cms;
// -- 气压计 --
int adc_P;
int adc_T;
} BLE_send_data_t;
static int count = 0;
// --- 环形缓冲区 ---
static circle_buffer_t BLE_send_buff; // 环形缓冲区管理结构体
BLE_send_data_t BLE_send_data[SENSOR_DATA_BUFFER_SIZE];
//END -- 变量定义
//////////////////////////////////////////////////////////////////////////////////////////////////
/**
* @brief 六轴静态校准
*
*/
void SC7U22_static_calibration(void){
signed short acc_data_buf[3];
signed short gyr_data_buf[3];
float angle[3];
float quaternion_output[3];
static signed short combined_raw_data[6];
static int calibration_done = 0;
char status = 0;
static first_set_flag = 0;
if(first_set_flag == 0){
first_set_flag = 1;
set_SC7U22_Error_Flag(0);
}
SL_SC7U22_RawData_Read(acc_data_buf,gyr_data_buf);
memcpy(&combined_raw_data[0], acc_data_buf, 3 * sizeof(signed short));
memcpy(&combined_raw_data[3], gyr_data_buf, 3 * sizeof(signed short));
status = Q_SL_SC7U22_Angle_Output(1, combined_raw_data, angle,NULL, 0, quaternion_output);
if(status == 1){ //校准完成
extern u16 SC7U22_calibration_id;
extern u8 SC7U22_init;
first_set_flag = 0;
SC7U22_init = 0x11;
close_process(&SC7U22_calibration_id, "SC7U22_calibration");
u8 send2_1[5] = {0xBB,0xBE,0x02,0x00,0x00};
send2_1[4] = SC7U22_init;
send_data_to_ble_client(&send2_1,5);
}
if(count > 100){
count = 0;
char log_buffer[100];
// snprintf( log_buffer, sizeof(log_buffer),"status:%d\n",status);
// send_data_to_ble_client(&log_buffer,strlen(log_buffer));
xlog("status:%d\n", status);
xlog("RawData:AX=%d,AY=%d,AZ=%d,GX=%d,GY=%d,GZ=%d\r\n",combined_raw_data[0],combined_raw_data[1],combined_raw_data[2],combined_raw_data[3],combined_raw_data[4],combined_raw_data[5]);
}
count++;
}
/**
* @brief 开始采集传感器数据和计算速度
*
*/
void start_collect_fuc(void){
// xlog("=======sensor_read_data START\n");
static signed short combined_raw_data[6];
static int initialized = 0;
static int calibration_done = 0;
char status = 0;
BLE_send_data_t BLE_send_data_tmp;
uint8_t mmc5603nj_buffer[9];
signed short acc_data_buf[3];
signed short gyr_data_buf[3];
float angle[3];
float quaternion_output[3];
// -- 读数据 --
SL_SC7U22_RawData_Read(acc_data_buf,gyr_data_buf);
mmc5603nj_read_origin_data(mmc5603nj_buffer);
bmp280_read_originanl_data(&BLE_send_data_tmp.adc_P, &BLE_send_data_tmp.adc_T);
memcpy(&combined_raw_data[0], acc_data_buf, 3 * sizeof(signed short));
memcpy(&combined_raw_data[3], gyr_data_buf, 3 * sizeof(signed short));
// -- 四元数 --
status = Q_SL_SC7U22_Angle_Output(0, combined_raw_data, angle,NULL, 0, quaternion_output);
// -- 速度计算 --
memcpy(acc_data_buf, &combined_raw_data[0], 3 * sizeof(signed short));
memcpy(gyr_data_buf, &combined_raw_data[3], 3 * sizeof(signed short));
uint16_t speed = sensor_processing_task(acc_data_buf,gyr_data_buf,angle, quaternion_output);
// -- 数据包装进结构体 --
memcpy(&BLE_send_data_tmp.SC7U22_data[0], acc_data_buf, 3 * sizeof(signed short));
memcpy(&BLE_send_data_tmp.SC7U22_data[3], gyr_data_buf, 3 * sizeof(signed short));
memcpy(BLE_send_data_tmp.mmc5603nj_buffer, mmc5603nj_buffer, 9);
BLE_send_data_tmp.speed_cms = speed;
// -- 放进缓冲区 --
if(circle_buffer_is_full(&BLE_send_buff) == 0){
circle_buffer_write(&BLE_send_buff, &BLE_send_data_tmp);
}
}
/**
* @brief ble数据发送函数
*
*/
void BLE_send_fuc(void){
BLE_send_data_t data_to_send;
if (circle_buffer_is_empty(&BLE_send_buff) == 0) {
circle_buffer_read(&BLE_send_buff, &data_to_send);
} else {
// 缓冲区为空，直接返回
return;
}
// --- 封装并发送六轴传感器数据 ---
{
// 协议定义: 包头(2) + 长度(1) + 类型(1) + 数据(12) = 16字节
const uint8_t IMU_PACKET_LEN = 16;
const uint8_t IMU_PAYLOAD_LEN = 13; // 类型(1) + 数据(12)
const uint8_t IMU_TYPE = 0x01;
uint8_t imu_packet[IMU_PACKET_LEN];
// 填充包头
imu_packet[0] = 0xBB;
imu_packet[1] = 0xBE;
imu_packet[2] = IMU_PAYLOAD_LEN;
imu_packet[3] = IMU_TYPE;
// 拷贝六轴数据
memcpy(&imu_packet[4], data_to_send.SC7U22_data, sizeof(data_to_send.SC7U22_data));
send_data_to_ble_client(&imu_packet, IMU_PACKET_LEN);
}
// --- 封装并发送磁力计数据 ---
{
// 协议定义: 包头(2) + 长度(1) + 类型(1) + 数据(9) = 13字节
const uint8_t MAG_PACKET_LEN = 13;
const uint8_t MAG_PAYLOAD_LEN = 10; // 类型(1) + 数据(9)
const uint8_t MAG_TYPE = 0x02;
uint8_t mag_packet[MAG_PACKET_LEN];
// 填充包头
mag_packet[0] = 0xBB;
mag_packet[1] = 0xBE;
mag_packet[2] = MAG_PAYLOAD_LEN;
mag_packet[3] = MAG_TYPE;
// 拷贝磁力计数据
memcpy(&mag_packet[4], data_to_send.mmc5603nj_buffer, sizeof(data_to_send.mmc5603nj_buffer));
send_data_to_ble_client(&mag_packet, MAG_PACKET_LEN);
}
// --- 封装并发送压力机计数据 ---
{
// 协议定义: 包头(2) + 长度(1) + 类型(1) + 数据(8) = 12字节
const uint8_t PT_PACKET_LEN = 12;
const uint8_t PT_PAYLOAD_LEN = 9; // 类型(1) + 数据(8)
const uint8_t PT_TYPE = 0x03;
uint8_t pt_packet[PT_PACKET_LEN];
// 填充包头
pt_packet[0] = 0xBB;
pt_packet[1] = 0xBE;
pt_packet[2] = PT_PAYLOAD_LEN;
pt_packet[3] = PT_TYPE;
// 打包压力数据 data_to_send.adc_P (占 pt_packet[4] 到 pt_packet[7])
pt_packet[4] = (uint8_t)(data_to_send.adc_P & 0xFF); // 最低字节 (LSB)
pt_packet[5] = (uint8_t)((data_to_send.adc_P >> 8) & 0xFF);
pt_packet[6] = (uint8_t)((data_to_send.adc_P >> 16) & 0xFF);
pt_packet[7] = (uint8_t)((data_to_send.adc_P >> 24) & 0xFF); // 最高字节 (MSB)
// 打包温度数据 data_to_send.adc_T (占 pt_packet[8] 到 pt_packet[11])
pt_packet[8] = (uint8_t)(data_to_send.adc_T & 0xFF); // 最低字节 (LSB)
pt_packet[9] = (uint8_t)((data_to_send.adc_T >> 8) & 0xFF);
pt_packet[10] = (uint8_t)((data_to_send.adc_T >> 16) & 0xFF);
pt_packet[11] = (uint8_t)((data_to_send.adc_T >> 24) & 0xFF); // 最高字节 (MSB)
send_data_to_ble_client(&pt_packet, PT_PACKET_LEN);
}
// --- 封装并发送速度数据 ---
{
// 协议定义: 包头(2) + 长度(1) + 类型(1) + 数据(2) = 6字节
const uint8_t SPEED_PACKET_LEN = 6;
const uint8_t SPEED_PAYLOAD_LEN = 3; // 类型(1) + 数据(2)
const uint8_t SPEED_TYPE = 0x04;
uint8_t speed_packet[SPEED_PACKET_LEN];
// 填充包头
speed_packet[0] = 0xBB;
speed_packet[1] = 0xBE;
// 填充长度
speed_packet[2] = SPEED_PAYLOAD_LEN;
// 填充类型
speed_packet[3] = SPEED_TYPE;
// 小端模式
speed_packet[4] = (uint8_t)(data_to_send.speed_cms & 0xFF); // 低字节
speed_packet[5] = (uint8_t)((data_to_send.speed_cms >> 8) & 0xFF); // 高字节
send_data_to_ble_client(&speed_packet, SPEED_PACKET_LEN);
}
}
static u8 bmp280_test_id = 0;
void xtell_task_create(void){
#if TCFG_GSENOR_USER_IIC_TYPE
int ret = hw_iic_init(0);
xlog("init iic result:%d\n", ret); //返回0成功
#else
int ret = soft_iic_init(0);
int num_chars_written = snprintf(log_buffer_1, sizeof(log_buffer_1),"init iic: %d\n", ret);
#endif
// MPU9250_Mag_Init();
//iic总线设备扫描
extern void i2c_scanner_probe(void);
i2c_scanner_probe();
xlog("xtell_task_create\n");
circle_buffer_init(&BLE_send_buff, BLE_send_data, SENSOR_DATA_BUFFER_SIZE, sizeof(BLE_send_data_t));
bmp280_init();
extern void bmp280_test(void);
xlog("barometer start measeure\n");
// create_process(&bmp280_test_id,"bmp280_test",NULL, bmp280_test, 100);
float Temp = 0;
float Press = 0;
xlog("test_func\n");
bmp280_read_data(&Temp, &Press);
xlog("Temp:%.2f, Press:%.2f\n",Temp,Press);
}
//////////////////////////////////////////////////////////////////////////////
//test
//
void bmp280_test(void){
}

133
apps/earphone/xtell_Sensor/sensor/AK8963.c
View File

@ -1,133 +0,0 @@
#include "AK8963.h"
#include "math.h"
#include "os/os_api.h"
#include "../xtell.h"
#include "printf.h"
// 用于存放从Fuse ROM读取的磁力计灵敏度校准值
static float mag_asa_x = 1.0f;
static float mag_asa_y = 1.0f;
static float mag_asa_z = 1.0f;
// 磁力计在16-bit分辨率下的转换因子 (单位: uT/LSB)
#define MAG_RAW_TO_UT_FACTOR (4912.0f / 32760.0f)
/**
* @brief 初始化MPU9250的磁力计AK8963
* @return 0: 成功, 1: MPU9250连接失败, 2: AK8963连接失败
*/
u8 MPU9250_Mag_Init(void) {
u8 temp_data[3];
// --- 检查 MPU9250 连接并复位 ---
_gravity_sensor_get_ndata(MPU9250_ADDR_R, MPU9250_WHO_AM_I, temp_data, 1);
if (temp_data[0] != 0x71 && temp_data[0] != 0x73) {
printf("MPU9250 comm failed, read ID: 0x%X\n", temp_data[0]);
return 1;
}
printf("MPU9250 get id:0x%X\n", temp_data[0]);
gravity_sensor_command(MPU9250_ADDR_W, MPU9250_PWR_MGMT_1, 0x80); // 软复位
os_time_dly(10); // 等待复位完成
gravity_sensor_command(MPU9250_ADDR_W, MPU9250_PWR_MGMT_1, 0x01); // 退出睡眠，选择时钟源
os_time_dly(2);
// --- 强制复位 I2C Master 模块并开启旁路 ---
gravity_sensor_command(MPU9250_ADDR_W, MPU9250_USER_CTRL, 0x20);
os_time_dly(1);
gravity_sensor_command(MPU9250_ADDR_W, MPU9250_USER_CTRL, 0x00);
os_time_dly(1);
gravity_sensor_command(MPU9250_ADDR_W, MPU9250_INT_PIN_CFG, 0x02);
os_time_dly(2);
// --- 再次验证 AK8963 连接 ---
_gravity_sensor_get_ndata(AK8963_ADDR_R, AK8963_WIA, temp_data, 1);
if (temp_data[0] != 0x48) {
printf("AK8963 comm failed after final attempt, read ID: 0x%X\n", temp_data[0]);
return 2;
}
printf("AK8963 get id: 0x%X\n", temp_data[0]);
// ------------------ 配置 AK8963 ------------------
// Power-down模式
gravity_sensor_command(AK8963_ADDR_W, AK8963_CNTL1, 0x00);
os_time_dly(1);
// Fuse ROM access模式
gravity_sensor_command(AK8963_ADDR_W, AK8963_CNTL1, 0x0F);
os_time_dly(1);
_gravity_sensor_get_ndata(AK8963_ADDR_R, AK8963_ASAX, temp_data, 3);
// 计算校准系数
mag_asa_x = (float)(temp_data[0] - 128) / 256.0f + 1.0f;
mag_asa_y = (float)(temp_data[1] - 128) / 256.0f + 1.0f;
mag_asa_z = (float)(temp_data[2] - 128) / 256.0f + 1.0f;
// 再次进入Power-down模式
gravity_sensor_command(AK8963_ADDR_W, AK8963_CNTL1, 0x00);
os_time_dly(1);
// 设置工作模式：16-bit分辨率，100Hz连续测量模式 (0x16)
gravity_sensor_command(AK8963_ADDR_W, AK8963_CNTL1, 0x16);
os_time_dly(1);
printf("AK8963 configured successfully.\n");
return 0; // 初始化成功
}
/**
* @brief 读取磁力计的三轴原始数据
* @param mx, my, mz - 用于存放X, Y, Z轴数据的指针 (int16_t类型)
* @return 0: 成功, 1: 数据未就绪, 2: 数据溢出
*/
u8 MPU9250_Read_Mag_Raw(int16_t *mx, int16_t *my, int16_t *mz) {
u8 read_buf[7];
// 检查数据是否准备好 (使用8位读地址)
_gravity_sensor_get_ndata(AK8963_ADDR_R, AK8963_ST1, read_buf, 1);
if (!(read_buf[0] & 0x01)) {
return 1; // 数据未就绪
}
// 连续读取7个字节 (使用8位读地址)
_gravity_sensor_get_ndata(AK8963_ADDR_R, AK8963_HXL, read_buf, 7);
// 检查数据是否溢出
if (read_buf[6] & 0x08) {
return 2; // 数据溢出
}
// 组合数据
*mx = (int16_t)((read_buf[1] << 8) | read_buf[0]);
*my = (int16_t)((read_buf[3] << 8) | read_buf[2]);
*mz = (int16_t)((read_buf[5] << 8) | read_buf[4]);
return 0; // 读取成功
}
/**
* @brief 读取磁力计的三轴数据，并转换为uT(微特斯拉) (此函数内部逻辑不变)
* @param mx, my, mz - 用于存放X, Y, Z轴数据的指针 (float类型)
* @return 0: 成功, 1: 数据未就绪, 2: 数据溢出
*/
u8 MPU9250_Read_Mag_uT(float *mx, float *my, float *mz) {
int16_t raw_mx, raw_my, raw_mz;
u8 status = MPU9250_Read_Mag_Raw(&raw_mx, &raw_my, &raw_mz);
if (status != 0) {
return status;
}
// 应用灵敏度校准，并转换为uT单位
*mx = (float)raw_mx * mag_asa_x * MAG_RAW_TO_UT_FACTOR;
*my = (float)raw_my * mag_asa_y * MAG_RAW_TO_UT_FACTOR;
*mz = (float)raw_mz * mag_asa_z * MAG_RAW_TO_UT_FACTOR;
return 0;
}

46
apps/earphone/xtell_Sensor/sensor/AK8963.h
View File

@ -1,46 +0,0 @@
// mpu9250_mag.h
#ifndef __MPU9250_MAG_H
#define __MPU9250_MAG_H
#include "stdint.h" // 假设你有标准整数类型，u8 对应 uint8_t
#include "gSensor/gSensor_manage.h"
//==================================================================================
// MPU9250 和 AK8963 的 I2C 地址 (已转换为8位格式)
//==================================================================================
// MPU9250的7位地址是 0x68(接地)
#define MPU9250_ADDR_7BIT 0x69
#define MPU9250_ADDR_W (MPU9250_ADDR_7BIT << 1 | 0) // 8位写地址: 0xD0
#define MPU9250_ADDR_R (MPU9250_ADDR_7BIT << 1 | 1) // 8位读地址: 0xD1
// AK8963磁力计的7位地址是 0x0C
#define AK8963_ADDR_7BIT 0x0C
#define AK8963_ADDR_W (AK8963_ADDR_7BIT << 1 | 0) // 8位写地址: 0x18
#define AK8963_ADDR_R (AK8963_ADDR_7BIT << 1 | 1) // 8位读地址: 0x19
//==================================================================================
// MPU9250 相关寄存器 (用于开启旁路模式)
//==================================================================================
#define MPU9250_WHO_AM_I 0x75
#define MPU9250_INT_PIN_CFG 0x37
#define MPU9250_USER_CTRL 0x6A
#define MPU9250_PWR_MGMT_1 0x6B
//==================================================================================
// AK8963 磁力计相关寄存器
//==================================================================================
#define AK8963_WIA 0x00
#define AK8963_ST1 0x02
#define AK8963_HXL 0x03
#define AK8963_ST2 0x09
#define AK8963_CNTL1 0x0A
#define AK8963_ASAX 0x10
u8 MPU9250_Mag_Init(void);
u8 MPU9250_Read_Mag_Raw(int16_t *mx, int16_t *my, int16_t *mz);
u8 MPU9250_Read_Mag_uT(float *mx, float *my, float *mz);
#endif // __MPU9250_MAG_H

211
apps/earphone/xtell_Sensor/sensor/BMP280.c
View File

@ -1,211 +0,0 @@
/*
气压计
*/
#include "BMP280.h"
#include <string.h>
#include "os/os_api.h"
#include "gSensor/gSensor_manage.h"
/*==================================================================================*/
/* BMP280 内部定义 */
/*==================================================================================*/
// 存储校准参数的静态全局变量
static uint16_t t1;
static int16_t t2, t3;
static uint16_t p1;
static int16_t p2, p3, p4, p5, p6, p7, p8, p9;
static int32_t t_fine;
/*==================================================================================*/
/* 封装的底层I2C读写函数 */
/*==================================================================================*/
/**
* @brief 写入单个字节到BMP280寄存器
*/
static uint8_t bmp280_write_reg(uint8_t reg, uint8_t data) {
gravity_sensor_command(BMP_IIC_WRITE_ADDRESS, reg, data);
return 0;
}
/**
* @brief 从BMP280读取多个字节
*/
static uint8_t bmp280_read_regs(uint8_t reg, uint8_t *buf, uint16_t len) {
return _gravity_sensor_get_ndata(BMP_IIC_READ_ADDRESS, reg, buf, len);
}
/*==================================================================================*/
/* 核心算法 */
/*==================================================================================*/
/**
* @brief 温度补偿计算
* @param adc_T - 原始温度数据
* @return 补偿后的温度值 (单位: °C)
*/
static float compensate_temperature(int32_t adc_T) {
float var1, var2, temperature;
var1 = (((float)adc_T) / 16384.0f - ((float)t1) / 1024.0f) * ((float)t2);
var2 = ((((float)adc_T) / 131072.0f - ((float)t1) / 8192.0f) *
(((float)adc_T) / 131072.0f - ((float)t1) / 8192.0f)) *
((float)t3);
t_fine = (int32_t)(var1 + var2);
temperature = (var1 + var2) / 5120.0f;
if (temperature < -40.0f) return -40.0f;
if (temperature > 85.0f) return 85.0f;
return temperature;
}
/**
* @brief 气压补偿计算
* @param adc_P - 原始气压数据
* @return 补偿后的气压值 (单位: Pa)
*/
static float compensate_pressure(int32_t adc_P) {
float var1, var2, pressure;
var1 = ((float)t_fine / 2.0f) - 64000.0f;
var2 = var1 * var1 * ((float)p6) / 32768.0f;
var2 = var2 + var1 * ((float)p5) * 2.0f;
var2 = (var2 / 4.0f) + (((float)p4) * 65536.0f);
var1 = (((float)p3) * var1 * var1 / 524288.0f + ((float)p2) * var1) / 524288.0f;
var1 = (1.0f + var1 / 32768.0f) * ((float)p1);
if (var1 == 0.0f) {
return 0; // 避免除以零
}
pressure = 1048576.0f - (float)adc_P;
pressure = (pressure - (var2 / 4096.0f)) * 6250.0f / var1;
var1 = ((float)p9) * pressure * pressure / 2147483648.0f;
var2 = pressure * ((float)p8) / 32768.0f;
pressure = pressure + (var1 + var2 + ((float)p7)) / 16.0f;
if (pressure < 30000.0f) return 30000.0f;
if (pressure > 110000.0f) return 110000.0f;
return pressure;
}
/*==================================================================================*/
/* 外部接口函数实现 */
/*==================================================================================*/
uint8_t bmp280_init(void) {
uint8_t id;
uint8_t calib_data[24];
// 1. 检查芯片ID
if (bmp280_read_regs(BMP280_REG_ID, &id, 1) == 0) {
printf("bmp280 get id error:%d\n",id );
return 1; // I2C读取失败
}
if (id != 0x58) {
printf("bmp280 check diff:%d\n",id );
return 1; // ID不匹配
}
printf("bmp280 get id:0%X\n",id );
// 2. 软复位
bmp280_write_reg(BMP280_REG_RESET, 0xB6);
os_time_dly(10); // 等待复位完成
// 3. 一次性读取所有校准参数
if (bmp280_read_regs(BMP280_REG_CALIB_START, calib_data, 24) == 0) {
return 2; // 读取校准数据失败
}
// 4. 解析校准参数
t1 = (uint16_t)(((uint16_t)calib_data[1] << 8) | calib_data[0]);
t2 = (int16_t)(((int16_t)calib_data[3] << 8) | calib_data[2]);
t3 = (int16_t)(((int16_t)calib_data[5] << 8) | calib_data[4]);
p1 = (uint16_t)(((uint16_t)calib_data[7] << 8) | calib_data[6]);
p2 = (int16_t)(((int16_t)calib_data[9] << 8) | calib_data[8]);
p3 = (int16_t)(((int16_t)calib_data[11] << 8) | calib_data[10]);
p4 = (int16_t)(((int16_t)calib_data[13] << 8) | calib_data[12]);
p5 = (int16_t)(((int16_t)calib_data[15] << 8) | calib_data[14]);
p6 = (int16_t)(((int16_t)calib_data[17] << 8) | calib_data[16]);
p7 = (int16_t)(((int16_t)calib_data[19] << 8) | calib_data[18]);
p8 = (int16_t)(((int16_t)calib_data[21] << 8) | calib_data[20]);
p9 = (int16_t)(((int16_t)calib_data[23] << 8) | calib_data[22]);
// 5. 配置传感器 (推荐设置: 正常模式，高精度)
// t_standby=0.5ms, filter=16, spi_en=0
uint8_t config_reg = (0 << 5) | (4 << 2) | (0 << 0);
bmp280_write_reg(BMP280_REG_CONFIG, config_reg);
// osrs_t=x2, osrs_p=x16, mode=normal
uint8_t ctrl_meas_reg = (2 << 5) | (5 << 2) | (3 << 0);
bmp280_write_reg(BMP280_REG_CTRL_MEAS, ctrl_meas_reg);
os_time_dly(10); // 等待配置生效
printf("bmp280 init success\n");
return 0; // 初始化成功
}
/**
* @brief 获取转换后的温度和压力数据
*
* @param temperature 传出，温度
* @param pressure 传出，压力
* @return uint8_t
*/
uint8_t bmp280_read_data(float *temperature, float *pressure) {
uint8_t data[6];
int32_t adc_P, adc_T;
// printf("==========debug1===========\n");
// 一次性读取6个字节的温度和气压原始数据
if (bmp280_read_regs(BMP280_REG_PRESS_MSB, data, 6) == 0) {
printf("bmp280:read data error\n");
return 1; // 读取失败
}
// printf("==========debug2===========\n");
// 组合原始数据 (20位)
adc_P = (int32_t)((((uint32_t)(data[0])) << 12) | (((uint32_t)(data[1])) << 4) | (((uint32_t)(data[2])) >> 4));
adc_T = (int32_t)((((uint32_t)(data[3])) << 12) | (((uint32_t)(data[4])) << 4) | (((uint32_t)(data[5])) >> 4));
// 如果没有数据，直接返回错误 (ADC读数为0x80000是未测量状态)
if (adc_T == 0x80000 || adc_P == 0x80000) {
*temperature = 0.0f;
*pressure = 0.0f;
printf("bmp280:no data\n");
return 1;
}
// printf("==========debug3===========\n");
// 进行补偿计算
*temperature = compensate_temperature(adc_T);
*pressure = compensate_pressure(adc_P);
return 0; // 成功
}
/**
* @brief 获取该气压计的原始adc数据
*
* @param adc_P 传出，气压
* @param adc_T 传出，温度
*/
void bmp280_read_originanl_data(int* adc_P, int* adc_T){
uint8_t data[6];
// 一次性读取6个字节的温度和气压原始数据
if (bmp280_read_regs(BMP280_REG_PRESS_MSB, data, 6) != 0) {
return; // 读取失败
}
// 组合原始数据 (20位)
adc_P = (int32_t)((((uint32_t)(data[0])) << 12) | (((uint32_t)(data[1])) << 4) | (((uint32_t)(data[2])) >> 4));
adc_T = (int32_t)((((uint32_t)(data[3])) << 12) | (((uint32_t)(data[4])) << 4) | (((uint32_t)(data[5])) >> 4));
}

54
apps/earphone/xtell_Sensor/sensor/BMP280.h
View File

@ -1,54 +0,0 @@
#ifndef BMP280_DRIVER_H
#define BMP280_DRIVER_H
#include <stdint.h>
#define BMP_PULL_UP 0 //外部是否接的上拉
// I2C 从设备地址
#if BMP_PULL_UP == 1 //外部接的高
#define BMP_IIC_7BIT_ADDRESS 0x76 //7位,外部接高为0x77
#define BMP_IIC_WRITE_ADDRESS (BMP_IIC_7BIT_ADDRESS<<1) //8位地址
#define BMP_IIC_READ_ADDRESS (BMP_IIC_WRITE_ADDRESS | 0x01)
#else
#define BMP_IIC_7BIT_ADDRESS 0x77 //7位,外部接低为0x76
#define BMP_IIC_WRITE_ADDRESS (BMP_IIC_7BIT_ADDRESS<<1) //8位地址
#define BMP_IIC_READ_ADDRESS (BMP_IIC_WRITE_ADDRESS | 0x01)
#endif
// BMP280 寄存器地址
#define BMP280_REG_CALIB_START 0x88
#define BMP280_REG_ID 0xD0
#define BMP280_REG_RESET 0xE0
#define BMP280_REG_STATUS 0xF3
#define BMP280_REG_CTRL_MEAS 0xF4
#define BMP280_REG_CONFIG 0xF5
#define BMP280_REG_PRESS_MSB 0xF7
/**
* @brief 初始化BMP280传感器
* @return 0: 成功, 1: 芯片ID错误, 2: 读取校准参数失败
* @note 此函数会完成ID检查、软复位、读取校准参数，并设置传感器为连续测量模式。
*/
uint8_t bmp280_init(void);
/**
* @brief 从BMP280读取温度和气压数据
* @param[out] temperature - 指向浮点数变量的指针，用于存储温度值 (单位: °C)
* @param[out] pressure - 指向浮点数变量的指针，用于存储气压值 (单位: Pa)
* @return 0: 成功, 1: 读取数据失败
*/
uint8_t bmp280_read_data(float *temperature, float *pressure);
/**
* @brief 获取该气压计的原始adc数据
*
* @param adc_P 传出，气压
* @param adc_T 传出，温度
*/
void bmp280_read_originanl_data(int* adc_P, int* adc_T);
#endif // BMP280_DRIVER_H

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

@ -1,283 +0,0 @@
// 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
View File

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

259
apps/earphone/xtell_Sensor/sensor/MMC56.c
View File

@ -1,259 +0,0 @@
#include "MMC56.h"
#include "math.h"
#include "os/os_api.h"
#include "../xtell.h"
#include "gSensor/gSensor_manage.h"
#include "printf.h"
#define CALIBRATION_TIME 20000 //校准持续时间 ms
#define SAMPLE_INTERVAL 100 //校准采样间隔
// 用于跟踪当前是否处于连续测量模式
static uint8_t g_continuous_mode_enabled = 0;
mmc5603nj_cal_data_t cal_data; //校准数据
static void mmc5603nj_write_reg(uint8_t reg, uint8_t data) {
gravity_sensor_command(MMC_IIC_WRITE_ADDRESS, reg, data);
}
static uint32_t mmc5603nj_read_regs(uint8_t reg, uint8_t *buf, uint8_t len) {
return _gravity_sensor_get_ndata(MMC_IIC_READ_ADDRESS, reg, buf, len);
}
// 外部接口函数实现
uint8_t mmc5603nj_get_pid(void) {
uint8_t pid = 0;
mmc5603nj_read_regs(MMC_PID, &pid, 1);
return pid;
}
int mmc5603nj_init(void) {
// ID
if ( mmc5603nj_get_pid() != 0x10) {
printf("MMC5603NJ init failed: wrong Product ID (read: 0x%X)\n", mmc5603nj_get_pid());
// return 0;
}
// 软件复位
mmc5603nj_write_reg(MMC_INCTRL1, 0x80); // SW_RESET bit
os_time_dly(20); // 等待复位完成
// 设置20位分辨率 (BW[1:0] = 11)
// 同时确保所有轴都使能 (X/Y/Z_inhibit = 0)
mmc5603nj_write_reg(MMC_INCTRL1, 0x03);
os_time_dly(1);
// 设置内部控制寄存器2
// CMM_EN = 1 (使能连续模式功能)
// HPOWER = 1 (高功耗模式，更稳定)
mmc5603nj_write_reg(MMC_INCTRL2, 0x90); // 0b10010000
// 设置自动SET/RESET功能
// AUTO_SR_EN = 1
mmc5603nj_write_reg(MMC_INCTRL0, 0x20); // 0b00100000
g_continuous_mode_enabled = 0;
printf("MMC5603NJ initialized successfully.\n");
mmc5603nj_enable_continuous_mode(0x04);
return 1;
}
void mmc5603nj_start_calibration(void){
printf("\n--- Magnetometer Calibration Start ---\n");
printf("Slowly rotate the device in all directions (like drawing a 3D '8')...\n");
printf("Calibration will last for 20 seconds.\n\n");
printf("will start after 5 seconds\n\n");
os_time_dly(500);
// 初始化最大最小值
// 使用一个临时变量来读取数据，避免干扰read函数的正常逻辑
mmc5603nj_mag_data_t temp_mag_data;
// 首次读取以获取初始值
mmc5603nj_read_mag_data(&temp_mag_data); // 首次读取不应用校准
float max_x = temp_mag_data.x;
float min_x = temp_mag_data.x;
float max_y = temp_mag_data.y;
float min_y = temp_mag_data.y;
float max_z = temp_mag_data.z;
float min_z = temp_mag_data.z;
uint32_t start_time = os_time_get(); // 假设os_time_get()返回毫秒级时间戳
int samples = 0;
int over = CALIBRATION_TIME/SAMPLE_INTERVAL;
while (samples <= over) {
// 读取原始磁力计数据
mmc5603nj_read_mag_data(&temp_mag_data);
// 更新最大最小值
if (temp_mag_data.x > max_x) max_x = temp_mag_data.x;
if (temp_mag_data.x < min_x) min_x = temp_mag_data.x;
if (temp_mag_data.y > max_y) max_y = temp_mag_data.y;
if (temp_mag_data.y < min_y) min_y = temp_mag_data.y;
if (temp_mag_data.z > max_z) max_z = temp_mag_data.z;
if (temp_mag_data.z < min_z) min_z = temp_mag_data.z;
samples++;
os_time_dly(SAMPLE_INTERVAL / 10);
}
// 检查数据范围是否合理，防止传感器未动或故障
if ((max_x - min_x < 0.1f) || (max_y - min_y < 0.1f) || (max_z - min_z < 0.1f)) {
printf("\n--- Calibration Failed ---\n");
printf("Device might not have been rotated enough.\n");
printf("X range: %.2f, Y range: %.2f, Z range: %.2f\n", max_x - min_x, max_y - min_y, max_z - min_z);
return;
}
// 计算硬磁偏移 (椭球中心)
cal_data.offset_x = (max_x + min_x) / 2.0f;
cal_data.offset_y = (max_y + min_y) / 2.0f;
cal_data.offset_z = (max_z + min_z) / 2.0f;
printf("\n--- Calibration Complete ---\n");
printf("Collected %d samples.\n", samples);
printf("Offsets (Gauss):\n");
printf(" X: %.4f\n", cal_data.offset_x);
printf(" Y: %.4f\n", cal_data.offset_y);
printf(" Z: %.4f\n", cal_data.offset_z);
printf("Please save these values and apply them in your code.\n\n");
}
void mmc5603nj_enable_continuous_mode(uint8_t rate) {
// 在连续模式下，ODR寄存器必须被设置
mmc5603nj_write_reg(MMC_ODR, rate); //要设置频率
// mmc5603nj_set_data_rate(0x04);
// 启用连续模式 (INCTRL2的CMM_EN位已在init中设置)
// 只需要设置 INCTRL0 的 CMM_FREQ_EN 位
mmc5603nj_write_reg(MMC_INCTRL0, 0xA0); // 0b10100000 (CMM_FREQ_EN=1, AUTO_SR_EN=1)
g_continuous_mode_enabled = 1;
}
void mmc5603nj_disable_continuous_mode(void) {
// 禁用连续模式
mmc5603nj_write_reg(MMC_INCTRL0, 0x20); // 恢复到仅使能 AUTO_SR_EN 的状态
g_continuous_mode_enabled = 0;
}
float mmc5603nj_get_temperature(void) {
uint8_t status = 0;
uint8_t temp_raw = 0;
uint8_t timeout = 20;
// 触发一次温度测量
mmc5603nj_write_reg(MMC_INCTRL0, 0x02); // TAKE_MEAS_T
// 等待测量完成
do {
os_time_dly(10);
mmc5603nj_read_regs(MMC_STATUS1, &status, 1);
timeout--;
} while ((status & 0x80) == 0 && timeout > 0);
if (timeout == 0) {
printf("Error: Temperature measurement timeout!\n");
return -273.15f; // 返回一个绝对零度的错误值
}
mmc5603nj_read_regs(MMC_TOUT, &temp_raw, 1);
return ((float)temp_raw * 0.8f) - 75.0f;
}
void mmc5603nj_read_mag_data(mmc5603nj_mag_data_t *mag_data) {
uint8_t buffer[9];
if (g_continuous_mode_enabled) {
// 连续模式下，只需检查数据是否就绪
uint8_t status = 0;
mmc5603nj_read_regs(MMC_STATUS1, &status, 1);
if ((status & 0x40) == 0) { // Meas_M_done bit
// 数据未就绪，可以选择返回或等待，这里我们直接返回旧数据
return;
}
} else {
// 单次测量模式
uint8_t status = 0;
uint8_t timeout = 20;
// 触发一次带自动SET/RESET的磁场测量
mmc5603nj_write_reg(MMC_INCTRL0, 0x21); // 0b00100001 (TAKE_MEAS_M=1, AUTO_SR_EN=1)
// 等待测量完成
do {
os_time_dly(10);
mmc5603nj_read_regs(MMC_STATUS1, &status, 1);
timeout--;
} while ((status & 0x40) == 0 && timeout > 0);
if (timeout == 0) {
printf("Error: Magnetic measurement timeout!\n");
mag_data->x = mag_data->y = mag_data->z = 0.0f;
return;
}
}
// 读取9个字节的原始数据
mmc5603nj_read_regs(MMC_XOUT0, buffer, 9);
// 解析数据 (20位分辨率)
int32_t raw_x = ((uint32_t)buffer[0] << 12) | ((uint32_t)buffer[1] << 4) | ((uint32_t)buffer[6] & 0x0F);
int32_t raw_y = ((uint32_t)buffer[2] << 12) | ((uint32_t)buffer[3] << 4) | ((uint32_t)buffer[6] >> 4);
int32_t raw_z = ((uint32_t)buffer[4] << 12) | ((uint32_t)buffer[5] << 4) | ((uint32_t)buffer[8] & 0x0F);
// 应用偏置和灵敏度进行转换
mag_data->x = ((float)raw_x - 524288.0f) / 16384.0f;
mag_data->y = ((float)raw_y - 524288.0f) / 16384.0f;
mag_data->z = ((float)raw_z - 524288.0f) / 16384.0f;
//减去偏移
mag_data->x -= cal_data.offset_x;
mag_data->y -= cal_data.offset_y;
mag_data->z -= cal_data.offset_z;
}
void mmc5603nj_read_origin_data(uint8_t *buffer) {
if (g_continuous_mode_enabled) {
// 连续模式下，只需检查数据是否就绪
uint8_t status = 0;
mmc5603nj_read_regs(MMC_STATUS1, &status, 1);
if ((status & 0x40) == 0) { // Meas_M_done bit
// 数据未就绪，可以选择返回或等待，这里我们直接返回旧数据
return;
}
} else {
// 单次测量模式
uint8_t status = 0;
uint8_t timeout = 20;
// 触发一次带自动SET/RESET的磁场测量
mmc5603nj_write_reg(MMC_INCTRL0, 0x21); // 0b00100001 (TAKE_MEAS_M=1, AUTO_SR_EN=1)
// 等待测量完成
do {
os_time_dly(10);
mmc5603nj_read_regs(MMC_STATUS1, &status, 1);
timeout--;
} while ((status & 0x40) == 0 && timeout > 0);
if (timeout == 0) {
printf("Error: Magnetic measurement timeout!\n");
return;
}
}
// 读取9个字节的原始数据
mmc5603nj_read_regs(MMC_XOUT0, buffer, 9);
}

103
apps/earphone/xtell_Sensor/sensor/MMC56.h
View File

@ -1,103 +0,0 @@
#ifndef MMC5603NJ_DRIVER_H
#define MMC5603NJ_DRIVER_H
#include <stdint.h>
//该芯片的iic地址是固定的, 没法通过外部上下拉来改变
#define BMP_IIC_7BIT_ADDRESS 0x30 //0110000 手册第12页
//8位地址:
#define MMC_IIC_WRITE_ADDRESS (BMP_IIC_7BIT_ADDRESS <<1) // 0x60 : 01100000
#define MMC_IIC_READ_ADDRESS (MMC_IIC_WRITE_ADDRESS | 0x01) // 0x61 : 01100001
// 寄存器地址定义 -- 数据手册第6页
#define MMC_XOUT0 0x00
#define MMC_XOUT1 0x01
#define MMC_YOUT0 0x02
#define MMC_YOUT1 0x03
#define MMC_ZOUT0 0x04
#define MMC_ZOUT1 0x05
#define MMC_XOUT2 0x06
#define MMC_YOUT2 0x07
#define MMC_ZOUT2 0x08
#define MMC_TOUT 0x09
#define MMC_STATUS1 0x18
#define MMC_ODR 0x1A
#define MMC_INCTRL0 0x1B
#define MMC_INCTRL1 0x1C
#define MMC_INCTRL2 0x1D
#define MMC_ST_X_TH 0x1E
#define MMC_ST_Y_TH 0x1F
#define MMC_ST_Z_TH 0x20
#define MMC_ST_X 0x27
#define MMC_ST_Y 0x28
#define MMC_ST_Z 0x29
#define MMC_PID 0x39
// 定义一个结构体来存放三轴磁场数据（原始数据）
typedef struct {
float x;
float y;
float z;
} mmc5603nj_original_data_t;
// 定义一个结构体来存放三轴磁场数据（单位：高斯 Gauss）
typedef struct {
float x;
float y;
float z;
} mmc5603nj_mag_data_t;
// 定义一个结构体来存放磁力计的硬磁偏移校准数据
typedef struct {
float offset_x;
float offset_y;
float offset_z;
} mmc5603nj_cal_data_t;
/**
* @brief 初始化MMC5603NJ传感器
* 该函数会对传感器进行软件复位，并检查设备ID。
* @return 0 表示成功, -1 表示失败 (设备ID不匹配).
*/
int mmc5603nj_init(void);
/**
* @brief 设置传感器的数据输出速率 (ODR - Output Data Rate)
* @param rate 速率值，具体含义请参考datasheet ODR寄存器说明。
*/
void mmc5603nj_set_data_rate(uint8_t rate);
/**
* @brief 启用连续测量模式
*/
void mmc5603nj_enable_continuous_mode(uint8_t rate);
/**
* @brief 禁用连续测量模式
*/
void mmc5603nj_disable_continuous_mode(void);
/**
* @brief 获取产品ID
* @return 产品的ID值，对于MMC5603NJ，应为0x10.
*/
uint8_t mmc5603nj_get_pid(void);
/**
* @brief 读取传感器的温度
* @return 温度值 (单位: 摄氏度 °C).
*/
float mmc5603nj_get_temperature(void);
/**
* @brief 读取三轴磁场数据
* 此函数会根据当前是连续模式还是单次模式来读取数据。
* @param mag_data 指向 mmc5603nj_mag_data_t 结构体的指针，用于存放结果。
*/
void mmc5603nj_read_mag_data(mmc5603nj_mag_data_t *mag_data);
#endif // MMC5603NJ_DRIVER_H

1979
apps/earphone/xtell_Sensor/sensor/SC7U22.c
View File

File diff suppressed because it is too large Load Diff

143
apps/earphone/xtell_Sensor/sensor/SC7U22.h
View File

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

927
apps/earphone/xtell_Sensor/sensor/SC7U22_Ma.c
View File

@ -1,927 +0,0 @@
#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

181
apps/earphone/xtell_Sensor/sensor/WF282A.c
View File

@ -1,181 +0,0 @@
/*
气压计 - WF282A
*/
#include "wf282a.h"
#include <math.h>
#include <stdint.h> // 推荐使用标准类型
#include "gSensor/gSensor_manage.h"
/*==================================================================================*/
/* WF282A 内部定义 */
/*==================================================================================*/
// 存储校准系数的静态全局变量
static int16_t c0, c1, c01, c11, c20, c21, c30;
static int32_t c00, c10;
/*==================================================================================*/
/* 封装的底层I2C读写函数 */
/*==================================================================================*/
/**
* @brief 写入单个字节到WF282A寄存器
*/
static void wf282a_write_reg(uint8_t reg, uint8_t data) {
gravity_sensor_command(WF_IIC_WRITE_ADDRESS, reg, data);
}
/**
* @brief 从WF282A读取多个字节
*/
static uint32_t wf282a_read_regs(uint8_t reg, uint8_t *buf, uint8_t len) {
return _gravity_sensor_get_ndata(WF_IIC_READ_ADDRESS, reg, buf, len);
}
/*==================================================================================*/
/* 内部辅助函数 */
/*==================================================================================*/
/**
* @brief 从缓冲区中解析所有校准系数
* @param buf 包含从寄存器0x10开始读取的18个字节的校准数据
*/
static void parse_calibration_data(const uint8_t *buf) {
// c0 (12-bit)
c0 = ((int16_t)buf[0] << 4) | (buf[1] >> 4);
if (c0 & (1 << 11)) c0 |= 0xF000;
// c1 (12-bit)
c1 = (((int16_t)buf[1] & 0x0F) << 8) | buf[2];
if (c1 & (1 << 11)) c1 |= 0xF000;
// c00 (20-bit)
c00 = ((int32_t)buf[3] << 12) | ((int32_t)buf[4] << 4) | (buf[5] >> 4);
if (c00 & (1 << 19)) c00 |= 0xFFF00000;
// c10 (20-bit)
c10 = (((int32_t)buf[5] & 0x0F) << 16) | ((int32_t)buf[6] << 8) | buf[7];
if (c10 & (1 << 19)) c10 |= 0xFFF00000;
// c01, c11, c20, c21, c30 (16-bit)
c01 = (int16_t)((uint16_t)buf[8] << 8 | buf[9]);
c11 = (int16_t)((uint16_t)buf[10] << 8 | buf[11]);
c20 = (int16_t)((uint16_t)buf[12] << 8 | buf[13]);
c21 = (int16_t)((uint16_t)buf[14] << 8 | buf[15]);
c30 = (int16_t)((uint16_t)buf[16] << 8 | buf[17]);
}
/**
* @brief 获取原始温度值 (ADC)
*/
static int32_t Get_Traw() {
uint8_t buff[3];
int32_t Traw;
// 从 MSB 寄存器 WF_TMP_B2 (0x03) 开始连续读取3个字节
wf282a_read_regs(WF_TMP_B2, buff, 3);
// buff[0] = B2 (MSB), buff[1] = B1, buff[2] = B0 (LSB)
Traw = (int32_t)buff[0] << 16 | (int32_t)buff[1] << 8 | (int32_t)buff[2];
// 24位二进制补码转32位
if (Traw & (1 << 23)) {
Traw |= 0xFF000000;
}
return Traw;
}
/**
* @brief 获取原始气压值 (ADC)
*/
static int32_t Get_Praw() {
uint8_t buff[3];
int32_t Praw;
// 从 MSB 寄存器 WF_PRS_B2 (0x00) 开始连续读取3个字节
wf282a_read_regs(WF_PRS_B2, buff, 3);
// buff[0] = B2 (MSB), buff[1] = B1, buff[2] = B0 (LSB)
Praw = (int32_t)buff[0] << 16 | (int32_t)buff[1] << 8 | (int32_t)buff[2];
// 24位二进制补码转32位
if (Praw & (1 << 23)) {
Praw |= 0xFF000000;
}
return Praw;
}
/*==================================================================================*/
/* 4. 外部接口函数实现 */
/*==================================================================================*/
uint8_t WF_Init() {
uint8_t calib_buf[18];
uint8_t check_cfg;
// 1. 配置传感器工作模式
// 推荐配置：压力8次过采样，温度1次过采样，测量速率16Hz
wf282a_write_reg(WF_PRS_CFG, (PM_RATE_16 << 4) | PM_PRC_8);
wf282a_write_reg(WF_TMP_CFG, (TMP_RATE_16 << 4) | TMP_PRC_1 | TMP_INT_SENSOR);
wf282a_write_reg(WF_MEAS_CFG, 0x07); // 启动连续压力和温度测量
wf282a_write_reg(WF_CFG_REG, 0x00); // 无中断或FIFO移位配置
// 2. 一次性读取所有校准系数 (从0x10到0x21，共18字节)
if (wf282a_read_regs(COEF_C0, calib_buf, 18) != 0) {
return 2; // 读取校准数据失败
}
parse_calibration_data(calib_buf);
// 3. 检查配置是否写入成功
wf282a_read_regs(WF_MEAS_CFG, &check_cfg, 1);
if (check_cfg != 0x07) {
return 1; // 错误
} else {
return 0; // 成功
}
}
void WF_Sleep() {
wf282a_write_reg(WF_MEAS_CFG, 0x00); // 待机模式
}
void WF_Wakeup() {
wf282a_write_reg(WF_MEAS_CFG, 0x07); // 恢复连续测量
}
uint8_t WF_GetID() {
uint8_t id;
wf282a_read_regs(WF_ID_REG, &id, 1);
return id;
}
float WF_Temperature_Calculate() {
float Traw_sc;
int32_t Traw = Get_Traw();
Traw_sc = (float)Traw / KT; // 缩放原始温度值
return (float)c0 * 0.5f + (float)c1 * Traw_sc;
}
float WF_Pressure_Calculate() {
float Traw_sc, Praw_sc, Pcomp;
int32_t Traw = Get_Traw();
int32_t Praw = Get_Praw();
Traw_sc = (float)Traw / KT; // 缩放原始温度值
Praw_sc = (float)Praw / KP; // 缩放原始压力值
// 公式: 手册给出
Pcomp = (float)c00
+ Praw_sc * ((float)c10 + Praw_sc * ((float)c20 + Praw_sc * (float)c30))
+ Traw_sc * (float)c01
+ Traw_sc * Praw_sc * ((float)c11 + Praw_sc * (float)c21);
return Pcomp;
}
float WF_Altitude_Calculate() {
float pressure_pa = WF_Pressure_Calculate();
// 使用标准大气压公式计算海拔
// P = P0 * (1 - L*h / T0)^(g*M / (R*L))
// 简化公式: h = 44330 * (1 - (P/P0)^(1/5.255))
// 1/5.255 ≈ 0.1903
if (pressure_pa <= 0) {
return 0.0f; // 避免无效计算
}
return 44330.0f * (1.0f - powf(pressure_pa / 101325.0f, 0.1902949f));
}

148
apps/earphone/xtell_Sensor/sensor/WF282A.h
View File

@ -1,148 +0,0 @@
#ifndef _WF282A_H_
#define _WF282A_H_
#include <stdint.h> // 使用标准整数类型
// 标定值
#define KT 524288.0f
#define KP 1572864.0f
#define WF_PULL_UP 1 //外部是否接的上拉
// I2C 从设备地址
#if WF_PULL_UP == 1 //外部接的高
#define WF_IIC_7BIT_ADDRESS 0x77 //7位,外部接高为0x77
#define WF_IIC_WRITE_ADDRESS (WF_IIC_7BIT_ADDRESS<<1) //8位地址
#define WF_IIC_READ_ADDRESS (WF_IIC_WRITE_ADDRESS | 0x01)
#else
#define WF_IIC_7BIT_ADDRESS 0x76 //7位,外部接低为0x76
#define WF_IIC_WRITE_ADDRESS (WF_IIC_7BIT_ADDRESS<<1) //8位地址
#define WF_IIC_READ_ADDRESS (WF_IIC_WRITE_ADDRESS | 0x01)
#endif
#define WF_CHIP_ID 0X10
// 寄存器映射
// 压力数据
#define WF_PRS_B2 0x00
#define WF_PRS_B1 0x01
#define WF_PRS_B0 0x02
// 温度数据
#define WF_TMP_B2 0x03
#define WF_TMP_B1 0x04
#define WF_TMP_B0 0x05
// 配置寄存器
#define WF_PRS_CFG 0x06
#define WF_TMP_CFG 0x07
#define WF_MEAS_CFG 0x08
#define WF_CFG_REG 0x09
#define WF_INT_STS 0x0A
#define WF_FIFO_STS 0x0B
#define WF_RESET_REG 0x0C
// ID寄存器
#define WF_ID_REG 0x0D
// 校准系数寄存器
#define COEF_C0 0x10
#define COEF_C0_C1 0x11
#define COEF_C1 0x12
#define COEF_C00_H 0x13
#define COEF_C00_L 0x14
#define COEF_C00_C10 0x15
#define COEF_C10_M 0x16
#define COEF_C10_L 0x17
#define COEF_C01_H 0x18
#define COEF_C01_L 0x19
#define COEF_C11_H 0x1A
#define COEF_C11_L 0x1B
#define COEF_C20_H 0x1C
#define COEF_C20_L 0x1D
#define COEF_C21_H 0x1E
#define COEF_C21_L 0x1F
#define COEF_C30_H 0x20
#define COEF_C30_L 0x21
// --- 配置宏 ---
// 压力配置 (PRS_CFG[6:4]) - 测量速率
#define PM_RATE_1 0x00 // 1 次/秒
#define PM_RATE_2 0x01 // 2 次/秒
#define PM_RATE_4 0x02 // 4 次/秒
#define PM_RATE_8 0x03 // 8 次/秒
#define PM_RATE_16 0x04 // 16 次/秒
#define PM_RATE_32 0x05 // 32 次/秒
#define PM_RATE_64 0x06 // 64 次/秒
#define PM_RATE_128 0x07 // 128 次/秒
// 压力配置 (PRS_CFG[3:0]) - 过采样率
#define PM_PRC_1 0x00 // 1 次 (单次)
#define PM_PRC_2 0x01 // 2 次 (低功耗)
#define PM_PRC_4 0x02 // 4 次
#define PM_PRC_8 0x03 // 8 次 (标准)
#define PM_PRC_16 0x04 // 16 次 (需要移位)
#define PM_PRC_32 0x05 // 32 次 (需要移位)
#define PM_PRC_64 0x06 // 64 次 (高精度, 需要移位)
#define PM_PRC_128 0x07 // 128 次 (需要移位)
// 温度配置 (TMP_CFG[7]) - 传感器源
#define TMP_EXT_SENSOR 0x80 // 使用外部传感器
#define TMP_INT_SENSOR 0x00 // 使用内部传感器
// 温度配置 (TMP_CFG[6:4]) - 测量速率
#define TMP_RATE_1 0x00 // 1 次/秒
#define TMP_RATE_2 0x01 // 2 次/秒
#define TMP_RATE_4 0x02 // 4 次/秒
#define TMP_RATE_8 0x03 // 8 次/秒
#define TMP_RATE_16 0x04 // 16 次/秒
#define TMP_RATE_32 0x05 // 32 次/秒
#define TMP_RATE_64 0x06 // 64 次/秒
#define TMP_RATE_128 0x07 // 128 次/秒
// 温度配置 (TMP_CFG[3:0]) - 过采样率
#define TMP_PRC_1 0x00 // 1 次
#define TMP_PRC_2 0x01 // 2 次
#define TMP_PRC_4 0x02 // 4 次
#define TMP_PRC_8 0x03 // 8 次
#define TMP_PRC_16 0x04 // 16 次
#define TMP_PRC_32 0x05 // 32 次
#define TMP_PRC_64 0x06 // 64 次
#define TMP_PRC_128 0x07 // 128 次
/**
* @brief 初始化WF282A传感器
* @return 0: 成功, 1: 失败
*/
uint8_t WF_Init(void);
/**
* @brief 使传感器进入休眠/待机模式
*/
void WF_Sleep(void);
/**
* @brief 唤醒传感器，开始连续测量
*/
void WF_Wakeup(void);
/**
* @brief 获取传感器芯片ID
* @return 芯片ID (应为 0x10)
*/
uint8_t WF_GetID(void);
/**
* @brief 计算并返回当前海拔高度
* @return 海拔高度 (单位: 米)
*/
float WF_Altitude_Calculate(void);
/**
* @brief 计算并返回补偿后的压力值
* @return 压力 (单位: Pa)
*/
float WF_Pressure_Calculate(void);
/**
* @brief 计算并返回补偿后的温度值
* @return 温度 (单位: °C)
*/
float WF_Temperature_Calculate(void);
#endif // _WF282A_H_

3
apps/earphone/xtell_Sensor/xtell_app_main.c
View File

@ -167,9 +167,6 @@ void xtell_app_main()
extern void xtell_task_create(void);
xtell_task_create();
xlog("==============xtell_app_end================\n");
}

69
apps/earphone/xtell_Sensor/xtell_handler.c
View File

@ -45,11 +45,6 @@
#include "default_event_handler.h"
#include "debug.h"
#include "system/event.h"
#include "./ano/ano_protocol.h"
#include "./sensor/MMC56.h"
#include "./sensor/BMP280.h"
#include "./sensor/AK8963.h"
#include "./calculate/skiing_tracker.h"
///////////////////////////////////////////////////////////////////////////////////////////////////
//宏定义
#define LOG_TAG_CONST EARPHONE
@ -94,6 +89,7 @@ u8 BMP280_init = 0x30; //气压计初始化
u16 SC7U22_calibration_id;
u16 start_collect_fuc_id;
u16 BLE_send_fuc_id;
u16 rfid_fuc_id;
//
///////////////////////////////////////////////////////////////////////////////////////////////////
extern int bt_hci_event_handler(struct bt_event *bt);
@ -214,70 +210,17 @@ void le_user_app_event_handler(struct sys_event* event){
if(event->u.app.buffer[2] == 0x01){ //后面的数据长度 1
switch (event->u.app.buffer[3]){
case 0xff: //测试
extern void i2c_scanner_probe(void);
// i2c_scanner_probe();
extern void rfid_task_fuc(void);
create_process(&rfid_fuc_id,"rfid",NULL,rfid_task_fuc,1000);
break;
default:
break;
}
}else if(event->u.app.buffer[2] == 0x02){ //后面数据长度为2
switch (event->u.app.buffer[3]){ //数据包类型
case 0x00: //数据包类型为：指定传感器初始化
u8 send2_0[5] = {0xBB,0xBE,0x02,0x00,0x00};
if(event->u.app.buffer[4] == 0x01){ //六轴
if (SL_SC7U22_Config() == 0) {
send2_0[4] = 0x00; //初始化失败
SC7U22_init = 0x10;
send_data_to_ble_client(&send2_0,5);
return;
}
create_process(&SC7U22_calibration_id,"SC7U22_calibration",NULL,SC7U22_static_calibration,10);
}else if(event->u.app.buffer[4] == 0x02){ //地磁
if(mmc5603nj_init() == 0){
MMC5603nj_init = 0x20;
send2_0[4] = MMC5603nj_init; //地磁初始化失败
send_data_to_ble_client(&send2_0,5);
return;
}
MMC5603nj_init = 0x21;
send2_0[4] = MMC5603nj_init; //地磁初始化成功
send_data_to_ble_client(&send2_0,5);
}else if(event->u.app.buffer[4] == 0x03){ //气压计初始化
if(bmp280_init() != 0){
//初始化失败
BMP280_init = 0x30;
send2_0[4] = BMP280_init;
send_data_to_ble_client(&send2_0,5);
return;
}
BMP280_init = 0x31;
send2_0[4] = BMP280_init; //气压计初始化成功
send_data_to_ble_client(&send2_0,5);
}
break;
case 0x01: //数据包类型为：获取指定传感器初始化状态
u8 send2_1[5] = {0xBB,0xBE,0x02,0x00,0x00};
if(event->u.app.buffer[4] == 0x01){ //六轴
send2_1[4] = SC7U22_init;
}else if(event->u.app.buffer[4] == 0x02){ //地磁
send2_1[4] = MMC5603nj_init;
}else if(event->u.app.buffer[4] == 0x03){ //气压计
send2_1[4] = BMP280_init;
}
send_data_to_ble_client(&send2_1,5);
break;
case 0x02: //开始/停止滑雪计算
if(event->u.app.buffer[4] == 0x01){ //开始滑雪计算
if(SC7U22_init == 0x10 || MMC5603nj_init == 0x20 || BMP280_init == 0x30){ //传感器未进行初始化
u8 send2_2[5] = {0xBB,0xBE,0x02,0x00,0x00};
send_data_to_ble_client(&send2_2,5);
return;
}
create_process(&start_collect_fuc_id,"start_collect",NULL,start_collect_fuc,10);
create_process(&BLE_send_fuc_id,"BLE_send_fuc",NULL,BLE_send_fuc,1);
}else if(event->u.app.buffer[4] == 0x02){ //停止滑雪计算
close_process(&start_collect_fuc_id,"start_collect");
close_process(&BLE_send_fuc_id,"BLE_send_fuc");
}
case 0x00:
break;
}
}