/** 功耗： 闲置模式功耗为250+ua 低功耗模式1，若前后脚地磁开启，板子地磁不开启，且不读取数据，功耗为330+ua ； 若只是读取前后脚地磁，则功耗为450+ua， 若读取前后脚地磁后再reset + 配置，则功耗为700+ua 低功耗模式2，若只开启后脚地磁，板子地磁不开启，且不读取数据，功耗为300+ua ； 若只是读取后脚地磁，则功耗为370-390+ua, 若读取后脚地磁后再reset + 配置， 则功耗为500+ua //以上低功耗测试都是没开六轴，只开地磁的情况 游戏模式spi_lsm，8.5ma 游戏模式i2c_lsm，8.7ma 前脚地磁供电关闭，后脚地磁供电开启，因为sda（标准输出）线共用，当sda输出0时，前脚地磁的供电IO和sda导通，导致短路，功耗为11ma，且后脚地磁数据异常。 所以通过开关电源来复位地磁失败！！！ 低功耗模式3，只开启后脚地磁，板子地磁不开启，读取完地磁和低功耗加速度后（都是用SPI读取），在睡眠前重新配置（不reset）后脚地磁，功耗为370 - 400ua PS：低功耗模式3中，读取低功耗加速度用SPI时，速度为152us，用模拟I2C读取，速度为457us，由于时间变长，功耗为400 - 430ua 更新时间：2021-10-12： 低功耗模式4，更改了结构板，将IMU移到前脚，同时I2C的SCL公用，SDA独立，取消每次读取地磁后reset+配置。 这时，低功耗模式（没跑算法）功耗为：350-380ua 、 闲置模式功耗为：180-270ua 、 游戏模式功耗为：12-13ma，若再扣去小板上的六轴挂起功耗(22ua)+地磁挂起功耗(9ua)的31ua， 则各个模式功耗为：低功耗模式（没跑算法）功耗为：319-349ua 、 闲置模式功耗为：149-239ua 、 游戏模式功耗为：12-13ma 更新时间：2021-11-25： IMU的FIFO读取地磁，会出现重复现象，目前猜测是IMU的HZ与地磁的HZ不同步，地磁是100HZ，IMU是104HZ，其次，地磁无出现数据0的现象。 更新时间：2021-11-29： 版本：1.9.1 板子本身功耗：12-14ua 测试，低功耗FIFO功耗（没跑日常计步算法）：537-549ua 测试，低功耗FIFO功耗（没跑日常计步算法），不读取IMUFIFO数据：454-464ua 测试，单纯设置外设，不跑IMU线程，功耗：453-462ua 测试，单纯设置外设（断电后脚外设供电,虽然其本身就是设置完后就挂起），不跑IMU线程，功耗：433-444ua 测试，断电前后脚外设，不跑IMU线程，功耗：175-224ua 测试，开启维持一个串口，功耗增加为：473ua 结论： 低功耗FIFO模式（100ms唤醒）下，不跑日常计步算法，总功耗为：537-549ua 外设功耗为：后脚地磁挂起（20ua）+前脚外设的IMU_104HZ_acc_timestamp_sensorhub_slv0123（128ua）+前脚外设地磁100HZ（150ua） 线程功耗为：IMU线程（84ua->读取IMU的FIFO数据10组，耗时1.1~1.2ms）+bat线程（5ua）+其余线程和广播（170ua） 游戏模式下（没焊接单线灯）功耗为： 设备直接进入：7.4ma 链接右鞋后，手机下发游戏指令，进入：8.5-8.6ma 更新时间：2022-3-7 版本：2.1 待机模式（不穿鞋）功耗：270ua ~ 467ua 正常模式（穿鞋）功耗：570ua ~ 630u 游戏模式功耗： */ /*Includes ----------------------------------------------*/ #include "tool.h" #include "nrf_delay.h" #include "bsp_time.h" #include "exception.h" #include "system.h" #include "drv_qmc6310_v2.h" #include "drv_lsm6ds3tr_c.h" #include "fml_imu.h" /*Private macro ------------------------------------------------------------------------------------------------------------------------------------*/ #define FML_IMU_DATA_GROUP_NUM_MAX 20 //IMU能存储的最大数据组数 #define FML_IMU_MONITOR_DATA_ERR_SUM_MAX 200 //数据监测错误累计最大值 /*STRUCTION ------------------------------------------------------------------------------------------------------------------------------------*/ typedef enum { FML_IMU_CONFIG_STAGE_DONE, //配置完成阶段 FML_IMU_CONFIG_STAGE_IN_PROGRESS, //配置进行阶段 FML_IMU_CONFIG_STAGE_FAIL, //配置失败阶段 } FML_IMU_CONFIG_STAGE_e; typedef union { drv_lsm_config_param_t lsm; //当前的LSM配置参数 drv_qmc_config_param_t qmc; //当前的QMC配置参数 } drv_param_u; typedef struct fml_imu { /*private member*/ FML_IMU_CONFIG_STAGE_e stage; //配置流程状态 drv_param_u drv_param[FML_IMU_DIR_NUM]; //驱动参数组 fml_imu_param_t config_param[FML_IMU_DIR_NUM]; //配置参数组 int cur_data_num[FML_IMU_DIR_NUM]; //当前的数据量 fml_imu_data_t cur_data[FML_IMU_DIR_NUM][FML_IMU_DATA_GROUP_NUM_MAX]; //当前的IMU数据缓存区 uint32_t config_flow; //配置流程 fml_imu_config_cb config_cb; //配置回调 fml_imu_data_notify_cb data_notify_cb; //数据通知回调 int16_t last_f_acc[3]; //上一次的前脚加速度值 int16_t last_f_gry[3]; //上一次的前脚陀螺仪值 int16_t last_f_mag[3]; //上一次的前脚地磁计值 int16_t last_b_mag[3]; //上一次的后脚地磁计值 int16_t last_f_acc_err_sum; //上一次的前脚加速度值错误累计 int16_t last_f_gry_err_sum; //上一次的前脚陀螺仪值错误累计 int16_t last_f_mag_err_sum; //上一次的前脚地磁计值错误累计 int16_t last_b_mag_err_sum; //上一次的后脚地磁计值错误累计 int16_t except_data_occur_sum[FML_IMU_DIR_NUM]; //异常数据产生次数累计 } Fml_Imu_t; /*Local Variable ------------------------------------------------------------------------------------------------------------------------------------*/ static Fml_Imu_t ob_fml_imu; /*Local Functions ------------------------------------------------------------------------------------------------------------------------------------*/ static void fml_imu_macro_conversion(FML_IMU_DIR_e dir, fml_imu_param_t *config_param, drv_param_u *drv_param) { switch(dir) { case FML_IMU_DIR_FRONT: //acc_odr switch(config_param->acc_odr) { case FML_IMU_ACC_ODR_OFF: drv_param->lsm.acc_odr = LSM_ACC_ODR_OFF; break; case FML_IMU_ACC_ODR_12HZ5: drv_param->lsm.acc_odr = LSM_ACC_ODR_12HZ5; break; case FML_IMU_ACC_ODR_104HZ: drv_param->lsm.acc_odr = LSM_ACC_ODR_104HZ; break; case FML_IMU_ACC_ODR_208HZ: drv_param->lsm.acc_odr = LSM_ACC_ODR_208HZ; break; case FML_IMU_ACC_ODR_416HZ: drv_param->lsm.acc_odr = LSM_ACC_ODR_416HZ; break; case FML_IMU_ACC_ODR_833HZ: drv_param->lsm.acc_odr = LSM_ACC_ODR_833HZ; break; } //gry_odr switch(config_param->gry_odr) { case FML_IMU_GRY_ODR_OFF: drv_param->lsm.gry_odr = LSM_GRY_ODR_OFF; break; case FML_IMU_GRY_ODR_12HZ5: drv_param->lsm.gry_odr = LSM_GRY_ODR_12HZ5; break; case FML_IMU_GRY_ODR_104HZ: drv_param->lsm.gry_odr = LSM_GRY_ODR_104HZ; break; case FML_IMU_GRY_ODR_208HZ: drv_param->lsm.gry_odr = LSM_GRY_ODR_208HZ; break; case FML_IMU_GRY_ODR_416HZ: drv_param->lsm.gry_odr = LSM_GRY_ODR_416HZ; break; case FML_IMU_GRY_ODR_833HZ: drv_param->lsm.gry_odr = LSM_GRY_ODR_833HZ; break; } //mag_odr switch(config_param->mag_odr) { case FML_IMU_MAG_ODR_OFF: drv_param->lsm.mag_odr = LSM_MAG_ODR_OFF; break; case FML_IMU_MAG_ODR_10HZ: drv_param->lsm.mag_odr = LSM_MAG_ODR_10HZ; break; case FML_IMU_MAG_ODR_100HZ: drv_param->lsm.mag_odr = LSM_MAG_ODR_100HZ; break; case FML_IMU_MAG_ODR_200HZ: drv_param->lsm.mag_odr = LSM_MAG_ODR_200HZ; break; } //fifo_odr switch(config_param->fifo_odr) { case FML_IMU_FIFO_ODR_OFF: drv_param->lsm.fifo_odr = LSM_FIFO_ODR_OFF; break; case FML_IMU_FIFO_ODR_12HZ5: drv_param->lsm.fifo_odr = LSM_FIFO_ODR_12HZ5; break; case FML_IMU_FIFO_ODR_104HZ: drv_param->lsm.fifo_odr = LSM_FIFO_ODR_104HZ; break; case FML_IMU_FIFO_ODR_208HZ: drv_param->lsm.fifo_odr = LSM_FIFO_ODR_208HZ; break; case FML_IMU_FIFO_ODR_416HZ: drv_param->lsm.fifo_odr = LSM_FIFO_ODR_416HZ; break; case FML_IMU_FIFO_ODR_833HZ: drv_param->lsm.fifo_odr = LSM_FIFO_ODR_833HZ; break; } //acc_power_mode switch(config_param->acc_power_mode) { case FML_IMU_ACC_POWER_MODE_HIGH_PERFORMANCE: drv_param->lsm.acc_power_mode = LSM_ACC_POWER_MODE_HIGH_PERFORMANCE; break; case FML_IMU_ACC_POWER_MODE_NORMAL: drv_param->lsm.acc_power_mode = LSM_ACC_POWER_MODE_NORMAL; break; } //gry_power_mode switch(config_param->gry_power_mode) { case FML_IMU_GRY_POWER_MODE_HIGH_PERFORMANCE: drv_param->lsm.gry_power_mode = LSM_GRY_POWER_MODE_HIGH_PERFORMANCE; break; case FML_IMU_GRY_POWER_MODE_NORMAL: drv_param->lsm.gry_power_mode = LSM_GRY_POWER_MODE_NORMAL; break; } //acc_fs switch(config_param->acc_fs) { case FML_IMU_ACC_FS_2G: drv_param->lsm.acc_fs = LSM_ACC_FS_2G; break; case FML_IMU_ACC_FS_16G: drv_param->lsm.acc_fs = LSM_ACC_FS_16G; break; } //gry_fs switch(config_param->gry_fs) { case FML_IMU_GRY_FS_250DPS: drv_param->lsm.gry_fs = LSM_GRY_FS_250DPS; break; case FML_IMU_GRY_FS_2000DPS: drv_param->lsm.gry_fs = LSM_GRY_FS_2000DPS; break; } //mag_fs switch(config_param->mag_fs) { case FML_IMU_MAG_FS_30GS: drv_param->lsm.mag_fs = LSM_MAG_FS_30GS; break; } //timestamp_resolution switch(config_param->timestamp_resolution) { case FML_IMU_TIMESTAMP_6MS4: drv_param->lsm.timestamp_resolution = LSM_TIMESTAMP_6MS4; break; case FML_IMU_TIMESTAMP_25US: drv_param->lsm.timestamp_resolution = LSM_TIMESTAMP_25US; break; } //timestamp_switch switch(config_param->timestamp_switch) { case FML_IMU_TIMESTAMP_OFF: drv_param->lsm.timestamp_switch = LSM_TIMESTAMP_OFF; break; case FML_IMU_TIMESTAMP_ON: drv_param->lsm.timestamp_switch = LSM_TIMESTAMP_ON; break; } break; case FML_IMU_DIR_BACK: //mag_odr switch(config_param->mag_odr) { case FML_IMU_MAG_ODR_OFF: drv_param->qmc.mag_odr = QMC_MAG_ODR_OFF; break; case FML_IMU_MAG_ODR_10HZ: drv_param->qmc.mag_odr = QMC_MAG_ODR_10HZ; break; case FML_IMU_MAG_ODR_100HZ: drv_param->qmc.mag_odr = QMC_MAG_ODR_100HZ; break; case FML_IMU_MAG_ODR_200HZ: drv_param->qmc.mag_odr = QMC_MAG_ODR_200HZ; break; } //mag_fs switch(config_param->mag_fs) { case FML_IMU_MAG_FS_30GS: drv_param->qmc.mag_fs = QMC_MAG_FS_30GS; break; } break; default: break; } } static int fml_imu_intergrated_setting(void) { int ret; int lsm_mag_flow = drv_lsm_get_mag_odr_flow(); int qmc_mag_flow = drv_qmc6310_get_mag_odr_flow(); drv_param_u lsm_drv_param; drv_param_u qmc_drv_param; static uint32_t tim = 0; switch(ob_fml_imu.config_flow) { case 0: //断电lsm ret = drv_lsm_power_off(); if(ret != 0)return -1; //断电qmc ret = drv_qmc6310_power_off(); if(ret != 0)return -1; //清空读取缓存 ob_fml_imu.cur_data_num[FML_IMU_DIR_FRONT] = 0; //清空读取缓存 ob_fml_imu.cur_data_num[FML_IMU_DIR_BACK] = 0; tim = 0; ob_fml_imu.config_flow = 1; break; case 1: //等待200ms if(tim == 0){ tim = TIME_GetTicks(); }else if(TIME_GetTicks()-tim>=200){ tim = 0; ob_fml_imu.config_flow = 2; } break; case 2: //上电lsm ret = drv_lsm_power_on(); if(ret != 0)return -1; //上电qmc ret = drv_qmc6310_power_on(); if(ret != 0)return -1; tim = 0; ob_fml_imu.config_flow = 3; break; case 3: //等待200ms if(tim == 0){ tim = TIME_GetTicks(); }else if(TIME_GetTicks()-tim>=200){ tim = 0; ob_fml_imu.config_flow = 4; } break; case 4: //lsm进行自检步骤1 ret = drv_lsm_self_check_1(); if(ret != 0)return -1; tim = 0; ob_fml_imu.config_flow = 5; break; case 5: //等待100ms if(tim == 0){ tim = TIME_GetTicks(); }else if(TIME_GetTicks()-tim>=100){ tim = 0; ob_fml_imu.config_flow = 6; } break; case 6: //lsm进行自检步骤2 ret = drv_lsm_self_check_2(); if(ret != 0)return -1; tim = 0; ob_fml_imu.config_flow = 7; break; case 7: //等待100ms if(tim == 0){ tim = TIME_GetTicks(); }else if(TIME_GetTicks()-tim>=100){ tim = 0; ob_fml_imu.config_flow = 8; } break; case 8: //lsm进行自检步骤3 ret = drv_lsm_self_check_3(); if(ret != 0)return -1; tim = 0; ob_fml_imu.config_flow = 9; break; case 9: //等待150ms if(tim == 0){ tim = TIME_GetTicks(); }else if(TIME_GetTicks()-tim>=150){ tim = 0; ob_fml_imu.config_flow = 10; } break; case 10: //lsm进行自检步骤4 ret = drv_lsm_self_check_4(); if(ret != 0)return -1; tim = 0; ob_fml_imu.config_flow = 11; break; case 11: //等待50ms if(tim == 0){ tim = TIME_GetTicks(); }else if(TIME_GetTicks()-tim>=50){ tim = 0; ob_fml_imu.config_flow = 12; } break; case 12: //lsm进行自检步骤5 ret = drv_lsm_self_check_5(); if(ret != 0)return -1; tim = 0; ob_fml_imu.config_flow = 13; break; case 13: //转换参数，用于配置驱动 fml_imu_macro_conversion(FML_IMU_DIR_FRONT, &ob_fml_imu.config_param[FML_IMU_DIR_FRONT], &lsm_drv_param); fml_imu_macro_conversion(FML_IMU_DIR_BACK, &ob_fml_imu.config_param[FML_IMU_DIR_BACK], &qmc_drv_param); //配置lsm的地磁计采样频率 if(ob_fml_imu.config_param[FML_IMU_DIR_FRONT].fifo_odr != FML_IMU_FIFO_ODR_OFF) { for(int i=1; i <=lsm_mag_flow;i++) { ret = drv_lsm_set_mag_odr(lsm_drv_param.lsm.mag_odr, 1, i); if(ret != 0)return -1; } tim = 0; ob_fml_imu.config_flow = 14; } else { for(int i=1; i <= lsm_mag_flow; i++) { ret = drv_lsm_set_mag_odr(lsm_drv_param.lsm.mag_odr, 0, i); if(ret != 0)return -1; } tim = 0; ob_fml_imu.config_flow = 14; } break; case 14: //转换参数，用于配置驱动 fml_imu_macro_conversion(FML_IMU_DIR_FRONT, &ob_fml_imu.config_param[FML_IMU_DIR_FRONT], &lsm_drv_param); fml_imu_macro_conversion(FML_IMU_DIR_BACK, &ob_fml_imu.config_param[FML_IMU_DIR_BACK], &qmc_drv_param); //配置lsm的陀螺仪采样频率 ret = drv_lsm_set_gry_odr(lsm_drv_param.lsm.gry_odr); if(ret != 0)return -1; //配置lsm的加速度采样频率 ret = drv_lsm_set_acc_odr(lsm_drv_param.lsm.acc_odr); if(ret != 0)return -1; tim = 0; ob_fml_imu.config_flow = 15; break; case 15: //等待20ms if(tim == 0){ tim = TIME_GetTicks(); }else if(TIME_GetTicks()-tim>=20){ tim = 0; ob_fml_imu.config_flow = 16; } break; case FML_IMU_CONFIG_FLOW_DONE: //配置完成才能进来此处。。。。。。。 break; default: //转换参数，用于配置驱动 fml_imu_macro_conversion(FML_IMU_DIR_FRONT, &ob_fml_imu.config_param[FML_IMU_DIR_FRONT], &lsm_drv_param); fml_imu_macro_conversion(FML_IMU_DIR_BACK, &ob_fml_imu.config_param[FML_IMU_DIR_BACK], &qmc_drv_param); //配置lsm的加速度工作模式 ret = drv_lsm_set_acc_power_mode(lsm_drv_param.lsm.acc_power_mode); if(ret != 0)return -1; ob_fml_imu.config_flow++; //配置lsm的陀螺仪工作模式 ret = drv_lsm_set_gry_power_mode(lsm_drv_param.lsm.gry_power_mode); if(ret != 0)return -1; ob_fml_imu.config_flow++; //配置lsm的加速度量程 ret = drv_lsm_set_acc_fs(lsm_drv_param.lsm.acc_fs); if(ret != 0)return -1; ob_fml_imu.config_flow++; //配置lsm的陀螺仪量程 ret = drv_lsm_set_gry_fs(lsm_drv_param.lsm.gry_fs); if(ret != 0)return -1; ob_fml_imu.config_flow++; //配置lsm的时间戳精度 ret = drv_lsm_set_timestamp_resolution(lsm_drv_param.lsm.timestamp_resolution); if(ret != 0)return -1; ob_fml_imu.config_flow++; //配置lsm的时间戳开关 ret = drv_lsm_set_timestamp_switch(lsm_drv_param.lsm.timestamp_switch); if(ret != 0)return -1; ob_fml_imu.config_flow++; //配置lsm的FIFO采样频率（必须以上都配置成功，才能配置FIFO） ret = drv_lsm_set_fifo_odr(lsm_drv_param.lsm.fifo_odr, \ lsm_drv_param.lsm.acc_odr, \ lsm_drv_param.lsm.gry_odr, \ lsm_drv_param.lsm.mag_odr, \ lsm_drv_param.lsm.timestamp_switch); if(ret != 0)return -1; ob_fml_imu.config_flow++; //配置qmc的地磁计采样频率 for(int i = 1; i <=qmc_mag_flow; i++) { ret = drv_qmc6310_set_mag_odr(qmc_drv_param.qmc.mag_odr,i); if(ret != 0)return -1; ob_fml_imu.config_flow++; } //配置完成！ ob_fml_imu.config_flow = FML_IMU_CONFIG_FLOW_DONE; break; } return 0; } static int fml_imu_read_data_lsm(void) { int i; int fifo_group_num; lsm_data_t temp_lsm_data; //读取FIFO数据 if(ob_fml_imu.config_param[FML_IMU_DIR_FRONT].fifo_odr != FML_IMU_FIFO_ODR_OFF) { //获取当前fifo里存在多少组数据。 fifo_group_num = drv_lsm_get_fifo_group_num(); fifo_group_num = fifo_group_num <= FML_IMU_DATA_GROUP_NUM_MAX ? fifo_group_num : FML_IMU_DATA_GROUP_NUM_MAX; if(fifo_group_num > 0) { for(i=0; i= FML_IMU_MONITOR_DATA_ERR_SUM_MAX){ Except_SetExceptype(EXCEPT_DATA_FRONT_ACC); } }else{ ob_fml_imu.last_f_acc_err_sum = 0; } ob_fml_imu.last_f_acc[0] = f_acc[0]; ob_fml_imu.last_f_acc[1] = f_acc[1]; ob_fml_imu.last_f_acc[2] = f_acc[2]; } /*前脚陀螺仪*/ if(f_gry != NULL) { if( ob_fml_imu.last_f_gry[0] == f_gry[0] && \ ob_fml_imu.last_f_gry[1] == f_gry[1] && \ ob_fml_imu.last_f_gry[2] == f_gry[2] ) { ob_fml_imu.last_f_gry_err_sum++; if(ob_fml_imu.last_f_gry_err_sum >= FML_IMU_MONITOR_DATA_ERR_SUM_MAX){ Except_SetExceptype(EXCEPT_DATA_FRONT_GRY); } }else{ ob_fml_imu.last_f_gry_err_sum = 0; } ob_fml_imu.last_f_gry[0] = f_gry[0]; ob_fml_imu.last_f_gry[1] = f_gry[1]; ob_fml_imu.last_f_gry[2] = f_gry[2]; } /*前脚地磁计*/ if(f_mag != NULL) { if( ob_fml_imu.last_f_mag[0] == f_mag[0] && \ ob_fml_imu.last_f_mag[1] == f_mag[1] && \ ob_fml_imu.last_f_mag[2] == f_mag[2] ) { ob_fml_imu.last_f_mag_err_sum++; if(ob_fml_imu.last_f_mag_err_sum >= FML_IMU_MONITOR_DATA_ERR_SUM_MAX){ Except_SetExceptype(EXCEPT_DATA_FRONT_MAG); } }else{ ob_fml_imu.last_f_mag_err_sum = 0; } ob_fml_imu.last_f_mag[0] = f_mag[0]; ob_fml_imu.last_f_mag[1] = f_mag[1]; ob_fml_imu.last_f_mag[2] = f_mag[2]; } /*后脚地磁计*/ if(b_mag != NULL) { if( ob_fml_imu.last_b_mag[0] == b_mag[0] && \ ob_fml_imu.last_b_mag[1] == b_mag[1] && \ ob_fml_imu.last_b_mag[2] == b_mag[2] ) { ob_fml_imu.last_b_mag_err_sum++; if(ob_fml_imu.last_b_mag_err_sum >= FML_IMU_MONITOR_DATA_ERR_SUM_MAX){ Except_SetExceptype(EXCEPT_DATA_BACK_MAG); } }else{ ob_fml_imu.last_b_mag_err_sum = 0; } ob_fml_imu.last_b_mag[0] = b_mag[0]; ob_fml_imu.last_b_mag[1] = b_mag[1]; ob_fml_imu.last_b_mag[2] = b_mag[2]; } //监测到前脚数据异常 if(Except_IsError(EXCEPT_DATA_FRONT_ACC) || Except_IsError(EXCEPT_DATA_FRONT_GRY) || Except_IsError(EXCEPT_DATA_FRONT_MAG)) { ob_fml_imu.except_data_occur_sum[FML_IMU_DIR_FRONT]++; if(ob_fml_imu.except_data_occur_sum[FML_IMU_DIR_FRONT] > 1) { if(Except_IsError(EXCEPT_DATA_FRONT_ACC)){Except_TxError(EXCEPT_DATA_FRONT_ACC,"front_acc_data_error");DEBUG_LOG("front_acc_data_error\n");} if(Except_IsError(EXCEPT_DATA_FRONT_GRY)){Except_TxError(EXCEPT_DATA_FRONT_GRY,"front_gry_data_error");DEBUG_LOG("front_gry_data_error\n");} if(Except_IsError(EXCEPT_DATA_FRONT_MAG)){Except_TxError(EXCEPT_DATA_FRONT_MAG,"front_mag_data_error");DEBUG_LOG("front_mag_data_error\n");} } //重启前脚LSM（主要是复位结构体） drv_lsm_power_off(); drv_lsm_power_on(); //重新开始配置 fml_imu_start_config(); //清除异常 Except_ClearExceptype(EXCEPT_DATA_FRONT_ACC); Except_ClearExceptype(EXCEPT_DATA_FRONT_GRY); Except_ClearExceptype(EXCEPT_DATA_FRONT_MAG); } else { ob_fml_imu.except_data_occur_sum[FML_IMU_DIR_FRONT] = 0; //清除异常 Except_ClearExceptype(EXCEPT_DATA_FRONT_ACC); Except_ClearExceptype(EXCEPT_DATA_FRONT_GRY); Except_ClearExceptype(EXCEPT_DATA_FRONT_MAG); } //监测到后脚数据异常 if(Except_IsError(EXCEPT_DATA_BACK_MAG)) { ob_fml_imu.except_data_occur_sum[FML_IMU_DIR_BACK]++; if(ob_fml_imu.except_data_occur_sum[FML_IMU_DIR_BACK] > 1) { if(Except_IsError(EXCEPT_DATA_BACK_MAG))Except_TxError(EXCEPT_DATA_BACK_MAG,"back_mag_data_error"); } //重启后脚QMC（主要是复位结构体） drv_qmc6310_power_off(); drv_qmc6310_power_on(); //重新开始配置 fml_imu_start_config(); //清除异常 Except_ClearExceptype(EXCEPT_DATA_BACK_MAG); } else { ob_fml_imu.except_data_occur_sum[FML_IMU_DIR_BACK] = 0; //清除异常 Except_ClearExceptype(EXCEPT_DATA_BACK_MAG); } } static void monitor_sensor_no_data(int cur_front_data_num, int cur_back_data_num) { static uint32_t front_no_data_tim = 0; static uint32_t back_no_data_tim = 0; static char buf[255]; //数据异常检测——前脚没有数据 if(cur_front_data_num == 0) { if(ob_fml_imu.drv_param[FML_IMU_DIR_FRONT].lsm.acc_odr != LSM_ACC_ODR_OFF || \ ob_fml_imu.drv_param[FML_IMU_DIR_FRONT].lsm.gry_odr != LSM_GRY_ODR_OFF || \ ob_fml_imu.drv_param[FML_IMU_DIR_FRONT].lsm.mag_odr != LSM_MAG_ODR_OFF) { //持续1000ms没数据 if(front_no_data_tim == 0){ front_no_data_tim = TIME_GetTicks(); }else if(TIME_GetTicks()-front_no_data_tim>=1000){ sprintf(buf,"Front Sensor short circuit\r\n"); Except_TxError(EXCEPT_IMU_SUSPEND_OVERFLOW,buf); front_no_data_tim = 0; //重启前脚LSM（主要是复位结构体） drv_lsm_power_off(); drv_lsm_power_on(); //重新开始配置 fml_imu_start_config(); } } } else front_no_data_tim = 0; //数据异常检测——后脚没有数据 if(cur_back_data_num == 0) { if(ob_fml_imu.drv_param[FML_IMU_DIR_BACK].qmc.mag_odr != QMC_MAG_ODR_OFF) { //持续1000ms没数据 if(back_no_data_tim == 0){ back_no_data_tim = TIME_GetTicks(); }else if(TIME_GetTicks()-back_no_data_tim>=1000){ sprintf(buf,"Back Sensor short circuit\r\n"); Except_TxError(EXCEPT_IMU_SUSPEND_OVERFLOW,buf); back_no_data_tim = 0; //重启后脚QMC（主要是复位结构体） drv_qmc6310_power_off(); drv_qmc6310_power_on(); //重新开始配置 fml_imu_start_config(); } } } else back_no_data_tim = 0; } static void fml_imu_monitor_sensor_data_process(int cur_front_data_num, int cur_back_data_num) { int i = 0; int16_t group_num = 0; static int16_t f_acc[3]; static int16_t f_gry[3]; static int16_t f_mag[3]; static int16_t b_mag[3]; fml_imu_data_t data; static uint32_t last_tim = 0; //检测数据异常——没数据 monitor_sensor_no_data(cur_front_data_num, cur_back_data_num); //检测数据异常——重复数据 if( ob_fml_imu.stage == FML_IMU_CONFIG_STAGE_DONE && \ ob_fml_imu.drv_param[FML_IMU_DIR_FRONT].lsm.acc_odr == LSM_ACC_ODR_104HZ && \ ob_fml_imu.drv_param[FML_IMU_DIR_FRONT].lsm.gry_odr == LSM_GRY_ODR_104HZ && \ (ob_fml_imu.drv_param[FML_IMU_DIR_FRONT].lsm.mag_odr == LSM_MAG_ODR_200HZ || ob_fml_imu.drv_param[FML_IMU_DIR_FRONT].lsm.mag_odr == LSM_MAG_ODR_100HZ) ) { if(TIME_GetTicks()-last_tim >= FullPower_Interval) //监测前脚传感器数据（acc + gry + mag）+ 监测后脚传感器数据（mag） { last_tim = TIME_GetTicks(); group_num = fml_imu_get_data_num(FML_IMU_DIR_FRONT); if(group_num > 0) { for(i=0;i= 1){ fml_imu_get_data(FML_IMU_DIR_BACK, 0, &data); b_mag[0] = data.mag[0];b_mag[1] = data.mag[1];b_mag[2] = data.mag[2]; monitor_sensor_data(NULL, NULL, NULL, b_mag); } } } else if( ob_fml_imu.stage == FML_IMU_CONFIG_STAGE_DONE && \ ob_fml_imu.drv_param[FML_IMU_DIR_FRONT].lsm.acc_odr == LSM_ACC_ODR_104HZ && \ (ob_fml_imu.drv_param[FML_IMU_DIR_FRONT].lsm.mag_odr == LSM_MAG_ODR_200HZ || ob_fml_imu.drv_param[FML_IMU_DIR_FRONT].lsm.mag_odr == LSM_MAG_ODR_100HZ) ) { if(TIME_GetTicks()-last_tim >= LowPower_Interval) //监测前脚传感器数据（acc + mag） { last_tim = TIME_GetTicks(); group_num = fml_imu_get_data_num(FML_IMU_DIR_FRONT); if(group_num > 0) { for(i=0;i= StandByPower_Interval) //监测前脚传感器数据（acc） { last_tim = TIME_GetTicks(); group_num = fml_imu_get_data_num(FML_IMU_DIR_FRONT); if(group_num > 0) { for(i=0;i 0){ ob_fml_imu.cur_data_num[FML_IMU_DIR_FRONT] = data_num[FML_IMU_DIR_FRONT]; dir_bit |= (1 << FML_IMU_DIR_FRONT); } //读取后脚IMU数据 + 更新后脚IMU数据量 data_num[FML_IMU_DIR_BACK] = hal_ser_imu_read_data(FML_IMU_DIR_BACK); if(data_num[FML_IMU_DIR_BACK] > 0){ ob_fml_imu.cur_data_num[FML_IMU_DIR_BACK] = data_num[FML_IMU_DIR_BACK]; dir_bit |= (1 << FML_IMU_DIR_BACK); } //数据异常检测 fml_imu_monitor_sensor_data_process(data_num[FML_IMU_DIR_FRONT], data_num[FML_IMU_DIR_BACK]); //数据回调通知 if(dir_bit != 0 && ob_fml_imu.data_notify_cb != NULL)ob_fml_imu.data_notify_cb(dir_bit); break; case FML_IMU_CONFIG_STAGE_IN_PROGRESS: //配置进行阶段 if(fml_imu_intergrated_setting() == -1) { //配置失败 ob_fml_imu.stage = FML_IMU_CONFIG_STAGE_FAIL; //通知配置失败 if(ob_fml_imu.config_cb != NULL)ob_fml_imu.config_cb(ob_fml_imu.config_flow); //上报异常 sprintf(buf,"front_imu_suspend_overflow:0x%x\r\n",ob_fml_imu.config_flow); Except_TxError(EXCEPT_IMU_SUSPEND_OVERFLOW,buf); //断电前后脚 fml_imu_close(FML_IMU_DIR_FRONT); fml_imu_close(FML_IMU_DIR_BACK); //解除全功率运行 Process_SetHoldOn(fml_imu_Process,0); } else { if(ob_fml_imu.config_flow == FML_IMU_CONFIG_FLOW_DONE) { //配置成功 ob_fml_imu.stage = FML_IMU_CONFIG_STAGE_DONE; //通知配置成功 if(ob_fml_imu.config_cb != NULL)ob_fml_imu.config_cb(ob_fml_imu.config_flow); //更新驱动LSM配置参数 drv_lsm_get_config_param(&ob_fml_imu.drv_param[FML_IMU_DIR_FRONT].lsm); //更新驱动LSM配置参数 drv_qmc6310_get_config_param(&ob_fml_imu.drv_param[FML_IMU_DIR_BACK].qmc); //解除全功率运行 Process_SetHoldOn(fml_imu_Process,0); } } break; default: //间隔1000ms上报配置失败错误 if(config_err_report_tim == 0){ config_err_report_tim = TIME_GetTicks(); }else if(TIME_GetTicks()-config_err_report_tim>=1000){ sprintf(buf,"front_imu_suspend_overflow:0x%x\r\n",ob_fml_imu.config_flow); Except_TxError(EXCEPT_IMU_SUSPEND_OVERFLOW,buf); config_err_report_tim = 0; } break; } } /*API ------------------------------------------------------------------------------------------------------------------------------------*/ /** @brief 初始化IMU功能模块 @param 无 @return 错误代码 - [out] -1失败，0成功 */ int fml_imu_Init(void) { int ret; /***************************************驱动层初始化***************************************************/ //重置结构体 memset(&ob_fml_imu,0,sizeof(ob_fml_imu)); //初始化驱动LSM ret = drv_lsm_Init(); //初始化驱动QMC ret += drv_qmc6310_Init(); //获取驱动LSM配置参数 drv_lsm_get_config_param(&ob_fml_imu.drv_param[FML_IMU_DIR_FRONT].lsm); //获取驱动LSM配置参数 drv_qmc6310_get_config_param(&ob_fml_imu.drv_param[FML_IMU_DIR_BACK].qmc); /***************************************业务逻辑层初始化***************************************************/ //初始化IMU服务结构体 ob_fml_imu.stage = FML_IMU_CONFIG_STAGE_DONE; //设置驱动配置线程 Process_Start(0,"fml_imu",fml_imu_Process); if(ret < 0)return -1; return 0; } /** @brief 设置需要配置IMU的参数 @param param - [in] 设置IMU的参数 @return 错误代码 - [out] -1失败，0成功 */ int fml_imu_config_param(FML_IMU_DIR_e dir, const fml_imu_param_t *param) { if(param == NULL || dir >= FML_IMU_DIR_NUM)return -1; memcpy(&ob_fml_imu.config_param[dir],param,sizeof(fml_imu_param_t)); return 0; } /** @brief 注册IMU配置结果回调函数 @param cb - [in] 回调函数 @return 错误代码 - [out] -1失败，0成功 */ int fml_imu_register_config_callback(fml_imu_config_cb cb) { if(cb == NULL)return -1; ob_fml_imu.config_cb = cb; return 0; } /** @brief 注册IMU数据通知回调函数 @param cb - [in] 回调函数 @return 错误代码 - [out] -1失败，0成功 */ int fml_imu_register_data_notify_callback(fml_imu_data_notify_cb cb) { if(cb == NULL)return -1; ob_fml_imu.data_notify_cb = cb; return 0; } /** @brief 开始配置IMU @param param 无 @return 返回配置状态： - [out] -1不需要重新配置，0需要重新配置 */ int fml_imu_start_config(void) { drv_param_u lsm_drv_param; drv_param_u qmc_drv_param; //获取驱动LSM配置参数 drv_lsm_get_config_param(&ob_fml_imu.drv_param[FML_IMU_DIR_FRONT].lsm); //获取驱动QMC配置参数 drv_qmc6310_get_config_param(&ob_fml_imu.drv_param[FML_IMU_DIR_BACK].qmc); //转换参数 fml_imu_macro_conversion(FML_IMU_DIR_FRONT, &ob_fml_imu.config_param[FML_IMU_DIR_FRONT], &lsm_drv_param); fml_imu_macro_conversion(FML_IMU_DIR_BACK, &ob_fml_imu.config_param[FML_IMU_DIR_BACK], &qmc_drv_param); //进行对比 if((memcmp(&ob_fml_imu.drv_param[FML_IMU_DIR_FRONT].lsm,&lsm_drv_param.lsm,sizeof(lsm_drv_param.lsm)) != 0) || \ (memcmp(&ob_fml_imu.drv_param[FML_IMU_DIR_BACK].qmc,&qmc_drv_param.qmc,sizeof(qmc_drv_param.qmc)) != 0) ) { ob_fml_imu.stage = FML_IMU_CONFIG_STAGE_IN_PROGRESS; ob_fml_imu.config_flow = 0; //全功率运行进行配置 Process_SetHoldOn(fml_imu_Process,1); return 0; } return -1; } /** @brief 读取当前IMU数据的数量 @param dir - [in] 方向 @return 返回当前IMU数据的数量 */ int fml_imu_get_data_num(FML_IMU_DIR_e dir) { if(dir >= FML_IMU_DIR_NUM)return 0; return ob_fml_imu.cur_data_num[dir]; } /** @brief 获取当前IMU数据 @param dir - [in] 方向 @param index - [in] 数据索引 @param pdata - [out] 返回的IMU数据指针 @return 错误代码 - [out] -1失败，0成功 */ int fml_imu_get_data(FML_IMU_DIR_e dir, int index, fml_imu_data_t *pdata) { if(index < 0 || pdata == NULL || dir >= FML_IMU_DIR_NUM)return -1; *pdata = ob_fml_imu.cur_data[dir][index]; return 0; } /** @brief 关闭IMU @param dir - [in] 方向 @return 错误代码 - [out] -1失败，0成功 */ int fml_imu_close(FML_IMU_DIR_e dir) { if(dir >= FML_IMU_DIR_NUM)return -1; switch(dir) { case FML_IMU_DIR_FRONT: drv_lsm_power_on(); //主要是复位结构体 drv_lsm_power_off(); memset(&ob_fml_imu.config_param[FML_IMU_DIR_FRONT],0,sizeof(ob_fml_imu.config_param[FML_IMU_DIR_FRONT])); //获取驱动LSM配置参数 drv_lsm_get_config_param(&ob_fml_imu.drv_param[FML_IMU_DIR_FRONT].lsm); //配置完成！ ob_fml_imu.config_flow = FML_IMU_CONFIG_FLOW_DONE; break; case FML_IMU_DIR_BACK: drv_qmc6310_power_on(); //主要是复位结构体 drv_qmc6310_power_off(); memset(&ob_fml_imu.config_param[FML_IMU_DIR_BACK],0,sizeof(ob_fml_imu.config_param[FML_IMU_DIR_BACK])); //获取驱动LSM配置参数 drv_qmc6310_get_config_param(&ob_fml_imu.drv_param[FML_IMU_DIR_BACK].qmc); //配置完成！ ob_fml_imu.config_flow = FML_IMU_CONFIG_FLOW_DONE; break; default: break; } return 0; }