From abda44e8867b1ef6eaa9c14de5ce84fcbaf19c93 Mon Sep 17 00:00:00 2001
From: Michael du Breuil <wicked.shell.scripts@gmail.com>
Date: Tue, 26 Jun 2018 10:50:11 -0700
Subject: [PATCH] AP_Notify: Move driver deletion to backend creation macro

Creates a dense array of drivers we want to update, reduces runtime
overhead, allows us to try detecting more backends then are present
---
 libraries/AP_Notify/AP_Notify.cpp | 24 +++++++++++++-----------
 libraries/AP_Notify/AP_Notify.h   |  2 ++
 2 files changed, 15 insertions(+), 11 deletions(-)

diff --git a/libraries/AP_Notify/AP_Notify.cpp b/libraries/AP_Notify/AP_Notify.cpp
index 641592cb68..1ede46a48f 100644
--- a/libraries/AP_Notify/AP_Notify.cpp
+++ b/libraries/AP_Notify/AP_Notify.cpp
@@ -114,7 +114,19 @@ struct AP_Notify::notify_events_type AP_Notify::events;
 NotifyDevice *AP_Notify::_devices[CONFIG_NOTIFY_DEVICES_MAX];
 uint8_t AP_Notify::_num_devices;
 
-#define ADD_BACKEND(backend) do { _devices[_num_devices++] = backend; if (_num_devices >= CONFIG_NOTIFY_DEVICES_MAX) return;} while(0)
+void AP_Notify::add_backend_helper(NotifyDevice *backend)
+{
+    _devices[_num_devices] = backend;
+    _devices[_num_devices]->pNotify = this;
+    if(!_devices[_num_devices]->init()) {
+        delete _devices[_num_devices];
+        _devices[_num_devices] = nullptr;
+    } else {
+      _num_devices++;
+    }
+}
+
+#define ADD_BACKEND(backend) do { add_backend_helper(backend); if (_num_devices >= CONFIG_NOTIFY_DEVICES_MAX) return;} while(0)
 
 // add notify backends to _devices array
 void AP_Notify::add_backends(void)
@@ -267,16 +279,6 @@ void AP_Notify::init(bool enable_external_leds)
 
     // add all the backends
     add_backends();
-
-    for (uint8_t i = 0; i < _num_devices; i++) {
-        if (_devices[i] != nullptr) {
-            _devices[i]->pNotify = this;
-            if(!_devices[i]->init()) {
-                delete _devices[i];
-                _devices[i] = nullptr;
-            }
-        }
-    }
 }
 
 // main update function, called at 50Hz
diff --git a/libraries/AP_Notify/AP_Notify.h b/libraries/AP_Notify/AP_Notify.h
index 4d3c4821b7..4de4ee39c8 100644
--- a/libraries/AP_Notify/AP_Notify.h
+++ b/libraries/AP_Notify/AP_Notify.h
@@ -150,6 +150,8 @@ private:
 
     static AP_Notify *_instance;
 
+    void add_backend_helper(NotifyDevice *backend);
+
     // parameters
     AP_Int8 _rgb_led_brightness;
     AP_Int8 _rgb_led_override;