MP地面站二次开发教程（三）Mavlink通讯协议（4）地面站数据显示调试及其指令填充

1、利用SimpleExample和Mavlink inspector的调试使用

2、Mavlink设置模式指令的填充

3、Mavlink控制指令的填充

4、MAV_CMD

在地面站做一些显示和指令写入的时候，因为PX4和Ardupilot固件的差异，在显示和指令写入都有不同。会有一些调试技巧，来帮助更好的调试。本次课程主要的MAVLINK库是剥离于MP的代码，所以课程的重点也是讲解ardupilot固件的地面站。甚至不同机型用不同的地面站，来降低地面站的兼容性和复杂程度。下面的PX4的固件地面站的编写只是做一个方向说明，在第三章不做深入讲解。

1、利用SimpleExample和Mavlink inspector的调试使用

如果调试Ardupilot固件SimpleExample(在前几节介绍过)，我们只要修改一下代码就可以知道打印出当前的消息ID。这个可以直观看到消息ID，然后我们在http://mavlink.org/messages/common 里面对于也可以找到目前飞控系统对外发布了什么消息，找到我们想要的消息，解析即可。

如果调试PX4的固件也有比较好的工具就是：

Mavlink inspector这是Mavlink消息监视工具，不过这个工具也支持Ardupilot固件。QGC的地面站同时指出Ardupilot固件和PX4的固件，其实QGC的软件也非常强大和开源，只是过年用的人比较少。用QGC这个工具也非常方便调试MAVLINK消息。

2、Mavlink设置模式指令的填充

虽然MAVLINK库做了比较好的封装，但是还是有一些填充要点要注意，不同机型的填充指令的区分，Ardopilt和PX4固件的多机型支持，尤其是模式指令发送，不同的机型的模式不一样，发送的指令ID也不一样，有些模式的发送要跟踪到控制器代码里面典型模式发送的mavlink库函数如下

 public void setMode(byte sysid, byte compid, string modein)
{mavlink_set_mode_t mode = new mavlink_set_mode_t();if (translateMode(sysid, compid, modein, ref mode)){setMode(sysid, compid, mode);}
}public void setMode(byte sysid, byte compid, mavlink_set_mode_t mode, MAV_MODE_FLAG base_mode = 0)
{mode.base_mode |= (byte)base_mode;generatePacket((byte)(byte)MAVLINK_MSG_ID.SET_MODE, mode, sysid, compid);Thread.Sleep(10);generatePacket((byte)(byte)MAVLINK_MSG_ID.SET_MODE, mode, sysid, compid);
}

translateMode 是做固件选择的函数，这个函数负责模式翻译，把不同类型的机架的模式控制筛选出来。这也是MP地面站支持不同种机型的地方，但是虽然里面对于PX4有选择的代码，但是支持还是不完善确切的就是不支持PX4固件。在模式设置里面要填充

