Cleanup geo* functions

- Rename gpsOrigin_s type to gpsOrigin_t
- Make geo* conversion functions return wether they succeeded or not
- Reorder geo* functions arguments to follow the (output, inputs)
convention
- Document all the geo* functions

src/main/navigation/navigation.c

@@ -1943,7 +1943,7 @@ bool isWaypointReached(const navWaypointPosition_t * waypoint, const bool isWayp
 
 static void updateHomePositionCompatibility(void)
 {
-    geoConvertLocalToGeodetic(&posControl.gpsOrigin, &posControl.homePosition.pos, &GPS_home);
+    geoConvertLocalToGeodetic(&GPS_home, &posControl.gpsOrigin, &posControl.homePosition.pos);
     GPS_distanceToHome = posControl.homeDistance / 100;
     GPS_directionToHome = posControl.homeDirection / 100;
 }
@@ -2480,7 +2480,7 @@ void getWaypoint(uint8_t wpNumber, navWaypoint_t * wpData)
     else if (wpNumber == 255) {
         gpsLocation_t wpLLH;
 
-        geoConvertLocalToGeodetic(&posControl.gpsOrigin, &navGetCurrentActualPositionAndVelocity()->pos, &wpLLH);
+        geoConvertLocalToGeodetic(&wpLLH, &posControl.gpsOrigin, &navGetCurrentActualPositionAndVelocity()->pos);
 
         wpData->lat = wpLLH.lat;
         wpData->lon = wpLLH.lon;
@@ -2507,7 +2507,7 @@ void setWaypoint(uint8_t wpNumber, const navWaypoint_t * wpData)
     // WP #0 - special waypoint - HOME
     if ((wpNumber == 0) && ARMING_FLAG(ARMED) && (posControl.flags.estPosStatus >= EST_USABLE) && posControl.gpsOrigin.valid && posControl.flags.isGCSAssistedNavigationEnabled) {
         // Forcibly set home position. Note that this is only valid if already armed, otherwise home will be reset instantly
-        geoConvertGeodeticToLocal(&posControl.gpsOrigin, &wpLLH, &wpPos.pos, GEO_ALT_RELATIVE);
+        geoConvertGeodeticToLocal(&wpPos.pos, &posControl.gpsOrigin, &wpLLH, GEO_ALT_RELATIVE);
         setHomePosition(&wpPos.pos, 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING, NAV_HOME_VALID_ALL);
     }
     // WP #255 - special waypoint - directly set desiredPosition
@@ -2516,7 +2516,7 @@ void setWaypoint(uint8_t wpNumber, const navWaypoint_t * wpData)
              ARMING_FLAG(ARMED) && (posControl.flags.estPosStatus == EST_TRUSTED) && posControl.gpsOrigin.valid && posControl.flags.isGCSAssistedNavigationEnabled &&
              (posControl.navState == NAV_STATE_POSHOLD_3D_IN_PROGRESS)) {
         // Convert to local coordinates
-        geoConvertGeodeticToLocal(&posControl.gpsOrigin, &wpLLH, &wpPos.pos, GEO_ALT_RELATIVE);
+        geoConvertGeodeticToLocal(&wpPos.pos, &posControl.gpsOrigin, &wpLLH, GEO_ALT_RELATIVE);
 
         navSetWaypointFlags_t waypointUpdateFlags = NAV_POS_UPDATE_XY;
 
@@ -2611,7 +2611,7 @@ static void mapWaypointToLocalPosition(fpVector3_t * localPos, const navWaypoint
     wpLLH.lon = waypoint->lon;
     wpLLH.alt = waypoint->alt;
 
-    geoConvertGeodeticToLocal(&posControl.gpsOrigin, &wpLLH, localPos, GEO_ALT_RELATIVE);
+    geoConvertGeodeticToLocal(localPos, &posControl.gpsOrigin, &wpLLH, GEO_ALT_RELATIVE);
 }
 
 static void calculateAndSetActiveWaypointToLocalPosition(const fpVector3_t * pos)

src/main/navigation/navigation.h

@@ -197,7 +197,7 @@ typedef struct gpsOrigin_s {
     int32_t lat;    // Lattitude * 1e+7
     int32_t lon;    // Longitude * 1e+7
     int32_t alt;    // Altitude in centimeters (meters * 100)
-} gpsOrigin_s;
+} gpsOrigin_t;
 
 typedef enum {
     NAV_WP_ACTION_WAYPOINT = 0x01,
@@ -379,9 +379,25 @@ typedef enum {
     GEO_ORIGIN_RESET_ALTITUDE
 } geoOriginResetMode_e;
 
-void geoSetOrigin(gpsOrigin_s * origin, const gpsLocation_t * llh, geoOriginResetMode_e resetMode);
+// geoSetOrigin stores the location provided in llh as a GPS origin in the
-void geoConvertGeodeticToLocal(gpsOrigin_s * origin, const gpsLocation_t * llh, fpVector3_t * pos, geoAltitudeConversionMode_e altConv);
+// provided origin parameter. resetMode indicates wether all origin coordinates
-void geoConvertLocalToGeodetic(const gpsOrigin_s * origin, const fpVector3_t * pos, gpsLocation_t * llh);
+// should be overwritten by llh (GEO_ORIGIN_SET) or just the altitude, leaving
+// other fields untouched (GEO_ORIGIN_RESET_ALTITUDE).
+void geoSetOrigin(gpsOrigin_t *origin, const gpsLocation_t *llh, geoOriginResetMode_e resetMode);
+// geoConvertGeodeticToLocal converts the geodetic location given in llh to
+// the local coordinate space and stores the result in pos. The altConv
+// indicates wether the altitude in llh is relative to the default GPS
+// origin (GEO_ALT_RELATIVE) or absolute (e.g. Earth frame)
+// (GEO_ALT_ABSOLUTE). If origin is invalid pos is set to
+// (0, 0, 0) and false is returned. It returns true otherwise.
+bool geoConvertGeodeticToLocal(fpVector3_t *pos, const gpsOrigin_t *origin, const gpsLocation_t *llh, geoAltitudeConversionMode_e altConv);
+// geoConvertGeodeticToLocalOrigin calls geoConvertGeodeticToLocal with the
+// default GPS origin.
+bool geoConvertGeodeticToLocalOrigin(fpVector3_t * pos, const gpsLocation_t *llh, geoAltitudeConversionMode_e altConv);
+// geoConvertLocalToGeodetic converts a local point as provided in pos to
+// geodetic coordinates using the provided GPS origin. It returns wether
+// the provided origin is valid and the conversion could be performed.
+bool geoConvertLocalToGeodetic(gpsLocation_t *llh, const gpsOrigin_t *origin, const fpVector3_t *pos);
 float geoCalculateMagDeclination(const gpsLocation_t * llh); // degrees units
 
 /* Failsafe-forced RTH mode */

src/main/navigation/navigation_geo.c

@@ -133,7 +133,7 @@ float geoCalculateMagDeclination(const gpsLocation_t * llh) // degrees units
 }
 #endif
 
-void geoSetOrigin(gpsOrigin_s * origin, const gpsLocation_t * llh, geoOriginResetMode_e resetMode)
+void geoSetOrigin(gpsOrigin_t *origin, const gpsLocation_t *llh, geoOriginResetMode_e resetMode)
 {
     if (resetMode == GEO_ORIGIN_SET) {
         origin->valid = true;
@@ -147,7 +147,7 @@ void geoSetOrigin(gpsOrigin_s * origin, const gpsLocation_t * llh, geoOriginRese
     }
 }
 
-void geoConvertGeodeticToLocal(gpsOrigin_s * origin, const gpsLocation_t * llh, fpVector3_t * pos, geoAltitudeConversionMode_e altConv)
+bool geoConvertGeodeticToLocal(fpVector3_t *pos, const gpsOrigin_t *origin, const gpsLocation_t *llh, geoAltitudeConversionMode_e altConv)
 {
     if (origin->valid) {
         pos->x = (llh->lat - origin->lat) * DISTANCE_BETWEEN_TWO_LONGITUDE_POINTS_AT_EQUATOR;
@@ -159,15 +159,21 @@ void geoConvertGeodeticToLocal(gpsOrigin_s * origin, const gpsLocation_t * llh,
         } else {
             pos->z = llh->alt - origin->alt;
         }
+        return true;
     }
-    else {
+
-        pos->x = 0.0f;
+    pos->x = 0.0f;
-        pos->y = 0.0f;
+    pos->y = 0.0f;
-        pos->z = 0.0f;
+    pos->z = 0.0f;
-    }
+    return false;
 }
 
-void geoConvertLocalToGeodetic(const gpsOrigin_s * origin, const fpVector3_t * pos, gpsLocation_t * llh)
+bool geoConvertGeodeticToLocalOrigin(fpVector3_t * pos, const gpsLocation_t *llh, geoAltitudeConversionMode_e altConv)
+{
+    return geoConvertGeodeticToLocal(pos, &posControl.gpsOrigin, llh, altConv);
+}
+
+bool geoConvertLocalToGeodetic(gpsLocation_t *llh, const gpsOrigin_t * origin, const fpVector3_t *pos)
 {
     float scaleLonDown;
 
@@ -187,6 +193,7 @@ void geoConvertLocalToGeodetic(const gpsOrigin_s * origin, const fpVector3_t * p
     llh->lat += lrintf(pos->x / DISTANCE_BETWEEN_TWO_LONGITUDE_POINTS_AT_EQUATOR);
     llh->lon += lrintf(pos->y / (DISTANCE_BETWEEN_TWO_LONGITUDE_POINTS_AT_EQUATOR * scaleLonDown));
     llh->alt += lrintf(pos->z);
+    return origin->valid;
 }
 
 

src/main/navigation/navigation_pos_estimator.c

@@ -228,7 +228,7 @@ void onNewGPSData(void)
 
         if (posControl.gpsOrigin.valid) {
             /* Convert LLH position to local coordinates */
-            geoConvertGeodeticToLocal(&posControl.gpsOrigin, &newLLH, & posEstimator.gps.pos, GEO_ALT_ABSOLUTE);
+            geoConvertGeodeticToLocal(&posEstimator.gps.pos, &posControl.gpsOrigin, &newLLH, GEO_ALT_ABSOLUTE);
 
             /* If not the first update - calculate velocities */
             if (!isFirstGPSUpdate) {

src/main/navigation/navigation_private.h

@@ -318,7 +318,7 @@ typedef struct {
     uint32_t                    lastValidAltitudeTimeMs;
 
     /* INAV GPS origin (position where GPS fix was first acquired) */
-    gpsOrigin_s                 gpsOrigin;
+    gpsOrigin_t                 gpsOrigin;
 
     /* Home parameters (NEU coordinated), geodetic position of home (LLH) is stores in GPS_home variable */
     rthSanityChecker_t          rthSanityChecker;