/**

功耗：

闲置模式功耗为250+ua

低功耗模式1，若前后脚地磁开启，板子地磁不开启，且不读取数据，功耗为330+ua ； 若只是读取前后脚地磁，则功耗为450+ua， 若读取前后脚地磁后再reset + 配置，则功耗为700+ua

低功耗模式2，若只开启后脚地磁，板子地磁不开启，且不读取数据，功耗为300+ua ； 若只是读取后脚地磁，则功耗为370-390+ua, 若读取后脚地磁后再reset + 配置， 则功耗为500+ua

//以上低功耗测试都是没开六轴，只开地磁的情况

游戏模式spi_lsm，8.5ma

游戏模式i2c_lsm，8.7ma

前脚地磁供电关闭，后脚地磁供电开启，因为sda（标准输出）线共用，当sda输出0时，前脚地磁的供电IO和sda导通，导致短路，功耗为11ma，且后脚地磁数据异常。
所以通过开关电源来复位地磁失败！！！

低功耗模式3，只开启后脚地磁，板子地磁不开启，读取完地磁和低功耗加速度后（都是用SPI读取），在睡眠前重新配置（不reset）后脚地磁，功耗为370 - 400ua

PS：低功耗模式3中，读取低功耗加速度用SPI时，速度为152us，用模拟I2C读取，速度为457us，由于时间变长，功耗为400 - 430ua

更新时间：2021-10-12：

低功耗模式4，更改了结构板，将IMU移到前脚，同时I2C的SCL公用，SDA独立，取消每次读取地磁后reset+配置。
这时，低功耗模式（没跑算法）功耗为：350-380ua 、 闲置模式功耗为：180-270ua 、 游戏模式功耗为：12-13ma，若再扣去小板上的六轴挂起功耗(22ua)+地磁挂起功耗(9ua)的31ua，
则各个模式功耗为：低功耗模式（没跑算法）功耗为：319-349ua 、 闲置模式功耗为：149-239ua 、 游戏模式功耗为：12-13ma

更新时间：2021-11-25：
IMU的FIFO读取地磁，会出现重复现象，目前猜测是IMU的HZ与地磁的HZ不同步，地磁是100HZ，IMU是104HZ，其次，地磁无出现数据0的现象。

更新时间：2021-11-29：
版本：1.9.1
板子本身功耗：12-14ua
测试，低功耗FIFO功耗（没跑日常计步算法）：537-549ua
测试，低功耗FIFO功耗（没跑日常计步算法），不读取IMUFIFO数据：454-464ua
测试，单纯设置外设，不跑IMU线程，功耗：453-462ua
测试，单纯设置外设（断电后脚外设供电,虽然其本身就是设置完后就挂起），不跑IMU线程，功耗：433-444ua
测试，断电前后脚外设，不跑IMU线程，功耗：175-224ua
测试，开启维持一个串口，功耗增加为：473ua
结论：
低功耗FIFO模式（100ms唤醒）下，不跑日常计步算法，总功耗为：537-549ua
外设功耗为：后脚地磁挂起（20ua）+前脚外设的IMU_104HZ_acc_timestamp_sensorhub_slv0123（128ua）+前脚外设地磁100HZ（150ua）
线程功耗为：IMU线程（84ua->读取IMU的FIFO数据10组，耗时1.1~1.2ms）+bat线程（5ua）+其余线程和广播（170ua）

游戏模式下（没焊接单线灯）功耗为：
设备直接进入：7.4ma
链接右鞋后，手机下发游戏指令，进入：8.5-8.6ma

更新时间：2022-3-7
版本：2.1

待机模式（不穿鞋）功耗：270ua ~ 467ua

正常模式（穿鞋）功耗：570ua ~ 630u

游戏模式功耗：

*/

/*Includes ----------------------------------------------*/
#include "exception.h"
#include "system.h"
#include "hal_ser_imu_mode_manage.h"
#include "hal_ser_imu.h"
#include "bsp_time.h"
#include "nrf_delay.h"


/*Private macro ------------------------------------------------------------------------------------------------------------------------------------*/
#define							SER_IMU_HANDLE_NUM_MAX											HAL_SER_IMU_MODE_MANAGE_NUM					//允许最大的句柄数量

#define							INVALID_HANDLE															0x00																//无效句柄

#define							VALID_HANDLE																0x01																//有效句柄

#define 						MAX(a,b) 																		(((a) > (b)) ? (a) : (b))						//对比大小宏

#define							IS_EQUAL(a,b)																(((a) == (b)) ? true : false)				//对比相等宏								
	
#define							SER_IMU_DATA_GROUP_NUM_MAX									20																	//服务IMU最大数据组数

#define 						MONITOR_DATA_ERR_SUM_MAX										100																	//数据监测错误累计最大值

#define 						MONITOR_SUSPEND_MODE_OVERFLOW_ERR_SUM_MAX		10																	//异常挂起模式持续错误累计最大值

#define							DATA_UPDATE_NOTIFY_MAX											32																	//数据更新通知回调最大数

/*STRUCTION ------------------------------------------------------------------------------------------------------------------------------------*/
typedef enum {
	IMU_CONFIG_STAGE_DONE,																																						//配置完成阶段
	
	IMU_CONFIG_STAGE_IN_PROGRESS,																																			//配置进行阶段
	
} IMU_CONFIG_STAGE_e;

typedef struct
{
	int														id;																																	//句柄id
	
	char													*name;																															//句柄名称
	
	ser_imu_config_param_t				op_param[SER_IMU_DIR_NUM];																					//操作的IMU配置参数副本
	
} ser_imu_handle_t;

typedef union
{
	drv_lsm_config_param_t				lsm;																																//当前的LSM配置参数
	
	drv_qmc_config_param_t				qmc;																																//当前的QMC配置参数
	
} drv_param_u;

typedef struct ser_imu
{
	/*private member*/
	IMU_CONFIG_STAGE_e									stage;																												//配置流程状态
	
	bool																is_need_config[SER_IMU_DIR_NUM];															//各个方位的IMU是否需要配置
	
	ser_imu_handle_t										handle[SER_IMU_HANDLE_NUM_MAX];																//句柄组
	
	drv_param_u													drv_param[SER_IMU_DIR_NUM];																		//驱动配置参数组
	
	ser_imu_config_param_t							compre_drv_param[SER_IMU_DIR_NUM];														//综合的驱动配置参数
	
	int																	cur_data_num[SER_IMU_DIR_NUM];																//当前的数据量
	
	ser_imu_data_t											cur_data[SER_IMU_DIR_NUM][SER_IMU_DATA_GROUP_NUM_MAX];				//当前的IMU数据缓存区
	
	int16_t 														last_f_acc[3];																								//上一次的前脚加速度值
	
	int16_t 														last_f_gry[3];																								//上一次的前脚陀螺仪值
	
	int16_t 														last_f_mag[3];																								//上一次的前脚地磁计值
	
	int16_t 														last_b_mag[3];																								//上一次的后脚地磁计值	
	
	int16_t															last_f_acc_err_sum;																						//上一次的前脚加速度值错误累计
	
	int16_t 														last_f_gry_err_sum;																						//上一次的前脚陀螺仪值错误累计
	
	int16_t 														last_f_mag_err_sum;																						//上一次的前脚地磁计值错误累计
	
	int16_t 														last_b_mag_err_sum;																						//上一次的后脚地磁计值错误累计
	
	int16_t															except_data_occur_sum[SER_IMU_DIR_NUM];												//异常数据产生次数累计
	
	int16_t															except_suspend_occur_sum;																			//异常挂起产生次数累计
	
	hal_ser_imu_data_update_callback		notify_cb[DATA_UPDATE_NOTIFY_MAX];														//数据更新通知回调
} Ser_Imu_t;

/*Local Variable ------------------------------------------------------------------------------------------------------------------------------------*/
static Ser_Imu_t ob_ser_imu;

