00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031 #include <stdio.h>
00032 #include <stdlib.h>
00033 #include <string.h>
00034 #include <math.h>
00035 #include <limits.h>
00036
00037 #include "asterisk.h"
00038
00039 ASTERISK_FILE_VERSION(__FILE__, "$Revision: 7221 $")
00040
00041 #include "asterisk/plc.h"
00042
00043 #if !defined(FALSE)
00044 #define FALSE 0
00045 #endif
00046 #if !defined(TRUE)
00047 #define TRUE (!FALSE)
00048 #endif
00049
00050 #if !defined(INT16_MAX)
00051 #define INT16_MAX (32767)
00052 #define INT16_MIN (-32767-1)
00053 #endif
00054
00055
00056 #define ATTENUATION_INCREMENT 0.0025
00057
00058 #define ms_to_samples(t) (((t)*SAMPLE_RATE)/1000)
00059
00060 static inline int16_t fsaturate(double damp)
00061 {
00062 if (damp > 32767.0)
00063 return INT16_MAX;
00064 if (damp < -32768.0)
00065 return INT16_MIN;
00066 return (int16_t) rint(damp);
00067 }
00068
00069 static void save_history(plc_state_t *s, int16_t *buf, int len)
00070 {
00071 if (len >= PLC_HISTORY_LEN) {
00072
00073 memcpy(s->history, buf + len - PLC_HISTORY_LEN, sizeof(int16_t)*PLC_HISTORY_LEN);
00074 s->buf_ptr = 0;
00075 return;
00076 }
00077 if (s->buf_ptr + len > PLC_HISTORY_LEN) {
00078
00079 memcpy(s->history + s->buf_ptr, buf, sizeof(int16_t)*(PLC_HISTORY_LEN - s->buf_ptr));
00080 len -= (PLC_HISTORY_LEN - s->buf_ptr);
00081 memcpy(s->history, buf + (PLC_HISTORY_LEN - s->buf_ptr), sizeof(int16_t)*len);
00082 s->buf_ptr = len;
00083 return;
00084 }
00085
00086 memcpy(s->history + s->buf_ptr, buf, sizeof(int16_t)*len);
00087 s->buf_ptr += len;
00088 }
00089
00090
00091
00092 static void normalise_history(plc_state_t *s)
00093 {
00094 int16_t tmp[PLC_HISTORY_LEN];
00095
00096 if (s->buf_ptr == 0)
00097 return;
00098 memcpy(tmp, s->history, sizeof(int16_t)*s->buf_ptr);
00099 memcpy(s->history, s->history + s->buf_ptr, sizeof(int16_t)*(PLC_HISTORY_LEN - s->buf_ptr));
00100 memcpy(s->history + PLC_HISTORY_LEN - s->buf_ptr, tmp, sizeof(int16_t)*s->buf_ptr);
00101 s->buf_ptr = 0;
00102 }
00103
00104
00105
00106 static int __inline__ amdf_pitch(int min_pitch, int max_pitch, int16_t amp[], int len)
00107 {
00108 int i;
00109 int j;
00110 int acc;
00111 int min_acc;
00112 int pitch;
00113
00114 pitch = min_pitch;
00115 min_acc = INT_MAX;
00116 for (i = max_pitch; i <= min_pitch; i++) {
00117 acc = 0;
00118 for (j = 0; j < len; j++)
00119 acc += abs(amp[i + j] - amp[j]);
00120 if (acc < min_acc) {
00121 min_acc = acc;
00122 pitch = i;
00123 }
00124 }
00125 return pitch;
00126 }
00127
00128
00129
00130 int plc_rx(plc_state_t *s, int16_t amp[], int len)
00131 {
00132 int i;
00133 int pitch_overlap;
00134 float old_step;
00135 float new_step;
00136 float old_weight;
00137 float new_weight;
00138 float gain;
00139
00140 if (s->missing_samples) {
00141
00142
00143
00144
00145
00146 pitch_overlap = s->pitch >> 2;
00147 if (pitch_overlap > len)
00148 pitch_overlap = len;
00149 gain = 1.0 - s->missing_samples*ATTENUATION_INCREMENT;
00150 if (gain < 0.0)
00151 gain = 0.0;
00152 new_step = 1.0/pitch_overlap;
00153 old_step = new_step*gain;
00154 new_weight = new_step;
00155 old_weight = (1.0 - new_step)*gain;
00156 for (i = 0; i < pitch_overlap; i++) {
00157 amp[i] = fsaturate(old_weight*s->pitchbuf[s->pitch_offset] + new_weight*amp[i]);
00158 if (++s->pitch_offset >= s->pitch)
00159 s->pitch_offset = 0;
00160 new_weight += new_step;
00161 old_weight -= old_step;
00162 if (old_weight < 0.0)
00163 old_weight = 0.0;
00164 }
00165 s->missing_samples = 0;
00166 }
00167 save_history(s, amp, len);
00168 return len;
00169 }
00170
00171
00172
00173 int plc_fillin(plc_state_t *s, int16_t amp[], int len)
00174 {
00175 int i;
00176 int pitch_overlap;
00177 float old_step;
00178 float new_step;
00179 float old_weight;
00180 float new_weight;
00181 float gain;
00182 int16_t *orig_amp;
00183 int orig_len;
00184
00185 orig_amp = amp;
00186 orig_len = len;
00187 if (s->missing_samples == 0) {
00188
00189
00190 normalise_history(s);
00191 s->pitch = amdf_pitch(PLC_PITCH_MIN, PLC_PITCH_MAX, s->history + PLC_HISTORY_LEN - CORRELATION_SPAN - PLC_PITCH_MIN, CORRELATION_SPAN);
00192
00193 pitch_overlap = s->pitch >> 2;
00194
00195
00196
00197 for (i = 0; i < s->pitch - pitch_overlap; i++)
00198 s->pitchbuf[i] = s->history[PLC_HISTORY_LEN - s->pitch + i];
00199
00200 new_step = 1.0/pitch_overlap;
00201 new_weight = new_step;
00202 for ( ; i < s->pitch; i++) {
00203 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;
00204 new_weight += new_step;
00205 }
00206
00207
00208
00209
00210
00211
00212 gain = 1.0;
00213 new_step = 1.0/pitch_overlap;
00214 old_step = new_step;
00215 new_weight = new_step;
00216 old_weight = 1.0 - new_step;
00217 for (i = 0; i < pitch_overlap; i++) {
00218 amp[i] = fsaturate(old_weight*s->history[PLC_HISTORY_LEN - 1 - i] + new_weight*s->pitchbuf[i]);
00219 new_weight += new_step;
00220 old_weight -= old_step;
00221 if (old_weight < 0.0)
00222 old_weight = 0.0;
00223 }
00224 s->pitch_offset = i;
00225 } else {
00226 gain = 1.0 - s->missing_samples*ATTENUATION_INCREMENT;
00227 i = 0;
00228 }
00229 for ( ; gain > 0.0 && i < len; i++) {
00230 amp[i] = s->pitchbuf[s->pitch_offset]*gain;
00231 gain -= ATTENUATION_INCREMENT;
00232 if (++s->pitch_offset >= s->pitch)
00233 s->pitch_offset = 0;
00234 }
00235 for ( ; i < len; i++)
00236 amp[i] = 0;
00237 s->missing_samples += orig_len;
00238 save_history(s, amp, len);
00239 return len;
00240 }
00241
00242
00243
00244 plc_state_t *plc_init(plc_state_t *s)
00245 {
00246 memset(s, 0, sizeof(*s));
00247 return s;
00248 }
00249
00250