BLE_UART_Bug.ino

// Audio Tone Input
// Copyright 2013 Tony DiCola (tony@tonydicola.com)

// This code is part of the guide at http://learn.adafruit.com/fft-fun-with-fourier-transforms/
// This code was modified by Miki Ryan (Cyborg-Supergirl / B34T-S)
// Neopixels are not being used at this point in time and can be left out of testing.

#define ARM_MATH_CM4
#include <arm_math.h>
#include <Adafruit_NeoPixel.h>
//#include <NoteList.h>

/*
NoteList.h
#ifndef NOTELIST_H_
#define NOTELIST_H_
#pragma once

#include <stdint.h>

////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//  NOTE FREQUENCY LOOKUP TABLE - Implemented from table found here:  https://pages.mtu.edu/~suits/notefreqs.html     //
//  THANKS GUYS!                                                                                                      //
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//  NoteList  //////////////////////////////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

class Note
{
public:
    Note(const char* name, float frequency);
    Note(const char* name, const char* altName, float frequency);

    const char* GetName() const;
    const char* GetAltName() const;
    float       GetFrequency() const;

private:
    const char* m_name;
    const char* m_altName;
    float       m_frequency;
};

struct NoteList
{
    static const uint8_t COUNT;
    static const uint8_t NO_NOTE;
    static const Note NOTES_TABLE[];
		
		static const Note& GetNote(uint8_t note);
};

#endif
*/
/*
NoteList.cpp
#include "NoteList.h"
#include <stdlib.h>
#include <assert.h>

Note::Note(const char* name, float frequency)
: m_name(name)
, m_altName(nullptr)
, m_frequency(frequency)
{
}

Note::Note(const char* name, const char* altName, float frequency)
: m_name(name)
, m_altName(altName)
, m_frequency(frequency)
{
}

const char* Note::GetName() const
{
    return m_name;
}

const char* Note::GetAltName() const
{
    if(m_altName != nullptr)
    {
        return m_altName;
    }
    static const char* s_default = "<none>";
    return s_default;
}

float Note::GetFrequency() const
{
    return m_frequency;
}

const uint8_t NoteList::COUNT = 108;
const uint8_t NoteList::NO_NOTE = COUNT + 1;
const Note NoteList::NOTES_TABLE[NoteList::COUNT] = {
    // It is _highly_ unlikely that a rinky-dink mic like the one I'm gonna use is ever gonna pick up notes this low, 
    // but I might as well include them.
    Note("C0",         16.35f),
    Note("Cs0", "Db0", 17.32f),
    Note("D0",         18.35f),
    Note("Ds0", "Eb0", 19.45f),
    Note("E0",         20.60f),
    Note("F0",         21.83f),
    Note("Fs0", "Gb0", 23.12f),
    Note("G0",         24.50f),
    Note("Gs0", "Ab0", 25.96f),
    Note("A0",         27.50f),
    Note("As0", "Bb0", 29.14f),
    Note("B0",         30.87f),
    Note("C1",         32.70f),
    Note("Cs1", "Db1", 34.65f),
    Note("D1",         36.71f),
    Note("Ds1", "Eb1", 38.89f),
    Note("E1",         41.20f),
    Note("F1",         43.65f),
    Note("Fs1", "Gb1", 46.25f),
    Note("G1",         49.00f),
    Note("Gs1", "Ab1", 51.91f),
    Note("A1",         55.00f),
    Note("As1", "Bb1", 58.27f),
    Note("B1",         61.74f),
    // Now entering the realm of sanity.
    Note("C2",         65.41f),
    Note("Cs2", "Db2", 69.30f),
    Note("D2",         73.42f),
    Note("Ds2", "Eb2", 77.78f),
    Note("E2",         82.41f),
    Note("F2",         87.31f),
    Note("Fs2", "Gb2", 92.50f),
    Note("G2",         98.00f),
    Note("Gs2", "Ab2", 103.83f),
    Note("A2",         110.00f),
    Note("As2", "Bb2", 116.54f),
    Note("B2",         123.47f),
    Note("C3",         130.81f),
    Note("Cs3", "Db3", 138.59f),
    Note("D3",         146.83f),
    Note("Ds3", "Eb3", 155.56f),
    Note("E3",         164.81f),
    Note("F3",         174.61f),
    Note("Fs3", "Gb3", 185.00f),
    Note("G3",         196.00f),
    Note("Gs3", "Ab3", 207.65f),
    Note("A3",         220.00f),
    Note("As3", "Bb3", 233.08f),
    Note("B3",         246.94f),
    Note("C4",         261.63f), // Middle C
    Note("Cs4", "Db4", 277.18f),
    Note("D4",         293.66f),
    Note("Ds4", "Eb4", 311.13f),
    Note("E4",         329.63f),
    Note("F4",         349.23f),
    Note("Fs4", "Gb4", 369.99f),
    Note("G4",         392.00f),
    Note("Gs4", "Ab4", 415.30f),
    Note("A4",         440.00f),
    Note("As4", "Bb4", 466.16f),
    Note("B4",         493.88f),
    Note("C5",         523.25f),
    Note("Cs5", "Db5", 554.37f),
    Note("D5",         587.33f),
    Note("Ds5", "Eb5", 622.25f),
    Note("E5",         659.25f),
    Note("F5",         698.46f),
    Note("Fs5", "Gb5", 739.99f),
    Note("G5",         783.99f),
    Note("Gs5", "Ab5", 830.61f),
    Note("A5",         880.00f),
    Note("As5", "Bb5", 932.33f),
    Note("B5",         987.77f),
    Note("C6",         1046.50f),
    Note("Cs6", "Db6", 1108.73f),
    Note("D6",         1174.66f),
    Note("Ds6", "Eb6", 1244.51f),
    Note("E6",         1318.51f),
    Note("F6",         1396.91f),
    Note("Fs6", "Gb6", 1479.98f),
    Note("G6",         1567.98f),
    Note("Gs6", "Ab6", 1661.22f),
    Note("A6",         1760.00f),
    Note("As6", "Bb6", 1864.66f),
    Note("B6",         1975.53f),
    Note("C7",         2093.00f),
    Note("Cs7", "Db7", 2217.46f),
    Note("D7",         2349.32f),
    Note("Ds7", "Eb7", 2489.02f),
    Note("E7",         2637.02f),
    Note("F7",         2793.83f),
    Note("Fs7", "Gb7", 2959.96f),
    Note("G7",         3135.96f),
    Note("Gs7", "Ab7", 3322.44f),
    Note("A7",         3520.00f),
    Note("As7", "Bb7", 3729.31f),
    Note("B7",         3951.07f),
    Note("C8",         4186.01f),
    Note("Cs8", "Db8", 4434.92f),
    Note("D8",         4698.63f),
    Note("Ds8", "Eb8", 4978.03f),
    Note("E8",         5274.04f),
    Note("F8",         5587.65f),
    Note("Fs8", "Gb8", 5919.91f),
    Note("G8",         6271.93f),
    Note("Gs8", "Ab8", 6644.88f),
    Note("A8",         7040.00f),
    Note("As8", "Bb8", 7458.62f),
    Note("B8",         7902.13f)
};

const Note& NoteList::GetNote(uint8_t note)
{
		assert(note >= 0 && note < COUNT);
		if(note == NO_NOTE)
		{
				static const Note s_default("<<NONE>>", 0.0f);
				return s_default;
		}
		return NOTES_TABLE[note];
}
*/

