/*Includes ----------------------------------------------*/
#include "tool.h"
#include "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_qma7981.h"
#include "hal_led.h"
#include "hal_battery.h"
#include "bll_imu.h"
#include "app_flash.h"
#include "system.h"

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

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

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

#define	SELFCHECK_BATTERY_PIN_ADC_MIN_THRESHOLD						                     3300																								//电池分压电阻的最小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_WEAR_INSOLE_MAG_NORM_MIN_THRESHOLD													 5000																								//穿鞋垫的地磁norm值最小阈值

#define	SELFCHECK_SENSOR_MAG_SHAKE_TRIGGER_THRESHOLD													 18000																							//地磁抖动触发检测

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

#define	SELFCHECK_SENSOR_MAG_SHAKE_THRESHOLD																	 200																								//地磁抖动阈值


/*STRUCTION ------------------------------------------------------------------------------------------------------------------------------------*/
typedef enum {
	SELFCHECK_RESULT_SUCCESS = 0,																																																		//自检成功
	
	SELFCHECK_RESULT_ERR_FRONT_SENSOR_CONFIG_SIX_AXIS,																																							//自检失败——前脚传感器配置六轴
	
	SELFCHECK_RESULT_ERR_FRONT_SENSOR_CONFIG_MAG,																																										//自检失败——前脚传感器配置地磁
	
	SELFCHECK_RESULT_ERR_FRONT_SIX_AXIS_NO_DATA_OR_EXCP_DATA,																																				//自检失败——前脚传感器六轴没数据或数据异常
	
	SELFCHECK_RESULT_ERR_FRONT_MAG_NO_DATA_OR_EXCP_DATA,																																						//自检失败——前脚传感器地磁没数据或数据异常
	
	SELFCHECK_RESULT_ERR_BACK_SENSOR_CONFIG,																																												//自检失败——后脚传感器配置
	
	SELFCHECK_RESULT_ERR_BACK_NO_DATA_OR_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_e;

typedef struct _selfcheck
{
	uint32_t																  			  selfcheck_result;																															//自检结果
	
	uint32_t     																			selfcheck_result_led_color;																										//自检结果的led灯颜色
	
	uint32_t																					selfcheck_result_flash_num;																										//自检结果闪烁次数
	
	bool																							selfcheck_is_led_display;																											//自检结果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 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);
		}
	}
	
	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);
		}
	}
	
	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(uint8_t *adv_data, uint16_t adv_data_len,int8_t rssi)
{
	ob_selfcheck.max_rssi = (ob_selfcheck.max_rssi > rssi)?ob_selfcheck.max_rssi:rssi;
}

static void selfcheck_led_display_process(void)
{
	static uint32_t cur_flash_num = 0;
	static uint8_t	level = LED_ENABLE;
	static uint8_t	red_off_led_count = 0;
	static uint32_t tim = 0;
	
	//喂狗
	feed_watchdog();

	//led display
	if(ob_selfcheck.selfcheck_is_led_display == true)
	{
		if(cur_flash_num == 0)tim = TIME_GetTicks();
		
		//判断是否结束
		if(level == LED_ENABLE)cur_flash_num++;
		if(cur_flash_num > ob_selfcheck.selfcheck_result_flash_num)
		{
			nrf_gpio_pin_write(PIN_LED_ENABLE,LED_DISABLE);
			if(ob_selfcheck.selfcheck_result_led_color == COLOR_RED && (TIME_GetTicks() - tim < 5000))return;
			cur_flash_num = 0;
			level = LED_ENABLE;
			red_off_led_count = 0;
			ob_selfcheck.selfcheck_is_led_display = false;
			Process_Stop(selfcheck_led_display_process);
			return;
		}
		
		
		if(ob_selfcheck.selfcheck_result_led_color == COLOR_RED)
		{
			Process_Start(50,"selfcheck_led_display_process",selfcheck_led_display_process);
			nrf_gpio_pin_write(PIN_LED_ENABLE,level);nrf_delay_ms(5);
			if(level == LED_ENABLE){WS2812_DisplayDot(ob_selfcheck.selfcheck_result_led_color);WS2812_Pwm_Play();}
		}
		else
		{
			Process_Start(500,"selfcheck_led_display_process",selfcheck_led_display_process);
			nrf_gpio_pin_write(PIN_LED_ENABLE,level);nrf_delay_ms(5);
			if(level == LED_ENABLE){WS2812_DisplayDot(ob_selfcheck.selfcheck_result_led_color);WS2812_Pwm_Play();}
		}
		
		//周期翻转
		if(ob_selfcheck.selfcheck_result_led_color == COLOR_RED)
		{
			if(level == LED_ENABLE){level = LED_DISABLE;red_off_led_count = 0;}
			if(level == LED_DISABLE){red_off_led_count++;if(red_off_led_count >= 6)level = LED_ENABLE;}
		}
		else
		{
			level = (level == LED_ENABLE)?LED_DISABLE:LED_ENABLE;
		}
	}
}

