liweiyan/shoe_mcu2.2.1_new_v2/app/app_step.c

#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"

//实时计步模式
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<group_num;i++){
			bll_imu_get_data(BLL_IMU_DIR_FRONT, i, &data);
			acc_front[i][0] = data.acc[0];acc_front[i][1] = data.acc[1];acc_front[i][2] = data.acc[2];
			mag6310_front[i][0] = data.mag[0];mag6310_front[i][1] = data.mag[1];mag6310_front[i][2] = data.mag[2];
		}
		if(bll_imu_get_data_num(BLL_IMU_DIR_BACK) >= 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){
		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(0,"app_math_DailyStep_Process",app_math_DailyStep_Process);
	
}