/** 功耗： 闲置模式功耗为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 "drv_qmc6310_v2.h" #include "drv_lsm6ds3tr_c.h" #include "fml_imu.h" /*Private macro ------------------------------------------------------------------------------------------------------------------------------------*/ #define FML_IMU_DATA_GROUP_NUM_MAX 20 //IMU能存储的最大数据组数 /*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; //数据通知回调 } 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_104HZ: drv_param->lsm.acc_odr = LSM_ACC_ODR_104HZ; break; case FML_IMU_ACC_ODR_12HZ5: drv_param->lsm.acc_odr = LSM_ACC_ODR_12HZ5; 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_104HZ: drv_param->lsm.gry_odr = LSM_GRY_ODR_104HZ; break; case FML_IMU_GRY_ODR_12HZ5: drv_param->lsm.gry_odr = LSM_GRY_ODR_12HZ5; 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_104HZ: drv_param->lsm.fifo_odr = LSM_FIFO_ODR_104HZ; 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 drv_param; 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; ob_fml_imu.config_flow = 1; break; case 1: //上电lsm ret = drv_lsm_power_on(); if(ret != 0)return -1; //上电qmc ret = drv_qmc6310_power_on(); if(ret != 0)return -1; ob_fml_imu.config_flow = 2; 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], &drv_param); fml_imu_macro_conversion(FML_IMU_DIR_BACK, &ob_fml_imu.config_param[FML_IMU_DIR_BACK], &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(drv_param.lsm.mag_odr, 1, i); if(ret != 0)return -1; ob_fml_imu.config_flow++; } } else { for(int i=1; i <= lsm_mag_flow; i++) { ret = drv_lsm_set_mag_odr(drv_param.lsm.mag_odr, 0, i); if(ret != 0)return -1; ob_fml_imu.config_flow++; } } //配置lsm的加速度工作模式 ret = drv_lsm_set_acc_power_mode(drv_param.lsm.acc_power_mode); if(ret != 0)return -1; ob_fml_imu.config_flow++; //配置lsm的陀螺仪工作模式 ret = drv_lsm_set_gry_power_mode(drv_param.lsm.gry_power_mode); if(ret != 0)return -1; ob_fml_imu.config_flow++; //配置lsm的加速度量程 ret = drv_lsm_set_acc_fs(drv_param.lsm.acc_fs); if(ret != 0)return -1; ob_fml_imu.config_flow++; //配置lsm的陀螺仪量程 ret = drv_lsm_set_gry_fs(drv_param.lsm.gry_fs); if(ret != 0)return -1; ob_fml_imu.config_flow++; //配置lsm的时间戳精度 ret = drv_lsm_set_timestamp_resolution(drv_param.lsm.timestamp_resolution); if(ret != 0)return -1; ob_fml_imu.config_flow++; //配置lsm的时间戳开关 ret = drv_lsm_set_timestamp_switch(drv_param.lsm.timestamp_switch); if(ret != 0)return -1; ob_fml_imu.config_flow++; //配置lsm的FIFO采样频率（必须以上都配置成功，才能配置FIFO） ret = drv_lsm_set_fifo_odr(drv_param.lsm.fifo_odr, \ drv_param.lsm.acc_odr, \ drv_param.lsm.gry_odr, \ drv_param.lsm.mag_odr, \ drv_param.lsm.timestamp_switch); if(ret != 0)return -1; ob_fml_imu.config_flow++; //配置lsm的加速度采样频率 ret = drv_lsm_set_acc_odr(drv_param.lsm.acc_odr); if(ret != 0)return -1; ob_fml_imu.config_flow++; //配置lsm的陀螺仪采样频率 ret = drv_lsm_set_gry_odr(drv_param.lsm.gry_odr); 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(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 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); } //数据回调通知 if(dir_bit != 0)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; //断电前后脚 fml_imu_close(FML_IMU_DIR_FRONT); fml_imu_close(FML_IMU_DIR_BACK); //通知配置失败 ob_fml_imu.config_cb(ob_fml_imu.config_flow); } else { if(ob_fml_imu.config_flow == FML_IMU_CONFIG_FLOW_DONE) { //配置成功 ob_fml_imu.stage = FML_IMU_CONFIG_STAGE_DONE; //通知配置成功 ob_fml_imu.config_cb(ob_fml_imu.config_flow); } } break; default: 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_Process",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, 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 param 无 @return 错误代码 - [out] -1失败，0成功 */ int fml_imu_start_config(void) { drv_param_u drv_param; //获取驱动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); //转换参数 fml_imu_macro_conversion(FML_IMU_DIR_FRONT, &ob_fml_imu.config_param[FML_IMU_DIR_FRONT], &drv_param); fml_imu_macro_conversion(FML_IMU_DIR_BACK, &ob_fml_imu.config_param[FML_IMU_DIR_BACK], &drv_param); //进行对比 if((memcmp(&ob_fml_imu.drv_param[FML_IMU_DIR_FRONT].lsm,&drv_param.lsm,sizeof(drv_param.lsm)) == 0) || \ (memcmp(&ob_fml_imu.drv_param[FML_IMU_DIR_BACK].qmc,&drv_param.qmc,sizeof(drv_param.qmc)) == 0) ) { ob_fml_imu.stage = FML_IMU_CONFIG_STAGE_IN_PROGRESS; ob_fml_imu.config_flow = 0; } 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 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(); break; case FML_IMU_DIR_BACK: drv_qmc6310_power_on(); //主要是复位结构体 drv_qmc6310_power_off(); break; default: break; } return 0; }