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

Question

我尝试倾斜补偿磁力计 (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 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 在平面上的投影定义了罗盘面上的“北”
• 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 上运行。稍后会进行优化。

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