[WIP] Improve gimbal support

[julianoes]

This work mostly done by @Davidsastresas aims to bring gimbal functionality as much as possible based on the gimbal v2 protocol.

• [x] Support acquire/release gimbal control
• [x] Support multigimbal
• [x] Joystick buttons control: supporting up/down/left/right and center
• [x] Gimbal control acquisition for AP
• [x] ROI to home in AP
• [x] Retract in AP
• [x] Gimbal control acquisition for PX4
• [ ] ROI to home in PX4
• [ ] Retract in PX4
• [ ] ...

FYI @olliw42, @rmackay9

[rmackay9]

@julianoes,

Really great to see this. For the gimbal control aquisition issue for AP, it's the GIMBAL_MANAGER_INFORMATION support that is needed?

EDIT: here is the AP PR to add support for gimbal-manager-information. https://github.com/ArduPilot/ardupilot/pull/23587

[kadabr]

Would it be reasonable to have a separate class like GimbalManager for related functionality to not overload the Vehicle class?

[Davidsastresas]

Would it be reasonable to have a separate class like GimbalManager for related functionality to not overload the Vehicle class?

That is one of the first tasks I would like to perform before undrafting this. Thanks!

[julianoes]

Would it be reasonable to have a separate class like GimbalManager for related functionality to not overload the Vehicle class?

Yes, especially because that will later allow us to have multiple gimbals, something that the protocol should support.

[julianoes]

@rmackay9 for control it's actually https://mavlink.io/en/messages/common.html#GIMBAL_MANAGER_STATUS. I want to request that topic at a low rate in a next commit.

[olliw42]

many thx for this, and for putting me in the loop

as much as possible based on the gimbal v2 protocol

This statement is unfortunately not true. It uses messages/commands of v2, but does is not compliant with the protocol.

The code appears to assume that autopilot component id and gimbal manager component id are equal. That's at least the conclusion which lines like these (and others) enforce:

• https://github.com/mavlink/qgroundcontrol/blob/310d1c6a6ca78f6a0bfd952ab77961c713982c9a/src/Vehicle/Vehicle.cc#L4213-L4215 => _defaultComponentId needs to be the gimbal manager id
• https://github.com/mavlink/qgroundcontrol/blob/310d1c6a6ca78f6a0bfd952ab77961c713982c9a/src/Vehicle/Vehicle.cc#L4228-L4230 => _defaultComponentId needs to be the autopilot id

ergo: gimbal manager id = autopilot id Q.E.D.

Proof of the taste of the pudding: When I would connect my AP fc with a STorM32 gimbal and with its (secret) gimbal manager v2 implementation enabled, things would just not work.

I may not see and overlook it, but I can't find any gimbal manager discovery code. Discovery seems to be based on GIMBAL_DEVICE_ATTITUDE_STATUS, https://github.com/mavlink/qgroundcontrol/blob/310d1c6a6ca78f6a0bfd952ab77961c713982c9a/src/Vehicle/Vehicle.cc#L4305-L4308, which can't reveal the gimbal manager id.

ergo: not yet standard conform

Btw, I may be wrong, but it seems to me that this could be relatively easily corrected, if as suggested elsewhere one would use GIMBAL_MANAGER_STATUS for discovery. In that case one only needs to move these lines https://github.com/mavlink/qgroundcontrol/blob/310d1c6a6ca78f6a0bfd952ab77961c713982c9a/src/Vehicle/Vehicle.cc#L4305-L4308 to here https://github.com/mavlink/qgroundcontrol/blob/310d1c6a6ca78f6a0bfd952ab77961c713982c9a/src/Vehicle/Vehicle.cc#L4334, memorize the message source id as _gimbalManagerComponentId, and use _gimbalManagerComponentId instead of _defaultComponentId in the places were it should be used. I probably miss a point though. Btw, along with _gimbalManagerComponentId one also should memorize the gimbal id field in this message (see next point).

I again may overlook this, the gimbal id seems to not be checked anywhere against the gimbal's component id, that is only one gimbal is allowed and it's not ensured if it's the one the gimbal manager relates to. This again should be easy to correct. E.g. GIMBAL_MANAGER_STATUS should be ignored if the source id isn't the gimbal manager's gimbal id.

I again may overlook this, it seems that GIMBAL_MANAGER_INFORMATION is not yet requested and not digested. Discovery could also be based on it. Moreover, the capabilities flags may be wanted before any actions being enabled, since they may be checked before some of the actions become available or are executed. Just checking for _haveGimbalData may not be desireable. Note: This is in fact not absolutely required, since one can assume/hope that the gimbal won't freak out if one asks it to do what it can't do, but it's just not so nice to make the user believe things are possible which they aren't.

