NAV: Fixed an option to compile out navigation while still having GPS for telemetry and OSD (home position, distance and bearing is calculated correctly)

src/main/config/config.c

@@ -213,7 +213,7 @@ void resetPidProfile(pidProfile_t *pidProfile)
 #endif
 }
 
-#ifdef GPS
+#ifdef NAV
 void resetNavConfig(navConfig_t * navConfig)
 {
     // Navigation flags
@@ -523,7 +523,7 @@ static void resetConf(void)
     masterConfig.gpsConfig.autoBaud = GPS_AUTOBAUD_OFF;
 #endif
 
-#ifdef GPS
+#ifdef NAV
     resetNavConfig(&masterConfig.navConfig);
 #endif
 
@@ -796,7 +796,7 @@ void activateConfig(void)
 
     imuConfigure(&imuRuntimeConfig, &currentProfile->pidProfile);
 
-#ifdef GPS
+#ifdef NAV
     navigationUseConfig(&masterConfig.navConfig);
     navigationUsePIDs(&currentProfile->pidProfile);
     navigationUseRcControlsConfig(&currentProfile->rcControlsConfig);

src/main/config/config_master.h

@@ -83,6 +83,9 @@ typedef struct master_t {
 
 #ifdef GPS
     gpsConfig_t gpsConfig;
+#endif
+
+#ifdef NAV
     navConfig_t navConfig;
 #endif
 

src/main/flight/navigation_rewrite.c

@@ -46,8 +46,14 @@
 #include "config/runtime_config.h"
 #include "config/config.h"
 
-#if defined(NAV)
+/*-----------------------------------------------------------
+ * Compatibility for home position
+ *-----------------------------------------------------------*/
+gpsLocation_t GPS_home;
+uint16_t      GPS_distanceToHome;        // distance to home point in meters
+int16_t       GPS_directionToHome;       // direction to home point in degrees
 
+#if defined(NAV)
 navigationPosControl_t  posControl;
 navSystemStatus_t       NAV_Status;
 
@@ -1376,13 +1382,6 @@ bool isWaypointReached(navWaypointPosition_t * waypoint)
     return (wpDistance <= posControl.navConfig->waypoint_radius);
 }
 
-/*-----------------------------------------------------------
- * Compatibility for home position
- *-----------------------------------------------------------*/
-gpsLocation_t GPS_home;
-uint16_t      GPS_distanceToHome;        // distance to home point in meters
-int16_t       GPS_directionToHome;       // direction to home point in degrees
-
 static void updateHomePositionCompatibility(void)
 {
     geoConvertLocalToGeodetic(&posControl.gpsOrigin, &posControl.homePosition.pos, &GPS_home);
@@ -2226,4 +2225,58 @@ rthState_e getStateOfForcedRTH(void)
     }
 }
 
+#else // NAV
+
+/* Fallback if navigation is not compiled in - handle GPS home coordinates */
+static float GPS_scaleLonDown;
+
+static void GPS_distance_cm_bearing(int32_t currentLat1, int32_t currentLon1, int32_t destinationLat2, int32_t destinationLon2, uint32_t *dist, int32_t *bearing)
+{
+    float dLat = destinationLat2 - currentLat1; // difference of latitude in 1/10 000 000 degrees
+    float dLon = (float)(destinationLon2 - currentLon1) * GPS_scaleLonDown;
+
+    *dist = sqrtf(sq(dLat) + sq(dLon)) * DISTANCE_BETWEEN_TWO_LONGITUDE_POINTS_AT_EQUATOR;
+
+    *bearing = 9000.0f + RADIANS_TO_CENTIDEGREES(atan2_approx(-dLat, dLon));      // Convert the output radians to 100xdeg
+
+    if (*bearing < 0)
+        *bearing += 36000;
+}
+
+void onNewGPSData(int32_t newLat, int32_t newLon, int32_t newAlt, int16_t velN, int16_t velE, int16_t velD, bool velNEValid, bool velDValid, int16_t hdop)
+{
+    UNUSED(velN);
+    UNUSED(velE);
+    UNUSED(velD);
+    UNUSED(velNEValid);
+    UNUSED(velDValid);
+    UNUSED(hdop);
+
+    if (!(sensors(SENSOR_GPS) && STATE(GPS_FIX) && GPS_numSat >= 5))
+        return;
+
+    if (ARMING_FLAG(ARMED)) {
+        if (STATE(GPS_FIX_HOME)) {
+            uint32_t dist;
+            int32_t dir;
+            GPS_distance_cm_bearing(newLat, newLon, GPS_home.lat, GPS_home.lon, &dist, &dir);
+            GPS_distanceToHome = dist / 100;
+            GPS_directionToHome = dir / 100;
+        } else {
+            GPS_distanceToHome = 0;
+            GPS_directionToHome = 0;
+        }
+    }
+    else {
+        // Set home position to current GPS coordinates
+        ENABLE_STATE(GPS_FIX_HOME);
+        GPS_home.lat = newLat;
+        GPS_home.lon = newLon;
+        GPS_home.alt = newAlt;
+        GPS_distanceToHome = 0;
+        GPS_directionToHome = 0;
+        GPS_scaleLonDown = cos_approx((ABS((float)newLat) / 10000000.0f) * 0.0174532925f);
+    }
+}
+
 #endif  // NAV

src/main/flight/navigation_rewrite.h

@@ -26,6 +26,22 @@
 #include "flight/pid.h"
 #include "flight/failsafe.h"
 
+/* LLH Location in NEU axis system */
+typedef struct gpsLocation_s {
+    int32_t lat;    // Lattitude * 1e+7
+    int32_t lon;    // Longitude * 1e+7
+    int32_t alt;    // Altitude in centimeters (meters * 100)
+} gpsLocation_t;
+
+/* GPS Home location data */
+extern gpsLocation_t        GPS_home;
+extern uint16_t             GPS_distanceToHome;        // distance to home point in meters
+extern int16_t              GPS_directionToHome;       // direction to home point in degrees
+
+/* Navigation system updates */
+void onNewGPSData(int32_t lat, int32_t lon, int32_t alt, int16_t vel_n, int16_t vel_e, int16_t vel_d, bool vel_ne_valid, bool vel_d_valid, int16_t hdop);
+
+#if defined(NAV)
 #if defined(BLACKBOX)
 #define NAV_BLACKBOX
 #endif
@@ -34,12 +50,9 @@
 //#define INAV_ENABLE_AUTO_MAG_DECLINATION
 #define INAV_ENABLE_GPS_GLITCH_DETECTION
 
-
 // Maximum number of waypoints, special waypoint 0 = home,
 #define NAV_MAX_WAYPOINTS       15
 