/*Local Functions ------------------------------------------------------------------------------------------------------------------------------------*/
static int hal_ser_imu_macro_conversion_acc_odr(SER_IMU_DIR_e dir, SER_IMU_ACC_ODR_e acc_odr)
{
	int ret = 0;
	
	if(dir == SER_IMU_DIR_FRONT)
	{
		switch(acc_odr)
		{
			case SER_IMU_ACC_ODR_OFF:
				ret = LSM_ACC_ODR_OFF;
				break;
			case SER_IMU_ACC_ODR_104HZ:
				ret = LSM_ACC_ODR_104HZ;
				break;
			case SER_IMU_ACC_ODR_12HZ5:
				ret = LSM_ACC_ODR_12HZ5;
				break;
		}
	}
	
	return ret;
}
static int hal_ser_imu_macro_conversion_gry_odr(SER_IMU_DIR_e dir, SER_IMU_GRY_ODR_e gry_odr)
{
	int ret = 0;
	
	if(dir == SER_IMU_DIR_FRONT)
	{
		switch(gry_odr)
		{
			case SER_IMU_GRY_ODR_OFF:
				ret =  LSM_GRY_ODR_OFF;
				break;
			case SER_IMU_GRY_ODR_104HZ:
				ret =  LSM_GRY_ODR_104HZ;
				break;
			case SER_IMU_GRY_ODR_12HZ5:
				ret =  LSM_GRY_ODR_12HZ5;
				break;
		}
	}	
	
	return ret;	
}
static int hal_ser_imu_macro_conversion_mag_odr(SER_IMU_DIR_e dir, SER_IMU_MAG_ODR_e mag_odr)
{
	int ret = 0;
	
	if(dir == SER_IMU_DIR_FRONT)
	{
		switch(mag_odr)
		{
			case SER_IMU_MAG_ODR_OFF:
				ret =  LSM_MAG_ODR_OFF;
				break;
			case SER_IMU_MAG_ODR_10HZ:
				ret =  LSM_MAG_ODR_10HZ;
				break;
			case SER_IMU_MAG_ODR_100HZ:
				ret =  LSM_MAG_ODR_100HZ;
				break;
			case SER_IMU_MAG_ODR_200HZ:
				ret =  LSM_MAG_ODR_200HZ;
				break;
		}
	}
	
	if(dir == SER_IMU_DIR_BACK)
	{
		switch(mag_odr)
		{
			case SER_IMU_MAG_ODR_OFF:
				ret =  QMC_MAG_ODR_OFF;
				break;
			case SER_IMU_MAG_ODR_10HZ:
				ret =  QMC_MAG_ODR_10HZ;
				break;
			case SER_IMU_MAG_ODR_100HZ:
				ret =  QMC_MAG_ODR_100HZ;
				break;
			case SER_IMU_MAG_ODR_200HZ:
				ret =  QMC_MAG_ODR_200HZ;
				break;
		}
	}
	
	return ret;
}
static int hal_ser_imu_macro_conversion_fifo_odr(SER_IMU_DIR_e dir, SER_IMU_FIFO_ODR_e fifo_odr)
{
	int ret = 0;
	
	if(dir == SER_IMU_DIR_FRONT)
	{
		switch(fifo_odr)
		{
			case SER_IMU_FIFO_ODR_OFF:
				ret =  LSM_FIFO_ODR_OFF;
				break;
			case SER_IMU_FIFO_ODR_104HZ:
				ret =  LSM_FIFO_ODR_104HZ;
				break;
		}
	}
	return ret;	
}
static int hal_ser_imu_macro_conversion_acc_power_mode(SER_IMU_DIR_e dir, SER_IMU_ACC_POWER_MODE_e acc_power_mode)
{
	int ret = 0;
	
	if(dir == SER_IMU_DIR_FRONT)
	{
		switch(acc_power_mode)
		{
			case SER_IMU_ACC_POWER_MODE_HIGH_PERFORMANCE:
				ret =  LSM_ACC_POWER_MODE_HIGH_PERFORMANCE;
				break;
			case SER_IMU_ACC_POWER_MODE_NORMAL:
				ret =  LSM_ACC_POWER_MODE_NORMAL;
				break;
		}
	}	
	return ret;	
}
static int hal_ser_imu_macro_conversion_gry_power_mode(SER_IMU_DIR_e dir, SER_IMU_GRY_POWER_MODE_e gry_power_mode)
{
	int ret = 0;
	
	if(dir == SER_IMU_DIR_FRONT)
	{
		switch(gry_power_mode)
		{
			case SER_IMU_GRY_POWER_MODE_HIGH_PERFORMANCE:
				ret =  LSM_GRY_POWER_MODE_HIGH_PERFORMANCE;
				break;
			case SER_IMU_GRY_POWER_MODE_NORMAL:
				ret =  LSM_GRY_POWER_MODE_NORMAL;
				break;
		}
	}	
	return ret;	
}
static int hal_ser_imu_macro_conversion_acc_fs(SER_IMU_DIR_e dir, SER_IMU_ACC_FS_e acc_fs)
{
	int ret = 0;
	
	if(dir == SER_IMU_DIR_FRONT)
	{
		switch(acc_fs)
		{
			case SER_IMU_ACC_FS_2G:
				ret =  LSM_ACC_FS_2G;
				break;
			case SER_IMU_ACC_FS_16G:
				ret =  LSM_ACC_FS_16G;
				break;
		}
	}
	return ret;
}
static int hal_ser_imu_macro_conversion_gry_fs(SER_IMU_DIR_e dir, SER_IMU_GRY_FS_e gry_fs)
{
	int ret = 0;
	
	if(dir == SER_IMU_DIR_FRONT)
	{
		switch(gry_fs)
		{
			case SER_IMU_GRY_FS_250DPS:
				ret =  LSM_GRY_FS_250DPS;
				break;
			case SER_IMU_GRY_FS_2000DPS:
				ret =  LSM_GRY_FS_2000DPS;
				break;
		}
	}
	return ret;	
}
static int hal_ser_imu_macro_conversion_mag_fs(SER_IMU_DIR_e dir, SER_IMU_MAG_FS_e mag_fs)
{
	int ret = 0;
	
	if(dir == SER_IMU_DIR_FRONT)
	{
		switch(mag_fs)
		{
			case SER_IMU_MAG_FS_30GS:
				ret =  LSM_MAG_FS_30GS;
				break;
		}
	}
	
	if(dir == SER_IMU_DIR_BACK)
	{
		switch(mag_fs)
		{
			case SER_IMU_MAG_FS_30GS:
				ret =  QMC_MAG_FS_30GS;
				break;
		}
	}
	
	
	return ret;	
}
static int hal_ser_imu_macro_conversion_timestamp_resolution(SER_IMU_DIR_e dir, SER_IMU_TIMESTAMP_RESOLUTION_e timestamp_resolution)
{
	int ret = 0;
	
	if(dir == SER_IMU_DIR_FRONT)
	{
		switch(timestamp_resolution)
		{
			case SER_IMU_TIMESTAMP_6MS4:
				ret =  LSM_TIMESTAMP_6MS4;
				break;
			case SER_IMU_TIMESTAMP_25US:
				ret =  LSM_TIMESTAMP_25US;
				break;
		}
	}
	return ret;	
}
static int hal_ser_imu_macro_conversion_timestamp_switch(SER_IMU_DIR_e dir, SER_IMU_TIMESTAMP_SWITCH_e timestamp_switch)
{
	int ret = 0;
	
	if(dir == SER_IMU_DIR_FRONT)
	{
		switch(timestamp_switch)
		{
			case SER_IMU_TIMESTAMP_OFF:
				ret =  LSM_TIMESTAMP_OFF;
				break;
			case SER_IMU_TIMESTAMP_ON:
				ret =  LSM_TIMESTAMP_ON;
				break;
		}
	}
	return ret;	
}

