/** 功耗： 闲置模式功耗为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 "exception.h" #include "system.h" #include "hal_ble_uart0.h" #include "hal_ser_imu_mode_manage.h" #include "hal_ser_imu.h" #include "bsp_time.h" #include "nrf_delay.h" #include "ble_comm.h" #include "app_flash.h" /*Private macro ------------------------------------------------------------------------------------------------------------------------------------*/ #define SER_IMU_HANDLE_NUM_MAX HAL_SER_IMU_MODE_MANAGE_NUM //允许最大的句柄数量 #define INVALID_HANDLE 0x00 //无效句柄 #define VALID_HANDLE 0x01 //有效句柄 #define MAX(a,b) (((a) > (b)) ? (a) : (b)) //对比大小宏 #define IS_EQUAL(a,b) (((a) == (b)) ? true : false) //对比相等宏 #define SER_IMU_DATA_GROUP_NUM_MAX 20 //服务IMU最大数据组数 #define MONITOR_DATA_ERR_SUM_MAX 200 //数据监测错误累计最大值 #define MONITOR_SUSPEND_MODE_OVERFLOW_ERR_SUM_MAX 10 //异常挂起模式持续错误累计最大值 #define DATA_UPDATE_NOTIFY_MAX 32 //数据更新通知回调最大数 /*STRUCTION ------------------------------------------------------------------------------------------------------------------------------------*/ typedef enum { IMU_CONFIG_STAGE_DONE, //配置完成阶段 IMU_CONFIG_STAGE_IN_PROGRESS, //配置进行阶段 } IMU_CONFIG_STAGE_e; typedef struct { int id; //句柄id char *name; //句柄名称 ser_imu_config_param_t op_param[SER_IMU_DIR_NUM]; //操作的IMU配置参数副本 } ser_imu_handle_t; typedef union { drv_lsm_config_param_t lsm; //当前的LSM配置参数 drv_qmc_config_param_t qmc; //当前的QMC配置参数 } drv_param_u; typedef struct ser_imu { /*private member*/ IMU_CONFIG_STAGE_e stage; //配置流程状态 bool is_need_config[SER_IMU_DIR_NUM]; //各个方位的IMU是否需要配置 ser_imu_handle_t handle[SER_IMU_HANDLE_NUM_MAX]; //句柄组 drv_param_u drv_param[SER_IMU_DIR_NUM]; //驱动配置参数组 ser_imu_config_param_t compre_drv_param[SER_IMU_DIR_NUM]; //综合的驱动配置参数 int cur_data_num[SER_IMU_DIR_NUM]; //当前的数据量 ser_imu_data_t cur_data[SER_IMU_DIR_NUM][SER_IMU_DATA_GROUP_NUM_MAX]; //当前的IMU数据缓存区 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[SER_IMU_DIR_NUM]; //异常数据产生次数累计 int16_t except_suspend_occur_sum; //异常挂起产生次数累计 hal_ser_imu_data_update_callback notify_cb[DATA_UPDATE_NOTIFY_MAX]; //数据更新通知回调 } Ser_Imu_t; /*Local Variable ------------------------------------------------------------------------------------------------------------------------------------*/ static Ser_Imu_t ob_ser_imu; static int update_cur_front_flow; /*Local Functions ------------------------------------------------------------------------------------------------------------------------------------*/ static int hal_ser_imu_macro_conversion_acc_odr(SER_IMU_DIR_e dir, SER_IMU_ACC_ODR_e acc_odr) { int ret = 0; if(dir == SER_IMU_DIR_FRONT) { switch(acc_odr) { case SER_IMU_ACC_ODR_OFF: ret = LSM_ACC_ODR_OFF; break; case SER_IMU_ACC_ODR_104HZ: ret = LSM_ACC_ODR_104HZ; break; case SER_IMU_ACC_ODR_12HZ5: ret = LSM_ACC_ODR_12HZ5; break; } } return ret; } static int hal_ser_imu_macro_conversion_gry_odr(SER_IMU_DIR_e dir, SER_IMU_GRY_ODR_e gry_odr) { int ret = 0; if(dir == SER_IMU_DIR_FRONT) { switch(gry_odr) { case SER_IMU_GRY_ODR_OFF: ret = LSM_GRY_ODR_OFF; break; case SER_IMU_GRY_ODR_104HZ: ret = LSM_GRY_ODR_104HZ; break; case SER_IMU_GRY_ODR_12HZ5: ret = LSM_GRY_ODR_12HZ5; break; } } return ret; } static int hal_ser_imu_macro_conversion_mag_odr(SER_IMU_DIR_e dir, SER_IMU_MAG_ODR_e mag_odr) { int ret = 0; if(dir == SER_IMU_DIR_FRONT) { switch(mag_odr) { case SER_IMU_MAG_ODR_OFF: ret = LSM_MAG_ODR_OFF; break; case SER_IMU_MAG_ODR_10HZ: ret = LSM_MAG_ODR_10HZ; break; case SER_IMU_MAG_ODR_100HZ: ret = LSM_MAG_ODR_100HZ; break; case SER_IMU_MAG_ODR_200HZ: ret = LSM_MAG_ODR_200HZ; break; } } if(dir == SER_IMU_DIR_BACK) { switch(mag_odr) { case SER_IMU_MAG_ODR_OFF: ret = QMC_MAG_ODR_OFF; break; case SER_IMU_MAG_ODR_10HZ: ret = QMC_MAG_ODR_10HZ; break; case SER_IMU_MAG_ODR_100HZ: ret = QMC_MAG_ODR_100HZ; break; case SER_IMU_MAG_ODR_200HZ: ret = QMC_MAG_ODR_200HZ; break; } } return ret; } static int hal_ser_imu_macro_conversion_fifo_odr(SER_IMU_DIR_e dir, SER_IMU_FIFO_ODR_e fifo_odr) { int ret = 0; if(dir == SER_IMU_DIR_FRONT) { switch(fifo_odr) { case SER_IMU_FIFO_ODR_OFF: ret = LSM_FIFO_ODR_OFF; break; case SER_IMU_FIFO_ODR_104HZ: ret = LSM_FIFO_ODR_104HZ; break; } } return ret; } static int hal_ser_imu_macro_conversion_acc_power_mode(SER_IMU_DIR_e dir, SER_IMU_ACC_POWER_MODE_e acc_power_mode) { int ret = 0; if(dir == SER_IMU_DIR_FRONT) { switch(acc_power_mode) { case SER_IMU_ACC_POWER_MODE_HIGH_PERFORMANCE: ret = LSM_ACC_POWER_MODE_HIGH_PERFORMANCE; break; case SER_IMU_ACC_POWER_MODE_NORMAL: ret = LSM_ACC_POWER_MODE_NORMAL; break; } } return ret; } static int hal_ser_imu_macro_conversion_gry_power_mode(SER_IMU_DIR_e dir, SER_IMU_GRY_POWER_MODE_e gry_power_mode) { int ret = 0; if(dir == SER_IMU_DIR_FRONT) { switch(gry_power_mode) { case SER_IMU_GRY_POWER_MODE_HIGH_PERFORMANCE: ret = LSM_GRY_POWER_MODE_HIGH_PERFORMANCE; break; case SER_IMU_GRY_POWER_MODE_NORMAL: ret = LSM_GRY_POWER_MODE_NORMAL; break; } } return ret; } static int hal_ser_imu_macro_conversion_acc_fs(SER_IMU_DIR_e dir, SER_IMU_ACC_FS_e acc_fs) { int ret = 0; if(dir == SER_IMU_DIR_FRONT) { switch(acc_fs) { case SER_IMU_ACC_FS_2G: ret = LSM_ACC_FS_2G; break; case SER_IMU_ACC_FS_16G: ret = LSM_ACC_FS_16G; break; } } return ret; } static int hal_ser_imu_macro_conversion_gry_fs(SER_IMU_DIR_e dir, SER_IMU_GRY_FS_e gry_fs) { int ret = 0; if(dir == SER_IMU_DIR_FRONT) { switch(gry_fs) { case SER_IMU_GRY_FS_250DPS: ret = LSM_GRY_FS_250DPS; break; case SER_IMU_GRY_FS_2000DPS: ret = LSM_GRY_FS_2000DPS; break; } } return ret; } static int hal_ser_imu_macro_conversion_mag_fs(SER_IMU_DIR_e dir, SER_IMU_MAG_FS_e mag_fs) { int ret = 0; if(dir == SER_IMU_DIR_FRONT) { switch(mag_fs) { case SER_IMU_MAG_FS_30GS: ret = LSM_MAG_FS_30GS; break; } } if(dir == SER_IMU_DIR_BACK) { switch(mag_fs) { case SER_IMU_MAG_FS_30GS: ret = QMC_MAG_FS_30GS; break; } } return ret; } static int hal_ser_imu_macro_conversion_timestamp_resolution(SER_IMU_DIR_e dir, SER_IMU_TIMESTAMP_RESOLUTION_e timestamp_resolution) { int ret = 0; if(dir == SER_IMU_DIR_FRONT) { switch(timestamp_resolution) { case SER_IMU_TIMESTAMP_6MS4: ret = LSM_TIMESTAMP_6MS4; break; case SER_IMU_TIMESTAMP_25US: ret = LSM_TIMESTAMP_25US; break; } } return ret; } static int hal_ser_imu_macro_conversion_timestamp_switch(SER_IMU_DIR_e dir, SER_IMU_TIMESTAMP_SWITCH_e timestamp_switch) { int ret = 0; if(dir == SER_IMU_DIR_FRONT) { switch(timestamp_switch) { case SER_IMU_TIMESTAMP_OFF: ret = LSM_TIMESTAMP_OFF; break; case SER_IMU_TIMESTAMP_ON: ret = LSM_TIMESTAMP_ON; break; } } return ret; } static void hal_ser_imu_get_compre_param(ser_imu_config_param_t *compre_drv_param, ser_imu_config_param_t *op_param) { compre_drv_param->acc_fs = MAX(compre_drv_param->acc_fs, op_param->acc_fs); compre_drv_param->acc_odr = MAX(compre_drv_param->acc_odr, op_param->acc_odr); compre_drv_param->acc_power_mode = MAX(compre_drv_param->acc_power_mode, op_param->acc_power_mode); compre_drv_param->fifo_odr = MAX(compre_drv_param->fifo_odr, op_param->fifo_odr); compre_drv_param->gry_fs = MAX(compre_drv_param->gry_fs, op_param->gry_fs); compre_drv_param->gry_odr = MAX(compre_drv_param->gry_odr, op_param->gry_odr); compre_drv_param->gry_power_mode = MAX(compre_drv_param->gry_power_mode, op_param->gry_power_mode); compre_drv_param->mag_fs = MAX(compre_drv_param->mag_fs, op_param->mag_fs); compre_drv_param->mag_odr = MAX(compre_drv_param->mag_odr, op_param->mag_odr); compre_drv_param->timestamp_resolution = MAX(compre_drv_param->timestamp_resolution, op_param->timestamp_resolution); compre_drv_param->timestamp_switch = MAX(compre_drv_param->timestamp_switch, op_param->timestamp_switch); } static bool hal_ser_imu_param_is_equal_drv_param_lsm(ser_imu_config_param_t *compre_drv_param, drv_param_u *drv_param) { if(IS_EQUAL(hal_ser_imu_macro_conversion_acc_fs(SER_IMU_DIR_FRONT, compre_drv_param->acc_fs),drv_param->lsm.acc_fs) == false){return false;} if(IS_EQUAL(hal_ser_imu_macro_conversion_acc_odr(SER_IMU_DIR_FRONT, compre_drv_param->acc_odr),drv_param->lsm.acc_odr) == false){return false;} if(IS_EQUAL(hal_ser_imu_macro_conversion_acc_power_mode(SER_IMU_DIR_FRONT, compre_drv_param->acc_power_mode),drv_param->lsm.acc_power_mode) == false){return false;} if(IS_EQUAL(hal_ser_imu_macro_conversion_fifo_odr(SER_IMU_DIR_FRONT, compre_drv_param->fifo_odr),drv_param->lsm.fifo_odr) == false){return false;} if(IS_EQUAL(hal_ser_imu_macro_conversion_gry_fs(SER_IMU_DIR_FRONT, compre_drv_param->gry_fs),drv_param->lsm.gry_fs) == false){return false;} if(IS_EQUAL(hal_ser_imu_macro_conversion_gry_odr(SER_IMU_DIR_FRONT, compre_drv_param->gry_odr),drv_param->lsm.gry_odr) == false){return false;} if(IS_EQUAL(hal_ser_imu_macro_conversion_gry_power_mode(SER_IMU_DIR_FRONT, compre_drv_param->gry_power_mode),drv_param->lsm.gry_power_mode) == false){return false;} if(IS_EQUAL(hal_ser_imu_macro_conversion_mag_fs(SER_IMU_DIR_FRONT, compre_drv_param->mag_fs),drv_param->lsm.mag_fs) == false){return false;} if(IS_EQUAL(hal_ser_imu_macro_conversion_mag_odr(SER_IMU_DIR_FRONT, compre_drv_param->mag_odr),drv_param->lsm.mag_odr) == false){return false;} if(IS_EQUAL(hal_ser_imu_macro_conversion_timestamp_resolution(SER_IMU_DIR_FRONT, compre_drv_param->timestamp_resolution),drv_param->lsm.timestamp_resolution) == false){return false;} if(IS_EQUAL(hal_ser_imu_macro_conversion_timestamp_switch(SER_IMU_DIR_FRONT, compre_drv_param->timestamp_switch),drv_param->lsm.timestamp_switch) == false){return false;} return true; } static bool hal_ser_imu_param_is_equal_drv_param_qmc(ser_imu_config_param_t *compre_drv_param, drv_param_u *drv_param) { if(IS_EQUAL(hal_ser_imu_macro_conversion_mag_fs(SER_IMU_DIR_BACK, compre_drv_param->mag_fs), drv_param->qmc.mag_fs) == false){return false;} if(IS_EQUAL(hal_ser_imu_macro_conversion_mag_odr(SER_IMU_DIR_BACK, compre_drv_param->mag_odr), drv_param->qmc.mag_odr) == false){return false;} return true; } static int hal_ser_imu_read_data_lsm(void) { int i; int fifo_group_num; lsm_data_t temp_lsm_data; //读取FIFO数据 if(ob_ser_imu.compre_drv_param[SER_IMU_DIR_FRONT].fifo_odr != SER_IMU_FIFO_ODR_OFF) { //获取当前fifo里存在多少组数据。 fifo_group_num = drv_lsm_get_fifo_group_num(); fifo_group_num = fifo_group_num <= SER_IMU_DATA_GROUP_NUM_MAX ? fifo_group_num : SER_IMU_DATA_GROUP_NUM_MAX; for(i=0; i= 1000) // { // sprintf(printfbuff,"i:%d;acc:%d,%d,%d;gry:%d,%d,%d;mag:%d,%d,%d;timestamp:%d;id:%d\r\n",i,ob_ser_imu.cur_data[SER_IMU_DIR_FRONT][i].acc[0], \ // ob_ser_imu.cur_data[SER_IMU_DIR_FRONT][i].acc[1], \ // ob_ser_imu.cur_data[SER_IMU_DIR_FRONT][i].acc[2], \ // ob_ser_imu.cur_data[SER_IMU_DIR_FRONT][i].gry[0], \ // ob_ser_imu.cur_data[SER_IMU_DIR_FRONT][i].gry[1], \ // ob_ser_imu.cur_data[SER_IMU_DIR_FRONT][i].gry[2], \ // ob_ser_imu.cur_data[SER_IMU_DIR_FRONT][i].mag[0], \ // ob_ser_imu.cur_data[SER_IMU_DIR_FRONT][i].mag[1], \ // ob_ser_imu.cur_data[SER_IMU_DIR_FRONT][i].mag[2], \ // ob_ser_imu.cur_data[SER_IMU_DIR_FRONT][i].fifo_timestamp, \ // drv_lsm_get_id()); // // tim = TIME_GetTicks(); // UART0_PrintfDebug(printfbuff); // } } } return fifo_group_num; } //只读取ACC数据 else if(ob_ser_imu.compre_drv_param[SER_IMU_DIR_FRONT].acc_odr != SER_IMU_ACC_ODR_OFF && \ ob_ser_imu.compre_drv_param[SER_IMU_DIR_FRONT].gry_odr == SER_IMU_GRY_ODR_OFF && \ ob_ser_imu.compre_drv_param[SER_IMU_DIR_FRONT].mag_odr == SER_IMU_MAG_ODR_OFF \ ) { if(drv_lsm_get_acc_data(&temp_lsm_data) != -1) { ob_ser_imu.cur_data[SER_IMU_DIR_FRONT][0].acc[0] = temp_lsm_data.acc[0]; ob_ser_imu.cur_data[SER_IMU_DIR_FRONT][0].acc[1] = temp_lsm_data.acc[1]; ob_ser_imu.cur_data[SER_IMU_DIR_FRONT][0].acc[2] = temp_lsm_data.acc[2]; return 1; } } return 0; } static int hal_ser_imu_read_data_qmc(void) { qmc_data_t temp_qmc_data; if(ob_ser_imu.compre_drv_param[SER_IMU_DIR_BACK].mag_odr != SER_IMU_MAG_ODR_OFF) { if(drv_qmc6310_get_mag_data(&temp_qmc_data) != -1) { ob_ser_imu.cur_data[SER_IMU_DIR_BACK][0].mag[0] = temp_qmc_data.mag[0]; ob_ser_imu.cur_data[SER_IMU_DIR_BACK][0].mag[1] = temp_qmc_data.mag[1]; ob_ser_imu.cur_data[SER_IMU_DIR_BACK][0].mag[2] = temp_qmc_data.mag[2]; return 1; } } return 0; } static int hal_ser_imu_read_data(SER_IMU_DIR_e dir) { int ret = 0; switch(dir) { case SER_IMU_DIR_FRONT: ret = hal_ser_imu_read_data_lsm(); break; case SER_IMU_DIR_BACK: ret = hal_ser_imu_read_data_qmc(); break; default: break; } return ret; } static bool hal_ser_imu_is_need_config(void) { int i; //重新获取综合的前脚配置，根据优先级来判定 memset(&ob_ser_imu.compre_drv_param[SER_IMU_DIR_FRONT],0,sizeof(ob_ser_imu.compre_drv_param[SER_IMU_DIR_FRONT])); for(i=0; i=200){ tim = 0; imu_config_flow_ctl = 2; } break; case 2: //配置lsm上电（默认配置挂起） if(drv_lsm_power_on() == 0){ imu_config_flow_ctl = 3; } break; case 3: //等待200ms if(tim == 0){ tim = TIME_GetTicks(); }else if(TIME_GetTicks()-tim>=200){ tim = 0; imu_mag_total_flow_ctl = drv_lsm_get_mag_odr_flow(); imu_mag_cur_flow_ctl = 1; imu_config_flow_ctl = 4; } break; case 4: //配置lsm的地磁计采样频率 if(ob_ser_imu.compre_drv_param[SER_IMU_DIR_FRONT].fifo_odr != SER_IMU_FIFO_ODR_OFF) { //使用hub if(drv_lsm_set_mag_odr((LSM_MAG_ODR_e)hal_ser_imu_macro_conversion_mag_odr(SER_IMU_DIR_FRONT,ob_ser_imu.compre_drv_param[SER_IMU_DIR_FRONT].mag_odr), 1, imu_mag_cur_flow_ctl) != -1){ if(imu_mag_cur_flow_ctl == imu_mag_total_flow_ctl){ imu_config_flow_ctl = 5; //进入下一个流程 } else{ imu_mag_cur_flow_ctl++; //进入下一个步骤 tim = 0; } } else{ if(tim == 0){ tim = TIME_GetTicks(); }else if(TIME_GetTicks()-tim>=1000){ tim = 0; imu_config_flow_ctl = 0; //累计超过1s，重新走流程 } } } else { //不使用hub if(drv_lsm_set_mag_odr((LSM_MAG_ODR_e)hal_ser_imu_macro_conversion_mag_odr(SER_IMU_DIR_FRONT,ob_ser_imu.compre_drv_param[SER_IMU_DIR_FRONT].mag_odr), 0, imu_mag_cur_flow_ctl) != -1){ if(imu_mag_cur_flow_ctl == imu_mag_total_flow_ctl){ imu_config_flow_ctl = 5; //进入下一个流程 tim = 0; } else{ imu_mag_cur_flow_ctl++; //进入下一个步骤 tim = 0; } } else{ if(tim == 0){ tim = TIME_GetTicks(); }else if(TIME_GetTicks()-tim>=1000){ tim = 0; imu_config_flow_ctl = 0; //累计超过1s，重新走流程 } } } break; case 5: //配置lsm的加速度工作模式 ret += drv_lsm_set_acc_power_mode((LSM_ACC_POWER_MODE_e)hal_ser_imu_macro_conversion_acc_power_mode(SER_IMU_DIR_FRONT,ob_ser_imu.compre_drv_param[SER_IMU_DIR_FRONT].acc_power_mode)); //配置lsm的陀螺仪工作模式 ret += drv_lsm_set_gry_power_mode((LSM_GRY_POWER_MODE_e)hal_ser_imu_macro_conversion_gry_power_mode(SER_IMU_DIR_FRONT,ob_ser_imu.compre_drv_param[SER_IMU_DIR_FRONT].gry_power_mode)); //配置lsm的加速度量程 ret += drv_lsm_set_acc_fs((LSM_ACC_FS_e)hal_ser_imu_macro_conversion_acc_fs(SER_IMU_DIR_FRONT,ob_ser_imu.compre_drv_param[SER_IMU_DIR_FRONT].acc_fs)); //配置lsm的陀螺仪量程 ret += drv_lsm_set_gry_fs((LSM_GRY_FS_e)hal_ser_imu_macro_conversion_gry_fs(SER_IMU_DIR_FRONT,ob_ser_imu.compre_drv_param[SER_IMU_DIR_FRONT].gry_fs)); //配置lsm的时间戳精度 ret += drv_lsm_set_timestamp_resolution((LSM_TIMESTAMP_RESOLUTION_e)hal_ser_imu_macro_conversion_timestamp_resolution(SER_IMU_DIR_FRONT,ob_ser_imu.compre_drv_param[SER_IMU_DIR_FRONT].timestamp_resolution)); //配置lsm的时间戳开关 ret += drv_lsm_set_timestamp_switch((LSM_TIMESTAMP_SWITCH_e)hal_ser_imu_macro_conversion_timestamp_switch(SER_IMU_DIR_FRONT,ob_ser_imu.compre_drv_param[SER_IMU_DIR_FRONT].timestamp_switch)); if(ret == 0) { imu_config_flow_ctl = 6; tim = 0; } else { if(tim == 0){ tim = TIME_GetTicks(); }else if(TIME_GetTicks()-tim>=1000){ tim = 0; imu_config_flow_ctl = 0; //累计超过1s，重新走流程 } } break; case 6: //配置lsm的FIFO采样频率（必须以上都配置成功，才能配置FIFO） ret += drv_lsm_set_fifo_odr((LSM_FIFO_ODR_e)hal_ser_imu_macro_conversion_fifo_odr(SER_IMU_DIR_FRONT,ob_ser_imu.compre_drv_param[SER_IMU_DIR_FRONT].fifo_odr), \ (LSM_ACC_ODR_e)hal_ser_imu_macro_conversion_acc_odr(SER_IMU_DIR_FRONT,ob_ser_imu.compre_drv_param[SER_IMU_DIR_FRONT].acc_odr), \ (LSM_GRY_ODR_e)hal_ser_imu_macro_conversion_gry_odr(SER_IMU_DIR_FRONT,ob_ser_imu.compre_drv_param[SER_IMU_DIR_FRONT].gry_odr), \ (LSM_MAG_ODR_e)hal_ser_imu_macro_conversion_mag_odr(SER_IMU_DIR_FRONT,ob_ser_imu.compre_drv_param[SER_IMU_DIR_FRONT].mag_odr), \ (LSM_TIMESTAMP_SWITCH_e)hal_ser_imu_macro_conversion_timestamp_switch(SER_IMU_DIR_FRONT,ob_ser_imu.compre_drv_param[SER_IMU_DIR_FRONT].timestamp_switch)); if(ret == 0) { imu_config_flow_ctl = 7; tim = 0; } else { if(tim == 0){ tim = TIME_GetTicks(); }else if(TIME_GetTicks()-tim>=1000){ tim = 0; imu_config_flow_ctl = 0; //累计超过1s，重新走流程 } } break; case 7: //配置lsm的加速度采样频率 ret += drv_lsm_set_acc_odr((LSM_ACC_ODR_e)hal_ser_imu_macro_conversion_acc_odr(SER_IMU_DIR_FRONT,ob_ser_imu.compre_drv_param[SER_IMU_DIR_FRONT].acc_odr)); //配置lsm的陀螺仪采样频率 ret += drv_lsm_set_gry_odr((LSM_GRY_ODR_e)hal_ser_imu_macro_conversion_gry_odr(SER_IMU_DIR_FRONT,ob_ser_imu.compre_drv_param[SER_IMU_DIR_FRONT].gry_odr)); if(ret == 0) { imu_config_flow_ctl = 8; tim = 0; } else { if(tim == 0){ tim = TIME_GetTicks(); }else if(TIME_GetTicks()-tim>=1000){ tim = 0; imu_config_flow_ctl = 0; //累计超过1s，重新走流程 } } break; case 8: if(ob_ser_imu.compre_drv_param[SER_IMU_DIR_FRONT].acc_odr != SER_IMU_ACC_ODR_OFF || \ ob_ser_imu.compre_drv_param[SER_IMU_DIR_FRONT].gry_odr != SER_IMU_GRY_ODR_OFF || \ ob_ser_imu.compre_drv_param[SER_IMU_DIR_FRONT].mag_odr != SER_IMU_MAG_ODR_OFF ) { if(hal_ser_imu_read_data(SER_IMU_DIR_FRONT) != 0) { imu_config_flow_ctl = 0; return true; } else { if(tim == 0){ tim = TIME_GetTicks(); }else if(TIME_GetTicks()-tim>=3000){ tim = 0; imu_config_flow_ctl = 0; //累计超过1s，重新走流程 } } } else { imu_config_flow_ctl = 0; return true; } break; } return false; } static bool hal_ser_imu_config_dir_back(void) { int ret = 0; static int imu_config_flow_ctl = 0; //配置流程控制 static int imu_mag_cur_flow_ctl; static int imu_mag_total_flow_ctl; static uint32_t tim = 0; switch(imu_config_flow_ctl) { case 0: //配置qmc断电 if(drv_qmc6310_power_off() == 0){imu_config_flow_ctl = 1;} break; case 1: //等待200ms if(tim == 0){ tim = TIME_GetTicks(); }else if(TIME_GetTicks()-tim>=200){ tim = 0; imu_config_flow_ctl = 2; } break; case 2: //配置qmc上电（默认配置挂起） if(drv_qmc6310_power_on() == 0){ imu_config_flow_ctl = 3; } break; case 3: //等待200ms if(tim == 0){ tim = TIME_GetTicks(); }else if(TIME_GetTicks()-tim>=200){ tim = 0; imu_mag_total_flow_ctl = drv_qmc6310_get_mag_odr_flow(); imu_mag_cur_flow_ctl = 1; imu_config_flow_ctl = 4; } break; case 4: //配置qmc的地磁计采样频率 ret = drv_qmc6310_set_mag_odr((QMC_MAG_ODR_e)hal_ser_imu_macro_conversion_mag_odr(SER_IMU_DIR_BACK,ob_ser_imu.compre_drv_param[SER_IMU_DIR_BACK].mag_odr),imu_mag_cur_flow_ctl); if(ret != -1) { if(imu_mag_cur_flow_ctl == imu_mag_total_flow_ctl){ imu_config_flow_ctl = 5; } else{ imu_mag_cur_flow_ctl++; //进入下一个步骤 tim = 0; } } else { if(tim == 0){ tim = TIME_GetTicks(); }else if(TIME_GetTicks()-tim>=1000){ tim = 0; imu_config_flow_ctl = 0; //累计超过1s，重新走流程 } } break; case 5: if(ob_ser_imu.compre_drv_param[SER_IMU_DIR_BACK].mag_odr != SER_IMU_MAG_ODR_OFF) { if(hal_ser_imu_read_data(SER_IMU_DIR_BACK) != 0) { imu_config_flow_ctl = 0; return true; } } else { imu_config_flow_ctl = 0; return true; } break; } return false; } static bool hal_ser_imu_is_config_done(SER_IMU_DIR_e dir) { bool ret = true; switch(dir) { case SER_IMU_DIR_FRONT: ret = hal_ser_imu_config_dir_front(); break; case SER_IMU_DIR_BACK: ret = hal_ser_imu_config_dir_back(); break; default: break; } return ret; } static void monitor_sensor_data(int16_t *f_acc, int16_t *f_gry, int16_t *f_mag, int16_t *b_mag) { /*前脚加速度*/ if(f_acc != NULL) { if( ob_ser_imu.last_f_acc[0] == f_acc[0] && \ ob_ser_imu.last_f_acc[1] == f_acc[1] && \ ob_ser_imu.last_f_acc[2] == f_acc[2] ) { ob_ser_imu.last_f_acc_err_sum++; if(ob_ser_imu.last_f_acc_err_sum >= MONITOR_DATA_ERR_SUM_MAX){ Except_SetExceptype(EXCEPT_DATA_FRONT_ACC); } }else{ ob_ser_imu.last_f_acc_err_sum = 0; } ob_ser_imu.last_f_acc[0] = f_acc[0]; ob_ser_imu.last_f_acc[1] = f_acc[1]; ob_ser_imu.last_f_acc[2] = f_acc[2]; } /*前脚陀螺仪*/ if(f_gry != NULL) { if( ob_ser_imu.last_f_gry[0] == f_gry[0] && \ ob_ser_imu.last_f_gry[1] == f_gry[1] && \ ob_ser_imu.last_f_gry[2] == f_gry[2] ) { ob_ser_imu.last_f_gry_err_sum++; if(ob_ser_imu.last_f_gry_err_sum >= MONITOR_DATA_ERR_SUM_MAX){ Except_SetExceptype(EXCEPT_DATA_FRONT_GRY); } }else{ ob_ser_imu.last_f_gry_err_sum = 0; } ob_ser_imu.last_f_gry[0] = f_gry[0]; ob_ser_imu.last_f_gry[1] = f_gry[1]; ob_ser_imu.last_f_gry[2] = f_gry[2]; } /*前脚地磁计*/ if(f_mag != NULL) { if( ob_ser_imu.last_f_mag[0] == f_mag[0] && \ ob_ser_imu.last_f_mag[1] == f_mag[1] && \ ob_ser_imu.last_f_mag[2] == f_mag[2] ) { ob_ser_imu.last_f_mag_err_sum++; if(ob_ser_imu.last_f_mag_err_sum >= MONITOR_DATA_ERR_SUM_MAX){ Except_SetExceptype(EXCEPT_DATA_FRONT_MAG); } }else{ ob_ser_imu.last_f_mag_err_sum = 0; } ob_ser_imu.last_f_mag[0] = f_mag[0]; ob_ser_imu.last_f_mag[1] = f_mag[1]; ob_ser_imu.last_f_mag[2] = f_mag[2]; } /*后脚地磁计*/ if(b_mag != NULL) { if( ob_ser_imu.last_b_mag[0] == b_mag[0] && \ ob_ser_imu.last_b_mag[1] == b_mag[1] && \ ob_ser_imu.last_b_mag[2] == b_mag[2] ) { ob_ser_imu.last_b_mag_err_sum++; if(ob_ser_imu.last_b_mag_err_sum >= MONITOR_DATA_ERR_SUM_MAX){ Except_SetExceptype(EXCEPT_DATA_BACK_MAG); } }else{ ob_ser_imu.last_b_mag_err_sum = 0; } ob_ser_imu.last_b_mag[0] = b_mag[0]; ob_ser_imu.last_b_mag[1] = b_mag[1]; ob_ser_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_ser_imu.except_data_occur_sum[SER_IMU_DIR_FRONT]++; if(ob_ser_imu.except_data_occur_sum[SER_IMU_DIR_FRONT] > 1) { if(Except_IsError(EXCEPT_DATA_FRONT_ACC))Except_TxError(EXCEPT_DATA_FRONT_ACC,"front_acc_data_error"); if(Except_IsError(EXCEPT_DATA_FRONT_GRY))Except_TxError(EXCEPT_DATA_FRONT_GRY,"front_gry_data_error"); if(Except_IsError(EXCEPT_DATA_FRONT_MAG))Except_TxError(EXCEPT_DATA_FRONT_MAG,"front_mag_data_error"); } //重启前脚LSM（主要是复位结构体） drv_lsm_power_off(); drv_lsm_power_on(); //清除异常 Except_ClearExceptype(EXCEPT_DATA_FRONT_ACC); Except_ClearExceptype(EXCEPT_DATA_FRONT_GRY); Except_ClearExceptype(EXCEPT_DATA_FRONT_MAG); } else { ob_ser_imu.except_data_occur_sum[SER_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_ser_imu.except_data_occur_sum[SER_IMU_DIR_BACK]++; if(ob_ser_imu.except_data_occur_sum[SER_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(); //清除异常 Except_ClearExceptype(EXCEPT_DATA_BACK_MAG); } else { ob_ser_imu.except_data_occur_sum[SER_IMU_DIR_BACK] = 0; //清除异常 Except_ClearExceptype(EXCEPT_DATA_BACK_MAG); } } static void hal_ser_imu_Process(void) { int data_num[SER_IMU_DIR_NUM]; switch(ob_ser_imu.stage) { case IMU_CONFIG_STAGE_DONE: //配置完成阶段 if(hal_ser_imu_is_need_config()) //判断是否需要配置 { ob_ser_imu.stage = IMU_CONFIG_STAGE_IN_PROGRESS; //进入配置进行阶段 Process_SetHoldOn(hal_ser_imu_Process,1); //全功率配置 } else //读数据 { //读取前脚IMU数据 + 更新前脚IMU数据量 data_num[SER_IMU_DIR_FRONT] = hal_ser_imu_read_data(SER_IMU_DIR_FRONT); if(data_num[SER_IMU_DIR_FRONT] > 0)ob_ser_imu.cur_data_num[SER_IMU_DIR_FRONT] = data_num[SER_IMU_DIR_FRONT]; //读取后脚IMU数据 + 更新后脚IMU数据量 data_num[SER_IMU_DIR_BACK] = hal_ser_imu_read_data(SER_IMU_DIR_BACK); if(data_num[SER_IMU_DIR_BACK] > 0)ob_ser_imu.cur_data_num[SER_IMU_DIR_BACK] = data_num[SER_IMU_DIR_BACK]; //回调通知 if(data_num[SER_IMU_DIR_FRONT] > 0 || data_num[SER_IMU_DIR_BACK] > 0) { for(int i = 0; i < DATA_UPDATE_NOTIFY_MAX; i++) { if(ob_ser_imu.notify_cb[i]) { ob_ser_imu.notify_cb[i](data_num[SER_IMU_DIR_FRONT],data_num[SER_IMU_DIR_BACK],0-host_get_rssi()); } } } } break; case IMU_CONFIG_STAGE_IN_PROGRESS: //配置进行阶段 if(ob_ser_imu.is_need_config[SER_IMU_DIR_FRONT]) //判断前脚是否需要配置 { if(hal_ser_imu_is_config_done(SER_IMU_DIR_FRONT)) { ob_ser_imu.is_need_config[SER_IMU_DIR_FRONT] = false; } } else if(ob_ser_imu.is_need_config[SER_IMU_DIR_BACK]) //判断后脚是否需要配置 { if(hal_ser_imu_is_config_done(SER_IMU_DIR_BACK)) { ob_ser_imu.is_need_config[SER_IMU_DIR_BACK] = false; } } else { if(!hal_ser_imu_is_need_config()) //若是在配置期间，操作的IMU配置参数副本更改，需要重新配置。 { ob_ser_imu.stage = IMU_CONFIG_STAGE_DONE; //配置完成 //读数据 Process_SetHoldOn(hal_ser_imu_Process,0); //解放线程 } } break; } } static void hal_ser_imu_monitor_sensor_data_process(void) { 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]; ser_imu_data_t data; static uint32_t last_tim = 0; if( ob_ser_imu.stage == IMU_CONFIG_STAGE_DONE && \ ob_ser_imu.drv_param[SER_IMU_DIR_FRONT].lsm.acc_odr == LSM_ACC_ODR_104HZ && \ ob_ser_imu.drv_param[SER_IMU_DIR_FRONT].lsm.gry_odr == LSM_GRY_ODR_104HZ && \ (ob_ser_imu.drv_param[SER_IMU_DIR_FRONT].lsm.mag_odr == LSM_MAG_ODR_200HZ || ob_ser_imu.drv_param[SER_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 = hal_ser_imu_get_data_num(SER_IMU_DIR_FRONT); if(group_num > 0) { for(i=0;i= 1){ hal_ser_imu_get_data(SER_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_ser_imu.stage == IMU_CONFIG_STAGE_DONE && \ ob_ser_imu.drv_param[SER_IMU_DIR_FRONT].lsm.acc_odr == LSM_ACC_ODR_104HZ && \ (ob_ser_imu.drv_param[SER_IMU_DIR_FRONT].lsm.mag_odr == LSM_MAG_ODR_200HZ || ob_ser_imu.drv_param[SER_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 = hal_ser_imu_get_data_num(SER_IMU_DIR_FRONT); if(group_num > 0) { for(i=0;i= StandByPower_Interval) //监测前脚传感器数据（acc） { last_tim = TIME_GetTicks(); group_num = hal_ser_imu_get_data_num(SER_IMU_DIR_FRONT); if(group_num > 0) { for(i=0;i= MONITOR_SUSPEND_MODE_OVERFLOW_ERR_SUM_MAX){ Except_SetExceptype(EXCEPT_IMU_SUSPEND_OVERFLOW); if(mFlash.isHost)sprintf(buf,"left front_imu_suspend_overflow:%d,%d,%d\r\n",ob_ser_imu.is_need_config[SER_IMU_DIR_FRONT],update_cur_front_flow,ob_ser_imu.is_need_config[SER_IMU_DIR_BACK]); else sprintf(buf,"right front_imu_suspend_overflow:%d,%d,%d\r\n",ob_ser_imu.is_need_config[SER_IMU_DIR_FRONT],update_cur_front_flow,ob_ser_imu.is_need_config[SER_IMU_DIR_BACK]); Except_TxError(EXCEPT_IMU_SUSPEND_OVERFLOW,buf); ob_ser_imu.except_suspend_occur_sum = MONITOR_SUSPEND_MODE_OVERFLOW_ERR_SUM_MAX; } } else { ob_ser_imu.except_suspend_occur_sum = 0; Except_ClearExceptype(EXCEPT_IMU_SUSPEND_OVERFLOW); } } static void hal_ser_imu_printf_Process(void) { SEGGER_RTT_printf(0,"hal_ser_imu----------------------------------------------------------------->\n"); SEGGER_RTT_printf(0,"stage:0x%x\n",ob_ser_imu.stage); SEGGER_RTT_printf(0,"is_need_config[SER_IMU_DIR_FRONT]:0x%x\n",ob_ser_imu.is_need_config[SER_IMU_DIR_FRONT]); SEGGER_RTT_printf(0,"is_need_config[SER_IMU_DIR_BACK]:0x%x\n",ob_ser_imu.is_need_config[SER_IMU_DIR_BACK]); // SEGGER_RTT_printf(0,"df acc_fs:0x%x\n",ob_ser_imu.drv_param[SER_IMU_DIR_FRONT].lsm.acc_fs); SEGGER_RTT_printf(0,"df acc_odr:0x%x\n",ob_ser_imu.drv_param[SER_IMU_DIR_FRONT].lsm.acc_odr); // SEGGER_RTT_printf(0,"df acc_power_mode:0x%x\n",ob_ser_imu.drv_param[SER_IMU_DIR_FRONT].lsm.acc_power_mode); SEGGER_RTT_printf(0,"df fifo_odr:0x%x\n",ob_ser_imu.drv_param[SER_IMU_DIR_FRONT].lsm.fifo_odr); // SEGGER_RTT_printf(0,"df gry_fs:0x%x\n",ob_ser_imu.drv_param[SER_IMU_DIR_FRONT].lsm.gry_fs); SEGGER_RTT_printf(0,"df gry_odr:0x%x\n",ob_ser_imu.drv_param[SER_IMU_DIR_FRONT].lsm.gry_odr); // SEGGER_RTT_printf(0,"df gry_power_mode:0x%x\n",ob_ser_imu.drv_param[SER_IMU_DIR_FRONT].lsm.gry_power_mode); // SEGGER_RTT_printf(0,"df mag_fs:0x%x\n",ob_ser_imu.drv_param[SER_IMU_DIR_FRONT].lsm.mag_fs); SEGGER_RTT_printf(0,"df mag_odr:0x%x\n",ob_ser_imu.drv_param[SER_IMU_DIR_FRONT].lsm.mag_odr); // SEGGER_RTT_printf(0,"f timestamp_resolution:0x%x\n",ob_ser_imu.drv_param[SER_IMU_DIR_FRONT].lsm.timestamp_resolution); // SEGGER_RTT_printf(0,"df timestamp_switch:0x%x\n",ob_ser_imu.drv_param[SER_IMU_DIR_FRONT].lsm.timestamp_switch); SEGGER_RTT_printf(0,"db mag_odr:0x%x\n",ob_ser_imu.drv_param[SER_IMU_DIR_BACK].qmc.mag_odr); // SEGGER_RTT_printf(0,"cf acc_fs:0x%x\n",ob_ser_imu.compre_drv_param[SER_IMU_DIR_FRONT].acc_fs); SEGGER_RTT_printf(0,"cf acc_odr:0x%x\n",ob_ser_imu.compre_drv_param[SER_IMU_DIR_FRONT].acc_odr); // SEGGER_RTT_printf(0,"cf acc_power_mode:0x%x\n",ob_ser_imu.compre_drv_param[SER_IMU_DIR_FRONT].acc_power_mode); SEGGER_RTT_printf(0,"cf fifo_odr:0x%x\n",ob_ser_imu.compre_drv_param[SER_IMU_DIR_FRONT].fifo_odr); // SEGGER_RTT_printf(0,"cf gry_fs:0x%x\n",ob_ser_imu.compre_drv_param[SER_IMU_DIR_FRONT].gry_fs); SEGGER_RTT_printf(0,"cf gry_odr:0x%x\n",ob_ser_imu.compre_drv_param[SER_IMU_DIR_FRONT].gry_odr); // SEGGER_RTT_printf(0,"cf gry_power_mode:0x%x\n",ob_ser_imu.compre_drv_param[SER_IMU_DIR_FRONT].gry_power_mode); // SEGGER_RTT_printf(0,"cf mag_fs:0x%x\n",ob_ser_imu.compre_drv_param[SER_IMU_DIR_FRONT].mag_fs); SEGGER_RTT_printf(0,"cf mag_odr:0x%x\n",ob_ser_imu.compre_drv_param[SER_IMU_DIR_FRONT].mag_odr); // SEGGER_RTT_printf(0,"cf timestamp_resolution:0x%x\n",ob_ser_imu.compre_drv_param[SER_IMU_DIR_FRONT].timestamp_resolution); // SEGGER_RTT_printf(0,"cf timestamp_switch:0x%x\n",ob_ser_imu.compre_drv_param[SER_IMU_DIR_FRONT].timestamp_switch); SEGGER_RTT_printf(0,"cb mag_odr:0x%x\n",ob_ser_imu.compre_drv_param[SER_IMU_DIR_BACK].mag_odr); SEGGER_RTT_printf(0,"ob_ser_imu.cur_data_num[SER_IMU_DIR_FRONT]:0x%x\n",ob_ser_imu.cur_data_num[SER_IMU_DIR_FRONT]); SEGGER_RTT_printf(0,"ob_ser_imu.cur_data_num[SER_IMU_DIR_BACK]:0x%x\n",ob_ser_imu.cur_data_num[SER_IMU_DIR_BACK]); SEGGER_RTT_printf(0,"<-----------------------------------------------------------------hal_ser_imu\n"); } /*API ------------------------------------------------------------------------------------------------------------------------------------*/ /** @brief 初始化IMU服务 @param 无 @return 错误代码 - [out] -1失败，0成功 */ int hal_ser_imu_Init(void) { int i; int ret; /***************************************驱动层初始化***************************************************/ //初始化驱动LSM ret = drv_lsm_Init(); //初始化驱动QMC ret += drv_qmc6310_Init(); //获取驱动LSM配置参数 drv_lsm_get_config_param(&ob_ser_imu.drv_param[SER_IMU_DIR_FRONT].lsm); //获取驱动LSM配置参数 drv_qmc6310_get_config_param(&ob_ser_imu.drv_param[SER_IMU_DIR_BACK].qmc); /***************************************业务逻辑层初始化***************************************************/ //重置结构体 memset(&ob_ser_imu,0,sizeof(ob_ser_imu)); //初始化IMU服务结构体 ob_ser_imu.stage = IMU_CONFIG_STAGE_DONE; for(i=0; iacc_fs){ret = -1;break;} if(ob_ser_imu.compre_drv_param[SER_IMU_DIR_FRONT].acc_odr < param->acc_odr){ret = -1;break;} if(ob_ser_imu.compre_drv_param[SER_IMU_DIR_FRONT].acc_power_mode < param->acc_power_mode){ret = -1;break;} if(ob_ser_imu.compre_drv_param[SER_IMU_DIR_FRONT].fifo_odr < param->fifo_odr){ret = -1;break;} if(ob_ser_imu.compre_drv_param[SER_IMU_DIR_FRONT].gry_fs < param->gry_fs){ret = -1;break;} if(ob_ser_imu.compre_drv_param[SER_IMU_DIR_FRONT].gry_odr < param->gry_odr){ret = -1;break;} if(ob_ser_imu.compre_drv_param[SER_IMU_DIR_FRONT].gry_power_mode < param->gry_power_mode){ret = -1;break;} if(ob_ser_imu.compre_drv_param[SER_IMU_DIR_FRONT].mag_fs < param->mag_fs){ret = -1;break;} if(ob_ser_imu.compre_drv_param[SER_IMU_DIR_FRONT].mag_odr < param->mag_odr){ret = -1;break;} if(ob_ser_imu.compre_drv_param[SER_IMU_DIR_FRONT].timestamp_resolution < param->timestamp_resolution){ret = -1;break;} if(ob_ser_imu.compre_drv_param[SER_IMU_DIR_FRONT].timestamp_switch < param->timestamp_switch){ret = -1;break;} break; case SER_IMU_DIR_BACK: if(ob_ser_imu.compre_drv_param[SER_IMU_DIR_BACK].mag_fs < param->mag_fs){ret = -1;break;} if(ob_ser_imu.compre_drv_param[SER_IMU_DIR_BACK].mag_odr < param->mag_odr){ret = -1;break;} break; default: break; } return ret; } /** @brief 获取IMU当前数据数量 @param dir - [in] IMU方向 - 前/后脚 @return 错误代码 - [out] 返回数据数量 */ int hal_ser_imu_get_data_num(SER_IMU_DIR_e dir) { int ret = 0; switch(dir) { case SER_IMU_DIR_FRONT: ret = ob_ser_imu.cur_data_num[SER_IMU_DIR_FRONT]; break; case SER_IMU_DIR_BACK: ret = ob_ser_imu.cur_data_num[SER_IMU_DIR_BACK]; break; default: break; } return ret; } /** @brief 获取IMU数据 @param dir - [in] IMU方向 - 前/后脚 @param index - [in] 第几组数据 @param pdata - [out] IMU数据 @return 错误代码 - [out] -1失败，0成功 */ int hal_ser_imu_get_data(SER_IMU_DIR_e dir, int index, ser_imu_data_t *pdata) { if(index < 0)return -1; *pdata = ob_ser_imu.cur_data[dir][index]; return 0; } /** @brief 数据更新通知 @param p_cb - [in] 需要回调的函数 @return 错误代码 - [out] -1失败，0成功，-2已存在 */ int hal_ser_imu_data_update_notify(hal_ser_imu_data_update_callback p_cb) { for(int i=0;i