From b31d2a59dae0ac83dd1d0f2fdec04fa78762fbe7 Mon Sep 17 00:00:00 2001 From: Pierre Kancir Date: Mon, 16 Jan 2017 10:19:28 +0100 Subject: [PATCH] APMRover2: Fix skid steer in when in skid steer out --- APMrover2/Steering.cpp | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/APMrover2/Steering.cpp b/APMrover2/Steering.cpp index 579066ee32..a326c453c2 100644 --- a/APMrover2/Steering.cpp +++ b/APMrover2/Steering.cpp @@ -242,6 +242,7 @@ void Rover::calc_nav_steer() { *****************************************/ void Rover::set_servos(void) { static uint16_t last_throttle; + bool apply_skid_mix = true; // Normaly true, false when the mixage is done by the controler with skid_steer_in = 1 if (control_mode == MANUAL || control_mode == LEARNING) { // do a direct pass through of radio values @@ -255,6 +256,9 @@ void Rover::set_servos(void) { SRV_Channels::set_output_pwm(SRV_Channel::k_steering, channel_steer->get_radio_trim()); } } + if (g.skid_steer_in) { + apply_skid_mix = false; + } } else { if (in_reverse) { SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, constrain_int16(SRV_Channels::get_output_scaled(SRV_Channel::k_throttle), @@ -303,6 +307,11 @@ void Rover::set_servos(void) { const float throttle_scaled = SRV_Channels::get_output_norm(SRV_Channel::k_throttle); float motor1 = throttle_scaled + 0.5f * steering_scaled; float motor2 = throttle_scaled - 0.5f * steering_scaled; + + if (!apply_skid_mix) { // Mixage is already done by a controller so just pass the value to motor + motor1 = steering_scaled; + motor2 = throttle_scaled; + } SRV_Channels::set_output_scaled(SRV_Channel::k_steering, 4500 * motor1); SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, 100 * motor2); }