Merge pull request #4607 from martinbudden/bfa_gps_nav_separation

Better separation between GPS and NAV

src/main/blackbox/blackbox.c

@@ -54,7 +54,6 @@
 
 #include "flight/failsafe.h"
 #include "flight/mixer.h"
-#include "flight/navigation.h"
 #include "flight/pid.h"
 #include "flight/servos.h"
 

src/main/fc/config.c

@@ -301,7 +301,7 @@ void activateConfig(void)
     useRcControlsConfig(currentPidProfile);
     useAdjustmentConfig(currentPidProfile);
 
-#ifdef USE_GPS
+#ifdef USE_NAV
     gpsUsePIDs(currentPidProfile);
 #endif
 

src/main/fc/fc_core.c

@@ -553,7 +553,7 @@ void processRx(timeUs_t currentTimeUs)
     }
 #endif
 
-#ifdef USE_GPS
+#ifdef USE_NAV
     if (sensors(SENSOR_GPS)) {
         updateGpsWaypointsAndMode();
     }
@@ -649,7 +649,7 @@ static void subTaskMainSubprocesses(timeUs_t currentTimeUs)
 
     processRcCommand();
 
-#ifdef USE_GPS
+#ifdef USE_NAV
     if (sensors(SENSOR_GPS)) {
         if ((FLIGHT_MODE(GPS_HOME_MODE) || FLIGHT_MODE(GPS_HOLD_MODE)) && STATE(GPS_FIX_HOME)) {
             updateGpsStateForHomeAndHoldMode();

src/main/fc/fc_init.c

@@ -607,7 +607,9 @@ void init(void)
 #ifdef USE_GPS
     if (feature(FEATURE_GPS)) {
         gpsInit();
+#ifdef USE_NAV
         navigationInit();
+#endif
     }
 #endif
 

src/main/fc/rc_controls.c

@@ -60,7 +60,6 @@
 #include "scheduler/scheduler.h"
 
 #include "flight/pid.h"
-#include "flight/navigation.h"
 #include "flight/failsafe.h"
 
 static pidProfile_t *pidProfile;

src/main/flight/navigation.c

@@ -22,7 +22,7 @@
 
 #include "platform.h"
 
-#ifdef USE_GPS
+#ifdef USE_NAV
 
 #include "build/debug.h"
 
@@ -72,16 +72,11 @@ bool areSticksInApModePosition(uint16_t ap_mode);
 // **********************
 // GPS
 // **********************
-int16_t GPS_angle[ANGLE_INDEX_COUNT] = { 0, 0 };    // it's the angles that must be applied for GPS correction
-int32_t GPS_home[2];
 int32_t GPS_hold[2];
 
-uint16_t GPS_distanceToHome;        // distance to home point in meters
-int16_t GPS_directionToHome;        // direction to home or hol point in degrees
 
 static int16_t nav[2];
 static int16_t nav_rated[2];               // Adding a rate controller to the navigation to make it smoother
-navigationMode_e nav_mode = NAV_MODE_NONE;    // Navigation mode
 
 // When using PWM input GPS usage reduces number of available channels by 2 - see pwm_common.c/pwmInit()
 void navigationInit(void)
@@ -104,23 +99,17 @@ void navigationInit(void)
 #define NAV_TAIL_FIRST             0    // true - copter comes in with tail first
 #define NAV_SET_TAKEOFF_HEADING    1    // true - when copter arrives to home position it rotates it's head to takeoff direction
 
-#define GPS_FILTERING              1    // add a 5 element moving average filter to GPS coordinates, helps eliminate gps noise but adds latency
 #define GPS_LOW_SPEED_D_FILTER     1    // below .5m/s speed ignore D term for POSHOLD_RATE, theoretically this also removed D term induced noise
 
-static void GPS_distance_cm_bearing(int32_t * lat1, int32_t * lon1, int32_t * lat2, int32_t * lon2, uint32_t * dist, int32_t * bearing);
 //static void GPS_distance(int32_t lat1, int32_t lon1, int32_t lat2, int32_t lon2, uint16_t* dist, int16_t* bearing);
-static void GPS_calc_longitude_scaling(int32_t lat);
-static void GPS_calc_velocity(void);
 static void GPS_calc_location_error(int32_t * target_lat, int32_t * target_lng, int32_t * gps_lat, int32_t * gps_lng);
 
-#ifdef USE_NAV
 static bool check_missed_wp(void);
 static void GPS_calc_poshold(void);
 static void GPS_calc_nav_rate(uint16_t max_speed);
 static void GPS_update_crosstrack(void);
 static uint16_t GPS_calc_desired_speed(uint16_t max_speed, bool _slow);
 static int32_t wrap_36000(int32_t angle);
-#endif
 
 static int32_t wrap_18000(int32_t error);
 
@@ -149,7 +138,6 @@ typedef struct {
 static PID posholdPID[2];
 static PID poshold_ratePID[2];
 
-#ifdef USE_NAV
 static PID_PARAM navPID_PARAM;
 static PID navPID[2];
 
@@ -181,7 +169,6 @@ static int32_t get_D(int32_t input, float *dt, PID *pid, PID_PARAM *pid_param)
     // add in derivative component
     return pid_param->kD * pid->derivative;
 }
-#endif
 
 static void reset_PID(PID *pid)
 {
@@ -190,8 +177,6 @@ static void reset_PID(PID *pid)
     pid->last_derivative = 0;
 }
 
-#define GPS_X 1
-#define GPS_Y 0
 
 /****************** PI and PID controllers for GPS ********************///32938 -> 33160
 
@@ -200,18 +185,13 @@ static void reset_PID(PID *pid)
 #define NAV_SLOW_NAV               true
 #define NAV_BANK_MAX               3000 // 30deg max banking when navigating (just for security and testing)
 
-static float dTnav;             // Delta Time in milliseconds for navigation computations, updated with every good GPS read
-static int16_t actual_speed[2] = { 0, 0 };
-static float GPS_scaleLonDown = 1.0f;  // this is used to offset the shrinking longitude as we go towards the poles
 static int32_t error[2];
 
-#ifdef USE_NAV
 // The difference between the desired rate of travel and the actual rate of travel
 // updated after GPS read - 5-10hz
 static int16_t rate_error[2];
 // The amount of angle correction applied to target_bearing to bring the copter back on its optimum path
 static int16_t crosstrack_error;
-#endif
 
 // Currently used WP
 static int32_t GPS_WP[2];
@@ -238,95 +218,14 @@ static uint32_t wp_distance;
 // used for slow speed wind up when start navigation;
 static int16_t waypoint_speed_gov;
 
-////////////////////////////////////////////////////////////////////////////////////
-// moving average filter variables
-//
-#define GPS_FILTER_VECTOR_LENGTH 5
-
-static uint8_t GPS_filter_index = 0;
-static int32_t GPS_filter[2][GPS_FILTER_VECTOR_LENGTH];
-static int32_t GPS_filter_sum[2];
-static int32_t GPS_read[2];
-static int32_t GPS_filtered[2];
-static int32_t GPS_degree[2];   //the lat lon degree without any decimals (lat/10 000 000)
-static uint16_t fraction3[2];
-
 // This is the angle from the copter to the "next_WP" location
 // with the addition of Crosstrack error in degrees * 100
 static int32_t nav_bearing;
 // saves the bearing at takeof (1deg = 1) used to rotate to takeoff direction when arrives at home
-static int16_t nav_takeoff_bearing;
 
-void GPS_calculateDistanceAndDirectionToHome(void)
+
+void navNewGpsData(void)
 {
-    if (STATE(GPS_FIX_HOME)) {      // If we don't have home set, do not display anything
-        uint32_t dist;
-        int32_t dir;
-        GPS_distance_cm_bearing(&gpsSol.llh.lat, &gpsSol.llh.lon, &GPS_home[LAT], &GPS_home[LON], &dist, &dir);
-        GPS_distanceToHome = dist / 100;
-        GPS_directionToHome = dir / 100;
-    } else {
-        GPS_distanceToHome = 0;
-        GPS_directionToHome = 0;
-    }
-}
-
-void onGpsNewData(void)
-{
-    static uint32_t nav_loopTimer;
-
-    if (!(STATE(GPS_FIX) && gpsSol.numSat >= 5)) {
-        return;
-    }
-
-    if (!ARMING_FLAG(ARMED))
-        DISABLE_STATE(GPS_FIX_HOME);
-
-    if (!STATE(GPS_FIX_HOME) && ARMING_FLAG(ARMED))
-        GPS_reset_home_position();
-
-    // Apply moving average filter to GPS data
-#if defined(GPS_FILTERING)
-    GPS_filter_index = (GPS_filter_index + 1) % GPS_FILTER_VECTOR_LENGTH;
-    for (int axis = 0; axis < 2; axis++) {
-        GPS_read[axis] = axis == LAT ? gpsSol.llh.lat : gpsSol.llh.lon; // latest unfiltered data is in GPS_latitude and GPS_longitude
-        GPS_degree[axis] = GPS_read[axis] / 10000000;   // get the degree to assure the sum fits to the int32_t
-
-        // How close we are to a degree line ? its the first three digits from the fractions of degree
-        // later we use it to Check if we are close to a degree line, if yes, disable averaging,
-        fraction3[axis] = (GPS_read[axis] - GPS_degree[axis] * 10000000) / 10000;
-
-        GPS_filter_sum[axis] -= GPS_filter[axis][GPS_filter_index];
-        GPS_filter[axis][GPS_filter_index] = GPS_read[axis] - (GPS_degree[axis] * 10000000);
-        GPS_filter_sum[axis] += GPS_filter[axis][GPS_filter_index];
-        GPS_filtered[axis] = GPS_filter_sum[axis] / GPS_FILTER_VECTOR_LENGTH + (GPS_degree[axis] * 10000000);
-        if (nav_mode == NAV_MODE_POSHOLD) {             // we use gps averaging only in poshold mode...
-            if (fraction3[axis] > 1 && fraction3[axis] < 999) {
-                if (axis == LAT) {
-                    gpsSol.llh.lat = GPS_filtered[LAT];
-                } else {
-                    gpsSol.llh.lon = GPS_filtered[LON];
-                }
-            }
-        }
-    }
-#endif
-
-    //
-    // Calculate time delta for navigation loop, range 0-1.0f, in seconds
-    //
-    // Time for calculating x,y speed and navigation pids
-    dTnav = (float)(millis() - nav_loopTimer) / 1000.0f;
-    nav_loopTimer = millis();
-    // prevent runup from bad GPS
-    dTnav = MIN(dTnav, 1.0f);
-
-    GPS_calculateDistanceAndDirectionToHome();
-
-    // calculate the current velocity based on gps coordinates continously to get a valid speed at the moment when we start navigating
-    GPS_calc_velocity();
-
-#ifdef USE_NAV
     if (FLIGHT_MODE(GPS_HOLD_MODE) || FLIGHT_MODE(GPS_HOME_MODE)) {
         // we are navigating
 
@@ -367,19 +266,6 @@ void onGpsNewData(void)
             break;
         }
     }                   //end of gps calcs
-#endif
-}
-
-void GPS_reset_home_position(void)
-{
-    if (STATE(GPS_FIX) && gpsSol.numSat >= 5) {
-        GPS_home[LAT] = gpsSol.llh.lat;
-        GPS_home[LON] = gpsSol.llh.lon;
-        GPS_calc_longitude_scaling(gpsSol.llh.lat); // need an initial value for distance and bearing calc
-        nav_takeoff_bearing = DECIDEGREES_TO_DEGREES(attitude.values.yaw);              // save takeoff heading
-        // Set ground altitude
-        ENABLE_STATE(GPS_FIX_HOME);
-    }
 }
 
 // reset navigation (stop the navigation processor, and clear nav)
@@ -393,9 +279,7 @@ void GPS_reset_nav(void)
         nav[i] = 0;
         reset_PID(&posholdPID[i]);
         reset_PID(&poshold_ratePID[i]);
-#ifdef USE_NAV
         reset_PID(&navPID[i]);
-#endif
     }
 }
 
@@ -411,12 +295,10 @@ void gpsUsePIDs(pidProfile_t *pidProfile)
     poshold_ratePID_PARAM.kD = (float)pidProfile->pid[PID_POSR].D / 1000.0f;
     poshold_ratePID_PARAM.Imax = POSHOLD_RATE_IMAX * 100;
 
-#ifdef USE_NAV
     navPID_PARAM.kP = (float)pidProfile->pid[PID_NAVR].P / 10.0f;
     navPID_PARAM.kI = (float)pidProfile->pid[PID_NAVR].I / 100.0f;
     navPID_PARAM.kD = (float)pidProfile->pid[PID_NAVR].D / 1000.0f;
     navPID_PARAM.Imax = POSHOLD_RATE_IMAX * 100;
-#endif
 }
 
 // OK here is the onboard GPS code
@@ -431,12 +313,6 @@ void gpsUsePIDs(pidProfile_t *pidProfile)
 // this is used to offset the shrinking longitude as we go towards the poles
 // It's ok to calculate this once per waypoint setting, since it changes a little within the reach of a multicopter
 //
-static void GPS_calc_longitude_scaling(int32_t lat)
-{
-    float rads = (ABS((float)lat) / 10000000.0f) * 0.0174532925f;
-    GPS_scaleLonDown = cos_approx(rads);
-}
-
 ////////////////////////////////////////////////////////////////////////////////////
 // Sets the waypoint to navigate, reset neccessary variables and calculate initial values
 //
