plc.c
Go to the documentation of this file.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
00034
00035
00036
00037 #include "asterisk.h"
00038
00039 ASTERISK_FILE_VERSION(__FILE__, "$Revision: 369001 $")
00040
00041 #include <math.h>
00042
00043 #include "asterisk/plc.h"
00044
00045 #if !defined(FALSE)
00046 #define FALSE 0
00047 #endif
00048 #if !defined(TRUE)
00049 #define TRUE (!FALSE)
00050 #endif
00051
00052 #if !defined(INT16_MAX)
00053 #define INT16_MAX (32767)
00054 #define INT16_MIN (-32767-1)
00055 #endif
00056
00057
00058 #define ATTENUATION_INCREMENT 0.0025
00059
00060 #define ms_to_samples(t) (((t)*DEFAULT_SAMPLE_RATE)/1000)
00061
00062 static inline int16_t fsaturate(double damp)
00063 {
00064 if (damp > 32767.0)
00065 return INT16_MAX;
00066 if (damp < -32768.0)
00067 return INT16_MIN;
00068 return (int16_t) rint(damp);
00069 }
00070
00071 static void save_history(plc_state_t *s, int16_t *buf, int len)
00072 {
00073 if (len >= PLC_HISTORY_LEN) {
00074
00075 memcpy(s->history, buf + len - PLC_HISTORY_LEN, sizeof(int16_t) * PLC_HISTORY_LEN);
00076 s->buf_ptr = 0;
00077 return;
00078 }
00079 if (s->buf_ptr + len > PLC_HISTORY_LEN) {
00080
00081 memcpy(s->history + s->buf_ptr, buf, sizeof(int16_t) * (PLC_HISTORY_LEN - s->buf_ptr));
00082 len -= (PLC_HISTORY_LEN - s->buf_ptr);
00083 memcpy(s->history, buf + (PLC_HISTORY_LEN - s->buf_ptr), sizeof(int16_t)*len);
00084 s->buf_ptr = len;
00085 return;
00086 }
00087
00088 memcpy(s->history + s->buf_ptr, buf, sizeof(int16_t)*len);
00089 s->buf_ptr += len;
00090 }
00091
00092
00093
00094 static void normalise_history(plc_state_t *s)
00095 {
00096 int16_t tmp[PLC_HISTORY_LEN];
00097
00098 if (s->buf_ptr == 0)
00099 return;
00100 memcpy(tmp, s->history, sizeof(int16_t)*s->buf_ptr);
00101 memcpy(s->history, s->history + s->buf_ptr, sizeof(int16_t) * (PLC_HISTORY_LEN - s->buf_ptr));
00102 memcpy(s->history + PLC_HISTORY_LEN - s->buf_ptr, tmp, sizeof(int16_t) * s->buf_ptr);
00103 s->buf_ptr = 0;
00104 }
00105
00106
00107
00108 static int __inline__ amdf_pitch(int min_pitch, int max_pitch, int16_t amp[], int len)
00109 {
00110 int i;
00111 int j;
00112 int acc;
00113 int min_acc;
00114 int pitch;
00115
00116 pitch = min_pitch;
00117 min_acc = INT_MAX;
00118 for (i = max_pitch; i <= min_pitch; i++) {
00119 acc = 0;
00120 for (j = 0; j < len; j++)
00121 acc += abs(amp[i + j] - amp[j]);
00122 if (acc < min_acc) {
00123 min_acc = acc;
00124 pitch = i;
00125 }
00126 }
00127 return pitch;
00128 }
00129
00130
00131
00132 int plc_rx(plc_state_t *s, int16_t amp[], int len)
00133 {
00134 int i;
00135 int pitch_overlap;
00136 float old_step;
00137 float new_step;
00138 float old_weight;
00139 float new_weight;
00140 float gain;
00141
00142 if (s->missing_samples) {
00143
00144
00145
00146
00147
00148 pitch_overlap = s->pitch >> 2;
00149 if (pitch_overlap > len)
00150 pitch_overlap = len;
00151 gain = 1.0 - s->missing_samples*ATTENUATION_INCREMENT;
00152 if (gain < 0.0)
00153 gain = 0.0;
00154 new_step = 1.0/pitch_overlap;
00155 old_step = new_step*gain;
00156 new_weight = new_step;
00157 old_weight = (1.0 - new_step)*gain;
00158 for (i = 0; i < pitch_overlap; i++) {
00159 amp[i] = fsaturate(old_weight * s->pitchbuf[s->pitch_offset] + new_weight * amp[i]);
00160 if (++s->pitch_offset >= s->pitch)
00161 s->pitch_offset = 0;
00162 new_weight += new_step;
00163 old_weight -= old_step;
00164 if (old_weight < 0.0)
00165 old_weight = 0.0;
00166 }
00167 s->missing_samples = 0;
00168 }
00169 save_history(s, amp, len);
00170 return len;
00171 }
00172
00173
00174
00175 int plc_fillin(plc_state_t *s, int16_t amp[], int len)
00176 {
00177 int i;
00178 int pitch_overlap;
00179 float old_step;
00180 float new_step;
00181 float old_weight;
00182 float new_weight;
00183 float gain;
00184 int orig_len;
00185
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