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