@@ -454,7 +330,6 @@ void GPS_set_next_wp(int32_t *lat, int32_t *lon)
     waypoint_speed_gov = navigationConfig()->nav_speed_min;
 }
 
-#ifdef USE_NAV
 ////////////////////////////////////////////////////////////////////////////////////
 // Check if we missed the destination somehow
 //
@@ -465,24 +340,7 @@ static bool check_missed_wp(void)
     temp = wrap_18000(temp);
     return (ABS(temp) > 10000); // we passed the waypoint by 100 degrees
 }
-#endif
 
-#define DISTANCE_BETWEEN_TWO_LONGITUDE_POINTS_AT_EQUATOR_IN_HUNDREDS_OF_KILOMETERS 1.113195f
-#define TAN_89_99_DEGREES 5729.57795f
-
-////////////////////////////////////////////////////////////////////////////////////
-// Get distance between two points in cm
-// Get bearing from pos1 to pos2, returns an 1deg = 100 precision
-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_IN_HUNDREDS_OF_KILOMETERS;
-
-    *bearing = 9000.0f + atan2_approx(-dLat, dLon) * TAN_89_99_DEGREES;      // Convert the output radians to 100xdeg
-    if (*bearing < 0)
-        *bearing += 36000;
-}
 
 ////////////////////////////////////////////////////////////////////////////////////
 // keep old calculation function for compatibility (could be removed later) distance in meters, bearing in degree
