Compare commits
17 Commits
| Author | SHA1 | Date | |
|---|---|---|---|
| 141b538bf5 | |||
| 23b14f0142 | |||
| c870b07b02 | |||
| 86199b822e | |||
| 754a529211 | |||
| 2bc443584c | |||
| f63092fe87 | |||
| ed475e870f | |||
| b8d48ebe19 | |||
| 976dafada1 | |||
| 27aa063107 | |||
| 9c0bce4568 | |||
| 6248a4fc34 | |||
| 627780ea20 | |||
| ade4b0a1f8 | |||
| 818e8c3778 | |||
| 892ed9267b |
16
.gitignore
vendored
16
.gitignore
vendored
@ -1,32 +1,16 @@
|
||||
# 编译生成的目标文件
|
||||
*.o
|
||||
*.a
|
||||
*.so
|
||||
*.d
|
||||
|
||||
# 编译生成的最终产物
|
||||
*.elf
|
||||
*.bin
|
||||
*.fw
|
||||
*.ufw
|
||||
*.map
|
||||
*.lst
|
||||
*.bc
|
||||
|
||||
# 编译产物目录
|
||||
# 看起来你所有的编译结果都在 objs/ 目录下，直接忽略整个目录更方便
|
||||
/objs/
|
||||
|
||||
# 工具链和构建产物
|
||||
# 根据你的路径，这些文件也应该被忽略
|
||||
/cpu/br28/tools/app.bin
|
||||
/cpu/br28/tools/data_code.bin
|
||||
/cpu/br28/tools/download/
|
||||
/cpu/br28/tools/isd_config.ini
|
||||
/cpu/br28/tools/sdk.elf*
|
||||
/cpu/br28/tools/sdk.lst
|
||||
/cpu/br28/tools/sdk.map
|
||||
/cpu/br28/sdk_used_list.used
|
||||
|
||||
# VSCode 用户个人设置
|
||||
# 团队协作时，每个人的配置可能不同，通常不建议提交
|
||||
|
||||
33
.vscode/settings.json
vendored
33
.vscode/settings.json
vendored
@ -1,33 +0,0 @@
|
||||
{
|
||||
"files.associations": {
|
||||
"board_config.h": "c",
|
||||
"board_jl701n_demo_cfg.h": "c",
|
||||
"colorful_lights.h": "c",
|
||||
"board_jl701n_anc_cfg.h": "c",
|
||||
"update_loader_download.h": "c",
|
||||
"iic_soft.h": "c",
|
||||
"circle_buffer.h": "c",
|
||||
"default_event_handler.h": "c",
|
||||
"ui_manage.h": "c",
|
||||
"charge.h": "c",
|
||||
"app_main.h": "c",
|
||||
"app_config.h": "c",
|
||||
"app_action.h": "c",
|
||||
"includes.h": "c",
|
||||
"key_event_deal.h": "c",
|
||||
"app_umidigi_chargestore.h": "c",
|
||||
"hci_lmp.h": "c",
|
||||
"bluetooth.h": "c",
|
||||
"SCU722.C": "cpp",
|
||||
"math.h": "c",
|
||||
"avctp_user.h": "c",
|
||||
"string.h": "c",
|
||||
"dev_manager.h": "c",
|
||||
"bt_tws.h": "c",
|
||||
"skiing_tracker.h": "c",
|
||||
"xtell.h": "c",
|
||||
"debug.h": "c",
|
||||
"ano_protocol.h": "c",
|
||||
"board_jl701n_demo_global_build_cfg.h": "c"
|
||||
}
|
||||
}
|
||||
35
Makefile
35
Makefile
@ -245,14 +245,7 @@ INCLUDES := \
|
||||
-Iinclude_lib/media/aispeech/enc/include \
|
||||
-Icpu/br28/audio_hearing \
|
||||
-Iinclude_lib/media/cvp \
|
||||
-Iapps/earphone/xtell_Sensor/buffer \
|
||||
-Iapps/earphone/xtell_Sensor/sensor \
|
||||
-Iapps/earphone/xtell_Sensor \
|
||||
-Iapps/earphone/xtell_Sensor/calculate \
|
||||
-Iapps/earphone/xtell_Sensor/ano \
|
||||
-Iapps/earphone/xtell_Sensor/sensor/ \
|
||||
-Iapps/earphone/xtell_Sensor/sensor/ \
|
||||
-Iapps/earphone/xtell_Sensor/sensor/ \
|
||||
-Iapps/earphone/xtell_remote_control \
|
||||
-I$(SYS_INC_DIR) \
|
||||
|
||||
|
||||
@ -617,19 +610,21 @@ c_SRC_FILES := \
|
||||
cpu/br28/uart_dev.c \
|
||||
cpu/br28/umidigi_chargestore.c \
|
||||
apps/common/colorful_lights/colorful_lights.c \
|
||||
apps/earphone/xtell_Sensor/xtell_app_main.c \
|
||||
apps/earphone/xtell_Sensor/xtell_handler.c \
|
||||
apps/earphone/xtell_Sensor/send_data.c \
|
||||
apps/earphone/xtell_Sensor/buffer/circle_buffer.c \
|
||||
apps/earphone/xtell_Sensor/sensor/LIS2DH12.c \
|
||||
apps/earphone/xtell_Sensor/sensor/SC7U22.c \
|
||||
apps/earphone/xtell_Sensor/calculate/skiing_tracker.c \
|
||||
apps/earphone/xtell_Sensor/ano/ano_protocol.c \
|
||||
apps/earphone/xtell_Sensor/sensor/MMC56.c \
|
||||
apps/earphone/xtell_Sensor/sensor/BMP280.c \
|
||||
apps/earphone/xtell_Sensor/sensor/AK8963.c \
|
||||
|
||||
|
||||
|
||||
# 定义需要自动搜索 .c 文件的目录列表
|
||||
C_SRC_DIRS := \
|
||||
apps/earphone/xtell_remote_control \
|
||||
|
||||
# 使用 shell 的 find 命令递归查找所有 .c 文件
|
||||
# foreach 遍历 C_SRC_DIRS 中的每一个目录
|
||||
# $(shell find $(dir) -name "*.c") 对每个目录执行 find 命令
|
||||
AUTO_C_SRC_FILES := $(foreach dir,$(C_SRC_DIRS),$(shell find $(dir) -name "*.c"))
|
||||
|
||||
# 将自动找到的文件列表追加到手动列表中
|
||||
c_SRC_FILES += $(AUTO_C_SRC_FILES)
|
||||
|
||||
# 需要编译的 .S 文件
|
||||
S_SRC_FILES := \
|
||||
apps/earphone/sdk_version.z.S \
|
||||
@ -749,6 +744,8 @@ LFLAGS := \
|
||||
--plugin-opt=-mattr=+fprev1 \
|
||||
|
||||
|
||||
|
||||
|
||||
LIBPATHS := \
|
||||
-L$(SYS_LIB_DIR) \
|
||||
|
||||
|
||||
@ -213,7 +213,7 @@ void write_gsensor_data_handle(void)
|
||||
}
|
||||
|
||||
// 临时的设备扫描诊断函数
|
||||
void i2c_scanner_probe(u8* device_addr, u8* found_number)
|
||||
void i2c_scanner_probe(void)
|
||||
{
|
||||
printf("Starting I2C bus scan...\n");
|
||||
int devices_found = 0;
|
||||
@ -224,13 +224,13 @@ void i2c_scanner_probe(u8* device_addr, u8* found_number)
|
||||
// 构建8位的写地址
|
||||
uint8_t write_addr_8bit = (addr_7bit << 1);
|
||||
|
||||
//传入使用的iic句柄编号
|
||||
iic_start(gSensor_info->iic_hdl);
|
||||
|
||||
// 尝试发送写地址，并检查返回值
|
||||
// iic_tx_byte 返回 1 表示收到了 ACK
|
||||
if (iic_tx_byte(gSensor_info->iic_hdl, write_addr_8bit))
|
||||
{
|
||||
device_addr[devices_found] = addr_7bit;
|
||||
printf("=====================================================================\n");
|
||||
printf("I2C device found at 7-bit address: 0x%02X\n", addr_7bit);
|
||||
printf("I2C device found at 8-bit address: 0x%02X\n", write_addr_8bit);
|
||||
@ -238,10 +238,10 @@ void i2c_scanner_probe(u8* device_addr, u8* found_number)
|
||||
devices_found++;
|
||||
}
|
||||
|
||||
//传入使用的iic句柄编号
|
||||
iic_stop(gSensor_info->iic_hdl);
|
||||
delay(gSensor_info->iic_delay); // 短暂延时
|
||||
}
|
||||
*found_number = devices_found;
|
||||
|
||||
if (devices_found == 0) {
|
||||
printf("Scan finished. No I2C devices found.\n");
|
||||
@ -266,7 +266,7 @@ u8 gravity_sensor_command(u8 w_chip_id, u8 register_address, u8 function_command
|
||||
// xlog("iic_tx_byte id\n");
|
||||
if (0 == iic_tx_byte(gSensor_info->iic_hdl, w_chip_id)) {
|
||||
ret = 0;
|
||||
xlog("\n gsen iic wr err 0\n");
|
||||
xlog("WRITE: I2C NACK on writing ADDR: 0x%X\n", w_chip_id - 1);
|
||||
strcpy(&w_log_buffer_1, "gsen iic wr err 0\n");
|
||||
goto __gcend;
|
||||
}
|
||||
@ -277,7 +277,7 @@ u8 gravity_sensor_command(u8 w_chip_id, u8 register_address, u8 function_command
|
||||
// xlog("iic_tx_byte: address\n");
|
||||
if (0 == iic_tx_byte(gSensor_info->iic_hdl, register_address)) {
|
||||
ret = 0;
|
||||
xlog("\n gsen iic wr err 1\n");
|
||||
xlog("WRITE: I2C NACK on writing ADDR: 0x%X\n", register_address);
|
||||
strcpy(&w_log_buffer_2, "gsen iic wr err 1\n");
|
||||
goto __gcend;
|
||||
}
|
||||
@ -316,7 +316,7 @@ u8 _gravity_sensor_get_ndata(u8 r_chip_id, u8 register_address, u8 *buf, u8 data
|
||||
|
||||
iic_start(gSensor_info->iic_hdl);
|
||||
if (0 == iic_tx_byte(gSensor_info->iic_hdl, r_chip_id - 1)) {
|
||||
xlog("I2C NACK on writing ADDR: 0x%X\n", r_chip_id - 1);
|
||||
xlog("GET: I2C NACK on writing ADDR: 0x%X\n", r_chip_id - 1);
|
||||
read_len = 0;
|
||||
strcpy(&sen_log_buffer_1, "gsen iic rd err 0\n");
|
||||
goto __gdend;
|
||||
@ -325,7 +325,7 @@ u8 _gravity_sensor_get_ndata(u8 r_chip_id, u8 register_address, u8 *buf, u8 data
|
||||
|
||||
delay(gSensor_info->iic_delay);
|
||||
if (0 == iic_tx_byte(gSensor_info->iic_hdl, register_address)) {
|
||||
xlog("I2C NACK on register ADDR: 0x%X\n", register_address);
|
||||
xlog("GET: I2C NACK on register ADDR: 0x%X\n", register_address);
|
||||
// xlog("\n gsen iic rd err 1\n");
|
||||
read_len = 0;
|
||||
strcpy(&sen_log_buffer_2, "gsen iic rd err 1\n");
|
||||
|
||||
@ -1663,7 +1663,8 @@ void send_version_to_sibling(void)
|
||||
data[2] = ver >> 8;
|
||||
tws_api_send_data_to_sibling(data, sizeof(data), TWS_FUNC_ID_SEQ_RAND_SYNC);
|
||||
}
|
||||
#endif
|
||||
|
||||
|
||||
|
||||
///////////////////////////////////////////////////////////////////////////////////////////////////////
|
||||
|
||||
@ -1689,4 +1690,5 @@ void send_data_to_ble_client(const u8* data, u16 length)
|
||||
} else {
|
||||
// printf("Insufficient buffer space to send data: Length %d\n", length);
|
||||
}
|
||||
}
|
||||
}
|
||||
#endif
|
||||
|
||||
@ -60,7 +60,8 @@ typedef struct {
|
||||
|
||||
typedef struct {
|
||||
//搜索匹配信息
|
||||
const client_match_cfg_t *match_dev_cfg[CLIENT_MATCH_CONN_MAX];
|
||||
// const client_match_cfg_t *match_dev_cfg[CLIENT_MATCH_CONN_MAX]; //xtell
|
||||
client_match_cfg_t *match_dev_cfg[CLIENT_MATCH_CONN_MAX];
|
||||
//加密保定配置 0 or 1
|
||||
u8 security_en;
|
||||
//搜索服务的个数
|
||||
|
||||
@ -7,7 +7,7 @@
|
||||
* notifications.
|
||||
*/
|
||||
// *****************************************************************************
|
||||
|
||||
#if 0
|
||||
#include "system/app_core.h"
|
||||
#include "system/includes.h"
|
||||
|
||||
@ -27,7 +27,7 @@
|
||||
|
||||
#if (TCFG_BLE_DEMO_SELECT == DEF_BLE_DEMO_WIRELESS_MIC_CLIENT)
|
||||
|
||||
#define SUPPORT_TEST_BOX_BLE_MASTER_TEST_EN 1
|
||||
#define SUPPORT_TEST_BOX_BLE_MASTER_TEST_EN 0
|
||||
#define SHOW_RX_DATA_RATE 1
|
||||
#define SHOW_TX_DATA_RATE 1
|
||||
|
||||
@ -189,8 +189,8 @@ static void default_report_data_deal(att_data_report_t *report_data, target_uuid
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
static const u8 test_remoter_name1[] = "AC637N_MX(BLE)";//
|
||||
static const u8 test_remoter_name1[] = "CM-22222";
|
||||
// static const u8 test_remoter_name1[] = "AC637N_MX(BLE)";//
|
||||
/* static const u8 test_remoter_name2[] = "AC630N_HID567(BLE)";// */
|
||||
static u16 default_client_write_handle;
|
||||
static u16 test_client_timer = 0;
|
||||
@ -275,7 +275,7 @@ static const client_conn_cfg_t client_conn_config_default = {
|
||||
#endif
|
||||
|
||||
//---------------------------------------------------------------------------
|
||||
static client_conn_cfg_t *client_config = NULL; //(void *) &client_conn_config_default ;
|
||||
static client_conn_cfg_t *client_config = (void *) &client_conn_config_default ;
|
||||
//----------------------------------------------------------------------------
|
||||
|
||||
|
||||
@ -729,7 +729,7 @@ static bool resolve_adv_report(u8 *adv_address, u8 data_length, u8 *data, s8 rss
|
||||
log_info_hexdump(adv_address, 6);
|
||||
adv_data_pt[lenght - 1] = tmp32;
|
||||
//-------
|
||||
#if 1 //无线麦产线通过识别特殊字符串进行近距离连接测试
|
||||
#if 0 //无线麦产线通过识别特殊字符串进行近距离连接测试
|
||||
if (0 == memcmp(adv_data_pt, SPECIFIC_STRING, strlen(SPECIFIC_STRING))) {
|
||||
flag_specific_adv_name = 1;
|
||||
flag_need_judge_rssi = 1;
|
||||
@ -1483,12 +1483,12 @@ void bt_ble_init(void)
|
||||
put_buf(match_name, config_name_len);
|
||||
printf("%s", match_name);
|
||||
#endif
|
||||
if (0 == memcmp(match_name, SPECIFIC_STRING, strlen(SPECIFIC_STRING))) {
|
||||
match_name = &match_name[strlen(SPECIFIC_STRING)];
|
||||
printf("specific scan%s", match_name);
|
||||
flag_specific_sacn = 1;
|
||||
// if (0 == memcmp(match_name, SPECIFIC_STRING, strlen(SPECIFIC_STRING))) {
|
||||
// match_name = &match_name[strlen(SPECIFIC_STRING)];
|
||||
// printf("specific scan%s", match_name);
|
||||
// flag_specific_sacn = 1;
|
||||
|
||||
}
|
||||
// }
|
||||
|
||||
set_ble_work_state(BLE_ST_INIT_OK);
|
||||
conn_pair_vm_do(&conn_pair_info, 0);
|
||||
@ -1557,10 +1557,25 @@ void client_send_conn_param_update(void)
|
||||
|
||||
|
||||
//----------------------------------------------------------------------------------
|
||||
//lmx
|
||||
void rcsp_adv_fill_mac_addr(u8 *mac_addr_buf)
|
||||
{
|
||||
#if (MUTIl_CHARGING_BOX_EN)
|
||||
u8 *mac_addr = get_chargebox_adv_addr();
|
||||
if (mac_addr) {
|
||||
swapX(mac_addr, mac_addr_buf, 6);
|
||||
}
|
||||
|
||||
/* printf("mac_addr:"); */
|
||||
/* printf_buf(mac_addr_buf, 6); */
|
||||
|
||||
#else
|
||||
swapX(bt_get_mac_addr(), mac_addr_buf, 6);
|
||||
#endif
|
||||
}
|
||||
|
||||
|
||||
#endif
|
||||
|
||||
|
||||
|
||||
#endif
|
||||
625
apps/earphone/94_rfid_stc/94_spi_test.c
Normal file
625
apps/earphone/94_rfid_stc/94_spi_test.c
Normal file
@ -0,0 +1,625 @@
|
||||
#include <stc8h.h>
|
||||
#include "xt_main.h"
|
||||
#include "intrins.h"
|
||||
#include <stdarg.h>
|
||||
|
||||
#include "READER.h"
|
||||
#include "READER_REG.h"
|
||||
// #include "board.h"
|
||||
#include "MIFARE.h"
|
||||
// #include "CPU_CARD.h"
|
||||
#include "NTAG.h"
|
||||
|
||||
|
||||
void TYPE_A_EVENT(void);
|
||||
void TYPE_B_EVENT(void);
|
||||
void TYPE_V_EVENT(void);
|
||||
void TYPE_F_EVENT(void);
|
||||
|
||||
// SPI????
|
||||
sbit SPI_SCK = P1^5; // SCK
|
||||
sbit SPI_MOSI = P1^3; // MOSI
|
||||
sbit SPI_MISO = P1^4; // MISO
|
||||
sbit SPI_SS = P1^2; // ??
|
||||
|
||||
bit busy1;
|
||||
|
||||
|
||||
void GPIO_init()
|
||||
{
|
||||
P_SW2 |= 0x80; // Enable access to XFR
|
||||
|
||||
// Set all ports to quasi-bidirectional mode
|
||||
P0M1 = 0; P0M0 = 0;
|
||||
P1M1 = 0; P1M0 = 0;
|
||||
P2M1 = 0; P2M0 = 0;
|
||||
P3M1 = 0; P3M0 = 0;
|
||||
P4M1 = 0; P4M0 = 0;
|
||||
P5M1 = 0; P5M0 = 0;
|
||||
P6M1 = 0; P6M0 = 0;
|
||||
P7M1 = 0; P7M0 = 0;
|
||||
}
|
||||
|
||||
// UART1 Functions
|
||||
void Uart1Send(unsigned char dat)
|
||||
{
|
||||
while (busy1);
|
||||
busy1 = 1;
|
||||
SBUF = dat;
|
||||
}
|
||||
|
||||
// ? Uart1SendString
|
||||
void Uart1SendString(unsigned char *str)
|
||||
{
|
||||
while (*str)
|
||||
{
|
||||
Uart1Send(*str);
|
||||
while (busy1); // ????????
|
||||
str++;
|
||||
}
|
||||
}
|
||||
void IntToHex(unsigned char *buf, unsigned char num)
|
||||
{
|
||||
unsigned char digits[] = "0123456789ABCDEF"; // 大写 HEX 为 %02X
|
||||
buf[0] = digits[(num >> 4) & 0x0F]; // 高 4 位
|
||||
buf[1] = digits[num & 0x0F]; // 低 4 位
|
||||
buf[2] = '\0';
|
||||
}
|
||||
// void Uart1Init(void) //115200bps@36MHz
|
||||
// {
|
||||
// SCON = 0x50; //8???,?????
|
||||
// AUXR |= 0x40; //?????1T??
|
||||
// AUXR &= 0xFE; //??1?????1???????
|
||||
// TMOD &= 0x0F; //???????
|
||||
// TL1 = 0xB2; //???????
|
||||
// TH1 = 0xFF; //???????
|
||||
// ET1 = 0; //???????
|
||||
// TR1 = 1; //???1????
|
||||
// }
|
||||
|
||||
void Uart1Init(void) //9600bps@36MHz
|
||||
{
|
||||
SCON = 0x50; //8位数据,可变波特率
|
||||
AUXR |= 0x40; //定时器时钟1T模式
|
||||
AUXR &= 0xFE; //串口1选择定时器1为波特率发生器
|
||||
TMOD &= 0x0F; //设置定时器模式
|
||||
TL1 = 0x56; //设置定时初始值
|
||||
TH1 = 0xFC; //设置定时初始值
|
||||
ET1 = 0; //禁止定时器中断
|
||||
TR1 = 1; //定时器1开始计时
|
||||
}
|
||||
|
||||
void Uart1Isr() interrupt 4
|
||||
{
|
||||
|
||||
unsigned char received;
|
||||
if (TI)
|
||||
{
|
||||
TI = 0;
|
||||
busy1 = 0;
|
||||
}
|
||||
if (RI)
|
||||
{
|
||||
received = SBUF; // Read received data
|
||||
RI = 0;
|
||||
//Uart2Send(received); // Send received data via UART2
|
||||
}
|
||||
}
|
||||
|
||||
void Delay100ms() //@36MHz
|
||||
{
|
||||
unsigned char data i, j, k;
|
||||
|
||||
//_nop_();
|
||||
i = 19;
|
||||
j = 68;
|
||||
k = 67;
|
||||
do
|
||||
{
|
||||
do
|
||||
{
|
||||
while (--k);
|
||||
} while (--j);
|
||||
} while (--i);
|
||||
}
|
||||
void Delay1ms() //@36MHz
|
||||
{
|
||||
unsigned char data i, j;
|
||||
|
||||
_nop_();
|
||||
i = 47;
|
||||
j = 190;
|
||||
do
|
||||
{
|
||||
while (--j);
|
||||
} while (--i);
|
||||
}
|
||||
|
||||
// SPI?????
|
||||
void SPI_Init_xt(void)
|
||||
{
|
||||
// ??SPI?????
|
||||
SPCTL =0xD0; // ??SPI,???,??Fosc/4,CPOL=0, CPHA=0
|
||||
SPSTAT = 0x00; // ??????
|
||||
P_SW1 = P_SW1 & 0xF3;
|
||||
// ?????????(SCK, MOSI, SS)
|
||||
P1M0 |= (1<<5) | (1<<3) | (1<<2); // ??P1.5, P1.3, P1.2???
|
||||
P1M1 &= ~((1<<5) | (1<<3) | (1<<2));
|
||||
|
||||
// MISO???????
|
||||
P1M0 &= ~(1<<4); // P1.4??
|
||||
P1M1 |= (1<<4);
|
||||
|
||||
SPI_SS = 1; // ???????
|
||||
}
|
||||
|
||||
// SPI??/???????
|
||||
unsigned char SPI_Transfer(unsigned char dat)
|
||||
{
|
||||
SPDAT = dat; // ??????????
|
||||
while(!(SPSTAT & 0x80)); // ??????
|
||||
SPSTAT = 0xC0; // ??????
|
||||
return SPDAT; // ????????
|
||||
}
|
||||
|
||||
// 读寄存器
|
||||
unsigned char GetReg(unsigned char address, unsigned char *reg_data)
|
||||
{
|
||||
unsigned char spi_data;
|
||||
|
||||
SPI_SS = 0; // ????,????
|
||||
*reg_data =0;
|
||||
// ????(??1??????)
|
||||
spi_data = (address << 1) | 0x01;
|
||||
SPI_Transfer(spi_data); // ????
|
||||
|
||||
// ???????
|
||||
*reg_data = SPI_Transfer(0x00); // ??????????
|
||||
|
||||
SPI_SS = 1; // ????,????
|
||||
|
||||
return SUCCESS;
|
||||
}
|
||||
|
||||
// 写寄存器值
|
||||
unsigned char SetReg(unsigned char address, unsigned char reg_data)
|
||||
{
|
||||
unsigned char spi_data;
|
||||
|
||||
SPI_SS = 0; // 拉低片选，开始通信
|
||||
|
||||
// 发送地址（左移1位并清写标志）
|
||||
spi_data = (address << 1) & 0xFE;
|
||||
SPI_Transfer(spi_data); // 发送地址
|
||||
|
||||
// 发送寄存器数据
|
||||
SPI_Transfer(reg_data); // 发送数据
|
||||
|
||||
SPI_SS = 1; // 拉高片选，结束通信
|
||||
|
||||
|
||||
return SUCCESS;
|
||||
}
|
||||
|
||||
unsigned char FM176XX_HardReset(void)
|
||||
{
|
||||
unsigned char reg_data;
|
||||
SPI_SS = 1;//NSS = 1
|
||||
P34 =1;//RST = 1
|
||||
Delay1ms();
|
||||
P34 =0;//RST = 0
|
||||
Delay1ms();
|
||||
GetReg(REG_COMMAND,®_data);
|
||||
if (reg_data != 0x40)
|
||||
return FAIL;
|
||||
return SUCCESS;
|
||||
}
|
||||
|
||||
// ?????
|
||||
unsigned char Reader_Set_HPD( unsigned char mode ) //mode = DISABLE <20>˳<EFBFBD>HPDģʽ <20><>mode = ENABLE <20><><EFBFBD><EFBFBD>HPDģʽ
|
||||
{
|
||||
if ( mode == ENABLE )
|
||||
{
|
||||
Delay1ms(); //<2F><>ʱ1ms
|
||||
|
||||
/******设置一个GPIO输出高*******xtell******/
|
||||
//PD_1; // PD = 1
|
||||
P34 = 1;
|
||||
}
|
||||
else
|
||||
{
|
||||
/******设置一个GPIO输出低*************/
|
||||
//PD_0; //PD = 0
|
||||
P34 =0;
|
||||
Delay1ms(); //<2F><>ʱ1ms<6D><73><EFBFBD>ȴ<EFBFBD>Reader<65><72><EFBFBD><EFBFBD>
|
||||
}
|
||||
return (mode);
|
||||
}
|
||||
|
||||
void printHex(unsigned char num)
|
||||
{
|
||||
unsigned char buf[3];
|
||||
IntToHex(buf, num);
|
||||
// Uart1SendString("Value: ");
|
||||
Uart1SendString(buf);
|
||||
// Uart1SendString("\r\n");
|
||||
}
|
||||
|
||||
|
||||
// // // 简化版 xlog，仅支持一个 %02X 或 %02x 参数
|
||||
// void xlog1(const unsigned char *fmt, unsigned char val)
|
||||
// {
|
||||
|
||||
// unsigned char hex[2];
|
||||
// int i;
|
||||
// while (*fmt)
|
||||
// {
|
||||
// if (*fmt == '%')
|
||||
// {
|
||||
// fmt++;
|
||||
// // 检查 %02X 或 %02x
|
||||
// if (*fmt == '0' && *(fmt+1) == '2' && (*(fmt+2) == 'X' || *(fmt+2) == 'x'))
|
||||
// {
|
||||
// // 处理 %02X 或 %02x
|
||||
|
||||
// // 转换为两位十六进制
|
||||
// hex[0] = (val >> 4) & 0x0F; // 高4位
|
||||
// hex[1] = val & 0x0F; // 低4位
|
||||
|
||||
// for ( i = 0; i < 2; i++)
|
||||
// {
|
||||
// if (hex[i] < 10)
|
||||
// Uart1Send(hex[i] + '0');
|
||||
// else
|
||||
// Uart1Send(hex[i] - 10 + 'A'); // 输出大写 A-F
|
||||
// while (busy1); // 等待发送完成
|
||||
// }
|
||||
// fmt += 3; // 跳过 "02X" 或 "02x"
|
||||
// }
|
||||
// else
|
||||
// {
|
||||
// // 未识别的格式，发送 % 和当前字符
|
||||
// Uart1Send('%');
|
||||
// while (busy1);
|
||||
// if (*fmt)
|
||||
// {
|
||||
// Uart1Send(*fmt);
|
||||
// while (busy1);
|
||||
// fmt++;
|
||||
// }
|
||||
// }
|
||||
// }
|
||||
// else
|
||||
// {
|
||||
// // 普通字符直接发送
|
||||
// Uart1Send(*fmt);
|
||||
// while (busy1);
|
||||
// fmt++;
|
||||
// }
|
||||
// }
|
||||
// }
|
||||
// P34 接芯片pdown
|
||||
void main(void)
|
||||
{
|
||||
unsigned char reg_value,i,tmp1,tmp2;
|
||||
unsigned char result,reg_data;
|
||||
ES =1;
|
||||
EA = 1;
|
||||
|
||||
Uart1Init();
|
||||
GPIO_init();
|
||||
SPI_Init_xt(); // ???SPI
|
||||
Uart1SendString("UART1: Hello, STC8H1K28!\r\n");
|
||||
//xlog("Data: %02X %02X %02x\n", 0xAB, 0x12, 0xFF);
|
||||
// xlog("1=%02X\r\n",0xAB);
|
||||
// PrintHex(0xAB); // 应输出 Test: ab
|
||||
Delay100ms();
|
||||
#if 0
|
||||
while(1)
|
||||
{
|
||||
// P34 =1;
|
||||
Uart1Send(0XAb);
|
||||
// ??:????0x01?????
|
||||
GetReg(0x7F, ®_value);
|
||||
Uart1Send(reg_value);
|
||||
LOG1("Value: %x", reg_value);
|
||||
// Delay100ms();
|
||||
// GetReg(0x02, &tmp1);
|
||||
// Uart1Send(tmp1);
|
||||
// Delay100ms();
|
||||
// // ??????reg_value???
|
||||
// SetReg(0x02,0x00);
|
||||
// GetReg(0x02, &tmp2);
|
||||
// Uart1Send(tmp2);
|
||||
// ????
|
||||
FM176XX_HardReset();
|
||||
for(i = 0; i < 10; i++){
|
||||
Delay100ms();
|
||||
}
|
||||
}
|
||||
#else
|
||||
while(1)
|
||||
{
|
||||
result = FM176XX_HardReset();
|
||||
if(result != SUCCESS)
|
||||
{
|
||||
Uart1SendString("FM17660 FAIL\r\n");
|
||||
Delay100ms();
|
||||
}
|
||||
else
|
||||
break;
|
||||
}
|
||||
GetReg(REG_VERSION,®_data);
|
||||
// printf( "REG_VERSION = %02X\r\n",reg_data );
|
||||
Uart1SendString("REG_VERSION =");
|
||||
printHex(reg_data);
|
||||
Uart1SendString("\n");
|
||||
while(1)
|
||||
{
|
||||
// Uart1SendString("main loop");
|
||||
for(i = 0; i < 2; i++){
|
||||
Delay100ms();
|
||||
}
|
||||
// TYPE_A_EVENT();
|
||||
// TYPE_B_EVENT();
|
||||
TYPE_V_EVENT();
|
||||
// TYPE_F_EVENT(); //这里有CPU_CARD.c的函数 暂时注释了
|
||||
|
||||
|
||||
/***现在版本只需要执行 V类型识别 15693 即可 */
|
||||
}
|
||||
#endif
|
||||
|
||||
}
|
||||
|
||||
|
||||
void TYPE_A_EVENT(void)
|
||||
{
|
||||
unsigned char result;
|
||||
// Uart1SendString(" TYPE_A_EVENT begin\n");
|
||||
Reader_Set_HPD(DISABLE);
|
||||
|
||||
result = ReaderA_Initial();
|
||||
// Uart1SendString("ReaderA_Initial return =");
|
||||
// printHex(result);
|
||||
if(result != SUCCESS)
|
||||
{
|
||||
Uart1SendString("INIT_ERROR\r\n");
|
||||
SetCW(DISABLE);
|
||||
return;
|
||||
}
|
||||
result = SetCW(ENABLE);
|
||||
// Uart1SendString("SetCW return =");
|
||||
// printHex(result);
|
||||
//xtell 注释
|
||||
// if(result != SUCCESS)
|
||||
// {
|
||||
// Uart1SendString("CW_ERROR\r\n");
|
||||
// SetCW(DISABLE);
|
||||
// return;
|
||||
// }
|
||||
result = ReaderA_CardActivate(&PICC_A);
|
||||
// Uart1SendString("ReaderA_CardActivate return =");
|
||||
// printHex(result);
|
||||
if(result != SUCCESS)
|
||||
{
|
||||
//printf("ReaderA_CardActivate_ERROR\r\n");
|
||||
SetCW(DISABLE);
|
||||
return;
|
||||
}
|
||||
Uart1SendString("************* TYPE A CARD************* \r\n");
|
||||
// printf("-> ATQA = %02X%02X\r\n",PICC_A.ATQA [0], PICC_A.ATQA [1]);
|
||||
|
||||
|
||||
if(PICC_A.UID_Length == 4)
|
||||
// printf( "-> UID = %02X%02X%02X%02X\r\n",PICC_A.UID[0], PICC_A.UID[1],PICC_A.UID[2],PICC_A.UID[3]);
|
||||
if(PICC_A.UID_Length == 8)
|
||||
// printf( "-> UID = %02X%02X%02X%02X%02X%02X%02X%02X\r\n",PICC_A.UID[0], PICC_A.UID[1],PICC_A.UID[2],PICC_A.UID[3],PICC_A.UID[4], PICC_A.UID[5],PICC_A.UID[6],PICC_A.UID[7]);
|
||||
// printf( "-> SAK = %02X\r\n",PICC_A.SAK[0]);
|
||||
|
||||
if(PICC_A.SAK[0] == 0x08)
|
||||
{
|
||||
Uart1SendString("************* Mifare CARD************* \r\n");
|
||||
result = MIFARE_CARD_EVENT();
|
||||
}
|
||||
if((PICC_A.SAK[0] == 0x28)||(PICC_A.SAK[0] == 0x20))
|
||||
{
|
||||
Uart1SendString("************* CPU CARD************* \r\n");
|
||||
// result = CPU_CARD_EVENT();
|
||||
}
|
||||
if(PICC_A.SAK[0] == 0x04)
|
||||
{
|
||||
Uart1SendString("************* NTAG CARD************* \r\n");
|
||||
result = NTAG_EVENT();
|
||||
}
|
||||
|
||||
SetCW(DISABLE);
|
||||
// LED();
|
||||
|
||||
}
|
||||
|
||||
|
||||
void TYPE_B_EVENT(void)
|
||||
{
|
||||
unsigned char result;
|
||||
// Uart1SendString("TYPE_B_EVENT begin\n");
|
||||
ReaderB_Initial();
|
||||
SetCW(ENABLE);
|
||||
result = ReaderB_Request(&PICC_B);
|
||||
if(result != SUCCESS)
|
||||
{
|
||||
SetCW(DISABLE);
|
||||
return;
|
||||
}
|
||||
//LED();
|
||||
Uart1SendString("************* TYPE B CARD************* \r\n");
|
||||
// printf("-> ATQB = %02X%02X%02X%02X%02X%02X%02X%02X%02X%02X%02X%02X\r\n",PICC_B.ATQB[0],PICC_B.ATQB[1],PICC_B.ATQB[2],PICC_B.ATQB[3],PICC_B.ATQB[4],PICC_B.ATQB[5],PICC_B.ATQB[6],PICC_B.ATQB[7],PICC_B.ATQB[8],PICC_B.ATQB[9],PICC_B.ATQB[10],PICC_B.ATQB[11]);
|
||||
|
||||
result = ReaderB_Attrib(&PICC_B);
|
||||
if(result != SUCCESS)
|
||||
{
|
||||
SetCW(DISABLE);
|
||||
return;
|
||||
}
|
||||
// printf("-> ATTRIB = %02X\r\n",PICC_B.CID);
|
||||
|
||||
result = ReaderB_Get_SN(&PICC_B);
|
||||
if(result != SUCCESS)
|
||||
{
|
||||
SetCW(DISABLE);
|
||||
return;
|
||||
}
|
||||
// printf("-> SN = %02X%02X%02X%02X%02X%02X%02X%02X\r\n",PICC_B.SN[0],PICC_B.SN[1],PICC_B.SN[2],PICC_B.SN[3],PICC_B.SN[4],PICC_B.SN[5],PICC_B.SN[6],PICC_B.SN[7]);
|
||||
SetCW(DISABLE);
|
||||
return;
|
||||
}
|
||||
|
||||
void TYPE_V_EVENT(void)
|
||||
{
|
||||
unsigned char result,i;
|
||||
|
||||
// Uart1SendString("TYPE_V_EVENT begin\n");
|
||||
ReaderV_Initial();
|
||||
SetCW(ENABLE);
|
||||
result = ReaderV_Inventory(&PICC_V);
|
||||
if (result != SUCCESS)
|
||||
{
|
||||
SetCW(DISABLE);
|
||||
//printf("-> ReaderV Inventory ERROR!\r\n");
|
||||
return;
|
||||
}
|
||||
// LED();
|
||||
// Uart1SendString("************* TYPE V CARD************* \r\n");
|
||||
// printf("UID = %02X%02X%02X%02X%02X%02X%02X%02X\r\n",PICC_V.UID[0],PICC_V.UID[1],PICC_V.UID[2],PICC_V.UID[3],PICC_V.UID[4],PICC_V.UID[5],PICC_V.UID[6],PICC_V.UID[7]);
|
||||
Uart1SendString("UID=");
|
||||
for(i=0;i<8;i++){
|
||||
printHex(PICC_V.UID[i]);
|
||||
}
|
||||
Uart1SendString("\r\n");
|
||||
result = ReaderV_Select(&PICC_V);
|
||||
if (result != SUCCESS)
|
||||
{
|
||||
SetCW(DISABLE);
|
||||
// Uart1SendString("-> ReaderV Select ERROR!\r\n");
|
||||
return;
|
||||
}
|
||||
memcpy(PICC_V.BLOCK_DATA,"\x00\x00\x00\x00",4);
|
||||
result = ReaderV_WriteSingleBlock(4,&PICC_V);
|
||||
if (result != SUCCESS)
|
||||
{
|
||||
SetCW(DISABLE);
|
||||
// Uart1SendString("-> ReaderV WriteSingleBlock ERROR!\r\n");
|
||||
return;
|
||||
}
|
||||
|
||||
// Uart1SendString("WriteSingleBlock SUCCESS\r\n");
|
||||
result = ReaderV_ReadSingleBlock(4,&PICC_V);
|
||||
if (result != SUCCESS)
|
||||
{
|
||||
SetCW(DISABLE);
|
||||
// Uart1SendString("-> ReaderV ReadSingleBlock ERROR!\r\n");
|
||||
return;
|
||||
}
|
||||
// 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]);
|
||||
// Uart1SendString("BLOCK DATA=");
|
||||
for(i=0;i<4;i++){
|
||||
// printHex(PICC_V.BLOCK_DATA[i]);
|
||||
}
|
||||
// Uart1SendString("\r\n");
|
||||
SetCW(DISABLE);
|
||||
}
|
||||
|
||||
|
||||
void TYPE_F_EVENT(void)
|
||||
{
|
||||
unsigned char result,i;
|
||||
unsigned char SendBuffer[255];
|
||||
unsigned char ReceiveBuffer[255];
|
||||
|
||||
transmission_struct TPDU;
|
||||
TPDU.pSendBuffer = SendBuffer;//<2F><><EFBFBD>÷<EFBFBD><C3B7><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||||
TPDU.pReceiveBuffer = ReceiveBuffer;//<2F><><EFBFBD>ý<EFBFBD><C3BD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||||
|
||||
// Uart1SendString("TYPE_F_EVENT begin\n");
|
||||
ReaderF_Initial();
|
||||
SetCW(ENABLE);
|
||||
result = ReaderF_Inventory(&PICC_F);
|
||||
if(result != SUCCESS)
|
||||
{
|
||||
SetCW(DISABLE);
|
||||
return;
|
||||
}
|
||||
// LED();
|
||||
Uart1SendString("************* TYPE F CARD************* \r\n");
|
||||
// printf("->TYPE F UID = %02X%02X%02X%02X%02X%02X%02X%02X\r\n",PICC_F.UID[0],PICC_F.UID[1],PICC_F.UID[2],PICC_F.UID[3],PICC_F.UID[4],PICC_F.UID[5],PICC_F.UID[6],PICC_F.UID[7]);
|
||||
|
||||
//1401 012E48C23C8C7C3F 00F100000001430088B4
|
||||
//01 2E 48 C2 3C 8C 7C 3F
|
||||
memcpy(TPDU.pSendBuffer,"\x1D\x02",2);
|
||||
memcpy(TPDU.pSendBuffer + 2,PICC_F.UID,8);
|
||||
memcpy(TPDU.pSendBuffer + 10,"\x09\xFF\xFF\x00\x00\x00\x08\x08\x10\x10\x01\x08\x02\x08\x03\x0C\x04\x08\x09",19);
|
||||
|
||||
TPDU.SendLength = 29;
|
||||
TPDU.Timeout = 100;
|
||||
// printf("->SEND 2 = ");
|
||||
// for(i=0;i<TPDU.SendLength;i++)
|
||||
// printf("%02X",TPDU.pSendBuffer[i]);
|
||||
// printf("\r\n");
|
||||
// result = CPU_TPDU(&TPDU); xtell 注释 CPU_CARD.c中的函数
|
||||
if(result != SUCCESS)
|
||||
{
|
||||
SetCW(DISABLE);
|
||||
return;
|
||||
}
|
||||
// printf("->RESPONE = ");
|
||||
// for(i=0;i<TPDU.ReceiveLength;i++)
|
||||
// printf("%02X",TPDU.pReceiveBuffer[i] );
|
||||
|
||||
// printf("\r\n");
|
||||
|
||||
memcpy(TPDU.pSendBuffer,"\x24\x10",2);
|
||||
memcpy(TPDU.pSendBuffer + 2,PICC_F.UID,8);
|
||||
memcpy(TPDU.pSendBuffer + 10,"\x02\x00\x00\x00\x08\x06\x08\x10\x10\x01\x08\x02\x08\x03\x0C\x04\x08\x09\x1B\x04\x42\xB1\xD6\x98\x06\x7A",26);
|
||||
|
||||
TPDU.SendLength = 36;
|
||||
TPDU.Timeout = 100;
|
||||
// printf("->SEND 3 = ");
|
||||
// for(i=0;i<TPDU.SendLength;i++)
|
||||
// printf("%02X",TPDU.pSendBuffer[i] );
|
||||
// printf("\r\n");
|
||||
// result = CPU_TPDU(&TPDU); xtell 注释 CPU_CARD.c中的函数
|
||||
if(result != SUCCESS)
|
||||
{
|
||||
SetCW(DISABLE);
|
||||
return;
|
||||
}
|
||||
// printf("->RESPONE = ");
|
||||
// for(i=0;i<TPDU.ReceiveLength;i++)
|
||||
// printf("%02X",TPDU.pReceiveBuffer[i]);
|
||||
// printf("\r\n");
|
||||
|
||||
|
||||
//12 12 01 01 07 01 03 17 A6 07 \x45\xC9\x03\xC4\x7B\xF0\xE2\x00
|
||||
memcpy(TPDU.pSendBuffer,"\x12\x12",2);
|
||||
memcpy(TPDU.pSendBuffer + 2,PICC_F.UID,8);
|
||||
memcpy(TPDU.pSendBuffer + 10,"\x45\xC9\x03\xC4\x7B\xF0\xE2\x00",8);//<2F><>8<EFBFBD>ֽ<EFBFBD>Ӧ<EFBFBD><D3A6><EFBFBD>Ǽ<EFBFBD><C7BC><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||||
|
||||
TPDU.SendLength = 18;
|
||||
TPDU.Timeout = 100;
|
||||
// printf("->SEND 4 = ");
|
||||
// for(i=0;i<TPDU.SendLength;i++)
|
||||
// printf("%02X",TPDU.pSendBuffer[i] );
|
||||
// printf("\r\n");
|
||||
// result = CPU_TPDU(&TPDU); xtell 注释 CPU_CARD.c中的函数
|
||||
if(result != SUCCESS)
|
||||
{
|
||||
SetCW(DISABLE);
|
||||
return;
|
||||
}
|
||||
// printf("->RESPONE = ");
|
||||
// for(i=0;i<TPDU.ReceiveLength;i++)
|
||||
// printf("%02X",TPDU.pReceiveBuffer[i]);
|
||||
// printf("\r\n");
|
||||
|
||||
return;
|
||||
}
|
||||
582
apps/earphone/94_rfid_stc/READER/CPU_CARD.c
Normal file
582
apps/earphone/94_rfid_stc/READER/CPU_CARD.c
Normal file
@ -0,0 +1,582 @@
|
||||
//2018<31><38>2<EFBFBD><32>2<EFBFBD>ձ༭<D5B1><E0BCAD>֧<EFBFBD><D6A7>CPU<50><55>Ƭָ<C6AC><EFBFBD><EEA3AC><EFBFBD>ӷ<EFBFBD>ʽ<EFBFBD><CABD><EFBFBD><EFBFBD>
|
||||
#include "READER.h"
|
||||
#include "CPU_CARD.h"
|
||||
#include <string.h>
|
||||
#include "READER_REG.h"
|
||||
#include "xt_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; /*<2A><><EFBFBD>÷<EFBFBD><C3B7><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> */
|
||||
APDU.pReceiveBuffer = ReceiveBuffer; /*<2A><><EFBFBD>ý<EFBFBD><C3BD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> */
|
||||
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.ReceiveLength;i++)
|
||||
printf("%02X", APDU.pReceiveBuffer[i] );
|
||||
printf( "\r\n" );
|
||||
|
||||
|
||||
memcpy(APDU.pSendBuffer,"\x00\xA4\x04\x00\x07\xD4\x10\x00\x00\x03\x00\x01",12);
|
||||
APDU.SendLength = 12;
|
||||
result = CPU_APDU(&APDU);
|
||||
if(result != SUCCESS)
|
||||
{
|
||||
SetCW(DISABLE);
|
||||
printf("-> APDU ERROR!\r\n");
|
||||
return result;
|
||||
}
|
||||
printf("-> APDU = ");
|
||||
for(i=0;i<APDU.ReceiveLength;i++)
|
||||
printf("%02X",APDU.pReceiveBuffer[i]);
|
||||
printf("\r\n");
|
||||
|
||||
|
||||
memcpy(APDU.pSendBuffer,"\x00\xA4\x04\x00\x0E\x32\x50\x41\x59\x2E\x53\x59\x53\x2E\x44\x44\x46\x30\x31",19);
|
||||
APDU.SendLength = 19;
|
||||
result = CPU_APDU(&APDU);
|
||||
if(result != SUCCESS)
|
||||
{
|
||||
SetCW(DISABLE);
|
||||
printf("-> APDU ERROR!\r\n");
|
||||
return result;
|
||||
}
|
||||
printf("-> Response = ");
|
||||
for(i=0;i<APDU.ReceiveLength;i++)
|
||||
printf("%02X",APDU.pReceiveBuffer[i]);
|
||||
printf("\r\n");
|
||||
|
||||
memcpy(APDU.pSendBuffer,"\x00\xA4\x04\x00\x08\xA0\x00\x00\x03\x33\x01\x01\x01\x00",14);
|
||||
APDU.SendLength = 14;
|
||||
result = CPU_APDU(&APDU);
|
||||
if(result != SUCCESS)
|
||||
{
|
||||
SetCW(DISABLE);
|
||||
printf("-> APDU ERROR!\r\n");
|
||||
return result;
|
||||
}
|
||||
printf("-> Response = ");
|
||||
for(i=0;i<APDU.ReceiveLength;i++)
|
||||
printf("%02X",APDU.pReceiveBuffer[i]);
|
||||
printf("\r\n");
|
||||
|
||||
memcpy(APDU.pSendBuffer,"\x80\xA8\x00\x00\x24\x83\x22\x28\x00\x00\x80\x00\x00\x00\x00\x00\x01\x00\x00\x00\x00\x00\x00\x01\x56\x00\x00\x00\x00\x00\x01\x56\x13\x08\x28\x82\x12\x34\x56\x78\x00\x00",42);
|
||||
APDU.SendLength = 42;
|
||||
result = CPU_APDU(&APDU);
|
||||
if(result != SUCCESS)
|
||||
{
|
||||
SetCW(DISABLE);
|
||||
printf("-> APDU ERROR!\r\n");
|
||||
return result;
|
||||
}
|
||||
printf("-> Response = ");
|
||||
for(i=0;i<APDU.ReceiveLength;i++)
|
||||
printf("%02X",APDU.pReceiveBuffer[i]);
|
||||
printf("\r\n");
|
||||
|
||||
|
||||
memcpy( APDU.pSendBuffer, "\x00\x84\x00\x00\x08", 5 );
|
||||
APDU.SendLength = 5;
|
||||
result = CPU_APDU( &APDU );
|
||||
if ( result != SUCCESS )
|
||||
{
|
||||
SetCW( DISABLE );
|
||||
printf( "-> APDU ERROR!\r\n" );
|
||||
return result;
|
||||
}
|
||||
printf( "-> Response = " );
|
||||
for(i=0;i<APDU.ReceiveLength;i++)
|
||||
printf("%02X",APDU.pReceiveBuffer[i]);
|
||||
printf( "\r\n" );
|
||||
return result;
|
||||
}
|
||||
#endif
|
||||
|
||||
#if 0
|
||||
void Write_FIFO(unsigned char length,unsigned char* buff)
|
||||
{
|
||||
|
||||
}
|
||||
#else
|
||||
void Write_FIFO(unsigned char length,unsigned char* buff)
|
||||
{
|
||||
unsigned char i;
|
||||
for(i=0;i<length;i++)
|
||||
{
|
||||
SetReg(REG_FIFODATA,buff[i]);
|
||||
}
|
||||
}
|
||||
#endif
|
||||
|
||||
#if 0
|
||||
void Read_FIFO(unsigned char length,unsigned char* buff)
|
||||
{
|
||||
}
|
||||
#else
|
||||
void Read_FIFO(unsigned char length,unsigned char* buff)
|
||||
{
|
||||
unsigned char i;
|
||||
for(i=0;i<length;i++)
|
||||
{
|
||||
GetReg(REG_FIFODATA,&buff[i]);
|
||||
}
|
||||
}
|
||||
#endif
|
||||
|
||||
#if 0
|
||||
unsigned char FM176XX_TPDU( transmission_struct *tpdu )
|
||||
{
|
||||
return FAIL;
|
||||
}
|
||||
#else
|
||||
unsigned char FM176XX_TPDU( transmission_struct *tpdu )
|
||||
{
|
||||
unsigned char result,irq0,irq1,error;
|
||||
unsigned int i;
|
||||
SetCommand(CMD_IDLE);
|
||||
SetReg(REG_TXDATANUM,0x08);
|
||||
SetReg(REG_IRQ0,0x7F);//Clear IRQ0
|
||||
SetReg(REG_IRQ1,0x7F); //Clear IRQ1
|
||||
ModifyReg(REG_FIFOCONTROL,BIT_FIFOFLUSH,ENABLE); //Clear FIFO
|
||||
Write_FIFO(tpdu->SendLength,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);//<2F><><EFBFBD>յȴ<D5B5><C8B4><EFBFBD>ʱ
|
||||
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;//<2F><><EFBFBD>յ<EFBFBD><D5B5><EFBFBD><EFBFBD><EFBFBD><EFBFBD>д<EFBFBD><D0B4><EFBFBD>
|
||||
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;//δ<><CEB4>timeoutʱ<74><CAB1><EFBFBD>ڽ<EFBFBD><DABD>յ<EFBFBD><D5B5><EFBFBD><EFBFBD><EFBFBD>
|
||||
// }
|
||||
|
||||
}
|
||||
result = FAIL;//δ<><CEB4>timeoutʱ<74><CAB1><EFBFBD>ڽ<EFBFBD><DABD>յ<EFBFBD><D5B5><EFBFBD><EFBFBD><EFBFBD>
|
||||
return result;
|
||||
}
|
||||
#endif
|
||||
|
||||
|
||||
/****************************************************************/
|
||||
/*<2A><><EFBFBD><EFBFBD>: Rats */
|
||||
/*<2A><><EFBFBD><EFBFBD>: Request for answer to select */
|
||||
/*<2A><><EFBFBD><EFBFBD>: */
|
||||
/* */
|
||||
/*<2A><><EFBFBD>: */
|
||||
/* ats_len<65><6E><EFBFBD><EFBFBD><EFBFBD>յ<EFBFBD>ATS<54><53><EFBFBD>ݳ<EFBFBD><DDB3><EFBFBD> */
|
||||
/* *ats<74><73><EFBFBD><EFBFBD><EFBFBD>յ<EFBFBD><D5B5><EFBFBD>ATS<54><53><EFBFBD><EFBFBD>ָ<EFBFBD><D6B8> */
|
||||
/* OK: Ӧ<><D3A6><EFBFBD><EFBFBD>ȷ */
|
||||
/* ERROR: Ӧ<><D3A6><EFBFBD><EFBFBD><EFBFBD> */
|
||||
/****************************************************************/
|
||||
#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;//Ĭ<><C4AC><EFBFBD><EFBFBD>ʱʱ<CAB1><CAB1>
|
||||
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<54><53><EFBFBD>ݽ<EFBFBD><DDBD><EFBFBD> */
|
||||
{
|
||||
return FAIL;
|
||||
}
|
||||
#else
|
||||
unsigned char Ats_Process( unsigned char ats_len, unsigned char *ats )/* ATS<54><53><EFBFBD>ݽ<EFBFBD><DDBD><EFBFBD> */
|
||||
{
|
||||
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<43><42>ʼֵΪ0x02 */
|
||||
return (result);
|
||||
}
|
||||
#endif
|
||||
|
||||
#if 1
|
||||
//xtell 同时把CPU_TPDU 和 CPU_NAk 用空函数替换 SPi正常
|
||||
unsigned char CPU_NAK( transmission_struct *tpdu ) /* <20><>Ƭ<EFBFBD><C6AC><EFBFBD><EFBFBD>NAK */
|
||||
{
|
||||
return FAIL;
|
||||
}
|
||||
#else
|
||||
unsigned char CPU_NAK( transmission_struct *tpdu ) /* <20><>Ƭ<EFBFBD><C6AC><EFBFBD><EFBFBD>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
|
||||
|
||||
/****************************************************************/
|
||||
/*<2A><><EFBFBD><EFBFBD>: CPU_APDU */
|
||||
/*<2A><><EFBFBD><EFBFBD>: <20>ú<EFBFBD><C3BA><EFBFBD>ʵ<EFBFBD><CAB5>ת<EFBFBD><D7AA>APDU<44><55><EFBFBD><EFBFBD><EFBFBD>ʽ<EFBFBD><CABD><EFBFBD><EFBFBD> CPU card reset */
|
||||
/*<2A><><EFBFBD><EFBFBD>: */
|
||||
/* */
|
||||
/*<2A><><EFBFBD>: */
|
||||
/* ats_len<65><6E><EFBFBD><EFBFBD><EFBFBD>յ<EFBFBD><D5B5><EFBFBD><EFBFBD>ݳ<EFBFBD><DDB3><EFBFBD> */
|
||||
/* ats<74><73><EFBFBD><EFBFBD><EFBFBD>յ<EFBFBD><D5B5><EFBFBD><EFBFBD><EFBFBD>ָ<EFBFBD><D6B8> */
|
||||
/* OK: Ӧ<><D3A6><EFBFBD><EFBFBD>ȷ */
|
||||
/* ERROR: Ӧ<><D3A6><EFBFBD><EFBFBD><EFBFBD> */
|
||||
/****************************************************************/
|
||||
#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; /*<2A><><EFBFBD>ô<EFBFBD><C3B4><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ݳ<EFBFBD><DDB3><EFBFBD> */
|
||||
|
||||
for ( i = 0; i < 16; i++ ) /*<2A><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>̣<EFBFBD><CCA3><EFBFBD><EFBFBD><EFBFBD>С<EFBFBD><D0A1><EFBFBD>鳤<EFBFBD><E9B3A4>16<31><36><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>16<31><36><EFBFBD>鷢<EFBFBD><E9B7A2> */
|
||||
{
|
||||
#if CPU_DEBUG
|
||||
printf("unsent_length = %02X\r\n",unsent_length);
|
||||
#endif
|
||||
if ( unsent_length < CPU_CARD.FSC )
|
||||
{
|
||||
/*<2A><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ݳ<EFBFBD><DDB3><EFBFBD>С<EFBFBD><D0A1><EFBFBD><EFBFBD><EFBFBD>֡<EFBFBD><D6A1><EFBFBD>ȣ<EFBFBD><C8A3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>д<EFBFBD><D0B4><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> */
|
||||
tpdu.pSendBuffer[0] = CPU_CARD.PCB; /*PCB<43>ֽ<EFBFBD>д<EFBFBD><D0B4>TPDU<44><55><EFBFBD><EFBFBD> */
|
||||
memcpy( tpdu.pSendBuffer + 1, apdu->pSendBuffer + apdu->SendLength - unsent_length, unsent_length ); /*APDU<44><55><EFBFBD><EFBFBD>д<EFBFBD><D0B4>TPDU<44><55><EFBFBD><EFBFBD> */
|
||||
tpdu.SendLength = unsent_length + 1; /*<2A><><EFBFBD>ͳ<EFBFBD><CDB3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>1 */
|
||||
#if CPU_DEBUG
|
||||
printf("--> ");
|
||||
for(i=0;i<tpdu.SendLength;i++)
|
||||
printf("%02X",tpdu.pSendBuffer[i]);
|
||||
printf("\r\n");
|
||||
#endif
|
||||
result = CPU_TPDU( &tpdu );
|
||||
if ( (result != SUCCESS) || (tpdu.ReceiveLength == 0) )
|
||||
return (result);
|
||||
#if CPU_DEBUG
|
||||
printf("<-- ");
|
||||
for(i=0;i<tpdu.ReceiveLength;i++)
|
||||
printf("%02X",tpdu.pReceiveBuffer[i]);
|
||||
printf("\r\n");
|
||||
#endif
|
||||
unsent_length = 0; /*<2A><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ݳ<EFBFBD><DDB3><EFBFBD><EFBFBD><EFBFBD>0 */
|
||||
break; /*<2A><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ݷ<EFBFBD><DDB7><EFBFBD><EFBFBD><EFBFBD><EFBFBD>̺<EFBFBD><CCBA>˳<EFBFBD>ѭ<EFBFBD><D1AD> */
|
||||
}
|
||||
else
|
||||
{
|
||||
/*<2A><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ݳ<EFBFBD><DDB3>ȴ<EFBFBD><C8B4>ڵ<EFBFBD><DAB5><EFBFBD><EFBFBD><EFBFBD><EFBFBD>֡<EFBFBD><D6A1><EFBFBD>ȣ<EFBFBD><C8A3><EFBFBD><EFBFBD>鷢<EFBFBD>ͣ<EFBFBD>ÿ<EFBFBD>鷢<EFBFBD>͵<EFBFBD><CDB5><EFBFBD><EFBFBD>ݳ<EFBFBD><DDB3><EFBFBD>Ϊ<EFBFBD><CEAA>CPU_CARD.FSC - 1<><31> */
|
||||
tpdu.pSendBuffer[0] = CPU_CARD.PCB | 0x10; /*PCB<43>ֽ<EFBFBD>д<EFBFBD><D0B4>TPDU<44><55><EFBFBD><EFBFBD>,<2C><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ӱ<EFBFBD>ʶ */
|
||||
memcpy( tpdu.pSendBuffer + 1, apdu->pSendBuffer + apdu->SendLength - unsent_length, CPU_CARD.FSC - 1 ); /*APDU<44><55><EFBFBD><EFBFBD>д<EFBFBD><D0B4>TPDU<44><55><EFBFBD><EFBFBD> */
|
||||
|
||||
tpdu.SendLength = CPU_CARD.FSC; /*<2A><><EFBFBD>鷢<EFBFBD>ͳ<EFBFBD><CDB3><EFBFBD> */
|
||||
#if CPU_DEBUG
|
||||
printf("..--> ");
|
||||
for(i=0;i<tpdu.SendLength;i++)
|
||||
printf("%02X",tpdu.pSendBuffer[i]);
|
||||
printf("\r\n");
|
||||
#endif
|
||||
result = CPU_TPDU( &tpdu );
|
||||
#if CPU_DEBUG
|
||||
printf("<-- ");
|
||||
for(i=0;i<tpdu.ReceiveLength;i++)
|
||||
printf("%02X",tpdu.pReceiveBuffer[i]);
|
||||
printf("\r\n");
|
||||
#endif
|
||||
if ( (result != SUCCESS) && (tpdu.ReceiveLength == 1) )
|
||||
return (result);
|
||||
memcpy( &pcb_byte, tpdu.pReceiveBuffer, 1 ); /* */
|
||||
if ( (pcb_byte == 0xA2) || (pcb_byte == 0xA3) )
|
||||
{
|
||||
unsent_length = unsent_length - (CPU_CARD.FSC - 1); /*<2A><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ô<EFBFBD><C3B4><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ݳ<EFBFBD><DDB3><EFBFBD> */
|
||||
CPU_CARD.PCB = (pcb_byte & 0x0F) ^ 0x01; /*<2A><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>PCB<43>ֽ<EFBFBD> */
|
||||
#if CPU_DEBUG
|
||||
printf("unsent_length = %02X\r\n",unsent_length);
|
||||
#endif
|
||||
|
||||
}
|
||||
else
|
||||
return (FAIL); /* */
|
||||
}
|
||||
}
|
||||
for ( i = 0; i < 255; i++ ) /*<2A><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>̣<EFBFBD><CCA3><EFBFBD><EFBFBD>֧<EFBFBD><D6A7>255<35><35>ACK<43><4B><EFBFBD><EFBFBD>WTXӦ<58><D3A6> */
|
||||
{
|
||||
if ( (result != SUCCESS) || (tpdu.ReceiveLength == 0) )
|
||||
return (FAIL);
|
||||
memcpy( &pcb_byte, tpdu.pReceiveBuffer, 1 ); /*<2A><>ȡPCB<43>ֽ<EFBFBD> */
|
||||
if ( (pcb_byte == 0x02) || (pcb_byte == 0x03) )
|
||||
{
|
||||
CPU_CARD.PCB = pcb_byte ^ 0x01; /*<2A><><EFBFBD><EFBFBD>PCB<43>ֽ<EFBFBD> */
|
||||
#if CPU_DEBUG
|
||||
printf("....CPU_CARD.PCB = %02X\r\n",CPU_CARD.PCB);
|
||||
#endif
|
||||
memcpy( apdu->pReceiveBuffer + apdu->ReceiveLength, tpdu.pReceiveBuffer + 1, tpdu.ReceiveLength - 1 );
|
||||
apdu->ReceiveLength = apdu->ReceiveLength + tpdu.ReceiveLength - 1;
|
||||
return (SUCCESS); /*<2A><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ݽ<EFBFBD><DDBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> */
|
||||
}
|
||||
|
||||
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; /*<2A>յ<EFBFBD><D5B5><EFBFBD><EFBFBD><EFBFBD>֡<EFBFBD><D6A1><EFBFBD><EFBFBD><EFBFBD><EFBFBD>PCB<43>ֽ<EFBFBD> */
|
||||
tpdu.SendLength = 1;
|
||||
#if CPU_DEBUG
|
||||
printf("...--> = ");
|
||||
for(i=0;i<tpdu.SendLength;i++)
|
||||
printf("%02X",tpdu.pSendBuffer[i]);
|
||||
printf("\r\n");
|
||||
#endif
|
||||
result = CPU_TPDU( &tpdu ); /*<2A>ط<EFBFBD>ACK */
|
||||
#if CPU_DEBUG
|
||||
printf("<-- = ");
|
||||
for(i=0;i<tpdu.ReceiveLength;i++)
|
||||
printf("%02X",tpdu.pReceiveBuffer[i]);
|
||||
printf("\r\n");
|
||||
#endif
|
||||
}
|
||||
|
||||
if ( (pcb_byte == 0xF2) || (pcb_byte == 0xF3) )
|
||||
{
|
||||
tpdu.SendLength = tpdu.ReceiveLength;
|
||||
memcpy(tpdu.pSendBuffer ,tpdu.pReceiveBuffer ,tpdu.SendLength); /*<2A>յ<EFBFBD>WTX֡<58><D6A1><EFBFBD><EFBFBD><EFBFBD><EFBFBD>PCB<43>ֽ<EFBFBD> */
|
||||
#if CPU_DEBUG
|
||||
printf("....--> = ");
|
||||
for(i=0;i<tpdu.SendLength;i++)
|
||||
printf("%02X",tpdu.pSendBuffer[i]);
|
||||
printf("\r\n");
|
||||
#endif
|
||||
result = CPU_TPDU( &tpdu ); /*<2A>ط<EFBFBD>WTX */
|
||||
#if CPU_DEBUG
|
||||
printf("<-- = ");
|
||||
for(i=0;i<tpdu.ReceiveLength;i++)
|
||||
printf("%02X",tpdu.pReceiveBuffer[i]);
|
||||
printf("\r\n");
|
||||
#endif
|
||||
}
|
||||
}
|
||||
return (FAIL); /*<2A><><EFBFBD>մ<EFBFBD><D5B4><EFBFBD> */
|
||||
}
|
||||
#endif
|
||||
58
apps/earphone/94_rfid_stc/READER/CPU_CARD.h
Normal file
58
apps/earphone/94_rfid_stc/READER/CPU_CARD.h
Normal file
@ -0,0 +1,58 @@
|
||||
#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
|
||||
491
apps/earphone/94_rfid_stc/READER/MIFARE.c
Normal file
491
apps/earphone/94_rfid_stc/READER/MIFARE.c
Normal file
@ -0,0 +1,491 @@
|
||||
#include "MIFARE.h"
|
||||
#include "READER.h"
|
||||
|
||||
#include "string.h"
|
||||
#include "READER_REG.h"
|
||||
|
||||
#include "xt_main.h"
|
||||
unsigned char SECTOR,BLOCK,BLOCK_NUM;
|
||||
unsigned char BLOCK_DATA[16];
|
||||
unsigned char KEY_A[16][6]=
|
||||
{{0xFF,0xFF,0xFF,0xFF,0xFF,0xFF},//0
|
||||
{0xFF,0xFF,0xFF,0xFF,0xFF,0xFF},//1
|
||||
{0xFF,0xFF,0xFF,0xFF,0xFF,0xFF},//2
|
||||
{0xFF,0xFF,0xFF,0xFF,0xFF,0xFF},//3
|
||||
{0xFF,0xFF,0xFF,0xFF,0xFF,0xFF},//4
|
||||
{0xFF,0xFF,0xFF,0xFF,0xFF,0xFF},//5
|
||||
{0xFF,0xFF,0xFF,0xFF,0xFF,0xFF},//6
|
||||
{0xFF,0xFF,0xFF,0xFF,0xFF,0xFF},//7
|
||||
{0xFF,0xFF,0xFF,0xFF,0xFF,0xFF},//8
|
||||
{0xFF,0xFF,0xFF,0xFF,0xFF,0xFF},//9
|
||||
{0xFF,0xFF,0xFF,0xFF,0xFF,0xFF},//10
|
||||
{0xFF,0xFF,0xFF,0xFF,0xFF,0xFF},//11
|
||||
{0xFF,0xFF,0xFF,0xFF,0xFF,0xFF},//12
|
||||
{0xFF,0xFF,0xFF,0xFF,0xFF,0xFF},//13
|
||||
{0xFF,0xFF,0xFF,0xFF,0xFF,0xFF},//14
|
||||
{0xFF,0xFF,0xFF,0xFF,0xFF,0xFF}};//15
|
||||
|
||||
unsigned char KEY_B[16][6]=
|
||||
{{0xFF,0xFF,0xFF,0xFF,0xFF,0xFF},//0
|
||||
{0xFF,0xFF,0xFF,0xFF,0xFF,0xFF},//1
|
||||
{0xFF,0xFF,0xFF,0xFF,0xFF,0xFF},//2
|
||||
{0xFF,0xFF,0xFF,0xFF,0xFF,0xFF},//3
|
||||
{0xFF,0xFF,0xFF,0xFF,0xFF,0xFF},//4
|
||||
{0xFF,0xFF,0xFF,0xFF,0xFF,0xFF},//5
|
||||
{0xFF,0xFF,0xFF,0xFF,0xFF,0xFF},//6
|
||||
{0xFF,0xFF,0xFF,0xFF,0xFF,0xFF},//7
|
||||
{0xFF,0xFF,0xFF,0xFF,0xFF,0xFF},//8
|
||||
{0xFF,0xFF,0xFF,0xFF,0xFF,0xFF},//9
|
||||
{0xFF,0xFF,0xFF,0xFF,0xFF,0xFF},//10
|
||||
{0xFF,0xFF,0xFF,0xFF,0xFF,0xFF},//11
|
||||
{0xFF,0xFF,0xFF,0xFF,0xFF,0xFF},//12
|
||||
{0xFF,0xFF,0xFF,0xFF,0xFF,0xFF},//13
|
||||
{0xFF,0xFF,0xFF,0xFF,0xFF,0xFF},//14
|
||||
{0xFF,0xFF,0xFF,0xFF,0xFF,0xFF}};//15
|
||||
|
||||
/*****************************************************************************************/
|
||||
/*<2A><><EFBFBD>ƣ<EFBFBD>Mifare_Clear_Crypto */
|
||||
/*<2A><><EFBFBD>ܣ<EFBFBD>Mifare_Clear_Crypto<74><6F><EFBFBD><EFBFBD><EFBFBD>֤<EFBFBD><D6A4>־ */
|
||||
/*<2A><><EFBFBD>룺 */
|
||||
/* */
|
||||
/*<2A><><EFBFBD>: */
|
||||
/* */
|
||||
/* */
|
||||
/*****************************************************************************************/
|
||||
void Mifare_Clear_Crypto(void)
|
||||
{
|
||||
ModifyReg(REG_STATUS,BIT_CRYPTO1ON,RESET);
|
||||
return;
|
||||
}
|
||||
|
||||
unsigned char MIFARE_CARD_EVENTAAAAAAA(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_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;
|
||||
}
|
||||
|
||||
/*****************************************************************************************/
|
||||
/*<2A><><EFBFBD>ƣ<EFBFBD>Mifare_Auth */
|
||||
/*<2A><><EFBFBD>ܣ<EFBFBD>Mifare_Auth<74><68>Ƭ<EFBFBD><C6AC>֤ */
|
||||
/*<2A><><EFBFBD>룺mode<64><65><EFBFBD><EFBFBD>֤ģʽ<C4A3><CABD>0<EFBFBD><30>key A<><41>֤<EFBFBD><D6A4>1<EFBFBD><31>key B<><42>֤<EFBFBD><D6A4><EFBFBD><EFBFBD>sector<6F><72><EFBFBD><EFBFBD>֤<EFBFBD><D6A4><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ţ<EFBFBD>0~15<31><35> */
|
||||
/* *mifare_key<65><79>6<EFBFBD>ֽ<EFBFBD><D6BD><EFBFBD>֤<EFBFBD><D6A4>Կ<EFBFBD><D4BF><EFBFBD>飻*card_uid<69><64>4<EFBFBD>ֽڿ<D6BD>ƬUID<49><44><EFBFBD><EFBFBD> */
|
||||
/*<2A><><EFBFBD>: */
|
||||
/* OK :<3A><>֤<EFBFBD>ɹ<EFBFBD> */
|
||||
/* ERROR :<3A><>֤ʧ<D6A4><CAA7> */
|
||||
/*****************************************************************************************/
|
||||
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<4D><31>ָ֤<D6A4><D6B8>
|
||||
ModifyReg(REG_RXTXCON,BIT_SHMODE,DISABLE);
|
||||
}
|
||||
if(key_mode == KEY_B_M1)
|
||||
{
|
||||
SetReg(REG_FIFODATA,0x61);//61 keyB M1<4D><31>ָ֤<D6A4><D6B8>
|
||||
ModifyReg(REG_RXTXCON,BIT_SHMODE,DISABLE);
|
||||
}
|
||||
|
||||
SetReg(REG_FIFODATA,sector * 4);//<2F><>֤<EFBFBD><D6A4><EFBFBD><EFBFBD><EFBFBD>Ŀ<EFBFBD>0<EFBFBD><30>ַ
|
||||
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)//<2F>жϼ<D0B6><CFBC>ܱ<EFBFBD>־λ<D6BE><CEBB>ȷ<EFBFBD><C8B7><EFBFBD><EFBFBD>֤<EFBFBD><D6A4><EFBFBD>
|
||||
return SUCCESS;
|
||||
}
|
||||
return FAIL;
|
||||
}
|
||||
/*****************************************************************************************/
|
||||
/*<2A><><EFBFBD>ƣ<EFBFBD>Mifare_Blockset */
|
||||
/*<2A><><EFBFBD>ܣ<EFBFBD>Mifare_Blockset<65><74>Ƭ<EFBFBD><C6AC>ֵ<EFBFBD><D6B5><EFBFBD><EFBFBD> */
|
||||
/*<2A><><EFBFBD>룺block<63><6B><EFBFBD><EFBFBD>ţ<EFBFBD>*buff<66><66><EFBFBD><EFBFBD>Ҫ<EFBFBD><D2AA><EFBFBD>õ<EFBFBD>4<EFBFBD>ֽ<EFBFBD><D6BD><EFBFBD>ֵ<EFBFBD><D6B5><EFBFBD><EFBFBD> */
|
||||
/* */
|
||||
/*<2A><><EFBFBD>: */
|
||||
/* OK :<3A><><EFBFBD>óɹ<C3B3> */
|
||||
/* ERROR :<3A><><EFBFBD><EFBFBD>ʧ<EFBFBD><CAA7> */
|
||||
/*****************************************************************************************/
|
||||
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;
|
||||
}
|
||||
/*****************************************************************************************/
|
||||
/*<2A><><EFBFBD>ƣ<EFBFBD>Mifare_Blockread */
|
||||
/*<2A><><EFBFBD>ܣ<EFBFBD>Mifare_Blockread<61><64>Ƭ<EFBFBD><C6AC><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> */
|
||||
/*<2A><><EFBFBD>룺block<63><6B><EFBFBD><EFBFBD>ţ<EFBFBD>0x00~0x3F<33><46><EFBFBD><EFBFBD>buff<66><66>16<31>ֽڶ<D6BD><DAB6><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> */
|
||||
/*<2A><><EFBFBD>: */
|
||||
/* OK :<3A>ɹ<EFBFBD> */
|
||||
/* ERROR :ʧ<><CAA7> */
|
||||
/*****************************************************************************************/
|
||||
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 <20><><EFBFBD><EFBFBD>ָ<EFBFBD><D6B8>
|
||||
SetReg(REG_FIFODATA,block);//<2F><><EFBFBD>ַ
|
||||
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) //<2F><><EFBFBD>յ<EFBFBD><D5B5><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ݳ<EFBFBD><DDB3><EFBFBD>Ϊ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;
|
||||
}
|
||||
/*****************************************************************************************/
|
||||
/*<2A><><EFBFBD>ƣ<EFBFBD>mifare_blockwrite */
|
||||
/*<2A><><EFBFBD>ܣ<EFBFBD>Mifare<72><65>Ƭд<C6AC><D0B4><EFBFBD><EFBFBD><EFBFBD> */
|
||||
/*<2A><><EFBFBD>룺block<63><6B><EFBFBD><EFBFBD>ţ<EFBFBD>0x00~0x3F<33><46><EFBFBD><EFBFBD>buff<66><66>16<31>ֽ<EFBFBD>д<EFBFBD><D0B4><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> */
|
||||
/*<2A><><EFBFBD>: */
|
||||
/* OK :<3A>ɹ<EFBFBD> */
|
||||
/* ERROR :ʧ<><CAA7> */
|
||||
/*****************************************************************************************/
|
||||
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 д<><D0B4>ָ<EFBFBD><D6B8>
|
||||
SetReg(REG_FIFODATA,block);//<2F><><EFBFBD>ַ
|
||||
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) //<2F><><EFBFBD>յ<EFBFBD><D5B5><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ݳ<EFBFBD><DDB3><EFBFBD>Ϊ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) //<2F><><EFBFBD>յ<EFBFBD><D5B5><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ݳ<EFBFBD><DDB3><EFBFBD>Ϊ1
|
||||
return FAIL;
|
||||
GetReg (REG_FIFODATA,®_data);
|
||||
if(reg_data != 0x0A)
|
||||
return FAIL;
|
||||
|
||||
return SUCCESS;
|
||||
|
||||
}
|
||||
|
||||
/*****************************************************************************************/
|
||||
/*<2A><><EFBFBD>ƣ<EFBFBD> */
|
||||
/*<2A><><EFBFBD>ܣ<EFBFBD>Mifare <20><>Ƭ<EFBFBD><C6AC>ֵ<EFBFBD><D6B5><EFBFBD><EFBFBD> */
|
||||
/*<2A><><EFBFBD>룺block<63><6B><EFBFBD><EFBFBD>ţ<EFBFBD>0x00~0x3F<33><46><EFBFBD><EFBFBD>buff<66><66>4<EFBFBD>ֽ<EFBFBD><D6BD><EFBFBD>ֵ<EFBFBD><D6B5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> */
|
||||
/*<2A><><EFBFBD>: */
|
||||
/* OK :<3A>ɹ<EFBFBD> */
|
||||
/* ERROR :ʧ<><CAA7> */
|
||||
/*****************************************************************************************/
|
||||
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 <20><>ֵָ<D6B5><D6B8>
|
||||
SetReg(REG_FIFODATA,block);//<2F><><EFBFBD>ַ
|
||||
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) //<2F><><EFBFBD>յ<EFBFBD><D5B5><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ݳ<EFBFBD><DDB3><EFBFBD>Ϊ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) //<2F><><EFBFBD>յ<EFBFBD><D5B5><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ݳ<EFBFBD><DDB3><EFBFBD>Ϊ1
|
||||
return FAIL;
|
||||
GetReg (REG_FIFODATA,®_data);
|
||||
|
||||
if(reg_data != 0x0A)
|
||||
return FAIL;
|
||||
|
||||
return SUCCESS;
|
||||
}
|
||||
|
||||
|
||||
|
||||
/*****************************************************************************************/
|
||||
/*<2A><><EFBFBD>ƣ<EFBFBD>mifare_blockdec */
|
||||
/*<2A><><EFBFBD>ܣ<EFBFBD>Mifare <20><>Ƭ<EFBFBD><C6AC>ֵ<EFBFBD><D6B5><EFBFBD><EFBFBD> */
|
||||
/*<2A><><EFBFBD>룺block<63><6B><EFBFBD><EFBFBD>ţ<EFBFBD>0x00~0x3F<33><46><EFBFBD><EFBFBD>buff<66><66>4<EFBFBD>ֽڼ<D6BD>ֵ<EFBFBD><D6B5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> */
|
||||
/*<2A><><EFBFBD>: */
|
||||
/* OK :<3A>ɹ<EFBFBD> */
|
||||
/* ERROR :ʧ<><CAA7> */
|
||||
/*****************************************************************************************/
|
||||
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 <20><>ֵָ<D6B5><D6B8>
|
||||
SetReg(REG_FIFODATA,block);//<2F><><EFBFBD>ַ
|
||||
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) //<2F><><EFBFBD>յ<EFBFBD><D5B5><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ݳ<EFBFBD><DDB3><EFBFBD>Ϊ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) //<2F><><EFBFBD>յ<EFBFBD><D5B5><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ݳ<EFBFBD><DDB3><EFBFBD>Ϊ1
|
||||
return FAIL;
|
||||
GetReg (REG_FIFODATA,®_data);
|
||||
if(reg_data != 0x0A)
|
||||
|
||||
return FAIL;
|
||||
|
||||
return SUCCESS;
|
||||
|
||||
}
|
||||
/*****************************************************************************************/
|
||||
/*<2A><><EFBFBD>ƣ<EFBFBD>mifare_transfer */
|
||||
/*<2A><><EFBFBD>ܣ<EFBFBD>Mifare <20><>Ƭtransfer<65><72><EFBFBD><EFBFBD> */
|
||||
/*<2A><><EFBFBD>룺block<63><6B><EFBFBD><EFBFBD>ţ<EFBFBD>0x00~0x3F<33><46> */
|
||||
/*<2A><><EFBFBD>: */
|
||||
/* OK :<3A>ɹ<EFBFBD> */
|
||||
/* ERROR :ʧ<><CAA7> */
|
||||
/*****************************************************************************************/
|
||||
unsigned char Mifare_Transfer(unsigned char block)
|
||||
{
|
||||
unsigned char reg_data;
|
||||
SetCommand(CMD_IDLE);
|
||||
SetReg(REG_TXDATANUM,0x08);
|
||||
SetReg(REG_FIFODATA,0xC1);//C1 Transferָ<72><D6B8>
|
||||
SetReg(REG_FIFODATA,block);//<2F><><EFBFBD>ַ
|
||||
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) //<2F><><EFBFBD>յ<EFBFBD><D5B5><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ݳ<EFBFBD><DDB3><EFBFBD>Ϊ1
|
||||
return FAIL;
|
||||
GetReg (REG_FIFODATA,®_data);
|
||||
if(reg_data != 0x0A)
|
||||
return FAIL;
|
||||
|
||||
return SUCCESS;
|
||||
|
||||
|
||||
}
|
||||
/*****************************************************************************************/
|
||||
/*<2A><><EFBFBD>ƣ<EFBFBD>mifare_restore */
|
||||
/*<2A><><EFBFBD>ܣ<EFBFBD>Mifare <20><>Ƭrestore<72><65><EFBFBD><EFBFBD> */
|
||||
/*<2A><><EFBFBD>룺block<63><6B><EFBFBD><EFBFBD>ţ<EFBFBD>0x00~0x3F<33><46> */
|
||||
/*<2A><><EFBFBD>: */
|
||||
/* OK :<3A>ɹ<EFBFBD> */
|
||||
/* ERROR :ʧ<><CAA7> */
|
||||
/*****************************************************************************************/
|
||||
|
||||
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ָ<72><D6B8>
|
||||
SetReg(REG_FIFODATA,block);//<2F><><EFBFBD>ַ
|
||||
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) //<2F><><EFBFBD>յ<EFBFBD><D5B5><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ݳ<EFBFBD><DDB3><EFBFBD>Ϊ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) //<2F><><EFBFBD>յ<EFBFBD><D5B5><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ݳ<EFBFBD><DDB3><EFBFBD>Ϊ1
|
||||
return FAIL;
|
||||
GetReg (REG_FIFODATA,®_data);
|
||||
if(reg_data != 0x0A)
|
||||
return FAIL;
|
||||
return SUCCESS;
|
||||
}
|
||||
247
apps/earphone/94_rfid_stc/READER/MIFARE_commented.c
Normal file
247
apps/earphone/94_rfid_stc/READER/MIFARE_commented.c
Normal file
@ -0,0 +1,247 @@
|
||||
/**
|
||||
* @file MIFARE.c
|
||||
* @brief MIFARE Classic 卡应用层驱动实现
|
||||
* @version 1.0
|
||||
* @date 2024-05-20
|
||||
*
|
||||
* @par 文件作用:
|
||||
* 该文件是 MIFARE.h 中声明的所有函数的具体实现。它负责处理与
|
||||
* MIFARE Classic 卡片交互的特定应用层逻辑，如认证、数据读写等。
|
||||
*/
|
||||
|
||||
#include "MIFARE.h"
|
||||
#include "READER.h"
|
||||
#include "string.h"
|
||||
#include "READER_REG.h"
|
||||
#include "xt_main.h"
|
||||
|
||||
//==================================================================================
|
||||
// 1. 全局变量定义
|
||||
//==================================================================================
|
||||
unsigned char SECTOR, BLOCK, BLOCK_NUM;
|
||||
unsigned char BLOCK_DATA[16]; // 16字节数据缓冲区
|
||||
|
||||
// MIFARE 各扇区默认密钥 (Key A 和 Key B)，通常出厂时均为全F
|
||||
unsigned char KEY_A[16][6] = {
|
||||
{0xFF,0xFF,0xFF,0xFF,0xFF,0xFF}, {0xFF,0xFF,0xFF,0xFF,0xFF,0xFF},
|
||||
// ... (共16个扇区的密钥) ...
|
||||
};
|
||||
unsigned char KEY_B[16][6] = {
|
||||
{0xFF,0xFF,0xFF,0xFF,0xFF,0xFF}, {0xFF,0xFF,0xFF,0xFF,0xFF,0xFF},
|
||||
// ... (共16个扇区的密钥) ...
|
||||
};
|
||||
|
||||
/**
|
||||
* @brief 清除芯片内部的 MIFARE 加密状态标志
|
||||
* @note 每次对新卡操作或认证失败后，都应调用此函数来复位加密状态。
|
||||
*/
|
||||
void Mifare_Clear_Crypto(void)
|
||||
{
|
||||
ModifyReg(REG_STATUS, BIT_CRYPTO1ON, 0); // 将状态寄存器中的 CRYPTO1ON 位清零
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief MIFARE 卡操作事件流程 (一个完整的读写示例)
|
||||
* @return unsigned char 操作结果
|
||||
*/
|
||||
unsigned char MIFARE_CARD_EVENT(void)
|
||||
{
|
||||
unsigned char result;
|
||||
Mifare_Clear_Crypto(); // 开始前先清除加密状态
|
||||
SECTOR = 1; // 示例：操作第1扇区
|
||||
|
||||
// 对指定扇区进行Key A认证
|
||||
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; // 计算块的绝对地址
|
||||
|
||||
// 示例：向块写入全FF
|
||||
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 DATA: ...\r\n"); // 打印读出的数据
|
||||
}
|
||||
SetCW(DISABLE);
|
||||
return result;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 加载6字节密钥到读卡器芯片内部的密钥缓冲器
|
||||
* @param mifare_key 指向6字节密钥数组的指针
|
||||
* @return unsigned char 成功或失败
|
||||
*/
|
||||
unsigned char Mifare_LoadKey(unsigned char *mifare_key)
|
||||
{
|
||||
unsigned char reg_data;
|
||||
SetCommand(CMD_IDLE);
|
||||
Clear_FIFO();
|
||||
// 1. 将6字节密钥依次写入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]);
|
||||
// 2. 发送加载密钥命令，芯片会自动从FIFO读取密钥
|
||||
SetCommand(CMD_LOADKEY);
|
||||
DelayMs(1);
|
||||
GetReg(REG_COMMAND, ®_data);
|
||||
if((reg_data & CMD_MASK) == CMD_IDLE) // 检查命令是否执行完毕
|
||||
return SUCCESS;
|
||||
else
|
||||
return FAIL;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief **核心函数**: 对 MIFARE 卡指定扇区进行密码认证
|
||||
* @param key_mode 认证模式 (KEY_A_M1 或 KEY_B_M1)
|
||||
* @param sector 要认证的扇区号 (0-15)
|
||||
* @param mifare_key 指向6字节密钥的指针
|
||||
* @param card_uid 指向4字节卡片UID的指针 (从寻卡流程中获得)
|
||||
* @return unsigned char 成功或失败
|
||||
*/
|
||||
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); // 1. 首先将密钥加载到芯片
|
||||
if (result != SUCCESS)
|
||||
return result;
|
||||
|
||||
SetCommand(CMD_IDLE);
|
||||
Clear_FIFO();
|
||||
// 2. 根据认证模式，将认证命令码 (0x60 for KeyA, 0x61 for KeyB) 写入FIFO
|
||||
if(key_mode == KEY_A_M1)
|
||||
{
|
||||
SetReg(REG_FIFODATA, 0x60);
|
||||
}
|
||||
else // KEY_B_M1
|
||||
{
|
||||
SetReg(REG_FIFODATA, 0x61);
|
||||
}
|
||||
|
||||
// 3. 将要认证的块地址、卡片UID写入FIFO
|
||||
SetReg(REG_FIFODATA, sector * 4); // MIFARE卡每个扇区4个块，认证扇区内任一地址即可
|
||||
SetReg(REG_FIFODATA, card_uid[0]);
|
||||
SetReg(REG_FIFODATA, card_uid[1]);
|
||||
SetReg(REG_FIFODATA, card_uid[2]);
|
||||
SetReg(REG_FIFODATA, card_uid[3]);
|
||||
|
||||
// 4. 发送认证命令，芯片将自动完成与卡片的三次握手加密认证流程
|
||||
SetCommand(CMD_AUTHENT);
|
||||
DelayMs(5);
|
||||
|
||||
// 5. 检查认证结果
|
||||
GetReg(REG_COMMAND, ®_data);
|
||||
if((reg_data & CMD_MASK) == CMD_IDLE) // 命令执行完毕
|
||||
{
|
||||
GetReg(REG_STATUS, ®_data);
|
||||
// 关键: 检查状态寄存器中的 CRYPTO1ON 位是否为1。
|
||||
// 此位由硬件在认证成功后自动置位。
|
||||
if(reg_data & BIT_CRYPTO1ON)
|
||||
return SUCCESS;
|
||||
}
|
||||
return FAIL;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 读取一个16字节的数据块
|
||||
* @param block 要读取的块的绝对地址
|
||||
* @param data_buff 用于存放读取数据的16字节缓冲区
|
||||
* @return unsigned char 成功或失败
|
||||
*/
|
||||
unsigned char Mifare_Blockread(unsigned char block, unsigned char *data_buff)
|
||||
{
|
||||
unsigned char reg_data, i;
|
||||
SetCommand(CMD_IDLE);
|
||||
Clear_FIFO();
|
||||
// 1. 准备读块指令 (命令码 0x30 + 块地址)
|
||||
SetReg(REG_FIFODATA, 0x30);
|
||||
SetReg(REG_FIFODATA, block);
|
||||
ModifyReg(REG_TXCRCCON, BIT_CRCEN, ENABLE); // 读写命令需要CRC
|
||||
ModifyReg(REG_RXCRCCON, BIT_CRCEN, ENABLE);
|
||||
// 2. 发送并等待接收
|
||||
SetCommand(CMD_TRANSCEIVE);
|
||||
DelayMs(2);
|
||||
GetReg(REG_FIFOLENGTH, ®_data);
|
||||
if (reg_data != 16) // 3. 检查是否收到了完整的16字节数据
|
||||
return FAIL;
|
||||
GetReg(REG_ERROR, ®_data);
|
||||
if(reg_data & 0x07) // 检查是否有通信错误
|
||||
return FAIL;
|
||||
// 4. 从FIFO中循环读出16字节数据
|
||||
for(i=0; i<16; i++)
|
||||
{
|
||||
GetReg(REG_FIFODATA, &data_buff[i]);
|
||||
}
|
||||
return SUCCESS;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 写入一个16字节的数据块
|
||||
* @param block 要写入的块的绝对地址
|
||||
* @param data_buff 包含要写入的16字节数据的缓冲区
|
||||
* @return unsigned char 成功或失败
|
||||
*/
|
||||
unsigned char Mifare_Blockwrite(unsigned char block, unsigned char *data_buff)
|
||||
{
|
||||
unsigned char reg_data, i;
|
||||
SetCommand(CMD_IDLE);
|
||||
Clear_FIFO();
|
||||
// 1. 准备写块指令 (命令码 0xA0 + 块地址)
|
||||
SetReg(REG_FIFODATA, 0xA0);
|
||||
SetReg(REG_FIFODATA, block);
|
||||
SetCommand(CMD_TRANSCEIVE);
|
||||
DelayMs(5);
|
||||
// 2. 卡片会返回一个ACK (0x0A)，检查是否收到
|
||||
GetReg(REG_FIFOLENGTH, ®_data);
|
||||
if (reg_data != 1)
|
||||
return FAIL;
|
||||
GetReg(REG_FIFODATA, ®_data);
|
||||
if(reg_data != 0x0A)
|
||||
return FAIL;
|
||||
|
||||
// 3. 收到ACK后，将16字节数据写入FIFO
|
||||
for(i=0; i<16; i++)
|
||||
{
|
||||
SetReg(REG_FIFODATA, data_buff[i]);
|
||||
}
|
||||
// 4. 再次发送，将数据写入卡片
|
||||
SetCommand(CMD_TRANSCEIVE);
|
||||
DelayMs(5);
|
||||
// 5. 卡片会再次返回一个ACK，检查是否收到
|
||||
GetReg(REG_FIFOLENGTH, ®_data);
|
||||
if (reg_data != 1)
|
||||
return FAIL;
|
||||
GetReg(REG_FIFODATA, ®_data);
|
||||
if(reg_data != 0x0A)
|
||||
return FAIL;
|
||||
|
||||
return SUCCESS;
|
||||
}
|
||||
|
||||
// ... (值块操作函数的实现: Blockset, Blockinc, Blockdec, Transfer, Restore) ...
|
||||
74
apps/earphone/94_rfid_stc/READER/NTAG.c
Normal file
74
apps/earphone/94_rfid_stc/READER/NTAG.c
Normal file
@ -0,0 +1,74 @@
|
||||
#include "READER.h"
|
||||
#include "NTAG.h"
|
||||
#include <string.h>
|
||||
#include "READER_REG.h"
|
||||
#include "xt_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;
|
||||
|
||||
}
|
||||
|
||||
8
apps/earphone/94_rfid_stc/READER/NTAG.h
Normal file
8
apps/earphone/94_rfid_stc/READER/NTAG.h
Normal file
@ -0,0 +1,8 @@
|
||||
#ifndef _NTAG_H
|
||||
#define _NTAG_H
|
||||
|
||||
extern unsigned char PAGE_DATA[16];
|
||||
extern unsigned char NTAG_EVENT(void);
|
||||
extern unsigned char Read_Page(unsigned char page_num,unsigned char *page_data);
|
||||
extern unsigned char Write_Page(unsigned char page_num,unsigned char *page_data);
|
||||
#endif
|
||||
814
apps/earphone/94_rfid_stc/READER/READER.c
Normal file
814
apps/earphone/94_rfid_stc/READER/READER.c
Normal file
@ -0,0 +1,814 @@
|
||||
#include "function.h"
|
||||
#include "board.h"
|
||||
//#include "fm15l0xx_ll_spi.h"
|
||||
#include "READER.h"
|
||||
#include "READER_REG.h"
|
||||
#include "print.h"
|
||||
#include "random_generator.h"
|
||||
#include <stdio.h>
|
||||
|
||||
|
||||
#include "xt_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 <20>˳<EFBFBD>HPDģʽ <20><>mode = ENABLE <20><><EFBFBD><EFBFBD>HPDģʽ
|
||||
// {
|
||||
// if ( mode == ENABLE )
|
||||
// {
|
||||
// DelayMs( 1 ); //<2F><>ʱ1ms
|
||||
|
||||
// /******设置一个GPIO输出高*******xtell******/
|
||||
// //PD_1; // PD = 1
|
||||
// P34 = 1;
|
||||
|
||||
// }
|
||||
// else
|
||||
// {
|
||||
// /******设置一个GPIO输出低*************/
|
||||
// //PD_0; //PD = 0
|
||||
// P34 =0;
|
||||
// DelayMs( 1 ); //<2F><>ʱ1ms<6D><73><EFBFBD>ȴ<EFBFBD>Reader<65><72><EFBFBD><EFBFBD>
|
||||
// }
|
||||
// return (mode);
|
||||
// }
|
||||
|
||||
|
||||
|
||||
|
||||
//***********************************************
|
||||
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ƣ<EFBFBD>GetReg(unsigned char addr,unsigned char *regdata)
|
||||
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ܣ<EFBFBD><DCA3><EFBFBD>ȡ<EFBFBD>Ĵ<EFBFBD><C4B4><EFBFBD>ֵ
|
||||
//<2F><>ڲ<EFBFBD><DAB2><EFBFBD><EFBFBD><EFBFBD>addr:Ŀ<><C4BF>Ĵ<EFBFBD><C4B4><EFBFBD><EFBFBD><EFBFBD>ַ regdata:<3A><>ȡ<EFBFBD><C8A1>ֵ
|
||||
//<2F><><EFBFBD>ڲ<EFBFBD><DAB2><EFBFBD><EFBFBD><EFBFBD>unsigned char TRUE<55><45><EFBFBD><EFBFBD>ȡ<EFBFBD>ɹ<EFBFBD> FALSE:ʧ<><CAA7>
|
||||
//***********************************************
|
||||
|
||||
// 从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;
|
||||
// }
|
||||
|
||||
//***********************************************
|
||||
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ƣ<EFBFBD>SetReg(unsigned char addr,unsigned char* regdata)
|
||||
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ܣ<EFBFBD>д<EFBFBD>Ĵ<EFBFBD><C4B4><EFBFBD>
|
||||
//<2F><>ڲ<EFBFBD><DAB2><EFBFBD><EFBFBD><EFBFBD>addr:Ŀ<><C4BF>Ĵ<EFBFBD><C4B4><EFBFBD><EFBFBD><EFBFBD>ַ regdata:Ҫд<D2AA><D0B4><EFBFBD>ֵ
|
||||
//<2F><><EFBFBD>ڲ<EFBFBD><DAB2><EFBFBD><EFBFBD><EFBFBD>unsigned char TRUE<55><45>д<EFBFBD>ɹ<EFBFBD> FALSE:дʧ<D0B4><CAA7>
|
||||
//***********************************************
|
||||
// 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<46><4F><EFBFBD><EFBFBD><EFBFBD><EFBFBD>գ<EFBFBD><D5A3><EFBFBD>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);//<2F><><EFBFBD>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<><31>UID
|
||||
{
|
||||
cascade_level = 1;
|
||||
picc_a->UID_Length = 4;
|
||||
}
|
||||
if ((picc_a->ATQA[0]&0xC0)==0x40) //2<><32>UID
|
||||
{
|
||||
cascade_level = 2;
|
||||
picc_a->UID_Length = 8;
|
||||
}
|
||||
if ((picc_a->ATQA[0]&0xC0)==0x80) //3<><33>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)//<2F>жϴ<D0B6><CFB4><EFBFBD><EFBFBD>־
|
||||
return FAIL;
|
||||
GetReg(REG_FIFOLENGTH,®_data);
|
||||
if(reg_data != 12)//<2F>жϽ<D0B6><CFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ݳ<EFBFBD><DDB3><EFBFBD>
|
||||
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;
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
157
apps/earphone/94_rfid_stc/READER/READER.h
Normal file
157
apps/earphone/94_rfid_stc/READER/READER.h
Normal file
@ -0,0 +1,157 @@
|
||||
|
||||
#ifndef _READER_H
|
||||
#define _READER_H
|
||||
|
||||
// #include "fm15l0xx_ll_gpio.h"
|
||||
#include "function.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Ӧ<42><D3A6>
|
||||
unsigned char PUPI[4];
|
||||
unsigned char APPLICATION_DATA[4];
|
||||
unsigned char PROTOCOL_INF[3];
|
||||
unsigned char CID;//ATTRIBӦ<42><D3A6>
|
||||
unsigned char Answer_to_HALT[1];//HALTӦ<54><D3A6>
|
||||
unsigned char SN[8];//<2F><><EFBFBD><EFBFBD>֤SN<53><4E><EFBFBD><EFBFBD>
|
||||
};
|
||||
|
||||
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;
|
||||
|
||||
|
||||
//<2F><><EFBFBD>ղ<EFBFBD><D5B2><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> TYPE A
|
||||
#define GAIN_A 7 //<2F><><EFBFBD>÷<EFBFBD>Χ0~7
|
||||
#define HPCF_A 3 //<2F><><EFBFBD>÷<EFBFBD>Χ0~7
|
||||
#define AMPLITUDE_A 255 //<2F><><EFBFBD>÷<EFBFBD>Χ0~255
|
||||
|
||||
|
||||
//<2F><><EFBFBD>ղ<EFBFBD><D5B2><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> TYPE B
|
||||
#define GAIN_B 7 //<2F><><EFBFBD>÷<EFBFBD>Χ0~7
|
||||
#define HPCF_B 3 //<2F><><EFBFBD>÷<EFBFBD>Χ0~7
|
||||
#define AMPLITUDE_B 255 //<2F><><EFBFBD>÷<EFBFBD>Χ0~255<35><35><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֵԽ<D6B5><D4BD><EFBFBD>ز<EFBFBD>Խ<EFBFBD><D4BD>
|
||||
|
||||
#define MODULATION_B 100//<2F><><EFBFBD>÷<EFBFBD>Χ0~255,<2C><><EFBFBD><EFBFBD>ֵԽС<D4BD><D0A1><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Խ<EFBFBD><D4BD>
|
||||
|
||||
//<2F><><EFBFBD>ղ<EFBFBD><D5B2><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> TYPE V
|
||||
#define GAIN_V 7//<2F><><EFBFBD>÷<EFBFBD>Χ0~7
|
||||
#define HPCF_V 4//<2F><><EFBFBD>÷<EFBFBD>Χ0~7
|
||||
#define AMPLITUDE_V 255 //<2F><><EFBFBD>÷<EFBFBD>Χ0~255<35><35><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֵԽ<D6B5><D4BD><EFBFBD>ز<EFBFBD>Խ<EFBFBD><D4BD>
|
||||
#define MODULATION_V 10 //<2F><><EFBFBD>÷<EFBFBD>Χ0~255,<2C><><EFBFBD><EFBFBD>ֵԽС<D4BD><D0A1><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Խ<EFBFBD><D4BD>
|
||||
|
||||
|
||||
//<2F><><EFBFBD>ղ<EFBFBD><D5B2><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> TYPE F
|
||||
#define GAIN_F 7//<2F><><EFBFBD>÷<EFBFBD>Χ0~7
|
||||
#define HPCF_F 4//<2F><><EFBFBD>÷<EFBFBD>Χ0~7
|
||||
#define AMPLITUDE_F 255 //<2F><><EFBFBD>÷<EFBFBD>Χ0~255<35><35><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֵԽ<D6B5><D4BD><EFBFBD>ز<EFBFBD>Խ<EFBFBD><D4BD>
|
||||
|
||||
#define MODULATION_F 100//<2F><><EFBFBD>÷<EFBFBD>Χ0~255,<2C><><EFBFBD><EFBFBD>ֵԽС<D4BD><D0A1><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Խ<EFBFBD><D4BD>
|
||||
|
||||
#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
|
||||
|
||||
478
apps/earphone/94_rfid_stc/READER/READER_REG.h
Normal file
478
apps/earphone/94_rfid_stc/READER/READER_REG.h
Normal file
@ -0,0 +1,478 @@
|
||||
/*********************************************************************
|
||||
* *
|
||||
* Copyright (c) 2010 Shanghai FuDan MicroElectronic Inc, Ltd. *
|
||||
* All rights reserved. Licensed Software Material. *
|
||||
* *
|
||||
* Unauthorized use, duplication, or distribution is strictly *
|
||||
* prohibited by law. *
|
||||
* *
|
||||
**********************************************************************/
|
||||
#ifndef _READER_REG_H
|
||||
#define _READER_REG_H
|
||||
|
||||
#define REG_COMMAND 0x00 //
|
||||
#define REG_HOSTCTRL 0x01 //
|
||||
#define REG_FIFOCONTROL 0x02 //
|
||||
#define REG_WATERLEVEL 0x03 //
|
||||
#define REG_FIFOLENGTH 0x04 //
|
||||
#define REG_FIFODATA 0x05 //
|
||||
#define REG_IRQ0 0x06 //
|
||||
#define REG_IRQ1 0x07 //
|
||||
#define REG_IRQ0EN 0x08 //
|
||||
#define REG_IRQ1EN 0x09 //
|
||||
#define REG_ERROR 0x0A //
|
||||
#define REG_STATUS 0x0B //
|
||||
#define REG_RXBITCTRL 0x0C //
|
||||
#define REG_RXCOLL 0x0D //
|
||||
#define REG_TCONTROL 0x0E //
|
||||
#define REG_T0CONTROL 0x0F //
|
||||
|
||||
|
||||
#define REG_T0RELOADHI 0x10 //
|
||||
#define REG_T0RELOADLO 0x11 //
|
||||
#define REG_T0COUNTERVALHI 0x12 //
|
||||
#define REG_T0COUNTERVALLO 0x13 //
|
||||
#define REG_T1CONTROL 0x14 //
|
||||
#define REG_T1RELOADHI 0x15 //
|
||||
#define REG_T1RELOADLO 0x16 //
|
||||
#define REG_T1COUNTERVALHI 0x17 //
|
||||
#define REG_T1COUNTERVALLO 0x18 //
|
||||
#define REG_T2CONTROL 0x19 //
|
||||
#define REG_T2RELOADHI 0x1A //
|
||||
#define REG_T2RELOADLO 0x1B //
|
||||
#define REG_T2COUNTERVALHI 0x1C //
|
||||
#define REG_T2COUNTERVALLO 0x1D //
|
||||
#define REG_T3CONTROL 0x1E //
|
||||
#define REG_T3RELOADHI 0x1F //
|
||||
|
||||
|
||||
#define REG_T3RELOADLO 0x20 //
|
||||
#define REG_T3COUNTERVALHI 0x21 //
|
||||
#define REG_T3COUNTERVALLO 0x22 //
|
||||
#define REG_T4CONTROL 0x23 //
|
||||
#define REG_T4RELOADHI 0x24 //
|
||||
#define REG_T4RELOADLO 0x25 //
|
||||
#define REG_T4COUNTERVALHI 0x26 //
|
||||
#define REG_T4COUNTERVALLO 0x27 //
|
||||
#define REG_TXMODE 0x28
|
||||
#define REG_TXAMP 0x29
|
||||
#define REG_TXCON 0x2A //
|
||||
#define REG_TXI 0x2B //
|
||||
#define REG_TXCRCCON 0x2C //
|
||||
#define REG_RXCRCCON 0x2D //
|
||||
#define REG_TXDATANUM 0x2E
|
||||
#define REG_TXMODWIDTH 0x2F //
|
||||
|
||||
|
||||
#define REG_TXSYM10BURSTLEN 0x30 //
|
||||
#define REG_TXWAITCTRL 0x31 //
|
||||
#define REG_TXWAITLO 0x32 //
|
||||
#define REG_FRAMECON 0x33 //
|
||||
#define REG_RXSOFD 0x34 //
|
||||
#define REG_RXCTRL 0x35 //
|
||||
#define REG_RXWAIT 0x36 //
|
||||
#define REG_RXTHRESHOLD 0x37 //
|
||||
#define REG_RCV 0x38 //
|
||||
#define REG_RXANA 0x39 //
|
||||
#define REG_LPCD_OPTIONS 0x3A //
|
||||
#define REG_SERIALSPEED 0x3B //
|
||||
#define REG_LFO_TRIMM 0x3C //
|
||||
#define REG_CLKOUT_CTRL 0x3D //
|
||||
#define REG_LPCD_THRESHOLD 0x3E //
|
||||
#define REG_LPCD_QMIN 0x3F //
|
||||
#define REG_LPCD_QMAX 0x40
|
||||
#define REG_LPCD_IMIN 0x41
|
||||
#define REG_LPCD_RESULT_I 0x42
|
||||
#define REG_LPCD_RESULT_Q 0x43
|
||||
#define REG_THNADJ 0x5F
|
||||
#define REG_THNSET 0x61
|
||||
#define REG_THNMIN 0x62
|
||||
#define REG_DSP_CTRL1 0x64
|
||||
#define REG_MISC 0x75
|
||||
#define REG_RXTXCON 0x77
|
||||
#define REG_ERROREXT 0x7E
|
||||
#define REG_VERSION 0x7F
|
||||
|
||||
#define CMD_MASK 0x1F
|
||||
|
||||
#define CMD_IDLE 0x00
|
||||
#define CMD_LPCD 0x01
|
||||
#define CMD_LOADKEY 0x02
|
||||
#define CMD_AUTHENT 0x03
|
||||
#define CMD_RECEIVE 0x05
|
||||
#define CMD_TRANSMIT 0x06
|
||||
#define CMD_TRANSCEIVE 0x07
|
||||
#define CMD_WRITEE2 0x08
|
||||
#define CMD_WRITEE2PAGE 0x09
|
||||
#define CMD_READE2 0x0A
|
||||
#define CMD_LOADREG 0x0C
|
||||
#define CMD_LOADPROTOCOL 0x0D
|
||||
#define CMD_LOADKEYE2 0x0E
|
||||
#define CMD_STOREKEYE2 0x0F
|
||||
#define CMD_CRCCALC 0x1B
|
||||
#define CMD_READRNR 0x1C
|
||||
#define CMD_SOFTRESET 0x1F
|
||||
|
||||
|
||||
/** \name Host-Control Register Contents (0x00)
|
||||
*/
|
||||
/*@{*/
|
||||
#define BIT_STANDBY 0x80U /**< Standby bit; If set, the IC transits to standby mode. */
|
||||
#define BIT_MODEMOFF 0x40U
|
||||
/*@{*/
|
||||
/** \name Host-Control Register Contents (0x01)
|
||||
*/
|
||||
/*@{*/
|
||||
#define BIT_I2CFORCEHS 0x01U
|
||||
//#define BIT_REGEN 0x80U
|
||||
//#define BIT_BUSHOST 0x40U
|
||||
//#define BIT_BUSSAM 0x20U
|
||||
//#define MASK_SAMINTERFACE 0x0CU
|
||||
/*@}*/
|
||||
|
||||
/** \name FIFO-Control Register Contents (0x02)
|
||||
*/
|
||||
/*@{*/
|
||||
#define BIT_FIFOSIZE 0x80U
|
||||
#define BIT_HIALERT 0x40U
|
||||
#define BIT_LOALERT 0x20U
|
||||
#define BIT_FIFOFLUSH 0x10U
|
||||
#define BIT_WATERLEVEL_HI 0x04U
|
||||
#define MASK_FIFOLENGTH_HI 0x03U
|
||||
/*@}*/
|
||||
|
||||
/** \name IRQ0 Register(s) Contents (0x06/0x08)
|
||||
*/
|
||||
/*@{*/
|
||||
#define BIT_SET 0x80U
|
||||
#define BIT_IRQINV 0x80U
|
||||
#define BIT_HIALERTIRQ 0x40U
|
||||
#define BIT_LOALERTIRQ 0x20U
|
||||
#define BIT_IDLEIRQ 0x10U
|
||||
#define BIT_TXIRQ 0x08U
|
||||
#define BIT_RXIRQ 0x04U
|
||||
#define BIT_ERRIRQ 0x02U
|
||||
#define BIT_RXSOFIRQ 0x01U
|
||||
/*@}*/
|
||||
|
||||
/** \name IRQ1 Register(s) Contents (0x07/0x09)
|
||||
*/
|
||||
/*@{*/
|
||||
/* #define BIT_SET 0x80U */
|
||||
#define BIT_IRQPUSHPULL 0x80U
|
||||
#define BIT_GLOBALIRQ 0x40U
|
||||
#define BIT_IRQPINEN 0x40U
|
||||
#define BIT_LPCDIRQ 0x20U
|
||||
#define BIT_TIMER4IRQ 0x10U
|
||||
#define BIT_TIMER3IRQ 0x08U
|
||||
#define BIT_TIMER2IRQ 0x04U
|
||||
#define BIT_TIMER1IRQ 0x02U
|
||||
#define BIT_TIMER0IRQ 0x01U
|
||||
/*@}*/
|
||||
|
||||
/** \name Error Register Contents (0x0A)
|
||||
*/
|
||||
/*@{*/
|
||||
#define BIT_CMDEE_ERR 0x80U
|
||||
#define BIT_FIFOWRERR 0x40U
|
||||
#define BIT_FIFOOVL 0x20U
|
||||
#define BIT_MINFRAMEERR 0x10U
|
||||
#define BIT_NODATAERR 0x08U
|
||||
#define BIT_COLLDET 0x04U
|
||||
#define BIT_PROTERR 0x02U
|
||||
#define BIT_INTEGERR 0x01U
|
||||
/*@}*/
|
||||
|
||||
/** \name Status Register Contents (0x0B)
|
||||
*/
|
||||
/*@{*/
|
||||
#define BIT_CRYPTO1ON 0x20U
|
||||
#define MASK_COMMSTATE 0x07U
|
||||
/*@}*/
|
||||
|
||||
/** \name Rx-Bit-Control Register Contents (0x0C)
|
||||
*/
|
||||
/*@{*/
|
||||
#define BIT_VALUESAFTERCOLL 0x80U
|
||||
#define BIT_NOCOLL 0x08U
|
||||
#define MASK_RXALIGN 0x70U
|
||||
#define MASK_RXLASTBITS 0x07U
|
||||
/*@}*/
|
||||
|
||||
/** \name Rx-Coll Register Contents (0x0D)
|
||||
*/
|
||||
/*@{*/
|
||||
#define BIT_COLLPOSVALID 0x80U
|
||||
#define MASK_COLLPOS 0x7FU
|
||||
/*@}*/
|
||||
|
||||
/** \name Timer-Control Register Contents (0x0E)
|
||||
*/
|
||||
/*@{*/
|
||||
#define BIT_T3RUNNING 0x80U
|
||||
#define BIT_T2RUNNING 0x40U
|
||||
#define BIT_T1RUNNING 0x20U
|
||||
#define BIT_T0RUNNING 0x10U
|
||||
#define BIT_T3STARTSTOPNOW 0x08U
|
||||
#define BIT_T2STARTSTOPNOW 0x04U
|
||||
#define BIT_T1STARTSTOPNOW 0x02U
|
||||
#define BIT_T0STARTSTOPNOW 0x01U
|
||||
/*@}*/
|
||||
|
||||
/** \name T[0-3]-Control Register Contents (0x0F/0x14/0x19/0x1E)
|
||||
*/
|
||||
/*@{*/
|
||||
#define BIT_TSTOP_RX 0x80U /**< Stop timer on receive interrupt. */
|
||||
#define BIT_TAUTORESTARTED 0x08U /**< Auto-restart timer after underflow. */
|
||||
#define BIT_TSTART_TX 0x10U /**< Start timer on transmit interrupt. */
|
||||
//#define BIT_TSTART_LFO 0x20U /**< Use this timer for LFO trimming. */
|
||||
//#define BIT_TSTART_LFO_UV 0x30U /**< Use this timer for LFO trimming (generate UV at a trimming event). */
|
||||
#define MASK_TSTART 0x30U /**< Mask for TSTART bits. */
|
||||
#define VALUE_TCLK_1356_MHZ 0x00U /**< Use 13.56MHz as input clock. */
|
||||
#define VALUE_TCLK_212_KHZ 0x01U /**< Use 212KHz as input clock. */
|
||||
#define VALUE_TCLK_T0 0x02U /**< Use timer0 as input clock. */
|
||||
#define VALUE_TCLK_T1 0x03U /**< Use timer1 as input clock. */
|
||||
/*@}*/
|
||||
|
||||
/** \name T4-Control Register Contents (0x23)
|
||||
*/
|
||||
/*@{*/
|
||||
#define BIT_T4RUNNING 0x80U
|
||||
#define BIT_T4STARTSTOPNOW 0x40U
|
||||
#define BIT_T4AUTOTRIMM 0x20U
|
||||
#define BIT_T4AUTOLPCD 0x10U
|
||||
#define BIT_T4AUTORESTARTED 0x08U
|
||||
#define BIT_T4AUTOWAKEUP 0x04U
|
||||
/*#define MASK_TSTART 0x30U*/
|
||||
#define VALUE_TCLK_LFO_64_KHZ 0x00U
|
||||
#define VALUE_TCLK_LFO_8_KHZ 0x01U
|
||||
#define VALUE_TCLK_LFO_4_KHZ 0x02U
|
||||
#define VALUE_TCLK_LFO_2_KHZ 0x03U
|
||||
/*@}*/
|
||||
|
||||
/** \name Driver Mode Register Contents (0x28)
|
||||
*/
|
||||
/*@{*/
|
||||
#define BIT_TX2INV 0x80U
|
||||
#define BIT_TX1INV 0x40U
|
||||
#define BIT_TXEN 0x08U
|
||||
#define VALUE_TXCLKMODE_HIGHIMPEDANCE 0x00U
|
||||
#define VALUE_TXCLKMODE_OUTPULL0 0x01U
|
||||
#define VALUE_TXCLKMODE_OUTPULL1 0x02U
|
||||
#define VALUE_TXCLKMODE_RFLOWPULL 0x05U
|
||||
#define VALUE_TXCLKMODE_RFHIGHPUSH 0x06U
|
||||
#define VALUE_TXCLKMODE_PUSHPULL 0x07U
|
||||
#define BIT_RFON 0x04U
|
||||
#define BIT_TPUSHON 0x02U
|
||||
#define BIT_TPULLON 0x01U
|
||||
/*@}*/
|
||||
|
||||
/** \name Tx Amplifier Register Contents (0x29)
|
||||
*/
|
||||
/*@{*/
|
||||
#define MASK_CW_AMPLITUDE 0x00U
|
||||
#define MASK_RESIDUAL_CARRIER 0x1FU
|
||||
/*@}*/
|
||||
|
||||
/** \name Driver Control Register Contents (0x2A)
|
||||
*/
|
||||
/*@{*/
|
||||
#define BIT_CWMAX 0x08U
|
||||
#define BIT_DRIVERINV 0x04U
|
||||
#define VALUE_DRIVERSEL_LOW 0x00U
|
||||
#define VALUE_DRIVERSEL_TXENV 0x01U
|
||||
#define VALUE_DRIVERSEL_SIGIN 0x02U
|
||||
/*@}*/
|
||||
|
||||
/** \name Tx-/Rx-CRC Control Register Contents (0x2C/0x2D)
|
||||
*/
|
||||
/*@{*/
|
||||
#define BIT_RXFORCECRCWRITE 0x80U
|
||||
#define BIT_CRCINVERT 0x02U
|
||||
#define BIT_CRCEN 0x01U
|
||||
#define MASK_CRCPRESETVAL 0x70U
|
||||
#define MASK_CRCTYPE 0x0CU
|
||||
#define MASK_CRCTYPE5 0x00U
|
||||
#define MASK_CRCTYPE16 0x08U
|
||||
/*@}*/
|
||||
|
||||
/** \name Tx-DataNum Register Contents (0x2E)
|
||||
*/
|
||||
/*@{*/
|
||||
#define BIT_KEEPBITGRID 0x10U
|
||||
#define BIT_DATAEN 0x08U
|
||||
#define MASK_TXLASTBITS 0x07U
|
||||
#define MASK_SYMBOL_SEND 0x08U
|
||||
/*@}*/
|
||||
|
||||
/** \name Tx-Wait Control Register Contents (0x31)
|
||||
*/
|
||||
/*@{*/
|
||||
#define BIT_TXWAIT_START_RX 0x80U
|
||||
#define BIT_TXWAIT_DBFREQ 0x40U
|
||||
#define MASK_TXWAITHI 0x38U
|
||||
#define MASK_TXSTOPBITLEN 0x07U
|
||||
/*@}*/
|
||||
|
||||
/** \name Frame Control Register Contents (0x33)
|
||||
*/
|
||||
/*@{*/
|
||||
#define BIT_TXPARITYEN 0x80U
|
||||
#define BIT_RXPARITYEN 0x40U
|
||||
#define VALUE_STOP_SYM3 0x0CU
|
||||
#define VALUE_STOP_SYM2 0x08U
|
||||
#define VALUE_STOP_SYM1 0x04U
|
||||
#define VALUE_START_SYM3 0x03U
|
||||
#define VALUE_START_SYM2 0x02U
|
||||
#define VALUE_START_SYM1 0x01U
|
||||
#define MASK_STARTSYM 0x03U
|
||||
#define MASK_STOPSYM 0x0CU
|
||||
/*@}*/
|
||||
|
||||
/** \name Rx Control Register Contents (0x35)
|
||||
*/
|
||||
/*@{*/
|
||||
#define BIT_RXALLOWBITS 0x80U
|
||||
#define BIT_RXMULTIPLE 0x40U
|
||||
#define BIT_RXEOFTYPE 0x20U
|
||||
#define BIT_EGT_CHECK 0x10U
|
||||
#define BIT_EMD_SUPPRESSION 0x08U
|
||||
#define MASK_RXBAUDRATE 0x07U
|
||||
/*@}*/
|
||||
|
||||
/** \name Rx-Wait Register Contents (0x36)
|
||||
*/
|
||||
/*@{*/
|
||||
#define BIT_RXWAITDBFREQ 0x80U
|
||||
#define MASK_RXWAIT 0x7FU
|
||||
/*@}*/
|
||||
|
||||
/** \name Rx-Threshold Register Contents (0x37)
|
||||
*/
|
||||
/*@{*/
|
||||
#define MASK_MINLEVEL 0xF0U
|
||||
#define MASK_MINLEVELP 0x0FU
|
||||
/*@}*/
|
||||
|
||||
/** \name Rx-Receiver Register Contents (0x38)
|
||||
*/
|
||||
/*@{*/
|
||||
#define BIT_RX_SINGLE 0x80U
|
||||
#define BIT_RX_SHORT_MIX2ADC 0x40U
|
||||
#define BIT_USE_SMALL_EVAL 0x04U
|
||||
#define MASK_RX_SIGPRO_IN_SEL 0x30U
|
||||
#define MASK_COLLLEVEL 0x03U
|
||||
/*@}*/
|
||||
|
||||
/** \name Rx-Analog Register Contents (0x39)
|
||||
*/
|
||||
/*@{*/
|
||||
#define BIT_RX_OC_ENABLE 0x20U
|
||||
#define BIT_RX_HP_LOWF 0x10U
|
||||
#define MASK_VMID_R_SEL 0xC0U
|
||||
#define MASK_RCV_HPCF 0x0CU
|
||||
#define MASK_RCV_GAIN 0x03U
|
||||
/*@}*/
|
||||
|
||||
/** \name Serial-Speed Register Contents (0x3B)
|
||||
*/
|
||||
/*@{*/
|
||||
#define MASK_BR_T0 0xE0U
|
||||
#define MASK_BR_T1 0x1FU
|
||||
/*@}*/
|
||||
|
||||
|
||||
/** \name LPCD Result(Q) Register Contents (0x43)
|
||||
*/
|
||||
/*@{*/
|
||||
#define BIT_LPCDIRQ_CLR 0x40U
|
||||
/*@}*/
|
||||
|
||||
/** \name Tx-BitMod Register Contents (0x48)
|
||||
*/
|
||||
/*@{*/
|
||||
#define BIT_TXMSBFIRST 0x80U
|
||||
#define BIT_TXPARITYTYPE 0x20U
|
||||
#define BIT_TXSTOPBITTYPE 0x08U
|
||||
#define BIT_TXSTARTBITTYPE 0x02U
|
||||
#define BIT_TXSTARTBITEN 0x01U
|
||||
/*@}*/
|
||||
|
||||
/** \name Rx-BitMod Register Contents (0x58)
|
||||
*/
|
||||
/*@{*/
|
||||
#define BIT_RXSTOPONINVPAR 0x20U
|
||||
#define BIT_RXSTOPONLEN 0x10U
|
||||
#define BIT_RXMSBFIRST 0x08U
|
||||
#define BIT_RXSTOPBITEN 0x04U
|
||||
#define BIT_RXPARITYTYPE 0x02U
|
||||
/*@}*/
|
||||
|
||||
/** \name Rx-Mod Register Contents (0x5D)
|
||||
*/
|
||||
/*@{*/
|
||||
#define BIT_PREFILTER 0x20U
|
||||
#define BIT_RECTFILTER 0x10U
|
||||
#define BIT_SYNCHIGH 0x08U
|
||||
#define BIT_CORRINV 0x04U
|
||||
#define BIT_FSK 0x02U
|
||||
#define BIT_BPSK 0x01U
|
||||
/*@}*/
|
||||
|
||||
/** \name RxSupCfg Register Contents (0x6E)
|
||||
*/
|
||||
/*@{*/
|
||||
#define BIT_RXNOERR 0x80U
|
||||
/*@}*/
|
||||
/** \name RxTxConReg Register Contents (0x77)
|
||||
*/
|
||||
/*@{*/
|
||||
#define BIT_SHMODE 0x08U //<2F>Ϻ<EFBFBD><CFBA>㷨
|
||||
/*@}*/
|
||||
|
||||
/** \name ErrorExtReg Register Contents (0x7E)
|
||||
*/
|
||||
/*@{*/
|
||||
#define PARITY_ERROR 0x08U
|
||||
#define CRC_ERROR 0x04U
|
||||
/*@{*/
|
||||
|
||||
#define LPCD_OPTION2 0x1DF
|
||||
|
||||
//---------------------------------------------------------------
|
||||
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Э<EFBFBD><D0AD><EFBFBD><EFBFBD>
|
||||
#define RX_TYPEA_106 0
|
||||
#define RX_TYPEA_212 1
|
||||
#define RX_TYPEA_424 2
|
||||
#define RX_TYPEA_848 3
|
||||
|
||||
#define RX_TYPEB_106 4
|
||||
#define RX_TYPEB_212 5
|
||||
#define RX_TYPEB_424 6
|
||||
#define RX_TYPEB_848 7
|
||||
|
||||
#define RX_TYPEV_26 10
|
||||
#define RX_TYPEV_53 11
|
||||
|
||||
#define RX_FELICA_212 19
|
||||
#define RX_FELICA_424 20
|
||||
|
||||
//<2F><><EFBFBD>巢<EFBFBD><E5B7A2>Э<EFBFBD><D0AD><EFBFBD><EFBFBD>
|
||||
#define TX_TYPEA_106 0
|
||||
#define TX_TYPEA_212 1
|
||||
#define TX_TYPEA_424 2
|
||||
#define TX_TYPEA_848 3
|
||||
|
||||
#define TX_TYPEB_106 4
|
||||
#define TX_TYPEB_212 5
|
||||
#define TX_TYPEB_424 6
|
||||
#define TX_TYPEB_848 7
|
||||
|
||||
#define TX_TYPEV_26 10
|
||||
#define TX_TYPEV_53 11
|
||||
|
||||
#define TX_FELICA_212 19
|
||||
#define TX_FELICA_424 20
|
||||
|
||||
#endif
|
||||
|
||||
225
apps/earphone/94_rfid_stc/READER/READER_REG_commented.h
Normal file
225
apps/earphone/94_rfid_stc/READER/READER_REG_commented.h
Normal file
@ -0,0 +1,225 @@
|
||||
/**
|
||||
* @file READER_REG.h
|
||||
* @brief RFID 读卡器芯片寄存器定义
|
||||
* @version 1.0
|
||||
* @date 2024-05-20
|
||||
*
|
||||
* @copyright Copyright (c) 2010 Shanghai FuDan MicroElectronic Inc, Ltd.
|
||||
*
|
||||
* @par 文件作用:
|
||||
* 这个头文件是整个RFID驱动库的最底层基石。它定义了RFID读卡器芯片
|
||||
* (例如FM176XX系列)内部所有寄存器的地址、每个寄存器中功能位的掩码，
|
||||
* 以及芯片支持的各种命令码。
|
||||
* MCU通过SPI接口读写这些寄存器，从而精确控制芯片的各项功能，
|
||||
* 例如启动/停止命令、配置通信协议、读写FIFO数据、查询状态等。
|
||||
* 所有上层对RFID的操作，最终都会转化为对这个文件中定义的寄存器的读写。
|
||||
*/
|
||||
#ifndef _READER_REG_H
|
||||
#define _READER_REG_H
|
||||
|
||||
//==================================================================================
|
||||
// 1. 寄存器地址定义 (Register Address Definitions)
|
||||
// 这些是芯片内部功能单元的地址，通过SPI访问。
|
||||
//==================================================================================
|
||||
#define REG_COMMAND 0x00 // 命令寄存器: 启动/停止芯片执行的命令 (如 Idle, Transceive)
|
||||
#define REG_HOSTCTRL 0x01 // 主机控制寄存器
|
||||
#define REG_FIFOCONTROL 0x02 // FIFO 控制寄存器 (如刷新FIFO)
|
||||
#define REG_WATERLEVEL 0x03 // FIFO 水位寄存器: 用于中断触发
|
||||
#define REG_FIFOLENGTH 0x04 // FIFO 长度寄存器: 当前FIFO中存储的字节数
|
||||
#define REG_FIFODATA 0x05 // FIFO 数据寄存器: 通过此寄存器写入要发送的数据，或读出接收到的数据
|
||||
#define REG_IRQ0 0x06 // 中断请求标志寄存器 0
|
||||
#define REG_IRQ1 0x07 // 中断请求标志寄存器 1
|
||||
#define REG_IRQ0EN 0x08 // 中断使能寄存器 0
|
||||
#define REG_IRQ1EN 0x09 // 中断使能寄存器 1
|
||||
#define REG_ERROR 0x0A // 错误标志寄存器: 显示通信中发生的错误 (如CRC错误, 冲突)
|
||||
#define REG_STATUS 0x0B // 状态寄存器: 显示芯片当前状态 (如MIFARE加密是否开启)
|
||||
#define REG_RXBITCTRL 0x0C // 接收数据位控制寄存器
|
||||
#define REG_RXCOLL 0x0D // 接收冲突检测寄存器
|
||||
#define REG_TCONTROL 0x0E // 定时器总体控制
|
||||
#define REG_T0CONTROL 0x0F // 定时器0 控制
|
||||
#define REG_T0RELOADHI 0x10 // 定时器0 重载值高字节
|
||||
#define REG_T0RELOADLO 0x11 // 定时器0 重载值低字节
|
||||
#define REG_T0COUNTERVALHI 0x12 // 定时器0 计数值高字节
|
||||
#define REG_T0COUNTERVALLO 0x13 // 定时器0 计数值低字节
|
||||
#define REG_T1CONTROL 0x14 // 定时器1 控制
|
||||
#define REG_T1RELOADHI 0x15 // 定时器1 重载值高字节
|
||||
#define REG_T1RELOADLO 0x16 // 定时器1 重载值低字节
|
||||
#define REG_T1COUNTERVALHI 0x17 // 定时器1 计数值高字节
|
||||
#define REG_T1COUNTERVALLO 0x18 // 定时器1 计数值低字节
|
||||
#define REG_T2CONTROL 0x19 // 定时器2 控制
|
||||
#define REG_T2RELOADHI 0x1A // 定时器2 重载值高字节
|
||||
#define REG_T2RELOADLO 0x1B // 定时器2 重载值低字节
|
||||
#define REG_T2COUNTERVALHI 0x1C // 定时器2 计数值高字节
|
||||
#define REG_T2COUNTERVALLO 0x1D // 定时器2 计数值低字节
|
||||
#define REG_T3CONTROL 0x1E // 定时器3 控制
|
||||
#define REG_T3RELOADHI 0x1F // 定时器3 重载值高字节
|
||||
#define REG_T3RELOADLO 0x20 // 定时器3 重载值低字节
|
||||
#define REG_T3COUNTERVALHI 0x21 // 定时器3 计数值高字节
|
||||
#define REG_T3COUNTERVALLO 0x22 // 定时器3 计数值低字节
|
||||
#define REG_T4CONTROL 0x23 // 定时器4 控制
|
||||
#define REG_T4RELOADHI 0x24 // 定时器4 重载值高字节
|
||||
#define REG_T4RELOADLO 0x25 // 定时器4 重载值低字节
|
||||
#define REG_T4COUNTERVALHI 0x26 // 定时器4 计数值高字节
|
||||
#define REG_T4COUNTERVALLO 0x27 // 定时器4 计数值低字节
|
||||
#define REG_TXMODE 0x28 // 发射模式寄存器
|
||||
#define REG_TXAMP 0x29 // 发射信号幅度(天线功率)寄存器
|
||||
#define REG_TXCON 0x2A // 发射控制寄存器 (调制深度等)
|
||||
#define REG_TXI 0x2B // 发射器内部控制
|
||||
#define REG_TXCRCCON 0x2C // 发射CRC控制寄存器
|
||||
#define REG_RXCRCCON 0x2D // 接收CRC控制寄存器
|
||||
#define REG_TXDATANUM 0x2E // 发送数据位数控制寄存器
|
||||
#define REG_TXMODWIDTH 0x2F // 发射调制脉宽寄存器
|
||||
#define REG_TXSYM10BURSTLEN 0x30 // TypeB的10%调制脉冲长度
|
||||
#define REG_TXWAITCTRL 0x31 // 发送等待控制寄存器
|
||||
#define REG_TXWAITLO 0x32 // 发送等待时间低字节
|
||||
#define REG_FRAMECON 0x33 // 帧控制寄存器 (奇偶校验等)
|
||||
#define REG_RXSOFD 0x34 // 接收起始标志位(SOF)配置
|
||||
#define REG_RXCTRL 0x35 // 接收控制寄存器
|
||||
#define REG_RXWAIT 0x36 // 接收等待时间寄存器
|
||||
#define REG_RXTHRESHOLD 0x37 // 接收信号阈值寄存器
|
||||
#define REG_RCV 0x38 // 接收器配置寄存器
|
||||
#define REG_RXANA 0x39 // 接收器模拟电路配置寄存器 (增益等)
|
||||
#define REG_LPCD_OPTIONS 0x3A // 低功耗寻卡(LPCD)选项
|
||||
#define REG_SERIALSPEED 0x3B // 串行接口速率配置
|
||||
#define REG_LFO_TRIMM 0x3C // 低频振荡器(LFO)微调
|
||||
#define REG_CLKOUT_CTRL 0x3D // 时钟输出控制
|
||||
#define REG_LPCD_THRESHOLD 0x3E // LPCD阈值
|
||||
#define REG_LPCD_QMIN 0x3F // LPCD Q通道最小值
|
||||
#define REG_LPCD_QMAX 0x40 // LPCD Q通道最大值
|
||||
#define REG_LPCD_IMIN 0x41 // LPCD I通道最小值
|
||||
#define REG_LPCD_RESULT_I 0x42 // LPCD I通道结果
|
||||
#define REG_LPCD_RESULT_Q 0x43 // LPCD Q通道结果
|
||||
#define REG_THNADJ 0x5F
|
||||
#define REG_THNSET 0x61
|
||||
#define REG_THNMIN 0x62
|
||||
#define REG_DSP_CTRL1 0x64
|
||||
#define REG_MISC 0x75 // 杂项配置
|
||||
#define REG_RXTXCON 0x77 // 收发控制寄存器
|
||||
#define REG_ERROREXT 0x7E // 扩展错误标志
|
||||
#define REG_VERSION 0x7F // 版本寄存器: 读取此寄存器可获取芯片型号和版本
|
||||
|
||||
//==================================================================================
|
||||
// 2. 命令码定义 (Command Codes)
|
||||
// 写入到 REG_COMMAND 寄存器的值，用于启动芯片执行特定任务。
|
||||
//==================================================================================
|
||||
#define CMD_MASK 0x1F // 命令码掩码
|
||||
|
||||
#define CMD_IDLE 0x00 // 空闲命令: 停止当前操作，进入空闲状态
|
||||
#define CMD_LPCD 0x01 // 低功耗寻卡 (Low-Power Card Detection)
|
||||
#define CMD_LOADKEY 0x02 // 加载密钥命令: 将密钥从FIFO加载到内部密钥缓冲器 (用于MIFARE认证)
|
||||
#define CMD_AUTHENT 0x03 // MIFARE认证命令: 执行MIFARE Classic卡的三重认证流程
|
||||
#define CMD_RECEIVE 0x05 // 接收命令: 仅开启接收器，等待接收数据
|
||||
#define CMD_TRANSMIT 0x06 // 发送命令: 将FIFO中的数据发送出去
|
||||
#define CMD_TRANSCEIVE 0x07 // 发送并接收命令: 核心命令，先发送FIFO中的数据，然后自动进入接收状态等待卡片响应
|
||||
#define CMD_WRITEE2 0x08 // 写E2PROM
|
||||
#define CMD_WRITEE2PAGE 0x09 // 写E2PROM页
|
||||
#define CMD_READE2 0x0A // 读E2PROM
|
||||
#define CMD_LOADREG 0x0C // 从E2PROM加载寄存器值
|
||||
#define CMD_LOADPROTOCOL 0x0D // 加载协议命令: 配置芯片以支持特定的通信协议 (Type A/B/V/F)
|
||||
#define CMD_LOADKEYE2 0x0E // 从E2PROM加载密钥
|
||||
#define CMD_STOREKEYE2 0x0F // 存储密钥到E2PROM
|
||||
#define CMD_CRCCALC 0x1B // CRC计算
|
||||
#define CMD_READRNR 0x1C // 读随机数
|
||||
#define CMD_SOFTRESET 0x1F // 软复位命令: 复位芯片
|
||||
|
||||
//==================================================================================
|
||||
// 3. 寄存器功能位定义 (Bit Definitions for Registers)
|
||||
// 定义了每个寄存器中特定位的功能，方便通过位操作来修改寄存器值。
|
||||
//==================================================================================
|
||||
|
||||
/** @name Command Register (0x00) 功能位 */
|
||||
#define BIT_STANDBY 0x80U // 置1使芯片进入待机模式
|
||||
#define BIT_MODEMOFF 0x40U // 关闭射频和调制解调器
|
||||
|
||||
/** @name FIFO-Control Register (0x02) 功能位 */
|
||||
#define BIT_FIFOSIZE 0x80U // FIFO大小选择 (0=64字节, 1=512字节)
|
||||
#define BIT_HIALERT 0x40U // 高水位中断标志
|
||||
#define BIT_LOALERT 0x20U // 低水位中断标志
|
||||
#define BIT_FIFOFLUSH 0x10U // FIFO刷新位。向此位写1可清空FIFO。
|
||||
#define BIT_WATERLEVEL_HI 0x04U
|
||||
#define MASK_FIFOLENGTH_HI 0x03U
|
||||
|
||||
/** @name IRQ0 Register (0x06) & IRQ0EN Register (0x08) 功能位 */
|
||||
#define BIT_IRQINV 0x80U // 反转IRQ引脚电平
|
||||
#define BIT_HIALERTIRQ 0x40U // FIFO高水位中断
|
||||
#define BIT_LOALERTIRQ 0x20U // FIFO低水位中断
|
||||
#define BIT_IDLEIRQ 0x10U // 空闲中断 (命令执行完毕)
|
||||
#define BIT_TXIRQ 0x08U // 发送完成中断
|
||||
#define BIT_RXIRQ 0x04U // 接收完成中断
|
||||
#define BIT_ERRIRQ 0x02U // 错误中断
|
||||
#define BIT_RXSOFIRQ 0x01U // 接收到起始帧(SOF)中断
|
||||
|
||||
/** @name IRQ1 Register (0x07) & IRQ1EN Register (0x09) 功能位 */
|
||||
#define BIT_IRQPUSHPULL 0x80U // IRQ引脚推挽/开漏模式选择
|
||||
#define BIT_GLOBALIRQ 0x40U // 全局中断使能
|
||||
#define BIT_LPCDIRQ 0x20U // 低功耗寻卡中断
|
||||
#define BIT_TIMER4IRQ 0x10U // 定时器4中断
|
||||
#define BIT_TIMER3IRQ 0x08U // 定时器3中断
|
||||
#define BIT_TIMER2IRQ 0x04U // 定时器2中断
|
||||
#define BIT_TIMER1IRQ 0x02U // 定时器1中断
|
||||
#define BIT_TIMER0IRQ 0x01U // 定时器0中断
|
||||
|
||||
/** @name Error Register (0x0A) 功能位 */
|
||||
#define BIT_CMDEE_ERR 0x80U // E2PROM命令错误
|
||||
#define BIT_FIFOWRERR 0x40U // FIFO写错误 (FIFO已满时写入)
|
||||
#define BIT_FIFOOVL 0x20U // FIFO溢出错误
|
||||
#define BIT_MINFRAMEERR 0x10U // 最小帧错误
|
||||
#define BIT_NODATAERR 0x08U // 无数据错误 (超时)
|
||||
#define BIT_COLLDET 0x04U // 检测到冲突错误
|
||||
#define BIT_PROTERR 0x02U // 协议错误
|
||||
#define BIT_INTEGERR 0x01U // CRC或奇偶校验错误
|
||||
|
||||
/** @name Status Register (0x0B) 功能位 */
|
||||
#define BIT_CRYPTO1ON 0x20U // MIFARE加密已开启标志位。认证成功后，此位由硬件自动置1。
|
||||
#define MASK_COMMSTATE 0x07U // 通信状态掩码
|
||||
|
||||
/** @name Rx-Bit-Control Register (0x0C) 功能位 */
|
||||
#define BIT_VALUESAFTERCOLL 0x80U // 冲突后接收到的位值
|
||||
#define BIT_NOCOLL 0x08U // 无冲突
|
||||
#define MASK_RXALIGN 0x70U // 接收位对齐掩码
|
||||
#define MASK_RXLASTBITS 0x07U // 最后一字节的有效位数
|
||||
|
||||
/** @name Tx-/Rx-CRC Control Register (0x2C/0x2D) 功能位 */
|
||||
#define BIT_CRCEN 0x01U // CRC使能位
|
||||
|
||||
/** @name Frame Control Register (0x33) 功能位 */
|
||||
#define BIT_TXPARITYEN 0x80U // 发送奇偶校验使能
|
||||
#define BIT_RXPARITYEN 0x40U // 接收奇偶校验使能
|
||||
|
||||
/** @name Rx-Analog Register (0x39) 功能位 */
|
||||
#define MASK_RCV_HPCF 0x0CU // 接收器高通滤波器截止频率
|
||||
#define MASK_RCV_GAIN 0x03U // 接收器增益
|
||||
|
||||
//==================================================================================
|
||||
// 4. 协议标识符定义 (Protocol Identifiers)
|
||||
// 用于 CMD_LOADPROTOCOL 命令，告诉芯片要使用哪种协议进行通信。
|
||||
//==================================================================================
|
||||
// --- 接收协议 ---
|
||||
#define RX_TYPEA_106 0 // ISO14443A, 106 kbit/s
|
||||
#define RX_TYPEA_212 1 // ISO14443A, 212 kbit/s
|
||||
#define RX_TYPEA_424 2 // ISO14443A, 424 kbit/s
|
||||
#define RX_TYPEA_848 3 // ISO14443A, 848 kbit/s
|
||||
#define RX_TYPEB_106 4 // ISO14443B, 106 kbit/s
|
||||
#define RX_TYPEB_212 5 // ISO14443B, 212 kbit/s
|
||||
#define RX_TYPEB_424 6 // ISO14443B, 424 kbit/s
|
||||
#define RX_TYPEB_848 7 // ISO14443B, 848 kbit/s
|
||||
#define RX_TYPEV_26 10 // ISO15693, 26 kbit/s
|
||||
#define RX_TYPEV_53 11 // ISO15693, 53 kbit/s
|
||||
#define RX_FELICA_212 19 // FeliCa, 212 kbit/s
|
||||
#define RX_FELICA_424 20 // FeliCa, 424 kbit/s
|
||||
|
||||
// --- 发送协议 ---
|
||||
#define TX_TYPEA_106 0 // ISO14443A, 106 kbit/s
|
||||
#define TX_TYPEA_212 1 // ISO14443A, 212 kbit/s
|
||||
#define TX_TYPEA_424 2 // ISO14443A, 424 kbit/s
|
||||
#define TX_TYPEA_848 3 // ISO14443A, 848 kbit/s
|
||||
#define TX_TYPEB_106 4 // ISO14443B, 106 kbit/s
|
||||
#define TX_TYPEB_212 5 // ISO14443B, 212 kbit/s
|
||||
#define TX_TYPEB_424 6 // ISO14443B, 424 kbit/s
|
||||
#define TX_TYPEB_848 7 // ISO14443B, 848 kbit/s
|
||||
#define TX_TYPEV_26 10 // ISO15693, 26 kbit/s
|
||||
#define TX_TYPEV_53 11 // ISO15693, 53 kbit/s
|
||||
#define TX_FELICA_212 19 // FeliCa, 212 kbit/s
|
||||
#define TX_FELICA_424 20 // FeliCa, 424 kbit/s
|
||||
|
||||
#endif
|
||||
294
apps/earphone/94_rfid_stc/READER/READER_commented.c
Normal file
294
apps/earphone/94_rfid_stc/READER/READER_commented.c
Normal file
@ -0,0 +1,294 @@
|
||||
/**
|
||||
* @file READER.c
|
||||
* @brief RFID 核心协议驱动实现
|
||||
* @version 1.0
|
||||
* @date 2024-05-20
|
||||
*
|
||||
* @par 文件作用:
|
||||
* 该文件是 READER.h 中声明的所有函数的具体实现。它是驱动库的核心逻辑所在，
|
||||
* 负责将上层应用的简单指令（如“寻卡”）转化为一系列复杂的、精确计时的
|
||||
* 寄存器读写操作。
|
||||
* 主要职责包括：
|
||||
* 1. 实现各协议的初始化函数，配置芯片进入相应的工作模式。
|
||||
* 2. 实现各协议的寻卡、防冲突、选卡等底层通信流程。
|
||||
* 3. 提供与芯片交互的基础原子操作函数 (SetCommand, Clear_FIFO 等)。
|
||||
*/
|
||||
|
||||
#include "function.h"
|
||||
#include "board.h"
|
||||
#include "READER.h"
|
||||
#include "READER_REG.h"
|
||||
#include "xt_main.h"
|
||||
#include <stdio.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 DelayUs( uint32_t xus ) { /* ... */ }
|
||||
void DelayMs( uint32_t xms ) { /* ... */ }
|
||||
|
||||
|
||||
/**
|
||||
* @brief 修改寄存器的特定位 (Read-Modify-Write)
|
||||
* @param reg_address 要修改的寄存器地址
|
||||
* @param mask 要操作的位的掩码 (例如 0x01 表示操作 bit 0)
|
||||
* @param set 1 表示置位 (set)，0 表示清零 (clear)
|
||||
*/
|
||||
void ModifyReg( unsigned char reg_address, unsigned char mask, unsigned char set )
|
||||
{
|
||||
unsigned char reg_data;
|
||||
GetReg( reg_address, ®_data ); // 1. 先读出寄存器当前的值
|
||||
|
||||
if ( set )
|
||||
{
|
||||
reg_data |= mask; // 2. 使用或运算，将指定位置1
|
||||
}
|
||||
else
|
||||
{
|
||||
reg_data &= ~mask; // 3. 使用与和非运算，将指定位清0
|
||||
}
|
||||
|
||||
SetReg( reg_address, reg_data ); // 4. 将修改后的值写回寄存器
|
||||
return;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 向芯片发送一个命令
|
||||
* @param command 要发送的命令码 (定义于 READER_REG.h, 如 CMD_TRANSCEIVE)
|
||||
* @return unsigned char 操作结果
|
||||
*/
|
||||
unsigned char SetCommand(unsigned char command)
|
||||
{
|
||||
return SetReg(REG_COMMAND, CMD_MASK & command);
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 开启或关闭天线载波 (射频场)
|
||||
* @param mode ENABLE 或 DISABLE
|
||||
* @return unsigned char 操作结果
|
||||
*/
|
||||
unsigned char SetCW(unsigned char mode)
|
||||
{
|
||||
if(mode == ENABLE)
|
||||
{
|
||||
// 通过清除 MODEMOFF 位并设置 TXEN 位来开启天线
|
||||
ModifyReg(REG_COMMAND, BIT_MODEMOFF, DISABLE);
|
||||
ModifyReg(REG_TXMODE, BIT0 | BIT1, ENABLE);
|
||||
}
|
||||
else
|
||||
{
|
||||
// 通过设置 MODEMOFF 位来关闭天线
|
||||
ModifyReg(REG_COMMAND, BIT_MODEMOFF, ENABLE);
|
||||
ModifyReg(REG_TXMODE, BIT0 | BIT1, DISABLE);
|
||||
}
|
||||
DelayMs(5);
|
||||
return SUCCESS;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 清空芯片内部的 FIFO 缓冲区
|
||||
*/
|
||||
void Clear_FIFO(void)
|
||||
{
|
||||
unsigned char fifolength;
|
||||
GetReg(REG_FIFOLENGTH, &fifolength); // 读取FIFO中当前字节数
|
||||
if((fifolength) != 0)
|
||||
{
|
||||
// 如果FIFO不为空，通过置位 FIFOFLUSH 来清空
|
||||
ModifyReg(REG_FIFOCONTROL, BIT_FIFOFLUSH, ENABLE);
|
||||
}
|
||||
return ;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 加载通信协议到芯片
|
||||
* @param p_rx 接收协议标识符 (来自 READER_REG.h)
|
||||
* @param p_tx 发送协议标识符 (来自 READER_REG.h)
|
||||
* @return unsigned char 成功或失败
|
||||
*/
|
||||
unsigned char LoadProtocol(unsigned char p_rx, unsigned char p_tx)
|
||||
{
|
||||
unsigned char reg_data = 0;
|
||||
SetCommand(CMD_IDLE);
|
||||
Clear_FIFO();
|
||||
SetReg(REG_FIFODATA, p_rx); // 将接收协议标识符写入FIFO
|
||||
SetReg(REG_FIFODATA, p_tx); // 将发送协议标识符写入FIFO
|
||||
SetCommand(CMD_LOADPROTOCOL); // 发送加载协议命令
|
||||
DelayMs(2);
|
||||
GetReg(REG_COMMAND, ®_data);
|
||||
if(reg_data != CMD_IDLE) // 检查命令是否执行完毕
|
||||
return FAIL;
|
||||
return SUCCESS;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 设置是否启用奇偶校验
|
||||
* @param state ENABLE 或 DISABLE
|
||||
*/
|
||||
void SetParity(unsigned char state)
|
||||
{
|
||||
ModifyReg(REG_FRAMECON, BIT_TXPARITYEN | BIT_RXPARITYEN, state);
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 初始化芯片以进行 ISO14443-A 通信
|
||||
* @return unsigned char 成功
|
||||
*/
|
||||
unsigned char ReaderA_Initial(void)
|
||||
{
|
||||
LoadProtocol(RX_TYPEA_106, TX_TYPEA_106); // 1. 加载Type A协议
|
||||
ModifyReg(REG_TXMODE, BIT2, ENABLE); // 2. 设置为100% ASK调制
|
||||
SetReg(REG_TXAMP, AMPLITUDE_A); // 3. 设置天线发射功率
|
||||
SetReg(REG_RXANA, (HPCF_A << 3) | GAIN_A); // 4. 设置接收器增益和高通滤波器
|
||||
SetParity(ENABLE); // 5. Type A 需要奇偶校验
|
||||
ModifyReg(REG_STATUS, BIT_CRYPTO1ON, 0); // 6. 清除MIFARE加密标志
|
||||
return SUCCESS;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 初始化芯片以进行 ISO14443-B 通信
|
||||
* @return unsigned char 成功
|
||||
*/
|
||||
unsigned char ReaderB_Initial(void)
|
||||
{
|
||||
LoadProtocol(RX_TYPEB_106, TX_TYPEB_106);
|
||||
// ... 配置Type B特定的寄存器 ...
|
||||
return SUCCESS;
|
||||
}
|
||||
|
||||
// ... 其他协议的初始化函数 (ReaderV_Initial, ReaderF_Initial) ...
|
||||
|
||||
/**
|
||||
* @brief (Type A) 发送 REQA 命令
|
||||
* @param picc_a 指向 picc_a_struct 的指针，用于存储响应
|
||||
* @return unsigned char 成功或失败
|
||||
*/
|
||||
unsigned char ReaderA_Request(struct picc_a_struct *picc_a)
|
||||
{
|
||||
unsigned char reg_data;
|
||||
SetCommand(CMD_IDLE);
|
||||
Clear_FIFO();
|
||||
SetReg(REG_FIFODATA, RF_CMD_REQA); // 1. 将 REQA 命令码 (0x26) 写入FIFO
|
||||
ModifyReg(REG_TXCRCCON, BIT_CRCEN, DISABLE); // 2. REQA 命令不带CRC
|
||||
ModifyReg(REG_RXCRCCON, BIT_CRCEN, DISABLE);
|
||||
SetCommand(CMD_TRANSCEIVE); // 3. 发送并等待接收
|
||||
DelayMs(2);
|
||||
GetReg(REG_FIFOLENGTH, ®_data); // 4. 检查接收到的数据长度
|
||||
if(reg_data != 2) // 5. 成功的响应 (ATQA) 应该是2个字节
|
||||
return FAIL;
|
||||
|
||||
GetReg(REG_FIFODATA, &picc_a->ATQA[0]); // 6. 从FIFO读出ATQA
|
||||
GetReg(REG_FIFODATA, &picc_a->ATQA[1]);
|
||||
|
||||
return SUCCESS;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief (Type A) 执行一级防冲突
|
||||
* @param picc_a 指向 picc_a_struct 的指针
|
||||
* @return unsigned char 成功或失败
|
||||
*/
|
||||
unsigned char ReaderA_Anticoll(struct picc_a_struct *picc_a)
|
||||
{
|
||||
unsigned char reg_data;
|
||||
SetCommand(CMD_IDLE);
|
||||
Clear_FIFO();
|
||||
// 1. 准备防冲突命令 (如 0x93 0x20)
|
||||
SetReg(REG_FIFODATA, RF_CMD_ANTICOLL[picc_a->CASCADE_LEVEL]);
|
||||
SetReg(REG_FIFODATA, 0x20);
|
||||
SetCommand(CMD_TRANSCEIVE);
|
||||
DelayMs(2);
|
||||
GetReg(REG_FIFOLENGTH, ®_data);
|
||||
if(reg_data != 5) // 2. 响应应为5字节 (4字节UID + 1字节BCC)
|
||||
return FAIL;
|
||||
|
||||
// 3. 从FIFO读出UID和BCC
|
||||
GetReg(REG_FIFODATA, &picc_a->UID[picc_a->CASCADE_LEVEL*4]);
|
||||
// ...
|
||||
GetReg(REG_FIFODATA, &picc_a->BCC[picc_a->CASCADE_LEVEL]);
|
||||
|
||||
// 4. 校验BCC (UID四个字节的异或值)
|
||||
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 指向 picc_a_struct 的指针
|
||||
* @return unsigned char 成功或失败
|
||||
*/
|
||||
unsigned char ReaderA_Select(struct picc_a_struct *picc_a)
|
||||
{
|
||||
unsigned char reg_data;
|
||||
SetCommand(CMD_IDLE);
|
||||
Clear_FIFO();
|
||||
// 1. 准备选卡命令 (如 0x93 0x70 + UID + BCC)
|
||||
SetReg(REG_FIFODATA, RF_CMD_SELECT[picc_a->CASCADE_LEVEL]);
|
||||
SetReg(REG_FIFODATA, 0x70);
|
||||
SetReg(REG_FIFODATA, picc_a->UID[picc_a->CASCADE_LEVEL*4]);
|
||||
// ...
|
||||
SetReg(REG_FIFODATA, picc_a->BCC[picc_a->CASCADE_LEVEL]);
|
||||
ModifyReg(REG_TXCRCCON, BIT_CRCEN, ENABLE); // 2. 选卡命令带CRC
|
||||
ModifyReg(REG_RXCRCCON, BIT_CRCEN, ENABLE);
|
||||
SetCommand(CMD_TRANSCEIVE);
|
||||
DelayMs(2);
|
||||
GetReg(REG_ERROR, ®_data); // 3. 检查是否有通信错误
|
||||
if((reg_data & 0x0F) != 0)
|
||||
return FAIL;
|
||||
GetReg(REG_FIFOLENGTH, ®_data);
|
||||
if(reg_data != 1) // 4. 响应为1字节的SAK
|
||||
return FAIL;
|
||||
GetReg(REG_FIFODATA, &picc_a->SAK[picc_a->CASCADE_LEVEL]); // 5. 读出SAK
|
||||
return SUCCESS;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief **核心函数**: 激活一张A类卡 (完成完整的寻卡、防冲突、选卡流程)
|
||||
* @param picc_a 指向 picc_a_struct 的指针
|
||||
* @return unsigned char 成功或失败
|
||||
*/
|
||||
unsigned char ReaderA_CardActivate(struct picc_a_struct *picc_a)
|
||||
{
|
||||
unsigned char result, cascade_level;
|
||||
result = ReaderA_Request(picc_a); // 1. 发送请求
|
||||
if (result != SUCCESS)
|
||||
return result;
|
||||
|
||||
// 2. 根据ATQA判断UID长度，确定级联等级
|
||||
if ((picc_a->ATQA[0] & 0xC0) == 0x00)
|
||||
{
|
||||
cascade_level = 1;
|
||||
picc_a->UID_Length = 4;
|
||||
}
|
||||
else if ((picc_a->ATQA[0] & 0xC0) == 0x40)
|
||||
{
|
||||
cascade_level = 2;
|
||||
picc_a->UID_Length = 7; // (修正: 通常是7字节)
|
||||
}
|
||||
// ...
|
||||
|
||||
// 3. 循环执行防冲突和选卡，直到获得完整的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;
|
||||
|
||||
// 如果SAK指示还有下一级UID，则继续循环
|
||||
if (!(picc_a->SAK[picc_a->CASCADE_LEVEL] & 0x04))
|
||||
break;
|
||||
}
|
||||
return result;
|
||||
}
|
||||
|
||||
// ... (其他协议 Type B, V, F 的函数实现) ...
|
||||
147
apps/earphone/94_rfid_stc/READER/READER_commented.h
Normal file
147
apps/earphone/94_rfid_stc/READER/READER_commented.h
Normal file
@ -0,0 +1,147 @@
|
||||
/**
|
||||
* @file READER.h
|
||||
* @brief RFID 核心协议驱动接口
|
||||
* @version 1.0
|
||||
* @date 2024-05-20
|
||||
*
|
||||
* @par 文件作用:
|
||||
* 该文件是核心协议驱动层(READER.c)的头文件，为上层应用(如MIFARE, NTAG)
|
||||
* 提供了标准化的接口来与不同协议的RFID卡片进行底层交互。
|
||||
* 它主要负责：
|
||||
* 1. 定义存储不同类型卡片信息的数据结构 (如 picc_a_struct)。
|
||||
* 2. 声明协议操作函数，如初始化、寻卡、选择、激活等。
|
||||
* 3. 定义通信过程中使用的标准命令码和硬件配置参数。
|
||||
* 上层应用通过调用本文件中声明的函数，可以完成对卡片的寻卡和选卡操作，
|
||||
* 从而为后续的认证和数据读写做准备。
|
||||
*/
|
||||
#ifndef _READER_H
|
||||
#define _READER_H
|
||||
|
||||
#include "function.h"
|
||||
|
||||
//==================================================================================
|
||||
// 1. ISO14443-A 标准命令码定义
|
||||
//==================================================================================
|
||||
static const unsigned char RF_CMD_REQA = 0x26; // REQA (Request Type A) 命令: 用于请求视场内的A类卡响应
|
||||
static const unsigned char RF_CMD_WUPA = 0x52; // WUPA (Wake-Up Type A) 命令: 用于唤醒视场内处于HALT状态的A类卡
|
||||
static const unsigned char RF_CMD_ANTICOLL[3] = {0x93, 0x95, 0x97}; // 防冲突命令码 (级联等级 1, 2, 3)
|
||||
static const unsigned char RF_CMD_SELECT[3] = {0x93, 0x95, 0x97}; // 选卡命令码 (级联等级 1, 2, 3)
|
||||
|
||||
//==================================================================================
|
||||
// 2. MIFARE Classic 命令码定义
|
||||
//==================================================================================
|
||||
static const unsigned char RF_CMD_KEYA = 0x60; // MIFARE Key A 认证命令
|
||||
static const unsigned char RF_CMD_KEYB = 0x61; // MIFARE Key B 认证命令
|
||||
|
||||
//==================================================================================
|
||||
// 3. 卡片信息数据结构定义
|
||||
// 用于存储从不同类型卡片读取到的信息。
|
||||
//==================================================================================
|
||||
|
||||
/** @brief ISO14443-B 卡片信息结构体 */
|
||||
struct picc_b_struct
|
||||
{
|
||||
unsigned char ATQB[12]; // REQB命令的响应 (Answer to Request B)
|
||||
unsigned char PUPI[4]; // 伪唯一PICCs标识符
|
||||
// ... 其他Type B卡片信息
|
||||
};
|
||||
extern struct picc_b_struct PICC_B; // 声明一个全局变量以存储Type B卡信息
|
||||
|
||||
/** @brief ISO14443-A 卡片信息结构体 */
|
||||
struct picc_a_struct
|
||||
{
|
||||
unsigned char ATQA[2]; // REQA命令的响应 (Answer to Request A)
|
||||
unsigned char CASCADE_LEVEL; // 级联等级 (表示UID的长度)
|
||||
unsigned char UID_Length; // UID 长度 (4, 7 或 10字节)
|
||||
unsigned char UID[15]; // 存储卡片的唯一ID (UID)
|
||||
unsigned char BCC[3]; // UID的校验字节
|
||||
unsigned char SAK[3]; // 选卡确认 (Select Acknowledge), 用于判断卡片具体类型
|
||||
};
|
||||
extern struct picc_a_struct PICC_A; // 声明一个全局变量以存储Type A卡信息
|
||||
|
||||
/** @brief ISO15693 (Type V) 卡片信息结构体 */
|
||||
struct picc_v_struct
|
||||
{
|
||||
unsigned char UID[8]; // Type V 卡的UID (8字节)
|
||||
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]; // Type F 卡的UID
|
||||
};
|
||||
extern struct picc_f_struct PICC_F;
|
||||
|
||||
//==================================================================================
|
||||
// 4. 硬件配置参数宏定义
|
||||
// 这些参数用于在初始化时配置RFID芯片的模拟电路，以优化特定协议的通信性能。
|
||||
//==================================================================================
|
||||
// --- 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
|
||||
#define HPCF_B 3
|
||||
#define AMPLITUDE_B 255
|
||||
#define MODULATION_B 100 // 调制深度 (范围 0~255)
|
||||
|
||||
// ... (其他协议的参数定义) ...
|
||||
|
||||
//==================================================================================
|
||||
// 5. MCU引脚定义 (移植时需要重点关注)
|
||||
// 定义了MCU与RFID芯片连接的SPI引脚和控制引脚。
|
||||
//==================================================================================
|
||||
#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 PD_GPIO GPIOA
|
||||
#define PD_PIN LL_GPIO_Pin7 // PA7 (Power Down引脚)
|
||||
|
||||
// ... (GPIO电平控制宏) ...
|
||||
|
||||
//==================================================================================
|
||||
// 6. 函数原型声明
|
||||
// 声明了由 READER.c 实现的函数，供其他模块调用。
|
||||
//==================================================================================
|
||||
|
||||
// --- 基础控制函数 ---
|
||||
extern unsigned char FM176XX_HardReset(void); // 硬复位RFID芯片
|
||||
extern void ModifyReg(unsigned char reg_address, unsigned char mask, unsigned char set); // 修改寄存器的特定位
|
||||
extern void Clear_FIFO(void); // 清空FIFO缓冲区
|
||||
extern unsigned char SetCommand(unsigned char command); // 向芯片发送命令
|
||||
extern unsigned char SetCW(unsigned char mode); // 开启或关闭天线载波
|
||||
|
||||
// --- 协议初始化函数 ---
|
||||
extern unsigned char ReaderA_Initial(void); // 初始化芯片以进行Type A通信
|
||||
extern unsigned char ReaderB_Initial(void); // 初始化芯片以进行Type B通信
|
||||
extern unsigned char ReaderV_Initial(void); // 初始化芯片以进行Type V通信
|
||||
extern unsigned char ReaderF_Initial(void); // 初始化芯片以进行Type F通信
|
||||
|
||||
// --- Type A 卡操作函数 ---
|
||||
extern unsigned char ReaderA_Request(struct picc_a_struct *picc_a); // 发送 REQA 命令
|
||||
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); // **核心函数**: 完成A类卡的寻卡、防冲突和选卡全过程，获取UID
|
||||
|
||||
// --- Type B 卡操作函数 ---
|
||||
extern unsigned char ReaderB_Request(struct picc_b_struct *picc_b); // 发送 REQB 命令
|
||||
extern unsigned char ReaderB_Attrib(struct picc_b_struct *picc_b); // 设置卡片参数
|
||||
// ...
|
||||
|
||||
// --- Type V 卡操作函数 ---
|
||||
extern unsigned char ReaderV_Inventory(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 卡操作函数 ---
|
||||
extern unsigned char ReaderF_Inventory(struct picc_f_struct *picc_f); // 盘点/寻卡
|
||||
|
||||
#endif
|
||||
192
apps/earphone/94_rfid_stc/board.c
Normal file
192
apps/earphone/94_rfid_stc/board.c
Normal file
@ -0,0 +1,192 @@
|
||||
/**
|
||||
******************************************************************************
|
||||
* @file board.c
|
||||
* @author SRG
|
||||
* @version V1.0.0
|
||||
* @date 2020-03-17
|
||||
* @brief
|
||||
******************************************************************************
|
||||
* @attention
|
||||
*
|
||||
* <h2><center>© Copyright (c) 2020 FudanMicroelectronics.
|
||||
* All rights reserved.</center></h2>
|
||||
*
|
||||
******************************************************************************
|
||||
*/
|
||||
#include "board.h"
|
||||
#include "READER.h"
|
||||
#include "fm15l0xx.h"
|
||||
#include "fm15l0xx_ll_cmu.h"
|
||||
#include "fm15l0xx_ll_gpio.h"
|
||||
#include "fm15l0xx_ll_uart.h"
|
||||
#include "fm15l0xx_ll_spi.h"
|
||||
#include "fm15l0xx_ll_svd.h"
|
||||
#include "fm15l0xx_ll_rng.h"
|
||||
|
||||
|
||||
|
||||
// <20>ж<EFBFBD>VDD<44>Ƿ<EFBFBD><C7B7><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֵ
|
||||
int Vdd_is_Low( void )
|
||||
{
|
||||
return (!LL_SVD_IsActiveFlag_SVDR() );
|
||||
}
|
||||
|
||||
void SVD_Init( void )
|
||||
{
|
||||
LL_SVD_EnableSVDDigitalFilter(); // ʹ<><CAB9><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>˲<EFBFBD><CBB2><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>뿪<EFBFBD><EBBFAA>
|
||||
LL_SVD_SetSVDMode( LL_SVD_SVDMOD_INTERVAL_ON ); // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||||
|
||||
LL_SVD_SetSVDInterval( LL_SVD_SVDITVL_62p5ms ); // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>62.5ms
|
||||
|
||||
LL_SVD_DisableV1P0(); // <20>ر<EFBFBD>1V<31><56><EFBFBD><D7BC>ѹ
|
||||
LL_SVD_EnableV0P95(); // ʹ<>û<EFBFBD><EFBFBD><D7BC>ѹ<EFBFBD><D1B9>0.95V
|
||||
LL_SVD_SetSVDLevel( LL_SVD_SVDLVL_LEVEL10 ); // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֵ<EFBFBD>ȼ<EFBFBD>10 - <20><>ֵ3.614V
|
||||
|
||||
LL_SVD_EnableSVD();
|
||||
}
|
||||
|
||||
|
||||
void RNG_Init( void )
|
||||
{
|
||||
LL_CMU_EnablePeriphBusClock_RNG(); // Enable bus clock(RCHF)
|
||||
LL_CMU_EnableClock_RNG(); // Enable RNG work clock
|
||||
LL_CMU_SetClockPrescaler_RNG( LL_CMU_PERPRSC_2 ); // 4MHz: RCHF / 2<><32>ʱ<EFBFBD><CAB1>Ƶ<EFBFBD>ʹ<EFBFBD><CAB9>ᵼ<DFBB><E1B5BC>SFAIL<49><4C><EFBFBD><EFBFBD>
|
||||
|
||||
LL_RNG_EnableNoiseSource();
|
||||
LL_RNG_Enable();
|
||||
}
|
||||
|
||||
|
||||
uint32_t Get_RandomNumber( void )
|
||||
{
|
||||
while ( 1 )
|
||||
{
|
||||
if ( LL_RNG_IsDataReady() || LL_RNG_IsActiveFlag_SFAIL() )
|
||||
{
|
||||
if ( LL_RNG_IsActiveFlag_SFAIL() )
|
||||
{
|
||||
// <20><><EFBFBD>մ<EFBFBD><D5B4><EFBFBD><EFBFBD><EFBFBD>־
|
||||
LL_RNG_ClearFlag_SFAIL();
|
||||
LL_RNG_GetRandomData();
|
||||
}
|
||||
else
|
||||
{
|
||||
break; // <20><>ȡ<EFBFBD>ɹ<EFBFBD>
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
return (LL_RNG_GetRandomData());
|
||||
}
|
||||
|
||||
|
||||
/*********************************************************************************************************
|
||||
** <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>: GPIO_NVIC_Init
|
||||
** <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>: <09><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ŵ<EFBFBD><C5B5>ж<EFBFBD><D0B6><EFBFBD><EFBFBD><EFBFBD>
|
||||
** <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>: <09><>
|
||||
** <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>: <09><>
|
||||
** <20><><EFBFBD><EFBFBD>ֵ: <09><>
|
||||
*********************************************************************************************************/
|
||||
void GPIO_NVIC_Init( void )
|
||||
{
|
||||
LL_CMU_EnableClock_EXTI();
|
||||
|
||||
//<2F><><EFBFBD><EFBFBD>оƬIRQ<52>ж<EFBFBD>
|
||||
LL_EXTI_SetEXTISelection( LL_EXTI_LINE_0, LL_EXTI_SELECTION_3 ); // PA3
|
||||
LL_EXTI_SetEXTIEdgeTrig( LL_EXTI_LINE_0, LL_EXTI_EDS_FALLING ); // Falling
|
||||
|
||||
//FM17<31><37><EFBFBD><EFBFBD>оƬIRQ<52>ж<EFBFBD>
|
||||
LL_EXTI_SetEXTISelection( LL_EXTI_LINE_1, LL_EXTI_SELECTION_3 ); // PA7
|
||||
LL_EXTI_SetEXTIEdgeTrig( LL_EXTI_LINE_1, LL_EXTI_EDS_FALLING ); // Falling
|
||||
|
||||
NVIC_EnableIRQ( GPIO_IRQn );
|
||||
}
|
||||
|
||||
|
||||
void GPIO_Init( void )
|
||||
{
|
||||
LL_CMU_EnablePeriphBusClock_GPIO(); // Bus clock enable
|
||||
|
||||
LL_GPIO_SetPinMode( GPIOA, LL_GPIO_Pin12, LL_GPIO_PINxMODE_DIGITAL ); // PA12 Digital function - UART1 RX
|
||||
LL_GPIO_SetPinDigitalFunc( GPIOA, LL_GPIO_Pin12, LL_GPIO_PinDFS0 );
|
||||
LL_GPIO_SetPinMode( GPIOA, LL_GPIO_Pin13, LL_GPIO_PINxMODE_DIGITAL ); // PA13 Digital function - UART1 TX
|
||||
LL_GPIO_SetPinDigitalFunc( GPIOA, LL_GPIO_Pin13, LL_GPIO_PinDFS0 );
|
||||
|
||||
|
||||
LL_GPIO_SetPinMode( LED_GPIO, LED1_PIN, LL_GPIO_PINxMODE_OUTPUT );
|
||||
LL_GPIO_SetPinMode( LED_GPIO, LED2_PIN, LL_GPIO_PINxMODE_OUTPUT );
|
||||
|
||||
LED1_On;
|
||||
LED2_On;
|
||||
DelayMs(200);
|
||||
LED1_Off;
|
||||
LED2_Off;
|
||||
|
||||
}
|
||||
|
||||
|
||||
void Uart_Init( void )
|
||||
{
|
||||
/* Bus clock enable */
|
||||
LL_CMU_EnablePeriphBusClock_UART1();
|
||||
|
||||
LL_CMU_SelectClock_UART1( LL_CMU_HFPER_CLK_APBCLKx ); // Clock source select
|
||||
LL_CMU_EnableClock_UART1(); // Operation clock enable
|
||||
|
||||
LL_UART_SetBaudRate( UART1, 412 ); // Set baudrate to 115200bps, reg = (uclk / baudrate) - 1
|
||||
LL_UART_SetDataWidth( UART1, LL_UART_DATA_WIDTH_8bit ); // 8 bit
|
||||
LL_UART_SetStopBitsLength( UART1, LL_UART_STOPCFG_1bit ); // 1 stop bit
|
||||
LL_UART_SetParity( UART1, LL_UART_PARITY_NONE ); // Parity none
|
||||
|
||||
LL_UART_EnableTx( UART1 ); // Tx enable
|
||||
LL_UART_EnableRx( UART1 ); // Rx enable
|
||||
}
|
||||
|
||||
|
||||
void uart_send_byte( UART_Typedef *uart, uint16_t c )
|
||||
{
|
||||
while ( !LL_UART_IsActiveFlag_TXBE( uart ) )
|
||||
;
|
||||
LL_UART_TransmitData( uart, (uint32_t) c );
|
||||
}
|
||||
|
||||
|
||||
/* Retarget fputc, then you can use printf. */
|
||||
int fputc( int ch, FILE *f )
|
||||
{
|
||||
uart_send_byte( UART1, ch );
|
||||
return (ch);
|
||||
}
|
||||
|
||||
void SPI_Init( SPI_Typedef *spi )
|
||||
{
|
||||
// Bus clock enable
|
||||
if ( spi == SPI1 )
|
||||
LL_CMU_EnablePeriphBusClock_SPI1();
|
||||
else if ( spi == SPI2 )
|
||||
LL_CMU_EnablePeriphBusClock_SPI2();
|
||||
|
||||
LL_SPI_DisableCtrlSSN( spi ); // Disable Software Control
|
||||
//LL_SPI_SetSSNOutput( spi, 1 ); // Set SSN High
|
||||
|
||||
LL_SPI_Disable( spi ); // Disable SPI
|
||||
LL_SPI_SetWorkMode( spi, LL_SPI_WORKMODE_MASTER ); // Mode select - master
|
||||
LL_SPI_SetBaudrate( spi, LL_SPI_BAUDRATE_DIV_4 ); // Baudrate Select - fPCLK/16
|
||||
LL_SPI_SetFrameFormat(spi,LL_SPI_FRAME_FMT_MSB);
|
||||
LL_SPI_SetClockPolarity(spi,LL_SPI_CPOL_NEGATIVE);
|
||||
LL_SPI_SetCommMode(spi,LL_SPI_COMM_MODE_STANDARD);
|
||||
LL_SPI_SetDataLength(spi,LL_SPI_DATA_LENGTH_8bit);
|
||||
LL_SPI_DisableHalfDuplexDummyCycle(spi);
|
||||
LL_SPI_Enable( spi ); // Enable SPI
|
||||
}
|
||||
|
||||
|
||||
void BSP_Init( void )
|
||||
{
|
||||
GPIO_Init();
|
||||
Uart_Init();
|
||||
//SPI_Init( SPI1);
|
||||
SVD_Init();
|
||||
RNG_Init();
|
||||
}
|
||||
|
||||
70
apps/earphone/94_rfid_stc/bsp.c
Normal file
70
apps/earphone/94_rfid_stc/bsp.c
Normal file
@ -0,0 +1,70 @@
|
||||
#include <stdio.h>
|
||||
#include "bsp.h"
|
||||
|
||||
void gpio_init( void )
|
||||
{
|
||||
LL_CMU_EnablePeriphBusClock_GPIO(); // Bus clock enable
|
||||
|
||||
LL_GPIO_SetPinMode( GPIOA, LL_GPIO_Pin0, LL_GPIO_PINxMODE_DIGITAL ); // PA0 Digital function - UART0 RX
|
||||
LL_GPIO_SetPinDigitalFunc( GPIOA, LL_GPIO_Pin0, LL_GPIO_PinDFS0 );
|
||||
LL_GPIO_SetPinMode( GPIOA, LL_GPIO_Pin1, LL_GPIO_PINxMODE_DIGITAL ); // PA1 Digital function - UART0 TX
|
||||
LL_GPIO_SetPinDigitalFunc( GPIOA, LL_GPIO_Pin1, LL_GPIO_PinDFS0 );
|
||||
}
|
||||
|
||||
|
||||
void uart_init( void )
|
||||
{
|
||||
/* Bus clock enable */
|
||||
LL_CMU_EnablePeriphBusClock_UART0();
|
||||
|
||||
LL_CMU_SelectClock_UART0( LL_CMU_HFPER_CLK_APBCLKx ); // Clock source select
|
||||
LL_CMU_EnableClock_UART0(); // Operation clock enable
|
||||
|
||||
LL_UART_SetBaudRate( UARTx, 412 ); // Set baudrate to 115200bps, reg = (uclk / baudrate) - 1
|
||||
LL_UART_SetDataWidth( UARTx, LL_UART_DATA_WIDTH_8bit ); // 8 bit
|
||||
LL_UART_SetStopBitsLength( UARTx, LL_UART_STOPCFG_1bit ); // 1 stop bit
|
||||
LL_UART_SetParity( UARTx, LL_UART_PARITY_NONE ); // Parity none
|
||||
|
||||
LL_UART_EnableTx( UARTx ); // Tx enable
|
||||
LL_UART_EnableRx( UARTx ); // Rx enable
|
||||
}
|
||||
|
||||
|
||||
void uart_send_byte( UART_Typedef *uart, uint16_t c )
|
||||
{
|
||||
while ( !LL_UART_IsActiveFlag_TXBE( uart ) )
|
||||
;
|
||||
LL_UART_TransmitData( uart, (uint32_t) c );
|
||||
}
|
||||
|
||||
|
||||
/* Retarget fputc, then you can use printf. */
|
||||
int fputc( int ch, FILE *f )
|
||||
{
|
||||
uart_send_byte( UARTx, ch );
|
||||
return (ch);
|
||||
}
|
||||
|
||||
|
||||
void svd_init( void )
|
||||
{
|
||||
LL_SVD_EnableSVDDigitalFilter(); // ʹ<><CAB9><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>˲<EFBFBD><CBB2><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>뿪<EFBFBD><EBBFAA>
|
||||
LL_SVD_SetSVDMode( LL_SVD_SVDMOD_INTERVAL_ON ); // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||||
|
||||
LL_SVD_SetSVDInterval( LL_SVD_SVDITVL_62p5ms ); // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>62.5ms
|
||||
|
||||
LL_SVD_DisableV1P0(); // <20>ر<EFBFBD>1V<31><56><EFBFBD><D7BC>ѹ
|
||||
LL_SVD_EnableV0P95(); // ʹ<>û<EFBFBD><EFBFBD><D7BC>ѹ<EFBFBD><D1B9>0.95V
|
||||
LL_SVD_SetSVDLevel( LL_SVD_SVDLVL_LEVEL10 ); // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֵ<EFBFBD>ȼ<EFBFBD>10 - <20><>ֵ3.614V
|
||||
|
||||
LL_SVD_EnableSVD();
|
||||
}
|
||||
|
||||
|
||||
void bsp_init( void )
|
||||
{
|
||||
gpio_init();
|
||||
uart_init();
|
||||
svd_init();
|
||||
}
|
||||
|
||||
613
apps/earphone/94_rfid_stc/decode_printcipher.c
Normal file
613
apps/earphone/94_rfid_stc/decode_printcipher.c
Normal file
@ -0,0 +1,613 @@
|
||||
#include <stdio.h>
|
||||
#include "print.h"
|
||||
#include "READER.h"
|
||||
#include "random_generator.h"
|
||||
unsigned char ciphertext_to_binary[48] ; //用来存储算法逆运算每轮的中间数据 需要定义成全局变量
|
||||
unsigned char lsfr[48];
|
||||
unsigned char lsfr_length;
|
||||
unsigned char decode_printcipher( unsigned char ciphertext[] , unsigned char long_key[] , unsigned char short_key[] )//解密函数
|
||||
{
|
||||
unsigned char a[6][8];
|
||||
unsigned char i , j ;
|
||||
unsigned char t ;
|
||||
unsigned char k ; //k是用在sbox中的
|
||||
unsigned char num ; //主循环
|
||||
unsigned char long_key_to_binary[48] ;
|
||||
unsigned char temp[48] ;
|
||||
unsigned char short_key_to_binary[32] ;
|
||||
unsigned char rev_counter[6] = {1,0,0,1,0,0};
|
||||
unsigned char temp_rev_counter[6];
|
||||
|
||||
|
||||
//----------------------------------ciphertext-------------------------------------------------//
|
||||
for(i=0 ; i<6 ; ++i) //
|
||||
{ //
|
||||
for(j=0 ; j<8 ; ++j) //
|
||||
{ // 该作用是把密文以二进制保存在数组中ciphertext
|
||||
a[i][8-1-j] = ciphertext[i] % 2 ; // a[7]应该是二进制最低位，a[0]最高位
|
||||
ciphertext[i] /= 2 ; //
|
||||
} //
|
||||
} //
|
||||
|
||||
for(i=0 ; i<6 ; ++i)
|
||||
{
|
||||
for(j=0 ; j<8 ; ++j)
|
||||
{
|
||||
ciphertext_to_binary[i*8+j] = a[i][j] ;
|
||||
}
|
||||
}
|
||||
|
||||
//printf("%x %x %x %x %x %x %x %x %x %x %x %x %x %x %x %x \n",ciphertext_to_binary[0],ciphertext_to_binary[1],ciphertext_to_binary[2],\
|
||||
ciphertext_to_binary[3],ciphertext_to_binary[4],ciphertext_to_binary[5],ciphertext_to_binary[6],ciphertext_to_binary[7],\
|
||||
ciphertext_to_binary[40],ciphertext_to_binary[41],ciphertext_to_binary[42],ciphertext_to_binary[43],ciphertext_to_binary[44],ciphertext_to_binary[45],ciphertext_to_binary[46],ciphertext_to_binary[47]);
|
||||
//-------------------------------------ciphertext---------------------------------------------//
|
||||
|
||||
//-------------------------------------long_key----------------------------------------------//
|
||||
for(i=0 ; i<6 ; ++i) //
|
||||
{ //
|
||||
for(j=0 ; j<8 ; ++j) //
|
||||
{ // 该作用是把密文以二进制保存在数组中long_key
|
||||
a[i][8-1-j] = long_key[i] % 2 ; //
|
||||
long_key[i] /= 2; //
|
||||
} //
|
||||
} //
|
||||
|
||||
for(i=0 ; i<6 ; ++i)
|
||||
{
|
||||
for(j=0 ; j<8 ; ++j)
|
||||
{
|
||||
long_key_to_binary[i*8+j] = a[i][j] ;
|
||||
}
|
||||
}
|
||||
|
||||
//printf("%x %x %x %x %x %x %x %x %x %x %x %x %x %x %x %x \n",long_key_to_binary[0],long_key_to_binary[1],long_key_to_binary[2],\
|
||||
long_key_to_binary[3],long_key_to_binary[4],long_key_to_binary[5],long_key_to_binary[6],long_key_to_binary[7],\
|
||||
long_key_to_binary[40],long_key_to_binary[41],long_key_to_binary[42],long_key_to_binary[43],long_key_to_binary[44],long_key_to_binary[45],long_key_to_binary[46],ciphertext_to_binary[47]);
|
||||
|
||||
|
||||
//-------------------------------- long_key---------------------------------------------------//
|
||||
|
||||
//-------------------------------- short_key-------------------------------------------------//
|
||||
for(i=0 ; i<4 ; ++i) //
|
||||
{ //
|
||||
for(j=0 ; j<8 ; ++j) //
|
||||
{ // 该作用是把密文以二进制保存在数组中short_key
|
||||
a[i][8-1-j] = short_key[i] % 2 ; //
|
||||
short_key[i] /= 2 ; //
|
||||
} //
|
||||
} //
|
||||
|
||||
for(i=0 ; i<4 ; ++i)
|
||||
{
|
||||
for(j=0 ; j<8 ; ++j)
|
||||
{
|
||||
short_key_to_binary[i*8+j] = a[i][j] ;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
//printf("%x %x %x %x %x %x %x %x %x %x %x %x %x %x %x %x \n",short_key_to_binary[0],short_key_to_binary[1],short_key_to_binary[2],\
|
||||
short_key_to_binary[3],short_key_to_binary[4],short_key_to_binary[5],short_key_to_binary[6],short_key_to_binary[7],\
|
||||
short_key_to_binary[24],short_key_to_binary[25],short_key_to_binary[26],short_key_to_binary[27],short_key_to_binary[28],short_key_to_binary[29],short_key_to_binary[30],short_key_to_binary[31]);
|
||||
|
||||
|
||||
|
||||
//-------------------------------- short_key-------------------------------------------------//
|
||||
for(num = 0 ; num < 48 ; num++)
|
||||
{
|
||||
for(k=0 ; k<16 ; k++)
|
||||
{
|
||||
if((short_key_to_binary[k*2]==0) &&(short_key_to_binary[k*2+1]==0) )
|
||||
{
|
||||
if((ciphertext_to_binary[k*3]==0) && (ciphertext_to_binary[k*3+1]==0)&&(ciphertext_to_binary[k*3+2]==0))
|
||||
{
|
||||
temp[k*3] = 0;
|
||||
temp[k*3+1]= 0;
|
||||
temp[k*3+2]= 0;
|
||||
}
|
||||
if((ciphertext_to_binary[k*3]==0) && (ciphertext_to_binary[k*3+1]==0)&&(ciphertext_to_binary[k*3+2]==1))
|
||||
{
|
||||
temp[k*3] = 0;
|
||||
temp[k*3+1]= 0;
|
||||
temp[k*3+2]= 1;
|
||||
}
|
||||
if((ciphertext_to_binary[k*3]==0) && (ciphertext_to_binary[k*3+1]==1)&&(ciphertext_to_binary[k*3+2]==0))
|
||||
{
|
||||
temp[k*3] = 1;
|
||||
temp[k*3+1]= 1;
|
||||
temp[k*3+2]= 1;
|
||||
}
|
||||
if((ciphertext_to_binary[k*3]==0) && (ciphertext_to_binary[k*3+1]==1)&&(ciphertext_to_binary[k*3+2]==1))
|
||||
{
|
||||
temp[k*3] = 0;
|
||||
temp[k*3+1]= 1;
|
||||
temp[k*3+2]= 0;
|
||||
}
|
||||
if((ciphertext_to_binary[k*3]==1) && (ciphertext_to_binary[k*3+1]==0)&&(ciphertext_to_binary[k*3+2]==0))
|
||||
{
|
||||
temp[k*3] = 1;
|
||||
temp[k*3+1]= 0;
|
||||
temp[k*3+2]= 1;
|
||||
}
|
||||
if((ciphertext_to_binary[k*3]==1) && (ciphertext_to_binary[k*3+1]==0)&&(ciphertext_to_binary[k*3+2]==1))
|
||||
{
|
||||
temp[k*3] = 1;
|
||||
temp[k*3+1]= 1;
|
||||
temp[k*3+2]= 0;
|
||||
}
|
||||
if((ciphertext_to_binary[k*3]==1) && (ciphertext_to_binary[k*3+1]==1)&&(ciphertext_to_binary[k*3+2]==0))
|
||||
{
|
||||
temp[k*3] = 0;
|
||||
temp[k*3+1]= 1;
|
||||
temp[k*3+2]= 1;
|
||||
}
|
||||
if((ciphertext_to_binary[k*3]==1) && (ciphertext_to_binary[k*3+1]==1)&&(ciphertext_to_binary[k*3+2]==1))
|
||||
{
|
||||
temp[k*3] = 1;
|
||||
temp[k*3+1]= 0;
|
||||
temp[k*3+2]= 0;
|
||||
}
|
||||
}
|
||||
//////////////////////////////01
|
||||
if((short_key_to_binary[k*2]==0) &&(short_key_to_binary[k*2+1]==1) )
|
||||
{
|
||||
if((ciphertext_to_binary[k*3]==0) && (ciphertext_to_binary[k*3+1]==0)&&(ciphertext_to_binary[k*3+2]==0))
|
||||
{
|
||||
temp[k*3] = 0;
|
||||
temp[k*3+1]= 0;
|
||||
temp[k*3+2]= 0;
|
||||
}
|
||||
if((ciphertext_to_binary[k*3]==0) && (ciphertext_to_binary[k*3+1]==0)&&(ciphertext_to_binary[k*3+2]==1))
|
||||
{
|
||||
temp[k*3] = 0;
|
||||
temp[k*3+1]= 0;
|
||||
temp[k*3+2]= 1;
|
||||
}
|
||||
if((ciphertext_to_binary[k*3]==0) && (ciphertext_to_binary[k*3+1]==1)&&(ciphertext_to_binary[k*3+2]==0))
|
||||
{
|
||||
temp[k*3] = 1;
|
||||
temp[k*3+1]= 1;
|
||||
temp[k*3+2]= 1;
|
||||
}
|
||||
if((ciphertext_to_binary[k*3]==0) && (ciphertext_to_binary[k*3+1]==1)&&(ciphertext_to_binary[k*3+2]==1))
|
||||
{
|
||||
temp[k*3] = 1;
|
||||
temp[k*3+1]= 0;
|
||||
temp[k*3+2]= 0;
|
||||
}
|
||||
if((ciphertext_to_binary[k*3]==1) && (ciphertext_to_binary[k*3+1]==0)&&(ciphertext_to_binary[k*3+2]==0))
|
||||
{
|
||||
temp[k*3] = 0;
|
||||
temp[k*3+1]= 1;
|
||||
temp[k*3+2]= 1;
|
||||
}
|
||||
if((ciphertext_to_binary[k*3]==1) && (ciphertext_to_binary[k*3+1]==0)&&(ciphertext_to_binary[k*3+2]==1))
|
||||
{
|
||||
temp[k*3] = 1;
|
||||
temp[k*3+1]= 1;
|
||||
temp[k*3+2]= 0;
|
||||
}
|
||||
if((ciphertext_to_binary[k*3]==1) && (ciphertext_to_binary[k*3+1]==1)&&(ciphertext_to_binary[k*3+2]==0))
|
||||
{
|
||||
temp[k*3] = 1;
|
||||
temp[k*3+1]= 0;
|
||||
temp[k*3+2]= 1;
|
||||
}
|
||||
if((ciphertext_to_binary[k*3]==1) && (ciphertext_to_binary[k*3+1]==1)&&(ciphertext_to_binary[k*3+2]==1))
|
||||
{
|
||||
temp[k*3] = 0;
|
||||
temp[k*3+1]= 1;
|
||||
temp[k*3+2]= 0;
|
||||
}
|
||||
}
|
||||
//////////////////////////////////////////10
|
||||
if((short_key_to_binary[k*2]==1) &&(short_key_to_binary[k*2+1]==0) )
|
||||
{
|
||||
if((ciphertext_to_binary[k*3]==0) && (ciphertext_to_binary[k*3+1]==0)&&(ciphertext_to_binary[k*3+2]==0))
|
||||
{
|
||||
temp[k*3] = 0;
|
||||
temp[k*3+1]= 0;
|
||||
temp[k*3+2]= 0;
|
||||
}
|
||||
if((ciphertext_to_binary[k*3]==0) && (ciphertext_to_binary[k*3+1]==0)&&(ciphertext_to_binary[k*3+2]==1))
|
||||
{
|
||||
temp[k*3] = 0;
|
||||
temp[k*3+1]= 1;
|
||||
temp[k*3+2]= 0;
|
||||
}
|
||||
if((ciphertext_to_binary[k*3]==0) && (ciphertext_to_binary[k*3+1]==1)&&(ciphertext_to_binary[k*3+2]==0))
|
||||
{
|
||||
temp[k*3] = 1;
|
||||
temp[k*3+1]= 1;
|
||||
temp[k*3+2]= 1;
|
||||
}
|
||||
if((ciphertext_to_binary[k*3]==0) && (ciphertext_to_binary[k*3+1]==1)&&(ciphertext_to_binary[k*3+2]==1))
|
||||
{
|
||||
temp[k*3] = 0;
|
||||
temp[k*3+1]= 0;
|
||||
temp[k*3+2]= 1;
|
||||
}
|
||||
if((ciphertext_to_binary[k*3]==1) && (ciphertext_to_binary[k*3+1]==0)&&(ciphertext_to_binary[k*3+2]==0))
|
||||
{
|
||||
temp[k*3] = 1;
|
||||
temp[k*3+1]= 1;
|
||||
temp[k*3+2]= 0;
|
||||
}
|
||||
if((ciphertext_to_binary[k*3]==1) && (ciphertext_to_binary[k*3+1]==0)&&(ciphertext_to_binary[k*3+2]==1))
|
||||
{
|
||||
temp[k*3] = 1;
|
||||
temp[k*3+1]= 0;
|
||||
temp[k*3+2]= 1;
|
||||
}
|
||||
if((ciphertext_to_binary[k*3]==1) && (ciphertext_to_binary[k*3+1]==1)&&(ciphertext_to_binary[k*3+2]==0))
|
||||
{
|
||||
temp[k*3] = 0;
|
||||
temp[k*3+1]= 1;
|
||||
temp[k*3+2]= 1;
|
||||
}
|
||||
if((ciphertext_to_binary[k*3]==1) && (ciphertext_to_binary[k*3+1]==1)&&(ciphertext_to_binary[k*3+2]==1))
|
||||
{
|
||||
temp[k*3] = 1;
|
||||
temp[k*3+1]= 0;
|
||||
temp[k*3+2]= 0;
|
||||
}
|
||||
}
|
||||
////////////////////////////////////////////11
|
||||
if((short_key_to_binary[k*2]==1) &&(short_key_to_binary[k*2+1]==1) )
|
||||
{
|
||||
if((ciphertext_to_binary[k*3]==0) && (ciphertext_to_binary[k*3+1]==0)&&(ciphertext_to_binary[k*3+2]==0))
|
||||
{
|
||||
temp[k*3] = 0;
|
||||
temp[k*3+1]= 0;
|
||||
temp[k*3+2]= 0;
|
||||
}
|
||||
if((ciphertext_to_binary[k*3]==0) && (ciphertext_to_binary[k*3+1]==0)&&(ciphertext_to_binary[k*3+2]==1))
|
||||
{
|
||||
temp[k*3] = 1;
|
||||
temp[k*3+1]= 0;
|
||||
temp[k*3+2]= 0;
|
||||
}
|
||||
if((ciphertext_to_binary[k*3]==0) && (ciphertext_to_binary[k*3+1]==1)&&(ciphertext_to_binary[k*3+2]==0))
|
||||
{
|
||||
temp[k*3] = 1;
|
||||
temp[k*3+1]= 1;
|
||||
temp[k*3+2]= 1;
|
||||
}
|
||||
if((ciphertext_to_binary[k*3]==0) && (ciphertext_to_binary[k*3+1]==1)&&(ciphertext_to_binary[k*3+2]==1))
|
||||
{
|
||||
temp[k*3] = 0;
|
||||
temp[k*3+1]= 1;
|
||||
temp[k*3+2]= 0;
|
||||
}
|
||||
if((ciphertext_to_binary[k*3]==1) && (ciphertext_to_binary[k*3+1]==0)&&(ciphertext_to_binary[k*3+2]==0))
|
||||
{
|
||||
temp[k*3] = 1;
|
||||
temp[k*3+1]= 0;
|
||||
temp[k*3+2]= 1;
|
||||
}
|
||||
if((ciphertext_to_binary[k*3]==1) && (ciphertext_to_binary[k*3+1]==0)&&(ciphertext_to_binary[k*3+2]==1))
|
||||
{
|
||||
temp[k*3] = 0;
|
||||
temp[k*3+1]= 1;
|
||||
temp[k*3+2]= 1;
|
||||
}
|
||||
if((ciphertext_to_binary[k*3]==1) && (ciphertext_to_binary[k*3+1]==1)&&(ciphertext_to_binary[k*3+2]==0))
|
||||
{
|
||||
temp[k*3] = 1;
|
||||
temp[k*3+1]= 1;
|
||||
temp[k*3+2]= 0;
|
||||
}
|
||||
if((ciphertext_to_binary[k*3]==1) && (ciphertext_to_binary[k*3+1]==1)&&(ciphertext_to_binary[k*3+2]==1))
|
||||
{
|
||||
temp[k*3] = 0;
|
||||
temp[k*3+1]= 0;
|
||||
temp[k*3+2]= 1;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// printf("%x %x %x %x %x %x %x %x %x %x %x %x %x %x %x %x %x %x %x %x %x %x %x %x \n",temp[0],temp[1],temp[2],temp[3],temp[4],temp[5],temp[6],temp[7],temp[8],temp[9],temp[10],temp[11],temp[12],temp[13],temp[14],temp[15],temp[40],temp[41],temp[42],temp[43],temp[44],temp[45],temp[46],temp[47]);
|
||||
|
||||
|
||||
|
||||
|
||||
//下面的函数是对RC的逆运算
|
||||
t = 1 ^ rev_counter[0] ^ rev_counter[5];
|
||||
temp_rev_counter [0] = rev_counter[1] ;
|
||||
temp_rev_counter [1] = rev_counter[2] ;
|
||||
temp_rev_counter [2] = rev_counter[3] ;
|
||||
temp_rev_counter [3] = rev_counter[4] ;
|
||||
temp_rev_counter [4] = rev_counter[5] ;
|
||||
temp_rev_counter [5] = t ;
|
||||
|
||||
for(i=0 ; i<6 ; i++ )
|
||||
{
|
||||
rev_counter [i] = temp_rev_counter[i] ; //rev_counter[5]是最高位
|
||||
}
|
||||
|
||||
temp[47] = rev_counter[0] ^ temp[47] ;
|
||||
temp[46] = rev_counter[1] ^ temp[46] ;
|
||||
temp[45] = rev_counter[2] ^ temp[45] ;
|
||||
temp[44] = rev_counter[3] ^ temp[44] ;
|
||||
temp[43] = rev_counter[4] ^ temp[43] ;
|
||||
temp[42] = rev_counter[5] ^ temp[42] ;
|
||||
|
||||
// printf("%x %x %x %x %x %x %x %x %x %x %x %x %x %x %x %x %x %x %x %x %x %x %x %x \n",temp[0],temp[1],temp[2],temp[3],temp[4],temp[5],temp[6],temp[7],temp[8],temp[9],temp[10],temp[11],temp[12],temp[13],temp[14],temp[15],temp[40],temp[41],temp[42],temp[43],temp[44],temp[45],temp[46],temp[47]);
|
||||
//下面是对linear diffusion的逆运算
|
||||
ciphertext_to_binary[0] = temp[0] ;
|
||||
ciphertext_to_binary[1] = temp[3] ;
|
||||
ciphertext_to_binary[2] = temp[6] ;
|
||||
ciphertext_to_binary[3] = temp[9] ;
|
||||
ciphertext_to_binary[4] = temp[12] ;
|
||||
ciphertext_to_binary[5] = temp[15] ;
|
||||
ciphertext_to_binary[6] = temp[18] ;
|
||||
ciphertext_to_binary[7] = temp[21] ;
|
||||
ciphertext_to_binary[8] = temp[24] ;
|
||||
ciphertext_to_binary[9] = temp[27] ;
|
||||
ciphertext_to_binary[10] = temp[30] ;
|
||||
ciphertext_to_binary[11] = temp[33] ;
|
||||
ciphertext_to_binary[12] = temp[36] ;
|
||||
ciphertext_to_binary[13] = temp[39] ;
|
||||
ciphertext_to_binary[14] = temp[42] ;
|
||||
ciphertext_to_binary[15] = temp[45] ;
|
||||
ciphertext_to_binary[16] = temp[1] ;
|
||||
ciphertext_to_binary[17] = temp[4] ;
|
||||
ciphertext_to_binary[18] = temp[7] ;
|
||||
ciphertext_to_binary[19] = temp[10] ;
|
||||
ciphertext_to_binary[20] = temp[13] ;
|
||||
ciphertext_to_binary[21] = temp[16] ;
|
||||
ciphertext_to_binary[22] = temp[19] ;
|
||||
ciphertext_to_binary[23] = temp[22] ;
|
||||
ciphertext_to_binary[24] = temp[25] ;
|
||||
ciphertext_to_binary[25] = temp[28] ;
|
||||
ciphertext_to_binary[26] = temp[31] ;
|
||||
ciphertext_to_binary[27] = temp[34] ;
|
||||
ciphertext_to_binary[28] = temp[37] ;
|
||||
ciphertext_to_binary[29] = temp[40] ;
|
||||
ciphertext_to_binary[30] = temp[43] ;
|
||||
ciphertext_to_binary[31] = temp[46] ;
|
||||
ciphertext_to_binary[32] = temp[2] ;
|
||||
ciphertext_to_binary[33] = temp[5] ;
|
||||
ciphertext_to_binary[34] = temp[8] ;
|
||||
ciphertext_to_binary[35] = temp[11] ;
|
||||
ciphertext_to_binary[36] = temp[14] ;
|
||||
ciphertext_to_binary[37] = temp[17] ;
|
||||
ciphertext_to_binary[38] = temp[20] ;
|
||||
ciphertext_to_binary[39] = temp[23] ;
|
||||
ciphertext_to_binary[40] = temp[26] ;
|
||||
ciphertext_to_binary[41] = temp[29] ;
|
||||
ciphertext_to_binary[42] = temp[32] ;
|
||||
ciphertext_to_binary[43] = temp[35] ;
|
||||
ciphertext_to_binary[44] = temp[38] ;
|
||||
ciphertext_to_binary[45] = temp[41] ;
|
||||
ciphertext_to_binary[46] = temp[44] ;
|
||||
ciphertext_to_binary[47] = temp[47] ;
|
||||
|
||||
for(i=0 ; i<48 ; i++)
|
||||
{
|
||||
ciphertext_to_binary[i] = ciphertext_to_binary[i] ^ long_key_to_binary[i] ;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
// printf (" %x %x %x %x %x %x %x %x\n %x %x %x %x %x %x %x %x\n %x %x %x %x %x %x %x %x\n %x %x %x %x %x %x %x %x\n %x %x %x %x %x %x %x %x\n %x %x %x %x %x %x %x %x\n",ciphertext_to_binary[0],ciphertext_to_binary[1],ciphertext_to_binary[2],ciphertext_to_binary[3],ciphertext_to_binary[4],ciphertext_to_binary[5],ciphertext_to_binary[6],ciphertext_to_binary[7],\
|
||||
ciphertext_to_binary[8],ciphertext_to_binary[9],ciphertext_to_binary[10],ciphertext_to_binary[11],ciphertext_to_binary[12],ciphertext_to_binary[13],ciphertext_to_binary[14],ciphertext_to_binary[15],\
|
||||
ciphertext_to_binary[16],ciphertext_to_binary[17],ciphertext_to_binary[18],ciphertext_to_binary[19],ciphertext_to_binary[20],ciphertext_to_binary[21],ciphertext_to_binary[22],ciphertext_to_binary[23],\
|
||||
ciphertext_to_binary[24],ciphertext_to_binary[25],ciphertext_to_binary[26],ciphertext_to_binary[27],ciphertext_to_binary[28],ciphertext_to_binary[29],ciphertext_to_binary[30],ciphertext_to_binary[31],\
|
||||
ciphertext_to_binary[32],ciphertext_to_binary[33],ciphertext_to_binary[34],ciphertext_to_binary[35],ciphertext_to_binary[36],ciphertext_to_binary[37],ciphertext_to_binary[38],ciphertext_to_binary[39],\
|
||||
ciphertext_to_binary[40],ciphertext_to_binary[41],ciphertext_to_binary[42],ciphertext_to_binary[43],ciphertext_to_binary[44],ciphertext_to_binary[45],ciphertext_to_binary[46],ciphertext_to_binary[47] );
|
||||
return 0 ;
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
void Print_AUTH(unsigned char *key,unsigned char *TOKEN,unsigned char *rece_buff)//print 认证
|
||||
{
|
||||
unsigned char i,j;
|
||||
unsigned char long_key[6] = {0xff,0xff,0xff,0xff,0xff,0xff}; //长key
|
||||
unsigned char short_key[4] = {0xff,0xff,0xff,0xff}; //短key，注意短key的最后一个字节是整个key的第一个字节
|
||||
unsigned char PWD_key[10] = {0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00};//tag的秘钥占10个字节
|
||||
for(i = 0;i<10;i++)
|
||||
{
|
||||
PWD_key[i] = key[9-i];//需要把PWD 倒顺序一下,做下反向,需要特别注意，之前的密码需要倒序一下
|
||||
}
|
||||
memcpy(&long_key[0],PWD_key,6);
|
||||
memcpy(&short_key[0],&PWD_key[6],4);
|
||||
decode_printcipher(TOKEN, &long_key[0] , &short_key[0]);
|
||||
for(i=0 ; i<6 ; i++)
|
||||
{
|
||||
for(j=0 ; j<8 ; j++)
|
||||
{
|
||||
rece_buff[i] = (rece_buff[i]<<1)+ciphertext_to_binary[i*8+j] ; //把最终的结果赋值给明文中
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
unsigned char lsfr_fm458(void) //length 这里是byte
|
||||
{
|
||||
char j ;
|
||||
unsigned char temp;
|
||||
unsigned char temp1;
|
||||
|
||||
temp1 = lsfr[47];
|
||||
temp = lsfr[47] ^ lsfr[27] ^ lsfr[26] ^ lsfr[0];
|
||||
for (j = 46; j >= 0; j--)
|
||||
{
|
||||
lsfr[j + 1] = lsfr[j];
|
||||
}
|
||||
lsfr[0] = temp;
|
||||
|
||||
return temp1;
|
||||
}
|
||||
|
||||
|
||||
void lsfr_init(unsigned char *S022_Token2)
|
||||
{
|
||||
unsigned char z,k;
|
||||
unsigned char load_key[6];
|
||||
//备份初始密钥流
|
||||
memcpy(load_key, S022_Token2, 6);
|
||||
//把R2和R3的结果加载进移位寄存器
|
||||
for (z = 0; z < 6; z++)
|
||||
{
|
||||
for (k = 0; k <= 7; k++)
|
||||
{
|
||||
if ((unsigned char)((load_key[z] << k) & 0x80) == 0)
|
||||
lsfr[47 - (k + z * 8)] = 0;
|
||||
else
|
||||
lsfr[47 - (k + z * 8)] = 1;
|
||||
}
|
||||
}
|
||||
//lsfr_length = 48;
|
||||
//printf("lsfr = %x%x%x%x%x%x%x%x\r\n",lsfr[0],lsfr[1],lsfr[2],lsfr[3],lsfr[4],lsfr[5],lsfr[6],lsfr[7]);
|
||||
}
|
||||
|
||||
|
||||
|
||||
//data_len 最多16byte
|
||||
//unsigned char PRINT_Calc_sendData(unsigned char *data_buf, unsigned char data_len, BOOL half_byte)
|
||||
unsigned char PRINT_Calc_sendData(unsigned char *data_buf, unsigned char data_len)
|
||||
{
|
||||
unsigned char i,y;
|
||||
unsigned char round;
|
||||
unsigned char temp[8];
|
||||
unsigned char temp1[8];
|
||||
unsigned char encry[64];
|
||||
unsigned char senddata[16];
|
||||
round = data_len * 8;
|
||||
for (y = 0; y < round; y++)
|
||||
{
|
||||
encry[y] = lsfr_fm458();
|
||||
}
|
||||
|
||||
|
||||
for (y = 0; y < data_len; y++)
|
||||
{
|
||||
for(i=0; i<8; i++)
|
||||
{
|
||||
temp[i] = (unsigned char)((data_buf[y] >> i) & 0x01);
|
||||
}
|
||||
|
||||
//发送的byte按位异或 加密
|
||||
for(i=0 ; i<8 ;i++)
|
||||
{
|
||||
temp1[i] = temp[i] ^ encry[i + y * 8]; //异或就是和密码流进行加密
|
||||
}
|
||||
|
||||
senddata[y] = (unsigned char)(temp1[0] * 1 + temp1[1] * 2 + temp1[2] * 4 + temp1[3] * 8 + temp1[4] * 16 + temp1[5] * 32 + temp1[6] * 64 + temp1[7] * 128);
|
||||
}
|
||||
|
||||
memcpy(data_buf, senddata, data_len);
|
||||
|
||||
// authstat = CAL_COMMAND_FLOW;
|
||||
|
||||
return SUCCESS;
|
||||
}
|
||||
|
||||
|
||||
|
||||
//unsigned char PRINT_Calc_recvData(unsigned char *data_buf, unsigned char data_len, BOOL half_byte)
|
||||
unsigned char PRINT_Calc_recvData(unsigned char *data_buf, unsigned char data_len)
|
||||
{
|
||||
unsigned char encry[128];
|
||||
unsigned char i,y;
|
||||
unsigned char temp[8];
|
||||
unsigned char temp1[8];
|
||||
unsigned char recvdata[16];
|
||||
unsigned char round;
|
||||
round = (data_len) * 8;
|
||||
|
||||
if (data_len == 0x01) //该添加为了类似写指令回发ACK等4bit的加密流方法。
|
||||
{
|
||||
round = round / 2;
|
||||
}
|
||||
|
||||
for (y = 0; y < round; y++)
|
||||
{
|
||||
encry[y] = lsfr_fm458();
|
||||
}
|
||||
|
||||
//已经有了加密数据，就要想办法进行异或操作
|
||||
for (y = 0; y < data_len; y++)
|
||||
{
|
||||
for (i = 0; i < 8; i++)
|
||||
{
|
||||
temp[i] = (unsigned char)((data_buf[y] >> i) & 0x01);
|
||||
}
|
||||
|
||||
//发送的byte按位异或 加密
|
||||
for (i = 0; i < 8; i++)
|
||||
{
|
||||
temp1[i] = temp[i] ^ encry[i + y * 8]; //异或就是和密码流进行加密
|
||||
}
|
||||
|
||||
recvdata[y] = (unsigned char)(temp1[0] * 1 + temp1[1] * 2 + temp1[2] * 4 + temp1[3] * 8 + temp1[4] * 16 + temp1[5] * 32 + temp1[6] * 64 + temp1[7] * 128);
|
||||
}
|
||||
|
||||
memcpy(data_buf, recvdata, data_len);
|
||||
|
||||
return SUCCESS;
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
457
apps/earphone/94_rfid_stc/function.c
Normal file
457
apps/earphone/94_rfid_stc/function.c
Normal file
@ -0,0 +1,457 @@
|
||||
/**
|
||||
************************************* Copyright ******************************
|
||||
* (C) Copyright 2019,TuYW,FMSH,China.
|
||||
* All Rights Reserved
|
||||
* By(Shanghai Fudan MicroeleCardTyperonics Group Company Limited)
|
||||
* https://www.fmsh.com
|
||||
*
|
||||
* FileName : function.c
|
||||
* Version : v1.0
|
||||
* Author : TuYW
|
||||
* Date : 2019-03-19
|
||||
* Description:
|
||||
*******************************************************************************/
|
||||
|
||||
#include "function.h"
|
||||
//#include "flash.h"
|
||||
|
||||
#include "intrins.h"
|
||||
|
||||
#include <stdio.h>
|
||||
/**
|
||||
* @brief Convert Hex 32Bits value into char
|
||||
* @param value: value to convert
|
||||
* @param pbuf: pointer to the buffer
|
||||
* @param len: buffer length
|
||||
* @retval None
|
||||
*/
|
||||
void IntToUnicode( uint32_t value, uint8_t *pbuf, uint8_t len )
|
||||
{
|
||||
uint8_t idx = 0;
|
||||
|
||||
for ( idx = 0; idx < len; idx++ )
|
||||
{
|
||||
if ( ( (value >> 28) ) < 0xA )
|
||||
{
|
||||
pbuf[2 * idx] = (value >> 28) + '0';
|
||||
}
|
||||
else
|
||||
{
|
||||
pbuf[2 * idx] = (value >> 28) + 'A' - 10;
|
||||
}
|
||||
|
||||
value = value << 4;
|
||||
|
||||
pbuf[2 * idx + 1] = 0;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
//תַ
|
||||
u8 uitoa( u16 n, u8* s )
|
||||
{
|
||||
u16 i = 0, j;
|
||||
u16 len = 0;
|
||||
u8 tmp[7] = { 0 };
|
||||
|
||||
do
|
||||
{
|
||||
tmp[i++] = n % 10 + '0'; //ȡһ
|
||||
}
|
||||
while ( (n /= 10) > 0 ); //ɾ
|
||||
|
||||
len = i;
|
||||
|
||||
for ( j = 0; j < len; j++, i-- ) //
|
||||
{
|
||||
s[j] = tmp[i - 1];
|
||||
}
|
||||
|
||||
return (len);
|
||||
}
|
||||
|
||||
|
||||
//ƽ
|
||||
u32 power( u8 n )
|
||||
{
|
||||
u8 i;
|
||||
u32 t;
|
||||
|
||||
t = 1;
|
||||
for ( i = 0; i < n; i++ )
|
||||
t *= 2;
|
||||
|
||||
return (t);
|
||||
}
|
||||
|
||||
|
||||
//m^n
|
||||
u32 calpow( u8 m, u8 n )
|
||||
{
|
||||
u32 result = 1;
|
||||
while ( n-- )
|
||||
result *= m;
|
||||
return (result);
|
||||
}
|
||||
|
||||
|
||||
//CRC
|
||||
u16 UpdateCrc( u8 ch, u16 *lpwCrc )
|
||||
{
|
||||
ch = (ch ^ (u8) ( (*lpwCrc) & 0x00FF) );
|
||||
ch = (ch ^ (ch << 4) );
|
||||
*lpwCrc = (*lpwCrc >> 8) ^ ( (u16) ch << 8) ^ ( (u16) ch << 3) ^ ( (u16) ch >> 4);
|
||||
return (*lpwCrc);
|
||||
}
|
||||
|
||||
|
||||
u16 ComputeCrc( const u16 InitCRC, const u8 *Data, const u32 Length )
|
||||
{
|
||||
u8 chBlock;
|
||||
u32 InLength = Length;
|
||||
u16 wCrc = InitCRC;
|
||||
do
|
||||
{
|
||||
chBlock = *Data++;
|
||||
UpdateCrc( chBlock, &wCrc );
|
||||
}
|
||||
while ( --InLength );
|
||||
return (~wCrc);;
|
||||
}
|
||||
|
||||
|
||||
//u16 CalculateIapCrc(void)
|
||||
//{
|
||||
// return ComputeCrc(0xffff,(u8 *)PROGRAM_IAP_BASE,PROGRAM_IAP_LENGTH);
|
||||
//}
|
||||
|
||||
//u16 CalculateAppCrc(void)
|
||||
//{
|
||||
// return ComputeCrc(0xffff,(u8 *)PROGRAM_APP_BASE,PROGRAM_APP_LENGTH);
|
||||
//}
|
||||
|
||||
|
||||
/*********************************************************************************************************
|
||||
** : CalcBCC
|
||||
** : bccУ
|
||||
** :
|
||||
** :
|
||||
** ֵ:
|
||||
*********************************************************************************************************/
|
||||
u8 CalcBCC( u8 *ibuf, u16 ilen )
|
||||
{
|
||||
u8 BCC = 0x00;
|
||||
u16 i = 0;
|
||||
for ( i = 0; i < ilen; i++ )
|
||||
{
|
||||
BCC = BCC ^ ibuf[i];
|
||||
}
|
||||
|
||||
return (BCC);
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
// C prototype : void StrToHex(BYTE *pbDest, BYTE *pbSrc, int nLen)
|
||||
// parameter(s): [OUT] pbDest -
|
||||
// [IN] pbSrc - ַ
|
||||
// [IN] nLen - 16ֽ(ַij/2)
|
||||
// return value:
|
||||
// remarks : ַתΪ16
|
||||
*/
|
||||
void StrToHex( BYTE *pbDest, BYTE *pbSrc, int nLen )
|
||||
{
|
||||
char h1, h2;
|
||||
BYTE s1, s2;
|
||||
int i;
|
||||
|
||||
for ( i = 0; i < nLen; i++ )
|
||||
{
|
||||
h1 = pbSrc[2 * i];
|
||||
h2 = pbSrc[2 * i + 1];
|
||||
|
||||
s1 = toupper( h1 ) - 0x30;
|
||||
if ( s1 > 9 )
|
||||
s1 -= 7;
|
||||
|
||||
s2 = toupper( h2 ) - 0x30;
|
||||
if ( s2 > 9 )
|
||||
s2 -= 7;
|
||||
|
||||
pbDest[i] = s1 * 16 + s2;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
// C prototype : void HexToStr(BYTE *pbDest, BYTE *pbSrc, int nLen)
|
||||
// parameter(s): [OUT] pbDest - Ŀַ
|
||||
// [IN] pbSrc - 16ʼַ
|
||||
// [IN] nLen - 16ֽ
|
||||
// return value:
|
||||
// remarks : 16תΪַ
|
||||
*/
|
||||
void HexToStr( BYTE *pbDest, BYTE *pbSrc, int nLen )
|
||||
{
|
||||
char ddl, ddh;
|
||||
int i;
|
||||
|
||||
for ( i = 0; i < nLen; i++ )
|
||||
{
|
||||
ddh = 48 + pbSrc[i] / 16;
|
||||
ddl = 48 + pbSrc[i] % 16;
|
||||
if ( ddh > 57 )
|
||||
ddh = ddh + 7;
|
||||
if ( ddl > 57 )
|
||||
ddl = ddl + 7;
|
||||
pbDest[i * 2] = ddh;
|
||||
pbDest[i * 2 + 1] = ddl;
|
||||
}
|
||||
|
||||
pbDest[nLen * 2] = '\0';
|
||||
}
|
||||
|
||||
|
||||
U32 le32p_to_cpu( U8*p )
|
||||
{
|
||||
return ( (U32) ( (p[3] << 24) | (p[2] << 16) | (p[1] << 8) | p[0]) );
|
||||
}
|
||||
|
||||
|
||||
U32 be32p_to_cpu( const U8*p )
|
||||
{
|
||||
return ( (U32) ( (p[0] << 24) | (p[1] << 16) | (p[2] << 8) | p[3]) );
|
||||
}
|
||||
|
||||
|
||||
void cpu_to_le32p( U8*p, U32 n )
|
||||
{
|
||||
p[3] = (U8) (n >> 24);
|
||||
p[2] = (U8) (n >> 16);
|
||||
p[1] = (U8) (n >> 8);
|
||||
p[0] = (U8) (n);
|
||||
}
|
||||
|
||||
|
||||
void cpu_to_be32p( U8*p, U32 n )
|
||||
{
|
||||
p[0] = (U8) (n >> 24);
|
||||
p[1] = (U8) (n >> 16);
|
||||
p[2] = (U8) (n >> 8);
|
||||
p[3] = (U8) (n);
|
||||
}
|
||||
|
||||
|
||||
U32 le16p_to_cpu( const U8*p )
|
||||
{
|
||||
return ( (U32) ( (p[1] << 8) | p[0]) );
|
||||
}
|
||||
|
||||
|
||||
U32 be16p_to_cpu( U8*p )
|
||||
{
|
||||
return ( (U32) ( (p[0] << 8) | p[1]) );
|
||||
}
|
||||
|
||||
|
||||
void cpu_to_le16p( U8*p, U32 n )
|
||||
{
|
||||
p[1] = (U8) (n >> 8);
|
||||
p[0] = (U8) (n);
|
||||
}
|
||||
|
||||
|
||||
void cpu_to_be16p( U8*p, U32 n )
|
||||
{
|
||||
p[0] = (U8) (n >> 8);
|
||||
p[1] = (U8) (n);
|
||||
}
|
||||
|
||||
|
||||
/*ַs转Ӧ*/
|
||||
//int atoi(char s[])
|
||||
//{
|
||||
// int i;
|
||||
// int n = 0;
|
||||
// for (i = 0; s[i] >= '0' && s[i] <= '9'; ++i)
|
||||
// {
|
||||
// n = 10 * n + (s[i] - '0');
|
||||
// }
|
||||
// return n;
|
||||
//}
|
||||
|
||||
int atoi_b( u8 *s, u32 sl )
|
||||
{
|
||||
int i;
|
||||
int n = 0;
|
||||
for ( i = 0; i < sl; i++ )
|
||||
{
|
||||
n = 10 * n + (s[i] - '0');
|
||||
}
|
||||
return (n);
|
||||
}
|
||||
|
||||
//xtell tolower改成my_tolower
|
||||
/*дĸ转Сдĸ*/
|
||||
int my_tolower( int c )
|
||||
{
|
||||
if ( c >= 'A' && c <= 'Z' )
|
||||
{
|
||||
return (c + 'a' - 'A');
|
||||
}
|
||||
else
|
||||
{
|
||||
return (c);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
//ʮƵַ转
|
||||
int htoi( char s[] )
|
||||
{
|
||||
int i;
|
||||
int n = 0;
|
||||
if ( s[0] == '0' && (s[1] == 'x' || s[1] == 'X') )
|
||||
{
|
||||
i = 2;
|
||||
}
|
||||
else
|
||||
{
|
||||
i = 0;
|
||||
}
|
||||
for (; (s[i] >= '0' && s[i] <= '9') || (s[i] >= 'a' && s[i] <= 'z') || (s[i] >= 'A' && s[i] <= 'Z'); ++i )
|
||||
{
|
||||
if ( my_tolower( s[i] ) > '9' )
|
||||
{
|
||||
n = 16 * n + (10 + my_tolower( s[i] ) - 'a');
|
||||
}
|
||||
else
|
||||
{
|
||||
n = 16 * n + (my_tolower( s[i] ) - '0');
|
||||
}
|
||||
}
|
||||
return (n);
|
||||
}
|
||||
|
||||
|
||||
// itoa (ʾ integer to alphanumeric)ǰ转ַһ
|
||||
char* itoa( long num, char* str, int radix )
|
||||
{
|
||||
char temp; // 移动到函数开头
|
||||
char index[] = "0123456789ABCDEFGHIJKLMNOPQRSTUVWXYZ"; //
|
||||
unsigned long unum; //Ҫ转ľֵ,转Ǹ
|
||||
int i = 0, j, k; //iָʾַӦλ转֮iʵַijȣ转˳ģkָʾ˳Ŀʼλ;jָʾ˳ʱĽ
|
||||
|
||||
//ȡҪ转ľֵ
|
||||
if ( radix == 10 && num < 0 ) //Ҫ转ʮǸ
|
||||
{
|
||||
unum = (unsigned long) -num; //numľֵunum
|
||||
str[i++] = '-'; //ַǰΪ'-'ţ1
|
||||
}
|
||||
else
|
||||
unum = (unsigned long) num; //numΪֱӸֵunum
|
||||
|
||||
//转֣ע转
|
||||
do
|
||||
{
|
||||
str[i++] = index[unum % (unsigned long) radix]; //ȡunumһλΪstrӦλָʾ1
|
||||
unum /= radix; //unumȥһλ
|
||||
}
|
||||
while ( unum ); //ֱunumΪ0˳ѭ
|
||||
|
||||
str[i] = '\0'; //ַ'\0'ַcַ'\0'
|
||||
|
||||
//˳
|
||||
if ( str[0] == '-' )
|
||||
k = 1; //ǸŲõӷź濪ʼ
|
||||
else
|
||||
k = 0; //ǸȫҪ
|
||||
|
||||
for ( j = k; j <= (i - 1) / 2; j++ ) //ͷβһһԳƽiʵַijȣֵȳ1
|
||||
{
|
||||
temp = str[j]; //ͷֵʱ
|
||||
str[j] = str[i - 1 + k - j]; //βֵͷ
|
||||
str[i - 1 + k - j] = temp; //ʱֵ(ʵ֮ǰͷֵ)β
|
||||
}
|
||||
|
||||
return (str); //转ַ
|
||||
}
|
||||
|
||||
|
||||
static const char *ALPHA_BASE = "ABCDEFGHIJKLMNOPQRSTUVWXYZabcdefghijklmnopqrstuvwxyz0123456789+/";
|
||||
|
||||
//base64뺯
|
||||
int base64_encode( const char *buf, const long size, char *base64Char )
|
||||
{
|
||||
int int63 = 0x3F; // 00111111 // 移动到函数开头
|
||||
int int255 = 0xFF; // 11111111 // 移动到函数开头
|
||||
int a = 0;
|
||||
int i = 0;
|
||||
int base64Len = 0;
|
||||
|
||||
while ( i < size )
|
||||
{
|
||||
char b0 = buf[i++];
|
||||
char b1 = (i < size) ? buf[i++] : 0;
|
||||
char b2 = (i < size) ? buf[i++] : 0;
|
||||
|
||||
base64Char[a++] = ALPHA_BASE[(b0 >> 2) & int63];
|
||||
base64Char[a++] = ALPHA_BASE[( (b0 << 4) | ( (b1 & int255) >> 4) ) & int63];
|
||||
base64Char[a++] = ALPHA_BASE[( (b1 << 2) | ( (b2 & int255) >> 6) ) & int63];
|
||||
base64Char[a++] = ALPHA_BASE[b2 & int63];
|
||||
}
|
||||
|
||||
base64Len = a;
|
||||
|
||||
switch ( size % 3 )
|
||||
{
|
||||
case 1:
|
||||
base64Char[--a] = '=';
|
||||
case 2:
|
||||
base64Char[--a] = '=';
|
||||
}
|
||||
|
||||
return (base64Len);
|
||||
}
|
||||
|
||||
|
||||
//base64뺯
|
||||
char *base64_decode( const char *base64Char, const long base64CharSize, char *originChar, long originCharSize )
|
||||
{
|
||||
int toInt[128] = { -1 };
|
||||
int int255 = 0xFF; // 移动到函数开头
|
||||
int index = 0; // 移动到函数开头
|
||||
int c2, c3; // 移动到函数开头
|
||||
int i = 0;
|
||||
int j = 0;
|
||||
for ( i = 0; i < 64; i++ )
|
||||
{
|
||||
toInt[ALPHA_BASE[i]] = i;
|
||||
}
|
||||
for ( j = 0; j < base64CharSize; j += 4 )
|
||||
{
|
||||
int c0 = toInt[base64Char[j]];
|
||||
int c1 = toInt[base64Char[j + 1]];
|
||||
originChar[index++] = ( ( (c0 << 2) | (c1 >> 4) ) & int255);
|
||||
if ( index >= originCharSize )
|
||||
{
|
||||
return (originChar);
|
||||
}
|
||||
c2 = toInt[base64Char[j + 2]];
|
||||
originChar[index++] = ( ( (c1 << 4) | (c2 >> 2) ) & int255);
|
||||
if ( index >= originCharSize )
|
||||
{
|
||||
return (originChar);
|
||||
}
|
||||
c3 = toInt[base64Char[j + 3]];
|
||||
originChar[index++] = ( ( (c2 << 6) | c3) & int255);
|
||||
}
|
||||
return (originChar);
|
||||
}
|
||||
|
||||
|
||||
//建
|
||||
void ClrLongBuf( u8* buf, u32 n )
|
||||
{
|
||||
memset( buf, 0x00, n );
|
||||
}
|
||||
21
apps/earphone/94_rfid_stc/inc/MIFARE.h
Normal file
21
apps/earphone/94_rfid_stc/inc/MIFARE.h
Normal file
@ -0,0 +1,21 @@
|
||||
#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
|
||||
73
apps/earphone/94_rfid_stc/inc/MIFARE_commented.h
Normal file
73
apps/earphone/94_rfid_stc/inc/MIFARE_commented.h
Normal file
@ -0,0 +1,73 @@
|
||||
/**
|
||||
* @file MIFARE.h
|
||||
* @brief MIFARE Classic 卡应用层驱动接口
|
||||
* @version 1.0
|
||||
* @date 2024-05-20
|
||||
*
|
||||
* @par 文件作用:
|
||||
* 该文件为操作 MIFARE Classic 卡片提供了高层接口。它建立在 READER.c
|
||||
* 提供的 Type A 协议基础之上，封装了 MIFARE 卡特有的操作，如密码认证、
|
||||
* 块读写和值块操作。
|
||||
* 主应用逻辑通过包含此文件并调用其声明的函数，来完成对 MIFARE 卡的
|
||||
* 具体业务操作。
|
||||
*/
|
||||
#ifndef _MIFARE_H_
|
||||
#define _MIFARE_H_
|
||||
|
||||
//==================================================================================
|
||||
// 1. 宏定义
|
||||
//==================================================================================
|
||||
#define KEY_A_M1 0 // 定义认证模式为 Key A
|
||||
#define KEY_B_M1 1 // 定义认证模式为 Key B
|
||||
|
||||
//==================================================================================
|
||||
// 2. 全局变量声明
|
||||
// 这些变量用于在不同函数间传递操作上下文。
|
||||
//==================================================================================
|
||||
extern unsigned char SECTOR; // 当前要操作的扇区号
|
||||
extern unsigned char BLOCK; // 当前要操作的扇区内块号 (0-3)
|
||||
extern unsigned char BLOCK_NUM; // 当前要操作的块的绝对地址 (0-63 for 1K)
|
||||
extern unsigned char BLOCK_DATA[16]; // 用于暂存读/写块数据的16字节缓冲区
|
||||
extern unsigned char KEY_A[16][6]; // 存储16个扇区的 Key A 密钥
|
||||
extern unsigned char KEY_B[16][6]; // 存储16个扇区的 Key B 密钥
|
||||
|
||||
//==================================================================================
|
||||
// 3. 函数原型声明
|
||||
//==================================================================================
|
||||
|
||||
/** @brief MIFARE 卡事件处理流程 (示例函数) */
|
||||
extern unsigned char MIFARE_CARD_EVENT(void);
|
||||
|
||||
/** @brief 清除芯片的 MIFARE 加密状态 */
|
||||
extern void Mifare_Clear_Crypto(void);
|
||||
|
||||
/** @brief 加载6字节密钥到读卡器芯片内部的密钥缓冲器 */
|
||||
unsigned char Mifare_LoadKey(unsigned char *mifare_key);
|
||||
|
||||
/** @brief **核心函数**: 对指定扇区进行密码认证 */
|
||||
extern unsigned char Mifare_Auth(unsigned char key_mode, unsigned char sector, unsigned char *mifare_key, unsigned char *card_uid);
|
||||
|
||||
/** @brief 读取一个16字节的数据块 */
|
||||
extern unsigned char Mifare_Blockread(unsigned char block, unsigned char *data_buff);
|
||||
|
||||
/** @brief 写入一个16字节的数据块 */
|
||||
extern unsigned char Mifare_Blockwrite(unsigned char block, unsigned char *data_buff);
|
||||
|
||||
// --- 值块操作函数 (用于电子钱包等应用) ---
|
||||
|
||||
/** @brief 将一个块格式化为值块 */
|
||||
extern unsigned char Mifare_Blockset(unsigned char block, unsigned char *data_buff);
|
||||
|
||||
/** @brief 对值块进行加值操作 */
|
||||
extern unsigned char Mifare_Blockinc(unsigned char block, unsigned char *data_buff);
|
||||
|
||||
/** @brief 对值块进行减值操作 */
|
||||
extern unsigned char Mifare_Blockdec(unsigned char block, unsigned char *data_buff);
|
||||
|
||||
/** @brief 将值块缓冲区的数据传送到数据块 */
|
||||
extern unsigned char Mifare_Transfer(unsigned char block);
|
||||
|
||||
/** @brief 将数据块内容加载到值块缓冲区 */
|
||||
extern unsigned char Mifare_Restore(unsigned char block);
|
||||
|
||||
#endif
|
||||
83
apps/earphone/94_rfid_stc/inc/board.h
Normal file
83
apps/earphone/94_rfid_stc/inc/board.h
Normal file
@ -0,0 +1,83 @@
|
||||
/**
|
||||
******************************************************************************
|
||||
* @file board.h
|
||||
* @author SRG
|
||||
* @version V1.0.0
|
||||
* @date 2020-03-17
|
||||
* @brief
|
||||
******************************************************************************
|
||||
* @attention
|
||||
*
|
||||
* <h2><center>© Copyright (c) 2020 FudanMicroelectronics.
|
||||
* All rights reserved.</center></h2>
|
||||
*
|
||||
******************************************************************************
|
||||
*/
|
||||
|
||||
#ifndef __BOARD_H_
|
||||
#define __BOARD_H_
|
||||
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
|
||||
//#include "fm15l0xx.h"
|
||||
|
||||
|
||||
|
||||
//typedef enum
|
||||
//{
|
||||
// Mode_In_Up = 0, //<2F><><EFBFBD>٣<EFBFBD>pullup
|
||||
// Mode_In_No, //<2F><><EFBFBD>٣<EFBFBD>nopull
|
||||
// Mode_In_Down, //<2F><><EFBFBD>٣<EFBFBD>pulldown
|
||||
// Mode_Out_Up, //<2F><><EFBFBD>٣<EFBFBD><D9A3><EFBFBD><EFBFBD>죬pullup
|
||||
// Mode_Out_No, //<2F><><EFBFBD>٣<EFBFBD><D9A3><EFBFBD><EFBFBD>죬nopull
|
||||
// Mode_Out_Down, //<2F><><EFBFBD>٣<EFBFBD><D9A3><EFBFBD><EFBFBD>죬pulldown
|
||||
//} eGPIOMode;
|
||||
|
||||
|
||||
//HMI LED
|
||||
|
||||
|
||||
#define LED1_PIN LL_GPIO_Pin3 //PC3
|
||||
#define LED2_PIN LL_GPIO_Pin4 //PC4
|
||||
#define LED_GPIO GPIOC
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
#define LED1_On LL_GPIO_SetOutputPin( LED_GPIO, LED1_PIN )
|
||||
#define LED1_Off LL_GPIO_ResetOutputPin( LED_GPIO, LED1_PIN )
|
||||
|
||||
#define LED2_On LL_GPIO_SetOutputPin( LED_GPIO, LED2_PIN )
|
||||
#define LED2_Off LL_GPIO_ResetOutputPin( LED_GPIO, LED2_PIN )
|
||||
|
||||
|
||||
|
||||
|
||||
#define TEST_ADC_CH LL_ADC_CHANNEL_1
|
||||
|
||||
uint8_t UserKey_GetData( void );
|
||||
|
||||
void GPIO_NVIC_Init( void );
|
||||
|
||||
void Set_Clk(unsigned char mode);
|
||||
void SVD_Init( void );
|
||||
void BSP_Init( void );
|
||||
|
||||
uint16_t Get_Adc_Average( uint32_t ch, uint32_t ch_in, uint8_t cnt );
|
||||
|
||||
void LowPower_test( void );
|
||||
|
||||
void MCU_EnterLowerPowerMode( void );
|
||||
|
||||
void Set_Output(unsigned char mode);
|
||||
void Set_Pwm(unsigned char mode);
|
||||
void Set_FM440_Power(unsigned char mode);
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
#endif
|
||||
10
apps/earphone/94_rfid_stc/inc/bsp.h
Normal file
10
apps/earphone/94_rfid_stc/inc/bsp.h
Normal file
@ -0,0 +1,10 @@
|
||||
#include "main.h"
|
||||
|
||||
void bsp_init( void );
|
||||
|
||||
|
||||
void gpio_init( void );
|
||||
|
||||
|
||||
void uart_init( void );
|
||||
|
||||
46
apps/earphone/94_rfid_stc/inc/fm15l0xx_it.h
Normal file
46
apps/earphone/94_rfid_stc/inc/fm15l0xx_it.h
Normal file
@ -0,0 +1,46 @@
|
||||
/**
|
||||
******************************************************************************
|
||||
* @file Templates_LL/Inc/fm15l0xx_it.h
|
||||
* @author FMSH AS Embedded Software Team
|
||||
* @brief This file contains the headers of the interrupt handlers.
|
||||
******************************************************************************
|
||||
*/
|
||||
|
||||
/* Define to prevent recursive inclusion -------------------------------------*/
|
||||
#ifndef __FM15L0XX_IT_H__
|
||||
#define __FM15L0XX_IT_H__
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
/* Includes ------------------------------------------------------------------*/
|
||||
// #include "main.h"
|
||||
|
||||
/* Exported types ------------------------------------------------------------*/
|
||||
/* Exported constants --------------------------------------------------------*/
|
||||
/* Exported macro ------------------------------------------------------------*/
|
||||
/* Exported functions ------------------------------------------------------- */
|
||||
|
||||
void NMI_Handler( void );
|
||||
|
||||
|
||||
void HardFault_Handler( void );
|
||||
|
||||
|
||||
void SVC_Handler( void );
|
||||
|
||||
|
||||
void PendSV_Handler( void );
|
||||
|
||||
|
||||
void SysTick_Handler( void );
|
||||
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
|
||||
#endif /* __FM15L0XX_IT_H__ */
|
||||
|
||||
/************************ (C) COPYRIGHT FMSH Microelectronics *****END OF FILE****/
|
||||
146
apps/earphone/94_rfid_stc/inc/fm15l0xx_ll_rng.h
Normal file
146
apps/earphone/94_rfid_stc/inc/fm15l0xx_ll_rng.h
Normal file
@ -0,0 +1,146 @@
|
||||
/**
|
||||
************************************************************************************************************
|
||||
* @file : fm15l0xx_ll_rng.h
|
||||
* @author : weifan
|
||||
* @version : V1.0.0
|
||||
* @date : 2020-12-15
|
||||
* @brief :
|
||||
************************************************************************************************************
|
||||
* @attention :
|
||||
*
|
||||
* <h2><center>© Copyright (c) 2020 FudanMicroelectronics.
|
||||
* All rights reserved.</center></h2>
|
||||
*
|
||||
************************************************************************************************************
|
||||
*/
|
||||
|
||||
#ifndef __FM15L0XX_LL_RNG_H__
|
||||
#define __FM15L0XX_LL_RNG_H__
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
#include "fm15l0xx.h"
|
||||
|
||||
// ----------------------------------------------------------------------------------------------------
|
||||
// @descrption : This Function Set RegBit 'RNG_CR_RNGEN' Value
|
||||
// @arguments : *RNGx : Struct RNG_Typedef Pointer
|
||||
// @return : None
|
||||
// ----------------------------------------------------------------------------------------------------
|
||||
__STATIC_INLINE void LL_RNG_Enable( void )
|
||||
{
|
||||
SET_BIT( RNG->CR, RNG_CR_RNGEN_MSK );
|
||||
}
|
||||
|
||||
|
||||
// ----------------------------------------------------------------------------------------------------
|
||||
// @descrption : This Function Set RegBit 'RNG_CR_RNGEN' Value
|
||||
// @arguments : *RNGx : Struct RNG_Typedef Pointer
|
||||
// @return : None
|
||||
// ----------------------------------------------------------------------------------------------------
|
||||
__STATIC_INLINE void LL_RNG_Disable( void )
|
||||
{
|
||||
CLEAR_BIT( RNG->CR, RNG_CR_RNGEN_MSK );
|
||||
}
|
||||
|
||||
|
||||
// ----------------------------------------------------------------------------------------------------
|
||||
// @descrption : This Function Get RegBit 'RNG_CR_RNGEN' Value
|
||||
// @arguments : *RNGx : Struct RNG_Typedef Pointer
|
||||
// @retrun : RNGEN : uint32_t, RNGEN
|
||||
// ----------------------------------------------------------------------------------------------------
|
||||
__STATIC_INLINE uint32_t LL_RNG_IsEnabled( void )
|
||||
{
|
||||
return ( (uint32_t) (READ_BIT( RNG->CR, RNG_CR_RNGEN_MSK ) == RNG_CR_RNGEN_MSK) );
|
||||
}
|
||||
|
||||
|
||||
// ----------------------------------------------------------------------------------------------------
|
||||
// @descrption : This Function Set RegBit 'RNG_CR_SRCEN' Value
|
||||
// @arguments : *RNGx : Struct RNG_Typedef Pointer
|
||||
// @return : None
|
||||
// ----------------------------------------------------------------------------------------------------
|
||||
__STATIC_INLINE void LL_RNG_EnableNoiseSource( void )
|
||||
{
|
||||
SET_BIT( RNG->CR, RNG_CR_SRCEN_MSK );
|
||||
}
|
||||
|
||||
|
||||
// ----------------------------------------------------------------------------------------------------
|
||||
// @descrption : This Function Set RegBit 'RNG_CR_SRCEN' Value
|
||||
// @arguments : *RNGx : Struct RNG_Typedef Pointer
|
||||
// @return : None
|
||||
// ----------------------------------------------------------------------------------------------------
|
||||
__STATIC_INLINE void LL_RNG_DisableNoiseSource( void )
|
||||
{
|
||||
CLEAR_BIT( RNG->CR, RNG_CR_SRCEN_MSK );
|
||||
}
|
||||
|
||||
|
||||
// ----------------------------------------------------------------------------------------------------
|
||||
// @descrption : This Function Get RegBit 'RNG_CR_SRCEN' Value
|
||||
// @arguments : *RNGx : Struct RNG_Typedef Pointer
|
||||
// @retrun : SRCEN : uint32_t, SRCEN
|
||||
// ----------------------------------------------------------------------------------------------------
|
||||
__STATIC_INLINE uint32_t LL_RNG_IsEnabledNoiseSource( void )
|
||||
{
|
||||
return ( (uint32_t) (READ_BIT( RNG->CR, RNG_CR_SRCEN_MSK ) == RNG_CR_SRCEN_MSK) );
|
||||
}
|
||||
|
||||
|
||||
// ----------------------------------------------------------------------------------------------------
|
||||
// @descrption : This Function Get RegBit 'RNG_SR_DRDY' Value
|
||||
// @arguments : *RNGx : Struct RNG_Typedef Pointer
|
||||
// @retrun : DRDY : uint32_t, DRDY
|
||||
// ----------------------------------------------------------------------------------------------------
|
||||
__STATIC_INLINE uint32_t LL_RNG_IsDataReady( void )
|
||||
{
|
||||
return ( (uint32_t) (READ_BIT( RNG->SR, RNG_SR_DRDY_MSK ) == RNG_SR_DRDY_MSK) );
|
||||
}
|
||||
|
||||
|
||||
// ----------------------------------------------------------------------------------------------------
|
||||
// @descrption : This Function Set RegBit 'RNG_SR_SFAIL' Value
|
||||
// @arguments : *RNGx : Struct RNG_Typedef Pointer
|
||||
// SFAIL : uint32_t, SFAIL
|
||||
// @return : None
|
||||
// ----------------------------------------------------------------------------------------------------
|
||||
__STATIC_INLINE void LL_RNG_ClearFlag_SFAIL( void )
|
||||
{
|
||||
RNG->SR = RNG_SR_SFAIL_MSK;
|
||||
}
|
||||
|
||||
|
||||
// ----------------------------------------------------------------------------------------------------
|
||||
// @descrption : This Function Get RegBit 'RNG_SR_SFAIL' Value
|
||||
// @arguments : *RNGx : Struct RNG_Typedef Pointer
|
||||
// @retrun : SFAIL : uint32_t, SFAIL
|
||||
// ----------------------------------------------------------------------------------------------------
|
||||
__STATIC_INLINE uint32_t LL_RNG_IsActiveFlag_SFAIL( void )
|
||||
{
|
||||
return ( (uint32_t) (READ_BIT( RNG->SR, RNG_SR_SFAIL_MSK ) == RNG_SR_SFAIL_MSK) );
|
||||
}
|
||||
|
||||
|
||||
// ----------------------------------------------------------------------------------------------------
|
||||
// @descrption : This Function Get RegBit 'RNG_DR_RNGOUT' Value
|
||||
// @arguments : *RNGx : Struct RNG_Typedef Pointer
|
||||
// @retrun : RNGOUT : uint32_t, RNGOUT
|
||||
// ----------------------------------------------------------------------------------------------------
|
||||
__STATIC_INLINE uint32_t LL_RNG_GetRandomData( void )
|
||||
{
|
||||
return ( (uint32_t) (READ_BIT( RNG->DR, RNG_DR_RNGOUT_MSK ) ) );
|
||||
}
|
||||
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif // __cplusplus
|
||||
|
||||
|
||||
#endif // __FM15L0XX_LL_RNG_H__
|
||||
|
||||
|
||||
/************************ (C) COPYRIGHT FudanMicroelectronics *****END OF FILE****/
|
||||
|
||||
191
apps/earphone/94_rfid_stc/inc/function.h
Normal file
191
apps/earphone/94_rfid_stc/inc/function.h
Normal file
@ -0,0 +1,191 @@
|
||||
/**
|
||||
************************************* Copyright ******************************
|
||||
* (C) Copyright 2019,TuYW,FMSH,China.
|
||||
* All Rights Reserved
|
||||
* By(Shanghai Fudan MicroeleCardTyperonics Group Company Limited)
|
||||
* https://www.fmsh.com
|
||||
*
|
||||
* FileName : function.h
|
||||
* Version : v1.0
|
||||
* Author : TuYW
|
||||
* Date : 2019-03-19
|
||||
* Description:
|
||||
*******************************************************************************/
|
||||
|
||||
|
||||
#ifndef __FUNCTION_H_
|
||||
#define __FUNCTION_H_
|
||||
|
||||
#include <stdio.h>
|
||||
#include <string.h>
|
||||
#include <ctype.h>
|
||||
#include <stdlib.h>
|
||||
#include <stdarg.h>
|
||||
//#include "stdint.h"
|
||||
|
||||
/* Includes ------------------------------------------------------------------*/
|
||||
|
||||
// #include "fm15l0xx.h"
|
||||
//#include "core_cm3.h"
|
||||
//#include "sys.h"
|
||||
|
||||
|
||||
//#if APP_NO_IAP == 1
|
||||
// #define APP_ADDR 1
|
||||
//#endif
|
||||
|
||||
/* Exported types ------------------------------------------------------------*/
|
||||
#define REG32( addr ) (*( ( volatile unsigned long *) (addr) ) )
|
||||
|
||||
//#define assert_param(expr) ((void)0)
|
||||
|
||||
typedef unsigned char BOOLEAN;
|
||||
typedef unsigned char INT8U; /* Unsigned 8 bit quantity */
|
||||
typedef signed char INT8S; /* Signed 8 bit quantity */
|
||||
typedef unsigned short INT16U; /* Unsigned 16 bit quantity */
|
||||
typedef signed short INT16S; /* Signed 16 bit quantity */
|
||||
typedef unsigned int INT32U; /* Unsigned 32 bit quantity */
|
||||
typedef signed int INT32S; /* Signed 32 bit quantity */
|
||||
typedef float FP32; /* Single precision floating point */
|
||||
typedef double FP64; /* Double precision floating point */
|
||||
|
||||
|
||||
/*********************************************************************************************************
|
||||
ͨ<>ú궨<C3BA><EAB6A8>
|
||||
*********************************************************************************************************/
|
||||
#define PROGRAM_IAP_BASE FLASH_IAP_ADDR
|
||||
#define PROGRAM_IAP_LENGTH 0X6000
|
||||
|
||||
#define PROGRAM_APP_BASE FLASH_APP1_ADDR
|
||||
#define PROGRAM_APP_LENGTH 0X30000
|
||||
|
||||
#ifndef HIGH
|
||||
#define HIGH 1
|
||||
#endif
|
||||
|
||||
#ifndef LOW
|
||||
#define LOW 0
|
||||
#endif
|
||||
|
||||
#ifndef TRUE
|
||||
#define TRUE 1
|
||||
#endif //TRUE
|
||||
|
||||
#ifndef FALSE
|
||||
#define FALSE 0
|
||||
#endif //FALSE
|
||||
|
||||
#ifndef ON
|
||||
#define ON 1
|
||||
#endif //ON
|
||||
|
||||
#ifndef OFF
|
||||
#define OFF 0
|
||||
#endif //OFF
|
||||
|
||||
#ifndef NULL
|
||||
#define NULL ( (void *) 0)
|
||||
//<2F><>ַΪ0<CEAA><30>ָ<EFBFBD><D6B8>,void*<2A><><EFBFBD>͵<EFBFBD>ָ<EFBFBD><D6B8><EFBFBD><EFBFBD><EFBFBD>Ĭ<EFBFBD><C4AC>ת<EFBFBD><D7AA><EFBFBD><EFBFBD><EFBFBD>κ<EFBFBD><CEBA><EFBFBD><EFBFBD>͵<EFBFBD>ָ<EFBFBD>롣<EFBFBD><EBA1A3><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ǵĿ<C7B5>ָ<EFBFBD><D6B8>Ͷ<EFBFBD><CDB6><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||||
#endif
|
||||
|
||||
//<2F><>ʼ<EFBFBD><CABC><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>,<2C><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||||
//#define ClrLongBuf(buf) memset(buf, 0x00, sizeof(buf));
|
||||
|
||||
|
||||
/** The upper 8 bits of a 32 bit value */
|
||||
//lint -emacro(572,MSB) // Suppress warning 572 "Excessive shift value"
|
||||
#define MSB_32( a ) ( ( (a) & 0xFF000000) >> 24)
|
||||
/** The lower 8 bits (of a 32 bit value) */
|
||||
#define LSB_32( a ) ( (a) & 0x000000FF)
|
||||
|
||||
/** The upper 8 bits of a 16 bit value */
|
||||
//lint -emacro(572,MSB_16) // Suppress warning 572 "Excessive shift value"
|
||||
#define MSB_16( a ) ( ( (a) & 0xFF00) >> 8)
|
||||
/** The lower 8 bits (of a 16 bit value) */
|
||||
#define LSB_16( a ) ( (a) & 0x00FF)
|
||||
|
||||
///** Leaves the minimum of the two 32-bit arguments */
|
||||
///*lint -emacro(506, MIN) */ /* Suppress "Constant value Boolean */
|
||||
//#define MIN( a, b ) ( (a) < (b) ? (a) : (b) )
|
||||
///** Leaves the maximum of the two 32-bit arguments */
|
||||
///*lint -emacro(506, MAX) */ /* Suppress "Constant value Boolean */
|
||||
//#define MAX( a, b ) ( (a) < (b) ? (b) : (a) )
|
||||
|
||||
|
||||
/** Concatenates two parameters. Useful as a second level of indirection,
|
||||
* when a parameter can be macro itself. */
|
||||
#define CONCAT_2( p1, p2 ) p1 ## p2
|
||||
|
||||
|
||||
/** Concatenates three parameters. Useful as a second level of indirection,
|
||||
* when a parameter can be macro itself. */
|
||||
#define CONCAT_3( p1, p2, p3 ) p1 ## p2 ## p3
|
||||
|
||||
|
||||
/**@brief Set a bit in the uint32 word.
|
||||
*
|
||||
* @param[in] W Word whose bit is being set.
|
||||
* @param[in] B Bit number in the word to be set.
|
||||
*/
|
||||
#ifndef SET_BIT
|
||||
#define SET_BIT( W, B ) ( (W) |= (uint32_t) (1U << (B) ) )
|
||||
#endif
|
||||
|
||||
|
||||
/**@brief Clears a bit in the uint32 word.
|
||||
*
|
||||
* @param[in] W Word whose bit is to be cleared.
|
||||
* @param[in] B Bit number in the word to be cleared.
|
||||
*/
|
||||
#ifndef SET_BIT
|
||||
#define CLR_BIT( W, B ) ( (W) &= (~( (uint32_t) 1 << (B) ) ) )
|
||||
#endif
|
||||
|
||||
//typedef unsigned char BOOLEAN;
|
||||
|
||||
typedef unsigned char UINT8, U8, BYTE, u8, uint8_t;
|
||||
typedef unsigned short UINT16, U16, u16, uint16_t;
|
||||
typedef unsigned int UINT32, U32, DWORD, u32, uint32_t;
|
||||
|
||||
//typedef int32_t s32;
|
||||
//typedef int16_t s16;
|
||||
//typedef int8_t s8;
|
||||
|
||||
//typedef const int32_t sc32; /*!< Read Only */
|
||||
//typedef const int16_t sc16; /*!< Read Only */
|
||||
//typedef const int8_t sc8; /*!< Read Only */
|
||||
|
||||
//typedef __IO int32_t vs32;
|
||||
//typedef __IO int16_t vs16;
|
||||
//typedef __IO int8_t vs8;
|
||||
|
||||
//typedef __I int32_t vsc32; /*!< Read Only */
|
||||
//typedef __I int16_t vsc16; /*!< Read Only */
|
||||
//typedef __I int8_t vsc8; /*!< Read Only */
|
||||
|
||||
//typedef uint32_t u32;
|
||||
//typedef uint16_t u16;
|
||||
//typedef uint8_t u8;
|
||||
|
||||
//typedef const uint32_t uc32; /*!< Read Only */
|
||||
//typedef const uint16_t uc16; /*!< Read Only */
|
||||
//typedef const uint8_t uc8; /*!< Read Only */
|
||||
|
||||
//typedef __IO uint32_t vu32;
|
||||
//typedef __IO uint16_t vu16;
|
||||
//typedef __IO uint8_t vu8;
|
||||
|
||||
//typedef __I uint32_t vuc32; /*!< Read Only */
|
||||
//typedef __I uint16_t vuc16; /*!< Read Only */
|
||||
//typedef __I uint8_t vuc8; /*!< Read Only */
|
||||
|
||||
typedef void (*pFunction)( void );
|
||||
|
||||
void StrToHex( BYTE *pbDest, BYTE *pbSrc, int nLen );
|
||||
|
||||
|
||||
void HexToStr( BYTE *pbDest, BYTE *pbSrc, int nLen );
|
||||
|
||||
|
||||
#endif
|
||||
|
||||
64
apps/earphone/94_rfid_stc/inc/main.h
Normal file
64
apps/earphone/94_rfid_stc/inc/main.h
Normal file
@ -0,0 +1,64 @@
|
||||
/**
|
||||
******************************************************************************
|
||||
* @file Templates_LL/Inc/main.h
|
||||
* @author FMSH AS Embedded Software Team
|
||||
* @brief Header for main.c module
|
||||
******************************************************************************
|
||||
* @attention
|
||||
*
|
||||
* <h2><center>© COPYRIGHT(c) 2016 FMSH Microelectronics</center></h2>
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without modification,
|
||||
* are permitted provided that the following conditions are met:
|
||||
* 1. Redistributions of source code must retain the above copyright notice,
|
||||
* this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright notice,
|
||||
* this list of conditions and the following disclaimer in the documentation
|
||||
* and/or other materials provided with the distribution.
|
||||
* 3. Neither the name of FMSH Microelectronics nor the names of its contributors
|
||||
* may be used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
|
||||
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
||||
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
|
||||
* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
|
||||
* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
|
||||
* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
|
||||
* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
||||
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
|
||||
* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
|
||||
* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
******************************************************************************
|
||||
*/
|
||||
|
||||
/* Define to prevent recursive inclusion -------------------------------------*/
|
||||
#ifndef __MAIN_H__
|
||||
#define __MAIN_H__
|
||||
|
||||
/* Includes ------------------------------------------------------------------*/
|
||||
/* LL drivers common to all LL examples */
|
||||
//#include "fm15l0xx_ll_cmu.h"
|
||||
/* LL drivers specific to LL examples IPs */
|
||||
//#include "fm15l0xx_ll_gpio.h"
|
||||
//#include "fm15l0xx_ll_uart.h"
|
||||
|
||||
|
||||
//#if defined(USE_FULL_ASSERT)
|
||||
// #include "fm15l0xx_assert.h"
|
||||
//#endif /* USE_FULL_ASSERT */
|
||||
|
||||
/* Exported types ------------------------------------------------------------*/
|
||||
/* Exported constants --------------------------------------------------------*/
|
||||
|
||||
/* ============== BOARD SPECIFIC CONFIGURATION CODE BEGIN ============== */
|
||||
/* ============== BOARD SPECIFIC CONFIGURATION CODE END ============== */
|
||||
|
||||
/* Exported macro ------------------------------------------------------------*/
|
||||
#define UARTx UART0
|
||||
/* Exported functions ------------------------------------------------------- */
|
||||
int vdd_is_low( void );
|
||||
#endif /* __MAIN_H__ */
|
||||
|
||||
/************************ (C) COPYRIGHT FMSH Microelectronics *****END OF FILE****/
|
||||
11
apps/earphone/94_rfid_stc/inc/print.h
Normal file
11
apps/earphone/94_rfid_stc/inc/print.h
Normal file
@ -0,0 +1,11 @@
|
||||
#ifndef __PRINT_H_
|
||||
#define __PRINT_H_
|
||||
extern unsigned char lsfr[48];
|
||||
extern unsigned char lsfr_length;
|
||||
extern unsigned char ciphertext_to_binary[48];
|
||||
extern void Print_AUTH(unsigned char *key,unsigned char *TOKEN,unsigned char *rece_buff);//print <20><>֤
|
||||
extern void lsfr_init(unsigned char *S022_Token2);
|
||||
extern unsigned char lsfr_fm458(void);
|
||||
extern unsigned char PRINT_Calc_sendData(unsigned char *data_buf, unsigned char data_len);
|
||||
extern unsigned char PRINT_Calc_recvData(unsigned char *data_buf, unsigned char data_len);
|
||||
#endif
|
||||
7
apps/earphone/94_rfid_stc/inc/random_generator.h
Normal file
7
apps/earphone/94_rfid_stc/inc/random_generator.h
Normal file
@ -0,0 +1,7 @@
|
||||
#include "main.h"
|
||||
|
||||
void RNG_init( void );
|
||||
|
||||
|
||||
uint32_t Get_RandomNumber( void );
|
||||
|
||||
55
apps/earphone/94_rfid_stc/xt_main.h
Normal file
55
apps/earphone/94_rfid_stc/xt_main.h
Normal file
@ -0,0 +1,55 @@
|
||||
#define uchar unsigned char
|
||||
#define uint unsigned int
|
||||
//typedef unsigned char u8;
|
||||
//typedef unsigned int u16;
|
||||
//typedef unsigned long u32;
|
||||
|
||||
|
||||
#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();
|
||||
@ -81,10 +81,10 @@ const struct task_info task_info_table[] = {
|
||||
#if AUDIO_ENC_MPT_SELF_ENABLE
|
||||
{"enc_mpt_self", 3, 0, 512, 128 },
|
||||
#endif/*AUDIO_ENC_MPT_SELF_ENABLE*/
|
||||
// {"update", 1, 0, 256, 0 },
|
||||
// {"tws_ota", 2, 0, 256, 0 },
|
||||
// {"tws_ota_msg", 2, 0, 256, 128 },
|
||||
// {"dw_update", 2, 0, 256, 128 },
|
||||
{"update", 1, 0, 256, 0 },
|
||||
{"tws_ota", 2, 0, 256, 0 },
|
||||
{"tws_ota_msg", 2, 0, 256, 128 },
|
||||
{"dw_update", 2, 0, 256, 128 },
|
||||
{"rcsp_task", 2, 0, 640, 128 },
|
||||
// {"aud_capture", 4, 0, 512, 256 },
|
||||
// {"data_export", 5, 0, 512, 256 },
|
||||
|
||||
@ -32,9 +32,9 @@
|
||||
// IIC配置 //
|
||||
//*********************************************************************************//
|
||||
/*软件IIC设置*/ //xtell
|
||||
#define TCFG_SW_I2C0_CLK_PORT IO_PORTA_05 //软件IIC CLK脚选择 XTELL
|
||||
#define TCFG_SW_I2C0_DAT_PORT IO_PORTA_06 //软件IIC DAT脚选择
|
||||
#define TCFG_SW_I2C0_DELAY_CNT 10 //IIC延时参数，影响通讯时钟频率
|
||||
#define TCFG_SW_I2C0_CLK_PORT IO_PORTA_05//IO_PORTB_04 //软件IIC CLK脚选择 XTELL
|
||||
#define TCFG_SW_I2C0_DAT_PORT IO_PORTA_06//IO_PORTB_05 //软件IIC DAT脚选择
|
||||
#define TCFG_SW_I2C0_DELAY_CNT 0 //IIC延时参数，影响通讯时钟频率
|
||||
|
||||
/*硬件IIC端口选择 -- 具体看手册，这里写的不准 -- lmx
|
||||
SCL SDA
|
||||
@ -44,18 +44,17 @@
|
||||
'D': IO_PORTA_05 IO_PORTA_06
|
||||
|
||||
具体要选择哪个iic口，去board_jl701n_demo.c中设置：hw_iic_cfg
|
||||
|
||||
*/
|
||||
#define TCFG_HW_I2C0_PORTS 'B'
|
||||
#define TCFG_HW_I2C0_CLK 1000000 //硬件IIC波特率:100k
|
||||
#define TCFG_HW_I2C0_CLK 100000
|
||||
|
||||
//*********************************************************************************//
|
||||
// 硬件SPI 配置 //
|
||||
//*********************************************************************************//
|
||||
#define TCFG_HW_SPI1_ENABLE 1
|
||||
#define TCFG_HW_SPI1_PORT_CLK 0//IO_PORTC_04//IO_PORTA_00 xtellio
|
||||
#define TCFG_HW_SPI1_PORT_DO IO_PORTC_05//IO_PORTA_01
|
||||
#define TCFG_HW_SPI1_PORT_DI 0//IO_PORTC_03//IO_PORTA_02 xtellio
|
||||
#define TCFG_HW_SPI1_PORT_CLK NO_CONFIG_PORT//IO_PORTC_04//IO_PORTC_04//IO_PORTA_00 xtellio
|
||||
#define TCFG_HW_SPI1_PORT_DO NO_CONFIG_PORT//IO_PORTC_05//IO_PORTA_01
|
||||
#define TCFG_HW_SPI1_PORT_DI NO_CONFIG_PORT//IO_PORTC_03//IO_PORTC_03//IO_PORTA_02 xtellio
|
||||
#define TCFG_HW_SPI1_BAUD 2400000L
|
||||
#define TCFG_HW_SPI1_MODE SPI_MODE_BIDIR_1BIT
|
||||
#define TCFG_HW_SPI1_ROLE SPI_ROLE_MASTER
|
||||
@ -87,7 +86,7 @@
|
||||
//*********************************************************************************//
|
||||
// USB 配置 //
|
||||
//*********************************************************************************//
|
||||
#define TCFG_PC_ENABLE 1//DISABLE_THIS_MOUDLE//PC模块使能
|
||||
#define TCFG_PC_ENABLE DISABLE_THIS_MOUDLE//PC模块使能
|
||||
#define TCFG_UDISK_ENABLE 0//ENABLE_THIS_MOUDLE//U盘模块使能
|
||||
#define TCFG_OTG_USB_DEV_EN BIT(0)//USB0 = BIT(0) USB1 = BIT(1)
|
||||
|
||||
@ -729,7 +728,7 @@ DAC硬件上的连接方式,可选的配置：
|
||||
// 充电舱/蓝牙测试盒/ANC测试三者为同级关系,开启任一功能都会初始化PP0通信接口 //
|
||||
//*********************************************************************************//
|
||||
#define TCFG_CHARGESTORE_ENABLE DISABLE_THIS_MOUDLE //是否支持智能充电舱
|
||||
#define TCFG_TEST_BOX_ENABLE ENABLE_THIS_MOUDLE //是否支持蓝牙测试盒 //xtell
|
||||
#define TCFG_TEST_BOX_ENABLE DISABLE_THIS_MOUDLE //是否支持蓝牙测试盒 //xtell
|
||||
#define TCFG_ANC_BOX_ENABLE CONFIG_ANC_ENABLE //是否支持ANC测试盒
|
||||
#define TCFG_UMIDIGI_BOX_ENABLE DISABLE_THIS_MOUDLE //是否支持UMIDIGI充电舱 //xtell
|
||||
#if TCFG_UMIDIGI_BOX_ENABLE
|
||||
@ -807,7 +806,7 @@ DAC硬件上的连接方式,可选的配置：
|
||||
// EQ配置 //
|
||||
//*********************************************************************************//
|
||||
//EQ配置，使用在线EQ时，EQ文件和EQ模式无效。有EQ文件时，使能TCFG_USE_EQ_FILE,默认不用EQ模式切换功能
|
||||
#define TCFG_EQ_ENABLE 1 //支持EQ功能,EQ总使能
|
||||
#define TCFG_EQ_ENABLE 0 //支持EQ功能,EQ总使能
|
||||
// #if TCFG_EQ_ENABLE
|
||||
#define TCFG_EQ_ONLINE_ENABLE 0 //支持在线EQ调试,如果使用蓝牙串口调试，需要打开宏 APP_ONLINE_DEBUG，否则，默认使用uart调试(二选一)
|
||||
#define TCFG_BT_MUSIC_EQ_ENABLE 1 //支持蓝牙音乐EQ
|
||||
@ -920,7 +919,7 @@ DAC硬件上的连接方式,可选的配置：
|
||||
#define TCFG_STK8321_EN 0
|
||||
#define TCFG_IRSENSOR_ENABLE 0
|
||||
#define TCFG_JSA1221_ENABLE 0
|
||||
#define TCFG_GSENOR_USER_IIC_TYPE 1 //0:软件IIC 1:硬件IIC
|
||||
#define TCFG_GSENOR_USER_IIC_TYPE 0 //0:软件IIC 1:硬件IIC
|
||||
|
||||
//*********************************************************************************//
|
||||
// imu-sensor配置 //
|
||||
@ -1010,7 +1009,7 @@ DAC硬件上的连接方式,可选的配置：
|
||||
//*********************************************************************************//
|
||||
#define TCFG_USER_TWS_ENABLE 0 //tws功能使能
|
||||
#define TCFG_USER_BLE_ENABLE 1 //BLE功能使能
|
||||
#define TCFG_BT_SUPPORT_AAC 1 //AAC格式支持
|
||||
#define TCFG_BT_SUPPORT_AAC 0 //AAC格式支持
|
||||
#define TCFG_BT_SUPPORT_LDAC 0 //LDAC格式支持
|
||||
|
||||
#if TCFG_BT_SUPPORT_LDAC
|
||||
@ -1084,7 +1083,7 @@ DAC硬件上的连接方式,可选的配置：
|
||||
// 编解码格式配置(CodecFormat) //
|
||||
//*********************************************************************************//
|
||||
/*解码格式使能*/
|
||||
#define TCFG_DEC_MP3_ENABLE ENABLE
|
||||
#define TCFG_DEC_MP3_ENABLE DISABLE
|
||||
#define TCFG_DEC_WTGV2_ENABLE ENABLE
|
||||
#define TCFG_DEC_G729_ENABLE DISABLE
|
||||
#define TCFG_DEC_WMA_ENABLE DISABLE
|
||||
|
||||
@ -8,8 +8,8 @@
|
||||
|
||||
/* Following Macros Affect Periods Of Both Code Compiling And Post-build */
|
||||
|
||||
#define CONFIG_DOUBLE_BANK_ENABLE 0 //单双备份选择(若打开了改宏,FLASH结构变为双备份结构，适用于接入第三方协议的OTA， PS: JL-OTA同样支持双备份升级, 需要根据实际FLASH大小同时配置CONFIG_FLASH_SIZE)
|
||||
#define CONFIG_APP_OTA_ENABLE 0 //是否支持RCSP升级(JL-OTA)
|
||||
#define CONFIG_DOUBLE_BANK_ENABLE 1 //单双备份选择(若打开了改宏,FLASH结构变为双备份结构，适用于接入第三方协议的OTA， PS: JL-OTA同样支持双备份升级, 需要根据实际FLASH大小同时配置CONFIG_FLASH_SIZE)
|
||||
#define CONFIG_APP_OTA_ENABLE 1 //是否支持RCSP升级(JL-OTA)
|
||||
|
||||
#define CONFIG_UPDATE_JUMP_TO_MASK 0 //配置升级到loader的方式0为直接reset,1为跳转(适用于芯片电源由IO口KEEP住的方案,需要注意检查跳转前是否将使用DMA的硬件模块全部关闭)
|
||||
|
||||
@ -33,7 +33,7 @@
|
||||
/* Following Macros Only For Post Bulid Configuaration */
|
||||
|
||||
#define CONFIG_DB_UPDATE_DATA_GENERATE_EN 0 //是否生成db_data.bin(用于第三方协议接入使用)
|
||||
#define CONFIG_ONLY_GRENERATE_ALIGN_4K_CODE 0 //ufw只生成1份4K对齐的代码
|
||||
#define CONFIG_ONLY_GRENERATE_ALIGN_4K_CODE 1 //ufw只生成1份4K对齐的代码
|
||||
|
||||
//config for supported chip version
|
||||
#ifdef CONFIG_BR30_C_VERSION
|
||||
|
||||
@ -70,6 +70,7 @@
|
||||
#include "bt_background.h"
|
||||
#include "default_event_handler.h"
|
||||
|
||||
|
||||
#define xlog(format, ...) printf("[%s] " format, __func__, ##__VA_ARGS__)
|
||||
|
||||
|
||||
@ -2790,3 +2791,4 @@ static int state_machine(struct application *app, enum app_state state, struct i
|
||||
// .ops = &app_earphone_ops,
|
||||
// .state = APP_STA_DESTROY,
|
||||
// };
|
||||
|
||||
|
||||
@ -14,7 +14,7 @@
|
||||
//#define CONFIG_DEBUG_LITE_ENABLE //轻量级打印开关, 默认关闭
|
||||
#endif
|
||||
|
||||
|
||||
#define BLE_WIRELESS_CLIENT_EN 1 //开启则作为主设备，lmx
|
||||
//*********************************************************************************//
|
||||
// AI配置 //
|
||||
//*********************************************************************************//
|
||||
@ -23,12 +23,16 @@
|
||||
#ifdef CONFIG_APP_BT_ENABLE
|
||||
#define TRANS_DATA_EN 0
|
||||
#define RCSP_BTMATE_EN 0
|
||||
#define RCSP_ADV_EN 1
|
||||
#if BLE_WIRELESS_CLIENT_EN == 0
|
||||
#define RCSP_ADV_EN 1 // lmx，打开这个，则作为ble从设备
|
||||
#else
|
||||
#define RCSP_ADV_EN 0
|
||||
#endif
|
||||
#define AI_APP_PROTOCOL 0
|
||||
#define LL_SYNC_EN 0
|
||||
#define TUYA_DEMO_EN 0
|
||||
#else
|
||||
#define TRANS_DATA_EN 1
|
||||
#define TRANS_DATA_EN 0
|
||||
#define RCSP_BTMATE_EN 0
|
||||
#define RCSP_ADV_EN 0
|
||||
#define AI_APP_PROTOCOL 0
|
||||
@ -65,11 +69,11 @@
|
||||
#if CONFIG_APP_OTA_ENABLE
|
||||
#define RCSP_UPDATE_EN 1 //是否支持rcsp升级
|
||||
#if CONFIG_DOUBLE_BANK_ENABLE //双备份才能打开同步升级流程
|
||||
#define OTA_TWS_SAME_TIME_ENABLE 1 //是否支持TWS同步升级
|
||||
#define OTA_TWS_SAME_TIME_NEW 1 //使用新的tws ota流程
|
||||
#define OTA_TWS_SAME_TIME_ENABLE 0 //是否支持TWS同步升级
|
||||
#define OTA_TWS_SAME_TIME_NEW 0 //使用新的tws ota流程
|
||||
#else
|
||||
#define OTA_TWS_SAME_TIME_ENABLE 1//0 xtellota //是否支持TWS同步升级
|
||||
#define OTA_TWS_SAME_TIME_NEW 1//0 //使用新的tws ota流程
|
||||
#define OTA_TWS_SAME_TIME_ENABLE 0 //xtell: 关闭TWS同步升级，允许单耳升级
|
||||
#define OTA_TWS_SAME_TIME_NEW 0 //xtell: 关闭TWS同步升级，此选项也应关闭
|
||||
#endif //CONFIG_DOUBLE_BANK_ENABLE
|
||||
#else
|
||||
#define RCSP_UPDATE_EN 0 //是否支持rcsp升级
|
||||
|
||||
@ -204,31 +204,31 @@ int app_protocol_sys_event_handler(struct sys_event *event);
|
||||
#define EARPHONE_STATE_SNIFF(a)
|
||||
#define EARPHONE_STATE_ROLE_SWITCH(a)
|
||||
|
||||
#else
|
||||
int adv_earphone_state_init();
|
||||
int adv_earphone_state_set_page_scan_enable();
|
||||
int adv_earphone_state_get_connect_mac_addr();
|
||||
int adv_earphone_state_cancel_page_scan();
|
||||
int adv_earphone_state_enter_soft_poweroff();
|
||||
int adv_earphone_state_tws_init(int paired);
|
||||
int adv_earphone_state_tws_connected(int first_pair, u8 *comm_addr);
|
||||
int adv_sys_event_handler_specific(struct sys_event *event);
|
||||
int adv_earphone_state_sniff(u8 state);
|
||||
int adv_earphone_state_role_switch(u8 role);
|
||||
// #else
|
||||
// int adv_earphone_state_init();
|
||||
// int adv_earphone_state_set_page_scan_enable();
|
||||
// int adv_earphone_state_get_connect_mac_addr();
|
||||
// int adv_earphone_state_cancel_page_scan();
|
||||
// int adv_earphone_state_enter_soft_poweroff();
|
||||
// int adv_earphone_state_tws_init(int paired);
|
||||
// int adv_earphone_state_tws_connected(int first_pair, u8 *comm_addr);
|
||||
// int adv_sys_event_handler_specific(struct sys_event *event);
|
||||
// int adv_earphone_state_sniff(u8 state);
|
||||
// int adv_earphone_state_role_switch(u8 role);
|
||||
|
||||
#define EARPHONE_STATE_INIT() adv_earphone_state_init()
|
||||
#define EARPHONE_STATE_SET_PAGE_SCAN_ENABLE() adv_earphone_state_set_page_scan_enable()
|
||||
#define EARPHONE_STATE_GET_CONNECT_MAC_ADDR() adv_earphone_state_get_connect_mac_addr()
|
||||
#define EARPHONE_STATE_CANCEL_PAGE_SCAN() adv_earphone_state_cancel_page_scan()
|
||||
#define EARPHONE_STATE_ENTER_SOFT_POWEROFF() adv_earphone_state_enter_soft_poweroff()
|
||||
#define EARPHONE_STATE_TWS_INIT(a) adv_earphone_state_tws_init(a)
|
||||
#define EARPHONE_STATE_TWS_CONNECTED(a, b) adv_earphone_state_tws_connected(a,b)
|
||||
#define SYS_EVENT_HANDLER_SPECIFIC(a) adv_sys_event_handler_specific(a)
|
||||
#define SYS_EVENT_REMAP(a) 0
|
||||
#define EARPHONE_STATE_SNIFF(a) adv_earphone_state_sniff(a)
|
||||
#define EARPHONE_STATE_ROLE_SWITCH(a) adv_earphone_state_role_switch(a)
|
||||
// #define EARPHONE_STATE_INIT() adv_earphone_state_init()
|
||||
// #define EARPHONE_STATE_SET_PAGE_SCAN_ENABLE() adv_earphone_state_set_page_scan_enable()
|
||||
// #define EARPHONE_STATE_GET_CONNECT_MAC_ADDR() adv_earphone_state_get_connect_mac_addr()
|
||||
// #define EARPHONE_STATE_CANCEL_PAGE_SCAN() adv_earphone_state_cancel_page_scan()
|
||||
// #define EARPHONE_STATE_ENTER_SOFT_POWEROFF() adv_earphone_state_enter_soft_poweroff()
|
||||
// #define EARPHONE_STATE_TWS_INIT(a) adv_earphone_state_tws_init(a)
|
||||
// #define EARPHONE_STATE_TWS_CONNECTED(a, b) adv_earphone_state_tws_connected(a,b)
|
||||
// #define SYS_EVENT_HANDLER_SPECIFIC(a) adv_sys_event_handler_specific(a)
|
||||
// #define SYS_EVENT_REMAP(a) 0
|
||||
// #define EARPHONE_STATE_SNIFF(a) adv_earphone_state_sniff(a)
|
||||
// #define EARPHONE_STATE_ROLE_SWITCH(a) adv_earphone_state_role_switch(a)
|
||||
|
||||
#endif
|
||||
// #endif
|
||||
|
||||
|
||||
#else
|
||||
@ -246,6 +246,7 @@ int adv_earphone_state_role_switch(u8 role);
|
||||
#define EARPHONE_STATE_ROLE_SWITCH(a)
|
||||
#endif
|
||||
|
||||
#endif
|
||||
|
||||
|
||||
|
||||
|
||||
@ -231,6 +231,7 @@ const int config_btctler_le_roles = (LE_ADV | LE_SLAVE);
|
||||
const uint64_t config_btctler_le_features = LE_ENCRYPTION;
|
||||
|
||||
#else
|
||||
|
||||
const int config_btctler_le_roles = (LE_ADV | LE_SLAVE);
|
||||
const uint64_t config_btctler_le_features = 0;
|
||||
#endif
|
||||
|
||||
@ -203,8 +203,8 @@ void cfg_file_parse(u8 idx)
|
||||
} else if (ret >= LOCAL_NAME_LEN) {
|
||||
memset(bt_cfg.edr_name, 0x00, LOCAL_NAME_LEN);
|
||||
// memcpy(bt_cfg.edr_name, tmp, LOCAL_NAME_LEN);
|
||||
extern char xt_ble_new_name[9];
|
||||
memcpy(bt_cfg.edr_name, xt_ble_new_name, LOCAL_NAME_LEN);
|
||||
// extern char xt_ble_new_name[9];
|
||||
// memcpy(bt_cfg.edr_name, xt_ble_new_name, LOCAL_NAME_LEN);
|
||||
bt_cfg.edr_name[LOCAL_NAME_LEN - 1] = 0;
|
||||
} else {
|
||||
memset(bt_cfg.edr_name, 0x00, LOCAL_NAME_LEN);
|
||||
|
||||
@ -1,219 +0,0 @@
|
||||
/*
|
||||
静态ZUPT+卡尔曼，效果貌似还行
|
||||
*/
|
||||
|
||||
#include "skiing_tracker.h"
|
||||
#include "../sensor/SC7U22.h" // 包含传感器驱动头文件以调用姿态解算函数
|
||||
#include <math.h>
|
||||
#include <string.h>
|
||||
|
||||
#define G_ACCELERATION 9.81f // 重力加速度 (m/s^2)
|
||||
#define DEG_TO_RAD (3.14159265f / 180.0f)
|
||||
|
||||
// --- 状态检测阈值 ---
|
||||
// 判断是否静止的加速度阈值 (m/s^2)。当加速度的模长减去重力后，小于此值，则认为可能静止。
|
||||
#define STATIC_ACC_THRESHOLD 1.0f
|
||||
// 连续多少帧满足静止条件才确认为静止状态
|
||||
#define STATIC_FRAMES_REQUIRED 50 // 累加，超过这个数加速度仍变化不大，说明近似静止
|
||||
|
||||
/**
|
||||
* @brief 初始化滑雪追踪器
|
||||
*/
|
||||
void skiing_tracker_init(skiing_tracker_t *tracker)
|
||||
{
|
||||
if (!tracker) {
|
||||
return;
|
||||
}
|
||||
memset(tracker, 0, sizeof(skiing_tracker_t));
|
||||
tracker->state = SKIING_STATE_STATIC; // 初始状态为静止
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 将设备坐标系下的加速度转换为世界坐标系
|
||||
* @param acc_device 设备坐标系下的加速度 [x, y, z]
|
||||
* @param angle 姿态角 [pitch, roll, yaw]，单位: 度
|
||||
* @param acc_world 输出：世界坐标系下的加速度 [x, y, z]
|
||||
*/
|
||||
static void transform_acc_to_world_frame(const float *acc_device, const float *angle, float *acc_world)
|
||||
{
|
||||
// 修正#1：驱动输出的角度与标准航空定义相反，需要取反才能用于标准旋转矩阵。
|
||||
float pitch = -angle[0] * DEG_TO_RAD;
|
||||
float roll = -angle[1] * DEG_TO_RAD;
|
||||
|
||||
float cp = cosf(pitch);
|
||||
float sp = sinf(pitch);
|
||||
float cr = cosf(roll);
|
||||
float sr = sinf(roll);
|
||||
|
||||
float ax = acc_device[0];
|
||||
float ay = acc_device[1];
|
||||
float az = acc_device[2];
|
||||
|
||||
// 修正#2：使用经过验证的、正确的身体坐标系到世界坐标系的旋转矩阵 (基于 Y-X 旋转顺序)
|
||||
// 这个矩阵将设备测量的加速度(ax, ay, az)正确地转换到世界坐标系(acc_world)。
|
||||
acc_world[0] = cp * ax + sp * sr * ay + sp * cr * az;
|
||||
acc_world[1] = 0 * ax + cr * ay - sr * az;
|
||||
acc_world[2] = -sp * ax + cp * sr * ay + cp * cr * az;
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* @brief 更新滑雪状态机
|
||||
*/
|
||||
static void update_state_machine(skiing_tracker_t *tracker, const float *acc_device)
|
||||
{
|
||||
// 计算当前加速度的模长
|
||||
float acc_magnitude = sqrtf(acc_device[0] * acc_device[0] + acc_device[1] * acc_device[1] + acc_device[2] * acc_device[2]);
|
||||
|
||||
// 状态判断逻辑
|
||||
switch (tracker->state) {
|
||||
case SKIING_STATE_STATIC:
|
||||
// 如果加速度变化较大，则切换到滑雪状态
|
||||
if (fabsf(acc_magnitude - G_ACCELERATION) > STATIC_ACC_THRESHOLD * 2.0f) { // 使用一个更大的阈值来启动
|
||||
tracker->state = SKIING_STATE_SKIING;
|
||||
tracker->static_frames_count = 0;
|
||||
}
|
||||
break;
|
||||
|
||||
case SKIING_STATE_SKIING:
|
||||
// 检测是否进入静止状态 (零速更新 ZUPT)
|
||||
if (fabsf(acc_magnitude - G_ACCELERATION) < STATIC_ACC_THRESHOLD) {
|
||||
tracker->static_frames_count++;
|
||||
if (tracker->static_frames_count >= STATIC_FRAMES_REQUIRED) {
|
||||
tracker->state = SKIING_STATE_STATIC;
|
||||
// 进入静止状态，强制将速度清零以消除漂移
|
||||
memset(tracker->velocity, 0, sizeof(tracker->velocity));
|
||||
tracker->speed = 0.0f;
|
||||
}
|
||||
} else {
|
||||
// 如果在运动，则重置静止计数器
|
||||
tracker->static_frames_count = 0;
|
||||
}
|
||||
break;
|
||||
|
||||
// 可以在此添加摔倒等其他状态的判断
|
||||
case SKIING_STATE_FALLEN:
|
||||
// TODO: 添加从摔倒状态恢复的逻辑
|
||||
break;
|
||||
|
||||
default:
|
||||
tracker->state = SKIING_STATE_UNKNOWN;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* @brief 主更新函数
|
||||
*/
|
||||
void skiing_tracker_update(skiing_tracker_t *tracker, float *acc, float *angle, float dt)
|
||||
{
|
||||
if (!tracker || !acc || !angle || dt <= 0) {
|
||||
return;
|
||||
}
|
||||
|
||||
// 将原始g单位的加速度转换为 m/s^2
|
||||
float acc_device_ms2[3];
|
||||
acc_device_ms2[0] = acc[0] * G_ACCELERATION;
|
||||
acc_device_ms2[1] = acc[1] * G_ACCELERATION;
|
||||
acc_device_ms2[2] = acc[2] * G_ACCELERATION;
|
||||
|
||||
// 更新状态机
|
||||
update_state_machine(tracker, acc_device_ms2);
|
||||
|
||||
// 只有在滑雪状态下才进行计算
|
||||
if (tracker->state == SKIING_STATE_SKIING) {
|
||||
// 坐标系转换: 首先，利用姿态角(Pitch, Roll)将传感器测得的总加速度（运动加速度+重力）
|
||||
// 从不断变化的“设备坐标系”转换到一个固定的“世界坐标系”。在这个世界坐标系里，Z轴永远垂直于地面指向上方。
|
||||
// 执行坐标系转换
|
||||
transform_acc_to_world_frame(acc_device_ms2, angle, tracker->acc_world);
|
||||
// 转换完成后，重力就变成了一个恒定的、方向沿Z轴向下的矢量。
|
||||
|
||||
// 修正#3：经过正确的坐标转换后，静止时重力在世界坐标系Z轴上的分量精确为+g。
|
||||
// 必须减去这个g，才能得到纯粹的运动加速度。
|
||||
tracker->acc_world[2] -= G_ACCELERATION;
|
||||
|
||||
// 积分计算速度 (v = v0 + a*t)
|
||||
tracker->velocity[0] += tracker->acc_world[0] * dt;
|
||||
tracker->velocity[1] += tracker->acc_world[1] * dt;
|
||||
tracker->velocity[2] += tracker->acc_world[2] * dt; // 垂直方向速度也计算在内
|
||||
|
||||
// 计算当前速率
|
||||
tracker->speed = sqrtf(tracker->velocity[0] * tracker->velocity[0] +
|
||||
tracker->velocity[1] * tracker->velocity[1] +
|
||||
tracker->velocity[2] * tracker->velocity[2]);
|
||||
|
||||
// 积分计算距离 (d = d0 + v*t)
|
||||
tracker->distance += tracker->speed * dt;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
// 传感器数据采集与处理任务
|
||||
void sensor_processing_task(signed short * acc_data_buf,signed short * gyr_data_buf) {
|
||||
// --- 1. 定义静态变量 ---
|
||||
static skiing_tracker_t my_skiing_tracker;
|
||||
static int initialized = 0;
|
||||
static int calibration_done = 0; // 新增：用于标记一次性静态校准是否完成
|
||||
|
||||
static signed short combined_raw_data[6];
|
||||
static float final_angle_data[3];
|
||||
static float calibrated_acc_g[3];
|
||||
|
||||
// sensor_processing_task的调用频率, dt = 0.001s
|
||||
const float delta_time = 0.01f;
|
||||
|
||||
// --- 2. 初始化 ---
|
||||
if (!initialized) {
|
||||
skiing_tracker_init(&my_skiing_tracker);
|
||||
initialized = 1;
|
||||
printf("Skiing Tracker Initialized. Waiting for sensor calibration...\n");
|
||||
}
|
||||
|
||||
// --- 3. 数据处理 ---
|
||||
//合并加速度和陀螺仪数据
|
||||
memcpy(&combined_raw_data[0], acc_data_buf, 3 * sizeof(signed short));
|
||||
memcpy(&combined_raw_data[3], gyr_data_buf, 3 * sizeof(signed short));
|
||||
|
||||
unsigned char status;
|
||||
if (get_calibration_state() == 0) { //正在校准
|
||||
//领票校准
|
||||
status = SL_SC7U22_Angle_Output(1, combined_raw_data, final_angle_data, 0);
|
||||
if (status == 1) {
|
||||
calibration_done = 1;
|
||||
printf("Sensor calibration successful! Skiing mode is active.\n");
|
||||
}
|
||||
} else {
|
||||
status = SL_SC7U22_Angle_Output(0, combined_raw_data, final_angle_data, 0);
|
||||
}
|
||||
|
||||
// c. 检查姿态解算是否成功
|
||||
if (status == 1) {
|
||||
// 将校准后的加速度数据从 LSB (原始值) 转换为 g (重力单位)
|
||||
// ±8g量程下，8192 LSB 对应 1g
|
||||
calibrated_acc_g[0] = (float)combined_raw_data[0] / 8192.0f;
|
||||
calibrated_acc_g[1] = (float)combined_raw_data[1] / 8192.0f;
|
||||
calibrated_acc_g[2] = (float)combined_raw_data[2] / 8192.0f;
|
||||
|
||||
skiing_tracker_update(&my_skiing_tracker, calibrated_acc_g, final_angle_data, delta_time);
|
||||
|
||||
static count = 0;
|
||||
if(count < 10){
|
||||
count++;
|
||||
return;
|
||||
}else{
|
||||
count = 0;
|
||||
}
|
||||
printf("State: %d, Speed: %.2f m/s, Distance: %.2f m\n",
|
||||
my_skiing_tracker.state,
|
||||
my_skiing_tracker.speed,
|
||||
my_skiing_tracker.distance);
|
||||
|
||||
} else if (status == 0) {
|
||||
// 传感器正在进行静态校准
|
||||
// printf("Sensor is calibrating...\n");
|
||||
} else {
|
||||
// status == 2, 表示校准失败或发生错误
|
||||
// printf("Angle calculation error or calibration not finished.\n");
|
||||
}
|
||||
}
|
||||
@ -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
|
||||
@ -1,596 +0,0 @@
|
||||
/*
|
||||
|
||||
*/
|
||||
#include "skiing_tracker.h"
|
||||
#include "../sensor/SC7U22.h"
|
||||
#include <math.h>
|
||||
#include <string.h>
|
||||
|
||||
#define G_ACCELERATION 9.81f
|
||||
#define DEG_TO_RAD (3.14159265f / 180.0f)
|
||||
|
||||
// --- 静止检测 ---
|
||||
//两个判断是否静止的必要条件：动态零速更新(ZUPT)阈值
|
||||
// 加速方差阈值，提高阈值，让“刹车”更灵敏，以便在波浪式前进等慢速漂移时也能触发零速更新
|
||||
#define STOP_ACC_VARIANCE_THRESHOLD 0.2f
|
||||
// 陀螺仪方差阈值
|
||||
#define STOP_GYR_VARIANCE_THRESHOLD 5.0f
|
||||
// 静止时候的陀螺仪模长
|
||||
#define STOP_GYR_MAG_THRESHOLD 15
|
||||
// --- --- ---
|
||||
|
||||
// --- 启动滑雪阈值 ---
|
||||
// 加速度模长与重力的差值大于此值，认为开始运动；降低阈值，让“油门”更灵敏，以便能捕捉到真实的慢速启动
|
||||
#define START_ACC_MAG_THRESHOLD 1.0f //0.5、1
|
||||
// 陀螺仪方差阈值，以允许启动瞬间的正常抖动，但仍能过滤掉混乱的、非滑雪的晃动。
|
||||
#define START_GYR_VARIANCE_THRESHOLD 15.0f
|
||||
// --- --- ---
|
||||
|
||||
// --- 滑雪过程 ---
|
||||
//加速度 模长(不含重力），低于此值视为 在做匀速运动
|
||||
#define SKIING_ACC_MAG_THRESHOLD 0.5f
|
||||
//陀螺仪 模长，高于此值视为 摔倒了
|
||||
#define FALLEN_GRY_MAG_THRESHOLD 2000.0f //未确定
|
||||
// --- --- ---
|
||||
|
||||
// --- 原地旋转抖动 ---
|
||||
// 加速度 方差 阈值。此值比 静止检测 阈值更宽松，
|
||||
#define WOBBLE_ACC_VARIANCE_THRESHOLD 0.5f
|
||||
// 加速度 模长 阈值
|
||||
#define WOBBLE_ACC_MAG_THRESHOLD 1.0f
|
||||
// 角速度 总模长 大于此值(度/秒)，认为正在进行非滑雪的旋转或摆动
|
||||
#define ROTATION_GYR_MAG_THRESHOLD 30.0f
|
||||
// --- --- ---
|
||||
|
||||
// --- 滑雪转弯动 ---
|
||||
// 加速度 方差 阈值，大于此值，滑雪过程可能发生了急转弯
|
||||
#define WHEEL_ACC_VARIANCE_THRESHOLD 7.0f
|
||||
// 角速度 总模长 大于此值(度/秒)，认为滑雪过程中进行急转弯
|
||||
#define WHEEL_GYR_MAG_THRESHOLD 500.0f //
|
||||
// --- --- ---
|
||||
|
||||
// --- 跳跃 ---
|
||||
// 加速度模长低于此值(g)，认为进入失重状态(IN_AIR)
|
||||
#define AIRBORNE_ACC_MAG_LOW_THRESHOLD 0.4f
|
||||
// 加速度模长高于此值(g)，认为发生落地冲击(LANDING)
|
||||
#define LANDING_ACC_MAG_HIGH_THRESHOLD 3.5f
|
||||
// 起跳加速度阈值(g)，用于进入TAKING_OFF状态
|
||||
#define TAKEOFF_ACC_MAG_HIGH_THRESHOLD 1.8f
|
||||
// 进入空中状态确认计数：需要连续3个采样点加速度低于阈值才判断为起跳
|
||||
#define AIRBORNE_CONFIRM_COUNT 3
|
||||
// 落地状态确认计数：加速度恢复到1g附近并持续2个采样点(20ms)则认为已落地
|
||||
#define GROUNDED_CONFIRM_COUNT 2
|
||||
// 最大滞空时间(秒)，超过此时间强制认为已落地，防止状态锁死
|
||||
#define MAX_TIME_IN_AIR 12.5f
|
||||
// --- --- ---
|
||||
|
||||
// --- 用于消除积分漂移的滤波器和阈值 ---
|
||||
// 高通滤波器系数 (alpha)。alpha 越接近1，滤除低频(直流偏移)的效果越强，但可能滤掉真实的慢速运动。
|
||||
// alpha = RC / (RC + dt)，参考RC电路而来，fc ≈ (1 - alpha) / (2 * π * dt)
|
||||
#define HPF_ALPHA 0.999f
|
||||
//0.995f： 0.08 Hz 的信号
|
||||
//0.999f： 0.0159 Hz
|
||||
// --- --- ---
|
||||
|
||||
// --- 低通滤波器 ---
|
||||
// 低通滤波器系数 (alpha)。alpha 越小，滤波效果越强（更平滑），但延迟越大。
|
||||
// alpha 推荐范围 0.7 ~ 0.95。可以从 0.85 开始尝试。
|
||||
#define LPF_ALPHA 0.7f
|
||||
|
||||
// 加速度死区阈值 (m/s^2)。低于此阈值的加速度被认为是噪声，不参与积分。
|
||||
// 设得太高会忽略真实的慢速启动，设得太低则无法有效抑制噪声。
|
||||
//参考：0.2f ~ 0.4f
|
||||
#define ACC_DEAD_ZONE_THRESHOLD 0.05f
|
||||
|
||||
// --- 模拟摩擦力，进行速度衰减 ---
|
||||
#define SPEED_ATTENUATION 1.0f //暂不模拟
|
||||
BLE_KS_send_data_t KS_data;
|
||||
|
||||
#ifdef XTELL_TEST
|
||||
|
||||
debug_t debug1;
|
||||
debug_t debug2;
|
||||
#endif
|
||||
|
||||
static skiing_tracker_t my_skiing_tracker;
|
||||
//////////////////////////////////////////////////////////////////////////////////////////////////
|
||||
//实现
|
||||
|
||||
void clear_speed(void){
|
||||
my_skiing_tracker.state = STATIC;
|
||||
memset(my_skiing_tracker.velocity, 0, sizeof(my_skiing_tracker.velocity));
|
||||
my_skiing_tracker.speed = 0;
|
||||
}
|
||||
|
||||
void start_detection(void){
|
||||
my_skiing_tracker.state = STATIC;
|
||||
memset(my_skiing_tracker.velocity, 0, sizeof(my_skiing_tracker.velocity));
|
||||
my_skiing_tracker.distance = 0;
|
||||
my_skiing_tracker.speed = 0;
|
||||
}
|
||||
|
||||
void stop_detection(void){
|
||||
my_skiing_tracker.state = STOP_DETECTION;
|
||||
memset(my_skiing_tracker.velocity, 0, sizeof(my_skiing_tracker.velocity));
|
||||
my_skiing_tracker.speed = 0;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 初始化滑雪追踪器
|
||||
*
|
||||
* @param tracker
|
||||
*/
|
||||
void skiing_tracker_init(skiing_tracker_t *tracker)
|
||||
{
|
||||
if (!tracker) {
|
||||
return;
|
||||
}
|
||||
// 使用memset一次性清零整个结构体，包括新增的缓冲区
|
||||
memset(tracker, 0, sizeof(skiing_tracker_t));
|
||||
tracker->state = STATIC;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 当检测到落地时，计算空中的水平飞行距离并累加到总距离
|
||||
*/
|
||||
static void calculate_air_distance(skiing_tracker_t *tracker) {
|
||||
float horizontal_speed_on_takeoff = sqrtf(
|
||||
tracker->initial_velocity_on_takeoff[0] * tracker->initial_velocity_on_takeoff[0] +
|
||||
tracker->initial_velocity_on_takeoff[1] * tracker->initial_velocity_on_takeoff[1]
|
||||
);
|
||||
float distance_in_air = horizontal_speed_on_takeoff * tracker->time_in_air;
|
||||
tracker->distance += distance_in_air;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 在设备坐标系下，从原始加速度数据中移除重力分量
|
||||
* @param acc_device 输入：设备坐标系下的原始加速度 [x, y, z], 单位 m/s^2
|
||||
* @param angle 输入：姿态角 [pitch, roll, yaw]，单位: 度
|
||||
* @param acc_linear_device 输出：设备坐标系下移除重力后的线性加速度 [x, y, z]
|
||||
*/
|
||||
void remove_gravity_in_device_frame(const float *acc_device, const float *angle, float *acc_linear_device)
|
||||
{
|
||||
float pitch = angle[0] * DEG_TO_RAD; // 绕 Y 轴
|
||||
float roll = angle[1] * DEG_TO_RAD; // 绕 X 轴
|
||||
float yaw = angle[2] * DEG_TO_RAD; // 绕 Z 轴
|
||||
|
||||
float cp = cosf(pitch);
|
||||
float sp = sinf(pitch);
|
||||
float cr = cosf(roll);
|
||||
float sr = sinf(roll);
|
||||
float cy = cosf(yaw);
|
||||
float sy = sinf(yaw);
|
||||
|
||||
// 世界坐标系下的重力矢量
|
||||
const float g_world[3] = {0.0f, 0.0f, G_ACCELERATION};
|
||||
|
||||
// 计算旋转矩阵 R 的转置矩阵 R_transpose
|
||||
// R (Z-Y-X) =
|
||||
// [ cy*cp, cy*sp*sr - sy*cr, cy*sp*cr + sy*sr]
|
||||
// [ sy*cp, sy*sp*sr + cy*cr, sy*sp*cr - cy*sr]
|
||||
// [ -sp, cp*sr, cp*cr ]
|
||||
//
|
||||
// R_transpose =
|
||||
// [ cy*cp, sy*cp, -sp ]
|
||||
// [ cy*sp*sr - sy*cr, sy*sp*sr + cy*cr, cp*sr ]
|
||||
// [ cy*sp*cr + sy*sr, sy*sp*cr - cy*sr, cp*cr ]
|
||||
|
||||
// 计算重力在设备坐标系下的投影 G_device = R_transpose * G_world
|
||||
// 由于 G_world 只有 z 分量，计算可以简化
|
||||
float g_device[3];
|
||||
g_device[0] = (-sp) * g_world[2];
|
||||
g_device[1] = (cp * sr) * g_world[2];
|
||||
g_device[2] = (cp * cr) * g_world[2];
|
||||
|
||||
// 从原始设备加速度中减去重力投影
|
||||
acc_linear_device[0] = acc_device[0] - g_device[0];
|
||||
acc_linear_device[1] = acc_device[1] - g_device[1];
|
||||
acc_linear_device[2] = acc_device[2] - g_device[2];
|
||||
}
|
||||
|
||||
|
||||
|
||||
/**
|
||||
* @brief 计算缓冲区内三轴数据的方差之和
|
||||
*
|
||||
* @param buffer 传进来的三轴数据：陀螺仪/加速度
|
||||
* @return float 返回方差和
|
||||
*/
|
||||
static float calculate_variance(float buffer[VARIANCE_BUFFER_SIZE][3])
|
||||
{
|
||||
float mean[3] = {0};
|
||||
float variance[3] = {0};
|
||||
|
||||
// 计算均值
|
||||
for (int i = 0; i < VARIANCE_BUFFER_SIZE; i++) {
|
||||
mean[0] += buffer[i][0];
|
||||
mean[1] += buffer[i][1];
|
||||
mean[2] += buffer[i][2];
|
||||
}
|
||||
mean[0] /= VARIANCE_BUFFER_SIZE;
|
||||
mean[1] /= VARIANCE_BUFFER_SIZE;
|
||||
mean[2] /= VARIANCE_BUFFER_SIZE;
|
||||
|
||||
// 计算方差
|
||||
for (int i = 0; i < VARIANCE_BUFFER_SIZE; i++) {
|
||||
variance[0] += (buffer[i][0] - mean[0]) * (buffer[i][0] - mean[0]);
|
||||
variance[1] += (buffer[i][1] - mean[1]) * (buffer[i][1] - mean[1]);
|
||||
variance[2] += (buffer[i][2] - mean[2]) * (buffer[i][2] - mean[2]);
|
||||
}
|
||||
variance[0] /= VARIANCE_BUFFER_SIZE;
|
||||
variance[1] /= VARIANCE_BUFFER_SIZE;
|
||||
variance[2] /= VARIANCE_BUFFER_SIZE;
|
||||
|
||||
// 返回三轴方差之和，作为一个综合的稳定度指标
|
||||
return variance[0] + variance[1] + variance[2];
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 摩擦力模拟，进行速度衰减
|
||||
*
|
||||
* @param tracker
|
||||
*/
|
||||
void forece_of_friction(skiing_tracker_t *tracker){
|
||||
// 增加速度衰减，模拟摩擦力
|
||||
tracker->velocity[0] *= SPEED_ATTENUATION;
|
||||
tracker->velocity[1] *= SPEED_ATTENUATION;
|
||||
tracker->velocity[2] = 0; // 垂直速度强制归零
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 状态机更新
|
||||
*
|
||||
* @param tracker 传入同步修改后传出
|
||||
* @param acc_device_ms2 三轴加速度，m/s^2
|
||||
* @param gyr_dps 三轴陀螺仪，dps
|
||||
*/
|
||||
static void update_state_machine(skiing_tracker_t *tracker, const float *acc_device_ms2, const float *gyr_dps)
|
||||
{
|
||||
// 缓冲区未填满时，不进行状态判断，默认为静止
|
||||
if (!tracker->buffer_filled) {
|
||||
tracker->state = STATIC;
|
||||
return;
|
||||
}
|
||||
|
||||
// --- 计算关键指标 ---
|
||||
float acc_variance = calculate_variance(tracker->acc_buffer); // 计算加速度方差
|
||||
float gyr_variance = calculate_variance(tracker->gyr_buffer); // 计算陀螺仪方差
|
||||
float gyr_magnitude = sqrtf(gyr_dps[0]*gyr_dps[0] + gyr_dps[1]*gyr_dps[1] + gyr_dps[2]*gyr_dps[2]); //dps
|
||||
float acc_magnitude = sqrtf(acc_device_ms2[0]*acc_device_ms2[0] + acc_device_ms2[1]*acc_device_ms2[1] + acc_device_ms2[2]*acc_device_ms2[2]); //m/s^s
|
||||
float acc_magnitude_g = acc_magnitude / G_ACCELERATION; // 转换为g单位，用于跳跃判断
|
||||
|
||||
#ifdef XTELL_TEST
|
||||
debug1.acc_variance =acc_variance;
|
||||
debug1.gyr_variance =gyr_variance;
|
||||
debug1.gyr_magnitude=gyr_magnitude;
|
||||
debug1.acc_magnitude=fabsf(acc_magnitude - G_ACCELERATION);
|
||||
#endif
|
||||
|
||||
|
||||
|
||||
// --- 状态机逻辑 (核心修改区域) ---
|
||||
|
||||
#if 0 //暂时不考虑空中
|
||||
// 1. 空中/落地状态的后续处理
|
||||
if (tracker->state == IN_AIR) {
|
||||
// A. 检测巨大冲击 -> 落地
|
||||
if (acc_magnitude_g > LANDING_ACC_MAG_HIGH_THRESHOLD) {
|
||||
tracker->state = LANDING;
|
||||
// B. 检测超时 -> 强制落地 (安全机制)
|
||||
} else if (tracker->time_in_air > MAX_TIME_IN_AIR) {
|
||||
tracker->state = LANDING;
|
||||
// C. 检测恢复正常重力 (平缓落地)
|
||||
} else if (acc_magnitude_g > 0.8f && acc_magnitude_g < 1.5f) {
|
||||
tracker->grounded_entry_counter++;
|
||||
if (tracker->grounded_entry_counter >= GROUNDED_CONFIRM_COUNT) {
|
||||
tracker->state = LANDING;
|
||||
}
|
||||
} else {
|
||||
tracker->grounded_entry_counter = 0;
|
||||
}
|
||||
return; // 在空中或刚切换到落地，结束本次状态判断
|
||||
}
|
||||
|
||||
// 2. 严格的 "起跳->空中" 状态转换逻辑
|
||||
// 只有当处于滑行状态时，才去检测起跳意图
|
||||
if (tracker->state == NO_CONSTANT_SPEED || tracker->state == CONSTANT_SPEED || tracker->state == WHEEL) {
|
||||
if (acc_magnitude_g > TAKEOFF_ACC_MAG_HIGH_THRESHOLD) {
|
||||
tracker->state = TAKING_OFF;
|
||||
tracker->airborne_entry_counter = 0; // 准备检测失重
|
||||
return;
|
||||
}
|
||||
}
|
||||
// 只有在TAKING_OFF状态下，才去检测是否进入失重
|
||||
if (tracker->state == TAKING_OFF) {
|
||||
if (acc_magnitude_g < AIRBORNE_ACC_MAG_LOW_THRESHOLD) {
|
||||
tracker->airborne_entry_counter++;
|
||||
if (tracker->airborne_entry_counter >= AIRBORNE_CONFIRM_COUNT) {
|
||||
memcpy(tracker->initial_velocity_on_takeoff, tracker->velocity, sizeof(tracker->velocity));
|
||||
tracker->time_in_air = 0;
|
||||
tracker->state = IN_AIR;
|
||||
tracker->airborne_entry_counter = 0;
|
||||
tracker->grounded_entry_counter = 0;
|
||||
return;
|
||||
}
|
||||
} else {
|
||||
// 如果在起跳冲击后一段时间内没有失重，说明只是一个颠簸，恢复滑行
|
||||
// 可以加一个小的超时计数器，这里为了简单先直接恢复
|
||||
tracker->state = NO_CONSTANT_SPEED;
|
||||
}
|
||||
return; // 无论是否切换，都结束本次判断
|
||||
}
|
||||
#endif
|
||||
|
||||
|
||||
// --- 静止判断 ---
|
||||
if (acc_variance < STOP_ACC_VARIANCE_THRESHOLD && gyr_variance < STOP_GYR_VARIANCE_THRESHOLD && gyr_magnitude < STOP_GYR_MAG_THRESHOLD) {
|
||||
tracker->state = STATIC;
|
||||
return;
|
||||
}
|
||||
|
||||
// --- 地面状态切换逻辑 ---
|
||||
switch (tracker->state) {
|
||||
case LANDING:
|
||||
tracker->state = STATIC;
|
||||
break;
|
||||
case STATIC:
|
||||
// 优先判断是否进入 WOBBLE 状态
|
||||
// 条件：陀螺仪活动剧烈，但整体加速度变化不大（说明是原地转或晃）
|
||||
if (gyr_magnitude > ROTATION_GYR_MAG_THRESHOLD && fabsf(acc_magnitude - G_ACCELERATION) < WOBBLE_ACC_MAG_THRESHOLD) {
|
||||
tracker->state = WOBBLE;
|
||||
}
|
||||
// 只有在陀螺仪和加速度都满足“前进”特征时，才启动
|
||||
else if (gyr_variance > START_GYR_VARIANCE_THRESHOLD && fabsf(acc_magnitude - G_ACCELERATION) > START_ACC_MAG_THRESHOLD) {
|
||||
tracker->state = NO_CONSTANT_SPEED;
|
||||
}
|
||||
break;
|
||||
|
||||
case WOBBLE:
|
||||
// 从 WOBBLE 状态启动的条件应该和从 STATIC 启动一样严格
|
||||
if (gyr_variance > START_GYR_VARIANCE_THRESHOLD && fabsf(acc_magnitude - G_ACCELERATION) > START_ACC_MAG_THRESHOLD) {
|
||||
tracker->state = NO_CONSTANT_SPEED;
|
||||
}
|
||||
// 如果陀螺仪活动减弱，则可能恢复静止
|
||||
else if (gyr_magnitude < ROTATION_GYR_MAG_THRESHOLD * 0.8f) { // 增加迟滞，避免抖动
|
||||
// 不直接跳回STATIC，而是依赖下一轮的全局静止判断
|
||||
}
|
||||
break;
|
||||
case NO_CONSTANT_SPEED: //非匀速状态
|
||||
//暂时不考虑摔倒
|
||||
// if (gyr_magnitude > FALLEN_GRY_MAG_THRESHOLD) {
|
||||
// tracker->state = FALLEN; //摔倒
|
||||
// } else
|
||||
if (gyr_magnitude > WHEEL_GYR_MAG_THRESHOLD && acc_variance > WHEEL_ACC_VARIANCE_THRESHOLD) {
|
||||
tracker->state = WHEEL; //转弯
|
||||
} else if (fabsf(acc_magnitude - G_ACCELERATION) < SKIING_ACC_MAG_THRESHOLD) {
|
||||
tracker->state = CONSTANT_SPEED; //匀速
|
||||
}
|
||||
break;
|
||||
|
||||
case CONSTANT_SPEED: //匀速状态
|
||||
if (fabsf(acc_magnitude - G_ACCELERATION) > START_ACC_MAG_THRESHOLD) {
|
||||
tracker->state = NO_CONSTANT_SPEED;
|
||||
}
|
||||
//TODO：可以添加进入转弯或摔倒的判断
|
||||
break;
|
||||
|
||||
case WHEEL:
|
||||
// 从转弯状态，检查转弯是否结束
|
||||
// 如果角速度和加速度方差都降下来了，就回到普通滑行状态
|
||||
if (gyr_magnitude < WHEEL_GYR_MAG_THRESHOLD * 0.8f && acc_variance < WHEEL_ACC_VARIANCE_THRESHOLD * 0.8f) { // 乘以一个滞后系数避免抖动
|
||||
tracker->state = NO_CONSTANT_SPEED;
|
||||
}
|
||||
break;
|
||||
|
||||
case FALLEN:
|
||||
// TODO：回到 STATIC
|
||||
break;
|
||||
}
|
||||
|
||||
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* @brief 主更新函数
|
||||
*
|
||||
* @param tracker
|
||||
* @param acc_g 三轴加速度，g
|
||||
* @param gyr_dps 三轴陀螺仪，dps
|
||||
* @param angle 欧若拉角
|
||||
* @param dt 采样时间间隔，会用来积分求速度
|
||||
*/
|
||||
void skiing_tracker_update(skiing_tracker_t *tracker, float *acc_g, float *gyr_dps, float *angle, float dt)
|
||||
{
|
||||
if (!tracker || !acc_g || !gyr_dps || !angle || dt <= 0) {
|
||||
return;
|
||||
}
|
||||
if(my_skiing_tracker.state == STOP_DETECTION)
|
||||
return;
|
||||
|
||||
// --- 数据预处理和缓冲 ---
|
||||
float acc_device_ms2[3];
|
||||
acc_device_ms2[0] = acc_g[0] * G_ACCELERATION;
|
||||
acc_device_ms2[1] = acc_g[1] * G_ACCELERATION;
|
||||
acc_device_ms2[2] = acc_g[2] * G_ACCELERATION;
|
||||
|
||||
// 将最新数据存入缓冲区
|
||||
memcpy(tracker->acc_buffer[tracker->buffer_index], acc_device_ms2, sizeof(acc_device_ms2));
|
||||
memcpy(tracker->gyr_buffer[tracker->buffer_index], gyr_dps, 3 * sizeof(float));
|
||||
|
||||
tracker->buffer_index++;
|
||||
if (tracker->buffer_index >= VARIANCE_BUFFER_SIZE) {
|
||||
tracker->buffer_index = 0;
|
||||
tracker->buffer_filled = 1; // 标记缓冲区已满
|
||||
}
|
||||
|
||||
// --- 更新状态机 ---
|
||||
update_state_machine(tracker, acc_device_ms2, gyr_dps);
|
||||
|
||||
// --- 根据状态执行不同的计算逻辑 ---
|
||||
switch (tracker->state) {
|
||||
case TAKING_OFF:
|
||||
tracker->speed = 0.0f;
|
||||
break;
|
||||
case IN_AIR:
|
||||
// 在空中时，只累加滞空时间
|
||||
tracker->time_in_air += dt;
|
||||
break;
|
||||
case LANDING:
|
||||
// 刚落地，计算空中距离
|
||||
calculate_air_distance(tracker);
|
||||
// 清理速度和滤波器状态，为恢复地面追踪做准备
|
||||
memset(tracker->velocity, 0, sizeof(tracker->velocity));
|
||||
tracker->speed = 0;
|
||||
memset(tracker->acc_world_unfiltered_prev, 0, sizeof(tracker->acc_world_unfiltered_prev));
|
||||
memset(tracker->acc_world_filtered, 0, sizeof(tracker->acc_world_filtered));
|
||||
memset(tracker->acc_world_lpf, 0, sizeof(tracker->acc_world_lpf)); // 清理新增的LPF状态
|
||||
break;
|
||||
case WHEEL:
|
||||
case NO_CONSTANT_SPEED:
|
||||
remove_gravity_in_device_frame(acc_device_ms2, angle, tracker->acc_no_g);
|
||||
|
||||
|
||||
float acc_world_temp[3]; // 临时变量存储当前周期的加速度
|
||||
for (int i = 0; i < 2; i++) { // 只处理水平方向的 x 和 y 轴
|
||||
|
||||
// --- 核心修改：颠倒滤波器顺序为 HPF -> LPF ---
|
||||
|
||||
// 1. 高通滤波 (HPF) 先行: 消除因姿态误差导致的重力泄漏（直流偏置）
|
||||
// HPF的瞬态响应会产生尖峰，这是正常的。
|
||||
tracker->acc_world_filtered[i] = HPF_ALPHA * (tracker->acc_world_filtered[i] + tracker->acc_no_g[i] - tracker->acc_world_unfiltered_prev[i]);
|
||||
tracker->acc_world_unfiltered_prev[i] = tracker->acc_no_g[i];
|
||||
|
||||
// 2. 低通滤波 (LPF) 殿后: 平滑掉HPF产生的尖峰和传感器自身的高频振动噪声。
|
||||
// 这里使用 tracker->acc_world_filtered[i] 作为LPF的输入。
|
||||
tracker->acc_world_lpf[i] = (1.0f - LPF_ALPHA) * tracker->acc_world_filtered[i] + LPF_ALPHA * tracker->acc_world_lpf[i];
|
||||
|
||||
// 将最终处理完的加速度值存入临时变量
|
||||
acc_world_temp[i] = tracker->acc_world_lpf[i];
|
||||
}
|
||||
|
||||
// 计算处理后加速度的水平模长
|
||||
float acc_horizontal_mag = sqrtf(acc_world_temp[0] * acc_world_temp[0] +
|
||||
acc_world_temp[1] * acc_world_temp[1]);
|
||||
#if XTELL_TEST
|
||||
debug2.acc_magnitude = acc_horizontal_mag;
|
||||
#endif
|
||||
// 应用死区，并积分
|
||||
if (acc_horizontal_mag > ACC_DEAD_ZONE_THRESHOLD) {
|
||||
tracker->velocity[0] += acc_world_temp[0] * dt;
|
||||
tracker->velocity[1] += acc_world_temp[1] * dt;
|
||||
}
|
||||
|
||||
// 更新速度和距离
|
||||
tracker->speed = sqrtf(tracker->velocity[0] * tracker->velocity[0] +
|
||||
tracker->velocity[1] * tracker->velocity[1]);
|
||||
tracker->distance += tracker->speed * dt;
|
||||
break;
|
||||
case CONSTANT_SPEED:
|
||||
//保持上次的速度不变。只更新距离
|
||||
tracker->distance += tracker->speed * dt;
|
||||
break;
|
||||
case STATIC:
|
||||
case WOBBLE:
|
||||
// 速度清零，抑制漂移
|
||||
memset(tracker->velocity, 0, sizeof(tracker->velocity));
|
||||
tracker->speed = 0.0f;
|
||||
memset(tracker->acc_world_unfiltered_prev, 0, sizeof(tracker->acc_world_unfiltered_prev));
|
||||
memset(tracker->acc_world_filtered, 0, sizeof(tracker->acc_world_filtered));
|
||||
memset(tracker->acc_world_lpf, 0, sizeof(tracker->acc_world_lpf)); // 清理新增的LPF状态
|
||||
#if XTELL_TEST
|
||||
debug2.acc_magnitude = 0;
|
||||
#endif
|
||||
break;
|
||||
case FALLEN:
|
||||
// TODO
|
||||
break;
|
||||
default:
|
||||
break;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
/**
|
||||
* @brief 滑雪数据计算
|
||||
*
|
||||
* @param acc_data_buf 传入的三轴加速度数据
|
||||
* @param gyr_data_buf 传入的三轴陀螺仪数据
|
||||
* @param angle_data 传入的欧若拉角数据
|
||||
* @return BLE_send_data_t 要发送给蓝牙的数据
|
||||
*/
|
||||
BLE_send_data_t sensor_processing_task(signed short* acc_data_buf, signed short* gyr_data_buf, float* angle_data) {
|
||||
|
||||
static int initialized = 0;
|
||||
static float acc_data_g[3];
|
||||
static float gyr_data_dps[3];
|
||||
|
||||
// const float delta_time = DELTA_TIME+0.01f;
|
||||
// const float delta_time = DELTA_TIME + 0.005f;
|
||||
const float delta_time = DELTA_TIME;
|
||||
BLE_send_data_t BLE_send_data;
|
||||
|
||||
if (!initialized) {
|
||||
skiing_tracker_init(&my_skiing_tracker);
|
||||
initialized = 1;
|
||||
printf("Skiing Tracker Initialized. Waiting for sensor calibration...\n");
|
||||
}
|
||||
|
||||
|
||||
#if ACC_RANGE==2
|
||||
// 加速度 LSB to g
|
||||
acc_data_g[0] = (float)acc_data_buf[0] / 16384.0f;
|
||||
acc_data_g[1] = (float)acc_data_buf[1] / 16384.0f;
|
||||
acc_data_g[2] = (float)acc_data_buf[2] / 16384.0f;
|
||||
#endif
|
||||
|
||||
#if ACC_RANGE==4
|
||||
// 加速度 LSB to g
|
||||
acc_data_g[0] = (float)acc_data_buf[0] / 8192.0f;
|
||||
acc_data_g[1] = (float)acc_data_buf[1] / 8192.0f;
|
||||
acc_data_g[2] = (float)acc_data_buf[2] / 8192.0f;
|
||||
#endif
|
||||
|
||||
#if ACC_RANGE==8
|
||||
//±8g 4096
|
||||
acc_data_g[0] = (float)acc_data_buf[0] / 4096.0f; //ax
|
||||
acc_data_g[1] = (float)acc_data_buf[1] / 4096.0f; //ay
|
||||
acc_data_g[2] = (float)acc_data_buf[2] / 4096.0f; //az
|
||||
#endif
|
||||
|
||||
#if ACC_RANGE==16
|
||||
//±16g 2048
|
||||
acc_data_g[0] = (float)acc_data_buf[0] / 2048.0f; //ax
|
||||
acc_data_g[1] = (float)acc_data_buf[1] / 2048.0f; //ay
|
||||
acc_data_g[2] = (float)acc_data_buf[2] / 2048.0f; //az
|
||||
#endif
|
||||
|
||||
// 陀螺仪 LSB to dps (度/秒)
|
||||
// ±2000dps量程下，转换系数约为 0.061
|
||||
gyr_data_dps[0] = (float)gyr_data_buf[0] * 0.061f;
|
||||
gyr_data_dps[1] = (float)gyr_data_buf[1] * 0.061f;
|
||||
gyr_data_dps[2] = (float)gyr_data_buf[2] * 0.061f;
|
||||
|
||||
skiing_tracker_update(&my_skiing_tracker, acc_data_g, gyr_data_dps, angle_data, delta_time);
|
||||
|
||||
BLE_send_data.skiing_state = my_skiing_tracker.state;
|
||||
for (int i = 0; i < 3; i++) {
|
||||
#ifdef XTELL_TEST
|
||||
BLE_send_data.acc_data[i] = (short)(acc_data_g[i] * 9.8f) * 100; //cm/^s2
|
||||
BLE_send_data.gyr_data[i] = (short)gyr_data_dps[i]; //dps
|
||||
BLE_send_data.angle_data[i] = angle_data[i];
|
||||
#else
|
||||
BLE_send_data.acc_data[i] = (short)acc_data_buf[i]; //原始adc数据
|
||||
BLE_send_data.gyr_data[i] = (short)gyr_data_buf[i]; //原始adc数据
|
||||
BLE_send_data.angle_data[i] = angle_data[i];
|
||||
#endif
|
||||
}
|
||||
BLE_send_data.speed_cms = (int)(my_skiing_tracker.speed * 100);
|
||||
BLE_send_data.distance_cm = (int)(my_skiing_tracker.distance * 100);
|
||||
// printf("Calculate the time interval =============== end\n");
|
||||
|
||||
return BLE_send_data;
|
||||
}
|
||||
|
||||
@ -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
|
||||
@ -1,377 +0,0 @@
|
||||
/*
|
||||
使用四元数求角度和去掉重力分量
|
||||
*/
|
||||
#include "skiing_tracker.h"
|
||||
#include "../sensor/SC7U22.h"
|
||||
#include <math.h>
|
||||
#include <string.h>
|
||||
|
||||
#define ENABLE_XLOG 1
|
||||
#ifdef xlog
|
||||
#undef xlog
|
||||
#endif
|
||||
#if ENABLE_XLOG
|
||||
#define xlog(format, ...) printf("[XT:%s] " format, __func__, ##__VA_ARGS__)
|
||||
#else
|
||||
#define xlog(format, ...) ((void)0)
|
||||
#endif
|
||||
|
||||
|
||||
#define G_ACCELERATION 9.81f
|
||||
#define DEG_TO_RAD (3.14159265f / 180.0f)
|
||||
|
||||
BLE_KS_send_data_t KS_data;
|
||||
static float quaternion_data[4];
|
||||
|
||||
#ifdef XTELL_TEST
|
||||
|
||||
debug_t debug1;
|
||||
debug_t debug2;
|
||||
#endif
|
||||
|
||||
static skiing_tracker_t my_skiing_tracker;
|
||||
//////////////////////////////////////////////////////////////////////////////////////////////////
|
||||
//实现
|
||||
|
||||
void clear_speed(void){
|
||||
my_skiing_tracker.state = STATIC;
|
||||
memset(my_skiing_tracker.velocity, 0, sizeof(my_skiing_tracker.velocity));
|
||||
my_skiing_tracker.speed = 0;
|
||||
}
|
||||
|
||||
void start_detection(void){
|
||||
my_skiing_tracker.state = STATIC;
|
||||
memset(my_skiing_tracker.velocity, 0, sizeof(my_skiing_tracker.velocity));
|
||||
my_skiing_tracker.distance = 0;
|
||||
my_skiing_tracker.speed = 0;
|
||||
}
|
||||
|
||||
void stop_detection(void){
|
||||
my_skiing_tracker.state = STOP_DETECTION;
|
||||
memset(my_skiing_tracker.velocity, 0, sizeof(my_skiing_tracker.velocity));
|
||||
my_skiing_tracker.speed = 0;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 初始化滑雪追踪器
|
||||
*
|
||||
* @param tracker
|
||||
*/
|
||||
void skiing_tracker_init(skiing_tracker_t *tracker)
|
||||
{
|
||||
if (!tracker) {
|
||||
return;
|
||||
}
|
||||
// 使用memset一次性清零整个结构体，包括新增的缓冲区
|
||||
memset(tracker, 0, sizeof(skiing_tracker_t));
|
||||
tracker->state = STATIC;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 当检测到落地时，计算空中的水平飞行距离并累加到总距离
|
||||
*/
|
||||
static void calculate_air_distance(skiing_tracker_t *tracker) {
|
||||
float horizontal_speed_on_takeoff = sqrtf(
|
||||
tracker->initial_velocity_on_takeoff[0] * tracker->initial_velocity_on_takeoff[0] +
|
||||
tracker->initial_velocity_on_takeoff[1] * tracker->initial_velocity_on_takeoff[1]
|
||||
);
|
||||
float distance_in_air = horizontal_speed_on_takeoff * tracker->time_in_air;
|
||||
tracker->distance += distance_in_air;
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* @brief 将设备坐标系下的加速度转换为世界坐标系，去掉重力分量
|
||||
*
|
||||
* @param acc_device
|
||||
* @param angle
|
||||
* @param acc_linear_world
|
||||
*/
|
||||
static void transform_acc_to_world_frame(const float *acc_device, const float *angle, float *acc_linear_world)
|
||||
{
|
||||
// 1. 将输入的角度从度转换为弧度
|
||||
// angle[0] -> pitch, angle[1] -> roll, angle[2] -> yaw
|
||||
float pitch_rad = -angle[0] * (M_PI / 180.0f);
|
||||
float roll_rad = -angle[1] * (M_PI / 180.0f);
|
||||
float yaw_rad = -angle[2] * (M_PI / 180.0f);
|
||||
|
||||
// 2. 预先计算三角函数值，以提高效率
|
||||
float c_r = cosf(roll_rad);
|
||||
float s_r = sinf(roll_rad);
|
||||
float c_p = cosf(pitch_rad);
|
||||
float s_p = sinf(pitch_rad);
|
||||
float c_y = cosf(yaw_rad);
|
||||
float s_y = sinf(yaw_rad);
|
||||
|
||||
// 3. 构建从设备坐标系到世界坐标系的旋转矩阵 R_device_to_world
|
||||
// 该矩阵基于 Z-Y-X (Yaw-Pitch-Roll) 欧拉角顺序
|
||||
// R = R_z(yaw) * R_y(pitch) * R_x(roll)
|
||||
float R[3][3];
|
||||
R[0][0] = c_y * c_p;
|
||||
R[0][1] = c_y * s_p * s_r - s_y * c_r;
|
||||
R[0][2] = c_y * s_p * c_r + s_y * s_r;
|
||||
|
||||
R[1][0] = s_y * c_p;
|
||||
R[1][1] = s_y * s_p * s_r + c_y * c_r;
|
||||
R[1][2] = s_y * s_p * c_r - c_y * s_r;
|
||||
|
||||
R[2][0] = -s_p;
|
||||
R[2][1] = c_p * s_r;
|
||||
R[2][2] = c_p * c_r;
|
||||
|
||||
// 4. 将设备坐标系的加速度计总读数旋转到世界坐标系
|
||||
// a_raw_world = R * acc_device
|
||||
float ax_raw_world = R[0][0] * acc_device[0] + R[0][1] * acc_device[1] + R[0][2] * acc_device[2];
|
||||
float ay_raw_world = R[1][0] * acc_device[0] + R[1][1] * acc_device[1] + R[1][2] * acc_device[2];
|
||||
float az_raw_world = R[2][0] * acc_device[0] + R[2][1] * acc_device[1] + R[2][2] * acc_device[2];
|
||||
|
||||
// 5. 在世界坐标系中减去重力分量，得到线性加速度
|
||||
// 假设世界坐标系Z轴垂直向上，重力矢量为 [0, 0, -g]
|
||||
// 线性加速度 = 总加速度 - 重力加速度
|
||||
// az_linear = az_raw_world - (-g) = az_raw_world + g (如果Z轴向上)
|
||||
// az_linear = az_raw_world - (+g) = az_raw_world - g (如果Z轴向下)
|
||||
// 这里我们采用 Z 轴向上的标准惯性系 (ENU)
|
||||
|
||||
acc_linear_world[0] = ax_raw_world;
|
||||
acc_linear_world[1] = ay_raw_world;
|
||||
acc_linear_world[2] = az_raw_world - G_ACCELERATION; // Z轴向上，重力为正值，所以减去
|
||||
}
|
||||
/**
|
||||
* @brief 在设备坐标系下，从原始加速度数据中移除重力分量
|
||||
* @param acc_device 输入：设备坐标系下的原始加速度 [x, y, z], 单位 m/s^2
|
||||
* @param angle 输入：姿态角 [pitch, roll, yaw]，单位: 度
|
||||
* @param acc_linear_device 输出：设备坐标系下移除重力后的线性加速度 [x, y, z]
|
||||
*/
|
||||
void remove_gravity_in_device_frame(const float *acc_device, const float *angle, float *acc_linear_device)
|
||||
{
|
||||
float pitch = -angle[0] * DEG_TO_RAD; // 绕 Y 轴
|
||||
float roll = -angle[1] * DEG_TO_RAD; // 绕 X 轴
|
||||
float yaw = -angle[2] * DEG_TO_RAD; // 绕 Z 轴
|
||||
|
||||
float cp = cosf(pitch);
|
||||
float sp = sinf(pitch);
|
||||
float cr = cosf(roll);
|
||||
float sr = sinf(roll);
|
||||
float cy = cosf(yaw);
|
||||
float sy = sinf(yaw);
|
||||
|
||||
// 世界坐标系下的重力矢量
|
||||
const float g_world[3] = {0.0f, 0.0f, G_ACCELERATION};
|
||||
|
||||
// 计算旋转矩阵 R 的转置矩阵 R_transpose
|
||||
// R (Z-Y-X) =
|
||||
// [ cy*cp, cy*sp*sr - sy*cr, cy*sp*cr + sy*sr]
|
||||
// [ sy*cp, sy*sp*sr + cy*cr, sy*sp*cr - cy*sr]
|
||||
// [ -sp, cp*sr, cp*cr ]
|
||||
//
|
||||
// R_transpose =
|
||||
// [ cy*cp, sy*cp, -sp ]
|
||||
// [ cy*sp*sr - sy*cr, sy*sp*sr + cy*cr, cp*sr ]
|
||||
// [ cy*sp*cr + sy*sr, sy*sp*cr - cy*sr, cp*cr ]
|
||||
|
||||
// 计算重力在设备坐标系下的投影 G_device = R_transpose * G_world
|
||||
// 由于 G_world 只有 z 分量，计算可以简化
|
||||
float g_device[3];
|
||||
g_device[0] = (-sp) * g_world[2];
|
||||
g_device[1] = (cp * sr) * g_world[2];
|
||||
g_device[2] = (cp * cr) * g_world[2];
|
||||
|
||||
// 从原始设备加速度中减去重力投影
|
||||
acc_linear_device[0] = acc_device[0] - g_device[0];
|
||||
acc_linear_device[1] = acc_device[1] - g_device[1];
|
||||
acc_linear_device[2] = acc_device[2] - g_device[2];
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* @brief 使用四元数直接从设备坐标系的加速度中移除重力分量
|
||||
* @details 这种方法比使用欧拉角更精确、更稳定，且避免了万向节死锁。
|
||||
* @param acc_device 输入：设备坐标系下的原始加速度 [x, y, z], 单位 m/s^2
|
||||
* @param q 输入：表示姿态的四元数 [w, x, y, z]
|
||||
* @param acc_linear_device 输出：设备坐标系下移除重力后的线性加速度 [x, y, z]
|
||||
*/
|
||||
void q_remove_gravity_with_quaternion(const float *acc_device, const float *q, float *acc_linear_device)
|
||||
{
|
||||
// 从四元数计算重力在设备坐标系下的投影
|
||||
// G_device = R_transpose * G_world
|
||||
// G_world = [0, 0, g]
|
||||
// R_transpose 的第三列即为重力投影方向
|
||||
float gx = 2.0f * (q[1] * q[3] - q[0] * q[2]);
|
||||
float gy = 2.0f * (q[0] * q[1] + q[2] * q[3]);
|
||||
float gz = q[0] * q[0] - q[1] * q[1] - q[2] * q[2] + q[3] * q[3];
|
||||
|
||||
// 从原始加速度中减去重力分量
|
||||
acc_linear_device[0] = acc_device[0] - gx * G_ACCELERATION;
|
||||
acc_linear_device[1] = acc_device[1] - gy * G_ACCELERATION;
|
||||
acc_linear_device[2] = acc_device[2] - gz * G_ACCELERATION;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 使用四元数将设备坐标系的线性加速度转换到世界坐标系,并且移除重力分量
|
||||
* @details 同样，此方法比使用欧拉角更优。
|
||||
* @param acc_linear_device 输入：设备坐标系下的线性加速度 [x, y, z]
|
||||
* @param q 输入：表示姿态的四元数 [w, x, y, z]
|
||||
* @param acc_linear_world 输出：世界坐标系下的线性加速度 [x, y, z]
|
||||
*/
|
||||
void q_transform_to_world_with_quaternion(const float *acc_linear_device, const float *q, float *acc_linear_world)
|
||||
{
|
||||
// 这是 R_device_to_world * acc_linear_device 的展开形式
|
||||
acc_linear_world[0] = (1.0f - 2.0f*q[2]*q[2] - 2.0f*q[3]*q[3]) * acc_linear_device[0] +
|
||||
(2.0f*q[1]*q[2] - 2.0f*q[0]*q[3]) * acc_linear_device[1] +
|
||||
(2.0f*q[1]*q[3] + 2.0f*q[0]*q[2]) * acc_linear_device[2];
|
||||
|
||||
acc_linear_world[1] = (2.0f*q[1]*q[2] + 2.0f*q[0]*q[3]) * acc_linear_device[0] +
|
||||
(1.0f - 2.0f*q[1]*q[1] - 2.0f*q[3]*q[3]) * acc_linear_device[1] +
|
||||
(2.0f*q[2]*q[3] - 2.0f*q[0]*q[1]) * acc_linear_device[2];
|
||||
|
||||
acc_linear_world[2] = (2.0f*q[1]*q[3] - 2.0f*q[0]*q[2]) * acc_linear_device[0] +
|
||||
(2.0f*q[2]*q[3] + 2.0f*q[0]*q[1]) * acc_linear_device[1] +
|
||||
(1.0f - 2.0f*q[1]*q[1] - 2.0f*q[2]*q[2]) * acc_linear_device[2];
|
||||
acc_linear_world[2] -= G_ACCELERATION;
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* @brief 主更新函数
|
||||
*
|
||||
* @param tracker
|
||||
* @param acc_g 三轴加速度，g
|
||||
* @param gyr_dps 三轴陀螺仪，dps
|
||||
* @param angle 欧若拉角
|
||||
* @param dt 采样时间间隔，会用来积分求速度
|
||||
*/
|
||||
void skiing_tracker_update(skiing_tracker_t *tracker, float *acc_g, float *gyr_dps, float *angle, float dt)
|
||||
{
|
||||
if (!tracker || !acc_g || !gyr_dps || !angle || dt <= 0) {
|
||||
return;
|
||||
}
|
||||
if(my_skiing_tracker.state == STOP_DETECTION)
|
||||
return;
|
||||
|
||||
// --- 数据预处理和缓冲 ---
|
||||
float acc_device_ms2[3];
|
||||
acc_device_ms2[0] = acc_g[0] * G_ACCELERATION;
|
||||
acc_device_ms2[1] = acc_g[1] * G_ACCELERATION;
|
||||
acc_device_ms2[2] = acc_g[2] * G_ACCELERATION;
|
||||
|
||||
#if 1 //测试禁止状态下陀螺仪的三轴加速度，去掉重力分量后是否正常
|
||||
float tmp_device_acc[3];
|
||||
float tmp_world_acc[3];
|
||||
// remove_gravity_in_device_frame(acc_device_ms2,angle,tmp_device_acc);
|
||||
// transform_acc_to_world_frame(acc_device_ms2,angle,tmp_world_acc);
|
||||
|
||||
q_remove_gravity_with_quaternion(acc_device_ms2,quaternion_data,tmp_device_acc);
|
||||
q_transform_to_world_with_quaternion(acc_device_ms2,quaternion_data,tmp_world_acc);
|
||||
|
||||
// 计算处理后加速度的水平模长
|
||||
float all_device_mag = sqrtf(tmp_device_acc[0] * tmp_device_acc[0] +
|
||||
tmp_device_acc[1] * tmp_device_acc[1] +
|
||||
tmp_device_acc[2] * tmp_device_acc[2]);
|
||||
|
||||
float all_world_mag = sqrtf(tmp_world_acc[0] * tmp_world_acc[0] +
|
||||
tmp_world_acc[1] * tmp_world_acc[1] +
|
||||
tmp_world_acc[2] * tmp_world_acc[2]);
|
||||
|
||||
|
||||
static int count = 0;
|
||||
if(count > 100){
|
||||
xlog("===original(g): x %.2f, y %.2f, z %.2f===\n",acc_g[0],acc_g[1],acc_g[2]);
|
||||
xlog("===device(m/s^2) no g: x %.2f, y %.2f, z %.2f, all %.2f===\n",tmp_device_acc[0],tmp_device_acc[1],tmp_device_acc[2],all_device_mag); //去掉重力加速度
|
||||
xlog("===world(m/s^2) no g: x %.2f, y %.2f, z %.2f, all %.2f===\n",tmp_world_acc[0],tmp_world_acc[1],tmp_world_acc[2],all_world_mag); //去掉重力加速度
|
||||
xlog("===gyr(dps) : x %.2f, y %.2f, z %.2f, all %.2f===\n",gyr_dps[0],gyr_dps[1],gyr_dps[2]); //angle
|
||||
xlog("===angle : x %.2f, y %.2f, z %.2f,===\n",angle[0],angle[1],angle[2]);
|
||||
count = 0;
|
||||
|
||||
}
|
||||
count++;
|
||||
|
||||
#endif
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
/**
|
||||
* @brief 滑雪数据计算
|
||||
*
|
||||
* @param acc_data_buf 传入的三轴加速度数据
|
||||
* @param gyr_data_buf 传入的三轴陀螺仪数据
|
||||
* @param angle_data 传入的欧若拉角数据
|
||||
* @return BLE_send_data_t 要发送给蓝牙的数据
|
||||
*/
|
||||
BLE_send_data_t sensor_processing_task(signed short* acc_data_buf, signed short* gyr_data_buf, float* angle_data, float* quaternion) {
|
||||
|
||||
static int initialized = 0;
|
||||
static float acc_data_g[3];
|
||||
static float gyr_data_dps[3];
|
||||
if(quaternion != NULL){
|
||||
memcpy(quaternion_data, quaternion, 4 * sizeof(float));
|
||||
}
|
||||
|
||||
// const float delta_time = DELTA_TIME+0.01f;
|
||||
// const float delta_time = DELTA_TIME + 0.005f;
|
||||
const float delta_time = DELTA_TIME;
|
||||
BLE_send_data_t BLE_send_data;
|
||||
|
||||
if (!initialized) {
|
||||
skiing_tracker_init(&my_skiing_tracker);
|
||||
initialized = 1;
|
||||
printf("Skiing Tracker Initialized. Waiting for sensor calibration...\n");
|
||||
}
|
||||
|
||||
|
||||
#if ACC_RANGE==2
|
||||
// 加速度 LSB to g
|
||||
acc_data_g[0] = (float)acc_data_buf[0] / 16384.0f;
|
||||
acc_data_g[1] = (float)acc_data_buf[1] / 16384.0f;
|
||||
acc_data_g[2] = (float)acc_data_buf[2] / 16384.0f;
|
||||
#endif
|
||||
|
||||
#if ACC_RANGE==4
|
||||
// 加速度 LSB to g
|
||||
acc_data_g[0] = (float)acc_data_buf[0] / 8192.0f;
|
||||
acc_data_g[1] = (float)acc_data_buf[1] / 8192.0f;
|
||||
acc_data_g[2] = (float)acc_data_buf[2] / 8192.0f;
|
||||
#endif
|
||||
|
||||
#if ACC_RANGE==8
|
||||
//±8g 4096
|
||||
acc_data_g[0] = (float)acc_data_buf[0] / 4096.0f; //ax
|
||||
acc_data_g[1] = (float)acc_data_buf[1] / 4096.0f; //ay
|
||||
acc_data_g[2] = (float)acc_data_buf[2] / 4096.0f; //az
|
||||
#endif
|
||||
|
||||
#if ACC_RANGE==16
|
||||
//±16g 2048
|
||||
acc_data_g[0] = (float)acc_data_buf[0] / 2048.0f; //ax
|
||||
acc_data_g[1] = (float)acc_data_buf[1] / 2048.0f; //ay
|
||||
acc_data_g[2] = (float)acc_data_buf[2] / 2048.0f; //az
|
||||
#endif
|
||||
|
||||
// 陀螺仪 LSB to dps (度/秒)
|
||||
// ±2000dps量程下，转换系数约为 0.061
|
||||
gyr_data_dps[0] = (float)gyr_data_buf[0] * 0.061f;
|
||||
gyr_data_dps[1] = (float)gyr_data_buf[1] * 0.061f;
|
||||
gyr_data_dps[2] = (float)gyr_data_buf[2] * 0.061f;
|
||||
|
||||
skiing_tracker_update(&my_skiing_tracker, acc_data_g, gyr_data_dps, angle_data, delta_time);
|
||||
|
||||
// BLE_send_data.skiing_state = my_skiing_tracker.state;
|
||||
// for (int i = 0; i < 3; i++) {
|
||||
// #ifdef XTELL_TEST
|
||||
// BLE_send_data.acc_data[i] = (short)(acc_data_g[i] * 9.8f) * 100; //cm/^s2
|
||||
// BLE_send_data.gyr_data[i] = (short)gyr_data_dps[i]; //dps
|
||||
// BLE_send_data.angle_data[i] = angle_data[i];
|
||||
// #else
|
||||
// BLE_send_data.acc_data[i] = (short)acc_data_buf[i]; //原始adc数据
|
||||
// BLE_send_data.gyr_data[i] = (short)gyr_data_buf[i]; //原始adc数据
|
||||
// BLE_send_data.angle_data[i] = angle_data[i];
|
||||
// #endif
|
||||
// }
|
||||
// BLE_send_data.speed_cms = (int)(my_skiing_tracker.speed * 100);
|
||||
// BLE_send_data.distance_cm = (int)(my_skiing_tracker.distance * 100);
|
||||
// // printf("Calculate the time interval =============== end\n");
|
||||
|
||||
return BLE_send_data;
|
||||
}
|
||||
|
||||
@ -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
|
||||
@ -1,259 +0,0 @@
|
||||
/*
|
||||
动态ZUPT+卡尔曼
|
||||
*/
|
||||
#include "skiing_tracker.h"
|
||||
#include "../sensor/SC7U22.h"
|
||||
#include <math.h>
|
||||
#include <string.h>
|
||||
|
||||
#define G_ACCELERATION 9.81f
|
||||
#define DEG_TO_RAD (3.14159265f / 180.0f)
|
||||
|
||||
// --- 算法阈值定义 ---
|
||||
// 动态零速更新(ZUPT)阈值：加速度方差小于此值，认为处于动态稳定状态
|
||||
#define ZUPT_ACC_VARIANCE_THRESHOLD 0.05f
|
||||
// 旋转检测阈值：Z轴角速度大于此值(度/秒)，认为正在原地旋转
|
||||
#define ROTATION_GYR_Z_THRESHOLD 60.0f
|
||||
// 启动滑雪阈值：加速度模长与重力的差值大于此值，认为开始运动
|
||||
#define START_SKIING_ACC_THRESHOLD 1.5f
|
||||
|
||||
/**
|
||||
* @brief 初始化滑雪追踪器
|
||||
*/
|
||||
void skiing_tracker_init(skiing_tracker_t *tracker)
|
||||
{
|
||||
if (!tracker) {
|
||||
return;
|
||||
}
|
||||
// 使用memset一次性清零整个结构体，包括新增的缓冲区
|
||||
memset(tracker, 0, sizeof(skiing_tracker_t));
|
||||
tracker->state = SKIING_STATE_STATIC;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 将设备坐标系下的加速度转换为世界坐标系
|
||||
* @param acc_device 设备坐标系下的加速度 [x, y, z]
|
||||
* @param angle 姿态角 [pitch, roll, yaw]，单位: 度
|
||||
* @param acc_world 输出：世界坐标系下的加速度 [x, y, z]
|
||||
*/
|
||||
static void transform_acc_to_world_frame(const float *acc_device, const float *angle, float *acc_world)
|
||||
{
|
||||
// 驱动输出的角度与标准航空定义相反，需要取反才能用于标准旋转矩阵。
|
||||
float pitch = -angle[0] * DEG_TO_RAD;
|
||||
float roll = -angle[1] * DEG_TO_RAD;
|
||||
|
||||
float cp = cosf(pitch);
|
||||
float sp = sinf(pitch);
|
||||
float cr = cosf(roll);
|
||||
float sr = sinf(roll);
|
||||
|
||||
float ax = acc_device[0];
|
||||
float ay = acc_device[1];
|
||||
float az = acc_device[2];
|
||||
|
||||
// 使用经过验证的、正确的身体坐标系到世界坐标系的旋转矩阵 (基于 Y-X 旋转顺序)
|
||||
// 这个矩阵将设备测量的加速度(ax, ay, az)正确地转换到世界坐标系(acc_world)。
|
||||
acc_world[0] = cp * ax + sp * sr * ay + sp * cr * az;
|
||||
acc_world[1] = 0 * ax + cr * ay - sr * az;
|
||||
acc_world[2] = -sp * ax + cp * sr * ay + cp * cr * az;
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* @brief 计算缓冲区内三轴数据的方差之和
|
||||
*/
|
||||
static float calculate_variance(float buffer[VARIANCE_BUFFER_SIZE][3])
|
||||
{
|
||||
float mean[3] = {0};
|
||||
float variance[3] = {0};
|
||||
|
||||
// 1. 计算均值
|
||||
for (int i = 0; i < VARIANCE_BUFFER_SIZE; i++) {
|
||||
mean[0] += buffer[i][0];
|
||||
mean[1] += buffer[i][1];
|
||||
mean[2] += buffer[i][2];
|
||||
}
|
||||
mean[0] /= VARIANCE_BUFFER_SIZE;
|
||||
mean[1] /= VARIANCE_BUFFER_SIZE;
|
||||
mean[2] /= VARIANCE_BUFFER_SIZE;
|
||||
|
||||
// 2. 计算方差
|
||||
for (int i = 0; i < VARIANCE_BUFFER_SIZE; i++) {
|
||||
variance[0] += (buffer[i][0] - mean[0]) * (buffer[i][0] - mean[0]);
|
||||
variance[1] += (buffer[i][1] - mean[1]) * (buffer[i][1] - mean[1]);
|
||||
variance[2] += (buffer[i][2] - mean[2]) * (buffer[i][2] - mean[2]);
|
||||
}
|
||||
variance[0] /= VARIANCE_BUFFER_SIZE;
|
||||
variance[1] /= VARIANCE_BUFFER_SIZE;
|
||||
variance[2] /= VARIANCE_BUFFER_SIZE;
|
||||
|
||||
// 返回三轴方差之和，作为一个综合的稳定度指标
|
||||
return variance[0] + variance[1] + variance[2];
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* @brief 升级后的状态机，包含旋转检测和动态零速更新
|
||||
*/
|
||||
static void update_state_machine(skiing_tracker_t *tracker, const float *acc_device_ms2, const float *gyr_dps)
|
||||
{
|
||||
// 缓冲区未填满时，不进行状态判断，默认为静止
|
||||
if (!tracker->buffer_filled) {
|
||||
tracker->state = SKIING_STATE_STATIC;
|
||||
return;
|
||||
}
|
||||
|
||||
// --- 计算关键指标 ---
|
||||
float acc_variance = calculate_variance(tracker->acc_buffer);
|
||||
float gyr_z_abs = fabsf(gyr_dps[2]);
|
||||
float acc_magnitude = sqrtf(acc_device_ms2[0]*acc_device_ms2[0] + acc_device_ms2[1]*acc_device_ms2[1] + acc_device_ms2[2]*acc_device_ms2[2]);
|
||||
|
||||
// --- 状态切换逻辑 (按优先级) ---
|
||||
|
||||
// 旋转检测
|
||||
// 如果Z轴角速度很大，则判断为原地旋转，暂停积分
|
||||
if (gyr_z_abs > ROTATION_GYR_Z_THRESHOLD) {
|
||||
tracker->state = SKIING_STATE_ROTATING;
|
||||
return; // 直接返回，不执行后续判断
|
||||
}
|
||||
|
||||
// 动态零速更新 (ZUPT)
|
||||
// 如果加速度在窗口期内非常稳定（方差很小），则认为是动态稳定状态，进行零速校正
|
||||
if (acc_variance < ZUPT_ACC_VARIANCE_THRESHOLD) {
|
||||
tracker->state = SKIING_STATE_STATIC;
|
||||
// 速度清零，抑制漂移
|
||||
memset(tracker->velocity, 0, sizeof(tracker->velocity));
|
||||
tracker->speed = 0.0f;
|
||||
return;
|
||||
}
|
||||
|
||||
// 从静止/旋转状态启动
|
||||
if (tracker->state == SKIING_STATE_STATIC || tracker->state == SKIING_STATE_ROTATING) {
|
||||
if (fabsf(acc_magnitude - G_ACCELERATION) > START_SKIING_ACC_THRESHOLD) {
|
||||
tracker->state = SKIING_STATE_SKIING;
|
||||
return;
|
||||
}
|
||||
}
|
||||
|
||||
// 滑雪
|
||||
if (tracker->state != SKIING_STATE_STATIC) {
|
||||
tracker->state = SKIING_STATE_SKIING;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* @brief 主更新函数
|
||||
*/
|
||||
void skiing_tracker_update(skiing_tracker_t *tracker, float *acc_g, float *gyr_dps, float *angle, float dt)
|
||||
{
|
||||
if (!tracker || !acc_g || !gyr_dps || !angle || dt <= 0) {
|
||||
return;
|
||||
}
|
||||
|
||||
// --- 1. 数据预处理和缓冲 ---
|
||||
float acc_device_ms2[3];
|
||||
acc_device_ms2[0] = acc_g[0] * G_ACCELERATION;
|
||||
acc_device_ms2[1] = acc_g[1] * G_ACCELERATION;
|
||||
acc_device_ms2[2] = acc_g[2] * G_ACCELERATION;
|
||||
|
||||
// 将最新数据存入缓冲区
|
||||
memcpy(tracker->acc_buffer[tracker->buffer_index], acc_device_ms2, sizeof(acc_device_ms2));
|
||||
memcpy(tracker->gyr_buffer[tracker->buffer_index], gyr_dps, 3 * sizeof(float));
|
||||
|
||||
tracker->buffer_index++;
|
||||
if (tracker->buffer_index >= VARIANCE_BUFFER_SIZE) {
|
||||
tracker->buffer_index = 0;
|
||||
tracker->buffer_filled = 1; // 标记缓冲区已满
|
||||
}
|
||||
|
||||
// --- 2. 更新状态机 ---
|
||||
update_state_machine(tracker, acc_device_ms2, gyr_dps);
|
||||
|
||||
// --- 3. 根据状态进行计算 ---
|
||||
// 只有在明确的“滑雪”状态下才进行积分
|
||||
if (tracker->state == SKIING_STATE_SKIING) {
|
||||
transform_acc_to_world_frame(acc_device_ms2, angle, tracker->acc_world);
|
||||
tracker->acc_world[2] -= G_ACCELERATION;
|
||||
|
||||
tracker->velocity[0] += tracker->acc_world[0] * dt;
|
||||
tracker->velocity[1] += tracker->acc_world[1] * dt;
|
||||
tracker->velocity[2] += tracker->acc_world[2] * dt;
|
||||
}
|
||||
// 在其他状态下（静止、旋转），速度已经在状态机内部被清零或保持不变
|
||||
|
||||
// --- 4. 更新速率和距离 ---
|
||||
// 速率和距离总是在更新，但在非滑雪状态下，速度为0，所以它们不会增加
|
||||
tracker->speed = sqrtf(tracker->velocity[0] * tracker->velocity[0] +
|
||||
tracker->velocity[1] * tracker->velocity[1] +
|
||||
tracker->velocity[2] * tracker->velocity[2]);
|
||||
tracker->distance += tracker->speed * dt;
|
||||
}
|
||||
|
||||
|
||||
// 传感器数据采集与处理任务
|
||||
void sensor_processing_task(signed short * acc_data_buf, signed short * gyr_data_buf) {
|
||||
static skiing_tracker_t my_skiing_tracker;
|
||||
static int initialized = 0;
|
||||
static int calibration_done = 0;
|
||||
|
||||
static signed short combined_raw_data[6];
|
||||
static float final_angle_data[3];
|
||||
static float calibrated_acc_g[3];
|
||||
static float calibrated_gyr_dps[3]; // 新增：用于存放转换后的陀螺仪数据
|
||||
|
||||
const float delta_time = 0.01f;
|
||||
|
||||
if (!initialized) {
|
||||
skiing_tracker_init(&my_skiing_tracker);
|
||||
initialized = 1;
|
||||
printf("Skiing Tracker Initialized. Waiting for sensor calibration...\n");
|
||||
}
|
||||
|
||||
memcpy(&combined_raw_data[0], acc_data_buf, 3 * sizeof(signed short));
|
||||
memcpy(&combined_raw_data[3], gyr_data_buf, 3 * sizeof(signed short));
|
||||
|
||||
unsigned char status;
|
||||
if (!calibration_done) {
|
||||
status = SL_SC7U22_Angle_Output(1, combined_raw_data, final_angle_data, 0);
|
||||
if (status == 1) {
|
||||
calibration_done = 1;
|
||||
printf("Sensor calibration successful! Skiing mode is active.\n");
|
||||
}
|
||||
} else {
|
||||
status = SL_SC7U22_Angle_Output(0, combined_raw_data, final_angle_data, 0);
|
||||
}
|
||||
|
||||
if (status == 1) {
|
||||
// 加速度 LSB to g
|
||||
calibrated_acc_g[0] = (float)combined_raw_data[0] / 8192.0f;
|
||||
calibrated_acc_g[1] = (float)combined_raw_data[1] / 8192.0f;
|
||||
calibrated_acc_g[2] = (float)combined_raw_data[2] / 8192.0f;
|
||||
|
||||
// 陀螺仪 LSB to dps (度/秒)
|
||||
// ±2000dps量程下，转换系数约为 0.061
|
||||
calibrated_gyr_dps[0] = (float)combined_raw_data[3] * 0.061f;
|
||||
calibrated_gyr_dps[1] = (float)combined_raw_data[4] * 0.061f;
|
||||
calibrated_gyr_dps[2] = (float)combined_raw_data[5] * 0.061f;
|
||||
|
||||
skiing_tracker_update(&my_skiing_tracker, calibrated_acc_g, calibrated_gyr_dps, final_angle_data, delta_time);
|
||||
|
||||
// 打印逻辑保持不变
|
||||
static int count = 0;
|
||||
if(count < 10){
|
||||
count++;
|
||||
return;
|
||||
} else {
|
||||
count = 0;
|
||||
}
|
||||
printf("State: %d, Speed: %.2f m/s, Distance: %.2f m\n",
|
||||
my_skiing_tracker.state,
|
||||
my_skiing_tracker.speed,
|
||||
my_skiing_tracker.distance);
|
||||
|
||||
} else if (status == 0) {
|
||||
// printf("Sensor is calibrating...\n");
|
||||
} else {
|
||||
// printf("Angle calculation error or calibration not finished.\n");
|
||||
}
|
||||
}
|
||||
@ -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
|
||||
@ -1,277 +0,0 @@
|
||||
/*
|
||||
动态ZUPT+卡尔曼
|
||||
*/
|
||||
#include "skiing_tracker.h"
|
||||
#include "../sensor/SC7U22.h"
|
||||
#include <math.h>
|
||||
#include <string.h>
|
||||
|
||||
#define G_ACCELERATION 9.81f
|
||||
#define DEG_TO_RAD (3.14159265f / 180.0f)
|
||||
|
||||
// --- 算法阈值定义 ---
|
||||
//两个判断是否静止的必要条件
|
||||
// 动态零速更新(ZUPT)阈值
|
||||
// 提高阈值，让“刹车”更灵敏，以便在波浪式前进等慢速漂移时也能触发零速更新
|
||||
#define ZUPT_ACC_VARIANCE_THRESHOLD 0.2f
|
||||
// 陀螺仪方差阈值
|
||||
#define ZUPT_GYR_VARIANCE_THRESHOLD 5.0f
|
||||
|
||||
// 旋转/摆动检测阈值：角速度总模长大于此值(度/秒)，认为正在进行非滑雪的旋转或摆动
|
||||
#define ROTATION_GYR_MAG_THRESHOLD 45.0f
|
||||
// 启动滑雪阈值：加速度模长与重力的差值大于此值，认为开始运动
|
||||
// 降低阈值，让“油门”更灵敏，以便能捕捉到真实的慢速启动
|
||||
#define START_SKIING_ACC_THRESHOLD 0.5f
|
||||
// 新增：速度阻尼系数，用于模拟摩擦力，抑制漂移
|
||||
#define VELOCITY_DAMPING_FACTOR 0.98f
|
||||
|
||||
/**
|
||||
* @brief 初始化滑雪追踪器
|
||||
*/
|
||||
void skiing_tracker_init(skiing_tracker_t *tracker)
|
||||
{
|
||||
if (!tracker) {
|
||||
return;
|
||||
}
|
||||
// 使用memset一次性清零整个结构体，包括新增的缓冲区
|
||||
memset(tracker, 0, sizeof(skiing_tracker_t));
|
||||
tracker->state = SKIING_STATE_STATIC;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 将设备坐标系下的加速度转换为世界坐标系
|
||||
* @param acc_device 设备坐标系下的加速度 [x, y, z]
|
||||
* @param angle 姿态角 [pitch, roll, yaw]，单位: 度
|
||||
* @param acc_world 输出：世界坐标系下的加速度 [x, y, z]
|
||||
*/
|
||||
static void transform_acc_to_world_frame(const float *acc_device, const float *angle, float *acc_world)
|
||||
{
|
||||
// 驱动输出的角度与标准航空定义相反，需要取反才能用于标准旋转矩阵。
|
||||
float pitch = -angle[0] * DEG_TO_RAD;
|
||||
float roll = -angle[1] * DEG_TO_RAD;
|
||||
|
||||
float cp = cosf(pitch);
|
||||
float sp = sinf(pitch);
|
||||
float cr = cosf(roll);
|
||||
float sr = sinf(roll);
|
||||
|
||||
float ax = acc_device[0];
|
||||
float ay = acc_device[1];
|
||||
float az = acc_device[2];
|
||||
|
||||
// 使用经过验证的、正确的身体坐标系到世界坐标系的旋转矩阵 (基于 Y-X 旋转顺序)
|
||||
// 这个矩阵将设备测量的加速度(ax, ay, az)正确地转换到世界坐标系(acc_world)。
|
||||
acc_world[0] = cp * ax + sp * sr * ay + sp * cr * az;
|
||||
acc_world[1] = 0 * ax + cr * ay - sr * az;
|
||||
acc_world[2] = -sp * ax + cp * sr * ay + cp * cr * az;
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* @brief 计算缓冲区内三轴数据的方差之和
|
||||
*/
|
||||
static float calculate_variance(float buffer[VARIANCE_BUFFER_SIZE][3])
|
||||
{
|
||||
float mean[3] = {0};
|
||||
float variance[3] = {0};
|
||||
|
||||
// 1. 计算均值
|
||||
for (int i = 0; i < VARIANCE_BUFFER_SIZE; i++) {
|
||||
mean[0] += buffer[i][0];
|
||||
mean[1] += buffer[i][1];
|
||||
mean[2] += buffer[i][2];
|
||||
}
|
||||
mean[0] /= VARIANCE_BUFFER_SIZE;
|
||||
mean[1] /= VARIANCE_BUFFER_SIZE;
|
||||
mean[2] /= VARIANCE_BUFFER_SIZE;
|
||||
|
||||
// 2. 计算方差
|
||||
for (int i = 0; i < VARIANCE_BUFFER_SIZE; i++) {
|
||||
variance[0] += (buffer[i][0] - mean[0]) * (buffer[i][0] - mean[0]);
|
||||
variance[1] += (buffer[i][1] - mean[1]) * (buffer[i][1] - mean[1]);
|
||||
variance[2] += (buffer[i][2] - mean[2]) * (buffer[i][2] - mean[2]);
|
||||
}
|
||||
variance[0] /= VARIANCE_BUFFER_SIZE;
|
||||
variance[1] /= VARIANCE_BUFFER_SIZE;
|
||||
variance[2] /= VARIANCE_BUFFER_SIZE;
|
||||
|
||||
// 返回三轴方差之和，作为一个综合的稳定度指标
|
||||
return variance[0] + variance[1] + variance[2];
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* @brief 升级后的状态机，包含旋转检测和动态零速更新
|
||||
*/
|
||||
static void update_state_machine(skiing_tracker_t *tracker, const float *acc_device_ms2, const float *gyr_dps)
|
||||
{
|
||||
// 缓冲区未填满时，不进行状态判断，默认为静止
|
||||
if (!tracker->buffer_filled) {
|
||||
tracker->state = SKIING_STATE_STATIC;
|
||||
return;
|
||||
}
|
||||
|
||||
// --- 1. 计算关键指标 ---
|
||||
float acc_variance = calculate_variance(tracker->acc_buffer); // 计算加速度方差
|
||||
float gyr_variance = calculate_variance(tracker->gyr_buffer); // 计算陀螺仪方差
|
||||
float gyr_magnitude = sqrtf(gyr_dps[0]*gyr_dps[0] + gyr_dps[1]*gyr_dps[1] + gyr_dps[2]*gyr_dps[2]);
|
||||
float acc_magnitude = sqrtf(acc_device_ms2[0]*acc_device_ms2[0] + acc_device_ms2[1]*acc_device_ms2[1] + acc_device_ms2[2]*acc_device_ms2[2]);
|
||||
|
||||
// --- 2. 状态切换逻辑 (按优先级) ---
|
||||
|
||||
// 优先级1：原地旋转/摆动检测 (最终版)
|
||||
// 增加一个关键前提：只在当前不处于滑雪状态时，才检测原地旋转。
|
||||
// 这可以防止滑雪过程中的高速转弯被误判为原地旋转。
|
||||
if (gyr_magnitude > ROTATION_GYR_MAG_THRESHOLD && tracker->state != SKIING_STATE_SKIING) {
|
||||
tracker->state = SKIING_STATE_ROTATING;
|
||||
return;
|
||||
}
|
||||
|
||||
// 动态零速更新 (ZUPT)
|
||||
// 必须同时满足加速度和角速度都稳定，才能判断为“真静止”，以区分匀速运动
|
||||
if (acc_variance < ZUPT_ACC_VARIANCE_THRESHOLD && gyr_variance < ZUPT_GYR_VARIANCE_THRESHOLD) {
|
||||
tracker->state = SKIING_STATE_STATIC;
|
||||
// 速度清零，抑制漂移
|
||||
memset(tracker->velocity, 0, sizeof(tracker->velocity));
|
||||
tracker->speed = 0.0f;
|
||||
return;
|
||||
}
|
||||
|
||||
// 从静止/旋转状态启动
|
||||
if (tracker->state == SKIING_STATE_STATIC || tracker->state == SKIING_STATE_ROTATING) {
|
||||
// 最终版启动逻辑：必须同时满足“有足够大的线性加速度”和“旋转稳定”两个条件
|
||||
if (fabsf(acc_magnitude - G_ACCELERATION) > START_SKIING_ACC_THRESHOLD && gyr_variance < ZUPT_GYR_VARIANCE_THRESHOLD) {
|
||||
tracker->state = SKIING_STATE_SKIING;
|
||||
return;
|
||||
}
|
||||
}
|
||||
|
||||
// 滑雪
|
||||
if (tracker->state != SKIING_STATE_STATIC) {
|
||||
tracker->state = SKIING_STATE_SKIING;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* @brief 主更新函数
|
||||
*/
|
||||
void skiing_tracker_update(skiing_tracker_t *tracker, float *acc_g, float *gyr_dps, float *angle, float dt)
|
||||
{
|
||||
if (!tracker || !acc_g || !gyr_dps || !angle || dt <= 0) {
|
||||
return;
|
||||
}
|
||||
|
||||
// --- 1. 数据预处理和缓冲 ---
|
||||
float acc_device_ms2[3];
|
||||
acc_device_ms2[0] = acc_g[0] * G_ACCELERATION;
|
||||
acc_device_ms2[1] = acc_g[1] * G_ACCELERATION;
|
||||
acc_device_ms2[2] = acc_g[2] * G_ACCELERATION;
|
||||
|
||||
// 将最新数据存入缓冲区
|
||||
memcpy(tracker->acc_buffer[tracker->buffer_index], acc_device_ms2, sizeof(acc_device_ms2));
|
||||
memcpy(tracker->gyr_buffer[tracker->buffer_index], gyr_dps, 3 * sizeof(float));
|
||||
|
||||
tracker->buffer_index++;
|
||||
if (tracker->buffer_index >= VARIANCE_BUFFER_SIZE) {
|
||||
tracker->buffer_index = 0;
|
||||
tracker->buffer_filled = 1; // 标记缓冲区已满
|
||||
}
|
||||
|
||||
// --- 2. 更新状态机 ---
|
||||
update_state_machine(tracker, acc_device_ms2, gyr_dps);
|
||||
|
||||
// --- 3. 根据状态进行计算 ---
|
||||
// 只有在明确的“滑雪”状态下才进行积分
|
||||
if (tracker->state == SKIING_STATE_SKIING) {
|
||||
transform_acc_to_world_frame(acc_device_ms2, angle, tracker->acc_world);
|
||||
tracker->acc_world[2] -= G_ACCELERATION;
|
||||
|
||||
tracker->velocity[0] += tracker->acc_world[0] * dt;
|
||||
tracker->velocity[1] += tracker->acc_world[1] * dt;
|
||||
tracker->velocity[2] += tracker->acc_world[2] * dt;
|
||||
}
|
||||
// 在其他状态下（静止、旋转），速度已经在状态机内部被清零或保持不变
|
||||
|
||||
// --- 4. 引入速度阻尼（软件摩擦力） ---
|
||||
// 只要不处于滑雪状态，就对速度进行衰减，以对抗漂移和抑制抖动产生的微小速度
|
||||
if (tracker->state != SKIING_STATE_SKIING) {
|
||||
tracker->velocity[0] *= VELOCITY_DAMPING_FACTOR;
|
||||
tracker->velocity[1] *= VELOCITY_DAMPING_FACTOR;
|
||||
tracker->velocity[2] *= VELOCITY_DAMPING_FACTOR;
|
||||
}
|
||||
|
||||
// --- 5. 更新速率和距离 ---
|
||||
tracker->speed = sqrtf(tracker->velocity[0] * tracker->velocity[0] +
|
||||
tracker->velocity[1] * tracker->velocity[1] +
|
||||
tracker->velocity[2] * tracker->velocity[2]);
|
||||
tracker->distance += tracker->speed * dt;
|
||||
}
|
||||
|
||||
|
||||
// 传感器数据采集与处理任务
|
||||
void sensor_processing_task(signed short * acc_data_buf, signed short * gyr_data_buf) {
|
||||
static skiing_tracker_t my_skiing_tracker;
|
||||
static int initialized = 0;
|
||||
static int calibration_done = 0;
|
||||
|
||||
static signed short combined_raw_data[6];
|
||||
static float final_angle_data[3]; // 计算得到的欧若拉角
|
||||
static float calibrated_acc_g[3]; // 转换后的加速度计数据
|
||||
static float calibrated_gyr_dps[3]; // 转换后的陀螺仪数据
|
||||
|
||||
const float delta_time = 0.01f;
|
||||
|
||||
if (!initialized) {
|
||||
skiing_tracker_init(&my_skiing_tracker);
|
||||
initialized = 1;
|
||||
printf("Skiing Tracker Initialized. Waiting for sensor calibration...\n");
|
||||
}
|
||||
|
||||
memcpy(&combined_raw_data[0], acc_data_buf, 3 * sizeof(signed short));
|
||||
memcpy(&combined_raw_data[3], gyr_data_buf, 3 * sizeof(signed short));
|
||||
|
||||
unsigned char status;
|
||||
if (!calibration_done) { //第1次启动，开启零漂检测
|
||||
status = SL_SC7U22_Angle_Output(1, combined_raw_data, final_angle_data, 0);
|
||||
if (status == 1) {
|
||||
calibration_done = 1;
|
||||
printf("Sensor calibration successful! Skiing mode is active.\n");
|
||||
}
|
||||
} else {
|
||||
status = SL_SC7U22_Angle_Output(0, combined_raw_data, final_angle_data, 0);
|
||||
}
|
||||
|
||||
if (status == 1) {
|
||||
// 加速度 LSB to g
|
||||
calibrated_acc_g[0] = (float)combined_raw_data[0] / 8192.0f;
|
||||
calibrated_acc_g[1] = (float)combined_raw_data[1] / 8192.0f;
|
||||
calibrated_acc_g[2] = (float)combined_raw_data[2] / 8192.0f;
|
||||
|
||||
// 陀螺仪 LSB to dps (度/秒)
|
||||
// ±2000dps量程下，转换系数约为 0.061
|
||||
calibrated_gyr_dps[0] = (float)combined_raw_data[3] * 0.061f;
|
||||
calibrated_gyr_dps[1] = (float)combined_raw_data[4] * 0.061f;
|
||||
calibrated_gyr_dps[2] = (float)combined_raw_data[5] * 0.061f;
|
||||
|
||||
skiing_tracker_update(&my_skiing_tracker, calibrated_acc_g, calibrated_gyr_dps, final_angle_data, delta_time);
|
||||
|
||||
// 打印逻辑保持不变
|
||||
static int count = 0;
|
||||
if(count < 10){
|
||||
count++;
|
||||
return;
|
||||
} else {
|
||||
count = 0;
|
||||
}
|
||||
printf("State: %d, Speed: %.2f m/s, Distance: %.2f m\n",
|
||||
my_skiing_tracker.state,
|
||||
my_skiing_tracker.speed,
|
||||
my_skiing_tracker.distance);
|
||||
|
||||
} else if (status == 0) {
|
||||
// printf("Sensor is calibrating...\n");
|
||||
} else {
|
||||
// printf("Angle calculation error or calibration not finished.\n");
|
||||
}
|
||||
}
|
||||
@ -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
|
||||
@ -1,465 +0,0 @@
|
||||
/**
|
||||
* 效果不行，对原地转动太灵敏了
|
||||
*/
|
||||
|
||||
#include "skiing_tracker.h"
|
||||
#include <math.h>
|
||||
#include <string.h>
|
||||
|
||||
// --- 核心算法参数 ---
|
||||
#define COMPLEMENTARY_FILTER_ALPHA 0.96f // 互补滤波器alpha值 (降低alpha, 增加对加速度计的信任, 更好地抑制陀螺仪漂移)
|
||||
#define ACC_SENSITIVITY 8192.0f // 加速度计灵敏度 (LSB/g), 对应 ±4g 量程
|
||||
#define GYRO_SENSITIVITY 16.4f // 陀螺仪灵敏度 (LSB/dps), 对应 ±2000dps 量程
|
||||
#define SKI_DT_FIXED 0.01f // 固定时间间隔 10ms (100Hz)
|
||||
|
||||
// --- 状态机和阈值 ---
|
||||
#define SKI_STATIC_THRESHOLD_ACC 0.3f // 静止检测加速度阈值 (m/s^2) - 提高以忽略更多噪声
|
||||
#define SKI_STATIC_THRESHOLD_GYRO 3.0f // 静止检测角速度阈值 (dps) - 更严格
|
||||
#define SKI_MOVEMENT_THRESHOLD 1.2f // 运动启动阈值 (m/s^2) - 显著提高，需要更明确的运动意图
|
||||
#define SKI_WINDOW_SIZE 10 // 滑动窗口大小 (当前未使用)
|
||||
#define SKI_CALIBRATION_SAMPLES 100 // 校准采样点数 (恢复到100，确保校准精度)
|
||||
|
||||
|
||||
// 滑雪数据结构
|
||||
typedef struct {
|
||||
// 状态相关
|
||||
SkiState_t current_state;
|
||||
uint32_t state_duration; // 状态持续时间(ms)
|
||||
uint8_t static_counter; // 静态计数器
|
||||
|
||||
// --- 核心运动数据 ---
|
||||
// 姿态数据 (Roll, Pitch)
|
||||
float attitude[2];
|
||||
// 世界坐标系下的加速度
|
||||
float world_acc[3];
|
||||
// 世界坐标系下的速度
|
||||
float velocity[3];
|
||||
// 水平面速度和总距离
|
||||
float horizontal_speed;
|
||||
float total_horizontal_distance;
|
||||
|
||||
// 历史数据 (用于滤波)
|
||||
float acc_history[SKI_WINDOW_SIZE][3];
|
||||
float gyro_history[SKI_WINDOW_SIZE][3];
|
||||
uint8_t history_index;
|
||||
|
||||
// 时间相关
|
||||
float dt; // 时间间隔
|
||||
|
||||
// 运动检测
|
||||
float movement_score; // 运动评分
|
||||
float turning_score; // 转弯评分
|
||||
|
||||
// 校准数据 (用于陀螺仪零偏)
|
||||
float gyro_bias[3];
|
||||
float acc_bias[3];
|
||||
float calibration_sum[6];
|
||||
uint32_t calibration_count;
|
||||
uint8_t calibrated;
|
||||
|
||||
// 状态标志和计数器
|
||||
uint32_t update_count;
|
||||
float velocity_decay_factor;
|
||||
uint8_t is_moving;
|
||||
|
||||
} SkiData_t;
|
||||
|
||||
static SkiData_t ski_data = {0};
|
||||
|
||||
// --- 内部核心函数声明 ---
|
||||
// 校准
|
||||
static void Ski_CalibrateSensor(float *acc, float *gyro); // 校准传感器
|
||||
// 姿态解算
|
||||
static void Ski_UpdateAttitude(float *acc, float *gyro); // 更新姿态
|
||||
// 坐标系转换
|
||||
static void Ski_TransformToWorldFrame(float *acc, float *world_acc); // 转换到世界坐标系
|
||||
// 速度与距离积分
|
||||
static void Ski_UpdateVelocityAndDistance(void); // 更新速度和距离
|
||||
|
||||
// --- 辅助函数声明 ---
|
||||
static float Ski_CalculateMovementScore(void); // 计算运动评分
|
||||
static float Ski_CalculateTurningScore(float *gyro);
|
||||
static void Ski_UpdateStateMachine(void);
|
||||
static void Ski_MovingAverageFilter(float *new_acc, float *new_gyro, float *acc_filtered, float *gyro_filtered);
|
||||
|
||||
// 修改初始化函数
|
||||
void Ski_Init(void)
|
||||
{
|
||||
memset(&ski_data, 0, sizeof(SkiData_t));
|
||||
ski_data.current_state = SKI_STATE_STATIC;
|
||||
|
||||
// 初始化参数
|
||||
ski_data.velocity_decay_factor = 0.8f; // 速度衰减因子
|
||||
ski_data.is_moving = 0;
|
||||
ski_data.update_count = 0;
|
||||
ski_data.dt = SKI_DT_FIXED;
|
||||
}
|
||||
// 滑动窗口滤波
|
||||
static void Ski_MovingAverageFilter(float *new_acc, float *new_gyro,
|
||||
float *acc_filtered, float *gyro_filtered)
|
||||
{
|
||||
// 更新历史数据
|
||||
ski_data.acc_history[ski_data.history_index][0] = new_acc[0];
|
||||
ski_data.acc_history[ski_data.history_index][1] = new_acc[1];
|
||||
ski_data.acc_history[ski_data.history_index][2] = new_acc[2];
|
||||
|
||||
ski_data.gyro_history[ski_data.history_index][0] = new_gyro[0];
|
||||
ski_data.gyro_history[ski_data.history_index][1] = new_gyro[1];
|
||||
ski_data.gyro_history[ski_data.history_index][2] = new_gyro[2];
|
||||
|
||||
ski_data.history_index = (ski_data.history_index + 1) % SKI_WINDOW_SIZE;
|
||||
|
||||
// 计算平均值
|
||||
memset(acc_filtered, 0, 3 * sizeof(float));
|
||||
memset(gyro_filtered, 0, 3 * sizeof(float));
|
||||
|
||||
for (int i = 0; i < SKI_WINDOW_SIZE; i++) {
|
||||
acc_filtered[0] += ski_data.acc_history[i][0];
|
||||
acc_filtered[1] += ski_data.acc_history[i][1];
|
||||
acc_filtered[2] += ski_data.acc_history[i][2];
|
||||
|
||||
gyro_filtered[0] += ski_data.gyro_history[i][0];
|
||||
gyro_filtered[1] += ski_data.gyro_history[i][1];
|
||||
gyro_filtered[2] += ski_data.gyro_history[i][2];
|
||||
}
|
||||
|
||||
acc_filtered[0] /= SKI_WINDOW_SIZE;
|
||||
acc_filtered[1] /= SKI_WINDOW_SIZE;
|
||||
acc_filtered[2] /= SKI_WINDOW_SIZE;
|
||||
|
||||
gyro_filtered[0] /= SKI_WINDOW_SIZE;
|
||||
gyro_filtered[1] /= SKI_WINDOW_SIZE;
|
||||
gyro_filtered[2] /= SKI_WINDOW_SIZE;
|
||||
}
|
||||
|
||||
|
||||
// =================================================================================
|
||||
// 核心更新函数 - 已重构
|
||||
// =================================================================================
|
||||
void Ski_UpdateData(int16_t acc_x, int16_t acc_y, int16_t acc_z,
|
||||
int16_t gyro_x, int16_t gyro_y, int16_t gyro_z)
|
||||
{
|
||||
ski_data.update_count++;
|
||||
|
||||
// 1. LSB原始数据转换为物理单位 (g 和 度/秒)
|
||||
float acc[3] = { (float)acc_x / ACC_SENSITIVITY, (float)acc_y / ACC_SENSITIVITY, (float)acc_z / ACC_SENSITIVITY };
|
||||
float gyro[3] = { (float)gyro_x / GYRO_SENSITIVITY, (float)gyro_y / GYRO_SENSITIVITY, (float)gyro_z / GYRO_SENSITIVITY };
|
||||
|
||||
// 2. 传感器校准 (移除零偏)
|
||||
if (!ski_data.calibrated) {
|
||||
Ski_CalibrateSensor(acc, gyro);
|
||||
return; // 等待校准完成
|
||||
}
|
||||
acc[0] -= ski_data.acc_bias[0];
|
||||
acc[1] -= ski_data.acc_bias[1];
|
||||
acc[2] -= ski_data.acc_bias[2];
|
||||
gyro[0] -= ski_data.gyro_bias[0];
|
||||
gyro[1] -= ski_data.gyro_bias[1];
|
||||
gyro[2] -= ski_data.gyro_bias[2];
|
||||
|
||||
// 3. 姿态解算 (互补滤波器)
|
||||
Ski_UpdateAttitude(acc, gyro);
|
||||
|
||||
// 4. 将身体坐标系的加速度转换为世界坐标系
|
||||
Ski_TransformToWorldFrame(acc, ski_data.world_acc);
|
||||
|
||||
// 5. 在世界坐标系中移除重力 (重力永远是 [0, 0, 1]g)
|
||||
ski_data.world_acc[2] -= 1.0f;
|
||||
|
||||
// 6. 对世界坐标系下的加速度进行滤波 (可选，但推荐)
|
||||
// 注意: 如果需要，可在此处重用滑动平均滤波器，但暂时我们直接使用原始世界坐标系加速度
|
||||
|
||||
// 7. 基于世界坐标系的数据更新状态机
|
||||
Ski_UpdateStateMachine();
|
||||
|
||||
// 8. 基于世界坐标系的数据更新速度和距离
|
||||
Ski_UpdateVelocityAndDistance();
|
||||
}
|
||||
// 计算转弯评分
|
||||
static float Ski_CalculateTurningScore(float *gyro)
|
||||
{
|
||||
// 主要考虑Z轴旋转(偏航)和X轴旋转(俯仰)
|
||||
float turning_score = fabsf(gyro[2]) * 0.6f + fabsf(gyro[0]) * 0.4f;
|
||||
return turning_score;
|
||||
}
|
||||
|
||||
|
||||
// =================================================================================
|
||||
// 新增核心算法函数
|
||||
// =================================================================================
|
||||
|
||||
/**
|
||||
* @brief 在静止状态下计算传感器零偏
|
||||
*/
|
||||
static void Ski_CalibrateSensor(float *acc, float *gyro)
|
||||
{
|
||||
float acc_mag = sqrtf(acc[0]*acc[0] + acc[1]*acc[1] + acc[2]*acc[2]);
|
||||
float gyro_mag = sqrtf(gyro[0]*gyro[0] + gyro[1]*gyro[1] + gyro[2]*gyro[2]);
|
||||
|
||||
// 放宽静止判断条件，使其更容易满足
|
||||
if (fabsf(acc_mag - 1.0f) < 0.1f && gyro_mag < SKI_STATIC_THRESHOLD_GYRO) {
|
||||
ski_data.calibration_sum[0] += acc[0];
|
||||
ski_data.calibration_sum[1] += acc[1];
|
||||
ski_data.calibration_sum[2] += acc[2];
|
||||
ski_data.calibration_sum[3] += gyro[0];
|
||||
ski_data.calibration_sum[4] += gyro[1];
|
||||
ski_data.calibration_sum[5] += gyro[2];
|
||||
ski_data.calibration_count++;
|
||||
|
||||
if (ski_data.calibration_count >= SKI_CALIBRATION_SAMPLES) {
|
||||
// 计算加速度计零偏
|
||||
ski_data.acc_bias[0] = ski_data.calibration_sum[0] / ski_data.calibration_count;
|
||||
ski_data.acc_bias[1] = ski_data.calibration_sum[1] / ski_data.calibration_count;
|
||||
// Z轴的零偏是相对于1g的, 需要找到重力轴
|
||||
float gravity_mag = sqrtf(powf(ski_data.calibration_sum[0] / ski_data.calibration_count, 2) +
|
||||
powf(ski_data.calibration_sum[1] / ski_data.calibration_count, 2) +
|
||||
powf(ski_data.calibration_sum[2] / ski_data.calibration_count, 2));
|
||||
ski_data.acc_bias[2] = ski_data.calibration_sum[2] / ski_data.calibration_count - gravity_mag;
|
||||
|
||||
|
||||
// 计算陀螺仪零偏
|
||||
ski_data.gyro_bias[0] = ski_data.calibration_sum[3] / ski_data.calibration_count;
|
||||
ski_data.gyro_bias[1] = ski_data.calibration_sum[4] / ski_data.calibration_count;
|
||||
ski_data.gyro_bias[2] = ski_data.calibration_sum[5] / ski_data.calibration_count;
|
||||
|
||||
ski_data.calibrated = 1;
|
||||
printf("传感器校准完成!\n");
|
||||
}
|
||||
} else {
|
||||
// 如果检测到移动，则重置校准计数
|
||||
ski_data.calibration_count = 0;
|
||||
memset(ski_data.calibration_sum, 0, sizeof(ski_data.calibration_sum));
|
||||
// 增加调试打印，方便定位问题
|
||||
// printf("Calibration reset. Acc mag: %.3f, Gyro mag: %.3f\n", acc_mag, gyro_mag);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* @brief 使用互补滤波器更新姿态角(Roll, Pitch)
|
||||
*/
|
||||
static void Ski_UpdateAttitude(float *acc, float *gyro)
|
||||
{
|
||||
float dt = ski_data.dt;
|
||||
float alpha = COMPLEMENTARY_FILTER_ALPHA;
|
||||
|
||||
// 从加速度计计算角度 (单位: 度)
|
||||
float roll_acc = atan2f(acc[1], acc[2]) * 180.0f / M_PI;
|
||||
float pitch_acc = atan2f(-acc[0], sqrtf(acc[1] * acc[1] + acc[2] * acc[2])) * 180.0f / M_PI;
|
||||
|
||||
// 陀螺仪积分预测角度
|
||||
// 注意: 这里的gyro单位是 度/秒
|
||||
float roll_gyro = ski_data.attitude[0] + gyro[0] * dt;
|
||||
float pitch_gyro = ski_data.attitude[1] + gyro[1] * dt;
|
||||
|
||||
// 互补滤波融合
|
||||
ski_data.attitude[0] = alpha * roll_gyro + (1.0f - alpha) * roll_acc;
|
||||
ski_data.attitude[1] = alpha * pitch_gyro + (1.0f - alpha) * pitch_acc;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 将身体坐标系的加速度转换为世界坐标系
|
||||
*/
|
||||
static void Ski_TransformToWorldFrame(float *acc, float *world_acc)
|
||||
{
|
||||
// 将姿态角转换为弧度
|
||||
float roll = ski_data.attitude[0] * M_PI / 180.0f;
|
||||
float pitch = ski_data.attitude[1] * M_PI / 180.0f;
|
||||
|
||||
float cos_roll = cosf(roll);
|
||||
float sin_roll = sinf(roll);
|
||||
float cos_pitch = cosf(pitch);
|
||||
float sin_pitch = sinf(pitch);
|
||||
|
||||
// 通过旋转矩阵将身体坐标系的加速度(acc)转换到世界坐标系(world_acc)
|
||||
// (简化版，假设偏航角yaw=0)
|
||||
world_acc[0] = acc[0] * cos_pitch + acc[2] * sin_pitch;
|
||||
world_acc[1] = acc[0] * sin_pitch * sin_roll + acc[1] * cos_roll - acc[2] * cos_pitch * sin_roll;
|
||||
world_acc[2] = -acc[0] * sin_pitch * cos_roll + acc[1] * sin_roll + acc[2] * cos_pitch * cos_roll;
|
||||
}
|
||||
|
||||
// 改进的运动评分计算
|
||||
static float Ski_CalculateMovementScore(void)
|
||||
{
|
||||
// 在世界坐标系下计算运动评分，只考虑水平加速度
|
||||
float horizontal_acc_mag = sqrtf(ski_data.world_acc[0]*ski_data.world_acc[0] + ski_data.world_acc[1]*ski_data.world_acc[1]);
|
||||
return horizontal_acc_mag * 9.81f; // 从 g 转换为 m/s^2
|
||||
}
|
||||
|
||||
// 改进的状态机 - 添加更严格的转换条件
|
||||
/**
|
||||
* @brief 基于世界坐标系下的加速度更新状态机
|
||||
*/
|
||||
static void Ski_UpdateStateMachine(void)
|
||||
{
|
||||
ski_data.state_duration++;
|
||||
ski_data.movement_score = Ski_CalculateMovementScore();
|
||||
|
||||
switch (ski_data.current_state) {
|
||||
case SKI_STATE_STATIC:
|
||||
if (ski_data.movement_score > SKI_MOVEMENT_THRESHOLD) {
|
||||
ski_data.static_counter++;
|
||||
if (ski_data.static_counter > 5) { // 需要连续几帧的运动才切换
|
||||
ski_data.current_state = SKI_STATE_MOVING;
|
||||
ski_data.is_moving = 1;
|
||||
ski_data.state_duration = 0;
|
||||
ski_data.static_counter = 0;
|
||||
}
|
||||
} else {
|
||||
ski_data.static_counter = 0;
|
||||
}
|
||||
break;
|
||||
|
||||
case SKI_STATE_MOVING:
|
||||
// 使用更低的阈值来检测停止，形成迟滞效应，防止状态抖动
|
||||
if (ski_data.movement_score < SKI_STATIC_THRESHOLD_ACC) {
|
||||
ski_data.static_counter++;
|
||||
if (ski_data.static_counter > 8) { // 减少进入静止状态的等待时间
|
||||
ski_data.current_state = SKI_STATE_STATIC;
|
||||
ski_data.is_moving = 0;
|
||||
ski_data.state_duration = 0;
|
||||
ski_data.static_counter = 0;
|
||||
// 停止时强制将速度清零，以防止漂移
|
||||
memset(ski_data.velocity, 0, sizeof(ski_data.velocity));
|
||||
}
|
||||
} else {
|
||||
ski_data.static_counter = 0;
|
||||
}
|
||||
// TODO: 如果需要，可以增加转弯状态的检测
|
||||
break;
|
||||
|
||||
// 如果需要，可以增加其他状态 (例如 TURNING, STOPPING)
|
||||
default:
|
||||
ski_data.current_state = SKI_STATE_STATIC;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
// 改进的速度和距离更新 - 添加零速度更新(ZUPT)
|
||||
/**
|
||||
* @brief 在世界坐标系下更新速度和距离
|
||||
*/
|
||||
static void Ski_UpdateVelocityAndDistance(void)
|
||||
{
|
||||
// 将世界坐标系下的加速度从 g 转换为 m/s^2
|
||||
float acc_ms2[3];
|
||||
acc_ms2[0] = ski_data.world_acc[0] * 9.81f;
|
||||
acc_ms2[1] = ski_data.world_acc[1] * 9.81f;
|
||||
acc_ms2[2] = ski_data.world_acc[2] * 9.81f;
|
||||
|
||||
// 是否进行积分的决定由状态机控制
|
||||
if (ski_data.is_moving) {
|
||||
// 速度积分
|
||||
ski_data.velocity[0] += acc_ms2[0] * ski_data.dt;
|
||||
ski_data.velocity[1] += acc_ms2[1] * ski_data.dt;
|
||||
// 如果需要，也可以对垂直速度进行积分
|
||||
// ski_data.velocity[2] += acc_ms2[2] * ski_data.dt;
|
||||
|
||||
// 通过累加每一步的位移来计算总距离
|
||||
float distance_step = sqrtf(
|
||||
(ski_data.velocity[0] * ski_data.dt) * (ski_data.velocity[0] * ski_data.dt) +
|
||||
(ski_data.velocity[1] * ski_data.dt) * (ski_data.velocity[1] * ski_data.dt)
|
||||
);
|
||||
ski_data.total_horizontal_distance += distance_step;
|
||||
|
||||
} else {
|
||||
// 当静止时，对速度进行衰减
|
||||
ski_data.velocity[0] *= ski_data.velocity_decay_factor;
|
||||
ski_data.velocity[1] *= ski_data.velocity_decay_factor;
|
||||
ski_data.velocity[2] *= ski_data.velocity_decay_factor;
|
||||
}
|
||||
|
||||
// 更新当前水平速度
|
||||
ski_data.horizontal_speed = sqrtf(ski_data.velocity[0]*ski_data.velocity[0] + ski_data.velocity[1]*ski_data.velocity[1]);
|
||||
}
|
||||
|
||||
|
||||
// 获取当前速度 (m/s)
|
||||
float Ski_GetSpeed(void)
|
||||
{
|
||||
return ski_data.horizontal_speed;
|
||||
}
|
||||
|
||||
// 获取总移动距离 (m)
|
||||
float Ski_GetDistance(void)
|
||||
{
|
||||
return ski_data.total_horizontal_distance;
|
||||
}
|
||||
|
||||
// 获取当前状态
|
||||
SkiState_t Ski_GetState(void)
|
||||
{
|
||||
return ski_data.current_state;
|
||||
}
|
||||
|
||||
// 重置距离计数
|
||||
void Ski_ResetDistance(void)
|
||||
{
|
||||
ski_data.total_horizontal_distance = 0;
|
||||
// 速度也应该一起重置
|
||||
memset(ski_data.velocity, 0, sizeof(ski_data.velocity));
|
||||
ski_data.horizontal_speed = 0;
|
||||
}
|
||||
|
||||
// 获取校准状态
|
||||
uint8_t Ski_IsCalibrated(void)
|
||||
{
|
||||
return ski_data.calibrated;
|
||||
}
|
||||
|
||||
// 手动设置重力向量
|
||||
// 这个函数不再需要，将被姿态估计算法取代
|
||||
/*
|
||||
void Ski_SetGravityVector(float gx, float gy, float gz)
|
||||
{
|
||||
...
|
||||
}
|
||||
*/
|
||||
|
||||
|
||||
// 添加调试信息函数
|
||||
void Ski_PrintDebugInfo(void)
|
||||
{
|
||||
printf("State: %d, MoveScore: %.2f, Speed: %.3f, Dist: %.3f, Roll: %.2f, Pitch: %.2f\n",
|
||||
ski_data.current_state, ski_data.movement_score, ski_data.horizontal_speed, ski_data.total_horizontal_distance,
|
||||
ski_data.attitude[0], ski_data.attitude[1]);
|
||||
}
|
||||
|
||||
void sensor_processing_task(void){
|
||||
static int first_init = 0;
|
||||
if(!first_init){
|
||||
Ski_Init();
|
||||
first_init = 1;
|
||||
}
|
||||
static signed short acc_data[3], gyr_data[3];
|
||||
static signed short combined_raw_data[6];
|
||||
static float final_angle_data[3];
|
||||
|
||||
// 读取传感器数据
|
||||
SL_SC7U22_RawData_Read(acc_data, gyr_data);
|
||||
memcpy(&combined_raw_data[0], acc_data, 3 * sizeof(signed short));
|
||||
memcpy(&combined_raw_data[3], gyr_data, 3 * sizeof(signed short));
|
||||
|
||||
// 校准逻辑已内置于Ski_UpdateData中，不再需要外部检查
|
||||
// if(get_calibration_state() == 0){ ... }
|
||||
|
||||
// 更新滑雪数据
|
||||
Ski_UpdateData(combined_raw_data[0], combined_raw_data[1], combined_raw_data[2],
|
||||
combined_raw_data[3], combined_raw_data[4], combined_raw_data[5]);
|
||||
|
||||
static int count = 0;
|
||||
if(count < 10){
|
||||
count++;
|
||||
return;
|
||||
}else{
|
||||
count = 0;
|
||||
}
|
||||
|
||||
// 打印结果和调试信息
|
||||
float speed = Ski_GetSpeed();
|
||||
float distance = Ski_GetDistance();
|
||||
SkiState_t state = Ski_GetState();
|
||||
|
||||
// printf("Speed: %.3f m/s, Distance: %.3f m, State: %d\n", speed, distance, state);
|
||||
Ski_PrintDebugInfo();
|
||||
}
|
||||
@ -1,40 +0,0 @@
|
||||
#ifndef SKI_SPEED_DISTANCE_H
|
||||
#define SKI_SPEED_DISTANCE_H
|
||||
|
||||
#include <stdint.h>
|
||||
|
||||
// 滑雪状态定义
|
||||
typedef enum {
|
||||
SKI_STATE_STATIC = 0, // 静止状态
|
||||
SKI_STATE_STARTING, // 启动状态
|
||||
SKI_STATE_MOVING, // 运动状态
|
||||
SKI_STATE_TURNING, // 转弯状态
|
||||
SKI_STATE_STOPPING // 停止状态
|
||||
} SkiState_t;
|
||||
|
||||
// 初始化滑雪模块
|
||||
void Ski_Init(void);
|
||||
|
||||
// 更新滑雪数据
|
||||
void Ski_UpdateData(int16_t acc_x, int16_t acc_y, int16_t acc_z,
|
||||
int16_t gyro_x, int16_t gyro_y, int16_t gyro_z);
|
||||
|
||||
// 获取当前速度 (m/s)
|
||||
float Ski_GetSpeed(void);
|
||||
|
||||
// 获取总移动距离 (m)
|
||||
float Ski_GetDistance(void);
|
||||
|
||||
// 获取当前状态
|
||||
SkiState_t Ski_GetState(void);
|
||||
|
||||
// 重置距离计数
|
||||
void Ski_ResetDistance(void);
|
||||
|
||||
// 获取校准状态
|
||||
uint8_t Ski_IsCalibrated(void);
|
||||
|
||||
// 手动设置重力向量（可选）
|
||||
void Ski_SetGravityVector(float gx, float gy, float gz);
|
||||
|
||||
#endif
|
||||
@ -1,311 +0,0 @@
|
||||
/*
|
||||
动态ZUPT+卡尔曼
|
||||
多了加速度死区、摩擦力速度衰减、高通滤波
|
||||
原地摆动产生的速度、距离变化还是没法消除
|
||||
水平移动、斜坡移动效果貌似还行
|
||||
*/
|
||||
#include "skiing_tracker.h"
|
||||
#include "../sensor/SC7U22.h"
|
||||
#include <math.h>
|
||||
#include <string.h>
|
||||
|
||||
#define G_ACCELERATION 9.81f
|
||||
#define DEG_TO_RAD (3.14159265f / 180.0f)
|
||||
|
||||
// --- 算法阈值定义 ---
|
||||
//两个判断是否静止的必要条件
|
||||
// 动态零速更新(ZUPT)阈值
|
||||
// 提高阈值，让“刹车”更灵敏，以便在波浪式前进等慢速漂移时也能触发零速更新
|
||||
#define ZUPT_ACC_VARIANCE_THRESHOLD 0.2f
|
||||
// 陀螺仪方差阈值
|
||||
#define ZUPT_GYR_VARIANCE_THRESHOLD 5.0f
|
||||
|
||||
// 旋转/摆动检测阈值：角速度总模长大于此值(度/秒)，认为正在进行非滑雪的旋转或摆动 -- 没法完全消除
|
||||
#define ROTATION_GYR_MAG_THRESHOLD 45.0f
|
||||
// 启动滑雪阈值：加速度模长与重力的差值大于此值，认为开始运动
|
||||
// 降低阈值，让“油门”更灵敏，以便能捕捉到真实的慢速启动
|
||||
#define START_SKIING_ACC_THRESHOLD 0.5f
|
||||
|
||||
// --- 用于消除积分漂移的滤波器和阈值 ---
|
||||
// 高通滤波器系数 (alpha)。alpha 越接近1，滤除低频(直流偏移)的效果越强，但可能滤掉真实的慢速运动。
|
||||
// alpha = RC / (RC + dt)，
|
||||
#define HPF_ALPHA 0.95f
|
||||
// 加速度死区阈值 (m/s^2)。低于此阈值的加速度被认为是噪声，不参与积分。
|
||||
// 设得太高会忽略真实的慢速启动，设得太低则无法有效抑制噪声。
|
||||
#define ACC_DEAD_ZONE_THRESHOLD 0.1f
|
||||
|
||||
#define SPEED_ATTENUATION 0.98f //模拟摩擦力，进行速度衰减
|
||||
|
||||
/**
|
||||
* @brief 初始化滑雪追踪器
|
||||
*/
|
||||
void skiing_tracker_init(skiing_tracker_t *tracker)
|
||||
{
|
||||
if (!tracker) {
|
||||
return;
|
||||
}
|
||||
// 使用memset一次性清零整个结构体，包括新增的缓冲区
|
||||
memset(tracker, 0, sizeof(skiing_tracker_t));
|
||||
tracker->state = SKIING_STATE_STATIC;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 将设备坐标系下的加速度转换为世界坐标系
|
||||
* @param acc_device 设备坐标系下的加速度 [x, y, z]
|
||||
* @param angle 姿态角 [pitch, roll, yaw]，单位: 度
|
||||
* @param acc_world 输出：世界坐标系下的加速度 [x, y, z]
|
||||
*/
|
||||
static void transform_acc_to_world_frame(const float *acc_device, const float *angle, float *acc_world)
|
||||
{
|
||||
// 驱动输出的角度与标准航空定义相反，需要取反才能用于标准旋转矩阵。
|
||||
float pitch = -angle[0] * DEG_TO_RAD;
|
||||
float roll = -angle[1] * DEG_TO_RAD;
|
||||
|
||||
// TODO: 当引入三轴磁力计后，这里的 yaw 应由磁力计和陀螺仪融合解算得出，以解决航向漂移问题。
|
||||
// 目前 yaw 暂时不参与计算，因为仅靠加速度计和陀螺仪无法获得准确的绝对航向角。
|
||||
// float yaw = -angle[2] * DEG_TO_RAD;
|
||||
|
||||
float cp = cosf(pitch);
|
||||
float sp = sinf(pitch);
|
||||
float cr = cosf(roll);
|
||||
float sr = sinf(roll);
|
||||
|
||||
float ax = acc_device[0];
|
||||
float ay = acc_device[1];
|
||||
float az = acc_device[2];
|
||||
|
||||
// 使用经过验证的、正确的身体坐标系到世界坐标系的旋转矩阵 (基于 Y-X 旋转顺序)
|
||||
// 这个矩阵将设备测量的加速度(ax, ay, az)正确地转换到世界坐标系(acc_world)。
|
||||
// 注意：这里没有使用yaw，主要关心的是坡面上的运动，绝对航向暂时不影响速度和距离的计算。
|
||||
// TODO
|
||||
acc_world[0] = cp * ax + sp * sr * ay + sp * cr * az;
|
||||
acc_world[1] = 0 * ax + cr * ay - sr * az;
|
||||
acc_world[2] = -sp * ax + cp * sr * ay + cp * cr * az;
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* @brief 计算缓冲区内三轴数据的方差之和
|
||||
*/
|
||||
static float calculate_variance(float buffer[VARIANCE_BUFFER_SIZE][3])
|
||||
{
|
||||
float mean[3] = {0};
|
||||
float variance[3] = {0};
|
||||
|
||||
// 1. 计算均值
|
||||
for (int i = 0; i < VARIANCE_BUFFER_SIZE; i++) {
|
||||
mean[0] += buffer[i][0];
|
||||
mean[1] += buffer[i][1];
|
||||
mean[2] += buffer[i][2];
|
||||
}
|
||||
mean[0] /= VARIANCE_BUFFER_SIZE;
|
||||
mean[1] /= VARIANCE_BUFFER_SIZE;
|
||||
mean[2] /= VARIANCE_BUFFER_SIZE;
|
||||
|
||||
// 2. 计算方差
|
||||
for (int i = 0; i < VARIANCE_BUFFER_SIZE; i++) {
|
||||
variance[0] += (buffer[i][0] - mean[0]) * (buffer[i][0] - mean[0]);
|
||||
variance[1] += (buffer[i][1] - mean[1]) * (buffer[i][1] - mean[1]);
|
||||
variance[2] += (buffer[i][2] - mean[2]) * (buffer[i][2] - mean[2]);
|
||||
}
|
||||
variance[0] /= VARIANCE_BUFFER_SIZE;
|
||||
variance[1] /= VARIANCE_BUFFER_SIZE;
|
||||
variance[2] /= VARIANCE_BUFFER_SIZE;
|
||||
|
||||
// 返回三轴方差之和，作为一个综合的稳定度指标
|
||||
return variance[0] + variance[1] + variance[2];
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* @brief 状态机更新
|
||||
*/
|
||||
static void update_state_machine(skiing_tracker_t *tracker, const float *acc_device_ms2, const float *gyr_dps)
|
||||
{
|
||||
// 缓冲区未填满时，不进行状态判断，默认为静止
|
||||
if (!tracker->buffer_filled) {
|
||||
tracker->state = SKIING_STATE_STATIC;
|
||||
return;
|
||||
}
|
||||
|
||||
// --- 计算关键指标 ---
|
||||
float acc_variance = calculate_variance(tracker->acc_buffer); // 计算加速度方差
|
||||
float gyr_variance = calculate_variance(tracker->gyr_buffer); // 计算陀螺仪方差
|
||||
float gyr_magnitude = sqrtf(gyr_dps[0]*gyr_dps[0] + gyr_dps[1]*gyr_dps[1] + gyr_dps[2]*gyr_dps[2]);
|
||||
float acc_magnitude = sqrtf(acc_device_ms2[0]*acc_device_ms2[0] + acc_device_ms2[1]*acc_device_ms2[1] + acc_device_ms2[2]*acc_device_ms2[2]);
|
||||
|
||||
// --- 状态切换逻辑---
|
||||
|
||||
// 原地旋转/摆动检测
|
||||
// 增加一个关键前提：只在当前不处于滑雪状态时，才检测原地旋转。
|
||||
// 这可以防止滑雪过程中的高速转弯被误判为原地旋转。
|
||||
// 暂时没办法完全消除
|
||||
if (gyr_magnitude > ROTATION_GYR_MAG_THRESHOLD && tracker->state != SKIING_STATE_SKIING) {
|
||||
tracker->state = SKIING_STATE_ROTATING;
|
||||
return;
|
||||
}
|
||||
|
||||
// 动态零速更新 (ZUPT)
|
||||
// 必须同时满足加速度和角速度都稳定，才能判断为“真静止”，以区分匀速运动
|
||||
if (acc_variance < ZUPT_ACC_VARIANCE_THRESHOLD && gyr_variance < ZUPT_GYR_VARIANCE_THRESHOLD) {
|
||||
tracker->state = SKIING_STATE_STATIC;
|
||||
// 速度清零，抑制漂移
|
||||
memset(tracker->velocity, 0, sizeof(tracker->velocity));
|
||||
tracker->speed = 0.0f;
|
||||
//当检测到静止时，必须重置高通滤波器的状态，否则下次启动时会有跳变
|
||||
memset(tracker->acc_world_unfiltered_prev, 0, sizeof(tracker->acc_world_unfiltered_prev));
|
||||
memset(tracker->acc_world_filtered, 0, sizeof(tracker->acc_world_filtered));
|
||||
return;
|
||||
}
|
||||
|
||||
// 从静止/旋转状态启动
|
||||
if (tracker->state == SKIING_STATE_STATIC || tracker->state == SKIING_STATE_ROTATING) {
|
||||
// 最终版启动逻辑：必须同时满足“有足够大的线性加速度”和“旋转稳定”两个条件
|
||||
if (fabsf(acc_magnitude - G_ACCELERATION) > START_SKIING_ACC_THRESHOLD && gyr_variance < ZUPT_GYR_VARIANCE_THRESHOLD) {
|
||||
tracker->state = SKIING_STATE_SKIING;
|
||||
return;
|
||||
}
|
||||
}
|
||||
|
||||
// 滑雪
|
||||
if (tracker->state != SKIING_STATE_STATIC) {
|
||||
tracker->state = SKIING_STATE_SKIING;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* @brief 主更新函数
|
||||
*/
|
||||
void skiing_tracker_update(skiing_tracker_t *tracker, float *acc_g, float *gyr_dps, float *angle, float dt)
|
||||
{
|
||||
if (!tracker || !acc_g || !gyr_dps || !angle || dt <= 0) {
|
||||
return;
|
||||
}
|
||||
|
||||
// --- 数据预处理和缓冲 ---
|
||||
float acc_device_ms2[3];
|
||||
acc_device_ms2[0] = acc_g[0] * G_ACCELERATION;
|
||||
acc_device_ms2[1] = acc_g[1] * G_ACCELERATION;
|
||||
acc_device_ms2[2] = acc_g[2] * G_ACCELERATION;
|
||||
|
||||
// 将最新数据存入缓冲区
|
||||
memcpy(tracker->acc_buffer[tracker->buffer_index], acc_device_ms2, sizeof(acc_device_ms2));
|
||||
memcpy(tracker->gyr_buffer[tracker->buffer_index], gyr_dps, 3 * sizeof(float));
|
||||
|
||||
tracker->buffer_index++;
|
||||
if (tracker->buffer_index >= VARIANCE_BUFFER_SIZE) {
|
||||
tracker->buffer_index = 0;
|
||||
tracker->buffer_filled = 1; // 标记缓冲区已满
|
||||
}
|
||||
|
||||
// --- 更新状态机 ---
|
||||
update_state_machine(tracker, acc_device_ms2, gyr_dps);
|
||||
|
||||
// --- 根据状态进行计算 ---
|
||||
if (tracker->state == SKIING_STATE_SKIING) {
|
||||
// 坐标转换 & 移除重力
|
||||
transform_acc_to_world_frame(acc_device_ms2, angle, tracker->acc_world);
|
||||
tracker->acc_world[2] -= G_ACCELERATION;
|
||||
|
||||
// 对世界坐标系下的加速度进行高通滤波，消除直流偏置和重力残差
|
||||
for (int i = 0; i < 3; i++) {
|
||||
tracker->acc_world_filtered[i] = HPF_ALPHA * (tracker->acc_world_filtered[i] + tracker->acc_world[i] - tracker->acc_world_unfiltered_prev[i]);
|
||||
tracker->acc_world_unfiltered_prev[i] = tracker->acc_world[i];
|
||||
}
|
||||
|
||||
// 应用加速度死区，忽略微小抖动和噪声
|
||||
float acc_horizontal_mag = sqrtf(tracker->acc_world_filtered[0] * tracker->acc_world_filtered[0] +
|
||||
tracker->acc_world_filtered[1] * tracker->acc_world_filtered[1]);
|
||||
|
||||
if (acc_horizontal_mag > ACC_DEAD_ZONE_THRESHOLD) {
|
||||
// 只有当水平加速度足够大时，才进行速度积分
|
||||
tracker->velocity[0] += tracker->acc_world_filtered[0] * dt;
|
||||
tracker->velocity[1] += tracker->acc_world_filtered[1] * dt;
|
||||
// 垂直方向的速度暂时不积分，极易受姿态误差影响而漂移
|
||||
// tracker->velocity[2] += tracker->acc_world_filtered[2] * dt;
|
||||
}
|
||||
// 如果加速度小于阈值，则不更新速度，相当于速度保持不变（或受下一步的阻尼影响而衰减）
|
||||
|
||||
} else {
|
||||
// 在静止或旋转状态下，速度已经在状态机内部被清零
|
||||
// 额外增加速度衰减，模拟摩擦力，进一步抑制漂移
|
||||
tracker->velocity[0] *= SPEED_ATTENUATION;
|
||||
tracker->velocity[1] *= SPEED_ATTENUATION;
|
||||
tracker->velocity[2] = 0; // 垂直速度强制归零
|
||||
}
|
||||
|
||||
// --- 更新速率和距离 ---
|
||||
// 只基于水平速度计算速率和距离
|
||||
tracker->speed = sqrtf(tracker->velocity[0] * tracker->velocity[0] +
|
||||
tracker->velocity[1] * tracker->velocity[1]);
|
||||
tracker->distance += tracker->speed * dt;
|
||||
}
|
||||
|
||||
|
||||
// 传感器数据采集与处理任务
|
||||
void sensor_processing_task(signed short * acc_data_buf, signed short * gyr_data_buf) {
|
||||
static skiing_tracker_t my_skiing_tracker;
|
||||
static int initialized = 0;
|
||||
static int calibration_done = 0;
|
||||
|
||||
static signed short combined_raw_data[6];
|
||||
static float final_angle_data[3]; // 计算得到的欧若拉角
|
||||
static float calibrated_acc_g[3]; // 转换后的加速度计数据
|
||||
static float calibrated_gyr_dps[3]; // 转换后的陀螺仪数据
|
||||
|
||||
const float delta_time = 0.01f;
|
||||
|
||||
if (!initialized) {
|
||||
skiing_tracker_init(&my_skiing_tracker);
|
||||
initialized = 1;
|
||||
printf("Skiing Tracker Initialized. Waiting for sensor calibration...\n");
|
||||
}
|
||||
|
||||
memcpy(&combined_raw_data[0], acc_data_buf, 3 * sizeof(signed short));
|
||||
memcpy(&combined_raw_data[3], gyr_data_buf, 3 * sizeof(signed short));
|
||||
|
||||
unsigned char status;
|
||||
if (!calibration_done) { //第1次启动，开启零漂检测
|
||||
status = SL_SC7U22_Angle_Output(1, combined_raw_data, final_angle_data, 0);
|
||||
if (status == 1) {
|
||||
calibration_done = 1;
|
||||
printf("Sensor calibration successful! Skiing mode is active.\n");
|
||||
}
|
||||
} else {
|
||||
status = SL_SC7U22_Angle_Output(0, combined_raw_data, final_angle_data, 0);
|
||||
}
|
||||
|
||||
if (status == 1) {
|
||||
// 加速度 LSB to g
|
||||
calibrated_acc_g[0] = (float)combined_raw_data[0] / 8192.0f;
|
||||
calibrated_acc_g[1] = (float)combined_raw_data[1] / 8192.0f;
|
||||
calibrated_acc_g[2] = (float)combined_raw_data[2] / 8192.0f;
|
||||
|
||||
// 陀螺仪 LSB to dps (度/秒)
|
||||
// ±2000dps量程下，转换系数约为 0.061
|
||||
calibrated_gyr_dps[0] = (float)combined_raw_data[3] * 0.061f;
|
||||
calibrated_gyr_dps[1] = (float)combined_raw_data[4] * 0.061f;
|
||||
calibrated_gyr_dps[2] = (float)combined_raw_data[5] * 0.061f;
|
||||
|
||||
skiing_tracker_update(&my_skiing_tracker, calibrated_acc_g, calibrated_gyr_dps, final_angle_data, delta_time);
|
||||
|
||||
// 打印逻辑保持不变
|
||||
static int count = 0;
|
||||
if(count < 10){
|
||||
count++;
|
||||
return;
|
||||
} else {
|
||||
count = 0;
|
||||
}
|
||||
printf("State: %d, Speed: %.2f m/s, Distance: %.2f m\n",
|
||||
my_skiing_tracker.state,
|
||||
my_skiing_tracker.speed,
|
||||
my_skiing_tracker.distance);
|
||||
|
||||
} else if (status == 0) {
|
||||
// printf("Sensor is calibrating...\n");
|
||||
} else {
|
||||
// printf("Angle calculation error or calibration not finished.\n");
|
||||
}
|
||||
}
|
||||
@ -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
|
||||
@ -1,374 +0,0 @@
|
||||
/*
|
||||
动态ZUPT+卡尔曼+巴特沃斯一阶滤波器
|
||||
针对启动滑雪和停止滑雪，设置不同阈值
|
||||
启动滑雪和ZUPT更新的陀螺仪方差阈值分开设置
|
||||
- 启动滑雪的陀螺仪阈值会更宽松一些
|
||||
原地旋转和ZUPT更新的加速度方差阈值分开设置
|
||||
- 原地旋转的加速度阈值更宽松
|
||||
能够从静止状态到变化状态，去根据阈值来判断这个“变化”：进入滑行状态 / 只是原地摆动
|
||||
- 但是还是不够灵敏
|
||||
|
||||
*/
|
||||
#include "skiing_tracker.h"
|
||||
#include "../sensor/SC7U22.h"
|
||||
#include <math.h>
|
||||
#include <string.h>
|
||||
|
||||
#define G_ACCELERATION 9.81f
|
||||
#define DEG_TO_RAD (3.14159265f / 180.0f)
|
||||
|
||||
// --- 算法阈值定义 ---
|
||||
//两个判断是否静止的必要条件：动态零速更新(ZUPT)阈值
|
||||
// 加速方差阈值，提高阈值，让“刹车”更灵敏，以便在波浪式前进等慢速漂移时也能触发零速更新
|
||||
#define ZUPT_ACC_VARIANCE_THRESHOLD 0.2f
|
||||
// 陀螺仪方差阈值
|
||||
#define ZUPT_GYR_VARIANCE_THRESHOLD 5.0f
|
||||
|
||||
// 用于原地旋转判断的加速度方差阈值。此值比ZUPT阈值更宽松，
|
||||
// 以允许原地旋转时身体的正常晃动，但仍能与真实滑行时的剧烈加速度变化区分开。
|
||||
#define ROTATING_ACC_VARIANCE_THRESHOLD 0.8f
|
||||
// 用于启动滑雪判断的陀螺仪方差阈值。此值比ZUPT阈值更宽松，
|
||||
// 以允许启动瞬间的正常抖动，但仍能过滤掉混乱的、非滑雪的晃动。
|
||||
#define SKIING_GYR_VARIANCE_THRESHOLD 15.0f
|
||||
|
||||
|
||||
// 旋转/摆动检测阈值：角速度总模长大于此值(度/秒)，认为正在进行非滑雪的旋转或摆动
|
||||
#define ROTATION_GYR_MAG_THRESHOLD 45.0f
|
||||
// 启动滑雪阈值：加速度模长与重力的差值大于此值，认为开始运动
|
||||
// 降低阈值，让“油门”更灵敏，以便能捕捉到真实的慢速启动
|
||||
#define START_SKIING_ACC_THRESHOLD 0.5f
|
||||
|
||||
// --- 用于消除积分漂移的滤波器和阈值 ---
|
||||
// 高通滤波器系数 (alpha)。alpha 越接近1，滤除低频(直流偏移)的效果越强，但可能滤掉真实的慢速运动。
|
||||
// alpha = RC / (RC + dt)，
|
||||
#define HPF_ALPHA 0.95f // 换算大概就是衰减频率低于约 0.84 Hz 的信号
|
||||
// 任何比“大约1秒钟变化一次”还要慢的运动，其加速度信号也会被部分衰减。
|
||||
// 而滑雪时的快速转弯、加减速等动作，其频率远高于 0.84 Hz，它们的信号会被保留下来。
|
||||
// 加速度死区阈值 (m/s^2)。低于此阈值的加速度被认为是噪声，不参与积分。
|
||||
// 设得太高会忽略真实的慢速启动，设得太低则无法有效抑制噪声。
|
||||
#define ACC_DEAD_ZONE_THRESHOLD 0.15f
|
||||
|
||||
// --- 模拟摩擦力，进行速度衰减 ---
|
||||
#define SPEED_ATTENUATION 0.98f
|
||||
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////////////////////////////
|
||||
//实现
|
||||
/**
|
||||
* @brief 初始化滑雪追踪器
|
||||
*
|
||||
* @param tracker
|
||||
*/
|
||||
void skiing_tracker_init(skiing_tracker_t *tracker)
|
||||
{
|
||||
if (!tracker) {
|
||||
return;
|
||||
}
|
||||
// 使用memset一次性清零整个结构体，包括新增的缓冲区
|
||||
memset(tracker, 0, sizeof(skiing_tracker_t));
|
||||
tracker->state = SKIING_STATE_STATIC;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 将设备坐标系下的加速度转换为世界坐标系
|
||||
* @param acc_device 设备坐标系下的加速度 [x, y, z]
|
||||
* @param angle 姿态角 [pitch, roll, yaw]，单位: 度
|
||||
* @param acc_world 输出：世界坐标系下的加速度 [x, y, z]
|
||||
*/
|
||||
static void transform_acc_to_world_frame(const float *acc_device, const float *angle, float *acc_world)
|
||||
{
|
||||
// 驱动输出的角度与标准航空定义相反，需要取反才能用于标准旋转矩阵。
|
||||
float pitch = -angle[0] * DEG_TO_RAD;
|
||||
float roll = -angle[1] * DEG_TO_RAD;
|
||||
|
||||
// TODO: 当引入三轴磁力计后，这里的 yaw 应由磁力计和陀螺仪融合解算得出，以解决航向漂移问题。
|
||||
// 目前 yaw 暂时不参与计算，因为仅靠加速度计和陀螺仪无法获得准确的绝对航向角。
|
||||
// float yaw = -angle[2] * DEG_TO_RAD;
|
||||
|
||||
float cp = cosf(pitch);
|
||||
float sp = sinf(pitch);
|
||||
float cr = cosf(roll);
|
||||
float sr = sinf(roll);
|
||||
|
||||
float ax = acc_device[0];
|
||||
float ay = acc_device[1];
|
||||
float az = acc_device[2];
|
||||
|
||||
// 使用经过验证的、正确的身体坐标系到世界坐标系的旋转矩阵 (基于 Y-X 旋转顺序)
|
||||
// 这个矩阵将设备测量的加速度(ax, ay, az)正确地转换到世界坐标系(acc_world)。
|
||||
// 注意：这里没有使用yaw，主要关心的是坡面上的运动，绝对航向暂时不影响速度和距离的计算。
|
||||
// TODO
|
||||
acc_world[0] = cp * ax + sp * sr * ay + sp * cr * az;
|
||||
acc_world[1] = 0 * ax + cr * ay - sr * az;
|
||||
acc_world[2] = -sp * ax + cp * sr * ay + cp * cr * az;
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* @brief 计算缓冲区内三轴数据的方差之和
|
||||
*
|
||||
* @param buffer 传进来的三轴数据：陀螺仪/加速度
|
||||
* @return float 返回方差和
|
||||
*/
|
||||
static float calculate_variance(float buffer[VARIANCE_BUFFER_SIZE][3])
|
||||
{
|
||||
float mean[3] = {0};
|
||||
float variance[3] = {0};
|
||||
|
||||
// 计算均值
|
||||
for (int i = 0; i < VARIANCE_BUFFER_SIZE; i++) {
|
||||
mean[0] += buffer[i][0];
|
||||
mean[1] += buffer[i][1];
|
||||
mean[2] += buffer[i][2];
|
||||
}
|
||||
mean[0] /= VARIANCE_BUFFER_SIZE;
|
||||
mean[1] /= VARIANCE_BUFFER_SIZE;
|
||||
mean[2] /= VARIANCE_BUFFER_SIZE;
|
||||
|
||||
// 计算方差
|
||||
for (int i = 0; i < VARIANCE_BUFFER_SIZE; i++) {
|
||||
variance[0] += (buffer[i][0] - mean[0]) * (buffer[i][0] - mean[0]);
|
||||
variance[1] += (buffer[i][1] - mean[1]) * (buffer[i][1] - mean[1]);
|
||||
variance[2] += (buffer[i][2] - mean[2]) * (buffer[i][2] - mean[2]);
|
||||
}
|
||||
variance[0] /= VARIANCE_BUFFER_SIZE;
|
||||
variance[1] /= VARIANCE_BUFFER_SIZE;
|
||||
variance[2] /= VARIANCE_BUFFER_SIZE;
|
||||
|
||||
// 返回三轴方差之和，作为一个综合的稳定度指标
|
||||
return variance[0] + variance[1] + variance[2];
|
||||
}
|
||||
|
||||
|
||||
|
||||
/**
|
||||
* @brief 状态机更新
|
||||
*
|
||||
* @param tracker
|
||||
* @param acc_device_ms2 三轴加速度，m/s^2
|
||||
* @param gyr_dps 三轴陀螺仪，dps
|
||||
*/
|
||||
static void update_state_machine(skiing_tracker_t *tracker, const float *acc_device_ms2, const float *gyr_dps)
|
||||
{
|
||||
// 缓冲区未填满时，不进行状态判断，默认为静止
|
||||
if (!tracker->buffer_filled) {
|
||||
tracker->state = SKIING_STATE_STATIC;
|
||||
return;
|
||||
}
|
||||
|
||||
// --- 计算关键指标 ---
|
||||
float acc_variance = calculate_variance(tracker->acc_buffer); // 计算加速度方差
|
||||
float gyr_variance = calculate_variance(tracker->gyr_buffer); // 计算陀螺仪方差
|
||||
float gyr_magnitude = sqrtf(gyr_dps[0]*gyr_dps[0] + gyr_dps[1]*gyr_dps[1] + gyr_dps[2]*gyr_dps[2]);
|
||||
float acc_magnitude = sqrtf(acc_device_ms2[0]*acc_device_ms2[0] + acc_device_ms2[1]*acc_device_ms2[1] + acc_device_ms2[2]*acc_device_ms2[2]);
|
||||
|
||||
|
||||
// --- 状态切换逻辑 (按优先级) ---
|
||||
|
||||
// 优先级1：动态零速更新 (ZUPT) - 最严格和最优先的“刹车”
|
||||
if (acc_variance < ZUPT_ACC_VARIANCE_THRESHOLD && gyr_variance < ZUPT_GYR_VARIANCE_THRESHOLD) {
|
||||
tracker->state = SKIING_STATE_STATIC;
|
||||
// 速度清零，抑制漂移
|
||||
memset(tracker->velocity, 0, sizeof(tracker->velocity));
|
||||
tracker->speed = 0.0f;
|
||||
// 关键：当检测到静止时，必须重置高通滤波器的状态
|
||||
memset(tracker->acc_world_unfiltered_prev, 0, sizeof(tracker->acc_world_unfiltered_prev));
|
||||
memset(tracker->acc_world_filtered, 0, sizeof(tracker->acc_world_filtered));
|
||||
return;
|
||||
}
|
||||
|
||||
// 优先级2：原地旋转 - 特殊的、非滑雪的运动状态
|
||||
// 条件：角速度很大，同时线性加速度的晃动在一个“中等”范围内。
|
||||
if (gyr_magnitude > ROTATION_GYR_MAG_THRESHOLD && acc_variance < ROTATING_ACC_VARIANCE_THRESHOLD) {
|
||||
tracker->state = SKIING_STATE_ROTATING;
|
||||
return;
|
||||
}
|
||||
|
||||
// 优先级3：启动滑雪 - “油门”
|
||||
// 条件：有足够大的线性加速度，同时陀螺仪的抖动在一个“合理”（而非“完全静止”）的范围内。
|
||||
if (fabsf(acc_magnitude - G_ACCELERATION) > START_SKIING_ACC_THRESHOLD && gyr_variance < SKIING_GYR_VARIANCE_THRESHOLD) {
|
||||
tracker->state = SKIING_STATE_SKIING;
|
||||
return;
|
||||
}
|
||||
|
||||
// 如果不满足任何启动或停止条件，则保持当前状态（滑雪中）
|
||||
// 如果当前是静止或旋转，但没有满足启动条件，则状态会保持，直到满足ZUPT或旋转条件。
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* @brief 主更新函数
|
||||
*
|
||||
* @param tracker
|
||||
* @param acc_g 三轴加速度，g
|
||||
* @param gyr_dps 三轴陀螺仪，dps
|
||||
* @param angle 欧若拉角
|
||||
* @param dt 采样时间间隔，会用来积分求速度
|
||||
*/
|
||||
void skiing_tracker_update(skiing_tracker_t *tracker, float *acc_g, float *gyr_dps, float *angle, float dt)
|
||||
{
|
||||
if (!tracker || !acc_g || !gyr_dps || !angle || dt <= 0) {
|
||||
return;
|
||||
}
|
||||
|
||||
// --- 数据预处理和缓冲 ---
|
||||
float acc_device_ms2[3];
|
||||
acc_device_ms2[0] = acc_g[0] * G_ACCELERATION;
|
||||
acc_device_ms2[1] = acc_g[1] * G_ACCELERATION;
|
||||
acc_device_ms2[2] = acc_g[2] * G_ACCELERATION;
|
||||
|
||||
// 将最新数据存入缓冲区
|
||||
memcpy(tracker->acc_buffer[tracker->buffer_index], acc_device_ms2, sizeof(acc_device_ms2));
|
||||
memcpy(tracker->gyr_buffer[tracker->buffer_index], gyr_dps, 3 * sizeof(float));
|
||||
|
||||
tracker->buffer_index++;
|
||||
if (tracker->buffer_index >= VARIANCE_BUFFER_SIZE) {
|
||||
tracker->buffer_index = 0;
|
||||
tracker->buffer_filled = 1; // 标记缓冲区已满
|
||||
}
|
||||
|
||||
// --- 更新状态机 ---
|
||||
update_state_machine(tracker, acc_device_ms2, gyr_dps);
|
||||
|
||||
// --- 根据状态进行计算 ---
|
||||
if (tracker->state == SKIING_STATE_SKIING) {
|
||||
// 坐标转换 & 移除重力
|
||||
transform_acc_to_world_frame(acc_device_ms2, angle, tracker->acc_world);
|
||||
tracker->acc_world[2] -= G_ACCELERATION;
|
||||
|
||||
// 对世界坐标系下的加速度进行高通滤波，消除直流偏置和重力残差
|
||||
for (int i = 0; i < 3; i++) {
|
||||
tracker->acc_world_filtered[i] = HPF_ALPHA * (tracker->acc_world_filtered[i] + tracker->acc_world[i] - tracker->acc_world_unfiltered_prev[i]);
|
||||
tracker->acc_world_unfiltered_prev[i] = tracker->acc_world[i];
|
||||
}
|
||||
|
||||
// 应用加速度死区，忽略微小抖动和噪声
|
||||
float acc_horizontal_mag = sqrtf(tracker->acc_world_filtered[0] * tracker->acc_world_filtered[0] +
|
||||
tracker->acc_world_filtered[1] * tracker->acc_world_filtered[1]);
|
||||
|
||||
if (acc_horizontal_mag > ACC_DEAD_ZONE_THRESHOLD) {
|
||||
// 只有当水平加速度足够大时，才进行速度积分
|
||||
tracker->velocity[0] += tracker->acc_world_filtered[0] * dt;
|
||||
tracker->velocity[1] += tracker->acc_world_filtered[1] * dt;
|
||||
// 垂直方向的速度暂时不积分，极易受姿态误差影响而漂移
|
||||
// tracker->velocity[2] += tracker->acc_world_filtered[2] * dt;
|
||||
}
|
||||
// 如果加速度小于阈值，则不更新速度，相当于速度保持不变（或受下一步的阻尼影响而衰减）
|
||||
|
||||
} else {
|
||||
// 在静止或旋转状态下，速度已经在状态机内部被清零
|
||||
// 额外增加速度衰减，模拟摩擦力，进一步抑制漂移
|
||||
tracker->velocity[0] *= SPEED_ATTENUATION;
|
||||
tracker->velocity[1] *= SPEED_ATTENUATION;
|
||||
tracker->velocity[2] = 0; // 垂直速度强制归零
|
||||
}
|
||||
|
||||
// --- 更新速率和距离 ---
|
||||
// 只基于水平速度计算速率和距离
|
||||
tracker->speed = sqrtf(tracker->velocity[0] * tracker->velocity[0] +
|
||||
tracker->velocity[1] * tracker->velocity[1]);
|
||||
tracker->distance += tracker->speed * dt;
|
||||
}
|
||||
|
||||
|
||||
|
||||
/**
|
||||
* @brief 传感器数据采集与处理任务，外部每10ms调用一次，如果需要更新时间间隔，也需要同步更新宏“ DELTA_TIME ”
|
||||
*
|
||||
* @param acc_data_buf 三轴加速度原始数据
|
||||
* @param gyr_data_buf 三轴陀螺仪原始数据
|
||||
* @return BLE_send_data_t
|
||||
*/
|
||||
BLE_send_data_t sensor_processing_task(signed short * acc_data_buf, signed short * gyr_data_buf) {
|
||||
static skiing_tracker_t my_skiing_tracker;
|
||||
static int initialized = 0;
|
||||
static int calibration_done = 0;
|
||||
|
||||
static signed short combined_raw_data[6];
|
||||
static float final_angle_data[3]; // 计算得到的欧若拉角
|
||||
static float calibrated_acc_g[3]; // 转换后的加速度计数据
|
||||
static float calibrated_gyr_dps[3]; // 转换后的陀螺仪数据
|
||||
|
||||
const float delta_time = DELTA_TIME;
|
||||
BLE_send_data_t BLE_send_data;
|
||||
BLE_KS_send_data_t KS_data;
|
||||
|
||||
if (!initialized) {
|
||||
skiing_tracker_init(&my_skiing_tracker);
|
||||
initialized = 1;
|
||||
printf("Skiing Tracker Initialized. Waiting for sensor calibration...\n");
|
||||
}
|
||||
|
||||
memcpy(&combined_raw_data[0], acc_data_buf, 3 * sizeof(signed short));
|
||||
memcpy(&combined_raw_data[3], gyr_data_buf, 3 * sizeof(signed short));
|
||||
|
||||
unsigned char status;
|
||||
if (!calibration_done) { //第1次启动，开启零漂检测
|
||||
status = SL_SC7U22_Angle_Output(1, combined_raw_data, final_angle_data, 0);
|
||||
if (status == 1) {
|
||||
calibration_done = 1;
|
||||
printf("Sensor calibration successful! Skiing mode is active.\n");
|
||||
}
|
||||
} else {
|
||||
status = SL_SC7U22_Angle_Output(0, combined_raw_data, final_angle_data, 0);
|
||||
}
|
||||
|
||||
if (status == 1) {
|
||||
// 加速度 LSB to g
|
||||
calibrated_acc_g[0] = (float)combined_raw_data[0] / 8192.0f;
|
||||
calibrated_acc_g[1] = (float)combined_raw_data[1] / 8192.0f;
|
||||
calibrated_acc_g[2] = (float)combined_raw_data[2] / 8192.0f;
|
||||
|
||||
// 陀螺仪 LSB to dps (度/秒)
|
||||
// ±2000dps量程下，转换系数约为 0.061
|
||||
calibrated_gyr_dps[0] = (float)combined_raw_data[3] * 0.061f;
|
||||
calibrated_gyr_dps[1] = (float)combined_raw_data[4] * 0.061f;
|
||||
calibrated_gyr_dps[2] = (float)combined_raw_data[5] * 0.061f;
|
||||
|
||||
skiing_tracker_update(&my_skiing_tracker, calibrated_acc_g, calibrated_gyr_dps, final_angle_data, delta_time);
|
||||
|
||||
// 打印逻辑保持不变
|
||||
// static int count = 0;
|
||||
// if(count >= 10){
|
||||
// printf("State: %d, Speed: %.2f m/s, Distance: %.2f m\n",
|
||||
// my_skiing_tracker.state,
|
||||
// my_skiing_tracker.speed,
|
||||
// my_skiing_tracker.distance);
|
||||
// count = 0;
|
||||
// } else {
|
||||
// count++;
|
||||
// }
|
||||
|
||||
BLE_send_data.sensor_state = status;
|
||||
BLE_send_data.skiing_state = my_skiing_tracker.state;
|
||||
for (int i = 0; i < 3; i++) {
|
||||
#ifndef XTELL_TEST
|
||||
BLE_send_data.acc_original[i] = (int)acc_data_buf[i];
|
||||
BLE_send_data.gyr_original[i] = (int)gyr_data_buf[i];
|
||||
#endif
|
||||
#if KS_BLE
|
||||
KS_data.acc_KS_g[i] = (int)calibrated_acc_g[i];
|
||||
KS_data.gyr_KS_dps[i] = (int)calibrated_gyr_dps[i];
|
||||
KS_data.angle_KS[i] = (int)final_angle_data[i];
|
||||
#endif
|
||||
}
|
||||
BLE_send_data.speed_cms = (int)(my_skiing_tracker.speed * 100);
|
||||
BLE_send_data.distance_cm = (int)(my_skiing_tracker.distance * 100);
|
||||
|
||||
} else if (status == 0) {
|
||||
memset(&BLE_send_data, 0, sizeof(BLE_send_data_t));
|
||||
BLE_send_data.sensor_state = status;
|
||||
#if KS_BLE
|
||||
memset(&KS_data, 0, sizeof(BLE_send_data_t));
|
||||
#endif
|
||||
// printf("Sensor is calibrating...\n");
|
||||
} else {
|
||||
memset(&BLE_send_data, 0, sizeof(BLE_send_data_t));
|
||||
BLE_send_data.sensor_state = status;
|
||||
#if KS_BLE
|
||||
memset(&KS_data, 0, sizeof(BLE_send_data_t));
|
||||
#endif
|
||||
// printf("Angle calculation error or calibration not finished.\n");
|
||||
}
|
||||
return BLE_send_data;
|
||||
}
|
||||
@ -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
|
||||
@ -1,276 +0,0 @@
|
||||
/*
|
||||
简化的滑雪追踪器：
|
||||
- 直接读取六轴数据
|
||||
- 分离重力分量
|
||||
- 直接积分求速度和距离
|
||||
*/
|
||||
#include "skiing_tracker.h"
|
||||
#include "../sensor/SC7U22.h"
|
||||
#include <math.h>
|
||||
#include <string.h>
|
||||
|
||||
#define ENABLE_XLOG 1
|
||||
#ifdef xlog
|
||||
#undef xlog
|
||||
#endif
|
||||
#if ENABLE_XLOG
|
||||
#define xlog(format, ...) printf("[XT:%s] " format, __func__, ##__VA_ARGS__)
|
||||
#else
|
||||
#define xlog(format, ...) ((void)0)
|
||||
#endif
|
||||
|
||||
#define G_ACCELERATION 9.81f
|
||||
#define DEG_TO_RAD (3.14159265f / 180.0f)
|
||||
|
||||
// 运动检测阈值 (m/s^2)。当水平加速度的模长大于此值时，认为处于运动状态。
|
||||
#define MOTION_THRESHOLD 0.2f
|
||||
|
||||
static skiing_tracker_t my_skiing_tracker;
|
||||
#ifdef XTELL_TEST
|
||||
BLE_KS_send_data_t KS_data;
|
||||
debug_t debug1;
|
||||
debug_t debug2;
|
||||
#endif
|
||||
//////////////////////////////////////////////////////////////////////////////////////////////////
|
||||
//实现
|
||||
/**
|
||||
* @brief 初始化滑雪追踪器
|
||||
*
|
||||
* @param tracker
|
||||
*/
|
||||
void skiing_tracker_init(skiing_tracker_t *tracker)
|
||||
{
|
||||
if (!tracker) {
|
||||
return;
|
||||
}
|
||||
memset(tracker, 0, sizeof(skiing_tracker_t));
|
||||
tracker->state = SKIING_STATE_STATIC;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 将设备坐标系下的加速度转换为世界坐标系
|
||||
* @param acc_device 设备坐标系下的加速度 [x, y, z]
|
||||
* @param angle 姿态角 [pitch, roll, yaw]，单位: 度
|
||||
* @param acc_world 输出：世界坐标系下的加速度 [x, y, z]
|
||||
*/
|
||||
static void transform_acc_to_world_frame(const float *acc_device, const float *angle, float *acc_world)
|
||||
{
|
||||
// 驱动输出的角度与标准航空定义相反，需要取反才能用于标准旋转矩阵。
|
||||
float pitch = -angle[0] * DEG_TO_RAD;
|
||||
float roll = -angle[1] * DEG_TO_RAD;
|
||||
|
||||
float cp = cosf(pitch);
|
||||
float sp = sinf(pitch);
|
||||
float cr = cosf(roll);
|
||||
float sr = sinf(roll);
|
||||
|
||||
float ax = acc_device[0];
|
||||
float ay = acc_device[1];
|
||||
float az = acc_device[2];
|
||||
|
||||
// 使用经过验证的、正确的身体坐标系到世界坐标系的旋转矩阵 (基于 Y-X 旋转顺序)
|
||||
acc_world[0] = cp * ax + sp * sr * ay + sp * cr * az;
|
||||
acc_world[1] = 0 * ax + cr * ay - sr * az;
|
||||
acc_world[2] = -sp * ax + cp * sr * ay + cp * cr * az;
|
||||
}
|
||||
|
||||
void clear_speed(void){
|
||||
my_skiing_tracker.state = SKIING_STATE_STATIC;
|
||||
memset(my_skiing_tracker.velocity, 0, sizeof(my_skiing_tracker.velocity));
|
||||
my_skiing_tracker.speed = 0;
|
||||
}
|
||||
|
||||
void start_detection(void){
|
||||
my_skiing_tracker.state = SKIING_STATE_STATIC;
|
||||
memset(my_skiing_tracker.velocity, 0, sizeof(my_skiing_tracker.velocity));
|
||||
my_skiing_tracker.distance = 0;
|
||||
my_skiing_tracker.speed = 0;
|
||||
}
|
||||
|
||||
void stop_detection(void){
|
||||
my_skiing_tracker.state = SKINNG_STOP_DETECTION;
|
||||
memset(my_skiing_tracker.velocity, 0, sizeof(my_skiing_tracker.velocity));
|
||||
my_skiing_tracker.speed = 0;
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* @brief 主更新函数
|
||||
*
|
||||
* @param tracker
|
||||
* @param acc_g 三轴加速度，g
|
||||
* @param gyr_dps 三轴陀螺仪，dps (当前简化版本未使用)
|
||||
* @param angle 欧拉角
|
||||
* @param dt 采样时间间隔，会用来积分求速度
|
||||
*/
|
||||
void skiing_tracker_update(skiing_tracker_t *tracker, float *acc_g, float *gyr_dps, float *angle, float dt)
|
||||
{
|
||||
if (!tracker || !acc_g || !angle || dt <= 0) {
|
||||
return;
|
||||
}
|
||||
//停止检测
|
||||
if(my_skiing_tracker.state == SKINNG_STOP_DETECTION)
|
||||
return;
|
||||
|
||||
// --- 数据预处理 ---
|
||||
float acc_device_ms2[3];
|
||||
acc_device_ms2[0] = acc_g[0] * G_ACCELERATION;
|
||||
acc_device_ms2[1] = acc_g[1] * G_ACCELERATION;
|
||||
acc_device_ms2[2] = acc_g[2] * G_ACCELERATION;
|
||||
|
||||
// --- 坐标转换 & 移除重力 ---
|
||||
// 1. 将设备坐标系下的总加速度转换到世界坐标系
|
||||
transform_acc_to_world_frame(acc_device_ms2, angle, tracker->acc_world);
|
||||
// 2. 在世界坐标系下，减去Z轴的重力分量，得到线性加速度
|
||||
tracker->acc_world[2] -= G_ACCELERATION;
|
||||
|
||||
// --- 运动状态判断与计算 ---
|
||||
// 计算水平方向的线性加速度模长
|
||||
float acc_horizontal_mag = sqrtf(tracker->acc_world[0] * tracker->acc_world[0] +
|
||||
tracker->acc_world[1] * tracker->acc_world[1]);
|
||||
|
||||
if (acc_horizontal_mag > MOTION_THRESHOLD) {
|
||||
// 加速
|
||||
tracker->state = SKIING_STATE_SKIING;
|
||||
|
||||
// 积分计算速度（只考虑水平方向，避免垂直方向漂移）
|
||||
tracker->velocity[0] += tracker->acc_world[0] * dt;
|
||||
tracker->velocity[1] += tracker->acc_world[1] * dt;
|
||||
tracker->velocity[2] = 0; // 垂直速度强制为0
|
||||
|
||||
// --- 更新速率和距离 ---
|
||||
// 只基于水平速度计算速率
|
||||
tracker->speed = sqrtf(tracker->velocity[0] * tracker->velocity[0] +
|
||||
tracker->velocity[1] * tracker->velocity[1]);
|
||||
// 累加距离
|
||||
tracker->distance += tracker->speed * dt;
|
||||
|
||||
return;
|
||||
|
||||
} else {
|
||||
//匀速
|
||||
tracker->distance += tracker->speed * dt;
|
||||
|
||||
return;
|
||||
}
|
||||
|
||||
|
||||
}
|
||||
|
||||
// 获取当前 RTC 时间戳
|
||||
time_t get_rtc_timestamp(void) {
|
||||
static void *rtc_hdl = NULL;
|
||||
struct sys_time rtc_time = {0};
|
||||
dev_ioctl(rtc_hdl, IOCTL_GET_SYS_TIME, (u32)&rtc_time);
|
||||
|
||||
return rtc_sys_time_to_timestamp(&rtc_time);
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 传感器数据采集与处理任务，外部每10ms调用一次，如果需要更新时间间隔，也需要同步更新宏“ DELTA_TIME ”
|
||||
*
|
||||
* @param acc_data_buf 三轴加速度原始数据
|
||||
* @param gyr_data_buf 三轴陀螺仪原始数据
|
||||
* @return BLE_send_data_t
|
||||
*/
|
||||
BLE_send_data_t sensor_processing_task(signed short * acc_data_buf, signed short * gyr_data_buf) {
|
||||
|
||||
static int initialized = 0;
|
||||
static int calibration_done = 0;
|
||||
|
||||
static signed short combined_raw_data[6];
|
||||
static float final_angle_data[3]; // 计算得到的欧若拉角
|
||||
static float calibrated_acc_g[3]; // 转换后的加速度计数据
|
||||
static float calibrated_gyr_dps[3]; // 转换后的陀螺仪数据
|
||||
|
||||
float delta_time = DELTA_TIME + 0.03f; //观察得到实际上调用间隔会多30ms
|
||||
BLE_send_data_t BLE_send_data;
|
||||
|
||||
// xlog("===========start\n");
|
||||
|
||||
if (!initialized) {
|
||||
skiing_tracker_init(&my_skiing_tracker);
|
||||
initialized = 1;
|
||||
printf("Skiing Tracker Initialized. Waiting for sensor calibration...\n");
|
||||
}
|
||||
|
||||
memcpy(&combined_raw_data[0], acc_data_buf, 3 * sizeof(signed short));
|
||||
memcpy(&combined_raw_data[3], gyr_data_buf, 3 * sizeof(signed short));
|
||||
|
||||
unsigned char status;
|
||||
if (!calibration_done) { //第1次启动，开启零漂检测
|
||||
status = SL_SC7U22_Angle_Output(1, combined_raw_data, final_angle_data, 0);
|
||||
if (status == 1) {
|
||||
calibration_done = 1;
|
||||
printf("Sensor calibration successful! Skiing mode is active.\n");
|
||||
}
|
||||
} else {
|
||||
// printf("Calculate the time interval =============== start\n");
|
||||
status = SL_SC7U22_Angle_Output(0, combined_raw_data, final_angle_data, 0);
|
||||
// printf("Calculate the time interval =============== end\n");
|
||||
}
|
||||
|
||||
if (status == 1) {
|
||||
// 加速度 LSB to g
|
||||
calibrated_acc_g[0] = (float)combined_raw_data[0] / 8192.0f;
|
||||
calibrated_acc_g[1] = (float)combined_raw_data[1] / 8192.0f;
|
||||
calibrated_acc_g[2] = (float)combined_raw_data[2] / 8192.0f;
|
||||
|
||||
// 陀螺仪 LSB to dps (度/秒)
|
||||
// ±2000dps量程下，转换系数约为 0.061
|
||||
calibrated_gyr_dps[0] = (float)combined_raw_data[3] * 0.061f;
|
||||
calibrated_gyr_dps[1] = (float)combined_raw_data[4] * 0.061f;
|
||||
calibrated_gyr_dps[2] = (float)combined_raw_data[5] * 0.061f;
|
||||
|
||||
skiing_tracker_update(&my_skiing_tracker, calibrated_acc_g, calibrated_gyr_dps, final_angle_data, delta_time);
|
||||
|
||||
// static int count = 0;
|
||||
// if(count >= 10){
|
||||
// printf("State: %d, Speed: %.2f m/s, Distance: %.2f m\n",
|
||||
// my_skiing_tracker.state,
|
||||
// my_skiing_tracker.speed,
|
||||
// my_skiing_tracker.distance);
|
||||
// printf("calibrated_acc_g: %.2f, %.2f, %.2f\n",
|
||||
// calibrated_acc_g[0],
|
||||
// calibrated_acc_g[1],
|
||||
// calibrated_acc_g[2]);
|
||||
// count = 0;
|
||||
// } else {
|
||||
// count++;
|
||||
// }
|
||||
|
||||
BLE_send_data.sensor_state = status;
|
||||
BLE_send_data.skiing_state = my_skiing_tracker.state;
|
||||
for (int i = 0; i < 3; i++) {
|
||||
#ifndef XTELL_TEST
|
||||
BLE_send_data.acc_original[i] = (int)acc_data_buf[i];
|
||||
BLE_send_data.gyr_original[i] = (int)gyr_data_buf[i];
|
||||
#endif
|
||||
#if KS_BLE
|
||||
KS_data.acc_KS[i] = (int)(calibrated_acc_g[i] * G_ACCELERATION * 100); //cm/s^s
|
||||
KS_data.gyr_KS_dps[i] = (int)calibrated_gyr_dps[i];
|
||||
KS_data.angle_KS[i] = (int)final_angle_data[i];
|
||||
#endif
|
||||
}
|
||||
BLE_send_data.speed_cms = (int)(my_skiing_tracker.speed * 100);
|
||||
BLE_send_data.distance_cm = (int)(my_skiing_tracker.distance * 100);
|
||||
//
|
||||
} else if (status == 0) {
|
||||
memset(&BLE_send_data, 0, sizeof(BLE_send_data_t));
|
||||
BLE_send_data.sensor_state = status;
|
||||
#if KS_BLE
|
||||
memset(&KS_data, 0, sizeof(BLE_send_data_t));
|
||||
#endif
|
||||
// printf("Sensor is calibrating...\n");
|
||||
} else {
|
||||
memset(&BLE_send_data, 0, sizeof(BLE_send_data_t));
|
||||
BLE_send_data.sensor_state = status;
|
||||
#if KS_BLE
|
||||
memset(&KS_data, 0, sizeof(BLE_send_data_t));
|
||||
#endif
|
||||
// printf("Angle calculation error or calibration not finished.\n");
|
||||
}
|
||||
|
||||
// xlog("end++++++++++++++\n");
|
||||
return BLE_send_data;
|
||||
}
|
||||
@ -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
|
||||
@ -1,501 +0,0 @@
|
||||
/*
|
||||
虽然sensor_processing_task是10ms调用一次
|
||||
但是实际上上一次调用该函数的时间点和下一次调用该函数的时间点，会相差40ms
|
||||
|
||||
*/
|
||||
#include "skiing_tracker.h"
|
||||
#include "../sensor/SC7U22.h"
|
||||
#include <math.h>
|
||||
#include <string.h>
|
||||
|
||||
#define G_ACCELERATION 9.81f
|
||||
#define DEG_TO_RAD (3.14159265f / 180.0f)
|
||||
|
||||
// --- ZUPT ---
|
||||
//两个判断是否静止的必要条件：动态零速更新(ZUPT)阈值
|
||||
// 加速方差阈值，提高阈值，让“刹车”更灵敏，以便在波浪式前进等慢速漂移时也能触发零速更新
|
||||
#define ZUPT_ACC_VARIANCE_THRESHOLD 0.2f
|
||||
// 陀螺仪方差阈值
|
||||
#define ZUPT_GYR_VARIANCE_THRESHOLD 5.0f
|
||||
|
||||
// --- 启动滑雪阈值 ---
|
||||
// 加速度模长与重力的差值大于此值，认为开始运动；降低阈值，让“油门”更灵敏，以便能捕捉到真实的慢速启动
|
||||
#define START_SKIING_ACC_THRESHOLD 0.5f
|
||||
// 陀螺仪方差阈值，以允许启动瞬间的正常抖动，但仍能过滤掉混乱的、非滑雪的晃动。
|
||||
#define SKIING_GYR_VARIANCE_THRESHOLD 15.0f
|
||||
|
||||
// --- 滑雪过程 ---
|
||||
//加速度 模长，低于此值视为 在做匀速运动
|
||||
#define SKIING_ACC_MAG_THRESHOLD 0.5f
|
||||
//陀螺仪 模长，高于此值视为 摔倒了
|
||||
#define FALLEN_GRY_MAG_THRESHOLD 1000.0f //未确定
|
||||
|
||||
// --- 原地旋转抖动 ---
|
||||
// 用于原地旋转判断的加速度方差阈值。此值比ZUPT阈值更宽松，
|
||||
// 以允许原地旋转时身体的正常晃动，但仍能与真实滑行时的剧烈加速度变化区分开。
|
||||
#define ROTATING_ACC_VARIANCE_THRESHOLD 0.8f
|
||||
// 旋转/摆动检测阈值：角速度总模长大于此值(度/秒)，认为正在进行非滑雪的旋转或摆动
|
||||
#define ROTATION_GYR_MAG_THRESHOLD 120.0f
|
||||
|
||||
// --- 滑雪转弯动 ---
|
||||
// 加速度方差阈值，大于此值，滑雪过程可能发生了急转弯
|
||||
#define WHEEL_ACC_VARIANCE_THRESHOLD 7.0f
|
||||
// 旋转/摆动检测阈值：角速度总模长大于此值(度/秒)，认为滑雪过程中进行急转弯
|
||||
#define WHEEL_GYR_MAG_THRESHOLD 220.0f // 150.0f 到 250.0f之间进行调整
|
||||
|
||||
// --- 用于消除积分漂移的滤波器和阈值 ---
|
||||
// 高通滤波器系数 (alpha)。alpha 越接近1，滤除低频(直流偏移)的效果越强，但可能滤掉真实的慢速运动。
|
||||
// alpha = RC / (RC + dt)，参考RC电路而来
|
||||
#define HPF_ALPHA 0.995
|
||||
|
||||
// 加速度死区阈值 (m/s^2)。低于此阈值的加速度被认为是噪声，不参与积分。
|
||||
// 设得太高会忽略真实的慢速启动，设得太低则无法有效抑制噪声。
|
||||
#define ACC_DEAD_ZONE_THRESHOLD 0.05f
|
||||
|
||||
// --- 模拟摩擦力，进行速度衰减 ---
|
||||
#define SPEED_ATTENUATION 1.0f //暂不模拟
|
||||
|
||||
#ifdef XTELL_TEST
|
||||
BLE_KS_send_data_t KS_data;
|
||||
debug_t debug1;
|
||||
debug_t debug2;
|
||||
#endif
|
||||
|
||||
static skiing_tracker_t my_skiing_tracker;
|
||||
//////////////////////////////////////////////////////////////////////////////////////////////////
|
||||
//实现
|
||||
|
||||
void clear_speed(void){
|
||||
my_skiing_tracker.state = STATIC;
|
||||
memset(my_skiing_tracker.velocity, 0, sizeof(my_skiing_tracker.velocity));
|
||||
my_skiing_tracker.speed = 0;
|
||||
}
|
||||
|
||||
void start_detection(void){
|
||||
my_skiing_tracker.state = STATIC;
|
||||
memset(my_skiing_tracker.velocity, 0, sizeof(my_skiing_tracker.velocity));
|
||||
my_skiing_tracker.distance = 0;
|
||||
my_skiing_tracker.speed = 0;
|
||||
}
|
||||
|
||||
void stop_detection(void){
|
||||
my_skiing_tracker.state = STOP_DETECTION;
|
||||
memset(my_skiing_tracker.velocity, 0, sizeof(my_skiing_tracker.velocity));
|
||||
my_skiing_tracker.speed = 0;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 初始化滑雪追踪器
|
||||
*
|
||||
* @param tracker
|
||||
*/
|
||||
void skiing_tracker_init(skiing_tracker_t *tracker)
|
||||
{
|
||||
if (!tracker) {
|
||||
return;
|
||||
}
|
||||
// 使用memset一次性清零整个结构体，包括新增的缓冲区
|
||||
memset(tracker, 0, sizeof(skiing_tracker_t));
|
||||
tracker->state = STATIC;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 将设备坐标系下的加速度转换为世界坐标系
|
||||
* @param acc_device 设备坐标系下的加速度 [x, y, z]
|
||||
* @param angle 姿态角 [pitch, roll, yaw]，单位: 度
|
||||
* @param acc_world 输出：世界坐标系下的加速度 [x, y, z]
|
||||
*/
|
||||
static void transform_acc_to_world_frame(const float *acc_device, const float *angle, float *acc_world)
|
||||
{
|
||||
// 驱动输出的角度与标准航空定义相反，需要取反才能用于标准旋转矩阵。
|
||||
float pitch = -angle[0] * DEG_TO_RAD;
|
||||
float roll = -angle[1] * DEG_TO_RAD;
|
||||
|
||||
// TODO: 当引入三轴磁力计后，这里的 yaw 应由磁力计和陀螺仪融合解算得出，以解决航向漂移问题。
|
||||
// 目前 yaw 暂时不参与计算，因为仅靠加速度计和陀螺仪无法获得准确的绝对航向角。
|
||||
// float yaw = -angle[2] * DEG_TO_RAD;
|
||||
|
||||
float cp = cosf(pitch);
|
||||
float sp = sinf(pitch);
|
||||
float cr = cosf(roll);
|
||||
float sr = sinf(roll);
|
||||
|
||||
float ax = acc_device[0];
|
||||
float ay = acc_device[1];
|
||||
float az = acc_device[2];
|
||||
|
||||
// 使用经过验证的、正确的身体坐标系到世界坐标系的旋转矩阵 (基于 Y-X 旋转顺序)
|
||||
// 这个矩阵将设备测量的加速度(ax, ay, az)正确地转换到世界坐标系(acc_world)。
|
||||
// 注意：这里没有使用yaw，主要关心的是坡面上的运动，绝对航向暂时不影响速度和距离的计算。
|
||||
// TODO
|
||||
acc_world[0] = cp * ax + sp * sr * ay + sp * cr * az;
|
||||
acc_world[1] = 0 * ax + cr * ay - sr * az;
|
||||
acc_world[2] = -sp * ax + cp * sr * ay + cp * cr * az;
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* @brief 计算缓冲区内三轴数据的方差之和
|
||||
*
|
||||
* @param buffer 传进来的三轴数据：陀螺仪/加速度
|
||||
* @return float 返回方差和
|
||||
*/
|
||||
static float calculate_variance(float buffer[VARIANCE_BUFFER_SIZE][3])
|
||||
{
|
||||
float mean[3] = {0};
|
||||
float variance[3] = {0};
|
||||
|
||||
// 计算均值
|
||||
for (int i = 0; i < VARIANCE_BUFFER_SIZE; i++) {
|
||||
mean[0] += buffer[i][0];
|
||||
mean[1] += buffer[i][1];
|
||||
mean[2] += buffer[i][2];
|
||||
}
|
||||
mean[0] /= VARIANCE_BUFFER_SIZE;
|
||||
mean[1] /= VARIANCE_BUFFER_SIZE;
|
||||
mean[2] /= VARIANCE_BUFFER_SIZE;
|
||||
|
||||
// 计算方差
|
||||
for (int i = 0; i < VARIANCE_BUFFER_SIZE; i++) {
|
||||
variance[0] += (buffer[i][0] - mean[0]) * (buffer[i][0] - mean[0]);
|
||||
variance[1] += (buffer[i][1] - mean[1]) * (buffer[i][1] - mean[1]);
|
||||
variance[2] += (buffer[i][2] - mean[2]) * (buffer[i][2] - mean[2]);
|
||||
}
|
||||
variance[0] /= VARIANCE_BUFFER_SIZE;
|
||||
variance[1] /= VARIANCE_BUFFER_SIZE;
|
||||
variance[2] /= VARIANCE_BUFFER_SIZE;
|
||||
|
||||
// 返回三轴方差之和，作为一个综合的稳定度指标
|
||||
return variance[0] + variance[1] + variance[2];
|
||||
}
|
||||
|
||||
|
||||
|
||||
/**
|
||||
* @brief 状态机更新
|
||||
*
|
||||
* @param tracker 传入同步修改后传出
|
||||
* @param acc_device_ms2 三轴加速度，m/s^2
|
||||
* @param gyr_dps 三轴陀螺仪，dps
|
||||
*/
|
||||
static void update_state_machine(skiing_tracker_t *tracker, const float *acc_device_ms2, const float *gyr_dps)
|
||||
{
|
||||
// 缓冲区未填满时，不进行状态判断，默认为静止
|
||||
if (!tracker->buffer_filled) {
|
||||
tracker->state = STATIC;
|
||||
return;
|
||||
}
|
||||
|
||||
// --- 计算关键指标 ---
|
||||
float acc_variance = calculate_variance(tracker->acc_buffer); // 计算加速度方差
|
||||
float gyr_variance = calculate_variance(tracker->gyr_buffer); // 计算陀螺仪方差
|
||||
float gyr_magnitude = sqrtf(gyr_dps[0]*gyr_dps[0] + gyr_dps[1]*gyr_dps[1] + gyr_dps[2]*gyr_dps[2]);
|
||||
float acc_magnitude = sqrtf(acc_device_ms2[0]*acc_device_ms2[0] + acc_device_ms2[1]*acc_device_ms2[1] + acc_device_ms2[2]*acc_device_ms2[2]);
|
||||
|
||||
#ifdef XTELL_TEST
|
||||
debug1.acc_variance =acc_variance;
|
||||
debug1.gyr_variance =gyr_variance;
|
||||
debug1.gyr_magnitude=gyr_magnitude;
|
||||
debug1.acc_magnitude=fabsf(acc_magnitude - G_ACCELERATION);
|
||||
#endif
|
||||
|
||||
#if 0
|
||||
//正在滑雪
|
||||
if(tracker->state == NO_CONSTANT_SPEED ) {
|
||||
//摔倒了
|
||||
if(gyr_magnitude > FALLEN_GRY_MAG_THRESHOLD){
|
||||
tracker->state = FALLEN;
|
||||
return;
|
||||
}
|
||||
//可能进入了匀速状态
|
||||
if(gyr_magnitude > SKIING_GYR_MAG_THRESHOLD && acc_magnitude < SKIING_ACC_MAG_THRESHOLD){
|
||||
tracker->state = CONSTANT_SPEED;
|
||||
return;
|
||||
}
|
||||
//急转弯
|
||||
if(gyr_magnitude > WHEEL_GYR_MAG_THRESHOLD && acc_variance > WHEEL_ACC_VARIANCE_THRESHOLD){
|
||||
//TODO：可以考虑清掉速度消除积分带来的漂移
|
||||
tracker->state = WHEEL;
|
||||
return;
|
||||
}
|
||||
}
|
||||
|
||||
// --- 状态切换逻辑 (按优先级) ---
|
||||
// 优先级1：静止
|
||||
if (acc_variance < ZUPT_ACC_VARIANCE_THRESHOLD && gyr_variance < ZUPT_GYR_VARIANCE_THRESHOLD) {
|
||||
tracker->state = STATIC;
|
||||
// 速度清零，抑制漂移
|
||||
memset(tracker->velocity, 0, sizeof(tracker->velocity));
|
||||
tracker->speed = 0.0f;
|
||||
memset(tracker->acc_world_unfiltered_prev, 0, sizeof(tracker->acc_world_unfiltered_prev));
|
||||
memset(tracker->acc_world_filtered, 0, sizeof(tracker->acc_world_filtered));
|
||||
return;
|
||||
}
|
||||
|
||||
|
||||
|
||||
// 优先级2：原地旋转 - 特殊的、非滑雪的运动状态
|
||||
// 条件：角速度很大，同时线性加速度的晃动在一个“中等”范围内。
|
||||
if (tracker->state == STATIC && gyr_magnitude > ROTATION_GYR_MAG_THRESHOLD && acc_variance < ROTATING_ACC_VARIANCE_THRESHOLD) {
|
||||
tracker->state = ROTATING;
|
||||
return;
|
||||
}
|
||||
|
||||
// 优先级3：启动滑雪 - “油门”
|
||||
// 条件：有足够大的线性加速度，同时陀螺仪的抖动在一个“合理”（而非“完全静止”）的范围内。
|
||||
if (fabsf(acc_magnitude - G_ACCELERATION) > START_SKIING_ACC_THRESHOLD && gyr_variance < SKIING_GYR_VARIANCE_THRESHOLD) {
|
||||
tracker->state = NO_CONSTANT_SPEED;
|
||||
return;
|
||||
}
|
||||
|
||||
// 如果不满足任何启动或停止条件，则保持当前状态（滑雪中）
|
||||
// 如果当前是静止或旋转，但没有满足启动条件，则状态会保持，直到满足ZUPT或旋转条件。
|
||||
#else
|
||||
|
||||
// 无论当前是什么状态，静止总是最高优先级
|
||||
if (acc_variance < ZUPT_ACC_VARIANCE_THRESHOLD && gyr_variance < ZUPT_GYR_VARIANCE_THRESHOLD) {
|
||||
tracker->state = STATIC;
|
||||
// 速度清零，抑制漂移
|
||||
memset(tracker->velocity, 0, sizeof(tracker->velocity));
|
||||
tracker->speed = 0.0f;
|
||||
memset(tracker->acc_world_unfiltered_prev, 0, sizeof(tracker->acc_world_unfiltered_prev));
|
||||
memset(tracker->acc_world_filtered, 0, sizeof(tracker->acc_world_filtered));
|
||||
return;
|
||||
}
|
||||
|
||||
switch (tracker->state) {
|
||||
case STATIC:
|
||||
//不break，会往下执行，判断是否进入非匀速状态
|
||||
case ROTATING: // 从静止或原地旋转可以启动
|
||||
if (fabsf(acc_magnitude - G_ACCELERATION) > START_SKIING_ACC_THRESHOLD && gyr_variance < SKIING_GYR_VARIANCE_THRESHOLD) {
|
||||
tracker->state = NO_CONSTANT_SPEED;
|
||||
} else if (gyr_magnitude > ROTATION_GYR_MAG_THRESHOLD && acc_variance < ROTATING_ACC_VARIANCE_THRESHOLD) {
|
||||
tracker->state = ROTATING;
|
||||
}
|
||||
break;
|
||||
case NO_CONSTANT_SPEED: //非匀速状态
|
||||
if (gyr_magnitude > FALLEN_GRY_MAG_THRESHOLD) {
|
||||
tracker->state = FALLEN; //摔倒
|
||||
} else if (gyr_magnitude > WHEEL_GYR_MAG_THRESHOLD && acc_variance > WHEEL_ACC_VARIANCE_THRESHOLD) {
|
||||
tracker->state = WHEEL; //转弯
|
||||
} else if (fabsf(acc_magnitude - G_ACCELERATION) < SKIING_ACC_MAG_THRESHOLD) {
|
||||
tracker->state = CONSTANT_SPEED; //匀速
|
||||
}
|
||||
break;
|
||||
|
||||
case CONSTANT_SPEED: //匀速状态
|
||||
if (fabsf(acc_magnitude - G_ACCELERATION) > START_SKIING_ACC_THRESHOLD) {
|
||||
tracker->state = NO_CONSTANT_SPEED;
|
||||
}
|
||||
//TODO：可以添加进入转弯或摔倒的判断
|
||||
break;
|
||||
|
||||
case WHEEL:
|
||||
// 从转弯状态，检查转弯是否结束
|
||||
// 如果角速度和加速度方差都降下来了，就回到普通滑行状态
|
||||
if (gyr_magnitude < WHEEL_GYR_MAG_THRESHOLD * 0.8f && acc_variance < WHEEL_ACC_VARIANCE_THRESHOLD * 0.8f) { // 乘以一个滞后系数避免抖动
|
||||
tracker->state = NO_CONSTANT_SPEED;
|
||||
}
|
||||
break;
|
||||
|
||||
case FALLEN:
|
||||
// TODO：回到 STATIC
|
||||
break;
|
||||
}
|
||||
#endif
|
||||
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* @brief 主更新函数
|
||||
*
|
||||
* @param tracker
|
||||
* @param acc_g 三轴加速度，g
|
||||
* @param gyr_dps 三轴陀螺仪，dps
|
||||
* @param angle 欧若拉角
|
||||
* @param dt 采样时间间隔，会用来积分求速度
|
||||
*/
|
||||
void skiing_tracker_update(skiing_tracker_t *tracker, float *acc_g, float *gyr_dps, float *angle, float dt)
|
||||
{
|
||||
if (!tracker || !acc_g || !gyr_dps || !angle || dt <= 0) {
|
||||
return;
|
||||
}
|
||||
if(my_skiing_tracker.state == STOP_DETECTION)
|
||||
return;
|
||||
|
||||
// --- 数据预处理和缓冲 ---
|
||||
float acc_device_ms2[3];
|
||||
acc_device_ms2[0] = acc_g[0] * G_ACCELERATION;
|
||||
acc_device_ms2[1] = acc_g[1] * G_ACCELERATION;
|
||||
acc_device_ms2[2] = acc_g[2] * G_ACCELERATION;
|
||||
|
||||
// 将最新数据存入缓冲区
|
||||
memcpy(tracker->acc_buffer[tracker->buffer_index], acc_device_ms2, sizeof(acc_device_ms2));
|
||||
memcpy(tracker->gyr_buffer[tracker->buffer_index], gyr_dps, 3 * sizeof(float));
|
||||
|
||||
tracker->buffer_index++;
|
||||
if (tracker->buffer_index >= VARIANCE_BUFFER_SIZE) {
|
||||
tracker->buffer_index = 0;
|
||||
tracker->buffer_filled = 1; // 标记缓冲区已满
|
||||
}
|
||||
|
||||
// --- 更新状态机 ---
|
||||
update_state_machine(tracker, acc_device_ms2, gyr_dps);
|
||||
|
||||
// 坐标转换 & 移除重力
|
||||
transform_acc_to_world_frame(acc_device_ms2, angle, tracker->acc_world);
|
||||
tracker->acc_world[2] -= G_ACCELERATION;
|
||||
|
||||
// 对世界坐标系下的加速度进行高通滤波，消除直流偏置和重力残差
|
||||
for (int i = 0; i < 3; i++) {
|
||||
tracker->acc_world_filtered[i] = HPF_ALPHA * (tracker->acc_world_filtered[i] + tracker->acc_world[i] - tracker->acc_world_unfiltered_prev[i]);
|
||||
tracker->acc_world_unfiltered_prev[i] = tracker->acc_world[i];
|
||||
}
|
||||
// 应用加速度死区，忽略微小抖动和噪声
|
||||
float acc_horizontal_mag = sqrtf(tracker->acc_world_filtered[0] * tracker->acc_world_filtered[0] +
|
||||
tracker->acc_world_filtered[1] * tracker->acc_world_filtered[1]);
|
||||
// --- 根据状态进行计算 ---
|
||||
if (tracker->state == NO_CONSTANT_SPEED) {
|
||||
if (acc_horizontal_mag > ACC_DEAD_ZONE_THRESHOLD) {
|
||||
// 只有当水平加速度足够大时，才进行速度积分
|
||||
tracker->velocity[0] += tracker->acc_world_filtered[0] * dt;
|
||||
tracker->velocity[1] += tracker->acc_world_filtered[1] * dt;
|
||||
// 垂直方向的速度暂时不积分，极易受姿态误差影响而漂移
|
||||
// tracker->velocity[2] += tracker->acc_world_filtered[2] * dt;
|
||||
}
|
||||
#ifdef XTELL_TEST
|
||||
debug2.acc_magnitude = acc_horizontal_mag;
|
||||
#endif
|
||||
// 如果加速度小于阈值，则不更新速度，相当于速度保持不变（或受下一步的阻尼影响而衰减）
|
||||
|
||||
// --- 更新速率和距离 ---
|
||||
// 只基于水平速度计算速率和距离
|
||||
tracker->speed = sqrtf(tracker->velocity[0] * tracker->velocity[0] +
|
||||
tracker->velocity[1] * tracker->velocity[1]);
|
||||
tracker->distance += tracker->speed * dt;
|
||||
|
||||
|
||||
}else if(tracker->state == CONSTANT_SPEED){ //匀速
|
||||
#ifdef XTELL_TEST
|
||||
debug2.acc_magnitude = acc_horizontal_mag;
|
||||
#endif
|
||||
//保持上次的速度不变。只更新距离
|
||||
tracker->distance += tracker->speed * dt;
|
||||
|
||||
}else{
|
||||
// 在静止或旋转状态下，速度已经在状态机内部被清零
|
||||
// 额外增加速度衰减，模拟摩擦力，进一步抑制漂移
|
||||
tracker->velocity[0] *= SPEED_ATTENUATION;
|
||||
tracker->velocity[1] *= SPEED_ATTENUATION;
|
||||
tracker->velocity[2] = 0; // 垂直速度强制归零
|
||||
}
|
||||
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
/**
|
||||
* @brief 传感器数据采集与处理任务，外部每10ms调用一次，如果需要更新时间间隔，也需要同步更新宏“ DELTA_TIME ”
|
||||
*
|
||||
* @param acc_data_buf 三轴加速度原始数据
|
||||
* @param gyr_data_buf 三轴陀螺仪原始数据
|
||||
* @return BLE_send_data_t
|
||||
*/
|
||||
BLE_send_data_t sensor_processing_task(signed short * acc_data_buf, signed short * gyr_data_buf) {
|
||||
|
||||
static int initialized = 0;
|
||||
static int calibration_done = 0;
|
||||
|
||||
static signed short combined_raw_data[6];
|
||||
static float final_angle_data[3]; // 计算得到的欧若拉角
|
||||
static float calibrated_acc_g[3]; // 转换后的加速度计数据
|
||||
static float calibrated_gyr_dps[3]; // 转换后的陀螺仪数据
|
||||
|
||||
const float delta_time = DELTA_TIME + 0.03f;
|
||||
BLE_send_data_t BLE_send_data;
|
||||
|
||||
if (!initialized) {
|
||||
skiing_tracker_init(&my_skiing_tracker);
|
||||
initialized = 1;
|
||||
printf("Skiing Tracker Initialized. Waiting for sensor calibration...\n");
|
||||
}
|
||||
|
||||
memcpy(&combined_raw_data[0], acc_data_buf, 3 * sizeof(signed short));
|
||||
memcpy(&combined_raw_data[3], gyr_data_buf, 3 * sizeof(signed short));
|
||||
|
||||
unsigned char status;
|
||||
if (!calibration_done) { //第1次启动，开启零漂检测
|
||||
status = SL_SC7U22_Angle_Output(1, combined_raw_data, final_angle_data, 0);
|
||||
if (status == 1) {
|
||||
calibration_done = 1;
|
||||
printf("Sensor calibration successful! Skiing mode is active.\n");
|
||||
}
|
||||
} else {
|
||||
// printf("Calculate the time interval =============== start\n");
|
||||
status = SL_SC7U22_Angle_Output(0, combined_raw_data, final_angle_data, 0);
|
||||
}
|
||||
|
||||
if (status == 1) {
|
||||
// 加速度 LSB to g
|
||||
calibrated_acc_g[0] = (float)combined_raw_data[0] / 8192.0f;
|
||||
calibrated_acc_g[1] = (float)combined_raw_data[1] / 8192.0f;
|
||||
calibrated_acc_g[2] = (float)combined_raw_data[2] / 8192.0f;
|
||||
|
||||
// 陀螺仪 LSB to dps (度/秒)
|
||||
// ±2000dps量程下，转换系数约为 0.061
|
||||
calibrated_gyr_dps[0] = (float)combined_raw_data[3] * 0.061f;
|
||||
calibrated_gyr_dps[1] = (float)combined_raw_data[4] * 0.061f;
|
||||
calibrated_gyr_dps[2] = (float)combined_raw_data[5] * 0.061f;
|
||||
|
||||
skiing_tracker_update(&my_skiing_tracker, calibrated_acc_g, calibrated_gyr_dps, final_angle_data, delta_time);
|
||||
|
||||
// static int count = 0;
|
||||
// if(count >= 10){
|
||||
// printf("State: %d, Speed: %.2f m/s, Distance: %.2f m\n",
|
||||
// my_skiing_tracker.state,
|
||||
// my_skiing_tracker.speed,
|
||||
// my_skiing_tracker.distance);
|
||||
// printf("calibrated_acc_g: %.2f, %.2f, %.2f\n",
|
||||
// calibrated_acc_g[0],
|
||||
// calibrated_acc_g[1],
|
||||
// calibrated_acc_g[2]);
|
||||
// count = 0;
|
||||
// } else {
|
||||
// count++;
|
||||
// }
|
||||
|
||||
BLE_send_data.sensor_state = status;
|
||||
BLE_send_data.skiing_state = my_skiing_tracker.state;
|
||||
for (int i = 0; i < 3; i++) {
|
||||
#ifndef XTELL_TEST
|
||||
BLE_send_data.acc_original[i] = (int)acc_data_buf[i];
|
||||
BLE_send_data.gyr_original[i] = (int)gyr_data_buf[i];
|
||||
#endif
|
||||
#if KS_BLE
|
||||
KS_data.acc_KS[i] = (int)(calibrated_acc_g[i] * G_ACCELERATION * 100); //cm/s^s
|
||||
KS_data.gyr_KS_dps[i] = (int)calibrated_gyr_dps[i];
|
||||
KS_data.angle_KS[i] = (int)final_angle_data[i];
|
||||
#endif
|
||||
}
|
||||
BLE_send_data.speed_cms = (int)(my_skiing_tracker.speed * 100);
|
||||
BLE_send_data.distance_cm = (int)(my_skiing_tracker.distance * 100);
|
||||
// printf("Calculate the time interval =============== end\n");
|
||||
} else if (status == 0) {
|
||||
memset(&BLE_send_data, 0, sizeof(BLE_send_data_t));
|
||||
BLE_send_data.sensor_state = status;
|
||||
#if KS_BLE
|
||||
memset(&KS_data, 0, sizeof(BLE_send_data_t));
|
||||
#endif
|
||||
// printf("Sensor is calibrating...\n");
|
||||
} else {
|
||||
memset(&BLE_send_data, 0, sizeof(BLE_send_data_t));
|
||||
BLE_send_data.sensor_state = status;
|
||||
#if KS_BLE
|
||||
memset(&KS_data, 0, sizeof(BLE_send_data_t));
|
||||
#endif
|
||||
// printf("Angle calculation error or calibration not finished.\n");
|
||||
}
|
||||
return BLE_send_data;
|
||||
}
|
||||
@ -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
|
||||
@ -1,583 +0,0 @@
|
||||
/*
|
||||
|
||||
*/
|
||||
#include "skiing_tracker.h"
|
||||
#include "../sensor/SC7U22.h"
|
||||
#include <math.h>
|
||||
#include <string.h>
|
||||
|
||||
#define G_ACCELERATION 9.81f
|
||||
#define DEG_TO_RAD (3.14159265f / 180.0f)
|
||||
|
||||
// --- 静止检测 ---
|
||||
//两个判断是否静止的必要条件：动态零速更新(ZUPT)阈值
|
||||
// 加速方差阈值，提高阈值，让“刹车”更灵敏，以便在波浪式前进等慢速漂移时也能触发零速更新
|
||||
#define STOP_ACC_VARIANCE_THRESHOLD 0.2f
|
||||
// 陀螺仪方差阈值
|
||||
#define STOP_GYR_VARIANCE_THRESHOLD 5.0f
|
||||
// 静止时候的陀螺仪模长
|
||||
#define STOP_GYR_MAG_THRESHOLD 15
|
||||
// --- --- ---
|
||||
|
||||
// --- 启动滑雪阈值 ---
|
||||
// 加速度模长与重力的差值大于此值，认为开始运动；降低阈值，让“油门”更灵敏，以便能捕捉到真实的慢速启动
|
||||
#define START_ACC_MAG_THRESHOLD 1.0f //0.5、1
|
||||
// 陀螺仪方差阈值，以允许启动瞬间的正常抖动，但仍能过滤掉混乱的、非滑雪的晃动。
|
||||
#define START_GYR_VARIANCE_THRESHOLD 15.0f
|
||||
// --- --- ---
|
||||
|
||||
// --- 滑雪过程 ---
|
||||
//加速度 模长(不含重力），低于此值视为 在做匀速运动
|
||||
#define SKIING_ACC_MAG_THRESHOLD 0.5f
|
||||
//陀螺仪 模长，高于此值视为 摔倒了
|
||||
#define FALLEN_GRY_MAG_THRESHOLD 2000.0f //未确定
|
||||
// --- --- ---
|
||||
|
||||
// --- 原地旋转抖动 ---
|
||||
// 加速度 方差 阈值。此值比 静止检测 阈值更宽松，
|
||||
#define WOBBLE_ACC_VARIANCE_THRESHOLD 0.5f
|
||||
// 加速度 模长 阈值
|
||||
#define WOBBLE_ACC_MAG_THRESHOLD 1.0f
|
||||
// 角速度 总模长 大于此值(度/秒)，认为正在进行非滑雪的旋转或摆动
|
||||
#define ROTATION_GYR_MAG_THRESHOLD 30.0f
|
||||
// --- --- ---
|
||||
|
||||
// --- 滑雪转弯动 ---
|
||||
// 加速度 方差 阈值，大于此值，滑雪过程可能发生了急转弯
|
||||
#define WHEEL_ACC_VARIANCE_THRESHOLD 7.0f
|
||||
// 角速度 总模长 大于此值(度/秒)，认为滑雪过程中进行急转弯
|
||||
#define WHEEL_GYR_MAG_THRESHOLD 500.0f //
|
||||
// --- --- ---
|
||||
|
||||
// --- 跳跃 ---
|
||||
// 加速度模长低于此值(g)，认为进入失重状态(IN_AIR)
|
||||
#define AIRBORNE_ACC_MAG_LOW_THRESHOLD 0.4f
|
||||
// 加速度模长高于此值(g)，认为发生落地冲击(LANDING)
|
||||
#define LANDING_ACC_MAG_HIGH_THRESHOLD 3.5f
|
||||
// 起跳加速度阈值(g)，用于进入TAKING_OFF状态
|
||||
#define TAKEOFF_ACC_MAG_HIGH_THRESHOLD 1.8f
|
||||
// 进入空中状态确认计数：需要连续3个采样点加速度低于阈值才判断为起跳
|
||||
#define AIRBORNE_CONFIRM_COUNT 3
|
||||
// 落地状态确认计数：加速度恢复到1g附近并持续2个采样点(20ms)则认为已落地
|
||||
#define GROUNDED_CONFIRM_COUNT 2
|
||||
// 最大滞空时间(秒)，超过此时间强制认为已落地，防止状态锁死
|
||||
#define MAX_TIME_IN_AIR 12.5f
|
||||
// --- --- ---
|
||||
|
||||
// --- 用于消除积分漂移的滤波器和阈值 ---
|
||||
// 高通滤波器系数 (alpha)。alpha 越接近1，滤除低频(直流偏移)的效果越强，但可能滤掉真实的慢速运动。
|
||||
// alpha = RC / (RC + dt)，参考RC电路而来，fc ≈ (1 - alpha) / (2 * π * dt)
|
||||
#define HPF_ALPHA 0.999f
|
||||
//0.995f： 0.08 Hz 的信号
|
||||
//0.999f： 0.0159 Hz
|
||||
// --- --- ---
|
||||
|
||||
// --- 低通滤波器 ---
|
||||
// 低通滤波器系数 (alpha)。alpha 越小，滤波效果越强（更平滑），但延迟越大。
|
||||
// alpha 推荐范围 0.7 ~ 0.95。可以从 0.85 开始尝试。
|
||||
#define LPF_ALPHA 0.7f
|
||||
|
||||
// 加速度死区阈值 (m/s^2)。低于此阈值的加速度被认为是噪声，不参与积分。
|
||||
// 设得太高会忽略真实的慢速启动，设得太低则无法有效抑制噪声。
|
||||
//参考：0.2f ~ 0.4f
|
||||
#define ACC_DEAD_ZONE_THRESHOLD 0.05f
|
||||
|
||||
// --- 模拟摩擦力，进行速度衰减 ---
|
||||
#define SPEED_ATTENUATION 1.0f //暂不模拟
|
||||
BLE_KS_send_data_t KS_data;
|
||||
|
||||
#ifdef XTELL_TEST
|
||||
|
||||
debug_t debug1;
|
||||
debug_t debug2;
|
||||
#endif
|
||||
|
||||
static skiing_tracker_t my_skiing_tracker;
|
||||
//////////////////////////////////////////////////////////////////////////////////////////////////
|
||||
//实现
|
||||
|
||||
void clear_speed(void){
|
||||
my_skiing_tracker.state = STATIC;
|
||||
memset(my_skiing_tracker.velocity, 0, sizeof(my_skiing_tracker.velocity));
|
||||
my_skiing_tracker.speed = 0;
|
||||
}
|
||||
|
||||
void start_detection(void){
|
||||
my_skiing_tracker.state = STATIC;
|
||||
memset(my_skiing_tracker.velocity, 0, sizeof(my_skiing_tracker.velocity));
|
||||
my_skiing_tracker.distance = 0;
|
||||
my_skiing_tracker.speed = 0;
|
||||
}
|
||||
|
||||
void stop_detection(void){
|
||||
my_skiing_tracker.state = STOP_DETECTION;
|
||||
memset(my_skiing_tracker.velocity, 0, sizeof(my_skiing_tracker.velocity));
|
||||
my_skiing_tracker.speed = 0;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 初始化滑雪追踪器
|
||||
*
|
||||
* @param tracker
|
||||
*/
|
||||
void skiing_tracker_init(skiing_tracker_t *tracker)
|
||||
{
|
||||
if (!tracker) {
|
||||
return;
|
||||
}
|
||||
// 使用memset一次性清零整个结构体，包括新增的缓冲区
|
||||
memset(tracker, 0, sizeof(skiing_tracker_t));
|
||||
tracker->state = STATIC;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 当检测到落地时，计算空中的水平飞行距离并累加到总距离
|
||||
*/
|
||||
static void calculate_air_distance(skiing_tracker_t *tracker) {
|
||||
float horizontal_speed_on_takeoff = sqrtf(
|
||||
tracker->initial_velocity_on_takeoff[0] * tracker->initial_velocity_on_takeoff[0] +
|
||||
tracker->initial_velocity_on_takeoff[1] * tracker->initial_velocity_on_takeoff[1]
|
||||
);
|
||||
float distance_in_air = horizontal_speed_on_takeoff * tracker->time_in_air;
|
||||
tracker->distance += distance_in_air;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 将设备坐标系下的加速度转换为世界坐标系
|
||||
* @param acc_device 设备坐标系下的加速度 [x, y, z]
|
||||
* @param angle 姿态角 [pitch, roll, yaw]，单位: 度
|
||||
* @param acc_world 输出：世界坐标系下的加速度 [x, y, z]
|
||||
*/
|
||||
static void transform_acc_to_world_frame(const float *acc_device, const float *angle, float *acc_world)
|
||||
{
|
||||
// 驱动输出的角度与标准航空定义相反，需要取反才能用于标准旋转矩阵。
|
||||
float pitch = -angle[0] * DEG_TO_RAD;
|
||||
float roll = -angle[1] * DEG_TO_RAD;
|
||||
|
||||
// TODO: 当引入三轴磁力计后，这里的 yaw 应由磁力计和陀螺仪融合解算得出，以解决航向漂移问题。
|
||||
// 目前 yaw 暂时不参与计算，因为仅靠加速度计和陀螺仪无法获得准确的绝对航向角。
|
||||
// float yaw = -angle[2] * DEG_TO_RAD;
|
||||
|
||||
float cp = cosf(pitch);
|
||||
float sp = sinf(pitch);
|
||||
float cr = cosf(roll);
|
||||
float sr = sinf(roll);
|
||||
|
||||
float ax = acc_device[0];
|
||||
float ay = acc_device[1];
|
||||
float az = acc_device[2];
|
||||
|
||||
// 使用经过验证的、正确的身体坐标系到世界坐标系的旋转矩阵 (基于 Y-X 旋转顺序)
|
||||
// 这个矩阵将设备测量的加速度(ax, ay, az)正确地转换到世界坐标系(acc_world)。
|
||||
// 注意：这里没有使用yaw，主要关心的是坡面上的运动，绝对航向暂时不影响速度和距离的计算。
|
||||
// TODO
|
||||
acc_world[0] = cp * ax + sp * sr * ay + sp * cr * az;
|
||||
acc_world[1] = 0 * ax + cr * ay - sr * az;
|
||||
acc_world[2] = -sp * ax + cp * sr * ay + cp * cr * az;
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* @brief 计算缓冲区内三轴数据的方差之和
|
||||
*
|
||||
* @param buffer 传进来的三轴数据：陀螺仪/加速度
|
||||
* @return float 返回方差和
|
||||
*/
|
||||
static float calculate_variance(float buffer[VARIANCE_BUFFER_SIZE][3])
|
||||
{
|
||||
float mean[3] = {0};
|
||||
float variance[3] = {0};
|
||||
|
||||
// 计算均值
|
||||
for (int i = 0; i < VARIANCE_BUFFER_SIZE; i++) {
|
||||
mean[0] += buffer[i][0];
|
||||
mean[1] += buffer[i][1];
|
||||
mean[2] += buffer[i][2];
|
||||
}
|
||||
mean[0] /= VARIANCE_BUFFER_SIZE;
|
||||
mean[1] /= VARIANCE_BUFFER_SIZE;
|
||||
mean[2] /= VARIANCE_BUFFER_SIZE;
|
||||
|
||||
// 计算方差
|
||||
for (int i = 0; i < VARIANCE_BUFFER_SIZE; i++) {
|
||||
variance[0] += (buffer[i][0] - mean[0]) * (buffer[i][0] - mean[0]);
|
||||
variance[1] += (buffer[i][1] - mean[1]) * (buffer[i][1] - mean[1]);
|
||||
variance[2] += (buffer[i][2] - mean[2]) * (buffer[i][2] - mean[2]);
|
||||
}
|
||||
variance[0] /= VARIANCE_BUFFER_SIZE;
|
||||
variance[1] /= VARIANCE_BUFFER_SIZE;
|
||||
variance[2] /= VARIANCE_BUFFER_SIZE;
|
||||
|
||||
// 返回三轴方差之和，作为一个综合的稳定度指标
|
||||
return variance[0] + variance[1] + variance[2];
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 摩擦力模拟，进行速度衰减
|
||||
*
|
||||
* @param tracker
|
||||
*/
|
||||
void forece_of_friction(skiing_tracker_t *tracker){
|
||||
// 增加速度衰减，模拟摩擦力
|
||||
tracker->velocity[0] *= SPEED_ATTENUATION;
|
||||
tracker->velocity[1] *= SPEED_ATTENUATION;
|
||||
tracker->velocity[2] = 0; // 垂直速度强制归零
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 状态机更新
|
||||
*
|
||||
* @param tracker 传入同步修改后传出
|
||||
* @param acc_device_ms2 三轴加速度，m/s^2
|
||||
* @param gyr_dps 三轴陀螺仪，dps
|
||||
*/
|
||||
static void update_state_machine(skiing_tracker_t *tracker, const float *acc_device_ms2, const float *gyr_dps)
|
||||
{
|
||||
// 缓冲区未填满时，不进行状态判断，默认为静止
|
||||
if (!tracker->buffer_filled) {
|
||||
tracker->state = STATIC;
|
||||
return;
|
||||
}
|
||||
|
||||
// --- 计算关键指标 ---
|
||||
float acc_variance = calculate_variance(tracker->acc_buffer); // 计算加速度方差
|
||||
float gyr_variance = calculate_variance(tracker->gyr_buffer); // 计算陀螺仪方差
|
||||
float gyr_magnitude = sqrtf(gyr_dps[0]*gyr_dps[0] + gyr_dps[1]*gyr_dps[1] + gyr_dps[2]*gyr_dps[2]); //dps
|
||||
float acc_magnitude = sqrtf(acc_device_ms2[0]*acc_device_ms2[0] + acc_device_ms2[1]*acc_device_ms2[1] + acc_device_ms2[2]*acc_device_ms2[2]); //m/s^s
|
||||
float acc_magnitude_g = acc_magnitude / G_ACCELERATION; // 转换为g单位，用于跳跃判断
|
||||
|
||||
#ifdef XTELL_TEST
|
||||
debug1.acc_variance =acc_variance;
|
||||
debug1.gyr_variance =gyr_variance;
|
||||
debug1.gyr_magnitude=gyr_magnitude;
|
||||
debug1.acc_magnitude=fabsf(acc_magnitude - G_ACCELERATION);
|
||||
#endif
|
||||
|
||||
|
||||
|
||||
// --- 状态机逻辑 (核心修改区域) ---
|
||||
|
||||
#if 0 //暂时不考虑空中
|
||||
// 1. 空中/落地状态的后续处理
|
||||
if (tracker->state == IN_AIR) {
|
||||
// A. 检测巨大冲击 -> 落地
|
||||
if (acc_magnitude_g > LANDING_ACC_MAG_HIGH_THRESHOLD) {
|
||||
tracker->state = LANDING;
|
||||
// B. 检测超时 -> 强制落地 (安全机制)
|
||||
} else if (tracker->time_in_air > MAX_TIME_IN_AIR) {
|
||||
tracker->state = LANDING;
|
||||
// C. 检测恢复正常重力 (平缓落地)
|
||||
} else if (acc_magnitude_g > 0.8f && acc_magnitude_g < 1.5f) {
|
||||
tracker->grounded_entry_counter++;
|
||||
if (tracker->grounded_entry_counter >= GROUNDED_CONFIRM_COUNT) {
|
||||
tracker->state = LANDING;
|
||||
}
|
||||
} else {
|
||||
tracker->grounded_entry_counter = 0;
|
||||
}
|
||||
return; // 在空中或刚切换到落地，结束本次状态判断
|
||||
}
|
||||
|
||||
// 2. 严格的 "起跳->空中" 状态转换逻辑
|
||||
// 只有当处于滑行状态时，才去检测起跳意图
|
||||
if (tracker->state == NO_CONSTANT_SPEED || tracker->state == CONSTANT_SPEED || tracker->state == WHEEL) {
|
||||
if (acc_magnitude_g > TAKEOFF_ACC_MAG_HIGH_THRESHOLD) {
|
||||
tracker->state = TAKING_OFF;
|
||||
tracker->airborne_entry_counter = 0; // 准备检测失重
|
||||
return;
|
||||
}
|
||||
}
|
||||
// 只有在TAKING_OFF状态下，才去检测是否进入失重
|
||||
if (tracker->state == TAKING_OFF) {
|
||||
if (acc_magnitude_g < AIRBORNE_ACC_MAG_LOW_THRESHOLD) {
|
||||
tracker->airborne_entry_counter++;
|
||||
if (tracker->airborne_entry_counter >= AIRBORNE_CONFIRM_COUNT) {
|
||||
memcpy(tracker->initial_velocity_on_takeoff, tracker->velocity, sizeof(tracker->velocity));
|
||||
tracker->time_in_air = 0;
|
||||
tracker->state = IN_AIR;
|
||||
tracker->airborne_entry_counter = 0;
|
||||
tracker->grounded_entry_counter = 0;
|
||||
return;
|
||||
}
|
||||
} else {
|
||||
// 如果在起跳冲击后一段时间内没有失重，说明只是一个颠簸，恢复滑行
|
||||
// 可以加一个小的超时计数器，这里为了简单先直接恢复
|
||||
tracker->state = NO_CONSTANT_SPEED;
|
||||
}
|
||||
return; // 无论是否切换，都结束本次判断
|
||||
}
|
||||
#endif
|
||||
|
||||
|
||||
// --- 静止判断 ---
|
||||
if (acc_variance < STOP_ACC_VARIANCE_THRESHOLD && gyr_variance < STOP_GYR_VARIANCE_THRESHOLD && gyr_magnitude < STOP_GYR_MAG_THRESHOLD) {
|
||||
tracker->state = STATIC;
|
||||
return;
|
||||
}
|
||||
|
||||
// --- 地面状态切换逻辑 ---
|
||||
switch (tracker->state) {
|
||||
case LANDING:
|
||||
tracker->state = STATIC;
|
||||
break;
|
||||
case STATIC:
|
||||
// 优先判断是否进入 WOBBLE 状态
|
||||
// 条件：陀螺仪活动剧烈，但整体加速度变化不大（说明是原地转或晃）
|
||||
if (gyr_magnitude > ROTATION_GYR_MAG_THRESHOLD && fabsf(acc_magnitude - G_ACCELERATION) < WOBBLE_ACC_MAG_THRESHOLD) {
|
||||
tracker->state = WOBBLE;
|
||||
}
|
||||
// 只有在陀螺仪和加速度都满足“前进”特征时，才启动
|
||||
else if (gyr_variance > START_GYR_VARIANCE_THRESHOLD && fabsf(acc_magnitude - G_ACCELERATION) > START_ACC_MAG_THRESHOLD) {
|
||||
tracker->state = NO_CONSTANT_SPEED;
|
||||
}
|
||||
break;
|
||||
|
||||
case WOBBLE:
|
||||
// 从 WOBBLE 状态启动的条件应该和从 STATIC 启动一样严格
|
||||
if (gyr_variance > START_GYR_VARIANCE_THRESHOLD && fabsf(acc_magnitude - G_ACCELERATION) > START_ACC_MAG_THRESHOLD) {
|
||||
tracker->state = NO_CONSTANT_SPEED;
|
||||
}
|
||||
// 如果陀螺仪活动减弱，则可能恢复静止
|
||||
else if (gyr_magnitude < ROTATION_GYR_MAG_THRESHOLD * 0.8f) { // 增加迟滞，避免抖动
|
||||
// 不直接跳回STATIC，而是依赖下一轮的全局静止判断
|
||||
}
|
||||
break;
|
||||
case NO_CONSTANT_SPEED: //非匀速状态
|
||||
//暂时不考虑摔倒
|
||||
// if (gyr_magnitude > FALLEN_GRY_MAG_THRESHOLD) {
|
||||
// tracker->state = FALLEN; //摔倒
|
||||
// } else
|
||||
if (gyr_magnitude > WHEEL_GYR_MAG_THRESHOLD && acc_variance > WHEEL_ACC_VARIANCE_THRESHOLD) {
|
||||
tracker->state = WHEEL; //转弯
|
||||
} else if (fabsf(acc_magnitude - G_ACCELERATION) < SKIING_ACC_MAG_THRESHOLD) {
|
||||
tracker->state = CONSTANT_SPEED; //匀速
|
||||
}
|
||||
break;
|
||||
|
||||
case CONSTANT_SPEED: //匀速状态
|
||||
if (fabsf(acc_magnitude - G_ACCELERATION) > START_ACC_MAG_THRESHOLD) {
|
||||
tracker->state = NO_CONSTANT_SPEED;
|
||||
}
|
||||
//TODO：可以添加进入转弯或摔倒的判断
|
||||
break;
|
||||
|
||||
case WHEEL:
|
||||
// 从转弯状态，检查转弯是否结束
|
||||
// 如果角速度和加速度方差都降下来了，就回到普通滑行状态
|
||||
if (gyr_magnitude < WHEEL_GYR_MAG_THRESHOLD * 0.8f && acc_variance < WHEEL_ACC_VARIANCE_THRESHOLD * 0.8f) { // 乘以一个滞后系数避免抖动
|
||||
tracker->state = NO_CONSTANT_SPEED;
|
||||
}
|
||||
break;
|
||||
|
||||
case FALLEN:
|
||||
// TODO：回到 STATIC
|
||||
break;
|
||||
}
|
||||
|
||||
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* @brief 主更新函数
|
||||
*
|
||||
* @param tracker
|
||||
* @param acc_g 三轴加速度，g
|
||||
* @param gyr_dps 三轴陀螺仪，dps
|
||||
* @param angle 欧若拉角
|
||||
* @param dt 采样时间间隔，会用来积分求速度
|
||||
*/
|
||||
void skiing_tracker_update(skiing_tracker_t *tracker, float *acc_g, float *gyr_dps, float *angle, float dt)
|
||||
{
|
||||
if (!tracker || !acc_g || !gyr_dps || !angle || dt <= 0) {
|
||||
return;
|
||||
}
|
||||
if(my_skiing_tracker.state == STOP_DETECTION)
|
||||
return;
|
||||
|
||||
// --- 数据预处理和缓冲 ---
|
||||
float acc_device_ms2[3];
|
||||
acc_device_ms2[0] = acc_g[0] * G_ACCELERATION;
|
||||
acc_device_ms2[1] = acc_g[1] * G_ACCELERATION;
|
||||
acc_device_ms2[2] = acc_g[2] * G_ACCELERATION;
|
||||
|
||||
// 将最新数据存入缓冲区
|
||||
memcpy(tracker->acc_buffer[tracker->buffer_index], acc_device_ms2, sizeof(acc_device_ms2));
|
||||
memcpy(tracker->gyr_buffer[tracker->buffer_index], gyr_dps, 3 * sizeof(float));
|
||||
|
||||
tracker->buffer_index++;
|
||||
if (tracker->buffer_index >= VARIANCE_BUFFER_SIZE) {
|
||||
tracker->buffer_index = 0;
|
||||
tracker->buffer_filled = 1; // 标记缓冲区已满
|
||||
}
|
||||
|
||||
// --- 更新状态机 ---
|
||||
update_state_machine(tracker, acc_device_ms2, gyr_dps);
|
||||
|
||||
// --- 根据状态执行不同的计算逻辑 ---
|
||||
switch (tracker->state) {
|
||||
case TAKING_OFF:
|
||||
tracker->speed = 0.0f;
|
||||
break;
|
||||
case IN_AIR:
|
||||
// 在空中时，只累加滞空时间
|
||||
tracker->time_in_air += dt;
|
||||
break;
|
||||
case LANDING:
|
||||
// 刚落地，计算空中距离
|
||||
calculate_air_distance(tracker);
|
||||
// 清理速度和滤波器状态，为恢复地面追踪做准备
|
||||
memset(tracker->velocity, 0, sizeof(tracker->velocity));
|
||||
tracker->speed = 0;
|
||||
memset(tracker->acc_world_unfiltered_prev, 0, sizeof(tracker->acc_world_unfiltered_prev));
|
||||
memset(tracker->acc_world_filtered, 0, sizeof(tracker->acc_world_filtered));
|
||||
memset(tracker->acc_world_lpf, 0, sizeof(tracker->acc_world_lpf)); // 清理新增的LPF状态
|
||||
break;
|
||||
case WHEEL:
|
||||
case NO_CONSTANT_SPEED:
|
||||
transform_acc_to_world_frame(acc_device_ms2, angle, tracker->acc_world);
|
||||
tracker->acc_world[2] -= G_ACCELERATION;
|
||||
|
||||
float acc_world_temp[3]; // 临时变量存储当前周期的加速度
|
||||
for (int i = 0; i < 2; i++) { // 只处理水平方向的 x 和 y 轴
|
||||
|
||||
// --- 核心修改：颠倒滤波器顺序为 HPF -> LPF ---
|
||||
|
||||
// 1. 高通滤波 (HPF) 先行: 消除因姿态误差导致的重力泄漏（直流偏置）
|
||||
// HPF的瞬态响应会产生尖峰，这是正常的。
|
||||
tracker->acc_world_filtered[i] = HPF_ALPHA * (tracker->acc_world_filtered[i] + tracker->acc_world[i] - tracker->acc_world_unfiltered_prev[i]);
|
||||
tracker->acc_world_unfiltered_prev[i] = tracker->acc_world[i];
|
||||
|
||||
// 2. 低通滤波 (LPF) 殿后: 平滑掉HPF产生的尖峰和传感器自身的高频振动噪声。
|
||||
// 这里使用 tracker->acc_world_filtered[i] 作为LPF的输入。
|
||||
tracker->acc_world_lpf[i] = (1.0f - LPF_ALPHA) * tracker->acc_world_filtered[i] + LPF_ALPHA * tracker->acc_world_lpf[i];
|
||||
|
||||
// 将最终处理完的加速度值存入临时变量
|
||||
acc_world_temp[i] = tracker->acc_world_lpf[i];
|
||||
}
|
||||
|
||||
// 计算处理后加速度的水平模长
|
||||
float acc_horizontal_mag = sqrtf(acc_world_temp[0] * acc_world_temp[0] +
|
||||
acc_world_temp[1] * acc_world_temp[1]);
|
||||
#if XTELL_TEST
|
||||
debug2.acc_magnitude = acc_horizontal_mag;
|
||||
#endif
|
||||
// 应用死区，并积分
|
||||
if (acc_horizontal_mag > ACC_DEAD_ZONE_THRESHOLD) {
|
||||
tracker->velocity[0] += acc_world_temp[0] * dt;
|
||||
tracker->velocity[1] += acc_world_temp[1] * dt;
|
||||
}
|
||||
|
||||
// 更新速度和距离
|
||||
tracker->speed = sqrtf(tracker->velocity[0] * tracker->velocity[0] +
|
||||
tracker->velocity[1] * tracker->velocity[1]);
|
||||
tracker->distance += tracker->speed * dt;
|
||||
break;
|
||||
case CONSTANT_SPEED:
|
||||
//保持上次的速度不变。只更新距离
|
||||
tracker->distance += tracker->speed * dt;
|
||||
break;
|
||||
case STATIC:
|
||||
case WOBBLE:
|
||||
// 速度清零，抑制漂移
|
||||
memset(tracker->velocity, 0, sizeof(tracker->velocity));
|
||||
tracker->speed = 0.0f;
|
||||
memset(tracker->acc_world_unfiltered_prev, 0, sizeof(tracker->acc_world_unfiltered_prev));
|
||||
memset(tracker->acc_world_filtered, 0, sizeof(tracker->acc_world_filtered));
|
||||
memset(tracker->acc_world_lpf, 0, sizeof(tracker->acc_world_lpf)); // 清理新增的LPF状态
|
||||
#if XTELL_TEST
|
||||
debug2.acc_magnitude = 0;
|
||||
#endif
|
||||
break;
|
||||
case FALLEN:
|
||||
// TODO
|
||||
break;
|
||||
default:
|
||||
break;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
/**
|
||||
* @brief 滑雪数据计算
|
||||
*
|
||||
* @param acc_data_buf 传入的三轴加速度数据
|
||||
* @param gyr_data_buf 传入的三轴陀螺仪数据
|
||||
* @param angle_data 传入的欧若拉角数据
|
||||
* @return BLE_send_data_t 要发送给蓝牙的数据
|
||||
*/
|
||||
BLE_send_data_t sensor_processing_task(signed short* acc_data_buf, signed short* gyr_data_buf, float* angle_data) {
|
||||
|
||||
static int initialized = 0;
|
||||
static float acc_data_g[3];
|
||||
static float gyr_data_dps[3];
|
||||
|
||||
// const float delta_time = DELTA_TIME+0.01f;
|
||||
// const float delta_time = DELTA_TIME + 0.005f;
|
||||
const float delta_time = DELTA_TIME;
|
||||
BLE_send_data_t BLE_send_data;
|
||||
|
||||
if (!initialized) {
|
||||
skiing_tracker_init(&my_skiing_tracker);
|
||||
initialized = 1;
|
||||
printf("Skiing Tracker Initialized. Waiting for sensor calibration...\n");
|
||||
}
|
||||
|
||||
|
||||
#if ACC_RANGE==2
|
||||
// 加速度 LSB to g
|
||||
acc_data_g[0] = (float)acc_data_buf[0] / 16384.0f;
|
||||
acc_data_g[1] = (float)acc_data_buf[1] / 16384.0f;
|
||||
acc_data_g[2] = (float)acc_data_buf[2] / 16384.0f;
|
||||
#endif
|
||||
|
||||
#if ACC_RANGE==4
|
||||
// 加速度 LSB to g
|
||||
acc_data_g[0] = (float)acc_data_buf[0] / 8192.0f;
|
||||
acc_data_g[1] = (float)acc_data_buf[1] / 8192.0f;
|
||||
acc_data_g[2] = (float)acc_data_buf[2] / 8192.0f;
|
||||
#endif
|
||||
|
||||
#if ACC_RANGE==8
|
||||
//±8g 4096
|
||||
acc_data_g[0] = (float)acc_data_buf[0] / 4096.0f; //ax
|
||||
acc_data_g[1] = (float)acc_data_buf[1] / 4096.0f; //ay
|
||||
acc_data_g[2] = (float)acc_data_buf[2] / 4096.0f; //az
|
||||
#endif
|
||||
|
||||
#if ACC_RANGE==16
|
||||
//±16g 2048
|
||||
acc_data_g[0] = (float)acc_data_buf[0] / 2048.0f; //ax
|
||||
acc_data_g[1] = (float)acc_data_buf[1] / 2048.0f; //ay
|
||||
acc_data_g[2] = (float)acc_data_buf[2] / 2048.0f; //az
|
||||
#endif
|
||||
|
||||
// 陀螺仪 LSB to dps (度/秒)
|
||||
// ±2000dps量程下，转换系数约为 0.061
|
||||
gyr_data_dps[0] = (float)gyr_data_buf[0] * 0.061f;
|
||||
gyr_data_dps[1] = (float)gyr_data_buf[1] * 0.061f;
|
||||
gyr_data_dps[2] = (float)gyr_data_buf[2] * 0.061f;
|
||||
|
||||
skiing_tracker_update(&my_skiing_tracker, acc_data_g, gyr_data_dps, angle_data, delta_time);
|
||||
|
||||
BLE_send_data.skiing_state = my_skiing_tracker.state;
|
||||
for (int i = 0; i < 3; i++) {
|
||||
#ifdef XTELL_TEST
|
||||
BLE_send_data.acc_data[i] = (short)(acc_data_g[i] * 9.8f) * 100; //cm/^s2
|
||||
BLE_send_data.gyr_data[i] = (short)gyr_data_dps[i]; //dps
|
||||
BLE_send_data.angle_data[i] = angle_data[i];
|
||||
#else
|
||||
BLE_send_data.acc_data[i] = (short)acc_data_buf[i]; //原始adc数据
|
||||
BLE_send_data.gyr_data[i] = (short)gyr_data_buf[i]; //原始adc数据
|
||||
BLE_send_data.angle_data[i] = angle_data[i];
|
||||
#endif
|
||||
}
|
||||
BLE_send_data.speed_cms = (int)(my_skiing_tracker.speed * 100);
|
||||
BLE_send_data.distance_cm = (int)(my_skiing_tracker.distance * 100);
|
||||
// printf("Calculate the time interval =============== end\n");
|
||||
|
||||
return BLE_send_data;
|
||||
}
|
||||
|
||||
@ -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
|
||||
@ -1,651 +0,0 @@
|
||||
/*
|
||||
|
||||
*/
|
||||
#include "skiing_tracker.h"
|
||||
#include "../sensor/SC7U22.h"
|
||||
#include <math.h>
|
||||
#include <string.h>
|
||||
|
||||
#define G_ACCELERATION 9.81f
|
||||
#define DEG_TO_RAD (3.14159265f / 180.0f)
|
||||
|
||||
#define ENABLE_XLOG 1
|
||||
#ifdef xlog
|
||||
#undef xlog
|
||||
#endif
|
||||
#if ENABLE_XLOG
|
||||
#define xlog(format, ...) printf("[XT:%s] " format, __func__, ##__VA_ARGS__)
|
||||
#else
|
||||
#define xlog(format, ...) ((void)0)
|
||||
#endif
|
||||
|
||||
// --- 静止检测 ---
|
||||
//两个判断是否静止的必要条件：动态零速更新(ZUPT)阈值
|
||||
// 加速方差阈值，提高阈值，让“刹车”更灵敏，以便在波浪式前进等慢速漂移时也能触发零速更新
|
||||
#define STOP_ACC_VARIANCE_THRESHOLD 0.2f
|
||||
// 陀螺仪方差阈值
|
||||
#define STOP_GYR_VARIANCE_THRESHOLD 5.0f
|
||||
// 静止时候的陀螺仪模长
|
||||
#define STOP_GYR_MAG_THRESHOLD 15
|
||||
// --- --- ---
|
||||
|
||||
// --- 启动滑雪阈值 ---
|
||||
// 加速度模长与重力的差值大于此值，认为开始运动；降低阈值，让“油门”更灵敏，以便能捕捉到真实的慢速启动
|
||||
#define START_ACC_MAG_THRESHOLD 1.0f //0.5、1
|
||||
// 陀螺仪方差阈值，以允许启动瞬间的正常抖动，但仍能过滤掉混乱的、非滑雪的晃动。
|
||||
#define START_GYR_VARIANCE_THRESHOLD 15.0f
|
||||
// --- --- ---
|
||||
|
||||
// --- 滑雪过程 ---
|
||||
//加速度 模长(不含重力），低于此值视为 在做匀速运动
|
||||
#define SKIING_ACC_MAG_THRESHOLD 0.5f
|
||||
//陀螺仪 模长，高于此值视为 摔倒了
|
||||
#define FALLEN_GRY_MAG_THRESHOLD 2000.0f //未确定
|
||||
// --- --- ---
|
||||
|
||||
// --- 原地旋转抖动 ---
|
||||
// 加速度 方差 阈值。此值比 静止检测 阈值更宽松，
|
||||
#define WOBBLE_ACC_VARIANCE_THRESHOLD 0.5f
|
||||
// 加速度 模长 阈值
|
||||
#define WOBBLE_ACC_MAG_THRESHOLD 1.0f
|
||||
// 角速度 总模长 大于此值(度/秒)，认为正在进行非滑雪的旋转或摆动
|
||||
#define ROTATION_GYR_MAG_THRESHOLD 30.0f
|
||||
// --- --- ---
|
||||
|
||||
// --- 滑雪转弯动 ---
|
||||
// 加速度 方差 阈值，大于此值，滑雪过程可能发生了急转弯
|
||||
#define WHEEL_ACC_VARIANCE_THRESHOLD 7.0f
|
||||
// 角速度 总模长 大于此值(度/秒)，认为滑雪过程中进行急转弯
|
||||
#define WHEEL_GYR_MAG_THRESHOLD 500.0f //
|
||||
// --- --- ---
|
||||
|
||||
// --- 跳跃 ---
|
||||
// 加速度模长低于此值(g)，认为进入失重状态(IN_AIR)
|
||||
#define AIRBORNE_ACC_MAG_LOW_THRESHOLD 0.4f
|
||||
// 加速度模长高于此值(g)，认为发生落地冲击(LANDING)
|
||||
#define LANDING_ACC_MAG_HIGH_THRESHOLD 3.5f
|
||||
// 起跳加速度阈值(g)，用于进入TAKING_OFF状态
|
||||
#define TAKEOFF_ACC_MAG_HIGH_THRESHOLD 1.8f
|
||||
// 进入空中状态确认计数：需要连续3个采样点加速度低于阈值才判断为起跳
|
||||
#define AIRBORNE_CONFIRM_COUNT 3
|
||||
// 落地状态确认计数：加速度恢复到1g附近并持续2个采样点(20ms)则认为已落地
|
||||
#define GROUNDED_CONFIRM_COUNT 2
|
||||
// 最大滞空时间(秒)，超过此时间强制认为已落地，防止状态锁死
|
||||
#define MAX_TIME_IN_AIR 12.5f
|
||||
// --- --- ---
|
||||
|
||||
// --- 用于消除积分漂移的滤波器和阈值 ---
|
||||
// 高通滤波器系数 (alpha)。alpha 越接近1，滤除低频(直流偏移)的效果越强，但可能滤掉真实的慢速运动。
|
||||
// alpha = RC / (RC + dt)，参考RC电路而来，fc ≈ (1 - alpha) / (2 * π * dt)
|
||||
#define HPF_ALPHA 0.999f
|
||||
//0.995f： 0.08 Hz 的信号
|
||||
//0.999f： 0.0159 Hz
|
||||
// --- --- ---
|
||||
|
||||
// --- 低通滤波器 ---
|
||||
// 低通滤波器系数 (alpha)。alpha 越小，滤波效果越强（更平滑），但延迟越大。
|
||||
// alpha 推荐范围 0.7 ~ 0.95。可以从 0.85 开始尝试。
|
||||
#define LPF_ALPHA 0.7f
|
||||
|
||||
// 加速度死区阈值 (m/s^2)。低于此阈值的加速度被认为是噪声，不参与积分。
|
||||
// 设得太高会忽略真实的慢速启动，设得太低则无法有效抑制噪声。
|
||||
//参考：0.2f ~ 0.4f
|
||||
#define ACC_DEAD_ZONE_THRESHOLD 0.05f
|
||||
|
||||
// --- 模拟摩擦力，进行速度衰减 ---
|
||||
#define SPEED_ATTENUATION 1.0f //暂不模拟
|
||||
BLE_KS_send_data_t KS_data;
|
||||
static float quaternion_data[4];
|
||||
#ifdef XTELL_TEST
|
||||
|
||||
debug_t debug1;
|
||||
debug_t debug2;
|
||||
#endif
|
||||
|
||||
static skiing_tracker_t my_skiing_tracker;
|
||||
//////////////////////////////////////////////////////////////////////////////////////////////////
|
||||
//实现
|
||||
|
||||
void clear_speed(void){
|
||||
my_skiing_tracker.state = STATIC;
|
||||
memset(my_skiing_tracker.velocity, 0, sizeof(my_skiing_tracker.velocity));
|
||||
my_skiing_tracker.speed = 0;
|
||||
}
|
||||
|
||||
void start_detection(void){
|
||||
my_skiing_tracker.state = STATIC;
|
||||
memset(my_skiing_tracker.velocity, 0, sizeof(my_skiing_tracker.velocity));
|
||||
my_skiing_tracker.distance = 0;
|
||||
my_skiing_tracker.speed = 0;
|
||||
}
|
||||
|
||||
void stop_detection(void){
|
||||
my_skiing_tracker.state = STOP_DETECTION;
|
||||
memset(my_skiing_tracker.velocity, 0, sizeof(my_skiing_tracker.velocity));
|
||||
my_skiing_tracker.speed = 0;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 初始化滑雪追踪器
|
||||
*
|
||||
* @param tracker
|
||||
*/
|
||||
void skiing_tracker_init(skiing_tracker_t *tracker)
|
||||
{
|
||||
if (!tracker) {
|
||||
return;
|
||||
}
|
||||
// 使用memset一次性清零整个结构体，包括新增的缓冲区
|
||||
memset(tracker, 0, sizeof(skiing_tracker_t));
|
||||
tracker->state = STATIC;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 当检测到落地时，计算空中的水平飞行距离并累加到总距离
|
||||
*/
|
||||
static void calculate_air_distance(skiing_tracker_t *tracker) {
|
||||
float horizontal_speed_on_takeoff = sqrtf(
|
||||
tracker->initial_velocity_on_takeoff[0] * tracker->initial_velocity_on_takeoff[0] +
|
||||
tracker->initial_velocity_on_takeoff[1] * tracker->initial_velocity_on_takeoff[1]
|
||||
);
|
||||
float distance_in_air = horizontal_speed_on_takeoff * tracker->time_in_air;
|
||||
tracker->distance += distance_in_air;
|
||||
}
|
||||
|
||||
|
||||
|
||||
/**
|
||||
* @brief 使用四元数直接从设备坐标系的加速度中移除重力分量
|
||||
* @details 这种方法比使用欧拉角更精确、更稳定，且避免了万向节死锁。
|
||||
* @param acc_device 输入：设备坐标系下的原始加速度 [x, y, z], 单位 m/s^2
|
||||
* @param q 输入：表示姿态的四元数 [w, x, y, z]
|
||||
* @param acc_linear_device 输出：设备坐标系下移除重力后的线性加速度 [x, y, z]
|
||||
*/
|
||||
void q_remove_gravity_with_quaternion(const float *acc_device, const float *q, float *acc_linear_device)
|
||||
{
|
||||
// 从四元数计算重力在设备坐标系下的投影
|
||||
// G_device = R_transpose * G_world
|
||||
// G_world = [0, 0, g]
|
||||
// R_transpose 的第三列即为重力投影方向
|
||||
float gx = 2.0f * (q[1] * q[3] - q[0] * q[2]);
|
||||
float gy = 2.0f * (q[0] * q[1] + q[2] * q[3]);
|
||||
float gz = q[0] * q[0] - q[1] * q[1] - q[2] * q[2] + q[3] * q[3];
|
||||
|
||||
// 从原始加速度中减去重力分量
|
||||
acc_linear_device[0] = acc_device[0] - gx * G_ACCELERATION;
|
||||
acc_linear_device[1] = acc_device[1] - gy * G_ACCELERATION;
|
||||
acc_linear_device[2] = acc_device[2] - gz * G_ACCELERATION;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 使用四元数将设备坐标系的线性加速度转换到世界坐标系
|
||||
* @details 同样，此方法比使用欧拉角更优。
|
||||
* @param acc_linear_device 输入：设备坐标系下的线性加速度 [x, y, z]
|
||||
* @param q 输入：表示姿态的四元数 [w, x, y, z]
|
||||
* @param acc_linear_world 输出：世界坐标系下的线性加速度 [x, y, z]
|
||||
*/
|
||||
void q_transform_to_world_with_quaternion(const float *acc_linear_device, const float *q, float *acc_linear_world)
|
||||
{
|
||||
// 这是 R_device_to_world * acc_linear_device 的展开形式
|
||||
acc_linear_world[0] = (1.0f - 2.0f*q[2]*q[2] - 2.0f*q[3]*q[3]) * acc_linear_device[0] +
|
||||
(2.0f*q[1]*q[2] - 2.0f*q[0]*q[3]) * acc_linear_device[1] +
|
||||
(2.0f*q[1]*q[3] + 2.0f*q[0]*q[2]) * acc_linear_device[2];
|
||||
|
||||
acc_linear_world[1] = (2.0f*q[1]*q[2] + 2.0f*q[0]*q[3]) * acc_linear_device[0] +
|
||||
(1.0f - 2.0f*q[1]*q[1] - 2.0f*q[3]*q[3]) * acc_linear_device[1] +
|
||||
(2.0f*q[2]*q[3] - 2.0f*q[0]*q[1]) * acc_linear_device[2];
|
||||
|
||||
acc_linear_world[2] = (2.0f*q[1]*q[3] - 2.0f*q[0]*q[2]) * acc_linear_device[0] +
|
||||
(2.0f*q[2]*q[3] + 2.0f*q[0]*q[1]) * acc_linear_device[1] +
|
||||
(1.0f - 2.0f*q[1]*q[1] - 2.0f*q[2]*q[2]) * acc_linear_device[2];
|
||||
// acc_linear_world[2] -= G_ACCELERATION;
|
||||
}
|
||||
|
||||
|
||||
|
||||
/**
|
||||
* @brief 计算缓冲区内三轴数据的方差之和
|
||||
*
|
||||
* @param buffer 传进来的三轴数据：陀螺仪/加速度
|
||||
* @return float 返回方差和
|
||||
*/
|
||||
static float calculate_variance(float buffer[VARIANCE_BUFFER_SIZE][3])
|
||||
{
|
||||
float mean[3] = {0};
|
||||
float variance[3] = {0};
|
||||
|
||||
// 计算均值
|
||||
for (int i = 0; i < VARIANCE_BUFFER_SIZE; i++) {
|
||||
mean[0] += buffer[i][0];
|
||||
mean[1] += buffer[i][1];
|
||||
mean[2] += buffer[i][2];
|
||||
}
|
||||
mean[0] /= VARIANCE_BUFFER_SIZE;
|
||||
mean[1] /= VARIANCE_BUFFER_SIZE;
|
||||
mean[2] /= VARIANCE_BUFFER_SIZE;
|
||||
|
||||
// 计算方差
|
||||
for (int i = 0; i < VARIANCE_BUFFER_SIZE; i++) {
|
||||
variance[0] += (buffer[i][0] - mean[0]) * (buffer[i][0] - mean[0]);
|
||||
variance[1] += (buffer[i][1] - mean[1]) * (buffer[i][1] - mean[1]);
|
||||
variance[2] += (buffer[i][2] - mean[2]) * (buffer[i][2] - mean[2]);
|
||||
}
|
||||
variance[0] /= VARIANCE_BUFFER_SIZE;
|
||||
variance[1] /= VARIANCE_BUFFER_SIZE;
|
||||
variance[2] /= VARIANCE_BUFFER_SIZE;
|
||||
|
||||
// 返回三轴方差之和，作为一个综合的稳定度指标
|
||||
return variance[0] + variance[1] + variance[2];
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 摩擦力模拟，进行速度衰减
|
||||
*
|
||||
* @param tracker
|
||||
*/
|
||||
void forece_of_friction(skiing_tracker_t *tracker){
|
||||
// 增加速度衰减，模拟摩擦力
|
||||
tracker->velocity[0] *= SPEED_ATTENUATION;
|
||||
tracker->velocity[1] *= SPEED_ATTENUATION;
|
||||
tracker->velocity[2] = 0; // 垂直速度强制归零
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 状态机更新
|
||||
*
|
||||
* @param tracker 传入同步修改后传出
|
||||
* @param acc_device_ms2 三轴加速度，m/s^2
|
||||
* @param gyr_dps 三轴陀螺仪，dps
|
||||
*/
|
||||
static void update_state_machine(skiing_tracker_t *tracker, const float *acc_device_ms2, const float *gyr_dps)
|
||||
{
|
||||
// 缓冲区未填满时，不进行状态判断，默认为静止
|
||||
if (!tracker->buffer_filled) {
|
||||
tracker->state = STATIC;
|
||||
return;
|
||||
}
|
||||
|
||||
// --- 计算关键指标 ---
|
||||
float acc_variance = calculate_variance(tracker->acc_buffer); // 计算加速度方差
|
||||
float gyr_variance = calculate_variance(tracker->gyr_buffer); // 计算陀螺仪方差
|
||||
float gyr_magnitude = sqrtf(gyr_dps[0]*gyr_dps[0] + gyr_dps[1]*gyr_dps[1] + gyr_dps[2]*gyr_dps[2]); //dps
|
||||
float acc_magnitude = sqrtf(acc_device_ms2[0]*acc_device_ms2[0] + acc_device_ms2[1]*acc_device_ms2[1] + acc_device_ms2[2]*acc_device_ms2[2]); //m/s^s
|
||||
float acc_magnitude_g = acc_magnitude / G_ACCELERATION; // 转换为g单位，用于跳跃判断
|
||||
|
||||
#ifdef XTELL_TEST
|
||||
debug1.acc_variance =acc_variance;
|
||||
debug1.gyr_variance =gyr_variance;
|
||||
debug1.gyr_magnitude=gyr_magnitude;
|
||||
debug1.acc_magnitude=fabsf(acc_magnitude - G_ACCELERATION);
|
||||
#endif
|
||||
|
||||
|
||||
|
||||
// --- 状态机逻辑 (核心修改区域) ---
|
||||
|
||||
#if 0 //暂时不考虑空中
|
||||
// 1. 空中/落地状态的后续处理
|
||||
if (tracker->state == IN_AIR) {
|
||||
// A. 检测巨大冲击 -> 落地
|
||||
if (acc_magnitude_g > LANDING_ACC_MAG_HIGH_THRESHOLD) {
|
||||
tracker->state = LANDING;
|
||||
// B. 检测超时 -> 强制落地 (安全机制)
|
||||
} else if (tracker->time_in_air > MAX_TIME_IN_AIR) {
|
||||
tracker->state = LANDING;
|
||||
// C. 检测恢复正常重力 (平缓落地)
|
||||
} else if (acc_magnitude_g > 0.8f && acc_magnitude_g < 1.5f) {
|
||||
tracker->grounded_entry_counter++;
|
||||
if (tracker->grounded_entry_counter >= GROUNDED_CONFIRM_COUNT) {
|
||||
tracker->state = LANDING;
|
||||
}
|
||||
} else {
|
||||
tracker->grounded_entry_counter = 0;
|
||||
}
|
||||
return; // 在空中或刚切换到落地，结束本次状态判断
|
||||
}
|
||||
|
||||
// 2. 严格的 "起跳->空中" 状态转换逻辑
|
||||
// 只有当处于滑行状态时，才去检测起跳意图
|
||||
if (tracker->state == NO_CONSTANT_SPEED || tracker->state == CONSTANT_SPEED || tracker->state == WHEEL) {
|
||||
if (acc_magnitude_g > TAKEOFF_ACC_MAG_HIGH_THRESHOLD) {
|
||||
tracker->state = TAKING_OFF;
|
||||
tracker->airborne_entry_counter = 0; // 准备检测失重
|
||||
return;
|
||||
}
|
||||
}
|
||||
// 只有在TAKING_OFF状态下，才去检测是否进入失重
|
||||
if (tracker->state == TAKING_OFF) {
|
||||
if (acc_magnitude_g < AIRBORNE_ACC_MAG_LOW_THRESHOLD) {
|
||||
tracker->airborne_entry_counter++;
|
||||
if (tracker->airborne_entry_counter >= AIRBORNE_CONFIRM_COUNT) {
|
||||
memcpy(tracker->initial_velocity_on_takeoff, tracker->velocity, sizeof(tracker->velocity));
|
||||
tracker->time_in_air = 0;
|
||||
tracker->state = IN_AIR;
|
||||
tracker->airborne_entry_counter = 0;
|
||||
tracker->grounded_entry_counter = 0;
|
||||
return;
|
||||
}
|
||||
} else {
|
||||
// 如果在起跳冲击后一段时间内没有失重，说明只是一个颠簸，恢复滑行
|
||||
// 可以加一个小的超时计数器，这里为了简单先直接恢复
|
||||
tracker->state = NO_CONSTANT_SPEED;
|
||||
}
|
||||
return; // 无论是否切换，都结束本次判断
|
||||
}
|
||||
#endif
|
||||
|
||||
|
||||
// --- 静止判断 ---
|
||||
if (acc_variance < STOP_ACC_VARIANCE_THRESHOLD && gyr_variance < STOP_GYR_VARIANCE_THRESHOLD && gyr_magnitude < STOP_GYR_MAG_THRESHOLD) {
|
||||
tracker->state = STATIC;
|
||||
return;
|
||||
}
|
||||
|
||||
// --- 地面状态切换逻辑 ---
|
||||
switch (tracker->state) {
|
||||
case LANDING:
|
||||
tracker->state = STATIC;
|
||||
break;
|
||||
case STATIC:
|
||||
// 优先判断是否进入 WOBBLE 状态
|
||||
// 条件：陀螺仪活动剧烈，但整体加速度变化不大（说明是原地转或晃）
|
||||
if (gyr_magnitude > ROTATION_GYR_MAG_THRESHOLD && fabsf(acc_magnitude - G_ACCELERATION) < WOBBLE_ACC_MAG_THRESHOLD) {
|
||||
tracker->state = WOBBLE;
|
||||
}
|
||||
// 只有在陀螺仪和加速度都满足“前进”特征时，才启动
|
||||
else if (gyr_variance > START_GYR_VARIANCE_THRESHOLD && fabsf(acc_magnitude - G_ACCELERATION) > START_ACC_MAG_THRESHOLD) {
|
||||
tracker->state = NO_CONSTANT_SPEED;
|
||||
}
|
||||
break;
|
||||
|
||||
case WOBBLE:
|
||||
// 从 WOBBLE 状态启动的条件应该和从 STATIC 启动一样严格
|
||||
if (gyr_variance < START_GYR_VARIANCE_THRESHOLD * 2 && fabsf(acc_magnitude - G_ACCELERATION) > START_ACC_MAG_THRESHOLD) {
|
||||
tracker->state = NO_CONSTANT_SPEED;
|
||||
}
|
||||
// 如果陀螺仪活动减弱，则可能恢复静止
|
||||
else if (gyr_magnitude < ROTATION_GYR_MAG_THRESHOLD * 0.8f) { // 增加迟滞，避免抖动
|
||||
// 不直接跳回STATIC，而是依赖下一轮的全局静止判断
|
||||
}
|
||||
break;
|
||||
case NO_CONSTANT_SPEED: //非匀速状态
|
||||
//暂时不考虑摔倒
|
||||
// if (gyr_magnitude > FALLEN_GRY_MAG_THRESHOLD) {
|
||||
// tracker->state = FALLEN; //摔倒
|
||||
// } else
|
||||
if (gyr_magnitude > WHEEL_GYR_MAG_THRESHOLD && acc_variance > WHEEL_ACC_VARIANCE_THRESHOLD) {
|
||||
tracker->state = WHEEL; //转弯
|
||||
} else if (fabsf(acc_magnitude - G_ACCELERATION) < SKIING_ACC_MAG_THRESHOLD) {
|
||||
tracker->state = CONSTANT_SPEED; //匀速
|
||||
}
|
||||
break;
|
||||
|
||||
case CONSTANT_SPEED: //匀速状态
|
||||
if (fabsf(acc_magnitude - G_ACCELERATION) > START_ACC_MAG_THRESHOLD) {
|
||||
tracker->state = NO_CONSTANT_SPEED;
|
||||
}
|
||||
//TODO：可以添加进入转弯或摔倒的判断
|
||||
break;
|
||||
|
||||
case WHEEL:
|
||||
// 从转弯状态，检查转弯是否结束
|
||||
// 如果角速度和加速度方差都降下来了，就回到普通滑行状态
|
||||
if (gyr_magnitude < WHEEL_GYR_MAG_THRESHOLD * 0.8f && acc_variance < WHEEL_ACC_VARIANCE_THRESHOLD * 0.8f) { // 乘以一个滞后系数避免抖动
|
||||
tracker->state = NO_CONSTANT_SPEED;
|
||||
}
|
||||
break;
|
||||
|
||||
case FALLEN:
|
||||
// TODO：回到 STATIC
|
||||
break;
|
||||
}
|
||||
|
||||
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* @brief 主更新函数
|
||||
*
|
||||
* @param tracker
|
||||
* @param acc_g 三轴加速度，g
|
||||
* @param gyr_dps 三轴陀螺仪，dps
|
||||
* @param angle 欧若拉角
|
||||
* @param dt 采样时间间隔，会用来积分求速度
|
||||
*/
|
||||
void skiing_tracker_update(skiing_tracker_t *tracker, float *acc_g, float *gyr_dps, float *angle, float dt)
|
||||
{
|
||||
if (!tracker || !acc_g || !gyr_dps || !angle || dt <= 0) {
|
||||
return;
|
||||
}
|
||||
if(my_skiing_tracker.state == STOP_DETECTION)
|
||||
return;
|
||||
|
||||
// --- 数据预处理和缓冲 ---
|
||||
float acc_device_ms2[3];
|
||||
acc_device_ms2[0] = acc_g[0] * G_ACCELERATION;
|
||||
acc_device_ms2[1] = acc_g[1] * G_ACCELERATION;
|
||||
acc_device_ms2[2] = acc_g[2] * G_ACCELERATION;
|
||||
|
||||
// 将最新数据存入缓冲区
|
||||
memcpy(tracker->acc_buffer[tracker->buffer_index], acc_device_ms2, sizeof(acc_device_ms2));
|
||||
memcpy(tracker->gyr_buffer[tracker->buffer_index], gyr_dps, 3 * sizeof(float));
|
||||
|
||||
tracker->buffer_index++;
|
||||
if (tracker->buffer_index >= VARIANCE_BUFFER_SIZE) {
|
||||
tracker->buffer_index = 0;
|
||||
tracker->buffer_filled = 1; // 标记缓冲区已满
|
||||
}
|
||||
|
||||
// --- 更新状态机 ---
|
||||
update_state_machine(tracker, acc_device_ms2, gyr_dps);
|
||||
|
||||
// --- 根据状态执行不同的计算逻辑 ---
|
||||
switch (tracker->state) {
|
||||
case TAKING_OFF:
|
||||
tracker->speed = 0.0f;
|
||||
break;
|
||||
case IN_AIR:
|
||||
// 在空中时，只累加滞空时间
|
||||
tracker->time_in_air += dt;
|
||||
break;
|
||||
case LANDING:
|
||||
// 刚落地，计算空中距离
|
||||
calculate_air_distance(tracker);
|
||||
// 清理速度和滤波器状态，为恢复地面追踪做准备
|
||||
memset(tracker->velocity, 0, sizeof(tracker->velocity));
|
||||
tracker->speed = 0;
|
||||
memset(tracker->acc_world_unfiltered_prev, 0, sizeof(tracker->acc_world_unfiltered_prev));
|
||||
memset(tracker->acc_world_filtered, 0, sizeof(tracker->acc_world_filtered));
|
||||
memset(tracker->acc_world_lpf, 0, sizeof(tracker->acc_world_lpf)); // 清理新增的LPF状态
|
||||
break;
|
||||
case WHEEL:
|
||||
case NO_CONSTANT_SPEED:
|
||||
float linear_acc_device[3];
|
||||
float linear_acc_world[3];
|
||||
// 在设备坐标系下，移除重力，得到线性加速度
|
||||
q_remove_gravity_with_quaternion(acc_device_ms2, quaternion_data, linear_acc_device);
|
||||
|
||||
// 将设备坐标系下的线性加速度，旋转到世界坐标系
|
||||
q_transform_to_world_with_quaternion(linear_acc_device, quaternion_data, linear_acc_world);
|
||||
// 将最终用于积分的加速度存入 tracker 结构体
|
||||
memcpy(tracker->acc_no_g, linear_acc_world, sizeof(linear_acc_world));
|
||||
|
||||
float acc_world_temp[3]; // 临时变量存储当前周期的加速度
|
||||
for (int i = 0; i < 2; i++) { // 只处理水平方向的 x 和 y 轴
|
||||
|
||||
// --- 核心修改：颠倒滤波器顺序为 HPF -> LPF ---
|
||||
|
||||
// 1. 高通滤波 (HPF) 先行: 消除因姿态误差导致的重力泄漏（直流偏置）
|
||||
// HPF的瞬态响应会产生尖峰，这是正常的。
|
||||
tracker->acc_world_filtered[i] = HPF_ALPHA * (tracker->acc_world_filtered[i] + tracker->acc_no_g[i] - tracker->acc_world_unfiltered_prev[i]);
|
||||
tracker->acc_world_unfiltered_prev[i] = tracker->acc_no_g[i];
|
||||
|
||||
// 2. 低通滤波 (LPF) 殿后: 平滑掉HPF产生的尖峰和传感器自身的高频振动噪声。
|
||||
// 这里使用 tracker->acc_world_filtered[i] 作为LPF的输入。
|
||||
tracker->acc_world_lpf[i] = (1.0f - LPF_ALPHA) * tracker->acc_world_filtered[i] + LPF_ALPHA * tracker->acc_world_lpf[i];
|
||||
|
||||
// 将最终处理完的加速度值存入临时变量
|
||||
acc_world_temp[i] = tracker->acc_world_lpf[i];
|
||||
}
|
||||
|
||||
// 计算处理后加速度的水平模长
|
||||
float acc_horizontal_mag = sqrtf(acc_world_temp[0] * acc_world_temp[0] +
|
||||
acc_world_temp[1] * acc_world_temp[1]);
|
||||
#if XTELL_TEST
|
||||
debug2.acc_magnitude = acc_horizontal_mag;
|
||||
#endif
|
||||
// 应用死区，并积分
|
||||
if (acc_horizontal_mag > ACC_DEAD_ZONE_THRESHOLD) {
|
||||
tracker->velocity[0] += acc_world_temp[0] * dt;
|
||||
tracker->velocity[1] += acc_world_temp[1] * dt;
|
||||
}
|
||||
|
||||
// 更新速度和距离
|
||||
tracker->speed = sqrtf(tracker->velocity[0] * tracker->velocity[0] +
|
||||
tracker->velocity[1] * tracker->velocity[1]);
|
||||
tracker->distance += tracker->speed * dt;
|
||||
break;
|
||||
case CONSTANT_SPEED:
|
||||
//保持上次的速度不变。只更新距离
|
||||
tracker->distance += tracker->speed * dt;
|
||||
break;
|
||||
case STATIC:
|
||||
case WOBBLE:
|
||||
// 速度清零，抑制漂移
|
||||
memset(tracker->velocity, 0, sizeof(tracker->velocity));
|
||||
tracker->speed = 0.0f;
|
||||
memset(tracker->acc_world_unfiltered_prev, 0, sizeof(tracker->acc_world_unfiltered_prev));
|
||||
memset(tracker->acc_world_filtered, 0, sizeof(tracker->acc_world_filtered));
|
||||
memset(tracker->acc_world_lpf, 0, sizeof(tracker->acc_world_lpf)); // 清理新增的LPF状态
|
||||
#if XTELL_TEST
|
||||
debug2.acc_magnitude = 0;
|
||||
#endif
|
||||
break;
|
||||
case FALLEN:
|
||||
// TODO
|
||||
break;
|
||||
default:
|
||||
break;
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
#if 1
|
||||
float linear_acc_device[3];
|
||||
float linear_acc_world[3];
|
||||
float tmp_world_acc[3];
|
||||
// 在设备坐标系下，移除重力，得到线性加速度
|
||||
q_remove_gravity_with_quaternion(acc_device_ms2, quaternion_data, linear_acc_device);
|
||||
|
||||
// 将设备坐标系下的线性加速度，旋转到世界坐标系
|
||||
q_transform_to_world_with_quaternion(linear_acc_device, quaternion_data, tmp_world_acc);
|
||||
|
||||
|
||||
float all_world_mag = sqrtf(tmp_world_acc[0] * tmp_world_acc[0] +
|
||||
tmp_world_acc[1] * tmp_world_acc[1] +
|
||||
tmp_world_acc[2] * tmp_world_acc[2]);
|
||||
|
||||
static int count = 0;
|
||||
if(count > 100){
|
||||
xlog("===original(g): x %.2f, y %.2f, z %.2f===\n",acc_g[0],acc_g[1],acc_g[2]);
|
||||
xlog("===world(m/s^2) no g: x %.2f, y %.2f, z %.2f, all %.2f===\n",tmp_world_acc[0],tmp_world_acc[1],tmp_world_acc[2],all_world_mag); //去掉重力加速度
|
||||
xlog("===gyr(dps) : x %.2f, y %.2f, z %.2f===\n",gyr_dps[0],gyr_dps[1],gyr_dps[2]); //angle
|
||||
xlog("===angle : x %.2f, y %.2f, z %.2f,===\n",angle[0],angle[1],angle[2]);
|
||||
count = 0;
|
||||
|
||||
}
|
||||
count++;
|
||||
#endif
|
||||
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
/**
|
||||
* @brief 滑雪数据计算
|
||||
*
|
||||
* @param acc_data_buf 传入的三轴加速度数据
|
||||
* @param gyr_data_buf 传入的三轴陀螺仪数据
|
||||
* @param angle_data 传入的欧若拉角数据
|
||||
* @return BLE_send_data_t 要发送给蓝牙的数据
|
||||
*/
|
||||
BLE_send_data_t sensor_processing_task(signed short* acc_data_buf, signed short* gyr_data_buf, float* angle_data, float* quaternion) {
|
||||
|
||||
static int initialized = 0;
|
||||
static float acc_data_g[3];
|
||||
static float gyr_data_dps[3];
|
||||
|
||||
if(quaternion != NULL){
|
||||
memcpy(quaternion_data, quaternion, 4 * sizeof(float));
|
||||
}
|
||||
|
||||
// const float delta_time = DELTA_TIME+0.01f;
|
||||
// const float delta_time = DELTA_TIME + 0.005f;
|
||||
const float delta_time = DELTA_TIME;
|
||||
BLE_send_data_t BLE_send_data;
|
||||
|
||||
if (!initialized) {
|
||||
skiing_tracker_init(&my_skiing_tracker);
|
||||
initialized = 1;
|
||||
printf("Skiing Tracker Initialized. Waiting for sensor calibration...\n");
|
||||
}
|
||||
|
||||
|
||||
#if ACC_RANGE==2
|
||||
// 加速度 LSB to g
|
||||
acc_data_g[0] = (float)acc_data_buf[0] / 16384.0f;
|
||||
acc_data_g[1] = (float)acc_data_buf[1] / 16384.0f;
|
||||
acc_data_g[2] = (float)acc_data_buf[2] / 16384.0f;
|
||||
#endif
|
||||
|
||||
#if ACC_RANGE==4
|
||||
// 加速度 LSB to g
|
||||
acc_data_g[0] = (float)acc_data_buf[0] / 8192.0f;
|
||||
acc_data_g[1] = (float)acc_data_buf[1] / 8192.0f;
|
||||
acc_data_g[2] = (float)acc_data_buf[2] / 8192.0f;
|
||||
#endif
|
||||
|
||||
#if ACC_RANGE==8
|
||||
//±8g 4096
|
||||
acc_data_g[0] = (float)acc_data_buf[0] / 4096.0f; //ax
|
||||
acc_data_g[1] = (float)acc_data_buf[1] / 4096.0f; //ay
|
||||
acc_data_g[2] = (float)acc_data_buf[2] / 4096.0f; //az
|
||||
#endif
|
||||
|
||||
#if ACC_RANGE==16
|
||||
//±16g 2048
|
||||
acc_data_g[0] = (float)acc_data_buf[0] / 2048.0f; //ax
|
||||
acc_data_g[1] = (float)acc_data_buf[1] / 2048.0f; //ay
|
||||
acc_data_g[2] = (float)acc_data_buf[2] / 2048.0f; //az
|
||||
#endif
|
||||
|
||||
// 陀螺仪 LSB to dps (度/秒)
|
||||
// ±2000dps量程下，转换系数约为 0.061
|
||||
gyr_data_dps[0] = (float)gyr_data_buf[0] * 0.061f;
|
||||
gyr_data_dps[1] = (float)gyr_data_buf[1] * 0.061f;
|
||||
gyr_data_dps[2] = (float)gyr_data_buf[2] * 0.061f;
|
||||
|
||||
skiing_tracker_update(&my_skiing_tracker, acc_data_g, gyr_data_dps, angle_data, delta_time);
|
||||
|
||||
BLE_send_data.skiing_state = my_skiing_tracker.state;
|
||||
for (int i = 0; i < 3; i++) {
|
||||
#ifdef XTELL_TEST
|
||||
BLE_send_data.acc_data[i] = (short)(acc_data_g[i] * 9.8f) * 100; //cm/^s2
|
||||
BLE_send_data.gyr_data[i] = (short)gyr_data_dps[i]; //dps
|
||||
BLE_send_data.angle_data[i] = angle_data[i];
|
||||
#else
|
||||
BLE_send_data.acc_data[i] = (short)acc_data_buf[i]; //原始adc数据
|
||||
BLE_send_data.gyr_data[i] = (short)gyr_data_buf[i]; //原始adc数据
|
||||
BLE_send_data.angle_data[i] = angle_data[i];
|
||||
#endif
|
||||
}
|
||||
BLE_send_data.speed_cms = (int)(my_skiing_tracker.speed * 100);
|
||||
BLE_send_data.distance_cm = (int)(my_skiing_tracker.distance * 100);
|
||||
// printf("Calculate the time interval =============== end\n");
|
||||
|
||||
return BLE_send_data;
|
||||
}
|
||||
|
||||
@ -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
|
||||
@ -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无法通过加速度计来消除偏差）
|
||||
@ -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
|
||||
}
|
||||
@ -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__
|
||||
@ -1,60 +0,0 @@
|
||||
#include "circle_buffer.h"
|
||||
#include <string.h>
|
||||
|
||||
// 初始化环形缓冲区
|
||||
void circle_buffer_init(circle_buffer_t *cb, void *buffer, u16 capacity, u16 element_size) {
|
||||
cb->buffer = (u8 *)buffer;
|
||||
cb->capacity = capacity;
|
||||
cb->element_size = element_size;
|
||||
cb->head = 0;
|
||||
cb->tail = 0;
|
||||
cb->size = 0;
|
||||
}
|
||||
|
||||
// 向环形缓冲区写入一个元素
|
||||
bool circle_buffer_write(circle_buffer_t *cb, const void *element) {
|
||||
if (circle_buffer_is_full(cb)) {
|
||||
return false; // 缓冲区已满
|
||||
}
|
||||
|
||||
u8 *dest = cb->buffer + (cb->head * cb->element_size);
|
||||
memcpy(dest, element, cb->element_size);
|
||||
|
||||
cb->head = (cb->head + 1) % cb->capacity;
|
||||
cb->size++;
|
||||
return true;
|
||||
}
|
||||
|
||||
// 从环形缓冲区读取一个元素
|
||||
bool circle_buffer_read(circle_buffer_t *cb, void *element) {
|
||||
if (circle_buffer_is_empty(cb)) {
|
||||
return false; // 缓冲区为空
|
||||
}
|
||||
|
||||
u8 *src = cb->buffer + (cb->tail * cb->element_size);
|
||||
memcpy(element, src, cb->element_size);
|
||||
|
||||
cb->tail = (cb->tail + 1) % cb->capacity;
|
||||
cb->size--;
|
||||
return true;
|
||||
}
|
||||
|
||||
// 获取已用空间的大小（以元素为单位）
|
||||
u16 circle_buffer_get_size(circle_buffer_t *cb) {
|
||||
return cb->size;
|
||||
}
|
||||
|
||||
// 获取剩余空间的大小（以元素为单位）
|
||||
u16 circle_buffer_get_free_space(circle_buffer_t *cb) {
|
||||
return cb->capacity - cb->size;
|
||||
}
|
||||
|
||||
// 检查缓冲区是否已满
|
||||
bool circle_buffer_is_full(circle_buffer_t *cb) {
|
||||
return cb->size == cb->capacity;
|
||||
}
|
||||
|
||||
// 检查缓冲区是否为空
|
||||
bool circle_buffer_is_empty(circle_buffer_t *cb) {
|
||||
return cb->size == 0;
|
||||
}
|
||||
@ -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
|
||||
@ -1,635 +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 0
|
||||
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 > 10){
|
||||
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]);
|
||||
xlog("===speed(cm/s): %d\n",(int)(tracker->speed*100) );
|
||||
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);
|
||||
}
|
||||
|
||||
@ -1,83 +0,0 @@
|
||||
#ifndef SKIING_TRACKER_H
|
||||
#define SKIING_TRACKER_H
|
||||
|
||||
#include "../xtell.h"
|
||||
#include <math.h>
|
||||
#include <string.h>
|
||||
|
||||
|
||||
// 定义滑雪者可能的状态
|
||||
typedef enum {
|
||||
STATIC, // 静止或动态稳定：0
|
||||
NO_CONSTANT_SPEED, // 正在滑雪，非匀速：1
|
||||
CONSTANT_SPEED, // 正在滑雪，匀速：2
|
||||
WOBBLE, // 正在原地旋转：3
|
||||
WHEEL, // 转弯：4
|
||||
FALLEN, // 已摔倒：5
|
||||
TAKING_OFF, // 起跳冲击阶段：6
|
||||
IN_AIR, // 空中失重阶段：7
|
||||
LANDING, // 落地冲击阶段：8
|
||||
STOP_DETECTION, // 停止检测：9
|
||||
UNKNOWN // 未知状态：10
|
||||
} skiing_state_t;
|
||||
|
||||
#define VARIANCE_BUFFER_SIZE 5 // 用于计算方差的数据窗口大小 (5个样本 @ 100Hz = 50ms)，减小延迟，提高实时性
|
||||
#define DELTA_TIME 0.01f
|
||||
|
||||
|
||||
// 追踪器数据结构体
|
||||
typedef struct {
|
||||
// 公开数据
|
||||
float velocity[3]; // 当前速度 (x, y, z)，单位: m/s
|
||||
float distance; // 总滑行距离，单位: m
|
||||
float speed; // 当前速率 (标量)，单位: m/s
|
||||
skiing_state_t state; // 当前滑雪状态
|
||||
|
||||
// 内部计算使用的私有成员
|
||||
float acc_no_g[3]; // 去掉重力分量后的加速度
|
||||
|
||||
// 用于空中距离计算
|
||||
float time_in_air; // 滞空时间计时器
|
||||
float initial_velocity_on_takeoff[3]; // 起跳瞬间的速度向量
|
||||
int airborne_entry_counter; // 进入空中状态的确认计数器
|
||||
int grounded_entry_counter; // 落地确认计数器
|
||||
|
||||
// --- 内部计算使用的私有成员 ---
|
||||
// 用于动态零速更新和旋转检测的缓冲区
|
||||
float acc_buffer[VARIANCE_BUFFER_SIZE][3]; // 加速度数据窗口
|
||||
float gyr_buffer[VARIANCE_BUFFER_SIZE][3]; // 角速度数据窗口
|
||||
int buffer_index; // 缓冲区当前索引
|
||||
int buffer_filled; // 缓冲区是否已填满的标志
|
||||
|
||||
// 用于高通滤波器（巴特沃斯一阶滤波器）的私有成员，以消除加速度的直流偏置
|
||||
float acc_world_filtered[3]; //过滤过的
|
||||
float acc_world_unfiltered_prev[3]; //上一次没过滤的
|
||||
|
||||
float acc_world_lpf[3]; // 经过低通滤波后的世界坐标系加速度
|
||||
} skiing_tracker_t;
|
||||
|
||||
|
||||
|
||||
typedef struct{
|
||||
int acc_KS[3]; //卡尔曼后，LSB转换后的 三轴加速度数据（cm/s^2）
|
||||
int gyr_KS_dps[3]; //卡尔曼后，LSB to dps 三轴陀螺仪数据
|
||||
int angle_KS[3]; //卡尔曼后，计算得到的欧若拉角数据
|
||||
}BLE_KS_send_data_t;
|
||||
|
||||
#ifdef XTELL_TEST
|
||||
typedef struct{
|
||||
float acc_variance; //三轴加速度方差之和
|
||||
float gyr_variance; //三轴陀螺仪方差之和
|
||||
float acc_magnitude; //三轴加速度模长
|
||||
float gyr_magnitude; //三轴陀螺仪模长
|
||||
}debug_t;
|
||||
#endif
|
||||
/**
|
||||
* @brief 初始化滑雪追踪器
|
||||
*
|
||||
* @param tracker 指向 skiing_tracker_t 结构体的指针
|
||||
*/
|
||||
void skiing_tracker_init(skiing_tracker_t *tracker);
|
||||
|
||||
uint16_t sensor_processing_task(signed short* acc_data_buf, signed short* gyr_data_buf, float* angle_data, float* quaternion);
|
||||
#endif // SKIING_TRACKER_H
|
||||
@ -1,333 +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 1000 // 定义缓冲区可以存储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);
|
||||
}
|
||||
|
||||
// xlog("=======sensor_read_data END\n");
|
||||
|
||||
}
|
||||
|
||||
/**
|
||||
* @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));
|
||||
for (int i = 0; i < 6; i++) {
|
||||
// SC7U22_data[i] 是一个 signed short (2字节)
|
||||
// 将其低字节放在前面
|
||||
imu_packet[4 + i * 2] = (uint8_t)(data_to_send.SC7U22_data[i] & 0xFF);
|
||||
// 将其高字节放在后面
|
||||
imu_packet[4 + i * 2 + 1] = (uint8_t)((data_to_send.SC7U22_data[i] >> 8) & 0xFF);
|
||||
}
|
||||
// xlog("imu %d\n",data_to_send.SC7U22_data[0]);
|
||||
// xlog("imu_packet: 0x%x 0x%x 0x%x\n",imu_packet[4],imu_packet[5],imu_packet[6]);
|
||||
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));
|
||||
for (int i = 0; i < 9; i++) {
|
||||
mag_packet[4 + i] = data_to_send.mmc5603nj_buffer[i];
|
||||
}
|
||||
// xlog("mag: 0x%x 0x%x 0x%x\n",mag_packet[4],mag_packet[5],mag_packet[6]);
|
||||
|
||||
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){
|
||||
|
||||
}
|
||||
@ -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;
|
||||
}
|
||||
@ -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
|
||||
@ -1,211 +0,0 @@
|
||||
/*
|
||||
气压计
|
||||
*/
|
||||
#include "BMP280.h"
|
||||
#include <string.h>
|
||||
#include "os/os_api.h"
|
||||
#include "gSensor/gSensor_manage.h"
|
||||
|
||||
/*==================================================================================*/
|
||||
/* BMP280 内部定义 */
|
||||
/*==================================================================================*/
|
||||
|
||||
// 存储校准参数的静态全局变量
|
||||
static uint16_t t1;
|
||||
static int16_t t2, t3;
|
||||
static uint16_t p1;
|
||||
static int16_t p2, p3, p4, p5, p6, p7, p8, p9;
|
||||
static int32_t t_fine;
|
||||
|
||||
/*==================================================================================*/
|
||||
/* 封装的底层I2C读写函数 */
|
||||
/*==================================================================================*/
|
||||
|
||||
/**
|
||||
* @brief 写入单个字节到BMP280寄存器
|
||||
*/
|
||||
static uint8_t bmp280_write_reg(uint8_t reg, uint8_t data) {
|
||||
gravity_sensor_command(BMP_IIC_WRITE_ADDRESS, reg, data);
|
||||
return 0;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 从BMP280读取多个字节
|
||||
*/
|
||||
static uint8_t bmp280_read_regs(uint8_t reg, uint8_t *buf, uint16_t len) {
|
||||
return _gravity_sensor_get_ndata(BMP_IIC_READ_ADDRESS, reg, buf, len);
|
||||
}
|
||||
|
||||
/*==================================================================================*/
|
||||
/* 核心算法 */
|
||||
/*==================================================================================*/
|
||||
|
||||
/**
|
||||
* @brief 温度补偿计算
|
||||
* @param adc_T - 原始温度数据
|
||||
* @return 补偿后的温度值 (单位: °C)
|
||||
*/
|
||||
static float compensate_temperature(int32_t adc_T) {
|
||||
float var1, var2, temperature;
|
||||
|
||||
var1 = (((float)adc_T) / 16384.0f - ((float)t1) / 1024.0f) * ((float)t2);
|
||||
var2 = ((((float)adc_T) / 131072.0f - ((float)t1) / 8192.0f) *
|
||||
(((float)adc_T) / 131072.0f - ((float)t1) / 8192.0f)) *
|
||||
((float)t3);
|
||||
t_fine = (int32_t)(var1 + var2);
|
||||
temperature = (var1 + var2) / 5120.0f;
|
||||
|
||||
if (temperature < -40.0f) return -40.0f;
|
||||
if (temperature > 85.0f) return 85.0f;
|
||||
|
||||
return temperature;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 气压补偿计算
|
||||
* @param adc_P - 原始气压数据
|
||||
* @return 补偿后的气压值 (单位: Pa)
|
||||
*/
|
||||
static float compensate_pressure(int32_t adc_P) {
|
||||
float var1, var2, pressure;
|
||||
|
||||
var1 = ((float)t_fine / 2.0f) - 64000.0f;
|
||||
var2 = var1 * var1 * ((float)p6) / 32768.0f;
|
||||
var2 = var2 + var1 * ((float)p5) * 2.0f;
|
||||
var2 = (var2 / 4.0f) + (((float)p4) * 65536.0f);
|
||||
var1 = (((float)p3) * var1 * var1 / 524288.0f + ((float)p2) * var1) / 524288.0f;
|
||||
var1 = (1.0f + var1 / 32768.0f) * ((float)p1);
|
||||
|
||||
if (var1 == 0.0f) {
|
||||
return 0; // 避免除以零
|
||||
}
|
||||
|
||||
pressure = 1048576.0f - (float)adc_P;
|
||||
pressure = (pressure - (var2 / 4096.0f)) * 6250.0f / var1;
|
||||
var1 = ((float)p9) * pressure * pressure / 2147483648.0f;
|
||||
var2 = pressure * ((float)p8) / 32768.0f;
|
||||
pressure = pressure + (var1 + var2 + ((float)p7)) / 16.0f;
|
||||
|
||||
if (pressure < 30000.0f) return 30000.0f;
|
||||
if (pressure > 110000.0f) return 110000.0f;
|
||||
|
||||
return pressure;
|
||||
}
|
||||
|
||||
/*==================================================================================*/
|
||||
/* 外部接口函数实现 */
|
||||
/*==================================================================================*/
|
||||
|
||||
|
||||
|
||||
|
||||
uint8_t bmp280_init(void) {
|
||||
uint8_t id;
|
||||
uint8_t calib_data[24];
|
||||
|
||||
// 1. 检查芯片ID
|
||||
if (bmp280_read_regs(BMP280_REG_ID, &id, 1) == 0) {
|
||||
printf("bmp280 get id error:%d\n",id );
|
||||
return 1; // I2C读取失败
|
||||
}
|
||||
if (id != 0x58) {
|
||||
printf("bmp280 check diff:%d\n",id );
|
||||
return 1; // ID不匹配
|
||||
}
|
||||
printf("bmp280 get id:0%X\n",id );
|
||||
|
||||
// 2. 软复位
|
||||
bmp280_write_reg(BMP280_REG_RESET, 0xB6);
|
||||
os_time_dly(10); // 等待复位完成
|
||||
|
||||
// 3. 一次性读取所有校准参数
|
||||
if (bmp280_read_regs(BMP280_REG_CALIB_START, calib_data, 24) == 0) {
|
||||
return 2; // 读取校准数据失败
|
||||
}
|
||||
|
||||
// 4. 解析校准参数
|
||||
t1 = (uint16_t)(((uint16_t)calib_data[1] << 8) | calib_data[0]);
|
||||
t2 = (int16_t)(((int16_t)calib_data[3] << 8) | calib_data[2]);
|
||||
t3 = (int16_t)(((int16_t)calib_data[5] << 8) | calib_data[4]);
|
||||
p1 = (uint16_t)(((uint16_t)calib_data[7] << 8) | calib_data[6]);
|
||||
p2 = (int16_t)(((int16_t)calib_data[9] << 8) | calib_data[8]);
|
||||
p3 = (int16_t)(((int16_t)calib_data[11] << 8) | calib_data[10]);
|
||||
p4 = (int16_t)(((int16_t)calib_data[13] << 8) | calib_data[12]);
|
||||
p5 = (int16_t)(((int16_t)calib_data[15] << 8) | calib_data[14]);
|
||||
p6 = (int16_t)(((int16_t)calib_data[17] << 8) | calib_data[16]);
|
||||
p7 = (int16_t)(((int16_t)calib_data[19] << 8) | calib_data[18]);
|
||||
p8 = (int16_t)(((int16_t)calib_data[21] << 8) | calib_data[20]);
|
||||
p9 = (int16_t)(((int16_t)calib_data[23] << 8) | calib_data[22]);
|
||||
|
||||
// 5. 配置传感器 (推荐设置: 正常模式，高精度)
|
||||
// t_standby=0.5ms, filter=16, spi_en=0
|
||||
uint8_t config_reg = (0 << 5) | (4 << 2) | (0 << 0);
|
||||
bmp280_write_reg(BMP280_REG_CONFIG, config_reg);
|
||||
|
||||
// osrs_t=x2, osrs_p=x16, mode=normal
|
||||
uint8_t ctrl_meas_reg = (2 << 5) | (5 << 2) | (3 << 0);
|
||||
bmp280_write_reg(BMP280_REG_CTRL_MEAS, ctrl_meas_reg);
|
||||
|
||||
os_time_dly(10); // 等待配置生效
|
||||
|
||||
printf("bmp280 init success\n");
|
||||
return 0; // 初始化成功
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 获取转换后的温度和压力数据
|
||||
*
|
||||
* @param temperature 传出，温度
|
||||
* @param pressure 传出，压力
|
||||
* @return uint8_t
|
||||
*/
|
||||
uint8_t bmp280_read_data(float *temperature, float *pressure) {
|
||||
uint8_t data[6];
|
||||
int32_t adc_P, adc_T;
|
||||
|
||||
// printf("==========debug1===========\n");
|
||||
// 一次性读取6个字节的温度和气压原始数据
|
||||
if (bmp280_read_regs(BMP280_REG_PRESS_MSB, data, 6) == 0) {
|
||||
printf("bmp280:read data error\n");
|
||||
return 1; // 读取失败
|
||||
}
|
||||
|
||||
// printf("==========debug2===========\n");
|
||||
// 组合原始数据 (20位)
|
||||
adc_P = (int32_t)((((uint32_t)(data[0])) << 12) | (((uint32_t)(data[1])) << 4) | (((uint32_t)(data[2])) >> 4));
|
||||
adc_T = (int32_t)((((uint32_t)(data[3])) << 12) | (((uint32_t)(data[4])) << 4) | (((uint32_t)(data[5])) >> 4));
|
||||
|
||||
// 如果没有数据，直接返回错误 (ADC读数为0x80000是未测量状态)
|
||||
if (adc_T == 0x80000 || adc_P == 0x80000) {
|
||||
*temperature = 0.0f;
|
||||
*pressure = 0.0f;
|
||||
printf("bmp280:no data\n");
|
||||
return 1;
|
||||
}
|
||||
|
||||
// printf("==========debug3===========\n");
|
||||
// 进行补偿计算
|
||||
*temperature = compensate_temperature(adc_T);
|
||||
*pressure = compensate_pressure(adc_P);
|
||||
|
||||
return 0; // 成功
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 获取该气压计的原始adc数据
|
||||
*
|
||||
* @param adc_P 传出，气压
|
||||
* @param adc_T 传出，温度
|
||||
*/
|
||||
void bmp280_read_originanl_data(int* adc_P, int* adc_T){
|
||||
uint8_t data[6];
|
||||
// 一次性读取6个字节的温度和气压原始数据
|
||||
if (bmp280_read_regs(BMP280_REG_PRESS_MSB, data, 6) != 0) {
|
||||
return; // 读取失败
|
||||
}
|
||||
|
||||
// 组合原始数据 (20位)
|
||||
adc_P = (int32_t)((((uint32_t)(data[0])) << 12) | (((uint32_t)(data[1])) << 4) | (((uint32_t)(data[2])) >> 4));
|
||||
adc_T = (int32_t)((((uint32_t)(data[3])) << 12) | (((uint32_t)(data[4])) << 4) | (((uint32_t)(data[5])) >> 4));
|
||||
|
||||
}
|
||||
@ -1,54 +0,0 @@
|
||||
#ifndef BMP280_DRIVER_H
|
||||
#define BMP280_DRIVER_H
|
||||
|
||||
#include <stdint.h>
|
||||
|
||||
|
||||
#define BMP_PULL_UP 0 //外部是否接的上拉
|
||||
|
||||
// I2C 从设备地址
|
||||
#if BMP_PULL_UP == 1 //外部接的高
|
||||
#define BMP_IIC_7BIT_ADDRESS 0x76 //7位,外部接高为0x77
|
||||
#define BMP_IIC_WRITE_ADDRESS (BMP_IIC_7BIT_ADDRESS<<1) //8位地址
|
||||
#define BMP_IIC_READ_ADDRESS (BMP_IIC_WRITE_ADDRESS | 0x01)
|
||||
#else
|
||||
#define BMP_IIC_7BIT_ADDRESS 0x77 //7位,外部接低为0x76
|
||||
#define BMP_IIC_WRITE_ADDRESS (BMP_IIC_7BIT_ADDRESS<<1) //8位地址
|
||||
#define BMP_IIC_READ_ADDRESS (BMP_IIC_WRITE_ADDRESS | 0x01)
|
||||
#endif
|
||||
|
||||
|
||||
|
||||
// BMP280 寄存器地址
|
||||
#define BMP280_REG_CALIB_START 0x88
|
||||
#define BMP280_REG_ID 0xD0
|
||||
#define BMP280_REG_RESET 0xE0
|
||||
#define BMP280_REG_STATUS 0xF3
|
||||
#define BMP280_REG_CTRL_MEAS 0xF4
|
||||
#define BMP280_REG_CONFIG 0xF5
|
||||
#define BMP280_REG_PRESS_MSB 0xF7
|
||||
|
||||
/**
|
||||
* @brief 初始化BMP280传感器
|
||||
* @return 0: 成功, 1: 芯片ID错误, 2: 读取校准参数失败
|
||||
* @note 此函数会完成ID检查、软复位、读取校准参数，并设置传感器为连续测量模式。
|
||||
*/
|
||||
uint8_t bmp280_init(void);
|
||||
|
||||
/**
|
||||
* @brief 从BMP280读取温度和气压数据
|
||||
* @param[out] temperature - 指向浮点数变量的指针，用于存储温度值 (单位: °C)
|
||||
* @param[out] pressure - 指向浮点数变量的指针，用于存储气压值 (单位: Pa)
|
||||
* @return 0: 成功, 1: 读取数据失败
|
||||
*/
|
||||
uint8_t bmp280_read_data(float *temperature, float *pressure);
|
||||
|
||||
/**
|
||||
* @brief 获取该气压计的原始adc数据
|
||||
*
|
||||
* @param adc_P 传出，气压
|
||||
* @param adc_T 传出，温度
|
||||
*/
|
||||
void bmp280_read_originanl_data(int* adc_P, int* adc_T);
|
||||
|
||||
#endif // BMP280_DRIVER_H
|
||||
@ -1,283 +0,0 @@
|
||||
// LIS2DH12驱动 - 由Kilo Code注释
|
||||
#include "gSensor/gSensor_manage.h"
|
||||
#include "app_config.h"
|
||||
#include "math.h"
|
||||
#include "LIS2DH12.h"
|
||||
#include "colorful_lights/colorful_lights.h"
|
||||
#include <string.h> // 用于 memcpy
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////////////////////////////
|
||||
//START -- 宏定义
|
||||
#define ENABLE_XLOG 1
|
||||
#ifdef xlog
|
||||
#undef xlog
|
||||
#endif
|
||||
#if ENABLE_XLOG
|
||||
#define xlog(format, ...) printf("[%s] " format, __func__, ##__VA_ARGS__)
|
||||
#else
|
||||
#define xlog(format, ...) ((void)0)
|
||||
#endif
|
||||
|
||||
// --- 运动检测核心参数 ---
|
||||
#define SAMPLE_COUNT 6 // 定义静止状态检测所需的样本数量
|
||||
#define THRESHOLD 50.00f // 定义静止状态检测的阈值（三轴数据方差），值越大，对微小抖动的容忍度越高
|
||||
#define LPF_ALPHA 0.95f // 低通滤波系数，越接近1，滤波效果越强，重力估算越平滑
|
||||
#define DEADZONE_MSS 0.2f // 加速度死区阈值 (m/s^2)，低于此值的线性加速度被视为噪声并忽略
|
||||
|
||||
// --- 原有业务逻辑宏定义 ---
|
||||
#define STATIC_MAX_TIME 60*5*5 // 传感器静止最大时间，单位 200ms
|
||||
#define DORMANCY_MAX_TIME 60*5 // 休眠检测时间，单位 200ms
|
||||
|
||||
// --- I2C地址定义 ---
|
||||
#define LIS2DH12_W_ADDR 0x32
|
||||
#define LIS2DH12_R_ADDR 0x33
|
||||
|
||||
// --- IIC 寄存器地址宏定义 ---
|
||||
#define LIS2DH12_WHO_AM_I 0x01 //0F
|
||||
#define LIS2DH12_CTRL_REG1 0x20
|
||||
#define LIS2DH12_CTRL_REG4 0x23
|
||||
#define LIS2DH12_CTRL_REG5 0x24
|
||||
#define LIS2DH12_OUT_X_L 0x28
|
||||
#define LIS2DH12_FIFO_CTRL_REG 0x2E
|
||||
#define LIS2DH12_SRC_REG 0x2F
|
||||
|
||||
//END -- 宏定义
|
||||
//////////////////////////////////////////////////////////////////////////////////////////////////
|
||||
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////////////////////////////
|
||||
//START -- 变量定义
|
||||
|
||||
u8 dormancy_flag = 0; // 休眠标识
|
||||
u8 dormancy_ago_moedl = 0; // 记录休眠前灯效
|
||||
u16 gsensor_static_flag; // 记录传感器静止的时间,单位 200ms
|
||||
axis_info_t current_data[32]; // 用于存储从FIFO读取的原始传感器数据
|
||||
|
||||
//运动数据全局变量
|
||||
static motion_data_t motion_data = {{0.0f, 0.0f, 0.0f}, {0.0f, 0.0f, 0.0f}}; // 存储最终计算出的速度和距离
|
||||
static axis_info_xtell gravity_vector = {0.0f, 0.0f, -GRAVITY_EARTH}; // 存储估算出的重力向量，初始假设Z轴朝下
|
||||
static bool sensor_is_stable = false; // 传感器是否静止的标志
|
||||
static axis_info_xtell linear_accel_global = {0.0f, 0.0f, 0.0f}; // 存储移除重力后的线性加速度，用于日志打印
|
||||
static axis_info_xtell zero_g_offset = {0.0f, 0.0f, 0.0f}; // 存储开机校准测得的零点偏移量
|
||||
|
||||
u8 gsensor_alarm;
|
||||
axis_info_xtell gsensor_xtell; // 存储is_sensor_stable计算出的平均值
|
||||
|
||||
|
||||
|
||||
//END -- 变量定义
|
||||
//////////////////////////////////////////////////////////////////////////////////////////////////
|
||||
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////////////////////////////
|
||||
//START -- 函数定义
|
||||
|
||||
|
||||
|
||||
//END -- 函数定义
|
||||
//////////////////////////////////////////////////////////////////////////////////////////////////
|
||||
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////////////////////////////
|
||||
//实现
|
||||
// --- I2C底层函数封装 ---
|
||||
static u32 SL_MEMS_i2cRead(u8 addr, u8 reg, u8 len, u8 *buf) {
|
||||
return _gravity_sensor_get_ndata(addr, reg, buf, len);
|
||||
}
|
||||
static u8 SL_MEMS_i2cWrite(u8 addr, u8 reg, u8 data) {
|
||||
gravity_sensor_command(addr, reg, data);
|
||||
return 0;
|
||||
}
|
||||
|
||||
// 检查传感器ID，确认设备是否正常连接
|
||||
char LIS2DH12_Check() {
|
||||
u8 reg_value = 0;
|
||||
SL_MEMS_i2cRead(LIS2DH12_R_ADDR, LIS2DH12_WHO_AM_I, 1, ®_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);
|
||||
}
|
||||
@ -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
|
||||
@ -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);
|
||||
|
||||
|
||||
}
|
||||
@ -1,103 +0,0 @@
|
||||
#ifndef MMC5603NJ_DRIVER_H
|
||||
#define MMC5603NJ_DRIVER_H
|
||||
|
||||
#include <stdint.h>
|
||||
|
||||
|
||||
//该芯片的iic地址是固定的, 没法通过外部上下拉来改变
|
||||
#define BMP_IIC_7BIT_ADDRESS 0x30 //0110000 手册第12页
|
||||
//8位地址:
|
||||
#define MMC_IIC_WRITE_ADDRESS (BMP_IIC_7BIT_ADDRESS <<1) // 0x60 : 01100000
|
||||
#define MMC_IIC_READ_ADDRESS (MMC_IIC_WRITE_ADDRESS | 0x01) // 0x61 : 01100001
|
||||
|
||||
|
||||
|
||||
// 寄存器地址定义 -- 数据手册第6页
|
||||
#define MMC_XOUT0 0x00
|
||||
#define MMC_XOUT1 0x01
|
||||
#define MMC_YOUT0 0x02
|
||||
#define MMC_YOUT1 0x03
|
||||
#define MMC_ZOUT0 0x04
|
||||
#define MMC_ZOUT1 0x05
|
||||
#define MMC_XOUT2 0x06
|
||||
#define MMC_YOUT2 0x07
|
||||
#define MMC_ZOUT2 0x08
|
||||
#define MMC_TOUT 0x09
|
||||
#define MMC_STATUS1 0x18
|
||||
#define MMC_ODR 0x1A
|
||||
#define MMC_INCTRL0 0x1B
|
||||
#define MMC_INCTRL1 0x1C
|
||||
#define MMC_INCTRL2 0x1D
|
||||
#define MMC_ST_X_TH 0x1E
|
||||
#define MMC_ST_Y_TH 0x1F
|
||||
#define MMC_ST_Z_TH 0x20
|
||||
#define MMC_ST_X 0x27
|
||||
#define MMC_ST_Y 0x28
|
||||
#define MMC_ST_Z 0x29
|
||||
#define MMC_PID 0x39
|
||||
|
||||
// 定义一个结构体来存放三轴磁场数据（原始数据）
|
||||
typedef struct {
|
||||
float x;
|
||||
float y;
|
||||
float z;
|
||||
} mmc5603nj_original_data_t;
|
||||
|
||||
|
||||
// 定义一个结构体来存放三轴磁场数据（单位：高斯 Gauss）
|
||||
typedef struct {
|
||||
float x;
|
||||
float y;
|
||||
float z;
|
||||
} mmc5603nj_mag_data_t;
|
||||
|
||||
// 定义一个结构体来存放磁力计的硬磁偏移校准数据
|
||||
typedef struct {
|
||||
float offset_x;
|
||||
float offset_y;
|
||||
float offset_z;
|
||||
} mmc5603nj_cal_data_t;
|
||||
|
||||
/**
|
||||
* @brief 初始化MMC5603NJ传感器
|
||||
* 该函数会对传感器进行软件复位，并检查设备ID。
|
||||
* @return 0 表示成功, -1 表示失败 (设备ID不匹配).
|
||||
*/
|
||||
int mmc5603nj_init(void);
|
||||
|
||||
/**
|
||||
* @brief 设置传感器的数据输出速率 (ODR - Output Data Rate)
|
||||
* @param rate 速率值，具体含义请参考datasheet ODR寄存器说明。
|
||||
*/
|
||||
void mmc5603nj_set_data_rate(uint8_t rate);
|
||||
|
||||
/**
|
||||
* @brief 启用连续测量模式
|
||||
*/
|
||||
void mmc5603nj_enable_continuous_mode(uint8_t rate);
|
||||
|
||||
/**
|
||||
* @brief 禁用连续测量模式
|
||||
*/
|
||||
void mmc5603nj_disable_continuous_mode(void);
|
||||
|
||||
/**
|
||||
* @brief 获取产品ID
|
||||
* @return 产品的ID值，对于MMC5603NJ，应为0x10.
|
||||
*/
|
||||
uint8_t mmc5603nj_get_pid(void);
|
||||
|
||||
/**
|
||||
* @brief 读取传感器的温度
|
||||
* @return 温度值 (单位: 摄氏度 °C).
|
||||
*/
|
||||
float mmc5603nj_get_temperature(void);
|
||||
|
||||
/**
|
||||
* @brief 读取三轴磁场数据
|
||||
* 此函数会根据当前是连续模式还是单次模式来读取数据。
|
||||
* @param mag_data 指向 mmc5603nj_mag_data_t 结构体的指针，用于存放结果。
|
||||
*/
|
||||
void mmc5603nj_read_mag_data(mmc5603nj_mag_data_t *mag_data);
|
||||
|
||||
#endif // MMC5603NJ_DRIVER_H
|
||||
File diff suppressed because it is too large
Load Diff
@ -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__
|
||||
@ -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
|
||||
|
||||
@ -1,181 +0,0 @@
|
||||
/*
|
||||
气压计 - WF282A
|
||||
*/
|
||||
#include "wf282a.h"
|
||||
#include <math.h>
|
||||
#include <stdint.h> // 推荐使用标准类型
|
||||
#include "gSensor/gSensor_manage.h"
|
||||
|
||||
/*==================================================================================*/
|
||||
/* WF282A 内部定义 */
|
||||
/*==================================================================================*/
|
||||
|
||||
// 存储校准系数的静态全局变量
|
||||
static int16_t c0, c1, c01, c11, c20, c21, c30;
|
||||
static int32_t c00, c10;
|
||||
|
||||
/*==================================================================================*/
|
||||
/* 封装的底层I2C读写函数 */
|
||||
/*==================================================================================*/
|
||||
|
||||
/**
|
||||
* @brief 写入单个字节到WF282A寄存器
|
||||
*/
|
||||
static void wf282a_write_reg(uint8_t reg, uint8_t data) {
|
||||
gravity_sensor_command(WF_IIC_WRITE_ADDRESS, reg, data);
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 从WF282A读取多个字节
|
||||
*/
|
||||
static uint32_t wf282a_read_regs(uint8_t reg, uint8_t *buf, uint8_t len) {
|
||||
return _gravity_sensor_get_ndata(WF_IIC_READ_ADDRESS, reg, buf, len);
|
||||
}
|
||||
|
||||
/*==================================================================================*/
|
||||
/* 内部辅助函数 */
|
||||
/*==================================================================================*/
|
||||
|
||||
/**
|
||||
* @brief 从缓冲区中解析所有校准系数
|
||||
* @param buf 包含从寄存器0x10开始读取的18个字节的校准数据
|
||||
*/
|
||||
static void parse_calibration_data(const uint8_t *buf) {
|
||||
// c0 (12-bit)
|
||||
c0 = ((int16_t)buf[0] << 4) | (buf[1] >> 4);
|
||||
if (c0 & (1 << 11)) c0 |= 0xF000;
|
||||
|
||||
// c1 (12-bit)
|
||||
c1 = (((int16_t)buf[1] & 0x0F) << 8) | buf[2];
|
||||
if (c1 & (1 << 11)) c1 |= 0xF000;
|
||||
|
||||
// c00 (20-bit)
|
||||
c00 = ((int32_t)buf[3] << 12) | ((int32_t)buf[4] << 4) | (buf[5] >> 4);
|
||||
if (c00 & (1 << 19)) c00 |= 0xFFF00000;
|
||||
|
||||
// c10 (20-bit)
|
||||
c10 = (((int32_t)buf[5] & 0x0F) << 16) | ((int32_t)buf[6] << 8) | buf[7];
|
||||
if (c10 & (1 << 19)) c10 |= 0xFFF00000;
|
||||
|
||||
// c01, c11, c20, c21, c30 (16-bit)
|
||||
c01 = (int16_t)((uint16_t)buf[8] << 8 | buf[9]);
|
||||
c11 = (int16_t)((uint16_t)buf[10] << 8 | buf[11]);
|
||||
c20 = (int16_t)((uint16_t)buf[12] << 8 | buf[13]);
|
||||
c21 = (int16_t)((uint16_t)buf[14] << 8 | buf[15]);
|
||||
c30 = (int16_t)((uint16_t)buf[16] << 8 | buf[17]);
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 获取原始温度值 (ADC)
|
||||
*/
|
||||
static int32_t Get_Traw() {
|
||||
uint8_t buff[3];
|
||||
int32_t Traw;
|
||||
// 从 MSB 寄存器 WF_TMP_B2 (0x03) 开始连续读取3个字节
|
||||
wf282a_read_regs(WF_TMP_B2, buff, 3);
|
||||
// buff[0] = B2 (MSB), buff[1] = B1, buff[2] = B0 (LSB)
|
||||
Traw = (int32_t)buff[0] << 16 | (int32_t)buff[1] << 8 | (int32_t)buff[2];
|
||||
// 24位二进制补码转32位
|
||||
if (Traw & (1 << 23)) {
|
||||
Traw |= 0xFF000000;
|
||||
}
|
||||
return Traw;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 获取原始气压值 (ADC)
|
||||
*/
|
||||
static int32_t Get_Praw() {
|
||||
uint8_t buff[3];
|
||||
int32_t Praw;
|
||||
// 从 MSB 寄存器 WF_PRS_B2 (0x00) 开始连续读取3个字节
|
||||
wf282a_read_regs(WF_PRS_B2, buff, 3);
|
||||
// buff[0] = B2 (MSB), buff[1] = B1, buff[2] = B0 (LSB)
|
||||
Praw = (int32_t)buff[0] << 16 | (int32_t)buff[1] << 8 | (int32_t)buff[2];
|
||||
// 24位二进制补码转32位
|
||||
if (Praw & (1 << 23)) {
|
||||
Praw |= 0xFF000000;
|
||||
}
|
||||
return Praw;
|
||||
}
|
||||
|
||||
/*==================================================================================*/
|
||||
/* 4. 外部接口函数实现 */
|
||||
/*==================================================================================*/
|
||||
|
||||
uint8_t WF_Init() {
|
||||
uint8_t calib_buf[18];
|
||||
uint8_t check_cfg;
|
||||
|
||||
// 1. 配置传感器工作模式
|
||||
// 推荐配置：压力8次过采样，温度1次过采样，测量速率16Hz
|
||||
wf282a_write_reg(WF_PRS_CFG, (PM_RATE_16 << 4) | PM_PRC_8);
|
||||
wf282a_write_reg(WF_TMP_CFG, (TMP_RATE_16 << 4) | TMP_PRC_1 | TMP_INT_SENSOR);
|
||||
wf282a_write_reg(WF_MEAS_CFG, 0x07); // 启动连续压力和温度测量
|
||||
wf282a_write_reg(WF_CFG_REG, 0x00); // 无中断或FIFO移位配置
|
||||
|
||||
// 2. 一次性读取所有校准系数 (从0x10到0x21，共18字节)
|
||||
if (wf282a_read_regs(COEF_C0, calib_buf, 18) != 0) {
|
||||
return 2; // 读取校准数据失败
|
||||
}
|
||||
parse_calibration_data(calib_buf);
|
||||
|
||||
// 3. 检查配置是否写入成功
|
||||
wf282a_read_regs(WF_MEAS_CFG, &check_cfg, 1);
|
||||
if (check_cfg != 0x07) {
|
||||
return 1; // 错误
|
||||
} else {
|
||||
return 0; // 成功
|
||||
}
|
||||
}
|
||||
|
||||
void WF_Sleep() {
|
||||
wf282a_write_reg(WF_MEAS_CFG, 0x00); // 待机模式
|
||||
}
|
||||
|
||||
void WF_Wakeup() {
|
||||
wf282a_write_reg(WF_MEAS_CFG, 0x07); // 恢复连续测量
|
||||
}
|
||||
|
||||
uint8_t WF_GetID() {
|
||||
uint8_t id;
|
||||
wf282a_read_regs(WF_ID_REG, &id, 1);
|
||||
return id;
|
||||
}
|
||||
|
||||
float WF_Temperature_Calculate() {
|
||||
float Traw_sc;
|
||||
int32_t Traw = Get_Traw();
|
||||
|
||||
Traw_sc = (float)Traw / KT; // 缩放原始温度值
|
||||
return (float)c0 * 0.5f + (float)c1 * Traw_sc;
|
||||
}
|
||||
|
||||
float WF_Pressure_Calculate() {
|
||||
float Traw_sc, Praw_sc, Pcomp;
|
||||
int32_t Traw = Get_Traw();
|
||||
int32_t Praw = Get_Praw();
|
||||
|
||||
Traw_sc = (float)Traw / KT; // 缩放原始温度值
|
||||
Praw_sc = (float)Praw / KP; // 缩放原始压力值
|
||||
|
||||
// 公式: 手册给出
|
||||
Pcomp = (float)c00
|
||||
+ Praw_sc * ((float)c10 + Praw_sc * ((float)c20 + Praw_sc * (float)c30))
|
||||
+ Traw_sc * (float)c01
|
||||
+ Traw_sc * Praw_sc * ((float)c11 + Praw_sc * (float)c21);
|
||||
|
||||
return Pcomp;
|
||||
}
|
||||
|
||||
float WF_Altitude_Calculate() {
|
||||
float pressure_pa = WF_Pressure_Calculate();
|
||||
// 使用标准大气压公式计算海拔
|
||||
// P = P0 * (1 - L*h / T0)^(g*M / (R*L))
|
||||
// 简化公式: h = 44330 * (1 - (P/P0)^(1/5.255))
|
||||
// 1/5.255 ≈ 0.1903
|
||||
if (pressure_pa <= 0) {
|
||||
return 0.0f; // 避免无效计算
|
||||
}
|
||||
return 44330.0f * (1.0f - powf(pressure_pa / 101325.0f, 0.1902949f));
|
||||
}
|
||||
@ -1,148 +0,0 @@
|
||||
#ifndef _WF282A_H_
|
||||
#define _WF282A_H_
|
||||
|
||||
#include <stdint.h> // 使用标准整数类型
|
||||
|
||||
// 标定值
|
||||
#define KT 524288.0f
|
||||
#define KP 1572864.0f
|
||||
|
||||
|
||||
#define WF_PULL_UP 1 //外部是否接的上拉
|
||||
|
||||
// I2C 从设备地址
|
||||
#if WF_PULL_UP == 1 //外部接的高
|
||||
#define WF_IIC_7BIT_ADDRESS 0x77 //7位,外部接高为0x77
|
||||
#define WF_IIC_WRITE_ADDRESS (WF_IIC_7BIT_ADDRESS<<1) //8位地址
|
||||
#define WF_IIC_READ_ADDRESS (WF_IIC_WRITE_ADDRESS | 0x01)
|
||||
#else
|
||||
#define WF_IIC_7BIT_ADDRESS 0x76 //7位,外部接低为0x76
|
||||
#define WF_IIC_WRITE_ADDRESS (WF_IIC_7BIT_ADDRESS<<1) //8位地址
|
||||
#define WF_IIC_READ_ADDRESS (WF_IIC_WRITE_ADDRESS | 0x01)
|
||||
#endif
|
||||
|
||||
#define WF_CHIP_ID 0X10
|
||||
|
||||
// 寄存器映射
|
||||
// 压力数据
|
||||
#define WF_PRS_B2 0x00
|
||||
#define WF_PRS_B1 0x01
|
||||
#define WF_PRS_B0 0x02
|
||||
// 温度数据
|
||||
#define WF_TMP_B2 0x03
|
||||
#define WF_TMP_B1 0x04
|
||||
#define WF_TMP_B0 0x05
|
||||
// 配置寄存器
|
||||
#define WF_PRS_CFG 0x06
|
||||
#define WF_TMP_CFG 0x07
|
||||
#define WF_MEAS_CFG 0x08
|
||||
#define WF_CFG_REG 0x09
|
||||
#define WF_INT_STS 0x0A
|
||||
#define WF_FIFO_STS 0x0B
|
||||
#define WF_RESET_REG 0x0C
|
||||
// ID寄存器
|
||||
#define WF_ID_REG 0x0D
|
||||
// 校准系数寄存器
|
||||
#define COEF_C0 0x10
|
||||
#define COEF_C0_C1 0x11
|
||||
#define COEF_C1 0x12
|
||||
#define COEF_C00_H 0x13
|
||||
#define COEF_C00_L 0x14
|
||||
#define COEF_C00_C10 0x15
|
||||
#define COEF_C10_M 0x16
|
||||
#define COEF_C10_L 0x17
|
||||
#define COEF_C01_H 0x18
|
||||
#define COEF_C01_L 0x19
|
||||
#define COEF_C11_H 0x1A
|
||||
#define COEF_C11_L 0x1B
|
||||
#define COEF_C20_H 0x1C
|
||||
#define COEF_C20_L 0x1D
|
||||
#define COEF_C21_H 0x1E
|
||||
#define COEF_C21_L 0x1F
|
||||
#define COEF_C30_H 0x20
|
||||
#define COEF_C30_L 0x21
|
||||
|
||||
// --- 配置宏 ---
|
||||
|
||||
// 压力配置 (PRS_CFG[6:4]) - 测量速率
|
||||
#define PM_RATE_1 0x00 // 1 次/秒
|
||||
#define PM_RATE_2 0x01 // 2 次/秒
|
||||
#define PM_RATE_4 0x02 // 4 次/秒
|
||||
#define PM_RATE_8 0x03 // 8 次/秒
|
||||
#define PM_RATE_16 0x04 // 16 次/秒
|
||||
#define PM_RATE_32 0x05 // 32 次/秒
|
||||
#define PM_RATE_64 0x06 // 64 次/秒
|
||||
#define PM_RATE_128 0x07 // 128 次/秒
|
||||
// 压力配置 (PRS_CFG[3:0]) - 过采样率
|
||||
#define PM_PRC_1 0x00 // 1 次 (单次)
|
||||
#define PM_PRC_2 0x01 // 2 次 (低功耗)
|
||||
#define PM_PRC_4 0x02 // 4 次
|
||||
#define PM_PRC_8 0x03 // 8 次 (标准)
|
||||
#define PM_PRC_16 0x04 // 16 次 (需要移位)
|
||||
#define PM_PRC_32 0x05 // 32 次 (需要移位)
|
||||
#define PM_PRC_64 0x06 // 64 次 (高精度, 需要移位)
|
||||
#define PM_PRC_128 0x07 // 128 次 (需要移位)
|
||||
|
||||
// 温度配置 (TMP_CFG[7]) - 传感器源
|
||||
#define TMP_EXT_SENSOR 0x80 // 使用外部传感器
|
||||
#define TMP_INT_SENSOR 0x00 // 使用内部传感器
|
||||
// 温度配置 (TMP_CFG[6:4]) - 测量速率
|
||||
#define TMP_RATE_1 0x00 // 1 次/秒
|
||||
#define TMP_RATE_2 0x01 // 2 次/秒
|
||||
#define TMP_RATE_4 0x02 // 4 次/秒
|
||||
#define TMP_RATE_8 0x03 // 8 次/秒
|
||||
#define TMP_RATE_16 0x04 // 16 次/秒
|
||||
#define TMP_RATE_32 0x05 // 32 次/秒
|
||||
#define TMP_RATE_64 0x06 // 64 次/秒
|
||||
#define TMP_RATE_128 0x07 // 128 次/秒
|
||||
// 温度配置 (TMP_CFG[3:0]) - 过采样率
|
||||
#define TMP_PRC_1 0x00 // 1 次
|
||||
#define TMP_PRC_2 0x01 // 2 次
|
||||
#define TMP_PRC_4 0x02 // 4 次
|
||||
#define TMP_PRC_8 0x03 // 8 次
|
||||
#define TMP_PRC_16 0x04 // 16 次
|
||||
#define TMP_PRC_32 0x05 // 32 次
|
||||
#define TMP_PRC_64 0x06 // 64 次
|
||||
#define TMP_PRC_128 0x07 // 128 次
|
||||
|
||||
/**
|
||||
* @brief 初始化WF282A传感器
|
||||
* @return 0: 成功, 1: 失败
|
||||
*/
|
||||
uint8_t WF_Init(void);
|
||||
|
||||
/**
|
||||
* @brief 使传感器进入休眠/待机模式
|
||||
*/
|
||||
void WF_Sleep(void);
|
||||
|
||||
/**
|
||||
* @brief 唤醒传感器，开始连续测量
|
||||
*/
|
||||
void WF_Wakeup(void);
|
||||
|
||||
/**
|
||||
* @brief 获取传感器芯片ID
|
||||
* @return 芯片ID (应为 0x10)
|
||||
*/
|
||||
uint8_t WF_GetID(void);
|
||||
|
||||
/**
|
||||
* @brief 计算并返回当前海拔高度
|
||||
* @return 海拔高度 (单位: 米)
|
||||
*/
|
||||
float WF_Altitude_Calculate(void);
|
||||
|
||||
/**
|
||||
* @brief 计算并返回补偿后的压力值
|
||||
* @return 压力 (单位: Pa)
|
||||
*/
|
||||
float WF_Pressure_Calculate(void);
|
||||
|
||||
/**
|
||||
* @brief 计算并返回补偿后的温度值
|
||||
* @return 温度 (单位: °C)
|
||||
*/
|
||||
float WF_Temperature_Calculate(void);
|
||||
|
||||
#endif // _WF282A_H_
|
||||
@ -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");
|
||||
|
||||
}
|
||||
|
||||
89
apps/earphone/xtell_remote_control/RFID/include/CPU_CARD.h
Normal file
89
apps/earphone/xtell_remote_control/RFID/include/CPU_CARD.h
Normal file
@ -0,0 +1,89 @@
|
||||
#ifndef CPU_CARD_H
|
||||
#define CPU_CARD_H 1
|
||||
|
||||
#include "rfid_main.h" // 包含 transmission_struct 的定义
|
||||
|
||||
/**
|
||||
* @brief 存储ATS (Answer to Select) 信息的结构体
|
||||
*/
|
||||
struct ATS_STR
|
||||
{
|
||||
unsigned char Length; /**< ATS数据长度 */
|
||||
unsigned char Ats_Data[255]; /**< ATS数据缓冲区 */
|
||||
};
|
||||
|
||||
/**
|
||||
* @brief 存储PPS (Protocol and Parameter Selection) 信息的结构体
|
||||
*/
|
||||
struct PPS_STR
|
||||
{
|
||||
unsigned char Length; /**< PPS数据长度 */
|
||||
unsigned char Pps_Data[1]; /**< PPS数据缓冲区 */
|
||||
};
|
||||
|
||||
/**
|
||||
* @brief 存储CPU卡通信参数的结构体
|
||||
*/
|
||||
struct CPU_CARD_STR
|
||||
{
|
||||
unsigned char FSCI; /**< Frame Size for proximity coupling Integer */
|
||||
unsigned char FSC; /**< Frame Size for proximity coupling (in bytes) */
|
||||
unsigned char FWI; /**< Frame Waiting time Integer */
|
||||
unsigned int FWT; /**< Frame Waiting Time (in ms) */
|
||||
unsigned char SFGI; /**< Start-up Frame Guard time Integer */
|
||||
unsigned char TA; /**< TA(1) parameter from ATS */
|
||||
unsigned char TB; /**< TB(1) parameter from ATS */
|
||||
unsigned char TC; /**< TC(1) parameter from ATS */
|
||||
unsigned char PCB; /**< Protocol Control Byte */
|
||||
unsigned char WTXM; /**< Waiting Time eXtension Multiplier */
|
||||
struct ATS_STR ATS; /**< ATS信息 */
|
||||
struct PPS_STR PPS; /**< PPS信息 */
|
||||
};
|
||||
|
||||
|
||||
extern struct CPU_CARD_STR CPU_CARD;
|
||||
|
||||
/**
|
||||
* @brief 解析ATS (Answer to Select) 数据。
|
||||
* @param ats_len [in] ATS数据的长度。
|
||||
* @param ats [in] 指向ATS数据的指针。
|
||||
* @return 操作状态，SUCCESS表示成功。
|
||||
*/
|
||||
extern unsigned char Ats_Process( unsigned char ats_len, unsigned char *ats );
|
||||
|
||||
/**
|
||||
* @brief CPU卡事件处理函数（示例）。
|
||||
* @return 操作状态，SUCCESS表示成功，FAIL表示失败。
|
||||
*/
|
||||
extern unsigned char CPU_CARD_EVENT( void );
|
||||
|
||||
/**
|
||||
* @brief 封装了重试逻辑的TPDU传输函数。
|
||||
* @param tpdu [in, out] 指向传输结构体的指针。
|
||||
* @return 操作状态。
|
||||
*/
|
||||
extern unsigned char CPU_TPDU( transmission_struct *tpdu );
|
||||
|
||||
/**
|
||||
* @brief 发送RATS (Request for Answer to Select) 命令。
|
||||
* @param ats_len [out] 指向用于存储ATS长度的变量的指针。
|
||||
* @param ats [out] 指向用于存储ATS数据的缓冲区的指针。
|
||||
* @return 操作状态，SUCCESS表示成功。
|
||||
*/
|
||||
extern unsigned char CPU_Rats( unsigned char *ats_len, unsigned char *ats );
|
||||
|
||||
/**
|
||||
* @brief 发送NAK (Negative Acknowledge) 响应。
|
||||
* @param tpdu [in, out] 指向传输结构体的指针。
|
||||
* @return 操作状态。
|
||||
*/
|
||||
extern unsigned char CPU_NAK( transmission_struct *tpdu );
|
||||
|
||||
/**
|
||||
* @brief 发送APDU (Application Protocol Data Unit) 命令。
|
||||
* @param apdu [in, out] 指向传输结构体的指针，包含APDU命令和响应。
|
||||
* @return 操作状态。
|
||||
*/
|
||||
extern unsigned char CPU_APDU( transmission_struct *apdu );
|
||||
|
||||
#endif
|
||||
97
apps/earphone/xtell_remote_control/RFID/include/MIFARE.h
Normal file
97
apps/earphone/xtell_remote_control/RFID/include/MIFARE.h
Normal file
@ -0,0 +1,97 @@
|
||||
#ifndef _MIFARE_H_
|
||||
#define _MIFARE_H_
|
||||
|
||||
// 定义Mifare认证密钥类型
|
||||
#define KEY_A_M1 0
|
||||
#define KEY_B_M1 1
|
||||
|
||||
// 声明全局变量
|
||||
extern unsigned char SECTOR,BLOCK,BLOCK_NUM;
|
||||
extern unsigned char BLOCK_DATA[16];
|
||||
extern unsigned char KEY_A[16][6];
|
||||
extern unsigned char KEY_B[16][6];
|
||||
|
||||
/**
|
||||
* @brief Mifare卡事件处理函数（示例）。
|
||||
* @return 操作状态，SUCCESS表示成功，FAIL表示失败。
|
||||
*/
|
||||
extern unsigned char MIFARE_CARD_EVENT(void);
|
||||
|
||||
/**
|
||||
* @brief 清除Mifare卡加密认证标志。
|
||||
* @return 无。
|
||||
*/
|
||||
extern void Mifare_Clear_Crypto(void);
|
||||
|
||||
/**
|
||||
* @brief 将6字节的Mifare密钥加载到芯片的密钥缓冲区。
|
||||
* @param mifare_key [in] 指向6字节密钥数组的指针。
|
||||
* @return 操作状态，SUCCESS表示成功，FAIL表示失败。
|
||||
*/
|
||||
unsigned char Mifare_LoadKey(unsigned char *mifare_key);
|
||||
|
||||
/**
|
||||
* @brief 执行Mifare卡的传输（Transfer）命令。
|
||||
* @param block [in] 块号。
|
||||
* @return 操作状态，SUCCESS表示成功，FAIL表示失败。
|
||||
*/
|
||||
extern unsigned char Mifare_Transfer(unsigned char block);
|
||||
|
||||
/**
|
||||
* @brief 执行Mifare卡的恢复（Restore）命令。
|
||||
* @param block [in] 块号。
|
||||
* @return 操作状态，SUCCESS表示成功，FAIL表示失败。
|
||||
*/
|
||||
extern unsigned char Mifare_Restore(unsigned char block);
|
||||
|
||||
/**
|
||||
* @brief 将4字节数据格式化为Mifare值块格式并写入指定块。
|
||||
* @param block [in] 目标块号。
|
||||
* @param data_buff [in] 指向4字节源数据的指针。
|
||||
* @return 操作状态，SUCCESS表示成功，FAIL表示失败。
|
||||
*/
|
||||
extern unsigned char Mifare_Blockset(unsigned char block,unsigned char *data_buff);
|
||||
|
||||
/**
|
||||
* @brief 对Mifare卡的指定值块执行增值操作。
|
||||
* @param block [in] 值块的块号。
|
||||
* @param data_buff [in] 指向4字节增值数据的指针。
|
||||
* @return 操作状态，SUCCESS表示成功，FAIL表示失败。
|
||||
*/
|
||||
extern unsigned char Mifare_Blockinc(unsigned char block,unsigned char *data_buff);
|
||||
|
||||
/**
|
||||
* @brief 对Mifare卡的指定值块执行减值操作。
|
||||
* @param block [in] 值块的块号。
|
||||
* @param data_buff [in] 指向4字节减值数据的指针。
|
||||
* @return 操作状态，SUCCESS表示成功，FAIL表示失败。
|
||||
*/
|
||||
extern unsigned char Mifare_Blockdec(unsigned char block,unsigned char *data_buff);
|
||||
|
||||
/**
|
||||
* @brief 向Mifare卡写入一个16字节的数据块。
|
||||
* @param block [in] 要写入的块号 (0x00 - 0x3F)。
|
||||
* @param data_buff [in] 指向16字节数据的指针。
|
||||
* @return 操作状态，SUCCESS表示成功，FAIL表示失败。
|
||||
*/
|
||||
extern unsigned char Mifare_Blockwrite(unsigned char block,unsigned char *data_buff);
|
||||
|
||||
/**
|
||||
* @brief 从Mifare卡读取一个16字节的数据块。
|
||||
* @param block [in] 要读取的块号 (0x00 - 0x3F)。
|
||||
* @param data_buff [out] 指向16字节缓冲区的指针，用于存储读取的数据。
|
||||
* @return 操作状态，SUCCESS表示成功，FAIL表示失败。
|
||||
*/
|
||||
extern unsigned char Mifare_Blockread(unsigned char block,unsigned char *data_buff);
|
||||
|
||||
/**
|
||||
* @brief 对Mifare Classic卡片的指定扇区进行认证。
|
||||
* @param key_mode [in] 认证模式，`KEY_A_M1` (0) 表示使用密钥A，`KEY_B_M1` (1) 表示使用密钥B。
|
||||
* @param sector [in] 要认证的扇区号 (0-15)。
|
||||
* @param mifare_key [in] 指向6字节认证密钥的指针。
|
||||
* @param card_uid [in] 指向4字节卡片UID的指针。
|
||||
* @return 操作状态，SUCCESS表示认证成功，FAIL表示失败。
|
||||
*/
|
||||
extern unsigned char Mifare_Auth(unsigned char key_mode,unsigned char sector,unsigned char *mifare_key,unsigned char *card_uid);
|
||||
|
||||
#endif
|
||||
8
apps/earphone/xtell_remote_control/RFID/include/NTAG.h
Normal file
8
apps/earphone/xtell_remote_control/RFID/include/NTAG.h
Normal file
@ -0,0 +1,8 @@
|
||||
#ifndef _NTAG_H
|
||||
#define _NTAG_H
|
||||
|
||||
extern unsigned char PAGE_DATA[16];
|
||||
extern unsigned char NTAG_EVENT(void);
|
||||
extern unsigned char Read_Page(unsigned char page_num,unsigned char *page_data);
|
||||
extern unsigned char Write_Page(unsigned char page_num,unsigned char *page_data);
|
||||
#endif
|
||||
153
apps/earphone/xtell_remote_control/RFID/include/READER.h
Normal file
153
apps/earphone/xtell_remote_control/RFID/include/READER.h
Normal file
@ -0,0 +1,153 @@
|
||||
/********************************************************************************************************
|
||||
* @file READER.h
|
||||
* @brief RFID 读卡器底层驱动及协议头文件
|
||||
* @details
|
||||
* 本文件定义了与RFID芯片交互所需的常量、数据结构和函数原型。
|
||||
*
|
||||
* @author Kilo Code
|
||||
* @date 2025-11-24
|
||||
* @version 1.0
|
||||
********************************************************************************************************/
|
||||
|
||||
#ifndef _READER_H
|
||||
#define _READER_H
|
||||
|
||||
/********************************************************************************************************
|
||||
* 常量定义
|
||||
********************************************************************************************************/
|
||||
|
||||
// ISO14443A 命令码
|
||||
static const unsigned char RF_CMD_REQA = 0x26; /**< 请求命令 */
|
||||
static const unsigned char RF_CMD_WUPA = 0x52; /**< 唤醒命令 */
|
||||
static const unsigned char RF_CMD_ANTICOLL[3] = {0x93, 0x95, 0x97}; /**< 防冲突命令，根据级联级别选择 */
|
||||
static const unsigned char RF_CMD_SELECT[3] = {0x93, 0x95, 0x97}; /**< 选择命令，根据级联级别选择 */
|
||||
|
||||
// MIFARE Classic 命令码
|
||||
static const unsigned char RF_CMD_KEYA = 0x60; /**< 密钥A认证 */
|
||||
static const unsigned char RF_CMD_KEYB = 0x61; /**< 密钥B认证 */
|
||||
|
||||
|
||||
/********************************************************************************************************
|
||||
* 卡片信息结构体
|
||||
********************************************************************************************************/
|
||||
|
||||
/**
|
||||
* @brief 存储ISO/IEC 14443 Type B卡片信息的结构体
|
||||
*/
|
||||
struct picc_b_struct
|
||||
{
|
||||
unsigned char ATQB[12]; /**< REQB/WUPB的响应 (Answer to Request B) */
|
||||
unsigned char PUPI[4]; /**< Pseudo-Unique PICC Identifier */
|
||||
unsigned char APPLICATION_DATA[4]; /**< 应用数据 */
|
||||
unsigned char PROTOCOL_INF[3]; /**< 协议信息 */
|
||||
unsigned char CID; /**< 卡片ID (Card Identifier) */
|
||||
unsigned char Answer_to_HALT[1]; /**< HALT命令的响应 */
|
||||
unsigned char SN[8]; /**< 序列号 (自定义命令获取) */
|
||||
};
|
||||
extern struct picc_b_struct PICC_B;
|
||||
|
||||
/**
|
||||
* @brief 存储ISO/IEC 14443 Type A卡片信息的结构体
|
||||
*/
|
||||
struct picc_a_struct
|
||||
{
|
||||
unsigned char ATQA[2]; /**< REQA/WUPA的响应 (Answer to Request A) */
|
||||
unsigned char CASCADE_LEVEL; /**< 当前级联级别 (用于处理多级UID) */
|
||||
unsigned char UID_Length; /**< UID的长度 (4, 7, or 10 bytes) */
|
||||
unsigned char UID[15]; /**< 卡片唯一ID (Unique Identifier) */
|
||||
unsigned char BCC[3]; /**< 块校验字符 (Block Check Character) */
|
||||
unsigned char SAK[3]; /**< 选择确认 (Select Acknowledge) */
|
||||
};
|
||||
extern struct picc_a_struct PICC_A;
|
||||
|
||||
/**
|
||||
* @brief 存储ISO/IEC 15693 (Type V) 卡片信息的结构体
|
||||
*/
|
||||
struct picc_v_struct
|
||||
{
|
||||
unsigned char UID[8]; /**< 卡片唯一ID (Unique Identifier) */
|
||||
unsigned char RESPONSE; /**< 命令响应标志 */
|
||||
unsigned char BLOCK_DATA[4]; /**< 读取或写入的块数据 */
|
||||
};
|
||||
extern struct picc_v_struct PICC_V;
|
||||
|
||||
/**
|
||||
* @brief 存储FeliCa (Type F) 卡片信息的结构体
|
||||
*/
|
||||
struct picc_f_struct
|
||||
{
|
||||
unsigned char UID[8]; /**< 卡片唯一ID (Unique Identifier) */
|
||||
};
|
||||
extern struct picc_f_struct PICC_F;
|
||||
|
||||
|
||||
/********************************************************************************************************
|
||||
* 芯片参数配置
|
||||
********************************************************************************************************/
|
||||
|
||||
// --- Type A 参数 ---
|
||||
#define GAIN_A 7 // 接收增益 (范围 0~7)
|
||||
#define HPCF_A 3 // 高通滤波器截止频率 (范围 0~7)
|
||||
#define AMPLITUDE_A 255 // RF场幅度 (范围 0~255)
|
||||
|
||||
// --- Type B 参数 ---
|
||||
#define GAIN_B 7 // 接收增益
|
||||
#define HPCF_B 3 // 高通滤波器截止频率
|
||||
#define AMPLITUDE_B 255 // RF场幅度
|
||||
#define MODULATION_B 100 // 调制深度 (范围 0~255, 值越小调制越深)
|
||||
|
||||
// --- Type V (ISO15693) 参数 ---
|
||||
#define GAIN_V 7 // 接收增益
|
||||
#define HPCF_V 4 // 高通滤波器截止频率
|
||||
#define AMPLITUDE_V 255 // RF场幅度
|
||||
#define MODULATION_V 10 // 调制深度
|
||||
|
||||
// --- Type F (FeliCa) 参数 ---
|
||||
#define GAIN_F 7 // 接收增益
|
||||
#define HPCF_F 4 // 高通滤波器截止频率
|
||||
#define AMPLITUDE_F 255 // RF场幅度
|
||||
#define MODULATION_F 100 // 调制深度
|
||||
|
||||
|
||||
/********************************************************************************************************
|
||||
* 函数原型声明
|
||||
********************************************************************************************************/
|
||||
|
||||
// --- 通用函数 ---
|
||||
extern void ModifyReg(unsigned char reg_address, unsigned char mask, unsigned char set);
|
||||
extern void Clear_FIFO(void);
|
||||
extern unsigned char SetCommand(unsigned char command);
|
||||
extern void SetParity(unsigned char state);
|
||||
extern void SetTimer(unsigned int timeout);
|
||||
extern unsigned char SetCW(unsigned char mode);
|
||||
|
||||
// --- 协议初始化函数 ---
|
||||
extern unsigned char ReaderA_Initial(void);
|
||||
extern unsigned char ReaderB_Initial(void);
|
||||
extern unsigned char ReaderV_Initial(void);
|
||||
extern unsigned char ReaderF_Initial(void);
|
||||
|
||||
// --- Type A 命令 ---
|
||||
extern unsigned char ReaderA_Wakeeup(struct picc_a_struct *picc_a);
|
||||
extern unsigned char ReaderA_Request(struct picc_a_struct *picc_a);
|
||||
extern unsigned char ReaderA_Anticoll(struct picc_a_struct *picc_a);
|
||||
extern unsigned char ReaderA_Select(struct picc_a_struct *picc_a);
|
||||
extern unsigned char ReaderA_CardActivate(struct picc_a_struct *picc_a);
|
||||
|
||||
// --- Type B 命令 ---
|
||||
extern unsigned char ReaderB_Wakeup(struct picc_b_struct *picc_b);
|
||||
extern unsigned char ReaderB_Request(struct picc_b_struct *picc_b);
|
||||
extern unsigned char ReaderB_Attrib(struct picc_b_struct *picc_b);
|
||||
extern unsigned char ReaderB_Halt(struct picc_b_struct *picc_b);
|
||||
extern unsigned char ReaderB_Get_SN(struct picc_b_struct *picc_b);
|
||||
|
||||
// --- Type V (ISO15693) 命令 ---
|
||||
extern unsigned char ReaderV_Inventory(struct picc_v_struct *picc_v);
|
||||
extern unsigned char ReaderV_Select(struct picc_v_struct *picc_v);
|
||||
extern unsigned char ReaderV_ReadSingleBlock(unsigned char block_num, struct picc_v_struct *picc_v);
|
||||
extern unsigned char ReaderV_WriteSingleBlock(unsigned char block_num, struct picc_v_struct *picc_v);
|
||||
|
||||
// --- Type F (FeliCa) 命令 ---
|
||||
extern unsigned char ReaderF_Inventory(struct picc_f_struct *picc_f);
|
||||
|
||||
#endif // _READER_H
|
||||
479
apps/earphone/xtell_remote_control/RFID/include/READER_REG.h
Normal file
479
apps/earphone/xtell_remote_control/RFID/include/READER_REG.h
Normal file
@ -0,0 +1,479 @@
|
||||
/*********************************************************************
|
||||
* *
|
||||
* Copyright (c) 2010 Shanghai FuDan MicroElectronic Inc, Ltd. *
|
||||
* All rights reserved. Licensed Software Material. *
|
||||
* *
|
||||
* Unauthorized use, duplication, or distribution is strictly *
|
||||
* prohibited by law. *
|
||||
* *
|
||||
**********************************************************************/
|
||||
#ifndef _READER_REG_H
|
||||
#define _READER_REG_H
|
||||
|
||||
#define REG_COMMAND 0x00 //
|
||||
#define REG_HOSTCTRL 0x01 //
|
||||
#define REG_FIFOCONTROL 0x02 //
|
||||
#define REG_WATERLEVEL 0x03 //
|
||||
#define REG_FIFOLENGTH 0x04 //
|
||||
#define REG_FIFODATA 0x05 //
|
||||
#define REG_IRQ0 0x06 //
|
||||
#define REG_IRQ1 0x07 //
|
||||
#define REG_IRQ0EN 0x08 //
|
||||
#define REG_IRQ1EN 0x09 //
|
||||
#define REG_ERROR 0x0A //
|
||||
#define REG_STATUS 0x0B //
|
||||
#define REG_RXBITCTRL 0x0C //
|
||||
#define REG_RXCOLL 0x0D //
|
||||
#define REG_TCONTROL 0x0E //
|
||||
#define REG_T0CONTROL 0x0F //
|
||||
|
||||
|
||||
#define REG_T0RELOADHI 0x10 //
|
||||
#define REG_T0RELOADLO 0x11 //
|
||||
#define REG_T0COUNTERVALHI 0x12 //
|
||||
#define REG_T0COUNTERVALLO 0x13 //
|
||||
#define REG_T1CONTROL 0x14 //
|
||||
#define REG_T1RELOADHI 0x15 //
|
||||
#define REG_T1RELOADLO 0x16 //
|
||||
#define REG_T1COUNTERVALHI 0x17 //
|
||||
#define REG_T1COUNTERVALLO 0x18 //
|
||||
#define REG_T2CONTROL 0x19 //
|
||||
#define REG_T2RELOADHI 0x1A //
|
||||
#define REG_T2RELOADLO 0x1B //
|
||||
#define REG_T2COUNTERVALHI 0x1C //
|
||||
#define REG_T2COUNTERVALLO 0x1D //
|
||||
#define REG_T3CONTROL 0x1E //
|
||||
#define REG_T3RELOADHI 0x1F //
|
||||
|
||||
|
||||
#define REG_T3RELOADLO 0x20 //
|
||||
#define REG_T3COUNTERVALHI 0x21 //
|
||||
#define REG_T3COUNTERVALLO 0x22 //
|
||||
#define REG_T4CONTROL 0x23 //
|
||||
#define REG_T4RELOADHI 0x24 //
|
||||
#define REG_T4RELOADLO 0x25 //
|
||||
#define REG_T4COUNTERVALHI 0x26 //
|
||||
#define REG_T4COUNTERVALLO 0x27 //
|
||||
#define REG_TXMODE 0x28
|
||||
#define REG_TXAMP 0x29
|
||||
#define REG_TXCON 0x2A //
|
||||
#define REG_TXI 0x2B //
|
||||
#define REG_TXCRCCON 0x2C //
|
||||
#define REG_RXCRCCON 0x2D //
|
||||
#define REG_TXDATANUM 0x2E
|
||||
#define REG_TXMODWIDTH 0x2F //
|
||||
|
||||
|
||||
#define REG_TXSYM10BURSTLEN 0x30 //
|
||||
#define REG_TXWAITCTRL 0x31 //
|
||||
#define REG_TXWAITLO 0x32 //
|
||||
#define REG_FRAMECON 0x33 //
|
||||
#define REG_RXSOFD 0x34 //
|
||||
#define REG_RXCTRL 0x35 //
|
||||
#define REG_RXWAIT 0x36 //
|
||||
#define REG_RXTHRESHOLD 0x37 //
|
||||
#define REG_RCV 0x38 //
|
||||
#define REG_RXANA 0x39 //
|
||||
#define REG_LPCD_OPTIONS 0x3A //
|
||||
#define REG_SERIALSPEED 0x3B //
|
||||
#define REG_LFO_TRIMM 0x3C //
|
||||
#define REG_CLKOUT_CTRL 0x3D //
|
||||
#define REG_LPCD_THRESHOLD 0x3E //
|
||||
#define REG_LPCD_QMIN 0x3F //
|
||||
#define REG_LPCD_QMAX 0x40
|
||||
#define REG_LPCD_IMIN 0x41
|
||||
#define REG_LPCD_RESULT_I 0x42
|
||||
#define REG_LPCD_RESULT_Q 0x43
|
||||
#define REG_THNADJ 0x5F
|
||||
#define REG_THNSET 0x61
|
||||
#define REG_THNMIN 0x62
|
||||
#define REG_DSP_CTRL1 0x64
|
||||
#define REG_MISC 0x75
|
||||
#define REG_RXTXCON 0x77
|
||||
#define REG_ERROREXT 0x7E
|
||||
#define REG_VERSION 0x7F
|
||||
|
||||
#define CMD_MASK 0x1F
|
||||
|
||||
#define CMD_IDLE 0x00
|
||||
#define CMD_LPCD 0x01
|
||||
#define CMD_LOADKEY 0x02
|
||||
#define CMD_AUTHENT 0x03
|
||||
#define CMD_RECEIVE 0x05
|
||||
#define CMD_TRANSMIT 0x06
|
||||
#define CMD_TRANSCEIVE 0x07
|
||||
#define CMD_WRITEE2 0x08
|
||||
#define CMD_WRITEE2PAGE 0x09
|
||||
#define CMD_READE2 0x0A
|
||||
#define CMD_LOADREG 0x0C
|
||||
#define CMD_LOADPROTOCOL 0x0D
|
||||
#define CMD_LOADKEYE2 0x0E
|
||||
#define CMD_STOREKEYE2 0x0F
|
||||
#define CMD_CRCCALC 0x1B
|
||||
#define CMD_READRNR 0x1C
|
||||
#define CMD_SOFTRESET 0x1F
|
||||
|
||||
|
||||
/** \name Host-Control Register Contents (0x00)
|
||||
*/
|
||||
/*@{*/
|
||||
#define BIT_STANDBY 0x80U /**< Standby bit; If set, the IC transits to standby mode. */
|
||||
#define BIT_MODEMOFF 0x40U
|
||||
/*@{*/
|
||||
/** \name Host-Control Register Contents (0x01)
|
||||
*/
|
||||
/*@{*/
|
||||
#define BIT_I2CFORCEHS 0x01U
|
||||
//#define BIT_REGEN 0x80U
|
||||
//#define BIT_BUSHOST 0x40U
|
||||
//#define BIT_BUSSAM 0x20U
|
||||
//#define MASK_SAMINTERFACE 0x0CU
|
||||
/*@}*/
|
||||
|
||||
/** \name FIFO-Control Register Contents (0x02)
|
||||
*/
|
||||
/*@{*/
|
||||
#define BIT_FIFOSIZE 0x80U
|
||||
#define BIT_HIALERT 0x40U
|
||||
#define BIT_LOALERT 0x20U
|
||||
#define BIT_FIFOFLUSH 0x10U
|
||||
#define BIT_WATERLEVEL_HI 0x04U
|
||||
#define MASK_FIFOLENGTH_HI 0x03U
|
||||
/*@}*/
|
||||
|
||||
/** \name IRQ0 Register(s) Contents (0x06/0x08)
|
||||
*/
|
||||
/*@{*/
|
||||
#define BIT_SET 0x80U
|
||||
#define BIT_IRQINV 0x80U
|
||||
#define BIT_HIALERTIRQ 0x40U
|
||||
#define BIT_LOALERTIRQ 0x20U
|
||||
#define BIT_IDLEIRQ 0x10U
|
||||
#define BIT_IDLE_IRQ BIT_IDLEIRQ /* Alias for compatibility */
|
||||
#define BIT_TXIRQ 0x08U
|
||||
#define BIT_RXIRQ 0x04U
|
||||
#define BIT_ERRIRQ 0x02U
|
||||
#define BIT_RXSOFIRQ 0x01U
|
||||
/*@}*/
|
||||
|
||||
/** \name IRQ1 Register(s) Contents (0x07/0x09)
|
||||
*/
|
||||
/*@{*/
|
||||
/* #define BIT_SET 0x80U */
|
||||
#define BIT_IRQPUSHPULL 0x80U
|
||||
#define BIT_GLOBALIRQ 0x40U
|
||||
#define BIT_IRQPINEN 0x40U
|
||||
#define BIT_LPCDIRQ 0x20U
|
||||
#define BIT_TIMER4IRQ 0x10U
|
||||
#define BIT_TIMER3IRQ 0x08U
|
||||
#define BIT_TIMER2IRQ 0x04U
|
||||
#define BIT_TIMER1IRQ 0x02U
|
||||
#define BIT_TIMER0IRQ 0x01U
|
||||
/*@}*/
|
||||
|
||||
/** \name Error Register Contents (0x0A)
|
||||
*/
|
||||
/*@{*/
|
||||
#define BIT_CMDEE_ERR 0x80U
|
||||
#define BIT_FIFOWRERR 0x40U
|
||||
#define BIT_FIFOOVL 0x20U
|
||||
#define BIT_MINFRAMEERR 0x10U
|
||||
#define BIT_NODATAERR 0x08U
|
||||
#define BIT_COLLDET 0x04U
|
||||
#define BIT_PROTERR 0x02U
|
||||
#define BIT_INTEGERR 0x01U
|
||||
/*@}*/
|
||||
|
||||
/** \name Status Register Contents (0x0B)
|
||||
*/
|
||||
/*@{*/
|
||||
#define BIT_CRYPTO1ON 0x20U
|
||||
#define MASK_COMMSTATE 0x07U
|
||||
/*@}*/
|
||||
|
||||
/** \name Rx-Bit-Control Register Contents (0x0C)
|
||||
*/
|
||||
/*@{*/
|
||||
#define BIT_VALUESAFTERCOLL 0x80U
|
||||
#define BIT_NOCOLL 0x08U
|
||||
#define MASK_RXALIGN 0x70U
|
||||
#define MASK_RXLASTBITS 0x07U
|
||||
/*@}*/
|
||||
|
||||
/** \name Rx-Coll Register Contents (0x0D)
|
||||
*/
|
||||
/*@{*/
|
||||
#define BIT_COLLPOSVALID 0x80U
|
||||
#define MASK_COLLPOS 0x7FU
|
||||
/*@}*/
|
||||
|
||||
/** \name Timer-Control Register Contents (0x0E)
|
||||
*/
|
||||
/*@{*/
|
||||
#define BIT_T3RUNNING 0x80U
|
||||
#define BIT_T2RUNNING 0x40U
|
||||
#define BIT_T1RUNNING 0x20U
|
||||
#define BIT_T0RUNNING 0x10U
|
||||
#define BIT_T3STARTSTOPNOW 0x08U
|
||||
#define BIT_T2STARTSTOPNOW 0x04U
|
||||
#define BIT_T1STARTSTOPNOW 0x02U
|
||||
#define BIT_T0STARTSTOPNOW 0x01U
|
||||
/*@}*/
|
||||
|
||||
/** \name T[0-3]-Control Register Contents (0x0F/0x14/0x19/0x1E)
|
||||
*/
|
||||
/*@{*/
|
||||
#define BIT_TSTOP_RX 0x80U /**< Stop timer on receive interrupt. */
|
||||
#define BIT_TAUTORESTARTED 0x08U /**< Auto-restart timer after underflow. */
|
||||
#define BIT_TSTART_TX 0x10U /**< Start timer on transmit interrupt. */
|
||||
//#define BIT_TSTART_LFO 0x20U /**< Use this timer for LFO trimming. */
|
||||
//#define BIT_TSTART_LFO_UV 0x30U /**< Use this timer for LFO trimming (generate UV at a trimming event). */
|
||||
#define MASK_TSTART 0x30U /**< Mask for TSTART bits. */
|
||||
#define VALUE_TCLK_1356_MHZ 0x00U /**< Use 13.56MHz as input clock. */
|
||||
#define VALUE_TCLK_212_KHZ 0x01U /**< Use 212KHz as input clock. */
|
||||
#define VALUE_TCLK_T0 0x02U /**< Use timer0 as input clock. */
|
||||
#define VALUE_TCLK_T1 0x03U /**< Use timer1 as input clock. */
|
||||
/*@}*/
|
||||
|
||||
/** \name T4-Control Register Contents (0x23)
|
||||
*/
|
||||
/*@{*/
|
||||
#define BIT_T4RUNNING 0x80U
|
||||
#define BIT_T4STARTSTOPNOW 0x40U
|
||||
#define BIT_T4AUTOTRIMM 0x20U
|
||||
#define BIT_T4AUTOLPCD 0x10U
|
||||
#define BIT_T4AUTORESTARTED 0x08U
|
||||
#define BIT_T4AUTOWAKEUP 0x04U
|
||||
/*#define MASK_TSTART 0x30U*/
|
||||
#define VALUE_TCLK_LFO_64_KHZ 0x00U
|
||||
#define VALUE_TCLK_LFO_8_KHZ 0x01U
|
||||
#define VALUE_TCLK_LFO_4_KHZ 0x02U
|
||||
#define VALUE_TCLK_LFO_2_KHZ 0x03U
|
||||
/*@}*/
|
||||
|
||||
/** \name Driver Mode Register Contents (0x28)
|
||||
*/
|
||||
/*@{*/
|
||||
#define BIT_TX2INV 0x80U
|
||||
#define BIT_TX1INV 0x40U
|
||||
#define BIT_TXEN 0x08U
|
||||
#define VALUE_TXCLKMODE_HIGHIMPEDANCE 0x00U
|
||||
#define VALUE_TXCLKMODE_OUTPULL0 0x01U
|
||||
#define VALUE_TXCLKMODE_OUTPULL1 0x02U
|
||||
#define VALUE_TXCLKMODE_RFLOWPULL 0x05U
|
||||
#define VALUE_TXCLKMODE_RFHIGHPUSH 0x06U
|
||||
#define VALUE_TXCLKMODE_PUSHPULL 0x07U
|
||||
#define BIT_RFON 0x04U
|
||||
#define BIT_TPUSHON 0x02U
|
||||
#define BIT_TPULLON 0x01U
|
||||
/*@}*/
|
||||
|
||||
/** \name Tx Amplifier Register Contents (0x29)
|
||||
*/
|
||||
/*@{*/
|
||||
#define MASK_CW_AMPLITUDE 0x00U
|
||||
#define MASK_RESIDUAL_CARRIER 0x1FU
|
||||
/*@}*/
|
||||
|
||||
/** \name Driver Control Register Contents (0x2A)
|
||||
*/
|
||||
/*@{*/
|
||||
#define BIT_CWMAX 0x08U
|
||||
#define BIT_DRIVERINV 0x04U
|
||||
#define VALUE_DRIVERSEL_LOW 0x00U
|
||||
#define VALUE_DRIVERSEL_TXENV 0x01U
|
||||
#define VALUE_DRIVERSEL_SIGIN 0x02U
|
||||
/*@}*/
|
||||
|
||||
/** \name Tx-/Rx-CRC Control Register Contents (0x2C/0x2D)
|
||||
*/
|
||||
/*@{*/
|
||||
#define BIT_RXFORCECRCWRITE 0x80U
|
||||
#define BIT_CRCINVERT 0x02U
|
||||
#define BIT_CRCEN 0x01U
|
||||
#define MASK_CRCPRESETVAL 0x70U
|
||||
#define MASK_CRCTYPE 0x0CU
|
||||
#define MASK_CRCTYPE5 0x00U
|
||||
#define MASK_CRCTYPE16 0x08U
|
||||
/*@}*/
|
||||
|
||||
/** \name Tx-DataNum Register Contents (0x2E)
|
||||
*/
|
||||
/*@{*/
|
||||
#define BIT_KEEPBITGRID 0x10U
|
||||
#define BIT_DATAEN 0x08U
|
||||
#define MASK_TXLASTBITS 0x07U
|
||||
#define MASK_SYMBOL_SEND 0x08U
|
||||
/*@}*/
|
||||
|
||||
/** \name Tx-Wait Control Register Contents (0x31)
|
||||
*/
|
||||
/*@{*/
|
||||
#define BIT_TXWAIT_START_RX 0x80U
|
||||
#define BIT_TXWAIT_DBFREQ 0x40U
|
||||
#define MASK_TXWAITHI 0x38U
|
||||
#define MASK_TXSTOPBITLEN 0x07U
|
||||
/*@}*/
|
||||
|
||||
/** \name Frame Control Register Contents (0x33)
|
||||
*/
|
||||
/*@{*/
|
||||
#define BIT_TXPARITYEN 0x80U
|
||||
#define BIT_RXPARITYEN 0x40U
|
||||
#define VALUE_STOP_SYM3 0x0CU
|
||||
#define VALUE_STOP_SYM2 0x08U
|
||||
#define VALUE_STOP_SYM1 0x04U
|
||||
#define VALUE_START_SYM3 0x03U
|
||||
#define VALUE_START_SYM2 0x02U
|
||||
#define VALUE_START_SYM1 0x01U
|
||||
#define MASK_STARTSYM 0x03U
|
||||
#define MASK_STOPSYM 0x0CU
|
||||
/*@}*/
|
||||
|
||||
/** \name Rx Control Register Contents (0x35)
|
||||
*/
|
||||
/*@{*/
|
||||
#define BIT_RXALLOWBITS 0x80U
|
||||
#define BIT_RXMULTIPLE 0x40U
|
||||
#define BIT_RXEOFTYPE 0x20U
|
||||
#define BIT_EGT_CHECK 0x10U
|
||||
#define BIT_EMD_SUPPRESSION 0x08U
|
||||
#define MASK_RXBAUDRATE 0x07U
|
||||
/*@}*/
|
||||
|
||||
/** \name Rx-Wait Register Contents (0x36)
|
||||
*/
|
||||
/*@{*/
|
||||
#define BIT_RXWAITDBFREQ 0x80U
|
||||
#define MASK_RXWAIT 0x7FU
|
||||
/*@}*/
|
||||
|
||||
/** \name Rx-Threshold Register Contents (0x37)
|
||||
*/
|
||||
/*@{*/
|
||||
#define MASK_MINLEVEL 0xF0U
|
||||
#define MASK_MINLEVELP 0x0FU
|
||||
/*@}*/
|
||||
|
||||
/** \name Rx-Receiver Register Contents (0x38)
|
||||
*/
|
||||
/*@{*/
|
||||
#define BIT_RX_SINGLE 0x80U
|
||||
#define BIT_RX_SHORT_MIX2ADC 0x40U
|
||||
#define BIT_USE_SMALL_EVAL 0x04U
|
||||
#define MASK_RX_SIGPRO_IN_SEL 0x30U
|
||||
#define MASK_COLLLEVEL 0x03U
|
||||
/*@}*/
|
||||
|
||||
/** \name Rx-Analog Register Contents (0x39)
|
||||
*/
|
||||
/*@{*/
|
||||
#define BIT_RX_OC_FUN_ENABLE 0x20U
|
||||
#define BIT_RX_HP_LOWF 0x10U
|
||||
#define MASK_VMID_R_SEL 0xC0U
|
||||
#define MASK_RCV_HPCF 0x0CU
|
||||
#define MASK_RCV_GAIN 0x03U
|
||||
/*@}*/
|
||||
|
||||
/** \name Serial-Speed Register Contents (0x3B)
|
||||
*/
|
||||
/*@{*/
|
||||
#define MASK_BR_T0 0xE0U
|
||||
#define MASK_BR_T1 0x1FU
|
||||
/*@}*/
|
||||
|
||||
|
||||
/** \name LPCD Result(Q) Register Contents (0x43)
|
||||
*/
|
||||
/*@{*/
|
||||
#define BIT_LPCDIRQ_CLR 0x40U
|
||||
/*@}*/
|
||||
|
||||
/** \name Tx-BitMod Register Contents (0x48)
|
||||
*/
|
||||
/*@{*/
|
||||
#define BIT_TXMSBFIRST 0x80U
|
||||
#define BIT_TXPARITYTYPE 0x20U
|
||||
#define BIT_TXSTOPBITTYPE 0x08U
|
||||
#define BIT_TXSTARTBITTYPE 0x02U
|
||||
#define BIT_TXSTARTBITEN 0x01U
|
||||
/*@}*/
|
||||
|
||||
/** \name Rx-BitMod Register Contents (0x58)
|
||||
*/
|
||||
/*@{*/
|
||||
#define BIT_RXSTOPONINVPAR 0x20U
|
||||
#define BIT_RXSTOPONLEN 0x10U
|
||||
#define BIT_RXMSBFIRST 0x08U
|
||||
#define BIT_RXSTOPBITEN 0x04U
|
||||
#define BIT_RXPARITYTYPE 0x02U
|
||||
/*@}*/
|
||||
|
||||
/** \name Rx-Mod Register Contents (0x5D)
|
||||
*/
|
||||
/*@{*/
|
||||
#define BIT_PREFILTER 0x20U
|
||||
#define BIT_RECTFILTER 0x10U
|
||||
#define BIT_SYNCHIGH 0x08U
|
||||
#define BIT_CORRINV 0x04U
|
||||
#define BIT_FSK 0x02U
|
||||
#define BIT_BPSK 0x01U
|
||||
/*@}*/
|
||||
|
||||
/** \name RxSupCfg Register Contents (0x6E)
|
||||
*/
|
||||
/*@{*/
|
||||
#define BIT_RXNOERR 0x80U
|
||||
/*@}*/
|
||||
/** \name RxTxConReg Register Contents (0x77)
|
||||
*/
|
||||
/*@{*/
|
||||
#define BIT_SHMODE 0x08U //<2F>Ϻ<EFBFBD><CFBA>㷨
|
||||
/*@}*/
|
||||
|
||||
/** \name ErrorExtReg Register Contents (0x7E)
|
||||
*/
|
||||
/*@{*/
|
||||
#define PARITY_ERROR 0x08U
|
||||
#define CRC_ERROR 0x04U
|
||||
/*@{*/
|
||||
|
||||
#define LPCD_OPTION2 0x1DF
|
||||
|
||||
//---------------------------------------------------------------
|
||||
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Э<EFBFBD><D0AD><EFBFBD>
|
||||
#define RX_TYPEA_106 0
|
||||
#define RX_TYPEA_212 1
|
||||
#define RX_TYPEA_424 2
|
||||
#define RX_TYPEA_848 3
|
||||
|
||||
#define RX_TYPEB_106 4
|
||||
#define RX_TYPEB_212 5
|
||||
#define RX_TYPEB_424 6
|
||||
#define RX_TYPEB_848 7
|
||||
|
||||
#define RX_TYPEV_26 10
|
||||
#define RX_TYPEV_53 11
|
||||
|
||||
#define RX_FELICA_212 19
|
||||
#define RX_FELICA_424 20
|
||||
|
||||
//<2F><><EFBFBD>巢<EFBFBD><E5B7A2>Э<EFBFBD><D0AD><EFBFBD>
|
||||
#define TX_TYPEA_106 0
|
||||
#define TX_TYPEA_212 1
|
||||
#define TX_TYPEA_424 2
|
||||
#define TX_TYPEA_848 3
|
||||
|
||||
#define TX_TYPEB_106 4
|
||||
#define TX_TYPEB_212 5
|
||||
#define TX_TYPEB_424 6
|
||||
#define TX_TYPEB_848 7
|
||||
|
||||
#define TX_TYPEV_26 10
|
||||
#define TX_TYPEV_53 11
|
||||
|
||||
#define TX_FELICA_212 19
|
||||
#define TX_FELICA_424 20
|
||||
|
||||
#endif
|
||||
|
||||
82
apps/earphone/xtell_remote_control/RFID/include/rfid_main.h
Normal file
82
apps/earphone/xtell_remote_control/RFID/include/rfid_main.h
Normal file
@ -0,0 +1,82 @@
|
||||
/********************************************************************************************************
|
||||
* @file rfid_main.h
|
||||
* @brief RFID 读卡器应用层主头文件
|
||||
* @details
|
||||
* 本文件定义了RFID应用层所需的数据结构、枚举类型和全局函数。
|
||||
*
|
||||
* @author Kilo Code
|
||||
* @date 2025-11-24
|
||||
* @version 1.0
|
||||
********************************************************************************************************/
|
||||
|
||||
#ifndef _RFID_MAIN_H
|
||||
#define _RFID_MAIN_H
|
||||
|
||||
// 包含项目的基础类型定义，如果您的项目中没有 "system/includes.h"，
|
||||
// 请替换为包含 stdint.h 或类似的头文件以获取 u8, u16, u32 等类型定义。
|
||||
#include "system/includes.h"
|
||||
|
||||
/**
|
||||
* @brief 操作状态枚举
|
||||
*/
|
||||
typedef enum {
|
||||
FAIL = 0U,
|
||||
SUCCESS = !FAIL
|
||||
} ErrorStatus;
|
||||
|
||||
/**
|
||||
* @brief 功能使能状态枚举
|
||||
*/
|
||||
typedef enum {
|
||||
FUN_DISABLE = 0U,
|
||||
FUN_ENABLE = !FUN_DISABLE
|
||||
} FunState;
|
||||
|
||||
/**
|
||||
* @brief 标志位状态枚举
|
||||
*/
|
||||
typedef enum {
|
||||
RESET = 0U,
|
||||
SET = !RESET
|
||||
} FlagStatus, ITStatus;
|
||||
|
||||
/**
|
||||
* @brief 通用位宏定义
|
||||
*/
|
||||
#define BIT0 (1 << 0)
|
||||
#define BIT1 (1 << 1)
|
||||
#define BIT2 (1 << 2)
|
||||
#define BIT3 (1 << 3)
|
||||
#define BIT4 (1 << 4)
|
||||
#define BIT5 (1 << 5)
|
||||
#define BIT6 (1 << 6)
|
||||
#define BIT7 (1 << 7)
|
||||
|
||||
|
||||
/**
|
||||
* @brief 数据传输结构体
|
||||
* @details 用于在不同函数间传递发送和接收数据缓冲区及其长度信息。
|
||||
*/
|
||||
typedef struct
|
||||
{
|
||||
unsigned char SendLength; /**< 要发送的数据长度 */
|
||||
unsigned char *pSendBuffer; /**< 指向发送数据缓冲区的指针 */
|
||||
unsigned char ReceiveLength; /**< 接收到的数据长度 */
|
||||
unsigned char *pReceiveBuffer; /**< 指向接收数据缓冲区的指针 */
|
||||
unsigned int Timeout; /**< 操作超时时间（单位：毫秒） */
|
||||
} transmission_struct;
|
||||
|
||||
|
||||
/********************************************************************************************************
|
||||
* 全局函数声明
|
||||
********************************************************************************************************/
|
||||
|
||||
/**
|
||||
* @brief RFID模块的主任务函数。
|
||||
* @details
|
||||
* 这是一个示例性的任务函数，展示了如何初始化RFID芯片并进入一个无限循环来轮询不同类型的卡片。
|
||||
* 您可以将此函数作为一个独立的任务运行，或者将其中的逻辑集成到您现有的任务调度中。
|
||||
*/
|
||||
void rfid_task(void);
|
||||
|
||||
#endif // _RFID_MAIN_H
|
||||
447
apps/earphone/xtell_remote_control/RFID/reader/CPU_CARD.c
Normal file
447
apps/earphone/xtell_remote_control/RFID/reader/CPU_CARD.c
Normal file
@ -0,0 +1,447 @@
|
||||
#include "../include/READER.h"
|
||||
#include "../include/CPU_CARD.h"
|
||||
#include "../include/READER_REG.h"
|
||||
#include "../include/rfid_main.h"
|
||||
#include "../rfid_hal.h"
|
||||
|
||||
#define FUN_ENABLE_XLOG 1
|
||||
#ifdef xlog
|
||||
#undef xlog
|
||||
#endif
|
||||
#if FUN_ENABLE_XLOG
|
||||
#define xlog(format, ...) printf("[XT:%s] " format, __func__, ##__VA_ARGS__)
|
||||
#else
|
||||
#define xlog(format, ...) ((void)0)
|
||||
#endif
|
||||
|
||||
|
||||
struct CPU_CARD_STR CPU_CARD;
|
||||
|
||||
// 声明一个静态函数用于TPDU传输，因为它只在本文件内部使用
|
||||
static unsigned char FM176XX_TPDU(transmission_struct *tpdu);
|
||||
|
||||
/**
|
||||
* @brief CPU卡事件处理函数（示例）。
|
||||
* @details
|
||||
* 1. 发送RATS (Request for Answer to Select) 命令以激活卡片并获取ATS (Answer to Select)。
|
||||
* 2. 解析ATS，获取卡片能力信息（如FSC, FWI等）。
|
||||
* 3. 发送一系列APDU (Application Protocol Data Unit) 指令与卡片应用进行交互。
|
||||
* @return 操作状态，SUCCESS表示成功，FAIL表示失败。
|
||||
*/
|
||||
unsigned char CPU_CARD_EVENT(void)
|
||||
{
|
||||
unsigned char result;
|
||||
unsigned char SendBuffer[255];
|
||||
unsigned char ReceiveBuffer[255];
|
||||
int i;
|
||||
transmission_struct APDU;
|
||||
APDU.pSendBuffer = SendBuffer;
|
||||
APDU.pReceiveBuffer = ReceiveBuffer;
|
||||
|
||||
result = CPU_Rats(&CPU_CARD.ATS.Length, CPU_CARD.ATS.Ats_Data);
|
||||
if (result != SUCCESS)
|
||||
{
|
||||
SetCW(FUN_DISABLE);
|
||||
xlog("-> RATS ERROR!\r\n");
|
||||
return result;
|
||||
}
|
||||
|
||||
xlog("-> ATS = ");
|
||||
for(i = 0; i < CPU_CARD.ATS.Length; i++)
|
||||
xlog("%02X", CPU_CARD.ATS.Ats_Data[i]);
|
||||
xlog("\r\n");
|
||||
|
||||
result = Ats_Process(CPU_CARD.ATS.Length, CPU_CARD.ATS.Ats_Data);
|
||||
if (result != SUCCESS)
|
||||
{
|
||||
SetCW(FUN_DISABLE);
|
||||
xlog("-> ATS Process ERROR!\r\n");
|
||||
return result;
|
||||
}
|
||||
|
||||
// 选择主文件(MF)
|
||||
memcpy(APDU.pSendBuffer, "\x00\xA4\x00\x00\x02\x3F\x00", 7);
|
||||
APDU.SendLength = 7;
|
||||
result = CPU_APDU(&APDU);
|
||||
if (result != SUCCESS)
|
||||
{
|
||||
SetCW(FUN_DISABLE);
|
||||
xlog("-> APDU ERROR!\r\n");
|
||||
return result;
|
||||
}
|
||||
xlog("-> Select MF Response = ");
|
||||
for(i=0; i<APDU.ReceiveLength; i++)
|
||||
xlog("%02X", APDU.pReceiveBuffer[i]);
|
||||
xlog("\r\n");
|
||||
|
||||
// ... 此处可以添加更多APDU指令 ...
|
||||
|
||||
return result;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 将数据写入芯片的FIFO缓冲区。
|
||||
* @param length [in] 要写入的数据长度。
|
||||
* @param buff [in] 指向源数据缓冲区的指针。
|
||||
* @return 无。
|
||||
*/
|
||||
static void Write_FIFO(unsigned char length, unsigned char* buff)
|
||||
{
|
||||
unsigned char i;
|
||||
for(i=0; i<length; i++)
|
||||
{
|
||||
SetReg(REG_FIFODATA,buff[i]);
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 从芯片的FIFO缓冲区读取数据。
|
||||
* @param length [in] 要读取的数据长度。
|
||||
* @param buff [out] 指向目标数据缓冲区的指针。
|
||||
* @return 无。
|
||||
*/
|
||||
static void Read_FIFO(unsigned char length, unsigned char* buff)
|
||||
{
|
||||
unsigned char i;
|
||||
for(i=0; i<length; i++)
|
||||
{
|
||||
GetReg(REG_FIFODATA,&buff[i]);
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 执行TPDU (Transmission Protocol Data Unit) 数据交换。
|
||||
* @param tpdu [in, out] 指向 `transmission_struct` 结构体的指针，包含发送和接收信息。
|
||||
* @return 操作状态，SUCCESS表示成功，FAIL表示超时或出错。
|
||||
* @details
|
||||
* 这是与卡片进行底层数据块交换的核心函数。它负责：
|
||||
* - 将数据写入FIFO。
|
||||
* - 启动TRANSCEIVE命令。
|
||||
* - 等待接收中断或超时。
|
||||
* - 从FIFO读取响应数据。
|
||||
* - 检查错误状态。
|
||||
*/
|
||||
static unsigned char FM176XX_TPDU(transmission_struct *tpdu)
|
||||
{
|
||||
unsigned char irq0, error;
|
||||
unsigned int i;
|
||||
SetCommand(CMD_IDLE);
|
||||
SetReg(REG_TXDATANUM,0x08);
|
||||
SetReg(REG_IRQ0,0x7F); // 清除IRQ0所有中断标志
|
||||
SetReg(REG_IRQ1,0x7F); // 清除IRQ1所有中断标志
|
||||
ModifyReg(REG_FIFOCONTROL,BIT_FIFOFLUSH,FUN_ENABLE); // 清空FIFO
|
||||
Write_FIFO(tpdu->SendLength,tpdu->pSendBuffer);
|
||||
ModifyReg(REG_TXCRCCON, BIT_CRCEN,FUN_ENABLE);
|
||||
ModifyReg(REG_RXCRCCON, BIT_CRCEN,FUN_ENABLE);
|
||||
// SetTimer(tpdu->Timeout); // 定时器功能可以根据需要启用
|
||||
SetCommand(CMD_TRANSCEIVE);
|
||||
|
||||
// 等待接收完成或超时
|
||||
for(i = 0; i < tpdu->Timeout; i++)
|
||||
{
|
||||
rfid_delay_ms(1);
|
||||
GetReg(REG_IRQ0,&irq0);
|
||||
|
||||
if(irq0 & BIT_RXIRQ) // 检查是否收到数据
|
||||
{
|
||||
GetReg(REG_ERROR, &error); // 获取错误状态
|
||||
error &= (BIT_NODATAERR | BIT_COLLDET | BIT_PROTERR | BIT_INTEGERR);
|
||||
if(error != 0)
|
||||
return FAIL; // 接收到错误
|
||||
GetReg(REG_FIFOLENGTH, &tpdu->ReceiveLength);
|
||||
if(tpdu->ReceiveLength > 0)
|
||||
{
|
||||
Read_FIFO(tpdu->ReceiveLength,tpdu->pReceiveBuffer);
|
||||
return SUCCESS;
|
||||
}
|
||||
}
|
||||
}
|
||||
return FAIL; // 超时
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 发送RATS (Request for Answer to Select) 命令。
|
||||
* @param ats_len [out] 指向用于存储ATS长度的变量的指针。
|
||||
* @param ats [out] 指向用于存储ATS数据的缓冲区的指针。
|
||||
* @return 操作状态，SUCCESS表示成功。
|
||||
* @details
|
||||
* RATS是激活ISO/IEC 14443-4卡片的第一步，用于获取卡片的基本通信参数。
|
||||
*/
|
||||
unsigned char CPU_Rats(unsigned char *ats_len, unsigned char *ats)
|
||||
{
|
||||
unsigned char result;
|
||||
unsigned char outbuffer[2], inbuffer[64];
|
||||
transmission_struct tpdu;
|
||||
tpdu.pSendBuffer = outbuffer;
|
||||
tpdu.pReceiveBuffer = inbuffer;
|
||||
tpdu.pSendBuffer[0] = 0xE0; // RATS命令起始字节
|
||||
tpdu.pSendBuffer[1] = 0x50; // 参数字节 (FSDI=5, CID=0)
|
||||
tpdu.SendLength = 2;
|
||||
tpdu.Timeout = 160; // 超时时间
|
||||
|
||||
result = FM176XX_TPDU(&tpdu);
|
||||
if (result == SUCCESS)
|
||||
{
|
||||
*ats_len = tpdu.ReceiveLength;
|
||||
memcpy(ats, tpdu.pReceiveBuffer, *ats_len);
|
||||
}
|
||||
return (result);
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 解析ATS (Answer to Select) 数据。
|
||||
* @param ats_len [in] ATS数据的长度。
|
||||
* @param ats [in] 指向ATS数据的指针。
|
||||
* @return 操作状态，SUCCESS表示成功。
|
||||
* @details
|
||||
* 此函数从ATS响应中提取关键参数，如FSC (Frame Size for proximity coupling)、
|
||||
* FWI (Frame Waiting time Integer)等，并存储在全局的 `CPU_CARD` 结构体中。
|
||||
*/
|
||||
unsigned char Ats_Process(unsigned char ats_len, unsigned char *ats)
|
||||
{
|
||||
unsigned char offset;
|
||||
|
||||
if (ats_len < 2) return FAIL;
|
||||
|
||||
// 解析FSCI (Frame Size for proximity coupling Integer) -> FSC
|
||||
CPU_CARD.FSCI = ats[1] & 0x0F;
|
||||
switch(CPU_CARD.FSCI) {
|
||||
case 0: CPU_CARD.FSC = 16; break;
|
||||
case 1: CPU_CARD.FSC = 24; break;
|
||||
case 2: CPU_CARD.FSC = 32; break;
|
||||
case 3: CPU_CARD.FSC = 40; break;
|
||||
case 4: CPU_CARD.FSC = 48; break;
|
||||
case 5: CPU_CARD.FSC = 64; break;
|
||||
case 6: CPU_CARD.FSC = 96; break;
|
||||
case 7: CPU_CARD.FSC = 128; break;
|
||||
case 8: CPU_CARD.FSC = 256; break;
|
||||
default: CPU_CARD.FSC = 32; break; // 默认值
|
||||
}
|
||||
xlog("-> CPU_CARD.FSC = %d\r\n", CPU_CARD.FSC);
|
||||
|
||||
offset = 0;
|
||||
if (ats[1] & BIT4) // TA(1) present
|
||||
{
|
||||
CPU_CARD.TA = ats[2];
|
||||
offset++;
|
||||
}
|
||||
|
||||
if (ats[1] & BIT5) // TB(1) present
|
||||
{
|
||||
CPU_CARD.TB = ats[2 + offset];
|
||||
CPU_CARD.SFGI = CPU_CARD.TB & 0x0F;
|
||||
CPU_CARD.FWI = (CPU_CARD.TB >> 4) & 0x0F;
|
||||
xlog("-> CPU_CARD.SFGI = %02X\r\n", CPU_CARD.SFGI);
|
||||
xlog("-> CPU_CARD.FWI = %02X\r\n", CPU_CARD.FWI);
|
||||
|
||||
// 根据FWI计算FWT (Frame Waiting Time)
|
||||
unsigned long base_fwt = 256 * 16 / 13560; // (256 * 16 / fc) in ms
|
||||
CPU_CARD.FWT = base_fwt * (1 << CPU_CARD.FWI);
|
||||
offset++;
|
||||
} else {
|
||||
CPU_CARD.FWT = 160; // 默认FWT
|
||||
}
|
||||
|
||||
if (ats[1] & BIT6) // TC(1) present
|
||||
{
|
||||
CPU_CARD.TC = ats[2 + offset];
|
||||
offset++;
|
||||
}
|
||||
|
||||
CPU_CARD.PCB = 0x02; // PCB初始值为0x02
|
||||
return SUCCESS;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 发送NAK (Negative Acknowledge) 响应。
|
||||
* @param tpdu [in, out] 指向传输结构体的指针。
|
||||
* @return 操作状态。
|
||||
* @details
|
||||
* 在TPDU交换中，如果接收到错误的数据块，会发送NAK请求重发。
|
||||
*/
|
||||
unsigned char CPU_NAK(transmission_struct *tpdu)
|
||||
{
|
||||
unsigned char result, tpdu_send_buffer[1], tpdu_receive_buffer[255];
|
||||
|
||||
tpdu->pSendBuffer = tpdu_send_buffer;
|
||||
tpdu->pReceiveBuffer = tpdu_receive_buffer;
|
||||
|
||||
tpdu->pSendBuffer[0] = 0xB0 | CPU_CARD.PCB; // NAK PCB
|
||||
tpdu->SendLength = 1;
|
||||
|
||||
result = FM176XX_TPDU(tpdu);
|
||||
return (result);
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 封装了重试逻辑的TPDU传输函数。
|
||||
* @param tpdu [in, out] 指向传输结构体的指针。
|
||||
* @return 操作状态。
|
||||
* @details
|
||||
* 此函数调用底层的 `FM176XX_TPDU`，并在失败时进行最多3次重试。
|
||||
* 它还处理ACK/NAK逻辑，以确保数据的可靠传输。
|
||||
*/
|
||||
unsigned char CPU_TPDU(transmission_struct *tpdu)
|
||||
{
|
||||
unsigned char result, i, pcb_byte;
|
||||
transmission_struct nak_tpdu;
|
||||
|
||||
result = FM176XX_TPDU(tpdu);
|
||||
|
||||
for (i = 0; i < 3; i++)
|
||||
{
|
||||
if (result != SUCCESS)
|
||||
{
|
||||
result = CPU_NAK(&nak_tpdu);
|
||||
if(result == SUCCESS && nak_tpdu.ReceiveLength > 0)
|
||||
{
|
||||
memcpy(&pcb_byte, nak_tpdu.pReceiveBuffer, 1);
|
||||
if((pcb_byte & 0xF0) == 0xA0) // R(ACK)
|
||||
{
|
||||
xlog("...pcb_byte = %02X\r\n", pcb_byte);
|
||||
xlog("...CPU_CARD.PCB = %02X\r\n", CPU_CARD.PCB);
|
||||
if((pcb_byte & 0x01) != (CPU_CARD.PCB & 0x01))
|
||||
{
|
||||
result = FM176XX_TPDU(tpdu);
|
||||
}
|
||||
else
|
||||
{
|
||||
tpdu->pSendBuffer[0] ^= 0x01; // 翻转序列号位
|
||||
CPU_CARD.PCB = tpdu->pSendBuffer[0] & 0x01;
|
||||
result = FM176XX_TPDU(tpdu);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
break; // 成功则退出循环
|
||||
}
|
||||
}
|
||||
return (result);
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 发送APDU (Application Protocol Data Unit) 命令。
|
||||
* @param apdu [in, out] 指向传输结构体的指针，包含APDU命令和响应。
|
||||
* @return 操作状态。
|
||||
* @details
|
||||
* 此函数处理APDU的块链接（chaining）逻辑。如果APDU长度超过卡片的最大帧大小（FSC），
|
||||
* 它会自动将APDU分割成多个TPDU块进行传输。
|
||||
*/
|
||||
unsigned char CPU_APDU(transmission_struct *apdu)
|
||||
{
|
||||
unsigned char result, pcb_byte, i;
|
||||
unsigned char tpdu_send_buffer[256], tpdu_receive_buffer[256];
|
||||
unsigned int unsent_length;
|
||||
transmission_struct tpdu;
|
||||
|
||||
tpdu.pSendBuffer = tpdu_send_buffer;
|
||||
tpdu.pReceiveBuffer = tpdu_receive_buffer;
|
||||
tpdu.Timeout = CPU_CARD.FWT;
|
||||
apdu->ReceiveLength = 0;
|
||||
unsent_length = apdu->SendLength;
|
||||
|
||||
// --- 发送阶段 ---
|
||||
for (i = 0; i < 16; i++) // 最多16个链式块
|
||||
{
|
||||
xlog("unsent_length = %d\r\n", unsent_length);
|
||||
if (unsent_length <= (CPU_CARD.FSC - 1))
|
||||
{
|
||||
// 最后一个或唯一的数据块
|
||||
tpdu.pSendBuffer[0] = CPU_CARD.PCB; // I-Block, no chaining
|
||||
memcpy(tpdu.pSendBuffer + 1, apdu->pSendBuffer + apdu->SendLength - unsent_length, unsent_length);
|
||||
tpdu.SendLength = unsent_length + 1;
|
||||
|
||||
xlog("--> ");
|
||||
for(int j=0; j<tpdu.SendLength; j++) xlog("%02X", tpdu.pSendBuffer[j]);
|
||||
xlog("\r\n");
|
||||
|
||||
result = CPU_TPDU(&tpdu);
|
||||
if ((result != SUCCESS) || (tpdu.ReceiveLength == 0))
|
||||
return (result);
|
||||
|
||||
xlog("<-- ");
|
||||
for(int j=0; j<tpdu.ReceiveLength; j++) xlog("%02X", tpdu.pReceiveBuffer[j]);
|
||||
xlog("\r\n");
|
||||
|
||||
unsent_length = 0;
|
||||
break; // 发送完成
|
||||
}
|
||||
else
|
||||
{
|
||||
// 需要分块传输
|
||||
tpdu.pSendBuffer[0] = CPU_CARD.PCB | 0x10; // I-Block with chaining
|
||||
memcpy(tpdu.pSendBuffer + 1, apdu->pSendBuffer + apdu->SendLength - unsent_length, CPU_CARD.FSC - 1);
|
||||
tpdu.SendLength = CPU_CARD.FSC;
|
||||
|
||||
xlog("..--> ");
|
||||
for(int j=0; j<tpdu.SendLength; j++) xlog("%02X", tpdu.pSendBuffer[j]);
|
||||
xlog("\r\n");
|
||||
|
||||
result = CPU_TPDU(&tpdu);
|
||||
|
||||
xlog("<-- ");
|
||||
for(int j=0; j<tpdu.ReceiveLength; j++) xlog("%02X", tpdu.pReceiveBuffer[j]);
|
||||
xlog("\r\n");
|
||||
|
||||
if ((result != SUCCESS) || (tpdu.ReceiveLength != 1))
|
||||
return (result);
|
||||
|
||||
memcpy(&pcb_byte, tpdu.pReceiveBuffer, 1);
|
||||
if ((pcb_byte & 0xE0) == 0xA0) // R(ACK) block
|
||||
{
|
||||
unsent_length -= (CPU_CARD.FSC - 1);
|
||||
CPU_CARD.PCB = (pcb_byte & 0x01) ^ 0x01; // 更新序列号
|
||||
xlog("unsent_length = %d\r\n", unsent_length);
|
||||
}
|
||||
else
|
||||
{
|
||||
return (FAIL); // 未收到预期的ACK
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// --- 接收阶段 ---
|
||||
for (i = 0; i < 255; i++) // 最多255个链式块
|
||||
{
|
||||
if ((result != SUCCESS) || (tpdu.ReceiveLength == 0))
|
||||
return (FAIL);
|
||||
|
||||
memcpy(&pcb_byte, tpdu.pReceiveBuffer, 1);
|
||||
|
||||
if ((pcb_byte & 0xC0) == 0x00) // I-Block
|
||||
{
|
||||
CPU_CARD.PCB = (pcb_byte & 0x01) ^ 0x01;
|
||||
memcpy(apdu->pReceiveBuffer + apdu->ReceiveLength, tpdu.pReceiveBuffer + 1, tpdu.ReceiveLength - 1);
|
||||
apdu->ReceiveLength += (tpdu.ReceiveLength - 1);
|
||||
|
||||
if (pcb_byte & 0x10) // 还有后续数据块
|
||||
{
|
||||
tpdu.pSendBuffer[0] = 0xA0 | CPU_CARD.PCB; // 发送ACK
|
||||
tpdu.SendLength = 1;
|
||||
xlog("...--> ACK = %02X\r\n", tpdu.pSendBuffer[0]);
|
||||
result = CPU_TPDU(&tpdu);
|
||||
}
|
||||
else // 最后一个数据块
|
||||
{
|
||||
return SUCCESS;
|
||||
}
|
||||
}
|
||||
else if ((pcb_byte & 0xE0) == 0xE0) // S-Block (WTX)
|
||||
{
|
||||
// 回复WTX响应
|
||||
memcpy(tpdu.pSendBuffer, tpdu.pReceiveBuffer, tpdu.ReceiveLength);
|
||||
tpdu.SendLength = tpdu.ReceiveLength;
|
||||
xlog("....--> WTX = ");
|
||||
for(int j=0; j<tpdu.SendLength; j++) xlog("%02X", tpdu.pSendBuffer[j]);
|
||||
xlog("\r\n");
|
||||
result = CPU_TPDU(&tpdu);
|
||||
}
|
||||
else
|
||||
{
|
||||
return FAIL; // 未知响应
|
||||
}
|
||||
}
|
||||
return (FAIL); // 接收块过多
|
||||
}
|
||||
455
apps/earphone/xtell_remote_control/RFID/reader/MIFARE.c
Normal file
455
apps/earphone/xtell_remote_control/RFID/reader/MIFARE.c
Normal file
@ -0,0 +1,455 @@
|
||||
#include "../include/MIFARE.h"
|
||||
#include "../include/READER.h"
|
||||
#include "../include/READER_REG.h"
|
||||
#include "../include/rfid_main.h"
|
||||
#include "../rfid_hal.h"
|
||||
|
||||
#define FUN_ENABLE_XLOG 1
|
||||
#ifdef xlog
|
||||
#undef xlog
|
||||
#endif
|
||||
#if FUN_ENABLE_XLOG
|
||||
#define xlog(format, ...) printf("[XT:%s] " format, __func__, ##__VA_ARGS__)
|
||||
#else
|
||||
#define xlog(format, ...) ((void)0)
|
||||
#endif
|
||||
|
||||
|
||||
unsigned char SECTOR,BLOCK,BLOCK_NUM;
|
||||
unsigned char BLOCK_DATA[16];
|
||||
unsigned char KEY_A[16][6]=
|
||||
{{0xFF,0xFF,0xFF,0xFF,0xFF,0xFF},//0
|
||||
{0xFF,0xFF,0xFF,0xFF,0xFF,0xFF},//1
|
||||
{0xFF,0xFF,0xFF,0xFF,0xFF,0xFF},//2
|
||||
{0xFF,0xFF,0xFF,0xFF,0xFF,0xFF},//3
|
||||
{0xFF,0xFF,0xFF,0xFF,0xFF,0xFF},//4
|
||||
{0xFF,0xFF,0xFF,0xFF,0xFF,0xFF},//5
|
||||
{0xFF,0xFF,0xFF,0xFF,0xFF,0xFF},//6
|
||||
{0xFF,0xFF,0xFF,0xFF,0xFF,0xFF},//7
|
||||
{0xFF,0xFF,0xFF,0xFF,0xFF,0xFF},//8
|
||||
{0xFF,0xFF,0xFF,0xFF,0xFF,0xFF},//9
|
||||
{0xFF,0xFF,0xFF,0xFF,0xFF,0xFF},//10
|
||||
{0xFF,0xFF,0xFF,0xFF,0xFF,0xFF},//11
|
||||
{0xFF,0xFF,0xFF,0xFF,0xFF,0xFF},//12
|
||||
{0xFF,0xFF,0xFF,0xFF,0xFF,0xFF},//13
|
||||
{0xFF,0xFF,0xFF,0xFF,0xFF,0xFF},//14
|
||||
{0xFF,0xFF,0xFF,0xFF,0xFF,0xFF}};//15
|
||||
|
||||
unsigned char KEY_B[16][6]=
|
||||
{{0xFF,0xFF,0xFF,0xFF,0xFF,0xFF},//0
|
||||
{0xFF,0xFF,0xFF,0xFF,0xFF,0xFF},//1
|
||||
{0xFF,0xFF,0xFF,0xFF,0xFF,0xFF},//2
|
||||
{0xFF,0xFF,0xFF,0xFF,0xFF,0xFF},//3
|
||||
{0xFF,0xFF,0xFF,0xFF,0xFF,0xFF},//4
|
||||
{0xFF,0xFF,0xFF,0xFF,0xFF,0xFF},//5
|
||||
{0xFF,0xFF,0xFF,0xFF,0xFF,0xFF},//6
|
||||
{0xFF,0xFF,0xFF,0xFF,0xFF,0xFF},//7
|
||||
{0xFF,0xFF,0xFF,0xFF,0xFF,0xFF},//8
|
||||
{0xFF,0xFF,0xFF,0xFF,0xFF,0xFF},//9
|
||||
{0xFF,0xFF,0xFF,0xFF,0xFF,0xFF},//10
|
||||
{0xFF,0xFF,0xFF,0xFF,0xFF,0xFF},//11
|
||||
{0xFF,0xFF,0xFF,0xFF,0xFF,0xFF},//12
|
||||
{0xFF,0xFF,0xFF,0xFF,0xFF,0xFF},//13
|
||||
{0xFF,0xFF,0xFF,0xFF,0xFF,0xFF},//14
|
||||
{0xFF,0xFF,0xFF,0xFF,0xFF,0xFF}};//15
|
||||
|
||||
/**
|
||||
* @brief 清除Mifare卡加密认证标志。
|
||||
* @details
|
||||
* 在对Mifare卡进行认证（Authentication）后，芯片内部会设置一个加密标志位（BIT_CRYPTO1ON）。
|
||||
* 此函数用于清除该标志，以便可以对新的扇区进行认证或执行非加密操作。
|
||||
* @return 无。
|
||||
*/
|
||||
void Mifare_Clear_Crypto(void)
|
||||
{
|
||||
ModifyReg(REG_STATUS,BIT_CRYPTO1ON,RESET);
|
||||
return;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Mifare卡事件处理函数（示例）。
|
||||
* @details
|
||||
* 这是一个示例函数，演示了对Mifare Classic卡进行读写操作的完整流程：
|
||||
* 1. 清除加密状态。
|
||||
* 2. 对指定的扇区（例如扇区1）使用密钥A进行认证。
|
||||
* 3. 如果认证成功，则遍历该扇区的数据块（块0到块2）。
|
||||
* 4. 对每个块先执行写操作，写入16字节的0xFF。
|
||||
* 5. 然后再执行读操作，将数据读回并打印。
|
||||
* @return 操作状态，SUCCESS表示成功，FAIL表示失败。
|
||||
*/
|
||||
unsigned char MIFARE_CARD_EVENT(void)
|
||||
{
|
||||
unsigned char result;
|
||||
int i;
|
||||
Mifare_Clear_Crypto();
|
||||
SECTOR = 1;
|
||||
//for(SECTOR = 0;SECTOR < 16; SECTOR++)
|
||||
{
|
||||
BLOCK_NUM = (SECTOR * 4) + BLOCK;
|
||||
result = Mifare_Auth(KEY_A_M1,SECTOR,KEY_A[SECTOR],PICC_A.UID);
|
||||
if(result != SUCCESS)
|
||||
{
|
||||
SetCW(FUN_DISABLE);
|
||||
xlog("-> AUTH ERROR!\r\n");
|
||||
return result;
|
||||
}
|
||||
|
||||
xlog("-> AUTH SUCCESS!\r\n");
|
||||
|
||||
for(BLOCK = 0;BLOCK < 3;BLOCK++)
|
||||
{
|
||||
BLOCK_NUM = (SECTOR * 4) + BLOCK;
|
||||
if(BLOCK_NUM == 0)
|
||||
BLOCK_NUM = 1;
|
||||
xlog("-> SECTOR = %02X\r\n",SECTOR);;
|
||||
xlog("-> BLOCK = %02X\r\n",BLOCK);
|
||||
xlog("-> BLOCK_NUM = %02X\r\n",BLOCK_NUM);
|
||||
memcpy(BLOCK_DATA,"\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF",16);
|
||||
result = Mifare_Blockwrite(BLOCK_NUM,BLOCK_DATA);
|
||||
if(result != SUCCESS)
|
||||
{
|
||||
SetCW(FUN_DISABLE);
|
||||
xlog("-> WRITE BLOCK ERROR!\r\n");
|
||||
return result;
|
||||
}
|
||||
xlog("-> WRITE BLOCK SUCCESS!\r\n");
|
||||
|
||||
result = Mifare_Blockread(BLOCK_NUM,BLOCK_DATA);
|
||||
if(result != SUCCESS)
|
||||
{
|
||||
SetCW(FUN_DISABLE);
|
||||
xlog("-> READ BLOCK ERROR!\r\n");
|
||||
return result;
|
||||
}
|
||||
|
||||
xlog("-> READ BLOCK = ");
|
||||
for(i=0; i<16; i++) xlog("%02X", BLOCK_DATA[i]);
|
||||
xlog("\r\n");
|
||||
}
|
||||
}
|
||||
SetCW(FUN_DISABLE);
|
||||
return result;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 将6字节的Mifare密钥加载到芯片的密钥缓冲区。
|
||||
* @param mifare_key [in] 指向6字节密钥数组的指针。
|
||||
* @return 操作状态，SUCCESS表示成功，FAIL表示失败。
|
||||
* @details
|
||||
* 在执行认证命令（Mifare_Auth）之前，必须先调用此函数将要使用的密钥加载到芯片内部。
|
||||
*/
|
||||
unsigned char Mifare_LoadKey(unsigned char *mifare_key)
|
||||
{
|
||||
unsigned char reg_data;
|
||||
SetCommand(CMD_IDLE);
|
||||
ModifyReg(REG_FIFOCONTROL,BIT_FIFOFLUSH,FUN_ENABLE); //Clear FIFO
|
||||
SetReg(REG_FIFODATA,mifare_key[0]);
|
||||
SetReg(REG_FIFODATA,mifare_key[1]);
|
||||
SetReg(REG_FIFODATA,mifare_key[2]);
|
||||
SetReg(REG_FIFODATA,mifare_key[3]);
|
||||
SetReg(REG_FIFODATA,mifare_key[4]);
|
||||
SetReg(REG_FIFODATA,mifare_key[5]);
|
||||
SetCommand(CMD_LOADKEY);
|
||||
rfid_delay_ms(1);
|
||||
GetReg(REG_COMMAND,®_data);
|
||||
if((reg_data & CMD_MASK) == CMD_IDLE)
|
||||
return SUCCESS;
|
||||
else
|
||||
return FAIL;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 对Mifare Classic卡片的指定扇区进行认证。
|
||||
* @param key_mode [in] 认证模式，`KEY_A_M1` (0) 表示使用密钥A，`KEY_B_M1` (1) 表示使用密钥B。
|
||||
* @param sector [in] 要认证的扇区号 (0-15)。
|
||||
* @param mifare_key [in] 指向6字节认证密钥的指针。
|
||||
* @param card_uid [in] 指向4字节卡片UID的指针。
|
||||
* @return 操作状态，SUCCESS表示认证成功，FAIL表示失败。
|
||||
* @details
|
||||
* 这是访问Mifare卡数据块之前的必要步骤。认证成功后，芯片会设置加密标志位。
|
||||
*/
|
||||
unsigned char Mifare_Auth(unsigned char key_mode,unsigned char sector,unsigned char *mifare_key,unsigned char *card_uid)
|
||||
{
|
||||
unsigned char result,reg_data;
|
||||
result = Mifare_LoadKey(mifare_key);
|
||||
if (result != SUCCESS)
|
||||
return result;
|
||||
SetCommand(CMD_IDLE);
|
||||
ModifyReg(REG_FIFOCONTROL,BIT_FIFOFLUSH,FUN_ENABLE); //Clear FIFO
|
||||
if(key_mode == KEY_A_M1)
|
||||
{
|
||||
SetReg(REG_FIFODATA,0x60);// 0x60: Key A认证指令
|
||||
ModifyReg(REG_RXTXCON,BIT_SHMODE,FUN_DISABLE);
|
||||
}
|
||||
if(key_mode == KEY_B_M1)
|
||||
{
|
||||
SetReg(REG_FIFODATA,0x61);// 0x61: Key B认证指令
|
||||
ModifyReg(REG_RXTXCON,BIT_SHMODE,FUN_DISABLE);
|
||||
}
|
||||
|
||||
SetReg(REG_FIFODATA,sector * 4);// 认证扇区的块0地址
|
||||
SetReg(REG_FIFODATA,card_uid[0]);
|
||||
SetReg(REG_FIFODATA,card_uid[1]);
|
||||
SetReg(REG_FIFODATA,card_uid[2]);
|
||||
SetReg(REG_FIFODATA,card_uid[3]);
|
||||
ModifyReg(REG_TXCRCCON, BIT_CRCEN,FUN_ENABLE);
|
||||
ModifyReg(REG_RXCRCCON, BIT_CRCEN,FUN_ENABLE);
|
||||
SetCommand(CMD_AUTHENT);
|
||||
rfid_delay_ms(5);
|
||||
GetReg(REG_COMMAND,®_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,FUN_ENABLE);
|
||||
ModifyReg(REG_RXCRCCON, BIT_CRCEN,FUN_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,FUN_ENABLE);
|
||||
ModifyReg(REG_RXCRCCON, BIT_CRCEN,FUN_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,FUN_ENABLE);
|
||||
ModifyReg(REG_RXCRCCON, BIT_CRCEN,FUN_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,FUN_ENABLE);
|
||||
ModifyReg(REG_RXCRCCON, BIT_CRCEN,FUN_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,FUN_ENABLE);
|
||||
ModifyReg(REG_RXCRCCON, BIT_CRCEN,FUN_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,FUN_ENABLE);
|
||||
ModifyReg(REG_RXCRCCON, BIT_CRCEN,FUN_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;
|
||||
}
|
||||
108
apps/earphone/xtell_remote_control/RFID/reader/NTAG.c
Normal file
108
apps/earphone/xtell_remote_control/RFID/reader/NTAG.c
Normal file
@ -0,0 +1,108 @@
|
||||
#include "../include/READER.h"
|
||||
#include "../include/NTAG.h"
|
||||
#include "../include/READER_REG.h"
|
||||
#include "../include/rfid_main.h"
|
||||
#include "../rfid_hal.h"
|
||||
|
||||
#define FUN_ENABLE_XLOG 1
|
||||
#ifdef xlog
|
||||
#undef xlog
|
||||
#endif
|
||||
#if FUN_ENABLE_XLOG
|
||||
#define xlog(format, ...) printf("[XT:%s] " format, __func__, ##__VA_ARGS__)
|
||||
#else
|
||||
#define xlog(format, ...) ((void)0)
|
||||
#endif
|
||||
|
||||
|
||||
|
||||
unsigned char PAGE_DATA[16];
|
||||
|
||||
/**
|
||||
* @brief NTAG卡事件处理函数（示例）。
|
||||
* @details
|
||||
* 这是一个示例函数，演示了对NTAG系列卡片进行读写操作的流程：
|
||||
* 1. 准备要写入的数据（4字节）。
|
||||
* 2. 调用 `Write_Page()` 函数将数据写入第8页。
|
||||
* 3. 调用 `Read_Page()` 函数从第8页读回数据并打印，以进行验证。
|
||||
* @return 操作状态，SUCCESS表示成功，FAIL表示失败。
|
||||
*/
|
||||
unsigned char NTAG_EVENT(void)
|
||||
{
|
||||
unsigned char result;
|
||||
memcpy(PAGE_DATA,"\x01\x02\x03\x04",4);
|
||||
result = Write_Page(8,PAGE_DATA);
|
||||
if (result != SUCCESS)
|
||||
return result;
|
||||
xlog("PAGE 8 Write OK\r\n");
|
||||
result = Read_Page(8,PAGE_DATA);
|
||||
xlog("PAGE 8 = %02X%02X%02X%02X\r\n",PAGE_DATA[0],PAGE_DATA[1],PAGE_DATA[2],PAGE_DATA[3]);
|
||||
return result;
|
||||
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 从NTAG卡读取一个或多个页的数据。
|
||||
* @param page_num [in] 要读取的起始页号。
|
||||
* @param page_data [out] 指向缓冲区的指针，用于存储读取的数据。对于NTAG21x系列，一次最少读取16字节（4页）。
|
||||
* @return 操作状态，SUCCESS表示成功，FAIL表示失败。
|
||||
*/
|
||||
unsigned char Read_Page(unsigned char page_num,unsigned char *page_data)
|
||||
{
|
||||
unsigned char reg_data,i;
|
||||
SetCommand(CMD_IDLE);
|
||||
Clear_FIFO();
|
||||
SetReg(REG_FIFODATA,0x30); // 读指令
|
||||
SetReg(REG_FIFODATA,page_num);
|
||||
|
||||
ModifyReg(REG_TXCRCCON, BIT_CRCEN,FUN_ENABLE);
|
||||
ModifyReg(REG_RXCRCCON, BIT_CRCEN,FUN_ENABLE);
|
||||
SetCommand(CMD_TRANSCEIVE);
|
||||
rfid_delay_ms(5);
|
||||
GetReg(REG_ERROR,®_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,FUN_ENABLE);
|
||||
ModifyReg(REG_RXCRCCON, BIT_CRCEN,FUN_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;
|
||||
|
||||
}
|
||||
|
||||
784
apps/earphone/xtell_remote_control/RFID/reader/READER.c
Normal file
784
apps/earphone/xtell_remote_control/RFID/reader/READER.c
Normal file
@ -0,0 +1,784 @@
|
||||
/********************************************************************************************************
|
||||
* @file READER.c
|
||||
* @brief RFID 读卡器底层驱动及协议实现
|
||||
* @details
|
||||
* 本文件实现了与FM176XX系列RFID芯片的底层通信协议。它包含以下功能：
|
||||
* - 控制芯片进入不同工作模式（如Type A, B, V, F）。
|
||||
* - 实现各种卡片类型的寻卡、防冲突、选择和数据交换命令。
|
||||
* - 管理芯片的FIFO、定时器和RF场。
|
||||
* 所有硬件相关的操作均通过 `rfid_hal.h` 中定义的接口完成。
|
||||
********************************************************************************************************/
|
||||
|
||||
#include "../include/READER.h"
|
||||
#include "../include/READER_REG.h"
|
||||
#include "../include/rfid_main.h"
|
||||
#include "../rfid_hal.h" // 引入硬件抽象层
|
||||
|
||||
#define FUN_ENABLE_XLOG 1
|
||||
#ifdef xlog
|
||||
#undef xlog
|
||||
#endif
|
||||
#if FUN_ENABLE_XLOG
|
||||
#define xlog(format, ...) printf("[XT:%s] " format, __func__, ##__VA_ARGS__)
|
||||
#else
|
||||
#define xlog(format, ...) ((void)0)
|
||||
#endif
|
||||
|
||||
// 定义全局变量以存储不同类型卡片的信息
|
||||
struct picc_a_struct PICC_A;
|
||||
struct picc_b_struct PICC_B;
|
||||
struct picc_v_struct PICC_V;
|
||||
struct picc_f_struct PICC_F;
|
||||
|
||||
|
||||
/********************************************************************************************************
|
||||
* 公共接口函数
|
||||
********************************************************************************************************/
|
||||
|
||||
/**
|
||||
* @brief 修改寄存器的特定位。
|
||||
* @param reg_address [in] 目标寄存器的地址。
|
||||
* @param mask [in] 要修改的位的掩码。
|
||||
* @param set [in] 如果为非0，则将掩码对应的位设置为1；如果为0，则清零。
|
||||
* @return 无。
|
||||
* @details
|
||||
* 这是一个“读-改-写”操作。首先读取寄存器的当前值，然后根据掩码和`set`参数修改它，
|
||||
* 最后将修改后的值写回寄存器。
|
||||
*/
|
||||
void ModifyReg(unsigned char reg_address, unsigned char mask, unsigned char set)
|
||||
{
|
||||
unsigned char reg_data;
|
||||
|
||||
GetReg(reg_address, ®_data);
|
||||
|
||||
if (set)
|
||||
{
|
||||
reg_data |= mask;
|
||||
}
|
||||
else
|
||||
{
|
||||
reg_data &= ~mask;
|
||||
}
|
||||
|
||||
SetReg(reg_address, reg_data);
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 向命令寄存器写入一个命令。
|
||||
* @param command [in] 要执行的命令代码（如CMD_IDLE, CMD_TRANSCEIVE等）。
|
||||
* @return 操作状态，SUCCESS表示成功。
|
||||
*/
|
||||
unsigned char SetCommand(unsigned char command)
|
||||
{
|
||||
return SetReg(REG_COMMAND, CMD_MASK & command);
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 设置芯片内部定时器的超时时间。
|
||||
* @param timeout [in] 超时时间，单位为毫秒(ms)。
|
||||
* @return 无。
|
||||
* @details
|
||||
* 根据输入的超时时间，计算合适的预分频值和重载值，并配置T0和T1定时器。
|
||||
* 这用于在收发数据时进行超时检测。
|
||||
*/
|
||||
void SetTimer(unsigned int timeout)
|
||||
{
|
||||
unsigned long prescale = 1;
|
||||
unsigned long t, fc;
|
||||
fc = timeout * 13560; // 13.56MHz时钟频率
|
||||
t = fc;
|
||||
|
||||
while (fc > 65535)
|
||||
{
|
||||
prescale *= 2;
|
||||
fc = t / prescale;
|
||||
if (fc * prescale != t)
|
||||
fc++;
|
||||
}
|
||||
|
||||
if (prescale > 1)
|
||||
{
|
||||
SetReg(REG_T0CONTROL, BIT_TSTOP_RX | BIT_TSTART_TX | BIT_TAUTORESTARTED | VALUE_TCLK_1356_MHZ);
|
||||
SetReg(REG_T0RELOADHI, (unsigned char)(fc >> 8));
|
||||
SetReg(REG_T0RELOADLO, (unsigned char)fc);
|
||||
|
||||
SetReg(REG_T1CONTROL, BIT_TSTOP_RX | BIT_TSTART_TX | VALUE_TCLK_T0);
|
||||
SetReg(REG_T1RELOADHI, (unsigned char)(prescale >> 8));
|
||||
SetReg(REG_T1RELOADLO, (unsigned char)prescale);
|
||||
}
|
||||
else
|
||||
{
|
||||
SetReg(REG_T1CONTROL, BIT_TSTOP_RX | BIT_TSTART_TX | VALUE_TCLK_1356_MHZ);
|
||||
SetReg(REG_T1RELOADHI, (unsigned char)(fc >> 8));
|
||||
SetReg(REG_T1RELOADLO, (unsigned char)fc);
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 打开或关闭RF场（载波）。
|
||||
* @param mode [in] FUN_ENABLE表示打开，FUN_DISABLE表示关闭。
|
||||
* @return 操作状态，SUCCESS表示成功。
|
||||
*/
|
||||
unsigned char SetCW(unsigned char mode)
|
||||
{
|
||||
if (mode == FUN_ENABLE)
|
||||
{
|
||||
ModifyReg(REG_COMMAND, BIT_MODEMOFF, FUN_DISABLE);
|
||||
ModifyReg(REG_TXMODE, BIT_TPUSHON | BIT_TPULLON, FUN_ENABLE);
|
||||
}
|
||||
else
|
||||
{
|
||||
ModifyReg(REG_COMMAND, BIT_MODEMOFF, FUN_ENABLE);
|
||||
ModifyReg(REG_TXMODE, BIT_TPUSHON | BIT_TPULLON, FUN_DISABLE);
|
||||
}
|
||||
rfid_delay_ms(10);
|
||||
return SUCCESS;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 清空芯片内部的FIFO缓冲区。
|
||||
* @return 无。
|
||||
*/
|
||||
void Clear_FIFO(void)
|
||||
{
|
||||
unsigned char fifolength;
|
||||
GetReg(REG_FIFOLENGTH, &fifolength);
|
||||
if (fifolength != 0)
|
||||
{
|
||||
ModifyReg(REG_FIFOCONTROL, BIT_FIFOFLUSH, FUN_ENABLE);
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 加载指定的通信协议参数到芯片。
|
||||
* @param p_rx [in] 接收协议代码。
|
||||
* @param p_tx [in] 发送协议代码。
|
||||
* @return 操作状态，SUCCESS表示成功，FAIL表示失败。
|
||||
* @details
|
||||
* 不同的卡片类型（A, B, V, F）使用不同的通信速率和编码方式，
|
||||
* 此函数用于将这些协议参数加载到芯片中。
|
||||
*/
|
||||
unsigned char LoadProtocol(unsigned char p_rx, unsigned char p_tx)
|
||||
{
|
||||
unsigned char reg_data = 0;
|
||||
SetCommand(CMD_IDLE);
|
||||
ModifyReg(REG_FIFOCONTROL, BIT_FIFOFLUSH, FUN_ENABLE); // 清空FIFO
|
||||
SetReg(REG_FIFODATA, p_rx); // 写入接收协议
|
||||
SetReg(REG_FIFODATA, p_tx); // 写入发送协议
|
||||
|
||||
SetCommand(CMD_LOADPROTOCOL);
|
||||
rfid_delay_ms(2);
|
||||
GetReg(REG_COMMAND, ®_data);
|
||||
if (reg_data != CMD_IDLE)
|
||||
return FAIL;
|
||||
return SUCCESS;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 设置发送和接收的奇偶校验位使能状态。
|
||||
* @param state [in] FUN_ENABLE或FUN_DISABLE。
|
||||
* @return 无。
|
||||
*/
|
||||
void SetParity(unsigned char state)
|
||||
{
|
||||
ModifyReg(REG_FRAMECON, BIT_TXPARITYEN | BIT_RXPARITYEN, state);
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 初始化读卡器以支持Type A卡片。
|
||||
* @return 操作状态，SUCCESS表示成功。
|
||||
*/
|
||||
unsigned char ReaderA_Initial(void)
|
||||
{
|
||||
LoadProtocol(RX_TYPEA_106, TX_TYPEA_106);
|
||||
ModifyReg(REG_TXMODE, BIT_RFON, FUN_ENABLE); // FORCE 100ask FUN_ENABLE
|
||||
SetReg(REG_TXAMP, AMPLITUDE_A);
|
||||
SetReg(REG_TXCON, 0x00);
|
||||
SetReg(REG_RXANA, (HPCF_A << 3) | GAIN_A);
|
||||
SetReg(0x5F, 0x08);
|
||||
SetReg(REG_THNSET, 0xFF);
|
||||
SetReg(REG_THNMIN, 0xC0);
|
||||
SetReg(REG_RXTXCON, 0x80);
|
||||
SetParity(FUN_ENABLE);
|
||||
SetReg(REG_STATUS, 0); // 清除Crypto1On位
|
||||
return SUCCESS;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 初始化读卡器以支持Type B卡片。
|
||||
* @return 操作状态，SUCCESS表示成功。
|
||||
*/
|
||||
unsigned char ReaderB_Initial(void)
|
||||
{
|
||||
LoadProtocol(RX_TYPEB_106, TX_TYPEB_106);
|
||||
ModifyReg(REG_TXMODE, BIT_RFON, FUN_DISABLE); // FORCE 100ask FUN_DISABLE
|
||||
SetReg(REG_TXAMP, AMPLITUDE_B);
|
||||
SetReg(REG_TXCON, MODULATION_B);
|
||||
SetReg(REG_RXANA, (HPCF_B << 3) | GAIN_B);
|
||||
SetReg(0x5F, 0x08);
|
||||
SetReg(REG_THNSET, 0xFF);
|
||||
SetReg(REG_THNMIN, 0xC0);
|
||||
SetReg(REG_RXTXCON, 0x80);
|
||||
return SUCCESS;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 初始化读卡器以支持Type V (ISO15693) 卡片。
|
||||
* @return 操作状态，SUCCESS表示成功。
|
||||
*/
|
||||
unsigned char ReaderV_Initial(void)
|
||||
{
|
||||
LoadProtocol(RX_TYPEV_26, RX_TYPEV_26);
|
||||
ModifyReg(REG_RXANA, MASK_RCV_GAIN | MASK_RCV_HPCF, FUN_DISABLE);
|
||||
ModifyReg(REG_RXANA, (HPCF_V << 3) | GAIN_V, FUN_ENABLE);
|
||||
SetParity(FUN_DISABLE);
|
||||
SetReg(REG_TXAMP, AMPLITUDE_V);
|
||||
SetReg(REG_TXCON, MODULATION_V);
|
||||
SetReg(REG_TXI, 0x06);
|
||||
SetReg(REG_THNSET, 0xFF);
|
||||
SetReg(REG_THNMIN, 0x80);
|
||||
SetReg(REG_THNADJ, 0x08);
|
||||
SetReg(REG_RXTXCON, 0);
|
||||
return SUCCESS;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 初始化读卡器以支持Type F (FeliCa) 卡片。
|
||||
* @return 操作状态，SUCCESS表示成功。
|
||||
*/
|
||||
unsigned char ReaderF_Initial(void)
|
||||
{
|
||||
ModifyReg(REG_MISC, 0x04, FUN_ENABLE);
|
||||
LoadProtocol(RX_FELICA_212, TX_FELICA_212);
|
||||
SetReg(REG_TXAMP, AMPLITUDE_F);
|
||||
SetReg(REG_TXCON, MODULATION_F);
|
||||
ModifyReg(REG_RXANA, MASK_RCV_GAIN | MASK_RCV_HPCF, FUN_DISABLE);
|
||||
ModifyReg(REG_RXANA, (HPCF_F << 3) | GAIN_F, FUN_ENABLE);
|
||||
SetParity(FUN_DISABLE);
|
||||
SetReg(REG_THNSET, 0xFF);
|
||||
SetReg(REG_THNMIN, 0x80);
|
||||
SetReg(REG_THNADJ, 0x08);
|
||||
ModifyReg(REG_MISC, 0x04, FUN_DISABLE);
|
||||
return SUCCESS;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 向Type A卡片发送WUPA（Wake-Up A）命令。
|
||||
* @param picc_a [out] 指向存储卡片信息的结构体。
|
||||
* @return 操作状态，SUCCESS表示成功，FAIL表示失败。
|
||||
*/
|
||||
unsigned char ReaderA_Wakeeup(struct picc_a_struct *picc_a)
|
||||
{
|
||||
unsigned char reg_data;
|
||||
SetCommand(CMD_IDLE);
|
||||
SetReg(REG_TXDATANUM, 0x0F);
|
||||
ModifyReg(REG_FIFOCONTROL, BIT_FIFOFLUSH, FUN_ENABLE);
|
||||
SetReg(REG_FIFODATA, RF_CMD_WUPA);
|
||||
ModifyReg(REG_TXCRCCON, BIT_CRCEN, FUN_DISABLE);
|
||||
ModifyReg(REG_RXCRCCON, BIT_CRCEN, FUN_DISABLE);
|
||||
SetCommand(CMD_TRANSCEIVE);
|
||||
rfid_delay_ms(2);
|
||||
GetReg(REG_FIFOLENGTH, ®_data);
|
||||
if (reg_data != 2)
|
||||
return FAIL;
|
||||
|
||||
GetReg(REG_FIFODATA, &picc_a->ATQA[0]);
|
||||
GetReg(REG_FIFODATA, &picc_a->ATQA[1]);
|
||||
|
||||
return SUCCESS;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 向Type A卡片发送REQA（Request A）命令。
|
||||
* @param picc_a [out] 指向存储卡片信息的结构体。
|
||||
* @return 操作状态，SUCCESS表示成功，FAIL表示失败。
|
||||
*/
|
||||
unsigned char ReaderA_Request(struct picc_a_struct *picc_a)
|
||||
{
|
||||
unsigned char reg_data;
|
||||
SetCommand(CMD_IDLE);
|
||||
SetReg(REG_TXDATANUM, 0x0F);
|
||||
ModifyReg(REG_FIFOCONTROL, BIT_FIFOFLUSH, FUN_ENABLE);
|
||||
SetReg(REG_FIFODATA, RF_CMD_REQA);
|
||||
ModifyReg(REG_TXCRCCON, BIT_CRCEN, FUN_DISABLE);
|
||||
ModifyReg(REG_RXCRCCON, BIT_CRCEN, FUN_DISABLE);
|
||||
SetCommand(CMD_TRANSCEIVE);
|
||||
rfid_delay_ms(2);
|
||||
GetReg(REG_FIFOLENGTH, ®_data);
|
||||
if (reg_data != 2)
|
||||
return FAIL;
|
||||
|
||||
GetReg(REG_FIFODATA, &picc_a->ATQA[0]);
|
||||
GetReg(REG_FIFODATA, &picc_a->ATQA[1]);
|
||||
|
||||
return SUCCESS;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 执行Type A卡片的防冲突流程。
|
||||
* @param picc_a [in, out] 指向存储卡片信息的结构体。
|
||||
* @return 操作状态，SUCCESS表示成功，FAIL表示失败。
|
||||
*/
|
||||
unsigned char ReaderA_Anticoll(struct picc_a_struct *picc_a)
|
||||
{
|
||||
unsigned char reg_data;
|
||||
SetCommand(CMD_IDLE);
|
||||
SetReg(REG_TXDATANUM, 0x08);
|
||||
ModifyReg(REG_FIFOCONTROL, BIT_FIFOFLUSH, FUN_ENABLE);
|
||||
SetReg(REG_FIFODATA, RF_CMD_ANTICOLL[picc_a->CASCADE_LEVEL]);
|
||||
SetReg(REG_FIFODATA, 0x20);
|
||||
ModifyReg(REG_TXCRCCON, BIT_CRCEN, FUN_DISABLE);
|
||||
ModifyReg(REG_RXCRCCON, BIT_CRCEN, FUN_DISABLE);
|
||||
SetCommand(CMD_TRANSCEIVE);
|
||||
rfid_delay_ms(2);
|
||||
GetReg(REG_FIFOLENGTH, ®_data);
|
||||
if (reg_data != 5)
|
||||
return FAIL;
|
||||
|
||||
GetReg(REG_FIFODATA, &picc_a->UID[picc_a->CASCADE_LEVEL * 4]);
|
||||
GetReg(REG_FIFODATA, &picc_a->UID[picc_a->CASCADE_LEVEL * 4 + 1]);
|
||||
GetReg(REG_FIFODATA, &picc_a->UID[picc_a->CASCADE_LEVEL * 4 + 2]);
|
||||
GetReg(REG_FIFODATA, &picc_a->UID[picc_a->CASCADE_LEVEL * 4 + 3]);
|
||||
GetReg(REG_FIFODATA, &picc_a->BCC[picc_a->CASCADE_LEVEL]);
|
||||
if ((picc_a->UID[picc_a->CASCADE_LEVEL * 4] ^ picc_a->UID[picc_a->CASCADE_LEVEL * 4 + 1] ^ picc_a->UID[picc_a->CASCADE_LEVEL * 4 + 2] ^ picc_a->UID[picc_a->CASCADE_LEVEL * 4 + 3]) == picc_a->BCC[picc_a->CASCADE_LEVEL])
|
||||
return SUCCESS;
|
||||
|
||||
return FAIL;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 选择一个已经过防冲突的Type A卡片。
|
||||
* @param picc_a [in, out] 指向存储卡片信息的结构体。
|
||||
* @return 操作状态，SUCCESS表示成功，FAIL表示失败。
|
||||
*/
|
||||
unsigned char ReaderA_Select(struct picc_a_struct *picc_a)
|
||||
{
|
||||
unsigned char reg_data;
|
||||
SetCommand(CMD_IDLE);
|
||||
SetReg(REG_TXDATANUM, 0x08);
|
||||
ModifyReg(REG_FIFOCONTROL, BIT_FIFOFLUSH, FUN_ENABLE);
|
||||
SetReg(REG_FIFODATA, RF_CMD_ANTICOLL[picc_a->CASCADE_LEVEL]);
|
||||
SetReg(REG_FIFODATA, 0x70);
|
||||
SetReg(REG_FIFODATA, picc_a->UID[picc_a->CASCADE_LEVEL * 4]);
|
||||
SetReg(REG_FIFODATA, picc_a->UID[picc_a->CASCADE_LEVEL * 4 + 1]);
|
||||
SetReg(REG_FIFODATA, picc_a->UID[picc_a->CASCADE_LEVEL * 4 + 2]);
|
||||
SetReg(REG_FIFODATA, picc_a->UID[picc_a->CASCADE_LEVEL * 4 + 3]);
|
||||
SetReg(REG_FIFODATA, picc_a->BCC[picc_a->CASCADE_LEVEL]);
|
||||
ModifyReg(REG_TXCRCCON, BIT_CRCEN, FUN_ENABLE);
|
||||
ModifyReg(REG_RXCRCCON, BIT_CRCEN, FUN_ENABLE);
|
||||
SetCommand(CMD_TRANSCEIVE);
|
||||
rfid_delay_ms(2);
|
||||
GetReg(REG_ERROR, ®_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, FUN_ENABLE);
|
||||
SetReg(REG_FIFODATA, 0x05); // APf
|
||||
SetReg(REG_FIFODATA, 0x00); // AFI (00:for all cards)
|
||||
SetReg(REG_FIFODATA, 0x08); // PARAM(WUPB, Number of slots = 0)
|
||||
ModifyReg(REG_TXCRCCON, BIT_CRCEN, FUN_ENABLE);
|
||||
ModifyReg(REG_RXCRCCON, BIT_CRCEN, FUN_ENABLE);
|
||||
SetCommand(CMD_TRANSCEIVE);
|
||||
rfid_delay_ms(10);
|
||||
GetReg(REG_ERROR, ®_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, FUN_ENABLE);
|
||||
SetReg(REG_FIFODATA, 0x05); // APf
|
||||
SetReg(REG_FIFODATA, 0x00); // AFI (00:for all cards)
|
||||
SetReg(REG_FIFODATA, 0x00); // PARAM(REQB, Number of slots = 0)
|
||||
ModifyReg(REG_TXCRCCON, BIT_CRCEN, FUN_ENABLE);
|
||||
ModifyReg(REG_RXCRCCON, BIT_CRCEN, FUN_ENABLE);
|
||||
SetCommand(CMD_TRANSCEIVE);
|
||||
rfid_delay_ms(10);
|
||||
GetReg(REG_ERROR, ®_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, FUN_ENABLE);
|
||||
SetReg(REG_FIFODATA, 0x1D);
|
||||
SetReg(REG_FIFODATA, picc_b->PUPI[0]);
|
||||
SetReg(REG_FIFODATA, picc_b->PUPI[1]);
|
||||
SetReg(REG_FIFODATA, picc_b->PUPI[2]);
|
||||
SetReg(REG_FIFODATA, picc_b->PUPI[3]);
|
||||
SetReg(REG_FIFODATA, 0x00); // Param1
|
||||
SetReg(REG_FIFODATA, 0x08); // Param2
|
||||
SetReg(REG_FIFODATA, 0x01); // COMPATIBLE WITH 14443-4
|
||||
SetReg(REG_FIFODATA, 0x01); // CID:01
|
||||
ModifyReg(REG_TXCRCCON, BIT_CRCEN, FUN_ENABLE);
|
||||
ModifyReg(REG_RXCRCCON, BIT_CRCEN, FUN_ENABLE);
|
||||
SetCommand(CMD_TRANSCEIVE);
|
||||
rfid_delay_ms(10);
|
||||
GetReg(REG_ERROR, ®_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, FUN_ENABLE);
|
||||
SetReg(REG_FIFODATA, 0x50);
|
||||
SetReg(REG_FIFODATA, picc_b->PUPI[0]);
|
||||
SetReg(REG_FIFODATA, picc_b->PUPI[1]);
|
||||
SetReg(REG_FIFODATA, picc_b->PUPI[2]);
|
||||
SetReg(REG_FIFODATA, picc_b->PUPI[3]);
|
||||
|
||||
ModifyReg(REG_TXCRCCON, BIT_CRCEN, FUN_ENABLE);
|
||||
ModifyReg(REG_RXCRCCON, BIT_CRCEN, FUN_ENABLE);
|
||||
SetCommand(CMD_TRANSCEIVE);
|
||||
rfid_delay_ms(10);
|
||||
GetReg(REG_ERROR, ®_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, FUN_ENABLE);
|
||||
SetReg(REG_FIFODATA, 0x00);
|
||||
SetReg(REG_FIFODATA, 0x36);
|
||||
SetReg(REG_FIFODATA, 0x00);
|
||||
SetReg(REG_FIFODATA, 0x00);
|
||||
SetReg(REG_FIFODATA, 0x08);
|
||||
|
||||
ModifyReg(REG_TXCRCCON, BIT_CRCEN, FUN_ENABLE);
|
||||
ModifyReg(REG_RXCRCCON, BIT_CRCEN, FUN_ENABLE);
|
||||
SetCommand(CMD_TRANSCEIVE);
|
||||
rfid_delay_ms(10);
|
||||
GetReg(REG_ERROR, ®_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, result; // 新增 result 变量
|
||||
SetCommand(CMD_IDLE);
|
||||
|
||||
|
||||
SetReg(REG_TXDATANUM, 0x08);
|
||||
ModifyReg(REG_FIFOCONTROL, BIT_FIFOFLUSH, FUN_ENABLE);
|
||||
SetReg(REG_FIFODATA, 0x26); // Corrected Inventory flag to indicate AFI is present
|
||||
SetReg(REG_FIFODATA, 0x01);
|
||||
SetReg(REG_FIFODATA, 0x00);
|
||||
|
||||
ModifyReg(REG_TXCRCCON, BIT_CRCEN, FUN_ENABLE);
|
||||
ModifyReg(REG_RXCRCCON, BIT_CRCEN, FUN_ENABLE);
|
||||
|
||||
SetCommand(CMD_TRANSCEIVE);
|
||||
rfid_delay_ms(50);
|
||||
|
||||
GetReg(REG_ERROR, ®_data);
|
||||
if ((reg_data & 0x0F) != 0){
|
||||
xlog("REG_ERROR = 0x%02X\n", reg_data);
|
||||
return FAIL;
|
||||
}
|
||||
|
||||
GetReg(REG_FIFOLENGTH, ®_data);
|
||||
if (reg_data != 10){
|
||||
xlog("FIFO Length is %d, expected 10.\n", reg_data);
|
||||
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]);
|
||||
// xlog("%d ",picc_v->UID[i]);
|
||||
}
|
||||
// xlog("\r\n");
|
||||
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, result;
|
||||
SetCommand(CMD_IDLE);
|
||||
SetReg(REG_TXDATANUM, 0x08);
|
||||
ModifyReg(REG_FIFOCONTROL, BIT_FIFOFLUSH, FUN_ENABLE);
|
||||
SetReg(REG_FIFODATA, 0x22); // Addressed flag
|
||||
SetReg(REG_FIFODATA, 0x25); // Select command
|
||||
SetReg(REG_FIFODATA, picc_v->UID[0]);
|
||||
SetReg(REG_FIFODATA, picc_v->UID[1]);
|
||||
SetReg(REG_FIFODATA, picc_v->UID[2]);
|
||||
SetReg(REG_FIFODATA, picc_v->UID[3]);
|
||||
SetReg(REG_FIFODATA, picc_v->UID[4]);
|
||||
SetReg(REG_FIFODATA, picc_v->UID[5]);
|
||||
SetReg(REG_FIFODATA, picc_v->UID[6]);
|
||||
SetReg(REG_FIFODATA, picc_v->UID[7]);
|
||||
ModifyReg(REG_TXCRCCON, BIT_CRCEN, FUN_ENABLE);
|
||||
ModifyReg(REG_RXCRCCON, BIT_CRCEN, FUN_ENABLE);
|
||||
|
||||
SetCommand(CMD_TRANSCEIVE);
|
||||
rfid_delay_ms(10);
|
||||
GetReg(REG_ERROR, ®_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, FUN_ENABLE);
|
||||
SetReg(REG_FIFODATA, 0x02); // Addressed flag
|
||||
SetReg(REG_FIFODATA, 0x20); // Read Single Block command
|
||||
SetReg(REG_FIFODATA, block_num);
|
||||
|
||||
ModifyReg(REG_TXCRCCON, BIT_CRCEN, FUN_ENABLE);
|
||||
ModifyReg(REG_RXCRCCON, BIT_CRCEN, FUN_ENABLE);
|
||||
|
||||
SetCommand(CMD_TRANSCEIVE);
|
||||
rfid_delay_ms(10);
|
||||
GetReg(REG_ERROR, ®_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, FUN_ENABLE);
|
||||
SetReg(REG_FIFODATA, 0x02); // Addressed flag
|
||||
SetReg(REG_FIFODATA, 0x21); // Write Single Block command
|
||||
SetReg(REG_FIFODATA, block_num);
|
||||
SetReg(REG_FIFODATA, picc_v->BLOCK_DATA[0]);
|
||||
SetReg(REG_FIFODATA, picc_v->BLOCK_DATA[1]);
|
||||
SetReg(REG_FIFODATA, picc_v->BLOCK_DATA[2]);
|
||||
SetReg(REG_FIFODATA, picc_v->BLOCK_DATA[3]);
|
||||
ModifyReg(REG_TXCRCCON, BIT_CRCEN, FUN_ENABLE);
|
||||
ModifyReg(REG_RXCRCCON, BIT_CRCEN, FUN_ENABLE);
|
||||
|
||||
SetCommand(CMD_TRANSCEIVE);
|
||||
rfid_delay_ms(10);
|
||||
GetReg(REG_ERROR, ®_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, FUN_ENABLE);
|
||||
|
||||
SetReg(REG_FIFODATA, 0x06);
|
||||
SetReg(REG_FIFODATA, 0x00);
|
||||
SetReg(REG_FIFODATA, 0xFF);
|
||||
SetReg(REG_FIFODATA, 0xFF);
|
||||
SetReg(REG_FIFODATA, 0x10);
|
||||
SetReg(REG_FIFODATA, 0x00);
|
||||
|
||||
ModifyReg(REG_TXCRCCON, BIT_CRCEN, FUN_ENABLE);
|
||||
ModifyReg(REG_RXCRCCON, BIT_CRCEN, FUN_ENABLE);
|
||||
|
||||
SetCommand(CMD_TRANSCEIVE);
|
||||
rfid_delay_ms(10);
|
||||
GetReg(REG_ERROR, ®_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;
|
||||
}
|
||||
|
||||
305
apps/earphone/xtell_remote_control/RFID/rfid_event.c
Normal file
305
apps/earphone/xtell_remote_control/RFID/rfid_event.c
Normal file
@ -0,0 +1,305 @@
|
||||
/********************************************************************************************************
|
||||
* @file rfid_main.c
|
||||
* @brief RFID 读卡器应用层主逻辑文件
|
||||
********************************************************************************************************/
|
||||
|
||||
#include "./include/rfid_main.h"
|
||||
#include "./include/READER.h"
|
||||
#include "./include/READER_REG.h"
|
||||
#include "./include/MIFARE.h"
|
||||
#include "./include/NTAG.h"
|
||||
#include "./include/CPU_CARD.h"
|
||||
#include "./rfid_hal.h"
|
||||
|
||||
|
||||
#define FUN_ENABLE_XLOG 1
|
||||
#ifdef xlog
|
||||
#undef xlog
|
||||
#endif
|
||||
#if FUN_ENABLE_XLOG
|
||||
#define xlog(format, ...) printf("[XT:%s] " format, __func__, ##__VA_ARGS__)
|
||||
#else
|
||||
#define xlog(format, ...) ((void)0)
|
||||
#endif
|
||||
|
||||
|
||||
/**
|
||||
* @brief 处理Type A卡片事件。uid：8位数组
|
||||
* @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(char* uid)
|
||||
{
|
||||
unsigned char result;
|
||||
int i;
|
||||
xlog("TYPE_A_EVENT begin\n");
|
||||
|
||||
// 初始化读卡器为Type A模式
|
||||
result = ReaderA_Initial();
|
||||
if (result != SUCCESS)
|
||||
{
|
||||
xlog("INIT_ERROR\r\n");
|
||||
SetCW(FUN_DISABLE);
|
||||
return;
|
||||
}
|
||||
|
||||
// 打开RF场（载波）
|
||||
result = SetCW(FUN_ENABLE);
|
||||
if (result != SUCCESS)
|
||||
{
|
||||
xlog("CW_ERROR\r\n");
|
||||
SetCW(FUN_DISABLE);
|
||||
return;
|
||||
}
|
||||
|
||||
// 激活Type A卡片
|
||||
result = ReaderA_CardActivate(&PICC_A);
|
||||
if (result != SUCCESS)
|
||||
{
|
||||
xlog("ReaderA_CardActivate_ERROR\r\n");
|
||||
SetCW(FUN_DISABLE);
|
||||
return;
|
||||
}
|
||||
|
||||
xlog("************* TYPE A CARD ************* \r\n");
|
||||
xlog("-> ATQA = %02X%02X\r\n", PICC_A.ATQA[0], PICC_A.ATQA[1]);
|
||||
|
||||
if (PICC_A.UID_Length > 0)
|
||||
{
|
||||
xlog("-> UID = ");
|
||||
for (i = 0; i < PICC_A.UID_Length; i++)
|
||||
{
|
||||
uid[i] = PICC_A.UID[i];
|
||||
xlog("%02X", PICC_A.UID[i]);
|
||||
}
|
||||
xlog("\r\n");
|
||||
}
|
||||
xlog("-> SAK = %02X\r\n", PICC_A.SAK[0]);
|
||||
|
||||
// 根据SAK值判断卡片类型
|
||||
if (PICC_A.SAK[0] == 0x08)
|
||||
{
|
||||
xlog("************* Mifare CARD ************* \r\n");
|
||||
result = MIFARE_CARD_EVENT();
|
||||
}
|
||||
else if ((PICC_A.SAK[0] == 0x28) || (PICC_A.SAK[0] == 0x20))
|
||||
{
|
||||
xlog("************* CPU CARD ************* \r\n");
|
||||
result = CPU_CARD_EVENT();
|
||||
}
|
||||
else if (PICC_A.SAK[0] == 0x04)
|
||||
{
|
||||
xlog("************* NTAG CARD ************* \r\n");
|
||||
result = NTAG_EVENT();
|
||||
}
|
||||
|
||||
SetCW(FUN_DISABLE); // 关闭RF场
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 处理Type B卡片事件。
|
||||
* @details
|
||||
* 该函数执行ISO/IEC 14443 Type B卡片的激活流程，包括：
|
||||
* 1. 初始化读卡器以支持Type B协议。
|
||||
* 2. 打开RF场。
|
||||
* 3. 发送REQB/WUPB命令寻卡。
|
||||
* 4. 发送ATTRIB命令选卡。
|
||||
* 5. 获取卡片序列号（SN）。
|
||||
* 6. 操作结束后关闭RF场。
|
||||
* @return 无。
|
||||
*/
|
||||
void TYPE_B_EVENT(void)
|
||||
{
|
||||
unsigned char result;
|
||||
int i;
|
||||
xlog("TYPE_B_EVENT begin\n");
|
||||
|
||||
ReaderB_Initial();
|
||||
SetCW(FUN_ENABLE);
|
||||
|
||||
result = ReaderB_Request(&PICC_B);
|
||||
if (result != SUCCESS)
|
||||
{
|
||||
SetCW(FUN_DISABLE);
|
||||
return;
|
||||
}
|
||||
|
||||
xlog("************* TYPE B CARD ************* \r\n");
|
||||
// 打印ATQB信息
|
||||
xlog("-> ATQB = ");
|
||||
for(i=0; i<12; i++) xlog("%02X", PICC_B.ATQB[i]);
|
||||
xlog("\r\n");
|
||||
|
||||
result = ReaderB_Attrib(&PICC_B);
|
||||
if (result != SUCCESS)
|
||||
{
|
||||
SetCW(FUN_DISABLE);
|
||||
return;
|
||||
}
|
||||
xlog("-> ATTRIB = %02X\r\n", PICC_B.CID);
|
||||
|
||||
result = ReaderB_Get_SN(&PICC_B);
|
||||
if (result != SUCCESS)
|
||||
{
|
||||
SetCW(FUN_DISABLE);
|
||||
return;
|
||||
}
|
||||
xlog("-> SN = ");
|
||||
for(i=0; i<8; i++) xlog("%02X", PICC_B.SN[i]);
|
||||
xlog("\r\n");
|
||||
|
||||
SetCW(FUN_DISABLE);
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 处理Type V (ISO/IEC 15693) 卡片事件。
|
||||
* @details
|
||||
* 该函数执行ISO/IEC 15693 Vicinity卡片的交互流程，包括：
|
||||
* 1. 初始化读卡器以支持15693协议。
|
||||
* 2. 打开RF场。
|
||||
* 3. 发送Inventory命令寻卡并获取UID。
|
||||
* 4. 发送Select命令选择卡片。
|
||||
* 5. 示例性地对第4块进行写操作，然后再读回校验。
|
||||
* 6. 操作结束后关闭RF场。
|
||||
* @return 无。
|
||||
*/
|
||||
void TYPE_V_EVENT(char* uid)
|
||||
{
|
||||
unsigned char result, i;
|
||||
xlog("TYPE_V_EVENT begin\n");
|
||||
|
||||
ReaderV_Initial();
|
||||
SetCW(FUN_ENABLE);
|
||||
|
||||
result = ReaderV_Inventory(&PICC_V);
|
||||
if (result != SUCCESS)
|
||||
{
|
||||
SetCW(FUN_DISABLE);
|
||||
xlog("-> ReaderV Inventory ERROR!\r\n");
|
||||
return;
|
||||
}
|
||||
|
||||
xlog("************* TYPE V CARD ************* \r\n");
|
||||
xlog("UID=");
|
||||
for (i = 0; i < 8; i++)
|
||||
{
|
||||
uid[i] = PICC_V.UID[i];
|
||||
xlog("%02X", PICC_V.UID[i]);
|
||||
}
|
||||
xlog("\r\n");
|
||||
|
||||
result = ReaderV_Select(&PICC_V);
|
||||
if (result != SUCCESS)
|
||||
{
|
||||
SetCW(FUN_DISABLE);
|
||||
xlog("-> ReaderV Select ERROR!\r\n");
|
||||
return;
|
||||
}
|
||||
|
||||
// 示例：写单个块
|
||||
memcpy(PICC_V.BLOCK_DATA, "\x00\x00\x00\x00", 4);
|
||||
result = ReaderV_WriteSingleBlock(4, &PICC_V);
|
||||
if (result != SUCCESS)
|
||||
{
|
||||
SetCW(FUN_DISABLE);
|
||||
xlog("-> ReaderV WriteSingleBlock ERROR!\r\n");
|
||||
return;
|
||||
}
|
||||
xlog("WriteSingleBlock SUCCESS\r\n");
|
||||
|
||||
// 示例：读单个块
|
||||
result = ReaderV_ReadSingleBlock(4, &PICC_V);
|
||||
if (result != SUCCESS)
|
||||
{
|
||||
SetCW(FUN_DISABLE);
|
||||
xlog("-> ReaderV ReadSingleBlock ERROR!\r\n");
|
||||
return;
|
||||
}
|
||||
xlog("BLOCK DATA = %02X%02X%02X%02X \r\n", PICC_V.BLOCK_DATA[0], PICC_V.BLOCK_DATA[1], PICC_V.BLOCK_DATA[2], PICC_V.BLOCK_DATA[3]);
|
||||
|
||||
SetCW(FUN_DISABLE);
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 处理Type F (FeliCa) 卡片事件。
|
||||
* @details
|
||||
* 该函数执行FeliCa卡片的交互流程，包括：
|
||||
* 1. 初始化读卡器以支持FeliCa协议。
|
||||
* 2. 打开RF场。
|
||||
* 3. 发送Inventory命令寻卡并获取UID。
|
||||
* 4. 后续可以添加与FeliCa卡的数据交换命令。
|
||||
* 5. 操作结束后关闭RF场。
|
||||
* @note 当前实现仅包含寻卡部分。
|
||||
* @return 无。
|
||||
*/
|
||||
void TYPE_F_EVENT(void)
|
||||
{
|
||||
unsigned char result, i;
|
||||
xlog("TYPE_F_EVENT begin\n");
|
||||
ReaderF_Initial();
|
||||
SetCW(FUN_ENABLE);
|
||||
|
||||
result = ReaderF_Inventory(&PICC_F);
|
||||
if (result != SUCCESS)
|
||||
{
|
||||
SetCW(FUN_DISABLE);
|
||||
return;
|
||||
}
|
||||
|
||||
xlog("************* TYPE F CARD ************* \r\n");
|
||||
xlog("->TYPE F UID = ");
|
||||
for(i=0; i<8; i++) xlog("%02X", PICC_F.UID[i]);
|
||||
xlog("\r\n");
|
||||
|
||||
SetCW(FUN_DISABLE);
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* @brief RFID模块的主任务函数。
|
||||
* @details
|
||||
* 利用定时器调用
|
||||
* @return 无。
|
||||
*/
|
||||
void rfid_task_fuc(void)
|
||||
{
|
||||
unsigned char result, reg_data;
|
||||
static u8 first_init = 0;
|
||||
if(first_init == 0){
|
||||
first_init = 1;
|
||||
|
||||
FM176XX_HardInit();
|
||||
rfid_delay_ms(5); // 硬件初始化后增加一个短暂延时，确保芯片稳定
|
||||
// 2. 复位 FM176XX 芯片
|
||||
|
||||
result = FM176XX_SoftReset();
|
||||
if (result != SUCCESS)
|
||||
{
|
||||
xlog("FM176XX HardReset FAIL\r\n");
|
||||
}
|
||||
else
|
||||
{
|
||||
xlog("FM176XX HardReset SUCCESS\r\n");
|
||||
}
|
||||
|
||||
rfid_delay_ms(10); // 复位后延时
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
// 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();
|
||||
}
|
||||
Some files were not shown because too many files have changed in this diff Show More
Reference in New Issue
Block a user