diff --git a/apps/earphone/xtell_Sensor/calculate/skiing_tracker.c b/apps/earphone/xtell_Sensor/calculate/skiing_tracker.c index c26f9ea..c5953a0 100644 --- a/apps/earphone/xtell_Sensor/calculate/skiing_tracker.c +++ b/apps/earphone/xtell_Sensor/calculate/skiing_tracker.c @@ -1,11 +1,14 @@ /* - 使用四元数求角度和去掉重力分量 + */ #include "skiing_tracker.h" #include "../sensor/SC7U22.h" #include #include +#define G_ACCELERATION 9.81f +#define DEG_TO_RAD (3.14159265f / 180.0f) + #define ENABLE_XLOG 1 #ifdef xlog #undef xlog @@ -16,13 +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; @@ -53,7 +126,7 @@ void stop_detection(void){ } /** - * @brief 初始化 + * @brief 初始化滑雪追踪器 * * @param tracker */ @@ -67,6 +140,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; +} + /** @@ -93,7 +178,7 @@ void q_remove_gravity_with_quaternion(const float *acc_device, const float *q, f } /** - * @brief 使用四元数将设备坐标系的线性加速度转换到世界坐标系,并且移除重力分量 + * @brief 使用四元数将设备坐标系的线性加速度转换到世界坐标系 * @details 同样，此方法比使用欧拉角更优。 * @param acc_linear_device 输入：设备坐标系下的线性加速度 [x, y, z] * @param q 输入：表示姿态的四元数 [w, x, y, z] @@ -117,6 +202,208 @@ void q_transform_to_world_with_quaternion(const float *acc_linear_device, const } + +/** + * @brief 计算缓冲区内三轴数据的方差之和 + * + * @param buffer 传进来的三轴数据：陀螺仪/加速度 + * @return float 返回方差和 + */ +static float calculate_variance(float buffer[VARIANCE_BUFFER_SIZE][3]) +{ + float mean[3] = {0}; + float variance[3] = {0}; + + // 计算均值 + for (int i = 0; i < VARIANCE_BUFFER_SIZE; i++) { + mean[0] += buffer[i][0]; + mean[1] += buffer[i][1]; + mean[2] += buffer[i][2]; + } + mean[0] /= VARIANCE_BUFFER_SIZE; + mean[1] /= VARIANCE_BUFFER_SIZE; + mean[2] /= VARIANCE_BUFFER_SIZE; + + // 计算方差 + for (int i = 0; i < VARIANCE_BUFFER_SIZE; i++) { + variance[0] += (buffer[i][0] - mean[0]) * (buffer[i][0] - mean[0]); + variance[1] += (buffer[i][1] - mean[1]) * (buffer[i][1] - mean[1]); + variance[2] += (buffer[i][2] - mean[2]) * (buffer[i][2] - mean[2]); + } + variance[0] /= VARIANCE_BUFFER_SIZE; + variance[1] /= VARIANCE_BUFFER_SIZE; + variance[2] /= VARIANCE_BUFFER_SIZE; + + // 返回三轴方差之和，作为一个综合的稳定度指标 + 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; + } + + +} + + /** * @brief 主更新函数 * @@ -140,49 +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]; + // 将最新数据存入缓冲区 + 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; // 标记缓冲区已满 + } + + // --- 更新状态机 --- + 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]; - // 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, linear_acc_device); - q_remove_gravity_with_quaternion(acc_device_ms2,quaternion_data,tmp_device_acc); - q_transform_to_world_with_quaternion(tmp_device_acc,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]); + // 将设备坐标系下的线性加速度，旋转到世界坐标系 + 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]); - - float gx_proj = 2.0f * (quaternion_data[1] * quaternion_data[3] - quaternion_data[0] * quaternion_data[2]); - float gy_proj = 2.0f * (quaternion_data[0] * quaternion_data[1] + quaternion_data[2] * quaternion_data[3]); - float gz_proj = quaternion_data[0] * quaternion_data[0] - quaternion_data[1] * quaternion_data[1] - quaternion_data[2] * quaternion_data[2] + quaternion_data[3] * quaternion_data[3]; - - + 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("===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===\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]); - xlog("GRAVITY VECTOR in device frame: gx=%.2f, gy=%.2f, gz=%.2f\n", gx_proj, gy_proj, gz_proj); - extern mmc5603nj_cal_data_t cal_data; - xlog("cal_data:X: %.4f, Y: %.4f, Z: %.4f\n", cal_data.offset_x,cal_data.offset_y,cal_data.offset_z); count = 0; - char send_tmp[1]; - send_tmp[0] = (char)all_world_mag; - send_data_to_ble_client(&send_tmp,1); } count++; - #endif + } @@ -193,13 +570,14 @@ void skiing_tracker_update(skiing_tracker_t *tracker, float *acc_g, float *gyr_d * @param acc_data_buf 传入的三轴加速度数据 * @param gyr_data_buf 传入的三轴陀螺仪数据 * @param angle_data 传入的欧若拉角数据 - * @return BLE_send_data_t 要发送给蓝牙的数据 + * @return 速度cm/s */ -BLE_send_data_t sensor_processing_task(signed short* acc_data_buf, signed short* gyr_data_buf, float* angle_data, float* quaternion) { +int 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)); } @@ -207,7 +585,6 @@ BLE_send_data_t sensor_processing_task(signed short* acc_data_buf, signed short* // 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); @@ -252,22 +629,7 @@ 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"); - return BLE_send_data; + return (int)(my_skiing_tracker.speed * 100); } diff --git a/apps/earphone/xtell_Sensor/calculate/skiing_tracker.h b/apps/earphone/xtell_Sensor/calculate/skiing_tracker.h index b1021f2..068f47f 100644 --- a/apps/earphone/xtell_Sensor/calculate/skiing_tracker.h +++ b/apps/earphone/xtell_Sensor/calculate/skiing_tracker.h @@ -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, float* quaternion); +int sensor_processing_task(signed short* acc_data_buf, signed short* gyr_data_buf, float* angle_data, float* quaternion); #endif // SKIING_TRACKER_H \ No newline at end of file diff --git a/apps/earphone/xtell_Sensor/send_data.c b/apps/earphone/xtell_Sensor/send_data.c index 3d04e00..e2a9966 100644 --- a/apps/earphone/xtell_Sensor/send_data.c +++ b/apps/earphone/xtell_Sensor/send_data.c @@ -35,6 +35,9 @@ #else #define xlog(format, ...) ((void)0) #endif + +#define SENSOR_DATA_BUFFER_SIZE 500 // 定义缓冲区可以存储XXX个sensor_data_t元素 + // /////////////////////////////////////////////////////////////////////////////////////////////////// @@ -42,6 +45,7 @@ //START -- 函数定义 void send_data_to_ble_client(const u8* data, u16 length); extern void create_process(u16* pid, const char* name, void *priv, void (*func)(void *priv), u32 msec); +extern void close_process(u16* pid,char* name); //END -- 函数定义 ////////////////////////////////////////////////////////////////////////////////////////////////// @@ -49,492 +53,109 @@ extern void create_process(u16* pid, const char* name, void *priv, void (*func)( ////////////////////////////////////////////////////////////////////////////////////////////////// //START -- 变量定义 -// --- 任务ID --- -static u16 xtell_i2c_test_id; -static u16 collect_data_id; -static u16 ble_send_data_id; -static u16 sensor_read_data_id; -static u16 calculate_data_id; -// --- 环形缓冲区 --- -#define SENSOR_DATA_BUFFER_SIZE 512 -static u8 sensor_data_buffer[SENSOR_DATA_BUFFER_SIZE]; -static circle_buffer_t sensor_cb; +typedef struct { + // -- 六轴 -- + signed short acc_data[3]; + signed short gyr_data[3]; + // -- 磁力计 -- + uint8_t mmc5603nj_buffer[9]; + // -- 速度 -- + int speed_cms; + // -- 气压计 -- + //... +} BLE_send_data_t; static int count = 0; -//--- test --- -// 全局变量 -u16 gsensor_id=0; -u16 test_id=0; +// --- 环形缓冲区 --- +static circle_buffer_t BLE_send_buff; // 环形缓冲区管理结构体 + + //END -- 变量定义 ////////////////////////////////////////////////////////////////////////////////////////////////// - - - - - - - -#if 0 -BLE_send_data_t BLE_send_data; -void test(){ - signed short acc_data_buf[3] = {0}; - signed short gyr_data_buf[3] = {0}; - signed short acc_gyro_input[6] = {0}; - float Angle_output[3] = {0}; - // xlog("============start\n"); - - SL_SC7U22_RawData_Read(acc_data_buf,gyr_data_buf); - - BLE_send_data = sensor_processing_task(acc_data_buf, gyr_data_buf); - u8 data[50]; - data[0] = 0xBB; - data[1] = 0xBE; - data[2] = 0x01; - data[3] = sizeof(BLE_send_data_t); //后续包的数据长度 - // send_data_to_ble_client(&data,sizeof(BLE_send_data_t)+4); - memcpy(&data[4], &BLE_send_data, sizeof(BLE_send_data_t)); - - static int count = 0; - if(count >=10){ - count = 0; - - char* division = "==========\n"; - send_data_to_ble_client(division,strlen(division)); - - char log_buffer[100]; // 100个字符应该足够了 - - // 使用 snprintf 进行格式化 - int num_chars_written = snprintf( - log_buffer, // 目标缓冲区 - sizeof(log_buffer), // 目标缓冲区的最大容量 - "s %d, %dcm/s, %dcm\n", // 格式化字符串 - BLE_send_data.skiing_state, // 第一个 %d 的参数 - BLE_send_data.speed_cms, // 第二个 %d 的参数 - BLE_send_data.distance_cm // 第三个 %d 的参数 - ); - send_data_to_ble_client(&log_buffer,strlen(log_buffer)); - - extern BLE_KS_send_data_t KS_data; - memset(&log_buffer, 0, 100); - num_chars_written = snprintf( - log_buffer, - sizeof(log_buffer), - "Acc:%d, %d, %d\n", - KS_data.acc_KS[0],KS_data.acc_KS[1],KS_data.acc_KS[2] - ); // cm/s^2 - send_data_to_ble_client(&log_buffer,strlen(log_buffer)); - - memset(&log_buffer, 0, 100); - num_chars_written = snprintf( - log_buffer, - sizeof(log_buffer), - "Gyr_dps:%d, %d, %d\n", - KS_data.gyr_KS_dps[0], - KS_data.gyr_KS_dps[1], - KS_data.gyr_KS_dps[2] - ); - send_data_to_ble_client(&log_buffer,strlen(log_buffer)); - - memset(&log_buffer, 0, 100); - num_chars_written = snprintf( - log_buffer, - sizeof(log_buffer), - "angle: %d, %d, %d\n", - KS_data.angle_KS[0], - KS_data.angle_KS[1], - KS_data.angle_KS[2] - ); - send_data_to_ble_client(&log_buffer,strlen(log_buffer)); - - extern debug_t debug1; - extern debug_t debug2; - memset(&log_buffer, 0, 100); - num_chars_written = snprintf( - log_buffer, - sizeof(log_buffer), - "debug:%.2f,%.2f,%.2f(%.2f),%.2f\n", - debug1.acc_variance, - debug1.gyr_variance, - debug1.acc_magnitude, - debug2.acc_magnitude, //滤波后的加速度 - debug1.gyr_magnitude - ); - send_data_to_ble_client(&log_buffer,strlen(log_buffer)); - - xlog("Call interval\n"); - } - count++; - - memset(&BLE_send_data, 0, sizeof(BLE_send_data_t)); - memset(&data, 0, 50); - // xlog("end============\n"); - -} -#endif - - - -#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]; //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 //读取传感器的数据放进缓冲区 + * @brief 六轴静态校准 * */ -void sensor_read_data(){ - +void SC7U22_static_calibration(void){ + signed short acc_data_buf[3]; + signed short gyr_data_buf[3]; + float angle[3]; + float quaternion_output[3]; + + static signed short combined_raw_data[6]; + static int calibration_done = 0; + char status = 0; + set_SC7U22_Error_Flag(0); + SL_SC7U22_RawData_Read(acc_data_buf,gyr_data_buf); + memcpy(&combined_raw_data[0], acc_data_buf, 3 * sizeof(signed short)); + memcpy(&combined_raw_data[3], gyr_data_buf, 3 * sizeof(signed short)); + status = Q_SL_SC7U22_Angle_Output(1, combined_raw_data, angle,NULL, 0, quaternion_output); + + if(status == 1){ //校准完成 + extern u16 SC7U22_calibration_id; + extern u8 SC7U22_init; + SC7U22_init = 1; + close_process(&SC7U22_calibration_id, "SC7U22_calibration"); + u8 send2_1[5] = {0xBB,0xBE,0x02,0x00,0x01}; + send_data_to_ble_client(&send2_1,strlen(send2_1)); + } + // if(count > 100){ + // count = 0; + // char log_buffer[100]; + // // snprintf( log_buffer, sizeof(log_buffer),"status:%d\n",status); + // // send_data_to_ble_client(&log_buffer,strlen(log_buffer)); + // xlog("status:%d\n", status); + // xlog("RawData:AX=%d,AY=%d,AZ=%d,GX=%d,GY=%d,GZ=%d\r\n",combined_raw_data[0],combined_raw_data[1],combined_raw_data[2],combined_raw_data[3],combined_raw_data[4],combined_raw_data[5]); + // } + // count++; +} + +/** + * @brief 开始采集传感器数据和计算速度 + * + */ +void start_collect_fuc(void){ // xlog("=======sensor_read_data START\n"); + static signed short combined_raw_data[6]; static int initialized = 0; static int calibration_done = 0; char status = 0; - if(circle_buffer_is_full(&sensor_read)){ - // xlog("sensor_read_data: read buffer full\n"); - return; - } + BLE_send_data_t BLE_send_data; + uint8_t mmc5603nj_buffer[9]; + signed short acc_data_buf[3]; + signed short gyr_data_buf[3]; + float angle[3]; + float quaternion_output[3]; + SL_SC7U22_RawData_Read(acc_data_buf,gyr_data_buf); + mmc5603nj_read_origin_data(mmc5603nj_buffer); + + memcpy(&combined_raw_data[0], acc_data_buf, 3 * sizeof(signed short)); + memcpy(&combined_raw_data[3], gyr_data_buf, 3 * sizeof(signed short)); + + status = Q_SL_SC7U22_Angle_Output(0, combined_raw_data, angle,NULL, 0, quaternion_output); + + memcpy(acc_data_buf, &combined_raw_data[0], 3 * sizeof(signed short)); + memcpy(gyr_data_buf, &combined_raw_data[3], 3 * sizeof(signed short)); + int speed = sensor_processing_task(acc_data_buf,gyr_data_buf,angle, quaternion_output); - 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,NULL, 0, tmp.quaternion_output); - - 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; - 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,NULL, 0, tmp.quaternion_output); - - memcpy(tmp.acc_data, &combined_raw_data[0], 3 * sizeof(signed short)); - memcpy(tmp.gyr_data, &combined_raw_data[3], 3 * sizeof(signed short)); - 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"); - -} - -void calculate_data(){ - // xlog("=======start\n"); - sensor_data_t tmp; - - if(circle_buffer_is_empty(&sensor_read)){ - // xlog("sensor_read_buffer: read buffer empty\n"); - return; - } - - 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); + memcpy(BLE_send_data.acc_data, acc_data_buf, 3 * sizeof(signed short)); + memcpy(BLE_send_data.gyr_data, gyr_data_buf, 3 * sizeof(signed short)); + memcpy(BLE_send_data.mmc5603nj_buffer, mmc5603nj_buffer, 9); + BLE_send_data.speed_cms = speed; - - if(circle_buffer_is_full(&sensor_send)) - return; - - if(mutex2 == 0){ - mutex2 = 1; - circle_buffer_write(&sensor_send, &data_by_calculate); - mutex2 = 0; - } - - - // extern void BLE_send_data(); - // BLE_send_data(); - // xlog("=======end\n"); -} - -extern char xt_Check_Flag; -void BLE_send_data(){ - // xlog("=======start\n"); - if(circle_buffer_is_empty(&sensor_send)){ - // xlog("sensor_send_buffer: send buffer empty\n"); - return; - } - #ifdef XTELL_TEST - // #if 0 - BLE_send_data_t tmp; - if(mutex2 == 0){ - mutex2 = 1; - circle_buffer_read(&sensor_send, &tmp); - mutex2 = 0; - }else{ - return; - } - - - if(count >=100){ - // extern debug_t debug2; - // xlog("s %d, %dcm/s, %dcm\n",tmp.skiing_state, tmp.speed_cms, tmp.distance_cm); - // xlog("Acc:%d, %d, %d\n",tmp.acc_data[0],tmp.acc_data[1],tmp.acc_data[2]); - // xlog("Gyr:%d, %d, %d\n", tmp.gyr_data[0],tmp.gyr_data[1],tmp.gyr_data[2]); - // xlog("debug2.acc_magnitude:%.2f\n", debug2.acc_magnitude); - - 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); - #if 1 - // 使用 snprintf 进行格式化 - num_chars_written = snprintf( - log_buffer, // 目标缓冲区 - sizeof(log_buffer), // 目标缓冲区的最大容量 - "s %d, %dcm/s, %dcm\n", // 格式化字符串 - tmp.skiing_state, // 第一个 %d 的参数 - tmp.speed_cms, // 第二个 %d 的参数 - tmp.distance_cm // 第三个 %d 的参数 - ); - send_data_to_ble_client(&log_buffer, strlen(log_buffer)); - - memset(&log_buffer, 0, 100); - num_chars_written = snprintf( - log_buffer, - sizeof(log_buffer), - "Acc:%d, %d, %d\n", - tmp.acc_data[0],tmp.acc_data[1],tmp.acc_data[2] - ); - send_data_to_ble_client(&log_buffer,strlen(log_buffer)); - - memset(&log_buffer, 0, 100); - num_chars_written = snprintf( - log_buffer, - sizeof(log_buffer), - "Gyr:%d, %d, %d\n", - tmp.gyr_data[0],tmp.gyr_data[1],tmp.gyr_data[2] - ); - send_data_to_ble_client(&log_buffer,strlen(log_buffer)); - - memset(&log_buffer, 0, 100); - num_chars_written = snprintf( - log_buffer, - sizeof(log_buffer), - "Angle:%.1f, %.1f, %1.f\n", - 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( - log_buffer, - sizeof(log_buffer), - "acc_cm/s^2:%d\n", - acc_mo_cms - ); - send_data_to_ble_client(&log_buffer,strlen(log_buffer)); - - // xlog("s %d, %dcm/s, %dcm\n",tmp.skiing_state, tmp.speed_cms, tmp.distance_cm); - // xlog("Acc:%d, %d, %d\n", tmp.acc_data[0],tmp.acc_data[1],tmp.acc_data[2]); - // xlog("GYR:%d, %d, %d\n", tmp.gyr_data[0],tmp.gyr_data[1],tmp.gyr_data[2]); - } - count++; - // xlog("=======end\n"); - #else - #endif -} - -//iic测试调用的 -#if 0 - -static u16 xt_iic_test_id; -char log_buffer_1[100]; -extern char sen_log_buffer_1[100]; -extern char sen_log_buffer_2[100]; -extern char sen_log_buffer_3[100]; -extern char sen_log_buffer_4[100]; -extern char sen_log_buffer_5[100]; -extern char w_log_buffer_1[100]; -extern char w_log_buffer_2[100]; -extern char w_log_buffer_3[100]; -extern char w_log_buffer_4[100]; -extern char w_log_buffer_5[100]; -void xt_iic_test(){ - - char log_buffer[100]; - send_data_to_ble_client(&log_buffer_1,strlen(log_buffer_1)); - extern char iic_read_len; - extern char iic_write_result; - int 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); - extern void send_data_to_ble_client(const u8* data, u16 length); - send_data_to_ble_client(&log_buffer,strlen(log_buffer)); - - if(sen_log_buffer_1 != NULL) - send_data_to_ble_client(&sen_log_buffer_1,strlen(sen_log_buffer_1)); - if(sen_log_buffer_2 != NULL) - send_data_to_ble_client(&sen_log_buffer_2,strlen(sen_log_buffer_2)); - if(sen_log_buffer_3 != NULL) - send_data_to_ble_client(&sen_log_buffer_3,strlen(sen_log_buffer_3)); - if(sen_log_buffer_4 != NULL) - send_data_to_ble_client(&sen_log_buffer_4,strlen(sen_log_buffer_4)); - if(sen_log_buffer_5 != NULL) - send_data_to_ble_client(&sen_log_buffer_5,strlen(sen_log_buffer_5)); - - if(w_log_buffer_1 != NULL) - send_data_to_ble_client(&w_log_buffer_1,strlen(w_log_buffer_1)); - if(w_log_buffer_2 != NULL) - send_data_to_ble_client(&w_log_buffer_2,strlen(w_log_buffer_2)); - if(w_log_buffer_3 != NULL) - send_data_to_ble_client(&w_log_buffer_3,strlen(w_log_buffer_3)); - if(w_log_buffer_4 != NULL) - send_data_to_ble_client(&w_log_buffer_4,strlen(w_log_buffer_4)); - if(w_log_buffer_5 != NULL) - send_data_to_ble_client(&w_log_buffer_5,strlen(w_log_buffer_5)); - - // 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 sensor_measure(void){ - // xlog("=======sensor_read_data START\n"); - static signed short combined_raw_data[6]; - static int initialized = 0; - static int calibration_done = 0; - char status = 0; - - - static sensor_data_t tmp; - mmc5603nj_mag_data_t mag_data; - SL_SC7U22_RawData_Read(tmp.acc_data,tmp.gyr_data); - // os_time_dly(1); - mmc5603nj_read_mag_data(&mag_data); - // xlog("=======sensor_read_data middle 1\n"); - memcpy(&combined_raw_data[0], tmp.acc_data, 3 * sizeof(signed short)); - memcpy(&combined_raw_data[3], tmp.gyr_data, 3 * sizeof(signed short)); - - if (!calibration_done) { //第1次启动，开启零漂检测 - // status = SL_SC7U22_Angle_Output(1, combined_raw_data, tmp.angle, 0); - // status = SIX_SL_SC7U22_Angle_Output(1, combined_raw_data, tmp.angle, 0); - // status = Original_SL_SC7U22_Angle_Output(1, combined_raw_data, tmp.angle, 0); - // status = Q_SL_SC7U22_Angle_Output(1, combined_raw_data, tmp.angle,&mag_data, 0, tmp.quaternion_output); - status = Q_SL_SC7U22_Angle_Output(1, combined_raw_data, tmp.angle,NULL, 0, tmp.quaternion_output); - - if(count > 100){ - count = 0; - char log_buffer[100]; - // snprintf( log_buffer, sizeof(log_buffer),"status:%d\n",status); - // send_data_to_ble_client(&log_buffer,strlen(log_buffer)); - xlog("status:%d\n", status); - xlog("RawData:AX=%d,AY=%d,AZ=%d,GX=%d,GY=%d,GZ=%d\r\n",combined_raw_data[0],combined_raw_data[1],combined_raw_data[2],combined_raw_data[3],combined_raw_data[4],combined_raw_data[5]); - } - count++; - - if (status == 1) { - calibration_done = 1; - printf("Sensor calibration successful! Skiing mode is active.\n"); - } - } else { - // printf("Calculate the time interval =============== start\n"); - // status = SL_SC7U22_Angle_Output(0, combined_raw_data, tmp.angle, 0); - // status = SIX_SL_SC7U22_Angle_Output(0, combined_raw_data, tmp.angle, 0); - // status = Original_SL_SC7U22_Angle_Output(0, combined_raw_data, tmp.angle, 0); - // status = Q_SL_SC7U22_Angle_Output(0, combined_raw_data, tmp.angle,&mag_data, 0, tmp.quaternion_output); - status = Q_SL_SC7U22_Angle_Output(0, combined_raw_data, tmp.angle,NULL, 0, tmp.quaternion_output); - - memcpy(tmp.acc_data, &combined_raw_data[0], 3 * sizeof(signed short)); - memcpy(tmp.gyr_data, &combined_raw_data[3], 3 * sizeof(signed short)); - BLE_send_data_t data_by_calculate = sensor_processing_task(tmp.acc_data, tmp.gyr_data,tmp.angle, tmp.quaternion_output); - extern void ano_send_attitude_data(float rol, float pit, float yaw, uint8_t fusion_sta) ; - ano_send_attitude_data(tmp.angle[0],tmp.angle[1],tmp.angle[2], 1); - } - - // mmc5603nj_mag_data_t mag_data; - // mmc5603nj_read_mag_data(&mag_data); - // float temperature = mmc5603nj_get_temperature(); - // count_test1++; - // if(count_test1 >500){ - // count_test1 =0; - // xlog("Mag X: %.4f, Y: %.4f, Z: %.4f Gauss\n", mag_data.x, mag_data.y, mag_data.z); - // } - - // xlog("=======sensor_read_data END\n"); } void xtell_task_create(void){ - // int ret = hw_iic_init(0); - // xlog("hw_iic_init result:%d\n",ret); - // //初始化传感器 - // SL_SC7U22_Config(); - #if TCFG_GSENOR_USER_IIC_TYPE - int ret = hw_iic_init(0); xlog("init iic result:%d\n", ret); //返回0成功 #else @@ -543,52 +164,13 @@ void xtell_task_create(void){ #endif - - // os_time_dly(10); - // delay_2ms(10); - - - - // if(bmp280_init() != 0){ - // xlog("bmp280 init error\n"); - // } - // float temp, press; - // bmp280_read_data(&temp, &press); - // xlog("get temp: %d, get press: %d\n",temp, press); - - - // MPU9250_Mag_Init(); //iic总线设备扫描 - // extern void i2c_scanner_probe(void); - // i2c_scanner_probe(); - + extern void i2c_scanner_probe(void); + i2c_scanner_probe(); xlog("xtell_task_create\n"); - // 初始化环形缓冲区 - // circle_buffer_init(&sensor_cb, sensor_data_buffer, SENSOR_DATA_BUFFER_SIZE); - - ano_protocol_init(115200); - - - 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)); - - - - - - // create_process(&sensor_read_data_id, "read",NULL, sensor_read_data, 10); -// - // create_process(&calculate_data_id, "calculate",NULL, calculate_data, 4); - - // create_process(&ble_send_data_id, "send",NULL, BLE_send_data, 1); - - #if 0 - hw_iic_init_result = ret; - create_process(&xt_iic_test_id,"iic_test",NULL,xt_hw_iic_test,1000); - #endif + circle_buffer_init(&BLE_send_buff, sensor_read_buffer, SENSOR_DATA_BUFFER_SIZE, sizeof(BLE_send_data_t)); } diff --git a/apps/earphone/xtell_Sensor/sensor/MMC56.c b/apps/earphone/xtell_Sensor/sensor/MMC56.c index 1d528af..e24b63f 100644 --- a/apps/earphone/xtell_Sensor/sensor/MMC56.c +++ b/apps/earphone/xtell_Sensor/sensor/MMC56.c @@ -32,7 +32,7 @@ int mmc5603nj_init(void) { // ID if ( mmc5603nj_get_pid() != 0x10) { printf("MMC5603NJ init failed: wrong Product ID (read: 0x%X)\n", mmc5603nj_get_pid()); - return -1; + return 0; } // 软件复位 @@ -58,7 +58,7 @@ int mmc5603nj_init(void) { mmc5603nj_enable_continuous_mode(0x04); - return 0; + return 1; } void mmc5603nj_start_calibration(void){ @@ -218,4 +218,42 @@ void mmc5603nj_read_mag_data(mmc5603nj_mag_data_t *mag_data) { mag_data->x -= cal_data.offset_x; mag_data->y -= cal_data.offset_y; mag_data->z -= cal_data.offset_z; +} + + +void mmc5603nj_read_origin_data(uint8_t *buffer) { + + if (g_continuous_mode_enabled) { + // 连续模式下，只需检查数据是否就绪 + uint8_t status = 0; + mmc5603nj_read_regs(MMC_STATUS1, &status, 1); + if ((status & 0x40) == 0) { // Meas_M_done bit + // 数据未就绪，可以选择返回或等待，这里我们直接返回旧数据 + return; + } + } else { + // 单次测量模式 + uint8_t status = 0; + uint8_t timeout = 20; + + // 触发一次带自动SET/RESET的磁场测量 + mmc5603nj_write_reg(MMC_INCTRL0, 0x21); // 0b00100001 (TAKE_MEAS_M=1, AUTO_SR_EN=1) + + // 等待测量完成 + do { + os_time_dly(10); + mmc5603nj_read_regs(MMC_STATUS1, &status, 1); + timeout--; + } while ((status & 0x40) == 0 && timeout > 0); + + if (timeout == 0) { + printf("Error: Magnetic measurement timeout!\n"); + return; + } + } + + // 读取9个字节的原始数据 + mmc5603nj_read_regs(MMC_XOUT0, buffer, 9); + + } \ No newline at end of file diff --git a/apps/earphone/xtell_Sensor/sensor/MMC56.h b/apps/earphone/xtell_Sensor/sensor/MMC56.h index 3ac1f3e..919063f 100644 --- a/apps/earphone/xtell_Sensor/sensor/MMC56.h +++ b/apps/earphone/xtell_Sensor/sensor/MMC56.h @@ -36,6 +36,14 @@ #define MMC_ST_Z 0x29 #define MMC_PID 0x39 +// 定义一个结构体来存放三轴磁场数据（原始数据） +typedef struct { + float x; + float y; + float z; +} mmc5603nj_original_data_t; + + // 定义一个结构体来存放三轴磁场数据（单位：高斯 Gauss） typedef struct { float x; diff --git a/apps/earphone/xtell_Sensor/sensor/SC7U22.c b/apps/earphone/xtell_Sensor/sensor/SC7U22.c index 0be8e81..4c67271 100644 --- a/apps/earphone/xtell_Sensor/sensor/SC7U22.c +++ b/apps/earphone/xtell_Sensor/sensor/SC7U22.c @@ -1167,10 +1167,19 @@ unsigned char Original_SL_SC7U22_Angle_Output(unsigned char calibration_en, sign return 2; // 校准未完成，返回错误状态 } -unsigned char get_calibration_state(void){ + +unsigned char get_SC7U22_Error_Flag(void){ return SL_SC7U22_Error_Flag; } +/** + * @brief 设置零漂检测标准位 + * + * @param flag 0：重新进行零漂检测 + */ +void set_SC7U22_Error_Flag(char flag){ + SL_SC7U22_Error_Flag = flag; +} ///////////////////////////////////////////////////////////////////////////////////////////////////////////////// diff --git a/apps/earphone/xtell_Sensor/sensor/SC7U22.h b/apps/earphone/xtell_Sensor/sensor/SC7U22.h index d8cdfce..dd88f2d 100644 --- a/apps/earphone/xtell_Sensor/sensor/SC7U22.h +++ b/apps/earphone/xtell_Sensor/sensor/SC7U22.h @@ -20,7 +20,7 @@ Copyright (c) 2022 Silan MEMS. All Rights Reserved. /***使用前请根据实际情况配置以下参数******/ /**SC7U22的SDO 接地: 0****************/ /**SC7U22的SDO 接电源:1****************/ -#define SL_SC7U22_SDO_VDD_GND 1 +#define SL_SC7U22_SDO_VDD_GND 0 /*****************************************/ /***使用前请根据实际IIC地址配置参数***/ /**SC7U22的IIC 接口地址为 7bits: 0****/ @@ -131,6 +131,7 @@ unsigned char SL_SC7U22_Angle_Output(unsigned char calibration_en,signed short * /**output Angle_output[2]: Yaw*******************************/ /**input yaw_rst: reset yaw value***************************/ +void set_SC7U22_Error_Flag(char flag); 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, const mmc5603nj_mag_data_t *mag_data_input, unsigned char yaw_rst, float *quaternion_output); diff --git a/apps/earphone/xtell_Sensor/xtell_handler.c b/apps/earphone/xtell_Sensor/xtell_handler.c index 4d9c709..0d78683 100644 --- a/apps/earphone/xtell_Sensor/xtell_handler.c +++ b/apps/earphone/xtell_Sensor/xtell_handler.c @@ -85,13 +85,21 @@ unsigned char xtell_bl_state=0; //存放经典蓝牙的连接状态，0断开 u8 bt_newname =0; unsigned char xt_ble_new_name[9] = "CM-22222"; static u16 play_poweron_ok_timer_id = 0; + +// -- 初始化标志位 -- +u8 SC7U22_init = 0; //六轴是否初始化 +u8 MMC5603nj_init = 0; //地磁是否初始化 + +// -- 线程id -- +u16 SC7U22_calibration_id; +u16 start_collect_fuc_id; // /////////////////////////////////////////////////////////////////////////////////////////////////// - - - extern int bt_hci_event_handler(struct bt_event *bt); - +extern void SC7U22_static_calibration(void); +extern void create_process(u16* pid, const char* name, void *priv, void (*func)(void *priv), u32 msec); +extern void close_process(u16* pid,char* name); +extern void start_collect_fuc(void); /////////////////////////////////////////////////////////////////////////////////////////////////// /* * 模式状态机, 通过start_app()控制状态切换 @@ -203,52 +211,56 @@ void le_user_app_event_handler(struct sys_event* event){ if (event->u.app.buffer[0] == 0xBE && event->u.app.buffer[1] == 0xBB) { if(event->u.app.buffer[2] == 0x01){ //后面的数据长度 1 switch (event->u.app.buffer[3]){ - case 0x01: - char* send_start = "will start after 5 seconds\n"; - send_data_to_ble_client(send_start,strlen(send_start)); - - if (mmc5603nj_init() != 0) { - xlog("MMC5603NJ initialization failed!\n"); - char* send_error = "calibration error\n"; - send_data_to_ble_client(send_error,strlen(send_error)); - } - xlog("MMC5603NJ PID: 0x%02X\n", mmc5603nj_get_pid()); - char* send_tmp = "8th calibration completed\n"; - send_data_to_ble_client(send_tmp,strlen(send_tmp)); - break; - case 0x02: - extern void create_process(u16* pid,char* name, void *priv, void (*func)(void *priv), u32 msec); - extern void sensor_measure(void); - static int test_id; - SL_SC7U22_Config(); - static skiing_tracker_t skiing_data; - skiing_tracker_init(&skiing_data); - create_process(&test_id, "test",NULL, sensor_measure, 10); - send_tmp = "The test has begun.\n"; - send_data_to_ble_client(send_tmp,strlen(send_tmp)); - break; - case 0x03: - extern void start_detection(void); - start_detection(); - send_tmp = "start_detection\n"; - send_data_to_ble_client(send_tmp,strlen(send_tmp)); - break; - case 0x04: - extern void stop_detection(void); - stop_detection(); - send_tmp = "stop_detection\n"; - send_data_to_ble_client(send_tmp,strlen(send_tmp)); - break; - case 0x05: - extern void clear_speed(void); - clear_speed(); - send_tmp = "Reset speed and distances to zero\n"; - send_data_to_ble_client(send_tmp,strlen(send_tmp)); - + case 0x01: break; default: break; - + } + }else if(event->u.app.buffer[2] == 0x02){ //后面数据长度为2 + switch (event->u.app.buffer[3]){ //数据包类型 + case 0x00: //数据包类型为：指定传感器初始化 + u8 send2_0[5] = {0xBB,0xBE,0x02,0x00,0x00}; + if(event->u.app.buffer[4] == 0x01){ //六轴 + if (SL_SC7U22_Config() == 0) { + send2_0[4] = 0x00; //初始化失败 + SC7U22_init = 0; + send_data_to_ble_client(&send2_0,strlen(send2_0)); + break; + } + create_process(&SC7U22_calibration_id,"SC7U22_calibration",NULL,SC7U22_static_calibration,10); + }else if(event->u.app.buffer[4] == 0x02){ //地磁 + if(mmc5603nj_init() == 0){ + MMC5603nj_init = 0; + break; + } + MMC5603nj_init = 1; + send2_0[4] = 0x03; //地磁初始化失败 + send_data_to_ble_client(&send2_0,strlen(send2_0)); + } + break; + case 0x01: //数据包类型为：获取指定传感器初始化状态 + u8 send2_1[5] = {0xBB,0xBE,0x02,0x01,0x00}; + if(event->u.app.buffer[4] == 0x01){ //六轴 + send2_1[4] = SC7U22_init; + }else if(event->u.app.buffer[4] == 0x02){ //地磁 + send2_1[4] = MMC5603nj_init + 2; + } + send_data_to_ble_client(&send2_1,strlen(send2_1)); + break; + case 0x02: //开始/停止滑雪计算 + if(event->u.app.buffer[4] == 0x01){ //开始滑雪计算 + if(SC7U22_init == 0 || MMC5603nj_init == 0){ //传感器未进行初始化 + u8 send2_2[5] = {0xBB,0xBE,0x02,0x01,0x00}; + send_data_to_ble_client(&send2_2,strlen(send2_2)); + send2_2[4] = 0x02; + send_data_to_ble_client(&send2_2,strlen(send2_2)); + return; + } + create_process(&start_collect_fuc_id,"start_collect",NULL,start_collect_fuc,10); + }else if(event->u.app.buffer[4] == 0x02){ //停止滑雪计算 + close_process(&start_collect_fuc_id,"start_collect"); + } + break; } } }