MirrorMirror
/
headphones
mirror of https://github.com/ploopyco/headphones.git
2
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

q3.28 changes

This commit is contained in:
ploopyco 2023-07-14 15:06:20 -04:00
parent 009dd2e698
commit 481219341d
7 changed files with 145 additions and 115 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

112
firmware/code/bqf.c
View File

@ -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) +

22
firmware/code/bqf.h
View File

@ -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

18
firmware/code/configuration_manager.c
View File

@ -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;
}

55
firmware/code/fix16.c
View File

@ -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.

39
firmware/code/fix16.h
View File

@ -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;
#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);
/// @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;
fix16_t fix16_mul(fix16_t, fix16_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
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

12
firmware/code/run.c
View File

@ -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);

2
firmware/code/run.h
View File

@ -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;