shoe_mcu/app/app_detectIsHost - 副本 (2).c

/*********************************************************************
 * INCLUDES
 */
#include "ble_comm.h"
#include "app_detectIsHost.h"
#include "hal_ble_client.h"
#include "app_flash.h"
#include "hal_qma.h"
#include "hal_mt.h"
#include "tool.h"
#include "bll_imu.h"
#include "MahonyAHRS.h"

#define DETECT_LR_TIMEOUT							10		

static DETECT_LR_e ob_detect_LR = DETECT_LR_INIT;

static uint8_t SetDeviceNameFlag = 0;

static void app_SetDeviceName_Porcess(void){
	 static uint8_t state =0;
	 char buf[16];
	 memset(buf,0,16);
	 switch(state){
		 case 0:
			     if(1 == SetDeviceNameFlag){
						  state =1; 
					 }
			     break;
		 case 1:
			     if(host_isconnect()){
						  host_disconnect();
					 }
					 else{
						 if(slave_isconnect()) 
							   slave_disconnect();
						 else{
							   advertising_stop();
							   ST_scan_stop();
							   state =2;
						 }
					 }
			     break;
		 case 2:
					if(app_Get_isHost()){	//
						#if BleNameHoldOn_ENANBLE
							slave_set_adv_name((char *) LEFT_NAME,sizeof(LEFT_NAME));
							DEBUG_LOG("AdvName(%d):%s\n",sizeof(LEFT_NAME),LEFT_NAME);
							host_set_scan_name((char *)RIGHT_NAME,sizeof(RIGHT_NAME));
							DEBUG_LOG("scanName(%d):%s\n",sizeof(RIGHT_NAME),RIGHT_NAME);
					  #else
							if(mFlash.mClient.isConfig == 'C'){ 
						    sprintf(buf,"%02X%02X%02X%02X%02X%02X",mFlash.macHost[0],mFlash.macHost[1],mFlash.macHost[2],mFlash.mClient.macAddr[3],mFlash.mClient.macAddr[4],mFlash.mClient.macAddr[5]);
			          DEBUG_LOG("scanName(%d):%s\n",strlen(buf),buf);
			          host_set_scan_name(buf,strlen(buf));
								memset(buf,0,sizeof(buf));
								sprintf(buf,"SH_%02X%02X",mFlash.macHost[1], mFlash.macHost[0]);
								slave_set_adv_name(buf,strlen(buf));
							}
					  #endif
					}else{ //
						#if BleNameHoldOn_ENANBLE 
							slave_set_adv_name((char *)RIGHT_NAME,sizeof(RIGHT_NAME));
							DEBUG_LOG("AdvName(%d):%s\n",sizeof(RIGHT_NAME),RIGHT_NAME);
						#else
							if(mFlash.mClient.isConfig=='C'){ //
								sprintf(buf,"%02X%02X%02X%02X%02X%02X",mFlash.mClient.macAddr[0],mFlash.mClient.macAddr[1],mFlash.mClient.macAddr[2],mFlash.macHost[3],mFlash.macHost[4],mFlash.macHost[5]);
								DEBUG_LOG("advName(%d):%s\n",strlen(buf),buf);
								slave_set_adv_name(buf,strlen(buf));
						  }
						  host_set_scan_name((char *)"12321321312",sizeof("12321321312"));
							ST_scan_stop();
						#endif
					 } 
					 
					 slave_adv_init();
	         advertising_start();
					 state =0;
					 SetDeviceNameFlag = 0;
			     break;
		 default:state =0;SetDeviceNameFlag = 0;break;
	 }
}

