地磁学习

配置了AK09919的地磁，从FIFO中读取数据

1. soft reset, 寄存器CONTROL3，After reset is completed, all registers and FIFO buffer are initialized and AK09919 transit to Power-down mode automatically.

2. 设置模式，Water-marker 根据ODR需要

3.设置continue mode

4.enable FIFO

 

FIFO mode下读取数据

根据ST1中的值判断数据是否ready，是否data overrun，确定buffer中数据的个数，先入先出的方式读取出来。

读取buffer中的数据，每次读取后再读取ST2的值，buffer中删除掉此值，判断里面是否还有数据，数据是否overflow（每次读取8个数据，6个是mag xyz,后面两个是temperature和ST2,读ST2后，buffer才会将这个数据删除）。

 

思想：地磁校准 地磁校准 

随手写了一个简单的地磁校准程序（不一定能用，没有验证），主要思想拿着设备水平方向旋转，获得两个对应地磁轴的极大值和极小值；再将设备垂直方向旋转180度，获得垂直方向上地磁的极大值和极小值；取平均值作为地磁的偏移中心点。每次读取的地磁值减去这个中心点即为地磁值，再根据gensor的数据求出姿态角（倾斜角），根据姿态角进行偏移角度，即为方位角。

地磁校准程序

/* send the imu's data to mag for calibration and angle */
void MagSendGsensorData2Mag(void *buffer, uint16_t bytes_num)
{
	uint16_t i = 0, j = 0;
	short *imu_data=(short *)buffer;
	static int16_t max_x = 0, max_y = 0, max_z = 0, min_x = 0, min_y = 0, min_z = 0;
	static float offset_x  = -245.5, offset_y = 245.5, offset_z = -164.5;
	float mag_x = 0.0, mag_y = 0.0, mag_z = 0.0;
	float angle = 0;
	float theta = 0.0,  fi = 0.0;

	/* for the mag calibration */
	for(i = 0, j = 0; i< bytes_num; i+=6, j+=3)
	{
		/* for calibration */
		if(MAG_CALIBRATION == 1)
		{
			if((abs(imu_data[i + 2]) < 5000)&&(abs(imu_data[i + 2]) > 4000))
			{
				min_x = min(min_x, mag_raw_data.mag_data_buffer[j]);
				min_y = min(min_y, mag_raw_data.mag_data_buffer[j + 1]);

				max_x = max(max_x, mag_raw_data.mag_data_buffer[j]);
				max_y = max(max_y, mag_raw_data.mag_data_buffer[j + 1]);

			}else if(abs(imu_data[i + 2]) < 500)
			{
				//LogPrintf("--------------minz:%d %d %d\n", imu_data[i],  imu_data[i + 1],  imu_data[i+2] );
				min_z = min(min_z, mag_raw_data.mag_data_buffer[j + 2]);
				max_z = max(max_z, mag_raw_data.mag_data_buffer[j + 2]);
			}

			/* save to the RAM */
			offset_x = (min_x + max_x)/2;
			offset_y = (min_y + max_y)/2;
			offset_z = (min_z + max_z)/2;

		}
		else
		{
			theta =asin( (double)(imu_data[i]/(4096*9.8)));
			fi = asin((double)(imu_data[i + 1]/(4096*9.8)));

			mag_x = (float)mag_raw_data.mag_data_buffer[j] - offset_x;
			mag_y = (float)mag_raw_data.mag_data_buffer[j + 1] - offset_y;
			mag_z = (float)mag_raw_data.mag_data_buffer[j + 2] - offset_z;
			
			MagAzimuth(&angle, theta, fi, mag_x, mag_y, mag_z);

			//LogPrintf("magxyz:%d %d %d offset:%lf %lf %lf\n ",mag_raw_data.mag_data_buffer[j] , mag_raw_data.mag_data_buffer[j + 1] ,mag_raw_data.mag_data_buffer[j + 2] ,
			//	offset_x, offset_y, offset_z);
			//LogPrintf("theta:%lf, fi:%lf, magx:%d, y:%d z:%d angle: %lf\n", theta, fi, mag_x, mag_y, mag_z, angle );

		}

		/* max: x:65 y:592 z:0 min:x:-482 y:-59 z:0 */
		/* max: x:57 y:618 z:37 min:x:-548 y:-127 z:-366 */
	}

	LogSensor("theta:%lf, fi:%lf, magx:%d, y:%d z:%d angle: %lf\n", theta, fi, mag_x, mag_y, mag_z, angle );
	
}

求方位角

void MagAzimuth(float* angle, double theta, double fi, float magx, float magy, float magz )
{
	float xh = 0.0, yh = 0.0;

	xh = magx*cos(fi)+magy*sin(theta)*sin(fi)-magz*cos(theta)*sin(fi);
	yh = magy*cos(theta) + magz*sin(theta);

	if(xh < 0)
	{
		*angle = 180-(atan(yh/xh)*180/PI);
	}else if((xh > 0)&&(yh < 0))
	{
		*angle = -atan(yh/xh)*180/PI;
	}else if((xh > 0)&&(yh > 0))
	{
		*angle = 360 - atan(yh/xh)*180/PI;
	}else if((xh == 0)&&(yh < 0))
	{
		*angle = 90;
	}else if((xh == 0)&&(yh > 0))
	{
		*angle = 270;
	}

}

...