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: sport_aerobatics trick additions and fixes

Browse Source
This commit is contained in:
Henry Wurzburg 2023-01-04 14:41:51 -06:00 committed by Andrew Tridgell
parent 9440e3c544
commit 787cce4fc3
2 changed files with 291 additions and 81 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

26
libraries/AP_Scripting/applets/Aerobatics/FixedWing/RateBased/README.md
View File

@ -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.

346
libraries/AP_Scripting/applets/Aerobatics/FixedWing/RateBased/sport_aerobatics.lua
View File

@ -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