@@ -495,32 +353,6 @@ static void GPS_distance_cm_bearing(int32_t *currentLat1, int32_t *currentLon1,
 //  *bearing = d2 /  100;      //convert to degrees
 //}
 
-////////////////////////////////////////////////////////////////////////////////////
-// Calculate our current speed vector from gps position data
-//
-static void GPS_calc_velocity(void)
-{
-    static int16_t speed_old[2] = { 0, 0 };
-    static int32_t last_coord[2] = { 0, 0 };
-    static uint8_t init = 0;
-
-    if (init) {
-        float tmp = 1.0f / dTnav;
-        actual_speed[GPS_X] = (float)(gpsSol.llh.lon - last_coord[LON]) * GPS_scaleLonDown * tmp;
-        actual_speed[GPS_Y] = (float)(gpsSol.llh.lat - last_coord[LAT]) * tmp;
-
-        actual_speed[GPS_X] = (actual_speed[GPS_X] + speed_old[GPS_X]) / 2;
-        actual_speed[GPS_Y] = (actual_speed[GPS_Y] + speed_old[GPS_Y]) / 2;
-
-        speed_old[GPS_X] = actual_speed[GPS_X];
-        speed_old[GPS_Y] = actual_speed[GPS_Y];
-    }
-    init = 1;
-
-    last_coord[LON] = gpsSol.llh.lon;
-    last_coord[LAT] = gpsSol.llh.lat;
-}
-
 ////////////////////////////////////////////////////////////////////////////////////
 // Calculate a location error between two gps coordinates
 // Because we are using lat and lon to do our distance errors here's a quick chart:
@@ -536,7 +368,6 @@ static void GPS_calc_location_error(int32_t *target_lat, int32_t *target_lng, in
     error[LAT] = *target_lat - *gps_lat;        // Y Error
 }
 
-#ifdef USE_NAV
 ////////////////////////////////////////////////////////////////////////////////////
 // Calculate nav_lat and nav_lon from the x and y error and the speed
 //
@@ -642,7 +473,6 @@ static uint16_t GPS_calc_desired_speed(uint16_t max_speed, bool _slow)
     }
     return max_speed;
 }
-#endif
 
 ////////////////////////////////////////////////////////////////////////////////////
 // Utilities
@@ -656,7 +486,6 @@ static int32_t wrap_18000(int32_t error)
     return error;
 }
 
-#ifdef USE_NAV
 static int32_t wrap_36000(int32_t angle)
 {
     if (angle > 36000)
@@ -665,7 +494,6 @@ static int32_t wrap_36000(int32_t angle)
         angle += 36000;
     return angle;
 }
