ADC FFT library & MicroPython Bindings

libraries/CMakeLists.txt

@@ -31,3 +31,4 @@ add_subdirectory(plasma2040)
 add_subdirectory(badger2040)
 add_subdirectory(servo2040)
 add_subdirectory(motor2040)
+add_subdirectory(adcfft)

libraries/adcfft/CMakeLists.txt

@@ -0,0 +1 @@
+include(adcfft.cmake)

libraries/adcfft/adcfft.cmake

@@ -0,0 +1,7 @@
+add_library(adcfft 
+    ${CMAKE_CURRENT_LIST_DIR}/adcfft.cpp
+)
+
+target_include_directories(adcfft INTERFACE ${CMAKE_CURRENT_LIST_DIR})
+
+target_link_libraries(adcfft pico_stdlib hardware_pio hardware_dma hardware_adc hardware_irq)

libraries/adcfft/adcfft.cpp

@@ -0,0 +1,193 @@
+/**
+ * Hunter Adams (vha3@cornell.edu)
+ * Reproduced and modified with explicit permission
+ * 
+ * Original code in action:
+ * https://www.youtube.com/watch?v=8aibPy4yzCk
+ *
+ */
+#include "adcfft.hpp"
+#include <algorithm>
+
+// Adapted from https://github.com/raspberrypi/pico-sdk/blob/master/src/host/pico_bit_ops/bit_ops.c
+uint16_t __always_inline __revs(uint16_t v) {
+    v = ((v & 0x5555u) << 1u) | ((v >> 1u) & 0x5555u);
+    v = ((v & 0x3333u) << 2u) | ((v >> 2u) & 0x3333u);
+    v = ((v & 0x0f0fu) << 4u) | ((v >> 4u) & 0x0f0fu);
+    return ((v >> 8u) & 0x00ffu) | ((v & 0x00ffu) << 8u);
+}
+
+ADCFFT::~ADCFFT() {
+    dma_channel_abort(dma_channel);
+    dma_channel_unclaim(dma_channel);
+    adc_run(false);
+}
+
+int ADCFFT::get_scaled(unsigned int i, unsigned int scale) {
+    return fix15_to_int(multiply_fix15(fr[i], int_to_fix15(scale)));
+}
+
+void ADCFFT::init() {
+
+    // Populate Filter and Sine tables
+    for (auto ii = 0u; ii < SAMPLE_COUNT; ii++) {
+        // Full sine wave with period NUM_SAMPLES
+        // Wolfram Alpha: Plot[(sin(2 * pi * (x / 1.0))), {x, 0, 1}]
+        sine_table[ii] = float_to_fix15(0.5f * sin((M_PI * 2.0f) * ((float) ii) / (float)SAMPLE_COUNT));
+
+        // This is a crude approximation of a Lanczos window.
+        // Wolfram Alpha Comparison: Plot[0.5 * (1.0 - cos(2 * pi * (x / 1.0))), {x, 0, 1}], Plot[LanczosWindow[x - 0.5], {x, 0, 1}]
+        filter_window[ii] = float_to_fix15(0.5f * (1.0f - cos((M_PI * 2.0f) * ((float) ii) / ((float)SAMPLE_COUNT))));
+    }
+
+    // ADC Configuration
+
+    // Init GPIO for analogue use: hi-Z, no pulls, disable digital input buffer.
+    adc_gpio_init(adc_pin);
+
+    // Initialize the ADC harware
+    // (resets it, enables the clock, spins until the hardware is ready)
+    adc_init();
+
+    // Select analog mux input (0...3 are GPIO 26, 27, 28, 29; 4 is temp sensor)
+    adc_select_input(adc_channel);
+
+    // Setup the FIFO
+    adc_fifo_setup(
+        true,    // Write each completed conversion to the sample FIFO
+        true,    // Enable DMA data request (DREQ)
+        1,       // DREQ (and IRQ) asserted when at least 1 sample present
+        false,   // We won't see the ERR bit because of 8 bit reads; disable.
+        true     // Shift each sample to 8 bits when pushing to FIFO
+    );
+
+    // Divisor of 0 -> full speed. Free-running capture with the divider is
+    // equivalent to pressing the ADC_CS_START_ONCE button once per `div + 1`
+    // cycles (div not necessarily an integer). Each conversion takes 96
+    // cycles, so in general you want a divider of 0 (hold down the button
+    // continuously) or > 95 (take samples less frequently than 96 cycle
+    // intervals). This is all timed by the 48 MHz ADC clock.
+    adc_set_clkdiv(48000000.0f / sample_rate);
+
+    // DMA Configuration
+
+    dma_channel_config dma_config = dma_channel_get_default_config(dma_channel);
+
+    // Reading from constant address, writing to incrementing byte addresses
+    channel_config_set_transfer_data_size(&dma_config, DMA_SIZE_8);
+    channel_config_set_read_increment(&dma_config, false);
+    channel_config_set_write_increment(&dma_config, true);
+
+    // Pace transfers based on availability of ADC samples
+    channel_config_set_dreq(&dma_config, DREQ_ADC);
+
+    dma_channel_configure(dma_channel,
+        &dma_config,    // channel config
+        sample_array,   // destination
+        &adc_hw->fifo,  // source
+        SAMPLE_COUNT,   // transfer count
+        true            // start immediately
+    );
+
+    adc_run(true);
+}
+
+void ADCFFT::update() {
+    float max_freq = 0;
+
+    // Wait for NUM_SAMPLES samples to be gathered
+    // Measure wait time with timer
+    dma_channel_wait_for_finish_blocking(dma_channel);
+
+    // Copy/window elements into a fixed-point array
+    for (auto i = 0u; i < SAMPLE_COUNT; i++) {
+        fr[i] = multiply_fix15(int_to_fix15((int)sample_array[i]), filter_window[i]);
+        fi[i] = (fix15)0;
+    }
+
+    // Restart the sample channel, now that we have our copy of the samples
+    dma_channel_set_write_addr(dma_channel, sample_array, true);
+
+    // Compute the FFT
+    FFT();
+
+    // Find the magnitudes
+    for (auto i = 0u; i < (SAMPLE_COUNT / 2u); i++) {
+        // get the approx magnitude
+        fr[i] = abs(fr[i]); //>>9
+        fi[i] = abs(fi[i]);
+        // reuse fr to hold magnitude
+        fr[i] = std::max(fr[i], fi[i]) + 
+                multiply_fix15(std::min(fr[i], fi[i]), float_to_fix15(0.4f)); 
+
+        // Keep track of maximum
+        if (fr[i] > max_freq && i >= 5u) {
+            max_freq = ADCFFT::fr[i];
+            max_freq_dex = i;
+        }
+    }
+}
+
+float ADCFFT::max_frequency() {
+    return max_freq_dex * (sample_rate / SAMPLE_COUNT);
+}
+
+void ADCFFT::FFT() {
+    // Bit Reversal Permutation
+    // Bit reversal code below originally based on that found here: 
+    // https://graphics.stanford.edu/~seander/bithacks.html#BitReverseObvious
+    // https://en.wikipedia.org/wiki/Bit-reversal_permutation
+    // Detail here: https://vanhunteradams.com/FFT/FFT.html#Single-point-transforms-(reordering)
+    //
+    // PH: Converted to stdlib functions and __revs so it doesn't hurt my eyes
+    for (auto m = 1u; m < SAMPLE_COUNT - 1u; m++) {
+        unsigned int mr = __revs(m) >> shift_amount;
+        // don't swap that which has already been swapped
+        if (mr <= m) continue;
+        // swap the bit-reveresed indices
+        std::swap(fr[m], fr[mr]);
+        std::swap(fi[m], fi[mr]);
+    }
+
+    // Danielson-Lanczos
+    // Adapted from code by:
+    // Tom Roberts 11/8/89 and Malcolm Slaney 12/15/94 malcolm@interval.com
+    // Detail here: https://vanhunteradams.com/FFT/FFT.html#Two-point-transforms
+    // Length of the FFT's being combined (starts at 1)
+    //
+    // PH: Moved variable declarations to first-use so types are visually explicit.
+    // PH: Removed div 2 on sine table values, have computed the sine table pre-divided.
+    unsigned int L = 1;
+    int k = log2_samples - 1;
+
+    // While the length of the FFT's being combined is less than the number of gathered samples
+    while (L < SAMPLE_COUNT) {
+        // Determine the length of the FFT which will result from combining two FFT's
+        int istep = L << 1;
+        // For each element in the FFT's that are being combined
+        for (auto m = 0u; m < L; ++m) { 
+            // Lookup the trig values for that element
+            int j = m << k; // index into sine_table
+            fix15 wr =  sine_table[j + SAMPLE_COUNT / 4];
+            fix15 wi = -sine_table[j];
+            // i gets the index of one of the FFT elements being combined
+            for (auto i = m; i < SAMPLE_COUNT; i += istep) {
+                // j gets the index of the FFT element being combined with i
+                int j = i + L;
+                // compute the trig terms (bottom half of the above matrix)
+                fix15 tr = multiply_fix15(wr, fr[j]) - multiply_fix15(wi, fi[j]);
+                fix15 ti = multiply_fix15(wr, fi[j]) + multiply_fix15(wi, fr[j]);
+                // divide ith index elements by two (top half of above matrix)
+                fix15 qr = fr[i] >> 1;
+                fix15 qi = fi[i] >> 1;
+                // compute the new values at each index
+                fr[j] = qr - tr;
+                fi[j] = qi - ti;
+                fr[i] = qr + tr;
+                fi[i] = qi + ti;
+            }    
+        }
+        --k;
+        L = istep;
+    }
+}

