From 5eee51b5a473be9492303e344c9dd06ee28a1a85 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <tridge@samba.org>
Date: Wed, 24 Sep 2014 11:03:19 +1000
Subject: [PATCH] APM_OBC: added heartbeat() method

this is used for when the plane is calibrating sensors, to ensure
heartbeat is continued to the failsafe board
---
 libraries/APM_OBC/APM_OBC.cpp | 18 ++++++++++++++++++
 libraries/APM_OBC/APM_OBC.h   |  5 +++++
 2 files changed, 23 insertions(+)

diff --git a/libraries/APM_OBC/APM_OBC.cpp b/libraries/APM_OBC/APM_OBC.cpp
index 6c0ad0aa83..d34d30e3f3 100644
--- a/libraries/APM_OBC/APM_OBC.cpp
+++ b/libraries/APM_OBC/APM_OBC.cpp
@@ -259,6 +259,24 @@ APM_OBC::check(APM_OBC::control_mode mode, uint32_t last_heartbeat_ms, bool geof
     }    
 }
 
+
+// send heartbeat messages during sensor calibration
+void
+APM_OBC::heartbeat(void)
+{    
+    if (!_enable) {
+        return;
+    }
+
+    // if we are not terminating or if there is a separate terminate
+    // pin configured then toggle the heartbeat pin at 10Hz
+    if (_heartbeat_pin != -1 && (_terminate_pin != -1 || !_terminate)) {
+        _heartbeat_pin_value = !_heartbeat_pin_value;
+        hal.gpio->pinMode(_heartbeat_pin, HAL_GPIO_OUTPUT);
+        hal.gpio->write(_heartbeat_pin, _heartbeat_pin_value);
+    }    
+}
+
 // check for altitude limit breach
 bool
 APM_OBC::check_altlimit(void)
diff --git a/libraries/APM_OBC/APM_OBC.h b/libraries/APM_OBC/APM_OBC.h
index b08ee8ce2e..7a90a6be43 100644
--- a/libraries/APM_OBC/APM_OBC.h
+++ b/libraries/APM_OBC/APM_OBC.h
@@ -64,8 +64,13 @@ public:
             _saved_wp = 0;
         }
 
+    // check that everything is OK
     void check(enum control_mode control_mode, uint32_t last_heartbeat_ms, bool geofence_breached, uint32_t last_valid_rc_ms);
 
+    // generate heartbeat msgs, so external failsafe boards are happy
+    // during sensor calibration
+    void heartbeat(void);
+
     // called in servo output code to set servos to crash position if needed
     void check_crash_plane(void);