diff --git a/src/audio.cpp b/src/audio.cpp
index da7a14ade..857e64835 100644
--- a/src/audio.cpp
+++ b/src/audio.cpp
@@ -14,11 +14,20 @@
 
 #include "open9x.h"
 
+
+#if defined(PCBV4)
+#define SPEAKER_OFF  TCCR0A &= ~(1<<COM0A0)  // tone off
+#define SPEAKER_ON   TCCR0A |= (1<<COM0A0)   // tone on
+#else
+#define SPEAKER_OFF  PORTE &= ~(1 << OUT_E_BUZZER) // speaker output 'low'
+#endif
+
 audioQueue::audioQueue()
 {
   aqinit();
 }
 
+// TODO should not be needed
 void audioQueue::aqinit()
 {
   //make sure haptic off by default
@@ -31,6 +40,7 @@ void audioQueue::aqinit()
   t_queueRidx = 0;
 
 #ifdef HAPTIC
+  toneHaptic = 0;
   hapticTick = 0;
 #endif
 
@@ -60,22 +70,37 @@ void audioQueue::heartbeat()
   if (toneTimeLeft > 0) {
 #if defined(PCBV4)
     OCR0A = (5000 / toneFreq); // sticking with old values approx 20(abs. min) to 90, 60 being the default tone(?).
-    TCCR0A |= (1<<COM0A0);  // tone on
+    SPEAKER_ON;
 #endif
     toneTimeLeft--; //time gets counted down
-    toneFreq += toneFreqIncr; // -2, 0 or 2
+    toneFreq += toneFreqIncr;
+
+#if defined(HAPTIC)
+    if (toneHaptic){
+      if (hapticTick-- >= 0) {
+        HAPTIC_ON; // haptic output 'high'
+      }
+      else {
+        HAPTIC_OFF; // haptic output 'low'
+        hapticTick = g_eeGeneral.hapticStrength;
+      }
+    }
+#endif
   }
   else {
-#if defined(PCBV4)
-    TCCR0A &= ~(1<<COM0A0);  // tone off
-#else
-    PORTE &= ~(1 << OUT_E_BUZZER); // speaker output 'low'
-#endif
+    SPEAKER_OFF;
+    HAPTIC_OFF;
+
     if (tonePause-- <= 0) {
       if (t_queueRidx != t_queueWidx) {
         toneFreq = queueToneFreq[t_queueRidx];
         toneTimeLeft = queueToneLength[t_queueRidx];
         toneFreqIncr = queueToneFreqIncr[t_queueRidx];
+        tonePause = queueTonePause[t_queueRidx];
+#if defined(HAPTIC)
+        toneHaptic = queueToneHaptic[t_queueRidx];
+        hapticTick = 0;
+#endif
         if (queueToneRepeat[t_queueRidx]--) {
           t_queueRidx = (t_queueRidx + 1) % AUDIO_QUEUE_LENGTH;
         }
@@ -84,23 +109,6 @@ void audioQueue::heartbeat()
   }
 }
 
-#ifdef HAPTIC
-    uint8_t hapticStrength = g_eeGeneral.hapticStrength;
-    if (toneHaptic == 1) {
-      if ((hapticTick <= hapticStrength - 1) && hapticStrength > 0) {
-        HAPTIC_ON; // haptic output 'high'
-        hapticTick++;
-      }
-      else {
-        HAPTIC_OFF; //haptic output low
-        hapticTick = 0;
-      }
-    }
-    else {
-      HAPTIC_OFF; // haptic output 'low'
-    }
-#endif
-
 inline uint8_t audioQueue::getToneLength(uint8_t tLen)
 {
   uint8_t result = tLen; // default
@@ -128,9 +136,10 @@ void audioQueue::playNow(uint8_t tFreq, uint8_t tLen, uint8_t tPause,
     toneFreq = tFreq + g_eeGeneral.speakerPitch + BEEP_OFFSET; // add pitch compensator
     toneTimeLeft = getToneLength(tLen);
     tonePause = tPause;
-  #ifdef HAPTIC
-    t_queueToneHaptic = tHaptic;
-  #endif
+#if defined(HAPTIC)
+    toneHaptic = tHaptic;
+    hapticTick = 0;
+#endif
     toneFreqIncr = tFreqIncr;
     t_queueWidx = t_queueRidx;
 
@@ -147,7 +156,7 @@ void audioQueue::playASAP(uint8_t tFreq, uint8_t tLen, uint8_t tPause,
     queueToneFreq[t_queueWidx] = tFreq + g_eeGeneral.speakerPitch + BEEP_OFFSET; // add pitch compensator
     queueToneLength[t_queueWidx] = getToneLength(tLen);
     queueTonePause[t_queueWidx] = tPause;
-#ifdef HAPTIC
+#if defined(HAPTIC)
     queueToneHaptic[t_queueWidx] = tHaptic;
 #endif
     queueToneRepeat[t_queueWidx] = tRepeat;
diff --git a/src/audio.h b/src/audio.h
index fde16cda3..e48c7066d 100644
--- a/src/audio.h
+++ b/src/audio.h
@@ -22,11 +22,6 @@
 #define BEEP_KEY_UP_FREQ  (BEEP_DEFAULT_FREQ+5)
 #define BEEP_KEY_DOWN_FREQ (BEEP_DEFAULT_FREQ-5)
 
-// #define AUDIO_QUEUE_HEARTBEAT_XSHORT (30) //speaker timing [Norm]
-// #define AUDIO_QUEUE_HEARTBEAT_SHORT (50) //speaker timing [Norm]
-// #define AUDIO_QUEUE_HEARTBEAT_NORM (77) //speaker timing [Norm]
-// #define AUDIO_QUEUE_HEARTBEAT_LONG (130) //speaker timing [Long]
-// #define AUDIO_QUEUE_HEARTBEAT_XLONG (200) //speaker timing [xLong]
 #ifdef HAPTIC
 #define HAPTIC_ON    PORTG |=  (1<<2)
 #define HAPTIC_OFF   PORTG &= ~(1<<2)