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