main/plc.c

   1 /*
   2  * Asterisk -- An open source telephony toolkit.
   3  *
   4  * Written by Steve Underwood <steveu@coppice.org>
   5  *
   6  * Copyright (C) 2004 Steve Underwood
   7  *
   8  * All rights reserved.
   9  *
  10  * See http://www.asterisk.org for more information about
  11  * the Asterisk project. Please do not directly contact
  12  * any of the maintainers of this project for assistance;
  13  * the project provides a web site, mailing lists and IRC
  14  * channels for your use.
  15  *
  16  * This program is free software, distributed under the terms of
  17  * the GNU General Public License Version 2. See the LICENSE file
  18  * at the top of the source tree.
  19  *
  20  * This version may be optionally licenced under the GNU LGPL licence.
  21  *
  22  * A license has been granted to Digium (via disclaimer) for the use of
  23  * this code.
  24  */
  25
  26 /*! \file
  27  *
  28  * \brief SpanDSP - a series of DSP components for telephony
  29  *
  30  * \author Steve Underwood <steveu@coppice.org>
  31  */
  32
  33 #include "asterisk.h"
  34
  35 ASTERISK_FILE_VERSION(__FILE__, "$Revision$")
  36
  37 #include <math.h>
  38
  39 #include "asterisk/plc.h"
  40
  41 #if !defined(FALSE)
  42 #define FALSE 0
  43 #endif
  44 #if !defined(TRUE)
  45 #define TRUE (!FALSE)
  46 #endif
  47
  48 #if !defined(INT16_MAX)
  49 #define INT16_MAX       (32767)
  50 #define INT16_MIN       (-32767-1)
  51 #endif
  52
  53 /* We do a straight line fade to zero volume in 50ms when we are filling in for missing data. */
  54 #define ATTENUATION_INCREMENT       0.0025                            /* Attenuation per sample */
  55
  56 #define ms_to_samples(t)            (((t)*DEFAULT_SAMPLE_RATE)/1000)
  57
  58 static inline int16_t fsaturate(double damp)
  59 {
  60         if (damp > 32767.0)
  61                 return  INT16_MAX;
  62         if (damp < -32768.0)
  63                 return  INT16_MIN;
  64         return (int16_t) rint(damp);
  65 }
  66
  67 static void save_history(plc_state_t *s, int16_t *buf, int len)
  68 {
  69         if (len >= PLC_HISTORY_LEN) {
  70                 /* Just keep the last part of the new data, starting at the beginning of the buffer */
  71                  memcpy(s->history, buf + len - PLC_HISTORY_LEN, sizeof(int16_t) * PLC_HISTORY_LEN);
  72                 s->buf_ptr = 0;
  73                 return;
  74         }
  75         if (s->buf_ptr + len > PLC_HISTORY_LEN) {
  76                 /* Wraps around - must break into two sections */
  77                 memcpy(s->history + s->buf_ptr, buf, sizeof(int16_t) * (PLC_HISTORY_LEN - s->buf_ptr));
  78                 len -= (PLC_HISTORY_LEN - s->buf_ptr);
  79                 memcpy(s->history, buf + (PLC_HISTORY_LEN - s->buf_ptr), sizeof(int16_t)*len);
  80                 s->buf_ptr = len;
  81                 return;
  82         }
  83         /* Can use just one section */
  84         memcpy(s->history + s->buf_ptr, buf, sizeof(int16_t)*len);
  85         s->buf_ptr += len;
  86 }
  87
  88 /*- End of function --------------------------------------------------------*/
  89
  90 static void normalise_history(plc_state_t *s)
  91 {
  92         int16_t tmp[PLC_HISTORY_LEN];
  93
  94         if (s->buf_ptr == 0)
  95                 return;
  96         memcpy(tmp, s->history, sizeof(int16_t)*s->buf_ptr);
  97         memcpy(s->history, s->history + s->buf_ptr, sizeof(int16_t) * (PLC_HISTORY_LEN - s->buf_ptr));
  98         memcpy(s->history + PLC_HISTORY_LEN - s->buf_ptr, tmp, sizeof(int16_t) * s->buf_ptr);
  99         s->buf_ptr = 0;
 100 }
 101
 102 /*- End of function --------------------------------------------------------*/
 103
 104 static int __inline__ amdf_pitch(int min_pitch, int max_pitch, int16_t amp[], int len)
 105 {
 106         int i;
 107         int j;
 108         int acc;
 109         int min_acc;
 110         int pitch;
 111
 112         pitch = min_pitch;
 113         min_acc = INT_MAX;
 114         for (i = max_pitch; i <= min_pitch; i++) {
 115                 acc = 0;
 116                 for (j = 0; j < len; j++)
 117                         acc += abs(amp[i + j] - amp[j]);
 118                 if (acc < min_acc) {
 119                         min_acc = acc;
 120                         pitch = i;
 121                 }
 122         }
 123         return pitch;
 124 }
 125
 126 /*- End of function --------------------------------------------------------*/
 127
 128 int plc_rx(plc_state_t *s, int16_t amp[], int len)
 129 {
 130         int i;
 131         int pitch_overlap;
 132         float old_step;
 133         float new_step;
 134         float old_weight;
 135         float new_weight;
 136         float gain;
 137
 138         if (s->missing_samples) {
 139                 /* Although we have a real signal, we need to smooth it to fit well
 140                 with the synthetic signal we used for the previous block */
 141
 142                 /* The start of the real data is overlapped with the next 1/4 cycle
 143                    of the synthetic data. */
 144                 pitch_overlap = s->pitch >> 2;
 145                 if (pitch_overlap > len)
 146                         pitch_overlap = len;
 147                 gain = 1.0 - s->missing_samples*ATTENUATION_INCREMENT;
 148                 if (gain < 0.0)
 149                         gain = 0.0;
 150                 new_step = 1.0/pitch_overlap;
 151                 old_step = new_step*gain;
 152                 new_weight = new_step;
 153                 old_weight = (1.0 - new_step)*gain;
 154                 for (i = 0; i < pitch_overlap; i++) {
 155                         amp[i] = fsaturate(old_weight * s->pitchbuf[s->pitch_offset] + new_weight * amp[i]);
 156                         if (++s->pitch_offset >= s->pitch)
 157                                 s->pitch_offset = 0;
 158                         new_weight += new_step;
 159                         old_weight -= old_step;
 160                         if (old_weight < 0.0)
 161                                 old_weight = 0.0;
 162                 }
 163                 s->missing_samples = 0;
 164         }
 165         save_history(s, amp, len);
 166         return len;
 167 }
 168
 169 /*- End of function --------------------------------------------------------*/
 170
 171 int plc_fillin(plc_state_t *s, int16_t amp[], int len)
 172 {
 173         int i;
 174         int pitch_overlap;
 175         float old_step;
 176         float new_step;
 177         float old_weight;
 178         float new_weight;
 179         float gain;
 180         int16_t *orig_amp;
 181         int orig_len;
 182
 183         orig_amp = amp;
 184         orig_len = len;
 185         if (s->missing_samples == 0) {
 186                 /* As the gap in real speech starts we need to assess the last known pitch,
 187                 and prepare the synthetic data we will use for fill-in */
 188                 normalise_history(s);
 189                 s->pitch = amdf_pitch(PLC_PITCH_MIN, PLC_PITCH_MAX, s->history + PLC_HISTORY_LEN - CORRELATION_SPAN - PLC_PITCH_MIN, CORRELATION_SPAN);
 190                 /* We overlap a 1/4 wavelength */
 191                 pitch_overlap = s->pitch >> 2;
 192                 /* Cook up a single cycle of pitch, using a single of the real signal with 1/4
 193                 cycle OLA'ed to make the ends join up nicely */
 194                 /* The first 3/4 of the cycle is a simple copy */
 195                 for (i = 0;  i < s->pitch - pitch_overlap;  i++)
 196                         s->pitchbuf[i] = s->history[PLC_HISTORY_LEN - s->pitch + i];
 197                 /* The last 1/4 of the cycle is overlapped with the end of the previous cycle */
 198                 new_step = 1.0/pitch_overlap;
 199                 new_weight = new_step;
 200                 for ( ; i < s->pitch; i++) {
 201                         s->pitchbuf[i] = s->history[PLC_HISTORY_LEN - s->pitch + i] * (1.0 - new_weight) + s->history[PLC_HISTORY_LEN - 2 * s->pitch + i]*new_weight;
 202                         new_weight += new_step;
 203                 }
 204                 /* We should now be ready to fill in the gap with repeated, decaying cycles
 205                 of what is in pitchbuf */
 206
 207                 /* We need to OLA the first 1/4 wavelength of the synthetic data, to smooth
 208                 it into the previous real data. To avoid the need to introduce a delay
 209                 in the stream, reverse the last 1/4 wavelength, and OLA with that. */
 210                 gain = 1.0;
 211                 new_step = 1.0 / pitch_overlap;
 212                 old_step = new_step;
 213                 new_weight = new_step;
 214                 old_weight = 1.0 - new_step;
 215                 for (i = 0; i < pitch_overlap; i++) {
 216                         amp[i] = fsaturate(old_weight * s->history[PLC_HISTORY_LEN - 1 - i] + new_weight * s->pitchbuf[i]);
 217                         new_weight += new_step;
 218                         old_weight -= old_step;
 219                         if (old_weight < 0.0)
 220                                 old_weight = 0.0;
 221                 }
 222                 s->pitch_offset = i;
 223         } else {
 224                 gain = 1.0 - s->missing_samples*ATTENUATION_INCREMENT;
 225                 i = 0;
 226         }
 227         for ( ; gain > 0.0 && i < len; i++) {
 228                 amp[i] = s->pitchbuf[s->pitch_offset] * gain;
 229                 gain -= ATTENUATION_INCREMENT;
 230                 if (++s->pitch_offset >= s->pitch)
 231                         s->pitch_offset = 0;
 232         }
 233         for ( ; i < len; i++)
 234                 amp[i] = 0;
 235         s->missing_samples += orig_len;
 236         save_history(s, amp, len);
 237         return len;
 238 }
 239
 240 /*- End of function --------------------------------------------------------*/
 241
 242 plc_state_t *plc_init(plc_state_t *s)
 243 {
 244         memset(s, 0, sizeof(*s));
 245         return s;
 246 }
 247 /*- End of function --------------------------------------------------------*/
 248 /*- End of file ------------------------------------------------------------*/