liweiyan/shoe_mcu_2022_2_10/drivers/drv_qmc6310_center.c

/**
  ******************************************************************************
  * @file    qmc6310.c
  * @author  STMicroelectronics
  * @version V1.0
  * @date    2013-xx-xx
  * @brief    qmc6310动
  ******************************************************************************
  * @attention
  *
  * 实验平台:秉火 指南者 开发板 
  * 论坛    :http://www.firebbs.cn
  * 淘宝    :https://fire-stm32.taobao.com
  *
  ******************************************************************************
  */ 

#include "drv_qmc6310.h"
#include <math.h>
#include "lsm6ds3tr_c.h"
#include "nrf_delay.h"

static uint8_t chipid = 0;


static QMC6310_map c_map;


//bool MPU9250_Write_Byte(uint8_t Device_Address,uint8_t REG_Address,uint8_t REG_data)
//{
//	return  IIC_WriteBytes(Device_Address,REG_Address,&REG_data,1);
//}

//bool MPU9250_Read_nBytes(uint8_t Device_Address,uint8_t REG_Address,uint8_t *readDataBuf,uint8_t readDataLen)
//{
//	return IIC_ReadBytes(Device_Address,REG_Address,readDataBuf,readDataLen);
//}
//bool MPU9250_register_write_len(uint8_t Device_Address,uint8_t register_address, uint8_t len,uint8_t *buf)
//{
//	return IIC_WriteBytes(Device_Address,register_address,buf,len);
//}
//bool MPU9250_register_read_len(uint8_t Device_Address,uint8_t register_address, uint8_t number_of_bytes,uint8_t * destination )
//{
//	return IIC_ReadBytes(Device_Address,register_address,destination,number_of_bytes);
//}


static int qmc6310_read_block(uint8_t addr, uint8_t *data, uint8_t len)
{
	if(lsm6ds3tr_c_switch_read_sensor_hub_cx(LSM_DEV_CENTER, 0x1C, addr, data,len) == 0)
	{
		return 1;
	}
	return 0;
}



static int qmc6310_write_reg(uint8_t addr, uint8_t data)
{	
	if(lsm6ds3_c_switch_write_sensor_hub_cx(LSM_DEV_CENTER, 0x1C, addr, &data) == 0)
	{
		return true;
	}
	return false;
}

static void qmc6310_set_layout(int layout)
{
	if(layout == 0)
	{
		c_map.sign[AXIS_X] = 1;
		c_map.sign[AXIS_Y] = 1;
		c_map.sign[AXIS_Z] = 1;
		c_map.map[AXIS_X] = AXIS_X;
		c_map.map[AXIS_Y] = AXIS_Y;
		c_map.map[AXIS_Z] = AXIS_Z;
	}
	else if(layout == 1)
	{
		c_map.sign[AXIS_X] = -1;
		c_map.sign[AXIS_Y] = 1;
		c_map.sign[AXIS_Z] = 1;
		c_map.map[AXIS_X] = AXIS_Y;
		c_map.map[AXIS_Y] = AXIS_X;
		c_map.map[AXIS_Z] = AXIS_Z;
	}
	else if(layout == 2)
	{
		c_map.sign[AXIS_X] = -1;
		c_map.sign[AXIS_Y] = -1;
		c_map.sign[AXIS_Z] = 1;
		c_map.map[AXIS_X] = AXIS_X;
		c_map.map[AXIS_Y] = AXIS_Y;
		c_map.map[AXIS_Z] = AXIS_Z;
	}
	else if(layout == 3)
	{
		c_map.sign[AXIS_X] = 1;
		c_map.sign[AXIS_Y] = -1;
		c_map.sign[AXIS_Z] = 1;
		c_map.map[AXIS_X] = AXIS_Y;
		c_map.map[AXIS_Y] = AXIS_X;
		c_map.map[AXIS_Z] = AXIS_Z;
	}	
	else if(layout == 4)
	{
		c_map.sign[AXIS_X] = -1;
		c_map.sign[AXIS_Y] = 1;
		c_map.sign[AXIS_Z] = -1;
		c_map.map[AXIS_X] = AXIS_X;
		c_map.map[AXIS_Y] = AXIS_Y;
		c_map.map[AXIS_Z] = AXIS_Z;
	}
	else if(layout == 5)
	{
		c_map.sign[AXIS_X] = 1;
		c_map.sign[AXIS_Y] = 1;
		c_map.sign[AXIS_Z] = -1;
		c_map.map[AXIS_X] = AXIS_Y;
		c_map.map[AXIS_Y] = AXIS_X;
		c_map.map[AXIS_Z] = AXIS_Z;
	}
	else if(layout == 6)
	{
		c_map.sign[AXIS_X] = 1;
		c_map.sign[AXIS_Y] = -1;
		c_map.sign[AXIS_Z] = -1;
		c_map.map[AXIS_X] = AXIS_X;
		c_map.map[AXIS_Y] = AXIS_Y;
		c_map.map[AXIS_Z] = AXIS_Z;
	}
	else if(layout == 7)
	{
		c_map.sign[AXIS_X] = -1;
		c_map.sign[AXIS_Y] = -1;
		c_map.sign[AXIS_Z] = -1;
		c_map.map[AXIS_X] = AXIS_Y;
		c_map.map[AXIS_Y] = AXIS_X;
		c_map.map[AXIS_Z] = AXIS_Z;
	}
	else		
	{
		c_map.sign[AXIS_X] = 1;
		c_map.sign[AXIS_Y] = 1;
		c_map.sign[AXIS_Z] = 1;
		c_map.map[AXIS_X] = AXIS_X;
		c_map.map[AXIS_Y] = AXIS_Y;
		c_map.map[AXIS_Z] = AXIS_Z;
	}
}

