Copter: accept do-change-speed during takeoff

[rmackay9]

This resolves issue #21080 by ensuring that DO_CHANGE_SPEED commands received during the takeoff phase of a mission are respected. This is done by caching any do-change-speed commands received (either from a mission command or via MAVLink in real-time) and then using those cached values when WPNav object is initialised. WPNav is re-initialised if it has ever become inactive.

This has been tested in SITL for xy, up and down using nearly vertical square (see below) with a do-change-speed command immediately after the TAKEOFF (the regular failure case)

Below are before-vs-after screen shots of the horizontal test.

A touch more testing is required to ensure that do-change-speed commands received via MAVLink also work but 99% sure they will.

[rmackay9]

rebased to hopefully get over the ap-periph CI failure

[IamPete1]

I think because this sets continually it will break the set_desired_speed call from lua. Now it will reset to the last do_change_speed on each new way-point. The fix would be to add a boolean flag to say that there is data pending and clear it after it has been used. You could also just reset desired_speed_override to zeros of course.

[IamPete1]

Or I guess add the setting of desired_speed_override in the scripting call. The problem is that it goes direct to wp_nav rather than via the mode set_speed_xy which would handle it correctly.

[rmackay9]

Hi @IamPete1,

Thanks for the review and feedback. If we've got callers directly calling wp_nav to change the speed then, yes, that's the problem. External callers shouldn't be going direct to wp_nav because then that would only work in some modes. I haven't immediately followed through the lua script set_desired_speed yet but from an initial glace it looked to be going to AP_Vehicle which I would have thought would call into the flight modes.

[rmackay9]

@IamPete1,

I've changed Copter's implementation of set-desired-speed so that it directly calls the active flight mode. I'll need to test this of course before we can merge it but hopefully this resolves the issue you mentioned.

Txs again for catching this.

[Ryanf55]

Request - add an autotest for this. Otherwise, LGTM.

[rmackay9]

@Ryanf55, @peterbarker I've slightly modified the existing DO_CHANGE_SPEED test so that it has a DO_CHANGE_SPEED command right after the takeoff. This test fails in master but in this PR it works.

I've also tested a Lua script that sets the desired speed in combination with Auto mode do-change-speed commaands and it all works as expected so I think this confirms that I've resolved @IamPete1's concerns.

Thanks!

[peterbarker]

Only looked at the test