libraries/adcfft/adcfft.hpp

@@ -0,0 +1,73 @@
+#include <stdio.h>
+#include <stdlib.h>
+#include <math.h>
+#include <cstring>
+
+#include "pico/stdlib.h"
+
+#include "hardware/pio.h"
+#include "hardware/dma.h"
+#include "hardware/adc.h"
+#include "hardware/irq.h"
+
+typedef signed int fix15;
+
+// Helpers for 16.15 fixed-point arithmetic
+constexpr __always_inline fix15 multiply_fix15(fix15 a, fix15 b) {return (fix15)(((signed long long)(a) * (signed long long)(b)) >> 15);}
+constexpr __always_inline fix15 float_to_fix15(float a) {return (fix15)(a * 32768.0f);}
+constexpr __always_inline float fix15_to_float(fix15 a) {return (float)(a) / 32768.0f;}
+constexpr __always_inline fix15 int_to_fix15(int a) {return (fix15)(a << 15);}
+constexpr __always_inline int fix15_to_int(fix15 a) {return (int)(a >> 15);}
+
+constexpr unsigned int SAMPLE_COUNT = 512u;
+
+class ADCFFT {
+    private:
+    
+        unsigned int adc_channel;
+        unsigned int adc_pin;
+        float sample_rate;
+
+        unsigned int log2_samples;
+        unsigned int shift_amount;
+
+        int dma_channel;
+
+        // Here's where we'll have the DMA channel put ADC samples
+        uint8_t sample_array[SAMPLE_COUNT];
+
+        // Lookup tables
+        fix15 sine_table[SAMPLE_COUNT];    // a table of sines for the FFT
+        fix15 filter_window[SAMPLE_COUNT]; // a table of window values for the FFT
+
+        // And here's where we'll copy those samples for FFT calculation
+        fix15 fr[SAMPLE_COUNT];
+        fix15 fi[SAMPLE_COUNT];
+
+        int max_freq_dex = 0;
+        
+        void FFT();
+        void init();
+    public:
+        ADCFFT() : ADCFFT(0, 26, 10000.0f) {};
+        ADCFFT(unsigned int adc_channel, unsigned int adc_pin) : ADCFFT(adc_channel, adc_pin, 10000.0f) {}
+        ADCFFT(unsigned int adc_channel, unsigned int adc_pin, float sample_rate) :
+            adc_channel(adc_channel), adc_pin(adc_pin), sample_rate(sample_rate) {
+                log2_samples = log2(SAMPLE_COUNT);
+                shift_amount = 16u - log2_samples;
+
+                dma_channel = dma_claim_unused_channel(true);
+
+                memset(sample_array, 0, SAMPLE_COUNT);
+
+                memset(fr, 0, SAMPLE_COUNT * sizeof(fix15));
+                memset(fi, 0, SAMPLE_COUNT * sizeof(fix15));
+
+                init();
+        };
+        ~ADCFFT();
+
+        void update();
+        float max_frequency();
+        int get_scaled(unsigned int i, unsigned int scale);
+};

