shoe_mcu/system/selfcheck.c

/*Includes ----------------------------------------------*/
#include "tool.h"
#include "hal_MahonyAHRS.h"
#include "ble_comm.h"
#include "nrf_delay.h"
#include "bsp_pwm.h"
#include "bsp_time.h"
#include "fml_adc.h"
#include "bsp_wdt.h"
#include "exception.h"
#include "selfcheck.h"
#include "system.h"
#include "drv_lsm6ds3tr_c.h"
#include "drv_qmc6310_v2.h"
#include "drv_qma7981.h"
#include "hal_led.h"
#include "hal_battery.h"
#include "hal_charge.h"
#include "hal_ble_common.h"
#include "bll_imu.h"
#include "app_flash.h"
#include "system.h"
#include "hal_mahonyAHRS.h"
#include "app_detectIsHost.h"
#include "drv_trigger.h"

/*Private macro ------------------------------------------------------------------------------------------------------------------------------------*/

#define	SELFCHECK_SCAN_DEVICE_RSSI_MIN_THRESHOLD_3					                    -18																								//需要扫描的设备的RSSI最小阈值，-76

#define	SELFCHECK_SCAN_DEVICE_RSSI_MIN_THRESHOLD_2					                   -60																								//需要扫描的设备的RSSI最小阈值，-76

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

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

#define	SELFCHECK_BATTERY_PIN_ADC_MAX_THRESHOLD						                     4300																								//电池分压电阻的最大ADC阈值，拆掉电阻后，原始值为3721，换算为5.49V

#define SELFCHECK_MIDDLE_ACC_PROS_ROLL_MIN_THRESHOLD													 0																									//中间加速度正向ROLL值最小阈值

#define	SELFCHECK_MIDDLE_ACC_PROS_ROLL_MAX_THRESHOLD													 35																									//中间加速度正向ROLL值最大阈值

#define SELFCHECK_MIDDLE_ACC_CONS_ROLL_MIN_THRESHOLD													 160																								//中间加速度反向ROLL值最小阈值

#define	SELFCHECK_MIDDLE_ACC_CONS_ROLL_MAX_THRESHOLD													 180																								//中间加速度反向ROLL值最大阈值

#define SELFCHECK_SENSOR_MAG_NO_WELDING_CAPACITOR_MIN_THRESHOLD								 200																								//地磁传感器没焊接电容的最小阈值

#define	SELFCHECK_LED_DISPLAY_CYCLE_THRESHOLD																	 300																								//自检灯显示周期时间，单位ms

#define	SELFCHECK_MIDDLE_ACC_CHECK_MT_MIN_THRESHOLD_2													 3000																								//中间加速度检测电机最小差值,不震动的差值是129,有个能震动的差值是1800+

#define	SELFCHECK_MIDDLE_ACC_CHECK_MT_MIN_THRESHOLD_3													 1000																								//中间加速度检测电机最小差值,不震动的差值是129,有个能震动的差值是1800+

#define SELFCHECK_IMU_MONITOR_DATA_ERR_SUM_MAX																 416																								//数据监测错误累计最大值

#define	SELFCHECK_CHARGE_CHIP_PIN_ADC_COUNT																		 5																									//充电芯片引脚检测次数


/*STRUCTION ------------------------------------------------------------------------------------------------------------------------------------*/
typedef enum {
	SELFCHECK_RESULT_SUCCESS = 0,																																																		//自检成功
	
	SELFCHECK_RESULT_ERR_FRONT_SENSOR_CONFIG_SIX_AXIS_ID,																																						//自检失败——前脚传感器配置六轴ID
	
	SELFCHECK_RESULT_ERR_FRONT_SENSOR_CONFIG_MAG_ID,																																								//自检失败——前脚传感器配置地磁ID
	
	SELFCHECK_RESULT_ERR_BACK_SENSOR_CONFIG_ID,																																											//自检失败——后脚传感器配置地磁ID
	
	SELFCHECK_RESULT_ERR_FRONT_IMU_REVERSE,																																													//自检失败——前脚传感器装反
	
	SELFCHECK_RESULT_ERR_FRONT_MAG_DATA_TO_SMALL,																																										//自检失败——前脚传感器地磁数据过小
	
	SELFCHECK_RESULT_ERR_BACK_DATA_TO_SMALL,																																												//自检失败——后脚传感器地磁数据过小
	
	SELFCHECK_RESULT_ERR_FRONT_SENSOR_CONFIG_SIX_AXIS,																																							//自检失败——前脚传感器配置六轴
	
	SELFCHECK_RESULT_ERR_FRONT_SENSOR_CONFIG_MAG,																																										//自检失败——前脚传感器配置地磁
	
	SELFCHECK_RESULT_ERR_FRONT_SIX_AXIS_NO_DATA,																																										//自检失败——前脚传感器六轴没数据
	
	SELFCHECK_RESULT_ERR_FRONT_SIX_AXIS_EXCP_DATA,																																									//自检失败——前脚传感器六轴数据异常(数据持续相等)
	
	SELFCHECK_RESULT_ERR_FRONT_MAG_NO_DATA,																																													//自检失败——前脚传感器地磁没数据
	
	SELFCHECK_RESULT_ERR_FRONT_MAG_EXCP_DATA,																																												//自检失败——前脚传感器地磁数据异常(数据持续相等)
	
	SELFCHECK_RESULT_ERR_BACK_SENSOR_CONFIG,																																												//自检失败——后脚传感器配置
	
	SELFCHECK_RESULT_ERR_BACK_NO_DATA,																																															//自检失败——后脚传感器没数据
	
	SELFCHECK_RESULT_ERR_BACK_EXCP_DATA,																																														//自检失败——后脚传感器数据异常(数据持续相等)
	
	SELFCHECK_RESULT_ERR_MIDDLE_SENSOR_CONFIG,																																											//自检失败——中间传感器配置
	
	SELFCHECK_RESULT_ERR_MIDDLE_NO_DATA_OR_EXCP_DATA,																																								//自检失败——中间传感器没数据或数据异常
	
	SELFCHECK_RESULT_ERR_CHARGE_CHIP_PIN_ADC,																																												//自检失败——充电芯片引脚ADC
	
	SELFCHECK_RESULT_ERR_BATTERY_PIN_ADC,																																														//自检失败——电池分压电阻ADC
	
	SELFCHECK_RESULT_ERR_RSSI,																																																			//自检失败——RSSI
	
	SELFCHECK_RESULT_ERR_MT,																																																				//自检失败——震动电机
	
} SELFCHECK_RESULT_e;

