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

163
apps/earphone/xtell_Sensor/ano/ano_protocol.c Normal file
View File

@ -0,0 +1,163 @@
#include "ano_protocol.h"
#include "asm/uart_dev.h"
#include "app_config.h" // 需要包含这个头文件来获取 TCFG_ONLINE_TX_PORT 等宏定义
// 定义匿名协议常量
#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 中定义
// 请确保您的 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
}

37
apps/earphone/xtell_Sensor/ano/ano_protocol.h Normal file
View File

@ -0,0 +1,37 @@
#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__

71
apps/earphone/xtell_Sensor/calculate/skiing_tracker.c
View File

@ -79,7 +79,62 @@ static void calculate_air_distance(skiing_tracker_t *tracker) {
}
/**
* @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
@ -88,9 +143,9 @@ static void calculate_air_distance(skiing_tracker_t *tracker) {
*/
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 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);
@ -189,20 +244,30 @@ void skiing_tracker_update(skiing_tracker_t *tracker, float *acc_g, float *gyr_d
#if 1 //测试禁止状态下陀螺仪的三轴加速度，去掉重力分量后是否正常
float tmp_device_acc[3];
float tmp_world_acc[3];
extern void remove_gravity_in_device_frame(const float *acc_device, const float *angle, float *acc_linear_device);
remove_gravity_in_device_frame(acc_device_ms2,angle,tmp_device_acc);
transform_acc_to_world_frame(acc_device_ms2,angle,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]),tmp_world_acc; //去掉重力加速度
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++;

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));

365
apps/earphone/xtell_Sensor/sensor/SC7U22.c
View File

