Plane: fixed px4_mixer for new function based mixing

this fixes the OVERRIDE_CHAN functionality to work correctly with the
new function based mixing code

It isn't a perfect match for ArduPilot mixing, but it is very close
for the key control surfaces, including mixed surfaces

ArduPlane/Plane.h

@@ -937,6 +937,10 @@ private:
     bool reached_loiter_target(void);
     bool print_buffer(char *&buf, uint16_t &buf_size, const char *fmt, ...);
     uint16_t create_mixer(char *buf, uint16_t buf_size, const char *filename);
+    bool mix_one_channel(char *&buf, uint16_t &buf_size, uint8_t out_chan, uint8_t in_chan);
+    bool mix_two_channels(char *&buf, uint16_t &buf_size, uint8_t out_chan, uint8_t in_chan1, uint8_t in_chan2, bool left_channel);
+    bool mix_passthrough(char *&buf, uint16_t &buf_size, uint8_t out_chan, uint8_t in_chan);
+    bool mix_trim_channel(char *&buf, uint16_t &buf_size, uint8_t out_chan);
     bool setup_failsafe_mixing(void);
     void set_control_channels(void);
     void init_rc_in();

ArduPlane/px4_mixer.cpp

@@ -20,6 +20,7 @@
 #include <systemlib/mixer/mixer.h>
 #include <modules/px4iofirmware/protocol.h>
 #include <GCS_MAVLink/include/mavlink/v2.0/checksum.h>
+#include <utility>
 
 #define PX4_LIM_RC_MIN 900
 #define PX4_LIM_RC_MAX 2100
@@ -42,6 +43,165 @@ bool Plane::print_buffer(char *&buf, uint16_t &buf_size, const char *fmt, ...)
     return true;
 }
 
