af_hrtf.c

君正早期ucos系统(只有早期的才不没有打包成库),MPLAYER,文件系统,图片解码,浏览,电子书,录音,想学ucos,识货的人就下吧 russblock fmradio explore set

/* Experimental audio filter that mixes 5.1 and 5.1 with matrix
   encoded rear channels into headphone signal using FIR filtering
   with HRTF.
*/
//#include <mplaylib.h>
#include "mplaylib.h"


#include <inttypes.h>

#include <math.h>

#include "af.h"
#include "dsp.h"

/* HRTF filter coefficients and adjustable parameters */
#include "af_hrtf.h"

typedef struct af_hrtf_s {
    /* Lengths */
    int dlbuflen, hrflen, basslen;
    /* L, C, R, Ls, Rs channels */
    float *lf, *rf, *lr, *rr, *cf, *cr;
    float *cf_ir, *af_ir, *of_ir, *ar_ir, *or_ir, *cr_ir;
    int cf_o, af_o, of_o, ar_o, or_o, cr_o;
    /* Bass */
    float *ba_l, *ba_r;
    float *ba_ir;
    /* Whether to matrix decode the rear center channel */
    int matrix_mode;
    /* How to decode the input:
       0 = 5/5+1 channels
       1 = 2 channels
       2 = matrix encoded 2 channels */
    int decode_mode;
    /* Full wave rectified (FWR) amplitudes and gain used to steer the
       active matrix decoding of front channels (variable names
       lpr/lmr means Lt + Rt, Lt - Rt) */
    float l_fwr, r_fwr, lpr_fwr, lmr_fwr;
    float adapt_l_gain, adapt_r_gain, adapt_lpr_gain, adapt_lmr_gain;
    /* Matrix input decoding require special FWR buffer, since the
       decoding is done in place. */
    float *fwrbuf_l, *fwrbuf_r, *fwrbuf_lr, *fwrbuf_rr;
    /* Rear channel delay buffer for matrix decoding */
    float *rear_dlbuf;
    /* Full wave rectified amplitude and gain used to steer the active
       matrix decoding of center rear channel */
    float lr_fwr, rr_fwr, lrprr_fwr, lrmrr_fwr;
    float adapt_lr_gain, adapt_rr_gain;
    float adapt_lrprr_gain, adapt_lrmrr_gain;
    /* Cyclic position on the ring buffer */
    int cyc_pos;
    int print_flag;
} af_hrtf_t;

/* Convolution on a ring buffer
 *    nx:	length of the ring buffer
 *    nk:	length of the convolution kernel
 *    sx:	ring buffer
 *    sk:	convolution kernel
 *    offset:	offset on the ring buffer, can be 
 */
static float conv(const int nx, const int nk, float *sx, float *sk,
		  const int offset)
{
    /* k = reminder of offset / nx */
    int k = offset >= 0 ? offset % nx : nx + (offset % nx);

    if(nk + k <= nx)
	return af_filter_fir(nk, sx + k, sk);
    else
	return af_filter_fir(nk + k - nx, sx, sk + nx - k) +
	    af_filter_fir(nx - k, sx + k, sk);
}

/* Detect when the impulse response starts (significantly) */
static int pulse_detect(float *sx)
{
    /* nmax must be the reference impulse response length (128) minus
       s->hrflen */
    const int nmax = 128 - HRTFFILTLEN;
    const float thresh = IRTHRESH;
    int i;

    for(i = 0; i < nmax; i++)
	if(fabs(sx[i]) > thresh)
	    return i;
    return 0;
}

/* Fuzzy matrix coefficient transfer function to "lock" the matrix on
   a effectively passive mode if the gain is approximately 1 */
static inline float passive_lock(float x)
{
   const float x1 = x - 1;
   const float ax1s = fabs(x - 1) * (1.0 / MATAGCLOCK);

   return x1 - x1 / (1 + ax1s * ax1s) + 1;
}

/* Unified active matrix decoder for 2 channel matrix encoded surround
   sources */