static void hal_ser_imu_get_compre_param(ser_imu_config_param_t *compre_drv_param, ser_imu_config_param_t *op_param)
{
	compre_drv_param->acc_fs 								= MAX(compre_drv_param->acc_fs, op_param->acc_fs);
	compre_drv_param->acc_odr 							= MAX(compre_drv_param->acc_odr, op_param->acc_odr);
	compre_drv_param->acc_power_mode 				= MAX(compre_drv_param->acc_power_mode, op_param->acc_power_mode);
	compre_drv_param->fifo_odr 							= MAX(compre_drv_param->fifo_odr, op_param->fifo_odr);
	compre_drv_param->gry_fs 								= MAX(compre_drv_param->gry_fs, op_param->gry_fs);
	compre_drv_param->gry_odr 							= MAX(compre_drv_param->gry_odr, op_param->gry_odr);
	compre_drv_param->gry_power_mode 				= MAX(compre_drv_param->gry_power_mode, op_param->gry_power_mode);
	compre_drv_param->mag_fs 								= MAX(compre_drv_param->mag_fs, op_param->mag_fs);
	compre_drv_param->mag_odr 							= MAX(compre_drv_param->mag_odr, op_param->mag_odr);
	compre_drv_param->timestamp_resolution 	= MAX(compre_drv_param->timestamp_resolution, op_param->timestamp_resolution);
	compre_drv_param->timestamp_switch 			= MAX(compre_drv_param->timestamp_switch, op_param->timestamp_switch);
}
static bool hal_ser_imu_param_is_equal_drv_param_lsm(ser_imu_config_param_t *compre_drv_param, drv_param_u *drv_param)
{
	if(IS_EQUAL(hal_ser_imu_macro_conversion_acc_fs(SER_IMU_DIR_FRONT, compre_drv_param->acc_fs),drv_param->lsm.acc_fs) == false){return false;}
	
	if(IS_EQUAL(hal_ser_imu_macro_conversion_acc_odr(SER_IMU_DIR_FRONT, compre_drv_param->acc_odr),drv_param->lsm.acc_odr) == false){return false;}
	
	if(IS_EQUAL(hal_ser_imu_macro_conversion_acc_power_mode(SER_IMU_DIR_FRONT, compre_drv_param->acc_power_mode),drv_param->lsm.acc_power_mode) == false){return false;}
	
	if(IS_EQUAL(hal_ser_imu_macro_conversion_fifo_odr(SER_IMU_DIR_FRONT, compre_drv_param->fifo_odr),drv_param->lsm.fifo_odr) == false){return false;}

	if(IS_EQUAL(hal_ser_imu_macro_conversion_gry_fs(SER_IMU_DIR_FRONT, compre_drv_param->gry_fs),drv_param->lsm.gry_fs) == false){return false;}
	
	if(IS_EQUAL(hal_ser_imu_macro_conversion_gry_odr(SER_IMU_DIR_FRONT, compre_drv_param->gry_odr),drv_param->lsm.gry_odr) == false){return false;}
	
	if(IS_EQUAL(hal_ser_imu_macro_conversion_gry_power_mode(SER_IMU_DIR_FRONT, compre_drv_param->gry_power_mode),drv_param->lsm.gry_power_mode) == false){return false;}
	
	if(IS_EQUAL(hal_ser_imu_macro_conversion_mag_fs(SER_IMU_DIR_FRONT, compre_drv_param->mag_fs),drv_param->lsm.mag_fs) == false){return false;}
	
	if(IS_EQUAL(hal_ser_imu_macro_conversion_mag_odr(SER_IMU_DIR_FRONT, compre_drv_param->mag_odr),drv_param->lsm.mag_odr) == false){return false;}

	if(IS_EQUAL(hal_ser_imu_macro_conversion_timestamp_resolution(SER_IMU_DIR_FRONT, compre_drv_param->timestamp_resolution),drv_param->lsm.timestamp_resolution) == false){return false;}
	
	if(IS_EQUAL(hal_ser_imu_macro_conversion_timestamp_switch(SER_IMU_DIR_FRONT, compre_drv_param->timestamp_switch),drv_param->lsm.timestamp_switch) == false){return false;}
	
	return true;
}
static bool hal_ser_imu_param_is_equal_drv_param_qmc(ser_imu_config_param_t *compre_drv_param, drv_param_u *drv_param)
{
	if(IS_EQUAL(hal_ser_imu_macro_conversion_mag_fs(SER_IMU_DIR_BACK, compre_drv_param->mag_fs), drv_param->qmc.mag_fs) == false){return false;}
	
	if(IS_EQUAL(hal_ser_imu_macro_conversion_mag_odr(SER_IMU_DIR_BACK, compre_drv_param->mag_odr), drv_param->qmc.mag_odr) == false){return false;}
	
	return true;
}
static int hal_ser_imu_read_data_lsm(void)
{
	int 								i;
	int									fifo_group_num;
	lsm_data_t					temp_lsm_data;
	
	//读取FIFO数据
	if(ob_ser_imu.compre_drv_param[SER_IMU_DIR_FRONT].fifo_odr != SER_IMU_FIFO_ODR_OFF)
	{
		//获取当前fifo里存在多少组数据。
		fifo_group_num = drv_lsm_get_fifo_group_num();

		fifo_group_num = fifo_group_num <= SER_IMU_DATA_GROUP_NUM_MAX ? fifo_group_num : SER_IMU_DATA_GROUP_NUM_MAX;
		for(i=0; i<fifo_group_num; i++)
		{
			if(drv_lsm_get_fifo_data(&temp_lsm_data) != -1)
			{
				ob_ser_imu.cur_data[SER_IMU_DIR_FRONT][i].acc[0] 					= temp_lsm_data.acc[0];
				ob_ser_imu.cur_data[SER_IMU_DIR_FRONT][i].acc[1] 					= temp_lsm_data.acc[1];
				ob_ser_imu.cur_data[SER_IMU_DIR_FRONT][i].acc[2] 					= temp_lsm_data.acc[2];
				
				ob_ser_imu.cur_data[SER_IMU_DIR_FRONT][i].gry[0] 					= temp_lsm_data.gry[0];
				ob_ser_imu.cur_data[SER_IMU_DIR_FRONT][i].gry[1] 					= temp_lsm_data.gry[1];
				ob_ser_imu.cur_data[SER_IMU_DIR_FRONT][i].gry[2] 					= temp_lsm_data.gry[2];
				
				ob_ser_imu.cur_data[SER_IMU_DIR_FRONT][i].mag[0] 					= temp_lsm_data.mag[0];
				ob_ser_imu.cur_data[SER_IMU_DIR_FRONT][i].mag[1] 					= temp_lsm_data.mag[1];
				ob_ser_imu.cur_data[SER_IMU_DIR_FRONT][i].mag[2] 					= temp_lsm_data.mag[2];
				
				ob_ser_imu.cur_data[SER_IMU_DIR_FRONT][i].fifo_timestamp 	= temp_lsm_data.fifo_timestamp;
			}
		}
		return fifo_group_num;
	}
	//只读取ACC数据
	else if(ob_ser_imu.compre_drv_param[SER_IMU_DIR_FRONT].acc_odr != SER_IMU_ACC_ODR_OFF && \
					ob_ser_imu.compre_drv_param[SER_IMU_DIR_FRONT].gry_odr == SER_IMU_GRY_ODR_OFF && \
					ob_ser_imu.compre_drv_param[SER_IMU_DIR_FRONT].mag_odr == SER_IMU_MAG_ODR_OFF \
				 )
	{
		if(drv_lsm_get_acc_data(&temp_lsm_data) != -1)
		{
			ob_ser_imu.cur_data[SER_IMU_DIR_FRONT][0].acc[0] 					= temp_lsm_data.acc[0];
			ob_ser_imu.cur_data[SER_IMU_DIR_FRONT][0].acc[1] 					= temp_lsm_data.acc[1];
			ob_ser_imu.cur_data[SER_IMU_DIR_FRONT][0].acc[2] 					= temp_lsm_data.acc[2];
			
			return 1;
		}
	}
	
	return 0;
}
static int hal_ser_imu_read_data_qmc(void)
{
	qmc_data_t	temp_qmc_data;
	
	if(ob_ser_imu.compre_drv_param[SER_IMU_DIR_BACK].mag_odr != SER_IMU_MAG_ODR_OFF)
	{
		if(drv_qmc6310_get_mag_data(&temp_qmc_data) != -1)
		{
			ob_ser_imu.cur_data[SER_IMU_DIR_BACK][0].mag[0] = temp_qmc_data.mag[0];
			ob_ser_imu.cur_data[SER_IMU_DIR_BACK][0].mag[1] = temp_qmc_data.mag[1];
			ob_ser_imu.cur_data[SER_IMU_DIR_BACK][0].mag[2] = temp_qmc_data.mag[2];
			
			return 1;
		}
	}

	return 0;
}
static int hal_ser_imu_read_data(SER_IMU_DIR_e dir)
{
	int ret = 0;
	
	switch(dir)
	{
		case SER_IMU_DIR_FRONT:
			ret =  hal_ser_imu_read_data_lsm();
			break;
		
		case SER_IMU_DIR_BACK:
			ret =  hal_ser_imu_read_data_qmc();
			break;
		
		default:
			break;
	}
	
	return ret;
}
static bool hal_ser_imu_is_need_config(void)
{
	int i;
	
	//重新获取综合的前脚配置，根据优先级来判定
	memset(&ob_ser_imu.compre_drv_param[SER_IMU_DIR_FRONT],0,sizeof(ob_ser_imu.compre_drv_param[SER_IMU_DIR_FRONT]));
	for(i=0; i<SER_IMU_HANDLE_NUM_MAX; i++)
	{
		if(ob_ser_imu.handle[i].id == VALID_HANDLE)
		{
			hal_ser_imu_get_compre_param(&ob_ser_imu.compre_drv_param[SER_IMU_DIR_FRONT], &ob_ser_imu.handle[i].op_param[SER_IMU_DIR_FRONT]);
		}
	}
	//重新获取驱动LSM配置参数
	drv_lsm_get_config_param(&ob_ser_imu.drv_param[SER_IMU_DIR_FRONT].lsm);
	
	//判断综合的前脚配置与当前的驱动配置是否一致
	if(hal_ser_imu_param_is_equal_drv_param_lsm(&ob_ser_imu.compre_drv_param[SER_IMU_DIR_FRONT], &ob_ser_imu.drv_param[SER_IMU_DIR_FRONT]) == false)
	{
		ob_ser_imu.is_need_config[SER_IMU_DIR_FRONT] = true;
	}

	/************************************************************************************************************************************************/

	//重新获取综合的后脚配置，根据优先级来判定
	memset(&ob_ser_imu.compre_drv_param[SER_IMU_DIR_BACK],0,sizeof(ob_ser_imu.compre_drv_param[SER_IMU_DIR_BACK]));
	for(i=0; i<SER_IMU_HANDLE_NUM_MAX; i++)
	{
		if(ob_ser_imu.handle[i].id == VALID_HANDLE)
		{
			hal_ser_imu_get_compre_param(&ob_ser_imu.compre_drv_param[SER_IMU_DIR_BACK], &ob_ser_imu.handle[i].op_param[SER_IMU_DIR_BACK]);
		}
	}
	//重新获取驱动QMC配置参数
	drv_qmc6310_get_config_param(&ob_ser_imu.drv_param[SER_IMU_DIR_BACK].qmc);
	//判断综合的前脚配置与当前的驱动配置是否一致
	if(hal_ser_imu_param_is_equal_drv_param_qmc(&ob_ser_imu.compre_drv_param[SER_IMU_DIR_BACK], &ob_ser_imu.drv_param[SER_IMU_DIR_BACK]) == false)
	{
		ob_ser_imu.is_need_config[SER_IMU_DIR_BACK] = true;
	}
	
	/************************************************************************************************************************************************/
	
	for(i=0; i<SER_IMU_DIR_NUM; i++)
	{
		if(ob_ser_imu.is_need_config[i] == true){
			return true;
		}
	}
	
	
	return false;
}
static bool hal_ser_imu_config_dir_front(void)
{
	int 								ret = 0;
	static int 					imu_config_flow_ctl = 0;																	//配置流程控制
	static int					imu_mag_cur_flow_ctl;
	static int					imu_mag_total_flow_ctl;
	static uint32_t 		tim = 0;
//	SEGGER_RTT_printf(0,"======= imu_config_flow_ctl:%d ======= \n",imu_config_flow_ctl);
	switch(imu_config_flow_ctl)
	{
		case 0:		
			//配置lsm断电
			if(drv_lsm_power_off() == 0){imu_config_flow_ctl = 1;}							
			break;
			
		case 1:
			//等待200ms
			if(tim == 0){
				tim = TIME_GetTicks();
			}else if(TIME_GetTicks()-tim>=200){
				tim = 0;
				imu_config_flow_ctl = 2;
			}
			break;			
			
		case 2:
			//配置lsm上电（默认配置挂起）
			if(drv_lsm_power_on() == 0){
				imu_config_flow_ctl = 3;
			}				
			break;
			
		case 3:
			//等待200ms
			if(tim == 0){
				tim = TIME_GetTicks();
			}else if(TIME_GetTicks()-tim>=200){
				tim = 0;
				imu_mag_total_flow_ctl = drv_lsm_get_mag_odr_flow();
				imu_mag_cur_flow_ctl = 1;
				imu_config_flow_ctl = 4;
			}
			break;				
			
		case 4:
			//配置lsm的地磁计采样频率
			if(ob_ser_imu.compre_drv_param[SER_IMU_DIR_FRONT].fifo_odr != SER_IMU_FIFO_ODR_OFF)
			{
				//使用hub
				if(drv_lsm_set_mag_odr((LSM_MAG_ODR_e)hal_ser_imu_macro_conversion_mag_odr(SER_IMU_DIR_FRONT,ob_ser_imu.compre_drv_param[SER_IMU_DIR_FRONT].mag_odr), 1, imu_mag_cur_flow_ctl) != -1){
					if(imu_mag_cur_flow_ctl == imu_mag_total_flow_ctl){
						imu_config_flow_ctl = 5;									//进入下一个流程
					}
					else{
						imu_mag_cur_flow_ctl++;										//进入下一个步骤
						tim = 0;
					}
				}
				else{
					if(tim == 0){
						tim = TIME_GetTicks();
					}else if(TIME_GetTicks()-tim>=1000){
						tim = 0;
						imu_config_flow_ctl = 0;									//累计超过1s，重新走流程
					}																									
				}
			}
			else
			{
				//不使用hub
				if(drv_lsm_set_mag_odr((LSM_MAG_ODR_e)hal_ser_imu_macro_conversion_mag_odr(SER_IMU_DIR_FRONT,ob_ser_imu.compre_drv_param[SER_IMU_DIR_FRONT].mag_odr), 0, imu_mag_cur_flow_ctl) != -1){
					if(imu_mag_cur_flow_ctl == imu_mag_total_flow_ctl){
						imu_config_flow_ctl = 5;									//进入下一个流程
						tim = 0;
					}
					else{
						imu_mag_cur_flow_ctl++;										//进入下一个步骤
						tim = 0;
					}
				}
				else{
					if(tim == 0){
						tim = TIME_GetTicks();
					}else if(TIME_GetTicks()-tim>=1000){
						tim = 0;
						imu_config_flow_ctl = 0;									//累计超过1s，重新走流程
					}																								
				}
			}
			break;
			
		case 5:
			//配置lsm的加速度工作模式
			ret += drv_lsm_set_acc_power_mode((LSM_ACC_POWER_MODE_e)hal_ser_imu_macro_conversion_acc_power_mode(SER_IMU_DIR_FRONT,ob_ser_imu.compre_drv_param[SER_IMU_DIR_FRONT].acc_power_mode));
			//配置lsm的陀螺仪工作模式
			ret += drv_lsm_set_gry_power_mode((LSM_GRY_POWER_MODE_e)hal_ser_imu_macro_conversion_gry_power_mode(SER_IMU_DIR_FRONT,ob_ser_imu.compre_drv_param[SER_IMU_DIR_FRONT].gry_power_mode));
			//配置lsm的加速度量程
			ret += drv_lsm_set_acc_fs((LSM_ACC_FS_e)hal_ser_imu_macro_conversion_acc_fs(SER_IMU_DIR_FRONT,ob_ser_imu.compre_drv_param[SER_IMU_DIR_FRONT].acc_fs));
			//配置lsm的陀螺仪量程
			ret += drv_lsm_set_gry_fs((LSM_GRY_FS_e)hal_ser_imu_macro_conversion_gry_fs(SER_IMU_DIR_FRONT,ob_ser_imu.compre_drv_param[SER_IMU_DIR_FRONT].gry_fs));
			//配置lsm的时间戳精度
			ret += drv_lsm_set_timestamp_resolution((LSM_TIMESTAMP_RESOLUTION_e)hal_ser_imu_macro_conversion_timestamp_resolution(SER_IMU_DIR_FRONT,ob_ser_imu.compre_drv_param[SER_IMU_DIR_FRONT].timestamp_resolution));
			//配置lsm的时间戳开关
			ret += drv_lsm_set_timestamp_switch((LSM_TIMESTAMP_SWITCH_e)hal_ser_imu_macro_conversion_timestamp_switch(SER_IMU_DIR_FRONT,ob_ser_imu.compre_drv_param[SER_IMU_DIR_FRONT].timestamp_switch));
			if(ret != -1)
			{
				imu_config_flow_ctl = 6;
			}
			break;
			
		case 6:
			//配置lsm的FIFO采样频率（必须以上都配置成功，才能配置FIFO）
			ret += drv_lsm_set_fifo_odr((LSM_FIFO_ODR_e)hal_ser_imu_macro_conversion_fifo_odr(SER_IMU_DIR_FRONT,ob_ser_imu.compre_drv_param[SER_IMU_DIR_FRONT].fifo_odr), \
																	(LSM_ACC_ODR_e)hal_ser_imu_macro_conversion_acc_odr(SER_IMU_DIR_FRONT,ob_ser_imu.compre_drv_param[SER_IMU_DIR_FRONT].acc_odr), \
																	(LSM_GRY_ODR_e)hal_ser_imu_macro_conversion_gry_odr(SER_IMU_DIR_FRONT,ob_ser_imu.compre_drv_param[SER_IMU_DIR_FRONT].gry_odr), \
																	(LSM_MAG_ODR_e)hal_ser_imu_macro_conversion_mag_odr(SER_IMU_DIR_FRONT,ob_ser_imu.compre_drv_param[SER_IMU_DIR_FRONT].mag_odr), \
																	(LSM_TIMESTAMP_SWITCH_e)hal_ser_imu_macro_conversion_timestamp_switch(SER_IMU_DIR_FRONT,ob_ser_imu.compre_drv_param[SER_IMU_DIR_FRONT].timestamp_switch));
			if(ret != -1)
			{
				imu_config_flow_ctl = 7;
			}
			break;
			
		case 7:
			//配置lsm的加速度采样频率
			ret += drv_lsm_set_acc_odr((LSM_ACC_ODR_e)hal_ser_imu_macro_conversion_acc_odr(SER_IMU_DIR_FRONT,ob_ser_imu.compre_drv_param[SER_IMU_DIR_FRONT].acc_odr));
			//配置lsm的陀螺仪采样频率
			ret += drv_lsm_set_gry_odr((LSM_GRY_ODR_e)hal_ser_imu_macro_conversion_gry_odr(SER_IMU_DIR_FRONT,ob_ser_imu.compre_drv_param[SER_IMU_DIR_FRONT].gry_odr));
			if(ret != -1)
			{
				imu_config_flow_ctl = 8;
			}
			break;
			
		case 8:
			if(ob_ser_imu.compre_drv_param[SER_IMU_DIR_FRONT].acc_odr != SER_IMU_ACC_ODR_OFF || \
				 ob_ser_imu.compre_drv_param[SER_IMU_DIR_FRONT].gry_odr != SER_IMU_GRY_ODR_OFF || \
				 ob_ser_imu.compre_drv_param[SER_IMU_DIR_FRONT].mag_odr != SER_IMU_MAG_ODR_OFF
				)
			{
				if(hal_ser_imu_read_data(SER_IMU_DIR_FRONT) != 0)
				{
					imu_config_flow_ctl = 0;
					
					return true;
				}
			}
			else
			{
				imu_config_flow_ctl = 0;
				
				return true;
			}
			break;
	}
	
	return false;
}
static bool hal_ser_imu_config_dir_back(void)
{
	int 								ret = 0;
	static int 					imu_config_flow_ctl = 0;																	//配置流程控制
	static int					imu_mag_cur_flow_ctl;
	static int					imu_mag_total_flow_ctl;	
	static uint32_t 		tim = 0;

	switch(imu_config_flow_ctl)
	{
		case 0:		
			//配置qmc断电
			if(drv_qmc6310_power_off() == 0){imu_config_flow_ctl = 1;}							
			break;
			
		case 1:
			//等待200ms
			if(tim == 0){
				tim = TIME_GetTicks();
			}else if(TIME_GetTicks()-tim>=200){
				tim = 0;
				imu_config_flow_ctl = 2;
			}
			break;			
			
		case 2:
			//配置qmc上电（默认配置挂起）
			if(drv_qmc6310_power_on() == 0){
				imu_config_flow_ctl = 3;
			}				
			break;
			
		case 3:
			//等待200ms
			if(tim == 0){
				tim = TIME_GetTicks();
			}else if(TIME_GetTicks()-tim>=200){
				tim = 0;
				imu_mag_total_flow_ctl = drv_qmc6310_get_mag_odr_flow();
				imu_mag_cur_flow_ctl = 1;				
				imu_config_flow_ctl = 4;
			}
			break;	
			
		case 4:
			//配置qmc的地磁计采样频率
			ret = drv_qmc6310_set_mag_odr((QMC_MAG_ODR_e)hal_ser_imu_macro_conversion_mag_odr(SER_IMU_DIR_BACK,ob_ser_imu.compre_drv_param[SER_IMU_DIR_BACK].mag_odr),imu_mag_cur_flow_ctl);
			if(ret != -1)
			{
				if(imu_mag_cur_flow_ctl == imu_mag_total_flow_ctl){
					imu_config_flow_ctl = 5;
				}
				else{
					imu_mag_cur_flow_ctl++;							//进入下一个步骤
					
					tim = 0;
				}
			}
			else
			{
				if(tim == 0){
					tim = TIME_GetTicks();
				}else if(TIME_GetTicks()-tim>=1000){
					tim = 0;
					imu_config_flow_ctl = 0;						//累计超过1s，重新走流程
				}
			}
			break;
			
		case 5:
			if(ob_ser_imu.compre_drv_param[SER_IMU_DIR_BACK].mag_odr != SER_IMU_MAG_ODR_OFF)
			{
				if(hal_ser_imu_read_data(SER_IMU_DIR_BACK) != 0)
				{
					imu_config_flow_ctl = 0;
					
					return true;
				}
			}
			else
			{
				imu_config_flow_ctl = 0;
				
				return true;
			}
			break;
	}
	
	return false;
}
static bool hal_ser_imu_is_config_done(SER_IMU_DIR_e dir)
{
	bool ret = true;
	
	switch(dir)
	{
		case SER_IMU_DIR_FRONT:
			ret =  hal_ser_imu_config_dir_front();
			break;
		
		case SER_IMU_DIR_BACK:
			ret =  hal_ser_imu_config_dir_back();
			break;
		
		default:
			break;
	}
	
	return ret;	
}
static void monitor_sensor_data(int16_t *f_acc, int16_t *f_gry, int16_t *f_mag, int16_t *b_mag)
{
	/*前脚加速度*/
	if(f_acc != NULL)
	{
		if(
				ob_ser_imu.last_f_acc[0] == f_acc[0] && \
				ob_ser_imu.last_f_acc[1] == f_acc[1] && \
				ob_ser_imu.last_f_acc[2] == f_acc[2]
			)
		{
			ob_ser_imu.last_f_acc_err_sum++;
			
			if(ob_ser_imu.last_f_acc_err_sum >= MONITOR_DATA_ERR_SUM_MAX){
				Except_SetExceptype(EXCEPT_DATA_FRONT_ACC);
			}
			
		}else{
			ob_ser_imu.last_f_acc_err_sum = 0;
		}
		
		ob_ser_imu.last_f_acc[0] = f_acc[0];
		ob_ser_imu.last_f_acc[1] = f_acc[1];
		ob_ser_imu.last_f_acc[2] = f_acc[2];
	}
	/*前脚陀螺仪*/
	if(f_gry != NULL)
	{
		if(
				ob_ser_imu.last_f_gry[0] == f_gry[0] && \
				ob_ser_imu.last_f_gry[1] == f_gry[1] && \
				ob_ser_imu.last_f_gry[2] == f_gry[2]
			)
		{
			ob_ser_imu.last_f_gry_err_sum++;
			
			if(ob_ser_imu.last_f_gry_err_sum >= MONITOR_DATA_ERR_SUM_MAX){
				Except_SetExceptype(EXCEPT_DATA_FRONT_GRY);
			}
				
		}else{
			ob_ser_imu.last_f_gry_err_sum = 0;
		}
		
		ob_ser_imu.last_f_gry[0] = f_gry[0];
		ob_ser_imu.last_f_gry[1] = f_gry[1];
		ob_ser_imu.last_f_gry[2] = f_gry[2];
	}
	/*前脚地磁计*/
	if(f_mag != NULL)
	{
		if(
				ob_ser_imu.last_f_mag[0] == f_mag[0] && \
				ob_ser_imu.last_f_mag[1] == f_mag[1] && \
				ob_ser_imu.last_f_mag[2] == f_mag[2]
			)
		{
			ob_ser_imu.last_f_mag_err_sum++;
			
			if(ob_ser_imu.last_f_mag_err_sum >= MONITOR_DATA_ERR_SUM_MAX){
				Except_SetExceptype(EXCEPT_DATA_FRONT_MAG);
			}
				
		}else{
			ob_ser_imu.last_f_mag_err_sum = 0;
		}
		
		ob_ser_imu.last_f_mag[0] = f_mag[0];
		ob_ser_imu.last_f_mag[1] = f_mag[1];
		ob_ser_imu.last_f_mag[2] = f_mag[2];
	}
	/*后脚地磁计*/
	if(b_mag != NULL)
	{
		if(
				ob_ser_imu.last_b_mag[0] == b_mag[0] && \
				ob_ser_imu.last_b_mag[1] == b_mag[1] && \
				ob_ser_imu.last_b_mag[2] == b_mag[2]
			)
		{
			ob_ser_imu.last_b_mag_err_sum++;
			
			if(ob_ser_imu.last_b_mag_err_sum >= MONITOR_DATA_ERR_SUM_MAX){
				Except_SetExceptype(EXCEPT_DATA_BACK_MAG);
			}
				
		}else{
			ob_ser_imu.last_b_mag_err_sum = 0;
		}
		
		ob_ser_imu.last_b_mag[0] = b_mag[0];
		ob_ser_imu.last_b_mag[1] = b_mag[1];
		ob_ser_imu.last_b_mag[2] = b_mag[2];
	}
	
	
	//监测到前脚数据异常
	if(Except_IsError(EXCEPT_DATA_FRONT_ACC) || Except_IsError(EXCEPT_DATA_FRONT_GRY) || Except_IsError(EXCEPT_DATA_FRONT_MAG))
	{
		ob_ser_imu.except_data_occur_sum[SER_IMU_DIR_FRONT]++;
		
		if(ob_ser_imu.except_data_occur_sum[SER_IMU_DIR_FRONT] > 1)
		{
			if(Except_IsError(EXCEPT_DATA_FRONT_ACC))Except_TxError(EXCEPT_DATA_FRONT_ACC,"front_acc_data_error");
			if(Except_IsError(EXCEPT_DATA_FRONT_GRY))Except_TxError(EXCEPT_DATA_FRONT_GRY,"front_gry_data_error");
			if(Except_IsError(EXCEPT_DATA_FRONT_MAG))Except_TxError(EXCEPT_DATA_FRONT_MAG,"front_mag_data_error");
		}
		//重启前脚LSM（主要是复位结构体）
		drv_lsm_power_off();
		drv_lsm_power_on();
		//清除异常
		Except_ClearExceptype(EXCEPT_DATA_FRONT_ACC);
		Except_ClearExceptype(EXCEPT_DATA_FRONT_GRY);
		Except_ClearExceptype(EXCEPT_DATA_FRONT_MAG);			
	}
	else
	{
		ob_ser_imu.except_data_occur_sum[SER_IMU_DIR_FRONT] = 0;
		//清除异常
		Except_ClearExceptype(EXCEPT_DATA_FRONT_ACC);
		Except_ClearExceptype(EXCEPT_DATA_FRONT_GRY);
		Except_ClearExceptype(EXCEPT_DATA_FRONT_MAG);		
	}
	
	//监测到后脚数据异常
	if(Except_IsError(EXCEPT_DATA_BACK_MAG))
	{
		ob_ser_imu.except_data_occur_sum[SER_IMU_DIR_BACK]++;
		
		if(ob_ser_imu.except_data_occur_sum[SER_IMU_DIR_BACK] > 1)
		{
			if(Except_IsError(EXCEPT_DATA_BACK_MAG))Except_TxError(EXCEPT_DATA_BACK_MAG,"back_mag_data_error");
		}
		//重启后脚QMC（主要是复位结构体）
		drv_qmc6310_power_off();
		drv_qmc6310_power_on();
		//清除异常
		Except_ClearExceptype(EXCEPT_DATA_BACK_MAG);		
	}
	else
	{
		ob_ser_imu.except_data_occur_sum[SER_IMU_DIR_BACK] = 0;
		//清除异常
		Except_ClearExceptype(EXCEPT_DATA_BACK_MAG);		
	}

}

