From 4b01cee33069fc6a62ec51e0bce3696ea6e19db9 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <tridge@samba.org>
Date: Wed, 24 Sep 2014 11:03:35 +1000
Subject: [PATCH] Plane: send heartbeat to AFS when calibrating sensors

---
 ArduPlane/ArduPlane.pde   | 5 +++++
 ArduPlane/GCS_Mavlink.pde | 2 ++
 ArduPlane/failsafe.pde    | 8 ++++++++
 3 files changed, 15 insertions(+)

diff --git a/ArduPlane/ArduPlane.pde b/ArduPlane/ArduPlane.pde
index 63ca703d7f..46d96d5e7d 100644
--- a/ArduPlane/ArduPlane.pde
+++ b/ArduPlane/ArduPlane.pde
@@ -280,6 +280,11 @@ static struct {
 // should throttle be pass-thru in guided?
 static bool guided_throttle_passthru;
 
+// are we doing calibration? This is used to allow heartbeat to
+// external failsafe boards during baro and airspeed calibration
+static bool in_calibration;
+
+
 ////////////////////////////////////////////////////////////////////////////////
 // GCS selection
 ////////////////////////////////////////////////////////////////////////////////
diff --git a/ArduPlane/GCS_Mavlink.pde b/ArduPlane/GCS_Mavlink.pde
index f530b7092b..6c513b40a4 100644
--- a/ArduPlane/GCS_Mavlink.pde
+++ b/ArduPlane/GCS_Mavlink.pde
@@ -984,10 +984,12 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
                 packet.param2 == 1) {
                 startup_INS_ground(true);
             } else if (packet.param3 == 1) {
+                in_calibration = true;
                 init_barometer();
                 if (airspeed.enabled()) {
                     zero_airspeed();
                 }
+                in_calibration = false;
             }
             if (packet.param4 == 1) {
                 trim_radio();
diff --git a/ArduPlane/failsafe.pde b/ArduPlane/failsafe.pde
index 3d96102707..4b54eeae96 100644
--- a/ArduPlane/failsafe.pde
+++ b/ArduPlane/failsafe.pde
@@ -39,6 +39,14 @@ void failsafe_check(void)
     if (in_failsafe && tnow - last_timestamp > 20000) {
         last_timestamp = tnow;
 
+#if OBC_FAILSAFE == ENABLED
+        if (in_calibration) {
+            // tell the failsafe system that we are calibrating
+            // sensors, so don't trigger failsafe
+            obc.heartbeat();
+        }
+#endif
+
         if (hal.rcin->num_channels() == 0) {
             // we don't have any RC input to pass through
             return;