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.

[timtuxworth]

@Davidsastresas note that I have changed the scaling in storage for the parameters - int16_t cd rather than raw degrees.

That means any vehicles flying this will have... problems... with the relevant mission item until the mission is re-uploaded and thus stored in the new format.

I'm wondering why you would do this @peterbarker, aren't we trying to get away from cd?

[peterbarker]

@Davidsastresas note that I have changed the scaling in storage for the parameters - int16_t cd rather than raw degrees.
That means any vehicles flying this will have... problems... with the relevant mission item until the mission is re-uploaded and thus stored in the new format.

I'm wondering why you would do this @peterbarker, aren't we trying to get away from cd?

This is for storage in eeprom, a resource-constrained environment. You get 12 bytes (roughly speaking) to store everything you want to store about the mission item.

I'm changing this because storing your roll offset in int8_t means you get (roughly) +/- 128 degrees of roll offset where a camera can actually roll +/- 180 degrees. So it's a range issue. I did pitch for consistency.

[peterbarker]

This looks pretty good. my only request, as mentioned on the dev call, is to double check that any new methods or members added appear close to related methods and members.

I've looked through and everything seems to be in reasonable shape there.

[rmackay9]

Hi @peterbarker,

Txs for that extra checking. Has this been tested with an actual ground station? even Mavproxy?

[peterbarker]

Txs for that extra checking. Has this been tested with an actual ground station? even Mavproxy?

Only QGC!

The interface elements required are quite substantial, so it would be a bit of a project to add it to either of MP or MAVProxy, even in the age of AI.

fixed