liweiyan/shoe_mcu1.1.28_v2/hal/fml_imu.c

/**

功耗：

闲置模式功耗为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 "drv_qmc6310_v2.h"
#include "drv_lsm6ds3tr_c.h"
#include "fml_imu.h"

/*Private macro ------------------------------------------------------------------------------------------------------------------------------------*/
#define							FML_IMU_DATA_GROUP_NUM_MAX									20																			//IMU能存储的最大数据组数


/*STRUCTION ------------------------------------------------------------------------------------------------------------------------------------*/
typedef enum {
	FML_IMU_CONFIG_STAGE_DONE,																																						//配置完成阶段
	
	FML_IMU_CONFIG_STAGE_IN_PROGRESS,																																			//配置进行阶段
	
	FML_IMU_CONFIG_STAGE_FAIL,																																						//配置失败阶段
	
} FML_IMU_CONFIG_STAGE_e;

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

typedef struct fml_imu
{
	/*private member*/
	FML_IMU_CONFIG_STAGE_e									stage;																												//配置流程状态
	
	drv_param_u															drv_param[FML_IMU_DIR_NUM];																		//驱动参数组
	
	fml_imu_param_t													config_param[FML_IMU_DIR_NUM];																//配置参数组
	
	int																			cur_data_num[FML_IMU_DIR_NUM];																//当前的数据量
	
	fml_imu_data_t													cur_data[FML_IMU_DIR_NUM][FML_IMU_DATA_GROUP_NUM_MAX];				//当前的IMU数据缓存区
	
	uint32_t																config_flow;																									//配置流程
	
	fml_imu_config_cb												config_cb;																										//配置回调
	
	fml_imu_data_notify_cb									data_notify_cb;																								//数据通知回调
	
} Fml_Imu_t;

/*Local Variable ------------------------------------------------------------------------------------------------------------------------------------*/
static Fml_Imu_t ob_fml_imu;