Note: The docs say that one should do the gimbal manager discovery by requesting GIMBAL_MANAGER_INFORMATION. As argued elsewhere, and also implied here, we may want to change the specification and also allow discovery via GIMBAL_MANAGER_STATUS, it's just often easier and simpler.

If I'm not mistaken, the angles extracted from GIMBAL_DEVICE_ATTITUDE_STATUS will produce nonsense when the gimbal is tilted down, since an inappropriate euler angle representation is used, https://github.com/mavlink/qgroundcontrol/blob/310d1c6a6ca78f6a0bfd952ab77961c713982c9a/src/Vehicle/Vehicle.cc#L4291. I think you tripped into this also not so long ago. The more interested may read up here, http://www.olliw.eu/2020/mavlink-gimbal-protocol-v2/#AttitudeQuirks, subsection "Status". Implementation one can find here: https://github.com/olliw42/BetaPilot/blob/BetaCopter43/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.cpp#L67-L102.

GIMBAL_MANAGER_STATUS. I want to request that topic at a low rate

While a technically possible approach, I think it is the least desirable. I think, the manager should sent it out voluntarily. The reasons are quite similar to why a component is supposed to send out the heartbeat voluntarily, and not only then the heartbeat is requested. Moreover, the manager can do smarter, e.g. send it out more frequently when some status has changed. In short, it makes things just so much easier. Protocol wise, code wise, implementation wise, traffic wise, feature wise.

Note: This is also what the docs/standard says ;) I argue this not accidentally so :)

Olli

[olliw42]

Q: I pulled that PR and tried to compile, but get an error "dependent C:\User....\FirmwarePlugin\APM\APMParameterFactMetaData.Copter.3.6.xml does not exist". I do get the problem also on master branch.

I'm probably doing something very simple worng, but lack the competence to solve it myself. What might be the issue?

I'm on Win10, use QtCreator, have installed things per docs, including the correct packages, and the compile indeed worked fine not too long ago (maybe 1/2 years). I also have done the submodule trick, and have the .xml files indeed on the disk, in the location suggested in APMResources.qrc, https://github.com/olliw42/qgroundcontrol/blob/master/src/FirmwarePlugin/APM/APMResources.qrc#L53

[julianoes]

@olliw42 thanks for the review. There might be a bit of a misunderstanding. This PR is intended to be, eventually, when it's done to be as much as possible based on the gimbal v2 protocol. In the current state, it is definitely not yet, it's a draft PR for @Davidsastresas, me, and whoever else has input to collaborate.

I would have thought that becomes clear when going through the code. It's clearly in a raw state with lots of TODOs.

I've changed the description to reflect that more closely. Also the title says WIP.

And just last night I added a separate class where we can shift the functionality over, in order to implement things like component IDs and multiple gimbals properly.

So things will happen. Just be patient.

This statement is unfortunately not true.

ergo: not yet standard conform

Just for next time: I find that language hard to cope with. It comes across condescending (or in German rechthaberisch).

---

Ok with that aside, let me respond to some of the (valuable) comments:

If I'm not mistaken, the angles extracted from GIMBAL_DEVICE_ATTITUDE_STATUS will produce nonsense when the gimbal is tilted down

You're right. Remind me what we should use instead? Or what about you use the GitHub review feature which allows you to do a Suggestion where you can put your code in directly.

---

GIMBAL_MANAGER_STATUS. I want to request that topic at a low rate

While a technically possible approach, I think it is the least desirable. I think, the manager should sent it out voluntarily. The reasons are quite similar to why a component is supposed to send out the heartbeat voluntarily, and not only then the heartbeat is requested. Moreover, the manager can do smarter, e.g. send it out more frequently when some status has changed. In short, it makes things just so much easier. Protocol wise, code wise, implementation wise, traffic wise, feature wise.

This is interesting. I had forgotten about it. The reason I suggest to request it from QGC is that is - I believe - generally how it's done. So by default an autopilot sends very little or no data but topics are requested as required. For instance if a ground station does not care about gimbals it shouldn't be flooded with gimbal messages. It depends on bandwidth and use case.

However, as you write, it would be good/best to send it out whenever there is a change and at a low rate in-between. So therefore, the optimum would be, if we could request the message at a low rate, and also get updates whenever it changes. Unfortunately, that's not exactly an option of MAV_CMD_SET_MESSAGE_INTERVAL. Looking at it, what is an option is "request it at default rate" and I guess that would make most sense. The gimbal manager can then interpret that as 0.5 Hz + updates.

