diff --git a/libraries/AP_Notify/Buzzer.cpp b/libraries/AP_Notify/Buzzer.cpp index 6e29297866..176df66926 100644 --- a/libraries/AP_Notify/Buzzer.cpp +++ b/libraries/AP_Notify/Buzzer.cpp @@ -56,119 +56,24 @@ bool Buzzer::init() // update - updates led according to timed_updated. Should be called at 50Hz void Buzzer::update() +{ + update_pattern_to_play(); + update_playing_pattern(); +} + +void Buzzer::update_pattern_to_play() { // check for arming failed event if (AP_Notify::events.arming_failed) { // arming failed buzz play_pattern(SINGLE_BUZZ); + return; } - // reduce 50hz call down to 10hz - _counter++; - if (_counter < 5) { + if (AP_HAL::millis() - _pattern_start_time < _pattern_start_interval_time_ms) { + // do not interrupt playing patterns / enforce minumum separation return; } - _counter = 0; - - // complete currently played pattern - if (_pattern != NONE) { - _pattern_counter++; - switch (_pattern) { - case SINGLE_BUZZ: - // buzz for 10th of a second - if (_pattern_counter == 1) { - on(true); - }else{ - on(false); - _pattern = NONE; - } - return; - case DOUBLE_BUZZ: - // buzz for 10th of a second - switch (_pattern_counter) { - case 1: - on(true); - break; - case 2: - on(false); - break; - case 3: - on(true); - break; - case 4: - default: - on(false); - _pattern = NONE; - break; - } - return; - case ARMING_BUZZ: - // record start time - if (_pattern_counter == 1) { - _arming_buzz_start_ms = AP_HAL::millis(); - on(true); - } else { - // turn off buzzer after 3 seconds - if (AP_HAL::millis() - _arming_buzz_start_ms >= BUZZER_ARMING_BUZZ_MS) { - _arming_buzz_start_ms = 0; - on(false); - _pattern = NONE; - } - } - return; - case BARO_GLITCH: - // four fast tones - switch (_pattern_counter) { - case 1: - case 3: - case 5: - case 7: - case 9: - on(true); - break; - case 2: - case 4: - case 6: - case 8: - on(false); - break; - case 10: - on(false); - _pattern = NONE; - break; - default: - // do nothing - break; - } - return; - case EKF_BAD: - // four tones getting shorter) - switch (_pattern_counter) { - case 1: - case 5: - case 8: - case 10: - on(true); - break; - case 4: - case 7: - case 9: - on(false); - break; - case 11: - on(false); - _pattern = NONE; - break; - default: - // do nothing - break; - } - return; - default: - // do nothing - break; - } - } // check if armed status has changed if (_flags.armed != AP_Notify::flags.armed) { @@ -196,12 +101,33 @@ void Buzzer::update() // if vehicle lost was enabled, starting beep if (AP_Notify::flags.vehicle_lost) { play_pattern(DOUBLE_BUZZ); + return; } // if battery failsafe constantly single buzz if (AP_Notify::flags.failsafe_battery) { play_pattern(SINGLE_BUZZ); + return; + } +} + + +void Buzzer::update_playing_pattern() +{ + if (_pattern == 0UL) { + return; + } + + const uint32_t now = AP_HAL::millis(); + const uint32_t delta = now - _pattern_start_time; + if (delta >= 3200) { + // finished playing pattern + on(false); + _pattern = 0UL; + return; } + const uint32_t bit = delta / 100UL; // each bit is 100ms + on(_pattern & (1U<<(31-bit))); } // on - turns the buzzer on or off @@ -220,9 +146,9 @@ void Buzzer::on(bool turn_on) } /// play_pattern - plays the defined buzzer pattern -void Buzzer::play_pattern(BuzzerPattern pattern_id) +void Buzzer::play_pattern(const uint32_t pattern) { - _pattern = pattern_id; - _pattern_counter = 0; + _pattern = pattern; + _pattern_start_time = AP_HAL::millis(); } diff --git a/libraries/AP_Notify/Buzzer.h b/libraries/AP_Notify/Buzzer.h index 1235e8f826..80567b5e58 100644 --- a/libraries/AP_Notify/Buzzer.h +++ b/libraries/AP_Notify/Buzzer.h @@ -19,8 +19,6 @@ #include -#define BUZZER_ARMING_BUZZ_MS 3000 // arming buzz length in milliseconds (i.e. 3 seconds) - #include "NotifyDevice.h" class Buzzer: public NotifyDevice @@ -35,24 +33,21 @@ public: /// update - updates buzzer according to timed_updated. Should be called at 50Hz void update() override; +private: + /// on - turns the buzzer on or off void on(bool on_off); - // Patterns - how many beeps will be played - enum BuzzerPattern { - NONE = 0, - SINGLE_BUZZ = 1, - DOUBLE_BUZZ = 2, - GPS_GLITCH = 3, // not used - ARMING_BUZZ = 4, - BARO_GLITCH = 5, - EKF_BAD = 6 - }; + // Patterns - how many beeps will be played; read from + // left-to-right, each bit represents 100ms + static const uint32_t SINGLE_BUZZ = 0b10000000000000000000000000000000UL; + static const uint32_t DOUBLE_BUZZ = 0b10100000000000000000000000000000UL; + static const uint32_t ARMING_BUZZ = 0b11111111111111111111111111111100UL; // 3s + static const uint32_t BARO_BUZZ = 0b10101010100000000000000000000000UL; + static const uint32_t EKF_BAD = 0b11101101010000000000000000000000UL; /// play_pattern - plays the defined buzzer pattern - void play_pattern(BuzzerPattern pattern_id); - -private: + void play_pattern(const uint32_t pattern); /// buzzer_flag_type - bitmask of current state and ap_notify states we track struct buzzer_flag_type { @@ -63,9 +58,14 @@ private: uint8_t ekf_bad : 1; // 1 if ekf position has gone bad } _flags; - uint8_t _counter; // reduces 50hz update down to 10hz for internal processing - BuzzerPattern _pattern; // current pattern - uint8_t _pattern_counter; // used to time on/off of current patter - uint32_t _arming_buzz_start_ms; // arming_buzz start time in milliseconds - uint8_t _pin; + uint32_t _pattern; // current pattern + uint8_t _pin; + uint32_t _pattern_start_time; + + // enforce minumum 100ms interval between patterns: + const uint16_t _pattern_start_interval_time_ms = 32*100 + 100; + + void update_playing_pattern(); + void update_pattern_to_play(); + };