micropython/modules/adcfft/adcfft.c

@@ -0,0 +1,37 @@
+#include "adcfft.h"
+
+// Class
+MP_DEFINE_CONST_FUN_OBJ_1(adcfft_update_obj, adcfft_update);
+MP_DEFINE_CONST_FUN_OBJ_3(adcfft_get_scaled_obj, adcfft_get_scaled);
+MP_DEFINE_CONST_FUN_OBJ_1(adcfft__del__obj, adcfft__del__);
+
+STATIC const mp_rom_map_elem_t adcfft_locals_dict_table[] = {
+    { MP_ROM_QSTR(MP_QSTR___del__), MP_ROM_PTR(&adcfft__del__obj) },
+    { MP_ROM_QSTR(MP_QSTR_update), MP_ROM_PTR(&adcfft_update_obj) },
+    { MP_ROM_QSTR(MP_QSTR_get_scaled), MP_ROM_PTR(&adcfft_get_scaled_obj) },
+};
+
+STATIC MP_DEFINE_CONST_DICT(adcfft_locals_dict, adcfft_locals_dict_table);
+
+const mp_obj_type_t adcfft_type = {
+    { &mp_type_type },
+    .name = MP_QSTR_ADCFFT,
+    .print = adcfft_print,
+    .make_new = adcfft_make_new,
+    .locals_dict = (mp_obj_dict_t*)&adcfft_locals_dict,
+};
+
+// Module
+STATIC const mp_map_elem_t adcfft_globals_table[] = {
+    { MP_OBJ_NEW_QSTR(MP_QSTR___name__), MP_OBJ_NEW_QSTR(MP_QSTR_adcfft) },  // Module name
+    { MP_OBJ_NEW_QSTR(MP_QSTR_ADCFFT), (mp_obj_t)&adcfft_type },             // Class name & type
+};
+STATIC MP_DEFINE_CONST_DICT(mp_module_adcfft_globals, adcfft_globals_table);
+
+
+const mp_obj_module_t adcfft_user_cmodule = {
+    .base = { &mp_type_module },
+    .globals = (mp_obj_dict_t*)&mp_module_adcfft_globals,
+};
+
+MP_REGISTER_MODULE(MP_QSTR_adcfft, adcfft_user_cmodule, MODULE_ADCFFT_ENABLED);

