diff --git a/docs/Cli.md b/docs/Cli.md
index 040b1cc2bf..4e1e4b5901 100644
--- a/docs/Cli.md
+++ b/docs/Cli.md
@@ -75,7 +75,8 @@ Re-apply any new defaults as desired.
 |------------------|------------------------------------------------|
 | `adjrange`       | show/set adjustment ranges settings            |
 | `aux`            | show/set aux settings                          |
-| `cmix`           | design custom mixer                            |
+| `mmix`           | design custom motor mixer                      |
+| `smix`           | design custom servo mixer                      |
 | `color`          | configure colors                               |
 | `defaults`       | reset to defaults and reboot                   |
 | `dump`           | print configurable settings in a pastable form |
@@ -200,7 +201,7 @@ Re-apply any new defaults as desired.
 | `failsafe_throttle`             |                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                        | 1000   | 2000   | 1200          | Profile      | UINT16   |
 | `rx_min_usec`                   | Defines the shortest pulse width value used when ensuring the channel value is valid.  If the receiver gives a pulse value lower than this value then the channel will be marked as bad and will default to the value of `mid_rc`.                                                                                                                                                                                                                                                                                                                                                                                                                     | 100    | 2000   | 985           | Profile      | UINT16   |
 | `rx_max_usec`                   | Defines the longest pulse width value used when ensuring the channel value is valid.  If the receiver gives a pulse value higher than this value then the channel will be marked as bad and will default to the value of `mid_rc`.                                                                                                                                                                                                                                                                                                                                                                                                                     | 100    | 3000   | 2115          | Profile      | UINT16   |
-| `gimbal_flags`                  | When feature SERVO_TILT is enabled, this can be a combination of the following numbers: 1=normal gimbal (default), 2=tiltmix gimbal, 4=in PPM (or SERIALRX) input mode, this will forward AUX1..4 RC inputs to PWM5..8 pins                                                                                                                                                                                                                                                                                                                                                                                                                            | 0      | 255    | 1             | Profile      | UINT8    |
+| `gimbal_mode`                  | When feature SERVO_TILT is enabled, this can be either 0=normal gimbal (default) or 1=tiltmix gimbal                                                                                                                                                                                                                                                                                                    | 0      | 1      | 0             | Profile      | UINT8    |
 | `acc_hardware`                  | This is used to suggest which accelerometer driver should load, or to force no accelerometer in case gyro-only flight is needed. Default (0) will attempt to auto-detect among enabled drivers. Otherwise, to force a particular device, set it to 2 for ADXL345, 3 for MPU6050 integrated accelerometer, 4 for MMA8452, 5 for BMA280, 6 for LSM303DLHC, 7 for SPI_MPU6000, 8 for SPI_MPU6500 or 1 to disable accelerometer alltogether - resulting in gyro-only operation.                                                                                                                                                                            | 0      | 9      | 0             | Master       | UINT8    |
 | `acc_lpf_factor`                | This setting controls the Low Pass Filter factor for ACC.  Increasing this value reduces ACC noise (visible in GUI), but would increase ACC lag time.  Zero = no filter                                                                                                                                                                                                                                                                                                                                                                                                                                                                                | 0      | 250    | 4             | Profile      | UINT8    |
 | `accxy_deadband`                |                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                        | 0      | 100    | 40            | Profile      | UINT8    |
diff --git a/docs/Migrating from baseflight.md b/docs/Migrating from baseflight.md
index 9f8b022f17..2ffd11edec 100644
--- a/docs/Migrating from baseflight.md	
+++ b/docs/Migrating from baseflight.md	
@@ -98,3 +98,8 @@ reason: renamed to `3d_neutral` for consistency
 
 ### alt_hold_throttle_neutral
 reason: renamed to `alt_hold_deadband` for consistency
+
+### gimbal_flags
+reason: seperation of features.
+
+see `gimbal_mode` and `CHANNEL_FORWARDING` feature
diff --git a/docs/Mixer.md b/docs/Mixer.md
index d34cb9e07e..0f76deb311 100644
--- a/docs/Mixer.md
+++ b/docs/Mixer.md
@@ -14,31 +14,33 @@ You can also use the Command Line Interface (CLI) to set the mixer type:
 
 ## Supported Mixer Types
 
-| Name          | Description               | Motors         | Servos           |
-| ------------- | ------------------------- | -------------- | ---------------- |
-| TRI           | Tricopter                 | M1-M3          | S1               |
-| QUADP         | Quadcopter-Plus           | M1-M4          | None             |
-| QUADX         | Quadcopter-X              | M1-M4          | None             |
-| BI            | Bicopter (left/right)     | M1-M2          | S1, S2           |
-| GIMBAL        | Gimbal control            | N/A            | S1, S2           |
-| Y6            | Y6-copter                 | M1-M6          | None             |
-| HEX6          | Hexacopter-Plus           | M1-M6          | None             |
-| FLYING_WING   | Fixed wing; elevons       | M1             | S1, S2           |
-| Y4            | Y4-copter                 | M1-M4          | None             |
-| HEX6X         | Hexacopter-X              | M1-M6          | None             |
-| OCTOX8        | Octocopter-X (over/under) | M1-M8          | None             |
-| OCTOFLATP     | Octocopter-FlatPlus       | M1-M8          | None             |
-| OCTOFLATX     | Octocopter-FlatX          | M1-M8          | None             |
-| AIRPLANE      | Fixed wing; Ax2, R, E     | M1             | S1, S2, S3, S4   |
-| HELI_120_CCPM |                           |                |                  |
-| HELI_90_DEG   |                           |                |                  |
-| VTAIL4        | Quadcopter with V-Tail    | M1-M4          | N/A              |
-| HEX6H         | Hexacopter-H              | M1-M6          | None             |
-| PPM_TO_SERVO  |                           |                |                  |
-| DUALCOPTER    | Dualcopter                | M1-M2          | S1, S2           |
-| SINGLECOPTER  | Conventional helicopter   | M1             | S1               |
-| ATAIL4        | Quadcopter with A-Tail    | M1-M4          | N/A              |
-| CUSTOM        | User-defined              |                |                  |
+| Name             | Description               | Motors         | Servos           |
+| ---------------- | ------------------------- | -------------- | ---------------- |
+| TRI              | Tricopter                 | M1-M3          | S1               |
+| QUADP            | Quadcopter-Plus           | M1-M4          | None             |
+| QUADX            | Quadcopter-X              | M1-M4          | None             |
+| BI               | Bicopter (left/right)     | M1-M2          | S1, S2           |
+| GIMBAL           | Gimbal control            | N/A            | S1, S2           |
+| Y6               | Y6-copter                 | M1-M6          | None             |
+| HEX6             | Hexacopter-Plus           | M1-M6          | None             |
+| FLYING_WING      | Fixed wing; elevons       | M1             | S1, S2           |
+| Y4               | Y4-copter                 | M1-M4          | None             |
+| HEX6X            | Hexacopter-X              | M1-M6          | None             |
+| OCTOX8           | Octocopter-X (over/under) | M1-M8          | None             |
+| OCTOFLATP        | Octocopter-FlatPlus       | M1-M8          | None             |
+| OCTOFLATX        | Octocopter-FlatX          | M1-M8          | None             |
+| AIRPLANE         | Fixed wing; Ax2, R, E     | M1             | S1, S2, S3, S4   |
+| HELI_120_CCPM    |                           |                |                  |
+| HELI_90_DEG      |                           |                |                  |
+| VTAIL4           | Quadcopter with V-Tail    | M1-M4          | N/A              |
+| HEX6H            | Hexacopter-H              | M1-M6          | None             |
+| PPM_TO_SERVO     |                           |                |                  |
+| DUALCOPTER       | Dualcopter                | M1-M2          | S1, S2           |
+| SINGLECOPTER     | Conventional helicopter   | M1             | S1               |
+| ATAIL4           | Quadcopter with A-Tail    | M1-M4          | N/A              |
+| CUSTOM           | User-defined              |                |                  |
+| CUSTOM AIRPLANE  | User-defined airplane     |                |                  |
+| CUSTOM TRICOPTER | User-defined tricopter    |                |                  |
 
 
 ## Servo filtering
@@ -76,8 +78,8 @@ Custom motor mixing allows for completely customized motor configurations. Each
 Steps to configure custom mixer in the CLI:
 
 1. Use `mixer custom` to enable the custom mixing.
-2. Use `cmix reset` to erase the any existing custom mixing. 
-3. Issue a cmix statement for each motor. 
+2. Use `mmix reset` to erase the any existing custom mixing. 
+3. Issue a `mmix` statement for each motor. 
 
 The cmix statement has the following syntax: `cmix n THROTTLE ROLL PITCH YAW` 
 
@@ -89,6 +91,42 @@ The cmix statement has the following syntax: `cmix n THROTTLE ROLL PITCH YAW`
 | PITCH	| Indicates the pitch authority this motor has over the flight controller. Also accepts values nominally from 1.0 to -1.0. |
 | YAW	| Indicates the direction of the motor rotation in relationship with the flight controller. 1.0 = CCW -1.0 = CW. |
 
+Note: the `mmix` command may show a motor mix that is not active, custom motor mixes are only active for models that use custom mixers. 
+
+## Custom Servo Mixing
+
+Custom servo mixing rules can be applied to each servo.  Rules are applied in the order they are defined.
+
+| id | Servo slot |
+| 0  | GIMBAL PITCH |
+| 1  | GIMBAL ROLL |
+| 2  | FLAPS |
+| 3  | FLAPPERON 1 (LEFT) / SINGLECOPTER_1 |
+| 4  | FLAPPERON 2 (RIGHT) / BICOPTER_LEFT / DUALCOPTER_LEFT / SINGLECOPTER_2 |
+| 5  | RUDDER / BICOPTER_RIGHT / DUALCOPTER_RIGHT / SINGLECOPTER_3 |
+| 6  | ELEVATOR / SINGLECOPTER_4 | 
+| 7  | THROTTLE (Based ONLY on the first motor output) | 
+
+
+| id | Input sources |
+| -- | ------------- |
+| 0  | Stabilised ROLL |
+| 1  | Stabilised PITCH |
+| 2  | Stabilised YAW |
+| 3  | Stabilised THROTTLE |
+| 4  | RC ROLL |
+| 5  | RC ITCH |
+| 6  | RC YAW |
+| 7  | RC THROTTLE |
+| 8  | RC AUX 1 |
+| 9  | RC AUX 2 |
+| 10 | RC AUX 3 |
+| 11 | RC AUX 4 |
+| 12 | GIMBAL PITCH |
+| 13 | GIMBAL ROLL |
+
+Note: the `smix` command may show a servo mix that is not active, custom servo mixes are only active for models that use custom mixers. 
+
 ### Example 1: A KK2.0 wired motor setup 
 Here's an example of a X configuration quad, but the motors are still wired using the KK board motor numbering scheme. 
 
@@ -103,11 +141,11 @@ KK2.0 Motor Layout
 ```
 
 1. Use `mixer custom`
-2. Use `cmix reset`
-3. Use `cmix 1 1.0,  1.0, -1.0, -1.0` for the Front Left motor. It tells the flight controller the #1 motor is used, provides positive roll, provides negative pitch and is turning CW.  
-4. Use `cmix 2 1.0, -1.0, -1.0,  1.0` for the Front Right motor. It still provides a negative pitch authority, but unlike the front left, it provides negative roll authority and turns CCW.
-5. Use `cmix 3 1.0, -1.0,  1.0, -1.0` for the Rear Right motor. It has negative roll, provides positive pitch when the speed is increased and turns CW.
-6. Use `cmix 4 1.0,  1.0,  1.0,  1.0` for the Rear Left motor. Increasing motor speed imparts positive roll, positive pitch and turns CCW.
+2. Use `mmix reset`
+3. Use `mmix 0 1.0,  1.0, -1.0, -1.0` for the Front Left motor. It tells the flight controller the #1 motor is used, provides positive roll, provides negative pitch and is turning CW.  
+4. Use `mmix 1 1.0, -1.0, -1.0,  1.0` for the Front Right motor. It still provides a negative pitch authority, but unlike the front left, it provides negative roll authority and turns CCW.
+5. Use `mmix 2 1.0, -1.0,  1.0, -1.0` for the Rear Right motor. It has negative roll, provides positive pitch when the speed is increased and turns CW.
+6. Use `mmix 3 1.0,  1.0,  1.0,  1.0` for the Rear Left motor. Increasing motor speed imparts positive roll, positive pitch and turns CCW.
 
 ### Example 2: A HEX-U Copter 
 
@@ -121,13 +159,37 @@ HEX6-U
 .5...FC...2. 
 ............
 ...6....1...
-  
 ```
+
 |Command| Roll | Pitch | Yaw |
 | ----- | ---- | ----- | --- | 
-| Use `cmix 1 1.0, -0.5,  1.0, -1.0` | half negative | full positive | CW |
-| Use `cmix 2 1.0, -1.0,  0.0,  1.0` | full negative | none | CCW | 
-| Use `cmix 3 1.0, -1.0, -1.0, -1.0` | full negative | full negative | CW | 
-| Use `cmix 4 1.0,  1.0, -1.0,  1.0` | full positive | full negative | CCW  | 
-| Use `cmix 5 1.0,  1.0,  0.0, -1.0` | full positive | none | CW | 
-| Use `cmix 6 1.0,  0.5,  1.0,  1.0` | half positive | full positive | CCW | 
+| Use `mmix 1 1.0, -0.5,  1.0, -1.0` | half negative | full positive | CW |
+| Use `mmix 1 1.0, -1.0,  0.0,  1.0` | full negative | none | CCW | 
+| Use `mmix 2 1.0, -1.0, -1.0, -1.0` | full negative | full negative | CW | 
+| Use `mmix 3 1.0,  1.0, -1.0,  1.0` | full positive | full negative | CCW  | 
+| Use `mmix 4 1.0,  1.0,  0.0, -1.0` | full positive | none | CW | 
+| Use `mmix 5 1.0,  0.5,  1.0,  1.0` | half positive | full positive | CCW | 
+
+### Example 3: Custom tricopter
+
+```
+mixer CUSTOMTRI
+mmix reset
+mmix 0 1.000 0.000 1.333 0.000
+mmix 1 1.000 -1.000 -0.667 0.000
+mmix 2 1.000 1.000 -0.667 0.000
+smix reset
+smix 0 6 3 100 0 0 100 0
+```
+
+
+## Servo Reversing
+
+Servos are reversed using the `smix reverse` command.
+
+e.g. when using the TRI mixer to reverse the tail servo on a tricopter use this:
+
+`smix reverse 5 2 r`
+
+i.e. when mixing rudder servo slot (`5`) using Stabilised YAW input source (`2`) reverse the direction (`r`)
+  
\ No newline at end of file
diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c
index 6ba5c7e423..c684b02171 100644
--- a/src/main/blackbox/blackbox.c
+++ b/src/main/blackbox/blackbox.c
@@ -366,7 +366,7 @@ static bool testBlackboxConditionUncached(FlightLogFieldCondition condition)
             return motorCount >= condition - FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_MOTORS_1 + 1;
         
         case FLIGHT_LOG_FIELD_CONDITION_TRICOPTER:
-            return masterConfig.mixerMode == MIXER_TRI;
+            return masterConfig.mixerMode == MIXER_TRI || masterConfig.mixerMode == MIXER_CUSTOM_TRI;
 
         case FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_D_0:
         case FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_D_1:
diff --git a/src/main/config/config.c b/src/main/config/config.c
index a9b26ea11c..b36a358561 100644
--- a/src/main/config/config.c
+++ b/src/main/config/config.c
@@ -350,10 +350,6 @@ static void setControlRateProfile(uint8_t profileIndex)
 static void resetConf(void)
 {
     int i;
-#ifdef USE_SERVOS
-    int8_t servoRates[MAX_SUPPORTED_SERVOS] = { 30, 30, 100, 100, 100, 100, 100, 100 };
-    ;
-#endif
 
     // Clear all configuration
     memset(&masterConfig, 0, sizeof(master_t));
@@ -420,7 +416,6 @@ static void resetConf(void)
 
     resetMixerConfig(&masterConfig.mixerConfig);
 
-    masterConfig.airplaneConfig.flaps_speed = 0;
     masterConfig.airplaneConfig.fixedwing_althold_dir = 1;
 
     // Motor/ESC/Servo
@@ -485,14 +480,14 @@ static void resetConf(void)
         currentProfile->servoConf[i].min = DEFAULT_SERVO_MIN;
         currentProfile->servoConf[i].max = DEFAULT_SERVO_MAX;
         currentProfile->servoConf[i].middle = DEFAULT_SERVO_MIDDLE;
-        currentProfile->servoConf[i].rate = servoRates[i];
+        currentProfile->servoConf[i].rate = 100;
         currentProfile->servoConf[i].angleAtMin = DEFAULT_SERVO_MIN_ANGLE;
         currentProfile->servoConf[i].angleAtMax = DEFAULT_SERVO_MAX_ANGLE;
         currentProfile->servoConf[i].forwardFromChannel = CHANNEL_FORWARDING_DISABLED;
     }
 
     // gimbal
