diff --git a/ArduSub/APM_Config.h b/ArduSub/APM_Config.h
index 3c6b5b5bd3..1e1ec27d3a 100644
--- a/ArduSub/APM_Config.h
+++ b/ArduSub/APM_Config.h
@@ -19,7 +19,7 @@
 #define AUTOTUNE_ENABLED      DISABLED            // disable the auto tune functionality to save 7k of flash
 //#define AC_FENCE              DISABLED            // disable fence to save 2k of flash
 //#define CAMERA                DISABLED            // disable camera trigger to save 1k of flash
-//#define CONFIG_SONAR          DISABLED            // disable sonar to save 1k of flash
+//#define RANGEFINDER_ENABLED   DISABLED            // disable rangefinder to save 1k of flash
 #define POSHOLD_ENABLED       DISABLED            // disable PosHold flight mode to save 4.5k of flash
 #define AC_RALLY              DISABLED            // disable rally points library (must also disable terrain which relies on rally)
 #define AC_TERRAIN            DISABLED            // disable terrain library
diff --git a/ArduSub/ArduSub.cpp b/ArduSub/ArduSub.cpp
index dc2ef39326..6788100605 100644
--- a/ArduSub/ArduSub.cpp
+++ b/ArduSub/ArduSub.cpp
@@ -94,7 +94,8 @@ const AP_Scheduler::Task Sub::scheduler_tasks[] = {
     SCHED_TASK(arm_motors_check,      10,     50),
     SCHED_TASK(auto_disarm_check,     10,     50),
     SCHED_TASK(auto_trim,             10,     75),
-    SCHED_TASK(update_altitude,       25,    140),
+	SCHED_TASK(read_rangefinder,      20,    100),
+	SCHED_TASK(update_altitude,       10,    100),
     SCHED_TASK(run_nav_updates,       50,    100),
     SCHED_TASK(update_thr_average,   100,     90),
     SCHED_TASK(three_hz_loop,          3,     75),
@@ -625,15 +626,12 @@ void Sub::read_AHRS(void)
     ahrs.update();
 }
 