[julianoes]

@olliw42 regarding the build problems, I would create a separate issue. I don't use Windows or QtCreator, so I probably can't help you.

[olliw42]

frankly, I think you want my language to come across like this. No chance to do it right, I give up. Cheers,

[julianoes]

@olliw42 I'm sorry I was assuming you had seen my other post where I wrote that I actually agree with you on the fact that iterative doesn't always work, for instance when it prevents us from being compatible in the long term. I had only realized that when I was actually working on the PR. But I can't find my post anymore so the dog must have eaten it (or I didn't hit send). That's why I was surprised why you still really needed to prove that you're right even though I had already agreed with you on that.

Sorry about that! Anyway, you're right, but let's turn that into action and get this PR in a good shape that works for STorM32, ArduPilot, and PX4.

[rmackay9]

Hi there, AP now supports the GIMBAL_MANAGER_STATUS and MAV_CMD_DO_GIMBAL_MANAGER_CONFIGURE messages (PR is here) so I think the top checkbox in this PR's description can be checked now.

There's actually little more work we need to do to better enforce only one controller of the gimbal but that shouldn't hold up this PR.

The next thing we're planning is better discovery of the camera and FOV feedback.

[Davidsastresas]

Thanks @rmackay9 . I think next week I will be able to flash AP master on my testing vehicle and advance on the QGC front on top of the amazing work Julian is doing. I don't think we are too far of having this ready. Thanks!

[julianoes]

@Davidsastresas I just rebased this on master and continued a little bit:

• I removed some of the Joystick stuff, hoping to add it back in but better and using rate control.
• I added another button to the gimbal UI where we can select the gimbals (if there are multiple connected). I'm not convinced this is the right way and it's not hooked up yet :(.

[Davidsastresas]

Thanks! I think next week I will be able to give it another push.

Regarding joystick, funny that you mentioned it, I worked a bit over it a few weeks ago, and I thought on bringing it back. Indeed when this gimbal manager integration is finished it would be great to link it to joystick for gimbal pan/tilt.

Thanks!

[Davidsastresas]

I spent some time today testing this with Ardupilot master. The handshake, creation of gimbal instance, etc work perfectly on Ardupilot for single gimbal, but for multiple gimbals it doesn't work, because the messages GIMBAL_DEVICE_ATTITUDE_STATUS would come from the same compid. We have a device id in all the control commands, but not on this feedback message. Julian made a PR that fixes this.

https://github.com/mavlink/mavlink/pull/2009

So after that PR makes it upstream and we adapt the current QGC implementation to account for it, I think everything should be ready from the Autopilot firmware side, and only polishing details on QGC would be needed.

A little detail, I pushed a commit to modify how we were requesting MAVLINK_MSG_ID_GIMBAL_MANAGER_STATUS. It was done sending the rate field at 0 ( use standard autopilot rate for that message ), but on Ardupilot that is not defined yet, because it isn't really supported yet. So if that attempt doesn't work, QGC will request it at 0.2 Hz ( and in the future when this is implemented in the autopilot firmwares, they would send this message as well whenever the control ownership changes ).

Is this 0.2 Hz fine? What do you think @rmackay9

Thanks!

[rmackay9]

Hi @Davidsastresas,

Maybe I'm misunderstanding something but AP doesn't send the gimbal-manager-status by default which means the default rate is zero so I think QGC will always need to say what rate it wants (when talking to AP). I guess we could change it so that AP does send it by default but at this point there are very few consumers of the message so this seems like a waste. Better for QGC to just request it I think.

0.2 hz seems slow to me but I don't mind really. if you guys think that's fine then I'm fine with it.

[Davidsastresas]

@rmackay9 sorry, maybe I wasn't clear. What you said is exactly what I meant. AP doesn't have a default rate for that message, which makes sense as functionality around it is not implemented, so I think it is totally fine that AP doesn't have a default rate for it for the moment.

So right now QGC will ask 3 times to the autopilot to stream that message at the default Autopilot rate. If the autopilot doesn't have a default rate for that message ( default rate 0 ), then QGC will try to request that message at 0.2 Hz.

I do agree 0.2 Hz is very slow, but that message is only carrying information about the ownership of control for that gimbal. So I really think those 0.2 Hz, plus AP sending that message whenever control ownership changes, could be enough. I didn't want to use a higher rate precisely because what you said, there are very few consumers of this message, I don't think it makes sense at this point to use a much higher rate, would be a waste of bandwidth.

If later on we have functionality in AP that could justify sending that message at a higher rate we can just adjust the default rate on AP and everything will work as expected.

[rmackay9]

@Davidsastresas, ok, sounds good!

[Davidsastresas]

I rebased to master and push -f, in case anyone is tracking this work. It was needed because lots of commit got into master since we started this, better to keep it updated.

I added a few commits:

• I brought the latest changes from mavlink around gimbal manager v2, as it is not WIP anymore
• I increased the request status message retries to 6, for some reason for Ardupilot it wasn't enough sometimes at 4, specially when QGC is open, and Ardupilot boots up right when connecting with QGC. As this only happens on the initial handshake I think it isn't a big deal to increase by 2 the request attempts.
• Latest commit is a lot of rework over GimbalController class. The initial implementation would discover gimbals based on heartbeat, and the internal sorting of gimbals was based on the compid the message they were coming from. This would not allow proper functioning of multigimbal systems where all the gimbals come from the same compid. So I implemented it following:

https://mavlink.io/en/services/gimbal_v2.html

Particularly the part "discovery of gimbal manager" where it says something like "Discovery should be based on GCS sending a MAV_CMD_REQUEST_MESSAGE for GIMBAL_MANAGER_INFORMATION and we should instantiate gimbals when we receive response to this message."

Doing it this way allows it to work for both, gimbal managers on different compids and gimbal managers within same compid. Now the gimbal_device_id concept is used to discover/store discovered gimbals.

I tested this on Ardupilot SITL with the following commits:

https://github.com/ArduPilot/ardupilot/pull/24354 https://github.com/ArduPilot/mavlink/pull/323

simulating several gimbals and it seems to work fine, although maybe when we keep advancing we need to polish some details about it.

NEXT STEPS BEFORE BEING READY TO PUSH

• I would like to do a rework on the frontend, to allow a nice way to select the active gimbal. I am now moving the gimbals to a qmlobjectlistmodel list, so it is nicer for this purpose.
• Add actions to GimbalController: right now they are over vehicle, and some of them are not using gimbal manager v2, we need to update all actions ( neutral, retract, etc ) to gimbal manager v2. Some of them would be good being buttons-indicators, like yaw lock/unlock, getting the flags from gimbal device I think that can be done now that all the parties have it implemented, it is much more compact and intuitive this way, rather than buttons that send actions with no feedback.

But I think we are close to having it ready. Thanks everybody involved, I am very happy this is moving forward!

[Davidsastresas]

In case anybody is following, I rebased to master and pushed -f. I will spend some time on this in the following weeks so it was needed.

[Davidsastresas]

I pushed a bunch of commits:

• A debug panel to show gimbal pitch and yaw, just provisional to make debug of functionality easier
• Add representation of gimbal yaw to vehicle overlay in map, it started as a debug functionality but It is actually cool: It will probably change in the future, but it is handy to leave it there now to have visual feedback of camera yaw for debugging.
• Most of gimbal functions that used to be in vehicle moved to gimbalController. This has been moved as it was in vehicle, but those functions need rework, to take into account gimbal control adquisition, etc. Also some of them are APM specific, like setting rc targetting mode.
• Adapt front and backend to new location of gimbal functions.
• Fix gimbal yaw interpretation from gimbal_device_attitude_status. The approach used using Eigen would only interpret yaw as 0-180, and when negative values ( left semi circle considering nose of vehicle ) it would repeat again positive 0-180. It uses now mavlink helper quaternion_to_euler. I wasn't making sense of this and that is why I prepared the gimbal yaw representation to vehicle icon, to try to understand what was happening.
• Make gimbal a qobject, and expose qproperties so qml can pick mount telemetry from there. Right now this is not used, right now front end is using the remains of gimbal present in vehicle, but will be moved to this soon. Also this change to Q_Object is needed in order to support properly multi gimbal control.

As it is now, everything works with Ardupilot. Work pending to be done for a minimum working version of this:

• Clean all the functions in gimbalController for gimbal actions. Remove apm specific code, and make sure gimbal adquisition etc is implemented as it should.
• Finish porting all gimbal parts from vehicle to gimbalController
• Finish gimbalController functionality to support multigimbal control, right now it is controlling first gimbal only. This will need to adapt the q_invokables, and also the gimbals vector, convert that to qmlobjectlistmodel so it plays nicer with qml.

Besides the above, what I would like to do before considering this finished:

• Rework UI. With the new implementation completed, hopefully we can make it much more compact ( merge yaw follow/yaw lock button for example as we could have status feedback ). Also I would like to add some indicators of gimbal pitch and yaw. There is a debug panel already showing this, but must be implemented nicely.
• Fix AP camera setup submenu. It needs to get updated as it is referencing a lot of outdated params right now.
• Click on screen functionality: needs to be cleaned up. Settings should be elsewhere ( right now they are in vehicle camera component menu ), probably accesible through the flyview UI, for quick setup of FOV, etc.

[Davidsastresas]

I pushed a bunch of new commits. The news are:

• Minor UI fixes: disable map tools when in video view, automatically disable "click on map to get coordinates" when that menu is hidden, etc
• Provisional button to manually release control. It will probably evolve in a release/adquire control button, depending on the current status. Although automatic gimbal adquisition was implemented, more below.
• The concept of "active gimbal" was implemented, so now all the commands refer to this gimbal, and it was properly linked when sending commands, filling the proper deviceId field. From here, just changing the active gimbal would allow us to operate multiple gimbals, it is what I am working over right now.
• Adquire/release control is working. I tested with multiple instances of QGC with different sysid. When attempting to send a gimbal command, if we are not in control a popup will appear asking us if we want to acquire control. This only happens when others are in control. If nobody is in control, QGC will attempt to get control, but will send the command anyway.

On this last part, I realized Ardupilot is not sending right away a gimbal_manager_status when the sysid/compid pair in control changes. It would be good to implement this in Ardupilot, so besides sending this message at the standard rate ( 0.2hz I believe, which is fine ), it would be good to send one single message when control changes. I will work on this shortly as well @rmackay9 .

[Davidsastresas]

I pushed another bunch of commits. Everything is in a pretty good state right now, I think we could start thinking on trying to get more people involved in testing, in case you are interested @rmackay9. I still want to finish some details before considering to merge this, also in case @julianoes gets to work on this before I come back to it:

• Multigimbal telemetry: right now we are only showing the active gimbal
• Limit how often the "acquire control" popup can appear. This will popup whenever we attempt to do a command without being in control. It could potentially appear a lot of times in a row, so I want to limit this to only show maximum one every few seconds. video showing this here: https://drive.google.com/file/d/1aDYX5VKTJNY2Vb6BUi-b6FU7uFevOOUL/view?usp=sharing
• Double check destructors: Would be good to link with vehicle destructor to delete gimbal objects.
• Redo UI: The one present right now really needs to be redone. We can merge a lot of buttons into a single button changing functionality depending on state, etc also it could be prettier, some panels are not aligned, etc

The new changes are:

• Acquire/release control button is now automatic: Will show/do release or acquire control depending on if we have or not control.
• Multi gimbal working: There is a select button in the gimbal toolbar, after clicking we will be able to select the current active gimbal. All the commands and telemetry will correspond to this new active gimbal
• Joystick button control working: We can configure now joystick buttons for gimbal up/down/left/right and center.
• Vehicle: I think everything related to gimbal is already moved to gimbalController

[Maarrk]

Hi, we're working on our gimbal, and it would be great to get it ready for the v2 protocol from the start.

I see that this has already had a lot of work done, can I ask @julianoes and @Davidsastresas whether you still expect big changes, or just some GUI improvements here and there? I'd love to start using these features ASAP, and just wanted to know your advice on this.

Thanks a lot for the feature, and sorry if I'm bothering you with this

[Davidsastresas]

Hello @Maarrk,

I would say the backend is pretty close to be completed, although some small details could be polished still. I would recommend to double check with the gimbal manager V2 documentation from mavlink in case you have doubts with any of it.

But as it is right now, I would say it is very close to what it will be on the first iteration, so for the most of it you can take it as reference for your system.

The UI related to this will change a lot probably, the one present right now was the first proof of concept and will be redone before this PR is completed.

If you want to discuss something in particular feel free to ping me on discord, I am davidsastresas.

Happy to hear a manufacturer is joining this! I wish you best luck on your projects. Best regards!

[DonLakeFlyer]

Why is FakeToolStripHoverButton needed?

[DonLakeFlyer]

Also need some screen shots of how this all works/looks ...

[DonLakeFlyer]

I tried rebasing this myself so could take a look but it turned into a nightmare. It's been sitting too long. Would be good to at least get it rebased.

[julianoes]

I had done a rebase recently and not pushed, I think. Let me check.

Edit: nope, not recent enough.