static void selfcheck_result_display_process(void)
{
	uint16_t 				front_mag_norm;
	
	//喂狗
	feed_watchdog();
	
	if(ob_selfcheck.selfcheck_is_led_display == false)
	{
		//根据自检结果显示结果：
		//前脚传感器——红色（前脚六轴配置问题闪烁1下，前脚地磁配置问题闪烁2下，前脚六轴数据读取失败或数据异常闪烁3下，前脚地磁数据读取失败或数据异常闪烁4下）
		//后脚传感器——红色（后脚地磁配置问题闪烁5下，后脚地磁数据读取失败或数据异常闪烁6下）
		//中间传感器——红色（中间加速度配置问题闪烁7下，加速度roll值不在范围内闪烁8下）
		//充电芯片和电池分压电阻和蓝牙天线rssi——红色（充电芯片问题闪烁9下，电池分压电阻闪烁10下，蓝牙天线rssi问题闪烁11下）
		//中间加速度检测震动电机（待定）
		//上述检测通过，蓝色（1秒周期，500ms亮，500ms灭（断电源线）），若检测到鞋垫，则绿色（1秒周期，500ms亮，500ms灭（断电源线））。 
		//LED电源引脚亮灯拉高，灭灯拉低。（4秒周期，40ms亮，160ms灭（断电源线）一组）
		
		//获取颜色+闪烁次数+周期
		if(ob_selfcheck.selfcheck_result == 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]));	
			if(SELFCHECK_WEAR_INSOLE_MAG_NORM_MIN_THRESHOLD <= front_mag_norm)
			{
				ob_selfcheck.selfcheck_result_led_color = COLOR_GREEN;
				ob_selfcheck.selfcheck_result_flash_num = 1;
			}
			else
			{
				ob_selfcheck.selfcheck_result_led_color = COLOR_BLUE;
				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;
				}
			}	
		}
		Process_Start(0,"selfcheck_led_display_process",selfcheck_led_display_process);
		ob_selfcheck.selfcheck_is_led_display = true;
	}
}

static void selfcheck_mt_process(void)
{
	//喂狗
	feed_watchdog();	
	
	nrf_gpio_pin_toggle(PIN_MT_EN);
}

