diff --git a/libraries/AP_Notify/AP_Notify.cpp b/libraries/AP_Notify/AP_Notify.cpp index 00cf42c448..77e6950cb8 100644 --- a/libraries/AP_Notify/AP_Notify.cpp +++ b/libraries/AP_Notify/AP_Notify.cpp @@ -20,7 +20,8 @@ #include "Buzzer.h" #include "Display.h" #include "ExternalLED.h" -#include "NavioLED_I2C.h" +#include "PCA9685LED_I2C.h" +#include "NCP5623.h" #include "OreoLED_PX4.h" #include "RCOutputRGBLed.h" #include "ToneAlarm.h" @@ -42,6 +43,64 @@ AP_Notify *AP_Notify::_instance; #define TOSHIBA_LED_I2C_BUS_INTERNAL 0 #define TOSHIBA_LED_I2C_BUS_EXTERNAL 1 +// all I2C_LEDS +#define I2C_LEDS (Notify_LED_ToshibaLED_I2C_Internal | Notify_LED_ToshibaLED_I2C_External | \ + Notify_LED_NCP5623_I2C_Internal | Notify_LED_NCP5623_I2C_External) + +#ifndef BUILD_DEFAULT_LED_TYPE +// 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 + #define BUILD_DEFAULT_LED_TYPE (Notify_LED_Board | I2C_LEDS | Notify_LED_OreoLED) + #else // All other px4 boards use standard devices + #define BUILD_DEFAULT_LED_TYPE (Notify_LED_Board | I2C_LEDS) + #endif + +// ChibiOS and VRBrain boards +#elif CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS || \ + CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN + #define BUILD_DEFAULT_LED_TYPE (Notify_LED_Board | I2C_LEDS) + +// Linux boards +#elif CONFIG_HAL_BOARD == HAL_BOARD_LINUX + #if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_NAVIO + #define BUILD_DEFAULT_LED_TYPE (Notify_LED_Board | I2C_LEDS |\ + Notify_LED_PCA9685LED_I2C_External) + + #elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_NAVIO2 + #define BUILD_DEFAULT_LED_TYPE (Notify_LED_Board | I2C_LEDS) + + #elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_EDGE + #define BUILD_DEFAULT_LED_TYPE (Notify_LED_Board | I2C_LEDS |\ + Notify_LED_UAVCAN) + #elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BBBMINI || \ + CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BLUE || \ + CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_POCKET || \ + CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_ERLEBRAIN2 || \ + CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_PXFMINI || \ + CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BH || \ + CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_DISCO + #define BUILD_DEFAULT_LED_TYPE (Notify_LED_Board) + + #elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_RST_ZYNQ + #define BUILD_DEFAULT_LED_TYPE (Notify_LED_ToshibaLED_I2C_External) + + #else // other linux + #define BUILD_DEFAULT_LED_TYPE (Notify_LED_Board | I2C_LEDS) + #endif + +// F4Light +#elif CONFIG_HAL_BOARD == HAL_BOARD_F4LIGHT + #define BUILD_DEFAULT_LED_TYPE (Notify_LED_Board | Notify_LED_ToshibaLED_I2C_External) + +// All other builds +#else + #define BUILD_DEFAULT_LED_TYPE (Notify_LED_Board | I2C_LEDS) + +#endif // board selection + +#endif // BUILD_DEFAULT_LED_TYPE + // table of user settable parameters const AP_Param::GroupInfo AP_Notify::var_info[] = { @@ -92,13 +151,20 @@ const AP_Param::GroupInfo AP_Notify::var_info[] = { AP_GROUPINFO("BUZZ_PIN", 5, AP_Notify, _buzzer_pin, 0), #endif + // @Param: LED_TYPES + // @DisplayName: LED Driver Types + // @Description: Controls what types of LEDs will be enabled + // @Bitmask: 0:Build in LED, 1:Internal ToshibaLED, 2:External ToshibaLED, 3:External PCA9685, 4:Oreo LED, 5:UAVCAN, 6:NCP5623 External, 7:NCP5623 Internal + // @User: Advanced + AP_GROUPINFO("LED_TYPES", 6, AP_Notify, _led_type, BUILD_DEFAULT_LED_TYPE), + AP_GROUPEND }; // Default constructor AP_Notify::AP_Notify() { - AP_Param::setup_object_defaults(this, var_info); + AP_Param::setup_object_defaults(this, var_info); if (_instance != nullptr) { AP_HAL::panic("AP_Notify must be singleton"); } @@ -133,147 +199,129 @@ void AP_Notify::add_backends(void) return; } -// 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 - ADD_BACKEND(new AP_BoardLED()); - ADD_BACKEND(new ToshibaLED_I2C(TOSHIBA_LED_I2C_BUS_EXTERNAL)); - ADD_BACKEND(new ToshibaLED_I2C(TOSHIBA_LED_I2C_BUS_INTERNAL)); - ADD_BACKEND(new AP_ToneAlarm()); - ADD_BACKEND(new Display()); + for (uint32_t i = 1; i < Notify_LED_MAX; i = i << 1) { + switch(_led_type & i) { + case Notify_LED_None: + break; + case Notify_LED_Board: + // select the most appropriate built in LED driver type +#if CONFIG_HAL_BOARD == HAL_BOARD_LINUX + #if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_NAVIO2 + ADD_BACKEND(new Led_Sysfs("rgb_led0", "rgb_led2", "rgb_led1")); + #elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_EDGE + ADD_BACKEND(new RCOutputRGBLedInverted(12, 13, 14)); + #elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BH + ADD_BACKEND(new RCOutputRGBLed(HAL_RCOUT_RGBLED_RED, HAL_RCOUT_RGBLED_GREEN, HAL_RCOUT_RGBLED_BLUE)); + #elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_DISCO + ADD_BACKEND(new DiscoLED()); + #endif +#endif // CONFIG_HAL_BOARD == HAL_BOARD_LINUX - // Oreo LED enable/disable by NTF_OREO_THEME parameter - if (_oreo_theme) { - ADD_BACKEND(new OreoLED_PX4(_oreo_theme)); +#if CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN + ADD_BACKEND(new AP_ExternalLED()); // despite the name this is a built in set of onboard LED's +#endif // CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN + +#if defined(HAL_HAVE_PIXRACER_LED) + ADD_BACKEND(new PixRacerLED()); +#elif (defined(HAL_GPIO_A_LED_PIN) && defined(HAL_GPIO_B_LED_PIN) && defined(HAL_GPIO_C_LED_PIN)) + #if CONFIG_HAL_BOARD != HAL_BOARD_VRBRAIN || CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_VRBRAIN_V45 + ADD_BACKEND(new AP_BoardLED()); + #else + ADD_BACKEND(new VRBoard_LED()); + #endif // CONFIG_HAL_BOARD != HAL_BOARD_VRBRAIN || CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_VRBRAIN_V45 +#elif (defined(HAL_GPIO_A_LED_PIN) && defined(HAL_GPIO_B_LED_PIN)) + ADD_BACKEND(new AP_BoardLED2()); +#endif + break; + case Notify_LED_ToshibaLED_I2C_Internal: + ADD_BACKEND(new ToshibaLED_I2C(TOSHIBA_LED_I2C_BUS_INTERNAL)); + break; + case Notify_LED_ToshibaLED_I2C_External: + ADD_BACKEND(new ToshibaLED_I2C(TOSHIBA_LED_I2C_BUS_EXTERNAL)); + break; +#if !HAL_MINIMIZE_FEATURES + case Notify_LED_NCP5623_I2C_External: + FOREACH_I2C_EXTERNAL(b) { + ADD_BACKEND(new NCP5623(b)); + } + break; + case Notify_LED_NCP5623_I2C_Internal: + ADD_BACKEND(new NCP5623(TOSHIBA_LED_I2C_BUS_INTERNAL)); + break; +#endif +#if !HAL_MINIMIZE_FEATURES + case Notify_LED_PCA9685LED_I2C_External: + ADD_BACKEND(new PCA9685LED_I2C()); + break; +#endif + case Notify_LED_OreoLED: + // OreoLED's are PX4-v3 build only +#if (CONFIG_HAL_BOARD == HAL_BOARD_PX4) && (CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_PX4_V3) + if (_oreo_theme) { + ADD_BACKEND(new OreoLED_PX4(_oreo_theme)); + } +#endif // (CONFIG_HAL_BOARD == HAL_BOARD_PX4) && (CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_PX4_V3) + break; + case Notify_LED_UAVCAN: +#if HAL_WITH_UAVCAN + ADD_BACKEND(new UAVCAN_RGB_LED(0)); +#endif // HAL_WITH_UAVCAN + break; + + } } - #elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_PX4_V4 // Has its own LED board - ADD_BACKEND(new PixRacerLED()); - ADD_BACKEND(new ToshibaLED_I2C(TOSHIBA_LED_I2C_BUS_EXTERNAL)); - ADD_BACKEND(new ToshibaLED_I2C(TOSHIBA_LED_I2C_BUS_INTERNAL)); - ADD_BACKEND(new AP_ToneAlarm()); + + // Always try and add a display backend ADD_BACKEND(new Display()); - #else // All other px4 boards use standard devices. - ADD_BACKEND(new AP_BoardLED()); - ADD_BACKEND(new ToshibaLED_I2C(TOSHIBA_LED_I2C_BUS_EXTERNAL)); - ADD_BACKEND(new ToshibaLED_I2C(TOSHIBA_LED_I2C_BUS_INTERNAL)); + // Add noise making devices +#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || \ + CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN ADD_BACKEND(new AP_ToneAlarm()); - ADD_BACKEND(new Display()); - #endif -// Notify devices for ChibiOS boards +// ChibiOS noise makers #elif CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS -# ifdef HAL_HAVE_PIXRACER_LED - ADD_BACKEND(new PixRacerLED()); -# elif (defined(HAL_GPIO_A_LED_PIN) && defined(HAL_GPIO_B_LED_PIN) && defined(HAL_GPIO_C_LED_PIN)) - ADD_BACKEND(new AP_BoardLED()); -#else - ADD_BACKEND(new AP_BoardLED2()); -# endif #ifdef HAL_BUZZER_PIN ADD_BACKEND(new Buzzer()); #endif #ifdef HAL_PWM_ALARM ADD_BACKEND(new AP_ToneAlarm()); #endif - ADD_BACKEND(new ToshibaLED_I2C(TOSHIBA_LED_I2C_BUS_EXTERNAL)); - ADD_BACKEND(new ToshibaLED_I2C(TOSHIBA_LED_I2C_BUS_INTERNAL)); - ADD_BACKEND(new Display()); -// 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 - ADD_BACKEND(new AP_BoardLED()); - ADD_BACKEND(new ToshibaLED_I2C(TOSHIBA_LED_I2C_BUS_EXTERNAL)); - ADD_BACKEND(new ToshibaLED_I2C(TOSHIBA_LED_I2C_BUS_INTERNAL)); - ADD_BACKEND(new AP_ToneAlarm()); - ADD_BACKEND(new ExternalLED()); - #else - ADD_BACKEND(new VRBoard_LED()); - ADD_BACKEND(new ToshibaLED_I2C(TOSHIBA_LED_I2C_BUS_EXTERNAL)); - ADD_BACKEND(new ToshibaLED_I2C(TOSHIBA_LED_I2C_BUS_INTERNAL)); - ADD_BACKEND(new AP_ToneAlarm()); - ADD_BACKEND(new ExternalLED()); - #endif - -// Notify devices for linux boards +// Linux noise makers #elif CONFIG_HAL_BOARD == HAL_BOARD_LINUX - #if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_NAVIO - ADD_BACKEND(new AP_BoardLED()); - ADD_BACKEND(new NavioLED_I2C()); - ADD_BACKEND(new ToshibaLED_I2C(TOSHIBA_LED_I2C_BUS_EXTERNAL)); - ADD_BACKEND(new ToshibaLED_I2C(TOSHIBA_LED_I2C_BUS_INTERNAL)); + #if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_NAVIO || \ + CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_NAVIO2 || \ + CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_EDGE || \ + CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_ERLEBRAIN2 || \ + CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_PXFMINI || \ + CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_RST_ZYNQ || \ + CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BLUE + // No noise makers, keep this though to ensure that the final else is safe - #elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_NAVIO2 - ADD_BACKEND(new Led_Sysfs("rgb_led0", "rgb_led2", "rgb_led1")); - ADD_BACKEND(new ToshibaLED_I2C(TOSHIBA_LED_I2C_BUS_EXTERNAL)); - ADD_BACKEND(new ToshibaLED_I2C(TOSHIBA_LED_I2C_BUS_INTERNAL)); - - #elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_EDGE - ADD_BACKEND(new RCOutputRGBLedInverted(12, 13, 14)); - ADD_BACKEND(new ToshibaLED_I2C(TOSHIBA_LED_I2C_BUS_EXTERNAL)); - ADD_BACKEND(new ToshibaLED_I2C(TOSHIBA_LED_I2C_BUS_INTERNAL)); - ADD_BACKEND(new UAVCAN_RGB_LED(0)); - - #elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BBBMINI - ADD_BACKEND(new AP_BoardLED()); + #elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BBBMINI || \ + CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_POCKET ADD_BACKEND(new Buzzer()); - ADD_BACKEND(new Display()); - - #elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BLUE - ADD_BACKEND(new AP_BoardLED()); - ADD_BACKEND(new Display()); - - #elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_POCKET - ADD_BACKEND(new AP_BoardLED()); - ADD_BACKEND(new Buzzer()); - ADD_BACKEND(new Display()); - - #elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_ERLEBRAIN2 || \ - CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_PXFMINI - ADD_BACKEND(new AP_BoardLED()); - - #elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BH - ADD_BACKEND(new AP_BoardLED()); - ADD_BACKEND(new RCOutputRGBLed(HAL_RCOUT_RGBLED_RED, HAL_RCOUT_RGBLED_GREEN, HAL_RCOUT_RGBLED_BLUE)); - - #elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_DISCO - ADD_BACKEND(new DiscoLED()); - ADD_BACKEND(new AP_ToneAlarm()); - - #elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_RST_ZYNQ - ADD_BACKEND(new ToshibaLED_I2C(TOSHIBA_LED_I2C_BUS_EXTERNAL)); #else // other linux - ADD_BACKEND(new AP_BoardLED()); - ADD_BACKEND(new ToshibaLED_I2C(TOSHIBA_LED_I2C_BUS_EXTERNAL)); - ADD_BACKEND(new ToshibaLED_I2C(TOSHIBA_LED_I2C_BUS_INTERNAL)); ADD_BACKEND(new AP_ToneAlarm()); #endif +// F4Light noise makers #elif CONFIG_HAL_BOARD == HAL_BOARD_F4LIGHT - ADD_BACKEND(new ToshibaLED_I2C(TOSHIBA_LED_I2C_BUS_EXTERNAL)); - ADD_BACKEND(new Display()); ADD_BACKEND(new Buzzer()); - ADD_BACKEND(new AP_BoardLED2()); // needs AP_BoardLED2 in master -#else - ADD_BACKEND(new AP_BoardLED()); - ADD_BACKEND(new ToshibaLED_I2C(TOSHIBA_LED_I2C_BUS_EXTERNAL)); - ADD_BACKEND(new ToshibaLED_I2C(TOSHIBA_LED_I2C_BUS_INTERNAL)); - ADD_BACKEND(new Display()); -#endif +#endif // Noise makers + } // initialisation -void AP_Notify::init(bool enable_external_leds) +void AP_Notify::init(bool dummy) { // clear all flags and events memset(&AP_Notify::flags, 0, sizeof(AP_Notify::flags)); memset(&AP_Notify::events, 0, sizeof(AP_Notify::events)); - AP_Notify::flags.external_leds = enable_external_leds; - // add all the backends add_backends(); } diff --git a/libraries/AP_Notify/AP_Notify.h b/libraries/AP_Notify/AP_Notify.h index 4de4ee39c8..3ce73f2440 100644 --- a/libraries/AP_Notify/AP_Notify.h +++ b/libraries/AP_Notify/AP_Notify.h @@ -58,36 +58,46 @@ public: OreoLED_Automobile = 2, // Automobile themed lighting (white front, red back) }; + enum Notify_LED_Type { + Notify_LED_None = 0, // not enabled + Notify_LED_Board = (1 << 0), // Built in board LED's + Notify_LED_ToshibaLED_I2C_Internal = (1 << 1), // Internal ToshibaLED_I2C + Notify_LED_ToshibaLED_I2C_External = (1 << 2), // External ToshibaLED_I2C + Notify_LED_PCA9685LED_I2C_External = (1 << 3), // External PCA9685_I2C + Notify_LED_OreoLED = (1 << 4), // Oreo + Notify_LED_UAVCAN = (1 << 5), // UAVCAN RGB LED + Notify_LED_NCP5623_I2C_External = (1 << 6), // External NCP5623 + Notify_LED_NCP5623_I2C_Internal = (1 << 7), // Internal NCP5623 + Notify_LED_MAX + }; + /// 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 - uint32_t gps_status : 3; // 0 = no gps, 1 = no lock, 2 = 2d lock, 3 = 3d lock, 4 = dgps lock, 5 = rtk lock - uint32_t gps_num_sats : 6; // number of sats - uint32_t flight_mode : 8; // flight mode - uint32_t armed : 1; // 0 = disarmed, 1 = armed - uint32_t pre_arm_check : 1; // 0 = failing checks, 1 = passed - uint32_t pre_arm_gps_check : 1; // 0 = failing pre-arm GPS checks, 1 = passed - uint32_t save_trim : 1; // 1 if gathering trim data - uint32_t esc_calibration : 1; // 1 if calibrating escs - uint32_t failsafe_radio : 1; // 1 if radio failsafe - uint32_t failsafe_battery : 1; // 1 if battery failsafe - uint32_t parachute_release : 1; // 1 if parachute is being released - uint32_t ekf_bad : 1; // 1 if ekf is reporting problems - uint32_t autopilot_mode : 1; // 1 if vehicle is in an autopilot flight mode (only used by OreoLEDs) - uint32_t firmware_update : 1; // 1 just before vehicle firmware is updated - 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 - uint32_t gps_glitching : 1; // 1 if GPS glitching is affecting navigation accuracy - uint32_t have_pos_abs : 1; // 0 = no absolute position available, 1 = absolute position available - - // additional flags - uint32_t external_leds : 1; // 1 if external LEDs are enabled (normally only used for copter) - uint32_t vehicle_lost : 1; // 1 when lost copter tone is requested (normally only used for copter) - uint32_t waiting_for_throw : 1; // 1 when copter is in THROW mode and waiting to detect the user hand launch - uint32_t powering_off : 1; // 1 when the vehicle is powering off - uint32_t video_recording : 1; // 1 when the vehicle is recording video + bool initialising; // true if initialising and the vehicle should not be moved + uint8_t gps_status; // see the GPS_0 = no gps, 1 = no lock, 2 = 2d lock, 3 = 3d lock, 4 = dgps lock, 5 = rtk lock + uint8_t gps_num_sats; // number of sats + uint8_t flight_mode; // flight mode + float battery_voltage; + bool armed; // 0 = disarmed, 1 = armed + bool pre_arm_check; // true if passing pre arm checks + bool pre_arm_gps_check; // true if passing pre arm gps checks + bool save_trim; // true if gathering trim data + bool esc_calibration; // true if calibrating escs + bool failsafe_radio; // true if radio failsafe + bool failsafe_battery; // true if battery failsafe + bool parachute_release; // true if parachute is being released + bool ekf_bad; // true if ekf is reporting problems + bool autopilot_mode; // true if vehicle is in an autopilot flight mode (only used by OreoLEDs) + bool firmware_update; // true just before vehicle firmware is updated + bool compass_cal_running; // true if a compass calibration is running + bool leak_detected; // true if leak detected + bool gps_fusion; // true if the GPS is in use by EKF, usable for flight + bool gps_glitching; // true f the GPS is believed to be glitching is affecting navigation accuracy + bool have_pos_abs; // true if absolute position is available + bool vehicle_lost; // true when lost copter tone is requested (normally only used for copter) + bool waiting_for_throw; // true when copter is in THROW mode and waiting to detect the user hand launch + bool powering_off; // true when the vehicle is powering off + bool video_recording; // true when the vehicle is recording video }; /// notify_events_type - bitmask of active events. @@ -118,10 +128,7 @@ public: static struct notify_events_type events; // initialisation - void init(bool enable_external_leds); - - // add all backends - void add_backends(void); + void init(bool dummy); /// update - allow updates of leds that cannot be updated during a timed interrupt void update(void); @@ -152,6 +159,9 @@ private: void add_backend_helper(NotifyDevice *backend); + // add all backends + void add_backends(void); + // parameters AP_Int8 _rgb_led_brightness; AP_Int8 _rgb_led_override; @@ -159,6 +169,7 @@ private: AP_Int8 _display_type; AP_Int8 _oreo_theme; AP_Int8 _buzzer_pin; + AP_Int32 _led_type; char _send_text[NOTIFY_TEXT_BUFFER_SIZE]; uint32_t _send_text_updated_millis; // last time text changed diff --git a/libraries/AP_Notify/Buzzer.cpp b/libraries/AP_Notify/Buzzer.cpp index 1547f56b9e..6e29297866 100644 --- a/libraries/AP_Notify/Buzzer.cpp +++ b/libraries/AP_Notify/Buzzer.cpp @@ -33,6 +33,9 @@ extern const AP_HAL::HAL& hal; bool Buzzer::init() { + if (pNotify->buzzer_enabled() == false) { + return false; + } #if defined(HAL_BUZZER_PIN) _pin = HAL_BUZZER_PIN; #else @@ -54,11 +57,6 @@ bool Buzzer::init() // update - updates led according to timed_updated. Should be called at 50Hz void Buzzer::update() { - // return immediately if disabled - if (!AP_Notify::flags.external_leds) { - return; - } - // check for arming failed event if (AP_Notify::events.arming_failed) { // arming failed buzz diff --git a/libraries/AP_Notify/Display.cpp b/libraries/AP_Notify/Display.cpp index 4fdb522dde..78e1f0542c 100644 --- a/libraries/AP_Notify/Display.cpp +++ b/libraries/AP_Notify/Display.cpp @@ -22,6 +22,7 @@ #include #include +#include #include @@ -505,7 +506,7 @@ void Display::update_ekf(uint8_t r) void Display::update_battery(uint8_t r) { char msg [DISPLAY_MESSAGE_SIZE]; - snprintf(msg, DISPLAY_MESSAGE_SIZE, "BAT1: %4.2fV", (double)AP_Notify::flags.battery_voltage) ; + snprintf(msg, DISPLAY_MESSAGE_SIZE, "BAT1: %4.2fV", (double)AP::battery().voltage()) ; draw_text(COLUMN(0), ROW(r), msg); } diff --git a/libraries/AP_Notify/ExternalLED.cpp b/libraries/AP_Notify/ExternalLED.cpp index 6740016d81..7671b2be70 100644 --- a/libraries/AP_Notify/ExternalLED.cpp +++ b/libraries/AP_Notify/ExternalLED.cpp @@ -25,11 +25,6 @@ extern const AP_HAL::HAL& hal; bool ExternalLED::init(void) { - // return immediately if disabled - if (!AP_Notify::flags.external_leds) { - return false; - } - // setup the main LEDs as outputs hal.gpio->pinMode(EXTERNAL_LED_ARMED, HAL_GPIO_OUTPUT); hal.gpio->pinMode(EXTERNAL_LED_GPS, HAL_GPIO_OUTPUT); @@ -49,11 +44,6 @@ bool ExternalLED::init(void) */ void ExternalLED::update(void) { - // return immediately if disabled - if (!AP_Notify::flags.external_leds) { - return; - } - // reduce update rate from 50hz to 10hz _counter++; if (_counter < 5) { diff --git a/libraries/AP_Notify/NCP5623.cpp b/libraries/AP_Notify/NCP5623.cpp new file mode 100644 index 0000000000..986ff88f93 --- /dev/null +++ b/libraries/AP_Notify/NCP5623.cpp @@ -0,0 +1,120 @@ +/* + NCP5623 I2C LED driver +*/ +/* + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . + */ + +#include "NCP5623.h" +#include + +#include + +extern const AP_HAL::HAL& hal; + +#define NCP5623_LED_BRIGHT 0x1f // full brightness +#define NCP5623_LED_MEDIUM 0x18 // medium brightness +#define NCP5623_LED_DIM 0x0f // dim +#define NCP5623_LED_OFF 0x00 // off + +#define NCP5623_LED_I2C_ADDR 0x38 // default I2C bus address +#define NCP5623_C_LED_I2C_ADDR 0x39 // default I2C bus address for the NCP5623C + +#define NCP5623_LED_PWM0 0x40 // pwm0 register +#define NCP5623_LED_PWM1 0x60 // pwm1 register +#define NCP5623_LED_PWM2 0x80 // pwm2 register +#define NCP5623_LED_ENABLE 0x20 // enable register + +NCP5623::NCP5623(uint8_t bus) + : RGBLed(NCP5623_LED_OFF, NCP5623_LED_BRIGHT, NCP5623_LED_MEDIUM, NCP5623_LED_DIM) + , _bus(bus) +{ +} + +bool NCP5623::write(uint8_t reg, uint8_t data) +{ + uint8_t msg[1] = { 0x00 }; + msg[0] = ((reg & 0xe0) | (data & 0x1f)); + bool ret = _dev->transfer(msg, 1, nullptr, 0); + return ret; +} + +bool NCP5623::write_pwm(uint8_t _rgb[3]) +{ + uint8_t reg = NCP5623_LED_PWM0; + for (uint8_t i=0; i<3; i++) { + if (!write(reg+i*0x20, _rgb[i])) { + return false; + } + } + return true; +} + +bool NCP5623::hw_init(void) +{ + uint8_t addrs[] = { NCP5623_LED_I2C_ADDR, NCP5623_C_LED_I2C_ADDR }; + for (uint8_t i=0; iget_device(_bus, addrs[i])); + if (!_dev) { + continue; + } + + _dev->get_semaphore()->take_blocking(); + _dev->set_retries(10); + + // enable the led + bool ret = write(NCP5623_LED_ENABLE, 0x1f); + if (!ret) { + _dev->get_semaphore()->give(); + continue; + } + + // update the red, green and blue values to zero + uint8_t off[3] = { _led_off, _led_off, _led_off }; + ret = write_pwm(off); + + _dev->set_retries(1); + + // give back i2c semaphore + _dev->get_semaphore()->give(); + + if (ret) { + _dev->register_periodic_callback(20000, FUNCTOR_BIND_MEMBER(&NCP5623::_timer, void)); + } + return true; + } + + return false; +} + +// set_rgb - set color as a combination of red, green and blue values +bool NCP5623::hw_set_rgb(uint8_t red, uint8_t green, uint8_t blue) +{ + rgb[0] = red; + rgb[1] = green; + rgb[2] = blue; + _need_update = true; + return true; +} + +void NCP5623::_timer(void) +{ + if (!_need_update) { + return; + } + _need_update = false; + + write_pwm(rgb); +} diff --git a/libraries/AP_Notify/NCP5623.h b/libraries/AP_Notify/NCP5623.h new file mode 100644 index 0000000000..580e4f762d --- /dev/null +++ b/libraries/AP_Notify/NCP5623.h @@ -0,0 +1,37 @@ +/* + NCP5623 Linux driver +*/ +/* + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . + */ +#pragma once + +#include +#include "RGBLed.h" + +class NCP5623 : public RGBLed { +public: + NCP5623(uint8_t bus); +protected: + bool hw_init(void) override; + bool hw_set_rgb(uint8_t r, uint8_t g, uint8_t b) override; +private: + AP_HAL::OwnPtr _dev; + void _timer(void); + bool write(uint8_t reg, uint8_t data); + bool write_pwm(uint8_t rgb[3]); + uint8_t rgb[3]; + bool _need_update; + uint8_t _bus; +}; diff --git a/libraries/AP_Notify/NavioLED_I2C.cpp b/libraries/AP_Notify/PCA9685LED_I2C.cpp similarity index 67% rename from libraries/AP_Notify/NavioLED_I2C.cpp rename to libraries/AP_Notify/PCA9685LED_I2C.cpp index c124882d8f..731283f164 100644 --- a/libraries/AP_Notify/NavioLED_I2C.cpp +++ b/libraries/AP_Notify/PCA9685LED_I2C.cpp @@ -1,5 +1,5 @@ /* - NavioLED I2C driver + PCA9685LED I2C driver */ /* This program is free software: you can redistribute it and/or modify @@ -15,7 +15,7 @@ You should have received a copy of the GNU General Public License along with this program. If not, see . */ -#include "NavioLED_I2C.h" +#include "PCA9685LED_I2C.h" #include @@ -25,16 +25,19 @@ #define NAVIO_LED_OFF 0xFF // off #define PCA9685_ADDRESS 0x40 +#define PCA9685_MODE1 0x00 #define PCA9685_PWM 0x6 +#define PCA9685_MODE_SLEEP (1 << 4) +#define PCA9685_MODE_AUTO_INCREMENT (1 << 5) extern const AP_HAL::HAL& hal; -NavioLED_I2C::NavioLED_I2C() : +PCA9685LED_I2C::PCA9685LED_I2C() : RGBLed(NAVIO_LED_OFF, NAVIO_LED_BRIGHT, NAVIO_LED_MEDIUM, NAVIO_LED_DIM) { } -bool NavioLED_I2C::hw_init() +bool PCA9685LED_I2C::hw_init() { _dev = hal.i2c_mgr->get_device(1, PCA9685_ADDRESS); @@ -42,13 +45,36 @@ bool NavioLED_I2C::hw_init() return false; } - _dev->register_periodic_callback(20000, FUNCTOR_BIND_MEMBER(&NavioLED_I2C::_timer, void)); + _dev->get_semaphore()->take_blocking(); + + _dev->set_retries(5); + + // read the current mode1 configuration + uint8_t mode1 = 0; + if (!_dev->read_registers(PCA9685_MODE1, &mode1, sizeof(mode1))) { + _dev->get_semaphore()->give(); + return false; + } + + // bring the device out of sleep, and enable auto register increment + uint8_t new_mode1 = (mode1 | PCA9685_MODE_AUTO_INCREMENT) & ~PCA9685_MODE_SLEEP; + const uint8_t config[2] = {PCA9685_MODE1, new_mode1}; + if (!_dev->transfer(config, sizeof(config), nullptr, 0)) { + _dev->get_semaphore()->give(); + return false; + } + + _dev->set_retries(1); + + _dev->get_semaphore()->give(); + + _dev->register_periodic_callback(20000, FUNCTOR_BIND_MEMBER(&PCA9685LED_I2C::_timer, void)); return true; } // set_rgb - set color as a combination of red, green and blue values -bool NavioLED_I2C::hw_set_rgb(uint8_t red, uint8_t green, uint8_t blue) +bool PCA9685LED_I2C::hw_set_rgb(uint8_t red, uint8_t green, uint8_t blue) { rgb.r = red; rgb.g = green; @@ -57,7 +83,7 @@ bool NavioLED_I2C::hw_set_rgb(uint8_t red, uint8_t green, uint8_t blue) return true; } -void NavioLED_I2C::_timer(void) +void PCA9685LED_I2C::_timer(void) { if (!_need_update) { return; diff --git a/libraries/AP_Notify/NavioLED_I2C.h b/libraries/AP_Notify/PCA9685LED_I2C.h similarity index 91% rename from libraries/AP_Notify/NavioLED_I2C.h rename to libraries/AP_Notify/PCA9685LED_I2C.h index f5113ea35f..238a9dfa6f 100644 --- a/libraries/AP_Notify/NavioLED_I2C.h +++ b/libraries/AP_Notify/PCA9685LED_I2C.h @@ -1,5 +1,5 @@ /* - NavioLED I2C driver + PCA9685LED I2C driver This program is free software: you can redistribute it and/or modify it under the terms of the GNU General Public License as published by @@ -19,10 +19,10 @@ #include #include "RGBLed.h" -class NavioLED_I2C : public RGBLed +class PCA9685LED_I2C : public RGBLed { public: - NavioLED_I2C(void); + PCA9685LED_I2C(void); protected: bool hw_init(void) override; bool hw_set_rgb(uint8_t r, uint8_t g, uint8_t b) override; diff --git a/libraries/AP_Notify/ToneAlarm.cpp b/libraries/AP_Notify/ToneAlarm.cpp index 6a9edd4fa7..8307d4fe0c 100644 --- a/libraries/AP_Notify/ToneAlarm.cpp +++ b/libraries/AP_Notify/ToneAlarm.cpp @@ -93,6 +93,9 @@ const AP_ToneAlarm::Tone AP_ToneAlarm::_tones[] { bool AP_ToneAlarm::init() { + if (pNotify->buzzer_enabled() == false) { + return false; + } if (!hal.util->toneAlarm_init()) { return false; }