liweiyan/shoe_mcu2.2_0711_add_selfcheck_v3/system/selfcheck.c

/*Includes ----------------------------------------------*/
#include "tool.h"
#include "MahonyAHRS.h"
#include "ble_comm.h"
#include "nrf_delay.h"
#include "bsp_pwm.h"
#include "bsp_time.h"
#include "bsp_adc.h"
#include "bsp_wdt.h"
#include "exception.h"
#include "selfcheck.h"
#include "system.h"
#include "drv_qma7981.h"
#include "hal_led.h"
#include "hal_battery.h"
#include "bll_imu.h"
#include "app_flash.h"

/*Private macro ------------------------------------------------------------------------------------------------------------------------------------*/
#define	SELFCHECK_FLOW_TOOLING_BOARD_PROCESS_CYCLE								             LowPower_Interval																	//工装自检线程周期，单位：ms

#define SELFCHECK_FLOW_COMPLETE_BOARD_PROCESS_1_CYCLE													 2000																								//完整板自检线程1周期，单位：ms			

#define SELFCHECK_FLOW_COMPLETE_BOARD_PROCESS_2_CYCLE													 LowPower_Interval																	//完整板自检线程2周期，单位：ms		

#define	SELFCHECK_SCAN_DEVICE_NAME												                     "123"																							//需要扫描的设备名

#define	SELFCHECK_SCAN_DEVICE_RSSI_MIN_THRESHOLD					                     -100																								//需要扫描的设备的RSSI最小阈值

#define SELFCHECK_LED_ENABLE															                     0																									//LED灯开

#define SELFCHECK_LED_DISABLE															                     1																									//LED灯关

#define SELFCHECK_THRESHOLD_IS_MET(MIN,VALUE,MAX)															 ((MIN <= VALUE && VALUE <= MAX)?true:false)				//阈值匹配

#define	SELFCHECK_PAIR_PIN_ADC_MIN_THRESHOLD							                     100																								//配对引脚的最小ADC阈值

#define	SELFCHECK_PAIR_PIN_ADC_MAX_THRESHOLD							                     200																								//配对引脚的最大ADC阈值

#define	SELFCHECK_CHARGE_CHIP_PIN_ADC_MIN_THRESHOLD				                     100																								//充电芯片引脚的最小ADC阈值

#define	SELFCHECK_CHARGE_CHIP_PIN_ADC_MAX_THRESHOLD				                     200																								//充电芯片引脚的最大ADC阈值

#define	SELFCHECK_BATTERY_PIN_ADC_MIN_THRESHOLD						                     100																								//电池分压电阻的最小ADC阈值

#define	SELFCHECK_BATTERY_PIN_ADC_MAX_THRESHOLD						                     200																								//电池分压电阻的最大ADC阈值

#define SELFCHECK_MIDDLE_ACC_ROLL_MIN_THRESHOLD																 5																									//中间加速度ROLL值最小阈值

#define	SELFCHECK_MIDDLE_ACC_ROLL_MAX_THRESHOLD																 10																									//中间加速度ROLL值最大阈值

#define SELFCHECK_WEAR_INSOLE_MAG_NORM_MIN_THRESHOLD													 100																								//穿鞋垫的地磁norm值最小阈值

#define	SELFCHECK_WEAR_INSOLE_MAG_NORM_MAX_THRESHOLD													 200																								//穿鞋垫的地磁norm值最大阈值

/*STRUCTION ------------------------------------------------------------------------------------------------------------------------------------*/
typedef enum {
	SELFCHECK_RESULT_SUCCESS,																																																				//自检成功
	
	SELFCHECK_RESULT_ERR_ADV_INIT,																																																	//自检失败——广播初始化
	
	SELFCHECK_RESULT_ERR_RSSI,																																																			//自检失败——RSSI
	
	SELFCHECK_RESULT_ERR_READ_ADC,																																																	//自检失败——ADC读取失败
	
	SELFCHECK_RESULT_ERR_PAIR_PIN_ADC,																																															//自检失败——配对引脚ADC
	
	SELFCHECK_RESULT_ERR_CHARGE_CHIP_PIN_ADC,																																												//自检失败——充电芯片引脚ADC
	
	SELFCHECK_RESULT_ERR_BATTERY_PIN_ADC,																																														//自检失败——电池分压电阻ADC
	
	SELFCHECK_RESULT_ERR_MIDDLE_SENSOR_CONFIG,																																											//自检失败——中间传感器配置
	
	SELFCHECK_RESULT_ERR_FRONT_SENSOR_CONFIG,																																												//自检失败——前脚传感器配置
	
	SELFCHECK_RESULT_ERR_BACK_SENSOR_CONFIG,																																												//自检失败——后脚传感器配置
	
	SELFCHECK_RESULT_ERR_MIDDLE_NO_DATA,																																														//自检失败——中间传感器没数据
	
	SELFCHECK_RESULT_ERR_FRONT_NO_DATA,																																															//自检失败——前脚传感器没数据
	
	SELFCHECK_RESULT_ERR_BACK_NO_DATA,																																															//自检失败——后脚传感器没数据
	
	SELFCHECK_RESULT_ERR_MIDDLE_ACC_ROLL,																																														//自检失败——中间传感器加速度roll值
	
	SELFCHECK_RESULT_ERR_FLASH_WRITE,																																																//自检失败——写flash
	
} SELFCHECK_RESULT_e;