static inline void matrix_decode(short *in, const int k, const int il,
			  const int ir, const int decode_rear,
			  const int dlbuflen,
			  float l_fwr, float r_fwr,
			  float lpr_fwr, float lmr_fwr,
			  float *adapt_l_gain, float *adapt_r_gain,
			  float *adapt_lpr_gain, float *adapt_lmr_gain,
			  float *lf, float *rf, float *lr,
			  float *rr, float *cf)
{
   const int kr = (k + MATREARDELAY) % dlbuflen;
   float l_gain = (l_fwr + r_fwr) /
      (1 + l_fwr + l_fwr);
   float r_gain = (l_fwr + r_fwr) /
      (1 + r_fwr + r_fwr);
   /* The 2nd axis has strong gain fluctuations, and therefore require
      limits.  The factor corresponds to the 1 / amplification of (Lt
      - Rt) when (Lt, Rt) is strongly correlated. (e.g. during
      dialogues).  It should be bigger than -12 dB to prevent
      distortion. */
   float lmr_lim_fwr = lmr_fwr > M9_03DB * lpr_fwr ?
      lmr_fwr : M9_03DB * lpr_fwr;
   float lpr_gain = (lpr_fwr + lmr_lim_fwr) /
      (1 + lpr_fwr + lpr_fwr);
   float lmr_gain = (lpr_fwr + lmr_lim_fwr) /
      (1 + lmr_lim_fwr + lmr_lim_fwr);
   float lmr_unlim_gain = (lpr_fwr + lmr_fwr) /
      (1 + lmr_fwr + lmr_fwr);
   float lpr, lmr;
   float l_agc, r_agc, lpr_agc, lmr_agc;
   float f, d_gain, c_gain, c_agc_cfk;

#if 0
   static int counter = 0;
   static FILE *fp_out;

   if(counter == 0)
      fp_out = fopen("af_hrtf.log", "w");
   if(counter % 240 == 0)
      fprintf(fp_out, "%g %g %g %g %g ", counter * (1.0 / 48000),
	      l_gain, r_gain, lpr_gain, lmr_gain);
#endif

   /*** AXIS NO. 1: (Lt, Rt) -> (C, Ls, Rs) ***/
   /* AGC adaption */
   d_gain = (fabs(l_gain - *adapt_l_gain) +
	     fabs(r_gain - *adapt_r_gain)) * 0.5;
   f = d_gain * (1.0 / MATAGCTRIG);
   f = MATAGCDECAY - MATAGCDECAY / (1 + f * f);
   *adapt_l_gain = (1 - f) * *adapt_l_gain + f * l_gain;
   *adapt_r_gain = (1 - f) * *adapt_r_gain + f * r_gain;
   /* Matrix */
   l_agc = in[il] * passive_lock(*adapt_l_gain);
   r_agc = in[ir] * passive_lock(*adapt_r_gain);
   cf[k] = (l_agc + r_agc) * M_SQRT1_2;
   if(decode_rear) {
      lr[kr] = rr[kr] = (l_agc - r_agc) * M_SQRT1_2;
      /* Stereo rear channel is steered with the same AGC steering as
	 the decoding matrix. Note this requires a fast updating AGC
	 at the order of 20 ms (which is the case here). */
      lr[kr] *= (l_fwr + l_fwr) /
	 (1 + l_fwr + r_fwr);
      rr[kr] *= (r_fwr + r_fwr) /
	 (1 + l_fwr + r_fwr);
   }

   /*** AXIS NO. 2: (Lt + Rt, Lt - Rt) -> (L, R) ***/
   lpr = (in[il] + in[ir]) * M_SQRT1_2;
   lmr = (in[il] - in[ir]) * M_SQRT1_2;
   /* AGC adaption */
   d_gain = fabs(lmr_unlim_gain - *adapt_lmr_gain);
   f = d_gain * (1.0 / MATAGCTRIG);
   f = MATAGCDECAY - MATAGCDECAY / (1 + f * f);
   *adapt_lpr_gain = (1 - f) * *adapt_lpr_gain + f * lpr_gain;
   *adapt_lmr_gain = (1 - f) * *adapt_lmr_gain + f * lmr_gain;
   /* Matrix */
   lpr_agc = lpr * passive_lock(*adapt_lpr_gain);
   lmr_agc = lmr * passive_lock(*adapt_lmr_gain);
   lf[k] = (lpr_agc + lmr_agc) * M_SQRT1_2;
   rf[k] = (lpr_agc - lmr_agc) * M_SQRT1_2;

   /*** CENTER FRONT CANCELLATION ***/
   /* A heuristic approach exploits that Lt + Rt gain contains the
      information about Lt, Rt correlation.  This effectively reshapes
      the front and rear "cones" to concentrate Lt + Rt to C and
      introduce Lt - Rt in L, R. */
   /* 0.67677 is the emprical lower bound for lpr_gain. */
   c_gain = 8 * (*adapt_lpr_gain - 0.67677);
   c_gain = c_gain > 0 ? c_gain : 0;
   /* c_gain should not be too high, not even reaching full
      cancellation (~ 0.50 - 0.55 at current AGC implementation), or
      the center will s0und too narrow. */
   c_gain = MATCOMPGAIN / (1 + c_gain * c_gain);
   c_agc_cfk = c_gain * cf[k];
   lf[k] -= c_agc_cfk;
   rf[k] -= c_agc_cfk;
   cf[k] += c_agc_cfk + c_agc_cfk;
#if 0
   if(counter % 240 == 0)
      fprintf(fp_out, "%g %g %g %g %g\n",
	      *adapt_l_gain, *adapt_r_gain,
	      *adapt_lpr_gain, *adapt_lmr_gain,
	      c_gain);
   counter++;
#endif
}

