Question

我尝试倾斜补偿磁力计（BMX055）的读数，并尝试了我在网上找到的各种方法，而不是单一的方法。

我几乎尝试过在Google上找到的几乎任何结果。

我在AVR上运行它，如果在没有复杂功能（三角等）的情况下找到适合50度角的东西，那将会非常棒。

我有一个融合的重力矢量（在浮子中签名的int16）来自陀螺仪+ acc（1g重力= 16k）。 attitude.vect_mag.x / y / z是一个浮点数，但是包含一个16位整数，范围从每轴约-250到+250。

目前我尝试使用此代码：

float rollRadians = attitude.roll * DEG_TO_RAD / 10;
float pitchRadians = attitude.pitch * DEG_TO_RAD / 10;
float cosRoll = cos(rollRadians);
float sinRoll = sin(rollRadians);
float cosPitch = cos(pitchRadians);
float sinPitch = sin(pitchRadians);
float Xh = attitude.vect_mag.x * cosPitch + attitude.vect_mag.z * sinPitch;
float Yh = attitude.vect_mag.x * sinRoll * sinPitch + attitude.vect_mag.y * cosRoll - attitude.vect_mag.z *sinRoll * cosPitch;

float heading = atan2(Yh, Xh);
attitude.yaw = heading*RAD_TO_DEG;

结果毫无意义，但没有倾斜补偿的值是正确的。

未补偿的公式：

atan2(attitude.vect_mag.y,attitude.vect_mag.x);

工作正常（不倾斜时）

我有点无能为力，正常的atan2返回一个好的结果（平衡时），但使用广泛的公式进行倾斜补偿完全失败。
我是否必须将mag矢量值保持在特定范围内才能使三角学工作？没有触发功能的任何方式进行补偿？

我很乐意帮助你。

更新：我发现BMX055磁力计的X和Y是倒置的，Y轴是-1 sin / cos函数现在似乎可以产生更好的结果。我正在尝试实现建议的矢量算法，到目前为止一直在努力：）

Answer 1

让我们看看。

（首先，请原谅我一点风格唠叨。关键字volatile意味着即使我们不在代码中自己更改变量，变量也可能会发生变化。这可能发生在一个由另一个进程（AVR上下文中的中断请求）。对于编译器volatile意味着变量必须始终加载并在使用时存储到内存中。请参阅：

http://en.wikipedia.org/wiki/Volatile_variable

所以，很可能你不希望你的花车有任何属性。）

您的意见：

三个表示加速度计数据的12位（11位+符号）整数
表示磁场的三个大约9位（8位+符号）整数

好消息（嗯......）是你的分辨率不是那么大，所以你可以使用整数算术，这要快得多。坏消息是，没有简单的神奇单行程可以解决您的问题。

首先，当设备倾斜时，您想要什么作为罗盘轴承？设备是否应该像没有倾斜一样，或者它是否应该实际显示屏幕上磁场线的正确投影？后者是普通罗盘的作用方式（如果针在倾斜时完全移动）。在这种情况下，您不应该对任何东西进行补偿，并且当侧向滚动时，设备可以显示磁力线的奇特垂直倾斜。

在任何情况下，尽量避免使用三角函数，这需要大量的代码空间和时间。矢量算术更简单，大多数时候你可以使用乘法和加法。

让我们尝试用矢量术语定义您的问题。实际上你有两个空间矢量开始， m 指向磁场的方向， g 指向重力方向。如果我已正确理解您的意图，您需要使用矢量 d ，它指向设备中某个固定方向。（如果我想到手机， d 将是与屏幕左边或右边平行的矢量。）

使用矢量数学，这看起来很简单：

g 是水平（真正水平）平面的法线
m 在此平面上的投影定义了水平罗盘显示的方向
d 在平面上的投影定义了＆＃34; north＆＃34;在罗盘面上
m 与 d 之间的角度给出指南针承载

现在我们对磁场的大小不感兴趣，我们可以根据需要扩展所有内容。这减少了使用计算成本昂贵的单位向量的需要。

所以，数学将是这样的：

# projection of m on g (. represents dot product)
mp := m - g (m.g) / (g.g)

# projection of d on g
dp := d - g (d.g) / (g.g)

# angle between mp and dp
cos2 := (mp.dp)^2 / (mp.mp * dp.dp)
sgn1 := sign(mp.dp)

# create a vector 90 rotated from d on the plane defined by g (x is cross product)
drot := dp x g
sin2 := (mp.drot)^2 / (mp.mp * drot.drot)
sgn2 := sign(mp.drot)

在此之后，您将获得指南针方向的罪^ 2和cos ^ 2。您需要为一个象限创建一个解析函数，然后使用符号确定正确的象限。解析函数可能听起来很难，但实际上你只需要为sin2 / cos2或cos2 / sin2创建一个表查找函数（以较小者为准）。它相对较快，查找中只需要几个点（双线性逼近甚至更少）。

所以，正如你所看到的，周围没有触发函数，甚至没有平方根。矢量点和十字架只是倍增。唯一稍微具有挑战性的技巧是在每次计算中将固定点算法缩放到正确的比例。

您可能会注意到有很多优化空间，因为多次使用相同的值。第一步是在具有正确结果的浮点PC上运行算法。优化会在稍后进行。

（对不起，我不会在这里写下实际的代码，但如果有需要澄清的话，我很乐意提供帮助。）

如何倾斜补偿我的磁力计？尝试了很多

1 个答案: