From a45041ce5dc9921295d0352008d13e398e306564 Mon Sep 17 00:00:00 2001
From: Peter Hall <33176108+IamPete1@users.noreply.github.com>
Date: Mon, 9 Dec 2019 21:06:35 +0000
Subject: [PATCH] AP_Notify: add handle rgb

---
 libraries/AP_Notify/AP_Notify.cpp  | 12 +++++++++++-
 libraries/AP_Notify/AP_Notify.h    |  3 +++
 libraries/AP_Notify/NotifyDevice.h |  4 ++++
 libraries/AP_Notify/RGBLed.cpp     | 12 ++++++++++++
 libraries/AP_Notify/RGBLed.h       |  6 +++++-
 5 files changed, 35 insertions(+), 2 deletions(-)

diff --git a/libraries/AP_Notify/AP_Notify.cpp b/libraries/AP_Notify/AP_Notify.cpp
index 8691922e89..c5123ae8b4 100644
--- a/libraries/AP_Notify/AP_Notify.cpp
+++ b/libraries/AP_Notify/AP_Notify.cpp
@@ -113,7 +113,7 @@ const AP_Param::GroupInfo AP_Notify::var_info[] = {
     // @Param: LED_OVERRIDE
     // @DisplayName: Specifies colour source for the RGBLed
     // @Description: Specifies the source for the colours and brightness for the LED.  OutbackChallenge conforms to the MedicalExpress (https://uavchallenge.org/medical-express/) rules, essentially "Green" is disarmed (safe-to-approach), "Red" is armed (not safe-to-approach). Traffic light is a simplified color set, red when armed, yellow when the safety switch is not surpressing outputs (but disarmed), and green when outputs are surpressed and disarmed, the LED will blink faster if disarmed and failing arming checks.
-    // @Values: 0:Standard,1:MAVLink,2:OutbackChallenge,3:TrafficLight
+    // @Values: 0:Standard,1:MAVLink/Scripting,2:OutbackChallenge,3:TrafficLight
     // @User: Advanced
     AP_GROUPINFO("LED_OVERRIDE", 2, AP_Notify, _rgb_led_override, 0),
 
@@ -352,6 +352,16 @@ void AP_Notify::handle_led_control(const mavlink_message_t &msg)
     }
 }
 
+// handle RGB from Scripting
+void AP_Notify::handle_rgb(uint8_t r, uint8_t g, uint8_t b, uint8_t rate_hz)
+{
+    for (uint8_t i = 0; i < _num_devices; i++) {
+        if (_devices[i] != nullptr) {
+            _devices[i]->rgb_control(r, g, b, rate_hz);
+        }
+    }
+}
+
 // handle a PLAY_TUNE message
 void AP_Notify::handle_play_tune(const mavlink_message_t &msg)
 {
diff --git a/libraries/AP_Notify/AP_Notify.h b/libraries/AP_Notify/AP_Notify.h
index a1fd98c1b2..e85f572147 100644
--- a/libraries/AP_Notify/AP_Notify.h
+++ b/libraries/AP_Notify/AP_Notify.h
@@ -138,6 +138,9 @@ public:
     // handle a LED_CONTROL message
     static void handle_led_control(const mavlink_message_t &msg);
 
+    // handle RGB from Scripting
+    static void handle_rgb(uint8_t r, uint8_t g, uint8_t b, uint8_t rate_hz = 0);
+
     // handle a PLAY_TUNE message
     static void handle_play_tune(const mavlink_message_t &msg);
 
diff --git a/libraries/AP_Notify/NotifyDevice.h b/libraries/AP_Notify/NotifyDevice.h
index 090ac759f9..b2e91e849c 100644
--- a/libraries/AP_Notify/NotifyDevice.h
+++ b/libraries/AP_Notify/NotifyDevice.h
@@ -23,6 +23,10 @@ public:
     // play a MML tune
     virtual void play_tune(const char *tune) {}
 
+    // RGB control
+    // give RGB and flash rate, used with scripting
+    virtual void rgb_control(uint8_t r, uint8_t g, uint8_t b, uint8_t rate_hz) {}
+
     // this pointer is used to read the parameters relative to devices
     const AP_Notify *pNotify;
 };
diff --git a/libraries/AP_Notify/RGBLed.cpp b/libraries/AP_Notify/RGBLed.cpp
index a20c17d6fa..f8799a931c 100644
--- a/libraries/AP_Notify/RGBLed.cpp
+++ b/libraries/AP_Notify/RGBLed.cpp
@@ -281,3 +281,15 @@ void RGBLed::update_override(void)
         _set_rgb(0, 0, 0);
     }
 }
+
+/*
+  RGB control
+  give RGB and flash rate, used with scripting
+*/
+void RGBLed::rgb_control(uint8_t r, uint8_t g, uint8_t b, uint8_t rate_hz)
+{
+    _led_override.rate_hz = rate_hz;
+    _led_override.r = r;
+    _led_override.g = g;
+    _led_override.b = b;
+}
diff --git a/libraries/AP_Notify/RGBLed.h b/libraries/AP_Notify/RGBLed.h
index 6b4bb54bc7..9786249f09 100644
--- a/libraries/AP_Notify/RGBLed.h
+++ b/libraries/AP_Notify/RGBLed.h
@@ -38,7 +38,11 @@ public:
 
     // handle LED control, only used when LED_OVERRIDE=1
     virtual void handle_led_control(const mavlink_message_t &msg) override;
-    
+
+    // RGB control
+    // give RGB and flash rate, used with scripting
+    virtual void rgb_control(uint8_t r, uint8_t g, uint8_t b, uint8_t rate_hz) override;
+
 protected:
     // methods implemented in hardware specific classes
     virtual bool hw_init(void) = 0;