-// read baro and sonar altitude at 10hz
+// read baro and rangefinder altitude at 10hz
 void Sub::update_altitude()
 {
     // read in baro altitude
     read_barometer();
 
-    // read in sonar altitude
-    sonar_alt           = read_sonar();
-
     // write altitude info to dataflash logs
     if (should_log(MASK_LOG_CTUN)) {
         Log_Write_Control_Tuning();
diff --git a/ArduSub/Attitude.cpp b/ArduSub/Attitude.cpp
index 41146be336..7c49b35a60 100644
--- a/ArduSub/Attitude.cpp
+++ b/ArduSub/Attitude.cpp
@@ -245,6 +245,7 @@ float Sub::get_throttle_pre_takeoff(float input_thr)
 //      returns climb rate (in cm/s) which should be passed to the position controller
 float Sub::get_surface_tracking_climb_rate(int16_t target_rate, float current_alt_target, float dt)
 {
+#if RANGEFINDER_ENABLED == ENABLED
     static uint32_t last_call_ms = 0;
     float distance_error;
     float velocity_correction;
@@ -253,27 +254,30 @@ float Sub::get_surface_tracking_climb_rate(int16_t target_rate, float current_al
     uint32_t now = millis();
 
     // reset target altitude if this controller has just been engaged
-    if (now - last_call_ms > SONAR_TIMEOUT_MS) {
-        target_sonar_alt = sonar_alt + current_alt_target - current_alt;
+    if (now - last_call_ms > RANGEFINDER_TIMEOUT_MS) {
+    	target_rangefinder_alt = rangefinder_state.alt_cm + current_alt_target - current_alt;
     }
     last_call_ms = now;
 
-    // adjust sonar target alt if motors have not hit their limits
+    // adjust rangefinder target alt if motors have not hit their limits
     if ((target_rate<0 && !motors.limit.throttle_lower) || (target_rate>0 && !motors.limit.throttle_upper)) {
-        target_sonar_alt += target_rate * dt;
+        target_rangefinder_alt += target_rate * dt;
     }
 
     // do not let target altitude get too far from current altitude above ground
     // Note: the 750cm limit is perhaps too wide but is consistent with the regular althold limits and helps ensure a smooth transition
-    target_sonar_alt = constrain_float(target_sonar_alt,sonar_alt-pos_control.get_leash_down_z(),sonar_alt+pos_control.get_leash_up_z());
+    target_rangefinder_alt = constrain_float(target_rangefinder_alt,rangefinder_state.alt_cm-pos_control.get_leash_down_z(),rangefinder_state.alt_cm+pos_control.get_leash_up_z());
 
-    // calc desired velocity correction from target sonar alt vs actual sonar alt (remove the error already passed to Altitude controller to avoid oscillations)
-    distance_error = (target_sonar_alt - sonar_alt) - (current_alt_target - current_alt);
-    velocity_correction = distance_error * g.sonar_gain;
+    // calc desired velocity correction from target rangefinder alt vs actual rangefinder alt (remove the error already passed to Altitude controller to avoid oscillations)
+    distance_error = (target_rangefinder_alt - rangefinder_state.alt_cm) - (current_alt_target - current_alt);
+    velocity_correction = distance_error * g.rangefinder_gain;
     velocity_correction = constrain_float(velocity_correction, -THR_SURFACE_TRACKING_VELZ_MAX, THR_SURFACE_TRACKING_VELZ_MAX);
 
-    // return combined pilot climb rate + rate to correct sonar alt error
+    // return combined pilot climb rate + rate to correct rangefinder alt error
     return (target_rate + velocity_correction);
+#else
+    return (float)target_rate;
+#endif
 }
 
 // set_accel_throttle_I_from_pilot_throttle - smoothes transition from pilot controlled throttle to autopilot throttle
diff --git a/ArduSub/Log.cpp b/ArduSub/Log.cpp
index d339005c9e..fd8e755d6f 100644
--- a/ArduSub/Log.cpp
+++ b/ArduSub/Log.cpp
@@ -304,8 +304,8 @@ struct PACKED log_Control_Tuning {
     float    desired_alt;
     float    inav_alt;
     int32_t  baro_alt;
-    int16_t  desired_sonar_alt;
-    int16_t  sonar_alt;
+    int16_t  desired_rangefinder_alt;
+    int16_t  rangefinder_alt;
     float    terr_alt;
     int16_t  desired_climb_rate;
     int16_t  climb_rate;
@@ -329,8 +329,8 @@ void Sub::Log_Write_Control_Tuning()
         desired_alt         : pos_control.get_alt_target() / 100.0f,
         inav_alt            : inertial_nav.get_altitude() / 100.0f,
         baro_alt            : baro_alt,
-        desired_sonar_alt   : (int16_t)target_sonar_alt,
-        sonar_alt           : sonar_alt,
+        desired_rangefinder_alt   : (int16_t)target_rangefinder_alt,
+        rangefinder_alt           : rangefinder_state.alt_cm,
 		terr_alt			: terr_alt,
         desired_climb_rate  : (int16_t)pos_control.get_vel_target_z(),
         climb_rate          : climb_rate
diff --git a/ArduSub/Parameters.cpp b/ArduSub/Parameters.cpp
index 9bc3ead61f..257aebb6d6 100644
--- a/ArduSub/Parameters.cpp
+++ b/ArduSub/Parameters.cpp
@@ -163,7 +163,7 @@ const AP_Param::Info Sub::var_info[] = {
     // @Range: 0.01 2.0
     // @Increment: 0.01
     // @User: Standard
-    GSCALAR(sonar_gain,     "RNGFND_GAIN",           SONAR_GAIN_DEFAULT),
+    GSCALAR(rangefinder_gain,     "RNGFND_GAIN",           RANGEFINDER_GAIN_DEFAULT),
 
     // @Param: FS_BATT_ENABLE
     // @DisplayName: Battery Failsafe Enable
@@ -465,8 +465,8 @@ const AP_Param::Info Sub::var_info[] = {
     // @Param: ARMING_CHECK
     // @DisplayName: Arming check
     // @Description: Allows enabling or disabling of pre-arming checks of receiver, accelerometer, barometer, compass and GPS
-    // @Values: 0:Disabled, 1:Enabled, -3:Skip Baro, -5:Skip Compass, -9:Skip GPS, -17:Skip INS, -33:Skip Params/Sonar, -65:Skip RC, 127:Skip Voltage
-    // @Bitmask: 0:All,1:Baro,2:Compass,3:GPS,4:INS,5:Parameters+Sonar,6:RC,7:Voltage
+    // @Values: 0:Disabled, 1:Enabled, -3:Skip Baro, -5:Skip Compass, -9:Skip GPS, -17:Skip INS, -33:Skip Params/Rangefinder, -65:Skip RC, 127:Skip Voltage
+    // @Bitmask: 0:All,1:Baro,2:Compass,3:GPS,4:INS,5:Parameters+Rangefinder,6:RC,7:Voltage
     // @User: Standard
     GSCALAR(arming_check, "ARMING_CHECK",           ARMING_CHECK_ALL),
 
@@ -974,10 +974,10 @@ const AP_Param::Info Sub::var_info[] = {
     // @Path: ../libraries/AP_RSSI/AP_RSSI.cpp
     GOBJECT(rssi, "RSSI_",  AP_RSSI),      
     
-#if CONFIG_SONAR == ENABLED
+#if RANGEFINDER_ENABLED == ENABLED
     // @Group: RNGFND
     // @Path: ../libraries/AP_RangeFinder/RangeFinder.cpp
-    GOBJECT(sonar,   "RNGFND", RangeFinder),
+    GOBJECT(rangefinder,   "RNGFND", RangeFinder),
 #endif
 
 #if AP_TERRAIN_AVAILABLE && AC_TERRAIN
diff --git a/ArduSub/Parameters.h b/ArduSub/Parameters.h
index 85eb8150a7..43f5eae63f 100644
--- a/ArduSub/Parameters.h
+++ b/ArduSub/Parameters.h
@@ -99,7 +99,7 @@ public:
         k_param_acro_trainer,
         k_param_pilot_velocity_z_max,
         k_param_circle_rate,                // deprecated - remove
-        k_param_sonar_gain,
+        k_param_rangefinder_gain,
         k_param_ch8_option,
         k_param_arming_check,
         k_param_sprayer,
@@ -122,7 +122,7 @@ public:
         k_param_serial1_baud,           // deprecated - remove
         k_param_serial2_baud,           // deprecated - remove
         k_param_land_repositioning,
-        k_param_sonar, // sonar object
+        k_param_rangefinder, // rangefinder object
         k_param_fs_ekf_thresh,
         k_param_terrain,
         k_param_acro_expo,
@@ -232,13 +232,13 @@ public:
         k_param_pack_capacity,  // deprecated - can be deleted
         k_param_compass_enabled,
         k_param_compass,
-        k_param_sonar_enabled_old, // deprecated
+        k_param_rangefinder_enabled_old, // deprecated
         k_param_frame_orientation,
         k_param_optflow_enabled,    // deprecated
         k_param_fs_batt_voltage,
         k_param_ch7_option,
         k_param_auto_slew_rate,     // deprecated - can be deleted
-        k_param_sonar_type_old,     // deprecated
+        k_param_rangefinder_type_old,     // deprecated
         k_param_super_simple = 155,
         k_param_axis_enabled = 157, // deprecated - remove with next eeprom number change
         k_param_copter_leds_mode,   // deprecated - remove with next eeprom number change
@@ -406,7 +406,7 @@ public:
     AP_Int16        rtl_altitude;
     AP_Int16        rtl_speed_cms;
     AP_Float        rtl_cone_slope;
-    AP_Float        sonar_gain;
+    AP_Float        rangefinder_gain;
 
     AP_Int8         failsafe_battery_enabled;   // battery failsafe enabled
     AP_Float        fs_batt_voltage;            // battery voltage below which failsafe will be triggered
diff --git a/ArduSub/Sub.cpp b/ArduSub/Sub.cpp
index 3ebc69850b..b1cce068ce 100644
--- a/ArduSub/Sub.cpp
+++ b/ArduSub/Sub.cpp
@@ -23,7 +23,6 @@ const AP_HAL::HAL& hal = AP_HAL::get_HAL();
 
 Sub::Sub(void) :
     flight_modes(&g.flight_mode1),
-    sonar_enabled(true),
     mission(ahrs, 
             FUNCTOR_BIND_MEMBER(&Sub::start_command, bool, const AP_Mission::Mission_Command &),
             FUNCTOR_BIND_MEMBER(&Sub::verify_command_callback, bool, const AP_Mission::Mission_Command &),
@@ -54,9 +53,7 @@ Sub::Sub(void) :
     frsky_telemetry(ahrs, battery),
 #endif
     climb_rate(0),
-    sonar_alt(0),
-    sonar_alt_health(0),
-    target_sonar_alt(0.0f),
+    target_rangefinder_alt(0.0f),
     baro_alt(0),
     baro_climbrate(0.0f),
     land_accel_ef_filter(LAND_DETECTOR_ACCEL_LPF_CUTOFF),
diff --git a/ArduSub/Sub.h b/ArduSub/Sub.h
index d622b207db..5c3ef1dcdf 100644
--- a/ArduSub/Sub.h
+++ b/ArduSub/Sub.h
@@ -173,17 +173,21 @@ private:
     Compass compass;
     AP_InertialSensor ins;
 
-#if CONFIG_SONAR == ENABLED
-    RangeFinder sonar {serial_manager};
-    bool sonar_enabled; // enable user switch for sonar
-#endif
+    RangeFinder rangefinder {serial_manager};
+    struct {
+        bool enabled:1;
+        bool alt_healthy:1; // true if we can trust the altitude from the rangefinder
+        int16_t alt_cm;     // tilt compensated altitude (in cm) from rangefinder
+        uint32_t last_healthy_ms;
+        LowPassFilterFloat alt_cm_filt; // altitude filter
+    } rangefinder_state = { false, false, 0, 0 };
 
     AP_RPM rpm_sensor;
 
     // Inertial Navigation EKF
-    NavEKF EKF{&ahrs, barometer, sonar};
-    NavEKF2 EKF2{&ahrs, barometer, sonar};
-    AP_AHRS_NavEKF ahrs{ins, barometer, gps, sonar, EKF, EKF2, AP_AHRS_NavEKF::FLAG_ALWAYS_USE_EKF};
+    NavEKF EKF{&ahrs, barometer, rangefinder};
+    NavEKF2 EKF2{&ahrs, barometer, rangefinder};
+    AP_AHRS_NavEKF ahrs{ins, barometer, gps, rangefinder, EKF, EKF2, AP_AHRS_NavEKF::FLAG_ALWAYS_USE_EKF};
 
 #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
     SITL::SITL sitl;
@@ -395,6 +399,10 @@ private:
     uint32_t brake_timeout_start;
     uint32_t brake_timeout_ms;
 
+    // Delay the next navigation command
+    int32_t nav_delay_time_max;  // used for delaying the navigation commands (eg land,takeoff etc.)
+    uint32_t nav_delay_time_start;
+
     // Flip
     Vector3f flip_orig_attitude;         // original Sub attitude before flip
 
@@ -415,10 +423,7 @@ private:
     // Altitude
     // The cm/s we are moving up or down based on filtered data - Positive = UP
     int16_t climb_rate;
-    // The altitude as reported by Sonar in cm - Values are 20 to 700 generally.
-    int16_t sonar_alt;
-    uint8_t sonar_alt_health;    // true if we can trust the altitude from the sonar
-    float target_sonar_alt;      // desired altitude in cm above the ground
+    float target_rangefinder_alt;      // desired altitude in cm above the ground
     int32_t baro_alt;            // barometer altitude in cm above home
     float baro_climbrate;        // barometer climbrate in cm/s
     LowPassFilterVector3f land_accel_ef_filter; // accelerations for land and crash detector tests
@@ -766,7 +771,7 @@ private:
     void guided_vel_control_start();
     void guided_posvel_control_start();
     void guided_angle_control_start();
-    void guided_set_destination(const Vector3f& destination);
+    bool guided_set_destination(const Vector3f& destination);
     bool guided_set_destination(const Location_Class& dest_loc);
     void guided_set_velocity(const Vector3f& velocity);
     void guided_set_destination_posvel(const Vector3f& destination, const Vector3f& velocity);
@@ -892,7 +897,7 @@ private:
     void pre_arm_rc_checks();
     bool pre_arm_gps_checks(bool display_failure);
     bool pre_arm_ekf_attitude_check();
-    bool pre_arm_terrain_check();
+    bool pre_arm_terrain_check(bool display_failure);
     bool arm_checks(bool display_failure, bool arming_from_gcs);
     void init_disarm_motors();
     void motors_output();
@@ -918,6 +923,7 @@ private:
     float pv_alt_above_home(float alt_above_origin_cm);
     float pv_get_bearing_cd(const Vector3f &origin, const Vector3f &destination);
     float pv_get_horizontal_distance_cm(const Vector3f &origin, const Vector3f &destination);
+    float pv_distance_to_home_cm(const Vector3f &destination);
     void default_dead_zones();
     void init_rc_in();
     void init_rc_out();
@@ -931,8 +937,9 @@ private:
     void radio_passthrough_to_motors();
     void init_barometer(bool full_calibration);
     void read_barometer(void);
-    void init_sonar(void);
-    int16_t read_sonar(void);
+    void init_rangefinder(void);
+    void read_rangefinder(void);
+    bool rangefinder_alt_ok(void);
     void init_compass();
     void init_optflow();
     void update_optical_flow(void);
@@ -1004,6 +1011,7 @@ private:
     void do_nav_guided_enable(const AP_Mission::Mission_Command& cmd);
     void do_guided_limits(const AP_Mission::Mission_Command& cmd);
 #endif
+    void do_nav_delay(const AP_Mission::Mission_Command& cmd);
     void do_wait_delay(const AP_Mission::Mission_Command& cmd);
     void do_within_distance(const AP_Mission::Mission_Command& cmd);
     void do_yaw(const AP_Mission::Mission_Command& cmd);
@@ -1027,6 +1035,8 @@ private:
 #if NAV_GUIDED == ENABLED
     bool verify_nav_guided_enable(const AP_Mission::Mission_Command& cmd);
 #endif
+    bool verify_nav_delay(const AP_Mission::Mission_Command& cmd);
+
     void auto_spline_start(const Location_Class& destination, bool stopped_at_start, AC_WPNav::spline_segment_end_type seg_end_type, const Location_Class& next_destination);
     void print_flight_mode(AP_HAL::BetterStream *port, uint8_t mode);
     void log_init(void);
@@ -1058,7 +1068,7 @@ public:
     int8_t test_optflow(uint8_t argc, const Menu::arg *argv);
     int8_t test_relay(uint8_t argc, const Menu::arg *argv);
     int8_t test_shell(uint8_t argc, const Menu::arg *argv);
-    int8_t test_sonar(uint8_t argc, const Menu::arg *argv);
+    int8_t test_rangefinder(uint8_t argc, const Menu::arg *argv);
 
     int8_t reboot_board(uint8_t argc, const Menu::arg *argv);
 };
diff --git a/ArduSub/arming_checks.cpp b/ArduSub/arming_checks.cpp
index d2c2dae035..cd02b02bd9 100644
--- a/ArduSub/arming_checks.cpp
+++ b/ArduSub/arming_checks.cpp
@@ -309,9 +309,9 @@ bool Sub::pre_arm_checks(bool display_failure)
             return false;
         }
 
-        #if CONFIG_SONAR == ENABLED && OPTFLOW == ENABLED
+        #if RANGEFINDER_ENABLED == ENABLED && OPTFLOW == ENABLED
         // check range finder if optflow enabled
-        if (optflow.enabled() && !sonar.pre_arm_check()) {
+        if (optflow.enabled() && !rangefinder.pre_arm_check()) {
             if (display_failure) {
                 gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: check range finder");
             }
@@ -320,8 +320,7 @@ bool Sub::pre_arm_checks(bool display_failure)
         #endif
 
         // check for missing terrain data
-		if (!pre_arm_terrain_check()) {
-			gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: Waiting for Terrain data");
+		if (!pre_arm_terrain_check(display_failure)) {
 			return false;
 		}
     }
@@ -475,7 +474,7 @@ bool Sub::pre_arm_ekf_attitude_check()
 }
 
 // check we have required terrain data
-bool Sub::pre_arm_terrain_check()
+bool Sub::pre_arm_terrain_check(bool display_failure)
 {
 #if AP_TERRAIN_AVAILABLE && AC_TERRAIN
     // succeed if not using terrain data
@@ -483,10 +482,21 @@ bool Sub::pre_arm_terrain_check()
         return true;
     }
 
+    // check if terrain following is enabled, using a range finder but RTL_ALT is higher than rangefinder's max range
+    // To-Do: modify RTL return path to fly at or above the RTL_ALT and remove this check
+    if ((rangefinder.num_sensors() > 0) && (g.rtl_altitude > rangefinder.max_distance_cm())) {
+        gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: RTL_ALT above rangefinder max range");
+        return false;
+    }
+
     // show terrain statistics
     uint16_t terr_pending, terr_loaded;
     terrain.get_statistics(terr_pending, terr_loaded);
-    return (terr_pending <= 0);
+    bool have_all_data = (terr_pending <= 0);
+    if (!have_all_data && display_failure) {
+        gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: Waiting for Terrain data");
+    }
+    return have_all_data;
 #else
     return true;
 #endif
@@ -637,8 +647,7 @@ bool Sub::arm_checks(bool display_failure, bool arming_from_gcs)
 
     // check for missing terrain data
 	if ((g.arming_check == ARMING_CHECK_ALL) || (g.arming_check & ARMING_CHECK_PARAMETERS)) {
-		if (!pre_arm_terrain_check()) {
-			gcs_send_text(MAV_SEVERITY_CRITICAL,"Arm: Waiting for Terrain data");
+		if (!pre_arm_terrain_check(display_failure)) {
 			return false;
 		}
 	}
diff --git a/ArduSub/commands_logic.cpp b/ArduSub/commands_logic.cpp
index 3bafc831b5..571b1e5aed 100644
--- a/ArduSub/commands_logic.cpp
+++ b/ArduSub/commands_logic.cpp
@@ -53,6 +53,10 @@ bool Sub::start_command(const AP_Mission::Mission_Command& cmd)
         break;
 #endif
 
+    case MAV_CMD_NAV_DELAY:                    // 94 Delay the next navigation command
+        do_nav_delay(cmd);
+        break;
+
     //
     // conditional commands
     //
@@ -212,6 +216,9 @@ bool Sub::verify_command(const AP_Mission::Mission_Command& cmd)
         return verify_nav_guided_enable(cmd);
 #endif
 
+    case MAV_CMD_NAV_DELAY:
+        return verify_nav_delay(cmd);
+
     ///
     /// conditional commands
     ///
@@ -515,6 +522,21 @@ void Sub::do_nav_guided_enable(const AP_Mission::Mission_Command& cmd)
 }
 #endif  // NAV_GUIDED
 
+// do_nav_delay - Delay the next navigation command
+void Sub::do_nav_delay(const AP_Mission::Mission_Command& cmd)
+{
+    nav_delay_time_start = millis();
+
+    if (cmd.content.nav_delay.seconds > 0) {
+        // relative delay
+        nav_delay_time_max = cmd.content.nav_delay.seconds * 1000; // convert seconds to milliseconds
+    } else {
+        // absolute delay to utc time
+        nav_delay_time_max = hal.util->get_time_utc(cmd.content.nav_delay.hour_utc, cmd.content.nav_delay.min_utc, cmd.content.nav_delay.sec_utc, 0);
+    }
+    gcs_send_text_fmt(MAV_SEVERITY_INFO, "Delaying %u sec",(unsigned int)(nav_delay_time_max/1000));
+}
+
 
 #if PARACHUTE == ENABLED
 // do_parachute - configure or release parachute
@@ -739,6 +761,15 @@ bool Sub::verify_nav_guided_enable(const AP_Mission::Mission_Command& cmd)
 }
 #endif  // NAV_GUIDED
 
+// verify_nav_delay - check if we have waited long enough
+bool Sub::verify_nav_delay(const AP_Mission::Mission_Command& cmd)
+{
+    if (millis() - nav_delay_time_start > (uint32_t)MAX(nav_delay_time_max,0)) {
+        nav_delay_time_max = 0;
+       return true;
+    }
+    return false;
+}
 
 /********************************************************************************/
 //	Condition (May) commands
diff --git a/ArduSub/config.h b/ArduSub/config.h
index a9b7cf8bff..bfd85e5cac 100644
--- a/ArduSub/config.h
+++ b/ArduSub/config.h
@@ -47,11 +47,6 @@
  #define HIL_MODE        HIL_MODE_DISABLED
 #endif
 
-#if HIL_MODE != HIL_MODE_DISABLED       // we are in HIL mode
- #undef CONFIG_SONAR
- #define CONFIG_SONAR DISABLED
-#endif
-
 #define MAGNETOMETER ENABLED
 
 // run at 400Hz on all systems
@@ -90,35 +85,35 @@
 #endif
 
 //////////////////////////////////////////////////////////////////////////////
-// Sonar
+// Rangefinder
 //
 
-#ifndef CONFIG_SONAR
- # define CONFIG_SONAR ENABLED
+#ifndef RANGEFINDER_ENABLED
+ # define RANGEFINDER_ENABLED ENABLED
 #endif
 
-#ifndef SONAR_ALT_HEALTH_MAX
- # define SONAR_ALT_HEALTH_MAX 3            // number of good reads that indicates a healthy sonar
+#ifndef RANGEFINDER_HEALTH_MAX
+ # define RANGEFINDER_HEALTH_MAX 3          // number of good reads that indicates a healthy rangefinder
 #endif
 
-#ifndef SONAR_RELIABLE_DISTANCE_PCT
- # define SONAR_RELIABLE_DISTANCE_PCT 0.60f // we trust the sonar out to 60% of it's maximum range
-#endif
-
-#ifndef SONAR_GAIN_DEFAULT
- # define SONAR_GAIN_DEFAULT 0.8f           // gain for controlling how quickly sonar range adjusts target altitude (lower means slower reaction)
+#ifndef RANGEFINDER_GAIN_DEFAULT
+ # define RANGEFINDER_GAIN_DEFAULT 0.8f     // gain for controlling how quickly rangefinder range adjusts target altitude (lower means slower reaction)
 #endif
 
 #ifndef THR_SURFACE_TRACKING_VELZ_MAX
- # define THR_SURFACE_TRACKING_VELZ_MAX 150 // max vertical speed change while surface tracking with sonar
+ # define THR_SURFACE_TRACKING_VELZ_MAX 150 // max vertical speed change while surface tracking with rangefinder
 #endif
 
-#ifndef SONAR_TIMEOUT_MS
- # define SONAR_TIMEOUT_MS  1000            // desired sonar alt will reset to current sonar alt after this many milliseconds without a good sonar alt
+#ifndef RANGEFINDER_TIMEOUT_MS
+ # define RANGEFINDER_TIMEOUT_MS  1000      // desired rangefinder alt will reset to current rangefinder alt after this many milliseconds without a good rangefinder alt
 #endif
 
-#ifndef SONAR_TILT_CORRECTION               // by disable tilt correction for use of range finder data by EKF
- # define SONAR_TILT_CORRECTION DISABLED
+#ifndef RANGEFINDER_WPNAV_FILT_HZ
+ # define RANGEFINDER_WPNAV_FILT_HZ   0.25f // filter frequency for rangefinder altitude provided to waypoint navigation class
+#endif
+
+#ifndef RANGEFINDER_TILT_CORRECTION         // by disable tilt correction for use of range finder data by EKF
+ # define RANGEFINDER_TILT_CORRECTION ENABLED
 #endif
 
 
diff --git a/ArduSub/control_althold.cpp b/ArduSub/control_althold.cpp
index 57766e9c89..556f91b1bb 100644
--- a/ArduSub/control_althold.cpp
+++ b/ArduSub/control_althold.cpp
@@ -128,9 +128,9 @@ void Sub::althold_run()
         // call attitude controller
         attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw_smooth(target_roll, target_pitch, target_yaw_rate, get_smoothing_gain());
 
-        // call throttle controller
-        if (sonar_enabled && (sonar_alt_health >= SONAR_ALT_HEALTH_MAX)) {
-            // if sonar is ok, use surface tracking
+        // adjust climb rate using rangefinder
+        if (rangefinder_alt_ok()) {
+            // if rangefinder is ok, use surface tracking
             target_climb_rate = get_surface_tracking_climb_rate(target_climb_rate, pos_control.get_alt_target(), G_Dt);
         }
 
diff --git a/ArduSub/control_auto.cpp b/ArduSub/control_auto.cpp
index 731342927d..e7926a3f20 100644
--- a/ArduSub/control_auto.cpp
+++ b/ArduSub/control_auto.cpp
@@ -25,6 +25,12 @@ bool Sub::auto_init(bool ignore_checks)
     if ((position_ok() && mission.num_commands() > 1) || ignore_checks) {
         auto_mode = Auto_Loiter;
 
+        // reject switching to auto mode if landed with motors armed but first command is not a takeoff (reduce change of flips)
+        if (motors.armed() && ap.land_complete && !mission.starts_with_takeoff_cmd()) {
+            gcs_send_text(MAV_SEVERITY_CRITICAL, "Auto: Missing Takeoff Cmd");
+            return false;
+        }
+
         // stop ROI from carrying over from previous runs of the mission
         // To-Do: reset the yaw as part of auto_wp_start when the previous command was not a wp command to remove the need for this special ROI check
         if (auto_yaw_mode == AUTO_YAW_ROI) {
@@ -195,6 +201,7 @@ void Sub::auto_wp_start(const Location_Class& dest_loc)
 	if (!wp_nav.set_wp_destination(dest_loc)) {
 		// failure to set destination can only be because of missing terrain data
 		failsafe_terrain_on_event();
+		return;
 	}
 
     // initialise yaw
@@ -261,9 +268,9 @@ void Sub::auto_spline_start(const Location_Class& destination, bool stopped_at_s
 
     // initialise wpnav
     if (!wp_nav.set_spline_destination(destination, stopped_at_start, seg_end_type, next_destination)) {
-        // failure to set destination (likely because of missing terrain data)
-        Log_Write_Error(ERROR_SUBSYSTEM_NAVIGATION, ERROR_CODE_FAILED_TO_SET_DESTINATION);
-        // To-Do: handle failure
+    	// failure to set destination can only be because of missing terrain data
+        failsafe_terrain_on_event();
+        return;
     }
 
     // initialise yaw
diff --git a/ArduSub/control_circle.cpp b/ArduSub/control_circle.cpp
index 9268b4921b..0626444b8b 100644
--- a/ArduSub/control_circle.cpp
+++ b/ArduSub/control_circle.cpp
@@ -85,9 +85,9 @@ void Sub::circle_run()
         attitude_control.input_euler_angle_roll_pitch_yaw(circle_nav.get_roll(), circle_nav.get_pitch(), circle_nav.get_yaw(),true);
     }
 
-    // run altitude controller
-    if (sonar_enabled && (sonar_alt_health >= SONAR_ALT_HEALTH_MAX)) {
-        // if sonar is ok, use surface tracking
+    // adjust climb rate using rangefinder
+    if (rangefinder_alt_ok()) {
+        // if rangefinder is ok, use surface tracking
         target_climb_rate = get_surface_tracking_climb_rate(target_climb_rate, pos_control.get_alt_target(), G_Dt);
     }
     // update altitude target and call position controller
diff --git a/ArduSub/control_guided.cpp b/ArduSub/control_guided.cpp
index ddb482f460..b413c8714c 100644
--- a/ArduSub/control_guided.cpp
+++ b/ArduSub/control_guided.cpp
@@ -166,22 +166,35 @@ void Sub::guided_angle_control_start()
 }
 
 // guided_set_destination - sets guided mode's target destination
-void Sub::guided_set_destination(const Vector3f& destination)
+// Returns true if the fence is enabled and guided waypoint is within the fence
+// else return false if the waypoint is outside the fence
+bool Sub::guided_set_destination(const Vector3f& destination)
 {
     // ensure we are in position control mode
     if (guided_mode != Guided_WP) {
         guided_pos_control_start();
     }
 
+#if AC_FENCE == ENABLED
+    // reject destination if outside the fence
+    if (!fence.check_destination_within_fence(pv_alt_above_home(destination.z)*0.01f, pv_distance_to_home_cm(destination)*0.01f)) {
+        Log_Write_Error(ERROR_SUBSYSTEM_NAVIGATION, ERROR_CODE_DEST_OUTSIDE_FENCE);
+        // failure is propagated to GCS with NAK
+        return false;
+    }
+#endif
+
     // no need to check return status because terrain data is not used
     wp_nav.set_wp_destination(destination, false);
 
     // log target
     Log_Write_GuidedTarget(guided_mode, destination, Vector3f());
+    return true;
 }
 
 // sets guided mode's target from a Location object
 // returns false if destination could not be set (probably caused by missing terrain data)
+// or if the fence is enabled and guided waypoint is outside the fence
 bool Sub::guided_set_destination(const Location_Class& dest_loc)
 {
     // ensure we are in position control mode
@@ -189,6 +202,19 @@ bool Sub::guided_set_destination(const Location_Class& dest_loc)
         guided_pos_control_start();
     }
 
+#if AC_FENCE == ENABLED
+    // reject destination outside the fence.
+    // Note: there is a danger that a target specified as a terrain altitude might not be checked if the conversion to alt-above-home fails
+    int32_t alt_target_cm;
+    if (dest_loc.get_alt_cm(Location_Class::ALT_FRAME_ABOVE_HOME, alt_target_cm)) {
+        if (!fence.check_destination_within_fence(alt_target_cm*0.01f, get_distance_cm(ahrs.get_home(), dest_loc)*0.01f)) {
+            Log_Write_Error(ERROR_SUBSYSTEM_NAVIGATION, ERROR_CODE_DEST_OUTSIDE_FENCE);
+            // failure is propagated to GCS with NAK
+            return false;
+        }
+    }
+#endif
+
     if (!wp_nav.set_wp_destination(dest_loc)) {
         // failure to set destination can only be because of missing terrain data
         Log_Write_Error(ERROR_SUBSYSTEM_NAVIGATION, ERROR_CODE_FAILED_TO_SET_DESTINATION);
diff --git a/ArduSub/control_land.cpp b/ArduSub/control_land.cpp
index 59c03a9c73..904570a2ea 100644
--- a/ArduSub/control_land.cpp
+++ b/ArduSub/control_land.cpp
@@ -216,10 +216,10 @@ void Sub::land_nogps_run()
 //      should be called at 100hz or higher
 float Sub::get_land_descent_speed()
 {
-#if CONFIG_SONAR == ENABLED
-    bool sonar_ok = sonar_enabled && (sonar.status() == RangeFinder::RangeFinder_Good);
+#if RANGEFINDER_ENABLED == ENABLED
+	bool rangefinder_ok = rangefinder_state.enabled && rangefinder_state.alt_healthy;
 #else
-    bool sonar_ok = false;
+    bool rangefinder_ok = false;
 #endif
 
     // get position controller's target altitude above terrain
@@ -234,8 +234,8 @@ float Sub::get_land_descent_speed()
 		Log_Write_Error(ERROR_SUBSYSTEM_TERRAIN, ERROR_CODE_MISSING_TERRAIN_DATA);
 	}
 
-    // if we are above 10m and the sonar does not sense anything perform regular alt hold descent
-    if (target_alt_cm <= LAND_START_ALT && !(sonar_ok && sonar_alt_health >= SONAR_ALT_HEALTH_MAX)) {
+	// if we are above 10m and the rangefinder does not sense anything perform regular alt hold descent
+	if ((target_alt_cm >= LAND_START_ALT) && !rangefinder_ok) {
         if (g.land_speed_high > 0) {
             // user has asked for a different landing speed than normal descent rate
             return -abs(g.land_speed_high);
diff --git a/ArduSub/control_loiter.cpp b/ArduSub/control_loiter.cpp
index f0b0362f91..5c0b2808b0 100644
--- a/ArduSub/control_loiter.cpp
+++ b/ArduSub/control_loiter.cpp
@@ -150,9 +150,9 @@ void Sub::loiter_run()
         // call attitude controller
         attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate);
 
-        // run altitude controller
-        if (sonar_enabled && (sonar_alt_health >= SONAR_ALT_HEALTH_MAX)) {
-            // if sonar is ok, use surface tracking
+        // adjust climb rate using rangefinder
+        if (rangefinder_alt_ok()) {
+            // if rangefinder is ok, use surface tracking
             target_climb_rate = get_surface_tracking_climb_rate(target_climb_rate, pos_control.get_alt_target(), G_Dt);
         }
 
diff --git a/ArduSub/control_poshold.cpp b/ArduSub/control_poshold.cpp
index 5184c59443..ddad37f107 100644
--- a/ArduSub/control_poshold.cpp
+++ b/ArduSub/control_poshold.cpp
@@ -519,9 +519,9 @@ void Copter::poshold_run()
         // update attitude controller targets
         attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(poshold.roll, poshold.pitch, target_yaw_rate);
 
-        // throttle control
-        if (sonar_enabled && (sonar_alt_health >= SONAR_ALT_HEALTH_MAX)) {
-            // if sonar is ok, use surface tracking
+        // adjust climb rate using rangefinder
+        if (rangefinder_alt_ok()) {
+            // if rangefinder is ok, use surface tracking
             target_climb_rate = get_surface_tracking_climb_rate(target_climb_rate, pos_control.get_alt_target(), G_Dt);
         }
         // update altitude target and call position controller
diff --git a/ArduSub/control_rtl.cpp b/ArduSub/control_rtl.cpp
index 7871937446..d91b8c2569 100644
--- a/ArduSub/control_rtl.cpp
+++ b/ArduSub/control_rtl.cpp
@@ -13,7 +13,7 @@
 bool Sub::rtl_init(bool ignore_checks)
 {
     if (position_ok() || ignore_checks) {
-    	rtl_build_path(true);
+    	rtl_build_path(!failsafe.terrain);
         rtl_climb_start();
         return true;
     }else{
diff --git a/ArduSub/control_sport.cpp b/ArduSub/control_sport.cpp
index 60ab7b3fc9..49014cbb13 100644
--- a/ArduSub/control_sport.cpp
+++ b/ArduSub/control_sport.cpp
@@ -107,9 +107,9 @@ void Sub::sport_run()
         // call attitude controller
         attitude_control.input_euler_rate_roll_pitch_yaw(target_roll_rate, target_pitch_rate, target_yaw_rate);
 
-        // call throttle controller
-        if (sonar_enabled && (sonar_alt_health >= SONAR_ALT_HEALTH_MAX)) {
-            // if sonar is ok, use surface tracking
+        // adjust climb rate using rangefinder
+        if (rangefinder_alt_ok()) {
+            // if rangefinder is ok, use surface tracking
             target_climb_rate = get_surface_tracking_climb_rate(target_climb_rate, pos_control.get_alt_target(), G_Dt);
         }
 
diff --git a/ArduSub/defines.h b/ArduSub/defines.h
index 9c46d1ac28..bd015ea08f 100644
--- a/ArduSub/defines.h
+++ b/ArduSub/defines.h
@@ -45,9 +45,9 @@ enum aux_sw_func {
     AUXSW_SAVE_TRIM =            5, // save current position as level
     AUXSW_SAVE_WP =              7, // save mission waypoint or RTL if in auto mode
     AUXSW_CAMERA_TRIGGER =       9, // trigger camera servo or relay
-    AUXSW_SONAR =               10, // allow enabling or disabling sonar in flight which helps avoid surface tracking when you are far above the ground
-    AUXSW_FENCE =               11, // allow enabling or disabling fence in flight
-    AUXSW_RESETTOARMEDYAW =     12, // changes yaw to be same as when quad was armed
+	AUXSW_RANGEFINDER =         10, // allow enabling or disabling rangefinder in flight which helps avoid surface tracking when you are far above the ground
+	AUXSW_FENCE =               11, // allow enabling or disabling fence in flight
+	AUXSW_RESETTOARMEDYAW =     12, // changes yaw to be same as when quad was armed
     AUXSW_SUPERSIMPLE_MODE =    13, // change to simple mode in middle, super simple at top
     AUXSW_ACRO_TRAINER =        14, // low = disabled, middle = leveled, high = leveled and limited
     AUXSW_SPRAYER =             15, // enable/disable the crop sprayer
@@ -157,8 +157,7 @@ enum tuning_func {
     TUNING_DECLINATION =                38, // compass declination in radians
     TUNING_CIRCLE_RATE =                39, // circle turn rate in degrees (hard coded to about 45 degrees in either direction)
     TUNING_ACRO_YAW_KP =                40, // acro controller's P term.  converts pilot input to a desired roll, pitch or yaw rate
-    TUNING_SONAR_GAIN =                 41, // sonar gain
-    TUNING_EKF_VERTICAL_POS =           42, // EKF's baro vs accel (higher rely on accels more, baro impact is reduced).  Range should be 0.2 ~ 4.0?  2.0 is default
+	TUNING_RANGEFINDER_GAIN =           41, // rangefinder gain     TUNING_EKF_VERTICAL_POS =           42, // EKF's baro vs accel (higher rely on accels more, baro impact is reduced).  Range should be 0.2 ~ 4.0?  2.0 is default
     TUNING_EKF_HORIZONTAL_POS =         43, // EKF's gps vs accel (higher rely on accels more, gps impact is reduced).  Range should be 1.0 ~ 3.0?  1.5 is default
     TUNING_EKF_ACCEL_NOISE =            44, // EKF's accel noise (lower means trust accels more, gps & baro less).  Range should be 0.02 ~ 0.5  0.5 is default (but very robust at that level)
     TUNING_RC_FEEL_RP =                 45, // roll-pitch input smoothing
@@ -313,17 +312,12 @@ enum ThrowModeState {
 #define MASK_LOG_ANY                    0xFFFF
 
 // DATA - event logging
-#define DATA_MAVLINK_FLOAT                  1
-#define DATA_MAVLINK_INT32                  2
-#define DATA_MAVLINK_INT16                  3
-#define DATA_MAVLINK_INT8                   4
 #define DATA_AP_STATE                       7
 #define DATA_SYSTEM_TIME_SET                8
 #define DATA_INIT_SIMPLE_BEARING            9
 #define DATA_ARMED                          10
 #define DATA_DISARMED                       11
 #define DATA_AUTO_ARMED                     15
-#define DATA_TAKEOFF                        16
 #define DATA_LAND_COMPLETE_MAYBE            17
 #define DATA_LAND_COMPLETE                  18
 #define DATA_NOT_LANDED                     28
@@ -344,7 +338,6 @@ enum ThrowModeState {
 #define DATA_AUTOTUNE_SAVEDGAINS            37
 #define DATA_SAVE_TRIM                      38
 #define DATA_SAVEWP_ADD_WP                  39
-#define DATA_SAVEWP_CLEAR_MISSION_RTL       40
 #define DATA_FENCE_ENABLE                   41
 #define DATA_FENCE_DISABLE                  42
 #define DATA_ACRO_TRAINER_DISABLED          43
@@ -352,7 +345,6 @@ enum ThrowModeState {
 #define DATA_ACRO_TRAINER_LIMITED           45
 #define DATA_EPM_GRAB                       46
 #define DATA_EPM_RELEASE                    47
-#define DATA_EPM_NEUTRAL                    48  // deprecated
 #define DATA_PARACHUTE_DISABLED             49
 #define DATA_PARACHUTE_ENABLED              50
 #define DATA_PARACHUTE_RELEASED             51
@@ -422,6 +414,7 @@ enum ThrowModeState {
 #define ERROR_CODE_FAILED_TO_SET_DESTINATION    2
 #define ERROR_CODE_RESTARTED_RTL            3
 #define ERROR_CODE_FAILED_CIRCLE_INIT       4
+#define ERROR_CODE_DEST_OUTSIDE_FENCE       5
 
 // parachute failed to deploy because of low altitude or landed
 #define ERROR_CODE_PARACHUTE_TOO_LOW        2
diff --git a/ArduSub/position_vector.cpp b/ArduSub/position_vector.cpp
index 643e5059b3..cbf109cf21 100644
--- a/ArduSub/position_vector.cpp
+++ b/ArduSub/position_vector.cpp
@@ -66,3 +66,10 @@ float Sub::pv_get_horizontal_distance_cm(const Vector3f &origin, const Vector3f
 {
     return pythagorous2(destination.x-origin.x,destination.y-origin.y);
 }
+
+// returns distance between a destination and home in cm
+float Sub::pv_distance_to_home_cm(const Vector3f &destination)
+{
+    Vector3f home = pv_location_to_vector(ahrs.get_home());
+    return pv_get_horizontal_distance_cm(home, destination);
+}
diff --git a/ArduSub/precision_landing.cpp b/ArduSub/precision_landing.cpp
index 4f48df33c2..3ae18199bc 100644
--- a/ArduSub/precision_landing.cpp
+++ b/ArduSub/precision_landing.cpp
@@ -18,8 +18,8 @@ void Copter::update_precland()
     float final_alt = current_loc.alt;
 
     // use range finder altitude if it is valid
-    if (sonar_enabled && (sonar_alt_health >= SONAR_ALT_HEALTH_MAX)) {
-        final_alt = sonar_alt;
+    if (rangefinder_alt_ok()) {
+        final_alt = rangefinder_state.alt_cm;
     }
 
     copter.precland.update(final_alt);
diff --git a/ArduSub/sensors.cpp b/ArduSub/sensors.cpp
index ba814b7667..9d6f633341 100644
--- a/ArduSub/sensors.cpp
+++ b/ArduSub/sensors.cpp
@@ -26,49 +26,61 @@ void Sub::read_barometer(void)
     motors.set_air_density_ratio(barometer.get_air_density_ratio());
 }
 
-#if CONFIG_SONAR == ENABLED
-void Sub::init_sonar(void)
+void Sub::init_rangefinder(void)
 {
-   sonar.init();
-}
+#if RANGEFINDER_ENABLED == ENABLED
+   rangefinder.init();
+   rangefinder_state.alt_cm_filt.set_cutoff_frequency(RANGEFINDER_WPNAV_FILT_HZ);
+   rangefinder_state.enabled = (rangefinder.num_sensors() >= 1);
 #endif
+}
 
-// return sonar altitude in centimeters
-int16_t Sub::read_sonar(void)
+// return rangefinder altitude in centimeters
+void Sub::read_rangefinder(void)
 {
-#if CONFIG_SONAR == ENABLED
-    sonar.update();
+#if RANGEFINDER_ENABLED == ENABLED
+    rangefinder.update();
 
-    // exit immediately if sonar is disabled
-    if (sonar.status() != RangeFinder::RangeFinder_Good) {
-        sonar_alt_health = 0;
-        return 0;
-    }
+    rangefinder_state.alt_healthy = ((rangefinder.status() == RangeFinder::RangeFinder_Good) && (rangefinder.range_valid_count() >= RANGEFINDER_HEALTH_MAX));
 
-    int16_t temp_alt = sonar.distance_cm();
+    int16_t temp_alt = rangefinder.distance_cm();
 
-    if (temp_alt >= sonar.min_distance_cm() && 
-        temp_alt <= sonar.max_distance_cm() * SONAR_RELIABLE_DISTANCE_PCT) {
-        if ( sonar_alt_health < SONAR_ALT_HEALTH_MAX ) {
-            sonar_alt_health++;
-        }
-    }else{
-        sonar_alt_health = 0;
-    }
-
- #if SONAR_TILT_CORRECTION == 1
-    // correct alt for angle of the sonar
-    float temp = ahrs.cos_pitch() * ahrs.cos_roll();
-    temp = MAX(temp, 0.707f);
-    temp_alt = (float)temp_alt * temp;
+ #if RANGEFINDER_TILT_CORRECTION == ENABLED
+    // correct alt for angle of the rangefinder
+    temp_alt = (float)temp_alt * MAX(0.707f, ahrs.get_rotation_body_to_ned().c.z);
  #endif
 
-    return temp_alt;
+    rangefinder_state.alt_cm = temp_alt;
+
+    // filter rangefinder for use by AC_WPNav
+    uint32_t now = AP_HAL::millis();
+
+    if (rangefinder_state.alt_healthy) {
+        if (now - rangefinder_state.last_healthy_ms > RANGEFINDER_TIMEOUT_MS) {
+            // reset filter if we haven't used it within the last second
+            rangefinder_state.alt_cm_filt.reset(rangefinder_state.alt_cm);
+        } else {
+            rangefinder_state.alt_cm_filt.apply(rangefinder_state.alt_cm, 0.05f);
+        }
+        rangefinder_state.last_healthy_ms = now;
+    }
+
+    // send rangefinder altitude and health to waypoint navigation library
+    wp_nav.set_rangefinder_alt(rangefinder_state.enabled, rangefinder_state.alt_healthy, rangefinder_state.alt_cm_filt.get());
+
 #else
-    return 0;
+    rangefinder_state.enabled = false;
+    rangefinder_state.alt_healthy = false;
+    rangefinder_state.alt_cm = 0;
 #endif
 }
 
+// return true if rangefinder_alt can be used
+bool Sub::rangefinder_alt_ok()
+{
+    return (rangefinder_state.enabled && rangefinder_state.alt_healthy);
+}
+
 /*
   update RPM sensors
  */
diff --git a/ArduSub/switches.cpp b/ArduSub/switches.cpp
index ff19e768eb..abfc3d61ac 100644
--- a/ArduSub/switches.cpp
+++ b/ArduSub/switches.cpp
@@ -228,7 +228,7 @@ void Sub::init_aux_switch_function(int8_t ch_option, uint8_t ch_flag)
     // init channel options
     switch(ch_option) {
         case AUXSW_SIMPLE_MODE:
-        case AUXSW_SONAR:
+        case AUXSW_RANGEFINDER:
         case AUXSW_FENCE:
         case AUXSW_RESETTOARMEDYAW:
         case AUXSW_SUPERSIMPLE_MODE:
@@ -352,14 +352,14 @@ void Sub::do_aux_switch_function(int8_t ch_function, uint8_t ch_flag)
             break;
 #endif
 
-        case AUXSW_SONAR:
+        case AUXSW_RANGEFINDER:
             // enable or disable the sonar
-#if CONFIG_SONAR == ENABLED
-            if (ch_flag == AUX_SWITCH_HIGH) {
-                sonar_enabled = true;
+#if RANGEFINDER_ENABLED == ENABLED
+            if ((ch_flag == AUX_SWITCH_HIGH) && (rangefinder.num_sensors() >= 1)) {
+                rangefinder_state.enabled = true;
             }else{
-                sonar_enabled = false;
-            }
+                rangefinder_state.enabled = false;
+			}
 #endif
             break;
 
diff --git a/ArduSub/system.cpp b/ArduSub/system.cpp
index c61fac14e9..285de64035 100644
--- a/ArduSub/system.cpp
+++ b/ArduSub/system.cpp
@@ -255,9 +255,9 @@ void Sub::init_ardupilot()
     //-----------------------------
     init_barometer(true);
 
-    // initialise sonar
-#if CONFIG_SONAR == ENABLED
-    init_sonar();
+    // initialise rangefinder
+#if RANGEFINDER_ENABLED == ENABLED
+    init_rangefinder();
 #endif
 
     // initialise AP_RPM library
diff --git a/ArduSub/terrain.cpp b/ArduSub/terrain.cpp
index b40c8ce4fd..1d01201d1f 100644
--- a/ArduSub/terrain.cpp
+++ b/ArduSub/terrain.cpp
@@ -10,10 +10,10 @@ void Sub::terrain_update()
 
     // tell the rangefinder our height, so it can go into power saving
     // mode if available
-#if CONFIG_SONAR == ENABLED
+#if RANGEFINDER_ENABLED == ENABLED
     float height;
     if (terrain.height_above_terrain(height, true)) {
-        sonar.set_estimated_terrain_height(height);
+        rangefinder.set_estimated_terrain_height(height);
     }
 #endif
 #endif
diff --git a/ArduSub/test.cpp b/ArduSub/test.cpp
index 3650011e6f..76f15e8a3f 100644
--- a/ArduSub/test.cpp
+++ b/ArduSub/test.cpp
@@ -20,7 +20,7 @@ static const struct Menu::command test_menu_commands[] = {
     {"shell", 				MENU_FUNC(test_shell)},
 #endif
 #if HIL_MODE == HIL_MODE_DISABLED
-    {"rangefinder",         MENU_FUNC(test_sonar)},
+    {"rangefinder",         MENU_FUNC(test_rangefinder)},
 #endif
 };
 
@@ -243,21 +243,21 @@ int8_t Sub::test_shell(uint8_t argc, const Menu::arg *argv)
 /*
  *  test the rangefinders
  */
-int8_t Sub::test_sonar(uint8_t argc, const Menu::arg *argv)
+int8_t Sub::test_rangefinder(uint8_t argc, const Menu::arg *argv)
 {
-#if CONFIG_SONAR == ENABLED
-	sonar.init();
+#if RANGEFINDER_ENABLED == ENABLED
+	rangefinder.init();
 
-    cliSerial->printf("RangeFinder: %d devices detected\n", sonar.num_sensors());
+    cliSerial->printf("RangeFinder: %d devices detected\n", rangefinder.num_sensors());
 
     print_hit_enter();
     while(1) {
         delay(100);
-        sonar.update();
+        rangefinder.update();
 
-        cliSerial->printf("Primary: status %d distance_cm %d \n", (int)sonar.status(), sonar.distance_cm());
+        cliSerial->printf("Primary: status %d distance_cm %d \n", (int)rangefinder.status(), rangefinder.distance_cm());
         cliSerial->printf("All: device_0 type %d status %d distance_cm %d, device_1 type %d status %d distance_cm %d\n",
-        (int)sonar._type[0], (int)sonar.status(0), sonar.distance_cm(0), (int)sonar._type[1], (int)sonar.status(1), sonar.distance_cm(1));
+        (int)rangefinder._type[0], (int)rangefinder.status(0), rangefinder.distance_cm(0), (int)rangefinder._type[1], (int)rangefinder.status(1), rangefinder.distance_cm(1));
 
         if(cliSerial->available() > 0) {
             return (0);
diff --git a/ArduSub/tuning.cpp b/ArduSub/tuning.cpp
index 898537dd16..5ca974ad1d 100644
--- a/ArduSub/tuning.cpp
+++ b/ArduSub/tuning.cpp
@@ -116,9 +116,9 @@ void Sub::tuning() {
         circle_nav.set_rate((float)g.rc_6.control_in/25.0f-20.0f);
         break;
 
-    case TUNING_SONAR_GAIN:
-        // set sonar gain
-        g.sonar_gain.set(tuning_value);
+    case TUNING_RANGEFINDER_GAIN:
+        // set rangefinder gain
+        g.rangefinder_gain.set(tuning_value);
         break;
 
 #if 0