140 lines
5.9 KiB
C
140 lines
5.9 KiB
C
#define SIGNAL_PATH_H
|
|
|
|
#include <math.h>
|
|
#include <stdint.h>
|
|
|
|
#define MAX_DELAY_SAMPS 16
|
|
#if BLOCK_LEN > MAX_FIR_COEFFS
|
|
#error "BLOCK_LEN must be smaller than MAX_FIR_COEFFS"
|
|
#endif
|
|
#define BITSHIFT_16_TO_32 16
|
|
|
|
static const int block_len=BLOCK_LEN; // TODO: save this an an cm3 accessible location
|
|
|
|
#ifdef PLATFORM_GENERIC
|
|
typedef long int accum_t;
|
|
// empty Macros definitions
|
|
#define chess_storage(mem)
|
|
#define DMA
|
|
#define DMB
|
|
#define DMIO
|
|
#define chess_loop_range(a,b)
|
|
#define isr0(a)
|
|
#define chess_flatten_loop
|
|
#endif
|
|
|
|
typedef struct BufferPtr{ // used as a pointer and length storage container for cirular buffers
|
|
int buffer_len;
|
|
int *ptr_start;
|
|
int *ptr_current;
|
|
} BufferPtr;
|
|
|
|
typedef struct BufferPtrDMB{
|
|
int buffer_len;
|
|
int chess_storage(DMB) *ptr_start;
|
|
int chess_storage(DMB) *ptr_current;
|
|
} BufferPtrDMB;
|
|
|
|
/*Stuct for storage of internal state and configuration for single signal path with a biquad element, a scaling element and a delay*/
|
|
typedef struct SingleSignalPath{
|
|
int input_scale; // The scaling bitshift bits for the input signal
|
|
int x_nbit_bitshift; // The number of bits to scale the input signal
|
|
int preemph_activated; //Deactivate by initializing with coefficients {1., 0., 0., 0., 0.}
|
|
int b_preemph[5]; // Preemphasis filter coefficients
|
|
int _preemph_scale_nbits; // The number of bits used to scale the pre emphasis filter
|
|
int _xd[2]; //preemphasis biquad filter buffers
|
|
int _yd[2];
|
|
int _delay_buffer[MAX_DELAY_SAMPS]; // The delay buffer for the given signal path // chess_storage(DMA)
|
|
BufferPtr delay_buffer; // The pointers to the delay buffer and actual used length
|
|
int n_delay_samps; // The delay for the given signal path in samples
|
|
int weight_actived; //Deactivate by initializing with weight 1.0
|
|
int weight; // The weight for the given signal path
|
|
int _weight_scale_nbits; // The number of bits used to scale the weight
|
|
} SingleSignalPath;
|
|
|
|
|
|
/*Stuct for storage of internal state and configuration for an adaptive fir-lms filter*/
|
|
// typedef struct LmsFilter{
|
|
// int lms_mu; // The learning rate for the lms algorithm
|
|
// int lms_num_fir_coeffs; // Number of coefficients for the adaptive filter
|
|
// #if BLOCK_LEN == 1
|
|
// //int _delay_line[MAX_FIR_COEFFS]; // The delay line for the adaptive filter //
|
|
// BufferDMB delay_line; // The pointer to the delay line
|
|
// //int chess_storage(DMB) *ptr_delay_line_current; // The pointer to the current position in the delay line
|
|
// #else
|
|
// //int chess_storage(%(sizeof(long long))) _delay_line[BLOCK_LEN + MAX_FIR_COEFFS]; // The delay line for the adaptive filter
|
|
// BufferPtr delay_line; // The pointer to the delay line
|
|
// //int chess_storage(DMA) *ptr_delay_line_current; // The pointer to the current position in the delay line
|
|
// //int chess_storage(%(sizeof(long long))) fir_coeffs[MAX_FIR_COEFFS]; // The coefficients for the adaptive filter
|
|
// #endif
|
|
// } LmsFilter;
|
|
// #if BLOCK_LEN == 1
|
|
// int fir_lms_coeffs[MAX_FIR_COEFFS]; // The coefficients for the adaptive filter //
|
|
// #else
|
|
// int chess_storage(%(sizeof(long long))) fir_lms_coeffs[MAX_FIR_COEFFS]; // The coefficients for the adaptive filter
|
|
// #endif
|
|
|
|
#if BLOCK_LEN == 1
|
|
BufferPtr extern ptr_fir_lms_coeffs;
|
|
BufferPtrDMB extern chess_storage(DMB) ptr_fir_lms_delay_line;
|
|
int extern chess_storage(DMB) fir_lms_delay_line[MAX_FIR_COEFFS];
|
|
|
|
#else
|
|
int extern chess_storage(DMA%(sizeof(long long))) fir_lms_delay_line[BLOCK_LEN + MAX_FIR_COEFFS]; // The delay line for the adaptive filter
|
|
BufferPtr extern ptr_fir_lms_delay_line;
|
|
BufferPtr extern ptr_fir_lms_coeffs;
|
|
#endif
|
|
|
|
//int extern chess_storage(DMA % (sizeof(long long))) fir_lms_coeffs[MAX_FIR_COEFFS]; // The coefficients for the adaptive filter
|
|
|
|
// typedef struct SignalPath{
|
|
// SingleSignalPath cSensorSignal;
|
|
// SingleSignalPath accSensorSignal;
|
|
// LmsFilter lms;
|
|
// volatile int chess_storage(DMIO:INPUT_PORT_ADD) input_port;
|
|
// int chess_storage(DMIO:OUTPUT_PORT_ADD) output_port;
|
|
// } SignalPath;
|
|
|
|
typedef enum OutputMode{
|
|
OUTPUT_MODE_C_SENSOR,
|
|
OUTPUT_MODE_ACC_SENSOR,
|
|
OUTPUT_MODE_FIR_LMS,
|
|
OUTPUT_MODE_FIR,
|
|
OUTPUT_MODE_FIR_LMS_LEAKY,
|
|
}OutputMode;
|
|
|
|
// void sig_init_preemph_coef(SingleSignalPath *signal, double b0, double b1, double b2, double a1, double a2, int scale_bits);
|
|
// int sig_init_delay(SingleSignalPath *signal, int delay_samps);
|
|
// void sig_init_weight(SingleSignalPath *signal, double weight, int scale_nbits);
|
|
// void sig_init_lms(LmsFilter *signal, double lms_mu, int lms_fir_num_coeffs, int scale_bits);
|
|
// int inline sig_delay_buffer_load_and_get(SingleSignalPath *signal, int x);
|
|
// int inline sig_calc_biquad(SingleSignalPath *signal, int x); //TODO: inline ?
|
|
// int inline sig_calc_weight(SingleSignalPath *signal, int x); //TODO: inline ?
|
|
// int inline sig_calc_fir_lms_single(LmsFilter *signal, int d, int x); //TODO: inline ?
|
|
|
|
//void adapt_coeffs_lpdsp32_single(LmsFilter chess_storage(DMB) *filter, int *fir_lms_coeffs, int out);
|
|
//sig_calc_fir_lpdsp32_single(BufferPtr *ptr_fir_lms_delay_line, BufferPtr *ptr_fir_lms_coeffs)
|
|
|
|
|
|
|
|
// top level init and calc functions
|
|
void init(
|
|
SingleSignalPath *cSensorSignal, SingleSignalPath *accSensorSignal,
|
|
//BufferPtrDMB chess_storage(DMB) *ptr_fir_lms_delay_line, BufferPtr *ptr_fir_lms_coeffs,
|
|
double *b_c, double *b_acc, int delay_c, int delay_acc, double weight_c, double weight_acc, double lms_mu, int lms_fir_num_coeffs);
|
|
void calc(
|
|
SingleSignalPath *cSensorSignal, SingleSignalPath *accSensorSignal,
|
|
//BufferPtrDMB chess_storage(DMB) *ptr_fir_lms_delay_line, BufferPtr *ptr_fir_lms_coeffs,
|
|
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) *accSensor,
|
|
#endif
|
|
int16_t volatile chess_storage(DMB) *out_16
|
|
);
|
|
|
|
|