public struct mavlink_set_mode_t
{/// <summary> The new autopilot-specific mode. This field can be ignored by an autopilot. </summary>public  uint custom_mode;/// <summary> The system setting the mode </summary>public  byte target_system;/// <summary> The new base mode MAV_MODE</summary>public  /*MAV_MODE*/byte base_mode;};

这个结构体要填充，其中basemode是1，固定的值。 custommode不同机型设置的值都不一样。
在旋翼固件里面如下： \ardupilot-master\ardupilot-master\ArduCopter\defines.h

// Auto Pilot Modes enumeration

enum control_mode_t {
STABILIZE =     0,  // manual airframe angle with manual throttle
ACRO =          1,  // manual body-frame angular rate with manual throttle
ALT_HOLD =      2,  // manual airframe angle with automatic throttle
AUTO =          3,  // fully automatic waypoint control using mission commands
GUIDED =        4,  // fully automatic fly to coordinate or fly at velocity/direction using GCS immediate commands
LOITER =        5,  // automatic horizontal acceleration with automatic throttle
RTL =           6,  // automatic return to launching point
CIRCLE =        7,  // automatic circular flight with automatic throttle
LAND =          9,  // automatic landing with horizontal position control
DRIFT =        11,  // semi-automous position, yaw and throttle control
SPORT =        13,  // manual earth-frame angular rate control with manual throttle
FLIP =         14,  // automatically flip the vehicle on the roll axis
AUTOTUNE =     15,  // automatically tune the vehicle's roll and pitch gains
POSHOLD =      16,  // automatic position hold with manual override, with automatic throttle
BRAKE =        17,  // full-brake using inertial/GPS system, no pilot input
THROW =        18,  // throw to launch mode using inertial/GPS system, no pilot input
AVOID_ADSB =   19,  // automatic avoidance of obstacles in the macro scale - e.g. full-sized aircraft
GUIDED_NOGPS = 20,  // guided mode but only accepts attitude and altitude
SMART_RTL =    21,  // SMART_RTL returns to home by retracing its steps
FLOWHOLD  =    22,  // FLOWHOLD holds position with optical flow without rangefinder
FOLLOW    =    23,  // follow attempts to follow another vehicle or ground station
};

\ardupilot-master\ardupilot-master\ArduPlane\defines.h

enum FlightMode {
MANUAL        = 0,
CIRCLE        = 1,
STABILIZE     = 2,
TRAINING      = 3,
ACRO          = 4,
FLY_BY_WIRE_A = 5,
FLY_BY_WIRE_B = 6,
CRUISE        = 7,
AUTOTUNE      = 8,
AUTO          = 10,
RTL           = 11,
LOITER        = 12,
AVOID_ADSB    = 14,
GUIDED        = 15,
INITIALISING  = 16,
QSTABILIZE    = 17,
QHOVER        = 18,
QLOITER       = 19,
QLAND         = 20,
QRTL          = 21
};

PX4固件

# Main state, i.e. what user wants. Controlled by RC or from ground station via telemetry link.
uint8 MAIN_STATE_MANUAL = 0
uint8 MAIN_STATE_ALTCTL = 1
uint8 MAIN_STATE_POSCTL = 2
uint8 MAIN_STATE_AUTO_MISSION = 3
uint8 MAIN_STATE_AUTO_LOITER = 4
uint8 MAIN_STATE_AUTO_RTL = 5
uint8 MAIN_STATE_ACRO = 6
uint8 MAIN_STATE_OFFBOARD = 7
uint8 MAIN_STATE_STAB = 8
uint8 MAIN_STATE_RATTITUDE = 9
uint8 MAIN_STATE_AUTO_TAKEOFF = 10
uint8 MAIN_STATE_AUTO_LAND = 11
uint8 MAIN_STATE_AUTO_FOLLOW_TARGET = 12
uint8 MAIN_STATE_AUTO_PRECLAND = 13
uint8 MAIN_STATE_MAX = 14
uint8 main_state                # main state machine

3、Mavlink控制指令的填充

  public bool doCommand(byte sysid, byte compid, MAV_CMD actionid, float p1, float p2, float p3, float p4, float p5, float p6, float p7, bool requireack = true, MethodInvoker uicallback = null){giveComport = true;MAVLinkMessage buffer;mavlink_command_long_t req = new mavlink_command_long_t();req.target_system = sysid;req.target_component = compid;req.command = (ushort)actionid;req.param1 = p1;req.param2 = p2;req.param3 = p3;req.param4 = p4;req.param5 = p5;req.param6 = p6;req.param7 = p7;//log.InfoFormat("doCommand cmd {0} {1} {2} {3} {4} {5} {6} {7}", actionid.ToString(), p1, p2, p3, p4, p5, p6,//p7);generatePacket((byte)MAVLINK_MSG_ID.COMMAND_LONG, req, sysid, compid);

} 发送指令的基本函数其中MAVCMD是指令ID 在Mavlink/Mavlink.cs的
public enum MAVCMD: ushort

包含了航点指令，悬停指令，一键起飞指令，跟踪指令，降落指令。mavlinkcommandlong_t是一个结构体，每次发送以前填充好这结构体里面的内容。不同指令参数不一样在http://mavlink.org/messages/common

4、MAV_CMD

generatePacket((byte)MAVLINKMSGID.COMMAND_LONG, req, sysid, compid);这个是具体的发送函数，把填充好的一包数据指令发送出去。飞控接收到相关指令，就会执行相应的动作。