【手搓固定翼fpv头追研究笔记】(三) 磁力计校准和9轴数据解算

序

博主因为是高三牲，最近比较忙，不能像第一篇那样详细介绍，但也保证各位看了我的文章能够正确的应用介绍的内容

磁力计的校准

主要是做磁力计数据的椭球拟合以修正磁力计的误差：
磁力计仪的干扰有两种：

• 硬磁干扰，是周边电子元器件，金属器件或磁体产生的磁场对磁力计的干扰，电路板上不可能只存在磁力计一个设备，一定存在其他设备带来的电磁干扰，这个干扰表现为对磁力计数据在三个轴方向上的偏移
• 软磁干扰，与其说是干扰，不如说是磁力计本身对三个轴向的磁力灵敏度不同导致的，软磁干扰使本应各向最大值为一个球形的数据范围变成一个椭球，也就是发生的变形

也就是说，解决软磁干扰，我们要将XYZ各轴的灵敏度统一，使其读取的数据的最大值落在一个球面上，解决硬磁干扰，我们要将XYZ各轴的偏移量归零，使球的中点落在坐标轴原点上

具体解决措施：
参考这篇文章，利用python作为上位机读取各方向磁力计数据而后带入矩阵利用最小二乘法得出所需要的参数：https://blog.csdn.net/weixin_44457994/article/details/121677784

注意：收集数据时：

1. 远离铁器，线圈，电容等电磁干扰，如：电脑，显示器，音响，麦克风等，否则校准之后的值是错误的，建议拿长一点的usb线原理之后再进行校准
2. 尽可能多收集数据，各角度各方向都需要经过，以获得最好的校准效果

9轴数据解算

上篇笔记我们介绍了madgwick算法解算6轴数据，现在我们的磁力计数据也已经得到，可以进行完全体的数据解算了：
在使用数据解算之前，我们还需要修改一部分东西：
首先在Madgwick.c中，我们需要将上一步的数得到的参数用来将hmc5883l原始数值转换为校准后的数值

在这里：

    mx = (float)mx_raw;
    my = (float)my_raw;
    mz = (float)mz_raw;

将我们得到的参数填入（这里根据自己的实际情况）

    mx = 0.9995799647559379f * ((float) mx_raw + 43.11426164828043f);
    my = 0.9313531251233554f * ((float) my_raw + 417.92855949271035f);
    mz = 1.080061520013562f * ((float) mz_raw + 51.083187110196775f);

使用
与上一章6轴数据解算的方式一样，先声明一个AttiAgls结构体用于存储三个姿态角的值，在数据解算开始之前的最后一步运行MadgwickAHRSInit();用于将madgwick_timer中的数据刷新至最新，而后在每一个训练周期调用MadgwickAHRSupdate_9()输入原始数据进行数据解算，然后调用Filter_Get_AttitudeAngles()获取姿态角

示例代码：

#include "mpu6050.h"
#include "hmc5883l.h"
#include "Madgwick.h"
#include "usbd_cdc_if.h"

int main(void)
{
	//原始数据包
    MPU6050_Data mpu_data;
    HMC_Data hmc_data;

	//姿态角
    AttiAgls attitudeAngles;

	MPU_6050_Init();//MPU6050初始化
    MPU6050_calibrate();//MPU6050校准
    HMC_Init();//HMC5883l初始化
    
	MadgwickAHRSInit();
	while(1)
	{
		//获取原始数据
        MPU_Read_ALL(&mpu_data);
        HMC_Read_ALL(&hmc_data);

		MadgwickAHRSupdate_9(mpu_data.Accel_X_RAW,
                             mpu_data.Accel_Y_RAW,
                             mpu_data.Accel_Z_RAW,
                             mpu_data.Gyro_X_RAW,
                             mpu_data.Gyro_Y_RAW,
                             mpu_data.Gyro_Z_RAW,
                             hmc_data.Magnetic_X_RAW,
                             hmc_data.Magnetic_Y_RAW,
                             hmc_data.Magnetic_Z_RAW);
        Filter_Get_AttitudeAngles(&attitudeAngles);
     	usb_printf("%.2f,%.2f,%.2f\n",
                   attitudeAngles.pitch,
                   attitudeAngles.roll,
                   attitudeAngles.yaw);
	}
}

上位机测试
利用vofa+的陀螺仪测试功能进行上位机测试观看测试结果：

9轴陀螺仪测试