Merge pull request #3385 from martinbudden/bf_use_nav

Added USE_NAV build flag

src/main/flight/navigation.c

@@ -107,19 +107,22 @@ void navigationInit(void)
 #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 bool check_missed_wp(void);
 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);
-static int32_t wrap_36000(int32_t angle);
 
 typedef struct {
     int16_t last_velocity;
@@ -134,7 +137,6 @@ typedef struct {
 
 static PID_PARAM posholdPID_PARAM;
 static PID_PARAM poshold_ratePID_PARAM;
-static PID_PARAM navPID_PARAM;
 
 typedef struct {
     float integrator;          // integrator value
@@ -146,6 +148,9 @@ typedef struct {
 
 static PID posholdPID[2];
 static PID poshold_ratePID[2];
+
+#ifdef USE_NAV
+static PID_PARAM navPID_PARAM;
 static PID navPID[2];
 
 static int32_t get_P(int32_t error, PID_PARAM *pid)
@@ -176,6 +181,7 @@ 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)
 {
@@ -197,11 +203,15 @@ static void reset_PID(PID *pid)
 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];
-static int32_t 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];
@@ -217,8 +227,6 @@ static int32_t target_bearing;
 // deg * 100, The original angle to the next_WP when the next_WP was set
 // Also used to check when we pass a WP
 static int32_t original_target_bearing;
-// The amount of angle correction applied to target_bearing to bring the copter back on its optimum path
-static int16_t crosstrack_error;
 ////////////////////////////////////////////////////////////////////////////////
 // The location of the copter in relation to home, updated every GPS read (1deg - 100)
 //static int32_t home_to_copter_bearing;
@@ -265,10 +273,7 @@ void GPS_calculateDistanceAndDirectionToHome(void)
 
 void onGpsNewData(void)
 {
-    int axis;
     static uint32_t nav_loopTimer;
-    uint16_t speed;
-
 
     if (!(STATE(GPS_FIX) && gpsSol.numSat >= 5)) {
         return;
@@ -283,7 +288,7 @@ void onGpsNewData(void)
     // Apply moving average filter to GPS data
 #if defined(GPS_FILTERING)
     GPS_filter_index = (GPS_filter_index + 1) % GPS_FILTER_VECTOR_LENGTH;
-    for (axis = 0; axis < 2; axis++) {
+    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
 
@@ -321,6 +326,7 @@ void onGpsNewData(void)
     // 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
 
@@ -328,6 +334,7 @@ void onGpsNewData(void)
         GPS_distance_cm_bearing(&gpsSol.llh.lat, &gpsSol.llh.lon, &GPS_WP[LAT], &GPS_WP[LON], &wp_distance, &target_bearing);
         GPS_calc_location_error(&GPS_WP[LAT], &GPS_WP[LON], &gpsSol.llh.lat, &gpsSol.llh.lon);
 
+        uint16_t speed;
         switch (nav_mode) {
         case NAV_MODE_POSHOLD:
             // Desired output is in nav_lat and nav_lon where 1deg inclination is 100
@@ -360,6 +367,7 @@ void onGpsNewData(void)
             break;
         }
     }                   //end of gps calcs
+#endif
 }
 
 void GPS_reset_home_position(void)
@@ -385,7 +393,9 @@ 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
     }
 }
 
@@ -401,10 +411,12 @@ 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
@@ -442,6 +454,7 @@ 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
 //
@@ -452,6 +465,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
@@ -522,6 +536,7 @@ 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
 //
@@ -627,6 +642,7 @@ static uint16_t GPS_calc_desired_speed(uint16_t max_speed, bool _slow)
     }
     return max_speed;
 }
+#endif
 
 ////////////////////////////////////////////////////////////////////////////////////
 // Utilities
@@ -640,6 +656,7 @@ static int32_t wrap_18000(int32_t error)
     return error;
 }
 
+#ifdef USE_NAV
 static int32_t wrap_36000(int32_t angle)
 {
     if (angle > 36000)
@@ -648,6 +665,7 @@ static int32_t wrap_36000(int32_t angle)
         angle += 36000;
     return angle;
 }
+#endif
 
 void updateGpsStateForHomeAndHoldMode(void)
 {

src/main/target/common_fc_pre.h

@@ -115,6 +115,7 @@
 #define TELEMETRY_SRXL
 #define USE_DASHBOARD
 #define USE_MSP_DISPLAYPORT
+#define USE_RCSPLIT
 #define USE_RX_MSP
 #define USE_SERIALRX_JETIEXBUS
 #define USE_SENSOR_NAMES
@@ -131,10 +132,8 @@
 #endif
 
 #if (FLASH_SIZE > 256)
-// Temporarily moved this here because of overflowing flash size on F3
+// Temporarily moved GPS here because of overflowing flash size on F3
 #define GPS
-
+#define USE_NAV
 #define USE_UNCOMMON_MIXERS
 #endif
-
-#define USE_RCSPLIT