typedef struct _selfcheck
{
	uint32_t																  			  selfcheck_result;																															//自检结果
	
	uint32_t     																			selfcheck_result_led_color;																										//自检结果的led灯颜色
	
	uint32_t																					selfcheck_result_flash_num;																										//自检结果闪烁次数
	
	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;																																//后脚传感器是否读到数据标志位
	
	char 																							order;																																				//指令
	
	uint32_t																					charge_chip_adc_max;																													//充电芯片阈值满足数
	
	int16_t																						battery_adc_max;																															//电量最大值
	
} 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,
};

static MahonyAHRS_t Self_Mind_Mahony={0};
static MahonyAHRS_t Self_Front_Mahony={0};


/*Local Functions ------------------------------------------------------------------------------------------------------------------------------------*/
static void monitor_sensor_data(int16_t *f_acc, int16_t *f_gry, int16_t *f_mag, int16_t *b_mag)
{
	static int16_t 																last_f_acc[3];																								//上一次的前脚加速度值
	
	static int16_t 																last_f_gry[3];																								//上一次的前脚陀螺仪值
	
	static int16_t 																last_f_mag[3];																								//上一次的前脚地磁计值
	
	static int16_t 																last_b_mag[3];																								//上一次的后脚地磁计值	
	
	static int16_t																last_f_acc_err_sum;																						//上一次的前脚加速度值错误累计
	
	static int16_t 																last_f_gry_err_sum;																						//上一次的前脚陀螺仪值错误累计
	
	static int16_t 																last_f_mag_err_sum;																						//上一次的前脚地磁计值错误累计
	
	static int16_t 																last_b_mag_err_sum;																						//上一次的后脚地磁计值错误累计
	
	/*前脚加速度*/
	if(f_acc != NULL)
	{
		if(
				last_f_acc[0] == f_acc[0] && \
				last_f_acc[1] == f_acc[1] && \
				last_f_acc[2] == f_acc[2]
			)
		{
			last_f_acc_err_sum++;
			
			if(last_f_acc_err_sum >= SELFCHECK_IMU_MONITOR_DATA_ERR_SUM_MAX){
				ob_selfcheck.selfcheck_result |= (1 << SELFCHECK_RESULT_ERR_FRONT_SIX_AXIS_EXCP_DATA);
			}
			
		}else{
			last_f_acc_err_sum = 0;
		}
		
		last_f_acc[0] = f_acc[0];
		last_f_acc[1] = f_acc[1];
		last_f_acc[2] = f_acc[2];
	}
	/*前脚陀螺仪*/
	if(f_gry != NULL)
	{
		if(
				last_f_gry[0] == f_gry[0] && \
				last_f_gry[1] == f_gry[1] && \
				last_f_gry[2] == f_gry[2]
			)
		{
			last_f_gry_err_sum++;
			
			if(last_f_gry_err_sum >= SELFCHECK_IMU_MONITOR_DATA_ERR_SUM_MAX){
				ob_selfcheck.selfcheck_result |= (1 << SELFCHECK_RESULT_ERR_FRONT_SIX_AXIS_EXCP_DATA);
			}
				
		}else{
			last_f_gry_err_sum = 0;
		}
		
		last_f_gry[0] = f_gry[0];
		last_f_gry[1] = f_gry[1];
		last_f_gry[2] = f_gry[2];
	}
	/*前脚地磁计*/
	if(f_mag != NULL)
	{
		if(
				last_f_mag[0] == f_mag[0] && \
				last_f_mag[1] == f_mag[1] && \
				last_f_mag[2] == f_mag[2]
			)
		{
			last_f_mag_err_sum++;
			
			if(last_f_mag_err_sum >= SELFCHECK_IMU_MONITOR_DATA_ERR_SUM_MAX){
				ob_selfcheck.selfcheck_result |= (1 << SELFCHECK_RESULT_ERR_FRONT_MAG_EXCP_DATA);
			}
				
		}else{
			last_f_mag_err_sum = 0;
		}
		
		last_f_mag[0] = f_mag[0];
		last_f_mag[1] = f_mag[1];
		last_f_mag[2] = f_mag[2];
	}
	/*后脚地磁计*/
	if(b_mag != NULL)
	{
		if(
				last_b_mag[0] == b_mag[0] && \
				last_b_mag[1] == b_mag[1] && \
				last_b_mag[2] == b_mag[2]
			)
		{
			last_b_mag_err_sum++;
			
			if(last_b_mag_err_sum >= (SELFCHECK_IMU_MONITOR_DATA_ERR_SUM_MAX/2)){
				ob_selfcheck.selfcheck_result |= (1 << SELFCHECK_RESULT_ERR_BACK_EXCP_DATA);
			}
				
		}else{
			last_b_mag_err_sum = 0;
		}
		
		last_b_mag[0] = b_mag[0];
		last_b_mag[1] = b_mag[1];
		last_b_mag[2] = b_mag[2];
	}
}
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);
		}
		monitor_sensor_data(ob_selfcheck.f_data.acc, ob_selfcheck.f_data.gry, ob_selfcheck.f_data.mag, NULL);
		Mahony_update(&Self_Front_Mahony,0,0,0,ob_selfcheck.f_data.acc[0],ob_selfcheck.f_data.acc[1],ob_selfcheck.f_data.acc[2],0,0,0);
	}
	
	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);
		}
		monitor_sensor_data(NULL, NULL, NULL, ob_selfcheck.b_data.mag);
	}
	
	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 scan_report_cb(ble_gap_evt_adv_report_t const * p_adv_report)
{
	ob_selfcheck.max_rssi = (ob_selfcheck.max_rssi > p_adv_report->rssi)?ob_selfcheck.max_rssi:p_adv_report->rssi;
}

