From 787cce4fc3d22a865f9bfd1b76170612ee13eb21 Mon Sep 17 00:00:00 2001 From: Henry Wurzburg Date: Wed, 4 Jan 2023 14:41:51 -0600 Subject: [PATCH] AP_Scripting: sport_aerobatics trick additions and fixes --- .../Aerobatics/FixedWing/RateBased/README.md | 26 +- .../FixedWing/RateBased/sport_aerobatics.lua | 346 ++++++++++++++---- 2 files changed, 291 insertions(+), 81 deletions(-) diff --git a/libraries/AP_Scripting/applets/Aerobatics/FixedWing/RateBased/README.md b/libraries/AP_Scripting/applets/Aerobatics/FixedWing/RateBased/README.md index 5ccada4db3..2a5719ddc1 100644 --- a/libraries/AP_Scripting/applets/Aerobatics/FixedWing/RateBased/README.md +++ b/libraries/AP_Scripting/applets/Aerobatics/FixedWing/RateBased/README.md @@ -28,8 +28,11 @@ the ground track. | 3 | Rolling Circle | yawrate(dps) | rollrate(dps) | No | | 4 | KnifeEdge | roll angle(deg) | length(sec) | No | | 5 | Pause | length(sec) | na | No | +| 6 | KnifeEdge Circle | yawrate(dps) | na | No | +| 7 | 4 point roll | rollrate(dps) | pause in sec at each point | No | +| 8 | Split-S | pitchrate(dps) | rollrate(dps) | Yes | -note: for Rolling Circle, the time it takes to make the circle is 360/yawrate. You should make sure that an integer number of rolls is commanded by the rollrate parameter in that time, ie rollrate should be set to the number of rolls * yawrate. +note: for Rolling Circle, the time it takes to make the circle is 360/yawrate. You should make sure that an integer number of rolls is commanded by the rollrate parameter in that time, ie rollrate should be set to the number of rolls * yawrate. In most cases negative rate, reverses the direction, ie in Rolls -45 for rollrate would roll left at 45dps. ## Loading the script @@ -39,7 +42,7 @@ APM/SCRIPTS directory. You can use MAVFtp to do this. Then set - SCR_ENABLE = 1 - - SCR_HEAP_SIZE = 200000 + - SCR_HEAP_SIZE = 150000 - SCR_VM_I_COUNT = 100000 You will need to refresh parameters after setting SCR_ENABLE. Then @@ -68,16 +71,16 @@ NAV_SCRIPT_TIME elements (shown as SCRIPT_TIME in MissionPlanner). These mission - the command ID from the table above - the timeout in seconds - - up to four arguments as shown in the above table + - up to two arguments as shown in the above table The aerobatics system will use the location of the previous and next waypoints to line up the manoeuvre. You need to plan a normal waypoint just before the location where you want to start the -manoeuvre, then the NAV_SCRIPT_TIME with the trick or schedule ID, a timeout that is long enough to allow the trick, and then a normal waypoint after the manoeuvre. +manoeuvre, then the NAV_SCRIPT_TIME with the trick or schedule ID, a timeout that is long enough to allow the trick, and then a normal waypoint after the manoeuvre. You can have consecutive tricks between waypoints. You can use the PAUSE trick after a framing waypoint to move the location of the beginning of the trick or to put some time between consecutive tricks. ## Use with "tricks on a switch" -You can trigger the manoeuvres using RC switches, allowing you to have +You can trigger the manoeuvres using RC switches (or using the GCS AUX Function tab), allowing you to have up to 11 tricks pre-programmed on your transmitter ready for use in fixed wing flight. You can trigger the tricks in the following flight modes: @@ -90,7 +93,6 @@ modes: - CRUISE - LOITER - Set TRIKR_COUNT to the number of tricks you want to make available and reboot, with a maximum of 11. (Why 11 when fewer than that are available? To allow variants, such as different knife-edges, ie 90 deg and 180 deg inverted flight) @@ -115,21 +117,25 @@ select tricks. It is best to use a knob for the trick selector so you can have u Work out which RC input channel you want to use for activation (a 3 position switch) and set - RCn_OPTION = 300 + +Note: It is not required to setup the 300 function for activation by the Transmitter switch. You can also activate a trick via the GCS. Mission Planner has an AUX Functions tab to enable setting the "300" function from the GCS. Then work out what RC input channel you want to use for selection and set - RCn_OPTION = 301 + + TRIKR_COUNT must be non-zero and an RC channel set to function 301 for Tricks on a Switch to function. ## Flying with tricks -When the activation channel (the one marked with option 300) is in the -middle position then when you move the knob the GCS will display the +When the activation channel (the one assigned option 300 or set via the GCS) is in the +middle position then when you move the trick selection knob/switch, the GCS will display the currently selected trick. -To activate a trick you move the activation channel to the top +To activate a trick you move the activation channel to the top (high) position, and the trick will run. -Moving the activation switch to the bottom position cancels any +Moving the activation switch to the bottom position (low) cancels any running trick and stops the trick system. Changing flight modes will also cancel any active trick. diff --git a/libraries/AP_Scripting/applets/Aerobatics/FixedWing/RateBased/sport_aerobatics.lua b/libraries/AP_Scripting/applets/Aerobatics/FixedWing/RateBased/sport_aerobatics.lua index 035a71c6b9..b8fcd87cb2 100644 --- a/libraries/AP_Scripting/applets/Aerobatics/FixedWing/RateBased/sport_aerobatics.lua +++ b/libraries/AP_Scripting/applets/Aerobatics/FixedWing/RateBased/sport_aerobatics.lua @@ -38,7 +38,7 @@ local wp_yaw_deg = 0 local initial_height = 0 local repeat_count = 0 local running = false -local roll_stage = 0 +local trick_stage = 0 local function TrickDef(id, arg1, arg2, arg3, arg4) local self = {} @@ -84,23 +84,31 @@ TRIK_ACT_FN = bind_add_param2("_ACT_FN", 3, 300) TRIK_COUNT = bind_add_param2("_COUNT", 4, 3) TRICKS = {} + +function tricks_exist() + if rc:find_channel_for_option(TRIK_SEL_FN:get()) + and math.floor(TRIK_COUNT:get()) then + return true + else + return false + end +end + -- setup parameters for tricks count = math.floor(TRIK_COUNT:get()) -count = constrain(count,1,11) -if count > 11 then - count = 1 +count = constrain(count,0,11) +if tricks_exist() then + for i = 1, count do + local k = 5*i + local prefix = string.format("%u", i) + TRICKS[i] = TrickDef(bind_add_param2(prefix .. "_ID", k+0, i), + bind_add_param2(prefix .. "_ARG1", k+1, 30), + bind_add_param2(prefix .. "_ARG2", k+2, 0), + bind_add_param2(prefix .. "_ARG3", k+3, 0), + bind_add_param2(prefix .. "_ARG4", k+4, 0)) + end + gcs:send_text(5, string.format("Enabled %u aerobatic tricks", count)) end -for i = 1, count do - local k = 5*i - local prefix = string.format("%u", i) - TRICKS[i] = TrickDef(bind_add_param2(prefix .. "_ID", k+0, i), - bind_add_param2(prefix .. "_ARG1", k+1, 30), - bind_add_param2(prefix .. "_ARG2", k+2, 0), - bind_add_param2(prefix .. "_ARG3", k+3, 0), - bind_add_param2(prefix .. "_ARG4", k+4, 0)) -end -gcs:send_text(5, string.format("Enabled %u aerobatic tricks", TRIK_COUNT:get())) - -- roll angle error 180 wrap to cope with errors while in inverted segments @@ -281,39 +289,40 @@ function do_axial_roll(arg1, arg2) running = true roll_num = 1 repeat_count = arg2 -1 - roll_stage = 0 + trick_stage = 0 height_PI.reset() - --gcs:send_text(5, string.format("Starting roll(s)")) + gcs:send_text(5, string.format("Starting %d Roll(s)", arg2)) end local roll_rate = arg1 local pitch_deg = math.deg(ahrs:get_pitch()) local roll_deg = math.deg(ahrs:get_roll()) - if roll_stage == 0 then + if trick_stage == 0 then if roll_deg > 45 then - roll_stage = 1 + trick_stage = 1 end - elseif roll_stage == 1 then + elseif trick_stage == 1 then if roll_deg > -5 and roll_deg < 5 then -- we're done with a roll - gcs:send_text(5, string.format("Finished roll %d", roll_num)) + gcs:send_text(5, string.format("Finished Roll %d", roll_num)) if repeat_count > 0 then - roll_stage = 0 + trick_stage = 0 repeat_count = repeat_count - 1 roll_num = roll_num + 1 else running = false + trick = 0 if vehicle:get_mode() == MODE_AUTO then vehicle:nav_script_time_done(last_id) else vehicle:nav_scripting_enable(255) end - roll_stage = 2 + trick_stage = 2 return end end end - if roll_stage < 2 then + if trick_stage < 2 then throttle = throttle_controller() target_pitch = height_PI.update(initial_height) pitch_rate, yaw_rate = pitch_controller(target_pitch, wp_yaw_deg, PITCH_TCONST:get()) @@ -321,21 +330,21 @@ function do_axial_roll(arg1, arg2) end end -local loop_stage = 0 + local target_vel function do_loop(arg1, arg2) - -- do one loop with controllable pitch rate arg1 is pitch rate, arg2 number of loops, 0 indicates 1/2 cuban8 reversal style turnaround + -- do one loop with controllable pitch rate arg1 is pitch rate, arg2 number of loops, 0 or less indicates 1/2 cuban8 reversal style turnaround if not running then running = true - loop_stage = 0 + trick_stage = 0 loop_number = 1 repeat_count = arg2 -1 target_vel = ahrs:get_velocity_NED():length() - if arg2 ~=0 then - gcs:send_text(5, string.format("Starting loop")) + if arg2 > 0 then + gcs:send_text(5, string.format("Starting %d Loop(s)",arg2)) else - gcs:send_text(5, string.format("Starting turnaround")) + gcs:send_text(5, string.format("Starting Turnaround")) end end @@ -347,27 +356,27 @@ function do_loop(arg1, arg2) local pitch_rate = pitch_rate * (1+ 2*((vel/target_vel)-1)) --increase/decrease rate based on velocity to round loop pitch_rate = constrain(pitch_rate,.5 * arg1, 3 * arg1) - if loop_stage == 0 then + if trick_stage == 0 then if pitch_deg > 60 then - loop_stage = 1 + trick_stage = 1 end - elseif loop_stage == 1 then + elseif trick_stage == 1 then if (math.abs(roll_deg) < 90 and pitch_deg > -5 and pitch_deg < 5 and repeat_count >= 0) then -- we're done with loop - gcs:send_text(5, string.format("Finished loop %d", loop_number)) - loop_stage = 2 --now recover stage + gcs:send_text(5, string.format("Finished Loop %d", loop_number)) + trick_stage = 2 --now recover stage height_PI.reset() elseif (math.abs(roll_deg) > 90 and pitch_deg > -5 and pitch_deg < 5 and repeat_count < 0) then - gcs:send_text(5, string.format("Finished turnaround ")) - loop_stage = 2 --now recover stage + gcs:send_text(5, string.format("Finished Turnaround ")) + trick_stage = 2 --now recover stage height_PI.reset() end - elseif loop_stage == 2 then + elseif trick_stage == 2 then -- recover alt if above or below start and terminate if math.abs(ahrs:get_position():alt()*0.01 - initial_height) > 3 then throttle, pitch_rate, yaw_rate = recover_alt() elseif repeat_count > 0 then - loop_stage = 0 + trick_stage = 0 repeat_count = repeat_count - 1 loop_number = loop_number + 1 else @@ -381,7 +390,7 @@ function do_loop(arg1, arg2) end end throttle = throttle_controller() - if loop_stage == 2 or loop_stage == 0 then + if trick_stage == 2 or trick_stage == 0 then level_type = 0 else level_type = 1 @@ -394,19 +403,19 @@ function do_loop(arg1, arg2) vehicle:set_target_throttle_rate_rpy(throttle, roll_rate, pitch_rate, 0) end -local rolling_circle_stage = 0 -local rolling_circle_yaw = 0 -local rolling_circle_last_ms = 0 + +local circle_yaw_deg = 0 +local circle_last_ms = 0 function do_rolling_circle(arg1, arg2) -- constant roll rate circle roll, arg1 yaw rate, positive to right, neg to left, arg2 is roll rate if not running then running = true - rolling_circle_stage = 0 - rolling_circle_yaw_deg = 0 - rolling_circle_last_ms = millis() + trick_stage = 0 + circle_yaw_deg = 0 + circle_last_ms = millis() height_PI.reset() - --gcs:send_text(5, string.format("Starting rolling circle")) + gcs:send_text(5, string.format("Starting Rolling Circle")) end local yaw_rate_dps = arg1 local roll_rate_dps = arg2 @@ -414,33 +423,33 @@ function do_rolling_circle(arg1, arg2) local roll_deg = math.deg(ahrs:get_roll()) local yaw_deg = math.deg(ahrs:get_yaw()) local now_ms = millis() - local dt = (now_ms - rolling_circle_last_ms):tofloat() * 0.001 - rolling_circle_last_ms = now_ms + local dt = (now_ms - circle_last_ms):tofloat() * 0.001 + circle_last_ms = now_ms - rolling_circle_yaw_deg = rolling_circle_yaw_deg + yaw_rate_dps * dt + circle_yaw_deg = circle_yaw_deg + yaw_rate_dps * dt - if rolling_circle_stage == 0 then - if math.abs(rolling_circle_yaw_deg) > 10.0 then - rolling_circle_stage = 1 + if trick_stage == 0 then + if math.abs(circle_yaw_deg) > 10.0 then + trick_stage = 1 end - elseif rolling_circle_stage == 1 then - if math.abs(rolling_circle_yaw_deg) >= 360.0 then + elseif trick_stage == 1 then + if math.abs(circle_yaw_deg) >= 360.0 then running = false -- we're done - gcs:send_text(5,"Finished rolling circle") + gcs:send_text(5,"Finished Rolling Circle") if vehicle:get_mode() == MODE_AUTO then vehicle:nav_script_time_done(last_id) else vehicle:nav_scripting_enable(255) end - rolling_circle_stage = 2 + trick_stage = 2 return end end - if rolling_circle_stage < 2 then + if trick_stage < 2 then target_pitch = height_PI.update(initial_height) vel = ahrs:get_velocity_NED() - pitch_rate, yaw_rate = pitch_controller(target_pitch, wrap_360(rolling_circle_yaw_deg+initial_yaw_deg), PITCH_TCONST:get()) + pitch_rate, yaw_rate = pitch_controller(target_pitch, wrap_360(circle_yaw_deg+initial_yaw_deg), PITCH_TCONST:get()) throttle = throttle_controller() throttle = constrain(throttle, 0, 100.0) vehicle:set_target_throttle_rate_rpy(throttle, roll_rate_dps, pitch_rate, yaw_rate) @@ -455,7 +464,7 @@ function do_knife_edge(arg1,arg2) running = true height_PI.reset() knife_edge_s = now - gcs:send_text(5, string.format("%d Knife edge", arg1)) + gcs:send_text(5, string.format("Starting %d Knife Edge", arg1)) end local i=0 if (now - knife_edge_s) < arg2 then @@ -474,7 +483,7 @@ function do_knife_edge(arg1,arg2) throttle = throttle_controller() vehicle:set_target_throttle_rate_rpy(throttle, roll_rate, pitch_rate, yaw_rate) else - gcs:send_text(5, string.format("Finished Knife edge", arg1)) + gcs:send_text(5, string.format("Finished Knife Edge", arg1)) running = false if vehicle:get_mode() == MODE_AUTO then vehicle:nav_script_time_done(last_id) @@ -504,6 +513,7 @@ function do_pause(arg1,arg2) vehicle:set_target_throttle_rate_rpy(throttle, roll_rate, pitch_rate, yaw_rate) else running = false + gcs:send_text(5, string.format("Pause Over")) if vehicle:get_mode() == MODE_AUTO then vehicle:nav_script_time_done(last_id) else @@ -514,7 +524,191 @@ function do_pause(arg1,arg2) end +local circle_yaw = 0 +local circle_last_ms = 0 +function do_knifedge_circle(arg1, arg2) + -- constant roll angle circle , arg1 yaw rate, positive to right, neg to left, arg2 is not used + if not running then + running = true + trick_stage = 0 + circle_yaw_deg = 0 + circle_last_ms = millis() + height_PI.reset() + gcs:send_text(5, string.format("Staring KnifeEdge Circle")) + end + local yaw_rate_dps = arg1 + local pitch_deg = math.deg(ahrs:get_pitch()) + local yaw_deg = math.deg(ahrs:get_yaw()) + local now_ms = millis() + local dt = (now_ms - circle_last_ms):tofloat() * 0.001 + circle_last_ms = now_ms + circle_yaw_deg = circle_yaw_deg + yaw_rate_dps * dt + if trick_stage == 0 then + if math.abs(circle_yaw_deg) > 10.0 then + trick_stage = 1 + end + elseif trick_stage == 1 then + if math.abs(circle_yaw_deg) >= 360.0 then + running = false + -- we're done + gcs:send_text(5,"Finished KnifeEdge Circle") + if vehicle:get_mode() == MODE_AUTO then + vehicle:nav_script_time_done(last_id) + else + vehicle:nav_scripting_enable(255) + end + trick_stage = 2 + return + end + end + if trick_stage < 2 then + local roll_deg = math.deg(ahrs:get_roll()) + if arg1 >0 then + angle = 90 + else + angle = -90 + end + local roll_angle_error = (angle - roll_deg) + if math.abs(roll_angle_error) > 180 then + if roll_angle_error > 0 then + roll_angle_error = roll_angle_error - 360 + else + roll_angle_error= roll_angle_error +360 + end + end + roll_rate_dps = roll_angle_error/RLL2SRV_TCONST:get() + target_pitch = height_PI.update(initial_height) + vel = ahrs:get_velocity_NED() + pitch_rate, yaw_rate = pitch_controller(target_pitch, wrap_360(circle_yaw_deg+initial_yaw_deg), PITCH_TCONST:get()) + throttle = throttle_controller() + throttle = constrain(throttle, 0, 100.0) + vehicle:set_target_throttle_rate_rpy(throttle, roll_rate_dps, pitch_rate, yaw_rate) + end +end + +function hold_roll_angle (angle) + local roll_deg = math.deg(ahrs:get_roll()) + local roll_angle_error = (angle - roll_deg) + if math.abs(roll_angle_error) > 180 then + if roll_angle_error > 0 then + roll_angle_error = roll_angle_error - 360 + else + roll_angle_error= roll_angle_error +360 + end + end + return roll_angle_error/RLL2SRV_TCONST:get() +end + +local roll_ms = 0 +function do_4point_roll(arg1, arg2) + -- constant roll rate axial roll with pauses at 90 deg intervals, arg1 roll rate, arg2 is pause time in sec + arg2 = arg2 * 1000 + if not running then + running = true + trick_stage = 0 + height_PI.reset() + gcs:send_text(5, string.format("Starting 4pt Roll")) + end + local pitch_deg = math.deg(ahrs:get_pitch()) + local roll_deg = math.deg(ahrs:get_roll()) + + if trick_stage == 0 then + roll_rate = arg1 + if roll_deg >= 90 then + trick_stage = 1 + roll_ms = millis() + end + elseif trick_stage == 1 then + roll_rate = hold_roll_angle(90) + if (millis() - roll_ms > arg2) then + trick_stage = 2 + end + elseif trick_stage == 2 then + roll_rate = arg1 + if roll_deg >= 175 or roll_deg <= -175 then + trick_stage = 3 + roll_ms = millis() + end + elseif trick_stage == 3 then + roll_rate = hold_roll_angle(180) + if (millis() - roll_ms >arg2) then + trick_stage = 4 + end + elseif trick_stage == 4 then + roll_rate = arg1 + if roll_deg >=-90 and roll_deg < 175 then + trick_stage = 5 + roll_ms = millis() + end + elseif trick_stage == 5 then + roll_rate = hold_roll_angle(-90) + if (millis() - roll_ms >arg2) then + trick_stage = 6 + end + elseif trick_stage == 6 then + roll_rate = arg1 + if roll_deg > -5 and roll_deg < 5 then + -- we're done with a roll + trick_stage = 7 + gcs:send_text(5, "Finished 4 pt Roll") + running = false + if vehicle:get_mode() == MODE_AUTO then + vehicle:nav_script_time_done(last_id) + else + vehicle:nav_scripting_enable(255) + end + end + end + if trick_stage < 7 then + throttle = throttle_controller() + target_pitch = height_PI.update(initial_height) + pitch_rate, yaw_rate = pitch_controller(target_pitch, wp_yaw_deg, PITCH_TCONST:get()) + vehicle:set_target_throttle_rate_rpy(throttle, roll_rate, pitch_rate, yaw_rate) + end +end + +function do_split_s(arg1, arg2) + -- roll inverted at arg2 rate, then 1/2 loop at arg1 pitch rate + if not running then + running = true + trick_stage = 0 --roll inverted, pitch level + height_PI.reset() + gcs:send_text(5, string.format("Starting Split-S")) + end + local pitch_deg = math.deg(ahrs:get_pitch()) + local roll_deg = math.deg(ahrs:get_roll()) + if trick_stage == 0 then + roll_rate = arg2 + wp_yaw_deg = math.deg(ahrs:get_yaw()) + target_pitch = height_PI.update(initial_height) + pitch_rate, yaw_rate = pitch_controller(target_pitch, wp_yaw_deg, PITCH_TCONST:get()) + if roll_deg >=175 or roll_deg <= -175 then + trick_stage = 1 --start the pitch rate, wings level + end + elseif trick_stage == 1 then + if math.abs(pitch_deg) > 85 and math.abs(pitch_deg) < 95 then + roll_rate = 0 + else + roll_rate = earth_frame_wings_level(1) + end + pitch_rate = arg1 + if (math.abs(roll_deg) < 90 and pitch_deg > -5 and pitch_deg < 5) then + trick_stage = 2 --trick over + gcs:send_text(5, string.format("Finished Split-S ")) + height_PI.reset() + running = false + if vehicle:get_mode() == MODE_AUTO then + vehicle:nav_script_time_done(last_id) + else + vehicle:nav_scripting_enable(255) + end + end + end + throttle = throttle_controller() + vehicle:set_target_throttle_rate_rpy(throttle, roll_rate, pitch_rate, 0) +end + function get_wp_location(i) local m = mission:get_item(i) local loc = Location() @@ -598,6 +792,10 @@ end name[2] = "Loop(s)/Turnaround" name[3] = "Rolling Circle" name[4] = "Knife-Edge" + name[5] = "Pause" + name[6] = "Knife Edge Circle" + name[7] = "4pt Roll" + name[8] = "Split-S" --[[ check for running a trick --]] @@ -626,8 +824,9 @@ function check_trick() end if running then -- let the task finish - return id + return selection end + -- trick action changed state if action ~= last_trick_action_state then last_trick_selection = selection last_trick_action_state = action @@ -640,19 +839,17 @@ function check_trick() gcs:send_text(5, string.format("%s selected ", name[id])) return 0 end - if action == 2 then + -- action changed to execute + if action == 2 and vehicle:get_likely_flying() then last_trick_selection = selection local current_mode = vehicle:get_mode() if not vehicle:nav_scripting_enable(current_mode) then gcs:send_text(0, string.format("Tricks not available in this mode")) return 0 end - if id ~= 2 then - gcs:send_text(5, string.format("%s started ", name[id])) - end wp_yaw_deg = math.deg(ahrs:get_yaw()) initial_height = ahrs:get_position():alt()*0.01 - return id + return selection end end return 0 @@ -669,18 +866,25 @@ function do_trick(cmd,arg1,arg2) do_knife_edge(arg1,arg2) elseif cmd == 5 then do_pause(arg1,arg2) + elseif cmd == 6 then + do_knifedge_circle(arg1,arg2) + elseif cmd == 7 then + do_4point_roll(arg1,arg2) + elseif cmd == 8 then + do_split_s(arg1,arg2) end end function update() if vehicle:get_mode() == MODE_AUTO then - check_auto_mission() - else + check_auto_mission() --run a trick mission item + elseif tricks_exist() then trick = check_trick() - if trick ~=0 then + if trick ~=0 then + id = TRICKS[trick].id:get() arg1 = TRICKS[trick].args[1]:get() arg2 = TRICKS[trick].args[2]:get() - do_trick(trick,arg1,arg2) + do_trick(id,arg1,arg2) end end return update, 10