diff --git a/apps/common/device/gSensor/gSensor_manage.c b/apps/common/device/gSensor/gSensor_manage.c index 7849604..fce5704 100644 --- a/apps/common/device/gSensor/gSensor_manage.c +++ b/apps/common/device/gSensor/gSensor_manage.c @@ -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); // 短暂延时 } diff --git a/apps/earphone/RFID/include/CPU_CARD.h b/apps/earphone/RFID/include/CPU_CARD.h deleted file mode 100644 index eb86941..0000000 --- a/apps/earphone/RFID/include/CPU_CARD.h +++ /dev/null @@ -1,58 +0,0 @@ -#ifndef CPU_CARD_H -#define CPU_CARD_H 1 - -typedef struct -{ - unsigned char SendLength; - unsigned char *pSendBuffer; - unsigned char ReceiveLength; - unsigned char *pReceiveBuffer; - unsigned int Timeout; -}transmission_struct; - -struct ATS_STR -{ - unsigned char Length; - unsigned char Ats_Data[255]; -}; -struct PPS_STR -{ - unsigned char Length; - unsigned char Pps_Data[1]; -}; - -struct CPU_CARD_STR -{ - unsigned char FSCI; - unsigned char FSC; - unsigned char FWI; - unsigned int FWT; - unsigned char SFGI; - unsigned char TA; - unsigned char TB; - unsigned char TC; - unsigned char PCB; - unsigned char WTXM; - struct ATS_STR ATS; - struct PPS_STR PPS; -}; - - -extern struct CPU_CARD_STR CPU_CARD; -extern unsigned char Ats_Process( unsigned char ats_len, unsigned char *ats ); - -extern unsigned char CPU_CARD_EVENT( void ); -extern unsigned char CPU_TPDU( transmission_struct *tpdu ); - -extern unsigned char CPU_Rats( unsigned char *ats_len, unsigned char *ats ); - - -extern unsigned char CPU_NAK( transmission_struct *tpdu ); - - -extern unsigned char CPU_APDU( transmission_struct *apdu ); - - -extern unsigned char CPU_TPDU( transmission_struct *tpdu ); - -#endif diff --git a/apps/earphone/RFID/include/MIFARE.h b/apps/earphone/RFID/include/MIFARE.h deleted file mode 100644 index b0b4b0f..0000000 --- a/apps/earphone/RFID/include/MIFARE.h +++ /dev/null @@ -1,21 +0,0 @@ -#ifndef _MIFARE_H_ -#define _MIFARE_H_ -#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]; -extern unsigned char MIFARE_CARD_EVENT(void); -extern void Mifare_Clear_Crypto(void); -unsigned char Mifare_LoadKey(unsigned char *mifare_key); -extern unsigned char Mifare_Transfer(unsigned char block); -extern unsigned char Mifare_Restore(unsigned char block); -extern unsigned char Mifare_Blockset(unsigned char block,unsigned char *data_buff); -extern unsigned char Mifare_Blockinc(unsigned char block,unsigned char *data_buff); -extern unsigned char Mifare_Blockdec(unsigned char block,unsigned char *data_buff); -extern unsigned char Mifare_Blockwrite(unsigned char block,unsigned char *data_buff); -extern unsigned char Mifare_Blockread(unsigned char block,unsigned char *data_buff); -extern unsigned char Mifare_Auth(unsigned char key_mode,unsigned char sector,unsigned char *mifare_key,unsigned char *card_uid); -#endif diff --git a/apps/earphone/RFID/include/READER.h b/apps/earphone/RFID/include/READER.h deleted file mode 100644 index 9064123..0000000 --- a/apps/earphone/RFID/include/READER.h +++ /dev/null @@ -1,156 +0,0 @@ - -#ifndef _READER_H -#define _READER_H - -// #include "fm15l0xx_ll_gpio.h" - -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} ; - -static const unsigned char RF_CMD_KEYA = 0x60; -static const unsigned char RF_CMD_KEYB = 0x61; - -struct picc_b_struct -{ -unsigned char ATQB[12];//REQBӦ�� -unsigned char PUPI[4]; -unsigned char APPLICATION_DATA[4]; -unsigned char PROTOCOL_INF[3]; -unsigned char CID;//ATTRIBӦ�� -unsigned char Answer_to_HALT[1];//HALTӦ�� -unsigned char SN[8];//����֤SN���� -}; - -extern struct picc_b_struct PICC_B; - -struct picc_a_struct -{ -unsigned char ATQA[2]; -unsigned char CASCADE_LEVEL; -unsigned char UID_Length; -unsigned char UID[15]; -unsigned char BCC[3]; -unsigned char SAK[3]; -}; - -extern struct picc_a_struct PICC_A; - -struct picc_v_struct -{ - unsigned char UID[8]; - unsigned char RESPONSE; - unsigned char BLOCK_DATA[4]; -}; - -extern struct picc_v_struct PICC_V; -struct picc_f_struct -{ - unsigned char UID[8]; -}; -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 //���÷�Χ0~255 - - -//���ղ������� TYPE B -#define GAIN_B 7 //���÷�Χ0~7 -#define HPCF_B 3 //���÷�Χ0~7 -#define AMPLITUDE_B 255 //���÷�Χ0~255������ֵԽ���ز�Խ�� - -#define MODULATION_B 100//���÷�Χ0~255,����ֵԽС������Խ�� - -//���ղ������� TYPE V -#define GAIN_V 7//���÷�Χ0~7 -#define HPCF_V 4//���÷�Χ0~7 -#define AMPLITUDE_V 255 //���÷�Χ0~255������ֵԽ���ز�Խ�� -#define MODULATION_V 10 //���÷�Χ0~255,����ֵԽС������Խ�� - - -//���ղ������� TYPE F -#define GAIN_F 7//���÷�Χ0~7 -#define HPCF_F 4//���÷�Χ0~7 -#define AMPLITUDE_F 255 //���÷�Χ0~255������ֵԽ���ز�Խ�� - -#define MODULATION_F 100//���÷�Χ0~255,����ֵԽС������Խ�� - -#define SPI_GPIO GPIOB - -#define SCK_PIN LL_GPIO_Pin1//PB1 -#define MISO_PIN LL_GPIO_Pin2//PB2 -#define MOSI_PIN LL_GPIO_Pin3//PB3 -#define NSS_PIN LL_GPIO_Pin0//PB0 - -#define SCK_0 0//LL_GPIO_ResetOutputPin( SPI_GPIO, SCK_PIN ) -#define SCK_1 0//LL_GPIO_SetOutputPin( SPI_GPIO, SCK_PIN ) - -#define MOSI_0 0//LL_GPIO_ResetOutputPin( SPI_GPIO, MOSI_PIN ) -#define MOSI_1 0//LL_GPIO_SetOutputPin( SPI_GPIO, MOSI_PIN ) - -#define PD_GPIO GPIOA -#define PD_PIN LL_GPIO_Pin7//PA7 - - -#define NSS_0 0//LL_GPIO_ResetOutputPin( SPI_GPIO, NSS_PIN ) -#define NSS_1 0//LL_GPIO_SetOutputPin( SPI_GPIO, NSS_PIN ) - - -#define PD_0 0//LL_GPIO_ResetOutputPin( PD_GPIO, PD_PIN ) -#define PD_1 0//LL_GPIO_SetOutputPin( PD_GPIO, PD_PIN ) - - - - -extern void DelayMs( uint32_t xms ); -extern void DelayUs( uint32_t xus ); - -void Reader_GPIO_Init( void ); - -// extern unsigned char Reader_Set_HPD( unsigned char mode ); - -extern unsigned char FM176XX_HardReset(void); - -// extern unsigned char Reader_Set_HPD( unsigned char mode ) ; - -// extern unsigned char SetReg(unsigned char address,unsigned char reg_data); - -// extern unsigned char GetReg(unsigned char address,unsigned char *reg_data); - -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); - -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); - -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); - -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); - -extern unsigned char ReaderF_Inventory(struct picc_f_struct *picc_f); - -#endif - diff --git a/apps/earphone/RFID/include/rfid_main.h b/apps/earphone/RFID/include/rfid_main.h deleted file mode 100644 index 3e5fd99..0000000 --- a/apps/earphone/RFID/include/rfid_main.h +++ /dev/null @@ -1,60 +0,0 @@ -#ifndef _RFID_MAIN_H -#define _RFID_MAIN_H - -#define uchar unsigned char -#define uint unsigned int -//typedef unsigned char u8; -//typedef unsigned int u16; -//typedef unsigned long u32; - -#include "system/includes.h" -#define BIT5 0x00000020U -#define BIT6 0x00000040U -#define BIT7 0x00000080U - - -#define BIT0 0x00000001U -#define BIT1 0x00000002U -#define BIT2 0x00000004U -#define BIT3 0x00000008U -#define BIT4 0x00000010U -#define BIT5 0x00000020U - - -void Uart1Send(unsigned char dat); -void Uart1SendString(unsigned char *str); - -void printHex(unsigned char num); - -typedef enum { - FAIL = 0U, - SUCCESS = !FAIL -} ErrorStatus; - -typedef enum { - DISABLE = 0U, - ENABLE = !DISABLE -} FunState; - -typedef enum { - RESET = 0U, - SET = !RESET -} FlagStatus, ITStatus; - - -typedef struct -{ - unsigned char SendLength; - unsigned char *pSendBuffer; - unsigned char ReceiveLength; - unsigned char *pReceiveBuffer; - unsigned int Timeout; -}transmission_struct; - - - - unsigned char GetReg(unsigned char address,unsigned char *reg_data); - unsigned char SetReg(unsigned char address, unsigned char reg_data); -void Delay1ms(); - -#endif \ No newline at end of file diff --git a/apps/earphone/RFID/reader/CPU_CARD.c b/apps/earphone/RFID/reader/CPU_CARD.c deleted file mode 100644 index 2623a5c..0000000 --- a/apps/earphone/RFID/reader/CPU_CARD.c +++ /dev/null @@ -1,584 +0,0 @@ - //2018��2��2�ձ༭��֧��CPU��Ƭָ����ӷ�ʽ���� -#include "../include/READER.h" -#include "../include/CPU_CARD.h" -#include -#include "../include/READER_REG.h" -#include "../include/rfid_main.h" - - - #define CPU_DEBUG 0 - - struct CPU_CARD_STR CPU_CARD; - -#if 0 -unsigned char CPU_CARD_EVENT( void ){ - return FAIL; -} -#else -unsigned char CPU_CARD_EVENT( void ) -{ - unsigned char result; - unsigned char SendBuffer[255]; - unsigned char ReceiveBuffer[255]; - unsigned char 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( DISABLE ); - printf( "-> RATS ERROR!\r\n" ); - return result; - } - - printf( "-> ATS = " ); - for(i = 0;i < CPU_CARD.ATS.Length;i++) - printf("%02X", CPU_CARD.ATS.Ats_Data[i]); - printf( "\r\n" ); - - result = Ats_Process( CPU_CARD.ATS.Length, CPU_CARD.ATS.Ats_Data ); - if ( result != SUCCESS ) - { - SetCW( DISABLE ); - printf( "-> RATS ERROR!\r\n" ); - return result; - } - - memcpy( APDU.pSendBuffer, "\x00\xA4\x00\x00\x02\x3F\x00", 7 ); - APDU.SendLength = 7; - result = CPU_APDU( &APDU ); - if ( result != SUCCESS ) - { - SetCW( DISABLE ); - printf( "-> APDU ERROR!\r\n" ); - return result; - } - printf( "->APDU = " ); - for(i=0;i APDU ERROR!\r\n"); - return result; - } - printf("-> APDU = "); - for(i=0;i APDU ERROR!\r\n"); - return result; - } - printf("-> Response = "); - for(i=0;i APDU ERROR!\r\n"); - return result; - } - printf("-> Response = "); - for(i=0;i APDU ERROR!\r\n"); - return result; - } - printf("-> Response = "); - for(i=0;i APDU ERROR!\r\n" ); - return result; - } - printf( "-> Response = " ); - for(i=0;iSendLength,tpdu->pSendBuffer); - ModifyReg(REG_TXCRCCON, BIT_CRCEN,ENABLE); - ModifyReg(REG_RXCRCCON, BIT_CRCEN,ENABLE); -// SetTimer(tpdu->Timeout); - SetCommand(CMD_TRANSCEIVE); - for(i = 0;i < tpdu->Timeout; i++) - { - DelayMs(1);//���յȴ���ʱ - GetReg(REG_IRQ0,&irq0); - - if(irq0 & BIT_RXIRQ) - { - GetReg( REG_ERROR, &error ); /*Get Error Status */ - 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); - result = SUCCESS; - return result; - } - } - GetReg(REG_IRQ1,&irq1); - -// if(irq1 & BIT_TIMER1IRQ) -// { -// result = FAIL;//δ��timeoutʱ���ڽ��յ����� -// } - - } - result = FAIL;//δ��timeoutʱ���ڽ��յ����� - return result; -} -#endif - - -/****************************************************************/ -/*����: Rats */ -/*����: Request for answer to select */ -/*����: */ -/* */ -/*���: */ -/* ats_len�����յ�ATS���ݳ��� */ -/* *ats�����յ���ATS����ָ�� */ -/* OK: Ӧ����ȷ */ -/* ERROR: Ӧ����� */ -/****************************************************************/ -#if 0 -unsigned char CPU_Rats( unsigned char *ats_len, unsigned char *ats ) -{ - return FAIL; -} -#else -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; /*Start byte */ - tpdu.pSendBuffer[1] = 0x50; /*default = 0x50 */ - tpdu.SendLength = 2; - tpdu.Timeout = 160; - //CPU_CARD.FWT = 160;//Ĭ����ʱʱ�� - result = FM176XX_TPDU( &tpdu ); - if ( result == SUCCESS ) - { - *ats_len = tpdu.ReceiveLength; - memcpy( ats, tpdu.pReceiveBuffer, *ats_len ); - } - return (result); -} -#endif - - -#if 0 -unsigned char Ats_Process( unsigned char ats_len, unsigned char *ats )/* ATS���ݽ��� */ -{ - return FAIL; -} -#else -unsigned char Ats_Process( unsigned char ats_len, unsigned char *ats )/* ATS���ݽ��� */ -{ - unsigned char result, offset; - - if ( (ats_len == ats[0]) || (ats_len > 2) )/*TL */ - { - CPU_CARD.FSCI = ats[1] & 0x0F; - if ( CPU_CARD.FSCI == 0 ) - CPU_CARD.FSC = 13; - if ( CPU_CARD.FSCI == 1 ) - CPU_CARD.FSC = 21; - if ( CPU_CARD.FSCI == 2 ) - CPU_CARD.FSC = 29; - if ( CPU_CARD.FSCI == 3 ) - CPU_CARD.FSC = 37; - if ( CPU_CARD.FSCI == 4 ) - CPU_CARD.FSC = 45; - if ( CPU_CARD.FSCI == 5 ) - CPU_CARD.FSC = 61; - if ( CPU_CARD.FSCI == 6 ) - CPU_CARD.FSC = 93; - if ( CPU_CARD.FSCI == 7 ) - CPU_CARD.FSC = 125; - if ( CPU_CARD.FSCI > 7 ) - CPU_CARD.FSC = 253; - //CPU_CARD.FSC = 13; - printf( "-> CPU_CARD.FSC = %02X\r\n",CPU_CARD.FSC ); - } - else - { - result = FAIL; - return (result); - } - offset = 0; - result = SUCCESS; - if ( ats[1] & BIT5 ) /*TA */ - { - CPU_CARD.TA = ats[2]; - offset++; - } - - if ( ats[1] & BIT6 ) /*TB */ - { - CPU_CARD.TB = ats[2 + offset]; - CPU_CARD.SFGI = CPU_CARD.TB & 0x0F; - CPU_CARD.FWI = (CPU_CARD.TB >> 4) & 0x0F; - printf( "-> CPU_CARD.SFGI = %02X\r\n",CPU_CARD.SFGI ); - printf( "-> CPU_CARD.FWI = %02X\r\n",CPU_CARD.FWI); - if ( CPU_CARD.FWI < 2 ) - CPU_CARD.FWT = 1; - if ( CPU_CARD.FWI == 2 ) - CPU_CARD.FWT = 2; - if ( CPU_CARD.FWI == 3 ) - CPU_CARD.FWT = 3; - if ( CPU_CARD.FWI == 4 ) - CPU_CARD.FWT = 5; - if ( CPU_CARD.FWI == 5 ) - CPU_CARD.FWT = 10; - if ( CPU_CARD.FWI == 6 ) - CPU_CARD.FWT = 20; - if ( CPU_CARD.FWI == 7 ) - CPU_CARD.FWT = 40; - if ( CPU_CARD.FWI == 8 ) - CPU_CARD.FWT = 80; - if ( CPU_CARD.FWI == 9 ) - CPU_CARD.FWT = 160; - if ( CPU_CARD.FWI == 10 ) - CPU_CARD.FWT = 310; - if ( CPU_CARD.FWI == 11 ) - CPU_CARD.FWT = 620; - if ( CPU_CARD.FWI == 12 ) - CPU_CARD.FWT = 1240; - if ( CPU_CARD.FWI == 13 ) - CPU_CARD.FWT = 2480; - if ( CPU_CARD.FWI > 13 ) - CPU_CARD.FWT = 4950; - offset++; - } - - if ( ats[1] & BIT7 )/*TC */ - { - CPU_CARD.TC = ats[2 + offset]; - offset++; - } - CPU_CARD.PCB = 0x02;/*PCB��ʼֵΪ0x02 */ - return (result); -} -#endif - -#if 1 -//xtell 同时把CPU_TPDU 和 CPU_NAk 用空函数替换 SPi正常 -unsigned char CPU_NAK( transmission_struct *tpdu ) /* ��Ƭ����NAK */ -{ - return FAIL; -} -#else -unsigned char CPU_NAK( transmission_struct *tpdu ) /* ��Ƭ����NAK */ -{ - unsigned char result, tpdu_send_buffer[255],tpdu_receive_buffer[255]; - - tpdu->pSendBuffer = tpdu_send_buffer; - tpdu->pReceiveBuffer = tpdu_receive_buffer; - - tpdu->pSendBuffer [0] = 0xB0 | CPU_CARD.PCB; - tpdu->SendLength = 1; - /*printf( "NAK\r\n" );*/ - - result = FM176XX_TPDU( tpdu ); - return (result); -} -#endif - -#if 0 -unsigned char CPU_TPDU( transmission_struct *tpdu ) -{ - return FAIL; -} -#else -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) - { - if(nak_tpdu.ReceiveLength > 0) - { - memcpy( &pcb_byte, nak_tpdu.pReceiveBuffer, 1 ); - if((pcb_byte & 0xF0) == 0xA0)//R(ACK) - { - #if CPU_DEBUG - printf("...pcb_byte = %02X\r\n",pcb_byte); - printf("...CPU_CARD.PCB = %02X\r\n",CPU_CARD.PCB); - #endif - if((pcb_byte & 0x03) != CPU_CARD.PCB) - { - result = FM176XX_TPDU( tpdu ); - } - else - { - tpdu->pSendBuffer[0] = tpdu->pSendBuffer[0] ^ 0x01;// - CPU_CARD.PCB = tpdu->pSendBuffer[0] & 0x03; - result = FM176XX_TPDU( tpdu ); - } - } - } - } - } - else - { - break; - } - } - return (result); -} -#endif - -/****************************************************************/ -/*����: CPU_APDU */ -/*����: �ú���ʵ��ת��APDU�����ʽ���� CPU card reset */ -/*����: */ -/* */ -/*���: */ -/* ats_len�����յ����ݳ��� */ -/* ats�����յ�����ָ�� */ -/* OK: Ӧ����ȷ */ -/* ERROR: Ӧ����� */ -/****************************************************************/ -#if 1 -//xtell 同时把CPU_TPDU 和 CPU_NAk 用空函数替换 SPi正常 -unsigned char CPU_APDU( transmission_struct *apdu ) -{ - return FAIL; -} -#else -unsigned char CPU_APDU( transmission_struct *apdu ) -{ - unsigned char result, pcb_byte, tpdu_send_buffer[255],tpdu_receive_buffer[255], i; - unsigned char 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��������16���鷢�� */ - { - #if CPU_DEBUG - printf("unsent_length = %02X\r\n",unsent_length); - #endif - if ( unsent_length < CPU_CARD.FSC ) - { - /*���������ݳ���С�����֡���ȣ��������д��������� */ - tpdu.pSendBuffer[0] = CPU_CARD.PCB; /*PCB�ֽ�д��TPDU���� */ - memcpy( tpdu.pSendBuffer + 1, apdu->pSendBuffer + apdu->SendLength - unsent_length, unsent_length ); /*APDU����д��TPDU���� */ - tpdu.SendLength = unsent_length + 1; /*���ͳ�������1 */ - #if CPU_DEBUG - printf("--> "); - for(i=0;ipSendBuffer + apdu->SendLength - unsent_length, CPU_CARD.FSC - 1 ); /*APDU����д��TPDU���� */ - - tpdu.SendLength = CPU_CARD.FSC; /*���鷢�ͳ��� */ - #if CPU_DEBUG - printf("..--> "); - for(i=0;ipReceiveBuffer + apdu->ReceiveLength, tpdu.pReceiveBuffer + 1, tpdu.ReceiveLength - 1 ); - apdu->ReceiveLength = apdu->ReceiveLength + tpdu.ReceiveLength - 1; - return (SUCCESS); /*������ݽ������� */ - } - - if ( (pcb_byte == 0x12) || (pcb_byte == 0x13) ) - { - memcpy( apdu->pReceiveBuffer + apdu->ReceiveLength, tpdu.pReceiveBuffer + 1, tpdu.ReceiveLength - 1 ); - apdu->ReceiveLength = apdu->ReceiveLength + tpdu.ReceiveLength - 1; - tpdu.pSendBuffer[0] = ( (pcb_byte & 0x03) | 0xA0) ^ 0x01; /*�յ�����֡������PCB�ֽ� */ - tpdu.SendLength = 1; - #if CPU_DEBUG - printf("...--> = "); - for(i=0;i = "); - for(i=0;i AUTH ERROR!\r\n"); - return result; - } - - printf("-> AUTH SUCCESS!\r\n"); - - for(BLOCK = 0;BLOCK < 3;BLOCK++) - { - BLOCK_NUM = (SECTOR * 4) + BLOCK; - if(BLOCK_NUM == 0) - BLOCK_NUM = 1; - printf("-> SECTOR = %02X\r\n",SECTOR);; - printf("-> BLOCK = %02X\r\n",BLOCK); - printf("-> 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(DISABLE); - printf("-> WRITE BLOCK ERROR!\r\n"); - return result; - } - printf("-> WRITE BLOCK SUCCESS!\r\n"); - - result = Mifare_Blockread(BLOCK_NUM,BLOCK_DATA); - if(result != SUCCESS) - { - SetCW(DISABLE); - printf("-> READ BLOCK ERROR!\r\n"); - return result; - } - - printf("-> READ BLOCK = %02X%02X%02X%02X%02X%02X%02X%02X%02X%02X%02X%02X%02X%02X%02X%02X\r\n",BLOCK_DATA[0],BLOCK_DATA[1],BLOCK_DATA[2],BLOCK_DATA[3],BLOCK_DATA[4],BLOCK_DATA[5],BLOCK_DATA[6],BLOCK_DATA[7],BLOCK_DATA[8],BLOCK_DATA[9],BLOCK_DATA[10],BLOCK_DATA[11],BLOCK_DATA[12],BLOCK_DATA[13],BLOCK_DATA[14],BLOCK_DATA[15]); - } - } - SetCW(DISABLE); - return result; -} - - -unsigned char MIFARE_CARD_EVENT(void) -{ - unsigned char result; - 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(DISABLE); - printf("-> AUTH ERROR!\r\n"); - return result; - } - - printf("-> AUTH SUCCESS!\r\n"); - - for(BLOCK = 0;BLOCK < 3;BLOCK++) - { - BLOCK_NUM = (SECTOR * 4) + BLOCK; - if(BLOCK_NUM == 0) - BLOCK_NUM = 1; - printf("-> SECTOR = %02X\r\n",SECTOR);; - printf("-> BLOCK = %02X\r\n",BLOCK); - printf("-> 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(DISABLE); - printf("-> WRITE BLOCK ERROR!\r\n"); - return result; - } - printf("-> WRITE BLOCK SUCCESS!\r\n"); - - result = Mifare_Blockread(BLOCK_NUM,BLOCK_DATA); - if(result != SUCCESS) - { - SetCW(DISABLE); - printf("-> READ BLOCK ERROR!\r\n"); - return result; - } - - printf("-> READ BLOCK = %02X%02X%02X%02X%02X%02X%02X%02X%02X%02X%02X%02X%02X%02X%02X%02X\r\n",BLOCK_DATA[0],BLOCK_DATA[1],BLOCK_DATA[2],BLOCK_DATA[3],BLOCK_DATA[4],BLOCK_DATA[5],BLOCK_DATA[6],BLOCK_DATA[7],BLOCK_DATA[8],BLOCK_DATA[9],BLOCK_DATA[10],BLOCK_DATA[11],BLOCK_DATA[12],BLOCK_DATA[13],BLOCK_DATA[14],BLOCK_DATA[15]); - } - } - SetCW(DISABLE); - return result; -} - - - unsigned char Mifare_LoadKey(unsigned char *mifare_key) - { - unsigned char reg_data; - SetCommand(CMD_IDLE); - ModifyReg(REG_FIFOCONTROL,BIT_FIFOFLUSH,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); - DelayMs(1); - GetReg(REG_COMMAND,®_data); - if((reg_data & CMD_MASK) == CMD_IDLE) - return SUCCESS; - else - return FAIL; - } - -/*****************************************************************************************/ -/*���ƣ�Mifare_Auth */ -/*���ܣ�Mifare_Auth��Ƭ��֤ */ -/*���룺mode����֤ģʽ��0��key A��֤��1��key B��֤����sector����֤�������ţ�0~15�� */ -/* *mifare_key��6�ֽ���֤��Կ���飻*card_uid��4�ֽڿ�ƬUID���� */ -/*���: */ -/* OK :��֤�ɹ� */ -/* ERROR :��֤ʧ�� */ -/*****************************************************************************************/ - 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,ENABLE); //Clear FIFO - if(key_mode == KEY_A_M1) - { - SetReg(REG_FIFODATA,0x60);//60 keyA M1��ָ֤�� - ModifyReg(REG_RXTXCON,BIT_SHMODE,DISABLE); - } - if(key_mode == KEY_B_M1) - { - SetReg(REG_FIFODATA,0x61);//61 keyB M1��ָ֤�� - ModifyReg(REG_RXTXCON,BIT_SHMODE,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,ENABLE); - ModifyReg(REG_RXCRCCON, BIT_CRCEN,ENABLE); - SetCommand(CMD_AUTHENT); - DelayMs(5); - GetReg(REG_COMMAND,®_data); - if((reg_data & CMD_MASK) == CMD_IDLE) - { - GetReg(REG_STATUS,®_data); - if(reg_data & BIT_CRYPTO1ON)//�жϼ��ܱ�־λ��ȷ����֤��� - return SUCCESS; - } - return FAIL; -} -/*****************************************************************************************/ -/*���ƣ�Mifare_Blockset */ -/*���ܣ�Mifare_Blockset��Ƭ��ֵ���� */ -/*���룺block����ţ�*buff����Ҫ���õ�4�ֽ���ֵ���� */ -/* */ -/*���: */ -/* OK :���óɹ� */ -/* ERROR :����ʧ�� */ -/*****************************************************************************************/ - 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; - } -/*****************************************************************************************/ -/*���ƣ�Mifare_Blockread */ -/*���ܣ�Mifare_Blockread��Ƭ������� */ -/*���룺block����ţ�0x00~0x3F����buff��16�ֽڶ����������� */ -/*���: */ -/* OK :�ɹ� */ -/* ERROR :ʧ�� */ -/*****************************************************************************************/ -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);//30 ����ָ�� - SetReg(REG_FIFODATA,block);//���ַ - ModifyReg(REG_TXCRCCON, BIT_CRCEN,ENABLE); - ModifyReg(REG_RXCRCCON, BIT_CRCEN,ENABLE); - SetCommand(CMD_TRANSCEIVE); - DelayMs(2); - GetReg(REG_FIFOLENGTH,®_data); - if (reg_data != 16) //���յ������ݳ���Ϊ16 - return FAIL; - GetReg(REG_ERROR,®_data); - if(reg_data & 0x07) - return FAIL; - for(i=0;i<16;i++) - { - GetReg (REG_FIFODATA,&data_buff[i]); - } - return SUCCESS; -} -/*****************************************************************************************/ -/*���ƣ�mifare_blockwrite */ -/*���ܣ�Mifare��Ƭд����� */ -/*���룺block����ţ�0x00~0x3F����buff��16�ֽ�д���������� */ -/*���: */ -/* OK :�ɹ� */ -/* ERROR :ʧ�� */ -/*****************************************************************************************/ -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);//A0 д��ָ�� - SetReg(REG_FIFODATA,block);//���ַ - ModifyReg(REG_TXCRCCON, BIT_CRCEN,ENABLE); - ModifyReg(REG_RXCRCCON, BIT_CRCEN,DISABLE); - SetCommand(CMD_TRANSCEIVE); - DelayMs(5); - GetReg(REG_FIFOLENGTH,®_data); - if (reg_data != 1) //���յ������ݳ���Ϊ1 - return FAIL; - GetReg (REG_FIFODATA,®_data); - if(reg_data != 0x0A) - return FAIL; - for(i=0;i<16;i++) - { - SetReg(REG_FIFODATA,data_buff[i]); - } - SetCommand(CMD_TRANSCEIVE); - DelayMs(5); - GetReg(REG_FIFOLENGTH,®_data); - if (reg_data != 1) //���յ������ݳ���Ϊ1 - return FAIL; - GetReg (REG_FIFODATA,®_data); - if(reg_data != 0x0A) - return FAIL; - - return SUCCESS; - -} - -/*****************************************************************************************/ -/*���ƣ� */ -/*���ܣ�Mifare ��Ƭ��ֵ���� */ -/*���룺block����ţ�0x00~0x3F����buff��4�ֽ���ֵ�������� */ -/*���: */ -/* OK :�ɹ� */ -/* ERROR :ʧ�� */ -/*****************************************************************************************/ -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);//C1 ��ֵָ�� - SetReg(REG_FIFODATA,block);//���ַ - ModifyReg(REG_TXCRCCON, BIT_CRCEN,ENABLE); - ModifyReg(REG_RXCRCCON, BIT_CRCEN,DISABLE); - SetCommand(CMD_TRANSCEIVE); - DelayMs(5); - GetReg(REG_FIFOLENGTH,®_data); - if (reg_data != 1) //���յ������ݳ���Ϊ1 - return FAIL; - GetReg (REG_FIFODATA,®_data); - if(reg_data != 0x0A) - return FAIL; - for(i=0;i<4;i++) - { - SetReg(REG_FIFODATA,data_buff[i]); - } - SetCommand(CMD_TRANSCEIVE); - DelayMs(5); - GetReg(REG_FIFOLENGTH,®_data); - if (reg_data != 1) //���յ������ݳ���Ϊ1 - return FAIL; - GetReg (REG_FIFODATA,®_data); - - if(reg_data != 0x0A) - return FAIL; - - return SUCCESS; -} - - - -/*****************************************************************************************/ -/*���ƣ�mifare_blockdec */ -/*���ܣ�Mifare ��Ƭ��ֵ���� */ -/*���룺block����ţ�0x00~0x3F����buff��4�ֽڼ�ֵ�������� */ -/*���: */ -/* OK :�ɹ� */ -/* ERROR :ʧ�� */ -/*****************************************************************************************/ -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);//C0 ��ֵָ�� - SetReg(REG_FIFODATA,block);//���ַ - ModifyReg(REG_TXCRCCON, BIT_CRCEN,ENABLE); - ModifyReg(REG_RXCRCCON, BIT_CRCEN,DISABLE); - SetCommand(CMD_TRANSCEIVE); - DelayMs(5); - GetReg(REG_FIFOLENGTH,®_data); - if (reg_data != 1) //���յ������ݳ���Ϊ1 - return FAIL; - GetReg (REG_FIFODATA,®_data); - if(reg_data != 0x0A) - return FAIL; - for(i=0;i<4;i++) - { - SetReg(REG_FIFODATA,data_buff[i]); - } - SetCommand(CMD_TRANSCEIVE); - DelayMs(5); - GetReg(REG_FIFOLENGTH,®_data); - if (reg_data != 1) //���յ������ݳ���Ϊ1 - return FAIL; - GetReg (REG_FIFODATA,®_data); - if(reg_data != 0x0A) - - return FAIL; - - return SUCCESS; - -} -/*****************************************************************************************/ -/*���ƣ�mifare_transfer */ -/*���ܣ�Mifare ��Ƭtransfer���� */ -/*���룺block����ţ�0x00~0x3F�� */ -/*���: */ -/* OK :�ɹ� */ -/* ERROR :ʧ�� */ -/*****************************************************************************************/ -unsigned char Mifare_Transfer(unsigned char block) -{ -unsigned char reg_data; - SetCommand(CMD_IDLE); - SetReg(REG_TXDATANUM,0x08); - SetReg(REG_FIFODATA,0xC1);//C1 Transferָ�� - SetReg(REG_FIFODATA,block);//���ַ - ModifyReg(REG_TXCRCCON, BIT_CRCEN,ENABLE); - ModifyReg(REG_RXCRCCON, BIT_CRCEN,DISABLE); - SetCommand(CMD_TRANSCEIVE); - DelayMs(5); - GetReg(REG_FIFOLENGTH,®_data); - if (reg_data != 1) //���յ������ݳ���Ϊ1 - return FAIL; - GetReg (REG_FIFODATA,®_data); - if(reg_data != 0x0A) - return FAIL; - - return SUCCESS; - - -} -/*****************************************************************************************/ -/*���ƣ�mifare_restore */ -/*���ܣ�Mifare ��Ƭrestore���� */ -/*���룺block����ţ�0x00~0x3F�� */ -/*���: */ -/* OK :�ɹ� */ -/* ERROR :ʧ�� */ -/*****************************************************************************************/ - -unsigned char Mifare_Restore(unsigned char block) -{ - unsigned char reg_data,i; - SetCommand(CMD_IDLE); - SetReg(REG_TXDATANUM,0x08); - SetReg(REG_FIFODATA,0xC2);//C1 Transferָ�� - SetReg(REG_FIFODATA,block);//���ַ - ModifyReg(REG_TXCRCCON, BIT_CRCEN,ENABLE); - ModifyReg(REG_RXCRCCON, BIT_CRCEN,DISABLE); - SetCommand(CMD_TRANSCEIVE); - DelayMs(5); - GetReg(REG_FIFOLENGTH,®_data); - if (reg_data != 1) //���յ������ݳ���Ϊ1 - return FAIL; - GetReg (REG_FIFODATA,®_data); - if(reg_data != 0x0A) - return FAIL; - for(i=0;i<4;i++) - { - SetReg(REG_FIFODATA,0); - } - SetCommand(CMD_TRANSCEIVE); - DelayMs(5); - GetReg(REG_FIFOLENGTH,®_data); - if (reg_data != 1) //���յ������ݳ���Ϊ1 - return FAIL; - GetReg (REG_FIFODATA,®_data); - if(reg_data != 0x0A) - return FAIL; - return SUCCESS; -} diff --git a/apps/earphone/RFID/reader/NTAG.c b/apps/earphone/RFID/reader/NTAG.c deleted file mode 100644 index 730eac5..0000000 --- a/apps/earphone/RFID/reader/NTAG.c +++ /dev/null @@ -1,76 +0,0 @@ -#include "../include/READER.h" -#include "../include/NTAG.h" -#include -#include "../include/READER_REG.h" -#include "../include/rfid_main.h" - - -unsigned char PAGE_DATA[16]; - -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; - printf("PAGE 8 Write OK\r\n"); - result = Read_Page(8,PAGE_DATA); - printf("PAGE 8 = %02X%02X%02X%02X\r\n",PAGE_DATA[0],PAGE_DATA[1],PAGE_DATA[2],PAGE_DATA[3]); - return result; - -} - -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,ENABLE); - ModifyReg(REG_RXCRCCON, BIT_CRCEN,ENABLE); - SetCommand(CMD_TRANSCEIVE); - DelayMs(5); - GetReg(REG_ERROR,®_data); - if(reg_data & 0x07) - return FAIL; - GetReg(REG_FIFOLENGTH,®_data); - if(reg_data != 16) - return FAIL; - for(i=0;i<16;i++) - { - GetReg(REG_FIFODATA,&page_data[i]); - } - return SUCCESS; - -} - -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,ENABLE); - ModifyReg(REG_RXCRCCON, BIT_CRCEN,DISABLE); - SetCommand(CMD_TRANSCEIVE); - DelayMs(5); - - GetReg(REG_FIFOLENGTH,®_data); - if(reg_data != 1) - return FAIL; - - GetReg(REG_FIFODATA,®_data); - if(reg_data != 0x0A) - return FAIL; - return SUCCESS; - -} - diff --git a/apps/earphone/RFID/reader/READER.c b/apps/earphone/RFID/reader/READER.c deleted file mode 100644 index d477502..0000000 --- a/apps/earphone/RFID/reader/READER.c +++ /dev/null @@ -1,809 +0,0 @@ -//#include "fm15l0xx_ll_spi.h" -#include "../include/READER.h" -#include "../include/READER_REG.h" -#include -#include "../include/rfid_main.h" - - -struct picc_a_struct PICC_A; -struct picc_b_struct PICC_B; -struct picc_v_struct PICC_V; -struct picc_f_struct PICC_F; -void DelayMs( uint32_t xms ); -void DelayUs( uint32_t xus ); -void DelayUs( uint32_t xus ) -{ - int t; - - while ( xus-- ) - { - t = 5; - while ( t-- ) - ; - } -} - - -void DelayMs( uint32_t xms ) -{ - DelayUs( xms * 1000 ); -} - - -// void Reader_GPIO_Init( void ) -// { - /******** 重做GPIO初始化 **********/ - - -// // LL_GPIO_SetPinMode( PD_GPIO, PD_PIN, LL_GPIO_PINxMODE_OUTPUT );//PA7 PD - - -// //LL_GPIO_EnablePinPullUp( SPI_GPIO, SCK_PIN | MOSI_PIN ); // Enable Pullup - -// //xtell注释 -// // LL_GPIO_SetPinMode( SPI_GPIO, SCK_PIN, LL_GPIO_PINxMODE_OUTPUT ); // PB1 Digital function - SPI1 SCK -// // LL_GPIO_SetPinMode( SPI_GPIO, MISO_PIN, LL_GPIO_PINxMODE_INPUT ); // PB2 Digital function - SPI1 MISO1 -// // LL_GPIO_SetPinMode( SPI_GPIO, MOSI_PIN, LL_GPIO_PINxMODE_OUTPUT ); // PB3 Digital function - SPI1 MOSI -// LL_GPIO_SetPinMode(); //xtell -// SCK_0; -// MOSI_0; -// -// LL_GPIO_SetPinMode(); //xtell -// //xtell注释 -// // LL_GPIO_SetPinMode( SPI_GPIO, NSS_PIN, LL_GPIO_PINxMODE_OUTPUT ); // PB0 Digital function - SPI1 NSS1 -// PD_0; -// NSS_1; -// } - -// unsigned char FM176XX_HardReset(void) -// { - - /*************NFC 硬件初始化 重做********************/ - -// unsigned char reg_data; -// NSS_1;//NSS = 1 -// PD_1;//RST = 1 -// DelayMs(1); -// PD_0;//RST = 0 -// DelayMs(1); -// GetReg(REG_COMMAND,®_data); -// if (reg_data != 0x40) -// return FAIL; -// return SUCCESS; -// } - -// unsigned char Reader_Set_HPD( unsigned char mode ) //mode = DISABLE �˳�HPDģʽ ��mode = ENABLE ����HPDģʽ -// { -// if ( mode == ENABLE ) -// { -// DelayMs( 1 ); //��ʱ1ms - -// /******设置一个GPIO输出高*******xtell******/ -// //PD_1; // PD = 1 -// P34 = 1; - -// } -// else -// { -// /******设置一个GPIO输出低*************/ -// //PD_0; //PD = 0 -// P34 =0; -// DelayMs( 1 ); //��ʱ1ms���ȴ�Reader���� -// } -// return (mode); -// } - - - - -//*********************************************** -//�������ƣ�GetReg(unsigned char addr,unsigned char *regdata) -//�������ܣ���ȡ�Ĵ���ֵ -//��ڲ�����addr:Ŀ��Ĵ�����ַ regdata:��ȡ��ֵ -//���ڲ�����unsigned char TRUE����ȡ�ɹ� FALSE:ʧ�� -//*********************************************** - -// 从FM17660 读取数据时，BIT7 置 1 -// unsigned char GetReg(unsigned char address,unsigned char *reg_data) -// { -// unsigned char spi_data,i; -// NSS_0; //NSS = 0; -// spi_data = (address << 1) | 0x01; - -// for(i=0;i<8;i++) -// { -// if(spi_data & 0x80) -// MOSI_1; -// else -// MOSI_0; -// SCK_1; -// spi_data = spi_data<<1; -// SCK_0; -// } - -// MOSI_0; -// *reg_data = 0; -// for(i=0;i<8;i++) -// { -// *reg_data = *reg_data<<1; -// SCK_1; - -// if(LL_GPIO_ReadInputPort(SPI_GPIO)& MISO_PIN) -// *reg_data = *reg_data | 0x01; -// SCK_0; -// } - -// NSS_1; //NSS = 1; -// return SUCCESS; -// } - -//*********************************************** -//�������ƣ�SetReg(unsigned char addr,unsigned char* regdata) -//�������ܣ�д�Ĵ��� -//��ڲ�����addr:Ŀ��Ĵ�����ַ regdata:Ҫд���ֵ -//���ڲ�����unsigned char TRUE��д�ɹ� FALSE:дʧ�� -//*********************************************** -// unsigned char SetReg(unsigned char address,unsigned char reg_data) -// { -// unsigned char spi_data,i; -// NSS_0; //NSS = 0; -// spi_data = (address << 1) & 0xFE; - -// for(i=0;i<8;i++) -// { - -// if(spi_data & 0x80) -// MOSI_1; -// else -// MOSI_0; -// SCK_1; -// spi_data = spi_data<<1; -// SCK_0; -// } -// MOSI_0; -// spi_data = reg_data; -// for(i=0;i<8;i++) -// { -// if(spi_data & 0x80) -// MOSI_1; -// else -// MOSI_0; -// SCK_1; -// spi_data = spi_data<<1; -// SCK_0; -// } -// SCK_0; -// MOSI_0; -// NSS_1; //NSS = 1; - -// return SUCCESS; -// } - -void ModifyReg( unsigned char reg_address, unsigned char mask, unsigned char set ) -{ - unsigned char reg_data; - // Uart1SendString(" ModifyReg begin "); - - GetReg( reg_address, ®_data ); - - if ( set ) - { - reg_data |= mask; - } - else - { - reg_data &= ~mask; - } - - SetReg( reg_address, reg_data ); - return; -} - -unsigned char SetCommand(unsigned char command) -{ - unsigned char result; - result = SetReg(REG_COMMAND,CMD_MASK & command); - return result; -} - -void SetTimer(unsigned int timeout) // -{ - unsigned long prescale = 1; - unsigned long t,fc; - fc = timeout*13560; - 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,(u8)(fc>>8)); - SetReg(REG_T0RELOADLO,(u8)fc); - - SetReg(REG_T1CONTROL, BIT_TSTOP_RX | BIT_TSTART_TX | VALUE_TCLK_T0 ); - SetReg(REG_T1RELOADHI,(u8)(prescale>>8)); - SetReg(REG_T1RELOADLO,(u8)prescale); - } - else - { - SetReg(REG_T1CONTROL, BIT_TSTOP_RX | BIT_TSTART_TX | VALUE_TCLK_1356_MHZ ); - SetReg(REG_T1RELOADHI,(u8)(fc>>8)); - SetReg(REG_T1RELOADLO,(u8)fc); - } - -} - -unsigned char SetCW(unsigned char mode) -{ - unsigned char result; - if(mode == ENABLE) - { - ModifyReg(REG_COMMAND,BIT_MODEMOFF,DISABLE); - ModifyReg(REG_TXMODE,BIT0 | BIT1,ENABLE); - } - else - { - ModifyReg(REG_COMMAND,BIT_MODEMOFF,ENABLE); - ModifyReg(REG_TXMODE,BIT0 | BIT1,DISABLE); - } - DelayMs(5); - return result; -} - -void Clear_FIFO(void) -{ - unsigned char fifolength; - - GetReg(REG_FIFOLENGTH,&fifolength); - if((fifolength) != 0) //FIFO������գ���FLUSH FIFO - { - ModifyReg(REG_FIFOCONTROL,BIT_FIFOFLUSH,ENABLE); - } - return ; -} - -unsigned char LoadProtocol(unsigned char p_rx,unsigned char p_tx) -{ - unsigned char reg_data = 0; - // Uart1SendString(" LoadProtocol begin "); - SetCommand(CMD_IDLE); // - ModifyReg(REG_FIFOCONTROL,BIT_FIFOFLUSH,ENABLE); //Clear FIFO - SetReg(REG_FIFODATA,p_rx);//Rx - SetReg(REG_FIFODATA,p_tx);//Tx - - SetCommand(CMD_LOADPROTOCOL); - DelayMs(2); - GetReg(REG_COMMAND,®_data); - if(reg_data != CMD_IDLE) - return FAIL; - return SUCCESS; -} - -void SetParity(unsigned char state) -{ - // Uart1SendString(" SetParity begin"); - ModifyReg(REG_FRAMECON,BIT_TXPARITYEN|BIT_RXPARITYEN,state); -} - -unsigned char ReaderA_Initial(void) -{ - - // Uart1SendString(" ReaderA_Initial begin"); - LoadProtocol(RX_TYPEA_106,TX_TYPEA_106); - ModifyReg(REG_TXMODE,BIT2,ENABLE);//FORCE 100ask 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(ENABLE); - SetReg(REG_STATUS,0);//���Cry1Onλ - - // Uart1SendString(" ReaderA_Initial end"); - return SUCCESS; -} - - -unsigned char ReaderB_Initial(void) -{ - LoadProtocol(RX_TYPEB_106,TX_TYPEB_106); - - ModifyReg(REG_TXMODE,BIT2,DISABLE);//FORCE 100ask 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; -} - - -unsigned char ReaderV_Initial(void) -{ - LoadProtocol(RX_TYPEV_26,RX_TYPEV_26); - ModifyReg(REG_RXANA,BIT3|BIT2|BIT1|BIT0,DISABLE); - ModifyReg(REG_RXANA,(HPCF_V<<3)|GAIN_V,ENABLE);//39h - SetParity(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; -} - -unsigned char ReaderF_Initial(void) -{ - ModifyReg(REG_MISC, 0x04,ENABLE); - LoadProtocol(RX_FELICA_212,TX_FELICA_212); - SetReg(REG_TXAMP,AMPLITUDE_F); // - SetReg(REG_TXCON,MODULATION_F); - ModifyReg(REG_RXANA,BIT3|BIT2|BIT1|BIT0,DISABLE); - ModifyReg(REG_RXANA,(HPCF_F<<3)|GAIN_F,ENABLE);//39h - SetParity(DISABLE); - SetReg(REG_THNSET,0xFF); - SetReg(REG_THNMIN,0x80); - SetReg(REG_THNADJ,0x08); - ModifyReg(REG_MISC, 0x04,DISABLE); - return SUCCESS; -} - -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,ENABLE); //Clear FIFO - SetReg(REG_FIFODATA,RF_CMD_WUPA); - ModifyReg(REG_TXCRCCON, BIT_CRCEN,DISABLE); - ModifyReg(REG_RXCRCCON, BIT_CRCEN,DISABLE); - SetCommand(CMD_TRANSCEIVE); - DelayMs(2); - GetReg(REG_FIFOLENGTH,®_data); - if(reg_data != 2) - return FAIL; - - GetReg(REG_FIFODATA,&picc_a->ATQA[0]); - GetReg(REG_FIFODATA,&picc_a->ATQA[1]); - - return SUCCESS; -} - -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,ENABLE); //Clear FIFO - SetReg(REG_FIFODATA,RF_CMD_REQA); - ModifyReg(REG_TXCRCCON, BIT_CRCEN,DISABLE); - ModifyReg(REG_RXCRCCON, BIT_CRCEN,DISABLE); - SetCommand(CMD_TRANSCEIVE); - // DelayMs(2); - Delay1ms(); - Delay1ms(); - GetReg(REG_FIFOLENGTH,®_data); - // Uart1SendString("REG_FIFOLENGTH data="); - // printHex(reg_data); - if(reg_data != 2) - return FAIL; - - GetReg(REG_FIFODATA,&picc_a->ATQA[0]); - GetReg(REG_FIFODATA,&picc_a->ATQA[1]); - - return SUCCESS; -} - -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,ENABLE); //Clear FIFO - SetReg(REG_FIFODATA,RF_CMD_ANTICOLL[picc_a->CASCADE_LEVEL]); - SetReg(REG_FIFODATA,0x20); - ModifyReg(REG_TXCRCCON, BIT_CRCEN,DISABLE); - ModifyReg(REG_RXCRCCON, BIT_CRCEN,DISABLE); - SetCommand(CMD_TRANSCEIVE); - DelayMs(2); - GetReg(REG_FIFOLENGTH,®_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; -} - -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,ENABLE); //Clear FIFO - 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,ENABLE); - ModifyReg(REG_RXCRCCON, BIT_CRCEN,ENABLE); - SetCommand(CMD_TRANSCEIVE); - DelayMs(2); - GetReg(REG_ERROR,®_data); - if((reg_data & 0x0F)!=0) - return FAIL; - GetReg(REG_FIFOLENGTH,®_data); - if(reg_data != 1) - return FAIL; - GetReg(REG_FIFODATA,&picc_a->SAK [picc_a->CASCADE_LEVEL]); - return SUCCESS; - -} - - -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) //1��UID - { - cascade_level = 1; - picc_a->UID_Length = 4; - } - if ((picc_a->ATQA[0]&0xC0)==0x40) //2��UID - { - cascade_level = 2; - picc_a->UID_Length = 8; - } - if ((picc_a->ATQA[0]&0xC0)==0x80) //3��UID - { - cascade_level = 3; - picc_a->UID_Length = 12; - } - 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; -} - - -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,ENABLE); //Clear FIFO - SetReg(REG_FIFODATA, 0x05); //APf - SetReg(REG_FIFODATA, 0x00); //AFI (00:for all cards) - SetReg(REG_FIFODATA, 0x08); //PARAM(REQB,Number of slots =0) - ModifyReg(REG_TXCRCCON, BIT_CRCEN,ENABLE); - ModifyReg(REG_RXCRCCON, BIT_CRCEN,ENABLE); - SetCommand(CMD_TRANSCEIVE); - DelayMs(10); - GetReg(REG_ERROR,®_data); - if((reg_data & 0x0F)!=0)//�жϴ����־ - return FAIL; - GetReg(REG_FIFOLENGTH,®_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; - -} - -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,ENABLE); //Clear FIFO - 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,ENABLE); - ModifyReg(REG_RXCRCCON, BIT_CRCEN,ENABLE); - SetCommand(CMD_TRANSCEIVE); - DelayMs(10); - GetReg(REG_ERROR,®_data); - if((reg_data & 0x0F)!=0) - return FAIL; - GetReg(REG_FIFOLENGTH,®_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; -} - -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,ENABLE); //Clear FIFO - 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 BIT0~BIT3 Frame Size 0 = 16, 1 = 24, 2 = 32, 3 = 40, 4 = 48, 5 = 64, 6 = 96, 7 = 128, 8 = 256 - //Param2 BIT4~BIT5 TX BaudRate BIT6~BIT7 RX BaudRate,00 = 106Kbps, 01 = 212Kbps, 10 = 424Kbps, 11 = 848Kbps - SetReg(REG_FIFODATA, 0x01); //COMPATIBLE WITH 14443-4 - SetReg(REG_FIFODATA, 0x01); //CID:01 - ModifyReg(REG_TXCRCCON, BIT_CRCEN,ENABLE); - ModifyReg(REG_RXCRCCON, BIT_CRCEN,ENABLE); - SetCommand(CMD_TRANSCEIVE); - DelayMs(10); - GetReg(REG_ERROR,®_data); - if((reg_data & 0x0F)!=0) - return FAIL; - GetReg(REG_FIFOLENGTH,®_data); - if(reg_data != 1) - return FAIL; - GetReg(REG_FIFODATA,®_data); - picc_b->CID = reg_data & 0x0F; - - return SUCCESS; - -} - -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,ENABLE); //Clear FIFO - 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,ENABLE); - ModifyReg(REG_RXCRCCON, BIT_CRCEN,ENABLE); - SetCommand(CMD_TRANSCEIVE); - DelayMs(10); - GetReg(REG_ERROR,®_data); - if((reg_data & 0x0F)!=0) - return FAIL; - GetReg(REG_FIFOLENGTH,®_data); - if(reg_data != 1) - return FAIL; - GetReg(REG_FIFODATA,®_data); - *picc_b->Answer_to_HALT = reg_data & 0x0F; - - return SUCCESS; - -} - -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,ENABLE); //Clear FIFO - 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,ENABLE); - ModifyReg(REG_RXCRCCON, BIT_CRCEN,ENABLE); - SetCommand(CMD_TRANSCEIVE); - DelayMs(10); - GetReg(REG_ERROR,®_data); - if((reg_data & 0x0F)!=0) - return FAIL; - GetReg(REG_FIFOLENGTH,®_data); - if(reg_data != 10) - return FAIL; - for(i=0;i<8;i++) - GetReg(REG_FIFODATA,&picc_b->SN[i]); - - return SUCCESS; - -} - -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,ENABLE); //Clear FIFO - SetReg(REG_FIFODATA, 0x26); // - SetReg(REG_FIFODATA, 0x01); - SetReg(REG_FIFODATA, 0x00); - - ModifyReg(REG_TXCRCCON, BIT_CRCEN,ENABLE); - ModifyReg(REG_RXCRCCON, BIT_CRCEN,ENABLE); - - SetCommand(CMD_TRANSCEIVE); - DelayMs(10); - GetReg(REG_ERROR,®_data); - if((reg_data & 0x0F)!=0) - return FAIL; - GetReg(REG_FIFOLENGTH,®_data); - if(reg_data != 10) - return FAIL; - GetReg(REG_FIFODATA,&picc_v->RESPONSE ); - GetReg(REG_FIFODATA,®_data); - for(i = 0;i < 8; i++) - { - GetReg(REG_FIFODATA,&picc_v->UID[i]); - } - return SUCCESS; -} - -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,ENABLE); //Clear FIFO - SetReg(REG_FIFODATA, 0x22); - SetReg(REG_FIFODATA, 0x25); // - 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,ENABLE); - ModifyReg(REG_RXCRCCON, BIT_CRCEN,ENABLE); - - SetCommand(CMD_TRANSCEIVE); - DelayMs(10); - GetReg(REG_ERROR,®_data); - if((reg_data & 0x0F)!=0) - return FAIL; - GetReg(REG_FIFOLENGTH,®_data); - if(reg_data != 1) - return FAIL; - GetReg(REG_FIFODATA,&picc_v->RESPONSE ); - return SUCCESS; -} - -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,ENABLE); //Clear FIFO - SetReg(REG_FIFODATA,0x12); - SetReg(REG_FIFODATA, 0x20); // - SetReg(REG_FIFODATA, block_num); - - ModifyReg(REG_TXCRCCON, BIT_CRCEN,ENABLE); - ModifyReg(REG_RXCRCCON, BIT_CRCEN,ENABLE); - - SetCommand(CMD_TRANSCEIVE); - DelayMs(10); - GetReg(REG_ERROR,®_data); - if((reg_data & 0x0F)!=0) - return FAIL; - GetReg(REG_FIFOLENGTH,®_data); - if(reg_data != 5) - return FAIL; - GetReg(REG_FIFODATA,&picc_v->RESPONSE ); - for(i = 0;i < 4; i++) - { - GetReg(REG_FIFODATA,&picc_v->BLOCK_DATA[i]); - } - return SUCCESS; -} - - - -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,ENABLE); //Clear FIFO - SetReg(REG_FIFODATA,0x02); - SetReg(REG_FIFODATA, 0x21); // - 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,ENABLE); - ModifyReg(REG_RXCRCCON, BIT_CRCEN,ENABLE); - - SetCommand(CMD_TRANSCEIVE); - DelayMs(10); - GetReg(REG_ERROR,®_data); - if((reg_data & 0x0F)!=0) - return FAIL; - GetReg(REG_FIFOLENGTH,®_data); - if(reg_data != 1) - return FAIL; - GetReg(REG_FIFODATA,&picc_v->RESPONSE ); - - return SUCCESS; -} - -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,ENABLE); //Clear FIFO - - 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,ENABLE); - ModifyReg(REG_RXCRCCON, BIT_CRCEN,ENABLE); - - SetCommand(CMD_TRANSCEIVE); - DelayMs(10); - GetReg(REG_ERROR,®_data); - if((reg_data & 0x0F)!=0) - return FAIL; - GetReg(REG_FIFOLENGTH,®_data); - if(reg_data != 18) - return FAIL; - GetReg(REG_FIFODATA,®_data); - GetReg(REG_FIFODATA,®_data); - for(i = 0;i < 8; i++) - { - GetReg(REG_FIFODATA,&picc_f->UID[i]); - } - return SUCCESS; - -} - - - - diff --git a/apps/earphone/RFID/reader/rfid_main.c b/apps/earphone/RFID/reader/rfid_main.c deleted file mode 100644 index 9723da7..0000000 --- a/apps/earphone/RFID/reader/rfid_main.c +++ /dev/null @@ -1,360 +0,0 @@ -/******************************************************************************************************** - * @file rfid_main.c - * @brief RFID 读卡器应用层主逻辑文件 - * @details - * 本文件包含了RFID读卡器的主要应用逻辑，负责初始化RFID模块，并根据不同的卡片类型（Type A, B, V, F） - * 执行相应的寻卡、激活和数据交换流程。 - * 原代码是为51单片机编写，现已进行重构，移除了所有硬件相关的代码（如GPIO、SPI、UART的直接操作）， - * 并引入了硬件抽象层（HAL）的概念。所有底层硬件操作都通过 `rfid_hal.h` 中定义的接口函数完成， - * 以便于在STM32等不同平台上进行移植。 - * - * @author Kilo Code - * @date 2025-11-24 - * @version 1.0 - * - * @note - * - 您需要在使用本模块前，在您的平台上实现 `rfid_hal.h` 中声明的所有硬件接口函数。 - * - `rfid_task()` 函数是一个示例性的主任务循环，您可以根据您的实际应用需求进行修改或集成。 - * - 文件中保留了对不同卡片类型事件的处理函数（`TYPE_A_EVENT`, `TYPE_B_EVENT`等）， - * 这些是RFID协议的核心逻辑。 - ********************************************************************************************************/ - -#include "../include/rfid_main.h" -#include -#include -#include - -#include "../include/READER.h" -#include "../include/READER_REG.h" -#include "../include/MIFARE.h" -#include "../include/NTAG.h" -#include "rfid_hal.h" // 引入硬件抽象层头文件 - -// 宏定义，用于调试信息输出。您需要实现 rfid_hal.h 中的 rfid_log_debug 函数 -#define rfid_printf rfid_log_debug - -/** - * @brief 将一个字节的整数转换为两位十六进制字符串。 - * @param buf [out] 存储转换后字符串的缓冲区，长度至少为3。 - * @param num [in] 要转换的无符号字符。 - * @return 无。 - */ -static void IntToHex(unsigned char *buf, unsigned char num) -{ - const unsigned char digits[] = "0123456789ABCDEF"; - buf[0] = digits[(num >> 4) & 0x0F]; // 高4位 - buf[1] = digits[num & 0x0F]; // 低4位 - buf[2] = '\0'; -} - -/** - * @brief 通过调试接口打印一个字节的十六进制值。 - * @param num [in] 要打印的无符号字符。 - * @return 无。 - */ -static void printHex(unsigned char num) -{ - unsigned char buf[3]; - IntToHex(buf, num); - rfid_printf((const char *)buf); -} - -/** - * @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; - rfid_printf("TYPE_A_EVENT begin\n"); - - // 初始化读卡器为Type A模式 - result = ReaderA_Initial(); - if (result != SUCCESS) - { - rfid_printf("INIT_ERROR\r\n"); - SetCW(DISABLE); - return; - } - - // 打开RF场（载波） - result = SetCW(ENABLE); - if (result != SUCCESS) - { - rfid_printf("CW_ERROR\r\n"); - SetCW(DISABLE); - return; - } - - // 激活Type A卡片 - result = ReaderA_CardActivate(&PICC_A); - if (result != SUCCESS) - { - // rfid_printf("ReaderA_CardActivate_ERROR\r\n"); - SetCW(DISABLE); - return; - } - - rfid_printf("************* TYPE A CARD ************* \r\n"); - rfid_printf("-> ATQA = %02X%02X\r\n", PICC_A.ATQA[0], PICC_A.ATQA[1]); - - if (PICC_A.UID_Length == 4) - { - rfid_printf("-> UID = %02X%02X%02X%02X\r\n", PICC_A.UID[0], PICC_A.UID[1], PICC_A.UID[2], PICC_A.UID[3]); - } - else if (PICC_A.UID_Length > 4) // 支持更长的UID - { - rfid_printf("-> UID = "); - for (int i = 0; i < PICC_A.UID_Length; i++) - { - rfid_printf("%02X", PICC_A.UID[i]); - } - rfid_printf("\r\n"); - } - rfid_printf("-> SAK = %02X\r\n", PICC_A.SAK[0]); - - // 根据SAK值判断卡片类型 - if (PICC_A.SAK[0] == 0x08) - { - rfid_printf("************* Mifare CARD ************* \r\n"); - result = MIFARE_CARD_EVENT(); - } - else if ((PICC_A.SAK[0] == 0x28) || (PICC_A.SAK[0] == 0x20)) - { - rfid_printf("************* CPU CARD ************* \r\n"); - // result = CPU_CARD_EVENT(); // CPU卡处理函数，暂未实现 - } - else if (PICC_A.SAK[0] == 0x04) - { - rfid_printf("************* NTAG CARD ************* \r\n"); - result = NTAG_EVENT(); - } - - SetCW(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; - rfid_printf("TYPE_B_EVENT begin\n"); - - ReaderB_Initial(); - SetCW(ENABLE); - - result = ReaderB_Request(&PICC_B); - if (result != SUCCESS) - { - SetCW(DISABLE); - return; - } - - rfid_printf("************* TYPE B CARD ************* \r\n"); - // 打印ATQB信息 - rfid_printf("-> ATQB = "); - for(int i=0; i<12; i++) rfid_printf("%02X", PICC_B.ATQB[i]); - rfid_printf("\r\n"); - - result = ReaderB_Attrib(&PICC_B); - if (result != SUCCESS) - { - SetCW(DISABLE); - return; - } - rfid_printf("-> ATTRIB = %02X\r\n", PICC_B.CID); - - result = ReaderB_Get_SN(&PICC_B); - if (result != SUCCESS) - { - SetCW(DISABLE); - return; - } - rfid_printf("-> SN = "); - for(int i=0; i<8; i++) rfid_printf("%02X", PICC_B.SN[i]); - rfid_printf("\r\n"); - - SetCW(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; - rfid_printf("TYPE_V_EVENT begin\n"); - - ReaderV_Initial(); - SetCW(ENABLE); - - result = ReaderV_Inventory(&PICC_V); - if (result != SUCCESS) - { - SetCW(DISABLE); - rfid_printf("-> ReaderV Inventory ERROR!\r\n"); - return; - } - - rfid_printf("************* TYPE V CARD ************* \r\n"); - rfid_printf("UID="); - for (i = 0; i < 8; i++) - { - printHex(PICC_V.UID[i]); - } - rfid_printf("\r\n"); - - result = ReaderV_Select(&PICC_V); - if (result != SUCCESS) - { - SetCW(DISABLE); - rfid_printf("-> 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(DISABLE); - rfid_printf("-> ReaderV WriteSingleBlock ERROR!\r\n"); - return; - } - rfid_printf("WriteSingleBlock SUCCESS\r\n"); - - // 示例：读单个块 - result = ReaderV_ReadSingleBlock(4, &PICC_V); - if (result != SUCCESS) - { - SetCW(DISABLE); - rfid_printf("-> ReaderV ReadSingleBlock ERROR!\r\n"); - return; - } - rfid_printf("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(DISABLE); -} - -/** - * @brief 处理Type F (FeliCa) 卡片事件。 - * @details - * 该函数执行FeliCa卡片的交互流程，包括： - * 1. 初始化读卡器以支持FeliCa协议。 - * 2. 打开RF场。 - * 3. 发送Inventory命令寻卡并获取UID。 - * 4. 后续可以添加与FeliCa卡的数据交换命令。 - * 5. 操作结束后关闭RF场。 - * @note 当前实现仅包含寻卡部分，具体的TPDU命令交换被注释掉了，因为它们依赖于一个未提供的 `CPU_TPDU` 函数。 - * @return 无。 - */ -void TYPE_F_EVENT(void) -{ - unsigned char result, i; - // unsigned char SendBuffer[255]; - // unsigned char ReceiveBuffer[255]; - // transmission_struct TPDU; - // TPDU.pSendBuffer = SendBuffer; - // TPDU.pReceiveBuffer = ReceiveBuffer; - - rfid_printf("TYPE_F_EVENT begin\n"); - ReaderF_Initial(); - SetCW(ENABLE); - - result = ReaderF_Inventory(&PICC_F); - if (result != SUCCESS) - { - SetCW(DISABLE); - return; - } - - rfid_printf("************* TYPE F CARD ************* \r\n"); - rfid_printf("->TYPE F UID = "); - for(i=0; i<8; i++) rfid_printf("%02X", PICC_F.UID[i]); - rfid_printf("\r\n"); - - // 此处省略了原代码中复杂的TPDU数据交换部分， - // 因为它依赖于一个未定义的 CPU_TPDU 函数。 - // 如果需要与CPU卡进行数据交换，您需要实现相关的APDU指令封装和解析。 - - SetCW(DISABLE); -} - - -/** - * @brief RFID模块的主任务函数。 - * @details - * 这是一个示例性的任务函数，展示了如何初始化RFID芯片并进入一个无限循环来轮询不同类型的卡片。 - * 您可以将此函数作为一个独立的任务运行，或者将其中的逻辑集成到您现有的任务调度中。 - * 1. 调用 `rfid_hal_init()` 初始化底层硬件。 - * 2. 调用 `FM176XX_HardReset()` 硬复位RFID芯片。 - * 3. 检查芯片版本号，确认通信正常。 - * 4. 在主循环中，依次调用不同卡片类型的事件处理函数。 - * @return 无。 - */ -void rfid_task(void) -{ - unsigned char result, reg_data; - - // 1. 初始化底层硬件 (SPI, GPIO, UART, Delay) - // 这个函数需要您在 rfid_hal.c 中实现 - rfid_hal_init(); - - // 2. 硬复位 FM176XX 芯片 - while (1) - { - result = FM176XX_HardReset(); - if (result != SUCCESS) - { - rfid_printf("FM176XX HardReset FAIL\r\n"); - rfid_delay_ms(1000); // 延时后重试 - } - else - { - rfid_printf("FM176XX HardReset SUCCESS\r\n"); - break; - } - } - - // 3. 读取芯片版本号，确认通信是否正常 - GetReg(REG_VERSION, ®_data); - rfid_printf("REG_VERSION = %02X\r\n", reg_data); - - // 4. 进入主循环，轮询不同类型的卡 - while (1) - { - // 您可以根据需要取消注释来测试不同类型的卡 - // TYPE_A_EVENT(); - // TYPE_B_EVENT(); - TYPE_V_EVENT(); // 当前默认只测试 Type V (15693) 卡 - // TYPE_F_EVENT(); - - rfid_delay_ms(500); // 每次轮询后延时 - } -} diff --git a/apps/earphone/remote_control/RC_app_main.c b/apps/earphone/remote_control/RC_app_main.c new file mode 100644 index 0000000..e69de29 diff --git a/apps/earphone/remote_control/RFID/include/CPU_CARD.h b/apps/earphone/remote_control/RFID/include/CPU_CARD.h new file mode 100644 index 0000000..dc60ee1 --- /dev/null +++ b/apps/earphone/remote_control/RFID/include/CPU_CARD.h @@ -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 diff --git a/apps/earphone/remote_control/RFID/include/MIFARE.h b/apps/earphone/remote_control/RFID/include/MIFARE.h new file mode 100644 index 0000000..fb65d43 --- /dev/null +++ b/apps/earphone/remote_control/RFID/include/MIFARE.h @@ -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 diff --git a/apps/earphone/RFID/include/NTAG.h b/apps/earphone/remote_control/RFID/include/NTAG.h similarity index 100% rename from apps/earphone/RFID/include/NTAG.h rename to apps/earphone/remote_control/RFID/include/NTAG.h diff --git a/apps/earphone/remote_control/RFID/include/READER.h b/apps/earphone/remote_control/RFID/include/READER.h new file mode 100644 index 0000000..9e1d464 --- /dev/null +++ b/apps/earphone/remote_control/RFID/include/READER.h @@ -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 diff --git a/apps/earphone/RFID/include/READER_REG.h b/apps/earphone/remote_control/RFID/include/READER_REG.h similarity index 100% rename from apps/earphone/RFID/include/READER_REG.h rename to apps/earphone/remote_control/RFID/include/READER_REG.h diff --git a/apps/earphone/remote_control/RFID/include/rfid_main.h b/apps/earphone/remote_control/RFID/include/rfid_main.h new file mode 100644 index 0000000..42b4824 --- /dev/null +++ b/apps/earphone/remote_control/RFID/include/rfid_main.h @@ -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 { + DISABLE = 0U, + ENABLE = !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 \ No newline at end of file diff --git a/apps/earphone/remote_control/RFID/reader/CPU_CARD.c b/apps/earphone/remote_control/RFID/reader/CPU_CARD.c new file mode 100644 index 0000000..964b936 --- /dev/null +++ b/apps/earphone/remote_control/RFID/reader/CPU_CARD.c @@ -0,0 +1,448 @@ +#include "../include/READER.h" +#include "../include/CPU_CARD.h" +#include +#include "../include/READER_REG.h" +#include "../include/rfid_main.h" +#include "../rfid_hal.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 + + +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(DISABLE); + rfid_log_debug("-> RATS ERROR!\r\n"); + return result; + } + + rfid_log_debug("-> ATS = "); + for(i = 0; i < CPU_CARD.ATS.Length; i++) + rfid_log_debug("%02X", CPU_CARD.ATS.Ats_Data[i]); + rfid_log_debug("\r\n"); + + result = Ats_Process(CPU_CARD.ATS.Length, CPU_CARD.ATS.Ats_Data); + if (result != SUCCESS) + { + SetCW(DISABLE); + rfid_log_debug("-> 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(DISABLE); + rfid_log_debug("-> APDU ERROR!\r\n"); + return result; + } + rfid_log_debug("-> Select MF Response = "); + for(i=0; iSendLength,tpdu->pSendBuffer); + ModifyReg(REG_TXCRCCON, BIT_CRCEN,ENABLE); + ModifyReg(REG_RXCRCCON, BIT_CRCEN,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; jpSendBuffer + apdu->SendLength - unsent_length, CPU_CARD.FSC - 1); + tpdu.SendLength = CPU_CARD.FSC; + + xlog("..--> "); + for(int j=0; jpReceiveBuffer + 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 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(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(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(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,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,®_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,ENABLE); //Clear FIFO + if(key_mode == KEY_A_M1) + { + SetReg(REG_FIFODATA,0x60);// 0x60: Key A认证指令 + ModifyReg(REG_RXTXCON,BIT_SHMODE,DISABLE); + } + if(key_mode == KEY_B_M1) + { + SetReg(REG_FIFODATA,0x61);// 0x61: Key B认证指令 + ModifyReg(REG_RXTXCON,BIT_SHMODE,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,ENABLE); + ModifyReg(REG_RXCRCCON, BIT_CRCEN,ENABLE); + SetCommand(CMD_AUTHENT); + rfid_delay_ms(5); + GetReg(REG_COMMAND,®_data); + if((reg_data & CMD_MASK) == CMD_IDLE) + { + GetReg(REG_STATUS,®_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,ENABLE); + ModifyReg(REG_RXCRCCON, BIT_CRCEN,ENABLE); + SetCommand(CMD_TRANSCEIVE); + rfid_delay_ms(2); + GetReg(REG_FIFOLENGTH,®_data); + if (reg_data != 16) // 成功时应返回16字节数据 + return FAIL; + GetReg(REG_ERROR,®_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,ENABLE); + ModifyReg(REG_RXCRCCON, BIT_CRCEN,DISABLE); + SetCommand(CMD_TRANSCEIVE); + rfid_delay_ms(5); + GetReg(REG_FIFOLENGTH,®_data); + if (reg_data != 1) // 接收到ACK (0x0A) + return FAIL; + GetReg (REG_FIFODATA,®_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,®_data); + if (reg_data != 1) // 接收到ACK (0x0A) + return FAIL; + GetReg (REG_FIFODATA,®_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,ENABLE); + ModifyReg(REG_RXCRCCON, BIT_CRCEN,DISABLE); + SetCommand(CMD_TRANSCEIVE); + rfid_delay_ms(5); + GetReg(REG_FIFOLENGTH,®_data); + if (reg_data != 1) + return FAIL; + GetReg (REG_FIFODATA,®_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,®_data); + if (reg_data != 1) + return FAIL; + GetReg (REG_FIFODATA,®_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,ENABLE); + ModifyReg(REG_RXCRCCON, BIT_CRCEN,DISABLE); + SetCommand(CMD_TRANSCEIVE); + rfid_delay_ms(5); + GetReg(REG_FIFOLENGTH,®_data); + if (reg_data != 1) + return FAIL; + GetReg (REG_FIFODATA,®_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,®_data); + if (reg_data != 1) + return FAIL; + GetReg (REG_FIFODATA,®_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,ENABLE); + ModifyReg(REG_RXCRCCON, BIT_CRCEN,DISABLE); + SetCommand(CMD_TRANSCEIVE); + rfid_delay_ms(5); + GetReg(REG_FIFOLENGTH,®_data); + if (reg_data != 1) + return FAIL; + GetReg (REG_FIFODATA,®_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,ENABLE); + ModifyReg(REG_RXCRCCON, BIT_CRCEN,DISABLE); + SetCommand(CMD_TRANSCEIVE); + rfid_delay_ms(5); + GetReg(REG_FIFOLENGTH,®_data); + if (reg_data != 1) + return FAIL; + GetReg (REG_FIFODATA,®_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,®_data); + if (reg_data != 1) + return FAIL; + GetReg (REG_FIFODATA,®_data); + if(reg_data != 0x0A) + return FAIL; + return SUCCESS; +} diff --git a/apps/earphone/remote_control/RFID/reader/NTAG.c b/apps/earphone/remote_control/RFID/reader/NTAG.c new file mode 100644 index 0000000..47ccde4 --- /dev/null +++ b/apps/earphone/remote_control/RFID/reader/NTAG.c @@ -0,0 +1,109 @@ +#include "../include/READER.h" +#include "../include/NTAG.h" +#include +#include "../include/READER_REG.h" +#include "../include/rfid_main.h" +#include "../rfid_hal.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 + + + +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,ENABLE); + ModifyReg(REG_RXCRCCON, BIT_CRCEN,ENABLE); + SetCommand(CMD_TRANSCEIVE); + rfid_delay_ms(5); + GetReg(REG_ERROR,®_data); + if(reg_data & 0x07) + return FAIL; + GetReg(REG_FIFOLENGTH,®_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,ENABLE); + ModifyReg(REG_RXCRCCON, BIT_CRCEN,DISABLE); + SetCommand(CMD_TRANSCEIVE); + rfid_delay_ms(5); + + GetReg(REG_FIFOLENGTH,®_data); + if(reg_data != 1) // 应该收到一个ACK + return FAIL; + + GetReg(REG_FIFODATA,®_data); + if(reg_data != 0x0A) // ACK的值为0x0A + return FAIL; + return SUCCESS; + +} + diff --git a/apps/earphone/remote_control/RFID/reader/READER.c b/apps/earphone/remote_control/RFID/reader/READER.c new file mode 100644 index 0000000..85b8e2a --- /dev/null +++ b/apps/earphone/remote_control/RFID/reader/READER.c @@ -0,0 +1,774 @@ +/******************************************************************************************************** + * @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" // 引入硬件抽象层 +#include + +#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 + +// 定义全局变量以存储不同类型卡片的信息 +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, ®_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] ENABLE表示打开，DISABLE表示关闭。 + * @return 操作状态，SUCCESS表示成功。 + */ +unsigned char SetCW(unsigned char mode) +{ + if (mode == ENABLE) + { + ModifyReg(REG_COMMAND, BIT_MODEMOFF, DISABLE); + ModifyReg(REG_TXMODE, BIT_TPUSHON | BIT_TPULLON, ENABLE); + } + else + { + ModifyReg(REG_COMMAND, BIT_MODEMOFF, ENABLE); + ModifyReg(REG_TXMODE, BIT_TPUSHON | BIT_TPULLON, 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, 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, ENABLE); // 清空FIFO + SetReg(REG_FIFODATA, p_rx); // 写入接收协议 + SetReg(REG_FIFODATA, p_tx); // 写入发送协议 + + SetCommand(CMD_LOADPROTOCOL); + rfid_delay_ms(2); + GetReg(REG_COMMAND, ®_data); + if (reg_data != CMD_IDLE) + return FAIL; + return SUCCESS; +} + +/** + * @brief 设置发送和接收的奇偶校验位使能状态。 + * @param state [in] ENABLE或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, ENABLE); // FORCE 100ask 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(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, DISABLE); // FORCE 100ask 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, DISABLE); + ModifyReg(REG_RXANA, (HPCF_V << 3) | GAIN_V, ENABLE); + SetParity(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, 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, DISABLE); + ModifyReg(REG_RXANA, (HPCF_F << 3) | GAIN_F, ENABLE); + SetParity(DISABLE); + SetReg(REG_THNSET, 0xFF); + SetReg(REG_THNMIN, 0x80); + SetReg(REG_THNADJ, 0x08); + ModifyReg(REG_MISC, 0x04, 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, ENABLE); + SetReg(REG_FIFODATA, RF_CMD_WUPA); + ModifyReg(REG_TXCRCCON, BIT_CRCEN, DISABLE); + ModifyReg(REG_RXCRCCON, BIT_CRCEN, DISABLE); + SetCommand(CMD_TRANSCEIVE); + rfid_delay_ms(2); + GetReg(REG_FIFOLENGTH, ®_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, ENABLE); + SetReg(REG_FIFODATA, RF_CMD_REQA); + ModifyReg(REG_TXCRCCON, BIT_CRCEN, DISABLE); + ModifyReg(REG_RXCRCCON, BIT_CRCEN, DISABLE); + SetCommand(CMD_TRANSCEIVE); + rfid_delay_ms(2); + GetReg(REG_FIFOLENGTH, ®_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, ENABLE); + SetReg(REG_FIFODATA, RF_CMD_ANTICOLL[picc_a->CASCADE_LEVEL]); + SetReg(REG_FIFODATA, 0x20); + ModifyReg(REG_TXCRCCON, BIT_CRCEN, DISABLE); + ModifyReg(REG_RXCRCCON, BIT_CRCEN, DISABLE); + SetCommand(CMD_TRANSCEIVE); + rfid_delay_ms(2); + GetReg(REG_FIFOLENGTH, ®_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, 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, ENABLE); + ModifyReg(REG_RXCRCCON, BIT_CRCEN, ENABLE); + SetCommand(CMD_TRANSCEIVE); + rfid_delay_ms(2); + GetReg(REG_ERROR, ®_data); + if ((reg_data & 0x0F) != 0) + return FAIL; + GetReg(REG_FIFOLENGTH, ®_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, 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, ENABLE); + ModifyReg(REG_RXCRCCON, BIT_CRCEN, ENABLE); + SetCommand(CMD_TRANSCEIVE); + rfid_delay_ms(10); + GetReg(REG_ERROR, ®_data); + if ((reg_data & 0x0F) != 0) + return FAIL; + GetReg(REG_FIFOLENGTH, ®_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, 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, ENABLE); + ModifyReg(REG_RXCRCCON, BIT_CRCEN, ENABLE); + SetCommand(CMD_TRANSCEIVE); + rfid_delay_ms(10); + GetReg(REG_ERROR, ®_data); + if ((reg_data & 0x0F) != 0) + return FAIL; + GetReg(REG_FIFOLENGTH, ®_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, 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, ENABLE); + ModifyReg(REG_RXCRCCON, BIT_CRCEN, ENABLE); + SetCommand(CMD_TRANSCEIVE); + rfid_delay_ms(10); + GetReg(REG_ERROR, ®_data); + if ((reg_data & 0x0F) != 0) + return FAIL; + GetReg(REG_FIFOLENGTH, ®_data); + if (reg_data != 1) + return FAIL; + GetReg(REG_FIFODATA, ®_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, 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, ENABLE); + ModifyReg(REG_RXCRCCON, BIT_CRCEN, ENABLE); + SetCommand(CMD_TRANSCEIVE); + rfid_delay_ms(10); + GetReg(REG_ERROR, ®_data); + if ((reg_data & 0x0F) != 0) + return FAIL; + GetReg(REG_FIFOLENGTH, ®_data); + if (reg_data != 1) + return FAIL; + GetReg(REG_FIFODATA, ®_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, 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, ENABLE); + ModifyReg(REG_RXCRCCON, BIT_CRCEN, ENABLE); + SetCommand(CMD_TRANSCEIVE); + rfid_delay_ms(10); + GetReg(REG_ERROR, ®_data); + if ((reg_data & 0x0F) != 0) + return FAIL; + GetReg(REG_FIFOLENGTH, ®_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, ENABLE); + SetReg(REG_FIFODATA, 0x26); + SetReg(REG_FIFODATA, 0x01); + SetReg(REG_FIFODATA, 0x00); + + ModifyReg(REG_TXCRCCON, BIT_CRCEN, ENABLE); + ModifyReg(REG_RXCRCCON, BIT_CRCEN, ENABLE); + + SetCommand(CMD_TRANSCEIVE); + rfid_delay_ms(10); + GetReg(REG_ERROR, ®_data); + if ((reg_data & 0x0F) != 0) + return FAIL; + GetReg(REG_FIFOLENGTH, ®_data); + if (reg_data != 10) + return FAIL; + GetReg(REG_FIFODATA, &picc_v->RESPONSE); + GetReg(REG_FIFODATA, ®_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, 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, ENABLE); + ModifyReg(REG_RXCRCCON, BIT_CRCEN, ENABLE); + + SetCommand(CMD_TRANSCEIVE); + rfid_delay_ms(10); + GetReg(REG_ERROR, ®_data); + if ((reg_data & 0x0F) != 0) + return FAIL; + GetReg(REG_FIFOLENGTH, ®_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, 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, ENABLE); + ModifyReg(REG_RXCRCCON, BIT_CRCEN, ENABLE); + + SetCommand(CMD_TRANSCEIVE); + rfid_delay_ms(10); + GetReg(REG_ERROR, ®_data); + if ((reg_data & 0x0F) != 0) + return FAIL; + GetReg(REG_FIFOLENGTH, ®_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, 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, ENABLE); + ModifyReg(REG_RXCRCCON, BIT_CRCEN, ENABLE); + + SetCommand(CMD_TRANSCEIVE); + rfid_delay_ms(10); + GetReg(REG_ERROR, ®_data); + if ((reg_data & 0x0F) != 0) + return FAIL; + GetReg(REG_FIFOLENGTH, ®_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, 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, ENABLE); + ModifyReg(REG_RXCRCCON, BIT_CRCEN, ENABLE); + + SetCommand(CMD_TRANSCEIVE); + rfid_delay_ms(10); + GetReg(REG_ERROR, ®_data); + if ((reg_data & 0x0F) != 0) + return FAIL; + GetReg(REG_FIFOLENGTH, ®_data); + if (reg_data != 18) + return FAIL; + GetReg(REG_FIFODATA, ®_data); // Length + GetReg(REG_FIFODATA, ®_data); // Response code + for (i = 0; i < 8; i++) + { + GetReg(REG_FIFODATA, &picc_f->UID[i]); + } + return SUCCESS; +} + diff --git a/apps/earphone/remote_control/RFID/rfid_event.c b/apps/earphone/remote_control/RFID/rfid_event.c new file mode 100644 index 0000000..c1763da --- /dev/null +++ b/apps/earphone/remote_control/RFID/rfid_event.c @@ -0,0 +1,306 @@ +/******************************************************************************************************** + * @file rfid_main.c + * @brief RFID 读卡器应用层主逻辑文件 + ********************************************************************************************************/ + +#include "./include/rfid_main.h" +#include +#include +#include + +#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 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 + + +/** + * @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(DISABLE); + return; + } + + // 打开RF场（载波） + result = SetCW(ENABLE); + if (result != SUCCESS) + { + xlog("CW_ERROR\r\n"); + SetCW(DISABLE); + return; + } + + // 激活Type A卡片 + result = ReaderA_CardActivate(&PICC_A); + if (result != SUCCESS) + { + // xlog("ReaderA_CardActivate_ERROR\r\n"); + SetCW(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(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(ENABLE); + + result = ReaderB_Request(&PICC_B); + if (result != SUCCESS) + { + SetCW(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(DISABLE); + return; + } + xlog("-> ATTRIB = %02X\r\n", PICC_B.CID); + + result = ReaderB_Get_SN(&PICC_B); + if (result != SUCCESS) + { + SetCW(DISABLE); + return; + } + xlog("-> SN = "); + for(i=0; i<8; i++) xlog("%02X", PICC_B.SN[i]); + xlog("\r\n"); + + SetCW(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(ENABLE); + + result = ReaderV_Inventory(&PICC_V); + if (result != SUCCESS) + { + SetCW(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(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(DISABLE); + xlog("-> ReaderV WriteSingleBlock ERROR!\r\n"); + return; + } + xlog("WriteSingleBlock SUCCESS\r\n"); + + // 示例：读单个块 + result = ReaderV_ReadSingleBlock(4, &PICC_V); + if (result != SUCCESS) + { + SetCW(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(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(ENABLE); + + result = ReaderF_Inventory(&PICC_F); + if (result != SUCCESS) + { + SetCW(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(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(); + // 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, ®_data); + xlog("REG_VERSION = %02X\r\n", reg_data); + + TYPE_A_EVENT(); + TYPE_B_EVENT(); + TYPE_V_EVENT(); + TYPE_F_EVENT(); +} diff --git a/apps/earphone/remote_control/RFID/rfid_hal.c b/apps/earphone/remote_control/RFID/rfid_hal.c new file mode 100644 index 0000000..45ebf35 --- /dev/null +++ b/apps/earphone/remote_control/RFID/rfid_hal.c @@ -0,0 +1,81 @@ +#include "./rfid_hal.h" +#include "gSensor/gSensor_manage.h" +#include "./include/rfid_main.h" +#include "./include/READER_REG.h" + +#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); +} diff --git a/apps/earphone/remote_control/RFID/rfid_hal.h b/apps/earphone/remote_control/RFID/rfid_hal.h new file mode 100644 index 0000000..d937ceb --- /dev/null +++ b/apps/earphone/remote_control/RFID/rfid_hal.h @@ -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 diff --git a/apps/earphone/xtell_Sensor/A_hide/1/skiing_tracker.c b/apps/earphone/xtell_Sensor/A_hide/1/skiing_tracker.c deleted file mode 100644 index c20abcc..0000000 --- a/apps/earphone/xtell_Sensor/A_hide/1/skiing_tracker.c +++ /dev/null @@ -1,219 +0,0 @@ -/* - 静态ZUPT+卡尔曼，效果貌似还行 -*/ - -#include "skiing_tracker.h" -#include "../sensor/SC7U22.h" // 包含传感器驱动头文件以调用姿态解算函数 -#include -#include - -#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"); - } -} \ No newline at end of file diff --git a/apps/earphone/xtell_Sensor/A_hide/1/skiing_tracker.h b/apps/earphone/xtell_Sensor/A_hide/1/skiing_tracker.h deleted file mode 100644 index cbaba27..0000000 --- a/apps/earphone/xtell_Sensor/A_hide/1/skiing_tracker.h +++ /dev/null @@ -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 \ No newline at end of file diff --git a/apps/earphone/xtell_Sensor/A_hide/10/skiing_tracker.c b/apps/earphone/xtell_Sensor/A_hide/10/skiing_tracker.c deleted file mode 100644 index 6cd3d7f..0000000 --- a/apps/earphone/xtell_Sensor/A_hide/10/skiing_tracker.c +++ /dev/null @@ -1,596 +0,0 @@ -/* - -*/ -#include "skiing_tracker.h" -#include "../sensor/SC7U22.h" -#include -#include - -#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; -} - diff --git a/apps/earphone/xtell_Sensor/A_hide/10/skiing_tracker.h b/apps/earphone/xtell_Sensor/A_hide/10/skiing_tracker.h deleted file mode 100644 index 3a5f883..0000000 --- a/apps/earphone/xtell_Sensor/A_hide/10/skiing_tracker.h +++ /dev/null @@ -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 \ No newline at end of file diff --git a/apps/earphone/xtell_Sensor/A_hide/11_test/skiing_tracker.c b/apps/earphone/xtell_Sensor/A_hide/11_test/skiing_tracker.c deleted file mode 100644 index 43aa73f..0000000 --- a/apps/earphone/xtell_Sensor/A_hide/11_test/skiing_tracker.c +++ /dev/null @@ -1,377 +0,0 @@ -/* - 使用四元数求角度和去掉重力分量 -*/ -#include "skiing_tracker.h" -#include "../sensor/SC7U22.h" -#include -#include - -#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; -} - diff --git a/apps/earphone/xtell_Sensor/A_hide/11_test/skiing_tracker.h b/apps/earphone/xtell_Sensor/A_hide/11_test/skiing_tracker.h deleted file mode 100644 index b1021f2..0000000 --- a/apps/earphone/xtell_Sensor/A_hide/11_test/skiing_tracker.h +++ /dev/null @@ -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 \ No newline at end of file diff --git a/apps/earphone/xtell_Sensor/A_hide/2/skiing_tracker.c b/apps/earphone/xtell_Sensor/A_hide/2/skiing_tracker.c deleted file mode 100644 index 5c4350f..0000000 --- a/apps/earphone/xtell_Sensor/A_hide/2/skiing_tracker.c +++ /dev/null @@ -1,259 +0,0 @@ -/* - 动态ZUPT+卡尔曼 -*/ -#include "skiing_tracker.h" -#include "../sensor/SC7U22.h" -#include -#include - -#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"); - } -} \ No newline at end of file diff --git a/apps/earphone/xtell_Sensor/A_hide/2/skiing_tracker.h b/apps/earphone/xtell_Sensor/A_hide/2/skiing_tracker.h deleted file mode 100644 index 2c7a87f..0000000 --- a/apps/earphone/xtell_Sensor/A_hide/2/skiing_tracker.h +++ /dev/null @@ -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 \ No newline at end of file diff --git a/apps/earphone/xtell_Sensor/A_hide/3/skiing_tracker.c b/apps/earphone/xtell_Sensor/A_hide/3/skiing_tracker.c deleted file mode 100644 index dc8f955..0000000 --- a/apps/earphone/xtell_Sensor/A_hide/3/skiing_tracker.c +++ /dev/null @@ -1,277 +0,0 @@ -/* - 动态ZUPT+卡尔曼 -*/ -#include "skiing_tracker.h" -#include "../sensor/SC7U22.h" -#include -#include - -#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"); - } -} \ No newline at end of file diff --git a/apps/earphone/xtell_Sensor/A_hide/3/skiing_tracker.h b/apps/earphone/xtell_Sensor/A_hide/3/skiing_tracker.h deleted file mode 100644 index 2c7a87f..0000000 --- a/apps/earphone/xtell_Sensor/A_hide/3/skiing_tracker.h +++ /dev/null @@ -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 \ No newline at end of file diff --git a/apps/earphone/xtell_Sensor/A_hide/4/skiing_tracker.c b/apps/earphone/xtell_Sensor/A_hide/4/skiing_tracker.c deleted file mode 100644 index 16bba6a..0000000 --- a/apps/earphone/xtell_Sensor/A_hide/4/skiing_tracker.c +++ /dev/null @@ -1,465 +0,0 @@ -/** - * 效果不行，对原地转动太灵敏了 - */ - -#include "skiing_tracker.h" -#include -#include - -// --- 核心算法参数 --- -#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(); -} \ No newline at end of file diff --git a/apps/earphone/xtell_Sensor/A_hide/4/skiing_tracker.h b/apps/earphone/xtell_Sensor/A_hide/4/skiing_tracker.h deleted file mode 100644 index 6f39130..0000000 --- a/apps/earphone/xtell_Sensor/A_hide/4/skiing_tracker.h +++ /dev/null @@ -1,40 +0,0 @@ -#ifndef SKI_SPEED_DISTANCE_H -#define SKI_SPEED_DISTANCE_H - -#include - -// 滑雪状态定义 -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 \ No newline at end of file diff --git a/apps/earphone/xtell_Sensor/A_hide/5/skiing_tracker.c b/apps/earphone/xtell_Sensor/A_hide/5/skiing_tracker.c deleted file mode 100644 index 767aee5..0000000 --- a/apps/earphone/xtell_Sensor/A_hide/5/skiing_tracker.c +++ /dev/null @@ -1,311 +0,0 @@ -/* - 动态ZUPT+卡尔曼 - 多了加速度死区、摩擦力速度衰减、高通滤波 - 原地摆动产生的速度、距离变化还是没法消除 - 水平移动、斜坡移动效果貌似还行 -*/ -#include "skiing_tracker.h" -#include "../sensor/SC7U22.h" -#include -#include - -#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"); - } -} \ No newline at end of file diff --git a/apps/earphone/xtell_Sensor/A_hide/5/skiing_tracker.h b/apps/earphone/xtell_Sensor/A_hide/5/skiing_tracker.h deleted file mode 100644 index 10cc8d9..0000000 --- a/apps/earphone/xtell_Sensor/A_hide/5/skiing_tracker.h +++ /dev/null @@ -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 \ No newline at end of file diff --git a/apps/earphone/xtell_Sensor/A_hide/6/skiing_tracker.c b/apps/earphone/xtell_Sensor/A_hide/6/skiing_tracker.c deleted file mode 100644 index 9a0b39e..0000000 --- a/apps/earphone/xtell_Sensor/A_hide/6/skiing_tracker.c +++ /dev/null @@ -1,374 +0,0 @@ -/* - 动态ZUPT+卡尔曼+巴特沃斯一阶滤波器 - 针对启动滑雪和停止滑雪，设置不同阈值 - 启动滑雪和ZUPT更新的陀螺仪方差阈值分开设置 - - 启动滑雪的陀螺仪阈值会更宽松一些 - 原地旋转和ZUPT更新的加速度方差阈值分开设置 - - 原地旋转的加速度阈值更宽松 - 能够从静止状态到变化状态，去根据阈值来判断这个“变化”：进入滑行状态 / 只是原地摆动 - - 但是还是不够灵敏 - -*/ -#include "skiing_tracker.h" -#include "../sensor/SC7U22.h" -#include -#include - -#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; -} \ No newline at end of file diff --git a/apps/earphone/xtell_Sensor/A_hide/6/skiing_tracker.h b/apps/earphone/xtell_Sensor/A_hide/6/skiing_tracker.h deleted file mode 100644 index 3f34990..0000000 --- a/apps/earphone/xtell_Sensor/A_hide/6/skiing_tracker.h +++ /dev/null @@ -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 \ No newline at end of file diff --git a/apps/earphone/xtell_Sensor/A_hide/7/skiing_tracker.c b/apps/earphone/xtell_Sensor/A_hide/7/skiing_tracker.c deleted file mode 100644 index 1dc7a10..0000000 --- a/apps/earphone/xtell_Sensor/A_hide/7/skiing_tracker.c +++ /dev/null @@ -1,276 +0,0 @@ -/* - 简化的滑雪追踪器： - - 直接读取六轴数据 - - 分离重力分量 - - 直接积分求速度和距离 -*/ -#include "skiing_tracker.h" -#include "../sensor/SC7U22.h" -#include -#include - -#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; -} \ No newline at end of file diff --git a/apps/earphone/xtell_Sensor/A_hide/7/skiing_tracker.h b/apps/earphone/xtell_Sensor/A_hide/7/skiing_tracker.h deleted file mode 100644 index b34cb1a..0000000 --- a/apps/earphone/xtell_Sensor/A_hide/7/skiing_tracker.h +++ /dev/null @@ -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 \ No newline at end of file diff --git a/apps/earphone/xtell_Sensor/A_hide/8/skiing_tracker.c b/apps/earphone/xtell_Sensor/A_hide/8/skiing_tracker.c deleted file mode 100644 index c80e178..0000000 --- a/apps/earphone/xtell_Sensor/A_hide/8/skiing_tracker.c +++ /dev/null @@ -1,501 +0,0 @@ -/* - 虽然sensor_processing_task是10ms调用一次 - 但是实际上上一次调用该函数的时间点和下一次调用该函数的时间点，会相差40ms - -*/ -#include "skiing_tracker.h" -#include "../sensor/SC7U22.h" -#include -#include - -#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; -} \ No newline at end of file diff --git a/apps/earphone/xtell_Sensor/A_hide/8/skiing_tracker.h b/apps/earphone/xtell_Sensor/A_hide/8/skiing_tracker.h deleted file mode 100644 index 3a6b36c..0000000 --- a/apps/earphone/xtell_Sensor/A_hide/8/skiing_tracker.h +++ /dev/null @@ -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 \ No newline at end of file diff --git a/apps/earphone/xtell_Sensor/A_hide/9/skiing_tracker.c b/apps/earphone/xtell_Sensor/A_hide/9/skiing_tracker.c deleted file mode 100644 index a1b8595..0000000 --- a/apps/earphone/xtell_Sensor/A_hide/9/skiing_tracker.c +++ /dev/null @@ -1,583 +0,0 @@ -/* - -*/ -#include "skiing_tracker.h" -#include "../sensor/SC7U22.h" -#include -#include - -#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; -} - diff --git a/apps/earphone/xtell_Sensor/A_hide/9/skiing_tracker.h b/apps/earphone/xtell_Sensor/A_hide/9/skiing_tracker.h deleted file mode 100644 index 5188afa..0000000 --- a/apps/earphone/xtell_Sensor/A_hide/9/skiing_tracker.h +++ /dev/null @@ -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 \ No newline at end of file diff --git a/apps/earphone/xtell_Sensor/A_hide/calculate/skiing_tracker.c b/apps/earphone/xtell_Sensor/A_hide/calculate/skiing_tracker.c deleted file mode 100644 index a45d496..0000000 --- a/apps/earphone/xtell_Sensor/A_hide/calculate/skiing_tracker.c +++ /dev/null @@ -1,651 +0,0 @@ -/* - -*/ -#include "skiing_tracker.h" -#include "../sensor/SC7U22.h" -#include -#include - -#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; -} - diff --git a/apps/earphone/xtell_Sensor/A_hide/calculate/skiing_tracker.h b/apps/earphone/xtell_Sensor/A_hide/calculate/skiing_tracker.h deleted file mode 100644 index 750180d..0000000 --- a/apps/earphone/xtell_Sensor/A_hide/calculate/skiing_tracker.h +++ /dev/null @@ -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 \ No newline at end of file diff --git a/apps/earphone/xtell_Sensor/README.md b/apps/earphone/xtell_Sensor/README.md deleted file mode 100644 index 659566d..0000000 --- a/apps/earphone/xtell_Sensor/README.md +++ /dev/null @@ -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无法通过加速度计来消除偏差） diff --git a/apps/earphone/xtell_Sensor/ano/ano_protocol.c b/apps/earphone/xtell_Sensor/ano/ano_protocol.c deleted file mode 100644 index e0720b8..0000000 --- a/apps/earphone/xtell_Sensor/ano/ano_protocol.c +++ /dev/null @@ -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 -} \ No newline at end of file diff --git a/apps/earphone/xtell_Sensor/ano/ano_protocol.h b/apps/earphone/xtell_Sensor/ano/ano_protocol.h deleted file mode 100644 index 4e0e2a6..0000000 --- a/apps/earphone/xtell_Sensor/ano/ano_protocol.h +++ /dev/null @@ -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__ \ No newline at end of file diff --git a/apps/earphone/xtell_Sensor/buffer/circle_buffer.c b/apps/earphone/xtell_Sensor/buffer/circle_buffer.c deleted file mode 100644 index 1b5bbaf..0000000 --- a/apps/earphone/xtell_Sensor/buffer/circle_buffer.c +++ /dev/null @@ -1,60 +0,0 @@ -#include "circle_buffer.h" -#include - -// 初始化环形缓冲区 -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; -} \ No newline at end of file diff --git a/apps/earphone/xtell_Sensor/buffer/circle_buffer.h b/apps/earphone/xtell_Sensor/buffer/circle_buffer.h deleted file mode 100644 index fdaf179..0000000 --- a/apps/earphone/xtell_Sensor/buffer/circle_buffer.h +++ /dev/null @@ -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 \ No newline at end of file diff --git a/apps/earphone/xtell_Sensor/calculate/skiing_tracker.c b/apps/earphone/xtell_Sensor/calculate/skiing_tracker.c deleted file mode 100644 index dab0b5b..0000000 --- a/apps/earphone/xtell_Sensor/calculate/skiing_tracker.c +++ /dev/null @@ -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); -} - diff --git a/apps/earphone/xtell_Sensor/calculate/skiing_tracker.h b/apps/earphone/xtell_Sensor/calculate/skiing_tracker.h deleted file mode 100644 index aa97f6c..0000000 --- a/apps/earphone/xtell_Sensor/calculate/skiing_tracker.h +++ /dev/null @@ -1,83 +0,0 @@ -#ifndef SKIING_TRACKER_H -#define SKIING_TRACKER_H - -#include "../xtell.h" -#include -#include - - -// 定义滑雪者可能的状态 -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 \ No newline at end of file diff --git a/apps/earphone/xtell_Sensor/example/example.c b/apps/earphone/xtell_Sensor/example/example.c deleted file mode 100644 index 051e287..0000000 --- a/apps/earphone/xtell_Sensor/example/example.c +++ /dev/null @@ -1,37 +0,0 @@ - -////////////////////////////////////////////////////////////////////////////////////////////////// -//START -- 宏定义 -#define ENABLE_XLOG 1 -#ifdef xlog -#undef xlog -#endif -#if ENABLE_XLOG - #define xlog(format, ...) printf("[%s] " format, __func__, ##__VA_ARGS__) -#else - #define xlog(format, ...) ((void)0) -#endif - -//END -- 宏定义 -////////////////////////////////////////////////////////////////////////////////////////////////// - - -////////////////////////////////////////////////////////////////////////////////////////////////// -//START -- 变量定义 - - - - -//END -- 变量定义 -////////////////////////////////////////////////////////////////////////////////////////////////// - - -////////////////////////////////////////////////////////////////////////////////////////////////// -//START -- 函数定义 - - -//END -- 函数定义 -////////////////////////////////////////////////////////////////////////////////////////////////// - - -////////////////////////////////////////////////////////////////////////////////////////////////// -//实现 diff --git a/apps/earphone/xtell_Sensor/send_data.c b/apps/earphone/xtell_Sensor/send_data.c deleted file mode 100644 index 64bf335..0000000 --- a/apps/earphone/xtell_Sensor/send_data.c +++ /dev/null @@ -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){ - -} \ No newline at end of file diff --git a/apps/earphone/xtell_Sensor/sensor/AK8963.c b/apps/earphone/xtell_Sensor/sensor/AK8963.c deleted file mode 100644 index e4e38ae..0000000 --- a/apps/earphone/xtell_Sensor/sensor/AK8963.c +++ /dev/null @@ -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; -} \ No newline at end of file diff --git a/apps/earphone/xtell_Sensor/sensor/AK8963.h b/apps/earphone/xtell_Sensor/sensor/AK8963.h deleted file mode 100644 index c2d9238..0000000 --- a/apps/earphone/xtell_Sensor/sensor/AK8963.h +++ /dev/null @@ -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 \ No newline at end of file diff --git a/apps/earphone/xtell_Sensor/sensor/BMP280.c b/apps/earphone/xtell_Sensor/sensor/BMP280.c deleted file mode 100644 index e0ebca4..0000000 --- a/apps/earphone/xtell_Sensor/sensor/BMP280.c +++ /dev/null @@ -1,211 +0,0 @@ -/* - 气压计 -*/ -#include "BMP280.h" -#include -#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)); - -} \ No newline at end of file diff --git a/apps/earphone/xtell_Sensor/sensor/BMP280.h b/apps/earphone/xtell_Sensor/sensor/BMP280.h deleted file mode 100644 index 469e00c..0000000 --- a/apps/earphone/xtell_Sensor/sensor/BMP280.h +++ /dev/null @@ -1,54 +0,0 @@ -#ifndef BMP280_DRIVER_H -#define BMP280_DRIVER_H - -#include - - -#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 \ No newline at end of file diff --git a/apps/earphone/xtell_Sensor/sensor/LIS2DH12.c b/apps/earphone/xtell_Sensor/sensor/LIS2DH12.c deleted file mode 100644 index ce02a91..0000000 --- a/apps/earphone/xtell_Sensor/sensor/LIS2DH12.c +++ /dev/null @@ -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 // 用于 memcpy - -////////////////////////////////////////////////////////////////////////////////////////////////// -//START -- 宏定义 -#define ENABLE_XLOG 1 -#ifdef xlog -#undef xlog -#endif -#if ENABLE_XLOG - #define xlog(format, ...) printf("[%s] " format, __func__, ##__VA_ARGS__) -#else - #define xlog(format, ...) ((void)0) -#endif - -// --- 运动检测核心参数 --- -#define SAMPLE_COUNT 6 // 定义静止状态检测所需的样本数量 -#define THRESHOLD 50.00f // 定义静止状态检测的阈值（三轴数据方差），值越大，对微小抖动的容忍度越高 -#define LPF_ALPHA 0.95f // 低通滤波系数，越接近1，滤波效果越强，重力估算越平滑 -#define DEADZONE_MSS 0.2f // 加速度死区阈值 (m/s^2)，低于此值的线性加速度被视为噪声并忽略 - -// --- 原有业务逻辑宏定义 --- -#define STATIC_MAX_TIME 60*5*5 // 传感器静止最大时间，单位 200ms -#define DORMANCY_MAX_TIME 60*5 // 休眠检测时间，单位 200ms - -// --- I2C地址定义 --- -#define LIS2DH12_W_ADDR 0x32 -#define LIS2DH12_R_ADDR 0x33 - -// --- IIC 寄存器地址宏定义 --- -#define LIS2DH12_WHO_AM_I 0x01 //0F -#define LIS2DH12_CTRL_REG1 0x20 -#define LIS2DH12_CTRL_REG4 0x23 -#define LIS2DH12_CTRL_REG5 0x24 -#define LIS2DH12_OUT_X_L 0x28 -#define LIS2DH12_FIFO_CTRL_REG 0x2E -#define LIS2DH12_SRC_REG 0x2F - -//END -- 宏定义 -////////////////////////////////////////////////////////////////////////////////////////////////// - - -////////////////////////////////////////////////////////////////////////////////////////////////// -//START -- 变量定义 - -u8 dormancy_flag = 0; // 休眠标识 -u8 dormancy_ago_moedl = 0; // 记录休眠前灯效 -u16 gsensor_static_flag; // 记录传感器静止的时间,单位 200ms -axis_info_t current_data[32]; // 用于存储从FIFO读取的原始传感器数据 - -//运动数据全局变量 -static motion_data_t motion_data = {{0.0f, 0.0f, 0.0f}, {0.0f, 0.0f, 0.0f}}; // 存储最终计算出的速度和距离 -static axis_info_xtell gravity_vector = {0.0f, 0.0f, -GRAVITY_EARTH}; // 存储估算出的重力向量，初始假设Z轴朝下 -static bool sensor_is_stable = false; // 传感器是否静止的标志 -static axis_info_xtell linear_accel_global = {0.0f, 0.0f, 0.0f}; // 存储移除重力后的线性加速度，用于日志打印 -static axis_info_xtell zero_g_offset = {0.0f, 0.0f, 0.0f}; // 存储开机校准测得的零点偏移量 - -u8 gsensor_alarm; -axis_info_xtell gsensor_xtell; // 存储is_sensor_stable计算出的平均值 - - - -//END -- 变量定义 -////////////////////////////////////////////////////////////////////////////////////////////////// - - -////////////////////////////////////////////////////////////////////////////////////////////////// -//START -- 函数定义 - - - -//END -- 函数定义 -////////////////////////////////////////////////////////////////////////////////////////////////// - - -////////////////////////////////////////////////////////////////////////////////////////////////// -//实现 -// --- I2C底层函数封装 --- -static u32 SL_MEMS_i2cRead(u8 addr, u8 reg, u8 len, u8 *buf) { - return _gravity_sensor_get_ndata(addr, reg, buf, len); -} -static u8 SL_MEMS_i2cWrite(u8 addr, u8 reg, u8 data) { - gravity_sensor_command(addr, reg, data); - return 0; -} - -// 检查传感器ID，确认设备是否正常连接 -char LIS2DH12_Check() { - u8 reg_value = 0; - SL_MEMS_i2cRead(LIS2DH12_R_ADDR, LIS2DH12_WHO_AM_I, 1, ®_value); - if (reg_value == 0x6A) { //0x33 - return 0x01; - } - return 0x00; -} - -// 从传感器FIFO读取一批原始数据 -void LIS2DH12_read_data(axis_info_t *sl_accel) { - u8 fifo_src = 0; - u8 samples_available = 0; - u8 data[192]; - s16 raw_x,raw_y,raw_z; - - SL_MEMS_i2cRead(LIS2DH12_R_ADDR, LIS2DH12_SRC_REG, 1, &fifo_src); - samples_available = fifo_src & 0x1F; - if (samples_available == 0) return; - - SL_MEMS_i2cRead(LIS2DH12_R_ADDR, LIS2DH12_OUT_X_L | 0x80, samples_available * 6, data); - - for (u8 i = 0; i < samples_available; i++) { - // 数据处理方式与 +/-8g 普通模式(10位) 匹配 - raw_x = (int16_t)((data[i * 6 + 1] << 8) | data[i * 6]) >> 6; - raw_y = (int16_t)((data[i * 6 + 3] << 8) | data[i * 6 + 2]) >> 6; - raw_z = (int16_t)((data[i * 6 + 5] << 8) | data[i * 6 + 4]) >> 6; - - sl_accel[i].x = raw_x; - sl_accel[i].y = raw_y; - sl_accel[i].z = raw_z; - } -} - -// 开机校准函数：测量传感器的静态零点偏移 -void LIS2DH12_calibrate() { - xlog("开始传感器校准...\n"); - axis_info_t cal_data[32]; - long x_sum = 0, y_sum = 0; - const int num_samples = 32; - - delay_2ms(100); // 等待约200ms，让FIFO填满数据 - - LIS2DH12_read_data(cal_data); - - for (int i = 0; i < num_samples; i++) { - x_sum += cal_data[i].x; - y_sum += cal_data[i].y; - } - - zero_g_offset.x = (float)x_sum / num_samples; - zero_g_offset.y = (float)y_sum / num_samples; - zero_g_offset.z = 0; // Z轴主要受重力影响，不进行校准 - - xlog("校准完成. X轴偏移: %.2f, Y轴偏移: %.2f\n", zero_g_offset.x, zero_g_offset.y); -} - -// 初始化并配置LIS2DH12传感器 -u8 LIS2DH12_Config(void) { - if (LIS2DH12_Check() != 1) { - xlog("LIS2DH12 I2C error\n"); - return -1; - } - - // 统一配置: 25Hz采样率, +/-8g量程, 普通模式(10位) - SL_MEMS_i2cWrite(LIS2DH12_W_ADDR, LIS2DH12_CTRL_REG1, 0x37); // 25 Hz ODR - SL_MEMS_i2cWrite(LIS2DH12_W_ADDR, LIS2DH12_CTRL_REG4, 0x20); // +/-8g, BDU enabled - SL_MEMS_i2cWrite(LIS2DH12_W_ADDR, LIS2DH12_CTRL_REG5, 0x40); // 使能FIFO - SL_MEMS_i2cWrite(LIS2DH12_W_ADDR, LIS2DH12_FIFO_CTRL_REG, 0x80); // 流模式 - - // 执行开机校准 - LIS2DH12_calibrate(); - - xlog("LIS2DH12 I2C success\n"); - return 0; -} - -// 判断传感器是否处于静止状态 -bool is_sensor_stable(axis_info_t *accel_data, int sample_count) { - float mean_x = 0, mean_y = 0, mean_z = 0; - float variance_x = 0, variance_y = 0, variance_z = 0; - - if (sample_count <= 1) return true; - - // 1. 计算均值 - for (int i = 0; i < sample_count; i++) { - mean_x += accel_data[i].x; - mean_y += accel_data[i].y; - mean_z += accel_data[i].z; - } - mean_x /= sample_count; - mean_y /= sample_count; - mean_z /= sample_count; - - gsensor_xtell.x = mean_x; - gsensor_xtell.y = mean_y; - gsensor_xtell.z = mean_z; - - // 2. 计算方差 - for (int i = 0; i < sample_count; i++) { - variance_x += (accel_data[i].x - mean_x) * (accel_data[i].x - mean_x); - variance_y += (accel_data[i].y - mean_y) * (accel_data[i].y - mean_y); - variance_z += (accel_data[i].z - mean_z) * (accel_data[i].z - mean_z); - } - variance_x /= (sample_count - 1); - variance_y /= (sample_count - 1); - variance_z /= (sample_count - 1); - - // 3. 如果方差大于阈值，则认为在运动 - if (variance_x > THRESHOLD || variance_y > THRESHOLD || variance_z > THRESHOLD) { - return false; - } - return true; -} - -// 获取当前的总加速度（包含重力），单位 m/s^2 -axis_info_xtell get_current_accel_mss(void) { - axis_info_xtell accel_mss; - // 灵敏度 @ +/-8g 普通模式 (10-bit) = 12 mg/LSB - const float sensitivity_g_per_lsb = 0.012f; - - // 在转换前，先减去校准测得的零点偏移 - accel_mss.x = ((float)gsensor_xtell.x - zero_g_offset.x) * sensitivity_g_per_lsb * GRAVITY_EARTH; - accel_mss.y = ((float)gsensor_xtell.y - zero_g_offset.y) * sensitivity_g_per_lsb * GRAVITY_EARTH; - accel_mss.z = (float)gsensor_xtell.z * sensitivity_g_per_lsb * GRAVITY_EARTH; - return accel_mss; -} - -// 获取计算好的运动数据（速度和距离） -void get_motion_data(motion_data_t *data) { - if (data) { - memcpy(data, &motion_data, sizeof(motion_data_t)); - } -} - -// 获取移除重力后的线性加速度 -axis_info_xtell get_linear_accel_mss(void) { - return linear_accel_global; -} - -// 核心计算任务，由定时器周期性调用 -void xtell_i2c_test() { - // 1. 读取一批最新的传感器数据 - LIS2DH12_read_data(current_data); - // 2. 判断传感器当前是否静止 - sensor_is_stable = is_sensor_stable(current_data, SAMPLE_COUNT); - - // 3. 获取校准和转换后的总加速度 (m/s^2) - axis_info_xtell current_accel_mss = get_current_accel_mss(); - - // 4. 使用低通滤波器估算重力向量 - gravity_vector.x = LPF_ALPHA * gravity_vector.x + (1.0f - LPF_ALPHA) * current_accel_mss.x; - gravity_vector.y = LPF_ALPHA * gravity_vector.y + (1.0f - LPF_ALPHA) * current_accel_mss.y; - gravity_vector.z = LPF_ALPHA * gravity_vector.z + (1.0f - LPF_ALPHA) * current_accel_mss.z; - - // 5. 从总加速度中减去重力，得到线性加速度 - linear_accel_global.x = current_accel_mss.x - gravity_vector.x; - linear_accel_global.y = current_accel_mss.y - gravity_vector.y; - linear_accel_global.z = current_accel_mss.z - gravity_vector.z; - - // 6. 应用死区：忽略过小的加速度值（噪声） - if (fabsf(linear_accel_global.x) < DEADZONE_MSS) linear_accel_global.x = 0.0f; - if (fabsf(linear_accel_global.y) < DEADZONE_MSS) linear_accel_global.y = 0.0f; - if (fabsf(linear_accel_global.z) < DEADZONE_MSS) linear_accel_global.z = 0.0f; - - // 7. 积分线性加速度，得到速度 - motion_data.velocity.x += linear_accel_global.x * SAMPLING_PERIOD_S; - motion_data.velocity.y += linear_accel_global.y * SAMPLING_PERIOD_S; - motion_data.velocity.z += linear_accel_global.z * SAMPLING_PERIOD_S; - - // 8. 如果传感器静止，重置速度和距离以消除漂移 - if (sensor_is_stable) { - motion_data.velocity.x = 0.0f; - motion_data.velocity.y = 0.0f; - motion_data.velocity.z = 0.0f; - motion_data.distance.x = 0.0f; - motion_data.distance.y = 0.0f; - motion_data.distance.z = 0.0f; - } - - // 9. 积分速度，得到距离 - motion_data.distance.x += motion_data.velocity.x * SAMPLING_PERIOD_S; - motion_data.distance.y += motion_data.velocity.y * SAMPLING_PERIOD_S; - motion_data.distance.z += motion_data.velocity.z * SAMPLING_PERIOD_S; - - // 10. 计算并打印总的移动距离（可选，用于调试） - float total_distance_magnitude = sqrtf(motion_data.distance.x * motion_data.distance.x + - motion_data.distance.y * motion_data.distance.y + - motion_data.distance.z * motion_data.distance.z); - // xlog("Total distance traveled: %.2f m\n", total_distance_magnitude); -} diff --git a/apps/earphone/xtell_Sensor/sensor/LIS2DH12.h b/apps/earphone/xtell_Sensor/sensor/LIS2DH12.h deleted file mode 100644 index 383f86a..0000000 --- a/apps/earphone/xtell_Sensor/sensor/LIS2DH12.h +++ /dev/null @@ -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 \ No newline at end of file diff --git a/apps/earphone/xtell_Sensor/sensor/MMC56.c b/apps/earphone/xtell_Sensor/sensor/MMC56.c deleted file mode 100644 index a5276b2..0000000 --- a/apps/earphone/xtell_Sensor/sensor/MMC56.c +++ /dev/null @@ -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); - - -} \ No newline at end of file diff --git a/apps/earphone/xtell_Sensor/sensor/MMC56.h b/apps/earphone/xtell_Sensor/sensor/MMC56.h deleted file mode 100644 index 919063f..0000000 --- a/apps/earphone/xtell_Sensor/sensor/MMC56.h +++ /dev/null @@ -1,103 +0,0 @@ -#ifndef MMC5603NJ_DRIVER_H -#define MMC5603NJ_DRIVER_H - -#include - - -//该芯片的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 \ No newline at end of file diff --git a/apps/earphone/xtell_Sensor/sensor/SC7U22.c b/apps/earphone/xtell_Sensor/sensor/SC7U22.c deleted file mode 100644 index 4c67271..0000000 --- a/apps/earphone/xtell_Sensor/sensor/SC7U22.c +++ /dev/null @@ -1,1979 +0,0 @@ -/* - 六轴 -*/ -#include "SC7U22.h" -#include "math.h" -#include "os/os_api.h" -#include "../xtell.h" - -#define ENABLE_XLOG 1 -#ifdef xlog -#undef xlog -#endif -#if ENABLE_XLOG - #define xlog(format, ...) printf("[XT:%s] " format, __func__, ##__VA_ARGS__) -#else - #define xlog(format, ...) ((void)0) -#endif - -//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); -} - -char iic_read_len; -char iic_write_result; -unsigned char SL_SC7U22_Check(void) -{ - unsigned char reg_value=0; - - iic_write_result = SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x7F, 0x00);//goto 0x00 - // xlog("SL_SC7U22_Check write: %d\n", iic_write_result); - - iic_read_len = SL_SC7U22_I2c_Spi_Read(SL_SPI_IIC_INTERFACE, SC7U22_WHO_AM_I, 1, ®_value); - // xlog("SL_SC7U22_Check read : %d\n", iic_write_result); - xlog("0x%x=0x%x\r\n",SC7U22_WHO_AM_I,reg_value); - if(reg_value==0x6A) //设备的id - return 0x01;//SC7U22 - else - return 0x00;//通信异常 -} -char xt_Check_Flag; -unsigned char SL_SC7U22_Config(void) -{ - xlog("SL_SC7U22_Config\n"); - 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;//强制初始化 - - xlog("SL_SC7U22_Check=0x%x\r\n",Check_Flag); - - - xt_Check_Flag = Check_Flag; - - if(Check_Flag==1) - { - Check_Flag= SL_SC7U22_POWER_DOWN(); - } - xlog("SL_SC7U22_POWER_DOWN=0x%x\r\n",Check_Flag); - if(Check_Flag==1) - { - Check_Flag= SL_SC7U22_SOFT_RESET(); - } - xlog("SL_SC7U22_SOFT_RESET=0x%x\r\n",Check_Flag); - 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, 0x06);//ACC_CONF 0x07=50Hz 0x06=25Hz - // SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x40, 0xA8);//高性能模式，连续4个数据平均1次，100Hz -- lmx - SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x40, 0xBC);//ACC_CON 高性能模式，1600Hz -- lmx - // SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x40, 0xBB);//ACC_CON 高性能模式，800Hz -- lmx - - SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x40, 0xA8);//ACC_CON 高性能模式，100Hz，平均数4 -- lmx - - #if ACC_RANGE==2 - SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x41, 0x00);//ACC_RANGE 00：±2G - #endif - #if ACC_RANGE==4 - SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x41, 0x01);//ACC_RANGE 01：±4G - #endif - #if ACC_RANGE==8 - SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x41, 0x02);//ACC_RANGE 02：±8G - #endif - #if ACC_RANGE==16 - SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x41, 0x03);//ACC_RANGE 03：±16G - #endif - - // SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x42, 0x86);//GYR_CONF 0x87=50Hz 0x86=25Hz - // SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x42, 0x8C);//GYR_CONF 1600Hz -- lmx - // SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x42, 0xAC);//GYR_CONF 1600Hz -- lmx - // SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x42, 0xAB);//GYR_CONF 800Hz -- lmx - SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x42, 0xE8);//GYR_CONF 100Hz, 噪声优化开启,4个平均一次 -- lmx - - SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x43, 0x00);//GYR_RANGE 2000dps - SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x43, 0x00);//GYR_RANGE 2000dps - SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x04, 0x50);//COM_CFG - -#if SL_SC7U22_RAWDATA_HPF_ENABLE ==0x01 - SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE,0x7F, 0x83);//goto 0x83 - sl_delay(1); - SL_SC7U22_I2c_Spi_Read(SL_SPI_IIC_INTERFACE, 0x26, 1, ®_value); - reg_value=reg_value|0xA0; - SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x26, reg_value);//HPF_CFG rawdata hpf -#endif - -#if SL_SC7U22_AOI_Wake_Up_ENABLE==0x01 - SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x30, 0x2A);//XYZ-ENABLE - SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x32, 0x01);//VTH - SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x33, 0x01);//TTH - SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x3F, 0x30);//HPF FOR AOI1&AOI2 -#endif - -#if SL_SC7U22_FIFO_ENABLE==0x01 - SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x1E,0x1D);// - SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x1D,0x00);// - SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x1D,0x20);// - SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x1C,0x37);// -#endif - - - - - -#if SL_SC7U22_SDO_PullUP_ENABLE ==0x01 - SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE,0x7F, 0x8C);//goto 0x8C - sl_delay(1); - SL_SC7U22_I2c_Spi_Read(SL_SPI_IIC_INTERFACE, 0x30, 1, ®_value); - reg_value=reg_value&0xFE;//CS PullUP_enable - reg_value=reg_value&0xFD;//SDO PullUP_enable - SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x30, reg_value); - - SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE,0x7F, 0x00);//goto 0x00 - os_time_dly(1); - SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE,0x7F, 0x00);//goto 0x00 - os_time_dly(1); -#else - SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE,0x7F, 0x8C);//goto 0x8C - sl_delay(1); - SL_SC7U22_I2c_Spi_Read(SL_SPI_IIC_INTERFACE, 0x30, 1, ®_value); - reg_value=reg_value&0xFE;//CS PullUP_enable - reg_value=reg_value|0x02;//SDO PullUP_disable - SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x30, reg_value); - - SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE,0x7F, 0x00);//goto 0x00 - sl_delay(1); - SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE,0x7F, 0x00);//goto 0x00 - sl_delay(1); -#endif - - return 1; - } - else - return 0; -} - - -//读取时间戳 -unsigned int SL_SC7U22_TimeStamp_Read(void) -{ - unsigned char time_data[3]; - unsigned int time_stamp; - - SL_SC7U22_I2c_Spi_Read(SL_SPI_IIC_INTERFACE, 0x18, 1, &time_data[0]); - SL_SC7U22_I2c_Spi_Read(SL_SPI_IIC_INTERFACE, 0x19, 1, &time_data[1]); - SL_SC7U22_I2c_Spi_Read(SL_SPI_IIC_INTERFACE, 0x20, 1, &time_data[2]); - - time_stamp=(unsigned int)(time_data[0]<<16|time_data[1]<<8|time_data[2]); - - return time_stamp; -} - -#if SL_SC7U22_FIFO_ENABLE ==0x00 -//100Hz 10ms read once -void SL_SC7U22_RawData_Read(signed short * acc_data_buf,signed short * gyr_data_buf) -{ - unsigned char raw_data[12]; - unsigned char drdy_satus=0x00; - unsigned short drdy_cnt=0; - - while((drdy_satus&0x03)!=0x03)//acc+gyro -// while((drdy_satus&0x01)!=0x01)//acc - { - drdy_satus=0x00; - sl_delay(1); - SL_SC7U22_I2c_Spi_Read(SL_SPI_IIC_INTERFACE, 0x0B, 1, &drdy_satus); - drdy_cnt++; - if(drdy_cnt>30000) break; - } - -// SL_SC7U22_I2c_Spi_Read(SL_SPI_IIC_INTERFACE, 0x30, 1, &drdy_satus); -// xlog("RawData:0x40=%x\r\n",drdy_satus); -// SL_SC7U22_I2c_Spi_Read(SL_SPI_IIC_INTERFACE, 0x40, 1, &drdy_satus); -// xlog("RawData:0x40=%x\r\n",drdy_satus); -// SL_SC7U22_I2c_Spi_Read(SL_SPI_IIC_INTERFACE, 0x06, 1, &drdy_satus); -// xlog("RawData:0x06=%x\r\n",drdy_satus); -// SL_SC7U22_I2c_Spi_Read(SL_SPI_IIC_INTERFACE, 0x07, 1, &drdy_satus); -// xlog("RawData:0x07=%x\r\n",drdy_satus); -// SL_SC7U22_I2c_Spi_Read(SL_SPI_IIC_INTERFACE, 0x7D, 1, &drdy_satus); -// xlog("RawData:0x7D=%x\r\n",drdy_satus); -// SL_SC7U22_I2c_Spi_Read(SL_SPI_IIC_INTERFACE, 0x31, 1, &drdy_satus); -// xlog("RawData:0x31=%x\r\n",drdy_satus); -// SL_SC7U22_I2c_Spi_Read(SL_SPI_IIC_INTERFACE, 0x02, 1, &drdy_satus); -// xlog("RawData:0x02=%x\r\n",drdy_satus); -// SL_SC7U22_I2c_Spi_Read(SL_SPI_IIC_INTERFACE, 0x03, 1, &drdy_satus); -// xlog("RawData:0x03=%x\r\n",drdy_satus); - - - 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位 - - // xlog("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]); - -} -#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 - xlog("SC7U22_FIFO_NUM1:%d\n",fifo_num); -#if SL_Sensor_Algo_Release_Enable==0x00 -// xlog("0x1F:0x%x 0x20:0x%x\n",fifo_num1,fifo_num2); -// xlog("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])) ; - xlog("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])) ; - xlog("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); - SL_SC7U22_I2c_Spi_Read(SL_SPI_IIC_INTERFACE, 0x04, 1,&SL_Read_Reg); - xlog("SL_SC7U22_SOFT_RESET1 0x04=0x%x\r\n",SL_Read_Reg); - SL_Read_Reg = 0xff; - SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x04, 0x10);//BOOT - - 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); - xlog("SL_SC7U22_SOFT_RESET2 0x08=0x%x\r\n",SL_Read_Reg); - 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) - { - xlog("SL_Read_Reg=0x%x SL_Read_Check=0x%x\r\n",SL_Read_Reg,SL_Read_Check); - 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) - { - xlog("SL_Read_Reg=0x%x 0x04\r\n",SL_Read_Reg); - 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 -// SL_SC7U22_I2c_Spi_Read(SL_SPI_IIC_INTERFACE, 0x40, 1, &SL_Read_Check); -// xlog("RawData:0x40=%x\r\n",SL_Read_Check); -// SL_SC7U22_I2c_Spi_Read(SL_SPI_IIC_INTERFACE, 0x41, 1, &SL_Read_Check); -// xlog("RawData:0x41=%x\r\n",SL_Read_Check); -// SL_SC7U22_I2c_Spi_Read(SL_SPI_IIC_INTERFACE, 0x42, 1, &SL_Read_Check); -// xlog("RawData:0x42=%x\r\n",SL_Read_Check); -// SL_SC7U22_I2c_Spi_Read(SL_SPI_IIC_INTERFACE, 0x43, 1, &SL_Read_Check); -// xlog("RawData:0x43=%x\r\n",SL_Read_Check); - - - SL_SC7U22_I2c_Spi_Read(SL_SPI_IIC_INTERFACE, 0x43, 1,&SL_Read_Check); - if(SL_Read_Check!=SL_gyro_range_Reg) - { - xlog("SL_Read_Check=0x%x SL_gyro_range_Reg=0x%x\r\n",SL_Read_Check,SL_gyro_range_Reg); - return 0; - } - return 1; -} - - -#if SL_SC7U22_FIFO_ENABLE ==0x00 -// ================================================================================================= -// 卡尔曼滤波器（Kalman Filter）相关变量定义 -// 卡尔曼滤波器是一种高效的递归滤波器，它能够从一系列不完全及包含噪声的测量中，估计动态系统的状态。 -// 在这里，它被用来融合加速度计和陀螺仪的数据，以获得更精确、更稳定的姿态角（Pitch 和 Roll）。 -// ------------------------------------------------------------------------------------------------- - -// --- 状态变量 --- -float angle[3] = {0, 0, 0}, angle_dot[3] = {0, 0, 0}; // 姿态角（Pitch, Roll, Yaw）和角速度 -float angle0[3] = {0, 0, 0}, angle_dot0[3] = {0, 0, 0}; // 姿态角的估计值 - -// --- 卡尔曼滤波器参数 --- -// Q_angle: 过程噪声协方差，表示角度预测模型的不确定性。值越小，表示越相信陀螺仪的积分结果。 -// Q_gyro: 过程噪声协方差，表示陀螺仪偏置（bias）的不确定性。 -// R_angle: 测量噪声协方差，表示通过加速度计计算出的角度测量值的不确定性。值越小，表示越相信加速度计的测量结果。 -// dt: 采样时间间隔（单位：秒），这里是10ms (0.01s)，对应100Hz的采样率。 -// float Q_angle=0.0003, Q_gyro=0.001, R_angle=0.005, dt=0.005;//5ms ST -//float Q_angle=0.00001, Q_gyro=0.00001, R_angle=0.005, dt=0.0025;//5ms ST -float Q_angle = 0.0003, Q_gyro = 0.001, R_angle = 0.005, dt = 0.01; //10ms - -// --- 协方差矩阵 P --- -// P矩阵表示系统状态估计的不确定性程度。它是一个2x2的矩阵： -// P[0][0]: 角度估计的方差 -// P[0][1], P[1][0]: 角度和陀螺仪偏置的协方差 -// P[1][1]: 陀螺仪偏置估计的方差 -// P0, P1, P2 分别用于 Pitch, Roll, Yaw 的计算（尽管Yaw未使用卡尔曼滤波）。 -float P[2][2] = {{ 1, 0 }, { 0, 1 }}; -float P0[2][2] = {{ 1, 0 }, { 0, 1 }}; // Pitch 轴的协方差矩阵 -float P1[2][2] = {{ 1, 0 }, { 0, 1 }}; // Roll 轴的协方差矩阵 -float P2[2][2] = {{ 1, 0 }, { 0, 1 }}; // Yaw 轴的协方差矩阵（未使用） - -// --- 中间计算变量 --- -float Pdot0[4] = {0, 0, 0, 0}; // P0矩阵的微分，用于预测步骤 -float Pdot1[4] = {0, 0, 0, 0}; // P1矩阵的微分，用于预测步骤 -float Pdot2[4] = {0, 0, 0, 0}; // P2矩阵的微分，用于预测步骤 -const float C_0 = 1.0; // 测量矩阵H的元素，因为直接测量角度，所以为1 -const float C_1 = 1.0; -const float C_2 = 1.0; -float q_bias0[3] = {0, 0, 0}; // 陀螺仪零点偏置（bias）的估计值 -float angle_err0[3] = {0, 0, 0}; // 测量值与预测值之间的误差 -float PCt0_0[3] = {0, 0, 0}, PCt0_1[3] = {0, 0, 0}; // 中间变量，用于计算卡尔曼增益 -float E0[3] = {0, 0, 0}; // 误差的协方差 -float K0_0[3] = {0, 0, 0}, K0_1[3] = {0, 0, 0}; // 卡尔曼增益K -float t0_0[3] = {0, 0, 0}, t0_1[3] = {0, 0, 0}; // 中间变量，用于更新P矩阵 -// ================================================================================================= - -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; -} - -unsigned char SL_SC7U22_Error_Flag=0; -unsigned char SL_SC7U22_Error_cnt=0; -unsigned char SL_SC7U22_Error_cnt2=0; -signed short Temp_Accgyro[6] ={0}; -signed short Error_Accgyro[6]={0}; -signed int Sum_Avg_Accgyro[6] ={0}; -/** - * @brief 姿态角解算函数 - * @details - * 该函数主要完成两项工作： - * 1. 静态校准：在初始阶段，检测传感器是否处于静止状态。如果是，则计算加速度计和陀螺仪的零点偏移（误差），用于后续的数据补偿。 - * 2. 姿态解算：使用卡尔曼滤波器融合经过校准后的加速度计和陀螺仪数据，计算出物体的俯仰角（Pitch）、横滚角（Roll）和偏航角（Yaw）。 - * - * @param calibration_en 传入：外部校准使能标志。如果为0，则强制认为已经校准完成。 - * @param acc_gyro_input 传入和传出：包含6轴原始数据的数组指针，顺序为 [ACC_X, ACC_Y, ACC_Z, GYR_X, GYR_Y, GYR_Z]。该函数会对其进行原地修改，填充为校准后的数据。 - * @param Angle_output 传出：滤波后的结果，顺序为 [Pitch, Roll, Yaw]。 - * @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; - float angle_acc[3] = {0}; - float gyro_val[3] = {0}; - - // 如果外部强制使能校准，则将标志位置1 - if (calibration_en == 0) { - SL_SC7U22_Error_Flag = 1; - } - - // ================================================================================= - // 步骤 1: 静态校准 - // --------------------------------------------------------------------------------- - // SL_SC7U22_Error_Flag 为0时，表示需要进行校准。 - // 校准的原理是：当传感器长时间处于静止状态时，认为其三轴加速度输出应为(0, 0, g)，三轴角速度输出应为(0, 0, 0)。 - // 通过采集一段时间的静止数据并求平均，可以得到传感器的零点偏移量。 - if (SL_SC7U22_Error_Flag == 0) { - // 计算当前数据与上一帧数据的差值，用于判断是否静止 - acc_gyro_delta[0] = 0; - acc_gyro_delta[1] = 0; - for (sl_i = 0; sl_i < 3; sl_i++) { - acc_gyro_delta[0] += SL_GetAbsShort(acc_gyro_input[sl_i] - Temp_Accgyro[sl_i]); - acc_gyro_delta[1] += SL_GetAbsShort(acc_gyro_input[3 + sl_i] - Temp_Accgyro[3 + sl_i]); - } - // 保存当前数据，用于下一帧比较 - for (sl_i = 0; sl_i < 6; sl_i++) { - Temp_Accgyro[sl_i] = acc_gyro_input[sl_i]; - } - - // 判断是否处于静止状态：加速度变化量、陀螺仪变化量、各轴加速度值都在一个很小的范围内 -#if (ACC_RANGE == 2) - if ((acc_gyro_delta[0] / 8 < 160) && (acc_gyro_delta[1] < 40) && (SL_GetAbsShort(acc_gyro_input[0]) < 3000) && (SL_GetAbsShort(acc_gyro_input[1]) < 3000) && (SL_GetAbsShort(acc_gyro_input[2] - 16384) < 3000)) { //acc<160mg gyro<40 lsb -#elif (ACC_RANGE == 4) - if ((acc_gyro_delta[0] / 8 < 160) && (acc_gyro_delta[1] < 40) && (SL_GetAbsShort(acc_gyro_input[0]) < 3000) && (SL_GetAbsShort(acc_gyro_input[1]) < 3000) && (SL_GetAbsShort(acc_gyro_input[2] - 8192) < 3000)) { //acc<160mg gyro<40 lsb -#elif (ACC_RANGE == 8) - if ((acc_gyro_delta[0] / 8 < 160) && (acc_gyro_delta[1] < 40) && (SL_GetAbsShort(acc_gyro_input[0]) < 3000) && (SL_GetAbsShort(acc_gyro_input[1]) < 3000) && (SL_GetAbsShort(acc_gyro_input[2] - 4096) < 3000)) { //acc<160mg gyro<40 lsb -#elif (ACC_RANGE == 16) - if ((acc_gyro_delta[0] / 8 < 160) && (acc_gyro_delta[1] < 40) && (SL_GetAbsShort(acc_gyro_input[0]) < 3000) && (SL_GetAbsShort(acc_gyro_input[1]) < 3000) && (SL_GetAbsShort(acc_gyro_input[2] - 2048) < 3000)) { //acc<160mg gyro<40 lsb -#endif - // 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; - } - - // 计算零点偏移：理想值 - 实际平均值 - Error_Accgyro[0] = 0 - Sum_Avg_Accgyro[0]; - Error_Accgyro[1] = 0 - Sum_Avg_Accgyro[1]; - #if ACC_RANGE==2 - Error_Accgyro[2] = 16384 - Sum_Avg_Accgyro[2]; - #endif - - #if ACC_RANGE==4 - Error_Accgyro[2] = 8192 - Sum_Avg_Accgyro[2]; - #endif - - #if ACC_RANGE==8 - Error_Accgyro[2] = 4096 - Sum_Avg_Accgyro[2]; - #endif - - #if ACC_RANGE==16 - Error_Accgyro[2] = 2048 - Sum_Avg_Accgyro[2]; - #endif - - Error_Accgyro[3] = 0 - Sum_Avg_Accgyro[3]; - Error_Accgyro[4] = 0 - Sum_Avg_Accgyro[4]; - Error_Accgyro[5] = 0 - Sum_Avg_Accgyro[5]; - xlog("AVG_Recode AX:%d,AY:%d,AZ:%d,GX:%d,GY:%d,GZ:%d\r\n", Sum_Avg_Accgyro[0], Sum_Avg_Accgyro[1], Sum_Avg_Accgyro[2], Sum_Avg_Accgyro[3], Sum_Avg_Accgyro[4], Sum_Avg_Accgyro[5]); - xlog("Error_Recode AX:%d,AY:%d,AZ:%d,GX:%d,GY:%d,GZ:%d\r\n", Error_Accgyro[0], Error_Accgyro[1], Error_Accgyro[2], Error_Accgyro[3], Error_Accgyro[4], Error_Accgyro[5]); - } - } else { - // 如果在累加过程中发生移动，则重新开始 - SL_SC7U22_Error_cnt2 = 0; - for (sl_i = 0; sl_i < 6; sl_i++) { - Sum_Avg_Accgyro[sl_i] = 0; - } - } - - return 0; // 返回0，表示正在校准 - } - - // ================================================================================= - // 步骤 2: 姿态解算 - // --------------------------------------------------------------------------------- - if (SL_SC7U22_Error_Flag == 1) { // 确认已经校准完成 - // --- 2.1 数据预处理 --- - // 应用零点偏移补偿 - for (sl_i = 0; sl_i < 6; sl_i++) { - Temp_Accgyro[sl_i] = acc_gyro_input[sl_i] + Error_Accgyro[sl_i]; - } - -#if 1 // 将校准后的数据写回输入数组 - for (sl_i = 0; sl_i < 6; sl_i++) { - acc_gyro_input[sl_i] = Temp_Accgyro[sl_i]; - } -#endif - // --- 2.2 使用加速度计计算姿态角 --- - // 将加速度原始值转换为归一化的重力分量 - #if ACC_RANGE==2 - //2g： - angle_acc[0] = (float)Temp_Accgyro[0] / 16384; //ax - angle_acc[1] = (float)Temp_Accgyro[1] / 16384; //ay - angle_acc[2] = (float)Temp_Accgyro[2] / 16384; //az - #endif - - #if ACC_RANGE==4 - //4g： - angle_acc[0] = (float)Temp_Accgyro[0] / 8192; //ax - angle_acc[1] = (float)Temp_Accgyro[1] / 8192; //ay - angle_acc[2] = (float)Temp_Accgyro[2] / 8192; //az - #endif - - #if ACC_RANGE==8 - angle_acc[0] = (float)Temp_Accgyro[0] / 4096; //ax - angle_acc[1] = (float)Temp_Accgyro[1] / 4096; //ay - angle_acc[2] = (float)Temp_Accgyro[2] / 4096; //az - #endif - - #if ACC_RANGE==16 - //±16g 2048 - angle_acc[0] = (float)Temp_Accgyro[0] / 2048; //ax - angle_acc[1] = (float)Temp_Accgyro[1] / 2048; //ay - angle_acc[2] = (float)Temp_Accgyro[2] / 2048; //az - #endif - - // 限制范围，防止asinf/atanf计算错误 - if (angle_acc[0] > 1.0) angle_acc[0] = 1.0; - if (angle_acc[0] < -1.0) angle_acc[0] = -1.0; - if (angle_acc[1] > 1.0) angle_acc[1] = 1.0; - if (angle_acc[1] < -1.0) angle_acc[1] = -1.0; - if (angle_acc[2] > 1.0) angle_acc[2] = 1.0; - if (angle_acc[2] < -1.0) angle_acc[2] = -1.0; - - // 根据重力分量计算Pitch和Roll角（单位：度） - // Pitch = arcsin(ax / g) - angle_acc[0] = asinf(angle_acc[0]) * 57.32484; //Pitch:-90~+90 - // Roll = -arctan(ay / az) - angle_acc[1] = -atanf(angle_acc[1] / angle_acc[2]) * 57.32484; //Roll: -180~+180 - // 对Roll角进行象限补偿 - if (angle_acc[2] < 0) { - if (angle_acc[1] >= 0) { - angle_acc[1] = -180 + angle_acc[1]; - } else { - angle_acc[1] = 180 + angle_acc[1]; - } - } - - // --- 2.3 转换陀螺仪数据单位 --- - // 将陀螺仪原始值（LSB）转换为角速度（度/秒） - // 转换系数0.061 ≈ 2000dps / 32768 LSB - gyro_val[0] = Temp_Accgyro[4] * 0.061; // GYR-Y -> Pitch - gyro_val[1] = Temp_Accgyro[3] * 0.061; // GYR-X -> Roll - gyro_val[2] = Temp_Accgyro[5] * 0.061; // GYR-Z -> Yaw - - // ================================================================================= - // 步骤 2.4: 卡尔曼滤波 - // 对Pitch和Roll分别进行滤波 - // --------------------------------------------------------------------------------- - - /************** Pitch 轴滤波 **************/ - // --- 预测步骤 --- - // 1. 预测状态：根据上一时刻的角度和当前角速度，预测当前角度 - angle0[0] += (gyro_val[0] - q_bias0[0]) * dt; - // 2. 预测协方差：更新P矩阵，表示预测状态的不确定性 - Pdot0[0] = Q_angle - P0[0][1] - P0[1][0] + P0[1][1] * dt; - Pdot0[1] = -P0[1][1]; - Pdot0[2] = -P0[1][1]; - Pdot0[3] = Q_gyro; - P0[0][0] += Pdot0[0] * dt; - P0[0][1] += Pdot0[1] * dt; - P0[1][0] += Pdot0[2] * dt; - P0[1][1] += Pdot0[3] * dt; - - // --- 更新步骤 --- - // 1. 计算卡尔曼增益 K - PCt0_0[0] = C_0 * P0[0][0]; - PCt0_1[0] = C_0 * P0[1][0]; - E0[0] = R_angle + C_0 * PCt0_0[0]; - if (E0[0] == 0) { E0[0] = 0.0001; } // 防止除零 - K0_0[0] = PCt0_0[0] / E0[0]; - K0_1[0] = PCt0_1[0] / E0[0]; - - // 2. 计算测量余差（innovation） - angle_err0[0] = angle_acc[0] - angle0[0]; - // 3. 更新状态估计：结合预测值和测量值，得到最优估计 - angle0[0] += K0_0[0] * angle_err0[0]; - // 4. 更新陀螺仪偏置估计 - q_bias0[0] += K0_1[0] * angle_err0[0]; - angle_dot0[0] = gyro_val[0] - q_bias0[0]; - - // 5. 更新协方差矩阵 P - t0_0[0] = PCt0_0[0]; - t0_1[0] = C_0 * P0[0][1]; - P0[0][0] -= K0_0[0] * t0_0[0]; - P0[0][1] -= K0_0[0] * t0_1[0]; - P0[1][0] -= K0_1[0] * t0_0[0]; - P0[1][1] -= K0_1[0] * t0_1[0]; - - // 输出最终的Pitch角 - Angle_output[0] = angle0[0]; - - /************** Roll 轴滤波 (过程同Pitch) **************/ - // --- 预测步骤 --- - angle0[1] += (gyro_val[1] - q_bias0[1]) * dt; - Pdot1[0] = Q_angle - P1[0][1] - P1[1][0] + P1[1][1] * dt; - Pdot1[1] = -P1[1][1]; - Pdot1[2] = -P1[1][1]; - Pdot1[3] = Q_gyro; - P1[0][0] += Pdot1[0] * dt; - P1[0][1] += Pdot1[1] * dt; - P1[1][0] += Pdot1[2] * dt; - P1[1][1] += Pdot1[3] * dt; - - // --- 更新步骤 --- - PCt0_0[1] = C_1 * P1[0][0]; - PCt0_1[1] = C_1 * P1[1][0]; - E0[1] = R_angle + C_1 * PCt0_0[1]; - if (E0[1] == 0) { E0[1] = 0.0001; } - K0_0[1] = PCt0_0[1] / E0[1]; - K0_1[1] = PCt0_1[1] / E0[1]; - angle_err0[1] = angle_acc[1] - angle0[1]; - angle0[1] += K0_0[1] * angle_err0[1]; - q_bias0[1] += K0_1[1] * angle_err0[1]; - angle_dot0[1] = gyro_val[1] - q_bias0[1]; - t0_0[1] = PCt0_0[1]; - t0_1[1] = C_1 * P1[0][1]; - P1[0][0] -= K0_0[1] * t0_0[1]; - P1[0][1] -= K0_0[1] * t0_1[1]; - P1[1][0] -= K0_1[1] * t0_0[1]; - P1[1][1] -= K0_1[1] * t0_1[1]; - - // 输出最终的Roll角 - Angle_output[1] = angle0[1]; - - /************** Yaw 轴计算 **************/ - // Yaw角无法通过加速度计（重力）来校正，因此这里只使用陀螺仪进行简单积分。 - // 这种方法会因为陀螺仪的漂移而导致误差随时间累积。 - if (yaw_rst == 1) { - Angle_output[2] = 0; // 如果有复位信号，则清零 - } - - // 增加一个简单的阈值，当角速度较小时，认为没有转动，以减少漂移 - if (SL_GetAbsShort(Temp_Accgyro[5]) > 8) { - Angle_output[2] += gyro_val[2] * dt; - } - - return 1; // 返回1，表示计算成功 - } - - return 2; // 校准未完成，返回错误状态 -} - -/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// -// -// -/** - * @brief 姿态角解算函数 (基于一阶互补滤波) - * @details - * @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 Original_SL_SC7U22_Angle_Output(unsigned char calibration_en, signed short *acc_gyro_input, float *Angle_output, unsigned char yaw_rst) -{ - // --- 静态变量和宏定义 --- - const float dt = 0.01f; // 假设采样周期为 10ms (100Hz) - // 互补滤波器系数. alpha 越大, 越相信陀螺仪的积分, 动态响应越好, 但长期会漂移. - // (1-alpha) 越大, 越相信加速度计的角度, 静态越准, 但动态响应差且易受运动干扰. - // 典型值在 0.95 ~ 0.98 之间. - const float FILTER_ALPHA = 0.98f; - - unsigned char sl_i = 0; - float angle_acc[3] = {0}; // [Pitch_acc, Roll_acc, unused] - float gyro_dps[3] = {0}; // [Roll_rate, Pitch_rate, Yaw_rate] - - // 如果外部强制禁用校准，则将标志位置1 - if (calibration_en == 0) { - SL_SC7U22_Error_Flag = 1; - } - - // ================================================================================= - // 步骤 1: 静态校准 (此部分逻辑完全保留) - // --------------------------------------------------------------------------------- - if (SL_SC7U22_Error_Flag == 0) { - unsigned short acc_gyro_delta[2]; - acc_gyro_delta[0] = 0; - acc_gyro_delta[1] = 0; - for (sl_i = 0; sl_i < 3; sl_i++) { - acc_gyro_delta[0] += SL_GetAbsShort(acc_gyro_input[sl_i] - Temp_Accgyro[sl_i]); - acc_gyro_delta[1] += SL_GetAbsShort(acc_gyro_input[3 + sl_i] - Temp_Accgyro[3 + sl_i]); - } - for (sl_i = 0; sl_i < 6; sl_i++) { - Temp_Accgyro[sl_i] = acc_gyro_input[sl_i]; - } -#if (ACC_RANGE == 2) - if ((acc_gyro_delta[0] / 8 < 160) && (acc_gyro_delta[1] < 40) && (SL_GetAbsShort(acc_gyro_input[0]) < 3000) && (SL_GetAbsShort(acc_gyro_input[1]) < 3000) && (SL_GetAbsShort(acc_gyro_input[2] - 16384) < 3000)) { -#elif (ACC_RANGE == 4) - if ((acc_gyro_delta[0] / 8 < 160) && (acc_gyro_delta[1] < 40) && (SL_GetAbsShort(acc_gyro_input[0]) < 3000) && (SL_GetAbsShort(acc_gyro_input[1]) < 3000) && (SL_GetAbsShort(acc_gyro_input[2] - 8192) < 3000)) { -#elif (ACC_RANGE == 8) - if ((acc_gyro_delta[0] / 8 < 160) && (acc_gyro_delta[1] < 40) && (SL_GetAbsShort(acc_gyro_input[0]) < 3000) && (SL_GetAbsShort(acc_gyro_input[1]) < 3000) && (SL_GetAbsShort(acc_gyro_input[2] - 4096) < 3000)) { -#elif (ACC_RANGE == 16) - if ((acc_gyro_delta[0] / 8 < 160) && (acc_gyro_delta[1] < 40) && (SL_GetAbsShort(acc_gyro_input[0]) < 3000) && (SL_GetAbsShort(acc_gyro_input[1]) < 3000) && (SL_GetAbsShort(acc_gyro_input[2] - 2048) < 3000)) { -#endif - if (SL_SC7U22_Error_cnt < 200) SL_SC7U22_Error_cnt++; - } else { - SL_SC7U22_Error_cnt = 0; - } - if (SL_SC7U22_Error_cnt > 190) { - for (sl_i = 0; sl_i < 6; sl_i++) Sum_Avg_Accgyro[sl_i] += acc_gyro_input[sl_i]; - SL_SC7U22_Error_cnt2++; - if (SL_SC7U22_Error_cnt2 > 49) { - SL_SC7U22_Error_Flag = 1; - SL_SC7U22_Error_cnt2 = 0; - SL_SC7U22_Error_cnt = 0; - for (sl_i = 0; sl_i < 6; sl_i++) Sum_Avg_Accgyro[sl_i] = Sum_Avg_Accgyro[sl_i] / 50; - Error_Accgyro[0] = 0 - Sum_Avg_Accgyro[0]; - Error_Accgyro[1] = 0 - Sum_Avg_Accgyro[1]; - #if ACC_RANGE==2 - Error_Accgyro[2] = 16384 - Sum_Avg_Accgyro[2]; - #elif ACC_RANGE==4 - Error_Accgyro[2] = 8192 - Sum_Avg_Accgyro[2]; - #elif ACC_RANGE==8 - Error_Accgyro[2] = 4096 - Sum_Avg_Accgyro[2]; - #elif ACC_RANGE==16 - Error_Accgyro[2] = 2048 - Sum_Avg_Accgyro[2]; - #endif - Error_Accgyro[3] = 0 - Sum_Avg_Accgyro[3]; - Error_Accgyro[4] = 0 - Sum_Avg_Accgyro[4]; - Error_Accgyro[5] = 0 - Sum_Avg_Accgyro[5]; - xlog("AVG_Recode AX:%d,AY:%d,AZ:%d,GX:%d,GY:%d,GZ:%d\r\n", Sum_Avg_Accgyro[0], Sum_Avg_Accgyro[1], Sum_Avg_Accgyro[2], Sum_Avg_Accgyro[3], Sum_Avg_Accgyro[4], Sum_Avg_Accgyro[5]); - xlog("Error_Recode AX:%d,AY:%d,AZ:%d,GX:%d,GY:%d,GZ:%d\r\n", Error_Accgyro[0], Error_Accgyro[1], Error_Accgyro[2], Error_Accgyro[3], Error_Accgyro[4], Error_Accgyro[5]); - } - } else { - SL_SC7U22_Error_cnt2 = 0; - for (sl_i = 0; sl_i < 6; sl_i++) Sum_Avg_Accgyro[sl_i] = 0; - } - return 0; // 返回0，表示正在校准 - } - - // ================================================================================= - // 步骤 2: 姿态解算 (一阶互补滤波) - // --------------------------------------------------------------------------------- - if (SL_SC7U22_Error_Flag == 1) { // 确认已经校准完成 - // --- 2.1 数据预处理 --- - // 应用零点偏移补偿 - for (sl_i = 0; sl_i < 6; sl_i++) { - Temp_Accgyro[sl_i] = acc_gyro_input[sl_i] + Error_Accgyro[sl_i]; - } - - // --- 2.2 使用加速度计计算姿态角 (Pitch 和 Roll) --- - float acc_x = (float)Temp_Accgyro[0]; - float acc_y = (float)Temp_Accgyro[1]; - float acc_z = (float)Temp_Accgyro[2]; - - // 使用 atan2 来计算角度，避免奇点问题和象限问题，更稳健 - // Pitch = arctan(-ax / sqrt(ay^2 + az^2)) - angle_acc[0] = atan2f(-acc_x, sqrtf(acc_y*acc_y + acc_z*acc_z)) * 57.29578f; - // Roll = arctan(ay / az) - angle_acc[1] = atan2f(acc_y, acc_z) * 57.29578f; - - // --- 2.3 转换陀螺仪数据单位 --- - // 将陀螺仪原始值（LSB）转换为角速度（度/秒） - // **注意轴向对应关系** - gyro_dps[0] = Temp_Accgyro[3] * 0.061f; // GYR-X -> Roll 速率 - gyro_dps[1] = Temp_Accgyro[4] * 0.061f; // GYR-Y -> Pitch 速率 - gyro_dps[2] = Temp_Accgyro[5] * 0.061f; // GYR-Z -> Yaw 速率 - - // ================================================================================= - // 步骤 2.4: 一阶互补滤波 - // 公式: angle = alpha * (angle + gyro_rate * dt) + (1 - alpha) * acc_angle - // --------------------------------------------------------------------------------- - // Pitch 轴融合 (Angle_output[0] 对应 Pitch) - Angle_output[0] = FILTER_ALPHA * (Angle_output[0] + gyro_dps[1] * dt) + (1.0f - FILTER_ALPHA) * angle_acc[0]; - - // Roll 轴融合 (Angle_output[1] 对应 Roll) - Angle_output[1] = FILTER_ALPHA * (Angle_output[1] + gyro_dps[0] * dt) + (1.0f - FILTER_ALPHA) * angle_acc[1]; - - /************** Yaw 轴计算 **************/ - // Yaw角无法通过加速度计（重力）来校正，因此只能使用陀螺仪进行简单积分。 - if (yaw_rst == 1) { - Angle_output[2] = 0; // 如果有复位信号，则清零 - } - - // 增加一个简单的阈值，当角速度较小时，认为没有转动，以减少漂移 - if (fabsf(gyro_dps[2]) > 0.1f) { // 使用转换后的dps值判断更直观 - Angle_output[2] += gyro_dps[2] * dt; - } - - // 保持Yaw角在 -180 到 180 度之间 (可选，但推荐) - if (Angle_output[2] > 180.0f) { - Angle_output[2] -= 360.0f; - } else if (Angle_output[2] < -180.0f) { - Angle_output[2] += 360.0f; - } - - // --- 将校准后的数据写回输入数组 (可选，根据你的需求保留或移除) --- -#if 1 - for (sl_i = 0; sl_i < 6; sl_i++) { - acc_gyro_input[sl_i] = Temp_Accgyro[sl_i]; - } -#endif - - return 1; // 返回1，表示计算成功 - } - - return 2; // 校准未完成，返回错误状态 -} - -unsigned char get_SC7U22_Error_Flag(void){ - return SL_SC7U22_Error_Flag; -} - -/** - * @brief 设置零漂检测标准位 - * - * @param flag 0：重新进行零漂检测 - */ -void set_SC7U22_Error_Flag(char flag){ - SL_SC7U22_Error_Flag = flag; -} - -///////////////////////////////////////////////////////////////////////////////////////////////////////////////// - -// ================================================================================================= -// Mahony AHRS (Attitude and Heading Reference System) 相关变量定义 -// 通过一个PI控制器来校正陀ar螺仪的积分漂移。 -// ------------------------------------------------------------------------------------------------- -// --- 滤波器参数 --- -// Kp: 比例增益，决定了加速度计数据校正陀螺仪的权重。值越大，对加速度计的响应越快，但对运动加速度更敏感。 -// Ki: 积分增益，决定了用于校正陀螺仪静态漂移的权重。 -// Q_dt: 采样时间间隔（单位：秒），这里是10ms (0.01s)，对应100Hz的采样率。 -#define HAVE_MAG 0 -#if HAVE_MAG == 0 -// -- 无地磁 -- -const float Kp = 2.0f; -const float Ki = 0.005f; -const float Q_dt = 0.01f; -#else -// -- 有地磁 -- -const float Kp = 2.0f; -const float Ki = 0.005f; -const float Q_dt = 0.01f; -#endif - -// --- 状态变量 --- -// 四元数 (Quaternion)，表示当前的姿态。初始化为 (1, 0, 0, 0)，代表初始姿态为水平。 -static float q0 = 1.0f, q1 = 0.0f, q2 = 0.0f, q3 = 0.0f; -// 陀螺仪积分误差项，用于补偿静态漂移 -static float exInt = 0.0f, eyInt = 0.0f, ezInt = 0.0f; - -// 磁力计校准相关的变量 -float Error_Mag_f[3] = {0.0f, 0.0f, 0.0f}; -double Sum_Avg_Mag_f[3] = {0.0, 0.0, 0.0}; // 使用double避免累加过程中的精度损失 -// 临时存储校准后数据的数组 -signed short Temp_AccGyro[6] = {0}; -float Temp_Mag[3] = {0.0f, 0.0f, 0.0f}; -// ================================================================================================= - - - - -/** -* @brief 姿态角解算函数 (基于四元数和Mahony滤波器) -* @details -* 该函数主要完成两项工作： -* 1. 静态校准：在初始阶段，检测传感器是否处于静止状态。如果是，则计算加速度计和陀螺仪的零点偏移（误差），用于后续的数据补偿。 -* 2. 姿态解算：使用基于四元数的Mahony互补滤波器融合经过校准后的加速度计和陀螺仪数据，计算出物体的俯仰角（Pitch）、横滚角（Roll）和偏航角（Yaw）。 -* 能避免万向节死锁问题。 -* -* @param calibration_en 传入：外部校准使能标志。如果为0，则强制认为已经校准完成。 -* @param acc_gyro_input 传入和传出：包含6轴原始数据的数组指针，顺序为 [ACC_X, ACC_Y, ACC_Z, GYR_X, GYR_Y, GYR_Z]。该函数会对其进行原地修改，填充为校准后的数据。 -* @param Angle_output 传出：滤波后的结果，顺序为 [Pitch, Roll, Yaw]，单位为度。 -* @param mag_data_input 传入：指向包含三轴磁力计数据的结构体指针。数据单位应为高斯(Gauss)。已经8面校准过的数据 -* @param yaw_rst 传入：Yaw轴重置标志。如果为1，则将整个姿态滤波器状态重置。 -* @param quaternion_output 传出, 四元数,用于后续重力分量的去除计算 -* @return -* - 0: 正在进行静态校准。 -* - 1: 姿态角计算成功。 -* - 2: 校准未完成，无法进行计算。 -*/ -unsigned char Q_SL_SC7U22_Angle_Output(unsigned char calibration_en, signed short *acc_gyro_input, float *Angle_output, const mmc5603nj_mag_data_t* _mag_data_input, unsigned char yaw_rst, float *quaternion_output) -{ -#if 0 //有地磁置1 - unsigned char sl_i = 0; - // 如果外部强制禁用校准，则将标志位置1 - if (calibration_en == 0) { - SL_SC7U22_Error_Flag = 1; - } - - // ====================== 坐标对齐 ====================== - mmc5603nj_mag_data_t mag_data_input; - - mag_data_input.x = - _mag_data_input->x; - mag_data_input.y = - _mag_data_input->y; - mag_data_input.z = _mag_data_input->z; - // ================================================================ - - - // ================================================================================= - // 静态校准 - // --------------------------------------------------------------------------------- - if (SL_SC7U22_Error_Flag == 0) { - unsigned short acc_gyro_delta[2]; - acc_gyro_delta[0] = 0; - acc_gyro_delta[1] = 0; - for (sl_i = 0; sl_i < 3; sl_i++) { - acc_gyro_delta[0] += SL_GetAbsShort(acc_gyro_input[sl_i] - Temp_Accgyro[sl_i]); - acc_gyro_delta[1] += SL_GetAbsShort(acc_gyro_input[3 + sl_i] - Temp_Accgyro[3 + sl_i]); - } - for (sl_i = 0; sl_i < 6; sl_i++) { - Temp_Accgyro[sl_i] = acc_gyro_input[sl_i]; - } -#if (ACC_RANGE == 2) - if ((acc_gyro_delta[0] / 8 < 160) && (acc_gyro_delta[1] < 40) && (SL_GetAbsShort(acc_gyro_input[0]) < 3000) && (SL_GetAbsShort(acc_gyro_input[1]) < 3000) && (SL_GetAbsShort(acc_gyro_input[2] - 16384) < 3000)) { -#elif (ACC_RANGE == 4) - if ((acc_gyro_delta[0] / 8 < 160) && (acc_gyro_delta[1] < 40) && (SL_GetAbsShort(acc_gyro_input[0]) < 3000) && (SL_GetAbsShort(acc_gyro_input[1]) < 3000) && (SL_GetAbsShort(acc_gyro_input[2] - 8192) < 3000)) { -#elif (ACC_RANGE == 8) - if ((acc_gyro_delta[0] / 8 < 160) && (acc_gyro_delta[1] < 40) && (SL_GetAbsShort(acc_gyro_input[0]) < 3000) && (SL_GetAbsShort(acc_gyro_input[1]) < 3000) && (SL_GetAbsShort(acc_gyro_input[2] - 4096) < 3000)) { -#elif (ACC_RANGE == 16) - if ((acc_gyro_delta[0] / 8 < 160) && (acc_gyro_delta[1] < 40) && (SL_GetAbsShort(acc_gyro_input[0]) < 3000) && (SL_GetAbsShort(acc_gyro_input[1]) < 3000) && (SL_GetAbsShort(acc_gyro_input[2] - 2048) < 3000)) { -#endif - if (SL_SC7U22_Error_cnt < 200) SL_SC7U22_Error_cnt++; - } else { - SL_SC7U22_Error_cnt = 0; - } - if (SL_SC7U22_Error_cnt > 190) { - //累加6轴数据 - for (sl_i = 0; sl_i < 6; sl_i++) Sum_Avg_Accgyro[sl_i] += acc_gyro_input[sl_i]; - - Sum_Avg_Mag_f[0] += mag_data_input.x; - Sum_Avg_Mag_f[1] += mag_data_input.y; - Sum_Avg_Mag_f[2] += mag_data_input.z; - - SL_SC7U22_Error_cnt2++; - if (SL_SC7U22_Error_cnt2 > 49) { - SL_SC7U22_Error_Flag = 1; - SL_SC7U22_Error_cnt2 = 0; - SL_SC7U22_Error_cnt = 0; - //6轴偏置 - for (sl_i = 0; sl_i < 6; sl_i++) Sum_Avg_Accgyro[sl_i] = Sum_Avg_Accgyro[sl_i] / 50; - Error_Accgyro[0] = 0 - Sum_Avg_Accgyro[0]; - Error_Accgyro[1] = 0 - Sum_Avg_Accgyro[1]; - #if ACC_RANGE==2 - Error_Accgyro[2] = 16384 - Sum_Avg_Accgyro[2]; - #elif ACC_RANGE==4 - Error_Accgyro[2] = 8192 - Sum_Avg_Accgyro[2]; - #elif ACC_RANGE==8 - Error_Accgyro[2] = 4096 - Sum_Avg_Accgyro[2]; - #elif ACC_RANGE==16 - Error_Accgyro[2] = 2048 - Sum_Avg_Accgyro[2]; - #endif - Error_Accgyro[3] = 0 - Sum_Avg_Accgyro[3]; - Error_Accgyro[4] = 0 - Sum_Avg_Accgyro[4]; - Error_Accgyro[5] = 0 - Sum_Avg_Accgyro[5]; - // //磁力计偏置 -- 不在这弄,在磁力计初始化的时候开始 - // Sum_Avg_Mag_f[0] /= 50.0; - // Sum_Avg_Mag_f[1] /= 50.0; - // Sum_Avg_Mag_f[2] /= 50.0; - // Error_Mag_f[0] = 0.0f - (float)Sum_Avg_Mag_f[0]; - // Error_Mag_f[1] = 0.0f - (float)Sum_Avg_Mag_f[1]; - // Error_Mag_f[2] = 0.0f - (float)Sum_Avg_Mag_f[2]; - } - } else { - SL_SC7U22_Error_cnt2 = 0; - for (sl_i = 0; sl_i < 6; sl_i++) Sum_Avg_Accgyro[sl_i] = 0; - // Sum_Avg_Mag_f[0] = 0.0; - // Sum_Avg_Mag_f[1] = 0.0; - // Sum_Avg_Mag_f[2] = 0.0; - } - return 0; // 返回0，表示正在校准 - } - - // ================================================================================= - // 姿态解算 (Mahony AHRS) - // --------------------------------------------------------------------------------- - if (SL_SC7U22_Error_Flag == 1) { // 确认已经校准完成 - - // --- Yaw轴/姿态重置 --- - // 注意：重置yaw会重置整个姿态滤波器，使设备回到初始水平姿态 - if (yaw_rst == 1) { - q0 = 1.0f; q1 = 0.0f; q2 = 0.0f; q3 = 0.0f; - exInt = 0.0f; eyInt = 0.0f; ezInt = 0.0f; - } - - // --- 数据预处理 --- - // 应用零点偏移补偿 - for (sl_i = 0; sl_i < 6; sl_i++) { - Temp_Accgyro[sl_i] = acc_gyro_input[sl_i] + Error_Accgyro[sl_i]; - } - - // Temp_Mag[0] = mag_data_input.x + Error_Mag_f[0]; - // Temp_Mag[1] = mag_data_input.y + Error_Mag_f[1]; - // Temp_Mag[2] = mag_data_input.z + Error_Mag_f[2]; - Temp_Mag[0] = mag_data_input.x; - Temp_Mag[1] = mag_data_input.y; - Temp_Mag[2] = mag_data_input.z; - - // 将校准后的数据写回输入数组 -#if 1 - for (sl_i = 0; sl_i < 6; sl_i++) { - acc_gyro_input[sl_i] = Temp_Accgyro[sl_i]; - } -#endif - - // 获取校准后的数据 - float ax = (float)Temp_Accgyro[0]; - float ay = (float)Temp_Accgyro[1]; - float az = (float)Temp_Accgyro[2]; - // 将陀螺仪数据从 LSB 转换为弧度/秒 (rad/s) - // 转换系数 0.061 ≈ 2000dps / 32768 LSB; PI/180 ≈ 0.01745 - float gx = (float)Temp_Accgyro[3] * 0.061f * 0.0174533f; // Roll rate - float gy = (float)Temp_Accgyro[4] * 0.061f * 0.0174533f; // Pitch rate - float gz = (float)Temp_Accgyro[5] * 0.061f * 0.0174533f; // Yaw rate - float mx = Temp_Mag[0]; - float my = Temp_Mag[1]; - float mz = Temp_Mag[2]; - - // --- Mahony算法核心 --- - float norm; - float q0q0 = q0 * q0; - float q0q1 = q0 * q1; - float q0q2 = q0 * q2; - float q0q3 = q0 * q3; - float q1q1 = q1 * q1; - float q1q2 = q1 * q2; - float q1q3 = q1 * q3; - float q2q2 = q2 * q2; - float q2q3 = q2 * q3; - float q3q3 = q3 * q3; - float hx, hy, bx, bz; - float vx, vy, vz, wx, wy, wz; - float ex, ey, ez; - - // 归一化加速度计测量值，得到单位重力向量 - norm = sqrtf(ax * ax + ay * ay + az * az); - if (norm > 0.0f) { ax /= norm; ay /= norm; az /= norm; } - else { return 1; } - - norm = sqrtf(mx * mx + my * my + mz * mz); - if (norm > 0.0f) { mx /= norm; my /= norm; mz /= norm; } - - // 根据当前姿态(四元数)估计重力向量的方向 - vx = 2.0f * (q1q3 - q0q2); - vy = 2.0f * (q0q1 + q2q3); - vz = q0q0 - q1q1 - q2q2 + q3q3; - - // 计算磁场误差 (倾斜补偿) - hx = 2.0f * mx * (0.5f - q2q2 - q3q3) + 2.0f * my * (q1q2 - q0q3) + 2.0f * mz * (q1q3 + q0q2); - hy = 2.0f * mx * (q1q2 + q0q3) + 2.0f * my * (0.5f - q1q1 - q3q3) + 2.0f * mz * (q2q3 - q0q1); - bx = sqrtf(hx * hx + hy * hy); - bz = 2.0f * mx * (q1q3 - q0q2) + 2.0f * my * (q2q3 + q0q1) + 2.0f * mz * (0.5f - q1q1 - q2q2); - wx = 2.0f * bx * (0.5f - q2q2 - q3q3) + 2.0f * bz * (q1q3 - q0q2); - wy = 2.0f * bx * (q1q2 - q0q3) + 2.0f * bz * (q0q1 + q2q3); - wz = 2.0f * bx * (q1q3 + q0q2) + 2.0f * bz * (0.5f - q1q1 - q2q2); - - // 合并重力和磁场误差 - ex = (ay * vz - az * vy) + (my * wz - mz * wy); - ey = (az * vx - ax * vz) + (mz * wx - mx * wz); - ez = (ax * vy - ay * vx) + (mx * wy - my * wx); - - // PI控制器 - if (Ki > 0.0f) { - exInt += ex * Ki * Q_dt; - eyInt += ey * Ki * Q_dt; - ezInt += ez * Ki * Q_dt; - } - gx += Kp * ex + exInt; - gy += Kp * ey + eyInt; - gz += Kp * ez + ezInt; - - // 使用校正后的角速度更新四元数 (一阶毕卡法) - q0 += (-q1 * gx - q2 * gy - q3 * gz) * 0.5f * Q_dt; - q1 += ( q0 * gx + q2 * gz - q3 * gy) * 0.5f * Q_dt; - q2 += ( q0 * gy - q1 * gz + q3 * gx) * 0.5f * Q_dt; - q3 += ( q0 * gz + q1 * gy - q2 * gx) * 0.5f * Q_dt; - - // 归一化四元数，保持其单位长度 - norm = sqrtf(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3); - q0 /= norm; q1 /= norm; q2 /= norm; q3 /= norm; - - // --- 将四元数转换为欧拉角 (Pitch, Roll, Yaw) --- - // Pitch (绕Y轴旋转) - Angle_output[0] = asinf(-2.0f * (q1 * q3 - q0 * q2)) * 57.29578f; - // Roll (绕X轴旋转) - Angle_output[1] = atan2f(2.0f * (q0 * q1 + q2 * q3), q0q0 - q1q1 - q2q2 + q3q3) * 57.29578f; - // Yaw (绕Z轴旋转) - Angle_output[2] = atan2f(2.0f * (q1 * q2 + q0 * q3), q0q0 + q1q1 - q2q2 - q3q3) * 57.29578f; - - //将四元数传出去 - if (quaternion_output != NULL) { - quaternion_output[0] = q0; // w - quaternion_output[1] = q1; // x - quaternion_output[2] = q2; // y - quaternion_output[3] = q3; // z - } - return 1; // 返回1，表示计算成功 - } - - return 2; // 校准未完成，返回错误状态 - -#else - unsigned char sl_i = 0; - - // 如果外部强制禁用校准，则将标志位置1 - if (calibration_en == 0) { - SL_SC7U22_Error_Flag = 1; - } - - // ================================================================================= - // 步骤 1: 静态校准 (此部分逻辑与原代码完全相同) - // --------------------------------------------------------------------------------- - if (SL_SC7U22_Error_Flag == 0) { - unsigned short acc_gyro_delta[2]; - acc_gyro_delta[0] = 0; - acc_gyro_delta[1] = 0; - for (sl_i = 0; sl_i < 3; sl_i++) { - acc_gyro_delta[0] += SL_GetAbsShort(acc_gyro_input[sl_i] - Temp_Accgyro[sl_i]); - acc_gyro_delta[1] += SL_GetAbsShort(acc_gyro_input[3 + sl_i] - Temp_Accgyro[3 + sl_i]); - } - for (sl_i = 0; sl_i < 6; sl_i++) { - Temp_Accgyro[sl_i] = acc_gyro_input[sl_i]; - } -#if (ACC_RANGE == 2) - if ((acc_gyro_delta[0] / 8 < 160) && (acc_gyro_delta[1] < 40) && (SL_GetAbsShort(acc_gyro_input[0]) < 3000) && (SL_GetAbsShort(acc_gyro_input[1]) < 3000) && (SL_GetAbsShort(acc_gyro_input[2] - 16384) < 3000)) { -#elif (ACC_RANGE == 4) - if ((acc_gyro_delta[0] / 8 < 160) && (acc_gyro_delta[1] < 40) && (SL_GetAbsShort(acc_gyro_input[0]) < 3000) && (SL_GetAbsShort(acc_gyro_input[1]) < 3000) && (SL_GetAbsShort(acc_gyro_input[2] - 8192) < 3000)) { -#elif (ACC_RANGE == 8) - if ((acc_gyro_delta[0] / 8 < 160) && (acc_gyro_delta[1] < 40) && (SL_GetAbsShort(acc_gyro_input[0]) < 3000) && (SL_GetAbsShort(acc_gyro_input[1]) < 3000) && (SL_GetAbsShort(acc_gyro_input[2] - 4096) < 3000)) { -#elif (ACC_RANGE == 16) - if ((acc_gyro_delta[0] / 8 < 160) && (acc_gyro_delta[1] < 40) && (SL_GetAbsShort(acc_gyro_input[0]) < 3000) && (SL_GetAbsShort(acc_gyro_input[1]) < 3000) && (SL_GetAbsShort(acc_gyro_input[2] - 2048) < 3000)) { -#endif - if (SL_SC7U22_Error_cnt < 200) SL_SC7U22_Error_cnt++; - } else { - // printf("error: The calibration process has undergone a shift.\n"); - SL_SC7U22_Error_cnt = 0; - } - if (SL_SC7U22_Error_cnt > 190) { - for (sl_i = 0; sl_i < 6; sl_i++) Sum_Avg_Accgyro[sl_i] += acc_gyro_input[sl_i]; - SL_SC7U22_Error_cnt2++; - if (SL_SC7U22_Error_cnt2 > 49) { - SL_SC7U22_Error_Flag = 1; - SL_SC7U22_Error_cnt2 = 0; - SL_SC7U22_Error_cnt = 0; - for (sl_i = 0; sl_i < 6; sl_i++) Sum_Avg_Accgyro[sl_i] = Sum_Avg_Accgyro[sl_i] / 50; - Error_Accgyro[0] = 0 - Sum_Avg_Accgyro[0]; - Error_Accgyro[1] = 0 - Sum_Avg_Accgyro[1]; - #if ACC_RANGE==2 - Error_Accgyro[2] = 16384 - Sum_Avg_Accgyro[2]; - #elif ACC_RANGE==4 - Error_Accgyro[2] = 8192 - Sum_Avg_Accgyro[2]; - #elif ACC_RANGE==8 - Error_Accgyro[2] = 4096 - Sum_Avg_Accgyro[2]; - #elif ACC_RANGE==16 - Error_Accgyro[2] = 2048 - Sum_Avg_Accgyro[2]; - #endif - Error_Accgyro[3] = 0 - Sum_Avg_Accgyro[3]; - Error_Accgyro[4] = 0 - Sum_Avg_Accgyro[4]; - Error_Accgyro[5] = 0 - Sum_Avg_Accgyro[5]; - // xlog("AVG_Recode AX:%d,AY:%d,AZ:%d,GX:%d,GY:%d,GZ:%d\r\n", Sum_Avg_Accgyro[0], Sum_Avg_Accgyro[1], Sum_Avg_Accgyro[2], Sum_Avg_Accgyro[3], Sum_Avg_Accgyro[4], Sum_Avg_Accgyro[5]); - // xlog("Error_Recode AX:%d,AY:%d,AZ:%d,GX:%d,GY:%d,GZ:%d\r\n", Error_Accgyro[0], Error_Accgyro[1], Error_Accgyro[2], Error_Accgyro[3], Error_Accgyro[4], Error_Accgyro[5]); - } - } else { - SL_SC7U22_Error_cnt2 = 0; - for (sl_i = 0; sl_i < 6; sl_i++) Sum_Avg_Accgyro[sl_i] = 0; - } - return 0; // 返回0，表示正在校准 - } - - // ================================================================================= - // 步骤 2: 姿态解算 (Mahony AHRS) - // --------------------------------------------------------------------------------- - if (SL_SC7U22_Error_Flag == 1) { // 确认已经校准完成 - - // --- 2.1 Yaw轴/姿态重置 --- - // 注意：重置yaw会重置整个姿态滤波器，使设备回到初始水平姿态 - if (yaw_rst == 1) { - q0 = 1.0f; q1 = 0.0f; q2 = 0.0f; q3 = 0.0f; - exInt = 0.0f; eyInt = 0.0f; ezInt = 0.0f; - } - - // --- 2.2 数据预处理 --- - // 应用零点偏移补偿 - 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 - - // 获取校准后的数据 - float ax = (float)Temp_Accgyro[0]; - float ay = (float)Temp_Accgyro[1]; - float az = (float)Temp_Accgyro[2]; - // 将陀螺仪数据从 LSB 转换为弧度/秒 (rad/s) - // 转换系数 0.061 ≈ 2000dps / 32768 LSB; PI/180 ≈ 0.01745 - float gx = (float)Temp_Accgyro[3] * 0.061f * 0.0174533f; // Roll rate - float gy = (float)Temp_Accgyro[4] * 0.061f * 0.0174533f; // Pitch rate - float gz = (float)Temp_Accgyro[5] * 0.061f * 0.0174533f; // Yaw rate - - // --- 2.3 Mahony算法核心 --- - float norm; - float vx, vy, vz; // 估计的重力向量 - float ex, ey, ez; // 加速度计测量和估计的重力向量之间的误差 - - // 归一化加速度计测量值，得到单位重力向量 - norm = sqrtf(ax * ax + ay * ay + az * az); - if (norm > 0.0f) { // 防止除以零 - ax = ax / norm; - ay = ay / norm; - az = az / norm; - - // 根据当前姿态(四元数)估计重力向量的方向 - vx = 2.0f * (q1 * q3 - q0 * q2); - vy = 2.0f * (q0 * q1 + q2 * q3); - vz = q0 * q0 - q1 * q1 - q2 * q2 + q3 * q3; - - // 计算测量值与估计值之间的误差（叉积） - ex = (ay * vz - az * vy); - ey = (az * vx - ax * vz); - ez = (ax * vy - ay * vx); - - // 积分误差项，用于消除陀螺仪的静态漂移 - exInt = exInt + ex * Ki * Q_dt; - eyInt = eyInt + ey * Ki * Q_dt; - ezInt = ezInt + ez * Ki * Q_dt; - - float kp_dynamic = Kp; // 默认使用全局Kp - - // // 计算重力向量与Z轴的夹角余弦值 - // // 当设备接近水平时，abs_az_component 接近 1 - // // 当设备接近垂直时，abs_az_component 接近 0 - // float abs_az_component = fabsf(az); - - // // 设置一个阈值，比如当与水平面的夹角大于75度时 (cos(75) approx 0.26) - // // 就开始降低Kp - // if (abs_az_component < 0.26f) { - // // 线性降低Kp，或者直接使用一个较小的值 - // // 越接近垂直，az越小，Kp也越小 - // kp_dynamic = Kp * (abs_az_component / 0.26f); - // } - - // 使用PI控制器校正陀螺仪的测量值 - gx += kp_dynamic * ex + exInt; - gy += kp_dynamic * ey + eyInt; - gz += kp_dynamic * ez + ezInt; - - - // gx = gx + Kp * ex + exInt; - // gy = gy + Kp * ey + eyInt; - // gz = gz + Kp * ez + ezInt; - } - - // 使用校正后的角速度更新四元数 (一阶毕卡法) - q0 = q0 + (-q1 * gx - q2 * gy - q3 * gz) * 0.5f * Q_dt; - q1 = q1 + (q0 * gx + q2 * gz - q3 * gy) * 0.5f * Q_dt; - q2 = q2 + (q0 * gy - q1 * gz + q3 * gx) * 0.5f * Q_dt; - q3 = q3 + (q0 * gz + q1 * gy - q2 * gx) * 0.5f * Q_dt; - - // 归一化四元数，保持其单位长度 - norm = sqrtf(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3); - q0 = q0 / norm; - q1 = q1 / norm; - q2 = q2 / norm; - q3 = q3 / norm; - - // --- 2.4 将四元数转换为欧拉角 (Pitch, Roll, Yaw) --- - // 转换公式将四元数转换为 ZYX 顺序的欧拉角 - // 结果单位为弧度，乘以 57.29578f 转换为度 - - // Pitch (绕Y轴旋转) - Angle_output[0] = asinf(-2.0f * (q1 * q3 - q0 * q2)) * 57.29578f; - - // Roll (绕X轴旋转) - Angle_output[1] = atan2f(2.0f * (q0 * q1 + q2 * q3), q0 * q0 - q1 * q1 - q2 * q2 + q3 * q3) * 57.29578f; - - // Yaw (绕Z轴旋转) - 注意：无磁力计的6轴IMU，Yaw角会随时间漂移 - Angle_output[2] = atan2f(2.0f * (q1 * q2 + q0 * q3), q0 * q0 + q1 * q1 - q2 * q2 - q3 * q3) * 57.29578f; - - - if (quaternion_output != NULL) { - quaternion_output[0] = q0; // w - quaternion_output[1] = q1; // x - quaternion_output[2] = q2; // y - quaternion_output[3] = q3; // z - } - - return 1; // 返回1，表示计算成功 - } - - return 2; // 校准未完成，返回错误状态 -#endif -} - -#endif - -/////////////////////////////////////////////////////////////////////////////////////////////////// - -#if 0 -// --- 新增：定义自动化校准状态 --- -typedef enum { - CAL_STATE_IDLE, // 0: 空闲，未开始校准 - CAL_STATE_WAIT_STILL, // 1: 等待设备静止 - CAL_STATE_COLLECTING, // 2: 正在采集数据 - CAL_STATE_PAUSE_BETWEEN_FACES, // 3: 面与面之间的暂停 - CAL_STATE_CALCULATING, // 4: 正在计算最终参数 - CAL_STATE_FINISHED, // 5: 校准完成，参数有效 -} SL_AutoCalib_State; - -// --- 新增：定义校准的面 --- -typedef enum { - CAL_FACE_POS_Z = 0, // Z+ - CAL_FACE_NEG_Z, // Z- - CAL_FACE_POS_Y, // Y+ - CAL_FACE_NEG_Y, // Y- - CAL_FACE_POS_X, // X+ - CAL_FACE_NEG_X, // X- - CAL_FACE_COUNT // 总面数 -} SL_Calib_Face; - -// --- 校准参数常量 --- -#define CAL_STILL_TIME_THRESHOLD 190 // 需要静止的采样周期数 (约1.9s) -#define CAL_SAMPLES_TO_COLLECT 50 // 每个面采集的样本数 -#define CAL_PAUSE_SECONDS 4 // 面间暂停秒数 - -// --- 修改/新增 全局变量 --- -static SL_AutoCalib_State g_calib_state = CAL_STATE_IDLE; -static SL_Calib_Face g_current_calib_face = CAL_FACE_POS_Z; -static unsigned short g_still_count = 0; -static unsigned char g_samples_collected = 0; -static unsigned short g_pause_timer = 0; - -static long g_calib_data_sum[6] = {0}; -static signed short g_calib_avg_data[CAL_FACE_COUNT][6] = {{0}}; - -static float g_acc_offset[3] = {0.0f}; -static float g_acc_scale[3] = {1.0f, 1.0f, 1.0f}; -static signed short g_gyro_offset[3] = {0}; // 陀螺仪偏移用整数更符合原始数据类型 - -static unsigned char g_calibration_valid = 0; - - - -#if (ACC_RANGE == 2) -#define G_VALUE (16384.0f) -#elif (ACC_RANGE == 4) -#define G_VALUE (8192.0f) -#elif (ACC_RANGE == 8) -#define G_VALUE (4096.0f) -#elif (ACC_RANGE == 16) -#define G_VALUE (2048.0f) -#endif -/** - * @brief IMU姿态解算函数，增加了六面校准功能。 - * @param calibration_cmd 校准命令，来自 SL_Calibration_Cmd 枚举。 - * @param acc_gyro_input 指向6轴原始数据的指针 (AccX,Y,Z, GyroX,Y,Z)。 - * @param Angle_output 指向3轴姿态角输出的指针 (Pitch, Roll, Yaw)。 - * @param yaw_rst Yaw轴复位标志。 - * @return unsigned char 0: 正在校准, 1: 计算成功, 2: 校准未完成，无法计算。 - */ -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 sl_i = 0; - - // 启动自动化校准流程 - if (auto_calib_start == 1 && g_calib_state == CAL_STATE_IDLE) { - g_calib_state = CAL_STATE_WAIT_STILL; - g_current_calib_face = CAL_FACE_POS_Z; - g_calibration_valid = 0; - xlog("\n\n===== Auto-Calibration Started =====\r\n"); - xlog("Step 1/%d: Place device with Z-axis pointing UPWARD and keep it still.\r\n", CAL_FACE_COUNT); - } - - // ================================================================================= - // 步骤 1: 自动化校准状态机 - // --------------------------------------------------------------------------------- - if (g_calib_state != CAL_STATE_IDLE && g_calib_state != CAL_STATE_FINISHED) { - // --- 静止检测 (通用逻辑) --- - unsigned short acc_delta = 0, gyro_delta = 0; - for (sl_i = 0; sl_i < 3; sl_i++) { - acc_delta += SL_GetAbsShort(acc_gyro_input[sl_i] - Temp_Accgyro[sl_i]); - gyro_delta += 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]; - - int is_still = (acc_delta < 160) && (gyro_delta < 40); - - switch (g_calib_state) { - case CAL_STATE_WAIT_STILL: - if (is_still) { - if (++g_still_count >= CAL_STILL_TIME_THRESHOLD) { - g_calib_state = CAL_STATE_COLLECTING; - g_samples_collected = 0; - g_still_count = 0; - for(sl_i = 0; sl_i < 6; sl_i++) g_calib_data_sum[sl_i] = 0; - xlog("Device is still. Collecting data...\r\n"); - } - } else { - g_still_count = 0; // 如果移动则重置计数 - } - break; - - case CAL_STATE_COLLECTING: - if (!is_still) { // 如果在采集中移动了，则重新开始等待静止 - g_calib_state = CAL_STATE_WAIT_STILL; - g_still_count = 0; - xlog("Movement detected! Please keep the device still.\r\n"); - break; - } - - if (g_samples_collected < CAL_SAMPLES_TO_COLLECT) { - for (sl_i = 0; sl_i < 6; sl_i++) { - g_calib_data_sum[sl_i] += acc_gyro_input[sl_i]; - } - g_samples_collected++; - } else { - // 当前面采集完成 - for (sl_i = 0; sl_i < 6; sl_i++) { - g_calib_avg_data[g_current_calib_face][sl_i] = g_calib_data_sum[sl_i] / CAL_SAMPLES_TO_COLLECT; - } - xlog("Face %d data collected.\r\n", g_current_calib_face + 1); - - if (g_current_calib_face < CAL_FACE_NEG_X) { - g_calib_state = CAL_STATE_PAUSE_BETWEEN_FACES; - g_pause_timer = 0; - xlog("Pausing for %d seconds. Please prepare for the next orientation.\r\n", CAL_PAUSE_SECONDS); - } else { - // 所有面都已完成 - g_calib_state = CAL_STATE_CALCULATING; - } - } - break; - - case CAL_STATE_PAUSE_BETWEEN_FACES: - if (++g_pause_timer >= (CAL_PAUSE_SECONDS * 1000 / dt)) { - g_current_calib_face++; - g_calib_state = CAL_STATE_WAIT_STILL; - g_still_count = 0; - switch(g_current_calib_face) { - case CAL_FACE_NEG_Z: xlog("Step 2/%d: Place device with Z-axis pointing DOWNWARD and keep it still.\r\n", CAL_FACE_COUNT); break; - case CAL_FACE_POS_Y: xlog("Step 3/%d: Place device with Y-axis pointing UPWARD and keep it still.\r\n", CAL_FACE_COUNT); break; - case CAL_FACE_NEG_Y: xlog("Step 4/%d: Place device with Y-axis pointing DOWNWARD and keep it still.\r\n", CAL_FACE_COUNT); break; - case CAL_FACE_POS_X: xlog("Step 5/%d: Place device with X-axis pointing UPWARD and keep it still.\r\n", CAL_FACE_COUNT); break; - case CAL_FACE_NEG_X: xlog("Step 6/%d: Place device with X-axis pointing DOWNWARD and keep it still.\r\n", CAL_FACE_COUNT); break; - } - } - break; - - case CAL_STATE_CALCULATING: - xlog("All data collected. Calculating calibration parameters...\r\n"); - // 计算陀螺仪偏移 - long gyro_sum[3] = {0}; - for (sl_i = 0; sl_i < CAL_FACE_COUNT; sl_i++) { - gyro_sum[0] += g_calib_avg_data[sl_i][3]; // Gx - gyro_sum[1] += g_calib_avg_data[sl_i][4]; // Gy - gyro_sum[2] += g_calib_avg_data[sl_i][5]; // Gz - } - g_gyro_offset[0] = gyro_sum[0] / CAL_FACE_COUNT; - g_gyro_offset[1] = gyro_sum[1] / CAL_FACE_COUNT; - g_gyro_offset[2] = gyro_sum[2] / CAL_FACE_COUNT; - - // 计算加速度计偏移和增益 - g_acc_offset[0] = (g_calib_avg_data[CAL_FACE_POS_X][0] + g_calib_avg_data[CAL_FACE_NEG_X][0]) / 2.0f; - g_acc_scale[0] = (2.0f * G_VALUE) / (g_calib_avg_data[CAL_FACE_POS_X][0] - g_calib_avg_data[CAL_FACE_NEG_X][0]); - g_acc_offset[1] = (g_calib_avg_data[CAL_FACE_POS_Y][1] + g_calib_avg_data[CAL_FACE_NEG_Y][1]) / 2.0f; - g_acc_scale[1] = (2.0f * G_VALUE) / (g_calib_avg_data[CAL_FACE_POS_Y][1] - g_calib_avg_data[CAL_FACE_NEG_Y][1]); - g_acc_offset[2] = (g_calib_avg_data[CAL_FACE_POS_Z][2] + g_calib_avg_data[CAL_FACE_NEG_Z][2]) / 2.0f; - g_acc_scale[2] = (2.0f * G_VALUE) / (g_calib_avg_data[CAL_FACE_POS_Z][2] - g_calib_avg_data[CAL_FACE_NEG_Z][2]); - - g_calibration_valid = 1; - g_calib_state = CAL_STATE_FINISHED; - xlog("===== Calibration Finished! =====\r\n"); - xlog("Acc Offset: %.2f, %.2f, %.2f\r\n", g_acc_offset[0], g_acc_offset[1], g_acc_offset[2]); - xlog("Acc Scale: %.4f, %.4f, %.4f\r\n", g_acc_scale[0], g_acc_scale[1], g_acc_scale[2]); - xlog("Gyro Offset: %d, %d, %d\r\n", g_gyro_offset[0], g_gyro_offset[1], g_gyro_offset[2]); - break; - default: - break; - } - return 0; // 只要还在校准流程中，就返回0 - } - - - // ================================================================================= - // 步骤 2: 姿态解算 - // --------------------------------------------------------------------------------- - if (g_calibration_valid == 0) { - return 2; // 校准未完成，无法进行姿态解算 - } - - float angle_acc[3] = {0}; - float gyro_val[3] = {0}; - float calibrated_acc[3] = {0}; - - // --- 2.1 数据预处理 (应用新的校准参数) --- - // 应用偏移和增益校准加速度计 - calibrated_acc[0] = ((float)acc_gyro_input[0] - g_acc_offset[0]) * g_acc_scale[0]; - calibrated_acc[1] = ((float)acc_gyro_input[1] - g_acc_offset[1]) * g_acc_scale[1]; - calibrated_acc[2] = ((float)acc_gyro_input[2] - g_acc_offset[2]) * g_acc_scale[2]; - - // 应用偏移校准陀螺仪 - gyro_val[0] = ((float)acc_gyro_input[4] - g_gyro_offset[1]) * 0.061; // GYR-Y -> Pitch - gyro_val[1] = ((float)acc_gyro_input[3] - g_gyro_offset[0]) * 0.061; // GYR-X -> Roll - gyro_val[2] = ((float)acc_gyro_input[5] - g_gyro_offset[2]) * 0.061; // GYR-Z -> Yaw - - // --- 2.2 使用校准后的加速度计计算姿态角 --- - // 将校准后的加速度值转换为归一化的重力分量 - angle_acc[0] = calibrated_acc[0] / G_VALUE; // ax - angle_acc[1] = calibrated_acc[1] / G_VALUE; // ay - angle_acc[2] = calibrated_acc[2] / G_VALUE; // az - - // 限制范围，防止asinf/atanf计算错误 - if (angle_acc[0] > 1.0f) angle_acc[0] = 1.0f; - if (angle_acc[0] < -1.0f) angle_acc[0] = -1.0f; - // ... (对 angle_acc[1] 和 angle_acc[2] 也做同样处理) - - angle_acc[0] = asinf(angle_acc[0]) * 57.29578f; // Pitch - angle_acc[1] = atan2f(angle_acc[1], angle_acc[2]) * 57.29578f; // Roll (使用atan2更稳健) - - // ================================================================================= - // 步骤 2.4: 卡尔曼滤波 - // 对Pitch和Roll分别进行滤波 - // --------------------------------------------------------------------------------- - - /************** Pitch 轴滤波 **************/ - // --- 预测步骤 --- - // 1. 预测状态：根据上一时刻的角度和当前角速度，预测当前角度 - angle0[0] += (gyro_val[0] - q_bias0[0]) * dt; - // 2. 预测协方差：更新P矩阵，表示预测状态的不确定性 - Pdot0[0] = Q_angle - P0[0][1] - P0[1][0] + P0[1][1] * dt; - Pdot0[1] = -P0[1][1]; - Pdot0[2] = -P0[1][1]; - Pdot0[3] = Q_gyro; - P0[0][0] += Pdot0[0] * dt; - P0[0][1] += Pdot0[1] * dt; - P0[1][0] += Pdot0[2] * dt; - P0[1][1] += Pdot0[3] * dt; - - // --- 更新步骤 --- - // 1. 计算卡尔曼增益 K - PCt0_0[0] = C_0 * P0[0][0]; - PCt0_1[0] = C_0 * P0[1][0]; - E0[0] = R_angle + C_0 * PCt0_0[0]; - if (E0[0] == 0) { E0[0] = 0.0001; } // 防止除零 - K0_0[0] = PCt0_0[0] / E0[0]; - K0_1[0] = PCt0_1[0] / E0[0]; - - // 2. 计算测量余差（innovation） - angle_err0[0] = angle_acc[0] - angle0[0]; - // 3. 更新状态估计：结合预测值和测量值，得到最优估计 - angle0[0] += K0_0[0] * angle_err0[0]; - // 4. 更新陀螺仪偏置估计 - q_bias0[0] += K0_1[0] * angle_err0[0]; - angle_dot0[0] = gyro_val[0] - q_bias0[0]; - - // 5. 更新协方差矩阵 P - t0_0[0] = PCt0_0[0]; - t0_1[0] = C_0 * P0[0][1]; - P0[0][0] -= K0_0[0] * t0_0[0]; - P0[0][1] -= K0_0[0] * t0_1[0]; - P0[1][0] -= K0_1[0] * t0_0[0]; - P0[1][1] -= K0_1[0] * t0_1[0]; - - // 输出最终的Pitch角 - Angle_output[0] = angle0[0]; - - /************** Roll 轴滤波 (过程同Pitch) **************/ - // --- 预测步骤 --- - angle0[1] += (gyro_val[1] - q_bias0[1]) * dt; - Pdot1[0] = Q_angle - P1[0][1] - P1[1][0] + P1[1][1] * dt; - Pdot1[1] = -P1[1][1]; - Pdot1[2] = -P1[1][1]; - Pdot1[3] = Q_gyro; - P1[0][0] += Pdot1[0] * dt; - P1[0][1] += Pdot1[1] * dt; - P1[1][0] += Pdot1[2] * dt; - P1[1][1] += Pdot1[3] * dt; - - // --- 更新步骤 --- - PCt0_0[1] = C_1 * P1[0][0]; - PCt0_1[1] = C_1 * P1[1][0]; - E0[1] = R_angle + C_1 * PCt0_0[1]; - if (E0[1] == 0) { E0[1] = 0.0001; } - K0_0[1] = PCt0_0[1] / E0[1]; - K0_1[1] = PCt0_1[1] / E0[1]; - angle_err0[1] = angle_acc[1] - angle0[1]; - angle0[1] += K0_0[1] * angle_err0[1]; - q_bias0[1] += K0_1[1] * angle_err0[1]; - angle_dot0[1] = gyro_val[1] - q_bias0[1]; - t0_0[1] = PCt0_0[1]; - t0_1[1] = C_1 * P1[0][1]; - P1[0][0] -= K0_0[1] * t0_0[1]; - P1[0][1] -= K0_0[1] * t0_1[1]; - P1[1][0] -= K0_1[1] * t0_0[1]; - P1[1][1] -= K0_1[1] * t0_1[1]; - - // 输出最终的Roll角 - Angle_output[1] = angle0[1]; - - /************** Yaw 轴计算 **************/ - // Yaw角无法通过加速度计（重力）来校正，因此这里只使用陀螺仪进行简单积分。 - // 这种方法会因为陀螺仪的漂移而导致误差随时间累积。 - if (yaw_rst == 1) { - Angle_output[2] = 0; // 如果有复位信号，则清零 - } - - // 增加一个简单的阈值，当角速度较小时，认为没有转动，以减少漂移 - if (SL_GetAbsShort(Temp_Accgyro[5]) > 8) { - Angle_output[2] += gyro_val[2] * dt; - } - - return 1; // 返回1，表示计算成功 -} -#endif \ No newline at end of file diff --git a/apps/earphone/xtell_Sensor/sensor/SC7U22.h b/apps/earphone/xtell_Sensor/sensor/SC7U22.h deleted file mode 100644 index e33a056..0000000 --- a/apps/earphone/xtell_Sensor/sensor/SC7U22.h +++ /dev/null @@ -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__ diff --git a/apps/earphone/xtell_Sensor/sensor/SC7U22_Ma.c b/apps/earphone/xtell_Sensor/sensor/SC7U22_Ma.c deleted file mode 100644 index 502b9da..0000000 --- a/apps/earphone/xtell_Sensor/sensor/SC7U22_Ma.c +++ /dev/null @@ -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, ®_value); -#if SL_Sensor_Algo_Release_Enable==0x00 - printf("0x%x=0x%x\r\n",SC7U22_WHO_AM_I,reg_value); -#endif - if(reg_value==0x6A) - return 0x01;//SC7U22 - else - return 0x00;//通信异常 -} - -unsigned char SL_SC7U22_Config(void) -{ - unsigned char Check_Flag=0; - unsigned char reg_value=0; - -#if SL_SPI_IIC_INTERFACE==0x00 //SPI - SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x7F, 0x00);//goto 0x90 - sl_delay(1); - SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x4A, 0x66); - sl_delay(1); - SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x7F, 0x83);//goto 0x6F - sl_delay(1); - SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x6F, 0x04);//I2C disable - sl_delay(1); - SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x7F, 0x00);//goto 0x6F - sl_delay(1); - SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x4A, 0x00); - sl_delay(1); -#endif - Check_Flag=SL_SC7U22_Check(); -// Check_Flag= SL_SC7U22_SOFT_RESET(); -// Check_Flag=1;//强制初始化 -#if SL_Sensor_Algo_Release_Enable==0x00 - printf("SL_SC7U22_Check=0x%x\r\n",Check_Flag); -#endif - if(Check_Flag==1) - { - Check_Flag= SL_SC7U22_POWER_DOWN(); - } -#if SL_Sensor_Algo_Release_Enable==0x00 - printf("SL_SC7U22_POWER_DOWN=0x%x\r\n",Check_Flag); -#endif - if(Check_Flag==1) - { - Check_Flag= SL_SC7U22_SOFT_RESET(); - } -#if SL_Sensor_Algo_Release_Enable==0x00 - printf("SL_SC7U22_SOFT_RESET=0x%x\r\n",Check_Flag); -#endif - if(Check_Flag==1) - { - SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x7F, 0x00);//goto 0x00 - os_time_dly(1);//10ms - SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x7D, 0x0E);//PWR_CTRL ENABLE ACC+GYR+TEMP - os_time_dly(1);//10ms - - SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x40, 0x08);//ACC_CONF 0x08=100Hz - SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x41, 0x01);//ACC_RANGE ±4G - SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x42, 0x88);//GYR_CONF 0x88=100Hz - SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x43, 0x00);//GYR_RANGE 2000dps - SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x04, 0x50);//COM_CFG - -#if SL_SC7U22_RAWDATA_HPF_ENABLE ==0x01 - SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE,0x7F, 0x83);//goto 0x83 - sl_delay(1); - SL_SC7U22_I2c_Spi_Read(SL_SPI_IIC_INTERFACE, 0x26, 1, ®_value); - reg_value=reg_value|0xA0; - SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x26, reg_value);//HPF_CFG rawdata hpf -#endif - -#if SL_SC7U22_AOI_Wake_Up_ENABLE==0x01 - SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x30, 0x2A);//XYZ-ENABLE - SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x32, 0x01);//VTH - SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x33, 0x01);//TTH - SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x3F, 0x30);//HPF FOR AOI1&AOI2 -#endif - -#if SL_SC7U22_FIFO_ENABLE==0x01 - SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x1E,0x1D);// - SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x1D,0x00);// - SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x1D,0x20);// - SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x1C,0x37);// -#endif - - -#if SL_SC7U22_SDO_PullUP_ENABLE ==0x01 - SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE,0x7F, 0x8C);//goto 0x8C - sl_delay(1); - SL_SC7U22_I2c_Spi_Read(SL_SPI_IIC_INTERFACE, 0x30, 1, ®_value); - reg_value=reg_value&0xFE;//CS PullUP_enable - reg_value=reg_value&0xFD;//SDO PullUP_enable - SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x30, reg_value); - - SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE,0x7F, 0x00);//goto 0x00 - os_time_dly(1); - SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE,0x7F, 0x00);//goto 0x00 - os_time_dly(1); -#else - SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE,0x7F, 0x8C);//goto 0x8C - sl_delay(1); - SL_SC7U22_I2c_Spi_Read(SL_SPI_IIC_INTERFACE, 0x30, 1, ®_value); - reg_value=reg_value&0xFE;//CS PullUP_enable - reg_value=reg_value|0x02;//SDO PullUP_disable - SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x30, reg_value); - - SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE,0x7F, 0x00);//goto 0x00 - sl_delay(1); - SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE,0x7F, 0x00);//goto 0x00 - sl_delay(1); -#endif - - return 1; - } - else - return 0; -} - - -//读取时间戳 -unsigned int SL_SC7U22_TimeStamp_Read(void) -{ - unsigned char time_data[3]; - unsigned int time_stamp; - - SL_SC7U22_I2c_Spi_Read(SL_SPI_IIC_INTERFACE, 0x18, 1, &time_data[0]); - SL_SC7U22_I2c_Spi_Read(SL_SPI_IIC_INTERFACE, 0x19, 1, &time_data[1]); - SL_SC7U22_I2c_Spi_Read(SL_SPI_IIC_INTERFACE, 0x20, 1, &time_data[2]); - - time_stamp=(unsigned int)(time_data[0]<<16|time_data[1]<<8|time_data[2]); - - return time_stamp; -} - -#if SL_SC7U22_FIFO_ENABLE ==0x00 -//100Hz 10ms read once -void SL_SC7U22_RawData_Read(signed short * acc_data_buf,signed short * gyr_data_buf) -{ - unsigned char raw_data[12]; - unsigned char drdy_satus=0x00; - unsigned short drdy_cnt=0; - - while((drdy_satus&0x03)!=0x03)//acc+gyro -// while((drdy_satus&0x01)!=0x01)//acc - { - drdy_satus=0x00; - sl_delay(1); - SL_SC7U22_I2c_Spi_Read(SL_SPI_IIC_INTERFACE, 0x0B, 1, &drdy_satus); - drdy_cnt++; - if(drdy_cnt>30000) break; - } - -#if SL_Sensor_Algo_Release_Enable==0x00 -// SL_SC7U22_I2c_Spi_Read(SL_SPI_IIC_INTERFACE, 0x30, 1, &drdy_satus); -// printf("RawData:0x40=%x\r\n",drdy_satus); -// SL_SC7U22_I2c_Spi_Read(SL_SPI_IIC_INTERFACE, 0x40, 1, &drdy_satus); -// printf("RawData:0x40=%x\r\n",drdy_satus); -// SL_SC7U22_I2c_Spi_Read(SL_SPI_IIC_INTERFACE, 0x06, 1, &drdy_satus); -// printf("RawData:0x06=%x\r\n",drdy_satus); -// SL_SC7U22_I2c_Spi_Read(SL_SPI_IIC_INTERFACE, 0x07, 1, &drdy_satus); -// printf("RawData:0x07=%x\r\n",drdy_satus); -// SL_SC7U22_I2c_Spi_Read(SL_SPI_IIC_INTERFACE, 0x7D, 1, &drdy_satus); -// printf("RawData:0x7D=%x\r\n",drdy_satus); -// SL_SC7U22_I2c_Spi_Read(SL_SPI_IIC_INTERFACE, 0x31, 1, &drdy_satus); -// printf("RawData:0x31=%x\r\n",drdy_satus); -// SL_SC7U22_I2c_Spi_Read(SL_SPI_IIC_INTERFACE, 0x02, 1, &drdy_satus); -// printf("RawData:0x02=%x\r\n",drdy_satus); -// SL_SC7U22_I2c_Spi_Read(SL_SPI_IIC_INTERFACE, 0x03, 1, &drdy_satus); -// printf("RawData:0x03=%x\r\n",drdy_satus); -#endif - - SL_SC7U22_I2c_Spi_Read(SL_SPI_IIC_INTERFACE, 0x0C, 12, &raw_data[0]); - - acc_data_buf[0] =(signed short)((((unsigned char)raw_data[0])* 256) + ((unsigned char)raw_data[1]));//ACCX-16位 - acc_data_buf[1] =(signed short)((((unsigned char)raw_data[2])* 256) + ((unsigned char)raw_data[3]));//ACCY-16位 - acc_data_buf[2] =(signed short)((((unsigned char)raw_data[4])* 256) + ((unsigned char)raw_data[5]));//ACCZ-16位 - gyr_data_buf[0] =(signed short)((((unsigned char)raw_data[6])* 256) + ((unsigned char)raw_data[7]));//GYRX-16位 - gyr_data_buf[1] =(signed short)((((unsigned char)raw_data[8])* 256) + ((unsigned char)raw_data[9]));//GYRY-16位 - gyr_data_buf[2] =(signed short)((((unsigned char)raw_data[10])* 256) + ((unsigned char)raw_data[11]));//GYRZ-16位 - -#if SL_Sensor_Algo_Release_Enable==0x00 - printf("RawData:AX=%d,AY=%d,AZ=%d,GX=%d,GY=%d,GZ=%d\r\n",acc_data_buf[0],acc_data_buf[1],acc_data_buf[2],gyr_data_buf[0],gyr_data_buf[1],gyr_data_buf[2]); -#endif - -} -#else - -#if SL_Sensor_Algo_Release_Enable==0x00 -#define SL_SC7U22_WAIT_FIFO_LEN_ENABLE 0x00//0x01 -#else -#define SL_SC7U22_WAIT_FIFO_LEN_ENABLE 0x00 -#endif -unsigned char Acc_FIFO_Num; -unsigned char Gyr_FIFO_Num; - -unsigned char SL_SC7U22_FIFO_DATA[1024]; - -unsigned short SL_SC7U22_FIFO_Read(signed short *accx_buf,signed short *accy_buf,signed short *accz_buf,signed short *gyrx_buf,signed short *gyry_buf,signed short *gyrz_buf) -{ - int16_t Acc_x = 0, Acc_y = 0, Acc_z = 0; - int16_t Gyr_x = 0, Gyr_y = 0, Gyr_z = 0; - unsigned char fifo_num1=0; - unsigned char fifo_num2=0; - unsigned short fifo_num=0; - unsigned short fifo_len=0; - unsigned short temp = 0; - unsigned short i = 0 ; - unsigned char header[2]; - unsigned short j; - -#if SL_Sensor_Algo_Release_Enable==0x00 //user can set to zero -#if SL_SC7U22_WAIT_FIFO_LEN_ENABLE==0x00 - while((fifo_num1&0x20)!=0x20) - { - sl_delay(200); - SL_SC7U22_I2c_Spi_Read(SL_SPI_IIC_INTERFACE, 0x1F,1,&fifo_num1); - } - SL_SC7U22_I2c_Spi_Read(SL_SPI_IIC_INTERFACE, 0x1F,1,&fifo_num1); - SL_SC7U22_I2c_Spi_Read(SL_SPI_IIC_INTERFACE, 0x20,1,&fifo_num2); - if((fifo_num1&0x10)==0x10) - { - fifo_num=2048; - } - else - { - fifo_num=(fifo_num1&0x0F)*256+fifo_num2; - } -#else - while(fifo_num2<194)//32 - { - SL_SC7U22_I2c_Spi_Read(SL_SPI_IIC_INTERFACE, 0x1F,1,&fifo_num1); - SL_SC7U22_I2c_Spi_Read(SL_SPI_IIC_INTERFACE, 0x20,1,&fifo_num2); - sl_delay(20); - fifo_wait++; - if(fifo_wait>30000) break; - } - fifo_wait=0; - fifo_num=fifo_num2; -#endif - -#else - SL_SC7U22_I2c_Spi_Read(SL_SPI_IIC_INTERFACE, 0x1F,1,&fifo_num1); - SL_SC7U22_I2c_Spi_Read(SL_SPI_IIC_INTERFACE, 0x20,1,&fifo_num2); - if((fifo_num1&0x10)==0x10) - { - fifo_num=2048; - } - else - { - fifo_num=(fifo_num1&0x0F)*256+fifo_num2; - } -#endif - - SL_SC7U22_I2c_Spi_Read(SL_SPI_IIC_INTERFACE, 0x21, fifo_num*2, SL_SC7U22_FIFO_DATA);//读取FIFO数据 BYTE NUM -// SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x1D, 0x00);//BY PASS MODE -// SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x1D, 0x20);//Stream MODE - printf("SC7U22_FIFO_NUM1:%d\n",fifo_num); -#if SL_Sensor_Algo_Release_Enable==0x00 -// printf("0x1F:0x%x 0x20:0x%x\n",fifo_num1,fifo_num2); -// printf("SC7U22_FIFO_NUM1:%d\n",fifo_num); -#endif - fifo_len=0; - - i = 0; - Acc_FIFO_Num=0; - Gyr_FIFO_Num=0; - while(i < fifo_num*2) - { - //header process 1 - header[0] = SL_SC7U22_FIFO_DATA[i + 0]; - header[1] = SL_SC7U22_FIFO_DATA[i + 1]; - i = i + 2; - - //timestamp process 2 - if(header[1] & 0x80) - { - i = i + 4;//every frame include the timestamp, 4 bytes - } - //acc process 3 - if(header[0] & 0x04) - { - accx_buf[Acc_FIFO_Num] = ((s16)(SL_SC7U22_FIFO_DATA[i + 0] * 256 + SL_SC7U22_FIFO_DATA[i + 1])) ; - accy_buf[Acc_FIFO_Num] = ((s16)(SL_SC7U22_FIFO_DATA[i + 2] * 256 + SL_SC7U22_FIFO_DATA[i + 3])) ; - accz_buf[Acc_FIFO_Num] = ((s16)(SL_SC7U22_FIFO_DATA[i + 4] * 256 + SL_SC7U22_FIFO_DATA[i + 5])) ; - printf("AccNum : %d ,Acc_x : %4d, Acc_y : %4d, Acc_z : %4d,\r\n",Acc_FIFO_Num, accx_buf[Acc_FIFO_Num], accy_buf[Acc_FIFO_Num], accz_buf[Acc_FIFO_Num]); - i = i + 6; - Acc_FIFO_Num++; - } - //gyro process 3 - if(header[0] & 0x02) - { - gyrx_buf[Gyr_FIFO_Num] = ((s16)(SL_SC7U22_FIFO_DATA[i + 0] * 256 + SL_SC7U22_FIFO_DATA[i + 1])) ; - gyry_buf[Gyr_FIFO_Num] = ((s16)(SL_SC7U22_FIFO_DATA[i + 2] * 256 + SL_SC7U22_FIFO_DATA[i + 3])) ; - gyrz_buf[Gyr_FIFO_Num] = ((s16)(SL_SC7U22_FIFO_DATA[i + 4] * 256 + SL_SC7U22_FIFO_DATA[i + 5])) ; - printf("GyrNum : %d, Gyr_x : %4d, Gyr_y : %4d, Gyr_z : %4d,\r\n",Gyr_FIFO_Num, gyrx_buf[Gyr_FIFO_Num], gyry_buf[Gyr_FIFO_Num], gyrz_buf[Gyr_FIFO_Num]); - i = i + 6; - Gyr_FIFO_Num++; - } - - //temperature process 1 - if(header[0] & 0x01) - { - i = i + 2; - } - } - - - return fifo_len; -} -#endif - - -unsigned char SL_SC7U22_POWER_DOWN(void) -{ - unsigned char SL_Read_Reg = 0xff; - - SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x7F, 0x00);//goto 0x00 - sl_delay(20); - SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x7D, 0x00);//POWER DOWN - sl_delay(200); - SL_SC7U22_I2c_Spi_Read(SL_SPI_IIC_INTERFACE, 0x7D, 1,&SL_Read_Reg); - if(SL_Read_Reg==0x00) return 1; - else return 0; -} - - -unsigned char SL_SC7U22_SOFT_RESET(void) -{ - unsigned char SL_Read_Reg = 0xff; - - SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x7F, 0x00);//goto 0x00 - os_time_dly(1); -#if SL_Sensor_Algo_Release_Enable==0x00 - SL_SC7U22_I2c_Spi_Read(SL_SPI_IIC_INTERFACE, 0x04, 1,&SL_Read_Reg); - printf("SL_SC7U22_SOFT_RESET1 0x04=0x%x\r\n",SL_Read_Reg); - SL_Read_Reg = 0xff; -#endif - SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x04, 0x10);//BOOT -#if SL_Sensor_Algo_Release_Enable==0x00 -#endif - SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x4A, 0xA5);//SOFT_RESET - SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x4A, 0xA5);//SOFT_RESET - os_time_dly(20); - SL_SC7U22_I2c_Spi_Read(SL_SPI_IIC_INTERFACE, 0x04, 1,&SL_Read_Reg); -#if SL_Sensor_Algo_Release_Enable==0x00 - printf("SL_SC7U22_SOFT_RESET2 0x08=0x%x\r\n",SL_Read_Reg); -#endif - if(SL_Read_Reg==0x50) return 1; - else return 0; - -} - - -/****acc_enable ==0 close acc;acc_enable ==1 open acc******/ -/****gyro_enable==0 close acc;gyro_enable==1 open acc******/ -unsigned char SL_SC7U22_Open_Close_SET(unsigned char acc_enable,unsigned char gyro_enable) -{ - unsigned char SL_Read_Reg = 0xff; - unsigned char SL_Read_Check= 0xff; - - SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x7F, 0x00);//goto 0x00 - sl_delay(1); - SL_SC7U22_I2c_Spi_Read(SL_SPI_IIC_INTERFACE, 0x7D, 1,&SL_Read_Reg); - - if(acc_enable==0) - { - SL_Read_Reg=SL_Read_Reg&0xFB;//Bit.ACC_EN=0 - } - else if(acc_enable==1) - { - SL_Read_Reg=SL_Read_Reg|0x04;//Bit.ACC_EN=1 - } - if(gyro_enable==0) - { - SL_Read_Reg=SL_Read_Reg&0xFD;//Bit.GYR_EN=0 - } - else if(gyro_enable==1) - { - SL_Read_Reg=SL_Read_Reg|0x02;//Bit.GYR_EN=1 - } - SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x7D, SL_Read_Reg);//PWR_CTRL ENABLE ACC+GYR+TEMP - sl_delay(5);//5ms - SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x7D, SL_Read_Reg);//PWR_CTRL ENABLE ACC+GYR+TEMP - sl_delay(20);//10ms - SL_SC7U22_I2c_Spi_Read(SL_SPI_IIC_INTERFACE, 0x7D, 1,&SL_Read_Check); - if(SL_Read_Reg!=SL_Read_Check) - { -#if SL_Sensor_Algo_Release_Enable==0x00 - printf("SL_Read_Reg=0x%x SL_Read_Check=0x%x\r\n",SL_Read_Reg,SL_Read_Check); -#endif - return 0; - } - return 1; -} - - -/*******开启中断******/ -unsigned char SL_SC7U22_IN_SLEEP_SET(unsigned char acc_odr,unsigned char vth,unsigned char tth,unsigned char int_io) -{ - unsigned char SL_Read_Reg = 0xff; - unsigned char SL_Acc_Odr_Reg = 0xff; - - SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x7F, 0x00);//goto 0x00 - sl_delay(1); - if(int_io==1) - { - SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x06, 0x02);//AOI1-INT1 - } - else if(int_io==2) - { - SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x08, 0x02);//AOI1-INT2 - } - - SL_SC7U22_I2c_Spi_Read(SL_SPI_IIC_INTERFACE, 0x04, 1, &SL_Read_Reg); -#if SL_SC7U22_INT_DEFAULT_LEVEL ==0x01 - SL_Read_Reg=SL_Read_Reg|0x04; - SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x04, SL_Read_Reg);//defalut high level&& push-pull -#else - reg_value=reg_value&0xDF; - SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x06, SL_Read_Reg);//defalut low level&& push-pull -#endif - SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x30, 0x2A);//AIO1-Enable - SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x32, vth);//VTH - SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x33, tth);//TTH - - if(acc_odr==12) - { - SL_Acc_Odr_Reg=0x05; - } - else if(acc_odr==25) - { - SL_Acc_Odr_Reg=0x06; - } - else if(acc_odr==50) - { - SL_Acc_Odr_Reg=0x07; - } - SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x40, SL_Acc_Odr_Reg);//ACC_CONF - os_time_dly(1);//5ms - SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x7D, 0x04);//acc open and gyro close - os_time_dly(1);//5ms - SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x7D, 0x04);//acc open and gyro close - sl_delay(200); - SL_SC7U22_I2c_Spi_Read(SL_SPI_IIC_INTERFACE, 0x7D, 1,&SL_Read_Reg); - - if(SL_Read_Reg!=0x04) - { -#if SL_Sensor_Algo_Release_Enable==0x00 - printf("SL_Read_Reg=0x%x 0x04\r\n",SL_Read_Reg); -#endif - return 0; - } - return 1; -} - -/*******ODR SET:25 50 100 200******************/ -/*******acc range:2 4 8 16*********************/ -/*******gyro range:125 250 500 1000 2000*******/ -/*******acc_hp_en: 0=disable 1=enable**********/ -/*******gyro_hp_en:0=disable 1=enable**********/ -unsigned char SL_SC7U22_WakeUp_SET(unsigned char odr_mode,unsigned char acc_range,unsigned char acc_hp_en,unsigned short gyro_range,unsigned char gyro_hp_en) -{ - unsigned char SL_Odr_Reg = 0x00; - unsigned char SL_acc_mode_Reg = 0x00; - unsigned char SL_gyro_mode_Reg = 0x00; - unsigned char SL_acc_range_Reg = 0x00; - unsigned char SL_gyro_range_Reg = 0x00; - unsigned char SL_Read_Check = 0xff; - - SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x7F, 0x00);//goto 0x00 - sl_delay(1); - SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x7D, 0x06);//PWR_CTRL ENABLE ACC+GYR - sl_delay(5); - SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x7D, 0x06);//PWR_CTRL ENABLE ACC+GYR - sl_delay(200); - SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x30, 0x00);//AIO1-disable - SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x32, 0xff);//vth - SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x33, 0xff);//tth - - if(odr_mode==25) - { - SL_Odr_Reg=0x06; - } - else if(odr_mode==50) - { - SL_Odr_Reg=0x07; - } - else if(odr_mode==100) - { - SL_Odr_Reg=0x08; - } - else if(odr_mode==200) - { - SL_Odr_Reg=0x09; - } - if(acc_hp_en==1) - SL_acc_mode_Reg=0x80; - - SL_acc_mode_Reg=SL_acc_mode_Reg|SL_Odr_Reg; - SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x40, SL_acc_mode_Reg);//ACC_CONF - - if(gyro_hp_en==1) - SL_gyro_mode_Reg=0x40; - else if(gyro_hp_en==2) - SL_gyro_mode_Reg=0x80; - else if(gyro_hp_en==3) - SL_gyro_mode_Reg=0xC0; - - SL_gyro_mode_Reg=SL_gyro_mode_Reg|SL_Odr_Reg; - SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x42, SL_gyro_mode_Reg);//GYR_CONF - - if(acc_range==2) - { - SL_acc_range_Reg=0x00; - } - else if(acc_range==4) - { - SL_acc_range_Reg=0x01; - } - else if(acc_range==8) - { - SL_acc_range_Reg=0x02; - } - else if(acc_range==16) - { - SL_acc_range_Reg=0x03; - } - SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x41, SL_acc_range_Reg);//ACC_RANGE - - if(gyro_range==2000) - { - SL_gyro_range_Reg=0x00; - } - else if(gyro_range==1000) - { - SL_gyro_range_Reg=0x01; - } - else if(gyro_range==500) - { - SL_gyro_range_Reg=0x02; - } - else if(gyro_range==250) - { - SL_gyro_range_Reg=0x03; - } - else if(gyro_range==125) - { - SL_gyro_range_Reg=0x04; - } - SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x43, SL_gyro_range_Reg);//GYR_RANGE 2000dps - SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x43, SL_gyro_range_Reg);//GYR_RANGE 2000dps -#if SL_Sensor_Algo_Release_Enable==0x00 -// SL_SC7U22_I2c_Spi_Read(SL_SPI_IIC_INTERFACE, 0x40, 1, &SL_Read_Check); -// printf("RawData:0x40=%x\r\n",SL_Read_Check); -// SL_SC7U22_I2c_Spi_Read(SL_SPI_IIC_INTERFACE, 0x41, 1, &SL_Read_Check); -// printf("RawData:0x41=%x\r\n",SL_Read_Check); -// SL_SC7U22_I2c_Spi_Read(SL_SPI_IIC_INTERFACE, 0x42, 1, &SL_Read_Check); -// printf("RawData:0x42=%x\r\n",SL_Read_Check); -// SL_SC7U22_I2c_Spi_Read(SL_SPI_IIC_INTERFACE, 0x43, 1, &SL_Read_Check); -// printf("RawData:0x43=%x\r\n",SL_Read_Check); -#endif - - SL_SC7U22_I2c_Spi_Read(SL_SPI_IIC_INTERFACE, 0x43, 1,&SL_Read_Check); - if(SL_Read_Check!=SL_gyro_range_Reg) - { -#if SL_Sensor_Algo_Release_Enable==0x00 - printf("SL_Read_Check=0x%x SL_gyro_range_Reg=0x%x\r\n",SL_Read_Check,SL_gyro_range_Reg); -#endif - return 0; - } - return 1; -} - - -#if SL_SC7U22_FIFO_ENABLE ==0x00 -// ================================================================================================= -// Madgwick AHRS 滤波器相关变量和函数 -// ------------------------------------------------------------------------------------------------- - -// 定义常量 -#define sampleFreq 100.0f // 传感器采样频率 (Hz)，必须与实际的传感器数据更新频率一致 -#define betaDef 0.1f // 算法的比例增益 beta，影响加速度计修正陀螺仪的权重 - -// 全局变量 -static volatile float beta = betaDef; // 算法增益 beta -static volatile float q0 = 1.0f, q1 = 0.0f, q2 = 0.0f, q3 = 0.0f; // 表示姿态的四元数 (w, x, y, z) - -/** - * @brief 快速计算 1/sqrt(x) - * @param x 输入的浮点数 - * @return 1/sqrt(x) 的近似值 - */ -static float invSqrt(float x) { - float halfx = 0.5f * x; - float y = x; - long i = *(long*)&y; - i = 0x5f3759df - (i>>1); - y = *(float*)&i; - y = y * (1.5f - (halfx * y * y)); // 牛顿迭代法，提高精度 - return y; -} - -/** - * @brief Madgwick AHRS 姿态更新函数 (IMU版本) - * @details 该函数融合了陀螺仪和加速度计的数据，计算出表示设备姿态的四元数。 - * 1. 使用陀螺仪数据积分，得到一个初步的姿态估计（预测）。 - * 2. 使用加速度计数据（当设备处于静止或低速运动时，加速度计主要测量重力）来修正这个估计（修正）。 - * 3. 通过梯度下降法找到一个最优的旋转，使得在当前姿态下，重力向量的方向与加速度计测量的方向最接近。 - * @param gx, gy, gz 陀螺仪三轴角速度 (单位: rad/s) - * @param ax, ay, az 加速度计三轴加速度 (单位: g) - */ -static void MadgwickAHRSupdateIMU(float gx, float gy, float gz, float ax, float ay, float az) { - float recipNorm; - float s0, s1, s2, s3; - float qDot1, qDot2, qDot3, qDot4; - float _2q0, _2q1, _2q2, _2q3, _4q0, _4q1, _4q2 ,_8q1, _8q2, q0q0, q1q1, q2q2, q3q3; - float dt = 1.0f / sampleFreq; // 采样时间间隔 - - // --- 1. 陀螺仪积分：计算四元数的变化率 --- - // 姿态运动学的基本方程，描述了姿态如何随角速度变化。 - qDot1 = 0.5f * (-q1 * gx - q2 * gy - q3 * gz); - qDot2 = 0.5f * (q0 * gx + q2 * gz - q3 * gy); - qDot3 = 0.5f * (q0 * gy - q1 * gz + q3 * gx); - qDot4 = 0.5f * (q0 * gz + q1 * gy - q2 * gx); - - // --- 2. 加速度计修正 --- - // 仅当加速度计读数有效时（即模长不为0）才进行修正，防止计算NaN。 - if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) { - - // 将加速度计读数归一化，得到单位向量 - recipNorm = invSqrt(ax * ax + ay * ay + az * az); - ax *= recipNorm; - ay *= recipNorm; - az *= recipNorm; - - // 预先计算一些重复使用的值，提高效率 - _2q0 = 2.0f * q0; - _2q1 = 2.0f * q1; - _2q2 = 2.0f * q2; - _2q3 = 2.0f * q3; - _4q0 = 4.0f * q0; - _4q1 = 4.0f * q1; - _4q2 = 4.0f * q2; - _8q1 = 8.0f * q1; - _8q2 = 8.0f * q2; - q0q0 = q0 * q0; - q1q1 = q1 * q1; - q2q2 = q2 * q2; - q3q3 = q3 * q3; - - // --- 梯度下降法：计算修正量 --- - // 目标函数 f(q, a) = [2(q1q3 - q0q2) - ax, 2(q0q1 + q2q3) - ay, 2(0.5 - q1^2 - q2^2) - az]^T - // s0, s1, s2, s3 是目标函数 f 对四元数 q 的雅可比矩阵 J 与 f 的乘积。 - // 这个结果代表了误差函数的梯度方向，用于修正四元数的变化率。 - s0 = _4q0 * q2q2 + _2q2 * ax + _4q0 * q1q1 - _2q1 * ay; - s1 = _4q1 * q3q3 - _2q3 * ax + 4.0f * q0q0 * q1 - _2q0 * ay - _4q1 + _8q1 * q1q1 + _8q1 * q2q2 + _4q1 * az; - s2 = 4.0f * q0q0 * q2 + _2q0 * ax + _4q2 * q3q3 - _2q3 * ay - _4q2 + _8q2 * q1q1 + _8q2 * q2q2 + _4q2 * az; - s3 = 4.0f * q1q1 * q3 - _2q1 * ax + 4.0f * q2q2 * q3 - _2q2 * ay; - recipNorm = invSqrt(s0 * s0 + s1 * s1 + s2 * s2 + s3 * s3); // 归一化梯度 - s0 *= recipNorm; - s1 *= recipNorm; - s2 *= recipNorm; - s3 *= recipNorm; - - // --- 应用修正量 --- - // 将计算出的修正量（梯度）从陀螺仪积分结果中减去，beta是修正的权重。 - qDot1 -= beta * s0; - qDot2 -= beta * s1; - qDot3 -= beta * s2; - qDot4 -= beta * s3; - } - - // --- 3. 积分：更新四元数 --- - // 使用一阶龙格-库塔法（即欧拉法）进行积分，得到新的四元数。 - q0 += qDot1 * dt; - q1 += qDot2 * dt; - q2 += qDot3 * dt; - q3 += qDot4 * dt; - - // --- 4. 归一化四元数 --- - // 保持四元数的模长为1，防止由于计算误差导致的累积漂移。 - recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3); - q0 *= recipNorm; - q1 *= recipNorm; - q2 *= recipNorm; - q3 *= recipNorm; -} -// ================================================================================================= - -// --- 静态校准相关变量 --- -static unsigned char SL_SC7U22_Error_Flag=0; -static unsigned char SL_SC7U22_Error_cnt=0; -static unsigned char SL_SC7U22_Error_cnt2=0; -static signed short Temp_Accgyro[6] ={0}; -static signed short Error_Accgyro[6]={0}; -static signed int Sum_Avg_Accgyro[6] ={0}; -static float yaw_offset = 0.0f; - -static signed short SL_GetAbsShort(signed short v_Val_s16r) -{ - if(v_Val_s16r==(-32768)) - return 32767; - return (v_Val_s16r < 0) ? -v_Val_s16r : v_Val_s16r; -} - -/** - * @brief 姿态角解算函数 - * @details - * 该函数主要完成两项工作： - * 1. 静态校准：在初始阶段，检测传感器是否处于静止状态。如果是，则计算加速度计和陀螺仪的零点偏移（误差），用于后续的数据补偿。 - * 2. 姿态解算：使用 Madgwick 滤波器融合经过校准后的加速度计和陀螺仪数据，计算出物体的俯仰角（Pitch）、横滚角（Roll）和偏航角（Yaw）。 - * - * @param calibration_en 传入：外部校准使能标志。如果为0，则强制认为已经校准完成。 - * @param acc_gyro_input 传入和传出：包含6轴原始数据的数组指针，顺序为 [ACC_X, ACC_Y, ACC_Z, GYR_X, GYR_Y, GYR_Z]。该函数会对其进行原地修改，填充为校准后的数据。 - * @param Angle_output 传出：滤波后的结果，顺序为 [Pitch, Roll, Yaw]。 - * @param yaw_rst 传入：Yaw轴重置标志。如果为1，则将Yaw角清零。 - * - * @return - * - 0: 正在进行静态校准。 - * - 1: 姿态角计算成功。 - * - 2: 校准未完成，无法进行计算。 - */ -unsigned char SL_SC7U22_Angle_Output(unsigned char calibration_en, signed short *acc_gyro_input, float *Angle_output, unsigned char yaw_rst) -{ - unsigned short acc_gyro_delta[2]; - unsigned char sl_i = 0; - - // 如果外部强制使能校准，则将标志位置1 - if (calibration_en == 0) { - SL_SC7U22_Error_Flag = 1; - } - - // ================================================================================= - // 步骤 1: 静态校准 (与原版逻辑相同) - // --------------------------------------------------------------------------------- - if (SL_SC7U22_Error_Flag == 0) { - // 计算当前数据与上一帧数据的差值，用于判断是否静止 - acc_gyro_delta[0] = 0; - acc_gyro_delta[1] = 0; - for (sl_i = 0; sl_i < 3; sl_i++) { - acc_gyro_delta[0] += SL_GetAbsShort(acc_gyro_input[sl_i] - Temp_Accgyro[sl_i]); - acc_gyro_delta[1] += SL_GetAbsShort(acc_gyro_input[3 + sl_i] - Temp_Accgyro[3 + sl_i]); - } - // 保存当前数据，用于下一帧比较 - for (sl_i = 0; sl_i < 6; sl_i++) { - Temp_Accgyro[sl_i] = acc_gyro_input[sl_i]; - } - - // 判断是否处于静止状态：加速度变化量、陀螺仪变化量、各轴加速度值都在一个很小的范围内 - // 假设1g = 8192 (对应 +/-4g 量程) - if ((acc_gyro_delta[0] / 8 < 80) && (acc_gyro_delta[1] < 20) && (SL_GetAbsShort(acc_gyro_input[0]) < 3000) && (SL_GetAbsShort(acc_gyro_input[1]) < 3000) && (SL_GetAbsShort(acc_gyro_input[2] - 8192) < 3000)) { //acc<80mg gyro<20 lsb - if (SL_SC7U22_Error_cnt < 200) { - SL_SC7U22_Error_cnt++; // 静止计数器累加 - } - } else { - SL_SC7U22_Error_cnt = 0; // 如果发生移动，则清空静止计数器 - } - - // 如果静止时间足够长（这里是190个采样周期，约1.9秒） - if (SL_SC7U22_Error_cnt > 190) { - // 开始累加50个采样点的数据 - for (sl_i = 0; sl_i < 6; sl_i++) { - Sum_Avg_Accgyro[sl_i] += acc_gyro_input[sl_i]; - } - SL_SC7U22_Error_cnt2++; - if (SL_SC7U22_Error_cnt2 > 49) { - // 累加满50个点后，计算平均值 - SL_SC7U22_Error_Flag = 1; // 标记校准完成 - SL_SC7U22_Error_cnt2 = 0; - SL_SC7U22_Error_cnt = 0; - for (sl_i = 0; sl_i < 6; sl_i++) { - Sum_Avg_Accgyro[sl_i] = Sum_Avg_Accgyro[sl_i] / 50; - } - - // 计算零点偏移：理想值 - 实际平均值 - // 加速度Z轴的理想值是8192，对应1g（假设量程为±4g） - Error_Accgyro[0] = 0 - Sum_Avg_Accgyro[0]; - Error_Accgyro[1] = 0 - Sum_Avg_Accgyro[1]; - Error_Accgyro[2] = 8192 - Sum_Avg_Accgyro[2]; - Error_Accgyro[3] = 0 - Sum_Avg_Accgyro[3]; - Error_Accgyro[4] = 0 - Sum_Avg_Accgyro[4]; - Error_Accgyro[5] = 0 - Sum_Avg_Accgyro[5]; -#if SL_Sensor_Algo_Release_Enable == 0x00 - printf("AVG_Recode AX:%d,AY:%d,AZ:%d,GX:%d,GY:%d,GZ:%d\r\n", Sum_Avg_Accgyro[0], Sum_Avg_Accgyro[1], Sum_Avg_Accgyro[2], Sum_Avg_Accgyro[3], Sum_Avg_Accgyro[4], Sum_Avg_Accgyro[5]); - printf("Error_Recode AX:%d,AY:%d,AZ:%d,GX:%d,GY:%d,GZ:%d\r\n", Error_Accgyro[0], Error_Accgyro[1], Error_Accgyro[2], Error_Accgyro[3], Error_Accgyro[4], Error_Accgyro[5]); -#endif - } - } else { - // 如果在累加过程中发生移动，则重新开始 - SL_SC7U22_Error_cnt2 = 0; - for (sl_i = 0; sl_i < 6; sl_i++) { - Sum_Avg_Accgyro[sl_i] = 0; - } - } - - return 0; // 返回0，表示正在校准 - } - - // ================================================================================= - // 步骤 2: 姿态解算 (Madgwick) - // --------------------------------------------------------------------------------- - if (SL_SC7U22_Error_Flag == 1) { // 确认已经校准完成 - // --- 2.1 数据预处理 --- - // 应用零点偏移补偿 - for (sl_i = 0; sl_i < 6; sl_i++) { - Temp_Accgyro[sl_i] = acc_gyro_input[sl_i] + Error_Accgyro[sl_i]; - } - -#if 1 // 将校准后的数据写回输入数组 - for (sl_i = 0; sl_i < 6; sl_i++) { - acc_gyro_input[sl_i] = Temp_Accgyro[sl_i]; - } -#endif - // --- 2.2 转换数据单位 --- - // 将校准后的传感器原始值 (LSB) 转换为 Madgwick 算法所需的物理单位。 - // 加速度: LSB -> g (重力加速度)。转换系数 = 量程 / (2^15)。假设 +/-4g 量程, 系数 = 4 / 32768 = 1/8192。 - float ax = (float)Temp_Accgyro[0] / 8192.0f; - float ay = (float)Temp_Accgyro[1] / 8192.0f; - float az = (float)Temp_Accgyro[2] / 8192.0f; - // 角速度: LSB -> rad/s (弧度/秒)。转换系数 = (量程 * PI) / (180 * 2^15)。 - // 假设 +/-2000dps 量程, 系数 = (2000 * 3.14159) / (180 * 32768) ≈ 0.001064 - float gx = (float)Temp_Accgyro[3] * 0.001064f; - float gy = (float)Temp_Accgyro[4] * 0.001064f; - float gz = (float)Temp_Accgyro[5] * 0.001064f; - - // --- 2.3 调用 Madgwick 更新函数 --- - // 将处理好的物理单位数据传入滤波器，更新姿态四元数。 - MadgwickAHRSupdateIMU(gx, gy, gz, ax, ay, az); - - // --- 2.4 将四元数转换为欧拉角 --- - // 欧拉角（Pitch, Roll, Yaw）更直观，便于使用。转换公式如下。 - // 转换结果单位为度 (乘以 180/PI ≈ 57.29578)。 - float yaw, pitch, roll; - - // Roll (横滚角，绕x轴旋转) - roll = atan2f(2.0f * (q0 * q1 + q2 * q3), 1.0f - 2.0f * (q1 * q1 + q2 * q2)) * 57.29578f; - // Pitch (俯仰角，绕y轴旋转) - float sinp = 2.0f * (q0 * q2 - q3 * q1); - if (fabsf(sinp) >= 1) - pitch = copysignf(3.14159265f / 2, sinp) * 57.29578f; // 防止万向节死锁，当sinp接近+/-1时，直接赋+/-90度 - else - pitch = asinf(sinp) * 57.29578f; - // Yaw (偏航角，绕z轴旋转) - yaw = atan2f(2.0f * (q0 * q3 + q1 * q2), 1.0f - 2.0f * (q2 * q2 + q3 * q3)) * 57.29578f; - - // --- 2.5 处理Yaw轴重置 --- - // Yaw角无法通过加速度计校正，会随时间漂移。提供一个重置机制，将当前Yaw角作为新的零点。 - if (yaw_rst) { - yaw_offset = yaw; - } - - // --- 2.6 输出最终角度 --- - // 将计算出的欧拉角存入输出数组。 - Angle_output[0] = pitch; - Angle_output[1] = roll; - Angle_output[2] = yaw - yaw_offset; // 输出减去偏移量的相对Yaw角 - - return 1; // 返回1，表示计算成功 - } - - return 2; // 校准未完成，返回错误状态 -} -#endif - diff --git a/apps/earphone/xtell_Sensor/sensor/WF282A.c b/apps/earphone/xtell_Sensor/sensor/WF282A.c deleted file mode 100644 index 41e795c..0000000 --- a/apps/earphone/xtell_Sensor/sensor/WF282A.c +++ /dev/null @@ -1,181 +0,0 @@ -/* - 气压计 - WF282A -*/ -#include "wf282a.h" -#include -#include // 推荐使用标准类型 -#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)); -} \ No newline at end of file diff --git a/apps/earphone/xtell_Sensor/sensor/WF282A.h b/apps/earphone/xtell_Sensor/sensor/WF282A.h deleted file mode 100644 index 8503907..0000000 --- a/apps/earphone/xtell_Sensor/sensor/WF282A.h +++ /dev/null @@ -1,148 +0,0 @@ -#ifndef _WF282A_H_ -#define _WF282A_H_ - -#include // 使用标准整数类型 - -// 标定值 -#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_ \ No newline at end of file diff --git a/apps/earphone/xtell_Sensor/xtell.h b/apps/earphone/xtell_Sensor/xtell.h deleted file mode 100644 index dc77190..0000000 --- a/apps/earphone/xtell_Sensor/xtell.h +++ /dev/null @@ -1,10 +0,0 @@ -#ifndef XTELL_H -#define XTELL_H -#include "system/includes.h" - -// #define KS_BLE 1 -#define XTELL_TEST 1 - -#define ACC_RANGE 16 //g，加速度满量程:2、4、8、16 - -#endif \ No newline at end of file diff --git a/apps/earphone/xtell_Sensor/xtell_app_main.c b/apps/earphone/xtell_Sensor/xtell_app_main.c deleted file mode 100644 index 96d1e06..0000000 --- a/apps/earphone/xtell_Sensor/xtell_app_main.c +++ /dev/null @@ -1,176 +0,0 @@ -#include "system/includes.h" -/*#include "btcontroller_config.h"*/ -#include "btstack/btstack_task.h" -#include "app_config.h" -#include "app_action.h" -#include "asm/pwm_led.h" -#include "tone_player.h" -#include "gpio.h" -#include "app_main.h" -#include "asm/charge.h" -#include "update.h" -#include "app_power_manage.h" -#include "app_charge.h" -#include "bt_profile_cfg.h" -#include "dev_manager/dev_manager.h" -#include "update_loader_download.h" -#include "avctp_user.h" -#include "debug.h" - - - -/////////////////////////////////////////////////////////////////////////////////////////////////// -//宏定义 -#define ENABLE_XLOG 1 -#ifdef xlog -#undef xlog -#endif -#if ENABLE_XLOG - #define xlog(format, ...) printf("[XT:%s] " format, __func__, ##__VA_ARGS__) -#else - #define xlog(format, ...) ((void)0) -#endif - -#define LOG_TAG_CONST APP -#define LOG_TAG "[APP]" -#define LOG_ERROR_ENABLE -#define LOG_DEBUG_ENABLE -#define LOG_INFO_ENABLE -/* #define LOG_DUMP_ENABLE */ -#define LOG_CLI_ENABLE -// -/////////////////////////////////////////////////////////////////////////////////////////////////// - -////////////////////////////////////////////////////////////////////////////////////////////////// -//变量 -extern APP_VAR app_var; -u16 close_BL_number=0; -// -////////////////////////////////////////////////////////////////////////////////////////////////// - -////////////////////////////////////////////////////////////////////////////////////////////////// -//函数定义 -extern void timer_2ms_handler(); -extern void app_var_init(void); -void app_earphone_play_voice_file(const char *name); -void clr_wdt(void); -extern void check_power_on_key(void); -extern int cpu_reset_by_soft(); -extern int audio_dec_init(); -extern int audio_enc_init(); -// -////////////////////////////////////////////////////////////////////////////////////////////////// - - - - - -/*充电拔出,CPU软件复位, 不检测按键，直接开机*/ -static void app_poweron_check(int update) -{ - if (!update && cpu_reset_by_soft()) { - app_var.play_poweron_tone = 0; - return; - } -} -void create_process(u16* pid,char* name, void *priv, void (*func)(void *priv), u32 msec){ - xlog("1 name=%s, pid =%d\n",name,*pid); - if (*pid != 0) return; - *pid = sys_timer_add(priv, func, msec); - xlog("2 name=%s, pid =%d\n",name,*pid); -} -void close_process(u16* pid,char* name){ - xlog("name=%s,pid =%d\n",name,*pid); - if (*pid == 0) return; - sys_timer_del(*pid); - *pid = 0; -} - - -void close_BL(){ - /**开机默认关闭 经典蓝牙 */ - // close_BL_flag++; - xlog("xtell Classic Bluetooth off\n"); - user_send_cmd_prepare(USER_CTRL_DISCONNECTION_HCI, 0, NULL); //断开此时经典蓝牙的连接，经典蓝牙还是可以被发现 - delay_2ms(50); - user_send_cmd_prepare(USER_CTRL_WRITE_SCAN_DISABLE, 0, NULL); //关闭蓝牙可发现,已连接时不能操作 - delay_2ms(50); - user_send_cmd_prepare(USER_CTRL_WRITE_CONN_DISABLE, 0, NULL); //关闭蓝牙可连接， - // sys_timer_del(close_BL_number); //删除定时器任务 - close_process(&close_BL_number,__func__); -} - -void xtell_set_ble_name(char* name){ - -} - - -extern u32 timer_get_ms(void); -void xtell_app_main() -{ - int update = 0; - u32 addr = 0, size = 0; - struct intent it; - - xlog("==============xtell_app_main start================\n"); - log_info("app_main\n"); - app_var.start_time = timer_get_ms(); - - - - if (!UPDATE_SUPPORT_DEV_IS_NULL()) { - update = update_result_deal(); - } - - app_var_init(); - - if (get_charge_online_flag()) { -#if(TCFG_SYS_LVD_EN == 1) - vbat_check_init(); -#endif - xlog("==============idle================\n"); - init_intent(&it); - it.name = "idle"; - it.action = ACTION_IDLE_MAIN; - start_app(&it); - } else { - xlog("==============handler start================\n"); - check_power_on_voltage(); - app_poweron_check(update); - init_intent(&it); - it.name = "handler"; - it.action = ACTION_EARPHONE_MAIN; - start_app(&it); - xlog("==============handler end================\n"); - } - - - - ////////////////////////////////////////////////// - - //开机必须延时关闭经典蓝牙，不然底层代码会再次把蓝牙 打开 - // create_process(&close_BL_number, "close_BL",NULL, close_BL, 3000); - - - - - u8 mac_data[6]; - extern void rcsp_adv_fill_mac_addr(u8 *mac_addr_buf); - rcsp_adv_fill_mac_addr(mac_data); //读取MAC地址 - xlog("xtell BT mac data:%x:%x:%x:%x:%x:%x",mac_data[0],mac_data[1],mac_data[2],mac_data[3],mac_data[4],mac_data[5]); - - - user_send_cmd_prepare(USER_CTRL_WRITE_SCAN_ENABLE, 0, NULL); //打开蓝牙可发现,已连接时不能操作 - delay_2ms(50); - user_send_cmd_prepare(USER_CTRL_WRITE_CONN_ENABLE, 0, NULL); //打开蓝牙可连接 - delay_2ms(50); - - - - extern void xtell_task_create(void); - xtell_task_create(); - - xlog("==============xtell_app_end================\n"); - -} - diff --git a/apps/earphone/xtell_Sensor/xtell_handler.c b/apps/earphone/xtell_Sensor/xtell_handler.c deleted file mode 100644 index 726dd7a..0000000 --- a/apps/earphone/xtell_Sensor/xtell_handler.c +++ /dev/null @@ -1,555 +0,0 @@ -#include "system/includes.h" -#include "media/includes.h" -#include "tone_player.h" -#include "earphone.h" - -#include "app_config.h" -#include "app_action.h" -#include "app_task.h" - -#include "btstack/avctp_user.h" -#include "btstack/btstack_task.h" -#include "btctrler/btctrler_task.h" -#include "btstack/frame_queque.h" -#include "user_cfg.h" -// #include "aec_user.h" -#include "classic/hci_lmp.h" -#include "bt_common.h" -#include "bt_ble.h" -#include "bt_tws.h" -#include "pbg_user.h" -#include "btstack/bluetooth.h" -#include "colorful_lights/colorful_lights.h" - -#include "app_chargestore.h" -#include "jl_kws/jl_kws_api.h" - -#include "asm/charge.h" -#include "app_charge.h" -#include "ui_manage.h" - -#include "app_chargestore.h" -#include "app_umidigi_chargestore.h" -#include "app_testbox.h" -#include "app_online_cfg.h" -#include "app_main.h" -#include "app_power_manage.h" -#include "gSensor/gSensor_manage.h" -#include "key_event_deal.h" -#include "classic/tws_api.h" -#include "asm/pwm_led.h" -#include "ir_sensor/ir_manage.h" -#include "in_ear_detect/in_ear_manage.h" -#include "vol_sync.h" -#include "bt_background.h" -#include "default_event_handler.h" -#include "debug.h" -#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 -#define LOG_TAG "[EARPHONE]" -#define LOG_ERROR_ENABLE -#define LOG_DEBUG_ENABLE -#define xlog_ENABLE - - -#if(USE_DMA_UART_TEST) //使用dm串口测试时不能同时打开 -#define MY_SNIFF_EN 0 -#else -#define MY_SNIFF_EN 1 //默认打开 -#endif - -#define ENABLE_XLOG 1 -#ifdef xlog -#undef xlog -#endif -#if ENABLE_XLOG - #define xlog(format, ...) printf("[XT:%s] " format, __func__, ##__VA_ARGS__) -#else - #define xlog(format, ...) ((void)0) -#endif -// -/////////////////////////////////////////////////////////////////////////////////////////////////// - -/////////////////////////////////////////////////////////////////////////////////////////////////// -//变量 -extern u8 init_ok; -extern u8 sniff_out; -unsigned char xtell_bl_state=0; //存放经典蓝牙的连接状态，0断开，1是连接 -u8 bt_newname =0; -unsigned char xt_ble_new_name[9] = "CM-22222"; -static u16 play_poweron_ok_timer_id = 0; - -// -- 初始化标志位 -- -u8 SC7U22_init = 0x10; //六轴是否初始化 -u8 MMC5603nj_init = 0x20; //地磁是否初始化 -u8 BMP280_init = 0x30; //气压计初始化 -// -- 线程id -- -u16 SC7U22_calibration_id; -u16 start_collect_fuc_id; -u16 BLE_send_fuc_id; -// -/////////////////////////////////////////////////////////////////////////////////////////////////// -extern int bt_hci_event_handler(struct bt_event *bt); -extern void SC7U22_static_calibration(void); -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); -extern void start_collect_fuc(void); -extern void BLE_send_fuc(void); -/////////////////////////////////////////////////////////////////////////////////////////////////// -/* - * 模式状态机, 通过start_app()控制状态切换 - */ -/* extern int audio_mic_init(); */ - -static int state_machine(struct application *app, enum app_state state, struct intent *it){ - int error = 0; - static u8 tone_player_err = 0; - xlog("bt_state_machine=%d\n", state); - switch (state) { - case APP_STA_CREATE: - xlog("APP_STA_CREATE\n"); - /* set_adjust_conn_dac_check(0); */ - - break; - case APP_STA_START: - xlog("APP_STA_START\n"); - if (!it) { - xlog("APP_STA_START:it none\n"); - break; - } - switch (it->action) { - case ACTION_EARPHONE_MAIN: - xlog("ble init\n"); - /* - * handler 初始化 - */ - clk_set("sys", BT_NORMAL_HZ); - u32 sys_clk = clk_get("sys"); - bt_pll_para(TCFG_CLOCK_OSC_HZ, sys_clk, 0, 0); - /* bredr_set_dut_enble(1, 1); */ - bt_function_select_init(); - bredr_handle_register(); - EARPHONE_STATE_INIT(); - btstack_init(); - sys_auto_shut_down_enable(); - bt_sniff_feature_init(); - sys_auto_sniff_controle(MY_SNIFF_EN, NULL); - app_var.dev_volume = -1; - break; - case ACTION_A2DP_START: //蓝牙音频传输协议 - xlog("ACTION_A2DP_START\n"); - break; - case ACTION_BY_KEY_MODE: - xlog("ACTION_BY_KEY_MODE\n"); - break; - case ACTION_TONE_PLAY: - xlog("ACTION_TONE_PLAY\n"); - // STATUS *p_tone = get_tone_config(); - // tone_play_index(p_tone->bt_init_ok, 1); - break; - case ACTION_DO_NOTHING: - xlog("ACTION_DO_NOTHING\n"); - break; - } - break; - case APP_STA_PAUSE: - xlog("APP_STA_PAUSE\n"); - break; - case APP_STA_RESUME: - xlog("APP_STA_RESUME\n"); - //恢复前台运行 - sys_auto_shut_down_disable(); - sys_key_event_enable(); - break; - case APP_STA_STOP: - xlog("APP_STA_STOP\n"); - break; - case APP_STA_DESTROY: - xlog("APP_STA_DESTROY\n"); - r_printf("APP_STA_DESTROY\n"); - if (!app_var.goto_poweroff_flag) { - bt_app_exit(NULL); - } - break; - } - xlog("state machine error\n"); - return error; -} - -/////////////////////////////////////////////////////////////////////////////////////////////////// -//handle - - -void le_user_app_send_event(size_t command, unsigned char* data, size_t size) -{ - // 中断->事件 - static unsigned char buffer[512]; - if(data && size && size <= sizeof(buffer)) { - // 拷贝到缓存，避免转发事件的时候，地址发送改变。 - memcpy(buffer, data, size); - struct sys_event event; - event.type = SYS_APP_USER_EVENT; - event.u.app.command = command; - event.u.app.buffer = buffer; - event.u.app.size = size; - sys_event_notify(&event); - } -} - -void le_user_app_event_handler(struct sys_event* event){ - - switch (event->type) { - // 打印接收到的数据 - printf("BLE data\n"); - put_buf(event->u.app.buffer, event->u.app.size); - case SYS_APP_USER_EVENT: - if (event->u.app.buffer[0] == 0xBE && event->u.app.buffer[1] == 0xBB) { - if(event->u.app.buffer[2] == 0x01){ //后面的数据长度 1 - switch (event->u.app.buffer[3]){ - case 0xff: //测试 - - 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"); - } - break; - } - } - } - break; - - default: - xlog("%d\n",event->type); - break; - } - - -} - - -static void play_poweron_ok_timer(void *priv) -{ - app_var.wait_timer_do = 0; - - log_d("\n-------play_poweron_ok_timer-------\n", priv); - if (is_dac_power_off()) { -#if TCFG_USER_TWS_ENABLE - bt_tws_poweron(); -#else - bt_wait_connect_and_phone_connect_switch(0); -#endif - return; - } - - app_var.wait_timer_do = sys_timeout_add(priv, play_poweron_ok_timer, 100); -} - -static void play_bt_connect_dly(void *priv) -{ - app_var.wait_timer_do = 0; - - log_d("\n-------play_bt_connect_dly-------\n", priv); - - if (!app_var.goto_poweroff_flag) { - STATUS *p_tone = get_tone_config(); - tone_play_index(p_tone->bt_connect_ok, 1); - } -} - - - -static int bt_connction_status_event_handler(struct bt_event *bt) -{ - STATUS *p_tone = get_tone_config(); - u8 *phone_number = NULL; - - switch (bt->event) { - case BT_STATUS_INIT_OK: - /* - * 蓝牙初始化完成 - */ - xlog("BT_STATUS_INIT_OK\n"); - init_ok = 1; - __set_sbc_cap_bitpool(38); - -#if (TCFG_USER_BLE_ENABLE) - if (BT_MODE_IS(BT_BQB)) { - ble_bqb_test_thread_init(); - } else { -#if !TCFG_WIRELESS_MIC_ENABLE - bt_ble_init(); -#endif - } -#endif - bt_init_ok_search_index(); -#if TCFG_TEST_BOX_ENABLE - testbox_set_bt_init_ok(1); -#endif - -#if ((CONFIG_BT_MODE == BT_BQB)||(CONFIG_BT_MODE == BT_PER)) - bt_wait_phone_connect_control(1); -#else - if (is_dac_power_off()) { - bt_wait_connect_and_phone_connect_switch(0); - } else { - app_var.wait_timer_do = sys_timeout_add(NULL, play_poweron_ok_timer, 100); - } -#endif - - /*if (app_var.play_poweron_tone) { - tone_play_index(p_tone->power_on, 1); - }*/ - break; - - case BT_STATUS_SECOND_CONNECTED: - clear_current_poweron_memory_search_index(0); - case BT_STATUS_FIRST_CONNECTED: - xlog("BT_STATUS_CONNECTED\n"); - xtell_bl_state = 1; //蓝牙连接成功 置1 - if(strcmp(xt_ble_new_name,"CM-11111") != 0){ - //蓝牙连接成功 - bt_newname =1; - u8 temp[5]={0xBB,0xBE,0x02,0x04,0x00}; - temp[4] = xtell_bl_state; //经典蓝牙连接状态 - send_data_to_ble_client(&temp,5); - } - earphone_change_pwr_mode(PWR_DCDC15, 3000); - sys_auto_shut_down_disable(); - - ui_update_status(STATUS_BT_CONN); //单台在此处设置连接状态,对耳的连接状态需要同步，在bt_tws.c中去设置 - - /* tone_play(TONE_CONN); */ - /*os_time_dly(40); // for test*/ - xlog("tone status:%d\n", tone_get_status()); - if (get_call_status() == BT_CALL_HANGUP) { - if (app_var.phone_dly_discon_time) { - sys_timeout_del(app_var.phone_dly_discon_time); - app_var.phone_dly_discon_time = 0; - } else { - app_var.wait_timer_do = sys_timeout_add(NULL, play_bt_connect_dly, 1600); - /* tone_play_index(p_tone->bt_connect_ok, 1); */ - } - } - - - /*int timeout = 5000 + rand32() % 10000; - sys_timeout_add(NULL, connect_phone_test, timeout);*/ - break; - case BT_STATUS_FIRST_DISCONNECT: - case BT_STATUS_SECOND_DISCONNECT: - xlog("BT_STATUS_DISCONNECT\n"); - xtell_bl_state = 0; //断开蓝牙 清0 - //蓝牙断开连接 - if(bt_newname){ //已经改成新蓝牙名字，断开才播报 - bt_newname=0; - u8 temp[5]={0xBB,0xBE,0x02,0x04,0x00}; - temp[4] = xtell_bl_state; //经典蓝牙连接状态 - send_data_to_ble_client(&temp,5); - } - if (app_var.goto_poweroff_flag) { - /*关机不播断开提示音*/ - /*关机时不改UI*/ - break; - } - // bt_discon_dly_handle(NULL); - break; - - //phone status deal - case BT_STATUS_PHONE_INCOME: - break; - case BT_STATUS_PHONE_OUT: - - break; - case BT_STATUS_PHONE_ACTIVE: - break; - case BT_STATUS_PHONE_HANGUP: - break; - case BT_STATUS_PHONE_NUMBER: - break; - case BT_STATUS_INBAND_RINGTONE: //铃声 - break; - case BT_STATUS_CALL_VOL_CHANGE: - - break; - case BT_STATUS_SNIFF_STATE_UPDATE: - xlog(" BT_STATUS_SNIFF_STATE_UPDATE %d\n", bt->value); //0退出SNIFF - if (bt->value == 0) { - sniff_out = 1; - sys_auto_sniff_controle(MY_SNIFF_EN, bt->args); - } else { - sys_auto_sniff_controle(0, bt->args); - } - break; - - case BT_STATUS_LAST_CALL_TYPE_CHANGE: - break; - - case BT_STATUS_CONN_A2DP_CH: - case BT_STATUS_CONN_HFP_CH: - - if ((!is_1t2_connection()) && (get_current_poweron_memory_search_index(NULL))) { //回连下一个device - if (get_esco_coder_busy_flag()) { - clear_current_poweron_memory_search_index(0); - } else { - user_send_cmd_prepare(USER_CTRL_START_CONNECTION, 0, NULL); - } - } - break; - case BT_STATUS_PHONE_MANUFACTURER: - break; - case BT_STATUS_VOICE_RECOGNITION: - - break; - case BT_STATUS_AVRCP_INCOME_OPID: - xlog("BT_STATUS_AVRCP_INCOME_OPID:%d\n", bt->value); - break; - default: - xlog(" BT STATUS DEFAULT\n"); - break; - } - return 0; -} - - -static int event_handler(struct application *app, struct sys_event *event) -{ - - le_user_app_event_handler(event); - - if (SYS_EVENT_REMAP(event)) { - g_printf("****SYS_EVENT_REMAP**** \n"); - return 0; - } - - switch (event->type) { - case SYS_KEY_EVENT: - break; - case SYS_BT_EVENT: - /* - * 蓝牙事件处理 - */ - if ((u32)event->arg == SYS_BT_EVENT_TYPE_CON_STATUS) { - printf("in event_handler:bt_connction_status_event_handler"); - bt_connction_status_event_handler(&event->u.bt); - } else if ((u32)event->arg == SYS_BT_EVENT_TYPE_HCI_STATUS) { - bt_hci_event_handler(&event->u.bt); - } - break; - case SYS_DEVICE_EVENT: - /* - * 系统设备事件处理 - */ - if ((u32)event->arg == DEVICE_EVENT_FROM_CHARGE) { - - } else if ((u32)event->arg == DEVICE_EVENT_FROM_POWER) { - return app_power_event_handler(&event->u.dev); - } -#if TCFG_UMIDIGI_BOX_ENABLE - else if ((u32)event->arg == DEVICE_EVENT_UMIDIGI_CHARGE_STORE) { - app_umidigi_chargestore_event_handler(&event->u.umidigi_chargestore); - } -#endif -#if TCFG_TEST_BOX_ENABLE - else if ((u32)event->arg == DEVICE_EVENT_TEST_BOX) { - app_testbox_event_handler(&event->u.testbox); - } -#endif - break; - - default: - return false; - } - - SYS_EVENT_HANDLER_SPECIFIC(event); -#ifdef CONFIG_BT_BACKGROUND_ENABLE - if (app) { - default_event_handler(event); - } -#endif - return false; - -} - -/////////////////////////////////////////////////////////////////////////////////////////////////// - -static const struct application_operation app_handler_ops = { - .state_machine = state_machine, - .event_handler = event_handler, -}; - - -/* - * 注册earphone模式 - */ -REGISTER_APPLICATION(app_handler) = { - .name = "handler", - .action = ACTION_EARPHONE_MAIN, - .ops = &app_handler_ops, - .state = APP_STA_DESTROY, -};