Copter: fix RTL_SPEED from affecting vehicle speed in Auto

[rmackay9]

This is an alternative solution to this PR which solves this issue.

The issue is that with the proper sequence of mode changes, the RTL_SPEED could be used in Auto mode. This is how to reproduce the issue:

• set RTL_SPEED = 200
• create a mission with at least one waypoint
• arm the vehicle and switch to Auto mode (the vehicle will fly at WPNAV_SPEED)
• switch to RTL mode (the vehicle will slow to the RTL_SPEED)
• switch back to Auto, the vehicle will incorrectly fly at RTL_SPEED instead of WPNAV_SPEED

The PR resolves the issue by

• AC_WPNav::set_speed_xy is changed so it does not update the _wp_speed_cms variable (which is the WPNAV_SPEED parameter). Instead the target speed is only passed to AC_PosControl::set_max_speed_xy.
• other places in AC_WPNav that used _wp_speed_cms (like the leash calculations) instead go to AC_PosControl to get the current target speed. Essentially AC_PosControl is trusted to hold the current target horizontal speed
• Similar changes are made to AC_WPNav::set_speed_z() that suffer a similar problem in which a DO_SET_SPEED command (either sent within a COMMAND_LONG or as part of a mission) permanently affected the WPNAV_SPEED_UP and WPNAV_SPEED_DN which meant all autonomous modes would fly at the new vertical speed (the speeds should be reset to their defaults when the mode is changed)

These changes have been tested in SITL. For the horizontal test, a mission was created like below

The vehicle was flown in Auto, switched to RTL, then back to Auto. In master we see the vehicle flies at 2m/s the 2nd time it enters Auto (it should fly at 8m/s)

Below is the after view and we can see the vehicle flies at full speed after entering Auto the 2nd time.

---

To test the climb rate issue a vertical mission was created in SITL like below:

Below are before and after pics of the climb rate:

[khancyr]

Tested successfully on SITL and approved!

[rmackay9]

@khancyr, great, thanks very much! I'll give it another 12hrs in case any one else wants to comment and then I'll merge. txs again.

[peterbarker]

I've pushed an autotest onto this which fails on master but passes here.

[rmackay9]

@peterbarker, cool, thanks!

[rmackay9]

.. hmm.. it seems to keep failing travis but it's not clear to me from looking at the travis output why it's failing..

[peterbarker]

On Wed, 30 Jan 2019, Randy Mackay wrote: .. hmm.. it seems to keep failing travis but it's not clear to me from looking at the travis output why it's failing..

Well, that would be because I'm an idiot: Unable to load /home/travis/build/ArduPilot/ardupilot/Tools/autotest/copter_rtl_speed.txt I've amended the commit and force-pushed.

[rmackay9]

@peterbarker, thanks!