typedef struct _selfcheck
{
	uint32_t																  			  selfcheck_result;																															//自检结果
	
	uint32_t     																			selfcheck_result_led_color;																										//自检结果的led灯颜色
	
	int16_t 																					max_rssi;																																			//最大的RSSI值
	
	fml_imu_data_t 																		f_data;																																				//前脚传感器数据
	
	fml_imu_data_t 																		b_data;																																				//后脚传感器数据
	
	qma_data_t 																				m_data;																																				//中间传感器数据
	
	bool																							f_is_read_data;																																//前脚传感器是否读到数据标志位
	
	bool																						  b_is_read_data;																																//后脚传感器是否读到数据标志位
	
} SelfCheck_t;

/*Local Variable ------------------------------------------------------------------------------------------------------------------------------------*/
static SelfCheck_t ob_selfcheck;

static const bll_imu_one_way_param_t game_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_416HZ,										//前脚 - 加速度采样频率 - 104HZ
	.gry_odr 									  = FML_IMU_GRY_ODR_416HZ,										//前脚 - 陀螺仪采样频率 - 104HZ
	.mag_odr 									  = FML_IMU_MAG_ODR_200HZ,										//前脚 - 地磁计采样频率 - 200HZ
	.fifo_odr 									= FML_IMU_FIFO_ODR_416HZ,	
};

static const bll_imu_one_way_param_t game_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 game_bll_imu_param_t={
	.config_param[FML_IMU_DIR_FRONT] = &game_front_param,
	.config_param[FML_IMU_DIR_BACK] =  &game_back_param,
};

/*Local Functions ------------------------------------------------------------------------------------------------------------------------------------*/
static uint8_t sleep_duration_control_low_power_interval(void)
{
	return 1;
}

static void UART0_SendChar(unsigned char ch)//发送一位数锯
{	
	NRF_UART0->TXD = (unsigned int)ch;
	while(NRF_UART0->EVENTS_TXDRDY == 0x0UL);
	NRF_UART0->EVENTS_TXDRDY = 0x0UL;
}

static uint32_t get_baud(uint32_t baud)
{
	switch(baud){
		case 1200:		return UART_BAUDRATE_BAUDRATE_Baud1200; 	
		case 2400: 		return UART_BAUDRATE_BAUDRATE_Baud2400; 	
		case 4800: 		return UART_BAUDRATE_BAUDRATE_Baud4800; 	
		case 9600: 		return UART_BAUDRATE_BAUDRATE_Baud9600; 	
		case 14400: 	return UART_BAUDRATE_BAUDRATE_Baud14400; 	
		case 19200: 	return UART_BAUDRATE_BAUDRATE_Baud19200; 	
		case 28800: 	return UART_BAUDRATE_BAUDRATE_Baud28800; 	
		case 31250: 	return UART_BAUDRATE_BAUDRATE_Baud31250; 	
		case 38400: 	return UART_BAUDRATE_BAUDRATE_Baud38400; 	
		case 56000: 	return UART_BAUDRATE_BAUDRATE_Baud56000; 	
		case 57600: 	return UART_BAUDRATE_BAUDRATE_Baud57600; 	
		case 76800: 	return UART_BAUDRATE_BAUDRATE_Baud76800; 	
		case 115200: 	return UART_BAUDRATE_BAUDRATE_Baud115200; 	
		case 230400: 	return UART_BAUDRATE_BAUDRATE_Baud230400; 	
		case 250000: 	return UART_BAUDRATE_BAUDRATE_Baud250000; 	
		case 460800: 	return UART_BAUDRATE_BAUDRATE_Baud460800; 	
		case 921600: 	return UART_BAUDRATE_BAUDRATE_Baud921600; 	
		case 1000000: 	return UART_BAUDRATE_BAUDRATE_Baud1M; 		
		default: 		return UART_BAUDRATE_BAUDRATE_Baud115200; 	
	}
}

static void UART0_Init(uint32_t txd, uint32_t rxd, uint32_t baud)
{
	NRF_UART0->INTENCLR = UART_INTENCLR_CTS_Msk | UART_INTENCLR_RXTO_Msk | UART_INTENCLR_NCTS_Msk | UART_INTENCLR_ERROR_Msk;
	NRF_UART0->INTENSET = UART_INTENSET_RXDRDY_Enabled << UART_INTENSET_RXDRDY_Pos;
	
	NRF_UART0->PSELTXD = txd;
	NRF_UART0->PSELRXD = rxd;

	NRF_UART0->BAUDRATE = get_baud(baud);
	NRF_UART0->ENABLE |= UART_ENABLE_ENABLE_Enabled;
	NRF_UART0->TASKS_STARTTX = 0XFFFFFFFF;
	NRF_UART0->TASKS_STARTRX = 0XFFFFFFFF;
	NVIC_SetPriority(UARTE0_UART0_IRQn, UART0_IRQ_PRIORITY);
	NVIC_EnableIRQ(UARTE0_UART0_IRQn);
	NRF_UART0->EVENTS_TXDRDY = 0x0UL;
}

