diff --git a/.gitignore b/.gitignore index 25d5aa65..96358b69 100644 --- a/.gitignore +++ b/.gitignore @@ -37,7 +37,6 @@ dox/ vortex_acoustics html/ latex/ -*.txt *.asv *.fda @@ -320,3 +319,4 @@ fabric.properties # CodeStream plugin # https://plugins.jetbrains.com/plugin/12206-codestream .idea/codestream.xml + diff --git a/cpp/teensy/.clang-format b/AcousticsMicrocontrollerCode/teensy41/.clang-format similarity index 100% rename from cpp/teensy/.clang-format rename to AcousticsMicrocontrollerCode/teensy41/.clang-format diff --git a/cpp/teensy/.gitignore b/AcousticsMicrocontrollerCode/teensy41/.gitignore similarity index 100% rename from cpp/teensy/.gitignore rename to AcousticsMicrocontrollerCode/teensy41/.gitignore diff --git a/cpp/teensy/.vscode/extensions.json b/AcousticsMicrocontrollerCode/teensy41/.vscode/extensions.json similarity index 100% rename from cpp/teensy/.vscode/extensions.json rename to AcousticsMicrocontrollerCode/teensy41/.vscode/extensions.json diff --git a/cpp/teensy/.vscode/settings.json b/AcousticsMicrocontrollerCode/teensy41/.vscode/settings.json similarity index 95% rename from cpp/teensy/.vscode/settings.json rename to AcousticsMicrocontrollerCode/teensy41/.vscode/settings.json index ee5acebf..6f8eec77 100644 --- a/cpp/teensy/.vscode/settings.json +++ b/AcousticsMicrocontrollerCode/teensy41/.vscode/settings.json @@ -34,5 +34,6 @@ "utility": "cpp", "typeinfo": "cpp", "string": "cpp" - } + }, + "cmake.configureOnOpen": false } \ No newline at end of file diff --git a/AcousticsMicrocontrollerCode/teensy41/README b/AcousticsMicrocontrollerCode/teensy41/README new file mode 100644 index 00000000..3460b7b1 --- /dev/null +++ b/AcousticsMicrocontrollerCode/teensy41/README @@ -0,0 +1,2 @@ +Code for the acoustics PCB +Upload code to Teensy 4.1 \ No newline at end of file diff --git a/cpp/teensy/include/README b/AcousticsMicrocontrollerCode/teensy41/include/README similarity index 100% rename from cpp/teensy/include/README rename to AcousticsMicrocontrollerCode/teensy41/include/README diff --git a/AcousticsMicrocontrollerCode/teensy41/lib/DSP/DSP.cpp b/AcousticsMicrocontrollerCode/teensy41/lib/DSP/DSP.cpp new file mode 100644 index 00000000..91b27c2e --- /dev/null +++ b/AcousticsMicrocontrollerCode/teensy41/lib/DSP/DSP.cpp @@ -0,0 +1,440 @@ +/* +Code for digital signal processing +Processes raw sound data sampled in a way that can be used further down the line +*/ +#include +#include + +namespace DigitalSignalProcessing { + // We do not care about frequencies up to 510k Hz, so we define a variable for + // indexes of indexes, go to the h file + const q15_t samplesOfInterest = FREQUENCY_LIMIT * SAMPLE_LENGTH / SAMPLE_RATE; + const int fOrder = 9; + const int fOrder2 = 2; + + /* + Coefficients for filter found at https://www.meme.net.au/butterworth.html, + put 9th order filter, 510kHz sampling rate and 50kHz cut-off + put 2th order filter, 430kHz sampling rate and 50kHz cut-off + */ + const float32_t aFilterCoeffs[fOrder] = {5.4569203401896500, -13.7047980216478000, 20.6476635308150000, -20.4748421533297000, 13.8143215886326000, -6.3261752484730100, 1.8924462642157100, -0.3350397779275800, 0.0267111235596287}; + const float32_t bFilterCoeffs[fOrder + 1] = {0.00000545381633879714, 0.00004908434704917420, 0.00019633738819669700, 0.00045812057245895900, 0.00068718085868843900, 0.00068718085868843900, 0.00045812057245895900, 0.00019633738819669700, 0.00004908434704917420, 0.00000545381633879714}; + + const float32_t aFilterCoeffs2[fOrder2] = {0.00101196462632, -0.00035885208947}; + const float32_t bFilterCoeffs2[fOrder2 + 1] = {0.000086700190740, 0.000173400381481, 0.000086700190740}; + + // Coefficients for 1th order filter, 430 kHz sampling rate, 50 kHz cut-off calculated manually with the help of this research paper + // https://www.researchgate.net/publication/338022014_Digital_Implementation_of_Butterworth_First-Order_Filter_Type_IIR + const float32_t aFilterCoeffs1[] = {1.0, -0.44669}; + const float32_t bFilterCoeffs1[] = {0.27665, 0.27665}; + + /* + Bit reversing is applied in a lot of FFT + algorithms for increase efficiency. + */ + const uint32_t doBitReverse = 1; + + // Constants in q_15 format done right + const q15_t PI_q15 = (q15_t)(PI * (1 << 15) + 0.5); + + q15_t* filter_butterwort_9th_order_50kHz(int16_t* samplesRaw) { + // Create array to store the filtered samples + static q15_t samples[SAMPLE_LENGTH]; + + /* + Implement Butterwort filter of "fOrder" + y = (a_1 * y_1 + .... + a_n * y_n) + (b_1 * x_1 + ... b_m * x_m) + Se Wiki: + http://vortex.a2hosted.com/index.php/Acoustics_Digital_Signal_Processing_(DSP) + Se source: https://www.meme.net.au/butterworth.html + */ + + /* + Iterate through each index of the raw samples, and apply filtering to + them. Starting at fOrder because we can't use an index outside of the + samples array. + */ + for (int i = fOrder; i < SAMPLE_LENGTH; i++) { + float32_t output_influence = 0; + /* We iterate through the previous filtered samples for the + filtering, as it is more clean and convenient. */ + for (int k = 0; k < fOrder; k++) { + output_influence += aFilterCoeffs[k] * samples[i - (k + 1)]; + } + + float32_t input_influence = 0; + /* We iterate through the previous unfilteredsamples for the + filtering, as it is more clean and convenient.*/ + for (int k = 0; k < fOrder + 1; k++) { + input_influence += bFilterCoeffs[k] * (samplesRaw[i - k]); + input_influence += bFilterCoeffs[k] * (samplesRaw[i - k]); + } + + float influenceTotalFloat = output_influence + input_influence; + + // Convert float to q15 datatype in the correct way + q15_t influenceTotalQ15 = (q15_t)influenceTotalFloat; + samples[i] = influenceTotalQ15 * FILTER_AMPLIFICATION; + } + return samples; + } + + q15_t* filter_butterwort_2th_order_50kHz(int16_t* samplesRaw) { + // Create array to store the filtered samples + static q15_t samples[SAMPLE_LENGTH]; + + /* + Implement Butterwort filter of "fOrder" + y = (a_1 * y_1 + .... + a_n * y_n) + (b_1 * x_1 + ... b_m * x_m) + Se Wiki: + http://vortex.a2hosted.com/index.php/Acoustics_Digital_Signal_Processing_(DSP) + Se source: https://www.meme.net.au/butterworth.html + */ + + /* + Iterate through each index of the raw samples, and apply filtering to + them. Starting at fOrder2 because we can't use an index outside of the + samples array. + */ + for (int i = fOrder2; i < SAMPLE_LENGTH; i++) { + float32_t output_influence = 0; + /* We iterate through the previous filtered samples for the + filtering, as it is more clean and convenient. */ + for (int k = 0; k < fOrder2; k++) { + output_influence += aFilterCoeffs2[k] * samples[i - (k + 1)]; + } + + float32_t input_influence = 0; + /* We iterate through the previous unfilteredsamples for the + filtering, as it is more clean and convenient.*/ + for (int k = 0; k < fOrder2 + 1; k++) { + input_influence += bFilterCoeffs2[k] * (samplesRaw[i - k] * FILTER_AMPLIFICATION); + } + + float influenceTotalFloat = output_influence + input_influence; + + // Convert float to q15 datatype in the correct way + q15_t influenceTotalQ15 = (q15_t)influenceTotalFloat; + samples[i] = influenceTotalQ15; + } + return samples; + } + + q15_t* filter_butterwort_1th_order_50kHz(int16_t* samplesRaw) { + // Create array to store the filtered samples + static q15_t samples[SAMPLE_LENGTH]; + int16_t i = 1; + static q15_t input[2] = {0, 0}; // Buffer for previous inputs + static q15_t output[2] = {0, 0}; // Buffer for previous outputs + q15_t inputTotal = 0; + q15_t outputTotal = 0; + + for (int index = 0; index < SAMPLE_LENGTH; index++) { + // shift the old samples + input[i - 1] = input[i]; + output[i - 1] = output[i]; + + // get the new input + input[i] = (q15_t)samplesRaw[index] * FILTER_AMPLIFICATION; + + // calculate the new output + inputTotal = bFilterCoeffs1[0] * input[i - 0] + bFilterCoeffs1[1] * input[i - 1]; + outputTotal = aFilterCoeffs1[1] * output[i - 1]; + output[i] = inputTotal - outputTotal; + output[i] = aFilterCoeffs1[0] * output[i - 0]; + + // store the new output + samples[index] = output[i]; + } + return samples; + } + + /* + Instead of taking the full FFT in a signle function we split it + We calculating first the raw values out of FFT witch are "Real" and "Imaginary" + values these values are really interesting since this raw format can be used to + calculate both amplitude, frequencies and phase shift of a signal + */ + q15_t* FFT_raw(q15_t* samples) { + /* + To store the results of fft with + complex numbers, need to have double the + size of the sample length + z = a + bi, (a1, b1, a2, b2, a3, b3 ... ) + */ + static q15_t resultsRaw[2 * SAMPLE_LENGTH]; + + /* Forward transform, which is what we want, + we want to go from time to frequency domain.*/ + uint32_t ifftFlag = 0; + + arm_rfft_instance_q15 fftInstance; // Must exist, nothing to say. + + // Initialize the rfft + arm_rfft_init_q15(&fftInstance, SAMPLE_LENGTH, ifftFlag, doBitReverse); + + // Scale the samples for better contrasts. + arm_scale_q15(samples, SCALE_FACTOR, BITSHIFT, samples, SAMPLE_LENGTH); + + // The FFT itself, output is the FFT complex array + arm_rfft_q15(&fftInstance, samples, resultsRaw); + + return resultsRaw; + } + + q15_t* FFT_mag(q15_t* resultsRaw) { + /* + Create an empty array to store the magnitude + calculations of the FFT. + As we are not dealing with complex numbers + anymore, it is the size of the sample length + */ + static q15_t results[SAMPLE_LENGTH]; + + // Converts the complex array into a magnitude array. + arm_cmplx_mag_q15(resultsRaw, results, SAMPLE_LENGTH); + + return results; + } + + /* + We will be returning q31_t datatypes as + the frequency numbers become too great to + handle for q15_t + Here we take inn FFT response and return the peaks we find + we return the peaks: + - Amplitude + - Frequency + - Phase shift + */ + std::vector> peak_detection(q15_t* resultsRaw, q15_t* results) { + // Dynamically allocate the 2D array + q31_t peaks[SAMPLE_LENGTH][2]; + + /* + Once we allocated the memory to the 2d array, the memory that we have + allocated was already in use, so it had values from before, from other + tasks. In order to have a clean slate we must iterate through the array to + make sure we don't read wrong values at the end + */ + + /* + Fill the array with 0s, this is important so we don't end up + having memory that is filled with other stuff + */ + for (int i = 0; i < SAMPLE_LENGTH; i++) { + peaks[i][0] = 0; + peaks[i][1] = 0; + } + + for (int i = 1; i < SAMPLE_LENGTH - 2; i++) { + // Check if a sample is greater than both of its neighboring samples. + if (results[i] >= results[i - 1] && results[i] >= results[i + 1]) { + peaks[i][0] = results[i]; + // Fill the array with the frequency and attached magnitude + peaks[i][1] = i * SAMPLE_RATE / SAMPLE_LENGTH; + } + } + + /* + We make an array with the results sorted for use in the median + calculation. A big part of the 510k Hz zone is either 0 or very low, so not + only is is performance wise wasteful, it is also for calculating a proper + median + */ + q15_t resultsSort[samplesOfInterest]; + + // Sorting algorithm + for (int i = 0; i < samplesOfInterest; i++) { + resultsSort[i] = results[i]; + } + + for (int i = 1; i < samplesOfInterest; i++) { + int key = resultsSort[i]; + int j = i - 1; + + while (j >= 0 && resultsSort[j] > key) { + resultsSort[j + 1] = resultsSort[j]; + j--; + } + + resultsSort[j + 1] = key; + } + + /* + As we may easily have an even number + of results, we calculate the median using the average of the two middle + values. We are aware the formatting is wonky, but that's how it + has to be + */ + q15_t avgMedian = (resultsSort[(samplesOfInterest / 2) - 1] + resultsSort[samplesOfInterest / 2]) / 2; + avgMedian += PEAK_THRESHOLD; + + /* + The next section is nothing short of a crime against all those who want + optimized code, we sincerely apologize for leaving you with this The + following code is so that we return a 2D array only the size of the number + of peaks we actually have + */ + int numNotNullValuesInList = 0; + for (int i = 0; i < SAMPLE_LENGTH; i++) { + /* + We filter by the median*3 because it works, the x3 + is there just because mediean is too small to filter + out all the "false" peaks + */ + if (peaks[i][0] <= avgMedian * 3) { + // if the magnitue does not pass the threshold we bring the entire + // index to 0 + peaks[i][0] = 0; + peaks[i][1] = 0; + } else { + // If it passes we count it towards the number of peaks we have + numNotNullValuesInList++; + } + } + /* + This is something that needs to be resolved ASAP, we are putting in the + length of the array in the very front of the array This is because q31_t + does not work seem well with the sizeof() function for arrays. This is why + the array has 1 extra index + */ + numNotNullValuesInList++; + + /* + Dynamically allocate space to a new array for the peaks we actually want to return + notice the use of numNonNull + */ + std::vector> peaksReturn(numNotNullValuesInList, std::vector(3)); + + /* + We send the length of the array through the 1st index of the array. It is + very important to find another solution. Maybe look into using the uint16_t + datatype + */ + peaksReturn[0][0] = numNotNullValuesInList; + peaksReturn[0][1] = numNotNullValuesInList; + peaksReturn[0][2] = numNotNullValuesInList; + + // Start with 1 because length of the list is in the first element + // Again, we need to find a better way to return length of the array then + // putting it in the first element of the array :/ + // UPDATE: We use vectors now, this is obsoslete, the problem is solved YAY :) but we were to lazy to delete this extra element and feature lol. so it just exists, plz fix in future and delerte this extra kebgt fucked up shit man jesjes + int tempCount = 1; + + q15_t real = 0; + q15_t imag = 0; + q15_t phase = 0; + + for (int i = 0; i < SAMPLE_LENGTH; i++) { + /* + We already filtered out non peak values + So all values will be 0 except the peaks that are left unchanged + + if (peaks[i][0] > avgMedian * 2) { + */ + if (peaks[i][0] > 0) { + // Fill the return array with the peaks. + peaksReturn[tempCount][0] = peaks[i][0]; + peaksReturn[tempCount][1] = peaks[i][1]; + + /* + Calculate phase shift of the peak frequency + + Since we took in the raw values of the FFT + We now know the: + Real -> x-axis + Imaginary -> y-axis + arctan(y/x) = angle + angle = phase shift + */ + real = resultsRaw[i * 2]; + imag = resultsRaw[i * 2 + 1]; + + /* + First we check if our Real value is 0 to insure we dont divide by 0 + If real is 0 we know that angle must be either 0 or +-pi/2 + */ + if ((real == 0) && (imag == 0)) { + phase = 0; + } else if ((real == 0) && (imag > 0)) { + phase = q15_divide(PI_q15, 2); + } else if ((real == 0) && (imag < 0)) { + phase = -q15_divide(PI_q15, 2); + } else { + phase = q15_taylor_atan(q15_divide(imag, real)); + } + + peaksReturn[tempCount][2] = phase; + + /* + As our first index is reserved for the length of the array + we use tempCount, which is 1 ahead of i, instead of i + */ + tempCount++; + } + } + + return peaksReturn; + } + + float32_t phaseQ31_to_radianFloat32(q31_t phaseQ15) { + float32_t pi = 3.141592653589793f; + float32_t conversionRatio = 32768.0f; + + return (phaseQ15 / conversionRatio) * pi; + } + + // Function for inside use only + // ================================================== + q15_t q15_divide(q15_t a, q15_t b) { + // Cast the dividend and divisor to int32_t to avoid overflow + int32_t a_scaled = (int32_t)a; + int32_t b_scaled = (int32_t)b; + + // Scale the dividend by the appropriate factor (2^15 for q15_t) + a_scaled <<= 15; + + // Perform the integer division + int32_t result = a_scaled / b_scaled; + + // Cast the result back to q15_t, if necessary + return (q15_t)result; + } + + /* + Since the CMSIS ARM libary uses FIXED pointer arithmetic we cant use + conventional means Normal Arduino math uses FLOAT pointer arithmetic which are + slower and not compatible with CMSIS q_15 data type This is why we make a FIXED + pointer arithmetic function to do "arctan" to get angle This method of arctan is + a aproximation algorithm using taylor series + + Check wiki for more info: + https://proofwiki.org/wiki/Power_Series_Expansion_for_Real_Arctangent_Function + */ + q15_t q15_taylor_atan(q15_t x) { + // Change this variable, the bigger the more accurate the value but the + // longer computational time required + int TAYLOR_SERIES_TERMS = 10; // + + // Variables + q15_t x_squared, term, result; + int32_t temp; + + x_squared = (q15_t)(((int32_t)x * x) >> 15); + term = x; + result = x; + + for (int i = 1; i < TAYLOR_SERIES_TERMS; i++) { + term = (q15_t)((((int32_t)term * x_squared) + (1 << 14)) >> 15); + temp = (int32_t)term / ((2 * i + 1) << 15); + if (i % 2 == 0) { + result += temp; + } else { + result -= temp; + } + } + + return result; + } +} \ No newline at end of file diff --git a/cpp/teensy/lib/DSP/DSP.h b/AcousticsMicrocontrollerCode/teensy41/lib/DSP/DSP.h similarity index 63% rename from cpp/teensy/lib/DSP/DSP.h rename to AcousticsMicrocontrollerCode/teensy41/lib/DSP/DSP.h index e0b9efb6..6bc61daf 100644 --- a/cpp/teensy/lib/DSP/DSP.h +++ b/AcousticsMicrocontrollerCode/teensy41/lib/DSP/DSP.h @@ -23,18 +23,18 @@ #define PEAK_THRESHOLD 30 namespace DigitalSignalProcessing { -q15_t* filter_butterwort_9th_order_50kHz(int16_t* samplesRaw); -q15_t* filter_butterwort_2th_order_50kHz(int16_t* samplesRaw); -q15_t* filter_butterwort_1th_order_50kHz(int16_t* samplesRaw); + q15_t* filter_butterwort_9th_order_50kHz(int16_t* samplesRaw); + q15_t* filter_butterwort_2th_order_50kHz(int16_t* samplesRaw); + q15_t* filter_butterwort_1th_order_50kHz(int16_t* samplesRaw); -q15_t* FFT_raw(q15_t* samples); -q15_t* FFT_mag(q15_t* resultsRaw); + q15_t* FFT_raw(q15_t* samples); + q15_t* FFT_mag(q15_t* resultsRaw); -std::vector> peak_detection(q15_t* resultsRaw, q15_t* results); + std::vector> peak_detection(q15_t* resultsRaw, q15_t* results); -float32_t phaseQ31_to_radianFloat32(q31_t phaseQ15); + float32_t phaseQ31_to_radianFloat32(q31_t phaseQ15); -// Making sure the inside functions are seen -q15_t q15_divide(q15_t a, q15_t b); -q15_t q15_taylor_atan(q15_t x); -} // namespace DigitalSignalProcessing \ No newline at end of file + // Making sure the inside functions are seen + q15_t q15_divide(q15_t a, q15_t b); + q15_t q15_taylor_atan(q15_t x); +} \ No newline at end of file diff --git a/cpp/teensy/lib/EthernetProtocol/ethernetModule.cpp b/AcousticsMicrocontrollerCode/teensy41/lib/EthernetProtocol/ethernetModule.cpp similarity index 79% rename from cpp/teensy/lib/EthernetProtocol/ethernetModule.cpp rename to AcousticsMicrocontrollerCode/teensy41/lib/EthernetProtocol/ethernetModule.cpp index de441fec..8d626e02 100644 --- a/cpp/teensy/lib/EthernetProtocol/ethernetModule.cpp +++ b/AcousticsMicrocontrollerCode/teensy41/lib/EthernetProtocol/ethernetModule.cpp @@ -1,7 +1,7 @@ #include "ethernetModule.h" // Networking variables -byte macAddressTeensy[] = {0xDE, 0xED, 0xBE, 0xEE, 0xFE, 0xED}; +byte macAddressTeensy[] = {0x00, 0x01, 0xBE, 0xEE, 0xFE, 0xED}; IPAddress ipAddressTeensy(10, 0, 0, 111); unsigned int localPort = 8888; @@ -18,27 +18,21 @@ void UDP_init() { // Start the ethernet connection Ethernet.begin(macAddressTeensy, ipAddressTeensy); + delay(1000); // Ethernet link status needs some time to connect // Check for Ethernet hardware present if (Ethernet.hardwareStatus() == EthernetNoHardware) { - Serial.println("Ethernet shield was not found. Sorry, can't run without hardware. :("); + Serial.println("ERROR: Ethernet shield was not found. Sorry, can't run without hardware. :("); } if (Ethernet.linkStatus() == LinkOFF) { - Serial.println("Ethernet cable is not connected."); - } - // Check for Ethernet hardware present - if (Ethernet.hardwareStatus() == EthernetNoHardware) { - Serial.println("Ethernet shield was not found. Sorry, can't run without hardware. :("); - } - if (Ethernet.linkStatus() == LinkOFF) { - Serial.println("Ethernet cable is not connected."); + Serial.println("ERROR: Ethernet cable is not connected."); } // Check that the port is available if (Udp.begin(localPort)) { - Serial.println("SUCCESS! Connected to network"); + Serial.println("Connected to network"); } else { - Serial.println("FAILED could not connect to network"); + Serial.println("ERROR: Could not connect to network"); Serial.println(Udp.begin(localPort)); } } @@ -67,6 +61,7 @@ uint16_t get_remotePort() { void UDP_send_ready_signal(uint8_t* remoteIPArray, uint16_t remotePort) { // Variables + char UDPReplyBuffer[] = "READY"; byte tempByte; @@ -80,6 +75,7 @@ void UDP_send_ready_signal(uint8_t* remoteIPArray, uint16_t remotePort) { Udp.write(tempByte); } Udp.endPacket(); + Serial.println("Sent ready signal"); } char* UDP_read_message() { @@ -109,4 +105,4 @@ void UDP_send_message(char* UDPReplyBuffer, int16_t sizeOfMessage, int16_t start void UDP_clean_message_memory() { memset(UDPReceiveBuffer, 0, UDP_TX_PACKET_MAX_SIZE); //clear out the packetBuffer array } -} // namespace ethernetModule \ No newline at end of file +} //ethernetModule \ No newline at end of file diff --git a/cpp/teensy/lib/EthernetProtocol/ethernetModule.h b/AcousticsMicrocontrollerCode/teensy41/lib/EthernetProtocol/ethernetModule.h similarity index 100% rename from cpp/teensy/lib/EthernetProtocol/ethernetModule.h rename to AcousticsMicrocontrollerCode/teensy41/lib/EthernetProtocol/ethernetModule.h diff --git a/cpp/teensy/lib/EthernetProtocol/teensyUDP.cpp b/AcousticsMicrocontrollerCode/teensy41/lib/EthernetProtocol/teensyUDP.cpp similarity index 57% rename from cpp/teensy/lib/EthernetProtocol/teensyUDP.cpp rename to AcousticsMicrocontrollerCode/teensy41/lib/EthernetProtocol/teensyUDP.cpp index f5866b14..be239ad6 100644 --- a/cpp/teensy/lib/EthernetProtocol/teensyUDP.cpp +++ b/AcousticsMicrocontrollerCode/teensy41/lib/EthernetProtocol/teensyUDP.cpp @@ -1,31 +1,20 @@ #include "teensyUDP.h" -// Variables -int32_t frequencyData[2]; - namespace teensyUDP { -int32_t* frequency_data_from_client() { - int32_t frequencyOfInterest; - int32_t frequencyVariance; - - // Read frequency client is interested in - while (!ethernetModule::UDP_check_if_connected()) - ; - frequencyOfInterest = atoi(ethernetModule::UDP_read_message()); - ethernetModule::UDP_clean_message_memory(); +void frequency_data_from_client(int32_t *frequenciesOfInterest, int32_t* frequencyVariances) { + for (int i = 0; i < FREQUENCY_LIST_LENGTH; i++) { + while (!ethernetModule::UDP_check_if_connected()); - // Read frequency variance client wants - while (!ethernetModule::UDP_check_if_connected()) - ; - frequencyVariance = atoi(ethernetModule::UDP_read_message()); - ethernetModule::UDP_clean_message_memory(); + char* frequencyMessage = ethernetModule::UDP_read_message(); + char* token; + + token = strtok(frequencyMessage, ","); - // Save data to a array - frequencyData[0] = frequencyOfInterest; - frequencyData[1] = frequencyVariance; + frequenciesOfInterest[i] = atoi(token); + frequencyVariances[i] = atoi(strtok(NULL, ",")); - // Return frequency data - return frequencyData; + Serial.print(frequenciesOfInterest[i]); Serial.print(", "); Serial.println(frequencyVariances[i]); + } } void send_data_16Bit(int16_t* data, int16_t lengthOfData) { @@ -66,9 +55,6 @@ void send_data_16Bit(int16_t* data, int16_t lengthOfData) { index -= amountLeftToSend; ethernetModule::UDP_send_message(dataBuffer, index, amountLeftToSend); } - // Send "DONE" as a signal, to signal that data transfer is finished - char finishingData[] = "DONE"; - ethernetModule::UDP_send_message(finishingData, 4, 0); // Free up allocated space since we don't use it anymore free(dataBuffer); @@ -112,20 +98,74 @@ void send_data_32Bit(int32_t* data, int32_t lengthOfData) { index -= amountLeftToSend; ethernetModule::UDP_send_message(dataBuffer, index, amountLeftToSend); } - // Send "DONE" as a signal, to signal that data transfer is finished - char finishingData[] = "DONE"; - ethernetModule::UDP_send_message(finishingData, 4, 0); // Free up allocated space since we don't use it anymore free(dataBuffer); } -void send_hydrophone_data(int16_t* hydrophone, int16_t lengthOfData) { send_data_16Bit(hydrophone, lengthOfData); } +void send_data_64Bit(double* data, int32_t lengthOfData) { + // for an int32_t, max length is about 15 (maybe 16?) digits plus decimal point plus null terminator + int8_t doubleTypeMaxLength = 18; + char str[doubleTypeMaxLength]; + + // Allocate memory to data buffer to be sent later on + char* dataBuffer = (char*)malloc(lengthOfData * doubleTypeMaxLength * sizeof(char)); + + // Format char data into a list of format "x,xxx,x,xxx,....,xx" + int32_t index = 0; + for (int32_t i = 0; i < lengthOfData; i++) { + // convert raw samples int to a string in decimal format + snprintf(str, doubleTypeMaxLength, "%.15f", data[i]); // should maybe work?? Idk Yea it works :) + // Convert string to char and save it into buffer we send later + // Loop until string is empty + for (int u = 0; str[u] != '\0'; u++) { + dataBuffer[index] = str[u]; + index++; + } + dataBuffer[index] = ','; + index++; + } + + // Send data in intervals to not overwhelm Client + int amountLeftToSend = 0; + while (amountLeftToSend < index) { + // Before sending the whole big interval, just double check that all of the registers are filled up, else skip + if ((amountLeftToSend + MAX_CLIENT_CAPACITY) < index) { + ethernetModule::UDP_send_message(dataBuffer, MAX_CLIENT_CAPACITY, amountLeftToSend); + } + amountLeftToSend += MAX_CLIENT_CAPACITY; + } + // There will be MOST of the time some data left unsent since it was too small for the big buffer, send the rest through here + if (amountLeftToSend != index) { + amountLeftToSend -= MAX_CLIENT_CAPACITY; + index -= amountLeftToSend; + ethernetModule::UDP_send_message(dataBuffer, index, amountLeftToSend); + } + + // Free up allocated space since we don't use it anymore + free(dataBuffer); +} + -void send_samples_raw_data(int16_t* samplesRaw, int16_t lengthOfData) { send_data_16Bit(samplesRaw, lengthOfData); } +void send_hydrophone_data(int16_t* hydrophone, int16_t lengthOfData, char hydrophone_num) { + char message[] = "HYDROPHONE_x"; + message[11] = hydrophone_num; + ethernetModule::UDP_send_message(message, 12, 0); + + send_data_16Bit(hydrophone, lengthOfData); +} + +void send_samples_raw_data(int16_t* samplesRaw, int16_t lengthOfData) { + char message[] = "SAMPLES_RAW"; + ethernetModule::UDP_send_message(message, 11, 0); + send_data_16Bit(samplesRaw, lengthOfData); +} void send_samples_filtered_data(q15_t* samplesFiltered, int16_t lengthOfData) { // Convert to correct datatype before sending + char message[] = "SAMPLES_FILTERED"; + ethernetModule::UDP_send_message(message, 16, 0); + int16_t tempSamplesFilteredBuffer[lengthOfData]; for (int i = 0; i < lengthOfData; i++) { tempSamplesFilteredBuffer[i] = (int16_t)samplesFiltered[i]; @@ -134,6 +174,8 @@ void send_samples_filtered_data(q15_t* samplesFiltered, int16_t lengthOfData) { } void send_FFT_data(q15_t* FFTdata, int16_t lengthOfData) { + char message[] = "FFT"; + ethernetModule::UDP_send_message(message, 3, 0); // Convert to correct datatype before sending int16_t tempFFTBuffer[lengthOfData]; for (int i = 0; i < lengthOfData; i++) { @@ -143,6 +185,8 @@ void send_FFT_data(q15_t* FFTdata, int16_t lengthOfData) { } void send_peak_data(std::vector> peakData, int16_t lengthOfPeakList) { + char message[] = "PEAK"; + ethernetModule::UDP_send_message(message, 4, 0); // Only send peaks if there is some useful peak data if (lengthOfPeakList < 2) { int32_t peakDataProcessed[1] = {0}; @@ -161,4 +205,25 @@ void send_peak_data(std::vector> peakData, int16_t lengthOfPe send_data_32Bit(peakDataProcessed, index); } } + +void send_tdoa_data(double* tdoaData, int8_t lengthOfData) { + char message[] = "TDOA"; + ethernetModule::UDP_send_message(message, 4, 0); + send_data_64Bit(tdoaData, lengthOfData); +} + +void send_location_data(double* locationData, int8_t lengthOfData) { + char message[] = "LOCATION"; + ethernetModule::UDP_send_message(message, 8, 0); + send_data_64Bit(locationData, lengthOfData); +} + +void setupTeensyCommunication(int32_t *frequenciesOfInterest, int32_t* frequencyVariances) { + ethernetModule::UDP_send_ready_signal(ethernetModule::get_remoteIP(), ethernetModule::get_remotePort()); + + // After this, the client and teensy are connected + teensyUDP::frequency_data_from_client(frequenciesOfInterest, frequencyVariances); + + ethernetModule::UDP_clean_message_memory(); +} } // namespace teensyUDP \ No newline at end of file diff --git a/cpp/teensy/lib/EthernetProtocol/teensyUDP.h b/AcousticsMicrocontrollerCode/teensy41/lib/EthernetProtocol/teensyUDP.h similarity index 59% rename from cpp/teensy/lib/EthernetProtocol/teensyUDP.h rename to AcousticsMicrocontrollerCode/teensy41/lib/EthernetProtocol/teensyUDP.h index a0b8a54e..6edf9adb 100644 --- a/cpp/teensy/lib/EthernetProtocol/teensyUDP.h +++ b/AcousticsMicrocontrollerCode/teensy41/lib/EthernetProtocol/teensyUDP.h @@ -8,17 +8,24 @@ #include "ethernetModule.h" // To not overwhelm the client we need to limit each data pack sent in size -#define MAX_CLIENT_CAPACITY 1000 +#define MAX_CLIENT_CAPACITY 100 +#define FREQUENCY_LIST_LENGTH 10 namespace teensyUDP { -int32_t* frequency_data_from_client(); +void frequency_data_from_client(int32_t *frequenciesOfInterest, int32_t* frequencyVariances); void send_data_16Bit(int16_t* data, int16_t lengthOfData); void send_data_32Bit(int32_t* data, int32_t lengthOfData); +void send_data_64Bit(double* data, int32_t lengthOfData); +void send_type_message(char* typeMessage, uint8_t messageLength); -void send_hydrophone_data(int16_t* hydrophone, int16_t lengthOfData); +void send_hydrophone_data(int16_t* hydrophone, int16_t lengthOfData, char hydrophone_message); void send_samples_raw_data(int16_t* samplesRaw, int16_t lengthOfData); void send_samples_filtered_data(q15_t* samplesFiltered, int16_t lengthOfData); void send_FFT_data(q15_t* FFTdata, int16_t lengthOfData); void send_peak_data(std::vector> peakData, int16_t lengthOfPeakList); +void send_tdoa_data(double* tdoaData, int8_t lengthOfData = 5); +void send_location_data(double* locationData, int8_t lengthOfData = 3); + +void setupTeensyCommunication(int32_t *frequenciesOfInterest, int32_t* frequencyVariances); } // namespace teensyUDP \ No newline at end of file diff --git a/cpp/teensy/lib/Include/arm_common_tables.h b/AcousticsMicrocontrollerCode/teensy41/lib/Include/arm_common_tables.h similarity index 100% rename from cpp/teensy/lib/Include/arm_common_tables.h rename to AcousticsMicrocontrollerCode/teensy41/lib/Include/arm_common_tables.h diff --git a/cpp/teensy/lib/Include/arm_const_structs.h b/AcousticsMicrocontrollerCode/teensy41/lib/Include/arm_const_structs.h similarity index 100% rename from cpp/teensy/lib/Include/arm_const_structs.h rename to AcousticsMicrocontrollerCode/teensy41/lib/Include/arm_const_structs.h diff --git a/cpp/teensy/lib/Include/arm_math.h b/AcousticsMicrocontrollerCode/teensy41/lib/Include/arm_math.h similarity index 100% rename from cpp/teensy/lib/Include/arm_math.h rename to AcousticsMicrocontrollerCode/teensy41/lib/Include/arm_math.h diff --git a/cpp/teensy/lib/Include/cmsis_armcc.h b/AcousticsMicrocontrollerCode/teensy41/lib/Include/cmsis_armcc.h similarity index 100% rename from cpp/teensy/lib/Include/cmsis_armcc.h rename to AcousticsMicrocontrollerCode/teensy41/lib/Include/cmsis_armcc.h diff --git a/cpp/teensy/lib/Include/cmsis_armcc_V6.h b/AcousticsMicrocontrollerCode/teensy41/lib/Include/cmsis_armcc_V6.h similarity index 100% rename from cpp/teensy/lib/Include/cmsis_armcc_V6.h rename to AcousticsMicrocontrollerCode/teensy41/lib/Include/cmsis_armcc_V6.h diff --git a/cpp/teensy/lib/Include/cmsis_armclang.h b/AcousticsMicrocontrollerCode/teensy41/lib/Include/cmsis_armclang.h similarity index 100% rename from cpp/teensy/lib/Include/cmsis_armclang.h rename to AcousticsMicrocontrollerCode/teensy41/lib/Include/cmsis_armclang.h diff --git a/cpp/teensy/lib/Include/cmsis_compiler.h b/AcousticsMicrocontrollerCode/teensy41/lib/Include/cmsis_compiler.h similarity index 100% rename from cpp/teensy/lib/Include/cmsis_compiler.h rename to AcousticsMicrocontrollerCode/teensy41/lib/Include/cmsis_compiler.h diff --git a/cpp/teensy/lib/Include/cmsis_gcc.h b/AcousticsMicrocontrollerCode/teensy41/lib/Include/cmsis_gcc.h similarity index 100% rename from cpp/teensy/lib/Include/cmsis_gcc.h rename to AcousticsMicrocontrollerCode/teensy41/lib/Include/cmsis_gcc.h diff --git a/cpp/teensy/lib/Include/cmsis_iccarm.h b/AcousticsMicrocontrollerCode/teensy41/lib/Include/cmsis_iccarm.h similarity index 100% rename from cpp/teensy/lib/Include/cmsis_iccarm.h rename to AcousticsMicrocontrollerCode/teensy41/lib/Include/cmsis_iccarm.h diff --git a/cpp/teensy/lib/Include/cmsis_version.h b/AcousticsMicrocontrollerCode/teensy41/lib/Include/cmsis_version.h similarity index 100% rename from cpp/teensy/lib/Include/cmsis_version.h rename to AcousticsMicrocontrollerCode/teensy41/lib/Include/cmsis_version.h diff --git a/cpp/teensy/lib/Include/core_armv8mbl.h b/AcousticsMicrocontrollerCode/teensy41/lib/Include/core_armv8mbl.h similarity index 100% rename from cpp/teensy/lib/Include/core_armv8mbl.h rename to AcousticsMicrocontrollerCode/teensy41/lib/Include/core_armv8mbl.h diff --git a/cpp/teensy/lib/Include/core_armv8mml.h b/AcousticsMicrocontrollerCode/teensy41/lib/Include/core_armv8mml.h similarity index 100% rename from cpp/teensy/lib/Include/core_armv8mml.h rename to AcousticsMicrocontrollerCode/teensy41/lib/Include/core_armv8mml.h diff --git a/cpp/teensy/lib/Include/core_cm0.h b/AcousticsMicrocontrollerCode/teensy41/lib/Include/core_cm0.h similarity index 100% rename from cpp/teensy/lib/Include/core_cm0.h rename to AcousticsMicrocontrollerCode/teensy41/lib/Include/core_cm0.h diff --git a/cpp/teensy/lib/Include/core_cm0plus.h b/AcousticsMicrocontrollerCode/teensy41/lib/Include/core_cm0plus.h similarity index 100% rename from cpp/teensy/lib/Include/core_cm0plus.h rename to AcousticsMicrocontrollerCode/teensy41/lib/Include/core_cm0plus.h diff --git a/cpp/teensy/lib/Include/core_cm1.h b/AcousticsMicrocontrollerCode/teensy41/lib/Include/core_cm1.h similarity index 100% rename from cpp/teensy/lib/Include/core_cm1.h rename to AcousticsMicrocontrollerCode/teensy41/lib/Include/core_cm1.h diff --git a/cpp/teensy/lib/Include/core_cm23.h b/AcousticsMicrocontrollerCode/teensy41/lib/Include/core_cm23.h similarity index 100% rename from cpp/teensy/lib/Include/core_cm23.h rename to AcousticsMicrocontrollerCode/teensy41/lib/Include/core_cm23.h diff --git a/cpp/teensy/lib/Include/core_cm3.h b/AcousticsMicrocontrollerCode/teensy41/lib/Include/core_cm3.h similarity index 100% rename from cpp/teensy/lib/Include/core_cm3.h rename to AcousticsMicrocontrollerCode/teensy41/lib/Include/core_cm3.h diff --git a/cpp/teensy/lib/Include/core_cm33.h b/AcousticsMicrocontrollerCode/teensy41/lib/Include/core_cm33.h similarity index 100% rename from cpp/teensy/lib/Include/core_cm33.h rename to AcousticsMicrocontrollerCode/teensy41/lib/Include/core_cm33.h diff --git a/cpp/teensy/lib/Include/core_cm4.h b/AcousticsMicrocontrollerCode/teensy41/lib/Include/core_cm4.h similarity index 100% rename from cpp/teensy/lib/Include/core_cm4.h rename to AcousticsMicrocontrollerCode/teensy41/lib/Include/core_cm4.h diff --git a/cpp/teensy/lib/Include/core_cm7.h b/AcousticsMicrocontrollerCode/teensy41/lib/Include/core_cm7.h similarity index 100% rename from cpp/teensy/lib/Include/core_cm7.h rename to AcousticsMicrocontrollerCode/teensy41/lib/Include/core_cm7.h diff --git a/cpp/teensy/lib/Include/core_cmFunc.h b/AcousticsMicrocontrollerCode/teensy41/lib/Include/core_cmFunc.h similarity index 100% rename from cpp/teensy/lib/Include/core_cmFunc.h rename to AcousticsMicrocontrollerCode/teensy41/lib/Include/core_cmFunc.h diff --git a/cpp/teensy/lib/Include/core_cmInstr.h b/AcousticsMicrocontrollerCode/teensy41/lib/Include/core_cmInstr.h similarity index 100% rename from cpp/teensy/lib/Include/core_cmInstr.h rename to AcousticsMicrocontrollerCode/teensy41/lib/Include/core_cmInstr.h diff --git a/cpp/teensy/lib/Include/core_cmSimd.h b/AcousticsMicrocontrollerCode/teensy41/lib/Include/core_cmSimd.h similarity index 100% rename from cpp/teensy/lib/Include/core_cmSimd.h rename to AcousticsMicrocontrollerCode/teensy41/lib/Include/core_cmSimd.h diff --git a/cpp/teensy/lib/Include/core_sc000.h b/AcousticsMicrocontrollerCode/teensy41/lib/Include/core_sc000.h similarity index 100% rename from cpp/teensy/lib/Include/core_sc000.h rename to AcousticsMicrocontrollerCode/teensy41/lib/Include/core_sc000.h diff --git a/cpp/teensy/lib/Include/core_sc300.h b/AcousticsMicrocontrollerCode/teensy41/lib/Include/core_sc300.h similarity index 100% rename from cpp/teensy/lib/Include/core_sc300.h rename to AcousticsMicrocontrollerCode/teensy41/lib/Include/core_sc300.h diff --git a/cpp/teensy/lib/Include/mpu_armv7.h b/AcousticsMicrocontrollerCode/teensy41/lib/Include/mpu_armv7.h similarity index 100% rename from cpp/teensy/lib/Include/mpu_armv7.h rename to AcousticsMicrocontrollerCode/teensy41/lib/Include/mpu_armv7.h diff --git a/cpp/teensy/lib/Include/mpu_armv8.h b/AcousticsMicrocontrollerCode/teensy41/lib/Include/mpu_armv8.h similarity index 100% rename from cpp/teensy/lib/Include/mpu_armv8.h rename to AcousticsMicrocontrollerCode/teensy41/lib/Include/mpu_armv8.h diff --git a/cpp/teensy/lib/Include/tz_context.h b/AcousticsMicrocontrollerCode/teensy41/lib/Include/tz_context.h similarity index 100% rename from cpp/teensy/lib/Include/tz_context.h rename to AcousticsMicrocontrollerCode/teensy41/lib/Include/tz_context.h diff --git a/cpp/teensy/lib/Sampling/GPT.cpp b/AcousticsMicrocontrollerCode/teensy41/lib/Sampling/GPT.cpp similarity index 100% rename from cpp/teensy/lib/Sampling/GPT.cpp rename to AcousticsMicrocontrollerCode/teensy41/lib/Sampling/GPT.cpp diff --git a/cpp/teensy/lib/Sampling/GPT.h b/AcousticsMicrocontrollerCode/teensy41/lib/Sampling/GPT.h similarity index 100% rename from cpp/teensy/lib/Sampling/GPT.h rename to AcousticsMicrocontrollerCode/teensy41/lib/Sampling/GPT.h diff --git a/cpp/teensy/lib/Sampling/PIT.cpp b/AcousticsMicrocontrollerCode/teensy41/lib/Sampling/PIT.cpp similarity index 100% rename from cpp/teensy/lib/Sampling/PIT.cpp rename to AcousticsMicrocontrollerCode/teensy41/lib/Sampling/PIT.cpp diff --git a/cpp/teensy/lib/Sampling/PIT.h b/AcousticsMicrocontrollerCode/teensy41/lib/Sampling/PIT.h similarity index 100% rename from cpp/teensy/lib/Sampling/PIT.h rename to AcousticsMicrocontrollerCode/teensy41/lib/Sampling/PIT.h diff --git a/cpp/teensy/lib/Sampling/adc.cpp b/AcousticsMicrocontrollerCode/teensy41/lib/Sampling/adc.cpp similarity index 99% rename from cpp/teensy/lib/Sampling/adc.cpp rename to AcousticsMicrocontrollerCode/teensy41/lib/Sampling/adc.cpp index d07a8fd0..632c966a 100644 --- a/cpp/teensy/lib/Sampling/adc.cpp +++ b/AcousticsMicrocontrollerCode/teensy41/lib/Sampling/adc.cpp @@ -188,7 +188,6 @@ void setup() { for (uint8_t i = 0; i < BUFFER_PER_CHANNEL; i++) { buffer_filled[i] = 0; // no sampling yet } - // ! connect beginRead() to BUSY/INT interrupt -> is done in trigger_conversion() } @@ -299,8 +298,11 @@ void triggerConversion() { void read_loop() { // timestamps[active_buffer][sample_index] = ARM_DWT_CYCCNT - clk_cyc; - if (stop_sampling) + + + if (stop_sampling) { return; + } NVIC_DISABLE_IRQ(IRQ_PIT); // detachInterrupt(BUSYINT_ARDUINO_PIN); @@ -328,9 +330,6 @@ void read_loop() { // stopwatch = elapsedMicros(); sample_index++; if (sample_index >= SAMPLE_LENGTH_ADC) { - - // unsigned long time_to_read = stopwatch; - // updating global variables buffer_filled[active_buffer] = 1; sample_index = sample_index % SAMPLE_LENGTH_ADC; // or maybe to 0 diff --git a/cpp/teensy/lib/Sampling/adc.h b/AcousticsMicrocontrollerCode/teensy41/lib/Sampling/adc.h similarity index 97% rename from cpp/teensy/lib/Sampling/adc.h rename to AcousticsMicrocontrollerCode/teensy41/lib/Sampling/adc.h index 53c4b79f..726e0053 100644 --- a/cpp/teensy/lib/Sampling/adc.h +++ b/AcousticsMicrocontrollerCode/teensy41/lib/Sampling/adc.h @@ -137,6 +137,7 @@ extern time_buff_3_1024 timestamps; extern buffer_ptr channel_buff_ptr[5]; volatile extern uint8_t active_buffer; // to know which one is being filled, [0, BUFFER_PER_CHANNEL-1] +volatile extern uint16_t sample_index; // To know what ring buffer memory we are in [0, SAMPLE_LENGTH_ADC-1] volatile extern uint8_t buffer_filled[BUFFER_PER_CHANNEL]; // to know which have been filled with new values volatile extern uint32_t overall_buffer_count; diff --git a/cpp/teensy/lib/Sampling/clock.cpp b/AcousticsMicrocontrollerCode/teensy41/lib/Sampling/clock.cpp similarity index 100% rename from cpp/teensy/lib/Sampling/clock.cpp rename to AcousticsMicrocontrollerCode/teensy41/lib/Sampling/clock.cpp diff --git a/cpp/teensy/lib/Sampling/clock.h b/AcousticsMicrocontrollerCode/teensy41/lib/Sampling/clock.h similarity index 100% rename from cpp/teensy/lib/Sampling/clock.h rename to AcousticsMicrocontrollerCode/teensy41/lib/Sampling/clock.h diff --git a/cpp/teensy/lib/Sampling/gpio.cpp b/AcousticsMicrocontrollerCode/teensy41/lib/Sampling/gpio.cpp similarity index 100% rename from cpp/teensy/lib/Sampling/gpio.cpp rename to AcousticsMicrocontrollerCode/teensy41/lib/Sampling/gpio.cpp diff --git a/cpp/teensy/lib/Sampling/gpio.h b/AcousticsMicrocontrollerCode/teensy41/lib/Sampling/gpio.h similarity index 100% rename from cpp/teensy/lib/Sampling/gpio.h rename to AcousticsMicrocontrollerCode/teensy41/lib/Sampling/gpio.h diff --git a/cpp/teensy/lib/Sampling/gpioInterrupt.cpp b/AcousticsMicrocontrollerCode/teensy41/lib/Sampling/gpioInterrupt.cpp similarity index 100% rename from cpp/teensy/lib/Sampling/gpioInterrupt.cpp rename to AcousticsMicrocontrollerCode/teensy41/lib/Sampling/gpioInterrupt.cpp diff --git a/cpp/teensy/lib/Sampling/gpioInterrupt.h b/AcousticsMicrocontrollerCode/teensy41/lib/Sampling/gpioInterrupt.h similarity index 100% rename from cpp/teensy/lib/Sampling/gpioInterrupt.h rename to AcousticsMicrocontrollerCode/teensy41/lib/Sampling/gpioInterrupt.h diff --git a/cpp/teensy/lib/Sampling/ringBuffer.h b/AcousticsMicrocontrollerCode/teensy41/lib/Sampling/ringBuffer.h similarity index 100% rename from cpp/teensy/lib/Sampling/ringBuffer.h rename to AcousticsMicrocontrollerCode/teensy41/lib/Sampling/ringBuffer.h diff --git a/cpp/teensy/lib/multilateration/correlation.cpp b/AcousticsMicrocontrollerCode/teensy41/lib/multilateration/correlation.cpp similarity index 100% rename from cpp/teensy/lib/multilateration/correlation.cpp rename to AcousticsMicrocontrollerCode/teensy41/lib/multilateration/correlation.cpp diff --git a/cpp/teensy/lib/multilateration/correlation.h b/AcousticsMicrocontrollerCode/teensy41/lib/multilateration/correlation.h similarity index 100% rename from cpp/teensy/lib/multilateration/correlation.h rename to AcousticsMicrocontrollerCode/teensy41/lib/multilateration/correlation.h diff --git a/cpp/teensy/lib/multilateration/multilateration.cpp b/AcousticsMicrocontrollerCode/teensy41/lib/multilateration/multilateration.cpp similarity index 100% rename from cpp/teensy/lib/multilateration/multilateration.cpp rename to AcousticsMicrocontrollerCode/teensy41/lib/multilateration/multilateration.cpp diff --git a/cpp/teensy/lib/multilateration/multilateration.h b/AcousticsMicrocontrollerCode/teensy41/lib/multilateration/multilateration.h similarity index 100% rename from cpp/teensy/lib/multilateration/multilateration.h rename to AcousticsMicrocontrollerCode/teensy41/lib/multilateration/multilateration.h diff --git a/cpp/teensy/platformio.ini b/AcousticsMicrocontrollerCode/teensy41/platformio.ini similarity index 95% rename from cpp/teensy/platformio.ini rename to AcousticsMicrocontrollerCode/teensy41/platformio.ini index d2fa06bd..6dfa9b68 100644 --- a/cpp/teensy/platformio.ini +++ b/AcousticsMicrocontrollerCode/teensy41/platformio.ini @@ -14,3 +14,4 @@ board = teensy41 framework = arduino monitor_speed = 9600 build_flags = -O3 +; monitor_port = COM3 diff --git a/AcousticsMicrocontrollerCode/teensy41/src/main.cpp b/AcousticsMicrocontrollerCode/teensy41/src/main.cpp new file mode 100644 index 00000000..fdaeec6a --- /dev/null +++ b/AcousticsMicrocontrollerCode/teensy41/src/main.cpp @@ -0,0 +1,335 @@ +/* +Main Firmware code for Acoustics PCB + +How the code works: +- Conncets to the Ethernet and waits for the Client +- Sets parameters specified by the Client. like frequency of interest +- Confgures ADC on the Acoustics PCB + +- Start endless loop of Sampling +- Does DSP (Digital Signal Processing) on the signals collected +- Checks for frequencies of interest in the signal +- Multilaterates the signal with the frequency of interest to find out the sound source it is comming from and how far away we are from it (OBS! NOT IMPLEMENTED YET, to be continued.....) +- Sends all the frequency of interest DSP data, Hydrophone recorded signal data and Multilaterated data back to the Client +- The infinite loop continues + +Code written by: Vortex NTNUs +All rights reserved to: Vortex NTNU +License: MIT +*/ + + + +// CMSIS Libraries +#include "arm_const_structs.h" +#include "arm_math.h" + +// Arduino Libraries +#include +#include + +// Sampling Analog to Digital Converter (ADC) Libraries +#include "GPT.h" +#include "PIT.h" +#include "adc.h" +#include "clock.h" +#include "gpio.h" +#include "gpioInterrupt.h" + +// Digital Signal Processing (DSP) Libraries +#include "DSP.h" + +// Libraries for Ethernet +#include "ethernetModule.h" +#include "teensyUDP.h" + + + +// Variables for Sampling ========== +float sample_period = 2.4; // >= MIN_SAMP_PERIOD_BLOCKING, Recomended: 2.4 +uint16_t number_samples = SAMPLE_LENGTH * 3; +#define SAMPLING_TIMEOUT 10000 // [ms] +int16_t samplesRawHydrophone1[SAMPLE_LENGTH * 3]; +int16_t samplesRawHydrophone2[SAMPLE_LENGTH * 3]; +int16_t samplesRawHydrophone3[SAMPLE_LENGTH * 3]; +int16_t samplesRawHydrophone4[SAMPLE_LENGTH * 3]; +int16_t samplesRawHydrophone5[SAMPLE_LENGTH * 3]; + +// Variables for Digital Signal Processing ========== +int16_t samplesRawForDSP[SAMPLE_LENGTH]; +q15_t* samplesFiltered; +q15_t* FFTResultsRaw; +q15_t* FFTResultsMagnified; +std::vector> peaks; +int16_t lengthOfPeakArray; + +// Variables for Peak Detection ========== +int32_t frequenciesOfInterest[FREQUENCY_LIST_LENGTH]; // 0 Hz +int32_t frequencyVariances[FREQUENCY_LIST_LENGTH]; // +-0 Hz +int32_t frequenciesOfInterestMax[FREQUENCY_LIST_LENGTH]; // 0 Hz +int32_t frequenciesOfInterestMin[FREQUENCY_LIST_LENGTH]; // 0 Hz + +// Variables for Multilateration ========== +#define TDOA_DATA_LENGHT 5 // TODO: Should be moved into multilateration library once that is operational +#define POSITION_DATA_LENGHT 3 // TODO: Should be moved into multilateration library once that is operational +double timeDifferenceOfArrival[TDOA_DATA_LENGHT]; // time difference for hydrophone 1, 2, 3, 4, 5 [s] +double soundLocation[POSITION_DATA_LENGHT]; // X, Y, Z [m] + +// Variables for data transmission ========== +int32_t lastSendTime = 0; +void setupTeensyCommunication(); +void sendDataToClient(); + + + +void setup() { + // Debugging Setup (START) ==================================================================================================== + Serial.begin(9600); + delay(5000); // pause to giving time to enter serial monitor + Serial.println("1 - Debuging Setup"); + Serial.println(); + // Debugging Setup (STOP) ==================================================================================================== + + + + // Ethernet Setup (START) ==================================================================================================== + /* + NOTE: This code NEEDS to come befor "Sampling Setup", otherwise some PINS and values are configured incorrectly in in Comunications + Why? I have no Idea, some configuration of the ISP protocol clock timer and PINS that both Comuniaction and Sampling codes uses from what it seems, probably... =_= + */ + + + + // Ethernet init + Serial.println("2 - Ethernet Setup"); + ethernetModule::UDP_init(); + + // delay(20000); // Test stuff + + // Wait until someone is connected and get their IP and Port address + Serial.println("Waiting for client connection..."); + while (!ethernetModule::UDP_check_if_connected()); + + // Wait for client input into what frequencies we sould detect and send sound signals of + Serial.println("Waiting for client configuration..."); + teensyUDP::setupTeensyCommunication(frequenciesOfInterest, frequencyVariances); + + for (int i = 0; i < FREQUENCY_LIST_LENGTH; i++) { + frequenciesOfInterestMax[i] = frequenciesOfInterest[i] + frequencyVariances[i]; + frequenciesOfInterestMin[i] = frequenciesOfInterest[i] - frequencyVariances[i]; + } + Serial.println("Client CONNECTED"); + Serial.println(); + // Ethernet Setup (STOP) ==================================================================================================== + + + + // Sampling Setup (START) ==================================================================================================== + // initializing ADC before being able to use it + Serial.println("3 - Sampling Setup"); + adc::init(); + + // Setup parameters for ADC + uint32_t ADC_reg_config; + ADC_reg_config = (1 << CONFIG_WRITE_EN) | (1 << CONFIG_PD_D) | (1 << CONFIG_REFEN) | (0x3FF << CONFIG_REFDAC) | (1 << CONFIG_VREF); + // Configure ADC + adc::config(ADC_reg_config); + adc::setup(); + + // Double check that the number of samples we want to take in doesn't overflow the ADC ring buffer space + if (number_samples > 3 * SAMPLE_LENGTH) { + number_samples = 3 * SAMPLE_LENGTH; + } + Serial.println("Sampling Setup complete"); + Serial.println(); + // Sampling Setup (STOP) ==================================================================================================== + + + + // Digital Signal Processing Setup (START) ==================================================================================================== + Serial.println("4 - DSP Setup"); + // Fill up buffers with 0s first to not get unexpected errors + samplesFiltered = DigitalSignalProcessing::filter_butterwort_9th_order_50kHz(samplesRawForDSP); + FFTResultsRaw = DigitalSignalProcessing::FFT_raw(samplesFiltered); + FFTResultsMagnified = DigitalSignalProcessing::FFT_mag(FFTResultsRaw); + peaks = DigitalSignalProcessing::peak_detection(FFTResultsRaw, FFTResultsMagnified); + lengthOfPeakArray = peaks[0][0]; + Serial.println("DSP Setup Complete"); + Serial.println(); + // Digital Signal Processing Setup (STOP) ==================================================================================================== + + + + Serial.println(); + Serial.println("=================================================="); + Serial.println("SETUP COMPLETE :D"); + Serial.println("=================================================="); + Serial.println(); +} + + + +void loop() { + // Sampling (START) ==================================================================================================== + /* + !IMPORTANT! + ! Digital Signal Processing (DSP) MUST be FASTER than Sampling ! + ! Now the whole DSP + peak detection (PD) is less than 700 us, this is good ! + ! DSP + PD should NEVER be over 2000 us, this will CRASH the system ! + + Have a endless loop that samples signals + Process signal and check for any peaks + If peaks of interest detected exit loop and process data further down the line + + If sampling takes to long to find a peak we exit the loop and try again later + */ + Serial.println("1 - SAMPLING: Start Sampling"); + + uint8_t found = 0; + uint8_t buffer_to_check = 0; // Reset buffer to start filling to 0 + unsigned long samplingStartTime = millis(); // For sampling timeout, in case we sample for to long, we want to break the loop + while (!found) { + // Start sampling into the buffer and wait until the latest one is filled before moving on and leting it continue to fill up into the next buffer + adc::startConversion(sample_period, adc::BLOCKING); + while (!adc::buffer_filled[buffer_to_check]); + + // Save raw sampled data from ADC + for (uint16_t i = 0; i < SAMPLE_LENGTH; i++) { + samplesRawForDSP[i] = (int16_t)adc::channel_buff_ptr[1][buffer_to_check][i]; + } + + // Digital Signal Processing (START) ==================================================================================================== + // Filter raw samples + samplesFiltered = DigitalSignalProcessing::filter_butterwort_1th_order_50kHz(samplesRawForDSP); + + // Preform FFT calculations on filtered samples + FFTResultsRaw = DigitalSignalProcessing::FFT_raw(samplesFiltered); + FFTResultsMagnified = DigitalSignalProcessing::FFT_mag(FFTResultsRaw); + + // Get peaks of frequencies that might be of interest and their useful information like amplitude, frequency and phase + peaks = DigitalSignalProcessing::peak_detection(FFTResultsRaw, FFTResultsMagnified); + + /* + Since we are storing the length of the array in the first index, we do not start from 0 in the array when printing out. + Find out how to get length of a 2D array of a q31_t datatype. + For now we return the length of the array in the first index of 2D array, This must be solved + this is NOT a good solution. + */ + lengthOfPeakArray = peaks[0][0]; + + /* + TIPS: For getting phase of the peak FFTs from q31 format that we dont understand to radians in floats, use this: + DigitalSignalProcessing::phaseQ31_to_radianFloat32(peaks[][2]); + */ + // Digital Signal Processing (STOP) ==================================================================================================== + + // Check peak list we got from sampled frequency data and compare with the frequencies we are interested in finding + // If the frequecies correspond, we exit the loop, else continue sampling for new sound data + for (int i = 1; i < lengthOfPeakArray; i++) { + int32_t peakFrequency = peaks[i][1]; + // Serial.println(); + // Serial.println("Found peak"); + for (int i = 0; i < FREQUENCY_LIST_LENGTH; i++) { + if ((peakFrequency < frequenciesOfInterestMax[i]) && (peakFrequency > frequenciesOfInterestMin[i])) { + found = 1; + } + } + } + if (found) { + Serial.println("1 - SAMPLING: Frequency of interest found"); + } + + buffer_to_check = (buffer_to_check + 1) % (BUFFER_PER_CHANNEL); + adc::stopConversion(); + // Check if sampling has taken to long and if so exit the loop and try again later + if (millis() - samplingStartTime > SAMPLING_TIMEOUT) { + Serial.println("1 - SAMPLING: !WARNING! Sampling timed out"); + break; + } + + // Iterate into the next ring buffer and stop sampling for this round + } + + // We make sure the last buffer that we are interested in is filled before continuing + // This ensures we have the not only the data signal of the peak, but also what happens after the peaks in the signal frequency we are interested in + adc::startConversion(sample_period, adc::BLOCKING); + while (!adc::buffer_filled[buffer_to_check]); + buffer_to_check = (buffer_to_check + 1) % (BUFFER_PER_CHANNEL); + adc::stopConversion(); + Serial.println("1 - SAMPLING: Stoped sampling"); + + // Process data from the ring buffers + // active buffer is one further than the last filled one, which is the oldest one now thats why we iterate with one + uint8_t bufferIndex = adc::active_buffer; + bufferIndex = (bufferIndex + 1) % BUFFER_PER_CHANNEL; + // Saving finished processed and sampled Hyfrophone data + uint16_t index = 0; + for (uint8_t i = 0; i < BUFFER_PER_CHANNEL; i++) { + // Combine all 3 buffers from chanels into one BIG array + for (uint16_t u = 0; u < SAMPLE_LENGTH; u++) { + index = (SAMPLE_LENGTH * i) + u; + + samplesRawHydrophone1[index] = (int16_t)adc::channel_buff_ptr[1][bufferIndex][u]; + samplesRawHydrophone2[index] = (int16_t)adc::channel_buff_ptr[2][bufferIndex][u]; + samplesRawHydrophone3[index] = (int16_t)adc::channel_buff_ptr[3][bufferIndex][u]; + samplesRawHydrophone4[index] = (int16_t)adc::channel_buff_ptr[4][bufferIndex][u]; + samplesRawHydrophone5[index] = (int16_t)adc::channel_buff_ptr[0][bufferIndex][u]; + } + bufferIndex = (bufferIndex + 1) % BUFFER_PER_CHANNEL; + } + // Clean ring buffers + // this is done so that next time we can add new data into the ring buffers withouth getting errors + for (uint8_t i = 0; i < BUFFER_PER_CHANNEL; i++) { + adc::buffer_filled[i] = 0; + } + // Sampling (STOP) ==================================================================================================== + + + + // Multilateration (START) ==================================================================================================== + // TODO: It is up to you my student finish acoustics for us T^T + Serial.println("2 - MULTILATERATION: Started the Calculations"); + timeDifferenceOfArrival[0] = 1.0; + timeDifferenceOfArrival[1] = 2.0; + timeDifferenceOfArrival[2] = 3.0; + timeDifferenceOfArrival[3] = 4.0; + timeDifferenceOfArrival[4] = 5.0; + + soundLocation[0] = 7.0; + soundLocation[1] = 8.0; + soundLocation[2] = 9.0; + Serial.println("2 - MULTILATERATION: Got the results"); + // Multilateration (STOP) ==================================================================================================== + + + + // Send data (START) ==================================================================================================== + Serial.println("3 - DATA SEND: Start sending data"); + teensyUDP::send_hydrophone_data(samplesRawHydrophone1, SAMPLE_LENGTH * BUFFER_PER_CHANNEL, '1'); + teensyUDP::send_hydrophone_data(samplesRawHydrophone2, SAMPLE_LENGTH * BUFFER_PER_CHANNEL, '2'); + teensyUDP::send_hydrophone_data(samplesRawHydrophone3, SAMPLE_LENGTH * BUFFER_PER_CHANNEL, '3'); + teensyUDP::send_hydrophone_data(samplesRawHydrophone4, SAMPLE_LENGTH * BUFFER_PER_CHANNEL, '4'); + teensyUDP::send_hydrophone_data(samplesRawHydrophone5, SAMPLE_LENGTH * BUFFER_PER_CHANNEL, '5'); + + teensyUDP::send_samples_filtered_data(samplesFiltered, SAMPLE_LENGTH); + teensyUDP::send_FFT_data(FFTResultsMagnified, SAMPLE_LENGTH); + + teensyUDP::send_peak_data(peaks, lengthOfPeakArray); + + teensyUDP::send_tdoa_data(timeDifferenceOfArrival, TDOA_DATA_LENGHT); + teensyUDP::send_location_data(soundLocation, POSITION_DATA_LENGHT); + + ethernetModule::UDP_clean_message_memory(); + Serial.println("3 - DATA SEND: Data sent sucsessfully"); + // Send data (STOP) ==================================================================================================== + + + Serial.println(); + Serial.println("--------------------------------------------------"); + Serial.println(); + + + // A small delay for debugging (Delete later) + // delay(1000); +} + diff --git a/cpp/teensy/test/README b/AcousticsMicrocontrollerCode/teensy41/test/README similarity index 100% rename from cpp/teensy/test/README rename to AcousticsMicrocontrollerCode/teensy41/test/README diff --git a/Diagrams/acoustic_architecture.drawio b/Diagrams/acoustic_architecture.drawio deleted file mode 100644 index 2aad9263..00000000 --- a/Diagrams/acoustic_architecture.drawio +++ /dev/null @@ -1 +0,0 @@ -3Vlbc6IwFP41zuw+1OHq5bFW207HzrrrzrT7mEKEOEDYEFT66/cAQaCxittRaV9aziEXzne+fCeJHf3G39wxFLqP1MZeR1PsTUcfdzStP1Dhb+pIcoeqav3c4zBiC1/pmJNXLJyK8MbExlGtIafU4ySsOy0aBNjiNR9ijK7rzRbUq88aIgdLjrmFPNn7RGzuFmH0huWLe0wcV0w9KOLzUdFYRBK5yKbrikufdPQbRinPn/zNDfZS8Apc8n6377zdfhjDAW/SYTZcLuOHvhY8eNOfy2WYKP7Tlarlw6yQF4uIxdfypIAA24CIMCnjLnVogLxJ6R0xGgc2TudRwCrbTCkNwamCc4k5T0R6UcwpuFzue+KtHIsIL6Ixs/C+AAS6HDEH830NxYhpNJUpBFZ3mPqYswQaMOwhTlb1/CNBI2fbrkQaHgTYRwCv6RLwMxoRTmgA3knECRAI78zFFL3AIqvhhzziBPBsAX6YgWOFGSfA4mvxwie2nacKR+QVvWTjpckKKQl4Fpw56pjjnbnYzxyYCW92rUUxS43uNaRFryulqw6GYrikNlLjXIjBZ2k05ciquWvQoj9dLCKgzNtMbj/w/5NbkLKS3N8YB1EatNFVpaxGa+J7KEjzEwGRuVgnmrJvaUjAvwuw2VPqOAyEvS4VTVOEz62ImWEoJ6K/zP4RCuwQRRF4b4kHLCaBk2l4qi3wMAMbqA1vGf4b48BKJBhLGUrXxNolHM9DlMnHGqrTIcnZn8rGaBt1sHVTwlrVdmDdOxXUhiw0sReBtChjzGGZZorzBskD2NWBPheSqnFpKM2WFsu9eB8soXrDCmq0qoD2ZIklfsZqslhgwMbCZSH9NBzXzEtzvP8lOW405HivVRwfSLl4jOHoA1Nj9olIrfcuTerhlyR1ryGpB60idXEQqyTjGa1Itrf79uvH/LuUmhNvj4da67bH6udS4caH9aYyrLZLh1V5Dz3nlGW7DUaD9D+qH1KKA3wkpe0kh5SjOf92a63JlD+vQqvyfm4L5pRayCOvZ6x4H8Vzi9PF8CwEo4LnfWIzGro5X+fEASVIT9sb8xLSgjeEP1ee/6RDdU1hjTdi5MxICiMAZJ6rRqVXapbdMqvo926NOChY2yvng4KltEqwNLnGXls0BlGywDuiiKVqpXUVKfUnrrXbOtqiWivvsmcML7I7qLOo9/Eotk+95Q32HPmhl93hnUGwPwxhCwRbvjO+hC4fCfjhLd+gqYIOz6SgYJY/uuU3/uVPl/rkHw==5VnbcpswEP0az7QPyQAG23l0Lk2vU0/oJe2bCjKoFRIVwpd+fVcgjImwg5PWpNMn0CIJdM7u2ZUYDC+S1bVAafyOh5gOHCtcDYaXA8exbXsEF2VZa8vItkpLJEiobbXBJ7+wNlbdchLirNFRck4lSZvGgDOGA9mwISH4stltzmnzrSmKsGHwA0RN62cSyrha2OisfvASkyjWr5444/JBgqrOeiVZjEK+3DINrwbDC8G5LO+S1QWmCr0Kl+v5my9vZl+TcXbzap58HL32P706KSd7cciQzRIEZvLPTu2UUy8QzTVeeq1yXQGIQ8BTN7mQMY84Q/Sqtp4LnrMQq9dY0Kr7vOU8BaMNxu9YyrV2DpRLDqZYJlQ/Ld+pXnSHs3sWrPtlPBcB3tPP1X6HRIT3zudsaIWAwDzBUqxhoMAUSbJofh3Sjhlt+tXgw43G/wAuhgYX0ySlROYhBrMvAYwIXLKNn7foG8RtA1NEScTgPgAIsQDDAgtJIC6m+kFCwrCkD2fkF/pWzKcITDlhslicdz7wLjf8qAnwatAStHpwHSr3MrcDaD37iXVqT860K64bb+xMhZ58phZTz2x7bZNW4/l8noGD3CVy84EP59Y1uP2AMcvUmt1T2+A0W5KEIqbYycBrpY4cx9pHRrdw2Qm6N7Ka2Ex0e1nLpmNpW7ylmK5rPT4ihO3eTJb2Cfn87v0PlF1P5ouLKr57VqeCgalKRSqcKMoyElTmF4RW3TALq06MF9SBRT+3HiFybkeR80yRawXVaveC42icZzB6jliYAqZgBbBAqgiLitSvaIObGbRBv+CpwD9zzIK14QM1w4qHZUwk9lNUgLWEqqYt1zwkfNy94ePeiR7XjB7baYme0R8IntbPHRlQz3KaqVRyiSWINuHMQPIe7JpAHwlJp3ckx09ChnrQj91+1Zd+TAwqlDOLhDBcfEOA4ROUbPhQ5CC6r256ir4+7N3Xz/4LXx919PXJkXx9d6JuUOEjtSFQ+fEf8OXeXdk2N7e3aEGKYuLZzXv/uQFjH2X3mdNn2d3+weZO9F8SgYr3e1XA7nwu4PWZ82xz8+hLLopKTvAi8c14RsqCzkJF1XyzJxNmBplHKqB3Ha/sqKCrg7j+BMTcrWwBvSSqqAAYgYMIZ+qQodN5zZG0+jCwHc/uG2xzv/JyHQqexqWLV258fB3CKyJvt+6/qKlOPd26XOmZi8a6ajDA5Ha7sTVKNethRasa92DN6yp5446StyPVH0nxzB3XNOB5JkkA1nOOhBI2+9Qy3KGPDL7Jzk/n4MwUrj4Cp3Fwps/Etk/NrOapWX20Vh+cPaoOGHcNiokZFK2w9hsU5t633g9YM4HnxfFZb8l9fFByHx5xd7CbywaanziV6mciVE+IFddp+B1EJ1EL7yuLH4Sq4/09VKFZ/+osf4vUf4yHV78B \ No newline at end of file diff --git a/Diagrams/plane_pinger_propagation.drawio b/Diagrams/plane_pinger_propagation.drawio deleted file mode 100644 index 6448b906..00000000 --- a/Diagrams/plane_pinger_propagation.drawio +++ /dev/null @@ -1 +0,0 @@ -5Vrbbts4EP0aAd2HCKKu9mNjpy2KFgjQh+32pWAsWmJDiQZFJ1a/fkmLulJylFay0RoIEM6IHF7OzJkhE8NZJYf3DO7izzRExLCt8GA4a8O2gWvbhvyxwrzQBEFQKCKGQ9WpVnzBP5FSWkq7xyHKWh05pYTjXVu5oWmKNrylg4zR53a3LSXtWXcwQpriywYSXfsvDnmstMBf1h8+IBzFauqFrfaXwLKz2kkWw5A+N1TOneGsGKW8aCWHFSLy8MpzKca9G/haLYyhlI8ZELCPN48w+nZ3T+Kv78EzwevHG6DgeYJkr3asVsvz8ghQKE5EiSlNxa9bRvdpiKRhS0iU8ZhGNIXkE6U7oQRC+QNxnis84Z5ToYp5QtRXlIZvJTq1SaF5hwlRJotFyJkHd6tUGd2zDTqxRbUhDlmE+Il+XoWJcGZEE8RZLsYxRCDHT+11QOVVUdWvGnpPsVihbakI8CzXDPxilAoBx7LaVoqlqYE1gqLRWEmtOuL6CoxtHePQsH0iDuM22z+IZiSbll0qxSwNfa8/fIIPItBbkEKCo1S0NwIhxITiCTGORSS9VR8SHIbk6D0owz/hw9GeBHsnd348C+/W8NZCs6UpV74jPLRcgzSIDkZP2CtjdbA1HeWE4+twK/M3lukG1qKNWyH9qkMoK7bbHkG32wzNgjtw/+zYnj4WwbIbi1WeKa0UdKLFombL9/S49s4c12BcXIOrimv3ZFxbpmMFzpRhXRJGOSQv5Y4zzBfnOr1/7nODkV5Qonv0hHuaYY5pC+WqjpEQhzCLK67QPKXhRYNOI8zt5NTJIZIVpImIwJ3JriaDIaZmgjeM7mLBJN+lIdJZVzXZoC+NIBY1wA00ggCBwvG5rv7KOI8bdV839pse0wL8tej6Z+JssdavavxR+E8KpleK60Pz4zpvSveIYbFZiUKhPGB+tGX6vq9kZQ6Ucm1PCnlD6FqbsBR0RpaCA7XBaIr4LcBBMI7Wr4nV/RdI3XacoJ3XJyH10kzJ6cG5KH1xqcptFhZYLp0/kQUGKonf9any9leWH/4vVqDiHtk25HhnLT+B7qR9PHVV1efiBZ4CrtuhFDBN9em5bbPnu2RafwFVXYBc5nltujgnLEdxwnW9NFn9UNeksJyoXKle+koWWJyLBZyXSeB49IjdPaECgealsHvXfKCc02SyuyaXnHLqolk06U2CNjFMjypC92G2Q/BxqrslAJ3QtPSLpdtzsQzmulgC/1LMffp5sGZ2029zuzsvt1+Is31Xe3RwFtO9SrpdB5o5BeiPzn2vUWOrwmt+jfJt3THcC79GeaPQHZvfrxldz9HQ9cCF0bX1PP7mYNir/B8NOrFxPl9VJrNGpqoyiZtIzo9oRQllddbYipTRVY0t5l6RtrvXdc8blba7z//TYaQTrMRo/PvgKh/f+3pxdxf6nwK8ceE5H/Q6+/ZCP5hce6Ef6H290PtuJ+SX7oVx1yv1XtwH024v7gO9rxd3z2vj7vmzxbsQ63/8Kirv+t/nnLv/AQ== \ No newline at end of file diff --git a/Diagrams/stm_multilateration_class_diagram.drawio b/Diagrams/stm_multilateration_class_diagram.drawio deleted file mode 100644 index 2312241c..00000000 --- a/Diagrams/stm_multilateration_class_diagram.drawio +++ /dev/null @@ -1 +0,0 @@ -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 \ No newline at end of file diff --git a/Include/analyze_data.h b/Include/analyze_data.h deleted file mode 100644 index 061acef7..00000000 --- a/Include/analyze_data.h +++ /dev/null @@ -1,87 +0,0 @@ -/** - * @file - * - * @brief Small library that implements the - * hydrophones for the AUV for Vortex NTNU - */ -#ifndef ACOUSTICS_ANALYZE_DATA_H -#define ACOUSTICS_ANALYZE_DATA_H - -#include "trilateration.h" - -namespace ANALYZE_DATA { - -/** - * @brief Function to find the index and the maximum abs value in an - * array. The function returns the index and the maximum value - * indirectly as references - * - * If @p array_length is invalid (negative), the function sets both - * @p max_val and @p idx to -1 - * - * @param data_array The array to find the maximum of - * - * @param array_length The length of the array - * - * @param idx The index containing the maximum abs value - * - * @param max_val Max absolute value - */ -void array_max_value(float32_t *data_array, const uint32_t &array_length, - uint32_t &idx, float32_t &max_val); - -/** - * @brief The function takes in raw data-signals, and uses the ARM - * Biquad IIR-filter to filter the data - * - * @param p_raw_data_array Raw data to be filtered - * It is assumed that - * @p p_raw_data_array = {p_raw_data_port, - * p_raw_data_starboard, - * p_raw_data_stern} - * - * @param p_filtered_data_array The filtered data - * Returned as - * @p p_filtered_data_array = {p_filtered_data_port, - * p_filtered_data_starboard, - * p_filtered_data_stern} - */ -void filter_raw_data(float32_t *p_raw_data_array[NUM_HYDROPHONES], - float32_t *p_filtered_data_array[NUM_HYDROPHONES]); - -/** - * @brief A function that crosscorrelates the filtered data arrays and - * returns an array which gives the number of samples between the - * measured signals at hyd1, hyd2, hyd3 - * - * @warning The values contained within @p p_lag_array are - * extremely sensitive to the sign. If the sign is negative, the - * hydrophone on the "first"-side measured the signal first. - * Example: - * - * Assuming that *p_lag_port_stern = -10 - * - * That means the port hydrophone measured the signal 10 samples - * BEFORE the stern hydrophone - * - * - * - * @param p_filtered_data_array The array containing the - * It is assumed that - * @p p_filtered_data_array = {p_filtered_data_port, - * p_filtered_data_starboard, - * P_filtered_data_stern} - * - * @param p_lag_array The array to hold the cross-correlated lags. - * It is assumed that - * @p p_lag_array = {p_lag_port_starboard, - * p_lag_port_stern, - * p_lag_starboard_stern} - */ -void calculate_xcorr_lag_array( - float32_t *p_filtered_data_array[NUM_HYDROPHONES], - uint32_t *p_lag_array[NUM_HYDROPHONES]); - -} /* namespace ANALYZE_DATA */ - -#endif // ACOUSTICS_ANALYZE_DATA_H diff --git a/Include/main.h b/Include/main.h deleted file mode 100644 index 2ae8ef8f..00000000 --- a/Include/main.h +++ /dev/null @@ -1,151 +0,0 @@ -/* USER CODE BEGIN Header */ -/** - ****************************************************************************** - * @file : main.h - * @brief : Header for main.cpp file. - * This file contains the common defines of the application. - ****************************************************************************** - * @attention - * - *

