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.

[xznhj8129]

I somehow missed this notification. Thank you, yes that's basically the situation now. I didn't want to make this issue the central "Mavlink adoption question" debate stage but it's clear that Mavlink has imposed itself as the standard for GCS, APIs and even simulators. The more compatible we become with it, the more we can benefit from the mountain of work that's been done for APM and Pixhawk. It would allow us to use MAVSDK as an API and Airsim for quadcopter simulation, for example. MSP is absolutely fine for configuring the aircraft and simple tools, but for everything else we're really missing out which is a shame considering how great Inav has become.

[b14ckyy]

My 2 cents from a heavy user perspective:
Adding Mavlink functions for things like external hardware support, telemetry, even RC control is one thing as its straight forward.
But supporting Mavlink based GCS software will be an absolute mess.

The problem is not the protocol itself but the way INAV Flight Modes work and are named.
As an example: INAV's ANGLE mode is identical to Ardupilot's FBWA. But Ardupilot's dedicated FBWB Mode has no direct INAV equivalent. In INAV that would be a combined ANGLE Mode with the ALTHOLD Modifier.
INAV HORIZON mode has no actual equivalent in ardupilot, the closest one would be STABILIZE but it acts differently still.

The same happens for WP missions. Ardupilot has dedicated WP functions for every action. INAV has Waypoints and WP additions, that are technically waypoints itself but act together with the previous waypoints to add functions. User-Actions are also not supported by any GCS applciation for Ardupilot.

So the problem is NOT the protocol. the problem is that the GCS needs to support the features of the firmware and that is just not the case. We could add a few workarounds and translations in firmware to support a few more things but that would be a complete mess for the user as long as the GCS is not filtering out all unsupported functions.

if QGC devs or MP devs would say "hey, lets add all the modes and functions that INAV supports in our application but we don't want to use MSP!" then it would be not a big deal to fully implement that on the INAV end. Right now that makes no sense at all.

[sensei-hacker]

As an example: INAV's ANGLE mode is identical to Ardupilot's FBWA. But Ardupilot's dedicated FBWB Mode has no direct INAV equivalent. In INAV that would be a combined ANGLE Mode with the ALTHOLD Modifier.

I think this nicely summarizes the problem. And much of the solution. It's not a really tidy translation where we have the exact same modes with the exact same names, but some are the the same and for others "that would be a combined ANGLE Mode with the ALTHOLD Modifier". It's not perfect, but it's likely doable - as doable as multirotor navigation without a compass or other things we've pulled off.

if QGC devs or MP devs would say "hey, lets add all the modes and functions that INAV supports in our application but we don't want to use MSP!" then it would be not a big deal to fully implement that on the INAV end.

Meeting in the middle might be the way. They don't want to put in a bunch of work to do things the INAV way. There may be spots where we can't easily match up with what the GCS systems do. It may be easier to get them to change the 10% that we can't do on our side if we first do the parts we can. If there are folks who want to put in the time to do it.

[b14ckyy]

and to be fair. With all the Effort that Jonathan is putting in MWP now to bring it to windows and also supporting MacOS now... Where is the point?
Except direct control through the GCS there is not much that MWP can't do that only MP or QGC can.

[stronnag]

and to be fair. With all the Effort that Jonathan is putting in MWP now to bring it to windows and also supporting MacOS now... Where is the point? Except direct control through the GCS there is not much that MWP can't do that only MP or QGC can.

mwp provides support for "follow me", either by the user setting the desired point or from GCS GPS (at least on OS where gpsd is available. Which is as close tp "guided" as the firmware is just now.

[b14ckyy]

I was referring to the "I put a gamepad to my Laptop and fly my Arduplane with that" kind of control :D

[xznhj8129]

It's true that MWP is making excellent progress, but i should point out that this wouldn't only help GCS software but also SDK compatibility, there are a lot more things that use mavlink apart from MP and QGC.

[b14ckyy]

Totally with you but as I pointed out above: speaking Mavlink does not mean they can communicate.

If you have 2 persons talking, one is a Game developer and talks about lightning systems in game engines and the other one is a car mechanic and talks about fuel mixture levels. They can both speak English and still have no idea what the other person is talking about. Same happens here.

the GCS needs to know what it can tell INAV to do. After that it does not matter if it tells INAV that over MSP or if we allow it to do that over Mavlink.