-    currentProfile->gimbalConfig.gimbal_flags = GIMBAL_NORMAL;
+    currentProfile->gimbalConfig.mode = GIMBAL_MODE_NORMAL;
 #endif
 
 #ifdef GPS
@@ -501,7 +496,7 @@ static void resetConf(void)
 
     // custom mixer. clear by defaults.
     for (i = 0; i < MAX_SUPPORTED_MOTORS; i++)
-        masterConfig.customMixer[i].throttle = 0.0f;
+        masterConfig.customMotorMixer[i].throttle = 0.0f;
 
 #ifdef LED_STRIP
     applyDefaultColors(masterConfig.colors, CONFIGURABLE_COLOR_COUNT);
@@ -547,52 +542,52 @@ static void resetConf(void)
     parseRcChannels("TAER1234", &masterConfig.rxConfig);
 
     //  { 1.0f, -0.5f,  1.0f, -1.0f },          // REAR_R
-    masterConfig.customMixer[0].throttle = 1.0f;
-    masterConfig.customMixer[0].roll = -0.5f;
-    masterConfig.customMixer[0].pitch = 1.0f;
-    masterConfig.customMixer[0].yaw = -1.0f;
+    masterConfig.customMotorMixer[0].throttle = 1.0f;
+    masterConfig.customMotorMixer[0].roll = -0.5f;
+    masterConfig.customMotorMixer[0].pitch = 1.0f;
+    masterConfig.customMotorMixer[0].yaw = -1.0f;
 
     //  { 1.0f, -0.5f, -1.0f,  1.0f },          // FRONT_R
-    masterConfig.customMixer[1].throttle = 1.0f;
-    masterConfig.customMixer[1].roll = -0.5f;
-    masterConfig.customMixer[1].pitch = -1.0f;
-    masterConfig.customMixer[1].yaw = 1.0f;
+    masterConfig.customMotorMixer[1].throttle = 1.0f;
+    masterConfig.customMotorMixer[1].roll = -0.5f;
+    masterConfig.customMotorMixer[1].pitch = -1.0f;
+    masterConfig.customMotorMixer[1].yaw = 1.0f;
 
     //  { 1.0f,  0.5f,  1.0f,  1.0f },          // REAR_L
-    masterConfig.customMixer[2].throttle = 1.0f;
-    masterConfig.customMixer[2].roll = 0.5f;
-    masterConfig.customMixer[2].pitch = 1.0f;
-    masterConfig.customMixer[2].yaw = 1.0f;
+    masterConfig.customMotorMixer[2].throttle = 1.0f;
+    masterConfig.customMotorMixer[2].roll = 0.5f;
+    masterConfig.customMotorMixer[2].pitch = 1.0f;
+    masterConfig.customMotorMixer[2].yaw = 1.0f;
 
     //  { 1.0f,  0.5f, -1.0f, -1.0f },          // FRONT_L
-    masterConfig.customMixer[3].throttle = 1.0f;
-    masterConfig.customMixer[3].roll = 0.5f;
-    masterConfig.customMixer[3].pitch = -1.0f;
-    masterConfig.customMixer[3].yaw = -1.0f;
+    masterConfig.customMotorMixer[3].throttle = 1.0f;
+    masterConfig.customMotorMixer[3].roll = 0.5f;
+    masterConfig.customMotorMixer[3].pitch = -1.0f;
+    masterConfig.customMotorMixer[3].yaw = -1.0f;
 
     //  { 1.0f, -1.0f, -0.5f, -1.0f },          // MIDFRONT_R
-    masterConfig.customMixer[4].throttle = 1.0f;
-    masterConfig.customMixer[4].roll = -1.0f;
-    masterConfig.customMixer[4].pitch = -0.5f;
-    masterConfig.customMixer[4].yaw = -1.0f;
+    masterConfig.customMotorMixer[4].throttle = 1.0f;
+    masterConfig.customMotorMixer[4].roll = -1.0f;
+    masterConfig.customMotorMixer[4].pitch = -0.5f;
+    masterConfig.customMotorMixer[4].yaw = -1.0f;
 
     //  { 1.0f,  1.0f, -0.5f,  1.0f },          // MIDFRONT_L
-    masterConfig.customMixer[5].throttle = 1.0f;
-    masterConfig.customMixer[5].roll = 1.0f;
-    masterConfig.customMixer[5].pitch = -0.5f;
-    masterConfig.customMixer[5].yaw = 1.0f;
+    masterConfig.customMotorMixer[5].throttle = 1.0f;
+    masterConfig.customMotorMixer[5].roll = 1.0f;
+    masterConfig.customMotorMixer[5].pitch = -0.5f;
+    masterConfig.customMotorMixer[5].yaw = 1.0f;
 
     //  { 1.0f, -1.0f,  0.5f,  1.0f },          // MIDREAR_R
-    masterConfig.customMixer[6].throttle = 1.0f;
-    masterConfig.customMixer[6].roll = -1.0f;
-    masterConfig.customMixer[6].pitch = 0.5f;
-    masterConfig.customMixer[6].yaw = 1.0f;
+    masterConfig.customMotorMixer[6].throttle = 1.0f;
+    masterConfig.customMotorMixer[6].roll = -1.0f;
+    masterConfig.customMotorMixer[6].pitch = 0.5f;
+    masterConfig.customMotorMixer[6].yaw = 1.0f;
 
     //  { 1.0f,  1.0f,  0.5f, -1.0f },          // MIDREAR_L
-    masterConfig.customMixer[7].throttle = 1.0f;
-    masterConfig.customMixer[7].roll = 1.0f;
-    masterConfig.customMixer[7].pitch = 0.5f;
-    masterConfig.customMixer[7].yaw = -1.0f;
+    masterConfig.customMotorMixer[7].throttle = 1.0f;
+    masterConfig.customMotorMixer[7].roll = 1.0f;
+    masterConfig.customMotorMixer[7].pitch = 0.5f;
+    masterConfig.customMotorMixer[7].yaw = -1.0f;
 #endif
 
     // copy first profile into remaining profile
diff --git a/src/main/config/config.h b/src/main/config/config.h
index e38e429088..fabc135da5 100644
--- a/src/main/config/config.h
+++ b/src/main/config/config.h
@@ -42,7 +42,8 @@ typedef enum {
     FEATURE_LED_STRIP = 1 << 16,
     FEATURE_DISPLAY = 1 << 17,
     FEATURE_ONESHOT125 = 1 << 18,
-    FEATURE_BLACKBOX = 1 << 19
+    FEATURE_BLACKBOX = 1 << 19,
+    FEATURE_CHANNEL_FORWARDING = 1 << 20
 } features_e;
 
 void handleOneshotFeatureChangeOnRestart(void);
diff --git a/src/main/config/config_master.h b/src/main/config/config_master.h
index 21dff780df..f223f41751 100644
--- a/src/main/config/config_master.h
+++ b/src/main/config/config_master.h
@@ -28,8 +28,10 @@ typedef struct master_t {
     uint16_t looptime;                      // imu loop time in us
     uint8_t emf_avoidance;                   // change pll settings to avoid noise in the uhf band
 
-    motorMixer_t customMixer[MAX_SUPPORTED_MOTORS]; // custom mixtable
-
+    motorMixer_t customMotorMixer[MAX_SUPPORTED_MOTORS];
+#ifdef USE_SERVOS
+    servoMixer_t customServoMixer[MAX_SERVO_RULES];
+#endif
     // motor/esc/servo related stuff
     escAndServoConfig_t escAndServoConfig;
     flight3DConfig_t flight3DConfig;
diff --git a/src/main/drivers/pwm_mapping.c b/src/main/drivers/pwm_mapping.c
index 3642c9f74b..aa6c6109ab 100644
--- a/src/main/drivers/pwm_mapping.c
+++ b/src/main/drivers/pwm_mapping.c
@@ -502,7 +502,7 @@ pwmOutputConfiguration_t *pwmInit(drv_pwm_config_t *init)
 #endif
         }
 
