From facd41d269c7a773c425be05d1951c9bd56e8964 Mon Sep 17 00:00:00 2001 From: Narsi G Date: Wed, 25 Oct 2023 18:37:19 -0700 Subject: [PATCH 1/2] created two new C functions that can be accessed from the python layer set_emg_filter and set_emg_decim_rate --- lib/cionic/emg_iir.c | 13 +++++++---- lib/cionic/emg_iir.h | 11 +++++++--- shared-bindings/ads1x9x/ADS1x9x.c | 18 ++++++++++++++++ shared-module/ads1x9x/ADS1x9x.c | 36 +++++++++++++++++++++++++++++++ shared-module/ads1x9x/ADS1x9x.h | 2 ++ 5 files changed, 73 insertions(+), 7 deletions(-) diff --git a/lib/cionic/emg_iir.c b/lib/cionic/emg_iir.c index 2cbdcb33d357d..aea7bfc07a682 100644 --- a/lib/cionic/emg_iir.c +++ b/lib/cionic/emg_iir.c @@ -15,6 +15,10 @@ float emg_lowpass_filter_sos[NO_OF_BQS][NO_OF_COEFFS_PER_BQ] = { {1.0,2.0,1.0,1.0,-1.4083885078365557,0.7369080185138834}, }; +uint8_t emg_sub_sampling_rate = 1; +uint16_t ma_size = EMG_RMS_MA_SIZE; + + // takes as input a bank of biquad coefficients ( NO_OF_BQS of them ) with the // order of the coefficients as shown below. conveniently, this is the same // order of the SOS ( second order section ) coefficients as produced by the @@ -80,12 +84,12 @@ emg_mwa_rms(emg_mwa_state_t *state, float val) state->sum -= state->mw[state->write_ptr]; // update the delay line state->mw[state->write_ptr] = val*val; - state->write_ptr = (state->write_ptr+1) % EMG_RMS_MA_SIZE; + state->write_ptr = (state->write_ptr+1) % ma_size; // the latest sum state->sum += val*val; double result = state->sum; - result = sqrt(result/EMG_RMS_MA_SIZE); + result = sqrt(result/ma_size); #ifdef EMG_RMS_DC_BLOCKING result = result + state->dc; state->dc = state->dc - result*state->alpha; @@ -101,7 +105,8 @@ emg_mwa_rms(emg_mwa_state_t *state, float val) void iir_filter_init(iir_filter_t *filter) { - filter->emg_rms_sub_sample_counter = EMG_RMS_SUBSAMPLING_FACTOR; + filter->emg_rms_sub_sample_counter = emg_sub_sampling_rate; + ma_size = EMG_RMS_MA_SIZE/emg_sub_sampling_rate; for( int i=0; iemg_lowpass_iir_state[i]); emg_iir_init(&filter->emg_highpass_iir_state[i]); @@ -140,7 +145,7 @@ iir_filter_process(iir_filter_t *filter, float *norms, int numchans, if (filter->emg_rms_sub_sample_counter <= 0) { *ts_out = ts_in; - filter->emg_rms_sub_sample_counter = EMG_RMS_SUBSAMPLING_FACTOR; + filter->emg_rms_sub_sample_counter = emg_sub_sampling_rate; return 0; } diff --git a/lib/cionic/emg_iir.h b/lib/cionic/emg_iir.h index b48fd5e2e3d3a..9701f4ad657eb 100644 --- a/lib/cionic/emg_iir.h +++ b/lib/cionic/emg_iir.h @@ -12,8 +12,9 @@ #define EMG_RMS_FILTER_ORDER (6) #define EMG_RMS_HIGHPASS_FC (50) #define EMG_RMS_LOWPASS_FC (199) -#define EMG_RMS_SUBSAMPLING_FACTOR (5) -#define EMG_RMS_MA_SIZE (300/(EMG_RMS_SUBSAMPLING_FACTOR)) +// #define EMG_RMS_SUBSAMPLING_FACTOR (5) +// #define EMG_RMS_MA_SIZE (300/(EMG_RMS_SUBSAMPLING_FACTOR)) +#define EMG_RMS_MA_SIZE (256) // maximum size #define EMG_RMS_MA_DC_BLOCK_FC (0.001) #define EMG_RMS_MA_DC_BLOCK_ALPHA (2*M_PI*EMG_RMS_MA_DC_BLOCK_FC/EMG_RMS_FS) @@ -35,7 +36,7 @@ typedef struct emg_mwa_state { } emg_mwa_state_t; typedef struct iir_filter_t { - int emg_rms_sub_sample_counter; + uint8_t emg_rms_sub_sample_counter; emg_filter_state_t emg_lowpass_iir_state[IIR_NUM_CHANNELS]; emg_filter_state_t emg_highpass_iir_state[IIR_NUM_CHANNELS]; emg_mwa_state_t emg_mwa_state[IIR_NUM_CHANNELS]; @@ -51,5 +52,9 @@ int iir_filter_process(iir_filter_t *filter, float *norms, int numchans, elapsed_t ts_in, const uint8_t *buffer, elapsed_t *ts_out, float *uv_out); +// generated coefficients +extern float emg_highpass_filter_sos[NO_OF_BQS][NO_OF_COEFFS_PER_BQ]; +extern float emg_lowpass_filter_sos[NO_OF_BQS][NO_OF_COEFFS_PER_BQ]; +extern uint8_t emg_sub_sampling_rate; #endif //_EMG_IIR_H_ diff --git a/shared-bindings/ads1x9x/ADS1x9x.c b/shared-bindings/ads1x9x/ADS1x9x.c index 3206e2e4ba071..d72edd5e6e09e 100644 --- a/shared-bindings/ads1x9x/ADS1x9x.c +++ b/shared-bindings/ads1x9x/ADS1x9x.c @@ -217,10 +217,28 @@ STATIC mp_obj_t ads1x9x_ads1x9x_deinit(mp_obj_t self_in) { } MP_DEFINE_CONST_FUN_OBJ_1(ads1x9x_ads1x9x_deinit_obj, ads1x9x_ads1x9x_deinit); +STATIC mp_obj_t ads1x9x_ads1x9x_set_emg_filter(mp_obj_t self_in, mp_obj_t coeffs_list, mp_obj_t low) { + ads1x9x_ADS1x9x_obj_t *self = (ads1x9x_ADS1x9x_obj_t *) self_in; + set_emg_filter(self, coeffs_list, low); + return mp_const_none; +} + +MP_DEFINE_CONST_FUN_OBJ_3(ads1x9x_ads1x9x_set_emg_filter_obj, ads1x9x_ads1x9x_set_emg_filter); + +STATIC mp_obj_t ads1x9x_ads1x9x_set_emg_decim_rate(mp_obj_t self_in, mp_obj_t decim_rate) { + ads1x9x_ADS1x9x_obj_t *self = (ads1x9x_ADS1x9x_obj_t *) self_in; + set_emg_decim_rate(self, decim_rate); + return mp_const_none; +} + +MP_DEFINE_CONST_FUN_OBJ_2(ads1x9x_ads1x9x_set_emg_decim_rate_obj, ads1x9x_ads1x9x_set_emg_decim_rate); + STATIC const mp_rom_map_elem_t ads1x9x_ads1x9x_locals_dict_table[] = { { MP_ROM_QSTR(MP_QSTR_reset), MP_ROM_PTR(&ads1x9x_ads1x9x_reset_obj) }, { MP_ROM_QSTR(MP_QSTR_sample_size_get), MP_ROM_PTR(&ads1x9x_ads1x9x_sample_size_get_obj) }, { MP_ROM_QSTR(MP_QSTR_filter_set), MP_ROM_PTR(&ads1x9x_ads1x9x_filter_set_obj) }, + { MP_ROM_QSTR(MP_QSTR_set_emg_filter), MP_ROM_PTR(&ads1x9x_ads1x9x_set_emg_filter_obj) }, + { MP_ROM_QSTR(MP_QSTR_set_emg_decim_rate), MP_ROM_PTR(&ads1x9x_ads1x9x_set_emg_decim_rate_obj) }, { MP_ROM_QSTR(MP_QSTR_read_reg), MP_ROM_PTR(&ads1x9x_ads1x9x_read_reg_obj) }, { MP_ROM_QSTR(MP_QSTR_write_reg), MP_ROM_PTR(&ads1x9x_ads1x9x_write_reg_obj) }, { MP_ROM_QSTR(MP_QSTR_start), MP_ROM_PTR(&ads1x9x_ads1x9x_start_obj) }, diff --git a/shared-module/ads1x9x/ADS1x9x.c b/shared-module/ads1x9x/ADS1x9x.c index d2c1441299971..8772e3b37a3cb 100644 --- a/shared-module/ads1x9x/ADS1x9x.c +++ b/shared-module/ads1x9x/ADS1x9x.c @@ -33,6 +33,7 @@ #include "shared-bindings/time/__init__.h" #include "shared-bindings/util.h" #include "shared-bindings/digitalio/DigitalInOut.h" +#include "lib/cionic/emg_iir.h" #include "py/mperrno.h" #include @@ -324,3 +325,38 @@ size_t common_hal_ads1x9x_ADS1x9x_read(ads1x9x_ADS1x9x_obj_t *self, mp_buffer_in memcpy(ptr, g_ads_buffer, buf_size); return buf_size; } + +void set_emg_filter(ads1x9x_ADS1x9x_obj_t *self, mp_obj_t coeffs_list, mp_obj_t low) +{ + + size_t array_size = 0; + mp_obj_t *values; + mp_obj_get_array(coeffs_list, &array_size, &values); + + + float (*emg_filter)[NO_OF_COEFFS_PER_BQ]; + + if (low == mp_const_true) { + emg_filter = emg_lowpass_filter_sos; + mp_printf(&mp_plat_print, "setting the low pass filter bank \n"); + } else { + emg_filter = emg_highpass_filter_sos; + mp_printf(&mp_plat_print, "setting the high pass filter bank \n"); + } + + for (int i = 0; i < NO_OF_BQS; i++) { + mp_printf(&mp_plat_print, "------------------------\n"); + mp_printf(&mp_plat_print, "bank %d:\n", i); + for (int j = 0; j < NO_OF_COEFFS_PER_BQ; j++) { + emg_filter[i][j] = mp_obj_get_float(values[i*NO_OF_COEFFS_PER_BQ+j]); + mp_printf(&mp_plat_print, "c[%d][%d]: %f;", i, j, (double) emg_filter[i][j]); + } + mp_printf(&mp_plat_print, "\n"); + } +} + +void set_emg_decim_rate(ads1x9x_ADS1x9x_obj_t *self, mp_obj_t decim_rate) +{ + emg_sub_sampling_rate = mp_obj_get_int(decim_rate); + mp_printf(&mp_plat_print, "emg decimation rate set to: %d\n", emg_sub_sampling_rate); +} diff --git a/shared-module/ads1x9x/ADS1x9x.h b/shared-module/ads1x9x/ADS1x9x.h index 7fcdbc7e93483..e630065d90f08 100644 --- a/shared-module/ads1x9x/ADS1x9x.h +++ b/shared-module/ads1x9x/ADS1x9x.h @@ -125,5 +125,7 @@ uint8_t common_hal_ads1x9x_ADS1x9x_read_reg(ads1x9x_ADS1x9x_obj_t *self, uint8_t void common_hal_ads1x9x_ADS1x9x_write_reg(ads1x9x_ADS1x9x_obj_t *self, uint8_t addr, uint8_t value); void common_hal_ads1x9x_ADS1x9x_read_data(ads1x9x_ADS1x9x_obj_t *self, uint8_t *data, uint16_t len); size_t common_hal_ads1x9x_ADS1x9x_read(ads1x9x_ADS1x9x_obj_t *self, mp_buffer_info_t *buf, uint16_t buf_size); +void set_emg_filter(ads1x9x_ADS1x9x_obj_t *self, mp_obj_t coeffs_list, mp_obj_t low); +void set_emg_decim_rate(ads1x9x_ADS1x9x_obj_t *self, mp_obj_t decim_rate); #endif // MICROPY_INCLUDED_SHARED_MODULE_ADS129X_ADS129X_H From 7259a328e8193be1c59e41362413faa1090d5be3 Mon Sep 17 00:00:00 2001 From: Narsi G Date: Sun, 5 Nov 2023 09:44:27 -0800 Subject: [PATCH 2/2] changed names of the globals to start with g_ made g_emg_sub_sampling_rate and g_ma_size static migrated set_emg functions to emg_iir.c --- lib/cionic/emg_iir.c | 49 ++++++++++++++++++++++++++----- lib/cionic/emg_iir.h | 3 +- shared-bindings/ads1x9x/ADS1x9x.c | 7 ++--- shared-module/ads1x9x/ADS1x9x.c | 37 +---------------------- shared-module/ads1x9x/ADS1x9x.h | 2 -- 5 files changed, 48 insertions(+), 50 deletions(-) diff --git a/lib/cionic/emg_iir.c b/lib/cionic/emg_iir.c index aea7bfc07a682..d18563e77c7a2 100644 --- a/lib/cionic/emg_iir.c +++ b/lib/cionic/emg_iir.c @@ -15,8 +15,8 @@ float emg_lowpass_filter_sos[NO_OF_BQS][NO_OF_COEFFS_PER_BQ] = { {1.0,2.0,1.0,1.0,-1.4083885078365557,0.7369080185138834}, }; -uint8_t emg_sub_sampling_rate = 1; -uint16_t ma_size = EMG_RMS_MA_SIZE; +static uint8_t g_emg_sub_sampling_rate = 1; +static uint16_t g_ma_size = EMG_RMS_MA_SIZE; // takes as input a bank of biquad coefficients ( NO_OF_BQS of them ) with the @@ -84,12 +84,12 @@ emg_mwa_rms(emg_mwa_state_t *state, float val) state->sum -= state->mw[state->write_ptr]; // update the delay line state->mw[state->write_ptr] = val*val; - state->write_ptr = (state->write_ptr+1) % ma_size; + state->write_ptr = (state->write_ptr+1) % g_ma_size; // the latest sum state->sum += val*val; double result = state->sum; - result = sqrt(result/ma_size); + result = sqrt(result/g_ma_size); #ifdef EMG_RMS_DC_BLOCKING result = result + state->dc; state->dc = state->dc - result*state->alpha; @@ -105,8 +105,8 @@ emg_mwa_rms(emg_mwa_state_t *state, float val) void iir_filter_init(iir_filter_t *filter) { - filter->emg_rms_sub_sample_counter = emg_sub_sampling_rate; - ma_size = EMG_RMS_MA_SIZE/emg_sub_sampling_rate; + filter->emg_rms_sub_sample_counter = g_emg_sub_sampling_rate; + g_ma_size = EMG_RMS_MA_SIZE/g_emg_sub_sampling_rate; for( int i=0; iemg_lowpass_iir_state[i]); emg_iir_init(&filter->emg_highpass_iir_state[i]); @@ -145,9 +145,44 @@ iir_filter_process(iir_filter_t *filter, float *norms, int numchans, if (filter->emg_rms_sub_sample_counter <= 0) { *ts_out = ts_in; - filter->emg_rms_sub_sample_counter = emg_sub_sampling_rate; + filter->emg_rms_sub_sample_counter = g_emg_sub_sampling_rate; return 0; } return -1; } + +void set_emg_filter(mp_obj_t coeffs_list, mp_obj_t low) +{ + + size_t array_size = 0; + mp_obj_t *values; + mp_obj_get_array(coeffs_list, &array_size, &values); + + + float (*emg_filter)[NO_OF_COEFFS_PER_BQ]; + + if (low == mp_const_true) { + emg_filter = emg_lowpass_filter_sos; + mp_printf(&mp_plat_print, "setting the low pass filter bank \n"); + } else { + emg_filter = emg_highpass_filter_sos; + mp_printf(&mp_plat_print, "setting the high pass filter bank \n"); + } + + for (int i = 0; i < NO_OF_BQS; i++) { + mp_printf(&mp_plat_print, "------------------------\n"); + mp_printf(&mp_plat_print, "bank %d:\n", i); + for (int j = 0; j < NO_OF_COEFFS_PER_BQ; j++) { + emg_filter[i][j] = mp_obj_get_float(values[i*NO_OF_COEFFS_PER_BQ+j]); + mp_printf(&mp_plat_print, "c[%d][%d]: %f;", i, j, (double) emg_filter[i][j]); + } + mp_printf(&mp_plat_print, "\n"); + } +} + +void set_emg_decim_rate(mp_obj_t decim_rate) +{ + g_emg_sub_sampling_rate = mp_obj_get_int(decim_rate); + mp_printf(&mp_plat_print, "emg decimation rate set to: %d\n", g_emg_sub_sampling_rate); +} diff --git a/lib/cionic/emg_iir.h b/lib/cionic/emg_iir.h index 9701f4ad657eb..24ca8fc704024 100644 --- a/lib/cionic/emg_iir.h +++ b/lib/cionic/emg_iir.h @@ -55,6 +55,7 @@ int iir_filter_process(iir_filter_t *filter, float *norms, int numchans, // generated coefficients extern float emg_highpass_filter_sos[NO_OF_BQS][NO_OF_COEFFS_PER_BQ]; extern float emg_lowpass_filter_sos[NO_OF_BQS][NO_OF_COEFFS_PER_BQ]; -extern uint8_t emg_sub_sampling_rate; +void set_emg_filter(mp_obj_t coeffs_list, mp_obj_t low); +void set_emg_decim_rate(mp_obj_t decim_rate); #endif //_EMG_IIR_H_ diff --git a/shared-bindings/ads1x9x/ADS1x9x.c b/shared-bindings/ads1x9x/ADS1x9x.c index d72edd5e6e09e..5191d979395f3 100644 --- a/shared-bindings/ads1x9x/ADS1x9x.c +++ b/shared-bindings/ads1x9x/ADS1x9x.c @@ -36,6 +36,7 @@ #include "shared-bindings/busio/SPI.h" #include "shared-bindings/microcontroller/Pin.h" #include "supervisor/flash.h" +#include "lib/cionic/emg_iir.h" //| class ADS1x9x: //| """ADS1x9x Interface @@ -218,16 +219,14 @@ STATIC mp_obj_t ads1x9x_ads1x9x_deinit(mp_obj_t self_in) { MP_DEFINE_CONST_FUN_OBJ_1(ads1x9x_ads1x9x_deinit_obj, ads1x9x_ads1x9x_deinit); STATIC mp_obj_t ads1x9x_ads1x9x_set_emg_filter(mp_obj_t self_in, mp_obj_t coeffs_list, mp_obj_t low) { - ads1x9x_ADS1x9x_obj_t *self = (ads1x9x_ADS1x9x_obj_t *) self_in; - set_emg_filter(self, coeffs_list, low); + set_emg_filter(coeffs_list, low); return mp_const_none; } MP_DEFINE_CONST_FUN_OBJ_3(ads1x9x_ads1x9x_set_emg_filter_obj, ads1x9x_ads1x9x_set_emg_filter); STATIC mp_obj_t ads1x9x_ads1x9x_set_emg_decim_rate(mp_obj_t self_in, mp_obj_t decim_rate) { - ads1x9x_ADS1x9x_obj_t *self = (ads1x9x_ADS1x9x_obj_t *) self_in; - set_emg_decim_rate(self, decim_rate); + set_emg_decim_rate(decim_rate); return mp_const_none; } diff --git a/shared-module/ads1x9x/ADS1x9x.c b/shared-module/ads1x9x/ADS1x9x.c index 8772e3b37a3cb..27fa49e2dbe6c 100644 --- a/shared-module/ads1x9x/ADS1x9x.c +++ b/shared-module/ads1x9x/ADS1x9x.c @@ -33,7 +33,6 @@ #include "shared-bindings/time/__init__.h" #include "shared-bindings/util.h" #include "shared-bindings/digitalio/DigitalInOut.h" -#include "lib/cionic/emg_iir.h" #include "py/mperrno.h" #include @@ -104,6 +103,7 @@ STATIC void ads129x_raw(ads1x9x_ADS1x9x_obj_t *self, uint8_t *in, float *out) int16_t ads_sample; for(i = 0 ; i < ADS1X9X_NUM_CHAN ; i++){ + // int16_t, not int24_t? ads_sample = READ_BE(int16_t, in); out[i] = (float)ads_sample * self->all_norms[i]; in += 2; @@ -325,38 +325,3 @@ size_t common_hal_ads1x9x_ADS1x9x_read(ads1x9x_ADS1x9x_obj_t *self, mp_buffer_in memcpy(ptr, g_ads_buffer, buf_size); return buf_size; } - -void set_emg_filter(ads1x9x_ADS1x9x_obj_t *self, mp_obj_t coeffs_list, mp_obj_t low) -{ - - size_t array_size = 0; - mp_obj_t *values; - mp_obj_get_array(coeffs_list, &array_size, &values); - - - float (*emg_filter)[NO_OF_COEFFS_PER_BQ]; - - if (low == mp_const_true) { - emg_filter = emg_lowpass_filter_sos; - mp_printf(&mp_plat_print, "setting the low pass filter bank \n"); - } else { - emg_filter = emg_highpass_filter_sos; - mp_printf(&mp_plat_print, "setting the high pass filter bank \n"); - } - - for (int i = 0; i < NO_OF_BQS; i++) { - mp_printf(&mp_plat_print, "------------------------\n"); - mp_printf(&mp_plat_print, "bank %d:\n", i); - for (int j = 0; j < NO_OF_COEFFS_PER_BQ; j++) { - emg_filter[i][j] = mp_obj_get_float(values[i*NO_OF_COEFFS_PER_BQ+j]); - mp_printf(&mp_plat_print, "c[%d][%d]: %f;", i, j, (double) emg_filter[i][j]); - } - mp_printf(&mp_plat_print, "\n"); - } -} - -void set_emg_decim_rate(ads1x9x_ADS1x9x_obj_t *self, mp_obj_t decim_rate) -{ - emg_sub_sampling_rate = mp_obj_get_int(decim_rate); - mp_printf(&mp_plat_print, "emg decimation rate set to: %d\n", emg_sub_sampling_rate); -} diff --git a/shared-module/ads1x9x/ADS1x9x.h b/shared-module/ads1x9x/ADS1x9x.h index e630065d90f08..7fcdbc7e93483 100644 --- a/shared-module/ads1x9x/ADS1x9x.h +++ b/shared-module/ads1x9x/ADS1x9x.h @@ -125,7 +125,5 @@ uint8_t common_hal_ads1x9x_ADS1x9x_read_reg(ads1x9x_ADS1x9x_obj_t *self, uint8_t void common_hal_ads1x9x_ADS1x9x_write_reg(ads1x9x_ADS1x9x_obj_t *self, uint8_t addr, uint8_t value); void common_hal_ads1x9x_ADS1x9x_read_data(ads1x9x_ADS1x9x_obj_t *self, uint8_t *data, uint16_t len); size_t common_hal_ads1x9x_ADS1x9x_read(ads1x9x_ADS1x9x_obj_t *self, mp_buffer_info_t *buf, uint16_t buf_size); -void set_emg_filter(ads1x9x_ADS1x9x_obj_t *self, mp_obj_t coeffs_list, mp_obj_t low); -void set_emg_decim_rate(ads1x9x_ADS1x9x_obj_t *self, mp_obj_t decim_rate); #endif // MICROPY_INCLUDED_SHARED_MODULE_ADS129X_ADS129X_H