diff --git a/src/ffi.rs b/src/ffi.rs
index 40a6e8a17390bcef09ca84d648ec83c07325beb0..77dc33ab00f44ce99ba218bbba003b32edf19c28 100644
--- a/src/ffi.rs
+++ b/src/ffi.rs
@@ -3,91 +3,91 @@ use serde::{Deserialize, Serialize};
#[derive(Debug, Clone, Default, Serialize, Deserialize)]
pub struct MavlinkPayloadFlightTM {
// [us] Timestamp in microseconds
- timestamp: u64,
+ pub timestamp: u64,
// [Pa] Pressure from digital sensor
- pressure_digi: f32,
+ pub pressure_digi: f32,
// [Pa] Pressure from static port
- pressure_static: f32,
+ pub pressure_static: f32,
// [m/s] Pitot airspeed
- airspeed_pitot: f32,
+ pub airspeed_pitot: f32,
// [m] Altitude above ground level
- altitude_agl: f32,
+ pub altitude_agl: f32,
// [m/s^2] Acceleration on X axis (body)
- acc_x: f32,
+ pub acc_x: f32,
// [m/s^2] Acceleration on Y axis (body)
- acc_y: f32,
+ pub acc_y: f32,
// [m/s^2] Acceleration on Z axis (body)
- acc_z: f32,
+ pub acc_z: f32,
// [rad/s] Angular speed on X axis (body)
- gyro_x: f32,
+ pub gyro_x: f32,
// [rad/s] Angular speed on Y axis (body)
- gyro_y: f32,
+ pub gyro_y: f32,
// [rad/s] Angular speed on Z axis (body)
- gyro_z: f32,
+ pub gyro_z: f32,
// [uT] Magnetic field on X axis (body)
- mag_x: f32,
+ pub mag_x: f32,
// [uT] Magnetic field on Y axis (body)
- mag_y: f32,
+ pub mag_y: f32,
// [uT] Magnetic field on Z axis (body)
- mag_z: f32,
+ pub mag_z: f32,
// [deg] Latitude
- gps_lat: f32,
+ pub gps_lat: f32,
// [deg] Longitude
- gps_lon: f32,
+ pub gps_lon: f32,
// [m] GPS Altitude
- gps_alt: f32,
+ pub gps_alt: f32,
// [deg] Left servo motor angle
- left_servo_angle: f32,
+ pub left_servo_angle: f32,
// [deg] Right servo motor angle
- right_servo_angle: f32,
+ pub right_servo_angle: f32,
// [deg] Navigation system estimated noth position
- nas_n: f32,
+ pub nas_n: f32,
// [deg] Navigation system estimated east position
- nas_e: f32,
+ pub nas_e: f32,
// [m] Navigation system estimated down position
- nas_d: f32,
+ pub nas_d: f32,
// [m/s] Navigation system estimated north velocity
- nas_vn: f32,
+ pub nas_vn: f32,
// [m/s] Navigation system estimated east velocity
- nas_ve: f32,
+ pub nas_ve: f32,
// [m/s] Navigation system estimated down velocity
- nas_vd: f32,
+ pub nas_vd: f32,
// [deg] Navigation system estimated attitude (qx)
- nas_qx: f32,
+ pub nas_qx: f32,
// [deg] Navigation system estimated attitude (qy)
- nas_qy: f32,
+ pub nas_qy: f32,
// [deg] Navigation system estimated attitude (qz)
- nas_qz: f32,
+ pub nas_qz: f32,
// [deg] Navigation system estimated attitude (qw)
- nas_qw: f32,
+ pub nas_qw: f32,
// Navigation system gyroscope bias on x axis
- nas_bias_x: f32,
+ pub nas_bias_x: f32,
// Navigation system gyroscope bias on y axis
- nas_bias_y: f32,
+ pub nas_bias_y: f32,
// Navigation system gyroscope bias on z axis
- nas_bias_z: f32,
+ pub nas_bias_z: f32,
// [m/s] Wind estimated north velocity
- wes_n: f32,
+ pub wes_n: f32,
// [m/s] Wind estimated east velocity
- wes_e: f32,
+ pub wes_e: f32,
// [V] Battery voltage
- vbat: f32,
+ pub vbat: f32,
// [V] Power supply 5V
- vsupply_5v: f32,
+ pub vsupply_5v: f32,
// [degC] Temperature
- temperature: f32,
+ pub temperature: f32,
// Flight Mode Manager State
- fmm_state: u8,
+ pub fmm_state: u8,
// Navigation System FSM State
- nas_state: u8,
+ pub nas_state: u8,
// Wind Estimation System FSM State
- wes_state: u8,
+ pub wes_state: u8,
// GPS fix (1 = fix, 0 = no fix)
- gps_fix: u8,
+ pub gps_fix: u8,
// Nosecone pin status (1 = connected, 0 = disconnected)
- pin_nosecone: u8,
+ pub pin_nosecone: u8,
// Logger error (0 = no error)
- logger_error: i8,
+ pub logger_error: i8,
}
impl MavlinkPayloadFlightTM {
diff --git a/src/main.rs b/src/main.rs
index cabf90e18fb30a4fd96d3827d85309c4672cc8d1..9269e0832f973351dc05b631cce5f34693b89e5d 100644
--- a/src/main.rs
+++ b/src/main.rs
@@ -2,7 +2,7 @@ mod ffi;
use std::io;
use std::io::{Read, Write};
-use std::time::Instant;
+use std::time::{Duration, Instant};
use ffi::MavlinkPayloadFlightTM;
use serialport::SerialPort;
@@ -18,6 +18,9 @@ fn main() {
packets.push(record);
}
+ // convert to packet sequence data structure
+ let mut packets = PacketSequence::from(packets);
+
// Open the first serialport available.
let port_name = get_first_stm32_serial_port().expect("No STM32 serial port found!");
let mut port = serialport::new(port_name, 115200)
@@ -25,13 +28,12 @@ fn main() {
.expect("Failed to open serial port");
let mut write_port = port.try_clone().expect("Failed to clone");
- let mut iter = packets.into_iter();
+ // let mut iter = packets.into_iter();
// read from buffer and print to stdout
let mut buffer = [0u8; 1000];
let mut start: Option<Instant> = None;
let mut times = Vec::new();
- let mut i = 0;
loop {
match port.read(&mut buffer) {
Ok(t) => {
@@ -47,22 +49,17 @@ fn main() {
// if we got the signal to start sending data, send it
if eot_f {
- if let Some(packet) = iter.next() {
+ if let Some(packet) = packets.wait_next() {
if let Some(t) = start {
times.push(t.elapsed().as_millis());
}
start = Some(Instant::now());
- write_port.write_packet(packet);
- i += 1;
+ write_port.write_packet(packet.to_owned());
} else {
break;
}
}
std::io::stdout().write_all(input.as_bytes()).unwrap();
-
- if i == 580 {
- break;
- }
}
Err(ref e) if e.kind() == io::ErrorKind::TimedOut => (),
Err(e) => eprintln!("{:?}", e),
@@ -72,6 +69,41 @@ fn main() {
print_stats(times);
}
+struct PacketSequence {
+ packets: Vec<MavlinkPayloadFlightTM>,
+ index: usize,
+ last_timestamp: u64,
+}
+
+impl From<Vec<MavlinkPayloadFlightTM>> for PacketSequence {
+ fn from(value: Vec<MavlinkPayloadFlightTM>) -> Self {
+ let last_timestamp = value[0].timestamp;
+ Self {
+ packets: value,
+ index: 0,
+ last_timestamp,
+ }
+ }
+}
+
+impl PacketSequence {
+ fn wait_next(&mut self) -> Option<&MavlinkPayloadFlightTM> {
+ let last = self.last_timestamp;
+ let to_sleep = self.packets[self.index].timestamp - last;
+ // return None if we reached the end of the sequence
+ if self.index == self.packets.len() {
+ return None;
+ }
+
+ // sleep for the difference between the last timestamp and the current one
+ std::thread::sleep(Duration::from_millis(to_sleep / 1000));
+
+ self.last_timestamp = self.packets[self.index].timestamp;
+ self.index += 1;
+ self.packets.get(self.index - 1)
+ }
+}
+
trait MavLinkPort {
fn write_packet(&mut self, packet: MavlinkPayloadFlightTM);
}