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: 316265 $")
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 int orig_len;
00181
00182 orig_len = len;
00183 if (s->missing_samples == 0) {
00184
00185
00186 normalise_history(s);
00187 s->pitch = amdf_pitch(PLC_PITCH_MIN, PLC_PITCH_MAX, s->history + PLC_HISTORY_LEN - CORRELATION_SPAN - PLC_PITCH_MIN, CORRELATION_SPAN);
00188
00189 pitch_overlap = s->pitch >> 2;
00190
00191
00192
00193 for (i = 0; i < s->pitch - pitch_overlap; i++)
00194 s->pitchbuf[i] = s->history[PLC_HISTORY_LEN - s->pitch + i];
00195
00196 new_step = 1.0/pitch_overlap;
00197 new_weight = new_step;
00198 for ( ; i < s->pitch; i++) {
00199 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;
00200 new_weight += new_step;
00201 }
00202
00203
00204
00205
00206
00207
00208 gain = 1.0;
00209 new_step = 1.0 / pitch_overlap;
00210 old_step = new_step;
00211 new_weight = new_step;
00212 old_weight = 1.0 - new_step;
00213 for (i = 0; i < pitch_overlap; i++) {
00214 amp[i] = fsaturate(old_weight * s->history[PLC_HISTORY_LEN - 1 - i] + new_weight * s->pitchbuf[i]);
00215 new_weight += new_step;
00216 old_weight -= old_step;
00217 if (old_weight < 0.0)
00218 old_weight = 0.0;
00219 }
00220 s->pitch_offset = i;
00221 } else {
00222 gain = 1.0 - s->missing_samples*ATTENUATION_INCREMENT;
00223 i = 0;
00224 }
00225 for ( ; gain > 0.0 && i < len; i++) {
00226 amp[i] = s->pitchbuf[s->pitch_offset] * gain;
00227 gain -= ATTENUATION_INCREMENT;
00228 if (++s->pitch_offset >= s->pitch)
00229 s->pitch_offset = 0;
00230 }
00231 for ( ; i < len; i++)
00232 amp[i] = 0;
00233 s->missing_samples += orig_len;
00234 save_history(s, amp, len);
00235 return len;
00236 }
00237
00238
00239
00240 plc_state_t *plc_init(plc_state_t *s)
00241 {
00242 memset(s, 0, sizeof(*s));
00243 return s;
00244 }
00245
00246