static void selfcheck_get_result_led(void);

static void selfcheck_result_display_process(void)
{
	static uint32_t led_display_count = 0;
	
	//根据自检结果显示结果：
	//前脚传感器——红色（前脚六轴配置问题闪烁1下，前脚地磁配置问题闪烁2下，前脚六轴数据读取失败或数据异常闪烁3下，前脚地磁数据读取失败或数据异常闪烁4下）
	//后脚传感器——红色（后脚地磁配置问题闪烁5下，后脚地磁数据读取失败或数据异常闪烁6下）
	//中间传感器——红色（中间加速度配置问题闪烁7下，加速度roll值不在范围内闪烁8下）
	//充电芯片和电池分压电阻和蓝牙天线rssi——红色（充电芯片问题闪烁9下，电池分压电阻闪烁10下，蓝牙天线rssi问题闪烁11下）
	//中间加速度检测震动电机（待定）
	//上述检测通过，蓝色（1秒周期，500ms亮，500ms灭（断电源线）），若检测到鞋垫，则绿色（1秒周期，500ms亮，500ms灭（断电源线））。 
	//LED电源引脚亮灯拉高，灭灯拉低。（4秒周期，40ms亮，160ms灭（断电源线）一组）
	
	//喂狗
	feed_watchdog();
	
	//天线如果没有检测到可以持续检测
	if(ob_selfcheck.selfcheck_result_flash_num == SELFCHECK_RESULT_ERR_RSSI && (ob_selfcheck.selfcheck_result & (1 << SELFCHECK_RESULT_ERR_RSSI)) == 0)
	{
		//检测震动电机是否通过
		if((ob_selfcheck.selfcheck_result & (1 << SELFCHECK_RESULT_ERR_MT)) == 0)
		{
			//重新根据自检结果设置灯
			ob_selfcheck.selfcheck_result_led_color = COLOR_GREEN;
			ob_selfcheck.selfcheck_result_flash_num = 1;
			led_display_count = 0;
			WS2812_DisplayDot(COLOR_BLACK);WS2812_Pwm_Play();
			return;
		}
		else
		{
			//重新根据自检结果设置灯
			ob_selfcheck.selfcheck_result_led_color = COLOR_RED;
			ob_selfcheck.selfcheck_result_flash_num = SELFCHECK_RESULT_ERR_MT;
			led_display_count = 0;
			WS2812_DisplayDot(COLOR_BLACK);WS2812_Pwm_Play();
			return;
		}
	}	
	
	Process_UpdatePeroid(selfcheck_result_display_process,SELFCHECK_LED_DISPLAY_CYCLE_THRESHOLD);
	
	led_display_count++;
	
	if(led_display_count % 2 == 0)
	{
		WS2812_DisplayDot(COLOR_BLACK);WS2812_Pwm_Play();
	}
	else
	{
		WS2812_DisplayDot(ob_selfcheck.selfcheck_result_led_color);WS2812_Pwm_Play();
	}
	if(led_display_count >= ob_selfcheck.selfcheck_result_flash_num * 2)
	{
		led_display_count = 0;
		WS2812_DisplayDot(COLOR_BLACK);WS2812_Pwm_Play();
		if(ob_selfcheck.selfcheck_result_led_color != COLOR_GREEN)Process_UpdatePeroid(selfcheck_result_display_process,SELFCHECK_LED_DISPLAY_CYCLE_THRESHOLD * 3);
	}
}

static void selfcheck_led_process(void)
{
	//喂狗
	feed_watchdog();	
	
	static uint8_t flow = 0;
	
	switch(flow)
	{
		case 0:
			WS2812_DisplayDot(COLOR_RED);WS2812_Pwm_Play();
			flow = 1;
			break;
		case 1:
			WS2812_DisplayDot(COLOR_GREEN);WS2812_Pwm_Play();
			flow = 2;
			break;
		case 2:
			WS2812_DisplayDot(COLOR_BLUE);WS2812_Pwm_Play();
			flow = 0;
			break;
	}
}

