Rework Vehicle Joystick Handling

[JMare]

#10648 fixed a bug identified in #10544 which had caused joystick control to get stuck on a vehicle.

However, the fix introduced two new bugs, and I think was not the ideal way to fix it.
The three new bugs introduced, are:

• Virtual Joystick not working #10664
• And also, that it is now impossible to disable the joystick if one is connected, since joystick enable is called on each change of active vehicle
• when a joystick is connected to the system, there is inconsistent behavior. On vehicle 2 the joystick will reconnect, on vehicle 1 it wont in my testing

I investigated and found a couple of problems, the first is obvious:

• Since the joystick is enabled on each change of active vehicle without regard to settings or the presence of a joystick, the above bugs are caused

The others are less obvious

• There is currently a hook to activeVehicleAvailableChanged which intends to stop the joystick thread if there are to longer any active vehicles. Reasonable enough, however:
  • It turns out that activeVehicleAvailableChanged(false) is briefly emitted on each change of active vehicle: see here
  • The fact that this code is in the vehicle class means that it couldn’t ever do what it is supposed to do, since if there truly were no more active vehicles, there would be no vehicle object to receive the signal.
• When a joystick is connected, all vehicles (active and not) try to enable and start the joystick

My Solution:

• A minor rework of the joystick handling code in the vehicle class which fixes all the bugs identified above
• Seperation of setJoystickEnabled from saveSettings - prevents weird coupling issues
• Disconnect from the activeVehicleAvailableChanged signal, since it couldn’t work as intended anyway. Delete the matching handler function.
• The only thing that is left unhandled is that when all active vehicles are deleted, the joystick thread doesn’t stop. I’d appreciate feedback on the best way to stop the thread when all active vehicles are gone, given that activeVehicleAvailableChanged can’t be trusted to tell us this.

Would appreciate review and testing - I have tested thoroughly on my enviroment. (linux using usb joystick)

Note that there is a bug in MAVProxy/MAVLink identified here: which makes this difficult to test on ardupilot.
I have been doing most of my testing on PX4 for this reason.

Leaving this as a draft until I have worked out what to do about stopping the thread when all vehicles are gone.

@zdanek @vorobotics

[zdanek]

Yeah, good work. TBH it's funny that vehicle has awareness of Joystick. This should not happen. But this is OSS and there's always a way to improve the code.
As we discussed with @JMare and @vorobotics there's an idea to refactor joystick code. We could join efforts.

[JMare]

Yeah, good work. TBH it's funny that vehicle has awareness of Joystick. This should not happen. But this is OSS and there's always a way to improve the code. As we discussed with @JMare and @vorobotics there's an idea to refactor joystick code. We could join efforts.

Yes, agreed and I'm still willing to do a deeper dive on the joystick support.

This PR is just a basic fix of the current bugs which I think is needed in the short term, certainly I needed it for my custom build.

Can you think of the best way to safely stop the thread when all the active vehicles are removed? I think it has to be in the joystick class rather than the vehicle class for that, since the vehicle doesn't know if it is the last one when it gets destructed.

[DonLakeFlyer]

Can you think of the best way to safely stop the thread when all the active vehicles are removed? I think it has to be in the joystick class rather than the vehicle class for that, since the vehicle doesn't know if it is the last one when it gets destructed.

Instead of using activeVehicle to trigger, connect to the MultiVehicleManager::vehicles countChanged signal. When the count goes to 0 all the vehicles are gone.

[DonLakeFlyer]

I looked through and the changes make sense to me. Would like to get this finished up and merged in. I need to make some changes above it to support a mode for Herelink which supports buttons only but no sticks.

[DonLakeFlyer]

Other suggestion for review is the ArduSub folks since they are all about joysticks.

[DonLakeFlyer]

@jaxxzer Any ArduSub folks to look at this?

[JMare]

@DonLakeFlyer Thanks for the review.
I will put in the hook to stop the thread today, then I can mark this as ready.

Instead of using activeVehicle to trigger, connect to the MultiVehicleManager::vehicles countChanged signal. When the count goes to 0 all the vehicles are gone.

I had a look and found MAVLinkProtocol::_vehicleCountChanged as a private slot which connects to
MultiVehicleManager::vehicleAdded and MultiVehicleManager::vehicleRemoved.

Should I connect to MultiVehicleManager::vehicleRemoved and then check for !(_toolbox->multiVehicleManager->activeVehicle()) to know if the last vehicle has been deleted?

[DonLakeFlyer]

In joystick manager, something along these lines:

connect(qgcApp()->toolbox()->multiVehicleManager()->vehicles, &QmlObjectListModel::countChanged, this, &JoystickManager::_vehicleCountChanged);
...
JoystickManager::_vehicleCountChanged(int count)
{
    if (count == 0 && joystick thread running) {
        stop joystick thread
    }
}

[JMare]

@DonLakeFlyer ahh thanks, yep, that worked perfectly.
I put it into the Joystick class because that is where the thread is managed from otherwise.

Only thing I noticed while testing this.
When a vehicle (not the last one) is disconnected, there is a brief moment when the joystick is trying to send data to a dead link before the new active vehicle picks it up.

VehicleLog: sendJoystickDataThreadSafe: primary link gone!
VehicleLog: ~Vehicle Vehicle(0x7fe1c0cba400)
VehicleLog: sendJoystickDataThreadSafe: primary link gone!
setCurrentPlanViewSeqNum
setCurrentPlanViewSeqNum
VehicleLog: Vehicle 1 is the new active vehicle
VehicleLog: Vehicle 1 Capture Joystick

Previously, before my changes, the joystick thread would be stopped in the destructor of the deleted vehicle, and then started again on activeVehicleChanged in the new active vehicle. (actually the thread would also stop and start on every change of active vehicle)

It seems to me better to avoid starting and stopping the thread, I can't see any danger in those brief errors in sendJoystickDataThreadSafe, but I thought I'd point it out in case I'm missing something.

Anyway, I implemented stopping the thread following your suggestion, and fixed up a few typos in my comments, whoops.
I have left perhaps too many debug prints in the code, but I figure it will help others test my changes, and I can remove them later.

[JMare]

Actually now that I look at it more, if joystick::_handleAxis fell at just the right moment, its appears possible, though extremely unlikely, that it could dereference a dead pointer to the just-deleted vehicle. Perhaps it is safer to stop and start the joystick thread any time a vehicle is deleted as before, so that the joystick thread is never running while its _activeVehicle pointer is pointing to a vehicle which is being deleted. I will take a closer look later on.

[JMare]

Ok after looking into this I think this can stand as is. I had a misunderstanding of how signals and slots worked.
I now understand that, in the code below, we can trust that slots connected to activeVehicleChanged() will execute before the function proceeds - ie that the signals don't get queued for later processing. If that is the case, I think this can stand as is and there should be no thread safety issues.

void MultiVehicleManager::_deleteVehiclePhase2(void)
{
    qCDebug(MultiVehicleManagerLog) << "_deleteVehiclePhase2" << _vehiclesBeingDeleted[0];

    /// Qml has been notified of vehicle about to go away and should be disconnected from it by now.
    /// This means we can now clear the active vehicle property and delete the Vehicle for real.

    Vehicle* newActiveVehicle = nullptr;
    if (_vehicles.count()) {
        newActiveVehicle = qobject_cast<Vehicle*>(_vehicles[0]);
    }

    _activeVehicle = newActiveVehicle;
    emit activeVehicleChanged(newActiveVehicle);

    if (_activeVehicle) {
        emit activeVehicleAvailableChanged(true);
        if (_activeVehicle->parameterManager()->parametersReady()) {
            emit parameterReadyVehicleAvailableChanged(true);
        }
    }

    delete _vehiclesBeingDeleted[0];
    _vehiclesBeingDeleted.removeAt(0);
}

[DonLakeFlyer]

ie that the signals don't get queued for later processing

Signals get queued only if they are crossing thread boundaries.

[JMare]

Signals get queued only if they are crossing thread boundaries.

Good, that means that by the time the vehicle is deleted, either the _activeVehicle pointer in the joystick class will have been updated, or the thread will have been stopped, depending on if it is the last vehicle or not. Either way this should be safe.

[DonLakeFlyer]

@JMare Could I ask for your help in reviewing and a quick test of this pull : #10764? For a normal build it should be a no-op in functionality. It allows custom builds to use a joystick where only the buttons are supported. This is for the Herelink such that it can create a custom build of QGC which doesn't need to modofy mainline QGC code. I've already tested it out on my new Herelink QGC version.

[DonLakeFlyer]

@JMare One thing I noticed which I'm not sure if I changed with my pull or not when on the Herelink: When you have a calibrated joystick and you go into the buttons page clicking buttons while on that page will execute the action for real. Seems a bit dangerous since you may just be clicking buttons just to see which is which. Not sure if that's the way it always was.

[DonLakeFlyer]

Sorry I hit the wrong button and closed the pull. Duh!

[JMare]

