Improve gimbal support for QGC 4.4

[Davidsastresas]

This is a rebased pull request from this draft PR #10667. As we are still some time from having a stable version from current master, we thought it would be good to add it to 4.3 for the next incremental release. If everybody is happy with this and it goes through, I can prepare right away an equivalent PR for master.

Thank you very much to @julianoes, we worked together specially on the backend of this PR, and besides a lot of contributions all over this PR, he did a huge part of the GimbalController logic. Also, he was one of the main developers behind this new gimbal protocol v2, which this PR is implementing. Thank you very much also to my partner @alexdelatorre, he is always behind the Frontend and user experience design and artwork of all the PRs we do from www.lincesystems.com.

This PR was sponsored by Lince Systems (www.lincesystems.com), Julian Oes (https://julianoes.com/), Harris Aerial (https://harrisaerial.com/), Ardupilot (https://ardupilot.org/) and SIYI (https://shop.siyi.biz/).

The idea is the same as the initial draft PR, just refactored and clean, as the draft PR had a lot of code that we were not going to use in the frontend, and it was really a draft anyway to test the proof of concept. Now, regarding the details:

FRONTEND

As per @DonLakeFlyer suggestions on the initial PR, now everything is based on a top toolbar indicator, minimizing new frontend code additions, in order to try to keep it as compact and maintainable friendly as possible. Basically most of the substantial code of the frontend is contained in GimbalIndicator.qml. This top toolbar indicator will appear when a valid gimbal has been identified:

It indicates the status of the gimbal according to its gimbal_device_flags, although only YAW_FOLLOW and RETRACTED flags are taken into account. Retracted has always priority, so if the gimbal is retracted, regardless of the yaw related flags, it will display retracted.

After clicking the indicator, we have a popup with the gimbal controls:

• Yaw lock button changes its name from "yaw lock" to "yaw follow", depending on the gimbal state. If it has yaw locked only yaw follow action will be shown and so on.
• Center, Tilt 90, and retract are using GIMBAL_MANAGER_PITCHYAW command. They are always shown and they always trigger the same, unlike yaw lock/follow button.
• Point home is using DO_SET_ROI with the home location received by the vehicle, using the usual ROI methods already present in vehicle.

The last button opens the QGC gimbal settings. This follows the idea of how master is evolving on this regard. I played around with the idea of backporting the neccesary commits to use the new ToolIndicatorPage from master, but it wasn't trivial so I did it this way. It is self contained in GimbalIndicator.qml anyway:

• The options related to onscreen control ( click and point and click and slide over video view ) only show if "Enable on screen camera control" is ticked. Also, depending on the type of control, the speed for click and slide or the fov for click and point will be shown.
• Show Azimuth indicator on map controls wether or not to show this indicator for gimbal yaw over vehicle:
  
• Use Azimuth instead of local yaw makes the top toolbar indicator to show Azimuth ( relative to North ) instead of yaw relative to nose of the vehicle:
  

Also it is worth mentioning that when a new gimbal is discovered, it is added to the available telemetry groups:

So we can place in the telemetry panel the following telemetry from gimbal: pitch, roll, yaw, azimuth and gimbal device id.

Finally, also 2 new icons were added to the icons available for the telemetry panel, they can be selected together with the rest of the usual icons:

If this extended settings panel is visible, it will persist closing and reopening the gimbal indicator popup. So, if we opened that menu, and then we clicked outside to close the whole popup, and then we click again on gimbal indicator on top toolbar, the extended settings panel will be still extended.

This is done on purpose, thinking on users wanting to change/adjust quick the speed or fov for on screen control. Usually you click around, and adjust fov/speed, click again, etc so during this process the user will need to access those settings and close the whole popup for testing the changes, and I thought this would be welcomed in that situation.

In case anybody misses the old ROI panel, I backported the new ROI functionality from master. In case somebody isn't familiar with it, now clicking over map and selecting ROI will send the action to the vehicle ( without slider confirmation ). And then, we can click on the ROI icon over the map for cancelling it or editing manually:

BACKEND

The main changes are:

• Vehicle: added gimbalController, and move all the gimbal related code to it.
• Flyviewvideo.qml: Added new qml class OnScreenGimbalController.qml. It receives the clicks done over Video window mouse area, and digests the clicks, press and release to carry on with the onscreen controls. I know in master we have in FlyViewVideo the new tracking controls, and it is going to somehow collide. I think having that separated we can easily identify when we have a "dumb" gimbal using onscreen controls, or an intelligent one using the tracking ones. Or we can always leave it up to a setting. Of course this only applies to the future equivalent master PR.
• GimbalController: New class implementing the logic for this gimbal protocol v2. It is compliant with the protocol as described here https://mavlink.io/en/services/gimbal_v2.html and it supports all the details like asking for control prior to sending commands to the gimbal, properly doing the handshake for gimbal discovery, etc.
• Joystick: some refactor and adaptation was done to have gimbal control using buttons. We can currently assign to joystick buttons: gimbal up, down, left and right, using steps, yaw lock/follow and center gimbal.

MULTIGIMBAL HANDLING

if several gimbals are detected, in the top toolbar indicator we will see the mavlink device id number of the current active gimbal, 1 in this case:

Then in the buttons popup, we will see the active gimbal indicated in the label at the left, and also close to the new button for selecting active gimbal:

After clicked, a dropdown will be shown to select the new active gimbal:

We will also have the representation of the non active gimbals over the vehicle map icon. The non active gimbals will appear more transparent than the active one, for the user to understand the active one:

Finally, if the user needs telemetry form the non active gimbals as well, it can be selected on the bottom telemetry panel as well:

TESTING

You can test this in Ardupilot launching a typical SITL instance from a root Ardupilot repository folder:

 Tools/autotest/sim_vehicle.py -v  ArduCopter

and then just set the MNT_TYPE parameter ( or also MNT2_TYPE if wanting to simulate 2 gimbals ) to 1, to simulate a servo gimbal. A few seconds after boot up QGC should finish the handshake with the vehicle and the new gimbal indicator should appear in the top toolbar. We have a new gimbalmanagerlog type of logging that records the details of the handshake between QGC and the autopilots, it could be useful if some manufacturer is testing a new gimbal with this.

Once having the Ardupilot instance running, we can control the gimbal with the panel shown above, also over the video screen clicking around using "click and point" or "click and drag" control types, or we can also connect a joystick and set up the buttons to play around with it.

[rmackay9]

Thanks very much for this. During testing with ArduPilot I found these two issues:

1. the lock/follow button produced odd behaviour where the gimbal would turn 90 degrees or so whenever the button was pushed. I think this is a known incompatibility with these recent ArduPilot changes Mount: improve yaw lock reporting to GCS ArduPilot/ardupilot#26564
2. I couldn't always get the new control to appear. It appeared once but I couldn't get it to re-appear later

[Davidsastresas]

Thank you very much for testing @rmackay9.

This PR was tested using this ArduPilot/ardupilot#26474 PR. I answered too In the ArduPilot/ardupilot#26564 PR you made substituting mine ( thank you very much for working on that this quick by the way! ), but just in case it is useful for somebody on this thread, it seems the reason for the erratic yaw behaviour is that Ardupilot is always sending yaw in vehicle frame, and you forgot to add GIMBAL_DEVICE_FLAGS_YAW_IN_VEHICLE_FRAME, so QGC was interpreting yaw angle from GIMBAL_DEVICE_FLAGS_YAW_LOCK exclusively. After adding this flag to the gimbal_device_attitude_status flags field on your branch it works correctly.

If anybody is interested, for more details about this, please see here: https://mavlink.io/en/services/gimbal_v2.html#how-to-interpret-gimbaldeviceattitudestatus-yaw-gimbal-angle

Now, regarding the gimbal indicator not appearing, I wasn't able to reproduce this locally. Please enable "gimbalManagerLog" next time you play around with this, as these logs report information of the status of the "handshake" between QGC and the gimbal manager connected to it.

Thank you!

[rmackay9]

@Davidsastresas,

Thanks for that, as discussed separately I've added your fix to AP and re-tested now.

Two small things I noticed with the "click-and-drag" feature:

1. I found the response very slow unless I increased the Max speed to 300 deg/s. If I clicked on the middle of the screen and pulled all the way to one edge, I would expect the desired rates sent would be this number.
2. At first I felt the direction was reversed. For example, if I click on the right side of the screen and drag left, I expected the scene to move to the left (e.g. the gimbal should rotate right). If you open google maps and then click-and-drag I think you'll see what I mean. Once I realised that that's not how it's suppose to be used though, it became quite natural. It's more like "click-and-drive".. so you click on the middle of the screen and then drag in the direction you want the gimbal to move.

BTW, I also could not re-produce the issue where the control didn't pop-up so let's forget about that for now.

Overall the new controls seem great to me!

[rmackay9]

With more testing I've found I can reproduce the issue of the gimbal control not appearing. The key is to startup QGC before the autopilot. So first start QGC, wait 10 seconds and then connect the USB to the autopilot to power it on.

[rmackay9]

I'm not qualified to review the code itself but from a usability point of view, I think this is merge-able.

[Davidsastresas]

Thanks @rmackay9 for the feedback.

About indicator not appearing: I really can not reproduce here on SITL. Please enable gimbal logs:

And let me know what you see. I can only report the indicator taking more time to appear when I start QGC before Ardupilot, but I guess it is because Ardupilot haven't yet started sending the gimbal messages. On those logs we can see what QGC is doing and what it has received so far from AP gimbal, hopefully we can debug that way why I can not reproduce that here.

About speed on Click and drag: The maximum rate sent would be what is sent when you click on the left on the screen, and slide all the way up to the right. Same for up and down. If you click just in the center and go all the way to the edges, it will only command maximum half of the maximum rate specified. I tested this real quick here again and in my SITL setup it makes sense and seems to work as expected. Let me know if that is fine for you.

About Click and drag reversed: I get your point, but I don't think that kind of "natural panning" will work on our approach. Let me explain. In a map application, you can actually "grab" the point of the map you click over, and slide up to the exact point you drop the click.

On our scenario, we can not "grab" what we click on the image, as it is rate based. it is more meant like if we had a huge and invisible virtual joystick in the middle of the screen, and we click and move around to command.

Considering what I said that we can not "grab" the image of the video feed to actually move just up to the point we "grabbed", I really think that reverse controls for click and drag are not going to be nice nor intuitive. Back at the time I played with the idea with the small Siyi A8 gimbal and it really felt nice this way, I didn't felt right at all reversed. If you really think that isn't the case, I can leave that as a setting, it is not a huge change.

@julianoes spotted that the gimbal icon would not appear on the light QGC theme. I forgot to add the coloring to it, so it was just showing the icon color, which was white. I fixed it now with proper theme colors so it should be fine. I rebased and force pushed, in case anybody has a previous local version of it.

Thanks!

[rmackay9]

Hi @Davidsastresas,

Yes, I'm totally fine with the click-and-drag feature as it is now. I'll get those logs re the control not appearing but I don't think we should hold up merging this PR in any case.

[julianoes]

Thanks @Davidsastresas. I've tested the basic functionality. Setting angles didn't work because the angular rate was set to 0, so it's basically limited at 0. I've fixed it by setting it to NAN but didn't have permission to push to this branch.

Here is the diff I need:

diff --git a/src/Gimbal/GimbalController.cc b/src/Gimbal/GimbalController.cc
index 954098a9f..d7e0f9d14 100644
--- a/src/Gimbal/GimbalController.cc
+++ b/src/Gimbal/GimbalController.cc
@@ -477,8 +477,8 @@ void GimbalController::sendPitchBodyYaw(float pitch, float yaw, bool showError)
                 showError,
                 pitch,
                 yaw,
-                0,
-                0,
+                NAN,
+                NAN,
                 flags,
                 0,
                 _activeGimbal->deviceId()->rawValue().toUInt());
