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 18:47:05 +08:00
parent ad3ab64b72
commit 054ea8644a
3 changed files with 90 additions and 8 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@ -179,7 +179,7 @@ typedef struct {
signed short acc_data[3];
signed short gyr_data[3];
float angle[3]; //pitch roll yaw
float quaternion_output[4];
float quaternion_output[4]; //四元数数据
} sensor_data_t;
static sensor_data_t sensor_read_buffer[SENSOR_DATA_BUFFER_SIZE]; // 存放sensor读到的数据
@ -213,10 +213,12 @@ 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 = 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){
count = 0;
@ -233,9 +235,11 @@ 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 = 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));
if(mutex1 == 0){
@ -329,7 +333,7 @@ void BLE_send_data(){
// send_data_to_ble_client(&log_buffer,strlen(log_buffer));
memset(&log_buffer, 0, 100);
#if 0
#if 1
// 使用 snprintf 进行格式化
num_chars_written = snprintf(
log_buffer, // 目标缓冲区
@ -557,7 +561,7 @@ void xtell_task_create(void){
// create_process(&sensor_read_data_id, "read",NULL, sensor_read_data, 10);
//
// create_process(&calculate_data_id, "calculate",NULL, calculate_data, 4);
// create_process(&ble_send_data_id, "send",NULL, BLE_send_data, 1);