static void hal_ser_imu_Process(void)
{
	int 						data_num[SER_IMU_DIR_NUM];
	
	switch(ob_ser_imu.stage)
	{
		case IMU_CONFIG_STAGE_DONE:																					//配置完成阶段
			
			if(hal_ser_imu_is_need_config())																	//判断是否需要配置
			{
				ob_ser_imu.stage = IMU_CONFIG_STAGE_IN_PROGRESS;								//进入配置进行阶段
				
				Process_SetHoldOn(hal_ser_imu_Process,1);												//全功率配置
			}
			else																															//读数据
			{
				//读取前脚IMU数据 + 更新前脚IMU数据量
				data_num[SER_IMU_DIR_FRONT] = hal_ser_imu_read_data(SER_IMU_DIR_FRONT);
				if(data_num[SER_IMU_DIR_FRONT] > 0)ob_ser_imu.cur_data_num[SER_IMU_DIR_FRONT] = data_num[SER_IMU_DIR_FRONT];

				//读取后脚IMU数据 + 更新后脚IMU数据量
				data_num[SER_IMU_DIR_BACK] = hal_ser_imu_read_data(SER_IMU_DIR_BACK);
				if(data_num[SER_IMU_DIR_BACK] > 0)ob_ser_imu.cur_data_num[SER_IMU_DIR_BACK] = data_num[SER_IMU_DIR_BACK];
				
				//回调通知
				if(data_num[SER_IMU_DIR_FRONT] > 0 || data_num[SER_IMU_DIR_BACK] > 0)
				{
					for(int i = 0; i < DATA_UPDATE_NOTIFY_MAX; i++)
					{
						if(ob_ser_imu.notify_cb[i])
						{
							ob_ser_imu.notify_cb[i](data_num[SER_IMU_DIR_FRONT],data_num[SER_IMU_DIR_BACK]);
						}
					}
				}
			}
			
			break;
		
		case IMU_CONFIG_STAGE_IN_PROGRESS:																	//配置进行阶段
			
			if(ob_ser_imu.is_need_config[SER_IMU_DIR_FRONT])									//判断前脚是否需要配置
			{
				if(hal_ser_imu_is_config_done(SER_IMU_DIR_FRONT))
				{
					ob_ser_imu.is_need_config[SER_IMU_DIR_FRONT] = false;
				}
			}
			else if(ob_ser_imu.is_need_config[SER_IMU_DIR_BACK])							//判断后脚是否需要配置
			{
				if(hal_ser_imu_is_config_done(SER_IMU_DIR_BACK))
				{
					ob_ser_imu.is_need_config[SER_IMU_DIR_BACK] = false;
				}
			}
			else
			{
				if(!hal_ser_imu_is_need_config())																//若是在配置期间，操作的IMU配置参数副本更改，需要重新配置。
				{
					ob_ser_imu.stage = IMU_CONFIG_STAGE_DONE;											//配置完成
					
					//读数据
					
					Process_SetHoldOn(hal_ser_imu_Process,0);											//解放线程
				}
			}
		
			break;
	}
	
}

