通过加速度传感器与磁力计计算方位角,实现电子罗盘功能,网上很多都是通过加速度,磁力计,陀螺仪去混合计算,太复杂了,而且现在我用的传感器只有加速度与磁力计,我只需要水平的方位角,实现电子罗盘功能。
//计算旋转角
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)
* 依赖 : 底层读写函数
* 作者 : cp1300@139.com
* 时间 : 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); //计算方位角