From 8e32c1645e9906ce6b0b54e2358b51e64b0c4d7f Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Wed, 19 Apr 2023 21:28:51 +0900 Subject: [PATCH] Copter: RTL accepts do-change-speed commands --- ArduCopter/mode.h | 4 ++++ ArduCopter/mode_rtl.cpp | 18 ++++++++++++++++++ 2 files changed, 22 insertions(+) diff --git a/ArduCopter/mode.h b/ArduCopter/mode.h index 6a28a92195..cf446eae11 100644 --- a/ArduCopter/mode.h +++ b/ArduCopter/mode.h @@ -1319,6 +1319,10 @@ public: bool use_pilot_yaw() const override; + bool set_speed_xy(float speed_xy_cms) override; + bool set_speed_up(float speed_up_cms) override; + bool set_speed_down(float speed_down_cms) override; + // RTL states enum class SubMode : uint8_t { STARTING, diff --git a/ArduCopter/mode_rtl.cpp b/ArduCopter/mode_rtl.cpp index fc08b39b19..baa806768b 100644 --- a/ArduCopter/mode_rtl.cpp +++ b/ArduCopter/mode_rtl.cpp @@ -554,4 +554,22 @@ bool ModeRTL::use_pilot_yaw(void) const return allow_yaw_option || land_repositioning || final_landing; } +bool ModeRTL::set_speed_xy(float speed_xy_cms) +{ + copter.wp_nav->set_speed_xy(speed_xy_cms); + return true; +} + +bool ModeRTL::set_speed_up(float speed_up_cms) +{ + copter.wp_nav->set_speed_up(speed_up_cms); + return true; +} + +bool ModeRTL::set_speed_down(float speed_down_cms) +{ + copter.wp_nav->set_speed_down(speed_down_cms); + return true; +} + #endif