void time_keeper_sleep_us(uint64_t t)
{
uint64_t now = time_keeper_get_us();
while (time_keeper_get_us() < now + t)
{
;
}
}
void time_keeper_delay_ms(uint64_t t)
{
uint64_t now = time_keeper_get_us();
while (time_keeper_get_us() < now + 1000 * t)
{
;
}
}
void Gps_mocap::callback(uint32_t sysid, mavlink_message_t* msg)
{
mavlink_att_pos_mocap_t packet;
mavlink_msg_att_pos_mocap_decode(msg, &packet);
// Get timing
float t = time_keeper_get_us();
// Update velocity
float dt_s = (t - last_update_us_) / 1000000;
if (dt_s > 0.0f)
{
velocity_lf_[X] = (packet.x - local_position_.pos[X]) / dt_s;
velocity_lf_[Y] = (packet.y - local_position_.pos[Y]) / dt_s;
velocity_lf_[Z] = (packet.z - local_position_.pos[Z]) / dt_s;
if (velocity_lf_[X] != 0.0f)
{
heading_ = quick_trig_atan(velocity_lf_[Y] / velocity_lf_[X]);
}
}
// Update position
local_position_.pos[X] = packet.x;
local_position_.pos[Y] = packet.y;
local_position_.pos[Z] = packet.z;
// Update timing
last_update_us_ = t;
}
void stabilisation_copter_send_outputs(stabilisation_copter_t* stabilisation_copter, const Mavlink_stream* mavlink_stream, mavlink_message_t* msg)
{
aero_attitude_t attitude_yaw_inverse;
quat_t q_rot, qtmp;
attitude_yaw_inverse = coord_conventions_quat_to_aero(stabilisation_copter->ahrs->qe);
attitude_yaw_inverse.rpy[0] = 0.0f;
attitude_yaw_inverse.rpy[1] = 0.0f;
attitude_yaw_inverse.rpy[2] = attitude_yaw_inverse.rpy[2];
q_rot = coord_conventions_quaternion_from_aero(attitude_yaw_inverse);
qtmp = quaternions_create_from_vector(stabilisation_copter->stabiliser_stack.velocity_stabiliser.output.rpy);
quat_t rpy_local;
quaternions_rotate_vector(quaternions_inverse(q_rot), qtmp.v, rpy_local.v);
mavlink_msg_debug_vect_pack(mavlink_stream->sysid(),
mavlink_stream->compid(),
msg,
"OutVel",
time_keeper_get_us(),
-rpy_local.v[X] * 1000,
rpy_local.v[Y] * 1000,
stabilisation_copter->stabiliser_stack.velocity_stabiliser.output.rpy[YAW] * 1000);
mavlink_stream->send(msg);
mavlink_msg_debug_vect_pack(mavlink_stream->sysid(),
mavlink_stream->compid(),
msg,
"OutAtt",
time_keeper_get_us(),
stabilisation_copter->stabiliser_stack.attitude_stabiliser.output.rpy[ROLL] * 1000,
stabilisation_copter->stabiliser_stack.attitude_stabiliser.output.rpy[PITCH] * 1000,
stabilisation_copter->stabiliser_stack.attitude_stabiliser.output.rpy[YAW] * 1000);
mavlink_stream->send(msg);
mavlink_msg_debug_vect_pack(mavlink_stream->sysid(),
mavlink_stream->compid(),
msg,
"OutRate",
time_keeper_get_us(),
stabilisation_copter->stabiliser_stack.rate_stabiliser.output.rpy[ROLL] * 1000,
stabilisation_copter->stabiliser_stack.rate_stabiliser.output.rpy[PITCH] * 1000,
stabilisation_copter->stabiliser_stack.rate_stabiliser.output.rpy[YAW] * 1000);
mavlink_stream->send(msg);
}
Flow_px4::Flow_px4(Serial& uart):
uart_(uart),
mavlink_stream_(uart_, Mavlink_stream::default_config())
{
// Init members
last_update_us = time_keeper_get_us();
handshake_state = FLOW_NO_HANDSHAKE;
of_count = 0;
}
bool Gps_mocap::update(void)
{
float t = time_keeper_get_us();
if ( (t - last_update_us()) > 1000000 )
{
is_healthy_ = false;
}
else
{
is_healthy_ = true;
}
return true;
}
void servos_telemetry_mavlink_send(servos_telemetry_t* servos_telemetry, Mavlink_stream* mavlink_stream, mavlink_message_t* msg)
{
mavlink_msg_servo_output_raw_pack(mavlink_stream->sysid(),
mavlink_stream->compid(),
msg,
time_keeper_get_us(),
0,
(uint16_t)(1500 + 500 * servos_telemetry->servos[0]->read()),
(uint16_t)(1500 + 500 * servos_telemetry->servos[1]->read()),
(uint16_t)(1500 + 500 * servos_telemetry->servos[2]->read()),
(uint16_t)(1500 + 500 * servos_telemetry->servos[3]->read()),
(uint16_t)( 1500 + 500 * servos_telemetry->servos[4]->read() ),
(uint16_t)( 1500 + 500 * servos_telemetry->servos[5]->read() ),
(uint16_t)( 1500 + 500 * servos_telemetry->servos[6]->read() ),
(uint16_t)( 1500 + 500 * servos_telemetry->servos[7]->read() )
);
}
bool Flow_px4::update(void)
{
Mavlink_stream::msg_received_t* rec = NULL;
// Receive incoming bytes
while (mavlink_stream_.receive(rec))
{
// Get pointer to new message
mavlink_message_t* msg = &rec->msg;
// declare messages
mavlink_optical_flow_t optical_flow_msg;
mavlink_data_transmission_handshake_t handshake_msg;
mavlink_encapsulated_data_t data_msg;
switch (msg->msgid)
{
case MAVLINK_MSG_ID_OPTICAL_FLOW:
// Decode message
mavlink_msg_optical_flow_decode(msg, &optical_flow_msg);
break;
case MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE:
// Decode message
mavlink_msg_data_transmission_handshake_decode(msg, &handshake_msg);
// Get type of handshake
switch (handshake_msg.jpg_quality)
{
case 0:
handshake_state = FLOW_HANDSHAKE_DATA;
break;
case 255:
handshake_state = FLOW_HANDSHAKE_METADATA;
break;
default:
handshake_state = FLOW_HANDSHAKE_DATA;
break;
}
// Get number of of vectors
of_count = handshake_msg.width;
if (of_count > 125)
{
of_count = 125;
}
// Get total payload size
packet_count_ = handshake_msg.packets;
byte_count_ = handshake_msg.size;
if (byte_count_ > 500)
{
byte_count_ = 500;
}
break;
case MAVLINK_MSG_ID_ENCAPSULATED_DATA:
// Decode message
mavlink_msg_encapsulated_data_decode(msg, &data_msg);
// Handle according to hanshake state
switch (handshake_state)
{
case FLOW_HANDSHAKE_METADATA:
if (data_msg.seqnr < packet_count_ - 1)
{
// not last packet
for (uint32_t i = 0; i < MAVLINK_MSG_ENCAPSULATED_DATA_FIELD_DATA_LEN; ++i)
{
of_loc_tmp_.data[i + data_msg.seqnr * MAVLINK_MSG_ENCAPSULATED_DATA_FIELD_DATA_LEN] = data_msg.data[i];
}
}
else if (data_msg.seqnr < packet_count_)
{
// last packet
for (uint32_t i = 0; i < byte_count_; ++i)
{
of_loc_tmp_.data[i + data_msg.seqnr * MAVLINK_MSG_ENCAPSULATED_DATA_FIELD_DATA_LEN] = data_msg.data[i];
}
// swap bytes
for (uint32_t i = 0; i < of_count; ++i)
{
of_loc.x[i] = endian_rev16s(of_loc_tmp_.x[i]);
of_loc.y[i] = endian_rev16s(of_loc_tmp_.y[i]);
}
}
break;
default:
if (data_msg.seqnr < (packet_count_ - 1))
{
// not last packet
for (uint32_t i = 0; i < MAVLINK_MSG_ENCAPSULATED_DATA_FIELD_DATA_LEN; ++i)
{
of_tmp_.data[i + data_msg.seqnr * MAVLINK_MSG_ENCAPSULATED_DATA_FIELD_DATA_LEN] = data_msg.data[i];
}
}
else if (data_msg.seqnr == (packet_count_ - 1))
{
// last packet
for (uint32_t i = 0; i < byte_count_ % MAVLINK_MSG_ENCAPSULATED_DATA_FIELD_DATA_LEN; ++i)
{
of_tmp_.data[i + data_msg.seqnr * MAVLINK_MSG_ENCAPSULATED_DATA_FIELD_DATA_LEN] = data_msg.data[i];
}
// swap bytes and filter for high frequency noise
for (int i = 0; i < of_count; ++i)
{
of.x[i] = filter_constant * 0.001f * (float)(endian_rev16s(of_tmp_.x[i])) +
(1.0f - filter_constant) * of.x[i];
of.y[i] = filter_constant * 0.001f * (float)(endian_rev16s(of_tmp_.y[i])) +
(1.0f - filter_constant) * of.y[i];
}
// Update time
last_update_us = time_keeper_get_us();
}
break;
}
break;
default:
break;
}
}
return true;
}
void time_keeper_delay_us(uint64_t microseconds)
{
uint64_t now = time_keeper_get_us();
while (time_keeper_get_us() < now + microseconds);
}
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;
}
