search

mavlink / qgroundcontrol

Cross-platform ground control station for drones (Android, iOS, Mac OS, Linux, Windows)
http://qgroundcontrol.io
3.27k stars 3.6k forks source link

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

Closed goasChris closed 4 months ago

goasChris commented 1 year ago

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

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 commented 1 year ago

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 commented 1 year ago

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 commented 1 year ago

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 commented 1 year ago

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 commented 1 year ago

@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 commented 1 year ago

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

vorobotics commented 1 year ago

Not yet, I'll make one.

vorobotics commented 1 year ago

@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 commented 1 year ago

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:

vorobotics commented 1 year ago

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 commented 1 year ago

@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 commented 1 year ago

@zdanek ArduPilot 4.3.2

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

zdanek commented 1 year ago

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 commented 1 year ago

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

zdanek commented 1 year ago

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

JMare commented 1 year ago

@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:

Test 1: Setup two vehicles on two links

Test 2: Setup two vehicles on one link

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 commented 1 year ago

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 commented 1 year ago

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 commented 1 year ago

@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 commented 1 year ago

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: https://github.com/mavlink/mavlink/issues/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 commented 1 year ago

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

@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 commented 1 year ago

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.