445 lines
14 KiB
Rust
445 lines
14 KiB
Rust
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]
|
|
pub 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();
|
|
}
|