static void scan_report_cb(uint8_t *adv_data, uint16_t adv_data_len,int8_t rssi)
{
	if(mFlash.selfcheck_flow == SELFCHECK_FLOW_TOOLING_BOARD)
	{
		uint8_t *device_name = ble_advdata_parse(adv_data,adv_data_len,BLE_GAP_AD_TYPE_COMPLETE_LOCAL_NAME);
		
		if(!device_name)
		{
			device_name = ble_advdata_parse(adv_data,adv_data_len,BLE_GAP_AD_TYPE_SHORT_LOCAL_NAME);
		}
		
		if(device_name)
		{
			char device_name_buff[128] = {0};
			
			uint16_t data_len = (adv_data + adv_data_len - device_name) / sizeof(uint8_t);

			memcpy(device_name_buff, device_name, data_len);
			
			//广播名匹配
			if(memcmp(device_name_buff,SELFCHECK_SCAN_DEVICE_NAME,strlen(SELFCHECK_SCAN_DEVICE_NAME)) == 0)
			{
				//筛选最大的RSSI
				ob_selfcheck.max_rssi = (ob_selfcheck.max_rssi > rssi)?ob_selfcheck.max_rssi : rssi;
			}
		}
	}
	else if(mFlash.selfcheck_flow == SELFCHECK_FLOW_COMPLETE_BOARD)
	{
		static uint32_t		last_tim = 0;
		
		if(TIME_GetTicks()-last_tim >= 1000)
		{
			last_tim = TIME_GetTicks();
			
			ob_selfcheck.max_rssi = -120;
		}

		ob_selfcheck.max_rssi = (ob_selfcheck.max_rssi > rssi)?ob_selfcheck.max_rssi:rssi;
	}

}

static void fb_data_notify_cb(uint32_t dir_bit)
{
	int data_len;

	if((dir_bit >> BLL_IMU_DIR_FRONT) & 0x01)
	{
		memset(&ob_selfcheck.f_data,0,sizeof(ob_selfcheck.f_data));
		data_len = bll_imu_get_data_num(BLL_IMU_DIR_FRONT);
		for(int i=0; i<data_len;i++)
		{
			bll_imu_get_data(BLL_IMU_DIR_FRONT, i, &ob_selfcheck.f_data);
//			DEBUG_LOG("front acc:%d,%d,%d; gry:%d,%d,%d; mag:%d,%d,%d; tp:%d diff:%d len:%d\n",f_data.acc[0],f_data.acc[1],f_data.acc[2], \
//																			f_data.gry[0],f_data.gry[1],f_data.gry[2], \
//																			f_data.mag[0],f_data.mag[1],f_data.mag[2], \
//																			f_data.fifo_timestamp,f_data.fifo_timestamp - last_tp,data_len);
		}
	}
	
	if((dir_bit >> BLL_IMU_DIR_BACK) & 0x01)
	{
		memset(&ob_selfcheck.b_data,0,sizeof(ob_selfcheck.b_data));
		data_len = bll_imu_get_data_num(BLL_IMU_DIR_BACK);
		for(int i=0; i<data_len;i++)
		{
			bll_imu_get_data(BLL_IMU_DIR_BACK, i, &ob_selfcheck.b_data);
//			DEBUG_LOG("back mag:%d,%d,%d;\n",b_data.mag[0],b_data.mag[1],b_data.mag[2]);
		}
	}
	
	if(((dir_bit >> BLL_IMU_DIR_FRONT) & 0x01))
	{
		 ob_selfcheck.f_is_read_data = true;
	}

	
	if((dir_bit >> BLL_IMU_DIR_BACK))
	{
		ob_selfcheck.b_is_read_data = true;
	}
	
}

static void selfcheck_flow_tooling_board_process(void)
{
	int16_t 					rol;
	static uint32_t 	t_count = 0;
	
	//喂狗
	feed_watchdog();
	
	//-----读取中间传感器数据，检测加速度roll值
	if(drv_qma_get_acc_data(&ob_selfcheck.m_data) != 0)ob_selfcheck.selfcheck_result |= (1 << SELFCHECK_RESULT_ERR_MIDDLE_NO_DATA);
	Mahony_process(0,0,0,ob_selfcheck.m_data.acc[0],ob_selfcheck.m_data.acc[1],ob_selfcheck.m_data.acc[2],0,0,0);
	
	t_count++;
	switch(t_count)
	{
		case (2000/LowPower_Interval):						//第2秒查看前后传感器是否配置且读取成功
			if(bll_imu_query_config_param_is_ready(BLL_IMU_DIR_FRONT,&game_bll_imu_param_t) != BLL_IMU_CONFIG_FINISH)ob_selfcheck.selfcheck_result |= (1 << SELFCHECK_RESULT_ERR_FRONT_SENSOR_CONFIG);
			if(bll_imu_query_config_param_is_ready(BLL_IMU_DIR_BACK,&game_bll_imu_param_t) != BLL_IMU_CONFIG_FINISH)ob_selfcheck.selfcheck_result |= (1 << SELFCHECK_RESULT_ERR_BACK_SENSOR_CONFIG);
			if(ob_selfcheck.f_is_read_data != true)ob_selfcheck.selfcheck_result |= (1 << SELFCHECK_RESULT_ERR_FRONT_NO_DATA);
			if(ob_selfcheck.b_is_read_data != true)ob_selfcheck.selfcheck_result |= (1 << SELFCHECK_RESULT_ERR_BACK_NO_DATA);
			break;
		
		case (4000/LowPower_Interval):						//第4秒检测加速度roll值
			rol  = (int16_t)(getRoll()*1);
			if(!(SELFCHECK_THRESHOLD_IS_MET(SELFCHECK_MIDDLE_ACC_ROLL_MIN_THRESHOLD,rol,SELFCHECK_MIDDLE_ACC_ROLL_MAX_THRESHOLD)))ob_selfcheck.selfcheck_result |= (1 << SELFCHECK_RESULT_ERR_MIDDLE_ACC_ROLL);
			break;
		
		case (10000/LowPower_Interval):						//第10秒能否读取到任意广播名字，且最小的RSSI是否满足条件
			if(SELFCHECK_SCAN_DEVICE_RSSI_MIN_THRESHOLD >= ob_selfcheck.max_rssi)ob_selfcheck.selfcheck_result |= (1 << SELFCHECK_RESULT_ERR_RSSI);
																							//若自检结果为通过，则将自检流程标志位改为“整体自检”，flash保存（先保存，再发送。不然发送完，还没保存就断电，导致没进入下个阶段就被拿去装鞋）
			if(ob_selfcheck.selfcheck_result == SELFCHECK_RESULT_SUCCESS)
			{
				mFlash.selfcheck_flow 	= SELFCHECK_FLOW_COMPLETE_BOARD;
				mBackup.selfcheck_flow 	= SELFCHECK_FLOW_COMPLETE_BOARD;
				if(Flash_SaveInfomation()!= ZONE_OP_SUCCESS)ob_selfcheck.selfcheck_result |= (1 << SELFCHECK_RESULT_ERR_FLASH_WRITE);
				if(Flash_SaveBackup() != ZONE_OP_SUCCESS)ob_selfcheck.selfcheck_result |= (1 << SELFCHECK_RESULT_ERR_FLASH_WRITE);
			}
			break;
	}
	
	//-----持续将自检结果发送给单片机3分钟，之后重启。（快速进入下一阶段，而不是等断电进入下一阶段，以防flash失败导致自检流程标志位异常）
	if(t_count > (10000/LowPower_Interval) && t_count < (15000/LowPower_Interval))
	{
		UART0_SendChar(ob_selfcheck.selfcheck_result);
	}
	else if(t_count > (15000/LowPower_Interval))
	{
		NVIC_SystemReset();
	}
}

