From 62d87623b7b2016ca7e129fcb9a8a31e1e48a6e5 Mon Sep 17 00:00:00 2001
From: yaozb <yaozb04@163.com>
Date: Fri, 8 May 2020 08:45:10 +0800
Subject: [PATCH] change rgbled to zr rgb mode

---
 libraries/AP_Notify/UAVCAN_RGB_LED.cpp        | 10 ++++++++-
 libraries/AP_Notify/UAVCAN_RGB_LED.h          |  3 ++-
 libraries/AP_UAVCAN/AP_UAVCAN.cpp             | 21 +++++++++++++++++++
 libraries/AP_UAVCAN/AP_UAVCAN.h               |  2 ++
 .../equipment/indication/30100.ZrRGB.uavcan   |  5 +++++
 5 files changed, 39 insertions(+), 2 deletions(-)
 create mode 100755 libraries/AP_UAVCAN/dsdl/zrzk/equipment/indication/30100.ZrRGB.uavcan

diff --git a/libraries/AP_Notify/UAVCAN_RGB_LED.cpp b/libraries/AP_Notify/UAVCAN_RGB_LED.cpp
index 7fe5794b6a..84c4a24749 100644
--- a/libraries/AP_Notify/UAVCAN_RGB_LED.cpp
+++ b/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
diff --git a/libraries/AP_Notify/UAVCAN_RGB_LED.h b/libraries/AP_Notify/UAVCAN_RGB_LED.h
index 210ffa57b4..4b176a4397 100644
--- a/libraries/AP_Notify/UAVCAN_RGB_LED.h
+++ b/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;
 };
 
diff --git a/libraries/AP_UAVCAN/AP_UAVCAN.cpp b/libraries/AP_UAVCAN/AP_UAVCAN.cpp
index b670847eff..e51b0ec180 100644
--- a/libraries/AP_UAVCAN/AP_UAVCAN.cpp
+++ b/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
diff --git a/libraries/AP_UAVCAN/AP_UAVCAN.h b/libraries/AP_UAVCAN/AP_UAVCAN.h
index 54377d6bf8..5e300afc3d 100644
--- a/libraries/AP_UAVCAN/AP_UAVCAN.h
+++ b/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:
diff --git a/libraries/AP_UAVCAN/dsdl/zrzk/equipment/indication/30100.ZrRGB.uavcan b/libraries/AP_UAVCAN/dsdl/zrzk/equipment/indication/30100.ZrRGB.uavcan
new file mode 100755
index 0000000000..2d63d0f537
--- /dev/null
+++ b/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