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

最新代码

Browse Source
This commit is contained in:
lmx
2025-11-13 09:50:42 +08:00
parent c88cb70bb1
commit 5e587e0527
29 changed files with 166924 additions and 166812 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@ -17,18 +17,23 @@
#define ZUPT_ACC_VARIANCE_THRESHOLD 0.2f
// 陀螺仪方差阈值
#define ZUPT_GYR_VARIANCE_THRESHOLD 5.0f
// 静止时候的陀螺仪模长
#define ZUPT_GYR_MAG_THRESHOLD 15
// --- --- ---
// --- 启动滑雪阈值 ---
// 加速度模长与重力的差值大于此值，认为开始运动；降低阈值，让“油门”更灵敏，以便能捕捉到真实的慢速启动
#define START_SKIING_ACC_THRESHOLD 0.5f
// 陀螺仪方差阈值，以允许启动瞬间的正常抖动，但仍能过滤掉混乱的、非滑雪的晃动。
#define SKIING_GYR_VARIANCE_THRESHOLD 15.0f
// --- --- ---
// --- 滑雪过程 ---
//加速度 模长，低于此值视为 在做匀速运动
#define SKIING_ACC_MAG_THRESHOLD 0.5f
//陀螺仪 模长，高于此值视为 摔倒了
#define FALLEN_GRY_MAG_THRESHOLD 2000.0f //未确定
// --- --- ---
// --- 原地旋转抖动 ---
// 用于原地旋转判断的加速度方差阈值。此值比ZUPT阈值更宽松，
@ -36,17 +41,36 @@
#define ROTATING_ACC_VARIANCE_THRESHOLD 0.8f
// 旋转/摆动检测阈值：角速度总模长大于此值(度/秒)，认为正在进行非滑雪的旋转或摆动
#define ROTATION_GYR_MAG_THRESHOLD 120.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电路而来
// alpha = RC / (RC + dt)，参考RC电路而来，fc ≈ (1 - alpha) / (2 * π * dt)
#define HPF_ALPHA 0.995
//0.995： 0.08 Hz 的信号
// --- --- ---
// 加速度死区阈值 (m/s^2)。低于此阈值的加速度被认为是噪声，不参与积分。
// 设得太高会忽略真实的慢速启动，设得太低则无法有效抑制噪声。
@ -100,6 +124,18 @@ void skiing_tracker_init(skiing_tracker_t *tracker)
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 设备坐标系下的加速度 [x, y, z]
@ -170,7 +206,17 @@ 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 状态机更新
@ -192,6 +238,7 @@ static void update_state_machine(skiing_tracker_t *tracker, const float *acc_dev
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]);
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]);
float acc_magnitude_g = acc_magnitude / G_ACCELERATION; // 转换为g单位，用于跳跃判断
#ifdef XTELL_TEST
debug1.acc_variance =acc_variance;
@ -199,72 +246,74 @@ static void update_state_machine(skiing_tracker_t *tracker, const float *acc_dev
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 0
//正在滑雪
if(tracker->state == NO_CONSTANT_SPEED ) {
//摔倒了
if(gyr_magnitude > FALLEN_GRY_MAG_THRESHOLD){
tracker->state = FALLEN;
return;
}
//可能进入了匀速状态
if(gyr_magnitude > SKIING_GYR_MAG_THRESHOLD && acc_magnitude < SKIING_ACC_MAG_THRESHOLD){
tracker->state = CONSTANT_SPEED;
return;
}
//急转弯
if(gyr_magnitude > WHEEL_GYR_MAG_THRESHOLD && acc_variance > WHEEL_ACC_VARIANCE_THRESHOLD){
//TODO：可以考虑清掉速度消除积分带来的漂移
tracker->state = WHEEL;
return;
}
}
// --- 状态切换逻辑 (按优先级) ---
// 优先级1：静止
if (acc_variance < ZUPT_ACC_VARIANCE_THRESHOLD && gyr_variance < ZUPT_GYR_VARIANCE_THRESHOLD) {
// --- 静止判断 ---
if (acc_variance < ZUPT_ACC_VARIANCE_THRESHOLD && gyr_variance < ZUPT_GYR_VARIANCE_THRESHOLD && gyr_magnitude < ZUPT_GYR_MAG_THRESHOLD) {
tracker->state = STATIC;
// 速度清零，抑制漂移
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));
return;
}
// 优先级2：原地旋转 - 特殊的、非滑雪的运动状态
// 条件：角速度很大，同时线性加速度的晃动在一个“中等”范围内。
if (tracker->state == STATIC && gyr_magnitude > ROTATION_GYR_MAG_THRESHOLD && acc_variance < ROTATING_ACC_VARIANCE_THRESHOLD) {
tracker->state = ROTATING;
return;
}
// 优先级3：启动滑雪 - “油门”
// 条件：有足够大的线性加速度，同时陀螺仪的抖动在一个“合理”（而非“完全静止”）的范围内。
if (fabsf(acc_magnitude - G_ACCELERATION) > START_SKIING_ACC_THRESHOLD && gyr_variance < SKIING_GYR_VARIANCE_THRESHOLD) {
tracker->state = NO_CONSTANT_SPEED;
return;
}
// 如果不满足任何启动或停止条件，则保持当前状态（滑雪中）
// 如果当前是静止或旋转，但没有满足启动条件，则状态会保持，直到满足ZUPT或旋转条件。
#else
// 无论当前是什么状态，静止总是最高优先级
if (acc_variance < ZUPT_ACC_VARIANCE_THRESHOLD && gyr_variance < ZUPT_GYR_VARIANCE_THRESHOLD) {
tracker->state = STATIC;
// 速度清零，抑制漂移
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));
return;
}
// --- 地面状态切换逻辑 ---
switch (tracker->state) {
case LANDING:
tracker->state = STATIC;
break;
case STATIC:
//不break，会往下执行，判断是否进入非匀速状态
case ROTATING: // 从静止或原地旋转可以启动
@ -275,9 +324,11 @@ static void update_state_machine(skiing_tracker_t *tracker, const float *acc_dev
}
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) {
//暂时不考虑摔倒
// 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; //匀速
@ -303,7 +354,7 @@ static void update_state_machine(skiing_tracker_t *tracker, const float *acc_dev
// TODO：回到 STATIC
break;
}
#endif
}
@ -343,7 +394,65 @@ void skiing_tracker_update(skiing_tracker_t *tracker, float *acc_g, float *gyr_d
// --- 更新状态机 ---
update_state_machine(tracker, acc_device_ms2, gyr_dps);
#if 1
// --- 根据状态执行不同的计算逻辑 ---
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));
break;
case WHEEL:
case NO_CONSTANT_SPEED:
transform_acc_to_world_frame(acc_device_ms2, angle, tracker->acc_world);
tracker->acc_world[2] -= G_ACCELERATION;
for (int i = 0; i < 3; i++) {
tracker->acc_world_filtered[i] = HPF_ALPHA * (tracker->acc_world_filtered[i] + tracker->acc_world[i] - tracker->acc_world_unfiltered_prev[i]);
tracker->acc_world_unfiltered_prev[i] = tracker->acc_world[i];
}
float acc_horizontal_mag = sqrtf(tracker->acc_world_filtered[0] * tracker->acc_world_filtered[0] +
tracker->acc_world_filtered[1] * tracker->acc_world_filtered[1]);
if (acc_horizontal_mag > ACC_DEAD_ZONE_THRESHOLD) {
tracker->velocity[0] += tracker->acc_world_filtered[0] * dt;
tracker->velocity[1] += tracker->acc_world_filtered[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:
// 速度清零，抑制漂移
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));
break;
case ROTATING:
tracker->speed = 0.0f;
break;
case FALLEN:
// TODO
break;
default:
break;
}
#else
// 坐标转换 & 移除重力
transform_acc_to_world_frame(acc_device_ms2, angle, tracker->acc_world);
tracker->acc_world[2] -= G_ACCELERATION;
@ -393,7 +502,7 @@ void skiing_tracker_update(skiing_tracker_t *tracker, float *acc_g, float *gyr_d
tracker->velocity[2] = 0; // 垂直速度强制归零
}
#endif
}
@ -520,8 +629,8 @@ BLE_send_data_t sensor_processing_task(signed short* acc_data_buf, signed short*
static float gyr_data_dps[3];
// const float delta_time = DELTA_TIME+0.01f;
const float delta_time = DELTA_TIME + 0.005f;
// const float delta_time = DELTA_TIME;
// const float delta_time = DELTA_TIME + 0.005f;
const float delta_time = DELTA_TIME;
BLE_send_data_t BLE_send_data;
if (!initialized) {

28
apps/earphone/xtell_Sensor/calculate/skiing_tracker.h
View File

@ -4,17 +4,17 @@
#include "../xtell.h"
// 定义滑雪者可能的状态
typedef enum {
STATIC, // 静止或动态稳定
NO_CONSTANT_SPEED, // 正在滑雪，非匀速
CONSTANT_SPEED, // 正在滑雪，匀速
ROTATING, // 正在原地旋转
WHEEL, // 转弯
FALLEN, // 已摔倒
TAKING_OFF, // 起跳冲击阶段
IN_AIR, // 空中失重阶段
LANDING, // 落地冲击阶段
STOP_DETECTION, // 停止检测
UNKNOWN // 未知状态
STATIC, // 静止或动态稳定：0
NO_CONSTANT_SPEED, // 正在滑雪，非匀速：1
CONSTANT_SPEED, // 正在滑雪，匀速：2
ROTATING, // 正在原地旋转：
WHEEL, // 转弯：3
FALLEN, // 已摔倒：4
TAKING_OFF, // 起跳冲击阶段：5
IN_AIR, // 空中失重阶段：6
LANDING, // 落地冲击阶段：7
STOP_DETECTION, // 停止检测：8
UNKNOWN // 未知状态：9
} skiing_state_t;
#define VARIANCE_BUFFER_SIZE 5 // 用于计算方差的数据窗口大小 (5个样本 @ 100Hz = 50ms)，减小延迟，提高实时性
@ -32,6 +32,12 @@ typedef struct {
// 内部计算使用的私有成员
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]; // 加速度数据窗口

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

@ -263,7 +263,7 @@ static BLE_send_data_t sensor_send_buffer[SENSOR_DATA_BUFFER_SIZE]; // 存放ble
*/
void sensor_read_data(){
xlog("=======sensor_read_data START\n");
// xlog("=======sensor_read_data START\n");
static signed short combined_raw_data[6];
static int initialized = 0;
static int calibration_done = 0;
@ -275,7 +275,7 @@ void sensor_read_data(){
static sensor_data_t tmp;
SL_SC7U22_RawData_Read(tmp.acc_data,tmp.gyr_data);
xlog("=======sensor_read_data middle 1\n");
// 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));
@ -292,7 +292,7 @@ void sensor_read_data(){
memcpy(tmp.gyr_data, &combined_raw_data[3], 3 * sizeof(signed short));
circle_buffer_write(&sensor_read, &tmp);
}
xlog("=======sensor_read_data END\n");
// xlog("=======sensor_read_data END\n");
}
@ -318,6 +318,7 @@ void calculate_data(){
}
static int count = 0;
extern char xt_Check_Flag;
void BLE_send_data(){
if(circle_buffer_is_empty(&sensor_send)){
// xlog("sensor_send_buffer: send buffer empty\n");
@ -328,14 +329,22 @@ void BLE_send_data(){
circle_buffer_read(&sensor_send, &tmp);
if(count >=50){
int num_chars_written;
count = 0;
char* division = "==========\n";
send_data_to_ble_client(division,strlen(division));
char log_buffer[100]; // 100个字符应该足够了
// extern char iic_read_len;
// extern char iic_write_result;
// num_chars_written = snprintf(log_buffer, sizeof(log_buffer),"SL_SC7U22_Check=0x%d, %d, %d\n", xt_Check_Flag, iic_read_len, iic_write_result);
// send_data_to_ble_client(&log_buffer,strlen(log_buffer));
memset(&log_buffer, 0, 100);
// 使用 snprintf 进行格式化
int num_chars_written = snprintf(
num_chars_written = snprintf(
log_buffer, // 目标缓冲区
sizeof(log_buffer), // 目标缓冲区的最大容量
"s %d, %dcm/s, %dcm\n", // 格式化字符串
@ -393,8 +402,8 @@ void BLE_send_data(){
}
//iic测试调用的
#if 1
extern char xt_Check_Flag = 10;
#if 0
static u16 xt_iic_test_id;
char log_buffer_1[100];
extern char sen_log_buffer_1[100];
@ -441,6 +450,21 @@ void xt_iic_test(){
// SL_SC7U22_Config();
}
#endif
#if 0
u16 xt_iic_test_id;
char hw_iic_init_result;
void xt_hw_iic_test(){
char log_buffer[100];
extern char iic_read_len;
extern char iic_write_result;
int num_chars_written = snprintf(log_buffer, sizeof(log_buffer),"init result:%d, SL_SC7U22_Check=0x%d,%d,%d\n",hw_iic_init_result, xt_Check_Flag, iic_read_len, iic_write_result);
extern void send_data_to_ble_client(const u8* data, u16 length);
send_data_to_ble_client(&log_buffer,strlen(log_buffer));
}
#endif
void xtell_task_create(void){
@ -484,7 +508,6 @@ void xtell_task_create(void){
// SkiingTracker_Init(&skiing_data);
xlog("SkiingTracker_Init\n");
// create_process(&test_id, "test",NULL, test, (int)(DELTA_TIME*1000));
create_process(&sensor_read_data_id, "read",NULL, sensor_read_data, 10);
@ -492,7 +515,9 @@ void xtell_task_create(void){
create_process(&ble_send_data_id, "send",NULL, BLE_send_data, 1);
// create_process(&xt_iic_test_id,"iic_test",NULL,xt_iic_test,1000);
#if 0
hw_iic_init_result = ret;
create_process(&xt_iic_test_id,"iic_test",NULL,xt_hw_iic_test,1000);
#endif
}

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

@ -4,7 +4,7 @@
#include "os/os_api.h"
#define ENABLE_XLOG 0
// #define ENABLE_XLOG 1
#ifdef xlog
#undef xlog
#endif
@ -126,15 +126,15 @@ unsigned char SL_SC7U22_Config(void)
// SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x40, 0x06);//ACC_CONF 0x07=50Hz 0x06=25Hz
// SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x40, 0xA8);//高性能模式，连续4个数据平均1次，100Hz -- lmx
// SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x40, 0xAC);//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, 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, 0x41, 0x01);//ACC_RANGE 10：±8G 01：±4G
// 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, 0xAB);//GYR_CONF 800Hz -- 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, 0x43, 0x00);//GYR_RANGE 2000dps
SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x43, 0x00);//GYR_RANGE 2000dps