#include "app_step.h" #include "nrf_gpio.h" #include "usr_config.h" #include "bsp_time.h" #include "system.h" #include "hal_mt.h" #include "app_host.h" #include "app_charge.h" #include "hal_ble_client.h" #include "hal_ble_host.h" #include "nrf_delay.h" #include "app_flash.h" #include "ble_comm.h" #include "exception.h" #include "bll_imu.h" #include "detect_zero_vel.h" #include "detect_step_by_mag.h" #include "pdrStatus.h" #include "hal_wearshoes.h" #include "app_game.h" //实时计步模式 static const bll_imu_one_way_param_t realstep_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_ON, //前脚 - 时间戳开启 .acc_odr = FML_IMU_ACC_ODR_104HZ, //前脚 - 加速度采样频率 - 104HZ .gry_odr = FML_IMU_GRY_ODR_104HZ, //前脚 - 陀螺仪采样频率 - 104HZ .mag_odr = FML_IMU_MAG_ODR_200HZ, //前脚 - 地磁计采样频率 - 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_one_way_param_t realstep_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_200HZ, //后脚 - 地磁计采样频率 - 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 realstep_bll_imu_param_t={ .config_param[FML_IMU_DIR_FRONT] = &realstep_front_param, .config_param[FML_IMU_DIR_BACK] = &realstep_back_param, }; static uint8_t realStepMode = BLE_REALTIMESTEP_EXIT; //是否为实时计步模式 static uint32_t realStepCur_L = 0; //左鞋步数记录点 static uint32_t realStepAdd_R = 0; //右鞋步数 static uint8_t realStepTimCnt = 0; //右鞋实时计步的心跳 static uint16_t realStepHeartCnt = 0; //实时计步模式心跳计时 static uint8_t RealStepFlag =BLE_REALTIMESTEP_EXIT; //实时计步的信号。 #define IMU_MAX_GROUP_NUM 5 static int16_t acc_front[IMU_MAX_GROUP_NUM][3]; static int16_t mag6310_front[IMU_MAX_GROUP_NUM][3]; static int16_t mag6310_back[3]; /********************** 函数声明区 *************************/ uint32_t app_step_GetStep_L(void) { return mFlash.mStep.stepCur[0]; } uint32_t app_step_GetStep_R(void) { return mFlash.mStep.stepCur[1]; } //发送实时左右鞋步数到CLIENT端 static void app_step_RealSendClient() { if(realStepMode == BLE_REALTIMESTEP_ENTER){ uint8_t buf[8]={0}; uint8_t L = 0; buf[L++] = BLE_REALTIMESTEP_ENTER; buf[L++] = (uint8_t)((mFlash.mStep.stepCur[0]-realStepCur_L)>>24); buf[L++] = (uint8_t)((mFlash.mStep.stepCur[0]-realStepCur_L)>>16); buf[L++] = (uint8_t)((mFlash.mStep.stepCur[0]-realStepCur_L)>>8); buf[L++] = (uint8_t)((mFlash.mStep.stepCur[0]-realStepCur_L)>>0); buf[L++] = (uint8_t)(realStepAdd_R>>24); buf[L++] = (uint8_t)(realStepAdd_R>>16); buf[L++] = (uint8_t)(realStepAdd_R>>8); buf[L++] = (uint8_t)(realStepAdd_R>>0); DEBUG_LOG("mFlash.mStep.stepCur[0]:%d,realStepCur_L:%d,realStepAdd_R:%d\n",mFlash.mStep.stepCur[0],realStepCur_L,realStepAdd_R); BLE_Client_Tx_Send(0,BLE_REALTIMESTEP,buf,L); } } //右鞋实时计步发送 static void app_step_RealSendProcess() { if(mFlash.isHost)return; app_step_RealSendClient(); } //IMU数据回调 static void real_data_notify_cb(uint32_t dir_bit) { int16_t group_num = 0; bll_imu_data_t data={0}; if(BLE_REALTIMESTEP_ENTER != realStepMode)return; if((dir_bit >> BLL_IMU_DIR_FRONT) & 0x01) { group_num = bll_imu_get_data_num(BLL_IMU_DIR_FRONT); if(group_num >IMU_MAX_GROUP_NUM)return; for(int i=0;i= 1){ bll_imu_get_data(BLL_IMU_DIR_BACK, 0, &data); mag6310_back[0] = data.mag[0];mag6310_back[1] = data.mag[1];mag6310_back[2] = data.mag[2]; } // DEBUG_LOG("======RealTimeStep=====%d\r\n",TIME_GetTicks()); if(RealTimeStep((int16_t*)mag6310_front, (int16_t*)mag6310_back, (int16_t*)acc_front)){ mFlash.mStep.stepCur[0]++; DEBUG_LOG("======RealTimeStep=====step %d\r\n",mFlash.mStep.stepCur[0]); } } } uint8_t app_step_Real_Get(void){ if(realStepMode == BLE_REALTIMESTEP_ENTER)return 1; else return 0; } //右鞋实时计步连接管理 void app_step_RealConnectProcess(void) { static uint8_t state =0; static uint8_t imu_configcnt =0; uint8_t mod =0; uint8_t front_CS =0,back_CS =0; switch(state){ case 0:{//空闲 if(RealStepFlag == BLE_REALTIMESTEP_ENTER){ state =1; imu_configcnt =0; bll_imu_Resume_config_param(&realstep_bll_imu_param_t); }else{ if(realStepTimCnt>0){ realStepTimCnt = 0; BLE_Host_Tx_Send(0,BLE_REALTIMESTEP,&RealStepFlag,1); } } break; } case 1:{//配置IMU front_CS = bll_imu_query_config_param_is_ready(BLL_IMU_DIR_FRONT,&realstep_bll_imu_param_t); back_CS = bll_imu_query_config_param_is_ready(BLL_IMU_DIR_BACK, &realstep_bll_imu_param_t); if(BLL_IMU_CONFIG_FINISH == front_CS && BLL_IMU_CONFIG_FINISH == back_CS){ DEBUG_LOG(">>>>>>into real step mode\n"); state =2; realStepCur_L = mFlash.mStep.stepCur[0]; //重新计数 realStepAdd_R = 0; realStepHeartCnt = 0; realStepMode = BLE_REALTIMESTEP_ENTER; } else if(BLL_IMU_CONFIG_DOING != front_CS || BLL_IMU_CONFIG_DOING != back_CS){ bll_imu_Resume_config_param(&realstep_bll_imu_param_t); imu_configcnt++; if(imu_configcnt >=20){ bll_imu_Resume_unregister_config_param(&realstep_bll_imu_param_t); state =0; RealStepFlag = BLE_REALTIMESTEP_EXIT; realStepMode = BLE_REALTIMESTEP_EXIT; } } break; } case 2://进入实时计步 realStepHeartCnt++; if(realStepHeartCnt >= 600){realStepHeartCnt=0; mod = BLE_REALTIMESTEP_EXIT; BLE_Host_Tx_Send(0,BLE_REALTIMESTEP,&mod,1); //退出从机实时步数 bll_imu_Resume_unregister_config_param(&realstep_bll_imu_param_t); realStepMode = BLE_REALTIMESTEP_EXIT; state =0; }else{ if(realStepTimCnt > 0) realStepTimCnt--; if(realStepTimCnt==0) { mod = BLE_REALTIMESTEP_ENTER; BLE_Host_Tx_Send(0,BLE_REALTIMESTEP,&mod,1); } } if(RealStepFlag == BLE_REALTIMESTEP_EXIT){ realStepMode = BLE_REALTIMESTEP_EXIT; bll_imu_Resume_unregister_config_param(&realstep_bll_imu_param_t); state =0; } break; default:RealStepFlag = BLE_REALTIMESTEP_EXIT;realStepMode = BLE_REALTIMESTEP_EXIT;state =0;break; } } //接收右鞋实时步数并发送到手机 void cb_BLE_Host_R_REALTIMESTEP(void* handle) { BLE_Client_Rx_t* target = handle; if(target->pDat[0]==BLE_REALTIMESTEP_ENTER){ if(realStepMode == BLE_REALTIMESTEP_ENTER){ realStepAdd_R = ((uint32_t)target->pDat[1]<<24) | (uint32_t)(target->pDat[2]<<16) | (uint32_t)(target->pDat[3]<<8) | ((uint32_t)target->pDat[4]<<0); app_step_RealSendClient(); DEBUG_LOG(">>>>>>realStepCur_L:%ld,realStepAdd_R:%ld,\n",realStepCur_L,realStepAdd_R); } realStepTimCnt = 3; } } //接收手机命令 void cb_BLE_Client_R_REALTIMESTEP(void* handle) { BLE_Client_Rx_t* target = handle; RealStepFlag = target->pDat[0]; if(RealStepFlag == BLE_REALTIMESTEP_ENTER){ realStepHeartCnt = 0; bll_imu_Resume_config_param(&realstep_bll_imu_param_t); } else{ bll_imu_Resume_unregister_config_param(&realstep_bll_imu_param_t); } BLE_Host_Tx_Send(0,BLE_REALTIMESTEP,&RealStepFlag,1); } //日常计步 static void app_math_DailyStep_Process(void) { int16_t acc[3]; int16_t mag6310[3]; int16_t group_num = 0; bll_imu_data_t data= {0}; if(hal_wearshoes_is_wearshoes() && realStepMode != BLE_REALTIMESTEP_ENTER && !app_game_GetGameMode()){//游戏模式下，APP的SDK会计算脚步 group_num = bll_imu_get_data_num(BLL_IMU_DIR_FRONT); for(int i=0; i < group_num; i++){ bll_imu_get_data(BLL_IMU_DIR_FRONT, i, &data); mag6310[0] = data.mag[0];mag6310[1] = data.mag[1];mag6310[2] = data.mag[2]; acc[0] = data.acc[0];acc[1] = data.acc[1];acc[2] = data.acc[2]; // DEBUG_LOG("f_mx=%d\r,f_my=%d\r,f_mz=%d\r\n",mag6310[0],mag6310[1],mag6310[2]); if(1 == detect_step_by_mag(mag6310,acc[2])){ mFlash.mStep.stepCur[0]++; DEBUG_LOG("DailyStep current step:%d\r\n",mFlash.mStep.stepCur[0]); break; } } } } void app_step_Init(void) { Process_Start(100,"step_RealSend",app_step_RealSendProcess); BLE_Client_Rx_Regist(BLE_REALTIMESTEP,cb_BLE_Client_R_REALTIMESTEP); Process_Start(100,"step_RealConnect",app_step_RealConnectProcess); BLE_Host_Rx_Regist(BLE_REALTIMESTEP,cb_BLE_Host_R_REALTIMESTEP); bll_imu_register_data_notify_callback(BLL_IMU_DATA_NOTIFY_CB_PRIORITY_1, real_data_notify_cb); Process_Start(100,"math_DailyStep",app_math_DailyStep_Process); }