From 581bf77b0dfb201aecd33b9130f9810b395907d0 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 23 Dec 2022 08:22:21 +1100 Subject: [PATCH] AP_Scripting: new rudder offset for aerobatics --- .../Aerobatics/FixedWing/plane_aerobatics.lua | 133 ++++++++++++------ 1 file changed, 91 insertions(+), 42 deletions(-) diff --git a/libraries/AP_Scripting/applets/Aerobatics/FixedWing/plane_aerobatics.lua b/libraries/AP_Scripting/applets/Aerobatics/FixedWing/plane_aerobatics.lua index 1b70d9d7ba..5f75c256e0 100644 --- a/libraries/AP_Scripting/applets/Aerobatics/FixedWing/plane_aerobatics.lua +++ b/libraries/AP_Scripting/applets/Aerobatics/FixedWing/plane_aerobatics.lua @@ -39,6 +39,8 @@ 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) AEROM_KE_TC = bind_add_param('KE_TC', 25, 0.5) +AEROM_KE_RUDD = bind_add_param('KE_RUDD', 26, 25) +AEROM_KE_RUDD_LK = bind_add_param('KE_RUDD_LK', 27, 0.25) -- cope with old param values if AEROM_ANG_ACCEL:get() < 100 and AEROM_ANG_ACCEL:get() > 0 then @@ -80,15 +82,6 @@ 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 @@ -1154,7 +1147,7 @@ function immelmann_turn(r, arg2, arg3, arg4) local rabs = math.abs(r) return make_paths("immelmann_turn", { { path_vertical_arc(r, 180), roll_angle(0) }, - { path_straight(rabs/2), roll_angle(180) }, + { path_straight(rabs), roll_angle(180) }, }) end @@ -1323,6 +1316,7 @@ function rudder_over(_direction, _min_speed) local initial_q = nil local last_t = nil local initial_z = nil + local desired_direction = nil --[[ the update() method is called during the rudder over, it @@ -1397,25 +1391,22 @@ function rudder_over(_direction, _min_speed) override rudder to maximum, basing PWM on the MIN/MAX of the channel according to the desired direction --]] - local rudd_pwm = nil - local desired_direction = direction - if desired_direction == 0 then - local c_y = get_ahrs_dcm_c_y() - if c_y > 0 then - desired_direction = 1 - else - desired_direction = -1 + if desired_direction == nil then + desired_direction = direction + if desired_direction == 0 then + local c_y = get_ahrs_dcm_c_y() + if c_y > 0 then + desired_direction = 1 + else + desired_direction = -1 + end end end - if desired_direction * (RUDD_REVERSED:get()*2-1) < 0 then - rudd_pwm = RUDD_MAX:get() - else - rudd_pwm = RUDD_MIN:get() - end if not pitch2_done then - SRV_Channels:set_output_pwm_chan_timeout(rudder_chan, rudd_pwm, math.floor(4*1000/LOOP_RATE)) + vehicle:set_rudder_offset(desired_direction * 100, false) + else + vehicle:set_rudder_offset(0, true) end - if not kick_started then return false end @@ -1453,9 +1444,6 @@ function rudder_over(_direction, _min_speed) -- ensure that the path will move fwd on the next step path_var.pos:z(path_var.pos:z()-10) - -- cancel rudder override - SRV_Channels:set_output_pwm_chan_timeout(rudder_chan, rudd_pwm, 0) - return false end @@ -1749,6 +1737,65 @@ function quat_projection_ground_plane(q) end +--[[ + calculate side slip and AOA correction + NOTE: currently unused +--]] +function calculate_side_slip_aoa_full(path_rate_bf_dps, ahrs_quat, airspeed_constrained, tv_unit, ahrs_velned, dt) + -- get our velocity along desired flight path + local vel_path_ef = tv_unit:scale(tv_unit:dot(ahrs_velned)) + local vel_path_bf = quat_earth_to_body(ahrs_quat, vel_path_ef) + + local path_rate_bf_rps = path_rate_bf_dps:scale(math.rad(1)) + + local manoeuvre_accel_bf = path_rate_bf_rps:cross(vel_path_bf) + local gravity_ef = makeVector3f(0, 0, GRAVITY_MSS) + local gravity_bf = quat_earth_to_body(ahrs_quat, gravity_ef) + + manoeuvre_accel_bf = manoeuvre_accel_bf - gravity_bf + + local manoeuvre_g_y = manoeuvre_accel_bf:y() / GRAVITY_MSS + + local airspeed_scaling = SCALING_SPEED:get()/airspeed_constrained + local sideslip_angle = (-manoeuvre_g_y) * math.rad(AEROM_KE_ANG:get()) * sq(airspeed_scaling) + + -- calculate low pass filtered sideslip angle and derivative + local tc = 0.2 + local alpha = dt / (dt + tc) + + local ss_angle_new = (1.0 - alpha) * path_var.ss_angle_filt + alpha * sideslip_angle + local ss_deriv = (ss_angle_new - path_var.ss_angle_filt) / dt + path_var.ss_angle_filt = ss_angle_new + path_var.ss_angle = sideslip_angle + + logger.write('AESS','SSA,SSAfil,SSder', 'fff', + math.deg(sideslip_angle), + math.deg(path_var.ss_angle_filt), + math.deg(ss_deriv)) + + return makeVector3f(0, 0, -ss_deriv):scale(math.deg(1)) +end + +--[[ + calculate rudder offset +--]] +function calculate_rudder_offset(ahrs_quat, ahrs_gyro, airspeed_constrained) + --[[ + look ahead for what our y projection will be at AEROM_KE_RUDD_LK + seconds forward in time + --]] + local qchange = Quaternion() + qchange:from_angular_velocity(ahrs_gyro, -AEROM_KE_RUDD_LK:get()) + local qnew = qchange * ahrs_quat + + local airspeed_scaling = SCALING_SPEED:get()/airspeed_constrained + local y_projection = get_quat_dcm_c_y(qnew:inverse()) + local rudder_ofs = -y_projection * AEROM_KE_RUDD:get() * sq(airspeed_scaling) + rudder_ofs = constrain(rudder_ofs, -100, 100) + + return rudder_ofs +end + path_var.count = 0 function do_path() @@ -1802,7 +1849,6 @@ function do_path() path_var.filtered_angular_velocity = Vector3f() path_var.last_time = now - 1.0/LOOP_RATE - path_var.ff_yaw_rate_rads = 0.0 path_var.last_ang_rate_dps = ahrs_gyro:scale(math.deg(1)) path_var.path_t = 0.0 @@ -1812,6 +1858,9 @@ function do_path() path_var.last_shift_xy = nil path_var.path_shift = Vector3f() + path_var.ss_angle = 0.0 + path_var.ss_angle_filt = 0.0 + -- get initial tangent local p1, r1 = rotate_path(path, path_var.path_t + 0.1/(path_var.total_time*LOOP_RATE), path_var.initial_ori, path_var.initial_ef_pos) @@ -2071,17 +2120,8 @@ function do_path() --[[ calculate an additional yaw rate to get us to the right angle of sideslip for knifeedge --]] - -- look ahead by AEROM_KE_TC seconds to get predicted attitude - local lookahead_rotation = Quaternion() - lookahead_rotation:from_angular_velocity(path_rate_bf_dps:scale(math.rad(1)), AEROM_KE_TC:get()) - local lkahead_q = orientation_rel_ef_with_roll_angle * lookahead_rotation - -- get sin(roll)*cos(pitch) for scaling the KE angle - local ke_angle = get_quat_dcm_c_y(lkahead_q) - -- scale by square of airspeed - local airspeed_scaling = SCALING_SPEED:get()/airspeed_constrained - local ff_yaw_rate_rads = math.rad(AEROM_KE_ANG:get()) * (-ke_angle) * sq(airspeed_scaling) - - local sideslip_rate_bf_dps = makeVector3f(0, 0, ff_yaw_rate_rads):scale(math.deg(1)) + -- local sideslip_rate_bf_dps = calculate_side_slip_aoa(path_rate_bf_dps, ahrs_quat, airspeed_constrained, tv_unit, ahrs_velned, actual_dt) + local sideslip_rate_bf_dps = Vector3f() --[[ total angular rate is sum of path rate, correction rate and roll correction rate @@ -2109,6 +2149,13 @@ function do_path() tot_ang_vel_bf_dps = path_var.last_ang_rate_dps + ang_rate_diff_dps path_var.last_ang_rate_dps = tot_ang_vel_bf_dps + --[[ + calculate a rudder offset for knife-edge + --]] + local rudder_offset_pct = 0 + if AEROM_KE_RUDD:get() > 0 then + rudder_offset_pct = calculate_rudder_offset(ahrs_quat, ahrs_gyro, airspeed_constrained) + end --[[ log POSM is pose-measured, POST is pose-track, POSB is pose-track without the roll @@ -2122,14 +2169,15 @@ function do_path() q_change_t, actual_dt) - logger.write('AERT','Cx,Cy,Cz,Px,Py,Pz,Ex,Tx,Ty,Tz,Perr,Aerr,Yff', 'fffffffffffff', + logger.write('AERT','Cx,Cy,Cz,Px,Py,Pz,Ex,Tx,Ty,Tz,Perr,Aerr,Yff,Rofs', 'ffffffffffffff', cor_ang_vel_bf_dps:x(), cor_ang_vel_bf_dps:y(), cor_ang_vel_bf_dps:z(), path_rate_bf_dps:x(), path_rate_bf_dps:y(), path_rate_bf_dps:z(), err_angle_rate_bf_dps:x(), tot_ang_vel_bf_dps:x(), tot_ang_vel_bf_dps:y(), tot_ang_vel_bf_dps:z(), pos_error_ef:length(), wrap_180(math.deg(err_angle_rad)), - math.deg(ff_yaw_rate_rads)) + sideslip_rate_bf_dps:z(), + rudder_offset_pct) --log_pose('POSB', p1, path_var.accumulated_orientation_rel_ef) @@ -2157,6 +2205,7 @@ function do_path() end vehicle:set_target_throttle_rate_rpy(throttle, tot_ang_vel_bf_dps:x(), tot_ang_vel_bf_dps:y(), tot_ang_vel_bf_dps:z()) + vehicle:set_rudder_offset(rudder_offset_pct, true) if now - last_named_float_t > 1.0 / NAME_FLOAT_RATE then last_named_float_t = now