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

Code gekürzt - kompilliert

Browse Source
This commit is contained in:
Patrick Hangl
2026-01-14 15:57:21 +01:00
parent f89dda4374
commit bd94b0e79f
2 changed files with 54 additions and 475 deletions
Download Patch File Download Diff File Expand all files Collapse all files

102
main.c
View File

@@ -1,10 +1,11 @@
// BLOCK LEN 1 und MAX_FIR_COEFFS 64 werden vom Compiler mitgegeben
//#define SIMULATE //#define SIMULATE
#ifdef SIMULATE #ifdef SIMULATE
#include <stdio.h> #include <stdio.h>
#endif #endif
#define BLOCK_LEN 1 // define block length for processing - currently only 1 is supported
#include <stdint.h> #include <stdint.h>
#include "signalProcessing/include/signal_path.h" #include "signalProcessing/include/signal_path.h"
@@ -15,48 +16,32 @@
// Shared Memory von ARM und DSP definieren // Shared Memory von ARM und DSP definieren
#define INPUT_PORT0_ADD 0x800000 // Feste Adressen für Eingangsdaten im Shared Memory #define INPUT_PORT0_ADD 0x800000 // Feste Adressen für Eingangsdaten im Shared Memory
//#define INPUT_PORT1_ADD INPUT_PORT0_ADD + 2 //DMB - warum auskommentiert? #define OUTPUT_PORT_ADD (INPUT_PORT0_ADD + 16) // Feste Adressen für Ausgangsdatensdaten im Shared Memory, 16 Byte von Eingangsadresse Weg
#define OUTPUT_PORT_ADD (INPUT_PORT0_ADD + 16) // Feste Adressen für Ausgangsdatensdaten im Shared Memory, 16 Byte von Eingangsadresse Weg (PS: 2* for 2 channels)
//Chess Compiler spezifisch: Interrupt-Register festlegen um ARM zu kontaktieren nach fertiger Berechnung (PS: Define the interrupt register to notify the ARM of a completed operation) //Chess Compiler spezifisch: Interrupt-Register festlegen um ARM zu kontaktieren nach fertiger Berechnung
volatile static unsigned char chess_storage(DMIO:CSS_CMD) CssCmdGen; volatile static unsigned char chess_storage(DMIO:CSS_CMD) css_cmd_flag;
// Interrupt-Flag, welche von ARM gesetzt wird, wenn eine Berechnung gewünscht ist // Interrupt-Flag, welche von ARM gesetzt wird, wenn eine Berechnung gewünscht ist
static volatile int actionRequired; static volatile int action_required;
// Structs anlegen für die Signalpfade - hier werden Konfigurationen abgelegt(signal_path.h) // Structs anlegen für die Signalpfade - hier werden Konfigurationen abgelegt(signal_path.h)
static SingleSignalPath cSensorSignal; static SingleSignalPath corrupted_signal;
static SingleSignalPath accSensorSignal; static SingleSignalPath reference_noise_signal;
// Umschaltung zwischen sampleweiser und blockweiser Verarbeitung static volatile int16_t chess_storage(DMB:INPUT_PORT0_ADD) input_port[4]; //Array mit 4x16 Bit Einträgen auf 2x32 Bit Registern - nur die ersten 2 werden genutzt
// Sampleweise Verarbeitung: Adresse aus Shared Memory wird direkt verwendet static volatile int16_t chess_storage(DMB:OUTPUT_PORT_ADD) output_port[4]; //Array mit 4x16 Bit Einträgen auf 2x32 Bit Registern - alle werden genutzt
// Blockweise Verarbeitung: Blöcke kopiert und verarbeitet? Offensichtlicch nicht genutzt bisher static volatile int16_t chess_storage(DMB) *input_pointer_0;
#if BLOCK_LEN == 1 static volatile int16_t chess_storage(DMB) *input_pointer_1;
static volatile int16_t chess_storage(DMB:INPUT_PORT0_ADD) intputPort[4]; //TODO: if BLOCK_LEN >1 is used, the data is interleaved: ch0ch1, ch0ch1 .... chess_storage(DMA % alignof(int)) ? static volatile int16_t chess_storage(DMB) *output_pointer;
//static volatile int16_t chess_storage(DMB:INPUT_PORT1_ADD) intputPort1[BLOCK_LEN]; static volatile int16_t chess_storage(DMB) *sample_pointer;
static volatile int16_t chess_storage(DMB:OUTPUT_PORT_ADD) outputPort[4]; static volatile int16_t chess_storage(DMB) sample; //Speicherplatz für Ergebnis der calc()-Funktion
static volatile int16_t chess_storage(DMB) *inPtr0;
static volatile int16_t chess_storage(DMB) *inPtr1;
static volatile int16_t chess_storage(DMB) *outPtr;
static volatile int16_t chess_storage(DMB) sample;
static volatile int16_t chess_storage(DMB) *sample_ptr;
#else
// Int-Array für Blockverarbeitung im Shared Memory DMA anlegen (Eingabe)
static int16_t chess_storage(DMA) intputPort[BLOCK_LEN]; //chess_storage(DMA:INPUT_PORT_ADD) TODO: volatile? chess_storage(DMA % alignof(int))
//static int16_t chess_storage(DMA) intputPort1[BLOCK_LEN]; //chess_storage(DMA:INPUT_PORT_ADD)
// Int-Array für Blockverarbeitung im Shared Memory DMA anlegen (Ausgabe)
static int16_t chess_storage(DMB) outputPort[BLOCK_LEN]; // chess_storage(DMB:OUTPUT_PORT_ADD) TODO: determine output port add
#endif
//void isr0() ist eine Interrupt Service Routine Funktion, welche als C Funktion deklariert wird //void isr0() ist eine Interrupt Service Routine Funktion, welche als C Funktion deklariert wird
// property (isr) ist Chess Compiler spezifisch und kennzeichnet eine Funktion als Interrupt Service Routine // property (isr) ist Chess Compiler spezifisch und kennzeichnet eine Funktion als Interrupt Service Routine
//wird Interrupt getriggert, wird actionRequired auf 1 gesetzt - etwas muss dannpassieren //wird Interrupt getriggert, wird action_required auf 1 gesetzt - etwas muss dannpassieren
extern "C" void isr0() property (isr) { extern "C" void isr0() property (isr) {
actionRequired = 1; action_required = 1;
} }
#ifdef __chess__
extern "C"
#endif
int main(void) { int main(void) {
// Enum, welcher den Ausgabemodus definiert - wird in calc()-Funktion verwendet // Enum, welcher den Ausgabemodus definiert - wird in calc()-Funktion verwendet
@@ -65,11 +50,11 @@ int main(void) {
// Alle 0 bis auf b[0] -> einfacher Gain auf 0,75 // Alle 0 bis auf b[0] -> einfacher Gain auf 0,75
double b0[5]={0.75, 0., 0., 0., 0.}; double b0[5]={0.75, 0., 0., 0., 0.};
double b1[5]={0.75, 0., 0., 0., 0.}; double b1[5]={0.75, 0., 0., 0., 0.};
int N_lms_fir_coeffs = MAX_FIR_COEFFS; // 64 Koeffizienten für ANR int coefficients = MAX_FIR_COEFFS; // 64 Koeffizienten für ANR
// Signale initialisieren, oben angelegte Structs mit Parametern füllen // Signale initialisieren, oben angelegte Structs mit Parametern füllen
init( init(
&cSensorSignal, &accSensorSignal, //Signal-Structs &corrupted_signal, &reference_noise_signal, //Signal-Structs
b0, // Biqquad Koeffizienten C-Sensor b0, // Biqquad Koeffizienten C-Sensor
b1, // Biqquad Koeffizienten Acc-Sensor b1, // Biqquad Koeffizienten Acc-Sensor
2, // Sample Delay C-Sensor 2, // Sample Delay C-Sensor
@@ -77,20 +62,9 @@ int main(void) {
0.9, //Gewichtung C-Sensor 0.9, //Gewichtung C-Sensor
0.9, //Gewichtung Acc-Sensor 0.9, //Gewichtung Acc-Sensor
0.01, // Mu 0.01, // Mu
N_lms_fir_coeffs // Anzahl Filterkoeffizienten coefficients // Anzahl Filterkoeffizienten
); );
// Fixer Filter wenn nicht adaptiv
if (mode == OUTPUT_MODE_FIR){
for (int i=0; i<N_lms_fir_coeffs; i++){
#ifdef LPDSP16
ptr_fir_lms_coeffs.ptr_start[i] = ((pow(2, 15)-1) /N_lms_fir_coeffs);
#else
ptr_fir_lms_coeffs.ptr_start[i] = ((pow(2, 31)-1) /N_lms_fir_coeffs);
#endif
}
}
//Simulationsmodus mit File I/O //Simulationsmodus mit File I/O
#ifdef SIMULATE #ifdef SIMULATE
FILE *fp1 = fopen("./test/testdata/input/chirp_disturber.txt", "r"); FILE *fp1 = fopen("./test/testdata/input/chirp_disturber.txt", "r");
@@ -103,19 +77,13 @@ int main(void) {
for (int i=0; i<BLOCK_LEN; i++){ for (int i=0; i<BLOCK_LEN; i++){
fscanf(fp1, "%d", &d0); //load blocks fscanf(fp1, "%d", &d0); //load blocks
fscanf(fp2, "%d", &d1); fscanf(fp2, "%d", &d1);
intputPort[i] = (int16_t) d0; input_port[i] = (int16_t) d0;
intputPort[i+1] = (int16_t) d1; input_port[i+1] = (int16_t) d1;
} }
calc( calc(
&cSensorSignal, &accSensorSignal, &corrupted_signal, &reference_noise_signal, mode, &input_port[0], &input_port[1], output_port);
//&ptr_fir_lms_delay_line, &ptr_fir_lms_coeffs,
mode,
&intputPort[0],
&intputPort[1],
outputPort
);
for (int i=0; i<BLOCK_LEN; i++){ for (int i=0; i<BLOCK_LEN; i++){
fprintf(fp3, "%d\n", outputPort[i]); fprintf(fp3, "%d\n", output_port[i]);
} }
} }
fclose(fp1); fclose(fp1);
@@ -125,20 +93,20 @@ int main(void) {
// Hardwaremodus mit Interrupts // Hardwaremodus mit Interrupts
#else #else
enable_interrupts(); //Interrupts aktivieren enable_interrupts(); //Interrupts aktivieren
outPtr = &outputPort[1]; // Zweite Hälfte des Ausgangspuffers zuerst füllen - warum 1 statt 2? output_pointer = &output_port[1]; // Zweite Hälfte des Ausgangspuffers zuerst füllen - warum 1 statt 2? Warum generell nicht 0?
sample_ptr = &sample; sample_pointer = &sample; //Sample-Pointer wird auf Adresse des Sample-Speicherplatzes gesetzt
//Endlosschleife für Interrupt-gesteuerte Verarbeitung //Endlosschleife für Interrupt-gesteuerte Verarbeitung
actionRequired = 0; action_required = 0;
while (1){ while (1){
CssCmdGen = CSS_CMD_1; // Interrupt Bit setzen um ARM zu signalisieren, dass der DSP abschaltet css_cmd_flag = CSS_CMD_1; // Interrupt Bit setzen um ARM zu signalisieren, dass der DSP schläft
core_halt(); core_halt();
if (actionRequired == 1) { if (action_required == 1) {
CssCmdGen = CSS_CMD_0; // Interrupt Bit setzen um ARM zu signalisieren, dass der DSP arbeitet css_cmd_flag = CSS_CMD_0; // Interrupt Bit setzen um ARM zu signalisieren, dass der DSP arbeitet
actionRequired = 0; action_required = 0; // Action-Flag setzen, damit Loop nicht automatisch startet
outPtr = cyclic_add(outPtr, 2, outputPort, 4); //Pointer inkrementieren - was passiert hier genau? output_pointer = cyclic_add(output_pointer, 2, output_port, 4); //Output Buffer um 2 Byte inkrementieren, hat insgesamt 4 Byte - Reset wenn Ende erreicht
*outPtr = *sample_ptr; *output_pointer = *sample_pointer; //Inhalt des Sample-Pointer Ziels (Ergenis der vorherigen Berechnung) in Output-Pointer schreiben
calc(&cSensorSignal, &accSensorSignal, mode, &intputPort[1], &intputPort[0], sample_ptr); calc(&corrupted_signal, &reference_noise_signal, mode, &input_port[1], &input_port[0], sample_pointer); //16 Bit Output Sample aus 2 16 Bit Input Samples berechnen
} }
} }
#endif #endif

405
signalProcessing/signal_path.c
View File

@@ -1,33 +1,17 @@
#include "include/signal_path.h" #include "include/signal_path.h"
#define BLOCK_LEN 1
/* Global variables decleration*/ /* Global variables decleration*/
static int counter=0; static int counter=0;
static int mu; static int mu;
#ifdef LPDSP16
//static int leak=24576; //0.75
//static int leak=29491; //0.9
//static int leak=31129; //0.95 // no effect
static int leak=32735; //0.999 // (1 ? µ?)
//static int leak=32766; //0.99999
#else
//static int leak=2145336164; //0.999 // (1 ? µ?)
static int leak=2147462173; //0.999 // (1 ? µ?) static int leak=2147462173; //0.999 // (1 ? µ?)
#endif
#if BLOCK_LEN == 1
int chess_storage(DMB) fir_lms_delay_line[MAX_FIR_COEFFS]; 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; BufferPtr ptr_fir_lms_coeffs;
#else
int chess_storage(DMA%(sizeof(long long))) fir_lms_delay_line[BLOCK_LEN + MAX_FIR_COEFFS]; // The delay line for the adaptive filter
BufferPtr ptr_fir_lms_delay_line;
BufferPtr ptr_fir_lms_coeffs;
#endif
int chess_storage(DMA % (sizeof(long long))) fir_lms_coeffs[MAX_FIR_COEFFS]; // The coefficients for the adaptive filter int chess_storage(DMA % (sizeof(long long))) fir_lms_coeffs[MAX_FIR_COEFFS]; // The coefficients for the adaptive filter
@@ -84,16 +68,6 @@ int chess_storage(DMA % (sizeof(long long))) fir_lms_coeffs[MAX_FIR_COEFFS]; //
} }
#endif #endif
/*Round saturate with 16 bits return value */
int static inline rnd_saturate16(accum_t acc){ //maybe int16_fast type?
acc = to_accum( // saturate
rnd_saturate(acc << 32)
);
return rnd_saturate(acc >> 16); //round
}
int sig_init_buffer(BufferPtr *buffer, int *buffer_start_add, int length, int max_buffer_len) { int sig_init_buffer(BufferPtr *buffer, int *buffer_start_add, int length, int max_buffer_len) {
buffer->buffer_len = length; buffer->buffer_len = length;
buffer->ptr_start = buffer_start_add; buffer->ptr_start = buffer_start_add;
@@ -155,6 +129,7 @@ void static inline sig_circular_buffer_ptr_put_block(BufferPtr *buffer, int* blo
} }
} }
//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 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 // Wenn b0=1 und Rest 0 -> kein Filter weil effektiv 1*Xn
if (b0 == 1. && b1 == 0. && b2 == 0. && a1 == 0. && a2 == 0.) { if (b0 == 1. && b1 == 0. && b2 == 0. && a1 == 0. && a2 == 0.) {
@@ -178,6 +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); return sig_init_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 sig_init_weight(SingleSignalPath *signal, double weight, int scale_nbits) {
// Wenn Gewichtung 1 -> kein Effekt // Wenn Gewichtung 1 -> kein Effekt
if (weight == 1.) { if (weight == 1.) {
@@ -202,12 +178,8 @@ int sig_calc_biquad(SingleSignalPath *signal, int x) {
fract_mult(x, signal->b_preemph[0]) + fract_mult(signal->_xd[0], signal->b_preemph[1]) + 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->_xd[1], signal->b_preemph[2]) + fract_mult(signal->_yd[0], signal->b_preemph[3]) +
fract_mult(signal->_yd[1],signal->b_preemph[4]); fract_mult(signal->_yd[1],signal->b_preemph[4]);
#ifdef LPDSP16
int y = rnd_saturate16(sum << 1);
#else
int y = rnd_saturate(sum << 1); int y = rnd_saturate(sum << 1);
#endif
signal->_xd[1] = signal->_xd[0]; signal->_xd[1] = signal->_xd[0];
signal->_xd[0] = x; signal->_xd[0] = x;
@@ -238,161 +210,6 @@ int sig_calc_weight(SingleSignalPath *signal, int x) {
return rnd_saturate(acc); return rnd_saturate(acc);
} }
#if BLOCK_LEN!=1 // Block processing
/*lpdsp32 fir filter example adapted from user guide
#define NS 256 //No. of samples
#define N 64 //No. of filter coefficients or No. of tap weights
int chess_storage(DMB) y[NS]; //Output Signal
int chess_storage(DMA %(sizeof(long long))) x[NS+N-1]; //Input Signal
//Filter coefficients or tap weights
int chess_storage(DMA %(sizeof(long long))) h[N];
*/
void sig_calc_fir_lpdsp32_block(BufferPtr *ptr_fir_lms_delay_line, BufferPtr *ptr_fir_lms_coeffs, int chess_storage(DMB) *out){
//void fir(int *y, int *x, int *h)
static int chess_storage(DMA) *p_x; // pointer to the start of the last added block
static int chess_storage(DMA) *p_h; // pointer to the start of the filter coefficients
static int chess_storage(DMB) *p_y; // pointer to the output port
p_y = out;
int *px_start = ptr_fir_lms_delay_line->ptr_start;
int *ph_start = 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 coef1, coef2;
int dat1, dat2;
for(unsigned int n=0; n<BLOCK_LEN; n+=2) chess_loop_range(1,){
//p_x = x + n;
p_x = cyclic_add(px_start, n, px_start, delay_line_len);
p_h = ph_start;
#ifdef PLATFORM_GENERIC
lldecompose(*((long long *)p_h), &coef1, &coef2);
#else
lldecompose(*((long long *)p_h), coef1, coef2);
#endif
p_h+=2;
#ifdef PLATFORM_GENERIC
lldecompose(*((long long *)p_x), &dat2, &dat1);
#else
lldecompose(*((long long *)p_x), dat2, dat1);
#endif
p_x = cyclic_add(p_x, -2, px_start, delay_line_len);
accum_t sum1 = fract_mult(dat1, coef1);
accum_t sum2 = fract_mult(dat2, coef1);
sum1 += fract_mult(dat2 , coef2);
sum1 = to_accum(rnd_saturate(sum1));
for(int k=2; k < n_coeff; k+=2) chess_loop_range(1,){
#ifdef PLATFORM_GENERIC
lldecompose(*((long long *)p_x), &dat2, &dat1);
#else
lldecompose(*((long long *)p_x), dat2, dat1);
#endif
p_x = cyclic_add(p_x, -2, px_start, delay_line_len);
sum2 += fract_mult(dat1, coef2);
sum2 = to_accum(rnd_saturate(sum2));
#ifdef PLATFORM_GENERIC
lldecompose(*((long long *)p_h), &coef1, &coef2);
#else
lldecompose(*((long long *)p_h), coef1, coef2);
#endif
p_h+=2;
sum1 += fract_mult(dat1, coef1);
sum2 += fract_mult(dat2, coef1);
sum1 += fract_mult(dat2, coef2);
sum1 = to_accum(rnd_saturate(sum1));
}
#ifdef PLATFORM_GENERIC
lldecompose(*((long long *)p_x), &dat2, &dat1);
#else
lldecompose(*((long long *)p_x), dat2, dat1);
#endif
sum2 += fract_mult(dat1, coef2);
sum2 = to_accum(rnd_saturate(sum2));
*p_y++ = extract_high(sum2);
*p_y++ = extract_high(sum1);
}
}
void sig_calc_fir_generic_block(BufferPtr *ptr_fir_lms_delay_line, BufferPtr *ptr_fir_lms_coeffs, int chess_storage(DMB) *out){
static int chess_storage(DMA) *p_x; // pointer to the start of the last added block
static int chess_storage(DMA) *p_h; // pointer to the start of the filter coefficients
static int chess_storage(DMB) *p_y; // pointer to the output port
static int coef1, coef2;
static int dat1, dat2;
p_x = ptr_fir_lms_delay_line->ptr_current;
p_h = ptr_fir_lms_coeffs->ptr_current;
p_y = out;
for(int n=0; n<BLOCK_LEN; n+=2)
{
p_x = cyclic_add(ptr_fir_lms_delay_line->ptr_current, n, ptr_fir_lms_delay_line->ptr_start, ptr_fir_lms_delay_line->buffer_len); // can be done in increments of two, assuming the buffer pointer increment is even
accum_t sum = to_accum(0);
for(int k=0; k < ptr_fir_lms_coeffs->buffer_len; k+=2) chess_loop_range(1,)
{
sum += fract_mult(p_x[0] , p_h[k]);
sum += fract_mult(p_x[1] , p_h[k+1]);
sum = to_accum(rnd_saturate(sum));
p_x = cyclic_add(p_x, -2, ptr_fir_lms_delay_line->ptr_start, ptr_fir_lms_delay_line->buffer_len); // can be done in increments of two, assuming the buffer pointer increment is even
}
*p_y++ = extract_high(sum);
}
}
/* "out" is actually an input to the function and is the output of the fir_lms filter system*/
void adapt_coeffs_lpdsp32_block(BufferPtr *ptr_fir_lms_delay_line, BufferPtr *ptr_fir_lms_coeffs, int out){ // only works for even delay line sample pointers!!
int *p_x = ptr_fir_lms_delay_line->ptr_current; // pointer to the start of the last added block - TODO: doublecheck this - might be wrong because the pointer actually points to the end of the block!
int *p_x_start = ptr_fir_lms_delay_line->ptr_start;
int *p_h = ptr_fir_lms_coeffs->ptr_current; // pointer to the start of the filter coefficients
int delay_line_len = ptr_fir_lms_delay_line->buffer_len;
int n_coeff = ptr_fir_lms_coeffs->buffer_len;
int prod0, x0, x1, h0, h1;
// Calculate the first term of the coefficient adaption
accum_t acc_C = fract_mult(mu, out);
prod0 = rnd_saturate(acc_C);
//acc_D = fract_mult(mu, out1);
//prod1 = rnd_saturate(acc_C);
for (int i=0; i<n_coeff; i+=2) chess_loop_range(1, ){
// Calculate the coefficient wise adaption
// utilize dual load and dual pointer update
// load first sample and coefficient
#ifdef PLATFORM_GENERIC
lldecompose(*((long long *)p_h), &h0, &h1);
#else
lldecompose(*((long long *)p_h), h0, h1);
#endif
accum_t acc_A = to_accum(h0);
accum_t acc_B = to_accum(h1);
#ifdef PLATFORM_GENERIC
lldecompose(*((long long *)p_x), &x0, &x1);
#else
lldecompose(*((long long *)p_x), x0, x1);
#endif
p_x = cyclic_add(p_x, -2, p_x_start, delay_line_len); // can be done in increments of two, assuming the buffer pointer increment is even
// initialize accumulators with old coefficients, calculate the adaptions and accumulate
acc_A += fract_mult(prod0, x0); // TODO: This can be further optimized by using all 4 available accums!
acc_B += fract_mult(prod0, x1);
// update the current filter coefficients - dual rnd_sat; dual store
*((long long *)p_h) = llcompose(rnd_saturate(acc_A), rnd_saturate(acc_B));// load/store hazard ! - 1nop
p_h+=2;
}
}
#else
int inline sig_calc_fir_lpdsp32_single(BufferPtrDMB chess_storage(DMB) *ptr_fir_lms_delay_line, BufferPtr *ptr_fir_lms_coeffs){ int inline sig_calc_fir_lpdsp32_single(BufferPtrDMB chess_storage(DMB) *ptr_fir_lms_delay_line, BufferPtr *ptr_fir_lms_coeffs){
// Calculate the fir filter output on x to get the canceller // Calculate the fir filter output on x to get the canceller
@@ -436,11 +253,7 @@ int inline sig_calc_fir_lpdsp32_single(BufferPtrDMB chess_storage(DMB) *ptr_fir_
// Calculate the output sample // Calculate the output sample
acc1_C = acc1_A + acc1_B; acc1_C = acc1_A + acc1_B;
//out32 = rnd_saturate(acc1_A); //out32 = rnd_saturate(acc1_A);
#ifdef LPDSP16
return rnd_saturate16(acc1_C);
#else
return rnd_saturate(acc1_C); return rnd_saturate(acc1_C);
#endif
} }
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 adapt_coeffs_lpdsp32_single_v1(BufferPtrDMB chess_storage(DMB) *ptr_fir_lms_delay_line, BufferPtr *ptr_fir_lms_coeffs, int out){
@@ -461,11 +274,8 @@ void static inline adapt_coeffs_lpdsp32_single_v1(BufferPtrDMB chess_storage(DMB
// Calculate the first term of the coefficient adaption // Calculate the first term of the coefficient adaption
accum_t acc_C = fract_mult(mu, out); accum_t acc_C = fract_mult(mu, out);
#ifdef LPDSP16
prod = rnd_saturate16(acc_C);
#else
prod = rnd_saturate(acc_C); prod = rnd_saturate(acc_C);
#endif
/* Abschätzung cycles per 2 coefficient: /* Abschätzung cycles per 2 coefficient:
dual load coeffs: 1 dual load coeffs: 1
single load tab value: 2 single load tab value: 2
@@ -484,13 +294,8 @@ void static inline adapt_coeffs_lpdsp32_single_v1(BufferPtrDMB chess_storage(DMB
acc_A = to_accum(h0); acc_A = to_accum(h0);
acc_B = to_accum(h1); acc_B = to_accum(h1);
#ifdef LPDSP16
acc_A += fract_mult(prod, *p_x0) << 16; // TODO: This could be further optimized by using all 4 available accums?
acc_B += fract_mult(prod, *p_x1) << 16;
#else
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); // TODO: This could be further optimized by using all 4 available accums?
acc_B += fract_mult(prod, *p_x1); acc_B += fract_mult(prod, *p_x1);
#endif
p_x0 = cyclic_add(p_x0, -2, px_start, delay_line_len); p_x0 = cyclic_add(p_x0, -2, px_start, delay_line_len);
p_x1 = cyclic_add(p_x1, -2, px_start, delay_line_len); p_x1 = cyclic_add(p_x1, -2, px_start, delay_line_len);
@@ -501,78 +306,6 @@ void static inline adapt_coeffs_lpdsp32_single_v1(BufferPtrDMB chess_storage(DMB
} }
} }
void static inline adapt_coeffs_lpdsp32_single_leaky(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 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;
accum_t acc_A, acc_B;
// Calculate the first term of the coefficient adaption
accum_t acc_C = fract_mult(mu, out);
#ifdef LPDSP16
prod = rnd_saturate16(acc_C);
#else
prod = rnd_saturate(acc_C);
#endif
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
acc_A = fract_mult(h0, leak); // leaky
acc_B = fract_mult(h1, leak);
acc_A += fract_mult(prod, *p_x0); // TODO: This could be further optimized by using all 4 available accums?
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
#ifdef LPDSP16
*((long long *)p_h0) = llcompose(rnd_saturate16(acc_A), rnd_saturate16(acc_B));//load/store hazard ! - 1 nop is needed
#else
*((long long *)p_h0) = llcompose(rnd_saturate(acc_A), rnd_saturate(acc_B));//load/store hazard ! - 1 nop is needed
#endif
p_h0+=2;
}
}
void adapt_coeffs_generic_single(BufferPtrDMB chess_storage(DMB) *ptr_fir_lms_delay_line, BufferPtr *ptr_fir_lms_coeffs, int out){
int *p_h0 = ptr_fir_lms_coeffs->ptr_start; //coeff load pointer
int chess_storage(DMB) *p_x0 = ptr_fir_lms_delay_line->ptr_current; // chess_storage(DMB)
int prod;
accum_t acc_A, acc_B;
// Calculate the first term of the coefficient adaption
accum_t acc_C = fract_mult(mu, out);
prod = rnd_saturate(acc_C);
for (int i=0; i< ptr_fir_lms_delay_line->buffer_len; i++){
// Calculate the coefficient wise adaption
acc_A = to_accum(p_h0[i]);
acc_A += fract_mult(prod, *p_x0);
p_x0 = cyclic_add(p_x0, -1, ptr_fir_lms_delay_line->ptr_start, ptr_fir_lms_delay_line->buffer_len);
p_h0[i]=rnd_saturate(acc_A);
}
}
#endif
void init( void init(
SingleSignalPath *cSensorSignal, SingleSignalPath *cSensorSignal,
SingleSignalPath *accSensorSignal, SingleSignalPath *accSensorSignal,
@@ -585,11 +318,7 @@ void init(
double lms_mu, double lms_mu,
int lms_fir_num_coeffs int lms_fir_num_coeffs
){ ){
#ifdef LPDSP16
int scale_bits=15;
#else
int scale_bits=31; int scale_bits=31;
#endif
// C-Sensor Initialisierung: Biquad, Delay, Weight skalieren und in Struct schreiben // 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_preemph_coef(cSensorSignal, b_c[0], b_c[1], b_c[2], b_c[3], b_c[4], scale_bits);
@@ -605,14 +334,9 @@ void init(
int scale = pow(2, scale_bits) - 1; int scale = pow(2, scale_bits) - 1;
mu = lms_mu * scale; mu = lms_mu * scale;
// Buffer Initialisierung (Delay Line und Koeffizienten) und anschließend alle Werte auf 0 setzen // Buffer Initialisierung (Delay Line und Koeffizienten) und anschließend alle Werte auf 0 setzen
#if BLOCK_LEN == 1
sig_init_buffer_DMB(&ptr_fir_lms_delay_line, fir_lms_delay_line, lms_fir_num_coeffs, MAX_FIR_COEFFS); 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); sig_init_buffer(&ptr_fir_lms_coeffs, fir_lms_coeffs, lms_fir_num_coeffs, MAX_FIR_COEFFS);
#else
sig_init_buffer(&ptr_fir_lms_delay_line, fir_lms_delay_line, lms_fir_num_coeffs + BLOCK_LEN, BLOCK_LEN + MAX_FIR_COEFFS);
sig_init_buffer(&ptr_fir_lms_coeffs, fir_lms_coeffs, lms_fir_num_coeffs, MAX_FIR_COEFFS);
#endif
for (int i = 0; i < lms_fir_num_coeffs; i++) { for (int i = 0; i < lms_fir_num_coeffs; i++) {
ptr_fir_lms_delay_line.ptr_start[i] = 0; ptr_fir_lms_delay_line.ptr_start[i] = 0;
ptr_fir_lms_coeffs.ptr_start[i] = 0; ptr_fir_lms_coeffs.ptr_start[i] = 0;
@@ -624,17 +348,9 @@ void init(
void calc( void calc(
SingleSignalPath *cSensorSignal, SingleSignalPath *cSensorSignal,
SingleSignalPath *accSensorSignal, SingleSignalPath *accSensorSignal,
// BufferPtrDMB *ptr_fir_lms_delay_line,
// BufferPtr *ptr_fir_lms_coeffs,
OutputMode output_mode, OutputMode output_mode,
#if BLOCK_LEN != 1
int16_t *cSensor,
int16_t *accSensor,
#else
int16_t volatile chess_storage(DMB) *cSensor, int16_t volatile chess_storage(DMB) *cSensor,
int16_t volatile chess_storage(DMB) *accSensor, int16_t volatile chess_storage(DMB) *accSensor,
#endif
int16_t volatile chess_storage(DMB) *out_16 int16_t volatile chess_storage(DMB) *out_16
){ ){
@@ -651,137 +367,32 @@ void calc(
static int chess_storage(DMB) *p_out_32=out_32; static int chess_storage(DMB) *p_out_32=out_32;
#ifdef LPDSP16
for (uint32_t i=0; i<BLOCK_LEN; i++) chess_loop_range(1,){
cSensor_32[i]= (int) cSensor[i] ;
accSensor_32[i]= (int) accSensor[i];
}
#else //LPDDSP32
for (uint32_t i=0; i<BLOCK_LEN; i++) chess_loop_range(1,){ for (uint32_t i=0; i<BLOCK_LEN; i++) chess_loop_range(1,){
cSensor_32[i] = ((int) cSensor[i]) << BITSHIFT_16_TO_32; cSensor_32[i] = ((int) cSensor[i]) << BITSHIFT_16_TO_32;
accSensor_32[i] = ((int) accSensor[i]) << BITSHIFT_16_TO_32; accSensor_32[i] = ((int) accSensor[i]) << BITSHIFT_16_TO_32;
} }
#endif
// Apply bitshift, calculate the pre emphasis filter, delay and weight to each channel // Apply bitshift, calculate the pre emphasis filter, delay and weight to each channel
//#define PRE_FILTER
#ifdef PRE_FILTER
int x_csensor_emph, x_accsensor_emph, x_csensor_emph_delay, x_accsensor_emph_delay;
for (uint32_t i=0; i<BLOCK_LEN; i++) chess_loop_range(1,){
x_csensor_emph = sig_calc_biquad(cSensorSignal, cSensor_32);
x_accsensor_emph = sig_calc_biquad(accSensorSignal, accSensor_32);
x_csensor_emph_delay = sig_delay_buffer_load_and_get(cSensorSignal, x_csensor_emph);
x_accsensor_emph_delay = sig_delay_buffer_load_and_get(accSensorSignal, x_accsensor_emph);
c_block_pre[i] = sig_calc_weight(cSensorSignal, x_csensor_emph_delay);
acc_block_pre[i] = sig_calc_weight(accSensorSignal, x_accsensor_emph_delay);
}
#else
for (uint32_t i=0; i<BLOCK_LEN; i++) chess_loop_range(1,){ for (uint32_t i=0; i<BLOCK_LEN; i++) chess_loop_range(1,){
c_block_pre[i] = cSensor_32[i]; c_block_pre[i] = cSensor_32[i];
acc_block_pre[i] = accSensor_32[i]; acc_block_pre[i] = accSensor_32[i];
} }
#endif
// Calculate the output in dependency of the selected output mode // apply lms filter on cSensor signal
switch (output_mode)
{
case OUTPUT_MODE_C_SENSOR:
for (uint32_t i=0; i<BLOCK_LEN; i++){
out_32[i] = c_block_pre[i];
}
break;
case OUTPUT_MODE_ACC_SENSOR:
for (uint32_t i=0; i<BLOCK_LEN; i++){
out_32[i] = acc_block_pre[i];
}
break;
case OUTPUT_MODE_FIR: //output filtered cSensor signal
#if BLOCK_LEN == 1
// Increment the buffer pointer and set the current sample to the delay line
sig_cirular_buffer_ptr_put_sample_DMB(&ptr_fir_lms_delay_line, c_block_pre[0]);
out_32[0] = sig_calc_fir_lpdsp32_single(&ptr_fir_lms_delay_line, &ptr_fir_lms_coeffs);
#else // Block processing
// Put the next block to the buffer
sig_circular_buffer_ptr_put_block(&ptr_fir_lms_delay_line, c_block_pre);
// Calculate the fir filter output
sig_calc_fir_lpdsp32_block(&ptr_fir_lms_delay_line, &ptr_fir_lms_coeffs, out_32);
// Increment the buffer pointer to get ready for the next block
//sig_cirular_buffer_ptr_increment(&lms->delay_line, BLOCK_LEN);
#endif
break;
case OUTPUT_MODE_FIR_LMS: // apply lms filter on cSensor signal
#if BLOCK_LEN == 1
// Increment the buffer pointer and set the current sample to the delay line // Increment the buffer pointer and set the current sample to the delay line
sig_cirular_buffer_ptr_put_sample_DMB(&ptr_fir_lms_delay_line, acc_block_pre[0]); sig_cirular_buffer_ptr_put_sample_DMB(&ptr_fir_lms_delay_line, acc_block_pre[0]);
//*ptr_fir_lms_delay_line.ptr_current = acc_block_pre[0];
//ptr_fir_lms_delay_line.ptr_current = cyclic_add(ptr_fir_lms_delay_line.ptr_current, 1, ptr_fir_lms_delay_line.ptr_start, ptr_fir_lms_delay_line.buffer_len);
// Calculate the fir filter output on acc to get the canceller // Calculate the fir filter output on acc to get the canceller
acc_block_filt[0]= sig_calc_fir_lpdsp32_single(&ptr_fir_lms_delay_line, &ptr_fir_lms_coeffs); 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 // Calculate the ouptut signal by c_block_pre - acc_block_filt
out_32[0] = c_block_pre[0] - acc_block_filt[0]; out_32[0] = c_block_pre[0] - acc_block_filt[0];
//if (counter >= 0){ //TODO: implement this and make it configurable
// Calculate the coefficient adaptation // Calculate the coefficient adaptation
//adapt_coeffs_generic_single(&ptr_fir_lms_delay_line, &ptr_fir_lms_coeffs, out_32[0]);
adapt_coeffs_lpdsp32_single_v1(&ptr_fir_lms_delay_line, &ptr_fir_lms_coeffs, out_32[0]); adapt_coeffs_lpdsp32_single_v1(&ptr_fir_lms_delay_line, &ptr_fir_lms_coeffs, out_32[0]);
//counter=0;
// }
// else{
// counter++;
// }
#else // Block processing
// Put the next block to the buffer
sig_circular_buffer_ptr_put_block(&ptr_fir_lms_delay_line, acc_block_pre);
// Calculate the fir filter output on acc to get the canceller
sig_calc_fir_lpdsp32_block(&ptr_fir_lms_delay_line, &ptr_fir_lms_coeffs, acc_block_filt);
// Calculate the ouptut signal by c_block_pre - acc_block_filt
for (int i=0; i<BLOCK_LEN; i++) chess_flatten_loop
{
//sig_cirular_buffer_ptr_put_sample(&lms->delay_line, acc_block_pre[i]);
//acc_block_filt[i]= sig_calc_fir_lpdsp32_single(lms);
out_32[i] = c_block_pre[i] - acc_block_filt[i]; // 15 cycles with 4 samples/block
// adapt the coefficients with respect to the last sample in the block
}
//adapt_coeffs_lpdsp32_single(lms, out_32[1]);
adapt_coeffs_lpdsp32_block(&ptr_fir_lms_delay_line, &ptr_fir_lms_coeffs, out_32[0]);
// Increment the buffer pointer to get ready for the next block
//sig_cirular_buffer_ptr_increment(&lms->delay_line, BLOCK_LEN);
#endif
break;
case OUTPUT_MODE_FIR_LMS_LEAKY: // apply lms filter on cSensor signal
// Increment the buffer pointer and set the current sample to the delay line
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
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
out_32[0] = c_block_pre[0] - acc_block_filt[0];
//if (counter >= 0){ //TODO: implement this and make it configurable
// Calculate the coefficient adaptation
//adapt_coeffs_generic_single(&ptr_fir_lms_delay_line, &ptr_fir_lms_coeffs, out_32[0]);
adapt_coeffs_lpdsp32_single_leaky(&ptr_fir_lms_delay_line, &ptr_fir_lms_coeffs, out_32[0]);
break;
default: // MUTED
for (uint32_t i=0; i<BLOCK_LEN; i++){
out_32[i] = 0;
}
break;
}
// TODO: Add a couple of biqads after ANC // TODO: Add a couple of biqads after ANC
for (uint32_t i=0; i<BLOCK_LEN; i++) chess_flatten_loop for (uint32_t i=0; i<BLOCK_LEN; i++) chess_flatten_loop
{ {
#ifdef LPDSP16
out_16[i] = (int16_t) out_32[i];
#else
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) 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)
#endif }
}
//out_16[0] = cSensor[0];
} }