static void hal_ser_imu_monitor_sensor_data_process(void)
{
	int 							i = 0;
	int16_t						group_num = 0;
	static int16_t 		f_acc[3];
	static int16_t		f_gry[3];
	static int16_t		f_mag[3];
	static int16_t		b_mag[3];
	ser_imu_data_t		data;
	static uint32_t		last_tim = 0;
	
	if(
			ob_ser_imu.stage == IMU_CONFIG_STAGE_DONE && \
			ob_ser_imu.drv_param[SER_IMU_DIR_FRONT].lsm.acc_odr == LSM_ACC_ODR_104HZ && \
			ob_ser_imu.drv_param[SER_IMU_DIR_FRONT].lsm.gry_odr == LSM_GRY_ODR_104HZ && \
			(ob_ser_imu.drv_param[SER_IMU_DIR_FRONT].lsm.mag_odr == LSM_MAG_ODR_200HZ || ob_ser_imu.drv_param[SER_IMU_DIR_FRONT].lsm.mag_odr == LSM_MAG_ODR_100HZ)
		)
	{
		if(TIME_GetTicks()-last_tim >= FullPower_Interval)				//监测前脚传感器数据（acc + gry + mag）+ 监测后脚传感器数据（mag）	
		{
			last_tim = TIME_GetTicks();
			
			group_num = hal_ser_imu_get_data_num(SER_IMU_DIR_FRONT);
			
			if(group_num > 0)
			{
				for(i=0;i<group_num;i++)
				{
					hal_ser_imu_get_data(SER_IMU_DIR_FRONT, i, &data);
					f_gry[0] = data.gry[0];f_gry[1] = data.gry[1];f_gry[2] = data.gry[2];
					f_acc[0] = data.acc[0];f_acc[1] = data.acc[1];f_acc[2] = data.acc[2];
					f_mag[0] = data.mag[0];f_mag[1] = data.mag[1];f_mag[2] = data.mag[2];
					
					monitor_sensor_data(f_acc, f_gry, f_mag, NULL);
				}
			}
			else
			{
				monitor_sensor_data(f_acc, f_gry, f_mag, NULL);
			}

			
			if(hal_ser_imu_get_data_num(SER_IMU_DIR_BACK) >= 1){
				hal_ser_imu_get_data(SER_IMU_DIR_BACK, 0, &data);
				b_mag[0] = data.mag[0];b_mag[1] = data.mag[1];b_mag[2] = data.mag[2];
			}
			
			monitor_sensor_data(NULL, NULL, NULL, b_mag);
		}
	}
	else if(	
						ob_ser_imu.stage == IMU_CONFIG_STAGE_DONE && \
						ob_ser_imu.drv_param[SER_IMU_DIR_FRONT].lsm.acc_odr == LSM_ACC_ODR_104HZ && \
						(ob_ser_imu.drv_param[SER_IMU_DIR_FRONT].lsm.mag_odr == LSM_MAG_ODR_200HZ || ob_ser_imu.drv_param[SER_IMU_DIR_FRONT].lsm.mag_odr == LSM_MAG_ODR_100HZ) 
				 )
	{
		if(TIME_GetTicks()-last_tim >= LowPower_Interval)				//监测前脚传感器数据（acc + mag）
		{
			last_tim = TIME_GetTicks();
			
			group_num = hal_ser_imu_get_data_num(SER_IMU_DIR_FRONT);
			
			if(group_num > 0)
			{
				for(i=0;i<group_num;i++)
				{
					hal_ser_imu_get_data(SER_IMU_DIR_FRONT, i, &data);
					f_acc[0] = data.acc[0];f_acc[1] = data.acc[1];f_acc[2] = data.acc[2];
					f_mag[0] = data.mag[0];f_mag[1] = data.mag[1];f_mag[2] = data.mag[2];
					
					monitor_sensor_data(f_acc, NULL, f_mag, NULL);		
				}
			}
			else
			{
				monitor_sensor_data(f_acc, NULL, f_mag, NULL);		
			}

		}
	}
	else if(ob_ser_imu.stage == IMU_CONFIG_STAGE_DONE && ob_ser_imu.drv_param[SER_IMU_DIR_FRONT].lsm.acc_odr == LSM_ACC_ODR_12HZ5)
	{
		if(TIME_GetTicks()-last_tim >= StandByPower_Interval)		//监测前脚传感器数据（acc）
		{
			last_tim = TIME_GetTicks();
			
			group_num = hal_ser_imu_get_data_num(SER_IMU_DIR_FRONT);
			
			if(group_num > 0)
			{
				for(i=0;i<group_num;i++)
				{
					hal_ser_imu_get_data(SER_IMU_DIR_FRONT, i, &data);
					f_acc[0] = data.acc[0];f_acc[1] = data.acc[1];f_acc[2] = data.acc[2];
					
					monitor_sensor_data(f_acc, NULL, NULL, NULL);		
				}
			}
			else
			{
				monitor_sensor_data(f_acc, NULL, NULL, NULL);		
			}
			
		}
	}
}

