From 750212eb8fdae1e52ff8282d5a6544f4f8f46a7e Mon Sep 17 00:00:00 2001 From: David Ingraham Date: Sat, 9 Sep 2017 15:49:28 -0700 Subject: [PATCH] Plane: Remove deprecated control surface mixing functions This commit removes the deprecated functions used for calculating elevon and vtail mixes. This code is currently unrechable. The functions that called these were removed in PR #6546. --- ArduPlane/servos.cpp | 95 -------------------------------------------- 1 file changed, 95 deletions(-) diff --git a/ArduPlane/servos.cpp b/ArduPlane/servos.cpp index c4aa35fc06..8be3a8589f 100644 --- a/ArduPlane/servos.cpp +++ b/ArduPlane/servos.cpp @@ -133,101 +133,6 @@ bool Plane::suppress_throttle(void) return true; } -/* - implement a software VTail or elevon mixer. There are 4 different mixing modes - */ -void Plane::channel_output_mixer_pwm(uint8_t mixing_type, uint16_t & chan1_out, uint16_t & chan2_out) const -{ - int16_t c1, c2; - int16_t v1, v2; - - // first get desired elevator and rudder as -500..500 values - c1 = chan1_out - 1500; - c2 = chan2_out - 1500; - - // apply MIXING_OFFSET to input channels using long-integer version - // of formula: x = x * (g.mixing_offset/100.0 + 1.0) - // -100 => 2x on 'c1', 100 => 2x on 'c2' - if (g.mixing_offset < 0) { - c1 = (int16_t)(((int32_t)c1) * (-g.mixing_offset+100) / 100); - } else if (g.mixing_offset > 0) { - c2 = (int16_t)(((int32_t)c2) * (g.mixing_offset+100) / 100); - } - - v1 = (c1 - c2) * g.mixing_gain; - v2 = (c1 + c2) * g.mixing_gain; - - // now map to mixed output - switch (mixing_type) { - case MIXING_DISABLED: - return; - - case MIXING_UPUP: - break; - - case MIXING_UPDN: - v2 = -v2; - break; - - case MIXING_DNUP: - v1 = -v1; - break; - - case MIXING_DNDN: - v1 = -v1; - v2 = -v2; - break; - - case MIXING_UPUP_SWP: - std::swap(v1, v2); - break; - - case MIXING_UPDN_SWP: - v2 = -v2; - std::swap(v1, v2); - break; - - case MIXING_DNUP_SWP: - v1 = -v1; - std::swap(v1, v2); - break; - - case MIXING_DNDN_SWP: - v1 = -v1; - v2 = -v2; - std::swap(v1, v2); - break; - } - - // scale for a 1500 center and 900..2100 range, symmetric - v1 = constrain_int16(v1, -600, 600); - v2 = constrain_int16(v2, -600, 600); - - chan1_out = 1500 + v1; - chan2_out = 1500 + v2; -} - -/* - output mixer based on two channel output types - */ -void Plane::channel_output_mixer(uint8_t mixing_type, SRV_Channel::Aux_servo_function_t func1, SRV_Channel::Aux_servo_function_t func2) -{ - SRV_Channel *chan1, *chan2; - if (!(chan1 = SRV_Channels::get_channel_for(func1)) || - !(chan2 = SRV_Channels::get_channel_for(func2))) { - return; - } - - uint16_t chan1_out, chan2_out; - chan1_out = chan1->get_output_pwm(); - chan2_out = chan2->get_output_pwm(); - - channel_output_mixer_pwm(mixing_type, chan1_out, chan2_out); - - chan1->set_output_pwm(chan1_out); - chan2->set_output_pwm(chan2_out); -} - /* mixer for elevon and vtail channels setup using designated servo