diff --git a/ArduPlane/navigation.pde b/ArduPlane/navigation.pde
index f38bf7de42..4c5f09b7b5 100644
--- a/ArduPlane/navigation.pde
+++ b/ArduPlane/navigation.pde
@@ -62,11 +62,6 @@ static void navigate()
     // ----------------------------
     wp_distance = get_distance(current_loc, next_WP_loc);
 
-    if (wp_distance < 0) {
-        gcs_send_text_P(SEVERITY_HIGH,PSTR("WP error - distance < 0"));
-        return;
-    }
-
     // update total loiter angle
     loiter_angle_update();
 
diff --git a/ArduPlane/setup.pde b/ArduPlane/setup.pde
index c3453a2a9c..97c3349990 100644
--- a/ArduPlane/setup.pde
+++ b/ArduPlane/setup.pde
@@ -6,7 +6,6 @@
 static int8_t   setup_radio                             (uint8_t argc, const Menu::arg *argv);
 static int8_t   setup_show                              (uint8_t argc, const Menu::arg *argv);
 static int8_t   setup_factory                   (uint8_t argc, const Menu::arg *argv);
-static int8_t   setup_flightmodes               (uint8_t argc, const Menu::arg *argv);
 static int8_t   setup_level                             (uint8_t argc, const Menu::arg *argv);
 #if !defined( __AVR_ATmega1280__ )
 static int8_t   setup_accel_scale                       (uint8_t argc, const Menu::arg *argv);
@@ -14,7 +13,6 @@ static int8_t   setup_set                               (uint8_t argc, const Men
 #endif
 static int8_t   setup_erase                             (uint8_t argc, const Menu::arg *argv);
 static int8_t   setup_compass                   (uint8_t argc, const Menu::arg *argv);
-static int8_t   setup_declination               (uint8_t argc, const Menu::arg *argv);
 
 
 // Command/function table for the setup menu
@@ -23,13 +21,11 @@ static const struct Menu::command setup_menu_commands[] PROGMEM = {
     // =======          ===============
     {"reset",                       setup_factory},
     {"radio",                       setup_radio},
-    {"modes",                       setup_flightmodes},
     {"level",                       setup_level},
 #if !defined( __AVR_ATmega1280__ )
     {"accel",                       setup_accel_scale},
 #endif
     {"compass",                     setup_compass},
-    {"declination",         setup_declination},
     {"show",                        setup_show},
 #if !defined( __AVR_ATmega1280__ )
     {"set",                         setup_set},
@@ -265,98 +261,6 @@ setup_radio(uint8_t argc, const Menu::arg *argv)
 }
 
 
-static int8_t
-setup_flightmodes(uint8_t argc, const Menu::arg *argv)
-{
-    uint8_t switchPosition, mode = 0;
-
-    cliSerial->printf_P(PSTR("\nMove RC toggle switch to each position to edit, move aileron stick to select modes."));
-    print_hit_enter();
-    trim_radio();
-
-    while(1) {
-        delay(20);
-        read_radio();
-        switchPosition = readSwitch();
-
-
-        // look for control switch change
-        if (oldSwitchPosition != switchPosition) {
-            // force position 5 to MANUAL
-            if (switchPosition > 4) {
-                flight_modes[switchPosition] = MANUAL;
-            }
-            // update our current mode
-            mode = flight_modes[switchPosition];
-
-            // update the user
-            print_switch(switchPosition, mode);
-
-            // Remember switch position
-            oldSwitchPosition = switchPosition;
-        }
-
-        // look for stick input
-        int16_t radioInputSwitch = radio_input_switch();
-
-        if (radioInputSwitch != 0) {
-
-            mode += radioInputSwitch;
-
-            while (
-                mode != MANUAL &&
-                mode != CIRCLE &&
-                mode != STABILIZE &&
-                mode != TRAINING &&
-                mode != ACRO &&
-                mode != FLY_BY_WIRE_A &&
-                mode != AUTOTUNE &&
-                mode != FLY_BY_WIRE_B &&
-                mode != CRUISE &&
-                mode != AUTO &&
-                mode != RTL &&
-                mode != LOITER)
-            {
-                if (mode < MANUAL)
-                    mode = LOITER;
-                else if (mode >LOITER)
-                    mode = MANUAL;
-                else
-                    mode += radioInputSwitch;
-            }
-
-            // Override position 5
-            if(switchPosition > 4)
-                mode = MANUAL;
-
-            // save new mode
-            flight_modes[switchPosition] = mode;
-
-            // print new mode
-            print_switch(switchPosition, mode);
-        }
-
-        // escape hatch
-        if(cliSerial->available() > 0) {
-            // save changes
-            for (mode=0; mode<6; mode++)
-                flight_modes[mode].save();
-            report_flight_modes();
-            print_done();
-            return (0);
-        }
-    }
-}
-
-static int8_t
-setup_declination(uint8_t argc, const Menu::arg *argv)
-{
-    compass.set_declination(radians(argv[1].f));
-    report_compass();
-    return 0;
-}
-
-
 static int8_t
 setup_erase(uint8_t argc, const Menu::arg *argv)
 {
@@ -490,10 +394,6 @@ static void report_compass()
 
     print_enabled(g.compass_enabled);
 
-    // mag declination
-    cliSerial->printf_P(PSTR("Mag Declination: %4.4f\n"),
-                    degrees(compass.get_declination()));
-
     Vector3f offsets = compass.get_offsets();
 
     // mag offsets
@@ -504,18 +404,6 @@ static void report_compass()
     print_blanks(2);
 }
 
-static void report_flight_modes()
-{
-    //print_blanks(2);
-    cliSerial->printf_P(PSTR("Flight modes\n"));
-    print_divider();
-
-    for(int16_t i = 0; i < 6; i++ ) {
-        print_switch(i, flight_modes[i]);
-    }
-    print_blanks(2);
-}
-
 /***************************************************************************/
 // CLI utilities
 /***************************************************************************/
@@ -534,14 +422,6 @@ print_radio_values()
 
 }
 
-static void
-print_switch(uint8_t p, uint8_t m)
-{
-    cliSerial->printf_P(PSTR("Pos %d: "),p);
-    print_flight_mode(cliSerial, m);
-    cliSerial->println();
-}
-
 static void
 print_done()
 {
@@ -566,33 +446,6 @@ print_divider(void)
     cliSerial->println();
 }
 
-static int8_t
-radio_input_switch(void)
-{
-    static int8_t bouncer = 0;
-
-
-    if (int16_t(channel_roll->radio_in - channel_roll->radio_trim) > 100) {
-        bouncer = 10;
-    }
-    if (int16_t(channel_roll->radio_in - channel_roll->radio_trim) < -100) {
-        bouncer = -10;
-    }
-    if (bouncer >0) {
-        bouncer--;
-    }
-    if (bouncer <0) {
-        bouncer++;
-    }
-
-    if (bouncer == 1 || bouncer == -1) {
-        return bouncer;
-    } else {
-        return 0;
-    }
-}
-
-
 static void zero_eeprom(void)
 {
     uint8_t b = 0;