static void selfcheck_mt_process(void)
{
	static int16_t max_acc_z	=	-32767;
	static int16_t min_acc_z	=	32767;
	
	//喂狗
	feed_watchdog();	
	
	//检测震动是否达标
	//读取中间传感器数据，计算加速度roll值
	if(ob_selfcheck.order == 0x02 || ob_selfcheck.order == 0x03)
	{
		memset(&ob_selfcheck.m_data,0,sizeof(ob_selfcheck.m_data));
		if(drv_qma_get_acc_data(&ob_selfcheck.m_data) == 0)
		{
			if(ob_selfcheck.m_data.acc[0]==0 && ob_selfcheck.m_data.acc[1]==0 && ob_selfcheck.m_data.acc[2]==0)return;
			if(ob_selfcheck.m_data.acc[0]==-1 && ob_selfcheck.m_data.acc[1]==-1 && ob_selfcheck.m_data.acc[2]==-1)return;
			
			max_acc_z = (max_acc_z > ob_selfcheck.m_data.acc[2])?max_acc_z:ob_selfcheck.m_data.acc[2];
			min_acc_z = (min_acc_z > ob_selfcheck.m_data.acc[2])?ob_selfcheck.m_data.acc[2]:min_acc_z;
			if(max_acc_z > min_acc_z)
			{
				if(ob_selfcheck.order == 0x02)
				{
					if((max_acc_z - min_acc_z) < SELFCHECK_MIDDLE_ACC_CHECK_MT_MIN_THRESHOLD_2)
					{
						ob_selfcheck.selfcheck_result |= (1 << SELFCHECK_RESULT_ERR_MT);
					}
					else 
					{
						ob_selfcheck.selfcheck_result &= ~(1 << SELFCHECK_RESULT_ERR_MT);
					}
				}
				
				if(ob_selfcheck.order == 0x03)
				{
					if((max_acc_z - min_acc_z) < SELFCHECK_MIDDLE_ACC_CHECK_MT_MIN_THRESHOLD_3)
					{
						ob_selfcheck.selfcheck_result |= (1 << SELFCHECK_RESULT_ERR_MT);
					}
					else 
					{
						ob_selfcheck.selfcheck_result &= ~(1 << SELFCHECK_RESULT_ERR_MT);
					}
				}
			}
			else
			{
				ob_selfcheck.selfcheck_result |= (1 << SELFCHECK_RESULT_ERR_MT);
			}
//			SEGGER_RTT_printf(0,"max_acc_z-min_acc_z:%d\r\n",max_acc_z-min_acc_z);
		}
	}
}

static void selfcheck_continue_mt_process(void)
{
	static uint8_t flow = 0;
	
	static uint32_t	shake_ms = 0;
	static uint32_t	shake2_ms = 0;
	
	if((ob_selfcheck.selfcheck_result & (1 << SELFCHECK_RESULT_ERR_MT)) == 0)
	{
		shake_ms = 300;
		shake2_ms = 700;
	}
	else
	{
		shake_ms = 1000;
		shake2_ms = 1000;
	}
	
	//喂狗
	feed_watchdog();	
	
	switch(flow)
	{
		case 0:
			nrf_gpio_pin_write(PIN_MT_EN,1);
			flow = 1;
			Process_SetHoldOn(selfcheck_continue_mt_process,1);
			Process_UpdatePeroid(selfcheck_continue_mt_process,shake_ms);
			break;
		case 1:
			nrf_gpio_pin_write(PIN_MT_EN,0);
			flow = 0;
			Process_SetHoldOn(selfcheck_continue_mt_process,0);
			Process_UpdatePeroid(selfcheck_continue_mt_process,shake2_ms);
			break;
	}
}

#define	N	10			//N > 3
int data[N];
int middleFilter(int in_data)
{
	int sum = 0;
	int temp[N];
	int change;
	int i,j;
	//记录数据
	for(i=0; i<(N-1); i++)
	{
		data[i]=data[i+1];
	}
	data[N-1] = in_data;
	//复制数据
	for(i=0; i<N; i++)
		temp[i] = data[i];
	//冒泡法排序
	for(i=1; i<N; i++)
		for(j=0; j<N-i; j++)
		{
			if(temp[i] > temp[i+1])
			{
				change = temp[i];
				temp[i] = temp[i+1];
				temp[i+1] = change;
			}
		}
	//求和
	for(i=1; i<(N-1); i++)
		sum = sum + temp[i];
	//返回平均值
	return(sum/((N-2)));
 
}

static void selfcheck_get_result_led(void)
{
	
	//根据自检结果设置灯
	if(ob_selfcheck.selfcheck_result == 0)
	{
		ob_selfcheck.selfcheck_result_led_color = COLOR_GREEN;
		ob_selfcheck.selfcheck_result_flash_num = 1;
	}
	else
	{
		ob_selfcheck.selfcheck_result_led_color = COLOR_RED;
		for(int i=0;i<sizeof(ob_selfcheck.selfcheck_result)*8;i++)
		{
			if((ob_selfcheck.selfcheck_result & (1 << i)) != 0)
			{
				ob_selfcheck.selfcheck_result_flash_num = i;
				break;
			}
		}	
	}
}