static bool is_wear_insole(void)
{
	uint16_t front_mag_norm = sqrt((double)(ob_selfcheck.f_data.mag[0]*ob_selfcheck.f_data.mag[0]) + (double)(ob_selfcheck.f_data.mag[1]*ob_selfcheck.f_data.mag[1]) + (double)(ob_selfcheck.f_data.mag[2]*ob_selfcheck.f_data.mag[2]));
		
	uint16_t back_mag_norm = sqrt((double)(ob_selfcheck.b_data.mag[0]*ob_selfcheck.b_data.mag[0]) + (double)(ob_selfcheck.b_data.mag[1]*ob_selfcheck.b_data.mag[1]) + (double)(ob_selfcheck.b_data.mag[2]*ob_selfcheck.b_data.mag[2]));

	return (SELFCHECK_THRESHOLD_IS_MET(SELFCHECK_WEAR_INSOLE_MAG_NORM_MIN_THRESHOLD,front_mag_norm,SELFCHECK_WEAR_INSOLE_MAG_NORM_MAX_THRESHOLD) && \
					SELFCHECK_THRESHOLD_IS_MET(SELFCHECK_WEAR_INSOLE_MAG_NORM_MIN_THRESHOLD,back_mag_norm,SELFCHECK_WEAR_INSOLE_MAG_NORM_MAX_THRESHOLD))?true:false;
}

static bool is_charge_chip_can_used(void)
{
	#define SELFCHECK_BATTERY_VOL_THRESHOLD_MIN											2500		//2.5V
	#define SELFCHECK_BATTERY_VOL_THRESHOLD_MAX											4000		//4V
	#define SELFCHECK_CHARGE_VOL_THRESHOLD													80			//充电前和充电期间的充电电压变化值,单位mv
	
	//监测电池和充电数据
	int16_t						bat_vol;
	int16_t						charge_vol;
	int16_t						charge_threshold;
	uint32_t 					ch = nrf_gpio_pin_read(PIN_CHARGING);
	static uint32_t 	charge_cycle = 20;
	static int16_t		before_charge_vol;		//充电前的电压值
	static int16_t		charge_vol_max;				//充电期间最大的电压值
	bool							ret = true;
	
	if(!ch)//没充电
	{		
		charge_vol = ADC_RESULT_IN_MILLI_VOLTS(ADC_GetValue(PIN_ADC_CHARGMEASURE_CHANNEL));
		before_charge_vol = before_charge_vol > charge_vol ? charge_vol : before_charge_vol;
		charge_vol_max = 0;
		charge_cycle = 20;
	}
	else  //充电
	{
		/* 过筛20轮 */
		if(charge_cycle != 0){
			charge_vol = ADC_RESULT_IN_MILLI_VOLTS(ADC_GetValue(PIN_ADC_CHARGMEASURE_CHANNEL));
			charge_vol_max = charge_vol_max < charge_vol ? charge_vol : charge_vol_max;
			charge_cycle--;
			return ret;
		}
		charge_vol = ADC_RESULT_IN_MILLI_VOLTS(ADC_GetValue(PIN_ADC_CHARGMEASURE_CHANNEL));
		charge_vol_max = charge_vol_max < charge_vol ? charge_vol : charge_vol_max;		
		bat_vol    = ADC_RESULT_IN_MILLI_VOLTS(ADC_GetValue(PIN_ADC_BAT_CHANNEL))*5/3;
		
		//当电池电量没满，充电（经测试，电量没满的充电电压跟电池电压有关，最小充电电压100+mv）
		charge_threshold = 	charge_vol_max - before_charge_vol;	//充电前和充电期间的充电电压变化值

		if(charge_threshold < SELFCHECK_CHARGE_VOL_THRESHOLD && bat_vol < SELFCHECK_BATTERY_VOL_THRESHOLD_MAX)
		{			
			ret = false;
		}
	}
	
	return ret;
}