+/* 
+   create a mixer for a normal angle channel
+*/
+bool Plane::mix_one_channel(char *&buf, uint16_t &buf_size, uint8_t out_chan, uint8_t in_chan)
+{
+    const float limit = 10000;
+    const RC_Channel *inch = RC_Channels::rc_channel(in_chan);
+    const SRV_Channel *outch = SRV_Channels::srv_channel(out_chan);
+    const float scale_out = limit*(outch->get_output_max() - outch->get_output_min()) / (PX4_LIM_RC_MAX - PX4_LIM_RC_MIN);
+
+    bool is_throttle = in_chan==rcmap.throttle()-1;
+    int16_t outch_trim = is_throttle?1500:outch->get_trim();
+
+    outch_trim = constrain_int16(outch_trim, outch->get_output_min()+1, outch->get_output_max()-1);
+
+    if (!print_buffer(buf, buf_size, "M: 1\n")) {
+        return false;
+    }
+
+    int32_t out_min = scale_out*(1500 - outch->get_output_min()) / (1500 - PX4_LIM_RC_MIN);
+    int32_t out_max = scale_out*(outch->get_output_max() - 1500) / (PX4_LIM_RC_MAX - 1500);
+    int32_t out_trim = scale_out*(outch_trim - 1500) / ((PX4_LIM_RC_MAX - PX4_LIM_RC_MIN) / 2);
+    int32_t in_mul = inch->get_reverse() == outch->get_reversed()?1:-1;
+
+    // note that RC trim is taken care of in rc_config
+    float scale_low =  limit*(1500 - PX4_LIM_RC_MIN)/(1500 - inch->get_radio_min());
+    float scale_high = limit*(PX4_LIM_RC_MAX - 1500)/(inch->get_radio_max() - 1500);
+    
+    if (!print_buffer(buf, buf_size, "O: %d %d %d %d %d\n",
+                      int(out_min),
+                      int(out_max),
+                      int(out_trim),
+                      int(-limit), int(limit))) {
+        return false;
+    }
+    if (!print_buffer(buf, buf_size, "S: 0 %u %d %d %d %d %d\n",
+                      in_chan,
+                      int(in_mul*scale_low), int(in_mul*scale_high),
+                      0,
+                      int(-limit), int(limit))) {
+        return false;
+    }
+    return true;
+}
+
+/*
+  mix two channels using elevon style mixer
+*/
+bool Plane::mix_two_channels(char *&buf, uint16_t &buf_size, uint8_t out_chan, uint8_t in_chan1, uint8_t in_chan2, bool left_channel)
+{
+    const float limit = 10000;
+    const RC_Channel *inch1 = RC_Channels::rc_channel(in_chan1);
+    const RC_Channel *inch2 = RC_Channels::rc_channel(in_chan2);
+    const SRV_Channel *outch = SRV_Channels::srv_channel(out_chan);
+    const float scale_out = limit*(outch->get_output_max() - outch->get_output_min()) / (PX4_LIM_RC_MAX - PX4_LIM_RC_MIN);
+    int16_t outch_trim = outch->get_trim();
+
+    outch_trim = constrain_int16(outch_trim, outch->get_output_min()+1, outch->get_output_max()-1);
+
+    if (!print_buffer(buf, buf_size, "M: 2\n")) {
+        return false;
+    }
+
+    int32_t out_min = scale_out*(1500 - outch->get_output_min()) / (1500 - PX4_LIM_RC_MIN);
+    int32_t out_max = scale_out*(outch->get_output_max() - 1500) / (PX4_LIM_RC_MAX - 1500);
+    int32_t out_trim = scale_out*(outch_trim - 1500) / ((PX4_LIM_RC_MAX - PX4_LIM_RC_MIN) / 2);
+    int32_t in_mul1 = inch1->get_reverse() == outch->get_reversed()?1:-1;
+    int32_t in_mul2 = inch2->get_reverse() == outch->get_reversed()?1:-1;
+    float in_gain = g.mixing_gain;
+
+    if (left_channel) {
+        in_mul2 = -in_mul2;
+    }
+
+    // note that RC trim is taken care of in rc_config
+    float scale_low1 =  limit*(1500 - PX4_LIM_RC_MIN)/(1500 - inch1->get_radio_min());
+    float scale_high1 = limit*(PX4_LIM_RC_MAX - 1500)/(inch1->get_radio_max() - 1500);
+    float scale_low2 =  limit*(1500 - PX4_LIM_RC_MIN)/(1500 - inch2->get_radio_min());
+    float scale_high2 = limit*(PX4_LIM_RC_MAX - 1500)/(inch2->get_radio_max() - 1500);
+    
+    if (!print_buffer(buf, buf_size, "O: %d %d %d %d %d\n",
+                      int(out_min),
+                      int(out_max),
+                      int(out_trim),
+                      int(-limit*2), int(limit*2))) {
+        return false;
+    }
+    if (!print_buffer(buf, buf_size, "S: 0 %u %d %d %d %d %d\n",
+                      in_chan1,
+                      int(in_mul1*in_gain*scale_low1), int(in_mul1*in_gain*scale_high1),
+                      0,
+                      int(-limit*2), int(limit*2))) {
+        return false;
+    }
+    if (!print_buffer(buf, buf_size, "S: 0 %u %d %d %d %d %d\n",
+                      in_chan2,
+                      int(in_mul2*in_gain*scale_low2), int(in_mul2*in_gain*scale_high2),
+                      0,
+                      int(-limit*2), int(limit*2))) {
+        return false;
+    }
+    return true;
+}
+
+/* 
+   create a mixer for k_manual and k_rcin*
+*/
+bool Plane::mix_passthrough(char *&buf, uint16_t &buf_size, uint8_t out_chan, uint8_t in_chan)
+{
+    const float limit = 10000;
+
+    if (!print_buffer(buf, buf_size, "M: 1\n")) {
+        return false;
+    }
+
+    if (!print_buffer(buf, buf_size, "O: %d %d %d %d %d\n",
+                      int(limit),
+                      int(limit),
+                      0,
+                      int(-limit), int(limit))) {
+        return false;
+    }
+    if (!print_buffer(buf, buf_size, "S: 0 %u %d %d %d %d %d\n",
+                      in_chan,
+                      int(limit), int(limit),
+                      0,
+                      int(-limit), int(limit))) {
+        return false;
+    }
+    return true;
+}
+
+/* 
+   create a mixer for outputting trim only
+*/
+bool Plane::mix_trim_channel(char *&buf, uint16_t &buf_size, uint8_t out_chan)
+{
+    const float limit = 10000;
+    const SRV_Channel *outch = SRV_Channels::srv_channel(out_chan);
+
+    int16_t outch_trim = outch->get_trim();
+    outch_trim = constrain_int16(outch_trim, outch->get_output_min()+1, outch->get_output_max()-1);
+
+    if (!print_buffer(buf, buf_size, "M: 0\n")) {
+        return false;
+    }
+
+    int32_t out_trim = limit*(outch_trim - 1500) / ((PX4_LIM_RC_MAX - PX4_LIM_RC_MIN) / 2);
+
+    if (!print_buffer(buf, buf_size, "O: %d %d %d %d %d\n",
+                      int(limit),
+                      int(limit),
+                      int(out_trim),
+                      int(-limit), int(limit))) {
+        return false;
+    }
+    return true;
+}
+
 /*
   create a PX4 mixer buffer given the current fixed wing parameters, returns the size of the buffer used
  */