@DonLakeFlyer I tested your pull #10764 on my Ubuntu setup with a USB joystick.
Seems to work well to me, including with multiple vehicles.
I found one issue which presented when I overlayed your pull on this work, being that the joystick thread would never get started if you disconnect and reconnect the joystick at runtime, so I pushed a tweak to this pull to make it compatible.

I guess nobody ever plugs an external joystick into the herelink but if someone did, it might give weird behavior due to the fact the active joystick select menu is hidden.

@JMare One thing I noticed which I'm not sure if I changed with my pull or not when on the Herelink: When you have a calibrated joystick and you go into the buttons page clicking buttons while on that page will execute the action for real. Seems a bit dangerous since you may just be clicking buttons just to see which is which. Not sure if that's the way it always was.

Yes, this behavior goes back at least to the latest stable. I have been caught out in the field by this with an unexpected arming of the vehicle while setting up buttons. Maybe a simple fix for this would be a red warning on the top of the page that appears if the joystick is enabled?

[DonLakeFlyer]

Thanks for the review and test @JMare. I'm going to merge it in. I have the live button thing in the back of my head. I'll see what I can do about it.

[DonLakeFlyer]

@JMare Anything left to do on this pull or should I merge?

[JMare]

The changes I made yesterday for compatibility with the herelink mode caused a double call to capture joystick sometimes, so I fixed that this morning and did a force push just now.

This pull is now ready to merge.

The only other issue I noticed is if for some reason you have joystick enabled true saved in the settings and then switch to a build with usebuttonsonly true, the joystick will stay fully enabled with axis and buttons working, and there will be no way to disable the joystick because the menu with the tickbox is hidden.
I'm not sure if existing herelink builds have joystick enable as true, if it did, could see this problem on the update and people might need to wipe settings.
If you want, I could add a check in Vehicle::_loadJoystickSettings() to prevent loading joystickenable if your new setting usebuttonsonly is set in the build?

[JMare]

Oh and I have quite a lot of logging prints in VehicleLog.
I think its helpful for debugging but let me know if you want me to cut it down.

Change of active vehicle:

VehicleLog: Vehicle 1 is the new active vehicle
VehicleLog: Vehicle 1 Capture Joystick

Enabling the joystick from menu:

VehicleLog: Vehicle 1 Joystick Enabled
VehicleLog: Vehicle 1 Capture Joystick
VehicleLog: Vehicle 1 Saving setting joystickenabled: true

Disconnecting the joystick:

VehicleLog: Vehicle 1 Notified that there is no active joystick
VehicleLog: Vehicle 1 Joystick Disabled
VehicleLog: Vehicle 2 Notified that there is no active joystick
VehicleLog: Vehicle 2 Joystick Disabled

Reconnecting the joystick:

VehicleLog: Vehicle 1 Notified of an active joystick. Loading setting joystickenabled: true
VehicleLog: Vehicle 1 Joystick Enabled
VehicleLog: Vehicle 1 Capture Joystick
VehicleLog: Vehicle 2 Notified of an active joystick. Loading setting joystickenabled: true
VehicleLog: Vehicle 2 Joystick Enabled

[DonLakeFlyer]

The only other issue I noticed is if for some reason you have joystick enabled true saved in the settings and then switch to a build with usebuttonsonly true, the joystick will stay fully enabled with axis and buttons working, and there will be no way to disable the joystick because the menu with the tickbox is hidden.
I'm not sure if existing herelink builds have joystick enable as true, if it did, could see this problem on the update and people might need to wipe settings.

Yeah, that's kind of a strange edge case. Given the use buttons only thing is pretty device specific it would have a to be a screw up somewhere which allowed a build onto the device which did not have that set correctly. So I'm not too worried about that.

For the logging could you move the joystick specific logging in Vehicle to JoyStickLog instead of VehicleLog? In order to do that you will likely have to move the definition of JoystickLog out of the joystick code and make it globally available. You do that by moving it to QGCLoggingCategory.h/cc.

[DonLakeFlyer]

If you want, I could add a check in Vehicle::_loadJoystickSettings() to prevent loading joystickenable if your new setting usebuttonsonly is set in the build?

Changed my mind. That's a simple change so might as well move this from probably won't happen to can't happen by fixing it.

[JMare]

Thanks @DonLakeFlyer,

• Rebased onto master
• Moved JoystickLog into QGCLoggingCategory (left notes in joystick.c/.h of where they went)
• Updated vehicle joystick code to log to JoystickLog
• Updated Vehicle::_loadJoystickSettings to prevent ever enabling joystick axis on a usebuttonsonly build

