diff --git a/libraries/AP_Scripting/examples/copter-wall-climber.lua b/libraries/AP_Scripting/examples/copter-wall-climber.lua index f1a2776aec..ecfc37f4b7 100644 --- a/libraries/AP_Scripting/examples/copter-wall-climber.lua +++ b/libraries/AP_Scripting/examples/copter-wall-climber.lua @@ -58,6 +58,11 @@ local roll_speed = 0 -- target horizontal roll speed in m/s local pitch_speed = 0 -- target horizontal pitch speed in m/s local wall_dist_target = 5 -- target distance to wall +-- get the maximum speed in order to decelerate to zero within the given distance +function get_speed_max(distance, accel_max) + return math.sqrt(2.0 * math.max(0, distance) * accel_max) +end + -- the main update function that uses the takeoff and velocity controllers to fly a rough square pattern function update() @@ -152,8 +157,9 @@ function update() climb_total = climb_total + climb_chg -- determine if we should pause - if (math.abs(climb_interval_total) >= climb_stop_interval) then - gcs:send_text(0, "WallClimb: pausing at " .. tostring(climb_total) .. "m") + local dist_to_interval = climb_stop_interval - math.abs(climb_interval_total) + if (dist_to_interval <= 0) then + gcs:send_text(0, "WallClimb: pausing at " .. string.format("%3.1f", climb_total) .. "m") climb_pause_counter = climb_pause_counter_max climb_interval_total = 0 @@ -169,6 +175,12 @@ function update() -- default target climb rate to lane climb rate local climb_rate_target = lane_climb_rate + -- limit target speed so vehicle can stop before next interval + -- speed limit is always at least 0.1m/s so copter doesn't get stuck near interval + local speed_max = math.max(get_speed_max(dist_to_interval, climb_accel_max), 0.1) + climb_rate_target = math.max(climb_rate_target, -speed_max) + climb_rate_target = math.min(climb_rate_target, speed_max) + -- if paused set target climb rate to zero if (climb_pause_counter > 0) then climb_pause_counter = climb_pause_counter - 1