diff --git a/ArduSub/config.h b/ArduSub/config.h
index bd35043c78..5bf48e22c6 100644
--- a/ArduSub/config.h
+++ b/ArduSub/config.h
@@ -395,7 +395,7 @@
  # define LAND_SPEED    50          // the descent speed for the final stage of landing in cm/s
 #endif
 #ifndef LAND_START_ALT
- # define LAND_START_ALT 1000         // altitude in cm where land controller switches to slow rate of descent
+ # define LAND_START_ALT -25         // altitude in cm where land controller switches to slow rate of descent
 #endif
 #ifndef LAND_REQUIRE_MIN_THROTTLE_TO_DISARM
  # define LAND_REQUIRE_MIN_THROTTLE_TO_DISARM DISABLED  // we do not require pilot to reduce throttle to minimum before vehicle will disarm in AUTO, LAND or RTL
diff --git a/ArduSub/control_althold.cpp b/ArduSub/control_althold.cpp
index 1645af0923..c23ba7fe2f 100644
--- a/ArduSub/control_althold.cpp
+++ b/ArduSub/control_althold.cpp
@@ -17,6 +17,7 @@ bool Sub::althold_init(bool ignore_checks)
 #endif
 
     // initialize vertical speeds and leash lengths
+    // sets the maximum speed up and down returned by position controller
     pos_control.set_speed_z(-g.pilot_velocity_z_max, g.pilot_velocity_z_max);
     pos_control.set_accel_z(g.pilot_accel_z);
 
@@ -62,18 +63,18 @@ void Sub::althold_run()
     bool takeoff_triggered = (ap.land_complete && (channel_throttle->control_in > get_takeoff_trigger_throttle()));
 #endif
 
-    // Alt Hold State Machine Determination
-    if(!ap.auto_armed) {
-        althold_state = AltHold_Disarmed;
-    } else if (!motors.get_interlock()){
-        althold_state = AltHold_MotorStop;
-    } else if (takeoff_state.running || takeoff_triggered){
-        althold_state = AltHold_Takeoff;
-    } else if (ap.land_complete){
-        althold_state = AltHold_Landed;
-    } else {
+//    // Alt Hold State Machine Determination
+//    if(!ap.auto_armed) {
+//        althold_state = AltHold_Disarmed;
+//    } else if (!motors.get_interlock()){
+//        althold_state = AltHold_MotorStop;
+//    } else if (takeoff_state.running || takeoff_triggered){
+//        althold_state = AltHold_Takeoff;
+//    } else if (ap.land_complete){
+//        althold_state = AltHold_Landed;
+//    } else {
         althold_state = AltHold_Flying;
-    }
+//    }
 
     // Alt Hold State Machine
     switch (althold_state) {
diff --git a/ArduSub/radio.cpp b/ArduSub/radio.cpp
index c4cd14bb2b..e0d478fcd7 100644
--- a/ArduSub/radio.cpp
+++ b/ArduSub/radio.cpp
@@ -238,7 +238,7 @@ void Sub::set_throttle_zero_flag(int16_t throttle_control)
     // if not using throttle interlock and non-zero throttle and not E-stopped,
     // or using motor interlock and it's enabled, then motors are running, 
     // and we are flying. Immediately set as non-zero
-    if ((!ap.using_interlock && (throttle_control > 0) && !ap.motor_emergency_stop) || (ap.using_interlock && motors.get_interlock())) {
+    if ((!ap.using_interlock && (throttle_control < 475 || throttle_control > 525)&& && !ap.motor_emergency_stop) || (ap.using_interlock && motors.get_interlock())) {
         last_nonzero_throttle_ms = tnow_ms;
         ap.throttle_zero = false;
     } else if (tnow_ms - last_nonzero_throttle_ms > THROTTLE_ZERO_DEBOUNCE_TIME_MS) {
diff --git a/ArduSub/system.cpp b/ArduSub/system.cpp
index 000fb3932b..ca90f488da 100644
--- a/ArduSub/system.cpp
+++ b/ArduSub/system.cpp
@@ -241,14 +241,18 @@ void Sub::init_ardupilot()
     ins.set_hil_mode();
 #endif
 
+    if(barometer.num_instances() > 1) {
+    //We have an external MS58XX pressure sensor connected
+    	for(int i = 1; i < barometer.num_instances(); i++) {
+			barometer.set_type(i, BARO_TYPE_WATER); //Altitude (depth) is calculated differently underwater
+			barometer.set_precision_multiplier(i, 10); //The MS58XX values reported need to be multiplied by 10 to match units everywhere else
+    	}
+    	barometer.set_primary_baro(1); //Set the primary baro to external MS58XX
+
+    }
     // read Baro pressure at ground
     //-----------------------------
     init_barometer(true);
-    if(barometer.num_instances() > 1) {
-    //We have an external MS58XX pressure sensor connected
-    	barometer.set_type(1, BARO_TYPE_WATER); //Altitude (depth) is calculated differently underwater 
-    	barometer.set_precision_multiplier(1, 10); //The MS58XX values reported need to be multiplied by 10 to match units everywhere else
-    }
 
     // initialise sonar
 #if CONFIG_SONAR == ENABLED