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