micropython/modules/adcfft/adcfft.cpp

@@ -0,0 +1,63 @@
+#include "libraries/adcfft/adcfft.hpp"
+#include <new>
+
+#define MP_OBJ_TO_PTR2(o, t) ((t *)(uintptr_t)(o))
+
+
+extern "C" {
+#include "adcfft.h"
+
+typedef struct _adcfft_obj_t {
+    mp_obj_base_t base;
+    ADCFFT *adcfft;
+} adcfft_obj_t;
+
+void adcfft_print(const mp_print_t *print, mp_obj_t self_in, mp_print_kind_t kind) {
+    (void)kind; //Unused input parameter    
+    adcfft_obj_t *self = MP_OBJ_TO_PTR2(self_in, adcfft_obj_t);
+    (void)self;
+    mp_print_str(print, "ADCFFT()");
+}
+
+mp_obj_t adcfft_make_new(const mp_obj_type_t *type, size_t n_args, size_t n_kw, const mp_obj_t *all_args) {
+    enum { ARG_adc_channel, ARG_adc_gpio, ARG_sample_rate };
+    static const mp_arg_t allowed_args[] = {
+        { MP_QSTR_adc_channel, MP_ARG_INT, {.u_int = 0} },
+        { MP_QSTR_adc_gpio, MP_ARG_INT, {.u_int = 26} },
+        { MP_QSTR_sample_rate, MP_ARG_INT, {.u_int = 10000u} }
+    };
+
+    // Parse args.
+    mp_arg_val_t args[MP_ARRAY_SIZE(allowed_args)];
+    mp_arg_parse_all_kw_array(n_args, n_kw, all_args, MP_ARRAY_SIZE(allowed_args), allowed_args, args);
+
+    adcfft_obj_t *self = m_new_obj_with_finaliser(adcfft_obj_t);
+    self->base.type = &adcfft_type;
+
+    unsigned int adc_channel = args[ARG_adc_channel].u_int;
+    unsigned int adc_gpio = args[ARG_adc_gpio].u_int;
+    float sample_rate = (float)args[ARG_sample_rate].u_int;
+
+    self->adcfft = new(m_new(ADCFFT, 1)) ADCFFT(adc_channel, adc_gpio, sample_rate);
+
+    return MP_OBJ_FROM_PTR(self);
+}
+
+mp_obj_t adcfft__del__(mp_obj_t self_in) {
+    adcfft_obj_t *self = MP_OBJ_TO_PTR2(self_in, adcfft_obj_t);
+    self->adcfft->~ADCFFT();
+    m_del(ADCFFT, self->adcfft, 1);
+    return mp_const_none;
+}
+
+mp_obj_t adcfft_update(mp_obj_t self_in) {
+    adcfft_obj_t *self = MP_OBJ_TO_PTR2(self_in, adcfft_obj_t);
+    self->adcfft->update();
+    return mp_const_none;
+}
+
+mp_obj_t adcfft_get_scaled(mp_obj_t self_in, mp_obj_t index, mp_obj_t scale) {
+    adcfft_obj_t *self = MP_OBJ_TO_PTR2(self_in, adcfft_obj_t);
+    return mp_obj_new_int(self->adcfft->get_scaled(mp_obj_get_int(index), mp_obj_get_int(scale)));
+}
+}