@@ -50,165 +210,49 @@ uint16_t Plane::create_mixer(char *buf, uint16_t buf_size, const char *filename)
     char *buf0 = buf;
     uint16_t buf_size0 = buf_size;
 
-    /*
-      this is the equivalent of channel_output_mixer()
-     */
-    const int8_t mixmul[5][2] = { { 0, 0 }, { 1, 1 }, { 1, -1 }, { -1, 1 }, { -1, -1 }};
-    // these are the internal clipping limits. Use scale_max1 when
-    // clipping to user specified min/max is wanted. Use scale_max2
-    // when no clipping is wanted (simulated by setting a very large
-    // clipping value)
-    const float scale_max1 = 10000;
-    const float scale_max2 = 1000000;
-    // range for mixers
-    const uint16_t mix_max = scale_max1 * g.mixing_gain;
-    // scaling factors used by PX4IO between pwm and internal values,
-    // as configured in setup_failsafe_mixing() below
-    const float pwm_min = PX4_LIM_RC_MIN;
-    const float pwm_max = PX4_LIM_RC_MAX;
-    const float pwm_scale = 2*scale_max1/(pwm_max - pwm_min);
-
     for (uint8_t i=0; i<8; i++) {
-        int32_t c1, c2, mix=0;
-        bool rev = false;
         SRV_Channel::Aux_servo_function_t function = SRV_Channels::channel_function(i);
-        if (i == rcmap.pitch()-1 && g.vtail_output > MIXING_DISABLED && g.vtail_output <= MIXING_DNDN) {
+        switch (function) {
-            // first channel of VTAIL mix
+        case SRV_Channel::k_aileron:
-            c1 = rcmap.yaw()-1;
+        case SRV_Channel::k_flaperon_left:
-            c2 = i;
+        case SRV_Channel::k_flaperon_right:
-            rev = false;
+            mix_one_channel(buf, buf_size, i, rcmap.roll()-1);
-            mix = -mix_max*mixmul[g.vtail_output][0];
+            break;
-        } else if (i == rcmap.yaw()-1 && g.vtail_output > MIXING_DISABLED && g.vtail_output <= MIXING_DNDN) {
+        case SRV_Channel::k_elevator:
-            // second channel of VTAIL mix
+            mix_one_channel(buf, buf_size, i, rcmap.pitch()-1);
-            c1 = rcmap.pitch()-1;
+            break;
-            c2 = i;
+        case SRV_Channel::k_throttle:
-            rev = true;
+            mix_one_channel(buf, buf_size, i, rcmap.throttle()-1);
-            mix = mix_max*mixmul[g.vtail_output][1];
+            break;
-        } else if (i == rcmap.roll()-1 && g.elevon_output > MIXING_DISABLED && 
+        case SRV_Channel::k_rudder:
-                   g.elevon_output <= MIXING_DNDN && g.vtail_output == 0) {
+        case SRV_Channel::k_steering:
-            // first channel of ELEVON mix
+            mix_one_channel(buf, buf_size, i, rcmap.yaw()-1);
-            c1 = i;
+            break;
-            c2 = rcmap.pitch()-1;
+        case SRV_Channel::k_elevon_left:
-            rev = true;
+        case SRV_Channel::k_dspoilerLeft1:
-            mix = mix_max*mixmul[g.elevon_output][1];
+        case SRV_Channel::k_dspoilerLeft2:
-        } else if (i == rcmap.pitch()-1 && g.elevon_output > MIXING_DISABLED && 
+            mix_two_channels(buf, buf_size, i, rcmap.pitch()-1, rcmap.roll()-1, true);
-                   g.elevon_output <= MIXING_DNDN && g.vtail_output == 0) {
+            break;
-            // second channel of ELEVON mix
+        case SRV_Channel::k_elevon_right:
-            c1 = i;
+        case SRV_Channel::k_dspoilerRight1:
-            c2 = rcmap.roll()-1;
+        case SRV_Channel::k_dspoilerRight2:
-            rev = false;
+            mix_two_channels(buf, buf_size, i, rcmap.pitch()-1, rcmap.roll()-1, false);
-            mix = mix_max*mixmul[g.elevon_output][0];
+            break;
-        } else if (function == SRV_Channel::k_aileron || 
+        case SRV_Channel::k_vtail_left:
-                   function == SRV_Channel::k_flaperon_left || 
+            mix_two_channels(buf, buf_size, i, rcmap.pitch()-1, rcmap.yaw()-1, true);
-                   function == SRV_Channel::k_flaperon_right ||
+            break;
-                   function == SRV_Channel::k_aileron_with_input) {
+        case SRV_Channel::k_vtail_right:
-            // a secondary aileron. We don't mix flap input in yet for flaperons
+            mix_two_channels(buf, buf_size, i, rcmap.pitch()-1, rcmap.yaw()-1, false);
-            c1 = rcmap.roll()-1;
+            break;
-        } else if (function == SRV_Channel::k_elevator ||
+        case SRV_Channel::k_manual:
-                   function == SRV_Channel::k_elevator_with_input) {
+            mix_passthrough(buf, buf_size, i, i);
-            // a secondary elevator
+            break;
-            c1 = rcmap.pitch()-1;
+        case SRV_Channel::k_rcin1 ... SRV_Channel::k_rcin16:
-        } else if (function == SRV_Channel::k_rudder || 
+            mix_passthrough(buf, buf_size, i, uint8_t(function - SRV_Channel::k_rcin1));
-                   function == SRV_Channel::k_steering) {
+            break;
-            // a secondary rudder or wheel
+        default:
-            c1 = rcmap.yaw()-1;
+            mix_trim_channel(buf, buf_size, i);
-        } else if (g.flapin_channel > 0 &&
+            break;
-                   (function == SRV_Channel::k_flap ||
-                    function == SRV_Channel::k_flap_auto)) {
-            // a flap output channel, and we have a manual flap input channel
-            c1 = g.flapin_channel-1;
-        } else if (i < 4 ||
-                   function == SRV_Channel::k_manual) {
-            // a pass-thru channel
-            c1 = i;
-        } else {
-            // a empty output
-            if (!print_buffer(buf, buf_size, "Z:\n")) {
-                return 0;
-            }
-            continue;
-        }
-        if (mix == 0) {
-            // pass through channel, possibly with reversal. We also
-            // adjust the gain based on the range of input and output
-            // channels and adjust for trims
-            const RC_Channel *chan1 = RC_Channels::rc_channel(i);
-            const RC_Channel *chan2 = RC_Channels::rc_channel(c1);
-            int16_t chan1_trim = (i==rcmap.throttle()-1?1500:chan1->get_radio_trim());
-            int16_t chan2_trim = (c1==rcmap.throttle()-1?1500:chan2->get_radio_trim());
-            chan1_trim = constrain_int16(chan1_trim, PX4_LIM_RC_MIN+1, PX4_LIM_RC_MAX-1);
-            chan2_trim = constrain_int16(chan2_trim, PX4_LIM_RC_MIN+1, PX4_LIM_RC_MAX-1);
-            // if the input and output channels are the same then we
-            // apply clipping. This allows for direct pass-thru
-            int32_t limit = (c1==i?scale_max2:scale_max1);
-            int32_t in_scale_low;
-            if (chan2_trim <= chan2->get_radio_min()) {
-                in_scale_low = scale_max1;
-            } else {
-                in_scale_low = scale_max1*(chan2_trim - pwm_min)/(float)(chan2_trim - chan2->get_radio_min());
-            }
-            int32_t in_scale_high;
-            if (chan2->get_radio_max() <= chan2_trim) {
-                in_scale_high = scale_max1;
-            } else {
-                in_scale_high = scale_max1*(pwm_max - chan2_trim)/(float)(chan2->get_radio_max() - chan2_trim);
-            }
-            if (chan1->get_reverse() != chan2->get_reverse()) {
-                in_scale_low = -in_scale_low;
-                in_scale_high = -in_scale_high;
-            }
-            if (!print_buffer(buf, buf_size, "M: 1\n") ||
-                !print_buffer(buf, buf_size, "O: %d %d %d %d %d\n",
-                              (int)(pwm_scale*(chan1_trim - chan1->get_radio_min())),
-                              (int)(pwm_scale*(chan1->get_radio_max() - chan1_trim)),
-                              (int)(pwm_scale*(chan1_trim - 1500)),
-                              (int)-scale_max2, (int)scale_max2) ||
-                !print_buffer(buf, buf_size, "S: 0 %u %d %d %d %d %d\n", c1,
-                              in_scale_low,
-                              in_scale_high,
-                              0,
-                              -limit, limit)) {
-                return 0;
-            }
-        } else {
-            const RC_Channel *chan1 = RC_Channels::rc_channel(c1);
-            const RC_Channel *chan2 = RC_Channels::rc_channel(c2);
-            int16_t chan1_trim = (c1==rcmap.throttle()-1?1500:chan1->get_radio_trim());
-            int16_t chan2_trim = (c2==rcmap.throttle()-1?1500:chan2->get_radio_trim());
-            chan1_trim = constrain_int16(chan1_trim, PX4_LIM_RC_MIN+1, PX4_LIM_RC_MAX-1);
-            chan2_trim = constrain_int16(chan2_trim, PX4_LIM_RC_MIN+1, PX4_LIM_RC_MAX-1);
-            // mix of two input channels to give an output channel. To
-            // make the mixer match the behaviour of APM we need to
-            // scale and offset the input channels to undo the affects
-            // of the PX4IO input processing
-            if (!print_buffer(buf, buf_size, "M: 2\n") ||
-                !print_buffer(buf, buf_size, "O: %d %d 0 %d %d\n", mix, mix, (int)-scale_max1, (int)scale_max1)) {
-                return 0;
-            }
-            int32_t in_scale_low = pwm_scale*(chan1_trim - pwm_min);
-            int32_t in_scale_high = pwm_scale*(pwm_max - chan1_trim);
-            int32_t offset = pwm_scale*(chan1_trim - 1500);
-            if (!print_buffer(buf, buf_size, "S: 0 %u %d %d %d %d %d\n", 
-                              c1, in_scale_low, in_scale_high, offset,
-                              (int)-scale_max2, (int)scale_max2)) {
-                return 0;
-            }
-            in_scale_low = pwm_scale*(chan2_trim - pwm_min);
-            in_scale_high = pwm_scale*(pwm_max - chan2_trim);
-            offset = pwm_scale*(chan2_trim - 1500);
-            if (rev) {
-                if (!print_buffer(buf, buf_size, "S: 0 %u %d %d %d %d %d\n", 
-                                  c2, in_scale_low, in_scale_high, offset,
-                                  (int)-scale_max2, (int)scale_max2)) {
-                    return 0;
-                }
-            } else {
-                if (!print_buffer(buf, buf_size, "S: 0 %u %d %d %d %d %d\n", 
-                                  c2, -in_scale_low, -in_scale_high, -offset,
-                                  (int)-scale_max2, (int)scale_max2)) {
-                    return 0;
-                }
-            }
         }
     }    
 
@@ -299,16 +343,11 @@ bool Plane::setup_failsafe_mixing(void)
             continue;
         }
         struct pwm_output_rc_config config;