@@ -510,8 +510,8 @@ void GimbalController::sendPitchAbsoluteYaw(float pitch, float yaw, bool showErr
                 showError,
                 pitch,
                 yaw,
-                0,
-                0,
+                NAN,
+                NAN,
                 flags,
                 0,
                 _activeGimbal->deviceId()->rawValue().toUInt());

• Retract doesn't work for PX4 yet, but I think that's because it's not implemented correctly in PX4.
• It would be nice to have a button to release control but we can add that later.

[Davidsastresas]

Thanks for the report both of you.

@julianoes I will fix that matter about nans and add under setting the acquire control button too. Wednesday at the latest. Thanks!

[Davidsastresas]

I uploaded a couple of commits addressing your fixes @julianoes.

This is how it looks like the setting and button for release/adquire control:

It seems to work fine checking GIMBAL_MANAGER_STATUS in mavlink inspector. Also I checked the NAN addition with Ardupilot and seems to work fine too.

Should we consider this ready to merge after you verify it? Thanks!

[rmackay9]

@Davidsastresas, great to see this. I'm very much looking forward to this being merged and released. Looks like there are some merge conflicts that need fixing?

[Davidsastresas]

@DonLakeFlyer thanks for pointing that out. I will check why it is failing and leave it fixed within the next 2 days. Other than that yes, I am alright with merging this. Thanks!

