Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-21 15:23:57 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_Scripting: added real stall turn

Browse Source 
reverses path at low throttle
This commit is contained in:
Andrew Tridgell 2022-11-25 12:05:12 +11:00
parent e56544515a
commit 1aedb709e0
1 changed files with 211 additions and 19 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

230
libraries/AP_Scripting/applets/Aerobatics/FixedWing/plane_aerobatics.lua
View File

@ -37,6 +37,8 @@ AEROM_YAW_ACCEL = bind_add_param('YAW_ACCEL', 19, 1500)
AEROM_LKAHD = bind_add_param('LKAHD', 20, 0.5)
AEROM_PATH_SCALE = bind_add_param('PATH_SCALE', 21, 1.0)
AEROM_BOX_WIDTH = bind_add_param('BOX_WIDTH', 22, 400)
AEROM_STALL_THR = bind_add_param('STALL_THR', 23, 40)
AEROM_STALL_PIT = bind_add_param('STALL_PIT', 24, -20)
-- cope with old param values
if AEROM_ANG_ACCEL:get() < 100 and AEROM_ANG_ACCEL:get() > 0 then
@ -47,6 +49,7 @@ if AEROM_ANG_TC:get() > 1.0 then
end
ACRO_ROLL_RATE = Parameter("ACRO_ROLL_RATE")
ACRO_YAW_RATE = Parameter('ACRO_YAW_RATE')
ARSPD_FBW_MIN = Parameter("ARSPD_FBW_MIN")
SCALING_SPEED = Parameter("SCALING_SPEED")
@ -72,6 +75,15 @@ local TRIK_SEL_FN = nil
local TRIK_ACT_FN = nil
local TRIK_COUNT = nil
--[[
find our rudder channel for stall turns
--]]
local K_RUDDER = 21
local rudder_chan = SRV_Channels:find_channel(K_RUDDER)
local RUDD_REVERSED = Parameter(string.format("SERVO%u_REVERSED", rudder_chan+1))
local RUDD_MIN = Parameter(string.format("SERVO%u_MIN", rudder_chan+1))
local RUDD_MAX = Parameter(string.format("SERVO%u_MAX", rudder_chan+1))
local function TrickDef(id, arg1, arg2, arg3, arg4)
local self = {}
self.id = id
@ -120,7 +132,7 @@ local NAV_SCRIPT_TIME = 42702
local MODE_AUTO = 10
local LOOP_RATE = 20
local LOOP_RATE = 40
local DO_JUMP = 177
local k_throttle = 70
local NAME_FLOAT_RATE = 2
@ -528,6 +540,14 @@ function PathComponent()
return self.thr_boost or false
end
function self.get_rate_override(t)
return self.rate_override
end
function self.get_pos_correction(t)
return self.pos_corr or 1.0
end
--[[ get the extents of the path on x axis. Can be overridden by a
more efficient and accurate path specific function
--]]
@ -593,6 +613,26 @@ function path_straight(_distance)
return self
end
--[[
path component that does a straight line then reverses direction
--]]
function path_reverse(_distance)
local self = PathComponent()
self.name = "path_straight"
local distance = _distance
function self.get_pos(t)
if t < 0.5 then
return makeVector3f(distance*t*2, 0, 0)
else
return makeVector3f(distance-(distance*(t-0.5)*2), 0, 0)
end
end
function self.get_length()
return distance*2
end
return self
end
--[[
path component that aligns to within the aerobatic box
--]]
@ -800,6 +840,12 @@ function Path(_path_component, _roll_component)
function self.get_throttle_boost(t)
return self.thr_boost or false
end
function self.get_rate_override(t)
return self.rate_override
end
function self.get_pos_correction(t)
return self.pos_corr or 1.0
end
function self.set_next_extents(extents, start_pos, start_orientation)
path_component.set_next_extents(extents, start_pos, start_orientation)
end
@ -855,6 +901,8 @@ function path_composer(_name, _subpaths)
cache_sp = subpaths[i][1](args[1], args[2], args[3], args[4], start_pos[i], start_orientation[i])
message = subpaths[i].message
cache_sp.thr_boost = subpaths[i].thr_boost
cache_sp.pos_corr = subpaths[i].pos_corr
cache_sp.rate_override = subpaths[i].rate_override
end
return cache_sp
end
@ -994,12 +1042,15 @@ function path_composer(_name, _subpaths)
function self.get_throttle_boost(t)
local subpath_t, i = self.get_subpath_t(t)
local sp = self.subpath(i)
if sp.thr_boost ~= nil then
return sp.thr_boost
end
return sp.get_throttle_boost(t)
return sp.thr_boost or sp.get_throttle_boost(subpath_t)
end
function self.get_pos_correction(t)
local subpath_t, i = self.get_subpath_t(t)
local sp = self.subpath(i)
return sp.pos_corr or sp.get_pos_correction(subpath_t)
end
local extents = nil
function self.get_extents_x()
if extents ~= nil then
@ -1008,6 +1059,27 @@ function path_composer(_name, _subpaths)
extents = get_extents_x(self.get_pos)
return extents
end
function self.get_rate_override(t)
local subpath_t, i = self.get_subpath_t(t)
local sp = self.subpath(i)
if sp.rate_override ~= nil then
return sp.rate_override
end
return sp.get_rate_override(subpath_t)
end
--[[
get the time that the next segment starts
--]]
function self.get_next_segment_start(t)
local subpath_t, i = self.get_subpath_t(t)
local sp = self.subpath(i)
if sp.get_next_segment_start ~= nil then
return start_time[i] + (sp.get_next_segment_start(subpath_t) * (end_time[i] - start_time[i]))
end
return end_time[i]
end
return self
end
@ -1030,7 +1102,9 @@ function make_paths(name, paths)
if paths[i].set_orient then
p[i].set_orient = paths[i].set_orient
end
p[i].rate_override = paths[i].rate_override
p[i].thr_boost = paths[i].thr_boost
p[i].pos_corr = paths[i].pos_corr
end
return path_composer(name, p)
end
@ -1283,6 +1357,113 @@ function figure_eight(r, bank_angle, arg3, arg4)
end
--[[
perform a rudder over maneuver
--]]
function rudder_over(_direction, _min_speed)
local self = {}
local direction = _direction
local min_speed = _min_speed
local reached_speed = false
local pitch1_done = false
local pitch2_done = false
local target_q = nil
local last_t = nil
--[[
the update() method is called during the rudder over, it
should return true when the maneuver is completed
--]]
function self.update(path, t, target_speed)
if pitch2_done then
-- we're all done
return true
end
local ahrs_quat = ahrs:get_quaternion()
local now = millis():tofloat() * 0.001
local pitch_threshold = 60.0
if target_q == nil then
target_q = ahrs_quat
last_t = now
end
local dt = now - last_t
last_t = now
local error_quat = ahrs_quat:inverse() * target_q
local rate_rads = Vector3f()
error_quat:to_axis_angle(rate_rads)
local tc = ROLL_CORR_TC:get()
local rate_dps = rate_rads:scale(math.deg(1)/tc)
-- use user set throttle for achieving the stall
local throttle = AEROM_STALL_THR:get()
local pitch_deg = math.deg(ahrs:get_pitch())
if not pitch1_done and pitch_deg < 45 then
pitch1_done = true
end
if pitch1_done then
-- when we are half way over cut the throttle to minimum
throttle = AEROM_THR_MIN:get()
end
vehicle:set_target_throttle_rate_rpy(throttle, rate_dps:x(), rate_dps:y(), rate_dps:z())
local current_speed = ahrs:get_velocity_NED():length()
log_pose('POSM', ahrs:get_relative_position_NED_origin(), ahrs:get_quaternion())
log_pose('POST', ahrs:get_relative_position_NED_origin(), target_q)
if not reached_speed and current_speed <= min_speed then
reached_speed = true
end
if not reached_speed then
return false
end
-- integrate desired attitude through yaw
local ahrs_gyro = ahrs:get_gyro()
local q_rate_rads = makeVector3f(0,0,ahrs_gyro:z())
local rotation = Quaternion()
rotation:from_angular_velocity(q_rate_rads, dt)
target_q = target_q * rotation
--[[
override rudder to maximum, basing PWM on the MIN/MAX of the channel
according to the desired direction
--]]
local rudd_pwm = nil
if direction * (RUDD_REVERSED:get()*2-1) < 0 then
rudd_pwm = RUDD_MAX:get()
else
rudd_pwm = RUDD_MIN:get()
end
SRV_Channels:set_output_pwm_chan_timeout(rudder_chan, rudd_pwm, math.floor(4*1000/LOOP_RATE))
-- see if we are nose down
if pitch_deg > AEROM_STALL_PIT:get() then
-- not done
return false
end
-- all done, update state
pitch_done2 = true
path_var.tangent = path_var.tangent:scale(-1)
path_var.path_t = path.get_next_segment_start(t)
path_var.accumulated_orientation_rel_ef = path_var.accumulated_orientation_rel_ef * qorient(0,0,180)
path_var.last_time = now
path_var.last_ang_rate_dps = ahrs_gyro:scale(math.deg(1))
-- cancel rudder override
SRV_Channels:set_output_pwm_chan_timeout(rudder_chan, rudd_pwm, 0)
return false
end
return self
end
--[[
stall turn is not really correct, as we don't fully stall. Needs to be
reworked
@ -1292,10 +1473,10 @@ function stall_turn(radius, height, direction, min_speed)
assert(h >= 0)
return make_paths("stall_turn", {
{ path_vertical_arc(radius, 90), roll_angle(0) },
{ path_straight(h), roll_angle(0), min_speed },
{ path_horizontal_arc(5*direction, 180), roll_angle(0), min_speed },
{ path_straight(h), roll_angle(0) },
{ path_vertical_arc(radius, 90), roll_angle(0) },
{ path_reverse(h/4), roll_angle(0), rate_override=rudder_over(direction,min_speed), set_orient=qorient(0,-90,0) },
{ path_straight(h), roll_angle(0), pos_corr=0.5 },
{ path_vertical_arc(-radius, 90), roll_angle(0), set_orient=qorient(0,0,180) },
})
end
@ -1974,6 +2155,8 @@ function rotate_path(path_f, t, orientation, offset)
local roll_correction = path_f.get_roll_correction(t)
local speed = path_f.get_speed(t)
local thr_boost = path_f.get_throttle_boost(t)
local pos_corr = path_f.get_pos_correction(t)
local rate_override = path_f.get_rate_override(t)
local point = quat_earth_to_body(orientation, point)
local scale = AEROM_PATH_SCALE:get()
@ -1988,7 +2171,7 @@ function rotate_path(path_f, t, orientation, offset)
point = quat_body_to_earth((orient * orient), point)
end
return point+offset, math.rad(angle+roll_correction), speed, thr_boost
return point+offset, math.rad(angle+roll_correction), speed, thr_boost, rate_override, pos_corr
end
--Given vec1, vec2, returns an (rotation axis, angle) tuple that rotates vec1 to be parallel to vec2
@ -2090,6 +2273,7 @@ function quat_projection_ground_plane(q)
return 2.0 * (q3q4 + q1q2)
end
path_var.count = 0
function do_path()
@ -2136,7 +2320,6 @@ function do_path()
speed_PI.reset()
path_var.accumulated_orientation_rel_ef = path_var.initial_ori
path_var.time_correction = 0.0
@ -2172,7 +2355,7 @@ function do_path()
path_var.last_time = now
local local_n_dt = actual_dt/path_var.total_time
local local_n_dt = (1.0/LOOP_RATE)/path_var.total_time
if path_var.path_t + local_n_dt > 1.0 then
-- all done
@ -2189,11 +2372,19 @@ function do_path()
local p0 = path_var.pos:copy()
local r0 = path_var.roll
local s0 = path_var.speed
local p1, r1, s1 = rotate_path(path, path_var.path_t + local_n_dt,
path_var.initial_ori, path_var.initial_ef_pos)
local p1, r1, s1, thr_boost, rate_override, pos_corr = rotate_path(path, path_var.path_t + local_n_dt,
path_var.initial_ori, path_var.initial_ef_pos)
local current_measured_pos_ef = ahrs_pos_NED:copy()
if rate_override ~= nil then
if not rate_override.update(path, path_var.path_t + local_n_dt, s1) then
-- not done yet
path_var.pos = current_measured_pos_ef
return true
end
end
--[[
get tangents to the path
--]]
@ -2229,7 +2420,6 @@ function do_path()
local p1, r1, s1, thr_boost = rotate_path(path,
constrain(path_var.path_t + path_t_delta, 0, 1),
path_var.initial_ori, path_var.initial_ef_pos)
local last_path_t = path_var.path_t
path_var.path_t = path_var.path_t + path_t_delta
@ -2258,10 +2448,9 @@ function do_path()
path_var.path_t = path_var.path_t + TIME_CORR_P:get() * path_err_t
-- get the path again with the corrected time
p1, r1, s1 = rotate_path(path,
constrain(path_var.path_t, 0, 1),
path_var.initial_ori, path_var.initial_ef_pos)
p1, r1, s1, thr_boost = rotate_path(path,
constrain(path_var.path_t, 0, 1),
path_var.initial_ori, path_var.initial_ef_pos)
-- recalculate the tangent to match the amount we advanced the path time
tangent2_ef = p1 - p0
@ -2295,6 +2484,9 @@ function do_path()
-- gains for error correction.
local acc_err_ef = B:scale(ERR_CORR_P:get()) + B_dot:scale(ERR_CORR_D:get())
-- scale by per-maneuver error correction scale factor
acc_err_ef = acc_err_ef:scale(pos_corr)
local acc_err_bf = quat_earth_to_body(ahrs_quat, acc_err_ef)
local TAS = constrain(ahrs:get_EAS2TAS()*airspeed_constrained, 3, 100)
@ -2669,7 +2861,7 @@ function load_trick(id)
end
function PathTask(fn, name, id, initial_yaw_deg, arg1, arg2, arg3, arg4)
self = {}
local self = {}
if type(fn) == "table" then
self.fn = fn
else