APM（Pixhawk）飞控添加自定义驱动程序的方法

怒而飞，其翼若垂天之云！我是怒飞。

无人机调试问题，飞控硬件定制，固件修改，log日志分析，飞控驱动添加

请QQ联系：3500985284

  

  APM飞控（运行在Pixhawk硬件之上）添加一个新的硬件驱动其实很容易，如果你想添加一个新的串口外设，比如超声波、毫米波雷达、激光测距仪、避障模块等等，可以参考飞控中原有的类似驱动新建一个驱动程序，然后稍作修改就行了，下面以一个RTK GPS（司南导航K系列）为例，介绍一下如何添加一个新的驱动。

    思路：APM代码中，已经有多个不同厂家的GPS驱动，我们要添加的GPS（含RTK）的驱动与其中的某些驱动有很大的相似之处，因此可以复制该驱动，在其基础上修改，从而减少了很多工作量。

    APM代码中GPS驱动在ardupilot/libraries/AP_GPS中，其中AP_GPS.cpp为前台顶层文件，用来与更顶层的飞控代码交互，提供解析后的GPS信息。GPS_Backend.cpp提供不同厂家、不同协议GPS的统一接口，不同厂家的驱动为AP_GPS_XXX.cpp（如ublox的为AP_GPS_UBLOX.cpp）,AP_GPS_XXX.cpp都是从GPS_Backend基类派生出来的（不得不说，在这一点，C++还是比C语言有挺大的优势）。

    一、新建驱动文件

    我们打算用NMEA协议，因此与之最接近的是“AP_GPS_NMEA.cpp”，将“AP_GPS_NMEA.cpp、AP_GPS_NMEA.h”复制并粘贴在AP_GPS文件夹，将其重命名为AP_GPS_ComNav.cpp和AP_GPS_ComNav.h，打开Eclipse，将工程刷新一下，新添加的文件将自动被添加到工程中，不需要任何设置，非常方便。

    二、修改驱动文件

    打开AP_GPS_ComNav.cpp和AP_GPS_ComNav.h文件，将其中的AP_GPS_NMEA替换为AP_GPS_ComNav，然后根据自己的需要，添加自定义的协议的解析。比如使用的是司南导航的K726双天线板卡，想提取航向信息，就需要添加GPYBM帧的解析功能（2019年3月7日更新：这里在AP_GPS_NOVA.cpp的基础上修改更方便，直接是二进制协议，传输效率更高，解析也很方便。另外，这个代码中的协议解析方法实在是太妙了，使用按字节对齐的union，在数据接收完成时其实就完成了数据解析，比我们之前一个字节一个字节拼接的方法高明很多）。

    三、添加顶层驱动的调用

    1、AP_GPS.h第86行，添加一种新的GPS类型，GPS_TYPE_COMNAV = 16；

    2、AP_GPS.cpp第37行，添加“#include “AP_GPS_ComNav.h””；

    3、AP_GPS.cpp第67行、75行，添加注释“16:COMNAV”；

    4、AP_GPS.cpp第413行添加如下代码：

    if(_type[instance] == GPS_TYPE_COMNAV)
    {
        dstate->detect_started_ms = 0; // specified, not detected
        new_gps = new AP_GPS_ComNav(*this, state[instance], _port[instance]);
        goto found_gps;
    }

    四、编译下载后，在飞控参数中设置GPS类型为16，设置好波特率，就可以了。

      文字+图片的方法感觉还是说不清楚，录个视频应该会更方便一点，大家有需求的话，请按照下面的联系方式告知我一下，如果需求足够，我可以录制一系列的视频，从装机到调参，从改代码到试飞，从飞控硬件设计到焊接等等（北航人，搞无人机9年，懂点飞机☺）。

怒而飞，其翼若垂天之云！我是怒飞。

无人机调试问题，飞控硬件定制，固件修改，log日志分析，飞控驱动添加，请QQ联系：3500985284