[julled]

is it possible to see the position the camera is pointing at (projection of the camera center to ground) on the map ?

I think thats included in this message: https://mavlink.io/en/messages/common.html#CAMERA_FOV_STATUS

[rmackay9]

@julled, ArduPilot has support for that message so its there is the GCS wants it but I don't think that is included in this set of changes. BTW, we plan to run a beta in the coming few weeks and we're looking forward to getting feedback from the community.

[julled]

@julled, ArduPilot has support for that message so its there is the GCS wants it but I don't think that is included in this set of changes. BTW, we plan to run a beta in the coming few weeks and we're looking forward to getting feedback from the community.

thanks for the info, so i guess it shouldnt be too hard to build this into QGC then. AFAIK mission planner supports draws that...

[Davidsastresas]

@DonLakeFlyer I fixed the tests. Gimbal controller sends several MAV_CMD_REQUEST_MESSAGE when a hearbeat is received to try to find new gimbals, and it was messing with some tests:

• RequestMessageTest: this was colliding directly, so on this one I disabled completely gimbalController. This test goes through the loop 2 times so this approach was more effective and simpler than trying to workaround it. We are just testing message requests on vehicle here so it should be good.

• SendMavCommandWithSignallingTest: Here it was colliding indirectly, getting the response to gimbal controller MAV_CMD_REQUEST_MESSAGE instead of the commands sent by the test, so I just added ignoring MAV_CMD_REQUEST_MESSAGE. I decided not to disable completely gimbal controller here because we might want to expand in the future the commands tested here, and it might be handy to have gimbal controller operational.

