Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 14:38:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_Scripting: copter-wall-climber accel limiting

Browse Source 
reduces climb rate slowly so it can stop before next interval
also fixup formatting of altitude output to user
This commit is contained in:
Randy Mackay 2020-10-14 15:06:34 +09:00
parent 2ab860b8b7
commit eab7c9520c
1 changed files with 14 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
libraries/AP_Scripting/examples/copter-wall-climber.lua
View File

@ -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 pitch_speed = 0 -- target horizontal pitch speed in m/s
local wall_dist_target = 5 -- target distance to wall 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 -- the main update function that uses the takeoff and velocity controllers to fly a rough square pattern
function update() function update()
@ -152,8 +157,9 @@ function update()
climb_total = climb_total + climb_chg climb_total = climb_total + climb_chg
-- determine if we should pause -- determine if we should pause
if (math.abs(climb_interval_total) >= climb_stop_interval) then local dist_to_interval = climb_stop_interval - math.abs(climb_interval_total)
gcs:send_text(0, "WallClimb: pausing at " .. tostring(climb_total) .. "m") 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_pause_counter = climb_pause_counter_max
climb_interval_total = 0 climb_interval_total = 0
@ -169,6 +175,12 @@ function update()
-- default target climb rate to lane climb rate -- default target climb rate to lane climb rate
local climb_rate_target = 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 paused set target climb rate to zero
if (climb_pause_counter > 0) then if (climb_pause_counter > 0) then
climb_pause_counter = climb_pause_counter - 1 climb_pause_counter = climb_pause_counter - 1