static void selfcheck_process(void)
{
	BLL_IMU_CONFIG_RESULT  	f_config_result,b_config_result;
	uint16_t 								front_mag_norm;
	uint16_t 								back_mag_norm;
	int16_t 								roll;
	uint8_t 								buf[256];
	uint8_t 								L=0;
	char 										mac_buf[16];	
	static uint32_t 				t_count = 0;
	static uint32_t					battery_adc_t_count = 0;
	static uint32_t					roll_t_count = 0;
	static uint32_t         sensor_t_count = 0;
	static uint32_t					last_tim = 0;
	static uint32_t					continue_trigger = 0;

	t_count++;
	
	//喂狗
	feed_watchdog();
	
	//读取中间传感器数据，计算加速度roll值
	memset(&ob_selfcheck.m_data,0,sizeof(ob_selfcheck.m_data));
	if(drv_qma_get_acc_data(&ob_selfcheck.m_data) != 0)
	{
		ob_selfcheck.selfcheck_result |= (1 << SELFCHECK_RESULT_ERR_MIDDLE_NO_DATA_OR_EXCP_DATA);
		//重新配置
		drv_qma_set_acc_odr(QMA_ACC_ODR_104HZ);		
	}
	
	Mahony_update(&Self_Mind_Mahony,0,0,0,ob_selfcheck.m_data.acc[0],ob_selfcheck.m_data.acc[1],ob_selfcheck.m_data.acc[2],0,0,0);
	
	//第3秒检测加速度roll值，设置自检结果。
	if(t_count - roll_t_count == (3000/100))
	{
		roll  = (int16_t)(Self_Mind_Mahony.roll*100);
		if(roll < 0)roll *= -1;
		roll = (roll > 0 && roll < 100)?1:roll/100;
		if(!( \
		(SELFCHECK_MIDDLE_ACC_PROS_ROLL_MIN_THRESHOLD < roll && roll < SELFCHECK_MIDDLE_ACC_PROS_ROLL_MAX_THRESHOLD) || \
		(SELFCHECK_MIDDLE_ACC_CONS_ROLL_MIN_THRESHOLD < roll && roll < SELFCHECK_MIDDLE_ACC_CONS_ROLL_MAX_THRESHOLD) \
		))ob_selfcheck.selfcheck_result |= (1 << SELFCHECK_RESULT_ERR_MIDDLE_NO_DATA_OR_EXCP_DATA);
		else
			ob_selfcheck.selfcheck_result &= ~(1 << SELFCHECK_RESULT_ERR_MIDDLE_NO_DATA_OR_EXCP_DATA);
		//更新计时
		roll_t_count = t_count;
		
//		SEGGER_RTT_printf(0,"middle roll:%d\r\n",roll);
	}

	//每1秒查看前后传感器是否配置且读取成功（至少要1秒才有结果）且值本身是否正常（如地磁没有焊接电容），设置自检结果。
	if(t_count - sensor_t_count >= (1000/100))
	{
		//前脚传感器判断
		f_config_result = bll_imu_query_config_param_is_ready(BLL_IMU_DIR_FRONT,&game_bll_imu_param_t);
		if(f_config_result != BLL_IMU_CONFIG_FINISH)
		{
			if(f_config_result == BLL_IMU_CONFIG_FAIL_FRONT_SIX_AXIS_GET_ID)ob_selfcheck.selfcheck_result |= (1 << SELFCHECK_RESULT_ERR_FRONT_SENSOR_CONFIG_SIX_AXIS_ID);
			if(f_config_result == BLL_IMU_CONFIG_FAIL_FRONT_MAG_GET_ID)ob_selfcheck.selfcheck_result |= (1 << SELFCHECK_RESULT_ERR_FRONT_SENSOR_CONFIG_MAG_ID);
			if(f_config_result == BLL_IMU_CONFIG_FAIL_FRONT_MAG)ob_selfcheck.selfcheck_result |= (1 << SELFCHECK_RESULT_ERR_FRONT_SENSOR_CONFIG_MAG);
			if(f_config_result == BLL_IMU_CONFIG_FAIL_FRONT_SIX_AXIS)ob_selfcheck.selfcheck_result |= (1 << SELFCHECK_RESULT_ERR_FRONT_SENSOR_CONFIG_SIX_AXIS);
		}
		else
		{
			//配置没问题
			ob_selfcheck.selfcheck_result &= ~(1 << SELFCHECK_RESULT_ERR_FRONT_SENSOR_CONFIG_MAG);
			ob_selfcheck.selfcheck_result &= ~(1 << SELFCHECK_RESULT_ERR_FRONT_SENSOR_CONFIG_SIX_AXIS);
			
			if(ob_selfcheck.f_is_read_data != true)ob_selfcheck.selfcheck_result |= (1 << SELFCHECK_RESULT_ERR_FRONT_SIX_AXIS_NO_DATA);
			else
			{
				//数据没问题
				ob_selfcheck.selfcheck_result &= ~(1 << SELFCHECK_RESULT_ERR_FRONT_SIX_AXIS_NO_DATA);
				ob_selfcheck.f_is_read_data = false;
				
				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]));
					
				//前脚传感器地磁没焊接电容
				if(front_mag_norm < SELFCHECK_SENSOR_MAG_NO_WELDING_CAPACITOR_MIN_THRESHOLD)ob_selfcheck.selfcheck_result |= (1 << SELFCHECK_RESULT_ERR_FRONT_MAG_DATA_TO_SMALL);
				else ob_selfcheck.selfcheck_result &= ~(1 << SELFCHECK_RESULT_ERR_FRONT_MAG_DATA_TO_SMALL);
			}
		}
		
		
		//后脚传感器判断
		b_config_result = bll_imu_query_config_param_is_ready(BLL_IMU_DIR_BACK,&game_bll_imu_param_t);
		if(b_config_result != BLL_IMU_CONFIG_FINISH)
		{
			if(b_config_result == BLL_IMU_CONFIG_FAIL_BACK_MAG_GET_ID)ob_selfcheck.selfcheck_result |= (1 << SELFCHECK_RESULT_ERR_BACK_SENSOR_CONFIG_ID);
			else ob_selfcheck.selfcheck_result |= (1 << SELFCHECK_RESULT_ERR_BACK_SENSOR_CONFIG);
		}
		else 
		{
			//配置没问题
			ob_selfcheck.selfcheck_result &= ~(1 << SELFCHECK_RESULT_ERR_BACK_SENSOR_CONFIG);
			
			if(ob_selfcheck.b_is_read_data != true)ob_selfcheck.selfcheck_result |= (1 << SELFCHECK_RESULT_ERR_BACK_NO_DATA);
			else
			{
				//数据没问题
				ob_selfcheck.selfcheck_result &= ~(1 << SELFCHECK_RESULT_ERR_BACK_NO_DATA);
				ob_selfcheck.b_is_read_data = false;
				
				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]));
					
				//后脚传感器地磁没焊接电容
				if(back_mag_norm < SELFCHECK_SENSOR_MAG_NO_WELDING_CAPACITOR_MIN_THRESHOLD)ob_selfcheck.selfcheck_result |= (1 << SELFCHECK_RESULT_ERR_BACK_DATA_TO_SMALL);
				else ob_selfcheck.selfcheck_result &= ~(1 << SELFCHECK_RESULT_ERR_BACK_DATA_TO_SMALL);
			}
			
		}

		//任意传感器配置失败，重新配置
		if(f_config_result != BLL_IMU_CONFIG_FINISH || b_config_result != BLL_IMU_CONFIG_FINISH || ob_selfcheck.b_is_read_data != true || ob_selfcheck.f_is_read_data != true)
		{
			bll_imu_Resume_config_param(&game_bll_imu_param_t);
		}

		//更新计时-
		sensor_t_count = t_count;
	}
	
	//第3秒获取前脚是否装反
	if(t_count == (3000/100) && ob_selfcheck.order == 0x03)
	{
		roll  = (int16_t)(Self_Front_Mahony.roll*100);
		if(roll < 0)roll *= -1;
		roll = (roll > 0 && roll < 100)?1:roll/100;
		
		if(!(roll >=SELFCHECK_MIDDLE_ACC_CONS_ROLL_MIN_THRESHOLD && roll <= SELFCHECK_MIDDLE_ACC_CONS_ROLL_MAX_THRESHOLD))
		{
			ob_selfcheck.selfcheck_result |= (1 << SELFCHECK_RESULT_ERR_FRONT_IMU_REVERSE);
		}
		
//		SEGGER_RTT_printf(0,"front roll:%d\r\n",roll);
	}
	
	//第3秒开启震动
	if(t_count == (3000/100))
	{
		nrf_gpio_pin_write(PIN_MT_EN,1);
		//初始化震动电机自检线程
		Process_Start(30,"selfcheck_mt_process",selfcheck_mt_process);	
		Process_SetHoldOn(selfcheck_mt_process,1);
	}

	//查看能否读取到任意广播名字，且最小的RSSI是否满足条件，设置自检结果。
	
	if(ob_selfcheck.order == 0x03)
	{
		if(SELFCHECK_SCAN_DEVICE_RSSI_MIN_THRESHOLD_3 >= ob_selfcheck.max_rssi)ob_selfcheck.selfcheck_result |= (1 << SELFCHECK_RESULT_ERR_RSSI);
		else ob_selfcheck.selfcheck_result &= ~(1 << SELFCHECK_RESULT_ERR_RSSI);
		
//		SEGGER_RTT_printf(0,"SELFCHECK_SCAN_DEVICE_RSSI_MIN_THRESHOLD\r\n");
	}
	else
	{
		if(SELFCHECK_SCAN_DEVICE_RSSI_MIN_THRESHOLD_2 >= ob_selfcheck.max_rssi)ob_selfcheck.selfcheck_result |= (1 << SELFCHECK_RESULT_ERR_RSSI);
		else ob_selfcheck.selfcheck_result &= ~(1 << SELFCHECK_RESULT_ERR_RSSI);
		
//		SEGGER_RTT_printf(0,"SELFCHECK_SCAN_DEVICE_RSSI_MIN_THRESHOLD_2\r\n");
	}
	

	
	//每4秒读取ADC，以5V电压测试电池分压电阻是否焊接，不考虑阻值，设置自检结果。
	if(t_count - battery_adc_t_count >= (4000/100))
	{
		if(!(SELFCHECK_BATTERY_PIN_ADC_MIN_THRESHOLD < ob_selfcheck.battery_adc_max && ob_selfcheck.battery_adc_max < SELFCHECK_BATTERY_PIN_ADC_MAX_THRESHOLD))ob_selfcheck.selfcheck_result |= (1 << SELFCHECK_RESULT_ERR_BATTERY_PIN_ADC);
		else ob_selfcheck.selfcheck_result &= ~(1 << SELFCHECK_RESULT_ERR_BATTERY_PIN_ADC);
		//更新计时
		battery_adc_t_count = t_count;
		
//		SEGGER_RTT_printf(0,"ob_selfcheck.battery_adc_max:%d\r\n",ob_selfcheck.battery_adc_max);
//		SEGGER_RTT_printf(0,"ob_selfcheck.charge_chip_adc_max:%d\r\n",ob_selfcheck.charge_chip_adc_max);
	}
	

	//第4秒关闭100毫秒震动电机自检线程
	if(t_count == (4000/100))
	{
		Process_Stop(selfcheck_mt_process);
		Process_Stop(selfcheck_led_process);
		WS2812_DisplayDot(COLOR_BLACK);WS2812_Pwm_Play();
		Process_Start(100,"selfcheck_continue_mt_process",selfcheck_continue_mt_process);
		Process_SetHoldOn(selfcheck_continue_mt_process,1);
	}
	

	//第4+SELFCHECK_LED_DISPLAY_CYCLE_THRESHOLD秒初始化自检显示结果线程，设置holdon，全功率运行。
	if(t_count == ((4000 + SELFCHECK_LED_DISPLAY_CYCLE_THRESHOLD)/100))
	{
		//根据自检结果设置灯
		selfcheck_get_result_led();
		Process_Start(SELFCHECK_LED_DISPLAY_CYCLE_THRESHOLD,"selfcheck_result_display_process",selfcheck_result_display_process);
		Process_SetHoldOn(selfcheck_result_display_process,1);
	}
	

	//上报中间传感器的加速度值和roll值
	L=0;
	int16_t rol  = (int16_t)(Mahony_M_GetRoll()*100);
	int16_t pitch = (int16_t)(Mahony_M_GetPitch()*100);
	int16_t yaw   = (int16_t)(Mahony_M_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[2];
	int32_t middle_acc_y = ob_selfcheck.m_data.acc[1];
	int16_t middle_acc_z = ob_selfcheck.m_data.acc[0];
	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值
	if(TIME_GetTicks()-last_tim >= 1000)
	{
		last_tim = TIME_GetTicks();
		
		if(ob_selfcheck.max_rssi == -120)
		{
			nrf_ble_scan_stop();
			host_disconnect();
			if(host_isconnect() == 0)
			{
				host_set_scan_name((char *)"***********",sizeof("***********"));
				advdata_report_Evt_Regist(scan_report_cb);
				Continuous_scan_start();
			}
		}
		ob_selfcheck.max_rssi = -120;
	}
	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);
	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]));
	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);
	
	
	//退出开关
	uint32_t ch = drv_GetChargeTrig();
	if(ch > 0)
	{
		if(continue_trigger <= 10)continue_trigger = 10;
	}
	else
	{
		if(continue_trigger != 0)continue_trigger--;
	}
	