#include <Adafruit_BluefruitLE_UART.h>

////////////////////////////////////////////////////////////////////////////////
// CONIFIGURATION 
// These values can be changed to alter the behavior of the spectrum display.
////////////////////////////////////////////////////////////////////////////////

int SAMPLE_RATE_HZ = 9000;             // Sample rate of the audio in hertz.
float TONE_ERROR_MARGIN_HZ = 50;       // Allowed fudge factor above and below the bounds for each tone input.
int TONE_WINDOW_MS = 4000;             // Maximum amount of milliseconds allowed to enter the full sequence.
float TONE_THRESHOLD_DB = 20.0;        // Threshold (in decibels) each tone must be above other frequencies to count.
const int FFT_SIZE = 256;              // Size of the FFT.  Realistically can only be at most 256 
                                       // without running out of memory for buffers and other state.
const int AUDIO_INPUT_PIN = 14;        // Input ADC pin for audio data.
const int ANALOG_READ_RESOLUTION = 10; // Bits of resolution for the ADC.
const int ANALOG_READ_AVERAGING = 16;  // Number of samples to average with each ADC reading.
const int POWER_LED_PIN = 13;          // Output pin for power LED (pin 13 to use Teensy 3.0's onboard LED).
const int NEO_PIXEL_PIN = 3;           // Output pin for neo pixels.
const int NEO_PIXEL_COUNT = 4;         // Number of neo pixels.  You should be able to increase this without
                                       // any other changes to the program.