-#if defined(NAV)
-
 enum {
     NAV_GPS_ATTI    = 0,                    // Pitch/roll stick controls attitude (pitch/roll lean angles)
     NAV_GPS_CRUISE  = 1                     // Pitch/roll stick controls velocity (forward/right speed)
@@ -119,13 +132,6 @@ typedef struct navConfig_s {
     uint8_t  fw_pitch_to_throttle;          // Pitch angle (in deg) to throttle gain (in 1/1000's of throttle) (*10)
 } navConfig_t;
 
-// LLH Location in NEU axis system
-typedef struct gpsLocation_s {
-    int32_t lat;    // Lattitude * 1e+7
-    int32_t lon;    // Longitude * 1e+7
-    int32_t alt;    // Altitude in centimeters (meters * 100)
-} gpsLocation_t;
-
 typedef struct gpsOrigin_s {
     bool    valid;
     float   scale;
@@ -223,7 +229,6 @@ void navigationInit(navConfig_t *initialnavConfig,
                     escAndServoConfig_t * initialEscAndServoConfig);
 
 /* Navigation system updates */
-void onNewGPSData(int32_t lat, int32_t lon, int32_t alt, int16_t vel_n, int16_t vel_e, int16_t vel_d, bool vel_ne_valid, bool vel_d_valid, int16_t hdop);
 void updateWaypointsAndNavigationMode(void);
 void updatePositionEstimator(void);
 void applyWaypointNavigationAndAltitudeHold(void);
@@ -259,9 +264,6 @@ void abortForcedRTH(void);
 rthState_e getStateOfForcedRTH(void);
 
 /* Compatibility data */
-extern gpsLocation_t        GPS_home;
-extern uint16_t             GPS_distanceToHome;        // distance to home point in meters
-extern int16_t              GPS_directionToHome;       // direction to home point in degrees
 extern navSystemStatus_t    NAV_Status;
 
 #if defined(BLACKBOX)
@@ -281,6 +283,7 @@ extern uint16_t navFlags;
 
 #define naivationRequiresAngleMode() (0)
 #define naivationGetHeadingControlState() (0)
-#define navigationControlsThrottleAngleCorrection() (0)
+#define navigationRequiresThrottleTiltCompensation() (0)
+#define getEstimatedActualVelocity(axis) (0)
 
 #endif

src/main/flight/navigation_rewrite_fixedwing.c

@@ -43,7 +43,6 @@
 #include "config/runtime_config.h"
 #include "config/config.h"
 
-
 #if defined(NAV)
 
 static bool isPitchAndThrottleAdjustmentValid = false;

src/main/flight/navigation_rewrite_pos_estimator.c

@@ -47,6 +47,8 @@
 #include "config/runtime_config.h"
 #include "config/config.h"
 
+#if defined(NAV)
+
 /**
  * Model-identification based position estimator
  * Based on INAV position estimator for PX4 by Anton Babushkin <anton.babushkin@me.com>
@@ -692,3 +694,5 @@ void updatePositionEstimator(void)
     /* Publish estimate */
     publishEstimatedTopic(currentTime);
 }
+
+#endif

src/main/flight/navigation_rewrite_private.h

@@ -17,21 +17,18 @@
 
 #pragma once
 
-#include "config/runtime_config.h"
-
 #define DISTANCE_BETWEEN_TWO_LONGITUDE_POINTS_AT_EQUATOR    1.113195f  // MagicEarthNumber from APM
 
+#if defined(NAV)
+
+#include "config/runtime_config.h"
+
 #define LAND_DETECTOR_TRIGGER_TIME_MS       2000        // 2 seconds
-
 #define MIN_POSITION_UPDATE_RATE_HZ         5       // Minimum position update rate at which XYZ controllers would be applied
-
 #define NAV_THROTTLE_CUTOFF_FREQENCY_HZ     4       // low-pass filter on throttle output
 #define NAV_ACCEL_CUTOFF_FREQUENCY_HZ       2       // low-pass filter on XY-acceleration target
-
 #define NAV_FW_VEL_CUTOFF_FREQENCY_HZ       2       // low-pass filter on Z-velocity for fixed wing
-
 #define NAV_DTERM_CUT_HZ                    10
-
 #define NAV_ACCELERATION_XY_MAX             980.0f  // cm/s/s       // approx 45 deg lean angle
 
 #define HZ2US(hz)   (1000000 / (hz))
@@ -331,3 +328,5 @@ void applyFixedWingNavigationController(navigationFSMStateFlags_t navStateFlags,
 
 bool isFixedWingLandingDetected(uint32_t * landingTimer);
 void calculateFixedWingInitialHoldPosition(t_fp_vector * pos);
+
+#endif

src/main/io/rc_controls.c

@@ -111,10 +111,12 @@ bool isUsingSticksForArming(void)
     return isUsingSticksToArm;
 }
 
+#if defined(NAV)
 bool isUsingNavigationModes(void)
 {
     return isUsingNAVModes;
 }
+#endif
 
 bool areSticksInApModePosition(uint16_t ap_mode)
 {

src/main/io/serial_msp.c

@@ -720,7 +720,7 @@ static bool processOutCommand(uint8_t cmdMSP)
 {
     uint32_t i, tmp, junk;
 
-#ifdef GPS
+#ifdef NAV
     int8_t msp_wp_no;
     navWaypoint_t msp_wp;
 #endif
@@ -1090,6 +1090,7 @@ static bool processOutCommand(uint8_t cmdMSP)
         serialize16(GPS_directionToHome);
         serialize8(GPS_update & 1);
         break;
+#ifdef NAV
     case MSP_NAV_STATUS:
         headSerialReply(7);
         serialize8(NAV_Status.mode);
@@ -1114,6 +1115,7 @@ static bool processOutCommand(uint8_t cmdMSP)
         serialize16(msp_wp.p3);     // P3
         serialize8(msp_wp.flag);    // flags
         break;
+#endif
     case MSP_GPSSVINFO:
         headSerialReply(1 + (GPS_numCh * 4));
         serialize8(GPS_numCh);
@@ -1327,7 +1329,7 @@ static bool processInCommand(void)
     uint16_t tmp;
     uint8_t rate;
 
-#ifdef GPS
+#ifdef NAV
     uint8_t msp_wp_no;
     navWaypoint_t msp_wp;
 #endif
@@ -1614,6 +1616,8 @@ static bool processInCommand(void)
         sensorsSet(SENSOR_GPS);
         onNewGPSData(GPS_coord[LAT], GPS_coord[LON], GPS_altitude, 0, 0, 0, false, false, 9999);
         break;
+#endif
+#ifdef NAV
     case MSP_SET_WP:
         msp_wp_no = read8();     // get the wp number
         msp_wp.action = read8();    // action

src/main/telemetry/ltm.c

@@ -122,6 +122,7 @@ static void ltm_finalise(void)
     serialWrite(ltmPort, ltm_crc);
 }
 
+#if defined(GPS)
 /*
  * GPS G-frame 5Hhz at > 2400 baud
  * LAT LON SPD ALT SAT/FIX
@@ -146,7 +147,7 @@ static void ltm_gframe(void)
     ltm_serialise_32(GPS_coord[LON]);
     ltm_serialise_8((uint8_t)(GPS_speed / 100));
 
-#if defined(BARO) || defined(SONAR)
+#if defined(NAV)
     ltm_alt = (sensors(SENSOR_SONAR) || sensors(SENSOR_BARO)) ? getEstimatedActualPosition(Z) : GPS_altitude * 100;
 #else
     ltm_alt = GPS_altitude * 100;
@@ -155,6 +156,7 @@ static void ltm_gframe(void)
     ltm_serialise_8((GPS_numSat << 2) | gps_fix_type);
     ltm_finalise();
 }
+#endif
 
 /*
  * Sensors S-frame 5Hhz at > 2400 baud
@@ -215,6 +217,7 @@ static void ltm_aframe()
     ltm_finalise();
 }
 
+#if defined(GPS)
 /*
  * OSD additional data frame, 1 Hz rate
  *  This frame will be ignored by Ghettostation, but processed by GhettOSD if it is used as standalone onboard OSD
@@ -230,7 +233,9 @@ static void ltm_oframe()
     ltm_serialise_8(STATE(GPS_FIX_HOME) ? 1 : 0);
     ltm_finalise();
 }
+#endif
 
+#if defined(NAV)
 /** OSD additional data frame, ~4 Hz rate, navigation system status
  */
 static void ltm_nframe(void)
@@ -244,6 +249,7 @@ static void ltm_nframe(void)
     ltm_serialise_8(NAV_Status.flags);
     ltm_finalise();
 }
+#endif
 
 #define LTM_BIT_AFRAME  (1 << 0)
 #define LTM_BIT_GFRAME  (1 << 1)
@@ -272,17 +278,21 @@ static void process_ltm(void)
     if (current_schedule & LTM_BIT_AFRAME)
         ltm_aframe();
 
+#if defined(GPS)
     if (current_schedule & LTM_BIT_GFRAME)
         ltm_gframe();
 
+    if (current_schedule & LTM_BIT_OFRAME)
+        ltm_oframe();
+#endif
+
     if (current_schedule & LTM_BIT_SFRAME)
         ltm_sframe();
 
-    if (current_schedule & LTM_BIT_OFRAME)
+#if defined(NAV)
-        ltm_oframe();
-
     if (current_schedule & LTM_BIT_NFRAME)
         ltm_nframe();
+#endif
 
     ltm_scheduler = (ltm_scheduler + 1) % 10;
 }