led 0 = all leds

diff --git a/src/hw_node.cpp b/src/hw_node.cpp
index 18d1ee6115a2345dc3a5e4bf277b446532bbb5ea..bc0e2a216b7e37ac37639a5329152d6b37526b3c 100644 (file)
--- a/src/hw_node.cpp
+++ b/src/hw_node.cpp
@@ -160,10 +160,17 @@ class A4wd3 : public rclcpp::Node {
                        }
                }
 
+               void write_led(const uint8_t num, const std::vector<uint8_t> buf) {
+                       int ret = i2c_write_reg(0x62, num, buf);
+                       if (ret != 0) {
+                               RCLCPP_ERROR(this->get_logger(), "Failed to write to LED Stripe err=%d", ret);
+                       }
+               }
+
                // ros2 topic pub -1 /led_stripe a4wd3/msg/LedStripe "{leds: [{num: 1, ww: 1}]}"
                void led_stripe_callback(const a4wd3::msg::LedStripe::SharedPtr msg) {
                        for (size_t i=0; i<msg->leds.size(); i++) {
-                               if (msg->leds[i].num < 1 || msg->leds[i].num > 10) {
+                               if (msg->leds[i].num > 10) {
                                        RCLCPP_ERROR(this->get_logger(), "Led %d out of range 1..10", msg->leds[i].num);
                                        continue;
                                }
@@ -174,10 +181,14 @@ class A4wd3 : public rclcpp::Node {
                                buf.push_back(msg->leds[i].ww);
                                buf.push_back(msg->leds[i].cw);
                                buf.push_back(i == msg->leds.size()-1 ? 0x1 : 0x2); // commit
-                               int ret = i2c_write_reg(0x62, msg->leds[i].num, buf);
-                               if (ret != 0) {
-                                       RCLCPP_ERROR(this->get_logger(), "Failed to write to LED Stripe err=%d", ret);
-                                       break;
+                               if (msg->leds[i].num == 0) {
+                                       // 0 = all leds
+                                       for(size_t k = 1; k<=10; k++) {
+                                               buf[5] = k == 10 ? 0x1 : 0x2;
+                                               write_led(k, buf);
+                                       }
+                               } else {
+                                       write_led(msg->leds[i].num, buf);
                                }
                        }
                }