-#endif
 
 void updateGpsStateForHomeAndHoldMode(void)
 {

src/main/flight/navigation.h

@@ -18,13 +18,8 @@
 #pragma once
 
 #include "common/axis.h"
+#include "io/gps.h"
 
-// navigation mode
-typedef enum {
-    NAV_MODE_NONE = 0,
-    NAV_MODE_POSHOLD,
-    NAV_MODE_WP
-} navigationMode_e;
 
 // FIXME ap_mode is badly named, it's a value that is compared to rcCommand, not a flag at it's name implies.
 
@@ -40,22 +35,14 @@ typedef struct navigationConfig_s {
 
 PG_DECLARE(navigationConfig_t, navigationConfig);
 
-extern int16_t GPS_angle[ANGLE_INDEX_COUNT];                // it's the angles that must be applied for GPS correction
 
-extern int32_t GPS_home[2];
 extern int32_t GPS_hold[2];
 
-extern uint16_t GPS_distanceToHome;        // distance to home point in meters
-extern int16_t GPS_directionToHome;        // direction to home or hol point in degrees
-
-extern navigationMode_e nav_mode;          // Navigation mode
 
 void navigationInit(void);
-void GPS_reset_home_position(void);
 void GPS_reset_nav(void);
 void GPS_set_next_wp(int32_t* lat, int32_t* lon);
 void gpsUsePIDs(struct pidProfile_s *pidProfile);
 void updateGpsStateForHomeAndHoldMode(void);
 void updateGpsWaypointsAndMode(void);
 
-void onGpsNewData(void);

src/main/flight/pid.c

@@ -45,7 +45,8 @@
 #include "flight/pid.h"
 #include "flight/imu.h"
 #include "flight/mixer.h"
-#include "flight/navigation.h"
+
+#include "io/gps.h"
 
 #include "sensors/gyro.h"
 #include "sensors/acceleration.h"

src/main/interface/msp.c

@@ -1251,7 +1251,7 @@ static mspResult_e mspFcProcessOutCommandWithArg(uint8_t cmdMSP, sbuf_t *arg, sb
 }
 #endif // USE_OSD_SLAVE
 
-#ifdef USE_GPS
+#ifdef USE_NAV
 static void mspFcWpCommand(sbuf_t *dst, sbuf_t *src)
 {
     uint8_t wp_no;
@@ -1312,7 +1312,7 @@ static mspResult_e mspProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
     uint32_t i;
     uint8_t value;
     const unsigned int dataSize = sbufBytesRemaining(src);
-#ifdef USE_GPS
+#ifdef USE_NAV
     uint8_t wp_no;
     int32_t lat = 0, lon = 0, alt = 0;
 #endif
@@ -1743,7 +1743,8 @@ static mspResult_e mspProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
         gpsSol.groundSpeed = sbufReadU16(src);
         GPS_update |= 2;        // New data signalisation to GPS functions // FIXME Magic Numbers
         break;
-
+#endif // USE_GPS
+#ifdef USE_NAV
     case MSP_SET_WP:
         wp_no = sbufReadU8(src);    //get the wp number
         lat = sbufReadU32(src);
@@ -1768,7 +1769,7 @@ static mspResult_e mspProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
             GPS_set_next_wp(&GPS_hold[LAT], &GPS_hold[LON]);
         }
         break;
-#endif
+#endif // USE_NAV
 
     case MSP_SET_FEATURE_CONFIG:
         featureClearAll();
@@ -2177,7 +2178,7 @@ mspResult_e mspFcProcessCommand(mspPacket_t *cmd, mspPacket_t *reply, mspPostPro
         mspFc4waySerialCommand(dst, src, mspPostProcessFn);
         ret = MSP_RESULT_ACK;
 #endif
-#ifdef USE_GPS
+#ifdef USE_NAV
     } else if (cmdMSP == MSP_WP) {
         mspFcWpCommand(dst, src);
         ret = MSP_RESULT_ACK;

src/main/io/gps.c

@@ -48,7 +48,7 @@
 #include "fc/config.h"
 #include "fc/runtime_config.h"
 
-#include "flight/navigation.h"
+#include "flight/imu.h"
 #include "flight/pid.h"
 
 #include "sensors/sensors.h"
@@ -71,6 +71,28 @@ static char *gpsPacketLogChar = gpsPacketLog;
 // **********************
 // GPS
 // **********************
+int32_t GPS_home[2];
+uint16_t GPS_distanceToHome;        // distance to home point in meters
+int16_t GPS_directionToHome;        // direction to home or hol point in degrees
+int16_t GPS_angle[ANGLE_INDEX_COUNT] = { 0, 0 };    // it's the angles that must be applied for GPS correction
+float dTnav;             // Delta Time in milliseconds for navigation computations, updated with every good GPS read
+int16_t actual_speed[2] = { 0, 0 };
+int16_t nav_takeoff_bearing;
+navigationMode_e nav_mode = NAV_MODE_NONE;    // Navigation mode
+
+// moving average filter variables
+#define GPS_FILTERING              1    // add a 5 element moving average filter to GPS coordinates, helps eliminate gps noise but adds latency
+#ifdef GPS_FILTERING
+#define GPS_FILTER_VECTOR_LENGTH 5
+static uint8_t GPS_filter_index = 0;
+static int32_t GPS_filter[2][GPS_FILTER_VECTOR_LENGTH];
+static int32_t GPS_filter_sum[2];
+static int32_t GPS_read[2];
+static int32_t GPS_filtered[2];
+static int32_t GPS_degree[2];   //the lat lon degree without any decimals (lat/10 000 000)
+static uint16_t fraction3[2];
+#endif
+
 gpsSolutionData_t gpsSol;
 uint32_t GPS_packetCount = 0;
 uint32_t GPS_svInfoReceivedCount = 0; // SV = Space Vehicle, counter increments each time SV info is received.
@@ -111,6 +133,7 @@ static const gpsInitData_t gpsInitData[] = {
 
 #define DEFAULT_BAUD_RATE_INDEX 0
 
+#ifdef USE_GPS_UBLOX
 static const uint8_t ubloxInit[] = {
     //Preprocessor Pedestrian Dynamic Platform Model Option
     #if defined(GPS_UBLOX_MODE_PEDESTRIAN)
@@ -179,7 +202,7 @@ static const ubloxSbas_t ubloxSbas[] = {
     { SBAS_MSAS,  { 0x00, 0x02, 0x02, 0x00, 0x35, 0xEF}},
     { SBAS_GAGAN, { 0x80, 0x01, 0x00, 0x00, 0xB2, 0xE8}}
 };
-
+#endif // USE_GPS_UBLOX
 
 typedef enum {
     GPS_UNKNOWN,
@@ -212,8 +235,12 @@ static void shiftPacketLog(void)
 }
 
 static void gpsNewData(uint16_t c);
+#ifdef USE_GPS_NMEA
 static bool gpsNewFrameNMEA(char c);
+#endif
+#ifdef USE_GPS_UBLOX
 static bool gpsNewFrameUBLOX(uint8_t data);
+#endif
 
 static void gpsSetState(gpsState_e state)
 {
@@ -268,6 +295,7 @@ void gpsInit(void)
     gpsSetState(GPS_INITIALIZING);
 }
 
+#ifdef USE_GPS_NMEA
 void gpsInitNmea(void)
 {
 #if defined(COLIBRI_RACE) || defined(LUX_RACE)
@@ -316,7 +344,9 @@ void gpsInitNmea(void)
             break;
     }
 }
+#endif // USE_GPS_NMEA
 
+#ifdef USE_GPS_UBLOX
 void gpsInitUblox(void)
 {
     uint32_t now;
@@ -400,17 +430,22 @@ void gpsInitUblox(void)
             break;
     }
 }
+#endif // USE_GPS_UBLOX
 
 void gpsInitHardware(void)
 {
     switch (gpsConfig()->provider) {
-        case GPS_NMEA:
+    case GPS_NMEA:
-            gpsInitNmea();
+#ifdef USE_GPS_NMEA
-            break;
+        gpsInitNmea();
+#endif
+        break;
 
-        case GPS_UBLOX:
+    case GPS_UBLOX:
-            gpsInitUblox();
+#ifdef USE_GPS_UBLOX
-            break;
+        gpsInitUblox();
+#endif
+        break;
     }
 }
 
@@ -494,12 +529,17 @@ static void gpsNewData(uint16_t c)
 bool gpsNewFrame(uint8_t c)
 {
     switch (gpsConfig()->provider) {
-        case GPS_NMEA:          // NMEA
+    case GPS_NMEA:          // NMEA
-            return gpsNewFrameNMEA(c);
+#ifdef USE_GPS_NMEA
-        case GPS_UBLOX:         // UBX binary
+        return gpsNewFrameNMEA(c);
-            return gpsNewFrameUBLOX(c);
+#endif
+        break;
+    case GPS_UBLOX:         // UBX binary
+#ifdef USE_GPS_UBLOX
+        return gpsNewFrameUBLOX(c);
+#endif
+        break;
     }
-
     return false;
 }
 
@@ -566,6 +606,7 @@ static uint32_t GPS_coord_to_degrees(char *coordinateString)
 */
 
 // helper functions
+#ifdef USE_GPS_NMEA
 static uint32_t grab_fields(char *src, uint8_t mult)
 {                               // convert string to uint32
     uint32_t i;
@@ -764,7 +805,9 @@ static bool gpsNewFrameNMEA(char c)
     }
     return frameOK;
 }
+#endif // USE_GPS_NMEA
 
+#ifdef USE_GPS_UBLOX
 // UBX support
 typedef struct {
     uint8_t preamble1;
@@ -1086,9 +1129,10 @@ static bool gpsNewFrameUBLOX(uint8_t data)
     }
     return parsed;
 }
+#endif // USE_GPS_UBLOX
 
 static void gpsHandlePassthrough(uint8_t data)
- {
+{
      gpsNewData(data);
  #ifdef USE_DASHBOARD
      if (feature(FEATURE_DASHBOARD)) {
@@ -1114,4 +1158,142 @@ void gpsEnablePassthrough(serialPort_t *gpsPassthroughPort)
 
     serialPassthrough(gpsPort, gpsPassthroughPort, &gpsHandlePassthrough, NULL);
 }
+
+float GPS_scaleLonDown = 1.0f;  // this is used to offset the shrinking longitude as we go towards the poles
+
+void GPS_calc_longitude_scaling(int32_t lat)
+{
+    float rads = (ABS((float)lat) / 10000000.0f) * 0.0174532925f;
+    GPS_scaleLonDown = cos_approx(rads);
+}
+
+
+void GPS_reset_home_position(void)
+{
+    if (STATE(GPS_FIX) && gpsSol.numSat >= 5) {
+        GPS_home[LAT] = gpsSol.llh.lat;
+        GPS_home[LON] = gpsSol.llh.lon;
+        GPS_calc_longitude_scaling(gpsSol.llh.lat); // need an initial value for distance and bearing calc
+#ifdef USE_NAV
+        nav_takeoff_bearing = DECIDEGREES_TO_DEGREES(attitude.values.yaw);              // save takeoff heading
+#endif
+        // Set ground altitude
+        ENABLE_STATE(GPS_FIX_HOME);
+    }
+}
+
+////////////////////////////////////////////////////////////////////////////////////
+#define DISTANCE_BETWEEN_TWO_LONGITUDE_POINTS_AT_EQUATOR_IN_HUNDREDS_OF_KILOMETERS 1.113195f
+#define TAN_89_99_DEGREES 5729.57795f
+// Get distance between two points in cm
+// Get bearing from pos1 to pos2, returns an 1deg = 100 precision
+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_IN_HUNDREDS_OF_KILOMETERS;
+
+    *bearing = 9000.0f + atan2_approx(-dLat, dLon) * TAN_89_99_DEGREES;      // Convert the output radians to 100xdeg
+    if (*bearing < 0)
+        *bearing += 36000;
+}
+
+void GPS_calculateDistanceAndDirectionToHome(void)
+{
+    if (STATE(GPS_FIX_HOME)) {      // If we don't have home set, do not display anything
+        uint32_t dist;
+        int32_t dir;
+        GPS_distance_cm_bearing(&gpsSol.llh.lat, &gpsSol.llh.lon, &GPS_home[LAT], &GPS_home[LON], &dist, &dir);
+        GPS_distanceToHome = dist / 100;
+        GPS_directionToHome = dir / 100;
+    } else {
+        GPS_distanceToHome = 0;
+        GPS_directionToHome = 0;
+    }
+}
+
+////////////////////////////////////////////////////////////////////////////////////
+// Calculate our current speed vector from gps position data
+//
+static void GPS_calc_velocity(void)
+{
+    static int16_t speed_old[2] = { 0, 0 };
+    static int32_t last_coord[2] = { 0, 0 };
+    static uint8_t init = 0;
+
+    if (init) {
+        float tmp = 1.0f / dTnav;
+        actual_speed[GPS_X] = (float)(gpsSol.llh.lon - last_coord[LON]) * GPS_scaleLonDown * tmp;
+        actual_speed[GPS_Y] = (float)(gpsSol.llh.lat - last_coord[LAT]) * tmp;
+
+        actual_speed[GPS_X] = (actual_speed[GPS_X] + speed_old[GPS_X]) / 2;
+        actual_speed[GPS_Y] = (actual_speed[GPS_Y] + speed_old[GPS_Y]) / 2;
+
+        speed_old[GPS_X] = actual_speed[GPS_X];
+        speed_old[GPS_Y] = actual_speed[GPS_Y];
+    }
+    init = 1;
+
+    last_coord[LON] = gpsSol.llh.lon;
+    last_coord[LAT] = gpsSol.llh.lat;
+}
+
+void onGpsNewData(void)
+{
+    if (!(STATE(GPS_FIX) && gpsSol.numSat >= 5)) {
+        return;
+    }
+
+    if (!ARMING_FLAG(ARMED))
+        DISABLE_STATE(GPS_FIX_HOME);
+
+    if (!STATE(GPS_FIX_HOME) && ARMING_FLAG(ARMED))
+        GPS_reset_home_position();
+
+    // Apply moving average filter to GPS data
+#if defined(GPS_FILTERING)
+    GPS_filter_index = (GPS_filter_index + 1) % GPS_FILTER_VECTOR_LENGTH;
+    for (int axis = 0; axis < 2; axis++) {
+        GPS_read[axis] = axis == LAT ? gpsSol.llh.lat : gpsSol.llh.lon; // latest unfiltered data is in GPS_latitude and GPS_longitude
+        GPS_degree[axis] = GPS_read[axis] / 10000000;   // get the degree to assure the sum fits to the int32_t
+
+        // How close we are to a degree line ? its the first three digits from the fractions of degree
+        // later we use it to Check if we are close to a degree line, if yes, disable averaging,
+        fraction3[axis] = (GPS_read[axis] - GPS_degree[axis] * 10000000) / 10000;
+
+        GPS_filter_sum[axis] -= GPS_filter[axis][GPS_filter_index];
+        GPS_filter[axis][GPS_filter_index] = GPS_read[axis] - (GPS_degree[axis] * 10000000);
+        GPS_filter_sum[axis] += GPS_filter[axis][GPS_filter_index];
+        GPS_filtered[axis] = GPS_filter_sum[axis] / GPS_FILTER_VECTOR_LENGTH + (GPS_degree[axis] * 10000000);
+        if (nav_mode == NAV_MODE_POSHOLD) {             // we use gps averaging only in poshold mode...
+            if (fraction3[axis] > 1 && fraction3[axis] < 999) {
+                if (axis == LAT) {
+                    gpsSol.llh.lat = GPS_filtered[LAT];
+                } else {
+                    gpsSol.llh.lon = GPS_filtered[LON];
+                }
+            }
+        }
+    }
+#endif
+
+    //
+    // Calculate time delta for navigation loop, range 0-1.0f, in seconds
+    //
+    // Time for calculating x,y speed and navigation pids
+    static uint32_t nav_loopTimer;
+    dTnav = (float)(millis() - nav_loopTimer) / 1000.0f;
+    nav_loopTimer = millis();
+    // prevent runup from bad GPS
+    dTnav = MIN(dTnav, 1.0f);
+
+    GPS_calculateDistanceAndDirectionToHome();
+    // calculate the current velocity based on gps coordinates continously to get a valid speed at the moment when we start navigating
+    GPS_calc_velocity();
+
+#ifdef USE_NAV
+    navNewGpsData();
+#endif
+}
+
 #endif

src/main/io/gps.h

@@ -17,6 +17,7 @@
 
 #pragma once
 
+#include "common/axis.h"
 #include "common/time.h"
 
 #include "config/parameter_group.h"
@@ -25,6 +26,8 @@
 #define LON 1
 
 #define GPS_DEGREES_DIVIDER 10000000L
+#define GPS_X 1
+#define GPS_Y 0
 
 typedef enum {
     GPS_NMEA = 0,
@@ -114,6 +117,22 @@ typedef struct gpsData_s {
 #define GPS_PACKET_LOG_ENTRY_COUNT 21 // To make this useful we should log as many packets as we can fit characters a single line of a OLED display.
 extern char gpsPacketLog[GPS_PACKET_LOG_ENTRY_COUNT];
 
+extern int32_t GPS_home[2];
+extern uint16_t GPS_distanceToHome;        // distance to home point in meters
+extern int16_t GPS_directionToHome;        // direction to home or hol point in degrees
+extern int16_t GPS_angle[ANGLE_INDEX_COUNT];                // it's the angles that must be applied for GPS correction
+extern float dTnav;             // Delta Time in milliseconds for navigation computations, updated with every good GPS read
+extern float GPS_scaleLonDown;  // this is used to offset the shrinking longitude as we go towards the poles
+extern int16_t actual_speed[2];
+extern int16_t nav_takeoff_bearing;
+// navigation mode
+typedef enum {
+    NAV_MODE_NONE = 0,
+    NAV_MODE_POSHOLD,
+    NAV_MODE_WP
+} navigationMode_e;
+extern navigationMode_e nav_mode;          // Navigation mode
+
 extern gpsData_t gpsData;
 extern gpsSolutionData_t gpsSol;
 
@@ -134,4 +153,9 @@ void gpsUpdate(timeUs_t currentTimeUs);
 bool gpsNewFrame(uint8_t c);
 struct serialPort_s;
 void gpsEnablePassthrough(struct serialPort_s *gpsPassthroughPort);
+void onGpsNewData(void);
+void GPS_reset_home_position(void);
+void GPS_calc_longitude_scaling(int32_t lat);
+void navNewGpsData(void);
+void GPS_distance_cm_bearing(int32_t *currentLat1, int32_t *currentLon1, int32_t *destinationLat2, int32_t *destinationLon2, uint32_t *dist, int32_t *bearing);
 

src/main/target/RCEXPLORERF3/target.h

@@ -104,6 +104,9 @@
 #define SERIALRX_PROVIDER       SERIALRX_SBUS
 #define SERIALRX_UART           SERIAL_PORT_USART1
 
+#define USE_GPS
+#define USE_GPS_UBLOX
+#define USE_GPS_NMEA
 #define USE_NAV
 
 #define USE_SERIAL_4WAY_BLHELI_INTERFACE

src/main/target/common_fc_pre.h

@@ -145,6 +145,8 @@
 #if (FLASH_SIZE > 256)
 // Temporarily moved GPS here because of overflowing flash size on F3
 #define USE_GPS
+#define USE_GPS_UBLOX
+#define USE_GPS_NMEA
 #define USE_NAV
 #define USE_UNCOMMON_MIXERS
 #define USE_OSD_ADJUSTMENTS

src/main/telemetry/hott.c

@@ -74,7 +74,6 @@
 
 #include "flight/altitude.h"
 #include "flight/pid.h"
-#include "flight/navigation.h"
 
 #include "io/gps.h"
 

src/main/telemetry/ltm.c

@@ -70,7 +70,6 @@
 #include "flight/imu.h"
 #include "flight/failsafe.h"
 #include "flight/altitude.h"
-#include "flight/navigation.h"
 
 #include "telemetry/telemetry.h"
 #include "telemetry/ltm.h"

src/main/telemetry/smartport.c

@@ -36,7 +36,6 @@
 #include "flight/imu.h"
 #include "flight/mixer.h"
 #include "flight/pid.h"
-#include "flight/navigation.h"
 
 #include "interface/msp.h"
 

src/test/unit/osd_unittest.cc

@@ -57,7 +57,7 @@ extern "C" {
     int16_t rcData[MAX_SUPPORTED_RC_CHANNEL_COUNT];
     uint8_t GPS_numSat;
     uint16_t GPS_distanceToHome;
-    uint16_t GPS_directionToHome;
+    int16_t GPS_directionToHome;
     int32_t GPS_coord[2];
     gpsSolutionData_t gpsSol;