From 228550536be2fdba2ff441e2ad521f9b51d0dc16 Mon Sep 17 00:00:00 2001 From: Federico Lolli <federico.lolli@skywarder.eu> Date: Mon, 14 Oct 2024 14:31:45 +0200 Subject: [PATCH] refactored for rovie --- high_rate_main.csv | 3 + high_rate_payload.csv | 3 + on-host/src/cli.rs | 17 +- on-host/src/main.rs | 69 +++++--- on-host/src/tm.rs | 112 +++++++++--- .../log-converter/log_converter_lyra_rovie.py | 161 ++++++++++++++++++ 6 files changed, 306 insertions(+), 59 deletions(-) create mode 100644 high_rate_main.csv create mode 100644 high_rate_payload.csv create mode 100644 tools/log-converter/log_converter_lyra_rovie.py diff --git a/high_rate_main.csv b/high_rate_main.csv new file mode 100644 index 0000000..538fc72 --- /dev/null +++ b/high_rate_main.csv @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:40b7f91e9378bc19b70013d6e4b8442527b3e145c6ec1c50af92f54d6d135168 +size 1943586 diff --git a/high_rate_payload.csv b/high_rate_payload.csv new file mode 100644 index 0000000..806e3ef --- /dev/null +++ b/high_rate_payload.csv @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:8494ea86c00709f149093138f3582eacadd1e5f920706551c027db6b902a4e64 +size 1498292 diff --git a/on-host/src/cli.rs b/on-host/src/cli.rs index 9b4d071..2f71e4a 100644 --- a/on-host/src/cli.rs +++ b/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)] diff --git a/on-host/src/main.rs b/on-host/src/main.rs index 7d2255f..53332cf 100644 --- a/on-host/src/main.rs +++ b/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) diff --git a/on-host/src/tm.rs b/on-host/src/tm.rs index 86abc24..6d5e9b0 100644 --- a/on-host/src/tm.rs +++ b/on-host/src/tm.rs @@ -1,7 +1,10 @@ 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,44 +25,101 @@ 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?; - let msg_data = ROCKET_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::ROCKET_STATS_TM(msg_data); - packets.push(TimedMessage::new(r.timestamp, mav_msg)); + match origin { + Origin::Main => { + let msg_data = ROCKET_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::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); - for result in rdr.deserialize() { - let r: Hrtm = result?; - let msg_data = ROCKET_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::ROCKET_FLIGHT_TM(msg_data); - packets.push(TimedMessage::new(r.timestamp, mav_msg)); + match origin { + Origin::Main => { + for result in rdr.deserialize() { + let r: HrtmMain = result?; + let msg_data = ROCKET_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, + 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) } diff --git a/tools/log-converter/log_converter_lyra_rovie.py b/tools/log-converter/log_converter_lyra_rovie.py new file mode 100644 index 0000000..051eac8 --- /dev/null +++ b/tools/log-converter/log_converter_lyra_rovie.py @@ -0,0 +1,161 @@ +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() -- GitLab