© Copyright (c) 2020 STMicroelectronics. - * All rights reserved.

- * - * This software component is licensed by ST under BSD 3-Clause license, - * the "License"; You may not use this file except in compliance with the - * License. You may obtain a copy of the License at: - * opensource.org/licenses/BSD-3-Clause - * - ****************************************************************************** - */ -/* USER CODE END Header */ - -/* Define to prevent recursive inclusion -------------------------------------*/ -#ifndef __MAIN_H -#define __MAIN_H - -/** - * @brief Important defines that unlocks multiple of the functions - * and defines from the include-files. Many of the files could be defined - * in the dependencies/lib-files, and therefore #ifndef #define #endif is - * used for each single #define - */ -#ifndef STM32_DEFINES -#define STM32_DEFINES - -#ifndef STM32F767xx -#define STM32F767xx /* Current MCU used for the project */ -#endif /* STM32F767xx */ - -#ifndef ARM_MATH_CM7 -#define ARM_MATH_CM7 /* Processor-version used on the STM32 */ -#endif /* ARM_MATH_CM7 */ - -#ifndef _USE_MATH_DEFINES -#define _USE_MATH_DEFINES /* Gives access to math-defines such as M_PI */ -#endif /* _USE_MATH_DEFINES */ - -#ifndef __SOFTFP__ -#define __SOFTFP__ /* Allows assert by software */ -#endif /* __SOFTFP__ */ - -#ifndef HAL_ADC_MODULE_ENABLED -#define HAL_ADC_MODULE_ENABLED /* Enables ADC */ -#endif /* HAL_ADC_MODULE_ENABLED */ - -#ifndef HAL_GPIO_MODULE_ENABLED -#define HAL_GPIO_MODULE_ENABLED /* Enables GPIO */ -#endif /* HAL_GPIO_MODULE_ENABLED */ - -#ifndef HAL_ETH_MODULE_ENABLED -#define HAL_ETH_MODULE_ENABLED /* Enables ETH */ -#endif /* HAL_ETH_MODULE_ENABLED */ - -#ifndef HAL_EXTI_MODULE_ENABLED -#define HAL_EXTI_MODULE_ENABLED /* Enables interrupt when GPIO is toggling */ -#endif /* HAL_EXTI_MODULE_ENABLED */ - -#ifndef HAL_DMA_MODULE_ENABLED -#define HAL_DMA_MODULE_ENABLED /* Enables DMA */ -#endif /* HAL_DMA_MODULE_ENABLED */ - -#ifndef HAL_RCC_MODULE_ENABLED -#define HAL_RCC_MODULE_ENABLED /* Enables peripheral clocks */ -#endif /* HAL_RCC_MODULE_ENABLED */ - -#ifndef HAL_FLASH_MODULE_ENABLED -#define HAL_FLASH_MODULE_ENABLED /* Enables flash */ -#endif /* HAL_FLASH_MODULE_ENABLED */ - -#ifndef HAL_PWR_MODULE_ENABLED -#define HAL_PWR_MODULE_ENABLED /* Enables power-control */ -#endif /* HAL_PWR_MODULE_ENABLED */ - -#ifndef HAL_I2C_MODULE_ENABLED -#define HAL_I2C_MODULE_ENABLED /* Enables I2C */ -#endif /* HAL_I2C_MODULE_ENABLED */ - -#ifndef HAL_CORTEX_MODULE_ENABLED -#define HAL_CORTEX_MODULE_ENABLED /* Enables driver for Cortex M7 */ -#endif /* HAL_CORTEX_MODULE_ENABLED */ - -#ifndef __IO -#define __IO volatile /* Low-level macro for compilation */ -#endif /* __IO */ - -#endif /* STM32_DEFINES */ - -/** - * Includes - */ -#include "stm32f767xx.h" -#include "stm32f7xx.h" -#include "system_stm32f7xx.h" - -/** - * @brief Enum to hold some of the potential errors that could occur - */ -typedef enum { - ERROR_ADC_INIT, /* Error on initializing ADC */ - ERROR_ADC_CONFIG, /* Error on configuring ADC */ - ERROR_DMA_START, /* Error while starting DMA */ - ERROR_DMA_STOP, /* Error while stopping DMA */ - ERROR_DMA_CONV, /* Error while converting values */ - ERROR_TRILATERATION_INIT, /* Error on initializing TRILATERATION */ - ERROR_TIME_SIGNAL, /* Error on calculating invalid time of signals */ - ERROR_UNIDENTIFIED, /* Unidentified error. Thrown using Error_handler() */ - ERROR_MEMORY, /* Out of memory for error_handling */ - ERROR_A_NOT_INVERTIBLE /* (A^T * A)-matrix not invertible */ -} ERROR_TYPES; /* enum ERROR_TYPES */ - -#ifdef __cplusplus -extern "C" { -#endif - -/** - * @brief Public handlers for - * ADC - * DMA - * ETH - * SPI - */ -extern ADC_HandleTypeDef hadc1; -extern DMA_HandleTypeDef hdma_adc1; -extern ETH_HandleTypeDef heth; -// extern SPI_HandleTypeDef hspi1; - -/** - * @brief This function is executed in case of error occurrence. - * Calls the private function log_error() with parameter - * ERROR_TYPES::ERROR_UNIDENTIFIED - * - * Called by standardized init/config-functions for ADC/ETH/DMA/... - */ -void Error_Handler(void); - -#ifdef __cplusplus -} -#endif - -#endif /* __MAIN_H */ - -/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/ diff --git a/Include/parameters.h b/Include/parameters.h deleted file mode 100644 index 133da1fd..00000000 --- a/Include/parameters.h +++ /dev/null @@ -1,274 +0,0 @@ -/** - * @file - * - * @brief Parameters and constants used for the system. - * Included parameters: - * DSP_CONSTANTS: - * Sampling-frequency - * Buffer-sizes for DMA, ADC, FFT and filter - * - * HYDROPHONE_DETAILS: - * Number hydrophones - * Hydrophone amplification - * Hydrophone position - * - * TEST_PARAMETERS: - * Position of fictional sound-source - * - * PHYSICAL_CONSTANTS: - * Speed of sound in water - * - * ARM_MATH_CM7: - * Enables specialized math-functions to the Cortex M7 MCU - * - * _USE_MATH_DEFINES: - * Enables math-defines and math-functions from and - * - * FILTER_SETUP - * Parameters for the arm_biquad_casd_df1_inst_f32 struct - * Note that the variables are defined in "analyze_data.cpp", and are - * only declared as extern in this headerfile - */ -#ifndef ACOUSTICS_PARAMETERS_H -#define ACOUSTICS_PARAMETERS_H - -/** - * @brief - * Sampling frequency - * - * Different buffer-length used - */ -#ifndef DSP_CONSTANTS -#define DSP_CONSTANTS - -#define SAMPLE_FREQUENCY \ - 112500.0f /* Sample frequency [Hz] */ -#define SAMPLE_TIME \ - 1 / SAMPLE_FREQUENCY /* Sample time [s] */ - -#define IN_BUFFER_LENGTH \ - 4096u /* Total number (real + imaginary) measurements */ - /* per hydrophone */ -#define DMA_BUFFER_LENGTH \ - IN_BUFFER_LENGTH / 2 /* Number real measurements transferred with DMA */ -#define IIR_SIZE \ - IN_BUFFER_LENGTH /* Number of data-points to filter */ - -#endif /* DSP_CONSTANTS */ - -/** - * @brief Defines that indicate the setup of the hydrophones - * - * These variables must be changed according to the hydrophones used. - * As of 03.01.21, three hydrophones of the type Benthowave BII-7014FG - * is used. See the datasheet at - * https://www.benthowave.com/products/Specs/BII-7014FGPGDatasheet.pdf - * for more information - * - * @note It is thought that the hydrophones are - * placed in a form of a triangle. Therefore, we - * have one on front starboard, one on front port - * and one on the stern in the middle. Could be changed - * in the future - * - * BOW - * - * Port Starboard - * - * HYD ''''''''''''''''''''' HYD - * ''''''''''''''''''''' - * ''''''''' A ''''''''' - * ''''''''' U ''''''''' - * ''''''''' V ''''''''' - * ''''''''''''''''''''' - * ''''''''''''''''''''' - * '''''''''HYD''''''''' - * - * STERN - * - * y - * ^ - * | - * | - * | - * | - * ----------------------> x - * | - * | - * - * - * @note The position of the hydrophones are relative - * to the center of the AUV - * - * @warning The axis in the xy-plane are rotated compared - * to the axis that mechanical works with. This implies - * that n_HYD_X equals mechanical's y-axis - * - */ -#ifndef HYDROPHONE_DETAILS -#define HYDROPHONE_DETAILS - -#define NUM_HYDROPHONES 3 /* Number of hydrophones used on the AUV */ -#define HYD_PREAMP_DB 40 /* Number of dB the signal is preamplifies */ -#define HYD_FFVS -173 /* Average FFVS for 20 - 40 kHz [dB V/μPa] */ - -#define SIGNAL_GAIN 1.0f /* Gain of signal (unknown as of 06.01) */ - -#define PORT_HYD_X -0.11f /* x - position of port hydrophone [m] */ -#define PORT_HYD_Y 0.31f /* y - position of port hydrophone [m] */ -#define PORT_HYD_Z 0.15f /* z - position of port hydrophone [m] */ - -#define STARBOARD_HYD_X \ - 0.11f /* x - position of starboard hydrophone [m] */ -#define STARBOARD_HYD_Y \ - 0.31f /* y - position of starboard hydrophone [m] */ -#define STARBOARD_HYD_Z \ - 0.15f /* z - position of starboard hydrophone [m] */ - -#define STERN_HYD_X 0.0f /* x - position of stern hydrophone [m] */ -#define STERN_HYD_Y -0.24f /* y - position of stern hydrophone [m] */ -#define STERN_HYD_Z 0.15f /* z - position of stern hydrophone [m] */ - -#endif /* HYDROPHONE_DETAILS */ - -/** - * @brief Defines that indicate the errors that we can allow and still - * get an acceptable result - */ -#ifndef SYSTEM_MARGINS -#define SYSTEM_MARGINS - -#define MARGIN_POS_ESTIMATE \ - 0.5f /* Error tolerable in estimating the position [m] */ -#define MARGIN_POS_SEARCH \ - 0.25f /* How much the search will deviate in x [m] */ - -#define MARGIN_INTENSITY \ - 20 /* Difference between the intensity-measurements \ - */ -#define MARGIN_TIME_EPSILON \ - 0.1f /* Determines the difference allowed between the */ - /* detected lags. With LAG_DIFF_EPSILON = 0.1 we */ - /* allow signals to arrive 1.1 * max_time and */ - /* still count. max_time is the maximum */ - /* possible time for sound to pass from one to */ - /* another hydrophone */ - -#endif /* SYSTEM_MARGINS */ - -/** - * @brief Defines that indicate which parameters are to be tested - */ -#ifndef TEST_PARAMETERS -#define TEST_PARAMETERS - -#define CURR_TESTING_BOOL \ - 0u /* Bool to indicate if the code is being tested */ - /* Not testing: 0u */ - /* Testing : 1u */ - -#define SOURCE_POS_X 10.0f /* x - position of sound-source */ -#define SOURCE_POS_Y 2.0f /* y - position of sound-source */ -#define SOURCE_POS_Z 0.0f /* z - position of sound-source */ - -#endif /* TEST_PARAMETERS */ - -/** - * @brief Defines that indicate physical characteristics/constants - * of the system - */ -#ifndef PHYSICAL_CONSTANTS -#define PHYSICAL_CONSTANTS - -#define SOUND_SPEED 1480.0f /* Speed of sound in water [m/s] */ - -#endif /* PHYSICAL_CONSTANTS */ - -#ifndef ARM_MATH_CM7 -#define ARM_MATH_CM7 /* Processor-version used on the STM32 */ -#endif /* ARM_MATH_CM7 */ - -#ifndef _USE_MATH_DEFINES -#define _USE_MATH_DEFINES /* Gives access to math-defines such as M_PI */ -#endif /* _USE_MATH_DEFINES */ - -#include -#include -#include -#include - -#include "arm_const_structs.h" -#include "arm_math.h" - -/** - * @brief A fourth-order IIR-filter to filter the data before - * processing it. By using this filter, we should (hopefully) - * eliminate unwanted frequencies. - * - * @warning Must have abs(filter) < 1 to prevent overflow - * - * @warning The filter is designed with the filterdesigner - * in MATLAB, using - * Fs = 112500 Sampling frequency - * Fc1 = 15000 Lower cut-off frequency - * Fc2 = 45000 Upper cut-off frequency - * - * Order = 4 Filter order - * Type = Butterworth Filter type - * - * - * The filter's transferfunction is given as - * H(z) = B(z) * 1 / A(z) - * where - * B(z) = b0 + b1 * z^(-1) + b2 * z^(-2) + b3 * z^(-3) + ... - * A(z) = 1 + a1 * z^(-1) + a2 * z^(-2) + a3 * z^(-3) + ... - * - * NOTE: Since the function uses a biquad-filter, every filter with order > 2 is - * split into multiple serial second-order filters, leaving at most one filter - * with order = 1 - * - * Example: - * 4th order filter with the original filter function given as - * H(z) = B(z)/A(z) with - * - * B(z) = b0 + b1 * z^(-1) + b2 * z^(-2) + b3 * z^(-3) + b4 * z^(-4) - * A(z) = 1 + a1 * z^(-1) + a2 * z^(-2) + a3 * z^(-3) + a4 * z^(-4) - * - * is instead given as H(z) = B_1(z)/A_1(z) * B_2(z)/A_2(z), where - * - * B_1(z) = b10 + b11 * z^(-1) + b12 * z^(-2) - * A_1(z) = 1 + a11 * z^(-1) + a12 * z^(-2) - * - * and equivalent for B_2(z) and A_2(z) - * - * NOTE: For more information, see - * https://arm-software.github.io/CMSIS_5/DSP/html/group__BiquadCascadeDF1__32x64.html - * - * NOTE: The MATLAB-script used to test the filter can be found in the Matlab - * folder in in the main directory. - * - * @param num_stages Number of second order cascade-filters. - * Determines the filter order. Order = 2 * num_stages - * - * @param state_coefficients Initial values for x[n-1], x[n-2], ..., - * x[n-(2*num_stages)] y[n-1], y[n-2], ..., y[n-(2*num_stages)] - * - * @param filter_coefficients Filter coefficients given as {b10, b11, b12, - * a11, a12 b20, b21, b22, a21, a22, ...} - * - * @param IIR_FILTER A struct describing a biquad DF1 IIR filter - */ -#ifndef FILTER_SETUP -#define FILTER_SETUP - -const uint32_t num_stages = 2; - -extern float32_t state_coefficients[4 * num_stages]; - -extern float32_t filter_coefficients[5 * num_stages]; - -extern const arm_biquad_casd_df1_inst_f32 IIR_FILTER; - -#endif /* FILTER_SETUP */ - -#endif /* ACOUSTICS_PARAMETERS_H */ diff --git a/Include/stm32f7xx_hal_conf.h b/Include/stm32f7xx_hal_conf.h deleted file mode 100644 index dbae795e..00000000 --- a/Include/stm32f7xx_hal_conf.h +++ /dev/null @@ -1,549 +0,0 @@ -/** - ****************************************************************************** - * @file stm32f7xx_hal_conf_template.h - * @author MCD Application Team - * @brief HAL configuration template file. - * This file should be copied to the application folder and renamed - * to stm32f7xx_hal_conf.h. - ****************************************************************************** - * @attention - * - *

