Skip to content
37 changes: 34 additions & 3 deletions src/daq_log_parse/correlate.rs
Original file line number Diff line number Diff line change
Expand Up @@ -3,6 +3,11 @@ use chrono::TimeZone as _;

use crate::daq_log_parse::parse::ParsedMessage;

// In that case we use this fallback slope value. It is generally safe to assume a
// slope of 1.0 since the DAQ runs at 1 tick = 1 ms, but in post-processing there
// is no reason not to run a proper linear regression when multiple GPS points exist.
const REGRESSION_FALLBACK_SLOPE: f64 = 1.0;

pub struct CorrelationFunction {
/// real_time ~= slope * log_time_ms + intercept_ms
///
Expand Down Expand Up @@ -149,6 +154,7 @@ pub fn time_correlate_chunk(chunk: Vec<ParsedMessage>) -> CorrelationChunkResult

if gps_points.is_empty() {
// No GPS points found, can't correlate
log::warn!("No GPS points found in chunk, cannot correlate timestamps");
return CorrelationChunkResult::uncorrelated_new(chunk);
}

Expand All @@ -163,7 +169,10 @@ pub fn time_correlate_chunk(chunk: Vec<ParsedMessage>) -> CorrelationChunkResult
let (slope, intercept) = match linear_regression(&points) {
Some(v) => v,
None => {
log::error!("Failed to refit correlation line");
log::error!(
"Failed to refit correlation line. Points count: {}",
points.len()
);
return CorrelationChunkResult::uncorrelated_new(chunk);
}
Comment thread
LelsersLasers marked this conversation as resolved.
};
Expand All @@ -190,6 +199,15 @@ pub fn time_correlate_chunk(chunk: Vec<ParsedMessage>) -> CorrelationChunkResult
points.len()
);

// Log error (but continue) if slope is not ~REGRESSION_FALLBACK_SLOPE
if (slope - REGRESSION_FALLBACK_SLOPE).abs() > 0.001 {
log::error!(
"GPS correlation slope ({:.9}) deviates from expected {:.3}.",
slope,
REGRESSION_FALLBACK_SLOPE,
);
}

CorrelationChunkResult::correlated_new(
chunk,
CorrelationFunction {
Expand All @@ -206,14 +224,27 @@ struct Point {

/// Least squares linear regression.
///
/// Fits:
/// Note: if there is only 1 point, uses REGRESSION_FALLBACK_SLOPE
///
/// Fits:
/// y = slope * x + intercept
fn linear_regression(points: &[Point]) -> Option<(f64, f64)> {
if points.len() < 2 {
if points.is_empty() {
return None;
}

// Fallback for single point: use a default slope and calculate intercept based on that point
if points.len() == 1 {
log::warn!(
"Only one GPS point found, using fallback slope of {}",
REGRESSION_FALLBACK_SLOPE
);
let p = &points[0];
let slope = REGRESSION_FALLBACK_SLOPE;
let intercept = p.y - slope * p.x;
return Some((slope, intercept));
}

let n = points.len() as f64;

let mut sum_x = 0.0;
Expand Down
Loading