decode_cvsd.c

军用话音常用到的CVSD算法

/*
##############################################################################
##                                                                          ##
##     COPYRIGHT (C)      ERICSSON MOBILE COMMUNICATIONS AB, 1998, 1999     ##
##                                                                          ##
##     The  copyright  to  the document(s)  herein  is  the property of     ##
##     Ericsson Mobile Communications AB, Sweden.                           ##
##                                                                          ##
##     The document(s) may be used  and/or copied only with the written     ##
##     permission from Ericsson Mobile Communications AB  or in             ##
##     accordance with the terms  and conditions  stipulated in the         ##
##     agreement/contract under which the document(s) have been supplied.   ##
##                                                                          ##
##############################################################################
*/

/* ================================ */
/* Include general libs             */
/* ================================ */


#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include "mex.h"
#include "matrix.h"



/* ========================================== */
/* Define templets for inputs                 */
/* ========================================== */

#define nargin           1


/* bit */
#define bit_pos            0




/* ========================================== */
/* Define templets for outputs                */
/* ========================================== */


#define nargout           1

/* y */
#define y_pos             0



long trunc(long num)
{
  if (num<0) {
    return floor(num)+1;
  } else {
    return floor(num);
  }
}

long sat(long num,long calcres)
{
  if (num<-pow(2.0,calcres-1)) {
    printf("UNDERFLOW\n");
    return -pow(2.0,calcres-1);
  } else { 
    if (num>(pow(2.0,calcres-1)-1)) {
      printf("OVERFLOW\n");
      return pow(2.0,calcres-1)-1;
    } else {
      return num;
    }
  }
}

long fix(long num)
{
  if (num<0) {
    return ceil(num);
  } else {
    return floor(num);
  }
}


/* ================================ */
/* mexFunction                      */
/* ================================ */

void mexFunction(
    int nlhs_,
    mxArray *argout_[],
    int nrhs_,
    const mxArray *argin_[])
 {

   long int nr_of_samples_bit, xres, accres, deltares;
   long int i,j,k;
   double *bit_d, *y_d;
   long int  *bit, *y, delta,deltamin,deltaincstep,deltamax,acc,accmax,accmin;
   long int hfract, betafract;
   short bits[4];
   short alpha, noofsentones;
/*    FILE *fp; */

   xres = 16;
   accres = 22;
   deltares = 26;
   deltamin = 10*pow(2.0,(deltares-xres));
   deltaincstep = 10*pow(2.0,(deltares-xres));
   deltamax = 1280*pow(2.0,(deltares-xres));
   accmin = -pow(2.0,(accres)-1);
   accmax = pow(2.0,(accres)-1);
   bits[0] = 1;
   bits[1] = 0;
   bits[2] = 1;
   bits[3] = 0;
   acc = 0;
   delta = deltamin;
   hfract = floor(pow(2.0,5));
   betafract = floor(pow(2.0,10));

if (nrhs_ == 0)
  {
    mexPrintf("decode_cvsd.c Version 0.1 27-Nov.-98, \n"); 
    mexPrintf("Hans Lindkvist, UW/UD, \n");
    mexPrintf("Ericsson Mobile Communications AB, Lund \n");
    for (i=0;i<nlhs_;i++)
      {
	argout_[i] = mxCreateDoubleMatrix(0,0,0);	
      }
    return;
  }




     /* ===================================== */	
     /* Matlab interface                      */
     /* ===================================== */

     /* ===================================== */	
     /* Allocate memory                       */
     /* ===================================== */



/* Check number of input arguments */
if (nrhs_ != nargin && nrhs_ != nargin-1)  
  mexErrMsgTxt(" Error in decode_cvsd: wrong number of input arguments ...");

/* Check number of output arguments */
if (nlhs_ > nargout)  
  mexErrMsgTxt(" Error in decode_cvsd: wrong number of output arguments ...");






/* Check vector bit */
nr_of_samples_bit = max(mxGetN(argin_[bit_pos]),mxGetM(argin_[bit_pos]));
if (min(mxGetN(argin_[bit_pos]),mxGetM(argin_[bit_pos])) != 1)    
  mexErrMsgTxt(" Error in decode_cvsd: bit  must be a vector ");

if (mxIsComplex(argin_[bit_pos]) == 1)    
  mexErrMsgTxt(" Error in decode_cvsd: bit  must be real valued ");



/* ================================================ */	
/* Assign pointers to the various input parameters  */
/* and allocate memory                               */
/* ================================================ */

/* bit */
 bit_d  = mxGetPr(argin_[bit_pos]);
 bit = (long*)mxCalloc(nr_of_samples_bit,sizeof(long));
 for (i = 0; i < nr_of_samples_bit ;i++)
   {
     bit[i] = (long)bit_d[i];
   }

/* ==================================================== */	
/*  Allocate Memory for output                          */
/* ==================================================== */

 y = (long*)mxCalloc(nr_of_samples_bit,sizeof(long));


 argout_[y_pos] = mxCreateDoubleMatrix(nr_of_samples_bit,1,0);
 y_d = mxGetPr(argout_[y_pos]);

 /* ========================================== */ 
/* Do the actual computations                 */
/* ========================================== */ 

 for (i = 0; i < nr_of_samples_bit ;i++) 
   {
     if (bit[i]==1)
       {
/* 	 acc = (int)floor((floor(hfract*(double)acc-(double)acc))/hfract) - (int)floor(delta/16); */
	 bits[3]= bits[2];
	 bits[2]= bits[1];
	 bits[1]= bits[0];
	 bits[0]= 1;
       } else {
/* 	 acc = (int)floor((floor(hfract*(double)acc-(double)acc))/hfract) + (int)floor(delta/16); */
	 bits[3]= bits[2];
	 bits[2]= bits[1];
	 bits[1]= bits[0];
	 bits[0]= 0;
       }
     noofsentones=0;
     for (j=0; j<4 ;j++) 
       {
	 if (bits[j]==1) 
	   noofsentones++;
       }
     alpha = ((noofsentones==4) || (noofsentones == 0));

     if (alpha==1)
       {
	 delta = delta + deltaincstep;
	 if (delta > deltamax)
	   {
	     delta = deltamax;
	   }
       } else {
	 delta = floor((double)delta - (double)delta/betafract);
	 if (delta < deltamin)
	   {
	     delta = deltamin;
	   }
       }

     if (acc > accmax) {
       acc = accmax;
     } else if (acc < accmin) {
       acc = accmin;
     } else {
       acc= acc;
     }

/*      y[i]=acc; */
     y[i]=(int)floor((floor(hfract*(double)acc-(double)acc))/hfract);

     acc = (int)floor((floor(hfract*(double)acc-(double)acc))/hfract) +(1-2*bits[0])*(int)floor(delta/16);

   }


/* ================================ */
/* Convert output to MatLab format  */
/* ================================ */ 

 for (i = 0; i < nr_of_samples_bit ;i++) 
   y_d[i] = floor((double)y[i]*pow(2.0,(xres-accres)));
 
/* ================================ */
/* Free memory                      */
/* ================================ */ 

  mxFree(y);
  
  return;

 }