static void hal_ser_imu_monitor_suspend_mode_overflow_process(void)
{
	//监测异常挂起模式是否持续时间超过临界点
	if(ob_ser_imu.is_need_config[SER_IMU_DIR_FRONT] == true || ob_ser_imu.is_need_config[SER_IMU_DIR_BACK] == true)
	{
		ob_ser_imu.except_suspend_occur_sum++;
		
		if(ob_ser_imu.except_suspend_occur_sum >= MONITOR_SUSPEND_MODE_OVERFLOW_ERR_SUM_MAX){
			Except_SetExceptype(EXCEPT_IMU_SUSPEND_OVERFLOW);		
			Except_TxError(EXCEPT_IMU_SUSPEND_OVERFLOW,"front_imu_suspend_overflow");			
			ob_ser_imu.except_suspend_occur_sum = MONITOR_SUSPEND_MODE_OVERFLOW_ERR_SUM_MAX;
			
		}
	}
	else
	{
		ob_ser_imu.except_suspend_occur_sum = 0;
		Except_ClearExceptype(EXCEPT_IMU_SUSPEND_OVERFLOW);
	}	
}

static void hal_ser_imu_printf_Process(void)
{
	SEGGER_RTT_printf(0,"hal_ser_imu----------------------------------------------------------------->\n");
	SEGGER_RTT_printf(0,"stage:0x%x\n",ob_ser_imu.stage);
	SEGGER_RTT_printf(0,"is_need_config[SER_IMU_DIR_FRONT]:0x%x\n",ob_ser_imu.is_need_config[SER_IMU_DIR_FRONT]);
	SEGGER_RTT_printf(0,"is_need_config[SER_IMU_DIR_BACK]:0x%x\n",ob_ser_imu.is_need_config[SER_IMU_DIR_BACK]);
	
//	SEGGER_RTT_printf(0,"df acc_fs:0x%x\n",ob_ser_imu.drv_param[SER_IMU_DIR_FRONT].lsm.acc_fs);
	SEGGER_RTT_printf(0,"df acc_odr:0x%x\n",ob_ser_imu.drv_param[SER_IMU_DIR_FRONT].lsm.acc_odr);
//	SEGGER_RTT_printf(0,"df acc_power_mode:0x%x\n",ob_ser_imu.drv_param[SER_IMU_DIR_FRONT].lsm.acc_power_mode);
	SEGGER_RTT_printf(0,"df fifo_odr:0x%x\n",ob_ser_imu.drv_param[SER_IMU_DIR_FRONT].lsm.fifo_odr);
//	SEGGER_RTT_printf(0,"df gry_fs:0x%x\n",ob_ser_imu.drv_param[SER_IMU_DIR_FRONT].lsm.gry_fs);
	SEGGER_RTT_printf(0,"df gry_odr:0x%x\n",ob_ser_imu.drv_param[SER_IMU_DIR_FRONT].lsm.gry_odr);
//	SEGGER_RTT_printf(0,"df gry_power_mode:0x%x\n",ob_ser_imu.drv_param[SER_IMU_DIR_FRONT].lsm.gry_power_mode);
//	SEGGER_RTT_printf(0,"df mag_fs:0x%x\n",ob_ser_imu.drv_param[SER_IMU_DIR_FRONT].lsm.mag_fs);
	SEGGER_RTT_printf(0,"df mag_odr:0x%x\n",ob_ser_imu.drv_param[SER_IMU_DIR_FRONT].lsm.mag_odr);
//	SEGGER_RTT_printf(0,"f timestamp_resolution:0x%x\n",ob_ser_imu.drv_param[SER_IMU_DIR_FRONT].lsm.timestamp_resolution);
//	SEGGER_RTT_printf(0,"df timestamp_switch:0x%x\n",ob_ser_imu.drv_param[SER_IMU_DIR_FRONT].lsm.timestamp_switch);
	SEGGER_RTT_printf(0,"db mag_odr:0x%x\n",ob_ser_imu.drv_param[SER_IMU_DIR_BACK].qmc.mag_odr);
	
//	SEGGER_RTT_printf(0,"cf acc_fs:0x%x\n",ob_ser_imu.compre_drv_param[SER_IMU_DIR_FRONT].acc_fs);
	SEGGER_RTT_printf(0,"cf acc_odr:0x%x\n",ob_ser_imu.compre_drv_param[SER_IMU_DIR_FRONT].acc_odr);
//	SEGGER_RTT_printf(0,"cf acc_power_mode:0x%x\n",ob_ser_imu.compre_drv_param[SER_IMU_DIR_FRONT].acc_power_mode);
	SEGGER_RTT_printf(0,"cf fifo_odr:0x%x\n",ob_ser_imu.compre_drv_param[SER_IMU_DIR_FRONT].fifo_odr);
//	SEGGER_RTT_printf(0,"cf gry_fs:0x%x\n",ob_ser_imu.compre_drv_param[SER_IMU_DIR_FRONT].gry_fs);
	SEGGER_RTT_printf(0,"cf gry_odr:0x%x\n",ob_ser_imu.compre_drv_param[SER_IMU_DIR_FRONT].gry_odr);
//	SEGGER_RTT_printf(0,"cf gry_power_mode:0x%x\n",ob_ser_imu.compre_drv_param[SER_IMU_DIR_FRONT].gry_power_mode);
//	SEGGER_RTT_printf(0,"cf mag_fs:0x%x\n",ob_ser_imu.compre_drv_param[SER_IMU_DIR_FRONT].mag_fs);
	SEGGER_RTT_printf(0,"cf mag_odr:0x%x\n",ob_ser_imu.compre_drv_param[SER_IMU_DIR_FRONT].mag_odr);
//	SEGGER_RTT_printf(0,"cf timestamp_resolution:0x%x\n",ob_ser_imu.compre_drv_param[SER_IMU_DIR_FRONT].timestamp_resolution);
//	SEGGER_RTT_printf(0,"cf timestamp_switch:0x%x\n",ob_ser_imu.compre_drv_param[SER_IMU_DIR_FRONT].timestamp_switch);
	SEGGER_RTT_printf(0,"cb mag_odr:0x%x\n",ob_ser_imu.compre_drv_param[SER_IMU_DIR_BACK].mag_odr);
	
	SEGGER_RTT_printf(0,"ob_ser_imu.cur_data_num[SER_IMU_DIR_FRONT]:0x%x\n",ob_ser_imu.cur_data_num[SER_IMU_DIR_FRONT]);
	
	SEGGER_RTT_printf(0,"ob_ser_imu.cur_data_num[SER_IMU_DIR_BACK]:0x%x\n",ob_ser_imu.cur_data_num[SER_IMU_DIR_BACK]);
	
	SEGGER_RTT_printf(0,"<-----------------------------------------------------------------hal_ser_imu\n");

}

