Initial import
This commit is contained in:
nzasch
292
demodulator.c
Normal file
292
demodulator.c
Normal file
@@ -0,0 +1,292 @@
|
||||
/* Title: demodulatore */
|
||||
|
||||
#include <stdint.h>
|
||||
#include <math.h>
|
||||
|
||||
#ifdef __XC16
|
||||
#include <dsp.h>
|
||||
#include <libq.h>
|
||||
#endif
|
||||
|
||||
#include "../phy/phy.h" // a chi cazzo serve?
|
||||
|
||||
#include "../peripheral/adc.h"
|
||||
#include "../peripheral/gpio.h"
|
||||
|
||||
// serve? direi di si
|
||||
#include "modem_types.h"
|
||||
|
||||
#include "demodulator.h"
|
||||
|
||||
// buffer dei simboli ricevuti
|
||||
uint8_t rx_symbol_buffer[MAX_SYMBOL_BUFFER_SIZE];
|
||||
uint8_t rx_symbol_buffer_index, rx_symbol_buffer_lenght;
|
||||
|
||||
uint8_t rx_sample_buffers_buffer[MAX_BUFFERS_PER_SYMBOL], rx_sample_buffer_index;
|
||||
|
||||
uint8_t volatile process_input_sample_buffer;
|
||||
|
||||
detector_symbol detector_constellation[MAX_MODULATION_ALPHABET_SIZE], symbol_error, min_symbol_distance;
|
||||
|
||||
// zona del simbolo da esaminare:
|
||||
uint8_t rx_symbol_detected_part_offset, rx_symbol_detected_part_length;
|
||||
|
||||
// NCO
|
||||
fractional tanlock_nco_rotor_inc;
|
||||
|
||||
// #####################################################
|
||||
|
||||
void demodulator_setup(void) {
|
||||
// RX
|
||||
set_rx_GPIOs();
|
||||
|
||||
// setup rotore NCO demodulatore, posizionalo al centro della costellazione
|
||||
tanlock_nco_rotor_inc = get_angular_frequency(find_constellation_center_values(mod.constellation).frequency);
|
||||
|
||||
demodulator_prepare_constellation_symbols(mod.constellation, detector_constellation);
|
||||
min_symbol_distance = demodulator_find_min_symbol_distance(detector_constellation);
|
||||
|
||||
// zona del simbolo da esaminare:
|
||||
// due quarti centrali del simbolo, da verificare
|
||||
// si lavora solo su mezzo simbolo, ma si regge una sfasatura di +-1/4 di simbolo
|
||||
rx_symbol_detected_part_offset = mod.buffers_per_symbol / 4;
|
||||
rx_symbol_detected_part_length = mod.buffers_per_symbol / 2;
|
||||
}
|
||||
|
||||
// trova valori medii dei parametri di modulazione
|
||||
|
||||
symbol find_constellation_center_values(symbol *constellation) {
|
||||
uint8_t i;
|
||||
symbol max, min, center;
|
||||
max.amplitude = max.frequency = max.phase = 0;
|
||||
min.frequency = min.phase = min.amplitude = 0xFFFF;
|
||||
|
||||
for (i = 0; i < mod.alphabet_size; i++) {
|
||||
if (constellation[i].frequency < min.frequency) min.frequency = constellation[i].frequency;
|
||||
if (constellation[i].frequency > max.frequency) max.frequency = constellation[i].frequency;
|
||||
|
||||
if (constellation[i].phase < min.phase) min.phase = constellation[i].phase;
|
||||
if (constellation[i].phase > max.phase) max.phase = constellation[i].phase;
|
||||
|
||||
if (constellation[i].amplitude < min.amplitude) min.amplitude = constellation[i].amplitude;
|
||||
if (constellation[i].amplitude > max.amplitude) max.amplitude = constellation[i].amplitude;
|
||||
|
||||
}
|
||||
center.frequency = min.frequency + ((max.frequency - min.frequency) / 2);
|
||||
center.phase = min.phase + ((max.phase - min.phase) / 2);
|
||||
center.amplitude = min.amplitude + ((max.amplitude - min.amplitude) / 2);
|
||||
return center;
|
||||
}
|
||||
|
||||
int16_t get_angular_frequency(uint16_t frequency) {
|
||||
int32_t tmpfreq = (int32_t) frequency << 16;
|
||||
return __builtin_divsd(tmpfreq, SAMPLING_RATE);
|
||||
}
|
||||
|
||||
void FIR_init(void) {
|
||||
#ifdef __DSP_LIB__
|
||||
// inizializza filtri passa basso
|
||||
FIRDelayInit(&ph_det_lpFilterQ);
|
||||
FIRDelayInit(&ph_det_lpFilterI);
|
||||
FIRDelayInit(&ph_det_lpFilterFreq);
|
||||
#endif
|
||||
}
|
||||
|
||||
void demodulator_sample_buffer_demodulate(fractional *buffer, detector_symbol* outsymbol) {
|
||||
uint8_t i;
|
||||
fractional demodulator_frequency_diff = 0, demodulator_phase_diff = 0;
|
||||
static fractional freq_lp[1];
|
||||
_Q15_16 frequency_accu = 0, phase_accu = 0, amplitude_accu = 0;
|
||||
|
||||
for (i = 0; i < SAMPLES_PER_BUFFER; i++) {
|
||||
// ampiezza
|
||||
amplitude_accu += _Q15abs(buffer[i]);
|
||||
|
||||
// fase
|
||||
demodulator_phase_diff = demodulator_phase_detector(buffer[i]);
|
||||
phase_accu += demodulator_phase_diff;
|
||||
|
||||
// frequenza
|
||||
demodulator_frequency_diff = demodulator_frequency_detector(demodulator_phase_diff);
|
||||
|
||||
#ifdef __DSP_LIB__
|
||||
FIR(1, &freq_lp[0], &demodulator_frequency_diff, &ph_det_lpFilterFreq);
|
||||
#endif
|
||||
frequency_accu = frequency_accu + freq_lp[0];
|
||||
}
|
||||
|
||||
// media
|
||||
outsymbol->amplitude = __builtin_divsd(amplitude_accu, SAMPLES_PER_BUFFER);
|
||||
outsymbol->phase = __builtin_divsd(phase_accu, SAMPLES_PER_BUFFER);
|
||||
outsymbol->frequency = __builtin_divsd(frequency_accu, SAMPLES_PER_BUFFER);
|
||||
}
|
||||
|
||||
fractional demodulator_phase_detector(fractional sig_in) {
|
||||
static fractional tanlock_nco_rotor_pos;
|
||||
static fractional Q_mix_lp[1], I_mix_lp[1];
|
||||
// MIX
|
||||
fractional I_mix, Q_mix, phasediff;
|
||||
// moltiplica per segnale iniettato 171 cicli
|
||||
|
||||
// perche' 15? perche i campioni sono da 12 bit
|
||||
I_mix = _Q16shr((__builtin_mulss(sig_in, _Q15sinPI(tanlock_nco_rotor_pos))), 16);
|
||||
Q_mix = _Q16shr((__builtin_mulss(sig_in, _Q15cosPI(tanlock_nco_rotor_pos))), 16);
|
||||
|
||||
#ifdef __DSP_LIB__
|
||||
// filtra l'i/q 270 cicli
|
||||
FIR(1, &I_mix_lp[0], &I_mix, &ph_det_lpFilterI);
|
||||
FIR(1, &Q_mix_lp[0], &Q_mix, &ph_det_lpFilterQ);
|
||||
#endif
|
||||
|
||||
// incrementa il rotore dell'oscillatore locale
|
||||
tanlock_nco_rotor_pos += tanlock_nco_rotor_inc; // non deve saturare!
|
||||
|
||||
// calcola arcotangente, circa 420 cicli
|
||||
phasediff = _Q15atan2circle(I_mix_lp[0], Q_mix_lp[0]);
|
||||
// phasediff = _Q15atan2circle(I_mix, Q_mix);
|
||||
|
||||
return phasediff;
|
||||
}
|
||||
|
||||
fractional demodulator_frequency_detector(fractional phase) {
|
||||
static fractional demodulator_last_phase, demodulator_last_delta;
|
||||
fractional demodulator_phase_delta = 0, demodulator_phase_delta_tmp = 0;
|
||||
|
||||
demodulator_phase_delta_tmp = _Q15sub(phase, demodulator_last_phase);
|
||||
|
||||
// antiskew
|
||||
if (_Q15abs(demodulator_phase_delta_tmp) < MAX_PHASE_SKEW) demodulator_phase_delta = demodulator_phase_delta_tmp;
|
||||
else {
|
||||
demodulator_phase_delta = demodulator_last_delta;
|
||||
}
|
||||
|
||||
// storici
|
||||
demodulator_last_phase = phase;
|
||||
demodulator_last_delta = demodulator_phase_delta;
|
||||
|
||||
return demodulator_phase_delta;
|
||||
}
|
||||
|
||||
// ####### symbolame ##########
|
||||
|
||||
void demodulator_prepare_constellation_symbols(symbol *constellation, detector_symbol *demod_constellation) {
|
||||
uint8_t i;
|
||||
for (i = 0; i < mod.alphabet_size; i++) {
|
||||
demod_constellation[i].frequency = _Q15sub(get_angular_frequency(constellation[i].frequency), tanlock_nco_rotor_inc);
|
||||
demod_constellation[i].phase = constellation[i].phase;
|
||||
demod_constellation[i].amplitude = constellation[i].amplitude;
|
||||
}
|
||||
}
|
||||
|
||||
detector_symbol demodulator_find_min_symbol_distance(detector_symbol *demod_constellation) {
|
||||
// find minimum distance between symbols
|
||||
uint8_t i, k;
|
||||
detector_symbol tmp_distance, min_distance;
|
||||
min_distance.frequency = min_distance.phase = min_distance.amplitude = 0x7FFF;
|
||||
for (i = 0; i < mod.alphabet_size; i++) {
|
||||
for (k = 0; k < mod.alphabet_size; k++) {
|
||||
if (k != i) {
|
||||
tmp_distance.frequency = _Q15abs(_Q15sub(demod_constellation[i].frequency, demod_constellation[k].frequency));
|
||||
if (tmp_distance.frequency < min_distance.frequency) min_distance.frequency = tmp_distance.frequency;
|
||||
|
||||
tmp_distance.phase = _Q15sub(demod_constellation[i].phase, demod_constellation[k].phase);
|
||||
tmp_distance.amplitude = _Q15sub(demod_constellation[i].amplitude, demod_constellation[k].amplitude);
|
||||
}
|
||||
}
|
||||
}
|
||||
return min_distance;
|
||||
}
|
||||
|
||||
// trova il simbolo piu vicino nella costellazione
|
||||
|
||||
uint8_t demodulator_find_nearest_symbol(detector_symbol *in_symbol, detector_symbol *target_constellation, detector_symbol *moderror) {
|
||||
detector_symbol distance, min_distance = {0x7FFF, 0x7FFF, 0x7FFF};
|
||||
uint8_t i, nearest_symbol = 0;
|
||||
for (i = 0; i < mod.alphabet_size; i++) {
|
||||
if (mod.frequency_keying) {
|
||||
distance.frequency = _Q15abs(_Q15sub(target_constellation[i].frequency, in_symbol->frequency));
|
||||
if (distance.frequency < min_distance.frequency) {
|
||||
min_distance.frequency = distance.frequency;
|
||||
nearest_symbol = i;
|
||||
moderror->frequency = distance.frequency;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
return nearest_symbol;
|
||||
}
|
||||
|
||||
// trova deviazione dal simbolo
|
||||
|
||||
uint8_t find_symbol_deviation(uint8_t *serie, uint8_t mean, uint8_t len) {
|
||||
int16_t sum_deviation = 0;
|
||||
uint8_t i;
|
||||
for (i = 0; i < len; ++i)
|
||||
sum_deviation += (serie[i] - mean)*(serie[i] - mean);
|
||||
return sqrt(sum_deviation / len);
|
||||
}
|
||||
// valore ricorrente
|
||||
|
||||
uint8_t demodulator_most_frequent_value(uint8_t *serie, uint8_t range, uint8_t len) {
|
||||
uint8_t i, prezzemolo = 0, prezzemolo_occurrences = 0;
|
||||
uint8_t occurrencez[MAX_MODULATION_ALPHABET_SIZE] = {0};
|
||||
// conta ricorrenze per ogni simbolo
|
||||
for (i = 0; i < len; i++) {
|
||||
occurrencez[serie[i]]++;
|
||||
}
|
||||
|
||||
// trova il simbolo con la ricorrenza maggiore
|
||||
for (i = 0; i < range; i++) {
|
||||
if (occurrencez[i] > prezzemolo_occurrences) {
|
||||
prezzemolo = i;
|
||||
prezzemolo_occurrences = occurrencez[i];
|
||||
}
|
||||
}
|
||||
return prezzemolo;
|
||||
}
|
||||
|
||||
fractional _Q15atan2circle(fractional Y, fractional X) {
|
||||
// ARCTAN a cerchio pieno
|
||||
fractional atang = 0, offset = 0, temp = 0;
|
||||
fractional X_sign = X;
|
||||
|
||||
if (Y <= 0) {
|
||||
temp = Y;
|
||||
Y = X;
|
||||
X = _Q15neg(temp);
|
||||
offset = _Q15add(offset, 16384);
|
||||
}
|
||||
|
||||
if (_Q15abs(Y) <= _Q15abs(X)) {
|
||||
if (X_sign < 0) {
|
||||
temp = _Q15add(X, Y);
|
||||
Y = _Q15sub(Y, X);
|
||||
} else {
|
||||
temp = _Q15sub(X, Y);
|
||||
Y = _Q15add(Y, X);
|
||||
}
|
||||
|
||||
X = temp;
|
||||
offset = _Q15add(offset, 8192);
|
||||
}
|
||||
// circa 300 cy
|
||||
#ifdef _libq_h_
|
||||
atang = _Q15atanYByXByPI(Y, X); // vuole il large code model ? no! 2.2.2 del man
|
||||
#endif
|
||||
if (atang < 0) {
|
||||
atang = _Q15sub(atang, offset);
|
||||
} else {
|
||||
atang = _Q15add(atang, offset);
|
||||
}
|
||||
return atang;
|
||||
}
|
||||
|
||||
// devel
|
||||
/*
|
||||
fractional delay_phase_detector(fractional sig_in) {
|
||||
fractional phased_sig_in, phasediff;
|
||||
phased_sig_in = delay(sig_in, 2, delay_buffer);
|
||||
phasediff = _Q15atan2circle(phased_sig_in, sig_in);
|
||||
return phasediff;
|
||||
}
|
||||
*/
|
||||
Reference in New Issue
Block a user