liweiyan/shoe_mcu_2022_2_22_v3/hal/hal_ser_imu.c

/*Includes ----------------------------------------------*/
#include "system.h"
#include "hal_ser_imu.h"


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

#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最大数据组数

/*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数据缓存区
	
} 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 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;																	//配置流程控制
	
	switch(imu_config_flow_ctl)
	{
		case 0:		
			//配置lsm挂起
			if(drv_lsm_suspend() == 0){imu_config_flow_ctl = 1;}				
			break;
			
		case 1:		
			//配置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) != -1){
					imu_config_flow_ctl = 2;
				}
			}
			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) != -1){
					imu_config_flow_ctl = 2;
				}
			}
			break;
			
		case 2:
			//配置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));
			//配置lsm的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));
			if(ret != -1)
			{
				imu_config_flow_ctl = 3;
			}
			break;
			
		case 3:
			//配置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 = 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;																	//配置流程控制
	
	
	switch(imu_config_flow_ctl)
	{
		case 0:					
			//配置qmc挂起
			if(drv_qmc6310_suspend() == 0){imu_config_flow_ctl = 1;}				
			break;
			
		case 1:
			//配置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));
			if(ret != -1)
			{
				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 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_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 void hal_ser_imu_drv_config_Process(void)
{
	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_drv_config_Process,1);						//全功率配置
			}
			else																															//读数据
			{
				//读取前脚IMU数据 + 更新前脚IMU数据量
				ob_ser_imu.cur_data_num[SER_IMU_DIR_FRONT] = hal_ser_imu_read_data(SER_IMU_DIR_FRONT);

				//读取后脚IMU数据 + 更新前脚IMU数据量
				ob_ser_imu.cur_data_num[SER_IMU_DIR_BACK] = hal_ser_imu_read_data(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_drv_config_Process,0);					//解放线程
				}
			}
		
			break;
	}
	
}

static void hal_ser_imu_printf_Process(void)
{
	SEGGER_RTT_printf(0,"hal_ser_imu----------------------------------------------------------------->\n");
	SEGGER_RTT_printf(0,"stage:0x%x\r",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();
	if(ret == -1)return -1;
	//获取驱动LSM配置参数
	drv_lsm_get_config_param(&ob_ser_imu.drv_param[SER_IMU_DIR_FRONT].lsm);
	
	//初始化驱动QMC
	ret = drv_qmc6310_Init();
	if(ret == -1)return -1;
	//获取驱动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_drv_config_Process",hal_ser_imu_drv_config_Process);
	
	Process_Start(1000,"hal_ser_imu_printf_Process",hal_ser_imu_printf_Process);
	
	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;
}