Multi-Vehicle: Joystick input gets stuck on a single vehicle

[goasChris]

Expected Behavior

When having multiple devices/vehicles connected to QGC I would expect that you can easily and in a stable manner switch between them when sending joystick/controller inputs.

Current Behavior

When having two vehicles connected to QGC, it will get stuck sending joystick inputs to a single vehicle after switching from one vehicle to another a few times (1-3). You will need to restart QGC, or recalibrate the controller in order for be able to switch again, but it will get stuck after switching.

Steps to Reproduce:

1. Have two vehicles set up. We used the docker version of ardusub companion. Set them up to point at QGC IP with port udp/14660. We used vehicle ID 10 and 13 for ours.
2. In QGC, set up a "comm link" using UDP/14660, and your local ipv4 adresse.
3. Confirm in the log that both have connected, and that you get the multi vehicle UI up, so that you can switch between them. Confirm you can arm/disarm both, and that both are delivering mavdata.
4. Have a USB controller set up, calibrated and configured.
5. Move the stick forward, so that you'll send thruster input to the currently selected vehicle (currently 10). Physically confirm the thrusters are engaging. Or you can send a simple arm/disarm command via the controller.
6. Let go of the stick/button, wait a few sec, and then switch to the other vehicle (now 13) in the drop down menu at the top. Repeat step 5.
7. Repeat step 6 (switch vehicle, up to 2-3 times), and observe how the thruster will not engage on the selected vehicle, nor will arm/disarm via controller input. Instead it is sending the packets to the wrong vehicle for thruster, arm/dismarm etc.
8. Try switching back and forth between the vehicle, and observe it being completely stuck on a single vehicle.

System Information

• Operating System: Win 10 22H2
• QGC Version: 4.2.3
• QGC build: Stable, from Github
• Flight Controller: Blue Robotics Navigator on pi4
• Autopilot (with version): unsure, rover type

Detailed Description

When controlling multiple robots (id Vehicle 10 and 13). We get info from both.
Thru the QGC UI we can arm and disarm both. Switching is fine, we get up to date information.

The issue is with USB controller input getting sent to the incorrect mavlink/robot at times.
Usually fine initially, but after switching between boths vehicles 1-3 times, it will get stuck sending USB controller/thrusts to a single vehicle.

Recalibrating the joystick seems to reset/resolve it temporarily. If we switch back and forth between them a few more times, it gets stuck sending to a single one again.
It does not seem to be more often on one or the other more often, it feels random.
Restarting QGC will resolve it (you can switch and control either), until it gets stuck again.

It seems to me like network communication is fine. We can always receive up to date data, and can arm/disarm in the QGC UI (not controller).

Log Files and Screenshots

logQGC-ControllerIssue.txt

[vorobotics]

I can confirm this happens for me as well. I find that if I toggle the "Enable" checkbox for the USB controller after switching vehicles that control will switch to the newly selected vehicle, but this is far from ideal. The expected behavior is that joystick input should switch to the currently selected vehicle.

[zdanek]

I will check this tomorrow as we plan to fly two drones with one joystick and QGC. If I will confirm this at my side I can fix it as I need this feature to work smoothly in my project.

[vorobotics]

I noticed that if I make these changes, switching active vehicle works and moves the joystick control to the new vehicle.

Basically when user changes the active vehicle, I set joystick disabled for the old vehicle (in setActiveVehicle()), then set it enabled for the new vehicle (in setActiveVehiclePhase2()).

It's a hack/workaround, as I am not intimately familiar with QGC code base QT or best practices; but it seems to work. I think it is really a matter of passing the right signals around when the user switches the active vehicle.

diff --git a/src/Vehicle/MultiVehicleManager.cc b/src/Vehicle/MultiVehicleManager.cc
index 1334d115a..548e65f83 100644
--- a/src/Vehicle/MultiVehicleManager.cc
+++ b/src/Vehicle/MultiVehicleManager.cc
@@ -271,6 +271,7 @@ void MultiVehicleManager::setActiveVehicle(Vehicle* vehicle)
             // any existing ui from the currently active vehicle.
             _activeVehicleAvailable = false;
             _parameterReadyVehicleAvailable = false;
