From 481219341dcec5ae4c12629b0391a7ac1bf102e1 Mon Sep 17 00:00:00 2001 From: ploopyco Date: Fri, 14 Jul 2023 15:06:20 -0400 Subject: [PATCH] q3.28 changes --- firmware/code/bqf.c | 112 +++++++++++++------------- firmware/code/bqf.h | 22 ++--- firmware/code/configuration_manager.c | 20 ++--- firmware/code/fix16.c | 55 +++++++++---- firmware/code/fix16.h | 37 +++++---- firmware/code/run.c | 12 +-- firmware/code/run.h | 2 +- 7 files changed, 145 insertions(+), 115 deletions(-) diff --git a/firmware/code/bqf.c b/firmware/code/bqf.c index 6e0b8e0..21b67fb 100644 --- a/firmware/code/bqf.c +++ b/firmware/code/bqf.c @@ -73,12 +73,12 @@ void bqf_lowpass_config(double fs, double f0, double Q, bqf_coeff_t *coefficient a2 = a2 / a0; a0 = 1.0; - coefficients->b0 = fix16_from_dbl(b0); - coefficients->b1 = fix16_from_dbl(b1); - coefficients->b2 = fix16_from_dbl(b2); - coefficients->a0 = fix16_from_dbl(a0); - coefficients->a1 = fix16_from_dbl(a1); - coefficients->a2 = fix16_from_dbl(a2); + coefficients->b0 = fix3_28_from_dbl(b0); + coefficients->b1 = fix3_28_from_dbl(b1); + coefficients->b2 = fix3_28_from_dbl(b2); + coefficients->a0 = fix3_28_from_dbl(a0); + coefficients->a1 = fix3_28_from_dbl(a1); + coefficients->a2 = fix3_28_from_dbl(a2); } /** @@ -123,12 +123,12 @@ void bqf_highpass_config(double fs, double f0, double Q, bqf_coeff_t *coefficien a2 = a2 / a0; a0 = 1.0; - coefficients->b0 = fix16_from_dbl(b0); - coefficients->b1 = fix16_from_dbl(b1); - coefficients->b2 = fix16_from_dbl(b2); - coefficients->a0 = fix16_from_dbl(a0); - coefficients->a1 = fix16_from_dbl(a1); - coefficients->a2 = fix16_from_dbl(a2); + coefficients->b0 = fix3_28_from_dbl(b0); + coefficients->b1 = fix3_28_from_dbl(b1); + coefficients->b2 = fix3_28_from_dbl(b2); + coefficients->a0 = fix3_28_from_dbl(a0); + coefficients->a1 = fix3_28_from_dbl(a1); + coefficients->a2 = fix3_28_from_dbl(a2); } /** @@ -168,12 +168,12 @@ void bqf_bandpass_skirt_config(double fs, double f0, double Q, bqf_coeff_t *coef a2 = a2 / a0; a0 = 1.0; - coefficients->b0 = fix16_from_dbl(b0); - coefficients->b1 = fix16_from_dbl(b1); - coefficients->b2 = fix16_from_dbl(b2); - coefficients->a0 = fix16_from_dbl(a0); - coefficients->a1 = fix16_from_dbl(a1); - coefficients->a2 = fix16_from_dbl(a2); + coefficients->b0 = fix3_28_from_dbl(b0); + coefficients->b1 = fix3_28_from_dbl(b1); + coefficients->b2 = fix3_28_from_dbl(b2); + coefficients->a0 = fix3_28_from_dbl(a0); + coefficients->a1 = fix3_28_from_dbl(a1); + coefficients->a2 = fix3_28_from_dbl(a2); } /** @@ -213,12 +213,12 @@ void bqf_bandpass_peak_config(double fs, double f0, double Q, bqf_coeff_t *coeff a2 = a2 / a0; a0 = 1.0; - coefficients->b0 = fix16_from_dbl(b0); - coefficients->b1 = fix16_from_dbl(b1); - coefficients->b2 = fix16_from_dbl(b2); - coefficients->a0 = fix16_from_dbl(a0); - coefficients->a1 = fix16_from_dbl(a1); - coefficients->a2 = fix16_from_dbl(a2); + coefficients->b0 = fix3_28_from_dbl(b0); + coefficients->b1 = fix3_28_from_dbl(b1); + coefficients->b2 = fix3_28_from_dbl(b2); + coefficients->a0 = fix3_28_from_dbl(a0); + coefficients->a1 = fix3_28_from_dbl(a1); + coefficients->a2 = fix3_28_from_dbl(a2); } /** @@ -257,12 +257,12 @@ void bqf_notch_config(double fs, double f0, double Q, bqf_coeff_t *coefficients) a2 = a2 / a0; a0 = 1.0; - coefficients->b0 = fix16_from_dbl(b0); - coefficients->b1 = fix16_from_dbl(b1); - coefficients->b2 = fix16_from_dbl(b2); - coefficients->a0 = fix16_from_dbl(a0); - coefficients->a1 = fix16_from_dbl(a1); - coefficients->a2 = fix16_from_dbl(a2); + coefficients->b0 = fix3_28_from_dbl(b0); + coefficients->b1 = fix3_28_from_dbl(b1); + coefficients->b2 = fix3_28_from_dbl(b2); + coefficients->a0 = fix3_28_from_dbl(a0); + coefficients->a1 = fix3_28_from_dbl(a1); + coefficients->a2 = fix3_28_from_dbl(a2); } /** @@ -300,12 +300,12 @@ void bqf_allpass_config(double fs, double f0, double Q, bqf_coeff_t *coefficient a2 = a2 / a0; a0 = 1.0; - coefficients->b0 = fix16_from_dbl(b0); - coefficients->b1 = fix16_from_dbl(b1); - coefficients->b2 = fix16_from_dbl(b2); - coefficients->a0 = fix16_from_dbl(a0); - coefficients->a1 = fix16_from_dbl(a1); - coefficients->a2 = fix16_from_dbl(a2); + coefficients->b0 = fix3_28_from_dbl(b0); + coefficients->b1 = fix3_28_from_dbl(b1); + coefficients->b2 = fix3_28_from_dbl(b2); + coefficients->a0 = fix3_28_from_dbl(a0); + coefficients->a1 = fix3_28_from_dbl(a1); + coefficients->a2 = fix3_28_from_dbl(a2); } /** @@ -353,12 +353,12 @@ void bqf_peaking_config(double fs, double f0, double dBgain, double Q, a2 = a2 / a0; a0 = 1.0; - coefficients->b0 = fix16_from_dbl(b0); - coefficients->b1 = fix16_from_dbl(b1); - coefficients->b2 = fix16_from_dbl(b2); - coefficients->a0 = fix16_from_dbl(a0); - coefficients->a1 = fix16_from_dbl(a1); - coefficients->a2 = fix16_from_dbl(a2); + coefficients->b0 = fix3_28_from_dbl(b0); + coefficients->b1 = fix3_28_from_dbl(b1); + coefficients->b2 = fix3_28_from_dbl(b2); + coefficients->a0 = fix3_28_from_dbl(a0); + coefficients->a1 = fix3_28_from_dbl(a1); + coefficients->a2 = fix3_28_from_dbl(a2); } /** @@ -406,12 +406,12 @@ void bqf_lowshelf_config(double fs, double f0, double dBgain, double Q, a2 = a2 / a0; a0 = 1.0; - coefficients->b0 = fix16_from_dbl(b0); - coefficients->b1 = fix16_from_dbl(b1); - coefficients->b2 = fix16_from_dbl(b2); - coefficients->a0 = fix16_from_dbl(a0); - coefficients->a1 = fix16_from_dbl(a1); - coefficients->a2 = fix16_from_dbl(a2); + coefficients->b0 = fix3_28_from_dbl(b0); + coefficients->b1 = fix3_28_from_dbl(b1); + coefficients->b2 = fix3_28_from_dbl(b2); + coefficients->a0 = fix3_28_from_dbl(a0); + coefficients->a1 = fix3_28_from_dbl(a1); + coefficients->a2 = fix3_28_from_dbl(a2); } /** @@ -459,16 +459,16 @@ void bqf_highshelf_config(double fs, double f0, double dBgain, double Q, a2 = a2 / a0; a0 = 1.0; - coefficients->b0 = fix16_from_dbl(b0); - coefficients->b1 = fix16_from_dbl(b1); - coefficients->b2 = fix16_from_dbl(b2); - coefficients->a0 = fix16_from_dbl(a0); - coefficients->a1 = fix16_from_dbl(a1); - coefficients->a2 = fix16_from_dbl(a2); + coefficients->b0 = fix3_28_from_dbl(b0); + coefficients->b1 = fix3_28_from_dbl(b1); + coefficients->b2 = fix3_28_from_dbl(b2); + coefficients->a0 = fix3_28_from_dbl(a0); + coefficients->a1 = fix3_28_from_dbl(a1); + coefficients->a2 = fix3_28_from_dbl(a2); } -fix16_t bqf_transform(fix16_t x, bqf_coeff_t *coefficients, bqf_mem_t *memory) { - fix16_t y = fix16_mul(coefficients->b0, x) - +fix3_28_t bqf_transform(fix3_28_t x, bqf_coeff_t *coefficients, bqf_mem_t *memory) { + fix3_28_t y = fix16_mul(coefficients->b0, x) - fix16_mul(coefficients->a1, memory->y_1) + fix16_mul(coefficients->b1, memory->x_1) - fix16_mul(coefficients->a2, memory->y_2) + diff --git a/firmware/code/bqf.h b/firmware/code/bqf.h index b582de6..dcdc038 100644 --- a/firmware/code/bqf.h +++ b/firmware/code/bqf.h @@ -26,19 +26,19 @@ #include "fix16.h" typedef struct _bqf_coeff_t { - fix16_t a0; - fix16_t a1; - fix16_t a2; - fix16_t b0; - fix16_t b1; - fix16_t b2; + fix3_28_t a0; + fix3_28_t a1; + fix3_28_t a2; + fix3_28_t b0; + fix3_28_t b1; + fix3_28_t b2; } bqf_coeff_t; typedef struct _bqf_mem_t { - fix16_t x_1; - fix16_t x_2; - fix16_t y_1; - fix16_t y_2; + fix3_28_t x_1; + fix3_28_t x_2; + fix3_28_t y_1; + fix3_28_t y_2; } bqf_mem_t; // In reality we do not have enough CPU resource to run 8 filtering @@ -65,7 +65,7 @@ void bqf_peaking_config(double, double, double, double, bqf_coeff_t *); void bqf_lowshelf_config(double, double, double, double, bqf_coeff_t *); void bqf_highshelf_config(double, double, double, double, bqf_coeff_t *); -fix16_t bqf_transform(fix16_t, bqf_coeff_t *, bqf_mem_t *); +fix3_28_t bqf_transform(fix3_28_t, bqf_coeff_t *, bqf_mem_t *); void bqf_memreset(bqf_mem_t *); #endif diff --git a/firmware/code/configuration_manager.c b/firmware/code/configuration_manager.c index 7c4fc74..85bf537 100644 --- a/firmware/code/configuration_manager.c +++ b/firmware/code/configuration_manager.c @@ -179,12 +179,12 @@ void apply_filter_configuration(filter_configuration_tlv *filters) { 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); + bqf_filters_left[filter_stages].a0 = fix3_28_from_dbl(1.0); + bqf_filters_left[filter_stages].a1 = fix3_28_from_dbl(args->a1/args->a0); + bqf_filters_left[filter_stages].a2 = fix3_28_from_dbl(args->a2/args->a0); + bqf_filters_left[filter_stages].b0 = fix3_28_from_dbl(args->b0/args->a0); + bqf_filters_left[filter_stages].b1 = fix3_28_from_dbl(args->b1/args->a0); + bqf_filters_left[filter_stages].b2 = fix3_28_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; } @@ -198,8 +198,8 @@ void apply_filter_configuration(filter_configuration_tlv *filters) { 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 }; + fix3_28_t left[2] = { bqf_filters_mem_left[filter_stages].x_2, bqf_filters_mem_left[filter_stages].x_1 }; + fix3_28_t right[2] = { bqf_filters_mem_right[filter_stages].x_2, bqf_filters_mem_right[filter_stages].x_1 }; bqf_memreset(&bqf_filters_mem_left[filter_stages]); bqf_memreset(&bqf_filters_mem_right[filter_stages]); @@ -305,7 +305,7 @@ bool apply_configuration(tlv_header *config) { #ifndef TEST_TARGET case PREPROCESSING_CONFIGURATION: { preprocessing_configuration_tlv* preprocessing_config = (preprocessing_configuration_tlv*) tlv; - preprocessing.preamp = fix16_from_dbl(1.0 + preprocessing_config->preamp); + preprocessing.preamp = fix3_28_from_dbl(1.0 + preprocessing_config->preamp); preprocessing.reverse_stereo = preprocessing_config->reverse_stereo; break; } @@ -546,4 +546,4 @@ void apply_config_changes() { apply_configuration((tlv_header*) working_configuration[active_configuration]); } } -#endif \ No newline at end of file +#endif diff --git a/firmware/code/fix16.c b/firmware/code/fix16.c index 50f05d9..1695ed6 100644 --- a/firmware/code/fix16.c +++ b/firmware/code/fix16.c @@ -60,17 +60,36 @@ fix16_t fix16_mul(fix16_t inArg0, fix16_t inArg1) { return inArg0 * inArg1; } #else -fix16_t fix16_from_s16sample(int16_t a) { - return a * fix16_lsb; + +/// @brief Produces a fixed point number from a 16-bit signed integer, normalized to ]-1,1[. +/// @param a Signed 16-bit integer. +/// @return A fixed point number in Q3.28 format, with input normalized to ]-1,1[. +fix3_28_t norm_fix3_28_from_s16sample(int16_t a) { + /* So, we're using a Q3.28 fixed point system here, and we want the incoming + audio signal to be represented as a number between -1 and 1. To do this, + we need the 16-bit value to map to the 28-bit right-of-decimal field in + our fixed point number. 28-16 = 12, so we shift the incoming value by + that much to covert it to the desired Q3.28 format and do the normalization + all in one go. + */ + return (fix3_28_t)a << 12; } -int16_t fix16_to_s16sample(fix16_t a) { +/// @brief Convert fixed point samples into signed integer. Used to convert +/// calculated sample to one that the DAC can understand. +/// @param a +/// @return Signed 16-bit integer. +int16_t norm_fix3_28_to_s16sample(fix3_28_t a) { // Handle rounding up front, adding one can cause an overflow/underflow + + // It's not clear exactly how this works, so we'll disable it for now. + /* if (a < 0) { a -= (fix16_lsb >> 1); } else { a += (fix16_lsb >> 1); } + */ // Saturate the value if an overflow has occurred uint32_t upper = (a >> 30); @@ -85,25 +104,31 @@ int16_t fix16_to_s16sample(fix16_t a) { return SHRT_MAX; } } - return (a >> 15); + /* When we converted the USB audio sample to a fixed point number, we applied + a normalization, or a gain of 1/65536. To convert it back, we can undo that + by shifting it back by the same amount we shifted it in the first place. */ + return (a >> 12); } -fix16_t fix16_from_dbl(double a) { + +fix3_28_t fix3_28_from_dbl(double a) { double temp = a * fix16_one; temp += (double)((temp >= 0) ? 0.5f : -0.5f); - return (fix16_t)temp; + return (fix3_28_t)temp; } -double fix16_to_dbl(fix16_t a) { - return (double)a / fix16_one; -} - -// We work in 64bits then shift the result to get -// the bit representing 1 back into the correct position. -// i.e. 1*1 == 1, so 20000000^2 >> 25 = 20000000 -fix16_t fix16_mul(fix16_t inArg0, fix16_t inArg1) { +/// @brief Multiplies two fixed point numbers in Q3.28 format together. +/// @param inArg0 Q3.28 format fixed point number. +/// @param inArg1 Q3.28 format fixed point number. +/// @return A Q3.28 fixed point number that represents the truncated result of inArg0 x inArg1. +fix3_28_t fix16_mul(fix3_28_t inArg0, fix3_28_t inArg1) { const int64_t product = (int64_t)inArg0 * inArg1; - fix16_t result = product >> 25; + + /* Since we're expecting 2 Q3.28 numbers, the multiplication result should be a Q7.56 number. + To bring this number back to the right order of magnitude, we need to shift + it to the right by 28. */ + fix3_28_t result = product >> 28; + // Handle rounding where we are choppping off low order bits // Disabled for now, too much load. We get crackling when adjusting // the volume. diff --git a/firmware/code/fix16.h b/firmware/code/fix16.h index 42b0c03..40acced 100644 --- a/firmware/code/fix16.h +++ b/firmware/code/fix16.h @@ -32,24 +32,29 @@ typedef double fix16_t; static const fix16_t fix16_zero = 0; static const fix16_t fix16_one = 1; #else -// We normalize all values into the range -32..32 with 1 extra bit for overflows -// and one bit for the sign. We allow fixed point values to overflow, but they -// are clipped at the point they are written back to a s16sample. -// -// The reason for normalizing the samples is because the filter coefficients are -// small (usually well within in the range -32..32), by normalizing everything the coefficients -// get lots of additional bits of precision. -typedef int32_t fix16_t; -static const fix16_t fix16_lsb = 0x8000; -static const fix16_t fix16_one = 0x002000000; -static const fix16_t fix16_zero = 0x00000000; + +/// @brief Fixed point math type, in format Q3.28. One sign bit, 3 bits for left-of-decimal +///and 28 for right-of-decimal. This arrangment works because we normalize the incoming USB +///audio data to ]-1,1[ before operating on it, to push as much of the precision in the signal +///to the right of the decimal as possible. +typedef int32_t fix3_28_t; + +/// @brief Represents the number 1 in Q3.28. There are 3 bits and a sign bit to the left of the +///decimal, so 1.0000 would be represented as 0b 0001 followed by 28 zeros. +static const fix3_28_t fix16_one = 0x10000000; + +/// @brief Represents zero in fixed point world. +static const fix3_28_t fix16_zero = 0x00000000; + #endif -fix16_t fix16_from_s16sample(int16_t); -int16_t fix16_to_s16sample(fix16_t); -fix16_t fix16_from_dbl(double); -double fix16_to_dbl(fix16_t); -fix16_t fix16_mul(fix16_t, fix16_t); +fix3_28_t norm_fix3_28_from_s16sample(int16_t); + +int16_t norm_fix3_28_to_s16sample(fix3_28_t); + +fix3_28_t fix3_28_from_dbl(double); + +fix3_28_t fix16_mul(fix3_28_t, fix3_28_t); #endif \ No newline at end of file diff --git a/firmware/code/run.c b/firmware/code/run.c index a5778d2..2f019a6 100644 --- a/firmware/code/run.c +++ b/firmware/code/run.c @@ -143,12 +143,12 @@ static void _as_audio_packet(struct usb_endpoint *ep) { for (int j = 0; j < filter_stages; j++) { // Left channel filter for (int i = 0; i < samples; i += 2) { - fix16_t x_f16 = fix16_mul(fix16_from_s16sample((int16_t) out[i]), preprocessing.preamp); + fix3_28_t x_f16 = fix16_mul(norm_fix3_28_from_s16sample((int16_t) out[i]), preprocessing.preamp); x_f16 = bqf_transform(x_f16, &bqf_filters_left[j], &bqf_filters_mem_left[j]); - out[i] = (int32_t) fix16_to_s16sample(x_f16); + out[i] = (int32_t) norm_fix3_28_to_s16sample(x_f16); } } @@ -189,12 +189,12 @@ void core1_entry() { for (int j = 0; j < filter_stages; j++) { for (int i = 1; i < samples; i += 2) { - fix16_t x_f16 = fix16_mul(fix16_from_s16sample((int16_t) out[i]), preprocessing.preamp); + fix3_28_t x_f16 = fix16_mul(norm_fix3_28_from_s16sample((int16_t) out[i]), preprocessing.preamp); x_f16 = bqf_transform(x_f16, &bqf_filters_right[j], &bqf_filters_mem_right[j]); - out[i] = (int16_t) fix16_to_s16sample(x_f16); + out[i] = (int16_t) norm_fix3_28_to_s16sample(x_f16); } } @@ -773,8 +773,8 @@ static bool do_set_current(struct usb_setup_packet *setup) { } static void _tf_send_control_in_ack(__unused struct usb_endpoint *endpoint, __unused struct usb_transfer *transfer) { - assert(endpoint == &usb_control_in); - assert(transfer == &_control_in_transfer); + //assert(endpoint == &usb_control_in); + //assert(transfer == &_control_in_transfer); usb_debug("_tf_setup_control_ack\n"); static struct usb_transfer _control_out_transfer; usb_start_empty_transfer(usb_get_control_out_endpoint(), &_control_out_transfer, 0); diff --git a/firmware/code/run.h b/firmware/code/run.h index 2a64beb..6cfcd48 100644 --- a/firmware/code/run.h +++ b/firmware/code/run.h @@ -109,7 +109,7 @@ typedef struct _audio_device_config { } audio_device_config; typedef struct _preprocessing_config { - fix16_t preamp; + fix3_28_t preamp; int reverse_stereo; } preprocessing_config;