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

地磁8面校准完成

Browse Source
This commit is contained in:
lmx
2025-11-20 19:30:34 +08:00
parent 2bfdc81991
commit 9ccf1acda8
17 changed files with 1475 additions and 603 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@ -23,6 +23,7 @@
#include "./ano/ano_protocol.h"
#include "./sensor/MMC56.h"
#include "./sensor/BMP280.h"
#include "./sensor/AK8963.h"
///////////////////////////////////////////////////////////////////////////////////////////////////
//宏定义
#define ENABLE_XLOG 1
@ -60,6 +61,7 @@ static u16 calculate_data_id;
static u8 sensor_data_buffer[SENSOR_DATA_BUFFER_SIZE];
static circle_buffer_t sensor_cb;
static int count = 0;
//--- test ---
// 全局变量
@ -219,9 +221,8 @@ void sensor_read_data(){
// 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);
status = Q_SL_SC7U22_Angle_Output(1, combined_raw_data, tmp.angle,NULL, 0, tmp.quaternion_output);
int count = 0;
if(count > 100){
count = 0;
char log_buffer[100]; // 100个字符应该足够了
@ -240,7 +241,7 @@ void sensor_read_data(){
// 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);
status = Q_SL_SC7U22_Angle_Output(0, combined_raw_data, tmp.angle,NULL, 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));
@ -295,7 +296,6 @@ void calculate_data(){
// xlog("=======end\n");
}
static int count = 0;
extern char xt_Check_Flag;
void BLE_send_data(){
// xlog("=======start\n");
@ -462,66 +462,63 @@ void xt_hw_iic_test(){
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;
static signed short combined_raw_data[6];
static int initialized = 0;
static int calibration_done = 0;
char status = 0;
// 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);
// // 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);
// status = Q_SL_SC7U22_Angle_Output(1, combined_raw_data, tmp.angle, 0, tmp.quaternion_output);
// 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);
// 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, 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);
// }
static sensor_data_t tmp;
mmc5603nj_mag_data_t mag_data;
// mmc5603nj_read_mag_data(&mag_data);
SL_SC7U22_RawData_Read(tmp.acc_data,tmp.gyr_data);
// os_time_dly(1);
mmc5603nj_read_mag_data(&mag_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(count_test2 > 100){
// count_test2++;
printf("Mag X: %.4f, Y: %.4f, Z: %.4f Gauss\n", mag_data.x, mag_data.y, mag_data.z);
// }
// count_test2++;
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);
status = Q_SL_SC7U22_Angle_Output(1, combined_raw_data, tmp.angle,&mag_data, 0, tmp.quaternion_output);
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++;
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);
status = Q_SL_SC7U22_Angle_Output(0, combined_raw_data, tmp.angle,&mag_data, 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, 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);
}
// mmc5603nj_mag_data_t mag_data;
// mmc5603nj_read_mag_data(&mag_data);
// float temperature = mmc5603nj_get_temperature();
// count_test1++;
// if(count_test1 >500){
// count_test1 =0;
// xlog("Mag X: %.4f, Y: %.4f, Z: %.4f Gauss\n", mag_data.x, mag_data.y, mag_data.z);
// }
// xlog("=======sensor_read_data END\n");
}
@ -550,27 +547,21 @@ void xtell_task_create(void){
// os_time_dly(10);
// delay_2ms(10);
SL_SC7U22_Config();
// if (mmc5603nj_init() != 0) {
// xlog("MMC5603NJ initialization failed!\n");
// if(bmp280_init() != 0){
// xlog("bmp280 init error\n");
// }
// xlog("MMC5603NJ PID: 0x%02X\n", mmc5603nj_get_pid());
// // 启用连续测量模式
// mmc5603nj_enable_continuous_mode();
// xlog("Continuous measurement mode enabled.\n");
// float temp, press;
// bmp280_read_data(&temp, &press);
// xlog("get temp: %d, get press: %d\n",temp, press);
// MPU9250_Mag_Init();
//iic总线设备扫描
extern void i2c_scanner_probe(void);
i2c_scanner_probe();
if(bmp280_init() != 0){
xlog("bmp280 init error\n");
}
float temp, press;
bmp280_read_data(&temp, &press);
xlog("get temp: %d, get press: %d\n",temp, press);
// extern void i2c_scanner_probe(void);
// i2c_scanner_probe();
xlog("xtell_task_create\n");
@ -579,7 +570,7 @@ void xtell_task_create(void){
ano_protocol_init(115200);
create_process(&calculate_data_id, "calculate",NULL, sensor_measure, 2000);
circle_buffer_init(&sensor_read, sensor_read_buffer, SENSOR_DATA_BUFFER_SIZE, sizeof(sensor_data_t));