void airspeed_analog_update(airspeed_analog_t* airspeed_analog)
{
float raw_airspeed = 0.0f;
///< measure differential pressure and remove offset
airspeed_analog->differential_pressure = airspeed_analog_get_pressure(airspeed_analog) - airspeed_analog->pressure_offset;
///< Avoid negative pressures
airspeed_analog->differential_pressure = abs(airspeed_analog->differential_pressure);
///< compute airspeed from differential pressure using Bernouilli
raw_airspeed = maths_fast_sqrt(airspeed_analog->differential_pressure * airspeed_analog->gain);
///< Filter airspeed estimation
airspeed_analog->airspeed = 0.7f * airspeed_analog->airspeed + 0.3f * raw_airspeed;
}
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;
}
void qfilter_update(qfilter_t *qf)
{
static uint32_t convergence_update_count = 0;
float omc[3], omc_mag[3] , tmp[3], snorm, norm, s_acc_norm, acc_norm, s_mag_norm, mag_norm;
quat_t qed, qtmp1, up, up_bf;
quat_t mag_global, mag_corrected_local;
quat_t front_vec_global =
{
.s = 0.0f,
.v = {1.0f, 0.0f, 0.0f}
};
float kp = qf->kp;
float kp_mag = qf->kp_mag;
float ki = qf->ki;
float ki_mag = qf->ki_mag;
// Update time in us
float now_us = time_keeper_get_micros();
// Delta t in seconds
float dt = 1e-6 * (float)(now_us - qf->ahrs->last_update);
// Write to ahrs structure
qf->ahrs->dt = dt;
qf->ahrs->last_update = now_us;
// up_bf = qe^-1 *(0,0,0,-1) * qe
up.s = 0; up.v[X] = UPVECTOR_X; up.v[Y] = UPVECTOR_Y; up.v[Z] = UPVECTOR_Z;
up_bf = quaternions_global_to_local(qf->ahrs->qe, up);
// calculate norm of acceleration vector
s_acc_norm = qf->imu->scaled_accelero.data[X] * qf->imu->scaled_accelero.data[X] + qf->imu->scaled_accelero.data[Y] * qf->imu->scaled_accelero.data[Y] + qf->imu->scaled_accelero.data[Z] * qf->imu->scaled_accelero.data[Z];
if ( (s_acc_norm > 0.7f * 0.7f) && (s_acc_norm < 1.3f * 1.3f) )
{
// approximate square root by running 2 iterations of newton method
acc_norm = maths_fast_sqrt(s_acc_norm);
tmp[X] = qf->imu->scaled_accelero.data[X] / acc_norm;
tmp[Y] = qf->imu->scaled_accelero.data[Y] / acc_norm;
tmp[Z] = qf->imu->scaled_accelero.data[Z] / acc_norm;
// omc = a x up_bf.v
CROSS(tmp, up_bf.v, omc);
}
else
{
omc[X] = 0;
omc[Y] = 0;
omc[Z] = 0;
}
// Heading computation
// transfer
qtmp1 = quaternions_create_from_vector(qf->imu->scaled_compass.data);
mag_global = quaternions_local_to_global(qf->ahrs->qe, qtmp1);
// calculate norm of compass vector
//s_mag_norm = SQR(mag_global.v[X]) + SQR(mag_global.v[Y]) + SQR(mag_global.v[Z]);
s_mag_norm = SQR(mag_global.v[X]) + SQR(mag_global.v[Y]);
if ( (s_mag_norm > 0.004f * 0.004f) && (s_mag_norm < 1.8f * 1.8f) )
{
mag_norm = maths_fast_sqrt(s_mag_norm);
mag_global.v[X] /= mag_norm;
mag_global.v[Y] /= mag_norm;
mag_global.v[Z] = 0.0f; // set z component in global frame to 0
// transfer magneto vector back to body frame
qf->ahrs->north_vec = quaternions_global_to_local(qf->ahrs->qe, front_vec_global);
mag_corrected_local = quaternions_global_to_local(qf->ahrs->qe, mag_global);
// omc = a x up_bf.v
CROSS(mag_corrected_local.v, qf->ahrs->north_vec.v, omc_mag);
}
else
{
omc_mag[X] = 0;
omc_mag[Y] = 0;
omc_mag[Z] = 0;
}
// get error correction gains depending on mode
switch (qf->ahrs->internal_state)
{
case AHRS_UNLEVELED:
kp = qf->kp * 10.0f;
kp_mag = qf->kp_mag * 10.0f;
ki = 0.5f * qf->ki;
ki_mag = 0.5f * qf->ki_mag;
convergence_update_count += 1;
if( convergence_update_count > 2000 )
{
convergence_update_count = 0;
qf->ahrs->internal_state = AHRS_CONVERGING;
print_util_dbg_print("End of AHRS attitude initialization.\r\n");
}
break;
case AHRS_CONVERGING:
kp = qf->kp;
kp_mag = qf->kp_mag;
ki = qf->ki * 3.0f;
ki_mag = qf->ki_mag * 3.0f;
convergence_update_count += 1;
if( convergence_update_count > 2000 )
{
convergence_update_count = 0;
qf->ahrs->internal_state = AHRS_READY;
print_util_dbg_print("End of AHRS leveling.\r\n");
}
break;
case AHRS_READY:
kp = qf->kp;
kp_mag = qf->kp_mag;
ki = qf->ki;
ki_mag = qf->ki_mag;
break;
}
// apply error correction with appropriate gains for accelerometer and compass
for (uint8_t i = 0; i < 3; i++)
{
qtmp1.v[i] = 0.5f * (qf->imu->scaled_gyro.data[i] + kp * omc[i] + kp_mag * omc_mag[i]);
}
qtmp1.s = 0;
// apply step rotation with corrections
qed = quaternions_multiply(qf->ahrs->qe,qtmp1);
// TODO: correct this formulas!
qf->ahrs->qe.s = qf->ahrs->qe.s + qed.s * dt;
qf->ahrs->qe.v[X] += qed.v[X] * dt;
qf->ahrs->qe.v[Y] += qed.v[Y] * dt;
qf->ahrs->qe.v[Z] += qed.v[Z] * dt;
snorm = qf->ahrs->qe.s * qf->ahrs->qe.s + qf->ahrs->qe.v[X] * qf->ahrs->qe.v[X] + qf->ahrs->qe.v[Y] * qf->ahrs->qe.v[Y] + qf->ahrs->qe.v[Z] * qf->ahrs->qe.v[Z];
if (snorm < 0.0001f)
{
norm = 0.01f;
}
else
{
// approximate square root by running 2 iterations of newton method
norm = maths_fast_sqrt(snorm);
}
qf->ahrs->qe.s /= norm;
qf->ahrs->qe.v[X] /= norm;
qf->ahrs->qe.v[Y] /= norm;
qf->ahrs->qe.v[Z] /= norm;
// bias estimate update
qf->imu->calib_gyro.bias[X] += - dt * ki * omc[X] / qf->imu->calib_gyro.scale_factor[X];
qf->imu->calib_gyro.bias[Y] += - dt * ki * omc[Y] / qf->imu->calib_gyro.scale_factor[Y];
qf->imu->calib_gyro.bias[Z] += - dt * ki * omc[Z] / qf->imu->calib_gyro.scale_factor[Z];
qf->imu->calib_gyro.bias[X] += - dt * ki_mag * omc_mag[X] / qf->imu->calib_compass.scale_factor[X];
qf->imu->calib_gyro.bias[Y] += - dt * ki_mag * omc_mag[Y] / qf->imu->calib_compass.scale_factor[Y];
qf->imu->calib_gyro.bias[Z] += - dt * ki_mag * omc_mag[Z] / qf->imu->calib_compass.scale_factor[Z];
// set up-vector (bodyframe) in attitude
qf->ahrs->up_vec.v[X] = up_bf.v[X];
qf->ahrs->up_vec.v[Y] = up_bf.v[Y];
qf->ahrs->up_vec.v[Z] = up_bf.v[Z];
// Update linear acceleration
qf->ahrs->linear_acc[X] = 9.81f * (qf->imu->scaled_accelero.data[X] - qf->ahrs->up_vec.v[X]) ; // TODO: review this line!
qf->ahrs->linear_acc[Y] = 9.81f * (qf->imu->scaled_accelero.data[Y] - qf->ahrs->up_vec.v[Y]) ; // TODO: review this line!
qf->ahrs->linear_acc[Z] = 9.81f * (qf->imu->scaled_accelero.data[Z] - qf->ahrs->up_vec.v[Z]) ; // TODO: review this line!
//update angular_speed.
qf->ahrs->angular_speed[X] = qf->imu->scaled_gyro.data[X];
qf->ahrs->angular_speed[Y] = qf->imu->scaled_gyro.data[Y];
qf->ahrs->angular_speed[Z] = qf->imu->scaled_gyro.data[Z];
}
