From 0d5e59eaa3b9de27b1ee5c37e651ae97e4ab999b Mon Sep 17 00:00:00 2001
From: pepevalbe <pepevalbe@gmail.com>
Date: Tue, 1 Dec 2015 21:22:26 +0100
Subject: [PATCH] AP_Notify: configurable RGB LED brightness

---
 libraries/AP_Notify/AP_Notify.cpp      | 20 ++++++++++++++++++++
 libraries/AP_Notify/AP_Notify.h        | 15 +++++++++++++++
 libraries/AP_Notify/NotifyDevice.h     |  5 +++++
 libraries/AP_Notify/RGBLed.cpp         | 18 +++++++++++++++++-
 libraries/AP_Notify/ToshibaLED_PX4.cpp |  2 +-
 5 files changed, 58 insertions(+), 2 deletions(-)

diff --git a/libraries/AP_Notify/AP_Notify.cpp b/libraries/AP_Notify/AP_Notify.cpp
index a38647a327..57bdd9fe02 100644
--- a/libraries/AP_Notify/AP_Notify.cpp
+++ b/libraries/AP_Notify/AP_Notify.cpp
@@ -16,6 +16,25 @@
 
 #include "AP_Notify.h"
 
+// table of user settable parameters
+const AP_Param::GroupInfo AP_Notify::var_info[] = {
+
+    // @Param: RGB_LED
+    // @DisplayName: RGB LED Brightness
+    // @Description: Select the RGB LED brightness level. When USB is connected brightness will always be low no matter the setting or OFF if that is configured.
+    // @Values: 0:Off,1:Low,2:Medium,3:High
+    // @User: Advanced
+    AP_GROUPINFO("LED_BRIGHT", 0, AP_Notify, _rgb_led_brightness, RGB_LED_HIGH),
+
+    AP_GROUPEND
+};
+
+// Default constructor
+AP_Notify::AP_Notify()
+{
+	AP_Param::setup_object_defaults(this, var_info);
+}
+
 // static flags, to allow for direct class update from device drivers
 struct AP_Notify::notify_flags_type AP_Notify::flags;
 struct AP_Notify::notify_events_type AP_Notify::events;
@@ -86,6 +105,7 @@ void AP_Notify::init(bool enable_external_leds)
     AP_Notify::flags.external_leds = enable_external_leds;
 
     for (uint8_t i = 0; i < CONFIG_NOTIFY_DEVICES_COUNT; i++) {
+        _devices[i]->pNotify = this;
         _devices[i]->init();
     }
 }
diff --git a/libraries/AP_Notify/AP_Notify.h b/libraries/AP_Notify/AP_Notify.h
index 6eaf59a9a6..965434f58e 100644
--- a/libraries/AP_Notify/AP_Notify.h
+++ b/libraries/AP_Notify/AP_Notify.h
@@ -20,6 +20,7 @@
 
 #include <AP_Common/AP_Common.h>
 #include <GCS_MAVLink/GCS_MAVLink.h>
+#include <AP_Param/AP_Param.h>
 #include "AP_BoardLED.h"
 #include "ToshibaLED.h"
 #include "ToshibaLED_I2C.h"
@@ -37,9 +38,19 @@
  # define OREOLED_ENABLED   0   // set to 1 to enable OreoLEDs
 #endif
 
+// Device parameters values
+#define RGB_LED_OFF     0
+#define RGB_LED_LOW     1
+#define RGB_LED_MEDIUM  2
+#define RGB_LED_HIGH    3
+
 class AP_Notify
 {
+    friend class RGBLed;    // RGBLed needs access to notify parameters
 public:
+    // Constructor
+    AP_Notify();
+
     /// notify_flags_type - bitmask of notification flags
     struct notify_flags_type {
         uint32_t initialising       : 1;    // 1 if initialising and copter should not be moved
@@ -94,8 +105,12 @@ public:
     // handle a LED_CONTROL message
     static void handle_led_control(mavlink_message_t* msg);
 
+    static const struct AP_Param::GroupInfo var_info[];
+
 private:
     static NotifyDevice* _devices[];
+
+    AP_Int8 _rgb_led_brightness;
 };
 
 #endif    // __AP_NOTIFY_H__
diff --git a/libraries/AP_Notify/NotifyDevice.h b/libraries/AP_Notify/NotifyDevice.h
index 2d9e8a8cd6..87b92b76ce 100644
--- a/libraries/AP_Notify/NotifyDevice.h
+++ b/libraries/AP_Notify/NotifyDevice.h
@@ -4,6 +4,8 @@
 #include <AP_Common/AP_Common.h>
 #include <GCS_MAVLink/GCS_MAVLink.h>
 
+class AP_Notify;
+
 class NotifyDevice {
 public:
     virtual ~NotifyDevice() {}
@@ -14,6 +16,9 @@ public:
     virtual void update() = 0;
     // handle a LED_CONTROL message, by default device ignore message
     virtual void handle_led_control(mavlink_message_t *msg) {}
+
+    // this pointer is used to read the parameters relative to devices
+    const AP_Notify *pNotify;
 };
 
 #endif
diff --git a/libraries/AP_Notify/RGBLed.cpp b/libraries/AP_Notify/RGBLed.cpp
index 51412685e0..b3fa48807d 100644
--- a/libraries/AP_Notify/RGBLed.cpp
+++ b/libraries/AP_Notify/RGBLed.cpp
@@ -72,6 +72,22 @@ void RGBLed::set_rgb(uint8_t red, uint8_t green, uint8_t blue)
 void RGBLed::update_colours(void)
 {
     uint8_t brightness = _led_bright;
+
+    switch (pNotify->_rgb_led_brightness) {
+    case RGB_LED_OFF:
+        brightness = _led_off;
+        break;
+    case RGB_LED_LOW:
+        brightness = _led_dim;
+        break;
+    case RGB_LED_MEDIUM:
+        brightness = _led_medium;
+        break;
+    case RGB_LED_HIGH:
+        brightness = _led_bright;
+        break;
+    }
+
     // slow rate from 50Hz to 10hz
     counter++;
     if (counter < 5) {
@@ -88,7 +104,7 @@ void RGBLed::update_colours(void)
     }
 
     // use dim light when connected through USB
-    if (hal.gpio->usb_connected()) {
+    if (hal.gpio->usb_connected() && brightness > _led_dim) {
         brightness = _led_dim;
     }
 
diff --git a/libraries/AP_Notify/ToshibaLED_PX4.cpp b/libraries/AP_Notify/ToshibaLED_PX4.cpp
index 2dc7df3ef8..d707ff3779 100644
--- a/libraries/AP_Notify/ToshibaLED_PX4.cpp
+++ b/libraries/AP_Notify/ToshibaLED_PX4.cpp
@@ -42,7 +42,7 @@ bool ToshibaLED_PX4::hw_init()
         return false;
     }
     ioctl(_rgbled_fd, RGBLED_SET_MODE, (unsigned long)RGBLED_MODE_ON);
-    last.v = 0;
+    last.v = 1;		// This is necessary so rgb value is written for the first time
     next.v = 0;
     hal.scheduler->register_io_process(FUNCTOR_BIND_MEMBER(&ToshibaLED_PX4::update_timer, void));
     return true;