+            _activeVehicle->setJoystickEnabled(false);
             emit activeVehicleAvailableChanged(false);
             emit parameterReadyVehicleAvailableChanged(false);
         }
@@ -301,6 +302,7 @@ void MultiVehicleManager::_setActiveVehiclePhase2(void)
     if (_activeVehicle) {
         _activeVehicleAvailable = true;
         emit activeVehicleAvailableChanged(true);
+        _activeVehicle->setJoystickEnabled(true);
 
         if (_activeVehicle->parameterManager()->parametersReady()) {
             _parameterReadyVehicleAvailable = true;

[rizvanovic]

We experienced this issue too whilst flight testing. Our operator could not change between the different vehicles without doing the manual workaround noted in this issue thread. We also saw issues with changing modes. If the operator had vehicle 2 selected and tried changing modes it would change modes on vehicle 1.

[vorobotics]

@rizvanovic Have you tried the patch I suggested above? I found that with this small change, I can reliably switch between vehicles and the joystick and button control always moves to the new vehicle.

[rizvanovic]

@vorobotics I have not. Hoping for a fix to be merged in the future. Is there an open PR for this?

[vorobotics]

Not yet, I'll make one.

[vorobotics]

@rizvanovic Please check, a fix has been merged into master in commit 434e76a. This should ensure that the joystick inputs control only the active vehicle when the active vehicle is changed using the drop down menu.

[zdanek]

It did not help in my situation. Trying to figure out why.

When QGC is connected to more than on drone through single connection (for example UDP that is forwarded to multiple drones) switching Vehicles makes joystick work with first Vehicle only (when it's active) and not with others.
This is a serious problem. Examples of connections:

• single RFD (radio) link, connected through UDP
• SITL sim, that can be run with:

./sim_vehicle.py -v ArduCopter -D -j 8 --console --map --osd --tonealarm -f quad -n 3 -I 1

[vorobotics]

What version of QGC are you using? I have tested this on the nightly build master:ce21b6843 2023-07-11 9:56:23 +0000 and I am able to switch between vehicles and joystick control moves to the new vehicle.

Here is a video:
https://drive.google.com/file/d/1UmYh2EBicGyBSRyDh-_z1vP1MW_2en-Q/view?usp=sharing

Note that when you are running multiple vehicles through the same link as you have described, you need to have different SYSID_THISMAV for each vehicle so you can select between them using the dropdown box as I show in the video.

Also, I have used this in the field with multiple real drones this way. I have connected a SteamDeck to multiple drones using QGC and I am able to switch between the drones easily using the dropdown menu, and take joystick control the selected vehicle.

[zdanek]

@vorobotics Thank you so much for the video. This helps me a lot.
I've used exactly that commit, head of master. Built today.
I know about vehicle's ID.

Have you used SITL of ardupilot? Which version / commit?

[vorobotics]

@zdanek ArduPilot 4.3.2

But I have also tested this on real drones running PX4 firmware.

[zdanek]

It's not working at my computer. I Ubuntu 22.04LTS.
Which OS you have? I'm so confused that I need to pin out the problem.
We tested it on Windows with RFD link and Windows, so my SITL on Linux seems not to be a problem.

[vorobotics]

@zdanek I have used this on Ubuntu 20.04 and 22.04 for both QGC and the SITL.

[zdanek]

@vorobotics thank you for your support.
Now I will dig into this.

[JMare]

@zdanek @vorobotics

I have been testing this as part of looking into #10664

I tested today with a USB Joystick, running multi vehicles, and I can confirm that:

In the case of Multiple Vehicles on Multiple Links, switching between vehicles works as intended. Control and joystick axis are transferring successfully.
In the case of Multiple Vehicles on One Link, switching between vehicles does not work as intended. In this case, when I switch to vehicle 2, I am unable to control either vehicle with the control axis, but the buttons are transferring over.

Test Setup:

• QGroundControl Master Daily Version 4fe11c2
• Ardupilot Master 25cec12223
• USB Joystick calibrated, enabled, with arm button, loiter button and guided

Test 1: Setup two vehicles on two links

• sim_vehicle.py -v ArduCopter -I 0 -w --sysid=1
• sim_vehicle.py -v ArduCopter -I 1 -w --sysid=2
• Start QGC and configure a UDP link on 14560 (for vehicle 2, vehicle 1 is autoconnect udp)
• Check Joystick Enabled
• Takeoff aircraft 1 in guided mode - verify yaw control with joystick once altitude is reached. Check you can switch between loiter and guided with buttons.
• Leave vehicle 1 in guided, switch to vehicle 2
• Takeoff aircraft 2 in guided mode - verify yaw control with joystick once altitude is reached. Check you can switch between loiter and guided with buttons.
• Switch Back to vehicle 1, verify sticks and buttons again.

Test 2: Setup two vehicles on one link

• (close all SITL and QGC Instances from last test)
• sim_vehicle.py -v ArduCopter -w -n 2 --auto-sysid
• Start qgroundcontrol, don't manually connect any comm links as both vehicles are now on autoconnect UDP
• Vehicle 2 will be selected by default
• Check Joystick Enabled
• Takeoff aircraft 2 in guided mode - once altitude is reached, check for yaw control and buttons working - I found that I had no yaw control, but buttons worked.
• Leave vehicle 2 in guided, switch to vehicle 1
• Takeoff in guided. Once altitude is reached, check for yaw control and buttons working I found that I had yaw control and buttons all working
• Leave vehicle 1 in guided, switch back to vehicle 2. Check again for yaw control and buttons working. I again found buttons working but no yaw control in vehicle 2.

This is a dangerous situation because on vehicle 2 I was able to switch to loiter, but because the joysticks were not working, it was acting as if I was giving zero throttle and immediately started descending at max rate.

[JMare]

I did a bit more testing this morning, and I think that this is actually not a bug in QGC.

First I repeated test 2 and checked the wireshark output from QGC - and everything looks good, when vehicle is changed, target system changes, and axis's appear working.

Then I had an idea and found that if you switch vehicle in mavproxy when you switch vehicle in qggroundcontrol, the functionality works.
Ie, when you switch from vehicle 1 to 2 using the dropdown in QGC, also type into the mavproxy instance attached to SITL "vehicle 2".

If you do that, everything seems to work as intended for my Test 2 case above.
So I think that there must be code in mavproxy which is doing the same thing as QGC is trying to do, stop joystick input being sent to the non-active vehicle.

@zdanek have you seen this issue in any situation where the links are not passing through a MAVProxy instance?

I am trying to setup a multi vehicle px4 simulation to compare, but I'm not very familar with PX4 so its taking me a bit to get going.

[JMare]

I can confirm that everything works in the following two scenarios:

Multi Vehicle PX4 Simulation:
Start two vehicles:
PX4_SYS_AUTOSTART=4001 PX4_GZ_MODEL=x500 ./build/px4_sitl_default/bin/px4 -i 0
PX4_SYS_AUTOSTART=4001 PX4_GZ_MODEL_POSE="0,2" PX4_GZ_MODEL=x500 ./build/px4_sitl_default/bin/px4 -i 1
Then open QGC, it will autoconnect to both on UDP 14550. Switching between and controlling both vehicles works as intended (set RC failsafe to HOLD to prevent them landing when you switch)

Multi Vehicle ArduCopter Simulation without shared mavproxy instance
Start two vehicles:
sim_vehicle.py -v ArduCopter -I 0 -w --sysid=1
sim_vehicle.py -v ArduCopter -I 1 -w --sysid=2
Start a router to multiplex the mavlink stream, I used cmavnode with the following config:

[sitl]
    type=udp
    localport=14550

[sitl 2]
    type=udp
    localport=14560

[gcs]
    type=udp
    targetport=14570

Then open QGC, make sure you have turned off UDP autoconnect, then start a link to 14570, both vehicles will connect on it.
You can then switch back and forward between the vehicles and the joystick control transfers as expected.

So I think this is actually not a problem except if using MAVProxy as the mavlink router, since it seems to be doing some sort of message filtering stopping the vehicle receiving these messages.

[vorobotics]

@JMare This is a great insight. I was doing all of my testing using separate instances of mavproxy, or separate PX4 instances, or separate real drones with separate links. Which is why I never was able to reproduce the multi vehicle issue.

[JMare]

It appears this issue is due to the MANUAL_CONTROL message having a field "target" rather than "target system". Because of this, MAVProxy doesn't know where to route it, and sends it to the active vehicle by default.

I opened an issue on the mavlink repo: mavlink/mavlink#2022

To work around it for now, apply this patch to mavproxy:

diff --git a/MAVProxy/mavproxy.py b/MAVProxy/mavproxy.py
index 7dff43a8..296348c0 100644
--- a/MAVProxy/mavproxy.py
+++ b/MAVProxy/mavproxy.py
@@ -858,6 +858,10 @@ def process_mavlink(slave):
     if allow_fwd:
         for m in msgs:
             target_sysid = getattr(m, 'target_system', -1)
+            '''MANUAL_CONTROL messages use target rather than target_system, so try both'''
+            if(target_sysid == -1):
+                target_sysid = getattr(m, 'target', -1)
+
             mbuf = m.get_msgbuf()
             if mpstate.settings.mavfwd_link > 0 and mpstate.settings.mavfwd_link <= len(mpstate.mav_master):
                 mpstate.mav_master[mpstate.settings.mavfwd_link-1].write(mbuf)

Then start SITL without MAVProxy:
sim_vehicle.py -v ArduCopter -w -n 2 --auto-sysid --no-mavproxy
And start your modified version of mavproxy:
python MAVProxy/mavproxy.py --master tcp:127.0.0.1:5760 --master tcp:127.0.0.1:5770 --out udp:127.0.0.1:14550

[zdanek]

This is a great analysis. @JMare we had experienced this on a physical connection. Single connection to RFD radio http://rfdesign.com.au/products/rfd900-modem/ with mesh config, that enables one antenna on ground station and a few drones connected.

AFAIK this radio uses mavlink, I mean, it's aware of mavlink, radio on ground station even sends heartbeat to gqc. So it interpretes mavlink somehow or just sends mavlink hb. Anyway, this happens on single physical conn.

The way I see it now, this might was due to

• some problem in rfd soft
• I was using old version of gqc (I have custom code) that had some problems with joystick (not likely as my colleague tested with official release and had same problem)
• anything with target system mentioned by @vorobotics
• an issue with rfd config that might be corrected (I doubt if this is real)

@JMare you've found an issue in mavproxy. Do you plan to make a PR?
I will test my gqc with your fix.

I'm still keen to rework joystick code. If you @JMare and maybe @vorobotics are you open for this, we might make a working y branch, meet online, and plan and split works to do.

[JMare]

Hmm, interesting that it affects a real connection through RFD900.
I would suggest, that if you can replicate the problem, take a look at the data in wireshark and check the output from QGC.
If you see that the QGC output is working, then we know the bug is elsewhere.
To get this data in wireshark, you could setup a UDP->Serial bridge to connect the RFD to QGroundcontrol, allowing you to inspect the packets in wireshark. I think this could be done with something like socat, or with mavlink_router or cmavnode (or even with mavproxy)

@JMare you've found an issue in mavproxy. Do you plan to make a PR?

I don't think the issue is really with mavproxy, I think the real issue is with the mavlink library.
The patch to mavproxy just works around the issue for testing.

Yep I'm open to helping rework the joystick support.