const int MAX_CHARS = 65;              // Max size of the input command buffer

uint8_t CURRENT_NOTE = NoteList::NO_NOTE;
uint8_t PREV_NOTE = NoteList::NO_NOTE;


////////////////////////////////////////////////////////////////////////////////
// INTERNAL STATE
// These shouldn't be modified unless you know what you're doing.
////////////////////////////////////////////////////////////////////////////////
IntervalTimer samplingTimer;
float samples[FFT_SIZE*2];
float magnitudes[FFT_SIZE];
int sampleCounter = 0;
Adafruit_NeoPixel pixels = Adafruit_NeoPixel(NEO_PIXEL_COUNT, NEO_PIXEL_PIN, NEO_GRB + NEO_KHZ800);
char commandBuffer[MAX_CHARS];
int tonePosition = 0;
unsigned long toneStart = 0;

// A small helper
void error(const __FlashStringHelper*err) {
  Serial.println(err);
  while (1);
}

//////////////////////////////////////////////////////////////////////////////////////////
// Bluefruit stuff
//////////////////////////////////////////////////////////////////////////////////////////
// https://forum.pjrc.com/threads/28906-Trouble-porting-Adafruit-nRF51-to-Teensy-3-1
//
// Modifications to the Adafruit_BluefruitLE_UART class had to be made as per
// the above forum thread. My UART friend is connected to a Teensy 3.2 in the
// manner described by user Neurogami.
//
// The Bluefruit CTS pin is connected to Teensy pin 8.
// The Bluefruit MOD pin is connected to Teensy pin 12.
//
// Bluefruit is switched to DATA ((marked as UART on mine)).
// Bluefruit RTS pin is connected to GND.
//
// Teensy rx2 (9)  goes to Bluefruit tx
// Teensy tx2 (10) goes to Bluefruit rx
////////////////////////////////////////////////////////////////////////////////
#define BLUEFRUIT_HWSERIAL_NAME        Serial2
#define BLUEFRUIT_UART_MODE_PIN        12    // Set to -1 if unused
#define VERBOSE_MODE                   true
#define FACTORY_RESET                  false
#define MODULE_NAME                    "B:Facemask"
Adafruit_BluefruitLE_UART ble(BLUEFRUIT_HWSERIAL_NAME, BLUEFRUIT_UART_MODE_PIN);

////////////////////////////////////////////////////////////////////////////////
// MAIN SKETCH FUNCTIONS
////////////////////////////////////////////////////////////////////////////////
void micSetup()
{
  // Set up ADC and audio input.
  pinMode(AUDIO_INPUT_PIN, INPUT);
  analogReadResolution(ANALOG_READ_RESOLUTION);
  analogReadAveraging(ANALOG_READ_AVERAGING);
  
  // Turn on the power indicator LED.
  pinMode(POWER_LED_PIN, OUTPUT);
  digitalWrite(POWER_LED_PIN, HIGH);
  
  // Initialize neo pixel library and turn off the LEDs
  pixels.begin();
  pixels.show(); 
  
  // Clear the input command buffer
  memset(commandBuffer, 0, sizeof(commandBuffer));
}

void bleSendNote();

bool canSendNote = false;
bool bluetoothConnected = false;
void bluetoothConnectedCallback()
{
  canSendNote = true;
  bluetoothConnected = true;
  Serial.println(F("********************************************************"));
  Serial.println(F("** CONNECTED! CHANGING TO DATA MODE!                  **"));
  Serial.println(F("********************************************************"));
  ble.setMode(BLUEFRUIT_MODE_DATA);
  ble.verbose(false);
}

void bluetoothDisconnectedCallback()
{
  //////////////////////////////////////////////////////////////////////////////////////////////////////
  // At a certain point after dispatching data, this callback gets fired off, but the blue 
  // "connected" LED remains on. There are no peripheral disconnection callbacks fired on my NodeJS
  // application (connected via noble). The UART friend also remains in DATA mode as confirmed by the 
  // mode indicator LED.
  //
  // When all of the lines except for the Serial.print* lines were commented out, the application
  // ran flawlessly, though whether that is consistent or a fluke remains to be seen.
  //////////////////////////////////////////////////////////////////////////////////////////////////////
  canSendNote = false;
  bluetoothConnected = false;
  Serial.println(F("********************************************************"));
  Serial.println(F("** DISCONNECTED! CHANGING TO COMMAND MODE!            **"));
  Serial.println(F("********************************************************"));
  ble.setMode(BLUEFRUIT_MODE_COMMAND);
}

