change rgbled to zr rgb mode

libraries/AP_Notify/UAVCAN_RGB_LED.cpp

@@ -57,9 +57,17 @@ bool UAVCAN_RGB_LED::hw_set_rgb(uint8_t red, uint8_t green, uint8_t blue)
     for (uint8_t i = 0; i < can_num_drivers; i++) {
         AP_UAVCAN *uavcan = AP_UAVCAN::get_uavcan(i);
         if (uavcan != nullptr) {
-            success = uavcan->led_write(_led_index, red, green, blue) || success;
+           // success = uavcan->led_write(_led_index, red, green, blue) || success;
+           success = uavcan->send_zr_rgb_led(rgb_mode,red, green, blue) || success;//(_led_index, red, green, blue) || success
         }
     }
     return success;
 }
+
+void UAVCAN_RGB_LED::set_rgb_mode(uint8_t mode)
+{
+    rgb_mode = mode;
+}
+
+
 #endif

libraries/AP_Notify/UAVCAN_RGB_LED.h

@@ -11,8 +11,9 @@ public:
 protected:
     bool hw_init() override;
     virtual bool hw_set_rgb(uint8_t red, uint8_t green, uint8_t blue) override;
-
+    void set_rgb_mode(uint8_t mode) override;
 private:
     uint8_t _led_index;
+     uint8_t rgb_mode;
 };
 

libraries/AP_UAVCAN/AP_UAVCAN.cpp

@@ -41,6 +41,8 @@
 #include <ardupilot/equipment/trafficmonitor/TrafficReport.hpp>
 #include <uavcan/equipment/gnss/RTCMStream.hpp>
 
+#include<zrzk/equipment/indication/ZrRGB.hpp>
+
 #include <AP_Baro/AP_Baro_UAVCAN.h>
 #include <AP_RangeFinder/AP_RangeFinder_UAVCAN.h>
 #include <AP_GPS/AP_GPS_UAVCAN.h>
@@ -109,6 +111,8 @@ static uavcan::Publisher<uavcan::equipment::indication::BeepCommand>* buzzer[MAX
 static uavcan::Publisher<ardupilot::indication::SafetyState>* safety_state[MAX_NUMBER_OF_CAN_DRIVERS];
 static uavcan::Publisher<uavcan::equipment::gnss::RTCMStream>* rtcm_stream[MAX_NUMBER_OF_CAN_DRIVERS];
 
+static uavcan::Publisher<zrzk::equipment::indication::ZrRGB>* zr_rgb_led[MAX_NUMBER_OF_CAN_DRIVERS];
+
 // subscribers
 
 // handler SafteyButton
@@ -262,6 +266,12 @@ void AP_UAVCAN::init(uint8_t driver_index, bool enable_filters)
     rtcm_stream[driver_index] = new uavcan::Publisher<uavcan::equipment::gnss::RTCMStream>(*_node);
     rtcm_stream[driver_index]->setTxTimeout(uavcan::MonotonicDuration::fromMSec(20));
     rtcm_stream[driver_index]->setPriority(uavcan::TransferPriority::OneHigherThanLowest);
+
+
+    zr_rgb_led[driver_index] = new uavcan::Publisher<zrzk::equipment::indication::ZrRGB>(*_node);
+    zr_rgb_led[driver_index]->setTxTimeout(uavcan::MonotonicDuration::fromMSec(20));
+    zr_rgb_led[driver_index]->setPriority(uavcan::TransferPriority::OneHigherThanLowest);
+
     
     safety_button_listener[driver_index] = new uavcan::Subscriber<ardupilot::indication::Button, ButtonCb>(*_node);
     if (safety_button_listener[driver_index]) {
@@ -712,4 +722,15 @@ void AP_UAVCAN::handle_traffic_report(AP_UAVCAN* ap_uavcan, uint8_t node_id, con
     adsb->handle_adsb_vehicle(vehicle);
 }
 
+bool AP_UAVCAN::send_zr_rgb_led(uint8_t mode, uint8_t red, uint8_t green, uint8_t blue){
+  zrzk::equipment::indication::ZrRGB  msg;
+  msg.rgb_mode = mode;
+  msg.rgb_r = red;
+  msg.rgb_g = green;
+  msg.rgb_b = blue;
+  zr_rgb_led[_driver_index]->broadcast(msg);
+  return true;
+}
+
+
 #endif // HAL_WITH_UAVCAN

libraries/AP_UAVCAN/AP_UAVCAN.h

@@ -91,6 +91,8 @@ public:
     // send RTCMStream packets
     void send_RTCMStream(const uint8_t *data, uint32_t len);
 
+    bool send_zr_rgb_led(uint8_t mode, uint8_t red, uint8_t green, uint8_t blue);
+
     template <typename DataType_>
     class RegistryBinder {
     protected:

libraries/AP_UAVCAN/dsdl/zrzk/equipment/indication/30100.ZrRGB.uavcan

@@ -0,0 +1,5 @@
+  
+uint8 rgb_mode
+uint8 rgb_r
+uint8 rgb_g
+uint8 rgb_b