From a7b9c8b9cd05377ab3edd80cd643863886ecf205 Mon Sep 17 00:00:00 2001
From: Pat Hickey <pat@moreproductive.org>
Date: Thu, 8 Dec 2011 21:27:50 -0800
Subject: [PATCH] AP_PeriodicProcess: AP_TimerProcess can register multiple
 callbacks.

---
 .../AP_TimerAperiodicProcess.cpp                |  6 ++++--
 .../AP_PeriodicProcess/AP_TimerProcess.cpp      | 17 +++++++++++------
 libraries/AP_PeriodicProcess/AP_TimerProcess.h  |  7 +++++--
 libraries/AP_PeriodicProcess/PeriodicProcess.h  |  2 ++
 4 files changed, 22 insertions(+), 10 deletions(-)

diff --git a/libraries/AP_PeriodicProcess/AP_TimerAperiodicProcess.cpp b/libraries/AP_PeriodicProcess/AP_TimerAperiodicProcess.cpp
index 0918b4e727..95231248d2 100644
--- a/libraries/AP_PeriodicProcess/AP_TimerAperiodicProcess.cpp
+++ b/libraries/AP_PeriodicProcess/AP_TimerAperiodicProcess.cpp
@@ -34,6 +34,8 @@ void AP_TimerAperiodicProcess::run(void)
     _timer_offset = (_timer_offset + 49) % 32;
     _period = TCNT2_781_HZ + _timer_offset;
     TCNT2 = _period;
-    if (_proc != NULL)
-        _proc();
+    for (int i = 0; i < _pidx; i++) {
+        if (_proc[i] != NULL)
+            _proc[i]();
+    }
 }
diff --git a/libraries/AP_PeriodicProcess/AP_TimerProcess.cpp b/libraries/AP_PeriodicProcess/AP_TimerProcess.cpp
index cc120d2926..2e6dad4a50 100644
--- a/libraries/AP_PeriodicProcess/AP_TimerProcess.cpp
+++ b/libraries/AP_PeriodicProcess/AP_TimerProcess.cpp
@@ -8,12 +8,12 @@ extern "C" {
 }
 
 int AP_TimerProcess::_period;
-void (*AP_TimerProcess::_proc)(void);
+ap_procedure AP_TimerProcess::_proc[AP_TIMERPROCESS_MAX_PROCS];
+int AP_TimerProcess::_pidx = 0;
 
 AP_TimerProcess::AP_TimerProcess(int period)
 {
     _period = period;
-    _proc = NULL;
 }
 
 void AP_TimerProcess::init( Arduino_Mega_ISR_Registry * isr_reg )
@@ -26,18 +26,23 @@ 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;
+
     isr_reg->register_signal( ISR_REGISTRY_TIMER2_OVF, AP_TimerProcess::run);
 }
 
 void AP_TimerProcess::register_process(void (*proc)(void) )
 {
-    _proc = proc;
-    TCNT2 = 1;                 // Should go off almost immediately.
+    if (_pidx < AP_TIMERPROCESS_MAX_PROCS)
+        _proc[_pidx++] = proc;
 }
 
 void AP_TimerProcess::run(void)
 {
     TCNT2 = _period;
-    if (_proc != NULL)
-        _proc();
+    for (int i = 0; i < _pidx; i++) {
+        if (_proc[i] != NULL)
+            _proc[i]();
+    }
 }
diff --git a/libraries/AP_PeriodicProcess/AP_TimerProcess.h b/libraries/AP_PeriodicProcess/AP_TimerProcess.h
index 6dd13da0da..2d9d2ae93f 100644
--- a/libraries/AP_PeriodicProcess/AP_TimerProcess.h
+++ b/libraries/AP_PeriodicProcess/AP_TimerProcess.h
@@ -8,6 +8,8 @@
 /* XXX this value is a total guess, will look up. */
 #define TIMERPROCESS_PER_DEFAULT (256)
 
+#define AP_TIMERPROCESS_MAX_PROCS 3
+
 class AP_TimerProcess : public AP_PeriodicProcess
 {
     public:
@@ -16,8 +18,9 @@ class AP_TimerProcess : public AP_PeriodicProcess
         void register_process(void (* proc)(void));
         static void run(void);
     protected:
-        static int  _period;
-        static void (*_proc)(void);
+        static int _period;
+        static ap_procedure _proc[AP_TIMERPROCESS_MAX_PROCS];
+        static int _pidx;
 };
 
 #endif // __AP_TIMERPROCESS_H__
diff --git a/libraries/AP_PeriodicProcess/PeriodicProcess.h b/libraries/AP_PeriodicProcess/PeriodicProcess.h
index 6213698521..f9ee860ffe 100644
--- a/libraries/AP_PeriodicProcess/PeriodicProcess.h
+++ b/libraries/AP_PeriodicProcess/PeriodicProcess.h
@@ -2,6 +2,8 @@
 #ifndef __PERIODICPROCESS_H__
 #define __PERIODICPROCESS_H__
 
+typedef void (*ap_procedure)(void);
+
 class AP_PeriodicProcess
 {
     public: