AP_Scripting: fixed rudder in sport aerobatics

This commit is contained in:
Andrew Tridgell 2023-10-19 12:09:26 +11:00
parent 4a5684dc8e
commit 2ec87690b3
1 changed files with 14 additions and 8 deletions

View File

@ -284,6 +284,12 @@ function recover_alt()
return target_pitch, pitch_rate, yaw_rate return target_pitch, pitch_rate, yaw_rate
end end
function set_rate_targets(throttle, roll_rate, pitch_rate, yaw_rate)
-- we don't want a rudder offset, and we do want yaw rate
vehicle:set_rudder_offset(0, true)
vehicle:set_target_throttle_rate_rpy(throttle, roll_rate, pitch_rate, yaw_rate)
end
-- start of trick routines--------------------------------------------------------------------------------- -- start of trick routines---------------------------------------------------------------------------------
function do_axial_roll(arg1, arg2) function do_axial_roll(arg1, arg2)
-- constant roll rate axial roll, arg1 roll rate, arg2 is number of rolls -- constant roll rate axial roll, arg1 roll rate, arg2 is number of rolls
@ -328,7 +334,7 @@ function do_axial_roll(arg1, arg2)
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())
vehicle:set_target_throttle_rate_rpy(throttle, roll_rate, pitch_rate, yaw_rate) set_rate_targets(throttle, roll_rate, pitch_rate, yaw_rate)
end end
end end
@ -402,7 +408,7 @@ function do_loop(arg1, arg2)
else else
roll_rate = earth_frame_wings_level(level_type) roll_rate = earth_frame_wings_level(level_type)
end end
vehicle:set_target_throttle_rate_rpy(throttle, roll_rate, pitch_rate, 0) set_rate_targets(throttle, roll_rate, pitch_rate, 0)
end end
@ -454,7 +460,7 @@ function do_rolling_circle(arg1, arg2)
pitch_rate, yaw_rate = pitch_controller(target_pitch, wrap_360(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) set_rate_targets(throttle, roll_rate_dps, pitch_rate, yaw_rate)
end end
end end
@ -483,7 +489,7 @@ function do_knife_edge(arg1,arg2)
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())
throttle = throttle_controller() throttle = throttle_controller()
vehicle:set_target_throttle_rate_rpy(throttle, roll_rate, pitch_rate, yaw_rate) set_rate_targets(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
@ -512,7 +518,7 @@ function do_pause(arg1,arg2)
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())
throttle = throttle_controller() throttle = throttle_controller()
vehicle:set_target_throttle_rate_rpy(throttle, roll_rate, pitch_rate, yaw_rate) set_rate_targets(throttle, roll_rate, pitch_rate, yaw_rate)
else else
running = false running = false
gcs:send_text(5, string.format("Pause Over")) gcs:send_text(5, string.format("Pause Over"))
@ -585,7 +591,7 @@ function do_knifedge_circle(arg1, arg2)
pitch_rate, yaw_rate = pitch_controller(target_pitch, wrap_360(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) set_rate_targets(throttle, roll_rate_dps, pitch_rate, yaw_rate)
end end
end end
@ -666,7 +672,7 @@ function do_4point_roll(arg1, arg2)
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())
vehicle:set_target_throttle_rate_rpy(throttle, roll_rate, pitch_rate, yaw_rate) set_rate_targets(throttle, roll_rate, pitch_rate, yaw_rate)
end end
end end
@ -708,7 +714,7 @@ function do_split_s(arg1, arg2)
end end
end end
throttle = throttle_controller() throttle = throttle_controller()
vehicle:set_target_throttle_rate_rpy(throttle, roll_rate, pitch_rate, 0) set_rate_targets(throttle, roll_rate, pitch_rate, 0)
end end
function get_wp_location(i) function get_wp_location(i)