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 has less aggressive slowdown at end

Browse Source
This commit is contained in:
Randy Mackay 2022-02-24 15:30:57 +09:00
parent 3efebcf1e0
commit 10e69373b3
1 changed files with 24 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -13,6 +13,7 @@ local circle_radius_rate_max_ms = 1 -- radius expands at max of this many m/s
local circle_radius_accel_mss = 1 -- radius expansion speed accelerates at this many m/s/s local circle_radius_accel_mss = 1 -- radius expansion speed accelerates at this many m/s/s
local accel_xy = 1 -- horizontal acceleration in m/s^2 local accel_xy = 1 -- horizontal acceleration in m/s^2
local accel_z = 1 -- target vertical acceleration is 1m/s/s local accel_z = 1 -- target vertical acceleration is 1m/s/s
local speed_z_slowdown = 0.1 -- target vertical speed during final slowdown
-- timing and state machine variables -- timing and state machine variables
local stage = 0 -- stage of descent local stage = 0 -- stage of descent
@ -123,6 +124,9 @@ function update()
end end
elseif (stage == 1) then -- Stage1: descend elseif (stage == 1) then -- Stage1: descend
-- get current position
local rel_pos_home_NED = ahrs:get_relative_position_NED_home()
-- increase circle radius -- increase circle radius
circle_radius_rate_ms = math.min(circle_radius_rate_ms + (circle_radius_accel_mss * dt), circle_radius_rate_max_ms) -- accelerate radius expansion circle_radius_rate_ms = math.min(circle_radius_rate_ms + (circle_radius_accel_mss * dt), circle_radius_rate_max_ms) -- accelerate radius expansion
circle_radius = math.min(circle_radius + (circle_radius_rate_ms * dt), circle_radius_max:get()) -- increase radius circle_radius = math.min(circle_radius + (circle_radius_rate_ms * dt), circle_radius_max:get()) -- increase radius
@ -132,8 +136,26 @@ function update()
speed_xy = math.max(speed_xy - (accel_xy * dt), 0) -- decelerate horizontal speed 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 speed_z = math.max(speed_z - (accel_z * dt), 0) -- decelerate vertical speed to zero
else else
speed_xy = math.min(speed_xy + (accel_xy * dt), speed_xy_max:get()) -- accelerate horizontal speed to max -- determine if below slowdown point
speed_z = math.min(speed_z + (accel_z * dt), speed_z_max:get()) -- accelerate to max descent rate 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
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
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
end
end end
-- calculate angular velocity -- calculate angular velocity
@ -184,7 +206,6 @@ function update()
vehicle:set_target_posvelaccel_NED(target_pos, target_vel, target_accel, true, target_yaw_deg, false, 0, false) 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
local rel_pos_home_NED = ahrs:get_relative_position_NED_home()
if (rel_pos_home_NED) then 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()) then
stage = stage + 1 stage = stage + 1