diff --git a/src/main/config/config.c b/src/main/config/config.c
index dc7fc82e32e..7839b192792 100755
--- a/src/main/config/config.c
+++ b/src/main/config/config.c
@@ -137,7 +137,7 @@ static uint32_t activeFeaturesLatch = 0;
 static uint8_t currentControlRateProfileIndex = 0;
 controlRateConfig_t *currentControlRateProfile;
 
-static const uint8_t EEPROM_CONF_VERSION = 112;
+static const uint8_t EEPROM_CONF_VERSION = 113;
 
 void resetPidProfile(pidProfile_t *pidProfile)
 {
@@ -300,10 +300,10 @@ static void resetControlRateConfig(controlRateConfig_t *controlRateConfig) {
     controlRateConfig->rcExpo8 = 85;
     controlRateConfig->thrMid8 = 50;
     controlRateConfig->dynThrPID = 0;
-    controlRateConfig->tpa_yaw_rate = 90;
     controlRateConfig->rcYawExpo8 = 83;
     controlRateConfig->tpa_breakpoint = 1500;
-    controlRateConfig->tpa_yaw_breakpoint = 1500;
+    controlRateConfig->tri_dynamic_yaw_minthrottle = 300; // 3x YAW gain at min throttle
+    controlRateConfig->tri_dynamic_yaw_maxthrottle = 100; // no reduction by default
 
     controlRateConfig->rates[FD_PITCH] = 55;
     controlRateConfig->rates[FD_ROLL] = 55;
@@ -335,8 +335,10 @@ static void resetMixerConfig(mixerConfig_t *mixerConfig) {
     mixerConfig->tri_servo_feedback = TRI_SERVO_FB_RSSI;
 #else
     mixerConfig->tri_servo_feedback = TRI_SERVO_FB_VIRTUAL;
-#endif
-#endif
+#endif //RCE
+    mixerConfig->tri_motor_acc_yaw_correction = 6;
+    mixerConfig->tri_motor_acceleration = 0.20f;
+#endif //USE_SERVOS
 }
 
 uint8_t getCurrentProfile(void)
@@ -369,6 +371,11 @@ uint16_t getCurrentMinthrottle(void)
     return masterConfig.escAndServoConfig.minthrottle;
 }
 
+uint16_t getCurrentMaxthrottle(void)
+{
+    return masterConfig.escAndServoConfig.maxthrottle;
+}
+
 // Default settings
 STATIC_UNIT_TESTED void resetConf(void)
 {
diff --git a/src/main/config/config.h b/src/main/config/config.h
index 26b00729d25..bbd97222a02 100644
--- a/src/main/config/config.h
+++ b/src/main/config/config.h
@@ -75,3 +75,4 @@ void changeControlRateProfile(uint8_t profileIndex);
 bool canSoftwareSerialBeUsed(void);
 
 uint16_t getCurrentMinthrottle(void);
+uint16_t getCurrentMaxthrottle(void);
diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c
index 55add840639..a7358712a57 100755
--- a/src/main/flight/mixer.c
+++ b/src/main/flight/mixer.c
@@ -691,80 +691,81 @@ STATIC_UNIT_TESTED void servoMixer(void)
     static int16_t currentOutput[MAX_SERVO_RULES];
     uint8_t i;
 
-    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
-        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;
-        }
+    if ((currentMixerMode == MIXER_TRI) || (currentMixerMode == MIXER_CUSTOM_TRI))
+    {
+        triServoMixer(axisPID[YAW]);
     }
-
-    input[INPUT_GIMBAL_PITCH] = scaleRange(attitude.values.pitch, -1800, 1800, -500, +500);
-    input[INPUT_GIMBAL_ROLL] = scaleRange(attitude.values.roll, -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
+    {
+        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 {
-            currentOutput[i] = 0;
+            // Assisted modes (gyro only or gyro+acc according to AUX configuration in Gui
+            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 = 0; i < MAX_SUPPORTED_SERVOS; i++) {
-        servo[i] = ((int32_t)servoConf[i].rate * servo[i]) / 100L;
-        servo[i] += determineServoMiddleOrForwardFromChannel(i);
+        input[INPUT_GIMBAL_PITCH] = scaleRange(attitude.values.pitch, -1800, 1800, -500, +500);
+        input[INPUT_GIMBAL_ROLL] = scaleRange(attitude.values.roll, -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;
+            }
+        }
 
-    if (currentMixerMode == MIXER_TRI || currentMixerMode == MIXER_CUSTOM_TRI)
-    {
-        triServoMixer();
+        for (i = 0; i < MAX_SUPPORTED_SERVOS; i++) {
+            servo[i] = ((int32_t)servoConf[i].rate * servo[i]) / 100L;
+            servo[i] += determineServoMiddleOrForwardFromChannel(i);
+        }
     }
 }
 #endif
diff --git a/src/main/flight/mixer.h b/src/main/flight/mixer.h
index c01a61b787e..2aebd50c17d 100644
--- a/src/main/flight/mixer.h
+++ b/src/main/flight/mixer.h
@@ -89,6 +89,8 @@ typedef struct mixerConfig_s {
     uint16_t tri_servo_min_adc;
     uint16_t tri_servo_mid_adc;
     uint16_t tri_servo_max_adc;
+    uint16_t tri_motor_acc_yaw_correction;
+    float tri_motor_acceleration;
 #endif
 } mixerConfig_t;
 
diff --git a/src/main/flight/mixer_tricopter.c b/src/main/flight/mixer_tricopter.c
index 19c59ef7273..0daddaeab2c 100644
--- a/src/main/flight/mixer_tricopter.c
+++ b/src/main/flight/mixer_tricopter.c
@@ -74,6 +74,7 @@
 #include "flight/failsafe.h"
 #include "flight/imu.h"
 #include "flight/navigation.h"
+#include "flight/pid.h"
 
 #include "config/runtime_config.h"
 #include "config/config.h"
@@ -82,10 +83,16 @@
 
 #include "mw.h"
 
+
 #ifdef USE_SERVOS
-#define TRI_TAIL_SERVO_ANGLE_MID (900)
-#define TRI_YAW_FORCE_CURVE_SIZE (100)
-#define TRI_TAIL_SERVO_MAX_ANGLE (500)
+#define USE_AUX_CHANNEL_TUNING      (0)
+#define TRI_TAIL_SERVO_ANGLE_MID    (900)
+#define TRI_YAW_FORCE_CURVE_SIZE    (100)
+#define TRI_TAIL_SERVO_MAX_ANGLE    (500)
+
+#define TRI_TAIL_MOTOR_INDEX        (0)
+
+#define TRI_YAW_FORCE_PRECISION     (1000)
 
 #define IsDelayElapsed_us(timestamp_us, delay_us) ((uint32_t)(micros() - timestamp_us) >= delay_us)
 #define IsDelayElapsed_ms(timestamp_ms, delay_ms) ((uint32_t)(millis() - timestamp_ms) >= delay_ms)
@@ -109,13 +116,21 @@ static int16_t tailMotorDecelerationDelay_ms = 100;
 static int16_t tailMotorAccelerationDelay_angle;
 static int16_t tailMotorDecelerationDelay_angle;
 
+//! Virtual tail motor speed feedback
+static float tailMotorVirtual = 1000.0f;
+//! Configured output throttle range (max - min)
+static int16_t throttleRange = 0;
+//! Motor acceleration in output units (us) / second
+static float motorAcceleration = 0;
+//! Reset the I term when tail motor deceleration has lasted (ms)
+static uint16_t resetITermDecelerationLasted_ms = 0;
+
 static servoParam_t * gpTailServoConf;
 static int16_t *gpTailServo;
 static mixerConfig_t *gpMixerConfig;
 
 static uint16_t tailServoADC = 0;
 static AdcChannel tailServoADCChannel = ADC_EXTERNAL1;
-
 static void initCurves();
 static uint16_t getServoValueAtAngle(servoParam_t * servoConf, uint16_t angle);
 static float getPitchCorrectionAtTailAngle(float angle, float thrustFactor);
@@ -126,7 +141,7 @@ static uint16_t getPitchCorrectionMaxPhaseShift(int16_t servoAngle,
         int16_t motorAccelerationDelayAngle,
         int16_t motorDecelerationDelayAngle,
         int16_t motorDirectionChangeAngle);
-static uint16_t getLinearServoValue(servoParam_t *servoConf, uint16_t servoValue);
+static uint16_t getLinearServoValue(servoParam_t *servoConf, int16_t constrainedPIDOutput);
 static uint16_t virtualServoStep(uint16_t currentAngle, int16_t servoSpeed, float dT, servoParam_t *servoConf, uint16_t servoValue);
 static uint16_t feedbackServoStep(mixerConfig_t *mixerConf, uint16_t tailServoADC);
 STATIC_UNIT_TESTED void tailTuneModeThrustTorque(thrustTorque_t *pTT, const bool isThrottleHigh);
@@ -134,6 +149,12 @@ static void tailTuneModeServoSetup(struct servoSetup_t *pSS, servoParam_t *pServ
 static void triTailTuneStep(servoParam_t *pServoConf, int16_t *pServoVal);
 static void updateServoAngle(void);
 static void updateServoFeedbackADCChannel(uint8_t tri_servo_feedback);
+static void checkMotorAcceleration(void);
+#if USE_AUX_CHANNEL_TUNING
+static int16_t scaleAUXChannel(u8 channel, int16_t scale);
+#endif
+static int16_t dynamicYaw(int16_t PIDoutput);
+static void tailMotorStep(int16_t setpoint, float dT);
 #endif
 
 void triInitMixer(servoParam_t *pTailServoConfig,
@@ -147,6 +168,12 @@ void triInitMixer(servoParam_t *pTailServoConfig,
     tailServoSpeed = pMixerConfig->tri_tail_servo_speed;
     gpMixerConfig = pMixerConfig;
 
+    throttleRange = getCurrentMaxthrottle() - getCurrentMinthrottle();
+    motorAcceleration = (float)throttleRange / gpMixerConfig->tri_motor_acceleration;
+
+    // Reset the I term when motor deceleration has lasted 35% of the min to max time
+    resetITermDecelerationLasted_ms = gpMixerConfig->tri_motor_acceleration * 1000.0f * 0.35f;
+
     initCurves();
     updateServoFeedbackADCChannel(gpMixerConfig->tri_servo_feedback);
 }
@@ -169,7 +196,7 @@ static void initCurves()
     for (int32_t i = 0; i < TRI_YAW_FORCE_CURVE_SIZE; i++)
     {
         const float angleRad = DEGREES_TO_RADIANS(angle / 10.0f);
-        yawForceCurve[i] = 1000.0f * (-tailServoThrustFactor * cosf(angleRad) - sinf(angleRad)) * getPitchCorrectionAtTailAngle(angleRad, tailServoThrustFactor);
+        yawForceCurve[i] = TRI_YAW_FORCE_PRECISION * (-tailServoThrustFactor * cosf(angleRad) - sinf(angleRad)) * getPitchCorrectionAtTailAngle(angleRad, tailServoThrustFactor);
         // Only calculate the top forces in the configured angle range
         if ((angle >= minAngle) && (angle <= maxAngle))
         {
@@ -187,20 +214,20 @@ uint16_t triGetCurrentServoAngle()
     return tailServoAngle;
 }
 
-static uint16_t getLinearServoValue(servoParam_t *servoConf, uint16_t servoValue)
+static uint16_t getLinearServoValue(servoParam_t *servoConf, int16_t constrainedPIDOutput)
 {
-    const int16_t servoMid = servoConf->middle;
-    // First find the yaw force at given servo value from a linear curve
-    const int16_t servoRange = (servoValue < servoMid) ? servoMid - servoConf->min : servoConf->max - servoMid;
-    const int32_t linearYawForceAtValue = (int32_t)(tailServoMaxYawForce) * (servoValue - servoMid) / servoRange;
+    const int32_t linearYawForceAtValue = tailServoMaxYawForce * constrainedPIDOutput / TRI_YAW_FORCE_PRECISION;
     const int16_t correctedAngle = getAngleFromYawCurveAtForce(linearYawForceAtValue);
-    debug[2] = servoValue;
-    debug[3] = tailServoMaxYawForce;
     return getServoValueAtAngle(servoConf, correctedAngle);
 }
 
-void triServoMixer()
+void triServoMixer(int16_t PIDoutput)
 {
+    // Dynamic yaw expects input [-1000, 1000]
+    PIDoutput = constrain(PIDoutput, -1000, 1000);
+    // Scale the PID output based on tail motor speed (thrust)
+    PIDoutput = dynamicYaw(PIDoutput);
+
     static filterStatePt1_t feedbackFilter;
     if (gpMixerConfig->tri_servo_feedback != TRI_SERVO_FB_VIRTUAL)
     {
@@ -211,18 +238,24 @@ void triServoMixer()
     // Linear servo logic only in armed state
     if (ARMING_FLAG(ARMED))
     {
-        *gpTailServo = getLinearServoValue(gpTailServoConf, *gpTailServo);
+        *gpTailServo = getLinearServoValue(gpTailServoConf, PIDoutput);
     }
 
     triTailTuneStep(gpTailServoConf, gpTailServo);
 
     updateServoAngle();
+
+    // Update the tail motor virtual feedback
+    tailMotorStep(motor[TRI_TAIL_MOTOR_INDEX], dT);
+
+    // Check the tail motor acceleration
+    checkMotorAcceleration();
 }
 
 int16_t triGetMotorCorrection(uint8_t motorIndex)
 {
     uint16_t correction = 0;
-    if (motorIndex == 0)
+    if (motorIndex == TRI_TAIL_MOTOR_INDEX)
     {
         // Adjust tail motor speed based on servo angle. Check how much to adjust speed from pitch force curve based on servo angle.
         // Take motor speed up lag into account by shifting the phase of the curve
@@ -239,7 +272,15 @@ int16_t triGetMotorCorrection(uint8_t motorIndex)
         }
 
         const int16_t futureServoAngle = constrain(servoAngle + angleDiff, TRI_TAIL_SERVO_ANGLE_MID - tailServoMaxAngle, TRI_TAIL_SERVO_ANGLE_MID + tailServoMaxAngle);
-        const uint16_t throttleMotorOutput = rcCommand[THROTTLE] - getCurrentMinthrottle();
+        uint16_t throttleMotorOutput = tailMotorVirtual - getCurrentMinthrottle();
+        /* Increased yaw authority at min throttle, always calculate the pitch
+         * correction on at least half motor output. This produces a little bit
+         * more forward pitch, but tested to be negligible.
+         *
+         * TODO: this is not the best way to achieve this, but how could the min_throttle
+         * pitch correction be calculated, as the thrust is zero?
+         */
+        throttleMotorOutput = constrain(throttleMotorOutput, throttleRange / 2, 1000);
         correction = (throttleMotorOutput * getPitchCorrectionAtTailAngle(DEGREES_TO_RADIANS(futureServoAngle / 10.0f), tailServoThrustFactor)) - throttleMotorOutput;
     }
 
@@ -767,3 +808,120 @@ static void updateServoFeedbackADCChannel(uint8_t tri_servo_feedback)
         break;
     }
 }
+
+#if USE_AUX_CHANNEL_TUNING
+static int16_t scaleAUXChannel(u8 channel, int16_t scale)
+{
+    int16_t constrained = constrain(rcData[channel], 1000, 2000);
+    constrained -= 1000;
+
+    return constrained * scale / 1000;
+}
+#endif
+
+static void checkMotorAcceleration(void)
+{
+    static float previousMotorSpeed = 1000.0f;
+    static uint32_t accelerationStartedAt_ms = 0;
+    static bool accelerating = false;
+
+    float tailMotorSpeed = tailMotorVirtual;
+    // Calculate how much the motor speed changed since last time
+    float acceleration = (tailMotorSpeed - previousMotorSpeed);
+    previousMotorSpeed = tailMotorSpeed;
+
+    // Check if acceleration has changed to deceleration and vice versa
+    if (acceleration > 0.0f)
+    {
+        if (!accelerating)
+        {
+            // Take a timestamp when direction changes
+            accelerationStartedAt_ms = millis();
+            accelerating = true;
+        }
+    }
+    else
+    {
+        if (accelerating)
+        {
+            accelerationStartedAt_ms = millis();
+            accelerating = false;
+        }
+    }
+
+    /* Reset the I term in a dynamic situation where the motor is causing yaw error for longer period of time.
+     * This helps especially when throttle is cut, the copter is quickly losing yaw authority and we can't
+     * have the slow integrator resisting the correction.
+     *
+     * Tests have shown that this is mostly needed when throttle is cut (motor decelerating), so only
+     * reset I term in that case.
+     */
+    if (!accelerating && IsDelayElapsed_ms(accelerationStartedAt_ms, resetITermDecelerationLasted_ms))
+    {
+        pidResetErrorGyroAxis(FD_YAW);
+    }
+
+    // Set the expected axis error based on tail motor acceleration and configured gain
+    pidSetExpectedGyroError(FD_YAW, acceleration * gpMixerConfig->tri_motor_acc_yaw_correction / 10);
+}
+
+static int16_t dynamicYaw(int16_t PIDoutput)
+{
+    const int16_t halfRange = throttleRange / 2;
+    const int16_t midpoint = getCurrentMinthrottle() + halfRange;
+
+    uint16_t gain;
+    // Select the yaw gain based on tail motor speed
+    if (tailMotorVirtual < midpoint)
+    {
+        /* Below midpoint, gain is increasing the output.
+         * e.g. 150 (%) increases the yaw output at min throttle by 150 % (1.5x)
+         * e.g. 250 (%) increases the yaw output at min throttle by 250 % (2.5x)
+         */
+        gain = currentControlRateProfile->tri_dynamic_yaw_minthrottle - 100;
+    }
+    else
+    {
+        /* Above midpoint, gain is decreasing the output.
+         * e.g. 75 (%) reduces the yaw output at max throttle by 25 % (0.75x)
+         * e.g. 20 (%) reduces the yaw output at max throttle by 80 % (0.2x)
+         */
+        gain = 100 - currentControlRateProfile->tri_dynamic_yaw_maxthrottle;
+    }
+
+    const int16_t distanceFromMid = (tailMotorVirtual - midpoint);
+    const int16_t scaledPIDoutput = (PIDoutput - distanceFromMid * gain * PIDoutput / (halfRange * 100));
+    return constrain(scaledPIDoutput, -1000, 1000);
+}
+
+static void tailMotorStep(int16_t setpoint, float dT)
+{
+    static float current = 1000;
+    float dS; // Max change of an speed since last check
+
+    dS = dT * motorAcceleration;
+
+    if ( ABS(current - setpoint) < dS )
+    {
+        // At set-point after this moment
+        current = setpoint;
+    }
+    else if (current < setpoint)
+    {
+        current += dS;
+    }
+    else
+    {
+        current -= dS;
+    }
+
+    // Use a PT1 low-pass filter to add "slowness" to the virtual motor feedback.
+    /* Cut-off to delay:
+     * 2  Hz -> 25 ms
+     * 5  Hz -> 14 ms
+     * 10 Hz -> 9  ms
+     */
+    static filterStatePt1_t motorFilter;
+    tailMotorVirtual = filterApplyPt1(current, &motorFilter, 20, dT);
+}
+
diff --git a/src/main/flight/mixer_tricopter.h b/src/main/flight/mixer_tricopter.h
index a33b53269ba..19d082e2ca1 100644
--- a/src/main/flight/mixer_tricopter.h
+++ b/src/main/flight/mixer_tricopter.h
@@ -24,6 +24,8 @@
 #define TAIL_THRUST_FACTOR_MIN_FLOAT  (TAIL_THRUST_FACTOR_MIN / 10.0f)
 #define TAIL_THRUST_FACTOR_MAX_FLOAT  (TAIL_THRUST_FACTOR_MAX / 10.0f)
 
+#define TRI_MOTOR_ACC_CORRECTION_MAX  (200)
+
 /** @brief Servo feedback sources. */
 typedef enum {
     TRI_SERVO_FB_VIRTUAL = 0,  ///< Virtual servo, no physical feedback signal from servo
@@ -52,9 +54,10 @@ uint16_t triGetCurrentServoAngle();
 
 /** @brief Perform tricopter mixer actions.
  *
+ *  @param PIDoutput output from PID controller, in scale of [-1000, 1000].
  *  @return Void.
  */
-void triServoMixer();
+void triServoMixer(int16_t PIDoutput);
 
 /** @brief Get amount of motor correction that must be applied
  * for given motor.
diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c
index a1fff9326cc..f42915282b1 100644
--- a/src/main/flight/pid.c
+++ b/src/main/flight/pid.c
@@ -51,9 +51,11 @@
 #include "config/runtime_config.h"
 #include "config/config_unittest.h"
 
+#include "debug.h"
+
 //! Integrator is disabled when rate error exceeds this limit
-#define LUXFLOAT_INTEGRATOR_DISABLE_LIMIT_DPS (40.0f)
-#define REWRITE_INTEGRATOR_DISABLE_LIMIT_DPS (160)
+#define LUXFLOAT_INTEGRATOR_DISABLE_LIMIT_DPS (50.0f)
+#define REWRITE_INTEGRATOR_DISABLE_LIMIT_DPS (50 * 41 / 10)
 
 extern uint8_t motorCount;
 extern float dT;
@@ -72,6 +74,8 @@ static float errorGyroIf[3];
 static int32_t errorAngleI[2];
 static float errorAngleIf[2];
 
+static int16_t expectedGyroError[3] = {0};
+
 static void pidMultiWiiRewrite(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig,
         uint16_t max_angle_inclination, rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig);
 
@@ -98,6 +102,17 @@ void pidResetErrorGyro(void)
     }
 }
 
+void pidResetErrorGyroAxis(flight_dynamics_index_t axis)
+{
+    errorGyroI[axis] = 0;
+    errorGyroIf[axis] = 0.0f;
+}
+
+void pidSetExpectedGyroError(flight_dynamics_index_t axis, int16_t error)
+{
+    expectedGyroError[axis] = error;
+}
+
 float scaleItermToRcInput(int axis) {
     float rcCommandDeflection = (float)rcCommand[axis] / 500.0f;
     static float iTermScaler[3] = {1.0f, 1.0f, 1.0f};
@@ -209,14 +224,14 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa
         // Used in stand-alone mode for ACRO, controlled by higher level regulators in other modes
         // -----calculate scaled error.AngleRates
         // multiplication of rcCommand corresponds to changing the sticks scaling here
-        RateError = AngleRate - gyroRate;
+        RateError = AngleRate - gyroRate + (float)expectedGyroError[axis];
 
         // -----calculate P component
 
         PTerm = RateError * pidProfile->P_f[axis] * PIDweight[axis] / 100;
 
         const float newIntegral = constrainf(errorGyroIf[axis] + RateError * dT * pidProfile->I_f[axis] * 10 * Iweigth[axis] / 100, -PID_LUX_FLOAT_MAX_I, PID_LUX_FLOAT_MAX_I);
-        if (fabsf(RateError) < LUXFLOAT_INTEGRATOR_DISABLE_LIMIT_DPS)
+        if (fabsf(gyroRate) < LUXFLOAT_INTEGRATOR_DISABLE_LIMIT_DPS)
         {
             errorGyroIf[axis] = newIntegral;
         }
@@ -496,7 +511,7 @@ static void pidMultiWiiRewrite(pidProfile_t *pidProfile, controlRateConfig_t *co
         // -----calculate scaled error.AngleRates
         // multiplication of rcCommand corresponds to changing the sticks scaling here
         gyroRate = gyroADC[axis] / 4;
-        RateError = AngleRateTmp - gyroRate;
+        RateError = AngleRateTmp - gyroRate + (expectedGyroError[axis] * 41);
 
         // -----calculate P component
         PTerm = (RateError * pidProfile->P8[axis] * PIDweight[axis] / 100) >> 7;
@@ -511,7 +526,7 @@ static void pidMultiWiiRewrite(pidProfile_t *pidProfile, controlRateConfig_t *co
         // I coefficient (I8) moved before integration to make limiting independent from PID settings
         newIntegral = constrain(newIntegral, (int32_t) - GYRO_I_MAX << 13, (int32_t) + GYRO_I_MAX << 13);
 
-        if (ABS(RateError) < REWRITE_INTEGRATOR_DISABLE_LIMIT_DPS)
+        if (ABS(gyroRate) < REWRITE_INTEGRATOR_DISABLE_LIMIT_DPS)
         {
             errorGyroI[axis] = newIntegral;
         }
diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h
index ccf406b1c07..e40ce027987 100644
--- a/src/main/flight/pid.h
+++ b/src/main/flight/pid.h
@@ -86,4 +86,5 @@ extern int32_t axisPID_P[3], axisPID_I[3], axisPID_D[3];
 void pidSetController(pidControllerType_e type);
 void pidResetErrorAngle(void);
 void pidResetErrorGyro(void);
-
+void pidResetErrorGyroAxis(flight_dynamics_index_t axis);
+void pidSetExpectedGyroError(flight_dynamics_index_t axis, int16_t error);
diff --git a/src/main/io/rc_controls.h b/src/main/io/rc_controls.h
index 7db68fd66d3..31a5c2cbc60 100644
--- a/src/main/io/rc_controls.h
+++ b/src/main/io/rc_controls.h
@@ -114,7 +114,6 @@ typedef enum {
 #define CONTROL_RATE_CONFIG_YAW_RATE_MAX         255
 
 #define CONTROL_RATE_CONFIG_TPA_MAX              100
-#define CONTROL_RATE_CONFIG_YAW_TPA_MAX          255
 
 // steps are 25 apart
 // a value of 0 corresponds to a channel value of 900 or less
@@ -140,10 +139,10 @@ typedef struct controlRateConfig_s {
     uint8_t thrExpo8;
     uint8_t rates[3];
     uint8_t dynThrPID;
-    uint8_t tpa_yaw_rate;
     uint8_t rcYawExpo8;
     uint16_t tpa_breakpoint;                // Breakpoint where TPA is activated
-    uint16_t tpa_yaw_breakpoint;
+    uint16_t tri_dynamic_yaw_minthrottle;
+    uint16_t tri_dynamic_yaw_maxthrottle;
 } controlRateConfig_t;
 
 extern int16_t rcCommand[4];
diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c
index 17d16739c8a..f9d4897f357 100644
--- a/src/main/io/serial_cli.c
+++ b/src/main/io/serial_cli.c
@@ -608,6 +608,8 @@ const clivalue_t valueTable[] = {
     { "tri_tail_motor_thrustfactor",VAR_INT16  | MASTER_VALUE, &masterConfig.mixerConfig.tri_tail_motor_thrustfactor, .config.minmax = { TAIL_THRUST_FACTOR_MIN, TAIL_THRUST_FACTOR_MAX } },
     { "tri_tail_servo_speed",       VAR_INT16  | MASTER_VALUE, &masterConfig.mixerConfig.tri_tail_servo_speed, .config.minmax = { 0, 1000 } },
     { "tri_servo_feedback",         VAR_UINT8  | MASTER_VALUE | MODE_LOOKUP, &masterConfig.mixerConfig.tri_servo_feedback, .config.lookup = { TABLE_SERVO_FEEDBACK } },
+    { "tri_motor_acc_yaw_correction",VAR_UINT16| MASTER_VALUE, &masterConfig.mixerConfig.tri_motor_acc_yaw_correction, .config.minmax = { 0, TRI_MOTOR_ACC_CORRECTION_MAX } },
+    { "tri_motor_acceleration",     VAR_FLOAT  | MASTER_VALUE, &masterConfig.mixerConfig.tri_motor_acceleration, .config.minmax = { 0.01f, 1.0f } },
 #endif
 
     { "default_rate_profile",       VAR_UINT8  | PROFILE_VALUE , &masterConfig.profile[0].defaultRateProfileIndex, .config.minmax = { 0,  MAX_CONTROL_RATE_PROFILE_COUNT - 1 } },
@@ -621,8 +623,8 @@ const clivalue_t valueTable[] = {
     { "yaw_rate",                   VAR_UINT8  | CONTROL_RATE_VALUE, &masterConfig.controlRateProfiles[0].rates[FD_YAW], .config.minmax = { 0,  CONTROL_RATE_CONFIG_YAW_RATE_MAX } },
     { "tpa_rate",                   VAR_UINT8  | CONTROL_RATE_VALUE, &masterConfig.controlRateProfiles[0].dynThrPID, .config.minmax = { 0,  CONTROL_RATE_CONFIG_TPA_MAX} },
     { "tpa_breakpoint",             VAR_UINT16 | CONTROL_RATE_VALUE, &masterConfig.controlRateProfiles[0].tpa_breakpoint, .config.minmax = { PWM_RANGE_MIN,  PWM_RANGE_MAX} },
-    { "tpa_yaw_rate",               VAR_UINT8  | CONTROL_RATE_VALUE, &masterConfig.controlRateProfiles[0].tpa_yaw_rate, .config.minmax = { 0, CONTROL_RATE_CONFIG_YAW_TPA_MAX} },
-    { "tpa_yaw_breakpoint",         VAR_UINT16 | CONTROL_RATE_VALUE, &masterConfig.controlRateProfiles[0].tpa_yaw_breakpoint, .config.minmax = { PWM_RANGE_MIN, PWM_RANGE_MAX} },
+    { "tri_dynamic_yaw_minthrottle",VAR_UINT16 | CONTROL_RATE_VALUE, &masterConfig.controlRateProfiles[0].tri_dynamic_yaw_minthrottle, .config.minmax = { 0, 500} },
+    { "tri_dynamic_yaw_maxthrottle",VAR_UINT16 | CONTROL_RATE_VALUE, &masterConfig.controlRateProfiles[0].tri_dynamic_yaw_maxthrottle, .config.minmax = { 0, 100} },
 
     { "failsafe_delay",             VAR_UINT8  | MASTER_VALUE,  &masterConfig.failsafeConfig.failsafe_delay, .config.minmax = { 0,  200 } },
     { "failsafe_off_delay",         VAR_UINT8  | MASTER_VALUE,  &masterConfig.failsafeConfig.failsafe_off_delay, .config.minmax = { 0,  200 } },
diff --git a/src/main/mw.c b/src/main/mw.c
index 437781f43d4..33d9b17792f 100644
--- a/src/main/mw.c
+++ b/src/main/mw.c
@@ -173,7 +173,7 @@ bool isCalibrating()
 void annexCode(void)
 {
     int32_t tmp, tmp2;
-    int32_t axis, prop1 = 0, prop2, yawPDWeigth, yawIWeigth;
+    int32_t axis, prop1 = 0, prop2, yawIWeigth;
 
     // PITCH & ROLL dynamic PID adjustment,  depending on throttle value
     if (rcData[THROTTLE] < currentControlRateProfile->tpa_breakpoint) {
@@ -186,31 +186,6 @@ void annexCode(void)
         }
     }
 
-    // YAW dynamic PID adjustment
-    if (rcData[THROTTLE] < currentControlRateProfile->tpa_yaw_breakpoint) {
-        yawPDWeigth = 100;
-    } else {
-        if (rcData[THROTTLE] < 2000) {
-            if (currentControlRateProfile->tpa_yaw_rate >= 100)
-            {
-                yawPDWeigth = 100 + (uint16_t)(currentControlRateProfile->tpa_yaw_rate - 100) * (rcData[THROTTLE] - currentControlRateProfile->tpa_yaw_breakpoint) / (2000 - currentControlRateProfile->tpa_yaw_breakpoint);
-            }
-            else
-            {
-                yawPDWeigth = 100 - (uint16_t)currentControlRateProfile->tpa_yaw_rate * (rcData[THROTTLE] - currentControlRateProfile->tpa_yaw_breakpoint) / (2000 - currentControlRateProfile->tpa_yaw_breakpoint);
-            }
-        } else {
-            if (currentControlRateProfile->tpa_yaw_rate >= 100)
-            {
-                yawPDWeigth = currentControlRateProfile->tpa_yaw_rate;
-            }
-            else
-            {
-                yawPDWeigth = 100 - currentControlRateProfile->tpa_yaw_rate;
-            }
-        }
-    }
-
     // YAW dynamic integral adjustment
     // Attenuate integral term proportional to the stick position, 100% attenuation starting at full stick position
     int32_t yawCommandFromCenter = ABS(rcData[YAW] - 1500);
@@ -250,7 +225,7 @@ void annexCode(void)
 
         // non coupled PID reduction scaler used in PID controller 1 and PID controller 2. YAW TPA disabled. 100 means 100% of the pids
         if (axis == YAW) {
-            PIDweight[axis] = yawPDWeigth;
+            PIDweight[axis] = 100;
             Iweigth[axis] = yawIWeigth;
         }
         else {
@@ -955,4 +930,4 @@ bool isRcAxisWithinDeadband(int32_t axis)
     }
 
     return ret;
-}
\ No newline at end of file
+}