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; }