修改了六轴配置
This commit is contained in:
@ -22,13 +22,13 @@
|
||||
// 加速度模长与重力的差值大于此值，认为开始运动；降低阈值，让“油门”更灵敏，以便能捕捉到真实的慢速启动
|
||||
#define START_SKIING_ACC_THRESHOLD 0.5f
|
||||
// 陀螺仪方差阈值，以允许启动瞬间的正常抖动，但仍能过滤掉混乱的、非滑雪的晃动。
|
||||
#define SKIING_GYR_VARIANCE_THRESHOLD 15.0f
|
||||
#define SKIING_GYR_VARIANCE_THRESHOLD 20.0f
|
||||
|
||||
// --- 滑雪过程 ---
|
||||
//加速度 模长，低于此值视为 在做匀速运动
|
||||
#define SKIING_ACC_MAG_THRESHOLD 0.5f
|
||||
//陀螺仪 模长，高于此值视为 摔倒了
|
||||
#define FALLEN_GRY_MAG_THRESHOLD 1000.0f //未确定
|
||||
#define FALLEN_GRY_MAG_THRESHOLD 2000.0f //未确定
|
||||
|
||||
// --- 原地旋转抖动 ---
|
||||
// 用于原地旋转判断的加速度方差阈值。此值比ZUPT阈值更宽松，
|
||||
@ -41,7 +41,7 @@
|
||||
// 加速度方差阈值，大于此值，滑雪过程可能发生了急转弯
|
||||
#define WHEEL_ACC_VARIANCE_THRESHOLD 7.0f
|
||||
// 旋转/摆动检测阈值：角速度总模长大于此值(度/秒)，认为滑雪过程中进行急转弯
|
||||
#define WHEEL_GYR_MAG_THRESHOLD 220.0f // 150.0f 到 250.0f之间进行调整
|
||||
#define WHEEL_GYR_MAG_THRESHOLD 500.0f //
|
||||
|
||||
// --- 用于消除积分漂移的滤波器和阈值 ---
|
||||
// 高通滤波器系数 (alpha)。alpha 越接近1，滤除低频(直流偏移)的效果越强，但可能滤掉真实的慢速运动。
|
||||
@ -54,9 +54,10 @@
|
||||
|
||||
// --- 模拟摩擦力，进行速度衰减 ---
|
||||
#define SPEED_ATTENUATION 1.0f //暂不模拟
|
||||
BLE_KS_send_data_t KS_data;
|
||||
|
||||
#ifdef XTELL_TEST
|
||||
BLE_KS_send_data_t KS_data;
|
||||
|
||||
debug_t debug1;
|
||||
debug_t debug2;
|
||||
#endif
|
||||
@ -377,12 +378,13 @@ void skiing_tracker_update(skiing_tracker_t *tracker, float *acc_g, float *gyr_d
|
||||
|
||||
|
||||
}else if(tracker->state == CONSTANT_SPEED){ //匀速
|
||||
#ifdef XTELL_TEST
|
||||
debug2.acc_magnitude = acc_horizontal_mag;
|
||||
#endif
|
||||
|
||||
//保持上次的速度不变。只更新距离
|
||||
tracker->distance += tracker->speed * dt;
|
||||
|
||||
}else if(tracker->state == WHEEL){ //匀速
|
||||
//暂时：保持上次的速度不变。只更新距离
|
||||
tracker->distance += tracker->speed * dt;
|
||||
}else{
|
||||
// 在静止或旋转状态下，速度已经在状态机内部被清零
|
||||
// 额外增加速度衰减，模拟摩擦力，进一步抑制漂移
|
||||
@ -395,10 +397,10 @@ void skiing_tracker_update(skiing_tracker_t *tracker, float *acc_g, float *gyr_d
|
||||
}
|
||||
|
||||
|
||||
|
||||
#if 0
|
||||
/**
|
||||
* @brief 传感器数据采集与处理任务，外部每10ms调用一次，如果需要更新时间间隔，也需要同步更新宏“ DELTA_TIME ”
|
||||
*
|
||||
* 使用到了卡尔曼
|
||||
* @param acc_data_buf 三轴加速度原始数据
|
||||
* @param gyr_data_buf 三轴陀螺仪原始数据
|
||||
* @return BLE_send_data_t
|
||||
@ -427,14 +429,14 @@ BLE_send_data_t sensor_processing_task(signed short * acc_data_buf, signed short
|
||||
|
||||
unsigned char status;
|
||||
if (!calibration_done) { //第1次启动，开启零漂检测
|
||||
status = SL_SC7U22_Angle_Output(1, combined_raw_data, final_angle_data, 0);
|
||||
status = Original_SL_SC7U22_Angle_Output(1, combined_raw_data, final_angle_data, 0);
|
||||
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, final_angle_data, 0);
|
||||
status = Original_SL_SC7U22_Angle_Output(0, combined_raw_data, final_angle_data, 0);
|
||||
}
|
||||
|
||||
if (status == 1) {
|
||||
@ -469,10 +471,11 @@ BLE_send_data_t sensor_processing_task(signed short * acc_data_buf, signed short
|
||||
BLE_send_data.sensor_state = status;
|
||||
BLE_send_data.skiing_state = my_skiing_tracker.state;
|
||||
for (int i = 0; i < 3; i++) {
|
||||
#ifndef XTELL_TEST
|
||||
// #ifndef XTELL_TEST
|
||||
BLE_send_data.acc_original[i] = (int)acc_data_buf[i];
|
||||
BLE_send_data.gyr_original[i] = (int)gyr_data_buf[i];
|
||||
#endif
|
||||
BLE_send_data.Angle[i] = final_angle_data[i];
|
||||
// #endif
|
||||
#if KS_BLE
|
||||
KS_data.acc_KS[i] = (int)(calibrated_acc_g[i] * G_ACCELERATION * 100); //cm/s^s
|
||||
KS_data.gyr_KS_dps[i] = (int)calibrated_gyr_dps[i];
|
||||
@ -498,4 +501,65 @@ BLE_send_data_t sensor_processing_task(signed short * acc_data_buf, signed short
|
||||
// printf("Angle calculation error or calibration not finished.\n");
|
||||
}
|
||||
return BLE_send_data;
|
||||
}
|
||||
}
|
||||
|
||||
#else
|
||||
|
||||
/**
|
||||
* @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) {
|
||||
|
||||
static int initialized = 0;
|
||||
static float acc_data_g[3];
|
||||
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+0.01f;
|
||||
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");
|
||||
}
|
||||
|
||||
// 加速度 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;
|
||||
|
||||
// 陀螺仪 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;
|
||||
}
|
||||
|
||||
#endif
|
||||
@ -42,15 +42,14 @@ typedef struct {
|
||||
} skiing_tracker_t;
|
||||
|
||||
//ble发送的数据
|
||||
typedef struct __attribute__((packed)){ //该结构体取消内存对齐
|
||||
typedef struct{ //__attribute__((packed)){ //该结构体取消内存对齐
|
||||
char sensor_state;
|
||||
char skiing_state;
|
||||
int speed_cms; //求出的速度，cm/s
|
||||
int distance_cm; //求出的距离，cm
|
||||
#ifndef XTELL_TEST
|
||||
int acc_original[3]; //直接读取传感器得到的原始三轴加速度
|
||||
int gyr_original[3]; //直接读取传感器得到的原始三轴陀螺仪
|
||||
#endif
|
||||
short acc_data[3]; //三轴加速度, g
|
||||
short gyr_data[3]; //三轴陀螺仪, dps
|
||||
float angle_data[3]; //欧若拉角
|
||||
}BLE_send_data_t;
|
||||
|
||||
typedef struct{
|
||||
@ -74,12 +73,5 @@ typedef struct{
|
||||
*/
|
||||
void skiing_tracker_init(skiing_tracker_t *tracker);
|
||||
|
||||
/**
|
||||
* @brief 传感器数据采集与处理任务，外部每10ms调用一次，如果需要更新时间间隔，也需要同步更新宏“ DELTA_TIME ”
|
||||
*
|
||||
* @param acc_data_buf 三轴加速度原始数据
|
||||
* @param gyr_data_buf 三轴陀螺仪原始数据
|
||||
* @return BLE_send_data_t
|
||||
*/
|
||||
BLE_send_data_t sensor_processing_task(signed short * acc_data_buf, signed short * gyr_data_buf) ;
|
||||
BLE_send_data_t sensor_processing_task(signed short* acc_data_buf, signed short* gyr_data_buf, float* angle_data) ;
|
||||
#endif // SKIING_TRACKER_H
|
||||
Reference in New Issue
Block a user