static void selfcheck_flow_complete_board_process_2(void)
{
	uint8_t 					buf[256];
	uint8_t 					L=0;
	static uint32_t 	t_count = 0;
	uint32_t					t_threshold = (3000/LowPower_Interval);
	static uint8_t		led_control = 0;
	static uint8_t		wdt_count = 2;
	
	//喂狗
	feed_watchdog();
	
	if(t_count != t_threshold)t_count++;
	
	//-----计算中间传感器的roll值
	drv_qma_get_acc_data(&ob_selfcheck.m_data);
	Mahony_process(0,0,0,ob_selfcheck.m_data.acc[0],ob_selfcheck.m_data.acc[1],ob_selfcheck.m_data.acc[2],0,0,0);
	
	//-----当电量未满4.0V时，充电芯片的充电电压是否超过阈值，若未超过，证明充电芯片异常，设置对应的失败灯
	if(is_charge_chip_can_used() == false)ob_selfcheck.selfcheck_result_led_color = COLOR_BLUE;
	
	if(t_count == t_threshold)
	{
		//-----上报中间传感器的加速度值和roll值
		L=0;
		int16_t rol  = (int16_t)(getRoll()*100);
		//-----roll值若有异常，则设置对应的失败灯。
		if(!(SELFCHECK_THRESHOLD_IS_MET(SELFCHECK_MIDDLE_ACC_ROLL_MIN_THRESHOLD,(rol/100),SELFCHECK_MIDDLE_ACC_ROLL_MAX_THRESHOLD)))ob_selfcheck.selfcheck_result_led_color = COLOR_LIGHPURPLE;
		int16_t pitch = (int16_t)(getPitch()*100);
		int16_t yaw   = (int16_t)(getYaw()*100);
		buf[L++] = (uint8_t)(rol>>8);
		buf[L++] = (uint8_t)(rol>>0);
		buf[L++] = (uint8_t)(pitch>>8);
		buf[L++] = (uint8_t)(pitch>>0);
		buf[L++] = (uint8_t)(yaw>>8);
		buf[L++] = (uint8_t)(yaw>>0);
		buf[L++] = 0;
		buf[L++] = 0;
		buf[L++] = 0;
		buf[L++] = 0;
		buf[L++] = 0; 
		buf[L++] = 0;
		buf[L++] = 0;
		Mahony_send_ANO(0x01,buf,L);
		L=0;
		int32_t middle_acc_x = ob_selfcheck.m_data.acc[0];
		int32_t middle_acc_y = ob_selfcheck.m_data.acc[1];
		int16_t middle_acc_z = ob_selfcheck.m_data.acc[2];
		middle_acc_x *= 100;
		middle_acc_y *= 100;
		middle_acc_z *= 10;
		buf[L++] = (uint8_t)(middle_acc_x>>24);
		buf[L++] = (uint8_t)(middle_acc_x>>16);
		buf[L++] = (uint8_t)(middle_acc_x>>8);
		buf[L++] = (uint8_t)(middle_acc_x>>0);
		buf[L++] = (uint8_t)(middle_acc_y>>24);
		buf[L++] = (uint8_t)(middle_acc_y>>16);
		buf[L++] = (uint8_t)(middle_acc_y>>8);
		buf[L++] = (uint8_t)(middle_acc_y>>0);
		buf[L++] = (uint8_t)(middle_acc_z>>8);
		buf[L++] = (uint8_t)(middle_acc_z>>0);
		Mahony_send_ANO(0x07,buf,L);
		
		//-----上报前脚传感器的加速度数据、陀螺仪数据、地磁计开平方和数据
		//-----上报后脚传感器的地磁计开平方和数据
		//-----上报扫描到的任意广播的rssi值
		L=0;
		buf[L++] = (uint8_t)(ob_selfcheck.f_data.acc[0]>>8);
		buf[L++] = (uint8_t)(ob_selfcheck.f_data.acc[0]>>0);
		buf[L++] = (uint8_t)(ob_selfcheck.f_data.acc[1]>>8);
		buf[L++] = (uint8_t)(ob_selfcheck.f_data.acc[1]>>0);
		buf[L++] = (uint8_t)(ob_selfcheck.f_data.acc[2]>>8);
		buf[L++] = (uint8_t)(ob_selfcheck.f_data.acc[2]>>0);
		buf[L++] = (uint8_t)(ob_selfcheck.f_data.gry[0]>>8);
		buf[L++] = (uint8_t)(ob_selfcheck.f_data.gry[0]>>0);
		buf[L++] = (uint8_t)(ob_selfcheck.f_data.gry[1]>>8);
		buf[L++] = (uint8_t)(ob_selfcheck.f_data.gry[1]>>0);
		buf[L++] = (uint8_t)(ob_selfcheck.f_data.gry[2]>>8);
		buf[L++] = (uint8_t)(ob_selfcheck.f_data.gry[2]>>0);
		uint16_t front_mag_norm = sqrt((double)(ob_selfcheck.f_data.mag[0]*ob_selfcheck.f_data.mag[0]) + (double)(ob_selfcheck.f_data.mag[1]*ob_selfcheck.f_data.mag[1]) + (double)(ob_selfcheck.f_data.mag[2]*ob_selfcheck.f_data.mag[2]));
		uint16_t back_mag_norm = sqrt((double)(ob_selfcheck.b_data.mag[0]*ob_selfcheck.b_data.mag[0]) + (double)(ob_selfcheck.b_data.mag[1]*ob_selfcheck.b_data.mag[1]) + (double)(ob_selfcheck.b_data.mag[2]*ob_selfcheck.b_data.mag[2]));
		buf[L++] = (uint8_t)(front_mag_norm>>8);
		buf[L++] = (uint8_t)(front_mag_norm>>0);
		buf[L++] = (uint8_t)(back_mag_norm>>8);
		buf[L++] = (uint8_t)(back_mag_norm>>0);
		buf[L++] = (uint8_t)(ob_selfcheck.max_rssi>> 8);
		buf[L++] = (uint8_t)(ob_selfcheck.max_rssi>>0);
		Mahony_send_ANO(0x02,buf,L);
		
		//-----第1个100毫秒亮灯，连续9个100毫秒灭灯
		led_control++;
		if(led_control == 1)
		{
			nrf_gpio_pin_write(PIN_LED_ENABLE,LED_ENABLE);
			Pwm_Initialize();
			nrf_delay_ms(5);
			WS2812_DisplayDot(ob_selfcheck.selfcheck_result_led_color);
			WS2812_Pwm_Play();
		}
		else
		{
			nrf_gpio_pin_write(PIN_LED_ENABLE,LED_DISABLE);
			Pwm_UnInitialize();
		}
		if(led_control == 10)led_control = 0;
		
		//-----检测到充电，震动200ms一次。
		if(nrf_gpio_pin_read(PIN_CHARGING))
		{
			if(wdt_count){nrf_gpio_cfg_output(PIN_MT_EN);nrf_gpio_pin_write(PIN_MT_EN,1);wdt_count--;}
			else nrf_gpio_pin_write(PIN_MT_EN,0);
		}
		else
		{
			wdt_count = 2;
		}
		
	}
}

