1 files changed, 25 insertions, 14 deletions

diff --git a/quantum/audio/audio.c b/quantum/audio/audio.c
index 5b8563093..baa364eec 100644
--- a/quantum/audio/audio.c
+++ b/quantum/audio/audio.c
@@ -99,7 +99,6 @@ uint16_t note_position = 0;
 float (* notes_pointer)[][2];
 uint16_t notes_count;
 bool     notes_repeat;
-float    notes_rest;
 bool     note_resting = false;
 
 uint8_t current_note = 0;
@@ -180,7 +179,7 @@ void audio_init()
     audio_initialized = true;
 
     if (audio_config.enable) {
-        PLAY_NOTE_ARRAY(startup_song, false, LEGATO);
+        PLAY_SONG(startup_song);
     }
 
 }
@@ -416,9 +415,12 @@ ISR(TIMER3_COMPA_vect)
         note_position++;
         bool end_of_note = false;
         if (TIMER_3_PERIOD > 0) {
-            end_of_note = (note_position >= (note_length / TIMER_3_PERIOD * 0xFFFF));
+            if (!note_resting) 
+                end_of_note = (note_position >= (note_length / TIMER_3_PERIOD * 0xFFFF - 1));
+            else
+                end_of_note = (note_position >= (note_length));
         } else {
-            end_of_note = (note_position >= (note_length * 0x7FF));
+            end_of_note = (note_position >= (note_length));
         }
 
         if (end_of_note) {
@@ -433,11 +435,16 @@ ISR(TIMER3_COMPA_vect)
                     return;
                 }
             }
-            if (!note_resting && (notes_rest > 0)) {
+            if (!note_resting) {
                 note_resting = true;
-                note_frequency = 0;
-                note_length = notes_rest;
                 current_note--;
+                if ((*notes_pointer)[current_note][0] == (*notes_pointer)[current_note + 1][0]) {
+                    note_frequency = 0;
+                    note_length = 1;
+                } else {
+                    note_frequency = (*notes_pointer)[current_note][0];
+                    note_length = 1;
+                }
             } else {
                 note_resting = false;
                 envelope_index = 0;
@@ -548,9 +555,9 @@ ISR(TIMER1_COMPA_vect)
         note_position++;
         bool end_of_note = false;
         if (TIMER_1_PERIOD > 0) {
-            end_of_note = (note_position >= (note_length / TIMER_1_PERIOD * 0xFFFF));
+            end_of_note = (note_position >= (note_length / TIMER_1_PERIOD * 0xFFFF - 1));
         } else {
-            end_of_note = (note_position >= (note_length * 0x7FF));
+            end_of_note = (note_position >= (note_length));
         }
 
         if (end_of_note) {
@@ -565,11 +572,16 @@ ISR(TIMER1_COMPA_vect)
                     return;
                 }
             }
-            if (!note_resting && (notes_rest > 0)) {
+            if (!note_resting) {
                 note_resting = true;
-                note_frequency = 0;
-                note_length = notes_rest;
                 current_note--;
+                if ((*notes_pointer)[current_note][0] == (*notes_pointer)[current_note + 1][0]) {
+                    note_frequency = 0;
+                    note_length = 1;
+                } else {
+                    note_frequency = (*notes_pointer)[current_note][0];
+                    note_length = 1;
+                }
             } else {
                 note_resting = false;
                 envelope_index = 0;
@@ -638,7 +650,7 @@ void play_note(float freq, int vol) {
 
 }
 
-void play_notes(float (*np)[][2], uint16_t n_count, bool n_repeat, float n_rest)
+void play_notes(float (*np)[][2], uint16_t n_count, bool n_repeat)
 {
 
     if (!audio_initialized) {
@@ -663,7 +675,6 @@ void play_notes(float (*np)[][2], uint16_t n_count, bool n_repeat, float n_rest)
         notes_pointer = np;
         notes_count = n_count;
         notes_repeat = n_repeat;
-        notes_rest = n_rest;
 
         place = 0;
         current_note = 0;