倾斜磁力仪输出-pitch和滚动倾斜补偿-我迷路了

Question

我已经在这个特定的问题上浪费了两天(和几个晚上)，并且完全无法补偿我的磁力仪输出。

我尝试了所有我能在谷歌上读到的东西，在开源的例子中，没有任何东西引导我对两者(滚动和俯仰)进行适当的倾斜补偿。。

设置:我使用校准的值，而不是单位值。

我有一个融合的重力矢量，它工作正常，准确。

该传感器是一个9dof BMX055 (http://www.mouser.com/ds/2/621/BST-BMX055-DS000-01-274824.pdf)，磁力计的最小/最大值在每个轴上都是+- 512 (差异很小，但所有轴都是零输出)。硬件采用Sam3X8e (Cortex M3)，全部用C语言编写。

浮点运算和三角运算可以很快地完成，所以这不成问题。BMX055芯片对齐，使引脚20、19、18指向前面。

数据表第159-161页显示了方向。当我上升到前面的时候，音调就会上升。

当我上升到左边时，滚动就会上升。

示例取值：

指向水平方向时，我的算法调用305deg

Pitch: 0  , Roll 0   : MAG cal:  x 132   y  93  z   -364  
Pitch: +24, Roll 0   : MAG cal:  x -109   y  93  z   -397  
Pitch: +46, Roll 0   : MAG cal:  x -303   y  89  z   -351  
Pitch: 0  , Roll -44 : MAG cal:  x 151   y  352  z   -235  
Pitch: 0  , Roll +36 : MAG cal:  x 130   y  -140  z   -328  
Pitch: 78 , Roll -2  : MAG cal:  x -503   y  93  z   -199  
Pitch: 7  , Roll -53 : MAG cal:  x 135   y  424  z   -180

对齐应该总是在305度左右(就我所能做到的最好)，也许是+- 5度。

公式：(几乎所有地方都使用的公式)

uint16_t compass_tilt_compensation(float roll_radians, float pitch_radians,float mag_y, float mag_x, float mag_z) 
{

  float tilt_compensated_heading;
  float MAG_X;
  float MAG_Y;
  float cos_roll;
  float sin_roll;
  float cos_pitch;
  float sin_pitch;
  int16_t heading;
  //pitch_radians =roll_radians;
  //roll_radians *= -1;
  //mag_x *= -1;
  //roll_radians=0.0f;
  //pitch_radians=0;v
  //pitch_radians *=-1;

  cos_roll = cos(roll_radians);
  sin_roll = sin(roll_radians);
  cos_pitch = cos(pitch_radians);
  sin_pitch = sin(pitch_radians);


#if 0
  MAG_X = mag_x*cos_pitch+mag_y*sin_roll*sin_pitch+mag_z*cos_roll*sin_pitch;
  MAG_Y = mag_y*cos_roll-mag_z*sin_roll;
  tilt_compensated_heading = atan2f(-MAG_Y,MAG_X); 
#else
    MAG_X = mag_x * cos_pitch + mag_z * sin_pitch;
    MAG_Y = mag_x * sin_roll * sin_pitch + mag_y * cos_roll - mag_z * sin_roll * cos_pitch;
    tilt_compensated_heading = atan2f(-MAG_Y,MAG_X);
    //tilt_compensated_heading = atan2f(-mag_y,mag_x); // works fine when leveled
#endif

//convert to degree from 0-3599
    heading = tilt_compensated_heading * RAD_TO_DEG * 10;
     if ( heading < 0 ) 
     {
         heading += 3600;
     }

  return heading;
}

我尝试了各种组合，我试着只固定一个轴，并始终将一个轴保留为0，在任何输入上交换X/Y俯仰/滚动，*-1。

结果总是完全错误的。有时(取决于我通过试验/错误尝试反转或不反转某个值的位置)，这些值似乎几乎是线性的。有时，一个轴在正值区域被补偿。

然而，滚动和俯仰总是会导致“随机”的跳跃和航向的变化。

数学从来不是我的最爱，现在我后悔了。

我个人的猜测是，公式在原理上是正确的，mag在原理上是有效的(毕竟我在调级时获得了适当的学位)，但我不知何故给出了错误的东西。(就像Y和X需要交换一样，z*-1，间距需要是*-1)

能不能请擅长这门学科的人来看看，并指导我如何获得一个合适的标题？

如果今晚能有几个小时的睡眠，而不必再梦到一个功能失调的算法，那就太好了:)

更新:当指向305deg方向时，这里的倾斜补偿适用于负滚动。这里也用到了：http://www.emcu.it/MEMS/Compass/MEMS_Compass_A_RTM1v3.pdf

Answer 1

3天的工作，我终于发现我的问题和倾斜补偿是有效的！

我在不同的论坛上看到相当多的人有这样的问题，所以我做了以下工作：

我会一步一步地解释我做了什么以及为什么要这样做。也许有类似问题的人会以这种方式找到解决方案。

1. 玩弄这些值可能会有所帮助(可能只需要反转俯仰或滚动，交换X或Y)。

然而，如果你的问题不仅仅是一个很小的问题，那么有相当多的值和组合的数量太高了。

• 发布的公式运行良好(位于active #if分支中的那个。

如果你有一个磁力计，而atan2(-y，x)在平整时给你一个合适的方向，那么这个公式也适用于你。

• 我所做的是从I2C二进制逻辑(和数据表)开始，完整地检查了我所有的传感器和向量。

重要事项在这种特殊情况下(BMX055)，数据表方向页是错误的！

关于轴(x，y)的方向以及旋转为正或负时，存在多个错误。有时右手法则适用，有时不适用，并且图纸具有误导性(错误)。博世在记录这个芯片(和前一个芯片)时做得很糟糕。他们似乎不希望人们正确地理解它，他们写了几次关于融合API的优化定点算术和高级校准，但它不对公众可用。

我需要做的是建立一个合适的身体参照系。确定X的位置，然后让所有的传感器在俯仰/滚动到同一方向(正/负)时正确地改变X轴。对俯仰、滚动和重力/磁场执行此操作。

一旦他们在一起玩得很好，我又重新开始了。

标题公式仍然不起作用，但现在我第一次相信了纠缠者。

我添加了一个矢量旋转矩阵函数，并使用滚动、俯仰和yaw=0将磁矢量旋转回来。令人惊讶的是:磁矢量被倾斜补偿。

现在我知道这是可以做到的:)

我回到了最初的公式，用Y替换了X(因为我已经交换了它们来匹配身体参考系(陀螺仪/磁鼓的X和Y)。

现在，倾斜补偿适用于俯仰，但不适用于滚动。

所以我反转了roll_radians，突然它就完全补偿了倾斜。

我现在有两个解决方案。一个是旋转矩阵解，另一个是大家都在用的标准解。我会看看它们中是否有一个表现更好，如果结果值得的话，我可能会在这里给出最后的更新。

Answer 2

首先，验证它确实是一个软件问题。你的方程式似乎是正确的。以防万一，生成一个填充了测试数据的表以传递给您的过程，并在代码正确的情况下将函数的输出与期望值进行比较。

您使用的是磁力计，这些都是非常灵敏的设备。我要检查的第一件事是传感器附近是否有任何大型金属结构。如果你能接触到示波器，探测芯片的电源线，看看电源是否稳定。增加一个1uF解耦上限可以解决电源问题。我还建议在采样频率大于100 if时绘制一张图表，看看跳跃是否是周期性的。如果信号是周期性的，频率为50 is，假设传感器没有移动，则表示来自电源的干扰。对数据执行FFT分析不会有什么坏处。我们也有过类似的问题，因为电源线在我们的实验室地板下运行。如果传感器继续随机跳动，芯片很可能已经死了。在处理它时，您是否使用了适当的ESD防护措施？

希望这能有所帮助。