飞控代码分析和高精度IMU传感器的使用

最近着急备赛很多文档只是处于零零散散状态没办法细节的整理出来专门放一份，等比赛结束会统一整理，现在只能先放一些研发过程中的技术文档上来了
目前我们的调试进度是，稳定完成了无人机通过openmv视觉识别跟随小车的任务，这也同时是我们对今年赛题的简单预判，我们还做了很多其他的准备。
文档现在是接着上次的继续往下写，很多东西还没写到，最近因为觉得mpu6050数据抖动太严重我们通过JTAG去仔细看了确实问题很大于是更换了更贵更好的传感器，但是在这一趟旅途中出现了好多好多的问题，不得不说是一次极限挑战，从IIC协议调不通到解算失败再到数据拟合度不够，出现问题就解决问题，最终还算是收获了比较好的结果，但是整整花了超过三天的时间，时间开销比预计打了两倍，这部分文档有点多，后续慢慢发上来。

飞控主程序研究关于姿态控制和运动学解算部分的主体逻辑框架

飞控主程序框架中首先在之前的状态机决策模型上分析得到，在理解开源代码和重构的过程中，我选择了自顶层下下的方法，一层一层分析了多个字程序间的调用关系，并且基于全局变量的架构过程和通过Debug过程中重设追踪式breakpoint的方法追踪了变量的更改逻辑捕捉到它在内存中的读写操作，通过该技术工具甚至可以确认当前追踪的变量在总体程序执行的时序过程中被多少个变量进行了改动。如图
在这里插入图片描述

在这里插入图片描述

飞机的姿态控制来源于三个维度的数据，

第一个维度是光流模组，这一模组包含了超声波传感器，光流传感器，具体的解算程序作为黑盒由匿名科创公司提供，这一部分的程序产生的数据包括了绝对高度信息和xy两个轴向的对地速度速度在光流信息解耦后和iMU数据进行融合后获取了相对稳定数据输出，能够较好的表述出飞机当前的对地运动情况进而可以进行稳定的悬停，但是由于超声波传感器的测量会受到飞机的气流和噪声影响我们正在考虑进行跟换，目前已经在openmv模组上加入了tof激光测距模组，在之后会考虑在单片机的系统中再多挂载激光面阵雷达和更多tof来进行精准的定位，因为目前飞机的姿态控制并不能完全达到理想的状态。

在这里插入图片描述

第二个维度是IMU系统，该系统中目前包括了mpu6050模组，模组通过IIC总线提供了如图所示的可接受控制范围内的数据输出，这些数据包括了六轴上的信息，六轴分别为xyz加速度，角速度，来源分别为MPU6050中的加速度计和陀螺仪，对其进行数据融合，可以计算出当前飞机的四元数得出欧拉角，进而获得飞机的自身姿态，算法具体细节在下一段中讲解，这里只讲宏观框架。

第三个维度是控制系统，控制系统在飞控系统中是最为重要的部分，一般FMU指的也是飞控系统单元，这部分在飞行器的程序中好比是飞行器程序的骨架，他将传感器控制算法优化算法集合在一起形成了对飞行器电机的控制使得它能够完成各种必要的任务，在我们的程序中控制部分主要采用了前面提到的两个维度作为其PID控制器的参数，期望值被设定为遥控器值和自动控制程序的结合体，这一部分是程序中最为关键的部分，当后面由于稳定性问题不得不更换传感器的时候在这里遇到了相当头疼的问题。
在这里插入图片描述
我们还通过上位机写了利用数传回传数据进行姿态仿真建模的unity3d程序

飞控主程序的程序来源之MPU6050的数据采集逻辑

飞控主程序分析之MPU6050的数据采集方法，MPU的数据分析主要包括了两部分，一部分是基础数据校准，另一部分是IMU对于MPU6050数据解析部分。

在这里插入图片描述

Wz_Est_Calcu是对齐东北天坐标系的z轴计算方法，其内含方法可以理解为是进行了一次z轴对加速度的积分获得速度，再对速度积分获得位移量，但是这种算法的效果相对误差较大，所以也进行了诸多的传感器的数据融合使得最终的数据可信度更高，飞行也能够更稳定。但是还是不太好用，比起真正的东北天坐标系传感器的基于磁场的计算效果要差很多不过也效果相当对得起价位了。我们的设备为了节省经费在传感器方面选用的确实比较廉价。

接下来主要讨论MPU6050数据的获取逻辑

首先MPU必须进行的工作是自检，链接是否正常，这直接关系到安全，所以这一项检查也相对繁琐，首先是寄存器收发写入功能正常

    uint8_t check = 0;

    check = I2C_Read_Byte(MPU6050_ADDRESS, 0x75);  //判断g_MPUManager地址
    
    if(check != MPU6050_PRODUCT_ID) //如果地址不正确
    {
    
    
        g_MPUManager.Check = false;
        return false;
    }
    else{
    
    ...}

然后通过复位采样设置时钟源设置低通滤波器标定单位操作确定传感器的输出值是我们期望的，其中最重要的，必须要在初始化完成后计算补偿值我们称其为offest值，offest值在控制实现这个值得过程中每一次进入都要根据上一次的情况进行修正

 memset(g_MPUManager.Offset, 0, 12);
    g_MPUManager.Offset[2] = 2048;   //根据手册量程设定加速度标定值 

    //丢弃前300个数据
    for(int i = 0;i < 300;i++)
    {   
        Delay_ms(2);
        GetMPU6050Data();
    }

其他的一系列确认包含了确认飞机的平放，是否已经稳定，均值滑动滤波等操作，具体以后有机会细说

接着是MPU6050GETDAtA的工作，这个相对简单只是做了buffer的数据存放拼接低通滤波也放在一起做了，代码如下

void GetMPU6050Data(void) 
{
    static float mpu_filter[2][6];
    int16_t mpu_filter_tmp[6];
    uint8_t buffer[12];

    //读取加速度计数据和陀螺仪数据
    Acc_Read();
    Gyro_Read();

    
    for(int i = 0; i < 6; i++)
    {
        //拼接读取到的原始数据
        mpu_filter_tmp[i] = (((int16_t)buffer[i << 1] << 8) | buffer[(i << 1) + 1])
                - g_MPUManager.Offset[i];

        //原始数据LPF
        mpu_filter[0][i] += 0.3f *(mpu_filter_tmp[i] - mpu_filter[0][i]);
        mpu_filter[1][i] += 0.3f *(mpu_filter[0][i]  - mpu_filter[1][i]);

        //赋值到结构体
        pMpu[i] = (int16_t)mpu_filter[1][i];
    }
}

飞控主程序IMU数据解算的对接和目前的数据融合逻辑

飞控的IMU数据解算主要使用的是AHRS解算算法，这个算法的流程大概展示如下

关于电子设计大赛无人机题的学习笔记(2)飞控代码分析和高精度IMU传感器的使用

飞控代码分析和高精度IMU传感器的使用

飞控主程序研究关于姿态控制和运动学解算部分的主体逻辑框架

飞控主程序的程序来源之MPU6050的数据采集逻辑

飞控主程序IMU数据解算的对接和目前的数据融合逻辑

猜你喜欢