From 1aedb709e0affdd191df978c4bd5f03909cad579 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 25 Nov 2022 12:05:12 +1100 Subject: [PATCH] AP_Scripting: added real stall turn reverses path at low throttle --- .../Aerobatics/FixedWing/plane_aerobatics.lua | 230 ++++++++++++++++-- 1 file changed, 211 insertions(+), 19 deletions(-) diff --git a/libraries/AP_Scripting/applets/Aerobatics/FixedWing/plane_aerobatics.lua b/libraries/AP_Scripting/applets/Aerobatics/FixedWing/plane_aerobatics.lua index bf73413952..eba107b396 100644 --- a/libraries/AP_Scripting/applets/Aerobatics/FixedWing/plane_aerobatics.lua +++ b/libraries/AP_Scripting/applets/Aerobatics/FixedWing/plane_aerobatics.lua @@ -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