From 4df6e9aca967038712d1be7d7d677537a7e6678c Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Alberto=20Garci=CC=81a=20Hierro?= <alberto@garciahierro.com>
Date: Wed, 26 Dec 2018 11:19:36 +0000
Subject: [PATCH] Add navCalculatePathToDestination() function

Calculates distance and bearing to the given location in local
coordinate space.

Added navDestinationPath_t, which encapsulates distance and
bearing.

Refactor calculateDistanceToDestination() and
calculateBearingToDestination() a bit to move the calculations to
static functions that can be reused by navCalculatePathTo()
---
 src/main/navigation/navigation.c | 41 +++++++++++++++++++++++++++-----
 src/main/navigation/navigation.h | 10 ++++++++
 2 files changed, 45 insertions(+), 6 deletions(-)

diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c
index 17eb7ec53c..6a9d61cff3 100755
--- a/src/main/navigation/navigation.c
+++ b/src/main/navigation/navigation.c
@@ -1899,20 +1899,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 14e2328ab5..8886c873fa 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;
@@ -384,6 +391,9 @@ void geoConvertGeodeticToLocal(gpsOrigin_s * origin, const gpsLocation_t * llh,
 void geoConvertLocalToGeodetic(const gpsOrigin_s * origin, const fpVector3_t * pos, gpsLocation_t * llh);
 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);