static void app_detect_LR_Porcess(void)
{
	static uint8_t runtime =0;
	int ret =-1;
	static uint8_t counter = 0;
	bll_imu_data_t f_data = {0};
	qma_data_t 	qma_data={0};
	
  static DETECT_LR_e direct_detect_LR = DETECT_LR_INIT;
	uint8_t direct_detect_host = 0;
	
	#define Bufflength 6
  static int32_t buff[Bufflength]={0};
	int32_t LvBobuff[Bufflength]={0};
	uint8_t i=0;
	int32_t acc_norm  =0;
	int32_t temp =0;
	static uint8_t firtRunflag =0; 
	
	//获取中间加速度值
	ret = drv_qma_get_acc_data(&qma_data);
	if(-1 == ret || (0 == qma_data.acc[0] && 0 == qma_data.acc[1] && 0== qma_data.acc[2]))return;
	
	acc_norm = sqrt(qma_data.acc[0] * qma_data.acc[0] +qma_data.acc[1] * qma_data.acc[1] + qma_data.acc[2] * qma_data.acc[2]);
	
	//获取前脚IMU的值
	if(bll_imu_get_data_num(BLL_IMU_DIR_FRONT) >= 0){
		if(-1 == bll_imu_get_data(BLL_IMU_DIR_FRONT, 0, &f_data))
			return;
	}
	//第一次启动给所有的BUFF赋值
  if( 0 == firtRunflag)	{firtRunflag =1;
		for(i=0;i< Bufflength;i++){
		    buff[i]=acc_norm;
	  }	
	}
	
	for(i=0;i<(Bufflength-1);i++){
		  buff[i]=buff[i+1];
	}
	buff[Bufflength-1] = acc_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;
				}
		 }
	}
	
	//	DEBUG_LOG("LvBobuff:%d,%d\r\n",LvBobuff[Bufflength-1],LvBobuff[0]);
	
	if((LvBobuff[Bufflength-1] - LvBobuff[0]) < 500){
//		 DEBUG_LOG("f_data.acc[2]:%6d,qma_data.acc[0]:%6d\r\n",f_data.acc[2],qma_data.acc[0]);
		  Mahony_process(0,0,0,qma_data.acc[0],qma_data.acc[1],qma_data.acc[2],0,0,0);
//		  if(f_data.acc[2] < -1850  && f_data.acc[2] > -2200 ){//平放的时候才判断左右鞋 
				if(runtime <15){runtime++;return;}
				int16_t rol  = (int16_t)(getRoll());
//				DEBUG_LOG("getRoll:%d\n",rol);
				 if(abs(rol) > 90){//反向 		
					 #if _SAME_DIRECTION
								if(DETECT_LR_IS_RIGHT == direct_detect_LR)counter++;
								else counter =0;
								direct_detect_LR = DETECT_LR_IS_RIGHT;	
						#else
								if(DETECT_LR_IS_LEFT == direct_detect_LR)counter++;
								else counter =0;
								direct_detect_LR = DETECT_LR_IS_LEFT;
						#endif
				 }
				 else {//同向		
						  #if _SAME_DIRECTION
								if(DETECT_LR_IS_LEFT == direct_detect_LR)counter++;
								else counter =0;
								direct_detect_LR = DETECT_LR_IS_LEFT;    
						  #else
								if(DETECT_LR_IS_RIGHT == direct_detect_LR)counter++;
								else counter =0;
								direct_detect_LR = DETECT_LR_IS_RIGHT;
						  #endif
				 }
//		 }
	}
	else counter =0;

	
	if(counter >= DETECT_LR_TIMEOUT){counter = 0;
//		DEBUG_LOG("direct_detect_host:%d\n",direct_detect_host);
		if(direct_detect_LR != ob_detect_LR){ob_detect_LR = direct_detect_LR;
				if(ob_detect_LR == DETECT_LR_IS_LEFT)
					direct_detect_host = 1;
				else 
					direct_detect_host = 0;
				
				if(direct_detect_host != mFlash.isHost){
					mFlash.isHost = direct_detect_host;
					Flash_SaveInfomation();
					SetDeviceNameFlag =1;
					MT_Run(500);

					mBackup.isHost= mFlash.isHost;
		      if(Flash_SaveBackup() != ZONE_OP_SUCCESS)Except_TxError(EXCEPT_FLASH,"app_detect_LR save backup fail");
					
				}
				direct_detect_LR = DETECT_LR_INIT;
		}
	}
}

/**
 @brief 返回主机标志位
 @param 无
 @return 主机标志位
*/
uint8_t app_Get_isHost(void)
{
	return mFlash.isHost;
}

