From c81421e275826743066b61ef0a6cce37995a62d2 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 25 May 2013 18:27:57 +1000 Subject: [PATCH] Plane: added MIXING_GAIN parameter this allows for the full range of output on both channels, but can saturate --- ArduPlane/Attitude.pde | 10 +++++----- ArduPlane/Parameters.h | 2 ++ ArduPlane/Parameters.pde | 11 +++++++++-- 3 files changed, 16 insertions(+), 7 deletions(-) diff --git a/ArduPlane/Attitude.pde b/ArduPlane/Attitude.pde index 6642369464..3a76aabc4c 100644 --- a/ArduPlane/Attitude.pde +++ b/ArduPlane/Attitude.pde @@ -512,8 +512,8 @@ static void channel_output_mixer(uint8_t mixing_type, int16_t &chan1_out, int16_ c1 = chan1_out - 1500; c2 = chan2_out - 1500; - v1 = (c1 - c2)/2; - v2 = (c1 + c2)/2; + v1 = (c1 - c2) * g.mixing_gain; + v2 = (c1 + c2) * g.mixing_gain; // now map to mixed output switch (mixing_type) { @@ -537,10 +537,10 @@ static void channel_output_mixer(uint8_t mixing_type, int16_t &chan1_out, int16_ break; } - v1 = constrain_int16(v1, -500, 500); - v2 = constrain_int16(v2, -500, 500); + // scale for a 1500 center and 900..2100 range, symmetric + v1 = constrain_int16(v1, -600, 600); + v2 = constrain_int16(v2, -600, 600); - // scale for a 1500 center and 1000..2000 range, symmetric chan1_out = 1500 + v1; chan2_out = 1500 + v2; } diff --git a/ArduPlane/Parameters.h b/ArduPlane/Parameters.h index 3b4204407a..9353f1f714 100644 --- a/ArduPlane/Parameters.h +++ b/ArduPlane/Parameters.h @@ -87,6 +87,7 @@ public: k_param_nav_controller, k_param_elevon_output, k_param_att_controller, + k_param_mixing_gain, // 110: Telemetry control // @@ -339,6 +340,7 @@ public: AP_Int8 mix_mode; AP_Int8 vtail_output; AP_Int8 elevon_output; + AP_Float mixing_gain; AP_Int8 reverse_elevons; AP_Int8 reverse_ch1_elevon; AP_Int8 reverse_ch2_elevon; diff --git a/ArduPlane/Parameters.pde b/ArduPlane/Parameters.pde index e5a361b8b5..8918721b47 100644 --- a/ArduPlane/Parameters.pde +++ b/ArduPlane/Parameters.pde @@ -483,18 +483,25 @@ const AP_Param::Info var_info[] PROGMEM = { // @Param: VTAIL_OUTPUT // @DisplayName: VTail output - // @Description: Enable VTail output in software. If enabled then the APM will provide software VTail mixing on the elevator and rudder channels. There are 4 different mixing modes available, which refer to the 4 ways the elevator can be mapped to the two VTail servos. Note that you must not use VTail output mixing with hardware pass-through of RC values, such as with channel 8 manual control on an APM1. So if you use an APM1 then set FLTMODE_CH to something other than 8 before you enable VTAIL_OUTPUT. + // @Description: Enable VTail output in software. If enabled then the APM will provide software VTail mixing on the elevator and rudder channels. There are 4 different mixing modes available, which refer to the 4 ways the elevator can be mapped to the two VTail servos. Note that you must not use VTail output mixing with hardware pass-through of RC values, such as with channel 8 manual control on an APM1. So if you use an APM1 then set FLTMODE_CH to something other than 8 before you enable VTAIL_OUTPUT. Please also see the MIXING_GAIN parameter for the output gain of the mixer. // @Values: 0:Disabled,1:UpUp,2:UpDown,3:DownUp,4:DownDown // @User: User GSCALAR(vtail_output, "VTAIL_OUTPUT", 0), // @Param: ELEVON_OUTPUT // @DisplayName: Elevon output - // @Description: Enable software elevon output mixer. If enabled then the APM will provide software elevon mixing on the aileron and elevator channels. There are 4 different mixing modes available, which refer to the 4 ways the elevator can be mapped to the two elevon servos. Note that you must not use elevon output mixing with hardware pass-through of RC values, such as with channel 8 manual control on an APM1. So if you use an APM1 then set FLTMODE_CH to something other than 8 before you enable ELEVON_OUTPUT. + // @Description: Enable software elevon output mixer. If enabled then the APM will provide software elevon mixing on the aileron and elevator channels. There are 4 different mixing modes available, which refer to the 4 ways the elevator can be mapped to the two elevon servos. Note that you must not use elevon output mixing with hardware pass-through of RC values, such as with channel 8 manual control on an APM1. So if you use an APM1 then set FLTMODE_CH to something other than 8 before you enable ELEVON_OUTPUT. Please also see the MIXING_GAIN parameter for the output gain of the mixer. // @Values: 0:Disabled,1:UpUp,2:UpDown,3:DownUp,4:DownDown // @User: User GSCALAR(elevon_output, "ELEVON_OUTPUT", 0), + // @Param: MIXING_GAIN + // @DisplayName: Mixing Gain + // @Description: The gain for the Vtail and elevon output mixers. The default is 1, which behaves like most brands of hardware mixers, but leaves open the possibility of saturating your outputs when both channels are offset from trim by large amounts. Using a smaller mixing ratio (such as 0.5) can guarantee that each channel operates independently. The mixer allows outputs in the range 900 to 2100 microseconds. + // @Range: 0.5 1.2 + // @User: User + GSCALAR(mixing_gain, "MIXING_GAIN", 1), + // @Param: SYS_NUM_RESETS // @DisplayName: Num Resets // @Description: Number of APM board resets