-        if (init->extraServos && !init->airplane) {
+        if (init->useChannelForwarding && !init->airplane) {
 #if defined(NAZE) && defined(LED_STRIP_TIMER)
             // if LED strip is active, PWM5-8 are unavailable, so map AUX1+AUX2 to PWM13+PWM14
             if (init->useLEDStrip) { 
diff --git a/src/main/drivers/pwm_mapping.h b/src/main/drivers/pwm_mapping.h
index dd9c89889d..ad7c88b502 100644
--- a/src/main/drivers/pwm_mapping.h
+++ b/src/main/drivers/pwm_mapping.h
@@ -63,7 +63,7 @@ typedef struct drv_pwm_config_t {
 #endif
 #ifdef USE_SERVOS
     bool useServos;
-    bool extraServos;    // configure additional 4 channels in PPM mode as servos, not motors
+    bool useChannelForwarding;    // configure additional channels as servos
     uint16_t servoPwmRate;
     uint16_t servoCenterPulse;
 #endif
diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c
index 1e58796ec1..26e333f873 100755
--- a/src/main/flight/mixer.c
+++ b/src/main/flight/mixer.c
@@ -18,6 +18,7 @@
 #include <stdbool.h>
 #include <stdint.h>
 #include <stdlib.h>
+#include <string.h>
 
 #include "platform.h"
 #include "debug.h"
@@ -62,8 +63,8 @@ typedef enum {
     SERVO_ELEVATOR = 6,
     SERVO_THROTTLE = 7, // for internal combustion (IC) planes
 
-    SERVO_BIPLANE_LEFT = 4,
-    SERVO_BIPLANE_RIGHT = 5,
+    SERVO_BICOPTER_LEFT = 4,
+    SERVO_BICOPTER_RIGHT = 5,
 
     SERVO_DUALCOPTER_LEFT = 4,
     SERVO_DUALCOPTER_RIGHT = 5,
@@ -76,7 +77,7 @@ typedef enum {
 } servoIndex_e;
 
 #define SERVO_PLANE_INDEX_MIN SERVO_FLAPS
-#define SERVO_PLANE_INDEX_MAX SERVO_ELEVATOR
+#define SERVO_PLANE_INDEX_MAX SERVO_THROTTLE
 
 #define SERVO_DUALCOPTER_INDEX_MIN SERVO_DUALCOPTER_LEFT
 #define SERVO_DUALCOPTER_INDEX_MAX SERVO_DUALCOPTER_RIGHT
@@ -90,6 +91,7 @@ typedef enum {
 //#define MIXER_DEBUG
 
 uint8_t motorCount = 0;
+
 int16_t motor[MAX_SUPPORTED_MOTORS];
 int16_t motor_disarmed[MAX_SUPPORTED_MOTORS];
 
@@ -99,10 +101,13 @@ static escAndServoConfig_t *escAndServoConfig;
 static airplaneConfig_t *airplaneConfig;
 static rxConfig_t *rxConfig;
 
-static motorMixer_t currentMixer[MAX_SUPPORTED_MOTORS];
 static mixerMode_e currentMixerMode;
+static motorMixer_t currentMixer[MAX_SUPPORTED_MOTORS];
+
 
 #ifdef USE_SERVOS
+static uint8_t servoRuleCount = 0;
+static servoMixer_t currentServoMixer[MAX_SERVO_RULES];
 static gimbalConfig_t *gimbalConfig;
 int16_t servo[MAX_SUPPORTED_SERVOS];
 static int useServo;
@@ -118,7 +123,7 @@ static const motorMixer_t mixerQuadX[] = {
     { 1.0f,  1.0f, -1.0f, -1.0f },          // FRONT_L
 };
 #ifndef USE_QUAD_MIXER_ONLY
-static const motorMixer_t mixerTri[] = {
+static const motorMixer_t mixerTricopter[] = {
     { 1.0f,  0.0f,  1.333333f,  0.0f },     // REAR
     { 1.0f, -1.0f, -0.666667f,  0.0f },     // RIGHT
     { 1.0f,  1.0f, -0.666667f,  0.0f },     // LEFT
@@ -131,7 +136,7 @@ static const motorMixer_t mixerQuadP[] = {
     { 1.0f,  0.0f, -1.0f, -1.0f },          // FRONT
 };
 
-static const motorMixer_t mixerBi[] = {
+static const motorMixer_t mixerBicopter[] = {
     { 1.0f,  1.0f,  0.0f,  0.0f },          // LEFT
     { 1.0f, -1.0f,  0.0f,  0.0f },          // RIGHT
 };
@@ -211,10 +216,10 @@ static const motorMixer_t mixerVtail4[] = {
 };
 
 static const motorMixer_t mixerAtail4[] = {
-    { 1.0f, 0.0f, 1.0f, 1.0f },             // REAR_R
-    { 1.0f, -1.0f, -1.0f, 0.0f },           // FRONT_R
-    { 1.0f, 0.0f, 1.0f, -1.0f },            // REAR_L
-    { 1.0f, 1.0f, -1.0f, -0.0f },           // FRONT_L
+    { 1.0f,  0.0f,  1.0f,  1.0f },          // REAR_R
+    { 1.0f, -1.0f, -1.0f,  0.0f },          // FRONT_R
+    { 1.0f,  0.0f,  1.0f, -1.0f },          // REAR_L
+    { 1.0f,  1.0f, -1.0f, -0.0f },          // FRONT_L
 };
 
 static const motorMixer_t mixerHex6H[] = {
@@ -231,36 +236,128 @@ static const motorMixer_t mixerDualcopter[] = {
     { 1.0f,  0.0f,  0.0f,  1.0f },          // RIGHT
 };
 
+static const motorMixer_t mixerSingleProp[] = {
+    { 1.0f,  0.0f,  0.0f, 0.0f },
+};
+
 // Keep synced with mixerMode_e
 const mixer_t mixers[] = {
-    // motors, servos, motor mixer
-    { 0, 0, NULL },                // entry 0
-    { 3, 1, mixerTri },            // MIXER_TRI
-    { 4, 0, mixerQuadP },          // MIXER_QUADP
-    { 4, 0, mixerQuadX },          // MIXER_QUADX
-    { 2, 1, mixerBi },             // MIXER_BI
-    { 0, 1, NULL },                // * MIXER_GIMBAL
-    { 6, 0, mixerY6 },             // MIXER_Y6
-    { 6, 0, mixerHex6P },          // MIXER_HEX6
-    { 1, 1, NULL },                // * MIXER_FLYING_WING
-    { 4, 0, mixerY4 },             // MIXER_Y4
-    { 6, 0, mixerHex6X },          // MIXER_HEX6X
-    { 8, 0, mixerOctoX8 },         // MIXER_OCTOX8
-    { 8, 0, mixerOctoFlatP },      // MIXER_OCTOFLATP
-    { 8, 0, mixerOctoFlatX },      // MIXER_OCTOFLATX
-    { 1, 1, NULL },                // * MIXER_AIRPLANE
-    { 0, 1, NULL },                // * MIXER_HELI_120_CCPM
-    { 0, 1, NULL },                // * MIXER_HELI_90_DEG
-    { 4, 0, mixerVtail4 },         // MIXER_VTAIL4
-    { 6, 0, mixerHex6H },          // MIXER_HEX6H
-    { 0, 1, NULL },                // * MIXER_PPM_TO_SERVO
-    { 2, 1, mixerDualcopter },     // MIXER_DUALCOPTER
-    { 1, 1, NULL },                // MIXER_SINGLECOPTER
-    { 4, 0, mixerAtail4 },         // MIXER_ATAIL4
-    { 0, 0, NULL },                // MIXER_CUSTOM
+    // motors, use servo, motor mixer
+    { 0, false, NULL },                // entry 0
+    { 3, true,  mixerTricopter },      // MIXER_TRI
+    { 4, false, mixerQuadP },          // MIXER_QUADP
+    { 4, false, mixerQuadX },          // MIXER_QUADX
+    { 2, true,  mixerBicopter },       // MIXER_BICOPTER
+    { 0, true,  NULL },                // * MIXER_GIMBAL
+    { 6, false, mixerY6 },             // MIXER_Y6
+    { 6, false, mixerHex6P },          // MIXER_HEX6
+    { 1, true,  mixerSingleProp },     // * MIXER_FLYING_WING
+    { 4, false, mixerY4 },             // MIXER_Y4
+    { 6, false, mixerHex6X },          // MIXER_HEX6X
+    { 8, false, mixerOctoX8 },         // MIXER_OCTOX8
+    { 8, false, mixerOctoFlatP },      // MIXER_OCTOFLATP
+    { 8, false, mixerOctoFlatX },      // MIXER_OCTOFLATX
+    { 1, true,  mixerSingleProp },     // * MIXER_AIRPLANE
+    { 0, true,  NULL },                // * MIXER_HELI_120_CCPM
+    { 0, true,  NULL },                // * MIXER_HELI_90_DEG
+    { 4, false, mixerVtail4 },         // MIXER_VTAIL4
+    { 6, false, mixerHex6H },          // MIXER_HEX6H
+    { 0, true,  NULL },                // * MIXER_PPM_TO_SERVO
+    { 2, true,  mixerDualcopter },     // MIXER_DUALCOPTER
+    { 1, true,  NULL },                // MIXER_SINGLECOPTER
+    { 4, false, mixerAtail4 },         // MIXER_ATAIL4
+    { 0, false, NULL },                // MIXER_CUSTOM
+    { 2, true,  NULL },                // MIXER_CUSTOM_AIRPLANE
+    { 3, true,  NULL },                // MIXER_CUSTOM_TRI
 };
 #endif
 
+#ifdef USE_SERVOS
+
+#define COUNT_SERVO_RULES(rules) (sizeof(rules) / sizeof(servoMixer_t))
+// mixer rule format servo, input, rate, speed, min, max, box
+static const servoMixer_t servoMixerAirplane[] = {
+    { SERVO_FLAPPERON_1, INPUT_STABILIZED_ROLL,  100, 0, 0, 100, 0 },
+    { SERVO_FLAPPERON_2, INPUT_STABILIZED_ROLL,  100, 0, 0, 100, 0 },
+    { SERVO_RUDDER, INPUT_STABILIZED_YAW,   100, 0, 0, 100, 0 },
+    { SERVO_ELEVATOR, INPUT_STABILIZED_PITCH, 100, 0, 0, 100, 0 },
+    { SERVO_THROTTLE, INPUT_STABILIZED_THROTTLE, 100, 0, 0, 100, 0 },
+};
+
+static const servoMixer_t servoMixerFlyingWing[] = {
+    { SERVO_FLAPPERON_1, INPUT_STABILIZED_ROLL,  100, 0, 0, 100, 0 },
+    { SERVO_FLAPPERON_1, INPUT_STABILIZED_PITCH, 100, 0, 0, 100, 0 },
+    { SERVO_FLAPPERON_2, INPUT_STABILIZED_ROLL,  -100, 0, 0, 100, 0 },
+    { SERVO_FLAPPERON_2, INPUT_STABILIZED_PITCH, 100, 0, 0, 100, 0 },
+    { SERVO_THROTTLE, INPUT_STABILIZED_THROTTLE, 100, 0, 0, 100, 0 },
+};
+
+static const servoMixer_t servoMixerBI[] = {
+    { SERVO_BICOPTER_LEFT, INPUT_STABILIZED_YAW,   100, 0, 0, 100, 0 },
+    { SERVO_BICOPTER_LEFT, INPUT_STABILIZED_PITCH, 100, 0, 0, 100, 0 },
+    { SERVO_BICOPTER_RIGHT, INPUT_STABILIZED_YAW,   100, 0, 0, 100, 0 },
+    { SERVO_BICOPTER_RIGHT, INPUT_STABILIZED_PITCH, 100, 0, 0, 100, 0 },
+};
+
+static const servoMixer_t servoMixerTri[] = {
+    { SERVO_RUDDER, INPUT_STABILIZED_YAW,   100, 0, 0, 100, 0 },
+};
+
+static const servoMixer_t servoMixerDual[] = {
+    { SERVO_DUALCOPTER_LEFT, INPUT_STABILIZED_PITCH, 100, 0, 0, 100, 0 },
+    { SERVO_DUALCOPTER_RIGHT, INPUT_STABILIZED_ROLL,  100, 0, 0, 100, 0 },
+};
+
+static const servoMixer_t servoMixerSingle[] = {
+    { SERVO_SINGLECOPTER_1, INPUT_STABILIZED_YAW,   100, 0, 0, 100, 0 },
+    { SERVO_SINGLECOPTER_1, INPUT_STABILIZED_PITCH, 100, 0, 0, 100, 0 },
+    { SERVO_SINGLECOPTER_2, INPUT_STABILIZED_YAW,   100, 0, 0, 100, 0 },
+    { SERVO_SINGLECOPTER_2, INPUT_STABILIZED_PITCH, 100, 0, 0, 100, 0 },
+    { SERVO_SINGLECOPTER_3, INPUT_STABILIZED_YAW,   100, 0, 0, 100, 0 },
+    { SERVO_SINGLECOPTER_3, INPUT_STABILIZED_ROLL,  100, 0, 0, 100, 0 },
+    { SERVO_SINGLECOPTER_4, INPUT_STABILIZED_YAW,   100, 0, 0, 100, 0 },
+    { SERVO_SINGLECOPTER_4, INPUT_STABILIZED_ROLL,  100, 0, 0, 100, 0 },
+};
+
+static const servoMixer_t servoMixerGimbal[] = {
+    { SERVO_GIMBAL_PITCH, INPUT_GIMBAL_PITCH, 125, 0, 0, 100, 0 },
+    { SERVO_GIMBAL_ROLL, INPUT_GIMBAL_ROLL,  125, 0, 0, 100, 0 },
+};
+
+
+const mixerRules_t servoMixers[] = {
+    { 0, NULL },                // entry 0
+    { COUNT_SERVO_RULES(servoMixerTri), servoMixerTri },       // MULTITYPE_TRI
+    { 0, NULL },                // MULTITYPE_QUADP
+    { 0, NULL },                // MULTITYPE_QUADX
+    { COUNT_SERVO_RULES(servoMixerBI), servoMixerBI },        // MULTITYPE_BI
+    { COUNT_SERVO_RULES(servoMixerGimbal), servoMixerGimbal },    // * MULTITYPE_GIMBAL
+    { 0, NULL },                // MULTITYPE_Y6
+    { 0, NULL },                // MULTITYPE_HEX6
+    { COUNT_SERVO_RULES(servoMixerFlyingWing), servoMixerFlyingWing },// * MULTITYPE_FLYING_WING
+    { 0, NULL },                // MULTITYPE_Y4
+    { 0, NULL },                // MULTITYPE_HEX6X
+    { 0, NULL },                // MULTITYPE_OCTOX8
+    { 0, NULL },                // MULTITYPE_OCTOFLATP
+    { 0, NULL },                // MULTITYPE_OCTOFLATX
+    { COUNT_SERVO_RULES(servoMixerAirplane), servoMixerAirplane },  // * MULTITYPE_AIRPLANE
+    { 0, NULL },                // * MULTITYPE_HELI_120_CCPM
+    { 0, NULL },                // * MULTITYPE_HELI_90_DEG
+    { 0, NULL },                // MULTITYPE_VTAIL4
+    { 0, NULL },                // MULTITYPE_HEX6H
+    { 0, NULL },                // * MULTITYPE_PPM_TO_SERVO
+    { COUNT_SERVO_RULES(servoMixerDual), servoMixerDual },      // MULTITYPE_DUALCOPTER
+    { COUNT_SERVO_RULES(servoMixerSingle), servoMixerSingle },    // MULTITYPE_SINGLECOPTER
+    { 0, NULL },                // MULTITYPE_ATAIL4
+    { 0, NULL },                // MULTITYPE_CUSTOM
+    { 0, NULL },                // MULTITYPE_CUSTOM_PLANE
+    { 0, NULL },                // MULTITYPE_CUSTOM_TRI
+};
+
+static servoMixer_t *customServoMixers;
+#endif
+
+
 static motorMixer_t *customMixers;
 
 void mixerUseConfigs(
@@ -297,15 +394,11 @@ int16_t determineServoMiddleOrForwardFromChannel(servoIndex_e servoIndex)
     return servoConf[servoIndex].middle;
 }
 
-int servoDirection(servoIndex_e servoIndex, int lr)
+
+int servoDirection(int servoIndex, int inputSource)
 {
-    // servo.rate is overloaded for servos that don't have a rate, but only need direction
-    // bit set = negative, clear = positive
-    // rate[2] = ???_direction
-    // rate[1] = roll_direction
-    // rate[0] = pitch_direction
-    // servo.rate is also used as gimbal gain multiplier (yeah)
-    if (servoConf[servoIndex].rate & lr)
+    // determine the direction (reversed or not) from the direction bitfield of the servo
+    if (servoConf[servoIndex].reversedSources & (1 << inputSource))
         return -1;
     else
         return 1;
@@ -313,11 +406,32 @@ int servoDirection(servoIndex_e servoIndex, int lr)
 #endif
 
 #ifndef USE_QUAD_MIXER_ONLY
-void mixerInit(mixerMode_e mixerMode, motorMixer_t *initialCustomMixers)
+
+void loadCustomServoMixer(void)
+{
+    uint8_t i;
+
+    // reset settings
+    servoRuleCount = 0;
+    memset(currentServoMixer, 0, sizeof(currentServoMixer));
+
+    // load custom mixer into currentServoMixer
+    for (i = 0; i < MAX_SERVO_RULES; i++) {
+        // check if done
+        if (customServoMixers[i].rate == 0)
+            break;
+            
+        currentServoMixer[i] = customServoMixers[i];
+        servoRuleCount++;
+    }
+}
+
+void mixerInit(mixerMode_e mixerMode, motorMixer_t *initialCustomMotorMixers, servoMixer_t *initialCustomServoMixers)
 {
     currentMixerMode = mixerMode;
 
-    customMixers = initialCustomMixers;
+    customMixers = initialCustomMotorMixers;
+    customServoMixers = initialCustomServoMixers;
 
     // enable servos for mixes that require them. note, this shifts motor counts.
     useServo = mixers[currentMixerMode].useServo;
@@ -337,7 +451,7 @@ void mixerUsePWMOutputConfiguration(pwmOutputConfiguration_t *pwmOutputConfigura
 
     servoCount = pwmOutputConfiguration->servoCount;
 
-    if (currentMixerMode == MIXER_CUSTOM) {
+    if (currentMixerMode == MIXER_CUSTOM || currentMixerMode == MIXER_CUSTOM_TRI || currentMixerMode == MIXER_CUSTOM_AIRPLANE) {
         // load custom mixer into currentMixer
         for (i = 0; i < MAX_SUPPORTED_MOTORS; i++) {
             // check if done
@@ -355,6 +469,14 @@ void mixerUsePWMOutputConfiguration(pwmOutputConfiguration_t *pwmOutputConfigura
         }
     }
 
+    if (useServo) {
+        servoRuleCount = servoMixers[currentMixerMode].servoRuleCount;
+        if (servoMixers[currentMixerMode].rule) {
+            for (i = 0; i < servoRuleCount; i++)
+                currentServoMixer[i] = servoMixers[currentMixerMode].rule[i];
+        }
+    }
+    
     // in 3D mode, mixer gain has to be halved
     if (feature(FEATURE_3D)) {
         if (motorCount > 1) {
@@ -368,12 +490,38 @@ void mixerUsePWMOutputConfiguration(pwmOutputConfiguration_t *pwmOutputConfigura
 
     // set flag that we're on something with wings
     if (currentMixerMode == MIXER_FLYING_WING ||
-            currentMixerMode == MIXER_AIRPLANE)
+        currentMixerMode == MIXER_AIRPLANE ||
+        currentMixerMode == MIXER_CUSTOM_AIRPLANE
+    ) {
         ENABLE_STATE(FIXED_WING);
-    else
+        
+        if (currentMixerMode == MIXER_CUSTOM_AIRPLANE) {
+            loadCustomServoMixer();
+        }
+    } else {
         DISABLE_STATE(FIXED_WING);
 
-    mixerResetMotors();
+        if (currentMixerMode == MIXER_CUSTOM_TRI) {
+            loadCustomServoMixer();
+        }
+    }
+
+    mixerResetDisarmedMotors();
+}
+
+
+void servoMixerLoadMix(int index, servoMixer_t *customServoMixers)
+{
+    int i;
+
+    // we're 1-based
+    index++;
+    // clear existing
+    for (i = 0; i < MAX_SERVO_RULES; i++)
+        customServoMixers[i].targetChannel = customServoMixers[i].inputSource = customServoMixers[i].rate = customServoMixers[i].box = 0;
+
+    for (i = 0; i < servoMixers[index].servoRuleCount; i++)
+        customServoMixers[i] = servoMixers[index].rule[i];
 }
 
 void mixerLoadMix(int index, motorMixer_t *customMixers)
@@ -415,11 +563,11 @@ void mixerUsePWMOutputConfiguration(pwmOutputConfiguration_t *pwmOutputConfigura
         currentMixer[i] = mixerQuadX[i];
     }
 
-    mixerResetMotors();
+    mixerResetDisarmedMotors();
 }
 #endif
 
-void mixerResetMotors(void)
+void mixerResetDisarmedMotors(void)
 {
     int i;
     // set disarmed motor values
@@ -451,12 +599,13 @@ void writeServos(void)
     uint8_t servoIndex = 0;
 
     switch (currentMixerMode) {
-        case MIXER_BI:
-            pwmWriteServo(servoIndex++, servo[SERVO_BIPLANE_LEFT]);
-            pwmWriteServo(servoIndex++, servo[SERVO_BIPLANE_RIGHT]);
+        case MIXER_BICOPTER:
+            pwmWriteServo(servoIndex++, servo[SERVO_BICOPTER_LEFT]);
+            pwmWriteServo(servoIndex++, servo[SERVO_BICOPTER_RIGHT]);
             break;
 
         case MIXER_TRI:
+        case MIXER_CUSTOM_TRI:
             if (mixerConfig->tri_unarmed_servo) {
                 // if unarmed flag set, we always move servo
                 pwmWriteServo(servoIndex++, servo[SERVO_RUDDER]);
@@ -479,6 +628,7 @@ void writeServos(void)
             pwmWriteServo(servoIndex++, servo[SERVO_DUALCOPTER_RIGHT]);
             break;
 
+        case MIXER_CUSTOM_AIRPLANE:
         case MIXER_AIRPLANE:
             for (int i = SERVO_PLANE_INDEX_MIN; i <= SERVO_PLANE_INDEX_MAX; i++) {
                 pwmWriteServo(servoIndex++, servo[i]);
@@ -496,13 +646,13 @@ void writeServos(void)
     }
 
     // Two servos for SERVO_TILT, if enabled
-    if (feature(FEATURE_SERVO_TILT)) {
+    if (feature(FEATURE_SERVO_TILT) || currentMixerMode == MIXER_GIMBAL) {
         updateGimbalServos(servoIndex);
         servoIndex += 2;
     }
 
     // forward AUX to remaining servo outputs (not constrained)
-    if (gimbalConfig->gimbal_flags & GIMBAL_FORWARDAUX) {
+    if (feature(FEATURE_CHANNEL_FORWARDING)) {
         forwardAuxChannelsToServos(servoIndex);
         servoIndex += MAX_AUX_CHANNEL_COUNT;
     }
@@ -545,52 +695,83 @@ void StopPwmAllMotors()
 }
 
 #ifndef USE_QUAD_MIXER_ONLY
-static void airplaneMixer(void)
+static void servoMixer(void)
 {
-    int16_t flapperons[2] = { 0, 0 };
-    int i;
+    int16_t input[INPUT_SOURCE_COUNT]; // Range [-500:+500]
+    static int16_t currentOutput[MAX_SERVO_RULES];
+    uint8_t i;
 
-    if (!ARMING_FLAG(ARMED))
-        servo[SERVO_THROTTLE] = escAndServoConfig->mincommand; // Kill throttle when disarmed
-    else
-        servo[SERVO_THROTTLE] = constrain(rcCommand[THROTTLE], escAndServoConfig->minthrottle, escAndServoConfig->maxthrottle);
-    motor[0] = servo[SERVO_THROTTLE];
-
-    if (airplaneConfig->flaps_speed) {
-        // configure SERVO3 middle point in GUI to using an AUX channel for FLAPS control
-        // use servo min, servo max and servo rate for proper endpoints adjust
-        static int16_t slow_LFlaps;
-        int16_t lFlap = determineServoMiddleOrForwardFromChannel(SERVO_FLAPS);
-
-        lFlap = constrain(lFlap, servoConf[SERVO_FLAPS].min, servoConf[SERVO_FLAPS].max);
-        lFlap = escAndServoConfig->servoCenterPulse - lFlap;
-        if (slow_LFlaps < lFlap)
-            slow_LFlaps += airplaneConfig->flaps_speed;
-        else if (slow_LFlaps > lFlap)
-            slow_LFlaps -= airplaneConfig->flaps_speed;
-
-        servo[SERVO_FLAPS] = ((int32_t)servoConf[SERVO_FLAPS].rate * slow_LFlaps) / 100L;
-        servo[SERVO_FLAPS] += escAndServoConfig->servoCenterPulse;
-    }
-
-    if (FLIGHT_MODE(PASSTHRU_MODE)) {   // Direct passthru from RX
-        servo[SERVO_FLAPPERON_1] = rcCommand[ROLL] + flapperons[0];
-        servo[SERVO_FLAPPERON_2] = rcCommand[ROLL] + flapperons[1];
-        servo[SERVO_RUDDER]      = rcCommand[YAW];
-        servo[SERVO_ELEVATOR]    = rcCommand[PITCH];
+    if (FLIGHT_MODE(PASSTHRU_MODE)) {
+        // Direct passthru from RX
+        input[INPUT_STABILIZED_ROLL] = rcCommand[ROLL];
+        input[INPUT_STABILIZED_PITCH] = rcCommand[PITCH];
+        input[INPUT_STABILIZED_YAW] = rcCommand[YAW];
     } else {
         // Assisted modes (gyro only or gyro+acc according to AUX configuration in Gui
-        servo[SERVO_FLAPPERON_1] = axisPID[ROLL] + flapperons[0];
-        servo[SERVO_FLAPPERON_2] = axisPID[ROLL] + flapperons[1];
-        servo[SERVO_RUDDER]      = axisPID[YAW];
-        servo[SERVO_ELEVATOR]    = axisPID[PITCH];
+        input[INPUT_STABILIZED_ROLL] = axisPID[ROLL];
+        input[INPUT_STABILIZED_PITCH] = axisPID[PITCH];
+        input[INPUT_STABILIZED_YAW] = axisPID[YAW];
+
+        // Reverse yaw servo when inverted in 3D mode
+        if (feature(FEATURE_3D) && (rcData[THROTTLE] < rxConfig->midrc)) {
+            input[INPUT_STABILIZED_YAW] *= -1;
+        }
     }
 
-    for (i = SERVO_PLANE_INDEX_MIN; i <= SERVO_PLANE_INDEX_MAX; i++) {
-        servo[i] = ((int32_t)servoConf[i].rate * servo[i]) / 100L; // servo rates
+    input[INPUT_GIMBAL_PITCH] = scaleRange(inclination.values.pitchDeciDegrees, -1800, 1800, -500, +500);
+    input[INPUT_GIMBAL_ROLL] = scaleRange(inclination.values.rollDeciDegrees, -1800, 1800, -500, +500);
+
+    input[INPUT_STABILIZED_THROTTLE] = motor[0] - 1000 - 500;  // Since it derives from rcCommand or mincommand and must be [-500:+500]
+
+    // center the RC input value around the RC middle value
+    // by subtracting the RC middle value from the RC input value, we get:
+    // data - middle = input
+    // 2000 - 1500 = +500
+    // 1500 - 1500 = 0
+    // 1000 - 1500 = -500
+    input[INPUT_RC_ROLL]     = rcData[ROLL]     - rxConfig->midrc;
+    input[INPUT_RC_PITCH]    = rcData[PITCH]    - rxConfig->midrc;
+    input[INPUT_RC_YAW]      = rcData[YAW]      - rxConfig->midrc;
+    input[INPUT_RC_THROTTLE] = rcData[THROTTLE] - rxConfig->midrc;
+    input[INPUT_RC_AUX1]     = rcData[AUX1]     - rxConfig->midrc;
+    input[INPUT_RC_AUX2]     = rcData[AUX2]     - rxConfig->midrc;
+    input[INPUT_RC_AUX3]     = rcData[AUX3]     - rxConfig->midrc;
+    input[INPUT_RC_AUX4]     = rcData[AUX4]     - rxConfig->midrc;
+
+    for (i = 0; i < MAX_SUPPORTED_SERVOS; i++)
+        servo[i] = 0;
+
+    // mix servos according to rules
+    for (i = 0; i < servoRuleCount; i++) {
+        // consider rule if no box assigned or box is active
+        if (currentServoMixer[i].box == 0 || IS_RC_MODE_ACTIVE(BOXSERVO1 + currentServoMixer[i].box - 1)) {
+            uint8_t target = currentServoMixer[i].targetChannel;
+            uint8_t from = currentServoMixer[i].inputSource;
+            uint16_t servo_width = servoConf[target].max - servoConf[target].min;
+            int16_t min = currentServoMixer[i].min * servo_width / 100 - servo_width / 2;
+            int16_t max = currentServoMixer[i].max * servo_width / 100 - servo_width / 2;
+
+            if (currentServoMixer[i].speed == 0)
+                currentOutput[i] = input[from];
+            else {
+                if (currentOutput[i] < input[from])
+                    currentOutput[i] = constrain(currentOutput[i] + currentServoMixer[i].speed, currentOutput[i], input[from]);
+                else if (currentOutput[i] > input[from])
+                    currentOutput[i] = constrain(currentOutput[i] - currentServoMixer[i].speed, input[from], currentOutput[i]);
+            }
+
+            servo[target] += servoDirection(target, from) * constrain(((int32_t)currentOutput[i] * currentServoMixer[i].rate) / 100, min, max);
+        } else {
+            currentOutput[i] = 0;
+        }
+    }
+
+    for (i = 0; i < MAX_SUPPORTED_SERVOS; i++) {
+        servo[i] = ((int32_t)servoConf[i].rate * servo[i]) / 100L;
         servo[i] += determineServoMiddleOrForwardFromChannel(i);
     }
 }
+
 #endif
 
 void mixTable(void)
@@ -598,114 +779,19 @@ void mixTable(void)
     uint32_t i;
 
     if (motorCount >= 4 && mixerConfig->yaw_jump_prevention_limit < YAW_JUMP_PREVENTION_LIMIT_HIGH) {
-        // prevent "yaw jump" during yaw correction (500 is disabled jump protection)
+        // prevent "yaw jump" during yaw correction
         axisPID[YAW] = constrain(axisPID[YAW], -mixerConfig->yaw_jump_prevention_limit - ABS(rcCommand[YAW]), mixerConfig->yaw_jump_prevention_limit + ABS(rcCommand[YAW]));
     }
 
     // motors for non-servo mixes
-    if (motorCount > 1) {
-        for (i = 0; i < motorCount; i++) {
-            motor[i] =
-                rcCommand[THROTTLE] * currentMixer[i].throttle +
-                axisPID[PITCH] * currentMixer[i].pitch +
-                axisPID[ROLL] * currentMixer[i].roll +
-                -mixerConfig->yaw_direction * axisPID[YAW] * currentMixer[i].yaw;
-        }
+    for (i = 0; i < motorCount; i++) {
+        motor[i] =
+            rcCommand[THROTTLE] * currentMixer[i].throttle +
+            axisPID[PITCH] * currentMixer[i].pitch +
+            axisPID[ROLL] * currentMixer[i].roll +
+            -mixerConfig->yaw_direction * axisPID[YAW] * currentMixer[i].yaw;
     }
 
-#if !defined(USE_QUAD_MIXER_ONLY) || defined(USE_SERVOS)
-    int8_t yawDirection3D = 1;
-
-    // Reverse yaw servo when inverted in 3D mode
-    if (feature(FEATURE_3D) && (rcData[THROTTLE] < rxConfig->midrc)) {
-        yawDirection3D = -1;
-    }
-
-    // airplane / servo mixes
-    switch (currentMixerMode) {
-        case MIXER_BI:
-            servo[SERVO_BIPLANE_LEFT] = (servoDirection(SERVO_BIPLANE_LEFT, 2) * axisPID[YAW]) + (servoDirection(SERVO_BIPLANE_LEFT, 1) * axisPID[PITCH]) + determineServoMiddleOrForwardFromChannel(SERVO_BIPLANE_LEFT);
-            servo[SERVO_BIPLANE_RIGHT] = (servoDirection(SERVO_BIPLANE_RIGHT, 2) * axisPID[YAW]) + (servoDirection(SERVO_BIPLANE_RIGHT, 1) * axisPID[PITCH]) + determineServoMiddleOrForwardFromChannel(SERVO_BIPLANE_RIGHT);
-            break;
-
-        case MIXER_TRI:
-            servo[SERVO_RUDDER] = (servoDirection(SERVO_RUDDER, 1) * axisPID[YAW] * yawDirection3D) + determineServoMiddleOrForwardFromChannel(SERVO_RUDDER);
-            break;
-
-        case MIXER_GIMBAL:
-            servo[SERVO_GIMBAL_PITCH] = (((int32_t)servoConf[SERVO_GIMBAL_PITCH].rate * inclination.values.pitchDeciDegrees) / 50) + determineServoMiddleOrForwardFromChannel(SERVO_GIMBAL_PITCH);
-            servo[SERVO_GIMBAL_ROLL] = (((int32_t)servoConf[SERVO_GIMBAL_ROLL].rate * inclination.values.rollDeciDegrees) / 50) + determineServoMiddleOrForwardFromChannel(SERVO_GIMBAL_ROLL);
-            break;
-
-        case MIXER_AIRPLANE:
-            airplaneMixer();
-            break;
-
-        case MIXER_FLYING_WING:
-            if (!ARMING_FLAG(ARMED))
-                servo[SERVO_THROTTLE] = escAndServoConfig->mincommand;
-            else
-                servo[SERVO_THROTTLE] = constrain(rcCommand[THROTTLE], escAndServoConfig->minthrottle, escAndServoConfig->maxthrottle);
-
-            motor[0] = servo[SERVO_THROTTLE];
-
-            if (FLIGHT_MODE(PASSTHRU_MODE)) {
-                // do not use sensors for correction, simple 2 channel mixing
-                servo[SERVO_FLAPPERON_1] = (servoDirection(SERVO_FLAPPERON_1, 1) * rcCommand[PITCH]) + (servoDirection(SERVO_FLAPPERON_1, 2) * rcCommand[ROLL]);
-                servo[SERVO_FLAPPERON_2] = (servoDirection(SERVO_FLAPPERON_2, 1) * rcCommand[PITCH]) + (servoDirection(SERVO_FLAPPERON_2, 2) * rcCommand[ROLL]);
-            } else {
-                // use sensors to correct (gyro only or gyro + acc)
-                servo[SERVO_FLAPPERON_1] = (servoDirection(SERVO_FLAPPERON_1, 1) * axisPID[PITCH]) + (servoDirection(SERVO_FLAPPERON_1, 2) * axisPID[ROLL]);
-                servo[SERVO_FLAPPERON_2] = (servoDirection(SERVO_FLAPPERON_2, 1) * axisPID[PITCH]) + (servoDirection(SERVO_FLAPPERON_2, 2) * axisPID[ROLL]);
-            }
-            servo[SERVO_FLAPPERON_1] += determineServoMiddleOrForwardFromChannel(SERVO_FLAPPERON_1);
-            servo[SERVO_FLAPPERON_2] += determineServoMiddleOrForwardFromChannel(SERVO_FLAPPERON_2);
-            break;
-
-        case MIXER_DUALCOPTER:
-            for (i = SERVO_DUALCOPTER_INDEX_MIN; i <= SERVO_DUALCOPTER_INDEX_MAX; i++) {
-                servo[i] = axisPID[SERVO_DUALCOPTER_INDEX_MAX - i] * servoDirection(i, 1); // mix and setup direction
-                servo[i] += determineServoMiddleOrForwardFromChannel(i);
-            }
-            break;
-
-        case MIXER_SINGLECOPTER:
-            for (i = SERVO_SINGLECOPTER_INDEX_MIN; i <= SERVO_SINGLECOPTER_INDEX_MAX; i++) {
-                servo[i] = (axisPID[YAW] * servoDirection(i, 2)) + (axisPID[(SERVO_SINGLECOPTER_INDEX_MAX - i) >> 1] * servoDirection(i, 1)); // mix and setup direction
-                servo[i] += determineServoMiddleOrForwardFromChannel(i);
-            }
-            motor[0] = rcCommand[THROTTLE];
-            break;
-
-        default:
-            break;
-    }
-
-    // do camstab
-    if (feature(FEATURE_SERVO_TILT)) {
-        // center at fixed position, or vary either pitch or roll by RC channel
-        servo[SERVO_GIMBAL_PITCH] = determineServoMiddleOrForwardFromChannel(0);
-        servo[SERVO_GIMBAL_ROLL] = determineServoMiddleOrForwardFromChannel(1);
-
-        if (IS_RC_MODE_ACTIVE(BOXCAMSTAB)) {
-            if (gimbalConfig->gimbal_flags & GIMBAL_MIXTILT) {
-                servo[SERVO_GIMBAL_PITCH] -= (-(int32_t)servoConf[SERVO_GIMBAL_PITCH].rate) * inclination.values.pitchDeciDegrees / 50 - (int32_t)servoConf[SERVO_GIMBAL_ROLL].rate * inclination.values.rollDeciDegrees / 50;
-                servo[SERVO_GIMBAL_ROLL] += (-(int32_t)servoConf[SERVO_GIMBAL_PITCH].rate) * inclination.values.pitchDeciDegrees / 50 + (int32_t)servoConf[SERVO_GIMBAL_ROLL].rate * inclination.values.rollDeciDegrees / 50;
-            } else {
-                servo[SERVO_GIMBAL_PITCH] += (int32_t)servoConf[SERVO_GIMBAL_PITCH].rate * inclination.values.pitchDeciDegrees / 50;
-                servo[SERVO_GIMBAL_ROLL] += (int32_t)servoConf[SERVO_GIMBAL_ROLL].rate * inclination.values.rollDeciDegrees  / 50;
-            }
-        }
-    }
-
-    // constrain servos
-    if (useServo) {
-        for (i = 0; i < MAX_SUPPORTED_SERVOS; i++) {
-            servo[i] = constrain(servo[i], servoConf[i].min, servoConf[i].max); // limit the values
-        }
-    }
-#endif
-
     if (ARMING_FLAG(ARMED)) {
 
         bool isFailsafeActive = failsafeIsActive();
@@ -758,6 +844,58 @@ void mixTable(void)
             motor[i] = motor_disarmed[i];
         }
     }
+
+    // motor outputs are used as sources for servo mixing, so motors must be calculated before servos.
+
+#if !defined(USE_QUAD_MIXER_ONLY) || defined(USE_SERVOS)
+
+    // airplane / servo mixes
+    switch (currentMixerMode) {
+        case MIXER_CUSTOM_AIRPLANE:
+        case MIXER_FLYING_WING:
+        case MIXER_AIRPLANE:
+        case MIXER_BICOPTER:
+        case MIXER_CUSTOM_TRI:
+        case MIXER_TRI:
+        case MIXER_DUALCOPTER:
+        case MIXER_SINGLECOPTER:
+        case MIXER_GIMBAL:
+            servoMixer();
+            break;
+
+        /*
+        case MIXER_GIMBAL:
+			servo[SERVO_GIMBAL_PITCH] = (((int32_t)servoConf[SERVO_GIMBAL_PITCH].rate * inclination.values.pitchDeciDegrees) / 50) + determineServoMiddleOrForwardFromChannel(SERVO_GIMBAL_PITCH);
+            servo[SERVO_GIMBAL_ROLL] = (((int32_t)servoConf[SERVO_GIMBAL_ROLL].rate * inclination.values.rollDeciDegrees) / 50) + determineServoMiddleOrForwardFromChannel(SERVO_GIMBAL_ROLL);
+            break;
+        */
+
+        default:
+            break;
+    }
+
+    // camera stabilization
+    if (feature(FEATURE_SERVO_TILT)) {
+        // center at fixed position, or vary either pitch or roll by RC channel
+        servo[SERVO_GIMBAL_PITCH] = determineServoMiddleOrForwardFromChannel(SERVO_GIMBAL_PITCH);
+        servo[SERVO_GIMBAL_ROLL] = determineServoMiddleOrForwardFromChannel(SERVO_GIMBAL_ROLL);
+
+        if (IS_RC_MODE_ACTIVE(BOXCAMSTAB)) {
+            if (gimbalConfig->mode == GIMBAL_MODE_MIXTILT) {
+                servo[SERVO_GIMBAL_PITCH] -= (-(int32_t)servoConf[SERVO_GIMBAL_PITCH].rate) * inclination.values.pitchDeciDegrees / 50 - (int32_t)servoConf[SERVO_GIMBAL_ROLL].rate * inclination.values.rollDeciDegrees / 50;
+                servo[SERVO_GIMBAL_ROLL] += (-(int32_t)servoConf[SERVO_GIMBAL_PITCH].rate) * inclination.values.pitchDeciDegrees / 50 + (int32_t)servoConf[SERVO_GIMBAL_ROLL].rate * inclination.values.rollDeciDegrees / 50;
+            } else {
+                servo[SERVO_GIMBAL_PITCH] += (int32_t)servoConf[SERVO_GIMBAL_PITCH].rate * inclination.values.pitchDeciDegrees / 50;
+                servo[SERVO_GIMBAL_ROLL] += (int32_t)servoConf[SERVO_GIMBAL_ROLL].rate * inclination.values.rollDeciDegrees  / 50;
+            }
+        }
+    }
+
+    // constrain servos
+    for (i = 0; i < MAX_SUPPORTED_SERVOS; i++) {
+        servo[i] = constrain(servo[i], servoConf[i].min, servoConf[i].max); // limit the values
+    }
+#endif
 }
 
 #ifdef USE_SERVOS
diff --git a/src/main/flight/mixer.h b/src/main/flight/mixer.h
index 9077959291..7d86b24ada 100644
--- a/src/main/flight/mixer.h
+++ b/src/main/flight/mixer.h
@@ -29,7 +29,7 @@ typedef enum mixerMode
     MIXER_TRI = 1,
     MIXER_QUADP = 2,
     MIXER_QUADX = 3,
-    MIXER_BI = 4,
+    MIXER_BICOPTER = 4,
     MIXER_GIMBAL = 5,
     MIXER_Y6 = 6,
     MIXER_HEX6 = 7,
@@ -48,7 +48,9 @@ typedef enum mixerMode
     MIXER_DUALCOPTER = 20,
     MIXER_SINGLECOPTER = 21,
     MIXER_ATAIL4 = 22,
-    MIXER_CUSTOM = 23
+    MIXER_CUSTOM = 23,
+    MIXER_CUSTOM_AIRPLANE = 24,
+    MIXER_CUSTOM_TRI = 25
 } mixerMode_e;
 
 // Custom mixer data per motor
@@ -85,20 +87,61 @@ typedef struct flight3DConfig_s {
 } flight3DConfig_t;
 
 typedef struct airplaneConfig_t {
-    uint8_t flaps_speed;                    // airplane mode flaps, 0 = no flaps, > 0 = flap speed, larger = faster
     int8_t fixedwing_althold_dir;           // +1 or -1 for pitch/althold gain. later check if need more than just sign
 } airplaneConfig_t;
 
 #define CHANNEL_FORWARDING_DISABLED (uint8_t)0xFF
 
 #ifdef USE_SERVOS
+
+// These must be consecutive, see 'reversedSources'
+enum {
+    INPUT_STABILIZED_ROLL = 0,
+    INPUT_STABILIZED_PITCH,
+    INPUT_STABILIZED_YAW,
+    INPUT_STABILIZED_THROTTLE,
+    INPUT_RC_ROLL,
+    INPUT_RC_PITCH,
+    INPUT_RC_YAW,
+    INPUT_RC_THROTTLE,
+    INPUT_RC_AUX1,
+    INPUT_RC_AUX2,
+    INPUT_RC_AUX3,
+    INPUT_RC_AUX4,
+    INPUT_GIMBAL_PITCH,
+    INPUT_GIMBAL_ROLL,
+
+    INPUT_SOURCE_COUNT
+} inputSource_e;
+
+typedef struct servoMixer_t {
+    uint8_t targetChannel;                  // servo that receives the output of the rule
+    uint8_t inputSource;                    // input channel for this rule
+    int8_t rate;                            // range [-125;+125] ; can be used to adjust a rate 0-125% and a direction
+    uint8_t speed;                          // reduces the speed of the rule, 0=unlimited speed
+    int8_t min;                             // lower bound of rule range [0;100]% of servo max-min
+    int8_t max;                             // lower bound of rule range [0;100]% of servo max-min
+    uint8_t box;                            // active rule if box is enabled, range [0;3], 0=no box, 1=BOXSERVO1, 2=BOXSERVO2, 3=BOXSERVO3
+} servoMixer_t;
+
+#define MAX_SERVO_RULES (2 * MAX_SUPPORTED_SERVOS)
+#define MAX_SERVO_SPEED UINT8_MAX
+#define MAX_SERVO_BOXES 3
+
+// Custom mixer configuration
+typedef struct mixerRules_t {
+    uint8_t servoRuleCount;
+    const servoMixer_t *rule;
+} mixerRules_t;
+
 typedef struct servoParam_t {
     int16_t min;                            // servo min
     int16_t max;                            // servo max
     int16_t middle;                         // servo middle
-    int8_t rate;                            // range [-100;+100] ; can be used to adjust a rate 0-100% and a direction
-    uint8_t angleAtMin;                       // range [0;180] the measured angle in degrees from the middle when the servo is at the 'min' value.
-    uint8_t angleAtMax;                       // range [0;180] the measured angle in degrees from the middle when the servo is at the 'max' value.
+    int8_t rate;                           // range [-125;+125] ; can be used to adjust a rate 0-125% and a direction
+    uint32_t reversedSources;              // the direction of servo movement for each input source of the servo mixer, bit set=inverted
+    uint8_t angleAtMin;                     // range [0;180] the measured angle in degrees from the middle when the servo is at the 'min' value.
+    uint8_t angleAtMax;                     // range [0;180] the measured angle in degrees from the middle when the servo is at the 'max' value.
     int8_t forwardFromChannel;              // RX channel index, 0 based.  See CHANNEL_FORWARDING_DISABLED
 } servoParam_t;
 
@@ -128,7 +171,12 @@ void mixerUseConfigs(
 
 void writeAllMotors(int16_t mc);
 void mixerLoadMix(int index, motorMixer_t *customMixers);
-void mixerResetMotors(void);
+#ifdef USE_SERVOS
+void servoMixerLoadMix(int index, servoMixer_t *customServoMixers);
+void loadCustomServoMixer(void);
+int servoDirection(int servoIndex, int fromChannel);
+#endif
+void mixerResetDisarmedMotors(void);
 void mixTable(void);
 void writeMotors(void);
 void stopMotors(void);
diff --git a/src/main/io/gimbal.h b/src/main/io/gimbal.h
index 089ed50c60..914ce05ba5 100644
--- a/src/main/io/gimbal.h
+++ b/src/main/io/gimbal.h
@@ -17,12 +17,13 @@
 
 #pragma once
 
-typedef enum GimbalFlags {
-    GIMBAL_NORMAL = 1 << 0,
-    GIMBAL_MIXTILT = 1 << 1,
-    GIMBAL_FORWARDAUX = 1 << 2,
-} GimbalFlags;
+typedef enum {
+    GIMBAL_MODE_NORMAL = 0,
+    GIMBAL_MODE_MIXTILT = 1
+} gimbalMode_e;
+
+#define GIMBAL_MODE_MAX (GIMBAL_MODE_MIXTILT)
 
 typedef struct gimbalConfig_s {
-    uint8_t gimbal_flags;                   // in servotilt mode, various things that affect stuff
+    uint8_t mode;
 } gimbalConfig_t;
diff --git a/src/main/io/rc_controls.h b/src/main/io/rc_controls.h
index 61b9c71b08..de15935c44 100644
--- a/src/main/io/rc_controls.h
+++ b/src/main/io/rc_controls.h
@@ -43,12 +43,15 @@ typedef enum {
     BOXTELEMETRY,
     BOXAUTOTUNE,
     BOXSONAR,
+    BOXSERVO1,
+    BOXSERVO2,
+    BOXSERVO3,
     CHECKBOX_ITEM_COUNT
 } boxId_e;
 
 extern uint32_t rcModeActivationMask;
 
-#define IS_RC_MODE_ACTIVE(modeId) ((1 << modeId) & rcModeActivationMask)
+#define IS_RC_MODE_ACTIVE(modeId) ((1 << (modeId)) & rcModeActivationMask)
 #define ACTIVATE_RC_MODE(modeId) (rcModeActivationMask |= (1 << modeId))
 
 typedef enum rc_alias {
diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c
index ffa564d661..e0c700165c 100644
--- a/src/main/io/serial_cli.c
+++ b/src/main/io/serial_cli.c
@@ -98,7 +98,7 @@ static serialPort_t *cliPort;
 
 static void cliAux(char *cmdline);
 static void cliAdjustmentRange(char *cmdline);
-static void cliCMix(char *cmdline);
+static void cliMotorMix(char *cmdline);
 static void cliDefaults(char *cmdline);
 static void cliDump(char *cmdLine);
 static void cliExit(char *cmdline);
@@ -111,6 +111,7 @@ static void cliReboot(void);
 static void cliSave(char *cmdline);
 static void cliSerial(char *cmdline);
 static void cliServo(char *cmdline);
+static void cliServoMix(char *cmdline);
 static void cliSet(char *cmdline);
 static void cliGet(char *cmdline);
 static void cliStatus(char *cmdline);
@@ -151,7 +152,7 @@ static const char * const mixerNames[] = {
     "FLYING_WING", "Y4", "HEX6X", "OCTOX8", "OCTOFLATP", "OCTOFLATX",
     "AIRPLANE", "HELI_120_CCPM", "HELI_90_DEG", "VTAIL4",
     "HEX6H", "PPM_TO_SERVO", "DUALCOPTER", "SINGLECOPTER",
-    "ATAIL4", "CUSTOM", NULL
+    "ATAIL4", "CUSTOM", "CUSTOMAIRPLANE", "CUSTOMTRI", NULL
 };
 #endif
 
@@ -161,7 +162,7 @@ static const char * const featureNames[] = {
     "SERVO_TILT", "SOFTSERIAL", "GPS", "FAILSAFE",
     "SONAR", "TELEMETRY", "CURRENT_METER", "3D", "RX_PARALLEL_PWM",
     "RX_MSP", "RSSI_ADC", "LED_STRIP", "DISPLAY", "ONESHOT125",
-    "BLACKBOX", NULL
+    "BLACKBOX", "CHANNEL_FORWARDING", NULL
 };
 
 #ifndef CJMCU
@@ -182,52 +183,91 @@ static const char * const sensorHardwareNames[4][11] = {
 
 typedef struct {
     const char *name;
-    const char *param;
+    const char *description;
+    const char *args;
+
     void (*func)(char *cmdline);
 } clicmd_t;
 
+#ifndef SKIP_CLI_COMMAND_HELP
+#define CLI_COMMAND_DEF(name, description, args, method) \
+{ \
+    name , \
+    description , \
+    args , \
+    method \
+}
+#else
+#define CLI_COMMAND_DEF(name, description, args, method) \
+{ \
+    name, \
+    NULL, \
+    NULL, \
+    method \
+}
+#endif
+
 // should be sorted a..z for bsearch()
 const clicmd_t cmdTable[] = {
-    { "adjrange", "show/set adjustment ranges settings", cliAdjustmentRange },
-    { "aux", "show/set aux settings", cliAux },
-    { "cmix", "design custom mixer", cliCMix },
+    CLI_COMMAND_DEF("adjrange", "configure adjustment ranges", NULL, cliAdjustmentRange),
+    CLI_COMMAND_DEF("aux", "configure modes", NULL, cliAux),
 #ifdef LED_STRIP
-    { "color", "configure colors", cliColor },
+    CLI_COMMAND_DEF("color", "configure colors", NULL, cliColor),
 #endif
-    { "defaults", "reset to defaults and reboot", cliDefaults },
-    { "dump", "dump configuration", cliDump },
-    { "exit", "", cliExit },
-    { "feature", "list or -val or val", cliFeature },
+    CLI_COMMAND_DEF("defaults", "reset to defaults and reboot", NULL, cliDefaults),
+    CLI_COMMAND_DEF("dump", "dump configuration",
+        "[master|profile]", cliDump),
+    CLI_COMMAND_DEF("exit", NULL, NULL, cliExit),
+    CLI_COMMAND_DEF("feature", "configure features",
+        "list\r\n"
+        "\t<+|->[name]", cliFeature),
 #ifdef USE_FLASHFS
-    { "flash_erase", "erase flash chip", cliFlashErase },
-    { "flash_info", "get flash chip details", cliFlashInfo },
-    { "flash_read", "read text from the given address", cliFlashRead },
-    { "flash_write", "write text to the given address", cliFlashWrite },
+    CLI_COMMAND_DEF("flash_erase", "erase flash chip", NULL, cliFlashErase),
+    CLI_COMMAND_DEF("flash_info", "show flash chip info", NULL, cliFlashInfo),
+    CLI_COMMAND_DEF("flash_read", NULL, "<length> <address>", cliFlashRead),
+    CLI_COMMAND_DEF("flash_write", NULL, "<address> <message>", cliFlashWrite),
 #endif
-    { "get", "get variable value", cliGet },
+    CLI_COMMAND_DEF("get", "get variable value",
+            "[name]", cliGet),
 #ifdef GPS
-    { "gpspassthrough", "passthrough gps to serial", cliGpsPassthrough },
+    CLI_COMMAND_DEF("gpspassthrough", "passthrough gps to serial", NULL, cliGpsPassthrough),
 #endif
-    { "help", "", cliHelp },
+    CLI_COMMAND_DEF("help", NULL, NULL, cliHelp),
 #ifdef LED_STRIP
-    { "led", "configure leds", cliLed },
+    CLI_COMMAND_DEF("led", "configure leds", NULL, cliLed),
 #endif
-    { "map", "mapping of rc channel order", cliMap },
+    CLI_COMMAND_DEF("map", "configure rc channel order",
+        "[<map>]", cliMap),
 #ifndef USE_QUAD_MIXER_ONLY
-    { "mixer", "mixer name or list", cliMixer },
+    CLI_COMMAND_DEF("mixer", "configure mixer",
+        "list\r\n"
+        "\t<name>", cliMixer),
 #endif
-    { "motor", "get/set motor output value", cliMotor },
-    { "play_sound", "index, or none for next", cliPlaySound },
-    { "profile", "index (0 to 2)", cliProfile },
-    { "rateprofile", "index (0 to 2)", cliRateProfile },
-    { "save", "save and reboot", cliSave },
-    { "serial", "show/set serial settings", cliSerial },
+    CLI_COMMAND_DEF("mmix", "custom motor mixer", NULL, cliMotorMix),
+    CLI_COMMAND_DEF("motor",  "get/set motor",
+       "<index> [<value>]", cliMotor),
+    CLI_COMMAND_DEF("play_sound", NULL,
+        "[<index>]\r\n", cliPlaySound),
+    CLI_COMMAND_DEF("profile", "change profile",
+        "[<index>]", cliProfile),
+    CLI_COMMAND_DEF("rateprofile", "change rate profile",
+        "[<index>]", cliRateProfile),
+    CLI_COMMAND_DEF("save", "save and reboot", NULL, cliSave),
+    CLI_COMMAND_DEF("serial", "configure serial ports", NULL, cliSerial),
 #ifdef USE_SERVOS
-    { "servo", "servo config", cliServo },
+    CLI_COMMAND_DEF("servo", "configure servos", NULL, cliServo),
 #endif
-    { "set", "name=value or blank or * for list", cliSet },
-    { "status", "show system status", cliStatus },
-    { "version", "", cliVersion },
+    CLI_COMMAND_DEF("set", "change setting",
+        "[<name>=<value>]", cliSet),
+#ifdef USE_SERVOS
+    CLI_COMMAND_DEF("smix", "servo mixer",
+        "<rule> <servo> <source> <rate> <speed> <min> <max> <box>\r\n"
+        "\treset\r\n"
+        "\tload <mixer>\r\n"
+        "\treverse <servo> <source> r|n", cliServoMix),
+#endif
+    CLI_COMMAND_DEF("status", "show status", NULL, cliStatus),
+    CLI_COMMAND_DEF("version", "show version", NULL, cliVersion),
 };
 #define CMD_COUNT (sizeof(cmdTable) / sizeof(clicmd_t))
 
@@ -285,8 +325,6 @@ const clivalue_t valueTable[] = {
     { "auto_disarm_delay",          VAR_UINT8  | MASTER_VALUE,  &masterConfig.auto_disarm_delay, 0, 60 },
     { "small_angle",                VAR_UINT8  | MASTER_VALUE,  &masterConfig.small_angle, 0, 180 },
 
-    { "flaps_speed",                VAR_UINT8  | MASTER_VALUE,  &masterConfig.airplaneConfig.flaps_speed, 0, 100 },
-
     { "fixedwing_althold_dir",      VAR_INT8   | MASTER_VALUE,  &masterConfig.airplaneConfig.fixedwing_althold_dir, -1, 1 },
 
     { "reboot_character",           VAR_UINT8  | MASTER_VALUE,  &masterConfig.serialConfig.reboot_character, 48, 126 },
@@ -389,7 +427,7 @@ const clivalue_t valueTable[] = {
     { "rx_max_usec",                VAR_UINT16 | MASTER_VALUE,  &masterConfig.rxConfig.rx_max_usec, PWM_PULSE_MIN, PWM_PULSE_MAX },
 
 #ifdef USE_SERVOS
-    { "gimbal_flags",               VAR_UINT8  | PROFILE_VALUE, &masterConfig.profile[0].gimbalConfig.gimbal_flags, 0, 255},
+    { "gimbal_mode",                VAR_UINT8  | PROFILE_VALUE, &masterConfig.profile[0].gimbalConfig.mode, 0, GIMBAL_MODE_MAX},
 #endif
 
     { "acc_hardware",               VAR_UINT8  | MASTER_VALUE,  &masterConfig.acc_hardware, 0, ACC_MAX },
@@ -471,38 +509,42 @@ static void cliSetVar(const clivalue_t *var, const int_float_value_t value);
 static void cliPrintVar(const clivalue_t *var, uint32_t full);
 static void cliPrint(const char *str);
 static void cliWrite(uint8_t ch);
+
 static void cliPrompt(void)
 {
     cliPrint("\r\n# ");
 }
 
-static int cliCompare(const void *a, const void *b)
+static void cliShowParseError(void)
 {
-    const clicmd_t *ca = a, *cb = b;
-    return strncasecmp(ca->name, cb->name, strlen(cb->name));
+    cliPrint("Parse error\r\n");
+}
+
+static void cliShowArgumentRangeError(char *name, int min, int max)
+{
+    printf("%s must be between %d and %d\r\n", name, min, max);
 }
 
 static char *processChannelRangeArgs(char *ptr, channelRange_t *range, uint8_t *validArgumentCount)
 {
     int val;
-    ptr = strchr(ptr, ' ');
-    if (ptr) {
-        val = atoi(++ptr);
-        val = CHANNEL_VALUE_TO_STEP(val);
-        if (val >= MIN_MODE_RANGE_STEP && val <= MAX_MODE_RANGE_STEP) {
-            range->startStep = val;
-            (*validArgumentCount)++;
-        }
-    }
-    ptr = strchr(ptr, ' ');
-    if (ptr) {
-        val = atoi(++ptr);
-        val = CHANNEL_VALUE_TO_STEP(val);
-        if (val >= MIN_MODE_RANGE_STEP && val <= MAX_MODE_RANGE_STEP) {
-            range->endStep = val;
-            (*validArgumentCount)++;
+
+    for (int argIndex = 0; argIndex < 2; argIndex++) {
+        ptr = strchr(ptr, ' ');
+        if (ptr) {
+            val = atoi(++ptr);
+            val = CHANNEL_VALUE_TO_STEP(val);
+            if (val >= MIN_MODE_RANGE_STEP && val <= MAX_MODE_RANGE_STEP) {
+                if (argIndex == 0) {
+                    range->startStep = val;
+                } else {
+                    range->endStep = val;
+                }
+                (*validArgumentCount)++;
+            }
         }
     }
+
     return ptr;
 }
 
@@ -557,7 +599,7 @@ static void cliAux(char *cmdline)
                 memset(mac, 0, sizeof(modeActivationCondition_t));
             }
         } else {
-            printf("index: must be < %u\r\n", MAX_MODE_ACTIVATION_CONDITION_COUNT);
+            cliShowArgumentRangeError("index", 0, MAX_MODE_ACTIVATION_CONDITION_COUNT - 1);
         }
     }
 }
@@ -651,7 +693,7 @@ static void cliSerial(char *cmdline)
     }
 
     if (validArgumentCount < 6) {
-        cliPrint("Parse error\r\n");
+        cliShowParseError();
         return;
     }
 
@@ -684,6 +726,7 @@ static void cliAdjustmentRange(char *cmdline)
         if (i < MAX_ADJUSTMENT_RANGE_COUNT) {
             adjustmentRange_t *ar = &currentProfile->adjustmentRanges[i];
             uint8_t validArgumentCount = 0;
+
             ptr = strchr(ptr, ' ');
             if (ptr) {
                 val = atoi(++ptr);
@@ -700,7 +743,9 @@ static void cliAdjustmentRange(char *cmdline)
                     validArgumentCount++;
                 }
             }
+
             ptr = processChannelRangeArgs(ptr, &ar->range, &validArgumentCount);
+
             ptr = strchr(ptr, ' ');
             if (ptr) {
                 val = atoi(++ptr);
@@ -720,14 +765,15 @@ static void cliAdjustmentRange(char *cmdline)
 
             if (validArgumentCount != 6) {
                 memset(ar, 0, sizeof(adjustmentRange_t));
+                cliShowParseError();
             }
         } else {
-            printf("index: must be < %u\r\n", MAX_ADJUSTMENT_RANGE_COUNT);
+            cliShowArgumentRangeError("index", 0, MAX_ADJUSTMENT_RANGE_COUNT - 1);
         }
     }
 }
 
-static void cliCMix(char *cmdline)
+static void cliMotorMix(char *cmdline)
 {
 #ifdef USE_QUAD_MIXER_ONLY
     UNUSED(cmdline);
@@ -736,49 +782,38 @@ static void cliCMix(char *cmdline)
     int num_motors = 0;
     uint8_t len;
     char buf[16];
-    float mixsum[3];
     char *ptr;
 
     if (isEmpty(cmdline)) {
-        cliPrint("Custom mixer: \r\nMotor\tThr\tRoll\tPitch\tYaw\r\n");
+        cliPrint("Motor\tThr\tRoll\tPitch\tYaw\r\n");
         for (i = 0; i < MAX_SUPPORTED_MOTORS; i++) {
-            if (masterConfig.customMixer[i].throttle == 0.0f)
+            if (masterConfig.customMotorMixer[i].throttle == 0.0f)
                 break;
             num_motors++;
-            printf("#%d:\t", i + 1);
-            printf("%s\t", ftoa(masterConfig.customMixer[i].throttle, buf));
-            printf("%s\t", ftoa(masterConfig.customMixer[i].roll, buf));
-            printf("%s\t", ftoa(masterConfig.customMixer[i].pitch, buf));
-            printf("%s\r\n", ftoa(masterConfig.customMixer[i].yaw, buf));
+            printf("#%d:\t", i);
+            printf("%s\t", ftoa(masterConfig.customMotorMixer[i].throttle, buf));
+            printf("%s\t", ftoa(masterConfig.customMotorMixer[i].roll, buf));
+            printf("%s\t", ftoa(masterConfig.customMotorMixer[i].pitch, buf));
+            printf("%s\r\n", ftoa(masterConfig.customMotorMixer[i].yaw, buf));
         }
-        mixsum[0] = mixsum[1] = mixsum[2] = 0.0f;
-        for (i = 0; i < num_motors; i++) {
-            mixsum[0] += masterConfig.customMixer[i].roll;
-            mixsum[1] += masterConfig.customMixer[i].pitch;
-            mixsum[2] += masterConfig.customMixer[i].yaw;
-        }
-        cliPrint("Sanity check:\t");
-        for (i = 0; i < 3; i++)
-            cliPrint(fabsf(mixsum[i]) > 0.01f ? "NG\t" : "OK\t");
-        cliPrint("\r\n");
         return;
     } else if (strncasecmp(cmdline, "reset", 5) == 0) {
         // erase custom mixer
         for (i = 0; i < MAX_SUPPORTED_MOTORS; i++)
-            masterConfig.customMixer[i].throttle = 0.0f;
+            masterConfig.customMotorMixer[i].throttle = 0.0f;
     } else if (strncasecmp(cmdline, "load", 4) == 0) {
         ptr = strchr(cmdline, ' ');
         if (ptr) {
             len = strlen(++ptr);
             for (i = 0; ; i++) {
                 if (mixerNames[i] == NULL) {
-                    cliPrint("Invalid mixer type\r\n");
+                    cliPrint("Invalid name\r\n");
                     break;
                 }
                 if (strncasecmp(ptr, mixerNames[i], len) == 0) {
-                    mixerLoadMix(i, masterConfig.customMixer);
-                    printf("Loaded %s mix\r\n", mixerNames[i]);
-                    cliCMix("");
+                    mixerLoadMix(i, masterConfig.customMotorMixer);
+                    printf("Loaded %s\r\n", mixerNames[i]);
+                    cliMotorMix("");
                     break;
                 }
             }
@@ -786,34 +821,34 @@ static void cliCMix(char *cmdline)
     } else {
         ptr = cmdline;
         i = atoi(ptr); // get motor number
-        if (--i < MAX_SUPPORTED_MOTORS) {
+        if (i < MAX_SUPPORTED_MOTORS) {
             ptr = strchr(ptr, ' ');
             if (ptr) {
-                masterConfig.customMixer[i].throttle = fastA2F(++ptr);
+                masterConfig.customMotorMixer[i].throttle = fastA2F(++ptr);
                 check++;
             }
             ptr = strchr(ptr, ' ');
             if (ptr) {
-                masterConfig.customMixer[i].roll = fastA2F(++ptr);
+                masterConfig.customMotorMixer[i].roll = fastA2F(++ptr);
                 check++;
             }
             ptr = strchr(ptr, ' ');
             if (ptr) {
-                masterConfig.customMixer[i].pitch = fastA2F(++ptr);
+                masterConfig.customMotorMixer[i].pitch = fastA2F(++ptr);
                 check++;
             }
             ptr = strchr(ptr, ' ');
             if (ptr) {
-                masterConfig.customMixer[i].yaw = fastA2F(++ptr);
+                masterConfig.customMotorMixer[i].yaw = fastA2F(++ptr);
                 check++;
             }
             if (check != 4) {
-                cliPrint("Wrong number of arguments, needs idx thr roll pitch yaw\r\n");
+                cliShowParseError();
             } else {
-                cliCMix("");
+                cliMotorMix("");
             }
         } else {
-            printf("Motor number must be between 1 and %d\r\n", MAX_SUPPORTED_MOTORS);
+            cliShowArgumentRangeError("index", 1, MAX_SUPPORTED_MOTORS);
         }
     }
 #endif
@@ -837,10 +872,10 @@ static void cliLed(char *cmdline)
         if (i < MAX_LED_STRIP_LENGTH) {
             ptr = strchr(cmdline, ' ');
             if (!parseLedStripConfig(i, ++ptr)) {
-                cliPrint("Parse error\r\n");
+                cliShowParseError();
             }
         } else {
-            printf("Invalid led index: must be < %u\r\n", MAX_LED_STRIP_LENGTH);
+            cliShowArgumentRangeError("index", 0, MAX_LED_STRIP_LENGTH - 1);
         }
     }
 }
@@ -852,7 +887,12 @@ static void cliColor(char *cmdline)
 
     if (isEmpty(cmdline)) {
         for (i = 0; i < CONFIGURABLE_COLOR_COUNT; i++) {
-            printf("color %u %d,%u,%u\r\n", i, masterConfig.colors[i].h, masterConfig.colors[i].s, masterConfig.colors[i].v);
+            printf("color %u %d,%u,%u\r\n",
+                i,
+                masterConfig.colors[i].h,
+                masterConfig.colors[i].s,
+                masterConfig.colors[i].v
+            );
         }
     } else {
         ptr = cmdline;
@@ -860,10 +900,10 @@ static void cliColor(char *cmdline)
         if (i < CONFIGURABLE_COLOR_COUNT) {
             ptr = strchr(cmdline, ' ');
             if (!parseColor(i, ++ptr)) {
-                cliPrint("Parse error\r\n");
+                cliShowParseError();
             }
         } else {
-            printf("Invalid color index: must be < %u\r\n", CONFIGURABLE_COLOR_COUNT);
+            cliShowArgumentRangeError("index", 0, CONFIGURABLE_COLOR_COUNT - 1);
         }
     }
 }
@@ -909,7 +949,7 @@ static void cliServo(char *cmdline)
         while (*ptr) {
             if (*ptr == '-' || (*ptr >= '0' && *ptr <= '9')) {
                 if (validArgumentCount >= SERVO_ARGUMENT_COUNT) {
-                    cliPrint("Parse error\r\n");
+                    cliShowParseError();
                     return;
                 }
 
@@ -921,14 +961,14 @@ static void cliServo(char *cmdline)
             } else if (*ptr == ' ') {
                 ptr++;
             } else {
-                cliPrint("Parse error\r\n");
+                cliShowParseError();
                 return;
             }
         }
 
         // Check we got the right number of args and the servo index is correct (don't validate the other values)
         if (validArgumentCount != SERVO_ARGUMENT_COUNT || arguments[0] < 0 || arguments[0] >= MAX_SUPPORTED_SERVOS) {
-            cliPrint("Parse error\r\n");
+            cliShowParseError();
             return;
         }
 
@@ -945,6 +985,142 @@ static void cliServo(char *cmdline)
 #endif
 }
 
+static void cliServoMix(char *cmdline)
+{
+#ifndef USE_SERVOS
+    UNUSED(cmdline);
+#else
+    int i;
+    uint8_t len;
+    char *ptr;
+    int args[8], check = 0;
+    len = strlen(cmdline);
+
+    if (len == 0) {
+
+        cliPrint("Rule\tServo\tSource\tRate\tSpeed\tMin\tMax\tBox\r\n");
+
+        for (i = 0; i < MAX_SERVO_RULES; i++) {
+            if (masterConfig.customServoMixer[i].rate == 0)
+                break;
+
+            printf("#%d:\t%d\t%d\t%d\t%d\t%d\t%d\t%d\r\n",
+                i,
+                masterConfig.customServoMixer[i].targetChannel,
+                masterConfig.customServoMixer[i].inputSource,
+                masterConfig.customServoMixer[i].rate,
+                masterConfig.customServoMixer[i].speed,
+                masterConfig.customServoMixer[i].min,
+                masterConfig.customServoMixer[i].max,
+                masterConfig.customServoMixer[i].box
+            );
+        }
+        printf("\r\n");
+        return;
+    } else if (strncasecmp(cmdline, "reset", 5) == 0) {
+        // erase custom mixer
+        memset(masterConfig.customServoMixer, 0, sizeof(masterConfig.customServoMixer));
+        for (i = 0; i < MAX_SUPPORTED_SERVOS; i++) {
+            currentProfile->servoConf[i].reversedSources = 0;
+        }
+    } else if (strncasecmp(cmdline, "load", 4) == 0) {
+        ptr = strchr(cmdline, ' ');
+        if (ptr) {
+            len = strlen(++ptr);
+            for (i = 0; ; i++) {
+                if (mixerNames[i] == NULL) {
+                    printf("Invalid name\r\n");
+                    break;
+                }
+                if (strncasecmp(ptr, mixerNames[i], len) == 0) {
+                    servoMixerLoadMix(i, masterConfig.customServoMixer);
+                    printf("Loaded %s\r\n", mixerNames[i]);
+                    cliServoMix("");
+                    break;
+                }
+            }
+        }
+    } else if (strncasecmp(cmdline, "reverse", 7) == 0) {
+        enum {SERVO = 0, INPUT, REVERSE, ARGS_COUNT};
+        int servoIndex, inputSource;
+        ptr = strchr(cmdline, ' ');
+
+        len = strlen(ptr);
+        if (len == 0) {
+            printf("s");
+            for (inputSource = 0; inputSource < INPUT_SOURCE_COUNT; inputSource++)
+                printf("\ti%d", inputSource);
+            printf("\r\n");
+
+            for (servoIndex = 0; servoIndex < MAX_SUPPORTED_SERVOS; servoIndex++) {
+                printf("%d", servoIndex);
+                for (inputSource = 0; inputSource < INPUT_SOURCE_COUNT; inputSource++)
+                    printf("\t%s  ", (currentProfile->servoConf[servoIndex].reversedSources & (1 << inputSource)) ? "r" : "n");
+                printf("\r\n");
+            }
+            return;
+        }
+
+        ptr = strtok(ptr, " ");
+        while (ptr != NULL && check < ARGS_COUNT - 1) {
+            args[check++] = atoi(ptr);
+            ptr = strtok(NULL, " ");
+        }
+
+        if (ptr == NULL || check != ARGS_COUNT - 1) {
+            cliShowParseError();
+            return;
+        }
+
+        if (args[SERVO] >= 0 && args[SERVO] < MAX_SUPPORTED_SERVOS
+                && args[INPUT] >= 0 && args[INPUT] < INPUT_SOURCE_COUNT
+                && (*ptr == 'r' || *ptr == 'n')) {
+            if (*ptr == 'r')
+                currentProfile->servoConf[args[SERVO]].reversedSources |= 1 << args[INPUT];
+            else
+                currentProfile->servoConf[args[SERVO]].reversedSources &= ~(1 << args[INPUT]);
+        } else
+            cliShowParseError();
+
+        cliServoMix("reverse");
+    } else {
+        enum {RULE = 0, TARGET, INPUT, RATE, SPEED, MIN, MAX, BOX, ARGS_COUNT};
+        ptr = strtok(cmdline, " ");
+        while (ptr != NULL && check < ARGS_COUNT) {
+            args[check++] = atoi(ptr);
+            ptr = strtok(NULL, " ");
+        }
+
+        if (ptr != NULL || check != ARGS_COUNT) {
+            cliShowParseError();
+            return;
+        }
+
+        i = args[RULE];
+        if (i >= 0 && i < MAX_SERVO_RULES &&
+            args[TARGET] >= 0 && args[TARGET] < MAX_SUPPORTED_SERVOS &&
+            args[INPUT] >= 0 && args[INPUT] < INPUT_SOURCE_COUNT &&
+            args[RATE] >= -100 && args[RATE] <= 100 &&
+            args[SPEED] >= 0 && args[SPEED] <= MAX_SERVO_SPEED &&
+            args[MIN] >= 0 && args[MIN] <= 100 &&
+            args[MAX] >= 0 && args[MAX] <= 100 && args[MIN] < args[MAX] &&
+            args[BOX] >= 0 && args[BOX] <= MAX_SERVO_BOXES) {
+            masterConfig.customServoMixer[i].targetChannel = args[TARGET];
+            masterConfig.customServoMixer[i].inputSource = args[INPUT];
+            masterConfig.customServoMixer[i].rate = args[RATE];
+            masterConfig.customServoMixer[i].speed = args[SPEED];
+            masterConfig.customServoMixer[i].min = args[MIN];
+            masterConfig.customServoMixer[i].max = args[MAX];
+            masterConfig.customServoMixer[i].box = args[BOX];
+            cliServoMix("");
+        } else {
+            cliShowParseError();
+        }
+    }
+#endif
+}
+
+
 #ifdef USE_FLASHFS
 
 static void cliFlashInfo(char *cmdline)
@@ -961,7 +1137,7 @@ static void cliFlashErase(char *cmdline)
 {
     UNUSED(cmdline);
 
-    printf("Erasing, please wait...\r\n");
+    printf("Erasing...\r\n");
     flashfsEraseCompletely();
 
     while (!flashfsIsReady()) {
@@ -977,7 +1153,7 @@ static void cliFlashWrite(char *cmdline)
     char *text = strchr(cmdline, ' ');
 
     if (!text) {
-        printf("Missing text to write.\r\n");
+        cliShowParseError();
     } else {
         flashfsSeekAbs(address);
         flashfsWrite((uint8_t*)text, strlen(text), true);
@@ -998,7 +1174,7 @@ static void cliFlashRead(char *cmdline)
     char *nextArg = strchr(cmdline, ' ');
 
     if (!nextArg) {
-        printf("Missing length argument.\r\n");
+        cliShowParseError();
     } else {
         length = atoi(nextArg);
 
@@ -1059,7 +1235,7 @@ static const char* const sectionBreak = "\r\n";
 
 static void cliDump(char *cmdline)
 {
-    unsigned int i;
+    unsigned int i, channel;
     char buf[16];
     uint32_t mask;
 
@@ -1089,30 +1265,59 @@ static void cliDump(char *cmdline)
 #ifndef USE_QUAD_MIXER_ONLY
         printf("mixer %s\r\n", mixerNames[masterConfig.mixerMode - 1]);
 
-        if (masterConfig.customMixer[0].throttle != 0.0f) {
-            for (i = 0; i < MAX_SUPPORTED_MOTORS; i++) {
-                if (masterConfig.customMixer[i].throttle == 0.0f)
-                    break;
-                thr = masterConfig.customMixer[i].throttle;
-                roll = masterConfig.customMixer[i].roll;
-                pitch = masterConfig.customMixer[i].pitch;
-                yaw = masterConfig.customMixer[i].yaw;
-                printf("cmix %d", i + 1);
-                if (thr < 0)
-                    cliWrite(' ');
-                printf("%s", ftoa(thr, buf));
-                if (roll < 0)
-                    cliWrite(' ');
-                printf("%s", ftoa(roll, buf));
-                if (pitch < 0)
-                    cliWrite(' ');
-                printf("%s", ftoa(pitch, buf));
-                if (yaw < 0)
-                    cliWrite(' ');
-                printf("%s\r\n", ftoa(yaw, buf));
-            }
-            printf("cmix %d 0 0 0 0\r\n", i + 1);
+        printf("mmix reset\r\n");
+
+        for (i = 0; i < MAX_SUPPORTED_MOTORS; i++) {
+            if (masterConfig.customMotorMixer[i].throttle == 0.0f)
+                break;
+            thr = masterConfig.customMotorMixer[i].throttle;
+            roll = masterConfig.customMotorMixer[i].roll;
+            pitch = masterConfig.customMotorMixer[i].pitch;
+            yaw = masterConfig.customMotorMixer[i].yaw;
+            printf("mmix %d", i);
+            if (thr < 0)
+                cliWrite(' ');
+            printf("%s", ftoa(thr, buf));
+            if (roll < 0)
+                cliWrite(' ');
+            printf("%s", ftoa(roll, buf));
+            if (pitch < 0)
+                cliWrite(' ');
+            printf("%s", ftoa(pitch, buf));
+            if (yaw < 0)
+                cliWrite(' ');
+            printf("%s\r\n", ftoa(yaw, buf));
         }
+
+        // print custom servo mixer if exists
+        printf("smix reset\r\n");
+
+        for (i = 0; i < MAX_SERVO_RULES; i++) {
+
+            if (masterConfig.customServoMixer[i].rate == 0)
+                break;
+
+            printf("smix %d %d %d %d %d %d %d %d\r\n",
+                i,
+                masterConfig.customServoMixer[i].targetChannel,
+                masterConfig.customServoMixer[i].inputSource,
+                masterConfig.customServoMixer[i].rate,
+                masterConfig.customServoMixer[i].speed,
+                masterConfig.customServoMixer[i].min,
+                masterConfig.customServoMixer[i].max,
+                masterConfig.customServoMixer[i].box
+            );
+        }
+
+        // print servo directions
+        for (i = 0; i < MAX_SUPPORTED_SERVOS; i++) {
+            for (channel = 0; channel < INPUT_SOURCE_COUNT; channel++) {
+                if (servoDirection(i, channel) < 0) {
+                    printf("smix reverse %d %d r\r\n", i , channel);
+                }
+            }
+        }
+
 #endif
 
         cliPrint("\r\n\r\n# feature\r\n");
@@ -1204,7 +1409,7 @@ static void cliExit(char *cmdline)
     bufferIndex = 0;
     cliMode = 0;
     // incase a motor was left running during motortest, clear it here
-    mixerResetMotors();
+    mixerResetDisarmedMotors();
     cliReboot();
 
     cliPort = NULL;
@@ -1220,7 +1425,7 @@ static void cliFeature(char *cmdline)
     mask = featureMask();
 
     if (len == 0) {
-        cliPrint("Enabled features: ");
+        cliPrint("Enabled: ");
         for (i = 0; ; i++) {
             if (featureNames[i] == NULL)
                 break;
@@ -1229,7 +1434,7 @@ static void cliFeature(char *cmdline)
         }
         cliPrint("\r\n");
     } else if (strncasecmp(cmdline, "list", len) == 0) {
-        cliPrint("Available features: ");
+        cliPrint("Available: ");
         for (i = 0; ; i++) {
             if (featureNames[i] == NULL)
                 break;
@@ -1248,7 +1453,7 @@ static void cliFeature(char *cmdline)
 
         for (i = 0; ; i++) {
             if (featureNames[i] == NULL) {
-                cliPrint("Invalid feature name\r\n");
+                cliPrint("Invalid name\r\n");
                 break;
             }
 
@@ -1257,24 +1462,24 @@ static void cliFeature(char *cmdline)
                 mask = 1 << i;
 #ifndef GPS
                 if (mask & FEATURE_GPS) {
-                    cliPrint("GPS unavailable\r\n");
+                    cliPrint("unavailable\r\n");
                     break;
                 }
 #endif
 #ifndef SONAR
                 if (mask & FEATURE_SONAR) {
-                    cliPrint("SONAR unavailable\r\n");
+                    cliPrint("unavailable\r\n");
                     break;
                 }
 #endif
                 if (remove) {
                     featureClear(mask);
-                    cliPrint("Disabled ");
+                    cliPrint("Disabled");
                 } else {
                     featureSet(mask);
-                    cliPrint("Enabled ");
+                    cliPrint("Enabled");
                 }
-                printf("%s\r\n", featureNames[i]);
+                printf(" %s\r\n", featureNames[i]);
                 break;
             }
         }
@@ -1296,9 +1501,16 @@ static void cliHelp(char *cmdline)
 
     UNUSED(cmdline);
 
-    cliPrint("Available commands:\r\n");
-    for (i = 0; i < CMD_COUNT; i++)
-        printf("%s\t%s\r\n", cmdTable[i].name, cmdTable[i].param);
+    for (i = 0; i < CMD_COUNT; i++) {
+        cliPrint(cmdTable[i].name);
+        if (cmdTable[i].description) {
+            printf(" - %s", cmdTable[i].description);
+        }
+        if (cmdTable[i].args) {
+            printf("\r\n\t%s", cmdTable[i].args);
+        }
+        cliPrint("\r\n");
+    }
 }
 
 static void cliMap(char *cmdline)
@@ -1316,12 +1528,12 @@ static void cliMap(char *cmdline)
         for (i = 0; i < 8; i++) {
             if (strchr(rcChannelLetters, cmdline[i]) && !strchr(cmdline + i + 1, cmdline[i]))
                 continue;
-            cliPrint("Must be any order of AETR1234\r\n");
+            cliShowParseError();
             return;
         }
         parseRcChannels(cmdline, &masterConfig.rxConfig);
     }
-    cliPrint("Current assignment: ");
+    cliPrint("Map: ");
     for (i = 0; i < 8; i++)
         out[masterConfig.rxConfig.rcmap[i]] = rcChannelLetters[i];
     out[i] = '\0';
@@ -1337,7 +1549,7 @@ static void cliMixer(char *cmdline)
     len = strlen(cmdline);
 
     if (len == 0) {
-        printf("Current mixer: %s\r\n", mixerNames[masterConfig.mixerMode - 1]);
+        printf("Mixer: %s\r\n", mixerNames[masterConfig.mixerMode - 1]);
         return;
     } else if (strncasecmp(cmdline, "list", len) == 0) {
         cliPrint("Available mixers: ");
@@ -1352,15 +1564,16 @@ static void cliMixer(char *cmdline)
 
     for (i = 0; ; i++) {
         if (mixerNames[i] == NULL) {
-            cliPrint("Invalid mixer type\r\n");
-            break;
+            cliPrint("Invalid name\r\n");
+            return;
         }
         if (strncasecmp(cmdline, mixerNames[i], len) == 0) {
             masterConfig.mixerMode = i + 1;
-            printf("Mixer set to %s\r\n", mixerNames[i]);
             break;
         }
     }
+
+    cliMixer("");
 }
 #endif
 
@@ -1373,7 +1586,7 @@ static void cliMotor(char *cmdline)
     char *saveptr;
 
     if (isEmpty(cmdline)) {
-        cliPrint("Usage:\r\nmotor index [value] - show [or set] motor value\r\n");
+        cliShowParseError();
         return;
     }
 
@@ -1392,22 +1605,20 @@ static void cliMotor(char *cmdline)
     }
 
     if (motor_index < 0 || motor_index >= MAX_SUPPORTED_MOTORS) {
-        printf("No such motor, use a number [0, %d]\r\n", MAX_SUPPORTED_MOTORS);
+        cliShowArgumentRangeError("index", 0, MAX_SUPPORTED_MOTORS);
         return;
     }
 
-    if (index < 2) {
-        printf("Motor %d is set at %d\r\n", motor_index, motor_disarmed[motor_index]);
-        return;
+    if (index == 2) {
+        if (motor_value < PWM_RANGE_MIN || motor_value > PWM_RANGE_MAX) {
+            cliShowArgumentRangeError("value", 1000, 2000);
+            return;
+        } else {
+            motor_disarmed[motor_index] = motor_value;
+        }
     }
 
-    if (motor_value < PWM_RANGE_MIN || motor_value > PWM_RANGE_MAX) {
-        cliPrint("Invalid motor value, 1000..2000\r\n");
-        return;
-    }
-
-    printf("Setting motor %d to %d\r\n", motor_index, motor_value);
-    motor_disarmed[motor_index] = motor_value;
+    printf("motor %d: %d\r\n", motor_index, motor_disarmed[motor_index]);
 }
 
 static void cliPlaySound(char *cmdline)
@@ -1647,7 +1858,7 @@ static void cliSet(char *cmdline)
                 return;
             }
         }
-        cliPrint("Unknown variable name\r\n");
+        cliPrint("Invalid name\r\n");
     } else {
         // no equals, check for matching variables.
     	cliGet(cmdline);
@@ -1676,7 +1887,7 @@ static void cliGet(char *cmdline)
     	return;
     }
 
-    cliPrint("Unknown variable name\r\n");
+    cliPrint("Invalid name\r\n");
 }
 
 static void cliStatus(char *cmdline)
@@ -1784,14 +1995,12 @@ void cliProcess(void)
         } else if (!bufferIndex && c == 4) {   // CTRL-D
             cliExit(cliBuffer);
             return;
-        } else if (c == 12) {
+        } else if (c == 12) {                  // NewPage / CTRL-L
             // clear screen
             cliPrint("\033[2J\033[1;1H");
             cliPrompt();
         } else if (bufferIndex && (c == '\n' || c == '\r')) {
             // enter pressed
-            clicmd_t *cmd = NULL;
-            clicmd_t target;
             cliPrint("\r\n");
 
             // Strip comment starting with # from line
@@ -1809,11 +2018,14 @@ void cliProcess(void)
             // Process non-empty lines
             if (bufferIndex > 0) {
                 cliBuffer[bufferIndex] = 0; // null terminate
-                target.name = cliBuffer;
-                target.param = NULL;
 
-                cmd = bsearch(&target, cmdTable, CMD_COUNT, sizeof cmdTable[0], cliCompare);
-                if (cmd)
+                const clicmd_t *cmd;
+                for (cmd = cmdTable; cmd < cmdTable + CMD_COUNT; cmd++) {
+                    if(!strncasecmp(cliBuffer, cmd->name, strlen(cmd->name))   // command names match
+                       && !isalnum((unsigned)cliBuffer[strlen(cmd->name)]))    // next characted in bufffer is not alphanumeric (command is correctly terminated)
+                        break;
+                }
+                if(cmd < cmdTable + CMD_COUNT)
                     cmd->func(cliBuffer + strlen(cmd->name) + 1);
                 else
                     cliPrint("Unknown command, try 'help'");
diff --git a/src/main/io/serial_msp.c b/src/main/io/serial_msp.c
index 4ee293b423..927c3069f3 100644
--- a/src/main/io/serial_msp.c
+++ b/src/main/io/serial_msp.c
@@ -131,7 +131,7 @@ void useRcControlsConfig(modeActivationCondition_t *modeActivationConditions, es
 #define MSP_PROTOCOL_VERSION                0
 
 #define API_VERSION_MAJOR                   1 // increment when major changes are made
-#define API_VERSION_MINOR                   11 // increment when any change is made, reset to zero when major changes are released after changing API_VERSION_MAJOR
+#define API_VERSION_MINOR                   12 // increment when any change is made, reset to zero when major changes are released after changing API_VERSION_MAJOR
 
 #define API_VERSION_LENGTH                  2
 
@@ -173,9 +173,6 @@ static const char * const boardIdentifier = TARGET_BOARD_IDENTIFIER;
 //
 // MSP commands for Cleanflight original features
 //
-#define MSP_CHANNEL_FORWARDING          32    //out message         Returns channel forwarding settings
-#define MSP_SET_CHANNEL_FORWARDING      33    //in message          Channel forwarding settings
-
 #define MSP_MODE_RANGES                 34    //out message         Returns all mode ranges
 #define MSP_SET_MODE_RANGE              35    //in message          Sets a single mode range
 
@@ -256,9 +253,9 @@ static const char * const boardIdentifier = TARGET_BOARD_IDENTIFIER;
 
 #define MSP_STATUS               101    //out message         cycletime & errors_count & sensor present & box activation & current setting number
 #define MSP_RAW_IMU              102    //out message         9 DOF
-#define MSP_SERVO                103    //out message         8 servos
-#define MSP_MOTOR                104    //out message         8 motors
-#define MSP_RC                   105    //out message         8 rc chan and more
+#define MSP_SERVO                103    //out message         servos
+#define MSP_MOTOR                104    //out message         motors
+#define MSP_RC                   105    //out message         rc channels and more
 #define MSP_RAW_GPS              106    //out message         fix, numsat, lat, lon, alt, speed, ground course
 #define MSP_COMP_GPS             107    //out message         distance home, direction home
 #define MSP_ATTITUDE             108    //out message         2 angles 1 heading
@@ -273,7 +270,7 @@ static const char * const boardIdentifier = TARGET_BOARD_IDENTIFIER;
 #define MSP_PIDNAMES             117    //out message         the PID names
 #define MSP_WP                   118    //out message         get a WP, WP# is in the payload, returns (WP#, lat, lon, alt, flags) WP#0-home, WP#16-poshold
 #define MSP_BOXIDS               119    //out message         get the permanent IDs associated to BOXes
-#define MSP_SERVO_CONF           120    //out message         Servo settings
+#define MSP_SERVO_CONFIGURATIONS 120    //out message         All servo configurations.
 #define MSP_NAV_STATUS           121    //out message         Returns navigation status
 #define MSP_NAV_CONFIG           122    //out message         Returns navigation parameters
 
@@ -289,7 +286,7 @@ static const char * const boardIdentifier = TARGET_BOARD_IDENTIFIER;
 #define MSP_SET_WP               209    //in message          sets a given WP (WP#,lat, lon, alt, flags)
 #define MSP_SELECT_SETTING       210    //in message          Select Setting Number (0-2)
 #define MSP_SET_HEAD             211    //in message          define a new heading hold direction
-#define MSP_SET_SERVO_CONF       212    //in message          Servo settings
+#define MSP_SET_SERVO_CONFIGURATION 212    //in message          Servo settings
 #define MSP_SET_MOTOR            214    //in message          PropBalance function
 #define MSP_SET_NAV_CONFIG       215    //in message          Sets nav config parameters - write to the eeprom
 
@@ -305,11 +302,11 @@ static const char * const boardIdentifier = TARGET_BOARD_IDENTIFIER;
 #define MSP_ACC_TRIM             240    //out message         get acc angle trim values
 #define MSP_SET_ACC_TRIM         239    //in message          set acc angle trim values
 #define MSP_GPSSVINFO            164    //out message         get Signal Strength (only U-Blox)
+#define MSP_SERVO_MIX_RULES      241    //out message         Returns servo mixer configuration
+#define MSP_SET_SERVO_MIX_RULE   242    //in message          Sets servo mixer configuration
 
 #define INBUF_SIZE 64
 
-#define SERVO_CHUNK_SIZE 7
-
 typedef struct box_e {
     const uint8_t boxId;         // see boxId_e
     const char *boxName;            // GUI-readable box name
@@ -341,6 +338,10 @@ static const box_t boxes[CHECKBOX_ITEM_COUNT + 1] = {
     { BOXTELEMETRY, "TELEMETRY;", 20 },
     { BOXAUTOTUNE, "AUTOTUNE;", 21 },
     { BOXSONAR, "SONAR;", 22 },
+    { BOXSERVO1, "SERVO1;", 23 },
+    { BOXSERVO2, "SERVO2;", 24 },
+    { BOXSERVO3, "SERVO3;", 25 },
+    
     { CHECKBOX_ITEM_COUNT, NULL, 0xFF }
 };
 
@@ -621,8 +622,6 @@ void mspReleasePortIfAllocated(serialPort_t *serialPort)
 
 void mspInit(serialConfig_t *serialConfig)
 {
-    BUILD_BUG_ON((SERVO_CHUNK_SIZE * MAX_SUPPORTED_SERVOS) > INBUF_SIZE);
-
     // calculate used boxes based on features and fill availableBoxes[] array
     memset(activeBoxIds, 0xFF, sizeof(activeBoxIds));
 
@@ -681,6 +680,14 @@ void mspInit(serialConfig_t *serialConfig)
         activeBoxIds[activeBoxIdCount++] = BOXSONAR;
     }
 
+#ifdef USE_SERVOS
+    if (masterConfig.mixerMode == MIXER_CUSTOM_AIRPLANE) {
+        activeBoxIds[activeBoxIdCount++] = BOXSERVO1;
+        activeBoxIds[activeBoxIdCount++] = BOXSERVO2;
+        activeBoxIds[activeBoxIdCount++] = BOXSERVO3;
+    }
+#endif
+
     memset(mspPorts, 0x00, sizeof(mspPorts));
     mspAllocateSerialPorts(serialConfig);
 }
@@ -764,7 +771,7 @@ static bool processOutCommand(uint8_t cmdMSP)
         serialize8(MW_VERSION);
         serialize8(masterConfig.mixerMode);
         serialize8(MSP_PROTOCOL_VERSION);
-        serialize32(CAP_DYNBALANCE | (masterConfig.airplaneConfig.flaps_speed ? CAP_FLAPS : 0)); // "capability"
+        serialize32(CAP_DYNBALANCE); // "capability"
         break;
 
     case MSP_STATUS:
@@ -827,8 +834,8 @@ static bool processOutCommand(uint8_t cmdMSP)
     case MSP_SERVO:
         s_struct((uint8_t *)&servo, MAX_SUPPORTED_SERVOS * 2);
         break;
-    case MSP_SERVO_CONF:
-        headSerialReply(MAX_SUPPORTED_SERVOS * 9);
+    case MSP_SERVO_CONFIGURATIONS:
+        headSerialReply(MAX_SUPPORTED_SERVOS * sizeof(servoParam_t));
         for (i = 0; i < MAX_SUPPORTED_SERVOS; i++) {
             serialize16(currentProfile->servoConf[i].min);
             serialize16(currentProfile->servoConf[i].max);
@@ -836,12 +843,20 @@ static bool processOutCommand(uint8_t cmdMSP)
             serialize8(currentProfile->servoConf[i].rate);
             serialize8(currentProfile->servoConf[i].angleAtMin);
             serialize8(currentProfile->servoConf[i].angleAtMax);
+            serialize32(currentProfile->servoConf[i].reversedSources);
+            serialize8(currentProfile->servoConf[i].forwardFromChannel);
         }
         break;
-    case MSP_CHANNEL_FORWARDING:
-        headSerialReply(MAX_SUPPORTED_SERVOS);
-        for (i = 0; i < MAX_SUPPORTED_SERVOS; i++) {
-            serialize8(currentProfile->servoConf[i].forwardFromChannel);
+    case MSP_SERVO_MIX_RULES:
+        headSerialReply(MAX_SERVO_RULES * sizeof(servoMixer_t));
+        for (i = 0; i < MAX_SERVO_RULES; i++) {
+            serialize8(masterConfig.customServoMixer[i].targetChannel);
+            serialize8(masterConfig.customServoMixer[i].inputSource);
+            serialize8(masterConfig.customServoMixer[i].rate);
+            serialize8(masterConfig.customServoMixer[i].speed);
+            serialize8(masterConfig.customServoMixer[i].min);
+            serialize8(masterConfig.customServoMixer[i].max);
+            serialize8(masterConfig.customServoMixer[i].box);
         }
         break;
 #endif
@@ -1016,7 +1031,9 @@ static bool processOutCommand(uint8_t cmdMSP)
         serialize8(masterConfig.batteryConfig.vbatmaxcellvoltage);
         serialize8(masterConfig.batteryConfig.vbatwarningcellvoltage);
         break;
+
     case MSP_MOTOR_PINS:
+        // FIXME This is hardcoded and should not be.
         headSerialReply(8);
         for (i = 0; i < 8; i++)
             serialize8(i + 1);
@@ -1410,39 +1427,42 @@ static bool processInCommand(void)
         for (i = 0; i < 8; i++) // FIXME should this use MAX_MOTORS or MAX_SUPPORTED_MOTORS instead of 8
             motor_disarmed[i] = read16();
         break;
-    case MSP_SET_SERVO_CONF:
+    case MSP_SET_SERVO_CONFIGURATION:
 #ifdef USE_SERVOS
-        if (currentPort->dataSize % SERVO_CHUNK_SIZE != 0) {
-            debug[0] = currentPort->dataSize;
+        i = read8();
+        if (i >= MAX_SUPPORTED_SERVOS) {
             headSerialError(0);
         } else {
-            uint8_t servoCount = currentPort->dataSize / SERVO_CHUNK_SIZE;
-            for (i = 0; i < MAX_SUPPORTED_SERVOS && i < servoCount; i++) {
-                currentProfile->servoConf[i].min = read16();
-                currentProfile->servoConf[i].max = read16();
-
-                // provide temporary support for old clients that try and send a channel index instead of a servo middle
-                uint16_t potentialServoMiddleOrChannelToForward = read16();
-                if (potentialServoMiddleOrChannelToForward < MAX_SUPPORTED_SERVOS) {
-                    currentProfile->servoConf[i].forwardFromChannel = potentialServoMiddleOrChannelToForward;
-                }
-                if (potentialServoMiddleOrChannelToForward >= PWM_RANGE_MIN && potentialServoMiddleOrChannelToForward <= PWM_RANGE_MAX) {
-                    currentProfile->servoConf[i].middle = potentialServoMiddleOrChannelToForward;
-                }
-                currentProfile->servoConf[i].rate = read8();
-                currentProfile->servoConf[i].angleAtMin = read8();
-                currentProfile->servoConf[i].angleAtMax = read8();
-            }
-        }
-#endif
-        break;
-    case MSP_SET_CHANNEL_FORWARDING:
-#ifdef USE_SERVOS
-        for (i = 0; i < MAX_SUPPORTED_SERVOS; i++) {
+            currentProfile->servoConf[i].min = read16();
+            currentProfile->servoConf[i].max = read16();
+            currentProfile->servoConf[i].middle = read16();
+            currentProfile->servoConf[i].rate = read8();
+            currentProfile->servoConf[i].angleAtMin = read8();
+            currentProfile->servoConf[i].angleAtMax = read8();
+            currentProfile->servoConf[i].reversedSources = read32();
             currentProfile->servoConf[i].forwardFromChannel = read8();
         }
 #endif
         break;
+        
+    case MSP_SET_SERVO_MIX_RULE:
+#ifdef USE_SERVOS
+        i = read8();
+        if (i >= MAX_SERVO_RULES) {
+            headSerialError(0);
+        } else {
+            masterConfig.customServoMixer[i].targetChannel = read8();
+            masterConfig.customServoMixer[i].inputSource = read8();
+            masterConfig.customServoMixer[i].rate = read8();
+            masterConfig.customServoMixer[i].speed = read8();
+            masterConfig.customServoMixer[i].min = read8();
+            masterConfig.customServoMixer[i].max = read8();
+            masterConfig.customServoMixer[i].box = read8();
+            loadCustomServoMixer();
+        }
+#endif
+        break;
+        
     case MSP_RESET_CONF:
         if (!ARMING_FLAG(ARMED)) {
             resetEEPROM();
diff --git a/src/main/main.c b/src/main/main.c
index 185ee0f0ff..1a212e5805 100644
--- a/src/main/main.c
+++ b/src/main/main.c
@@ -106,7 +106,11 @@ void mspInit(serialConfig_t *serialConfig);
 void cliInit(serialConfig_t *serialConfig);
 void failsafeInit(rxConfig_t *intialRxConfig);
 pwmOutputConfiguration_t *pwmInit(drv_pwm_config_t *init);
-void mixerInit(mixerMode_e mixerMode, motorMixer_t *customMixers);
+#ifdef USE_SERVOS
+void mixerInit(mixerMode_e mixerMode, motorMixer_t *customMotorMixers, servoMixer_t *customServoMixers);
+#else
+void mixerInit(mixerMode_e mixerMode, motorMixer_t *customMotorMixers);
+#endif
 void mixerUsePWMOutputConfiguration(pwmOutputConfiguration_t *pwmOutputConfiguration);
 void rxInit(rxConfig_t *rxConfig);
 void gpsInit(serialConfig_t *serialConfig, gpsConfig_t *initialGpsConfig);
@@ -197,7 +201,11 @@ void init(void)
 
     serialInit(&masterConfig.serialConfig, feature(FEATURE_SOFTSERIAL));
 
-    mixerInit(masterConfig.mixerMode, masterConfig.customMixer);
+#ifdef USE_SERVOS
+    mixerInit(masterConfig.mixerMode, masterConfig.customMotorMixer, masterConfig.customServoMixer);
+#else
+    mixerInit(masterConfig.mixerMode, masterConfig.customMotorMixer);
+#endif
 
     memset(&pwm_params, 0, sizeof(pwm_params));
 
@@ -216,7 +224,7 @@ void init(void)
 #endif
 
     // when using airplane/wing mixer, servo/motor outputs are remapped
-    if (masterConfig.mixerMode == MIXER_AIRPLANE || masterConfig.mixerMode == MIXER_FLYING_WING)
+    if (masterConfig.mixerMode == MIXER_AIRPLANE || masterConfig.mixerMode == MIXER_FLYING_WING || masterConfig.mixerMode == MIXER_CUSTOM_AIRPLANE)
         pwm_params.airplane = true;
     else
         pwm_params.airplane = false;
@@ -241,7 +249,7 @@ void init(void)
 
 #ifdef USE_SERVOS
     pwm_params.useServos = isMixerUsingServos();
-    pwm_params.extraServos = currentProfile->gimbalConfig.gimbal_flags & GIMBAL_FORWARDAUX;
+    pwm_params.useChannelForwarding = feature(FEATURE_CHANNEL_FORWARDING);
     pwm_params.servoCenterPulse = masterConfig.escAndServoConfig.servoCenterPulse;
     pwm_params.servoPwmRate = masterConfig.servo_pwm_rate;
 #endif
diff --git a/src/main/mw.c b/src/main/mw.c
index 4e2995e093..059823dd9f 100644
--- a/src/main/mw.c
+++ b/src/main/mw.c
@@ -780,7 +780,7 @@ void loop(void)
         // Allow yaw control for tricopters if the user wants the servo to move even when unarmed.
         if (isUsingSticksForArming() && rcData[THROTTLE] <= masterConfig.rxConfig.mincheck
 #ifndef USE_QUAD_MIXER_ONLY
-                && !(masterConfig.mixerMode == MIXER_TRI && masterConfig.mixerConfig.tri_unarmed_servo)
+                && !((masterConfig.mixerMode == MIXER_TRI || masterConfig.mixerMode == MIXER_CUSTOM_TRI) && masterConfig.mixerConfig.tri_unarmed_servo)
                 && masterConfig.mixerMode != MIXER_AIRPLANE
                 && masterConfig.mixerMode != MIXER_FLYING_WING
 #endif
diff --git a/src/main/target/CC3D/target.h b/src/main/target/CC3D/target.h
index 2504bde7aa..f3f8e4e64d 100644
--- a/src/main/target/CC3D/target.h
+++ b/src/main/target/CC3D/target.h
@@ -120,6 +120,7 @@
 // disabled some features for OPBL build due to code size.
 #undef AUTOTUNE
 #undef SONAR
+#define SKIP_CLI_COMMAND_HELP
 #endif
 
 
diff --git a/src/test/unit/flight_mixer_unittest.cc b/src/test/unit/flight_mixer_unittest.cc
index f4981f5196..a1cac21195 100644
--- a/src/test/unit/flight_mixer_unittest.cc
+++ b/src/test/unit/flight_mixer_unittest.cc
@@ -48,7 +48,7 @@ extern "C" {
     extern uint8_t servoCount;
     void forwardAuxChannelsToServos(uint8_t firstServoIndex);
 
-    void mixerInit(mixerMode_e mixerMode, motorMixer_t *initialCustomMixers);
+    void mixerInit(mixerMode_e mixerMode, motorMixer_t *initialCustomMixers, servoMixer_t *initialCustomServoMixers);
     void mixerUsePWMOutputConfiguration(pwmOutputConfiguration_t *pwmOutputConfiguration);
 }
 
@@ -144,12 +144,16 @@ TEST(FlightMixerTest, TestForwardAuxChannelsToServosWithLessRemainingServosThanA
     EXPECT_EQ(1250, servos[MAX_SUPPORTED_SERVOS - 0 - 1].value);
 }
 
+
 TEST(FlightMixerTest, TestTricopterServo)
 {
     // given
     mixerConfig_t mixerConfig;
     memset(&mixerConfig, 0, sizeof(mixerConfig));
 
+    rxConfig_t rxConfig;
+    memset(&rxConfig, 0, sizeof(rxConfig));
+
     mixerConfig.tri_unarmed_servo = 1;
 
     escAndServoConfig_t escAndServoConfig;
@@ -165,7 +169,7 @@ TEST(FlightMixerTest, TestTricopterServo)
     servoConf[5].forwardFromChannel = CHANNEL_FORWARDING_DISABLED;
 
     gimbalConfig_t gimbalConfig = {
-        .gimbal_flags = 0
+        .mode = GIMBAL_MODE_NORMAL
     };
 
     mixerUseConfigs(
@@ -175,13 +179,16 @@ TEST(FlightMixerTest, TestTricopterServo)
         &escAndServoConfig,
         &mixerConfig,
         NULL,
-        NULL
+        &rxConfig
     );
 
     motorMixer_t customMixer[MAX_SUPPORTED_MOTORS];
     memset(&customMixer, 0, sizeof(customMixer));
 
-    mixerInit(MIXER_TRI, customMixer);
+    servoMixer_t customServoMixer[MAX_SUPPORTED_SERVOS];
+    memset(&customServoMixer, 0, sizeof(customServoMixer));
+
+    mixerInit(MIXER_TRI, customMixer, customServoMixer);
 
     // and
     pwmOutputConfiguration_t pwmOutputConfiguration = {
@@ -213,31 +220,38 @@ TEST(FlightMixerTest, TestQuadMotors)
     mixerConfig_t mixerConfig;
     memset(&mixerConfig, 0, sizeof(mixerConfig));
 
-    //servoParam_t servoConf[MAX_SUPPORTED_SERVOS];
-    //memset(&servoConf, 0, sizeof(servoConf));
+    rxConfig_t rxConfig;
+    memset(&rxConfig, 0, sizeof(rxConfig));
+
+    servoParam_t servoConf[MAX_SUPPORTED_SERVOS];
+    memset(&servoConf, 0, sizeof(servoConf));
 
     escAndServoConfig_t escAndServoConfig;
     memset(&escAndServoConfig, 0, sizeof(escAndServoConfig));
     escAndServoConfig.mincommand = TEST_MIN_COMMAND;
 
     gimbalConfig_t gimbalConfig = {
-        .gimbal_flags = 0
+        .mode = GIMBAL_MODE_NORMAL
     };
 
     mixerUseConfigs(
-        NULL,// servoConf,
+        servoConf,
         &gimbalConfig,
         NULL,
         &escAndServoConfig,
         &mixerConfig,
         NULL,
-        NULL
+        &rxConfig
     );
 
     motorMixer_t customMixer[MAX_SUPPORTED_MOTORS];
     memset(&customMixer, 0, sizeof(customMixer));
 
-    mixerInit(MIXER_QUADX, customMixer);
+    servoMixer_t customServoMixer[MAX_SUPPORTED_SERVOS];
+    memset(&customServoMixer, 0, sizeof(customServoMixer));
+
+
+    mixerInit(MIXER_QUADX, customMixer, customServoMixer);
 
     // and
     pwmOutputConfiguration_t pwmOutputConfiguration = {