#include <ADC.h>
#include <DMAChannel.h>
#include <SD.h>
#define BETTER_PI 3.141592654
//const int MeasurementPin = A4; // ADC0
//const int MeasurementPin2 = A2; // ADC1
/* We are now using the differential ADC to measure voltages
* The only exposed differential pair on the Teensy is connected to ADC1
* using pins A11 (ADC1_DM0, minus) and A10 (ADC1_DP0, plus)
*/
const int MeasurementPin_P = A10;
const int MeasurementPin_M = A11;
// SD Card pin thingy
const int chipSelect = BUILTIN_SDCARD;
// Number of samples to take before evaluating
const uint32_t NUM_SAMPLES = 200;
volatile uint16_t sample_cnt = 0;
// Frequency to detect in Hz
const uint16_t TGT_FREQ1 = 1000;
const uint16_t TGT_FREQ2 = 2000;
volatile int counter = 0;
/* We don't actually need to keep all the samples but
* it looks like DMA is the quickest way to get samples
* out of the ADC and on top of that the best way to
* know when we have NUM_SAMPLES full is with an interrupt
* when the array is full.
*/
uint16_t samples[NUM_SAMPLES];
const uint32_t SAMPLE_FREQ = 22050;
/* This Goertzel algorithm implementation based off the
* Embedded Programming Magazine article by Kevin Banks
* August 20, 2002 (accessed 1/29/2019)
*/
const int32_t k1 = (int)(0.5 + ((NUM_SAMPLES * TGT_FREQ1) / SAMPLE_FREQ));
const double w1 = ((2 * BETTER_PI) / NUM_SAMPLES) * k1;
const double cosine_w1 = cos(w1);
const double sine_w1 = sin(w1);
const double GOERTZ_COEFF1 = 2 * cosine_w1;
const int32_t k2 = (int)(0.5 + ((NUM_SAMPLES * TGT_FREQ2) / SAMPLE_FREQ));
const double w2 = ((2 * BETTER_PI) / NUM_SAMPLES) * k2;
const double cosine_w2 = cos(w2);
const double sine_w2 = sin(w2);
const double GOERTZ_COEFF2 = 2 * cosine_w2;
// Per-sample processing variables
volatile double Q01 = 0;
volatile double Q11 = 0;
volatile double Q21 = 0;
volatile double Q02 = 0;
volatile double Q12 = 0;
volatile double Q22 = 0;
// Check some values for mind sanity
// sound of mind sound of body... hopefully
volatile double min_gerd1 = 0;
volatile double max_gerd1 = 0;
volatile double min_gerd2 = 0;
volatile double max_gerd2 = 0;
volatile bool has_gerd = false;
volatile double moving_avg1 = 0;
volatile double moving_avg2 = 0;
ADC *adc = new ADC(); // ADC object
DMAChannel dma; // DMA channel
// Track where we are in samples
int buffCount = 0;
void setup() {
pinMode(LED_BUILTIN, OUTPUT);
// According to Stoffgren forum post somewhere either don't
// touch or use INPUT_DISABLE
//pinMode(MeasurementPin_P, INPUT);
//pinMode(MeasurementPin_M, INPUT);
Serial.begin(9600);
while(!Serial);
Serial.println("Begin setup");
Serial.println("Constants: ");
Serial.print("Goertz coeff1: ");
Serial.println(GOERTZ_COEFF1);
Serial.print("Goertz coeff2: ");
Serial.println(GOERTZ_COEFF2);
Serial.print("Samples (N): ");
Serial.println(NUM_SAMPLES);
Serial.print("Tgt Frequency1: ");
Serial.println(TGT_FREQ1);
Serial.print("Tgt Frequency2: ");
Serial.println(TGT_FREQ2);
Serial.print("Measurement pin P: ");
Serial.println(MeasurementPin_P);
Serial.print("Measurement pin M: ");
Serial.println(MeasurementPin_M);
dmaInit();
adcInit();
Serial.println("Initializing SD card...");
// see if the card is present and can be initialized:
if (!SD.begin(chipSelect)) {
Serial.println("Card failed, or not present");
// don't do anything more:
return;
}
Serial.println("card initialized.");
Serial.println("Starting PDB");
adc->adc1->startPDB(SAMPLE_FREQ);
Serial.println("End of setup");
}
char c = 0;
int value;
int value2;
void loop() {
if(Serial.available()) {
c = Serial.read();
if(c == 'd') {
for(int i = 0; i < 16; i++) {
Serial.println(samples[i]);
}
Serial.println(" ");
} else if (c == 'p') {
Serial.print("Prescaler: ");
Serial.println( (PDB0_SC & 0x7000) >> 12, HEX);
Serial.print("Mult: ");
Serial.println( (PDB0_SC & 0xC) >> 2, HEX);
} else if (c == 'a') {
Serial.println("Starting DMA sample again");
// Run sampling again
dma.attachInterrupt(dma_isr);
}
}
/* fail_flag containes all possible errors, they are defined in ADC_Module.h as:
*
* ADC_ERROR_OTHER
* ADC_ERROR_CALIB
* ADC_ERROR_WRONG_PIN
* ADC_ERROR_ANALOG_READ
* ADC_ERROR_COMPARISON
* ADC_ERROR_ANALOG_DIFF_READ
* ADC_ERROR_CONT
* ADC_ERROR_CONT_DIFF
* ADC_ERROR_WRONG_ADC
* ADC_ERROR_SYNCH
*
* You can compare the value of the flag with those masks to know what's the error.
*/
if(adc->adc1->fail_flag) {
Serial.print("ADC0 error flags: 0x");
Serial.println(adc->adc1->fail_flag, HEX);
if(adc->adc1->fail_flag == ADC_ERROR_COMPARISON) {
adc->adc1->fail_flag &= ~ADC_ERROR_COMPARISON; // Clear the error
Serial.println("Comparison error in ADC1");
}
}
}
void dmaInit() {
dma.source(*(uint16_t*) &ADC1_RA);
dma.destinationBuffer(samples, sizeof(samples));
dma.attachInterrupt(dma_isr);
dma.interruptAtCompletion();
dma.triggerAtHardwareEvent(DMAMUX_SOURCE_ADC1);
Serial.print("Samples start ");
Serial.println(millis());
dma.enable();
}
void adcInit() {
// ADC to use:
int8_t adc_num = ADC_1;
// reference can be ADC_REF_3V3, ADC_REF_1V2 (not for Teensy LC) or ADC_REF_EXT
adc->setReference(ADC_REFERENCE::REF_3V3, adc_num);
adc->setAveraging(4, adc_num);
adc->setResolution(12, adc_num);
// it can be ADC_VERY_LOW_SPEED, ADC_LOW_SPEED, ADC_MED_SPEED,
// ADC_HIGH_SPEED_16BITS, ADC_HIGH_SPEED, ADC_VERY_HIGH_SPEED
adc->setConversionSpeed(ADC_CONVERSION_SPEED::HIGH_SPEED_16BITS, adc_num);
// it can be ADC_VERY_LOW_SPEED, ADC_LOW_SPEED, ADC_MED_SPEED, ADC_HIGH_SPEED, or ADC_VERY HIGH SPEED
adc->setSamplingSpeed(ADC_SAMPLING_SPEED::HIGH_SPEED, adc_num); /// change the sampling speed
adc->enableInterrupts(adc_num); // it's necessary to enable interrupts for PDB to work
adc->enableDMA(adc_num); // enable DMA requeste ADC0_SC2 |= | ADC_SC2_ADTRG | ADC_SC2_DMAEN
adc->analogReadDifferential(MeasurementPin_P, MeasurementPin_M, adc_num);
//adc->analogRead(MeasurementPin, ADC_0); // Call this to setup everything before the PDB starts
Serial.print("Is this adc in diff mode? ");
Serial.println(adc->isDifferential(adc_num));
}
void adc1_isr() {
// Frequency 1
Q01 = GOERTZ_COEFF1 * Q11 - Q21 + samples[sample_cnt];
Q21 = Q11;
Q11 = Q01;
// Frequency 2
Q02 = GOERTZ_COEFF2 * Q12 - Q22 + samples[sample_cnt];
Q22 = Q12;
Q12 = Q02;
sample_cnt++;
}
// pdb interrupt is enabled in case it is needed
void pdb_isr(void) {
PDB0_SC &= ~PDB_SC_PDBIF; // clear interrupt
//Serial.print("pdb isr: ");
//Serial.println(millis());
//digitalWriteFast(LED_BUILTIN, !digitalReadFast(LED_BUILTIN) );
}
void dma_isr() {
adc->adc1->stopPDB();
dma.detachInterrupt();
dma.disable();
uint16_t sample_cpy[NUM_SAMPLES];
memcpy(sample_cpy, samples, sizeof(samples));
memset(samples, 0, sizeof(samples));
String dataString = "";
String statString = "";
for(int i = 0; i < NUM_SAMPLES; i++) {
dataString += sample_cpy[i];
dataString += ",";
}
dataString += "fin";
dma.clearInterrupt();
// Serial.print("Samples finished ");
// Serial.println(millis());
// Goertzel calcs freq 1
double magnitude_sq1 = (Q11 * Q11) + (Q21 * Q21) - Q11 * Q21 * GOERTZ_COEFF1;
Serial.print("Gerds^2 1: ");
Serial.println(magnitude_sq1);
double magnitude1 = sqrt(magnitude_sq1);
// Goertzel calcs freq 2
double magnitude_sq2 = (Q12 * Q12) + (Q22 * Q22) - Q12 * Q22 * GOERTZ_COEFF2;
Serial.print("Gerds^2 2: ");
Serial.println(magnitude_sq2);
double magnitude2 = sqrt(magnitude_sq2);
Serial.print("Gerds Gertzels1,");
Serial.print("Gerds Gertzels2,");
Serial.print(magnitude1);
Serial.print(",");
Serial.println(magnitude2);
statString += "Goertzel LF: ";
statString += magnitude1;
statString += "\nGoertzel HF: ";
statString += magnitude2;
statString += "\n";
if(magnitude1 < min_gerd1) {
min_gerd1 = magnitude1;
}
if(magnitude1 > max_gerd1) {
max_gerd1 = magnitude1;
}
moving_avg1 = (moving_avg1 + magnitude1) / 2;
if(magnitude2 < min_gerd2) {
min_gerd2 = magnitude2;
}
if(magnitude2 > max_gerd2) {
max_gerd2 = magnitude2;
}
moving_avg2 = (moving_avg2 + magnitude2) / 2;
counter++;
if(counter >= 100) {
Serial.println("**********************************************");
Serial.println("************** 200 Samples *****************");
Serial.println("**********************************************");
counter = 0;
delay(10000);
}
// Set min_gerd on first run
if(!has_gerd) {
min_gerd1 = magnitude1;
min_gerd2 = magnitude2;
}
// Reset for next run of good ole Gertz
Q11 = 0;
Q21 = 0;
Q12 = 0;
Q22 = 0;
sample_cnt = 0;
File dataFile = SD.open("datalog.txt", FILE_WRITE);
if(dataFile) {
dataFile.println(dataString);
dataFile.print(statString);
dataFile.close();
Serial.println("Written to file!");
} else {
Serial.println("Error opening datalog.txt");
}
dataFile.close();
adc->adc1->startPDB(SAMPLE_FREQ);
dma.attachInterrupt(dma_isr);
dma.enable();
}