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

Calc Funktion weitergecoded - kompiliert

Browse Source
This commit is contained in:
Patrick Hangl
2026-01-27 16:38:58 +01:00
parent 9b09cb21fa
commit 6f52b7ace4
93 changed files with 15970 additions and 14407 deletions
Download Patch File Download Diff File Expand all files Collapse all files

162
simulation/signal_processing/signal_path.c
View File

@@ -1,17 +1,19 @@
#include "include/signal_path.h"
#define BLOCK_LEN 1
/* Global variables decleration*/
//Globale Variable setzen
static int counter=0;
static int mu;
static int leak=2147462173; //0.999 // (1 ? <20>?)
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 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];
BufferPtrDMB chess_storage(DMB) ptr_fir_lms_delay_line;
BufferPtr ptr_fir_lms_coeffs;
// Structs für Pointerinkrementierung auf Delay Line- und Koeffizieten-Arrays anlegen
BufferPtrDMB chess_storage(DMB) pointer_delay_line;
BufferPtr pointer_filter_coefficients;
@@ -68,7 +70,8 @@ BufferPtr ptr_fir_lms_coeffs;
}
#endif
int sig_init_buffer(BufferPtr *buffer, int *buffer_start_add, int length, int max_buffer_len) {
//Allgemeinen Buffer initialisieren
int initialize_buffer(BufferPtr *buffer, int *buffer_start_add, int length, int max_buffer_len) {
buffer->buffer_len = length;
buffer->ptr_start = buffer_start_add;
buffer->ptr_current = buffer_start_add;
@@ -84,7 +87,8 @@ int sig_init_buffer(BufferPtr *buffer, int *buffer_start_add, int length, int ma
}
}
int sig_init_buffer_DMB(BufferPtrDMB chess_storage(DMB) *buffer, int chess_storage(DMB) *buffer_start_add, int length, int max_buffer_len){
//DMB Buffer initialisieren
int initialize_buffer_dmb(BufferPtrDMB chess_storage(DMB) *buffer, int chess_storage(DMB) *buffer_start_add, int length, int max_buffer_len){
buffer->buffer_len = length;
buffer->ptr_start = buffer_start_add;
buffer->ptr_current = buffer_start_add;
@@ -100,25 +104,28 @@ int sig_init_buffer_DMB(BufferPtrDMB chess_storage(DMB) *buffer, int chess_stora
}
}
void sig_cirular_buffer_ptr_increment(BufferPtr *buffer, int i_incr){
//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);
}
void sig_cirular_buffer_ptr_increment_DMB(BufferPtrDMB *buffer, int i_incr){
//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);
}
void sig_cirular_buffer_ptr_put_sample(BufferPtr *buffer, int sample){
//Ü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);
}
void sig_cirular_buffer_ptr_put_sample_DMB(BufferPtrDMB chess_storage(DMB) *buffer, int sample){
//Übergabesample in DMB Buffer schreiben (Delay-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 sig_circular_buffer_ptr_put_block(BufferPtr *buffer, int* block){
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
@@ -129,7 +136,7 @@ void static inline sig_circular_buffer_ptr_put_block(BufferPtr *buffer, int* blo
}
}
//Initialisierungsfunktion f<EFBFBD>r Biquad Filter Koeffizienten
//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) {
// Wenn b0=1 und Rest 0 -> kein Filter weil effektiv 1*Xn
if (b0 == 1. && b1 == 0. && b2 == 0. && a1 == 0. && a2 == 0.) {
@@ -150,10 +157,10 @@ 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) {
return sig_init_buffer(&signal->delay_buffer, signal->_delay_buffer, n_delay, MAX_DELAY_SAMPS);
return initialize_buffer(&signal->delay_buffer, signal->_delay_buffer, n_delay, MAX_DELAY_SAMPS);
}
//Initialisierungsfunktion f<EFBFBD>r Gewichtung
//Initialisierungsfunktion für Gewichtung
void sig_init_weight(SingleSignalPath *signal, double weight, int scale_nbits) {
// Wenn Gewichtung 1 -> kein Effekt
if (weight == 1.) {
@@ -197,7 +204,7 @@ int sig_delay_buffer_load_and_get(SingleSignalPath *signal, int x) {
}
int out = *signal->delay_buffer.ptr_current;
*signal->delay_buffer.ptr_current = x;
sig_cirular_buffer_ptr_increment(&signal->delay_buffer, 1);
increment_buffer(&signal->delay_buffer, 1);
return out;
}
@@ -210,15 +217,15 @@ int sig_calc_weight(SingleSignalPath *signal, int x) {
return rnd_saturate(acc);
}
int inline sig_calc_fir_lpdsp32_single(BufferPtrDMB chess_storage(DMB) *ptr_fir_lms_delay_line, BufferPtr *ptr_fir_lms_coeffs){
int inline apply_fir_filter(BufferPtrDMB chess_storage(DMB) *pointer_delay_line, BufferPtr *pointer_filter_coefficients){
// 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 = 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;
int chess_storage(DMB) *p_x0 = pointer_delay_line->ptr_current;
int chess_storage(DMB) *px_start = pointer_delay_line->ptr_start;
int *p_h = pointer_filter_coefficients->ptr_current;
int delay_line_len = pointer_delay_line->buffer_len;
int n_coeff = pointer_filter_coefficients->buffer_len;
//Variablen und Akkumulatoren (72-Bit) anlegen
int d0,d1,h0,h1;
@@ -226,12 +233,6 @@ int inline sig_calc_fir_lpdsp32_single(BufferPtrDMB chess_storage(DMB) *ptr_fir_
accum_t acc1_B = to_accum(0);
accum_t acc1_C;
// iterate over the coefficients to calculate the filter on x - the canceller
/* Abschaetzung cycles per 2coefficient:
dual - load : 1
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,){
d0 = *p_x0; //Sample 1 aus Delay Line
@@ -252,18 +253,18 @@ int inline sig_calc_fir_lpdsp32_single(BufferPtrDMB chess_storage(DMB) *ptr_fir_
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){
void static inline update_filter_coefficients(BufferPtrDMB chess_storage(DMB) *pointer_delay_line, BufferPtr *pointer_filter_coefficients, int out){
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 chess_storage(DMA) *p_h0 = 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) *px_start = pointer_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 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 prod, x0, x1, h0, h1;
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
p_x1 = cyclic_add(p_x1, -1, pointer_delay_line->ptr_start, pointer_delay_line->buffer_len); //Current-Pointer 2 dekrementieren um 1
accum_t acc_A, acc_B;
@@ -279,11 +280,7 @@ void static inline adapt_coeffs_lpdsp32_single_v1(BufferPtrDMB chess_storage(DMB
*/
for (int i=0; i< n_coeff; i+=2) chess_loop_range(1,){
// Calculate the coefficient wise adaption
#ifdef PLATFORM_GENERIC
lldecompose(*((long long *)p_h0), &h0, &h1);
#else
lldecompose(*((long long *)p_h0), h0, h1);
#endif
lldecompose(*((long long *)p_h0), h0, h1);
acc_A = to_accum(h0);
acc_B = to_accum(h1);
@@ -301,8 +298,8 @@ void static inline adapt_coeffs_lpdsp32_single_v1(BufferPtrDMB chess_storage(DMB
}
void init(
SingleSignalPath *cSensorSignal,
SingleSignalPath *accSensorSignal,
SingleSignalPath *c_sensor_signal_t,
SingleSignalPath *acc_sensor_signal_t,
double *b_c,
double *b_acc,
int delay_c,
@@ -310,87 +307,86 @@ void init(
double weight_c,
double weight_acc,
double lms_mu,
int lms_fir_num_coeffs
int number_coefficients
){
int scale_bits=31;
// C-Sensor Initialisierung: Biquad, Delay, Weight skalieren und in Struct schreiben
sig_init_preemph_coef(cSensorSignal, b_c[0], b_c[1], b_c[2], b_c[3], b_c[4], scale_bits);
sig_init_delay(cSensorSignal, delay_c);
sig_init_weight(cSensorSignal, weight_c, scale_bits);
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);
// Acc-Sensor Initialisierung: Biquad, Delay, Weight skalieren und in Struct schreiben
sig_init_preemph_coef(accSensorSignal, b_acc[0], b_acc[1], b_acc[2], b_acc[3], b_acc[4], scale_bits);
sig_init_delay(accSensorSignal, delay_acc);
sig_init_weight(accSensorSignal, weight_acc, 31);
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);
//Mu Skalierung und in globale Variable schreiben
int scale = pow(2, scale_bits) - 1;
mu = lms_mu * scale;
// 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);
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);
// 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;
for (int i = 0; i < number_coefficients; i++) {
pointer_delay_line.ptr_start[i] = 0;
pointer_filter_coefficients.ptr_start[i] = 0;
}
}
// Data d(cSensor) is signal + noise
// x (accSensor) is reference noise signal
// C-Sensor (d) = Corrupted Signal (Desired Signal + Corruption Noise Signal)
// Acc-Sensor (x) = Reference Noise Signal
void calc(
SingleSignalPath *cSensorSignal,
SingleSignalPath *accSensorSignal,
OutputMode output_mode,
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
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
int16_t volatile chess_storage(DMB) *acc_sensor_input, //Pointer auf Input-Port im Shared Memory
int16_t volatile chess_storage(DMB) *output_port //Pointer auf Output-Port im Shared Memory
){
//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(DMA) c_sensor_32[BLOCK_LEN]; //Speicherbereich für 32-Bit C-Sensor Input
static int chess_storage(DMA) acc_sensor_32[BLOCK_LEN]; //Speicherbereich für 32-Bit Acc-Sensor Input
static int chess_storage(DMA) c_sensor_pre[BLOCK_LEN]; //Speicherbereich für C-Sensor Preemphasis Input
static int chess_storage(DMA) acc_sensor_pre[BLOCK_LEN]; //Speicherbereich für Acc-Sensor Preemphasis Input
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
static int chess_storage(DMB) filter_accumulator[BLOCK_LEN]; //Speicherbereich für Akkumulator Output des adaptiven Filters
static int chess_storage(DMB) output_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;
// Pointer auf Sample-Speicherbereiche legen - wird nicht benötigt, wenn allgemeine allgemein Arrays für Blockverarbeitung verwendet werden (Array -> automatisch Pointer)
// static int chess_storage(DMA) *pointer_c_sensor_pre =c_sensor_pre;
// static int chess_storage(DMA) *pointer_filter_accumulator =acc_sensor_pre;
// static int chess_storage(DMB) *pointer_output_32=output_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;
c_sensor_32[i] = ((int) c_sensor_input[i]) << BITSHIFT_16_TO_32;
acc_sensor_32[i] = ((int) acc_sensor_input[i]) << BITSHIFT_16_TO_32;
}
// 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];
c_sensor_pre[i] = c_sensor_32[i];
acc_sensor_pre[i] = acc_sensor_32[i];
}
// 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]);
write_buffer_dmb(&pointer_delay_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
acc_block_filt[0]= sig_calc_fir_lpdsp32_single(&ptr_fir_lms_delay_line, &ptr_fir_lms_coeffs);
filter_accumulator[0] = apply_fir_filter(&pointer_delay_line, &pointer_filter_coefficients);
// Output-Signal berechnen -> C-Sensor Sample - Akkumulator Output des adaptiven Filters
out_32[0] = c_block_pre[0] - acc_block_filt[0];
output_32[0] = c_sensor_pre[0] - filter_accumulator[0];
// Filterkoeffizienten adaptieren
adapt_coeffs_lpdsp32_single_v1(&ptr_fir_lms_delay_line, &ptr_fir_lms_coeffs, out_32[0]);
update_filter_coefficients(&pointer_delay_line, &pointer_filter_coefficients, 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
{
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)
output_port[i] = rnd_saturate(to_accum(output_32[i]) >> BITSHIFT_16_TO_32);
}
}