Sanity check, is this a sensible way to store the value in the Vehicle class?
static const bool _useButtonsOnly = qgcApp()->toolbox()->corePlugin()->options()->joystickUseButtonsOnly();

Full new function for context.

void Vehicle::_loadJoystickSettings()
{
    QSettings settings;
    settings.beginGroup(QString(_settingsGroup).arg(_id));
    static const bool _useButtonsOnly = qgcApp()->toolbox()->corePlugin()->options()->joystickUseButtonsOnly();

    if (!_useButtonsOnly && _toolbox->joystickManager()->activeJoystick()) {
        qCDebug(JoystickLog) << "Vehicle " << this->id() << " Notified of an active joystick. Loading setting joystickenabled: " << settings.value(_joystickEnabledSettingsKey, false).toBool();
        setJoystickEnabled(settings.value(_joystickEnabledSettingsKey, false).toBool());
    }
    else if(_useButtonsOnly)
    {
        // in a build with _useButtonsOnly set, joystickenable should always be left false
        // this prevents the sticks from doing anything
        qCDebug(JoystickLog) << "Vehicle " << this->id() << " Skipping Load of Joystick Settings because QCC Options useButtonsOnly is set";
        setJoystickEnabled(false);
    }
    else
    {
        qCDebug(JoystickLog) << "Vehicle " << this->id() << " Notified that there is no active joystick";
        setJoystickEnabled(false);
    }
}

I haven't tested this properly yet, I won't be able to until Monday as I'm taking time off work and I cant get either Arducopter or PX4 sitl to work on my mac right now. I'll get to it on Monday and confirm that I haven't broken anything with this minor change.

[DonLakeFlyer]

static const bool _useButtonsOnly = qgcApp()->toolbox()->corePlugin()->options()->joystickUseButtonsOnly();

Since you are setting the value each time using static this doesn't really achieve anything other than move the storage location off the stack. Just remove the static. Unless I"m not quite sure what you are trying to achieve here?

[JMare]

static const bool _useButtonsOnly = qgcApp()->toolbox()->corePlugin()->options()->joystickUseButtonsOnly();

Since you are setting the value each time using static this doesn't really achieve anything other than move the storage location off the stack. Just remove the static. Unless I"m not quite sure what you are trying to achieve here?

I had thought that the static keyword would cause the call to only run once and then that value would be stored as const for the rest of execution.
Now that I think about it the function call to get the setting will probably get optimised out anyway so there is no need to prevent repeated calls. Will update and do final testing on Monday then this will be complete.

[DonLakeFlyer]

Ah, ok that's what wanted. Like you say there is no need to do that. The compiler will end up doing the same thing for you by itself.

[JMare]

@DonLakeFlyer updated, tested the changes.

Logging now goes to joystick log rather than vehicle log.
The edge case if joystickenabled is true for a usebuttonsonly build is now handled.

This PR is ready to merge.

[DonLakeFlyer]

@JMare Thanks for all the work on this.

[JMare]

Thanks for the help and review @DonLakeFlyer
This resolves #10664 and #10544

[zdanek]

Huge thanks and kudos @JMare!
You were so quick and robust at this fix.
I will test it at the end of August with real multi drone setup.

@DonLakeFlyer as usual great support not to be missed.
This is the power of OSS community 💪🚁✈️

Thanks!

[DonLakeFlyer]

@JMare I ran into a problem with this where QGC goes into an infinite loop and blows the stack. It specifically happens when you have a disabled joystick which is uncalibrated:

• Vehicle::_captureJoystick is called even on disabled joysticks
• This causes Joystick::startPolling to be called
• In Joystick::startPolling there is a check for uncalibrated. If so Vehicle::setJoystickEnabled(false) is called
• Which in turn causes Vehicle::_captureJoystick to be called again
• Round an round you go till you run out of stack

I think I have the details right. It's a little tricky.

[JMare]

@DonLakeFlyer, apologies, that's a bad bug that is my mistake.

I thought I was being clever by having setJoystickEnabled(false) call capturejoystick so it would disconnect the signals to the vehicle once the joystick was disabled. I changed that in one of my last updates to that PR and didn't test uncalibrated joysticks since. It doesn't actually matter if the signals are disconnected, because the joystick class checks for joystickenabled before sending signals.

This is the change I made which introduced this issue I believe. reverting this should be the fix.

But I will try and replicate this issue today and confirm 100% that this is the fix and doesn't affect anything else.

[JMare]

Update, I realized that since a default calibration is used for gamepads and all my testing was on gamepads, I never tested this with an calibrated joystick. I replicated the issue just now by disabling the loading of default calibration from game pads, and incrementing Joystick::_calibratedSettingsKey to force recalibration, and I immediately saw the unbounded loop you found.

