From cc8766b43acf80fdc10786b3b145e6bab0ead110 Mon Sep 17 00:00:00 2001
From: drowe67 <drowe67@01035d8c-6547-0410-b346-abe4f91aad63>
Date: Mon, 15 Nov 2010 01:22:56 +0000
Subject: [PATCH] added LSP interpolator to c2enc, c2dec, should hopefully
 remove AM modulation from background noise

git-svn-id: https://svn.code.sf.net/p/freetel/code@226 01035d8c-6547-0410-b346-abe4f91aad63
---
 codec2/src/codec2.c   | 55 ++++++++++++++++++++++++++++++++-----------
 codec2/src/lsp.c      |  9 +++----
 codec2/src/quantise.c | 10 ++++----
 codec2/src/quantise.h |  4 +++-
 4 files changed, 54 insertions(+), 24 deletions(-)

diff --git a/codec2/src/codec2.c b/codec2/src/codec2.c
index 03a28ed0..8c505f8f 100644
--- a/codec2/src/codec2.c
+++ b/codec2/src/codec2.c
@@ -44,16 +44,18 @@
 #include "codec2.h"
 
 typedef struct {
-    float  Sn[M];        /* input speech                              */
-    float  w[M];	 /* time domain hamming window                */
-    COMP   W[FFT_ENC];	 /* DFT of w[]                                */
-    float  Pn[2*N];	 /* trapezoidal synthesis window              */
-    float  Sn_[2*N];	 /* synthesised speech                        */
-    float  prev_Wo;      /* previous frame's pitch estimate           */
-    float  ex_phase;     /* excitation model phase track              */
-    float  bg_est;       /* background noise estimate for post filter */
-    MODEL  prev_model;   /* model parameters from 20ms ago            */
-    void  *nlp;          /* pitch predictor states                    */
+    float  w[M];	       /* time domain hamming window                */
+    COMP   W[FFT_ENC];	       /* DFT of w[]                                */
+    float  Pn[2*N];	       /* trapezoidal synthesis window              */
+    float  Sn[M];              /* input speech                              */
+    void  *nlp;                /* pitch predictor states                    */
+    float  Sn_[2*N];	       /* synthesised output speech                 */
+    float  ex_phase;           /* excitation model phase track              */
+    float  bg_est;             /* background noise estimate for post filter */
+    float  prev_Wo;            /* previous frame's pitch estimate           */
+    MODEL  prev_model;         /* previous frame's model parameters         */
+    float  prev_lsps[LPC_ORD]; /* previous frame's LSPs                     */
+    float  prev_energy;        /* previous frame's LPC energy               */
 } CODEC2;
 
 /*---------------------------------------------------------------------------*\
@@ -111,6 +113,11 @@ void *codec2_create()
     c2->prev_model.L = PI/c2->prev_model.Wo;
     c2->prev_model.voiced = 0;
 
+    for(i=0; i<LPC_ORD; i++) {
+      c2->prev_lsps[i] = i*PI/(LPC_ORD+1);
+    }
+    c2->prev_energy = 1;
+
     c2->nlp = nlp_create();
     if (c2->nlp == NULL) {
 	free (c2);
@@ -228,10 +235,13 @@ void codec2_decode(void *codec2_state, short speech[],
     MODEL   model;
     int     voiced1, voiced2;
     int     lsp_indexes[LPC_ORD];
+    float   lsps[LPC_ORD];
     int     lpc_correction;
     int     energy_index;
+    float   energy;
     int     Wo_index;
     float   ak[LPC_ORD+1];
+    float   ak_interp[LPC_ORD+1];
     int     i;
     unsigned int nbit = 0;
     MODEL   model_interp;
@@ -239,6 +249,8 @@ void codec2_decode(void *codec2_state, short speech[],
     assert(codec2_state != NULL);
     c2 = (CODEC2*)codec2_state;
 
+    /* unpack bit stream to integer codes */
+
     Wo_index = unpack(bits, &nbit, WO_BITS);
     for(i=0; i<LPC_ORD; i++) {
 	lsp_indexes[i] = unpack(bits, &nbit, lsp_bits(i));
@@ -249,6 +261,8 @@ void codec2_decode(void *codec2_state, short speech[],
     voiced2 = unpack(bits, &nbit, 1);
     assert(nbit == CODEC2_BITS_PER_FRAME);
 
+    /* decode integer codes to model parameters */
+
     model.Wo = decode_Wo(Wo_index);
     model.L = PI/model.Wo;
     memset(&model.A, 0, (model.L+1)*sizeof(model.A[0]));
@@ -256,18 +270,31 @@ void codec2_decode(void *codec2_state, short speech[],
 		      ak,
 		      lsp_indexes,
 		      lpc_correction, 
-		      energy_index);
+		      energy_index,
+		      lsps,
+		      &energy);
 
     model.voiced = voiced2;
     model_interp.voiced = voiced1;
     model_interp.Wo = P_MAX/2;
     memset(&model_interp.A, 0, MAX_AMP*sizeof(model_interp.A[0]));
-    interpolate(&model_interp, &c2->prev_model, &model);
 
-    synthesise_one_frame(c2,  speech,     &model_interp, ak);
-    synthesise_one_frame(c2, &speech[N],  &model, ak);
+    /* interpolate middle frame's model parameters for adjacent frames */
+
+    interpolate_lsp(&model_interp, &c2->prev_model, &model,
+    		    c2->prev_lsps, c2->prev_energy, lsps, energy, ak_interp);
+    apply_lpc_correction(&model_interp, lpc_correction);
+
+    /* synthesis two 10ms frames */
+
+    synthesise_one_frame(c2, speech, &model_interp, ak_interp);
+    synthesise_one_frame(c2, &speech[N], &model, ak);
+
+    /* update memories (decode states) for next time */
 
     memcpy(&c2->prev_model, &model, sizeof(MODEL));
+    memcpy(c2->prev_lsps, lsps, sizeof(lsps));
+    c2->prev_energy = energy;
 }
 
 /*---------------------------------------------------------------------------*\
diff --git a/codec2/src/lsp.c b/codec2/src/lsp.c
index f51c13ce..47001c1e 100644
--- a/codec2/src/lsp.c
+++ b/codec2/src/lsp.c
@@ -257,11 +257,11 @@ int lpc_to_lsp (float *a, int lpcrdr, float *freq, int nb, float delta)
   DATE CREATED: 24/2/93
 
   This function converts LSP coefficients to LPC coefficients.  In the
-  Speex code we worked out a wayto simplify this significantly.
+  Speex code we worked out a way to simplify this significantly.
 
 \*---------------------------------------------------------------------------*/
 
