diff --git a/make/source.mk b/make/source.mk
index 7bfae38991..fd7f3bb529 100644
--- a/make/source.mk
+++ b/make/source.mk
@@ -135,7 +135,6 @@ FC_SRC = \
drivers/rangefinder/rangefinder_lidartf.c \
drivers/serial_escserial.c \
drivers/vtx_common.c \
- flight/navigation.c \
io/dashboard.c \
io/displayport_max7456.c \
io/displayport_msp.c \
diff --git a/src/main/fc/config.c b/src/main/fc/config.c
index f06e07fc0d..121dcc6474 100644
--- a/src/main/fc/config.c
+++ b/src/main/fc/config.c
@@ -42,7 +42,6 @@
#include "flight/failsafe.h"
#include "flight/imu.h"
#include "flight/mixer.h"
-#include "flight/navigation.h"
#include "flight/pid.h"
#include "flight/servos.h"
@@ -141,10 +140,6 @@ void activateConfig(void)
useRcControlsConfig(currentPidProfile);
useAdjustmentConfig(currentPidProfile);
-#ifdef USE_NAV
- gpsUsePIDs(currentPidProfile);
-#endif
-
failsafeReset();
setAccelerationTrims(&accelerometerConfigMutable()->accZero);
accInitFilters();
diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c
index d4304b6742..43912d31b0 100644
--- a/src/main/fc/fc_core.c
+++ b/src/main/fc/fc_core.c
@@ -87,7 +87,6 @@
#include "flight/failsafe.h"
#include "flight/imu.h"
#include "flight/mixer.h"
-#include "flight/navigation.h"
#include "flight/pid.h"
#include "flight/servos.h"
@@ -756,13 +755,7 @@ bool processRx(timeUs_t currentTimeUs)
}
}
#endif
-
-#ifdef USE_NAV
- if (sensors(SENSOR_GPS)) {
- updateGpsWaypointsAndMode();
- }
-#endif
-
+
if (IS_RC_MODE_ACTIVE(BOXPASSTHRU)) {
ENABLE_FLIGHT_MODE(PASSTHRU_MODE);
} else {
@@ -889,14 +882,6 @@ static NOINLINE void subTaskMainSubprocesses(timeUs_t currentTimeUs)
processRcCommand();
-#ifdef USE_NAV
- if (sensors(SENSOR_GPS)) {
- if ((FLIGHT_MODE(GPS_HOME_MODE) || FLIGHT_MODE(GPS_HOLD_MODE)) && STATE(GPS_FIX_HOME)) {
- updateGpsStateForHomeAndHoldMode();
- }
- }
-#endif
-
#ifdef USE_SDCARD
afatfs_poll();
#endif
diff --git a/src/main/fc/fc_init.c b/src/main/fc/fc_init.c
index 6c13b952bf..dcce15e03c 100644
--- a/src/main/fc/fc_init.c
+++ b/src/main/fc/fc_init.c
@@ -148,7 +148,6 @@
#include "flight/failsafe.h"
#include "flight/imu.h"
#include "flight/mixer.h"
-#include "flight/navigation.h"
#include "flight/pid.h"
#include "flight/servos.h"
@@ -662,9 +661,6 @@ void init(void)
#ifdef USE_GPS
if (feature(FEATURE_GPS)) {
gpsInit();
-#ifdef USE_NAV
- navigationInit();
-#endif
}
#endif
diff --git a/src/main/flight/navigation.c b/src/main/flight/navigation.c
deleted file mode 100644
index 60e80a8154..0000000000
--- a/src/main/flight/navigation.c
+++ /dev/null
@@ -1,591 +0,0 @@
-/*
- * This file is part of Cleanflight and Betaflight.
- *
- * Cleanflight and Betaflight are free software. You can redistribute
- * this software and/or modify this software under the terms of the
- * GNU General Public License as published by the Free Software
- * Foundation, either version 3 of the License, or (at your option)
- * any later version.
- *
- * Cleanflight and Betaflight are distributed in the hope that they
- * will be useful, but WITHOUT ANY WARRANTY; without even the implied
- * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
- * See the GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this software.
- *
- * If not, see .
- */
-
-#include
-#include
-#include
-
-#include "platform.h"
-
-#ifdef USE_NAV
-
-#include "build/debug.h"
-
-#include "common/gps_conversion.h"
-#include "common/maths.h"
-#include "common/time.h"
-
-#include "pg/pg.h"
-#include "pg/pg_ids.h"
-
-#include "drivers/time.h"
-
-#include "fc/config.h"
-#include "fc/fc_core.h"
-#include "fc/rc_modes.h"
-#include "fc/runtime_config.h"
-
-#include "flight/imu.h"
-#include "flight/navigation.h"
-#include "flight/pid.h"
-
-#include "io/beeper.h"
-#include "io/serial.h"
-#include "io/gps.h"
-
-#include "rx/rx.h"
-
-#include "sensors/acceleration.h"
-#include "sensors/boardalignment.h"
-#include "sensors/sensors.h"
-
-
-PG_REGISTER_WITH_RESET_TEMPLATE(navigationConfig_t, navigationConfig, PG_NAVIGATION_CONFIG, 0);
-
-PG_RESET_TEMPLATE(navigationConfig_t, navigationConfig,
- .gps_wp_radius = 200,
- .gps_lpf = 20,
- .nav_slew_rate = 30,
- .nav_controls_heading = 1,
- .nav_speed_min = 100,
- .nav_speed_max = 300,
- .ap_mode = 40
-);
-
-bool areSticksInApModePosition(uint16_t ap_mode);
-
-// **********************
-// GPS
-// **********************
-int32_t GPS_hold[2];
-
-
-static int16_t nav[2];
-static int16_t nav_rated[2]; // Adding a rate controller to the navigation to make it smoother
-
-// When using PWM input GPS usage reduces number of available channels by 2 - see pwm_common.c/pwmInit()
-void navigationInit(void)
-{
- gpsUsePIDs(currentPidProfile);
-}
-
-
-
-/*-----------------------------------------------------------
- *
- * Multiwii GPS code - revision: 1097
- *
- *-----------------------------------------------------------*/
-#define POSHOLD_IMAX 20 // degrees
-#define POSHOLD_RATE_IMAX 20 // degrees
-#define NAV_IMAX 20 // degrees
-
-/* GPS navigation can control the heading */
-#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_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(int32_t lat1, int32_t lon1, int32_t lat2, int32_t lon2, uint16_t* dist, int16_t* bearing);
-static void GPS_calc_location_error(int32_t * target_lat, int32_t * target_lng, int32_t * gps_lat, int32_t * gps_lng);
-
-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);
-
-static int32_t wrap_18000(int32_t error);
-
-typedef struct {
- int16_t last_velocity;
-} LeadFilter_PARAM;
-
-typedef struct {
- float kP;
- float kI;
- float kD;
- float Imax;
-} PID_PARAM;
-
-static PID_PARAM posholdPID_PARAM;
-static PID_PARAM poshold_ratePID_PARAM;
-
-typedef struct {
- float integrator; // integrator value
- int32_t last_input; // last input for derivative
- float last_derivative; // last derivative for low-pass filter
- float output;
- float derivative;
-} PID;
-
-static PID posholdPID[2];
-static PID poshold_ratePID[2];
-
-static PID_PARAM navPID_PARAM;
-static PID navPID[2];
-
-static int32_t get_P(int32_t error, PID_PARAM *pid)
-{
- return (float)error * pid->kP;
-}
-
-static int32_t get_I(int32_t error, float *dt, PID *pid, PID_PARAM *pid_param)
-{
- pid->integrator += ((float)error * pid_param->kI) * *dt;
- pid->integrator = constrain(pid->integrator, -pid_param->Imax, pid_param->Imax);
- return pid->integrator;
-}
-
-static int32_t get_D(int32_t input, float *dt, PID *pid, PID_PARAM *pid_param)
-{
- pid->derivative = (input - pid->last_input) / *dt;
-
- // Low pass filter cut frequency for derivative calculation
- // Set to "1 / ( 2 * PI * gps_lpf )
- float pidFilter = (1.0f / (2.0f * M_PIf * (float)navigationConfig()->gps_lpf));
- // discrete low pass filter, cuts out the
- // high frequency noise that can drive the controller crazy
- pid->derivative = pid->last_derivative + (*dt / (pidFilter + *dt)) * (pid->derivative - pid->last_derivative);
- // update state
- pid->last_input = input;
- pid->last_derivative = pid->derivative;
- // add in derivative component
- return pid_param->kD * pid->derivative;
-}
-
-static void reset_PID(PID *pid)
-{
- pid->integrator = 0;
- pid->last_input = 0;
- pid->last_derivative = 0;
-}
-
-
-/****************** PI and PID controllers for GPS ********************///32938 -> 33160
-
-#define RADX100 0.000174532925f
-#define CROSSTRACK_GAIN 1
-#define NAV_SLOW_NAV true
-#define NAV_BANK_MAX 3000 // 30deg max banking when navigating (just for security and testing)
-
-static int32_t error[2];
-
-// 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;
-
-// Currently used WP
-static int32_t GPS_WP[2];
-
-////////////////////////////////////////////////////////////////////////////////
-// Location & Navigation
-////////////////////////////////////////////////////////////////////////////////
-// This is the angle from the copter to the "next_WP" location in degrees * 100
-static int32_t target_bearing;
-////////////////////////////////////////////////////////////////////////////////
-// Crosstrack
-////////////////////////////////////////////////////////////////////////////////
-// 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 location of the copter in relation to home, updated every GPS read (1deg - 100)
-//static int32_t home_to_copter_bearing;
-// distance between plane and home in cm
-//static int32_t home_distance;
-// distance between plane and next_WP in cm
-static uint32_t wp_distance;
-
-// used for slow speed wind up when start navigation;
-static int16_t waypoint_speed_gov;
-
-// 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
-
-
-void navNewGpsData(void)
-{
- if (FLIGHT_MODE(GPS_HOLD_MODE) || FLIGHT_MODE(GPS_HOME_MODE)) {
- // we are navigating
-
- // gps nav calculations, these are common for nav and poshold
- 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
- GPS_calc_poshold();
- break;
-
- case NAV_MODE_WP:
- speed = GPS_calc_desired_speed(navigationConfig()->nav_speed_max, NAV_SLOW_NAV); // slow navigation
- // use error as the desired rate towards the target
- // Desired output is in nav_lat and nav_lon where 1deg inclination is 100
- GPS_calc_nav_rate(speed);
-
- // Tail control
- if (navigationConfig()->nav_controls_heading) {
- if (NAV_TAIL_FIRST) {
- magHold = wrap_18000(nav_bearing - 18000) / 100;
- } else {
- magHold = nav_bearing / 100;
- }
- }
- // Are we there yet ?(within x meters of the destination)
- if ((wp_distance <= navigationConfig()->gps_wp_radius) || check_missed_wp()) { // if yes switch to poshold mode
- nav_mode = NAV_MODE_POSHOLD;
- if (NAV_SET_TAKEOFF_HEADING) {
- magHold = nav_takeoff_bearing;
- }
- }
- break;
- default:
- break;
- }
- } //end of gps calcs
-}
-
-// reset navigation (stop the navigation processor, and clear nav)
-void GPS_reset_nav(void)
-{
- int i;
-
- for (i = 0; i < 2; i++) {
- GPS_angle[i] = 0;
- nav_rated[i] = 0;
- nav[i] = 0;
- reset_PID(&posholdPID[i]);
- reset_PID(&poshold_ratePID[i]);
- reset_PID(&navPID[i]);
- }
-}
-
-// Get the relevant P I D values and set the PID controllers
-void gpsUsePIDs(pidProfile_t *pidProfile)
-{
- posholdPID_PARAM.kP = (float)pidProfile->pid[PID_POS].P / 100.0f;
- posholdPID_PARAM.kI = (float)pidProfile->pid[PID_POS].I / 100.0f;
- posholdPID_PARAM.Imax = POSHOLD_RATE_IMAX * 100;
-
- poshold_ratePID_PARAM.kP = (float)pidProfile->pid[PID_POSR].P / 10.0f;
- poshold_ratePID_PARAM.kI = (float)pidProfile->pid[PID_POSR].I / 100.0f;
- poshold_ratePID_PARAM.kD = (float)pidProfile->pid[PID_POSR].D / 1000.0f;
- poshold_ratePID_PARAM.Imax = POSHOLD_RATE_IMAX * 100;
-
- 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;
-}
-
-// OK here is the onboard GPS code
-
-////////////////////////////////////////////////////////////////////////////////////
-// PID based GPS navigation functions
-// Author : EOSBandi
-// Based on code and ideas from the Arducopter team: Jason Short,Randy Mackay, Pat Hickey, Jose Julio, Jani Hirvinen
-// Andrew Tridgell, Justin Beech, Adam Rivera, Jean-Louis Naudin, Roberto Navoni
-
-////////////////////////////////////////////////////////////////////////////////////
-// 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
-//
-////////////////////////////////////////////////////////////////////////////////////
-// Sets the waypoint to navigate, reset neccessary variables and calculate initial values
-//
-void GPS_set_next_wp(int32_t *lat, int32_t *lon)
-{
- GPS_WP[LAT] = *lat;
- GPS_WP[LON] = *lon;
-
- GPS_calc_longitude_scaling(*lat);
- GPS_distance_cm_bearing(&gpsSol.llh.lat, &gpsSol.llh.lon, &GPS_WP[LAT], &GPS_WP[LON], &wp_distance, &target_bearing);
-
- nav_bearing = target_bearing;
- GPS_calc_location_error(&GPS_WP[LAT], &GPS_WP[LON], &gpsSol.llh.lat, &gpsSol.llh.lon);
- original_target_bearing = target_bearing;
- waypoint_speed_gov = navigationConfig()->nav_speed_min;
-}
-
-////////////////////////////////////////////////////////////////////////////////////
-// Check if we missed the destination somehow
-//
-static bool check_missed_wp(void)
-{
- int32_t temp;
- temp = target_bearing - original_target_bearing;
- temp = wrap_18000(temp);
- return (ABS(temp) > 10000); // we passed the waypoint by 100 degrees
-}
-
-
-////////////////////////////////////////////////////////////////////////////////////
-// keep old calculation function for compatibility (could be removed later) distance in meters, bearing in degree
-//
-//static void GPS_distance(int32_t lat1, int32_t lon1, int32_t lat2, int32_t lon2, uint16_t* dist, int16_t* bearing) {
-// uint32_t d1;
-// int32_t d2;
-// GPS_distance_cm_bearing(&lat1,&lon1,&lat2,&lon2,&d1,&d2);
-// *dist = d1 / 100; //convert to meters
-// *bearing = d2 / 100; //convert to degrees
-//}
-
-////////////////////////////////////////////////////////////////////////////////////
-// 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:
-// 100 = 1m
-// 1000 = 11m = 36 feet
-// 1800 = 19.80m = 60 feet
-// 3000 = 33m
-// 10000 = 111m
-//
-static void GPS_calc_location_error(int32_t *target_lat, int32_t *target_lng, int32_t *gps_lat, int32_t *gps_lng)
-{
- error[LON] = (float)(*target_lng - *gps_lng) * GPS_scaleLonDown; // X Error
- error[LAT] = *target_lat - *gps_lat; // Y Error
-}
-
-////////////////////////////////////////////////////////////////////////////////////
-// Calculate nav_lat and nav_lon from the x and y error and the speed
-//
-static void GPS_calc_poshold(void)
-{
- int32_t d;
- int32_t target_speed;
- int axis;
-
- for (axis = 0; axis < 2; axis++) {
- target_speed = get_P(error[axis], &posholdPID_PARAM); // calculate desired speed from lon error
- rate_error[axis] = target_speed - actual_speed[axis]; // calc the speed error
-
- nav[axis] = get_P(rate_error[axis], &poshold_ratePID_PARAM) +
- get_I(rate_error[axis] + error[axis], &dTnav, &poshold_ratePID[axis], &poshold_ratePID_PARAM);
- d = get_D(error[axis], &dTnav, &poshold_ratePID[axis], &poshold_ratePID_PARAM);
- d = constrain(d, -2000, 2000);
-
- // get rid of noise
-#if defined(GPS_LOW_SPEED_D_FILTER)
- if (ABS(actual_speed[axis]) < 50)
- d = 0;
-#endif
-
- nav[axis] += d;
- nav[axis] = constrain(nav[axis], -NAV_BANK_MAX, NAV_BANK_MAX);
- navPID[axis].integrator = poshold_ratePID[axis].integrator;
- }
-}
-
-////////////////////////////////////////////////////////////////////////////////////
-// Calculate the desired nav_lat and nav_lon for distance flying such as RTH
-//
-static void GPS_calc_nav_rate(uint16_t max_speed)
-{
- float trig[2];
- float temp;
- int axis;
-
- // push us towards the original track
- GPS_update_crosstrack();
-
- // nav_bearing includes crosstrack
- temp = (9000l - nav_bearing) * RADX100;
- trig[GPS_X] = cos_approx(temp);
- trig[GPS_Y] = sin_approx(temp);
-
- for (axis = 0; axis < 2; axis++) {
- rate_error[axis] = (trig[axis] * max_speed) - actual_speed[axis];
- rate_error[axis] = constrain(rate_error[axis], -1000, 1000);
- // P + I + D
- nav[axis] = get_P(rate_error[axis], &navPID_PARAM) +
- get_I(rate_error[axis], &dTnav, &navPID[axis], &navPID_PARAM) +
- get_D(rate_error[axis], &dTnav, &navPID[axis], &navPID_PARAM);
-
- nav[axis] = constrain(nav[axis], -NAV_BANK_MAX, NAV_BANK_MAX);
- poshold_ratePID[axis].integrator = navPID[axis].integrator;
- }
-}
-
-////////////////////////////////////////////////////////////////////////////////////
-// Calculating cross track error, this tries to keep the copter on a direct line
-// when flying to a waypoint.
-//
-static void GPS_update_crosstrack(void)
-{
- if (ABS(wrap_18000(target_bearing - original_target_bearing)) < 4500) { // If we are too far off or too close we don't do track following
- float temp = (target_bearing - original_target_bearing) * RADX100;
- crosstrack_error = sin_approx(temp) * (wp_distance * CROSSTRACK_GAIN); // Meters we are off track line
- nav_bearing = target_bearing + constrain(crosstrack_error, -3000, 3000);
- nav_bearing = wrap_36000(nav_bearing);
- } else {
- nav_bearing = target_bearing;
- }
-}
-
-////////////////////////////////////////////////////////////////////////////////////
-// Determine desired speed when navigating towards a waypoint, also implement slow
-// speed rampup when starting a navigation
-//
-// |< WP Radius
-// 0 1 2 3 4 5 6 7 8m
-// ...|...|...|...|...|...|...|...|
-// 100 | 200 300 400cm/s
-// | +|+
-// |< we should slow to 1.5 m/s as we hit the target
-//
-static uint16_t GPS_calc_desired_speed(uint16_t max_speed, bool _slow)
-{
- // max_speed is default 400 or 4m/s
- if (_slow) {
- max_speed = MIN(max_speed, wp_distance / 2);
- } else {
- max_speed = MIN(max_speed, wp_distance);
- max_speed = MAX(max_speed, navigationConfig()->nav_speed_min); // go at least 100cm/s
- }
-
- // limit the ramp up of the speed
- // waypoint_speed_gov is reset to 0 at each new WP command
- if (max_speed > waypoint_speed_gov) {
- waypoint_speed_gov += (int)(100.0f * dTnav); // increase at .5/ms
- max_speed = waypoint_speed_gov;
- }
- return max_speed;
-}
-
-////////////////////////////////////////////////////////////////////////////////////
-// Utilities
-//
-static int32_t wrap_18000(int32_t error)
-{
- if (error > 18000)
- error -= 36000;
- if (error < -18000)
- error += 36000;
- return error;
-}
-
-static int32_t wrap_36000(int32_t angle)
-{
- if (angle > 36000)
- angle -= 36000;
- if (angle < 0)
- angle += 36000;
- return angle;
-}
-
-void updateGpsStateForHomeAndHoldMode(void)
-{
- float sin_yaw_y = sin_approx(DECIDEGREES_TO_DEGREES(attitude.values.yaw) * 0.0174532925f);
- float cos_yaw_x = cos_approx(DECIDEGREES_TO_DEGREES(attitude.values.yaw) * 0.0174532925f);
- if (navigationConfig()->nav_slew_rate) {
- nav_rated[LON] += constrain(wrap_18000(nav[LON] - nav_rated[LON]), -navigationConfig()->nav_slew_rate, navigationConfig()->nav_slew_rate); // TODO check this on uint8
- nav_rated[LAT] += constrain(wrap_18000(nav[LAT] - nav_rated[LAT]), -navigationConfig()->nav_slew_rate, navigationConfig()->nav_slew_rate);
- GPS_angle[AI_ROLL] = (nav_rated[LON] * cos_yaw_x - nav_rated[LAT] * sin_yaw_y) / 10;
- GPS_angle[AI_PITCH] = (nav_rated[LON] * sin_yaw_y + nav_rated[LAT] * cos_yaw_x) / 10;
- } else {
- GPS_angle[AI_ROLL] = (nav[LON] * cos_yaw_x - nav[LAT] * sin_yaw_y) / 10;
- GPS_angle[AI_PITCH] = (nav[LON] * sin_yaw_y + nav[LAT] * cos_yaw_x) / 10;
- }
-}
-
-void updateGpsWaypointsAndMode(void)
-{
- bool resetNavNow = false;
- static bool gpsReadyBeepDone = false;
-
- if (STATE(GPS_FIX) && gpsSol.numSat >= 5) {
-
- //
- // process HOME mode
- //
- // HOME mode takes priority over HOLD mode.
-
- if (IS_RC_MODE_ACTIVE(BOXGPSHOME)) {
- if (!FLIGHT_MODE(GPS_HOME_MODE)) {
-
- // Transition to HOME mode
- ENABLE_FLIGHT_MODE(GPS_HOME_MODE);
- DISABLE_FLIGHT_MODE(GPS_HOLD_MODE);
- GPS_set_next_wp(&GPS_home[LAT], &GPS_home[LON]);
- nav_mode = NAV_MODE_WP;
- resetNavNow = true;
- }
- } else {
- if (FLIGHT_MODE(GPS_HOME_MODE)) {
-
- // Transition from HOME mode
- DISABLE_FLIGHT_MODE(GPS_HOME_MODE);
- nav_mode = NAV_MODE_NONE;
- resetNavNow = true;
- }
-
- //
- // process HOLD mode
- //
-
- if (IS_RC_MODE_ACTIVE(BOXGPSHOLD) && areSticksInApModePosition(navigationConfig()->ap_mode)) {
- if (!FLIGHT_MODE(GPS_HOLD_MODE)) {
-
- // Transition to HOLD mode
- ENABLE_FLIGHT_MODE(GPS_HOLD_MODE);
- GPS_hold[LAT] = gpsSol.llh.lat;
- GPS_hold[LON] = gpsSol.llh.lon;
- GPS_set_next_wp(&GPS_hold[LAT], &GPS_hold[LON]);
- nav_mode = NAV_MODE_POSHOLD;
- resetNavNow = true;
- }
- } else {
- if (FLIGHT_MODE(GPS_HOLD_MODE)) {
-
- // Transition from HOLD mode
- DISABLE_FLIGHT_MODE(GPS_HOLD_MODE);
- nav_mode = NAV_MODE_NONE;
- resetNavNow = true;
- }
- }
- }
- if (!gpsReadyBeepDone) { //if 'ready' beep not yet done
- beeper(BEEPER_READY_BEEP); //do ready beep now
- gpsReadyBeepDone = true; //only beep once
- }
- } else {
- if (FLIGHT_MODE(GPS_HOLD_MODE | GPS_HOME_MODE)) {
-
- // Transition from HOME or HOLD mode
- DISABLE_FLIGHT_MODE(GPS_HOME_MODE);
- DISABLE_FLIGHT_MODE(GPS_HOLD_MODE);
- nav_mode = NAV_MODE_NONE;
- resetNavNow = true;
- }
- }
-
- if (resetNavNow) {
- GPS_reset_nav();
- }
-}
-
-#endif
diff --git a/src/main/flight/navigation.h b/src/main/flight/navigation.h
deleted file mode 100644
index 14476c7164..0000000000
--- a/src/main/flight/navigation.h
+++ /dev/null
@@ -1,51 +0,0 @@
-/*
- * This file is part of Cleanflight and Betaflight.
- *
- * Cleanflight and Betaflight are free software. You can redistribute
- * this software and/or modify this software under the terms of the
- * GNU General Public License as published by the Free Software
- * Foundation, either version 3 of the License, or (at your option)
- * any later version.
- *
- * Cleanflight and Betaflight are distributed in the hope that they
- * will be useful, but WITHOUT ANY WARRANTY; without even the implied
- * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
- * See the GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this software.
- *
- * If not, see .
- */
-
-#pragma once
-
-#include "common/axis.h"
-#include "io/gps.h"
-
-
-// FIXME ap_mode is badly named, it's a value that is compared to rcCommand, not a flag at it's name implies.
-
-typedef struct navigationConfig_s {
- uint16_t gps_wp_radius; // if we are within this distance to a waypoint then we consider it reached (distance is in cm)
- uint8_t gps_lpf; // Low pass filter cut frequency for derivative calculation (default 20Hz)
- uint8_t nav_slew_rate; // Adds a rate control to nav output, will smoothen out nav angle spikes
- uint8_t nav_controls_heading; // copter faces toward the navigation point, maghold must be enabled for it
- uint16_t nav_speed_min; // cm/sec
- uint16_t nav_speed_max; // cm/sec
- uint16_t ap_mode; // Temporarily Disables GPS_HOLD_MODE to be able to make it possible to adjust the Hold-position when moving the sticks, creating a deadspan for GPS
-} navigationConfig_t;
-
-PG_DECLARE(navigationConfig_t, navigationConfig);
-
-
-extern int32_t GPS_hold[2];
-
-
-void navigationInit(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);
-
diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c
index c78ac6a88e..fbe6c61c63 100644
--- a/src/main/flight/pid.c
+++ b/src/main/flight/pid.c
@@ -422,9 +422,6 @@ static float pidLevel(int axis, const pidProfile_t *pidProfile, const rollAndPit
// calculate error angle and limit the angle to the max inclination
// rcDeflection is in range [-1.0, 1.0]
float angle = pidProfile->levelAngleLimit * getRcDeflection(axis);
-#ifdef USE_GPS
- angle += GPS_angle[axis];
-#endif
angle = constrainf(angle, -pidProfile->levelAngleLimit, pidProfile->levelAngleLimit);
const float errorAngle = angle - ((attitude.raw[axis] - angleTrim->raw[axis]) / 10.0f);
if (FLIGHT_MODE(ANGLE_MODE)) {
diff --git a/src/main/interface/cli.c b/src/main/interface/cli.c
index 45d4cd03af..f60500fc82 100644
--- a/src/main/interface/cli.c
+++ b/src/main/interface/cli.c
@@ -96,7 +96,6 @@ extern uint8_t __config_end;
#include "flight/failsafe.h"
#include "flight/imu.h"
#include "flight/mixer.h"
-#include "flight/navigation.h"
#include "flight/pid.h"
#include "flight/servos.h"
diff --git a/src/main/interface/msp.c b/src/main/interface/msp.c
index 77b06b6a9a..53021236f5 100644
--- a/src/main/interface/msp.c
+++ b/src/main/interface/msp.c
@@ -73,7 +73,6 @@
#include "flight/failsafe.h"
#include "flight/imu.h"
#include "flight/mixer.h"
-#include "flight/navigation.h"
#include "flight/pid.h"
#include "flight/servos.h"
diff --git a/src/main/interface/settings.c b/src/main/interface/settings.c
index 95c45a2b85..7092e6dd55 100644
--- a/src/main/interface/settings.c
+++ b/src/main/interface/settings.c
@@ -49,7 +49,6 @@
#include "flight/failsafe.h"
#include "flight/imu.h"
#include "flight/mixer.h"
-#include "flight/navigation.h"
#include "flight/pid.h"
#include "flight/servos.h"
@@ -683,15 +682,6 @@ const clivalue_t valueTable[] = {
{ "gps_auto_config", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_GPS_CONFIG, offsetof(gpsConfig_t, autoConfig) },
{ "gps_auto_baud", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_GPS_CONFIG, offsetof(gpsConfig_t, autoBaud) },
#endif
-
-// PG_NAVIGATION_CONFIG
-#ifdef USE_NAV
- { "gps_wp_radius", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, 2000 }, PG_NAVIGATION_CONFIG, offsetof(navigationConfig_t, gps_wp_radius) },
- { "nav_controls_heading", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_NAVIGATION_CONFIG, offsetof(navigationConfig_t, nav_controls_heading) },
- { "nav_speed_min", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 10, 2000 }, PG_NAVIGATION_CONFIG, offsetof(navigationConfig_t, nav_speed_min) },
- { "nav_speed_max", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 10, 2000 }, PG_NAVIGATION_CONFIG, offsetof(navigationConfig_t, nav_speed_max) },
- { "nav_slew_rate", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 0, 100 }, PG_NAVIGATION_CONFIG, offsetof(navigationConfig_t, nav_slew_rate) },
-#endif
{ "deadband", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 0, 32 }, PG_RC_CONTROLS_CONFIG, offsetof(rcControlsConfig_t, deadband) },
{ "yaw_deadband", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 0, 100 }, PG_RC_CONTROLS_CONFIG, offsetof(rcControlsConfig_t, yaw_deadband) },
@@ -757,17 +747,6 @@ const clivalue_t valueTable[] = {
{ "horizon_tilt_effect", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 250 }, PG_PID_PROFILE, offsetof(pidProfile_t, horizon_tilt_effect) },
{ "horizon_tilt_expert_mode", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_PID_PROFILE, offsetof(pidProfile_t, horizon_tilt_expert_mode) },
-#ifdef USE_NAV
- { "gps_pos_p", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, pid[PID_POS].P) },
- { "gps_pos_i", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, pid[PID_POS].I) },
- { "gps_pos_d", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, pid[PID_POS].D) },
- { "gps_posr_p", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, pid[PID_POSR].P) },
- { "gps_posr_i", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, pid[PID_POSR].I) },
- { "gps_posr_d", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, pid[PID_POSR].D) },
- { "gps_nav_p", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, pid[PID_NAVR].P) },
- { "gps_nav_i", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, pid[PID_NAVR].I) },
- { "gps_nav_d", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, pid[PID_NAVR].D) },
-#endif
// PG_TELEMETRY_CONFIG
#ifdef USE_TELEMETRY
diff --git a/src/main/io/dashboard.c b/src/main/io/dashboard.c
index 6bacd433b5..3f855799ad 100644
--- a/src/main/io/dashboard.c
+++ b/src/main/io/dashboard.c
@@ -59,7 +59,6 @@
#include "flight/pid.h"
#include "flight/imu.h"
#include "flight/failsafe.h"
-#include "flight/navigation.h"
#include "io/gps.h"
#include "io/dashboard.h"
@@ -427,20 +426,6 @@ static void showGpsPage(void)
padHalfLineBuffer();
i2c_OLED_set_line(bus, rowIndex++);
i2c_OLED_send_string(bus, lineBuffer);
-
-#ifdef GPS_PH_DEBUG
- tfp_sprintf(lineBuffer, "Angles: P:%d R:%d", GPS_angle[PITCH], GPS_angle[ROLL]);
- padLineBuffer();
- i2c_OLED_set_line(bus, rowIndex++);
- i2c_OLED_send_string(bus, lineBuffer);
-#endif
-
-#if 0
- tfp_sprintf(lineBuffer, "%d %d %d %d", debug[0], debug[1], debug[2], debug[3]);
- padLineBuffer();
- i2c_OLED_set_line(bus, rowIndex++);
- i2c_OLED_send_string(bus, lineBuffer);
-#endif
}
#endif
diff --git a/src/main/io/gps.c b/src/main/io/gps.c
index 667c46a71d..93011459f9 100644
--- a/src/main/io/gps.c
+++ b/src/main/io/gps.c
@@ -76,7 +76,6 @@ static char *gpsPacketLogChar = gpsPacketLog;
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;
@@ -1213,9 +1212,6 @@ void GPS_reset_home_position(void)
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);
}
@@ -1329,10 +1325,6 @@ void onGpsNewData(void)
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
diff --git a/src/main/io/gps.h b/src/main/io/gps.h
index f7c8a226b5..8b3cea4151 100644
--- a/src/main/io/gps.h
+++ b/src/main/io/gps.h
@@ -159,6 +159,5 @@ 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);
diff --git a/src/main/io/ledstrip.c b/src/main/io/ledstrip.c
index a826b00dbf..5581b6e179 100644
--- a/src/main/io/ledstrip.c
+++ b/src/main/io/ledstrip.c
@@ -53,7 +53,6 @@
#include "flight/failsafe.h"
#include "flight/imu.h"
#include "flight/mixer.h"
-#include "flight/navigation.h"
#include "flight/pid.h"
#include "flight/servos.h"
diff --git a/src/main/target/COLIBRI_RACE/i2c_bst.c b/src/main/target/COLIBRI_RACE/i2c_bst.c
index 8d9d4a331e..51c1824ee9 100644
--- a/src/main/target/COLIBRI_RACE/i2c_bst.c
+++ b/src/main/target/COLIBRI_RACE/i2c_bst.c
@@ -61,7 +61,6 @@
#include "flight/failsafe.h"
#include "flight/imu.h"
#include "flight/mixer.h"
-#include "flight/navigation.h"
#include "flight/pid.h"
#include "flight/servos.h"
diff --git a/src/main/target/common_fc_pre.h b/src/main/target/common_fc_pre.h
index 6e8c8694bd..7441541bdd 100644
--- a/src/main/target/common_fc_pre.h
+++ b/src/main/target/common_fc_pre.h
@@ -202,7 +202,6 @@
#define USE_GPS
#define USE_GPS_NMEA
#define USE_GPS_UBLOX
-#define USE_NAV
#define USE_OSD_ADJUSTMENTS
#define USE_SENSOR_NAMES
#define USE_SERIALRX_JETIEXBUS
diff --git a/src/main/telemetry/ibus_shared.c b/src/main/telemetry/ibus_shared.c
index 420bc2384c..7f3dc9122c 100644
--- a/src/main/telemetry/ibus_shared.c
+++ b/src/main/telemetry/ibus_shared.c
@@ -55,7 +55,6 @@ static uint16_t calculateChecksum(const uint8_t *ibusPacket);
#include "sensors/barometer.h"
#include "flight/imu.h"
#include "flight/position.h"
-#include "flight/navigation.h"
#include "io/gps.h"
diff --git a/src/main/telemetry/mavlink.c b/src/main/telemetry/mavlink.c
index b07ff2a4a0..6f1722d3e4 100644
--- a/src/main/telemetry/mavlink.c
+++ b/src/main/telemetry/mavlink.c
@@ -51,7 +51,6 @@
#include "flight/pid.h"
#include "flight/imu.h"
#include "flight/failsafe.h"
-#include "flight/navigation.h"
#include "flight/position.h"
#include "io/serial.h"
diff --git a/src/test/unit/pid_unittest.cc b/src/test/unit/pid_unittest.cc
index 4b2bf3f762..a526c39c7d 100644
--- a/src/test/unit/pid_unittest.cc
+++ b/src/test/unit/pid_unittest.cc
@@ -60,7 +60,6 @@ extern "C" {
gyro_t gyro;
attitudeEulerAngles_t attitude;
- int16_t GPS_angle[ANGLE_INDEX_COUNT];
float getThrottlePIDAttenuation(void) { return simulatedThrottlePIDAttenuation; }
float getMotorMixRange(void) { return simulatedMotorMixRange; }