static inline void update_ch(af_hrtf_t *s, short *in, const int k)
{
    const int fwr_pos = (k + FWRDURATION) % s->dlbuflen;
    /* Update the full wave rectified total amplitude */
    /* Input matrix decoder */
    if(s->decode_mode == HRTF_MIX_MATRIX2CH) {
       s->l_fwr += abs(in[0]) - fabs(s->fwrbuf_l[fwr_pos]);
       s->r_fwr += abs(in[1]) - fabs(s->fwrbuf_r[fwr_pos]);
       s->lpr_fwr += abs(in[0] + in[1]) -
	  fabs(s->fwrbuf_l[fwr_pos] + s->fwrbuf_r[fwr_pos]);
       s->lmr_fwr += abs(in[0] - in[1]) -
	  fabs(s->fwrbuf_l[fwr_pos] - s->fwrbuf_r[fwr_pos]);
    }
    /* Rear matrix decoder */
    if(s->matrix_mode) {
       s->lr_fwr += abs(in[2]) - fabs(s->fwrbuf_lr[fwr_pos]);
       s->rr_fwr += abs(in[3]) - fabs(s->fwrbuf_rr[fwr_pos]);
       s->lrprr_fwr += abs(in[2] + in[3]) -
	  fabs(s->fwrbuf_lr[fwr_pos] + s->fwrbuf_rr[fwr_pos]);
       s->lrmrr_fwr += abs(in[2] - in[3]) -
	  fabs(s->fwrbuf_lr[fwr_pos] - s->fwrbuf_rr[fwr_pos]);
    }

    switch (s->decode_mode) {
    case HRTF_MIX_51:
       /* 5/5+1 channel sources */
       s->lf[k] = in[0];
       s->cf[k] = in[4];
       s->rf[k] = in[1];
       s->fwrbuf_lr[k] = s->lr[k] = in[2];
       s->fwrbuf_rr[k] = s->rr[k] = in[3];
       break;
    case HRTF_MIX_MATRIX2CH:
       /* Matrix encoded 2 channel sources */
       s->fwrbuf_l[k] = in[0];
       s->fwrbuf_r[k] = in[1];
       matrix_decode(in, k, 0, 1, 1, s->dlbuflen,
		     s->l_fwr, s->r_fwr,
		     s->lpr_fwr, s->lmr_fwr,
		     &(s->adapt_l_gain), &(s->adapt_r_gain),
		     &(s->adapt_lpr_gain), &(s->adapt_lmr_gain),
		     s->lf, s->rf, s->lr, s->rr, s->cf);
       break;
    case HRTF_MIX_STEREO:
       /* Stereo sources */
       s->lf[k] = in[0];
       s->rf[k] = in[1];
       s->cf[k] = s->lr[k] = s->rr[k] = 0;
       break;
    }

    /* We need to update the bass compensation delay line, too. */
    s->ba_l[k] = in[0] + in[4] + in[2];
    s->ba_r[k] = in[4] + in[1] + in[3];
}

/* Initialization and runtime control */
static int control(struct af_instance_s *af, int cmd, void* arg)
{
    af_hrtf_t *s = af->setup;
    int test_output_res;
    char mode;

    switch(cmd) {
    case AF_CONTROL_REINIT:
	af->data->rate   = ((af_data_t*)arg)->rate;
	if(af->data->rate != 48000) {
	    // automatic samplerate adjustment in the filter chain
	    // is not yet supported.
	    af_msg(AF_MSG_ERROR,
		   "[hrtf] ERROR: Sampling rate is not 48000 Hz (%d)!\n",
		   af->data->rate);
	    return AF_ERROR;
	}
	af->data->nch    = ((af_data_t*)arg)->nch;
	    if(af->data->nch == 2) {
 	       /* 2 channel input */
 	       if(s->decode_mode != HRTF_MIX_MATRIX2CH) {
   		  /* Default behavior is stereo mixing. */
 		  s->decode_mode = HRTF_MIX_STEREO;
	       }
	    }
	    else if (af->data->nch < 5)
	      af->data->nch = 5;
	af->data->format = AF_FORMAT_S16_NE;
	af->data->bps    = 2;
	test_output_res = af_test_output(af, (af_data_t*)arg);
	af->mul.n = 2;
	af->mul.d = af->data->nch;
	// after testing input set the real output format
	af->data->nch = 2;
	s->print_flag = 1;
	return test_output_res;
    case AF_CONTROL_COMMAND_LINE:
	sscanf((char*)arg, "%c", &mode);
	switch(mode) {
	case 'm':
	    /* Use matrix rear decoding. */
	    s->matrix_mode = 1;
	    break;
	case 's':
	    /* Input needs matrix decoding. */
	    s->decode_mode = HRTF_MIX_MATRIX2CH;
	    break;
	case '0':
	    s->matrix_mode = 0;
	    break;
	default:
	    af_msg(AF_MSG_ERROR,
		   "[hrtf] Mode is neither 'm', 's', nor '0' (%c).\n",
		   mode);
	    return AF_ERROR;
	}
	s->print_flag = 1;
	return AF_OK;
    }    

    return AF_UNKNOWN;
}

/* Deallocate memory */
static void uninit(struct af_instance_s *af)
{
    if(af->setup) {
	af_hrtf_t *s = af->setup;