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_qma7981.h"
#include "hal_led.h"
#include "hal_battery.h"
#include "bll_imu.h"
#include "app_flash.h"
#include "system.h"
#include "hal_mahonyAHRS.h"
#include "app_detectIsHost.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_NO_WELDING_CAPACITOR_MIN_THRESHOLD								 200																								//地磁传感器没焊接电容的最小阈值

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

#define	SELFCHECK_MIDDLE_ACC_CHECK_MT_MIN_THRESHOLD														 5000																								//中间加速度检测电机最小阈值

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


/*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_CHARGE_CHIP_PIN_ADC,																																												//自检失败——充电芯片引脚ADC
	
	SELFCHECK_RESULT_ERR_BATTERY_PIN_ADC,																																														//自检失败——电池分压电阻ADC
	
	SELFCHECK_RESULT_ERR_RSSI,																																																			//自检失败——RSSI
	
	SELFCHECK_RESULT_ERR_MT,																																																				//自检失败——震动电机
	
	SELFCHECK_RESULT_ERR_FRONT_IMU_REVERSE,																																													//自检失败——前脚传感器装反
	
	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_FRONT_MAG_DATA_TO_SMALL,																																										//自检失败——前脚传感器地磁数据过小
	
	SELFCHECK_RESULT_ERR_BACK_SENSOR_CONFIG,																																												//自检失败——后脚传感器配置
	
	SELFCHECK_RESULT_ERR_BACK_NO_DATA,																																															//自检失败——后脚传感器没数据
	
	SELFCHECK_RESULT_ERR_BACK_EXCP_DATA,																																														//自检失败——后脚传感器数据异常(数据持续相等)
	
	SELFCHECK_RESULT_ERR_BACK_DATA_TO_SMALL,																																												//自检失败——后脚传感器地磁数据过小
	
	SELFCHECK_RESULT_ERR_MIDDLE_SENSOR_CONFIG,																																											//自检失败——中间传感器配置
	
	SELFCHECK_RESULT_ERR_MIDDLE_NO_DATA_OR_EXCP_DATA,																																								//自检失败——中间传感器没数据或数据异常
	
} 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;																																				//指令
	
} 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_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();
	
	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();
		Process_UpdatePeroid(selfcheck_result_display_process,SELFCHECK_LED_DISPLAY_CYCLE_THRESHOLD * 3);
	}
}

static void selfcheck_mt_process(void)
{
	static int16_t	max_acc_z = 0;
	int16_t	acc_z = 0;
	static uint8_t flow = 0;
	//喂狗
	feed_watchdog();	
	
	nrf_gpio_pin_write(PIN_MT_EN,1);
	
	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;
	}
	
	//检测震动是否达标
	//读取中间传感器数据，计算加速度roll值
	if(ob_selfcheck.order == 0x52)
	{
		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[2] < 0)acc_z = ob_selfcheck.m_data.acc[2]*-1;
			else acc_z = ob_selfcheck.m_data.acc[2];
			
			max_acc_z = (max_acc_z > acc_z)?max_acc_z:acc_z;
			if(max_acc_z < SELFCHECK_MIDDLE_ACC_CHECK_MT_MIN_THRESHOLD)ob_selfcheck.selfcheck_result |= (1 << SELFCHECK_RESULT_ERR_MT);
			else ob_selfcheck.selfcheck_result &= ~(1 << SELFCHECK_RESULT_ERR_MT);
//			SEGGER_RTT_printf(0,"middle acc %d,%d,%d acc_z:%d,max:%d\r\n",ob_selfcheck.m_data.acc[0],ob_selfcheck.m_data.acc[1],ob_selfcheck.m_data.acc[2],acc_z,max_acc_z);
		}
	}
}

#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_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;

	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);
	
	
	//筛选最大的电池分压后的电压
	fml_adc_get_value(PIN_ADC_BAT_CHANNEL,&adc_value);
	adc_value = ADC_RESULT_IN_MILLI_VOLTS(adc_value) * 5 / 3;
	battery_adc_max = middleFilter(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)(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;
	}

	//每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 == 0x52)
	{
		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秒查看能否读取到任意广播名字，且最小的RSSI是否满足条件，设置自检结果。
	if(t_count == (3000/100))
	{
		if(SELFCHECK_SCAN_DEVICE_RSSI_MIN_THRESHOLD >= ob_selfcheck.max_rssi)ob_selfcheck.selfcheck_result |= (1 << SELFCHECK_RESULT_ERR_RSSI);
	}
	
	//第3秒关闭100毫秒震动电机自检线程
	if(t_count == (3000/100))
	{
		WS2812_DisplayDot(COLOR_BLACK);WS2812_Pwm_Play();
		nrf_gpio_pin_write(PIN_MT_EN,0);
		Process_Stop(selfcheck_mt_process);
	}

	//第3+SELFCHECK_LED_DISPLAY_CYCLE_THRESHOLD秒初始化自检显示结果线程，设置holdon，全功率运行。
	if(t_count == ((3000 + SELFCHECK_LED_DISPLAY_CYCLE_THRESHOLD)/100))
	{
		//根据自检结果设置灯
		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(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[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--;
	}
	

//	SEGGER_RTT_printf(0,"continue_trigger:%d,%d\r\n",continue_trigger,adc_value);
//	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);
					
	//第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));
		
		//恢复出厂信息
		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)
{
	//纯粹做覆盖
}

/*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.order													=	order;
	
	//关闭所有线程，初始化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;
	}
	nrf_ble_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);	
	
	//配置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);
	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);
	
	//初始化震动电机自检线程
	Process_Start(SELFCHECK_LED_DISPLAY_CYCLE_THRESHOLD,"selfcheck_mt_process",selfcheck_mt_process);	
	Process_SetHoldOn(selfcheck_mt_process,1);

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