diff --git a/lib/cionic/emg_iir.c b/lib/cionic/emg_iir.c index 2cbdcb33d357d..d18563e77c7a2 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}, }; +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 // 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) % g_ma_size; // the latest sum state->sum += val*val; double result = state->sum; - result = sqrt(result/EMG_RMS_MA_SIZE); + result = sqrt(result/g_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 = 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]); @@ -140,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_RMS_SUBSAMPLING_FACTOR; + 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 b48fd5e2e3d3a..24ca8fc704024 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,10 @@ 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]; +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 3206e2e4ba071..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 @@ -217,10 +218,26 @@ 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) { + 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) { + set_emg_decim_rate(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..27fa49e2dbe6c 100644 --- a/shared-module/ads1x9x/ADS1x9x.c +++ b/shared-module/ads1x9x/ADS1x9x.c @@ -103,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;