vibrato and polyphony paratmeters

1 files changed, 110 insertions, 50 deletions

diff --git a/quantum/audio.c b/quantum/audio.c
index c92cb53731..8ea1bf6ff0 100644
--- a/quantum/audio.c
+++ b/quantum/audio.c
@@ -10,10 +10,13 @@
 
 #include "eeconfig.h"
 
+#include "vibrato_lut.h"
+
 #define PI 3.14159265
 
 #define CPU_PRESCALER 8
 
+// Largely untested PWM audio mode (doesn't sound as good)
 // #define PWM_AUDIO
 
 #ifdef PWM_AUDIO
@@ -67,10 +70,11 @@ bool note_resting = false;
 uint8_t current_note = 0;
 uint8_t rest_counter = 0;
 
-uint8_t vibrato_counter = 0;
-float vibrato_strength = 0;
+float vibrato_counter = 0;
+float vibrato_strength = .5;
+float vibrato_rate = 0.125;
 
-float polyphony_rate = 0;
+float polyphony_rate = .5;
 
 audio_config_t audio_config;
 
@@ -90,6 +94,81 @@ void audio_off(void) {
     eeconfig_write_audio(audio_config.raw);
 }
 
+// Vibrato rate functions
+
+void set_vibrato_rate(float rate) {
+    vibrato_rate = rate;
+}
+
+void increase_vibrato_rate(float change) {
+    vibrato_rate *= change;
+}
+
+void decrease_vibrato_rate(float change) {
+    vibrato_rate /= change;
+}
+
+#ifdef VIBRATO_STRENGTH_ENABLE
+
+void set_vibrato_strength(float strength) {
+    vibrato_strength = strength;
+}
+
+void increase_vibrato_strength(float change) {
+    vibrato_strength *= change;
+}
+
+void decrease_vibrato_strength(float change) {
+    vibrato_strength /= change;
+}
+
+#endif
+
+// Polyphony functions
+
+void set_polyphony_rate(float rate) {
+    polyphony_rate = rate;
+}
+
+void enable_polyphony() {
+    polyphony_rate = 5;
+}
+
+void disable_polyphony() {
+    polyphony_rate = 0;
+}
+
+void increase_polyphony_rate(float change) {
+    polyphony_rate *= change;
+}
+
+void decrease_polyphony_rate(float change) {
+    polyphony_rate /= change;
+}
+
+// Timbre function
+
+void set_timbre(float timbre) {
+    note_timbre = timbre;
+}
+
+// Tempo functions
+
+void set_tempo(float tempo) {
+    note_tempo = tempo;
+}
+
+void decrease_tempo(uint8_t tempo_change) {
+    note_tempo += (float) tempo_change;
+}
+
+void increase_tempo(uint8_t tempo_change) {
+    if (note_tempo - (float) tempo_change < 10) {
+        note_tempo = 10;
+    } else {
+        note_tempo -= (float) tempo_change;
+    }
+}
 
 void stop_all_notes() {
     voices = 0;
@@ -112,6 +191,7 @@ void stop_all_notes() {
 
 void stop_note(double freq) {
     if (note) {
+        cli();
         #ifdef PWM_AUDIO
             freq = freq / SAMPLE_RATE;
         #endif
@@ -125,6 +205,7 @@ void stop_note(double freq) {
                     volumes[j] = volumes[j+1];
                     volumes[j+1] = 0;
                 }
+                break;
             }
         }
         voices--;
@@ -144,6 +225,7 @@ void stop_note(double freq) {
             volume = 0;
             note = false;
         }
+        sei();
     }
 }
 
@@ -190,6 +272,16 @@ float mod(float a, int b)
     return r < 0 ? r + b : r;
 }
 
