I’ll just leave this here…
It’s not perfect, but holy crap it WORKS! Enjoy
#include "daisysp.h"
#include "daisy_patch.h"
#include <string>
#include <cmath>
#include <complex>
#include <vector>
#include <algorithm>
# define M_PI 3.14159265358979323846 /* pi */
// Interleaved audio definitions
#define LEFT (i)
#define RIGHT (i + 1)
//Set FFT window size
#define WINDOW 2048.0
// Set print interval in ms
#define PRINT_INTERVAL 250
//Set sample rate, block size
#define SAMPLE_RATE 48000.0
#define BLOCK_SIZE 64
using namespace daisysp;
using namespace daisy;
// Set up Daisy hardware seed
static DaisyPatch hw;
// Global variables
float detectedPitch = 440.0;
float lastPitch=100000000000.0;
double count = 0.0;
int printout=0;
// begin CHATGPT-written pitch detection code------------------------------------------------------------------------------
static DelayLine<float, static_cast<size_t>(WINDOW)> DSY_SDRAM_BSS PDsignal;
float findInterpolatedFrequency(const std::vector<std::complex<float>>& signal, float sampleRate) {
size_t N = signal.size();
float maxMag = 0.0f;
size_t maxIndex = 0;
// Find the index with the highest magnitude (real part squared + imaginary part squared)
for (size_t i = 1; i < N / 2 - 1; ++i) { // Avoid boundary issues
float magnitude = std::norm(signal[i]);
if (magnitude > maxMag) {
maxMag = magnitude;
maxIndex = i;
}
}
// Parabolic interpolation for more accurate peak frequency estimation
size_t i = maxIndex;
float magnitude0 = std::norm(signal[i - 1]);
float magnitude1 = std::norm(signal[i]);
float magnitude2 = std::norm(signal[i + 1]);
// Apply quadratic interpolation formula:
// f = f_max + 0.5 * (magnitude1 - magnitude2) / (magnitude0 - 2 * magnitude1 + magnitude2)
float peakShift = 0.5f * (magnitude0 - magnitude2) / (magnitude0 - 2.0f * magnitude1 + magnitude2);
// The frequency corresponding to the peak
float frequency = (float)i * sampleRate / N + peakShift * sampleRate / N;
return frequency;
}
void applyHanningWindow(std::vector<std::complex<float>>& signal) {
size_t N = signal.size();
for (size_t i = 0; i < N; ++i) {
float windowValue = 0.5f * (1.0f - cos(2.0f * (float)M_PI * i / (N - 1)));
signal[i] *= windowValue; // Apply the window to each signal sample
}
}
class PitchDetector {
public:
PitchDetector(float sampleRate) : m_sampleRate(sampleRate) {}
// Function to perform the FFT (Cooley-Tukey Radix-2 FFT)
void fft(std::vector<std::complex<float>>& signal) {
size_t N = signal.size();
// Base case: single element, do nothing
if (N <= 1)
return;
// Split the signal into even and odd indexed parts
std::vector<std::complex<float>> even(N / 2);
std::vector<std::complex<float>> odd(N / 2);
for (size_t i = 0; i < N / 2; ++i) {
even[i] = signal[2 * i]; // Even indexed
odd[i] = signal[2 * i + 1]; // Odd indexed
}
// Recursively compute the FFT of the even and odd parts
fft(even);
fft(odd);
// Combine the results
for (size_t k = 0; k < N / 2; ++k) {
std::complex<float> t = std::polar(1.0f, -2.0f * (float)M_PI * k / N) * odd[k];
signal[k] = even[k] + t;
signal[k + N / 2] = even[k] - t;
}
}
// Function to find the frequency with the highest magnitude (FFT result)
float findDominantFrequency(const std::vector<std::complex<float>>& signal, float sampleRate) {
size_t N = signal.size();
float maxMag = 0.0f;
size_t maxIndex = 0;
// Find the index with the highest magnitude (real part squared + imaginary part squared)
for (size_t i = 0; i < N / 2; ++i) {
float magnitude = std::norm(signal[i]); // norm = real^2 + imag^2
if (magnitude > maxMag) {
maxMag = magnitude;
maxIndex = i;
}
}
// Calculate the frequency corresponding to that index
return (float)maxIndex * sampleRate / N;
}
// Function to detect harmonics and the fundamental frequency (up to the 5th harmonic)
float detectPitch() {
// Create a complex vector to hold the FFT result (size must be a power of 2)
size_t N = 1024;
while (N < WINDOW) N *= 2; // Ensure N is a power of 2 (padded if necessary)
std::vector<std::complex<float>> fftSignal(N, {0.0f, 0.0f});
// Copy the delay line (time-domain signal) into the real part of the FFT input
for (int i = 0; i < WINDOW; ++i) {
fftSignal[i] = std::complex<float>(PDsignal.ReadSetLoc(i), 0.0f); // Only use the real part (left channel)
}
// Apply the Hanning window to the signal
applyHanningWindow(fftSignal);
// Perform the FFT
fft(fftSignal);
// Find the dominant frequency (fundamental frequency)
float fundamentalFrequency = findInterpolatedFrequency(fftSignal, m_sampleRate);
// Detect harmonics up to the 5th harmonic
std::vector<float> harmonics;
for (int i = 1; i <= 5; ++i) {
float harmonicFrequency = fundamentalFrequency * i;
// Find the closest FFT bin to the harmonic frequency
size_t binIndex = (size_t)(harmonicFrequency * N / m_sampleRate);
// If the bin has a significant magnitude, consider it a harmonic
if (binIndex < N / 2 && std::norm(fftSignal[binIndex]) > 0.5f) {
harmonics.push_back(harmonicFrequency);
}
}
// If we found harmonics, we return the fundamental frequency as the most likely pitch
if (!harmonics.empty()) {
return fundamentalFrequency;
}
// Otherwise, return the fundamental frequency based on the highest peak
return fundamentalFrequency;
}
private:
float m_sampleRate; // Sample rate of the audio (e.g., 44100 Hz)
};
// end CHATGPT-written pitch detection code----------------------------------------------------------------------------------------------
// DECLARE PITCH DETECTOR
PitchDetector pitchDetector(SAMPLE_RATE);
//
// Audio Callback function (the audio loop)
//
static void AudioCallback(AudioHandle::InterleavingInputBuffer in,
AudioHandle::InterleavingOutputBuffer out,
size_t size)
{
for(size_t i = 0; i < size; i += 2)
{
PDsignal.Write(in[LEFT]);
out[LEFT] = in[LEFT];
out[RIGHT] = in[RIGHT];
}
count += BLOCK_SIZE;
}
int main(void)
{
// Initialize seed hardware and daisysp modules
hw.seed.Configure();
hw.Init();
hw.SetAudioBlockSize(BLOCK_SIZE);
// Initialize and set up the delay lines
PDsignal.Init();
PDsignal.SetDelay((float)1200.0);
// Attempt serial communication
hw.seed.StartLog(false);
// Start audio loop
hw.seed.StartAudio(AudioCallback);
while(1) {
detectedPitch = pitchDetector.detectPitch();
// Trigger pring flag every PRINT_INTERVAL ms
if (count >= (((float)PRINT_INTERVAL)*(SAMPLE_RATE/1000.0f))){
printout = 1;
count = 0;
}
// Print messages if print flag is triggered
if((printout>0)){
hw.seed.PrintLine("pitch=%f",
detectedPitch);
}else{
hw.seed.Print("");
}
}
}