Skip to content

Commit

Permalink
AP_Scripting: fixed rudder in sport aerobatics
Browse files Browse the repository at this point in the history
  • Loading branch information
tridge committed Oct 22, 2023
1 parent 8018d30 commit 18d964a
Showing 1 changed file with 14 additions and 8 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -284,6 +284,12 @@ function recover_alt()
return target_pitch, pitch_rate, yaw_rate
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---------------------------------------------------------------------------------
function do_axial_roll(arg1, arg2)
-- constant roll rate axial roll, arg1 roll rate, arg2 is number of rolls
Expand Down Expand Up @@ -328,7 +334,7 @@ function do_axial_roll(arg1, arg2)
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)
set_rate_targets(throttle, roll_rate, pitch_rate, yaw_rate)
end
end

Expand Down Expand Up @@ -402,7 +408,7 @@ function do_loop(arg1, arg2)
else
roll_rate = earth_frame_wings_level(level_type)
end
vehicle:set_target_throttle_rate_rpy(throttle, roll_rate, pitch_rate, 0)
set_rate_targets(throttle, roll_rate, pitch_rate, 0)
end


Expand Down Expand Up @@ -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())
throttle = throttle_controller()
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

Expand Down Expand Up @@ -483,7 +489,7 @@ function do_knife_edge(arg1,arg2)
target_pitch = height_PI.update(initial_height)
pitch_rate, yaw_rate = pitch_controller(target_pitch, wp_yaw_deg, PITCH_TCONST:get())
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
gcs:send_text(5, string.format("Finished Knife Edge", arg1))
running = false
Expand Down Expand Up @@ -512,7 +518,7 @@ function do_pause(arg1,arg2)
target_pitch = height_PI.update(initial_height)
pitch_rate, yaw_rate = pitch_controller(target_pitch, wp_yaw_deg, PITCH_TCONST:get())
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
running = false
gcs:send_text(5, string.format("Pause Over"))
Expand Down Expand Up @@ -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())
throttle = throttle_controller()
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

Expand Down Expand Up @@ -666,7 +672,7 @@ function do_4point_roll(arg1, arg2)
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)
set_rate_targets(throttle, roll_rate, pitch_rate, yaw_rate)
end
end

Expand Down Expand Up @@ -708,7 +714,7 @@ function do_split_s(arg1, arg2)
end
end
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

function get_wp_location(i)
Expand Down

0 comments on commit 18d964a

Please sign in to comment.