© Copyright (c) 2017 STMicroelectronics. - * All rights reserved.

- * - * This software component is licensed by ST under BSD 3-Clause license, - * the "License"; You may not use this file except in compliance with the - * License. You may obtain a copy of the License at: - * opensource.org/licenses/BSD-3-Clause - * - ****************************************************************************** - */ - -/* Define to prevent recursive inclusion -------------------------------------*/ -#ifndef __STM32F7xx_HAL_CONF_H -#define __STM32F7xx_HAL_CONF_H - -#ifdef __cplusplus -extern "C" { -#endif - -/* Exported types ------------------------------------------------------------*/ -/* Exported constants --------------------------------------------------------*/ - -/* ########################## Module Selection ############################## */ -/** - * @brief This is the list of modules to be used in the HAL driver - */ -#define HAL_MODULE_ENABLED - -#define HAL_ADC_MODULE_ENABLED -/* #define HAL_CRYP_MODULE_ENABLED */ -/* #define HAL_CAN_MODULE_ENABLED */ -/* #define HAL_CEC_MODULE_ENABLED */ -/* #define HAL_CRC_MODULE_ENABLED */ -/* #define HAL_CRYP_MODULE_ENABLED */ -/* #define HAL_DAC_MODULE_ENABLED */ -/* #define HAL_DCMI_MODULE_ENABLED */ -/* #define HAL_DMA2D_MODULE_ENABLED */ -#define HAL_ETH_MODULE_ENABLED -/* #define HAL_NAND_MODULE_ENABLED */ -/* #define HAL_NOR_MODULE_ENABLED */ -/* #define HAL_SRAM_MODULE_ENABLED */ -/* #define HAL_SDRAM_MODULE_ENABLED */ -/* #define HAL_HASH_MODULE_ENABLED */ -/* #define HAL_I2S_MODULE_ENABLED */ -/* #define HAL_IWDG_MODULE_ENABLED */ -/* #define HAL_LPTIM_MODULE_ENABLED */ -/* #define HAL_LTDC_MODULE_ENABLED */ -/* #define HAL_QSPI_MODULE_ENABLED */ -/* #define HAL_RNG_MODULE_ENABLED */ -/* #define HAL_RTC_MODULE_ENABLED */ -/* #define HAL_SAI_MODULE_ENABLED */ -/* #define HAL_SD_MODULE_ENABLED */ -/* #define HAL_MMC_MODULE_ENABLED */ -/* #define HAL_SPDIFRX_MODULE_ENABLED */ -/* #define HAL_SPI_MODULE_ENABLED */ -/* #define HAL_TIM_MODULE_ENABLED */ -/* #define HAL_UART_MODULE_ENABLED */ -/* #define HAL_USART_MODULE_ENABLED */ -/* #define HAL_IRDA_MODULE_ENABLED */ -/* #define HAL_SMARTCARD_MODULE_ENABLED */ -/* #define HAL_WWDG_MODULE_ENABLED */ -/* #define HAL_PCD_MODULE_ENABLED */ -/* #define HAL_HCD_MODULE_ENABLED */ -/* #define HAL_DFSDM_MODULE_ENABLED */ -/* #define HAL_DSI_MODULE_ENABLED */ -/* #define HAL_JPEG_MODULE_ENABLED */ -/* #define HAL_MDIOS_MODULE_ENABLED */ -/* #define HAL_SMBUS_MODULE_ENABLED */ -/* #define HAL_EXTI_MODULE_ENABLED */ -#define HAL_GPIO_MODULE_ENABLED -#define HAL_EXTI_MODULE_ENABLED -#define HAL_DMA_MODULE_ENABLED -#define HAL_RCC_MODULE_ENABLED -#define HAL_FLASH_MODULE_ENABLED -#define HAL_PWR_MODULE_ENABLED -#define HAL_I2C_MODULE_ENABLED -#define HAL_CORTEX_MODULE_ENABLED - -/* ########################## HSE/HSI Values adaptation ##################### */ -/** - * @brief Adjust the value of External High Speed oscillator (HSE) used in your - * application. This value is used by the RCC HAL module to compute the system - * frequency (when HSE is used as system clock source, directly or through the - * PLL). - */ -#if !defined(HSE_VALUE) -#define HSE_VALUE \ - ((uint32_t)25000000U) /*!< Value of the External oscillator in Hz */ -#endif /* HSE_VALUE */ - -#if !defined(HSE_STARTUP_TIMEOUT) -#define HSE_STARTUP_TIMEOUT \ - ((uint32_t)100U) /*!< Time out for HSE start up, in ms */ -#endif /* HSE_STARTUP_TIMEOUT */ - -/** - * @brief Internal High Speed oscillator (HSI) value. - * This value is used by the RCC HAL module to compute the system - * frequency (when HSI is used as system clock source, directly or through the - * PLL). - */ -#if !defined(HSI_VALUE) -#define HSI_VALUE \ - ((uint32_t)16000000U) /*!< Value of the Internal oscillator in Hz*/ -#endif /* HSI_VALUE */ - -/** - * @brief Internal Low Speed oscillator (LSI) value. - */ -#if !defined(LSI_VALUE) -#define LSI_VALUE ((uint32_t)32000U) /*!< LSI Typical Value in Hz*/ -#endif /* LSI_VALUE */ /*!< Value of the Internal Low Speed oscillator in Hz \ - The real value may vary depending on the variations \ - in voltage and temperature. */ -/** - * @brief External Low Speed oscillator (LSE) value. - */ -#if !defined(LSE_VALUE) -#define LSE_VALUE \ - ((uint32_t)32768U) /*!< Value of the External Low Speed oscillator in Hz */ -#endif /* LSE_VALUE */ - -#if !defined(LSE_STARTUP_TIMEOUT) -#define LSE_STARTUP_TIMEOUT \ - ((uint32_t)5000U) /*!< Time out for LSE start up, in ms */ -#endif /* LSE_STARTUP_TIMEOUT */ - -/** - * @brief External clock source for I2S peripheral - * This value is used by the I2S HAL module to compute the I2S clock - * source frequency, this source is inserted directly through I2S_CKIN pad. - */ -#if !defined(EXTERNAL_CLOCK_VALUE) -#define EXTERNAL_CLOCK_VALUE \ - ((uint32_t)12288000U) /*!< Value of the Internal oscillator in Hz*/ -#endif /* EXTERNAL_CLOCK_VALUE */ - -/* Tip: To avoid modifying this file each time you need to use different HSE, - === you can define the HSE value in your toolchain compiler preprocessor. */ - -/* ########################### System Configuration ######################### */ -/** - * @brief This is the HAL system configuration section - */ -#define VDD_VALUE ((uint32_t)3300U) /*!< Value of VDD in mv */ -#define TICK_INT_PRIORITY ((uint32_t)0U) /*!< tick interrupt priority */ -#define USE_RTOS 0U -#define PREFETCH_ENABLE 0U -#define ART_ACCLERATOR_ENABLE \ - 0U /* To enable instruction cache and prefetch \ - */ - -#define USE_HAL_ADC_REGISTER_CALLBACKS \ - 0U /* ADC register callback disabled */ -#define USE_HAL_CAN_REGISTER_CALLBACKS \ - 0U /* CAN register callback disabled */ -#define USE_HAL_CEC_REGISTER_CALLBACKS \ - 0U /* CEC register callback disabled */ -#define USE_HAL_CRYP_REGISTER_CALLBACKS \ - 0U /* CRYP register callback disabled */ -#define USE_HAL_DAC_REGISTER_CALLBACKS \ - 0U /* DAC register callback disabled */ -#define USE_HAL_DCMI_REGISTER_CALLBACKS \ - 0U /* DCMI register callback disabled */ -#define USE_HAL_DFSDM_REGISTER_CALLBACKS \ - 0U /* DFSDM register callback disabled */ -#define USE_HAL_DMA2D_REGISTER_CALLBACKS \ - 0U /* DMA2D register callback disabled */ -#define USE_HAL_DSI_REGISTER_CALLBACKS \ - 0U /* DSI register callback disabled */ -#define USE_HAL_ETH_REGISTER_CALLBACKS \ - 0U /* ETH register callback disabled */ -#define USE_HAL_HASH_REGISTER_CALLBACKS \ - 0U /* HASH register callback disabled */ -#define USE_HAL_HCD_REGISTER_CALLBACKS \ - 0U /* HCD register callback disabled */ -#define USE_HAL_I2C_REGISTER_CALLBACKS \ - 0U /* I2C register callback disabled */ -#define USE_HAL_I2S_REGISTER_CALLBACKS \ - 0U /* I2S register callback disabled */ -#define USE_HAL_IRDA_REGISTER_CALLBACKS \ - 0U /* IRDA register callback disabled */ -#define USE_HAL_JPEG_REGISTER_CALLBACKS \ - 0U /* JPEG register callback disabled */ -#define USE_HAL_LPTIM_REGISTER_CALLBACKS \ - 0U /* LPTIM register callback disabled */ -#define USE_HAL_LTDC_REGISTER_CALLBACKS \ - 0U /* LTDC register callback disabled */ -#define USE_HAL_MDIOS_REGISTER_CALLBACKS \ - 0U /* MDIOS register callback disabled */ -#define USE_HAL_MMC_REGISTER_CALLBACKS \ - 0U /* MMC register callback disabled */ -#define USE_HAL_NAND_REGISTER_CALLBACKS \ - 0U /* NAND register callback disabled */ -#define USE_HAL_NOR_REGISTER_CALLBACKS \ - 0U /* NOR register callback disabled */ -#define USE_HAL_PCD_REGISTER_CALLBACKS \ - 0U /* PCD register callback disabled */ -#define USE_HAL_QSPI_REGISTER_CALLBACKS \ - 0U /* QSPI register callback disabled */ -#define USE_HAL_RNG_REGISTER_CALLBACKS \ - 0U /* RNG register callback disabled */ -#define USE_HAL_RTC_REGISTER_CALLBACKS \ - 0U /* RTC register callback disabled */ -#define USE_HAL_SAI_REGISTER_CALLBACKS \ - 0U /* SAI register callback disabled */ -#define USE_HAL_SD_REGISTER_CALLBACKS \ - 0U /* SD register callback disabled */ -#define USE_HAL_SMARTCARD_REGISTER_CALLBACKS \ - 0U /* SMARTCARD register callback disabled */ -#define USE_HAL_SDRAM_REGISTER_CALLBACKS \ - 0U /* SDRAM register callback disabled */ -#define USE_HAL_SRAM_REGISTER_CALLBACKS \ - 0U /* SRAM register callback disabled */ -#define USE_HAL_SPDIFRX_REGISTER_CALLBACKS \ - 0U /* SPDIFRX register callback disabled */ -#define USE_HAL_SMBUS_REGISTER_CALLBACKS \ - 0U /* SMBUS register callback disabled */ -#define USE_HAL_SPI_REGISTER_CALLBACKS \ - 0U /* SPI register callback disabled */ -#define USE_HAL_TIM_REGISTER_CALLBACKS \ - 0U /* TIM register callback disabled */ -#define USE_HAL_UART_REGISTER_CALLBACKS \ - 0U /* UART register callback disabled */ -#define USE_HAL_USART_REGISTER_CALLBACKS \ - 0U /* USART register callback disabled */ -#define USE_HAL_WWDG_REGISTER_CALLBACKS \ - 0U /* WWDG register callback disabled */ - -/* ########################## Assert Selection ############################## */ -/** - * @brief Uncomment the line below to expanse the "assert_param" macro in the - * HAL drivers code - */ -/* #define USE_FULL_ASSERT 1U */ - -/* ################## Ethernet peripheral configuration ##################### */ - -/* Section 1 : Ethernet peripheral configuration */ - -/* MAC ADDRESS: MAC_ADDR0:MAC_ADDR1:MAC_ADDR2:MAC_ADDR3:MAC_ADDR4:MAC_ADDR5 */ -#define MAC_ADDR0 2U -#define MAC_ADDR1 0U -#define MAC_ADDR2 0U -#define MAC_ADDR3 0U -#define MAC_ADDR4 0U -#define MAC_ADDR5 0U - -/* Definition of the Ethernet driver buffers size and count */ -#define ETH_RX_BUF_SIZE \ - ETH_MAX_PACKET_SIZE /* buffer size for receive */ -#define ETH_TX_BUF_SIZE \ - ETH_MAX_PACKET_SIZE /* buffer size for transmit */ -#define ETH_RXBUFNB ((uint32_t)4U) /* 4 Rx buffers of size ETH_RX_BUF_SIZE */ -#define ETH_TXBUFNB ((uint32_t)4U) /* 4 Tx buffers of size ETH_TX_BUF_SIZE */ - -/* Section 2: PHY configuration section */ - -/* LAN8742A_PHY_ADDRESS Address*/ -#define LAN8742A_PHY_ADDRESS 1 -/* PHY Reset delay these values are based on a 1 ms Systick interrupt*/ -#define PHY_RESET_DELAY ((uint32_t)0x000000FFU) -/* PHY Configuration delay */ -#define PHY_CONFIG_DELAY ((uint32_t)0x00000FFFU) - -#define PHY_READ_TO ((uint32_t)0x0000FFFFU) -#define PHY_WRITE_TO ((uint32_t)0x0000FFFFU) - -/* Section 3: Common PHY Registers */ - -#define PHY_BCR ((uint16_t)0x00U) /*!< Transceiver Basic Control Register */ -#define PHY_BSR ((uint16_t)0x01U) /*!< Transceiver Basic Status Register */ - -#define PHY_RESET ((uint16_t)0x8000U) /*!< PHY Reset */ -#define PHY_LOOPBACK ((uint16_t)0x4000U) /*!< Select loop-back mode */ -#define PHY_FULLDUPLEX_100M \ - ((uint16_t)0x2100U) /*!< Set the full-duplex mode at 100 Mb/s */ -#define PHY_HALFDUPLEX_100M \ - ((uint16_t)0x2000U) /*!< Set the half-duplex mode at 100 Mb/s */ -#define PHY_FULLDUPLEX_10M \ - ((uint16_t)0x0100U) /*!< Set the full-duplex mode at 10 Mb/s */ -#define PHY_HALFDUPLEX_10M \ - ((uint16_t)0x0000U) /*!< Set the half-duplex mode at 10 Mb/s */ -#define PHY_AUTONEGOTIATION \ - ((uint16_t)0x1000U) /*!< Enable auto-negotiation function */ -#define PHY_RESTART_AUTONEGOTIATION \ - ((uint16_t)0x0200U) /*!< Restart auto-negotiation function */ -#define PHY_POWERDOWN \ - ((uint16_t)0x0800U) /*!< Select the power down mode */ -#define PHY_ISOLATE \ - ((uint16_t)0x0400U) /*!< Isolate PHY from MII */ - -#define PHY_AUTONEGO_COMPLETE \ - ((uint16_t)0x0020U) /*!< Auto-Negotiation process completed */ -#define PHY_LINKED_STATUS \ - ((uint16_t)0x0004U) /*!< Valid link established */ -#define PHY_JABBER_DETECTION \ - ((uint16_t)0x0002U) /*!< Jabber condition detected */ - -/* Section 4: Extended PHY Registers */ -#define PHY_SR \ - ((uint16_t)0x1FU) /*!< PHY status register Offset */ - -#define PHY_SPEED_STATUS \ - ((uint16_t)0x0004U) /*!< PHY Speed mask */ -#define PHY_DUPLEX_STATUS \ - ((uint16_t)0x0010U) /*!< PHY Duplex mask */ - -#define PHY_ISFR \ - ((uint16_t)0x001DU) /*!< PHY Interrupt Source Flag register Offset */ -#define PHY_ISFR_INT4 ((uint16_t)0x000BU) /*!< PHY Link down inturrupt */ - -/* ################## SPI peripheral configuration ########################## */ - -/* CRC FEATURE: Use to activate CRC feature inside HAL SPI Driver - * Activated: CRC code is present inside driver - * Deactivated: CRC code cleaned from driver - */ - -#define USE_SPI_CRC 0U - -/* Includes ------------------------------------------------------------------*/ -/** - * @brief Include module's header file - */ - -#ifdef HAL_RCC_MODULE_ENABLED -#include "stm32f7xx_hal_rcc.h" -#endif /* HAL_RCC_MODULE_ENABLED */ - -#ifdef HAL_EXTI_MODULE_ENABLED -#include "stm32f7xx_hal_exti.h" -#endif /* HAL_EXTI_MODULE_ENABLED */ - -#ifdef HAL_GPIO_MODULE_ENABLED -#include "stm32f7xx_hal_gpio.h" -#endif /* HAL_GPIO_MODULE_ENABLED */ - -#ifdef HAL_DMA_MODULE_ENABLED -#include "stm32f7xx_hal_dma.h" -#endif /* HAL_DMA_MODULE_ENABLED */ - -#ifdef HAL_CORTEX_MODULE_ENABLED -#include "stm32f7xx_hal_cortex.h" -#endif /* HAL_CORTEX_MODULE_ENABLED */ - -#ifdef HAL_ADC_MODULE_ENABLED -#include "stm32f7xx_hal_adc.h" -#endif /* HAL_ADC_MODULE_ENABLED */ - -#ifdef HAL_CAN_MODULE_ENABLED -#include "stm32f7xx_hal_can.h" -#endif /* HAL_CAN_MODULE_ENABLED */ - -#ifdef HAL_CEC_MODULE_ENABLED -#include "stm32f7xx_hal_cec.h" -#endif /* HAL_CEC_MODULE_ENABLED */ - -#ifdef HAL_CRC_MODULE_ENABLED -#include "stm32f7xx_hal_crc.h" -#endif /* HAL_CRC_MODULE_ENABLED */ - -#ifdef HAL_CRYP_MODULE_ENABLED -#include "stm32f7xx_hal_cryp.h" -#endif /* HAL_CRYP_MODULE_ENABLED */ - -#ifdef HAL_DMA2D_MODULE_ENABLED -#include "stm32f7xx_hal_dma2d.h" -#endif /* HAL_DMA2D_MODULE_ENABLED */ - -#ifdef HAL_DAC_MODULE_ENABLED -#include "stm32f7xx_hal_dac.h" -#endif /* HAL_DAC_MODULE_ENABLED */ - -#ifdef HAL_DCMI_MODULE_ENABLED -#include "stm32f7xx_hal_dcmi.h" -#endif /* HAL_DCMI_MODULE_ENABLED */ - -#ifdef HAL_ETH_MODULE_ENABLED -#include "stm32f7xx_hal_eth.h" -#endif /* HAL_ETH_MODULE_ENABLED */ - -#ifdef HAL_FLASH_MODULE_ENABLED -#include "stm32f7xx_hal_flash.h" -#endif /* HAL_FLASH_MODULE_ENABLED */ - -#ifdef HAL_SRAM_MODULE_ENABLED -#include "stm32f7xx_hal_sram.h" -#endif /* HAL_SRAM_MODULE_ENABLED */ - -#ifdef HAL_NOR_MODULE_ENABLED -#include "stm32f7xx_hal_nor.h" -#endif /* HAL_NOR_MODULE_ENABLED */ - -#ifdef HAL_NAND_MODULE_ENABLED -#include "stm32f7xx_hal_nand.h" -#endif /* HAL_NAND_MODULE_ENABLED */ - -#ifdef HAL_SDRAM_MODULE_ENABLED -#include "stm32f7xx_hal_sdram.h" -#endif /* HAL_SDRAM_MODULE_ENABLED */ - -#ifdef HAL_HASH_MODULE_ENABLED -#include "stm32f7xx_hal_hash.h" -#endif /* HAL_HASH_MODULE_ENABLED */ - -#ifdef HAL_I2C_MODULE_ENABLED -#include "stm32f7xx_hal_i2c.h" -#endif /* HAL_I2C_MODULE_ENABLED */ - -#ifdef HAL_I2S_MODULE_ENABLED -#include "stm32f7xx_hal_i2s.h" -#endif /* HAL_I2S_MODULE_ENABLED */ - -#ifdef HAL_IWDG_MODULE_ENABLED -#include "stm32f7xx_hal_iwdg.h" -#endif /* HAL_IWDG_MODULE_ENABLED */ - -#ifdef HAL_LPTIM_MODULE_ENABLED -#include "stm32f7xx_hal_lptim.h" -#endif /* HAL_LPTIM_MODULE_ENABLED */ - -#ifdef HAL_LTDC_MODULE_ENABLED -#include "stm32f7xx_hal_ltdc.h" -#endif /* HAL_LTDC_MODULE_ENABLED */ - -#ifdef HAL_PWR_MODULE_ENABLED -#include "stm32f7xx_hal_pwr.h" -#endif /* HAL_PWR_MODULE_ENABLED */ - -#ifdef HAL_QSPI_MODULE_ENABLED -#include "stm32f7xx_hal_qspi.h" -#endif /* HAL_QSPI_MODULE_ENABLED */ - -#ifdef HAL_RNG_MODULE_ENABLED -#include "stm32f7xx_hal_rng.h" -#endif /* HAL_RNG_MODULE_ENABLED */ - -#ifdef HAL_RTC_MODULE_ENABLED -#include "stm32f7xx_hal_rtc.h" -#endif /* HAL_RTC_MODULE_ENABLED */ - -#ifdef HAL_SAI_MODULE_ENABLED -#include "stm32f7xx_hal_sai.h" -#endif /* HAL_SAI_MODULE_ENABLED */ - -#ifdef HAL_SD_MODULE_ENABLED -#include "stm32f7xx_hal_sd.h" -#endif /* HAL_SD_MODULE_ENABLED */ - -#ifdef HAL_MMC_MODULE_ENABLED -#include "stm32f7xx_hal_mmc.h" -#endif /* HAL_MMC_MODULE_ENABLED */ - -#ifdef HAL_SPDIFRX_MODULE_ENABLED -#include "stm32f7xx_hal_spdifrx.h" -#endif /* HAL_SPDIFRX_MODULE_ENABLED */ - -#ifdef HAL_SPI_MODULE_ENABLED -#include "stm32f7xx_hal_spi.h" -#endif /* HAL_SPI_MODULE_ENABLED */ - -#ifdef HAL_TIM_MODULE_ENABLED -#include "stm32f7xx_hal_tim.h" -#endif /* HAL_TIM_MODULE_ENABLED */ - -#ifdef HAL_UART_MODULE_ENABLED -#include "stm32f7xx_hal_uart.h" -#endif /* HAL_UART_MODULE_ENABLED */ - -#ifdef HAL_USART_MODULE_ENABLED -#include "stm32f7xx_hal_usart.h" -#endif /* HAL_USART_MODULE_ENABLED */ - -#ifdef HAL_IRDA_MODULE_ENABLED -#include "stm32f7xx_hal_irda.h" -#endif /* HAL_IRDA_MODULE_ENABLED */ - -#ifdef HAL_SMARTCARD_MODULE_ENABLED -#include "stm32f7xx_hal_smartcard.h" -#endif /* HAL_SMARTCARD_MODULE_ENABLED */ - -#ifdef HAL_WWDG_MODULE_ENABLED -#include "stm32f7xx_hal_wwdg.h" -#endif /* HAL_WWDG_MODULE_ENABLED */ - -#ifdef HAL_PCD_MODULE_ENABLED -#include "stm32f7xx_hal_pcd.h" -#endif /* HAL_PCD_MODULE_ENABLED */ - -#ifdef HAL_HCD_MODULE_ENABLED -#include "stm32f7xx_hal_hcd.h" -#endif /* HAL_HCD_MODULE_ENABLED */ - -#ifdef HAL_DFSDM_MODULE_ENABLED -#include "stm32f7xx_hal_dfsdm.h" -#endif /* HAL_DFSDM_MODULE_ENABLED */ - -#ifdef HAL_DSI_MODULE_ENABLED -#include "stm32f7xx_hal_dsi.h" -#endif /* HAL_DSI_MODULE_ENABLED */ - -#ifdef HAL_JPEG_MODULE_ENABLED -#include "stm32f7xx_hal_jpeg.h" -#endif /* HAL_JPEG_MODULE_ENABLED */ - -#ifdef HAL_MDIOS_MODULE_ENABLED -#include "stm32f7xx_hal_mdios.h" -#endif /* HAL_MDIOS_MODULE_ENABLED */ - -#ifdef HAL_SMBUS_MODULE_ENABLED -#include "stm32f7xx_hal_smbus.h" -#endif /* HAL_SMBUS_MODULE_ENABLED */ - -/* Exported macro ------------------------------------------------------------*/ -#ifdef USE_FULL_ASSERT -/** - * @brief The assert_param macro is used for function's parameters check. - * @param expr: If expr is false, it calls assert_failed function - * which reports the name of the source file and the source - * line number of the call that failed. - * If expr is true, it returns no value. - * @retval None - */ -#define assert_param(expr) \ - ((expr) ? (void)0U : assert_failed((uint8_t *)__FILE__, __LINE__)) -/* Exported functions ------------------------------------------------------- */ -void assert_failed(uint8_t *file, uint32_t line); -#else -#define assert_param(expr) ((void)0U) -#endif /* USE_FULL_ASSERT */ - -#ifdef __cplusplus -} -#endif - -#endif /* __STM32F7xx_HAL_CONF_H */ - -/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/ diff --git a/Include/stm32f7xx_it.h b/Include/stm32f7xx_it.h deleted file mode 100644 index 284ae34e..00000000 --- a/Include/stm32f7xx_it.h +++ /dev/null @@ -1,57 +0,0 @@ -/* USER CODE BEGIN Header */ -/** - ****************************************************************************** - * @file stm32f7xx_it.h - * @brief This file contains the headers of the interrupt handlers. - ****************************************************************************** - * @attention - * - *

