diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c
index 5638cccc70..566eab484e 100755
--- a/src/main/navigation/navigation.c
+++ b/src/main/navigation/navigation.c
@@ -1900,20 +1900,49 @@ const navEstimatedPosVel_t * navGetCurrentActualPositionAndVelocity(void)
 /*-----------------------------------------------------------
  * Calculates distance and bearing to destination point
  *-----------------------------------------------------------*/
+static uint32_t calculateDistanceFromDelta(float deltaX, float deltaY)
+{
+    return sqrtf(sq(deltaX) + sq(deltaY));
+}
+
+static int32_t calculateBearingFromDelta(float deltaX, float deltaY)
+{
+    return wrap_36000(RADIANS_TO_CENTIDEGREES(atan2_approx(deltaY, deltaX)));
+}
+
 uint32_t calculateDistanceToDestination(const fpVector3_t * destinationPos)
 {
-    const float deltaX = destinationPos->x - navGetCurrentActualPositionAndVelocity()->pos.x;
-    const float deltaY = destinationPos->y - navGetCurrentActualPositionAndVelocity()->pos.y;
+    const navEstimatedPosVel_t *posvel = navGetCurrentActualPositionAndVelocity();
+    const float deltaX = destinationPos->x - posvel->pos.x;
+    const float deltaY = destinationPos->y - posvel->pos.y;
 
-    return sqrtf(sq(deltaX) + sq(deltaY));
+    return calculateDistanceFromDelta(deltaX, deltaY);
 }
 
 int32_t calculateBearingToDestination(const fpVector3_t * destinationPos)
 {
-    const float deltaX = destinationPos->x - navGetCurrentActualPositionAndVelocity()->pos.x;
-    const float deltaY = destinationPos->y - navGetCurrentActualPositionAndVelocity()->pos.y;
+    const navEstimatedPosVel_t *posvel = navGetCurrentActualPositionAndVelocity();
+    const float deltaX = destinationPos->x - posvel->pos.x;
+    const float deltaY = destinationPos->y - posvel->pos.y;
 
-    return wrap_36000(RADIANS_TO_CENTIDEGREES(atan2_approx(deltaY, deltaX)));
+    return calculateBearingFromDelta(deltaX, deltaY);
+}
+
+bool navCalculatePathToDestination(navDestinationPath_t *result, const fpVector3_t * destinationPos)
+{
+    if (posControl.flags.estPosStatus == EST_NONE ||
+        posControl.flags.estHeadingStatus == EST_NONE) {
+
+        return false;
+    }
+
+    const navEstimatedPosVel_t *posvel = navGetCurrentActualPositionAndVelocity();
+    const float deltaX = destinationPos->x - posvel->pos.x;
+    const float deltaY = destinationPos->y - posvel->pos.y;
+
+    result->distance = calculateDistanceFromDelta(deltaX, deltaY);
+    result->bearing = calculateBearingFromDelta(deltaX, deltaY);
+    return true;
 }
 
 /*-----------------------------------------------------------
diff --git a/src/main/navigation/navigation.h b/src/main/navigation/navigation.h
index 39075eab4f..75b869816c 100755
--- a/src/main/navigation/navigation.h
+++ b/src/main/navigation/navigation.h
@@ -17,6 +17,8 @@
 
 #pragma once
 
+#include <stdbool.h>
+
 #include "common/axis.h"
 #include "common/filter.h"
 #include "common/maths.h"
@@ -222,6 +224,11 @@ typedef struct {
     int32_t     yaw;             // deg * 100
 } navWaypointPosition_t;
 
+typedef struct navDestinationPath_s {
+    uint32_t distance; // meters * 100
+    int32_t bearing; // deg * 100
+} navDestinationPath_t;
+
 typedef struct {
     float kP;
     float kI;
@@ -400,6 +407,9 @@ bool geoConvertGeodeticToLocalOrigin(fpVector3_t * pos, const gpsLocation_t *llh
 bool geoConvertLocalToGeodetic(gpsLocation_t *llh, const gpsOrigin_t *origin, const fpVector3_t *pos);
 float geoCalculateMagDeclination(const gpsLocation_t * llh); // degrees units
 
+/* Distance/bearing calculation */
+bool navCalculatePathToDestination(navDestinationPath_t *result, const fpVector3_t * destinationPos);
+
 /* Failsafe-forced RTH mode */
 void activateForcedRTH(void);
 void abortForcedRTH(void);