liweiyan/shoe_mcu1.1.28_v4/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 "tool.h"
#include "nrf_delay.h"
#include "bsp_time.h"
#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能存储的最大数据组数

#define 						FML_IMU_MONITOR_DATA_ERR_SUM_MAX						10																			//数据监测错误累计最大值


/*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;																								//数据通知回调
	
	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[FML_IMU_DIR_NUM];												//异常数据产生次数累计
	
} 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  			lsm_drv_param;
	drv_param_u  			qmc_drv_param;
	static uint32_t 	tim = 0;
	
	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;
			
			tim = 0;
			ob_fml_imu.config_flow = 1;
			break;
		
		case 1:
			//等待200ms
			if(tim == 0){
				tim = TIME_GetTicks();
			}else if(TIME_GetTicks()-tim>=200){
				tim = 0;
				ob_fml_imu.config_flow = 2;
			}
			break;

		case 2:
			//上电lsm
			ret = drv_lsm_power_on();
			if(ret != 0)return -1;
			//上电qmc
			ret = drv_qmc6310_power_on();
			if(ret != 0)return -1;
		
			tim = 0;
			ob_fml_imu.config_flow = 3;
			break;
		
		case 3:
			//等待200ms
			if(tim == 0){
				tim = TIME_GetTicks();
			}else if(TIME_GetTicks()-tim>=200){
				tim = 0;
				ob_fml_imu.config_flow = 4;
			}
			break;
			
		case 4:
			//lsm进行自检步骤1
			ret = drv_lsm_self_check_1();
			if(ret != 0)return -1;
			tim = 0;
			ob_fml_imu.config_flow = 5;
			break;
		
		case 5:
			//等待100ms
			if(tim == 0){
				tim = TIME_GetTicks();
			}else if(TIME_GetTicks()-tim>=100){
				tim = 0;
				ob_fml_imu.config_flow = 6;
			}
			break;
		
		case 6:
			//lsm进行自检步骤2
			ret = drv_lsm_self_check_2();
			if(ret != 0)return -1;
			tim = 0;
			ob_fml_imu.config_flow = 7;
			break;
		
		case 7:
			//等待100ms
			if(tim == 0){
				tim = TIME_GetTicks();
			}else if(TIME_GetTicks()-tim>=100){
				tim = 0;
				ob_fml_imu.config_flow = 8;
			}
			break;
		
		case 8:
			//lsm进行自检步骤3
			ret = drv_lsm_self_check_3();
			if(ret != 0)return -1;
			tim = 0;
			ob_fml_imu.config_flow = 9;
			break;
		
		case 9:
			//等待150ms
			if(tim == 0){
				tim = TIME_GetTicks();
			}else if(TIME_GetTicks()-tim>=150){
				tim = 0;
				ob_fml_imu.config_flow = 10;
			}
			break;
		
		case 10:
			//lsm进行自检步骤4
			ret = drv_lsm_self_check_4();
			if(ret != 0)return -1;
			tim = 0;
			ob_fml_imu.config_flow = 11;
			break;
		
		case 11:
			//等待50ms
			if(tim == 0){
				tim = TIME_GetTicks();
			}else if(TIME_GetTicks()-tim>=50){
				tim = 0;
				ob_fml_imu.config_flow = 12;
			}
			break;
		
		case 12:
			//lsm进行自检步骤5
			ret = drv_lsm_self_check_5();
			if(ret != 0)return -1;
			tim = 0;
			ob_fml_imu.config_flow = 13;
			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], &lsm_drv_param);
			fml_imu_macro_conversion(FML_IMU_DIR_BACK, &ob_fml_imu.config_param[FML_IMU_DIR_BACK], &qmc_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(lsm_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(lsm_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(lsm_drv_param.lsm.acc_power_mode);
			if(ret != 0)return -1;
			ob_fml_imu.config_flow++;
			//配置lsm的陀螺仪工作模式
			ret = drv_lsm_set_gry_power_mode(lsm_drv_param.lsm.gry_power_mode);
			if(ret != 0)return -1;
			ob_fml_imu.config_flow++;
			//配置lsm的加速度量程
			ret = drv_lsm_set_acc_fs(lsm_drv_param.lsm.acc_fs);
			if(ret != 0)return -1;
			ob_fml_imu.config_flow++;
			//配置lsm的陀螺仪量程
			ret = drv_lsm_set_gry_fs(lsm_drv_param.lsm.gry_fs);
			if(ret != 0)return -1;
			ob_fml_imu.config_flow++;
			//配置lsm的时间戳精度
			ret = drv_lsm_set_timestamp_resolution(lsm_drv_param.lsm.timestamp_resolution);
			if(ret != 0)return -1;
			ob_fml_imu.config_flow++;
			//配置lsm的时间戳开关
			ret = drv_lsm_set_timestamp_switch(lsm_drv_param.lsm.timestamp_switch);
			if(ret != 0)return -1;
			ob_fml_imu.config_flow++;
			//配置lsm的FIFO采样频率（必须以上都配置成功，才能配置FIFO）
			ret = drv_lsm_set_fifo_odr(lsm_drv_param.lsm.fifo_odr, \
																	lsm_drv_param.lsm.acc_odr, \
																	lsm_drv_param.lsm.gry_odr, \
																	lsm_drv_param.lsm.mag_odr, \
																	lsm_drv_param.lsm.timestamp_switch);
			if(ret != 0)return -1;
			ob_fml_imu.config_flow++;
			//配置lsm的加速度采样频率
			ret = drv_lsm_set_acc_odr(lsm_drv_param.lsm.acc_odr);
			if(ret != 0)return -1;
			ob_fml_imu.config_flow++;
			//配置lsm的陀螺仪采样频率
			ret = drv_lsm_set_gry_odr(lsm_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(qmc_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 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_fml_imu.last_f_acc[0] == f_acc[0] && \
				ob_fml_imu.last_f_acc[1] == f_acc[1] && \
				ob_fml_imu.last_f_acc[2] == f_acc[2]
			)
		{
			ob_fml_imu.last_f_acc_err_sum++;
			
			if(ob_fml_imu.last_f_acc_err_sum >= FML_IMU_MONITOR_DATA_ERR_SUM_MAX){
				Except_SetExceptype(EXCEPT_DATA_FRONT_ACC);
			}
			
		}else{
			ob_fml_imu.last_f_acc_err_sum = 0;
		}
		
		ob_fml_imu.last_f_acc[0] = f_acc[0];
		ob_fml_imu.last_f_acc[1] = f_acc[1];
		ob_fml_imu.last_f_acc[2] = f_acc[2];
	}
	/*前脚陀螺仪*/
	if(f_gry != NULL)
	{
		if(
				ob_fml_imu.last_f_gry[0] == f_gry[0] && \
				ob_fml_imu.last_f_gry[1] == f_gry[1] && \
				ob_fml_imu.last_f_gry[2] == f_gry[2]
			)
		{
			ob_fml_imu.last_f_gry_err_sum++;
			
			if(ob_fml_imu.last_f_gry_err_sum >= FML_IMU_MONITOR_DATA_ERR_SUM_MAX){
				Except_SetExceptype(EXCEPT_DATA_FRONT_GRY);
			}
				
		}else{
			ob_fml_imu.last_f_gry_err_sum = 0;
		}
		
		ob_fml_imu.last_f_gry[0] = f_gry[0];
		ob_fml_imu.last_f_gry[1] = f_gry[1];
		ob_fml_imu.last_f_gry[2] = f_gry[2];
	}
	/*前脚地磁计*/
	if(f_mag != NULL)
	{
		if(
				ob_fml_imu.last_f_mag[0] == f_mag[0] && \
				ob_fml_imu.last_f_mag[1] == f_mag[1] && \
				ob_fml_imu.last_f_mag[2] == f_mag[2]
			)
		{
			ob_fml_imu.last_f_mag_err_sum++;
			
			if(ob_fml_imu.last_f_mag_err_sum >= FML_IMU_MONITOR_DATA_ERR_SUM_MAX){
				Except_SetExceptype(EXCEPT_DATA_FRONT_MAG);
			}
				
		}else{
			ob_fml_imu.last_f_mag_err_sum = 0;
		}
		
		ob_fml_imu.last_f_mag[0] = f_mag[0];
		ob_fml_imu.last_f_mag[1] = f_mag[1];
		ob_fml_imu.last_f_mag[2] = f_mag[2];
	}
	/*后脚地磁计*/
	if(b_mag != NULL)
	{
		if(
				ob_fml_imu.last_b_mag[0] == b_mag[0] && \
				ob_fml_imu.last_b_mag[1] == b_mag[1] && \
				ob_fml_imu.last_b_mag[2] == b_mag[2]
			)
		{
			ob_fml_imu.last_b_mag_err_sum++;
			
			if(ob_fml_imu.last_b_mag_err_sum >= FML_IMU_MONITOR_DATA_ERR_SUM_MAX){
				Except_SetExceptype(EXCEPT_DATA_BACK_MAG);
			}
				
		}else{
			ob_fml_imu.last_b_mag_err_sum = 0;
		}
		
		ob_fml_imu.last_b_mag[0] = b_mag[0];
		ob_fml_imu.last_b_mag[1] = b_mag[1];
		ob_fml_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_fml_imu.except_data_occur_sum[FML_IMU_DIR_FRONT]++;
		
		if(ob_fml_imu.except_data_occur_sum[FML_IMU_DIR_FRONT] > 1)
		{
			if(Except_IsError(EXCEPT_DATA_FRONT_ACC)){Except_TxError(EXCEPT_DATA_FRONT_ACC,"front_acc_data_error");DEBUG_LOG("front_acc_data_error\n");}
			if(Except_IsError(EXCEPT_DATA_FRONT_GRY)){Except_TxError(EXCEPT_DATA_FRONT_GRY,"front_gry_data_error");DEBUG_LOG("front_gry_data_error\n");}
			if(Except_IsError(EXCEPT_DATA_FRONT_MAG)){Except_TxError(EXCEPT_DATA_FRONT_MAG,"front_mag_data_error");DEBUG_LOG("front_mag_data_error\n");}
		}
		//重启前脚LSM（主要是复位结构体）
		drv_lsm_power_off();
		drv_lsm_power_on();
		//重新开始配置
		fml_imu_start_config();
		//清除异常
		Except_ClearExceptype(EXCEPT_DATA_FRONT_ACC);
		Except_ClearExceptype(EXCEPT_DATA_FRONT_GRY);
		Except_ClearExceptype(EXCEPT_DATA_FRONT_MAG);					
	}
	else
	{
		ob_fml_imu.except_data_occur_sum[FML_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_fml_imu.except_data_occur_sum[FML_IMU_DIR_BACK]++;
		
		if(ob_fml_imu.except_data_occur_sum[FML_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();
		//重新开始配置
		fml_imu_start_config();		
		//清除异常
		Except_ClearExceptype(EXCEPT_DATA_BACK_MAG);		
	}
	else
	{
		ob_fml_imu.except_data_occur_sum[FML_IMU_DIR_BACK] = 0;
		//清除异常
		Except_ClearExceptype(EXCEPT_DATA_BACK_MAG);		
	}

}
static void fml_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];
	fml_imu_data_t		data;
	static uint32_t		last_tim = 0;
	
	if(
			ob_fml_imu.stage == FML_IMU_CONFIG_STAGE_DONE && \
			ob_fml_imu.drv_param[FML_IMU_DIR_FRONT].lsm.acc_odr == LSM_ACC_ODR_104HZ && \
			ob_fml_imu.drv_param[FML_IMU_DIR_FRONT].lsm.gry_odr == LSM_GRY_ODR_104HZ && \
			(ob_fml_imu.drv_param[FML_IMU_DIR_FRONT].lsm.mag_odr == LSM_MAG_ODR_200HZ || ob_fml_imu.drv_param[FML_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 = fml_imu_get_data_num(FML_IMU_DIR_FRONT);
			
			if(group_num > 0)
			{
				for(i=0;i<group_num;i++)
				{
					fml_imu_get_data(FML_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);
				}
			}

			
			if(fml_imu_get_data_num(FML_IMU_DIR_BACK) >= 1){
				fml_imu_get_data(FML_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_fml_imu.stage == FML_IMU_CONFIG_STAGE_DONE && \
						ob_fml_imu.drv_param[FML_IMU_DIR_FRONT].lsm.acc_odr == LSM_ACC_ODR_104HZ && \
						(ob_fml_imu.drv_param[FML_IMU_DIR_FRONT].lsm.mag_odr == LSM_MAG_ODR_200HZ || ob_fml_imu.drv_param[FML_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 = fml_imu_get_data_num(FML_IMU_DIR_FRONT);
			
			if(group_num > 0)
			{
				for(i=0;i<group_num;i++)
				{
					fml_imu_get_data(FML_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 if(ob_fml_imu.stage == FML_IMU_CONFIG_STAGE_DONE && ob_fml_imu.drv_param[FML_IMU_DIR_FRONT].lsm.acc_odr == LSM_ACC_ODR_12HZ5)
	{
		if(TIME_GetTicks()-last_tim >= StandByPower_Interval)		//监测前脚传感器数据（acc）
		{
			last_tim = TIME_GetTicks();
			
			group_num = fml_imu_get_data_num(FML_IMU_DIR_FRONT);
			
			if(group_num > 0)
			{
				for(i=0;i<group_num;i++)
				{
					fml_imu_get_data(FML_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);		
				}
			}
			
		}
	}
}
static void fml_imu_monitor_sensor_no_data_process(int cur_front_data_num, int cur_back_data_num)
{
	static uint32_t		front_no_data_tim = 0;
	static uint32_t		back_no_data_tim = 0;
	static char 			buf[255];
	
	//数据异常检测——前脚没有数据
	if(cur_front_data_num == 0)
	{
		if(ob_fml_imu.drv_param[FML_IMU_DIR_FRONT].lsm.acc_odr != LSM_ACC_ODR_OFF || \
			ob_fml_imu.drv_param[FML_IMU_DIR_FRONT].lsm.gry_odr != LSM_GRY_ODR_OFF || \
			ob_fml_imu.drv_param[FML_IMU_DIR_FRONT].lsm.mag_odr != LSM_MAG_ODR_OFF)
		{
			//持续1000ms没数据
			if(front_no_data_tim == 0){
				front_no_data_tim = TIME_GetTicks();
			}else if(TIME_GetTicks()-front_no_data_tim>=1000){
				sprintf(buf,"Front Sensor short circuit\r\n");
				Except_TxError(EXCEPT_IMU_SUSPEND_OVERFLOW,buf);
				front_no_data_tim = 0;
				//重启前脚LSM（主要是复位结构体）
				drv_lsm_power_off();
				drv_lsm_power_on();
				//重新开始配置
				fml_imu_start_config();
			}
		}
	}
	else front_no_data_tim = 0;
	
	
	//数据异常检测——后脚没有数据
	if(cur_back_data_num == 0)
	{
		if(ob_fml_imu.drv_param[FML_IMU_DIR_BACK].qmc.mag_odr != QMC_MAG_ODR_OFF)
		{
			//持续1000ms没数据
			if(back_no_data_tim == 0){
				back_no_data_tim = TIME_GetTicks();
			}else if(TIME_GetTicks()-back_no_data_tim>=1000){
				sprintf(buf,"Back Sensor short circuit\r\n");
				Except_TxError(EXCEPT_IMU_SUSPEND_OVERFLOW,buf);
				back_no_data_tim = 0;
				//重启后脚QMC（主要是复位结构体）
				drv_qmc6310_power_off();
				drv_qmc6310_power_on();
				//重新开始配置
				fml_imu_start_config();	
			}
		}
	}
	else back_no_data_tim = 0;
}
static void fml_imu_Process(void)
{
	int 							data_num[FML_IMU_DIR_NUM];
	uint32_t					dir_bit;
	static char 			buf[255];
	static uint32_t   config_err_report_tim = 0;
	
	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);
			}
			//数据异常检测——没有数据
			fml_imu_monitor_sensor_no_data_process(data_num[FML_IMU_DIR_FRONT], data_num[FML_IMU_DIR_BACK]);
			//数据异常检测——重复数据
			fml_imu_monitor_sensor_data_process();
			//数据回调通知
			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);
				//上报异常
				sprintf(buf,"front_imu_suspend_overflow:0x%x\r\n",ob_fml_imu.config_flow);
				Except_TxError(EXCEPT_IMU_SUSPEND_OVERFLOW,buf);
				//解除全功率运行
				Process_SetHoldOn(fml_imu_Process,0);
			}
			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);
					//更新驱动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);
					//解除全功率运行
					Process_SetHoldOn(fml_imu_Process,0);
				}
			}
			break;
		
		default:
			//间隔1000ms上报配置失败错误
			if(config_err_report_tim == 0){
				config_err_report_tim = TIME_GetTicks();
			}else if(TIME_GetTicks()-config_err_report_tim>=1000){
				sprintf(buf,"front_imu_suspend_overflow:0x%x\r\n",ob_fml_imu.config_flow);
				Except_TxError(EXCEPT_IMU_SUSPEND_OVERFLOW,buf);
				config_err_report_tim = 0;
			}
			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 		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 		param 						无
	@return 	错误代码					- [out]	-1失败，0成功
*/
int fml_imu_start_config(void)
{
	drv_param_u  			lsm_drv_param;
	drv_param_u  			qmc_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], &lsm_drv_param);
	fml_imu_macro_conversion(FML_IMU_DIR_BACK, &ob_fml_imu.config_param[FML_IMU_DIR_BACK], &qmc_drv_param);

	//进行对比
	if((memcmp(&ob_fml_imu.drv_param[FML_IMU_DIR_FRONT].lsm,&lsm_drv_param.lsm,sizeof(lsm_drv_param.lsm)) != 0) || \
		 (memcmp(&ob_fml_imu.drv_param[FML_IMU_DIR_BACK].qmc,&qmc_drv_param.qmc,sizeof(qmc_drv_param.qmc)) != 0)
		)
	{
		ob_fml_imu.stage = FML_IMU_CONFIG_STAGE_IN_PROGRESS;
		ob_fml_imu.config_flow = 0;
		//全功率运行进行配置
		Process_SetHoldOn(fml_imu_Process,1);
		DEBUG_LOG("------------------------------------------------------------->enter fml_imu_start_config\n");
		nrf_delay_ms(3);
	}
	
	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;
}