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

存档

Browse Source
This commit is contained in:
lmx
2025-11-18 10:15:00 +08:00
parent b621ef7e44
commit d0d9c0a630
46 changed files with 172875 additions and 173101 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@ -20,6 +20,7 @@
#include "btstack/avctp_user.h"
#include "calculate/skiing_tracker.h"
#include "xtell.h"
#include "./ano/ano_protocol.h"
///////////////////////////////////////////////////////////////////////////////////////////////////
//宏定义
#define ENABLE_XLOG 1
@ -66,83 +67,11 @@ u16 test_id=0;
//////////////////////////////////////////////////////////////////////////////////////////////////
// /**
// * @brief 向匿名上位机发送数据帧
// * @param function_id 功能码 (例如 0x01, 0x03)
// * @param data 指向 int16_t 数据数组的指针 (例如加速度、欧拉角)
// * @param data_len int16_t 数据的个数 (例如发送6轴数据时为6，发送3个欧拉角时为3)
// * @param status_byte 附加的状态字节 (例如 SHOCK_STA 或 FUSION_STA)
// */
// void send_data_anotc(uint8_t function_id, int16_t* data, uint8_t data_len, uint8_t status_byte) {
// // 定义一个足够大的缓冲区来构建数据帧
// // 最大长度(ID 0x01): 1(HEAD)+1(D_ADDR)+1(ID)+1(LEN)+13(DATA)+1(SC)+1(AC) = 19字节
// uint8_t buffer[32];
// uint8_t data_payload_len = data_len * sizeof(int16_t) + sizeof(uint8_t);
// // 1. 填充帧头和地址
// buffer[0] = 0xAA; // 帧头 HEAD
// buffer[1] = 0xFF; // 目标地址 D_ADDR
// // 2. 填充功能码和数据长度
// buffer[2] = function_id;
// buffer[3] = data_payload_len;
// // 3. 填充数据内容 (DATA)
// // 首先使用 memcpy 拷贝主要的 int16_t 数组数据
// // &buffer[4] 是数据区的起始地址
// memcpy(&buffer[4], (uint8_t*)data, data_len * sizeof(int16_t));
// // 然后在数据区末尾填充状态字节
// buffer[4 + data_len * sizeof(int16_t)] = status_byte;
// // 4. 计算校验和 (SC 和 AC)
// uint8_t sum_check = 0;
// uint8_t add_check = 0;
// // SC: 和校验 (从帧头到数据区最后一个字节)
// for (int i = 0; i < 4 + data_payload_len; ++i) {
// sum_check += buffer[i];
// }
// // 将SC填充到缓冲区
// buffer[4 + data_payload_len] = sum_check;
// // AC: 附加校验 (从帧头到SC)
// for (int i = 0; i < 4 + data_payload_len + 1; ++i) {
// add_check += buffer[i];
// }
// // 将AC填充到缓冲区
// buffer[4 + data_payload_len + 1] = add_check;
// // 5. 发送整个数据帧
// uint16_t frame_length = 4 + data_payload_len + 2;
// // Serial_Send_Buffer(buffer, frame_length);
// for (int i = 0; i < frame_length; ++i) {
// // 使用 %c 格式化字符来发送单个字节的原始值
// printf("%c", buffer[i]);
// }
// printf("\n");
// }
void ble_send_data(signed short *acc_gyro_input, float *Angle_output){
char buffer[50]; //一次最多发送50字节
u8 len = 0;
//AA FF 01 六轴数据 EE
//TO DO
send_data_to_ble_client(&buffer,len);
//AA FF 02 欧若拉角数据 EE
// TO DO
send_data_to_ble_client(&buffer,len);
}
// 从环形缓冲区读取数据并发送
void send_sensor_data_task(void) {
// printf("xtell_ble_send\n");
}
#if 0
BLE_send_data_t BLE_send_data;
@ -249,13 +178,15 @@ static circle_buffer_t sensor_read; // 环形缓冲区管理结构体
typedef struct {
signed short acc_data[3];
signed short gyr_data[3];
float angle[3];
float angle[3]; //pitch roll yaw
} sensor_data_t;
static sensor_data_t sensor_read_buffer[SENSOR_DATA_BUFFER_SIZE]; // 存放sensor读到的数据
static circle_buffer_t sensor_send; // 环形缓冲区管理结构体
static BLE_send_data_t sensor_send_buffer[SENSOR_DATA_BUFFER_SIZE]; // 存放ble要发送的数据
static u8 mutex1 = 0;
static u8 mutex2 = 0;
/**
* @brief //读取传感器的数据放进缓冲区
@ -280,16 +211,19 @@ void sensor_read_data(){
memcpy(&combined_raw_data[3], tmp.gyr_data, 3 * sizeof(signed short));
if (!calibration_done) { //第1次启动，开启零漂检测
// status = SL_SC7U22_Angle_Output(1, combined_raw_data, tmp.angle, 0);
status = Original_SL_SC7U22_Angle_Output(1, combined_raw_data, tmp.angle, 0);
status = SL_SC7U22_Angle_Output(1, combined_raw_data, tmp.angle, 0);
// status = SIX_SL_SC7U22_Angle_Output(1, combined_raw_data, tmp.angle, 0);
// status = Original_SL_SC7U22_Angle_Output(1, combined_raw_data, tmp.angle, 0);
// static count = 0;
// if(count > 100){
// count = 0;
// xlog("SL_SC7U22_Angle_Output status:%d",status);
// xlog("RawData:AX=%d,AY=%d,AZ=%d,GX=%d,GY=%d,GZ=%d\r\n",tmp.acc_data[0],tmp.acc_data[1],tmp.acc_data[2],tmp.gyr_data[0],tmp.gyr_data[1],tmp.gyr_data[2]);
// }
// count++;
int count = 0;
if(count > 100){
count = 0;
char log_buffer[100]; // 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);
}
count++;
if (status == 1) {
calibration_done = 1;
@ -297,11 +231,18 @@ void sensor_read_data(){
}
} else {
// printf("Calculate the time interval =============== start\n");
// status = SL_SC7U22_Angle_Output(0, combined_raw_data, tmp.angle, 0);
status = Original_SL_SC7U22_Angle_Output(0, combined_raw_data, tmp.angle, 0);
status = SL_SC7U22_Angle_Output(0, combined_raw_data, tmp.angle, 0);
// status = SIX_SL_SC7U22_Angle_Output(0, combined_raw_data, tmp.angle, 0);
// status = Original_SL_SC7U22_Angle_Output(0, combined_raw_data, tmp.angle, 0);
memcpy(tmp.acc_data, &combined_raw_data[0], 3 * sizeof(signed short));
memcpy(tmp.gyr_data, &combined_raw_data[3], 3 * sizeof(signed short));
circle_buffer_write(&sensor_read, &tmp);
if(mutex1 == 0){
mutex1 = 1;
circle_buffer_write(&sensor_read, &tmp);
mutex1 = 0;
}
extern void ano_send_attitude_data(float rol, float pit, float yaw, uint8_t fusion_sta) ;
ano_send_attitude_data(tmp.angle[0],tmp.angle[1],tmp.angle[2], 1);
}
// xlog("=======sensor_read_data END\n");
@ -316,12 +257,27 @@ void calculate_data(){
return;
}
circle_buffer_read(&sensor_read, &tmp);
if(mutex1 == 0){
mutex1 = 1;
circle_buffer_read(&sensor_read, &tmp);
mutex1 = 0;
}else{
return;
}
BLE_send_data_t data_by_calculate = sensor_processing_task(tmp.acc_data, tmp.gyr_data,tmp.angle);
if(circle_buffer_is_full(&sensor_send))
return;
circle_buffer_write(&sensor_send, &data_by_calculate);
if(mutex2 == 0){
mutex2 = 1;
circle_buffer_write(&sensor_send, &data_by_calculate);
mutex2 = 0;
}
// extern void BLE_send_data();
// BLE_send_data();
@ -339,7 +295,14 @@ void BLE_send_data(){
#ifdef XTELL_TEST
// #if 0
BLE_send_data_t tmp;
circle_buffer_read(&sensor_send, &tmp);
if(mutex2 == 0){
mutex2 = 1;
circle_buffer_read(&sensor_send, &tmp);
mutex2 = 0;
}else{
return;
}
if(count >=100){
// extern debug_t debug2;
@ -361,6 +324,7 @@ void BLE_send_data(){
// send_data_to_ble_client(&log_buffer,strlen(log_buffer));
memset(&log_buffer, 0, 100);
#if 0
// 使用 snprintf 进行格式化
num_chars_written = snprintf(
log_buffer, // 目标缓冲区
@ -398,7 +362,7 @@ void BLE_send_data(){
tmp.angle_data[0],tmp.angle_data[1],tmp.angle_data[2]
);
send_data_to_ble_client(&log_buffer,strlen(log_buffer));
#endif
short acc_mo_cms = sqrtf(tmp.acc_data[0]*tmp.acc_data[0] + tmp.acc_data[1]*tmp.acc_data[1] + tmp.acc_data[2]*tmp.acc_data[2])-900;
memset(&log_buffer, 0, 100);
num_chars_written = snprintf(
@ -485,6 +449,56 @@ void xt_hw_iic_test(){
}
#endif
void sensor_measure(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;
if(circle_buffer_is_full(&sensor_read)){
// xlog("sensor_read_data: read buffer full\n");
return;
}
static sensor_data_t tmp;
SL_SC7U22_RawData_Read(tmp.acc_data,tmp.gyr_data);
// xlog("=======sensor_read_data middle 1\n");
memcpy(&combined_raw_data[0], tmp.acc_data, 3 * sizeof(signed short));
memcpy(&combined_raw_data[3], tmp.gyr_data, 3 * sizeof(signed short));
if (!calibration_done) { //第1次启动，开启零漂检测
status = SL_SC7U22_Angle_Output(1, combined_raw_data, tmp.angle, 0);
// status = SIX_SL_SC7U22_Angle_Output(1, combined_raw_data, tmp.angle, 0);
// status = Original_SL_SC7U22_Angle_Output(1, combined_raw_data, tmp.angle, 0);
int count = 0;
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);
}
count++;
if (status == 1) {
calibration_done = 1;
printf("Sensor calibration successful! Skiing mode is active.\n");
}
} else {
// printf("Calculate the time interval =============== start\n");
status = SL_SC7U22_Angle_Output(0, combined_raw_data, tmp.angle, 0);
// status = SIX_SL_SC7U22_Angle_Output(0, combined_raw_data, tmp.angle, 0);
// status = Original_SL_SC7U22_Angle_Output(0, combined_raw_data, tmp.angle, 0);
memcpy(tmp.acc_data, &combined_raw_data[0], 3 * sizeof(signed short));
memcpy(tmp.gyr_data, &combined_raw_data[3], 3 * sizeof(signed short));
BLE_send_data_t data_by_calculate = sensor_processing_task(tmp.acc_data, tmp.gyr_data,tmp.angle);
extern void ano_send_attitude_data(float rol, float pit, float yaw, uint8_t fusion_sta) ;
ano_send_attitude_data(tmp.angle[0],tmp.angle[1],tmp.angle[2], 1);
}
}
void xtell_task_create(void){
// int ret = hw_iic_init(0);
@ -507,7 +521,7 @@ void xtell_task_create(void){
gpio_direction_output(IO_PORTE_05,1);
// os_time_dly(10);
delay_2ms(10);
// delay_2ms(10);
SL_SC7U22_Config();
// extern u8 LIS2DH12_Config(void);
@ -518,6 +532,9 @@ void xtell_task_create(void){
// circle_buffer_init(&sensor_cb, sensor_data_buffer, SENSOR_DATA_BUFFER_SIZE);
ano_protocol_init(115200);
// create_process(&calculate_data_id, "calculate",NULL, sensor_measure, 10);
circle_buffer_init(&sensor_read, sensor_read_buffer, SENSOR_DATA_BUFFER_SIZE, sizeof(sensor_data_t));
circle_buffer_init(&sensor_send, sensor_send_buffer, SENSOR_DATA_BUFFER_SIZE, sizeof(BLE_send_data_t));