linux下i2c通用接口读取和处理mag3110地磁传感器程序

/*mag3110是freescale公司的一个三轴地磁传感器模块，

可以感知地球磁场，通过算法处理得知地磁指向。*/

 

/*一个.c文件，没有分文件，不要在意这些细节，

包括了读取和数据处理得过程

用到反正切函数atan（），编译时加入-lm*/

/*
* I2C testing utility
*/
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 

#include 
#include 
#include 
#include 
#include 

#define I2C_DEV		"/dev/i2c-1"
#define CHIP_ADDR	0x0e		

#define DR_STATUS 	0X00		
#define XYZ_DATA 	0X01		//x,y,z DATA register
#define WHO_AM_I 	0X07		//x,y,z check register
#define CTRL_REG  	0X16



int MAG3110_XOFF=0,MAG3110_YOFF=0;
int MAG3110_XMax,MAG3110_YMax,MAG3110_XMin,MAG3110_YMin;

int ang;

int i2c_read_byte(int *fd, __u16 mem_addr);
int i2c_write_byte(int *fd, __u16 mem_addr, __u8 data);
/*************************************************************************/



static int read_data_from_mag3110(int *fd,short *x,short *y,short *z)
{
	int i;
	char tmp_data[6] ="";
	
	printf(".>>>%d..\n",i2c_read_byte(fd, DR_STATUS));
	if(i2c_read_byte(fd, DR_STATUS) & 0x08)	//一次数据转换完成
	{
		printf(",,enter well done\n");
		for(i = 0;i<6;i++)
		{
			tmp_data[i] = i2c_read_byte(fd, XYZ_DATA+i);
		}
		
		*x = ((tmp_data[0] << 8) & 0xff00) | tmp_data[1];
		*y = ((tmp_data[2] << 8) & 0xff00) | tmp_data[3];
		*z = ((tmp_data[4] << 8) & 0xff00) | tmp_data[5];
		
		return 1;
	}
	
	return 0;
}

int  mag3110_adjust_position(short *data_x,short *data_y,short *data_z)
{
	static first_flag = 1;
	if (first_flag)
	{

		MAG3110_XMax = *data_x;
		MAG3110_XMin = *data_x;
		MAG3110_YMax = *data_y;
		MAG3110_YMin = *data_y;
		first_flag = 0;
	}
	if (*data_x > MAG3110_XMax)
	{
		MAG3110_XMax =  *data_x;
	}
	else if (*data_x < MAG3110_XMin)
	{
		MAG3110_XMin =  *data_x;
	}
	if (*data_y > MAG3110_YMax)
	{
		MAG3110_YMax =  *data_y;
	}
	else if (*data_y < MAG3110_YMin)
	{
		MAG3110_YMin =  *data_y;
	}
	MAG3110_XOFF = (MAG3110_XMax + MAG3110_XMin) / 2;
	MAG3110_YOFF = (MAG3110_YMax + MAG3110_YMin) / 2;
	puts("**********************************\n");
	puts("turn a lap to adjust global variable\n");
    printf("MAG3110_XMax：%d\n",MAG3110_XMax);
	printf("MAG3110_XMin：%d\n",MAG3110_XMin);
    printf("MAG3110_XOFF：%d\n",MAG3110_XOFF);
	printf("\n");
    printf("MAG3110_YMax：%d\n",MAG3110_YMax);
	printf("MAG3110_YMin：%d\n",MAG3110_YMin);
    printf("MAG3110_YOFF：%d\n",MAG3110_YOFF);
	
	return 1;
}

static inline __s32 i2c_smbus_access(int file, char read_write, __u8 command, int size, union i2c_smbus_data *data)
{
	struct i2c_smbus_ioctl_data args;

	args.read_write = read_write;
	args.command = command;
	args.size = size;
	args.data = data;
	
	return ioctl(file, I2C_SMBUS, &args);
}

static int init_mag3110(int *fd)
{
	int i, ret;
	
	int data = 0x00;
	i2c_write_byte(fd, CTRL_REG, data);	//STANDBY 模式
	
	data |= 0x04;
	i2c_write_byte(fd, CTRL_REG, data);	//RF8位模式
	
	data += 1;
	i2c_write_byte(fd, CTRL_REG, data);	 //ACTIVE  模式

	usleep(500*1000);
	
	return 0;
}


