bassofono/codice/Core/Src/rx.c

#include <stdint.h>
#include <arm_math.h>
#include <stm32g4xx_ll_cordic.h>

#include "FIRFilterCode.h"
#include "bassofono.h"
#include "rx.h"

q31_t rx_nco1_increment;

// RX ADC
volatile uint8_t rx_adc_buffer_ready;
uint16_t rx_adc_buffer[RX_ADC_BUFFER_SIZE];

// RX DAC
volatile uint8_t rx_dac_buffer_toggle, rx_dac_buffer_mezzo_vuoto;
q31_t prefilter_rx_dac_buffer[RX_DAC_BUFFER_SIZE];
q31_t rx_dac_buffer[2][RX_DAC_BUFFER_SIZE];

uint8_t rx_signal, rx_signal_last;

// 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[RX_AUDIO_FILTER_BLOCK_SIZE + RX_AUDIO_FILTER_TAP_NUM - 1];
int32_t audio_filter_coeffs[RX_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[RX_AUDIO_FILTER_TAP_NUM];

void ricevi(void){
               if(rx_adc_buffer_ready){
                        rx_mixer(rx_adc_buffer, RX_ADC_BUFFER_SIZE, if_I, if_Q, rx_nco1_increment);
                        rx_adc_buffer_ready = 0;
                }
                if(rx_dac_buffer_mezzo_vuoto){
                        if (canale.modulazione == MOD_DC) dc_demodulator(if_I, RX_DAC_BUFFER_SIZE, prefilter_rx_dac_buffer);
                        else if(canale.modulazione == MOD_LSB || canale.modulazione == MOD_USB || canale.modulazione == MOD_CWL || canale.modulazione == MOD_CWU) ssb_demodulator(if_I, if_Q, RX_DAC_BUFFER_SIZE, prefilter_rx_dac_buffer, NCO2_INCREMENT);
                        else if (canale.modulazione == MOD_AM) am_demodulator(if_I, if_Q, RX_DAC_BUFFER_SIZE, prefilter_rx_dac_buffer);
                        arm_fir_q31(&audio_filter_struct, prefilter_rx_dac_buffer, rx_dac_buffer[rx_dac_buffer_toggle], RX_AUDIO_FILTER_BLOCK_SIZE);
                        rx_dac_buffer_mezzo_vuoto = 0;
                }
}

// 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*(stato.volume*32), in_I[i]);
                Q = sat_mult_q31(cos*(stato.volume*32), in_Q[i]);
                if (canale.modulazione == 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 < RX_DAC_BUFFER_SIZE){
                out[i] = ((in[i]*(int32_t)stato.volume))/4096;
                i++;
        }

}

// filtri

// filtro halfband fir generato da tools/fir_generate.c 

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((canale.modulazione == MOD_LSB) || (canale.modulazione == MOD_USB || canale.modulazione == MOD_CWL || canale.modulazione == MOD_CWU)){
                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 ((canale.modulazione == MOD_DC) || (canale.modulazione == MOD_AM || canale.modulazione == MOD_FM)){
                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 rx_inizializza_filtro_audio(void){
        arm_fir_init_q31 (&audio_filter_struct, RX_AUDIO_FILTER_TAP_NUM, audio_filter_coeffs, &audio_filter_state[0], RX_AUDIO_FILTER_BLOCK_SIZE);
}

void audio_filter_generate_coeffs(int32_t *Coeffs, uint32_t freq, uint32_t bw, uint8_t beta){
        double FPCoeff[RX_AUDIO_FILTER_TAP_NUM + 1];
        double normalized_center_freq, normalized_bw, fp_beta;
        normalized_center_freq = (double)(freq*2)/RX_DAC_SAMPLE_RATE;
        normalized_bw = (double)(bw*2)/RX_DAC_SAMPLE_RATE;
        fp_beta = beta/4;
        RectWinFIR(FPCoeff, RX_AUDIO_FILTER_TAP_NUM, firBPF, normalized_center_freq, normalized_bw);
        FIRFilterWindow(FPCoeff, RX_AUDIO_FILTER_TAP_NUM, wtKAISER, fp_beta);

        for( int index = 0; index < RX_AUDIO_FILTER_TAP_NUM; index++){
                Coeffs[index] = (int32_t) ( (double)(FPCoeff[index])*(double)0x7FFFFFFF );
        }
}

void set_rx_audio_filter(void){
        audio_filter_generate_coeffs(audio_filter_coeffs, audio_filter_freq, audio_filter_bw, audio_filter_beta);
}

void set_rx_nco1_freq(int32_t freq){
        int64_t tmp_increment;
        if(canale.modulazione == MOD_USB || canale.modulazione == MOD_CWU) freq += (RX_NCO_FREQUENCY);
        else if(canale.modulazione == MOD_LSB || canale.modulazione == MOD_CWL) freq -= (RX_NCO_FREQUENCY);
        tmp_increment = ((int64_t)freq * 0x100000000LL)/RX_ADC_SAMPLE_RATE;
        rx_nco1_increment = (int32_t)tmp_increment;
}