From a5e974c227cefa254103cedbf50cc93ab4322f7c Mon Sep 17 00:00:00 2001 From: Michael du Breuil Date: Wed, 25 Jul 2018 18:09:34 -0700 Subject: [PATCH] AP_Notify: Reduce flash consumption, fix some drivers, always allow buzzer --- libraries/AP_Notify/AP_Notify.cpp | 8 ++-- libraries/AP_Notify/AP_Notify.h | 60 ++++++++++++++--------------- libraries/AP_Notify/Buzzer.cpp | 5 --- libraries/AP_Notify/Display.cpp | 3 +- libraries/AP_Notify/ExternalLED.cpp | 10 ----- 5 files changed, 35 insertions(+), 51 deletions(-) diff --git a/libraries/AP_Notify/AP_Notify.cpp b/libraries/AP_Notify/AP_Notify.cpp index 8a25c778bf..8c82c86070 100644 --- a/libraries/AP_Notify/AP_Notify.cpp +++ b/libraries/AP_Notify/AP_Notify.cpp @@ -212,6 +212,10 @@ void AP_Notify::add_backends(void) #endif #endif // CONFIG_HAL_BOARD == HAL_BOARD_LINUX +#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)) @@ -295,14 +299,12 @@ void AP_Notify::add_backends(void) } // initialisation -void AP_Notify::init(bool enable_external_leds) +void AP_Notify::init(void) { // 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 70251bbf78..0d167f490e 100644 --- a/libraries/AP_Notify/AP_Notify.h +++ b/libraries/AP_Notify/AP_Notify.h @@ -71,34 +71,30 @@ public: /// 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 + 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. @@ -129,10 +125,7 @@ public: static struct notify_events_type events; // initialisation - void init(bool enable_external_leds); - - // add all backends - void add_backends(void); + void init(void); /// update - allow updates of leds that cannot be updated during a timed interrupt void update(void); @@ -163,6 +156,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; diff --git a/libraries/AP_Notify/Buzzer.cpp b/libraries/AP_Notify/Buzzer.cpp index 1e5699bfa7..6e29297866 100644 --- a/libraries/AP_Notify/Buzzer.cpp +++ b/libraries/AP_Notify/Buzzer.cpp @@ -57,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) {