AP_Notify: Complete rework of notify device selection

Jaime did the hard work on this one.  He reworked notify device
selection to take place on init rather than on compile like before.  The
notify decivces are mostly set on compile using preprocessor directives
based on board type.  I created NTF_OREO_THEME.  This will allow the
user to enable/disable the OreoLED driver. And it also allows you to
select between aircraft and rover lighting themes. This allows the Solo
to use the OreoLEDs, and doesn't waste the memory on vehicles not
equipped with Oreo LEDs. The OreoLED driver is restricted to Pixhawk 2
FCs by proprocessor directive due to memory constraints. So it will
never work by accident on another board.

There is also a new notify flag for GPS Fusion.  This flag is true when
the EKF is happy with the GPS, actively using it for position
information.

libraries/AP_Notify/AP_Notify.cpp

@@ -25,7 +25,6 @@
 #include "RCOutputRGBLed.h"
 #include "ToneAlarm_Linux.h"
 #include "ToneAlarm_PX4.h"
-#include "ToneAlarm_PX4_Solo.h"
 #include "ToshibaLED.h"
 #include "ToshibaLED_I2C.h"
 #include "VRBoard_LED.h"
@@ -33,6 +32,7 @@
 #include "DiscoLED.h"
 #include <stdio.h>
 
+#define CONFIG_NOTIFY_DEVICES_COUNT 5
 
 // table of user settable parameters
 const AP_Param::GroupInfo AP_Notify::var_info[] = {
@@ -65,6 +65,13 @@ const AP_Param::GroupInfo AP_Notify::var_info[] = {
     // @User: Advanced
     AP_GROUPINFO("DISPLAY_TYPE", 3, AP_Notify, _display_type, 0),
 
+    // @Param: OREO_THEME
+    // @DisplayName: OreoLED Theme
+    // @Description: Enable/Disable Solo Oreo LED driver, 0 to disable, 1 for Aircraft theme, 2 for Rover theme
+    // @Values: 0:Disabled,1:Aircraft,2:Rover
+    // @User: Advanced
+    AP_GROUPINFO("OREO_THEME", 4, AP_Notify, _oreo_theme, 0),
+
     AP_GROUPEND
 };
 
@@ -78,93 +85,101 @@ AP_Notify::AP_Notify()
 struct AP_Notify::notify_flags_and_values_type AP_Notify::flags;
 struct AP_Notify::notify_events_type AP_Notify::events;
 
-#if CONFIG_HAL_BOARD == HAL_BOARD_PX4
+NotifyDevice *AP_Notify::_devices[] = {nullptr, nullptr, nullptr, nullptr, nullptr};
-#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_PX4_V4
-    PixRacerLED boardled;
-#else
-    AP_BoardLED boardled;
-#endif
-    ToshibaLED_I2C toshibaled;
-    Display display;
-
-#if AP_NOTIFY_SOLO_TONES == 1
-    ToneAlarm_PX4_Solo tonealarm;
-#else
-    ToneAlarm_PX4 tonealarm;
-#endif
-
-#if AP_NOTIFY_OREOLED == 1
-    OreoLED_PX4 oreoled;
-    NotifyDevice *AP_Notify::_devices[] = {&boardled, &toshibaled, &tonealarm, &oreoled, &display};
-#else
-    NotifyDevice *AP_Notify::_devices[] = {&boardled, &toshibaled, &tonealarm, &display};
-#endif
-
-#elif CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
-    ToneAlarm_PX4 tonealarm;
-#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_VRBRAIN_V45
-    AP_BoardLED boardled;
-#else
-    VRBoard_LED boardled;
-#endif
-    ToshibaLED_I2C toshibaled;
-    ExternalLED externalled;
-    NotifyDevice *AP_Notify::_devices[] = {&boardled, &toshibaled, &externalled, &tonealarm};
-
-#elif CONFIG_HAL_BOARD == HAL_BOARD_LINUX
-    #if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_NAVIO
-        NavioLED_I2C navioled;
-        ToshibaLED_I2C toshibaled;
-        NotifyDevice *AP_Notify::_devices[] = {&navioled, &toshibaled};
-    #elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_NAVIO2
-        DiscreteRGBLed navioled(4, 27, 6, false);
-        ToshibaLED_I2C toshibaled;
-        NotifyDevice *AP_Notify::_devices[] = {&navioled, &toshibaled};
-    #elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BBBMINI
-        AP_BoardLED boardled;
-        Buzzer buzzer;
-        Display display;
-        NotifyDevice *AP_Notify::_devices[] = {&boardled, &display, &buzzer};
-    #elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BLUE
-        AP_BoardLED boardled;
-        Display display;
-        NotifyDevice *AP_Notify::_devices[] = {&boardled, &display};
-    #elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_RASPILOT
-        ToshibaLED_I2C toshibaled;
-        ToneAlarm_Linux tonealarm;
-        NotifyDevice *AP_Notify::_devices[] = {&toshibaled, &tonealarm};
-    #elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_MINLURE
-        RCOutputRGBLedOff led(15, 13, 14, 255);
-        NotifyDevice *AP_Notify::_devices[] = { &led };
-    #elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_ERLEBRAIN2 || \
-      CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_PXFMINI
-        AP_BoardLED boardled;
-        NotifyDevice *AP_Notify::_devices[] = {&boardled};
-    #elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BH
-        AP_BoardLED boardled;
-        RCOutputRGBLed bhled(HAL_RCOUT_RGBLED_RED, HAL_RCOUT_RGBLED_GREEN, HAL_RCOUT_RGBLED_BLUE);
-        NotifyDevice *AP_Notify::_devices[] = {&boardled, &bhled};
-    #elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_DISCO
-        DiscoLED discoled;
-        ToneAlarm_Linux tonealarm;
-        NotifyDevice *AP_Notify::_devices[] = {&discoled, &tonealarm};
-    #else
-        AP_BoardLED boardled;
-        ToshibaLED_I2C toshibaled;
-        ToneAlarm_Linux tonealarm;
-        NotifyDevice *AP_Notify::_devices[] = {&boardled, &toshibaled, &tonealarm};
-    #endif
-#else
-    AP_BoardLED boardled;
-    ToshibaLED_I2C toshibaled;
-    NotifyDevice *AP_Notify::_devices[] = {&boardled, &toshibaled};
-#endif
-
-#define CONFIG_NOTIFY_DEVICES_COUNT (ARRAY_SIZE(AP_Notify::_devices))
 
 // initialisation
 void AP_Notify::init(bool enable_external_leds)
 {
+
+// Notify devices for PX4 boards
+#if CONFIG_HAL_BOARD == HAL_BOARD_PX4
+    #if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_PX4_V3 // Has enough memory for Oreo LEDs
+        _devices[0] = new AP_BoardLED();
+        _devices[1] = new ToshibaLED_I2C();
+        _devices[2] = new ToneAlarm_PX4();
+        _devices[3] = new Display();
+
+        // Oreo LED enable/disable by NTF_OREO_THEME parameter
+        if (_oreo_theme) {
+            _devices[4] = new OreoLED_PX4(_oreo_theme);
+        }
+
+    #elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_PX4_V4 // Has its own LED board
+        _devices[0] = new PixRacerLED();
+        _devices[1] = new ToshibaLED_I2C();
+        _devices[2] = new ToneAlarm_PX4();
+        _devices[3] = new Display();
+
+    #else   // All other px4 boards use standard devices.
+        _devices[0] = new AP_BoardLED();
+        _devices[1] = new ToshibaLED_I2C();
+        _devices[2] = new ToneAlarm_PX4();
+        _devices[3] = new Display();
+    #endif
+
+// Notify devices for VRBRAIN boards
+#elif CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN  
+    #if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_VRBRAIN_V45 // Uses px4 LED board
+        _devices[0] = new AP_BoardLED();
+        _devices[1] = new ToshibaLED_I2C();
+        _devices[2] = new ToneAlarm_PX4();
+        _devices[3] = new ExternalLED();
+    #else
+        _devices[0] = new VRBoard_LED();
+        _devices[1] = new ToshibaLED_I2C();
+        _devices[2] = new ToneAlarm_PX4();
+        _devices[3] = new ExternalLED();
+    #endif
+
+// Notify devices for linux boards    
+#elif CONFIG_HAL_BOARD == HAL_BOARD_LINUX
+    #if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_NAVIO
+        _devices[0] = new NavioLED_I2C();
+        _devices[1] = new ToshibaLED_I2C();
+
+    #elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_NAVIO2
+        _devices[0] = new DiscreteRGBLed(4, 27, 6, false);
+        _devices[1] = new ToshibaLED_I2C();
+
+    #elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BBBMINI
+        _devices[0] = new AP_BoardLED();
+        _devices[1] = new Buzzer();
+        _devices[2] = new Display();
+        
+    #elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BLUE
+        _devices[0] = new AP_BoardLED();
+        _devices[1] = new Display();
+
+    #elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_RASPILOT
+        _devices[0] = new ToshibaLED_I2C();
+        _devices[1] = new ToneAlarm_Linux();
+
+    #elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_MINLURE
+        _devices[0] = new RCOutputRGBLedOff(15, 13, 14, 255);
+
+    #elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_ERLEBRAIN2 || \
+      CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_PXFMINI
+        _devices[0] = new AP_BoardLED();
+
+    #elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BH
+        _devices[0] = new AP_BoardLED();
+        _devices[1] = new RCOutputRGBLed(HAL_RCOUT_RGBLED_RED, HAL_RCOUT_RGBLED_GREEN, HAL_RCOUT_RGBLED_BLUE);
+
+    #elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_DISCO
+        _devices[0] = new DiscoLED();
+        _devices[1] = new ToneAlarm_Linux();
+
+    #else
+        _devices[0] = new AP_BoardLED();
+        _devices[1] = new ToshibaLED_I2C();
+        _devices[2] = new ToneAlarm_Linux();
+    #endif
+
+#else
+    _devices[0] = new AP_BoardLED();
+    _devices[1] = new ToshibaLED_I2C();
+#endif
+
     // clear all flags and events
     memset(&AP_Notify::flags, 0, sizeof(AP_Notify::flags));
     memset(&AP_Notify::events, 0, sizeof(AP_Notify::events));
@@ -177,17 +192,21 @@ 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++) {
+        if (_devices[i] != nullptr) {
             _devices[i]->pNotify = this;
             _devices[i]->init();
         }
     }
+}
 
 // main update function, called at 50Hz
 void AP_Notify::update(void)
 {
     for (uint8_t i = 0; i < CONFIG_NOTIFY_DEVICES_COUNT; i++) {
+        if (_devices[i] != nullptr) {
             _devices[i]->update();
         }
+    }
 
     //reset the events
     memset(&AP_Notify::events, 0, sizeof(AP_Notify::events));
@@ -197,17 +216,21 @@ void AP_Notify::update(void)
 void AP_Notify::handle_led_control(mavlink_message_t *msg)
 {
     for (uint8_t i = 0; i < CONFIG_NOTIFY_DEVICES_COUNT; i++) {
+        if (_devices[i] != nullptr) {
             _devices[i]->handle_led_control(msg);
         }
     }
+}
 
 // handle a PLAY_TUNE message
 void AP_Notify::handle_play_tune(mavlink_message_t *msg)
 {
     for (uint8_t i = 0; i < CONFIG_NOTIFY_DEVICES_COUNT; i++) {
+        if (_devices[i] != nullptr) {
             _devices[i]->handle_play_tune(msg);
         }
     }
+}
 
 // set flight mode string
 void AP_Notify::set_flight_mode_str(const char *str)

libraries/AP_Notify/AP_Notify.h

@@ -20,15 +20,6 @@
 
 #include "NotifyDevice.h"
 
-
-#ifndef AP_NOTIFY_OREOLED
-#define AP_NOTIFY_OREOLED 0
-#endif
-
-#ifndef AP_NOTIFY_SOLO_TONES
-#define AP_NOTIFY_SOLO_TONES 0
-#endif
-
 // Device parameters values
 #define RGB_LED_OFF     0
 #define RGB_LED_LOW     1
@@ -52,6 +43,13 @@ public:
     // Constructor
     AP_Notify();   
 
+    // Oreo LED Themes
+    enum Oreo_LED_Theme {
+        OreoLED_Disabled        = 0,    // Disabled the OLED driver entirely
+        OreoLED_Aircraft        = 1,    // Standard aviation themed lighting
+        OreoLED_Automobile      = 2,    // Automobile themed lighting (white front, red back)
+    };
+
     /// notify_flags_type - bitmask of notification flags
     struct notify_flags_and_values_type {
         uint32_t initialising       : 1;    // 1 if initialising and copter should not be moved
@@ -72,6 +70,7 @@ public:
         uint32_t compass_cal_running: 1;    // 1 if a compass calibration is running
         uint32_t leak_detected      : 1;    // 1 if leak detected
         float    battery_voltage       ;    // battery voltage
+        uint32_t gps_fusion         : 1;    // 0 = GPS fix rejected by EKF, not usable for flight. 1 = GPS in use by EKF, usable for flight
 
         // additional flags
         uint32_t external_leds      : 1;    // 1 if external LEDs are enabled (normally only used for copter)
@@ -138,6 +137,7 @@ private:
     AP_Int8 _rgb_led_override;
     AP_Int8 _buzzer_enable;
     AP_Int8 _display_type;
+    AP_Int8 _oreo_theme;
 
     char _send_text[NOTIFY_TEXT_BUFFER_SIZE];
     uint32_t _send_text_updated_millis; // last time text changed