From 8f275ca2c4f868ace2c0339736c2595bf4cdb9ff Mon Sep 17 00:00:00 2001 From: Robert Lefebvre Date: Sat, 23 May 2015 18:57:17 -0400 Subject: [PATCH] Copter: Employ heli_radio_passthrough() for servo setup --- ArduCopter/flight_mode.pde | 3 +++ ArduCopter/heli.pde | 6 ++++++ ArduCopter/heli_control_acro.pde | 3 +++ ArduCopter/heli_control_stabilize.pde | 5 ++++- 4 files changed, 16 insertions(+), 1 deletion(-) diff --git a/ArduCopter/flight_mode.pde b/ArduCopter/flight_mode.pde index 08c49730a6..79af1534e4 100644 --- a/ArduCopter/flight_mode.pde +++ b/ArduCopter/flight_mode.pde @@ -241,6 +241,9 @@ static void exit_mode(uint8_t old_control_mode, uint8_t new_control_mode) if (old_control_mode == ACRO) { attitude_control.use_flybar_passthrough(false); } + + // reset RC Passthrough to motors + motors.reset_radio_passthrough(); #endif //HELI_FRAME } diff --git a/ArduCopter/heli.pde b/ArduCopter/heli.pde index 7ec0c532a3..870fe138b0 100644 --- a/ArduCopter/heli.pde +++ b/ArduCopter/heli.pde @@ -157,4 +157,10 @@ static void heli_update_rotor_speed_targets() } } +// heli_radio_passthrough send RC inputs direct into motors library for use during manual passthrough for helicopter setup +static void heli_radio_passthrough() +{ + motors.set_radio_passthrough(channel_roll->control_in, channel_pitch->control_in, channel_throttle->control_in, channel_yaw->control_in); +} + #endif // FRAME_CONFIG == HELI_FRAME diff --git a/ArduCopter/heli_control_acro.pde b/ArduCopter/heli_control_acro.pde index 9b7a2bdea5..27f15411ee 100644 --- a/ArduCopter/heli_control_acro.pde +++ b/ArduCopter/heli_control_acro.pde @@ -36,6 +36,9 @@ static void heli_acro_run() attitude_control.relax_bf_rate_controller(); } + // send RC inputs direct into motors library for use during manual passthrough for helicopter setup + heli_radio_passthrough(); + if (!motors.has_flybar()){ // convert the input to the desired body frame rate get_pilot_desired_angle_rates(channel_roll->control_in, channel_pitch->control_in, channel_yaw->control_in, target_roll, target_pitch, target_yaw); diff --git a/ArduCopter/heli_control_stabilize.pde b/ArduCopter/heli_control_stabilize.pde index c2ce2c1fb4..bee3428b59 100644 --- a/ArduCopter/heli_control_stabilize.pde +++ b/ArduCopter/heli_control_stabilize.pde @@ -36,7 +36,10 @@ static void heli_stabilize_run() heli_flags.init_targets_on_arming=false; attitude_control.relax_bf_rate_controller(); } - + + // send RC inputs direct into motors library for use during manual passthrough for helicopter setup + heli_radio_passthrough(); + // apply SIMPLE mode transform to pilot inputs update_simple_mode();