AcousticController.cpp

00001 /*
00002  * Author: Joseph DelPreto
00003  */
00004 
00005 #include "AcousticController.h"
00006 
00007 #ifdef acousticControl
00008 
00009 // The static instance
00010 AcousticController acousticController;
00011 
00012 // Map received state to fish values
00013 const float pitchLookupAcoustic[] = {0.2, 0.3, 0.4, 0.5, 0.6, 0.7, 0.8}; // [0.2 - 0.8]
00014 const float yawLookupAcoustic[] = {-1, -0.7, -0.5, 0, 0.5, 0.7, 1}; // [-1, 1]
00015 const float thrustLookupAcoustic[] = {0, 0.25, 0.50, 0.75};
00016 const float frequencyLookupAcoustic[] = {0.0000009, 0.0000012, 0.0000014, 0.0000016}; // cycles/us // NOTE also update periodHalfLookup if you update these values
00017 const float periodHalfLookupAcoustic[] = {555555, 416666, 357142, 312500}; // 1/(2*frequencyLookup) -> us
00018 
00019 // AGC definition
00020 const uint8_t agcGains[] = {1, 1, 2, 5, 10, 20, 50, 100};  // the possible gains of the AGC
00021 const uint8_t agcGainsLength = sizeof(agcGains)/sizeof(agcGains[0]);
00022 const uint16_t signalLevelBufferLength = (agcUpdateWindow)/(sampleWindow*sampleInterval);         // first constant is window length for updating agc in microseconds
00023 const int32_t signalLevelSumDesired = ((uint32_t)500)*((uint32_t)signalLevelBufferLength); // desire about 2 VPP, so signal minus offset should have 1V amplitude, so say mean a bit smaller, about 0.4V ~= 500
00024 
00025 // Fish timeout
00026 const uint16_t fishTimeoutAcousticBuffers = fishTimeoutAcousticWindow/(sampleWindow*sampleInterval); // first constant is timeout in microseconds
00027 
00028 // Apparently it's hard to get a proper function pointer to a member function
00029 // so this dummy method will be set as the toneDetector callback function
00030 void toneDetectorCallback(int32_t* newTonePowers, uint32_t signalLevel)
00031 {
00032     acousticController.processTonePowers(newTonePowers, signalLevel);
00033 }
00034 
00035 // Constructor
00036 AcousticController::AcousticController(Serial* usbSerialObject /* = NULL */) :
00037     // Initialize variables
00038     bufferCount(0),
00039     lastDataWord(0),
00040     timeSinceGoodWord(0),
00041     streamCurFishStateAcoustic(0),
00042     streamCurFishStateEventAcoustic(0)
00043 {
00044     // AGC Pins
00045     gain0 = new DigitalOut(agcPin0);
00046     gain1 = new DigitalOut(agcPin1);
00047     gain2 = new DigitalOut(agcPin2);
00048     agc[0] = gain0;
00049     agc[1] = gain1;
00050     agc[2] = gain2;
00051     #ifdef AGCLeds
00052     agcLED0 = new DigitalOut(LED1);
00053     agcLED1 = new DigitalOut(LED2);
00054     agcLED2 = new DigitalOut(LED3);
00055     agcLEDs[0] = agcLED0;
00056     agcLEDs[1] = agcLED1;
00057     agcLEDs[2] = agcLED2;
00058     #endif
00059 
00060     // Low battery
00061     lowBatteryVoltageInput = new DigitalIn(lowBatteryVoltagePin);
00062 
00063     // Misc
00064     #ifdef artificialPowers
00065     FILE* finPowers; = fopen("/local/powers.wp", "r");
00066     #endif
00067 
00068     // Complete initialization
00069     init(usbSerialObject);
00070 }
00071 
00072 // Initialization
00073 void AcousticController::init(Serial* usbSerialObject /* = NULL */)
00074 {
00075     // Create usb serial object or use provided one
00076     if(usbSerialObject == NULL)
00077     {
00078         usbSerialObject = new Serial(USBTX, USBRX);
00079         usbSerialObject->baud(serialDefaultBaudUSB);
00080     }
00081     usbSerial = usbSerialObject;
00082     // Miscellaneous
00083     bufferCount = 0;
00084     lastDataWord = 0;
00085     timeSinceGoodWord = 0;
00086 
00087     // Will check if battery is low in every buffer callback
00088     lowBatteryVoltageInput->mode(PullUp);
00089     //lowBatteryInterrupt.fall(&lowBatteryCallback);
00090 
00091     // TODO remove this?
00092     wait(1.5);
00093 
00094     // Configure the tone detector
00095     toneDetector.setCallback(&toneDetectorCallback);
00096     toneDetector.init();
00097 
00098     // Clear detection arrays
00099     #if defined(threshold2)
00100     for(int t = 0; t < numTones; t++)
00101     {
00102         detectSums[t] = 0;
00103         for(uint8_t p = 0; p < detectWindow; p++)
00104             tonesPresent[p][t] = 0;
00105     }
00106     detectWindowIndex = 0;
00107     readyToThreshold = false;
00108     powerHistoryIndex = 0;
00109     powerHistoryDetectIndex = (powerHistoryLength - 1) - (powerHistoryDetectWindow-1) + 1; // assumes powerHistoryLength >= detectWindow > 1
00110     for(uint8_t t = 0; t < numTones; t++)
00111     {
00112         powerHistorySumDetect[t] = 0;
00113         powerHistorySumNoDetect[t] = 0;
00114         powerHistoryMaxDetect[t] = 0;
00115         powerHistoryMaxNoDetect[t] = 0;
00116         for(uint16_t p = 0; p < powerHistoryLength; p++)
00117         {
00118             powerHistory[p][t] = 0;
00119         }
00120     }
00121     #elif defined(threshold1)
00122     for(int t = 0; t < numTones; t++)
00123     {
00124         detectSums[t] = 0;
00125     }
00126     powerHistoryIndex = 0;
00127     powerHistoryDetectIndex = (powerHistoryLength - 1) - (detectWindow-1) + 1; // assumes powerHistoryLength >= detectWindow > 1
00128     for(uint8_t t = 0; t < numTones; t++)
00129     {
00130         powerHistorySum[t] = 0;
00131         powerHistoryMax[t] = 0;
00132         for(uint16_t p = 0; p < powerHistoryLength; p++)
00133         {
00134             powerHistory[p][t] = 0;
00135         }
00136     }
00137     readyToThreshold = false;
00138     #endif
00139     #ifdef singleDataStream
00140         #ifdef saveData
00141         dataIndex = 0;
00142         #endif
00143     #else
00144         #ifdef saveData
00145         dataWordIndex = 0;
00146         #endif
00147         dataBitIndex = 0;
00148         interWord = false;
00149     #endif
00150     waitingForEnd = false;
00151     periodIndex = 0;
00152     fskIndex = 0;
00153 
00154     // Initialize adjustable gain control
00155     signalLevelBufferIndex = 0;
00156     signalLevelSum = 0;
00157     currentGainIndex = 4;
00158     for(uint8_t i = 0; i < sizeof(agc)/sizeof(agc[0]); i++)
00159     {
00160         agc[i]->write(currentGainIndex & (1 << i));
00161         #ifdef AGCLeds
00162         agcLEDs[i].write(currentGainIndex & (1 << i));
00163         #endif
00164     }
00165 }
00166 
00167 
00168 // Called by toneDetector when new tone powers are computed (once per buffer)
00169 void AcousticController::processTonePowers(int32_t* newTonePowers, uint32_t signalLevel)
00170 {
00171     #ifdef streamAcousticControlLog
00172     acousticControlLogToStream[4] = -10;
00173     #endif
00174     // Check for low battery and if so terminate tone detector
00175     if(lowBatteryVoltageInput == 0)
00176     {
00177         lowBatteryCallback();
00178         return;
00179     }
00180     // See if we're done and if so terminate the tone detector
00181     if(bufferCount == loopCount)
00182     {
00183         #ifndef infiniteLoopAcoustic
00184         toneDetector.stop();
00185         return;
00186         #else
00187         //bufferCount = 0;
00188         #endif
00189     }
00190     bufferCount++;
00191     // See if we're in autonomous mode and if so just let it be
00192     if(fishController.autoMode)
00193         return;
00194 
00195     periodIndex++;
00196     timeSinceGoodWord++;
00197 
00198     #if defined(threshold2)
00199     // Update threshold window state for each tone
00200     for(uint8_t t = 0; t < numTones; t++)
00201     {
00202         // Update our history of powers for this tone
00203         powerHistory[powerHistoryIndex][t] = (newTonePowers[t] >> 5);
00204         // Compute max/sum of the current window (only for frequencies we currently anticipate)
00205         if((t == fskIndex*2 || t == fskIndex*2+1) && !waitingForEnd)
00206         {
00207             powerHistoryMaxDetect[t] = 0;
00208             powerHistoryMaxNoDetect[t] = 0;
00209             powerHistorySumDetect[t] = 0;
00210             powerHistorySumNoDetect[t] = 0;
00211             // Look at non-detection zone
00212             for(uint16_t p = ((powerHistoryIndex+1)%powerHistoryLength); p != powerHistoryDetectIndex; p=(p+1)%powerHistoryLength)
00213             {
00214                 powerHistorySumNoDetect[t] += powerHistory[p][t];
00215                 if(powerHistory[p][t] > powerHistoryMaxNoDetect[t])
00216                     powerHistoryMaxNoDetect[t] = powerHistory[p][t];
00217             }
00218             // Look at detection zone
00219             for(uint16_t p = powerHistoryDetectIndex; p != ((powerHistoryIndex+1)%powerHistoryLength); p=((p+1)%powerHistoryLength))
00220             {
00221                 powerHistorySumDetect[t] += powerHistory[p][t];
00222                 if(powerHistory[p][t] > powerHistoryMaxDetect[t])
00223                     powerHistoryMaxDetect[t] = powerHistory[p][t];
00224             }
00225         }
00226     }
00227     // Advance our power history index (circular buffer)
00228     powerHistoryIndex = (powerHistoryIndex+1) % powerHistoryLength;
00229     powerHistoryDetectIndex = (powerHistoryDetectIndex+1) % powerHistoryLength;
00230     readyToThreshold = readyToThreshold || (powerHistoryIndex == 0);
00231     // If not waiting out silence until next pulse is expected, see if a tone is present
00232     if(!waitingForEnd && readyToThreshold)
00233     {
00234         // Based on new max/mean, see how many powers indicate a tone present in the last detectWindow readings
00235         for(uint8_t t = fskIndex*2; t <= fskIndex*2+1; t++)
00236         {
00237             detectSums[t] -= tonesPresent[detectWindowIndex][t];
00238             if(
00239                    ((powerHistorySumDetect[t] << 2) > powerHistorySumNoDetect[t])
00240                 && (powerHistoryMaxDetect[t] > powerHistoryMaxNoDetect[t])
00241                 && ((powerHistorySumDetect[t] - (powerHistorySumDetect[t] >> 3)) > powerHistorySumDetect[fskIndex*2+((t+1)%2)])
00242                 && ((powerHistorySumDetect[t] << 2) > powerHistorySumNoDetect[fskIndex*2+((t+1)%2)])
00243                 && ((powerHistoryMaxDetect[t] << 4) > powerHistorySumNoDetect[t])
00244                 && (powerHistoryMaxDetect[t] > 100000)
00245                 )
00246                 tonesPresent[detectWindowIndex][t] = 1;
00247             else
00248                 tonesPresent[detectWindowIndex][t] = 0;
00249             detectSums[t] += tonesPresent[detectWindowIndex][t];
00250         }
00251         detectWindowIndex = (detectWindowIndex+1) % detectWindow;
00252         // If both are considered present (should be very rare?), choose the one with a higher mean
00253         if(detectSums[fskIndex*2] > detectThresh && detectSums[fskIndex*2+1] > detectThresh)
00254         {
00255             if(powerHistorySumDetect[fskIndex*2] > powerHistorySumDetect[fskIndex*2+1])
00256                 detectSums[fskIndex*2+1] = 0;
00257             else
00258                 detectSums[fskIndex*2] = 0;
00259         }
00260         // See if a tone is present
00261         int tonePresent = -1;
00262         if(detectSums[fskIndex*2] > detectThresh)
00263             tonePresent = fskIndex*2;
00264         else if(detectSums[fskIndex*2+1] > detectThresh)
00265             tonePresent = fskIndex*2+1;
00266         // Record data and update state
00267         if(tonePresent > -1)
00268         {
00269             #ifdef singleDataStream // if we just want a stream of bits instead of segmenting into words
00270                 #ifdef saveData
00271                 data[dataIndex++] = tonePresent%2;
00272                 #endif
00273                 #ifdef streamData
00274                 usbSerial->printf("%ld\t%d\n", bufferCount, (bool)(tonePresent%2));
00275                 #endif
00276                 periodIndex = detectSums[tonePresent];
00277                 fskIndex = (fskIndex+1) % numFSKGroups;
00278             #else
00279             // See if it has been a long time since last pulse
00280             if(periodIndex >= interWordWait)
00281             {
00282                 // If we currently think that we're between words, then that's good so process the previous data word
00283                 // If we don't think we're between words, then we missed a bit so discard whatever we've collected in the word so far
00284                 // Either way, this is the start of a new word so reset dataBitIndex
00285                 if(interWord && dataBitIndex == dataWordLength)
00286                 {
00287                     timeSinceGoodWord = 0;
00288                     streamCurFishStateEventAcoustic = 5;
00289                     // Decode last word and then send it out
00290                     #ifdef saveData
00291                     decodeAcousticWord(data[dataWordIndex]);
00292                     dataWordIndex++;
00293                     #else
00294                     decodeAcousticWord(dataWord);
00295                     #endif
00296                     dataBitIndex = 0;
00297                     interWord = false;
00298                 }
00299                 else if(!interWord) // missed a bit
00300                 {
00301                     #ifdef streamData
00302                     usbSerial->printf("%ld\t0\t-1\n", bufferCount);
00303                     #endif
00304                     #ifdef saveData
00305                     for(int dbi = 0; dbi < dataWordLength; dbi++)
00306                         data[dataWordIndex][dbi] = 0;
00307                     data[dataWordIndex][dataWordLength-1] = 1;
00308                     dataWordIndex++;
00309                     #endif
00310                     #ifdef streamAcousticControlLog
00311                     acousticControlLogToStream[4] = -1;
00312                     streamCurFishStateEventAcoustic = 1;
00313                     #endif
00314                     lastDataWord = 0;
00315                 }
00316                 // TODO this is a debug check - if transmitter not putting space between words, set interWordWait to 1
00317                 if(interWordWait > 1)
00318                 {
00319                     dataBitIndex = 0;
00320                     interWord = false;
00321                 }
00322             }
00323             else if(interWord)
00324             {
00325                 // It has not been a long time since the last pulse, yet we thought it should be
00326                 // Seems like we erroneously detected a bit that shouldn't have existed
00327                 // Discard current word as garbage and start again
00328                 dataBitIndex = 0;
00329                 #ifdef streamData
00330                 usbSerial->printf("%ld\t0\t-2\n", bufferCount);
00331                 #endif
00332                 #ifdef saveData
00333                 for(int dbi = 0; dbi < dataWordLength; dbi++)
00334                     data[dataWordIndex][dbi] = 0;
00335                 data[dataWordIndex][dataWordLength-2] = 1;
00336                 dataWordIndex++;
00337                 #endif
00338                 #ifdef streamAcousticControlLog
00339                 acousticControlLogToStream[4] = -2;
00340                 streamCurFishStateEventAcoustic = 2;
00341                 #endif
00342                 lastDataWord = 0;
00343             }
00344             // If we're not between words (either normally or because we were just reset above), store the new bit
00345             if(!interWord)
00346             {
00347                 #ifdef saveData
00348                 data[dataWordIndex][dataBitIndex++] = tonePresent%2;
00349                 #else
00350                 dataWord[dataBitIndex++] = tonePresent%2;
00351                 #endif
00352                 // Rotate through which FSK frequency we expect for the next pulse
00353                 fskIndex = (fskIndex+1) % numFSKGroups;
00354                 // If we've finished a word, say we're waiting between words
00355                 // Word won't be processed until next word begins though in case we accidentally detected a nonexistent bit (see logic above)
00356                 if(dataBitIndex == dataWordLength)
00357                 {
00358                     interWord = true;
00359                     fskIndex = 0;
00360                 }
00361             }
00362             periodIndex = detectSums[tonePresent];  // Use number of detected points rather than 0 to get it closer to actual rising edge
00363             #endif
00364             // Wait out reflections until next pulse
00365             waitingForEnd = true;
00366         }
00367     }
00368     else if(periodIndex > period) // done waiting for next bit (waiting out reflections)
00369     {
00370         waitingForEnd = false;
00371         // Reset the sums and indicators
00372         for(uint8_t t = 0; t < numTones; t++)
00373         {
00374             detectSums[t] = 0;
00375             for(uint8_t p = 0; p < detectWindow; p++)
00376                 tonesPresent[p][t] = 0;
00377         }
00378     }
00379     #elif defined(threshold1)
00380     // Update threshold window state for each tone
00381     for(uint8_t t = 0; t < numTones; t++)
00382     {
00383         // Update our history of powers for this tone
00384         powerHistorySum[t] -= powerHistory[powerHistoryIndex][t];
00385         powerHistory[powerHistoryIndex][t] = newTonePowers[t];
00386         powerHistorySum[t] += newTonePowers[t];
00387         // Compute max of the current window (only for frequencies we currently anticipate)
00388         if((t == fskIndex*2 || t == fskIndex*2+1) && !waitingForEnd)
00389         {
00390             powerHistoryMax[t] = 0;
00391             for(uint16_t p = 0; p < powerHistoryLength; p++)
00392             {
00393                 if(powerHistory[p][t] > powerHistoryMax[t])
00394                     powerHistoryMax[t] = powerHistory[p][t];
00395             }
00396         }
00397     }
00398     // Advance our power history index (circular buffer)
00399     powerHistoryIndex = (powerHistoryIndex+1) % powerHistoryLength;
00400     readyToThreshold = readyToThreshold || (powerHistoryIndex == 0);
00401     // If not waiting until next pulse is expected, see if a tone is present
00402     if(!waitingForEnd && readyToThreshold)
00403     {
00404         // Based on new max/mean, see how many powers indicate a tone present in the last detectWindow readings
00405         for(uint8_t t = fskIndex*2; t <= fskIndex*2+1; t++)
00406         {
00407             detectSums[t] = 0;
00408             for(uint16_t p = powerHistoryDetectIndex; p != powerHistoryIndex; p = (p+1) % powerHistoryLength)
00409             {
00410                 if((powerHistory[p][t] > (powerHistorySum[fskIndex*2+((t+1)%2)] >> 2)) // power greater than 12.5 times mean of other channel
00411                     && (powerHistory[p][t] > (powerHistoryMax[t] >> 2))                // power greater than 1/4 of the max on this channel
00412                     && (powerHistory[p][t] > 1000)                                     // power greater than a fixed threshold
00413                     && powerHistoryMax[t] > (powerHistorySum[t] >> 3))                 // max on this channel is 6.25 times greater than the mean on this channel (in case this channel noise is just must higher than other channel)
00414                     detectSums[t]++;
00415             }
00416         }
00417         // If both are considered present (should be very rare?), choose the one with a higher mean
00418         if(detectSums[fskIndex*2] > detectThresh && detectSums[fskIndex*2+1] > detectThresh)
00419         {
00420             if(powerHistorySum[fskIndex*2] > powerHistorySum[fskIndex*2+1])
00421                 detectSums[fskIndex*2+1] = 0;
00422             else
00423                 detectSums[fskIndex*2] = 0;
00424         }
00425         // See if a tone is present
00426         int tonePresent = -1;
00427         if(detectSums[fskIndex*2] > detectThresh)
00428             tonePresent = fskIndex*2;
00429         else if(detectSums[fskIndex*2+1] > detectThresh)
00430             tonePresent = fskIndex*2+1;
00431         // Record data and update state
00432         if(tonePresent > -1)
00433         {
00434             #ifdef singleDataStream
00435                 #ifdef saveData
00436                 data[dataIndex++] = tonePresent%2;
00437                 #endif
00438                 #ifdef streamData
00439                 usbSerial->printf("%d", (bool)(tonePresent%2));
00440                 #endif
00441                 periodIndex = detectSums[tonePresent];
00442                 fskIndex = (fskIndex+1) % numFSKGroups;
00443             #else
00444             // See if it has been a long time since last pulse
00445             if(periodIndex >= interWordWait)
00446             {
00447                 // If we currently think that we're between words, then that's good so process the previous data word
00448                 // If we don't think we're between words, then we missed a bit so discard whatever we've collected in the word so far
00449                 // Either way, this is the start of a new word so reset dataBitIndex
00450                 if(interWord && dataBitIndex == dataWordLength)
00451                 {
00452                     // Decode last word and then send it out
00453                     #ifdef saveData
00454                     decodeAcousticWord(data[dataWordIndex]);
00455                     dataWordIndex++;
00456                     #else
00457                     decodeAcousticWord(dataWord);
00458                     #endif
00459                     dataBitIndex = 0;
00460                     interWord = false;
00461 
00462                     timeSinceGoodWord = 0;
00463                 }
00464                 else if(interWord)
00465                 {
00466                     #ifdef streamData
00467                     usbSerial->printf("0\t-1\n");
00468                     #endif
00469                     lastDataWord = 0;
00470                 }
00471                 // TODO this is a debug check - if transmitter not putting space between words, set interWordWait to 1
00472                 if(interWordWait > 1)
00473                 {
00474                     dataBitIndex = 0;
00475                     interWord = false;
00476                 }
00477             }
00478             else if(interWord)
00479             {
00480                 // It has not been a long time since the last pulse, yet we thought it should be
00481                 // Seems like we erroneously detected a bit that shouldn't have existed
00482                 // Discard current word as garbage and start again
00483                 dataBitIndex = 0;
00484                 #ifdef streamData
00485                 usbSerial->printf("0\t-2\n");
00486                 #endif
00487                 lastDataWord = 0;
00488             }
00489             // If we're not between words (either normally or because we were just reset above), store the new bit
00490             if(!interWord)
00491             {
00492                 #ifdef saveData
00493                 data[dataWordIndex][dataBitIndex++] = tonePresent%2;
00494                 #else
00495                 dataWord[dataBitIndex++] = tonePresent%2;
00496                 #endif
00497                 fskIndex = (fskIndex+1) % numFSKGroups;
00498                 periodIndex = detectSums[tonePresent];  // Use number of detected points rather than 0 to get it closer to actual rising edge
00499                 // If we've finished a word, say we're waiting between words
00500                 // Word won't be processed until next word begins though in case we accidentally detected a nonexistent bit (see logic above)
00501                 if(dataBitIndex == dataWordLength)
00502                 {
00503                     interWord = true;
00504                     fskIndex = 0;
00505                 }
00506             }
00507             #endif
00508             // Wait out reflections until next pulse
00509             waitingForEnd = true;
00510         }
00511     }
00512     else if(periodIndex > period) // done waiting for next bit (waiting out reflections)?
00513     {
00514         waitingForEnd = false;
00515     }
00516     // Advance our power history start detect index (circular buffer) to stay detectWindow elements behind main index
00517     powerHistoryDetectIndex = (powerHistoryDetectIndex+1) % powerHistoryLength;
00518     #endif // end switch between threshold1 or threshold2
00519 
00520     // Update signal level history
00521     signalLevelSum += signalLevel;
00522     signalLevelBufferIndex = (signalLevelBufferIndex+1) % signalLevelBufferLength;
00523     #ifdef streamSignalLevel
00524     //usbSerial->printf("%ld\t%d\t%d\n", signalLevel, currentGainIndex, lastDataWord);
00525     usbSerial->printf("%ld\t%d\n", signalLevel, currentGainIndex);
00526     #endif
00527     #ifdef streamAcousticControlLog
00528     acousticControlLogToStream[3] = currentGainIndex;
00529     #endif
00530     // If have seen enough readings, choose a new gain
00531     if(signalLevelBufferIndex == 0)
00532     {
00533         // Calculate ideal gain for signalLevelSum to become signalLevelSumDesired
00534         int32_t newGain = ((int64_t)signalLevelSumDesired * (int64_t)agcGains[currentGainIndex]) / (int64_t)signalLevelSum;
00535         // See which available gain is closest
00536         uint8_t bestIndex = currentGainIndex;
00537         int32_t minDiff = 10000;
00538         for(uint8_t g = 0; g < agcGainsLength; g++)
00539         {
00540             int32_t diff = (int32_t)agcGains[g] - newGain;
00541             if(diff > 0 && diff < minDiff)
00542             {
00543                 minDiff = diff;
00544                 bestIndex = g;
00545             }
00546             else if(diff < 0 && -1*diff < minDiff)
00547             {
00548                 minDiff = -1*diff;
00549                 bestIndex = g;
00550             }
00551         }
00552         // Set the gain
00553         currentGainIndex = bestIndex;
00554         for(uint8_t i = 0; i < 3; i++)
00555         {
00556             #ifdef useAGC
00557             agc[i]->write(currentGainIndex & (1 << i));
00558             #endif
00559             #ifdef AGCLeds
00560             agcLEDs[i].write(currentGainIndex & (1 << i));
00561             #endif
00562         }
00563         // Reset sum
00564         signalLevelSum = 0;
00565     }
00566 
00567     #ifdef artificialPowers
00568     usbSerial->printf("%ld \t%ld \t%ld \t%d \t%ld \t%ld \t%ld \t%ld \t%d \t%d \t%d \n", newTonePowers[0], newTonePowers[1], detectSums[0], detectSums[1], powerHistorySum[0], powerHistorySum[1], powerHistoryMax[0], powerHistoryMax[1], fskIndex, waitingForEnd, periodIndex);
00569     #endif
00570 
00571     // Check for timeout
00572     #ifdef fishTimeoutAcoustic
00573     if(timeSinceGoodWord >= fishTimeoutAcousticBuffers)
00574     {
00575         #ifndef singleDataStream
00576         if(timeSinceGoodWord == fishTimeoutAcousticBuffers)
00577         {
00578             bool neutralWord[] = {0, 0, 0, 0, 1, 1, 0, 0, 1, 1, 0, 0, 0, 0, 0, 0}; // Decodes to state 54, which is neutral state
00579             decodeAcousticWord(neutralWord);
00580             streamCurFishStateEventAcoustic = 4;
00581         }
00582         #endif
00583         timeSinceGoodWord = fishTimeoutAcousticBuffers+1; // make sure we won't constantly send a neutral command (only timeout once)
00584     }
00585     #endif
00586 
00587     // Stream data
00588     #ifdef streamAcousticControlLog
00589     //usbSerial->printf("%ld %ld %ld %ld %ld %ld\n", bufferCount-1, acousticControlLogToStream[0], acousticControlLogToStream[1], acousticControlLogToStream[2], acousticControlLogToStream[3], acousticControlLogToStream[4]);
00590     //usbSerial->printf("%ld %ld %ld\n", acousticControlLogToStream[0], acousticControlLogToStream[1], acousticControlLogToStream[2]);
00591     //usbSerial->printf("%ld %ld %ld %ld %ld\n", acousticControlLogToStream[0], acousticControlLogToStream[1], acousticControlLogToStream[2], acousticControlLogToStream[3], acousticControlLogToStream[4]);
00592     //FunctionPointer fp = &tempMethod;
00593     //acousticControlLogToStream[0] = 6100;
00594     //acousticControlLogToStream[1] = 62;
00595     //acousticControlLogToStream[2] = 63;
00596     //acousticControlLogToStream[3] = 64;
00597     //acousticControlLogToStream[4] = -10;
00598     uint32_t streamGoertzel1 = acousticControlLogToStream[0];// >> 8; // second Fiji day included >> 8 to reduce bits
00599     uint32_t streamGoertzel2 = acousticControlLogToStream[1];// >> 8; // second Fiji day included >> 8 to reduce bits
00600     uint16_t streamSignalLevel = ((acousticControlLogToStream[2] >> 2) & ((uint16_t)2047)) + (((uint16_t)21) << 11); // first Fiji day was just acousticControlLogToStream[2], second day included code word
00601     uint8_t streamGain = acousticControlLogToStream[3] + 168; // only used in first day
00602     //int16_t streamWord = acousticControlLogToStream[4];
00603     // Decide the event to transmit, giving priority to acoustic events over button board events
00604     uint16_t streamFishStateEvent = fishController.streamFishStateEventController;
00605     if(streamCurFishStateEventAcoustic > 0)
00606         streamFishStateEvent = streamCurFishStateEventAcoustic;
00607     uint16_t streamWord = (uint16_t)streamCurFishStateAcoustic | ((uint16_t)streamFishStateEvent << 11); // same for both Fiji days
00608 
00609     uint8_t* bufferOut = (uint8_t*) (&streamGoertzel1);
00610     usbSerial->putc(bufferOut[0]);
00611     usbSerial->putc(bufferOut[1]);
00612     usbSerial->putc(bufferOut[2]);
00613     usbSerial->putc(bufferOut[3]); // commented out for second day
00614 
00615     bufferOut = (uint8_t*) (&streamGoertzel2);
00616     usbSerial->putc(bufferOut[0]);
00617     usbSerial->putc(bufferOut[1]);
00618     usbSerial->putc(bufferOut[2]);
00619     usbSerial->putc(bufferOut[3]); // commented out for second day
00620 
00621     bufferOut = (uint8_t*) (&streamSignalLevel);
00622     usbSerial->putc(bufferOut[0]);
00623     usbSerial->putc(bufferOut[1]);
00624 
00625     bufferOut = (uint8_t*) (&streamGain);
00626     usbSerial->putc(bufferOut[0]); // commented out for second day
00627 
00628     bufferOut = (uint8_t*) (&streamWord);
00629     usbSerial->putc(bufferOut[0]);
00630     usbSerial->putc(bufferOut[1]);
00631     //usbSerial->printf("%d %d\n", streamCurFishState, streamWord);
00632 
00633     /*uint8_t* bufferOut = (uint8_t*) acousticControlLogToStream;
00634     for(uint8_t i = 0; i < 20; i++)
00635         usbSerial->putc(bufferOut[i]);*/
00636 
00637     // Reset the events so we don't constantly print them
00638     // TODO remove this so we do constantly print them, and only look for changes in the column in case some are dropped?
00639     streamCurFishStateEventAcoustic = 0;
00640     fishController.streamFishStateEventController = 0;
00641     #endif
00642 }
00643 
00644 
00645 #ifndef singleDataStream
00646 // Use Hamming code to decode the data word and check for an error
00647 // TODO make constants/defines for magic numbers below that indicate length of hamming code
00648 void AcousticController::decodeAcousticWord(volatile bool* data)
00649 {
00650     // Note that the below code is written such that "5" (number of parity bits) can be a variable / #define, but it's just not yet
00651     // Check whether each parity bit is correct
00652     bool paritySums[5] = {0};
00653     for(uint8_t b = 0; b < dataWordLength-1; b++)
00654     {
00655         for(uint8_t p = 0; p < 5; p++)
00656         {
00657             if(((b+1) & (1 << p)) > 0 || p == 5-1) // check if bit belongs to parity set (overall parity at end includes everything)
00658                 paritySums[p] ^= data[b];
00659         }
00660     }
00661     paritySums[5-1] ^= data[dataWordLength-1]; // overall parity bit
00662     // See if we have an error to correct
00663     uint8_t errorBit = 0;
00664     uint8_t numParityErrors = 0;
00665     for(uint8_t p = 0; p < 5-1; p++)
00666     {
00667         if(paritySums[p])
00668         {
00669             errorBit += (1 << p);
00670             numParityErrors++;
00671         }
00672     }
00673     // Flip the erroneous bit if applicable
00674     if(numParityErrors > 1)
00675         data[errorBit-1] = !data[errorBit-1];
00676     // If using final parity bit, check that we don't detect an uncorrectable error
00677     if(!(numParityErrors > 0 && !paritySums[5-1]))
00678     {
00679         uint16_t word = 0;
00680         uint8_t i = 0;
00681         uint8_t nextParity = 1;
00682         for(uint8_t b = 0; b < dataWordLength; b++)
00683         {
00684             if(b == nextParity-1)
00685                 nextParity <<= 1;
00686             else
00687                 word |=  data[b] << i++;
00688         }
00689         // Update the fish
00690         processAcousticWord(word);
00691         lastDataWord = word;
00692         #ifdef streamData
00693         if(timeSinceGoodWord >= fishTimeoutAcousticBuffers)
00694             usbSerial->printf("%ld\t%d\t-4\n", bufferCount, (int)word);
00695         else
00696             usbSerial->printf("%ld\t%d\t1\n", bufferCount, (int)word);
00697         #endif
00698         #ifdef streamAcousticControlLog
00699         if(timeSinceGoodWord >= fishTimeoutAcousticBuffers) // check if we're timed out, since we may have reached this method when the timeout checker called us to reset the fish
00700             acousticControlLogToStream[4] = -4;
00701         else
00702             acousticControlLogToStream[4] = word;
00703         #endif
00704     }
00705     else // there was an uncorrectable error
00706     {
00707         #ifdef streamData
00708         uint16_t word = 0;
00709         for(int i = 0; i < 16; i++)
00710             word += data[i] << i;
00711         usbSerial->printf("%ld\t%d\t-3\n", bufferCount, word);
00712         #endif
00713         #ifdef streamAcousticControlLog
00714         acousticControlLogToStream[4] = -3;
00715         streamCurFishStateEventAcoustic = 3;
00716         #endif
00717         lastDataWord = 0;
00718     }
00719 }
00720 
00721 void AcousticController::processAcousticWord(uint16_t word)
00722 {
00723     // Extract state from word
00724     newSelectButtonIndex = getSelectIndexAcoustic(word);
00725     newPitchIndex = getPitchIndexAcoustic(word);
00726     newYawIndex = getYawIndexAcoustic(word);
00727     newThrustIndex = getThrustIndexAcoustic(word);
00728     newFrequencyIndex = getFrequencyIndexAcoustic(word);
00729     // Log it
00730     streamCurFishStateAcoustic = getCurStateWordAcoustic;
00731     // Set the states
00732     #ifdef acousticControllerControlFish
00733     fishController.setSelectButton(newSelectButtonIndex > 0);
00734     fishController.setPitch(pitchLookupAcoustic[newPitchIndex]);
00735     fishController.setYaw(yawLookupAcoustic[newYawIndex]);
00736     fishController.setThrust(thrustLookupAcoustic[newThrustIndex]);
00737     fishController.setFrequency(frequencyLookupAcoustic[newFrequencyIndex], periodHalfLookupAcoustic[newFrequencyIndex]);
00738     #endif
00739 }
00740 #endif
00741 
00742 // Stop the AcousticController
00743 // Will stop the tone detector and will stop the fishController
00744 // Note that the acoustic controller's run() method is blocking, so if you
00745 //  want to use this stop method you should launch run() in a separate thread
00746 void AcousticController::stop()
00747 {
00748     // Stop the tone detector
00749     // This will end the toneDetector's run() method at the next buffer
00750     // which will cause our run() method to advance and finish up
00751     toneDetector.stop();
00752 }
00753 
00754 // Main loop
00755 // Is blocking, so won't return until loopCount is reached or another thread calls stop()
00756 void AcousticController::run()
00757 {
00758     // Create file for logging data words
00759     #ifdef saveData
00760     int fileNum = -1;
00761     char filename[25];
00762     foutDataWords = NULL;
00763     do
00764     {
00765         fileNum++;
00766         fclose(foutDataWords);
00767         sprintf(filename, "/local/%d.txt", fileNum);
00768         foutDataWords = fopen(filename, "r");
00769         usbSerial->printf("%d\n", fileNum);
00770     } while(foutDataWords != NULL);
00771     foutDataWords = fopen(filename, "w");
00772     #endif
00773 
00774     #ifdef acousticControllerControlFish
00775     // Start the fish controller
00776     fishController.start();
00777     #endif
00778 
00779     #ifndef artificialPowers
00780     // Start listening for tones
00781     programTimer.start();
00782     toneDetector.run();
00783     programTimer.stop();
00784     toneDetector.finish(); // we won't include the time this takes to write files in the elapsed time
00785     #else
00786     // Read powers from file
00787     usbSerial->printf("newPower[0] \tnewPower[1] \tdetectSums[0] \tdetectSums[1] \tsum[0] \tsum[1] \tmax[0] \tmax[1] \tfskIndex \twaiting \tperiodIndex \n");
00788     int32_t maxSignalValTemp = 0;
00789     int res = fscanf(finPowers, "%ld\t%ld\n", &nextPowers[0], &nextPowers[1]);
00790     while(res > 0)
00791     {
00792         processTonePowers(nextPowers, maxSignalValTemp);
00793         res = fscanf(finPowers, "%ld\t%ld\n", &nextPowers[0], &nextPowers[1]);
00794     }
00795     fclose(finPowers);
00796     #endif
00797 
00798     #ifdef acousticControllerControlFish
00799     // Stop the fish controller
00800     fishController.stop();
00801     // If battery died, wait a bit for pi to clean up and shutdown and whatnot
00802     if(lowBatteryVoltageInput == 0)
00803     {
00804         wait(90); // Give the Pi time to shutdown
00805         fishController.setLEDs(255, false);
00806     }
00807     #endif
00808 
00809     // Print results
00810     #ifdef printBufferSummary
00811     int elapsed = programTimer.read_us();
00812     usbSerial->printf("\n");
00813     usbSerial->printf("Buffers processed: %ld\n", bufferCount);
00814     usbSerial->printf("Elapsed time  : %d us\n", elapsed);
00815     usbSerial->printf("Per-sample time : %f us\n", (double)elapsed/(double)bufferCount/(double)sampleWindow);
00816     usbSerial->printf("  Sample frequency: %f kHz\n", (double)bufferCount*(double)sampleWindow/(double)elapsed*1000.0);
00817     usbSerial->printf("Per-buffer time : %f us\n", (double)elapsed/(double)bufferCount);
00818     usbSerial->printf("  Buffer-processing frequency: %f kHz\n", (double)bufferCount/(double)elapsed*1000.0);
00819 
00820     usbSerial->printf("\nComputed powers from last buffer: \n");
00821     int32_t* lastTonePowers = toneDetector.getTonePowers();
00822     for(int i = 0; i < numTones; i++)
00823         usbSerial->printf("  Tone %d: %f Hz -> %f\n", i, targetTones[i], toFloat(lastTonePowers[i]));
00824     #endif
00825     #if defined(singleDataStream) && defined(saveData)
00826     usbSerial->printf("\nData received (%d bits):\n", dataIndex);
00827     fprintf(foutDataWords, "\nData received (%d bits):\n", dataIndex);
00828     long errors = 0;
00829     for(int d = 5; d < dataIndex; d++)
00830     {
00831         usbSerial->printf("%d", data[d]);
00832         fprintf(foutDataWords, "%d", data[d]);
00833         if(d > 0 && data[d] == data[d-1])
00834             errors++;
00835     }
00836     usbSerial->printf("\n");
00837     usbSerial->printf("errors: %ld\n", errors);
00838     fprintf(foutDataWords, "\n");
00839     fprintf(foutDataWords, "errors: %ld\n", errors);
00840     fclose(foutDataWords);
00841     #ifdef debugLEDs
00842     if(errors > 0)
00843         led1 = 1;
00844     #endif
00845     #elif defined(saveData)
00846     usbSerial->printf("\nData received (%d words):\n", dataWordIndex);
00847     fprintf(foutDataWords, "\nData received (%d words):\n", dataWordIndex);
00848     long errors = 0;
00849     long badWords = 0;
00850     for(int w = 0; w < dataWordIndex; w++)
00851     {
00852         errors = 0;
00853         usbSerial->printf("  ");
00854         fprintf(foutDataWords, "  ");
00855         for(int b = 0; b < dataWordLength; b++)
00856         {
00857             usbSerial->printf("%d", data[w][b]);
00858             fprintf(foutDataWords, "%d", data[w][b]);
00859             if(b > 0 && data[w][b-1] == data[w][b])
00860                 errors++;
00861         }
00862         if(errors > 0)
00863         {
00864             usbSerial->printf(" X");
00865             fprintf(foutDataWords, " X");
00866             badWords++;
00867         }
00868         usbSerial->printf("\n");
00869         fprintf(foutDataWords, "\n");
00870     }
00871     usbSerial->printf("\nbad words: %d\n", badWords);
00872     fprintf(foutDataWords, "\nbad words: %d\n", badWords);
00873     fclose(foutDataWords);
00874     #ifdef debugLEDs
00875     if(badWords > 0)
00876         led1 = 1;
00877     #endif
00878     #endif
00879 
00880     // TODO remove these waits?
00881     wait(1);
00882     #ifdef printBufferSummary
00883     usbSerial->printf("\nAcousticController Done!");
00884     #endif
00885     wait(3);
00886     #ifdef printBufferSummary
00887     usbSerial->printf("\n");
00888     #endif
00889 }
00890 
00891 
00892 void AcousticController::lowBatteryCallback()
00893 {
00894     // Stop the tone detector
00895     // This will end the main call to start, causing main to terminate
00896     // Main will also stop the fish controller once this method ends
00897     toneDetector.stop();
00898     // Also force the pin low to signal the Pi
00899     // (should have already been done, but just in case)
00900     // TODO check that this really forces it low after this method ends and the pin object may be deleted
00901     DigitalOut simBatteryLow(lowBatteryVoltagePin);
00902     simBatteryLow = 0;
00903 }
00904 
00905 #endif // #ifdef acousticControl