Add support for MAV_CMD_DO_SET_ROI_WPNEXT_OFFSET

[peterbarker]

Closes #7658

I've only tested this as far as QGC can upload a site-scan mission and the vehicle seems to run it through to completion.

I have not made sure the gimbal actually tracks the next waypoint location properly...

Now includes a simple autotest.

The the way this waypoint works seems to be pretty straight forward in its use by sitescan. The vehicle flies parallel to a polygon defining the structure. The gimbal is instructed to point towards the next waypoint - but with a 90-degree yaw - so pointing at the structure. In the case of a circular structure the craft follows a regular polygon approximating a circle - so I would imagine that the gimbal would not keep the circular building centered in the frame.

I think this functionality could probably reasonably be put behind a define....

[rmackay9]

nice to see all these gimbal related enhancements! I'm not sure how much use the DO_SET_ROI_WPNEXT_OFFSET command will get but I added that issue because Don from QGC dev said there were some mapping related features that wouldn't work in AP without these.

[rmackay9]

This looks pretty good to me but it seems like maybe only the Gremsy and Servo gimbals are supported? We should add support for all gimbals really.

I'm slightly surprised (but not really) that we need a new gimbal mode to do this but I think this is essentially an existing issue that will only be fixed by moving the mode handling up into the backend code... that's beyond the scope of this PR though so if we can at least get all the other backends to support this new mode then I'm happy.

[rmackay9]

There's a request for this feature here in the forums. https://discuss.ardupilot.org/t/viewpro-gimbal-point-camera-here-function/98279/27

[Davidsastresas]

I just tested this, adding these 2 commits:

• Some redundant case handlers for ROI_NONE, probably from a recent rebase Davidsastresas@92b6d45
• While testing, I realized the yaw of the camera wiggles a lot when very close to reaching next waypoint. As the WPNEXT_OFFSET logic calculates an offset to the next waypoint, I imagine when very close it was hard to keep the calculations steady, so I added that quick check to just ignore new heading if next waypoint is closer than 4 meters. It solved the issue, but I am not sure if it is the most elegant approach. At first I tested a simple filter for yaw rate, but I thought it was overkill for this.
  Davidsastresas@cc54059

It seems to work fine in QGC 4 and 5.

Also I realized in b155377 in some parts it is added code related to ROI_NONE that might not be needed.

[peterbarker]

I just tested this, adding these 2 commits:

Fantastic, thanks!

