float shift_addition_cc(complexf *input, complexf* output, int input_size, shift_addition_data_t d, float starting_phase)
{
//The original idea was taken from wdsp:
//http://svn.tapr.org/repos_sdr_hpsdr/trunk/W5WC/PowerSDR_HPSDR_mRX_PS/Source/wdsp/shift.c
//However, this method introduces noise (from floating point rounding errors), which increases until the end of the buffer.
//fprintf(stderr, "cosd=%g sind=%g\n", d.cosdelta, d.sindelta);
float cosphi=cos(starting_phase);
float sinphi=sin(starting_phase);
float cosphi_last, sinphi_last;
for(int i=0;i<input_size;i++) //@shift_addition_cc: work
{
iof(output,i)=cosphi*iof(input,i)-sinphi*qof(input,i);
qof(output,i)=sinphi*iof(input,i)+cosphi*qof(input,i);
//using the trigonometric addition formulas
//cos(phi+delta)=cos(phi)cos(delta)-sin(phi)*sin(delta)
cosphi_last=cosphi;
sinphi_last=sinphi;
cosphi=cosphi_last*d.cosdelta-sinphi_last*d.sindelta;
sinphi=sinphi_last*d.cosdelta+cosphi_last*d.sindelta;
}
starting_phase+=d.rate*PI*input_size;
while(starting_phase>PI) starting_phase-=2*PI; //@shift_addition_cc: normalize starting_phase
while(starting_phase<-PI) starting_phase+=2*PI;
return starting_phase;
}
void logpower_cf(complexf* input, float* output, int size, float add_db)
{
for(int i=0;i<size;i++) output[i]=iof(input,i)*iof(input,i) + qof(input,i)*qof(input,i); //@logpower_cf: pass 1
for(int i=0;i<size;i++) output[i]=log10(output[i]); //@logpower_cf: pass 2
for(int i=0;i<size;i++) output[i]=10*output[i]+add_db; //@logpower_cf: pass 3
}
void apply_window_c(complexf* input, complexf* output, int size, window_t window)
{
float (*window_function)(float)=firdes_get_window_kernel(window);
for(int i=0;i<size;i++) //@apply_window_c
{
float rate=(float)i/(size-1);
iof(output,i)=iof(input,i)*window_function(2.0*rate+1.0);
qof(output,i)=qof(input,i)*window_function(2.0*rate+1.0);
}
}
void amdemod_cf(complexf* input, float *output, int input_size)
{
//@amdemod: i*i+q*q
for (int i=0; i<input_size; i++)
{
output[i]=iof(input,i)*iof(input,i)+qof(input,i)*qof(input,i);
}
//@amdemod: sqrt
for (int i=0; i<input_size; i++)
{
output[i]=sqrt(output[i]);
}
}
complexf fmdemod_quadri_novect_cf(complexf* input, float* output, int input_size, complexf last_sample)
{
output[0]=fmdemod_quadri_K*(iof(input,0)*(qof(input,0)-last_sample.q)-qof(input,0)*(iof(input,0)-last_sample.i))/(iof(input,0)*iof(input,0)+qof(input,0)*qof(input,0));
for (int i=1; i<input_size; i++) //@fmdemod_quadri_novect_cf
{
float qnow=qof(input,i);
float qlast=qof(input,i-1);
float inow=iof(input,i);
float ilast=iof(input,i-1);
output[i]=fmdemod_quadri_K*(inow*(qnow-qlast)-qnow*(inow-ilast))/(inow*inow+qnow*qnow);
//TODO: expression can be simplified as: (qnow*ilast-inow*qlast)/(inow*inow+qnow*qnow)
}
return input[input_size-1];
}
void amdemod_estimator_cf(complexf* input, float *output, int input_size, float alpha, float beta)
{
//concept is explained here:
//http://www.dspguru.com/dsp/tricks/magnitude-estimator
//default: optimize for min RMS error
if(alpha==0)
{
alpha=0.947543636291;
beta=0.392485425092;
}
//@amdemod_estimator
for (int i=0; i<input_size; i++)
{
float abs_i=iof(input,i);
if(abs_i<0) abs_i=-abs_i;
float abs_q=qof(input,i);
if(abs_q<0) abs_q=-abs_q;
float max_iq=abs_i;
if(abs_q>max_iq) max_iq=abs_q;
float min_iq=abs_i;
if(abs_q<min_iq) min_iq=abs_q;
output[i]=alpha*max_iq+beta*min_iq;
}
}
complexf fmdemod_quadri_cf(complexf* input, float* output, int input_size, float *temp, complexf last_sample)
{
float* temp_dq=temp;
float* temp_di=temp+input_size;
temp_dq[0]=qof(input,0)-last_sample.q;
for (int i=1; i<input_size; i++) //@fmdemod_quadri_cf: dq
{
temp_dq[i]=qof(input,i)-qof(input,i-1);
}
temp_di[0]=iof(input,0)-last_sample.i;
for (int i=1; i<input_size; i++) //@fmdemod_quadri_cf: di
{
temp_di[i]=iof(input,i)-iof(input,i-1);
}
for (int i=0; i<input_size; i++) //@fmdemod_quadri_cf: output numerator
{
output[i]=(iof(input,i)*temp_dq[i]-qof(input,i)*temp_di[i]);
}
for (int i=0; i<input_size; i++) //@fmdemod_quadri_cf: output denomiator
{
temp[i]=iof(input,i)*iof(input,i)+qof(input,i)*qof(input,i);
}
for (int i=0; i<input_size; i++) //@fmdemod_quadri_cf: output division
{
output[i]=fmdemod_quadri_K*output[i]/temp[i];
}
return input[input_size-1];
}
int fir_decimate_cc(complexf *input, complexf *output, int input_size, int decimation, float *taps, int taps_length)
{
//Theory: http://www.dspguru.com/dsp/faqs/multirate/decimation
//It uses real taps. It returns the number of output samples actually written.
//It needs overlapping input based on its returned value:
//number of processed input samples = returned value * decimation factor
//The output buffer should be at least input_length / 3.
// i: input index | ti: tap index | oi: output index
int oi=0;
for(int i=0; i<input_size; i+=decimation) //@fir_decimate_cc: outer loop
{
if(i+taps_length>input_size) break;
float acci=0;
for(int ti=0; ti<taps_length; ti++) acci += (iof(input,i+ti)) * taps[ti]; //@fir_decimate_cc: i loop
float accq=0;
for(int ti=0; ti<taps_length; ti++) accq += (qof(input,i+ti)) * taps[ti]; //@fir_decimate_cc: q loop
iof(output,oi)=acci;
qof(output,oi)=accq;
oi++;
}
return oi;
}
float shift_math_cc(complexf *input, complexf* output, int input_size, float rate, float starting_phase)
{
rate*=2;
//Shifts the complex spectrum. Basically a complex mixer. This version uses cmath.
float phase=starting_phase;
float phase_increment=rate*PI;
float cosval, sinval;
for(int i=0;i<input_size; i++) //@shift_math_cc
{
cosval=cos(phase);
sinval=sin(phase);
//we multiply two complex numbers.
//how? enter this to maxima (software) for explanation:
// (a+b*%i)*(c+d*%i), rectform;
iof(output,i)=cosval*iof(input,i)-sinval*qof(input,i);
qof(output,i)=sinval*iof(input,i)+cosval*qof(input,i);
phase+=phase_increment;
while(phase>2*PI) phase-=2*PI; //@shift_math_cc: normalize phase
while(phase<0) phase+=2*PI;
}
return phase;
}
float shift_table_cc(complexf* input, complexf* output, int input_size, float rate, shift_table_data_t table_data, float starting_phase)
{
//RTODO
rate*=2;
//Shifts the complex spectrum. Basically a complex mixer. This version uses a pre-built sine table.
float phase=starting_phase;
float phase_increment=rate*PI;
float cosval, sinval;
for(int i=0;i<input_size; i++) //@shift_math_cc
{
int sin_index, cos_index, temp_index, sin_sign, cos_sign;
//float vphase=fmodf(phase,PI/2); //between 0 and 90deg
int quadrant=phase/(PI/2); //between 0 and 3
float vphase=phase-quadrant*(PI/2);
sin_index=(vphase/(PI/2))*table_data.table_size;
cos_index=table_data.table_size-1-sin_index;
if(quadrant&1) //in quadrant 1 and 3
{
temp_index=sin_index;
sin_index=cos_index;
cos_index=temp_index;
}
sin_sign=(quadrant>1)?-1:1; //in quadrant 2 and 3
cos_sign=(quadrant&&quadrant<3)?-1:1; //in quadrant 1 and 2
sinval=sin_sign*table_data.table[sin_index];
cosval=cos_sign*table_data.table[cos_index];
//we multiply two complex numbers.
//how? enter this to maxima (software) for explanation:
// (a+b*%i)*(c+d*%i), rectform;
iof(output,i)=cosval*iof(input,i)-sinval*qof(input,i);
qof(output,i)=sinval*iof(input,i)+cosval*qof(input,i);
phase+=phase_increment;
while(phase>2*PI) phase-=2*PI; //@shift_math_cc: normalize phase
while(phase<0) phase+=2*PI;
}
return phase;
}
void apply_fir_fft_cc(FFT_PLAN_T* plan, FFT_PLAN_T* plan_inverse, complexf* taps_fft, complexf* last_overlap, int overlap_size)
{
//use the overlap & add method for filtering
//calculate FFT on input buffer
fft_execute(plan);
//multiply the filter and the input
complexf* in = plan->output;
complexf* out = plan_inverse->input;
for(int i=0;i<plan->size;i++) //@apply_fir_fft_cc: multiplication
{
iof(out,i)=iof(in,i)*iof(taps_fft,i)-qof(in,i)*qof(taps_fft,i);
qof(out,i)=iof(in,i)*qof(taps_fft,i)+qof(in,i)*iof(taps_fft,i);
}
//calculate inverse FFT on multiplied buffer
fft_execute(plan_inverse);
//add the overlap of the previous segment
complexf* result = plan_inverse->output;
for(int i=0;i<plan->size;i++) //@apply_fir_fft_cc: normalize by fft_size
{
iof(result,i)/=plan->size;
qof(result,i)/=plan->size;
}
for(int i=0;i<overlap_size;i++) //@apply_fir_fft_cc: add overlap
{
iof(result,i)=iof(result,i)+iof(last_overlap,i);
qof(result,i)=qof(result,i)+qof(last_overlap,i);
}
}
void fft_swap_sides(complexf* io, int fft_size)
{
int middle=fft_size/2;
complexf temp;
for(int i=0;i<middle;i++)
{
iof(&temp,0)=iof(io,i);
qof(&temp,0)=qof(io,i);
iof(io,i)=iof(io,i+middle);
qof(io,i)=qof(io,i+middle);
iof(io,i+middle)=iof(&temp,0);
qof(io,i+middle)=qof(&temp,0);
}
}
void firdes_bandpass_c(complexf *output, int length, float lowcut, float highcut, window_t window)
{
//To generate a complex filter:
// 1. we generate a real lowpass filter with a bandwidth of highcut-lowcut
// 2. we shift the filter taps spectrally by multiplying with e^(j*w), so we get complex taps
//(tnx HA5FT)
float* realtaps = (float*)malloc(sizeof(float)*length);
firdes_lowpass_f(realtaps, length, (highcut-lowcut)/2, window);
float filter_center=(highcut+lowcut)/2;
float phase=0, sinval, cosval;
for(int i=0; i<length; i++) //@@firdes_bandpass_c
{
cosval=cos(phase);
sinval=sin(phase);
phase+=2*PI*filter_center;
while(phase>2*PI) phase-=2*PI; //@@firdes_bandpass_c
while(phase<0) phase+=2*PI;
iof(output,i)=cosval*realtaps[i];
qof(output,i)=sinval*realtaps[i];
//output[i] := realtaps[i] * e^j*w
}
}
decimating_shift_addition_status_t fastddc_inv_cc(complexf* input, complexf* output, fastddc_t* ddc, FFT_PLAN_T* plan_inverse, complexf* taps_fft, decimating_shift_addition_status_t shift_stat)
{
//implements DDC by using the overlap & scrap method
//TODO: +/-1s on overlap_size et al
//input shoud have ddc->fft_size number of elements
complexf* inv_input = plan_inverse->input;
complexf* inv_output = plan_inverse->output;
//Initialize buffers for inverse FFT to zero
for(int i=0;i<plan_inverse->size;i++)
{
iof(inv_input,i)=0;
qof(inv_input,i)=0;
}
//Alias & shift & filter at once
fft_swap_sides(input, ddc->fft_size); //TODO this is not very optimal, but now we stick with this slow solution until we got the algorithm working
//fprintf(stderr, " === fastddc_inv_cc() ===\n");
//The problem is, we have to say that the output_index should be the _center_ of the spectrum when i is at startbin! (startbin is at the _center_ of the input to downconvert, not at its first bin!)
for(int i=0;i<ddc->fft_size;i++)
{
int output_index = (ddc->fft_size+i-ddc->offsetbin+(ddc->fft_inv_size/2))%plan_inverse->size;
int tap_index = i;
//fprintf(stderr, "output_index = %d , tap_index = %d, input index = %d\n", output_index, tap_index, i);
//cmultadd(inv_input+output_index, input+i, taps_fft+tap_index); //cmultadd(output, input1, input2): complex output += complex input1 * complex input 2
// (a+b*i)*(c+d*i) = (ac-bd)+(ad+bc)*i
// a = iof(input,i)
// b = qof(input,i)
// c = iof(taps_fft,i)
// d = qof(taps_fft,i)
iof(inv_input,output_index) += iof(input,i) * iof(taps_fft,i) - qof(input,i) * qof(taps_fft,i);
qof(inv_input,output_index) += iof(input,i) * qof(taps_fft,i) + qof(input,i) * iof(taps_fft,i);
//iof(inv_input,output_index) += iof(input,i); //no filter
//qof(inv_input,output_index) += qof(input,i);
}
//Normalize inv fft bins (now our output level is not higher than the input... but we may optimize this into the later loop when we normalize by size)
for(int i=0;i<plan_inverse->size;i++)
{
iof(inv_input,i)/=ddc->pre_decimation;
qof(inv_input,i)/=ddc->pre_decimation;
}
fft_swap_sides(inv_input,plan_inverse->size);
fft_execute(plan_inverse);
//Normalize data
for(int i=0;i<plan_inverse->size;i++) //@fastddc_inv_cc: normalize by size
{
iof(inv_output,i)/=plan_inverse->size;
qof(inv_output,i)/=plan_inverse->size;
}
//Overlap is scrapped, not added
//Shift correction
shift_stat=decimating_shift_addition_cc(inv_output+ddc->scrap, output, ddc->post_input_size, ddc->dsadata, ddc->post_decimation, shift_stat);
//shift_stat.output_size = ddc->post_input_size; //bypass shift correction
//memcpy(output, inv_output+ddc->scrap, sizeof(complexf)*ddc->post_input_size);
return shift_stat;
}