static void selfcheck_flow_complete_board_process_1(void)
{
	int 										ret = -1;
	BLL_IMU_CONFIG_RESULT  	config_result;
	
	//喂狗
	feed_watchdog();
	
	config_result = bll_imu_query_config_param_is_ready(BLL_IMU_DIR_FRONT,&game_bll_imu_param_t);

	//-----判断前后脚IMU配置是否成功，若成功，进行下一步判断；若失败，设置对应的失败灯，且重新配置前后脚。
	if(config_result != BLL_IMU_CONFIG_FINISH)
	{
		if(config_result == BLL_IMU_CONFIG_FAIL_FRONT_MAG)ob_selfcheck.selfcheck_result_led_color = COLOR_LIGHRED;//前脚mag配置失败
		if(config_result == BLL_IMU_CONFIG_FAIL_BACK_MAG)ob_selfcheck.selfcheck_result_led_color = COLOR_WHITE;//后脚mag配置失败
		if(config_result == BLL_IMU_CONFIG_FAIL_FRONT_SIX_AXIS)ob_selfcheck.selfcheck_result_led_color = COLOR_RED;//前脚lsm配置失败
		
		//重新配置
		bll_imu_close();
		bll_imu_Init();
		bll_imu_register_data_notify_callback(BLL_IMU_DATA_NOTIFY_CB_PRIORITY_1, fb_data_notify_cb);
		bll_imu_Resume_config_param(&game_bll_imu_param_t);
		return;
	}
	
	//-----判断是否正常读取前脚IMU数据，若成功，进行下一步判断，若失败，设置对应的失败灯，且重新配置前后脚。
	if(ob_selfcheck.f_is_read_data == false)
	{
		ob_selfcheck.selfcheck_result_led_color = COLOR_CYAN;
		//重新配置
		bll_imu_close();
		bll_imu_Init();
		bll_imu_register_data_notify_callback(BLL_IMU_DATA_NOTIFY_CB_PRIORITY_1, fb_data_notify_cb);
		bll_imu_Resume_config_param(&game_bll_imu_param_t);
		return;
	}
	else
	{
		ob_selfcheck.f_is_read_data = false;
	}
	
	//-----判断是否正常读取后脚IMU数据，若成功，进行下一步判断，若失败，设置对应的失败灯，且重新配置前后脚。
	if(ob_selfcheck.b_is_read_data == false)
	{
		ob_selfcheck.selfcheck_result_led_color = COLOR_YELLOW;
		//重新配置
		bll_imu_close();
		bll_imu_Init();
		bll_imu_register_data_notify_callback(BLL_IMU_DATA_NOTIFY_CB_PRIORITY_1, fb_data_notify_cb);
		bll_imu_Resume_config_param(&game_bll_imu_param_t);
		return;
	}
	else
	{
		ob_selfcheck.b_is_read_data = false;
	}
	
	//-----判断是否正常读取中间IMU数据，若成功，进行下一步判断，若失败，设置对应的失败灯，且重新配置中间IMU。
	memset(&ob_selfcheck.m_data,0,sizeof(ob_selfcheck.m_data));
	
	ret = drv_qma_get_acc_data(&ob_selfcheck.m_data);
	if(ret != 0)
	{
		ob_selfcheck.selfcheck_result_led_color = COLOR_PURPLE;
		//重新配置
		drv_qma_Init();
		nrf_delay_ms(20);
		drv_qma_set_acc_odr(QMA_ACC_ODR_104HZ);
		
		return;
	}
	
	//-----上述自检项目都通过，则判断当前是否充电，充电则设置橙灯，否则设置绿灯。  
	if(nrf_gpio_pin_read(PIN_CHARGING))
	{
		ob_selfcheck.selfcheck_result_led_color = COLOR_PURPLE;
	}
	else
	{
		ob_selfcheck.selfcheck_result_led_color = COLOR_GREEN;
	}
	
	//-----根据前后脚地磁数据读取成功情况下，若判断为穿鞋垫，则将自检流程标志位改为“自检完成”，flash保存，重启。（避免到用户手上时，亮灯）
	if(is_wear_insole())
	{
		mFlash.selfcheck_flow 	= SELFCHECK_FLOW_DONE;
		mBackup.selfcheck_flow 	= SELFCHECK_FLOW_DONE;
		
		if(Flash_SaveInfomation()!= ZONE_OP_SUCCESS)
		{
			while(1)
			{
				//喂狗
				feed_watchdog();
				//打印错误信息
				DEBUG_LOG("selfcheck:Flash_SaveInfomation()!= ZONE_OP_SUCCESS\r\n");
				//除了配对信息等基础信息外，擦除整个区块，然后重新保存，保存成功，break，否则延时100毫秒
				Flash_DeleteAllInfor();
				if(mBackup.head == FLASH_HEAD)
				{
					uint8_t i =0;
					for(i=0;i<RecordMacAddrL;i++){
					 mFlash.macHost[i] =	mBackup.macAddr_L[i];	      //主机地址
					 mFlash.mClient.macAddr[i] = mBackup.macAddr_R[i];//从机地址
					}

					mFlash.mClient.hardVersion = mBackup.hardVersion;
					mFlash.mClient.sotfVersion = mBackup.sotfVersion;
					mFlash.mClient.isConfig = mBackup.isConfig;
					mFlash.isHost = mBackup.isHost;
					mFlash.LR_FLAG = mBackup.LR_FLAG;
					mFlash.selfcheck_flow = mBackup.selfcheck_flow;
				}
				if(Flash_SaveInfomation()== ZONE_OP_SUCCESS)break;else nrf_delay_ms(100);
			}
		}
		if(Flash_SaveBackup() != ZONE_OP_SUCCESS)
		{
			while(1)
			{
				//喂狗
				feed_watchdog();
				//打印错误信息
				DEBUG_LOG("selfcheck:Flash_SaveBackup()!= ZONE_OP_SUCCESS\r\n");
				//除了配对信息等基础信息外，擦除整个区块，然后重新保存，保存成功，break，否则延时100毫秒
				Flash_DeleteAllBackup();
				if(mFlash.head == FLASH_HEAD)
				{
					uint8_t i =0;
					for(i=0;i<RecordMacAddrL;i++){
					 mBackup.macAddr_L[i] = mFlash.macHost[i];	      //主机地址
					 mBackup.macAddr_R[i] = mFlash.mClient.macAddr[i];//从机地址
					}

					mBackup.hardVersion = mFlash.mClient.hardVersion;
					mBackup.sotfVersion = mFlash.mClient.sotfVersion;
					mBackup.isConfig = mFlash.mClient.isConfig;
					mBackup.isHost = mFlash.isHost;
					mBackup.LR_FLAG = mFlash.LR_FLAG;
					mBackup.selfcheck_flow = mFlash.selfcheck_flow;
				}
				if(Flash_SaveBackup()== ZONE_OP_SUCCESS)break;else nrf_delay_ms(100);
			}
		}
		
		NVIC_SystemReset();
	}
	
	
	//-----完整板自检线程2
	Process_Start(SELFCHECK_FLOW_COMPLETE_BOARD_PROCESS_2_CYCLE,"selfcheck_flow_complete_board_process_2",selfcheck_flow_complete_board_process_2);
}