© Copyright (c) 2020 STMicroelectronics. - * All rights reserved.

- * - * This software component is licensed by ST under BSD 3-Clause license, - * the "License"; You may not use this file except in compliance with the - * License. You may obtain a copy of the License at: - * opensource.org/licenses/BSD-3-Clause - * - ****************************************************************************** - */ -/* USER CODE END Header */ - -/* Define to prevent recursive inclusion -------------------------------------*/ -#ifndef __STM32F7xx_IT_H -#define __STM32F7xx_IT_H - -#ifdef __cplusplus -extern "C" { -#endif - -/* Includes ----------------------------------------------------------*/ -#include "main.h" -#include "stm32f7xx_hal.h" -/* Exported types ------------------------------------------------------------*/ -/* Exported constants --------------------------------------------------------*/ -/* Exported macro ------------------------------------------------------------*/ -/* USER CODE BEGIN EM */ -/* Exported functions prototypes ---------------------------------------------*/ -void NMI_Handler(void); -void HardFault_Handler(void); -void MemManage_Handler(void); -void BusFault_Handler(void); -void UsageFault_Handler(void); -void SVC_Handler(void); -void DebugMon_Handler(void); -void PendSV_Handler(void); -void SysTick_Handler(void); -void DMA2_Stream0_IRQHandler(void); -/* USER CODE BEGIN EFP */ - -/* USER CODE END EFP */ - -#ifdef __cplusplus -} -#endif - -#endif /* __STM32F7xx_IT_H */ - -/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/ diff --git a/Include/testing.h b/Include/testing.h deleted file mode 100644 index a254eed0..00000000 --- a/Include/testing.h +++ /dev/null @@ -1,65 +0,0 @@ -/** - * @file - * - * @brief File to implement tests for code analysis - * - * Required tests: - * 1. Memory usage and leakage - * 2. DMA correctly set up - * 3. ADC correctly set up - * 4. ETH correctly set up - * 5. Filter coefficients - * 6. ... - * - * @warning For optimum performance, the code should be "public", such - * that this file has access to the code. This is not done as of 22.12.2020, - * however should pherhaps be implemented over the following days/weeks - */ -#ifndef ACOUSTICS_TESTING_H -#define ACOUSTICS_TESTING_H - -#include "analyze_data.h" - -#if CURR_TESTING_BOOL - -#include - -/** - * @brief Namespace to hold the testing-functions - */ -namespace TESTING { - -/** - * @brief Function that generates TOA of the sound-signals for the different - * hydrophones. Returns the calculated TOA indirectly via @p lag_array - * - * The TOA is calculated based on the following parameters: - * @p HYD_xxx_POS - * @p SOURCE_POS - * @p SOUND_SPEED - * @p SAMPLE_FREQUENCY - * all of which are given in "parameters.h" - * - * @param lag_array An array giving the time the sound will be detected - * by the hydrophones. The array expands to - * @p lag_array = { lag_port, lag_starboard, lag_stern } - * - * @param bool_valid_parameters An int describing if the parameters - * given in parameters.h are valid. - */ -void calculate_toa_array(uint32_t lag_array[NUM_HYDROPHONES], - uint8_t &bool_valid_parameters); - -/** - * @brief Function that checks if the fictional sound-source position is within - * @p MARGIN_POS_ESTIMATE distance from the estimated position - * - * Writes the distance estimated and the actual distance to the terminal - */ -void test_trilateration_algorithm(); - -} /* namespace TESTING */ - -#endif /* CURR_TESTING_BOOL */ - -#endif /* ACOUSTICS_TESTING_H */ diff --git a/Include/trilateration.h b/Include/trilateration.h deleted file mode 100644 index 650e723c..00000000 --- a/Include/trilateration.h +++ /dev/null @@ -1,202 +0,0 @@ -/** - * @file - * - * @brief Basic functions to trilaterate the position of - * the acoustic pinger. Preferable to change it with the MANYEARS- - * library - */ -#ifndef ACOUSTICS_TRILATERATION_H -#define ACOUSTICS_TRILATERATION_H - -/** - * @brief Defines used to specify the workflow from the Eigen-library - */ -#ifndef EIGEN_DEFINES -#define EIGEN_DEFINES - -#ifndef EIGEN_NO_STATIC_ASSERT -#define EIGEN_NO_STATIC_ASSERT /* Prevents static assertion */ -#endif /* EIGEN_NO_STATIC_ASSERT */ - -#endif /* EIGEN_DEFINES */ - -#include -#include -#include -#include - -#include "parameters.h" - -/** - * @brief Typedef used during trilateration - */ -typedef Eigen::Matrix Matrix_2_3_f; -typedef Eigen::Matrix Vector_2_1_f; - -/** - * @brief Namespace/wrapper for the trilateration - */ -namespace TRILATERATION { - -/** - * @brief Constants used for the trilaterations - * - * @param max_hydrophone_distance The maximum distance - * measured between the hydrophones. - * - * @param max_time_diff The maximum time-difference that - * should be possible between the data signals - */ -extern float32_t max_hydrophone_distance; -extern float32_t max_time_diff; - -/** - * @brief Function that calculates the maximum distance - * between the hydrophones, and use it to calculate the - * allowed time_difference. See the overview in hydrophones.h - * for a better explenation of the hydrophones positioning - * - * @retval Returns 1/0 to indicate whether the values are initialized - * correctly. Returns 0 if either max_hydrophone_distance or - * maximum_time_diff is unchanged - * - * @param pos_hyd_port Position of port hydrophone - * - * @param pos_hyd_starboard Position of starboard hydrophone - * - * @param pos_hyd_stern Position of stern hydrophone - */ -uint8_t initialize_trilateration_globals(); - -/** - * @brief Initializes the matrix @p A to a 2x3 0-matrix - * - * @retval Returns a 2x3 matrix with all entries set to 0 - */ -Matrix_2_3_f initialize_A_matrix(); - -/** - * @brief Initializes the vector @p B to a 2x3 0-matrix - * - * @retval Returns a 2x1 vector with both entries set to 0 - */ -Vector_2_1_f initialize_B_vector(); - -/** - * @brief Function that check if the global variables - * @p max_hydrophone_distance and @p max_time_diff have been - * initialized correctly - * - * @retval Returns true if set up correctly - */ -uint8_t check_initialized_globals(); - -/** - * @brief Function to check the validy of each signal - * - * @retval Returns true if the values are valid, and false if not. - * If false is returned, @p bool_time_error is set to 1 - * - * @param p_lag_array Array containing the measured lags. - * @p lag_array expands to - * p_lag_array = { *p_lag_port_starboard, - * *p_lag_port_stern, - * *p_lag_starboard_stern } - * - * @param bool_time_error Int used to indicate time-error - */ -uint8_t check_valid_signals(uint32_t *p_lag_array[NUM_HYDROPHONES], - uint8_t &bool_time_error); - -/** - * @brief Helper function. Checks if the time-difference between two - * signals are valid. The time difference between the signals are checked - * against TRILATERATION::maximum_time_diff - * - * @retval Returns false/true (0/1) depending on the result of the test - * - * @param time_lhs One of the time-samples to check against - * - * @param time_rhs The other time-sample to check - */ -uint8_t check_valid_time(const uint32_t &time_lhs, const uint32_t &time_rhs); - -/** - * @brief Helper function. Checks if a given time-difference is valid. - * The time difference between the signals are checked - * against TRILATERATION::maximum_time_diff - * - * @retval Returns false/true (0/1) depending on the result of the test - * - * @param time_diff One of the time-samples to check against - */ -uint8_t check_valid_time(const uint32_t &time_diff); - -/** - * @brief Function to trilaterate the position of the acoustic pinger based - * on the time of arrival. The function uses the TDOA and linear algebra to - * calculate the position using minimal square error. - * - * The mathematics are copy/paste from - * https://math.stackexchange.com/questions/1722021/trilateration-using-tdoa - * - * Abbrivations used: - * TOA: Time of arrival - * TDOA: Time-difference of arrival - * - * @retval Returns the estimated x- and y-value indirectly using references - * - * @warning The code assumes that the hydrophones are on the same plane/level - * as the acoustic pinger. The estimates will therefore exceed the actual - * position of the acoustic pinger somewhat. - * - * @warning The code does not take into consideration any pitch/roll which will - * affect the hydrophones' position - * - * @param A A @c Matrix_2_3_f that holds the positions of, and the distances - * between the hydrophones. Size: 4x4 - * - * @param B A @c Vector_2_1_f that holds the minimal solutions to the equations. - * Size: 4x1 - * - * @param p_lag_array Array containing pointers to the cross-correlated lags. - * @p lag_array expands to - * p_lag_array = { *p_lag_port_starboard, - * *p_lag_port_stern, - * *p_lag_starboard_stern } - * - * @param x_estimate Reference to the estimated x-position. Used to return - * the x-position indirectly - * - * @param y_estimate Reference to the estimated y-position. Used to return - * the y-position indirectly - */ -uint8_t trilaterate_pinger_position(Matrix_2_3_f &A, Vector_2_1_f &B, - uint32_t *p_lag_array[NUM_HYDROPHONES], - float32_t &x_estimate, - float32_t &y_estimate); - -/** - * @brief Helper-function that set the matrices @p A and @p B to the desired - * values specified in @p TDOA_array - * - * The values of @p A and @p B are calculated from @p TDOA_array and the - * position of the hydrophones. Calcualtions are described at - * https://math.stackexchange.com/questions/1722021/trilateration-using-tdoa - * - * @param TDOA_array Array containg the calculated TDOA - * The array is given as - * @p TDOA_array = { TDOA_port_starboard, TDOA_port_stern, - * TDOA_starboard_stern } where ex. TDOA_port_starboard is the time-difference - * between port and starboard - * - * @param A The matrix containing the parameters to the linear equations - * - * @param B A vector containing the solutions to the linear equations - */ -void calculate_tdoa_matrices(float32_t TDOA_array[NUM_HYDROPHONES], - Matrix_2_3_f &A, Vector_2_1_f &B); - -} /* namespace TRILATERATION */ - -#endif /* ACOUSTICS_TRILATERATION_H */ diff --git a/Matlab/Filter_coefficients_test_script.m b/Matlab/Filter_coefficients_test_script.m deleted file mode 100644 index 2ad8c506..00000000 --- a/Matlab/Filter_coefficients_test_script.m +++ /dev/null @@ -1,96 +0,0 @@ -%% Brief -%{ -This script is designed to test if the filter coefficients used for the -DF1 IIR-filter is sufficient for the acoustic filter. - -The script creates a signal with a multitude of different frequencies. The -signal is filtered with the coefficients developed for the acoustic-project -for Vortex NTNU. - -To indicate the effect of the filter, the frequency-response is plotted -before and after filtering -%} - -%% Filter -% Filter-coefficients (numerator) -b0 = 0.58810799; -b1 = 0; -b2 = -0.58810799; -B = [b0, b1, b2]; - -% Filter coefficients (denominator) -a0 = 1; -a1 = -0.31465750; -a2 = -0.17621597; -A = [a0, a1, a2]; - -%% Signal generation -% Min frequency [Hz] -f_min = 5000; - -% Max frequency [Hz] -f_max = 50000; - -% Difference between each signal [Hz] -f_diff = 5000; - -% Number of frequencies in the signal -num_f = (f_max + f_min) / f_diff; - -% Sampling-frequency -F_s = 150000; - -% Length of signal -L = 5000; - -% Time (for signal generation) -dt = 1/F_s; -t = (0:L-1)*dt; - -% Generating the signal -y = 0; -for f = 0:num_f - 1 - y = y + sin(2*pi*(f_max + f*f_diff)*t); -end - -%% FFT and filtering -Y = fft(y); - -y_filter = filter(B, A, y); -Y_filter = fft(y_filter); - -%% Plotting -%{ -The frequency is mirrored at f = 0, where abs(Y(-f_hat)) = abs(Y(f_hat)) - -The plot only consists of the response at f > 0 to make the plot more -readable -%} - -% Frequency -f = F_s*(0:(L/2))/L; - -% The result before filtering -P2_b = abs(Y/L); -P1_b = P2_b(1:L/2+1); -P1_b(2:end-1) = 2*P1_b(2:end-1); - -% The result after filtering -P2_a = abs(Y_filter/L); -P1_a = P2_a(1:L/2+1); -P1_a(2:end-1) = 2*P1_a(2:end-1); - -subplot(1,2,1); -plot(f, P1_b); -xlabel("f"); -ylabel("|Y|"); -title("Magnitude before filtering"); -grid on; - -subplot(1,2,2); -plot(f, P1_a); -xlabel("f"); -ylabel("|Y|"); -title("Magnitude after filtering"); -grid on; - diff --git a/Matlab/butterworth_df2_4th_order.fda b/Matlab/butterworth_df2_4th_order.fda deleted file mode 100644 index 5588e3fd..00000000 Binary files a/Matlab/butterworth_df2_4th_order.fda and /dev/null differ diff --git a/Matlab/chebychev_type_1.fda b/Matlab/chebychev_type_1.fda deleted file mode 100644 index 317b2c14..00000000 Binary files a/Matlab/chebychev_type_1.fda and /dev/null differ diff --git a/Python/conftest.py b/Python/conftest.py deleted file mode 100644 index 5dd0cbfd..00000000 --- a/Python/conftest.py +++ /dev/null @@ -1,24 +0,0 @@ -import pytest - - -def pytest_addoption(parser): - parser.addoption( - "--runplot", - action="store_true", - default=False, - help="Run tests with plots", - ) - - -def pytest_configure(config): - config.addinivalue_line("markers", "plot: mark test containing plotting") - - -def pytest_collection_modifyitems(config, items): - if config.getoption("--runplot"): - # --runplot given in cli: do not skip test with plots - return - skip_plots = pytest.mark.skip(reason="need --runplot option to run") - for item in items: - if "plot" in item.keywords: - item.add_marker(skip_plots) diff --git a/Python/correlation/tdoa.py b/Python/correlation/tdoa.py index 8bc3bc58..38c10f58 100644 --- a/Python/correlation/tdoa.py +++ b/Python/correlation/tdoa.py @@ -2,6 +2,7 @@ Provide functions for finding the tdoa array from correlation. Provide function for evaluating tdoa array. """ + import numpy as np diff --git a/Python/correlation/test/test_vary_frames.py b/Python/correlation/test/test_vary_frames.py index fb775a6b..f0c77a12 100644 --- a/Python/correlation/test/test_vary_frames.py +++ b/Python/correlation/test/test_vary_frames.py @@ -2,6 +2,7 @@ Provides the possibility to change frames. Plot the signals together with the correlated signals, and find the accuracy of the result. """ + import correlation.correlation as correl import matplotlib.pyplot as plt import numpy as np diff --git a/Python/ethernet_protocol/ethernetProtocolTeensy.py b/Python/ethernet_protocol/ethernetProtocolTeensy.py deleted file mode 100644 index 08c79e8b..00000000 --- a/Python/ethernet_protocol/ethernetProtocolTeensy.py +++ /dev/null @@ -1,149 +0,0 @@ -import time -from socket import * - - -class TeensyCommunicationUDP: - # Setup the communications with Teensy on initialization - def __init__( - self, - TEENSY_IP="10.0.0.111", - TEENSY_PORT=8888, # (non-privileged ports are > 1023) - MY_PORT=9999, - MAX_PACKAGE_SIZE_RECEIVED=65536, - TIMEOUT=10, # Wait period before giving up on communications [seconds], Remember teensy takes time to calculate everything) - ): - # Teensy networking Setup - self.TEENSY_IP = TEENSY_IP - self.TEENSY_PORT = TEENSY_PORT - self.MY_PORT = MY_PORT - self.MAX_PACKAGE_SIZE_RECEIVED = MAX_PACKAGE_SIZE_RECEIVED - self.TIMEOUT = TIMEOUT - self.address = (TEENSY_IP, TEENSY_PORT) - - # Code words for communication - self.INITIALIZATION_MESSAGE = "HELLO :D" # This is a message only sent once to establish 2 way communication between Teensy and client - self.SEND_SKIP = "ss" # Send SKIP command, teensy will stop waiting for data transfer and will continue with its calculations - self.SEND_FREQUENCY = "sf" # Send frequency to look for and variance - self.GET_HYDROPHONE_DATA = "gh" # Get all 5 hydrophone raw sample data - self.GET_DSP_DATA = "gd" # Get Digital Signal Processing data -> raw data, filtered data, FFT data and peak data - - # Get PC IP - pcHostname = gethostname() - self.MY_IP = gethostbyname(pcHostname) - - # Socket setup - self.clientSocket = socket(AF_INET, SOCK_DGRAM) - self.clientSocket.settimeout(TIMEOUT) - self.clientSocket.bind((self.MY_IP, self.MY_PORT)) - - # send initialization signal - self.send_acknowledge_signal() - - def send_acknowledge_signal(self): - try: - self.clientSocket.sendto(self.INITIALIZATION_MESSAGE.encode(), self.address) - except: - pass - - def check_if_available(self): - try: - # Read data - rec_data, addr = self.clientSocket.recvfrom(self.MAX_PACKAGE_SIZE_RECEIVED) - messageReceived = rec_data.decode() - - # Check if correct signal was sent - if messageReceived == "READY": - return True - else: - return False - except: - return False - - def send_SKIP(self): - try: - # Send a request to send frequency data - self.clientSocket.sendto(self.SEND_SKIP.encode(), self.address) - except: - print("Couldn't send SKIP command...") - - def send_frequency_of_interest(self, frequencyOfInterest, frequencyVariance): - try: - # Send a request to send frequency data - self.clientSocket.sendto(self.SEND_FREQUENCY.encode(), self.address) - - # Send data - data = str(frequencyOfInterest).encode() - self.clientSocket.sendto(data, self.address) - data = str(frequencyVariance).encode() - self.clientSocket.sendto(data, self.address) - except: - print("Couldn't send Frequency data...") - - # Universal function for getting data from Teensy, it is standardized in Teensy to send data back in such format - def get_data(self): - data = [] - done = False - tempString = "" - - while not done: - # Read data - rec_data, addr = self.clientSocket.recvfrom(self.MAX_PACKAGE_SIZE_RECEIVED) - messageReceived = rec_data.decode() - - # Check if data we are receiving is a READy signal, sometimes it leaks over to the raw data signal so we need to handle it by just ignoring it - # Else check if data is done sending, else save - if messageReceived == "READY": - pass - elif messageReceived == "DONE": - done = True - else: - tempString += messageReceived - - # Try saving string into a integer array, if error -> string empty - try: - tempString = tempString[0:-1] - data = list(map(int, tempString.split(","))) - except: - data = [0] - - return data - - def get_raw_hydrophone_data(self): - # Send request - self.clientSocket.sendto(self.GET_HYDROPHONE_DATA.encode(), self.address) - - try: - # Read response from Teensy 5 times because of 5 hydrophones - allHydrophoneData = [[], [], [], [], []] - hydrophoneNr = 0 - while hydrophoneNr < 5: - allHydrophoneData[hydrophoneNr] = self.get_data() - hydrophoneNr += 1 - return allHydrophoneData - except: - return [[], [], [], [], []] - - def get_DSP_data(self): - # Send request - self.clientSocket.sendto(self.GET_DSP_DATA.encode(), self.address) - - try: - """ - DSP data is sent in a specific way since all the data types are different - That is why it is important to keep the order as it is now - - Raw samples - - Filtered samples - - FFT - - Peaks - """ - rawSampleData = self.get_data() - filteredSampleData = self.get_data() - FFTData = self.get_data() - - # For peek data we need to process it a bit more since its actually a array of [[amplitude, frequency, phase], [amplitude, frequency, phase],...] - peakDataRaw = self.get_data() - peakData = [peakDataRaw[i : i + 3] for i in range(0, len(peakDataRaw), 3)] - - return rawSampleData, filteredSampleData, FFTData, peakData - except: - return [0], [0], [0], [0] diff --git a/Python/multilateration/multilateration.py b/Python/multilateration/multilateration.py index 30ebdf19..bcd730a5 100644 --- a/Python/multilateration/multilateration.py +++ b/Python/multilateration/multilateration.py @@ -1,7 +1,6 @@ """Estimates position of sound source in xy-coordinates based on time difference of arrival between hydrophones. """ - import multilateration.parameters as param import numpy as np from signal_generation.positioning import find_maximum_distance diff --git a/Python/pulse_detection/__init__.py b/Python/pulse_detection/__init__.py deleted file mode 100644 index e69de29b..00000000 diff --git a/Python/pulse_detection/envelope_detection.py b/Python/pulse_detection/envelope_detection.py deleted file mode 100644 index 316909d1..00000000 --- a/Python/pulse_detection/envelope_detection.py +++ /dev/null @@ -1,35 +0,0 @@ -import numpy as np -import scipy.signal - - -def asynchronous_full_wave( - signal, - carrier_frequency, # [kHz] - sampling_frequency, # [kHz] -): - cutoff_frequency = ( - carrier_frequency / 2 - ) # High pass filter. Removes all the frequencies under half the carrier frequencies - ( - b, - a, - ) = scipy.signal.butter( # Using Butterworthfilter, b corresponds to zeroes and a to poles in the z-plane - # Butterworth is maximally flat in the passband so all frequencies kept are amplified by same factor N=4, # The order of the filter - Wn=cutoff_frequency * 1000, # Convert to form kHz to Hz - fs=sampling_frequency * 1000, - ) - - signal = scipy.signal.lfilter( - b, a, np.abs(signal) - ) # Filters the signal with the poles and zeroes from Butterworth - - return signal - - -def asynchronous_hilbert_complex( - signal, -): - hil_signal = scipy.signal.hilbert( - signal - ) # Hilbert filter, determines amplitude of envelope - return np.abs(hil_signal) diff --git a/Python/pulse_detection/pulse_detection.py b/Python/pulse_detection/pulse_detection.py deleted file mode 100644 index 4735649b..00000000 --- a/Python/pulse_detection/pulse_detection.py +++ /dev/null @@ -1,94 +0,0 @@ -import logging - -import numpy as np -import pulse_detection.envelope_detection as pd_ed -import scipy.signal - - -def detect_pulse_by_envelope_and_mean_variance( # Where does envelope come from? - envelope, # Envelope of signal, with interval of about 2s - carrier_frequency, - frame_length: int = 10, - sampling_frequency: float = 500, # [kHz] - threshold: float = None, -): - """Uses the signal envelope and threshold generated from mean and - variance to detect pulses. - """ - # calculate threshold: - # the idea is that given a pulse length of a few micro seconds and - # period of 2s, calculating the historic mean and variance will align - # with the noise mean and variance - historic_mean = np.mean( - envelope - ) # Getting the average of the noise in the enviroment - historic_variance = np.var(envelope) # Variance - logging.debug( - f"Historic Mean: {historic_mean:.3f}" - ) # Information for the user only about noise mean and variance - logging.debug(f"Historic Variance: {historic_variance:.3f}") - - pulse_detected = np.zeros( - len(envelope) - frame_length, dtype=np.bool_ - ) # Making new array full of zeroes in the envelope, minus frame length so that - # the array used later will not be to long, see for loop under - - if threshold is None: - threshold = ( - historic_mean + 3 * historic_variance - ) # Making a threshold based on previous mean and variance - logging.debug(f"Threshold set at: {threshold:.2f}") - - for index in range(0, len(envelope) - frame_length): - frame = envelope[ - index : (index + frame_length) - ] # Makes frames for each element in envelope - above_treshold = ( - frame > threshold - ) # True statement will give 1 at the given index. This means that signal amplitude at this index is higher than the threshold - pulse_detected[index] = np.all( - above_treshold - ) # Makes new array where the detected signal is specified - - return pulse_detected - - -def detect_pulse_by_envelope_and_differentiation( # Other way of detecting pulse - signal, - carrier_frequency, - downsampling_factor: int = 300, - sampling_frequency: float = 500, # [kHz] -): - envelope = pd_ed.asynchronous_full_wave( # Creating "envelope" (not really, should change name to filteredSignal) from filtered signal - signal=signal, - carrier_frequency=carrier_frequency, - sampling_frequency=sampling_frequency, - ) - - frame_length = downsampling_factor - envelope_means = np.zeros( - len(envelope) // frame_length - ) # Creates an envelope list that reduces length of envelope by a factor of downsampling factor - for index in range( - 0, len(envelope_means) - ): # This loop creates average of an envelope frame for all the frames - env_index = index * frame_length - envelope_means[index] = np.mean(envelope[env_index : env_index + frame_length]) - - differentiated_means = scipy.signal.lfilter( # returns current mean value minus the previous to get the difference - [1, -1], - [1], - envelope_means, - ) - diff_mean = np.mean(differentiated_means) - diff_var = np.var(differentiated_means) - logging.debug(f"Mean: {diff_mean}") # For debugging - logging.debug(f"Variance: {diff_var}") - - threshold = diff_mean + 3 * diff_var - - transition_detected = ( - differentiated_means > threshold - ) # Goes through differntiated_means array and checks if its a transition from one mean to next. Transition_detected is an array - - return transition_detected, differentiated_means diff --git a/Python/pulse_detection/pulse_determination.py b/Python/pulse_detection/pulse_determination.py deleted file mode 100644 index 74f8d549..00000000 --- a/Python/pulse_detection/pulse_determination.py +++ /dev/null @@ -1,49 +0,0 @@ -import numpy as np - - -def short_time_fourier_transform( # Search up STFT - signal: np.array, - fft_size: int, - sampling_frequency: float, # [kHz] - pulse_length: int, # [ms] -) -> np.array: - MN_point_fft = np.zeros(fft_size // 2 + 1, dtype=np.complex) - M = (pulse_length * sampling_frequency) // fft_size - for i in range(M): - N_point_fft = np.fft.rfft(signal[i * fft_size : (i + 1) * fft_size]) - - for index in range(np.size(N_point_fft)): - MN_point_fft[index] += N_point_fft[index] - - return MN_point_fft - - -def find_tone( # Determines the frequency - fft: np.array, - sampling_frequency: float, - fft_size: int, -) -> float: - frequency_bins = np.fft.rfftfreq( - fft_size, 1 / sampling_frequency - ) # divides the frequency axis into bins - tone = frequency_bins[ - np.argmax(fft) - ] # Determines which bin contains the greatest frequency magnitude - - return tone - - -def find_optimal_sampling_frequency(fft_size: int): - pinger_frequencies = {25.0, 30.0, 35.0, 40.0} - min_cost = 5000000 - optimal_sampling_freq = -1 - for sampling_freq in range(350, 500): - cost = 0 - frequency_bins = np.fft.rfftfreq(fft_size, 1 / sampling_freq) - for pinger_freq in pinger_frequencies: - cost += np.min(np.abs(frequency_bins - pinger_freq)) ** 2 - if cost < min_cost: - min_cost = cost - optimal_sampling_freq = sampling_freq - - return optimal_sampling_freq, np.fft.rfftfreq(fft_size, 1 / optimal_sampling_freq) diff --git a/Python/pulse_detection/test/test_envelope_detection.py b/Python/pulse_detection/test/test_envelope_detection.py deleted file mode 100644 index ccbca6c3..00000000 --- a/Python/pulse_detection/test/test_envelope_detection.py +++ /dev/null @@ -1,285 +0,0 @@ -import matplotlib.pyplot as plt -import numpy as np -import pytest -import scipy.signal -from pulse_detection.envelope_detection import ( - asynchronous_full_wave, - asynchronous_hilbert_complex, -) -from signal_generation import noise as sg_noise -from signal_generation import positioning as sg_pos -from signal_generation import receiver as sg_rec -from signal_generation import source as sg_src - -CARRIER_FREQUENCY = 25 # [kHz] -SAMPLING_FREQUENCY = 500 # [kHz] - - -def generate_noisy_pulses( - source_position: sg_pos.Position = sg_pos.Position(10, 10, 10), - length: int = 25000, - carrier_frequency: int = 25, # [kHz] - sampling_frequency: int = 100, # [kHz] - noise_variance: float = None, -): - positions = np.array( - [ - source_position, - ] - ) - - hydro_array = sg_rec.HydrophoneArray( - positions=positions, - ) - - pinger = [ - sg_src.Pinger( - frequency=carrier_frequency, # [kHz] - period=500.0, - pulse_length=4.0, - use_window=True, - position=sg_pos.Position(), - sampling_frequency=sampling_frequency, # [kHz] - ), - ] - - result = sg_rec.generate_signals( - sources=pinger, - receivers=hydro_array, - output_length=length, - sound_speed=1500.0, - geometric_spreading=False, - )[0] - - if noise_variance is not None: - result += sg_noise.generate_gaussian_noise( - length=length, - variance=noise_variance, - maximum_allowed_value=None, - ) - - return result - - -@pytest.mark.plot -def test_given_noisy_signal_when_asynchronous_full_wave(): - signal = generate_noisy_pulses( - length=100000, - noise_variance=0.1, - carrier_frequency=CARRIER_FREQUENCY, - sampling_frequency=SAMPLING_FREQUENCY, - ) - - result = asynchronous_full_wave( - signal=signal, - carrier_frequency=CARRIER_FREQUENCY, - sampling_frequency=SAMPLING_FREQUENCY, - ) - - fig, ax = plt.subplots(2, 1) - fig.suptitle("asynchronous full wave") - ax[0].plot(signal, label="signal") - ax[1].plot(result, label="envelop") - - plt.show() - - -@pytest.mark.plot -@pytest.mark.parametrize( - "filter_length, filter_order", - [ - (251, 2), - (51, 5), - ], -) -def test_given_noisy_signal_when_asynchronous_full_wave_and_savgol_filtered( - filter_length, - filter_order, -): - """ - Result is additionally filtered with savitzky golay filter to remove - some of the noise. - Larger filter length with small order favors envelope filtering (filtering post envelope detection) - """ - signal = generate_noisy_pulses( - length=100000, - noise_variance=0.1, - carrier_frequency=CARRIER_FREQUENCY, - sampling_frequency=SAMPLING_FREQUENCY, - ) - - filtered = scipy.signal.savgol_filter( - x=signal, - window_length=filter_length, - polyorder=filter_order, - ) - - result = asynchronous_full_wave( - signal=signal, - carrier_frequency=CARRIER_FREQUENCY, - sampling_frequency=SAMPLING_FREQUENCY, - ) - - result_pre_filtered = asynchronous_full_wave( - signal=filtered, - carrier_frequency=CARRIER_FREQUENCY, - sampling_frequency=SAMPLING_FREQUENCY, - ) - - result_post_filtered = scipy.signal.savgol_filter( - x=result, - window_length=filter_length, - polyorder=filter_order, - ) - - fig, ax = plt.subplots(2, 1) - fig.suptitle( - f"asynchronous full wave - savitzky golay len:{filter_length} order:{filter_order}" - ) - ax[0].set_title("signal") - ax[0].plot(signal, label="noisy") - ax[0].plot(filtered, label="filtered") - ax[0].legend() - ax[0].set_title("envelope") - ax[1].plot(result, label="noisy") - ax[1].plot(result_pre_filtered, label="pre-filtered") - ax[1].plot(result_post_filtered, label="post-filtered") - ax[1].legend() - - plt.tight_layout() - plt.show() - - -@pytest.mark.plot -@pytest.mark.parametrize( - "filter_length", - [ - (51), - (251), - ], -) -def test_given_noisy_signal_when_asynchronous_full_wave_and_wiener_filtered( - filter_length, -): - """ - Result is additionally filtered with wiener filter to remove - some of the noise. - """ - signal = generate_noisy_pulses( - length=100000, - noise_variance=0.1, - carrier_frequency=CARRIER_FREQUENCY, - sampling_frequency=SAMPLING_FREQUENCY, - ) - - filtered = scipy.signal.wiener( - im=signal, - mysize=filter_length, - ) - - result = asynchronous_full_wave( - signal=signal, - carrier_frequency=CARRIER_FREQUENCY, - sampling_frequency=SAMPLING_FREQUENCY, - ) - - result_pre_filtered = asynchronous_full_wave( - signal=filtered, - carrier_frequency=CARRIER_FREQUENCY, - sampling_frequency=SAMPLING_FREQUENCY, - ) - - result_post_filtered = scipy.signal.wiener( - im=result, - mysize=filter_length, - ) - - fig, ax = plt.subplots(2, 1) - fig.suptitle(f"asynchronous full wave filtered with wiener len:{filter_length}") - ax[0].set_title("signal") - ax[0].plot(signal, label="noisy") - ax[0].plot(filtered, label="filtered") - ax[0].legend() - ax[1].set_title("envelope") - ax[1].plot(result, label="noisy") - ax[1].plot(result_pre_filtered, label="pre-filtered") - ax[1].plot(result_post_filtered, label="post-filtered") - ax[1].legend() - - plt.tight_layout() - plt.show() - - -@pytest.mark.plot -@pytest.mark.parametrize( - "noise_variance", - [ - (None), - (0.01), - (0.1), - ], -) -def test_given_noisy_signal_when_asynchronous_hilbert_complex( - noise_variance, -): - signal = generate_noisy_pulses( - length=100000, - noise_variance=noise_variance, - carrier_frequency=CARRIER_FREQUENCY, - sampling_frequency=SAMPLING_FREQUENCY, - ) - - result = asynchronous_hilbert_complex( - signal=signal, - ) - - fig, ax = plt.subplots(2, 1) - fig.suptitle("asynchronous hilbert complex") - ax[0].set_title(f"signal with noise variance = {noise_variance}") - ax[0].plot(signal, label="signal") - - ax[1].set_title("envelope") - ax[1].plot(result, label="envelope") - - plt.tight_layout() - plt.show() - - -@pytest.mark.plot -@pytest.mark.parametrize( - "filter_length", - [ - (51), - (251), - ], -) -def test_given_noisy_signal_when_asynchronous_hilbert_complex_and_wiener_pre_filtered( - filter_length, -): - signal = generate_noisy_pulses( - length=100000, - noise_variance=0.1, - carrier_frequency=CARRIER_FREQUENCY, - sampling_frequency=SAMPLING_FREQUENCY, - ) - - filtered = scipy.signal.wiener( - im=signal, - mysize=filter_length, - ) - - result = asynchronous_hilbert_complex( - signal=filtered, - ) - - fig, ax = plt.subplots(2, 1) - fig.suptitle( - f"asynchronous hilbert complex filtered with wiener len:{filter_length}" - ) - ax[0].set_title("signal") - ax[0].plot(signal, label="signal") - ax[1].set_title("envelope") - ax[1].plot(result, label="envelop") - - plt.tight_layout() - plt.show() diff --git a/Python/pulse_detection/test/test_pulse_determination.py b/Python/pulse_detection/test/test_pulse_determination.py deleted file mode 100644 index d0e71e0e..00000000 --- a/Python/pulse_detection/test/test_pulse_determination.py +++ /dev/null @@ -1,63 +0,0 @@ -import numpy as np -import pulse_detection.pulse_determination as pd_pdeter - -CARRIER_FREQUENCY = 25 # [kHz] -SAMPLING_FREQUENCY = 427 # [kHz] -FFT_SIZE = 256 - - -def test_short_time_fourier_transform(): - pulse_length = 4 - noise_amplitude = 0.7 - noise_variance = 5 - fft_size = FFT_SIZE - - signal = generate_cosine_wave( - pulse_length, - CARRIER_FREQUENCY, - SAMPLING_FREQUENCY, - noise_amplitude, - noise_variance, - ) - - fft = pd_pdeter.short_time_fourier_transform( - signal, fft_size, SAMPLING_FREQUENCY, pulse_length - ) - tone = pd_pdeter.find_tone(fft, SAMPLING_FREQUENCY, fft_size) - - tolerance = SAMPLING_FREQUENCY / fft_size - - assert abs(tone - CARRIER_FREQUENCY) < tolerance - - -def test_find_optimal_sampling_frequency(): - fft_size = FFT_SIZE - sample_frequency, frequency_bins = pd_pdeter.find_optimal_sampling_frequency( - fft_size - ) - - # print("\nFIND OPTIMAL SAMPLING FREQUENCY\n") - # print("\nThe optimal sampling frequency is: ", sample_frequency) - # print("\nThe frequency bins are: \n ", frequency_bins) - - -def generate_cosine_wave( - pulse_length: int, - carrier_frequency: float, - sampling_frequency: float, - amplitude_of_secondary_freq: float, - noise_variance: float, -) -> np.array: - dt = 1 / sampling_frequency - secondary_frequency = 61.7158 - time = np.arange(0, pulse_length, dt) - - signal = np.cos(time * np.pi * 2 * carrier_frequency) - secondary_signal = amplitude_of_secondary_freq * np.cos( - time * np.pi * 2 * secondary_frequency - ) - noise = amplitude_of_secondary_freq * np.random.normal( - 0, np.sqrt(noise_variance), pulse_length * SAMPLING_FREQUENCY - ) - - return signal + secondary_signal + noise diff --git a/Python/signal_generation/conversion.py b/Python/signal_generation/conversion.py index 50489354..5aca6568 100644 --- a/Python/signal_generation/conversion.py +++ b/Python/signal_generation/conversion.py @@ -10,6 +10,7 @@ resulting_type=np.uint16, """ + import re import numpy as np diff --git a/Python/signal_generation/source.py b/Python/signal_generation/source.py index 75d30323..7e93485c 100644 --- a/Python/signal_generation/source.py +++ b/Python/signal_generation/source.py @@ -13,6 +13,7 @@ length=5000, ) """ + import numpy as np import scipy.signal.windows from positioning import Position diff --git a/Python/signal_generation/test/test_signal_generation.py b/Python/signal_generation/test/test_signal_generation.py index 3c1e375c..2c450b08 100644 --- a/Python/signal_generation/test/test_signal_generation.py +++ b/Python/signal_generation/test/test_signal_generation.py @@ -1,5 +1,6 @@ """ Provides tests to demonstrate usage of modules inside package """ + import matplotlib.pyplot as plt import numpy as np import pytest diff --git a/Python/utilities/__init__.py b/Python/utilities/__init__.py deleted file mode 100644 index e69de29b..00000000 diff --git a/Python/utilities/plot_live_teensy_data/README b/Python/utilities/plot_live_teensy_data/README deleted file mode 100644 index e1025a62..00000000 --- a/Python/utilities/plot_live_teensy_data/README +++ /dev/null @@ -1,9 +0,0 @@ -This Folder is for getting data from Teensy once it is set up with ethernet cable and is sampling some REAL data -Use this folder to start reading and displaying data Teensy is gathering and analyze the signals -This folder provides live data feedback on graphical display and data storage for later analysis - -1. Make sure Teensy has power and is connected with ethernet cable -2. Run "read_data_from_teensy.py" file -3. Run "plot_live_data_from_teensy.py" file - -Read more here: https://vortex.a2hosted.com/index.php/Teensy_communication \ No newline at end of file diff --git a/Python/utilities/plot_live_teensy_data/__init__.py b/Python/utilities/plot_live_teensy_data/__init__.py deleted file mode 100644 index e69de29b..00000000 diff --git a/Python/utilities/plot_live_teensy_data/plot_live_data_from_teensy.py b/Python/utilities/plot_live_teensy_data/plot_live_data_from_teensy.py deleted file mode 100644 index 9c7fd145..00000000 --- a/Python/utilities/plot_live_teensy_data/plot_live_data_from_teensy.py +++ /dev/null @@ -1,183 +0,0 @@ -# Import libraries -import os -import sys - -PARENT_DIR = os.path.dirname( - os.path.dirname(os.path.dirname(os.path.abspath(__file__))) -) -MY_FILE_DIR = os.path.join(PARENT_DIR, "utilities", "plot_live_teensy_data") -sys.path.append(PARENT_DIR) - -import ast -import glob -import os - -import matplotlib.animation as animation -import matplotlib.gridspec as gridspec -import matplotlib.pyplot as plt -import pandas as pd - -# Important variables for later -SAMPLE_RATE = 430_000 # 430 kHz -MAX_FREQUENCY_TO_SHOW = 60_000 # 60 kHz -FPS = 2 - -# Make a good plot layout ================================================== -fig = plt.figure() -# Create an outer GridSpec for the two columns -outer_gs = gridspec.GridSpec(1, 2, figure=fig, width_ratios=[1, 1]) -# Create an inner GridSpec for the first column -gs_hydrophone = gridspec.GridSpecFromSubplotSpec( - 5, 1, subplot_spec=outer_gs[0], hspace=0.1 -) -# Create an inner GridSpec for the second column, with height ratios for the 70%/30% split -gs_dsp = gridspec.GridSpecFromSubplotSpec( - 2, 1, subplot_spec=outer_gs[1], height_ratios=[7, 3], hspace=0.3 -) - -hydrophoneAxis = [None] * 5 - -# Add subplots in the first column for hydrophone data -for i in range(5): - hydrophoneAxis[i] = fig.add_subplot( - gs_hydrophone[i, 0], sharex=hydrophoneAxis[0] if i else None - ) - hydrophoneAxis[i].label_outer() -fig.text(0.25, 0.965, "Hydrophone Data", ha="center") - -# Add subplots in the second column -FFTAxis = fig.add_subplot(gs_dsp[0]) -filterAxis = fig.add_subplot(gs_dsp[1]) - -# Plot type so that the size is dynamic -plt.tight_layout() - -# Select nice color pallet for graphs -colorSoftPurple = (168 / 255, 140 / 255, 220 / 255) -colorSoftBlue = (135 / 255, 206 / 255, 250 / 255) -colorSoftGreen = (122 / 255, 200 / 255, 122 / 255) - -# Get the latest csv file names -listOfHydrophoneFiles = glob.glob(os.path.join(MY_FILE_DIR, "hydrophone_data", "*.csv")) -latestHydrophoneFile = max(listOfHydrophoneFiles, key=os.path.getctime) -listOfDSPFiles = glob.glob(os.path.join(MY_FILE_DIR, "DSP_data", "*.csv")) -latestDSPFile = max(listOfDSPFiles, key=os.path.getctime) - - -def display_live_data(frame): - # Read latest data - hydrophoneDataFrame = pd.read_csv(latestHydrophoneFile) - DSPDataFrame = pd.read_csv(latestDSPFile) - - # Get latest hydrophone data - hydrophoneData = [[], [], [], [], []] - try: - hydrophoneData[0] = ast.literal_eval( - hydrophoneDataFrame["hydrophone1"].tail(1).values[0] - ) - hydrophoneData[1] = ast.literal_eval( - hydrophoneDataFrame["hydrophone2"].tail(1).values[0] - ) - hydrophoneData[2] = ast.literal_eval( - hydrophoneDataFrame["hydrophone3"].tail(1).values[0] - ) - hydrophoneData[3] = ast.literal_eval( - hydrophoneDataFrame["hydrophone4"].tail(1).values[0] - ) - hydrophoneData[4] = ast.literal_eval( - hydrophoneDataFrame["hydrophone5"].tail(1).values[0] - ) - except: - print("ERROR reading hydrophone data") - - # Get DSP data - rawData = [] - filterData = [] - FFTData = [] - peaksData = [] - try: - rawData = ast.literal_eval(DSPDataFrame["raw_samples"].tail(1).values[0]) - filterData = ast.literal_eval( - DSPDataFrame["filtered_samples"].tail(1).values[0] - ) - FFTData = ast.literal_eval(DSPDataFrame["FFT"].tail(1).values[0]) - peaksData = ast.literal_eval(DSPDataFrame["peaks"].tail(1).values[0]) - except: - print("ERROR reading DSP data") - - """ - Post process DSP data to desired scale and amount - - 1. Convert FFTData to its corresponding frequency amount - 2. Cut out big FFT frequencies out as they are not relevant - 3. Cut out big peak frequencies as they are not relevant - """ - sampleLength = len(FFTData) - FFTDataFrequency = [(i * (SAMPLE_RATE / sampleLength)) for i in range(sampleLength)] - - maxFrequencyIndex = int(MAX_FREQUENCY_TO_SHOW * sampleLength / SAMPLE_RATE) - FFTDataFrequency = FFTDataFrequency[0:maxFrequencyIndex] - FFTDataAmplitude = FFTData[0:maxFrequencyIndex] - - peaksDataAmplitude = [] - peaksDataFrequency = [] - try: - peaksData = [peak for peak in peaksData if peak[1] < MAX_FREQUENCY_TO_SHOW] - peaksDataAmplitude = [subList[0] for subList in peaksData] - peaksDataFrequency = [subList[1] for subList in peaksData] - except: - print("ERROR processing DSP data") - - # Plot hydrophone data - for i in range(5): - xHydrophone = list(range(len(hydrophoneData[i]))) - hydrophoneAxis[i].clear() - hydrophoneAxis[i].plot( - xHydrophone, - hydrophoneData[i], - label=f"Hydrophone {i + 1}", - color=colorSoftBlue, - alpha=1, - ) - hydrophoneAxis[i].legend(loc="upper right", fontsize="xx-small") - - # Plot Filter response - xRaw = list(range(len(rawData))) - xFilter = list(range(len(filterData))) - filterAxis.clear() - filterAxis.set_title("Filter response") - filterAxis.plot(xRaw, rawData, label="Raw", color=colorSoftBlue, alpha=0.5) - filterAxis.plot( - xFilter, filterData, label="Filter", color=colorSoftGreen, alpha=0.7 - ) - filterAxis.legend(loc="upper right", fontsize="xx-small") - - # Plot FFT data - FFTAxis.clear() - FFTAxis.set_title("FFT") - FFTAxis.set_xlabel("Frequency [Hz]") - FFTAxis.set_ylabel("Amplitude") - FFTAxis.bar( - FFTDataFrequency, - FFTDataAmplitude, - label="FFT", - color=colorSoftPurple, - alpha=1, - width=500, - ) - FFTAxis.scatter( - peaksDataFrequency, - peaksDataAmplitude, - label="Peaks", - color="red", - alpha=0.7, - s=30, - linewidths=1.4, - marker="x", - ) - FFTAxis.legend(loc="upper right", fontsize="xx-small") - - -# Plotting live data -ani = animation.FuncAnimation(fig, display_live_data, interval=1000 / FPS) -plt.show() diff --git a/Python/utilities/plot_live_teensy_data/read_data_from_teensy.py b/Python/utilities/plot_live_teensy_data/read_data_from_teensy.py deleted file mode 100644 index d3784673..00000000 --- a/Python/utilities/plot_live_teensy_data/read_data_from_teensy.py +++ /dev/null @@ -1,126 +0,0 @@ -# Setting up libraries -import os -import sys - -PARENT_DIR = os.path.dirname( - os.path.dirname(os.path.dirname(os.path.abspath(__file__))) -) -MY_FILE_DIR = os.path.join(PARENT_DIR, "utilities", "plot_live_teensy_data") -sys.path.append(PARENT_DIR) - -import csv -import time -from datetime import datetime - -from ethernet_protocol import ethernetProtocolTeensy - -# Variables ================================================== -frequencyOfInterest = 30_000 # 20 kHz -frequencyVariance = 30_000 # 2 kHz - -# Timeout variables -# DON'T have timeout less than < 10 seconds, this WILL BRICK TEENSY!!! -timeoutMax = 60 - -# Setup ethernet protocol -teensy = ethernetProtocolTeensy.TeensyCommunicationUDP( - TEENSY_IP="10.0.0.111", - TEENSY_PORT=8888, - MY_PORT=9999, - MAX_PACKAGE_SIZE_RECEIVED=65536, - TIMEOUT=1, -) - -# Create files we will write data to -dateAndTime = datetime.now() -formattedDateAndTime = dateAndTime.strftime("%Y_%m_%d___%H_%M_%S") -hydrophoneDataHeader = [ - "hydrophone1", - "hydrophone2", - "hydrophone3", - "hydrophone4", - "hydrophone5", -] -DSPHeader = ["raw_samples", "filtered_samples", "FFT", "peaks"] - -with open( - os.path.join( - MY_FILE_DIR, "hydrophone_data", f"hydrophone_{formattedDateAndTime}.csv" - ), - "w+", - encoding="UTF8", - newline="", -) as f: - writer = csv.writer(f) - writer.writerow(hydrophoneDataHeader) - -with open( - os.path.join(MY_FILE_DIR, "DSP_data", f"DSP_{formattedDateAndTime}.csv"), - "w", - encoding="UTF8", - newline="", -) as f: - writer = csv.writer(f) - writer.writerow(DSPHeader) - -# Infinite loop for reading data -count = 0 -while True: - try: - timeStart = time.time() - while not teensy.check_if_available(): - """ - IMPORTANT! - DO NOT have "time.sleep(x)" value SMALLER than 1 second!!! - This will interrupt sampling by asking teensy if its available to many times - If less than 1 second you risc crashing teensy to PC communication O_O - """ - print("Pause time: " + str(time.time() - timeStart)) - time.sleep(1) - if time.time() - timeStart > timeoutMax: - break - teensy.send_acknowledge_signal() - - teensy.send_frequency_of_interest(frequencyOfInterest, frequencyVariance) - hydrophoneData = teensy.get_raw_hydrophone_data() - rawSampleData, filteredSampleData, FFTData, peakData = teensy.get_DSP_data() - teensy.send_SKIP() # Once we are done we NEED to send teensy a confirmation code so that it can continue to calculate with the new given information - - # Save data to csv files - try: - with open( - os.path.join( - MY_FILE_DIR, - "hydrophone_data", - f"hydrophone_{formattedDateAndTime}.csv", - ), - "a", - encoding="UTF8", - newline="", - ) as f: - writer = csv.writer(f) - writer.writerow(hydrophoneData) - - with open( - os.path.join( - MY_FILE_DIR, "DSP_data", f"DSP_{formattedDateAndTime}.csv" - ), - "a", - encoding="UTF8", - newline="", - ) as f: - writer = csv.writer(f) - writer.writerow([rawSampleData, filteredSampleData, FFTData, peakData]) - print("Data Saved") - except: - print("ERROR saving") - except: - print("ERROR") - - # For users to see that the loop is updating - count += 1 - print(f"Try count: {count}") - print() - - # A little pause to not overwhelm the processor - time.sleep(1) diff --git a/Python/utilities/test/__init__.py b/Python/utilities/test/__init__.py deleted file mode 100644 index e69de29b..00000000 diff --git a/Resource/CMSIS/Lib/ARM/arm_cortexM7b_math.lib b/Resource/CMSIS/Lib/ARM/arm_cortexM7b_math.lib deleted file mode 100644 index 5a39aa0d..00000000 Binary files a/Resource/CMSIS/Lib/ARM/arm_cortexM7b_math.lib and /dev/null differ diff --git a/Resource/CMSIS/Lib/ARM/arm_cortexM7bfdp_math.lib b/Resource/CMSIS/Lib/ARM/arm_cortexM7bfdp_math.lib deleted file mode 100644 index 08b54238..00000000 Binary files a/Resource/CMSIS/Lib/ARM/arm_cortexM7bfdp_math.lib and /dev/null differ diff --git a/Resource/CMSIS/Lib/ARM/arm_cortexM7bfsp_math.lib b/Resource/CMSIS/Lib/ARM/arm_cortexM7bfsp_math.lib deleted file mode 100644 index c5a3584c..00000000 Binary files a/Resource/CMSIS/Lib/ARM/arm_cortexM7bfsp_math.lib and /dev/null differ diff --git a/Resource/CMSIS/Lib/ARM/arm_cortexM7l_math.lib b/Resource/CMSIS/Lib/ARM/arm_cortexM7l_math.lib deleted file mode 100644 index f16d4653..00000000 Binary files a/Resource/CMSIS/Lib/ARM/arm_cortexM7l_math.lib and /dev/null differ diff --git a/Resource/CMSIS/Lib/ARM/arm_cortexM7lfdp_math.lib b/Resource/CMSIS/Lib/ARM/arm_cortexM7lfdp_math.lib deleted file mode 100644 index 7cba53f8..00000000 Binary files a/Resource/CMSIS/Lib/ARM/arm_cortexM7lfdp_math.lib and /dev/null differ diff --git a/Resource/CMSIS/Lib/ARM/arm_cortexM7lfsp_math.lib b/Resource/CMSIS/Lib/ARM/arm_cortexM7lfsp_math.lib deleted file mode 100644 index fa75da1d..00000000 Binary files a/Resource/CMSIS/Lib/ARM/arm_cortexM7lfsp_math.lib and /dev/null differ diff --git a/Resource/CMSIS/Lib/GCC/libarm_cortexM7l_math.a b/Resource/CMSIS/Lib/GCC/libarm_cortexM7l_math.a deleted file mode 100644 index 75aac266..00000000 Binary files a/Resource/CMSIS/Lib/GCC/libarm_cortexM7l_math.a and /dev/null differ diff --git a/Resource/CMSIS/Lib/GCC/libarm_cortexM7lfdp_math.a b/Resource/CMSIS/Lib/GCC/libarm_cortexM7lfdp_math.a deleted file mode 100644 index 777cb049..00000000 Binary files a/Resource/CMSIS/Lib/GCC/libarm_cortexM7lfdp_math.a and /dev/null differ diff --git a/Resource/CMSIS/Lib/GCC/libarm_cortexM7lfsp_math.a b/Resource/CMSIS/Lib/GCC/libarm_cortexM7lfsp_math.a deleted file mode 100644 index 0f4894e4..00000000 Binary files a/Resource/CMSIS/Lib/GCC/libarm_cortexM7lfsp_math.a and /dev/null differ diff --git a/cpp/teensy/lib/DSP/DSP.cpp b/cpp/teensy/lib/DSP/DSP.cpp deleted file mode 100644 index 8985b000..00000000 --- a/cpp/teensy/lib/DSP/DSP.cpp +++ /dev/null @@ -1,439 +0,0 @@ -/* -Code for digital signal processing -Processes raw sound data sampled in a way that can be used further down the line -*/ -#include -#include - -namespace DigitalSignalProcessing { -// We do not care about frequencies up to 510k Hz, so we define a variable for -// indexes of indexes, go to the h file -const q15_t samplesOfInterest = FREQUENCY_LIMIT * SAMPLE_LENGTH / SAMPLE_RATE; -const int fOrder = 9; -const int fOrder2 = 2; - -/* - Coefficients for filter found at https://www.meme.net.au/butterworth.html, - put 9th order filter, 510kHz sampling rate and 50kHz cut-off - put 2th order filter, 430kHz sampling rate and 50kHz cut-off - */ -const float32_t aFilterCoeffs[fOrder] = {5.4569203401896500, -13.7047980216478000, 20.6476635308150000, -20.4748421533297000, 13.8143215886326000, -6.3261752484730100, 1.8924462642157100, -0.3350397779275800, 0.0267111235596287}; -const float32_t bFilterCoeffs[fOrder + 1] = {0.00000545381633879714, 0.00004908434704917420, 0.00019633738819669700, 0.00045812057245895900, 0.00068718085868843900, 0.00068718085868843900, 0.00045812057245895900, 0.00019633738819669700, 0.00004908434704917420, 0.00000545381633879714}; - -const float32_t aFilterCoeffs2[fOrder2] = {0.00101196462632, -0.00035885208947}; -const float32_t bFilterCoeffs2[fOrder2 + 1] = {0.000086700190740, 0.000173400381481, 0.000086700190740}; - -// Coefficients for 1th order filter, 430 kHz sampling rate, 50 kHz cut-off calculated manually with the help of this research paper -// https://www.researchgate.net/publication/338022014_Digital_Implementation_of_Butterworth_First-Order_Filter_Type_IIR -const float32_t aFilterCoeffs1[] = {1.0, -0.44669}; -const float32_t bFilterCoeffs1[] = {0.27665, 0.27665}; - -/* - Bit reversing is applied in a lot of FFT - algorithms for increase efficiency. - */ -const uint32_t doBitReverse = 1; - -// Constants in q_15 format done right -const q15_t PI_q15 = (q15_t)(PI * (1 << 15) + 0.5); - -q15_t* filter_butterwort_9th_order_50kHz(int16_t* samplesRaw) { - // Create array to store the filtered samples - static q15_t samples[SAMPLE_LENGTH]; - - /* - Implement Butterwort filter of "fOrder" - y = (a_1 * y_1 + .... + a_n * y_n) + (b_1 * x_1 + ... b_m * x_m) - Se Wiki: - http://vortex.a2hosted.com/index.php/Acoustics_Digital_Signal_Processing_(DSP) - Se source: https://www.meme.net.au/butterworth.html - */ - - /* - Iterate through each index of the raw samples, and apply filtering to - them. Starting at fOrder because we can't use an index outside of the - samples array. - */ - for (int i = fOrder; i < SAMPLE_LENGTH; i++) { - float32_t output_influence = 0; - /* We iterate through the previous filtered samples for the - filtering, as it is more clean and convenient. */ - for (int k = 0; k < fOrder; k++) { - output_influence += aFilterCoeffs[k] * samples[i - (k + 1)]; - } - - float32_t input_influence = 0; - /* We iterate through the previous unfilteredsamples for the - filtering, as it is more clean and convenient.*/ - for (int k = 0; k < fOrder + 1; k++) { - input_influence += bFilterCoeffs[k] * (samplesRaw[i - k]); - input_influence += bFilterCoeffs[k] * (samplesRaw[i - k]); - } - - float influenceTotalFloat = output_influence + input_influence; - - // Convert float to q15 datatype in the correct way - q15_t influenceTotalQ15 = (q15_t)influenceTotalFloat; - samples[i] = influenceTotalQ15 * FILTER_AMPLIFICATION; - } - return samples; -} - -q15_t* filter_butterwort_2th_order_50kHz(int16_t* samplesRaw) { - // Create array to store the filtered samples - static q15_t samples[SAMPLE_LENGTH]; - - /* - Implement Butterwort filter of "fOrder" - y = (a_1 * y_1 + .... + a_n * y_n) + (b_1 * x_1 + ... b_m * x_m) - Se Wiki: - http://vortex.a2hosted.com/index.php/Acoustics_Digital_Signal_Processing_(DSP) - Se source: https://www.meme.net.au/butterworth.html - */ - - /* - Iterate through each index of the raw samples, and apply filtering to - them. Starting at fOrder2 because we can't use an index outside of the - samples array. - */ - for (int i = fOrder2; i < SAMPLE_LENGTH; i++) { - float32_t output_influence = 0; - /* We iterate through the previous filtered samples for the - filtering, as it is more clean and convenient. */ - for (int k = 0; k < fOrder2; k++) { - output_influence += aFilterCoeffs2[k] * samples[i - (k + 1)]; - } - - float32_t input_influence = 0; - /* We iterate through the previous unfilteredsamples for the - filtering, as it is more clean and convenient.*/ - for (int k = 0; k < fOrder2 + 1; k++) { - input_influence += bFilterCoeffs2[k] * (samplesRaw[i - k] * FILTER_AMPLIFICATION); - } - - float influenceTotalFloat = output_influence + input_influence; - - // Convert float to q15 datatype in the correct way - q15_t influenceTotalQ15 = (q15_t)influenceTotalFloat; - samples[i] = influenceTotalQ15; - } - return samples; -} - -q15_t* filter_butterwort_1th_order_50kHz(int16_t* samplesRaw) { - // Create array to store the filtered samples - static q15_t samples[SAMPLE_LENGTH]; - int16_t i = 1; - static q15_t input[2] = {0, 0}; // Buffer for previous inputs - static q15_t output[2] = {0, 0}; // Buffer for previous outputs - q15_t inputTotal = 0; - q15_t outputTotal = 0; - - for (int index = 0; index < SAMPLE_LENGTH; index++) { - // shift the old samples - input[i - 1] = input[i]; - output[i - 1] = output[i]; - - // get the new input - input[i] = (q15_t)samplesRaw[index] * FILTER_AMPLIFICATION; - - // calculate the new output - inputTotal = bFilterCoeffs1[0] * input[i - 0] + bFilterCoeffs1[1] * input[i - 1]; - outputTotal = aFilterCoeffs1[1] * output[i - 1]; - output[i] = inputTotal - outputTotal; - output[i] = aFilterCoeffs1[0] * output[i - 0]; - - // store the new output - samples[index] = output[i]; - } - return samples; -} - -/* - Instead of taking the full FFT in a signle function we split it - We calculating first the raw values out of FFT witch are "Real" and "Imaginary" - values these values are really interesting since this raw format can be used to - calculate both amplitude, frequencies and phase shift of a signal - */ -q15_t* FFT_raw(q15_t* samples) { - /* - To store the results of fft with - complex numbers, need to have double the - size of the sample length - z = a + bi, (a1, b1, a2, b2, a3, b3 ... ) - */ - static q15_t resultsRaw[2 * SAMPLE_LENGTH]; - - /* Forward transform, which is what we want, - we want to go from time to frequency domain.*/ - uint32_t ifftFlag = 0; - - arm_rfft_instance_q15 fftInstance; // Must exist, nothing to say. - - // Initialize the rfft - arm_rfft_init_q15(&fftInstance, SAMPLE_LENGTH, ifftFlag, doBitReverse); - - // Scale the samples for better contrasts. - arm_scale_q15(samples, SCALE_FACTOR, BITSHIFT, samples, SAMPLE_LENGTH); - - // The FFT itself, output is the FFT complex array - arm_rfft_q15(&fftInstance, samples, resultsRaw); - - return resultsRaw; -} - -q15_t* FFT_mag(q15_t* resultsRaw) { - /* - Create an empty array to store the magnitude - calculations of the FFT. - As we are not dealing with complex numbers - anymore, it is the size of the sample length - */ - static q15_t results[SAMPLE_LENGTH]; - - // Converts the complex array into a magnitude array. - arm_cmplx_mag_q15(resultsRaw, results, SAMPLE_LENGTH); - - return results; -} - -/* - We will be returning q31_t datatypes as - the frequency numbers become too great to - handle for q15_t - Here we take inn FFT response and return the peaks we find - we return the peaks: - - Amplitude - - Frequency - - Phase shift - */ -std::vector> peak_detection(q15_t* resultsRaw, q15_t* results) { - // Dynamically allocate the 2D array - q31_t peaks[SAMPLE_LENGTH][2]; - - /* - Once we allocated the memory to the 2d array, the memory that we have - allocated was already in use, so it had values from before, from other - tasks. In order to have a clean slate we must iterate through the array to - make sure we don't read wrong values at the end - */ - - /* - Fill the array with 0s, this is important so we don't end up - having memory that is filled with other stuff - */ - for (int i = 0; i < SAMPLE_LENGTH; i++) { - peaks[i][0] = 0; - peaks[i][1] = 0; - } - - for (int i = 1; i < SAMPLE_LENGTH - 2; i++) { - // Check if a sample is greater than both of its neighboring samples. - if (results[i] >= results[i - 1] && results[i] >= results[i + 1]) { - peaks[i][0] = results[i]; - // Fill the array with the frequency and attached magnitude - peaks[i][1] = i * SAMPLE_RATE / SAMPLE_LENGTH; - } - } - - /* - We make an array with the results sorted for use in the median - calculation. A big part of the 510k Hz zone is either 0 or very low, so not - only is is performance wise wasteful, it is also for calculating a proper - median - */ - q15_t resultsSort[samplesOfInterest]; - - // Sorting algorithm - for (int i = 0; i < samplesOfInterest; i++) { - resultsSort[i] = results[i]; - } - - for (int i = 1; i < samplesOfInterest; i++) { - int key = resultsSort[i]; - int j = i - 1; - - while (j >= 0 && resultsSort[j] > key) { - resultsSort[j + 1] = resultsSort[j]; - j--; - } - - resultsSort[j + 1] = key; - } - - /* - As we may easily have an even number - of results, we calculate the median using the average of the two middle - values. We are aware the formatting is wonky, but that's how it - has to be - */ - q15_t avgMedian = (resultsSort[(samplesOfInterest / 2) - 1] + resultsSort[samplesOfInterest / 2]) / 2; - avgMedian += PEAK_THRESHOLD; - - /* - The next section is nothing short of a crime against all those who want - optimized code, we sincerely apologize for leaving you with this The - following code is so that we return a 2D array only the size of the number - of peaks we actually have - */ - int numNotNullValuesInList = 0; - for (int i = 0; i < SAMPLE_LENGTH; i++) { - /* - We filter by the median*3 because it works, the x3 - is there just because mediean is too small to filter - out all the "false" peaks - */ - if (peaks[i][0] <= avgMedian * 3) { - // if the magnitue does not pass the threshold we bring the entire - // index to 0 - peaks[i][0] = 0; - peaks[i][1] = 0; - } else { - // If it passes we count it towards the number of peaks we have - numNotNullValuesInList++; - } - } - /* - This is something that needs to be resolved ASAP, we are putting in the - length of the array in the very front of the array This is because q31_t - does not work seem well with the sizeof() function for arrays. This is why - the array has 1 extra index - */ - numNotNullValuesInList++; - - /* - Dynamically allocate space to a new array for the peaks we actually want to return - notice the use of numNonNull - */ - std::vector> peaksReturn(numNotNullValuesInList, std::vector(3)); - - /* - We send the length of the array through the 1st index of the array. It is - very important to find another solution. Maybe look into using the uint16_t - datatype - */ - peaksReturn[0][0] = numNotNullValuesInList; - peaksReturn[0][1] = numNotNullValuesInList; - peaksReturn[0][2] = numNotNullValuesInList; - - // Start with 1 because length of the list is in the first element - // Again, we need to find a better way to return length of the array then - // putting it in the first element of the array :/ - int tempCount = 1; - - q15_t real = 0; - q15_t imag = 0; - q15_t phase = 0; - - for (int i = 0; i < SAMPLE_LENGTH; i++) { - /* - We already filtered out non peak values - So all values will be 0 except the peaks that are left unchanged - - if (peaks[i][0] > avgMedian * 2) { - */ - if (peaks[i][0] > 0) { - // Fill the return array with the peaks. - peaksReturn[tempCount][0] = peaks[i][0]; - peaksReturn[tempCount][1] = peaks[i][1]; - - /* - Calculate phase shift of the peak frequency - - Since we took in the raw values of the FFT - We now know the: - Real -> x-axis - Imaginary -> y-axis - arctan(y/x) = angle - angle = phase shift - */ - real = resultsRaw[i * 2]; - imag = resultsRaw[i * 2 + 1]; - - /* - First we check if our Real value is 0 to insure we dont divide by 0 - If real is 0 we know that angle must be either 0 or +-pi/2 - */ - if ((real == 0) && (imag == 0)) { - phase = 0; - } else if ((real == 0) && (imag > 0)) { - phase = q15_divide(PI_q15, 2); - } else if ((real == 0) && (imag < 0)) { - phase = -q15_divide(PI_q15, 2); - } else { - phase = q15_taylor_atan(q15_divide(imag, real)); - } - - peaksReturn[tempCount][2] = phase; - - /* - As our first index is reserved for the length of the array - we use tempCount, which is 1 ahead of i, instead of i - */ - tempCount++; - } - } - - return peaksReturn; -} - -float32_t phaseQ31_to_radianFloat32(q31_t phaseQ15) { - float32_t pi = 3.141592653589793f; - float32_t conversionRatio = 32768.0f; - - return (phaseQ15 / conversionRatio) * pi; -} - -// Function for inside use only -// ================================================== -q15_t q15_divide(q15_t a, q15_t b) { - // Cast the dividend and divisor to int32_t to avoid overflow - int32_t a_scaled = (int32_t)a; - int32_t b_scaled = (int32_t)b; - - // Scale the dividend by the appropriate factor (2^15 for q15_t) - a_scaled <<= 15; - - // Perform the integer division - int32_t result = a_scaled / b_scaled; - - // Cast the result back to q15_t, if necessary - return (q15_t)result; -} - -/* - Since the CMSIS ARM libary uses FIXED pointer arithmetic we cant use - conventional means Normal Arduino math uses FLOAT pointer arithmetic which are - slower and not compatible with CMSIS q_15 data type This is why we make a FIXED - pointer arithmetic function to do "arctan" to get angle This method of arctan is - a aproximation algorithm using taylor series - - Check wiki for more info: - https://proofwiki.org/wiki/Power_Series_Expansion_for_Real_Arctangent_Function - */ -q15_t q15_taylor_atan(q15_t x) { - // Change this variable, the bigger the more accurate the value but the - // longer computational time required - int TAYLOR_SERIES_TERMS = 10; // - - // Variables - q15_t x_squared, term, result; - int32_t temp; - - x_squared = (q15_t)(((int32_t)x * x) >> 15); - term = x; - result = x; - - for (int i = 1; i < TAYLOR_SERIES_TERMS; i++) { - term = (q15_t)((((int32_t)term * x_squared) + (1 << 14)) >> 15); - temp = (int32_t)term / ((2 * i + 1) << 15); - if (i % 2 == 0) { - result += temp; - } else { - result -= temp; - } - } - - return result; -} -} // namespace DigitalSignalProcessing \ No newline at end of file diff --git a/cpp/teensy/src/main.cpp b/cpp/teensy/src/main.cpp deleted file mode 100644 index 22fa9886..00000000 --- a/cpp/teensy/src/main.cpp +++ /dev/null @@ -1,315 +0,0 @@ -/* -IMPORTANT INFORMATION! - -Some libraries are not imported properly when downloading from GitHub -This is because they are to big and so they get auto deleted -For now the libraries that need to be manually installed are -- CMSIS -To import libraries properly do this: -Copy file from ".\vortex-acoustics\Resource\CMSIS" to ".\vortex-acoustics\cpp\teensy\.pio\libdeps\teensy41\CMSIS" - -Code written by: Vortex NTNU -*/ - -// CMSIS Libraries -#include "arm_const_structs.h" -#include "arm_math.h" - -// Arduino Libraries -#include -#include - -// Sampling Analog to Digital Converter (ADC) Libraries -#include "GPT.h" -#include "PIT.h" -#include "adc.h" -#include "clock.h" -#include "gpio.h" -#include "gpioInterrupt.h" - -// Digital Signal Processing (DSP) Libraries -#include "DSP.h" - -// Libraries for Ethernet -#include "ethernetModule.h" -#include "teensyUDP.h" - -// Variables for Debugging ========== -/* -These are just to measure the time it takes to run the entire code. -*/ -unsigned long timeDiff; -unsigned long startTime; -unsigned long endTime; - -// Variables for Sampling ========== -// to be safe should be a bit under 1500. If it sampled more than 1500 for some reason, the data gathered will be inconsistent. -uint16_t number_samples = SAMPLE_LENGTH * 3; -float sample_period = 2.3; // >= MIN_SAMP_PERIOD_TIMER -int16_t samplesRawHydrophone1[SAMPLE_LENGTH * 3]; -int16_t samplesRawHydrophone2[SAMPLE_LENGTH * 3]; -int16_t samplesRawHydrophone3[SAMPLE_LENGTH * 3]; -int16_t samplesRawHydrophone4[SAMPLE_LENGTH * 3]; -int16_t samplesRawHydrophone5[SAMPLE_LENGTH * 3]; -#define SAMPLING_TIMEOUT 10000 // [10 s] If sampling takes to long before finding a frequency of interest we exit the loop and later try again - -// Variables for Digital Signal Processing ========== -int16_t samplesRawForDSP[SAMPLE_LENGTH]; -q15_t* samplesFiltered; -q15_t* FFTResultsRaw; -q15_t* FFTResults; -std::vector> peaks; -int16_t lengthOfPeakArray; - -// Variables for Peak Detection ========== -int32_t frequencyOfInterest = 0; // 0 Hz -int32_t frequencyVariance = 0; // +-0 Hz - -// Variables for data transmission ========== -uint8_t* clientIP; -uint16_t clientPort; -void communicationTeensy(); - -void setup() { - Serial.begin(9600); - delay(1000); // 1 second pause for giving time to enter serial monitor - Serial.println("1 - Serial connected"); - Serial.println(); - - // Ethernet Setup PART 1 (START) ==================================================================================================== - /* - NOTE: This code HAS to come befor "Sampling Setup", otherwise some values are configured incorrectly - Why? I have no Idea, some memory magic probably =_= - */ - // Ethernet init - Serial.print("2 - Ethernet Configuration"); - ethernetModule::UDP_init(); - Serial.println(); - // Ethernet Setup PART 1 (STOP) ==================================================================================================== - - // Sampling Setup (START) ==================================================================================================== - // initializing ADC before being able to use it - Serial.println("3 - Initializing ADC"); - adc::init(); - // Setup parameters for ADC - uint32_t ADC_reg_config; - ADC_reg_config = (1 << CONFIG_WRITE_EN) | (1 << CONFIG_PD_D) | (1 << CONFIG_REFEN) | (0x3FF << CONFIG_REFDAC) | (1 << CONFIG_VREF); - // Configure ADC - adc::config(ADC_reg_config); - adc::setup(); - // Double check that the number of samples we want to take in doesn't overflow the ADC ring buffer space - if (number_samples > 3 * SAMPLE_LENGTH) { - number_samples = 3 * SAMPLE_LENGTH; - } - Serial.println("3 - ADC setup done"); - Serial.println(); - // Sampling Setup (STOP) ==================================================================================================== - - // Digital Signal Processing Setup (START) ==================================================================================================== - // Fill up buffers with 0s first to not get unexpected errors - samplesFiltered = DigitalSignalProcessing::filter_butterwort_9th_order_50kHz(samplesRawForDSP); - FFTResultsRaw = DigitalSignalProcessing::FFT_raw(samplesFiltered); - FFTResults = DigitalSignalProcessing::FFT_mag(FFTResultsRaw); - peaks = DigitalSignalProcessing::peak_detection(FFTResultsRaw, FFTResults); - lengthOfPeakArray = peaks[0][0]; - Serial.println("4 - DSP Setup Complete"); - Serial.println(); - // Digital Signal Processing Setup (STOP) ==================================================================================================== - - // Ethernet Setup PART 2 (START) ==================================================================================================== - /* - NOTE: This code HAS to come after "Digital Signal Processing Setup" - Otherwise when client request some data, the data we are pointing to has not been setup yet - This will cause Teensy to look for data that doesn't exist and will crash the system O_O - NOTE: This code HAS to come after "Digital Signal Processing Setup" - Otherwise when client request some data, the data we are pointing to has not been setup yet - This will cause Teensy to look for data that doesn't exist and will crash the system O_O - */ - // Wait until someone is connected and get their IP and Port address - Serial.println("5 - Waiting for client connection..."); - while (!ethernetModule::UDP_check_if_connected()) - ; - clientIP = ethernetModule::get_remoteIP(); - clientPort = ethernetModule::get_remotePort(); - // Wait until client sends SKIP request to indicate they are ready to start receiving data - Serial.println("5 - Waiting for client configuration..."); - communicationTeensy(); - Serial.println("5 - Client CONNECTED"); - Serial.println(); - // Ethernet Setup PART 2 (STOP) ==================================================================================================== - - Serial.println(); - Serial.println("=================================================="); - Serial.println("SETUP COMPLETE :D"); - Serial.println("=================================================="); - Serial.println(); -} - -void loop() { - // Sampling (START) ==================================================================================================== - // Start sampling ONLY use BLOCKING, others are not implemented - adc::startConversion(sample_period, adc::BLOCKING); - // Start sampling into the buffer - uint8_t buffer_to_check = adc::active_buffer; - - /* - !IMPORTANT! - ! Digital Signal Processing (DSP) MUST be FASTER than Sampling ! - ! Now the whole DSP + peak detection (PD) is less than 700 us, this is good ! - ! DSP + PD should NEVER be over 2000 us, this will CRASH the system ! - - Have a endless loop that samples signals - Process signal and check for any peaks - If peaks of interest detected exit loop and process data further down the line - - If sampling takes to long to find a peak we exit the loop and try again later - */ - int32_t frequencyOfInterestMax = frequencyOfInterest + frequencyVariance; - int32_t frequencyOfInterestMin = frequencyOfInterest - frequencyVariance; - uint8_t found = 0; - unsigned long samplingStartTime = millis(); - while (!found) { - // Wait until first ring buffer is filled - while (!adc::buffer_filled[buffer_to_check]) - ; - - // Save raw sampled data - for (uint16_t i = 0; i < SAMPLE_LENGTH; i++) { - samplesRawForDSP[i] = (int16_t)adc::channel_buff_ptr[1][buffer_to_check][i]; - } - - // Digital Signal Processing (START) ==================================================================================================== - // Filter raw samples - samplesFiltered = DigitalSignalProcessing::filter_butterwort_1th_order_50kHz(samplesRawForDSP); - - // Preform FFT calculations on filtered samples - FFTResultsRaw = DigitalSignalProcessing::FFT_raw(samplesFiltered); - FFTResults = DigitalSignalProcessing::FFT_mag(FFTResultsRaw); - - // Get peaks of frequencies that might be of interest and their useful information like amplitude, frequency and phase - peaks = DigitalSignalProcessing::peak_detection(FFTResultsRaw, FFTResults); - - /* - Since we are storing the length of the array in the first index, we do not start from 0 in the array when printing out. - Find out how to get length of a 2D array of a q31_t datatype. - For now we return the length of the array in the first index of 2D array, This must be solved - this is NOT a good solution. - */ - lengthOfPeakArray = peaks[0][0]; - - /* - TIPS: For getting phase of the peak FFTs from q31 format that we dont understand to radians in floats, use this: - phaseQ31_to_radianFloat32(peaks[x][2]); - */ - // Digital Signal Processing (STOP) ==================================================================================================== - // Check if any of the peaks are of interest - for (int i = 1; i < lengthOfPeakArray; i++) { - int32_t peakFrequency = peaks[i][1]; - if ((peakFrequency < frequencyOfInterestMax) && (peakFrequency > frequencyOfInterestMin)) { - found = 1; - found = 1; - } - } - // Increment buffer to check - buffer_to_check = (buffer_to_check + 1) % (BUFFER_PER_CHANNEL); - - // Check if sampling has taken to long and if so exit the loop and try again later - if ((millis() - samplingStartTime) > SAMPLING_TIMEOUT) { - break; - } - } - // After finding peaks of interest let the last sampling sequence finish - // while (!adc::buffer_filled[buffer_to_check]); - // Stop Sampling - adc::stopConversion(); - Serial.println("Stoped sampling"); - - // Process data from the ring-buffer - // active buffer is one further than the last filled one, which is the oldest one now - uint8_t bufferIndex = adc::active_buffer; - bufferIndex = (bufferIndex + 1) % BUFFER_PER_CHANNEL; - // Saving data into array we will use further down the line - uint16_t index = 0; - for (uint8_t i = 0; i < BUFFER_PER_CHANNEL; i++) { - // Combine all 3 buffers from chanels into one BIG array - for (uint16_t u = 0; u < SAMPLE_LENGTH; u++) { - index = (SAMPLE_LENGTH * i) + u; - - samplesRawHydrophone1[index] = (int16_t)adc::channel_buff_ptr[1][bufferIndex][u]; - samplesRawHydrophone2[index] = (int16_t)adc::channel_buff_ptr[2][bufferIndex][u]; - samplesRawHydrophone3[index] = (int16_t)adc::channel_buff_ptr[3][bufferIndex][u]; - samplesRawHydrophone4[index] = (int16_t)adc::channel_buff_ptr[4][bufferIndex][u]; - samplesRawHydrophone5[index] = (int16_t)adc::channel_buff_ptr[0][bufferIndex][u]; - } - bufferIndex = (bufferIndex + 1) % BUFFER_PER_CHANNEL; - } - // Clean ring-buffers - // this is done so that next time we can add new data into the ring-buffers - for (uint8_t i = 0; i < BUFFER_PER_CHANNEL; i++) { - adc::buffer_filled[i] = 0; - } - - // Sampling (STOP) ==================================================================================================== - - // Send data (START) ==================================================================================================== - Serial.println("Waiting for clients requests..."); - communicationTeensy(); - Serial.println("Data transfer complete"); - Serial.println(); - // Send data (STOP) ==================================================================================================== -} - -void communicationTeensy() { - // Necessary ethernet variables ========== - char* messageToReceive; - char tempCharA = '0'; - char tempCharB = '0'; - - // Send signal that we are ready - while (!ethernetModule::UDP_check_if_connected()) { - ethernetModule::UDP_send_ready_signal(clientIP, clientPort); - // Necessary delay so that client doesn't get overwhelmed with data - delay(100); - } - - // Endless loop until SKIP command is sent from client - while (true) { - // wait until a request is sent from client - while (!ethernetModule::UDP_check_if_connected()) - ; - - messageToReceive = ethernetModule::UDP_read_message(); - tempCharA = messageToReceive[0]; - tempCharB = messageToReceive[1]; - - // Check witch request is being sent - // ss - Send SKIP request - if ((tempCharA == 's') && (tempCharB == 's')) { - break; - } - // sf - Send Frequency data - if ((tempCharA == 's') && (tempCharB == 'f')) { - int32_t* frequencyDataFromClient; - frequencyDataFromClient = teensyUDP::frequency_data_from_client(); - frequencyOfInterest = frequencyDataFromClient[0]; - frequencyVariance = frequencyDataFromClient[1]; - } - // gh - Get Hydrophone data - if ((tempCharA == 'g') && (tempCharB == 'h')) { - teensyUDP::send_hydrophone_data(samplesRawHydrophone1, (SAMPLE_LENGTH * 3)); - teensyUDP::send_hydrophone_data(samplesRawHydrophone2, (SAMPLE_LENGTH * 3)); - teensyUDP::send_hydrophone_data(samplesRawHydrophone3, (SAMPLE_LENGTH * 3)); - teensyUDP::send_hydrophone_data(samplesRawHydrophone4, (SAMPLE_LENGTH * 3)); - teensyUDP::send_hydrophone_data(samplesRawHydrophone5, (SAMPLE_LENGTH * 3)); - } - // gd - Get Digital Signal Processing (DSP) data - if ((tempCharA == 'g') && (tempCharB == 'd')) { - teensyUDP::send_samples_raw_data(samplesRawForDSP, SAMPLE_LENGTH); - teensyUDP::send_samples_filtered_data(samplesFiltered, SAMPLE_LENGTH); - teensyUDP::send_FFT_data(FFTResults, SAMPLE_LENGTH); - teensyUDP::send_peak_data(peaks, lengthOfPeakArray); - } - ethernetModule::UDP_clean_message_memory(); - } - ethernetModule::UDP_clean_message_memory(); -} diff --git a/cpp/teensy/test/test_correlation/test_correlation.cpp b/cpp/teensy/test/test_correlation/test_correlation.cpp deleted file mode 100644 index 397b580c..00000000 --- a/cpp/teensy/test/test_correlation/test_correlation.cpp +++ /dev/null @@ -1,63 +0,0 @@ -#include "Arduino.h" -#include "correlation.h" -#include "stdio.h" -#include "unity.h" - -void test_arm_lib() { - float_t expectedVal = -1.0; - float_t cosOutput = arm_cos_f32(3.14); - TEST_ASSERT_EQUAL_FLOAT(expectedVal, cosOutput); -} - -void test_correlation_with_simple_arrays() { - float32_t result_from_correlation_in_python[] = {17.1, 49.74, 84.58, 113.87, 167.56, 192.7, 221.34, 251.39, 313.45, 251.39, 221.34, 192.7, 167.56, 113.87, 84.58, 49.74, 17.1, 0., 0., 0., 0.}; - - float32_t arr1[] = {3.0, 5.2, 5.2, 6.7, 8.9, 2.0, 6.7, 6.7, 5.7, 0.0, 0.0}; - float32_t arr2[] = {0.0, 0.0, 3.0, 5.2, 5.2, 6.7, 8.9, 2.0, 6.7, 6.7, 5.7}; - - int inputSize = 11; - int outputSize = 2 * inputSize - 1; - - float32_t result_from_correlation_on_teensy[outputSize]; - arm_correlate_f32(arr1, inputSize, arr2, inputSize, result_from_correlation_on_teensy); - - TEST_ASSERT_EQUAL_FLOAT_ARRAY(result_from_correlation_in_python, result_from_correlation_on_teensy, outputSize); -} - -void test_find_lag() { - float32_t arr1[] = {3.0, 5.2, 5.2, 6.7, 8.9, 2.0, 6.7, 6.7, 5.7, 0.0, 0.0}; - float32_t arr2[] = {0.0, 0.0, 3.0, 5.2, 5.2, 6.7, 8.9, 2.0, 6.7, 6.7, 5.7}; - uint32_t signalLength = sizeof(arr1) / sizeof(arr1[0]); - int offset = 2; - - int computedLag = findLag(arr1, arr2, signalLength); - TEST_ASSERT_EQUAL_INT32((signalLength - 1 - offset), computedLag); -} - -void test_compute_tdoa_array() { - float32_t arr1[] = {3.0, 5.2, 5.2, 6.7, 8.9, 2.0, 6.7, 6.7, 5.7, 0.0, 0.0, 0.0, 0.0}; - float32_t arr2[] = {0.0, 3.0, 5.2, 5.2, 6.7, 8.9, 2.0, 6.7, 6.7, 5.7, 0.0, 0.0, 0.0}; - float32_t arr3[] = {0.0, 0.0, 3.0, 5.2, 5.2, 6.7, 8.9, 2.0, 6.7, 6.7, 5.7, 0.0, 0.0}; - float32_t arr4[] = {0.0, 0.0, 0.0, 3.0, 5.2, 5.2, 6.7, 8.9, 2.0, 6.7, 6.7, 5.7, 0.0}; - - uint32_t numberOfSignals = 4; - uint32_t signalLength = sizeof(arr1) / sizeof(arr1[0]); - - float32_t* signals[] = {arr1, arr2, arr3, arr4}; - - int32_t tdoaArray[numberOfSignals - 1]; - computeTdoaArray(signals, numberOfSignals, signalLength, tdoaArray); - - int32_t expectedTdoaVaules[] = {(signalLength - 1 - 1), (signalLength - 1 - 2), (signalLength - 1 - 3)}; - - TEST_ASSERT_EQUAL_INT32_ARRAY(expectedTdoaVaules, tdoaArray, 3); -} - -int main(int argc, char** argv) { - UNITY_BEGIN(); - RUN_TEST(test_arm_lib); - RUN_TEST(test_correlation_with_simple_arrays); - RUN_TEST(test_find_lag); - RUN_TEST(test_compute_tdoa_array); - UNITY_END(); -} diff --git a/cpp/teensy/test/test_multilateration/test_multilateration.cpp b/cpp/teensy/test/test_multilateration/test_multilateration.cpp deleted file mode 100644 index fb815e6d..00000000 --- a/cpp/teensy/test/test_multilateration/test_multilateration.cpp +++ /dev/null @@ -1,72 +0,0 @@ -#include "Arduino.h" -#include "multilateration.h" -#include "unity.h" - -void test_calculate_pinger_position() { - float32_t toleranse = 0.01; - int32_t tdoaAray[NUM_HYDROPHONES - 1] = {610, -67, 549, 290}; - - arm_matrix_instance_f32 A = {NUM_HYDROPHONES - 1, NUM_DIMENSIONS + 1, new float32_t[NUM_HYDROPHONES * (NUM_DIMENSIONS + 1)]}; - arm_matrix_instance_f32 B = {NUM_HYDROPHONES - 1, 1, new float32_t[NUM_HYDROPHONES]}; - - Position* sourcePosition = new Position; - - initialComputationA(A.pData, hydrophonePositions); - arm_status status = calculatePingerPosition(tdoaAray, hydrophonePositions, &A, &B, sourcePosition); - TEST_ASSERT_TRUE(status == ARM_MATH_SUCCESS); - - Position expectedSourcePosition = {3.909, 2.123, -0.607}; - - TEST_ASSERT_FLOAT_WITHIN(toleranse, expectedSourcePosition.X, (*sourcePosition).X); - TEST_ASSERT_FLOAT_WITHIN(toleranse, expectedSourcePosition.Y, (*sourcePosition).Y); - TEST_ASSERT_FLOAT_WITHIN(toleranse, expectedSourcePosition.Z, (*sourcePosition).Z); - - delete[] A.pData; - delete[] B.pData; -} - -void test_with_small_values_for_tdoa() { - float32_t toleranse = 0.01; - int32_t tdoaAray[NUM_HYDROPHONES - 1] = {1, 0, 2, 0}; - - arm_matrix_instance_f32 A = {NUM_HYDROPHONES - 1, NUM_DIMENSIONS + 1, new float32_t[NUM_HYDROPHONES * (NUM_DIMENSIONS + 1)]}; - arm_matrix_instance_f32 B = {NUM_HYDROPHONES - 1, 1, new float32_t[NUM_HYDROPHONES]}; - - Position* sourcePosition = new Position; - - initialComputationA(A.pData, hydrophonePositions); - arm_status status = calculatePingerPosition(tdoaAray, hydrophonePositions, &A, &B, sourcePosition); - TEST_ASSERT_TRUE(status == ARM_MATH_SUCCESS); - - Position expectedSourcePosition = {0.587, 0.113, 0.326}; - - TEST_ASSERT_FLOAT_WITHIN(toleranse, expectedSourcePosition.X, (*sourcePosition).X); - TEST_ASSERT_FLOAT_WITHIN(toleranse, expectedSourcePosition.Y, (*sourcePosition).Y); - TEST_ASSERT_FLOAT_WITHIN(toleranse, expectedSourcePosition.Z, (*sourcePosition).Z); - - delete[] A.pData; - delete[] B.pData; -} - -void test_tdoa_with_zero_values() { - int32_t tdoaAray[NUM_HYDROPHONES - 1] = {0, 0, 0, 0}; - - arm_matrix_instance_f32 A = {NUM_HYDROPHONES - 1, NUM_DIMENSIONS + 1, new float32_t[NUM_HYDROPHONES * (NUM_DIMENSIONS + 1)]}; - arm_matrix_instance_f32 B = {NUM_HYDROPHONES - 1, 1, new float32_t[NUM_HYDROPHONES]}; - Position* sourcePosition = new Position; - - initialComputationA(A.pData, hydrophonePositions); - arm_status status = calculatePingerPosition(tdoaAray, hydrophonePositions, &A, &B, sourcePosition); - TEST_ASSERT_FALSE(status == ARM_MATH_SUCCESS); - - delete[] A.pData; - delete[] B.pData; -} - -int main(int argc, char** argv) { - UNITY_BEGIN(); - RUN_TEST(test_with_small_values_for_tdoa); - RUN_TEST(test_tdoa_with_zero_values); - RUN_TEST(test_calculate_pinger_position); - UNITY_END(); -} \ No newline at end of file diff --git a/cpp/teensy/test/test_pulse_detection/test_pulse_detection.cpp b/cpp/teensy/test/test_pulse_detection/test_pulse_detection.cpp deleted file mode 100644 index 19bbbe84..00000000 --- a/cpp/teensy/test/test_pulse_detection/test_pulse_detection.cpp +++ /dev/null @@ -1,7 +0,0 @@ -#include "test_pulse_determination.h" - -int main(int argc, char** argv) { - UNITY_BEGIN(); - RUN_TEST(pulse_determination::test_short_time_fourier_transform); - UNITY_END(); -} diff --git a/cpp/teensy/test/test_pulse_detection/test_pulse_determination.h b/cpp/teensy/test/test_pulse_detection/test_pulse_determination.h deleted file mode 100644 index 56a691fd..00000000 --- a/cpp/teensy/test/test_pulse_detection/test_pulse_determination.h +++ /dev/null @@ -1,45 +0,0 @@ -#include "Arduino.h" -#include "arm_math.h" -#include "pulse_determination.h" -#include "unity.h" - -/* -Cosine wave generated from Python/pulse_detction/test/pulse_determination.py -generate_cosine_wave(). - -Parameters: - pulse length = 4 ms - carrier frequency = 25 kHz - sampling frequency = 427 kHz - noise amplitude = 0.9 - noise variance = 5 -*/ -namespace pulse_determination { -float32_t COSINE_WAVE[1708] = { - 0.7020062, 2.7188883, 0.6845851, -1.0477405, -1.824456, 1.4874636, 1.1812806, -2.7206507, -3.6606524, -2.313306, -2.2799873, -1.3671145, -3.0273764, -1.1534117, 1.7931819, 0.5899217, -1.7592102, -1.8548586, 0.16250853, 0.071365446, 1.0143479, 0.8353053, -2.0730164, 3.5786717, -1.0255975, 0.046720706, -1.6740634, 1.871118, 0.3350679, 0.19609344, -4.3552027, -1.6436931, 0.9496789, 0.4154565, 0.14360254, 4.199994, -3.7818096, 2.1325147, -1.4738101, 3.3857555, 5.326554, 0.7990733, -2.0476756, 0.8920766, -0.48430935, -1.0804363, -0.45522115, 0.036292057, 0.9049582, 3.8125255, 1.8583329, 0.8659207, 1.1599191, -3.1653032, 4.14679, 0.16656688, 4.419554, -2.845414, -2.6055868, -0.5725064, 1.6946731, 1.2894609, 1.637547, -0.98854274, -2.10053, -4.462052, -0.4362734, -0.6963257, 2.9475722, 1.0683382, 1.2016292, 3.9133482, 2.652229, 1.1067374, 0.1153815, -0.23180829, 0.52756226, 2.3967385, -0.81950825, -5.501674, -2.7156932, -1.6912264, 0.9901776, 4.379866, 1.2613177, 0.74362046, 1.6668321, -0.05779138, 2.44394, 1.0393727, 0.110951, -2.8342156, -2.1727588, -4.438296, 0.16470468, 1.8338182, -1.777917, 0.42291078, 2.337089, 0.61060095, 0.39362526, -1.2128409, 2.5314305, -2.607846, 3.2194839, 3.4924986, 0.46308252, -3.2688859, -0.43511224, -0.021326052, -2.3832865, -1.4141058, -1.2269208, -1.9337702, -2.08668, 3.9247725, 4.18978, 2.8660867, 1.816127, 1.3056875, 1.7421803, 2.0042434, -0.37765574, 0.14437105, 2.8157094, 0.52804774, -0.04799737, -2.4675689, -0.26488584, -5.086627, -4.0861163, 1.170951, 1.0939962, -1.7373595, 4.3454976, 1.0948411, 2.248978, -1.5172744, 6.776865, 0.970612, -0.088825256, -1.6862099, 2.1444466, 0.08684691, -1.4890673, 2.385027, 0.9707843, -0.6972016, -0.9706254, 2.159821, 1.6962031, 3.174125, 1.3976344, 4.394617, 0.29195198, -0.618631, 2.5598829, -0.24226041, 0.56022257, -1.5006375, -0.49089178, -2.3715036, -0.8627552, 0.951296, -1.213204, -0.7790385, 2.1592927, 2.7658322, 0.46882108, -1.1343205, 2.572473, -1.0676221, 0.49935213, -0.8600234, -1.1423861, -2.7918606, -3.0486887, -1.7638942, -2.225516, -2.576731, 3.896803, -1.1361263, -1.5062906, -0.5921356, 1.5543675, 2.9440064, 3.137069, 3.9805975, 2.9798489, 3.1324487, 0.722931, 0.9821584, -0.892533, -2.7526991, 2.3424346, -1.7306925, -2.581912, -6.441542, -2.604661, -4.2671323, 2.9914083, -2.7316742, -0.45796677, 2.289107, 1.4852741, 1.4499786, 2.0120108, 8.709465, 2.3416622, -0.9056993, 0.12697294, -1.8090197, 2.8928196, -0.7958718, -1.8896369, -0.21449994, -0.9949365, -0.87993973, -0.0108866785, 0.8906159, 1.0357524, -1.9811796, 2.7919698, 1.1376461, -0.8868288, 0.16566452, 0.965815, -1.657332, 2.6395664, 2.278737, -1.3056935, -3.1339173, 3.1811717, -4.274048, 3.496867, 3.8851867, 3.3082526, 1.4012581, 2.903616, -1.7288527, 0.61988, -2.5528927, 1.7666965, 4.00179, 0.050323337, -4.9645386, 1.6205713, -0.51694334, 0.0033036121, -1.8464607, 0.52597374, -1.6360422, -1.9425769, 1.9950588, 0.7585985, 0.64394635, 3.0923822, 1.4945936, 2.488353, 0.7627429, -0.62244034, 0.8685706, -3.0210857, -1.9230433, -0.71009445, -0.8296203, -2.4839745, -0.818143, -0.6065734, 1.9530846, 1.3444037, -0.6052623, 0.6407545, 1.4387094, 2.620937, 0.45964518, 8.15694, 4.9575906, 0.4164328, 1.9982138, -1.5901388, -0.12916894, -0.77952415, 1.9123615, -4.2236915, -1.8826605, 0.18237248, -0.23741099, -3.1410444, -2.3577375, 4.144238, 3.04981, 0.6095224, -1.3573315, -2.971716, -1.5104395, -1.9077032, 3.41159, -0.9063322, 0.82354224, -1.426318, -2.7741735, -1.8967389, 2.800871, 1.7833564, 2.4522965, -0.11326979, 0.929248, -0.59625286, 1.4999774, 1.2308366, 0.753845, -1.5445479, -1.1839815, 1.8487903, -2.522649, -2.2385254, -1.9031315, -0.37156707, 1.892049, 0.77503276, -2.36668, 0.0729696, -1.447277, 2.8321552, 2.6329846, 4.0864277, 3.1840997, 3.4731042, 1.0484138, -3.0400758, 0.045192733, 0.6653598, 1.2568315, 1.7753707, -2.661384, -0.82648796, -1.1856732, 0.23308131, -0.2572318, 1.1268443, -0.41191706, 1.5381075, 2.5184608, 4.5318174, -1.0135175, 3.3080711, -2.738378, -2.3107696, -3.0562623, -0.58905816, 1.112811, -0.5480636, -0.83382857, 1.8804011, 0.4395622, -1.5520866, 1.703321, -1.1752226, 0.69226587, 3.4700346, 2.1984916, -2.3322887, -0.9994362, -0.6888838, -0.99365956, 1.4222194, 0.2183156, 1.094543, 2.0874152, -4.1793737, 0.86769354, -1.1695414, -0.6799388, 4.083768, 2.1004224, 1.8483393, 0.63705593, 1.6706684, 4.442249, 2.5186331, -1.7182697, -0.9751241, 0.10220256, -1.9595314, -2.5531793, -0.7220367, -1.2871339, -0.085733615, -1.4958521, 1.2351421, -0.851967, 1.5173765, 0.3300492, 2.916071, 2.8954256, 1.8049126, 0.25834644, -1.0328332, -0.17928867, 0.28350294, 0.83484566, -0.2115288, 1.5530024, -0.7599113, -3.4880357, -2.1317403, 5.0145254, 1.7405785, -0.8291236, -1.6154784, -1.8319762, -2.596872, -0.83043545, 0.09406147, -3.322505, 0.39144826, 0.5949982, -2.6582131, -0.9583311, 1.2438248, -4.130349, 1.2790946, 5.5536566, -1.2573644, -1.6272389, -1.3231164, 0.14918494, 0.26968068, -0.86490023, 1.7204907, 0.06902127, 0.29611784, 0.1305036, 2.2262442, 1.3788137, 1.2696162, -1.5145105, -5.171485, -0.35161754, -0.57268095, -0.71526873, 1.3616631, 0.7564384, 2.5771935, 0.97588974, 1.6918148, 0.07936801, 2.2569778, -2.7171218, -0.2947723, -5.6954646, -3.6818595, -2.7584834, 1.1861484, 1.8307258, 0.05534357, 2.7475934, -1.635352, -5.2304764, 1.430644, 0.81251955, 0.010519862, 2.8257284, 3.311384, -3.1290874, -0.9927925, -0.6590874, -2.854053, -2.1623328, -1.5147959, 1.4498948, 1.1986023, -3.162912, -2.971863, -3.5228553, 3.425806, 1.332312, 4.3040514, -1.0848311, -3.3312454, 0.48575982, 1.2804663, 0.74915504, -2.5488458, -2.0637267, -0.505148, -1.6376826, -4.4053226, -2.479043, -0.15107027, 2.3052857, 0.80753154, 4.381756, -2.9730914, -1.8314012, 1.910203, 3.0399911, 3.039641, 1.0235004, -2.1093652, 2.014104, -0.0075972937, 2.6393967, -2.2807257, 1.3598032, -0.44890845, -2.355869, -1.6544574, -1.4092046, -2.4154532, 1.2561089, 4.2672186, 5.062979, 1.9435849, 4.6848774, -2.2609458, -1.6429172, 0.43376625, 1.9558954, -6.4530497, -2.3825397, -3.0685508, -4.233747, 3.0698617, 4.467878, 0.51395595, -0.62570065, 0.32947463, 2.350985, 3.2834864, -2.5423071, 2.2628357, -1.7406335, 1.6204143, -3.9670484, -0.068941034, -1.2134855, 1.167248, 1.8458034, -1.853091, 0.011450924, 1.9340856, -2.1563118, 1.2824314, -1.0940704, -2.5523536, 1.0284842, 1.6212276, 0.5150513, -0.98924834, -0.12404699, 1.4043286, 0.6425344, -2.8529282, -1.2729378, 0.5595152, -3.2834952, 1.7621136, 0.38959184, 3.0899732, 0.7171288, 4.463386, -0.88136613, -1.034717, 1.7963732, 0.765121, -0.44615713, -4.3591976, 4.420159, - -0.7337828, -4.259702, -1.4781462, 2.8085022, 1.9396186, -2.4337418, -2.6123328, -1.8567406, 2.8534362, -2.1855233, -0.886284, -3.0778797, -0.37843347, -0.7079875, -0.22562777, -0.75180787, 2.8565316, 2.8411114, 1.868099, -1.0955378, -0.18591961, -2.7389438, -3.7420242, 2.3252466, 0.17202927, -2.6400933, 0.5845219, -0.9211511, 1.8789357, -4.728878, 3.2397766, 2.7490144, 0.040187325, 4.8037577, -3.6481488, -3.190204, 1.4279082, -3.014089, -0.80039054, 0.116636746, 0.40591776, 1.8408417, -0.18071125, 3.4774766, 1.1744162, 3.8006816, 2.8505902, 1.9178946, -3.8486357, -4.9926496, 1.0402979, 0.1466654, 0.5754982, 0.99540955, -3.91759, -3.3332071, -3.2823398, 0.63786244, 2.9501755, 3.473854, 3.0815966, 1.1199027, 2.8444629, 1.9562668, -0.5471511, 3.1038883, 3.1003628, 2.234075, -1.8591994, -0.9336664, -1.7641686, 0.63425773, 1.4719073, 0.16246316, 1.5976541, -0.3452068, 2.658577, 0.36777496, 2.4328876, 0.46385777, 1.0486106, 1.9329426, 1.3865846, -1.7327102, 0.12532005, -2.6704206, 1.1160737, 1.3736081, 3.0189605, -1.8270054, -2.596981, 1.3404709, 0.26175943, -0.5301124, 0.52536935, 4.582109, 0.4422025, -1.780633, -0.68189913, 0.24349329, -0.3913693, 0.3189565, 2.6873744, 0.80397236, -2.6660876, -3.3302495, -0.7133615, -0.20074548, -2.2207825, -0.77838624, -2.9513702, 1.9476713, 2.1613238, 1.4910496, 3.2536595, 4.731371, -0.6071082, 2.4517133, 1.0453182, 0.38805532, -4.1658998, -0.50546175, 0.23225681, 0.9233147, -0.7494498, 0.97283435, 0.24756658, -5.073198, 1.0628331, 0.6060905, 4.578924, 0.16766073, -1.4353789, -1.3956366, -0.25262445, 3.084133, -0.6678705, 0.4008066, -1.2212021, -3.0012443, -1.0568981, 0.34177113, -0.2822301, -2.0758154, 4.217281, -0.0041234973, 1.4531337, 0.41876167, 0.45304242, -0.88348156, -0.28985453, -0.49379504, 0.4737224, -2.799966, -0.90841126, -0.4201612, -0.1972264, -0.22007391, -2.5248067, -0.9378249, -2.3173928, -3.5362647, 1.0180794, 0.4136958, 2.8227277, 2.8654473, -0.9286911, -4.9293866, -4.309729, 1.7008939, -0.48069525, -2.9179616, 0.8361479, -3.7995737, 1.1627828, -3.4426112, -0.35112435, 1.6593482, -2.8475866, -0.109460354, 1.0334908, -2.054132, -1.7709237, -0.24630447, 2.8340864, -0.9269379, 1.1904736, -2.5973728, -3.3738005, 0.9615252, 0.32995558, 0.1720286, 1.9834423, -0.7690603, -2.1862557, -0.047066506, 2.3769393, 4.3812723, 0.9010458, 1.0330125, -0.37419516, 1.991957, -2.4442465, -2.2167215, 1.9828796, 2.3614008, 1.0905255, -3.0044508, -1.867122, -1.7078543, 2.8060253, -1.5586823, 2.4591079, -0.28424007, 0.7053135, -1.1541212, 1.8907591, 4.795971, 1.7836751, -4.0263085, 2.1038191, 2.0690594, -4.473109, -3.1515594, 0.5602016, 0.9621787, 1.3964005, 1.5933361, -1.1526941, 0.98389494, 1.7131141, -1.8249582, -0.73761034, 3.9761713, 3.5270195, 1.5311408, -0.30451256, -2.7817736, 2.3475087, -0.28642094, -0.17028394, -0.3864465, -0.35022086, -2.9665666, -4.5863023, 2.5973032, 2.3913238, 1.8093876, -2.5116687, -3.185453, -0.31947622, 1.1385388, -1.1444883, 2.2222183, -0.5262894, 2.717357, -1.9583012, -0.7188468, -0.42252046, 1.1424819, 0.2820003, 2.9916573, -3.0268788, 0.040588588, 2.6399963, -3.343655, -0.7612901, 2.8397267, 2.904309, 1.2945305, -1.9884151, -2.7240093, -0.4308215, 0.746556, -2.2587428, -1.0729467, -3.599597, -2.6619024, -0.29159614, -4.35208, 3.7444105, 3.6560774, 0.3002431, 0.7450489, -0.881891, -0.6702849, -2.826042, 2.3132594, 7.605556, -5.1181507, -2.3504808, -4.871105, 2.1089792, 0.16394879, 2.1335247, -0.71261626, -1.4453762, 0.8676329, -2.2728808, -3.370515, 0.6676939, -0.080424614, 2.9066782, 3.6743271, -2.6230297, -0.14423923, -3.2511203, -2.2156963, 4.573114, 1.050148, -3.766222, -1.452215, 2.8773568, -3.3920481, -2.1490848, -3.0505126, 0.08244243, 2.3694108, 5.06644, 1.250852, 0.17556292, -2.1674526, 0.8641988, 2.4726648, 0.53703386, -0.35661575, -2.3343017, -3.351604, -3.7782767, 0.8965238, 0.44752896, 1.6438625, -0.38422042, -2.043197, 2.5164666, -2.2515852, 2.9713514, 1.1513911, -0.8497893, 2.1476557, 0.53616625, 1.0464759, 2.6098404, -0.18650055, -0.05064997, -2.2753396, -0.65276134, -4.777318, -0.2083684, 1.1540678, 2.0666785, 1.317964, 1.168703, 2.387476, -3.2379344, 2.1077318, -0.48438945, 4.050464, -2.1379623, -0.08691217, -0.5825202, -4.90972, 2.8193412, -3.7109575, 1.0161414, 1.3173314, 0.058841247, 0.42051548, 0.5628216, -0.3546686, 0.88449776, 1.9602319, -2.9538367, -1.2569048, -3.6598563, -3.8180463, -0.7705863, -0.80714244, -1.1951272, -2.178653, 0.77806264, -1.9402008, -1.4724292, 1.8741769, 3.4442995, 2.5969043, 3.0005472, 3.9925652, -1.5296353, 1.097424, -0.9766697, -1.4568144, 0.5019237, 1.7344642, -1.4048889, -2.839477, -2.1147113, -3.7974155, 2.6379237, 4.185493, 1.9960022, -1.652018, -1.9100589, 0.60999197, 1.541236, 1.2654145, 2.8971372, 0.3899325, 0.16458175, -4.8219566, 0.014839224, -3.687334, -2.5238655, 2.0016923, 0.92783844, -0.4906534, -2.8887136, -0.9922888, 3.227696, 0.6883636, -0.07921877, 3.3716369, -1.7967696, -3.0303478, -0.6625077, -1.2314384, 0.25019428, 3.909294, -4.570424, -3.6313813, 0.8001716, 3.2485952, -3.1632369, 0.49705082, -1.8432935, 1.9150428, 1.7186648, 2.8638875, -0.37918207, -0.8896832, 6.7226415, 2.9685063, 0.44602224, -5.421928, -0.10432589, -1.9081297, -1.7837359, 4.896712, 1.2911199, 2.6003127, 0.06193712, -3.0551322, 4.175638, -0.35094798, 2.7538629, 4.0654383, 1.1652485, 2.5306609, -0.85242915, -0.9147942, 1.8007073, -2.935899, 0.87940127, -2.718677, -0.57297164, -3.6615713, 0.038380377, -1.0887463, 3.2645223, 1.8837885, -3.716117, -1.6959641, -1.6784005, 4.0092983, 0.032077566, 0.97569597, -0.020674156, 2.6898081, -5.222261, -0.77221805, -0.79905367, -3.3382995, -1.743185, -1.0473603, -2.7137408, -0.41983235, 0.80616146, 3.4492817, 3.200496, 3.1594806, -0.7128164, 3.5097506, 0.7051475, -0.63350827, -3.1590095, -2.6080241, 1.867097, 1.3845252, -0.40964428, -0.9625051, 0.76517206, 0.5539091, 3.1230128, 0.27874652, 1.8786356, 1.8083832, -0.8864153, -1.5288215, -2.1375325, -0.65916765, -1.7309265, 0.21005864, -3.9823744, -4.4389725, -1.5268549, -1.7071804, 0.56879723, -0.38270617, -2.503477, -2.8204253, -3.0125637, 3.197173, 2.8777905, 2.2054017, 2.124739, 1.5215944, -5.360349, 1.3153238, -4.7492146, 0.6028294, -1.0645467, 0.23445107, -1.1861556, -3.2003217, -1.5438063, 0.7195621, -1.8143773, 0.5137529, -2.8829691, 1.7177397, -3.0475712, -1.7782617, 5.327337, -0.21868502, 4.272002, 4.014728, 0.42307994, -2.2556474, 0.15004274, 3.643712, 1.3793927, 1.4850008, 3.3509533, 1.547133, -1.6452122, -0.34640867, -3.1432762, 2.9942057, 4.4773655, 3.9925961, -1.4077996, -0.4101753, 1.2475412, -1.721008, -1.2940207, 2.1446702, 0.320953, -4.9834123, -0.8613742, -1.9479203, - -0.31437433, 0.16217417, 0.45749506, 0.13680467, 1.1323957, 1.6118842, 1.123796, 3.226014, 2.1168118, 0.24478395, -3.2244103, 2.052193, -2.553393, -1.0643561, 1.108011, 1.2230963, -2.4334173, 0.28692785, -2.095508, -3.6824436, 4.1634226, -0.57946235, 1.1333596, 3.395744, 1.3915285, -3.3637044, -0.57221115, 3.3809166, 2.8096986, 0.6577414, -3.1345363, -4.0315003, -1.2473654, -3.3730798, 2.9361172, -0.7823493, 0.7137545, 1.7222419, -1.0652742, -0.37961328, 0.007448507, 1.1076752, 4.1421537, 3.2992346, -0.37890613, -3.157527, -2.6757834, -0.14606957, -3.6226888, 2.2033308, 2.2012196, -2.1906335, -0.6361447, 1.3005216, -0.86937463, 3.030738, 3.767834, 5.58011, 3.309805, -0.08567785, -4.2953, -2.3421397, -2.553145, 1.2944207, -1.7421887, 0.36181775, 0.31342855, -1.7853658, -1.2927254, -1.5998536, -1.1270378, 3.6469927, -1.213324, -2.9459245, 0.87636185, 0.31934935, -0.26556286, -0.22071485, 0.39599285, -2.588831, -3.6380124, 0.5702441, -1.2854286, -0.6111006, 3.8997078, -1.3410009, 5.300957, 1.9640481, -2.3732493, -0.48571295, 3.4425828, 4.205382, -0.40700376, 1.0869861, 0.3524896, -0.5332948, 0.31366912, -4.294421, 0.4530993, -1.3415233, -0.6114474, -3.1463091, -1.4027063, -1.0541369, 0.5345127, 3.0283017, 1.8913656, -3.1576707, -4.1282997, -2.9164343, 3.3447962, 3.5665903, 3.3173535, -0.3091354, 2.8110514, -4.204252, -4.0686502, -0.8819621, -0.13885683, -0.13028583, 0.81391454, -0.46619254, 1.9652392, -0.18829599, 3.5656803, 0.9403475, -1.054169, -0.043163806, 0.30793855, 0.1927952, -1.5129375, -2.1168501, -2.8531804, 0.008393191, -1.5794224, -0.7706998, -2.1333408, 1.4935061, -1.2987784, 3.1579704, 4.505435, 2.1425962, -1.9628218, 1.5175188, 0.3949766, 1.7153302, -0.8674341, -0.7734573, -1.7085196, -1.2315488, 0.9682545, -2.1354246, 1.5420277, -1.7975208, 3.0099545, -1.9810028, -1.5213085, 2.562081, 1.5176389, 1.5606663, 0.6455673, 2.9788876, -1.1596516, 2.4144864, -2.4074383, -3.6380944, 1.6851093, -1.3970933, 2.284142, -3.1691103, -1.8092996, 1.6017601, -1.2707378, -4.703915, -0.7806056, 4.6952, 1.8495044, -0.48234522, -1.5643535, 0.8542769, 1.5906763, 0.96053714, -1.070777, -0.4733354, -0.2749412, -2.1837134, -0.1391308, -0.17901114, 3.8529623, 2.3007052, -0.9289801, 1.9968331, 3.3126588, 0.3221299, 3.118174, -1.0282435, 4.1339517, -0.6721579, -0.6884616, -5.6774178, -1.9073404, -2.451029, 1.4196066, -1.7933822, 0.17521237, -0.8207581, 5.815313, 1.1486421, -0.9241873, 4.643419, -0.27267453, 1.1319674, 1.8711559, -4.640392, 0.71193296, -2.0774825, 0.25530052, -1.2436103, -2.9260945, -3.006301, -3.9217894, -3.5023482, 1.1775639, -1.9230711, 4.005662, 0.22290081, 3.6914315, -1.3787766, 1.4770217, 0.66494507, -0.46884307, 0.3470082, 1.0073502, -1.5989257, -2.2739918, -1.3149171, 0.13223083, 0.5079973, -0.053165477, -1.0081189, -1.8080469, 0.53186876, 1.3337891, 1.1499598, 3.636718, 0.04989129, 1.2487166, -0.38993165, -1.727303, -6.437255, 0.09784787, -2.0842664, -0.8295711, 0.7752482, -2.2614038, 1.2848365, -0.38483897, 3.2820802, -0.65844953, 0.95424736, 1.0926678, 2.5618265, -1.23085, 3.3489416, 2.123931, -2.1440604, -0.02312768, -1.276653, -3.2521796, 0.034942396, -3.6782575, 0.3973959, -1.1935037, 0.8435042, -1.602029, -0.6105784, 4.839523, -2.111457, 3.693246, 3.7386293, 0.40241045, -1.9914652, -2.8179936, -2.0995467, -2.628709, -1.5632463, 1.9008406, -0.47844854, -0.789208, -6.4262495, 0.6271127, 1.5056992, 3.1031218, 3.420178, 0.13229042, 1.0440193, -1.7961745, 0.18540536, -1.4063413, 3.8865728, 0.97926736, -0.10138011, -2.9248729, -0.48516658, -0.71680677, 0.6700909, -0.30252248, 0.6350524, -0.8701628, 1.7133168, -1.556702, 1.0609232, 4.2924566, 3.6700184, 0.66946864, -0.06135692, -1.9655144, -2.5567486, -2.4564984, -3.8527794, -1.8641833, -1.9560933, -3.397485, -0.8143287, -0.33789745, 1.6804463, 0.2406491, 1.4644552, 0.6556546, 2.862851, 1.1714284, 0.22966379, -0.22263132, -1.9279008, -0.028387848, 4.0817237, -1.6181694, -2.1643178, -3.722279, 1.9899753, 2.8100913, -3.185049, -3.0398872, -6.4939394, -0.90912616, 0.1269929, 0.39510316, 4.4245, -0.28238487, 1.8992732, 0.5798227, 0.7722769, 1.240241, 2.0316799, -2.0225165, -3.5953553, -0.48975566, -0.030338526, -0.4364222, 0.60799056, 2.1559062, 4.124986, 5.7652345, 5.0990057, -2.3544302, -3.1404927, 0.0016712632, -0.266678, 0.9458797, -0.3945683, -2.0367327, -2.4746861, -0.28266355, -0.053384542, -0.91534936, 0.07164746, 2.5320177, 1.19603, -0.08377507, -3.9544547, 1.7633394, 2.6035225, 3.6624982, 1.743421, 0.51679754, -2.3953426, -0.45217296, 3.3985047, 0.72220165, 2.59441, -0.16610008, -0.6827486, -3.25876, -3.5630789, 0.39642736, 2.6981668, 0.518698, 2.1651387, -0.36628503, -1.457879, 0.5167917, -0.011340978, 1.5176368, -2.827247, -1.0892535, -2.2234282, -4.533881, -0.024716021, 2.1772745, -1.5185609, -2.9411018, -3.3570893, 4.584189, -2.588014, 3.335608, -0.98289776, 3.2388425, 0.20629893, 4.1741366, 0.7038931, -2.6888187, 3.0856235, 0.6138711, -1.1443311, -0.65147364, -1.0620317, -0.9133227, -0.26389796, -4.0438814, 1.1374729, 2.2229486, 4.565879, -1.6510109, -0.043481212, -1.6593988, -0.7020764, 3.2981043, 5.207249, -0.23434855, 0.8082117, -1.9983732, -1.6916591, -1.7631156, 2.555326, 0.6397992, 1.9027077, 2.1077013, 1.6841308, 0.7697296, 4.5383363, 0.97808427, 2.7988603, -0.85185224, -2.7903748, -3.4583008, -0.03828187, 0.36465764, 3.3619049, 3.855637, -3.5385485, -0.85856074, -1.3319527, -3.0101845, 1.5072151, 0.44064558, 0.3073134, 1.1060525, -2.4240565, 2.8109317, 0.33885124, 0.23723553, -3.2333522, 1.52136, 3.1475568, 0.16613156, 1.675439, -2.0859385, -1.6454599, -1.6434406, 2.7533236, -1.7938387, -0.4515533, -1.904652, -0.35860163, 1.11317, 0.72095466, 2.6195633, -2.050234, -0.12340601, -0.4281685, -3.2191415, -0.3247173, -0.09693233, 1.880586, -1.5289235, -0.06196795, 1.0268698, -0.21298233, 1.8693445, 0.3325241, -1.7322328, 1.6425831, 0.17895474, 1.1558307, -1.1262492, -1.2570959, -1.1504493, -0.73787415, -0.20666689, -1.6742809, -1.4698519, -4.589824, 1.3463221, 1.4091227, 2.8691583, -0.11526528, 0.0026236936, 0.11950199, 0.86112696, -0.2901353, -1.0452216, 0.26968446, 0.7850103, -1.1616488, 2.957213, -3.206064, -3.3210902, 0.41880882, 0.5322817, -0.79842156, -0.69474614, -1.8162234, 1.0544405, -0.9728486, 1.8729428, 3.4604137, 0.7786607, 2.240304, 1.0091852, -1.8374918, 2.0552297, -2.6283324, -0.4366169, -0.47909513, -4.0852056, -0.42276758, 2.3005743, 3.5022566, 2.5025895, 2.5067, -4.376304, 2.204801, -0.47309914, 3.9122968, 0.11853697, 2.7859337, -0.77221185, -2.5376868, -1.0785336, -1.8679672, -3.4886076, -1.266914, 2.3683162, -0.8835571, 1.218209, 0.1372898, -1.179711, 2.320037, -}; -float32_t EXPECTED_CARRIER_FREQ = 25.019531; - -void test_short_time_fourier_transform() { - uint32_t pulseLength = 4; // [ms] - - float32_t* pulse = new float32_t[pulseLength * SAMPLING_FREQUENCY]; - for (uint32_t i = 0; i < pulseLength * SAMPLING_FREQUENCY; i++) { - pulse[i] = COSINE_WAVE[i]; - } - - float32_t* fft = new float32_t[FFT_SIZE]; - arm_status status = shortTimeFourierTransform(pulse, pulseLength, fft); - TEST_ASSERT_TRUE(status == ARM_MATH_SUCCESS); - - float32_t computedCarrierFrequency = computeCarrierFrequency(fft); - - float32_t tolerance = static_cast(SAMPLING_FREQUENCY) / FFT_SIZE; - TEST_ASSERT_FLOAT_WITHIN(tolerance, EXPECTED_CARRIER_FREQ, computedCarrierFrequency); - - delete[] pulse; - delete[] fft; -} -} // namespace pulse_determination diff --git a/cpp/teensy/test/test_unity_testframework/test_unity_testframework.cpp b/cpp/teensy/test/test_unity_testframework/test_unity_testframework.cpp deleted file mode 100644 index 9b05e767..00000000 --- a/cpp/teensy/test/test_unity_testframework/test_unity_testframework.cpp +++ /dev/null @@ -1,10 +0,0 @@ -#include "unity.h" -#include "unity_testframework.h" - -int main(int argc, char** argv) { - - UNITY_BEGIN(); - RUN_TEST(test_summation); - RUN_TEST(test_return_true); - UNITY_END(); -} diff --git a/old_makefile b/old_makefile deleted file mode 100644 index bdc04819..00000000 --- a/old_makefile +++ /dev/null @@ -1,133 +0,0 @@ - -#SOURCES := DSP.c fourier.c hydrophones.c triliteration.c - -#SOURCE_DIR := src -#BUILD_DIR := build -#DOX_DIR := dox - -#OBJ := $(patsubst %.c,$(BUILD_DIR)/%.o,$(SOURCES)) - -#OUT := vortex_acoustics - -#DRIVER_ARCHIVE := $(BUILD_DIR)/libdriver.a -#DRIVER_SOURCE := hardware.c io.c - -#CC := gcc -# CFLAGS := -O0 -g3 -Wall -Werror -std=c11 -I$(SOURCE_DIR) -#CFLAGS := -O0 -g3 -Wall -Wno-unused-variable -Wno-switch -std=c11 -I$(SOURCE_DIR) -#LDFLAGS := -L$(BUILD_DIR) -ldriver -lcomedi - -#.DEFAULT_GOAL := $(OUT) - -#vortex_acoustics : $(OBJ) #| $(DRIVER_ARCHIVE) -# $(CC) $(CFLAGS) $^ -o $@ $(LDFLAGS) - -#$(BUILD_DIR) : -# mkdir -p $@/src - -#$(BUILD_DIR)/%.o : $(SOURCE_DIR)/%.c | $(BUILD_DIR) -# $(CC) $(CFLAGS) -c $< -o $@ - -#$(BUILD_DIR)/src/%.o : $(SOURCE_DIR)/src/%.c | $(BUILD_DIR) -# $(CC) $(CFLAGS) -c $< -o $@ - -#$(DRIVER_ARCHIVE) : $(DRIVER_SOURCE:%.c=$(BUILD_DIR)/driver/%.o) -# ar rcs $@ $^ - -#.PHONY: clean clean_dox -#clean : -# rm -rf $(BUILD_DIR) $(OUT) - -#clean_dox: -# rm -rf $(DOX_DIR) - - - - - -# The makefile is basicly copy-pasted from -# https://www.partow.net/programming/makefile/index.html - -# CXX := -c++ -CXX := g++ -CC := gcc - -# CXXFLAGS := -pedantic-errors -Wall -Wextra -Werror -CPPFLAGS := -pedantic-errors -Wall -Wextra -Werror - -# LDFLAGS := -L/usr/lib -lstdc++ -lm -LDFLAGS := -lm -BUILD := ./build -OBJ_DIR := $(BUILD)/objects -APP_DIR := $(BUILD)/apps -TARGET := program - -CINCLUDE := - -IResource/STM32F7xx_HAL_Driver/Src - -IResource/CMSIS/DSP_Lib/Source/BasicMathFunctions - -IResource/CMSIS/DSP_Lib/Source/CommonTables - -IResource/CMSIS/DSP_Lib/Source/ComplexMathFunctions - -IResource/CMSIS/DSP_Lib/Source/ControllerFunctions - -IResource/CMSIS/DSP_Lib/Source/FastMathFunctions - -IResource/CMSIS/DSP_Lib/Source/FilteringFunctions - -IResource/CMSIS/DSP_Lib/Source/MatrixFunctions - -IResource/CMSIS/DSP_Lib/Source/StatisticsFunctions - -IResource/CMSIS/DSP_Lib/Source/SupportFunctions - -IResource/CMSIS/DSP_Lib/Source/TransformFunctions - -CXXINCLUDE := - -ISource - -CSRC := \ - $(wildcard Resource/STM32F7xx_HAL_Driver/Src/*.c) \ - $(wildcard Resource/CMSIS/DSP_Lib/Source/BasicMathFunctions/*.c) \ - $(wildcard Resource/CMSIS/DSP_Lib/Source/CommonTables/*.c) \ - $(wildcard Resource/CMSIS/DSP_Lib/Source/ComplexMathFunctions/*.c) \ - $(wildcard Resource/CMSIS/DSP_Lib/Source/ControllerFunctions/*.c) \ - $(wildcard Resource/CMSIS/DSP_Lib/Source/FastMathFunctions/*.c) \ - $(wildcard Resource/CMSIS/DSP_Lib/Source/FilteringFunctions/*.c) \ - $(wildcard Resource/CMSIS/DSP_Lib/Source/MatrixFunctions/*.c) \ - $(wildcard Resource/CMSIS/DSP_Lib/Source/StatisticsFunctions/*.c) \ - $(wildcard Resource/CMSIS/DSP_Lib/Source/SupportFunctions/*.c) \ - $(wildcard Resource/CMSIS/DSP_Lib/Source/TransformFunctions/*.c) \ - -CXXSRC := - $(wildcard Source/*.cpp) \ - -COBJ := $(CSRC:%.c=$(OBJ_DIR)/%.o) -CXXOBJ := $(CXXSRC:%.cpp=$(OBJ_DIR)/%.o) - - -all: build $(APP_DIR)/$(TARGET) - -$(OBJ_DIR)/%.o: %.c - @mkdir -p $(@D) - $(CC) $(CPPFLAGS) $(CINCLUDE) -c $< -o $@ $(LDFLAGS) - -$(APP_DIR)/$(TARGET): $(OBJECTS) - @mkdir -p $(@D) - $(CC) $(CPPFLAGS) -o $(APP_DIR)/$(TARGET) $^ $(LDFLAGS) - -$(OBJ_DIR)/%.o: %.cpp - @mkdir -p $(@D) - $(CXX) $(CXXFLAGS) $(CXXINCLUDE) -c $< -o $@ $(LDFLAGS) - -$(APP_DIR)/$(TARGET): $(OBJECTS) - @mkdir -p $(@D) - $(CXX) $(CXXFLAGS) -o $(APP_DIR)/$(TARGET) $^ $(LDFLAGS) - -.PHONY: all build clean debug release - -build: - @mkdir -p $(APP_DIR) - @mkdir -p $(OBJ_DIR) - -debug: CPPFLAGS += -DDEBUG -g -debug: all - -release: CPPFLAGS += -O2 -release: all - -clean: - -@rm -rvf $(OBJ_DIR)/*.o - -@rm -rvf $(APP_DIR)/*.o \ No newline at end of file diff --git a/Python/utilities/README.md b/utilities/README.md similarity index 100% rename from Python/utilities/README.md rename to utilities/README.md diff --git a/Python/__init__.py b/utilities/__init__.py similarity index 100% rename from Python/__init__.py rename to utilities/__init__.py diff --git a/Python/utilities/__main__.py b/utilities/__main__.py similarity index 100% rename from Python/utilities/__main__.py rename to utilities/__main__.py diff --git a/Python/utilities/drone.py b/utilities/drone.py similarity index 100% rename from Python/utilities/drone.py rename to utilities/drone.py diff --git a/Python/utilities/generate_headers.py b/utilities/generate_headers.py similarity index 100% rename from Python/utilities/generate_headers.py rename to utilities/generate_headers.py diff --git a/Python/utilities/hyperboles.py b/utilities/hyperboles.py similarity index 100% rename from Python/utilities/hyperboles.py rename to utilities/hyperboles.py diff --git a/Python/utilities/plot_cli_helpers.py b/utilities/plot_cli_helpers.py similarity index 100% rename from Python/utilities/plot_cli_helpers.py rename to utilities/plot_cli_helpers.py diff --git a/Python/utilities/plots.py b/utilities/plots.py similarity index 100% rename from Python/utilities/plots.py rename to utilities/plots.py diff --git a/Python/utilities/tdoa.py b/utilities/tdoa.py similarity index 100% rename from Python/utilities/tdoa.py rename to utilities/tdoa.py diff --git a/Python/utilities/templates/template_header.h b/utilities/templates/template_header.h similarity index 100% rename from Python/utilities/templates/template_header.h rename to utilities/templates/template_header.h diff --git a/Python/ethernet_protocol/__init__.py b/utilities/test/__init__.py similarity index 100% rename from Python/ethernet_protocol/__init__.py rename to utilities/test/__init__.py diff --git a/Python/utilities/test/test_generate_header.py b/utilities/test/test_generate_header.py similarity index 100% rename from Python/utilities/test/test_generate_header.py rename to utilities/test/test_generate_header.py diff --git a/Python/utilities/test/test_hyperboles.py b/utilities/test/test_hyperboles.py similarity index 100% rename from Python/utilities/test/test_hyperboles.py rename to utilities/test/test_hyperboles.py diff --git a/Python/utilities/test/test_plots.py b/utilities/test/test_plots.py similarity index 100% rename from Python/utilities/test/test_plots.py rename to utilities/test/test_plots.py