From c0ce62d601cbb18134092416eade842dea848c99 Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Thu, 29 Jun 2017 13:17:58 +0100 Subject: [PATCH] Added USE_NAV build flag --- src/main/flight/navigation.c | 38 ++++++++++++++++++++++++--------- src/main/target/common_fc_pre.h | 7 +++--- 2 files changed, 31 insertions(+), 14 deletions(-) diff --git a/src/main/flight/navigation.c b/src/main/flight/navigation.c index c34b66819..d107939d0 100644 --- a/src/main/flight/navigation.c +++ b/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) { diff --git a/src/main/target/common_fc_pre.h b/src/main/target/common_fc_pre.h index a99bbb0e3..5ce914c9e 100644 --- a/src/main/target/common_fc_pre.h +++ b/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