From 268c9c3dcc609f9d61e8d76784e720a2a3cbabe4 Mon Sep 17 00:00:00 2001
From: Federico Lolli <federico.lolli@skywarder.eu>
Date: Mon, 22 Jan 2024 10:24:08 +0100
Subject: [PATCH] added FFI module and moved mavlink package there
---
src/ffi.rs | 139 ++++++++++++++++++++++++++++++++++++++++++++++++++++
src/main.rs | 13 +++--
2 files changed, 145 insertions(+), 7 deletions(-)
create mode 100644 src/ffi.rs
diff --git a/src/ffi.rs b/src/ffi.rs
new file mode 100644
index 0000000..ea42be8
--- /dev/null
+++ b/src/ffi.rs
@@ -0,0 +1,139 @@
+#[derive(Debug, Clone, Default)]
+pub struct MavlinkPayloadFlightTM {
+ // [us] Timestamp in microseconds
+ timestamp: u64,
+ // [Pa] Pressure from digital sensor
+ pressure_digi: f32,
+ // [Pa] Pressure from static port
+ pressure_static: f32,
+ // [m/s] Pitot airspeed
+ airspeed_pitot: f32,
+ // [m] Altitude above ground level
+ altitude_agl: f32,
+ // [m/s^2] Acceleration on X axis (body)
+ acc_x: f32,
+ // [m/s^2] Acceleration on Y axis (body)
+ acc_y: f32,
+ // [m/s^2] Acceleration on Z axis (body)
+ acc_z: f32,
+ // [rad/s] Angular speed on X axis (body)
+ gyro_x: f32,
+ // [rad/s] Angular speed on Y axis (body)
+ gyro_y: f32,
+ // [rad/s] Angular speed on Z axis (body)
+ gyro_z: f32,
+ // [uT] Magnetic field on X axis (body)
+ mag_x: f32,
+ // [uT] Magnetic field on Y axis (body)
+ mag_y: f32,
+ // [uT] Magnetic field on Z axis (body)
+ mag_z: f32,
+ // [deg] Latitude
+ gps_lat: f32,
+ // [deg] Longitude
+ gps_lon: f32,
+ // [m] GPS Altitude
+ gps_alt: f32,
+ // [deg] Left servo motor angle
+ left_servo_angle: f32,
+ // [deg] Right servo motor angle
+ right_servo_angle: f32,
+ // [deg] Navigation system estimated noth position
+ nas_n: f32,
+ // [deg] Navigation system estimated east position
+ nas_e: f32,
+ // [m] Navigation system estimated down position
+ nas_d: f32,
+ // [m/s] Navigation system estimated north velocity
+ nas_vn: f32,
+ // [m/s] Navigation system estimated east velocity
+ nas_ve: f32,
+ // [m/s] Navigation system estimated down velocity
+ nas_vd: f32,
+ // [deg] Navigation system estimated attitude (qx)
+ nas_qx: f32,
+ // [deg] Navigation system estimated attitude (qy)
+ nas_qy: f32,
+ // [deg] Navigation system estimated attitude (qz)
+ nas_qz: f32,
+ // [deg] Navigation system estimated attitude (qw)
+ nas_qw: f32,
+ // Navigation system gyroscope bias on x axis
+ nas_bias_x: f32,
+ // Navigation system gyroscope bias on y axis
+ nas_bias_y: f32,
+ // Navigation system gyroscope bias on z axis
+ nas_bias_z: f32,
+ // [m/s] Wind estimated north velocity
+ wes_n: f32,
+ // [m/s] Wind estimated east velocity
+ wes_e: f32,
+ // [V] Battery voltage
+ vbat: f32,
+ // [V] Power supply 5V
+ vsupply_5v: f32,
+ // [degC] Temperature
+ temperature: f32,
+ // Flight Mode Manager State
+ fmm_state: u8,
+ // Navigation System FSM State
+ nas_state: u8,
+ // Wind Estimation System FSM State
+ wes_state: u8,
+ // GPS fix (1 = fix, 0 = no fix)
+ gps_fix: u8,
+ // Nosecone pin status (1 = connected, 0 = disconnected)
+ pin_nosecone: u8,
+ // Logger error (0 = no error)
+ 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 82d7258..cec2efa 100644
--- a/src/main.rs
+++ b/src/main.rs
@@ -1,7 +1,11 @@
+mod ffi;
+
use std::io::{Read, Write};
use std::time::Duration;
use std::{io, thread};
+use ffi::MavlinkPayloadFlightTM;
+
fn main() {
// Open the first serialport available.
let port_name = "/dev/tty.usbmodem143203";
@@ -15,16 +19,11 @@ fn main() {
// Send out 4 bytes every second
thread::spawn(move || {
thread::sleep(Duration::from_millis(2000));
- let mut mavlink_buffer = [0u8; 158];
- mavlink_buffer[0..8].copy_from_slice(&1_000_000_000u64.to_le_bytes());
- for i in 0..36 {
- mavlink_buffer[8 + i * 4..12 + i * 4].copy_from_slice(&999f32.to_le_bytes());
- }
- mavlink_buffer[152..158].fill(100u8);
+ let mav = MavlinkPayloadFlightTM::default();
clone
// .write_all(&send_buffer)
- .write_all(&mavlink_buffer)
+ .write_all(&mav.into_le_bytes())
.expect("Failed to write to serial port");
});
--
GitLab