-        /*
-          we use a min/max of 900/2100 to allow for pass-thru of
-          larger values than the RC min/max range. This mimics the APM
-          behaviour of pass-thru in manual, which allows for dual-rate
-          transmitter setups in manual mode to go beyond the ranges
-          used in stabilised modes
-         */
         config.channel = i;
-        config.rc_min = 900;
+        // use high rc limits to allow for correct pass-thru channels
-        config.rc_max = 2100;
+        // without limits
+        config.rc_min = PX4_LIM_RC_MIN;
+        config.rc_max = PX4_LIM_RC_MAX;
         if (rcmap.throttle()-1 == i) {
             // throttle uses a trim of 1500, so we don't get division
             // by small numbers near RC3_MIN
@@ -318,16 +357,8 @@ bool Plane::setup_failsafe_mixing(void)
         }
         config.rc_dz = 0; // zero for the purposes of manual takeover
 
-        // we set reverse as false, as users of ArduPilot will have
+        // undo the reversal of channel2 in px4io
-        // input reversed on transmitter, so from the point of view of
+        config.rc_reverse = i==1?true:false;
-        // the mixer the input is never reversed. The one exception is
-        // the 2nd channel, which is reversed inside the PX4IO code,
-        // so needs to be unreversed here to give sane behaviour.
-        if (i == 1) {
-            config.rc_reverse = true;
-        } else {
-            config.rc_reverse = false;
-        }
 
         if (i+1 == g.override_channel.get()) {
             /*
@@ -357,7 +388,7 @@ bool Plane::setup_failsafe_mixing(void)
         if (SRV_Channel::is_motor(SRV_Channels::channel_function(i))) {
             pwm_values.values[i] = quadplane.thr_min_pwm;
         } else {
-            pwm_values.values[i] = 900;
+            pwm_values.values[i] = PX4_LIM_RC_MIN;
         }
     }
     if (ioctl(px4io_fd, PWM_SERVO_SET_MIN_PWM, (long unsigned int)&pwm_values) != 0) {
@@ -370,7 +401,7 @@ bool Plane::setup_failsafe_mixing(void)
             hal.rcout->write(i, quadplane.thr_min_pwm);
             pwm_values.values[i] = quadplane.thr_min_pwm;
         } else {
-            pwm_values.values[i] = 2100;
+            pwm_values.values[i] = PX4_LIM_RC_MAX;
         }
     }
     if (ioctl(px4io_fd, PWM_SERVO_SET_MAX_PWM, (long unsigned int)&pwm_values) != 0) {