* Some redundant case handlers for ROI_NONE, probably from a recent rebase [Davidsastresas@92b6d45](https://github.com/Davidsastresas/ardupilot/commit/92b6d4533f7c434be2f5caa99d7881a20418c242)

Pulled that commit in here, thanks!

* While testing, I realized the yaw of the camera wiggles a lot when very close to reaching next waypoint. As the WPNEXT_OFFSET logic calculates an offset to the next waypoint, I imagine when very close it was hard to keep the calculations steady, so I added that quick check to just ignore new heading if next waypoint is closer than 4 meters. It solved the issue, but I am not sure if it is the most elegant approach. At first I tested a simple filter for yaw rate, but I thought it was overkill for this.
  [Davidsastresas@cc54059](https://github.com/Davidsastresas/ardupilot/commit/cc540590af3fe34eaef236c20fb1f210e7af5161)

So like you I'm a little unsure of this one :-)

We probably want to base this on the acceptance radius of the waypoint. I suggest we leave this patch out for the time being and revisit it later - just so we can get this merged now it's been properly tested!

Also I realized in b155377 in some parts it is added code related to ROI_NONE that might not be needed.

No, I think those are needed; they augment the use of None in missions.

[Davidsastresas]

Since MAVLink gimbal manager V2 is now widely adopted (Mission Planner and QGC both use it), MAV_MOUNT_MODE appears to be used only internally in ArduPilot and for legacy features outside gimbal manager V2 (like "follow system ID").

If this works for the project, that's fine - but we should push toward using the new protocol fully since it provides better controls and feedback.

Two suggestions:
1 - Move MAV_MOUNT* to ArduPilot dialect to avoid drift from upstream common.xml, or if that is too much trouble here and upstream maybe do the equivalent "non deprecated" version for internal AP use in ardupilotmega.xml.
2 - Standardize missing features upstream - if gimbal manager V2 lacks features like "follow system ID", let's add them to the standard protocol. The rest of features mentioned right now are fully supported with feedback included in the new protocol if I am not mistaken.

Every now and then I help maintaining that part in QGC and these kind of "exceptions" are hard to make sustainable in the long term.

Thanks!

[tridge]

needed for all the other backends too?

[peterbarker]

Since MAVLink gimbal manager V2 is now widely adopted (Mission Planner and QGC both use it), MAV_MOUNT_MODE appears to be used only internally in ArduPilot and for legacy features outside gimbal manager V2 (like "follow system ID").

In what way, shape or form is "follow system ID" "legacy"? Likewise "point home" - a quite-recent feature contributed by FPV flyers that wanted an easy way to come home.

If this works for the project, that's fine - but we should push toward using the new protocol fully since it provides better controls and feedback.

There's no reason to think the features couldn't be implemented in the new sub-protocol. But somebody has to do that work.

Two suggestions: 1 - Move MAV_MOUNT* to ArduPilot dialect to avoid drift from upstream common.xml, or if that is too much trouble here and upstream maybe do the equivalent "non deprecated" version for internal AP use in ardupilotmega.xml. 2 - Standardize missing features upstream - if gimbal manager V2 lacks features like "follow system ID", let's add them to the standard protocol. The rest of features mentioned right now are fully supported with feedback included in the new protocol if I am not mistaken.

It was agreed at the call that ArduPilot should stop using the mavlink enumeration internally for storing state - we've already done that for several other enumerations (e.g. GPS_Fix_Type)

[Davidsastresas]

Maybe "legacy" was an unfortunate word choice. I meant features that do not have a gimbal manager v2 dedicated command.

From a GCS perspective ( taking QGC as an example ), point home can be achieved right now sending a ROI to the home position, which we always have feedback for, so I don't think there is need for a dedicated command.

"Point to system ID" on the other hand, I don't think has support in any way from gimbal manager v2 messages/commands.

I am happy to work on leveraging all the remaining gimbal features on the new protocol in all fronts ( Ardupilot, mavlink and QGC ). But as it happens with the "multi GCS front", I need an Ardupilot insider to just give me high level guidelines, as I am not super involved in Ardupilot and I might need a lot of iterations ( and a lot of time ) to nail it so it fits properly in AP codebase.

Moving on, I will open an issue in Ardupilot to start working on this #31501.

Thanks!

[peterbarker]

From a GCS perspective ( taking QGC as an example ), point home can be achieved right now sending a ROI to the home position, which we always have feedback for, so I don't think there is need for a dedicated command.

That's not the same thing - home can move over time, so unless you want the GCS to track home position updates and re-issue the command you won't get the same effect. Some people even use moving home in their (generally bad, IMO...) worflows as a matter of course.

[Davidsastresas]

@peterbarker that's a good point, if we expect users to move home a lot during operation it would be over-complicated to keep track of that in the GCS, so I am also adding point home in #31501.

As mentioned earlier, I don't mind to do the heavy lifting but I need high level guidelines. Thanks!

[peterbarker]

@peterbarker that's a good point, if we expect users to move home a lot during operation it would be over-complicated to keep track of that in the GCS, so I am also adding point home in #31501.

As mentioned earlier, I don't mind to do the heavy lifting but I need high level guidelines. Thanks!

Update is that at DevCall people wanted the mount backend stuff cleaned up before this one went in. #31505 is the first PR which moves stuff up, there will be at least one more that big. Waiting on Randy to approve that one.

[Davidsastresas]

Great, thank you. I will wait until all of this is sorted out to proceed. Also please, let me know, or update #31501 if any new requirement is needed to add the proper interfaces to QGC. Thanks.

[peterbarker]

@Davidsastresas well, we're no longer playing around in all of the backends individually to support this. If you have an easy testing setup you can use on this then some testing would be nice. I'd like to get this in for 4.7