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 <stdio.h>
  38 #include <stdlib.h>
  39 #include <string.h>
  40 #include <math.h>
  41
  42 #include "asterisk/plc.h"
  43
  44 #if !defined(FALSE)
  45 #define FALSE 0
  46 #endif
  47 #if !defined(TRUE)
  48 #define TRUE (!FALSE)
  49 #endif
  50
  51 #if !defined(INT16_MAX)
  52 #define INT16_MAX       (32767)
  53 #define INT16_MIN       (-32767-1)
  54 #endif
  55
  56 /* We do a straight line fade to zero volume in 50ms when we are filling in for missing data. */
  57 #define ATTENUATION_INCREMENT       0.0025                            /* Attenuation per sample */
  58
  59 #define ms_to_samples(t)            (((t)*DEFAULT_SAMPLE_RATE)/1000)
  60
  61 static inline int16_t fsaturate(double damp)
  62 {
  63         if (damp > 32767.0)
  64                 return  INT16_MAX;
  65         if (damp < -32768.0)
  66                 return  INT16_MIN;
  67         return (int16_t) rint(damp);
  68 }
  69
  70 static void save_history(plc_state_t *s, int16_t *buf, int len)
  71 {
  72         if (len >= PLC_HISTORY_LEN) {
  73                 /* Just keep the last part of the new data, starting at the beginning of the buffer */
  74                  memcpy(s->history, buf + len - PLC_HISTORY_LEN, sizeof(int16_t) * PLC_HISTORY_LEN);
  75                 s->buf_ptr = 0;
  76                 return;
  77         }
  78         if (s->buf_ptr + len > PLC_HISTORY_LEN) {
  79                 /* Wraps around - must break into two sections */
  80                 memcpy(s->history + s->buf_ptr, buf, sizeof(int16_t) * (PLC_HISTORY_LEN - s->buf_ptr));
  81                 len -= (PLC_HISTORY_LEN - s->buf_ptr);
  82                 memcpy(s->history, buf + (PLC_HISTORY_LEN - s->buf_ptr), sizeof(int16_t)*len);
  83                 s->buf_ptr = len;
  84                 return;
  85         }
  86         /* Can use just one section */
  87         memcpy(s->history + s->buf_ptr, buf, sizeof(int16_t)*len);
  88         s->buf_ptr += len;
  89 }
  90
  91 /*- End of function --------------------------------------------------------*/
  92
  93 static void normalise_history(plc_state_t *s)
  94 {
  95         int16_t tmp[PLC_HISTORY_LEN];
  96
  97         if (s->buf_ptr == 0)
  98                 return;
  99         memcpy(tmp, s->history, sizeof(int16_t)*s->buf_ptr);
 100         memcpy(s->history, s->history + s->buf_ptr, sizeof(int16_t) * (PLC_HISTORY_LEN - s->buf_ptr));
 101         memcpy(s->history + PLC_HISTORY_LEN - s->buf_ptr, tmp, sizeof(int16_t) * s->buf_ptr);
 102         s->buf_ptr = 0;
 103 }
 104
 105 /*- End of function --------------------------------------------------------*/
 106
 107 static int __inline__ amdf_pitch(int min_pitch, int max_pitch, int16_t amp[], int len)
 108 {
 109         int i;
 110         int j;
 111         int acc;
 112         int min_acc;
 113         int pitch;
 114
 115         pitch = min_pitch;
 116         min_acc = INT_MAX;
 117         for (i = max_pitch; i <= min_pitch; i++) {
 118                 acc = 0;
 119                 for (j = 0; j < len; j++)
 120                         acc += abs(amp[i + j] - amp[j]);
 121                 if (acc < min_acc) {
 122                         min_acc = acc;
 123                         pitch = i;
 124                 }
 125         }
 126         return pitch;
 127 }
 128
 129 /*- End of function --------------------------------------------------------*/
 130
 131 int plc_rx(plc_state_t *s, int16_t amp[], int len)
 132 {
 133         int i;
 134         int pitch_overlap;
 135         float old_step;
 136         float new_step;
 137         float old_weight;
 138         float new_weight;
 139         float gain;
 140
 141         if (s->missing_samples) {
 142                 /* Although we have a real signal, we need to smooth it to fit well
 143                 with the synthetic signal we used for the previous block */
 144
 145                 /* The start of the real data is overlapped with the next 1/4 cycle
 146                    of the synthetic data. */
 147                 pitch_overlap = s->pitch >> 2;
 148                 if (pitch_overlap > len)
 149                         pitch_overlap = len;
 150                 gain = 1.0 - s->missing_samples*ATTENUATION_INCREMENT;
 151                 if (gain < 0.0)
 152                         gain = 0.0;
 153                 new_step = 1.0/pitch_overlap;
 154                 old_step = new_step*gain;
 155                 new_weight = new_step;
 156                 old_weight = (1.0 - new_step)*gain;
 157                 for (i = 0; i < pitch_overlap; i++) {
 158                         amp[i] = fsaturate(old_weight * s->pitchbuf[s->pitch_offset] + new_weight * amp[i]);
 159                         if (++s->pitch_offset >= s->pitch)
 160                                 s->pitch_offset = 0;
 161                         new_weight += new_step;
 162                         old_weight -= old_step;
 163                         if (old_weight < 0.0)
 164                                 old_weight = 0.0;
 165                 }
 166                 s->missing_samples = 0;
 167         }
 168         save_history(s, amp, len);
 169         return len;
 170 }
 171
 172 /*- End of function --------------------------------------------------------*/
 173
 174 int plc_fillin(plc_state_t *s, int16_t amp[], int len)
 175 {
 176         int i;
 177         int pitch_overlap;
 178         float old_step;
 179         float new_step;
 180         float old_weight;
 181         float new_weight;
 182         float gain;
 183         int16_t *orig_amp;
 184         int orig_len;
 185
 186         orig_amp = amp;
 187         orig_len = len;
 188         if (s->missing_samples == 0) {
 189                 /* As the gap in real speech starts we need to assess the last known pitch,
 190                 and prepare the synthetic data we will use for fill-in */
 191                 normalise_history(s);
 192                 s->pitch = amdf_pitch(PLC_PITCH_MIN, PLC_PITCH_MAX, s->history + PLC_HISTORY_LEN - CORRELATION_SPAN - PLC_PITCH_MIN, CORRELATION_SPAN);
 193                 /* We overlap a 1/4 wavelength */
 194                 pitch_overlap = s->pitch >> 2;
 195                 /* Cook up a single cycle of pitch, using a single of the real signal with 1/4
 196                 cycle OLA'ed to make the ends join up nicely */
 197                 /* The first 3/4 of the cycle is a simple copy */
 198                 for (i = 0;  i < s->pitch - pitch_overlap;  i++)
 199                         s->pitchbuf[i] = s->history[PLC_HISTORY_LEN - s->pitch + i];
 200                 /* The last 1/4 of the cycle is overlapped with the end of the previous cycle */
 201                 new_step = 1.0/pitch_overlap;
 202                 new_weight = new_step;
 203                 for ( ; i < s->pitch; i++) {
 204                         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;
 205                         new_weight += new_step;
 206                 }
 207                 /* We should now be ready to fill in the gap with repeated, decaying cycles
 208                 of what is in pitchbuf */
 209
 210                 /* We need to OLA the first 1/4 wavelength of the synthetic data, to smooth
 211                 it into the previous real data. To avoid the need to introduce a delay
 212                 in the stream, reverse the last 1/4 wavelength, and OLA with that. */
 213                 gain = 1.0;
 214                 new_step = 1.0 / pitch_overlap;
 215                 old_step = new_step;
 216                 new_weight = new_step;
 217                 old_weight = 1.0 - new_step;
 218                 for (i = 0; i < pitch_overlap; i++) {
 219                         amp[i] = fsaturate(old_weight * s->history[PLC_HISTORY_LEN - 1 - i] + new_weight * s->pitchbuf[i]);
 220                         new_weight += new_step;
 221                         old_weight -= old_step;
 222                         if (old_weight < 0.0)
 223                                 old_weight = 0.0;
 224                 }
 225                 s->pitch_offset = i;
 226         } else {
 227                 gain = 1.0 - s->missing_samples*ATTENUATION_INCREMENT;
 228                 i = 0;
 229         }
 230         for ( ; gain > 0.0 && i < len; i++) {
 231                 amp[i] = s->pitchbuf[s->pitch_offset] * gain;
 232                 gain -= ATTENUATION_INCREMENT;
 233                 if (++s->pitch_offset >= s->pitch)
 234                         s->pitch_offset = 0;
 235         }
 236         for ( ; i < len; i++)
 237                 amp[i] = 0;
 238         s->missing_samples += orig_len;
 239         save_history(s, amp, len);
 240         return len;
 241 }
 242
 243 /*- End of function --------------------------------------------------------*/
 244
 245 plc_state_t *plc_init(plc_state_t *s)
 246 {
 247         memset(s, 0, sizeof(*s));
 248         return s;
 249 }
 250 /*- End of function --------------------------------------------------------*/
 251 /*- End of file ------------------------------------------------------------*/