Juan Salazar / robotic_fish_7

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

Dependencies:   mbed ESC mbed MODDMA

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

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

Who changed what in which revision?

UserRevisionLine numberNew contents of line
juansal12 0:c3a329a5b05d 1 /*
juansal12 0:c3a329a5b05d 2 * Author: Joseph DelPreto
juansal12 0:c3a329a5b05d 3 */
juansal12 0:c3a329a5b05d 4
juansal12 0:c3a329a5b05d 5 #include "AcousticController.h"
juansal12 0:c3a329a5b05d 6
juansal12 0:c3a329a5b05d 7 #ifdef acousticControl
juansal12 0:c3a329a5b05d 8
juansal12 0:c3a329a5b05d 9 // The static instance
juansal12 0:c3a329a5b05d 10 AcousticController acousticController;
juansal12 0:c3a329a5b05d 11
juansal12 0:c3a329a5b05d 12 // Map received state to fish values
juansal12 0:c3a329a5b05d 13 const float pitchLookupAcoustic[] = {0.2, 0.3, 0.4, 0.5, 0.6, 0.7, 0.8}; // [0.2 - 0.8]
juansal12 0:c3a329a5b05d 14 const float yawLookupAcoustic[] = {-1, -0.7, -0.5, 0, 0.5, 0.7, 1}; // [-1, 1]
juansal12 0:c3a329a5b05d 15 const float thrustLookupAcoustic[] = {0, 0.25, 0.50, 0.75};
juansal12 0:c3a329a5b05d 16 const float frequencyLookupAcoustic[] = {0.0000009, 0.0000012, 0.0000014, 0.0000016}; // cycles/us // NOTE also update periodHalfLookup if you update these values
juansal12 0:c3a329a5b05d 17 const float periodHalfLookupAcoustic[] = {555555, 416666, 357142, 312500}; // 1/(2*frequencyLookup) -> us
juansal12 0:c3a329a5b05d 18
juansal12 0:c3a329a5b05d 19 // AGC definition
juansal12 0:c3a329a5b05d 20 const uint8_t agcGains[] = {1, 1, 2, 5, 10, 20, 50, 100}; // the possible gains of the AGC
juansal12 0:c3a329a5b05d 21 const uint8_t agcGainsLength = sizeof(agcGains)/sizeof(agcGains[0]);
juansal12 0:c3a329a5b05d 22 const uint16_t signalLevelBufferLength = (agcUpdateWindow)/(sampleWindow*sampleInterval); // first constant is window length for updating agc in microseconds
juansal12 0:c3a329a5b05d 23 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
juansal12 0:c3a329a5b05d 24
juansal12 0:c3a329a5b05d 25 // Fish timeout
juansal12 0:c3a329a5b05d 26 const uint16_t fishTimeoutAcousticBuffers = fishTimeoutAcousticWindow/(sampleWindow*sampleInterval); // first constant is timeout in microseconds
juansal12 0:c3a329a5b05d 27
juansal12 0:c3a329a5b05d 28 // Apparently it's hard to get a proper function pointer to a member function
juansal12 0:c3a329a5b05d 29 // so this dummy method will be set as the toneDetector callback function
juansal12 0:c3a329a5b05d 30 void toneDetectorCallback(int32_t* newTonePowers, uint32_t signalLevel)
juansal12 0:c3a329a5b05d 31 {
juansal12 0:c3a329a5b05d 32 acousticController.processTonePowers(newTonePowers, signalLevel);
juansal12 0:c3a329a5b05d 33 }
juansal12 0:c3a329a5b05d 34
juansal12 0:c3a329a5b05d 35 // Constructor
juansal12 0:c3a329a5b05d 36 AcousticController::AcousticController(Serial* usbSerialObject /* = NULL */) :
juansal12 0:c3a329a5b05d 37 // Initialize variables
juansal12 0:c3a329a5b05d 38 bufferCount(0),
juansal12 0:c3a329a5b05d 39 lastDataWord(0),
juansal12 0:c3a329a5b05d 40 timeSinceGoodWord(0),
juansal12 0:c3a329a5b05d 41 streamCurFishStateAcoustic(0),
juansal12 0:c3a329a5b05d 42 streamCurFishStateEventAcoustic(0)
juansal12 0:c3a329a5b05d 43 {
juansal12 0:c3a329a5b05d 44 // AGC Pins
juansal12 0:c3a329a5b05d 45 gain0 = new DigitalOut(agcPin0);
juansal12 0:c3a329a5b05d 46 gain1 = new DigitalOut(agcPin1);
juansal12 0:c3a329a5b05d 47 gain2 = new DigitalOut(agcPin2);
juansal12 0:c3a329a5b05d 48 agc[0] = gain0;
juansal12 0:c3a329a5b05d 49 agc[1] = gain1;
juansal12 0:c3a329a5b05d 50 agc[2] = gain2;
juansal12 0:c3a329a5b05d 51 #ifdef AGCLeds
juansal12 0:c3a329a5b05d 52 agcLED0 = new DigitalOut(LED1);
juansal12 0:c3a329a5b05d 53 agcLED1 = new DigitalOut(LED2);
juansal12 0:c3a329a5b05d 54 agcLED2 = new DigitalOut(LED3);
juansal12 0:c3a329a5b05d 55 agcLEDs[0] = agcLED0;
juansal12 0:c3a329a5b05d 56 agcLEDs[1] = agcLED1;
juansal12 0:c3a329a5b05d 57 agcLEDs[2] = agcLED2;
juansal12 0:c3a329a5b05d 58 #endif
juansal12 0:c3a329a5b05d 59
juansal12 0:c3a329a5b05d 60 // Low battery
juansal12 0:c3a329a5b05d 61 lowBatteryVoltageInput = new DigitalIn(lowBatteryVoltagePin);
juansal12 0:c3a329a5b05d 62
juansal12 0:c3a329a5b05d 63 // Misc
juansal12 0:c3a329a5b05d 64 #ifdef artificialPowers
juansal12 0:c3a329a5b05d 65 FILE* finPowers; = fopen("/local/powers.wp", "r");
juansal12 0:c3a329a5b05d 66 #endif
juansal12 0:c3a329a5b05d 67
juansal12 0:c3a329a5b05d 68 // Complete initialization
juansal12 0:c3a329a5b05d 69 init(usbSerialObject);
juansal12 0:c3a329a5b05d 70 }
juansal12 0:c3a329a5b05d 71
juansal12 0:c3a329a5b05d 72 // Initialization
juansal12 0:c3a329a5b05d 73 void AcousticController::init(Serial* usbSerialObject /* = NULL */)
juansal12 0:c3a329a5b05d 74 {
juansal12 0:c3a329a5b05d 75 // Create usb serial object or use provided one
juansal12 0:c3a329a5b05d 76 if(usbSerialObject == NULL)
juansal12 0:c3a329a5b05d 77 {
juansal12 0:c3a329a5b05d 78 usbSerialObject = new Serial(USBTX, USBRX);
juansal12 0:c3a329a5b05d 79 usbSerialObject->baud(serialDefaultBaudUSB);
juansal12 0:c3a329a5b05d 80 }
juansal12 0:c3a329a5b05d 81 usbSerial = usbSerialObject;
juansal12 0:c3a329a5b05d 82 // Miscellaneous
juansal12 0:c3a329a5b05d 83 bufferCount = 0;
juansal12 0:c3a329a5b05d 84 lastDataWord = 0;
juansal12 0:c3a329a5b05d 85 timeSinceGoodWord = 0;
juansal12 0:c3a329a5b05d 86
juansal12 0:c3a329a5b05d 87 // Will check if battery is low in every buffer callback
juansal12 0:c3a329a5b05d 88 lowBatteryVoltageInput->mode(PullUp);
juansal12 0:c3a329a5b05d 89 //lowBatteryInterrupt.fall(&lowBatteryCallback);
juansal12 0:c3a329a5b05d 90
juansal12 0:c3a329a5b05d 91 // TODO remove this?
juansal12 0:c3a329a5b05d 92 wait(1.5);
juansal12 0:c3a329a5b05d 93
juansal12 0:c3a329a5b05d 94 // Configure the tone detector
juansal12 0:c3a329a5b05d 95 toneDetector.setCallback(&toneDetectorCallback);
juansal12 0:c3a329a5b05d 96 toneDetector.init();
juansal12 0:c3a329a5b05d 97
juansal12 0:c3a329a5b05d 98 // Clear detection arrays
juansal12 0:c3a329a5b05d 99 #if defined(threshold2)
juansal12 0:c3a329a5b05d 100 for(int t = 0; t < numTones; t++)
juansal12 0:c3a329a5b05d 101 {
juansal12 0:c3a329a5b05d 102 detectSums[t] = 0;
juansal12 0:c3a329a5b05d 103 for(uint8_t p = 0; p < detectWindow; p++)
juansal12 0:c3a329a5b05d 104 tonesPresent[p][t] = 0;
juansal12 0:c3a329a5b05d 105 }
juansal12 0:c3a329a5b05d 106 detectWindowIndex = 0;
juansal12 0:c3a329a5b05d 107 readyToThreshold = false;
juansal12 0:c3a329a5b05d 108 powerHistoryIndex = 0;
juansal12 0:c3a329a5b05d 109 powerHistoryDetectIndex = (powerHistoryLength - 1) - (powerHistoryDetectWindow-1) + 1; // assumes powerHistoryLength >= detectWindow > 1
juansal12 0:c3a329a5b05d 110 for(uint8_t t = 0; t < numTones; t++)
juansal12 0:c3a329a5b05d 111 {
juansal12 0:c3a329a5b05d 112 powerHistorySumDetect[t] = 0;
juansal12 0:c3a329a5b05d 113 powerHistorySumNoDetect[t] = 0;
juansal12 0:c3a329a5b05d 114 powerHistoryMaxDetect[t] = 0;
juansal12 0:c3a329a5b05d 115 powerHistoryMaxNoDetect[t] = 0;
juansal12 0:c3a329a5b05d 116 for(uint16_t p = 0; p < powerHistoryLength; p++)
juansal12 0:c3a329a5b05d 117 {
juansal12 0:c3a329a5b05d 118 powerHistory[p][t] = 0;
juansal12 0:c3a329a5b05d 119 }
juansal12 0:c3a329a5b05d 120 }
juansal12 0:c3a329a5b05d 121 #elif defined(threshold1)
juansal12 0:c3a329a5b05d 122 for(int t = 0; t < numTones; t++)
juansal12 0:c3a329a5b05d 123 {
juansal12 0:c3a329a5b05d 124 detectSums[t] = 0;
juansal12 0:c3a329a5b05d 125 }
juansal12 0:c3a329a5b05d 126 powerHistoryIndex = 0;
juansal12 0:c3a329a5b05d 127 powerHistoryDetectIndex = (powerHistoryLength - 1) - (detectWindow-1) + 1; // assumes powerHistoryLength >= detectWindow > 1
juansal12 0:c3a329a5b05d 128 for(uint8_t t = 0; t < numTones; t++)
juansal12 0:c3a329a5b05d 129 {
juansal12 0:c3a329a5b05d 130 powerHistorySum[t] = 0;
juansal12 0:c3a329a5b05d 131 powerHistoryMax[t] = 0;
juansal12 0:c3a329a5b05d 132 for(uint16_t p = 0; p < powerHistoryLength; p++)
juansal12 0:c3a329a5b05d 133 {
juansal12 0:c3a329a5b05d 134 powerHistory[p][t] = 0;
juansal12 0:c3a329a5b05d 135 }
juansal12 0:c3a329a5b05d 136 }
juansal12 0:c3a329a5b05d 137 readyToThreshold = false;
juansal12 0:c3a329a5b05d 138 #endif
juansal12 0:c3a329a5b05d 139 #ifdef singleDataStream
juansal12 0:c3a329a5b05d 140 #ifdef saveData
juansal12 0:c3a329a5b05d 141 dataIndex = 0;
juansal12 0:c3a329a5b05d 142 #endif
juansal12 0:c3a329a5b05d 143 #else
juansal12 0:c3a329a5b05d 144 #ifdef saveData
juansal12 0:c3a329a5b05d 145 dataWordIndex = 0;
juansal12 0:c3a329a5b05d 146 #endif
juansal12 0:c3a329a5b05d 147 dataBitIndex = 0;
juansal12 0:c3a329a5b05d 148 interWord = false;
juansal12 0:c3a329a5b05d 149 #endif
juansal12 0:c3a329a5b05d 150 waitingForEnd = false;
juansal12 0:c3a329a5b05d 151 periodIndex = 0;
juansal12 0:c3a329a5b05d 152 fskIndex = 0;
juansal12 0:c3a329a5b05d 153
juansal12 0:c3a329a5b05d 154 // Initialize adjustable gain control
juansal12 0:c3a329a5b05d 155 signalLevelBufferIndex = 0;
juansal12 0:c3a329a5b05d 156 signalLevelSum = 0;
juansal12 0:c3a329a5b05d 157 currentGainIndex = 4;
juansal12 0:c3a329a5b05d 158 for(uint8_t i = 0; i < sizeof(agc)/sizeof(agc[0]); i++)
juansal12 0:c3a329a5b05d 159 {
juansal12 0:c3a329a5b05d 160 agc[i]->write(currentGainIndex & (1 << i));
juansal12 0:c3a329a5b05d 161 #ifdef AGCLeds
juansal12 0:c3a329a5b05d 162 agcLEDs[i].write(currentGainIndex & (1 << i));
juansal12 0:c3a329a5b05d 163 #endif
juansal12 0:c3a329a5b05d 164 }
juansal12 0:c3a329a5b05d 165 }
juansal12 0:c3a329a5b05d 166
juansal12 0:c3a329a5b05d 167
juansal12 0:c3a329a5b05d 168 // Called by toneDetector when new tone powers are computed (once per buffer)
juansal12 0:c3a329a5b05d 169 void AcousticController::processTonePowers(int32_t* newTonePowers, uint32_t signalLevel)
juansal12 0:c3a329a5b05d 170 {
juansal12 0:c3a329a5b05d 171 #ifdef streamAcousticControlLog
juansal12 0:c3a329a5b05d 172 acousticControlLogToStream[4] = -10;
juansal12 0:c3a329a5b05d 173 #endif
juansal12 0:c3a329a5b05d 174 // Check for low battery and if so terminate tone detector
juansal12 0:c3a329a5b05d 175 if(lowBatteryVoltageInput == 0)
juansal12 0:c3a329a5b05d 176 {
juansal12 0:c3a329a5b05d 177 lowBatteryCallback();
juansal12 0:c3a329a5b05d 178 return;
juansal12 0:c3a329a5b05d 179 }
juansal12 0:c3a329a5b05d 180 // See if we're done and if so terminate the tone detector
juansal12 0:c3a329a5b05d 181 if(bufferCount == loopCount)
juansal12 0:c3a329a5b05d 182 {
juansal12 0:c3a329a5b05d 183 #ifndef infiniteLoopAcoustic
juansal12 0:c3a329a5b05d 184 toneDetector.stop();
juansal12 0:c3a329a5b05d 185 return;
juansal12 0:c3a329a5b05d 186 #else
juansal12 0:c3a329a5b05d 187 //bufferCount = 0;
juansal12 0:c3a329a5b05d 188 #endif
juansal12 0:c3a329a5b05d 189 }
juansal12 0:c3a329a5b05d 190 bufferCount++;
juansal12 0:c3a329a5b05d 191 // See if we're in autonomous mode and if so just let it be
juansal12 0:c3a329a5b05d 192 if(fishController.autoMode)
juansal12 0:c3a329a5b05d 193 return;
juansal12 0:c3a329a5b05d 194
juansal12 0:c3a329a5b05d 195 periodIndex++;
juansal12 0:c3a329a5b05d 196 timeSinceGoodWord++;
juansal12 0:c3a329a5b05d 197
juansal12 0:c3a329a5b05d 198 #if defined(threshold2)
juansal12 0:c3a329a5b05d 199 // Update threshold window state for each tone
juansal12 0:c3a329a5b05d 200 for(uint8_t t = 0; t < numTones; t++)
juansal12 0:c3a329a5b05d 201 {
juansal12 0:c3a329a5b05d 202 // Update our history of powers for this tone
juansal12 0:c3a329a5b05d 203 powerHistory[powerHistoryIndex][t] = (newTonePowers[t] >> 5);
juansal12 0:c3a329a5b05d 204 // Compute max/sum of the current window (only for frequencies we currently anticipate)
juansal12 0:c3a329a5b05d 205 if((t == fskIndex*2 || t == fskIndex*2+1) && !waitingForEnd)
juansal12 0:c3a329a5b05d 206 {
juansal12 0:c3a329a5b05d 207 powerHistoryMaxDetect[t] = 0;
juansal12 0:c3a329a5b05d 208 powerHistoryMaxNoDetect[t] = 0;
juansal12 0:c3a329a5b05d 209 powerHistorySumDetect[t] = 0;
juansal12 0:c3a329a5b05d 210 powerHistorySumNoDetect[t] = 0;
juansal12 0:c3a329a5b05d 211 // Look at non-detection zone
juansal12 0:c3a329a5b05d 212 for(uint16_t p = ((powerHistoryIndex+1)%powerHistoryLength); p != powerHistoryDetectIndex; p=(p+1)%powerHistoryLength)
juansal12 0:c3a329a5b05d 213 {
juansal12 0:c3a329a5b05d 214 powerHistorySumNoDetect[t] += powerHistory[p][t];
juansal12 0:c3a329a5b05d 215 if(powerHistory[p][t] > powerHistoryMaxNoDetect[t])
juansal12 0:c3a329a5b05d 216 powerHistoryMaxNoDetect[t] = powerHistory[p][t];
juansal12 0:c3a329a5b05d 217 }
juansal12 0:c3a329a5b05d 218 // Look at detection zone
juansal12 0:c3a329a5b05d 219 for(uint16_t p = powerHistoryDetectIndex; p != ((powerHistoryIndex+1)%powerHistoryLength); p=((p+1)%powerHistoryLength))
juansal12 0:c3a329a5b05d 220 {
juansal12 0:c3a329a5b05d 221 powerHistorySumDetect[t] += powerHistory[p][t];
juansal12 0:c3a329a5b05d 222 if(powerHistory[p][t] > powerHistoryMaxDetect[t])
juansal12 0:c3a329a5b05d 223 powerHistoryMaxDetect[t] = powerHistory[p][t];
juansal12 0:c3a329a5b05d 224 }
juansal12 0:c3a329a5b05d 225 }
juansal12 0:c3a329a5b05d 226 }
juansal12 0:c3a329a5b05d 227 // Advance our power history index (circular buffer)
juansal12 0:c3a329a5b05d 228 powerHistoryIndex = (powerHistoryIndex+1) % powerHistoryLength;
juansal12 0:c3a329a5b05d 229 powerHistoryDetectIndex = (powerHistoryDetectIndex+1) % powerHistoryLength;
juansal12 0:c3a329a5b05d 230 readyToThreshold = readyToThreshold || (powerHistoryIndex == 0);
juansal12 0:c3a329a5b05d 231 // If not waiting out silence until next pulse is expected, see if a tone is present
juansal12 0:c3a329a5b05d 232 if(!waitingForEnd && readyToThreshold)
juansal12 0:c3a329a5b05d 233 {
juansal12 0:c3a329a5b05d 234 // Based on new max/mean, see how many powers indicate a tone present in the last detectWindow readings
juansal12 0:c3a329a5b05d 235 for(uint8_t t = fskIndex*2; t <= fskIndex*2+1; t++)
juansal12 0:c3a329a5b05d 236 {
juansal12 0:c3a329a5b05d 237 detectSums[t] -= tonesPresent[detectWindowIndex][t];
juansal12 0:c3a329a5b05d 238 if(
juansal12 0:c3a329a5b05d 239 ((powerHistorySumDetect[t] << 2) > powerHistorySumNoDetect[t])
juansal12 0:c3a329a5b05d 240 && (powerHistoryMaxDetect[t] > powerHistoryMaxNoDetect[t])
juansal12 0:c3a329a5b05d 241 && ((powerHistorySumDetect[t] - (powerHistorySumDetect[t] >> 3)) > powerHistorySumDetect[fskIndex*2+((t+1)%2)])
juansal12 0:c3a329a5b05d 242 && ((powerHistorySumDetect[t] << 2) > powerHistorySumNoDetect[fskIndex*2+((t+1)%2)])
juansal12 0:c3a329a5b05d 243 && ((powerHistoryMaxDetect[t] << 4) > powerHistorySumNoDetect[t])
juansal12 0:c3a329a5b05d 244 && (powerHistoryMaxDetect[t] > 100000)
juansal12 0:c3a329a5b05d 245 )
juansal12 0:c3a329a5b05d 246 tonesPresent[detectWindowIndex][t] = 1;
juansal12 0:c3a329a5b05d 247 else
juansal12 0:c3a329a5b05d 248 tonesPresent[detectWindowIndex][t] = 0;
juansal12 0:c3a329a5b05d 249 detectSums[t] += tonesPresent[detectWindowIndex][t];
juansal12 0:c3a329a5b05d 250 }
juansal12 0:c3a329a5b05d 251 detectWindowIndex = (detectWindowIndex+1) % detectWindow;
juansal12 0:c3a329a5b05d 252 // If both are considered present (should be very rare?), choose the one with a higher mean
juansal12 0:c3a329a5b05d 253 if(detectSums[fskIndex*2] > detectThresh && detectSums[fskIndex*2+1] > detectThresh)
juansal12 0:c3a329a5b05d 254 {
juansal12 0:c3a329a5b05d 255 if(powerHistorySumDetect[fskIndex*2] > powerHistorySumDetect[fskIndex*2+1])
juansal12 0:c3a329a5b05d 256 detectSums[fskIndex*2+1] = 0;
juansal12 0:c3a329a5b05d 257 else
juansal12 0:c3a329a5b05d 258 detectSums[fskIndex*2] = 0;
juansal12 0:c3a329a5b05d 259 }
juansal12 0:c3a329a5b05d 260 // See if a tone is present
juansal12 0:c3a329a5b05d 261 int tonePresent = -1;
juansal12 0:c3a329a5b05d 262 if(detectSums[fskIndex*2] > detectThresh)
juansal12 0:c3a329a5b05d 263 tonePresent = fskIndex*2;
juansal12 0:c3a329a5b05d 264 else if(detectSums[fskIndex*2+1] > detectThresh)
juansal12 0:c3a329a5b05d 265 tonePresent = fskIndex*2+1;
juansal12 0:c3a329a5b05d 266 // Record data and update state
juansal12 0:c3a329a5b05d 267 if(tonePresent > -1)
juansal12 0:c3a329a5b05d 268 {
juansal12 0:c3a329a5b05d 269 #ifdef singleDataStream // if we just want a stream of bits instead of segmenting into words
juansal12 0:c3a329a5b05d 270 #ifdef saveData
juansal12 0:c3a329a5b05d 271 data[dataIndex++] = tonePresent%2;
juansal12 0:c3a329a5b05d 272 #endif
juansal12 0:c3a329a5b05d 273 #ifdef streamData
juansal12 0:c3a329a5b05d 274 usbSerial->printf("%ld\t%d\n", bufferCount, (bool)(tonePresent%2));
juansal12 0:c3a329a5b05d 275 #endif
juansal12 0:c3a329a5b05d 276 periodIndex = detectSums[tonePresent];
juansal12 0:c3a329a5b05d 277 fskIndex = (fskIndex+1) % numFSKGroups;
juansal12 0:c3a329a5b05d 278 #else
juansal12 0:c3a329a5b05d 279 // See if it has been a long time since last pulse
juansal12 0:c3a329a5b05d 280 if(periodIndex >= interWordWait)
juansal12 0:c3a329a5b05d 281 {
juansal12 0:c3a329a5b05d 282 // If we currently think that we're between words, then that's good so process the previous data word
juansal12 0:c3a329a5b05d 283 // 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
juansal12 0:c3a329a5b05d 284 // Either way, this is the start of a new word so reset dataBitIndex
juansal12 0:c3a329a5b05d 285 if(interWord && dataBitIndex == dataWordLength)
juansal12 0:c3a329a5b05d 286 {
juansal12 0:c3a329a5b05d 287 timeSinceGoodWord = 0;
juansal12 0:c3a329a5b05d 288 streamCurFishStateEventAcoustic = 5;
juansal12 0:c3a329a5b05d 289 // Decode last word and then send it out
juansal12 0:c3a329a5b05d 290 #ifdef saveData
juansal12 0:c3a329a5b05d 291 decodeAcousticWord(data[dataWordIndex]);
juansal12 0:c3a329a5b05d 292 dataWordIndex++;
juansal12 0:c3a329a5b05d 293 #else
juansal12 0:c3a329a5b05d 294 decodeAcousticWord(dataWord);
juansal12 0:c3a329a5b05d 295 #endif
juansal12 0:c3a329a5b05d 296 dataBitIndex = 0;
juansal12 0:c3a329a5b05d 297 interWord = false;
juansal12 0:c3a329a5b05d 298 }
juansal12 0:c3a329a5b05d 299 else if(!interWord) // missed a bit
juansal12 0:c3a329a5b05d 300 {
juansal12 0:c3a329a5b05d 301 #ifdef streamData
juansal12 0:c3a329a5b05d 302 usbSerial->printf("%ld\t0\t-1\n", bufferCount);
juansal12 0:c3a329a5b05d 303 #endif
juansal12 0:c3a329a5b05d 304 #ifdef saveData
juansal12 0:c3a329a5b05d 305 for(int dbi = 0; dbi < dataWordLength; dbi++)
juansal12 0:c3a329a5b05d 306 data[dataWordIndex][dbi] = 0;
juansal12 0:c3a329a5b05d 307 data[dataWordIndex][dataWordLength-1] = 1;
juansal12 0:c3a329a5b05d 308 dataWordIndex++;
juansal12 0:c3a329a5b05d 309 #endif
juansal12 0:c3a329a5b05d 310 #ifdef streamAcousticControlLog
juansal12 0:c3a329a5b05d 311 acousticControlLogToStream[4] = -1;
juansal12 0:c3a329a5b05d 312 streamCurFishStateEventAcoustic = 1;
juansal12 0:c3a329a5b05d 313 #endif
juansal12 0:c3a329a5b05d 314 lastDataWord = 0;
juansal12 0:c3a329a5b05d 315 }
juansal12 0:c3a329a5b05d 316 // TODO this is a debug check - if transmitter not putting space between words, set interWordWait to 1
juansal12 0:c3a329a5b05d 317 if(interWordWait > 1)
juansal12 0:c3a329a5b05d 318 {
juansal12 0:c3a329a5b05d 319 dataBitIndex = 0;
juansal12 0:c3a329a5b05d 320 interWord = false;
juansal12 0:c3a329a5b05d 321 }
juansal12 0:c3a329a5b05d 322 }
juansal12 0:c3a329a5b05d 323 else if(interWord)
juansal12 0:c3a329a5b05d 324 {
juansal12 0:c3a329a5b05d 325 // It has not been a long time since the last pulse, yet we thought it should be
juansal12 0:c3a329a5b05d 326 // Seems like we erroneously detected a bit that shouldn't have existed
juansal12 0:c3a329a5b05d 327 // Discard current word as garbage and start again
juansal12 0:c3a329a5b05d 328 dataBitIndex = 0;
juansal12 0:c3a329a5b05d 329 #ifdef streamData
juansal12 0:c3a329a5b05d 330 usbSerial->printf("%ld\t0\t-2\n", bufferCount);
juansal12 0:c3a329a5b05d 331 #endif
juansal12 0:c3a329a5b05d 332 #ifdef saveData
juansal12 0:c3a329a5b05d 333 for(int dbi = 0; dbi < dataWordLength; dbi++)
juansal12 0:c3a329a5b05d 334 data[dataWordIndex][dbi] = 0;
juansal12 0:c3a329a5b05d 335 data[dataWordIndex][dataWordLength-2] = 1;
juansal12 0:c3a329a5b05d 336 dataWordIndex++;
juansal12 0:c3a329a5b05d 337 #endif
juansal12 0:c3a329a5b05d 338 #ifdef streamAcousticControlLog
juansal12 0:c3a329a5b05d 339 acousticControlLogToStream[4] = -2;
juansal12 0:c3a329a5b05d 340 streamCurFishStateEventAcoustic = 2;
juansal12 0:c3a329a5b05d 341 #endif
juansal12 0:c3a329a5b05d 342 lastDataWord = 0;
juansal12 0:c3a329a5b05d 343 }
juansal12 0:c3a329a5b05d 344 // If we're not between words (either normally or because we were just reset above), store the new bit
juansal12 0:c3a329a5b05d 345 if(!interWord)
juansal12 0:c3a329a5b05d 346 {
juansal12 0:c3a329a5b05d 347 #ifdef saveData
juansal12 0:c3a329a5b05d 348 data[dataWordIndex][dataBitIndex++] = tonePresent%2;
juansal12 0:c3a329a5b05d 349 #else
juansal12 0:c3a329a5b05d 350 dataWord[dataBitIndex++] = tonePresent%2;
juansal12 0:c3a329a5b05d 351 #endif
juansal12 0:c3a329a5b05d 352 // Rotate through which FSK frequency we expect for the next pulse
juansal12 0:c3a329a5b05d 353 fskIndex = (fskIndex+1) % numFSKGroups;
juansal12 0:c3a329a5b05d 354 // If we've finished a word, say we're waiting between words
juansal12 0:c3a329a5b05d 355 // Word won't be processed until next word begins though in case we accidentally detected a nonexistent bit (see logic above)
juansal12 0:c3a329a5b05d 356 if(dataBitIndex == dataWordLength)
juansal12 0:c3a329a5b05d 357 {
juansal12 0:c3a329a5b05d 358 interWord = true;
juansal12 0:c3a329a5b05d 359 fskIndex = 0;
juansal12 0:c3a329a5b05d 360 }
juansal12 0:c3a329a5b05d 361 }
juansal12 0:c3a329a5b05d 362 periodIndex = detectSums[tonePresent]; // Use number of detected points rather than 0 to get it closer to actual rising edge
juansal12 0:c3a329a5b05d 363 #endif
juansal12 0:c3a329a5b05d 364 // Wait out reflections until next pulse
juansal12 0:c3a329a5b05d 365 waitingForEnd = true;
juansal12 0:c3a329a5b05d 366 }
juansal12 0:c3a329a5b05d 367 }
juansal12 0:c3a329a5b05d 368 else if(periodIndex > period) // done waiting for next bit (waiting out reflections)
juansal12 0:c3a329a5b05d 369 {
juansal12 0:c3a329a5b05d 370 waitingForEnd = false;
juansal12 0:c3a329a5b05d 371 // Reset the sums and indicators
juansal12 0:c3a329a5b05d 372 for(uint8_t t = 0; t < numTones; t++)
juansal12 0:c3a329a5b05d 373 {
juansal12 0:c3a329a5b05d 374 detectSums[t] = 0;
juansal12 0:c3a329a5b05d 375 for(uint8_t p = 0; p < detectWindow; p++)
juansal12 0:c3a329a5b05d 376 tonesPresent[p][t] = 0;
juansal12 0:c3a329a5b05d 377 }
juansal12 0:c3a329a5b05d 378 }
juansal12 0:c3a329a5b05d 379 #elif defined(threshold1)
juansal12 0:c3a329a5b05d 380 // Update threshold window state for each tone
juansal12 0:c3a329a5b05d 381 for(uint8_t t = 0; t < numTones; t++)
juansal12 0:c3a329a5b05d 382 {
juansal12 0:c3a329a5b05d 383 // Update our history of powers for this tone
juansal12 0:c3a329a5b05d 384 powerHistorySum[t] -= powerHistory[powerHistoryIndex][t];
juansal12 0:c3a329a5b05d 385 powerHistory[powerHistoryIndex][t] = newTonePowers[t];
juansal12 0:c3a329a5b05d 386 powerHistorySum[t] += newTonePowers[t];
juansal12 0:c3a329a5b05d 387 // Compute max of the current window (only for frequencies we currently anticipate)
juansal12 0:c3a329a5b05d 388 if((t == fskIndex*2 || t == fskIndex*2+1) && !waitingForEnd)
juansal12 0:c3a329a5b05d 389 {
juansal12 0:c3a329a5b05d 390 powerHistoryMax[t] = 0;
juansal12 0:c3a329a5b05d 391 for(uint16_t p = 0; p < powerHistoryLength; p++)
juansal12 0:c3a329a5b05d 392 {
juansal12 0:c3a329a5b05d 393 if(powerHistory[p][t] > powerHistoryMax[t])
juansal12 0:c3a329a5b05d 394 powerHistoryMax[t] = powerHistory[p][t];
juansal12 0:c3a329a5b05d 395 }
juansal12 0:c3a329a5b05d 396 }
juansal12 0:c3a329a5b05d 397 }
juansal12 0:c3a329a5b05d 398 // Advance our power history index (circular buffer)
juansal12 0:c3a329a5b05d 399 powerHistoryIndex = (powerHistoryIndex+1) % powerHistoryLength;
juansal12 0:c3a329a5b05d 400 readyToThreshold = readyToThreshold || (powerHistoryIndex == 0);
juansal12 0:c3a329a5b05d 401 // If not waiting until next pulse is expected, see if a tone is present
juansal12 0:c3a329a5b05d 402 if(!waitingForEnd && readyToThreshold)
juansal12 0:c3a329a5b05d 403 {
juansal12 0:c3a329a5b05d 404 // Based on new max/mean, see how many powers indicate a tone present in the last detectWindow readings
juansal12 0:c3a329a5b05d 405 for(uint8_t t = fskIndex*2; t <= fskIndex*2+1; t++)
juansal12 0:c3a329a5b05d 406 {
juansal12 0:c3a329a5b05d 407 detectSums[t] = 0;
juansal12 0:c3a329a5b05d 408 for(uint16_t p = powerHistoryDetectIndex; p != powerHistoryIndex; p = (p+1) % powerHistoryLength)
juansal12 0:c3a329a5b05d 409 {
juansal12 0:c3a329a5b05d 410 if((powerHistory[p][t] > (powerHistorySum[fskIndex*2+((t+1)%2)] >> 2)) // power greater than 12.5 times mean of other channel
juansal12 0:c3a329a5b05d 411 && (powerHistory[p][t] > (powerHistoryMax[t] >> 2)) // power greater than 1/4 of the max on this channel
juansal12 0:c3a329a5b05d 412 && (powerHistory[p][t] > 1000) // power greater than a fixed threshold
juansal12 0:c3a329a5b05d 413 && 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)
juansal12 0:c3a329a5b05d 414 detectSums[t]++;
juansal12 0:c3a329a5b05d 415 }
juansal12 0:c3a329a5b05d 416 }
juansal12 0:c3a329a5b05d 417 // If both are considered present (should be very rare?), choose the one with a higher mean
juansal12 0:c3a329a5b05d 418 if(detectSums[fskIndex*2] > detectThresh && detectSums[fskIndex*2+1] > detectThresh)
juansal12 0:c3a329a5b05d 419 {
juansal12 0:c3a329a5b05d 420 if(powerHistorySum[fskIndex*2] > powerHistorySum[fskIndex*2+1])
juansal12 0:c3a329a5b05d 421 detectSums[fskIndex*2+1] = 0;
juansal12 0:c3a329a5b05d 422 else
juansal12 0:c3a329a5b05d 423 detectSums[fskIndex*2] = 0;
juansal12 0:c3a329a5b05d 424 }
juansal12 0:c3a329a5b05d 425 // See if a tone is present
juansal12 0:c3a329a5b05d 426 int tonePresent = -1;
juansal12 0:c3a329a5b05d 427 if(detectSums[fskIndex*2] > detectThresh)
juansal12 0:c3a329a5b05d 428 tonePresent = fskIndex*2;
juansal12 0:c3a329a5b05d 429 else if(detectSums[fskIndex*2+1] > detectThresh)
juansal12 0:c3a329a5b05d 430 tonePresent = fskIndex*2+1;
juansal12 0:c3a329a5b05d 431 // Record data and update state
juansal12 0:c3a329a5b05d 432 if(tonePresent > -1)
juansal12 0:c3a329a5b05d 433 {
juansal12 0:c3a329a5b05d 434 #ifdef singleDataStream
juansal12 0:c3a329a5b05d 435 #ifdef saveData
juansal12 0:c3a329a5b05d 436 data[dataIndex++] = tonePresent%2;
juansal12 0:c3a329a5b05d 437 #endif
juansal12 0:c3a329a5b05d 438 #ifdef streamData
juansal12 0:c3a329a5b05d 439 usbSerial->printf("%d", (bool)(tonePresent%2));
juansal12 0:c3a329a5b05d 440 #endif
juansal12 0:c3a329a5b05d 441 periodIndex = detectSums[tonePresent];
juansal12 0:c3a329a5b05d 442 fskIndex = (fskIndex+1) % numFSKGroups;
juansal12 0:c3a329a5b05d 443 #else
juansal12 0:c3a329a5b05d 444 // See if it has been a long time since last pulse
juansal12 0:c3a329a5b05d 445 if(periodIndex >= interWordWait)
juansal12 0:c3a329a5b05d 446 {
juansal12 0:c3a329a5b05d 447 // If we currently think that we're between words, then that's good so process the previous data word
juansal12 0:c3a329a5b05d 448 // 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
juansal12 0:c3a329a5b05d 449 // Either way, this is the start of a new word so reset dataBitIndex
juansal12 0:c3a329a5b05d 450 if(interWord && dataBitIndex == dataWordLength)
juansal12 0:c3a329a5b05d 451 {
juansal12 0:c3a329a5b05d 452 // Decode last word and then send it out
juansal12 0:c3a329a5b05d 453 #ifdef saveData
juansal12 0:c3a329a5b05d 454 decodeAcousticWord(data[dataWordIndex]);
juansal12 0:c3a329a5b05d 455 dataWordIndex++;
juansal12 0:c3a329a5b05d 456 #else
juansal12 0:c3a329a5b05d 457 decodeAcousticWord(dataWord);
juansal12 0:c3a329a5b05d 458 #endif
juansal12 0:c3a329a5b05d 459 dataBitIndex = 0;
juansal12 0:c3a329a5b05d 460 interWord = false;
juansal12 0:c3a329a5b05d 461
juansal12 0:c3a329a5b05d 462 timeSinceGoodWord = 0;
juansal12 0:c3a329a5b05d 463 }
juansal12 0:c3a329a5b05d 464 else if(interWord)
juansal12 0:c3a329a5b05d 465 {
juansal12 0:c3a329a5b05d 466 #ifdef streamData
juansal12 0:c3a329a5b05d 467 usbSerial->printf("0\t-1\n");
juansal12 0:c3a329a5b05d 468 #endif
juansal12 0:c3a329a5b05d 469 lastDataWord = 0;
juansal12 0:c3a329a5b05d 470 }
juansal12 0:c3a329a5b05d 471 // TODO this is a debug check - if transmitter not putting space between words, set interWordWait to 1
juansal12 0:c3a329a5b05d 472 if(interWordWait > 1)
juansal12 0:c3a329a5b05d 473 {
juansal12 0:c3a329a5b05d 474 dataBitIndex = 0;
juansal12 0:c3a329a5b05d 475 interWord = false;
juansal12 0:c3a329a5b05d 476 }
juansal12 0:c3a329a5b05d 477 }
juansal12 0:c3a329a5b05d 478 else if(interWord)
juansal12 0:c3a329a5b05d 479 {
juansal12 0:c3a329a5b05d 480 // It has not been a long time since the last pulse, yet we thought it should be
juansal12 0:c3a329a5b05d 481 // Seems like we erroneously detected a bit that shouldn't have existed
juansal12 0:c3a329a5b05d 482 // Discard current word as garbage and start again
juansal12 0:c3a329a5b05d 483 dataBitIndex = 0;
juansal12 0:c3a329a5b05d 484 #ifdef streamData
juansal12 0:c3a329a5b05d 485 usbSerial->printf("0\t-2\n");
juansal12 0:c3a329a5b05d 486 #endif
juansal12 0:c3a329a5b05d 487 lastDataWord = 0;
juansal12 0:c3a329a5b05d 488 }
juansal12 0:c3a329a5b05d 489 // If we're not between words (either normally or because we were just reset above), store the new bit
juansal12 0:c3a329a5b05d 490 if(!interWord)
juansal12 0:c3a329a5b05d 491 {
juansal12 0:c3a329a5b05d 492 #ifdef saveData
juansal12 0:c3a329a5b05d 493 data[dataWordIndex][dataBitIndex++] = tonePresent%2;
juansal12 0:c3a329a5b05d 494 #else
juansal12 0:c3a329a5b05d 495 dataWord[dataBitIndex++] = tonePresent%2;
juansal12 0:c3a329a5b05d 496 #endif
juansal12 0:c3a329a5b05d 497 fskIndex = (fskIndex+1) % numFSKGroups;
juansal12 0:c3a329a5b05d 498 periodIndex = detectSums[tonePresent]; // Use number of detected points rather than 0 to get it closer to actual rising edge
juansal12 0:c3a329a5b05d 499 // If we've finished a word, say we're waiting between words
juansal12 0:c3a329a5b05d 500 // Word won't be processed until next word begins though in case we accidentally detected a nonexistent bit (see logic above)
juansal12 0:c3a329a5b05d 501 if(dataBitIndex == dataWordLength)
juansal12 0:c3a329a5b05d 502 {
juansal12 0:c3a329a5b05d 503 interWord = true;
juansal12 0:c3a329a5b05d 504 fskIndex = 0;
juansal12 0:c3a329a5b05d 505 }
juansal12 0:c3a329a5b05d 506 }
juansal12 0:c3a329a5b05d 507 #endif
juansal12 0:c3a329a5b05d 508 // Wait out reflections until next pulse
juansal12 0:c3a329a5b05d 509 waitingForEnd = true;
juansal12 0:c3a329a5b05d 510 }
juansal12 0:c3a329a5b05d 511 }
juansal12 0:c3a329a5b05d 512 else if(periodIndex > period) // done waiting for next bit (waiting out reflections)?
juansal12 0:c3a329a5b05d 513 {
juansal12 0:c3a329a5b05d 514 waitingForEnd = false;
juansal12 0:c3a329a5b05d 515 }
juansal12 0:c3a329a5b05d 516 // Advance our power history start detect index (circular buffer) to stay detectWindow elements behind main index
juansal12 0:c3a329a5b05d 517 powerHistoryDetectIndex = (powerHistoryDetectIndex+1) % powerHistoryLength;
juansal12 0:c3a329a5b05d 518 #endif // end switch between threshold1 or threshold2
juansal12 0:c3a329a5b05d 519
juansal12 0:c3a329a5b05d 520 // Update signal level history
juansal12 0:c3a329a5b05d 521 signalLevelSum += signalLevel;
juansal12 0:c3a329a5b05d 522 signalLevelBufferIndex = (signalLevelBufferIndex+1) % signalLevelBufferLength;
juansal12 0:c3a329a5b05d 523 #ifdef streamSignalLevel
juansal12 0:c3a329a5b05d 524 //usbSerial->printf("%ld\t%d\t%d\n", signalLevel, currentGainIndex, lastDataWord);
juansal12 0:c3a329a5b05d 525 usbSerial->printf("%ld\t%d\n", signalLevel, currentGainIndex);
juansal12 0:c3a329a5b05d 526 #endif
juansal12 0:c3a329a5b05d 527 #ifdef streamAcousticControlLog
juansal12 0:c3a329a5b05d 528 acousticControlLogToStream[3] = currentGainIndex;
juansal12 0:c3a329a5b05d 529 #endif
juansal12 0:c3a329a5b05d 530 // If have seen enough readings, choose a new gain
juansal12 0:c3a329a5b05d 531 if(signalLevelBufferIndex == 0)
juansal12 0:c3a329a5b05d 532 {
juansal12 0:c3a329a5b05d 533 // Calculate ideal gain for signalLevelSum to become signalLevelSumDesired
juansal12 0:c3a329a5b05d 534 int32_t newGain = ((int64_t)signalLevelSumDesired * (int64_t)agcGains[currentGainIndex]) / (int64_t)signalLevelSum;
juansal12 0:c3a329a5b05d 535 // See which available gain is closest
juansal12 0:c3a329a5b05d 536 uint8_t bestIndex = currentGainIndex;
juansal12 0:c3a329a5b05d 537 int32_t minDiff = 10000;
juansal12 0:c3a329a5b05d 538 for(uint8_t g = 0; g < agcGainsLength; g++)
juansal12 0:c3a329a5b05d 539 {
juansal12 0:c3a329a5b05d 540 int32_t diff = (int32_t)agcGains[g] - newGain;
juansal12 0:c3a329a5b05d 541 if(diff > 0 && diff < minDiff)
juansal12 0:c3a329a5b05d 542 {
juansal12 0:c3a329a5b05d 543 minDiff = diff;
juansal12 0:c3a329a5b05d 544 bestIndex = g;
juansal12 0:c3a329a5b05d 545 }
juansal12 0:c3a329a5b05d 546 else if(diff < 0 && -1*diff < minDiff)
juansal12 0:c3a329a5b05d 547 {
juansal12 0:c3a329a5b05d 548 minDiff = -1*diff;
juansal12 0:c3a329a5b05d 549 bestIndex = g;
juansal12 0:c3a329a5b05d 550 }
juansal12 0:c3a329a5b05d 551 }
juansal12 0:c3a329a5b05d 552 // Set the gain
juansal12 0:c3a329a5b05d 553 currentGainIndex = bestIndex;
juansal12 0:c3a329a5b05d 554 for(uint8_t i = 0; i < 3; i++)
juansal12 0:c3a329a5b05d 555 {
juansal12 0:c3a329a5b05d 556 #ifdef useAGC
juansal12 0:c3a329a5b05d 557 agc[i]->write(currentGainIndex & (1 << i));
juansal12 0:c3a329a5b05d 558 #endif
juansal12 0:c3a329a5b05d 559 #ifdef AGCLeds
juansal12 0:c3a329a5b05d 560 agcLEDs[i].write(currentGainIndex & (1 << i));
juansal12 0:c3a329a5b05d 561 #endif
juansal12 0:c3a329a5b05d 562 }
juansal12 0:c3a329a5b05d 563 // Reset sum
juansal12 0:c3a329a5b05d 564 signalLevelSum = 0;
juansal12 0:c3a329a5b05d 565 }
juansal12 0:c3a329a5b05d 566
juansal12 0:c3a329a5b05d 567 #ifdef artificialPowers
juansal12 0:c3a329a5b05d 568 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);
juansal12 0:c3a329a5b05d 569 #endif
juansal12 0:c3a329a5b05d 570
juansal12 0:c3a329a5b05d 571 // Check for timeout
juansal12 0:c3a329a5b05d 572 #ifdef fishTimeoutAcoustic
juansal12 0:c3a329a5b05d 573 if(timeSinceGoodWord >= fishTimeoutAcousticBuffers)
juansal12 0:c3a329a5b05d 574 {
juansal12 0:c3a329a5b05d 575 #ifndef singleDataStream
juansal12 0:c3a329a5b05d 576 if(timeSinceGoodWord == fishTimeoutAcousticBuffers)
juansal12 0:c3a329a5b05d 577 {
juansal12 0:c3a329a5b05d 578 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
juansal12 0:c3a329a5b05d 579 decodeAcousticWord(neutralWord);
juansal12 0:c3a329a5b05d 580 streamCurFishStateEventAcoustic = 4;
juansal12 0:c3a329a5b05d 581 }
juansal12 0:c3a329a5b05d 582 #endif
juansal12 0:c3a329a5b05d 583 timeSinceGoodWord = fishTimeoutAcousticBuffers+1; // make sure we won't constantly send a neutral command (only timeout once)
juansal12 0:c3a329a5b05d 584 }
juansal12 0:c3a329a5b05d 585 #endif
juansal12 0:c3a329a5b05d 586
juansal12 0:c3a329a5b05d 587 // Stream data
juansal12 0:c3a329a5b05d 588 #ifdef streamAcousticControlLog
juansal12 0:c3a329a5b05d 589 //usbSerial->printf("%ld %ld %ld %ld %ld %ld\n", bufferCount-1, acousticControlLogToStream[0], acousticControlLogToStream[1], acousticControlLogToStream[2], acousticControlLogToStream[3], acousticControlLogToStream[4]);
juansal12 0:c3a329a5b05d 590 //usbSerial->printf("%ld %ld %ld\n", acousticControlLogToStream[0], acousticControlLogToStream[1], acousticControlLogToStream[2]);
juansal12 0:c3a329a5b05d 591 //usbSerial->printf("%ld %ld %ld %ld %ld\n", acousticControlLogToStream[0], acousticControlLogToStream[1], acousticControlLogToStream[2], acousticControlLogToStream[3], acousticControlLogToStream[4]);
juansal12 0:c3a329a5b05d 592 //FunctionPointer fp = &tempMethod;
juansal12 0:c3a329a5b05d 593 //acousticControlLogToStream[0] = 6100;
juansal12 0:c3a329a5b05d 594 //acousticControlLogToStream[1] = 62;
juansal12 0:c3a329a5b05d 595 //acousticControlLogToStream[2] = 63;
juansal12 0:c3a329a5b05d 596 //acousticControlLogToStream[3] = 64;
juansal12 0:c3a329a5b05d 597 //acousticControlLogToStream[4] = -10;
juansal12 0:c3a329a5b05d 598 uint32_t streamGoertzel1 = acousticControlLogToStream[0];// >> 8; // second Fiji day included >> 8 to reduce bits
juansal12 0:c3a329a5b05d 599 uint32_t streamGoertzel2 = acousticControlLogToStream[1];// >> 8; // second Fiji day included >> 8 to reduce bits
juansal12 0:c3a329a5b05d 600 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
juansal12 0:c3a329a5b05d 601 uint8_t streamGain = acousticControlLogToStream[3] + 168; // only used in first day
juansal12 0:c3a329a5b05d 602 //int16_t streamWord = acousticControlLogToStream[4];
juansal12 0:c3a329a5b05d 603 // Decide the event to transmit, giving priority to acoustic events over button board events
juansal12 0:c3a329a5b05d 604 uint16_t streamFishStateEvent = fishController.streamFishStateEventController;
juansal12 0:c3a329a5b05d 605 if(streamCurFishStateEventAcoustic > 0)
juansal12 0:c3a329a5b05d 606 streamFishStateEvent = streamCurFishStateEventAcoustic;
juansal12 0:c3a329a5b05d 607 uint16_t streamWord = (uint16_t)streamCurFishStateAcoustic | ((uint16_t)streamFishStateEvent << 11); // same for both Fiji days
juansal12 0:c3a329a5b05d 608
juansal12 0:c3a329a5b05d 609 uint8_t* bufferOut = (uint8_t*) (&streamGoertzel1);
juansal12 0:c3a329a5b05d 610 usbSerial->putc(bufferOut[0]);
juansal12 0:c3a329a5b05d 611 usbSerial->putc(bufferOut[1]);
juansal12 0:c3a329a5b05d 612 usbSerial->putc(bufferOut[2]);
juansal12 0:c3a329a5b05d 613 usbSerial->putc(bufferOut[3]); // commented out for second day
juansal12 0:c3a329a5b05d 614
juansal12 0:c3a329a5b05d 615 bufferOut = (uint8_t*) (&streamGoertzel2);
juansal12 0:c3a329a5b05d 616 usbSerial->putc(bufferOut[0]);
juansal12 0:c3a329a5b05d 617 usbSerial->putc(bufferOut[1]);
juansal12 0:c3a329a5b05d 618 usbSerial->putc(bufferOut[2]);
juansal12 0:c3a329a5b05d 619 usbSerial->putc(bufferOut[3]); // commented out for second day
juansal12 0:c3a329a5b05d 620
juansal12 0:c3a329a5b05d 621 bufferOut = (uint8_t*) (&streamSignalLevel);
juansal12 0:c3a329a5b05d 622 usbSerial->putc(bufferOut[0]);
juansal12 0:c3a329a5b05d 623 usbSerial->putc(bufferOut[1]);
juansal12 0:c3a329a5b05d 624
juansal12 0:c3a329a5b05d 625 bufferOut = (uint8_t*) (&streamGain);
juansal12 0:c3a329a5b05d 626 usbSerial->putc(bufferOut[0]); // commented out for second day
juansal12 0:c3a329a5b05d 627
juansal12 0:c3a329a5b05d 628 bufferOut = (uint8_t*) (&streamWord);
juansal12 0:c3a329a5b05d 629 usbSerial->putc(bufferOut[0]);
juansal12 0:c3a329a5b05d 630 usbSerial->putc(bufferOut[1]);
juansal12 0:c3a329a5b05d 631 //usbSerial->printf("%d %d\n", streamCurFishState, streamWord);
juansal12 0:c3a329a5b05d 632
juansal12 0:c3a329a5b05d 633 /*uint8_t* bufferOut = (uint8_t*) acousticControlLogToStream;
juansal12 0:c3a329a5b05d 634 for(uint8_t i = 0; i < 20; i++)
juansal12 0:c3a329a5b05d 635 usbSerial->putc(bufferOut[i]);*/
juansal12 0:c3a329a5b05d 636
juansal12 0:c3a329a5b05d 637 // Reset the events so we don't constantly print them
juansal12 0:c3a329a5b05d 638 // TODO remove this so we do constantly print them, and only look for changes in the column in case some are dropped?
juansal12 0:c3a329a5b05d 639 streamCurFishStateEventAcoustic = 0;
juansal12 0:c3a329a5b05d 640 fishController.streamFishStateEventController = 0;
juansal12 0:c3a329a5b05d 641 #endif
juansal12 0:c3a329a5b05d 642 }
juansal12 0:c3a329a5b05d 643
juansal12 0:c3a329a5b05d 644
juansal12 0:c3a329a5b05d 645 #ifndef singleDataStream
juansal12 0:c3a329a5b05d 646 // Use Hamming code to decode the data word and check for an error
juansal12 0:c3a329a5b05d 647 // TODO make constants/defines for magic numbers below that indicate length of hamming code
juansal12 0:c3a329a5b05d 648 void AcousticController::decodeAcousticWord(volatile bool* data)
juansal12 0:c3a329a5b05d 649 {
juansal12 0:c3a329a5b05d 650 // 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
juansal12 0:c3a329a5b05d 651 // Check whether each parity bit is correct
juansal12 0:c3a329a5b05d 652 bool paritySums[5] = {0};
juansal12 0:c3a329a5b05d 653 for(uint8_t b = 0; b < dataWordLength-1; b++)
juansal12 0:c3a329a5b05d 654 {
juansal12 0:c3a329a5b05d 655 for(uint8_t p = 0; p < 5; p++)
juansal12 0:c3a329a5b05d 656 {
juansal12 0:c3a329a5b05d 657 if(((b+1) & (1 << p)) > 0 || p == 5-1) // check if bit belongs to parity set (overall parity at end includes everything)
juansal12 0:c3a329a5b05d 658 paritySums[p] ^= data[b];
juansal12 0:c3a329a5b05d 659 }
juansal12 0:c3a329a5b05d 660 }
juansal12 0:c3a329a5b05d 661 paritySums[5-1] ^= data[dataWordLength-1]; // overall parity bit
juansal12 0:c3a329a5b05d 662 // See if we have an error to correct
juansal12 0:c3a329a5b05d 663 uint8_t errorBit = 0;
juansal12 0:c3a329a5b05d 664 uint8_t numParityErrors = 0;
juansal12 0:c3a329a5b05d 665 for(uint8_t p = 0; p < 5-1; p++)
juansal12 0:c3a329a5b05d 666 {
juansal12 0:c3a329a5b05d 667 if(paritySums[p])
juansal12 0:c3a329a5b05d 668 {
juansal12 0:c3a329a5b05d 669 errorBit += (1 << p);
juansal12 0:c3a329a5b05d 670 numParityErrors++;
juansal12 0:c3a329a5b05d 671 }
juansal12 0:c3a329a5b05d 672 }
juansal12 0:c3a329a5b05d 673 // Flip the erroneous bit if applicable
juansal12 0:c3a329a5b05d 674 if(numParityErrors > 1)
juansal12 0:c3a329a5b05d 675 data[errorBit-1] = !data[errorBit-1];
juansal12 0:c3a329a5b05d 676 // If using final parity bit, check that we don't detect an uncorrectable error
juansal12 0:c3a329a5b05d 677 if(!(numParityErrors > 0 && !paritySums[5-1]))
juansal12 0:c3a329a5b05d 678 {
juansal12 0:c3a329a5b05d 679 uint16_t word = 0;
juansal12 0:c3a329a5b05d 680 uint8_t i = 0;
juansal12 0:c3a329a5b05d 681 uint8_t nextParity = 1;
juansal12 0:c3a329a5b05d 682 for(uint8_t b = 0; b < dataWordLength; b++)
juansal12 0:c3a329a5b05d 683 {
juansal12 0:c3a329a5b05d 684 if(b == nextParity-1)
juansal12 0:c3a329a5b05d 685 nextParity <<= 1;
juansal12 0:c3a329a5b05d 686 else
juansal12 0:c3a329a5b05d 687 word |= data[b] << i++;
juansal12 0:c3a329a5b05d 688 }
juansal12 0:c3a329a5b05d 689 // Update the fish
juansal12 0:c3a329a5b05d 690 processAcousticWord(word);
juansal12 0:c3a329a5b05d 691 lastDataWord = word;
juansal12 0:c3a329a5b05d 692 #ifdef streamData
juansal12 0:c3a329a5b05d 693 if(timeSinceGoodWord >= fishTimeoutAcousticBuffers)
juansal12 0:c3a329a5b05d 694 usbSerial->printf("%ld\t%d\t-4\n", bufferCount, (int)word);
juansal12 0:c3a329a5b05d 695 else
juansal12 0:c3a329a5b05d 696 usbSerial->printf("%ld\t%d\t1\n", bufferCount, (int)word);
juansal12 0:c3a329a5b05d 697 #endif
juansal12 0:c3a329a5b05d 698 #ifdef streamAcousticControlLog
juansal12 0:c3a329a5b05d 699 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
juansal12 0:c3a329a5b05d 700 acousticControlLogToStream[4] = -4;
juansal12 0:c3a329a5b05d 701 else
juansal12 0:c3a329a5b05d 702 acousticControlLogToStream[4] = word;
juansal12 0:c3a329a5b05d 703 #endif
juansal12 0:c3a329a5b05d 704 }
juansal12 0:c3a329a5b05d 705 else // there was an uncorrectable error
juansal12 0:c3a329a5b05d 706 {
juansal12 0:c3a329a5b05d 707 #ifdef streamData
juansal12 0:c3a329a5b05d 708 uint16_t word = 0;
juansal12 0:c3a329a5b05d 709 for(int i = 0; i < 16; i++)
juansal12 0:c3a329a5b05d 710 word += data[i] << i;
juansal12 0:c3a329a5b05d 711 usbSerial->printf("%ld\t%d\t-3\n", bufferCount, word);
juansal12 0:c3a329a5b05d 712 #endif
juansal12 0:c3a329a5b05d 713 #ifdef streamAcousticControlLog
juansal12 0:c3a329a5b05d 714 acousticControlLogToStream[4] = -3;
juansal12 0:c3a329a5b05d 715 streamCurFishStateEventAcoustic = 3;
juansal12 0:c3a329a5b05d 716 #endif
juansal12 0:c3a329a5b05d 717 lastDataWord = 0;
juansal12 0:c3a329a5b05d 718 }
juansal12 0:c3a329a5b05d 719 }
juansal12 0:c3a329a5b05d 720
juansal12 0:c3a329a5b05d 721 void AcousticController::processAcousticWord(uint16_t word)
juansal12 0:c3a329a5b05d 722 {
juansal12 0:c3a329a5b05d 723 // Extract state from word
juansal12 0:c3a329a5b05d 724 newSelectButtonIndex = getSelectIndexAcoustic(word);
juansal12 0:c3a329a5b05d 725 newPitchIndex = getPitchIndexAcoustic(word);
juansal12 0:c3a329a5b05d 726 newYawIndex = getYawIndexAcoustic(word);
juansal12 0:c3a329a5b05d 727 newThrustIndex = getThrustIndexAcoustic(word);
juansal12 0:c3a329a5b05d 728 newFrequencyIndex = getFrequencyIndexAcoustic(word);
juansal12 0:c3a329a5b05d 729 // Log it
juansal12 0:c3a329a5b05d 730 streamCurFishStateAcoustic = getCurStateWordAcoustic;
juansal12 0:c3a329a5b05d 731 // Set the states
juansal12 0:c3a329a5b05d 732 #ifdef acousticControllerControlFish
juansal12 0:c3a329a5b05d 733 fishController.setSelectButton(newSelectButtonIndex > 0);
juansal12 0:c3a329a5b05d 734 fishController.setPitch(pitchLookupAcoustic[newPitchIndex]);
juansal12 0:c3a329a5b05d 735 fishController.setYaw(yawLookupAcoustic[newYawIndex]);
juansal12 0:c3a329a5b05d 736 fishController.setThrust(thrustLookupAcoustic[newThrustIndex]);
juansal12 0:c3a329a5b05d 737 fishController.setFrequency(frequencyLookupAcoustic[newFrequencyIndex], periodHalfLookupAcoustic[newFrequencyIndex]);
juansal12 0:c3a329a5b05d 738 #endif
juansal12 0:c3a329a5b05d 739 }
juansal12 0:c3a329a5b05d 740 #endif
juansal12 0:c3a329a5b05d 741
juansal12 0:c3a329a5b05d 742 // Stop the AcousticController
juansal12 0:c3a329a5b05d 743 // Will stop the tone detector and will stop the fishController
juansal12 0:c3a329a5b05d 744 // Note that the acoustic controller's run() method is blocking, so if you
juansal12 0:c3a329a5b05d 745 // want to use this stop method you should launch run() in a separate thread
juansal12 0:c3a329a5b05d 746 void AcousticController::stop()
juansal12 0:c3a329a5b05d 747 {
juansal12 0:c3a329a5b05d 748 // Stop the tone detector
juansal12 0:c3a329a5b05d 749 // This will end the toneDetector's run() method at the next buffer
juansal12 0:c3a329a5b05d 750 // which will cause our run() method to advance and finish up
juansal12 0:c3a329a5b05d 751 toneDetector.stop();
juansal12 0:c3a329a5b05d 752 }
juansal12 0:c3a329a5b05d 753
juansal12 0:c3a329a5b05d 754 // Main loop
juansal12 0:c3a329a5b05d 755 // Is blocking, so won't return until loopCount is reached or another thread calls stop()
juansal12 0:c3a329a5b05d 756 void AcousticController::run()
juansal12 0:c3a329a5b05d 757 {
juansal12 0:c3a329a5b05d 758 // Create file for logging data words
juansal12 0:c3a329a5b05d 759 #ifdef saveData
juansal12 0:c3a329a5b05d 760 int fileNum = -1;
juansal12 0:c3a329a5b05d 761 char filename[25];
juansal12 0:c3a329a5b05d 762 foutDataWords = NULL;
juansal12 0:c3a329a5b05d 763 do
juansal12 0:c3a329a5b05d 764 {
juansal12 0:c3a329a5b05d 765 fileNum++;
juansal12 0:c3a329a5b05d 766 fclose(foutDataWords);
juansal12 0:c3a329a5b05d 767 sprintf(filename, "/local/%d.txt", fileNum);
juansal12 0:c3a329a5b05d 768 foutDataWords = fopen(filename, "r");
juansal12 0:c3a329a5b05d 769 usbSerial->printf("%d\n", fileNum);
juansal12 0:c3a329a5b05d 770 } while(foutDataWords != NULL);
juansal12 0:c3a329a5b05d 771 foutDataWords = fopen(filename, "w");
juansal12 0:c3a329a5b05d 772 #endif
juansal12 0:c3a329a5b05d 773
juansal12 0:c3a329a5b05d 774 #ifdef acousticControllerControlFish
juansal12 0:c3a329a5b05d 775 // Start the fish controller
juansal12 0:c3a329a5b05d 776 fishController.start();
juansal12 0:c3a329a5b05d 777 #endif
juansal12 0:c3a329a5b05d 778
juansal12 0:c3a329a5b05d 779 #ifndef artificialPowers
juansal12 0:c3a329a5b05d 780 // Start listening for tones
juansal12 0:c3a329a5b05d 781 programTimer.start();
juansal12 0:c3a329a5b05d 782 toneDetector.run();
juansal12 0:c3a329a5b05d 783 programTimer.stop();
juansal12 0:c3a329a5b05d 784 toneDetector.finish(); // we won't include the time this takes to write files in the elapsed time
juansal12 0:c3a329a5b05d 785 #else
juansal12 0:c3a329a5b05d 786 // Read powers from file
juansal12 0:c3a329a5b05d 787 usbSerial->printf("newPower[0] \tnewPower[1] \tdetectSums[0] \tdetectSums[1] \tsum[0] \tsum[1] \tmax[0] \tmax[1] \tfskIndex \twaiting \tperiodIndex \n");
juansal12 0:c3a329a5b05d 788 int32_t maxSignalValTemp = 0;
juansal12 0:c3a329a5b05d 789 int res = fscanf(finPowers, "%ld\t%ld\n", &nextPowers[0], &nextPowers[1]);
juansal12 0:c3a329a5b05d 790 while(res > 0)
juansal12 0:c3a329a5b05d 791 {
juansal12 0:c3a329a5b05d 792 processTonePowers(nextPowers, maxSignalValTemp);
juansal12 0:c3a329a5b05d 793 res = fscanf(finPowers, "%ld\t%ld\n", &nextPowers[0], &nextPowers[1]);
juansal12 0:c3a329a5b05d 794 }
juansal12 0:c3a329a5b05d 795 fclose(finPowers);
juansal12 0:c3a329a5b05d 796 #endif
juansal12 0:c3a329a5b05d 797
juansal12 0:c3a329a5b05d 798 #ifdef acousticControllerControlFish
juansal12 0:c3a329a5b05d 799 // Stop the fish controller
juansal12 0:c3a329a5b05d 800 fishController.stop();
juansal12 0:c3a329a5b05d 801 // If battery died, wait a bit for pi to clean up and shutdown and whatnot
juansal12 0:c3a329a5b05d 802 if(lowBatteryVoltageInput == 0)
juansal12 0:c3a329a5b05d 803 {
juansal12 0:c3a329a5b05d 804 wait(90); // Give the Pi time to shutdown
juansal12 0:c3a329a5b05d 805 fishController.setLEDs(255, false);
juansal12 0:c3a329a5b05d 806 }
juansal12 0:c3a329a5b05d 807 #endif
juansal12 0:c3a329a5b05d 808
juansal12 0:c3a329a5b05d 809 // Print results
juansal12 0:c3a329a5b05d 810 #ifdef printBufferSummary
juansal12 0:c3a329a5b05d 811 int elapsed = programTimer.read_us();
juansal12 0:c3a329a5b05d 812 usbSerial->printf("\n");
juansal12 0:c3a329a5b05d 813 usbSerial->printf("Buffers processed: %ld\n", bufferCount);
juansal12 0:c3a329a5b05d 814 usbSerial->printf("Elapsed time : %d us\n", elapsed);
juansal12 0:c3a329a5b05d 815 usbSerial->printf("Per-sample time : %f us\n", (double)elapsed/(double)bufferCount/(double)sampleWindow);
juansal12 0:c3a329a5b05d 816 usbSerial->printf(" Sample frequency: %f kHz\n", (double)bufferCount*(double)sampleWindow/(double)elapsed*1000.0);
juansal12 0:c3a329a5b05d 817 usbSerial->printf("Per-buffer time : %f us\n", (double)elapsed/(double)bufferCount);
juansal12 0:c3a329a5b05d 818 usbSerial->printf(" Buffer-processing frequency: %f kHz\n", (double)bufferCount/(double)elapsed*1000.0);
juansal12 0:c3a329a5b05d 819
juansal12 0:c3a329a5b05d 820 usbSerial->printf("\nComputed powers from last buffer: \n");
juansal12 0:c3a329a5b05d 821 int32_t* lastTonePowers = toneDetector.getTonePowers();
juansal12 0:c3a329a5b05d 822 for(int i = 0; i < numTones; i++)
juansal12 0:c3a329a5b05d 823 usbSerial->printf(" Tone %d: %f Hz -> %f\n", i, targetTones[i], toFloat(lastTonePowers[i]));
juansal12 0:c3a329a5b05d 824 #endif
juansal12 0:c3a329a5b05d 825 #if defined(singleDataStream) && defined(saveData)
juansal12 0:c3a329a5b05d 826 usbSerial->printf("\nData received (%d bits):\n", dataIndex);
juansal12 0:c3a329a5b05d 827 fprintf(foutDataWords, "\nData received (%d bits):\n", dataIndex);
juansal12 0:c3a329a5b05d 828 long errors = 0;
juansal12 0:c3a329a5b05d 829 for(int d = 5; d < dataIndex; d++)
juansal12 0:c3a329a5b05d 830 {
juansal12 0:c3a329a5b05d 831 usbSerial->printf("%d", data[d]);
juansal12 0:c3a329a5b05d 832 fprintf(foutDataWords, "%d", data[d]);
juansal12 0:c3a329a5b05d 833 if(d > 0 && data[d] == data[d-1])
juansal12 0:c3a329a5b05d 834 errors++;
juansal12 0:c3a329a5b05d 835 }
juansal12 0:c3a329a5b05d 836 usbSerial->printf("\n");
juansal12 0:c3a329a5b05d 837 usbSerial->printf("errors: %ld\n", errors);
juansal12 0:c3a329a5b05d 838 fprintf(foutDataWords, "\n");
juansal12 0:c3a329a5b05d 839 fprintf(foutDataWords, "errors: %ld\n", errors);
juansal12 0:c3a329a5b05d 840 fclose(foutDataWords);
juansal12 0:c3a329a5b05d 841 #ifdef debugLEDs
juansal12 0:c3a329a5b05d 842 if(errors > 0)
juansal12 0:c3a329a5b05d 843 led1 = 1;
juansal12 0:c3a329a5b05d 844 #endif
juansal12 0:c3a329a5b05d 845 #elif defined(saveData)
juansal12 0:c3a329a5b05d 846 usbSerial->printf("\nData received (%d words):\n", dataWordIndex);
juansal12 0:c3a329a5b05d 847 fprintf(foutDataWords, "\nData received (%d words):\n", dataWordIndex);
juansal12 0:c3a329a5b05d 848 long errors = 0;
juansal12 0:c3a329a5b05d 849 long badWords = 0;
juansal12 0:c3a329a5b05d 850 for(int w = 0; w < dataWordIndex; w++)
juansal12 0:c3a329a5b05d 851 {
juansal12 0:c3a329a5b05d 852 errors = 0;
juansal12 0:c3a329a5b05d 853 usbSerial->printf(" ");
juansal12 0:c3a329a5b05d 854 fprintf(foutDataWords, " ");
juansal12 0:c3a329a5b05d 855 for(int b = 0; b < dataWordLength; b++)
juansal12 0:c3a329a5b05d 856 {
juansal12 0:c3a329a5b05d 857 usbSerial->printf("%d", data[w][b]);
juansal12 0:c3a329a5b05d 858 fprintf(foutDataWords, "%d", data[w][b]);
juansal12 0:c3a329a5b05d 859 if(b > 0 && data[w][b-1] == data[w][b])
juansal12 0:c3a329a5b05d 860 errors++;
juansal12 0:c3a329a5b05d 861 }
juansal12 0:c3a329a5b05d 862 if(errors > 0)
juansal12 0:c3a329a5b05d 863 {
juansal12 0:c3a329a5b05d 864 usbSerial->printf(" X");
juansal12 0:c3a329a5b05d 865 fprintf(foutDataWords, " X");
juansal12 0:c3a329a5b05d 866 badWords++;
juansal12 0:c3a329a5b05d 867 }
juansal12 0:c3a329a5b05d 868 usbSerial->printf("\n");
juansal12 0:c3a329a5b05d 869 fprintf(foutDataWords, "\n");
juansal12 0:c3a329a5b05d 870 }
juansal12 0:c3a329a5b05d 871 usbSerial->printf("\nbad words: %d\n", badWords);
juansal12 0:c3a329a5b05d 872 fprintf(foutDataWords, "\nbad words: %d\n", badWords);
juansal12 0:c3a329a5b05d 873 fclose(foutDataWords);
juansal12 0:c3a329a5b05d 874 #ifdef debugLEDs
juansal12 0:c3a329a5b05d 875 if(badWords > 0)
juansal12 0:c3a329a5b05d 876 led1 = 1;
juansal12 0:c3a329a5b05d 877 #endif
juansal12 0:c3a329a5b05d 878 #endif
juansal12 0:c3a329a5b05d 879
juansal12 0:c3a329a5b05d 880 // TODO remove these waits?
juansal12 0:c3a329a5b05d 881 wait(1);
juansal12 0:c3a329a5b05d 882 #ifdef printBufferSummary
juansal12 0:c3a329a5b05d 883 usbSerial->printf("\nAcousticController Done!");
juansal12 0:c3a329a5b05d 884 #endif
juansal12 0:c3a329a5b05d 885 wait(3);
juansal12 0:c3a329a5b05d 886 #ifdef printBufferSummary
juansal12 0:c3a329a5b05d 887 usbSerial->printf("\n");
juansal12 0:c3a329a5b05d 888 #endif
juansal12 0:c3a329a5b05d 889 }
juansal12 0:c3a329a5b05d 890
juansal12 0:c3a329a5b05d 891
juansal12 0:c3a329a5b05d 892 void AcousticController::lowBatteryCallback()
juansal12 0:c3a329a5b05d 893 {
juansal12 0:c3a329a5b05d 894 // Stop the tone detector
juansal12 0:c3a329a5b05d 895 // This will end the main call to start, causing main to terminate
juansal12 0:c3a329a5b05d 896 // Main will also stop the fish controller once this method ends
juansal12 0:c3a329a5b05d 897 toneDetector.stop();
juansal12 0:c3a329a5b05d 898 // Also force the pin low to signal the Pi
juansal12 0:c3a329a5b05d 899 // (should have already been done, but just in case)
juansal12 0:c3a329a5b05d 900 // TODO check that this really forces it low after this method ends and the pin object may be deleted
juansal12 0:c3a329a5b05d 901 DigitalOut simBatteryLow(lowBatteryVoltagePin);
juansal12 0:c3a329a5b05d 902 simBatteryLow = 0;
juansal12 0:c3a329a5b05d 903 }
juansal12 0:c3a329a5b05d 904
juansal12 0:c3a329a5b05d 905 #endif // #ifdef acousticControl