diff --git a/ArduSub/GCS_Mavlink.cpp b/ArduSub/GCS_Mavlink.cpp
index fceb1b691f..b1afd43bd9 100644
--- a/ArduSub/GCS_Mavlink.cpp
+++ b/ArduSub/GCS_Mavlink.cpp
@@ -1320,7 +1320,7 @@ void GCS_MAVLINK_Sub::handleMessage(mavlink_message_t* msg)
                 if (!sub.ap.depth_sensor_present || sub.motors.armed() || sub.barometer.get_pressure() > 110000) {
                     result = MAV_RESULT_FAILED;
                 } else {
-                    sub.init_barometer(false);
+                    sub.init_barometer(true);
                     result = MAV_RESULT_ACCEPTED;
                 }
             } else if (is_equal(packet.param4,1.0f)) {
diff --git a/ArduSub/Sub.h b/ArduSub/Sub.h
index 0d587036f4..678c15bbd8 100644
--- a/ArduSub/Sub.h
+++ b/ArduSub/Sub.h
@@ -767,7 +767,7 @@ private:
     void default_js_buttons(void);
     void set_throttle_zero_flag(int16_t throttle_control);
     void radio_passthrough_to_motors();
-    void init_barometer(bool full_calibration);
+    void init_barometer(bool save);
     void read_barometer(void);
     void init_rangefinder(void);
     void read_rangefinder(void);
diff --git a/ArduSub/sensors.cpp b/ArduSub/sensors.cpp
index 44b64a5d15..f14ef14504 100644
--- a/ArduSub/sensors.cpp
+++ b/ArduSub/sensors.cpp
@@ -1,13 +1,9 @@
 #include "Sub.h"
 
-void Sub::init_barometer(bool full_calibration)
+void Sub::init_barometer(bool save)
 {
     gcs_send_text(MAV_SEVERITY_INFO, "Calibrating barometer");
-    if (full_calibration) {
-        barometer.calibrate();
-    } else {
-        barometer.update_calibration();
-    }
+    barometer.calibrate(save);
     gcs_send_text(MAV_SEVERITY_INFO, "Barometer calibration complete");
 }
 
diff --git a/ArduSub/system.cpp b/ArduSub/system.cpp
index 22d60e1b31..3b08442a4e 100644
--- a/ArduSub/system.cpp
+++ b/ArduSub/system.cpp
@@ -229,7 +229,9 @@ void Sub::init_ardupilot()
     ins.set_hil_mode();
 #endif
 
-    barometer.calibrate();
+    // read Baro pressure at ground
+    //-----------------------------
+    init_barometer(false);
     barometer.update();
 
     if (barometer.healthy(1)) { // We have an external MS58XX pressure sensor connected
@@ -247,10 +249,6 @@ void Sub::init_ardupilot()
 
     leak_detector.init();
 
-    // read Baro pressure at ground
-    //-----------------------------
-    init_barometer(true);
-
     // backwards compatibility
     if (attitude_control.get_accel_yaw_max() < 110000.0f) {
         attitude_control.save_accel_yaw_max(110000.0f);