static void selfcheck_process(void)
{
	BLL_IMU_CONFIG_RESULT  	f_config_result,b_config_result;
	int16_t									adc_value = 0;
	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					charge_chip_adc_max = 0;
	static int16_t					battery_adc_max = 0;
	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;
	static uint32_t					front_mag_norm_max = 0;
	static uint32_t					back_mag_norm_max = 0;
	static uint32_t					front_mag_norm_min = 0xffffffff;
	static uint32_t					back_mag_norm_min = 0xffffffff;
	static bool							front_mag_shake_trigger = false;
	static bool							back_mag_shake_trigger = false;

	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_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);
	
	
	//筛选最大的电池分压后的电压
	fml_adc_get_value(PIN_ADC_BAT_CHANNEL,&adc_value);
	adc_value = ADC_RESULT_IN_MILLI_VOLTS(adc_value) * 5 / 3;
	battery_adc_max = adc_value;
	
	//每3秒读取ADC，以5V电压测试电池分压电阻是否焊接，不考虑阻值，设置自检结果。
	if(t_count - battery_adc_t_count > (3000/100))
	{
		if(!(SELFCHECK_BATTERY_PIN_ADC_MIN_THRESHOLD < battery_adc_max && 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;
	}
	
	//充电芯片检测
	fml_adc_get_value(PIN_ADC_CHARGMEASURE_CHANNEL,&adc_value);
	if(SELFCHECK_CHARGE_CHIP_PIN_ADC_MIN_THRESHOLD < adc_value)
	{
		charge_chip_adc_max++;
	}
	if(charge_chip_adc_max > 10)
	{
		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);
	}
	
	

	//每3秒检测加速度roll值，设置自检结果。
	if(t_count - roll_t_count > (3000/100))
	{
		roll  = (int16_t)(getRoll()*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;
	}

	//每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_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_OR_EXCP_DATA);
			else
			{
				//数据没问题
				ob_selfcheck.selfcheck_result &= ~(1 << SELFCHECK_RESULT_ERR_FRONT_SIX_AXIS_NO_DATA_OR_EXCP_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_NO_DATA_OR_EXCP_DATA);
				else ob_selfcheck.selfcheck_result &= ~(1 << SELFCHECK_RESULT_ERR_FRONT_MAG_NO_DATA_OR_EXCP_DATA);
			}
		}
		
		
		//后脚传感器判断
		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)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_OR_EXCP_DATA);
			else
			{
				//数据没问题
				ob_selfcheck.selfcheck_result &= ~(1 << SELFCHECK_RESULT_ERR_BACK_NO_DATA_OR_EXCP_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_NO_DATA_OR_EXCP_DATA);
				else ob_selfcheck.selfcheck_result &= ~(1 << SELFCHECK_RESULT_ERR_BACK_NO_DATA_OR_EXCP_DATA);
			}
			
		}

		//任意传感器配置失败，重新配置
		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;
	}
	
	//fornt back mag data check shake
	if(t_count > (2000/100))
	{
		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]));
		
		if(front_mag_norm >= SELFCHECK_SENSOR_MAG_SHAKE_TRIGGER_THRESHOLD)
		{
			//get max
			front_mag_norm_max = (front_mag_norm_max > front_mag_norm)?front_mag_norm_max:front_mag_norm;
			//get min
			front_mag_norm_min = (front_mag_norm_min > front_mag_norm)?front_mag_norm:front_mag_norm_min;
			
			front_mag_shake_trigger = true;
		}
		
		if(back_mag_norm >= SELFCHECK_SENSOR_MAG_SHAKE_TRIGGER_THRESHOLD)
		{
			//get max
			back_mag_norm_max = (back_mag_norm_max > back_mag_norm)?back_mag_norm_max:back_mag_norm;
			//get min
			back_mag_norm_min = (back_mag_norm_min > back_mag_norm)?back_mag_norm:back_mag_norm_min;
			
			back_mag_shake_trigger = true;
		}

		
		DEBUG_LOG("front_mag_norm_max:%d,%d,%d,%d,%d,%d,%d,%d,%d,%d\r\n",front_mag_norm,back_mag_norm,front_mag_norm_max,back_mag_norm_max, \
		                                                                front_mag_norm_min,back_mag_norm_min,front_mag_norm_max - front_mag_norm_min, \
		                                                                back_mag_norm_max - back_mag_norm_min,front_mag_shake_trigger,back_mag_shake_trigger);
		
		if(t_count >= (5000/100))
		{
			if(front_mag_norm_max - front_mag_norm_min >= SELFCHECK_SENSOR_MAG_SHAKE_THRESHOLD && front_mag_shake_trigger)
			{
				ob_selfcheck.selfcheck_result |= (1 << SELFCHECK_RESULT_ERR_FRONT_MAG_NO_DATA_OR_EXCP_DATA);
			}
			
			if(back_mag_norm_max - back_mag_norm_min >= SELFCHECK_SENSOR_MAG_SHAKE_THRESHOLD && back_mag_shake_trigger)
			{
				ob_selfcheck.selfcheck_result |= (1 << SELFCHECK_RESULT_ERR_BACK_NO_DATA_OR_EXCP_DATA);
			}
		}
	}

	//第5秒查看能否读取到任意广播名字，且最小的RSSI是否满足条件，设置自检结果。
	if(t_count == (5000/100))
	{
		if(SELFCHECK_SCAN_DEVICE_RSSI_MIN_THRESHOLD >= ob_selfcheck.max_rssi)ob_selfcheck.selfcheck_result |= (1 << SELFCHECK_RESULT_ERR_RSSI);
	}

	//第5秒关闭100毫秒震动电机自检线程，初始化40毫秒自检显示结果线程，设置holdon，全功率运行。
	if(t_count == (5000/100))
	{
		nrf_gpio_pin_write(PIN_MT_EN,0);
		Process_Stop(selfcheck_mt_process);
		Process_Start(0,"selfcheck_result_display_process",selfcheck_result_display_process);
		Process_SetHoldOn(selfcheck_result_display_process,1);
	}

	//上报中间传感器的加速度值和roll值
	L=0;
	int16_t rol  = (int16_t)(getRoll()*100);
	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值
	if(TIME_GetTicks()-last_tim >= 1000)
	{
		last_tim = TIME_GetTicks();
		
		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);
	
	fml_adc_get_value(PIN_CHARGING_CHANNEL,&adc_value);
	if(adc_value >= 2000)
	{
		if(continue_trigger <= 50)continue_trigger++;
	}
	else
	{
		if(continue_trigger != 0)continue_trigger--;
	}
					
	//第30秒重启
	if(t_count >= (30000/100) && !slave_isconnect() && 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
		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)
{
	//纯粹做覆盖
}

