mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-02-22 07:44:03 -04:00
AP_Scripting: sport_aerobatics trick additions and fixes
This commit is contained in:
parent
9440e3c544
commit
787cce4fc3
@ -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)
|
||||||
|
|
||||||
@ -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
|
Work out which RC input channel you want to use for activation (a 3 position switch) and set
|
||||||
|
|
||||||
- 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.
|
||||||
|
@ -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,7 +524,191 @@ 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)
|
||||||
local loc = Location()
|
local loc = Location()
|
||||||
@ -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
|
||||||
|
Loading…
Reference in New Issue
Block a user