Let me know if it looks fine to you. Thanks!

[DonLakeFlyer]

Hmm, interesting. I'm going to have to think a bit about how to handle this well and keep unit tests working. I have a feeling the unit tests themselves can be rewritten such that they handle this case.

[DonLakeFlyer]

But for now, the changes you made are fine. So I'm merging.

[Davidsastresas]

Thanks @DonLakeFlyer. About the tests, I've been some time playing with the idea of doing the tests directly with PX4 and AP SITL, instead of using mocklink. I think it would have some advantages, and even though the initial effort would be considerable of course I think in the future it would make it easier to expand new tests. If anybody else is interested on this I am happy to join efforts! Thanks!

[mrpollo]

That would be an excellent addition to the CI infrastructure @Davidsastresas, LMK if there's anything I can do to help. Thanks for the work on this PR.

[andrefreitas97]

I have been testing this new feature and I noticed a bug. If I open the QGC app and then turn on the drone, the toolbar gimbal indicator does not show up. However, if I turn on the drone and then open QGC then it all works as expected. I have been changing things in the code but could not find the source of this problem yet.

Edit: I think I solved this problem by increasing "requestGimbalManagerInformationRetries" in GimbalController.h from 6 to 9. I don´t know why numbers like 9 and upwards work and below do not but it is working. Hope it does not introduce any other problem

