Prototype
This commit is contained in:
@@ -0,0 +1,27 @@
|
||||
[package]
|
||||
name = "libsvld"
|
||||
version = "0.1.0"
|
||||
edition = "2024"
|
||||
|
||||
[lib]
|
||||
crate-type = ["lib"]
|
||||
bench = false
|
||||
|
||||
[dependencies]
|
||||
can-dbc = "9.1.0"
|
||||
codegen = "0.3.0"
|
||||
dbc-codegen = "0.3.0"
|
||||
nom = "8.0.0"
|
||||
bytes = "1.11.1"
|
||||
memmap2 = "0.9.10"
|
||||
memchr = "2.8.0"
|
||||
crossbeam-channel = "0.5.15"
|
||||
hashbrown = { version = "0.17.1", features = ["rayon"] }
|
||||
bitvec = "1.0.1"
|
||||
byteorder = "1.5.0"
|
||||
page_size = "0.6.0"
|
||||
positioned-io = "0.3.5"
|
||||
atoi_simd = "0.18.1"
|
||||
voracious_radix_sort = { version = "1.2.0", features = ["voracious_multithread"] }
|
||||
rayon = "1.12.0"
|
||||
chrono = { version = "0.4", default-features = false, features = ["std"] }
|
||||
@@ -0,0 +1,84 @@
|
||||
use crate::can_frame::{CanFrame, Direction};
|
||||
|
||||
#[inline]
|
||||
fn next_token<'a>(input: &mut &'a [u8]) -> Option<&'a [u8]> {
|
||||
*input = input.trim_ascii_start();
|
||||
if input.is_empty() {
|
||||
return None;
|
||||
}
|
||||
|
||||
// Stop on space, tab, or CR. Lines end with CRLF, so stopping on CR keeps
|
||||
// the cursor on the line; the leading \r\n is then consumed by the next
|
||||
// call's trim_ascii_start — no need to ever search for '\n'.
|
||||
let end = memchr::memchr3(b' ', b'\t', b'\r', input).unwrap_or(input.len());
|
||||
let token = &input[..end];
|
||||
*input = &input[end..];
|
||||
Some(token)
|
||||
}
|
||||
|
||||
pub enum AscLineType {
|
||||
Frame(CanFrame),
|
||||
Errorframe,
|
||||
}
|
||||
|
||||
/// Parse an ASC header date line of the form:
|
||||
/// `date Sun May 3 08:14:59 pm 2026`
|
||||
///
|
||||
/// Returns a naive (timezone-less) datetime. Format it as ISO 8601 with
|
||||
/// `dt.format("%Y-%m-%dT%H:%M:%S").to_string()`.
|
||||
pub fn parse_asc_date(input: &[u8]) -> Option<chrono::NaiveDateTime> {
|
||||
let s = std::str::from_utf8(input).ok()?.trim();
|
||||
chrono::NaiveDateTime::parse_from_str(s, "date %a %b %e %I:%M:%S %P %Y").ok()
|
||||
}
|
||||
|
||||
/// Parses one ASC line starting at `input[0]`.
|
||||
///
|
||||
/// Returns the parsed line type and the number of bytes consumed. On success
|
||||
/// the consumed count leaves `input[consumed..]` pointing at the trailing
|
||||
/// `\r\n`, which the next call's `trim_ascii_start` will eat.
|
||||
///
|
||||
/// `None` means the line could not be parsed (header line, truncated tail,
|
||||
/// corrupt frame). The caller is responsible for advancing past the bad line.
|
||||
pub fn parse_asc_line(input: &[u8]) -> Option<(AscLineType, usize)> {
|
||||
let mut cursor = input;
|
||||
|
||||
let timestamp: f64 = {
|
||||
let tok = next_token(&mut cursor)?;
|
||||
unsafe { std::str::from_utf8_unchecked(tok) }.parse().ok()?
|
||||
};
|
||||
let channel = atoi_simd::parse_pos::<u8, false>(next_token(&mut cursor)?).ok()?;
|
||||
|
||||
let tok = next_token(&mut cursor)?;
|
||||
if tok[0] == b'E' {
|
||||
return Some((AscLineType::Errorframe, input.len() - cursor.len()));
|
||||
}
|
||||
let can_id = atoi_simd::parse_pos::<u32, false>(tok).ok()?;
|
||||
|
||||
let direction = match next_token(&mut cursor)? {
|
||||
b"Rx" => Direction::Rx,
|
||||
b"Tx" => Direction::Tx,
|
||||
_ => return None,
|
||||
};
|
||||
|
||||
// 'D' frame type
|
||||
next_token(&mut cursor)?;
|
||||
|
||||
let dlc = atoi_simd::parse_pos::<u8, false>(next_token(&mut cursor)?).ok()?;
|
||||
|
||||
let mut data = [0u8; 8];
|
||||
for byte in data.iter_mut().take(dlc as usize) {
|
||||
*byte = atoi_simd::parse_pos::<u8, false>(next_token(&mut cursor)?).ok()?;
|
||||
}
|
||||
|
||||
Some((
|
||||
AscLineType::Frame(CanFrame {
|
||||
timestamp,
|
||||
channel,
|
||||
can_id,
|
||||
direction,
|
||||
dlc,
|
||||
data,
|
||||
}),
|
||||
input.len() - cursor.len(),
|
||||
))
|
||||
}
|
||||
@@ -0,0 +1,40 @@
|
||||
use voracious_radix_sort::Radixable;
|
||||
|
||||
pub type CanId = u32;
|
||||
|
||||
#[derive(Clone, Debug, PartialEq, Copy)]
|
||||
pub enum Direction {
|
||||
Rx,
|
||||
Tx,
|
||||
}
|
||||
|
||||
#[derive(Debug, Clone, Copy)]
|
||||
pub struct CanFrame {
|
||||
pub timestamp: f64,
|
||||
pub channel: u8,
|
||||
pub can_id: CanId,
|
||||
pub direction: Direction,
|
||||
pub dlc: u8,
|
||||
pub data: [u8; 8],
|
||||
}
|
||||
|
||||
impl PartialOrd for CanFrame {
|
||||
fn partial_cmp(&self, other: &Self) -> Option<std::cmp::Ordering> {
|
||||
self.timestamp.partial_cmp(&other.timestamp)
|
||||
}
|
||||
}
|
||||
|
||||
// It's fine
|
||||
impl PartialEq for CanFrame {
|
||||
fn eq(&self, other: &Self) -> bool {
|
||||
self.timestamp == other.timestamp
|
||||
}
|
||||
}
|
||||
|
||||
impl Radixable<f64> for CanFrame {
|
||||
type Key = f64;
|
||||
#[inline]
|
||||
fn key(&self) -> Self::Key {
|
||||
self.timestamp
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,236 @@
|
||||
mod asc;
|
||||
pub mod can_frame;
|
||||
mod motec;
|
||||
|
||||
use std::{
|
||||
fs::File,
|
||||
path::Path,
|
||||
thread,
|
||||
};
|
||||
|
||||
use can_dbc::Dbc;
|
||||
use hashbrown::HashMap;
|
||||
#[cfg(unix)]
|
||||
use memmap2::Advice;
|
||||
use memmap2::Mmap;
|
||||
use voracious_radix_sort::RadixSort;
|
||||
|
||||
use crate::{
|
||||
asc::{parse_asc_date, parse_asc_line},
|
||||
can_frame::{CanFrame, CanId},
|
||||
motec::{MessageMetadata, SignalMetadata},
|
||||
};
|
||||
|
||||
fn frame_worker(
|
||||
mmap: &[u8],
|
||||
dbc: &Dbc,
|
||||
) -> (
|
||||
HashMap<CanId, Vec<CanFrame>>,
|
||||
HashMap<CanId, MessageMetadata>,
|
||||
) {
|
||||
let mut channel_data = HashMap::<CanId, Vec<CanFrame>>::new();
|
||||
let mut metadatas = HashMap::<CanId, MessageMetadata>::new();
|
||||
|
||||
let get_default = |name: &str| match dbc
|
||||
.attribute_defaults
|
||||
.iter()
|
||||
.find(|x| x.name == name)
|
||||
.unwrap()
|
||||
.value
|
||||
{
|
||||
can_dbc::AttributeValue::Uint(val) => val,
|
||||
_ => panic!("Bad {} value type, expected Uint.", name),
|
||||
};
|
||||
|
||||
let default_freqency = get_default("Frequency");
|
||||
let default_shift = get_default("ld_shift");
|
||||
let default_scale = get_default("ld_scale");
|
||||
let default_decimal_places = get_default("ld_decimal_places");
|
||||
|
||||
let mut add_metadata = |id| {
|
||||
let msg = match dbc.messages.iter().find(|x| x.id.raw() == id) {
|
||||
Some(msg) => msg,
|
||||
None => {
|
||||
eprintln!("Could not find msg with id: {}", id);
|
||||
return;
|
||||
}
|
||||
};
|
||||
let signals = &msg.signals;
|
||||
let mut signals_meta = Vec::<SignalMetadata>::new();
|
||||
for signal in signals {
|
||||
let decimal_places = dbc
|
||||
.attribute_values_signal
|
||||
.iter()
|
||||
.find(|x| {
|
||||
x.message_id.raw() == id
|
||||
&& x.signal_name == signal.name
|
||||
&& x.name == "ld_decimal_places"
|
||||
})
|
||||
.map_or(default_decimal_places, |x| match x.value {
|
||||
can_dbc::AttributeValue::Uint(val) => val,
|
||||
_ => panic!("Bad ld_decimal_places value type, expected Uint."),
|
||||
});
|
||||
|
||||
signals_meta.push(SignalMetadata {
|
||||
shift: default_shift as u16,
|
||||
scale: default_scale as u16,
|
||||
decimal_places: decimal_places as u16,
|
||||
name: signal.name.clone(),
|
||||
unit: signal.unit.clone(),
|
||||
signal: signal.clone(),
|
||||
});
|
||||
}
|
||||
|
||||
let frequency = dbc
|
||||
.attribute_values_message
|
||||
.iter()
|
||||
.find(|x| x.message_id.raw() == id && x.name == "Frequency")
|
||||
.map_or(default_freqency, |x| match x.value {
|
||||
can_dbc::AttributeValue::Uint(val) => val,
|
||||
_ => panic!("Bad Frequency value type, expected Uint."),
|
||||
});
|
||||
|
||||
metadatas.insert(
|
||||
id,
|
||||
MessageMetadata {
|
||||
id,
|
||||
frequency: frequency as u16,
|
||||
signals: signals_meta,
|
||||
},
|
||||
);
|
||||
};
|
||||
|
||||
// Caller has aligned the chunk to line boundaries, so we always start at
|
||||
// a fresh line. Chunk 0's ASC header lines fail to parse and fall into
|
||||
// the None branch below, which advances past them.
|
||||
let mut line_start = 0usize;
|
||||
|
||||
while line_start < mmap.len() {
|
||||
match parse_asc_line(&mmap[line_start..]) {
|
||||
Some((asc::AscLineType::Frame(frame), consumed)) => {
|
||||
match channel_data.get_mut(&frame.can_id) {
|
||||
Some(vec) => vec.push(frame),
|
||||
None => {
|
||||
unsafe {
|
||||
channel_data.insert_unique_unchecked(frame.can_id, vec![frame]);
|
||||
}
|
||||
add_metadata(frame.can_id);
|
||||
}
|
||||
}
|
||||
line_start += consumed;
|
||||
}
|
||||
Some((asc::AscLineType::Errorframe, consumed)) => {
|
||||
line_start += consumed;
|
||||
}
|
||||
None => {
|
||||
// Header lines (chunk 0), corrupt frames, or a truncated tail.
|
||||
// Walk to the next '\n' by hand — no memchr.
|
||||
while line_start < mmap.len() && mmap[line_start] != b'\n' {
|
||||
line_start += 1;
|
||||
}
|
||||
line_start += 1;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
(channel_data, metadatas)
|
||||
}
|
||||
|
||||
pub fn decode(dbc: &Dbc,
|
||||
asc_path: &Path,
|
||||
output_dir: &Path) {
|
||||
let thread_count = std::thread::available_parallelism().unwrap().get();
|
||||
|
||||
let file = File::open(asc_path).unwrap();
|
||||
let mmap = unsafe { Mmap::map(&file).unwrap() };
|
||||
|
||||
#[cfg(unix)]
|
||||
mmap.advise(Advice::Sequential).unwrap();
|
||||
#[cfg(unix)]
|
||||
mmap.advise(Advice::WillNeed).unwrap();
|
||||
|
||||
let date = parse_asc_date(&mmap[..memchr::memchr(b'\r', &mmap).unwrap()]).unwrap();
|
||||
|
||||
let date = date
|
||||
.and_utc()
|
||||
.to_rfc3339_opts(chrono::SecondsFormat::Secs, true);
|
||||
println!("Date: {}", date);
|
||||
|
||||
let page_size = page_size::get();
|
||||
let pages_per_chunk = (mmap.len() / page_size) / thread_count;
|
||||
let chunk_size = pages_per_chunk * page_size;
|
||||
|
||||
println!("Page size: {}", page_size);
|
||||
println!("Pages per chunk: {}", pages_per_chunk);
|
||||
println!("Chunk size: {}", chunk_size);
|
||||
println!("Thread count: {}", thread_count);
|
||||
|
||||
// Align chunk ends to '\n' so every line is wholly contained in one chunk.
|
||||
// Without this, the line spanning each boundary is lost on both sides.
|
||||
let mut bounds: Vec<usize> = Vec::with_capacity(thread_count + 1);
|
||||
bounds.push(0);
|
||||
let mut pos = 0usize;
|
||||
while pos + chunk_size < mmap.len() {
|
||||
let mut p = pos + chunk_size;
|
||||
// TODO: memchr
|
||||
while p < mmap.len() && mmap[p] != b'\n' {
|
||||
p += 1;
|
||||
}
|
||||
if p >= mmap.len() {
|
||||
break;
|
||||
}
|
||||
p += 1; // include the '\n'
|
||||
bounds.push(p);
|
||||
pos = p;
|
||||
}
|
||||
bounds.push(mmap.len());
|
||||
|
||||
let mut data = HashMap::<u32, Vec<CanFrame>>::new();
|
||||
let mut metadata = HashMap::<CanId, MessageMetadata>::new();
|
||||
thread::scope(|s| {
|
||||
let mut handles = vec![];
|
||||
for w in bounds.windows(2) {
|
||||
let mem = &mmap[w[0]..w[1]];
|
||||
let dbc = dbc.clone();
|
||||
handles.push(s.spawn(move || frame_worker(mem, &dbc)));
|
||||
}
|
||||
|
||||
for handle in handles {
|
||||
let (datah, metadatah) = handle.join().unwrap();
|
||||
for (id, mut datap) in datah {
|
||||
match data.get_mut(&id) {
|
||||
Some(out) => out.append(&mut datap),
|
||||
None => unsafe {
|
||||
data.insert_unique_unchecked(id, datap);
|
||||
},
|
||||
}
|
||||
}
|
||||
for (id, datap) in metadatah {
|
||||
if !metadata.contains_key(&id) {
|
||||
unsafe {
|
||||
metadata.insert_unique_unchecked(id, datap);
|
||||
};
|
||||
}
|
||||
}
|
||||
}
|
||||
});
|
||||
|
||||
println!("Post processing");
|
||||
|
||||
for (_, frames) in &mut data {
|
||||
// frames.voracious_mt_sort(thread_count);
|
||||
frames.voracious_sort();
|
||||
}
|
||||
|
||||
let msgs: Vec<MessageMetadata> = metadata.values().cloned().collect();
|
||||
|
||||
println!("Messages: {}", msgs.len());
|
||||
println!("Data: {}", data.len());
|
||||
|
||||
println!("Write to file...");
|
||||
// TODO joint paths
|
||||
let out_file_name = format!("{}.ld", date);
|
||||
let out_name = Path::new(&out_file_name);
|
||||
motec::write(&output_dir.join(out_name), &msgs, data);
|
||||
println!("Finished!");
|
||||
}
|
||||
@@ -0,0 +1,444 @@
|
||||
use std::{
|
||||
fs::OpenOptions,
|
||||
io::{Cursor, Seek, SeekFrom, Write},
|
||||
path::Path,
|
||||
};
|
||||
|
||||
use byteorder::{LittleEndian, WriteBytesExt};
|
||||
use can_dbc::{ByteOrder, MultiplexIndicator, Signal, ValueType};
|
||||
use hashbrown::HashMap;
|
||||
use memmap2::MmapMut;
|
||||
use rayon::prelude::*;
|
||||
|
||||
use crate::can_frame::{CanFrame, CanId};
|
||||
|
||||
const HEADER_SIZE: usize = 2914;
|
||||
|
||||
const METADATA_SIZE: u32 = 124;
|
||||
const METADATA_OFFSET: u32 = 2914;
|
||||
|
||||
#[derive(Debug, Clone)]
|
||||
pub struct SignalMetadata {
|
||||
pub shift: u16,
|
||||
pub scale: u16,
|
||||
pub decimal_places: u16,
|
||||
pub name: String,
|
||||
pub unit: String,
|
||||
pub signal: Signal,
|
||||
}
|
||||
|
||||
#[derive(Debug, Clone)]
|
||||
pub struct MessageMetadata {
|
||||
pub id: CanId,
|
||||
pub frequency: u16,
|
||||
pub signals: Vec<SignalMetadata>,
|
||||
}
|
||||
|
||||
fn write_header(mmap: &mut [u8], metadata_count: u32) {
|
||||
let mut cursor = Cursor::new(mmap);
|
||||
|
||||
cursor.seek(SeekFrom::Start(0)).unwrap();
|
||||
cursor.write_u32::<LittleEndian>(0x40).unwrap();
|
||||
|
||||
cursor.seek(SeekFrom::Start(0x08)).unwrap();
|
||||
cursor
|
||||
.write_u32::<LittleEndian>(HEADER_SIZE as u32)
|
||||
.unwrap();
|
||||
|
||||
cursor.seek(SeekFrom::Start(0x0C)).unwrap();
|
||||
cursor
|
||||
.write_u32::<LittleEndian>(METADATA_OFFSET + (METADATA_SIZE * metadata_count))
|
||||
.unwrap();
|
||||
|
||||
cursor.seek(SeekFrom::Start(0x24)).unwrap();
|
||||
cursor.write_u32::<LittleEndian>(0x6e2).unwrap();
|
||||
|
||||
cursor.seek(SeekFrom::Start(0x42)).unwrap();
|
||||
cursor.write_u16::<LittleEndian>(0x4240).unwrap();
|
||||
|
||||
cursor.seek(SeekFrom::Start(0x44)).unwrap();
|
||||
cursor.write_u16::<LittleEndian>(0xf).unwrap();
|
||||
|
||||
cursor.seek(SeekFrom::Start(0x46)).unwrap();
|
||||
cursor.write_u32::<LittleEndian>(0x2ee7).unwrap();
|
||||
|
||||
cursor.seek(SeekFrom::Start(0x4a)).unwrap();
|
||||
cursor.write_u8(b'A').unwrap();
|
||||
cursor.write_u8(b'D').unwrap();
|
||||
cursor.write_u8(b'L').unwrap();
|
||||
|
||||
cursor.seek(SeekFrom::Start(0x52)).unwrap();
|
||||
cursor.write_u16::<LittleEndian>(0x1a4).unwrap();
|
||||
|
||||
cursor.seek(SeekFrom::Start(0x54)).unwrap();
|
||||
cursor.write_u16::<LittleEndian>(0x80).unwrap();
|
||||
|
||||
cursor.seek(SeekFrom::Start(0x56)).unwrap();
|
||||
cursor.write_u32::<LittleEndian>(metadata_count).unwrap();
|
||||
|
||||
cursor.seek(SeekFrom::Start(0x5de)).unwrap();
|
||||
cursor.write_u32::<LittleEndian>(0xd20822).unwrap();
|
||||
}
|
||||
|
||||
pub fn get_metadata_total_count(
|
||||
msgs: &[MessageMetadata],
|
||||
data: &HashMap<CanId, Vec<CanFrame>>,
|
||||
) -> usize {
|
||||
data.iter()
|
||||
.map(|(id, _)| {
|
||||
msgs.iter()
|
||||
.find(|x| x.id == *id)
|
||||
.map_or(0, |x| x.signals.len())
|
||||
})
|
||||
.sum()
|
||||
}
|
||||
|
||||
/// Decode one signal's physical value from a CAN payload (LE-packed into
|
||||
/// `bytes_le`). Mirrors `calc_bits` + `calc_signed_bits` + factor/offset in
|
||||
/// _ref_lib/src/process.cc.
|
||||
#[inline]
|
||||
fn extract_signal(bytes_le: u64, sig: &Signal) -> f64 {
|
||||
let size = sig.size;
|
||||
let shifted = match sig.byte_order {
|
||||
ByteOrder::BigEndian => bytes_le.swap_bytes() >> (64 - size - sig.start_bit),
|
||||
ByteOrder::LittleEndian => bytes_le >> sig.start_bit,
|
||||
};
|
||||
let mask = if size >= 64 {
|
||||
u64::MAX
|
||||
} else {
|
||||
(1u64 << size) - 1
|
||||
};
|
||||
let b = shifted & mask;
|
||||
let raw = match sig.value_type {
|
||||
ValueType::Signed => {
|
||||
if size >= 64 {
|
||||
b as i64 as f64
|
||||
} else {
|
||||
let sign_bit = 1u64 << (size - 1);
|
||||
if b & sign_bit != 0 {
|
||||
((b | (u64::MAX << size)) as i64) as f64
|
||||
} else {
|
||||
b as f64
|
||||
}
|
||||
}
|
||||
}
|
||||
ValueType::Unsigned => b as f64,
|
||||
};
|
||||
raw * sig.factor + sig.offset
|
||||
}
|
||||
|
||||
/// Resample one message's signals onto a uniform `data_length`-slot grid at
|
||||
/// `frequency` Hz with forward-fill — see `order_frame` in
|
||||
/// _ref_lib/src/process.cc:427. Multiplexing is not implemented; every signal
|
||||
/// is extracted from every payload.
|
||||
///
|
||||
/// Output layout: row-major `grid[j * data_length + k]` for signal `j`, slot `k`.
|
||||
fn resample(
|
||||
samples: &[CanFrame],
|
||||
signals: &[SignalMetadata],
|
||||
frequency: u16,
|
||||
last_timestamp: f64,
|
||||
data_length: usize,
|
||||
) -> Vec<f64> {
|
||||
let nsig = signals.len();
|
||||
let mut grid = vec![0.0f64; nsig * data_length];
|
||||
if data_length == 0 {
|
||||
return grid;
|
||||
}
|
||||
let freq = frequency as f64;
|
||||
let mut vals = vec![0.0f64; nsig];
|
||||
let mut frames_ctr: usize = 0;
|
||||
for frame in samples {
|
||||
if frame.timestamp > last_timestamp {
|
||||
continue;
|
||||
}
|
||||
let mut new_fc = (frame.timestamp * freq) as usize;
|
||||
if new_fc >= data_length {
|
||||
new_fc = data_length - 1;
|
||||
}
|
||||
|
||||
let bytes_le = u64::from_le_bytes(frame.data);
|
||||
for j in 0..nsig {
|
||||
vals[j] = extract_signal(bytes_le, &signals[j].signal);
|
||||
}
|
||||
|
||||
if new_fc == frames_ctr {
|
||||
for j in 0..nsig {
|
||||
grid[j * data_length + frames_ctr] = vals[j];
|
||||
}
|
||||
} else if new_fc > frames_ctr {
|
||||
for j in 0..nsig {
|
||||
let row = j * data_length;
|
||||
let carry = grid[row + frames_ctr];
|
||||
grid[row + frames_ctr + 1..row + new_fc].fill(carry);
|
||||
grid[row + new_fc] = vals[j];
|
||||
}
|
||||
frames_ctr = new_fc;
|
||||
}
|
||||
}
|
||||
// Forward-fill the tail past the last observed sample.
|
||||
for j in 0..nsig {
|
||||
if signals[j].signal.multiplexer_indicator != MultiplexIndicator::Plain {
|
||||
panic!("Multiplex not implemented");
|
||||
}
|
||||
let row = j * data_length;
|
||||
let carry = grid[row + frames_ctr];
|
||||
grid[row + frames_ctr + 1..row + data_length].fill(carry);
|
||||
}
|
||||
grid
|
||||
}
|
||||
|
||||
/// Quantize one signal's resampled grid row into LD's int32-LE representation.
|
||||
/// Matches `encode_ld_data` in _ref_lib/src/process.cc:608.
|
||||
#[inline]
|
||||
fn quantize_signal_into(grid_row: &[f64], signal: &SignalMetadata, dst: &mut [u8]) {
|
||||
debug_assert_eq!(dst.len(), grid_row.len() * 4);
|
||||
let inv_scale = 1.0f64 / signal.scale.max(1) as f64;
|
||||
let p10 = 10f64.powi(signal.decimal_places as i32);
|
||||
let shift = signal.shift as f64;
|
||||
for (k, &v) in grid_row.iter().enumerate() {
|
||||
let q = (v * inv_scale * p10 + shift) as i32;
|
||||
dst[k * 4..k * 4 + 4].copy_from_slice(&q.to_le_bytes());
|
||||
}
|
||||
}
|
||||
|
||||
/// Fill one 124-byte channel meta record at fixed offsets. The byte layout is
|
||||
/// opaque LD format; do not interpret beyond "what MoTeC writes here".
|
||||
fn write_meta_record(
|
||||
dst: &mut [u8],
|
||||
prev_offset: u32,
|
||||
next_offset: u32,
|
||||
data_offset: u32,
|
||||
sample_count: u32,
|
||||
signal_i: u32,
|
||||
frequency: u16,
|
||||
signal: &SignalMetadata,
|
||||
) {
|
||||
debug_assert_eq!(dst.len(), METADATA_SIZE as usize);
|
||||
dst[0..4].copy_from_slice(&prev_offset.to_le_bytes());
|
||||
dst[4..8].copy_from_slice(&next_offset.to_le_bytes());
|
||||
dst[8..12].copy_from_slice(&data_offset.to_le_bytes());
|
||||
dst[12..16].copy_from_slice(&sample_count.to_le_bytes());
|
||||
dst[16..18].copy_from_slice(&(0x2ee1u16 + signal_i as u16).to_le_bytes());
|
||||
dst[18..20].copy_from_slice(&5u16.to_le_bytes());
|
||||
dst[20..22].copy_from_slice(&4u16.to_le_bytes());
|
||||
dst[22..24].copy_from_slice(&frequency.to_le_bytes());
|
||||
dst[24..26].copy_from_slice(&signal.shift.to_le_bytes());
|
||||
dst[26..28].copy_from_slice(&1u16.to_le_bytes());
|
||||
dst[28..30].copy_from_slice(&signal.scale.to_le_bytes());
|
||||
dst[30..32].copy_from_slice(&signal.decimal_places.to_le_bytes());
|
||||
|
||||
let nb = signal.name.as_bytes();
|
||||
let nl = nb.len().min(31);
|
||||
dst[32..32 + nl].copy_from_slice(&nb[..nl]);
|
||||
let sl = nb.len().min(7);
|
||||
dst[64..64 + sl].copy_from_slice(&nb[..sl]);
|
||||
|
||||
let ub = signal.unit.as_bytes();
|
||||
let ul = ub.len().min(7);
|
||||
dst[72..72 + ul].copy_from_slice(&ub[..ul]);
|
||||
}
|
||||
|
||||
/// Per-message output plan: where its meta records and sample data go in the
|
||||
/// final file, and what global signal index it starts at. Built sequentially
|
||||
/// so all offsets are deterministic; the actual work can then run in parallel.
|
||||
struct MsgPlan<'a> {
|
||||
msg: &'a MessageMetadata,
|
||||
samples: &'a [CanFrame],
|
||||
data_length: usize,
|
||||
/// Global signal index of this message's first signal.
|
||||
base_signal_i: u32,
|
||||
/// Absolute file offset of this message's first signal's data block.
|
||||
first_data_offset: u32,
|
||||
}
|
||||
|
||||
impl<'a> MsgPlan<'a> {
|
||||
#[inline]
|
||||
fn meta_bytes(&self) -> usize {
|
||||
self.msg.signals.len() * METADATA_SIZE as usize
|
||||
}
|
||||
#[inline]
|
||||
fn data_bytes(&self) -> usize {
|
||||
self.msg.signals.len() * self.data_length * 4
|
||||
}
|
||||
}
|
||||
|
||||
/// Walk `msgs` in order, assign per-message data offsets and starting signal
|
||||
/// indices. Messages with no recorded samples are skipped — they contribute
|
||||
/// nothing to the file.
|
||||
fn plan_layout<'a>(
|
||||
msgs: &'a [MessageMetadata],
|
||||
data: &'a HashMap<CanId, Vec<CanFrame>>,
|
||||
metadata_total_count: u32,
|
||||
last_timestamp: f64,
|
||||
) -> Vec<MsgPlan<'a>> {
|
||||
let mut plans = Vec::with_capacity(msgs.len());
|
||||
let mut signal_i: u32 = 0;
|
||||
let mut data_off = METADATA_OFFSET + METADATA_SIZE * metadata_total_count;
|
||||
for msg in msgs {
|
||||
let samples = match data.get(&msg.id) {
|
||||
Some(s) => s.as_slice(),
|
||||
None => continue,
|
||||
};
|
||||
let data_length = (last_timestamp * msg.frequency as f64) as usize;
|
||||
let plan = MsgPlan {
|
||||
msg,
|
||||
samples,
|
||||
data_length,
|
||||
base_signal_i: signal_i,
|
||||
first_data_offset: data_off,
|
||||
};
|
||||
signal_i += msg.signals.len() as u32;
|
||||
data_off += plan.data_bytes() as u32;
|
||||
plans.push(plan);
|
||||
}
|
||||
plans
|
||||
}
|
||||
|
||||
/// Resample, quantize, and emit meta records for one message. `meta_dst` and
|
||||
/// `data_dst` are exact-sized disjoint slices of the final output, so this
|
||||
/// can run in parallel across messages with no locking.
|
||||
fn process_message(plan: &MsgPlan, last_timestamp: f64, meta_dst: &mut [u8], data_dst: &mut [u8]) {
|
||||
let nsig = plan.msg.signals.len();
|
||||
let data_length = plan.data_length;
|
||||
let signals = &plan.msg.signals;
|
||||
let frequency = plan.msg.frequency;
|
||||
|
||||
let grid = resample(
|
||||
plan.samples,
|
||||
signals,
|
||||
frequency,
|
||||
last_timestamp,
|
||||
data_length,
|
||||
);
|
||||
|
||||
let row_bytes = data_length * 4;
|
||||
let sample_count = data_length as u32;
|
||||
for j in 0..nsig {
|
||||
let signal_i = plan.base_signal_i + j as u32;
|
||||
let prev_offset = if signal_i == 0 {
|
||||
0
|
||||
} else {
|
||||
METADATA_OFFSET + (signal_i - 1) * METADATA_SIZE
|
||||
};
|
||||
let next_offset = METADATA_OFFSET + (signal_i + 1) * METADATA_SIZE;
|
||||
let data_offset = plan.first_data_offset + (j as u32) * row_bytes as u32;
|
||||
|
||||
let grid_row = &grid[j * data_length..(j + 1) * data_length];
|
||||
let data_slot = &mut data_dst[j * row_bytes..(j + 1) * row_bytes];
|
||||
quantize_signal_into(grid_row, &signals[j], data_slot);
|
||||
|
||||
let meta_start = j * METADATA_SIZE as usize;
|
||||
let meta_slot = &mut meta_dst[meta_start..meta_start + METADATA_SIZE as usize];
|
||||
write_meta_record(
|
||||
meta_slot,
|
||||
prev_offset,
|
||||
next_offset,
|
||||
data_offset,
|
||||
sample_count,
|
||||
signal_i,
|
||||
frequency,
|
||||
&signals[j],
|
||||
);
|
||||
}
|
||||
}
|
||||
|
||||
/// Carve a pre-allocated buffer into per-plan disjoint mutable slices. Lengths
|
||||
/// come from `len_of(plan)`; the slices appear in `plans` order.
|
||||
fn split_by_plan<'b, 'a: 'b, F>(
|
||||
buf: &'b mut [u8],
|
||||
plans: &[MsgPlan<'a>],
|
||||
len_of: F,
|
||||
) -> Vec<&'b mut [u8]>
|
||||
where
|
||||
F: Fn(&MsgPlan<'a>) -> usize,
|
||||
{
|
||||
let mut slices = Vec::with_capacity(plans.len());
|
||||
let mut rest = buf;
|
||||
for p in plans {
|
||||
let (head, tail) = rest.split_at_mut(len_of(p));
|
||||
slices.push(head);
|
||||
rest = tail;
|
||||
}
|
||||
slices
|
||||
}
|
||||
|
||||
fn write_ldx(outpath: &Path, last_timestamp: f64) -> std::io::Result<()> {
|
||||
let mut file = OpenOptions::new()
|
||||
.create(true)
|
||||
.write(true)
|
||||
.truncate(true)
|
||||
.open(outpath)?;
|
||||
|
||||
let minutes = (last_timestamp as u64) / 60;
|
||||
let seconds = last_timestamp - (minutes as f64 * 60.0);
|
||||
let fastest_time = format!("{}:{}", minutes, seconds);
|
||||
|
||||
write!(file,
|
||||
"<?xml version=\"1.0\"?>
|
||||
<LDXFile Locale=\"English_United States.1252\" DefaultLocale=\"C\" Version=\"1.6\">
|
||||
<Layers>
|
||||
<Layer>
|
||||
<MarkerBlock>
|
||||
<MarkerGroup Name=\"Beacons\" Index=\"3\"/>
|
||||
</MarkerBlock>
|
||||
<RangeBlock/>
|
||||
</Layer>
|
||||
<Details>
|
||||
<String Id=\"Total Laps\" Value=\"1\"/>
|
||||
<String Id=\"Fastest Time\" Value=\"{}\"/>
|
||||
<String Id=\"Fastest Lap\" Value=\"0\"/>
|
||||
</Details>
|
||||
</Layers>
|
||||
</LDXFile>
|
||||
", fastest_time)?;
|
||||
|
||||
Ok(())
|
||||
}
|
||||
|
||||
pub fn write(outfile: &Path, msgs: &[MessageMetadata], data: HashMap<CanId, Vec<CanFrame>>) {
|
||||
let metadata_total_count = get_metadata_total_count(msgs, &data) as u32;
|
||||
|
||||
// Samples per CAN id are already time-sorted, so `last()` is the per-channel max.
|
||||
let last_timestamp: f64 = data
|
||||
.par_values()
|
||||
.filter_map(|v| v.last())
|
||||
.map(|f| f.timestamp)
|
||||
.reduce(|| 0.0f64, f64::max);
|
||||
|
||||
let plans = plan_layout(msgs, &data, metadata_total_count, last_timestamp);
|
||||
|
||||
let meta_total = METADATA_SIZE as usize * metadata_total_count as usize;
|
||||
let data_total: usize = plans.iter().map(|p| p.data_bytes()).sum();
|
||||
|
||||
let file = OpenOptions::new()
|
||||
.read(true)
|
||||
.write(true)
|
||||
.create(true)
|
||||
.truncate(true)
|
||||
.open(outfile)
|
||||
.unwrap();
|
||||
file.set_len((HEADER_SIZE + meta_total + data_total) as u64)
|
||||
.unwrap();
|
||||
|
||||
let mut mmap = unsafe { MmapMut::map_mut(&file).unwrap() };
|
||||
|
||||
write_header(&mut mmap, metadata_total_count);
|
||||
|
||||
let tailbuff = &mut mmap[HEADER_SIZE..];
|
||||
|
||||
let (meta_region, data_region) = tailbuff.split_at_mut(meta_total);
|
||||
|
||||
let meta_slices = split_by_plan(meta_region, &plans, |p| p.meta_bytes());
|
||||
let data_slices = split_by_plan(data_region, &plans, |p| p.data_bytes());
|
||||
|
||||
plans
|
||||
.par_iter()
|
||||
.zip(meta_slices.into_par_iter())
|
||||
.zip(data_slices.into_par_iter())
|
||||
.for_each(|((plan, meta), data)| {
|
||||
process_message(plan, last_timestamp, meta, data);
|
||||
});
|
||||
|
||||
write_ldx(&outfile.with_extension("ldx"), last_timestamp).unwrap();
|
||||
}
|
||||
Reference in New Issue
Block a user