diff --git a/on-host/Cargo.toml b/on-host/Cargo.toml
index 6b93b1d545675c9d2c4d3ce414a041b479851057..7b00d38d72dbea63a4ebb164192707303a705360 100644
--- a/on-host/Cargo.toml
+++ b/on-host/Cargo.toml
@@ -19,11 +19,5 @@ git = "https://git.skywarder.eu/avn/swd/mavlink/mavlink-skyward-lib.git"
branch = "main"
features = ["serde", "lyra"]
-# [dependencies.tdl-core]
-# git = "ssh://git@git.skywarder.eu/federico.lolli/tdl.git"
-# branch = "main"
-# optional = true
-
[features]
default = []
-# tdl = ["dep:tdl-core"]
diff --git a/on-host/src/cli.rs b/on-host/src/cli.rs
index 53b8f94e1c3f63ade55296e0c26aae359180c2fc..9b4d071b4f10f51b651c3a1ee5e9d201f81e8d04 100644
--- a/on-host/src/cli.rs
+++ b/on-host/src/cli.rs
@@ -8,32 +8,6 @@ use clap::{
Parser,
};
-// #[cfg(feature = "tdl")]
-// #[derive(Debug, Parser)]
-// #[command(styles=get_styles())]
-// pub struct Cli {
-// /// The mode to use
-// #[clap(subcommand)]
-// pub mode: ModeFile,
-
-// /// The serial port to use
-// #[clap(short, long, value_name = "PORT")]
-// pub port: Option<String>,
-
-// /// baud rate
-// #[clap(short, long, default_value = "115200")]
-// pub baud_rate: u32,
-
-// /// disable SYNC MODE (wait for a sync signal from the device before proceding)
-// #[clap(short, long)]
-// pub no_sync: bool,
-
-// /// The log level
-// #[clap(short, long)]
-// pub verbose: bool,
-// }
-
-// #[cfg(not(feature = "tdl"))]
#[derive(Debug, Parser)]
#[command(styles=get_styles())]
pub struct Cli {
@@ -62,34 +36,6 @@ pub struct Cli {
pub verbose: bool,
}
-// #[cfg(feature = "tdl")]
-// #[derive(Debug, Parser)]
-// pub enum ModeFile {
-// /// build mavlink packets from a TDL file
-// Tdl {
-// /// The TDL file to read from
-// tdl_file: PathBuf,
-
-// /// initial GPS latitude
-// #[clap(short = 'n', long, value_name = "LAT")]
-// lat: f64,
-
-// /// initial GPS longitude
-// #[clap(short = 'e', long, value_name = "LON")]
-// lon: f64,
-
-// /// initial GPS altitude
-// #[clap(short = 'u', long, value_name = "ALT")]
-// alt: f64,
-// },
-
-// /// parse mavlink packets from a CSV file
-// Csv {
-// /// The CSV file to read from
-// csv_file: PathBuf,
-// },
-// }
-
pub fn get_styles() -> Styles {
Styles::styled()
.usage(
diff --git a/on-host/src/main.rs b/on-host/src/main.rs
index ce76fa676efdee0aeaca7648a12d0df1405e0dce..7d2255f3a3bc80ec6f25c087ed497d88399c5f8b 100644
--- a/on-host/src/main.rs
+++ b/on-host/src/main.rs
@@ -1,7 +1,5 @@
mod cli;
mod packet;
-// #[cfg(feature = "tdl")]
-// mod tdl;
mod tm;
mod utils;
@@ -22,8 +20,6 @@ use skyward_mavlink::{
};
use tm::{read_high_rate_telemetry, read_low_rate_telemetry};
-// #[cfg(feature = "tdl")]
-// use crate::cli::ModeFile;
use crate::{
cli::Cli,
packet::PacketSequence,
@@ -108,28 +104,6 @@ fn main() -> Result<()> {
Ok(())
}
-// #[cfg(feature = "tdl")]
-// fn get_packets(args: &Cli) -> Result<Vec<MavMessage>> {
-// let packets: Vec<MavMessage> = match &args.mode {
-// ModeFile::Csv { csv_file } => {
-// info!("reading from csv file {}", csv_file.display());
-// read_packets_from_csv(File::open(csv_file).into_diagnostic()?).into_diagnostic()?
-// }
-// ModeFile::Tdl {
-// tdl_file,
-// lat,
-// lon,
-// alt,
-// } => {
-// info!("reading from tdl file {}", tdl_file.display());
-// tdl::generate_packets_from_tdl(tdl_file, *lat, *lon, *alt)?
-// }
-// };
-// debug!("collected {} packets", packets.len());
-// Ok(packets)
-// }
-
-// #[cfg(not(feature = "tdl"))]
fn get_packets(args: &Cli) -> Result<Vec<TimedMessage>> {
info!(
"reading low rate telemetry from file {}",
diff --git a/on-host/src/tdl.rs b/on-host/src/tdl.rs
deleted file mode 100644
index 77807b527a217c353745f6faf73b36f9bcd029cf..0000000000000000000000000000000000000000
--- a/on-host/src/tdl.rs
+++ /dev/null
@@ -1,198 +0,0 @@
-use std::{path::Path, time::Instant};
-
-use miette::{miette, Context, IntoDiagnostic, Result};
-use skyward_mavlink::lyra::{MavMessage, ROCKET_FLIGHT_TM_DATA};
-use tdl_core::{
- semantics::{Executor, Point, Trajectory},
- syntax::CachedLexer,
-};
-
-pub fn generate_packets_from_tdl(
- tdl_file: impl AsRef<Path>,
- gps_lat: f64,
- gps_lon: f64,
- gps_alt: f64,
-) -> Result<Vec<MavMessage>> {
- let tdl_content = std::fs::read_to_string(tdl_file.as_ref())
- .into_diagnostic()
- .wrap_err("Failed to read TDL file")?;
- let program = CachedLexer::new(&tdl_content)
- .parse_program()
- .wrap_err("Failed to parse TDL program")?;
- let traj = Executor::new(program)
- .run()
- .or(Err(miette!("Failed to execute TDL program")))?;
- let generator = RocketFlightGenerator::new(gps_lat, gps_lon, gps_alt);
- generator.generate_packets(traj)
-}
-
-trait MessageGenerator {
- fn generate_packets(self, traj: Trajectory) -> Result<Vec<MavMessage>>;
-}
-
-struct RocketFlightGenerator {
- normalization: Option<u128>,
- max_altitude: f64,
- initial_gps: (f64, f64, f64),
- last_time: Option<f64>,
- last_pos: Option<(f64, f64, f64)>,
- last_vel: Option<(f64, f64, f64)>,
- last_acc: Option<(f64, f64, f64)>,
- ada_flip: bool,
-}
-
-impl RocketFlightGenerator {
- fn new(lat: f64, lon: f64, alt: f64) -> Self {
- Self {
- initial_gps: (lat, lon, alt),
- normalization: None,
- max_altitude: 0.0,
- last_time: None,
- last_pos: None,
- last_vel: None,
- last_acc: None,
- ada_flip: false,
- }
- }
-
- fn compute_velocity(&self, point: Point) -> Option<(f64, f64, f64)> {
- if let Some((x, y, z)) = self.last_pos {
- let dt = point.time - self.last_time?;
- let dx = point.x - x;
- let dy = point.y - y;
- let dz = point.z - z;
- Some((dx / dt, dy / dt, dz / dt))
- } else {
- None
- }
- }
-
- fn compute_acceleration(
- &self,
- vel: Option<(f64, f64, f64)>,
- time: f64,
- ) -> Option<(f64, f64, f64)> {
- let (vx, vy, vz) = vel?;
- if let Some((lvx, lvy, lvz)) = self.last_vel {
- let dt = time - self.last_time?;
- let dvx = vx - lvx;
- let dvy = vy - lvy;
- let dvz = vz - lvz;
- Some((dvx / dt, dvy / dt, dvz / dt))
- } else {
- None
- }
- }
-
- fn consume_point(&mut self, point: Point) -> ROCKET_FLIGHT_TM_DATA {
- let Point { x, y, z, time: t } = point;
-
- // normalize time relative to now
- let time = if let Some(offset) = self.normalization {
- (t * 1e6).floor().min(u128::MAX as f64) as u128 + offset
- } else {
- let time_us = (t * 1e6).floor().min(u128::MAX as f64) as u128;
- self.normalization = Some(Instant::now().elapsed().as_micros() - time_us);
- 0
- };
-
- // check for overflow
- if time > u64::MAX as u128 {
- panic!("Time overflow");
- }
-
- // calculate velocity and acceleration
- let vel = self.compute_velocity(point);
- let acc = self.compute_acceleration(vel, t).unwrap_or_default();
- let vel = vel.unwrap_or_default();
- // and save them
- self.last_time = Some(t);
- self.last_pos = Some((x, y, z));
- self.last_vel = Some(vel);
- self.last_acc = Some(acc);
-
- // check for max altitude reached
- if z >= self.max_altitude && !self.ada_flip {
- self.max_altitude = z;
- } else {
- self.ada_flip = true;
- }
-
- // deconstruct the tuple
- let (x, y, z) = (x as f32, y as f32, z as f32);
- let (vx, vy, vz) = (vel.0 as f32, vel.1 as f32, vel.2 as f32);
- let (ax, ay, az) = (acc.0 as f32, acc.1 as f32, acc.2 as f32);
-
- let pin = if self.ada_flip { 1 } else { 0 };
-
- let (lat, lon, alt) = self.initial_gps;
- let (lat, lon, alt) = (lat as f32, lon as f32, alt as f32);
-
- ROCKET_FLIGHT_TM_DATA {
- timestamp: time as u64,
- pressure_digi: 88400.0,
- pressure_static: 88400.0,
- airspeed_pitot: 10.0,
- altitude_agl: z,
- acc_x: ax,
- acc_y: ay,
- acc_z: az,
- gyro_x: 0.0,
- gyro_y: 0.0,
- gyro_z: 0.0,
- mag_x: 0.0,
- mag_y: 0.0,
- mag_z: 0.0,
- gps_lat: lat,
- gps_lon: lon,
- gps_alt: alt,
- nas_n: y,
- nas_e: x,
- nas_d: -z,
- nas_vn: vy,
- nas_ve: vx,
- nas_vd: -vz,
- nas_qx: 0.0,
- nas_qy: 0.0,
- nas_qz: 0.0,
- nas_qw: 1.0,
- nas_bias_x: 0.0,
- nas_bias_y: 0.0,
- nas_bias_z: 0.0,
- battery_voltage: 8.0,
- cam_battery_voltage: 8.0,
- current_consumption: 0.0,
- temperature: 40.0,
- fmm_state: 0,
- nas_state: 0,
- gps_fix: 3,
- pin_nosecone: pin,
- cutter_presence: 0,
- logger_error: 0,
- pressure_ada: 0.0,
- pressure_dpl: 0.0,
- ada_vert_speed: 0.0,
- mea_mass: 0.0,
- abk_angle: 0.0,
- pin_quick_connector: 0.0,
- ada_state: 0,
- dpl_state: 0,
- abk_state: 0,
- mea_state: 0,
- pin_launch: 0,
- pin_expulsion: 0,
- }
- }
-}
-
-impl MessageGenerator for RocketFlightGenerator {
- fn generate_packets(mut self, traj: Trajectory) -> Result<Vec<MavMessage>> {
- let Trajectory { points } = traj;
- let mut packets = Vec::new();
- for point in points {
- let packet = self.consume_point(point);
- packets.push(MavMessage::ROCKET_FLIGHT_TM(packet));
- }
- Ok(packets)
- }
-}