AP_HAL_AVR: add ISRRegistry class, member of HAL_AVR

* derived from Arduino_Mega_ISR_Registry class

libraries/AP_HAL_AVR/AP_HAL_AVR_Namespace.h

@@ -19,6 +19,7 @@ namespace AP_HAL_AVR {
     class APM1RCOutput;
     class APM2RCOutput;
     class ArduinoScheduler;
+    class ISRRegistry;
 }
 
 #endif //__AP_HAL_AVR_NAMESPACE_H__

libraries/AP_HAL_AVR/HAL_AVR.h

@@ -4,6 +4,7 @@
 
 #include <AP_HAL.h>
 #include "AP_HAL_AVR_Namespace.h"
+#include "utility/ISRRegistry.h"
 
 /**
  * HAL_AVR class derives from HAL but provides an AVR-specific
@@ -33,6 +34,7 @@ public:
                         _rcout, _scheduler) {}
 
     void init(void* opts) const;
+    AP_HAL_AVR::ISRRegistry isr_registry;
 };
 #endif // __AP_HAL_AVR_HAL_AVR_H__
 

libraries/AP_HAL_AVR/utility/ISRRegistry.cpp

@@ -0,0 +1,52 @@
+
+#include <stdlib.h>
+#include <avr/interrupt.h>
+
+#include "ISRRegistry.h"
+
+using namespace AP_HAL_AVR;
+
+proc_ptr ISRRegistry::_registry[ISR_REGISTRY_NUM_SLOTS];
+
+void ISRRegistry::init() {
+    for(int i = 0; i < ISR_REGISTRY_NUM_SLOTS; i++) {
+        _registry[i] = NULL;
+    }
+}
+
+int ISRRegistry::register_signal(int signal, proc_ptr proc)
+{
+    if (signal >= 0 && signal < ISR_REGISTRY_NUM_SLOTS) {
+        _registry[signal] = proc;
+        return 0;
+    }
+    return -1;
+}
+
+int ISRRegistry::unregister_signal(int signal)
+{
+    if (signal >= 0 && signal < ISR_REGISTRY_NUM_SLOTS) {
+        _registry[signal] = NULL;
+        return 0;
+    }
+    return -1;
+}
+
+/* ========== ISR IMPLEMENTATIONS ========== */
+
+extern "C" ISR(TIMER2_OVF_vect) {
+    if ( ISRRegistry::_registry[ISR_REGISTRY_TIMER2_OVF] != NULL)
+         ISRRegistry::_registry[ISR_REGISTRY_TIMER2_OVF]();
+}
+
+
+extern "C" ISR(TIMER4_CAPT_vect) {
+    if ( ISRRegistry::_registry[ISR_REGISTRY_TIMER4_CAPT] != NULL)
+         ISRRegistry::_registry[ISR_REGISTRY_TIMER4_CAPT]();
+}
+
+extern "C" ISR(TIMER5_CAPT_vect) {
+    if ( ISRRegistry::_registry[ISR_REGISTRY_TIMER5_CAPT] != NULL)
+         ISRRegistry::_registry[ISR_REGISTRY_TIMER5_CAPT]();
+}
+

libraries/AP_HAL_AVR/utility/ISRRegistry.h

@@ -0,0 +1,24 @@
+
+
+#ifndef __AP_HAL_AVR_ISR_REGISTRY_H__
+#define __AP_HAL_AVR_ISR_REGISTRY_H__
+
+#include "AP_HAL_AVR_Namespace.h"
+
+#define ISR_REGISTRY_TIMER2_OVF  0
+#define ISR_REGISTRY_TIMER4_CAPT 1
+#define ISR_REGISTRY_TIMER5_CAPT 2
+#define ISR_REGISTRY_NUM_SLOTS   3
+
+typedef void (*proc_ptr)(void);
+
+class AP_HAL_AVR::ISRRegistry {
+public:
+    void init();
+    int register_signal(int isr_number, proc_ptr proc);
+    int unregister_signal(int isr_number);
+    
+    static proc_ptr _registry[ISR_REGISTRY_NUM_SLOTS];
+};
+
+#endif // __AP_HAL_AVR_ISR_REGISTRY_H__