c - 倾斜磁力计输出 - 俯仰和滚动倾斜补偿 - 我迷路了

Question

我在这个特定问题上浪费了 2 天(和两夜)，但在倾斜补偿我的磁力计输出方面完全失败了。
我尝试了我在谷歌和开源示例中可以阅读的所有内容，但没有任何内容指导我对两者(滚动和俯仰)进行适当的倾斜补偿。 .

设置:我使用校准值而不是统一值。
我有一个融合的重力 vector ，它工作正常且准确。
传感器是 9dof BMX055 ( http://www.mouser.com/ds/2/621/BST-BMX055-DS000-01-274824.pdf )每个轴上的磁力计最小值/最大值为 +- 512(差异很小，但所有轴都归零)。硬件为Sam3X8e(Cortex M3)，全部用C语言编写。
float 和三角函数可以很快完成，所以这没问题。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。

结果总是完全错误的。有时(取决于我尝试通过试验/错误来反转或不反转值的位置)这些值似乎接近线性。有时在正区域补偿一个轴。
然而，滚动和俯仰总是会导致“随机”跳跃和航向变化。
数学从来不是我的最爱，现在我后悔了。

我个人的猜测是公式在原则上是正确的，杂志在原则上是有效的(毕竟我在升级时获得了适当的学位)但我不知何故喂错了东西。(比如Y和X需要互换，z*-1，pitch需要*-1)

谁能看一看这方面的知识并指导我如何获得正确的标题？
今晚能睡上几个小时，而不必再梦到功能失调的算法，那就太好了 :)

更新:当指向 305 度航向时，此处的倾斜补偿适用于负侧倾。也用在这里:http://www.emcu.it/MEMS/Compass/MEMS_Compass_A_RTM1v3.pdf

Answer 1

3 天的工作，终于我找到了我遇到的问题，倾斜补偿正在工作!
我在各种论坛上看到很多人遇到过这样的问题，所以我是这样做的:

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

1. 玩弄这些值会有所帮助(也许只是需要颠倒俯仰或滚动，必须交换 X 或 Y)。
   但是，如果您的问题不止一个小问题，那么值会很多，并且组合的数量会太多。

2. 发布的公式工作正常(在事件 #if 分支中的那个。
   如果您有磁力计，并且 atan2(-y,x) 在放平时为您提供正确的航向，那么这个公式也适用于您。

3. 我所做的是从 I2C 二进制逻辑(和数据表)开始完全检查我所有的传感器和 vector 。
   在此特殊案例 (BMX055) 中很重要，数据表方向页面错误!
   关于轴 (x,y) 的方向以及旋转为正或负时，存在多个错误。有时右手法则适用，有时不适用，并且图纸具有误导性(错误)。博世在记录该芯片(以及之前的芯片)方面做得很糟糕。他们似乎不希望人们正确理解它，他们多次使用优化的定点算法和高级校准编写融合 API，但它不对公众开放。

我需要做的是制作一个合适的 body 引用系统。自己决定 X 的位置，然后让所有传感器在向同一方向(正/负)倾斜/滚动时正确地改变 X 轴。对俯仰、滚动和重力/磁场执行此操作。

一旦他们一起玩得很好，我就重新开始。
标题公式仍然不正常，但现在我第一次相信了 vextors。
我添加了一个 vector 旋转矩阵函数，并使用 roll 和 pitch 以及 yaw=0 将磁 vector 旋转回来。惊喜:磁 vector 被倾斜补偿。

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

我回到原来的公式，用 Y 替换 X(因为我已经交换它们以匹配 body 引用系统(陀螺仪/磁体的 X 和 Y)。
现在倾斜补偿适用于俯仰但不适用于横滚。
所以我反转了 roll_radians 突然它得到了完美的倾斜补偿。

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