kurtis.heimerl | ce31733 | 2011-11-26 03:18:39 +0000 | [diff] [blame] | 1 | /* |
| 2 | * Radio device interface with sample rate conversion |
| 3 | * Written by Thomas Tsou <ttsou@vt.edu> |
| 4 | * |
| 5 | * Copyright 2011 Free Software Foundation, Inc. |
| 6 | * |
| 7 | * This program is free software: you can redistribute it and/or modify |
| 8 | * it under the terms of the GNU Affero General Public License as published by |
| 9 | * the Free Software Foundation, either version 3 of the License, or |
| 10 | * (at your option) any later version. |
| 11 | * |
| 12 | * This program is distributed in the hope that it will be useful, |
| 13 | * but WITHOUT ANY WARRANTY; without even the implied warranty of |
| 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the |
| 15 | * GNU Affero General Public License for more details. |
| 16 | * |
| 17 | * You should have received a copy of the GNU Affero General Public License |
| 18 | * along with this program. If not, see <http://www.gnu.org/licenses/>. |
| 19 | * See the COPYING file in the main directory for details. |
| 20 | */ |
| 21 | |
| 22 | #include <radioInterface.h> |
| 23 | #include <Logger.h> |
| 24 | |
| 25 | /* New chunk sizes for resampled rate */ |
| 26 | #ifdef INCHUNK |
| 27 | #undef INCHUNK |
| 28 | #endif |
| 29 | #ifdef OUTCHUNK |
| 30 | #undef OUTCHUNK |
| 31 | #endif |
| 32 | |
| 33 | /* Resampling parameters */ |
| 34 | #define INRATE 65 * SAMPSPERSYM |
| 35 | #define INHISTORY INRATE * 2 |
| 36 | #define INCHUNK INRATE * 9 |
| 37 | |
| 38 | #define OUTRATE 96 * SAMPSPERSYM |
| 39 | #define OUTHISTORY OUTRATE * 2 |
| 40 | #define OUTCHUNK OUTRATE * 9 |
| 41 | |
| 42 | /* Resampler low pass filters */ |
| 43 | signalVector *tx_lpf = 0; |
| 44 | signalVector *rx_lpf = 0; |
| 45 | |
| 46 | /* Resampler history */ |
| 47 | signalVector *tx_hist = 0; |
| 48 | signalVector *rx_hist = 0; |
| 49 | |
| 50 | /* Resampler input buffer */ |
| 51 | signalVector *tx_vec = 0; |
| 52 | signalVector *rx_vec = 0; |
| 53 | |
kurtis.heimerl | b99cbf3 | 2011-11-26 03:19:33 +0000 | [diff] [blame^] | 54 | /* |
| 55 | * High rate (device facing) buffers |
| 56 | * |
| 57 | * Transmit side samples are pushed after each burst so accomodate |
| 58 | * a resampled burst plus up to a chunk left over from the previous |
| 59 | * resampling operation. |
| 60 | * |
| 61 | * Receive side samples always pulled with a fixed size. |
| 62 | */ |
| 63 | short tx_buf[INCHUNK * 2 * 4]; |
kurtis.heimerl | ce31733 | 2011-11-26 03:18:39 +0000 | [diff] [blame] | 64 | short rx_buf[OUTCHUNK * 2 * 2]; |
| 65 | |
| 66 | /* |
| 67 | * Utilities and Conversions |
| 68 | * |
| 69 | * Manipulate signal vectors dynamically for two reasons. For one, |
| 70 | * it's simpler. And two, it doesn't make any reasonable difference |
| 71 | * relative to the high overhead generated by the resampling. |
| 72 | */ |
| 73 | |
| 74 | /* Concatenate signal vectors. Deallocate input vectors. */ |
| 75 | signalVector *concat(signalVector *a, signalVector *b) |
| 76 | { |
| 77 | signalVector *vec = new signalVector(*a, *b); |
| 78 | delete a; |
| 79 | delete b; |
| 80 | |
| 81 | return vec; |
| 82 | } |
| 83 | |
| 84 | /* Segment a signal vector. Deallocate the input vector. */ |
| 85 | signalVector *segment(signalVector *a, int indx, int sz) |
| 86 | { |
| 87 | signalVector *vec = new signalVector(sz); |
| 88 | a->segmentCopyTo(*vec, indx, sz); |
| 89 | delete a; |
| 90 | |
| 91 | return vec; |
| 92 | } |
| 93 | |
| 94 | /* Create a new signal vector from a short array. */ |
| 95 | signalVector *short_to_sigvec(short *smpls, size_t sz) |
| 96 | { |
| 97 | int i; |
| 98 | signalVector *vec = new signalVector(sz); |
| 99 | signalVector::iterator itr = vec->begin(); |
| 100 | |
| 101 | for (i = 0; i < sz; i++) { |
| 102 | *itr++ = Complex<float>(smpls[2 * i + 0], smpls[2 * i + 1]); |
| 103 | } |
| 104 | |
| 105 | return vec; |
| 106 | } |
| 107 | |
| 108 | /* Convert and deallocate a signal vector into a short array. */ |
| 109 | int sigvec_to_short(signalVector *vec, short *smpls) |
| 110 | { |
| 111 | int i; |
| 112 | signalVector::iterator itr = vec->begin(); |
| 113 | |
| 114 | for (i = 0; i < vec->size(); i++) { |
| 115 | smpls[2 * i + 0] = itr->real(); |
| 116 | smpls[2 * i + 1] = itr->imag(); |
| 117 | itr++; |
| 118 | } |
| 119 | delete vec; |
| 120 | |
| 121 | return i; |
| 122 | } |
| 123 | |
| 124 | /* Create a new signal vector from a float array. */ |
| 125 | signalVector *float_to_sigvec(float *smpls, int sz) |
| 126 | { |
| 127 | int i; |
| 128 | signalVector *vec = new signalVector(sz); |
| 129 | signalVector::iterator itr = vec->begin(); |
| 130 | |
| 131 | for (i = 0; i < sz; i++) { |
| 132 | *itr++ = Complex<float>(smpls[2 * i + 0], smpls[2 * i + 1]); |
| 133 | } |
| 134 | |
| 135 | return vec; |
| 136 | } |
| 137 | |
| 138 | /* Convert and deallocate a signal vector into a float array. */ |
| 139 | int sigvec_to_float(signalVector *vec, float *smpls) |
| 140 | { |
| 141 | int i; |
| 142 | signalVector::iterator itr = vec->begin(); |
| 143 | |
| 144 | for (i = 0; i < vec->size(); i++) { |
| 145 | smpls[2 * i + 0] = itr->real(); |
| 146 | smpls[2 * i + 1] = itr->imag(); |
| 147 | itr++; |
| 148 | } |
| 149 | delete vec; |
| 150 | |
| 151 | return i; |
| 152 | } |
| 153 | |
| 154 | /* Initialize resampling signal vectors */ |
| 155 | void init_resampler(signalVector **lpf, |
| 156 | signalVector **buf, |
| 157 | signalVector **hist, |
| 158 | int tx) |
| 159 | { |
| 160 | int P, Q, taps, hist_len; |
| 161 | float cutoff_freq; |
| 162 | |
| 163 | if (tx) { |
| 164 | LOG(INFO) << "Initializing Tx resampler"; |
| 165 | P = OUTRATE; |
| 166 | Q = INRATE; |
| 167 | taps = 651; |
| 168 | hist_len = INHISTORY; |
| 169 | } else { |
| 170 | LOG(INFO) << "Initializing Rx resampler"; |
| 171 | P = INRATE; |
| 172 | Q = OUTRATE; |
| 173 | taps = 961; |
| 174 | hist_len = OUTHISTORY; |
| 175 | } |
| 176 | |
| 177 | if (!*lpf) { |
| 178 | cutoff_freq = (P < Q) ? (1.0/(float) Q) : (1.0/(float) P); |
| 179 | *lpf = createLPF(cutoff_freq, taps, P); |
| 180 | } |
| 181 | |
| 182 | if (!*buf) { |
| 183 | *buf = new signalVector(); |
| 184 | } |
| 185 | |
| 186 | if (!*hist); |
| 187 | *hist = new signalVector(hist_len); |
| 188 | } |
| 189 | |
| 190 | /* Resample a signal vector |
| 191 | * |
| 192 | * The input vector is deallocated and the pointer returned with a vector |
| 193 | * of any unconverted samples. |
| 194 | */ |
| 195 | signalVector *resmpl_sigvec(signalVector *hist, signalVector **vec, |
| 196 | signalVector *lpf, double in_rate, |
| 197 | double out_rate, int chunk_sz) |
| 198 | { |
| 199 | signalVector *resamp_vec; |
| 200 | int num_chunks = (*vec)->size() / chunk_sz; |
| 201 | |
| 202 | /* Truncate to a chunk multiple */ |
| 203 | signalVector trunc_vec(num_chunks * chunk_sz); |
| 204 | (*vec)->segmentCopyTo(trunc_vec, 0, num_chunks * chunk_sz); |
| 205 | |
| 206 | /* Update sample buffer with remainder */ |
| 207 | *vec = segment(*vec, trunc_vec.size(), (*vec)->size() - trunc_vec.size()); |
| 208 | |
| 209 | /* Add history and resample */ |
| 210 | signalVector input_vec(*hist, trunc_vec); |
| 211 | resamp_vec = polyphaseResampleVector(input_vec, in_rate, |
| 212 | out_rate, lpf); |
| 213 | |
| 214 | /* Update history */ |
| 215 | trunc_vec.segmentCopyTo(*hist, trunc_vec.size() - hist->size(), |
| 216 | hist->size()); |
| 217 | return resamp_vec; |
| 218 | } |
| 219 | |
| 220 | /* Wrapper for receive-side integer-to-float array resampling */ |
| 221 | int rx_resmpl_int_flt(float *smpls_out, short *smpls_in, int num_smpls) |
| 222 | { |
| 223 | int num_resmpld, num_chunks; |
| 224 | signalVector *convert_vec, *resamp_vec, *trunc_vec; |
| 225 | |
| 226 | if (!rx_lpf || !rx_vec || !rx_hist) |
| 227 | init_resampler(&rx_lpf, &rx_vec, &rx_hist, false); |
| 228 | |
| 229 | /* Convert and add samples to the receive buffer */ |
| 230 | convert_vec = short_to_sigvec(smpls_in, num_smpls); |
| 231 | rx_vec = concat(rx_vec, convert_vec); |
| 232 | |
| 233 | num_chunks = rx_vec->size() / OUTCHUNK; |
| 234 | if (num_chunks < 1) |
| 235 | return 0; |
| 236 | |
| 237 | /* Resample */ |
| 238 | resamp_vec = resmpl_sigvec(rx_hist, &rx_vec, rx_lpf, |
| 239 | INRATE, OUTRATE, OUTCHUNK); |
| 240 | /* Truncate */ |
| 241 | trunc_vec = segment(resamp_vec, INHISTORY, |
| 242 | resamp_vec->size() - INHISTORY); |
| 243 | /* Convert */ |
| 244 | num_resmpld = sigvec_to_float(trunc_vec, smpls_out); |
| 245 | |
| 246 | return num_resmpld; |
| 247 | } |
| 248 | |
| 249 | /* Wrapper for transmit-side float-to-int array resampling */ |
| 250 | int tx_resmpl_flt_int(short *smpls_out, float *smpls_in, int num_smpls) |
| 251 | { |
| 252 | int num_resmpl, num_chunks; |
| 253 | signalVector *convert_vec, *resamp_vec; |
| 254 | |
| 255 | if (!tx_lpf || !tx_vec || !tx_hist) |
| 256 | init_resampler(&tx_lpf, &tx_vec, &tx_hist, true); |
| 257 | |
| 258 | /* Convert and add samples to the transmit buffer */ |
| 259 | convert_vec = float_to_sigvec(smpls_in, num_smpls); |
| 260 | tx_vec = concat(tx_vec, convert_vec); |
| 261 | |
| 262 | num_chunks = tx_vec->size() / INCHUNK; |
| 263 | if (num_chunks < 1) |
| 264 | return 0; |
| 265 | |
| 266 | /* Resample and convert to an integer array */ |
| 267 | resamp_vec = resmpl_sigvec(tx_hist, &tx_vec, tx_lpf, |
| 268 | OUTRATE, INRATE, INCHUNK); |
| 269 | num_resmpl = sigvec_to_short(resamp_vec, smpls_out); |
| 270 | |
| 271 | return num_resmpl; |
| 272 | } |
| 273 | |
| 274 | /* Receive a timestamped chunk from the device */ |
| 275 | void RadioInterface::pullBuffer() |
| 276 | { |
| 277 | int num_cv, num_rd; |
| 278 | bool local_underrun; |
| 279 | |
| 280 | /* Read samples. Fail if we don't get what we want. */ |
| 281 | num_rd = mRadio->readSamples(rx_buf, OUTCHUNK, &overrun, |
| 282 | readTimestamp, &local_underrun); |
| 283 | |
kurtis.heimerl | 4e59d66 | 2011-11-26 03:19:11 +0000 | [diff] [blame] | 284 | LOG(DEBUG) << "Rx read " << num_rd << " samples from device"; |
kurtis.heimerl | ce31733 | 2011-11-26 03:18:39 +0000 | [diff] [blame] | 285 | assert(num_rd == OUTCHUNK); |
| 286 | |
| 287 | underrun |= local_underrun; |
| 288 | readTimestamp += (TIMESTAMP) num_rd; |
| 289 | |
| 290 | /* Convert and resample */ |
| 291 | num_cv = rx_resmpl_int_flt(rcvBuffer + 2 * rcvCursor, |
| 292 | rx_buf, num_rd); |
| 293 | |
kurtis.heimerl | 4e59d66 | 2011-11-26 03:19:11 +0000 | [diff] [blame] | 294 | LOG(DEBUG) << "Rx read " << num_cv << " samples from resampler"; |
kurtis.heimerl | ce31733 | 2011-11-26 03:18:39 +0000 | [diff] [blame] | 295 | |
| 296 | rcvCursor += num_cv; |
| 297 | } |
| 298 | |
| 299 | /* Send a timestamped chunk to the device */ |
| 300 | void RadioInterface::pushBuffer() |
| 301 | { |
| 302 | int num_cv, num_wr; |
| 303 | |
| 304 | if (sendCursor < INCHUNK) |
| 305 | return; |
| 306 | |
kurtis.heimerl | 4e59d66 | 2011-11-26 03:19:11 +0000 | [diff] [blame] | 307 | LOG(DEBUG) << "Tx wrote " << sendCursor << " samples to resampler"; |
kurtis.heimerl | ce31733 | 2011-11-26 03:18:39 +0000 | [diff] [blame] | 308 | |
| 309 | /* Resample and convert */ |
| 310 | num_cv = tx_resmpl_flt_int(tx_buf, sendBuffer, sendCursor); |
| 311 | assert(num_cv > sendCursor); |
| 312 | |
| 313 | /* Write samples. Fail if we don't get what we want. */ |
| 314 | num_wr = mRadio->writeSamples(tx_buf + OUTHISTORY * 2, |
| 315 | num_cv - OUTHISTORY, |
| 316 | &underrun, |
| 317 | writeTimestamp); |
| 318 | |
kurtis.heimerl | 4e59d66 | 2011-11-26 03:19:11 +0000 | [diff] [blame] | 319 | LOG(DEBUG) << "Tx wrote " << num_wr << " samples to device"; |
kurtis.heimerl | ce31733 | 2011-11-26 03:18:39 +0000 | [diff] [blame] | 320 | assert(num_wr == num_wr); |
| 321 | |
| 322 | writeTimestamp += (TIMESTAMP) num_wr; |
| 323 | sendCursor = 0; |
| 324 | } |