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?
User | Revision | Line number | New 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 |