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

cun

Browse Source
This commit is contained in:
lmx
2025-11-18 17:27:06 +08:00
parent d0d9c0a630
commit ebca849be3
8 changed files with 1123 additions and 165 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@ -173,12 +173,13 @@ void test(){
#define SENSOR_DATA_BUFFER_SIZE 100 // 定义缓冲区可以存储100个sensor_data_t元素
#define SENSOR_DATA_BUFFER_SIZE 500 // 定义缓冲区可以存储100个sensor_data_t元素
static circle_buffer_t sensor_read; // 环形缓冲区管理结构体
typedef struct {
signed short acc_data[3];
signed short gyr_data[3];
float angle[3]; //pitch roll yaw
float quaternion_output[4];
} sensor_data_t;
static sensor_data_t sensor_read_buffer[SENSOR_DATA_BUFFER_SIZE]; // 存放sensor读到的数据
@ -187,7 +188,8 @@ static BLE_send_data_t sensor_send_buffer[SENSOR_DATA_BUFFER_SIZE]; // 存放ble
static u8 mutex1 = 0;
static u8 mutex2 = 0;
static int count_test1 = 0;
static int count_test2 = 0;
/**
* @brief //读取传感器的数据放进缓冲区
*
@ -238,6 +240,8 @@ void sensor_read_data(){
memcpy(tmp.gyr_data, &combined_raw_data[3], 3 * sizeof(signed short));
if(mutex1 == 0){
mutex1 = 1;
// count_test1++;
// xlog("count_test_1: %d\n",count_test1);
circle_buffer_write(&sensor_read, &tmp);
mutex1 = 0;
}
@ -264,8 +268,9 @@ void calculate_data(){
}else{
return;
}
BLE_send_data_t data_by_calculate = sensor_processing_task(tmp.acc_data, tmp.gyr_data,tmp.angle);
BLE_send_data_t data_by_calculate = sensor_processing_task(tmp.acc_data, tmp.gyr_data,tmp.angle,tmp.quaternion_output);
@ -455,10 +460,12 @@ void sensor_measure(void){
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;
if(count_test1 >= 100){
count_test1 = 0;
xlog("count_test1\n");
}
count_test1++;
static sensor_data_t tmp;
SL_SC7U22_RawData_Read(tmp.acc_data,tmp.gyr_data);
@ -467,9 +474,10 @@ void sensor_measure(void){
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 = 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);
status = Q_SL_SC7U22_Angle_Output(1, combined_raw_data, tmp.angle, 0, tmp.quaternion_output);
int count = 0;
if(count > 100){
@ -487,15 +495,18 @@ void sensor_measure(void){
}
} else {
// printf("Calculate the time interval =============== start\n");
status = 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);
status = Q_SL_SC7U22_Angle_Output(0, combined_raw_data, tmp.angle, 0, tmp.quaternion_output);
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);
BLE_send_data_t data_by_calculate = sensor_processing_task(tmp.acc_data, tmp.gyr_data,tmp.angle, tmp.quaternion_output);
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");
}
@ -533,7 +544,7 @@ void xtell_task_create(void){
ano_protocol_init(115200);
// create_process(&calculate_data_id, "calculate",NULL, sensor_measure, 10);
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));
@ -545,11 +556,11 @@ void xtell_task_create(void){
xlog("SkiingTracker_Init\n");
create_process(&sensor_read_data_id, "read",NULL, sensor_read_data, 10);
// create_process(&sensor_read_data_id, "read",NULL, sensor_read_data, 10);
create_process(&calculate_data_id, "calculate",NULL, calculate_data, 6);
// create_process(&calculate_data_id, "calculate",NULL, calculate_data, 4);
create_process(&ble_send_data_id, "send",NULL, BLE_send_data, 1);
// create_process(&ble_send_data_id, "send",NULL, BLE_send_data, 1);
#if 0
hw_iic_init_result = ret;