/* Audio Spectrum Display
Copyright 2013 Tony DiCola (tony@tonydicola.com)
This code is part of the guide at http://learn.adafruit.com/fft-fun-with-fourier-transforms/
Functions borrowed from sqrt_integer.h taken from Audio Library for Teensy 3.X. The following restrictions apply
* Audio Library for Teensy 3.X
* Copyright (c) 2014, Paul Stoffregen, paul@pjrc.com
*
* Development of this audio library was funded by PJRC.COM, LLC by sales of
* Teensy and Audio Adaptor boards. Please support PJRC's efforts to develop
* open source software by purchasing Teensy or other PJRC products.
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
* of this software and associated documentation files (the "Software"), to deal
* in the Software without restriction, including without limitation the rights
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
* copies of the Software, and to permit persons to whom the Software is
* furnished to do so, subject to the following conditions:
*
* The above copyright notice, development funding notice, and this permission
* notice shall be included in all copies or substantial portions of the Software.
*
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
* THE SOFTWARE.
*/
#define ARM_MATH_CM4
#include <arm_math.h>
#include <Adafruit_NeoPixel.h>
#include <stdint.h>
////////////////////////////////////////////////////////////////////////////////
// CONIFIGURATION
// These values can be changed to alter the behavior of the spectrum display.
////////////////////////////////////////////////////////////////////////////////
int SAMPLE_RATE_HZ = 66000; // Sample rate of the audio in hertz.
float SPECTRUM_MIN_DB = 30.0; // Audio intensity (in decibels) that maps to low LED brightness.
float SPECTRUM_MAX_DB = 60.0; // Audio intensity (in decibels) that maps to high LED brightness.
int LEDS_ENABLED = 1; // Control if the LED's should display the spectrum or not. 1 is true, 0 is false.
// Useful for turning the LED display on and off with commands from the serial port.
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 = 16; // Input ADC pin for audio data.
const int ANALOG_READ_RESOLUTION = 10; // Bits of resolution for the ADC.
const int ANALOG_READ_AVERAGING = 1; // 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
const int OUTPUT_RATE_HZ = 22000; // Sample rate of the audio in hertz.
IntervalTimer outputTimer;
int outputCounter = -1;
const int TARGET_SAMPLE_KHZ = 20000; // We want this...
const int TARGET_OUTPUT_KHZ = 4000; // Moved onto this...
// Pitch shift values
int targetFq = 0;
int lowBin = 0;
int highBin = 0;
int binOffset = 0;
int availableBeforeMirror = 0;
////////////////////////////////////////////////////////////////////////////////
// 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];
short samplesq15[FFT_SIZE*2];
short magnitudesq15[FFT_SIZE];
int sampleCounter = 0;
bool shouldFFTNow = false;
Adafruit_NeoPixel pixels = Adafruit_NeoPixel(NEO_PIXEL_COUNT, NEO_PIXEL_PIN, NEO_GRB + NEO_KHZ800);
char commandBuffer[MAX_CHARS];
float frequencyWindow[NEO_PIXEL_COUNT+1];
float hues[NEO_PIXEL_COUNT];
arm_cfft_radix4_instance_q15 q15Fft_inst;
arm_cfft_radix4_instance_q15 q15iFft_inst;
////////////////////////////////////////////////////////////////////////////////
// MAIN SKETCH FUNCTIONS
////////////////////////////////////////////////////////////////////////////////
void setup() {
// Set up serial port.
Serial.begin(38400);
// Set up ADC and audio input.
pinMode(AUDIO_INPUT_PIN, INPUT);
analogReadResolution(ANALOG_READ_RESOLUTION);
analogReadAveraging(ANALOG_READ_AVERAGING);
analogWriteResolution(10);
// 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));
arm_cfft_radix4_init_q15(&q15Fft_inst, FFT_SIZE, 0, 1);
arm_cfft_radix4_init_q15(&q15iFft_inst, FFT_SIZE, 1, 1);
setupPitchShift();
// Initialize spectrum display
spectrumSetup();
// Begin sampling audio
samplingBegin();
}
void loop() {
// Calculate FFT if a full sample is available.
if (shouldFFTNow) {
shouldFFTNow = false;
// Run FFT on sample data.
//arm_cfft_radix4_instance_q15 q15Fft_inst;
//arm_cfft_radix4_init_q15(&q15Fft_inst, FFT_SIZE, 0, 1);
for (int i=0; i < FFT_SIZE; i++)
{
int j = (i*2);
samplesq15[j] = myF32ToQ15(samples[j] / 1024.0);
samplesq15[j+1] = 0;
}
// FFT sample
arm_cfft_radix4_q15(&q15Fft_inst, samplesq15);
for (int i=0; i < 128; i++) {
uint32_t tmp = *((uint32_t *)samplesq15 + i); // real & imag
uint32_t magsq = multiply_16tx16t_add_16bx16b(tmp, tmp);
magnitudes[i] = sqrt_uint32_approx(magsq);
if (magnitudes[i] < 0.0)
{
magnitudes[i] = 0.0;
}
}
pitchShift(samplesq15);
// iFFT samples
arm_cfft_radix4_q15(&q15iFft_inst, samplesq15);
outputBegin();
if (LEDS_ENABLED == 1)
{
spectrumLoop();
}
// Restart audio sampling.
samplingBegin();
}
// Parse any pending commands.
parserLoop();
}
void setupPitchShift()
{
targetFq = TARGET_OUTPUT_KHZ;//(TARGET_OUTPUT_KHZ * SAMPLE_RATE_HZ) / OUTPUT_RATE_HZ;
lowBin = (int)((targetFq / (float)SAMPLE_RATE_HZ) * FFT_SIZE); // 10
highBin = (int)((TARGET_SAMPLE_KHZ / (float)SAMPLE_RATE_HZ) * FFT_SIZE); //104
binOffset = (highBin - lowBin) * 2; // 188 (94*2)
availableBeforeMirror = (FFT_SIZE - binOffset)-2; // (256 - 188 = 68)
}
void pitchShift(q15_t* bufferToShiftPtr)
{
q15_t *destinationPtr = bufferToShiftPtr + 2; // Leave the first value alone...
q15_t *sourcePtr = destinationPtr + binOffset;
for (int i=0; i < availableBeforeMirror; ++i, destinationPtr++, sourcePtr++)
{
// Copy down...
destinationPtr[0] = sourcePtr[0];
}
for (int i= availableBeforeMirror; i < ((FFT_SIZE*2)-2); ++i, destinationPtr++)
{
// pad to 0...
destinationPtr[0] = 0;
}
}
float phase = 0.0;
// [[ Output ]]
void outputCallback() {
analogWrite(A14, (int)samplesq15[outputCounter] );
outputCounter += 2;
if (outputCounter >= 200){//FFT_SIZE*2) {
//digitalWrite(POWER_LED_PIN, LOW);
outputTimer.end();
// Restart audio sampling.
samplingBegin();
}
}
void outputBegin() {
// Reset output buffer position and start callback at necessary rate.
digitalWrite(POWER_LED_PIN, HIGH);
outputCounter = 0;
outputTimer.begin(outputCallback, 1000000/OUTPUT_RATE_HZ);
}
q15_t myF32ToQ15(float32_t floatVal)
{
q15_t q15Val = (1<<15)*(floatVal);
if (floatVal == 1.0)
{
q15Val = 32767;
}
return q15Val;
}
////////////////////////////////////////////////////////////////////////////////
// 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 from HSV values (in floating point 0 to 1.0) to RGB colors usable
// by neo pixel functions.
uint32_t pixelHSVtoRGBColor(float hue, float saturation, float value) {
// Implemented from algorithm at http://en.wikipedia.org/wiki/HSL_and_HSV#From_HSV
float chroma = value * saturation;
float h1 = float(hue)/60.0;
float x = chroma*(1.0-fabs(fmod(h1, 2.0)-1.0));
float r = 0;
float g = 0;
float b = 0;
if (h1 < 1.0) {
r = chroma;
g = x;
}
else if (h1 < 2.0) {
r = x;
g = chroma;
}
else if (h1 < 3.0) {
g = chroma;
b = x;
}
else if (h1 < 4.0) {
g = x;
b = chroma;
}
else if (h1 < 5.0) {
r = x;
b = chroma;
}
else // h1 <= 6.0
{
r = chroma;
b = x;
}
float m = value - chroma;
r += m;
g += m;
b += m;
return pixels.Color(int(255*r), int(255*g), int(255*b));
}
////////////////////////////////////////////////////////////////////////////////
// SPECTRUM DISPLAY FUNCTIONS
///////////////////////////////////////////////////////////////////////////////
void spectrumSetup() {
// Set the frequency window values by evenly dividing the possible frequency
// spectrum across the number of neo pixels.
float windowSize = (SAMPLE_RATE_HZ / 2.0) / float(NEO_PIXEL_COUNT);
for (int i = 0; i < NEO_PIXEL_COUNT+1; ++i) {
frequencyWindow[i] = i*windowSize;
}
// Evenly spread hues across all pixels.
for (int i = 0; i < NEO_PIXEL_COUNT; ++i) {
hues[i] = 360.0*(float(i)/float(NEO_PIXEL_COUNT-1));
}
}
void spectrumLoop() {
// Update each LED based on the intensity of the audio
// in the associated frequency window.
float intensity, otherMean;
for (int i = 0; i < NEO_PIXEL_COUNT; ++i) {
windowMean(magnitudes,
frequencyToBin(frequencyWindow[i]),
frequencyToBin(frequencyWindow[i+1]),
&intensity,
&otherMean);
// Convert intensity to decibels.
intensity = 20.0*log10(intensity);
// Scale the intensity and clamp between 0 and 1.0.
intensity -= SPECTRUM_MIN_DB;
intensity = intensity < 0.0 ? 0.0 : intensity;
intensity /= (SPECTRUM_MAX_DB-SPECTRUM_MIN_DB);
intensity = intensity > 1.0 ? 1.0 : intensity;
pixels.setPixelColor(i, pixelHSVtoRGBColor(hues[i], 1.0, intensity));
}
pixels.show();
}
////////////////////////////////////////////////////////////////////////////////
// SAMPLING FUNCTIONS
////////////////////////////////////////////////////////////////////////////////
void samplingCallback() {
samples[sampleCounter] = 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) {
shouldFFTNow = true;
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(LEDS_ENABLED)
GET_AND_SET(SPECTRUM_MIN_DB)
GET_AND_SET(SPECTRUM_MAX_DB)
// Update spectrum display values if sample rate was changed.
if (strstr(command, "SET SAMPLE_RATE_HZ ") != NULL) {
spectrumSetup();
}
// Turn off the LEDs if the state changed.
if (LEDS_ENABLED == 0) {
for (int i = 0; i < NEO_PIXEL_COUNT; ++i) {
pixels.setPixelColor(i, 0);
}
pixels.show();
}
}
#ifdef __cplusplus
extern "C" const uint16_t sqrt_integer_guess_table[];
#else
extern const uint16_t sqrt_integer_guess_table[];
#endif
inline uint32_t sqrt_uint32(uint32_t in) __attribute__((always_inline,unused));
inline uint32_t sqrt_uint32(uint32_t in)
{
uint32_t n = sqrt_integer_guess_table[__builtin_clz(in)];
n = ((in / n) + n) / 2;
n = ((in / n) + n) / 2;
n = ((in / n) + n) / 2;
return n;
}
inline uint32_t sqrt_uint32_approx(uint32_t in) __attribute__((always_inline,unused));
inline uint32_t sqrt_uint32_approx(uint32_t in)
{
uint32_t n = sqrt_integer_guess_table[__builtin_clz(in)];
n = ((in / n) + n) / 2;
n = ((in / n) + n) / 2;
return n;
}
const uint16_t sqrt_integer_guess_table[33] = {
55109,
38968,
27555,
19484,
13778,
9742,
6889,
4871,
3445,
2436,
1723,
1218,
862,
609,
431,
305,
216,
153,
108,
77,
54,
39,
27,
20,
14,
10,
7,
5,
4,
3,
2,
1,
0
};
// computes ((a[15:0] * b[15:0]) + (a[31:16] * b[31:16]))
static inline int32_t multiply_16tx16t_add_16bx16b(uint32_t a, uint32_t b) __attribute__((always_inline, unused));
static inline int32_t multiply_16tx16t_add_16bx16b(uint32_t a, uint32_t b)
{
int32_t out;
asm volatile("smuad %0, %1, %2" : "=r" (out) : "r" (a), "r" (b));
return out;
}
) 