micropython/modules/adcfft/adcfft.h

@@ -0,0 +1,15 @@
+// Include MicroPython API.
+#include "py/runtime.h"
+
+/***** Constants *****/
+
+/***** Extern of Class Definition *****/
+extern const mp_obj_type_t adcfft_type;
+
+/***** Extern of Class Methods *****/
+extern void adcfft_print(const mp_print_t *print, mp_obj_t self_in, mp_print_kind_t kind);
+extern mp_obj_t adcfft_make_new(const mp_obj_type_t *type, size_t n_args, size_t n_kw, const mp_obj_t *all_args);
+extern mp_obj_t adcfft__del__(mp_obj_t self_in);
+
+extern mp_obj_t adcfft_update(mp_obj_t self_in);
+extern mp_obj_t adcfft_get_scaled(mp_obj_t self_in, mp_obj_t index, mp_obj_t scale);

micropython/modules/adcfft/micropython.cmake

@@ -0,0 +1,24 @@
+set(MOD_NAME adcfft)
+string(TOUPPER ${MOD_NAME} MOD_NAME_UPPER)
+add_library(usermod_${MOD_NAME} INTERFACE)
+
+target_sources(usermod_${MOD_NAME} INTERFACE
+    ${CMAKE_CURRENT_LIST_DIR}/${MOD_NAME}.c
+    ${CMAKE_CURRENT_LIST_DIR}/${MOD_NAME}.cpp
+    ${CMAKE_CURRENT_LIST_DIR}/../../../libraries/adcfft/adcfft.cpp
+)
+
+target_include_directories(usermod_${MOD_NAME} INTERFACE
+    ${CMAKE_CURRENT_LIST_DIR}
+)
+
+target_compile_definitions(usermod_${MOD_NAME} INTERFACE
+    MODULE_ADCFFT_ENABLED=1
+)
+
+target_link_libraries(usermod INTERFACE usermod_${MOD_NAME}
+    hardware_pio
+    hardware_dma
+    hardware_adc
+    hardware_irq
+)

micropython/modules/micropython.cmake

@@ -45,6 +45,7 @@ include(encoder/micropython)
 include(motor/micropython)
 include(ulab/code/micropython)
 include(qrcode/micropython/micropython)
+include(adcfft/micropython)
 
 include(st7789/micropython)