Feature Request: Add Mavlink Guided Mode via MAV_CMD Handling

[xznhj8129]

Current Behavior

Currently INAV does not implement handling MAV_CMD messages (COMMAND_INT, COMMAND_LONG) which is how most Mavlink control is done, so most commands will fall on deaf ears. We say INAV "can't do" bi-directional MAVLINK control, but it's really just not implemented yet, there's nothing stopping us. A big portion of those commands are either not necessary for INAV or just untranslateable because of fundamental differences (ie, changing flight modes). One very useful mode is Guided mode, which already exists in INAV in the form of GCS NAV, but there is no way for now to use ie QGC or MP to control an INAV craft in Guided mode.

Desired Behavior

When the craft is in LOITER+GCS NAV mode, it should 1. return GUIDED as it's mavlink mode (PR already submitted), and 2. it should allow the GCS to command it by setting Waypoint 255.

Suggested Solution

Basically it needs to be able to handle MAV_CMD commands in processMAVLinkIncomingTelemetry(), namely COMMAND_INT messages. LONG vs INT is explained here

It would look something like this:

static bool processMAVLinkIncomingTelemetry(void)
{
    while (serialRxBytesWaiting(mavlinkPort) > 0) {
        // Limit handling to one message per cycle
        char c = serialRead(mavlinkPort);
        uint8_t result = mavlink_parse_char(0, c, &mavRecvMsg, &mavRecvStatus);
        if (result == MAVLINK_FRAMING_OK) {
            switch (mavRecvMsg.msgid) {
                case MAVLINK_MSG_ID_HEARTBEAT:
                   return handleIncoming_HEARTBEAT();
                ...
                case MAVLINK_MSG_ID_COMMAND_LONG;
                    handleIncoming_COMMAND_LONG();
                case MAVLINK_MSG_ID_COMMAND_INT;
                    handleIncoming_COMMAND_INT();

And then the main function to handle COMMAND_INT messages, which encapsulates many other types of messages, but namely handles MAV_CMD_DO_REPOSITION (for copters) and if BOXGCSNAV is active sets Waypoint 255 and returns the right message to the GCS (this Mavlink code might be wrong, i haven't completed it yet, it's for demonstration).

static bool handleIncoming_COMMAND_INT(void)
{
    mavlink_command_int_t msg;
    mavlink_msg_command_int_decode(&mavRecvMsg, &msg);

    // Check if this message is for us
    if (msg.target_system == mavSystemId) {

        if (msg.command == MAV_CMD_DO_REPOSITION) {
            if (IS_RC_MODE_ACTIVE(BOXGCSNAV)) {
                navWaypoint_t wp;
                wp.action = NAV_WP_ACTION_WAYPOINT;
                wp.lat = (int32_t)(msg.x * 1e7f);
                wp.lon = (int32_t)(msg.y * 1e7f);
                wp.alt = msg.z * 100.0f;
                wp.p1 = 0;
                wp.p2 = 0;
                wp.p3 = 0;
                wp.flag = 0;
                setWaypoint(255, &wp);
                mavlink_msg_command_ack_pack(mavSystemId, mavComponentId, &mavSendMsg, mavRecvMsg.sysid, mavRecvMsg.compid, MAV_MISSION_ACCEPTED, MAV_MISSION_TYPE_MISSION);
                mavlinkSendMessage();
            }
            else {
                mavlink_msg_command_ack_pack(mavSystemId, mavComponentId, &mavSendMsg, mavRecvMsg.sysid, mavRecvMsg.compid, MAV_MISSION_INVALID, MAV_MISSION_TYPE_MISSION);
                mavlinkSendMessage();
            }

So the craft will be on it's way to the new position.

Who does this impact? Who is this for?

This is going to be a big improvement for people who want to use Mavlink with INAV, Guided mode is a very useful and popular way of controlling non-FPV planes and copters with a GCS or co-computers using Mavsdk or similar. The code to handle MAV_CMD messages is also going to be the entry point for implementing more and more Mavlink functionality as to make INAV more compatible and interoperable with many more types of GCS software including Mission Planner, QGC, APMPlanner, UASTOOL, etc. This will also make stuff like ELRS-Mavlink and mLRS a lot more useful with INAV.

Additional context

I already started the beginning of a (probably not the best) implementation here, i mostly covered the basis above

master...xznhj8129:inav:mavlink_gcs_nav

There is an additional small issue that in some GCS, namely QGC and MP, they do not correctly map the modes returned by INAV; ie they show "Custom mode 0x4" or something instead of the correct ie ACRO; and that locks out most of the mode-related commands (such as GOTO). I found that changing MAV_AUTOPILOT_GENERIC to MAV_AUTOPILOT_ARDUPILOTMEGA in mavlink_msg_heartbeat_pack will make them correctly recognize the modes and unlock the functionality, but it makes them very angry that they're not getting parameters. Interestingly, Mavproxy has no problem with that. I haven't PR'ed this change because i'm not sure if this causes effects i don't know about and i'm not sure where the problem lies exactly, in INAV or the GCS softwares; someone more knowledgeable could look into that.

Some other MAV_CMD commands (not this one) also take some time to complete, so you also need to send IN_PROGRESS messages while waiting for the result, see here https://mavlink.io/en/services/command.html#long_running_commands

[MrD-RC]

I'm wondering if the GCS ever gets any feature requests to use the MSP protocol? That would make the GCS more flexible. Instead, requests always go to the firmware, which is limited on space. The right solution to me, would be to use a GCS that works with MSP. Not force INAV to implement a protocol to work with ArduPilot's GCS.

[xznhj8129]

Space is an issue, which is definitely why there's a hard limit on mavlink implementation (although this specific feature wouldn't take much space at all), i've started a halfway compromise that is a translator/meta-controller running on a Pi that acts as mediator between MSP and Mavlink, but it's far from ideal or reliable.

There are unfortunately no MSP GCS that run on android and no MSP SDKs at all (save for the one i am working on in my free time as an amateur, forked from a 10yo project) and can't possibly be trusted with real airframes). I think MWPtools is the only MSP GCS still alive, everything else is 10 years dead, and up to date documentation is pretty hard to find too.

[sensei-hacker]

I'm wondering if the GCS ever gets any feature requests to use the MSP protocol?

I have asked about that in the QGroundControl Discord. Note on GitHub, QGroundControl is under the Mavlink/ organization, so that may be a hint.

The right solution to me, would be to use a GCS that works with MSP. Not force INAV to implement a protocol to work with ArduPilot's GCS.

I've said the same many times. You've probably seen me say that in the INAV Discord.
Yet at this point there are a bunch of ground control options - Mission Planner, QGroundControl, Cockpit, and others. All of which use Mavlink. Mavlink isn't just Mission Planner anymore. At the same time, the MSP-based options have gone away. There's ONE GUY maintaining one GCS that uses MSP. All of the others use Mavlink. Over the last few years, Mavlink has become the de facto standard for GCS.