Patrick Hangl
389 lines
17 KiB
C
389 lines
17 KiB
C
#include <stdio.h>
|
|
#include <stdint.h>
|
|
#include "include/signal_path.h"
|
|
#define BLOCK_LEN 1
|
|
|
|
//Globale Variable setzen
|
|
static int counter=0;
|
|
static int mu;
|
|
|
|
static int leak=2147462173; //0.999 // (1 ? �?)
|
|
|
|
// Int Arrays für Delay Line (Acc-Sensor Samples) sowie Filterkoeffizienten anlegen
|
|
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_sample_line;
|
|
BufferPtr pointer_coefficient_line;
|
|
|
|
#ifdef PLATFORM_GENERIC
|
|
// lpdsp32 functionallity moddeling functions
|
|
accum_t fract_mult(int a, int b){
|
|
long int a_long = a;
|
|
long int b_long = b;
|
|
return (b_long * a_long);
|
|
}
|
|
accum_t to_accum(int a){
|
|
long int a_long = (long int) a;
|
|
return a_long << 31;
|
|
}
|
|
int rnd_saturate(accum_t a){
|
|
return a >> 31;
|
|
}
|
|
int extract_high(accum_t a){
|
|
return a >> 31;
|
|
}
|
|
void lldecompose(unsigned long long l, int* int1, int* int2){
|
|
*int2 = (int)(l >> 32);
|
|
*int1 = (int)(l);
|
|
}
|
|
uint64_t llcompose(int a, int b) {
|
|
uint64_t result = (uint64_t)b; // Assign b to the higher 32 bits of the result
|
|
result <<= 32; // Shift the higher 32 bits to the left
|
|
result |= (uint32_t)a; // Bitwise OR operation with the lower 32 bits of a
|
|
return result;
|
|
}
|
|
// unsigned long long llcompose(int a, int b){
|
|
// unsigned long long l;
|
|
// l = a << 32;
|
|
// l |= b;
|
|
// return l;
|
|
//}
|
|
int* cyclic_add(int *ptr, int i_pp, int *ptr_start, int buffer_len){
|
|
int *p_ptr=ptr;
|
|
for (int i=0; i < abs(i_pp); i+=1){ // end of buffer wraparound
|
|
if (i_pp > 0){
|
|
p_ptr ++;
|
|
if (p_ptr >= ptr_start + buffer_len){
|
|
p_ptr=ptr_start;
|
|
}
|
|
}
|
|
else{ // start of buffer wraparound
|
|
p_ptr--;
|
|
if (p_ptr < ptr_start){
|
|
p_ptr=ptr_start + (buffer_len -1);
|
|
}
|
|
}
|
|
}
|
|
return p_ptr;
|
|
}
|
|
#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;
|
|
buffer->ptr_start = buffer_start_add;
|
|
buffer->ptr_current = buffer_start_add;
|
|
// initialize delay line with 0
|
|
for (int i = 0; i < length; i++) {
|
|
buffer_start_add[i] = 0;
|
|
}
|
|
if (length<max_buffer_len){
|
|
return 0;
|
|
}
|
|
else{
|
|
return 1;
|
|
}
|
|
}
|
|
|
|
//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;
|
|
// initialize delay line with 0
|
|
for (int i = 0; i < length; i++) {
|
|
buffer_start_add[i] = 0;
|
|
}
|
|
if (length<max_buffer_len){
|
|
return 0;
|
|
}
|
|
else{
|
|
return 1;
|
|
}
|
|
}
|
|
|
|
//Ü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 (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
|
|
}
|
|
|
|
//Initialisierungsfunktion für Biquad Filter Koeffizienten
|
|
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;
|
|
}
|
|
else{
|
|
signal->preemph_activated = 1; // Schreibe Eintrag in Struct
|
|
signal->_preemph_scale_nbits = scale_bits; // Schreibe Eintrag in Struct - wieviel Bits wird skaliert
|
|
int scale = pow(2, scale_bits) - 1; //2^n -1 Skalierung
|
|
// Skaliere Koeffizienten zu Interger und schreibe Eintrag in Struct
|
|
signal->b_preemph[0] = b0 * scale;
|
|
signal->b_preemph[1] = b1 * scale;
|
|
signal->b_preemph[2] = b2 * scale;
|
|
signal->b_preemph[3] = a1 * scale;
|
|
signal->b_preemph[4] = a2 * scale;
|
|
}
|
|
}
|
|
|
|
//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 set_weight(SingleSignalPath *signal, double weight, int scale_nbits) {
|
|
// Wenn Gewichtung 1 -> kein Effekt
|
|
if (weight == 1.) {
|
|
signal->weight_actived = 0;
|
|
}
|
|
// Wenn Gewichtung != 1 -> Zu Integer skalieren und Eintrag in Struct schreiben
|
|
else{
|
|
signal->weight_actived = 1;
|
|
int scale = pow(2, scale_nbits) - 1;
|
|
signal->weight = weight * scale;
|
|
signal->_weight_scale_nbits = scale_nbits;
|
|
}
|
|
}
|
|
|
|
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 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;
|
|
accum_t acc_fir_1 = to_accum(0);
|
|
accum_t acc_fir_2 = to_accum(0);
|
|
accum_t acc_fir;
|
|
|
|
// 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 Sample-Line
|
|
w0 = *p_w; //Koeffizient 1 aus Koeffizienten Array
|
|
p_w++; //Koeffizienten-Pointer inkrementieren
|
|
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 Sample-Line
|
|
w1 = *p_w; //Koeffizient 2 aus Koeffizienten Array
|
|
p_w++; //Koeffizienten-Pointer inkrementieren
|
|
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
|
|
}
|
|
// Akkumulatoren addieren um das Filterergebnis zu erhalten
|
|
acc_fir = acc_fir_1 + acc_fir_2;
|
|
return rnd_saturate(acc_fir);
|
|
}
|
|
|
|
void static inline update_filter_coefficients(FILE *fp, BufferPtrDMB chess_storage(DMB) *pointer_sample_line, BufferPtr *pointer_coefficient_line, int output){
|
|
|
|
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 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_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
|
|
|
|
for (int i=0; i< n_coeff; i+=2) chess_loop_range(1,){
|
|
// Filterkoeffizienten vom 64 Bit Format am Ort wo der p_w0 Pointer hinzeigt in 2 32-Bit Werte zerlegen - 1 Cycle
|
|
lldecompose(*((long long *)p_w0), w0, w1);
|
|
// Filter Koeffizienten in Accum-Format bringen (oberste 32 Bit, Rest Nullen) - 2 Cycle?
|
|
acc_w0 = to_accum(w0);
|
|
acc_w1 = to_accum(w1);
|
|
// Filterkoeffizienten mit Korrekturterm*Acc-Sensor-Sample updaten - 1 Cycle
|
|
acc_w0 += fract_mult(correction, *p_x0);
|
|
acc_w1 += fract_mult(correction, *p_x1);
|
|
// Filterkoeffizienten in .txt-File schreiben
|
|
//fprintf(fp, "%d, %d, ", rnd_saturate(acc_w0), rnd_saturate(acc_w1));
|
|
//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
|
|
}
|
|
// Absatz im Filterkoeffizienten-File
|
|
//fprintf(fp, "\n");
|
|
}
|
|
|
|
void initialize_signal(
|
|
SingleSignalPath *c_sensor_signal_t,
|
|
SingleSignalPath *acc_sensor_signal_t,
|
|
double *b_c,
|
|
double *b_acc,
|
|
int delay_c,
|
|
int delay_acc,
|
|
double weight_c,
|
|
double weight_acc,
|
|
double lms_mu,
|
|
int number_coefficients
|
|
){
|
|
int scale_bits=31;
|
|
|
|
// C-Sensor Initialisierung: Biquad, Delay, Weight skalieren und in Struct schreiben
|
|
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
|
|
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_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_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 calculate_output(
|
|
FILE *fp,
|
|
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_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) 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 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, Kopie der Samples in funktionseigenem neuen Speicherbereich ablegen (Kein Pointer mehr!)
|
|
for (uint32_t i=0; i<BLOCK_LEN; i++) chess_loop_range(1,){
|
|
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_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
|
|
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 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(fp, &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
|
|
{
|
|
output_port[i] = rnd_saturate(to_accum(output_32[i]) >> BITSHIFT_16_TO_32);
|
|
}
|
|
|
|
}
|
|
|