////全功率模式
//static const bll_imu_one_way_param_t all_front_param={
//	.acc_power_mode 						= FML_IMU_ACC_POWER_MODE_NORMAL,						//前脚 - 加速度正常模式
//	.gry_power_mode 						= FML_IMU_GRY_POWER_MODE_NORMAL,						//前脚 - 陀螺仪正常模式
//	.timestamp_resolution 			= FML_IMU_TIMESTAMP_25US,										//前脚 - 时间戳25US精度
//	.timestamp_switch 					= FML_IMU_TIMESTAMP_ON,											//前脚 - 时间戳开启
//	.acc_fs 										= FML_IMU_ACC_FS_16G,												//前脚 - 加速度量程 - 16G
//	.gry_fs 										= FML_IMU_GRY_FS_2000DPS,										//前脚 - 陀螺仪量程 - 2000DPS
//	.mag_fs 										= FML_IMU_MAG_FS_30GS,											//前脚 - 地磁计量程 - 30GS
//	.acc_odr 									  = FML_IMU_ACC_ODR_104HZ,										//前脚 - 加速度采样频率 - 104HZ
//	.gry_odr 									  = FML_IMU_GRY_ODR_104HZ,										//前脚 - 陀螺仪采样频率 - 104HZ
//	.mag_odr 									  = FML_IMU_MAG_ODR_200HZ,										//前脚 - 地磁计采样频率 - 200HZ
//	.fifo_odr 									= FML_IMU_FIFO_ODR_104HZ,	
//};

//static const bll_imu_one_way_param_t all_back_param={
//	.acc_power_mode 						= FML_IMU_ACC_POWER_MODE_NORMAL,						//后脚 - 加速度正常模式
//	.gry_power_mode 						= FML_IMU_GRY_POWER_MODE_NORMAL,						//后脚 - 陀螺仪正常模式
//	.timestamp_resolution 			= FML_IMU_TIMESTAMP_25US,										//后脚 - 时间戳25US精度
//	.timestamp_switch 					= FML_IMU_TIMESTAMP_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
//	.acc_odr 										= FML_IMU_ACC_ODR_OFF,											//后脚 - 加速度采样频率 - 关闭
//	.gry_odr 										= FML_IMU_GRY_ODR_OFF,											//后脚 - 陀螺仪采样频率 - 关闭
//	.mag_odr 										= FML_IMU_MAG_ODR_200HZ,										//后脚 - 地磁计采样频率 - 200HZ
//	.fifo_odr 									= FML_IMU_FIFO_ODR_OFF,	
//};

//static const bll_imu_param_t all_bll_imu_param_t={
//	.config_param[FML_IMU_DIR_FRONT] = &all_front_param,
//	.config_param[FML_IMU_DIR_BACK] =  &all_back_param,
//};

//#include "MahonyAHRS.h"
////IMU数据回调
//static void all_data_notify_cb(uint32_t dir_bit)
//{
//	int16_t 				group_num = 0;
//	bll_imu_data_t data={0};
//	qma_data_t 	qma_data={0};
//	int ret = drv_qma_get_acc_data(&qma_data);
//	if(-1 == ret || (0 == qma_data.acc[0] && 0 == qma_data.acc[1] && 0== qma_data.acc[2]))return;
//	if((dir_bit >> BLL_IMU_DIR_FRONT) & 0x01){
//		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);
//			//获取中间加速度值
////			app_send_adc(data.acc,qma_data.acc);
//			Mahony_process(0,0,0,qma_data.acc[0],qma_data.acc[1],qma_data.acc[2],0,0,0);
//		}
//	}
//}

void app_detect_Init(void)
{
	SetDeviceNameFlag =1;
	Process_Start(100,"app_SetDeviceName_Porcess",app_SetDeviceName_Porcess);
  Process_Start(1000,"app_detect_LR_Init_Porcess",app_detect_LR_Porcess);
	Mahony_Init(10);
//	Process_SetHoldOn(app_detect_LR_Porcess,1);
	if(mFlash.isHost){
		DEBUG_LOG("======= Left  shooe ======= \n");
	}else{
		DEBUG_LOG("======= Right shooe ======= \n");
	}
	DEBUG_LOG("======= mFlash.mClient.isConfig:%d ======= \n",mFlash.mClient.isConfig);
//	Mahony_Init(100);
//	Process_SetHoldOn(app_detect_LR_Porcess,1);
//	bll_imu_Resume_config_param(&all_bll_imu_param_t);
//	bll_imu_register_data_notify_callback(BLL_IMU_DATA_NOTIFY_CB_PRIORITY_1, all_data_notify_cb);
}