三轴地磁方位角原理及代码实现

开发平台:Arduino
控制板:Mbsbot-Mega2560版本
姿态传感器:GY85九轴模块

实现目标: 通过地磁计模块获取姿态角，位移量，估算当前相对位移

1.获取地磁计方位角

库函数获取地磁计原始数据

  float* magScaled = sen10724.readMagScaled();max_x = max(magScaled[0], max_x);min_x = min(magScaled[0], min_x);max_y = max(magScaled[1], max_y);min_y = min(magScaled[1], min_y);

这里使用库函数获取到三轴磁力计原始数据，每次采样都保存最大值和最小值，

  mag_angle_x = line_remap(magScaled[0], min_x, max_x, -1, 1);mag_angle_y = line_remap(magScaled[1], min_y, max_y, -1, 1);      //将磁偏角映射到-1~1

将磁偏角映射到单位圆中，转换为实际方位角

获取的方位角为0-360度，在越过0点的时候会有一个跳变的数据，会影响到实际的方位角

这里通过获取偏差进行输入补偿，得到的波形已经可以反映当前方位角的信息了

      if(abs(mag_angle - mag_last)  > max){mag_zero_bias = mag_angle - mag_last;}else{mag_zero_bias = 0;}if(mag_zero_bias != 0){mag_Bias += mag_zero_bias;  //获取偏移}Bias_value = mag_angle - mag_Bias;mag_last = mag_angle;

使用一阶低通滤波器优化后得到的方位角更加稳定

float Lowpass_filter(float Value)
{static int Last_val;int Save_val;Save_val = Value;Value *= 0.7;Value += Last_val * 0.3;Last_val = Save_val;return Value;
}

有了前面的方位角信息，接下来通过加速度计获取到里程就可以估算出一次运动的实际位移矢量了。