通过加速度与磁力计计算方位角

通过加速度传感器与磁力计计算方位角，实现电子罗盘功能，网上很多都是通过加速度，磁力计，陀螺仪去混合计算，太复杂了，而且现在我用的传感器只有加速度与磁力计，我只需要水平的方位角，实现电子罗盘功能。

 

//计算旋转角
static float getYaw(float accVals[3], float magVals[3]) 
{
	float roll = (float)atan2(accVals[0],accVals[2]);
	float pitch = -(float)atan(accVals[1]/(accVals[0]*sin(roll)+accVals[2]*cos(roll)));
	float yaw = (float)atan2(magVals[0]*sin(roll)*sin(pitch)+magVals[2]*cos(roll)*sin(pitch)+magVals[1]*cos(pitch),
		   magVals[0]*cos(roll)-magVals[2]*sin(roll));
	return yaw;
}


/*************************************************************************************************************************
* 函数			:	float AC_Azimuth(s16 ax,s16 ay, s16 az, s16 mx, s16 my, s16 mz)
* 功能			:	通过加速度传感器与磁力计计算方位角
* 参数			:	ax,ay,az:xyz方向加速度原始值;mx,my,mz:xyz方向的磁场强度原始值
* 返回			:	扩大10倍（正北为0）
* 依赖			:	底层读写函数
* 作者			:	[email protected]
* 时间			:	2020-04-28
* 最后修改时间 	: 	2020-04-28
* 说明			: 	
*************************************************************************************************************************/ 
float AC_Azimuth(s16 ax,s16 ay, s16 az, s16 mx, s16 my, s16 mz)
{
	float accVals[3], magVals[3];
	float ftemp;
	
	accVals[0] = ax;
	accVals[1] = az;
	accVals[2] = ay;
	magVals[0] = mx;
	magVals[1] = mz;
	magVals[2] = my;
			
	ftemp = getYaw(accVals, magVals) * 180.0f / 3.141593f;
	if(ftemp > 0) ftemp = -180.0f + (ftemp - 180.0f) ;
	ftemp = 0.0f - ftemp;
	ftemp += 90.0f;
	ftemp += -2.0f;	//补偿磁偏角，不同地区会不一样
	if(ftemp > 360.0f) ftemp -= 360.0f;
	return ftemp;	
}

LSM303DLH_ReadAcceleration(&g_SysGlobal.LSM303DLH_Handle, &iax, &iay, &iaz);	//LSM303DLH 读取三轴加速度
		LSM303DLH_ReadMagnetic(&g_SysGlobal.LSM303DLH_Handle, &imx, &imy, &imz);		//LSM303DLH 读取磁场强度值
AC_Azimuth(iax, iay, iaz, imx,imy,imz);					//计算方位角