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