void check_normal(int *fd)
{
	char ret;
	if(!(0xc4 == i2c_read_byte(fd,WHO_AM_I)))
	{
		printf("check ID error\n");
		exit(-1);
	}
}
int i2c_read_byte(int *fd, __u16 mem_addr)
{
	int r;
	union i2c_smbus_data data;

	ioctl(*fd, BLKFLSBUF);  //clear kernel read buffer

	__u8 buf =  mem_addr & 0x0ff;
	r = i2c_smbus_access(*fd, I2C_SMBUS_WRITE, buf, I2C_SMBUS_BYTE, NULL);
	
	if (r < 0)
		return r;
	if (i2c_smbus_access(*fd, I2C_SMBUS_READ, 0, I2C_SMBUS_BYTE, &data))
		return -1;
	else
		return 0x0FF & data.byte;
}


int MAG3110_DataProcess(short MAG3110_XData,short MAG3110_YData)
{

	MAG3110_XData -= MAG3110_XOFF;
	MAG3110_YData -= MAG3110_YOFF;
	if (MAG3110_XData == 0)
	{
		if (MAG3110_YData>0)
		{
			ang = 90;
		}
		else
		{
			ang = 270;
		}
	}
	else if (MAG3110_YData == 0)
	{
		if (MAG3110_XData>0)
		{
			ang = 0;
		}
		else
		{
			ang = 180;
		}
	}
	else if((MAG3110_XData > 0) && (MAG3110_YData > 0))
	{
		ang = ( atan( ( (float)MAG3110_YData) / ( (float) MAG3110_XData ) ) ) * 180 / 3.14;
	}
	else if ((MAG3110_XData < 0) && (MAG3110_YData > 0))
	{
		MAG3110_XData = -MAG3110_XData;
		ang = 180 - ( atan( ( (float)MAG3110_YData) / ( (float) MAG3110_XData ) ) ) * 180 / 3.14;
	}
	else if ((MAG3110_XData < 0) && (MAG3110_YData < 0))
	{
		MAG3110_XData = -MAG3110_XData;
		MAG3110_YData = -MAG3110_YData;
		ang = (atan( ( (float)MAG3110_YData) / ( (float) MAG3110_XData ) ) ) * 180 / 3.14 + 180;
	}
	else if ((MAG3110_XData > 0) && (MAG3110_YData < 0))
	{
		MAG3110_YData = -MAG3110_YData;
		ang = 360 -(atan( ( (float)MAG3110_YData) / ( (float) MAG3110_XData ) ) ) * 180 / 3.14;

	}

	return 	 ang;
}


int iic_open(int *fd,char *dev_name, int addr)
{
	int funcs;
	int r;
	*fd = open(I2C_DEV, O_RDWR);
	if(fd <= 0)
	{
		fprintf(stderr, "Error eeprom_open: %s\n", strerror(errno));
		return -1;
	}
	if((r = ioctl(*fd, I2C_FUNCS, &funcs) < 0))
	{
		fprintf(stderr, "Error eeprom_open: %s\n", strerror(errno));
		return -1;
	}
	if ((r = ioctl(*fd, I2C_SLAVE, addr)) < 0)
	{
		fprintf(stderr, "Error eeprom_open: %s\n", strerror(errno));
		return -1;
	}
	usleep(500*1000);
	return 0;
}

int i2c_write_byte(int *fd, __u16 mem_addr, __u8 data)
{
	int r;
	__u8 command = mem_addr & 0x00ff;
	union i2c_smbus_data i2c_data;

	i2c_data.byte = data;
	r = i2c_smbus_access(*fd, I2C_SMBUS_WRITE, command, I2C_SMBUS_BYTE_DATA, &i2c_data);

	usleep(10);

	return r;
}



int main(int argc, char** argv)
{
	int fd;
	int ret;
	short data_x,data_y,data_z;
	/*1. open device..*/
	iic_open(&fd,I2C_DEV,CHIP_ADDR);

	/*2. checking whether normal*/
	check_normal(&fd);
	
	/*3. init the sensor*/
	init_mag3110(&fd);		

	/*4. begin main loop*/
	printf(" Reading xyz data from mag3110\n");
	

	while(1)
	{		
		system("clear");
		/*5.read raw data*/
		if(read_data_from_mag3110(&fd,&data_x,&data_y,&data_z)) 
		/*6.adjust data*/
			if(mag3110_adjust_position(&data_x,&data_y,&data_z))
				/*data process*/
				{
					MAG3110_DataProcess(data_x,data_y);
					printf("Point to the south angle  %d°\n",ang);	
				
				}
			
		sleep(1);
	}
}