From f61ba903dd6c82f3603c1741ed8963546fde366e Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Tue, 24 May 2016 11:31:27 +1000
Subject: [PATCH] Rover: don't stall EKF during baro cal

---
 APMrover2/GCS_Mavlink.cpp | 2 +-
 APMrover2/Rover.h         | 2 +-
 APMrover2/sensors.cpp     | 8 ++++++--
 APMrover2/system.cpp      | 2 +-
 4 files changed, 9 insertions(+), 5 deletions(-)

diff --git a/APMrover2/GCS_Mavlink.cpp b/APMrover2/GCS_Mavlink.cpp
index e8147e95c9a..9999cbaa739 100644
--- a/APMrover2/GCS_Mavlink.cpp
+++ b/APMrover2/GCS_Mavlink.cpp
@@ -942,7 +942,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
                         result = MAV_RESULT_FAILED;
                     }
                 } else if (is_equal(packet.param3,1.0f)) {
-                    rover.init_barometer();
+                    rover.init_barometer(false);
                     result = MAV_RESULT_ACCEPTED;
                 } else if (is_equal(packet.param4,1.0f)) {
                     rover.trim_radio();
diff --git a/APMrover2/Rover.h b/APMrover2/Rover.h
index b763ea7cb83..653f5ac158f 100644
--- a/APMrover2/Rover.h
+++ b/APMrover2/Rover.h
@@ -464,7 +464,7 @@ private:
     bool throttle_failsafe_active();
     void trim_control_surfaces();
     void trim_radio();
-    void init_barometer(void);
+    void init_barometer(bool full_calibration);
     void init_sonar(void);
     void read_battery(void);
     void read_receiver_rssi(void);
diff --git a/APMrover2/sensors.cpp b/APMrover2/sensors.cpp
index cbeb2ee5189..cddead068c8 100644
--- a/APMrover2/sensors.cpp
+++ b/APMrover2/sensors.cpp
@@ -2,10 +2,14 @@
 
 #include "Rover.h"
 
-void Rover::init_barometer(void)
+void Rover::init_barometer(bool full_calibration)
 {
     gcs_send_text(MAV_SEVERITY_INFO, "Calibrating barometer");
-    barometer.calibrate();
+    if (full_calibration) {
+        barometer.calibrate();
+    } else {
+        barometer.update_calibration();
+    }
     gcs_send_text(MAV_SEVERITY_INFO, "Barometer calibration complete");
 }
 
diff --git a/APMrover2/system.cpp b/APMrover2/system.cpp
index d6c013edb8b..1361492bc6c 100644
--- a/APMrover2/system.cpp
+++ b/APMrover2/system.cpp
@@ -149,7 +149,7 @@ void Rover::init_ardupilot()
     init_sonar();
 
     // and baro for EKF
-    init_barometer();
+    init_barometer(true);
 
     // Do GPS init
     gps.init(&DataFlash, serial_manager);