liweiyan/shoe_mcu2.2_0711_add_selfcheck_v3/hal/hal_wearshoes.c

#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);
	
	//-----�零槨踞누糾榴檄왠齡金추鷺퍅
	Sleep_Duration_Control_Regist(hal_wearshoes_is_wearshoes);
}