diff --git a/libraries/AP_Notify/AP_Notify.cpp b/libraries/AP_Notify/AP_Notify.cpp index c81ad5e762..12487534b3 100644 --- a/libraries/AP_Notify/AP_Notify.cpp +++ b/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 +#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 -#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)) +NotifyDevice *AP_Notify::_devices[] = {nullptr, nullptr, nullptr, nullptr, nullptr}; // 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,8 +192,10 @@ 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(); + if (_devices[i] != nullptr) { + _devices[i]->pNotify = this; + _devices[i]->init(); + } } } @@ -186,7 +203,9 @@ void AP_Notify::init(bool enable_external_leds) void AP_Notify::update(void) { for (uint8_t i = 0; i < CONFIG_NOTIFY_DEVICES_COUNT; i++) { - _devices[i]->update(); + if (_devices[i] != nullptr) { + _devices[i]->update(); + } } //reset the events @@ -197,7 +216,9 @@ 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++) { - _devices[i]->handle_led_control(msg); + if (_devices[i] != nullptr) { + _devices[i]->handle_led_control(msg); + } } } @@ -205,7 +226,9 @@ void AP_Notify::handle_led_control(mavlink_message_t *msg) void AP_Notify::handle_play_tune(mavlink_message_t *msg) { for (uint8_t i = 0; i < CONFIG_NOTIFY_DEVICES_COUNT; i++) { - _devices[i]->handle_play_tune(msg); + if (_devices[i] != nullptr) { + _devices[i]->handle_play_tune(msg); + } } } diff --git a/libraries/AP_Notify/AP_Notify.h b/libraries/AP_Notify/AP_Notify.h index 75326f5fec..e90e64982c 100644 --- a/libraries/AP_Notify/AP_Notify.h +++ b/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 @@ -50,7 +41,14 @@ class AP_Notify friend class Display; // Display needs access to notify parameters public: // Constructor - AP_Notify(); + 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 { @@ -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