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

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

diff --git a/AntennaTracker/GCS_Mavlink.cpp b/AntennaTracker/GCS_Mavlink.cpp
index a4d3888b55..f8a59e552a 100644
--- a/AntennaTracker/GCS_Mavlink.cpp
+++ b/AntennaTracker/GCS_Mavlink.cpp
@@ -592,7 +592,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
                     }
                 } 
                 if (is_equal(packet.param3,1.0f)) {
-                    tracker.init_barometer();
+                    tracker.init_barometer(false);
                     // zero the altitude difference on next baro update
                     tracker.nav_status.need_altitude_calibration = true;
                 }
diff --git a/AntennaTracker/Tracker.h b/AntennaTracker/Tracker.h
index baae59dfb6..d547d80673 100644
--- a/AntennaTracker/Tracker.h
+++ b/AntennaTracker/Tracker.h
@@ -214,7 +214,7 @@ private:
     void update_scan(void);
     bool servo_test_set_servo(uint8_t servo_num, uint16_t pwm);
     void read_radio();
-    void init_barometer(void);
+    void init_barometer(bool full_calibration);
     void update_barometer(void);
     void update_ahrs();
     void update_compass(void);
diff --git a/AntennaTracker/sensors.cpp b/AntennaTracker/sensors.cpp
index 059f3669e9..c4b3997aec 100644
--- a/AntennaTracker/sensors.cpp
+++ b/AntennaTracker/sensors.cpp
@@ -2,10 +2,14 @@
 
 #include "Tracker.h"
 
-void Tracker::init_barometer(void)
+void Tracker::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/AntennaTracker/system.cpp b/AntennaTracker/system.cpp
index d8855ecda4..00902e2b96 100644
--- a/AntennaTracker/system.cpp
+++ b/AntennaTracker/system.cpp
@@ -77,7 +77,7 @@ void Tracker::init_tracker()
     ins.init(scheduler.get_loop_rate_hz());
     ahrs.reset();
 
-    init_barometer();
+    init_barometer(true);
 
     // set serial ports non-blocking
     serial_manager.set_blocking_writes_all(false);