diff --git a/src/main/flight/navigation_rewrite.c b/src/main/flight/navigation_rewrite.c
index 380eb8dd39..1dfcb34132 100755
--- a/src/main/flight/navigation_rewrite.c
+++ b/src/main/flight/navigation_rewrite.c
@@ -1831,6 +1831,22 @@ bool isApproachingLastWaypoint(void)
     }
 }
 
+float getActiveWaypointSpeed(void)
+{
+    uint16_t waypointSpeed = posControl.navConfig->max_speed;
+
+    if (navGetStateFlags(posControl.navState) & NAV_AUTO_WP) {
+        if (posControl.waypointCount > 0 && posControl.waypointList[posControl.activeWaypointIndex].action == NAV_WP_ACTION_WAYPOINT) {
+            waypointSpeed = posControl.waypointList[posControl.activeWaypointIndex].p1;
+
+            if (waypointSpeed < 50 || waypointSpeed > posControl.navConfig->max_speed) {
+                waypointSpeed = posControl.navConfig->max_speed;
+            }
+        }
+    }
+
+    return waypointSpeed;
+}
 
 /*-----------------------------------------------------------
  * A function to reset navigation PIDs and states
diff --git a/src/main/flight/navigation_rewrite_multicopter.c b/src/main/flight/navigation_rewrite_multicopter.c
index 7a0b97fb1b..94e5b1a738 100755
--- a/src/main/flight/navigation_rewrite_multicopter.c
+++ b/src/main/flight/navigation_rewrite_multicopter.c
@@ -259,10 +259,10 @@ static float getVelocityHeadingAttenuationFactor(void)
     }
 }
 
-static float getVelocityExpoAttenuationFactor(float velTotal)
+static float getVelocityExpoAttenuationFactor(float velTotal, float velMax)
 {
     // Calculate factor of how velocity with applied expo is different from unchanged velocity
-    float velScale = constrainf(velTotal / posControl.navConfig->max_speed, 0.01f, 1.0f);
+    float velScale = constrainf(velTotal / velMax, 0.01f, 1.0f);
 
     // posControl.navConfig->max_speed * ((velScale * velScale * velScale) * posControl.posResponseExpo + velScale * (1 - posControl.posResponseExpo)) / velTotal;
     // ((velScale * velScale * velScale) * posControl.posResponseExpo + velScale * (1 - posControl.posResponseExpo)) / velScale
@@ -279,17 +279,20 @@ static void updatePositionVelocityController_MC(void)
     float newVelX = posErrorX * posControl.pids.pos[X].param.kP;
     float newVelY = posErrorY * posControl.pids.pos[Y].param.kP;
 
+    // Get max speed from generic NAV (waypoint specific), don't allow to move slower than 0.5 m/s
+    float maxSpeed = getActiveWaypointSpeed();
+
     // Scale velocity to respect max_speed
     float newVelTotal = sqrtf(sq(newVelX) + sq(newVelY));
-    if (newVelTotal > posControl.navConfig->max_speed) {
-        newVelX = posControl.navConfig->max_speed * (newVelX / newVelTotal);
-        newVelY = posControl.navConfig->max_speed * (newVelY / newVelTotal);
-        newVelTotal = posControl.navConfig->max_speed;
+    if (newVelTotal > maxSpeed) {
+        newVelX = maxSpeed * (newVelX / newVelTotal);
+        newVelY = maxSpeed * (newVelY / newVelTotal);
+        newVelTotal = maxSpeed;
     }
 
     // Apply expo & attenuation if heading in wrong direction - turn first, accelerate later (effective only in WP mode)
     float velHeadFactor = getVelocityHeadingAttenuationFactor();
-    float velExpoFactor = getVelocityExpoAttenuationFactor(newVelTotal);
+    float velExpoFactor = getVelocityExpoAttenuationFactor(newVelTotal, maxSpeed);
     posControl.desiredState.vel.V.X = newVelX * velHeadFactor * velExpoFactor;
     posControl.desiredState.vel.V.Y = newVelY * velHeadFactor * velExpoFactor;
 
diff --git a/src/main/flight/navigation_rewrite_private.h b/src/main/flight/navigation_rewrite_private.h
index 42c3d489c9..5699ed810c 100755
--- a/src/main/flight/navigation_rewrite_private.h
+++ b/src/main/flight/navigation_rewrite_private.h
@@ -286,6 +286,7 @@ void setDesiredPositionToFarAwayTarget(int32_t yaw, int32_t distance, navSetWayp
 bool isWaypointReached(navWaypointPosition_t * waypoint);
 bool isWaypointMissed(navWaypointPosition_t * waypoint);
 bool isApproachingLastWaypoint(void);
+float getActiveWaypointSpeed(void);
 
 int16_t rcCommandToLeanAngle(int16_t rcCommand);
 int16_t leanAngleToRcCommand(int16_t leanAngle);