AP_Scripting: sport_aerobatics trick additions and fixes

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

View File

@ -28,8 +28,11 @@ the ground track.
| 3 | Rolling Circle | yawrate(dps) | rollrate(dps) | No | | 3 | Rolling Circle | yawrate(dps) | rollrate(dps) | No |
| 4 | KnifeEdge | roll angle(deg) | length(sec) | No | | 4 | KnifeEdge | roll angle(deg) | length(sec) | No |
| 5 | Pause | length(sec) | na | 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 ## Loading the script
@ -39,7 +42,7 @@ APM/SCRIPTS directory. You can use MAVFtp to do this.
Then set Then set
- SCR_ENABLE = 1 - SCR_ENABLE = 1
- SCR_HEAP_SIZE = 200000 - SCR_HEAP_SIZE = 150000
- SCR_VM_I_COUNT = 100000 - SCR_VM_I_COUNT = 100000
You will need to refresh parameters after setting SCR_ENABLE. Then 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 command ID from the table above
- the timeout in seconds - 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 The aerobatics system will use the location of the previous and next
waypoints to line up the manoeuvre. You need to plan a normal waypoints to line up the manoeuvre. You need to plan a normal
waypoint just before the location where you want to start the 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" ## 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 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 fixed wing flight. You can trigger the tricks in the following flight
modes: modes:
@ -90,7 +93,6 @@ modes:
- CRUISE - CRUISE
- LOITER - LOITER
Set TRIKR_COUNT to the number of tricks you want to make available and reboot, 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) 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)
@ -116,20 +118,24 @@ Work out which RC input channel you want to use for activation (a 3 position swi
- RCn_OPTION = 300 - 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 Then work out what RC input channel you want to use for selection and set
- RCn_OPTION = 301 - 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 ## Flying with tricks
When the activation channel (the one marked with option 300) is in 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 knob the GCS will display the middle position then when you move the trick selection knob/switch, the GCS will display the
currently selected trick. 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. 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. running trick and stops the trick system.
Changing flight modes will also cancel any active trick. Changing flight modes will also cancel any active trick.

View File

@ -38,7 +38,7 @@ local wp_yaw_deg = 0
local initial_height = 0 local initial_height = 0
local repeat_count = 0 local repeat_count = 0
local running = false local running = false
local roll_stage = 0 local trick_stage = 0
local function TrickDef(id, arg1, arg2, arg3, arg4) local function TrickDef(id, arg1, arg2, arg3, arg4)
local self = {} local self = {}
@ -84,23 +84,31 @@ TRIK_ACT_FN = bind_add_param2("_ACT_FN", 3, 300)
TRIK_COUNT = bind_add_param2("_COUNT", 4, 3) TRIK_COUNT = bind_add_param2("_COUNT", 4, 3)
TRICKS = {} 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 -- setup parameters for tricks
count = math.floor(TRIK_COUNT:get()) count = math.floor(TRIK_COUNT:get())
count = constrain(count,1,11) count = constrain(count,0,11)
if count > 11 then if tricks_exist() then
count = 1 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 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 -- 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 running = true
roll_num = 1 roll_num = 1
repeat_count = arg2 -1 repeat_count = arg2 -1
roll_stage = 0 trick_stage = 0
height_PI.reset() height_PI.reset()
--gcs:send_text(5, string.format("Starting roll(s)")) gcs:send_text(5, string.format("Starting %d Roll(s)", arg2))
end end
local roll_rate = arg1 local roll_rate = arg1
local pitch_deg = math.deg(ahrs:get_pitch()) local pitch_deg = math.deg(ahrs:get_pitch())
local roll_deg = math.deg(ahrs:get_roll()) local roll_deg = math.deg(ahrs:get_roll())
if roll_stage == 0 then if trick_stage == 0 then
if roll_deg > 45 then if roll_deg > 45 then
roll_stage = 1 trick_stage = 1
end end
elseif roll_stage == 1 then elseif trick_stage == 1 then
if roll_deg > -5 and roll_deg < 5 then if roll_deg > -5 and roll_deg < 5 then
-- we're done with a roll -- 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 if repeat_count > 0 then
roll_stage = 0 trick_stage = 0
repeat_count = repeat_count - 1 repeat_count = repeat_count - 1
roll_num = roll_num + 1 roll_num = roll_num + 1
else else
running = false running = false
trick = 0
if vehicle:get_mode() == MODE_AUTO then if vehicle:get_mode() == MODE_AUTO then
vehicle:nav_script_time_done(last_id) vehicle:nav_script_time_done(last_id)
else else
vehicle:nav_scripting_enable(255) vehicle:nav_scripting_enable(255)
end end
roll_stage = 2 trick_stage = 2
return return
end end
end end
end end
if roll_stage < 2 then if trick_stage < 2 then
throttle = throttle_controller() throttle = throttle_controller()
target_pitch = height_PI.update(initial_height) target_pitch = height_PI.update(initial_height)
pitch_rate, yaw_rate = pitch_controller(target_pitch, wp_yaw_deg, PITCH_TCONST:get()) 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
end end
local loop_stage = 0
local target_vel local target_vel
function do_loop(arg1, arg2) 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 if not running then
running = true running = true
loop_stage = 0 trick_stage = 0
loop_number = 1 loop_number = 1
repeat_count = arg2 -1 repeat_count = arg2 -1
target_vel = ahrs:get_velocity_NED():length() target_vel = ahrs:get_velocity_NED():length()
if arg2 ~=0 then if arg2 > 0 then
gcs:send_text(5, string.format("Starting loop")) gcs:send_text(5, string.format("Starting %d Loop(s)",arg2))
else else
gcs:send_text(5, string.format("Starting turnaround")) gcs:send_text(5, string.format("Starting Turnaround"))
end end
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 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) pitch_rate = constrain(pitch_rate,.5 * arg1, 3 * arg1)
if loop_stage == 0 then if trick_stage == 0 then
if pitch_deg > 60 then if pitch_deg > 60 then
loop_stage = 1 trick_stage = 1
end 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 if (math.abs(roll_deg) < 90 and pitch_deg > -5 and pitch_deg < 5 and repeat_count >= 0) then
-- we're done with loop -- we're done with loop
gcs:send_text(5, string.format("Finished loop %d", loop_number)) gcs:send_text(5, string.format("Finished Loop %d", loop_number))
loop_stage = 2 --now recover stage trick_stage = 2 --now recover stage
height_PI.reset() height_PI.reset()
elseif (math.abs(roll_deg) > 90 and pitch_deg > -5 and pitch_deg < 5 and repeat_count < 0) then 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 ")) gcs:send_text(5, string.format("Finished Turnaround "))
loop_stage = 2 --now recover stage trick_stage = 2 --now recover stage
height_PI.reset() height_PI.reset()
end end
elseif loop_stage == 2 then elseif trick_stage == 2 then
-- recover alt if above or below start and terminate -- recover alt if above or below start and terminate
if math.abs(ahrs:get_position():alt()*0.01 - initial_height) > 3 then if math.abs(ahrs:get_position():alt()*0.01 - initial_height) > 3 then
throttle, pitch_rate, yaw_rate = recover_alt() throttle, pitch_rate, yaw_rate = recover_alt()
elseif repeat_count > 0 then elseif repeat_count > 0 then
loop_stage = 0 trick_stage = 0
repeat_count = repeat_count - 1 repeat_count = repeat_count - 1
loop_number = loop_number + 1 loop_number = loop_number + 1
else else
@ -381,7 +390,7 @@ function do_loop(arg1, arg2)
end end
end end
throttle = throttle_controller() throttle = throttle_controller()
if loop_stage == 2 or loop_stage == 0 then if trick_stage == 2 or trick_stage == 0 then
level_type = 0 level_type = 0
else else
level_type = 1 level_type = 1
@ -394,19 +403,19 @@ function do_loop(arg1, arg2)
vehicle:set_target_throttle_rate_rpy(throttle, roll_rate, pitch_rate, 0) vehicle:set_target_throttle_rate_rpy(throttle, roll_rate, pitch_rate, 0)
end end
local rolling_circle_stage = 0
local rolling_circle_yaw = 0 local circle_yaw_deg = 0
local rolling_circle_last_ms = 0 local circle_last_ms = 0
function do_rolling_circle(arg1, arg2) function do_rolling_circle(arg1, arg2)
-- constant roll rate circle roll, arg1 yaw rate, positive to right, neg to left, arg2 is roll rate -- constant roll rate circle roll, arg1 yaw rate, positive to right, neg to left, arg2 is roll rate
if not running then if not running then
running = true running = true
rolling_circle_stage = 0 trick_stage = 0
rolling_circle_yaw_deg = 0 circle_yaw_deg = 0
rolling_circle_last_ms = millis() circle_last_ms = millis()
height_PI.reset() height_PI.reset()
--gcs:send_text(5, string.format("Starting rolling circle")) gcs:send_text(5, string.format("Starting Rolling Circle"))
end end
local yaw_rate_dps = arg1 local yaw_rate_dps = arg1
local roll_rate_dps = arg2 local roll_rate_dps = arg2
@ -414,33 +423,33 @@ function do_rolling_circle(arg1, arg2)
local roll_deg = math.deg(ahrs:get_roll()) local roll_deg = math.deg(ahrs:get_roll())
local yaw_deg = math.deg(ahrs:get_yaw()) local yaw_deg = math.deg(ahrs:get_yaw())
local now_ms = millis() local now_ms = millis()
local dt = (now_ms - rolling_circle_last_ms):tofloat() * 0.001 local dt = (now_ms - circle_last_ms):tofloat() * 0.001
rolling_circle_last_ms = now_ms 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 trick_stage == 0 then
if math.abs(rolling_circle_yaw_deg) > 10.0 then if math.abs(circle_yaw_deg) > 10.0 then
rolling_circle_stage = 1 trick_stage = 1
end end
elseif rolling_circle_stage == 1 then elseif trick_stage == 1 then
if math.abs(rolling_circle_yaw_deg) >= 360.0 then if math.abs(circle_yaw_deg) >= 360.0 then
running = false running = false
-- we're done -- we're done
gcs:send_text(5,"Finished rolling circle") gcs:send_text(5,"Finished Rolling Circle")
if vehicle:get_mode() == MODE_AUTO then if vehicle:get_mode() == MODE_AUTO then
vehicle:nav_script_time_done(last_id) vehicle:nav_script_time_done(last_id)
else else
vehicle:nav_scripting_enable(255) vehicle:nav_scripting_enable(255)
end end
rolling_circle_stage = 2 trick_stage = 2
return return
end end
end end
if rolling_circle_stage < 2 then if trick_stage < 2 then
target_pitch = height_PI.update(initial_height) target_pitch = height_PI.update(initial_height)
vel = ahrs:get_velocity_NED() 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 = throttle_controller()
throttle = constrain(throttle, 0, 100.0) throttle = constrain(throttle, 0, 100.0)
vehicle:set_target_throttle_rate_rpy(throttle, roll_rate_dps, pitch_rate, yaw_rate) 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 running = true
height_PI.reset() height_PI.reset()
knife_edge_s = now 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 end
local i=0 local i=0
if (now - knife_edge_s) < arg2 then if (now - knife_edge_s) < arg2 then
@ -474,7 +483,7 @@ function do_knife_edge(arg1,arg2)
throttle = throttle_controller() throttle = throttle_controller()
vehicle:set_target_throttle_rate_rpy(throttle, roll_rate, pitch_rate, yaw_rate) vehicle:set_target_throttle_rate_rpy(throttle, roll_rate, pitch_rate, yaw_rate)
else else
gcs:send_text(5, string.format("Finished Knife edge", arg1)) gcs:send_text(5, string.format("Finished Knife Edge", arg1))
running = false running = false
if vehicle:get_mode() == MODE_AUTO then if vehicle:get_mode() == MODE_AUTO then
vehicle:nav_script_time_done(last_id) 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) vehicle:set_target_throttle_rate_rpy(throttle, roll_rate, pitch_rate, yaw_rate)
else else
running = false running = false
gcs:send_text(5, string.format("Pause Over"))
if vehicle:get_mode() == MODE_AUTO then if vehicle:get_mode() == MODE_AUTO then
vehicle:nav_script_time_done(last_id) vehicle:nav_script_time_done(last_id)
else else
@ -514,6 +524,190 @@ function do_pause(arg1,arg2)
end 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) function get_wp_location(i)
local m = mission:get_item(i) local m = mission:get_item(i)
@ -598,6 +792,10 @@ end
name[2] = "Loop(s)/Turnaround" name[2] = "Loop(s)/Turnaround"
name[3] = "Rolling Circle" name[3] = "Rolling Circle"
name[4] = "Knife-Edge" 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 check for running a trick
--]] --]]
@ -626,8 +824,9 @@ function check_trick()
end end
if running then if running then
-- let the task finish -- let the task finish
return id return selection
end end
-- trick action changed state
if action ~= last_trick_action_state then if action ~= last_trick_action_state then
last_trick_selection = selection last_trick_selection = selection
last_trick_action_state = action last_trick_action_state = action
@ -640,19 +839,17 @@ function check_trick()
gcs:send_text(5, string.format("%s selected ", name[id])) gcs:send_text(5, string.format("%s selected ", name[id]))
return 0 return 0
end end
if action == 2 then -- action changed to execute
if action == 2 and vehicle:get_likely_flying() then
last_trick_selection = selection last_trick_selection = selection
local current_mode = vehicle:get_mode() local current_mode = vehicle:get_mode()
if not vehicle:nav_scripting_enable(current_mode) then if not vehicle:nav_scripting_enable(current_mode) then
gcs:send_text(0, string.format("Tricks not available in this mode")) gcs:send_text(0, string.format("Tricks not available in this mode"))
return 0 return 0
end end
if id ~= 2 then
gcs:send_text(5, string.format("%s started ", name[id]))
end
wp_yaw_deg = math.deg(ahrs:get_yaw()) wp_yaw_deg = math.deg(ahrs:get_yaw())
initial_height = ahrs:get_position():alt()*0.01 initial_height = ahrs:get_position():alt()*0.01
return id return selection
end end
end end
return 0 return 0
@ -669,18 +866,25 @@ function do_trick(cmd,arg1,arg2)
do_knife_edge(arg1,arg2) do_knife_edge(arg1,arg2)
elseif cmd == 5 then elseif cmd == 5 then
do_pause(arg1,arg2) 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
end end
function update() function update()
if vehicle:get_mode() == MODE_AUTO then if vehicle:get_mode() == MODE_AUTO then
check_auto_mission() check_auto_mission() --run a trick mission item
else elseif tricks_exist() then
trick = check_trick() trick = check_trick()
if trick ~=0 then if trick ~=0 then
id = TRICKS[trick].id:get()
arg1 = TRICKS[trick].args[1]:get() arg1 = TRICKS[trick].args[1]:get()
arg2 = TRICKS[trick].args[2]:get() arg2 = TRICKS[trick].args[2]:get()
do_trick(trick,arg1,arg2) do_trick(id,arg1,arg2)
end end
end end
return update, 10 return update, 10