From 7dd1eae37779e1ed6b007385d6141ffbea5de831 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Tue, 28 Nov 2023 20:36:19 +0900 Subject: [PATCH 1/4] Copter: minor comment fix --- ArduCopter/mode.h | 1 + 1 file changed, 1 insertion(+) diff --git a/ArduCopter/mode.h b/ArduCopter/mode.h index fb787077b9059..46f3ae711203a 100644 --- a/ArduCopter/mode.h +++ b/ArduCopter/mode.h @@ -679,6 +679,7 @@ class ModeAuto : public Mode { int32_t condition_value; // used in condition commands (eg delay, change alt, etc.) uint32_t condition_start; + // Land within Auto state enum class State { FlyToLocation = 0, Descending = 1 From 30af603ff7399597a9112ed96d9f51dcdef198e2 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Tue, 28 Nov 2023 20:36:08 +0900 Subject: [PATCH 2/4] Copter: fix do-change-speed received during takeoff --- ArduCopter/mode.h | 7 +++++++ ArduCopter/mode_auto.cpp | 20 +++++++++++++++++++- 2 files changed, 26 insertions(+), 1 deletion(-) diff --git a/ArduCopter/mode.h b/ArduCopter/mode.h index 46f3ae711203a..afb348c1dc652 100644 --- a/ArduCopter/mode.h +++ b/ArduCopter/mode.h @@ -714,6 +714,13 @@ class ModeAuto : public Mode { float climb_rate; // climb rate in m/s. provided by mission command uint32_t start_ms; // system time that nav attitude time command was received (used for timeout) } nav_attitude_time; + + // desired speeds + struct { + float xy; // desired speed horizontally in m/s. 0 if unset + float up; // desired speed upwards in m/s. 0 if unset + float down; // desired speed downwards in m/s. 0 if unset + } desired_speed_override; }; #if AUTOTUNE_ENABLED == ENABLED diff --git a/ArduCopter/mode_auto.cpp b/ArduCopter/mode_auto.cpp index 74bd78a15db91..c33c9d98d7ecb 100644 --- a/ArduCopter/mode_auto.cpp +++ b/ArduCopter/mode_auto.cpp @@ -41,6 +41,9 @@ bool ModeAuto::init(bool ignore_checks) // initialise waypoint and spline controller wp_nav->wp_and_spline_init(); + // initialise desired speed overrides + desired_speed_override = {0, 0, 0}; + // set flag to start mission waiting_to_start = true; @@ -370,7 +373,16 @@ bool ModeAuto::wp_start(const Location& dest_loc) stopping_point = takeoff_complete_pos.tofloat(); } } - wp_nav->wp_and_spline_init(0, stopping_point); + float des_speed_xy_cm = is_positive(desired_speed_override.xy) ? (desired_speed_override.xy * 100) : 0; + wp_nav->wp_and_spline_init(des_speed_xy_cm, stopping_point); + + // override speeds up and down if necessary + if (is_positive(desired_speed_override.up)) { + wp_nav->set_speed_up(desired_speed_override.up * 100.0); + } + if (is_positive(desired_speed_override.down)) { + wp_nav->set_speed_down(desired_speed_override.down * 100.0); + } } if (!wp_nav->set_wp_destination_loc(dest_loc)) { @@ -585,18 +597,21 @@ bool ModeAuto::use_pilot_yaw(void) const bool ModeAuto::set_speed_xy(float speed_xy_cms) { copter.wp_nav->set_speed_xy(speed_xy_cms); + desired_speed_override.xy = speed_xy_cms * 0.01; return true; } bool ModeAuto::set_speed_up(float speed_up_cms) { copter.wp_nav->set_speed_up(speed_up_cms); + desired_speed_override.up = speed_up_cms * 0.01; return true; } bool ModeAuto::set_speed_down(float speed_down_cms) { copter.wp_nav->set_speed_down(speed_down_cms); + desired_speed_override.down = speed_down_cms * 0.01; return true; } @@ -1855,10 +1870,13 @@ void ModeAuto::do_change_speed(const AP_Mission::Mission_Command& cmd) if (cmd.content.speed.target_ms > 0) { if (cmd.content.speed.speed_type == 2) { copter.wp_nav->set_speed_up(cmd.content.speed.target_ms * 100.0f); + desired_speed_override.up = cmd.content.speed.target_ms; } else if (cmd.content.speed.speed_type == 3) { copter.wp_nav->set_speed_down(cmd.content.speed.target_ms * 100.0f); + desired_speed_override.down = cmd.content.speed.target_ms; } else { copter.wp_nav->set_speed_xy(cmd.content.speed.target_ms * 100.0f); + desired_speed_override.xy = cmd.content.speed.target_ms; } } } From 3928e25c950c9d1dc3196c06441583322c2c1625 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Tue, 5 Dec 2023 08:52:52 +0900 Subject: [PATCH 3/4] Copter: set-desired-speed applied to flightmode --- ArduCopter/Copter.cpp | 8 +------- 1 file changed, 1 insertion(+), 7 deletions(-) diff --git a/ArduCopter/Copter.cpp b/ArduCopter/Copter.cpp index f5242feb0d95d..c121865134239 100644 --- a/ArduCopter/Copter.cpp +++ b/ArduCopter/Copter.cpp @@ -404,13 +404,7 @@ bool Copter::set_circle_rate(float rate_dps) // set desired speed (m/s). Used for scripting. bool Copter::set_desired_speed(float speed) { - // exit if vehicle is not in auto mode - if (!flightmode->is_autopilot()) { - return false; - } - - wp_nav->set_speed_xy(speed * 100.0f); - return true; + return flightmode->set_speed_xy(speed * 100.0f); } #if MODE_AUTO_ENABLED == ENABLED From b8fc7fb97fa940862f857498047e97b2cd368bd2 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Tue, 5 Dec 2023 19:43:19 +0900 Subject: [PATCH 4/4] Tools: copter do-change-speed autotest checks takeoff --- .../DO_CHANGE_SPEED/mission.txt | 23 ++++++++++--------- Tools/autotest/arducopter.py | 13 ++++++++--- 2 files changed, 22 insertions(+), 14 deletions(-) diff --git a/Tools/autotest/ArduCopter_Tests/DO_CHANGE_SPEED/mission.txt b/Tools/autotest/ArduCopter_Tests/DO_CHANGE_SPEED/mission.txt index f4ef0ef2a1f1f..846be46dd5c00 100644 --- a/Tools/autotest/ArduCopter_Tests/DO_CHANGE_SPEED/mission.txt +++ b/Tools/autotest/ArduCopter_Tests/DO_CHANGE_SPEED/mission.txt @@ -1,11 +1,12 @@ -QGC WPL 110 -0 0 0 16 0.000000 0.000000 0.000000 0.000000 -35.363262 149.165237 584.090027 1 -1 0 3 22 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 20.000000 1 -2 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.362656 149.165072 20.000000 1 -3 0 0 178 1.000000 15.000000 0.000000 0.000000 0.000000 0.000000 0.000000 1 -4 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.360174 149.167908 14.000000 1 -5 0 0 178 1.000000 10.000000 0.000000 0.000000 0.000000 0.000000 0.000000 1 -6 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.363296 149.169244 18.000000 1 -7 0 0 178 1.000000 16.000000 0.000000 0.000000 0.000000 0.000000 0.000000 1 -8 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.364115 149.165747 20.000000 1 -9 0 0 20 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 1 +QGC WPL 110 +0 1 0 16 0 0 0 0 -35.3632620 149.1652370 584.090000 1 +1 0 3 22 0.00000000 0.00000000 0.00000000 0.00000000 0.00000000 0.00000000 20.000000 1 +2 0 3 178 0.00000000 4.00000000 0.00000000 0.00000000 0.00000000 0.00000000 0.000000 1 +3 0 3 16 0.00000000 0.00000000 0.00000000 0.00000000 -35.36265600 149.16507200 20.000000 1 +4 0 0 178 1.00000000 15.00000000 0.00000000 0.00000000 0.00000000 0.00000000 0.000000 1 +5 0 3 16 0.00000000 0.00000000 0.00000000 0.00000000 -35.36017400 149.16790800 14.000000 1 +6 0 0 178 1.00000000 10.00000000 0.00000000 0.00000000 0.00000000 0.00000000 0.000000 1 +7 0 3 16 0.00000000 0.00000000 0.00000000 0.00000000 -35.36329600 149.16924400 18.000000 1 +8 0 0 178 1.00000000 16.00000000 0.00000000 0.00000000 0.00000000 0.00000000 0.000000 1 +9 0 3 16 0.00000000 0.00000000 0.00000000 0.00000000 -35.36411500 149.16574700 20.000000 1 +10 0 0 20 0.00000000 0.00000000 0.00000000 0.00000000 0.00000000 0.00000000 0.000000 1 diff --git a/Tools/autotest/arducopter.py b/Tools/autotest/arducopter.py index f8afa254b4cec..449b09064e06a 100644 --- a/Tools/autotest/arducopter.py +++ b/Tools/autotest/arducopter.py @@ -9505,14 +9505,21 @@ def DO_CHANGE_SPEED(self): self.wait_ready_to_arm() self.arm_vehicle() - self.wait_current_waypoint(2) + self.wait_current_waypoint(1) + self.wait_groundspeed( + 3.5, 4.5, + minimum_duration=5, + timeout=60, + ) + + self.wait_current_waypoint(3) self.wait_groundspeed( 14.5, 15.5, minimum_duration=10, timeout=60, ) - self.wait_current_waypoint(4) + self.wait_current_waypoint(5) self.wait_groundspeed( 9.5, 11.5, minimum_duration=10, @@ -9520,7 +9527,7 @@ def DO_CHANGE_SPEED(self): ) self.set_parameter("ANGLE_MAX", 6000) - self.wait_current_waypoint(6) + self.wait_current_waypoint(7) self.wait_groundspeed( 15.5, 16.5, minimum_duration=10,