1

我尝试倾斜补偿磁力计（BMX055）读数并尝试了我在网上找到的各种方法，但没有一种方法有效。

我最终尝试了几乎所有在 Google 上找到的结果。

我在 AVR 上运行它，如果能找到在 50 度以下的角度没有复杂函数（三角函数等）的东西，那就太棒了。

我有一个来自 gyro+acc（1g 重力=16k）的融合重力矢量（int16 以浮点数签名）。态度.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 正弦/余弦函数现在似乎可以产生更好的结果。我正在尝试实现建议的矢量算法，到目前为止一直在努力:)

4

1 回答 1

0

让我们看看。

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

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

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

您的输入：

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

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

首先，当设备倾斜时，您希望指南针的方位是什么？设备应该表现得好像它没有倾斜一样，还是应该在屏幕上实际显示磁场线的正确投影？后者是普通指南针的作用方式（如果指针在倾斜时完全移动）。在这种情况下，您不应该进行任何补偿，并且该设备可以在横向滚动时显示磁力线的奇特垂直倾斜。

无论如何，尽量避免三角函数，它会占用大量的代码空间和时间。向量算术要简单得多，而且大多数时候您可以使用乘法和加法。

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

使用向量数学，这看起来相当简单：

  • g是水平（真正水平）平面的法线
  • m在这个平面上的投影定义了水平罗盘将显示的方向
  • d在平面上的投影定义了罗盘面上的“北”
  • 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)

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

因此，如您所见，周围没有三角函数，甚至没有平方根。矢量点和十字只是相乘。唯一稍微具有挑战性的技巧是在每次计算中将定点算术缩放到正确的比例。

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

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

于 2014-06-27T20:46:39.953 回答