static int qmc6310_get_chipid(void)
{
	int ret = 0; 	
	ret = qmc6310_read_block(QMC6310_CHIP_ID_REG, &chipid , 1);
	if(ret == 0){
		SEGGER_RTT_printf(0,"change_iic_addr = 0x3c\n");
	}
	else
	{
		SEGGER_RTT_printf(0,"QMC6308_get_chipid chipid = 0x%x,i2c_addr = 0x2c\n", chipid);
		return 1;
	}
	
	ret = qmc6310_read_block(QMC6310_CHIP_ID_REG, &chipid , 1);
	if(ret == 0){
		SEGGER_RTT_printf(0,"change_iic_addr = 0x1c\n");
	}
	else
	{
		SEGGER_RTT_printf(0,"QMC6310_get_chipid chipid = 0x%x,i2c_addr = 0x3c\n", chipid);
		return 1;
	}
	
	ret = qmc6310_read_block(QMC6310_CHIP_ID_REG, &chipid , 1);
	if(ret == 0){
		SEGGER_RTT_printf(0,"Get_Chip_ID_Failed!\n");
		return 0;
	}
	else
	{
		SEGGER_RTT_printf(0,"QMC6310_get_chipid chipid = 0x%x,i2c_addr = 0x1c\n", chipid);
		return 1;
	}
}


uint8_t qmc6310_center_read_mag_xyz(int16_t *data)
{
	int res;
	unsigned char mag_data[6];
	int16_t hw_d[3] = {0};
	int16_t raw_c[3];
	int t1 = 0;
	unsigned char rdy = 0;

	/* Check status register for data availability */
	while(!(rdy & 0x01) && (t1 < 5))
	{
		rdy = QMC6310_STATUS_REG;
		res = qmc6310_read_block(QMC6310_STATUS_REG, &rdy, 1);
		t1++;
	}

	mag_data[0] = QMC6310_DATA_OUT_X_LSB_REG;

	res = qmc6310_read_block(QMC6310_DATA_OUT_X_LSB_REG, mag_data, 6);
	if(res == 0)
	{
		return 0;
	}

	hw_d[0] = (int16_t)(((mag_data[1]) << 8) | mag_data[0]);
	hw_d[1] = (int16_t)(((mag_data[3]) << 8) | mag_data[2]);
	hw_d[2] = (int16_t)(((mag_data[5]) << 8) | mag_data[4]);

	//Unit:mG  1G = 100uT = 1000mG
	//SEGGER_RTT_printf(0,"Hx=%d, Hy=%d, Hz=%d\n",hw_d[0],hw_d[1],hw_d[2]);
	raw_c[AXIS_X] = (int16_t)(c_map.sign[AXIS_X]*hw_d[c_map.map[AXIS_X]]);
	raw_c[AXIS_Y] = (int16_t)(c_map.sign[AXIS_Y]*hw_d[c_map.map[AXIS_Y]]);
	raw_c[AXIS_Z] = (int16_t)(c_map.sign[AXIS_Z]*hw_d[c_map.map[AXIS_Z]]);
	
	#if DEBUG_IMU
	SEGGER_RTT_printf(0,"center raw_c[0]=%d,raw_c[1]=%d,raw_c[2]=%d\n",raw_c[0],raw_c[1],raw_c[2]);
	#endif
	
	data[0] = raw_c[0];
	data[1] = raw_c[1];
	data[2] = raw_c[2];

	return res;
}


///* Set the sensor mode */
//static int qmc6310_set_mode(unsigned char mode)
//{
//	int err = 0;
//	unsigned char ctrl1_value = 0;
//	
//	err = qmc6310_read_block(QMC6310_CTL_REG_ONE, &ctrl1_value, 1);
//	ctrl1_value = (ctrl1_value&(~0x03))|mode;
//	SEGGER_RTT_printf(0,"QMC6310_set_mode, 0x0A = [%02x]->[%02x] \r\n", QMC6310_CTL_REG_ONE,ctrl1_value);
//	err = qmc6310_write_reg(QMC6310_CTL_REG_ONE, ctrl1_value);

//	return err;
//}

//static int qmc6310_set_output_data_rate(unsigned char rate){
//	
//	int err = 0;
//	unsigned char ctrl1_value = 0;
//	
//	err = qmc6310_read_block(QMC6310_CTL_REG_ONE, &ctrl1_value, 1);
//	ctrl1_value = (ctrl1_value& (~0xE8)) | (rate << 5);
//	SEGGER_RTT_printf(0,"QMC6310_set_output_data_rate, 0x0A = [%02x]->[%02x] \r\n", QMC6310_CTL_REG_ONE,ctrl1_value);
//	err = qmc6310_write_reg(QMC6310_CTL_REG_ONE, ctrl1_value);

//	return err;	
//}

static int qmc6310_reset(void)
{
	uint16_t timeout = 200;
	unsigned char ctrl_value = 0;
	do{
					qmc6310_write_reg(0x0b, 0x80); //reset
				nrf_delay_ms(100);
					qmc6310_read_block(0x0b, &ctrl_value, 1);
					timeout--;
					SEGGER_RTT_printf(0,"---->center qmc6310_reset timeout:%d ctrl_value:0x%x\r\n",timeout,ctrl_value);
	}while(timeout !=0 && ctrl_value != 0x80);
	if(timeout !=0 && ctrl_value != 0x80)SEGGER_RTT_printf(0,"---->center qmc6310_reset timeout:%d\r\n",timeout);
	qmc6310_write_reg(0x0b, 0x00); //取消reset
	return 1;
}

static int qmc6310_center_CheckIsConfigure(uint8_t hz)
{
	unsigned char ctrl_value;
	
	qmc6310_read_block(0x0d, &ctrl_value, 1);
	nrf_delay_ms(3);
//	SEGGER_RTT_printf(0,"QMC6310 02 0x%x=0x%x \r\n", 0x0d, ctrl_value);
	if(ctrl_value != 0x40) return 0;
	
	qmc6310_read_block(QMC6310_CTL_REG_TWO, &ctrl_value, 1);
	nrf_delay_ms(3);
//	SEGGER_RTT_printf(0,"QMC6310 02 0x%x=0x%x \r\n", QMC6310_CTL_REG_TWO, ctrl_value);
	if(ctrl_value != 0x00) return 0;
	
	qmc6310_read_block(QMC6310_CTL_REG_ONE, &ctrl_value, 1);
	nrf_delay_ms(3);
//	SEGGER_RTT_printf(0,"QMC6310 02 0x%x=0x%x \r\n", QMC6310_CTL_REG_ONE, ctrl_value);
	if(ctrl_value != hz) return 0;
	
	return 1;
}

int qmc6310_center_ReConfigure(uint8_t hz)
{
	if(!qmc6310_write_reg(0x29, 0x06))return 0;//(Define the sign for X Y and Z axis)
	if(!qmc6310_write_reg(0x0d, 0x40))return 0;
	if(!qmc6310_write_reg(0x0b, 0x00))return 0; //30 GS
	if(!qmc6310_write_reg(0x0a, hz))return 0;

	
	return 1;
}

int qmc6310_center_Suspend(void)
{
	return qmc6310_reset();
}

QMC6310_map qmc6310_center_GetMap(void)
{
	return c_map;
}

int qmc6310_center_Init(uint8_t hz)
{
	qmc6310_reset();
	
	int ret = 0;
	ret = qmc6310_get_chipid();
	if(ret==0)
	{
		return 0;
	}
	qmc6310_set_layout(0);
	
	qmc6310_reset();
	
	qmc6310_center_ReConfigure(hz);
	
	return qmc6310_center_CheckIsConfigure(hz);
}