static void c2demo(int mode, char inputfile[], char outputfile[])
{
struct CODEC2 *codec2;
short *inbuf, *outbuf;
unsigned char *bits;
int nsam, nbit;
FILE *fin, *fout;
int frame;
PROFILE_VAR(enc_start, dec_start);
codec2 = codec2_create(mode);
nsam = codec2_samples_per_frame(codec2);
outbuf = (short*)malloc(nsam*sizeof(short));
inbuf = (short*)malloc(nsam*sizeof(short));
nbit = codec2_bits_per_frame(codec2);
bits = (unsigned char*)malloc(nbit*sizeof(char));
fin = fopen(inputfile, "rb");
if (fin == NULL) {
printf("Error opening input file: %s\n\nTerminating....\n",inputfile);
exit(1);
}
fout = fopen(outputfile, "wb");
if (fout == NULL) {
printf("Error opening output file: %s\n\nTerminating....\n",outputfile);
exit(1);
}
#ifdef DUMP
dump_on("stm32f4");
#endif
frame = 0;
while (fread(inbuf, sizeof(short), nsam, fin) == nsam) {
PROFILE_SAMPLE(enc_start);
codec2_encode(codec2, bits, inbuf);
PROFILE_SAMPLE_AND_LOG(dec_start, enc_start, " enc");
codec2_decode(codec2, outbuf, bits);
PROFILE_SAMPLE_AND_LOG2(dec_start, " dec");
PROFILE_SAMPLE_AND_LOG2(enc_start, " enc & dec");
fwrite((char*)outbuf, sizeof(short), nsam, fout);
printf("frame: %d\n", ++frame);
machdep_profile_print_logged_samples();
}
#ifdef DUMP
dump_off("sm32f4");
#endif
fclose(fin);
fclose(fout);
free(inbuf);
free(outbuf);
free(bits);
codec2_destroy(codec2);
}
void synthesise_one_frame(struct CODEC2 *c2, short speech[], MODEL *model, COMP Aw[])
{
int i;
PROFILE_VAR(phase_start, pf_start, synth_start);
#ifdef DUMP
dump_quantised_model(model);
#endif
PROFILE_SAMPLE(phase_start);
phase_synth_zero_order(c2->fft_fwd_cfg, model, &c2->ex_phase, Aw);
PROFILE_SAMPLE_AND_LOG(pf_start, phase_start, " phase_synth");
postfilter(model, &c2->bg_est);
PROFILE_SAMPLE_AND_LOG(synth_start, pf_start, " postfilter");
synthesise(c2->fft_inv_cfg, c2->Sn_, model, c2->Pn, 1);
PROFILE_SAMPLE_AND_LOG2(synth_start, " synth");
ear_protection(c2->Sn_, N);
for(i=0; i<N; i++) {
if (c2->Sn_[i] > 32767.0)
speech[i] = 32767;
else if (c2->Sn_[i] < -32767.0)
speech[i] = -32767;
else
speech[i] = c2->Sn_[i];
}
}
int main(int argc, char *argv[]) {
struct freedv *f;
short inbuf[FREEDV_NSAMPLES], outbuf[FREEDV_NSAMPLES];
FILE *fin, *fout;
int frame;
PROFILE_VAR(freedv_start);
machdep_profile_init();
f = freedv_open(FREEDV_MODE_1600);
// Transmit ---------------------------------------------------------------------
fin = fopen("stm_in.raw", "rb");
if (fin == NULL) {
printf("Error opening input file\n");
exit(1);
}
fout = fopen("mod.raw", "wb");
if (fout == NULL) {
printf("Error opening output file\n");
exit(1);
}
frame = 0;
while (fread(inbuf, sizeof(short), FREEDV_NSAMPLES, fin) == FREEDV_NSAMPLES) {
PROFILE_SAMPLE(freedv_start);
freedv_tx(f, outbuf, inbuf);
PROFILE_SAMPLE_AND_LOG2(freedv_start, " freedv_tx");
fwrite(outbuf, sizeof(short), FREEDV_NSAMPLES, fout);
printf("frame: %d\n", ++frame);
machdep_profile_print_logged_samples();
}
fclose(fin);
fclose(fout);
return 0;
}
void analyse_one_frame(struct CODEC2 *c2, MODEL *model, short speech[])
{
COMP Sw[FFT_ENC];
COMP Sw_[FFT_ENC];
COMP Ew[FFT_ENC];
float pitch;
int i;
PROFILE_VAR(dft_start, nlp_start, model_start, two_stage, estamps);
/* Read input speech */
for(i=0; i<M-N; i++)
c2->Sn[i] = c2->Sn[i+N];
for(i=0; i<N; i++)
c2->Sn[i+M-N] = speech[i];
PROFILE_SAMPLE(dft_start);
dft_speech(c2->fft_fwd_cfg, Sw, c2->Sn, c2->w);
PROFILE_SAMPLE_AND_LOG(nlp_start, dft_start, " dft_speech");
/* Estimate pitch */
nlp(c2->nlp,c2->Sn,N,P_MIN,P_MAX,&pitch,Sw, c2->W, &c2->prev_Wo_enc);
PROFILE_SAMPLE_AND_LOG(model_start, nlp_start, " nlp");
model->Wo = TWO_PI/pitch;
model->L = PI/model->Wo;
/* estimate model parameters */
two_stage_pitch_refinement(model, Sw);
PROFILE_SAMPLE_AND_LOG(two_stage, model_start, " two_stage");
estimate_amplitudes(model, Sw, c2->W, 0);
PROFILE_SAMPLE_AND_LOG(estamps, two_stage, " est_amps");
est_voicing_mbe(model, Sw, c2->W, Sw_, Ew);
c2->prev_Wo_enc = model->Wo;
PROFILE_SAMPLE_AND_LOG2(estamps, " est_voicing");
#ifdef DUMP
dump_model(model);
#endif
}
int main(int argc, char *argv[]) {
struct freedv *f;
short adc16k[FDMDV_OS_TAPS_16K+FREEDV_NSAMPLES_16K];
short dac16k[FREEDV_NSAMPLES_16K];
short adc8k[FREEDV_NSAMPLES];
short dac8k[FDMDV_OS_TAPS_8K+FREEDV_NSAMPLES];
FILE *fin, *fout, *ftotal;
int frame, nin_16k, nin, i, nout = 0;
struct FDMDV_STATS stats;
PROFILE_VAR(fdmdv_16_to_8_start, freedv_rx_start, fdmdv_8_to_16_start);
machdep_profile_init();
f = freedv_open(FREEDV_MODE_1600);
// Receive ---------------------------------------------------------------------
frame = 0;
fin = fopen("mod_16k.raw", "rb");
if (fin == NULL) {
printf("Error opening input file\n");
exit(1);
}
fout = fopen("speechout_16k.raw", "wb");
if (fout == NULL) {
printf("Error opening output file\n");
exit(1);
}
ftotal = fopen("total.txt", "wt");
assert(ftotal != NULL);
/* clear filter memories */
for(i=0; i<FDMDV_OS_TAPS_16K; i++)
adc16k[i] = 0.0;
for(i=0; i<FDMDV_OS_TAPS_8K; i++)
dac8k[i] = 0.0;
nin = freedv_nin(f);
nin_16k = 2*nin;
nout = nin;
while (fread(&adc16k[FDMDV_OS_TAPS_16K], sizeof(short), nin_16k, fin) == nin_16k) {
PROFILE_SAMPLE(fdmdv_16_to_8_start);
fdmdv_16_to_8_short(adc8k, &adc16k[FDMDV_OS_TAPS_16K], nin);
PROFILE_SAMPLE_AND_LOG(freedv_rx_start, fdmdv_16_to_8_start, " fdmdv_16_to_8");
nout = freedv_rx(f, &dac8k[FDMDV_OS_TAPS_8K], adc8k);
nin = freedv_nin(f); nin_16k = 2*nin;
fdmdv_get_demod_stats(f->fdmdv, &stats);
PROFILE_SAMPLE_AND_LOG(fdmdv_8_to_16_start, freedv_rx_start, " freedv_rx");
fdmdv_8_to_16_short(dac16k, &dac8k[FDMDV_OS_TAPS_8K], nout);
PROFILE_SAMPLE_AND_LOG2(fdmdv_8_to_16_start, " fdmdv_8_to_16");
fprintf(ftotal, "%d\n", machdep_profile_sample() - fdmdv_16_to_8_start);
machdep_profile_print_logged_samples();
fwrite(dac16k, sizeof(short), 2*nout, fout);
fdmdv_get_demod_stats(f->fdmdv, &stats);
printf("frame: %d nin_16k: %d sync: %d SNR: %3.2f \n",
++frame, nin_16k, stats.sync, (double)stats.snr_est);
}
fclose(fin);
fclose(fout);
fclose(ftotal);
return 0;
}
void codec2_decode_1300(struct CODEC2 *c2, short speech[], const unsigned char * bits, float ber_est)
{
MODEL model[4];
int lsp_indexes[LPC_ORD];
float lsps[4][LPC_ORD];
int Wo_index, e_index;
float e[4];
float snr;
float ak[4][LPC_ORD+1];
int i,j;
unsigned int nbit = 0;
float weight;
COMP Aw[FFT_ENC];
PROFILE_VAR(recover_start);
assert(c2 != NULL);
/* only need to zero these out due to (unused) snr calculation */
for(i=0; i<4; i++)
for(j=1; j<=MAX_AMP; j++)
model[i].A[j] = 0.0;
/* unpack bits from channel ------------------------------------*/
/* this will partially fill the model params for the 4 x 10ms
frames */
model[0].voiced = unpack_natural_or_gray(bits, &nbit, 1, c2->gray);
model[1].voiced = unpack_natural_or_gray(bits, &nbit, 1, c2->gray);
model[2].voiced = unpack_natural_or_gray(bits, &nbit, 1, c2->gray);
model[3].voiced = unpack_natural_or_gray(bits, &nbit, 1, c2->gray);
Wo_index = unpack_natural_or_gray(bits, &nbit, WO_BITS, c2->gray);
model[3].Wo = decode_Wo(Wo_index);
model[3].L = PI/model[3].Wo;
e_index = unpack_natural_or_gray(bits, &nbit, E_BITS, c2->gray);
e[3] = decode_energy(e_index);
for(i=0; i<LSP_SCALAR_INDEXES; i++) {
lsp_indexes[i] = unpack_natural_or_gray(bits, &nbit, lsp_bits(i), c2->gray);
}
decode_lsps_scalar(&lsps[3][0], lsp_indexes, LPC_ORD);
check_lsp_order(&lsps[3][0], LPC_ORD);
bw_expand_lsps(&lsps[3][0], LPC_ORD, 50.0, 100.0);
if (ber_est > 0.15) {
model[0].voiced = model[1].voiced = model[2].voiced = model[3].voiced = 0;
e[3] = decode_energy(10);
bw_expand_lsps(&lsps[3][0], LPC_ORD, 200.0, 200.0);
fprintf(stderr, "soft mute\n");
}
/* interpolate ------------------------------------------------*/
/* Wo, energy, and LSPs are sampled every 40ms so we interpolate
the 3 frames in between */
PROFILE_SAMPLE(recover_start);
for(i=0, weight=0.25; i<3; i++, weight += 0.25) {
interpolate_lsp_ver2(&lsps[i][0], c2->prev_lsps_dec, &lsps[3][0], weight);
interp_Wo2(&model[i], &c2->prev_model_dec, &model[3], weight);
e[i] = interp_energy2(c2->prev_e_dec, e[3],weight);
}
/* then recover spectral amplitudes */
for(i=0; i<4; i++) {
lsp_to_lpc(&lsps[i][0], &ak[i][0], LPC_ORD);
aks_to_M2(c2->fft_fwd_cfg, &ak[i][0], LPC_ORD, &model[i], e[i], &snr, 0, 0,
c2->lpc_pf, c2->bass_boost, c2->beta, c2->gamma, Aw);
apply_lpc_correction(&model[i]);
synthesise_one_frame(c2, &speech[N*i], &model[i], Aw);
}
PROFILE_SAMPLE_AND_LOG2(recover_start, " recover");
#ifdef DUMP
dump_lsp_(&lsps[3][0]);
dump_ak_(&ak[3][0], LPC_ORD);
#endif
/* update memories for next frame ----------------------------*/
c2->prev_model_dec = model[3];
c2->prev_e_dec = e[3];
for(i=0; i<LPC_ORD; i++)
c2->prev_lsps_dec[i] = lsps[3][i];
}
float nlp(
void *nlp_state,
float Sn[], /* input speech vector */
int n, /* frames shift (no. new samples in Sn[]) */
int pmin, /* minimum pitch value */
int pmax, /* maximum pitch value */
float *pitch, /* estimated pitch period in samples */
COMP Sw[], /* Freq domain version of Sn[] */
COMP W[], /* Freq domain window */
float *prev_Wo
)
{
NLP *nlp;
float notch; /* current notch filter output */
COMP fw[PE_FFT_SIZE]; /* DFT of squared signal (input) */
COMP Fw[PE_FFT_SIZE]; /* DFT of squared signal (output) */
float gmax;
int gmax_bin;
int m, i,j;
float best_f0;
PROFILE_VAR(start, tnotch, filter, peakpick, window, fft, magsq, shiftmem);
assert(nlp_state != NULL);
nlp = (NLP*)nlp_state;
m = nlp->m;
PROFILE_SAMPLE(start);
/* Square, notch filter at DC, and LP filter vector */
for(i=m-n; i<m; i++) /* square latest speech samples */
nlp->sq[i] = Sn[i]*Sn[i];
for(i=m-n; i<m; i++) { /* notch filter at DC */
notch = nlp->sq[i] - nlp->mem_x;
notch += COEFF*nlp->mem_y;
nlp->mem_x = nlp->sq[i];
nlp->mem_y = notch;
nlp->sq[i] = notch + 1.0; /* With 0 input vectors to codec,
kiss_fft() would take a long
time to execute when running in
real time. Problem was traced
to kiss_fft function call in
this function. Adding this small
constant fixed problem. Not
exactly sure why. */
}
PROFILE_SAMPLE_AND_LOG(tnotch, start, " square and notch");
for(i=m-n; i<m; i++) { /* FIR filter vector */
for(j=0; j<NLP_NTAP-1; j++)
nlp->mem_fir[j] = nlp->mem_fir[j+1];
nlp->mem_fir[NLP_NTAP-1] = nlp->sq[i];
nlp->sq[i] = 0.0;
for(j=0; j<NLP_NTAP; j++)
nlp->sq[i] += nlp->mem_fir[j]*nlp_fir[j];
}
PROFILE_SAMPLE_AND_LOG(filter, tnotch, " filter");
/* Decimate and DFT */
for(i=0; i<PE_FFT_SIZE; i++) {
fw[i].real = 0.0;
fw[i].imag = 0.0;
}
for(i=0; i<m/DEC; i++) {
fw[i].real = nlp->sq[i*DEC]*nlp->w[i];
}
PROFILE_SAMPLE_AND_LOG(window, filter, " window");
#ifdef DUMP
dump_dec(Fw);
#endif
kiss_fft(nlp->fft_cfg, (kiss_fft_cpx *)fw, (kiss_fft_cpx *)Fw);
PROFILE_SAMPLE_AND_LOG(fft, window, " fft");
for(i=0; i<PE_FFT_SIZE; i++)
Fw[i].real = Fw[i].real*Fw[i].real + Fw[i].imag*Fw[i].imag;
PROFILE_SAMPLE_AND_LOG(magsq, fft, " mag sq");
#ifdef DUMP
dump_sq(nlp->sq);
dump_Fw(Fw);
#endif
/* find global peak */
gmax = 0.0;
gmax_bin = PE_FFT_SIZE*DEC/pmax;
for(i=PE_FFT_SIZE*DEC/pmax; i<=PE_FFT_SIZE*DEC/pmin; i++) {
if (Fw[i].real > gmax) {
gmax = Fw[i].real;
gmax_bin = i;
}
}
PROFILE_SAMPLE_AND_LOG(peakpick, magsq, " peak pick");
//#define POST_PROCESS_MBE
#ifdef POST_PROCESS_MBE
best_f0 = post_process_mbe(Fw, pmin, pmax, gmax, Sw, W, prev_Wo);
#else
best_f0 = post_process_sub_multiples(Fw, pmin, pmax, gmax, gmax_bin, prev_Wo);
#endif
PROFILE_SAMPLE_AND_LOG(shiftmem, peakpick, " post process");
/* Shift samples in buffer to make room for new samples */
for(i=0; i<m-n; i++)
nlp->sq[i] = nlp->sq[i+n];
/* return pitch and F0 estimate */
*pitch = (float)SAMPLE_RATE/best_f0;
PROFILE_SAMPLE_AND_LOG2(shiftmem, " shift mem");
PROFILE_SAMPLE_AND_LOG2(start, " nlp int");
return(best_f0);
}
