#include "hal_wearshoes.h" #include "bsp_time.h" #include "system.h" #include "tool.h" #include "exception.h" #include "bll_imu.h" #define HAL_WEARSHOES_TIMEOUT 60000 #define Bufflength 6 static int32_t buff[Bufflength]={0}; static uint8_t refreshFlag =1; //穿鞋 static const bll_imu_one_way_param_t shoes_front_param={ .acc_power_mode = FML_IMU_ACC_POWER_MODE_NORMAL, //前脚 - 加速度正常模式 .gry_power_mode = FML_IMU_GRY_POWER_MODE_NORMAL, //前脚 - 陀螺仪正常模式 .fifo_odr = FML_IMU_FIFO_ODR_104HZ, //前脚 --FIFO采集频率 .timestamp_resolution = FML_IMU_TIMESTAMP_25US, //前脚 - 时间戳25US精度 .timestamp_switch = FML_IMU_TIMESTAMP_OFF, //前脚 - 时间戳关闭 .acc_odr = FML_IMU_ACC_ODR_104HZ, //前脚 - 加速度采样频率 - 104HZ .gry_odr = FML_IMU_GRY_ODR_OFF, //前脚 - 陀螺仪采样频率 关闭 .mag_odr = FML_IMU_MAG_ODR_100HZ, //前脚 - 地磁计采样频率 - 100HZ .acc_fs = FML_IMU_ACC_FS_16G, //前脚 - 加速度量程 - 16G .gry_fs = FML_IMU_GRY_FS_2000DPS, //前脚 - 陀螺仪量程 - 2000DPS .mag_fs = FML_IMU_MAG_FS_30GS, //前脚 - 地磁计量程 - 30GS }; static const bll_imu_one_way_param_t shoes_back_param={ .acc_power_mode = FML_IMU_ACC_POWER_MODE_NORMAL, //后脚 - 加速度正常模式 .gry_power_mode = FML_IMU_GRY_POWER_MODE_NORMAL, //后脚 - 陀螺仪正常模式 .fifo_odr = FML_IMU_FIFO_ODR_OFF, //后脚 --FIFO采集频率 .timestamp_resolution = FML_IMU_TIMESTAMP_25US, //后脚 - 时间戳25US精度 .timestamp_switch = FML_IMU_TIMESTAMP_OFF, //后脚 - 时间戳关闭 .acc_odr = FML_IMU_ACC_ODR_OFF, //后脚 - 加速度采样频率 - 关闭 .gry_odr = FML_IMU_GRY_ODR_OFF, //后脚 - 陀螺仪采样频率 - 关闭 .mag_odr = FML_IMU_MAG_ODR_OFF, //后脚 - 地磁计采样频率 - 200HZ .acc_fs = FML_IMU_ACC_FS_16G, //后脚 - 加速度量程 - 16G .gry_fs = FML_IMU_GRY_FS_2000DPS, //后脚 - 陀螺仪量程 - 2000DPS .mag_fs = FML_IMU_MAG_FS_30GS, //后脚 - 地磁计量程 - 30GS }; static const bll_imu_param_t shoes_bll_imu_param_t={ .config_param[FML_IMU_DIR_FRONT] = (bll_imu_one_way_param_t *)&shoes_front_param, .config_param[FML_IMU_DIR_BACK] = (bll_imu_one_way_param_t *)&shoes_back_param, }; static const fml_imu_param_t noshoes_front_param={ .acc_power_mode = FML_IMU_ACC_POWER_MODE_NORMAL, //前脚 - 加速度正常模式 .gry_power_mode = FML_IMU_GRY_POWER_MODE_NORMAL, //前脚 - 陀螺仪正常模式 .fifo_odr = FML_IMU_FIFO_ODR_OFF, //前脚 - FIFO采样频率 - 104HZ .timestamp_resolution = FML_IMU_TIMESTAMP_25US, //前脚 - 时间戳25US精度 .timestamp_switch = FML_IMU_TIMESTAMP_OFF, //前脚 - 时间戳关闭 .acc_odr = FML_IMU_ACC_ODR_12HZ5, //前脚 - 加速度采样频率 - 12.5HZ .gry_odr = FML_IMU_GRY_ODR_OFF, //前脚 - 陀螺仪采样频率 - 关闭 .mag_odr = FML_IMU_MAG_ODR_OFF, //前脚 - 地磁计采样频率 - 关闭 .acc_fs = FML_IMU_ACC_FS_16G, //前脚 - 加速度量程 - 16G .gry_fs = FML_IMU_GRY_FS_2000DPS, //前脚 - 陀螺仪量程 - 2000DPS .mag_fs = FML_IMU_MAG_FS_30GS, //前脚 - 地磁计量程 - 30GS }; static const fml_imu_param_t noshoes_back_param={ .acc_power_mode = FML_IMU_ACC_POWER_MODE_NORMAL, //后脚 - 加速度正常模式 .gry_power_mode = FML_IMU_GRY_POWER_MODE_NORMAL, //后脚 - 陀螺仪正常模式 .fifo_odr = FML_IMU_FIFO_ODR_OFF, //后脚 - FIFO采样频率 - 关闭 .timestamp_resolution = FML_IMU_TIMESTAMP_25US, //后脚 - 时间戳25US精度 .timestamp_switch = FML_IMU_TIMESTAMP_OFF, //后脚 - 时间戳关闭 .acc_odr = FML_IMU_ACC_ODR_OFF, //后脚 - 加速度采样频率 - 关闭 .gry_odr = FML_IMU_GRY_ODR_OFF, //后脚 - 陀螺仪采样频率 - 关闭 .mag_odr = FML_IMU_MAG_ODR_10HZ, //后脚 - 地磁计采样频率 - 10HZ .acc_fs = FML_IMU_ACC_FS_16G, //后脚 - 加速度量程 - 16G .gry_fs = FML_IMU_GRY_FS_2000DPS, //后脚 - 陀螺仪量程 - 2000DPS .mag_fs = FML_IMU_MAG_FS_30GS, //后脚 - 地磁计量程 - 30GS }; static const bll_imu_param_t noshoes_bll_imu_param_t={ .config_param[FML_IMU_DIR_FRONT] = (bll_imu_one_way_param_t *)&noshoes_front_param, .config_param[FML_IMU_DIR_BACK] = (bll_imu_one_way_param_t *)&noshoes_back_param, }; enum{ WEARSHOES_INIT=0, WEARSHOES_NO, WEARSHOES_YES }; static uint8_t isWearShoes = WEARSHOES_NO; static uint8_t IsWearShoes(int16_t mag[]) { uint8_t flag = 0; int32_t mag_norm =0; int32_t LvBobuff[Bufflength]={0}; int32_t temp =0; uint8_t i=0; mag_norm = (int32_t)(sqrt((float) (mag[0] *mag[0] + mag[1] * mag[1] + mag[2] *mag[2]))); if(1 == refreshFlag){refreshFlag =0; for(i=0;i< Bufflength;i++){ buff[i]=mag_norm; } }else{ for(i=0;i<(Bufflength-1);i++){ buff[i]=buff[i+1]; } buff[Bufflength-1] = mag_norm; } memcpy(LvBobuff,buff,sizeof(buff)); for(uint8_t d=0;d<(Bufflength-1);d++){ for(uint8_t b=0;b<(Bufflength-1-d);b++){ if(LvBobuff[b]>LvBobuff[b+1]){ temp=LvBobuff[b+1]; LvBobuff[b+1]=LvBobuff[b]; LvBobuff[b]=temp; } } } if((LvBobuff[Bufflength-1] - LvBobuff[0]) >= 1000){ flag = 1; } return flag; } static uint8_t shoes_signal =WEARSHOES_INIT; static void hal_wearshoes_determine(void) { bll_imu_data_t data={0}; uint8_t front_CS =0,back_CS =0; static uint32_t noshoestim = 0; static BLL_IMU_DIR_e BLL_IMU_Set =BLL_IMU_DIR_NUM; //选择前脚还是后脚的磁力计 front_CS = bll_imu_query_config_param_is_ready(BLL_IMU_DIR_FRONT,&shoes_bll_imu_param_t); back_CS = bll_imu_query_config_param_is_ready(BLL_IMU_DIR_BACK,&shoes_bll_imu_param_t); if(BLL_IMU_CONFIG_FINISH == front_CS && BLL_IMU_CONFIG_FINISH == back_CS){ if(BLL_IMU_Set != BLL_IMU_DIR_FRONT){ refreshFlag = 1; } BLL_IMU_Set =BLL_IMU_DIR_FRONT; } else{ front_CS = bll_imu_query_config_param_is_ready(BLL_IMU_DIR_FRONT,&noshoes_bll_imu_param_t); back_CS = bll_imu_query_config_param_is_ready(BLL_IMU_DIR_BACK,&noshoes_bll_imu_param_t); if(front_CS == BLL_IMU_CONFIG_FINISH && back_CS == BLL_IMU_CONFIG_FINISH){ if(BLL_IMU_Set != BLL_IMU_DIR_BACK){ refreshFlag = 1; } BLL_IMU_Set =BLL_IMU_DIR_BACK; } else { BLL_IMU_Set =BLL_IMU_DIR_NUM; return; } } int number = bll_imu_get_data_num(BLL_IMU_Set); if(number >= 1){ int8_t result = bll_imu_get_data(BLL_IMU_Set, 0, &data); if(0 == data.mag[0] && 0 == data.mag[1] && 0 == data.mag[2]) { DEBUG_LOG("!!!------->>>>>>>>>>>>>>>value:%d,%d,%d\r\n",number,result,BLL_IMU_Set); DEBUG_LOG("!!!------->>>>>>>>>>>>>>>mag_norm:%d,%d,%d\r\n",data.mag[0],data.mag[1],data.mag[2]); DEBUG_LOG("!!!------->>>>>>>>>>>>>>>acc_norm:%d,%d,%d\r\n",data.acc[0],data.acc[1],data.acc[2]); DEBUG_LOG("!!!------->>>>>>>>>>>>>>>gry_norm:%d,%d,%d\r\n",data.gry[0],data.gry[1],data.gry[2]); DEBUG_LOG("!!!------->>>>>>>>>>>>>>>fifo_timestamp:%d\r\n",data.fifo_timestamp); return;//磁力计数据产生的数据比加速度的数据慢，所以会有一个时间差 } if(1 == IsWearShoes(data.mag)){ noshoestim = TIME_GetTicks(); if(WEARSHOES_NO == isWearShoes && shoes_signal == WEARSHOES_INIT){ shoes_signal = WEARSHOES_YES; } }else if(WEARSHOES_YES == isWearShoes) { if(shoes_signal == WEARSHOES_INIT && (TIME_GetTicks() -noshoestim >= HAL_WEARSHOES_TIMEOUT)) { shoes_signal = WEARSHOES_NO; } } } } static void hal_wearshoes_Process(void) { static uint8_t imu_configcnt =0; uint8_t front_CS =0,back_CS =0; static uint8_t errorsetFlag =0; switch(isWearShoes){ case WEARSHOES_NO:{//不穿鞋 if(WEARSHOES_INIT == shoes_signal){ front_CS = bll_imu_query_config_param_is_ready(BLL_IMU_DIR_FRONT,&noshoes_bll_imu_param_t); back_CS = bll_imu_query_config_param_is_ready(BLL_IMU_DIR_BACK,&noshoes_bll_imu_param_t); if(BLL_IMU_CONFIG_FINISH == front_CS && BLL_IMU_CONFIG_FINISH == back_CS){ hal_wearshoes_determine(); if(1 == errorsetFlag){errorsetFlag =0; Process_Start(100,"hal_wearshoes_Process",hal_wearshoes_Process); } } else if(BLL_IMU_CONFIG_DOING != front_CS || BLL_IMU_CONFIG_DOING != back_CS){ bll_imu_Resume_unregister_config_param(&shoes_bll_imu_param_t); bll_imu_Resume_config_param(&noshoes_bll_imu_param_t); } } else { front_CS = bll_imu_query_config_param_is_ready(BLL_IMU_DIR_FRONT,&shoes_bll_imu_param_t); back_CS = bll_imu_query_config_param_is_ready(BLL_IMU_DIR_BACK,&shoes_bll_imu_param_t); if(BLL_IMU_CONFIG_FINISH == front_CS && BLL_IMU_CONFIG_FINISH == back_CS){ isWearShoes = WEARSHOES_YES; DEBUG_LOG("!!!------->>>>>I am WearShoes\r\n"); shoes_signal =WEARSHOES_INIT; } else if(BLL_IMU_CONFIG_DOING != front_CS || BLL_IMU_CONFIG_DOING != back_CS){ bll_imu_Resume_unregister_config_param(&noshoes_bll_imu_param_t); bll_imu_Resume_config_param(&shoes_bll_imu_param_t); imu_configcnt++; if(imu_configcnt > 100){ imu_configcnt =0; if(errorsetFlag ==0){errorsetFlag =1; Process_Start(10000,"hal_wearshoes_Process",hal_wearshoes_Process); } } } } } break; case WEARSHOES_YES:{//穿鞋状态 if(WEARSHOES_INIT == shoes_signal){ front_CS = bll_imu_query_config_param_is_ready(BLL_IMU_DIR_FRONT,&shoes_bll_imu_param_t); back_CS = bll_imu_query_config_param_is_ready(BLL_IMU_DIR_BACK,&shoes_bll_imu_param_t); if(BLL_IMU_CONFIG_FINISH == front_CS && BLL_IMU_CONFIG_FINISH == back_CS){ hal_wearshoes_determine(); if(1 == errorsetFlag){errorsetFlag =0; Process_Start(100,"hal_wearshoes_Process",hal_wearshoes_Process); } } else if(BLL_IMU_CONFIG_DOING != front_CS || BLL_IMU_CONFIG_DOING != back_CS){ bll_imu_Resume_unregister_config_param(&noshoes_bll_imu_param_t); bll_imu_Resume_config_param(&shoes_bll_imu_param_t); imu_configcnt++; if(imu_configcnt > 100){ imu_configcnt =0; } if(errorsetFlag ==0){errorsetFlag =1; Process_Start(10000,"hal_wearshoes_Process",hal_wearshoes_Process); } } } else{ front_CS = bll_imu_query_config_param_is_ready(BLL_IMU_DIR_FRONT,&noshoes_bll_imu_param_t); back_CS = bll_imu_query_config_param_is_ready(BLL_IMU_DIR_BACK,&noshoes_bll_imu_param_t); if(BLL_IMU_CONFIG_FINISH == front_CS && BLL_IMU_CONFIG_FINISH == back_CS){ isWearShoes = WEARSHOES_NO; DEBUG_LOG("I am not WearShoes\r\n"); shoes_signal =WEARSHOES_INIT; } else if(BLL_IMU_CONFIG_DOING != front_CS || BLL_IMU_CONFIG_DOING != back_CS){ bll_imu_Resume_unregister_config_param(&shoes_bll_imu_param_t); bll_imu_Resume_config_param(&noshoes_bll_imu_param_t); imu_configcnt++; if(imu_configcnt > 100){ imu_configcnt =0; } } } } break; default :isWearShoes = WEARSHOES_NO;imu_configcnt=0;break; } } uint8_t hal_wearshoes_is_wearshoes(void) { if(WEARSHOES_YES == isWearShoes)return 1; else return 0; } void hal_wearshoes_Init(void) { if(isWearShoes == WEARSHOES_NO){ bll_imu_Resume_config_param(&noshoes_bll_imu_param_t); }else if(isWearShoes == WEARSHOES_YES){ bll_imu_Resume_config_param(&shoes_bll_imu_param_t); } Process_Start(100,"hal_wearshoes_Process",hal_wearshoes_Process); }