//	SEGGER_RTT_printf(0,"ch:%d\r\n",ch);
	

//	SEGGER_RTT_printf(0,"continue_trigger:%d,%d,%d\r\n",continue_trigger,adc_value,ch);
//	SEGGER_RTT_printf(0,"front mag:%d,%d,%d,%d\r\n",ob_selfcheck.f_data.mag[0],ob_selfcheck.f_data.mag[1],ob_selfcheck.f_data.mag[2],front_mag_norm);
//	SEGGER_RTT_printf(0,"back mag:%d,%d,%d,%d\r\n",ob_selfcheck.b_data.mag[0],ob_selfcheck.b_data.mag[1],ob_selfcheck.b_data.mag[2],back_mag_norm);
					
	//第6秒重启
//	if(t_count >= (5000/100) && !slave_isconnect() && continue_trigger==0)
	if(t_count >= (6000/100) && continue_trigger==0)
	{
    memset(mac_buf, 0, sizeof(mac_buf));
    sprintf(mac_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]);
		ST_scan_stop();
    host_set_scan_name(mac_buf, strlen(mac_buf));
		
		Flash_DeleteAllInfor();
		Flash_DeleteAllStep();
		Flash_DeleteAllBackup();
		
		//恢复出厂信息
		memset(&mFlash,0xFF,sizeof(mFlash));
		memset(&mBackup,0xFF,sizeof(mBackup));
		
		//保存数据到flash
		if(Flash_SaveStep() != ZONE_OP_SUCCESS)Except_TxError(EXCEPT_Power,"save step fail");
		extern battercb_t battery_record;
