static void dsp_calculate_fft(dsp16_t *fft, dsp16_t *signal)
{
int i;
dsp16_t max_value;
dsp16_t temp[BUFFER_LENGTH];
dsp16_complex_t temp_res[BUFFER_LENGTH];
dsp16_vect_dotmul(temp, signal, fft_window, BUFFER_LENGTH);
dsp16_trans_realcomplexfft(temp_res, temp, BUFFER_LENGTH_LOG);
dsp16_vect_complex_abs(fft, temp_res, BUFFER_LENGTH);
for (i = 0; i < FFT_LENGTH; i++)
fft[i] += fft[BUFFER_LENGTH - i - 1];
max_value = dsp16_vect_max(fft, FFT_LENGTH);
dsp16_vect_realdiv(fft, fft, FFT_LENGTH, max_value + 1);
}
void calculate_radar(dsp16_t i_buffer[], dsp16_t q_buffer[])
{
int32_t i = 0;
int32_t j = 0;
int32_t counter = 0;
int32_t reading_status = 3;
int32_t* c = 0;
int32_t read_value = 0;
int32_t index = 0;
dsp16_trans_realcomplexfft(vect_outputI, i_buffer, FFT_POWER); //WARNING !! If you change the buffer size you need to change the base 2 power size
dsp16_trans_realcomplexfft(vect_outputQ, q_buffer, FFT_POWER); //WARNING !! If you change the buffer size you need to change the base 2 power size
for (i = 0; i < RADAR_BUFFER_SIZE; i++)
{
fft_amp[i] = maths_fast_sqrt(SQR(vect_outputI[i].real) + SQR(vect_outputI[i].imag));
}
//Find maximum of FFT and corresponding frequency
//time1 = time_keeper_get_us();
amplitude = 0;
index = 0;
for (i = 1; i < RADAR_BUFFER_SIZE / 2 - 1; i++) //ignore the element 0 (low frequency noise)
{
if (fft_amp[i] > amplitude)
{
amplitude = fft_amp[i];
index = i; //find index of max
}
}
//Let's find the second maximum peak
amplitude2 = 0;
index2 = 0;
for (i = 1; i < RADAR_BUFFER_SIZE / 2 - 1; i++)
{
if (fft_amp[i] > amplitude2 && i != index)
{
amplitude2 = fft_amp[i];
index2 = i;
}
}
//Don't need to test the following with index2 because index corresponds to the absolute maximum anyway
if (index > 1)
{
mean_amp = (2 * fft_amp[index] + fft_amp[index + 1] + fft_amp[index - 1]) / 4; //Average on the three peaks in Fourier domain
}
else
{
mean_amp = (fft_amp[index] + fft_amp[index + 1]) / 2; //Same average
}
if (mean_amp < THRESHOLD)
{
mean_amp = 0;
index = 0;
index2 = 0;
amp_old = 0;
speed = 0;
}
else
{
//lowpass exponential filtering applied (alpha =0.75f so we multiply by 100, do the operations then divide by 100)
if (index > 1)
{
mean_amp = (alpha * ((fft_amp[index] + fft_amp[index + 1] + fft_amp[index - 1]) / 3) + (filter_conversion - alpha) * amp_old) / filter_conversion;
amp_old = mean_amp; //update the value of the previous amplitude for next iteration
}
else
{
mean_amp = (alpha * ((fft_amp[index] + fft_amp[index + 1]) / 2) + (filter_conversion - alpha) * amp_old) / filter_conversion;
amp_old = mean_amp;
}
//Factor 1000 for speed to avoid decimal value
//The true speed is (speed) / 100 (m / s) or interpret it as cm / s
frequency = f_v_factor * 10 * (index);
speed = 1000 * (3 * powf(10, 8) * frequency / (2 * 2415 * pow(10, 9))); //compute speed (*0.01f because of Fsample and *10 because of freq)
frequency = f_v_factor * 10 * (index2);
speed2 = 1000 * (3 * powf(10, 8) * frequency / (2 * 2415 * pow(10, 9))); //compute speed (*0.01f because of Fsample and *10 because of freq)
//Let's find the speed that is closer to the previous one (to avoid high frequency changes for the speed)
if (abs(speed - speed_old) <= abs(speed2 - speed_old))
{
speed_old = speed; //update the old speed
}
else if (abs(speed - speed_old) > abs(speed2 - speed_old)) // in this case the speed is the one corresponding to the second peak
{
speed_old = speed2; //update the old speed
speed = speed2;
index = index2;
if (index > 1)
{
mean_amp = (alpha * ((fft_amp[index2] + fft_amp[index2 + 1] + fft_amp[index2 - 1]) / 3) + (filter_conversion - alpha) * amp_old) / filter_conversion;
amp_old = mean_amp;
}
else
{
//mean_amp = (vect_inputQ[index2] + vect_inputQ[index2 + 1]) / 2;
mean_amp = (alpha * ((fft_amp[index2] + fft_amp[index2 + 1]) / 2) + (filter_conversion - alpha) * amp_old) / filter_conversion;
amp_old = mean_amp;
}
}
}
//Find the direction of motion by doing vector product between I and Q channel
if (vect_outputI[index].real * vect_outputQ[index].imag - vect_outputI[index].imag * vect_outputQ[index].real < 0)
{
direction = -1;
}
else if (speed == 0)
{
direction = 0;
}
else
{
direction = 1;
}
for (i = 0; i < RADAR_BUFFER_SIZE; i++)
{
if (vect_outputI[index].real * vect_outputQ[index].imag - vect_outputI[index].imag * vect_outputQ[index].real < 0)
{
fft_amp[i] = -fft_amp[i];
}
}
time2 = time_keeper_get_us();
time_result = time2 - time1;
time1 = time_keeper_get_us();
main_target.velocity = direction * speed / 100.0f;
//main_target.velocity = speed / 100.0f;
main_target.amplitude = amplitude;
amplitude = 0;
amplitude2 = 0;
index = 0;
index2 = 0;
input_min = 0;
input_max = 0;
rms = 0;
mean_amp = 0;
read_value = 0;
}
