From 50e4363c32a150785e8c5cad776e059d1e00721a Mon Sep 17 00:00:00 2001
From: Federico Lolli <federico.lolli@skywarder.eu>
Date: Mon, 11 Mar 2024 15:31:19 +0100
Subject: [PATCH] Added initial version of mavlink message parsing
---
src/ffi.rs | 141 --------------------------------------------------
src/main.rs | 131 ++++++++++++----------------------------------
src/packet.rs | 100 +++++++++++++++++++++++++++++++++++
src/utils.rs | 32 ++++++++++++
4 files changed, 166 insertions(+), 238 deletions(-)
delete mode 100644 src/ffi.rs
create mode 100644 src/packet.rs
create mode 100644 src/utils.rs
diff --git a/src/ffi.rs b/src/ffi.rs
deleted file mode 100644
index 77dc33a..0000000
--- a/src/ffi.rs
+++ /dev/null
@@ -1,141 +0,0 @@
-use serde::{Deserialize, Serialize};
-
-#[derive(Debug, Clone, Default, Serialize, Deserialize)]
-pub struct MavlinkPayloadFlightTM {
- // [us] Timestamp in microseconds
- pub timestamp: u64,
- // [Pa] Pressure from digital sensor
- pub pressure_digi: f32,
- // [Pa] Pressure from static port
- pub pressure_static: f32,
- // [m/s] Pitot airspeed
- pub airspeed_pitot: f32,
- // [m] Altitude above ground level
- pub altitude_agl: f32,
- // [m/s^2] Acceleration on X axis (body)
- pub acc_x: f32,
- // [m/s^2] Acceleration on Y axis (body)
- pub acc_y: f32,
- // [m/s^2] Acceleration on Z axis (body)
- pub acc_z: f32,
- // [rad/s] Angular speed on X axis (body)
- pub gyro_x: f32,
- // [rad/s] Angular speed on Y axis (body)
- pub gyro_y: f32,
- // [rad/s] Angular speed on Z axis (body)
- pub gyro_z: f32,
- // [uT] Magnetic field on X axis (body)
- pub mag_x: f32,
- // [uT] Magnetic field on Y axis (body)
- pub mag_y: f32,
- // [uT] Magnetic field on Z axis (body)
- pub mag_z: f32,
- // [deg] Latitude
- pub gps_lat: f32,
- // [deg] Longitude
- pub gps_lon: f32,
- // [m] GPS Altitude
- pub gps_alt: f32,
- // [deg] Left servo motor angle
- pub left_servo_angle: f32,
- // [deg] Right servo motor angle
- pub right_servo_angle: f32,
- // [deg] Navigation system estimated noth position
- pub nas_n: f32,
- // [deg] Navigation system estimated east position
- pub nas_e: f32,
- // [m] Navigation system estimated down position
- pub nas_d: f32,
- // [m/s] Navigation system estimated north velocity
- pub nas_vn: f32,
- // [m/s] Navigation system estimated east velocity
- pub nas_ve: f32,
- // [m/s] Navigation system estimated down velocity
- pub nas_vd: f32,
- // [deg] Navigation system estimated attitude (qx)
- pub nas_qx: f32,
- // [deg] Navigation system estimated attitude (qy)
- pub nas_qy: f32,
- // [deg] Navigation system estimated attitude (qz)
- pub nas_qz: f32,
- // [deg] Navigation system estimated attitude (qw)
- pub nas_qw: f32,
- // Navigation system gyroscope bias on x axis
- pub nas_bias_x: f32,
- // Navigation system gyroscope bias on y axis
- pub nas_bias_y: f32,
- // Navigation system gyroscope bias on z axis
- pub nas_bias_z: f32,
- // [m/s] Wind estimated north velocity
- pub wes_n: f32,
- // [m/s] Wind estimated east velocity
- pub wes_e: f32,
- // [V] Battery voltage
- pub vbat: f32,
- // [V] Power supply 5V
- pub vsupply_5v: f32,
- // [degC] Temperature
- pub temperature: f32,
- // Flight Mode Manager State
- pub fmm_state: u8,
- // Navigation System FSM State
- pub nas_state: u8,
- // Wind Estimation System FSM State
- pub wes_state: u8,
- // GPS fix (1 = fix, 0 = no fix)
- pub gps_fix: u8,
- // Nosecone pin status (1 = connected, 0 = disconnected)
- pub pin_nosecone: u8,
- // Logger error (0 = no error)
- pub logger_error: i8,
-}
-
-impl MavlinkPayloadFlightTM {
- pub fn into_le_bytes(self) -> [u8; 158] {
- let mut buffer = [0u8; 158];
- buffer[0..8].copy_from_slice(&self.timestamp.to_le_bytes());
- buffer[8..12].copy_from_slice(&self.pressure_digi.to_le_bytes());
- buffer[12..16].copy_from_slice(&self.pressure_static.to_le_bytes());
- buffer[16..20].copy_from_slice(&self.airspeed_pitot.to_le_bytes());
- buffer[20..24].copy_from_slice(&self.altitude_agl.to_le_bytes());
- buffer[24..28].copy_from_slice(&self.acc_x.to_le_bytes());
- buffer[28..32].copy_from_slice(&self.acc_y.to_le_bytes());
- buffer[32..36].copy_from_slice(&self.acc_z.to_le_bytes());
- buffer[36..40].copy_from_slice(&self.gyro_x.to_le_bytes());
- buffer[40..44].copy_from_slice(&self.gyro_y.to_le_bytes());
- buffer[44..48].copy_from_slice(&self.gyro_z.to_le_bytes());
- buffer[48..52].copy_from_slice(&self.mag_x.to_le_bytes());
- buffer[52..56].copy_from_slice(&self.mag_y.to_le_bytes());
- buffer[56..60].copy_from_slice(&self.mag_z.to_le_bytes());
- buffer[60..64].copy_from_slice(&self.gps_lat.to_le_bytes());
- buffer[64..68].copy_from_slice(&self.gps_lon.to_le_bytes());
- buffer[68..72].copy_from_slice(&self.gps_alt.to_le_bytes());
- buffer[72..76].copy_from_slice(&self.left_servo_angle.to_le_bytes());
- buffer[76..80].copy_from_slice(&self.right_servo_angle.to_le_bytes());
- buffer[80..84].copy_from_slice(&self.nas_n.to_le_bytes());
- buffer[84..88].copy_from_slice(&self.nas_e.to_le_bytes());
- buffer[88..92].copy_from_slice(&self.nas_d.to_le_bytes());
- buffer[92..96].copy_from_slice(&self.nas_vn.to_le_bytes());
- buffer[96..100].copy_from_slice(&self.nas_ve.to_le_bytes());
- buffer[100..104].copy_from_slice(&self.nas_vd.to_le_bytes());
- buffer[104..108].copy_from_slice(&self.nas_qx.to_le_bytes());
- buffer[108..112].copy_from_slice(&self.nas_qy.to_le_bytes());
- buffer[112..116].copy_from_slice(&self.nas_qz.to_le_bytes());
- buffer[116..120].copy_from_slice(&self.nas_qw.to_le_bytes());
- buffer[120..124].copy_from_slice(&self.nas_bias_x.to_le_bytes());
- buffer[124..128].copy_from_slice(&self.nas_bias_y.to_le_bytes());
- buffer[128..132].copy_from_slice(&self.nas_bias_z.to_le_bytes());
- buffer[132..136].copy_from_slice(&self.wes_n.to_le_bytes());
- buffer[136..140].copy_from_slice(&self.wes_e.to_le_bytes());
- buffer[140..144].copy_from_slice(&self.vbat.to_le_bytes());
- buffer[144..148].copy_from_slice(&self.vsupply_5v.to_le_bytes());
- buffer[148..152].copy_from_slice(&self.temperature.to_le_bytes());
- buffer[152..153].copy_from_slice(&self.fmm_state.to_le_bytes());
- buffer[153..154].copy_from_slice(&self.nas_state.to_le_bytes());
- buffer[154..155].copy_from_slice(&self.wes_state.to_le_bytes());
- buffer[155..156].copy_from_slice(&self.gps_fix.to_le_bytes());
- buffer[156..157].copy_from_slice(&self.pin_nosecone.to_le_bytes());
- buffer[157..158].copy_from_slice(&self.logger_error.to_le_bytes());
- buffer
- }
-}
diff --git a/src/main.rs b/src/main.rs
index bf6c009..a994985 100644
--- a/src/main.rs
+++ b/src/main.rs
@@ -1,16 +1,25 @@
-mod ffi;
+mod packet;
+mod utils;
-use std::fs::File;
-use std::io;
-use std::io::Read;
-use std::path::PathBuf;
-use std::time::{Duration, Instant};
+use std::{
+ fs::File,
+ io::{self, Read},
+ path::PathBuf,
+ time::{Duration, Instant},
+};
use clap::Parser;
use log::{debug, error, info, warn};
use serialport::SerialPort;
+use skyward_mavlink::{
+ lyra::MavMessage,
+ mavlink::{write_v1_msg, MavHeader},
+};
-use ffi::MavlinkPayloadFlightTM;
+use crate::{
+ packet::PacketSequence,
+ utils::{get_first_stm32_serial_port, print_stats},
+};
const ACK: u8 = 0x06;
@@ -42,8 +51,7 @@ fn main() {
info!("reading from {}", args.csv_input.display());
let mut rdr = csv::Reader::from_reader(File::open(args.csv_input).unwrap());
for result in rdr.deserialize() {
- let record: MavlinkPayloadFlightTM = result.unwrap();
- packets.push(record);
+ packets.push(MavMessage::PAYLOAD_FLIGHT_TM(result.unwrap()));
}
debug!("collected {} packets", packets.len());
@@ -51,11 +59,11 @@ fn main() {
let mut packets = PacketSequence::from(packets);
// Open the first serialport available.
- let port_name = if let Some(port_name) = args.port {
- port_name.to_owned()
- } else if let Some(p) = get_first_stm32_serial_port() {
- debug!("found STM32 device on {}", p);
- p
+ let port_name = if let Some(name) = args.port {
+ name
+ } else if let Some(name) = get_first_stm32_serial_port() {
+ debug!("found STM32 device on {}", name);
+ name
} else {
error!("FATAL: no STM32 serial port found, your device may not be connected properly");
std::process::exit(1);
@@ -80,19 +88,23 @@ fn main() {
}
start = Some(Instant::now());
write_port.write_packet(packet.to_owned());
- debug!("sent packet ({}/{})", packets.index, packets.packets.len());
+ debug!(
+ "sent packet ({}/{})",
+ packets.sent_packets(),
+ packets.total_packets()
+ );
// wait ACK
loop {
match port.read(&mut buffer) {
Ok(t) => {
+ eprint!("{}", String::from_utf8_lossy(&buffer[..t]));
// if didn't get the signal then print the buffer and exit with error
if buffer[..t].contains(&ACK) {
break;
} else {
warn!("didn't get the signal");
std::thread::sleep(Duration::from_millis(100));
- // eprintln!("{}", String::from_utf8_lossy(&buffer[..t]));
}
}
Err(ref e) if e.kind() == io::ErrorKind::TimedOut => (),
@@ -102,95 +114,20 @@ fn main() {
}
}
}
+ eprintln!();
}
- info!("sent {} packets", packets.index);
+ info!("sent {} packets", packets.sent_packets());
print_stats(times);
}
-struct PacketSequence {
- packets: Vec<MavlinkPayloadFlightTM>,
- index: usize,
- delta_from_start: u64,
- start_instant: Option<Instant>,
-}
-
-impl From<Vec<MavlinkPayloadFlightTM>> for PacketSequence {
- fn from(value: Vec<MavlinkPayloadFlightTM>) -> Self {
- let delta_from_start = value[0].timestamp;
- Self {
- packets: value,
- index: 0,
- delta_from_start,
- start_instant: None,
- }
- }
-}
-
-impl PacketSequence {
- fn wait_next(&mut self) -> Option<&MavlinkPayloadFlightTM> {
- // return None if we reached the end of the sequence
- if self.index == self.packets.len() {
- return None;
- }
-
- // get time delta from the start
- let next_timestamp = self.packets[self.index].timestamp;
- let time_delta = next_timestamp - self.delta_from_start;
-
- // get the instant the first packet was sent or set it if it's the first packet
- if let Some(start_inst) = self.start_instant {
- // get the instant to wait until
- let till = start_inst + Duration::from_micros(time_delta);
-
- // sleep for the difference between the last timestamp and the current one
- std::thread::sleep(till - Instant::now());
- } else {
- // set the start instant
- self.start_instant = Some(Instant::now());
- }
- self.index += 1;
- self.packets.get(self.index - 1)
- }
-}
-
trait MavLinkPort {
- fn write_packet(&mut self, packet: MavlinkPayloadFlightTM);
+ fn write_packet(&mut self, packet: MavMessage);
}
impl<T: AsRef<dyn SerialPort> + std::io::Write> MavLinkPort for T {
- fn write_packet(&mut self, packet: MavlinkPayloadFlightTM) {
- self.write_all(&packet.into_le_bytes())
- .expect("Failed to write to serial port");
+ fn write_packet(&mut self, packet: MavMessage) {
+ let header = MavHeader::default();
+ write_v1_msg(self, header, &packet).expect("Failed to write to serial port");
}
}
-
-fn get_first_stm32_serial_port() -> Option<String> {
- let ports = serialport::available_ports().expect("No serial ports found!");
- for port in ports {
- if let serialport::SerialPortType::UsbPort(info) = port.port_type {
- if let Some(p) = info.product {
- if p.contains("STM32") || p.contains("ST-LINK") {
- return Some(port.port_name);
- }
- }
- }
- }
- None
-}
-
-/// print stats (mean, median, std, etc)
-fn print_stats(times: Vec<u128>) {
- let mean = times.iter().sum::<u128>() as f64 / times.len() as f64;
- let median = times[times.len() / 2] as f64;
- let std = times
- .iter()
- .map(|&x| (x as f64 - mean).powi(2))
- .sum::<f64>()
- .sqrt()
- / times.len() as f64;
- info!("Stats:");
- info!("Mean: {}ms", mean);
- info!("Median: {}ms", median);
- info!("Std: {}ms", std);
-}
diff --git a/src/packet.rs b/src/packet.rs
new file mode 100644
index 0000000..91406d2
--- /dev/null
+++ b/src/packet.rs
@@ -0,0 +1,100 @@
+use std::time::{Duration, Instant};
+
+use serde::{Deserialize, Serialize};
+use skyward_mavlink::{
+ lyra::{MavMessage, ACK_TM_DATA, PAYLOAD_FLIGHT_TM_DATA},
+ mavlink::Message,
+};
+
+pub trait TimedMessage: Message {
+ fn timestamp(&self) -> Option<u64>;
+}
+
+#[derive(Debug, Serialize, Deserialize)]
+#[serde(untagged)]
+pub enum PacketKind {
+ Dummy(ACK_TM_DATA),
+ PayloadFlightTm(PAYLOAD_FLIGHT_TM_DATA),
+}
+
+impl From<PacketKind> for MavMessage {
+ fn from(value: PacketKind) -> Self {
+ match value {
+ PacketKind::Dummy(d) => MavMessage::ACK_TM(d),
+ PacketKind::PayloadFlightTm(p) => MavMessage::PAYLOAD_FLIGHT_TM(p),
+ }
+ }
+}
+
+impl Default for PacketKind {
+ fn default() -> Self {
+ PacketKind::Dummy(ACK_TM_DATA {
+ recv_msgid: 0,
+ seq_ack: 0,
+ })
+ }
+}
+
+impl TimedMessage for MavMessage {
+ fn timestamp(&self) -> Option<u64> {
+ match self {
+ MavMessage::PAYLOAD_FLIGHT_TM(data) => Some(data.timestamp),
+ MavMessage::ACK_TM(_) => None,
+ _ => None,
+ }
+ }
+}
+
+pub struct PacketSequence {
+ packets: Vec<MavMessage>,
+ index: usize,
+ delta_from_start: u64,
+ start_instant: Option<Instant>,
+}
+
+impl From<Vec<MavMessage>> for PacketSequence {
+ fn from(value: Vec<MavMessage>) -> Self {
+ let delta_from_start = value[0].timestamp().unwrap();
+ Self {
+ packets: value,
+ index: 0,
+ delta_from_start,
+ start_instant: None,
+ }
+ }
+}
+
+impl PacketSequence {
+ pub fn sent_packets(&self) -> usize {
+ self.index
+ }
+
+ pub fn total_packets(&self) -> usize {
+ self.packets.len()
+ }
+
+ pub fn wait_next(&mut self) -> Option<&MavMessage> {
+ // return None if we reached the end of the sequence
+ if self.index == self.packets.len() {
+ return None;
+ }
+
+ // get time delta from the start
+ let next_timestamp = self.packets[self.index].timestamp().unwrap();
+ let time_delta = next_timestamp - self.delta_from_start;
+
+ // get the instant the first packet was sent or set it if it's the first packet
+ if let Some(start_inst) = self.start_instant {
+ // get the instant to wait until
+ let till = start_inst + Duration::from_micros(time_delta);
+
+ // sleep for the difference between the last timestamp and the current one
+ std::thread::sleep(till - Instant::now());
+ } else {
+ // set the start instant
+ self.start_instant = Some(Instant::now());
+ }
+ self.index += 1;
+ self.packets.get(self.index - 1)
+ }
+}
diff --git a/src/utils.rs b/src/utils.rs
new file mode 100644
index 0000000..1c76055
--- /dev/null
+++ b/src/utils.rs
@@ -0,0 +1,32 @@
+use log::info;
+
+/// Get the first serial port that contains "STM32" or "ST-LINK" in its product name
+pub fn get_first_stm32_serial_port() -> Option<String> {
+ let ports = serialport::available_ports().expect("No serial ports found!");
+ for port in ports {
+ if let serialport::SerialPortType::UsbPort(info) = port.port_type {
+ if let Some(p) = info.product {
+ if p.contains("STM32") || p.contains("ST-LINK") {
+ return Some(port.port_name);
+ }
+ }
+ }
+ }
+ None
+}
+
+/// print stats (mean, median, std, etc)
+pub fn print_stats(times: Vec<u128>) {
+ let mean = times.iter().sum::<u128>() as f64 / times.len() as f64;
+ let median = times[times.len() / 2] as f64;
+ let std = times
+ .iter()
+ .map(|&x| (x as f64 - mean).powi(2))
+ .sum::<f64>()
+ .sqrt()
+ / times.len() as f64;
+ info!("Stats:");
+ info!("Mean: {}ms", mean);
+ info!("Median: {}ms", median);
+ info!("Std: {}ms", std);
+}
--
GitLab