void bluetoothDataCallback(char data[], uint16_t len)
{
}

void bluetoothSetup()
{
  if(!ble.begin(VERBOSE_MODE))
  {
    error(F("Couldn't find Bluefruit, make sure it's in CoMmanD mode & check wiring?"));
  }
  
  if(FACTORY_RESET)
  {
    if(!ble.factoryReset())
    {
      error(F("Could not perform factory reset."));
    }
  }

  ble.echo(false);
  ble.info();

  if(!ble.sendCommandCheckOK("AT+GAPDEVNAME=" MODULE_NAME))
  {
    error(F("Could not set the devname of the module " MODULE_NAME));
  }

  ble.reset();

  Serial.println(F("Please use Adafruit Bluefruit LE app to connect in UART mode"));
  ble.setConnectCallback(bluetoothConnectedCallback);
  ble.setDisconnectCallback(bluetoothDisconnectedCallback);
  ble.setBleUartRxCallback(bluetoothDataCallback);
}

void setup()
{
  // Set up serial port.
  //Serial.begin(38400);
  Serial.begin(115200);
  
  micSetup();
  bluetoothSetup();
  
  // Begin sampling audio
  samplingBegin();
}

void samplerLoop()
{
  // Calculate FFT if a full sample is available.
  if (samplingIsDone())
  {
    // Run FFT on sample data.
    arm_cfft_radix4_instance_f32 fft_inst;
    arm_cfft_radix4_init_f32(&fft_inst, FFT_SIZE, 0, 1);
    arm_cfft_radix4_f32(&fft_inst, samples);
    // Calculate magnitude of complex numbers output by the FFT.
    arm_cmplx_mag_f32(samples, magnitudes, FFT_SIZE);

    // Detect tone sequence.
    PREV_NOTE = CURRENT_NOTE;
    CURRENT_NOTE = toneLoop();
    if(PREV_NOTE != CURRENT_NOTE)
    {
      bleSendNote();
    }
  
    // Restart audio sampling.
    samplingBegin();
  }
}

void bleSendNote()
{
  if(bluetoothConnected && canSendNote)
  {
    // Only send note changes.
    //.if(PREV_NOTE != CURRENT_NOTE)
    {
      //union { uint8_t d[sizeof(float)]; float f; } freq;
      //freq.f = NoteList::NOTES_TABLE[CURRENT_NOTE].GetFrequency();
      //ble.print(freq.d, sizeof(float));
      //ble.flush();
      //ble.print(NoteList::NOTES_TABLE[CURRENT_NOTE].GetFrequency());
      //Serial.println((uint16_t)CURRENT_NOTE);
      ble.write(CURRENT_NOTE);
      ble.flush();
    }
  }
}

void bluetoothLoop()
{
  // Update the connected and disconnected states.
  ble.update(200);
}

void loop() 
{
  samplerLoop();
  bluetoothLoop();
  
  // Parse any pending commands.
  parserLoop();
}


////////////////////////////////////////////////////////////////////////////////
// UTILITY FUNCTIONS
////////////////////////////////////////////////////////////////////////////////

// Compute the average magnitude of a target frequency window vs. all other frequencies.
void windowMean(float* magnitudes, int lowBin, int highBin, float* windowMean, float* otherMean)
{
    *windowMean = 0;
    *otherMean = 0;
    // Notice the first magnitude bin is skipped because it represents the
    // average power of the signal.
    for (int i = 1; i < FFT_SIZE/2; ++i) {
      if (i >= lowBin && i <= highBin) {
        *windowMean += magnitudes[i];
      }
      else {
        *otherMean += magnitudes[i];
      }
    }
    *windowMean /= (highBin - lowBin) + 1;
    *otherMean /= (FFT_SIZE / 2 - (highBin - lowBin));
}

// Convert a frequency to the appropriate FFT bin it will fall within.
int frequencyToBin(float frequency)
{
  float binFrequency = float(SAMPLE_RATE_HZ) / float(FFT_SIZE);
  return int(frequency / binFrequency);
}

