diff options
Diffstat (limited to 'src/fmfsk.c')
| -rw-r--r-- | src/fmfsk.c | 573 |
1 files changed, 284 insertions, 289 deletions
diff --git a/src/fmfsk.c b/src/fmfsk.c index 442cd7c..b1745fa 100644 --- a/src/fmfsk.c +++ b/src/fmfsk.c @@ -26,348 +26,343 @@ along with this program; if not, see <http://www.gnu.org/licenses/>. */ +#include "fmfsk.h" + #include <assert.h> +#include <math.h> #include <stdint.h> +#include <stdio.h> #include <stdlib.h> -#include <math.h> #include <string.h> -#include <stdio.h> - -#include "fmfsk.h" -#include "modem_probe.h" #include "comp_prim.h" +#include "modem_probe.h" #define STD_PROC_BITS 96 /* * Create a new fmfsk modem instance. - * + * * int Fs - sample rate * int Rb - non-manchester bitrate * returns - new struct FMFSK on success, NULL on failure */ -struct FMFSK * fmfsk_create(int Fs,int Rb){ - assert( Fs % (Rb*2) == 0 ); /* Sample freq must be divisible by symbol rate */ - - int nbits = STD_PROC_BITS; - - /* Allocate the struct */ - struct FMFSK *fmfsk = malloc(sizeof(struct FMFSK)); - if(fmfsk==NULL) return NULL; - - /* Set up static parameters */ - fmfsk->Rb = Rb; - fmfsk->Rs = Rb*2; - fmfsk->Fs = Fs; - fmfsk->Ts = Fs/fmfsk->Rs; - fmfsk->N = nbits*2*fmfsk->Ts; - fmfsk->nmem = fmfsk->N+(fmfsk->Ts*4); - fmfsk->nsym = nbits*2; - fmfsk->nbit = nbits; - - /* Set up demod state */ - fmfsk->lodd = 0; - fmfsk->nin = fmfsk->N; - fmfsk->snr_mean = 0; - - float *oldsamps = malloc(sizeof(float)*fmfsk->nmem); - if(oldsamps == NULL){ - free(fmfsk); - return NULL; - } - for(int i=0; i<fmfsk->nmem; i++) oldsamps[i] = 0.0; - fmfsk->oldsamps = oldsamps; - - fmfsk->stats = (struct MODEM_STATS*)malloc(sizeof(struct MODEM_STATS)); - if (fmfsk->stats == NULL) { - free(oldsamps); - free(fmfsk); - return NULL; - } - - return fmfsk; +struct FMFSK *fmfsk_create(int Fs, int Rb) { + assert(Fs % (Rb * 2) == 0); /* Sample freq must be divisible by symbol rate */ + + int nbits = STD_PROC_BITS; + + /* Allocate the struct */ + struct FMFSK *fmfsk = malloc(sizeof(struct FMFSK)); + if (fmfsk == NULL) return NULL; + + /* Set up static parameters */ + fmfsk->Rb = Rb; + fmfsk->Rs = Rb * 2; + fmfsk->Fs = Fs; + fmfsk->Ts = Fs / fmfsk->Rs; + fmfsk->N = nbits * 2 * fmfsk->Ts; + fmfsk->nmem = fmfsk->N + (fmfsk->Ts * 4); + fmfsk->nsym = nbits * 2; + fmfsk->nbit = nbits; + + /* Set up demod state */ + fmfsk->lodd = 0; + fmfsk->nin = fmfsk->N; + fmfsk->snr_mean = 0; + + float *oldsamps = malloc(sizeof(float) * fmfsk->nmem); + if (oldsamps == NULL) { + free(fmfsk); + return NULL; + } + for (int i = 0; i < fmfsk->nmem; i++) oldsamps[i] = 0.0; + fmfsk->oldsamps = oldsamps; + + fmfsk->stats = (struct MODEM_STATS *)malloc(sizeof(struct MODEM_STATS)); + if (fmfsk->stats == NULL) { + free(oldsamps); + free(fmfsk); + return NULL; + } + + return fmfsk; } /* * Destroys an fmfsk modem and deallocates memory */ -void fmfsk_destroy(struct FMFSK *fmfsk){ - free(fmfsk->oldsamps); - free(fmfsk); +void fmfsk_destroy(struct FMFSK *fmfsk) { + free(fmfsk->oldsamps); + free(fmfsk); } /* * Returns the number of samples that must be fed to fmfsk_demod the next * cycle */ -uint32_t fmfsk_nin(struct FMFSK *fmfsk){ - return (uint32_t)fmfsk->nin; -} +uint32_t fmfsk_nin(struct FMFSK *fmfsk) { return (uint32_t)fmfsk->nin; } -void fmfsk_get_demod_stats(struct FMFSK *fmfsk,struct MODEM_STATS *stats){ - /* copy from internal stats, note we can't overwrite stats completely - as it has other states rqd by caller, also we want a consistent - interface across modem types for the freedv_api. - */ +void fmfsk_get_demod_stats(struct FMFSK *fmfsk, struct MODEM_STATS *stats) { + /* copy from internal stats, note we can't overwrite stats completely + as it has other states rqd by caller, also we want a consistent + interface across modem types for the freedv_api. + */ - stats->clock_offset = fmfsk->stats->clock_offset; - stats->snr_est = fmfsk->stats->snr_est; // TODO: make this SNR not Eb/No - stats->rx_timing = fmfsk->stats->rx_timing; - stats->foff = fmfsk->stats->foff; + stats->clock_offset = fmfsk->stats->clock_offset; + stats->snr_est = fmfsk->stats->snr_est; // TODO: make this SNR not Eb/No + stats->rx_timing = fmfsk->stats->rx_timing; + stats->foff = fmfsk->stats->foff; #ifndef __EMBEDDED__ - stats->neyesamp = fmfsk->stats->neyesamp; - stats->neyetr = fmfsk->stats->neyetr; - memcpy(stats->rx_eye, fmfsk->stats->rx_eye, sizeof(stats->rx_eye)); -#endif // !__EMBEDDED__ + stats->neyesamp = fmfsk->stats->neyesamp; + stats->neyetr = fmfsk->stats->neyetr; + memcpy(stats->rx_eye, fmfsk->stats->rx_eye, sizeof(stats->rx_eye)); +#endif // !__EMBEDDED__ - /* these fields not used for FSK so set to something sensible */ + /* these fields not used for FSK so set to something sensible */ - stats->sync = 0; - stats->nr = fmfsk->stats->nr; - stats->Nc = fmfsk->stats->Nc; + stats->sync = 0; + stats->nr = fmfsk->stats->nr; + stats->Nc = fmfsk->stats->Nc; } /* * Modulates nbit bits into N samples to be sent through an FM radio - * + * * struct FSK *fsk - FSK config/state struct, set up by fsk_create * float mod_out[] - Buffer for N samples of modulated FMFSK * uint8_t tx_bits[] - Buffer containing Nbits unpacked bits */ -void fmfsk_mod(struct FMFSK *fmfsk, float fmfsk_out[],uint8_t bits_in[]){ - int i,j; - int nbit = fmfsk->nbit; - int Ts = fmfsk->Ts; - - for(i=0; i<nbit; i++){ - /* Save a manchester-encoded 0 */ - if(bits_in[i] == 0){ - for(j=0; j<Ts; j++) - fmfsk_out[ j+i*Ts*2] = -1; - for(j=0; j<Ts; j++) - fmfsk_out[Ts+j+i*Ts*2] = 1; - } else { - /* Save a manchester-encoded 1 */ - for(j=0; j<Ts; j++) - fmfsk_out[ j+i*Ts*2] = 1; - for(j=0; j<Ts; j++) - fmfsk_out[Ts+j+i*Ts*2] = -1; - } +void fmfsk_mod(struct FMFSK *fmfsk, float fmfsk_out[], uint8_t bits_in[]) { + int i, j; + int nbit = fmfsk->nbit; + int Ts = fmfsk->Ts; + + for (i = 0; i < nbit; i++) { + /* Save a manchester-encoded 0 */ + if (bits_in[i] == 0) { + for (j = 0; j < Ts; j++) fmfsk_out[j + i * Ts * 2] = -1; + for (j = 0; j < Ts; j++) fmfsk_out[Ts + j + i * Ts * 2] = 1; + } else { + /* Save a manchester-encoded 1 */ + for (j = 0; j < Ts; j++) fmfsk_out[j + i * Ts * 2] = 1; + for (j = 0; j < Ts; j++) fmfsk_out[Ts + j + i * Ts * 2] = -1; } + } } /* - * Demodulate some number of FMFSK samples. The number of samples to be + * Demodulate some number of FMFSK samples. The number of samples to be * demodulated can be found by calling fmfsk_nin(). - * + * * struct FMFSK *fsk - FMFSK config/state struct, set up by fsk_create * uint8_t rx_bits[] - Buffer for nbit unpacked bits to be written * float fsk_in[] - nin samples of modualted FMFSK from an FM radio */ -void fmfsk_demod(struct FMFSK *fmfsk, uint8_t rx_bits[],float fmfsk_in[]){ - int i,j,k; - int Ts = fmfsk->Ts; - int Fs = fmfsk->Fs; - int Rs = fmfsk->Rs; - int nin = fmfsk->nin; - int N = fmfsk->N; - int nsym = fmfsk->nsym; - int nbit = fmfsk->nbit; - int nmem = fmfsk->nmem; - float *oldsamps = fmfsk->oldsamps; - int nold = nmem-nin; - COMP phi_ft,dphi_ft; /* Phase and delta-phase for fine timing estimator */ - float t; - COMP x; /* Magic fine timing angle */ - float norm_rx_timing,old_norm_rx_timing,d_norm_rx_timing,appm; - int rx_timing,sample_offset; - int next_nin; - float apeven,apodd; /* Approx. prob of even or odd stream being correct */ - float currv,mdiff,lastv; - int neyesamp; - int neyeoffset; - float eye_max; - uint8_t mbit; - float var_signal = 0, var_noise = 0, lastFabsV; - - /* Shift in nin samples */ - memmove(&oldsamps[0] , &oldsamps[nmem-nold], sizeof(float)*nold); - memcpy (&oldsamps[nold], &fmfsk_in[0] , sizeof(float)*nin ); - - /* Allocate memory for filtering */ - float *rx_filt = malloc(sizeof(float)*(nsym+1)*Ts); - - /* Integrate over Ts input symbols at every offset */ - for(i=0; i<(nsym+1)*Ts; i++){ - t=0; - /* Integrate over some samples */ - for(j=i;j<i+Ts;j++){ - t += oldsamps[j]; - } - rx_filt[i] = t; - } - - /* - * Fine timing estimation - * - * Estimate fine timing using line at Rs/2 that Manchester encoding provides - * We need this to sync up to Manchester codewords. - */ - - /* init fine timing extractor */ - phi_ft.real = 1; - phi_ft.imag = 0; - - /* Set up delta-phase */ - dphi_ft.real = cosf(2*M_PI*((float)Rs)/((float)Fs)); - dphi_ft.imag = sinf(2*M_PI*((float)Rs)/((float)Fs)); - - x.real = 0; - x.imag = 0; - - for(i=0; i<(nsym+1)*Ts; i++){ - /* Apply non-linearity */ - t = rx_filt[i]*rx_filt[i]; - - /* Shift Rs/2 down to DC and accumulate */ - x = cadd(x,fcmult(t,phi_ft)); - - /* Spin downshift oscillator */ - phi_ft = cmult(dphi_ft,phi_ft); - modem_probe_samp_c("t_phi_ft",&phi_ft,1); - } - - /* Figure out the normalized RX timing, using David's magic number */ - norm_rx_timing = atan2f(x.imag,x.real)/(2*M_PI) - .42; - rx_timing = (int)lroundf(norm_rx_timing*(float)Ts); - - old_norm_rx_timing = fmfsk->norm_rx_timing; - fmfsk->norm_rx_timing = norm_rx_timing; - - /* Estimate sample clock offset */ - d_norm_rx_timing = norm_rx_timing - old_norm_rx_timing; - - /* Filter out big jumps in due to nin change */ - if(fabsf(d_norm_rx_timing) < .2){ - appm = 1e6*d_norm_rx_timing/(float)nsym; - fmfsk->ppm = .9*fmfsk->ppm + .1*appm; - } - - /* Figure out how far offset the sample points are */ - sample_offset = (Ts/2)+Ts+rx_timing-1; - - /* Request fewer or greater samples next time, if fine timing is far - * enough off. This also makes it possible to tolerate clock offsets */ - next_nin = N; - if(norm_rx_timing > -.2) - next_nin += Ts/2; - if(norm_rx_timing < -.65) - next_nin -= Ts/2; - fmfsk->nin = next_nin; - - /* Make first diff of this round the last sample of the last round, - * for the odd stream */ - lastv = fmfsk->lodd; - lastFabsV = fabs(lastv); - apeven = 0; - apodd = 0; - for(i=0; i<nsym; i++){ - /* Sample a filtered value */ - currv = rx_filt[sample_offset+(i*Ts)]; - modem_probe_samp_f("t_symsamp",&currv,1); - mdiff = lastv - currv; - mbit = mdiff>0 ? 1 : 0; - lastv = currv; - - // Calculate the signal variance. Note that the mean is zero - var_signal += currv * currv; - - /* Calculate the variance of the noise between samples (symbols). A quick variance estimate - * without calculating mean can be done by differentiating (remove mean) and then - * dividing by 2. Fabs the samples as we are looking at how close the samples are to each - * other as if they were all the same polarity/symbol. */ - currv = fabs(currv); - var_noise += (currv - lastFabsV) * (currv - lastFabsV); - lastFabsV = currv; - - mdiff = mdiff>0 ? mdiff : 0-mdiff; - - /* Put bit in it's stream */ - if((i%2)==1){ - apeven += mdiff; - /* Even stream goes in LSB */ - rx_bits[i>>1] |= mbit ? 0x1 : 0x0; - }else{ - apodd += mdiff; - /* Odd in second-to-LSB */ - rx_bits[i>>1] = mbit ? 0x2 : 0x0; - } +void fmfsk_demod(struct FMFSK *fmfsk, uint8_t rx_bits[], float fmfsk_in[]) { + int i, j, k; + int Ts = fmfsk->Ts; + int Fs = fmfsk->Fs; + int Rs = fmfsk->Rs; + int nin = fmfsk->nin; + int N = fmfsk->N; + int nsym = fmfsk->nsym; + int nbit = fmfsk->nbit; + int nmem = fmfsk->nmem; + float *oldsamps = fmfsk->oldsamps; + int nold = nmem - nin; + COMP phi_ft, dphi_ft; /* Phase and delta-phase for fine timing estimator */ + float t; + COMP x; /* Magic fine timing angle */ + float norm_rx_timing, old_norm_rx_timing, d_norm_rx_timing, appm; + int rx_timing, sample_offset; + int next_nin; + float apeven, apodd; /* Approx. prob of even or odd stream being correct */ + float currv, mdiff, lastv; + int neyesamp; + int neyeoffset; + float eye_max; + uint8_t mbit; + float var_signal = 0, var_noise = 0, lastFabsV; + + /* Shift in nin samples */ + memmove(&oldsamps[0], &oldsamps[nmem - nold], sizeof(float) * nold); + memcpy(&oldsamps[nold], &fmfsk_in[0], sizeof(float) * nin); + + /* Allocate memory for filtering */ + float *rx_filt = malloc(sizeof(float) * (nsym + 1) * Ts); + + /* Integrate over Ts input symbols at every offset */ + for (i = 0; i < (nsym + 1) * Ts; i++) { + t = 0; + /* Integrate over some samples */ + for (j = i; j < i + Ts; j++) { + t += oldsamps[j]; } + rx_filt[i] = t; + } - /* Div by 2 to correct variance when doing via differentiation.*/ - var_noise *= 0.5; - - if(apeven>apodd){ - /* Zero out odd bits from output bitstream */ - for(i=0;i<nbit;i++) - rx_bits[i] &= 0x1; - }else{ - /* Shift odd bits into LSB and even bits out of existence */ - for(i=0;i<nbit;i++) - rx_bits[i] = (rx_bits[i]&0x2)>>1; + /* + * Fine timing estimation + * + * Estimate fine timing using line at Rs/2 that Manchester encoding provides + * We need this to sync up to Manchester codewords. + */ + + /* init fine timing extractor */ + phi_ft.real = 1; + phi_ft.imag = 0; + + /* Set up delta-phase */ + dphi_ft.real = cosf(2 * M_PI * ((float)Rs) / ((float)Fs)); + dphi_ft.imag = sinf(2 * M_PI * ((float)Rs) / ((float)Fs)); + + x.real = 0; + x.imag = 0; + + for (i = 0; i < (nsym + 1) * Ts; i++) { + /* Apply non-linearity */ + t = rx_filt[i] * rx_filt[i]; + + /* Shift Rs/2 down to DC and accumulate */ + x = cadd(x, fcmult(t, phi_ft)); + + /* Spin downshift oscillator */ + phi_ft = cmult(dphi_ft, phi_ft); + modem_probe_samp_c("t_phi_ft", &phi_ft, 1); + } + + /* Figure out the normalized RX timing, using David's magic number */ + norm_rx_timing = atan2f(x.imag, x.real) / (2 * M_PI) - .42; + rx_timing = (int)lroundf(norm_rx_timing * (float)Ts); + + old_norm_rx_timing = fmfsk->norm_rx_timing; + fmfsk->norm_rx_timing = norm_rx_timing; + + /* Estimate sample clock offset */ + d_norm_rx_timing = norm_rx_timing - old_norm_rx_timing; + + /* Filter out big jumps in due to nin change */ + if (fabsf(d_norm_rx_timing) < .2) { + appm = 1e6 * d_norm_rx_timing / (float)nsym; + fmfsk->ppm = .9 * fmfsk->ppm + .1 * appm; + } + + /* Figure out how far offset the sample points are */ + sample_offset = (Ts / 2) + Ts + rx_timing - 1; + + /* Request fewer or greater samples next time, if fine timing is far + * enough off. This also makes it possible to tolerate clock offsets */ + next_nin = N; + if (norm_rx_timing > -.2) next_nin += Ts / 2; + if (norm_rx_timing < -.65) next_nin -= Ts / 2; + fmfsk->nin = next_nin; + + /* Make first diff of this round the last sample of the last round, + * for the odd stream */ + lastv = fmfsk->lodd; + lastFabsV = fabs(lastv); + apeven = 0; + apodd = 0; + for (i = 0; i < nsym; i++) { + /* Sample a filtered value */ + currv = rx_filt[sample_offset + (i * Ts)]; + modem_probe_samp_f("t_symsamp", &currv, 1); + mdiff = lastv - currv; + mbit = mdiff > 0 ? 1 : 0; + lastv = currv; + + // Calculate the signal variance. Note that the mean is zero + var_signal += currv * currv; + + /* Calculate the variance of the noise between samples (symbols). A quick + * variance estimate without calculating mean can be done by differentiating + * (remove mean) and then dividing by 2. Fabs the samples as we are looking + * at how close the samples are to each other as if they were all the same + * polarity/symbol. */ + currv = fabs(currv); + var_noise += (currv - lastFabsV) * (currv - lastFabsV); + lastFabsV = currv; + + mdiff = mdiff > 0 ? mdiff : 0 - mdiff; + + /* Put bit in it's stream */ + if ((i % 2) == 1) { + apeven += mdiff; + /* Even stream goes in LSB */ + rx_bits[i >> 1] |= mbit ? 0x1 : 0x0; + } else { + apodd += mdiff; + /* Odd in second-to-LSB */ + rx_bits[i >> 1] = mbit ? 0x2 : 0x0; } - - /* Save last sample of int stream for next demod round */ - fmfsk->lodd = lastv; - - /* Save demod statistics */ - fmfsk->stats->Nc = 0; - fmfsk->stats->nr = 0; - - /* Clock offset and RX timing are all we know here */ - fmfsk->stats->clock_offset = fmfsk->ppm; - fmfsk->stats->rx_timing = (float)rx_timing; - - /* Zero out all of the other things */ - fmfsk->stats->foff = 0; - - /* Use moving average to smooth SNR */ - var_signal += 1E-6/3.1; var_noise += 1E-6; /* prevent NAN and bias towards -5dB SNR for zero signal inputs */ - if(fmfsk->snr_mean < 0.1) - fmfsk->snr_mean = (10.0 * log10f(var_signal / var_noise)); - else - fmfsk->snr_mean = 0.9 * fmfsk->snr_mean + 0.1 * (10.0 * log10f(var_signal / var_noise)); - fmfsk->stats->snr_est = fmfsk->snr_mean; + } + + /* Div by 2 to correct variance when doing via differentiation.*/ + var_noise *= 0.5; + + if (apeven > apodd) { + /* Zero out odd bits from output bitstream */ + for (i = 0; i < nbit; i++) rx_bits[i] &= 0x1; + } else { + /* Shift odd bits into LSB and even bits out of existence */ + for (i = 0; i < nbit; i++) rx_bits[i] = (rx_bits[i] & 0x2) >> 1; + } + + /* Save last sample of int stream for next demod round */ + fmfsk->lodd = lastv; + + /* Save demod statistics */ + fmfsk->stats->Nc = 0; + fmfsk->stats->nr = 0; + + /* Clock offset and RX timing are all we know here */ + fmfsk->stats->clock_offset = fmfsk->ppm; + fmfsk->stats->rx_timing = (float)rx_timing; + + /* Zero out all of the other things */ + fmfsk->stats->foff = 0; + + /* Use moving average to smooth SNR */ + var_signal += 1E-6 / 3.1; + var_noise += + 1E-6; /* prevent NAN and bias towards -5dB SNR for zero signal inputs */ + if (fmfsk->snr_mean < 0.1) + fmfsk->snr_mean = (10.0 * log10f(var_signal / var_noise)); + else + fmfsk->snr_mean = + 0.9 * fmfsk->snr_mean + 0.1 * (10.0 * log10f(var_signal / var_noise)); + fmfsk->stats->snr_est = fmfsk->snr_mean; #ifndef __EMBEDDED__ - /* Collect an eye diagram */ - /* Take a sample for the eye diagrams */ - neyesamp = fmfsk->stats->neyesamp = Ts*4; - neyeoffset = sample_offset+(Ts*2*28); - - fmfsk->stats->neyetr = 8; - for(k=0; k<fmfsk->stats->neyetr; k++) - for(j=0; j<neyesamp; j++) - fmfsk->stats->rx_eye[k][j] = rx_filt[k*neyesamp+neyeoffset+j]; - //fmfsk->stats->rx_eye[k][j] = fmfsk_in[k*neyesamp+neyeoffset+j]; - eye_max = 0; - - /* Normalize eye to +/- 1 */ - for(i=0; i<fmfsk->stats->neyetr; i++) - for(j=0; j<neyesamp; j++) - if(fabsf(fmfsk->stats->rx_eye[i][j])>eye_max) - eye_max = fabsf(fmfsk->stats->rx_eye[i][j]); - - for(i=0; i<fmfsk->stats->neyetr; i++) - for(j=0; j<neyesamp; j++) - fmfsk->stats->rx_eye[i][j] = (fmfsk->stats->rx_eye[i][j]/(2*eye_max))+.5; -#endif // !__EMBEDDED__ - - modem_probe_samp_f("t_norm_rx_timing",&norm_rx_timing,1); - modem_probe_samp_f("t_rx_filt",rx_filt,(nsym+1)*Ts); - - free(rx_filt); + /* Collect an eye diagram */ + /* Take a sample for the eye diagrams */ + neyesamp = fmfsk->stats->neyesamp = Ts * 4; + neyeoffset = sample_offset + (Ts * 2 * 28); + + fmfsk->stats->neyetr = 8; + for (k = 0; k < fmfsk->stats->neyetr; k++) + for (j = 0; j < neyesamp; j++) + fmfsk->stats->rx_eye[k][j] = rx_filt[k * neyesamp + neyeoffset + j]; + // fmfsk->stats->rx_eye[k][j] = fmfsk_in[k*neyesamp+neyeoffset+j]; + eye_max = 0; + + /* Normalize eye to +/- 1 */ + for (i = 0; i < fmfsk->stats->neyetr; i++) + for (j = 0; j < neyesamp; j++) + if (fabsf(fmfsk->stats->rx_eye[i][j]) > eye_max) + eye_max = fabsf(fmfsk->stats->rx_eye[i][j]); + + for (i = 0; i < fmfsk->stats->neyetr; i++) + for (j = 0; j < neyesamp; j++) + fmfsk->stats->rx_eye[i][j] = + (fmfsk->stats->rx_eye[i][j] / (2 * eye_max)) + .5; +#endif // !__EMBEDDED__ + + modem_probe_samp_f("t_norm_rx_timing", &norm_rx_timing, 1); + modem_probe_samp_f("t_rx_filt", rx_filt, (nsym + 1) * Ts); + + free(rx_filt); } |