Huang
791d449b6d
git-svn-id: http://svn.eurecom.fr/openair4G/trunk@5085 818b1a75-f10b-46b9-bf7c-635c3b92a50f
62 lines
1.9 KiB
C++
62 lines
1.9 KiB
C++
#include "polyphaseResample.h"
|
|
#include <iostream>
|
|
#include <stdio.h>
|
|
using namespace std;
|
|
|
|
// table_len should be decied by the length of the filter output
|
|
// but a default value of 50000 is adequate for our app
|
|
void buildPolyphaseCache(const int P, const int Q, int *&branch_table, int *&offset_table, const int table_len) {
|
|
branch_table = new int[table_len];
|
|
offset_table = new int[table_len];
|
|
for (int i = 0; i < table_len; ++i) {
|
|
branch_table[i] = (i * Q) % P;
|
|
offset_table[i] = (i * Q - branch_table[i]) / P;
|
|
}
|
|
}
|
|
|
|
void polyphaseResample(short *in_data, unsigned int in_sample_count,
|
|
short *out_data, unsigned int out_sample_count,
|
|
short *filter_coeff, unsigned int filter_len,
|
|
const int P, const int Q, const unsigned int fix_shift,
|
|
int *branch_table, int *offset_table) {
|
|
buildPolyphaseCache( P, Q, branch_table, offset_table, 50000);
|
|
// calculate the sample count of output
|
|
out_sample_count = (int)(in_sample_count * (float)P / (float)Q);
|
|
|
|
short *output_ptr = out_data;
|
|
unsigned int output_ix = 0;
|
|
short const *input_end = in_data + in_sample_count * 2;
|
|
short const *output_end = out_data + out_sample_count * 2;
|
|
short const *filter_end = filter_coeff + filter_len;
|
|
while (output_ptr < output_end)
|
|
{
|
|
int output_branch = branch_table[output_ix];
|
|
int input_offset = offset_table[output_ix];
|
|
|
|
short *input_ptr = in_data + 2 * input_offset;
|
|
short *filter_ptr = filter_coeff + output_branch;
|
|
|
|
while (input_ptr >= input_end) {
|
|
input_ptr -= 2;
|
|
filter_ptr += P;
|
|
}
|
|
|
|
int sum_real = 0, sum_imag = 0;
|
|
while ((input_ptr >= in_data) && (filter_ptr < filter_end)) {
|
|
sum_real += (int)(*input_ptr) * (int)(*filter_ptr);
|
|
sum_imag += (int)(*(input_ptr + 1)) * (int)(*filter_ptr);
|
|
input_ptr -= 2;
|
|
filter_ptr += P;
|
|
}
|
|
|
|
*output_ptr = (short)(sum_real >> fix_shift);
|
|
*(output_ptr + 1) = (short)(sum_imag >> fix_shift);
|
|
|
|
output_ptr += 2;
|
|
output_ix++;
|
|
}
|
|
}
|
|
|
|
|
|
|