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帧的解析功能。

    三、添加顶层驱动的调用

    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;
    }

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

如有其它疑问，请QQ联系：3500985284