diff --git a/libraries/CMakeLists.txt b/libraries/CMakeLists.txt index bbe1263a..0304d153 100644 --- a/libraries/CMakeLists.txt +++ b/libraries/CMakeLists.txt @@ -31,3 +31,4 @@ add_subdirectory(plasma2040) add_subdirectory(badger2040) add_subdirectory(servo2040) add_subdirectory(motor2040) +add_subdirectory(adcfft) diff --git a/libraries/adcfft/CMakeLists.txt b/libraries/adcfft/CMakeLists.txt new file mode 100644 index 00000000..0cb95843 --- /dev/null +++ b/libraries/adcfft/CMakeLists.txt @@ -0,0 +1 @@ +include(adcfft.cmake) \ No newline at end of file diff --git a/libraries/adcfft/adcfft.cmake b/libraries/adcfft/adcfft.cmake new file mode 100644 index 00000000..0d2046d8 --- /dev/null +++ b/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) \ No newline at end of file diff --git a/libraries/adcfft/adcfft.cpp b/libraries/adcfft/adcfft.cpp new file mode 100644 index 00000000..e5433d57 --- /dev/null +++ b/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 + +// 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; + } +} \ No newline at end of file diff --git a/libraries/adcfft/adcfft.hpp b/libraries/adcfft/adcfft.hpp new file mode 100644 index 00000000..ec81faaa --- /dev/null +++ b/libraries/adcfft/adcfft.hpp @@ -0,0 +1,73 @@ +#include +#include +#include +#include + +#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); +}; \ No newline at end of file diff --git a/micropython/modules/adcfft/adcfft.c b/micropython/modules/adcfft/adcfft.c new file mode 100644 index 00000000..e0610f5d --- /dev/null +++ b/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); \ No newline at end of file diff --git a/micropython/modules/adcfft/adcfft.cpp b/micropython/modules/adcfft/adcfft.cpp new file mode 100644 index 00000000..a1c78767 --- /dev/null +++ b/micropython/modules/adcfft/adcfft.cpp @@ -0,0 +1,63 @@ +#include "libraries/adcfft/adcfft.hpp" +#include + +#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))); +} +} \ No newline at end of file diff --git a/micropython/modules/adcfft/adcfft.h b/micropython/modules/adcfft/adcfft.h new file mode 100644 index 00000000..5cb45e9b --- /dev/null +++ b/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); \ No newline at end of file diff --git a/micropython/modules/adcfft/micropython.cmake b/micropython/modules/adcfft/micropython.cmake new file mode 100644 index 00000000..69302b62 --- /dev/null +++ b/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 +) diff --git a/micropython/modules/micropython.cmake b/micropython/modules/micropython.cmake index 46549325..44a39f9b 100644 --- a/micropython/modules/micropython.cmake +++ b/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)