// Convert intensity to decibels
float intensityDb(float intensity)
{
  return 20.0*log10(intensity);
}


////////////////////////////////////////////////////////////////////////////////
// SPECTRUM DISPLAY FUNCTIONS
///////////////////////////////////////////////////////////////////////////////

uint8_t toneLoop()
{
  for(uint8_t i = 0; i<NoteList::COUNT; i++)
  {
    // Calculate the low and high frequency bins for the currently expected tone.
    const Note& note = NoteList::NOTES_TABLE[i];
    int lowBin = frequencyToBin(note.GetFrequency() - TONE_ERROR_MARGIN_HZ);
    int highBin = frequencyToBin(note.GetFrequency() + TONE_ERROR_MARGIN_HZ);
    
    // Get the average intensity of frequencies inside and outside the tone window.
    float window, other;
    windowMean(magnitudes, lowBin, highBin, &window, &other);
    window = intensityDb(window);
    other = intensityDb(other);
    // Check if tone intensity is above the threshold to detect a step in the sequence.
    if((window - other) >= TONE_THRESHOLD_DB)
    {
      return i;
    }
  }
  return NoteList::NO_NOTE;
}

////////////////////////////////////////////////////////////////////////////////
// SAMPLING FUNCTIONS
////////////////////////////////////////////////////////////////////////////////

void samplingCallback() 
{
  samples[sampleCounter] = (float32_t)analogRead(AUDIO_INPUT_PIN);
  // Complex FFT functions require a coefficient for the imaginary part of the input.
  // Since we only have real data, set this coefficient to zero.
  samples[sampleCounter+1] = 0.0;
  // Update sample buffer position and stop after the buffer is filled
  sampleCounter += 2;
  if (sampleCounter >= FFT_SIZE*2)
  {
    samplingTimer.end();
  }
}

void samplingBegin() {
  // Reset sample buffer position and start callback at necessary rate.
  sampleCounter = 0;
  samplingTimer.begin(samplingCallback, 1000000/SAMPLE_RATE_HZ);
}

boolean samplingIsDone() {
  return sampleCounter >= FFT_SIZE*2;
}


////////////////////////////////////////////////////////////////////////////////
// COMMAND PARSING FUNCTIONS
// These functions allow parsing simple commands input on the serial port.
// Commands allow reading and writing variables that control the device.
//
// All commands must end with a semicolon character.
// 
// Example commands are:
// GET SAMPLE_RATE_HZ;
// - Get the sample rate of the device.
// SET SAMPLE_RATE_HZ 400;
// - Set the sample rate of the device to 400 hertz.
// 
////////////////////////////////////////////////////////////////////////////////

void parserLoop() {
  // Process any incoming characters from the serial port
  while (Serial.available() > 0) {
    char c = Serial.read();
    // Add any characters that aren't the end of a command (semicolon) to the input buffer.
    if (c != ';') {
      c = toupper(c);
      strncat(commandBuffer, &c, 1);
    }
    else
    {
      // Parse the command because an end of command token was encountered.
      parseCommand(commandBuffer);
      // Clear the input buffer
      memset(commandBuffer, 0, sizeof(commandBuffer));
    }
  }
}

// Macro used in parseCommand function to simplify parsing get and set commands for a variable
#define GET_AND_SET(variableName) \
  else if (strcmp(command, "GET " #variableName) == 0) { \
    Serial.println(variableName); \
  } \
  else if (strstr(command, "SET " #variableName " ") != NULL) { \
    variableName = (typeof(variableName)) atof(command+(sizeof("SET " #variableName " ")-1)); \
  }

void parseCommand(char* command) {
  if (strcmp(command, "GET MAGNITUDES") == 0) {
    for (int i = 0; i < FFT_SIZE; ++i) {
      Serial.println(magnitudes[i]);
    }
  }
  else if (strcmp(command, "GET SAMPLES") == 0) {
    for (int i = 0; i < FFT_SIZE*2; i+=2) {
      Serial.println(samples[i]);
    }
  }
  else if (strcmp(command, "GET FFT_SIZE") == 0) {
    Serial.println(FFT_SIZE);
  }
  GET_AND_SET(SAMPLE_RATE_HZ)
  GET_AND_SET(TONE_ERROR_MARGIN_HZ)
  GET_AND_SET(TONE_WINDOW_MS)
  GET_AND_SET(TONE_THRESHOLD_DB)
}