+float vibrato(float average_freq) {
+    #ifdef VIBRATO_STRENGTH_ENABLE
+        float vibrated_freq = average_freq * pow(VIBRATO_LUT[(int)vibrato_counter], vibrato_strength);
+    #else
+        float vibrated_freq = average_freq * VIBRATO_LUT[(int)vibrato_counter];
+    #endif
+    vibrato_counter = mod((vibrato_counter + vibrato_rate * (1.0 + 440.0/average_freq)), VIBRATO_LUT_LENGTH);
+    return vibrated_freq;
+}
+
 ISR(TIMER3_COMPA_vect) {
     if (note) {
         #ifdef PWM_AUDIO
@@ -241,34 +333,30 @@ ISR(TIMER3_COMPA_vect) {
             }
         #else
             if (voices > 0) {
-                if (false && polyphony_rate > 0) {                
+                if (polyphony_rate > 0) {                
                     if (voices > 1) {
                         voice_place %= voices;
-                        if (place++ > (frequencies[voice_place] / polyphony_rate / CPU_PRESCALER / voices)) {
+                        if (place++ > (frequencies[voice_place] / polyphony_rate / CPU_PRESCALER)) {
                             voice_place = (voice_place + 1) % voices;
                             place = 0.0;
                         }
                     }
                     if (vibrato_strength > 0) {
-                        freq = frequencies[voice_place] * pow(VIBRATO_LUT[(int)vibrato_counter], vibrato_strength);
-                        vibrato_counter = mod((vibrato_counter + 1), VIBRATO_LUT_LENGTH);
+                        freq = vibrato(frequencies[voice_place]);
                     } else {
                         freq = frequencies[voice_place];
                     } 
                 } else {
-                    if (frequency != 0) {
-                        if (frequency < frequencies[voices - 1]) {
-                            frequency = frequency * pow(2, 440/frequencies[voices - 1]/12/4);
-                        } else if (frequency > frequencies[voices - 1]) {
-                            frequency = frequency * pow(2, -440/frequencies[voices - 1]/12/4);
-                        }
+                    if (frequency != 0 && frequency < frequencies[voices - 1] && frequency < frequencies[voices - 1] * pow(2, -440/frequencies[voices - 1]/12/2)) {
+                        frequency = frequency * pow(2, 440/frequency/12/2);
+                    } else if (frequency != 0 && frequency > frequencies[voices - 1] && frequency > frequencies[voices - 1] * pow(2, 440/frequencies[voices - 1]/12/2)) {
+                        frequency = frequency * pow(2, -440/frequency/12/2);
                     } else {
                         frequency = frequencies[voices - 1];
                     }
 
-                    if (false && vibrato_strength > 0) {
-                        freq = frequency * pow(VIBRATO_LUT[(int)vibrato_counter], vibrato_strength);
-                        vibrato_counter = mod((vibrato_counter + 1 + 440/frequencies[voices - 1]), VIBRATO_LUT_LENGTH);
+                    if (vibrato_strength > 0) {
+                        freq = vibrato(frequency);
                     } else {
                         freq = frequency;
                     } 
@@ -302,9 +390,8 @@ ISR(TIMER3_COMPA_vect) {
             if (note_frequency > 0) {
                 float freq;
 
-                if (false && vibrato_strength > 0) {
-                    freq = note_frequency * pow(VIBRATO_LUT[(int)vibrato_counter], vibrato_strength);
-                    vibrato_counter = mod((vibrato_counter + 1), VIBRATO_LUT_LENGTH);
+                if (vibrato_strength > 0) {
+                    freq = vibrato(note_frequency);
                 } else {
                     freq = note_frequency;
                 }
@@ -369,7 +456,7 @@ ISR(TIMER3_COMPA_vect) {
 void play_notes(float (*np)[][2], uint8_t n_count, bool n_repeat, float n_rest) {
 
 if (audio_config.enable) {
-
+    cli();
 	// Cancel note if a note is playing
     if (note)
         stop_all_notes();
@@ -398,7 +485,7 @@ if (audio_config.enable) {
         TIMSK3 |= _BV(OCIE3A);
         TCCR3A |= _BV(COM3A1);
     #endif
-
+    sei();
 }
 
 }
@@ -425,7 +512,7 @@ if (audio_config.enable) {
 void play_note(double freq, int vol) {
 
 if (audio_config.enable && voices < 8) {
-
+    cli();
     // Cancel notes if notes are playing
     if (notes)
         stop_all_notes();
@@ -445,36 +532,9 @@ if (audio_config.enable && voices < 8) {
         TIMSK3 |= _BV(OCIE3A);
         TCCR3A |= _BV(COM3A1);
     #endif
-
-}
-
-}
-
-void set_timbre(float timbre)
-{
-	note_timbre = timbre;
+    sei();
 }
 
-void set_tempo(float tempo)
-{
-	note_tempo = tempo;
-}
-
-void decrease_tempo(uint8_t tempo_change)
-{
-	note_tempo += (float) tempo_change;
-}
-
-void increase_tempo(uint8_t tempo_change)
-{
-	if (note_tempo - (float) tempo_change < 10)
-		{
-			note_tempo = 10;
-		}
-	else
-		{
-		note_tempo -= (float) tempo_change;
-		}
 }
 
 //------------------------------------------------------------------------------