| MP地面站二次开发教程（三）Mavlink通讯协议（4）地面站数据显示调试及其指令填充 | 您所在的位置：网站首页 › mp地面站教程 › MP地面站二次开发教程（三）Mavlink通讯协议（4）地面站数据显示调试及其指令填充 |
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 { /// The new autopilot-specific mode. This field can be ignored by an autopilot. public uint custom_mode; /// The system setting the mode public byte target_system; /// The new base mode MAV_MODE 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
generatePacket((byte)MAVLINKMSGID.COMMAND_LONG, req, sysid, compid);这个是具体的发送函数，把填充好的一包数据指令发送出去。飞控接收到相关指令，就会执行相应的动作。 |
【本文地址】