/*Includes ----------------------------------------------*/ #include "system.h" #include "exception.h" #include "bsp_time.h" #include "hal_ser_imu.h" #include "hal_ser_imu_mode_manage.h" /*Private macro ------------------------------------------------------------------------------------------------------------------------------------*/ /*STRUCTION ------------------------------------------------------------------------------------------------------------------------------------*/ typedef struct ser_imu_mode_manage { /*private member*/ bool is_imu_id_unusual; //imu的ID是否异常 bool is_imu_damage; //IMU是否损坏 HAL_SER_IMU_MODE_MANAGE_e last_enter_mode; //上次成功进入的模式 int mode_switch[HAL_SER_IMU_MODE_MANAGE_NUM]; //模式开关 int ser_imu_handle_id[HAL_SER_IMU_MODE_MANAGE_NUM]; //模式的IMU服务句柄ID ser_imu_config_param_t ser_imu_param[HAL_SER_IMU_MODE_MANAGE_NUM][SER_IMU_DIR_NUM]; //模式的IMU配置参数 ser_imu_data_t ser_imu_data[HAL_SER_IMU_MODE_MANAGE_NUM][SER_IMU_DIR_NUM]; //模式的IMU服务数据 } Ser_Imu_Mode_Manage_t; /*Local Variable ------------------------------------------------------------------------------------------------------------------------------------*/ static Ser_Imu_Mode_Manage_t ob_ser_imu_mode_manage; //模式管理服务对象 static ser_imu_config_param_t imu_power_off_param; //关机配置参数 /*Local Functions ------------------------------------------------------------------------------------------------------------------------------------*/ static void hal_ser_imu_mode_manage_imu_power_off_init(ser_imu_config_param_t *p_param) { p_param->acc_power_mode = SER_IMU_ACC_POWER_MODE_NORMAL; //前脚 - 加速度正常模式 p_param->gry_power_mode = SER_IMU_GRY_POWER_MODE_NORMAL; //前脚 - 陀螺仪正常模式 p_param->timestamp_resolution = SER_IMU_TIMESTAMP_25US; //前脚 - 时间戳25US精度 p_param->timestamp_switch = SER_IMU_TIMESTAMP_OFF; //前脚 - 时间戳关闭 p_param->acc_fs = SER_IMU_ACC_FS_16G; //前脚 - 加速度量程 - 16G p_param->gry_fs = SER_IMU_GRY_FS_2000DPS; //前脚 - 陀螺仪量程 - 2000DPS p_param->mag_fs = SER_IMU_MAG_FS_30GS; //前脚 - 地磁计量程 - 30GS p_param->acc_odr = SER_IMU_ACC_ODR_OFF; //后脚 - 加速度采样频率 - 关闭 p_param->gry_odr = SER_IMU_GRY_ODR_OFF; //前脚 - 陀螺仪采样频率 - 关闭 p_param->mag_odr = SER_IMU_MAG_ODR_OFF; //前脚 - 地磁计采样频率 - 关闭 p_param->fifo_odr = SER_IMU_FIFO_ODR_OFF; //前脚 - FIFO采样频率 - 关闭 } static void hal_ser_imu_mode_manage_imu_standby_power_init(HAL_SER_IMU_MODE_MANAGE_e mode) { ob_ser_imu_mode_manage.ser_imu_param[mode][SER_IMU_DIR_FRONT].acc_power_mode = SER_IMU_ACC_POWER_MODE_NORMAL; //前脚 - 加速度正常模式 ob_ser_imu_mode_manage.ser_imu_param[mode][SER_IMU_DIR_FRONT].gry_power_mode = SER_IMU_GRY_POWER_MODE_NORMAL; //前脚 - 陀螺仪正常模式 ob_ser_imu_mode_manage.ser_imu_param[mode][SER_IMU_DIR_FRONT].timestamp_resolution = SER_IMU_TIMESTAMP_25US; //前脚 - 时间戳25US精度 ob_ser_imu_mode_manage.ser_imu_param[mode][SER_IMU_DIR_FRONT].timestamp_switch = SER_IMU_TIMESTAMP_OFF; //前脚 - 时间戳关闭 ob_ser_imu_mode_manage.ser_imu_param[mode][SER_IMU_DIR_FRONT].acc_fs = SER_IMU_ACC_FS_16G; //前脚 - 加速度量程 - 16G ob_ser_imu_mode_manage.ser_imu_param[mode][SER_IMU_DIR_FRONT].gry_fs = SER_IMU_GRY_FS_2000DPS; //前脚 - 陀螺仪量程 - 2000DPS ob_ser_imu_mode_manage.ser_imu_param[mode][SER_IMU_DIR_FRONT].mag_fs = SER_IMU_MAG_FS_30GS; //前脚 - 地磁计量程 - 30GS ob_ser_imu_mode_manage.ser_imu_param[mode][SER_IMU_DIR_FRONT].acc_odr = SER_IMU_ACC_ODR_12HZ5; //前脚 - 加速度采样频率 - 12.5HZ ob_ser_imu_mode_manage.ser_imu_param[mode][SER_IMU_DIR_FRONT].gry_odr = SER_IMU_GRY_ODR_OFF; //前脚 - 陀螺仪采样频率 - 关闭 ob_ser_imu_mode_manage.ser_imu_param[mode][SER_IMU_DIR_FRONT].mag_odr = SER_IMU_MAG_ODR_OFF; //前脚 - 地磁计采样频率 - 关闭 ob_ser_imu_mode_manage.ser_imu_param[mode][SER_IMU_DIR_FRONT].fifo_odr = SER_IMU_FIFO_ODR_OFF; //前脚 - FIFO采样频率 - 关闭 ob_ser_imu_mode_manage.ser_imu_param[mode][SER_IMU_DIR_BACK].acc_power_mode = SER_IMU_ACC_POWER_MODE_NORMAL; //后脚 - 加速度正常模式 ob_ser_imu_mode_manage.ser_imu_param[mode][SER_IMU_DIR_BACK].gry_power_mode = SER_IMU_GRY_POWER_MODE_NORMAL; //后脚 - 陀螺仪正常模式 ob_ser_imu_mode_manage.ser_imu_param[mode][SER_IMU_DIR_BACK].timestamp_resolution = SER_IMU_TIMESTAMP_25US; //后脚 - 时间戳25US精度 ob_ser_imu_mode_manage.ser_imu_param[mode][SER_IMU_DIR_BACK].timestamp_switch = SER_IMU_TIMESTAMP_OFF; //后脚 - 时间戳关闭 ob_ser_imu_mode_manage.ser_imu_param[mode][SER_IMU_DIR_BACK].acc_fs = SER_IMU_ACC_FS_16G; //后脚 - 加速度量程 - 16G ob_ser_imu_mode_manage.ser_imu_param[mode][SER_IMU_DIR_BACK].gry_fs = SER_IMU_GRY_FS_2000DPS; //后脚 - 陀螺仪量程 - 2000DPS ob_ser_imu_mode_manage.ser_imu_param[mode][SER_IMU_DIR_BACK].mag_fs = SER_IMU_MAG_FS_30GS; //后脚 - 地磁计量程 - 30GS ob_ser_imu_mode_manage.ser_imu_param[mode][SER_IMU_DIR_BACK].acc_odr = SER_IMU_ACC_ODR_OFF; //后脚 - 加速度采样频率 - 关闭 ob_ser_imu_mode_manage.ser_imu_param[mode][SER_IMU_DIR_BACK].gry_odr = SER_IMU_GRY_ODR_OFF; //后脚 - 陀螺仪采样频率 - 关闭 ob_ser_imu_mode_manage.ser_imu_param[mode][SER_IMU_DIR_BACK].mag_odr = SER_IMU_MAG_ODR_10HZ; //后脚 - 地磁计采样频率 - 10HZ ob_ser_imu_mode_manage.ser_imu_param[mode][SER_IMU_DIR_BACK].fifo_odr = SER_IMU_FIFO_ODR_OFF; //后脚 - FIFO采样频率 - 关闭 } static void hal_ser_imu_mode_manage_imu_low_power_init(HAL_SER_IMU_MODE_MANAGE_e mode) { ob_ser_imu_mode_manage.ser_imu_param[mode][SER_IMU_DIR_FRONT].acc_power_mode = SER_IMU_ACC_POWER_MODE_NORMAL; //前脚 - 加速度正常模式 ob_ser_imu_mode_manage.ser_imu_param[mode][SER_IMU_DIR_FRONT].gry_power_mode = SER_IMU_GRY_POWER_MODE_NORMAL; //前脚 - 陀螺仪正常模式 ob_ser_imu_mode_manage.ser_imu_param[mode][SER_IMU_DIR_FRONT].timestamp_resolution = SER_IMU_TIMESTAMP_25US; //前脚 - 时间戳25US精度 ob_ser_imu_mode_manage.ser_imu_param[mode][SER_IMU_DIR_FRONT].timestamp_switch = SER_IMU_TIMESTAMP_OFF; //前脚 - 时间戳关闭 ob_ser_imu_mode_manage.ser_imu_param[mode][SER_IMU_DIR_FRONT].acc_fs = SER_IMU_ACC_FS_16G; //前脚 - 加速度量程 - 16G ob_ser_imu_mode_manage.ser_imu_param[mode][SER_IMU_DIR_FRONT].gry_fs = SER_IMU_GRY_FS_2000DPS; //前脚 - 陀螺仪量程 - 2000DPS ob_ser_imu_mode_manage.ser_imu_param[mode][SER_IMU_DIR_FRONT].mag_fs = SER_IMU_MAG_FS_30GS; //前脚 - 地磁计量程 - 30GS ob_ser_imu_mode_manage.ser_imu_param[mode][SER_IMU_DIR_FRONT].acc_odr = SER_IMU_ACC_ODR_104HZ; //前脚 - 加速度采样频率 - 104HZ ob_ser_imu_mode_manage.ser_imu_param[mode][SER_IMU_DIR_FRONT].gry_odr = SER_IMU_GRY_ODR_OFF; //前脚 - 陀螺仪采样频率 - 关闭 ob_ser_imu_mode_manage.ser_imu_param[mode][SER_IMU_DIR_FRONT].mag_odr = SER_IMU_MAG_ODR_100HZ; //前脚 - 地磁计采样频率 - 100HZ ob_ser_imu_mode_manage.ser_imu_param[mode][SER_IMU_DIR_FRONT].fifo_odr = SER_IMU_FIFO_ODR_104HZ; //前脚 - FIFO采样频率 - 104HZ ob_ser_imu_mode_manage.ser_imu_param[mode][SER_IMU_DIR_BACK].acc_power_mode = SER_IMU_ACC_POWER_MODE_NORMAL; //后脚 - 加速度正常模式 ob_ser_imu_mode_manage.ser_imu_param[mode][SER_IMU_DIR_BACK].gry_power_mode = SER_IMU_GRY_POWER_MODE_NORMAL; //后脚 - 陀螺仪正常模式 ob_ser_imu_mode_manage.ser_imu_param[mode][SER_IMU_DIR_BACK].timestamp_resolution = SER_IMU_TIMESTAMP_25US; //后脚 - 时间戳25US精度 ob_ser_imu_mode_manage.ser_imu_param[mode][SER_IMU_DIR_BACK].timestamp_switch = SER_IMU_TIMESTAMP_OFF; //后脚 - 时间戳关闭 ob_ser_imu_mode_manage.ser_imu_param[mode][SER_IMU_DIR_BACK].acc_fs = SER_IMU_ACC_FS_16G; //后脚 - 加速度量程 - 16G ob_ser_imu_mode_manage.ser_imu_param[mode][SER_IMU_DIR_BACK].gry_fs = SER_IMU_GRY_FS_2000DPS; //后脚 - 陀螺仪量程 - 2000DPS ob_ser_imu_mode_manage.ser_imu_param[mode][SER_IMU_DIR_BACK].mag_fs = SER_IMU_MAG_FS_30GS; //后脚 - 地磁计量程 - 30GS ob_ser_imu_mode_manage.ser_imu_param[mode][SER_IMU_DIR_BACK].acc_odr = SER_IMU_ACC_ODR_OFF; //后脚 - 加速度采样频率 - 关闭 ob_ser_imu_mode_manage.ser_imu_param[mode][SER_IMU_DIR_BACK].gry_odr = SER_IMU_GRY_ODR_OFF; //后脚 - 陀螺仪采样频率 - 关闭 ob_ser_imu_mode_manage.ser_imu_param[mode][SER_IMU_DIR_BACK].mag_odr = SER_IMU_MAG_ODR_OFF; //后脚 - 地磁计采样频率 - 关闭 ob_ser_imu_mode_manage.ser_imu_param[mode][SER_IMU_DIR_BACK].fifo_odr = SER_IMU_FIFO_ODR_OFF; //后脚 - FIFO采样频率 - 关闭 } static void hal_ser_imu_mode_manage_imu_full_power_init(HAL_SER_IMU_MODE_MANAGE_e mode) { ob_ser_imu_mode_manage.ser_imu_param[mode][SER_IMU_DIR_FRONT].acc_power_mode = SER_IMU_ACC_POWER_MODE_NORMAL; //前脚 - 加速度正常模式 ob_ser_imu_mode_manage.ser_imu_param[mode][SER_IMU_DIR_FRONT].gry_power_mode = SER_IMU_GRY_POWER_MODE_NORMAL; //前脚 - 陀螺仪正常模式 ob_ser_imu_mode_manage.ser_imu_param[mode][SER_IMU_DIR_FRONT].timestamp_resolution = SER_IMU_TIMESTAMP_25US; //前脚 - 时间戳25US精度 ob_ser_imu_mode_manage.ser_imu_param[mode][SER_IMU_DIR_FRONT].timestamp_switch = SER_IMU_TIMESTAMP_ON; //前脚 - 时间戳开启 ob_ser_imu_mode_manage.ser_imu_param[mode][SER_IMU_DIR_FRONT].acc_fs = SER_IMU_ACC_FS_16G; //前脚 - 加速度量程 - 16G ob_ser_imu_mode_manage.ser_imu_param[mode][SER_IMU_DIR_FRONT].gry_fs = SER_IMU_GRY_FS_2000DPS; //前脚 - 陀螺仪量程 - 2000DPS ob_ser_imu_mode_manage.ser_imu_param[mode][SER_IMU_DIR_FRONT].mag_fs = SER_IMU_MAG_FS_30GS; //前脚 - 地磁计量程 - 30GS ob_ser_imu_mode_manage.ser_imu_param[mode][SER_IMU_DIR_FRONT].acc_odr = SER_IMU_ACC_ODR_104HZ; //前脚 - 加速度采样频率 - 104HZ ob_ser_imu_mode_manage.ser_imu_param[mode][SER_IMU_DIR_FRONT].gry_odr = SER_IMU_GRY_ODR_104HZ; //前脚 - 陀螺仪采样频率 - 104HZ ob_ser_imu_mode_manage.ser_imu_param[mode][SER_IMU_DIR_FRONT].mag_odr = SER_IMU_MAG_ODR_200HZ; //前脚 - 地磁计采样频率 - 200HZ ob_ser_imu_mode_manage.ser_imu_param[mode][SER_IMU_DIR_FRONT].fifo_odr = SER_IMU_FIFO_ODR_104HZ; //前脚 - FIFO采样频率 - 104HZ ob_ser_imu_mode_manage.ser_imu_param[mode][SER_IMU_DIR_BACK].acc_power_mode = SER_IMU_ACC_POWER_MODE_NORMAL; //后脚 - 加速度正常模式 ob_ser_imu_mode_manage.ser_imu_param[mode][SER_IMU_DIR_BACK].gry_power_mode = SER_IMU_GRY_POWER_MODE_NORMAL; //后脚 - 陀螺仪正常模式 ob_ser_imu_mode_manage.ser_imu_param[mode][SER_IMU_DIR_BACK].timestamp_resolution = SER_IMU_TIMESTAMP_25US; //后脚 - 时间戳25US精度 ob_ser_imu_mode_manage.ser_imu_param[mode][SER_IMU_DIR_BACK].timestamp_switch = SER_IMU_TIMESTAMP_OFF; //后脚 - 时间戳关闭 ob_ser_imu_mode_manage.ser_imu_param[mode][SER_IMU_DIR_BACK].acc_fs = SER_IMU_ACC_FS_16G; //后脚 - 加速度量程 - 16G ob_ser_imu_mode_manage.ser_imu_param[mode][SER_IMU_DIR_BACK].gry_fs = SER_IMU_GRY_FS_2000DPS; //后脚 - 陀螺仪量程 - 2000DPS ob_ser_imu_mode_manage.ser_imu_param[mode][SER_IMU_DIR_BACK].mag_fs = SER_IMU_MAG_FS_30GS; //后脚 - 地磁计量程 - 30GS ob_ser_imu_mode_manage.ser_imu_param[mode][SER_IMU_DIR_BACK].acc_odr = SER_IMU_ACC_ODR_OFF; //后脚 - 加速度采样频率 - 关闭 ob_ser_imu_mode_manage.ser_imu_param[mode][SER_IMU_DIR_BACK].gry_odr = SER_IMU_GRY_ODR_OFF; //后脚 - 陀螺仪采样频率 - 关闭 ob_ser_imu_mode_manage.ser_imu_param[mode][SER_IMU_DIR_BACK].mag_odr = SER_IMU_MAG_ODR_200HZ; //后脚 - 地磁计采样频率 - 200HZ ob_ser_imu_mode_manage.ser_imu_param[mode][SER_IMU_DIR_BACK].fifo_odr = SER_IMU_FIFO_ODR_OFF; //后脚 - FIFO采样频率 - 关闭 } static void hal_ser_imu_mode_manage_printf_Process(void) { DEBUG_LOG("hal_ser_imu_mode_manage----------------------------------------------------------------->\n"); DEBUG_LOG("mode_switch:%d,%d,%d,%d,%d\n",ob_ser_imu_mode_manage.mode_switch[HAL_SER_IMU_MODE_MANAGE_STANDBY], \ ob_ser_imu_mode_manage.mode_switch[HAL_SER_IMU_MODE_MANAGE_NORMAL], \ ob_ser_imu_mode_manage.mode_switch[HAL_SER_IMU_MODE_MANAGE_SELF_CHECK], \ ob_ser_imu_mode_manage.mode_switch[HAL_SER_IMU_MODE_MANAGE_GAME], \ ob_ser_imu_mode_manage.mode_switch[HAL_SER_IMU_MODE_MANAGE_REALSTEP]); DEBUG_LOG("<-----------------------------------------------------------------hal_ser_imu_mode_manage\n"); } static void hal_ser_imu_mode_manage_err_update_Process(void) { char err_buff[100]; if(ob_ser_imu_mode_manage.is_imu_damage) { //发送一次不能进入其他模式的异常上报。 sprintf(err_buff,"enter_imu_damage_mode,last_enter_mode:%d,%d\r\n",ob_ser_imu_mode_manage.last_enter_mode,ob_ser_imu_mode_manage.is_imu_id_unusual); Except_TxError(EXCEPT_IMU_SUSPEND_OVERFLOW,err_buff); } else { //如果IMU配置失败进入挂起模式 if(Except_IsError(EXCEPT_IMU_SUSPEND_OVERFLOW)){ //IMU损坏 ob_ser_imu_mode_manage.is_imu_damage = true; } } } /*API ------------------------------------------------------------------------------------------------------------------------------------*/ /** @brief 初始化模式管理服务 @param 无 @return 错误代码 - [out] -1失败，0成功 */ int hal_ser_imu_mode_manage_Init(void) { int ret = 0; //初始化imu服务 if(hal_ser_imu_Init() == -1)ret = -1; //重置模式管理服务结构体 memset(&ob_ser_imu_mode_manage, 0, sizeof(ob_ser_imu_mode_manage)); /*初始化模式管理服务结构体*/ if(ret == -1) { ob_ser_imu_mode_manage.is_imu_id_unusual = true; ob_ser_imu_mode_manage.is_imu_damage = true; } else { ob_ser_imu_mode_manage.is_imu_id_unusual = false; ob_ser_imu_mode_manage.is_imu_damage = false; } ob_ser_imu_mode_manage.last_enter_mode = HAL_SER_IMU_MODE_MANAGE_NUM; //初始化-待机模式配置 ob_ser_imu_mode_manage.mode_switch[HAL_SER_IMU_MODE_MANAGE_STANDBY] = 0; //模式关闭 hal_ser_imu_get_handle("HAL_SER_IMU_MODE_MANAGE_STANDBY", &ob_ser_imu_mode_manage.ser_imu_handle_id[HAL_SER_IMU_MODE_MANAGE_STANDBY]); //获取操作IMU服务句柄ID hal_ser_imu_mode_manage_imu_standby_power_init(HAL_SER_IMU_MODE_MANAGE_STANDBY); //设置IMU服务配置参数 - 待机配置 //初始化-正常模式配置 ob_ser_imu_mode_manage.mode_switch[HAL_SER_IMU_MODE_MANAGE_NORMAL] = 0; //模式关闭 hal_ser_imu_get_handle("HAL_SER_IMU_MODE_MANAGE_NORMAL", &ob_ser_imu_mode_manage.ser_imu_handle_id[HAL_SER_IMU_MODE_MANAGE_NORMAL]); //获取操作IMU服务句柄ID hal_ser_imu_mode_manage_imu_low_power_init(HAL_SER_IMU_MODE_MANAGE_NORMAL); //设置IMU服务配置参数 - 低功耗配置 //初始化-自检模式配置 ob_ser_imu_mode_manage.mode_switch[HAL_SER_IMU_MODE_MANAGE_SELF_CHECK] = 0; //模式关闭 hal_ser_imu_get_handle("HAL_SER_IMU_MODE_MANAGE_SELF_CHECK", &ob_ser_imu_mode_manage.ser_imu_handle_id[HAL_SER_IMU_MODE_MANAGE_SELF_CHECK]); //获取操作IMU服务句柄ID hal_ser_imu_mode_manage_imu_full_power_init(HAL_SER_IMU_MODE_MANAGE_SELF_CHECK); //设置IMU服务配置参数 - 全功率配置 //初始化-游戏模式配置 ob_ser_imu_mode_manage.mode_switch[HAL_SER_IMU_MODE_MANAGE_GAME] = 0; //模式关闭 hal_ser_imu_get_handle("HAL_SER_IMU_MODE_MANAGE_GAME", &ob_ser_imu_mode_manage.ser_imu_handle_id[HAL_SER_IMU_MODE_MANAGE_GAME]); //获取操作IMU服务句柄ID hal_ser_imu_mode_manage_imu_full_power_init(HAL_SER_IMU_MODE_MANAGE_GAME); //设置IMU服务配置参数 - 全功率配置 //初始化-校准模式配置 ob_ser_imu_mode_manage.mode_switch[HAL_SER_IMU_MODE_MANAGE_CALIBRATION] = 0; //模式关闭 hal_ser_imu_get_handle("HAL_SER_IMU_MODE_MANAGE_CALIBRATION", &ob_ser_imu_mode_manage.ser_imu_handle_id[HAL_SER_IMU_MODE_MANAGE_CALIBRATION]);//获取操作IMU服务句柄ID hal_ser_imu_mode_manage_imu_full_power_init(HAL_SER_IMU_MODE_MANAGE_CALIBRATION); //初始化-实时计步模式配置 ob_ser_imu_mode_manage.mode_switch[HAL_SER_IMU_MODE_MANAGE_REALSTEP] = 0; //模式关闭 hal_ser_imu_get_handle("HAL_SER_IMU_MODE_MANAGE_REALSTEP", &ob_ser_imu_mode_manage.ser_imu_handle_id[HAL_SER_IMU_MODE_MANAGE_REALSTEP]); //获取操作IMU服务句柄ID hal_ser_imu_mode_manage_imu_full_power_init(HAL_SER_IMU_MODE_MANAGE_REALSTEP); //设置IMU服务配置参数 - 全功率配置 //初始化关机配置参数 hal_ser_imu_mode_manage_imu_power_off_init(&imu_power_off_param); DEBUG_LOG("sizeof(%d)\n",sizeof(ob_ser_imu_mode_manage)); // Process_Start(1000,"hal_ser_imu_mode_manage_printf_Process",hal_ser_imu_mode_manage_printf_Process); Process_Start(5000,"hal_ser_imu_mode_manage_err_update_Process",hal_ser_imu_mode_manage_err_update_Process); if(ret == -1)return -1; return 0; } /** @brief 请求需要的模式 @param mode - [in] 需要请求的模式 @param imu_mode_switch - [in] 请求的模式开关 @return 错误代码 - [out] -1失败，0成功 */ int hal_ser_imu_mode_manage_set_required(HAL_SER_IMU_MODE_MANAGE_e mode, HAL_SER_IMU_MODE_MANAGE_IMU_MODE_SWITCH_e imu_mode_switch) { //操作模式不能超过模式数量 if(mode >= HAL_SER_IMU_MODE_MANAGE_NUM)return -1; //设置模式开关 switch(imu_mode_switch) { case HAL_SER_IMU_MODE_MANAGE_IMU_MODE_ON: if(ob_ser_imu_mode_manage.mode_switch[mode] == 0)ob_ser_imu_mode_manage.mode_switch[mode]++; break; case HAL_SER_IMU_MODE_MANAGE_IMU_MODE_OFF: if(ob_ser_imu_mode_manage.mode_switch[mode] != 0)ob_ser_imu_mode_manage.mode_switch[mode]--; break; } // //如果IMU损毁 // if(ob_ser_imu_mode_manage.is_imu_damage) // { // //清空 // memset(ob_ser_imu_mode_manage.mode_switch,0,sizeof(ob_ser_imu_mode_manage.mode_switch)); // //直接强制性不穿鞋模式 // ob_ser_imu_mode_manage.mode_switch[HAL_SER_IMU_MODE_MANAGE_STANDBY] = 1; // } //设置各个模式下的配置 for(int i = 0; i < HAL_SER_IMU_MODE_MANAGE_NUM; i++) { if(ob_ser_imu_mode_manage.mode_switch[i] > 0)//模式开启 { //设置前脚IMU配置参数 if(hal_ser_imu_set_required_param(ob_ser_imu_mode_manage.ser_imu_handle_id[i], SER_IMU_DIR_FRONT, &ob_ser_imu_mode_manage.ser_imu_param[i][SER_IMU_DIR_FRONT]) == -1)return -1; //设置后脚IMU配置参数 if(hal_ser_imu_set_required_param(ob_ser_imu_mode_manage.ser_imu_handle_id[i], SER_IMU_DIR_BACK, &ob_ser_imu_mode_manage.ser_imu_param[i][SER_IMU_DIR_BACK]) == -1)return -1; } else //模式关闭 { //设置前脚IMU配置参数 if(hal_ser_imu_set_required_param(ob_ser_imu_mode_manage.ser_imu_handle_id[i], SER_IMU_DIR_FRONT, &imu_power_off_param) == -1)return -1; //设置后脚IMU配置参数 if(hal_ser_imu_set_required_param(ob_ser_imu_mode_manage.ser_imu_handle_id[i], SER_IMU_DIR_BACK, &imu_power_off_param) == -1)return -1; } } return 0; } /** @brief 模式是否准备好 @param mode - [in] 需要的模式 @return 错误代码 - [out] -1失败，0成功 */ int hal_ser_imu_mode_manage_get_ready(HAL_SER_IMU_MODE_MANAGE_e mode) { //操作模式不能超过模式数量 if(mode >= HAL_SER_IMU_MODE_MANAGE_NUM)return -1; if(ob_ser_imu_mode_manage.mode_switch[mode] == 0) { return -1; } else { //判断前脚IMU配置参数是否准备好 if(hal_ser_imu_is_param_get_ready(SER_IMU_DIR_FRONT, &ob_ser_imu_mode_manage.ser_imu_param[mode][SER_IMU_DIR_FRONT]) != -1) { //判断后脚IMU配置参数是否准备好 if(hal_ser_imu_is_param_get_ready(SER_IMU_DIR_BACK, &ob_ser_imu_mode_manage.ser_imu_param[mode][SER_IMU_DIR_BACK]) != -1) { ob_ser_imu_mode_manage.last_enter_mode = mode; return 0; } } } return -1; } /** @brief 获取当前模式的IMU当前数据数量 @param mode - [in] 需要的模式 @param dir - [in] IMU方向 - 前/后脚 @return 错误代码 - [out] 返回数据数量 */ int hal_ser_imu_mode_manage_get_data_num(SER_IMU_DIR_e dir) { return hal_ser_imu_get_data_num(dir); } /** @brief 获取当前模式的IMU数据 @param dir - [in] IMU方向 - 前/后脚 @param index - [in] 第几组数据 @param pdata - [out] IMU数据 @return 错误代码 - [out] -1失败，0成功 */ int hal_ser_imu_mode_manage_get_data(SER_IMU_DIR_e dir, int index, ser_imu_data_t *pdata) { return hal_ser_imu_get_data(dir, index, pdata); } /** @brief 查询当前是否挂起模式 @return 错误代码 - [out] -1 非挂起模式，0 挂起模式 */ int hal_ser_imu_mode_IsSuspend(void) { for(uint8_t i = HAL_SER_IMU_MODE_MANAGE_STANDBY;i< HAL_SER_IMU_MODE_MANAGE_NUM;i++){ if( 0 == hal_ser_imu_mode_manage_get_ready((HAL_SER_IMU_MODE_MANAGE_e)i)){ return -1; } } return 0; }