/*API ------------------------------------------------------------------------------------------------------------------------------------*/
/**
	@brief 		初始化自检
	@param 		无
	@return 	当前的自检流程
*/
SELFCHECK_FLOW_e selfcheck_Init(void)
{
	int 							error;
	int16_t 					adc_value;
	SELFCHECK_FLOW_e 	ret;
	
	//初始化结构体
	memset(&ob_selfcheck,0,sizeof(ob_selfcheck));
	ob_selfcheck.max_rssi 											= SELFCHECK_SCAN_DEVICE_RSSI_MIN_THRESHOLD;
	ob_selfcheck.selfcheck_result 							= SELFCHECK_RESULT_SUCCESS;
	ob_selfcheck.selfcheck_result_led_color			= COLOR_BLACK;
	ob_selfcheck.f_is_read_data									= false;
	ob_selfcheck.b_is_read_data									= false;
	
	//读取flash的自检流程标志位，判断当前自检流程
	switch(mFlash.selfcheck_flow)
	{
		case SELFCHECK_FLOW_DONE:																																//自检完成
			//-----保留该case，以防后期需要在此添加需求。
      //-----自检流程标志位只有该应用可更改，其余应用不可更改。
			ret = SELFCHECK_FLOW_DONE;
			break;
		
		case SELFCHECK_FLOW_COMPLETE_BOARD:																											//完整板自检
			//-----震动电机200ms
			nrf_gpio_cfg_output(PIN_MT_EN);					
			nrf_gpio_pin_write(PIN_MT_EN,1);			
			nrf_delay_ms(200);			
			nrf_gpio_pin_write(PIN_MT_EN,0);
		
			//-----开启扫描，上报扫描到的任意广播的rssi值
			ST_scan_stop();
			error = ST_scan_start();
			if(error != APP_SUCCESS)ob_selfcheck.selfcheck_result |= (1 << SELFCHECK_RESULT_ERR_ADV_INIT);
			advdata_report_Evt_Regist(scan_report_cb);
		
			//-----初始化LED灯
			LED_Init();
		
			//-----初始化前中后传感器
			bll_imu_Init();
			bll_imu_register_data_notify_callback(BLL_IMU_DATA_NOTIFY_CB_PRIORITY_1, fb_data_notify_cb);
			bll_imu_Resume_config_param(&game_bll_imu_param_t);
			error = drv_qma_Init();
			if(error == -1)ob_selfcheck.selfcheck_result |= (1 << SELFCHECK_RESULT_ERR_MIDDLE_SENSOR_CONFIG);
			nrf_delay_ms(20);
			error = drv_qma_set_acc_odr(QMA_ACC_ODR_104HZ);
			if(error == -1)ob_selfcheck.selfcheck_result |= (1 << SELFCHECK_RESULT_ERR_MIDDLE_SENSOR_CONFIG);
			
			//-----初始化计算roll值算法
			Mahony_Init(10);
			
			//-----完整板自检线程1
			Process_Start(SELFCHECK_FLOW_COMPLETE_BOARD_PROCESS_1_CYCLE,"selfcheck_flow_complete_board_process_1",selfcheck_flow_complete_board_process_1);
		
			ret = SELFCHECK_FLOW_COMPLETE_BOARD;
			break;
		
		case SELFCHECK_FLOW_TOOLING_BOARD:																											//工装板自检
		default:
			//-----将LGB_DATA引脚配置成串口发送
			UART0_Init(PIN_LED_CONTROL,0xFF,115200);
		
			//-----开启扫描，10秒内能否读取到任意广播名字，且最小的RSSI是否满足条件
			ST_scan_stop();
			error = ST_scan_start();
			if(error != APP_SUCCESS)ob_selfcheck.selfcheck_result |= (1 << SELFCHECK_RESULT_ERR_ADV_INIT);
			advdata_report_Evt_Regist(scan_report_cb);

			//-----打开LED电源输出
			nrf_gpio_cfg(
			PIN_LED_ENABLE,
			NRF_GPIO_PIN_DIR_OUTPUT,
			NRF_GPIO_PIN_INPUT_DISCONNECT,
			NRF_GPIO_PIN_NOPULL,
			NRF_GPIO_PIN_S0D1,
			NRF_GPIO_PIN_NOSENSE);
			nrf_gpio_pin_write(PIN_LED_ENABLE,SELFCHECK_LED_ENABLE);
			
			//-----电机引脚置高
			nrf_gpio_cfg_output(PIN_MT_EN);		nrf_gpio_pin_write(PIN_MT_EN,1);
			
			//-----读取配对引脚的ADC值是否满足条件
			ADC_Disable();
			ADC_SetPinChannel(PIN_CHARGING, PIN_CHARGING_CHANNEL, NRF_GPIO_PIN_NOPULL);
			ADC_Initialize();
			error = ADC_Read(PIN_CHARGING_CHANNEL, &adc_value);
			if(error != ADC_OP_SUCCESS)ob_selfcheck.selfcheck_result |= (1 << SELFCHECK_RESULT_ERR_READ_ADC);
			else if(!(SELFCHECK_THRESHOLD_IS_MET(SELFCHECK_PAIR_PIN_ADC_MIN_THRESHOLD,adc_value,SELFCHECK_PAIR_PIN_ADC_MAX_THRESHOLD))){
				ob_selfcheck.selfcheck_result |= (1 << SELFCHECK_RESULT_ERR_PAIR_PIN_ADC);
			}
			
			//-----电池用电阻代替，读取ADC，测试充电芯片是否正常
			error = ADC_Read(PIN_ADC_CHARGMEASURE_CHANNEL, &adc_value);
			if(error != ADC_OP_SUCCESS)ob_selfcheck.selfcheck_result |= (1 << SELFCHECK_RESULT_ERR_READ_ADC);
			else if(!(SELFCHECK_THRESHOLD_IS_MET(SELFCHECK_CHARGE_CHIP_PIN_ADC_MIN_THRESHOLD,adc_value,SELFCHECK_CHARGE_CHIP_PIN_ADC_MAX_THRESHOLD))){
				ob_selfcheck.selfcheck_result |= (1 << SELFCHECK_RESULT_ERR_CHARGE_CHIP_PIN_ADC);
			}
			
			//-----电池用电阻代替，读取ADC，测试电池分压电阻是否正常
			error = ADC_Read(PIN_ADC_BAT_CHANNEL, &adc_value);
			if(error != ADC_OP_SUCCESS)ob_selfcheck.selfcheck_result |= (1 << SELFCHECK_RESULT_ERR_READ_ADC);
			else if(!(SELFCHECK_THRESHOLD_IS_MET(SELFCHECK_BATTERY_PIN_ADC_MIN_THRESHOLD,adc_value,SELFCHECK_BATTERY_PIN_ADC_MAX_THRESHOLD))){
				ob_selfcheck.selfcheck_result |= (1 << SELFCHECK_RESULT_ERR_BATTERY_PIN_ADC);
			}
			
			//-----配置前脚传感器、中间传感器、后脚传感器
			bll_imu_Init();
			bll_imu_register_data_notify_callback(BLL_IMU_DATA_NOTIFY_CB_PRIORITY_1, fb_data_notify_cb);
			bll_imu_Resume_config_param(&game_bll_imu_param_t);
			error = drv_qma_Init();
			if(error == -1)ob_selfcheck.selfcheck_result |= (1 << SELFCHECK_RESULT_ERR_MIDDLE_SENSOR_CONFIG);
			nrf_delay_ms(20);
			error = drv_qma_set_acc_odr(QMA_ACC_ODR_104HZ);
			if(error == -1)ob_selfcheck.selfcheck_result |= (1 << SELFCHECK_RESULT_ERR_MIDDLE_SENSOR_CONFIG);
			
			//-----初始化计算roll值算法
			Mahony_Init(10);
			
			//-----工装自检线程
			Process_Start(SELFCHECK_FLOW_TOOLING_BOARD_PROCESS_CYCLE,"selfcheck_flow_tooling_board_process",selfcheck_flow_tooling_board_process);
			
			ret = SELFCHECK_FLOW_TOOLING_BOARD;
			break;
	}
	
	//-----设置为低功耗休眠
	Sleep_Duration_Control_Regist(sleep_duration_control_low_power_interval);
	
	//开启看门狗
	watchdog_init();
	feed_watchdog();	
	
	return ret;
}