@ -4,7 +4,7 @@
#include "os/os_api.h"
#include "../xtell.h"
// #define ENABLE_XLOG 1
#define ENABLE_XLOG 1
#ifdef xlog
#undef xlog
#endif
@ -270,7 +270,7 @@ void SL_SC7U22_RawData_Read(signed short * acc_data_buf,signed short * gyr_data_
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位
xlog("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]);
// xlog("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]);
}
#else
@ -666,7 +666,7 @@ float angle0[3] = {0, 0, 0}, angle_dot0[3] = {0, 0, 0}; // 姿态角的估计值
// Q_gyro: 过程噪声协方差，表示陀螺仪偏置（bias）的不确定性。
// R_angle: 测量噪声协方差，表示通过加速度计计算出的角度测量值的不确定性。值越小，表示越相信加速度计的测量结果。
// dt: 采样时间间隔（单位：秒），这里是10ms (0.01s)，对应100Hz的采样率。
//float Q_angle=0.0003, Q_gyro=0.001, R_angle=0.005, dt=0.005;//5ms ST
// float Q_angle=0.0003, Q_gyro=0.001, R_angle=0.005, dt=0.005;//5ms ST
//float Q_angle=0.00001, Q_gyro=0.00001, R_angle=0.005, dt=0.0025;//5ms ST
float Q_angle = 0.0003, Q_gyro = 0.001, R_angle = 0.005, dt = 0.01; //10ms
@ -1166,72 +1166,321 @@ unsigned char get_calibration_state(void){
#endif
///////////////////////////////////////////////////////////////////////////////////////////////////
/*
//-----------------------------------------
调用示例-by_lmx：
#if 0
// --- 新增：定义自动化校准状态 ---
typedef enum {
CAL_STATE_IDLE, // 0: 空闲，未开始校准
CAL_STATE_WAIT_STILL, // 1: 等待设备静止
CAL_STATE_COLLECTING, // 2: 正在采集数据
CAL_STATE_PAUSE_BETWEEN_FACES, // 3: 面与面之间的暂停
CAL_STATE_CALCULATING, // 4: 正在计算最终参数
CAL_STATE_FINISHED, // 5: 校准完成，参数有效
} SL_AutoCalib_State;
// --- 新增：定义校准的面 ---
typedef enum {
CAL_FACE_POS_Z = 0, // Z+
CAL_FACE_NEG_Z, // Z-
CAL_FACE_POS_Y, // Y+
CAL_FACE_NEG_Y, // Y-
CAL_FACE_POS_X, // X+
CAL_FACE_NEG_X, // X-
CAL_FACE_COUNT // 总面数
} SL_Calib_Face;
// --- 校准参数常量 ---
#define CAL_STILL_TIME_THRESHOLD 190 // 需要静止的采样周期数 (约1.9s)
#define CAL_SAMPLES_TO_COLLECT 50 // 每个面采集的样本数
#define CAL_PAUSE_SECONDS 4 // 面间暂停秒数
// --- 修改/新增 全局变量 ---
static SL_AutoCalib_State g_calib_state = CAL_STATE_IDLE;
static SL_Calib_Face g_current_calib_face = CAL_FACE_POS_Z;
static unsigned short g_still_count = 0;
static unsigned char g_samples_collected = 0;
static unsigned short g_pause_timer = 0;
static long g_calib_data_sum[6] = {0};
static signed short g_calib_avg_data[CAL_FACE_COUNT][6] = {{0}};
static float g_acc_offset[3] = {0.0f};
static float g_acc_scale[3] = {1.0f, 1.0f, 1.0f};
static signed short g_gyro_offset[3] = {0}; // 陀螺仪偏移用整数更符合原始数据类型
static unsigned char g_calibration_valid = 0;
//1.
// 定义用于存放传感器数据的缓冲区
static signed short acc_raw_data[3]; // [0]: acc_x, [1]: acc_y, [2]: acc_z
static signed short gyr_raw_data[3]; // [0]: gyr_x, [1]: gyr_y, [2]: gyr_z
static signed short combined_raw_data[6]; // 用于合并 acc 和 gyr 数据
static float final_angle_data[3]; // [0]: Pitch, [1]: Roll, [2]: Yaw
// 传感器数据处理任务
void sensor_processing_task(void *priv)
#if (ACC_RANGE == 2)
#define G_VALUE (16384.0f)
#elif (ACC_RANGE == 4)
#define G_VALUE (8192.0f)
#elif (ACC_RANGE == 8)
#define G_VALUE (4096.0f)
#elif (ACC_RANGE == 16)
#define G_VALUE (2048.0f)
#endif
/**
* @brief IMU姿态解算函数，增加了六面校准功能。
* @param calibration_cmd 校准命令，来自 SL_Calibration_Cmd 枚举。
* @param acc_gyro_input 指向6轴原始数据的指针 (AccX,Y,Z, GyroX,Y,Z)。
* @param Angle_output 指向3轴姿态角输出的指针 (Pitch, Roll, Yaw)。
* @param yaw_rst Yaw轴复位标志。
* @return unsigned char 0: 正在校准, 1: 计算成功, 2: 校准未完成，无法计算。
*/
unsigned char SIX_SL_SC7U22_Angle_Output(unsigned char auto_calib_start, signed short *acc_gyro_input, float *Angle_output, unsigned char yaw_rst)
{
while (1) {
// 4. 周期性调用读取函数，获取原始数据
SL_SC7U22_RawData_Read(acc_raw_data, gyr_raw_data);
unsigned char sl_i = 0;
// 5. 合并加速度和角速度数据到一个数组中
// SL_SC7U22_Angle_Output 函数需要一个包含6个元素的数组作为输入
memcpy(&combined_raw_data[0], acc_raw_data, sizeof(acc_raw_data));
memcpy(&combined_raw_data[3], gyr_raw_data, sizeof(gyr_raw_data));
// 启动自动化校准流程
if (auto_calib_start == 1 && g_calib_state == CAL_STATE_IDLE) {
g_calib_state = CAL_STATE_WAIT_STILL;
g_current_calib_face = CAL_FACE_POS_Z;
g_calibration_valid = 0;
xlog("\n\n===== Auto-Calibration Started =====\r\n");
xlog("Step 1/%d: Place device with Z-axis pointing UPWARD and keep it still.\r\n", CAL_FACE_COUNT);
}
// 6. 调用姿态解算函数
// 参数: (校准使能, 输入的6轴数据, 输出的角度数据, Yaw轴复位标志)
// calibration_en = 1: 让函数内部自动管理校准过程
// yaw_rst = 0: 不复位Yaw角
unsigned char status = SL_SC7U22_Angle_Output(1, combined_raw_data, final_angle_data, 0);
// 7. 检查函数返回的状态
if (status == 1) {
// 计算成功，final_angle_data 中的数据有效
xlog("Pitch: %.2f, Roll: %.2f, Yaw: %.2f\n", final_angle_data[0], final_angle_data[1], final_angle_data[2]);
} else if (status == 0) {
// 传感器正在进行静态校准，此时角度数据可能不准确
xlog("Sensor is calibrating...\n");
} else {
// status == 2, 表示校准未完成或发生错误
xlog("Angle calculation error or calibration not finished.\n");
// =================================================================================
// 步骤 1: 自动化校准状态机
// ---------------------------------------------------------------------------------
if (g_calib_state != CAL_STATE_IDLE && g_calib_state != CAL_STATE_FINISHED) {
// --- 静止检测 (通用逻辑) ---
unsigned short acc_delta = 0, gyro_delta = 0;
for (sl_i = 0; sl_i < 3; sl_i++) {
acc_delta += SL_GetAbsShort(acc_gyro_input[sl_i] - Temp_Accgyro[sl_i]);
gyro_delta += 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];
int is_still = (acc_delta < 160) && (gyro_delta < 40);
// 延时一段时间，例如10ms (对应100Hz)
os_time_dly(1);
switch (g_calib_state) {
case CAL_STATE_WAIT_STILL:
if (is_still) {
if (++g_still_count >= CAL_STILL_TIME_THRESHOLD) {
g_calib_state = CAL_STATE_COLLECTING;
g_samples_collected = 0;
g_still_count = 0;
for(sl_i = 0; sl_i < 6; sl_i++) g_calib_data_sum[sl_i] = 0;
xlog("Device is still. Collecting data...\r\n");
}
} else {
g_still_count = 0; // 如果移动则重置计数
}
break;
case CAL_STATE_COLLECTING:
if (!is_still) { // 如果在采集中移动了，则重新开始等待静止
g_calib_state = CAL_STATE_WAIT_STILL;
g_still_count = 0;
xlog("Movement detected! Please keep the device still.\r\n");
break;
}
if (g_samples_collected < CAL_SAMPLES_TO_COLLECT) {
for (sl_i = 0; sl_i < 6; sl_i++) {
g_calib_data_sum[sl_i] += acc_gyro_input[sl_i];
}
g_samples_collected++;
} else {
// 当前面采集完成
for (sl_i = 0; sl_i < 6; sl_i++) {
g_calib_avg_data[g_current_calib_face][sl_i] = g_calib_data_sum[sl_i] / CAL_SAMPLES_TO_COLLECT;
}
xlog("Face %d data collected.\r\n", g_current_calib_face + 1);
if (g_current_calib_face < CAL_FACE_NEG_X) {
g_calib_state = CAL_STATE_PAUSE_BETWEEN_FACES;
g_pause_timer = 0;
xlog("Pausing for %d seconds. Please prepare for the next orientation.\r\n", CAL_PAUSE_SECONDS);
} else {
// 所有面都已完成
g_calib_state = CAL_STATE_CALCULATING;
}
}
break;
case CAL_STATE_PAUSE_BETWEEN_FACES:
if (++g_pause_timer >= (CAL_PAUSE_SECONDS * 1000 / dt)) {
g_current_calib_face++;
g_calib_state = CAL_STATE_WAIT_STILL;
g_still_count = 0;
switch(g_current_calib_face) {
case CAL_FACE_NEG_Z: xlog("Step 2/%d: Place device with Z-axis pointing DOWNWARD and keep it still.\r\n", CAL_FACE_COUNT); break;
case CAL_FACE_POS_Y: xlog("Step 3/%d: Place device with Y-axis pointing UPWARD and keep it still.\r\n", CAL_FACE_COUNT); break;
case CAL_FACE_NEG_Y: xlog("Step 4/%d: Place device with Y-axis pointing DOWNWARD and keep it still.\r\n", CAL_FACE_COUNT); break;
case CAL_FACE_POS_X: xlog("Step 5/%d: Place device with X-axis pointing UPWARD and keep it still.\r\n", CAL_FACE_COUNT); break;
case CAL_FACE_NEG_X: xlog("Step 6/%d: Place device with X-axis pointing DOWNWARD and keep it still.\r\n", CAL_FACE_COUNT); break;
}
}
break;
case CAL_STATE_CALCULATING:
xlog("All data collected. Calculating calibration parameters...\r\n");
// 计算陀螺仪偏移
long gyro_sum[3] = {0};
for (sl_i = 0; sl_i < CAL_FACE_COUNT; sl_i++) {
gyro_sum[0] += g_calib_avg_data[sl_i][3]; // Gx
gyro_sum[1] += g_calib_avg_data[sl_i][4]; // Gy
gyro_sum[2] += g_calib_avg_data[sl_i][5]; // Gz
}
g_gyro_offset[0] = gyro_sum[0] / CAL_FACE_COUNT;
g_gyro_offset[1] = gyro_sum[1] / CAL_FACE_COUNT;
g_gyro_offset[2] = gyro_sum[2] / CAL_FACE_COUNT;
// 计算加速度计偏移和增益
g_acc_offset[0] = (g_calib_avg_data[CAL_FACE_POS_X][0] + g_calib_avg_data[CAL_FACE_NEG_X][0]) / 2.0f;
g_acc_scale[0] = (2.0f * G_VALUE) / (g_calib_avg_data[CAL_FACE_POS_X][0] - g_calib_avg_data[CAL_FACE_NEG_X][0]);
g_acc_offset[1] = (g_calib_avg_data[CAL_FACE_POS_Y][1] + g_calib_avg_data[CAL_FACE_NEG_Y][1]) / 2.0f;
g_acc_scale[1] = (2.0f * G_VALUE) / (g_calib_avg_data[CAL_FACE_POS_Y][1] - g_calib_avg_data[CAL_FACE_NEG_Y][1]);
g_acc_offset[2] = (g_calib_avg_data[CAL_FACE_POS_Z][2] + g_calib_avg_data[CAL_FACE_NEG_Z][2]) / 2.0f;
g_acc_scale[2] = (2.0f * G_VALUE) / (g_calib_avg_data[CAL_FACE_POS_Z][2] - g_calib_avg_data[CAL_FACE_NEG_Z][2]);
g_calibration_valid = 1;
g_calib_state = CAL_STATE_FINISHED;
xlog("===== Calibration Finished! =====\r\n");
xlog("Acc Offset: %.2f, %.2f, %.2f\r\n", g_acc_offset[0], g_acc_offset[1], g_acc_offset[2]);
xlog("Acc Scale: %.4f, %.4f, %.4f\r\n", g_acc_scale[0], g_acc_scale[1], g_acc_scale[2]);
xlog("Gyro Offset: %d, %d, %d\r\n", g_gyro_offset[0], g_gyro_offset[1], g_gyro_offset[2]);
break;
default:
break;
}
return 0; // 只要还在校准流程中，就返回0
}
}
// 应用程序主入口或初始化函数
void app_main()
{
// ... 其他初始化代码 ...
// 2. 调用初始化函数来配置SCU722传感器
unsigned char init_success = SL_SC7U22_Config();
if (init_success) {
xlog("SCU722 初始化成功!\n");
// 3. 创建一个任务来周期性地读取和处理数据
task_create(sensor_processing_task, NULL, "sensor_task");
} else {
xlog("SCU722 初始化失败!\n");
// =================================================================================
// 步骤 2: 姿态解算
// ---------------------------------------------------------------------------------
if (g_calibration_valid == 0) {
return 2; // 校准未完成，无法进行姿态解算
}
// ...
}
float angle_acc[3] = {0};
float gyro_val[3] = {0};
float calibrated_acc[3] = {0};
//-----------------------------------------
*/
// --- 2.1 数据预处理 (应用新的校准参数) ---
// 应用偏移和增益校准加速度计
calibrated_acc[0] = ((float)acc_gyro_input[0] - g_acc_offset[0]) * g_acc_scale[0];
calibrated_acc[1] = ((float)acc_gyro_input[1] - g_acc_offset[1]) * g_acc_scale[1];
calibrated_acc[2] = ((float)acc_gyro_input[2] - g_acc_offset[2]) * g_acc_scale[2];
// 应用偏移校准陀螺仪
gyro_val[0] = ((float)acc_gyro_input[4] - g_gyro_offset[1]) * 0.061; // GYR-Y -> Pitch
gyro_val[1] = ((float)acc_gyro_input[3] - g_gyro_offset[0]) * 0.061; // GYR-X -> Roll
gyro_val[2] = ((float)acc_gyro_input[5] - g_gyro_offset[2]) * 0.061; // GYR-Z -> Yaw
// --- 2.2 使用校准后的加速度计计算姿态角 ---
// 将校准后的加速度值转换为归一化的重力分量
angle_acc[0] = calibrated_acc[0] / G_VALUE; // ax
angle_acc[1] = calibrated_acc[1] / G_VALUE; // ay
angle_acc[2] = calibrated_acc[2] / G_VALUE; // az
// 限制范围，防止asinf/atanf计算错误
if (angle_acc[0] > 1.0f) angle_acc[0] = 1.0f;
if (angle_acc[0] < -1.0f) angle_acc[0] = -1.0f;
// ... (对 angle_acc[1] 和 angle_acc[2] 也做同样处理)
angle_acc[0] = asinf(angle_acc[0]) * 57.29578f; // Pitch
angle_acc[1] = atan2f(angle_acc[1], angle_acc[2]) * 57.29578f; // Roll (使用atan2更稳健)
// =================================================================================
// 步骤 2.4: 卡尔曼滤波
// 对Pitch和Roll分别进行滤波
// ---------------------------------------------------------------------------------
/************** Pitch 轴滤波 **************/
// --- 预测步骤 ---
// 1. 预测状态：根据上一时刻的角度和当前角速度，预测当前角度
angle0[0] += (gyro_val[0] - q_bias0[0]) * dt;
// 2. 预测协方差：更新P矩阵，表示预测状态的不确定性
Pdot0[0] = Q_angle - P0[0][1] - P0[1][0] + P0[1][1] * dt;
Pdot0[1] = -P0[1][1];
Pdot0[2] = -P0[1][1];
Pdot0[3] = Q_gyro;
P0[0][0] += Pdot0[0] * dt;
P0[0][1] += Pdot0[1] * dt;
P0[1][0] += Pdot0[2] * dt;
P0[1][1] += Pdot0[3] * dt;
// --- 更新步骤 ---
// 1. 计算卡尔曼增益 K
PCt0_0[0] = C_0 * P0[0][0];
PCt0_1[0] = C_0 * P0[1][0];
E0[0] = R_angle + C_0 * PCt0_0[0];
if (E0[0] == 0) { E0[0] = 0.0001; } // 防止除零
K0_0[0] = PCt0_0[0] / E0[0];
K0_1[0] = PCt0_1[0] / E0[0];
// 2. 计算测量余差（innovation）
angle_err0[0] = angle_acc[0] - angle0[0];
// 3. 更新状态估计：结合预测值和测量值，得到最优估计
angle0[0] += K0_0[0] * angle_err0[0];
// 4. 更新陀螺仪偏置估计
q_bias0[0] += K0_1[0] * angle_err0[0];
angle_dot0[0] = gyro_val[0] - q_bias0[0];
// 5. 更新协方差矩阵 P
t0_0[0] = PCt0_0[0];
t0_1[0] = C_0 * P0[0][1];
P0[0][0] -= K0_0[0] * t0_0[0];
P0[0][1] -= K0_0[0] * t0_1[0];
P0[1][0] -= K0_1[0] * t0_0[0];
P0[1][1] -= K0_1[0] * t0_1[0];
// 输出最终的Pitch角
Angle_output[0] = angle0[0];
/************** Roll 轴滤波 (过程同Pitch) **************/
// --- 预测步骤 ---
angle0[1] += (gyro_val[1] - q_bias0[1]) * dt;
Pdot1[0] = Q_angle - P1[0][1] - P1[1][0] + P1[1][1] * dt;
Pdot1[1] = -P1[1][1];
Pdot1[2] = -P1[1][1];
Pdot1[3] = Q_gyro;
P1[0][0] += Pdot1[0] * dt;
P1[0][1] += Pdot1[1] * dt;
P1[1][0] += Pdot1[2] * dt;
P1[1][1] += Pdot1[3] * dt;
// --- 更新步骤 ---
PCt0_0[1] = C_1 * P1[0][0];
PCt0_1[1] = C_1 * P1[1][0];
E0[1] = R_angle + C_1 * PCt0_0[1];
if (E0[1] == 0) { E0[1] = 0.0001; }
K0_0[1] = PCt0_0[1] / E0[1];
K0_1[1] = PCt0_1[1] / E0[1];
angle_err0[1] = angle_acc[1] - angle0[1];
angle0[1] += K0_0[1] * angle_err0[1];
q_bias0[1] += K0_1[1] * angle_err0[1];
angle_dot0[1] = gyro_val[1] - q_bias0[1];
t0_0[1] = PCt0_0[1];
t0_1[1] = C_1 * P1[0][1];
P1[0][0] -= K0_0[1] * t0_0[1];
P1[0][1] -= K0_0[1] * t0_1[1];
P1[1][0] -= K0_1[1] * t0_0[1];
P1[1][1] -= K0_1[1] * t0_1[1];
// 输出最终的Roll角
Angle_output[1] = angle0[1];
/************** Yaw 轴计算 **************/
// Yaw角无法通过加速度计（重力）来校正，因此这里只使用陀螺仪进行简单积分。
// 这种方法会因为陀螺仪的漂移而导致误差随时间累积。
if (yaw_rst == 1) {
Angle_output[2] = 0; // 如果有复位信号，则清零
}
// 增加一个简单的阈值，当角速度较小时，认为没有转动，以减少漂移
if (SL_GetAbsShort(Temp_Accgyro[5]) > 8) {
Angle_output[2] += gyro_val[2] * dt;
}
return 1; // 返回1，表示计算成功
}
#endif

2
apps/earphone/xtell_Sensor/sensor/SC7U22.h
View File

@ -131,7 +131,7 @@ unsigned char SL_SC7U22_Angle_Output(unsigned char calibration_en,signed short *
/**input yaw_rst: reset yaw value***************************/
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 get_calibration_state(void);
/**寄存器宏定义*******************************/
#define SC7U22_WHO_AM_I 0x01

69
apps/earphone/xtell_Sensor/sensor/SC7U22_Ma.c
View File

@ -925,72 +925,3 @@ unsigned char SL_SC7U22_Angle_Output(unsigned char calibration_en, signed short
}
#endif
/*
//-----------------------------------------
调用示例-by_lmx：
//1.
// 定义用于存放传感器数据的缓冲区
static signed short acc_raw_data[3]; // [0]: acc_x, [1]: acc_y, [2]: acc_z
static signed short gyr_raw_data[3]; // [0]: gyr_x, [1]: gyr_y, [2]: gyr_z
static signed short combined_raw_data[6]; // 用于合并 acc 和 gyr 数据
static float final_angle_data[3]; // [0]: Pitch, [1]: Roll, [2]: Yaw
// 传感器数据处理任务
void sensor_processing_task(void *priv)
{
while (1) {
// 4. 周期性调用读取函数，获取原始数据
SL_SC7U22_RawData_Read(acc_raw_data, gyr_raw_data);
// 5. 合并加速度和角速度数据到一个数组中
// SL_SC7U22_Angle_Output 函数需要一个包含6个元素的数组作为输入
memcpy(&combined_raw_data[0], acc_raw_data, sizeof(acc_raw_data));
memcpy(&combined_raw_data[3], gyr_raw_data, sizeof(gyr_raw_data));
// 6. 调用姿态解算函数
// 参数: (校准使能, 输入的6轴数据, 输出的角度数据, Yaw轴复位标志)
// calibration_en = 1: 让函数内部自动管理校准过程
// yaw_rst = 0: 不复位Yaw角
unsigned char status = SL_SC7U22_Angle_Output(1, combined_raw_data, final_angle_data, 0);
// 7. 检查函数返回的状态
if (status == 1) {
// 计算成功，final_angle_data 中的数据有效
printf("Pitch: %.2f, Roll: %.2f, Yaw: %.2f\n", final_angle_data[0], final_angle_data[1], final_angle_data[2]);
} else if (status == 0) {
// 传感器正在进行静态校准，此时角度数据可能不准确
printf("Sensor is calibrating...\n");
} else {
// status == 2, 表示校准未完成或发生错误
printf("Angle calculation error or calibration not finished.\n");
}
// 延时一段时间，例如10ms (对应100Hz)
os_time_dly(1);
}
}
// 应用程序主入口或初始化函数
void app_main()
{
// ... 其他初始化代码 ...
// 2. 调用初始化函数来配置SCU722传感器
unsigned char init_success = SL_SC7U22_Config();
if (init_success) {
printf("SCU722 初始化成功!\n");
// 3. 创建一个任务来周期性地读取和处理数据
task_create(sensor_processing_task, NULL, "sensor_task");
} else {
printf("SCU722 初始化失败!\n");
}
// ...
}
//-----------------------------------------
*/