diff --git a/firmware/code/configuration_manager.c b/firmware/code/configuration_manager.c index b033b95..7c4fc74 100644 --- a/firmware/code/configuration_manager.c +++ b/firmware/code/configuration_manager.c @@ -103,10 +103,8 @@ bool validate_filter_configuration(filter_configuration_tlv *filters) case BANDPASSPEAK: case NOTCH: case ALLPASS: { - //filter2 *args = (filter2 *)ptr; - //printf("Found Filter %d: %0.2f %0.2f\n", args->type, args->f0, args->Q); if (remaining < sizeof(filter2)) { - printf("Error! Not enough data left for filter2 (%d)..\n", remaining); + printf("Error! Not enough data left for filter2 (%d)\n", remaining); return false; } ptr += sizeof(filter2); @@ -115,15 +113,27 @@ bool validate_filter_configuration(filter_configuration_tlv *filters) case PEAKING: case LOWSHELF: case HIGHSHELF: { - //filter3 *args = (filter3 *)ptr; - //printf("Found Filter %d: %0.2f %0.2f %0.2f\n", args->type, args->f0, args->db_gain, args->Q); if (remaining < sizeof(filter3)) { - printf("Error! Not enough data left for filter3 (%d)..\n", remaining); + printf("Error! Not enough data left for filter3 (%d)\n", remaining); return false; } ptr += sizeof(filter3); break; } + case CUSTOMIIR: { + filter6 *args = (filter6 *)ptr; + if (remaining < sizeof(filter6)) { + printf("Error! Not enough data left for filter6 (%d)\n", remaining); + return false; + } + if (args->a0 == 0.0) { + printf("Error! The a0 co-efficient of an IIR filter must not be 0.\n"); + return false; + } + ptr += sizeof(filter6); + break; + } + default: printf("Unknown filter type\n"); return false; @@ -164,26 +174,42 @@ void apply_filter_configuration(filter_configuration_tlv *filters) { case PEAKING: INIT_FILTER3(peaking); case LOWSHELF: INIT_FILTER3(lowshelf); case HIGHSHELF: INIT_FILTER3(highshelf); + case CUSTOMIIR: { + filter6 *args = (filter6 *)ptr; + uint32_t checksum = 0; + for (int i = 0; i < sizeof(filter6) / 4; i++) checksum ^= ((uint32_t*) args)[i]; + if (checksum != bqf_filter_checksum[filter_stages]) { + bqf_filters_left[filter_stages].a0 = fix16_from_dbl(1.0); + bqf_filters_left[filter_stages].a1 = fix16_from_dbl(args->a1/args->a0); + bqf_filters_left[filter_stages].a2 = fix16_from_dbl(args->a2/args->a0); + bqf_filters_left[filter_stages].b0 = fix16_from_dbl(args->b0/args->a0); + bqf_filters_left[filter_stages].b1 = fix16_from_dbl(args->b1/args->a0); + bqf_filters_left[filter_stages].b2 = fix16_from_dbl(args->b2/args->a0); + memcpy(&bqf_filters_right[filter_stages], &bqf_filters_left[filter_stages], sizeof(bqf_coeff_t)); + bqf_filter_checksum[filter_stages] = checksum; + } + ptr += sizeof(filter6); + type_changed = true; // Always flush our memory + break; + } default: break; } - filter_stages++; - } + if (type_changed) { + // The memory structure stores the last 2 input samples, we can replay them into + // the new filter rather than starting again from scratch. + fix16_t left[2] = { bqf_filters_mem_left[filter_stages].x_2, bqf_filters_mem_left[filter_stages].x_1 }; + fix16_t right[2] = { bqf_filters_mem_right[filter_stages].x_2, bqf_filters_mem_right[filter_stages].x_1 }; - if (type_changed) { - // The memory structure stores the last 2 input samples, we can replay them into - // the new filter rather than starting again from scratch. - fix16_t left[2] = { bqf_filters_mem_left[0].x_2, bqf_filters_mem_left[0].x_1 }; - fix16_t right[2] = { bqf_filters_mem_right[0].x_2, bqf_filters_mem_right[0].x_1 }; - for (int i=0; i #define FLASH_MAGIC 0x2E8AFEDD -#define CONFIG_VERSION 1 +#define CONFIG_VERSION 2 #define MINIMUM_CONFIG_VERSION 1 enum structure_types { @@ -65,6 +65,17 @@ typedef struct __attribute__((__packed__)) _filter3 { double Q; } filter3; +typedef struct __attribute__((__packed__)) _filter6 { + uint8_t type; + uint8_t reserved[3]; + double a0; + double a1; + double a2; + double b0; + double b1; + double b2; +} filter6; + enum filter_type { LOWPASS = 0, HIGHPASS, @@ -74,7 +85,8 @@ enum filter_type { ALLPASS, PEAKING, LOWSHELF, - HIGHSHELF + HIGHSHELF, + CUSTOMIIR }; typedef struct __attribute__((__packed__)) _flash_header_tlv {