apps/plugins/pitch_detector.c

   1 /**************************************************************************
   2  *             __________               __   ___.
   3  *   Open      \______   \ ____   ____ |  | _\_ |__   _______  ___
   4  *   Source     |       _//  _ \_/ ___\|  |/ /| __ \ /  _ \  \/  /
   5  *   Jukebox    |    |   (  <_> )  \___|    < | \_\ (  <_> > <  <
   6  *   Firmware   |____|_  /\____/ \___  >__|_ \|___  /\____/__/\_ \
   7  *                     \/            \/     \/    \/            \/
   8  * $Id$
   9  *
  10  * Copyright (C) 2008    Lechner Michael / smoking gnu
  11  *
  12  * All files in this archive are subject to the GNU General Public License.
  13  * See the file COPYING in the source tree root for full license agreement.
  14  *
  15  * This software is distributed on an "AS IS" basis, WITHOUT WARRANTY OF ANY
  16  * KIND, either express or implied.
  17  *
  18  * ----------------------------------------------------------------------------
  19  *
  20  * INTRODUCTION:
  21  * OK, this is an attempt to write an instrument tuner for rockbox.
  22  * It uses a Schmitt trigger algorithm, which I copied from
  23  * tuneit [ (c) 2004 Mario Lang <mlang@delysid.org> ], for detecting the
  24  * fundamental freqency of a sound. A FFT algorithm would be more accurate
  25  * but also much slower.
  26  *
  27  * TODO:
  28  * - Adapt the Yin FFT algorithm, which would reduce complexity from O(n^2)
  29  *   to O(nlogn), theoretically reducing latency by a factor of ~10. -David
  30  *
  31  * MAJOR CHANGES:
  32  * 08.03.2008        Started coding
  33  * 21.03.2008        Pitch detection works more or less
  34  *                   Button definitions for most targets added
  35  * 02.04.2008        Proper GUI added
  36  *                   Todo, Major Changes and Current Limitations added
  37  * 08.19.2009        Brought the code up to date with current plugin standards
  38  *                   Made it work more nicely with color, BW and grayscale
  39  *                   Changed pitch detection to use the Yin algorithm (better
  40  *                      detection, but slower -- would be ~4x faster with
  41  *                      fixed point math, I think).  Code was poached from the
  42  *                      Aubio sound processing library (aubio.org). -David
  43  * 08.31.2009        Lots of changes:
  44  *                   Added a menu to tweak settings
  45  *                   Converted everything to fixed point (greatly improving
  46  *                       latency)
  47  *                   Improved the display
  48  *                   Improved efficiency with judicious use of cpu_boost, the
  49  *                       backlight, and volume detection to limit unneeded
  50  *                       calculation
  51  *                   Fixed a problem that caused an octave-off error
  52  *                   -David
  53  * 05.14.2010        Multibuffer continuous recording with two buffers
  54  *
  55  *
  56  * CURRENT LIMITATIONS:
  57  * - No gapless recording.  Strictly speaking true gappless isn't possible,
  58  *   since the algorithm takes longer to calculate than the length of the
  59  *   sample, but latency could be improved a bit with proper use of the DMA
  60  *   recording functions.
  61  * - Due to how the Yin algorithm works, latency is higher for lower
  62  *   frequencies.
  63  */
  64
  65 #include "plugin.h"
  66 #include "lib/pluginlib_actions.h"
  67 #include "lib/picture.h"
  68 #include "lib/helper.h"
  69 #include "pluginbitmaps/pitch_notes.h"
  70
  71 PLUGIN_HEADER
  72 PLUGIN_IRAM_DECLARE
  73
  74 /* Some fixed point calculation stuff */
  75 typedef int32_t fixed_data;
  76 struct _fixed
  77 {
  78     fixed_data a;
  79 };
  80 typedef struct _fixed fixed;
  81 #define FIXED_PRECISION 18
  82 #define FP_MAX ((fixed) {0x7fffffff})
  83 #define FP_MIN ((fixed) {-0x80000000})
  84 #define int2fixed(x)    ((fixed){(x) << FIXED_PRECISION})
  85 #define int2mantissa(x) ((fixed){x})
  86 #define fixed2int(x)    ((int)((x).a >> FIXED_PRECISION))
  87 #define fixed2float(x)  (((float)(x).a) / ((float)(1 << FIXED_PRECISION)))
  88 #define float2fixed(x) \
  89     ((fixed){(fixed_data)(x * (float)(1 << FIXED_PRECISION))})
  90 /* I adapted these ones from the Rockbox fixed point library */
  91 #define fp_mul(x, y) \
  92     ((fixed){(((int64_t)((x).a)) * ((int64_t)((y).a))) >> (FIXED_PRECISION)})
  93 #define fp_div(x, y) \
  94     ((fixed){(((int64_t)((x).a)) << (FIXED_PRECISION)) / ((int64_t)((y).a))})
  95 /* Operators for fixed point */
  96 #define fp_add(x, y) ((fixed){(x).a + (y).a})
  97 #define fp_sub(x, y) ((fixed){(x).a - (y).a})
  98 #define fp_shl(x, y) ((fixed){(x).a << y})
  99 #define fp_shr(x, y) ((fixed){(x).a >> y})
 100 #define fp_neg(x)    ((fixed){-(x).a})
 101 #define fp_gt(x, y)  ((x).a > (y).a)
 102 #define fp_gte(x, y) ((x).a >= (y).a)
 103 #define fp_lt(x, y)  ((x).a < (y).a)
 104 #define fp_lte(x, y) ((x).a <= (y).a)
 105 #define fp_sqr(x)    fp_mul((x), (x))
 106 #define fp_equal(x, y) ((x).a == (y).a)
 107 #define fp_round(x)  (fixed2int(fp_add((x), float2fixed(0.5))))
 108 #define fp_data(x)   ((x).a)
 109 #define fp_frac(x)   (fp_sub((x), int2fixed(fixed2int(x))))
 110 #define FP_ZERO      ((fixed){0})
 111 #define FP_LOW       ((fixed){2})
 112
 113 /* Some defines for converting between period and frequency  */
 114
 115 /* I introduce some divisors in this because the fixed point */
 116 /* variables aren't big enough to hold higher than a certain */
 117 /* value.  This loses a bit of precision but it means we     */
 118 /* don't have to use 32.32 variables (yikes).                */
 119 /* With an 18-bit decimal precision, the max value in the    */
 120 /* integer part is 8192.  Divide 44100 by 7 and it'll fit in */
 121 /* that variable.                                            */
 122 #define fp_period2freq(x) fp_div(int2fixed(sample_rate / 7), \
 123                                  fp_div((x),int2fixed(7)))
 124 #define fp_freq2period(x) fp_period2freq(x)
 125 #define period2freq(x)    (sample_rate / (x))
 126 #define freq2period(x)    period2freq(x)
 127
 128 #define sqr(x)    ((x)*(x))
 129
 130 /* Some constants for tuning */
 131 #define A_FREQ          float2fixed(440.0f)
 132 #define D_NOTE          float2fixed(1.059463094359f)
 133 #define LOG_D_NOTE      float2fixed(1.0f/12.0f)
 134 #define D_NOTE_SQRT     float2fixed(1.029302236643f)
 135 #define LOG_2           float2fixed(1.0f)
 136
 137 /* The recording buffer size */
 138 /* This is how much is sampled at a time. */
 139 /* It also determines latency -- if BUFFER_SIZE == sample_rate then */
 140 /*   there'll be one sample per second, or a latency of one second. */
 141 /* Furthermore, the lowest detectable frequency will be about twice */
 142 /*   the number of reads per second                                 */
 143 /* If we ever switch to Yin FFT algorithm then this needs to be
 144    a power of 2 */
 145 #define BUFFER_SIZE 4096
 146 #define SAMPLE_SIZE 4096
 147 #define SAMPLE_SIZE_MIN 1024
 148 #define YIN_BUFFER_SIZE (BUFFER_SIZE / 4)
 149
 150 #define LCD_FACTOR (fp_div(int2fixed(LCD_WIDTH), int2fixed(100)))
 151 /* The threshold for the YIN algorithm */
 152 #define DEFAULT_YIN_THRESHOLD 5  /* 0.10 */
 153 const fixed yin_threshold_table[] IDATA_ATTR =
 154 {
 155     float2fixed(0.01),
 156     float2fixed(0.02),
 157     float2fixed(0.03),
 158     float2fixed(0.04),
 159     float2fixed(0.05),
 160     float2fixed(0.10),
 161     float2fixed(0.15),
 162     float2fixed(0.20),
 163     float2fixed(0.25),
 164     float2fixed(0.30),
 165     float2fixed(0.35),
 166     float2fixed(0.40),
 167     float2fixed(0.45),
 168     float2fixed(0.50),
 169 };
 170
 171 /* Structure for the reference frequency (frequency of A)
 172  * It's used for scaling the frequency before finding out
 173  * the note. The frequency is scaled in a way that the main
 174  * algorithm can assume the frequency of A to be 440 Hz.
 175  */
 176 struct freq_A_entry
 177 {
 178     const int frequency;  /* Frequency in Hz */
 179     const fixed ratio;    /* 440/frequency   */
 180     const fixed logratio; /* log2(factor)    */
 181 };
 182
 183 const struct freq_A_entry freq_A[] =
 184 {
 185     {435, float2fixed(1.011363636), float2fixed( 0.016301812)},
 186     {436, float2fixed(1.009090909), float2fixed( 0.013056153)},
 187     {437, float2fixed(1.006818182), float2fixed( 0.009803175)},
 188     {438, float2fixed(1.004545455), float2fixed( 0.006542846)},
 189     {439, float2fixed(1.002272727), float2fixed( 0.003275132)},
 190     {440, float2fixed(1.000000000), float2fixed( 0.000000000)},
 191     {441, float2fixed(0.997727273), float2fixed(-0.003282584)},
 192     {442, float2fixed(0.995454545), float2fixed(-0.006572654)},
 193     {443, float2fixed(0.993181818), float2fixed(-0.009870244)},
 194     {444, float2fixed(0.990909091), float2fixed(-0.013175389)},
 195     {445, float2fixed(0.988636364), float2fixed(-0.016488123)},
 196 };
 197
 198 /* Index of the entry for 440 Hz in the table (default frequency for A) */
 199 #define DEFAULT_FREQ_A 5
 200 #define NUM_FREQ_A (sizeof(freq_A)/sizeof(freq_A[0]))
 201
 202 /* How loud the audio has to be to start displaying pitch  */
 203 /* Must be between 0 and 100                               */
 204 #define VOLUME_THRESHOLD (50)
 205
 206 /* Change to AUDIO_SRC_LINEIN if you want to record from line-in */
 207 #ifdef HAVE_MIC_IN
 208 #define INPUT_TYPE AUDIO_SRC_MIC
 209 #else
 210 #define INPUT_TYPE AUDIO_SRC_LINEIN
 211 #endif
 212
 213 /* How many decimal places to display for the Hz value */
 214 #define DISPLAY_HZ_PRECISION 100
 215
 216 /* Where to put the various GUI elements */
 217 int note_y;
 218 int bar_grad_y;
 219 #define LCD_RES_MIN (LCD_HEIGHT < LCD_WIDTH ? LCD_HEIGHT : LCD_WIDTH)
 220 #define BAR_PADDING (LCD_RES_MIN / 32)
 221 #define BAR_Y       (LCD_HEIGHT * 3 / 4)
 222 #define BAR_HEIGHT  (LCD_RES_MIN / 4 - BAR_PADDING)
 223 #define BAR_HLINE_Y (BAR_Y - BAR_PADDING)
 224 #define BAR_HLINE_Y2 (BAR_Y + BAR_HEIGHT + BAR_PADDING - 1)
 225 #define HZ_Y        0
 226 #define GRADUATION  10       /* Subdivisions of the whole 100-cent scale */
 227
 228 /* Bitmaps for drawing the note names.  These need to have height
 229    <= (bar_grad_y - note_y), or 15/32 * LCD_HEIGHT
 230  */
 231 #define NUM_NOTE_IMAGES  9
 232 #define NOTE_INDEX_A     0
 233 #define NOTE_INDEX_B     1
 234 #define NOTE_INDEX_C     2
 235 #define NOTE_INDEX_D     3
 236 #define NOTE_INDEX_E     4
 237 #define NOTE_INDEX_F     5
 238 #define NOTE_INDEX_G     6
 239 #define NOTE_INDEX_SHARP 7
 240 #define NOTE_INDEX_FLAT  8
 241 const struct picture note_bitmaps =
 242 {
 243     pitch_notes,
 244     BMPWIDTH_pitch_notes,
 245     BMPHEIGHT_pitch_notes,
 246     BMPHEIGHT_pitch_notes/NUM_NOTE_IMAGES
 247 };
 248
 249
 250 static unsigned int sample_rate;
 251 static int audio_head = 0; /* which of the two buffers to use? */
 252 static volatile int audio_tail = 0; /* which of the two buffers to record? */
 253 /* It's stereo, so make the buffer twice as big */
 254 #ifndef SIMULATOR
 255 static int16_t audio_data[2][BUFFER_SIZE] __attribute__((aligned(CACHEALIGN_SIZE)));
 256 static fixed yin_buffer[YIN_BUFFER_SIZE] IBSS_ATTR;
 257 #ifdef PLUGIN_USE_IRAM
 258 static int16_t iram_audio_data[BUFFER_SIZE] IBSS_ATTR;
 259 #else
 260 #define iram_audio_data audio_data[audio_head]
 261 #endif
 262 #endif
 263
 264 /* Description of a note of scale */
 265 struct note_entry
 266 {
 267     const char *name;    /* Name of the note, e.g. "A#" */
 268     const fixed freq;    /* Note frequency, Hz          */
 269     const fixed logfreq; /* log2(frequency)             */
 270 };
 271
 272 /* Notes within one (reference) scale */
 273 static const struct note_entry notes[] =
 274 {
 275     {"A" , float2fixed(440.0000000f), float2fixed(8.781359714f)},
 276     {"A#", float2fixed(466.1637615f), float2fixed(8.864693047f)},
 277     {"B" , float2fixed(493.8833013f), float2fixed(8.948026380f)},
 278     {"C" , float2fixed(523.2511306f), float2fixed(9.031359714f)},
 279     {"C#", float2fixed(554.3652620f), float2fixed(9.114693047f)},
 280     {"D" , float2fixed(587.3295358f), float2fixed(9.198026380f)},
 281     {"D#", float2fixed(622.2539674f), float2fixed(9.281359714f)},
 282     {"E" , float2fixed(659.2551138f), float2fixed(9.364693047f)},
 283     {"F" , float2fixed(698.4564629f), float2fixed(9.448026380f)},
 284     {"F#", float2fixed(739.9888454f), float2fixed(9.531359714f)},
 285     {"G" , float2fixed(783.9908720f), float2fixed(9.614693047f)},
 286     {"G#", float2fixed(830.6093952f), float2fixed(9.698026380f)},
 287 };
 288
 289 /* GUI */
 290 #if LCD_DEPTH > 1
 291 static unsigned front_color;
 292 #endif
 293 static int font_w,font_h;
 294 static int bar_x_0;
 295 static int lbl_x_minus_50, lbl_x_minus_20, lbl_x_0, lbl_x_20, lbl_x_50;
 296
 297 /* Settings for the plugin */
 298 struct tuner_settings
 299 {
 300     unsigned volume_threshold;
 301     unsigned record_gain;
 302     unsigned sample_size;
 303     unsigned lowest_freq;
 304     unsigned yin_threshold;
 305     int      freq_A;        /* Index of the frequency of A */
 306     bool     use_sharps;
 307     bool     display_hz;
 308 } tuner_settings;
 309
 310 /*=================================================================*/
 311 /* Settings loading and saving(adapted from the clock plugin)      */
 312 /*=================================================================*/
 313
 314 #define SETTINGS_FILENAME PLUGIN_APPS_DIR "/.pitch_settings"
 315
 316 enum message
 317 {
 318     MESSAGE_LOADING,
 319     MESSAGE_LOADED,
 320     MESSAGE_ERRLOAD,
 321     MESSAGE_SAVING,
 322     MESSAGE_SAVED,
 323     MESSAGE_ERRSAVE
 324 };
 325
 326 enum settings_file_status
 327 {
 328     LOADED, ERRLOAD,
 329     SAVED, ERRSAVE
 330 };
 331
 332 /* The settings as they exist on the hard disk, so that
 333  * we can know at saving time if changes have been made */
 334 struct tuner_settings hdd_tuner_settings;
 335
 336 /*---------------------------------------------------------------------*/
 337
 338 bool settings_needs_saving(struct tuner_settings* settings)
 339 {
 340     return(rb->memcmp(settings, &hdd_tuner_settings, sizeof(*settings)));
 341 }
 342
 343 /*---------------------------------------------------------------------*/
 344
 345 void tuner_settings_reset(struct tuner_settings* settings)
 346 {
 347     settings->volume_threshold = VOLUME_THRESHOLD;
 348     settings->record_gain = rb->global_settings->rec_mic_gain;
 349     settings->sample_size = BUFFER_SIZE;
 350     settings->lowest_freq = period2freq(BUFFER_SIZE / 4);
 351     settings->yin_threshold = DEFAULT_YIN_THRESHOLD;
 352     settings->freq_A = DEFAULT_FREQ_A;
 353     settings->use_sharps = true;
 354     settings->display_hz = false;
 355 }
 356
 357 /*---------------------------------------------------------------------*/
 358
 359 enum settings_file_status tuner_settings_load(struct tuner_settings* settings,
 360                                               char* filename)
 361 {
 362     int fd = rb->open(filename, O_RDONLY);
 363     if(fd >= 0){ /* does file exist? */
 364         /* basic consistency check */
 365         if(rb->filesize(fd) == sizeof(*settings)){
 366             rb->read(fd, settings, sizeof(*settings));
 367             rb->close(fd);
 368             rb->memcpy(&hdd_tuner_settings, settings, sizeof(*settings));
 369             return(LOADED);
 370         }
 371     }
 372     /* Initializes the settings with default values at least */
 373     tuner_settings_reset(settings);
 374     return(ERRLOAD);
 375 }
 376
 377 /*---------------------------------------------------------------------*/
 378
 379 enum settings_file_status tuner_settings_save(struct tuner_settings* settings,
 380                                               char* filename)
 381 {
 382     int fd = rb->creat(filename, 0666);
 383     if(fd >= 0){ /* does file exist? */
 384         rb->write (fd, settings, sizeof(*settings));
 385         rb->close(fd);
 386         return(SAVED);
 387     }
 388     return(ERRSAVE);
 389 }
 390
 391 /*---------------------------------------------------------------------*/
 392
 393 void load_settings(void)
 394 {
 395     tuner_settings_load(&tuner_settings, SETTINGS_FILENAME);
 396
 397     rb->storage_sleep();
 398 }
 399
 400 /*---------------------------------------------------------------------*/
 401
 402 void save_settings(void)
 403 {
 404     if(!settings_needs_saving(&tuner_settings))
 405         return;
 406
 407     tuner_settings_save(&tuner_settings, SETTINGS_FILENAME);
 408 }
 409
 410 /*=================================================================*/
 411 /* MENU                                                            */
 412 /*=================================================================*/
 413
 414 /* Keymaps */
 415 const struct button_mapping* plugin_contexts[]={
 416     generic_actions,
 417     generic_increase_decrease,
 418     generic_directions,
 419 #if NB_SCREENS == 2
 420     remote_directions
 421 #endif
 422 };
 423 #define PLA_ARRAY_COUNT sizeof(plugin_contexts)/sizeof(plugin_contexts[0])
 424
 425 /* Option strings */
 426
 427 /* This has to match yin_threshold_table */
 428 static const struct opt_items yin_threshold_text[] =
 429 {
 430     { "0.01", -1 },
 431     { "0.02", -1 },
 432     { "0.03", -1 },
 433     { "0.04", -1 },
 434     { "0.05", -1 },
 435     { "0.10", -1 },
 436     { "0.15", -1 },
 437     { "0.20", -1 },
 438     { "0.25", -1 },
 439     { "0.30", -1 },
 440     { "0.35", -1 },
 441     { "0.40", -1 },
 442     { "0.45", -1 },
 443     { "0.50", -1 },
 444 };
 445
 446 static const struct opt_items accidental_text[] =
 447 {
 448     { "Flat", -1 },
 449     { "Sharp", -1 },
 450 };
 451
 452 void set_min_freq(int new_freq)
 453 {
 454     tuner_settings.sample_size = freq2period(new_freq) * 4;
 455
 456     /* clamp the sample size between min and max */
 457     if(tuner_settings.sample_size <= SAMPLE_SIZE_MIN)
 458         tuner_settings.sample_size = SAMPLE_SIZE_MIN;
 459     else if(tuner_settings.sample_size >= BUFFER_SIZE)
 460         tuner_settings.sample_size = BUFFER_SIZE;
 461
 462     /* sample size must be divisible by 4 - round up */
 463     tuner_settings.sample_size = (tuner_settings.sample_size + 3) & ~3;
 464 }
 465
 466 bool main_menu(void)
 467 {
 468     int selection = 0;
 469     bool done = false;
 470     bool exit_tuner = false;
 471     int choice;
 472     int freq_val;
 473     bool reset;
 474
 475     backlight_use_settings();
 476 #ifdef HAVE_SCHEDULER_BOOSTCTRL
 477     rb->cancel_cpu_boost();
 478 #endif
 479
 480     MENUITEM_STRINGLIST(menu,"Tuner Settings",NULL,
 481                         "Return to Tuner",
 482                         "Volume Threshold",
 483                         "Listening Volume",
 484                         "Lowest Frequency",
 485                         "Algorithm Pickiness",
 486                         "Accidentals",
 487                         "Display Frequency (Hz)",
 488                         "Frequency of A (Hz)",
 489                         "Reset Settings",
 490                         "Quit");
 491
 492     while(!done)
 493     {
 494         choice = rb->do_menu(&menu, &selection, NULL, false);
 495         switch(choice)
 496         {
 497             case 1:
 498                 rb->set_int("Volume Threshold", "%", UNIT_INT,
 499                                &tuner_settings.volume_threshold,
 500                                NULL, 5, 5, 95, NULL);
 501                 break;
 502             case 2:
 503                 rb->set_int("Listening Volume", "%", UNIT_INT,
 504                                &tuner_settings.record_gain,
 505                                NULL, 1, rb->sound_min(SOUND_MIC_GAIN),
 506                                rb->sound_max(SOUND_MIC_GAIN), NULL);
 507                 break;
 508             case 3:
 509                 rb->set_int("Lowest Frequency", "Hz", UNIT_INT,
 510                                &tuner_settings.lowest_freq, set_min_freq, 1,
 511                                /* Range depends on the size of the buffer */
 512                                sample_rate / (BUFFER_SIZE / 4),
 513                                sample_rate / (SAMPLE_SIZE_MIN / 4), NULL);
 514                 break;
 515             case 4:
 516                 rb->set_option(
 517                     "Algorithm Pickiness (Lower -> more discriminating)",
 518                     &tuner_settings.yin_threshold,
 519                     INT, yin_threshold_text,
 520                     sizeof(yin_threshold_text) / sizeof(yin_threshold_text[0]),
 521                     NULL);
 522                 break;
 523             case 5:
 524                 rb->set_option("Display Accidentals As",
 525                                &tuner_settings.use_sharps,
 526                                BOOL, accidental_text, 2, NULL);
 527                 break;
 528             case 6:
 529                 rb->set_bool("Display Frequency (Hz)",
 530                              &tuner_settings.display_hz);
 531                 break;
 532             case 7:
 533                 freq_val = freq_A[tuner_settings.freq_A].frequency;
 534                 rb->set_int("Frequency of A (Hz)",
 535                     "Hz", UNIT_INT, &freq_val, NULL,
 536                     1, freq_A[0].frequency, freq_A[NUM_FREQ_A-1].frequency,
 537                     NULL);
 538                 tuner_settings.freq_A = freq_val - freq_A[0].frequency;
 539                 break;
 540             case 8:
 541                 reset = false;
 542                 rb->set_bool("Reset Tuner Settings?", &reset);
 543                 if (reset)
 544                     tuner_settings_reset(&tuner_settings);
 545                 break;
 546             case 9:
 547                 exit_tuner = true;
 548                 done = true;
 549                 break;
 550             case 0:
 551             default:
 552                 /* Return to the tuner */
 553                 done = true;
 554                 break;
 555         }
 556     }
 557
 558     backlight_force_on();
 559     return exit_tuner;
 560 }
 561
 562 /*=================================================================*/
 563 /* Binary Log                                                      */
 564 /*=================================================================*/
 565
 566 /* Fixed-point log base 2*/
 567 /* Adapted from python code at
 568    http://en.wikipedia.org/wiki/Binary_logarithm#Algorithm
 569 */
 570 fixed log(fixed inp)
 571 {
 572     fixed x = inp;
 573     fixed fp = int2fixed(1);
 574     fixed res = int2fixed(0);
 575
 576     if(fp_lte(x, FP_ZERO))
 577     {
 578         return FP_MIN;
 579     }
 580
 581     /* Integer part*/
 582     /* while x<1 */
 583     while(fp_lt(x, int2fixed(1)))
 584     {
 585         res = fp_sub(res, int2fixed(1));
 586         x = fp_shl(x, 1);
 587     }
 588     /* while x>=2 */
 589     while(fp_gte(x, int2fixed(2)))
 590     {
 591         res = fp_add(res, int2fixed(1));
 592         x = fp_shr(x, 1);
 593     }
 594
 595     /* Fractional part */
 596     /* while fp > 0 */
 597     while(fp_gt(fp, FP_ZERO))
 598     {
 599         fp = fp_shr(fp, 1);
 600         x = fp_mul(x, x);
 601         /* if x >= 2 */
 602         if(fp_gte(x, int2fixed(2)))
 603         {
 604             x = fp_shr(x, 1);
 605             res = fp_add(res, fp);
 606         }
 607     }
 608
 609     return res;
 610 }
 611
 612 /*=================================================================*/
 613 /* GUI Stuff                                                       */
 614 /*=================================================================*/
 615
 616 /* The function name is pretty self-explaining ;) */
 617 void print_int_xy(int x, int y, int v)
 618 {
 619     char temp[20];
 620 #if LCD_DEPTH > 1
 621     rb->lcd_set_foreground(front_color);
 622 #endif
 623     rb->snprintf(temp,20,"%d",v);
 624     rb->lcd_putsxy(x,y,temp);
 625 }
 626
 627 /* Print out the frequency etc */
 628 void print_str(char* s)
 629 {
 630 #if LCD_DEPTH > 1
 631     rb->lcd_set_foreground(front_color);
 632 #endif
 633     rb->lcd_putsxy(0, HZ_Y, s);
 634 }
 635
 636 /* What can I say? Read the function name... */
 637 void print_char_xy(int x, int y, char c)
 638 {
 639     char temp[2];
 640
 641     temp[0]=c;
 642     temp[1]=0;
 643 #if LCD_DEPTH > 1
 644     rb->lcd_set_foreground(front_color);
 645 #endif
 646
 647     rb->lcd_putsxy(x, y, temp);
 648 }
 649
 650 /* Draw the note bitmap */
 651 void draw_note(const char *note)
 652 {
 653     int i;
 654     int note_x = (LCD_WIDTH - BMPWIDTH_pitch_notes) / 2;
 655     int accidental_index = NOTE_INDEX_SHARP;
 656
 657     i = note[0]-'A';
 658
 659     if(note[1] == '#')
 660     {
 661         if(!(tuner_settings.use_sharps))
 662         {
 663             i = (i + 1) % 7;
 664             accidental_index = NOTE_INDEX_FLAT;
 665         }
 666
 667         vertical_picture_draw_sprite(rb->screens[0],
 668                                      &note_bitmaps,
 669                                      accidental_index,
 670                                      LCD_WIDTH / 2,
 671                                      note_y);
 672         note_x = LCD_WIDTH / 2 - BMPWIDTH_pitch_notes;
 673     }
 674
 675     vertical_picture_draw_sprite(rb->screens[0], &note_bitmaps, i,
 676                                  note_x,
 677                                  note_y);
 678 }
 679 /* Draw the red bar and the white lines */
 680 void draw_bar(fixed wrong_by_cents)
 681 {
 682     unsigned n;
 683     int x;
 684
 685 #ifdef HAVE_LCD_COLOR
 686     rb->lcd_set_foreground(LCD_RGBPACK(255,255,255)); /* Color screens */
 687 #elif LCD_DEPTH > 1
 688     rb->lcd_set_foreground(LCD_BLACK);      /* Greyscale screens */
 689 #endif
 690
 691     rb->lcd_hline(0,LCD_WIDTH-1, BAR_HLINE_Y);
 692     rb->lcd_hline(0,LCD_WIDTH-1, BAR_HLINE_Y2);
 693
 694     /* Draw graduation lines on the off-by readout */
 695     for(n = 0; n <= GRADUATION; n++)
 696     {
 697         x = (LCD_WIDTH * n + GRADUATION / 2) / GRADUATION;
 698         if (x >= LCD_WIDTH)
 699             x = LCD_WIDTH - 1;
 700         rb->lcd_vline(x, BAR_HLINE_Y, BAR_HLINE_Y2);
 701     }
 702
 703     print_int_xy(lbl_x_minus_50    ,bar_grad_y, -50);
 704     print_int_xy(lbl_x_minus_20    ,bar_grad_y, -20);
 705     print_int_xy(lbl_x_0           ,bar_grad_y,   0);
 706     print_int_xy(lbl_x_20          ,bar_grad_y,  20);
 707     print_int_xy(lbl_x_50          ,bar_grad_y,  50);
 708
 709 #ifdef HAVE_LCD_COLOR
 710     rb->lcd_set_foreground(LCD_RGBPACK(255,0,0));   /* Color screens */
 711 #elif LCD_DEPTH > 1
 712     rb->lcd_set_foreground(LCD_DARKGRAY);           /* Greyscale screens */
 713 #endif
 714
 715     if (fp_gt(wrong_by_cents, FP_ZERO))
 716     {
 717         rb->lcd_fillrect(bar_x_0, BAR_Y,
 718             fixed2int(fp_mul(wrong_by_cents, LCD_FACTOR)), BAR_HEIGHT);
 719     }
 720     else
 721     {
 722         rb->lcd_fillrect(bar_x_0 + fixed2int(fp_mul(wrong_by_cents,LCD_FACTOR)),
 723                          BAR_Y,
 724                          fixed2int(fp_mul(wrong_by_cents, LCD_FACTOR)) * -1,
 725                          BAR_HEIGHT);
 726     }
 727 }
 728
 729 /* Calculate how wrong the note is and draw the GUI */
 730 void display_frequency (fixed freq)
 731 {
 732     fixed ldf, mldf;
 733     fixed lfreq, nfreq;
 734     fixed orig_freq;
 735     int i, note = 0;
 736     char str_buf[30];
 737
 738     if (fp_lt(freq, FP_LOW))
 739         freq = FP_LOW;
 740
 741     /* We calculate the frequency and its log as if */
 742     /* the reference frequency of A were 440 Hz.    */
 743     orig_freq = freq;
 744     lfreq = fp_add(log(freq), freq_A[tuner_settings.freq_A].logratio);
 745     freq = fp_mul(freq, freq_A[tuner_settings.freq_A].ratio);
 746
 747     /* This calculates a log freq offset for note A */
 748     /* Get the frequency to within the range of our reference table, */
 749     /* i.e. into the right octave.                                   */
 750     while (fp_lt(lfreq, fp_sub(notes[0].logfreq, fp_shr(LOG_D_NOTE, 1))))
 751         lfreq = fp_add(lfreq, LOG_2);
 752     while (fp_gte(lfreq, fp_sub(fp_add(notes[0].logfreq, LOG_2),
 753            fp_shr(LOG_D_NOTE, 1))))
 754         lfreq = fp_sub(lfreq, LOG_2);
 755     mldf = LOG_D_NOTE;
 756     for (i=0; i<12; i++)
 757     {
 758         ldf = fp_gt(fp_sub(lfreq,notes[i].logfreq), FP_ZERO) ?
 759                  fp_sub(lfreq,notes[i].logfreq) : fp_neg(fp_sub(lfreq,notes[i].logfreq));
 760         if (fp_lt(ldf, mldf))
 761         {
 762             mldf = ldf;
 763             note = i;
 764         }
 765     }
 766     nfreq = notes[note].freq;
 767     while (fp_gt(fp_div(nfreq, freq), D_NOTE_SQRT))
 768         nfreq = fp_shr(nfreq, 1);
 769
 770     while (fp_gt(fp_div(freq, nfreq), D_NOTE_SQRT))
 771         nfreq = fp_shl(nfreq, 1);
 772
 773     ldf = fp_mul(int2fixed(1200), log(fp_div(freq,nfreq)));
 774
 775     rb->lcd_clear_display();
 776     draw_bar(ldf);                /* The red bar */
 777     if(fp_round(freq) != 0)
 778     {
 779         draw_note(notes[note].name);
 780         if(tuner_settings.display_hz)
 781         {
 782             rb->snprintf(str_buf,30, "%s : %d cents (%d.%02dHz)",
 783                          notes[note].name, fp_round(ldf) ,fixed2int(orig_freq),
 784                          fp_round(fp_mul(fp_frac(orig_freq),
 785                                          int2fixed(DISPLAY_HZ_PRECISION))));
 786             print_str(str_buf);
 787         }
 788     }
 789     rb->lcd_update();
 790 }
 791
 792 /*-----------------------------------------------------------------------
 793  * Functions for the Yin algorithm
 794  *
 795  * These were all adapted from the versions in Aubio v0.3.2
 796  * Here's what the Aubio documentation has to say:
 797  *
 798  * This algorithm was developped by A. de Cheveigne and H. Kawahara and
 799  * published in:
 800  *
 801  * de Cheveign?, A., Kawahara, H. (2002) "YIN, a fundamental frequency
 802  * estimator for speech and music", J. Acoust. Soc. Am. 111, 1917-1930.
 803  *
 804  * see http://recherche.ircam.fr/equipes/pcm/pub/people/cheveign.html
 805 -------------------------------------------------------------------------*/
 806
 807 /* Find the index of the minimum element of an array of floats */
 808 unsigned vec_min_elem(fixed *s, unsigned buflen)
 809 {
 810     unsigned j, pos=0.0f;
 811     fixed tmp = s[0];
 812     for (j=0; j < buflen; j++)
 813     {
 814         if(fp_gt(tmp, s[j]))
 815         {
 816             pos = j;
 817             tmp = s[j];
 818         }
 819     }
 820     return pos;
 821 }
 822
 823
 824 static inline fixed aubio_quadfrac(fixed s0, fixed s1, fixed s2, fixed pf)
 825 {
 826     /* Original floating point version: */
 827     /* tmp = s0 + (pf/2.0f) * (pf * ( s0 - 2.0f*s1 + s2 ) -
 828              3.0f*s0 + 4.0f*s1 - s2);*/
 829     /* Converted to explicit operator precedence: */
 830     /* tmp = s0 + ((pf/2.0f) * ((((pf * ((s0 - (2*s1)) + s2)) -
 831             (3*s0)) + (4*s1)) - s2)); */
 832
 833     /* I made it look like this so I could easily track the precedence and */
 834     /* make sure it matched the original expression                        */
 835     /* Oy, this is when I really wish I could do C++ operator overloading  */
 836     fixed tmp = fp_add
 837                 (
 838                   s0,
 839                   fp_mul
 840                   (
 841                     fp_shr(pf, 1),
 842                     fp_sub
 843                     (
 844                       fp_add
 845                       (
 846                         fp_sub
 847                         (
 848                           fp_mul
 849                           (
 850                             pf,
 851                             fp_add
 852                             (
 853                               fp_sub
 854                               (
 855                                 s0,
 856                                 fp_shl(s1, 1)
 857                               ),
 858                               s2
 859                             )
 860                           ),
 861                           fp_mul
 862                           (
 863                             float2fixed(3.0f),
 864                             s0
 865                           )
 866                         ),
 867                         fp_shl(s1, 2)
 868                       ),
 869                       s2
 870                     )
 871                   )
 872                 );
 873     return tmp;
 874 }
 875
 876 #define QUADINT_STEP float2fixed(1.0f/200.0f)
 877
 878 fixed ICODE_ATTR vec_quadint_min(fixed *x, unsigned bufsize, unsigned pos, unsigned span)
 879 {
 880     fixed res, frac, s0, s1, s2;
 881     fixed exactpos = int2fixed(pos);
 882     /* init resold to something big (in case x[pos+-span]<0)) */
 883     fixed resold = FP_MAX;
 884
 885     if ((pos > span) && (pos < bufsize-span))
 886     {
 887         s0 = x[pos-span];
 888         s1 = x[pos]     ;
 889         s2 = x[pos+span];
 890         /* increase frac */
 891         for (frac = float2fixed(0.0f);
 892              fp_lt(frac, float2fixed(2.0f));
 893              frac = fp_add(frac, QUADINT_STEP))
 894         {
 895             res = aubio_quadfrac(s0, s1, s2, frac);
 896             if (fp_lt(res, resold))
 897             {
 898                 resold = res;
 899             }
 900             else
 901             {
 902                 /* exactpos += (frac-QUADINT_STEP)*span - span/2.0f; */
 903                 exactpos = fp_add(exactpos,
 904                                   fp_sub(
 905                                     fp_mul(
 906                                       fp_sub(frac, QUADINT_STEP),
 907                                       int2fixed(span)
 908                                     ),
 909                                     int2fixed(span)
 910                                   )
 911                                  );
 912                 break;
 913             }
 914         }
 915     }
 916     return exactpos;
 917 }
 918
 919
 920 /* Calculate the period of the note in the
 921      buffer using the YIN algorithm */
 922 /* The yin pointer is just a buffer that the algorithm uses as a work
 923      space.  It needs to be half the length of the input buffer. */
 924
 925 fixed ICODE_ATTR pitchyin(int16_t *input, fixed *yin)
 926 {
 927     fixed retval;
 928     unsigned j,tau = 0;
 929     int period;
 930     unsigned yin_size = tuner_settings.sample_size / 4;
 931
 932     fixed tmp = FP_ZERO, tmp2 = FP_ZERO;
 933     yin[0] = int2fixed(1);
 934     for (tau = 1; tau < yin_size; tau++)
 935     {
 936         yin[tau] = FP_ZERO;
 937         for (j = 0; j < yin_size; j++)
 938         {
 939             tmp = fp_sub(int2mantissa(input[2 * j]),
 940                          int2mantissa(input[2 * (j + tau)]));
 941             yin[tau] = fp_add(yin[tau], fp_mul(tmp, tmp));
 942         }
 943         tmp2 = fp_add(tmp2, yin[tau]);
 944         if(!fp_equal(tmp2, FP_ZERO))
 945         {
 946             yin[tau] = fp_mul(yin[tau], fp_div(int2fixed(tau), tmp2));
 947         }
 948         period = tau - 3;
 949         if(tau > 4 && fp_lt(yin[period],
 950                             yin_threshold_table[tuner_settings.yin_threshold])
 951                    && fp_lt(yin[period], yin[period+1]))
 952         {
 953             retval = vec_quadint_min(yin, yin_size, period, 1);
 954             return retval;
 955         }
 956     }
 957     retval =  vec_quadint_min(yin, yin_size,
 958                              vec_min_elem(yin, yin_size), 1);
 959     return retval;
 960     /*return FP_ZERO;*/
 961 }
 962
 963 /*-----------------------------------------------------------------*/
 964
 965 uint32_t ICODE_ATTR buffer_magnitude(int16_t *input)
 966 {
 967     unsigned n;
 968     uint64_t tally = 0;
 969     const unsigned size = tuner_settings.sample_size;
 970
 971     /* Operate on only one channel of the stereo signal */
 972     for(n = 0; n < size; n+=2)
 973     {
 974         int s = input[n];
 975         tally += s * s;
 976     }
 977
 978     tally /= size / 2;
 979
 980     /* now tally holds the average of the squares of all the samples */
 981     /* It must be between 0 and 0x7fff^2, so it fits in 32 bits      */
 982     return (uint32_t)tally;
 983 }
 984
 985 /* Stop the recording when the buffer is full */
 986 #ifndef SIMULATOR
 987 int recording_callback(int status)
 988 {
 989     int tail = audio_tail ^ 1;
 990
 991     /* Do not overrun the reader. Reuse current buffer if full. */
 992     if (tail != audio_head)
 993         audio_tail = tail;
 994
 995     /* Always record full buffer, even if not required */
 996     rb->pcm_record_more(audio_data[tail],
 997                         BUFFER_SIZE * sizeof (int16_t));
 998
 999     return 0;
1000     (void)status;
1001 }
1002 #endif
1003
1004 /* Start recording */
1005 static void record_data(void)
1006 {
1007 #ifndef SIMULATOR
1008     /* Always record full buffer, even if not required */
1009     rb->pcm_record_data(recording_callback, audio_data[audio_tail],
1010                         BUFFER_SIZE * sizeof (int16_t));
1011 #endif
1012 }
1013
1014 /* The main program loop */
1015 void record_and_get_pitch(void)
1016 {
1017     int quit=0, button;
1018     bool redraw = true;
1019     /* For tracking the latency */
1020     /*
1021     long timer;
1022     char debug_string[20];
1023     */
1024 #ifndef SIMULATOR
1025     fixed period;
1026     bool waiting = false;
1027 #else
1028     audio_tail = 1;
1029 #endif
1030
1031     backlight_force_on();
1032
1033     record_data();
1034
1035     while(!quit)
1036     {
1037         while (audio_head == audio_tail && !quit)      /* wait for the buffer to be filled */
1038         {
1039             button=pluginlib_getaction(HZ/100, plugin_contexts, PLA_ARRAY_COUNT);
1040
1041             switch(button)
1042             {
1043                 case PLA_QUIT:
1044                     quit=true;
1045                     break;
1046
1047                 case PLA_MENU:
1048                     rb->pcm_stop_recording();
1049                     quit = main_menu() != 0;
1050                     if(!quit)
1051                     {
1052                         redraw = true;
1053                         record_data();
1054                     }
1055                     break;
1056
1057                 break;
1058             }
1059         }
1060
1061         if(!quit)
1062         {
1063 #ifndef SIMULATOR
1064             /* Only do the heavy lifting if the volume is high enough */
1065             if(buffer_magnitude(audio_data[audio_head]) >
1066                 sqr(tuner_settings.volume_threshold *
1067                     rb->sound_max(SOUND_MIC_GAIN)))
1068             {
1069                 waiting = false;
1070                 redraw = false;
1071
1072             #ifdef HAVE_SCHEDULER_BOOSTCTRL
1073                 rb->trigger_cpu_boost();
1074             #endif
1075 #ifdef PLUGIN_USE_IRAM
1076                 rb->memcpy(iram_audio_data, audio_data[audio_head],
1077                            tuner_settings.sample_size * sizeof (int16_t));
1078 #endif
1079                 /* This returns the period of the detected pitch in samples */
1080                 period = pitchyin(iram_audio_data, yin_buffer);
1081                 /* Hz = sample rate / period */
1082                 if(fp_gt(period, FP_ZERO))
1083                 {
1084                     display_frequency(fp_period2freq(period));
1085                 }
1086                 else
1087                 {
1088                     display_frequency(FP_ZERO);
1089                 }
1090             }
1091             else if(redraw || !waiting)
1092             {
1093                 waiting = true;
1094                 redraw = false;
1095                 display_frequency(FP_ZERO);
1096             #ifdef HAVE_ADJUSTABLE_CPU_FREQ
1097                 rb->cancel_cpu_boost();
1098             #endif
1099             }
1100
1101             /* Move to next buffer if not empty (but empty *shouldn't* happen
1102              * here). */
1103             if (audio_head != audio_tail)
1104                 audio_head ^= 1;
1105 #else /* SIMULATOR */
1106             /* Display a preselected frequency */
1107             display_frequency(int2fixed(445));
1108 #endif
1109         }
1110     }
1111     rb->pcm_close_recording();
1112     rb->pcm_set_frequency(HW_SAMPR_DEFAULT);
1113 #ifdef HAVE_SCHEDULER_BOOSTCTRL
1114     rb->cancel_cpu_boost();
1115 #endif
1116
1117     backlight_use_settings();
1118 }
1119
1120 /* Init recording, tuning, and GUI */
1121 void init_everything(void)
1122 {
1123     /* Disable all talking before initializing IRAM */
1124     rb->talk_disable(true);
1125
1126     PLUGIN_IRAM_INIT(rb);
1127
1128     load_settings();
1129
1130     /* Stop all playback (if no IRAM, otherwise IRAM_INIT would have) */
1131     rb->plugin_get_audio_buffer(NULL);
1132
1133     /* --------- Init the audio recording ----------------- */
1134     rb->audio_set_output_source(AUDIO_SRC_PLAYBACK);
1135     rb->audio_set_input_source(INPUT_TYPE, SRCF_RECORDING);
1136
1137     /* set to maximum gain */
1138     rb->audio_set_recording_gain(tuner_settings.record_gain,
1139                                  tuner_settings.record_gain,
1140                                  AUDIO_GAIN_MIC);
1141
1142     /* Highest C on piano is approx 4.186 kHz, so we need just over
1143      * 8.372 kHz to pass it. */
1144     sample_rate = rb->round_value_to_list32(9000, rb->rec_freq_sampr,
1145                                             REC_NUM_FREQ, false);
1146     sample_rate = rb->rec_freq_sampr[sample_rate];
1147     rb->pcm_set_frequency(sample_rate);
1148     rb->pcm_init_recording();
1149
1150     /* GUI */
1151 #if LCD_DEPTH > 1
1152     front_color = rb->lcd_get_foreground();
1153 #endif
1154     rb->lcd_getstringsize("X", &font_w, &font_h);
1155
1156     bar_x_0  = LCD_WIDTH / 2;
1157     lbl_x_minus_50 = 0;
1158     lbl_x_minus_20 = (LCD_WIDTH / 2) -
1159                      fixed2int(fp_mul(LCD_FACTOR, int2fixed(20))) - font_w;
1160     lbl_x_0  = (LCD_WIDTH - font_w) / 2;
1161     lbl_x_20 = (LCD_WIDTH / 2) +
1162                fixed2int(fp_mul(LCD_FACTOR, int2fixed(20))) - font_w;
1163     lbl_x_50 = LCD_WIDTH - 2 * font_w;
1164
1165     bar_grad_y = BAR_Y - BAR_PADDING - font_h;
1166     /* Put the note right between the top and bottom text elements */
1167     note_y = ((font_h + bar_grad_y - note_bitmaps.slide_height) / 2);
1168
1169     rb->talk_disable(false);
1170 }
1171
1172
1173 enum plugin_status plugin_start(const void* parameter) NO_PROF_ATTR
1174 {
1175     (void)parameter;
1176
1177     init_everything();
1178     record_and_get_pitch();
1179     save_settings();
1180
1181     return 0;
1182 }