applied led stash

applied formatting change from pr comment
ArduPilot · Nov 23, 2023 · 16a40fc · 16a40fc
1 parent c230b0c
commit 16a40fc
Show file tree

Hide file tree

Showing 3 changed files with 28 additions and 6 deletions.
diff --git a/Tools/AP_Periph/can.cpp b/Tools/AP_Periph/can.cpp
@@ -544,10 +544,19 @@ void AP_Periph_FW::handle_arming_status(CanardInstance* canard_instance, CanardR
 
 
 #if defined(AP_PERIPH_HAVE_LED_WITHOUT_NOTIFY) || defined(HAL_PERIPH_ENABLE_NOTIFY)
-void AP_Periph_FW::set_rgb_led(uint8_t red, uint8_t green, uint8_t blue)
-{
+    void AP_Periph_FW::set_rgb_led(uint8_t red, uint8_t green, uint8_t blue)
+    {
+        uint8_t light_id = -1;
+        set_rgb_led(red, green, blue, light_id);
+    }
+
+    void AP_Periph_FW::set_rgb_led(uint8_t red, uint8_t green, uint8_t blue, uint8_t light_id){
 #ifdef HAL_PERIPH_ENABLE_NOTIFY
-    notify.handle_rgb(red, green, blue);
+        if(light_id > 0) {
+            notify.handle_rgb_id(red, green, blue, light_id);
+        } else {
+            notify.handle_rgb(red, green, blue);
+        }
 #ifdef HAL_PERIPH_ENABLE_RC_OUT
     rcout_has_new_data_to_update = true;
 #endif // HAL_PERIPH_ENABLE_RC_OUT
@@ -631,13 +640,15 @@ void AP_Periph_FW::set_rgb_led(uint8_t red, uint8_t green, uint8_t blue)
 void AP_Periph_FW::handle_lightscommand(CanardInstance* canard_instance, CanardRxTransfer* transfer)
 {
     uavcan_equipment_indication_LightsCommand req;
+
     if (uavcan_equipment_indication_LightsCommand_decode(transfer, &req)) {
         return;
     }
     for (uint8_t i=0; i<req.commands.len; i++) {
         uavcan_equipment_indication_SingleLightCommand &cmd = req.commands.data[i];
         // to get the right color proportions we scale the green so that is uses the
         // same number of bits as red and blue
+        uint8_t light_id = cmd.light_id;
         uint8_t red = cmd.color.red<<3U;
         uint8_t green = (cmd.color.green>>1U)<<3U;
         uint8_t blue = cmd.color.blue<<3U;
@@ -652,7 +663,7 @@ void AP_Periph_FW::handle_lightscommand(CanardInstance* canard_instance, CanardR
             green = constrain_int16(green * scale, 0, 255);
             blue = constrain_int16(blue * scale, 0, 255);
         }
-        set_rgb_led(red, green, blue);
+        set_rgb_led(red, green, blue, light_id);
     }
 }
 #endif // AP_PERIPH_HAVE_LED_WITHOUT_NOTIFY

diff --git a/libraries/AP_Notify/SerialLED.cpp b/libraries/AP_Notify/SerialLED.cpp
@@ -30,8 +30,18 @@ bool SerialLED::init()
     return true;
 }
 
+void SerialLED::rgb_set_id(uint8_t red, uint8_t green, uint8_t blue, uint8_t id)
+{
+    _set_rgb_local(red, green, blue, id);
+}
+
 bool SerialLED::hw_set_rgb(uint8_t red, uint8_t green, uint8_t blue)
 {
+    uint8_t id = -1;
+    return _set_rgb_local(red, green, blue, id);
+}
+
+bool SerialLED::_set_rgb_local(uint8_t red, uint8_t green, uint8_t blue, uint8_t id){
     if (enable_mask == 0) {
         // nothing is enabled, no pins set as LED output
         return true;
@@ -44,7 +54,7 @@ bool SerialLED::hw_set_rgb(uint8_t red, uint8_t green, uint8_t blue)
 
     for (uint16_t chan=0; chan<16; chan++) {
         if ((1U<<chan) & enable_mask) {
-            led->set_RGB(chan+1, -1, red, green, blue);
+            led->set_RGB(chan+1, id, red, green, blue);
         }
     }
 
@@ -56,5 +66,4 @@ bool SerialLED::hw_set_rgb(uint8_t red, uint8_t green, uint8_t blue)
 
     return true;
 }
-
 #endif  // AP_NOTIFY_SERIALLED_ENABLED
diff --git a/libraries/AP_Notify/SerialLED.h b/libraries/AP_Notify/SerialLED.h
@@ -39,9 +39,11 @@ class SerialLED: public RGBLed {
 protected:
 
     bool hw_set_rgb(uint8_t r, uint8_t g, uint8_t b) override;
+    void rgb_set_id(uint8_t r, uint8_t g, uint8_t b, uint8_t id) override;
 
 private:
     uint16_t enable_mask;
+    bool _set_rgb_local(uint8_t red, uint8_t green, uint8_t blue, uint8_t lught_id);
 
     HAL_Semaphore _sem;
 };