Add yaw-relative-to-heading function to Copter Guided mode

[peterbarker]

Also adds an option bit that preserves yaw as the vehicle is moved around in guided mode, which should arguably be the default.

As it stands in master you can't point to an absolute yaw angle and then move the vehicle using reposition - we reset the yaw all the time. If the option is set we only initialise if we are entering a different submode.

[rmackay9]

I think you can set WPNAV_YAW_BEHAVE = 0 and it will stop turning with new position commands.

[peterbarker]

Closes #2427 , kind of. From the original description it seems we didn't reset, now we always reset to lookahead when repositioning the vehicle. This PR makes the always-reset behaviour optional (with an option bit) and then gives you the facility to point forward using param4 of condition-yaw (i.e. look-ahead with offset 0)

I

[peterbarker]

WPNAV_YAW_BEHAVE

That parameter sets the default auto_yaw behaviour. But the bulk of the patches here stop us calling set_yaw_state, which we currently do any time we receive a target. That call passes in new yaw targets and mode which are unconditionally acted upon AFAICS. I just tried retrofitting the autotest and it shows the targets changing to a fixed absolute yaw rather than remaining in look-ahead mode.

AT-0007.9: Heading=48.00 (got 45.000000 +- 2.500000) (maintain=4.4/10.0)
AT-0008.0: Heading=48.00 (got 45.000000 +- 2.500000) (maintain=6.0/10.0)
AT-0008.0: Heading=48.00 (got 45.000000 +- 2.500000) (maintain=7.0/10.0)
AT-0008.1: Heading=48.00 (got 45.000000 +- 2.500000) (maintain=8.2/10.0)
AT-0008.1: Heading=48.00 (got 45.000000 +- 2.500000) (maintain=9.2/10.0)
AT-0008.1: Attained Heading=48.020833333333336
AT-0008.1: Heading east, checking yaw remains 45-degrees-to-heading
AT-0008.2: Waiting for Heading=135.00 with accuracy 2.50
AT-0008.2: Heading=47.00 (want 135.000000 +- 2.500000)
AT-0008.2: Heading=48.00 (want 135.000000 +- 2.500000)
AT-0008.3: Heading=48.00 (want 135.000000 +- 2.500000)
AT-0008.3: Heading=47.00 (want 135.000000 +- 2.500000)

[rmackay9]

@peterbarker, OK thanks. I think the bug is that we should not be changing the yaw mode when we receive a new position target. the only time we should change it is if that command also includes a yaw angle or yaw rate. With the mavlink messages that I"m familiar with (which doesn't really include DO_REPOSITION) there are ignore flags that should be used to know if the caller has provided a yaw angle or yaw rate.

[peterbarker]

@peterbarker, OK thanks. I think the bug is that we should not be changing the yaw mode when we receive a new position target. the only time we should change it is if that command also includes a yaw angle or yaw rate. With the mavlink messages that I"m familiar with (which doesn't really include DO_REPOSITION) there are ignore flags that should be used to know if the caller has provided a yaw angle or yaw rate.

Yes, I think that's entirely correct.

The complexity in this PR is that I've tried to preserve the existing behaviour across an update with the option bit. The workflow we might break is user uses go-to to go to a location, then raws the vehicle around for a while looking at things, then uses "fly to" to fly somewhere else and is expecting the vehicle to yaw-to-cog.

If you're happy to change behaviour we can simplify this PR - this was just an attempt at a conservative approach.

[peterbarker]

... we could also invert the bit, meaning you can get the old behaviour back if you want, and change the default behaviour to preserve your current yaw mode.

[burgeruser]

I believe you can directly set the desired heading within the ground station software like Mission Planner. On the map page, there's usually an option to "Fly to Here," where you can specify the heading angle. For instance, 0 degrees would typically represent true north in the navigation settings. Users can input their intended direction for the drone to fly.

[rmackay9]

Hi @peterbarker,

I'm happy with a change in behaviour..

[peterbarker]

I believe you can directly set the desired heading within the ground station software like Mission Planner. On the map page, there's usually an option to "Fly to Here," where you can specify the heading angle. For instance, 0 degrees would typically represent true north in the navigation settings. Users can input their intended direction for the drone to fly.

Quite right. But this allows relative-to-current-heading, so if you want to see where you've come from from rather than where you're going, you can do that.

More to the point, we're debating whether to change the current behaviour so that yaw is not reset when a new fly-to command is received.

[peterbarker]

I believe you can directly set the desired heading within the ground station software like Mission Planner. On the map page, there's usually an option to "Fly to Here," where you can specify the heading angle. For instance, 0 degrees would typically represent true north in the navigation settings. Users can input their intended direction for the drone to fly.

Cool. Interface changes along with this PR would allow the user to specify that heading relative to the vehicle's course-over-ground, rather than a magnetic heading.