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

Calc-Funktion kommentiert - kompiliert

Browse Source
This commit is contained in:
Patrick Hangl
2026-01-27 11:17:03 +01:00
parent 5e701bd41d
commit 9b09cb21fa
140 changed files with 2420830 additions and 12680 deletions
Download Patch File Download Diff File Expand all files Collapse all files

119
simulation/signal_processing/signal_path.c
View File

@@ -5,14 +5,14 @@
static int counter=0;
static int mu;
static int leak=2147462173; //0.999 // (1 ? µ?)
static int leak=2147462173; //0.999 // (1 ? <EFBFBD>?)
int chess_storage(DMB) fir_lms_delay_line[MAX_FIR_COEFFS]; //Int-Array für Acc-Sensors Samples (Delay Line) anlegen
int chess_storage(DMA % (sizeof(long long))) fir_lms_coeffs[MAX_FIR_COEFFS]; //Int-Array für Filterkoeffizienten anlegen
int chess_storage(DMB) fir_lms_delay_line[MAX_FIR_COEFFS];
BufferPtrDMB chess_storage(DMB) ptr_fir_lms_delay_line;
BufferPtrDMB chess_storage(DMB) ptr_fir_lms_delay_line;
BufferPtr ptr_fir_lms_coeffs;
int chess_storage(DMA % (sizeof(long long))) fir_lms_coeffs[MAX_FIR_COEFFS]; // The coefficients for the adaptive filter
#ifdef PLATFORM_GENERIC
@@ -114,8 +114,8 @@ void sig_cirular_buffer_ptr_put_sample(BufferPtr *buffer, int sample){
}
void sig_cirular_buffer_ptr_put_sample_DMB(BufferPtrDMB chess_storage(DMB) *buffer, int sample){
*buffer->ptr_current = sample;
buffer->ptr_current = cyclic_add(buffer->ptr_current, 1, buffer->ptr_start, buffer->buffer_len);
*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 sig_circular_buffer_ptr_put_block(BufferPtr *buffer, int* block){
@@ -129,7 +129,7 @@ void static inline sig_circular_buffer_ptr_put_block(BufferPtr *buffer, int* blo
}
}
//Initialisierungsfunktion für Biquad Filter Koeffizienten
//Initialisierungsfunktion f<EFBFBD>r Biquad Filter Koeffizienten
void sig_init_preemph_coef(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.) {
@@ -153,7 +153,7 @@ int sig_init_delay(SingleSignalPath *signal, int n_delay) {
return sig_init_buffer(&signal->delay_buffer, signal->_delay_buffer, n_delay, MAX_DELAY_SAMPS);
}
//Initialisierungsfunktion für Gewichtung
//Initialisierungsfunktion f<EFBFBD>r Gewichtung
void sig_init_weight(SingleSignalPath *signal, double weight, int scale_nbits) {
// Wenn Gewichtung 1 -> kein Effekt
if (weight == 1.) {
@@ -211,14 +211,16 @@ int sig_calc_weight(SingleSignalPath *signal, int x) {
}
int inline sig_calc_fir_lpdsp32_single(BufferPtrDMB chess_storage(DMB) *ptr_fir_lms_delay_line, BufferPtr *ptr_fir_lms_coeffs){
// Filterkoeffizienten mit Acc-Sensor Samples multiplizieren und aufsummieren um Akkumulator Output des adaptiven Filters zu erhalten
// Calculate the fir filter output on x to get the canceller
int chess_storage(DMB) *p_x0 = ptr_fir_lms_delay_line->ptr_current; // chess_storage(DMB)
//Pointer für Koeffizienten und Delay Line Samples anlegen
int chess_storage(DMB) *p_x0 = ptr_fir_lms_delay_line->ptr_current;
int chess_storage(DMB) *px_start = ptr_fir_lms_delay_line->ptr_start;
int *p_h = ptr_fir_lms_coeffs->ptr_current;
int delay_line_len = ptr_fir_lms_delay_line->buffer_len;
int n_coeff = ptr_fir_lms_coeffs->buffer_len;
//Variablen und Akkumulatoren (72-Bit) anlegen
int d0,d1,h0,h1;
accum_t acc1_A = to_accum(0);
accum_t acc1_B = to_accum(0);
@@ -230,50 +232,42 @@ int inline sig_calc_fir_lpdsp32_single(BufferPtrDMB chess_storage(DMB) *ptr_fir_
dual mac and dual load: 1
-> 48/2 * 2 = 48 cycles for 48 coefficents
*/
// 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,){
// Use dual load and dual pointer update
d0 = *p_x0;
h0 = *p_h;
p_h++;
p_x0 = cyclic_add(p_x0, -1, px_start, delay_line_len);
d0 = *p_x0; //Sample 1 aus Delay Line
h0 = *p_h; //Koeffizient 1 aus Koeffizienten Array
p_h++; //Koeffizienten-Pointer inkrementieren
p_x0 = cyclic_add(p_x0, -1, px_start, delay_line_len); //Delay-Line-Pointer dekrementieren (rueckwaerts durch Delay Line)
d1 = *p_x0;
h1 = *p_h;
p_h++;
p_x0 = cyclic_add(p_x0, -1, px_start, delay_line_len);
d1 = *p_x0; //Sample 2 aus Delay Line
h1 = *p_h; //Koeffizient 2 aus Koeffizienten Array
p_h++; //Koeffizienten-Pointer inkrementieren
p_x0 = cyclic_add(p_x0, -1, px_start, delay_line_len); //Delay-Line-Pointer dekrementieren (rueckwaerts durch Delay Line)
acc1_A+=fract_mult(d0, h0);
acc1_B+=fract_mult(d1, h1);
#ifndef LPDSP16
acc1_A = to_accum(rnd_saturate(acc1_A));
acc1_B = to_accum(rnd_saturate(acc1_B));
#endif
acc1_A+=fract_mult(d0, h0); //Akkumulator 1 mit Sample 1 * Koeffizient 1 addieren
acc1_B+=fract_mult(d1, h1); //Akkumulator 2 mit Sample 2 * Koeffizient 2 addieren
}
// Calculate the output sample
// Akkumulatoren addieren um das Filterergebnis zu erhalten
acc1_C = acc1_A + acc1_B;
//out32 = rnd_saturate(acc1_A);
return rnd_saturate(acc1_C);
}
void static inline adapt_coeffs_lpdsp32_single_v1(BufferPtrDMB chess_storage(DMB) *ptr_fir_lms_delay_line, BufferPtr *ptr_fir_lms_coeffs, int out){
int chess_storage(DMA) *p_h0 = ptr_fir_lms_coeffs->ptr_start; //coeff load pointer
//int chess_storage(DMA) *p_h1 = ptr_fir_lms_coeffs->ptr_start; //coeff store pointer
int chess_storage(DMB) *p_x0 = ptr_fir_lms_delay_line->ptr_current; // chess_storage(DMB)
int chess_storage(DMB) *p_x1 = ptr_fir_lms_delay_line->ptr_current; // chess_storage(DMB)
p_x1 = cyclic_add(p_x1, -1, ptr_fir_lms_delay_line->ptr_start, ptr_fir_lms_delay_line->buffer_len);
int chess_storage(DMA) *p_h0 = ptr_fir_lms_coeffs->ptr_start; //Pointer auf Filterkoeffizienten-Array
int chess_storage(DMB) *p_x0 = ptr_fir_lms_delay_line->ptr_current; //Current-Pointer 1 auf Delay-Line Array
int chess_storage(DMB) *p_x1 = ptr_fir_lms_delay_line->ptr_current; //Current-Pointer 2 auf Delay-Line Array
int chess_storage(DMB) *px_start = ptr_fir_lms_delay_line->ptr_start; //Start-Pointer auf Delay-Line Array
int delay_line_len = ptr_fir_lms_delay_line->buffer_len; // Länge des Delay-Line Arrays
int n_coeff = ptr_fir_lms_coeffs->buffer_len; // Anzahl der Filterkoeffizienten
int prod, x0, x1, h0, h1;
int chess_storage(DMB) *px_start = ptr_fir_lms_delay_line->ptr_start;
int delay_line_len = ptr_fir_lms_delay_line->buffer_len;
int n_coeff = ptr_fir_lms_coeffs->buffer_len;
p_x1 = cyclic_add(p_x1, -1, ptr_fir_lms_delay_line->ptr_start, ptr_fir_lms_delay_line->buffer_len); //Current-Pointer 2 dekrementieren um 1
accum_t acc_A, acc_B;
// Calculate the first term of the coefficient adaption
accum_t acc_C = fract_mult(mu, out);
accum_t acc_C = fract_mult(mu, out); //Korrektursignal * mu um Filterkoeffizienten anzupassen
prod = rnd_saturate(acc_C);
/* Abschätzung cycles per 2 coefficient:
@@ -294,13 +288,13 @@ void static inline adapt_coeffs_lpdsp32_single_v1(BufferPtrDMB chess_storage(DMB
acc_A = to_accum(h0);
acc_B = to_accum(h1);
acc_A += fract_mult(prod, *p_x0); // TODO: This could be further optimized by using all 4 available accums?
acc_A += fract_mult(prod, *p_x0);
acc_B += fract_mult(prod, *p_x1);
p_x0 = cyclic_add(p_x0, -2, px_start, delay_line_len);
p_x1 = cyclic_add(p_x1, -2, px_start, delay_line_len);
// update the current filter coefficients - dual sat; dual store
// Filterkoeffizienten updaten - dual sat; dual store
*((long long *)p_h0) = llcompose(rnd_saturate(acc_A), rnd_saturate(acc_B));//load/store hazard ! - 1 nop is needed
p_h0+=2;
}
@@ -333,10 +327,11 @@ void init(
//Mu Skalierung und in globale Variable schreiben
int scale = pow(2, scale_bits) - 1;
mu = lms_mu * scale;
// Buffer Initialisierung (Delay Line und Koeffizienten) und anschließend alle Werte auf 0 setzen
// Buffer Initialisierung (Delay Line und Koeffizienten)
sig_init_buffer_DMB(&ptr_fir_lms_delay_line, fir_lms_delay_line, lms_fir_num_coeffs, MAX_FIR_COEFFS);
sig_init_buffer(&ptr_fir_lms_coeffs, fir_lms_coeffs, lms_fir_num_coeffs, MAX_FIR_COEFFS);
// Einträge in Delay Line und Koeffizienten-Array auf 0 setzen
for (int i = 0; i < lms_fir_num_coeffs; i++) {
ptr_fir_lms_delay_line.ptr_start[i] = 0;
ptr_fir_lms_coeffs.ptr_start[i] = 0;
@@ -349,46 +344,50 @@ void calc(
SingleSignalPath *cSensorSignal,
SingleSignalPath *accSensorSignal,
OutputMode output_mode,
int16_t volatile chess_storage(DMB) *cSensor,
int16_t volatile chess_storage(DMB) *accSensor,
int16_t volatile chess_storage(DMB) *out_16
int16_t volatile chess_storage(DMB) *cSensor, //Pointer auf Input-Port im Shared Memory
int16_t volatile chess_storage(DMB) *accSensor, //Pointer auf Input-Port im Shared Memory
int16_t volatile chess_storage(DMB) *out_16 //Pointer auf Output-Port im Shared Memory
){
static int chess_storage(DMA) c_block_pre[BLOCK_LEN];
static int chess_storage(DMA) acc_block_pre[BLOCK_LEN];
static int chess_storage(DMA) cSensor_32[BLOCK_LEN];
static int chess_storage(DMA) accSensor_32[BLOCK_LEN];
//Speicherbereiche anlegen -> bei blockweiser Verarbeitung hat jedes Array nur den Eintrag [0]
static int chess_storage(DMA) c_block_pre[BLOCK_LEN]; //Speicherbereich für C-Sensor Preemphasis Input
static int chess_storage(DMA) acc_block_pre[BLOCK_LEN]; //Speicherbereich für Acc-Sensor Preemphasis Input
static int chess_storage(DMA) cSensor_32[BLOCK_LEN]; //Speicherbereich für 32-Bit C-Sensor Input
static int chess_storage(DMA) accSensor_32[BLOCK_LEN]; //Speicherbereich für 32-Bit Acc-Sensor Input
static int chess_storage(DMB) acc_block_filt[BLOCK_LEN];
static int chess_storage(DMB) out_32[BLOCK_LEN];
static int chess_storage(DMB) acc_block_filt[BLOCK_LEN]; //Speicherbereich für Akkumulator Output des adaptiven Filters
static int chess_storage(DMB) out_32[BLOCK_LEN]; //Speicherbereich für 32-Bit Output Signal
// Pointer auf die Arrays anlegen
static int chess_storage(DMA) *p_c_block_pre =c_block_pre;
static int chess_storage(DMA) *p_acc_block_filt =acc_block_pre;
static int chess_storage(DMB) *p_out_32=out_32;
// 16-Bit Eingangssignale auf 32-Bit konvertieren mit Bitshift, in neuem Speicherbereich ablegen
for (uint32_t i=0; i<BLOCK_LEN; i++) chess_loop_range(1,){
cSensor_32[i] = ((int) cSensor[i]) << BITSHIFT_16_TO_32;
accSensor_32[i] = ((int) accSensor[i]) << BITSHIFT_16_TO_32;
}
// Apply bitshift, calculate the pre emphasis filter, delay and weight to each channel
// Preemphasis Filter anwenden - wird hier aber nicht genutzt (nur Durchreichen), in neuen Speicherbereich ablegen
for (uint32_t i=0; i<BLOCK_LEN; i++) chess_loop_range(1,){
c_block_pre[i] = cSensor_32[i];
acc_block_pre[i] = accSensor_32[i];
}
// apply lms filter on cSensor signal
// Increment the buffer pointer and set the current sample to the delay line
// 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
sig_cirular_buffer_ptr_put_sample_DMB(&ptr_fir_lms_delay_line, acc_block_pre[0]);
// Calculate the fir filter output on acc to get the canceller
// 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
acc_block_filt[0]= sig_calc_fir_lpdsp32_single(&ptr_fir_lms_delay_line, &ptr_fir_lms_coeffs);
// Calculate the ouptut signal by c_block_pre - acc_block_filt
// Output-Signal berechnen -> C-Sensor Sample - Akkumulator Output des adaptiven Filters
out_32[0] = c_block_pre[0] - acc_block_filt[0];
// Calculate the coefficient adaptation
// Filterkoeffizienten adaptieren
adapt_coeffs_lpdsp32_single_v1(&ptr_fir_lms_delay_line, &ptr_fir_lms_coeffs, out_32[0]);
// TODO: Add a couple of biqads after ANC
// Bitshift zurück auf 16-Bit und in Ausgangsarray schreiben
for (uint32_t i=0; i<BLOCK_LEN; i++) chess_flatten_loop
{
out_16[i] = rnd_saturate(to_accum(out_32[i]) >> BITSHIFT_16_TO_32); // 12 cycles for blocksize 4 //TODO: use rnd_saturate(out_32[i] >> input_nbit_bitshift)