AP_PeriodicProcess: AP_TimerProcess can register multiple callbacks.

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]();
+    }
 }

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]();
+    }
 }

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:
@@ -17,7 +19,8 @@ class AP_TimerProcess : public AP_PeriodicProcess
         static void run(void);
     protected:
         static int _period;
-        static void (*_proc)(void);
+        static ap_procedure _proc[AP_TIMERPROCESS_MAX_PROCS];
+        static int _pidx;
 };
 
 #endif // __AP_TIMERPROCESS_H__

libraries/AP_PeriodicProcess/PeriodicProcess.h

@@ -2,6 +2,8 @@
 #ifndef __PERIODICPROCESS_H__
 #define __PERIODICPROCESS_H__
 
+typedef void (*ap_procedure)(void);
+
 class AP_PeriodicProcess
 {
     public: