LoopbackAnalyzer.h revision 4a764a3b450095cef05b6025a72c3876c95b6a14
1/* 2 * Copyright (C) 2017 The Android Open Source Project 3 * 4 * Licensed under the Apache License, Version 2.0 (the "License"); 5 * you may not use this file except in compliance with the License. 6 * You may obtain a copy of the License at 7 * 8 * http://www.apache.org/licenses/LICENSE-2.0 9 * 10 * Unless required by applicable law or agreed to in writing, software 11 * distributed under the License is distributed on an "AS IS" BASIS, 12 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 * See the License for the specific language governing permissions and 14 * limitations under the License. 15 */ 16 17/** 18 * Tools for measuring latency and for detecting glitches. 19 * These classes are pure math and can be used with any audio system. 20 */ 21 22#ifndef AAUDIO_EXAMPLES_LOOPBACK_ANALYSER_H 23#define AAUDIO_EXAMPLES_LOOPBACK_ANALYSER_H 24 25#include <algorithm> 26#include <assert.h> 27#include <cctype> 28#include <math.h> 29#include <stdio.h> 30#include <stdlib.h> 31#include <unistd.h> 32 33#include <audio_utils/sndfile.h> 34 35// Tag for machine readable results as property = value pairs 36#define LOOPBACK_RESULT_TAG "RESULT: " 37#define LOOPBACK_SAMPLE_RATE 48000 38 39#define MILLIS_PER_SECOND 1000 40 41#define MAX_ZEROTH_PARTIAL_BINS 40 42constexpr double MAX_ECHO_GAIN = 10.0; // based on experiments, otherwise autocorrelation too noisy 43 44static const float s_Impulse[] = { 45 0.0f, 0.0f, 0.0f, 0.0f, 0.2f, // silence on each side of the impulse 46 0.5f, 0.9999f, 0.0f, -0.9999, -0.5f, // bipolar 47 -0.2f, 0.0f, 0.0f, 0.0f, 0.0f 48}; 49 50class PseudoRandom { 51public: 52 PseudoRandom() {} 53 PseudoRandom(int64_t seed) 54 : mSeed(seed) 55 {} 56 57 /** 58 * Returns the next random double from -1.0 to 1.0 59 * 60 * @return value from -1.0 to 1.0 61 */ 62 double nextRandomDouble() { 63 return nextRandomInteger() * (0.5 / (((int32_t)1) << 30)); 64 } 65 66 /** Calculate random 32 bit number using linear-congruential method. */ 67 int32_t nextRandomInteger() { 68 // Use values for 64-bit sequence from MMIX by Donald Knuth. 69 mSeed = (mSeed * (int64_t)6364136223846793005) + (int64_t)1442695040888963407; 70 return (int32_t) (mSeed >> 32); // The higher bits have a longer sequence. 71 } 72 73private: 74 int64_t mSeed = 99887766; 75}; 76 77static double calculateCorrelation(const float *a, 78 const float *b, 79 int windowSize) 80{ 81 double correlation = 0.0; 82 double sumProducts = 0.0; 83 double sumSquares = 0.0; 84 85 // Correlate a against b. 86 for (int i = 0; i < windowSize; i++) { 87 float s1 = a[i]; 88 float s2 = b[i]; 89 // Use a normalized cross-correlation. 90 sumProducts += s1 * s2; 91 sumSquares += ((s1 * s1) + (s2 * s2)); 92 } 93 94 if (sumSquares >= 0.00000001) { 95 correlation = (float) (2.0 * sumProducts / sumSquares); 96 } 97 return correlation; 98} 99 100static int calculateCorrelations(const float *haystack, int haystackSize, 101 const float *needle, int needleSize, 102 float *results, int resultSize) 103{ 104 int maxCorrelations = haystackSize - needleSize; 105 int numCorrelations = std::min(maxCorrelations, resultSize); 106 107 for (int ic = 0; ic < numCorrelations; ic++) { 108 double correlation = calculateCorrelation(&haystack[ic], needle, needleSize); 109 results[ic] = correlation; 110 } 111 112 return numCorrelations; 113} 114 115/*==========================================================================================*/ 116/** 117 * Scan until we get a correlation of a single scan that goes over the tolerance level, 118 * peaks then drops back down. 119 */ 120static double findFirstMatch(const float *haystack, int haystackSize, 121 const float *needle, int needleSize, double threshold ) 122{ 123 int ic; 124 // How many correlations can we calculate? 125 int numCorrelations = haystackSize - needleSize; 126 double maxCorrelation = 0.0; 127 int peakIndex = -1; 128 double location = -1.0; 129 const double backThresholdScaler = 0.5; 130 131 for (ic = 0; ic < numCorrelations; ic++) { 132 double correlation = calculateCorrelation(&haystack[ic], needle, needleSize); 133 134 if( (correlation > maxCorrelation) ) { 135 maxCorrelation = correlation; 136 peakIndex = ic; 137 } 138 139 //printf("PaQa_FindFirstMatch: ic = %4d, correlation = %8f, maxSum = %8f\n", 140 // ic, correlation, maxSum ); 141 // Are we past what we were looking for? 142 if((maxCorrelation > threshold) && (correlation < backThresholdScaler * maxCorrelation)) { 143 location = peakIndex; 144 break; 145 } 146 } 147 148 return location; 149} 150 151typedef struct LatencyReport_s { 152 double latencyInFrames; 153 double confidence; 154} LatencyReport; 155 156// Apply a technique similar to Harmonic Product Spectrum Analysis to find echo fundamental. 157// Using first echo instead of the original impulse for a better match. 158static int measureLatencyFromEchos(const float *haystack, int haystackSize, 159 const float *needle, int needleSize, 160 LatencyReport *report) { 161 const double threshold = 0.1; 162 printf("measureLatencyFromEchos: haystackSize = %d, needleSize = %d\n", 163 haystackSize, needleSize); 164 165 // Find first peak 166 int first = (int) (findFirstMatch(haystack, 167 haystackSize, 168 needle, 169 needleSize, 170 threshold) + 0.5); 171 172 // Use first echo as the needle for the other echos because 173 // it will be more similar. 174 needle = &haystack[first]; 175 int again = (int) (findFirstMatch(haystack, 176 haystackSize, 177 needle, 178 needleSize, 179 threshold) + 0.5); 180 181 printf("measureLatencyFromEchos: first = %d, again at %d\n", first, again); 182 first = again; 183 184 // Allocate results array 185 int remaining = haystackSize - first; 186 const int maxReasonableLatencyFrames = 48000 * 2; // arbitrary but generous value 187 int numCorrelations = std::min(remaining, maxReasonableLatencyFrames); 188 float *correlations = new float[numCorrelations]; 189 float *harmonicSums = new float[numCorrelations](); // set to zero 190 191 // Generate correlation for every position. 192 numCorrelations = calculateCorrelations(&haystack[first], remaining, 193 needle, needleSize, 194 correlations, numCorrelations); 195 196 // Add higher harmonics mapped onto lower harmonics. 197 // This reinforces the "fundamental" echo. 198 const int numEchoes = 10; 199 for (int partial = 1; partial < numEchoes; partial++) { 200 for (int i = 0; i < numCorrelations; i++) { 201 harmonicSums[i / partial] += correlations[i] / partial; 202 } 203 } 204 205 // Find highest peak in correlation array. 206 float maxCorrelation = 0.0; 207 float sumOfPeaks = 0.0; 208 int peakIndex = 0; 209 const int skip = MAX_ZEROTH_PARTIAL_BINS; // skip low bins 210 for (int i = skip; i < numCorrelations; i++) { 211 if (harmonicSums[i] > maxCorrelation) { 212 maxCorrelation = harmonicSums[i]; 213 sumOfPeaks += maxCorrelation; 214 peakIndex = i; 215 printf("maxCorrelation = %f at %d\n", maxCorrelation, peakIndex); 216 } 217 } 218 219 report->latencyInFrames = peakIndex; 220 if (sumOfPeaks < 0.0001) { 221 report->confidence = 0.0; 222 } else { 223 report->confidence = maxCorrelation / sumOfPeaks; 224 } 225 226 delete[] correlations; 227 delete[] harmonicSums; 228 return 0; 229} 230 231class AudioRecording 232{ 233public: 234 AudioRecording() { 235 } 236 ~AudioRecording() { 237 delete[] mData; 238 } 239 240 void allocate(int maxFrames) { 241 delete[] mData; 242 mData = new float[maxFrames]; 243 mMaxFrames = maxFrames; 244 } 245 246 // Write SHORT data from the first channel. 247 int write(int16_t *inputData, int inputChannelCount, int numFrames) { 248 // stop at end of buffer 249 if ((mFrameCounter + numFrames) > mMaxFrames) { 250 numFrames = mMaxFrames - mFrameCounter; 251 } 252 for (int i = 0; i < numFrames; i++) { 253 mData[mFrameCounter++] = inputData[i * inputChannelCount] * (1.0f / 32768); 254 } 255 return numFrames; 256 } 257 258 // Write FLOAT data from the first channel. 259 int write(float *inputData, int inputChannelCount, int numFrames) { 260 // stop at end of buffer 261 if ((mFrameCounter + numFrames) > mMaxFrames) { 262 numFrames = mMaxFrames - mFrameCounter; 263 } 264 for (int i = 0; i < numFrames; i++) { 265 mData[mFrameCounter++] = inputData[i * inputChannelCount]; 266 } 267 return numFrames; 268 } 269 270 int size() { 271 return mFrameCounter; 272 } 273 274 float *getData() { 275 return mData; 276 } 277 278 void setSampleRate(int32_t sampleRate) { 279 mSampleRate = sampleRate; 280 } 281 282 int32_t getSampleRate() { 283 return mSampleRate; 284 } 285 286 int save(const char *fileName, bool writeShorts = true) { 287 SNDFILE *sndFile = nullptr; 288 int written = 0; 289 SF_INFO info = { 290 .frames = mFrameCounter, 291 .samplerate = mSampleRate, 292 .channels = 1, 293 .format = SF_FORMAT_WAV | (writeShorts ? SF_FORMAT_PCM_16 : SF_FORMAT_FLOAT) 294 }; 295 296 sndFile = sf_open(fileName, SFM_WRITE, &info); 297 if (sndFile == nullptr) { 298 printf("AudioRecording::save(%s) failed to open file\n", fileName); 299 return -errno; 300 } 301 302 written = sf_writef_float(sndFile, mData, mFrameCounter); 303 304 sf_close(sndFile); 305 return written; 306 } 307 308 int load(const char *fileName) { 309 SNDFILE *sndFile = nullptr; 310 SF_INFO info; 311 312 sndFile = sf_open(fileName, SFM_READ, &info); 313 if (sndFile == nullptr) { 314 printf("AudioRecording::load(%s) failed to open file\n", fileName); 315 return -errno; 316 } 317 318 assert(info.channels == 1); 319 320 allocate(info.frames); 321 mFrameCounter = sf_readf_float(sndFile, mData, info.frames); 322 323 sf_close(sndFile); 324 return mFrameCounter; 325 } 326 327private: 328 float *mData = nullptr; 329 int32_t mFrameCounter = 0; 330 int32_t mMaxFrames = 0; 331 int32_t mSampleRate = 48000; // common default 332}; 333 334// ==================================================================================== 335class LoopbackProcessor { 336public: 337 virtual ~LoopbackProcessor() = default; 338 339 340 virtual void reset() {} 341 342 virtual void process(float *inputData, int inputChannelCount, 343 float *outputData, int outputChannelCount, 344 int numFrames) = 0; 345 346 347 virtual void report() = 0; 348 349 virtual void printStatus() {}; 350 351 virtual int getResult() { 352 return -1; 353 } 354 355 virtual bool isDone() { 356 return false; 357 } 358 359 virtual int save(const char *fileName) { 360 (void) fileName; 361 return AAUDIO_ERROR_UNIMPLEMENTED; 362 } 363 364 virtual int load(const char *fileName) { 365 (void) fileName; 366 return AAUDIO_ERROR_UNIMPLEMENTED; 367 } 368 369 virtual void setSampleRate(int32_t sampleRate) { 370 mSampleRate = sampleRate; 371 } 372 373 int32_t getSampleRate() { 374 return mSampleRate; 375 } 376 377 // Measure peak amplitude of buffer. 378 static float measurePeakAmplitude(float *inputData, int inputChannelCount, int numFrames) { 379 float peak = 0.0f; 380 for (int i = 0; i < numFrames; i++) { 381 float pos = fabs(*inputData); 382 if (pos > peak) { 383 peak = pos; 384 } 385 inputData += inputChannelCount; 386 } 387 return peak; 388 } 389 390 391private: 392 int32_t mSampleRate = LOOPBACK_SAMPLE_RATE; 393}; 394 395class PeakDetector { 396public: 397 float process(float input) { 398 float output = mPrevious * mDecay; 399 if (input > output) { 400 output = input; 401 } 402 mPrevious = output; 403 return output; 404 } 405 406private: 407 float mDecay = 0.99f; 408 float mPrevious = 0.0f; 409}; 410 411 412static void printAudioScope(float sample) { 413 const int maxStars = 80 414 ; // arbitrary, fits on one line 415 char c = '*'; 416 if (sample < -1.0) { 417 sample = -1.0; 418 c = '$'; 419 } else if (sample > 1.0) { 420 sample = 1.0; 421 c = '$'; 422 } 423 int numSpaces = (int) (((sample + 1.0) * 0.5) * maxStars); 424 for (int i = 0; i < numSpaces; i++) { 425 putchar(' '); 426 } 427 printf("%c\n", c); 428} 429 430// ==================================================================================== 431/** 432 * Measure latency given a loopback stream data. 433 * Uses a state machine to cycle through various stages including: 434 * 435 */ 436class EchoAnalyzer : public LoopbackProcessor { 437public: 438 439 EchoAnalyzer() : LoopbackProcessor() { 440 mAudioRecording.allocate(2 * getSampleRate()); 441 mAudioRecording.setSampleRate(getSampleRate()); 442 } 443 444 void setSampleRate(int32_t sampleRate) override { 445 LoopbackProcessor::setSampleRate(sampleRate); 446 mAudioRecording.setSampleRate(sampleRate); 447 } 448 449 void reset() override { 450 mDownCounter = 200; 451 mLoopCounter = 0; 452 mMeasuredLoopGain = 0.0f; 453 mEchoGain = 1.0f; 454 mState = STATE_INITIAL_SILENCE; 455 } 456 457 virtual int getResult() { 458 return mState == STATE_DONE ? 0 : -1; 459 } 460 461 virtual bool isDone() { 462 return mState == STATE_DONE || mState == STATE_FAILED; 463 } 464 465 void setGain(float gain) { 466 mEchoGain = gain; 467 } 468 469 float getGain() { 470 return mEchoGain; 471 } 472 473 void report() override { 474 475 printf("EchoAnalyzer ---------------\n"); 476 printf(LOOPBACK_RESULT_TAG "measured.gain = %f\n", mMeasuredLoopGain); 477 printf(LOOPBACK_RESULT_TAG "echo.gain = %f\n", mEchoGain); 478 printf(LOOPBACK_RESULT_TAG "test.state = %d\n", mState); 479 if (mMeasuredLoopGain >= 0.9999) { 480 printf(" ERROR - clipping, turn down volume slightly\n"); 481 } else { 482 const float *needle = s_Impulse; 483 int needleSize = (int) (sizeof(s_Impulse) / sizeof(float)); 484 float *haystack = mAudioRecording.getData(); 485 int haystackSize = mAudioRecording.size(); 486 measureLatencyFromEchos(haystack, haystackSize, needle, needleSize, &mLatencyReport); 487 if (mLatencyReport.confidence < 0.01) { 488 printf(" ERROR - confidence too low = %f\n", mLatencyReport.confidence); 489 } else { 490 double latencyMillis = 1000.0 * mLatencyReport.latencyInFrames / getSampleRate(); 491 printf(LOOPBACK_RESULT_TAG "latency.frames = %8.2f\n", mLatencyReport.latencyInFrames); 492 printf(LOOPBACK_RESULT_TAG "latency.msec = %8.2f\n", latencyMillis); 493 printf(LOOPBACK_RESULT_TAG "latency.confidence = %8.6f\n", mLatencyReport.confidence); 494 } 495 } 496 } 497 498 void printStatus() override { 499 printf("state = %d, echo gain = %f ", mState, mEchoGain); 500 } 501 502 static void sendImpulse(float *outputData, int outputChannelCount) { 503 for (float sample : s_Impulse) { 504 *outputData = sample; 505 outputData += outputChannelCount; 506 } 507 } 508 509 void process(float *inputData, int inputChannelCount, 510 float *outputData, int outputChannelCount, 511 int numFrames) override { 512 int channelsValid = std::min(inputChannelCount, outputChannelCount); 513 float peak = 0.0f; 514 int numWritten; 515 int numSamples; 516 517 echo_state_t nextState = mState; 518 519 switch (mState) { 520 case STATE_INITIAL_SILENCE: 521 // Output silence at the beginning. 522 numSamples = numFrames * outputChannelCount; 523 for (int i = 0; i < numSamples; i++) { 524 outputData[i] = 0; 525 } 526 if (mDownCounter-- <= 0) { 527 nextState = STATE_MEASURING_GAIN; 528 //printf("%5d: switch to STATE_MEASURING_GAIN\n", mLoopCounter); 529 mDownCounter = 8; 530 } 531 break; 532 533 case STATE_MEASURING_GAIN: 534 sendImpulse(outputData, outputChannelCount); 535 peak = measurePeakAmplitude(inputData, inputChannelCount, numFrames); 536 // If we get several in a row then go to next state. 537 if (peak > mPulseThreshold) { 538 if (mDownCounter-- <= 0) { 539 //printf("%5d: switch to STATE_WAITING_FOR_SILENCE, measured peak = %f\n", 540 // mLoopCounter, peak); 541 mDownCounter = 8; 542 mMeasuredLoopGain = peak; // assumes original pulse amplitude is one 543 // Calculate gain that will give us a nice decaying echo. 544 mEchoGain = mDesiredEchoGain / mMeasuredLoopGain; 545 if (mEchoGain > MAX_ECHO_GAIN) { 546 printf("ERROR - loop gain too low. Increase the volume.\n"); 547 nextState = STATE_FAILED; 548 } else { 549 nextState = STATE_WAITING_FOR_SILENCE; 550 } 551 } 552 } else { 553 mDownCounter = 8; 554 } 555 break; 556 557 case STATE_WAITING_FOR_SILENCE: 558 // Output silence. 559 numSamples = numFrames * outputChannelCount; 560 for (int i = 0; i < numSamples; i++) { 561 outputData[i] = 0; 562 } 563 peak = measurePeakAmplitude(inputData, inputChannelCount, numFrames); 564 // If we get several in a row then go to next state. 565 if (peak < mSilenceThreshold) { 566 if (mDownCounter-- <= 0) { 567 nextState = STATE_SENDING_PULSE; 568 //printf("%5d: switch to STATE_SENDING_PULSE\n", mLoopCounter); 569 mDownCounter = 8; 570 } 571 } else { 572 mDownCounter = 8; 573 } 574 break; 575 576 case STATE_SENDING_PULSE: 577 mAudioRecording.write(inputData, inputChannelCount, numFrames); 578 sendImpulse(outputData, outputChannelCount); 579 nextState = STATE_GATHERING_ECHOS; 580 //printf("%5d: switch to STATE_GATHERING_ECHOS\n", mLoopCounter); 581 break; 582 583 case STATE_GATHERING_ECHOS: 584 numWritten = mAudioRecording.write(inputData, inputChannelCount, numFrames); 585 peak = measurePeakAmplitude(inputData, inputChannelCount, numFrames); 586 if (peak > mMeasuredLoopGain) { 587 mMeasuredLoopGain = peak; // AGC might be raising gain so adjust it on the fly. 588 // Recalculate gain that will give us a nice decaying echo. 589 mEchoGain = mDesiredEchoGain / mMeasuredLoopGain; 590 } 591 // Echo input to output. 592 for (int i = 0; i < numFrames; i++) { 593 int ic; 594 for (ic = 0; ic < channelsValid; ic++) { 595 outputData[ic] = inputData[ic] * mEchoGain; 596 } 597 for (; ic < outputChannelCount; ic++) { 598 outputData[ic] = 0; 599 } 600 inputData += inputChannelCount; 601 outputData += outputChannelCount; 602 } 603 if (numWritten < numFrames) { 604 nextState = STATE_DONE; 605 //printf("%5d: switch to STATE_DONE\n", mLoopCounter); 606 } 607 break; 608 609 case STATE_DONE: 610 default: 611 break; 612 } 613 614 mState = nextState; 615 mLoopCounter++; 616 } 617 618 int save(const char *fileName) override { 619 return mAudioRecording.save(fileName); 620 } 621 622 int load(const char *fileName) override { 623 return mAudioRecording.load(fileName); 624 } 625 626private: 627 628 enum echo_state_t { 629 STATE_INITIAL_SILENCE, 630 STATE_MEASURING_GAIN, 631 STATE_WAITING_FOR_SILENCE, 632 STATE_SENDING_PULSE, 633 STATE_GATHERING_ECHOS, 634 STATE_DONE, 635 STATE_FAILED 636 }; 637 638 int mDownCounter = 500; 639 int mLoopCounter = 0; 640 float mPulseThreshold = 0.02f; 641 float mSilenceThreshold = 0.002f; 642 float mMeasuredLoopGain = 0.0f; 643 float mDesiredEchoGain = 0.95f; 644 float mEchoGain = 1.0f; 645 echo_state_t mState = STATE_INITIAL_SILENCE; 646 647 AudioRecording mAudioRecording; // contains only the input after the gain detection burst 648 LatencyReport mLatencyReport; 649 // PeakDetector mPeakDetector; 650}; 651 652 653// ==================================================================================== 654/** 655 * Output a steady sinewave and analyze the return signal. 656 * 657 * Use a cosine transform to measure the predicted magnitude and relative phase of the 658 * looped back sine wave. Then generate a predicted signal and compare with the actual signal. 659 */ 660class SineAnalyzer : public LoopbackProcessor { 661public: 662 663 virtual int getResult() { 664 return mState == STATE_LOCKED ? 0 : -1; 665 } 666 667 void report() override { 668 printf("SineAnalyzer ------------------\n"); 669 printf(LOOPBACK_RESULT_TAG "peak.amplitude = %7.5f\n", mPeakAmplitude); 670 printf(LOOPBACK_RESULT_TAG "sine.magnitude = %7.5f\n", mMagnitude); 671 printf(LOOPBACK_RESULT_TAG "phase.offset = %7.5f\n", mPhaseOffset); 672 printf(LOOPBACK_RESULT_TAG "ref.phase = %7.5f\n", mPhase); 673 printf(LOOPBACK_RESULT_TAG "frames.accumulated = %6d\n", mFramesAccumulated); 674 printf(LOOPBACK_RESULT_TAG "sine.period = %6d\n", mPeriod); 675 printf(LOOPBACK_RESULT_TAG "test.state = %6d\n", mState); 676 printf(LOOPBACK_RESULT_TAG "frame.count = %6d\n", mFrameCounter); 677 // Did we ever get a lock? 678 bool gotLock = (mState == STATE_LOCKED) || (mGlitchCount > 0); 679 if (!gotLock) { 680 printf("ERROR - failed to lock on reference sine tone\n"); 681 } else { 682 // Only print if meaningful. 683 printf(LOOPBACK_RESULT_TAG "glitch.count = %6d\n", mGlitchCount); 684 } 685 } 686 687 void printStatus() override { 688 printf(" state = %d, glitches = %d,", mState, mGlitchCount); 689 } 690 691 double calculateMagnitude(double *phasePtr = NULL) { 692 if (mFramesAccumulated == 0) { 693 return 0.0; 694 } 695 double sinMean = mSinAccumulator / mFramesAccumulated; 696 double cosMean = mCosAccumulator / mFramesAccumulated; 697 double magnitude = 2.0 * sqrt( (sinMean * sinMean) + (cosMean * cosMean )); 698 if( phasePtr != NULL ) 699 { 700 double phase = M_PI_2 - atan2( sinMean, cosMean ); 701 *phasePtr = phase; 702 } 703 return magnitude; 704 } 705 706 /** 707 * @param inputData contains microphone data with sine signal feedback 708 * @param outputData contains the reference sine wave 709 */ 710 void process(float *inputData, int inputChannelCount, 711 float *outputData, int outputChannelCount, 712 int numFrames) override { 713 float peak = measurePeakAmplitude(inputData, inputChannelCount, numFrames); 714 if (peak > mPeakAmplitude) { 715 mPeakAmplitude = peak; 716 } 717 718 for (int i = 0; i < numFrames; i++) { 719 float sample = inputData[i * inputChannelCount]; 720 721 float sinOut = sinf(mPhase); 722 723 switch (mState) { 724 case STATE_IMMUNE: 725 case STATE_WAITING_FOR_SIGNAL: 726 break; 727 case STATE_WAITING_FOR_LOCK: 728 mSinAccumulator += sample * sinOut; 729 mCosAccumulator += sample * cosf(mPhase); 730 mFramesAccumulated++; 731 // Must be a multiple of the period or the calculation will not be accurate. 732 if (mFramesAccumulated == mPeriod * 4) { 733 mPhaseOffset = 0.0; 734 mMagnitude = calculateMagnitude(&mPhaseOffset); 735 if (mMagnitude > mThreshold) { 736 if (fabs(mPreviousPhaseOffset - mPhaseOffset) < 0.001) { 737 mState = STATE_LOCKED; 738 //printf("%5d: switch to STATE_LOCKED\n", mFrameCounter); 739 } 740 mPreviousPhaseOffset = mPhaseOffset; 741 } 742 resetAccumulator(); 743 } 744 break; 745 746 case STATE_LOCKED: { 747 // Predict next sine value 748 float predicted = sinf(mPhase + mPhaseOffset) * mMagnitude; 749 // printf(" predicted = %f, actual = %f\n", predicted, sample); 750 751 float diff = predicted - sample; 752 if (fabs(diff) > mTolerance) { 753 mGlitchCount++; 754 //printf("%5d: Got a glitch # %d, predicted = %f, actual = %f\n", 755 // mFrameCounter, mGlitchCount, predicted, sample); 756 mState = STATE_IMMUNE; 757 //printf("%5d: switch to STATE_IMMUNE\n", mFrameCounter); 758 mDownCounter = mPeriod; // Set duration of IMMUNE state. 759 } 760 } break; 761 } 762 763 // Output sine wave so we can measure it. 764 outputData[i * outputChannelCount] = (sinOut * mOutputAmplitude) 765 + (mWhiteNoise.nextRandomDouble() * mNoiseAmplitude); 766 // printf("%5d: sin(%f) = %f, %f\n", i, mPhase, sinOut, mPhaseIncrement); 767 768 // advance and wrap phase 769 mPhase += mPhaseIncrement; 770 if (mPhase > M_PI) { 771 mPhase -= (2.0 * M_PI); 772 } 773 774 mFrameCounter++; 775 } 776 777 // Do these once per buffer. 778 switch (mState) { 779 case STATE_IMMUNE: 780 mDownCounter -= numFrames; 781 if (mDownCounter <= 0) { 782 mState = STATE_WAITING_FOR_SIGNAL; 783 //printf("%5d: switch to STATE_WAITING_FOR_SIGNAL\n", mFrameCounter); 784 } 785 break; 786 case STATE_WAITING_FOR_SIGNAL: 787 if (peak > mThreshold) { 788 mState = STATE_WAITING_FOR_LOCK; 789 //printf("%5d: switch to STATE_WAITING_FOR_LOCK\n", mFrameCounter); 790 resetAccumulator(); 791 } 792 break; 793 case STATE_WAITING_FOR_LOCK: 794 case STATE_LOCKED: 795 break; 796 } 797 798 } 799 800 void resetAccumulator() { 801 mFramesAccumulated = 0; 802 mSinAccumulator = 0.0; 803 mCosAccumulator = 0.0; 804 } 805 806 void reset() override { 807 mGlitchCount = 0; 808 mState = STATE_IMMUNE; 809 mPhaseIncrement = 2.0 * M_PI / mPeriod; 810 printf("phaseInc = %f for period %d\n", mPhaseIncrement, mPeriod); 811 resetAccumulator(); 812 } 813 814private: 815 816 enum sine_state_t { 817 STATE_IMMUNE, 818 STATE_WAITING_FOR_SIGNAL, 819 STATE_WAITING_FOR_LOCK, 820 STATE_LOCKED 821 }; 822 823 int mPeriod = 79; 824 double mPhaseIncrement = 0.0; 825 double mPhase = 0.0; 826 double mPhaseOffset = 0.0; 827 double mPreviousPhaseOffset = 0.0; 828 double mMagnitude = 0.0; 829 double mThreshold = 0.005; 830 double mTolerance = 0.01; 831 int32_t mFramesAccumulated = 0; 832 double mSinAccumulator = 0.0; 833 double mCosAccumulator = 0.0; 834 int32_t mGlitchCount = 0; 835 double mPeakAmplitude = 0.0; 836 int mDownCounter = 4000; 837 int32_t mFrameCounter = 0; 838 float mOutputAmplitude = 0.75; 839 840 PseudoRandom mWhiteNoise; 841 float mNoiseAmplitude = 0.00; // Used to experiment with warbling caused by DRC. 842 843 sine_state_t mState = STATE_IMMUNE; 844}; 845 846 847#undef LOOPBACK_SAMPLE_RATE 848#undef LOOPBACK_RESULT_TAG 849 850#endif /* AAUDIO_EXAMPLES_LOOPBACK_ANALYSER_H */ 851