/*API ------------------------------------------------------------------------------------------------------------------------------------*/
/**
	@brief 		初始化IMU服务
	@param 		无
	@return 	错误代码					- [out]	-1失败，0成功
*/
int	hal_ser_imu_Init(void)
{
	int i;
	int ret;
	
	/***************************************驱动层初始化***************************************************/
	//初始化驱动LSM
	ret = drv_lsm_Init();
	//初始化驱动QMC
	ret += drv_qmc6310_Init();
	
	//获取驱动LSM配置参数
	drv_lsm_get_config_param(&ob_ser_imu.drv_param[SER_IMU_DIR_FRONT].lsm);
	
	//获取驱动LSM配置参数
	drv_qmc6310_get_config_param(&ob_ser_imu.drv_param[SER_IMU_DIR_BACK].qmc);
	
	/***************************************业务逻辑层初始化***************************************************/
	
	//重置结构体
	memset(&ob_ser_imu,0,sizeof(ob_ser_imu));	
	//初始化IMU服务结构体
	ob_ser_imu.stage = IMU_CONFIG_STAGE_DONE;
	
	for(i=0; i<SER_IMU_HANDLE_NUM_MAX; i++)
	{
		ob_ser_imu.handle[i].id 											= INVALID_HANDLE;
		ob_ser_imu.handle[i].name											=	NULL;
	}
	
	for(i=0; i<SER_IMU_DIR_NUM; i++)
	{
		ob_ser_imu.is_need_config[i] 									= false;
	}
	
	//设置驱动配置线程
	Process_Start(0,"hal_ser_imu_Process",hal_ser_imu_Process);
	
	Process_Start(10,"hal_ser_imu_monitor_sensor_data_process",hal_ser_imu_monitor_sensor_data_process);
	
//	Process_Start(1000,"hal_ser_imu_printf_Process",hal_ser_imu_printf_Process);
	
	Process_Start(3000,"hal_ser_imu_monitor_suspend_mode_overflow_process",hal_ser_imu_monitor_suspend_mode_overflow_process);
	

	if(ret < 0)return -1;
	
	return 0;
	
}
/**
	@brief 		获取IMU服务句柄
	@param 		p_name 						- [in]	为服务句柄设置的名字
	@param 		p_handle_id 			- [out]	返回的服务句柄ID
	@return 	错误代码					- [out]	-1失败，0成功
*/
int	hal_ser_imu_get_handle(char *p_name, int *p_handle_id)
{
	for(int i=0; i<SER_IMU_HANDLE_NUM_MAX; i++)
	{
		//判断是否有空余的句柄
		if(ob_ser_imu.handle[i].id == INVALID_HANDLE)
		{
			//设置该句柄的名字
			ob_ser_imu.handle[i].name = p_name;
			//该句柄使能有效
			ob_ser_imu.handle[i].id = VALID_HANDLE;
			//返回服务句柄ID
			*p_handle_id = i;
			
			return 0;
		}
	}
	
	return -1;
}
/**
	@brief 		设置IMU需要的配置参数
	@param 		handle 						- [in]	操作的IMU服务句柄
	@param 		foot 							- [in]	IMU方向 - 前/后脚
	@param 		param 						- [in]	IMU配置参数
	@return 	错误代码					- [out]	-1失败，0成功
*/
int	hal_ser_imu_set_required_param(int handle_id, SER_IMU_DIR_e dir, ser_imu_config_param_t	*param)
{
	//无效句柄返回
	if(handle_id < 0 || ob_ser_imu.handle[handle_id].id == INVALID_HANDLE)return -1;	
	
	//更新操作的配置参数副本
	memcpy(&ob_ser_imu.handle[handle_id].op_param[dir], param ,sizeof(ob_ser_imu.handle[handle_id].op_param[dir]));
	
	return 0;
}
/**
	@brief 		IMU配置参数是否准备好
	@param 		foot 							- [in]	IMU方向 - 前/后脚
	@param 		param 						- [in]	IMU配置参数
	@return 	错误代码					- [out]	-1失败，0成功
*/
int hal_ser_imu_is_param_get_ready(SER_IMU_DIR_e dir, ser_imu_config_param_t	*param)
{
	int ret = 0;
	
	//查看是否需要进行配置
	if(hal_ser_imu_is_need_config())
	{
		return -1;
	}
	
	
	//判断当前配置是否满足
	switch(dir)
	{
		case SER_IMU_DIR_FRONT:
			if(ob_ser_imu.compre_drv_param[SER_IMU_DIR_FRONT].acc_fs < param->acc_fs){ret = -1;break;}
			if(ob_ser_imu.compre_drv_param[SER_IMU_DIR_FRONT].acc_odr < param->acc_odr){ret = -1;break;}
			if(ob_ser_imu.compre_drv_param[SER_IMU_DIR_FRONT].acc_power_mode < param->acc_power_mode){ret = -1;break;}
			if(ob_ser_imu.compre_drv_param[SER_IMU_DIR_FRONT].fifo_odr < param->fifo_odr){ret = -1;break;}
			if(ob_ser_imu.compre_drv_param[SER_IMU_DIR_FRONT].gry_fs < param->gry_fs){ret = -1;break;}
			if(ob_ser_imu.compre_drv_param[SER_IMU_DIR_FRONT].gry_odr < param->gry_odr){ret = -1;break;}
			if(ob_ser_imu.compre_drv_param[SER_IMU_DIR_FRONT].gry_power_mode < param->gry_power_mode){ret = -1;break;}
			if(ob_ser_imu.compre_drv_param[SER_IMU_DIR_FRONT].mag_fs < param->mag_fs){ret = -1;break;}
			if(ob_ser_imu.compre_drv_param[SER_IMU_DIR_FRONT].mag_odr < param->mag_odr){ret = -1;break;}
			if(ob_ser_imu.compre_drv_param[SER_IMU_DIR_FRONT].timestamp_resolution < param->timestamp_resolution){ret = -1;break;}
			if(ob_ser_imu.compre_drv_param[SER_IMU_DIR_FRONT].timestamp_switch < param->timestamp_switch){ret = -1;break;}
			break;
		
		case SER_IMU_DIR_BACK:
			if(ob_ser_imu.compre_drv_param[SER_IMU_DIR_BACK].mag_fs < param->mag_fs){ret = -1;break;}
			if(ob_ser_imu.compre_drv_param[SER_IMU_DIR_BACK].mag_odr < param->mag_odr){ret = -1;break;}
			break;
		
		default:
			break;
	}
	
	return ret;

}
/**
	@brief 		获取IMU当前数据数量
	@param 		dir 							- [in]	IMU方向 - 前/后脚
	@return 	错误代码					- [out]	返回数据数量
*/
int	hal_ser_imu_get_data_num(SER_IMU_DIR_e dir)
{
	int ret = 0;
	
	switch(dir)
	{
		case SER_IMU_DIR_FRONT:
			ret =  ob_ser_imu.cur_data_num[SER_IMU_DIR_FRONT];
			break;
		
		case SER_IMU_DIR_BACK:
			ret =  ob_ser_imu.cur_data_num[SER_IMU_DIR_BACK];
			break;
		
		default:
			break;
	}
	
	return ret;
}
/**
	@brief 		获取IMU数据
	@param 		dir 							- [in]	IMU方向 - 前/后脚
	@param 		index 						- [in]	第几组数据
	@param 		pdata 						- [out]	IMU数据
	@return 	错误代码					- [out]	-1失败，0成功
*/
int	hal_ser_imu_get_data(SER_IMU_DIR_e dir, int index, ser_imu_data_t *pdata)
{
	if(index < 0)return -1;
	
	*pdata = ob_ser_imu.cur_data[dir][index];
	
	return 0;
}
/**
	@brief 		数据更新通知
	@param 		p_cb 							- [in]	需要回调的函数
	@return 	错误代码					- [out]	-1失败，0成功，-2已存在
*/
int	hal_ser_imu_data_update_notify(hal_ser_imu_data_update_callback p_cb)
{
	for(int i=0;i<DATA_UPDATE_NOTIFY_MAX;i++) 
	{
		if(ob_ser_imu.notify_cb[i]==p_cb) return -2;
		
		if(ob_ser_imu.notify_cb[i]==0)
		{
			ob_ser_imu.notify_cb[i] = p_cb;  //回调函数
			return 0;
		}
	}
	return -1;
}