From 7f177abae84fa9a941ec2f2e9e6e415aa4904211 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Sat, 24 Jun 2023 20:01:15 +1000
Subject: [PATCH] AP_Notify: fixed DroneCAN LEDs

---
 libraries/AP_Notify/DroneCAN_RGB_LED.cpp | 28 +++++++++---------------
 libraries/AP_Notify/DroneCAN_RGB_LED.h   |  2 --
 2 files changed, 10 insertions(+), 20 deletions(-)

diff --git a/libraries/AP_Notify/DroneCAN_RGB_LED.cpp b/libraries/AP_Notify/DroneCAN_RGB_LED.cpp
index b06387a5db..5cb4e0cfe3 100644
--- a/libraries/AP_Notify/DroneCAN_RGB_LED.cpp
+++ b/libraries/AP_Notify/DroneCAN_RGB_LED.cpp
@@ -44,39 +44,31 @@ DroneCAN_RGB_LED::DroneCAN_RGB_LED(uint8_t led_off,
 
 bool DroneCAN_RGB_LED::init()
 {
-    const uint8_t can_num_drivers = AP::can().get_num_drivers();
-    for (uint8_t i = 0; i < can_num_drivers; i++) {
-        AP_DroneCAN *uavcan = AP_DroneCAN::get_dronecan(i);
-        if (uavcan != nullptr) {
-            return true;
-        }
-    }
-    // no UAVCAN drivers
-    return false;
+    // LEDs can turn up later
+    return true;
 }
 
 
 bool DroneCAN_RGB_LED::hw_set_rgb(uint8_t red, uint8_t green, uint8_t blue)
 {
-    bool success = false;
+    uavcan_equipment_indication_LightsCommand msg {};
     msg.commands.len = 1;
     msg.commands.data[0].light_id =0;
     msg.commands.data[0].color.red = red >> 3;
     msg.commands.data[0].color.green = green >> 2;
     msg.commands.data[0].color.blue = blue >> 3;
-    return success;
-}
 
-void DroneCAN_RGB_LED::update()
-{
     // broadcast the message on all ifaces
     uint8_t can_num_drivers = AP::can().get_num_drivers();
+    bool ok = false;
     for (uint8_t i = 0; i < can_num_drivers; i++) {
-        AP_DroneCAN *uavcan = AP_DroneCAN::get_dronecan(i);
-        if (uavcan != nullptr) {
-            uavcan->rgb_led.broadcast(msg);
+        auto *dronecan = AP_DroneCAN::get_dronecan(i);
+        if (dronecan != nullptr) {
+            ok |= dronecan->rgb_led.broadcast(msg);
         }
     }
+    return ok;
 }
 
-#endif
+#endif // HAL_ENABLE_DRONECAN_DRIVERS
+
diff --git a/libraries/AP_Notify/DroneCAN_RGB_LED.h b/libraries/AP_Notify/DroneCAN_RGB_LED.h
index a00e98d47f..e4da1b65b9 100644
--- a/libraries/AP_Notify/DroneCAN_RGB_LED.h
+++ b/libraries/AP_Notify/DroneCAN_RGB_LED.h
@@ -11,9 +11,7 @@ public:
                    uint8_t led_medium, uint8_t led_dim);
     DroneCAN_RGB_LED();
     bool init() override;
-    void update() override;
 protected:
     virtual bool hw_set_rgb(uint8_t red, uint8_t green, uint8_t blue) override;
-    uavcan_equipment_indication_LightsCommand msg;
 };
 #endif