shoe_mcu/drivers/drv_qmc6310_v2.c

/*Includes ----------------------------------------------*/
#include "drv_iic_back.h"
#include "nrf_delay.h"
#include "drv_qmc6310_v2.h"
#include "exception.h"
#include "system.h"


/*Private macro ------------------------------------------------*/
/* vendor chip id*/

#define QMC6310U_IIC_ADDR							(0x1c<<1)
#define QMC6310N_IIC_ADDR							(0x3c<<1)
#define QMC6310_CHIP_ID_REG						0x00

/*data output register*/
#define QMC6310_DATA_OUT_X_LSB_REG		0x01
#define QMC6310_DATA_OUT_X_MSB_REG		0x02
#define QMC6310_DATA_OUT_Y_LSB_REG		0x03
#define QMC6310_DATA_OUT_Y_MSB_REG		0x04
#define QMC6310_DATA_OUT_Z_LSB_REG		0x05
#define QMC6310_DATA_OUT_Z_MSB_REG		0x06
/*Status registers */
#define QMC6310_STATUS_REG						0x09
/* configuration registers */
#define QMC6310_CTL_REG_ONE						0x0A  /* Contrl register one */
#define QMC6310_CTL_REG_TWO						0x0B  /* Contrl register two */
	
/* Magnetic Sensor Operating Mode MODE[1:0]*/
#define QMC6310_SUSPEND_MODE					0x00
#define QMC6310_NORMAL_MODE						0x01
#define QMC6310_SINGLE_MODE						0x02
#define QMC6310_H_PFM_MODE						0x03

/*data output rate OSR2[2:0]*/
#define OUTPUT_DATA_RATE_800HZ 				0x00
#define OUTPUT_DATA_RATE_400HZ 				0x01
#define OUTPUT_DATA_RATE_200HZ 				0x02
#define OUTPUT_DATA_RATE_100HZ 				0x03

/*oversample Ratio  OSR[1]*/
#define OVERSAMPLE_RATE_256						0x01
#define OVERSAMPLE_RATE_128 					0x00

#define SET_RESET_ON 									0x00
#define SET_ONLY_ON 									0x01
#define SET_RESET_OFF 								0x02

#define QMC6310_DEFAULT_DELAY					200

#define QMC6310_MAXHZ									0xC3		
#define QMC6310_200HZ									0x3D		
#define QMC6310_100HZ									0x39		
#define QMC6310_10HZ									0x31


/*STRUCTION -----------------------------------------------------*/
typedef struct drv_qmc6310
{
	bool											(*write)(uint8_t add,uint8_t reg,uint8_t* p,uint8_t len);							//IIC - 写
	
	bool											(*read)(uint8_t add,uint8_t reg,uint8_t* p,uint8_t len);							//IIC - 读
	
	qmc_data_t								cur_data;																															//当前QMC的数据
	
	drv_qmc_config_param_t		cur_param;																														//当前QMC的配置
	
} Drv_Qmc6310_t;


/*Local Variable ----------------------------------------------*/
static Drv_Qmc6310_t	ob_qmc6310;
static uint32_t 			iic_write_error_counter = 0;								//iic写错误上报
static uint32_t 			iic_read_error_counter = 0;									//iic读错误上报

/*Local Functions ----------------------------------------------*/
static bool qmc6310_get_chipid(void)
{
	bool		ret;	
	uint8_t chipid = 0xFF;
	
	ret = ob_qmc6310.read(QMC6310U_IIC_ADDR,QMC6310_CHIP_ID_REG,&chipid,1);
	
	if(ret == false)
	{
		return false;
	}
	
//	SEGGER_RTT_printf(0,"chipid:%d\r\n",chipid);
	
	if(chipid != 0x80)
	{
		return false;
	}

	return true;
}

static bool platform_read(uint8_t add,uint8_t reg,uint8_t* p,uint8_t len)
{
	bool ierror;
	
	ierror = IIC_BACK_ReadBytes(add,reg,p,len);
	
	if(ierror != true)
	{
		iic_read_error_counter++;
	}
	
	return ierror;
}

static bool platform_write(uint8_t add,uint8_t reg,uint8_t* p,uint8_t len)
{
	bool ierror;
	
	ierror = IIC_BACK_WriteBytes(add,reg,p,len);
	
	if(ierror != true)
	{
		iic_write_error_counter++;
	}
	
	return ierror;
}

static void drv_qmc_iic_error_report_process(void)
{
	char buff[30]={0};
	
	if(iic_write_error_counter > 0)
	{
		sprintf(buff,"iic_write_err,%d\r\n",iic_write_error_counter);
		Except_TxError(EXCEPT_IIC_RW,(const char *)buff);
	}
	
	if(iic_read_error_counter > 0)
	{
		sprintf(buff,"iic_read_err,%d\r\n",iic_read_error_counter);
		Except_TxError(EXCEPT_IIC_RW,(const char *)buff);
	}

	iic_write_error_counter = 0;
	iic_read_error_counter = 0;
}


/*API ----------------------------------------------*/
/**
	@brief 		初始化QMC6310驱动
	@param 		无
	@return 	错误代码							- [out]	-1失败，0成功
*/
int drv_qmc6310_Init(void)
{
	nrf_gpio_cfg(
	PIN_BACK_SENSE_POWER,
	NRF_GPIO_PIN_DIR_OUTPUT,
	NRF_GPIO_PIN_INPUT_DISCONNECT,
	NRF_GPIO_PIN_NOPULL,
	NRF_GPIO_PIN_H0H1,
	NRF_GPIO_PIN_NOSENSE);

	nrf_gpio_cfg_output(PIN_BACK_SCL);
	nrf_gpio_cfg_output(PIN_BACK_SDA);
	nrf_gpio_pin_write(PIN_BACK_SCL,0);
	nrf_gpio_pin_write(PIN_BACK_SDA,0);
	
	//供电
	nrf_gpio_pin_write(PIN_BACK_SENSE_POWER,0);
	nrf_delay_ms(5);
	nrf_gpio_pin_write(PIN_BACK_SENSE_POWER,1);
	nrf_delay_ms(5);
	//初始化IIC错误上报
	Process_Start(1000,"qmc_iic_error_report",drv_qmc_iic_error_report_process);
	//初始化IIC
	IIC_BACK_Init();
	//初始化结构体
	memset(&ob_qmc6310, 0, sizeof(ob_qmc6310));
	
	ob_qmc6310.read							 		= platform_read;
	ob_qmc6310.write 								= platform_write;
	ob_qmc6310.cur_param.mag_fs 		= QMC_MAG_FS_30GS;
	ob_qmc6310.cur_param.mag_odr 		= QMC_MAG_ODR_OFF;
	
	if(qmc6310_get_chipid())
	{
		return 0;
	}
	
	return -1;
}
/**
	@brief 		QMC6310断电
	@param 		无
	@return 	错误代码							- [out]	-1失败，0成功
*/
int drv_qmc6310_power_off(void)
{
	nrf_gpio_cfg(
	PIN_BACK_SENSE_POWER,
	NRF_GPIO_PIN_DIR_OUTPUT,
	NRF_GPIO_PIN_INPUT_DISCONNECT,
	NRF_GPIO_PIN_NOPULL,
	NRF_GPIO_PIN_H0H1,
	NRF_GPIO_PIN_NOSENSE);

	nrf_gpio_cfg_output(PIN_BACK_SCL);
	nrf_gpio_cfg_output(PIN_BACK_SDA);
	nrf_gpio_pin_write(PIN_BACK_SCL,0);
	nrf_gpio_pin_write(PIN_BACK_SDA,0);
	
	//供电
	nrf_gpio_pin_write(PIN_BACK_SENSE_POWER,0);
	
	return 0;
}
/**
	@brief 		QMC6310上电（默认配置挂起）
	@param 		无
	@return 	错误代码							- [out]	-1失败，0成功
*/
int drv_qmc6310_power_on(void)
{
	nrf_gpio_pin_write(PIN_BACK_SENSE_POWER,1);
	
	//初始化IIC
	IIC_BACK_Init();
	memset(&ob_qmc6310.cur_data,0,sizeof(ob_qmc6310.cur_data));
	ob_qmc6310.cur_param.mag_fs 		= QMC_MAG_FS_30GS;
	ob_qmc6310.cur_param.mag_odr 		= QMC_MAG_ODR_OFF;
	
	return 0;
}
/**
	@brief 		设置MAG量程
	@param 		mag_fs 								- [in]	MAG量程
	@return 	错误代码							- [out]	-1失败，0成功
*/
int	drv_qmc6310_set_mag_fs(QMC_MAG_FS_e mag_fs)
{
	bool 		ret;
	uint8_t data;
	
	if(ob_qmc6310.cur_param.mag_fs != mag_fs)
	{
		switch(mag_fs)
		{
			case QMC_MAG_FS_30GS:
				data = QMC_MAG_FS_30GS;
				ret = ob_qmc6310.write(QMC6310U_IIC_ADDR,QMC6310_CTL_REG_TWO,&data,1);
				if(ret == false)return -1;
				data = 0xFF;
				ret = ob_qmc6310.read(QMC6310U_IIC_ADDR,QMC6310_CTL_REG_TWO,&data,1);
				if(ret == false || data != QMC_MAG_FS_30GS)return -1;
				ob_qmc6310.cur_param.mag_fs = QMC_MAG_FS_30GS;
				break;
		}
	}
	
	return 0;
}
/**
	@brief 		获取配置MAG采样频率需要的步骤数量
	@return 	错误代码							- [out]	配置MAG采样频率需要的步骤数量
*/
int	drv_qmc6310_get_mag_odr_flow(void)
{
	return 4;
}
/**
	@brief 		设置MAG采样频率
	@param 		mag_odr 							- [in]	MAG采样频率
	@param 		flow 									- [in]	当前需要处理的步骤
	@return 	错误代码							- [out]	-1失败，0成功
*/
int	drv_qmc6310_set_mag_odr(QMC_MAG_ODR_e mag_odr, int flow)
{
	int 				ret = -1;
	bool				err;
	uint8_t			data;
	
	if(flow <= 0 || flow >= 5)return -1;
	
	if(ob_qmc6310.cur_param.mag_odr != mag_odr)
	{
		switch(flow)
		{
			case 1:
				//define the sign for x y and z axis
				if(mag_odr != QMC_MAG_ODR_OFF)
				{
					data = 0x06;
					err = ob_qmc6310.write(QMC6310U_IIC_ADDR,0x29,&data,1);
					if(err == false)return -1;
					else ret = 0;
				}
				else
				{
					ret = 0;
				}
				break;
			
			case 2:
				//define the sign for x y and z axis
				if(mag_odr != QMC_MAG_ODR_OFF)
				{
					data = 0xFF;
					err = ob_qmc6310.read(QMC6310U_IIC_ADDR,0x29,&data,1);
					if(err == false || data != 0x06)return -1;
					else ret = 0;
				}
				else
				{
					ret = 0;
				}
				break;
			
			case 3:
				//set odr
				data = mag_odr;
				err = ob_qmc6310.write(QMC6310U_IIC_ADDR,QMC6310_CTL_REG_ONE,&data,1);
				if(err == false)return -1;
				else ret = 0;
				break;
			
			case 4:
				data = 0xFF;
				err = ob_qmc6310.read(QMC6310U_IIC_ADDR,QMC6310_CTL_REG_ONE,&data,1);
				if(err == false || data != mag_odr)return -1;	
				else{
					ret = 0;
					ob_qmc6310.cur_param.mag_odr = mag_odr;
				}
				
				break;
		}
	}
	else
	{
		return 0;
	}
	
	return ret;
}
/**
	@brief 		获取LSM配置参数
	@param 		param 								- [in]	LSM配置参数
	@return 	错误代码							- [out]	-1失败，0成功
*/
int	drv_qmc6310_get_config_param(drv_qmc_config_param_t	*p_param)
{
	
	*p_param = ob_qmc6310.cur_param;
	
	return 0;
}
/**
	@brief 		获取LSM的ACC数据
	@param 		p_data 								- [out]	返回的ACC三轴数据
	@return 	错误代码							- [out]	-1失败，0成功
*/
int	drv_qmc6310_get_mag_data(qmc_data_t	*p_data)
{
	bool						ret;
	unsigned char 	mag_data[6];
	
	mag_data[0] = QMC6310_DATA_OUT_X_LSB_REG;
	
	ret = ob_qmc6310.read(QMC6310U_IIC_ADDR,QMC6310_DATA_OUT_X_LSB_REG,mag_data,6);
	if(ret == false)return -1;
	
	if(p_data != NULL)
	{
		p_data->mag[0] = (int16_t)(((mag_data[1]) << 8) | mag_data[0]);
		p_data->mag[1] = (int16_t)(((mag_data[3]) << 8) | mag_data[2]);
		p_data->mag[2] = (int16_t)(((mag_data[5]) << 8) | mag_data[4]);
	}
	
	return 0;
}