-void lsp_to_lpc(float *freq, float *ak, int lpcrdr)
+void lsp_to_lpc(float *lsp, float *ak, int lpcrdr)
 /*  float *freq         array of LSP frequencies in radians     	*/
 /*  float *ak 		array of LPC coefficients 			*/
 /*  int lpcrdr  	order of LPC coefficients 			*/
@@ -272,12 +272,13 @@ void lsp_to_lpc(float *freq, float *ak, int lpcrdr)
     float xout1,xout2,xin1,xin2;
     float *pw,*n1,*n2,*n3,*n4 = 0;
     int m = lpcrdr/2;
+    float freq[LSP_MAX_ORDER];
     float Wp[(LSP_MAX_ORDER * 4) + 2];
-
+    
     /* convert from radians to the x=cos(w) domain */
 
     for(i=0; i<lpcrdr; i++)
-	freq[i] = cos(freq[i]);
+	freq[i] = cos(lsp[i]);
 
     pw = Wp;
 
diff --git a/codec2/src/quantise.c b/codec2/src/quantise.c
index 635d4eeb..f4646e69 100644
--- a/codec2/src/quantise.c
+++ b/codec2/src/quantise.c
@@ -755,18 +755,18 @@ float decode_amplitudes(MODEL *model,
 			float  ak[],
 		        int    lsp_indexes[], 
 		        int    lpc_correction,
-		        int    energy_index
+		        int    energy_index,
+			float  lsps[],
+			float *e
 )
 {
-    float lsps[LPC_ORD];
-    float e;
     float snr;
 
     decode_lsps(lsps, lsp_indexes, LPC_ORD);
     bw_expand_lsps(lsps, LPC_ORD);
     lsp_to_lpc(lsps, ak, LPC_ORD);
-    e = decode_energy(energy_index);
-    aks_to_M2(ak, LPC_ORD, model, e, &snr, 1); 
+    *e = decode_energy(energy_index);
+    aks_to_M2(ak, LPC_ORD, model, *e, &snr, 1); 
     apply_lpc_correction(model, lpc_correction);
 
     return snr;
diff --git a/codec2/src/quantise.h b/codec2/src/quantise.h
index 387d70bd..f60372a5 100644
--- a/codec2/src/quantise.h
+++ b/codec2/src/quantise.h
@@ -59,7 +59,9 @@ float decode_amplitudes(MODEL *model,
 			float  ak[],
 			int lsp_indexes[],
 			int lpc_correction, 
-			int energy_index);
+			int energy_index,
+			float  lsps[],
+			float *e);
 
 void pack(unsigned char * bits, unsigned int *nbit, int index, unsigned int index_bits);
 int  unpack(const unsigned char * bits, unsigned int *nbit, unsigned int index_bits);
-- 
2.25.1