As above, the fix is the following git diff:

diff --git a/src/Vehicle/Vehicle.cc b/src/Vehicle/Vehicle.cc
index 4fd1e1d52..052e37c1e 100644
--- a/src/Vehicle/Vehicle.cc
+++ b/src/Vehicle/Vehicle.cc
@@ -2142,9 +2142,7 @@ void Vehicle::setJoystickEnabled(bool enabled)
 
     // if we are the active vehicle, call start polling on the active joystick
     // This routes the joystick signals to this vehicle
-    // We do this even if we are disabling the joystick
-    // because it will trigger disconnection of the signals
-    if (_toolbox->multiVehicleManager()->activeVehicle() == this){
+    if (enabled && _toolbox->multiVehicleManager()->activeVehicle() == this){
         _captureJoystick();
     }

The only downside of this is that it will leave these signals connected if the user disables a joystick that was previously enabled.

disconnect(this, &Joystick::setArmed,           _activeVehicle, &Vehicle::setArmedShowError);
disconnect(this, &Joystick::setVtolInFwdFlight, _activeVehicle, &Vehicle::setVtolInFwdFlight);
disconnect(this, &Joystick::setFlightMode,      _activeVehicle, &Vehicle::setFlightMode);
disconnect(this, &Joystick::gimbalPitchStep,    _activeVehicle, &Vehicle::gimbalPitchStep);
disconnect(this, &Joystick::gimbalYawStep,      _activeVehicle, &Vehicle::gimbalYawStep);
disconnect(this, &Joystick::centerGimbal,       _activeVehicle, &Vehicle::centerGimbal);
disconnect(this, &Joystick::gimbalControlValue, _activeVehicle, &Vehicle::gimbalControlValue);
disconnect(this, &Joystick::emergencyStop,      _activeVehicle, &Vehicle::emergencyStop);
disconnect(this, &Joystick::gripperAction,      _activeVehicle, &Vehicle::setGripperAction);

Additionally the joystick will retain a pointer to the vehicle which now no longer wants the joystick data.
Luckily, the joystick thread already checks for the vehicle having joystick enabled before sending axis data.

if (_activeVehicle->joystickEnabled() && !_calibrationMode && _calibrated) {
.....
            _activeVehicle->sendJoystickDataThreadSafe(roll, pitch, yaw, throttle, shortButtons);
}

And has a similar check before it emits a button action:

if (!_activeVehicle || (!_activeVehicle->joystickEnabled() && !_useButtonsOnly) || action == _buttonActionNone) {
    return;
}
if (action == _buttonActionArm) {
    if (buttonDown) emit setArmed(true);
    ....
    

Not sure how I can add commits to a pull request that is already merged. Wanted to provide a solution to this fast so I put the quick fix on my fork https://github.com/JMare/qgroundcontrol/tree/joystick-loop-fix.
I'm confident this will fix the issue without adding any new ones, but this issue is evidence it would be great if others with different setups could test as well in case I'm missing anything.

[DonLakeFlyer]

Just submit a new pull.

[DonLakeFlyer]

If the signals are not disconnected to the vehicle when the joystick is disabled. Doesn't that means that in a multi-vehicle situation if the user re-enables a joystick the button presses will go to in-active vehicles as well as active vehicles?

[DonLakeFlyer]

This sequence on events:

• Two vehicles A and B
• Vehicle A is active with enabled joystcik
• User disables joystick
• Active vehicle changed to B
• User enables joystick
• User presses button
• Button presses go to A and B?

[JMare]

In the scenario above, when active vehicle changes to B, the joystick is captured by the new active vehicle, which redirects all the signals.
The other options here are:

• Have setJoystickEnabled(false) call stopPolling which will disconnect signals and stop the thread (will be restarted on next active vehicle change
• the best option but the most work is: seperate the "capturing" of the joystick from the control of the joystick thread, so that the joystick class has seperate methods and the active vehicle

The reason I think we can solve this with the solution I posted last night rather than the two above is that I have aimed to:

• keep the joystick thread running regardless of what the vehicle class is doing
• avoid editing the joystick class where possible, because I think it will be a bit of a rabbit hole

[JMare]

I created another PR with the quick solution. I'm happy to look at different solutions to this which are neater if you like.

Again my apologies I didn't catch that issue before this got merged.

[DonLakeFlyer]

In the scenario above, when active vehicle changes to B, the joystick is captured by the new active vehicle, which redirects all the signals.

Ok, I see that now. Thanks. All ok then.