int drv_qmc6310_get_mag_id(void)
{
	if(qmc6310_get_chipid())
	{
		return 0;
	}
	
	return -1;
}


int drv_qmc6310_selfcheck_mag(void)
{
	int 						ret = 0;
	uint8_t					data;
	unsigned char 	mag_data1[6];
	unsigned char 	mag_data2[6];
	int16_t					mag_delta[3];
	
//	SEGGER_RTT_printf(0,"Write Register 29H by 0x06 (Define the sign for X Y and Z axis)\r\n");
	data = 0x06;
	ret = ob_qmc6310.write(QMC6310U_IIC_ADDR,0x29,&data,1);
	if(ret == -1)return -1;
	data = 0xFF;
	ret = ob_qmc6310.read(QMC6310U_IIC_ADDR,0x29,&data,1);
	if(ret == -1 || data != 0x06)return -2;
//	SEGGER_RTT_printf(0,"Write Register 0AH by 0x03 (set continuous mode)\r\n");
	data = 0x03;
	ret = ob_qmc6310.write(QMC6310U_IIC_ADDR,0x0A,&data,1);
	if(ret == -1)return -3;
	data = 0xFF;
	ret = ob_qmc6310.read(QMC6310U_IIC_ADDR,0x0A,&data,1);
	if(ret == -1 || data != 0x03)return -4;
//	SEGGER_RTT_printf(0,"Check status register 09H[0], 1 means ready\r\n");
	uint32_t timeout = 2000;
	do
	{
		data = 0xFF;
		ret = ob_qmc6310.read(QMC6310U_IIC_ADDR,0x09,&data,1);
		timeout--;
	}while((data & 0x01) != 0x01 && timeout != 0);
	if(ret == -1 || (data & 0x01) != 0x01)return -5;
//	SEGGER_RTT_printf(0,"Read data Register 01H ~ 06H, recording as datax1/datay1/dataz1\r\n");
	mag_data1[0] = QMC6310_DATA_OUT_X_LSB_REG;
	ret = ob_qmc6310.read(QMC6310U_IIC_ADDR,QMC6310_DATA_OUT_X_LSB_REG,mag_data1,6);
	if(ret == -1)return -6;
//	SEGGER_RTT_printf(0,"Write Register 0BH by 0x40(enter self-test function)\r\n");
	data = 0x40;
	ret = ob_qmc6310.write(QMC6310U_IIC_ADDR,0x0B,&data,1);
	if(ret == -1)return -7;
	data = 0xFF;
	ret = ob_qmc6310.read(QMC6310U_IIC_ADDR,0x0B,&data,1);
	if(ret == -1 || data != 0x40)return -8;
//	SEGGER_RTT_printf(0,"Waiting 5 millisecond until measurement ends\r\n");
	nrf_delay_ms(5);
//	SEGGER_RTT_printf(0,"Read data Register 01H ~ 06H, recording as datax2/datay2/dataz2\r\n");
	mag_data2[0] = QMC6310_DATA_OUT_X_LSB_REG;
	ret = ob_qmc6310.read(QMC6310U_IIC_ADDR,QMC6310_DATA_OUT_X_LSB_REG,mag_data2,6);
	if(ret == -1)return -9;
//	SEGGER_RTT_printf(0,"Calculate the delta (datax1-datax2), (datay1-datay2), (dataz1-dataz2)\r\n");
	mag_delta[0] = (int16_t)(((mag_data1[1]) << 8) | mag_data1[0])  - (int16_t)(((mag_data2[1]) << 8) | mag_data2[0]);
	mag_delta[1] = (int16_t)(((mag_data1[3]) << 8) | mag_data1[2])	-	(int16_t)(((mag_data2[3]) << 8) | mag_data2[2]);
	mag_delta[2] = (int16_t)(((mag_data1[5]) << 8) | mag_data1[4])	-	(int16_t)(((mag_data2[5]) << 8) | mag_data2[4]);
//	SEGGER_RTT_printf(0,"mag_delta:%d,%d,%d\r\n",mag_delta[0],mag_delta[1],mag_delta[2]);
//	extern void JS_RTT_Print(signed int Sine1,signed int Sine2,signed int Sine3);
//	JS_RTT_Print(mag_delta[0],mag_delta[1],mag_delta[2]);

	if((mag_delta[0] >= 600 && mag_delta[0] <= 1100) && (mag_delta[1] >= -1000 && mag_delta[1] <= -600) && (mag_delta[2] >= -500 && mag_delta[2] <= -100))
		return 0;
	else
		return -10;
}