[andrefreitas97]

Another "bug" I found is that it seems that the function "click to point" does not take into account the zoom level that the user selected and the screen controls behaves always as if there was no zoom.

[julianoes]

Thanks for testing and reporting @andrefreitas97 !

I think I solved this problem by increasing "requestGimbalManagerInformationRetries" in GimbalController.h from 6 to 9. I don´t know why numbers like 9 and upwards work

Could this be a timing thing?

the function "click to point" does not take into account the zoom level

I'd say this is a feature for a future version.

[Davidsastresas]

@andrefreitas97 , the beta testing have already started, so if that increase in retries fixed it we can modify it quickly. It is true that during this development I increased similar retries several times. For Ardupilot at least, the moment where parameters were downloaded, depending on the link, could saturate it and gimbal controller in QGC would have a hard time to pick up the hand shake, so it makes total sense that you had such issue and the increase in retries fixed it. We need to have more reports of it to modify it though. Thank you for the feedback.

Regarding zoom, it is not supported on the current version. A lot of camera and zoom work has been done very recently on mavlink-Ardupilot, it has been changing and evolving until very little time ago, so unfortunately for the moment we are not supporting it. We probably will in the future. Thanks.

@rmackay9, It would be good to double check what @andrefreitas97 reported about the retries. Lets keep an eye on it in case more users experience this, to fix it and update the release. Thanks!

[cuhome]

@Davidsastresas Does it support camera tracking image control and IR/RGB camera switching? I tested this version but I didn't find it.

[Davidsastresas]

@cuhome what camera are you trying to work with? such features are not really completely standardized nowadays in mavlink ecosystem still. You can make that work in several different ways, and a lot of manufacturers manage to do it, but it depends a lot on how your camera is presenting it.

[cuhome]

@Davidsastresas I use Vierpro A30TR, which is an IR+RGB+laser camera. I'm trying to fully integrate it into QGC with complete control. Do you have any comments or suggestions?

[Davidsastresas]

@cuhome I don't have any information about that gimbal in particular. The manufacturer should probably provide you with support for it, or information about how to add support for it in QGC.

[rmackay9]

@cuhome, I have that camera gimbal as well and if you're using AP then I think it will work fine with Copter-4.5.2 which should go stable later today. We're planning to start a beta of QGC 4.4 which includes the improved gimbal support tomorrow. That beta will be on the AP QGC support forum.

[cuhome]

@rmackay9
yes, I use AP and I'm trying to add support for these, but I'm afraid it won't become standard with PX4 and ardupilot. So I need to know your plans for this.

[rmackay9]

@cuhome
Regarding image tracking AP supports sending the image tracking commands to the camera but only via an RC auxiliary switch. I'm hopeful that in a future enhancement to QGC we can allow the user to click on the image or select an area of the image to trigger the image tracking. So AP supports that in general but we don't have QGC support yet.

[cuhome]

@rmackay9 Yes, I have tested using rc control; so I asked qgc if it has been implemented. I want to use PC to completely control the aircraft and camera. Currently I have to choose Viewlink+QGC, which is a terrible experience.

[cuhome]

@rmackay9 Are your skype and discord online? I have some questions and need your help!

[rmackay9]

Hi @cuhome, sure, I'm "rmackay9" on skype and discord.