Compare commits
4 Commits
b621ef7e44
...
054ea8644a
| Author | SHA1 | Date | |
|---|---|---|---|
| 054ea8644a | |||
| ad3ab64b72 | |||
| ebca849be3 | |||
| d0d9c0a630 |
4
.vscode/settings.json
vendored
4
.vscode/settings.json
vendored
@ -26,6 +26,8 @@
|
||||
"bt_tws.h": "c",
|
||||
"skiing_tracker.h": "c",
|
||||
"xtell.h": "c",
|
||||
"debug.h": "c"
|
||||
"debug.h": "c",
|
||||
"ano_protocol.h": "c",
|
||||
"board_jl701n_demo_global_build_cfg.h": "c"
|
||||
}
|
||||
}
|
||||
2
Makefile
2
Makefile
@ -249,6 +249,7 @@ INCLUDES := \
|
||||
-Iapps/earphone/xtell_Sensor/sensor \
|
||||
-Iapps/earphone/xtell_Sensor \
|
||||
-Iapps/earphone/xtell_Sensor/calculate \
|
||||
-Iapps/earphone/xtell_Sensor/ano \
|
||||
-I$(SYS_INC_DIR) \
|
||||
|
||||
|
||||
@ -620,6 +621,7 @@ c_SRC_FILES := \
|
||||
apps/earphone/xtell_Sensor/sensor/LIS2DH12.c \
|
||||
apps/earphone/xtell_Sensor/sensor/SC7U22.c \
|
||||
apps/earphone/xtell_Sensor/calculate/skiing_tracker.c \
|
||||
apps/earphone/xtell_Sensor/ano/ano_protocol.c \
|
||||
|
||||
|
||||
# 需要编译的 .S 文件
|
||||
|
||||
@ -57,14 +57,14 @@ const struct task_info task_info_table[] = {
|
||||
#else
|
||||
{"btstack", 3, 0, 768, 256 },
|
||||
#endif
|
||||
{"audio_dec", 5, 0, 800, 128 },
|
||||
{"aud_effect", 5, 1, 800, 128 },
|
||||
// {"audio_dec", 5, 0, 800, 128 },
|
||||
// {"aud_effect", 5, 1, 800, 128 },
|
||||
/*
|
||||
*为了防止dac buf太大，通话一开始一直解码，
|
||||
*导致编码输入数据需要很大的缓存，这里提高编码的优先级
|
||||
*/
|
||||
{"audio_enc", 6, 0, 768, 128 },
|
||||
{"aec", 2, 1, 768, 128 },
|
||||
// {"audio_enc", 6, 0, 768, 128 },
|
||||
// {"aec", 2, 1, 768, 128 },
|
||||
#if TCFG_AUDIO_HEARING_AID_ENABLE
|
||||
{"HearingAid", 6, 0, 768, 128 },
|
||||
#endif/*TCFG_AUDIO_HEARING_AID_ENABLE*/
|
||||
@ -81,14 +81,14 @@ const struct task_info task_info_table[] = {
|
||||
#if AUDIO_ENC_MPT_SELF_ENABLE
|
||||
{"enc_mpt_self", 3, 0, 512, 128 },
|
||||
#endif/*AUDIO_ENC_MPT_SELF_ENABLE*/
|
||||
{"update", 1, 0, 256, 0 },
|
||||
{"tws_ota", 2, 0, 256, 0 },
|
||||
{"tws_ota_msg", 2, 0, 256, 128 },
|
||||
{"dw_update", 2, 0, 256, 128 },
|
||||
// {"update", 1, 0, 256, 0 },
|
||||
// {"tws_ota", 2, 0, 256, 0 },
|
||||
// {"tws_ota_msg", 2, 0, 256, 128 },
|
||||
// {"dw_update", 2, 0, 256, 128 },
|
||||
{"rcsp_task", 2, 0, 640, 128 },
|
||||
{"aud_capture", 4, 0, 512, 256 },
|
||||
{"data_export", 5, 0, 512, 256 },
|
||||
{"anc", 3, 1, 512, 128 },
|
||||
// {"aud_capture", 4, 0, 512, 256 },
|
||||
// {"data_export", 5, 0, 512, 256 },
|
||||
// {"anc", 3, 1, 512, 128 },
|
||||
#endif
|
||||
|
||||
#if TCFG_GX8002_NPU_ENABLE
|
||||
@ -102,9 +102,9 @@ const struct task_info task_info_table[] = {
|
||||
#if TCFG_KWS_VOICE_RECOGNITION_ENABLE
|
||||
{"kws", 2, 0, 256, 64 },
|
||||
#endif /* #if TCFG_KWS_VOICE_RECOGNITION_ENABLE */
|
||||
{"usb_msd", 1, 0, 512, 128 },
|
||||
// {"usb_msd", 1, 0, 512, 128 },
|
||||
#if !TCFG_USB_MIC_CVP_ENABLE
|
||||
{"usbmic_write", 2, 0, 256, 128 },
|
||||
// {"usbmic_write", 2, 0, 256, 128 },
|
||||
#endif
|
||||
#if AI_APP_PROTOCOL
|
||||
{"app_proto", 2, 0, 768, 64 },
|
||||
@ -112,12 +112,12 @@ const struct task_info task_info_table[] = {
|
||||
#if (TCFG_SPI_LCD_ENABLE||TCFG_SIMPLE_LCD_ENABLE)
|
||||
{"ui", 2, 0, 768, 256 },
|
||||
#else
|
||||
{"ui", 3, 0, 384 - 64, 128 },
|
||||
// {"ui", 3, 0, 384 - 64, 128 },
|
||||
#endif
|
||||
#if (TCFG_DEV_MANAGER_ENABLE)
|
||||
{"dev_mg", 3, 0, 512, 512 },
|
||||
#endif
|
||||
{"audio_vad", 1, 1, 512, 128 },
|
||||
// {"audio_vad", 1, 1, 512, 128 },
|
||||
#if TCFG_KEY_TONE_EN
|
||||
{"key_tone", 5, 0, 256, 32 },
|
||||
#endif
|
||||
@ -137,7 +137,7 @@ const struct task_info task_info_table[] = {
|
||||
{"icsd_src", 2, 1, 512, 128 },
|
||||
#endif /*TCFG_AUDIO_ANC_ACOUSTIC_DETECTOR_EN*/
|
||||
{"pmu_task", 6, 0, 256, 128 },
|
||||
{"WindDetect", 2, 0, 256, 128 },
|
||||
// {"WindDetect", 2, 0, 256, 128 },
|
||||
{0, 0},
|
||||
};
|
||||
|
||||
|
||||
@ -249,7 +249,7 @@ const struct vad_mic_platform_data vad_mic_data = {
|
||||
.mic_ldo2PAD_en = 1,
|
||||
.mic_bias_en = 0,
|
||||
.mic_bias_res = 0,
|
||||
.mic_bias_inside = TCFG_AUDIO_MIC0_BIAS_EN,
|
||||
// .mic_bias_inside = TCFG_AUDIO_MIC0_BIAS_EN,
|
||||
/* ADC偏置电阻配置*/
|
||||
.adc_rbs = 3,
|
||||
/* ADC输入电阻配置*/
|
||||
|
||||
@ -22,9 +22,11 @@
|
||||
// UART配置 //
|
||||
//*********************************************************************************//
|
||||
#define TCFG_UART0_ENABLE ENABLE_THIS_MOUDLE //串口打印模块使能
|
||||
// #define TCFG_UART0_ENABLE 0 //串口打印模块使能
|
||||
#define TCFG_UART0_RX_PORT NO_CONFIG_PORT //串口接收脚配置（用于打印可以选择NO_CONFIG_PORT）
|
||||
#define TCFG_UART0_TX_PORT IO_PORT_DP //串口发送脚配置
|
||||
#define TCFG_UART0_BAUDRATE 1000000 //串口波特率配置
|
||||
// #define TCFG_UART0_BAUDRATE 1000000 //串口波特率配置
|
||||
#define TCFG_UART0_BAUDRATE 115200 //串口波特率配置
|
||||
|
||||
//*********************************************************************************//
|
||||
// IIC配置 //
|
||||
@ -45,7 +47,7 @@
|
||||
|
||||
*/
|
||||
#define TCFG_HW_I2C0_PORTS 'B'
|
||||
#define TCFG_HW_I2C0_CLK 4000000 //硬件IIC波特率:100k
|
||||
#define TCFG_HW_I2C0_CLK 1000000 //硬件IIC波特率:100k
|
||||
|
||||
//*********************************************************************************//
|
||||
// 硬件SPI 配置 //
|
||||
@ -104,7 +106,7 @@
|
||||
|
||||
#define MULT_KEY_ENABLE 1//DISABLE //是否使能组合按键消息, 使能后需要配置组合按键映射表
|
||||
|
||||
#define TCFG_KEY_TONE_EN ENABLE//DISABLE xtell // 按键提示音。
|
||||
#define TCFG_KEY_TONE_EN DISABLE//DISABLE xtell // 按键提示音。
|
||||
|
||||
//*********************************************************************************//
|
||||
// iokey 配置 //
|
||||
@ -727,9 +729,9 @@ DAC硬件上的连接方式,可选的配置：
|
||||
// 充电舱/蓝牙测试盒/ANC测试三者为同级关系,开启任一功能都会初始化PP0通信接口 //
|
||||
//*********************************************************************************//
|
||||
#define TCFG_CHARGESTORE_ENABLE DISABLE_THIS_MOUDLE //是否支持智能充电舱
|
||||
#define TCFG_TEST_BOX_ENABLE ENABLE_THIS_MOUDLE //是否支持蓝牙测试盒
|
||||
#define TCFG_TEST_BOX_ENABLE ENABLE_THIS_MOUDLE //是否支持蓝牙测试盒 //xtell
|
||||
#define TCFG_ANC_BOX_ENABLE CONFIG_ANC_ENABLE //是否支持ANC测试盒
|
||||
#define TCFG_UMIDIGI_BOX_ENABLE ENABLE_THIS_MOUDLE //是否支持UMIDIGI充电舱
|
||||
#define TCFG_UMIDIGI_BOX_ENABLE DISABLE_THIS_MOUDLE //是否支持UMIDIGI充电舱 //xtell
|
||||
#if TCFG_UMIDIGI_BOX_ENABLE
|
||||
#define _20MS_BIT 20 //传输20ms/Bit时使用
|
||||
#define _40MS_BIT 40 //传输40ms/Bit时使用
|
||||
|
||||
377
apps/earphone/xtell_Sensor/A_hide/11_test/skiing_tracker.c
Normal file
377
apps/earphone/xtell_Sensor/A_hide/11_test/skiing_tracker.c
Normal file
@ -0,0 +1,377 @@
|
||||
/*
|
||||
使用四元数求角度和去掉重力分量
|
||||
*/
|
||||
#include "skiing_tracker.h"
|
||||
#include "../sensor/SC7U22.h"
|
||||
#include <math.h>
|
||||
#include <string.h>
|
||||
|
||||
#define ENABLE_XLOG 1
|
||||
#ifdef xlog
|
||||
#undef xlog
|
||||
#endif
|
||||
#if ENABLE_XLOG
|
||||
#define xlog(format, ...) printf("[XT:%s] " format, __func__, ##__VA_ARGS__)
|
||||
#else
|
||||
#define xlog(format, ...) ((void)0)
|
||||
#endif
|
||||
|
||||
|
||||
#define G_ACCELERATION 9.81f
|
||||
#define DEG_TO_RAD (3.14159265f / 180.0f)
|
||||
|
||||
BLE_KS_send_data_t KS_data;
|
||||
static float quaternion_data[4];
|
||||
|
||||
#ifdef XTELL_TEST
|
||||
|
||||
debug_t debug1;
|
||||
debug_t debug2;
|
||||
#endif
|
||||
|
||||
static skiing_tracker_t my_skiing_tracker;
|
||||
//////////////////////////////////////////////////////////////////////////////////////////////////
|
||||
//实现
|
||||
|
||||
void clear_speed(void){
|
||||
my_skiing_tracker.state = STATIC;
|
||||
memset(my_skiing_tracker.velocity, 0, sizeof(my_skiing_tracker.velocity));
|
||||
my_skiing_tracker.speed = 0;
|
||||
}
|
||||
|
||||
void start_detection(void){
|
||||
my_skiing_tracker.state = STATIC;
|
||||
memset(my_skiing_tracker.velocity, 0, sizeof(my_skiing_tracker.velocity));
|
||||
my_skiing_tracker.distance = 0;
|
||||
my_skiing_tracker.speed = 0;
|
||||
}
|
||||
|
||||
void stop_detection(void){
|
||||
my_skiing_tracker.state = STOP_DETECTION;
|
||||
memset(my_skiing_tracker.velocity, 0, sizeof(my_skiing_tracker.velocity));
|
||||
my_skiing_tracker.speed = 0;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 初始化滑雪追踪器
|
||||
*
|
||||
* @param tracker
|
||||
*/
|
||||
void skiing_tracker_init(skiing_tracker_t *tracker)
|
||||
{
|
||||
if (!tracker) {
|
||||
return;
|
||||
}
|
||||
// 使用memset一次性清零整个结构体，包括新增的缓冲区
|
||||
memset(tracker, 0, sizeof(skiing_tracker_t));
|
||||
tracker->state = STATIC;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 当检测到落地时，计算空中的水平飞行距离并累加到总距离
|
||||
*/
|
||||
static void calculate_air_distance(skiing_tracker_t *tracker) {
|
||||
float horizontal_speed_on_takeoff = sqrtf(
|
||||
tracker->initial_velocity_on_takeoff[0] * tracker->initial_velocity_on_takeoff[0] +
|
||||
tracker->initial_velocity_on_takeoff[1] * tracker->initial_velocity_on_takeoff[1]
|
||||
);
|
||||
float distance_in_air = horizontal_speed_on_takeoff * tracker->time_in_air;
|
||||
tracker->distance += distance_in_air;
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* @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
|
||||
* @param angle 输入：姿态角 [pitch, roll, yaw]，单位: 度
|
||||
* @param acc_linear_device 输出：设备坐标系下移除重力后的线性加速度 [x, y, z]
|
||||
*/
|
||||
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 cp = cosf(pitch);
|
||||
float sp = sinf(pitch);
|
||||
float cr = cosf(roll);
|
||||
float sr = sinf(roll);
|
||||
float cy = cosf(yaw);
|
||||
float sy = sinf(yaw);
|
||||
|
||||
// 世界坐标系下的重力矢量
|
||||
const float g_world[3] = {0.0f, 0.0f, G_ACCELERATION};
|
||||
|
||||
// 计算旋转矩阵 R 的转置矩阵 R_transpose
|
||||
// R (Z-Y-X) =
|
||||
// [ cy*cp, cy*sp*sr - sy*cr, cy*sp*cr + sy*sr]
|
||||
// [ sy*cp, sy*sp*sr + cy*cr, sy*sp*cr - cy*sr]
|
||||
// [ -sp, cp*sr, cp*cr ]
|
||||
//
|
||||
// R_transpose =
|
||||
// [ cy*cp, sy*cp, -sp ]
|
||||
// [ cy*sp*sr - sy*cr, sy*sp*sr + cy*cr, cp*sr ]
|
||||
// [ cy*sp*cr + sy*sr, sy*sp*cr - cy*sr, cp*cr ]
|
||||
|
||||
// 计算重力在设备坐标系下的投影 G_device = R_transpose * G_world
|
||||
// 由于 G_world 只有 z 分量，计算可以简化
|
||||
float g_device[3];
|
||||
g_device[0] = (-sp) * g_world[2];
|
||||
g_device[1] = (cp * sr) * g_world[2];
|
||||
g_device[2] = (cp * cr) * g_world[2];
|
||||
|
||||
// 从原始设备加速度中减去重力投影
|
||||
acc_linear_device[0] = acc_device[0] - g_device[0];
|
||||
acc_linear_device[1] = acc_device[1] - g_device[1];
|
||||
acc_linear_device[2] = acc_device[2] - g_device[2];
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* @brief 使用四元数直接从设备坐标系的加速度中移除重力分量
|
||||
* @details 这种方法比使用欧拉角更精确、更稳定，且避免了万向节死锁。
|
||||
* @param acc_device 输入：设备坐标系下的原始加速度 [x, y, z], 单位 m/s^2
|
||||
* @param q 输入：表示姿态的四元数 [w, x, y, z]
|
||||
* @param acc_linear_device 输出：设备坐标系下移除重力后的线性加速度 [x, y, z]
|
||||
*/
|
||||
void q_remove_gravity_with_quaternion(const float *acc_device, const float *q, float *acc_linear_device)
|
||||
{
|
||||
// 从四元数计算重力在设备坐标系下的投影
|
||||
// G_device = R_transpose * G_world
|
||||
// G_world = [0, 0, g]
|
||||
// R_transpose 的第三列即为重力投影方向
|
||||
float gx = 2.0f * (q[1] * q[3] - q[0] * q[2]);
|
||||
float gy = 2.0f * (q[0] * q[1] + q[2] * q[3]);
|
||||
float gz = q[0] * q[0] - q[1] * q[1] - q[2] * q[2] + q[3] * q[3];
|
||||
|
||||
// 从原始加速度中减去重力分量
|
||||
acc_linear_device[0] = acc_device[0] - gx * G_ACCELERATION;
|
||||
acc_linear_device[1] = acc_device[1] - gy * G_ACCELERATION;
|
||||
acc_linear_device[2] = acc_device[2] - gz * G_ACCELERATION;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 使用四元数将设备坐标系的线性加速度转换到世界坐标系,并且移除重力分量
|
||||
* @details 同样，此方法比使用欧拉角更优。
|
||||
* @param acc_linear_device 输入：设备坐标系下的线性加速度 [x, y, z]
|
||||
* @param q 输入：表示姿态的四元数 [w, x, y, z]
|
||||
* @param acc_linear_world 输出：世界坐标系下的线性加速度 [x, y, z]
|
||||
*/
|
||||
void q_transform_to_world_with_quaternion(const float *acc_linear_device, const float *q, float *acc_linear_world)
|
||||
{
|
||||
// 这是 R_device_to_world * acc_linear_device 的展开形式
|
||||
acc_linear_world[0] = (1.0f - 2.0f*q[2]*q[2] - 2.0f*q[3]*q[3]) * acc_linear_device[0] +
|
||||
(2.0f*q[1]*q[2] - 2.0f*q[0]*q[3]) * acc_linear_device[1] +
|
||||
(2.0f*q[1]*q[3] + 2.0f*q[0]*q[2]) * acc_linear_device[2];
|
||||
|
||||
acc_linear_world[1] = (2.0f*q[1]*q[2] + 2.0f*q[0]*q[3]) * acc_linear_device[0] +
|
||||
(1.0f - 2.0f*q[1]*q[1] - 2.0f*q[3]*q[3]) * acc_linear_device[1] +
|
||||
(2.0f*q[2]*q[3] - 2.0f*q[0]*q[1]) * acc_linear_device[2];
|
||||
|
||||
acc_linear_world[2] = (2.0f*q[1]*q[3] - 2.0f*q[0]*q[2]) * acc_linear_device[0] +
|
||||
(2.0f*q[2]*q[3] + 2.0f*q[0]*q[1]) * acc_linear_device[1] +
|
||||
(1.0f - 2.0f*q[1]*q[1] - 2.0f*q[2]*q[2]) * acc_linear_device[2];
|
||||
acc_linear_world[2] -= G_ACCELERATION;
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* @brief 主更新函数
|
||||
*
|
||||
* @param tracker
|
||||
* @param acc_g 三轴加速度，g
|
||||
* @param gyr_dps 三轴陀螺仪，dps
|
||||
* @param angle 欧若拉角
|
||||
* @param dt 采样时间间隔，会用来积分求速度
|
||||
*/
|
||||
void skiing_tracker_update(skiing_tracker_t *tracker, float *acc_g, float *gyr_dps, float *angle, float dt)
|
||||
{
|
||||
if (!tracker || !acc_g || !gyr_dps || !angle || dt <= 0) {
|
||||
return;
|
||||
}
|
||||
if(my_skiing_tracker.state == STOP_DETECTION)
|
||||
return;
|
||||
|
||||
// --- 数据预处理和缓冲 ---
|
||||
float acc_device_ms2[3];
|
||||
acc_device_ms2[0] = acc_g[0] * G_ACCELERATION;
|
||||
acc_device_ms2[1] = acc_g[1] * G_ACCELERATION;
|
||||
acc_device_ms2[2] = acc_g[2] * G_ACCELERATION;
|
||||
|
||||
#if 1 //测试禁止状态下陀螺仪的三轴加速度，去掉重力分量后是否正常
|
||||
float tmp_device_acc[3];
|
||||
float tmp_world_acc[3];
|
||||
// remove_gravity_in_device_frame(acc_device_ms2,angle,tmp_device_acc);
|
||||
// transform_acc_to_world_frame(acc_device_ms2,angle,tmp_world_acc);
|
||||
|
||||
q_remove_gravity_with_quaternion(acc_device_ms2,quaternion_data,tmp_device_acc);
|
||||
q_transform_to_world_with_quaternion(acc_device_ms2,quaternion_data,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],all_world_mag); //去掉重力加速度
|
||||
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++;
|
||||
|
||||
#endif
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
/**
|
||||
* @brief 滑雪数据计算
|
||||
*
|
||||
* @param acc_data_buf 传入的三轴加速度数据
|
||||
* @param gyr_data_buf 传入的三轴陀螺仪数据
|
||||
* @param angle_data 传入的欧若拉角数据
|
||||
* @return BLE_send_data_t 要发送给蓝牙的数据
|
||||
*/
|
||||
BLE_send_data_t sensor_processing_task(signed short* acc_data_buf, signed short* gyr_data_buf, float* angle_data, float* quaternion) {
|
||||
|
||||
static int initialized = 0;
|
||||
static float acc_data_g[3];
|
||||
static float gyr_data_dps[3];
|
||||
if(quaternion != NULL){
|
||||
memcpy(quaternion_data, quaternion, 4 * sizeof(float));
|
||||
}
|
||||
|
||||
// const float delta_time = DELTA_TIME+0.01f;
|
||||
// const float delta_time = DELTA_TIME + 0.005f;
|
||||
const float delta_time = DELTA_TIME;
|
||||
BLE_send_data_t BLE_send_data;
|
||||
|
||||
if (!initialized) {
|
||||
skiing_tracker_init(&my_skiing_tracker);
|
||||
initialized = 1;
|
||||
printf("Skiing Tracker Initialized. Waiting for sensor calibration...\n");
|
||||
}
|
||||
|
||||
|
||||
#if ACC_RANGE==2
|
||||
// 加速度 LSB to g
|
||||
acc_data_g[0] = (float)acc_data_buf[0] / 16384.0f;
|
||||
acc_data_g[1] = (float)acc_data_buf[1] / 16384.0f;
|
||||
acc_data_g[2] = (float)acc_data_buf[2] / 16384.0f;
|
||||
#endif
|
||||
|
||||
#if ACC_RANGE==4
|
||||
// 加速度 LSB to g
|
||||
acc_data_g[0] = (float)acc_data_buf[0] / 8192.0f;
|
||||
acc_data_g[1] = (float)acc_data_buf[1] / 8192.0f;
|
||||
acc_data_g[2] = (float)acc_data_buf[2] / 8192.0f;
|
||||
#endif
|
||||
|
||||
#if ACC_RANGE==8
|
||||
//±8g 4096
|
||||
acc_data_g[0] = (float)acc_data_buf[0] / 4096.0f; //ax
|
||||
acc_data_g[1] = (float)acc_data_buf[1] / 4096.0f; //ay
|
||||
acc_data_g[2] = (float)acc_data_buf[2] / 4096.0f; //az
|
||||
#endif
|
||||
|
||||
#if ACC_RANGE==16
|
||||
//±16g 2048
|
||||
acc_data_g[0] = (float)acc_data_buf[0] / 2048.0f; //ax
|
||||
acc_data_g[1] = (float)acc_data_buf[1] / 2048.0f; //ay
|
||||
acc_data_g[2] = (float)acc_data_buf[2] / 2048.0f; //az
|
||||
#endif
|
||||
|
||||
// 陀螺仪 LSB to dps (度/秒)
|
||||
// ±2000dps量程下，转换系数约为 0.061
|
||||
gyr_data_dps[0] = (float)gyr_data_buf[0] * 0.061f;
|
||||
gyr_data_dps[1] = (float)gyr_data_buf[1] * 0.061f;
|
||||
gyr_data_dps[2] = (float)gyr_data_buf[2] * 0.061f;
|
||||
|
||||
skiing_tracker_update(&my_skiing_tracker, acc_data_g, gyr_data_dps, angle_data, delta_time);
|
||||
|
||||
// BLE_send_data.skiing_state = my_skiing_tracker.state;
|
||||
// for (int i = 0; i < 3; i++) {
|
||||
// #ifdef XTELL_TEST
|
||||
// BLE_send_data.acc_data[i] = (short)(acc_data_g[i] * 9.8f) * 100; //cm/^s2
|
||||
// BLE_send_data.gyr_data[i] = (short)gyr_data_dps[i]; //dps
|
||||
// BLE_send_data.angle_data[i] = angle_data[i];
|
||||
// #else
|
||||
// BLE_send_data.acc_data[i] = (short)acc_data_buf[i]; //原始adc数据
|
||||
// BLE_send_data.gyr_data[i] = (short)gyr_data_buf[i]; //原始adc数据
|
||||
// BLE_send_data.angle_data[i] = angle_data[i];
|
||||
// #endif
|
||||
// }
|
||||
// BLE_send_data.speed_cms = (int)(my_skiing_tracker.speed * 100);
|
||||
// BLE_send_data.distance_cm = (int)(my_skiing_tracker.distance * 100);
|
||||
// // printf("Calculate the time interval =============== end\n");
|
||||
|
||||
return BLE_send_data;
|
||||
}
|
||||
|
||||
88
apps/earphone/xtell_Sensor/A_hide/11_test/skiing_tracker.h
Normal file
88
apps/earphone/xtell_Sensor/A_hide/11_test/skiing_tracker.h
Normal file
@ -0,0 +1,88 @@
|
||||
#ifndef SKIING_TRACKER_H
|
||||
#define SKIING_TRACKER_H
|
||||
|
||||
#include "../xtell.h"
|
||||
// 定义滑雪者可能的状态
|
||||
typedef enum {
|
||||
STATIC, // 静止或动态稳定：0
|
||||
NO_CONSTANT_SPEED, // 正在滑雪，非匀速：1
|
||||
CONSTANT_SPEED, // 正在滑雪，匀速：2
|
||||
WOBBLE, // 正在原地旋转：3
|
||||
WHEEL, // 转弯：4
|
||||
FALLEN, // 已摔倒：5
|
||||
TAKING_OFF, // 起跳冲击阶段：6
|
||||
IN_AIR, // 空中失重阶段：7
|
||||
LANDING, // 落地冲击阶段：8
|
||||
STOP_DETECTION, // 停止检测：9
|
||||
UNKNOWN // 未知状态：10
|
||||
} skiing_state_t;
|
||||
|
||||
#define VARIANCE_BUFFER_SIZE 5 // 用于计算方差的数据窗口大小 (5个样本 @ 100Hz = 50ms)，减小延迟，提高实时性
|
||||
#define DELTA_TIME 0.01f
|
||||
|
||||
|
||||
// 追踪器数据结构体
|
||||
typedef struct {
|
||||
// 公开数据
|
||||
float velocity[3]; // 当前速度 (x, y, z)，单位: m/s
|
||||
float distance; // 总滑行距离，单位: m
|
||||
float speed; // 当前速率 (标量)，单位: m/s
|
||||
skiing_state_t state; // 当前滑雪状态
|
||||
|
||||
// 内部计算使用的私有成员
|
||||
float acc_world[3]; // 在世界坐标系下的加速度
|
||||
|
||||
// 用于空中距离计算
|
||||
float time_in_air; // 滞空时间计时器
|
||||
float initial_velocity_on_takeoff[3]; // 起跳瞬间的速度向量
|
||||
int airborne_entry_counter; // 进入空中状态的确认计数器
|
||||
int grounded_entry_counter; // 落地确认计数器
|
||||
|
||||
// --- 内部计算使用的私有成员 ---
|
||||
// 用于动态零速更新和旋转检测的缓冲区
|
||||
float acc_buffer[VARIANCE_BUFFER_SIZE][3]; // 加速度数据窗口
|
||||
float gyr_buffer[VARIANCE_BUFFER_SIZE][3]; // 角速度数据窗口
|
||||
int buffer_index; // 缓冲区当前索引
|
||||
int buffer_filled; // 缓冲区是否已填满的标志
|
||||
|
||||
// 用于高通滤波器（巴特沃斯一阶滤波器）的私有成员，以消除加速度的直流偏置
|
||||
float acc_world_filtered[3]; //过滤过的
|
||||
float acc_world_unfiltered_prev[3]; //上一次没过滤的
|
||||
|
||||
float acc_world_lpf[3]; // 经过低通滤波后的世界坐标系加速度
|
||||
} skiing_tracker_t;
|
||||
|
||||
//ble发送的数据
|
||||
typedef struct{ //__attribute__((packed)){ //该结构体取消内存对齐
|
||||
char sensor_state;
|
||||
char skiing_state;
|
||||
int speed_cms; //求出的速度，cm/s
|
||||
int distance_cm; //求出的距离，cm
|
||||
short acc_data[3]; //三轴加速度, g
|
||||
short gyr_data[3]; //三轴陀螺仪, dps
|
||||
float angle_data[3]; //欧若拉角
|
||||
}BLE_send_data_t;
|
||||
|
||||
typedef struct{
|
||||
int acc_KS[3]; //卡尔曼后，LSB转换后的 三轴加速度数据（cm/s^2）
|
||||
int gyr_KS_dps[3]; //卡尔曼后，LSB to dps 三轴陀螺仪数据
|
||||
int angle_KS[3]; //卡尔曼后，计算得到的欧若拉角数据
|
||||
}BLE_KS_send_data_t;
|
||||
|
||||
#ifdef XTELL_TEST
|
||||
typedef struct{
|
||||
float acc_variance; //三轴加速度方差之和
|
||||
float gyr_variance; //三轴陀螺仪方差之和
|
||||
float acc_magnitude; //三轴加速度模长
|
||||
float gyr_magnitude; //三轴陀螺仪模长
|
||||
}debug_t;
|
||||
#endif
|
||||
/**
|
||||
* @brief 初始化滑雪追踪器
|
||||
*
|
||||
* @param tracker 指向 skiing_tracker_t 结构体的指针
|
||||
*/
|
||||
void skiing_tracker_init(skiing_tracker_t *tracker);
|
||||
|
||||
BLE_send_data_t sensor_processing_task(signed short* acc_data_buf, signed short* gyr_data_buf, float* angle_data, float* quaternion);
|
||||
#endif // SKIING_TRACKER_H
|
||||
@ -32,6 +32,7 @@ sensor_processing_task当中就进行了计算，包括卡尔曼等，在timer
|
||||
|
||||
|
||||
# 11.13
|
||||
|
||||
代码主要文件夹：apps\earphone\xtell_Sensor
|
||||
|
||||
- apps\earphone\xtell_Sensor\send_data.c，‘ xtell_task_create ’ 函数，传感器计算程序逻辑开始位置，包括传感器读取数据的任务、蓝牙发送任务、速度距离计算任务
|
||||
@ -53,3 +54,42 @@ sensor_processing_task当中就进行了计算，包括卡尔曼等，在timer
|
||||
- 要使用只需要把代码复制粘贴到calculate文件夹下
|
||||
|
||||
|
||||
|
||||
# 11.18
|
||||
|
||||
去除重力分量后仍有误差：
|
||||
|
||||
- 数据对齐？
|
||||
- 有没有丢数据？
|
||||
- 重复定位的数据？
|
||||
- 静态时的角度误差？
|
||||
|
||||
|
||||
|
||||
定时器1的回调函数（10ms调用一次）**A**：读取传感器数据，放进buff
|
||||
|
||||
定时器2的回调函数（5ms调用一次）**B**：读取buff的传感器数据，去除重力分离的计算
|
||||
|
||||
- **数据没有对齐**，A 的回调调用计数 > B 的回调调用计数
|
||||
- **丢数据了**，A 读取传感器数据的回调函数中，打印了buff已满的log
|
||||
- **重复定位**：移动后回到原先的位置，前后的计算得到的三轴角度相同
|
||||
- **静态时的角度误差**：1°左右
|
||||
- 定时器2不进行重力分离计算，只进行计数，也仍然有数据没有对齐和丢数据的情况
|
||||
|
||||
|
||||
|
||||
将读取传感器数据、去除重力分量计算放到同一个任务下，同步进行
|
||||
|
||||
- 数据没有丢失，数据也对齐了
|
||||
- 在小倾斜的坡面下，去除重力分量后的总的加速度，**小于0.1m/s^2**
|
||||
- 在大倾斜的坡面下（如旋转超过70°），去除重力分量后的总的加速度，在**0.4m/s^2上下**
|
||||
- 貌似是角度越大，越接近方向锁，导致角度更容易漂移造错数据错误
|
||||
|
||||
采用四元数的方式做去除重力分量的计算：
|
||||
|
||||
- 将读取传感器数据、去除重力分量计算放到同一个任务下
|
||||
|
||||
- 在小倾斜的坡面下，去除重力分量后的总的加速度，低于**0.04m/s^2**
|
||||
- 在大倾斜的坡面下（如旋转超过70°），去除重力分量后的总的加速度，在**0.1m/s^2上下**
|
||||
|
||||
- 大倾斜角度的误差要靠磁力计来消除（yaw无法通过加速度计来消除偏差）
|
||||
|
||||
163
apps/earphone/xtell_Sensor/ano/ano_protocol.c
Normal file
163
apps/earphone/xtell_Sensor/ano/ano_protocol.c
Normal 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
37
apps/earphone/xtell_Sensor/ano/ano_protocol.h
Normal 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__
|
||||
@ -6,6 +6,9 @@
|
||||
#include <math.h>
|
||||
#include <string.h>
|
||||
|
||||
#define G_ACCELERATION 9.81f
|
||||
#define DEG_TO_RAD (3.14159265f / 180.0f)
|
||||
|
||||
#define ENABLE_XLOG 1
|
||||
#ifdef xlog
|
||||
#undef xlog
|
||||
@ -16,12 +19,83 @@
|
||||
#define xlog(format, ...) ((void)0)
|
||||
#endif
|
||||
|
||||
// --- 静止检测 ---
|
||||
//两个判断是否静止的必要条件：动态零速更新(ZUPT)阈值
|
||||
// 加速方差阈值，提高阈值，让“刹车”更灵敏，以便在波浪式前进等慢速漂移时也能触发零速更新
|
||||
#define STOP_ACC_VARIANCE_THRESHOLD 0.2f
|
||||
// 陀螺仪方差阈值
|
||||
#define STOP_GYR_VARIANCE_THRESHOLD 5.0f
|
||||
// 静止时候的陀螺仪模长
|
||||
#define STOP_GYR_MAG_THRESHOLD 15
|
||||
// --- --- ---
|
||||
|
||||
#define G_ACCELERATION 9.81f
|
||||
#define DEG_TO_RAD (3.14159265f / 180.0f)
|
||||
// --- 启动滑雪阈值 ---
|
||||
// 加速度模长与重力的差值大于此值，认为开始运动；降低阈值，让“油门”更灵敏，以便能捕捉到真实的慢速启动
|
||||
#define START_ACC_MAG_THRESHOLD 1.0f //0.5、1
|
||||
// 陀螺仪方差阈值，以允许启动瞬间的正常抖动，但仍能过滤掉混乱的、非滑雪的晃动。
|
||||
#define START_GYR_VARIANCE_THRESHOLD 15.0f
|
||||
// --- --- ---
|
||||
|
||||
// --- 滑雪过程 ---
|
||||
//加速度 模长(不含重力），低于此值视为 在做匀速运动
|
||||
#define SKIING_ACC_MAG_THRESHOLD 0.5f
|
||||
//陀螺仪 模长，高于此值视为 摔倒了
|
||||
#define FALLEN_GRY_MAG_THRESHOLD 2000.0f //未确定
|
||||
// --- --- ---
|
||||
|
||||
// --- 原地旋转抖动 ---
|
||||
// 加速度 方差 阈值。此值比 静止检测 阈值更宽松，
|
||||
#define WOBBLE_ACC_VARIANCE_THRESHOLD 0.5f
|
||||
// 加速度 模长 阈值
|
||||
#define WOBBLE_ACC_MAG_THRESHOLD 1.0f
|
||||
// 角速度 总模长 大于此值(度/秒)，认为正在进行非滑雪的旋转或摆动
|
||||
#define ROTATION_GYR_MAG_THRESHOLD 30.0f
|
||||
// --- --- ---
|
||||
|
||||
// --- 滑雪转弯动 ---
|
||||
// 加速度 方差 阈值，大于此值，滑雪过程可能发生了急转弯
|
||||
#define WHEEL_ACC_VARIANCE_THRESHOLD 7.0f
|
||||
// 角速度 总模长 大于此值(度/秒)，认为滑雪过程中进行急转弯
|
||||
#define WHEEL_GYR_MAG_THRESHOLD 500.0f //
|
||||
// --- --- ---
|
||||
|
||||
// --- 跳跃 ---
|
||||
// 加速度模长低于此值(g)，认为进入失重状态(IN_AIR)
|
||||
#define AIRBORNE_ACC_MAG_LOW_THRESHOLD 0.4f
|
||||
// 加速度模长高于此值(g)，认为发生落地冲击(LANDING)
|
||||
#define LANDING_ACC_MAG_HIGH_THRESHOLD 3.5f
|
||||
// 起跳加速度阈值(g)，用于进入TAKING_OFF状态
|
||||
#define TAKEOFF_ACC_MAG_HIGH_THRESHOLD 1.8f
|
||||
// 进入空中状态确认计数：需要连续3个采样点加速度低于阈值才判断为起跳
|
||||
#define AIRBORNE_CONFIRM_COUNT 3
|
||||
// 落地状态确认计数：加速度恢复到1g附近并持续2个采样点(20ms)则认为已落地
|
||||
#define GROUNDED_CONFIRM_COUNT 2
|
||||
// 最大滞空时间(秒)，超过此时间强制认为已落地，防止状态锁死
|
||||
#define MAX_TIME_IN_AIR 12.5f
|
||||
// --- --- ---
|
||||
|
||||
// --- 用于消除积分漂移的滤波器和阈值 ---
|
||||
// 高通滤波器系数 (alpha)。alpha 越接近1，滤除低频(直流偏移)的效果越强，但可能滤掉真实的慢速运动。
|
||||
// alpha = RC / (RC + dt)，参考RC电路而来，fc ≈ (1 - alpha) / (2 * π * dt)
|
||||
#define HPF_ALPHA 0.999f
|
||||
//0.995f： 0.08 Hz 的信号
|
||||
//0.999f： 0.0159 Hz
|
||||
// --- --- ---
|
||||
|
||||
// --- 低通滤波器 ---
|
||||
// 低通滤波器系数 (alpha)。alpha 越小，滤波效果越强（更平滑），但延迟越大。
|
||||
// alpha 推荐范围 0.7 ~ 0.95。可以从 0.85 开始尝试。
|
||||
#define LPF_ALPHA 0.7f
|
||||
|
||||
// 加速度死区阈值 (m/s^2)。低于此阈值的加速度被认为是噪声，不参与积分。
|
||||
// 设得太高会忽略真实的慢速启动，设得太低则无法有效抑制噪声。
|
||||
//参考：0.2f ~ 0.4f
|
||||
#define ACC_DEAD_ZONE_THRESHOLD 0.05f
|
||||
|
||||
// --- 模拟摩擦力，进行速度衰减 ---
|
||||
#define SPEED_ATTENUATION 1.0f //暂不模拟
|
||||
BLE_KS_send_data_t KS_data;
|
||||
|
||||
static float quaternion_data[4];
|
||||
#ifdef XTELL_TEST
|
||||
|
||||
debug_t debug1;
|
||||
@ -81,51 +155,54 @@ static void calculate_air_distance(skiing_tracker_t *tracker) {
|
||||
|
||||
|
||||
/**
|
||||
* @brief 在设备坐标系下，从原始加速度数据中移除重力分量
|
||||
* @brief 使用四元数直接从设备坐标系的加速度中移除重力分量
|
||||
* @details 这种方法比使用欧拉角更精确、更稳定，且避免了万向节死锁。
|
||||
* @param acc_device 输入：设备坐标系下的原始加速度 [x, y, z], 单位 m/s^2
|
||||
* @param angle 输入：姿态角 [pitch, roll, yaw]，单位: 度
|
||||
* @param q 输入：表示姿态的四元数 [w, x, y, z]
|
||||
* @param acc_linear_device 输出：设备坐标系下移除重力后的线性加速度 [x, y, z]
|
||||
*/
|
||||
void remove_gravity_in_device_frame(const float *acc_device, const float *angle, float *acc_linear_device)
|
||||
void q_remove_gravity_with_quaternion(const float *acc_device, const float *q, 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 轴
|
||||
// 从四元数计算重力在设备坐标系下的投影
|
||||
// G_device = R_transpose * G_world
|
||||
// G_world = [0, 0, g]
|
||||
// R_transpose 的第三列即为重力投影方向
|
||||
float gx = 2.0f * (q[1] * q[3] - q[0] * q[2]);
|
||||
float gy = 2.0f * (q[0] * q[1] + q[2] * q[3]);
|
||||
float gz = q[0] * q[0] - q[1] * q[1] - q[2] * q[2] + q[3] * q[3];
|
||||
|
||||
float cp = cosf(pitch);
|
||||
float sp = sinf(pitch);
|
||||
float cr = cosf(roll);
|
||||
float sr = sinf(roll);
|
||||
float cy = cosf(yaw);
|
||||
float sy = sinf(yaw);
|
||||
|
||||
// 世界坐标系下的重力矢量
|
||||
const float g_world[3] = {0.0f, 0.0f, G_ACCELERATION};
|
||||
|
||||
// 计算旋转矩阵 R 的转置矩阵 R_transpose
|
||||
// R (Z-Y-X) =
|
||||
// [ cy*cp, cy*sp*sr - sy*cr, cy*sp*cr + sy*sr]
|
||||
// [ sy*cp, sy*sp*sr + cy*cr, sy*sp*cr - cy*sr]
|
||||
// [ -sp, cp*sr, cp*cr ]
|
||||
//
|
||||
// R_transpose =
|
||||
// [ cy*cp, sy*cp, -sp ]
|
||||
// [ cy*sp*sr - sy*cr, sy*sp*sr + cy*cr, cp*sr ]
|
||||
// [ cy*sp*cr + sy*sr, sy*sp*cr - cy*sr, cp*cr ]
|
||||
|
||||
// 计算重力在设备坐标系下的投影 G_device = R_transpose * G_world
|
||||
// 由于 G_world 只有 z 分量，计算可以简化
|
||||
float g_device[3];
|
||||
g_device[0] = (-sp) * g_world[2];
|
||||
g_device[1] = (cp * sr) * g_world[2];
|
||||
g_device[2] = (cp * cr) * g_world[2];
|
||||
|
||||
// 从原始设备加速度中减去重力投影
|
||||
acc_linear_device[0] = acc_device[0] - g_device[0];
|
||||
acc_linear_device[1] = acc_device[1] - g_device[1];
|
||||
acc_linear_device[2] = acc_device[2] - g_device[2];
|
||||
// 从原始加速度中减去重力分量
|
||||
acc_linear_device[0] = acc_device[0] - gx * G_ACCELERATION;
|
||||
acc_linear_device[1] = acc_device[1] - gy * G_ACCELERATION;
|
||||
acc_linear_device[2] = acc_device[2] - gz * G_ACCELERATION;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 使用四元数将设备坐标系的线性加速度转换到世界坐标系
|
||||
* @details 同样，此方法比使用欧拉角更优。
|
||||
* @param acc_linear_device 输入：设备坐标系下的线性加速度 [x, y, z]
|
||||
* @param q 输入：表示姿态的四元数 [w, x, y, z]
|
||||
* @param acc_linear_world 输出：世界坐标系下的线性加速度 [x, y, z]
|
||||
*/
|
||||
void q_transform_to_world_with_quaternion(const float *acc_linear_device, const float *q, float *acc_linear_world)
|
||||
{
|
||||
// 这是 R_device_to_world * acc_linear_device 的展开形式
|
||||
acc_linear_world[0] = (1.0f - 2.0f*q[2]*q[2] - 2.0f*q[3]*q[3]) * acc_linear_device[0] +
|
||||
(2.0f*q[1]*q[2] - 2.0f*q[0]*q[3]) * acc_linear_device[1] +
|
||||
(2.0f*q[1]*q[3] + 2.0f*q[0]*q[2]) * acc_linear_device[2];
|
||||
|
||||
acc_linear_world[1] = (2.0f*q[1]*q[2] + 2.0f*q[0]*q[3]) * acc_linear_device[0] +
|
||||
(1.0f - 2.0f*q[1]*q[1] - 2.0f*q[3]*q[3]) * acc_linear_device[1] +
|
||||
(2.0f*q[2]*q[3] - 2.0f*q[0]*q[1]) * acc_linear_device[2];
|
||||
|
||||
acc_linear_world[2] = (2.0f*q[1]*q[3] - 2.0f*q[0]*q[2]) * acc_linear_device[0] +
|
||||
(2.0f*q[2]*q[3] + 2.0f*q[0]*q[1]) * acc_linear_device[1] +
|
||||
(1.0f - 2.0f*q[1]*q[1] - 2.0f*q[2]*q[2]) * acc_linear_device[2];
|
||||
// acc_linear_world[2] -= G_ACCELERATION;
|
||||
}
|
||||
|
||||
|
||||
|
||||
/**
|
||||
* @brief 计算缓冲区内三轴数据的方差之和
|
||||
*
|
||||
@ -161,7 +238,170 @@ static float calculate_variance(float buffer[VARIANCE_BUFFER_SIZE][3])
|
||||
return variance[0] + variance[1] + variance[2];
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 摩擦力模拟，进行速度衰减
|
||||
*
|
||||
* @param tracker
|
||||
*/
|
||||
void forece_of_friction(skiing_tracker_t *tracker){
|
||||
// 增加速度衰减，模拟摩擦力
|
||||
tracker->velocity[0] *= SPEED_ATTENUATION;
|
||||
tracker->velocity[1] *= SPEED_ATTENUATION;
|
||||
tracker->velocity[2] = 0; // 垂直速度强制归零
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 状态机更新
|
||||
*
|
||||
* @param tracker 传入同步修改后传出
|
||||
* @param acc_device_ms2 三轴加速度，m/s^2
|
||||
* @param gyr_dps 三轴陀螺仪，dps
|
||||
*/
|
||||
static void update_state_machine(skiing_tracker_t *tracker, const float *acc_device_ms2, const float *gyr_dps)
|
||||
{
|
||||
// 缓冲区未填满时，不进行状态判断，默认为静止
|
||||
if (!tracker->buffer_filled) {
|
||||
tracker->state = STATIC;
|
||||
return;
|
||||
}
|
||||
|
||||
// --- 计算关键指标 ---
|
||||
float acc_variance = calculate_variance(tracker->acc_buffer); // 计算加速度方差
|
||||
float gyr_variance = calculate_variance(tracker->gyr_buffer); // 计算陀螺仪方差
|
||||
float gyr_magnitude = sqrtf(gyr_dps[0]*gyr_dps[0] + gyr_dps[1]*gyr_dps[1] + gyr_dps[2]*gyr_dps[2]); //dps
|
||||
float acc_magnitude = sqrtf(acc_device_ms2[0]*acc_device_ms2[0] + acc_device_ms2[1]*acc_device_ms2[1] + acc_device_ms2[2]*acc_device_ms2[2]); //m/s^s
|
||||
float acc_magnitude_g = acc_magnitude / G_ACCELERATION; // 转换为g单位，用于跳跃判断
|
||||
|
||||
#ifdef XTELL_TEST
|
||||
debug1.acc_variance =acc_variance;
|
||||
debug1.gyr_variance =gyr_variance;
|
||||
debug1.gyr_magnitude=gyr_magnitude;
|
||||
debug1.acc_magnitude=fabsf(acc_magnitude - G_ACCELERATION);
|
||||
#endif
|
||||
|
||||
|
||||
|
||||
// --- 状态机逻辑 (核心修改区域) ---
|
||||
|
||||
#if 0 //暂时不考虑空中
|
||||
// 1. 空中/落地状态的后续处理
|
||||
if (tracker->state == IN_AIR) {
|
||||
// A. 检测巨大冲击 -> 落地
|
||||
if (acc_magnitude_g > LANDING_ACC_MAG_HIGH_THRESHOLD) {
|
||||
tracker->state = LANDING;
|
||||
// B. 检测超时 -> 强制落地 (安全机制)
|
||||
} else if (tracker->time_in_air > MAX_TIME_IN_AIR) {
|
||||
tracker->state = LANDING;
|
||||
// C. 检测恢复正常重力 (平缓落地)
|
||||
} else if (acc_magnitude_g > 0.8f && acc_magnitude_g < 1.5f) {
|
||||
tracker->grounded_entry_counter++;
|
||||
if (tracker->grounded_entry_counter >= GROUNDED_CONFIRM_COUNT) {
|
||||
tracker->state = LANDING;
|
||||
}
|
||||
} else {
|
||||
tracker->grounded_entry_counter = 0;
|
||||
}
|
||||
return; // 在空中或刚切换到落地，结束本次状态判断
|
||||
}
|
||||
|
||||
// 2. 严格的 "起跳->空中" 状态转换逻辑
|
||||
// 只有当处于滑行状态时，才去检测起跳意图
|
||||
if (tracker->state == NO_CONSTANT_SPEED || tracker->state == CONSTANT_SPEED || tracker->state == WHEEL) {
|
||||
if (acc_magnitude_g > TAKEOFF_ACC_MAG_HIGH_THRESHOLD) {
|
||||
tracker->state = TAKING_OFF;
|
||||
tracker->airborne_entry_counter = 0; // 准备检测失重
|
||||
return;
|
||||
}
|
||||
}
|
||||
// 只有在TAKING_OFF状态下，才去检测是否进入失重
|
||||
if (tracker->state == TAKING_OFF) {
|
||||
if (acc_magnitude_g < AIRBORNE_ACC_MAG_LOW_THRESHOLD) {
|
||||
tracker->airborne_entry_counter++;
|
||||
if (tracker->airborne_entry_counter >= AIRBORNE_CONFIRM_COUNT) {
|
||||
memcpy(tracker->initial_velocity_on_takeoff, tracker->velocity, sizeof(tracker->velocity));
|
||||
tracker->time_in_air = 0;
|
||||
tracker->state = IN_AIR;
|
||||
tracker->airborne_entry_counter = 0;
|
||||
tracker->grounded_entry_counter = 0;
|
||||
return;
|
||||
}
|
||||
} else {
|
||||
// 如果在起跳冲击后一段时间内没有失重，说明只是一个颠簸，恢复滑行
|
||||
// 可以加一个小的超时计数器，这里为了简单先直接恢复
|
||||
tracker->state = NO_CONSTANT_SPEED;
|
||||
}
|
||||
return; // 无论是否切换，都结束本次判断
|
||||
}
|
||||
#endif
|
||||
|
||||
|
||||
// --- 静止判断 ---
|
||||
if (acc_variance < STOP_ACC_VARIANCE_THRESHOLD && gyr_variance < STOP_GYR_VARIANCE_THRESHOLD && gyr_magnitude < STOP_GYR_MAG_THRESHOLD) {
|
||||
tracker->state = STATIC;
|
||||
return;
|
||||
}
|
||||
|
||||
// --- 地面状态切换逻辑 ---
|
||||
switch (tracker->state) {
|
||||
case LANDING:
|
||||
tracker->state = STATIC;
|
||||
break;
|
||||
case STATIC:
|
||||
// 优先判断是否进入 WOBBLE 状态
|
||||
// 条件：陀螺仪活动剧烈，但整体加速度变化不大（说明是原地转或晃）
|
||||
if (gyr_magnitude > ROTATION_GYR_MAG_THRESHOLD && fabsf(acc_magnitude - G_ACCELERATION) < WOBBLE_ACC_MAG_THRESHOLD) {
|
||||
tracker->state = WOBBLE;
|
||||
}
|
||||
// 只有在陀螺仪和加速度都满足“前进”特征时，才启动
|
||||
else if (gyr_variance > START_GYR_VARIANCE_THRESHOLD && fabsf(acc_magnitude - G_ACCELERATION) > START_ACC_MAG_THRESHOLD) {
|
||||
tracker->state = NO_CONSTANT_SPEED;
|
||||
}
|
||||
break;
|
||||
|
||||
case WOBBLE:
|
||||
// 从 WOBBLE 状态启动的条件应该和从 STATIC 启动一样严格
|
||||
if (gyr_variance < START_GYR_VARIANCE_THRESHOLD * 2 && fabsf(acc_magnitude - G_ACCELERATION) > START_ACC_MAG_THRESHOLD) {
|
||||
tracker->state = NO_CONSTANT_SPEED;
|
||||
}
|
||||
// 如果陀螺仪活动减弱，则可能恢复静止
|
||||
else if (gyr_magnitude < ROTATION_GYR_MAG_THRESHOLD * 0.8f) { // 增加迟滞，避免抖动
|
||||
// 不直接跳回STATIC，而是依赖下一轮的全局静止判断
|
||||
}
|
||||
break;
|
||||
case NO_CONSTANT_SPEED: //非匀速状态
|
||||
//暂时不考虑摔倒
|
||||
// if (gyr_magnitude > FALLEN_GRY_MAG_THRESHOLD) {
|
||||
// tracker->state = FALLEN; //摔倒
|
||||
// } else
|
||||
if (gyr_magnitude > WHEEL_GYR_MAG_THRESHOLD && acc_variance > WHEEL_ACC_VARIANCE_THRESHOLD) {
|
||||
tracker->state = WHEEL; //转弯
|
||||
} else if (fabsf(acc_magnitude - G_ACCELERATION) < SKIING_ACC_MAG_THRESHOLD) {
|
||||
tracker->state = CONSTANT_SPEED; //匀速
|
||||
}
|
||||
break;
|
||||
|
||||
case CONSTANT_SPEED: //匀速状态
|
||||
if (fabsf(acc_magnitude - G_ACCELERATION) > START_ACC_MAG_THRESHOLD) {
|
||||
tracker->state = NO_CONSTANT_SPEED;
|
||||
}
|
||||
//TODO：可以添加进入转弯或摔倒的判断
|
||||
break;
|
||||
|
||||
case WHEEL:
|
||||
// 从转弯状态，检查转弯是否结束
|
||||
// 如果角速度和加速度方差都降下来了，就回到普通滑行状态
|
||||
if (gyr_magnitude < WHEEL_GYR_MAG_THRESHOLD * 0.8f && acc_variance < WHEEL_ACC_VARIANCE_THRESHOLD * 0.8f) { // 乘以一个滞后系数避免抖动
|
||||
tracker->state = NO_CONSTANT_SPEED;
|
||||
}
|
||||
break;
|
||||
|
||||
case FALLEN:
|
||||
// TODO：回到 STATIC
|
||||
break;
|
||||
}
|
||||
|
||||
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
@ -187,27 +427,139 @@ void skiing_tracker_update(skiing_tracker_t *tracker, float *acc_g, float *gyr_d
|
||||
acc_device_ms2[1] = acc_g[1] * G_ACCELERATION;
|
||||
acc_device_ms2[2] = acc_g[2] * G_ACCELERATION;
|
||||
|
||||
#if 1 //测试禁止状态下陀螺仪的三轴加速度，去掉重力分量后是否正常
|
||||
float tmp_device_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);
|
||||
// 将最新数据存入缓冲区
|
||||
memcpy(tracker->acc_buffer[tracker->buffer_index], acc_device_ms2, sizeof(acc_device_ms2));
|
||||
memcpy(tracker->gyr_buffer[tracker->buffer_index], gyr_dps, 3 * sizeof(float));
|
||||
|
||||
tracker->buffer_index++;
|
||||
if (tracker->buffer_index >= VARIANCE_BUFFER_SIZE) {
|
||||
tracker->buffer_index = 0;
|
||||
tracker->buffer_filled = 1; // 标记缓冲区已满
|
||||
}
|
||||
|
||||
// 计算处理后加速度的水平模长
|
||||
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]);
|
||||
// --- 更新状态机 ---
|
||||
update_state_machine(tracker, acc_device_ms2, gyr_dps);
|
||||
|
||||
// --- 根据状态执行不同的计算逻辑 ---
|
||||
switch (tracker->state) {
|
||||
case TAKING_OFF:
|
||||
tracker->speed = 0.0f;
|
||||
break;
|
||||
case IN_AIR:
|
||||
// 在空中时，只累加滞空时间
|
||||
tracker->time_in_air += dt;
|
||||
break;
|
||||
case LANDING:
|
||||
// 刚落地，计算空中距离
|
||||
calculate_air_distance(tracker);
|
||||
// 清理速度和滤波器状态，为恢复地面追踪做准备
|
||||
memset(tracker->velocity, 0, sizeof(tracker->velocity));
|
||||
tracker->speed = 0;
|
||||
memset(tracker->acc_world_unfiltered_prev, 0, sizeof(tracker->acc_world_unfiltered_prev));
|
||||
memset(tracker->acc_world_filtered, 0, sizeof(tracker->acc_world_filtered));
|
||||
memset(tracker->acc_world_lpf, 0, sizeof(tracker->acc_world_lpf)); // 清理新增的LPF状态
|
||||
break;
|
||||
case WHEEL:
|
||||
case NO_CONSTANT_SPEED:
|
||||
float linear_acc_device[3];
|
||||
float linear_acc_world[3];
|
||||
// 在设备坐标系下，移除重力，得到线性加速度
|
||||
q_remove_gravity_with_quaternion(acc_device_ms2, quaternion_data, linear_acc_device);
|
||||
|
||||
// 将设备坐标系下的线性加速度，旋转到世界坐标系
|
||||
q_transform_to_world_with_quaternion(linear_acc_device, quaternion_data, linear_acc_world);
|
||||
// 将最终用于积分的加速度存入 tracker 结构体
|
||||
memcpy(tracker->acc_no_g, linear_acc_world, sizeof(linear_acc_world));
|
||||
|
||||
float acc_world_temp[3]; // 临时变量存储当前周期的加速度
|
||||
for (int i = 0; i < 2; i++) { // 只处理水平方向的 x 和 y 轴
|
||||
|
||||
// --- 核心修改：颠倒滤波器顺序为 HPF -> LPF ---
|
||||
|
||||
// 1. 高通滤波 (HPF) 先行: 消除因姿态误差导致的重力泄漏（直流偏置）
|
||||
// HPF的瞬态响应会产生尖峰，这是正常的。
|
||||
tracker->acc_world_filtered[i] = HPF_ALPHA * (tracker->acc_world_filtered[i] + tracker->acc_no_g[i] - tracker->acc_world_unfiltered_prev[i]);
|
||||
tracker->acc_world_unfiltered_prev[i] = tracker->acc_no_g[i];
|
||||
|
||||
// 2. 低通滤波 (LPF) 殿后: 平滑掉HPF产生的尖峰和传感器自身的高频振动噪声。
|
||||
// 这里使用 tracker->acc_world_filtered[i] 作为LPF的输入。
|
||||
tracker->acc_world_lpf[i] = (1.0f - LPF_ALPHA) * tracker->acc_world_filtered[i] + LPF_ALPHA * tracker->acc_world_lpf[i];
|
||||
|
||||
// 将最终处理完的加速度值存入临时变量
|
||||
acc_world_temp[i] = tracker->acc_world_lpf[i];
|
||||
}
|
||||
|
||||
// 计算处理后加速度的水平模长
|
||||
float acc_horizontal_mag = sqrtf(acc_world_temp[0] * acc_world_temp[0] +
|
||||
acc_world_temp[1] * acc_world_temp[1]);
|
||||
#if XTELL_TEST
|
||||
debug2.acc_magnitude = acc_horizontal_mag;
|
||||
#endif
|
||||
// 应用死区，并积分
|
||||
if (acc_horizontal_mag > ACC_DEAD_ZONE_THRESHOLD) {
|
||||
tracker->velocity[0] += acc_world_temp[0] * dt;
|
||||
tracker->velocity[1] += acc_world_temp[1] * dt;
|
||||
}
|
||||
|
||||
// 更新速度和距离
|
||||
tracker->speed = sqrtf(tracker->velocity[0] * tracker->velocity[0] +
|
||||
tracker->velocity[1] * tracker->velocity[1]);
|
||||
tracker->distance += tracker->speed * dt;
|
||||
break;
|
||||
case CONSTANT_SPEED:
|
||||
//保持上次的速度不变。只更新距离
|
||||
tracker->distance += tracker->speed * dt;
|
||||
break;
|
||||
case STATIC:
|
||||
case WOBBLE:
|
||||
// 速度清零，抑制漂移
|
||||
memset(tracker->velocity, 0, sizeof(tracker->velocity));
|
||||
tracker->speed = 0.0f;
|
||||
memset(tracker->acc_world_unfiltered_prev, 0, sizeof(tracker->acc_world_unfiltered_prev));
|
||||
memset(tracker->acc_world_filtered, 0, sizeof(tracker->acc_world_filtered));
|
||||
memset(tracker->acc_world_lpf, 0, sizeof(tracker->acc_world_lpf)); // 清理新增的LPF状态
|
||||
#if XTELL_TEST
|
||||
debug2.acc_magnitude = 0;
|
||||
#endif
|
||||
break;
|
||||
case FALLEN:
|
||||
// TODO
|
||||
break;
|
||||
default:
|
||||
break;
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
#if 1
|
||||
float linear_acc_device[3];
|
||||
float linear_acc_world[3];
|
||||
float tmp_world_acc[3];
|
||||
// 在设备坐标系下，移除重力，得到线性加速度
|
||||
q_remove_gravity_with_quaternion(acc_device_ms2, quaternion_data, linear_acc_device);
|
||||
|
||||
// 将设备坐标系下的线性加速度，旋转到世界坐标系
|
||||
q_transform_to_world_with_quaternion(linear_acc_device, quaternion_data, tmp_world_acc);
|
||||
|
||||
|
||||
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],all_world_mag); //去掉重力加速度
|
||||
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++;
|
||||
|
||||
#endif
|
||||
|
||||
|
||||
}
|
||||
|
||||
|
||||
@ -220,12 +572,16 @@ void skiing_tracker_update(skiing_tracker_t *tracker, float *acc_g, float *gyr_d
|
||||
* @param angle_data 传入的欧若拉角数据
|
||||
* @return BLE_send_data_t 要发送给蓝牙的数据
|
||||
*/
|
||||
BLE_send_data_t sensor_processing_task(signed short* acc_data_buf, signed short* gyr_data_buf, float* angle_data) {
|
||||
BLE_send_data_t sensor_processing_task(signed short* acc_data_buf, signed short* gyr_data_buf, float* angle_data, float* quaternion) {
|
||||
|
||||
static int initialized = 0;
|
||||
static float acc_data_g[3];
|
||||
static float gyr_data_dps[3];
|
||||
|
||||
if(quaternion != NULL){
|
||||
memcpy(quaternion_data, quaternion, 4 * sizeof(float));
|
||||
}
|
||||
|
||||
// const float delta_time = DELTA_TIME+0.01f;
|
||||
// const float delta_time = DELTA_TIME + 0.005f;
|
||||
const float delta_time = DELTA_TIME;
|
||||
@ -274,21 +630,21 @@ BLE_send_data_t sensor_processing_task(signed short* acc_data_buf, signed short*
|
||||
|
||||
skiing_tracker_update(&my_skiing_tracker, acc_data_g, gyr_data_dps, angle_data, delta_time);
|
||||
|
||||
// BLE_send_data.skiing_state = my_skiing_tracker.state;
|
||||
// for (int i = 0; i < 3; i++) {
|
||||
// #ifdef XTELL_TEST
|
||||
// BLE_send_data.acc_data[i] = (short)(acc_data_g[i] * 9.8f) * 100; //cm/^s2
|
||||
// BLE_send_data.gyr_data[i] = (short)gyr_data_dps[i]; //dps
|
||||
// BLE_send_data.angle_data[i] = angle_data[i];
|
||||
// #else
|
||||
// BLE_send_data.acc_data[i] = (short)acc_data_buf[i]; //原始adc数据
|
||||
// BLE_send_data.gyr_data[i] = (short)gyr_data_buf[i]; //原始adc数据
|
||||
// BLE_send_data.angle_data[i] = angle_data[i];
|
||||
// #endif
|
||||
// }
|
||||
// BLE_send_data.speed_cms = (int)(my_skiing_tracker.speed * 100);
|
||||
// BLE_send_data.distance_cm = (int)(my_skiing_tracker.distance * 100);
|
||||
// // printf("Calculate the time interval =============== end\n");
|
||||
BLE_send_data.skiing_state = my_skiing_tracker.state;
|
||||
for (int i = 0; i < 3; i++) {
|
||||
#ifdef XTELL_TEST
|
||||
BLE_send_data.acc_data[i] = (short)(acc_data_g[i] * 9.8f) * 100; //cm/^s2
|
||||
BLE_send_data.gyr_data[i] = (short)gyr_data_dps[i]; //dps
|
||||
BLE_send_data.angle_data[i] = angle_data[i];
|
||||
#else
|
||||
BLE_send_data.acc_data[i] = (short)acc_data_buf[i]; //原始adc数据
|
||||
BLE_send_data.gyr_data[i] = (short)gyr_data_buf[i]; //原始adc数据
|
||||
BLE_send_data.angle_data[i] = angle_data[i];
|
||||
#endif
|
||||
}
|
||||
BLE_send_data.speed_cms = (int)(my_skiing_tracker.speed * 100);
|
||||
BLE_send_data.distance_cm = (int)(my_skiing_tracker.distance * 100);
|
||||
// printf("Calculate the time interval =============== end\n");
|
||||
|
||||
return BLE_send_data;
|
||||
}
|
||||
|
||||
@ -30,7 +30,7 @@ typedef struct {
|
||||
skiing_state_t state; // 当前滑雪状态
|
||||
|
||||
// 内部计算使用的私有成员
|
||||
float acc_world[3]; // 在世界坐标系下的加速度
|
||||
float acc_no_g[3]; // 去掉重力分量后的加速度
|
||||
|
||||
// 用于空中距离计算
|
||||
float time_in_air; // 滞空时间计时器
|
||||
@ -84,5 +84,5 @@ typedef struct{
|
||||
*/
|
||||
void skiing_tracker_init(skiing_tracker_t *tracker);
|
||||
|
||||
BLE_send_data_t sensor_processing_task(signed short* acc_data_buf, signed short* gyr_data_buf, float* angle_data) ;
|
||||
BLE_send_data_t sensor_processing_task(signed short* acc_data_buf, signed short* gyr_data_buf, float* angle_data, float* quaternion);
|
||||
#endif // SKIING_TRACKER_H
|
||||
@ -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;
|
||||
@ -244,19 +173,23 @@ 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];
|
||||
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读到的数据
|
||||
|
||||
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;
|
||||
static int count_test1 = 0;
|
||||
static int count_test2 = 0;
|
||||
/**
|
||||
* @brief //读取传感器的数据放进缓冲区
|
||||
*
|
||||
@ -281,15 +214,20 @@ void sensor_read_data(){
|
||||
|
||||
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 = 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++;
|
||||
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]; // 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;
|
||||
@ -298,10 +236,21 @@ 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 = 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));
|
||||
circle_buffer_write(&sensor_read, &tmp);
|
||||
if(mutex1 == 0){
|
||||
mutex1 = 1;
|
||||
// count_test1++;
|
||||
// xlog("count_test_1: %d\n",count_test1);
|
||||
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 +265,28 @@ void calculate_data(){
|
||||
return;
|
||||
}
|
||||
|
||||
circle_buffer_read(&sensor_read, &tmp);
|
||||
BLE_send_data_t data_by_calculate = sensor_processing_task(tmp.acc_data, tmp.gyr_data,tmp.angle);
|
||||
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,tmp.quaternion_output);
|
||||
|
||||
|
||||
|
||||
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 +304,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 +333,7 @@ void BLE_send_data(){
|
||||
// send_data_to_ble_client(&log_buffer,strlen(log_buffer));
|
||||
|
||||
memset(&log_buffer, 0, 100);
|
||||
#if 1
|
||||
// 使用 snprintf 进行格式化
|
||||
num_chars_written = snprintf(
|
||||
log_buffer, // 目标缓冲区
|
||||
@ -398,7 +371,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 +458,62 @@ 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(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);
|
||||
}
|
||||
|
||||
// xlog("=======sensor_read_data END\n");
|
||||
}
|
||||
|
||||
|
||||
void xtell_task_create(void){
|
||||
|
||||
// int ret = hw_iic_init(0);
|
||||
@ -507,7 +536,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 +547,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));
|
||||
@ -528,11 +560,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, 4);
|
||||
|
||||
create_process(&calculate_data_id, "calculate",NULL, calculate_data, 6);
|
||||
|
||||
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;
|
||||
|
||||
@ -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
|
||||
@ -129,6 +129,8 @@ unsigned char SL_SC7U22_Config(void)
|
||||
SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x40, 0xBC);//ACC_CON 高性能模式，1600Hz -- lmx
|
||||
// SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x40, 0xBB);//ACC_CON 高性能模式，800Hz -- lmx
|
||||
|
||||
SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x40, 0xA8);//ACC_CON 高性能模式，100Hz，平均数4 -- lmx
|
||||
|
||||
#if ACC_RANGE==2
|
||||
SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x41, 0x00);//ACC_RANGE 00：±2G
|
||||
#endif
|
||||
@ -144,8 +146,9 @@ unsigned char SL_SC7U22_Config(void)
|
||||
|
||||
// SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x42, 0x86);//GYR_CONF 0x87=50Hz 0x86=25Hz
|
||||
// SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x42, 0x8C);//GYR_CONF 1600Hz -- lmx
|
||||
SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x42, 0xAC);//GYR_CONF 1600Hz -- lmx
|
||||
// SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x42, 0xAC);//GYR_CONF 1600Hz -- lmx
|
||||
// SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x42, 0xAB);//GYR_CONF 800Hz -- lmx
|
||||
SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x42, 0xE8);//GYR_CONF 100Hz, 噪声优化开启,4个平均一次 -- lmx
|
||||
|
||||
SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x43, 0x00);//GYR_RANGE 2000dps
|
||||
SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x43, 0x00);//GYR_RANGE 2000dps
|
||||
@ -270,7 +273,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 +669,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
|
||||
|
||||
@ -997,6 +1000,10 @@ unsigned char SL_SC7U22_Angle_Output(unsigned char calibration_en, signed short
|
||||
|
||||
return 2; // 校准未完成，返回错误状态
|
||||
}
|
||||
|
||||
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
|
||||
//
|
||||
//
|
||||
/**
|
||||
* @brief 姿态角解算函数 (基于一阶互补滤波)
|
||||
* @details
|
||||
@ -1164,74 +1171,544 @@ unsigned char get_calibration_state(void){
|
||||
}
|
||||
|
||||
|
||||
#endif
|
||||
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////
|
||||
// 如果没有定义 PI，请取消下面的注释
|
||||
// #define PI 3.14159265358979323846f
|
||||
|
||||
// =================================================================================================
|
||||
// Mahony AHRS (Attitude and Heading Reference System) 相关变量定义
|
||||
// Mahony滤波器是一种高效的互补滤波器，它使用四元数来表示姿态，从而避免了万向节死锁问题。
|
||||
// 它通过一个PI控制器来校正陀ar螺仪的积分漂移。
|
||||
// -------------------------------------------------------------------------------------------------
|
||||
// --- 滤波器参数 ---
|
||||
// Kp: 比例增益，决定了加速度计数据校正陀螺仪的权重。值越大，对加速度计的响应越快，但对运动加速度更敏感。
|
||||
// Ki: 积分增益，决定了用于校正陀螺仪静态漂移的权重。
|
||||
// Q_dt: 采样时间间隔（单位：秒），这里是10ms (0.01s)，对应100Hz的采样率。
|
||||
const float Kp = 2.0f;
|
||||
const float Ki = 0.005f;
|
||||
const float Q_dt = 0.01f;
|
||||
|
||||
// --- 状态变量 ---
|
||||
// 四元数 (Quaternion)，表示当前的姿态。初始化为 (1, 0, 0, 0)，代表初始姿态为水平。
|
||||
static float q0 = 1.0f, q1 = 0.0f, q2 = 0.0f, q3 = 0.0f;
|
||||
// 陀螺仪积分误差项，用于补偿静态漂移
|
||||
static float exInt = 0.0f, eyInt = 0.0f, ezInt = 0.0f;
|
||||
// =================================================================================================
|
||||
|
||||
|
||||
/*
|
||||
//-----------------------------------------
|
||||
调用示例-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)
|
||||
/**
|
||||
* @brief 姿态角解算函数 (基于四元数和Mahony滤波器)
|
||||
* @details
|
||||
* 该函数主要完成两项工作：
|
||||
* 1. 静态校准：在初始阶段，检测传感器是否处于静止状态。如果是，则计算加速度计和陀螺仪的零点偏移（误差），用于后续的数据补偿。
|
||||
* 2. 姿态解算：使用基于四元数的Mahony互补滤波器融合经过校准后的加速度计和陀螺仪数据，计算出物体的俯仰角（Pitch）、横滚角（Roll）和偏航角（Yaw）。
|
||||
* 这种方法精度高，且能避免万向节死锁问题。
|
||||
*
|
||||
* @param calibration_en 传入：外部校准使能标志。如果为0，则强制认为已经校准完成。
|
||||
* @param acc_gyro_input 传入和传出：包含6轴原始数据的数组指针，顺序为 [ACC_X, ACC_Y, ACC_Z, GYR_X, GYR_Y, GYR_Z]。该函数会对其进行原地修改，填充为校准后的数据。
|
||||
* @param Angle_output 传出：滤波后的结果，顺序为 [Pitch, Roll, Yaw]，单位为度。
|
||||
* @param yaw_rst 传入：Yaw轴重置标志。如果为1，则将整个姿态滤波器状态重置。
|
||||
*
|
||||
* @return
|
||||
* - 0: 正在进行静态校准。
|
||||
* - 1: 姿态角计算成功。
|
||||
* - 2: 校准未完成，无法进行计算。
|
||||
*/
|
||||
unsigned char Q_SL_SC7U22_Angle_Output(unsigned char calibration_en, signed short *acc_gyro_input, float *Angle_output, unsigned char yaw_rst, float *quaternion_output)
|
||||
{
|
||||
while (1) {
|
||||
// 4. 周期性调用读取函数，获取原始数据
|
||||
SL_SC7U22_RawData_Read(acc_raw_data, gyr_raw_data);
|
||||
unsigned char sl_i = 0;
|
||||
|
||||
// 如果外部强制禁用校准，则将标志位置1
|
||||
if (calibration_en == 0) {
|
||||
SL_SC7U22_Error_Flag = 1;
|
||||
}
|
||||
|
||||
// 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 中的数据有效
|
||||
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");
|
||||
// =================================================================================
|
||||
// 步骤 1: 静态校准 (此部分逻辑与原代码完全相同)
|
||||
// ---------------------------------------------------------------------------------
|
||||
if (SL_SC7U22_Error_Flag == 0) {
|
||||
unsigned short acc_gyro_delta[2];
|
||||
acc_gyro_delta[0] = 0;
|
||||
acc_gyro_delta[1] = 0;
|
||||
for (sl_i = 0; sl_i < 3; sl_i++) {
|
||||
acc_gyro_delta[0] += SL_GetAbsShort(acc_gyro_input[sl_i] - Temp_Accgyro[sl_i]);
|
||||
acc_gyro_delta[1] += 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];
|
||||
}
|
||||
#if (ACC_RANGE == 2)
|
||||
if ((acc_gyro_delta[0] / 8 < 160) && (acc_gyro_delta[1] < 40) && (SL_GetAbsShort(acc_gyro_input[0]) < 3000) && (SL_GetAbsShort(acc_gyro_input[1]) < 3000) && (SL_GetAbsShort(acc_gyro_input[2] - 16384) < 3000)) {
|
||||
#elif (ACC_RANGE == 4)
|
||||
if ((acc_gyro_delta[0] / 8 < 160) && (acc_gyro_delta[1] < 40) && (SL_GetAbsShort(acc_gyro_input[0]) < 3000) && (SL_GetAbsShort(acc_gyro_input[1]) < 3000) && (SL_GetAbsShort(acc_gyro_input[2] - 8192) < 3000)) {
|
||||
#elif (ACC_RANGE == 8)
|
||||
if ((acc_gyro_delta[0] / 8 < 160) && (acc_gyro_delta[1] < 40) && (SL_GetAbsShort(acc_gyro_input[0]) < 3000) && (SL_GetAbsShort(acc_gyro_input[1]) < 3000) && (SL_GetAbsShort(acc_gyro_input[2] - 4096) < 3000)) {
|
||||
#elif (ACC_RANGE == 16)
|
||||
if ((acc_gyro_delta[0] / 8 < 160) && (acc_gyro_delta[1] < 40) && (SL_GetAbsShort(acc_gyro_input[0]) < 3000) && (SL_GetAbsShort(acc_gyro_input[1]) < 3000) && (SL_GetAbsShort(acc_gyro_input[2] - 2048) < 3000)) {
|
||||
#endif
|
||||
if (SL_SC7U22_Error_cnt < 200) SL_SC7U22_Error_cnt++;
|
||||
} else {
|
||||
// status == 2, 表示校准未完成或发生错误
|
||||
xlog("Angle calculation error or calibration not finished.\n");
|
||||
SL_SC7U22_Error_cnt = 0;
|
||||
}
|
||||
if (SL_SC7U22_Error_cnt > 190) {
|
||||
for (sl_i = 0; sl_i < 6; sl_i++) Sum_Avg_Accgyro[sl_i] += acc_gyro_input[sl_i];
|
||||
SL_SC7U22_Error_cnt2++;
|
||||
if (SL_SC7U22_Error_cnt2 > 49) {
|
||||
SL_SC7U22_Error_Flag = 1;
|
||||
SL_SC7U22_Error_cnt2 = 0;
|
||||
SL_SC7U22_Error_cnt = 0;
|
||||
for (sl_i = 0; sl_i < 6; sl_i++) Sum_Avg_Accgyro[sl_i] = Sum_Avg_Accgyro[sl_i] / 50;
|
||||
Error_Accgyro[0] = 0 - Sum_Avg_Accgyro[0];
|
||||
Error_Accgyro[1] = 0 - Sum_Avg_Accgyro[1];
|
||||
#if ACC_RANGE==2
|
||||
Error_Accgyro[2] = 16384 - Sum_Avg_Accgyro[2];
|
||||
#elif ACC_RANGE==4
|
||||
Error_Accgyro[2] = 8192 - Sum_Avg_Accgyro[2];
|
||||
#elif ACC_RANGE==8
|
||||
Error_Accgyro[2] = 4096 - Sum_Avg_Accgyro[2];
|
||||
#elif ACC_RANGE==16
|
||||
Error_Accgyro[2] = 2048 - Sum_Avg_Accgyro[2];
|
||||
#endif
|
||||
Error_Accgyro[3] = 0 - Sum_Avg_Accgyro[3];
|
||||
Error_Accgyro[4] = 0 - Sum_Avg_Accgyro[4];
|
||||
Error_Accgyro[5] = 0 - Sum_Avg_Accgyro[5];
|
||||
// xlog("AVG_Recode AX:%d,AY:%d,AZ:%d,GX:%d,GY:%d,GZ:%d\r\n", Sum_Avg_Accgyro[0], Sum_Avg_Accgyro[1], Sum_Avg_Accgyro[2], Sum_Avg_Accgyro[3], Sum_Avg_Accgyro[4], Sum_Avg_Accgyro[5]);
|
||||
// xlog("Error_Recode AX:%d,AY:%d,AZ:%d,GX:%d,GY:%d,GZ:%d\r\n", Error_Accgyro[0], Error_Accgyro[1], Error_Accgyro[2], Error_Accgyro[3], Error_Accgyro[4], Error_Accgyro[5]);
|
||||
}
|
||||
} else {
|
||||
SL_SC7U22_Error_cnt2 = 0;
|
||||
for (sl_i = 0; sl_i < 6; sl_i++) Sum_Avg_Accgyro[sl_i] = 0;
|
||||
}
|
||||
return 0; // 返回0，表示正在校准
|
||||
}
|
||||
|
||||
// =================================================================================
|
||||
// 步骤 2: 姿态解算 (Mahony AHRS)
|
||||
// ---------------------------------------------------------------------------------
|
||||
if (SL_SC7U22_Error_Flag == 1) { // 确认已经校准完成
|
||||
|
||||
// --- 2.1 Yaw轴/姿态重置 ---
|
||||
// 注意：重置yaw会重置整个姿态滤波器，使设备回到初始水平姿态
|
||||
if (yaw_rst == 1) {
|
||||
q0 = 1.0f; q1 = 0.0f; q2 = 0.0f; q3 = 0.0f;
|
||||
exInt = 0.0f; eyInt = 0.0f; ezInt = 0.0f;
|
||||
}
|
||||
|
||||
// 延时一段时间，例如10ms (对应100Hz)
|
||||
os_time_dly(1);
|
||||
// --- 2.2 数据预处理 ---
|
||||
// 应用零点偏移补偿
|
||||
for (sl_i = 0; sl_i < 6; sl_i++) {
|
||||
Temp_Accgyro[sl_i] = acc_gyro_input[sl_i] + Error_Accgyro[sl_i];
|
||||
}
|
||||
|
||||
// 将校准后的数据写回输入数组 (可选)
|
||||
#if 1
|
||||
for (sl_i = 0; sl_i < 6; sl_i++) {
|
||||
acc_gyro_input[sl_i] = Temp_Accgyro[sl_i];
|
||||
}
|
||||
#endif
|
||||
|
||||
// 获取校准后的数据
|
||||
float ax = (float)Temp_Accgyro[0];
|
||||
float ay = (float)Temp_Accgyro[1];
|
||||
float az = (float)Temp_Accgyro[2];
|
||||
// 将陀螺仪数据从 LSB 转换为弧度/秒 (rad/s)
|
||||
// 转换系数 0.061 ≈ 2000dps / 32768 LSB; PI/180 ≈ 0.01745
|
||||
float gx = (float)Temp_Accgyro[3] * 0.061f * 0.0174533f; // Roll rate
|
||||
float gy = (float)Temp_Accgyro[4] * 0.061f * 0.0174533f; // Pitch rate
|
||||
float gz = (float)Temp_Accgyro[5] * 0.061f * 0.0174533f; // Yaw rate
|
||||
|
||||
// --- 2.3 Mahony算法核心 ---
|
||||
float norm;
|
||||
float vx, vy, vz; // 估计的重力向量
|
||||
float ex, ey, ez; // 加速度计测量和估计的重力向量之间的误差
|
||||
|
||||
// 归一化加速度计测量值，得到单位重力向量
|
||||
norm = sqrtf(ax * ax + ay * ay + az * az);
|
||||
if (norm > 0.0f) { // 防止除以零
|
||||
ax = ax / norm;
|
||||
ay = ay / norm;
|
||||
az = az / norm;
|
||||
|
||||
// 根据当前姿态(四元数)估计重力向量的方向
|
||||
vx = 2.0f * (q1 * q3 - q0 * q2);
|
||||
vy = 2.0f * (q0 * q1 + q2 * q3);
|
||||
vz = q0 * q0 - q1 * q1 - q2 * q2 + q3 * q3;
|
||||
|
||||
// 计算测量值与估计值之间的误差（叉积）
|
||||
ex = (ay * vz - az * vy);
|
||||
ey = (az * vx - ax * vz);
|
||||
ez = (ax * vy - ay * vx);
|
||||
|
||||
// 积分误差项，用于消除陀螺仪的静态漂移
|
||||
exInt = exInt + ex * Ki * Q_dt;
|
||||
eyInt = eyInt + ey * Ki * Q_dt;
|
||||
ezInt = ezInt + ez * Ki * Q_dt;
|
||||
|
||||
// 使用PI控制器校正陀螺仪的测量值
|
||||
gx = gx + Kp * ex + exInt;
|
||||
gy = gy + Kp * ey + eyInt;
|
||||
gz = gz + Kp * ez + ezInt;
|
||||
}
|
||||
|
||||
// 使用校正后的角速度更新四元数 (一阶毕卡法)
|
||||
q0 = q0 + (-q1 * gx - q2 * gy - q3 * gz) * 0.5f * Q_dt;
|
||||
q1 = q1 + (q0 * gx + q2 * gz - q3 * gy) * 0.5f * Q_dt;
|
||||
q2 = q2 + (q0 * gy - q1 * gz + q3 * gx) * 0.5f * Q_dt;
|
||||
q3 = q3 + (q0 * gz + q1 * gy - q2 * gx) * 0.5f * Q_dt;
|
||||
|
||||
// 归一化四元数，保持其单位长度
|
||||
norm = sqrtf(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
|
||||
q0 = q0 / norm;
|
||||
q1 = q1 / norm;
|
||||
q2 = q2 / norm;
|
||||
q3 = q3 / norm;
|
||||
|
||||
// --- 2.4 将四元数转换为欧拉角 (Pitch, Roll, Yaw) ---
|
||||
// 转换公式将四元数转换为 ZYX 顺序的欧拉角
|
||||
// 结果单位为弧度，乘以 57.29578f 转换为度
|
||||
|
||||
// Pitch (绕Y轴旋转)
|
||||
Angle_output[0] = asinf(-2.0f * (q1 * q3 - q0 * q2)) * 57.29578f;
|
||||
|
||||
// Roll (绕X轴旋转)
|
||||
Angle_output[1] = atan2f(2.0f * (q0 * q1 + q2 * q3), q0 * q0 - q1 * q1 - q2 * q2 + q3 * q3) * 57.29578f;
|
||||
|
||||
// Yaw (绕Z轴旋转) - 注意：无磁力计的6轴IMU，Yaw角会随时间漂移
|
||||
Angle_output[2] = atan2f(2.0f * (q1 * q2 + q0 * q3), q0 * q0 + q1 * q1 - q2 * q2 - q3 * q3) * 57.29578f;
|
||||
|
||||
|
||||
if (quaternion_output != NULL) {
|
||||
quaternion_output[0] = q0; // w
|
||||
quaternion_output[1] = q1; // x
|
||||
quaternion_output[2] = q2; // y
|
||||
quaternion_output[3] = q3; // z
|
||||
}
|
||||
|
||||
return 1; // 返回1，表示计算成功
|
||||
}
|
||||
|
||||
return 2; // 校准未完成，返回错误状态
|
||||
}
|
||||
|
||||
// 应用程序主入口或初始化函数
|
||||
void app_main()
|
||||
#endif
|
||||
|
||||
///////////////////////////////////////////////////////////////////////////////////////////////////
|
||||
|
||||
#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;
|
||||
|
||||
|
||||
|
||||
#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)
|
||||
{
|
||||
// ... 其他初始化代码 ...
|
||||
unsigned char sl_i = 0;
|
||||
|
||||
// 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");
|
||||
// 启动自动化校准流程
|
||||
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);
|
||||
}
|
||||
|
||||
// ...
|
||||
}
|
||||
// =================================================================================
|
||||
// 步骤 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);
|
||||
|
||||
//-----------------------------------------
|
||||
*/
|
||||
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
|
||||
}
|
||||
|
||||
|
||||
// =================================================================================
|
||||
// 步骤 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
|
||||
@ -131,7 +131,8 @@ 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 Q_SL_SC7U22_Angle_Output(unsigned char calibration_en, signed short *acc_gyro_input, float *Angle_output, unsigned char yaw_rst, float *quaternion_output);
|
||||
unsigned char get_calibration_state(void);
|
||||
/**寄存器宏定义*******************************/
|
||||
#define SC7U22_WHO_AM_I 0x01
|
||||
|
||||
@ -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");
|
||||
}
|
||||
|
||||
// ...
|
||||
}
|
||||
|
||||
//-----------------------------------------
|
||||
*/
|
||||
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
File diff suppressed because one or more lines are too long
@ -61,42 +61,6 @@ objs/apps/common/audio/audio_plc.c.o
|
||||
objs/apps/common/audio/audio_utils.c.o
|
||||
-r=objs/apps/common/audio/audio_utils.c.o,digital_phase_inverter_s16,pl
|
||||
objs/apps/common/audio/decode/audio_key_tone.c.o
|
||||
-r=objs/apps/common/audio/decode/audio_key_tone.c.o,audio_key_tone_destroy,pl
|
||||
-r=objs/apps/common/audio/decode/audio_key_tone.c.o,os_taskq_post_msg,l
|
||||
-r=objs/apps/common/audio/decode/audio_key_tone.c.o,os_time_dly,l
|
||||
-r=objs/apps/common/audio/decode/audio_key_tone.c.o,task_kill,l
|
||||
-r=objs/apps/common/audio/decode/audio_key_tone.c.o,local_irq_disable,l
|
||||
-r=objs/apps/common/audio/decode/audio_key_tone.c.o,free,l
|
||||
-r=objs/apps/common/audio/decode/audio_key_tone.c.o,local_irq_enable,l
|
||||
-r=objs/apps/common/audio/decode/audio_key_tone.c.o,audio_key_tone_init,pl
|
||||
-r=objs/apps/common/audio/decode/audio_key_tone.c.o,zalloc,l
|
||||
-r=objs/apps/common/audio/decode/audio_key_tone.c.o,task_create,l
|
||||
-r=objs/apps/common/audio/decode/audio_key_tone.c.o,audio_key_tone_play_sin,pl
|
||||
-r=objs/apps/common/audio/decode/audio_key_tone.c.o,audio_key_tone_play_name,pl
|
||||
-r=objs/apps/common/audio/decode/audio_key_tone.c.o,audio_key_tone_play_index,pl
|
||||
-r=objs/apps/common/audio/decode/audio_key_tone.c.o,audio_key_tone_play,pl
|
||||
-r=objs/apps/common/audio/decode/audio_key_tone.c.o,audio_key_tone_is_play,pl
|
||||
-r=objs/apps/common/audio/decode/audio_key_tone.c.o,audio_key_tone_digvol_set,pl
|
||||
-r=objs/apps/common/audio/decode/audio_key_tone.c.o,os_taskq_pend,l
|
||||
-r=objs/apps/common/audio/decode/audio_key_tone.c.o,audio_dec_sine_app_create_by_parm,l
|
||||
-r=objs/apps/common/audio/decode/audio_key_tone.c.o,audio_dec_app_start,l
|
||||
-r=objs/apps/common/audio/decode/audio_key_tone.c.o,fopen,l
|
||||
-r=objs/apps/common/audio/decode/audio_key_tone.c.o,fget_name,l
|
||||
-r=objs/apps/common/audio/decode/audio_key_tone.c.o,fclose,l
|
||||
-r=objs/apps/common/audio/decode/audio_key_tone.c.o,ASCII_StrCmpNoCase,l
|
||||
-r=objs/apps/common/audio/decode/audio_key_tone.c.o,audio_dec_sine_app_create,l
|
||||
-r=objs/apps/common/audio/decode/audio_key_tone.c.o,audio_dec_file_app_create,l
|
||||
-r=objs/apps/common/audio/decode/audio_key_tone.c.o,audio_dec_app_set_frame_info,l
|
||||
-r=objs/apps/common/audio/decode/audio_key_tone.c.o,audio_dec_sine_app_probe,l
|
||||
-r=objs/apps/common/audio/decode/audio_key_tone.c.o,audio_mixer_get_ch_num,l
|
||||
-r=objs/apps/common/audio/decode/audio_key_tone.c.o,app_audio_state_switch,l
|
||||
-r=objs/apps/common/audio/decode/audio_key_tone.c.o,get_tone_vol,l
|
||||
-r=objs/apps/common/audio/decode/audio_key_tone.c.o,audio_dec_file_app_close,l
|
||||
-r=objs/apps/common/audio/decode/audio_key_tone.c.o,audio_dec_sine_app_close,l
|
||||
-r=objs/apps/common/audio/decode/audio_key_tone.c.o,app_audio_state_exit,l
|
||||
-r=objs/apps/common/audio/decode/audio_key_tone.c.o,os_taskq_post_type,l
|
||||
-r=objs/apps/common/audio/decode/audio_key_tone.c.o,strlen,l
|
||||
-r=objs/apps/common/audio/decode/audio_key_tone.c.o,mixer,l
|
||||
objs/apps/common/audio/decode/decode.c.o
|
||||
-r=objs/apps/common/audio/decode/decode.c.o,audio_dec_app_create_param_init,pl
|
||||
-r=objs/apps/common/audio/decode/decode.c.o,audio_dac_get_channel,l
|
||||
@ -3008,8 +2972,6 @@ objs/apps/earphone/board/br28/board_jl701n_demo.c.o
|
||||
-r=objs/apps/earphone/board/br28/board_jl701n_demo.c.o,sleep_enter_callback_common,l
|
||||
-r=objs/apps/earphone/board/br28/board_jl701n_demo.c.o,power_init,l
|
||||
-r=objs/apps/earphone/board/br28/board_jl701n_demo.c.o,power_set_callback,l
|
||||
-r=objs/apps/earphone/board/br28/board_jl701n_demo.c.o,umidigi_chargestore_message_callback,l
|
||||
-r=objs/apps/earphone/board/br28/board_jl701n_demo.c.o,app_umidigi_chargetore_message_deal,l
|
||||
-r=objs/apps/earphone/board/br28/board_jl701n_demo.c.o,power_keep_dacvdd_en,l
|
||||
-r=objs/apps/earphone/board/br28/board_jl701n_demo.c.o,power_wakeup_init,l
|
||||
-r=objs/apps/earphone/board/br28/board_jl701n_demo.c.o,power_awakeup_set_callback,l
|
||||
@ -3020,7 +2982,6 @@ objs/apps/earphone/board/br28/board_jl701n_demo.c.o
|
||||
-r=objs/apps/earphone/board/br28/board_jl701n_demo.c.o,chargestore_open,l
|
||||
-r=objs/apps/earphone/board/br28/board_jl701n_demo.c.o,chargestore_close,l
|
||||
-r=objs/apps/earphone/board/br28/board_jl701n_demo.c.o,chargestore_write,l
|
||||
-r=objs/apps/earphone/board/br28/board_jl701n_demo.c.o,ldo_port_wakeup_to_cmessage,l
|
||||
-r=objs/apps/earphone/board/br28/board_jl701n_demo.c.o,chargestore_ldo5v_fall_deal,l
|
||||
-r=objs/apps/earphone/board/br28/board_jl701n_demo.c.o,status_config,pl
|
||||
-r=objs/apps/earphone/board/br28/board_jl701n_demo.c.o,key_table,pl
|
||||
@ -3242,7 +3203,6 @@ objs/apps/earphone/earphone.c.o
|
||||
-r=objs/apps/earphone/earphone.c.o,tone_play_index,l
|
||||
-r=objs/apps/earphone/earphone.c.o,app_earphone_key_event_handler,l
|
||||
-r=objs/apps/earphone/earphone.c.o,app_power_event_handler,l
|
||||
-r=objs/apps/earphone/earphone.c.o,app_umidigi_chargestore_event_handler,l
|
||||
-r=objs/apps/earphone/earphone.c.o,app_testbox_event_handler,l
|
||||
-r=objs/apps/earphone/earphone.c.o,rcsp_sys_event_handler_specific,l
|
||||
-r=objs/apps/earphone/earphone.c.o,get_tone_config,l
|
||||
@ -3288,7 +3248,6 @@ objs/apps/earphone/idle.c.o
|
||||
-r=objs/apps/earphone/idle.c.o,get_ui_busy_status,l
|
||||
-r=objs/apps/earphone/idle.c.o,dac_power_off,l
|
||||
-r=objs/apps/earphone/idle.c.o,power_set_soft_poweroff,l
|
||||
-r=objs/apps/earphone/idle.c.o,app_umidigi_chargestore_event_handler,l
|
||||
-r=objs/apps/earphone/idle.c.o,app_testbox_event_handler,l
|
||||
-r=objs/apps/earphone/idle.c.o,default_event_handler,l
|
||||
-r=objs/apps/earphone/idle.c.o,log_tag_const_i_APP_IDLE,l
|
||||
@ -4206,42 +4165,6 @@ objs/apps/earphone/power_manage/app_power_manage.c.o
|
||||
-r=objs/apps/earphone/power_manage/app_power_manage.c.o,log_tag_const_i_APP_POWER,l
|
||||
-r=objs/apps/earphone/power_manage/app_power_manage.c.o,VBAT_STATUS,pl
|
||||
objs/apps/earphone/power_manage/app_umidigi_chargestore.c.o
|
||||
-r=objs/apps/earphone/power_manage/app_umidigi_chargestore.c.o,umidigi_chargestore_get_power_level,pl
|
||||
-r=objs/apps/earphone/power_manage/app_umidigi_chargestore.c.o,get_charge_online_flag,l
|
||||
-r=objs/apps/earphone/power_manage/app_umidigi_chargestore.c.o,umidigi_chargestore_get_power_status,pl
|
||||
-r=objs/apps/earphone/power_manage/app_umidigi_chargestore.c.o,umidigi_chargestore_get_cover_status,pl
|
||||
-r=objs/apps/earphone/power_manage/app_umidigi_chargestore.c.o,umidigi_chargestore_get_earphone_online,pl
|
||||
-r=objs/apps/earphone/power_manage/app_umidigi_chargestore.c.o,umidigi_chargestore_set_bt_init_ok,pl
|
||||
-r=objs/apps/earphone/power_manage/app_umidigi_chargestore.c.o,umidigi_chargestore_sync_chg_level,pl
|
||||
-r=objs/apps/earphone/power_manage/app_umidigi_chargestore.c.o,umidigi_chargestore_set_sibling_chg_lev,pl
|
||||
-r=objs/apps/earphone/power_manage/app_umidigi_chargestore.c.o,umidigi_chargestore_set_power_level,pl
|
||||
-r=objs/apps/earphone/power_manage/app_umidigi_chargestore.c.o,umidigi_chargestore_check_going_to_poweroff,pl
|
||||
-r=objs/apps/earphone/power_manage/app_umidigi_chargestore.c.o,umidigi_chargestore_set_phone_disconnect,pl
|
||||
-r=objs/apps/earphone/power_manage/app_umidigi_chargestore.c.o,bt_ble_adv_ioctl,l
|
||||
-r=objs/apps/earphone/power_manage/app_umidigi_chargestore.c.o,umidigi_chargestore_set_phone_connect,pl
|
||||
-r=objs/apps/earphone/power_manage/app_umidigi_chargestore.c.o,umidigi_chargestore_timeout_deal,pl
|
||||
-r=objs/apps/earphone/power_manage/app_umidigi_chargestore.c.o,get_current_app,l
|
||||
-r=objs/apps/earphone/power_manage/app_umidigi_chargestore.c.o,strcmp,l
|
||||
-r=objs/apps/earphone/power_manage/app_umidigi_chargestore.c.o,sys_enter_soft_poweroff,l
|
||||
-r=objs/apps/earphone/power_manage/app_umidigi_chargestore.c.o,tws_api_get_role,l
|
||||
-r=objs/apps/earphone/power_manage/app_umidigi_chargestore.c.o,app_umidigi_chargestore_event_handler,pl
|
||||
-r=objs/apps/earphone/power_manage/app_umidigi_chargestore.c.o,log_print,l
|
||||
-r=objs/apps/earphone/power_manage/app_umidigi_chargestore.c.o,user_send_cmd_prepare,l
|
||||
-r=objs/apps/earphone/power_manage/app_umidigi_chargestore.c.o,sys_timeout_add,l
|
||||
-r=objs/apps/earphone/power_manage/app_umidigi_chargestore.c.o,sys_timer_modify,l
|
||||
-r=objs/apps/earphone/power_manage/app_umidigi_chargestore.c.o,umidigi_chargestore_event_to_user,pl
|
||||
-r=objs/apps/earphone/power_manage/app_umidigi_chargestore.c.o,sys_event_notify,l
|
||||
-r=objs/apps/earphone/power_manage/app_umidigi_chargestore.c.o,app_umidigi_chargetore_message_deal,pl
|
||||
-r=objs/apps/earphone/power_manage/app_umidigi_chargestore.c.o,testbox_get_status,l
|
||||
-r=objs/apps/earphone/power_manage/app_umidigi_chargestore.c.o,p33_soft_reset,l
|
||||
-r=objs/apps/earphone/power_manage/app_umidigi_chargestore.c.o,get_vbat_need_shutdown,l
|
||||
-r=objs/apps/earphone/power_manage/app_umidigi_chargestore.c.o,power_set_mode,l
|
||||
-r=objs/apps/earphone/power_manage/app_umidigi_chargestore.c.o,vbat_check_init,l
|
||||
-r=objs/apps/earphone/power_manage/app_umidigi_chargestore.c.o,task_switch,l
|
||||
-r=objs/apps/earphone/power_manage/app_umidigi_chargestore.c.o,puts,l
|
||||
-r=objs/apps/earphone/power_manage/app_umidigi_chargestore.c.o,log_tag_const_i_APP_UMIDIGI_CHARGESTORE,l
|
||||
-r=objs/apps/earphone/power_manage/app_umidigi_chargestore.c.o,log_tag_const_e_APP_UMIDIGI_CHARGESTORE,l
|
||||
-r=objs/apps/earphone/power_manage/app_umidigi_chargestore.c.o,app_var,l
|
||||
objs/apps/earphone/rcsp/jl_phone_app.c.o
|
||||
-r=objs/apps/earphone/rcsp/jl_phone_app.c.o,jl_phone_app_init,pl
|
||||
-r=objs/apps/earphone/rcsp/jl_phone_app.c.o,bt_music_info_handle_register,l
|
||||
@ -4693,7 +4616,6 @@ objs/cpu/br28/audio_dec.c.o
|
||||
-r=objs/cpu/br28/audio_dec.c.o,mic_trim_run,pl
|
||||
-r=objs/cpu/br28/audio_dec.c.o,audio_dec_init,pl
|
||||
-r=objs/cpu/br28/audio_dec.c.o,tone_play_init,l
|
||||
-r=objs/cpu/br28/audio_dec.c.o,audio_key_tone_init,l
|
||||
-r=objs/cpu/br28/audio_dec.c.o,audio_decoder_task_create,l
|
||||
-r=objs/cpu/br28/audio_dec.c.o,request_irq,l
|
||||
-r=objs/cpu/br28/audio_dec.c.o,sound_pcm_driver_init,l
|
||||
@ -6257,11 +6179,6 @@ objs/cpu/br28/uart_dev.c.o
|
||||
-r=objs/cpu/br28/uart_dev.c.o,CONFIG_UART2_ENABLE_TX_DMA,pl
|
||||
-r=objs/cpu/br28/uart_dev.c.o,jiffies,l
|
||||
objs/cpu/br28/umidigi_chargestore.c.o
|
||||
-r=objs/cpu/br28/umidigi_chargestore.c.o,umidigi_chargestore_message_callback,pl
|
||||
-r=objs/cpu/br28/umidigi_chargestore.c.o,ldo_port_wakeup_to_cmessage,pl
|
||||
-r=objs/cpu/br28/umidigi_chargestore.c.o,usr_timer_add,l
|
||||
-r=objs/cpu/br28/umidigi_chargestore.c.o,gpio_read,l
|
||||
-r=objs/cpu/br28/umidigi_chargestore.c.o,usr_timer_del,l
|
||||
objs/apps/common/colorful_lights/colorful_lights.c.o
|
||||
-r=objs/apps/common/colorful_lights/colorful_lights.c.o,colorful_lights_function,pl
|
||||
-r=objs/apps/common/colorful_lights/colorful_lights.c.o,sys_timeout_del,l
|
||||
@ -6325,7 +6242,6 @@ objs/apps/earphone/xtell_Sensor/xtell_handler.c.o
|
||||
-r=objs/apps/earphone/xtell_Sensor/xtell_handler.c.o,bt_app_exit,l
|
||||
-r=objs/apps/earphone/xtell_Sensor/xtell_handler.c.o,bt_hci_event_handler,l
|
||||
-r=objs/apps/earphone/xtell_Sensor/xtell_handler.c.o,app_power_event_handler,l
|
||||
-r=objs/apps/earphone/xtell_Sensor/xtell_handler.c.o,app_umidigi_chargestore_event_handler,l
|
||||
-r=objs/apps/earphone/xtell_Sensor/xtell_handler.c.o,app_testbox_event_handler,l
|
||||
-r=objs/apps/earphone/xtell_Sensor/xtell_handler.c.o,rcsp_sys_event_handler_specific,l
|
||||
-r=objs/apps/earphone/xtell_Sensor/xtell_handler.c.o,default_event_handler,l
|
||||
@ -6358,30 +6274,30 @@ objs/apps/earphone/xtell_Sensor/xtell_handler.c.o
|
||||
-r=objs/apps/earphone/xtell_Sensor/xtell_handler.c.o,config_btctler_mode,l
|
||||
-r=objs/apps/earphone/xtell_Sensor/xtell_handler.c.o,sniff_out,l
|
||||
objs/apps/earphone/xtell_Sensor/send_data.c.o
|
||||
-r=objs/apps/earphone/xtell_Sensor/send_data.c.o,ble_send_data,pl
|
||||
-r=objs/apps/earphone/xtell_Sensor/send_data.c.o,send_data_to_ble_client,l
|
||||
-r=objs/apps/earphone/xtell_Sensor/send_data.c.o,send_sensor_data_task,pl
|
||||
-r=objs/apps/earphone/xtell_Sensor/send_data.c.o,sensor_read_data,pl
|
||||
-r=objs/apps/earphone/xtell_Sensor/send_data.c.o,circle_buffer_is_full,l
|
||||
-r=objs/apps/earphone/xtell_Sensor/send_data.c.o,SL_SC7U22_RawData_Read,l
|
||||
-r=objs/apps/earphone/xtell_Sensor/send_data.c.o,Original_SL_SC7U22_Angle_Output,l
|
||||
-r=objs/apps/earphone/xtell_Sensor/send_data.c.o,SL_SC7U22_Angle_Output,l
|
||||
-r=objs/apps/earphone/xtell_Sensor/send_data.c.o,printf,l
|
||||
-r=objs/apps/earphone/xtell_Sensor/send_data.c.o,circle_buffer_write,l
|
||||
-r=objs/apps/earphone/xtell_Sensor/send_data.c.o,ano_send_attitude_data,l
|
||||
-r=objs/apps/earphone/xtell_Sensor/send_data.c.o,calculate_data,pl
|
||||
-r=objs/apps/earphone/xtell_Sensor/send_data.c.o,circle_buffer_is_empty,l
|
||||
-r=objs/apps/earphone/xtell_Sensor/send_data.c.o,circle_buffer_read,l
|
||||
-r=objs/apps/earphone/xtell_Sensor/send_data.c.o,sensor_processing_task,l
|
||||
-r=objs/apps/earphone/xtell_Sensor/send_data.c.o,BLE_send_data,pl
|
||||
-r=objs/apps/earphone/xtell_Sensor/send_data.c.o,send_data_to_ble_client,l
|
||||
-r=objs/apps/earphone/xtell_Sensor/send_data.c.o,strlen,l
|
||||
-r=objs/apps/earphone/xtell_Sensor/send_data.c.o,snprintf,l
|
||||
-r=objs/apps/earphone/xtell_Sensor/send_data.c.o,sqrtf,l
|
||||
-r=objs/apps/earphone/xtell_Sensor/send_data.c.o,snprintf,l
|
||||
-r=objs/apps/earphone/xtell_Sensor/send_data.c.o,sensor_measure,pl
|
||||
-r=objs/apps/earphone/xtell_Sensor/send_data.c.o,xtell_task_create,pl
|
||||
-r=objs/apps/earphone/xtell_Sensor/send_data.c.o,hw_iic_init,l
|
||||
-r=objs/apps/earphone/xtell_Sensor/send_data.c.o,gpio_set_direction,l
|
||||
-r=objs/apps/earphone/xtell_Sensor/send_data.c.o,gpio_set_pull_up,l
|
||||
-r=objs/apps/earphone/xtell_Sensor/send_data.c.o,gpio_direction_output,l
|
||||
-r=objs/apps/earphone/xtell_Sensor/send_data.c.o,delay_2ms,l
|
||||
-r=objs/apps/earphone/xtell_Sensor/send_data.c.o,SL_SC7U22_Config,l
|
||||
-r=objs/apps/earphone/xtell_Sensor/send_data.c.o,ano_protocol_init,l
|
||||
-r=objs/apps/earphone/xtell_Sensor/send_data.c.o,circle_buffer_init,l
|
||||
-r=objs/apps/earphone/xtell_Sensor/send_data.c.o,create_process,l
|
||||
-r=objs/apps/earphone/xtell_Sensor/send_data.c.o,puts,l
|
||||
@ -6421,6 +6337,7 @@ objs/apps/earphone/xtell_Sensor/sensor/SC7U22.c.o
|
||||
-r=objs/apps/earphone/xtell_Sensor/sensor/SC7U22.c.o,SL_SC7U22_I2c_Spi_Write,pl
|
||||
-r=objs/apps/earphone/xtell_Sensor/sensor/SC7U22.c.o,SL_SC7U22_I2c_Spi_Read,pl
|
||||
-r=objs/apps/earphone/xtell_Sensor/sensor/SC7U22.c.o,SL_SC7U22_Check,pl
|
||||
-r=objs/apps/earphone/xtell_Sensor/sensor/SC7U22.c.o,printf,l
|
||||
-r=objs/apps/earphone/xtell_Sensor/sensor/SC7U22.c.o,SL_SC7U22_Config,pl
|
||||
-r=objs/apps/earphone/xtell_Sensor/sensor/SC7U22.c.o,SL_SC7U22_POWER_DOWN,pl
|
||||
-r=objs/apps/earphone/xtell_Sensor/sensor/SC7U22.c.o,SL_SC7U22_SOFT_RESET,pl
|
||||
@ -6484,15 +6401,18 @@ objs/apps/earphone/xtell_Sensor/calculate/skiing_tracker.c.o
|
||||
-r=objs/apps/earphone/xtell_Sensor/calculate/skiing_tracker.c.o,remove_gravity_in_device_frame,pl
|
||||
-r=objs/apps/earphone/xtell_Sensor/calculate/skiing_tracker.c.o,cosf,l
|
||||
-r=objs/apps/earphone/xtell_Sensor/calculate/skiing_tracker.c.o,sinf,l
|
||||
-r=objs/apps/earphone/xtell_Sensor/calculate/skiing_tracker.c.o,forece_of_friction,pl
|
||||
-r=objs/apps/earphone/xtell_Sensor/calculate/skiing_tracker.c.o,skiing_tracker_update,pl
|
||||
-r=objs/apps/earphone/xtell_Sensor/calculate/skiing_tracker.c.o,sqrtf,l
|
||||
-r=objs/apps/earphone/xtell_Sensor/calculate/skiing_tracker.c.o,printf,l
|
||||
-r=objs/apps/earphone/xtell_Sensor/calculate/skiing_tracker.c.o,sensor_processing_task,pl
|
||||
-r=objs/apps/earphone/xtell_Sensor/calculate/skiing_tracker.c.o,fabsf,l
|
||||
-r=objs/apps/earphone/xtell_Sensor/calculate/skiing_tracker.c.o,puts,l
|
||||
-r=objs/apps/earphone/xtell_Sensor/calculate/skiing_tracker.c.o,debug2,pl
|
||||
-r=objs/apps/earphone/xtell_Sensor/calculate/skiing_tracker.c.o,KS_data,pl
|
||||
-r=objs/apps/earphone/xtell_Sensor/calculate/skiing_tracker.c.o,debug1,pl
|
||||
-r=objs/apps/earphone/xtell_Sensor/calculate/skiing_tracker.c.o,debug2,pl
|
||||
objs/apps/earphone/xtell_Sensor/ano/ano_protocol.c.o
|
||||
-r=objs/apps/earphone/xtell_Sensor/ano/ano_protocol.c.o,ano_protocol_init,pl
|
||||
-r=objs/apps/earphone/xtell_Sensor/ano/ano_protocol.c.o,ano_send_inertial_data,pl
|
||||
-r=objs/apps/earphone/xtell_Sensor/ano/ano_protocol.c.o,ano_send_attitude_data,pl
|
||||
cpu/br28/liba/cpu.a.llvm.19376.crc16.c
|
||||
-r=cpu/br28/liba/cpu.a.llvm.19376.crc16.c,__crc16_mutex_init,pl
|
||||
-r=cpu/br28/liba/cpu.a.llvm.19376.crc16.c,os_mutex_create,l
|
||||
|
||||
343201
cpu/br28/tools/sdk.lst
343201
cpu/br28/tools/sdk.lst
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
Binary file not shown.
@ -88,7 +88,4 @@ objs/apps/common/device/key/key_driver.c.o: \
|
||||
include_lib/btctrler/classic/hci_lmp.h \
|
||||
include_lib/system/device\rdec_key.h \
|
||||
include_lib/driver/cpu/br28\asm/rdec.h \
|
||||
include_lib/system/device\tent600_key.h \
|
||||
apps/earphone/include\tone_player.h cpu/br28\tone_player_api.h \
|
||||
cpu/br28/audio_config.h apps/common/audio\sine_make.h \
|
||||
include_lib/system\debug.h
|
||||
include_lib/system/device\tent600_key.h include_lib/system\debug.h
|
||||
|
||||
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
127
objs/apps/earphone/xtell_Sensor/ano/ano_protocol.c.d
Normal file
127
objs/apps/earphone/xtell_Sensor/ano/ano_protocol.c.d
Normal file
@ -0,0 +1,127 @@
|
||||
objs/apps/earphone/xtell_Sensor/ano/ano_protocol.c.o: \
|
||||
apps/earphone/xtell_Sensor/ano/ano_protocol.c \
|
||||
apps/earphone/xtell_Sensor/ano/ano_protocol.h \
|
||||
include_lib\system/includes.h include_lib/system/init.h \
|
||||
include_lib/system/event.h include_lib/system/generic/typedef.h \
|
||||
include_lib/driver/cpu/br28\asm/cpu.h \
|
||||
include_lib/driver/cpu/br28\asm/br28.h \
|
||||
include_lib/driver/cpu/br28\asm/io_omap.h \
|
||||
include_lib/driver/cpu/br28\asm/io_imap.h \
|
||||
include_lib/driver/cpu/br28\asm/csfr.h \
|
||||
include_lib/driver/cpu/br28\asm/cache.h \
|
||||
include_lib/driver/cpu/br28\asm/irq.h \
|
||||
include_lib/driver/cpu/br28\asm/hwi.h \
|
||||
include_lib/system\generic/printf.h include_lib\system/generic/log.h \
|
||||
include_lib/system\generic/errno-base.h \
|
||||
C:/JL/pi32/pi32v2-include\string.h C:/JL/pi32/pi32v2-include/_ansi.h \
|
||||
C:/JL/pi32/pi32v2-include\newlib.h \
|
||||
C:/JL/pi32/pi32v2-include\sys/config.h \
|
||||
C:/JL/pi32/pi32v2-include\machine/ieeefp.h \
|
||||
C:/JL/pi32/pi32v2-include\sys/features.h \
|
||||
C:/JL/pi32/pi32v2-include\sys/reent.h \
|
||||
C:/JL/pi32/pi32v2-include\sys/_types.h \
|
||||
C:/JL/pi32/pi32v2-include\machine/_types.h \
|
||||
C:/JL/pi32/pi32v2-include\machine/_default_types.h \
|
||||
C:/JL/pi32/pi32v2-include\sys/lock.h \
|
||||
C:/JL/pi32/pi32v2-include\sys/cdefs.h \
|
||||
C:/JL/pi32/pi32v2-include\sys/string.h \
|
||||
C:/JL/pi32/pi32v2-include\strings.h \
|
||||
C:/JL/pi32/pi32v2-include\sys/types.h \
|
||||
C:/JL/pi32/pi32v2-include\sys/_stdint.h \
|
||||
C:/JL/pi32/pi32v2-include\machine/types.h include_lib\system/malloc.h \
|
||||
include_lib/system/generic/list.h include_lib/system/generic/rect.h \
|
||||
include_lib/system/spinlock.h include_lib/system/generic\cpu.h \
|
||||
include_lib/system/generic\irq.h include_lib/system/task.h \
|
||||
include_lib/system/os/os_api.h include_lib/system\os/os_cpu.h \
|
||||
include_lib/system/generic\jiffies.h include_lib/system\os/os_error.h \
|
||||
include_lib/system\os/os_type.h include_lib/system\os/ucos_ii.h \
|
||||
include_lib/system\os/os_cfg.h include_lib/system\os/os_api.h \
|
||||
include_lib/system/timer.h include_lib/system/wait.h \
|
||||
include_lib/system/app_core.h include_lib/system/app_msg.h \
|
||||
include_lib/system/database.h include_lib/system/fs/fs.h \
|
||||
include_lib/system\generic/ioctl.h include_lib/system\generic/atomic.h \
|
||||
include_lib\system/sys_time.h include_lib/system/fs/fs_file_name.h \
|
||||
include_lib/system/fs/sdfile.h include_lib/system/power_manage.h \
|
||||
include_lib/system/syscfg_id.h include_lib/system/bank_switch.h \
|
||||
include_lib/system/generic/includes.h \
|
||||
include_lib/system/generic/ascii.h include_lib/system/generic/gpio.h \
|
||||
include_lib/driver/cpu/br28\asm/gpio.h \
|
||||
include_lib/system/generic/version.h include_lib/system/generic/lbuf.h \
|
||||
include_lib/system/generic/lbuf_lite.h \
|
||||
include_lib/system/generic/circular_buf.h \
|
||||
include_lib/system/generic/index.h \
|
||||
include_lib/system/generic/debug_lite.h \
|
||||
include_lib/system/device/includes.h \
|
||||
include_lib/system/device/device.h \
|
||||
include_lib/system\device/ioctl_cmds.h \
|
||||
include_lib/system/device/key_driver.h \
|
||||
include_lib/system/device/iokey.h include_lib/system/device/irkey.h \
|
||||
include_lib/system/device/adkey.h \
|
||||
include_lib/driver/cpu/br28\asm/adc_api.h \
|
||||
include_lib/system/device/slidekey.h \
|
||||
include_lib/system/device/touch_key.h \
|
||||
include_lib/driver/cpu/br28\asm/plcnt.h \
|
||||
include_lib/system/device/rdec_key.h \
|
||||
include_lib/driver/cpu/br28\asm/rdec.h \
|
||||
include_lib/driver/cpu/br28\asm/includes.h \
|
||||
include_lib/driver/cpu/br28\asm/crc16.h \
|
||||
include_lib/driver/cpu/br28\asm/clock.h \
|
||||
include_lib/driver/cpu/br28\asm/clock_hw.h \
|
||||
include_lib/driver/cpu/br28\asm/clock_define.h \
|
||||
include_lib/driver/cpu/br28\asm/uart.h \
|
||||
include_lib/driver\device/uart.h \
|
||||
include_lib/driver/cpu/br28\asm/uart_dev.h \
|
||||
include_lib/driver/cpu/br28\asm/spiflash.h \
|
||||
include_lib/driver\device/spiflash.h \
|
||||
include_lib/driver/cpu/br28\asm/power_interface.h \
|
||||
include_lib/driver/cpu/br28\asm/power/p33.h \
|
||||
include_lib/driver/cpu/br28/asm/power/p33_sfr.h \
|
||||
include_lib/driver/cpu/br28/asm/power/p33_app.h \
|
||||
include_lib/driver/cpu/br28/asm/power/p33_io_app.h \
|
||||
include_lib/driver/cpu/br28/asm/power/rtc_app.h \
|
||||
include_lib/driver/cpu/br28\asm/power/p11.h \
|
||||
include_lib/driver/cpu/br28/asm/power/p11_csfr.h \
|
||||
include_lib/driver/cpu/br28/asm/power/p11_sfr.h \
|
||||
include_lib/driver/cpu/br28/asm/power/p11_io_omap.h \
|
||||
include_lib/driver/cpu/br28/asm/power/p11_io_imap.h \
|
||||
include_lib/driver/cpu/br28/asm/power/p11_app.h \
|
||||
include_lib/driver/cpu/br28/asm/power/p11.h \
|
||||
include_lib/driver/cpu/br28\asm/power/power_api.h \
|
||||
include_lib/driver/cpu/br28\asm/power/power_port.h \
|
||||
include_lib/driver/cpu/br28\asm/power/power_wakeup.h \
|
||||
include_lib/driver/cpu/br28\asm/power/power_reset.h \
|
||||
include_lib/driver/cpu/br28\asm/power/power_compat.h \
|
||||
include_lib/driver/cpu/br28\asm/power/lp_ipc.h \
|
||||
include_lib/driver/cpu/br28/asm/power/m2p_msg.h \
|
||||
include_lib/driver/cpu/br28/asm/power/p2m_msg.h \
|
||||
include_lib/driver/cpu/br28\asm/efuse.h \
|
||||
include_lib/driver/cpu/br28\asm/wdt.h \
|
||||
include_lib/driver/cpu/br28\asm/debug.h \
|
||||
include_lib/driver/cpu/br28\asm/timer.h \
|
||||
include_lib/driver/cpu/br28\asm/rtc.h \
|
||||
include_lib/driver\device/sdio_host_init.h \
|
||||
apps/earphone/include\app_config.h \
|
||||
apps/earphone/board/br28\board_config.h include_lib\media/audio_def.h \
|
||||
apps/earphone/board/br28/board_jl701n_demo_cfg.h \
|
||||
apps/earphone/board/br28/board_jl701n_demo_global_build_cfg.h \
|
||||
apps/common/device/usb\usb_std_class_def.h \
|
||||
apps/earphone/board/br28/board_jl701n_btemitter_cfg.h \
|
||||
apps/earphone/board/br28/board_jl701n_btemitter_global_build_cfg.h \
|
||||
apps/earphone/board/br28/board_jl701n_anc_cfg.h \
|
||||
apps/earphone/board/br28/board_jl701n_anc_global_build_cfg.h \
|
||||
apps/earphone/board/br28/board_jl7016g_hybrid_cfg.h \
|
||||
apps/earphone/board/br28/board_jl7016g_hybrid_global_build_cfg.h \
|
||||
apps/earphone/board/br28/board_jl7018f_demo_cfg.h \
|
||||
apps/earphone/board/br28/board_jl7018f_demo_global_build_cfg.h \
|
||||
apps/common/device/usb\usb_common_def.h \
|
||||
include_lib/btctrler\btcontroller_mode.h \
|
||||
apps/earphone/include/user_cfg_id.h \
|
||||
apps/common/config/include\bt_profile_cfg.h \
|
||||
include_lib/btctrler\btcontroller_modules.h \
|
||||
include_lib/btctrler/hci_transport.h include_lib/btctrler/ble/hci_ll.h \
|
||||
C:/JL/pi32/pi32v2-include\stdint.h \
|
||||
C:/JL/pi32/pi32v2-include\sys/_intsup.h \
|
||||
C:/JL/pi32/pi32v2-include\stdlib.h \
|
||||
C:/JL/pi32/pi32v2-include\machine/stdlib.h \
|
||||
C:/JL/pi32/pi32v2-include\alloca.h \
|
||||
include_lib/btctrler/classic/hci_lmp.h
|
||||
BIN
objs/apps/earphone/xtell_Sensor/ano/ano_protocol.c.o
Normal file
BIN
objs/apps/earphone/xtell_Sensor/ano/ano_protocol.c.o
Normal file
Binary file not shown.
Binary file not shown.
@ -141,4 +141,5 @@ objs/apps/earphone/xtell_Sensor/send_data.c.o: \
|
||||
apps/earphone/xtell_Sensor/./buffer/circle_buffer.h \
|
||||
include_lib\btstack/avctp_user.h include_lib/btstack/btstack_typedef.h \
|
||||
apps/earphone/xtell_Sensor/calculate/skiing_tracker.h \
|
||||
apps/earphone/xtell_Sensor/calculate/../xtell.h
|
||||
apps/earphone/xtell_Sensor/calculate/../xtell.h \
|
||||
apps/earphone/xtell_Sensor/./ano/ano_protocol.h
|
||||
|
||||
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Reference in New Issue
Block a user