[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.

• Support acquire/release gimbal control
• Support multigimbal
• Joystick buttons control: supporting up/down/left/right and center
• Gimbal control acquisition for AP
• ROI to home in AP
• Retract in AP
• 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. ArduPilot/ardupilot#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:

• qgroundcontrol/src/Vehicle/Vehicle.cc

  Lines 4213 to 4215 in 310d1c6

  	sendMavCommand(
  	_defaultComponentId,
  	MAV_CMD_DO_GIMBAL_MANAGER_PITCHYAW,

  => _defaultComponentId needs to be the gimbal manager id

• qgroundcontrol/src/Vehicle/Vehicle.cc

  Lines 4228 to 4230 in 310d1c6

  	sendMavCommand(
  	_defaultComponentId,
  	MAV_CMD_DO_MOUNT_CONTROL,

  => _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,

qgroundcontrol/src/Vehicle/Vehicle.cc

Lines 4305 to 4308 in 310d1c6

	if (!_haveGimbalData) {
	_haveGimbalData = true;
	emit gimbalDataChanged();
	}

, 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

qgroundcontrol/src/Vehicle/Vehicle.cc

Lines 4305 to 4308 in 310d1c6

	if (!_haveGimbalData) {
	_haveGimbalData = true;
	emit gimbalDataChanged();
	}

to here

qgroundcontrol/src/Vehicle/Vehicle.cc

Line 4334 in 310d1c6

}

, 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,

qgroundcontrol/src/Vehicle/Vehicle.cc

Line 4291 in 310d1c6

mavlink_quaternion_to_euler(o.q, &roll, &pitch, &yaw);

. 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.

mavlink/mavlink#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:

ArduPilot/ardupilot#24354
ArduPilot/mavlink#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!

[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.

[DonLakeFlyer]

So in looking at this my thinking is that the Gimbal stuff should be a new Toolbar indicator as opposed to controls which are always showing on the Fly View. Then with the new Indicator stuff you can use expanded page for settings and so forth.

Works like a flight mode menu. Current mode shows in the toolbar. Click to select new mode. Actions and settings are available in the expanded indicator.

[DonLakeFlyer]

Also the video click stuff seems to be a corollary to an ROI. Why not just make the video clicks just set a regular ROI? Why does it need to be specially handled? Just let a normal DO_SET_ROI_LOCATION handle everything.

[rmackay9]

@DonLakeFlyer, thanks very much for the reviews!

[Davidsastresas]

I will finish this PR, and rebase it to master, and to 4.3 in the following days. Thank you very much @DonLakeFlyer for taking a look at it. Answering your points:

FakeToolStripHoverButton:

This is a work in progress, it is meant to be used in a panel that could combine those buttons with labels, etc in multiple columns and rows, but preserving the aesthetics and dimensions of toolbar buttons. I was playing around with the idea of such a panel, and I found the extra complexity of ToolStripHoverButton, depending on a toolStripAction, harder to play with on this panel concept. Right now it is only used for gimbal selection. Most likely I can get this to work with new properties over ToolStripHoverButton, but as I still don't know what we will finally need I found faster to iterate this way, in a separate UI element.

Screenshots:

Yes, definitely. One of the pending items is to redo the UI, so when undrafting this PR my idea was to present a short video and/or screenshots to make evaluation of the PR easier. As the UI was still evolving I was holding on this.

ROI - Video click stuff

I am not sure I follow you on this one. I think maybe you mixed 2 functionalities, the ROI panel, which has functionality that effectively overlaps with standard QGC ROI ( more on that below ), and the click over video to point gimbal.

The click on video feature is meant to be used like this ( min 0:43 - 1:10):

https://www.youtube.com/watch?v=ROyj6F6XieE&t=19s

So if this is what you meant, I don't understand how do you suggest we use DO_SET_ROI_LOCATION instead. That would mean calculating the angle our click represented, interpolate that with terrain altitude to obtain a ROI, and send that instead. Although overkill just for this "click and point" feature, it would be pretty cool. but it would have some issues like, what happens if we click in something in the sky, in a way that we don't have terrain to interpolate with, where we would set the ROI in this case?

Now, If you meant the ROI panel, I agree, that overlaps functionality with the existing ROI functionality. I was playing with the idea of having a panel to introduce manually the coordinates for ROI, as well as the Altitude for ROI, and I also left the shortcut to "get coordinates from map" in case that was useful. It is not polished and it needs some fixes, but I think the concept is cool, a panel to work quickly with ROI, rather than relying on clicking on map and confiming slider every time. Clicking around over the map is a good way of looking around with a gimbal, and I was playing with this. If you don't think this is useful or we should do it differently I am of course open to suggestions.

Please clarify what you meant on this point, I don't think I got it.

Toolbar indicator versus current panel in fly view approach:

The idea was that this new toolbar appears in fly view only when a gimbal is detected, so It won't be there all the time. And even when it is there, there is a button to hide it, so It barely occupies the space of a regular toolbar button when collapsed.

At first I thought about adding all of this in a top toolbar indicator, but after playing with the concept, I really don't think it makes sense for gimbal control. I 100% see your point, the top toolbar is a great place for this on paper, but I think it is quite different from what we usually have in the top toolbar.

In a vehicle with a gimbal attached, unless it is a survey vehicle and the gimbal stays on the same place most of the time, the user is going to interact a lot with it. And even though the top toolbar is "quick" to click, it is not near as handy as the fly view space is. The fly view toolbar has permanently some actions like Land or RTL that are only triggered once or twice per mission, and yet they have their own super handy space in the left margin. Same with PlanView button, there are a lot of solid QGC use cases that will use very little plan view.

These spaces in left and right margins are super handy on small mobile devices because they are within range of the thumbs. You can click them without moving your hand from "stick position". And that is why I tried to place this handy in the flyview, because the user is really going to interact with them, and the handier they are, the nicer the user experience is going to be with gimbals. And the left margin is even more atractive than the right one, because usually most people operate in mode 2, and they usually fly in a altitude assisted mode, so usually the left stick is less important than the right one, so it is better to drop left thumb than right thumb from the sticks for frequent actions.

Again, This is a draft PR, and I am more than open to discussion, If you consider we should place it following the top toolbar scheme, maybe we can do it like that on default, with a very compact set of controls on the dropdown panel, but then in the expanded settings we could allow the user to place this over fly view instead, for the reasons mentioned above.

Would you be okay with this? I really think these are controls the user will interact with a lot, so I think it is worth at least leaving it optional. I think it is really going to be good for some users. Gimbal controls are used a lot during some operations, and I think this kind of users are really going to welcome the possibility of having such controls super handy.

Thank you very much for taking the time to review all of this!

[DonLakeFlyer]

Now, If you meant the ROI panel, I agree,

Yes, this is what I was talking about. As far as editing the position of a point on the map there is already scaffolding for this. For example, modifying the position of a survey area polygon vertex. Click the vertex, then select edit (and I just found a new Qt6 bug that needs fixing!):

This is used all over Plan.

That's a generic position editing dialog which is available for use anywhere. I had already been in the process of cleaning up ROI last week. I got rid of the ROI toolbar thing which didn't make sense. You now just click the ROI to remove it. I was planning on adding drag support which is also a standard ability of map indicators (as is click signaling support). Adding position editing I think makes sense as well. I can do that. You are also correct that the whole slider to confirm an ROI is total overkill. I can remove that.

Toolbar indicator versus current panel in fly view approach

I don't disagree with the toolbar not being quite a fit for this. That said my main complaint is adding yet another new UI thingy to control the gimbal. As well as chewing more real estate in the fly view. There are already plenty of usable controls/places to support this. Less is more. Also more custom controls that are only used a little create maintenance overhead (as you can see from all the stuff Qt6 broke). There has to be high bang for the buck. The regular Toolstrip/DropPanel support seems perfectly fine for it. I was able to hack a mockup in QGC using your branch in a couple minutes:

This uses all the currently available UI parts of QGC to make it work. And it doesn't chew permanent valuable real estate from the fly view. You can put icons in regular QGCButton (although I discovered yet another Qt6 break that icons positioning is broken). My quick work looks like shit, but can easily be made to look nice.

[DonLakeFlyer]

Also if the ToolStrip action stuff is too much of a pain to work with we can change it to simplify. No need to make a whole new thing. I already need to go in there to add support for custom builds to add actions to the Fly Toolstrip since I need that myself. When I do that I'll look at revising how that whole thing works.

[Davidsastresas]

@DonLakeFlyer I get your point, thank you.

On the next following days I will attempt to rework all of it to better stick to your guidelines. I totally understand your point of maintenance overhead, etc So I think it makes sense to do this first iteration as little invasive as possible.

However, if after this first iteration we find it isn't optimal for gimbal/camera control, I would like to review this again, even if that means major rework of fly view UI. I think cameras/gimbals are a very important part on a good portion of the userbase of QGC, so if after this first minimal/non-invasive iteration we find there is really a lot of room for improvement in user experience, I will pursue that when the time comes, I hope that is fine for you.

But for the moment, I will stick to your suggestions, as I understand your points and they make sense to me as well for this first revision.

Thanks!

[DonLakeFlyer]

However, if after this first iteration we find it isn't optimal for gimbal/camera control, I would like to review this again

Absolutely. That's true for anything.

[Davidsastresas]

Since this was included here #11276 and that other pull is basically where we are iterating the fixes for this gimbal support, I think we could close this one.