//		extern void printbatter_cb(battercb_t *c,unsigned char *buff);
		memcpy(&mFlash.mbattercb_t,&battery_record,sizeof(battercb_t));
		mBackup.RestartCnt =0;
		if(Flash_SaveBackup() != ZONE_OP_SUCCESS)Except_TxError(EXCEPT_Power,"pwr save backup fail");
		if(Flash_SaveInfomation() != ZONE_OP_SUCCESS)Except_TxError(EXCEPT_Power,"pwr save information fail");
		NVIC_SystemReset();
	}
}

static void adc_callback(uint32_t sample_point, Fml_Adc_All_Channel_Adc_Value_t all_adc_value)
{
	int16_t battery_adc_value = 0;
	int16_t charge_adc_value = 0;
	
	//筛选最大的电池分压后的电压
	fml_adc_get_value(PIN_ADC_BAT_CHANNEL,&battery_adc_value);
	battery_adc_value = ADC_RESULT_IN_MILLI_VOLTS(battery_adc_value) * 5 / 3;
	ob_selfcheck.battery_adc_max = middleFilter(battery_adc_value);
	
	
	
	//充电芯片检测
	fml_adc_get_value(PIN_ADC_CHARGMEASURE_CHANNEL,&charge_adc_value);
	//充电电流超过500ma就异常
	if(charge_adc_value >= 1560)
	{
		ob_selfcheck.selfcheck_result |= (1 << SELFCHECK_RESULT_ERR_CHARGE_CHIP_PIN_ADC);
	}
	else		//充电电流正常的情况
	{
		//电压大于4V，不检测充电芯片，因为充电芯片断闸。
		if(battery_adc_value >= 4000)
		{
			ob_selfcheck.charge_chip_adc_max = SELFCHECK_CHARGE_CHIP_PIN_ADC_COUNT;
		}
		
		if(SELFCHECK_CHARGE_CHIP_PIN_ADC_MIN_THRESHOLD < charge_adc_value)ob_selfcheck.charge_chip_adc_max++;

		if(ob_selfcheck.charge_chip_adc_max >= SELFCHECK_CHARGE_CHIP_PIN_ADC_COUNT)ob_selfcheck.selfcheck_result &= ~(1 << SELFCHECK_RESULT_ERR_CHARGE_CHIP_PIN_ADC);
		else ob_selfcheck.selfcheck_result |= (1 << SELFCHECK_RESULT_ERR_CHARGE_CHIP_PIN_ADC);
	}
	
//	SEGGER_RTT_printf(0,"charge_chip_adc_max:%d,%d\r\n",ob_selfcheck.charge_chip_adc_max,charge_adc_value);
}