/*API ------------------------------------------------------------------------------------------------------------------------------------*/
/**
	@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.selfcheck_is_led_display       = false;
	
	//关闭所有线程，初始化0毫秒自检线程，设置holdon，全功率运行。
	Process_All_Stop();
	Process_Start(100,"selfcheck_process",selfcheck_process);	
	Process_SetHoldOn(selfcheck_process,1);

	//关闭扫描，设置任意设备扫描回调，开启扫描
	while(TIME_GetTicks() - last_tim <= 1000)
	{
		host_disconnect();
		if(host_isconnect() == 0)break;
	}
	ST_scan_stop();
	host_set_scan_name((char *)"***********",sizeof("***********"));
	advdata_report_Evt_Regist(scan_report_cb);
	error = ST_scan_start();
	if(error != APP_SUCCESS)ob_selfcheck.selfcheck_result |= (1 << SELFCHECK_RESULT_ERR_RSSI);

	//配置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);	

	//重新初始化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);
	ADC_SetPinChannel(PIN_CHARGING, PIN_CHARGING_CHANNEL,NRF_GPIO_PIN_NOPULL);
	fml_adc_sample_update_notify_register(adc_callback);

	//配置前脚传感器、中间传感器、后脚传感器
	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);
	
	//初始化500ms震动电机自检线程，拉高震动电机引脚500ms然后拉低500ms（周期）
	Process_Start(500,"selfcheck_mt_process",selfcheck_mt_process);	
	Process_SetHoldOn(selfcheck_mt_process,1);

	//初始化计算roll值算法
	Mahony_Init(10);
	
	//喂狗
	feed_watchdog();	
	
}