Juan Salazar / robotic_fish_7

All mbed code for control over dive planes, pump motor, valve motor, BCUs, UART interface, etc.

Dependencies:   mbed ESC mbed MODDMA

robotic_fish_6/AcousticControl/ToneDetector.cpp@0:c3a329a5b05d, 2020-01-14 (annotated)

Committer:
juansal12
Date:
Tue Jan 14 19:17:05 2020 +0000
Revision:
0:c3a329a5b05d
Sofi7 mbed code;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
juansal12 0:c3a329a5b05d 1 /*
juansal12 0:c3a329a5b05d 2 * Author: Joseph DelPreto
juansal12 0:c3a329a5b05d 3 */
juansal12 0:c3a329a5b05d 4
juansal12 0:c3a329a5b05d 5 #include "ToneDetector.h"
juansal12 0:c3a329a5b05d 6
juansal12 0:c3a329a5b05d 7 #ifdef acousticControl
juansal12 0:c3a329a5b05d 8
juansal12 0:c3a329a5b05d 9 // The static instance
juansal12 0:c3a329a5b05d 10 ToneDetector toneDetector;
juansal12 0:c3a329a5b05d 11 #ifdef streamAcousticControlLog
juansal12 0:c3a329a5b05d 12 int32_t acousticControlLogToStream[5] = {0,0,0,0,0};
juansal12 0:c3a329a5b05d 13 #endif
juansal12 0:c3a329a5b05d 14
juansal12 0:c3a329a5b05d 15 //============================================
juansal12 0:c3a329a5b05d 16 // Initialization
juansal12 0:c3a329a5b05d 17 //============================================
juansal12 0:c3a329a5b05d 18
juansal12 0:c3a329a5b05d 19 // Constructor
juansal12 0:c3a329a5b05d 20 ToneDetector::ToneDetector() :
juansal12 0:c3a329a5b05d 21 terminated(0),
juansal12 0:c3a329a5b05d 22 fillingBuffer0(false),
juansal12 0:c3a329a5b05d 23 transferComplete(false),
juansal12 0:c3a329a5b05d 24 readyToBegin(false),
juansal12 0:c3a329a5b05d 25 callbackFunction(0)
juansal12 0:c3a329a5b05d 26 #ifdef artificialSamplesMode
juansal12 0:c3a329a5b05d 27 , numTestSamples(0),
juansal12 0:c3a329a5b05d 28 testSampleIndex(0),
juansal12 0:c3a329a5b05d 29 sampleIndex(0)
juansal12 0:c3a329a5b05d 30 #endif
juansal12 0:c3a329a5b05d 31 {
juansal12 0:c3a329a5b05d 32 }
juansal12 0:c3a329a5b05d 33
juansal12 0:c3a329a5b05d 34 // Set a callback function that should be called after each buffer is processed
juansal12 0:c3a329a5b05d 35 void ToneDetector::setCallback(void (*myFunction)(int32_t* tonePowers, uint32_t signalLevel))
juansal12 0:c3a329a5b05d 36 {
juansal12 0:c3a329a5b05d 37 callbackFunction = myFunction;
juansal12 0:c3a329a5b05d 38 }
juansal12 0:c3a329a5b05d 39
juansal12 0:c3a329a5b05d 40 // Initialize the Goertzel variables, arrays, etc.
juansal12 0:c3a329a5b05d 41 // sampleWindow, sampleInterval, and tones must have been previously set
juansal12 0:c3a329a5b05d 42 // if not, this method will return without doing anything
juansal12 0:c3a329a5b05d 43 void ToneDetector::init()
juansal12 0:c3a329a5b05d 44 {
juansal12 0:c3a329a5b05d 45 // Make sure sampleWindow, sampleInterval, and desired tones have been set
juansal12 0:c3a329a5b05d 46 if(sampleWindow == 0 || sampleInterval == 0 || numTones == 0)
juansal12 0:c3a329a5b05d 47 return;
juansal12 0:c3a329a5b05d 48
juansal12 0:c3a329a5b05d 49 // Initialize sample buffers
juansal12 0:c3a329a5b05d 50 //printf("Sampling interval: %d us\n", sampleInterval);
juansal12 0:c3a329a5b05d 51 //printf("Buffer size: %d samples\n", sampleWindow);
juansal12 0:c3a329a5b05d 52 for(uint16_t i = 0; i < sampleWindow; i++) // not using memset since sizeof seems to not work with uint16_t?
juansal12 0:c3a329a5b05d 53 {
juansal12 0:c3a329a5b05d 54 sampleBuffer0[i] = 0;
juansal12 0:c3a329a5b05d 55 sampleBuffer1[i] = 0;
juansal12 0:c3a329a5b05d 56 }
juansal12 0:c3a329a5b05d 57 #if defined(recordSamples) && !defined(recordStreaming)
juansal12 0:c3a329a5b05d 58 savedSamplesIndex = 0;
juansal12 0:c3a329a5b05d 59 for(uint16_t i = 0; i < numSavedSamples; i++) // not using memset since sizeof seems to not work with uint16_t?
juansal12 0:c3a329a5b05d 60 savedSamples[i] = 0;
juansal12 0:c3a329a5b05d 61 #endif
juansal12 0:c3a329a5b05d 62
juansal12 0:c3a329a5b05d 63 // Initialize goertzel arrays
juansal12 0:c3a329a5b05d 64 tonePowersWindowIndex = 0;
juansal12 0:c3a329a5b05d 65 for(int i = 0; i < numTones; i++)
juansal12 0:c3a329a5b05d 66 {
juansal12 0:c3a329a5b05d 67 goertzelCoefficients[i] = 0;
juansal12 0:c3a329a5b05d 68 for(int w = 0; w < tonePowersWindow; w++)
juansal12 0:c3a329a5b05d 69 tonePowers[i][w] = 0;
juansal12 0:c3a329a5b05d 70 tonePowersSum[i] = 0;
juansal12 0:c3a329a5b05d 71 }
juansal12 0:c3a329a5b05d 72 readyToThreshold = false;
juansal12 0:c3a329a5b05d 73
juansal12 0:c3a329a5b05d 74 // Some convenience variables as doubles for precision (will cast to fixed point later)
juansal12 0:c3a329a5b05d 75 double sampleFrequency = 1000.0/((double)sampleInterval/1000.0); // Hz
juansal12 0:c3a329a5b05d 76 double N = (double) sampleWindow;
juansal12 0:c3a329a5b05d 77
juansal12 0:c3a329a5b05d 78 // Calculate the coefficient for each tone
juansal12 0:c3a329a5b05d 79 //printf("Initializing Goertzel algorithm\n");
juansal12 0:c3a329a5b05d 80 //printf(" Bin size: %f\n", sampleFrequency/N);
juansal12 0:c3a329a5b05d 81 for(int i = 0; i < numTones; i++)
juansal12 0:c3a329a5b05d 82 {
juansal12 0:c3a329a5b05d 83 // Determine K and then f for desired tone (f = desiredF/Fs)
juansal12 0:c3a329a5b05d 84 // tone/fs = f = K/N --> K = (int)(tone*N/fs)
juansal12 0:c3a329a5b05d 85 double tone = targetTones[i];
juansal12 0:c3a329a5b05d 86 int k = (int) (0.5 + ((N * tone) / sampleFrequency));
juansal12 0:c3a329a5b05d 87 float f = ((float)k) / N;
juansal12 0:c3a329a5b05d 88 //printf(" Desired tone %f -> %f", tone, f*sampleFrequency);
juansal12 0:c3a329a5b05d 89 float cosine = cos(2.0 * PI * f);
juansal12 0:c3a329a5b05d 90 // Store the result as a fixed-point number
juansal12 0:c3a329a5b05d 91 goertzelCoefficients[i] = toFixedPoint(2.0*cosine);
juansal12 0:c3a329a5b05d 92 //printf("\t (coefficient: %f)\n", toFloat(goertzelCoefficients[i]));
juansal12 0:c3a329a5b05d 93 }
juansal12 0:c3a329a5b05d 94
juansal12 0:c3a329a5b05d 95 #if defined(recordOutput) && !defined(recordStreaming)
juansal12 0:c3a329a5b05d 96 savedTonePowersIndex = 0;
juansal12 0:c3a329a5b05d 97 for(uint16_t i = 0; i < numSavedTonePowers; i++) // not using memset since sizeof seems to not work with uint16_t?
juansal12 0:c3a329a5b05d 98 {
juansal12 0:c3a329a5b05d 99 for(uint8_t t = 0; t < numTones; t++)
juansal12 0:c3a329a5b05d 100 savedTonePowers[i][t] = 0;
juansal12 0:c3a329a5b05d 101 }
juansal12 0:c3a329a5b05d 102 #endif
juansal12 0:c3a329a5b05d 103
juansal12 0:c3a329a5b05d 104 // We're ready to process some tunes!
juansal12 0:c3a329a5b05d 105 readyToBegin = true;
juansal12 0:c3a329a5b05d 106 }
juansal12 0:c3a329a5b05d 107
juansal12 0:c3a329a5b05d 108 #ifndef artificialSamplesMode
juansal12 0:c3a329a5b05d 109 // Configure and start the DMA channels for ADC sample gathering
juansal12 0:c3a329a5b05d 110 // Will have a linked list of two DMA operations, one for each buffer
juansal12 0:c3a329a5b05d 111 void ToneDetector::startDMA()
juansal12 0:c3a329a5b05d 112 {
juansal12 0:c3a329a5b05d 113 // Create the Linked List Items
juansal12 0:c3a329a5b05d 114 lli[0] = new MODDMA_LLI;
juansal12 0:c3a329a5b05d 115 lli[1] = new MODDMA_LLI;
juansal12 0:c3a329a5b05d 116
juansal12 0:c3a329a5b05d 117 // Prepare DMA configuration, which will be chained (one operation for each buffer)
juansal12 0:c3a329a5b05d 118 dmaConf = new MODDMA_Config;
juansal12 0:c3a329a5b05d 119 dmaConf
juansal12 0:c3a329a5b05d 120 ->channelNum ( MODDMA::Channel_0 )
juansal12 0:c3a329a5b05d 121 ->srcMemAddr ( 0 )
juansal12 0:c3a329a5b05d 122 ->dstMemAddr ( (uint32_t)sampleBuffer0 )
juansal12 0:c3a329a5b05d 123 ->transferSize ( sampleWindow )
juansal12 0:c3a329a5b05d 124 ->transferType ( MODDMA::p2m )
juansal12 0:c3a329a5b05d 125 ->transferWidth ( MODDMA::word )
juansal12 0:c3a329a5b05d 126 ->srcConn ( MODDMA::ADC )
juansal12 0:c3a329a5b05d 127 ->dstConn ( 0 )
juansal12 0:c3a329a5b05d 128 ->dmaLLI ( (uint32_t)lli[1] ) // Looks like it does the above setup and then calls this LLI - thus we have this setup mimic lli[0] and then the chain actually starts by calling lli[1]
juansal12 0:c3a329a5b05d 129 ->attach_tc ( &TC0_callback )
juansal12 0:c3a329a5b05d 130 ->attach_err ( &ERR0_callback )
juansal12 0:c3a329a5b05d 131 ;
juansal12 0:c3a329a5b05d 132 // Create LLI to transfer from ADC to adcBuffer0 (and then launch lli[1])
juansal12 0:c3a329a5b05d 133 lli[0]->SrcAddr = (uint32_t)dma.LUTPerAddr(dmaConf->srcConn());
juansal12 0:c3a329a5b05d 134 lli[0]->DstAddr = (uint32_t)sampleBuffer0;
juansal12 0:c3a329a5b05d 135 lli[0]->NextLLI = (uint32_t) lli[1];
juansal12 0:c3a329a5b05d 136 lli[0]->Control = dma.CxControl_TransferSize(dmaConf->transferSize())
juansal12 0:c3a329a5b05d 137 | dma.CxControl_SBSize((uint32_t)dma.LUTPerBurst(dmaConf->srcConn()))
juansal12 0:c3a329a5b05d 138 | dma.CxControl_DBSize((uint32_t)dma.LUTPerBurst(dmaConf->srcConn()))
juansal12 0:c3a329a5b05d 139 | dma.CxControl_SWidth((uint32_t)dma.LUTPerWid(dmaConf->srcConn()))
juansal12 0:c3a329a5b05d 140 | dma.CxControl_DWidth((uint32_t)dma.LUTPerWid(dmaConf->srcConn()))
juansal12 0:c3a329a5b05d 141 | dma.CxControl_DI()
juansal12 0:c3a329a5b05d 142 | dma.CxControl_I();
juansal12 0:c3a329a5b05d 143 // Create LLI to transfer from ADC to adcBuffer1 (and then launch lli[0] to repeat)
juansal12 0:c3a329a5b05d 144 lli[1]->SrcAddr = (uint32_t)dma.LUTPerAddr(dmaConf->srcConn());
juansal12 0:c3a329a5b05d 145 lli[1]->DstAddr = (uint32_t)sampleBuffer1;
juansal12 0:c3a329a5b05d 146 lli[1]->NextLLI = (uint32_t) lli[0];
juansal12 0:c3a329a5b05d 147 lli[1]->Control = dma.CxControl_TransferSize(dmaConf->transferSize())
juansal12 0:c3a329a5b05d 148 | dma.CxControl_SBSize((uint32_t)dma.LUTPerBurst(dmaConf->srcConn()))
juansal12 0:c3a329a5b05d 149 | dma.CxControl_DBSize((uint32_t)dma.LUTPerBurst(dmaConf->srcConn()))
juansal12 0:c3a329a5b05d 150 | dma.CxControl_SWidth((uint32_t)dma.LUTPerWid(dmaConf->srcConn()))
juansal12 0:c3a329a5b05d 151 | dma.CxControl_DWidth((uint32_t)dma.LUTPerWid(dmaConf->srcConn()))
juansal12 0:c3a329a5b05d 152 | dma.CxControl_DI()
juansal12 0:c3a329a5b05d 153 | dma.CxControl_I();
juansal12 0:c3a329a5b05d 154
juansal12 0:c3a329a5b05d 155 // Start the DMA chain
juansal12 0:c3a329a5b05d 156 fillingBuffer0 = true;
juansal12 0:c3a329a5b05d 157 transferComplete = false;
juansal12 0:c3a329a5b05d 158 if (!dma.Prepare(dmaConf)) {
juansal12 0:c3a329a5b05d 159 error("Doh! Error preparing initial dma configuration");
juansal12 0:c3a329a5b05d 160 }
juansal12 0:c3a329a5b05d 161 }
juansal12 0:c3a329a5b05d 162
juansal12 0:c3a329a5b05d 163 // Configure the ADC to trigger on Timer1
juansal12 0:c3a329a5b05d 164 // Start the timer with the desired sampling interval
juansal12 0:c3a329a5b05d 165 void ToneDetector::startADC()
juansal12 0:c3a329a5b05d 166 {
juansal12 0:c3a329a5b05d 167 // We use the ADC irq to trigger DMA and the manual says
juansal12 0:c3a329a5b05d 168 // that in this case the NVIC for ADC must be disabled.
juansal12 0:c3a329a5b05d 169 NVIC_DisableIRQ(ADC_IRQn);
juansal12 0:c3a329a5b05d 170
juansal12 0:c3a329a5b05d 171 // Power up the ADC and set PCLK
juansal12 0:c3a329a5b05d 172 LPC_SC->PCONP |= (1UL << 12); // enable power
juansal12 0:c3a329a5b05d 173 LPC_SC->PCLKSEL0 &= ~(3UL << 24); // Clear divider to use CCLK/8 = 12MHz directly (see page 57) // original example code comment: PCLK = CCLK/4 96M/4 = 24MHz
juansal12 0:c3a329a5b05d 174
juansal12 0:c3a329a5b05d 175 // Enable the ADC, 12MHz, ADC0.0
juansal12 0:c3a329a5b05d 176 LPC_ADC->ADCR = (1UL << 21);
juansal12 0:c3a329a5b05d 177 LPC_ADC->ADCR &= ~(255 << 8); // No clock divider (use the 12MHz directly)
juansal12 0:c3a329a5b05d 178
juansal12 0:c3a329a5b05d 179 // Set the pin functions to ADC (use pin p15)
juansal12 0:c3a329a5b05d 180 LPC_PINCON->PINSEL1 &= ~(3UL << 14); /* P0.23, Mbed p15. */
juansal12 0:c3a329a5b05d 181 LPC_PINCON->PINSEL1 |= (1UL << 14);
juansal12 0:c3a329a5b05d 182
juansal12 0:c3a329a5b05d 183 // Enable ADC irq flag (to DMA).
juansal12 0:c3a329a5b05d 184 // Note, don't set the individual flags,
juansal12 0:c3a329a5b05d 185 // just set the global flag.
juansal12 0:c3a329a5b05d 186 LPC_ADC->ADINTEN = 0x100;
juansal12 0:c3a329a5b05d 187
juansal12 0:c3a329a5b05d 188 // (see page 586 of http://www.nxp.com/documents/user_manual/UM10360.pdf)
juansal12 0:c3a329a5b05d 189 // Disable burst mode
juansal12 0:c3a329a5b05d 190 LPC_ADC->ADCR &= ~(1 << 16);
juansal12 0:c3a329a5b05d 191 // Have the ADC convert based on timer 1
juansal12 0:c3a329a5b05d 192 LPC_ADC->ADCR |= (6 << 24); // Trigger on MAT1.0
juansal12 0:c3a329a5b05d 193 LPC_ADC->ADCR |= (1 << 27); // Falling edge
juansal12 0:c3a329a5b05d 194
juansal12 0:c3a329a5b05d 195 // Set up timer 1
juansal12 0:c3a329a5b05d 196 LPC_SC->PCONP |= (1UL << 2); // Power on Timer1
juansal12 0:c3a329a5b05d 197 LPC_SC->PCLKSEL0 &= ~(3 << 4); // No clock divider (use 12MHz directly) (see page 57 of datasheet)
juansal12 0:c3a329a5b05d 198 LPC_TIM1->PR = 11; // TC clocks at 1MHz since we selected 12MHz above (see page 507 of datasheet)
juansal12 0:c3a329a5b05d 199 LPC_TIM1->MR0 = sampleInterval-1; // sampling interval in us
juansal12 0:c3a329a5b05d 200 LPC_TIM1->MCR = 3; // Reset TCR to zero on match
juansal12 0:c3a329a5b05d 201 LPC_TIM1->EMR = (3UL<<4)|1; // Make MAT1.0 toggle.
juansal12 0:c3a329a5b05d 202 //NVIC_EnableIRQ(TIMER1_IRQn); // Enable timer1 interrupt NOTE: enabling the interrupt when MCR is 3 will make everything stop working. enabling the interrupt when MCR is 2 will work but the interrupt isn't actually called.
juansal12 0:c3a329a5b05d 203
juansal12 0:c3a329a5b05d 204 // Start the timer (which thus starts the ADC)
juansal12 0:c3a329a5b05d 205 LPC_TIM1->TCR=0;
juansal12 0:c3a329a5b05d 206 LPC_TIM1->TCR=1;
juansal12 0:c3a329a5b05d 207 }
juansal12 0:c3a329a5b05d 208 #endif // end if(not artificial samples mode)
juansal12 0:c3a329a5b05d 209
juansal12 0:c3a329a5b05d 210 //============================================
juansal12 0:c3a329a5b05d 211 // Execution Control
juansal12 0:c3a329a5b05d 212 //============================================
juansal12 0:c3a329a5b05d 213
juansal12 0:c3a329a5b05d 214 // Start acquiring and processing samples
juansal12 0:c3a329a5b05d 215 // Will run forever or until stop() is called
juansal12 0:c3a329a5b05d 216 void ToneDetector::run()
juansal12 0:c3a329a5b05d 217 {
juansal12 0:c3a329a5b05d 218 if(!readyToBegin)
juansal12 0:c3a329a5b05d 219 return;
juansal12 0:c3a329a5b05d 220 terminated = false;
juansal12 0:c3a329a5b05d 221 //printf("\nTone detector starting...\n");
juansal12 0:c3a329a5b05d 222 #ifdef recordStreaming
juansal12 0:c3a329a5b05d 223 #ifdef recordSamples
juansal12 0:c3a329a5b05d 224 printf("\tSample (V)");
juansal12 0:c3a329a5b05d 225 printf("\n");
juansal12 0:c3a329a5b05d 226 #endif
juansal12 0:c3a329a5b05d 227 #ifdef recordOutput
juansal12 0:c3a329a5b05d 228 for(uint8_t t = 0; t < numTones; t++)
juansal12 0:c3a329a5b05d 229 printf("\t%f Hz", targetTones[t]);
juansal12 0:c3a329a5b05d 230 printf("\n");
juansal12 0:c3a329a5b05d 231 #endif
juansal12 0:c3a329a5b05d 232 #endif
juansal12 0:c3a329a5b05d 233
juansal12 0:c3a329a5b05d 234 // Set up initial buffer configuration
juansal12 0:c3a329a5b05d 235 samplesWriting = sampleBuffer0;
juansal12 0:c3a329a5b05d 236 samplesProcessing = sampleBuffer1;
juansal12 0:c3a329a5b05d 237 fillingBuffer0 = true;
juansal12 0:c3a329a5b05d 238 transferComplete = false;
juansal12 0:c3a329a5b05d 239 // Start periodically sampling
juansal12 0:c3a329a5b05d 240 #ifdef artificialSamplesMode
juansal12 0:c3a329a5b05d 241 initTestModeSamples(); // artificially create samples
juansal12 0:c3a329a5b05d 242 sampleTicker.attach_us(&toneDetector, &ToneDetector::tickerCallback, sampleInterval); // "sample" artificial samples at desired rate
juansal12 0:c3a329a5b05d 243 #else
juansal12 0:c3a329a5b05d 244 startDMA();
juansal12 0:c3a329a5b05d 245 if(!terminated) // If DMA got an error, terminated will be set true
juansal12 0:c3a329a5b05d 246 startADC();
juansal12 0:c3a329a5b05d 247 #endif
juansal12 0:c3a329a5b05d 248
juansal12 0:c3a329a5b05d 249 #ifdef debugLEDs
juansal12 0:c3a329a5b05d 250 led1 = 1; // Indicate start of tone detection
juansal12 0:c3a329a5b05d 251 #endif
juansal12 0:c3a329a5b05d 252 #ifdef debugPins // after LEDs to be more accurate
juansal12 0:c3a329a5b05d 253 debugPin1 = 1; // Indicate start of tone detection
juansal12 0:c3a329a5b05d 254 #endif
juansal12 0:c3a329a5b05d 255
juansal12 0:c3a329a5b05d 256 // Main loop
juansal12 0:c3a329a5b05d 257 // Wait for buffers to fill and then process them
juansal12 0:c3a329a5b05d 258 while(!terminated)
juansal12 0:c3a329a5b05d 259 {
juansal12 0:c3a329a5b05d 260 // Check if a buffer of samples is gathered and if so process it
juansal12 0:c3a329a5b05d 261 if(transferComplete)
juansal12 0:c3a329a5b05d 262 {
juansal12 0:c3a329a5b05d 263 transferComplete = false;
juansal12 0:c3a329a5b05d 264 processSamples();
juansal12 0:c3a329a5b05d 265 }
juansal12 0:c3a329a5b05d 266 }
juansal12 0:c3a329a5b05d 267
juansal12 0:c3a329a5b05d 268 #ifdef debugPins // before LEDs to be more accurate
juansal12 0:c3a329a5b05d 269 debugPin1 = 0; // Indicate cessation of tone detection
juansal12 0:c3a329a5b05d 270 debugPin2 = 0; // Turn off indicator that at least two buffers were processed
juansal12 0:c3a329a5b05d 271 debugPin4 = 1; // Indicate completion
juansal12 0:c3a329a5b05d 272 #endif
juansal12 0:c3a329a5b05d 273 #ifdef debugLEDs
juansal12 0:c3a329a5b05d 274 led1 = 0; // Indicate cessation of tone detection
juansal12 0:c3a329a5b05d 275 led2 = 0; // Turn off indicator that at least one buffer was processed
juansal12 0:c3a329a5b05d 276 led4 = 1; // Indicate completion
juansal12 0:c3a329a5b05d 277 #endif
juansal12 0:c3a329a5b05d 278 }
juansal12 0:c3a329a5b05d 279
juansal12 0:c3a329a5b05d 280 // Finish up (write results to file and whatnot)
juansal12 0:c3a329a5b05d 281 // This is separate method so that main program can time the running itself without this extra overhead
juansal12 0:c3a329a5b05d 282 void ToneDetector::finish()
juansal12 0:c3a329a5b05d 283 {
juansal12 0:c3a329a5b05d 284 //printf("Tone detector finished\n");
juansal12 0:c3a329a5b05d 285
juansal12 0:c3a329a5b05d 286 #if defined(recordSamples) && !defined(recordStreaming)
juansal12 0:c3a329a5b05d 287 // Write saved samples to file
juansal12 0:c3a329a5b05d 288 LocalFileSystem local("local");
juansal12 0:c3a329a5b05d 289 FILE *foutSamples = fopen("/local/samples.wp", "w"); // Open "samples.wp" on the local file system for writing
juansal12 0:c3a329a5b05d 290 fprintf(foutSamples, "Sample (V)\n");
juansal12 0:c3a329a5b05d 291 uint16_t savedSamplesN = savedSamplesIndex;
juansal12 0:c3a329a5b05d 292 do
juansal12 0:c3a329a5b05d 293 {
juansal12 0:c3a329a5b05d 294 fprintf(foutSamples, "%f\n", toFloat(savedSamples[savedSamplesN])/4.0*3.3);
juansal12 0:c3a329a5b05d 295 savedSamplesN++;
juansal12 0:c3a329a5b05d 296 savedSamplesN %= numSavedSamples;
juansal12 0:c3a329a5b05d 297 } while(savedSamplesN != savedSamplesIndex);
juansal12 0:c3a329a5b05d 298 fclose(foutSamples);
juansal12 0:c3a329a5b05d 299 #endif // recordSamples && !recordStreaming
juansal12 0:c3a329a5b05d 300 #if defined(recordOutput) && !defined(recordStreaming)
juansal12 0:c3a329a5b05d 301 // Write saved outputs to file
juansal12 0:c3a329a5b05d 302 #ifndef recordSamples
juansal12 0:c3a329a5b05d 303 LocalFileSystem local("local");
juansal12 0:c3a329a5b05d 304 #endif // not recordSamples
juansal12 0:c3a329a5b05d 305 FILE *foutOutput = fopen("/local/out.wp", "w"); // Open "out.wp" on the local file system for writing
juansal12 0:c3a329a5b05d 306 for(uint8_t t = 0; t < numTones; t++)
juansal12 0:c3a329a5b05d 307 fprintf(foutOutput, "%f Hz\t", tones[t]);
juansal12 0:c3a329a5b05d 308 uint16_t savedTonePowersN = savedTonePowersIndex;
juansal12 0:c3a329a5b05d 309 do
juansal12 0:c3a329a5b05d 310 {
juansal12 0:c3a329a5b05d 311 for(uint8_t t = 0; t < numTones; t++)
juansal12 0:c3a329a5b05d 312 fprintf(foutOutput, "%ld \t", savedTonePowers[savedTonePowersN][t]);
juansal12 0:c3a329a5b05d 313 fprintf(foutOutput, "\n");
juansal12 0:c3a329a5b05d 314 savedTonePowersN++;
juansal12 0:c3a329a5b05d 315 savedTonePowersN %= numSavedTonePowers;
juansal12 0:c3a329a5b05d 316 } while(savedTonePowersN != savedTonePowersIndex);
juansal12 0:c3a329a5b05d 317 fclose(foutOutput);
juansal12 0:c3a329a5b05d 318 #endif // recordOutput
juansal12 0:c3a329a5b05d 319 }
juansal12 0:c3a329a5b05d 320
juansal12 0:c3a329a5b05d 321 // Terminate the tone detector
juansal12 0:c3a329a5b05d 322 // Note: Will actually terminate after next time buffer1 is filled, so won't be instantaneous
juansal12 0:c3a329a5b05d 323 void ToneDetector::stop()
juansal12 0:c3a329a5b05d 324 {
juansal12 0:c3a329a5b05d 325 // Stop sampling
juansal12 0:c3a329a5b05d 326 #ifdef artificialSamplesMode
juansal12 0:c3a329a5b05d 327 sampleTicker.detach();
juansal12 0:c3a329a5b05d 328 #else
juansal12 0:c3a329a5b05d 329 lli[1]->Control = 0; // Make the DMA stop after next time buffer1 is filled
juansal12 0:c3a329a5b05d 330 while(!(!fillingBuffer0 && transferComplete)); // Wait for buffer1 to be filled
juansal12 0:c3a329a5b05d 331 LPC_TIM1->TCR=0; // Stop the timer (and thus the ADC)
juansal12 0:c3a329a5b05d 332 LPC_SC->PCONP &= ~(1UL << 2); // Power off the timer
juansal12 0:c3a329a5b05d 333 #endif
juansal12 0:c3a329a5b05d 334
juansal12 0:c3a329a5b05d 335 // Stop the main loop
juansal12 0:c3a329a5b05d 336 terminated = true;
juansal12 0:c3a329a5b05d 337 }
juansal12 0:c3a329a5b05d 338
juansal12 0:c3a329a5b05d 339 //============================================
juansal12 0:c3a329a5b05d 340 // Sampling / Processing
juansal12 0:c3a329a5b05d 341 //============================================
juansal12 0:c3a329a5b05d 342
juansal12 0:c3a329a5b05d 343 // Acquire a new sample
juansal12 0:c3a329a5b05d 344 #ifdef artificialSamplesMode
juansal12 0:c3a329a5b05d 345 // If a buffer has been filled, swap buffers and signal the main thread to process it
juansal12 0:c3a329a5b05d 346 void ToneDetector::tickerCallback()
juansal12 0:c3a329a5b05d 347 {
juansal12 0:c3a329a5b05d 348 // Get a sample
juansal12 0:c3a329a5b05d 349 samplesWriting[sampleIndex] = testSamples[testSampleIndex];
juansal12 0:c3a329a5b05d 350 testSampleIndex++;
juansal12 0:c3a329a5b05d 351 testSampleIndex %= numTestSamples;
juansal12 0:c3a329a5b05d 352
juansal12 0:c3a329a5b05d 353 // Increment sample index
juansal12 0:c3a329a5b05d 354 sampleIndex++;
juansal12 0:c3a329a5b05d 355 sampleIndex %= sampleWindow;
juansal12 0:c3a329a5b05d 356
juansal12 0:c3a329a5b05d 357 // See if we just finished a buffer
juansal12 0:c3a329a5b05d 358 if(sampleIndex == 0)
juansal12 0:c3a329a5b05d 359 {
juansal12 0:c3a329a5b05d 360 // Swap writing and processing buffers
juansal12 0:c3a329a5b05d 361 // Let the main tone detector thread know that processing should take place
juansal12 0:c3a329a5b05d 362 if(fillingBuffer0)
juansal12 0:c3a329a5b05d 363 {
juansal12 0:c3a329a5b05d 364 samplesProcessing = sampleBuffer0;
juansal12 0:c3a329a5b05d 365 samplesWriting = sampleBuffer1;
juansal12 0:c3a329a5b05d 366 }
juansal12 0:c3a329a5b05d 367 else
juansal12 0:c3a329a5b05d 368 {
juansal12 0:c3a329a5b05d 369 samplesProcessing = sampleBuffer1;
juansal12 0:c3a329a5b05d 370 samplesWriting = sampleBuffer0;
juansal12 0:c3a329a5b05d 371 }
juansal12 0:c3a329a5b05d 372 transferComplete = true;
juansal12 0:c3a329a5b05d 373 fillingBuffer0 = !fillingBuffer0;
juansal12 0:c3a329a5b05d 374 }
juansal12 0:c3a329a5b05d 375 }
juansal12 0:c3a329a5b05d 376 #else // not artificial mode - we want real samples!
juansal12 0:c3a329a5b05d 377 // Callback for DMA channel 0
juansal12 0:c3a329a5b05d 378 void TC0_callback(void) // static method
juansal12 0:c3a329a5b05d 379 {
juansal12 0:c3a329a5b05d 380 // Swap writing and processing buffers used by main loop
juansal12 0:c3a329a5b05d 381 if(toneDetector.fillingBuffer0)
juansal12 0:c3a329a5b05d 382 {
juansal12 0:c3a329a5b05d 383 toneDetector.samplesProcessing = toneDetector.sampleBuffer0;
juansal12 0:c3a329a5b05d 384 toneDetector.samplesWriting = toneDetector.sampleBuffer1;
juansal12 0:c3a329a5b05d 385 }
juansal12 0:c3a329a5b05d 386 else
juansal12 0:c3a329a5b05d 387 {
juansal12 0:c3a329a5b05d 388 toneDetector.samplesProcessing = toneDetector.sampleBuffer1;
juansal12 0:c3a329a5b05d 389 toneDetector.samplesWriting = toneDetector.sampleBuffer0;
juansal12 0:c3a329a5b05d 390 }
juansal12 0:c3a329a5b05d 391 // Tell main() loop that this buffer is ready for processing
juansal12 0:c3a329a5b05d 392 toneDetector.fillingBuffer0 = !toneDetector.fillingBuffer0;
juansal12 0:c3a329a5b05d 393 toneDetector.transferComplete = true;
juansal12 0:c3a329a5b05d 394
juansal12 0:c3a329a5b05d 395 // Clear DMA IRQ flags.
juansal12 0:c3a329a5b05d 396 if(toneDetector.dma.irqType() == MODDMA::TcIrq) toneDetector.dma.clearTcIrq();
juansal12 0:c3a329a5b05d 397 if(toneDetector.dma.irqType() == MODDMA::ErrIrq) toneDetector.dma.clearErrIrq();
juansal12 0:c3a329a5b05d 398 }
juansal12 0:c3a329a5b05d 399
juansal12 0:c3a329a5b05d 400 // Configuration callback on Error for channel 0
juansal12 0:c3a329a5b05d 401 void ERR0_callback(void) // static method
juansal12 0:c3a329a5b05d 402 {
juansal12 0:c3a329a5b05d 403 // Stop sampling
juansal12 0:c3a329a5b05d 404 LPC_TIM1->TCR = 0; // Stop the timer (and thus the ADC)
juansal12 0:c3a329a5b05d 405 LPC_SC->PCONP &= ~(1UL << 2); // Power off the timer
juansal12 0:c3a329a5b05d 406
juansal12 0:c3a329a5b05d 407 // Stop the main loop (don't call stop() since that would wait for next buffer to fill)
juansal12 0:c3a329a5b05d 408 toneDetector.terminated = true;
juansal12 0:c3a329a5b05d 409
juansal12 0:c3a329a5b05d 410 error("Oh no! My Mbed EXPLODED! :( Only kidding, go find the problem (DMA chan 0)");
juansal12 0:c3a329a5b05d 411 }
juansal12 0:c3a329a5b05d 412 #endif
juansal12 0:c3a329a5b05d 413
juansal12 0:c3a329a5b05d 414 // Goertzelize the process buffer
juansal12 0:c3a329a5b05d 415 void ToneDetector::processSamples()
juansal12 0:c3a329a5b05d 416 {
juansal12 0:c3a329a5b05d 417 #ifdef debugLEDs
juansal12 0:c3a329a5b05d 418 if(fillingBuffer0)
juansal12 0:c3a329a5b05d 419 led2 = 1; // Indicate that at least two buffers have been recorded
juansal12 0:c3a329a5b05d 420 led3 = 1; // Indicate start of processing
juansal12 0:c3a329a5b05d 421 #endif
juansal12 0:c3a329a5b05d 422 #ifdef debugPins // after LEDs and timer to be more accurate
juansal12 0:c3a329a5b05d 423 if(fillingBuffer0)
juansal12 0:c3a329a5b05d 424 debugPin2 = 1; // Indicate that at least two buffers have been recorded
juansal12 0:c3a329a5b05d 425 debugPin3 = 1; // Indicate start of processing
juansal12 0:c3a329a5b05d 426 #endif
juansal12 0:c3a329a5b05d 427
juansal12 0:c3a329a5b05d 428 // Create variables for storing the Goertzel series
juansal12 0:c3a329a5b05d 429 int32_t s0, s1, s2;
juansal12 0:c3a329a5b05d 430 volatile int32_t newTonePower;
juansal12 0:c3a329a5b05d 431 // Create variables for getting max input value
juansal12 0:c3a329a5b05d 432 int32_t sample = 0;
juansal12 0:c3a329a5b05d 433 uint32_t signalLevel = 0;
juansal12 0:c3a329a5b05d 434 // For each desired tone, compute power and then reset the Goertzel
juansal12 0:c3a329a5b05d 435 for(uint8_t i = 0; i < numTones; i++)
juansal12 0:c3a329a5b05d 436 {
juansal12 0:c3a329a5b05d 437 // Reset
juansal12 0:c3a329a5b05d 438 s0 = 0;
juansal12 0:c3a329a5b05d 439 s1 = 0;
juansal12 0:c3a329a5b05d 440 s2 = 0;
juansal12 0:c3a329a5b05d 441 // Compute the Goertzel series
juansal12 0:c3a329a5b05d 442 for(uint16_t n = 0; n < sampleWindow; n++)
juansal12 0:c3a329a5b05d 443 {
juansal12 0:c3a329a5b05d 444 // Note: bottom 4 bits from ADC indicate channel, top 12 are the actual data
juansal12 0:c3a329a5b05d 445 // Note: Assuming Q10 format, we are automatically scaling the ADC value to [0, 4] range
juansal12 0:c3a329a5b05d 446 sample = ((int32_t)((samplesProcessing[n] >> 4) & 0xFFF));
juansal12 0:c3a329a5b05d 447 // Get signal level indicator
juansal12 0:c3a329a5b05d 448 if(i == 0)
juansal12 0:c3a329a5b05d 449 {
juansal12 0:c3a329a5b05d 450 if(sample-2048 >= 0)
juansal12 0:c3a329a5b05d 451 signalLevel += (sample-2048);
juansal12 0:c3a329a5b05d 452 else
juansal12 0:c3a329a5b05d 453 signalLevel += (2048-sample);
juansal12 0:c3a329a5b05d 454 }
juansal12 0:c3a329a5b05d 455 // TODO check the effect of this subtraction?
juansal12 0:c3a329a5b05d 456 //sample -= 2048;
juansal12 0:c3a329a5b05d 457 s0 = sample + fixedPointMultiply(goertzelCoefficients[i], s1) - s2;
juansal12 0:c3a329a5b05d 458 s2 = s1;
juansal12 0:c3a329a5b05d 459 s1 = s0;
juansal12 0:c3a329a5b05d 460 }
juansal12 0:c3a329a5b05d 461 // Compute the power
juansal12 0:c3a329a5b05d 462 newTonePower = fixedPointMultiply(s2,s2) + fixedPointMultiply(s1,s1) - fixedPointMultiply(fixedPointMultiply(goertzelCoefficients[i],s1),s2);
juansal12 0:c3a329a5b05d 463 // Update the running sum
juansal12 0:c3a329a5b05d 464 tonePowersSum[i] -= tonePowers[i][tonePowersWindowIndex];
juansal12 0:c3a329a5b05d 465 tonePowersSum[i] += newTonePower;
juansal12 0:c3a329a5b05d 466 // Update the history of powers
juansal12 0:c3a329a5b05d 467 tonePowers[i][tonePowersWindowIndex] = newTonePower;
juansal12 0:c3a329a5b05d 468 }
juansal12 0:c3a329a5b05d 469 // See if first circular buffer has been filled
juansal12 0:c3a329a5b05d 470 readyToThreshold = readyToThreshold || (tonePowersWindowIndex == tonePowersWindow-1);
juansal12 0:c3a329a5b05d 471 // Deliver results if a callback function was provided
juansal12 0:c3a329a5b05d 472 if(callbackFunction != 0 && readyToThreshold)
juansal12 0:c3a329a5b05d 473 callbackFunction(tonePowersSum, signalLevel >> 7); // divide signal level by 128 is basically making it an average (125 samples/buffer)
juansal12 0:c3a329a5b05d 474 #ifdef streamAcousticControlLog
juansal12 0:c3a329a5b05d 475 acousticControlLogToStream[0] = tonePowers[0][tonePowersWindowIndex];
juansal12 0:c3a329a5b05d 476 acousticControlLogToStream[1] = tonePowers[1][tonePowersWindowIndex];
juansal12 0:c3a329a5b05d 477 acousticControlLogToStream[2] = signalLevel >> 7;
juansal12 0:c3a329a5b05d 478 #endif
juansal12 0:c3a329a5b05d 479 #ifdef debugLEDs
juansal12 0:c3a329a5b05d 480 led3 = 0; // Indicate completion of processing
juansal12 0:c3a329a5b05d 481 #endif
juansal12 0:c3a329a5b05d 482 #ifdef recordSamples
juansal12 0:c3a329a5b05d 483 #ifdef recordStreaming
juansal12 0:c3a329a5b05d 484 for(uint16_t n = 0; n < sampleWindow; n++)
juansal12 0:c3a329a5b05d 485 printf("%ld\n", (samplesProcessing[n] >> 4) & 0xFFF);
juansal12 0:c3a329a5b05d 486 #else
juansal12 0:c3a329a5b05d 487 for(uint16_t n = 0; n < sampleWindow; n++)
juansal12 0:c3a329a5b05d 488 {
juansal12 0:c3a329a5b05d 489 savedSamples[savedSamplesIndex++] = (samplesProcessing[n] >> 4) & 0xFFF;
juansal12 0:c3a329a5b05d 490 savedSamplesIndex %= numSavedSamples;
juansal12 0:c3a329a5b05d 491 }
juansal12 0:c3a329a5b05d 492 #endif
juansal12 0:c3a329a5b05d 493 #endif
juansal12 0:c3a329a5b05d 494 #ifdef recordOutput
juansal12 0:c3a329a5b05d 495 #ifdef recordStreaming
juansal12 0:c3a329a5b05d 496 for(uint8_t t = 0; t < numTones; t++)
juansal12 0:c3a329a5b05d 497 printf("%ld\t", tonePowers[t][tonePowersWindowIndex] >> 10); // used to shift 10
juansal12 0:c3a329a5b05d 498 printf("\n");
juansal12 0:c3a329a5b05d 499 #else
juansal12 0:c3a329a5b05d 500 for(uint8_t t = 0; t < numTones; t++)
juansal12 0:c3a329a5b05d 501 {
juansal12 0:c3a329a5b05d 502 savedTonePowers[savedTonePowersIndex][t] = tonePowers[t][tonePowersWindowIndex];
juansal12 0:c3a329a5b05d 503 }
juansal12 0:c3a329a5b05d 504 savedTonePowersIndex++;
juansal12 0:c3a329a5b05d 505 savedTonePowersIndex %= numSavedTonePowers;
juansal12 0:c3a329a5b05d 506 #endif
juansal12 0:c3a329a5b05d 507 #endif
juansal12 0:c3a329a5b05d 508 // Increment window index (circularly)
juansal12 0:c3a329a5b05d 509 tonePowersWindowIndex = (tonePowersWindowIndex+1) % tonePowersWindow;
juansal12 0:c3a329a5b05d 510
juansal12 0:c3a329a5b05d 511 #ifdef debugPins // before LEDs, timer, and recordSamples to be more accurate
juansal12 0:c3a329a5b05d 512 debugPin3 = 0; // Indicate completion of processing
juansal12 0:c3a329a5b05d 513 #endif
juansal12 0:c3a329a5b05d 514 }
juansal12 0:c3a329a5b05d 515
juansal12 0:c3a329a5b05d 516 int32_t* ToneDetector::getTonePowers()
juansal12 0:c3a329a5b05d 517 {
juansal12 0:c3a329a5b05d 518 return tonePowersSum;
juansal12 0:c3a329a5b05d 519 }
juansal12 0:c3a329a5b05d 520
juansal12 0:c3a329a5b05d 521 //============================================
juansal12 0:c3a329a5b05d 522 // Testing / Debugging
juansal12 0:c3a329a5b05d 523 //============================================
juansal12 0:c3a329a5b05d 524
juansal12 0:c3a329a5b05d 525 #ifdef artificialSamplesMode
juansal12 0:c3a329a5b05d 526 // Use artificial samples, either from a file or from summing cosine waves of given frequencies
juansal12 0:c3a329a5b05d 527 // Samples in a file will be interpreted as volts, so should be in range [0, 3.3]
juansal12 0:c3a329a5b05d 528 void ToneDetector::initTestModeSamples()
juansal12 0:c3a329a5b05d 529 {
juansal12 0:c3a329a5b05d 530 #ifdef sampleFilename
juansal12 0:c3a329a5b05d 531 LocalFileSystem local("local");
juansal12 0:c3a329a5b05d 532 printf("Using samples from file: "); printf(sampleFilename); printf("\n");
juansal12 0:c3a329a5b05d 533 FILE *fin = fopen(sampleFilename, "r"); // Open "setup.txt" on the local file system for read
juansal12 0:c3a329a5b05d 534 if(fin == 0)
juansal12 0:c3a329a5b05d 535 {
juansal12 0:c3a329a5b05d 536 printf(" *** Cannot open file! ***\n");
juansal12 0:c3a329a5b05d 537 wait_ms(3000);
juansal12 0:c3a329a5b05d 538 numTestSamples = 1;
juansal12 0:c3a329a5b05d 539 testSamples = new uint32_t[numTestSamples];
juansal12 0:c3a329a5b05d 540 testSamples[0] = 0;
juansal12 0:c3a329a5b05d 541 testSampleIndex = 0;
juansal12 0:c3a329a5b05d 542 return;
juansal12 0:c3a329a5b05d 543 }
juansal12 0:c3a329a5b05d 544 // See how long the file is
juansal12 0:c3a329a5b05d 545 int numTestSamples = 0;
juansal12 0:c3a329a5b05d 546 float val;
juansal12 0:c3a329a5b05d 547 float maxVal = 0;
juansal12 0:c3a329a5b05d 548 float minVal = 0;
juansal12 0:c3a329a5b05d 549 float avgVal = 0;
juansal12 0:c3a329a5b05d 550 int res = fscanf(fin, "%d\n", &val);
juansal12 0:c3a329a5b05d 551 while(res > 0)
juansal12 0:c3a329a5b05d 552 {
juansal12 0:c3a329a5b05d 553 numTestSamples++;
juansal12 0:c3a329a5b05d 554 res = fscanf(fin, "%f\n", &val);
juansal12 0:c3a329a5b05d 555 if(val > maxVal)
juansal12 0:c3a329a5b05d 556 maxVal = val;
juansal12 0:c3a329a5b05d 557 if(val < minVal)
juansal12 0:c3a329a5b05d 558 minVal = val;
juansal12 0:c3a329a5b05d 559 avgVal = numTestSamples > 1 ? (avgVal + val)/2.0 : val;
juansal12 0:c3a329a5b05d 560 }
juansal12 0:c3a329a5b05d 561 printf(" Found %d samples in the file, max %f, min %f, avg %f\n", numTestSamples, maxVal, minVal, avgVal);
juansal12 0:c3a329a5b05d 562 if(minVal < 0)
juansal12 0:c3a329a5b05d 563 printf(" WARNING: File supposed to represent voltage input to ADC, so negative numbers will be interpreted as 0\n");
juansal12 0:c3a329a5b05d 564 if(maxVal > 3.3)
juansal12 0:c3a329a5b05d 565 printf(" WARNING: File supposed to represent voltage input to ADC, so numbers greater than 3.3 will be interpreted as 3.3\n");
juansal12 0:c3a329a5b05d 566 fclose(fin);
juansal12 0:c3a329a5b05d 567 // Read the samples
juansal12 0:c3a329a5b05d 568 testSamples = new uint32_t[numTestSamples];
juansal12 0:c3a329a5b05d 569 fin = fopen(sampleFilename, "r"); // Open "setup.txt" on the local file system for read
juansal12 0:c3a329a5b05d 570 for(int i = 0; i < numTestSamples; i++)
juansal12 0:c3a329a5b05d 571 {
juansal12 0:c3a329a5b05d 572 // Read the voltage
juansal12 0:c3a329a5b05d 573 fscanf(fin, "%f\n", &val);
juansal12 0:c3a329a5b05d 574 // Clip it like the ADC would
juansal12 0:c3a329a5b05d 575 val = val > 3.3 ? 3.3 : val;
juansal12 0:c3a329a5b05d 576 val = val < 0 ? 0 : val;
juansal12 0:c3a329a5b05d 577 // Convert voltage to 12-bit ADC reading
juansal12 0:c3a329a5b05d 578 testSamples[i] = val/3.3*4096.0;
juansal12 0:c3a329a5b05d 579 // Shift it by 4 to mimic what the DMA would write for a reading (lower 4 would indicate ADC channel)
juansal12 0:c3a329a5b05d 580 testSamples[i] = testSamples[i] << 4;
juansal12 0:c3a329a5b05d 581 }
juansal12 0:c3a329a5b05d 582 fclose(fin);
juansal12 0:c3a329a5b05d 583 testSampleIndex = 0;
juansal12 0:c3a329a5b05d 584 sampleIndex = 0;
juansal12 0:c3a329a5b05d 585
juansal12 0:c3a329a5b05d 586 #else // not using file for samples, will create a sum of cosine waves instead
juansal12 0:c3a329a5b05d 587
juansal12 0:c3a329a5b05d 588 numTestSamples = 1000;
juansal12 0:c3a329a5b05d 589 testSamples = new uint32_t[numTestSamples];
juansal12 0:c3a329a5b05d 590 testSampleIndex = 0;
juansal12 0:c3a329a5b05d 591
juansal12 0:c3a329a5b05d 592 // Adjust overall amplitude and offset - make sure it's nonnegative
juansal12 0:c3a329a5b05d 593 float amplitude = 1; // volts
juansal12 0:c3a329a5b05d 594 float baseline = 1.5; // volts
juansal12 0:c3a329a5b05d 595 // Print out the frequencies being used
juansal12 0:c3a329a5b05d 596 float frequencies[] = sumSampleFrequencies; // Test signal will be a summation of cosines at these frequencies (in Hz)
juansal12 0:c3a329a5b05d 597 printf("Using samples from cosines with the following frequencies:\n");
juansal12 0:c3a329a5b05d 598 for(int f = 0; f < sizeof(frequencies)/sizeof(float); f++)
juansal12 0:c3a329a5b05d 599 printf(" %f\n", frequencies[f]);
juansal12 0:c3a329a5b05d 600 printf("\n");
juansal12 0:c3a329a5b05d 601
juansal12 0:c3a329a5b05d 602 // Create the samples
juansal12 0:c3a329a5b05d 603 float sampleFrequency = 1000.0/((double)sampleInterval/1000.0);
juansal12 0:c3a329a5b05d 604 for(uint16_t n = 0; n < numTestSamples; n++)
juansal12 0:c3a329a5b05d 605 {
juansal12 0:c3a329a5b05d 606 float nextSample = 0;
juansal12 0:c3a329a5b05d 607 // Sum the frequencies
juansal12 0:c3a329a5b05d 608 for(int f = 0; f < sizeof(frequencies)/sizeof(float); f++)
juansal12 0:c3a329a5b05d 609 nextSample += cos(2.0*PI*frequencies[f]*(float)n/sampleFrequency);
juansal12 0:c3a329a5b05d 610 // Normalize
juansal12 0:c3a329a5b05d 611 nextSample /= (float)(sizeof(frequencies)/sizeof(float));
juansal12 0:c3a329a5b05d 612 // Amplify
juansal12 0:c3a329a5b05d 613 nextSample *= amplitude;
juansal12 0:c3a329a5b05d 614 // Positivify
juansal12 0:c3a329a5b05d 615 nextSample += baseline;
juansal12 0:c3a329a5b05d 616 // Convert to 12-bit ADC reading
juansal12 0:c3a329a5b05d 617 testSamples[n] = ((uint32_t)(nextSample/3.3*4095.0));
juansal12 0:c3a329a5b05d 618 // Shift it by 4 to mimic what the DMA would write for a reading (lower 4 would indicate ADC channel)
juansal12 0:c3a329a5b05d 619 testSamples[n] = testSamples[n] << 4;
juansal12 0:c3a329a5b05d 620 }
juansal12 0:c3a329a5b05d 621 sampleIndex = 0;
juansal12 0:c3a329a5b05d 622 #endif // which sample mode
juansal12 0:c3a329a5b05d 623 }
juansal12 0:c3a329a5b05d 624 #endif // artificialSamplesMode
juansal12 0:c3a329a5b05d 625
juansal12 0:c3a329a5b05d 626 #endif // #ifdef acousticControl