AP_Notify: added NTF_LED_OVERRIDE parameter

when this is set the board RGB LED will be controlled by MAVLink
instead of internally. This is useful for cases where the LED patterns
and colours needed are specified by an external authority (such as the
OBC organisers)

libraries/AP_Notify/AP_Notify.cpp

@@ -49,6 +49,13 @@ const AP_Param::GroupInfo AP_Notify::var_info[] = {
     // @User: Advanced
     AP_GROUPINFO("BUZZ_ENABLE", 1, AP_Notify, _buzzer_enable, BUZZER_ON),
 
+    // @Param: LED_OVERRIDE
+    // @DisplayName: Setup for MAVLink LED override
+    // @Description: This sets up the board RGB LED for override by MAVLink. Normal notify LED control is disabled
+    // @Values: 0:Disable,1:Enable
+    // @User: Advanced
+    AP_GROUPINFO("LED_OVERRIDE", 2, AP_Notify, _rgb_led_override, 0),
+    
     AP_GROUPEND
 };
 

libraries/AP_Notify/AP_Notify.h

@@ -114,5 +114,6 @@ private:
     static NotifyDevice* _devices[];
 
     AP_Int8 _rgb_led_brightness;
+    AP_Int8 _rgb_led_override;
     AP_Int8 _buzzer_enable;
 };

libraries/AP_Notify/RGBLed.cpp

@@ -48,13 +48,8 @@ bool RGBLed::init()
 }
 
 // set_rgb - set color as a combination of red, green and blue values
-void RGBLed::set_rgb(uint8_t red, uint8_t green, uint8_t blue)
+void RGBLed::_set_rgb(uint8_t red, uint8_t green, uint8_t blue)
 {
-    // return immediately if not enabled
-    if (!_healthy) {
-        return;
-    }
-
     if (red != _red_curr ||
         green != _green_curr ||
         blue != _blue_curr) {
@@ -67,6 +62,20 @@ void RGBLed::set_rgb(uint8_t red, uint8_t green, uint8_t blue)
     }
 }
 
+// set_rgb - set color as a combination of red, green and blue values
+void RGBLed::set_rgb(uint8_t red, uint8_t green, uint8_t blue)
+{
+    // return immediately if not enabled
+    if (!_healthy) {
+        return;
+    }
+    if (pNotify->_rgb_led_override) {
+        // don't set if in override mode
+        return;
+    }
+    _set_rgb(red, green, blue);
+}
+
 
 // _scheduled_update - updates _red, _green, _blue according to notify flags
 void RGBLed::update_colours(void)
@@ -323,6 +332,66 @@ void RGBLed::update()
     if (!_healthy) {
         return;
     }
-    update_colours();
-    set_rgb(_red_des, _green_des, _blue_des);
+    if (!pNotify->_rgb_led_override) {
+        update_colours();
+        set_rgb(_red_des, _green_des, _blue_des);
+    } else {
+        update_override();
+    }
+}
+
+/*
+  handle LED control, only used when LED_OVERRIDE=1
+*/
+void RGBLed::handle_led_control(mavlink_message_t *msg)
+{
+    if (!pNotify->_rgb_led_override) {
+        // ignore LED_CONTROL commands if not in LED_OVERRIDE mode
+        return;
+    }
+
+    // decode mavlink message
+    mavlink_led_control_t packet;
+    mavlink_msg_led_control_decode(msg, &packet);
+
+    _led_override.start_ms = AP_HAL::millis();
+    
+    switch (packet.custom_len) {
+    case 3:
+        _led_override.rate_hz = 0;
+        _led_override.r = packet.custom_bytes[0];
+        _led_override.g = packet.custom_bytes[1];
+        _led_override.b = packet.custom_bytes[2];
+        break;
+    case 4:
+        _led_override.rate_hz = packet.custom_bytes[3];
+        _led_override.r = packet.custom_bytes[0];
+        _led_override.g = packet.custom_bytes[1];
+        _led_override.b = packet.custom_bytes[2];
+        break;
+    default:
+        // not understood
+        break;
+    }
+}
+
+/*
+  update LED when in override mode
+ */
+void RGBLed::update_override(void)
+{
+    if (_led_override.rate_hz == 0) {
+        // solid colour
+        _set_rgb(_led_override.r, _led_override.g, _led_override.b);
+        return;
+    }
+    // blinking
+    uint32_t ms_per_cycle = 1000 / _led_override.rate_hz;
+    uint32_t cycle = (AP_HAL::millis() - _led_override.start_ms) % ms_per_cycle;
+    if (cycle > ms_per_cycle / 2) {
+        // on
+        _set_rgb(_led_override.r, _led_override.g, _led_override.b);
+    } else {
+        _set_rgb(0, 0, 0);
+    }
 }

libraries/AP_Notify/RGBLed.h

@@ -39,11 +39,19 @@ public:
     // called at 50Hz
     virtual void update();
 
+    // handle LED control, only used when LED_OVERRIDE=1
+    virtual void handle_led_control(mavlink_message_t *msg) override;
+    
 protected:
     // methods implemented in hardware specific classes
     virtual bool hw_init(void) = 0;
     virtual bool hw_set_rgb(uint8_t red, uint8_t green, uint8_t blue) = 0;
 
+    // set_rgb - set color as a combination of red, green and blue levels from 0 ~ 15
+    virtual void _set_rgb(uint8_t red, uint8_t green, uint8_t blue);
+
+    virtual void update_override();
+    
     // meta-data common to all hw devices
     uint8_t counter;
     uint8_t step;
@@ -54,6 +62,13 @@ protected:
     uint8_t _led_bright;
     uint8_t _led_medium;
     uint8_t _led_dim;
+
+    struct {
+        uint8_t r, g, b;
+        uint8_t rate_hz;
+        uint32_t start_ms;
+    } _led_override;
+    
 private:
     virtual void update_colours();
 };