/*Local Functions ------------------------------------------------------------------------------------------------------------------------------------*/
static void fml_imu_macro_conversion(FML_IMU_DIR_e dir, fml_imu_param_t *config_param, drv_param_u *drv_param)
{
	switch(dir)
	{
		case FML_IMU_DIR_FRONT:
			//acc_odr
			switch(config_param->acc_odr)
			{
				case FML_IMU_ACC_ODR_OFF:
					drv_param->lsm.acc_odr = LSM_ACC_ODR_OFF;
					break;
				case FML_IMU_ACC_ODR_104HZ:
					drv_param->lsm.acc_odr = LSM_ACC_ODR_104HZ;
					break;
				case FML_IMU_ACC_ODR_12HZ5:
					drv_param->lsm.acc_odr = LSM_ACC_ODR_12HZ5;
					break;
			}
			//gry_odr
			switch(config_param->gry_odr)
			{
				case FML_IMU_GRY_ODR_OFF:
					drv_param->lsm.gry_odr =  LSM_GRY_ODR_OFF;
					break;
				case FML_IMU_GRY_ODR_104HZ:
					drv_param->lsm.gry_odr =  LSM_GRY_ODR_104HZ;
					break;
				case FML_IMU_GRY_ODR_12HZ5:
					drv_param->lsm.gry_odr =  LSM_GRY_ODR_12HZ5;
					break;
			}
			//mag_odr
			switch(config_param->mag_odr)
			{
				case FML_IMU_MAG_ODR_OFF:
					drv_param->lsm.mag_odr =  LSM_MAG_ODR_OFF;
					break;
				case FML_IMU_MAG_ODR_10HZ:
					drv_param->lsm.mag_odr =  LSM_MAG_ODR_10HZ;
					break;
				case FML_IMU_MAG_ODR_100HZ:
					drv_param->lsm.mag_odr =  LSM_MAG_ODR_100HZ;
					break;
				case FML_IMU_MAG_ODR_200HZ:
					drv_param->lsm.mag_odr =  LSM_MAG_ODR_200HZ;
					break;
			}
			//fifo_odr
			switch(config_param->fifo_odr)
			{
				case FML_IMU_FIFO_ODR_OFF:
					drv_param->lsm.fifo_odr =  LSM_FIFO_ODR_OFF;
					break;
				case FML_IMU_FIFO_ODR_104HZ:
					drv_param->lsm.fifo_odr =  LSM_FIFO_ODR_104HZ;
					break;
			}
			//acc_power_mode
			switch(config_param->acc_power_mode)
			{
				case FML_IMU_ACC_POWER_MODE_HIGH_PERFORMANCE:
					drv_param->lsm.acc_power_mode =  LSM_ACC_POWER_MODE_HIGH_PERFORMANCE;
					break;
				case FML_IMU_ACC_POWER_MODE_NORMAL:
					drv_param->lsm.acc_power_mode =  LSM_ACC_POWER_MODE_NORMAL;
					break;
			}
			//gry_power_mode
			switch(config_param->gry_power_mode)
			{
				case FML_IMU_GRY_POWER_MODE_HIGH_PERFORMANCE:
					drv_param->lsm.gry_power_mode =  LSM_GRY_POWER_MODE_HIGH_PERFORMANCE;
					break;
				case FML_IMU_GRY_POWER_MODE_NORMAL:
					drv_param->lsm.gry_power_mode =  LSM_GRY_POWER_MODE_NORMAL;
					break;
			}
			//acc_fs
			switch(config_param->acc_fs)
			{
				case FML_IMU_ACC_FS_2G:
					drv_param->lsm.acc_fs =  LSM_ACC_FS_2G;
					break;
				case FML_IMU_ACC_FS_16G:
					drv_param->lsm.acc_fs =  LSM_ACC_FS_16G;
					break;
			}
			//gry_fs
			switch(config_param->gry_fs)
			{
				case FML_IMU_GRY_FS_250DPS:
					drv_param->lsm.gry_fs =  LSM_GRY_FS_250DPS;
					break;
				case FML_IMU_GRY_FS_2000DPS:
					drv_param->lsm.gry_fs =  LSM_GRY_FS_2000DPS;
					break;
			}
			//mag_fs
			switch(config_param->mag_fs)
			{
				case FML_IMU_MAG_FS_30GS:
					drv_param->lsm.mag_fs =  LSM_MAG_FS_30GS;
					break;
			}
			//timestamp_resolution
			switch(config_param->timestamp_resolution)
			{
				case FML_IMU_TIMESTAMP_6MS4:
					drv_param->lsm.timestamp_resolution =  LSM_TIMESTAMP_6MS4;
					break;
				case FML_IMU_TIMESTAMP_25US:
					drv_param->lsm.timestamp_resolution =  LSM_TIMESTAMP_25US;
					break;
			}
			//timestamp_switch
			switch(config_param->timestamp_switch)
			{
				case FML_IMU_TIMESTAMP_OFF:
					drv_param->lsm.timestamp_switch =  LSM_TIMESTAMP_OFF;
					break;
				case FML_IMU_TIMESTAMP_ON:
					drv_param->lsm.timestamp_switch =  LSM_TIMESTAMP_ON;
					break;
			}
			break;
		
		case FML_IMU_DIR_BACK:
			//mag_odr
			switch(config_param->mag_odr)
			{
				case FML_IMU_MAG_ODR_OFF:
					drv_param->qmc.mag_odr =  QMC_MAG_ODR_OFF;
					break;
				case FML_IMU_MAG_ODR_10HZ:
					drv_param->qmc.mag_odr =  QMC_MAG_ODR_10HZ;
					break;
				case FML_IMU_MAG_ODR_100HZ:
					drv_param->qmc.mag_odr =  QMC_MAG_ODR_100HZ;
					break;
				case FML_IMU_MAG_ODR_200HZ:
					drv_param->qmc.mag_odr =  QMC_MAG_ODR_200HZ;
					break;
			}
			//mag_fs
			switch(config_param->mag_fs)
			{
				case FML_IMU_MAG_FS_30GS:
					drv_param->qmc.mag_fs =  QMC_MAG_FS_30GS;
					break;
			}
			break;
		
		default:
			break;
	}
}
static int fml_imu_intergrated_setting(void)
{
	int 					ret;
	int						lsm_mag_flow = drv_lsm_get_mag_odr_flow();
	int						qmc_mag_flow = drv_qmc6310_get_mag_odr_flow();
	drv_param_u  	drv_param;
	
	switch(ob_fml_imu.config_flow)
	{
		case 0:
			//断电lsm
			ret = drv_lsm_power_off();
			if(ret != 0)return -1;
			//断电qmc
			ret = drv_qmc6310_power_off();
			if(ret != 0)return -1;
		
			//清空读取缓存
			ob_fml_imu.cur_data_num[FML_IMU_DIR_FRONT] = 0;
			//清空读取缓存
			ob_fml_imu.cur_data_num[FML_IMU_DIR_BACK] = 0;
		
			ob_fml_imu.config_flow = 1;
			break;
		
		case 1:
			//上电lsm
			ret = drv_lsm_power_on();
			if(ret != 0)return -1;
			//上电qmc
			ret = drv_qmc6310_power_on();
			if(ret != 0)return -1;
			
			ob_fml_imu.config_flow = 2;
			break;
		
		case FML_IMU_CONFIG_FLOW_DONE:
			//配置完成才能进来此处。。。。。。。
			break;
		
		default:
			//转换参数，用于配置驱动
			fml_imu_macro_conversion(FML_IMU_DIR_FRONT, &ob_fml_imu.config_param[FML_IMU_DIR_FRONT], &drv_param);
			fml_imu_macro_conversion(FML_IMU_DIR_BACK, &ob_fml_imu.config_param[FML_IMU_DIR_BACK], &drv_param);
			//配置lsm的地磁计采样频率
			if(ob_fml_imu.config_param[FML_IMU_DIR_FRONT].fifo_odr != FML_IMU_FIFO_ODR_OFF)
			{
				for(int i=1; i <=lsm_mag_flow;i++)
				{
					ret = drv_lsm_set_mag_odr(drv_param.lsm.mag_odr, 1, i);
					if(ret != 0)return -1;
					ob_fml_imu.config_flow++;
				}
			}
			else
			{
				for(int i=1; i <= lsm_mag_flow; i++)
				{
					ret = drv_lsm_set_mag_odr(drv_param.lsm.mag_odr, 0, i);
					if(ret != 0)return -1;
					ob_fml_imu.config_flow++;
				}
			}
			//配置lsm的加速度工作模式
			ret = drv_lsm_set_acc_power_mode(drv_param.lsm.acc_power_mode);
			if(ret != 0)return -1;
			ob_fml_imu.config_flow++;
			//配置lsm的陀螺仪工作模式
			ret = drv_lsm_set_gry_power_mode(drv_param.lsm.gry_power_mode);
			if(ret != 0)return -1;
			ob_fml_imu.config_flow++;
			//配置lsm的加速度量程
			ret = drv_lsm_set_acc_fs(drv_param.lsm.acc_fs);
			if(ret != 0)return -1;
			ob_fml_imu.config_flow++;
			//配置lsm的陀螺仪量程
			ret = drv_lsm_set_gry_fs(drv_param.lsm.gry_fs);
			if(ret != 0)return -1;
			ob_fml_imu.config_flow++;
			//配置lsm的时间戳精度
			ret = drv_lsm_set_timestamp_resolution(drv_param.lsm.timestamp_resolution);
			if(ret != 0)return -1;
			ob_fml_imu.config_flow++;
			//配置lsm的时间戳开关
			ret = drv_lsm_set_timestamp_switch(drv_param.lsm.timestamp_switch);
			if(ret != 0)return -1;
			ob_fml_imu.config_flow++;
			//配置lsm的FIFO采样频率（必须以上都配置成功，才能配置FIFO）
			ret = drv_lsm_set_fifo_odr(drv_param.lsm.fifo_odr, \
																	drv_param.lsm.acc_odr, \
																	drv_param.lsm.gry_odr, \
																	drv_param.lsm.mag_odr, \
																	drv_param.lsm.timestamp_switch);
			if(ret != 0)return -1;
			ob_fml_imu.config_flow++;
			//配置lsm的加速度采样频率
			ret = drv_lsm_set_acc_odr(drv_param.lsm.acc_odr);
			if(ret != 0)return -1;
			ob_fml_imu.config_flow++;
			//配置lsm的陀螺仪采样频率
			ret = drv_lsm_set_gry_odr(drv_param.lsm.gry_odr);
			if(ret != 0)return -1;
			ob_fml_imu.config_flow++;	
			//配置qmc的地磁计采样频率
			for(int i = 1; i <=qmc_mag_flow; i++)
			{
				ret = drv_qmc6310_set_mag_odr(drv_param.qmc.mag_odr,i);
				if(ret != 0)return -1;
				ob_fml_imu.config_flow++;	
			}
			//配置完成！
			ob_fml_imu.config_flow = FML_IMU_CONFIG_FLOW_DONE;	
			break;
	}
	
	return 0;
}
static int fml_imu_read_data_lsm(void)
{
	int 								i;
	int									fifo_group_num;
	lsm_data_t					temp_lsm_data;

	//读取FIFO数据
	if(ob_fml_imu.config_param[FML_IMU_DIR_FRONT].fifo_odr != FML_IMU_FIFO_ODR_OFF)
	{
		//获取当前fifo里存在多少组数据。
		fifo_group_num = drv_lsm_get_fifo_group_num();

		fifo_group_num = fifo_group_num <= FML_IMU_DATA_GROUP_NUM_MAX ? fifo_group_num : FML_IMU_DATA_GROUP_NUM_MAX;
		
		if(fifo_group_num > 0)
		{
			for(i=0; i<fifo_group_num; i++)
			{
				if(drv_lsm_get_fifo_data(&temp_lsm_data) != -1)
				{
					ob_fml_imu.cur_data[FML_IMU_DIR_FRONT][i].acc[0] 					= temp_lsm_data.acc[0];
					ob_fml_imu.cur_data[FML_IMU_DIR_FRONT][i].acc[1] 					= temp_lsm_data.acc[1];
					ob_fml_imu.cur_data[FML_IMU_DIR_FRONT][i].acc[2] 					= temp_lsm_data.acc[2];
					
					ob_fml_imu.cur_data[FML_IMU_DIR_FRONT][i].gry[0] 					= temp_lsm_data.gry[0];
					ob_fml_imu.cur_data[FML_IMU_DIR_FRONT][i].gry[1] 					= temp_lsm_data.gry[1];
					ob_fml_imu.cur_data[FML_IMU_DIR_FRONT][i].gry[2] 					= temp_lsm_data.gry[2];
					
					ob_fml_imu.cur_data[FML_IMU_DIR_FRONT][i].mag[0] 					= temp_lsm_data.mag[0];
					ob_fml_imu.cur_data[FML_IMU_DIR_FRONT][i].mag[1] 					= temp_lsm_data.mag[1];
					ob_fml_imu.cur_data[FML_IMU_DIR_FRONT][i].mag[2] 					= temp_lsm_data.mag[2];
					
					ob_fml_imu.cur_data[FML_IMU_DIR_FRONT][i].fifo_timestamp 	= temp_lsm_data.fifo_timestamp;
				}
				else
				{
					break;
				}
			}
		}
		else
		{
			return 0;
		}
		
		return i;
	}
	//只读取ACC数据
	else if(ob_fml_imu.config_param[FML_IMU_DIR_FRONT].acc_odr != FML_IMU_ACC_ODR_OFF && \
					ob_fml_imu.config_param[FML_IMU_DIR_FRONT].gry_odr == FML_IMU_GRY_ODR_OFF && \
					ob_fml_imu.config_param[FML_IMU_DIR_FRONT].mag_odr == FML_IMU_MAG_ODR_OFF \
				 )
	{
		if(drv_lsm_get_acc_data(&temp_lsm_data) != -1)
		{
			ob_fml_imu.cur_data[FML_IMU_DIR_FRONT][0].acc[0] 					= temp_lsm_data.acc[0];
			ob_fml_imu.cur_data[FML_IMU_DIR_FRONT][0].acc[1] 					= temp_lsm_data.acc[1];
			ob_fml_imu.cur_data[FML_IMU_DIR_FRONT][0].acc[2] 					= temp_lsm_data.acc[2];
			
			return 1;
		}
	}
	
	return 0;
}
static int fml_imu_read_data_qmc(void)
{
	qmc_data_t	temp_qmc_data;
	
	if(ob_fml_imu.config_param[FML_IMU_DIR_BACK].mag_odr != FML_IMU_MAG_ODR_OFF)
	{
		if(drv_qmc6310_get_mag_data(&temp_qmc_data) != -1)
		{
			ob_fml_imu.cur_data[FML_IMU_DIR_BACK][0].mag[0] = temp_qmc_data.mag[0];
			ob_fml_imu.cur_data[FML_IMU_DIR_BACK][0].mag[1] = temp_qmc_data.mag[1];
			ob_fml_imu.cur_data[FML_IMU_DIR_BACK][0].mag[2] = temp_qmc_data.mag[2];
			
			return 1;
		}
	}

	return 0;
}
static int hal_ser_imu_read_data(FML_IMU_DIR_e dir)
{
	int ret = 0;
	
	switch(dir)
	{
		case FML_IMU_DIR_FRONT:
			ret =  fml_imu_read_data_lsm();
			break;
		
		case FML_IMU_DIR_BACK:
			ret =  fml_imu_read_data_qmc();
			break;
		
		default:
			break;
	}
	
	return ret;
}
static void fml_imu_Process(void)
{
	int 			data_num[FML_IMU_DIR_NUM];
	uint32_t	dir_bit;
	
	switch(ob_fml_imu.stage)
	{
		case FML_IMU_CONFIG_STAGE_DONE:																																			//配置完成阶段
			dir_bit = 0;
			//读取前脚IMU数据 + 更新前脚IMU数据量
			data_num[FML_IMU_DIR_FRONT] = hal_ser_imu_read_data(FML_IMU_DIR_FRONT);
			if(data_num[FML_IMU_DIR_FRONT] > 0){
				ob_fml_imu.cur_data_num[FML_IMU_DIR_FRONT] = data_num[FML_IMU_DIR_FRONT];
				dir_bit |= (1 << FML_IMU_DIR_FRONT);
			}
			//读取后脚IMU数据 + 更新后脚IMU数据量
			data_num[FML_IMU_DIR_BACK] = hal_ser_imu_read_data(FML_IMU_DIR_BACK);
			if(data_num[FML_IMU_DIR_BACK] > 0){
				ob_fml_imu.cur_data_num[FML_IMU_DIR_BACK] = data_num[FML_IMU_DIR_BACK];
				dir_bit |= (1 << FML_IMU_DIR_BACK);
			}
			//数据回调通知
			if(dir_bit != 0)ob_fml_imu.data_notify_cb(dir_bit);
			break;
		
		case FML_IMU_CONFIG_STAGE_IN_PROGRESS:																															//配置进行阶段
			if(fml_imu_intergrated_setting() == -1)
			{
				//配置失败
				ob_fml_imu.stage = FML_IMU_CONFIG_STAGE_FAIL;
				//断电前后脚
				fml_imu_close(FML_IMU_DIR_FRONT);
				fml_imu_close(FML_IMU_DIR_BACK);
				//通知配置失败
				ob_fml_imu.config_cb(ob_fml_imu.config_flow);
			}
			else
			{
				if(ob_fml_imu.config_flow == FML_IMU_CONFIG_FLOW_DONE)
				{
					//配置成功
					ob_fml_imu.stage = FML_IMU_CONFIG_STAGE_DONE;
					//通知配置成功
					ob_fml_imu.config_cb(ob_fml_imu.config_flow);
				}
			}
			break;
		
		default:																																			
			break;
	}
}

