refactored for rovie

on-host/src/cli.rs

@@ -23,13 +23,20 @@ pub struct Cli {
     #[clap(short, long)]
     pub no_sync: bool,
 
-    /// The low rate telemetry CSV file to read from
-    #[clap(value_name = "LOW_RATE_FILE")]
-    pub low_rate_file: PathBuf,
+    // /// The low rate telemetry CSV file to read from
+    // #[clap(value_name = "LOW_RATE_MAIN_FILE")]
+    // pub low_rate_file_main: PathBuf,
 
+    // /// The low rate telemetry CSV file to read from
+    // #[clap(value_name = "LOW_RATE_PAYLOAD_FILE")]
+    // pub low_rate_file_payload: PathBuf,
     /// The high rate telemetry CSV file to read from
-    #[clap(value_name = "HIGH_RATE_FILE")]
-    pub high_rate_file: PathBuf,
+    #[clap(value_name = "HIGH_RATE_MAIN_FILE")]
+    pub high_rate_file_main: PathBuf,
+
+    /// The high rate telemetry CSV file to read from
+    #[clap(value_name = "HIGH_RATE_PAYLOAD_FILE")]
+    pub high_rate_file_payload: PathBuf,
 
     /// The log level
     #[clap(short, long)]

on-host/src/main.rs

@@ -18,7 +18,7 @@ use skyward_mavlink::{
     lyra::MavMessage,
     mavlink::{write_v1_msg, MavHeader},
 };
-use tm::{read_high_rate_telemetry, read_low_rate_telemetry};
+use tm::{read_high_rate_telemetry, read_low_rate_telemetry, Origin};
 
 use crate::{
     cli::Cli,
@@ -58,21 +58,21 @@ fn main() -> Result<()> {
     let mut packets = PacketSequence::from(packets);
 
     // Open the first serialport available.
-    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);
-    };
-    debug!("connecting to serial port {}...", port_name);
-    let mut port = serialport::new(port_name, args.baud_rate)
-        .open()
-        .into_diagnostic()
-        .wrap_err("Failed to open serial port")?;
-    let mut write_port = port.try_clone().into_diagnostic()?;
+    // 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);
+    // };
+    // debug!("connecting to serial port {}...", port_name);
+    // let mut port = serialport::new(port_name, args.baud_rate)
+    //     .open()
+    //     .into_diagnostic()
+    //     .wrap_err("Failed to open serial port")?;
+    // let mut write_port = port.try_clone().into_diagnostic()?;
 
     // read from buffer and print to stdout
     let mut buffer = [0u8; 1000];
@@ -85,7 +85,7 @@ fn main() -> Result<()> {
             times.push(t.elapsed().as_millis());
         }
         start = Some(Instant::now());
-        write_port.write_packet(packet.to_owned())?;
+        // write_port.write_packet(packet.to_owned())?;
         debug!(
             "sent packet ({}/{})",
             packets.sent_packets(),
@@ -93,9 +93,9 @@ fn main() -> Result<()> {
         );
 
         // if sync mode is enabled then wait for ACK
-        if !args.no_sync {
-            wait_for_ack(&mut port, &mut buffer);
-        }
+        // if !args.no_sync {
+        //     wait_for_ack(&mut port, &mut buffer);
+        // }
     }
 
     info!("sent {} packets", packets.sent_packets());
@@ -105,18 +105,31 @@ fn main() -> Result<()> {
 }
 
 fn get_packets(args: &Cli) -> Result<Vec<TimedMessage>> {
+    // info!(
+    //     "reading low rate main telemetry from file {}",
+    //     args.low_rate_file_main.display()
+    // );
+    // info!(
+    //     "reading low rate payload telemetry from file {}",
+    //     args.low_rate_file_payload.display()
+    // );
     info!(
-        "reading low rate telemetry from file {}",
-        args.low_rate_file.display()
+        "reading high rate main telemetry from file {}",
+        args.high_rate_file_main.display()
     );
     info!(
-        "reading high rate telemetry from file {}",
-        args.high_rate_file.display()
+        "reading high rate payload telemetry from file {}",
+        args.high_rate_file_payload.display()
     );
-    let lrtf = File::open(&args.low_rate_file).into_diagnostic()?;
-    let hrtf = File::open(&args.high_rate_file).into_diagnostic()?;
-    let mut packets = read_low_rate_telemetry(lrtf).into_diagnostic()?;
-    packets.extend(read_high_rate_telemetry(hrtf).into_diagnostic()?);
+    // let lrtf_main = File::open(&args.low_rate_file_main).into_diagnostic()?;
+    // let lrtf_payload = File::open(&args.low_rate_file_payload).into_diagnostic()?;
+    let hrtf_main = File::open(&args.high_rate_file_main).into_diagnostic()?;
+    let hrtf_payload = File::open(&args.high_rate_file_payload).into_diagnostic()?;
+    // let mut packets = read_low_rate_telemetry(lrtf_payload, Origin::Payload).into_diagnostic()?;
+    // packets.extend(read_low_rate_telemetry(lrtf_main, Origin::Main).into_diagnostic()?);
+    let mut packets = read_high_rate_telemetry(hrtf_payload, Origin::Payload).into_diagnostic()?;
+    // packets.extend(read_high_rate_telemetry(hrtf_payload, Origin::Payload).into_diagnostic()?);
+    packets.extend(read_high_rate_telemetry(hrtf_main, Origin::Main).into_diagnostic()?);
     packets.sort();
     debug!("collected {} packets", packets.len());
     Ok(packets)

on-host/src/tm.rs

 use std::io::{Read, Result};
 
 use serde::Deserialize;
-use skyward_mavlink::lyra::{MavMessage, ROCKET_FLIGHT_TM_DATA, ROCKET_STATS_TM_DATA};
+use skyward_mavlink::lyra::{
+    MavMessage, PAYLOAD_FLIGHT_TM_DATA, PAYLOAD_STATS_TM_DATA, ROCKET_FLIGHT_TM_DATA,
+    ROCKET_STATS_TM_DATA,
+};
 
 use crate::packet::TimedMessage;
 
@@ -14,7 +17,7 @@ struct Lrtm {
 }
 
 #[derive(Debug, Deserialize)]
-struct Hrtm {
+struct HrtmMain {
     timestamp: u64,
     n: f64,
     e: f64,
@@ -22,14 +25,36 @@ struct Hrtm {
     vn: f64,
     ve: f64,
     vd: f64,
+    #[serde(rename = "aglAltitude")]
+    altitude_agl: f64,
+    #[serde(rename = "verticalSpeed")]
+    vertical_speed: f64,
+}
+
+#[derive(Debug, Deserialize)]
+struct HrtmPayload {
+    timestamp: u64,
+    n: f64,
+    e: f64,
+    d: f64,
+    vn: f64,
+    ve: f64,
+    vd: f64,
+}
+
+pub enum Origin {
+    Main,
+    Payload,
 }
 
 /// Read low rate telemetry data from a CSV file
-pub fn read_low_rate_telemetry(reader: impl Read) -> Result<Vec<TimedMessage>> {
+pub fn read_low_rate_telemetry(reader: impl Read, origin: Origin) -> Result<Vec<TimedMessage>> {
     let mut packets = Vec::new();
     let mut rdr = csv::Reader::from_reader(reader);
     for result in rdr.deserialize() {
         let r: Lrtm = result?;
+        match origin {
+            Origin::Main => {
                 let msg_data = ROCKET_STATS_TM_DATA {
                     ref_lat: r.latitude as f32,
                     ref_lon: r.longitude as f32,
@@ -39,15 +64,29 @@ pub fn read_low_rate_telemetry(reader: impl Read) -> Result<Vec<TimedMessage>> {
                 let mav_msg = MavMessage::ROCKET_STATS_TM(msg_data);
                 packets.push(TimedMessage::new(r.timestamp, mav_msg));
             }
+            Origin::Payload => {
+                let msg_data = PAYLOAD_STATS_TM_DATA {
+                    ref_lat: r.latitude as f32,
+                    ref_lon: r.longitude as f32,
+                    ref_alt: r.altitude as f32,
+                    ..Default::default()
+                };
+                let mav_msg = MavMessage::PAYLOAD_STATS_TM(msg_data);
+                packets.push(TimedMessage::new(r.timestamp, mav_msg));
+            }
+        }
+    }
     Ok(packets)
 }
 
 /// Read high rate telemtry data from a CSV file
-pub fn read_high_rate_telemetry(reader: impl Read) -> Result<Vec<TimedMessage>> {
+pub fn read_high_rate_telemetry(reader: impl Read, origin: Origin) -> Result<Vec<TimedMessage>> {
     let mut packets = Vec::new();
     let mut rdr = csv::Reader::from_reader(reader);
+    match origin {
+        Origin::Main => {
             for result in rdr.deserialize() {
-        let r: Hrtm = result?;
+                let r: HrtmMain = result?;
                 let msg_data = ROCKET_FLIGHT_TM_DATA {
                     timestamp: r.timestamp,
                     nas_n: r.n as f32,
@@ -56,10 +95,31 @@ pub fn read_high_rate_telemetry(reader: impl Read) -> Result<Vec<TimedMessage>>
                     nas_vn: r.vn as f32,
                     nas_ve: r.ve as f32,
                     nas_vd: r.vd as f32,
+                    altitude_agl: r.altitude_agl as f32,
+                    ada_vert_speed: r.vertical_speed as f32,
                     ..Default::default()
                 };
                 let mav_msg = MavMessage::ROCKET_FLIGHT_TM(msg_data);
                 packets.push(TimedMessage::new(r.timestamp, mav_msg));
             }
+        }
+        Origin::Payload => {
+            for result in rdr.deserialize() {
+                let r: HrtmPayload = result?;
+                let msg_data = PAYLOAD_FLIGHT_TM_DATA {
+                    timestamp: r.timestamp,
+                    nas_n: r.n as f32,
+                    nas_e: r.e as f32,
+                    nas_d: r.d as f32,
+                    nas_vn: r.vn as f32,
+                    nas_ve: r.ve as f32,
+                    nas_vd: r.vd as f32,
+                    ..Default::default()
+                };
+                let mav_msg = MavMessage::PAYLOAD_FLIGHT_TM(msg_data);
+                packets.push(TimedMessage::new(r.timestamp, mav_msg));
+            }
+        }
+    }
     Ok(packets)
 }

tools/log-converter/log_converter_lyra_rovie.py

+import os
+from pathlib import Path
+
+import click
+import polars as pl
+
+NAS_CALIBRATE_STATE = 1
+MAIN_FLYING_FMM_STATE = 10
+PAYLOAD_FLYING_FMM_STATE = 9
+SECONDS_BEFORE_FLYING = 15
+
+
+@click.command()
+@click.argument("path", type=click.Path(exists=True))
+@click.option("--output", type=click.Path(), default=".")
+@click.option("--origin-main", "origin", flag_value="main", default=True)
+@click.option("--origin-payload", "origin", flag_value="payload")
+def main(path: Path, output: Path, origin: str):
+    """
+    Convert the logs found in the given path to a format compliant with ARPIST.
+    """
+    # now walk in the directory pointed by path the files: main_Boardcore_EventData.csv, main_Boardcore_NASState.csv, main_Boardcore_ReferenceValues.csv and save their path to variables
+    nas_controller_status_path = None
+    nas_state_path = None
+    ada_state_path = None
+    fmm_status_path = None
+    gps_path = None
+
+    for p, _, fn in os.walk(path):
+        for f in fn:
+            if origin == "main":
+                if f == "main_Main_NASControllerStatus.csv":
+                    nas_controller_status_path = os.path.join(p, f)
+                elif f == "main_Main_FlightModeManagerStatus.csv":
+                    fmm_status_path = os.path.join(p, f)
+                elif f == "main_Boardcore_NASState.csv":
+                    nas_state_path = os.path.join(p, f)
+                elif f == "main_Boardcore_ADAState.csv":
+                    ada_state_path = os.path.join(p, f)
+            elif origin == "payload":
+                if f == "payload_Payload_NASControllerStatus.csv":
+                    nas_controller_status_path = os.path.join(p, f)
+                elif f == "payload_Payload_FlightModeManagerStatus.csv":
+                    fmm_status_path = os.path.join(p, f)
+                elif f == "payload_Boardcore_NASState.csv":
+                    nas_state_path = os.path.join(p, f)
+                elif f == "payload_Boardcore_UBXGPSData.csv":
+                    gps_path = os.path.join(p, f)
+
+    if nas_controller_status_path is None:
+        raise FileNotFoundError("NAS controller status file not found")
+    if nas_state_path is None:
+        raise FileNotFoundError("NAS state file not found")
+    if ada_state_path is None and origin == "main":
+        raise FileNotFoundError("ADA state file not found")
+    if fmm_status_path is None:
+        raise FileNotFoundError("FMM status file not found")
+    if origin == "payload" and gps_path is None:
+        raise FileNotFoundError("GPS data file not found")
+
+    nas_controller_status = pl.read_csv(nas_controller_status_path)
+    nas_state = pl.read_csv(nas_state_path)
+    if origin == "main":
+        ada_state = pl.read_csv(ada_state_path, infer_schema_length=10000000)
+    fmm_status = pl.read_csv(fmm_status_path)
+
+    # sort by timestamp and extract the timestamp associated to the calibrate event and topic
+    nas_controller_status = nas_controller_status.sort("timestamp")
+    calibrate_tms = nas_controller_status.filter(
+        pl.col("state") == NAS_CALIBRATE_STATE
+    ).select("timestamp")
+
+    nas_state = nas_state.select(
+        pl.from_epoch(pl.col("timestamp"), time_unit="us"),
+        "n",
+        "e",
+        "d",
+        "vn",
+        "ve",
+        "vd",
+    )
+    if origin == "main":
+        ada_state = ada_state.select(
+            pl.from_epoch(pl.col("timestamp"), time_unit="us"),
+            "aglAltitude",
+            "verticalSpeed",
+        )
+    fmm_status = fmm_status.select(
+        pl.from_epoch(pl.col("timestamp"), time_unit="us"), "state"
+    )
+
+    # find stop timestamp based on last state of the fmm plus 10 seconds
+    stop_ts = fmm_status.select(pl.col("timestamp") + pl.duration(seconds=2))[-1, 0]
+
+    # filter the reference values and nas state dataframes
+    nas_state = nas_state.filter(pl.col("timestamp") <= stop_ts)
+    if origin == "main":
+        ada_state = ada_state.filter(pl.col("timestamp") <= stop_ts)
+
+    # find max timestamp
+    max_ts = nas_state.select("timestamp").max().item(0, 0)
+
+    # upsample and downsample the dataframes
+    nas_state = (
+        nas_state.group_by_dynamic(pl.col("timestamp"), every="50ms")
+        .agg(pl.all().last())
+        .upsample(time_column="timestamp", every="50ms")
+        .fill_null(strategy="forward")
+    )
+    if origin == "main":
+        ada_state = (
+            ada_state.group_by_dynamic(pl.col("timestamp"), every="50ms")
+            .agg(pl.all().last())
+            .upsample(time_column="timestamp", every="50ms")
+            .fill_null(strategy="forward")
+        )
+
+    # filter from 15 seconds before flying
+    flying_fmm_state = (
+        MAIN_FLYING_FMM_STATE if origin == "main" else PAYLOAD_FLYING_FMM_STATE
+    )
+    start_ts = fmm_status.filter(pl.col("state") == flying_fmm_state).select(
+        "timestamp"
+    )[0, 0] - pl.duration(seconds=SECONDS_BEFORE_FLYING)
+    nas_state = nas_state.filter(pl.col("timestamp") >= start_ts)
+    if origin == "main":
+        ada_state = ada_state.filter(pl.col("timestamp") >= start_ts)
+
+    if origin == "main":
+        nas_state = nas_state.with_columns(
+            ada_state.select("aglAltitude", "verticalSpeed")
+        )
+
+    # save the dataframes to csv
+    output = Path(output)
+    if origin == "main":
+        nas_state.select(
+            pl.col("timestamp").dt.timestamp(time_unit="us"),
+            "n",
+            "e",
+            "d",
+            "vn",
+            "ve",
+            "vd",
+            "aglAltitude",
+            "verticalSpeed",
+        ).write_csv(output / "high_rate.csv")
+    elif origin == "payload":
+        nas_state.select(
+            pl.col("timestamp").dt.timestamp(time_unit="us"),
+            "n",
+            "e",
+            "d",
+            "vn",
+            "ve",
+            "vd",
+        ).write_csv(output / "high_rate.csv")
+
+
+if __name__ == "__main__":
+    main()