diff --git a/src/imu.c b/src/imu.c
index 70a1fb51c..7d45b6ac8 100644
--- a/src/imu.c
+++ b/src/imu.c
@@ -391,24 +391,30 @@ int getEstimatedAltitude(void)
 
     // set vario
     vario = applyDeadband(vel_tmp, 5);
+    
+    if (abs(angle[ROLL]) < 800 && abs(angle[PITCH]) < 800) { // only calculate pid if the copters thrust is facing downwards(<80deg)
+        // Altitude P-Controller
+        error = constrain(AltHold - EstAlt, -500, 500);
+        error = applyDeadband(error, 10);       // remove small P parametr to reduce noise near zero position
+        setVel = constrain((cfg.P8[PIDALT] * error / 128), -300, +300); // limit velocity to +/- 3 m/s
 
-    // Altitude P-Controller
-    error = constrain(AltHold - EstAlt, -500, 500);
-    error = applyDeadband(error, 10);       // remove small P parametr to reduce noise near zero position
-    setVel = constrain((cfg.P8[PIDALT] * error / 128), -300, +300); // limit velocity to +/- 3 m/s
+        // Velocity PID-Controller
+        // P
+        error = setVel - vel_tmp;
+        BaroPID = constrain((cfg.P8[PIDVEL] * error / 32), -300, +300);
 
-    // Velocity PID-Controller
-    // P
-    error = setVel - vel_tmp;
-    BaroPID = constrain((cfg.P8[PIDVEL] * error / 32), -300, +300);
+        // I
+        errorAltitudeI += (cfg.I8[PIDVEL] * error) / 8;
+        errorAltitudeI = constrain(errorAltitudeI, -(1024 * 200), (1024 * 200));
+        BaroPID += errorAltitudeI / 1024;     // I in range +/-200
 
-    // I
-    errorAltitudeI += (cfg.I8[PIDVEL] * error) / 8;
-    errorAltitudeI = constrain(errorAltitudeI, -(1024 * 200), (1024 * 200));
-    BaroPID += errorAltitudeI / 1024;     // I in range +/-200
-
-    // D
-    BaroPID -= constrain(cfg.D8[PIDVEL] * (accZ_tmp + accZ_old) / 64, -150, 150);
+        // D
+        BaroPID -= constrain(cfg.D8[PIDVEL] * (accZ_tmp + accZ_old) / 64, -150, 150);
+        
+    } else {
+        BaroPID = 0;
+    }
+    
     accZ_old = accZ_tmp;
 
     return 1;
diff --git a/src/mw.c b/src/mw.c
index d92fe6eef..43a0881ae 100755
--- a/src/mw.c
+++ b/src/mw.c
@@ -854,7 +854,7 @@ void loop(void)
                                 AltHold = EstAlt;
                                 isAltHoldChanged = 0;
                             }
-                            rcCommand[THROTTLE] = initialThrottleHold + BaroPID;
+                            rcCommand[THROTTLE] = constrain(initialThrottleHold + BaroPID, mcfg.minthrottle + 100, mcfg.maxthrottle);
                         }
                     } else {
                         // slow alt changes for apfags
@@ -869,8 +869,7 @@ void loop(void)
                             AltHoldCorr = 0;
                             isAltHoldChanged = 0;
                         }
-                        rcCommand[THROTTLE] = initialThrottleHold + BaroPID;
-                        rcCommand[THROTTLE] = constrain(rcCommand[THROTTLE], mcfg.minthrottle + 150, mcfg.maxthrottle);
+                        rcCommand[THROTTLE] = constrain(initialThrottleHold + BaroPID, mcfg.minthrottle + 100, mcfg.maxthrottle);
                     }
                 } else {
                     // handle fixedwing-related althold. UNTESTED! and probably wrong