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

AP_Scripting: copter fast descent gets improved slowdown

Browse Source
This commit is contained in:
Randy Mackay 2022-03-14 11:47:37 +09:00
parent 53c1c235b8
commit fc668b4bca
1 changed files with 21 additions and 18 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

39
libraries/AP_Scripting/examples/copter-fast-descent.lua
View File

@ -136,25 +136,28 @@ function update()
speed_xy = math.max(speed_xy - (accel_xy * dt), 0) -- decelerate horizontal speed to zero
speed_z = math.max(speed_z - (accel_z * dt), 0) -- decelerate vertical speed to zero
else
-- determine if below slowdown point
local slowdown = false
local stopping_distance_z = 0.5 * speed_z_max:get() * speed_z_max:get() / accel_z
if (rel_pos_home_NED) then
if (-rel_pos_home_NED:z() <= alt_above_home_min:get() + stopping_distance_z) then
slowdown = true
end
-- calculate conversion between alt-above-home and alt-above-ekf-origin
local home_alt_above_origin = 0
local home = ahrs:get_home()
local ekf_origin = ahrs:get_origin()
if home and ekf_origin then
local dist_NED = home:get_distance_NED(ekf_origin)
home_alt_above_origin = dist_NED:z()
end
if (slowdown) then
-- slow down vertical and then horizontal speed
speed_z = math.max(speed_z - (accel_z * dt), math.min(speed_z_slowdown, speed_z_max:get())) -- decelerate to 0.1m/s vertically
if speed_z <= speed_z_slowdown then
speed_xy = math.max(speed_xy - (accel_xy * dt), 0)
end
-- calculate target speeds
local target_dist_to_alt_min = -target_alt_D - home_alt_above_origin - alt_above_home_min:get() -- alt target's distance to alt_min
if (target_dist_to_alt_min > 0) then
local speed_z_limit = speed_z_max:get()
speed_z_limit = math.min(speed_z_limit, math.sqrt(2.0 * target_dist_to_alt_min * accel_z)) -- limit speed so vehicle can stop at ALT_MIN
speed_z_limit = math.max(speed_z_limit, speed_z_slowdown) -- vertical speed should never be less than 0.1 m/s when above ALT_MIN
speed_z = math.min(speed_z + (accel_z * dt), speed_z_limit)
speed_xy = math.min(speed_xy + (accel_xy * dt), speed_xy_max:get()) -- accelerate horizontal speed to maximum
else
-- normal speed
speed_xy = math.min(speed_xy + (accel_xy * dt), speed_xy_max:get()) -- accelerate horizontal speed to max
speed_z = math.min(speed_z + (accel_z * dt), speed_z_max:get()) -- accelerate to max descent rate
-- below alt min so decelerate target speeds to zero
speed_xy = math.max(speed_xy - (accel_xy * dt), 0) -- decelerate horizontal speed to zero
speed_z = math.max(speed_z - (accel_z * dt), 0) -- decelerate vertical speed to zero
end
end
@ -205,9 +208,9 @@ function update()
-- send targets to vehicle with yaw target
vehicle:set_target_posvelaccel_NED(target_pos, target_vel, target_accel, true, target_yaw_deg, false, 0, false)
-- advance to stage 2 when below target altitude
-- advance to stage 2 when below target altitude and target speeds are zero
if (rel_pos_home_NED) then
if (-rel_pos_home_NED:z() <= alt_above_home_min:get()) then
if (-rel_pos_home_NED:z() <= alt_above_home_min:get() and (speed_xy==0) and (speed_z==0)) then
stage = stage + 1
end
if (update_user) then