quantum/audio/audio_arm.c

   1 /* Copyright 2016 Jack Humbert
   2  *
   3  * This program is free software: you can redistribute it and/or modify
   4  * it under the terms of the GNU General Public License as published by
   5  * the Free Software Foundation, either version 2 of the License, or
   6  * (at your option) any later version.
   7  *
   8  * This program is distributed in the hope that it will be useful,
   9  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  10  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  11  * GNU General Public License for more details.
  12  *
  13  * You should have received a copy of the GNU General Public License
  14  * along with this program.  If not, see <http://www.gnu.org/licenses/>.
  15  */
  16
  17 #include "audio.h"
  18 #include "ch.h"
  19 #include "hal.h"
  20
  21 #include <string.h>
  22 #include "print.h"
  23 #include "keymap.h"
  24
  25 #include "eeconfig.h"
  26
  27 // -----------------------------------------------------------------------------
  28
  29 int voices = 0;
  30 int voice_place = 0;
  31 float frequency = 0;
  32 float frequency_alt = 0;
  33 int volume = 0;
  34 long position = 0;
  35
  36 float frequencies[8] = {0, 0, 0, 0, 0, 0, 0, 0};
  37 int volumes[8] = {0, 0, 0, 0, 0, 0, 0, 0};
  38 bool sliding = false;
  39
  40 float place = 0;
  41
  42 uint8_t * sample;
  43 uint16_t sample_length = 0;
  44
  45 bool     playing_notes = false;
  46 bool     playing_note = false;
  47 float    note_frequency = 0;
  48 float    note_length = 0;
  49 uint8_t  note_tempo = TEMPO_DEFAULT;
  50 float    note_timbre = TIMBRE_DEFAULT;
  51 uint16_t note_position = 0;
  52 float (* notes_pointer)[][2];
  53 uint16_t notes_count;
  54 bool     notes_repeat;
  55 bool     note_resting = false;
  56
  57 uint8_t current_note = 0;
  58 uint8_t rest_counter = 0;
  59
  60 #ifdef VIBRATO_ENABLE
  61 float vibrato_counter = 0;
  62 float vibrato_strength = .5;
  63 float vibrato_rate = 0.125;
  64 #endif
  65
  66 float polyphony_rate = 0;
  67
  68 static bool audio_initialized = false;
  69
  70 audio_config_t audio_config;
  71
  72 uint16_t envelope_index = 0;
  73 bool glissando = true;
  74
  75 #ifndef STARTUP_SONG
  76     #define STARTUP_SONG SONG(STARTUP_SOUND)
  77 #endif
  78 float startup_song[][2] = STARTUP_SONG;
  79
  80 static void gpt_cb8(GPTDriver *gptp);
  81
  82 #define DAC_BUFFER_SIZE 360
  83
  84 #define START_CHANNEL_1() gptStart(&GPTD6, &gpt6cfg1); \
  85     gptStartContinuous(&GPTD6, 2U)
  86 #define START_CHANNEL_2() gptStart(&GPTD7, &gpt7cfg1); \
  87     gptStartContinuous(&GPTD7, 2U)
  88 #define STOP_CHANNEL_1() gptStopTimer(&GPTD6)
  89 #define STOP_CHANNEL_2() gptStopTimer(&GPTD7)
  90 #define RESTART_CHANNEL_1() STOP_CHANNEL_1(); \
  91     START_CHANNEL_1()
  92 #define RESTART_CHANNEL_2() STOP_CHANNEL_2(); \
  93     START_CHANNEL_2()
  94 #define UPDATE_CHANNEL_1_FREQ(freq) gpt6cfg1.frequency = freq * DAC_BUFFER_SIZE; \
  95     RESTART_CHANNEL_1()
  96 #define UPDATE_CHANNEL_2_FREQ(freq) gpt7cfg1.frequency = freq * DAC_BUFFER_SIZE; \
  97     RESTART_CHANNEL_2()
  98 #define GET_CHANNEL_1_FREQ gpt6cfg1.frequency
  99 #define GET_CHANNEL_2_FREQ gpt7cfg1.frequency
 100
 101
 102 /*
 103  * GPT6 configuration.
 104  */
 105 // static const GPTConfig gpt6cfg1 = {
 106 //   .frequency    = 1000000U,
 107 //   .callback     = NULL,
 108 //   .cr2          = TIM_CR2_MMS_1,    /* MMS = 010 = TRGO on Update Event.    */
 109 //   .dier         = 0U
 110 // };
 111
 112 GPTConfig gpt6cfg1 = {
 113   .frequency    = 440U*DAC_BUFFER_SIZE,
 114   .callback     = NULL,
 115   .cr2          = TIM_CR2_MMS_1,    /* MMS = 010 = TRGO on Update Event.    */
 116   .dier         = 0U
 117 };
 118
 119 GPTConfig gpt7cfg1 = {
 120   .frequency    = 440U*DAC_BUFFER_SIZE,
 121   .callback     = NULL,
 122   .cr2          = TIM_CR2_MMS_1,    /* MMS = 010 = TRGO on Update Event.    */
 123   .dier         = 0U
 124 };
 125
 126 GPTConfig gpt8cfg1 = {
 127   .frequency    = 10,
 128   .callback     = gpt_cb8,
 129   .cr2          = TIM_CR2_MMS_1,    /* MMS = 010 = TRGO on Update Event.    */
 130   .dier         = 0U
 131 };
 132
 133
 134 /*
 135  * DAC test buffer (sine wave).
 136  */
 137 // static const dacsample_t dac_buffer[DAC_BUFFER_SIZE] = {
 138 //   2047, 2082, 2118, 2154, 2189, 2225, 2260, 2296, 2331, 2367, 2402, 2437,
 139 //   2472, 2507, 2542, 2576, 2611, 2645, 2679, 2713, 2747, 2780, 2813, 2846,
 140 //   2879, 2912, 2944, 2976, 3008, 3039, 3070, 3101, 3131, 3161, 3191, 3221,
 141 //   3250, 3278, 3307, 3335, 3362, 3389, 3416, 3443, 3468, 3494, 3519, 3544,
 142 //   3568, 3591, 3615, 3637, 3660, 3681, 3703, 3723, 3744, 3763, 3782, 3801,
 143 //   3819, 3837, 3854, 3870, 3886, 3902, 3917, 3931, 3944, 3958, 3970, 3982,
 144 //   3993, 4004, 4014, 4024, 4033, 4041, 4049, 4056, 4062, 4068, 4074, 4078,
 145 //   4082, 4086, 4089, 4091, 4092, 4093, 4094, 4093, 4092, 4091, 4089, 4086,
 146 //   4082, 4078, 4074, 4068, 4062, 4056, 4049, 4041, 4033, 4024, 4014, 4004,
 147 //   3993, 3982, 3970, 3958, 3944, 3931, 3917, 3902, 3886, 3870, 3854, 3837,
 148 //   3819, 3801, 3782, 3763, 3744, 3723, 3703, 3681, 3660, 3637, 3615, 3591,
 149 //   3568, 3544, 3519, 3494, 3468, 3443, 3416, 3389, 3362, 3335, 3307, 3278,
 150 //   3250, 3221, 3191, 3161, 3131, 3101, 3070, 3039, 3008, 2976, 2944, 2912,
 151 //   2879, 2846, 2813, 2780, 2747, 2713, 2679, 2645, 2611, 2576, 2542, 2507,
 152 //   2472, 2437, 2402, 2367, 2331, 2296, 2260, 2225, 2189, 2154, 2118, 2082,
 153 //   2047, 2012, 1976, 1940, 1905, 1869, 1834, 1798, 1763, 1727, 1692, 1657,
 154 //   1622, 1587, 1552, 1518, 1483, 1449, 1415, 1381, 1347, 1314, 1281, 1248,
 155 //   1215, 1182, 1150, 1118, 1086, 1055, 1024,  993,  963,  933,  903,  873,
 156 //    844,  816,  787,  759,  732,  705,  678,  651,  626,  600,  575,  550,
 157 //    526,  503,  479,  457,  434,  413,  391,  371,  350,  331,  312,  293,
 158 //    275,  257,  240,  224,  208,  192,  177,  163,  150,  136,  124,  112,
 159 //    101,   90,   80,   70,   61,   53,   45,   38,   32,   26,   20,   16,
 160 //     12,    8,    5,    3,    2,    1,    0,    1,    2,    3,    5,    8,
 161 //     12,   16,   20,   26,   32,   38,   45,   53,   61,   70,   80,   90,
 162 //    101,  112,  124,  136,  150,  163,  177,  192,  208,  224,  240,  257,
 163 //    275,  293,  312,  331,  350,  371,  391,  413,  434,  457,  479,  503,
 164 //    526,  550,  575,  600,  626,  651,  678,  705,  732,  759,  787,  816,
 165 //    844,  873,  903,  933,  963,  993, 1024, 1055, 1086, 1118, 1150, 1182,
 166 //   1215, 1248, 1281, 1314, 1347, 1381, 1415, 1449, 1483, 1518, 1552, 1587,
 167 //   1622, 1657, 1692, 1727, 1763, 1798, 1834, 1869, 1905, 1940, 1976, 2012
 168 // };
 169
 170 // squarewave
 171 static const dacsample_t dac_buffer[DAC_BUFFER_SIZE] = {
 172   2047, 2047, 2047, 2047, 2047, 2047, 2047, 2047, 2047, 2047, 2047, 2047,
 173   2047, 2047, 2047, 2047, 2047, 2047, 2047, 2047, 2047, 2047, 2047, 2047,
 174   2047, 2047, 2047, 2047, 2047, 2047, 2047, 2047, 2047, 2047, 2047, 2047,
 175   2047, 2047, 2047, 2047, 2047, 2047, 2047, 2047, 2047, 2047, 2047, 2047,
 176   2047, 2047, 2047, 2047, 2047, 2047, 2047, 2047, 2047, 2047, 2047, 2047,
 177   2047, 2047, 2047, 2047, 2047, 2047, 2047, 2047, 2047, 2047, 2047, 2047,
 178   2047, 2047, 2047, 2047, 2047, 2047, 2047, 2047, 2047, 2047, 2047, 2047,
 179   2047, 2047, 2047, 2047, 2047, 2047, 2047, 2047, 2047, 2047, 2047, 2047,
 180   2047, 2047, 2047, 2047, 2047, 2047, 2047, 2047, 2047, 2047, 2047, 2047,
 181   2047, 2047, 2047, 2047, 2047, 2047, 2047, 2047, 2047, 2047, 2047, 2047,
 182   2047, 2047, 2047, 2047, 2047, 2047, 2047, 2047, 2047, 2047, 2047, 2047,
 183   2047, 2047, 2047, 2047, 2047, 2047, 2047, 2047, 2047, 2047, 2047, 2047,
 184   2047, 2047, 2047, 2047, 2047, 2047, 2047, 2047, 2047, 2047, 2047, 2047,
 185   2047, 2047, 2047, 2047, 2047, 2047, 2047, 2047, 2047, 2047, 2047, 2047,
 186   2047, 2047, 2047, 2047, 2047, 2047, 2047, 2047, 2047, 2047, 2047, 2047,
 187
 188   0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
 189   0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
 190   0, 0, 0, 0, 0, 0, 0,  0,  0,  0,  0,  0,
 191    0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,
 192    0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,
 193    0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,
 194    0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,
 195     0,    0,    0,    0,    0,    0,    0,    0,    0,    0,    0,    0,
 196     0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,
 197    0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,
 198    0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,
 199    0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,
 200    0,  0,  0,  0,  0,  0, 0, 0, 0, 0, 0, 0,
 201   0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
 202   0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0
 203 };
 204
 205 // squarewave
 206 static const dacsample_t dac_buffer_2[DAC_BUFFER_SIZE] = {
 207   2047, 2047, 2047, 2047, 2047, 2047, 2047, 2047, 2047, 2047, 2047, 2047,
 208   2047, 2047, 2047, 2047, 2047, 2047, 2047, 2047, 2047, 2047, 2047, 2047,
 209   2047, 2047, 2047, 2047, 2047, 2047, 2047, 2047, 2047, 2047, 2047, 2047,
 210   2047, 2047, 2047, 2047, 2047, 2047, 2047, 2047, 2047, 2047, 2047, 2047,
 211   2047, 2047, 2047, 2047, 2047, 2047, 2047, 2047, 2047, 2047, 2047, 2047,
 212   2047, 2047, 2047, 2047, 2047, 2047, 2047, 2047, 2047, 2047, 2047, 2047,
 213   2047, 2047, 2047, 2047, 2047, 2047, 2047, 2047, 2047, 2047, 2047, 2047,
 214   2047, 2047, 2047, 2047, 2047, 2047, 2047, 2047, 2047, 2047, 2047, 2047,
 215   2047, 2047, 2047, 2047, 2047, 2047, 2047, 2047, 2047, 2047, 2047, 2047,
 216   2047, 2047, 2047, 2047, 2047, 2047, 2047, 2047, 2047, 2047, 2047, 2047,
 217   2047, 2047, 2047, 2047, 2047, 2047, 2047, 2047, 2047, 2047, 2047, 2047,
 218   2047, 2047, 2047, 2047, 2047, 2047, 2047, 2047, 2047, 2047, 2047, 2047,
 219   2047, 2047, 2047, 2047, 2047, 2047, 2047, 2047, 2047, 2047, 2047, 2047,
 220   2047, 2047, 2047, 2047, 2047, 2047, 2047, 2047, 2047, 2047, 2047, 2047,
 221   2047, 2047, 2047, 2047, 2047, 2047, 2047, 2047, 2047, 2047, 2047, 2047,
 222
 223   0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
 224   0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
 225   0, 0, 0, 0, 0, 0, 0,  0,  0,  0,  0,  0,
 226    0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,
 227    0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,
 228    0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,
 229    0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,
 230     0,    0,    0,    0,    0,    0,    0,    0,    0,    0,    0,    0,
 231     0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,
 232    0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,
 233    0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,
 234    0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,
 235    0,  0,  0,  0,  0,  0, 0, 0, 0, 0, 0, 0,
 236   0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
 237   0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0
 238 };
 239
 240 /*
 241  * DAC streaming callback.
 242  */
 243 size_t nx = 0, ny = 0, nz = 0;
 244 static void end_cb1(DACDriver *dacp, dacsample_t *buffer, size_t n) {
 245
 246   (void)dacp;
 247
 248   nz++;
 249   if (dac_buffer == buffer) {
 250     nx += n;
 251   }
 252   else {
 253     ny += n;
 254   }
 255
 256   if ((nz % 1000) == 0) {
 257     // palTogglePad(GPIOD, GPIOD_LED3);
 258   }
 259 }
 260
 261 /*
 262  * DAC error callback.
 263  */
 264 static void error_cb1(DACDriver *dacp, dacerror_t err) {
 265
 266   (void)dacp;
 267   (void)err;
 268
 269   chSysHalt("DAC failure");
 270 }
 271
 272 static const DACConfig dac1cfg1 = {
 273   .init         = 2047U,
 274   .datamode     = DAC_DHRM_12BIT_RIGHT
 275 };
 276
 277 static const DACConversionGroup dacgrpcfg1 = {
 278   .num_channels = 1U,
 279   .end_cb       = end_cb1,
 280   .error_cb     = error_cb1,
 281   .trigger      = DAC_TRG(0)
 282 };
 283
 284 static const DACConfig dac1cfg2 = {
 285   .init         = 2047U,
 286   .datamode     = DAC_DHRM_12BIT_RIGHT
 287 };
 288
 289 static const DACConversionGroup dacgrpcfg2 = {
 290   .num_channels = 1U,
 291   .end_cb       = end_cb1,
 292   .error_cb     = error_cb1,
 293   .trigger      = DAC_TRG(0)
 294 };
 295
 296 void audio_init()
 297 {
 298
 299     if (audio_initialized)
 300         return;
 301
 302     // Check EEPROM
 303     // if (!eeconfig_is_enabled())
 304     // {
 305     //     eeconfig_init();
 306     // }
 307     // audio_config.raw = eeconfig_read_audio();
 308     audio_config.enable = true;
 309
 310   /*
 311    * Starting DAC1 driver, setting up the output pin as analog as suggested
 312    * by the Reference Manual.
 313    */
 314   palSetPadMode(GPIOA, 4, PAL_MODE_INPUT_ANALOG);
 315   palSetPadMode(GPIOA, 5, PAL_MODE_INPUT_ANALOG);
 316   dacStart(&DACD1, &dac1cfg1);
 317   dacStart(&DACD2, &dac1cfg2);
 318
 319   /*
 320    * Starting GPT6 driver, it is used for triggering the DAC.
 321    */
 322   START_CHANNEL_1();
 323   START_CHANNEL_2();
 324
 325   /*
 326    * Starting a continuous conversion.
 327    */
 328   dacStartConversion(&DACD1, &dacgrpcfg1,
 329                      (dacsample_t *)dac_buffer, DAC_BUFFER_SIZE);
 330   dacStartConversion(&DACD2, &dacgrpcfg2,
 331                      (dacsample_t *)dac_buffer_2, DAC_BUFFER_SIZE);
 332   // gptStartContinuous(&GPTD6, 2U);
 333
 334
 335     audio_initialized = true;
 336
 337     if (audio_config.enable) {
 338         PLAY_SONG(startup_song);
 339     }
 340
 341 }
 342
 343 void stop_all_notes()
 344 {
 345     dprintf("audio stop all notes");
 346
 347     if (!audio_initialized) {
 348         audio_init();
 349     }
 350     voices = 0;
 351
 352     gptStopTimer(&GPTD6);
 353     gptStopTimer(&GPTD7);
 354     gptStopTimer(&GPTD8);
 355
 356     playing_notes = false;
 357     playing_note = false;
 358     frequency = 0;
 359     frequency_alt = 0;
 360     volume = 0;
 361
 362     for (uint8_t i = 0; i < 8; i++)
 363     {
 364         frequencies[i] = 0;
 365         volumes[i] = 0;
 366     }
 367 }
 368
 369 void stop_note(float freq)
 370 {
 371     dprintf("audio stop note freq=%d", (int)freq);
 372
 373     if (playing_note) {
 374         if (!audio_initialized) {
 375             audio_init();
 376         }
 377         for (int i = 7; i >= 0; i--) {
 378             if (frequencies[i] == freq) {
 379                 frequencies[i] = 0;
 380                 volumes[i] = 0;
 381                 for (int j = i; (j < 7); j++) {
 382                     frequencies[j] = frequencies[j+1];
 383                     frequencies[j+1] = 0;
 384                     volumes[j] = volumes[j+1];
 385                     volumes[j+1] = 0;
 386                 }
 387                 break;
 388             }
 389         }
 390         voices--;
 391         if (voices < 0)
 392             voices = 0;
 393         if (voice_place >= voices) {
 394             voice_place = 0;
 395         }
 396         if (voices == 0) {
 397             STOP_CHANNEL_1();
 398             STOP_CHANNEL_2();
 399             gptStopTimer(&GPTD8);
 400             frequency = 0;
 401             frequency_alt = 0;
 402             volume = 0;
 403             playing_note = false;
 404         }
 405     }
 406 }
 407
 408 #ifdef VIBRATO_ENABLE
 409
 410 float mod(float a, int b)
 411 {
 412     float r = fmod(a, b);
 413     return r < 0 ? r + b : r;
 414 }
 415
 416 float vibrato(float average_freq) {
 417     #ifdef VIBRATO_STRENGTH_ENABLE
 418         float vibrated_freq = average_freq * pow(vibrato_lut[(int)vibrato_counter], vibrato_strength);
 419     #else
 420         float vibrated_freq = average_freq * vibrato_lut[(int)vibrato_counter];
 421     #endif
 422     vibrato_counter = mod((vibrato_counter + vibrato_rate * (1.0 + 440.0/average_freq)), VIBRATO_LUT_LENGTH);
 423     return vibrated_freq;
 424 }
 425
 426 #endif
 427
 428 static void gpt_cb8(GPTDriver *gptp) {
 429     float freq;
 430
 431     if (playing_note) {
 432         if (voices > 0) {
 433
 434             float freq_alt = 0;
 435                 if (voices > 1) {
 436                     if (polyphony_rate == 0) {
 437                         if (glissando) {
 438                             if (frequency_alt != 0 && frequency_alt < frequencies[voices - 2] && frequency_alt < frequencies[voices - 2] * pow(2, -440/frequencies[voices - 2]/12/2)) {
 439                                 frequency_alt = frequency_alt * pow(2, 440/frequency_alt/12/2);
 440                             } else if (frequency_alt != 0 && frequency_alt > frequencies[voices - 2] && frequency_alt > frequencies[voices - 2] * pow(2, 440/frequencies[voices - 2]/12/2)) {
 441                                 frequency_alt = frequency_alt * pow(2, -440/frequency_alt/12/2);
 442                             } else {
 443                                 frequency_alt = frequencies[voices - 2];
 444                             }
 445                         } else {
 446                             frequency_alt = frequencies[voices - 2];
 447                         }
 448
 449                         #ifdef VIBRATO_ENABLE
 450                             if (vibrato_strength > 0) {
 451                                 freq_alt = vibrato(frequency_alt);
 452                             } else {
 453                                 freq_alt = frequency_alt;
 454                             }
 455                         #else
 456                             freq_alt = frequency_alt;
 457                         #endif
 458                     }
 459
 460                     if (envelope_index < 65535) {
 461                         envelope_index++;
 462                     }
 463
 464                     freq_alt = voice_envelope(freq_alt);
 465
 466                     if (freq_alt < 30.517578125) {
 467                         freq_alt = 30.52;
 468                     }
 469
 470                     if (GET_CHANNEL_2_FREQ != (uint16_t)freq_alt) {
 471                         UPDATE_CHANNEL_2_FREQ(freq_alt);
 472                     }
 473                     //note_timbre;
 474                 }
 475
 476             if (polyphony_rate > 0) {
 477                 if (voices > 1) {
 478                     voice_place %= voices;
 479                     if (place++ > (frequencies[voice_place] / polyphony_rate)) {
 480                         voice_place = (voice_place + 1) % voices;
 481                         place = 0.0;
 482                     }
 483                 }
 484
 485                 #ifdef VIBRATO_ENABLE
 486                     if (vibrato_strength > 0) {
 487                         freq = vibrato(frequencies[voice_place]);
 488                     } else {
 489                         freq = frequencies[voice_place];
 490                     }
 491                 #else
 492                     freq = frequencies[voice_place];
 493                 #endif
 494             } else {
 495                 if (glissando) {
 496                     if (frequency != 0 && frequency < frequencies[voices - 1] && frequency < frequencies[voices - 1] * pow(2, -440/frequencies[voices - 1]/12/2)) {
 497                         frequency = frequency * pow(2, 440/frequency/12/2);
 498                     } else if (frequency != 0 && frequency > frequencies[voices - 1] && frequency > frequencies[voices - 1] * pow(2, 440/frequencies[voices - 1]/12/2)) {
 499                         frequency = frequency * pow(2, -440/frequency/12/2);
 500                     } else {
 501                         frequency = frequencies[voices - 1];
 502                     }
 503                 } else {
 504                     frequency = frequencies[voices - 1];
 505                 }
 506
 507                 #ifdef VIBRATO_ENABLE
 508                     if (vibrato_strength > 0) {
 509                         freq = vibrato(frequency);
 510                     } else {
 511                         freq = frequency;
 512                     }
 513                 #else
 514                     freq = frequency;
 515                 #endif
 516             }
 517
 518             if (envelope_index < 65535) {
 519                 envelope_index++;
 520             }
 521
 522             freq = voice_envelope(freq);
 523
 524             if (freq < 30.517578125) {
 525                 freq = 30.52;
 526             }
 527
 528
 529             if (GET_CHANNEL_1_FREQ != (uint16_t)freq) {
 530                 UPDATE_CHANNEL_1_FREQ(freq);
 531             }
 532             //note_timbre;
 533         }
 534     }
 535
 536     if (playing_notes) {
 537         if (note_frequency > 0) {
 538             #ifdef VIBRATO_ENABLE
 539                 if (vibrato_strength > 0) {
 540                     freq = vibrato(note_frequency);
 541                 } else {
 542                     freq = note_frequency;
 543                 }
 544             #else
 545                     freq = note_frequency;
 546             #endif
 547
 548             if (envelope_index < 65535) {
 549                 envelope_index++;
 550             }
 551             freq = voice_envelope(freq);
 552
 553
 554             if (GET_CHANNEL_1_FREQ != (uint16_t)freq) {
 555                 UPDATE_CHANNEL_1_FREQ(freq);
 556                 UPDATE_CHANNEL_2_FREQ(freq);
 557             }
 558             //note_timbre;
 559         } else {
 560             // gptStopTimer(&GPTD6);
 561             // gptStopTimer(&GPTD7);
 562         }
 563
 564         note_position++;
 565         bool end_of_note = false;
 566         if (GET_CHANNEL_1_FREQ > 0) {
 567             if (!note_resting)
 568                 end_of_note = (note_position >= (note_length*16 - 1));
 569             else
 570                 end_of_note = (note_position >= (note_length*16));
 571         } else {
 572             end_of_note = (note_position >= (note_length*16));
 573         }
 574
 575         if (end_of_note) {
 576             current_note++;
 577             if (current_note >= notes_count) {
 578                 if (notes_repeat) {
 579                     current_note = 0;
 580                 } else {
 581                     STOP_CHANNEL_1();
 582                     STOP_CHANNEL_2();
 583                     // gptStopTimer(&GPTD8);
 584                     playing_notes = false;
 585                     return;
 586                 }
 587             }
 588             if (!note_resting) {
 589                 note_resting = true;
 590                 current_note--;
 591                 if ((*notes_pointer)[current_note][0] == (*notes_pointer)[current_note + 1][0]) {
 592                     note_frequency = 0;
 593                     note_length = 1;
 594                 } else {
 595                     note_frequency = (*notes_pointer)[current_note][0];
 596                     note_length = 1;
 597                 }
 598             } else {
 599                 note_resting = false;
 600                 envelope_index = 0;
 601                 note_frequency = (*notes_pointer)[current_note][0];
 602                 note_length = ((*notes_pointer)[current_note][1] / 4) * (((float)note_tempo) / 100);
 603             }
 604
 605             note_position = 0;
 606         }
 607     }
 608
 609     if (!audio_config.enable) {
 610         playing_notes = false;
 611         playing_note = false;
 612     }
 613 }
 614
 615 void play_note(float freq, int vol) {
 616
 617     dprintf("audio play note freq=%d vol=%d", (int)freq, vol);
 618
 619     if (!audio_initialized) {
 620         audio_init();
 621     }
 622
 623     if (audio_config.enable && voices < 8) {
 624
 625         // Cancel notes if notes are playing
 626         if (playing_notes)
 627             stop_all_notes();
 628
 629         playing_note = true;
 630
 631         envelope_index = 0;
 632
 633         if (freq > 0) {
 634             frequencies[voices] = freq;
 635             volumes[voices] = vol;
 636             voices++;
 637         }
 638
 639         gptStart(&GPTD8, &gpt8cfg1);
 640         gptStartContinuous(&GPTD8, 2U);
 641         RESTART_CHANNEL_1();
 642         RESTART_CHANNEL_2();
 643     }
 644
 645 }
 646
 647 void play_notes(float (*np)[][2], uint16_t n_count, bool n_repeat)
 648 {
 649
 650     if (!audio_initialized) {
 651         audio_init();
 652     }
 653
 654     if (audio_config.enable) {
 655
 656         // Cancel note if a note is playing
 657         if (playing_note)
 658             stop_all_notes();
 659
 660         playing_notes = true;
 661
 662         notes_pointer = np;
 663         notes_count = n_count;
 664         notes_repeat = n_repeat;
 665
 666         place = 0;
 667         current_note = 0;
 668
 669         note_frequency = (*notes_pointer)[current_note][0];
 670         note_length = ((*notes_pointer)[current_note][1] / 4) * (((float)note_tempo) / 100);
 671         note_position = 0;
 672
 673         gptStart(&GPTD8, &gpt8cfg1);
 674         gptStartContinuous(&GPTD8, 2U);
 675         RESTART_CHANNEL_1();
 676         RESTART_CHANNEL_2();
 677     }
 678
 679 }
 680
 681 bool is_playing_notes(void) {
 682     return playing_notes;
 683 }
 684
 685 bool is_audio_on(void) {
 686     return (audio_config.enable != 0);
 687 }
 688
 689 void audio_toggle(void) {
 690     audio_config.enable ^= 1;
 691     eeconfig_update_audio(audio_config.raw);
 692     if (audio_config.enable)
 693         audio_on_user();
 694 }
 695
 696 void audio_on(void) {
 697     audio_config.enable = 1;
 698     eeconfig_update_audio(audio_config.raw);
 699     audio_on_user();
 700 }
 701
 702 void audio_off(void) {
 703     audio_config.enable = 0;
 704     eeconfig_update_audio(audio_config.raw);
 705 }
 706
 707 #ifdef VIBRATO_ENABLE
 708
 709 // Vibrato rate functions
 710
 711 void set_vibrato_rate(float rate) {
 712     vibrato_rate = rate;
 713 }
 714
 715 void increase_vibrato_rate(float change) {
 716     vibrato_rate *= change;
 717 }
 718
 719 void decrease_vibrato_rate(float change) {
 720     vibrato_rate /= change;
 721 }
 722
 723 #ifdef VIBRATO_STRENGTH_ENABLE
 724
 725 void set_vibrato_strength(float strength) {
 726     vibrato_strength = strength;
 727 }
 728
 729 void increase_vibrato_strength(float change) {
 730     vibrato_strength *= change;
 731 }
 732
 733 void decrease_vibrato_strength(float change) {
 734     vibrato_strength /= change;
 735 }
 736
 737 #endif  /* VIBRATO_STRENGTH_ENABLE */
 738
 739 #endif /* VIBRATO_ENABLE */
 740
 741 // Polyphony functions
 742
 743 void set_polyphony_rate(float rate) {
 744     polyphony_rate = rate;
 745 }
 746
 747 void enable_polyphony() {
 748     polyphony_rate = 5;
 749 }
 750
 751 void disable_polyphony() {
 752     polyphony_rate = 0;
 753 }
 754
 755 void increase_polyphony_rate(float change) {
 756     polyphony_rate *= change;
 757 }
 758
 759 void decrease_polyphony_rate(float change) {
 760     polyphony_rate /= change;
 761 }
 762
 763 // Timbre function
 764
 765 void set_timbre(float timbre) {
 766     note_timbre = timbre;
 767 }
 768
 769 // Tempo functions
 770
 771 void set_tempo(uint8_t tempo) {
 772     note_tempo = tempo;
 773 }
 774
 775 void decrease_tempo(uint8_t tempo_change) {
 776     note_tempo += tempo_change;
 777 }
 778
 779 void increase_tempo(uint8_t tempo_change) {
 780     if (note_tempo - tempo_change < 10) {
 781         note_tempo = 10;
 782     } else {
 783         note_tempo -= tempo_change;
 784     }
 785 }