C/DSP_Simulation
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Calc-Funktion weiter, unnötige Funktionen auskommentiert (kompiliert)

Browse Source
This commit is contained in:
Patrick Hangl
2026-02-02 16:12:41 +01:00
parent fa787bec48
commit f6fa0ed021
85 changed files with 16931 additions and 12836 deletions
Download Patch File Download Diff File Expand all files Collapse all files

220
simulation/signal_processing/signal_path.c
View File

@@ -8,13 +8,12 @@ static int mu;
static int leak=2147462173; //0.999 // (1 ? <20>?)
// Int Arrays für Delay Line (Acc-Sensor Samples) sowie Filterkoeffizienten anlegen
int chess_storage(DMB) delay_line[MAX_FIR_COEFFS];
int chess_storage(DMA % (sizeof(long long))) filter_coefficients[MAX_FIR_COEFFS];
int chess_storage(DMB) sample_line[MAX_FIR_COEFFS];
int chess_storage(DMA % (sizeof(long long))) coefficient_line[MAX_FIR_COEFFS];
// Structs für Pointerinkrementierung auf Delay Line- und Koeffizieten-Arrays anlegen
BufferPtrDMB chess_storage(DMB) pointer_delay_line;
BufferPtr pointer_filter_coefficients;
BufferPtrDMB chess_storage(DMB) pointer_sample_line;
BufferPtr pointer_coefficient_line;
#ifdef PLATFORM_GENERIC
@@ -70,6 +69,69 @@ BufferPtr pointer_filter_coefficients;
}
#endif
//Allgemeinen Buffer um bestimmten Eingabewert inkrementieren - nicht in Verwendung
/* void increment_buffer(BufferPtr *buffer, int i_incr){
buffer->ptr_current = cyclic_add(buffer->ptr_current, i_incr, buffer->ptr_start, buffer->buffer_len);
}
*/
//DMB-Buffer um bestimmten Eingabewert inkrementieren - nicht in Verwendung
/* void increment_buffert_DMB(BufferPtrDMB *buffer, int i_incr){
buffer->ptr_current = cyclic_add(buffer->ptr_current, i_incr, buffer->ptr_start, buffer->buffer_len);
} */
//Übergabeblock in allgemeinen Buffer schreiben und Buffer inkrementieren - nicht in Verwendung
//void static inline write_buffer_block(BufferPtr *buffer, int* block){
// for (int i=0; i<BLOCK_LEN; i+=2){
// buffer->ptr_current[0] = block[i]; // TODO: use llcompose
// buffer->ptr_current[1] = block[i+1];
// buffer->ptr_current = cyclic_add(buffer->ptr_current, 2, buffer->ptr_start, buffer->buffer_len);
// }
//}
//Nicht verwendet
/* int sig_calc_biquad(SingleSignalPath *signal, int x) {
if (signal->preemph_activated == 0) {
return x;
}
accum_t sum =
fract_mult(x, signal->b_preemph[0]) + fract_mult(signal->_xd[0], signal->b_preemph[1]) +
fract_mult(signal->_xd[1], signal->b_preemph[2]) + fract_mult(signal->_yd[0], signal->b_preemph[3]) +
fract_mult(signal->_yd[1],signal->b_preemph[4]);
int y = rnd_saturate(sum << 1);
signal->_xd[1] = signal->_xd[0];
signal->_xd[0] = x;
signal->_yd[1] = signal->_yd[0];
signal->_yd[0] = y;
return y;
} */
//Nicht verwendet
/* int inline sig_get_delayed_sample(SingleSignalPath *signal) {
return *signal->delay_buffer.ptr_current;
} */
//Nicht verwendet
/* int sig_delay_buffer_load_and_get(SingleSignalPath *signal, int x) {
if (signal->delay_buffer.buffer_len == 0) {
return x;
}
int out = *signal->delay_buffer.ptr_current;
*signal->delay_buffer.ptr_current = x;
increment_buffer(&signal->delay_buffer, 1);
return out;
} */
//Nicht verwendet
/* int sig_calc_weight(SingleSignalPath *signal, int x) {
if (signal->weight_actived == 0) {
return x;
}
accum_t acc = fract_mult(x, signal->weight);
return rnd_saturate(acc);
} */
//Allgemeinen Buffer initialisieren
int initialize_buffer(BufferPtr *buffer, int *buffer_start_add, int length, int max_buffer_len) {
buffer->buffer_len = length;
@@ -104,40 +166,20 @@ int initialize_buffer_dmb(BufferPtrDMB chess_storage(DMB) *buffer, int chess_sto
}
}
//Allgemeinen Buffer um bestimmten Eingabewert inkrementieren - nicht in Verwendung
void increment_buffer(BufferPtr *buffer, int i_incr){
buffer->ptr_current = cyclic_add(buffer->ptr_current, i_incr, buffer->ptr_start, buffer->buffer_len);
}
//DMB-Buffer um bestimmten Eingabewert inkrementieren - nicht in Verwendung
void increment_buffert_DMB(BufferPtrDMB *buffer, int i_incr){
buffer->ptr_current = cyclic_add(buffer->ptr_current, i_incr, buffer->ptr_start, buffer->buffer_len);
}
//Übergabesample in allgemeinen Buffer schreiben und Buffer inkrementieren - nicht in Verwendung
void write_buffer(BufferPtr *buffer, int sample){
*buffer->ptr_current = sample;
buffer->ptr_current = cyclic_add(buffer->ptr_current, 1, buffer->ptr_start, buffer->buffer_len);
}
//Übergabesample in DMB Buffer schreiben (Delay-Line) und Buffer inkrementieren
//Übergabesample in DMB Buffer schreiben (Sample-Line) und Buffer inkrementieren
void write_buffer_dmb(BufferPtrDMB chess_storage(DMB) *buffer, int sample){
*buffer->ptr_current = sample; //Sample des Acc-Sensors wird in Adresse geschrieben, auf die der Pointer zeigt
buffer->ptr_current = cyclic_add(buffer->ptr_current, 1, buffer->ptr_start, buffer->buffer_len); //Pointer wird inkrementiert
}
void static inline write_buffer_block(BufferPtr *buffer, int* block){
// increment pointer to oldest block
//buffer->ptr_current = cyclic_add(buffer->ptr_current, BLOCK_LEN, buffer->ptr_start, buffer->buffer_len);
// load the next block
for (int i=0; i<BLOCK_LEN; i+=2){
buffer->ptr_current[0] = block[i]; // TODO: use llcompose
buffer->ptr_current[1] = block[i+1];
buffer->ptr_current = cyclic_add(buffer->ptr_current, 2, buffer->ptr_start, buffer->buffer_len);
}
}
//Initialisierungsfunktion für Biquad Filter Koeffizienten
void sig_init_preemph_coef(SingleSignalPath *signal, double b0, double b1, double b2, double a1, double a2, int scale_bits) {
void scale_preemph_filter(SingleSignalPath *signal, double b0, double b1, double b2, double a1, double a2, int scale_bits) {
// Wenn b0=1 und Rest 0 -> kein Filter weil effektiv 1*Xn
if (b0 == 1. && b1 == 0. && b2 == 0. && a1 == 0. && a2 == 0.) {
signal->preemph_activated = 0;
@@ -155,13 +197,13 @@ void sig_init_preemph_coef(SingleSignalPath *signal, double b0, double b1, doubl
}
}
/*Initialization functions - make sure all of them were called to ensure functionality*/
int sig_init_delay(SingleSignalPath *signal, int n_delay) {
//Initialisierungsfunktion für Delay
int set_delay(SingleSignalPath *signal, int n_delay) {
return initialize_buffer(&signal->delay_buffer, signal->_delay_buffer, n_delay, MAX_DELAY_SAMPS);
}
//Initialisierungsfunktion für Gewichtung
void sig_init_weight(SingleSignalPath *signal, double weight, int scale_nbits) {
void set_weight(SingleSignalPath *signal, double weight, int scale_nbits) {
// Wenn Gewichtung 1 -> kein Effekt
if (weight == 1.) {
signal->weight_actived = 0;
@@ -175,57 +217,15 @@ void sig_init_weight(SingleSignalPath *signal, double weight, int scale_nbits) {
}
}
/*Calculator functions for the given signal path*/
/*Calculate one biquad filter element*/
int sig_calc_biquad(SingleSignalPath *signal, int x) {
if (signal->preemph_activated == 0) {
return x;
}
accum_t sum =
fract_mult(x, signal->b_preemph[0]) + fract_mult(signal->_xd[0], signal->b_preemph[1]) +
fract_mult(signal->_xd[1], signal->b_preemph[2]) + fract_mult(signal->_yd[0], signal->b_preemph[3]) +
fract_mult(signal->_yd[1],signal->b_preemph[4]);
int y = rnd_saturate(sum << 1);
signal->_xd[1] = signal->_xd[0];
signal->_xd[0] = x;
signal->_yd[1] = signal->_yd[0];
signal->_yd[0] = y;
return y;
}
int inline sig_get_delayed_sample(SingleSignalPath *signal) {
return *signal->delay_buffer.ptr_current;
}
int sig_delay_buffer_load_and_get(SingleSignalPath *signal, int x) {
if (signal->delay_buffer.buffer_len == 0) {
return x;
}
int out = *signal->delay_buffer.ptr_current;
*signal->delay_buffer.ptr_current = x;
increment_buffer(&signal->delay_buffer, 1);
return out;
}
int sig_calc_weight(SingleSignalPath *signal, int x) {
if (signal->weight_actived == 0) {
return x;
}
accum_t acc = fract_mult(x, signal->weight);
return rnd_saturate(acc);
}
int inline apply_fir_filter(BufferPtrDMB chess_storage(DMB) *pointer_delay_line, BufferPtr *pointer_filter_coefficients){
int inline apply_fir_filter(BufferPtrDMB chess_storage(DMB) *pointer_sample_line, BufferPtr *pointer_coefficient_line){
// Filterkoeffizienten mit Acc-Sensor Samples multiplizieren und aufsummieren um Akkumulator Output des adaptiven Filters zu erhalten
//Pointer für Koeffizienten und Delay Line Samples anlegen
int chess_storage(DMB) *p_x0 = pointer_delay_line->ptr_current;
int chess_storage(DMB) *p_xstart = pointer_delay_line->ptr_start;
int *p_w = pointer_filter_coefficients->ptr_current;
int delay_line_len = pointer_delay_line->buffer_len;
int n_coeff = pointer_filter_coefficients->buffer_len;
//Pointer für Koeffizienten und Sample-Line Samples anlegen
int chess_storage(DMB) *p_x0 = pointer_sample_line->ptr_current;
int chess_storage(DMB) *p_xstart = pointer_sample_line->ptr_start;
int *p_w = pointer_coefficient_line->ptr_current;
int sample_line_len = pointer_sample_line->buffer_len;
int n_coeff = pointer_coefficient_line->buffer_len;
//Variablen und Akkumulatoren (72-Bit) anlegen
int x0, x1, w0, w1;
@@ -235,15 +235,15 @@ int inline apply_fir_filter(BufferPtrDMB chess_storage(DMB) *pointer_delay_line,
// In 2er Schritten durch die Koeffizienten iterieren, immer 2 Samples und 2 Koeffizienten pro Schleifendurchlauf -> DUAL LOAD und DUAL MAC
for (int i=0; i < n_coeff; i+=2) chess_loop_range(1,){
x0 = *p_x0; //Sample 1 aus Delay Line
x0 = *p_x0; //Sample 1 aus Sample-Line
w0 = *p_w; //Koeffizient 1 aus Koeffizienten Array
p_w++; //Koeffizienten-Pointer inkrementieren
p_x0 = cyclic_add(p_x0, -1, p_xstart, delay_line_len); //Delay-Line-Pointer dekrementieren (rueckwaerts durch Delay Line)
p_x0 = cyclic_add(p_x0, -1, p_xstart, sample_line_len); //Sample-Line-Pointer dekrementieren (rueckwaerts durch Delay Line)
x1 = *p_x0; //Sample 2 aus Delay Line
x1 = *p_x0; //Sample 2 aus Sample-Line
w1 = *p_w; //Koeffizient 2 aus Koeffizienten Array
p_w++; //Koeffizienten-Pointer inkrementieren
p_x0 = cyclic_add(p_x0, -1, p_xstart, delay_line_len); //Delay-Line-Pointer dekrementieren (rueckwaerts durch Delay Line)
p_x0 = cyclic_add(p_x0, -1, p_xstart, sample_line_len); //Sample-Line-Pointer dekrementieren (rueckwaerts durch Sample-Line)
acc_fir_1+=fract_mult(x0, w0); //Akkumulator 1 mit Sample 1 * Koeffizient 1 addieren
acc_fir_2+=fract_mult(x1, w1); //Akkumulator 2 mit Sample 2 * Koeffizient 2 addieren
@@ -253,20 +253,20 @@ int inline apply_fir_filter(BufferPtrDMB chess_storage(DMB) *pointer_delay_line,
return rnd_saturate(acc_fir);
}
void static inline update_filter_coefficients(BufferPtrDMB chess_storage(DMB) *pointer_delay_line, BufferPtr *pointer_filter_coefficients, int output){
void static inline update_filter_coefficients(BufferPtrDMB chess_storage(DMB) *pointer_sample_line, BufferPtr *pointer_coefficient_line, int output){
int chess_storage(DMA) *p_w0 = pointer_filter_coefficients->ptr_start; //Pointer auf Filterkoeffizienten-Array
int chess_storage(DMB) *p_x0 = pointer_delay_line->ptr_current; //Current-Pointer 1 auf Delay-Line Array
int chess_storage(DMB) *p_x1 = pointer_delay_line->ptr_current; //Current-Pointer 2 auf Delay-Line Array
int chess_storage(DMB) *p_xstart = pointer_delay_line->ptr_start; //Start-Pointer auf Delay-Line Array
int chess_storage(DMA) *p_w0 = pointer_coefficient_line->ptr_start; //Pointer auf Filterkoeffizienten-Array
int chess_storage(DMB) *p_x0 = pointer_sample_line->ptr_current; //Current-Pointer 1 auf Sample-Line Array
int chess_storage(DMB) *p_x1 = pointer_sample_line->ptr_current; //Current-Pointer 2 auf Sample-Line Array
int chess_storage(DMB) *p_xstart = pointer_sample_line->ptr_start; //Start-Pointer auf Sample-Line Array
int delay_line_len = pointer_delay_line->buffer_len; // Länge des Delay-Line Arrays
int n_coeff = pointer_filter_coefficients->buffer_len; // Anzahl der Filterkoeffizienten
int sample_line_len = pointer_sample_line->buffer_len; // Länge des Sample-Line Arrays
int n_coeff = pointer_coefficient_line->buffer_len; // Anzahl der Filterkoeffizienten
int correction, x0, x1, w0, w1;
accum_t acc_w0, acc_w1, product;
p_x1 = cyclic_add(p_x1, -1, pointer_delay_line->ptr_start, pointer_delay_line->buffer_len); //Current-Pointer 2 dekrementieren um 1
p_x1 = cyclic_add(p_x1, -1, pointer_sample_line->ptr_start, pointer_sample_line->buffer_len); //Current-Pointer 2 dekrementieren um 1
product = fract_mult(mu, output); //FIR-Output mit mu multiplizieren -> Korrektursignal. aktuell noch im accum-Format
correction = rnd_saturate(product); //Korrektursignal wieder ins 32-Bit Format
@@ -279,16 +279,16 @@ void static inline update_filter_coefficients(BufferPtrDMB chess_storage(DMB) *p
// Filterkoeffizienten mit Korrekturterm*Acc-Sensor-Sample updaten - 1 Cycle
acc_w0 += fract_mult(correction, *p_x0);
acc_w1 += fract_mult(correction, *p_x1);
//Beide Pointer in der Delay-Line um 2 dekrementieren
p_x0 = cyclic_add(p_x0, -2, p_xstart, delay_line_len);
p_x1 = cyclic_add(p_x1, -2, p_xstart, delay_line_len);
//Beide Pointer in der Sample-Line um 2 dekrementieren
p_x0 = cyclic_add(p_x0, -2, p_xstart, sample_line_len);
p_x1 = cyclic_add(p_x1, -2, p_xstart, sample_line_len);
// Filterkoeffizienten in 64-Bit Wort schreiben - wird dann in mit einem Store-Vorgang an Ort wo p_w0 hinzeigt abgelegt - 1 Cycle
*((long long *)p_w0) = llcompose(rnd_saturate(acc_w0), rnd_saturate(acc_w1));//LOAD/STORE-Hazard - +1 NOP benötigt - 1 Cycle
p_w0+=2; //Koeffizienten-Pointer um 2 inkrementieren
}
}
void init(
void initialize_signal(
SingleSignalPath *c_sensor_signal_t,
SingleSignalPath *acc_sensor_signal_t,
double *b_c,
@@ -303,32 +303,32 @@ void init(
int scale_bits=31;
// C-Sensor Initialisierung: Biquad, Delay, Weight skalieren und in Struct schreiben
sig_init_preemph_coef(c_sensor_signal_t, b_c[0], b_c[1], b_c[2], b_c[3], b_c[4], scale_bits);
sig_init_delay(c_sensor_signal_t, delay_c);
sig_init_weight(c_sensor_signal_t, weight_c, scale_bits);
scale_preemph_filter(c_sensor_signal_t, b_c[0], b_c[1], b_c[2], b_c[3], b_c[4], scale_bits);
set_delay(c_sensor_signal_t, delay_c);
set_weight(c_sensor_signal_t, weight_c, scale_bits);
// Acc-Sensor Initialisierung: Biquad, Delay, Weight skalieren und in Struct schreiben
sig_init_preemph_coef(acc_sensor_signal_t, b_acc[0], b_acc[1], b_acc[2], b_acc[3], b_acc[4], scale_bits);
sig_init_delay(acc_sensor_signal_t, delay_acc);
sig_init_weight(acc_sensor_signal_t, weight_acc, 31);
scale_preemph_filter(acc_sensor_signal_t, b_acc[0], b_acc[1], b_acc[2], b_acc[3], b_acc[4], scale_bits);
set_delay(acc_sensor_signal_t, delay_acc);
set_weight(acc_sensor_signal_t, weight_acc, 31);
//Mu Skalierung und in globale Variable schreiben
int scale = pow(2, scale_bits) - 1;
mu = lms_mu * scale;
// Buffer Initialisierung (Delay Line und Koeffizienten)
initialize_buffer_dmb(&pointer_delay_line, delay_line, number_coefficients, MAX_FIR_COEFFS);
initialize_buffer(&pointer_filter_coefficients, filter_coefficients, number_coefficients, MAX_FIR_COEFFS);
initialize_buffer_dmb(&pointer_sample_line, sample_line, number_coefficients, MAX_FIR_COEFFS);
initialize_buffer(&pointer_coefficient_line, coefficient_line, number_coefficients, MAX_FIR_COEFFS);
// Einträge in Delay Line und Koeffizienten-Array auf 0 setzen
for (int i = 0; i < number_coefficients; i++) {
pointer_delay_line.ptr_start[i] = 0;
pointer_filter_coefficients.ptr_start[i] = 0;
pointer_sample_line.ptr_start[i] = 0;
pointer_coefficient_line.ptr_start[i] = 0;
}
}
// C-Sensor (d) = Corrupted Signal (Desired Signal + Corruption Noise Signal)
// Acc-Sensor (x) = Reference Noise Signal
void calc(
void calculate_output(
SingleSignalPath *c_sensor_signal_t,
SingleSignalPath *acc_sensor_signal_t,
int16_t volatile chess_storage(DMB) *c_sensor_input, //Pointer auf Input-Port im Shared Memory
@@ -364,14 +364,14 @@ void calc(
// Adaptiven Filter auf C-Sensor Signal anwenden
//Aktuelles Sample des Acc-Sensors wird in aktuelle Speicheradresse des Pointers der Delay Line geschrieben, dann wird der Pointer inkrementiert -> Delay Line hat Länge der Filterkoeffizienten
write_buffer_dmb(&pointer_delay_line, acc_sensor_pre[0]);
write_buffer_dmb(&pointer_sample_line, acc_sensor_pre[0]);
// Filter auf Acc-Sensor Signal anwenden und Korrektursignal berechnen
// Sample des Acc-Sensors in der Delay-Line werden mit den Filterkoeffizienten multipliziert und aufsummiert -> Akkumulator Output des adaptiven Filters
filter_accumulator[0] = apply_fir_filter(&pointer_delay_line, &pointer_filter_coefficients);
// Sample des Acc-Sensors in der Sample-Line werden mit den Filterkoeffizienten multipliziert und aufsummiert -> Akkumulator Output des adaptiven Filters
filter_accumulator[0] = apply_fir_filter(&pointer_sample_line, &pointer_coefficient_line);
// Output-Signal berechnen -> C-Sensor Sample - Akkumulator Output des adaptiven Filters
output_32[0] = c_sensor_pre[0] - filter_accumulator[0];
// Filterkoeffizienten adaptieren
update_filter_coefficients(&pointer_delay_line, &pointer_filter_coefficients, output_32[0]);
update_filter_coefficients(&pointer_sample_line, &pointer_coefficient_line, output_32[0]);
// Bitshift zurück auf 16-Bit und in Ausgangsarray schreiben
for (uint32_t i=0; i<BLOCK_LEN; i++) chess_flatten_loop
{