/*API ------------------------------------------------------------------------------------------------------------------------------------*/
/**
	@brief 		初始化IMU功能模块
	@param 		无
	@return 	错误代码					- [out]	-1失败，0成功
*/
int	fml_imu_Init(void)
{
	int ret;
	
	/***************************************驱动层初始化***************************************************/
	
	//重置结构体
	memset(&ob_fml_imu,0,sizeof(ob_fml_imu));		
	
	//初始化驱动LSM
	ret = drv_lsm_Init();
	//初始化驱动QMC
	ret += drv_qmc6310_Init();
	
	//获取驱动LSM配置参数
	drv_lsm_get_config_param(&ob_fml_imu.drv_param[FML_IMU_DIR_FRONT].lsm);
	
	//获取驱动LSM配置参数
	drv_qmc6310_get_config_param(&ob_fml_imu.drv_param[FML_IMU_DIR_BACK].qmc);
	
	/***************************************业务逻辑层初始化***************************************************/
	
	//初始化IMU服务结构体
	ob_fml_imu.stage = FML_IMU_CONFIG_STAGE_DONE;


	//设置驱动配置线程
	Process_Start(0,"fml_imu_Process",fml_imu_Process);
	
	if(ret < 0)return -1;
	
	return 0;
	
}
/**
	@brief 		设置需要配置IMU的参数
	@param 		param 						- [in]	设置IMU的参数
	@return 	错误代码					- [out]	-1失败，0成功
*/
int fml_imu_config_param(FML_IMU_DIR_e dir, fml_imu_param_t *param)
{
	if(param == NULL || dir >= FML_IMU_DIR_NUM)return -1;
	
	memcpy(&ob_fml_imu.config_param[dir],param,sizeof(fml_imu_param_t));
	
	return 0;
}
/**
	@brief 		开始配置IMU
	@param 		param 						无
	@return 	错误代码					- [out]	-1失败，0成功
*/
int fml_imu_start_config(void)
{
	drv_param_u  	drv_param;
	
	//获取驱动LSM配置参数
	drv_lsm_get_config_param(&ob_fml_imu.drv_param[FML_IMU_DIR_FRONT].lsm);
	
	//获取驱动LSM配置参数
	drv_qmc6310_get_config_param(&ob_fml_imu.drv_param[FML_IMU_DIR_BACK].qmc);
	
	//转换参数
	fml_imu_macro_conversion(FML_IMU_DIR_FRONT, &ob_fml_imu.config_param[FML_IMU_DIR_FRONT], &drv_param);
	fml_imu_macro_conversion(FML_IMU_DIR_BACK, &ob_fml_imu.config_param[FML_IMU_DIR_BACK], &drv_param);
	
	//进行对比
	if((memcmp(&ob_fml_imu.drv_param[FML_IMU_DIR_FRONT].lsm,&drv_param.lsm,sizeof(drv_param.lsm)) == 0) || \
		 (memcmp(&ob_fml_imu.drv_param[FML_IMU_DIR_BACK].qmc,&drv_param.qmc,sizeof(drv_param.qmc)) == 0)
		)
	{
		ob_fml_imu.stage = FML_IMU_CONFIG_STAGE_IN_PROGRESS;
		ob_fml_imu.config_flow = 0;
	}
	
	return 0;
}
/**
	@brief 		注册IMU配置结果回调函数
	@param 		cb 								- [in]	回调函数
	@return 	错误代码					- [out]	-1失败，0成功
*/
int fml_imu_register_config_callback(fml_imu_config_cb cb)
{
	if(cb == NULL)return -1;
	
	ob_fml_imu.config_cb = cb;
	
	return 0;
}
/**
	@brief 		注册IMU数据通知回调函数
	@param 		cb 								- [in]	回调函数
	@return 	错误代码					- [out]	-1失败，0成功
*/
int fml_imu_register_data_notify_callback(fml_imu_data_notify_cb cb)
{
	if(cb == NULL)return -1;
	
	ob_fml_imu.data_notify_cb = cb;
	
	return 0;
}
/**
	@brief 		读取当前IMU数据的数量
	@param 		dir 							- [in]	方向
	@return 	返回当前IMU数据的数量
*/
int fml_imu_get_data_num(FML_IMU_DIR_e dir)
{
	if(dir >= FML_IMU_DIR_NUM)return 0;
	
	return ob_fml_imu.cur_data_num[dir];
}
/**
	@brief 		获取当前IMU数据
	@param 		dir 							- [in]	方向
	@param 		index 						- [in]	数据索引
	@param 		pdata 						- [out]	返回的IMU数据指针
	@return 	错误代码					- [out]	-1失败，0成功
*/
int fml_imu_get_data(FML_IMU_DIR_e dir, int index, fml_imu_data_t *pdata)
{
	if(index < 0 || pdata == NULL || dir >= FML_IMU_DIR_NUM)return -1;
	
	*pdata = ob_fml_imu.cur_data[dir][index];
	
	return 0;
}
/**
	@brief 		关闭IMU
	@param 		dir 							- [in]	方向
	@return 	错误代码					- [out]	-1失败，0成功
*/
int fml_imu_close(FML_IMU_DIR_e dir)
{
	if(dir >= FML_IMU_DIR_NUM)return -1;
	
	switch(dir)
	{
		case FML_IMU_DIR_FRONT:
			drv_lsm_power_on();			//主要是复位结构体
			drv_lsm_power_off();
			break;
		
		case FML_IMU_DIR_BACK:
			drv_qmc6310_power_on();	//主要是复位结构体
			drv_qmc6310_power_off();
			break;
		
		default:
			break;
	}

	return 0;
}