| /* |
| * Radio device interface with sample rate conversion |
| * Written by Thomas Tsou <ttsou@vt.edu> |
| * |
| * Copyright 2011 Free Software Foundation, Inc. |
| * |
| * This program is free software: you can redistribute it and/or modify |
| * it under the terms of the GNU Affero General Public License as published by |
| * the Free Software Foundation, either version 3 of the License, or |
| * (at your option) any later version. |
| * |
| * This program is distributed in the hope that it will be useful, |
| * but WITHOUT ANY WARRANTY; without even the implied warranty of |
| * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the |
| * GNU Affero General Public License for more details. |
| * |
| * You should have received a copy of the GNU Affero General Public License |
| * along with this program. If not, see <http://www.gnu.org/licenses/>. |
| * See the COPYING file in the main directory for details. |
| */ |
| |
| #include <radioInterface.h> |
| #include <Logger.h> |
| |
| /* New chunk sizes for resampled rate */ |
| #ifdef INCHUNK |
| #undef INCHUNK |
| #endif |
| #ifdef OUTCHUNK |
| #undef OUTCHUNK |
| #endif |
| |
| /* Resampling parameters */ |
| #define INRATE 65 * SAMPSPERSYM |
| #define INHISTORY INRATE * 2 |
| #define INCHUNK INRATE * 9 |
| |
| #define OUTRATE 96 * SAMPSPERSYM |
| #define OUTHISTORY OUTRATE * 2 |
| #define OUTCHUNK OUTRATE * 9 |
| |
| /* Resampler low pass filters */ |
| signalVector *tx_lpf = 0; |
| signalVector *rx_lpf = 0; |
| |
| /* Resampler history */ |
| signalVector *tx_hist = 0; |
| signalVector *rx_hist = 0; |
| |
| /* Resampler input buffer */ |
| signalVector *tx_vec = 0; |
| signalVector *rx_vec = 0; |
| |
| /* High rate (device facing) buffers */ |
| short tx_buf[INCHUNK * 2 * 2]; |
| short rx_buf[OUTCHUNK * 2 * 2]; |
| |
| /* |
| * Utilities and Conversions |
| * |
| * Manipulate signal vectors dynamically for two reasons. For one, |
| * it's simpler. And two, it doesn't make any reasonable difference |
| * relative to the high overhead generated by the resampling. |
| */ |
| |
| /* Concatenate signal vectors. Deallocate input vectors. */ |
| signalVector *concat(signalVector *a, signalVector *b) |
| { |
| signalVector *vec = new signalVector(*a, *b); |
| delete a; |
| delete b; |
| |
| return vec; |
| } |
| |
| /* Segment a signal vector. Deallocate the input vector. */ |
| signalVector *segment(signalVector *a, int indx, int sz) |
| { |
| signalVector *vec = new signalVector(sz); |
| a->segmentCopyTo(*vec, indx, sz); |
| delete a; |
| |
| return vec; |
| } |
| |
| /* Create a new signal vector from a short array. */ |
| signalVector *short_to_sigvec(short *smpls, size_t sz) |
| { |
| int i; |
| signalVector *vec = new signalVector(sz); |
| signalVector::iterator itr = vec->begin(); |
| |
| for (i = 0; i < sz; i++) { |
| *itr++ = Complex<float>(smpls[2 * i + 0], smpls[2 * i + 1]); |
| } |
| |
| return vec; |
| } |
| |
| /* Convert and deallocate a signal vector into a short array. */ |
| int sigvec_to_short(signalVector *vec, short *smpls) |
| { |
| int i; |
| signalVector::iterator itr = vec->begin(); |
| |
| for (i = 0; i < vec->size(); i++) { |
| smpls[2 * i + 0] = itr->real(); |
| smpls[2 * i + 1] = itr->imag(); |
| itr++; |
| } |
| delete vec; |
| |
| return i; |
| } |
| |
| /* Create a new signal vector from a float array. */ |
| signalVector *float_to_sigvec(float *smpls, int sz) |
| { |
| int i; |
| signalVector *vec = new signalVector(sz); |
| signalVector::iterator itr = vec->begin(); |
| |
| for (i = 0; i < sz; i++) { |
| *itr++ = Complex<float>(smpls[2 * i + 0], smpls[2 * i + 1]); |
| } |
| |
| return vec; |
| } |
| |
| /* Convert and deallocate a signal vector into a float array. */ |
| int sigvec_to_float(signalVector *vec, float *smpls) |
| { |
| int i; |
| signalVector::iterator itr = vec->begin(); |
| |
| for (i = 0; i < vec->size(); i++) { |
| smpls[2 * i + 0] = itr->real(); |
| smpls[2 * i + 1] = itr->imag(); |
| itr++; |
| } |
| delete vec; |
| |
| return i; |
| } |
| |
| /* Initialize resampling signal vectors */ |
| void init_resampler(signalVector **lpf, |
| signalVector **buf, |
| signalVector **hist, |
| int tx) |
| { |
| int P, Q, taps, hist_len; |
| float cutoff_freq; |
| |
| if (tx) { |
| LOG(INFO) << "Initializing Tx resampler"; |
| P = OUTRATE; |
| Q = INRATE; |
| taps = 651; |
| hist_len = INHISTORY; |
| } else { |
| LOG(INFO) << "Initializing Rx resampler"; |
| P = INRATE; |
| Q = OUTRATE; |
| taps = 961; |
| hist_len = OUTHISTORY; |
| } |
| |
| if (!*lpf) { |
| cutoff_freq = (P < Q) ? (1.0/(float) Q) : (1.0/(float) P); |
| *lpf = createLPF(cutoff_freq, taps, P); |
| } |
| |
| if (!*buf) { |
| *buf = new signalVector(); |
| } |
| |
| if (!*hist); |
| *hist = new signalVector(hist_len); |
| } |
| |
| /* Resample a signal vector |
| * |
| * The input vector is deallocated and the pointer returned with a vector |
| * of any unconverted samples. |
| */ |
| signalVector *resmpl_sigvec(signalVector *hist, signalVector **vec, |
| signalVector *lpf, double in_rate, |
| double out_rate, int chunk_sz) |
| { |
| signalVector *resamp_vec; |
| int num_chunks = (*vec)->size() / chunk_sz; |
| |
| /* Truncate to a chunk multiple */ |
| signalVector trunc_vec(num_chunks * chunk_sz); |
| (*vec)->segmentCopyTo(trunc_vec, 0, num_chunks * chunk_sz); |
| |
| /* Update sample buffer with remainder */ |
| *vec = segment(*vec, trunc_vec.size(), (*vec)->size() - trunc_vec.size()); |
| |
| /* Add history and resample */ |
| signalVector input_vec(*hist, trunc_vec); |
| resamp_vec = polyphaseResampleVector(input_vec, in_rate, |
| out_rate, lpf); |
| |
| /* Update history */ |
| trunc_vec.segmentCopyTo(*hist, trunc_vec.size() - hist->size(), |
| hist->size()); |
| return resamp_vec; |
| } |
| |
| /* Wrapper for receive-side integer-to-float array resampling */ |
| int rx_resmpl_int_flt(float *smpls_out, short *smpls_in, int num_smpls) |
| { |
| int num_resmpld, num_chunks; |
| signalVector *convert_vec, *resamp_vec, *trunc_vec; |
| |
| if (!rx_lpf || !rx_vec || !rx_hist) |
| init_resampler(&rx_lpf, &rx_vec, &rx_hist, false); |
| |
| /* Convert and add samples to the receive buffer */ |
| convert_vec = short_to_sigvec(smpls_in, num_smpls); |
| rx_vec = concat(rx_vec, convert_vec); |
| |
| num_chunks = rx_vec->size() / OUTCHUNK; |
| if (num_chunks < 1) |
| return 0; |
| |
| /* Resample */ |
| resamp_vec = resmpl_sigvec(rx_hist, &rx_vec, rx_lpf, |
| INRATE, OUTRATE, OUTCHUNK); |
| /* Truncate */ |
| trunc_vec = segment(resamp_vec, INHISTORY, |
| resamp_vec->size() - INHISTORY); |
| /* Convert */ |
| num_resmpld = sigvec_to_float(trunc_vec, smpls_out); |
| |
| return num_resmpld; |
| } |
| |
| /* Wrapper for transmit-side float-to-int array resampling */ |
| int tx_resmpl_flt_int(short *smpls_out, float *smpls_in, int num_smpls) |
| { |
| int num_resmpl, num_chunks; |
| signalVector *convert_vec, *resamp_vec; |
| |
| if (!tx_lpf || !tx_vec || !tx_hist) |
| init_resampler(&tx_lpf, &tx_vec, &tx_hist, true); |
| |
| /* Convert and add samples to the transmit buffer */ |
| convert_vec = float_to_sigvec(smpls_in, num_smpls); |
| tx_vec = concat(tx_vec, convert_vec); |
| |
| num_chunks = tx_vec->size() / INCHUNK; |
| if (num_chunks < 1) |
| return 0; |
| |
| /* Resample and convert to an integer array */ |
| resamp_vec = resmpl_sigvec(tx_hist, &tx_vec, tx_lpf, |
| OUTRATE, INRATE, INCHUNK); |
| num_resmpl = sigvec_to_short(resamp_vec, smpls_out); |
| |
| return num_resmpl; |
| } |
| |
| /* Receive a timestamped chunk from the device */ |
| void RadioInterface::pullBuffer() |
| { |
| int num_cv, num_rd; |
| bool local_underrun; |
| |
| /* Read samples. Fail if we don't get what we want. */ |
| num_rd = mRadio->readSamples(rx_buf, OUTCHUNK, &overrun, |
| readTimestamp, &local_underrun); |
| |
| LOG(DEEPDEBUG) << "Rx read " << num_rd << " samples from device"; |
| assert(num_rd == OUTCHUNK); |
| |
| underrun |= local_underrun; |
| readTimestamp += (TIMESTAMP) num_rd; |
| |
| /* Convert and resample */ |
| num_cv = rx_resmpl_int_flt(rcvBuffer + 2 * rcvCursor, |
| rx_buf, num_rd); |
| |
| LOG(DEEPDEBUG) << "Rx read " << num_cv << " samples from resampler"; |
| |
| rcvCursor += num_cv; |
| } |
| |
| /* Send a timestamped chunk to the device */ |
| void RadioInterface::pushBuffer() |
| { |
| int num_cv, num_wr; |
| |
| if (sendCursor < INCHUNK) |
| return; |
| |
| LOG(DEEPDEBUG) << "Tx wrote " << sendCursor << " samples to resampler"; |
| |
| /* Resample and convert */ |
| num_cv = tx_resmpl_flt_int(tx_buf, sendBuffer, sendCursor); |
| assert(num_cv > sendCursor); |
| |
| /* Write samples. Fail if we don't get what we want. */ |
| num_wr = mRadio->writeSamples(tx_buf + OUTHISTORY * 2, |
| num_cv - OUTHISTORY, |
| &underrun, |
| writeTimestamp); |
| |
| LOG(DEEPDEBUG) << "Tx wrote " << num_wr << " samples to device"; |
| assert(num_wr == num_wr); |
| |
| writeTimestamp += (TIMESTAMP) num_wr; |
| sendCursor = 0; |
| } |