AP_Notify: simplify buzzer playing

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();
 }
 

libraries/AP_Notify/Buzzer.h

@@ -19,8 +19,6 @@
 
 #include <AP_HAL/AP_HAL.h>
 
-#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();
+
 };