Juan Salazar
test2
Dependencies: MODDMA mbed sofi7
Revision 1:b0d4dbf4e611, committed 2021-12-03
- Comitter:
- arizonat
- Date:
- Fri Dec 03 23:09:57 2021 +0000
- Parent:
- 0:eac551d0186f
- Child:
- 2:7ca59d64e460
- Commit message:
- Deleted Acoustic Control stuff and ESC related stuff so it would compile
Changed in this revision
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/MODDMA.lib Fri Dec 03 23:09:57 2021 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/users/AjK/code/MODDMA/#97a16bf2ff43
--- a/robotic_fish_6/AcousticControl/AcousticController.cpp Fri Dec 03 23:03:20 2021 +0000
+++ /dev/null Thu Jan 01 00:00:00 1970 +0000
@@ -1,905 +0,0 @@
-/*
- * Author: Joseph DelPreto
- */
-
-#include "AcousticController.h"
-
-#ifdef acousticControl
-
-// The static instance
-AcousticController acousticController;
-
-// Map received state to fish values
-const float pitchLookupAcoustic[] = {0.2, 0.3, 0.4, 0.5, 0.6, 0.7, 0.8}; // [0.2 - 0.8]
-const float yawLookupAcoustic[] = {-1, -0.7, -0.5, 0, 0.5, 0.7, 1}; // [-1, 1]
-const float thrustLookupAcoustic[] = {0, 0.25, 0.50, 0.75};
-const float frequencyLookupAcoustic[] = {0.0000009, 0.0000012, 0.0000014, 0.0000016}; // cycles/us // NOTE also update periodHalfLookup if you update these values
-const float periodHalfLookupAcoustic[] = {555555, 416666, 357142, 312500}; // 1/(2*frequencyLookup) -> us
-
-// AGC definition
-const uint8_t agcGains[] = {1, 1, 2, 5, 10, 20, 50, 100}; // the possible gains of the AGC
-const uint8_t agcGainsLength = sizeof(agcGains)/sizeof(agcGains[0]);
-const uint16_t signalLevelBufferLength = (agcUpdateWindow)/(sampleWindow*sampleInterval); // first constant is window length for updating agc in microseconds
-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
-
-// Fish timeout
-const uint16_t fishTimeoutAcousticBuffers = fishTimeoutAcousticWindow/(sampleWindow*sampleInterval); // first constant is timeout in microseconds
-
-// Apparently it's hard to get a proper function pointer to a member function
-// so this dummy method will be set as the toneDetector callback function
-void toneDetectorCallback(int32_t* newTonePowers, uint32_t signalLevel)
-{
- acousticController.processTonePowers(newTonePowers, signalLevel);
-}
-
-// Constructor
-AcousticController::AcousticController(Serial* usbSerialObject /* = NULL */) :
- // Initialize variables
- bufferCount(0),
- lastDataWord(0),
- timeSinceGoodWord(0),
- streamCurFishStateAcoustic(0),
- streamCurFishStateEventAcoustic(0)
-{
- // AGC Pins
- gain0 = new DigitalOut(agcPin0);
- gain1 = new DigitalOut(agcPin1);
- gain2 = new DigitalOut(agcPin2);
- agc[0] = gain0;
- agc[1] = gain1;
- agc[2] = gain2;
- #ifdef AGCLeds
- agcLED0 = new DigitalOut(LED1);
- agcLED1 = new DigitalOut(LED2);
- agcLED2 = new DigitalOut(LED3);
- agcLEDs[0] = agcLED0;
- agcLEDs[1] = agcLED1;
- agcLEDs[2] = agcLED2;
- #endif
-
- // Low battery
- lowBatteryVoltageInput = new DigitalIn(lowBatteryVoltagePin);
-
- // Misc
- #ifdef artificialPowers
- FILE* finPowers; = fopen("/local/powers.wp", "r");
- #endif
-
- // Complete initialization
- init(usbSerialObject);
-}
-
-// Initialization
-void AcousticController::init(Serial* usbSerialObject /* = NULL */)
-{
- // Create usb serial object or use provided one
- if(usbSerialObject == NULL)
- {
- usbSerialObject = new Serial(USBTX, USBRX);
- usbSerialObject->baud(serialDefaultBaudUSB);
- }
- usbSerial = usbSerialObject;
- // Miscellaneous
- bufferCount = 0;
- lastDataWord = 0;
- timeSinceGoodWord = 0;
-
- // Will check if battery is low in every buffer callback
- lowBatteryVoltageInput->mode(PullUp);
- //lowBatteryInterrupt.fall(&lowBatteryCallback);
-
- // TODO remove this?
- wait(1.5);
-
- // Configure the tone detector
- toneDetector.setCallback(&toneDetectorCallback);
- toneDetector.init();
-
- // Clear detection arrays
- #if defined(threshold2)
- for(int t = 0; t < numTones; t++)
- {
- detectSums[t] = 0;
- for(uint8_t p = 0; p < detectWindow; p++)
- tonesPresent[p][t] = 0;
- }
- detectWindowIndex = 0;
- readyToThreshold = false;
- powerHistoryIndex = 0;
- powerHistoryDetectIndex = (powerHistoryLength - 1) - (powerHistoryDetectWindow-1) + 1; // assumes powerHistoryLength >= detectWindow > 1
- for(uint8_t t = 0; t < numTones; t++)
- {
- powerHistorySumDetect[t] = 0;
- powerHistorySumNoDetect[t] = 0;
- powerHistoryMaxDetect[t] = 0;
- powerHistoryMaxNoDetect[t] = 0;
- for(uint16_t p = 0; p < powerHistoryLength; p++)
- {
- powerHistory[p][t] = 0;
- }
- }
- #elif defined(threshold1)
- for(int t = 0; t < numTones; t++)
- {
- detectSums[t] = 0;
- }
- powerHistoryIndex = 0;
- powerHistoryDetectIndex = (powerHistoryLength - 1) - (detectWindow-1) + 1; // assumes powerHistoryLength >= detectWindow > 1
- for(uint8_t t = 0; t < numTones; t++)
- {
- powerHistorySum[t] = 0;
- powerHistoryMax[t] = 0;
- for(uint16_t p = 0; p < powerHistoryLength; p++)
- {
- powerHistory[p][t] = 0;
- }
- }
- readyToThreshold = false;
- #endif
- #ifdef singleDataStream
- #ifdef saveData
- dataIndex = 0;
- #endif
- #else
- #ifdef saveData
- dataWordIndex = 0;
- #endif
- dataBitIndex = 0;
- interWord = false;
- #endif
- waitingForEnd = false;
- periodIndex = 0;
- fskIndex = 0;
-
- // Initialize adjustable gain control
- signalLevelBufferIndex = 0;
- signalLevelSum = 0;
- currentGainIndex = 4;
- for(uint8_t i = 0; i < sizeof(agc)/sizeof(agc[0]); i++)
- {
- agc[i]->write(currentGainIndex & (1 << i));
- #ifdef AGCLeds
- agcLEDs[i].write(currentGainIndex & (1 << i));
- #endif
- }
-}
-
-
-// Called by toneDetector when new tone powers are computed (once per buffer)
-void AcousticController::processTonePowers(int32_t* newTonePowers, uint32_t signalLevel)
-{
- #ifdef streamAcousticControlLog
- acousticControlLogToStream[4] = -10;
- #endif
- // Check for low battery and if so terminate tone detector
- if(lowBatteryVoltageInput == 0)
- {
- lowBatteryCallback();
- return;
- }
- // See if we're done and if so terminate the tone detector
- if(bufferCount == loopCount)
- {
- #ifndef infiniteLoopAcoustic
- toneDetector.stop();
- return;
- #else
- //bufferCount = 0;
- #endif
- }
- bufferCount++;
- // See if we're in autonomous mode and if so just let it be
- if(fishController.autoMode)
- return;
-
- periodIndex++;
- timeSinceGoodWord++;
-
- #if defined(threshold2)
- // Update threshold window state for each tone
- for(uint8_t t = 0; t < numTones; t++)
- {
- // Update our history of powers for this tone
- powerHistory[powerHistoryIndex][t] = (newTonePowers[t] >> 5);
- // Compute max/sum of the current window (only for frequencies we currently anticipate)
- if((t == fskIndex*2 || t == fskIndex*2+1) && !waitingForEnd)
- {
- powerHistoryMaxDetect[t] = 0;
- powerHistoryMaxNoDetect[t] = 0;
- powerHistorySumDetect[t] = 0;
- powerHistorySumNoDetect[t] = 0;
- // Look at non-detection zone
- for(uint16_t p = ((powerHistoryIndex+1)%powerHistoryLength); p != powerHistoryDetectIndex; p=(p+1)%powerHistoryLength)
- {
- powerHistorySumNoDetect[t] += powerHistory[p][t];
- if(powerHistory[p][t] > powerHistoryMaxNoDetect[t])
- powerHistoryMaxNoDetect[t] = powerHistory[p][t];
- }
- // Look at detection zone
- for(uint16_t p = powerHistoryDetectIndex; p != ((powerHistoryIndex+1)%powerHistoryLength); p=((p+1)%powerHistoryLength))
- {
- powerHistorySumDetect[t] += powerHistory[p][t];
- if(powerHistory[p][t] > powerHistoryMaxDetect[t])
- powerHistoryMaxDetect[t] = powerHistory[p][t];
- }
- }
- }
- // Advance our power history index (circular buffer)
- powerHistoryIndex = (powerHistoryIndex+1) % powerHistoryLength;
- powerHistoryDetectIndex = (powerHistoryDetectIndex+1) % powerHistoryLength;
- readyToThreshold = readyToThreshold || (powerHistoryIndex == 0);
- // If not waiting out silence until next pulse is expected, see if a tone is present
- if(!waitingForEnd && readyToThreshold)
- {
- // Based on new max/mean, see how many powers indicate a tone present in the last detectWindow readings
- for(uint8_t t = fskIndex*2; t <= fskIndex*2+1; t++)
- {
- detectSums[t] -= tonesPresent[detectWindowIndex][t];
- if(
- ((powerHistorySumDetect[t] << 2) > powerHistorySumNoDetect[t])
- && (powerHistoryMaxDetect[t] > powerHistoryMaxNoDetect[t])
- && ((powerHistorySumDetect[t] - (powerHistorySumDetect[t] >> 3)) > powerHistorySumDetect[fskIndex*2+((t+1)%2)])
- && ((powerHistorySumDetect[t] << 2) > powerHistorySumNoDetect[fskIndex*2+((t+1)%2)])
- && ((powerHistoryMaxDetect[t] << 4) > powerHistorySumNoDetect[t])
- && (powerHistoryMaxDetect[t] > 100000)
- )
- tonesPresent[detectWindowIndex][t] = 1;
- else
- tonesPresent[detectWindowIndex][t] = 0;
- detectSums[t] += tonesPresent[detectWindowIndex][t];
- }
- detectWindowIndex = (detectWindowIndex+1) % detectWindow;
- // If both are considered present (should be very rare?), choose the one with a higher mean
- if(detectSums[fskIndex*2] > detectThresh && detectSums[fskIndex*2+1] > detectThresh)
- {
- if(powerHistorySumDetect[fskIndex*2] > powerHistorySumDetect[fskIndex*2+1])
- detectSums[fskIndex*2+1] = 0;
- else
- detectSums[fskIndex*2] = 0;
- }
- // See if a tone is present
- int tonePresent = -1;
- if(detectSums[fskIndex*2] > detectThresh)
- tonePresent = fskIndex*2;
- else if(detectSums[fskIndex*2+1] > detectThresh)
- tonePresent = fskIndex*2+1;
- // Record data and update state
- if(tonePresent > -1)
- {
- #ifdef singleDataStream // if we just want a stream of bits instead of segmenting into words
- #ifdef saveData
- data[dataIndex++] = tonePresent%2;
- #endif
- #ifdef streamData
- usbSerial->printf("%ld\t%d\n", bufferCount, (bool)(tonePresent%2));
- #endif
- periodIndex = detectSums[tonePresent];
- fskIndex = (fskIndex+1) % numFSKGroups;
- #else
- // See if it has been a long time since last pulse
- if(periodIndex >= interWordWait)
- {
- // If we currently think that we're between words, then that's good so process the previous data word
- // 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
- // Either way, this is the start of a new word so reset dataBitIndex
- if(interWord && dataBitIndex == dataWordLength)
- {
- timeSinceGoodWord = 0;
- streamCurFishStateEventAcoustic = 5;
- // Decode last word and then send it out
- #ifdef saveData
- decodeAcousticWord(data[dataWordIndex]);
- dataWordIndex++;
- #else
- decodeAcousticWord(dataWord);
- #endif
- dataBitIndex = 0;
- interWord = false;
- }
- else if(!interWord) // missed a bit
- {
- #ifdef streamData
- usbSerial->printf("%ld\t0\t-1\n", bufferCount);
- #endif
- #ifdef saveData
- for(int dbi = 0; dbi < dataWordLength; dbi++)
- data[dataWordIndex][dbi] = 0;
- data[dataWordIndex][dataWordLength-1] = 1;
- dataWordIndex++;
- #endif
- #ifdef streamAcousticControlLog
- acousticControlLogToStream[4] = -1;
- streamCurFishStateEventAcoustic = 1;
- #endif
- lastDataWord = 0;
- }
- // TODO this is a debug check - if transmitter not putting space between words, set interWordWait to 1
- if(interWordWait > 1)
- {
- dataBitIndex = 0;
- interWord = false;
- }
- }
- else if(interWord)
- {
- // It has not been a long time since the last pulse, yet we thought it should be
- // Seems like we erroneously detected a bit that shouldn't have existed
- // Discard current word as garbage and start again
- dataBitIndex = 0;
- #ifdef streamData
- usbSerial->printf("%ld\t0\t-2\n", bufferCount);
- #endif
- #ifdef saveData
- for(int dbi = 0; dbi < dataWordLength; dbi++)
- data[dataWordIndex][dbi] = 0;
- data[dataWordIndex][dataWordLength-2] = 1;
- dataWordIndex++;
- #endif
- #ifdef streamAcousticControlLog
- acousticControlLogToStream[4] = -2;
- streamCurFishStateEventAcoustic = 2;
- #endif
- lastDataWord = 0;
- }
- // If we're not between words (either normally or because we were just reset above), store the new bit
- if(!interWord)
- {
- #ifdef saveData
- data[dataWordIndex][dataBitIndex++] = tonePresent%2;
- #else
- dataWord[dataBitIndex++] = tonePresent%2;
- #endif
- // Rotate through which FSK frequency we expect for the next pulse
- fskIndex = (fskIndex+1) % numFSKGroups;
- // If we've finished a word, say we're waiting between words
- // Word won't be processed until next word begins though in case we accidentally detected a nonexistent bit (see logic above)
- if(dataBitIndex == dataWordLength)
- {
- interWord = true;
- fskIndex = 0;
- }
- }
- periodIndex = detectSums[tonePresent]; // Use number of detected points rather than 0 to get it closer to actual rising edge
- #endif
- // Wait out reflections until next pulse
- waitingForEnd = true;
- }
- }
- else if(periodIndex > period) // done waiting for next bit (waiting out reflections)
- {
- waitingForEnd = false;
- // Reset the sums and indicators
- for(uint8_t t = 0; t < numTones; t++)
- {
- detectSums[t] = 0;
- for(uint8_t p = 0; p < detectWindow; p++)
- tonesPresent[p][t] = 0;
- }
- }
- #elif defined(threshold1)
- // Update threshold window state for each tone
- for(uint8_t t = 0; t < numTones; t++)
- {
- // Update our history of powers for this tone
- powerHistorySum[t] -= powerHistory[powerHistoryIndex][t];
- powerHistory[powerHistoryIndex][t] = newTonePowers[t];
- powerHistorySum[t] += newTonePowers[t];
- // Compute max of the current window (only for frequencies we currently anticipate)
- if((t == fskIndex*2 || t == fskIndex*2+1) && !waitingForEnd)
- {
- powerHistoryMax[t] = 0;
- for(uint16_t p = 0; p < powerHistoryLength; p++)
- {
- if(powerHistory[p][t] > powerHistoryMax[t])
- powerHistoryMax[t] = powerHistory[p][t];
- }
- }
- }
- // Advance our power history index (circular buffer)
- powerHistoryIndex = (powerHistoryIndex+1) % powerHistoryLength;
- readyToThreshold = readyToThreshold || (powerHistoryIndex == 0);
- // If not waiting until next pulse is expected, see if a tone is present
- if(!waitingForEnd && readyToThreshold)
- {
- // Based on new max/mean, see how many powers indicate a tone present in the last detectWindow readings
- for(uint8_t t = fskIndex*2; t <= fskIndex*2+1; t++)
- {
- detectSums[t] = 0;
- for(uint16_t p = powerHistoryDetectIndex; p != powerHistoryIndex; p = (p+1) % powerHistoryLength)
- {
- if((powerHistory[p][t] > (powerHistorySum[fskIndex*2+((t+1)%2)] >> 2)) // power greater than 12.5 times mean of other channel
- && (powerHistory[p][t] > (powerHistoryMax[t] >> 2)) // power greater than 1/4 of the max on this channel
- && (powerHistory[p][t] > 1000) // power greater than a fixed threshold
- && 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)
- detectSums[t]++;
- }
- }
- // If both are considered present (should be very rare?), choose the one with a higher mean
- if(detectSums[fskIndex*2] > detectThresh && detectSums[fskIndex*2+1] > detectThresh)
- {
- if(powerHistorySum[fskIndex*2] > powerHistorySum[fskIndex*2+1])
- detectSums[fskIndex*2+1] = 0;
- else
- detectSums[fskIndex*2] = 0;
- }
- // See if a tone is present
- int tonePresent = -1;
- if(detectSums[fskIndex*2] > detectThresh)
- tonePresent = fskIndex*2;
- else if(detectSums[fskIndex*2+1] > detectThresh)
- tonePresent = fskIndex*2+1;
- // Record data and update state
- if(tonePresent > -1)
- {
- #ifdef singleDataStream
- #ifdef saveData
- data[dataIndex++] = tonePresent%2;
- #endif
- #ifdef streamData
- usbSerial->printf("%d", (bool)(tonePresent%2));
- #endif
- periodIndex = detectSums[tonePresent];
- fskIndex = (fskIndex+1) % numFSKGroups;
- #else
- // See if it has been a long time since last pulse
- if(periodIndex >= interWordWait)
- {
- // If we currently think that we're between words, then that's good so process the previous data word
- // 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
- // Either way, this is the start of a new word so reset dataBitIndex
- if(interWord && dataBitIndex == dataWordLength)
- {
- // Decode last word and then send it out
- #ifdef saveData
- decodeAcousticWord(data[dataWordIndex]);
- dataWordIndex++;
- #else
- decodeAcousticWord(dataWord);
- #endif
- dataBitIndex = 0;
- interWord = false;
-
- timeSinceGoodWord = 0;
- }
- else if(interWord)
- {
- #ifdef streamData
- usbSerial->printf("0\t-1\n");
- #endif
- lastDataWord = 0;
- }
- // TODO this is a debug check - if transmitter not putting space between words, set interWordWait to 1
- if(interWordWait > 1)
- {
- dataBitIndex = 0;
- interWord = false;
- }
- }
- else if(interWord)
- {
- // It has not been a long time since the last pulse, yet we thought it should be
- // Seems like we erroneously detected a bit that shouldn't have existed
- // Discard current word as garbage and start again
- dataBitIndex = 0;
- #ifdef streamData
- usbSerial->printf("0\t-2\n");
- #endif
- lastDataWord = 0;
- }
- // If we're not between words (either normally or because we were just reset above), store the new bit
- if(!interWord)
- {
- #ifdef saveData
- data[dataWordIndex][dataBitIndex++] = tonePresent%2;
- #else
- dataWord[dataBitIndex++] = tonePresent%2;
- #endif
- fskIndex = (fskIndex+1) % numFSKGroups;
- periodIndex = detectSums[tonePresent]; // Use number of detected points rather than 0 to get it closer to actual rising edge
- // If we've finished a word, say we're waiting between words
- // Word won't be processed until next word begins though in case we accidentally detected a nonexistent bit (see logic above)
- if(dataBitIndex == dataWordLength)
- {
- interWord = true;
- fskIndex = 0;
- }
- }
- #endif
- // Wait out reflections until next pulse
- waitingForEnd = true;
- }
- }
- else if(periodIndex > period) // done waiting for next bit (waiting out reflections)?
- {
- waitingForEnd = false;
- }
- // Advance our power history start detect index (circular buffer) to stay detectWindow elements behind main index
- powerHistoryDetectIndex = (powerHistoryDetectIndex+1) % powerHistoryLength;
- #endif // end switch between threshold1 or threshold2
-
- // Update signal level history
- signalLevelSum += signalLevel;
- signalLevelBufferIndex = (signalLevelBufferIndex+1) % signalLevelBufferLength;
- #ifdef streamSignalLevel
- //usbSerial->printf("%ld\t%d\t%d\n", signalLevel, currentGainIndex, lastDataWord);
- usbSerial->printf("%ld\t%d\n", signalLevel, currentGainIndex);
- #endif
- #ifdef streamAcousticControlLog
- acousticControlLogToStream[3] = currentGainIndex;
- #endif
- // If have seen enough readings, choose a new gain
- if(signalLevelBufferIndex == 0)
- {
- // Calculate ideal gain for signalLevelSum to become signalLevelSumDesired
- int32_t newGain = ((int64_t)signalLevelSumDesired * (int64_t)agcGains[currentGainIndex]) / (int64_t)signalLevelSum;
- // See which available gain is closest
- uint8_t bestIndex = currentGainIndex;
- int32_t minDiff = 10000;
- for(uint8_t g = 0; g < agcGainsLength; g++)
- {
- int32_t diff = (int32_t)agcGains[g] - newGain;
- if(diff > 0 && diff < minDiff)
- {
- minDiff = diff;
- bestIndex = g;
- }
- else if(diff < 0 && -1*diff < minDiff)
- {
- minDiff = -1*diff;
- bestIndex = g;
- }
- }
- // Set the gain
- currentGainIndex = bestIndex;
- for(uint8_t i = 0; i < 3; i++)
- {
- #ifdef useAGC
- agc[i]->write(currentGainIndex & (1 << i));
- #endif
- #ifdef AGCLeds
- agcLEDs[i].write(currentGainIndex & (1 << i));
- #endif
- }
- // Reset sum
- signalLevelSum = 0;
- }
-
- #ifdef artificialPowers
- 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);
- #endif
-
- // Check for timeout
- #ifdef fishTimeoutAcoustic
- if(timeSinceGoodWord >= fishTimeoutAcousticBuffers)
- {
- #ifndef singleDataStream
- if(timeSinceGoodWord == fishTimeoutAcousticBuffers)
- {
- 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
- decodeAcousticWord(neutralWord);
- streamCurFishStateEventAcoustic = 4;
- }
- #endif
- timeSinceGoodWord = fishTimeoutAcousticBuffers+1; // make sure we won't constantly send a neutral command (only timeout once)
- }
- #endif
-
- // Stream data
- #ifdef streamAcousticControlLog
- //usbSerial->printf("%ld %ld %ld %ld %ld %ld\n", bufferCount-1, acousticControlLogToStream[0], acousticControlLogToStream[1], acousticControlLogToStream[2], acousticControlLogToStream[3], acousticControlLogToStream[4]);
- //usbSerial->printf("%ld %ld %ld\n", acousticControlLogToStream[0], acousticControlLogToStream[1], acousticControlLogToStream[2]);
- //usbSerial->printf("%ld %ld %ld %ld %ld\n", acousticControlLogToStream[0], acousticControlLogToStream[1], acousticControlLogToStream[2], acousticControlLogToStream[3], acousticControlLogToStream[4]);
- //FunctionPointer fp = &tempMethod;
- //acousticControlLogToStream[0] = 6100;
- //acousticControlLogToStream[1] = 62;
- //acousticControlLogToStream[2] = 63;
- //acousticControlLogToStream[3] = 64;
- //acousticControlLogToStream[4] = -10;
- uint32_t streamGoertzel1 = acousticControlLogToStream[0];// >> 8; // second Fiji day included >> 8 to reduce bits
- uint32_t streamGoertzel2 = acousticControlLogToStream[1];// >> 8; // second Fiji day included >> 8 to reduce bits
- 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
- uint8_t streamGain = acousticControlLogToStream[3] + 168; // only used in first day
- //int16_t streamWord = acousticControlLogToStream[4];
- // Decide the event to transmit, giving priority to acoustic events over button board events
- uint16_t streamFishStateEvent = fishController.streamFishStateEventController;
- if(streamCurFishStateEventAcoustic > 0)
- streamFishStateEvent = streamCurFishStateEventAcoustic;
- uint16_t streamWord = (uint16_t)streamCurFishStateAcoustic | ((uint16_t)streamFishStateEvent << 11); // same for both Fiji days
-
- uint8_t* bufferOut = (uint8_t*) (&streamGoertzel1);
- usbSerial->putc(bufferOut[0]);
- usbSerial->putc(bufferOut[1]);
- usbSerial->putc(bufferOut[2]);
- usbSerial->putc(bufferOut[3]); // commented out for second day
-
- bufferOut = (uint8_t*) (&streamGoertzel2);
- usbSerial->putc(bufferOut[0]);
- usbSerial->putc(bufferOut[1]);
- usbSerial->putc(bufferOut[2]);
- usbSerial->putc(bufferOut[3]); // commented out for second day
-
- bufferOut = (uint8_t*) (&streamSignalLevel);
- usbSerial->putc(bufferOut[0]);
- usbSerial->putc(bufferOut[1]);
-
- bufferOut = (uint8_t*) (&streamGain);
- usbSerial->putc(bufferOut[0]); // commented out for second day
-
- bufferOut = (uint8_t*) (&streamWord);
- usbSerial->putc(bufferOut[0]);
- usbSerial->putc(bufferOut[1]);
- //usbSerial->printf("%d %d\n", streamCurFishState, streamWord);
-
- /*uint8_t* bufferOut = (uint8_t*) acousticControlLogToStream;
- for(uint8_t i = 0; i < 20; i++)
- usbSerial->putc(bufferOut[i]);*/
-
- // Reset the events so we don't constantly print them
- // TODO remove this so we do constantly print them, and only look for changes in the column in case some are dropped?
- streamCurFishStateEventAcoustic = 0;
- fishController.streamFishStateEventController = 0;
- #endif
-}
-
-
-#ifndef singleDataStream
-// Use Hamming code to decode the data word and check for an error
-// TODO make constants/defines for magic numbers below that indicate length of hamming code
-void AcousticController::decodeAcousticWord(volatile bool* data)
-{
- // 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
- // Check whether each parity bit is correct
- bool paritySums[5] = {0};
- for(uint8_t b = 0; b < dataWordLength-1; b++)
- {
- for(uint8_t p = 0; p < 5; p++)
- {
- if(((b+1) & (1 << p)) > 0 || p == 5-1) // check if bit belongs to parity set (overall parity at end includes everything)
- paritySums[p] ^= data[b];
- }
- }
- paritySums[5-1] ^= data[dataWordLength-1]; // overall parity bit
- // See if we have an error to correct
- uint8_t errorBit = 0;
- uint8_t numParityErrors = 0;
- for(uint8_t p = 0; p < 5-1; p++)
- {
- if(paritySums[p])
- {
- errorBit += (1 << p);
- numParityErrors++;
- }
- }
- // Flip the erroneous bit if applicable
- if(numParityErrors > 1)
- data[errorBit-1] = !data[errorBit-1];
- // If using final parity bit, check that we don't detect an uncorrectable error
- if(!(numParityErrors > 0 && !paritySums[5-1]))
- {
- uint16_t word = 0;
- uint8_t i = 0;
- uint8_t nextParity = 1;
- for(uint8_t b = 0; b < dataWordLength; b++)
- {
- if(b == nextParity-1)
- nextParity <<= 1;
- else
- word |= data[b] << i++;
- }
- // Update the fish
- processAcousticWord(word);
- lastDataWord = word;
- #ifdef streamData
- if(timeSinceGoodWord >= fishTimeoutAcousticBuffers)
- usbSerial->printf("%ld\t%d\t-4\n", bufferCount, (int)word);
- else
- usbSerial->printf("%ld\t%d\t1\n", bufferCount, (int)word);
- #endif
- #ifdef streamAcousticControlLog
- 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
- acousticControlLogToStream[4] = -4;
- else
- acousticControlLogToStream[4] = word;
- #endif
- }
- else // there was an uncorrectable error
- {
- #ifdef streamData
- uint16_t word = 0;
- for(int i = 0; i < 16; i++)
- word += data[i] << i;
- usbSerial->printf("%ld\t%d\t-3\n", bufferCount, word);
- #endif
- #ifdef streamAcousticControlLog
- acousticControlLogToStream[4] = -3;
- streamCurFishStateEventAcoustic = 3;
- #endif
- lastDataWord = 0;
- }
-}
-
-void AcousticController::processAcousticWord(uint16_t word)
-{
- // Extract state from word
- newSelectButtonIndex = getSelectIndexAcoustic(word);
- newPitchIndex = getPitchIndexAcoustic(word);
- newYawIndex = getYawIndexAcoustic(word);
- newThrustIndex = getThrustIndexAcoustic(word);
- newFrequencyIndex = getFrequencyIndexAcoustic(word);
- // Log it
- streamCurFishStateAcoustic = getCurStateWordAcoustic;
- // Set the states
- #ifdef acousticControllerControlFish
- fishController.setSelectButton(newSelectButtonIndex > 0);
- fishController.setPitch(pitchLookupAcoustic[newPitchIndex]);
- fishController.setYaw(yawLookupAcoustic[newYawIndex]);
- fishController.setThrust(thrustLookupAcoustic[newThrustIndex]);
- fishController.setFrequency(frequencyLookupAcoustic[newFrequencyIndex], periodHalfLookupAcoustic[newFrequencyIndex]);
- #endif
-}
-#endif
-
-// Stop the AcousticController
-// Will stop the tone detector and will stop the fishController
-// Note that the acoustic controller's run() method is blocking, so if you
-// want to use this stop method you should launch run() in a separate thread
-void AcousticController::stop()
-{
- // Stop the tone detector
- // This will end the toneDetector's run() method at the next buffer
- // which will cause our run() method to advance and finish up
- toneDetector.stop();
-}
-
-// Main loop
-// Is blocking, so won't return until loopCount is reached or another thread calls stop()
-void AcousticController::run()
-{
- // Create file for logging data words
- #ifdef saveData
- int fileNum = -1;
- char filename[25];
- foutDataWords = NULL;
- do
- {
- fileNum++;
- fclose(foutDataWords);
- sprintf(filename, "/local/%d.txt", fileNum);
- foutDataWords = fopen(filename, "r");
- usbSerial->printf("%d\n", fileNum);
- } while(foutDataWords != NULL);
- foutDataWords = fopen(filename, "w");
- #endif
-
- #ifdef acousticControllerControlFish
- // Start the fish controller
- fishController.start();
- #endif
-
- #ifndef artificialPowers
- // Start listening for tones
- programTimer.start();
- toneDetector.run();
- programTimer.stop();
- toneDetector.finish(); // we won't include the time this takes to write files in the elapsed time
- #else
- // Read powers from file
- usbSerial->printf("newPower[0] \tnewPower[1] \tdetectSums[0] \tdetectSums[1] \tsum[0] \tsum[1] \tmax[0] \tmax[1] \tfskIndex \twaiting \tperiodIndex \n");
- int32_t maxSignalValTemp = 0;
- int res = fscanf(finPowers, "%ld\t%ld\n", &nextPowers[0], &nextPowers[1]);
- while(res > 0)
- {
- processTonePowers(nextPowers, maxSignalValTemp);
- res = fscanf(finPowers, "%ld\t%ld\n", &nextPowers[0], &nextPowers[1]);
- }
- fclose(finPowers);
- #endif
-
- #ifdef acousticControllerControlFish
- // Stop the fish controller
- fishController.stop();
- // If battery died, wait a bit for pi to clean up and shutdown and whatnot
- if(lowBatteryVoltageInput == 0)
- {
- wait(90); // Give the Pi time to shutdown
- fishController.setLEDs(255, false);
- }
- #endif
-
- // Print results
- #ifdef printBufferSummary
- int elapsed = programTimer.read_us();
- usbSerial->printf("\n");
- usbSerial->printf("Buffers processed: %ld\n", bufferCount);
- usbSerial->printf("Elapsed time : %d us\n", elapsed);
- usbSerial->printf("Per-sample time : %f us\n", (double)elapsed/(double)bufferCount/(double)sampleWindow);
- usbSerial->printf(" Sample frequency: %f kHz\n", (double)bufferCount*(double)sampleWindow/(double)elapsed*1000.0);
- usbSerial->printf("Per-buffer time : %f us\n", (double)elapsed/(double)bufferCount);
- usbSerial->printf(" Buffer-processing frequency: %f kHz\n", (double)bufferCount/(double)elapsed*1000.0);
-
- usbSerial->printf("\nComputed powers from last buffer: \n");
- int32_t* lastTonePowers = toneDetector.getTonePowers();
- for(int i = 0; i < numTones; i++)
- usbSerial->printf(" Tone %d: %f Hz -> %f\n", i, targetTones[i], toFloat(lastTonePowers[i]));
- #endif
- #if defined(singleDataStream) && defined(saveData)
- usbSerial->printf("\nData received (%d bits):\n", dataIndex);
- fprintf(foutDataWords, "\nData received (%d bits):\n", dataIndex);
- long errors = 0;
- for(int d = 5; d < dataIndex; d++)
- {
- usbSerial->printf("%d", data[d]);
- fprintf(foutDataWords, "%d", data[d]);
- if(d > 0 && data[d] == data[d-1])
- errors++;
- }
- usbSerial->printf("\n");
- usbSerial->printf("errors: %ld\n", errors);
- fprintf(foutDataWords, "\n");
- fprintf(foutDataWords, "errors: %ld\n", errors);
- fclose(foutDataWords);
- #ifdef debugLEDs
- if(errors > 0)
- led1 = 1;
- #endif
- #elif defined(saveData)
- usbSerial->printf("\nData received (%d words):\n", dataWordIndex);
- fprintf(foutDataWords, "\nData received (%d words):\n", dataWordIndex);
- long errors = 0;
- long badWords = 0;
- for(int w = 0; w < dataWordIndex; w++)
- {
- errors = 0;
- usbSerial->printf(" ");
- fprintf(foutDataWords, " ");
- for(int b = 0; b < dataWordLength; b++)
- {
- usbSerial->printf("%d", data[w][b]);
- fprintf(foutDataWords, "%d", data[w][b]);
- if(b > 0 && data[w][b-1] == data[w][b])
- errors++;
- }
- if(errors > 0)
- {
- usbSerial->printf(" X");
- fprintf(foutDataWords, " X");
- badWords++;
- }
- usbSerial->printf("\n");
- fprintf(foutDataWords, "\n");
- }
- usbSerial->printf("\nbad words: %d\n", badWords);
- fprintf(foutDataWords, "\nbad words: %d\n", badWords);
- fclose(foutDataWords);
- #ifdef debugLEDs
- if(badWords > 0)
- led1 = 1;
- #endif
- #endif
-
- // TODO remove these waits?
- wait(1);
- #ifdef printBufferSummary
- usbSerial->printf("\nAcousticController Done!");
- #endif
- wait(3);
- #ifdef printBufferSummary
- usbSerial->printf("\n");
- #endif
-}
-
-
-void AcousticController::lowBatteryCallback()
-{
- // Stop the tone detector
- // This will end the main call to start, causing main to terminate
- // Main will also stop the fish controller once this method ends
- toneDetector.stop();
- // Also force the pin low to signal the Pi
- // (should have already been done, but just in case)
- // TODO check that this really forces it low after this method ends and the pin object may be deleted
- DigitalOut simBatteryLow(lowBatteryVoltagePin);
- simBatteryLow = 0;
-}
-
-#endif // #ifdef acousticControl
--- a/robotic_fish_6/AcousticControl/AcousticController.h Fri Dec 03 23:03:20 2021 +0000
+++ /dev/null Thu Jan 01 00:00:00 1970 +0000
@@ -1,256 +0,0 @@
-/*
- * AcousticController.h
- * Author: Joseph DelPreto
- */
-
-//#define acousticControl
-
-#ifdef acousticControl
-
-#ifndef ACOUSTICCONTROL_ACOUSTICCONTROLLER_H_
-#define ACOUSTICCONTROL_ACOUSTICCONTROLLER_H_
-
-#include "ToneDetector.h"
-#include "FishController.h"
-#include "mbed.h"
-
-// ===== INPUT/OUTPUT =====
-// NOTE: To be silent (to not use usbSerial object at all), undefine printBufferSummary, streamAcousticControlLog, streamData, and streamSignalLevel
-// To stream the log info used in Fiji, just define streamAcousticControlLog - these are not human-readable streams
-// To view the data as it's being received/decoded, define streamData (or streamSignalLevel for signal level) - these are human-readable streams
-// To view a data summary (ex. number of buffers processed, words received, etc.) define printBufferSummary
-#define serialDefaultBaudUSB 921600 // IMPORTANT: must be 921600 when using streaAcousticControlLog (to be fast enough to keep up with data stream)
-#define printBufferSummary // print a summary of how many buffers were processed, the last computed tone powers, etc. once the controller is stopped
-/* SEE ALSO: streamAcousticControlLog, defined in ToneDetector.h
- If defined, will update the following (see end of processTonePowers for how they're packaged):
-
- acousticControlLogToStream:
- [0]: goertzel powers f1
- [1]: goerztel powers f2
- [2]: signal level
- [3]: AGC gain
- [4]: fish state/events over Serial
- -1: erroneously missed a bit
- -2: erroneously added a bit
- -3: detected an uncorrectable error in the Hamming decoding
- -4: fish timeout (reset to neutral)
- >0: the data word that was successfully decoded and processed
- -10: no event
-
-UPDATE:
-
- acousticControlLogToStream[4] has been deprecated and replaced with streamCurFishStateEventAcoustic and streamCurFishStateAcoustic
- streamCurFishStateEventAcoustic can have the following values:
- 1: erroneously missed a bit
- 2: erroneously added a bit
- 3: detected an uncorrectable error in the Hamming decoding
- 4: fish timeout (reset to neutral)
- 5: successfully received a word (note streamCurFishState indicates the current fish state)
- 6: button board yaw left
- 7: button board yaw right
- 8: button board faster
- 9: button board slower
- 10: button board pitch up
- 11: button board pitch down
- 12: button board shutdown pi
- 13: button board reset mbed
- 14: button board auto mode
- 15: button board unknown
-*/
-
-//#define artificialPowers // will use tone powers saved in a file rather than actually sampling (expects two tab-separated columns of powers in /local/powers.wp)
-//#define singleDataStream // just read a single continuous stream of bits (as opposed to segmenting into words/decoding)
-
-//#define saveData // save data such as received words to an array, then save it to a file at the end - faster than printing continuously but worry about overflow
-//#define streamData // stream data to the screen as it's received (controlled independently of saveData)
-//#define streamSignalLevel // stream current signal level to the screen
-
-// ===== EXECUTION =====
-#define infiniteLoopAcoustic // if undefined, will stop after loopCount buffers are processed
-#define loopCount 20000 // number of buffers to process before terminating the program (if infiniteLoopAcoustic is not defined)
-#define fishTimeoutAcoustic // whether to reset the fish to neutral if an acoustic word hasn't been received in a while. Not used with singleDataStream.
-#define fishTimeoutAcousticWindow 5000000 // time between good words that cause a timeout (in microseconds)
-
-#define acousticControllerControlFish // whether to start fishController to control the servos and motor
-
-#define useAGC // if undefined, will only set agc (adjustable gain control of the acoustic input board) once at startup
-//#define AGCLeds // if defined, will light the mBed LEDs to indicate the chosen agc gain index (whether or not useAGC is on)
-#define agcUpdateWindow 10000000 // how often to update the AGC gain (in microseconds)
-
-#define dataRate 20 // data rate in bps. Can be 20, 50, 77, or 100. 20 was used in Fiji. Above 50 may be less reliable.
-
-//#define threshold1 // older algorithm for processing Goertzel buffers (not sure if it still works)
-#define threshold2
-
-// ===== PINS =====
-#define agcPin0 p16
-#define agcPin1 p17
-#define agcPin2 p18
-// note lowBatteryVoltagePin is defined in FishController
-
-// ===== Sampling / Thresholding configuration =====
-// 20 bps (5/45 ms tone/silence)
-#if dataRate == 20
-#define detectWindow 10 // number of buffers to look at when deciding if a tone is present
-#define detectThresh 6 // number of 1s that must be present in a detectWindow group of buffer outputs to declare a tone present
-#define period 80 // the number of buffers from a rising edge to when we start looking for the next rising edge
-#define bitPeriod 100 // only used with saveData: how many buffers it should actually be between rising edges (only used to size the data array)
-#define interWordWait 300 // how many buffers of silence to expect between words
-#elif dataRate == 50
-// 50 bps (5/15 ms tone/silence)
-#define detectWindow 10 // number of buffers to look at when deciding if a tone is present
-#define detectThresh 6 // number of 1s that must be present in a detectWindow group of buffer outputs to declare a tone present
-#define period 20 // the number of buffers from a rising edge to when we start looking for the next rising edge
-#define bitPeriod 40 // only used with saveData: how many buffers it should actually be between rising edges (only used to size the data array)
-#define interWordWait 300 // how many buffers of silence to expect between words
-#elif dataRate == 77
-// 77 bps (5/8 ms tone/silence)
-#define detectWindow 10 // number of buffers to look at when deciding if a tone is present
-#define detectThresh 6 // number of 1s that must be present in a detectWindow group of buffer outputs to declare a tone present
-#define period 10 // the number of buffers from a rising edge to when we start looking for the next rising edge
-#define bitPeriod 26 // only used with saveData: how many buffers it should actually be between rising edges (only used to size the data array)
-#define interWordWait 300 // how many buffers of silence to expect between words
-#elif dataRate == 100
-// 100 bps (5/5 ms tone/silence)
-#define detectWindow 10 // number of buffers to look at when deciding if a tone is present
-#define detectThresh 6 // number of 1s that must be present in a detectWindow group of buffer outputs to declare a tone present
-#define period 6 // the number of buffers from a rising edge to when we start looking for the next rising edge
-#define bitPeriod 20 // only used with saveData: how many buffers it should actually be between rising edges (only used to size the data array)
-#define interWordWait 300 // how many buffers of silence to expect between words
-#endif
-
-// ===== Macros for extracting state from data words =====
-#define getSelectIndexAcoustic(d) ((d & (1 << 0)) >> 0)
-#define getPitchIndexAcoustic(d) ((d & (7 << 1)) >> 1)
-#define getYawIndexAcoustic(d) ((d & (7 << 4)) >> 4)
-#define getThrustIndexAcoustic(d) ((d & (3 << 7)) >> 7)
-#define getFrequencyIndexAcoustic(d) ((d & (3 << 9)) >> 9)
-
-#define getCurStateWordAcoustic (uint16_t)((uint16_t)newSelectButtonIndex + ((uint16_t)newPitchIndex << 1) + ((uint16_t)newYawIndex << 4) + ((uint16_t)newThrustIndex << 7) + ((uint16_t)newFrequencyIndex << 9));
-
-class AcousticController
-{
- public:
- // Initialization
- AcousticController(Serial* usbSerialObject = NULL); // if serial object is null, one will be created
- void init(Serial* usbSerialObject = NULL); // if serial object is null, one will be created
- // Execution control
- void run();
- void stop();
- // Called by toneDetector when new tone powers are computed
- // Only public since we needed a dummy non-member function to call it from the static instance to pass a pointer to tonedetector (see comments on toneDetectorCallback)
- void processTonePowers(int32_t* newTonePowers, uint32_t signalLevel);
- private:
- // Control
- volatile uint32_t bufferCount;
- Timer programTimer;
- Serial* usbSerial;
-
- #ifndef singleDataStream
- void decodeAcousticWord(volatile bool* data); // Use Hamming code to decode the data word and check for an error
- void processAcousticWord(uint16_t word); // Parse the decoded word into the various fish states
- #endif
-
- // TODO check what actually needs to be volatile
-
- // Data detection
- volatile bool waitingForEnd; // got data, now waiting until next bit transmission is expected (waiting until "period" buffers have elapsed since edge)
- volatile uint16_t periodIndex; // how many buffers it's been since the last clock edge
- volatile uint8_t fskIndex; // which set of 0/1 frequencies are next expected
-
- #if defined(threshold2)
- #define powerHistoryLength 50 // number of buffer results to consider when deciding if a tone is present. should be half of a bit-width
- #define powerHistoryDetectWindow 20 // portion of powerHistory to consider as the detection zone (where a peak is expected)
- volatile int32_t powerHistory[powerHistoryLength][numTones]; // History of Goertzel results for the last powerHistoryLength buffers
- volatile int16_t powerHistoryIndex; // Index into powerHistory (circular buffer)
- volatile int16_t powerHistoryDetectIndex; // Marks where in powerHistory to start window for detection; will always be detectWindow elements behind powerHistoryIndex in the circular buffer
- volatile int32_t powerHistorySumDetect[numTones]; // Sum of powers stored in powerHistory in the detect window (stand-in for mean)
- volatile int32_t powerHistorySumNoDetect[numTones]; // Sum of powers stored in powerHistory outside the detect window (stand-in for mean)
- volatile int32_t powerHistoryMaxDetect[numTones]; // Max of powers stored in powerHistory in the detect window
- volatile int32_t powerHistoryMaxNoDetect[numTones]; // Max of powers stored in powerHistory outside the detect window
- volatile bool readyToThreshold; // Whether powerHistory has been filled at least once
- volatile bool tonesPresent[detectWindow][numTones]; // Circular buffer of whether tones were detected in last detectWindow powers
- volatile uint8_t detectSums[numTones]; // Rolling sum over last detectWindow tonesPresent results (when > detectWindow/2, declares data bit)
- volatile uint8_t detectWindowIndex;
- #elif defined(threshold1)
- #define powerHistoryLength 50 // number of results to consider when deciding if a tone is present. should be half of a bit-width
- volatile uint8_t detectSums[numTones]; // Rolling sum over last detectWindow buffer results (when > detectWindow/2, declares clock/data)
- int32_t powerHistory[powerHistoryLength][numTones]; // History of Goertzel results for the last powerHistoryLength buffers
- volatile int16_t powerHistoryIndex; // Index into powerHistory (circular buffer)
- volatile int16_t powerHistoryDetectIndex; // Marks where in powerHistory to start window for detection; will always be detectWindow elements behind powerHistoryIndex in the circular buffer
- int32_t powerHistorySum[numTones]; // Sum of powers stored in powerHistory (stand-in for mean)
- int32_t powerHistoryMax[numTones]; // Max of powers stored in powerHistory
- volatile bool readyToThreshold; // Whether powerHistory has been filled at least once
- #endif
-
- // Segmenting of data into bits/words
- #if defined(singleDataStream) && defined(saveData)
- volatile bool data[loopCount/(bitPeriod) * (numTones-1) + 50]; // Received data (each clock pulse is associated with numTones-1 bits) // TODO update for multi-hop FSK, but this is fine for one FSK group
- volatile uint16_t dataIndex;
- #else
- #define dataWordLength 16 // bits per word. note that changing this also requires adjusting the decodeAcousticWord algorithm, since it currently assumes Hamming encoding
- volatile uint8_t dataBitIndex; // current bit of that word
- volatile bool interWord; // whether we are currently in between words (waiting out the long inter-word pause)
- #ifdef saveData
- volatile bool data[loopCount/(bitPeriod*dataWordLength)][dataWordLength]; // Each element is a word as an array of bits
- volatile uint16_t dataWordIndex; // current data word
- #else
- volatile bool dataWord[dataWordLength]; // an array of bits making up the current word
- #endif
- #endif
- volatile uint16_t lastDataWord; // not used anymore, but still updated
-
- // Adjustable gain control (AGC)
- volatile uint8_t currentGainIndex; // the current index into agcGains
- volatile int32_t signalLevelSum; // the running sum of signal level readings
- volatile uint16_t signalLevelBufferIndex; // index into signal level buffer // note we don't have a buffer anymore, but use this to know when to reset the sum
-
- DigitalOut* gain0;
- DigitalOut* gain1;
- DigitalOut* gain2;
- DigitalOut* agc[3]; // facilitates loops and whatnot
- #ifdef AGCLeds
- #undef debugLEDs
- DigitalOut* agcLED0;
- DigitalOut* agcLED1;
- DigitalOut* agcLED2;
- DigitalOut* agcLEDs[3];
- #endif
-
- // Fish reset timeout
- volatile uint16_t timeSinceGoodWord; // measured in buffers
-
- // Low battery monitor
- DigitalIn* lowBatteryVoltageInput;
- void lowBatteryCallback();
-
- // Testing/debugging
- #ifdef saveData
- LocalFileSystem local("local");
- FILE* foutDataWords;
- #endif
- #ifdef artificialPowers
- #if (!defined recordStreaming || (!defined recordOutput && !defined recordSamples)) && !defined saveData
- LocalFileSystem local("local");
- #endif
- FILE* finPowers;
- int32_t nextPowers[numTones];
- #endif
- volatile uint16_t streamCurFishStateAcoustic; // the current state of the fish
- volatile uint16_t streamCurFishStateEventAcoustic; // events such as decoding words, errors, etc.
-
- // Fish state extracted from acoustic data word (index into lookup tables defined above)
- volatile uint8_t newSelectButtonIndex;
- volatile uint8_t newPitchIndex;
- volatile uint8_t newYawIndex;
- volatile uint8_t newThrustIndex;
- volatile uint8_t newFrequencyIndex;
-};
-
-
-// Create a static instance of AcousticController to be used by anyone doing detection
-extern AcousticController acousticController;
-
-#endif
-
-#endif // #ifdef acousticControl
--- a/robotic_fish_6/AcousticControl/ToneDetector.cpp Fri Dec 03 23:03:20 2021 +0000
+++ /dev/null Thu Jan 01 00:00:00 1970 +0000
@@ -1,626 +0,0 @@
-/*
- * Author: Joseph DelPreto
- */
-
-#include "ToneDetector.h"
-
-#ifdef acousticControl
-
-// The static instance
-ToneDetector toneDetector;
-#ifdef streamAcousticControlLog
-int32_t acousticControlLogToStream[5] = {0,0,0,0,0};
-#endif
-
-//============================================
-// Initialization
-//============================================
-
-// Constructor
-ToneDetector::ToneDetector() :
- terminated(0),
- fillingBuffer0(false),
- transferComplete(false),
- readyToBegin(false),
- callbackFunction(0)
- #ifdef artificialSamplesMode
- , numTestSamples(0),
- testSampleIndex(0),
- sampleIndex(0)
- #endif
-{
-}
-
-// Set a callback function that should be called after each buffer is processed
-void ToneDetector::setCallback(void (*myFunction)(int32_t* tonePowers, uint32_t signalLevel))
-{
- callbackFunction = myFunction;
-}
-
-// Initialize the Goertzel variables, arrays, etc.
-// sampleWindow, sampleInterval, and tones must have been previously set
-// if not, this method will return without doing anything
-void ToneDetector::init()
-{
- // Make sure sampleWindow, sampleInterval, and desired tones have been set
- if(sampleWindow == 0 || sampleInterval == 0 || numTones == 0)
- return;
-
- // Initialize sample buffers
- //printf("Sampling interval: %d us\n", sampleInterval);
- //printf("Buffer size: %d samples\n", sampleWindow);
- for(uint16_t i = 0; i < sampleWindow; i++) // not using memset since sizeof seems to not work with uint16_t?
- {
- sampleBuffer0[i] = 0;
- sampleBuffer1[i] = 0;
- }
- #if defined(recordSamples) && !defined(recordStreaming)
- savedSamplesIndex = 0;
- for(uint16_t i = 0; i < numSavedSamples; i++) // not using memset since sizeof seems to not work with uint16_t?
- savedSamples[i] = 0;
- #endif
-
- // Initialize goertzel arrays
- tonePowersWindowIndex = 0;
- for(int i = 0; i < numTones; i++)
- {
- goertzelCoefficients[i] = 0;
- for(int w = 0; w < tonePowersWindow; w++)
- tonePowers[i][w] = 0;
- tonePowersSum[i] = 0;
- }
- readyToThreshold = false;
-
- // Some convenience variables as doubles for precision (will cast to fixed point later)
- double sampleFrequency = 1000.0/((double)sampleInterval/1000.0); // Hz
- double N = (double) sampleWindow;
-
- // Calculate the coefficient for each tone
- //printf("Initializing Goertzel algorithm\n");
- //printf(" Bin size: %f\n", sampleFrequency/N);
- for(int i = 0; i < numTones; i++)
- {
- // Determine K and then f for desired tone (f = desiredF/Fs)
- // tone/fs = f = K/N --> K = (int)(tone*N/fs)
- double tone = targetTones[i];
- int k = (int) (0.5 + ((N * tone) / sampleFrequency));
- float f = ((float)k) / N;
- //printf(" Desired tone %f -> %f", tone, f*sampleFrequency);
- float cosine = cos(2.0 * PI * f);
- // Store the result as a fixed-point number
- goertzelCoefficients[i] = toFixedPoint(2.0*cosine);
- //printf("\t (coefficient: %f)\n", toFloat(goertzelCoefficients[i]));
- }
-
- #if defined(recordOutput) && !defined(recordStreaming)
- savedTonePowersIndex = 0;
- for(uint16_t i = 0; i < numSavedTonePowers; i++) // not using memset since sizeof seems to not work with uint16_t?
- {
- for(uint8_t t = 0; t < numTones; t++)
- savedTonePowers[i][t] = 0;
- }
- #endif
-
- // We're ready to process some tunes!
- readyToBegin = true;
-}
-
-#ifndef artificialSamplesMode
-// Configure and start the DMA channels for ADC sample gathering
-// Will have a linked list of two DMA operations, one for each buffer
-void ToneDetector::startDMA()
-{
- // Create the Linked List Items
- lli[0] = new MODDMA_LLI;
- lli[1] = new MODDMA_LLI;
-
- // Prepare DMA configuration, which will be chained (one operation for each buffer)
- dmaConf = new MODDMA_Config;
- dmaConf
- ->channelNum ( MODDMA::Channel_0 )
- ->srcMemAddr ( 0 )
- ->dstMemAddr ( (uint32_t)sampleBuffer0 )
- ->transferSize ( sampleWindow )
- ->transferType ( MODDMA::p2m )
- ->transferWidth ( MODDMA::word )
- ->srcConn ( MODDMA::ADC )
- ->dstConn ( 0 )
- ->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]
- ->attach_tc ( &TC0_callback )
- ->attach_err ( &ERR0_callback )
- ;
- // Create LLI to transfer from ADC to adcBuffer0 (and then launch lli[1])
- lli[0]->SrcAddr = (uint32_t)dma.LUTPerAddr(dmaConf->srcConn());
- lli[0]->DstAddr = (uint32_t)sampleBuffer0;
- lli[0]->NextLLI = (uint32_t) lli[1];
- lli[0]->Control = dma.CxControl_TransferSize(dmaConf->transferSize())
- | dma.CxControl_SBSize((uint32_t)dma.LUTPerBurst(dmaConf->srcConn()))
- | dma.CxControl_DBSize((uint32_t)dma.LUTPerBurst(dmaConf->srcConn()))
- | dma.CxControl_SWidth((uint32_t)dma.LUTPerWid(dmaConf->srcConn()))
- | dma.CxControl_DWidth((uint32_t)dma.LUTPerWid(dmaConf->srcConn()))
- | dma.CxControl_DI()
- | dma.CxControl_I();
- // Create LLI to transfer from ADC to adcBuffer1 (and then launch lli[0] to repeat)
- lli[1]->SrcAddr = (uint32_t)dma.LUTPerAddr(dmaConf->srcConn());
- lli[1]->DstAddr = (uint32_t)sampleBuffer1;
- lli[1]->NextLLI = (uint32_t) lli[0];
- lli[1]->Control = dma.CxControl_TransferSize(dmaConf->transferSize())
- | dma.CxControl_SBSize((uint32_t)dma.LUTPerBurst(dmaConf->srcConn()))
- | dma.CxControl_DBSize((uint32_t)dma.LUTPerBurst(dmaConf->srcConn()))
- | dma.CxControl_SWidth((uint32_t)dma.LUTPerWid(dmaConf->srcConn()))
- | dma.CxControl_DWidth((uint32_t)dma.LUTPerWid(dmaConf->srcConn()))
- | dma.CxControl_DI()
- | dma.CxControl_I();
-
- // Start the DMA chain
- fillingBuffer0 = true;
- transferComplete = false;
- if (!dma.Prepare(dmaConf)) {
- error("Doh! Error preparing initial dma configuration");
- }
-}
-
-// Configure the ADC to trigger on Timer1
-// Start the timer with the desired sampling interval
-void ToneDetector::startADC()
-{
- // We use the ADC irq to trigger DMA and the manual says
- // that in this case the NVIC for ADC must be disabled.
- NVIC_DisableIRQ(ADC_IRQn);
-
- // Power up the ADC and set PCLK
- LPC_SC->PCONP |= (1UL << 12); // enable power
- 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
-
- // Enable the ADC, 12MHz, ADC0.0
- LPC_ADC->ADCR = (1UL << 21);
- LPC_ADC->ADCR &= ~(255 << 8); // No clock divider (use the 12MHz directly)
-
- // Set the pin functions to ADC (use pin p15)
- LPC_PINCON->PINSEL1 &= ~(3UL << 14); /* P0.23, Mbed p15. */
- LPC_PINCON->PINSEL1 |= (1UL << 14);
-
- // Enable ADC irq flag (to DMA).
- // Note, don't set the individual flags,
- // just set the global flag.
- LPC_ADC->ADINTEN = 0x100;
-
- // (see page 586 of http://www.nxp.com/documents/user_manual/UM10360.pdf)
- // Disable burst mode
- LPC_ADC->ADCR &= ~(1 << 16);
- // Have the ADC convert based on timer 1
- LPC_ADC->ADCR |= (6 << 24); // Trigger on MAT1.0
- LPC_ADC->ADCR |= (1 << 27); // Falling edge
-
- // Set up timer 1
- LPC_SC->PCONP |= (1UL << 2); // Power on Timer1
- LPC_SC->PCLKSEL0 &= ~(3 << 4); // No clock divider (use 12MHz directly) (see page 57 of datasheet)
- LPC_TIM1->PR = 11; // TC clocks at 1MHz since we selected 12MHz above (see page 507 of datasheet)
- LPC_TIM1->MR0 = sampleInterval-1; // sampling interval in us
- LPC_TIM1->MCR = 3; // Reset TCR to zero on match
- LPC_TIM1->EMR = (3UL<<4)|1; // Make MAT1.0 toggle.
- //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.
-
- // Start the timer (which thus starts the ADC)
- LPC_TIM1->TCR=0;
- LPC_TIM1->TCR=1;
-}
-#endif // end if(not artificial samples mode)
-
-//============================================
-// Execution Control
-//============================================
-
-// Start acquiring and processing samples
-// Will run forever or until stop() is called
-void ToneDetector::run()
-{
- if(!readyToBegin)
- return;
- terminated = false;
- //printf("\nTone detector starting...\n");
- #ifdef recordStreaming
- #ifdef recordSamples
- printf("\tSample (V)");
- printf("\n");
- #endif
- #ifdef recordOutput
- for(uint8_t t = 0; t < numTones; t++)
- printf("\t%f Hz", targetTones[t]);
- printf("\n");
- #endif
- #endif
-
- // Set up initial buffer configuration
- samplesWriting = sampleBuffer0;
- samplesProcessing = sampleBuffer1;
- fillingBuffer0 = true;
- transferComplete = false;
- // Start periodically sampling
- #ifdef artificialSamplesMode
- initTestModeSamples(); // artificially create samples
- sampleTicker.attach_us(&toneDetector, &ToneDetector::tickerCallback, sampleInterval); // "sample" artificial samples at desired rate
- #else
- startDMA();
- if(!terminated) // If DMA got an error, terminated will be set true
- startADC();
- #endif
-
- #ifdef debugLEDs
- led1 = 1; // Indicate start of tone detection
- #endif
- #ifdef debugPins // after LEDs to be more accurate
- debugPin1 = 1; // Indicate start of tone detection
- #endif
-
- // Main loop
- // Wait for buffers to fill and then process them
- while(!terminated)
- {
- // Check if a buffer of samples is gathered and if so process it
- if(transferComplete)
- {
- transferComplete = false;
- processSamples();
- }
- }
-
- #ifdef debugPins // before LEDs to be more accurate
- debugPin1 = 0; // Indicate cessation of tone detection
- debugPin2 = 0; // Turn off indicator that at least two buffers were processed
- debugPin4 = 1; // Indicate completion
- #endif
- #ifdef debugLEDs
- led1 = 0; // Indicate cessation of tone detection
- led2 = 0; // Turn off indicator that at least one buffer was processed
- led4 = 1; // Indicate completion
- #endif
-}
-
-// Finish up (write results to file and whatnot)
-// This is separate method so that main program can time the running itself without this extra overhead
-void ToneDetector::finish()
-{
- //printf("Tone detector finished\n");
-
- #if defined(recordSamples) && !defined(recordStreaming)
- // Write saved samples to file
- LocalFileSystem local("local");
- FILE *foutSamples = fopen("/local/samples.wp", "w"); // Open "samples.wp" on the local file system for writing
- fprintf(foutSamples, "Sample (V)\n");
- uint16_t savedSamplesN = savedSamplesIndex;
- do
- {
- fprintf(foutSamples, "%f\n", toFloat(savedSamples[savedSamplesN])/4.0*3.3);
- savedSamplesN++;
- savedSamplesN %= numSavedSamples;
- } while(savedSamplesN != savedSamplesIndex);
- fclose(foutSamples);
- #endif // recordSamples && !recordStreaming
- #if defined(recordOutput) && !defined(recordStreaming)
- // Write saved outputs to file
- #ifndef recordSamples
- LocalFileSystem local("local");
- #endif // not recordSamples
- FILE *foutOutput = fopen("/local/out.wp", "w"); // Open "out.wp" on the local file system for writing
- for(uint8_t t = 0; t < numTones; t++)
- fprintf(foutOutput, "%f Hz\t", tones[t]);
- uint16_t savedTonePowersN = savedTonePowersIndex;
- do
- {
- for(uint8_t t = 0; t < numTones; t++)
- fprintf(foutOutput, "%ld \t", savedTonePowers[savedTonePowersN][t]);
- fprintf(foutOutput, "\n");
- savedTonePowersN++;
- savedTonePowersN %= numSavedTonePowers;
- } while(savedTonePowersN != savedTonePowersIndex);
- fclose(foutOutput);
- #endif // recordOutput
-}
-
-// Terminate the tone detector
-// Note: Will actually terminate after next time buffer1 is filled, so won't be instantaneous
-void ToneDetector::stop()
-{
- // Stop sampling
- #ifdef artificialSamplesMode
- sampleTicker.detach();
- #else
- lli[1]->Control = 0; // Make the DMA stop after next time buffer1 is filled
- while(!(!fillingBuffer0 && transferComplete)); // Wait for buffer1 to be filled
- LPC_TIM1->TCR=0; // Stop the timer (and thus the ADC)
- LPC_SC->PCONP &= ~(1UL << 2); // Power off the timer
- #endif
-
- // Stop the main loop
- terminated = true;
-}
-
-//============================================
-// Sampling / Processing
-//============================================
-
-// Acquire a new sample
-#ifdef artificialSamplesMode
-// If a buffer has been filled, swap buffers and signal the main thread to process it
-void ToneDetector::tickerCallback()
-{
- // Get a sample
- samplesWriting[sampleIndex] = testSamples[testSampleIndex];
- testSampleIndex++;
- testSampleIndex %= numTestSamples;
-
- // Increment sample index
- sampleIndex++;
- sampleIndex %= sampleWindow;
-
- // See if we just finished a buffer
- if(sampleIndex == 0)
- {
- // Swap writing and processing buffers
- // Let the main tone detector thread know that processing should take place
- if(fillingBuffer0)
- {
- samplesProcessing = sampleBuffer0;
- samplesWriting = sampleBuffer1;
- }
- else
- {
- samplesProcessing = sampleBuffer1;
- samplesWriting = sampleBuffer0;
- }
- transferComplete = true;
- fillingBuffer0 = !fillingBuffer0;
- }
-}
-#else // not artificial mode - we want real samples!
-// Callback for DMA channel 0
-void TC0_callback(void) // static method
-{
- // Swap writing and processing buffers used by main loop
- if(toneDetector.fillingBuffer0)
- {
- toneDetector.samplesProcessing = toneDetector.sampleBuffer0;
- toneDetector.samplesWriting = toneDetector.sampleBuffer1;
- }
- else
- {
- toneDetector.samplesProcessing = toneDetector.sampleBuffer1;
- toneDetector.samplesWriting = toneDetector.sampleBuffer0;
- }
- // Tell main() loop that this buffer is ready for processing
- toneDetector.fillingBuffer0 = !toneDetector.fillingBuffer0;
- toneDetector.transferComplete = true;
-
- // Clear DMA IRQ flags.
- if(toneDetector.dma.irqType() == MODDMA::TcIrq) toneDetector.dma.clearTcIrq();
- if(toneDetector.dma.irqType() == MODDMA::ErrIrq) toneDetector.dma.clearErrIrq();
-}
-
-// Configuration callback on Error for channel 0
-void ERR0_callback(void) // static method
-{
- // Stop sampling
- LPC_TIM1->TCR = 0; // Stop the timer (and thus the ADC)
- LPC_SC->PCONP &= ~(1UL << 2); // Power off the timer
-
- // Stop the main loop (don't call stop() since that would wait for next buffer to fill)
- toneDetector.terminated = true;
-
- error("Oh no! My Mbed EXPLODED! :( Only kidding, go find the problem (DMA chan 0)");
-}
-#endif
-
-// Goertzelize the process buffer
-void ToneDetector::processSamples()
-{
- #ifdef debugLEDs
- if(fillingBuffer0)
- led2 = 1; // Indicate that at least two buffers have been recorded
- led3 = 1; // Indicate start of processing
- #endif
- #ifdef debugPins // after LEDs and timer to be more accurate
- if(fillingBuffer0)
- debugPin2 = 1; // Indicate that at least two buffers have been recorded
- debugPin3 = 1; // Indicate start of processing
- #endif
-
- // Create variables for storing the Goertzel series
- int32_t s0, s1, s2;
- volatile int32_t newTonePower;
- // Create variables for getting max input value
- int32_t sample = 0;
- uint32_t signalLevel = 0;
- // For each desired tone, compute power and then reset the Goertzel
- for(uint8_t i = 0; i < numTones; i++)
- {
- // Reset
- s0 = 0;
- s1 = 0;
- s2 = 0;
- // Compute the Goertzel series
- for(uint16_t n = 0; n < sampleWindow; n++)
- {
- // Note: bottom 4 bits from ADC indicate channel, top 12 are the actual data
- // Note: Assuming Q10 format, we are automatically scaling the ADC value to [0, 4] range
- sample = ((int32_t)((samplesProcessing[n] >> 4) & 0xFFF));
- // Get signal level indicator
- if(i == 0)
- {
- if(sample-2048 >= 0)
- signalLevel += (sample-2048);
- else
- signalLevel += (2048-sample);
- }
- // TODO check the effect of this subtraction?
- //sample -= 2048;
- s0 = sample + fixedPointMultiply(goertzelCoefficients[i], s1) - s2;
- s2 = s1;
- s1 = s0;
- }
- // Compute the power
- newTonePower = fixedPointMultiply(s2,s2) + fixedPointMultiply(s1,s1) - fixedPointMultiply(fixedPointMultiply(goertzelCoefficients[i],s1),s2);
- // Update the running sum
- tonePowersSum[i] -= tonePowers[i][tonePowersWindowIndex];
- tonePowersSum[i] += newTonePower;
- // Update the history of powers
- tonePowers[i][tonePowersWindowIndex] = newTonePower;
- }
- // See if first circular buffer has been filled
- readyToThreshold = readyToThreshold || (tonePowersWindowIndex == tonePowersWindow-1);
- // Deliver results if a callback function was provided
- if(callbackFunction != 0 && readyToThreshold)
- callbackFunction(tonePowersSum, signalLevel >> 7); // divide signal level by 128 is basically making it an average (125 samples/buffer)
- #ifdef streamAcousticControlLog
- acousticControlLogToStream[0] = tonePowers[0][tonePowersWindowIndex];
- acousticControlLogToStream[1] = tonePowers[1][tonePowersWindowIndex];
- acousticControlLogToStream[2] = signalLevel >> 7;
- #endif
- #ifdef debugLEDs
- led3 = 0; // Indicate completion of processing
- #endif
- #ifdef recordSamples
- #ifdef recordStreaming
- for(uint16_t n = 0; n < sampleWindow; n++)
- printf("%ld\n", (samplesProcessing[n] >> 4) & 0xFFF);
- #else
- for(uint16_t n = 0; n < sampleWindow; n++)
- {
- savedSamples[savedSamplesIndex++] = (samplesProcessing[n] >> 4) & 0xFFF;
- savedSamplesIndex %= numSavedSamples;
- }
- #endif
- #endif
- #ifdef recordOutput
- #ifdef recordStreaming
- for(uint8_t t = 0; t < numTones; t++)
- printf("%ld\t", tonePowers[t][tonePowersWindowIndex] >> 10); // used to shift 10
- printf("\n");
- #else
- for(uint8_t t = 0; t < numTones; t++)
- {
- savedTonePowers[savedTonePowersIndex][t] = tonePowers[t][tonePowersWindowIndex];
- }
- savedTonePowersIndex++;
- savedTonePowersIndex %= numSavedTonePowers;
- #endif
- #endif
- // Increment window index (circularly)
- tonePowersWindowIndex = (tonePowersWindowIndex+1) % tonePowersWindow;
-
- #ifdef debugPins // before LEDs, timer, and recordSamples to be more accurate
- debugPin3 = 0; // Indicate completion of processing
- #endif
-}
-
-int32_t* ToneDetector::getTonePowers()
-{
- return tonePowersSum;
-}
-
-//============================================
-// Testing / Debugging
-//============================================
-
-#ifdef artificialSamplesMode
-// Use artificial samples, either from a file or from summing cosine waves of given frequencies
-// Samples in a file will be interpreted as volts, so should be in range [0, 3.3]
-void ToneDetector::initTestModeSamples()
-{
- #ifdef sampleFilename
- LocalFileSystem local("local");
- printf("Using samples from file: "); printf(sampleFilename); printf("\n");
- FILE *fin = fopen(sampleFilename, "r"); // Open "setup.txt" on the local file system for read
- if(fin == 0)
- {
- printf(" *** Cannot open file! ***\n");
- wait_ms(3000);
- numTestSamples = 1;
- testSamples = new uint32_t[numTestSamples];
- testSamples[0] = 0;
- testSampleIndex = 0;
- return;
- }
- // See how long the file is
- int numTestSamples = 0;
- float val;
- float maxVal = 0;
- float minVal = 0;
- float avgVal = 0;
- int res = fscanf(fin, "%d\n", &val);
- while(res > 0)
- {
- numTestSamples++;
- res = fscanf(fin, "%f\n", &val);
- if(val > maxVal)
- maxVal = val;
- if(val < minVal)
- minVal = val;
- avgVal = numTestSamples > 1 ? (avgVal + val)/2.0 : val;
- }
- printf(" Found %d samples in the file, max %f, min %f, avg %f\n", numTestSamples, maxVal, minVal, avgVal);
- if(minVal < 0)
- printf(" WARNING: File supposed to represent voltage input to ADC, so negative numbers will be interpreted as 0\n");
- if(maxVal > 3.3)
- printf(" WARNING: File supposed to represent voltage input to ADC, so numbers greater than 3.3 will be interpreted as 3.3\n");
- fclose(fin);
- // Read the samples
- testSamples = new uint32_t[numTestSamples];
- fin = fopen(sampleFilename, "r"); // Open "setup.txt" on the local file system for read
- for(int i = 0; i < numTestSamples; i++)
- {
- // Read the voltage
- fscanf(fin, "%f\n", &val);
- // Clip it like the ADC would
- val = val > 3.3 ? 3.3 : val;
- val = val < 0 ? 0 : val;
- // Convert voltage to 12-bit ADC reading
- testSamples[i] = val/3.3*4096.0;
- // Shift it by 4 to mimic what the DMA would write for a reading (lower 4 would indicate ADC channel)
- testSamples[i] = testSamples[i] << 4;
- }
- fclose(fin);
- testSampleIndex = 0;
- sampleIndex = 0;
-
- #else // not using file for samples, will create a sum of cosine waves instead
-
- numTestSamples = 1000;
- testSamples = new uint32_t[numTestSamples];
- testSampleIndex = 0;
-
- // Adjust overall amplitude and offset - make sure it's nonnegative
- float amplitude = 1; // volts
- float baseline = 1.5; // volts
- // Print out the frequencies being used
- float frequencies[] = sumSampleFrequencies; // Test signal will be a summation of cosines at these frequencies (in Hz)
- printf("Using samples from cosines with the following frequencies:\n");
- for(int f = 0; f < sizeof(frequencies)/sizeof(float); f++)
- printf(" %f\n", frequencies[f]);
- printf("\n");
-
- // Create the samples
- float sampleFrequency = 1000.0/((double)sampleInterval/1000.0);
- for(uint16_t n = 0; n < numTestSamples; n++)
- {
- float nextSample = 0;
- // Sum the frequencies
- for(int f = 0; f < sizeof(frequencies)/sizeof(float); f++)
- nextSample += cos(2.0*PI*frequencies[f]*(float)n/sampleFrequency);
- // Normalize
- nextSample /= (float)(sizeof(frequencies)/sizeof(float));
- // Amplify
- nextSample *= amplitude;
- // Positivify
- nextSample += baseline;
- // Convert to 12-bit ADC reading
- testSamples[n] = ((uint32_t)(nextSample/3.3*4095.0));
- // Shift it by 4 to mimic what the DMA would write for a reading (lower 4 would indicate ADC channel)
- testSamples[n] = testSamples[n] << 4;
- }
- sampleIndex = 0;
- #endif // which sample mode
-}
-#endif // artificialSamplesMode
-
-#endif // #ifdef acousticControl
--- a/robotic_fish_6/AcousticControl/ToneDetector.h Fri Dec 03 23:03:20 2021 +0000
+++ /dev/null Thu Jan 01 00:00:00 1970 +0000
@@ -1,182 +0,0 @@
-/**
- * Author: Joseph DelPreto
- * A class for sampling a signal and detecting whether certain frequencies are present
- * Implements the Goertzel algorithm for tone detection
- * Uses fixed-point arithmetic to a > 10x speedup over a floating-point implementation
- * Uses Q15 number format
- *
- * Can easily configure the sampling frequency, the tones to detect, and the length of the buffer window
- * Can set a callback function to be called at the end of each buffer processing to see the relative powers of each target tone
- * Make sure the callback function execution time, plus the time to process a buffer of samples, takes less time than it takes to acquire the buffer!
- * You can check how long it takes to process a buffer of samples by enabling "timeProcess" below then using initProcessTimer() and getProcessTimes()
- *
- * Debugging options such as LEDs to indicate status are also available
- * Can also choose to time each processing stage to ensure it is not taking longer the time needed to acquire the buffer of samples
- * Can also record samples and write them to a file (max 1000 will be recorded)
- * LED pattern if enabled:
- * LED1 indicates tone detection in progress
- * LED2 indicates at least two buffers have been filled
- * LED3 turns on when processing starts and off when it finishes (so lower duty cycle is better)
- * LED4 turns on when tone detection is stopped (and then other LEDs turn off)
- * Debug pins will go high/low in same pattern as described above for LEDs, but using pins p5-p8
- *
- * Test mode can be enabled, in which case samples can be artificially generated instead of using the ADC
- *
- * Currently only set up for one instance of ToneDetector to be in operation
- * Use the object toneDetector, declared in this file, for all of your tone detecting needs!
- *
- */
-
-#define acousticControl
-
-#ifdef acousticControl
-
-#ifndef TONE_DETECTOR_H
-#define TONE_DETECTOR_H
-
-//#define streamAcousticControlLog // for every buffer, streams goertzel powers f1, goerztel powers f2, signal level, gain, fish state/events over Serial
-
-// Configuration
-#define sampleInterval 4 // us between samples
-#define sampleWindow 125 // number of samples in a Goertzel buffer
-#define numTones 2
-#define numFSKGroups 1
-static const double targetTones[numTones] = {36000, 30000}; // Tones to detect (in Hz): first tone is a 0 bit second is a 1 bit
-
-// Test / Debugging options
-//#define artificialSamplesMode // if this is defined, should define either sampleFilename OR sumSampleFrequencies
-//#define sampleFilename "/local/2tone11.txt"
-//#define sumSampleFrequencies {10000,30000} // will be used to initialize float[] array. Test signal will be summation of cosines with these frequencies (in Hz)
-
-#define debugLEDs
-#define debugPins
-#define recordStreaming // print to COM each sample/output (save everything), as opposed to only write last few hundred to a file (but faster since don't write to file while processing)
- // note that either recordOutput or recordSamples must be undefined to actually record anything
-//#define recordOutput // save tone powers - will either stream to COM or save the last numSavedTonePowers powers to a file (based on recordStreaming)
-//#define recordSamples // save samples - will either stream to COM or save the last numSavedSamples samples to a file (based on recordStreaming)
-
-
-#if defined(recordSamples) && !defined(recordStreaming)
-#define numSavedSamples 1000
-#endif
-#if defined(recordOutput) && !defined(recordStreaming)
-#define numSavedTonePowers 250
-#endif
-
-// Will use fixed-point arithmetic for speed
-// numbers will be int32_t with 10 bits as fractional portion
-// note: this means the 12 bits from the ADC can be used directly, implicitly scaling them to be floats in range [0,4]
-// (so if you want to change the Q format, make sure to shift the samples accordingly in the processSamples() method)
-#define toFixedPoint(floatNum) ((int32_t)((float)(floatNum) * 1024.0f))
-#define toFloat(fixedPointNum) ((float)(fixedPointNum)/1024.0f)
-#define fixedPointMultiply(a,b) ((int32_t)(((int64_t)a * (int64_t)b) >> 10))
-
-// Constants
-#define tonePowersWindow 32 // change to 5 for threshold1
-
-// Include files
-#include <stdint.h> // for types like int32_t
-#include <math.h> // for cos // TODO remove this once samples are real samples
-#define PI 3.1415926 // not included with math.h for some reason
-#include "mbed.h" // for the ticker
-#ifndef artificialSamplesMode
-#include "MODDMA.h" // DMA for reading the ADC
-#endif
-
-// Debugging
-#ifdef debugLEDs
-static DigitalOut led1(LED1);
-static DigitalOut led2(LED2);
-static DigitalOut led3(LED3);
-static DigitalOut led4(LED4);
-#endif
-#ifdef debugPins
-static DigitalOut debugPin1(p5);
-static DigitalOut debugPin2(p6);
-static DigitalOut debugPin3(p7);
-static DigitalOut debugPin4(p8);
-#endif
-
-// Define the ToneDetector!
-class ToneDetector
-{
- public:
- // Initialization
- ToneDetector();
- void init();
- void setCallback(void (*myFunction)(int32_t* tonePowers, uint32_t signalLevel));
- // Execution Control
- virtual void run(); // main loop, runs forever or until stop() is called
- virtual void stop(); // stop the main loop
- void finish(); // write final output to files and whatnot
- volatile bool terminated; // indicates main loop should stop (may be set by error callback or by stop())
- // Sampling / Processing (these are public to allow DMA callbacks to access them)
- volatile bool fillingBuffer0; // Whether sampling is currently writing to buffer 0 or buffer 1
- volatile bool transferComplete; // Signals that samplesProcessing is ready to be processed
- uint32_t sampleBuffer0[sampleWindow]; // Buffer of samples used by one DMA operation
- uint32_t sampleBuffer1[sampleWindow]; // Buffer of samples used by second DMA operation
- volatile uint32_t* samplesWriting; // Pointer to the buffer to which we should write (alternates between buffer0 and buffer1)
- volatile uint32_t* samplesProcessing; // Pointer to the buffer which we should process (alternates between buffer0 and buffer1)
- int32_t* getTonePowers(); // Get the most recent tone powers
- #ifndef artificialSamplesMode
- MODDMA dma; // DMA controller
- #endif
-
- private:
- // Initialization
- bool readyToBegin; // Whether sample window, sample interval, and tones have all been specified
- #ifndef artificialSamplesMode
- MODDMA_Config *dmaConf; // Overall DMA configuration
- MODDMA_LLI *lli[2]; // Linked List Items for chaining DMA operations (will have one for each ADC buffer)
- void startDMA();
- void startADC();
- #endif
- // Sampling
- // Goertzel Processing (arrays will have an element per desired tone)
- // Tone detecting
- int32_t goertzelCoefficients[numTones]; // Pre-computed coefficients based on target tones
- int32_t tonePowersSum[numTones]; // Result of Goertzel algorithm (summed over tonePowersWindow to smooth results)
- int32_t tonePowers[numTones][tonePowersWindow]; // For each tone, store the most recent tonePowersWindow powers (in circular buffers)
- uint8_t tonePowersWindowIndex; // Index into the circular buffers of tonePowers
- bool readyToThreshold; // Whether thresholding is ready, or first buffer is still being filled
- void (*callbackFunction)(int32_t* tonePowers, uint32_t signalLevel); // Called when new powers are computed
- void processSamples(); // Process a buffer of samples to detect tones
-
- // Testing / Debugging
- #ifdef artificialSamplesMode
- Ticker sampleTicker; // Triggers a sample to be read from the pre-filled array
- void initTestModeSamples(); // Creates the samples, either from a file or by summing tones
- void tickerCallback(); // Called each time a sample is read
- uint32_t* testSamples; // Array of pre-stored sample values to use
- uint16_t numTestSamples; // Length of the testSamples array (need not be same as buffer length)
- volatile uint16_t testSampleIndex; // Current index into testSamples
- volatile uint16_t sampleIndex; // Index into current sample buffer
- #endif
- #ifndef recordStreaming
- #ifdef recordSamples
- uint16_t savedSamplesIndex;
- uint32_t savedSamples[numSavedSamples];
- #endif
- #ifdef recordOutput
- uint16_t savedTonePowersIndex;
- int32_t savedTonePowers[numSavedTonePowers][numTones];
- #endif
- #endif
-};
-
-// DMA IRQ callback methods
-void TC0_callback(); // Callback for when DMA channel 0 has filled a buffer
-void ERR0_callback(); // Error on dma channel 0
-
-// Create a static instance of ToneDetector to be used by anyone doing detection
-// This allows the ticker callback to be a member function
-extern ToneDetector toneDetector;
-#ifdef streamAcousticControlLog
-extern int32_t acousticControlLogToStream[5]; // goertzel powers f1, goerztel powers f2, signal level, gain, events (events is deprecated)
-#endif
-
-#endif // ifndef TONE_DETECTOR_H
-
-#endif // #ifdef acousticControl
-
-
--- a/robotic_fish_6/BrushlessMotor.lib Fri Dec 03 23:03:20 2021 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,1 +0,0 @@ -http://developer.mbed.org/teams/jetfishteam/code/ESC/#b27ff453747f
--- a/robotic_fish_6/FishController.h Fri Dec 03 23:03:20 2021 +0000 +++ b/robotic_fish_6/FishController.h Fri Dec 03 23:09:57 2021 +0000 @@ -17,7 +17,7 @@ #include "ButtonBoard.h" #ifdef FISH4 #include "Servo.h" -#include "esc.h" // brushless motor controller +//#include "esc.h" // brushless motor controller #endif #ifdef FISH6 #include "Servo.h"
--- a/robotic_fish_6/MODDMA.lib Fri Dec 03 23:03:20 2021 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,1 +0,0 @@ -https://developer.mbed.org/teams/jetfishteam/code/MODDMA/#108ae3896ebb
--- a/robotic_fish_6/main.cpp Fri Dec 03 23:03:20 2021 +0000 +++ b/robotic_fish_6/main.cpp Fri Dec 03 23:09:57 2021 +0000 @@ -2,7 +2,7 @@ // NOTE look at the below h files to define whether that control mode is enabled #include "mbed.h" -#include "AcousticControl/AcousticController.h" // also need to define acousticControl in ToneDetector.h +//#include "AcousticControl/AcousticController.h" // also need to define acousticControl in ToneDetector.h #include "SerialControl/SerialController.h" #include "ROSControl/ROSController.h"
Repository toolbox
| Export to desktop IDE |
| Build repository |
Repository details
| Type: | Program |
|---|---|
| Created: | 03 Dec 2021 |
| Imports: | 1 |
| Forks: | 0 |
| Commits: | 4 |
| Dependents: | 0 |
| Dependencies: | 3 |
| Followers: | 2 |
