PeriodicProcess: added set_failsafe() call

this adds an optional failsafe function that will be called in the
timer loop

libraries/AP_PeriodicProcess/AP_PeriodicProcessStub.cpp

@@ -2,5 +2,6 @@
 #include "AP_PeriodicProcessStub.h"
 AP_PeriodicProcessStub::AP_PeriodicProcessStub(int period) {}
 void AP_PeriodicProcessStub::init( Arduino_Mega_ISR_Registry * isr_reg ){}
-void AP_PeriodicProcessStub::register_process(void (*proc)(void) ) {}
+void AP_PeriodicProcessStub::register_process(ap_procedure proc) {}
+void AP_PeriodicProcessStub::set_failsafe(ap_procedure proc) {}
 void AP_PeriodicProcessStub::run(void) {}

libraries/AP_PeriodicProcess/AP_PeriodicProcessStub.h

@@ -11,11 +11,13 @@ class AP_PeriodicProcessStub : public AP_PeriodicProcess
     public:
         AP_PeriodicProcessStub(int period = 0);
         void init( Arduino_Mega_ISR_Registry * isr_reg );
-        void register_process(void (* proc)(void));
+        void register_process(ap_procedure proc);
+        void set_failsafe(ap_procedure proc);
         static void run(void);
     protected:
         static int  _period;
         static void (*_proc)(void);
+        static void (*_failsafe)(void);
 };
 
 #endif

libraries/AP_PeriodicProcess/AP_TimerProcess.cpp

@@ -10,6 +10,8 @@ extern "C" {
 
 int AP_TimerProcess::_period;
 ap_procedure AP_TimerProcess::_proc[AP_TIMERPROCESS_MAX_PROCS];
+ap_procedure AP_TimerProcess::_failsafe;
+bool AP_TimerProcess::_in_timer_call;
 int AP_TimerProcess::_pidx = 0;
 
 AP_TimerProcess::AP_TimerProcess(int period)
@@ -27,31 +29,68 @@ void AP_TimerProcess::init( Arduino_Mega_ISR_Registry * isr_reg )
 	TIFR2  = _BV(TOV2);	        // clear pending interrupts;
 	TIMSK2 = _BV(TOIE2);        // enable the overflow interrupt
 
-    for (int i = 0; i < AP_TIMERPROCESS_MAX_PROCS; i++)
-        _proc[i] = NULL;
+	_failsafe = NULL;
+	_in_timer_call = false;
 
-    isr_reg->register_signal( ISR_REGISTRY_TIMER2_OVF, AP_TimerProcess::run);
+	for (int i = 0; i < AP_TIMERPROCESS_MAX_PROCS; i++)
+		_proc[i] = NULL;
+
+	isr_reg->register_signal( ISR_REGISTRY_TIMER2_OVF, AP_TimerProcess::run);
 }
 
-void AP_TimerProcess::register_process(void (*proc)(void) )
+void AP_TimerProcess::register_process(ap_procedure proc)
 {
     if (_pidx < AP_TIMERPROCESS_MAX_PROCS)
         _proc[_pidx++] = proc;
 }
 
+void AP_TimerProcess::set_failsafe(ap_procedure proc)
+{
+	_failsafe = proc;
+}
+
 void AP_TimerProcess::run(void)
 {
-	// we re-enable interrupts here as some of these functions
-	// take a long time, and we don't want to block other critical
-	// interrupts, especially the serial interrupt
+	// we enable the interrupt again immediately and also enable
+	// interrupts. This allows other time critical interrupts to
+	// run (such as the serial receive interrupt). We catch the
+	// timer calls taking too long using _in_timer_call. 
+	// This approach also gives us a nice uniform spacing between
+	// timer calls
+	TCNT2 = _period;
 	sei();
 
+	uint32_t tnow = micros();
+
+	if (_in_timer_call) {
+		// the timer calls took longer than the period of the
+		// timer. This is bad, and may indicate a serious
+		// driver failure. We can't just call the drivers
+		// again, as we could run out of stack. So we only
+		// call the _failsafe call. It's job is to detect if
+		// the drivers or the main loop are indeed dead and to
+		// activate whatever failsafe it thinks may help if
+		// need be.  We assume the failsafe code can't
+		// block. If it does then we will recurse and die when
+		// we run out of stack
+		if (_failsafe != NULL) {
+			_failsafe(tnow);
+		}
+		return;
+	}
+	_in_timer_call = true;
+
+	// now call the timer based drivers
 	for (int i = 0; i < _pidx; i++) {
-		if (_proc[i] != NULL)
-			_proc[i]();
+		if (_proc[i] != NULL) {
+			_proc[i](tnow);
+		}
 	}
 
-	// to prevent recursion, we don't enable the timer interrupt
-	// again until we've completed the tasks
-	TCNT2 = _period;
+	// and the failsafe, if one is setup
+	if (_failsafe != NULL) {
+		_failsafe(tnow);
+	}
+
+	_in_timer_call = false;
 }

libraries/AP_PeriodicProcess/AP_TimerProcess.h

@@ -15,12 +15,15 @@ class AP_TimerProcess : public AP_PeriodicProcess
     public:
         AP_TimerProcess(int period = TIMERPROCESS_PER_DEFAULT);
         void init( Arduino_Mega_ISR_Registry * isr_reg );
-        void register_process(void (* proc)(void));
+        void register_process(ap_procedure proc);
+        void set_failsafe(ap_procedure proc);
         static void run(void);
     protected:
         static int _period;
         static ap_procedure _proc[AP_TIMERPROCESS_MAX_PROCS];
+        static ap_procedure _failsafe;
         static int _pidx;
+	static bool _in_timer_call;
 };
 
 #endif // __AP_TIMERPROCESS_H__

libraries/AP_PeriodicProcess/PeriodicProcess.h

@@ -2,12 +2,17 @@
 #ifndef __PERIODICPROCESS_H__
 #define __PERIODICPROCESS_H__
 
-typedef void (*ap_procedure)(void);
+#include <stdint.h>
+
+// the callback type for periodic processes. They are passed the time
+// in microseconds since boot 
+typedef void (*ap_procedure)(uint32_t );
 
 class AP_PeriodicProcess
 {
     public:
-        virtual void register_process(void (* proc)(void)) = 0;
+        virtual void register_process(ap_procedure proc) = 0;
+        virtual void set_failsafe(ap_procedure proc) = 0;
 };
 
 #endif // __PERIODICPROCESS_H__