273 lines
11 KiB
C
273 lines
11 KiB
C
#include <stdint.h>
|
|
#include <arm_math.h>
|
|
#include <stm32g4xx_ll_cordic.h>
|
|
|
|
#include "FIRFilterCode.h"
|
|
#include "bassofono.h"
|
|
#include "rx.h"
|
|
|
|
int32_t rx_signal;
|
|
|
|
// filtro 2
|
|
arm_fir_decimate_instance_q31 st2_filter_I_struct;
|
|
arm_fir_decimate_instance_q31 st2_filter_Q_struct;
|
|
q31_t st2_filter_I_state[ST2_FILTER_BLOCK_SIZE + ST2_FILTER_TAP_NUM - 1];
|
|
q31_t st2_filter_Q_state[ST2_FILTER_BLOCK_SIZE + ST2_FILTER_TAP_NUM - 1];
|
|
|
|
// filtro audio
|
|
arm_fir_instance_q31 audio_filter_struct;
|
|
q31_t audio_filter_state[AUDIO_FILTER_BLOCK_SIZE + AUDIO_FILTER_TAP_NUM - 1];
|
|
int32_t audio_filter_coeffs[AUDIO_FILTER_TAP_NUM];
|
|
|
|
int32_t ms_hbf_filter_taps[MS_HBF_TAP_NUM] = {
|
|
-181552, 0, 3414100, 0,
|
|
-16841686, 0, 80782761, 134217728,
|
|
80782761, 0, -16841686, 0,
|
|
3414100, 0, -181552,
|
|
};
|
|
|
|
// fir 127t 0,02fc 32bit x SSB
|
|
int32_t st2_filter_taps[ST2_FILTER_TAP_NUM] = {
|
|
-1126183, -1060446, -1001030, -944190,
|
|
-885669, -820733, -744204, -650507,
|
|
-533714, -387602, -205707, 18609,
|
|
292094, 621539, 1013699, 1475225,
|
|
2012590, 2632009, 3339367, 4140147,
|
|
5039355, 6041454, 7150298, 8369068,
|
|
9700219, 11145427, 12705542, 14380553,
|
|
16169554, 18070720, 20081290, 22197560,
|
|
24414882, 26727671, 29129426, 31612749,
|
|
34169384, 36790257, 39465521, 42184622,
|
|
44936354, 47708936, 50490086, 53267102,
|
|
56026952, 58756364, 61441918, 64070145,
|
|
66627626, 69101088, 71477504, 73744193,
|
|
75888914, 77899962, 79766252, 81477413,
|
|
83023862, 84396881, 85588686, 86592486,
|
|
87402540, 88014198, 88423939, 88629403,
|
|
88629403, 88423939, 88014198, 87402540,
|
|
86592486, 85588686, 84396881, 83023862,
|
|
81477413, 79766252, 77899962, 75888914,
|
|
73744193, 71477504, 69101088, 66627626,
|
|
64070145, 61441918, 58756364, 56026952,
|
|
53267102, 50490086, 47708936, 44936354,
|
|
42184622, 39465521, 36790257, 34169384,
|
|
31612749, 29129426, 26727671, 24414882,
|
|
22197560, 20081290, 18070720, 16169554,
|
|
14380553, 12705542, 11145427, 9700219,
|
|
8369068, 7150298, 6041454, 5039355,
|
|
4140147, 3339367, 2632009, 2012590,
|
|
1475225, 1013699, 621539, 292094,
|
|
18609, -205707, -387602, -533714,
|
|
-650507, -744204, -820733, -885669,
|
|
-944190, -1001030, -1060446, -1126183,
|
|
};
|
|
|
|
// fir 127t 0,08fc 32bit x DC & AM
|
|
int32_t st2_wide_taps[ST2_FILTER_TAP_NUM] = {
|
|
-97978, 335568, 778895, 1221284,
|
|
1647428, 2035927, 2358700, 2581561,
|
|
2666121, 2573011, 2266239, 1718351,
|
|
915904, -135341, -1406144, -2841077,
|
|
-4357945, -5849758, -7189482, -8237567,
|
|
-8851951, -8900018, -8271675, -6892581,
|
|
-4736355, -1834599, 1716434, 5753496,
|
|
10049098, 14319768, 18239836, 21460339,
|
|
23632183, 24432297, 23591167, 20919867,
|
|
16334581, 9876614, 1726037, -7792580,
|
|
-18213471, -28942584, -39280046, -48451497,
|
|
-55646916, -60065020, -60960793, -57693344,
|
|
-49771152, -36891699, -18972776, 3826929,
|
|
31099731, 62194426, 96235614, 132156305,
|
|
168742396, 204686835, 238650645, 269327486,
|
|
295508153, 316141338, 330387150, 337660283,
|
|
337660283, 330387150, 316141338, 295508153,
|
|
269327486, 238650645, 204686835, 168742396,
|
|
132156305, 96235614, 62194426, 31099731,
|
|
3826929, -18972776, -36891699, -49771152,
|
|
-57693344, -60960793, -60065020, -55646916,
|
|
-48451497, -39280046, -28942584, -18213471,
|
|
-7792580, 1726037, 9876614, 16334581,
|
|
20919867, 23591167, 24432297, 23632183,
|
|
21460339, 18239836, 14319768, 10049098,
|
|
5753496, 1716434, -1834599, -4736355,
|
|
-6892581, -8271675, -8900018, -8851951,
|
|
-8237567, -7189482, -5849758, -4357945,
|
|
-2841077, -1406144, -135341, 915904,
|
|
1718351, 2266239, 2573011, 2666121,
|
|
2581561, 2358700, 2035927, 1647428,
|
|
1221284, 778895, 335568, -97978,
|
|
};
|
|
|
|
int32_t audio_filter_coeffs[AUDIO_FILTER_TAP_NUM];
|
|
|
|
// mix
|
|
|
|
void rx_mixer(uint16_t *in, uint16_t size, q31_t *I_out, q31_t *Q_out, int32_t nco_freq){
|
|
uint16_t i = 0, j = 0, k = 0;
|
|
static int32_t phase_accu;
|
|
int32_t scaled_in, sin, cos, tmp;
|
|
q31_t I_tmp2[ST2_BUFFER_SIZE], Q_tmp2[ST2_BUFFER_SIZE];
|
|
// rb
|
|
static uint8_t mixed_samples_ringbuffer_index, mixed_samples_2m_ringbuffer_index;
|
|
static q31_t I_mixed_rb[MIXED_SAMPLES_RINGBUFFER_SIZE], Q_mixed_rb[MIXED_SAMPLES_RINGBUFFER_SIZE];
|
|
static q31_t I_mixed_2m_rb[MIXED_SAMPLES_2M_RINGBUFFER_SIZE], Q_mixed_2m_rb[MIXED_SAMPLES_2M_RINGBUFFER_SIZE];
|
|
|
|
LL_CORDIC_WriteData(CORDIC, phase_accu);
|
|
|
|
while(i < size){
|
|
// in 12 + 1 bit
|
|
scaled_in = (int32_t)in[i]-4096;
|
|
// printf("%d\n", scaled_in);
|
|
tmp = LL_CORDIC_ReadData(CORDIC);
|
|
phase_accu += nco_freq;
|
|
LL_CORDIC_WriteData(CORDIC, phase_accu);
|
|
sin = (int16_t)tmp;
|
|
cos = tmp>>16;
|
|
I_mixed_rb[mixed_samples_ringbuffer_index] = (int32_t)sin * scaled_in;
|
|
Q_mixed_rb[mixed_samples_ringbuffer_index] = (int32_t)cos * scaled_in;
|
|
|
|
// MS HB FIR
|
|
if(i & 1) {
|
|
I_mixed_2m_rb[mixed_samples_2m_ringbuffer_index] = hb_fir15(I_mixed_rb, mixed_samples_ringbuffer_index, MIXED_SAMPLES_RINGBUFFER_SIZE_MASK, ms_hbf_filter_taps);
|
|
Q_mixed_2m_rb[mixed_samples_2m_ringbuffer_index] = hb_fir15(Q_mixed_rb, mixed_samples_ringbuffer_index, MIXED_SAMPLES_RINGBUFFER_SIZE_MASK, ms_hbf_filter_taps);
|
|
if(j & 1) {
|
|
I_tmp2[k] = hb_fir15(I_mixed_2m_rb, mixed_samples_2m_ringbuffer_index, MIXED_SAMPLES_2M_RINGBUFFER_SIZE_MASK, ms_hbf_filter_taps);
|
|
Q_tmp2[k] = hb_fir15(Q_mixed_2m_rb, mixed_samples_2m_ringbuffer_index, MIXED_SAMPLES_2M_RINGBUFFER_SIZE_MASK, ms_hbf_filter_taps);
|
|
k++;
|
|
}
|
|
ringbuf_increment(&mixed_samples_2m_ringbuffer_index, MIXED_SAMPLES_2M_RINGBUFFER_SIZE_MASK);
|
|
j++;
|
|
}
|
|
ringbuf_increment(&mixed_samples_ringbuffer_index, MIXED_SAMPLES_RINGBUFFER_SIZE_MASK);
|
|
i++;
|
|
}
|
|
|
|
arm_fir_decimate_q31(&st2_filter_I_struct, I_tmp2, I_out, ST2_FILTER_BLOCK_SIZE);
|
|
arm_fir_decimate_q31(&st2_filter_Q_struct, Q_tmp2, Q_out, ST2_FILTER_BLOCK_SIZE);
|
|
tmp = LL_CORDIC_ReadData(CORDIC);
|
|
}
|
|
|
|
|
|
// demodulatori
|
|
|
|
void am_demodulator(q31_t *in_I, q31_t *in_Q, uint16_t size, q31_t *out){
|
|
uint16_t i;
|
|
q31_t acc0, acc1;
|
|
i = 0;
|
|
while(i < size){
|
|
acc0 = (q31_t) (((q63_t) in_I[i] * in_I[i]) >> 33);
|
|
acc1 = (q31_t) (((q63_t) in_Q[i] * in_Q[i]) >> 33);
|
|
out[i] = acc0 + acc1;
|
|
i++;
|
|
}
|
|
}
|
|
|
|
void ssb_demodulator(q31_t *in_I, q31_t *in_Q, uint16_t size, q31_t *out, q31_t nco_freq){
|
|
uint16_t i;
|
|
static int32_t phase_accu;
|
|
int32_t sin, cos, tmp, I, Q;
|
|
LL_CORDIC_WriteData(CORDIC, phase_accu);
|
|
i = 0;
|
|
|
|
while(i < size){
|
|
tmp = LL_CORDIC_ReadData(CORDIC);
|
|
phase_accu += nco_freq;
|
|
LL_CORDIC_WriteData(CORDIC, phase_accu);
|
|
sin = (int16_t)tmp;
|
|
cos = tmp>>16;
|
|
I = sat_mult_q31(sin*(volume*32), in_I[i]);
|
|
Q = sat_mult_q31(cos*(volume*32), in_Q[i]);
|
|
if (modulation == MOD_USB) out[i] = (I + Q);
|
|
else {
|
|
out[i] = (I - Q);
|
|
}
|
|
i++;
|
|
}
|
|
tmp = LL_CORDIC_ReadData(CORDIC);
|
|
}
|
|
|
|
void dc_demodulator(q31_t *in, uint16_t size, q31_t *out){
|
|
uint16_t i;
|
|
i = 0;
|
|
while(i < LF_BUFFER_SIZE){
|
|
out[i] = ((in[i]*(int32_t)volume))/4096;
|
|
i++;
|
|
}
|
|
|
|
}
|
|
|
|
void nco(uint16_t size, q31_t *out, q31_t nco_freq){
|
|
uint16_t i;
|
|
int32_t sin, cos, tmp;
|
|
static int32_t phase_accu;
|
|
LL_CORDIC_WriteData(CORDIC, phase_accu);
|
|
i = 0;
|
|
while(i < size){
|
|
tmp = LL_CORDIC_ReadData(CORDIC);
|
|
phase_accu += nco_freq;
|
|
LL_CORDIC_WriteData(CORDIC, phase_accu);
|
|
sin = (int16_t)tmp;
|
|
cos = tmp>>16;
|
|
out[i] = cos/volume;
|
|
i++;
|
|
}
|
|
tmp = LL_CORDIC_ReadData(CORDIC);
|
|
|
|
}
|
|
|
|
// filtri
|
|
|
|
inline q31_t hb_fir15(q31_t * samples_ringbuf, uint8_t sample_index, uint8_t buff_size_mask, q31_t * coefficients){
|
|
q63_t sum0 = 0;
|
|
sum0 += (q63_t) samples_ringbuf[(sample_index - 0) & buff_size_mask] * coefficients[0];
|
|
sum0 += (q63_t) samples_ringbuf[(sample_index - 14) & buff_size_mask] * coefficients[0];
|
|
sum0 += (q63_t) samples_ringbuf[(sample_index - 2) & buff_size_mask] * coefficients[2];
|
|
sum0 += (q63_t) samples_ringbuf[(sample_index - 12) & buff_size_mask] * coefficients[2];
|
|
sum0 += (q63_t) samples_ringbuf[(sample_index - 4) & buff_size_mask] * coefficients[4];
|
|
sum0 += (q63_t) samples_ringbuf[(sample_index - 10) & buff_size_mask] * coefficients[4];
|
|
sum0 += (q63_t) samples_ringbuf[(sample_index - 6) & buff_size_mask] * coefficients[6];
|
|
sum0 += (q63_t) samples_ringbuf[(sample_index - 8) & buff_size_mask] * coefficients[6];
|
|
sum0 += (q63_t) samples_ringbuf[(sample_index - 7) & buff_size_mask] * coefficients[7];
|
|
return (q31_t) (sum0 >> 31);
|
|
}
|
|
|
|
void st2_filter_init(void){
|
|
if((modulation == MOD_LSB) || (modulation == MOD_USB)){
|
|
arm_fir_decimate_init_q31(&st2_filter_I_struct, ST2_FILTER_TAP_NUM, ST2_DECIMATION_FACTOR, &st2_filter_taps[0], &st2_filter_I_state[0], ST2_FILTER_BLOCK_SIZE);
|
|
arm_fir_decimate_init_q31(&st2_filter_Q_struct, ST2_FILTER_TAP_NUM, ST2_DECIMATION_FACTOR, &st2_filter_taps[0], &st2_filter_Q_state[0], ST2_FILTER_BLOCK_SIZE);
|
|
} else if ((modulation == MOD_DC) || (modulation == MOD_AM)){
|
|
arm_fir_decimate_init_q31(&st2_filter_I_struct, ST2_FILTER_TAP_NUM, ST2_DECIMATION_FACTOR, &st2_wide_taps[0], &st2_filter_I_state[0], ST2_FILTER_BLOCK_SIZE);
|
|
arm_fir_decimate_init_q31(&st2_filter_Q_struct, ST2_FILTER_TAP_NUM, ST2_DECIMATION_FACTOR, &st2_wide_taps[0], &st2_filter_Q_state[0], ST2_FILTER_BLOCK_SIZE);
|
|
}
|
|
}
|
|
|
|
void audio_filter_init(void){
|
|
arm_fir_init_q31 (&audio_filter_struct, AUDIO_FILTER_TAP_NUM, audio_filter_coeffs, &audio_filter_state[0], AUDIO_FILTER_BLOCK_SIZE);
|
|
}
|
|
|
|
void audio_filter_generate_coeffs(int32_t *Coeffs, uint32_t freq, uint32_t bw, uint8_t beta){
|
|
double FPCoeff[AUDIO_FILTER_TAP_NUM + 1];
|
|
double normalized_center_freq, normalized_bw, fp_beta;
|
|
normalized_center_freq = (double)(freq*2)/DAC_SAMPLE_RATE;
|
|
normalized_bw = (double)(bw*2)/DAC_SAMPLE_RATE;
|
|
fp_beta = beta/4;
|
|
RectWinFIR(FPCoeff, AUDIO_FILTER_TAP_NUM, firBPF, normalized_center_freq, normalized_bw);
|
|
FIRFilterWindow(FPCoeff, AUDIO_FILTER_TAP_NUM, wtKAISER, fp_beta);
|
|
|
|
for( int index = 0; index < AUDIO_FILTER_TAP_NUM; index++){
|
|
Coeffs[index] = (int32_t) ( (double)(FPCoeff[index])*(double)0x7FFFFFFF );
|
|
}
|
|
}
|
|
|
|
void rx_measure_signal(q31_t *samples, uint16_t size){
|
|
static int32_t old_rx_signal;
|
|
uint32_t index;
|
|
arm_max_q31(samples, size, &old_rx_signal, &index);
|
|
old_rx_signal >>= 10;
|
|
if(old_rx_signal != rx_signal) {
|
|
rx_signal = old_rx_signal;
|
|
// rx_signal = 45;
|
|
set_changed(8);
|
|
}
|
|
}
|