static void selfcheck_trigger_Init_Porcess(void)
{
	if(ob_selfcheck.order != 0)
	{
		selfcheck_trigger_callback(ob_selfcheck.order);				//会关掉所有线程
	}
}

/*API ------------------------------------------------------------------------------------------------------------------------------------*/
/**
	@brief 		自检触发初始化
	@param 		无
	@return 	无
*/
void selfcheck_trigger_Init(void)
{
	memset(&ob_selfcheck, 0, sizeof(ob_selfcheck.order));
	Process_Start(0,"selfcheck_trigger_Init_Porcess",selfcheck_trigger_Init_Porcess);
}

/**
	@brief 		设置自检触发指令
	@param 		order							-	[in]	指令类型
	@return 	无
*/
void selfcheck_trigger_set_order(char order)
{
//	SEGGER_RTT_printf(0,"order:0x%x,0x%x\r\n",order,ob_selfcheck.order);
	if(ob_selfcheck.order == 0)ob_selfcheck.order	=	order;
}


/**
	@brief 		自检触发回调
	@param 		order							-	[in] 回调触发的指令
	@return 	无
*/
void selfcheck_trigger_callback(char order)
{
	int 							error;
	uint32_t					last_tim = TIME_GetTicks();
	
	//喂狗
	feed_watchdog();
	
	//初始化结构体，自检结果为成功
	memset(&ob_selfcheck,0,sizeof(ob_selfcheck));
	ob_selfcheck.max_rssi 											= -120;
	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;
	ob_selfcheck.order													=	order;
	
//	SEGGER_RTT_printf(0,"order:0x%x\r\n",order);
	
	//关闭所有线程，初始化0毫秒自检线程，设置holdon，全功率运行。
	Process_All_Stop();
	Process_Start(100,"selfcheck_process",selfcheck_process);	
	Process_SetHoldOn(selfcheck_process,1);
	
	//关闭前后脚传感器电源
	drv_qmc6310_power_off();
	drv_lsm_power_off();
	nrf_delay_ms(200);
	feed_watchdog();


	//关闭扫描，设置任意设备扫描回调，开启扫描
	while(TIME_GetTicks() - last_tim <= 1000)
	{
		nrf_ble_scan_stop();
		host_disconnect();
		if(host_isconnect() == 0)break;
	}
	host_set_scan_name((char *)"***********",sizeof("***********"));
	advdata_report_Evt_Regist(scan_report_cb);
	Continuous_scan_start();

	//配置LED电源引脚为输出SOD1
	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);

	//配置震动电机引脚为输出SOS1
	nrf_gpio_cfg_output(PIN_MT_EN);
	nrf_gpio_pin_write(PIN_MT_EN,0);	
	
	//配置led灯电源引脚为输出
	nrf_gpio_pin_write(PIN_LED_ENABLE,LED_ENABLE);

	//重新初始化ADC，配置所有通道。
	ADC_SetPinChannel(PIN_ADC_CHARGMEASURE, PIN_ADC_CHARGMEASURE_CHANNEL,NRF_GPIO_PIN_NOPULL);
	ADC_SetPinChannel(PIN_ADC_BAT_IN, PIN_ADC_BAT_CHANNEL,NRF_GPIO_PIN_NOPULL);
	fml_adc_sample_update_notify_register(adc_callback);
	
	//插电IO口初始化
	nrf_gpio_cfg_input(PIN_CHARGING, NRF_GPIO_PIN_NOPULL);

	//配置前脚传感器、中间传感器、后脚传感器
	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);
	
	//前脚地磁自检
	if(drv_lsm_selfcheck_mag() != 0)
	{
		ob_selfcheck.selfcheck_result |= (1 << SELFCHECK_RESULT_ERR_FRONT_MAG_DATA_TO_SMALL);
//		SEGGER_RTT_printf(0,"drv_lsm_selfcheck_mag() != 0!!!!!!!!!!!!!!\r\n");
	}
	//后脚地磁自检
	if(drv_qmc6310_selfcheck_mag() != 0)
	{
		ob_selfcheck.selfcheck_result |= (1 << SELFCHECK_RESULT_ERR_BACK_DATA_TO_SMALL);
//		SEGGER_RTT_printf(0,"drv_qmc6310_selfcheck_mag() != 0!!!!!!!!!!!!!!\r\n");
	}
	
	//初始化LED自检线程
	Process_Start(300,"selfcheck_led_process",selfcheck_led_process);	
	Process_SetHoldOn(selfcheck_led_process,1);
	

	//初始化计算roll值算法
	Mahony_Init(&Self_Mind_Mahony,104);
	
	Mahony_Init(&Self_Front_Mahony,416);
	
	//喂狗
	feed_watchdog();	
	
}