AP_Notify: add OBC colour scheme

Gers963 · Mar 5, 2019 · 2006ecf · 2006ecf
1 parent f8ac5a5
commit 2006ecf
Show file tree

Hide file tree

Showing 3 changed files with 45 additions and 21 deletions.
diff --git a/libraries/AP_Notify/AP_Notify.cpp b/libraries/AP_Notify/AP_Notify.cpp
@@ -109,9 +109,9 @@ const AP_Param::GroupInfo AP_Notify::var_info[] = {
     AP_GROUPINFO("BUZZ_ENABLE", 1, AP_Notify, _buzzer_enable, BUZZER_ENABLE_DEFAULT),
 
     // @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
+    // @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).
+    // @Values: 0:Standard,1:MAVLink,2:OutbackChallenge
     // @User: Advanced
     AP_GROUPINFO("LED_OVERRIDE", 2, AP_Notify, _rgb_led_override, 0),
 

diff --git a/libraries/AP_Notify/RGBLed.cpp b/libraries/AP_Notify/RGBLed.cpp
@@ -52,10 +52,15 @@ void RGBLed::_set_rgb(uint8_t red, uint8_t green, uint8_t blue)
     }
 }
 
+RGBLed::rgb_source_t RGBLed::rgb_source() const
+{
+    return rgb_source_t(pNotify->_rgb_led_override.get());
+}
+
 // 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)
 {
-    if (pNotify->_rgb_led_override) {
+    if (rgb_source() == mavlink) {
         // don't set if in override mode
         return;
     }
@@ -88,6 +93,14 @@ uint8_t RGBLed::get_brightness(void) const
     return brightness;
 }
 
+uint32_t RGBLed::get_colour_sequence_obc(void) const
+{
+    if (AP_Notify::flags.armed) {
+        return DEFINE_COLOUR_SEQUENCE_SOLID(RED);
+    }
+    return DEFINE_COLOUR_SEQUENCE_SOLID(GREEN);
+}
+
 // _scheduled_update - updates _red, _green, _blue according to notify flags
 uint32_t RGBLed::get_colour_sequence(void) const
 {
@@ -149,12 +162,25 @@ uint32_t RGBLed::get_colour_sequence(void) const
     return sequence_disarmed_bad_gps;
 }
 
-// _scheduled_update - updates _red, _green, _blue according to notify flags
-void RGBLed::update_colours(void)
+// update - updates led according to timed_updated.  Should be called
+// at 50Hz
+void RGBLed::update()
 {
-    const uint8_t brightness = get_brightness();
+    uint32_t current_colour_sequence = 0;
 
-    const uint32_t current_colour_sequence = get_colour_sequence();
+    switch (rgb_source()) {
+    case mavlink:
+        update_override();
+        return; // note this is a return not a break!
+    case standard:
+        current_colour_sequence = get_colour_sequence();
+        break;
+    case obc:
+        current_colour_sequence = get_colour_sequence_obc();
+        break;
+    }
+
+    const uint8_t brightness = get_brightness();
 
     uint8_t step = (AP_HAL::millis()/100) % 10;
 
@@ -169,26 +195,16 @@ void RGBLed::update_colours(void)
     _red_des = (colour & RED) ? brightness : 0;
     _green_des = (colour & GREEN) ? brightness : 0;
     _blue_des = (colour & BLUE) ? brightness : 0;
-}
 
-// update - updates led according to timed_updated.  Should be called
-// at 50Hz
-void RGBLed::update()
-{
-    if (!pNotify->_rgb_led_override) {
-        update_colours();
-        set_rgb(_red_des, _green_des, _blue_des);
-    } else {
-        update_override();
-    }
+    set_rgb(_red_des, _green_des, _blue_des);
 }
 
 /*
   handle LED control, only used when LED_OVERRIDE=1
 */
 void RGBLed::handle_led_control(mavlink_message_t *msg)
 {
-    if (!pNotify->_rgb_led_override) {
+    if (rgb_source() == mavlink) {
         // ignore LED_CONTROL commands if not in LED_OVERRIDE mode
         return;
     }

diff --git a/libraries/AP_Notify/RGBLed.h b/libraries/AP_Notify/RGBLed.h
@@ -47,7 +47,7 @@ class RGBLed: public NotifyDevice {
     // 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();
+    void update_override();
 
     // meta-data common to all hw devices
     uint8_t _red_des, _green_des, _blue_des;     // color requested by timed update
@@ -66,6 +66,7 @@ class RGBLed: public NotifyDevice {
 private:
     void update_colours();
     uint32_t get_colour_sequence() const;
+    uint32_t get_colour_sequence_obc() const;
 
     uint8_t get_brightness(void) const;
 
@@ -103,4 +104,11 @@ class RGBLed: public NotifyDevice {
     const uint32_t sequence_disarmed_bad_gps = DEFINE_COLOUR_SEQUENCE_SLOW(BLUE);
 
     uint8_t last_step;
+    enum rgb_source_t {
+        standard = 0,
+        mavlink = 1,
+        obc = 2,
+    };
+    rgb_source_t rgb_source() const;
+
 };