diff --git a/libraries/AP_Compass/AP_Compass_Calibration.cpp b/libraries/AP_Compass/AP_Compass_Calibration.cpp
index af8648636a..1dcf474edd 100644
--- a/libraries/AP_Compass/AP_Compass_Calibration.cpp
+++ b/libraries/AP_Compass/AP_Compass_Calibration.cpp
@@ -38,22 +38,21 @@ Compass::compass_cal_update()
 bool
 Compass::start_calibration(uint8_t i, bool retry, bool autosave, float delay, bool autoreboot)
 {
-    if (healthy(i)) {
-        memset(_reports_sent,0,sizeof(_reports_sent));
-        if (!is_calibrating() && delay > 0.5f) {
-            AP_Notify::events.initiated_compass_cal = 1;
-        }
-        if (i == get_primary()) {
-            _calibrator[i].set_tolerance(8);
-        } else {
-            _calibrator[i].set_tolerance(16);
-        }
-        _calibrator[i].start(retry, autosave, delay);
-        _compass_cal_autoreboot = autoreboot;
-        return true;
-    } else {
+    if (!healthy(i)) {
         return false;
     }
+    memset(_reports_sent,0,sizeof(_reports_sent));
+    if (!is_calibrating() && delay > 0.5f) {
+        AP_Notify::events.initiated_compass_cal = 1;
+    }
+    if (i == get_primary()) {
+        _calibrator[i].set_tolerance(8);
+    } else {
+        _calibrator[i].set_tolerance(16);
+    }
+    _calibrator[i].start(retry, autosave, delay);
+    _compass_cal_autoreboot = autoreboot;
+    return true;
 }
 
 bool
diff --git a/modules/PX4Firmware b/modules/PX4Firmware
index 34e1d543e4..51858c810a 160000
--- a/modules/PX4Firmware
+++ b/modules/PX4Firmware
@@ -1 +1 @@
-Subproject commit 34e1d543e49c2756b8fa4b81ef30468497ea792a
+Subproject commit 51858c810a3cb086d7c2041328bd32adec9fa724