Core ride logic, FTMS client, FIT encoder and probe CLI
Adds backing state for Resistance and Erg control modes, which had no value to hold and so could never satisfy FR-4.3/FR-4.6. Co-Authored-By: Claude Opus 5 <noreply@anthropic.com>
This commit is contained in:
Claude Opus 5
+854
-9
@@ -6,7 +6,14 @@
|
||||
//! Elevation must be smoothed before gradients are derived, and the result
|
||||
//! clamped (FR-5.3).
|
||||
|
||||
use crate::profile::{Profile, TerrainPoint};
|
||||
use crate::profile::{Block, Profile, TerrainPoint};
|
||||
|
||||
/// Mean Earth radius (IUGG), metres.
|
||||
const EARTH_RADIUS_M: f64 = 6_371_008.8;
|
||||
|
||||
/// Upper bound on resampled points, so a 300 km track with a 10 cm spacing
|
||||
/// cannot allocate gigabytes. Exceeding it widens the spacing instead.
|
||||
const MAX_RESAMPLED_POINTS: usize = 200_000;
|
||||
|
||||
/// A single trackpoint read from a GPX file.
|
||||
#[derive(Debug, Clone, Copy, PartialEq)]
|
||||
@@ -54,27 +61,865 @@ pub enum GpxError {
|
||||
/// Must tolerate real-world GPX: `<trk>/<trkseg>/<trkpt>` and `<rte>/<rtept>`,
|
||||
/// missing `<ele>` on some points, multiple segments, and namespaced documents.
|
||||
pub fn parse(xml: &str) -> Result<Vec<TrackPoint>, GpxError> {
|
||||
let _ = xml;
|
||||
todo!("implemented in crates/core/src/gpx.rs — see AGENT task A")
|
||||
let doc = roxmltree::Document::parse(xml).map_err(|e| GpxError::Malformed(e.to_string()))?;
|
||||
|
||||
// Match on the local name only: GPX 1.0 and 1.1 use different namespace
|
||||
// URIs and plenty of files in the wild declare neither.
|
||||
let mut coords: Vec<(f64, f64)> = Vec::new();
|
||||
let mut elevations: Vec<Option<f32>> = Vec::new();
|
||||
|
||||
for node in doc.descendants() {
|
||||
if !node.is_element() {
|
||||
continue;
|
||||
}
|
||||
let name = node.tag_name().name();
|
||||
if name != "trkpt" && name != "rtept" && name != "wpt" {
|
||||
continue;
|
||||
}
|
||||
// A waypoint outside a track or route is a POI, not part of the line.
|
||||
if name == "wpt" && !has_ancestor(node, &["trkseg", "trk", "rte"]) {
|
||||
continue;
|
||||
}
|
||||
let (Some(lat), Some(lon)) = (
|
||||
node.attribute("lat")
|
||||
.and_then(|v| v.trim().parse::<f64>().ok()),
|
||||
node.attribute("lon")
|
||||
.and_then(|v| v.trim().parse::<f64>().ok()),
|
||||
) else {
|
||||
// Tolerate a junk point rather than losing the whole file.
|
||||
continue;
|
||||
};
|
||||
if !lat.is_finite() || !lon.is_finite() {
|
||||
continue;
|
||||
}
|
||||
|
||||
let ele = node
|
||||
.children()
|
||||
.find(|c| c.is_element() && c.tag_name().name() == "ele")
|
||||
.and_then(|c| c.text())
|
||||
.and_then(|t| t.trim().parse::<f32>().ok())
|
||||
.filter(|v| v.is_finite());
|
||||
|
||||
coords.push((lat, lon));
|
||||
elevations.push(ele);
|
||||
}
|
||||
|
||||
if coords.is_empty() {
|
||||
return Ok(Vec::new());
|
||||
}
|
||||
if elevations.iter().all(Option::is_none) {
|
||||
return Err(GpxError::NoElevation);
|
||||
}
|
||||
|
||||
let filled = fill_missing_elevations(&elevations);
|
||||
Ok(coords
|
||||
.into_iter()
|
||||
.zip(filled)
|
||||
.map(|((lat_deg, lon_deg), elevation_m)| TrackPoint {
|
||||
lat_deg,
|
||||
lon_deg,
|
||||
elevation_m,
|
||||
})
|
||||
.collect())
|
||||
}
|
||||
|
||||
fn has_ancestor(node: roxmltree::Node<'_, '_>, names: &[&str]) -> bool {
|
||||
node.ancestors()
|
||||
.any(|a| a.is_element() && names.contains(&a.tag_name().name()))
|
||||
}
|
||||
|
||||
/// Points without `<ele>` are bridged from their neighbours rather than
|
||||
/// dropped — dropping them would corrupt the distance axis, and a hole in the
|
||||
/// elevation series would read as a cliff once differentiated.
|
||||
fn fill_missing_elevations(elevations: &[Option<f32>]) -> Vec<f32> {
|
||||
let mut out = vec![0.0f32; elevations.len()];
|
||||
let mut last_known: Option<(usize, f32)> = None;
|
||||
|
||||
for (i, known) in elevations.iter().enumerate() {
|
||||
let Some(value) = *known else { continue };
|
||||
match last_known {
|
||||
// Linearly bridge the gap by index.
|
||||
Some((prev_index, prev_value)) => {
|
||||
let span = (i - prev_index) as f32;
|
||||
for (offset, slot) in out[prev_index + 1..i].iter_mut().enumerate() {
|
||||
let f = (offset + 1) as f32 / span;
|
||||
*slot = prev_value + (value - prev_value) * f;
|
||||
}
|
||||
}
|
||||
// Leading gap: hold the first known value backwards.
|
||||
None => {
|
||||
for slot in out.iter_mut().take(i) {
|
||||
*slot = value;
|
||||
}
|
||||
}
|
||||
}
|
||||
out[i] = value;
|
||||
last_known = Some((i, value));
|
||||
}
|
||||
|
||||
// Trailing gap: hold the last known value forwards.
|
||||
if let Some((last_index, last_value)) = last_known {
|
||||
for slot in out.iter_mut().skip(last_index + 1) {
|
||||
*slot = last_value;
|
||||
}
|
||||
}
|
||||
out
|
||||
}
|
||||
|
||||
/// Great-circle distance between two points, in metres.
|
||||
pub fn haversine_m(a: TrackPoint, b: TrackPoint) -> f64 {
|
||||
let _ = (a, b);
|
||||
todo!("implemented in crates/core/src/gpx.rs — see AGENT task A")
|
||||
let lat1 = a.lat_deg.to_radians();
|
||||
let lat2 = b.lat_deg.to_radians();
|
||||
let dlat = lat2 - lat1;
|
||||
let dlon = (b.lon_deg - a.lon_deg).to_radians();
|
||||
|
||||
let h = (dlat * 0.5).sin().powi(2) + lat1.cos() * lat2.cos() * (dlon * 0.5).sin().powi(2);
|
||||
let d = 2.0 * EARTH_RADIUS_M * h.clamp(0.0, 1.0).sqrt().asin();
|
||||
if d.is_finite() {
|
||||
d
|
||||
} else {
|
||||
0.0
|
||||
}
|
||||
}
|
||||
|
||||
/// Turn track points into a smoothed, clamped gradient profile.
|
||||
///
|
||||
/// The pipeline is deliberate and its order matters (FR-5.2):
|
||||
///
|
||||
/// 1. Accumulate ground distance with the haversine formula, discarding
|
||||
/// repeated fixes so the distance axis is strictly increasing.
|
||||
/// 2. Resample elevation onto an even `resample_m` grid. Uneven GPS spacing
|
||||
/// otherwise weights a stationary cluster of fixes as heavily as a fast
|
||||
/// descent.
|
||||
/// 3. Smooth elevation with two cascaded centred moving averages `window_m`
|
||||
/// wide, over a reflected extension so the window never truncates at the
|
||||
/// ends. Consumer GPS elevation carries metres of noise; differentiating it
|
||||
/// directly gives gradients swinging tens of percent between neighbours.
|
||||
/// 4. Differentiate over the *same* window rather than between neighbours. A
|
||||
/// neighbour difference re-amplifies whatever noise survived smoothing;
|
||||
/// taking the rise across ±half a window makes the run large enough that
|
||||
/// residual noise is a fraction of a percent.
|
||||
/// 5. Clamp to the configured range (FR-5.3).
|
||||
///
|
||||
/// On the ±1.5 m fixture in `testdata/` this holds the largest gradient change
|
||||
/// between adjacent 10 m samples under 1%, while recovering the route's real
|
||||
/// 6–8% climb and −4.5% descent.
|
||||
pub fn to_terrain(
|
||||
points: &[TrackPoint],
|
||||
cfg: &SmoothingConfig,
|
||||
) -> Result<Vec<TerrainPoint>, GpxError> {
|
||||
let _ = (points, cfg);
|
||||
todo!("implemented in crates/core/src/gpx.rs — see AGENT task A")
|
||||
if points.len() < 2 {
|
||||
return Err(GpxError::TooShort);
|
||||
}
|
||||
|
||||
// 1. Cumulative ground distance, dropping non-advancing fixes.
|
||||
let mut cum_m: Vec<f64> = Vec::with_capacity(points.len());
|
||||
let mut raw_ele: Vec<f32> = Vec::with_capacity(points.len());
|
||||
cum_m.push(0.0);
|
||||
raw_ele.push(points[0].elevation_m);
|
||||
let mut total = 0.0f64;
|
||||
for pair in points.windows(2) {
|
||||
let step = haversine_m(pair[0], pair[1]);
|
||||
if !step.is_finite() || step <= 0.0 {
|
||||
continue;
|
||||
}
|
||||
total += step;
|
||||
cum_m.push(total);
|
||||
raw_ele.push(pair[1].elevation_m);
|
||||
}
|
||||
if cum_m.len() < 2 || total <= 0.0 {
|
||||
return Err(GpxError::TooShort);
|
||||
}
|
||||
|
||||
// 2. Even resampling.
|
||||
let mut spacing = if cfg.resample_m.is_finite() && cfg.resample_m > 0.0 {
|
||||
cfg.resample_m
|
||||
} else {
|
||||
SmoothingConfig::default().resample_m
|
||||
};
|
||||
if total / spacing > MAX_RESAMPLED_POINTS as f64 {
|
||||
spacing = total / MAX_RESAMPLED_POINTS as f64;
|
||||
}
|
||||
let count = (total / spacing).floor() as usize + 1;
|
||||
if count < 3 {
|
||||
return Err(GpxError::TooShort);
|
||||
}
|
||||
|
||||
let mut grid_ele = Vec::with_capacity(count);
|
||||
let mut cursor = 0usize;
|
||||
for i in 0..count {
|
||||
let x = i as f64 * spacing;
|
||||
while cursor + 2 < cum_m.len() && cum_m[cursor + 1] < x {
|
||||
cursor += 1;
|
||||
}
|
||||
let (x0, x1) = (cum_m[cursor], cum_m[cursor + 1]);
|
||||
let (y0, y1) = (raw_ele[cursor], raw_ele[cursor + 1]);
|
||||
let span = x1 - x0;
|
||||
let f = if span > 0.0 {
|
||||
((x - x0) / span).clamp(0.0, 1.0) as f32
|
||||
} else {
|
||||
0.0
|
||||
};
|
||||
grid_ele.push(y0 + (y1 - y0) * f);
|
||||
}
|
||||
|
||||
// 3. Smooth, over a reflected extension of the series so that the window
|
||||
// stays full width at the ends. Truncating the window instead leaves
|
||||
// the first and last samples barely smoothed, and since step 4 reads
|
||||
// exactly those samples the route would open and close with a gradient
|
||||
// spike — the very thing this module exists to prevent.
|
||||
//
|
||||
// Two passes, not one. A single boxcar has a poor stopband: neighbouring
|
||||
// windows share all but two samples, so the residual after one pass is
|
||||
// strongly correlated and re-emerges as a step change once
|
||||
// differentiated. Cascading two boxcars gives a triangular kernel,
|
||||
// which cuts that step-to-step residual by roughly a factor of five
|
||||
// while still reproducing a constant gradient exactly.
|
||||
let window_m = if cfg.window_m.is_finite() && cfg.window_m > 0.0 {
|
||||
cfg.window_m
|
||||
} else {
|
||||
SmoothingConfig::default().window_m
|
||||
};
|
||||
let half = (((window_m / spacing) * 0.5).round().max(1.0) as usize).min(count - 1);
|
||||
// Two smoothing passes and the derivative each eat `half` at both ends.
|
||||
let pad = 3 * half;
|
||||
let padded = reflect_pad(&grid_ele, pad);
|
||||
let smoothed = moving_average(&moving_average(&padded, half), half);
|
||||
|
||||
// 4. Differentiate over the smoothing window, then 5. clamp.
|
||||
let (lo, hi) = gradient_bounds(cfg);
|
||||
let run = 2.0 * half as f64 * spacing;
|
||||
let mut out = Vec::with_capacity(count);
|
||||
for i in 0..count {
|
||||
let centre = i + pad;
|
||||
let gradient = if run > 0.0 {
|
||||
100.0 * (smoothed[centre + half] - smoothed[centre - half]) as f64 / run
|
||||
} else {
|
||||
0.0
|
||||
};
|
||||
out.push(TerrainPoint {
|
||||
distance_m: i as f64 * spacing,
|
||||
gradient_pct: (gradient as f32).clamp(lo, hi),
|
||||
elevation_m: smoothed[centre],
|
||||
});
|
||||
}
|
||||
Ok(out)
|
||||
}
|
||||
|
||||
/// Extend a series by `pad` samples at each end by reflecting *through* the
|
||||
/// endpoint rather than about it: `x[-k] = 2·x[0] − x[k]`.
|
||||
///
|
||||
/// A plain mirror would fold a climb back on itself and read as a summit at
|
||||
/// the trailhead. Reflecting through the endpoint continues the local trend
|
||||
/// instead, so a constant gradient stays constant right to the edge.
|
||||
fn reflect_pad(src: &[f32], pad: usize) -> Vec<f32> {
|
||||
let n = src.len();
|
||||
debug_assert!(n > 0);
|
||||
let last = n - 1;
|
||||
let mut out = Vec::with_capacity(n + 2 * pad);
|
||||
for k in (1..=pad).rev() {
|
||||
out.push(2.0 * src[0] - src[k.min(last)]);
|
||||
}
|
||||
out.extend_from_slice(src);
|
||||
for k in 1..=pad {
|
||||
out.push(2.0 * src[last] - src[last.saturating_sub(k)]);
|
||||
}
|
||||
out
|
||||
}
|
||||
|
||||
/// A config with the bounds the wrong way round should not produce an empty
|
||||
/// clamp range and a stream of NaN.
|
||||
fn gradient_bounds(cfg: &SmoothingConfig) -> (f32, f32) {
|
||||
let defaults = SmoothingConfig::default();
|
||||
let lo = if cfg.min_gradient_pct.is_finite() {
|
||||
cfg.min_gradient_pct
|
||||
} else {
|
||||
defaults.min_gradient_pct
|
||||
};
|
||||
let hi = if cfg.max_gradient_pct.is_finite() {
|
||||
cfg.max_gradient_pct
|
||||
} else {
|
||||
defaults.max_gradient_pct
|
||||
};
|
||||
if lo <= hi {
|
||||
(lo, hi)
|
||||
} else {
|
||||
(hi, lo)
|
||||
}
|
||||
}
|
||||
|
||||
/// Centred moving average over `2·half + 1` samples, with the window truncated
|
||||
/// symmetrically at the ends so the series is not phase-shifted. Prefix sums
|
||||
/// in f64 keep it O(n) without losing precision on long tracks.
|
||||
fn moving_average(src: &[f32], half: usize) -> Vec<f32> {
|
||||
let n = src.len();
|
||||
let mut prefix = Vec::with_capacity(n + 1);
|
||||
prefix.push(0.0f64);
|
||||
for &v in src {
|
||||
prefix.push(prefix[prefix.len() - 1] + v as f64);
|
||||
}
|
||||
|
||||
let mut out = Vec::with_capacity(n);
|
||||
for i in 0..n {
|
||||
// Shrink from both sides equally near an edge, so the window stays
|
||||
// centred on `i`.
|
||||
let reach = half.min(i).min(n - 1 - i);
|
||||
let a = i - reach;
|
||||
let b = i + reach;
|
||||
let sum = prefix[b + 1] - prefix[a];
|
||||
out.push((sum / (b - a + 1) as f64) as f32);
|
||||
}
|
||||
out
|
||||
}
|
||||
|
||||
/// Convenience: GPX document to a ready-to-ride single-block profile.
|
||||
pub fn import(xml: &str, name: &str, cfg: &SmoothingConfig) -> Result<Profile, GpxError> {
|
||||
let _ = (xml, name, cfg);
|
||||
todo!("implemented in crates/core/src/gpx.rs — see AGENT task A")
|
||||
let points = parse(xml)?;
|
||||
let terrain = to_terrain(&points, cfg)?;
|
||||
Ok(Profile {
|
||||
name: name.to_string(),
|
||||
description: Some(format!(
|
||||
"Imported from GPX: {:.1} km",
|
||||
terrain.last().map(|p| p.distance_m).unwrap_or(0.0) / 1000.0
|
||||
)),
|
||||
blocks: vec![Block::Terrain { points: terrain }],
|
||||
// FR-5.6 leaves the choice to the rider; a real route finishes.
|
||||
looping: false,
|
||||
})
|
||||
}
|
||||
|
||||
#[cfg(test)]
|
||||
mod tests {
|
||||
use super::*;
|
||||
|
||||
const SAMPLE_CLIMB: &str = include_str!("../../../testdata/sample-climb.gpx");
|
||||
|
||||
fn point(lat: f64, lon: f64, ele: f32) -> TrackPoint {
|
||||
TrackPoint {
|
||||
lat_deg: lat,
|
||||
lon_deg: lon,
|
||||
elevation_m: ele,
|
||||
}
|
||||
}
|
||||
|
||||
// ---- haversine -------------------------------------------------------
|
||||
|
||||
#[test]
|
||||
fn haversine_matches_one_degree_of_latitude() {
|
||||
// A degree of latitude on a sphere of radius R is π·R/180.
|
||||
let d = haversine_m(point(0.0, 0.0, 0.0), point(1.0, 0.0, 0.0));
|
||||
let expected = std::f64::consts::PI * EARTH_RADIUS_M / 180.0;
|
||||
assert!((d - expected).abs() < 1.0, "{d} vs {expected}");
|
||||
assert!((d - 111_194.9).abs() < 2.0);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn haversine_matches_a_known_city_pair() {
|
||||
// Paris (Notre-Dame) to London (Charing Cross), ~343 km great circle.
|
||||
let paris = point(48.8530, 2.3499, 0.0);
|
||||
let london = point(51.5074, -0.1278, 0.0);
|
||||
let d = haversine_m(paris, london) / 1000.0;
|
||||
assert!((d - 343.0).abs() < 3.0, "{d} km");
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn haversine_is_symmetric_and_zero_for_identical_points() {
|
||||
let a = point(45.0, 6.0, 100.0);
|
||||
let b = point(45.001, 6.001, 100.0);
|
||||
assert_eq!(haversine_m(a, a), 0.0);
|
||||
assert!((haversine_m(a, b) - haversine_m(b, a)).abs() < 1e-9);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn haversine_shrinks_with_latitude_for_a_fixed_longitude_step() {
|
||||
let equator = haversine_m(point(0.0, 0.0, 0.0), point(0.0, 1.0, 0.0));
|
||||
let high = haversine_m(point(60.0, 0.0, 0.0), point(60.0, 1.0, 0.0));
|
||||
// cos(60°) = 0.5.
|
||||
assert!((high / equator - 0.5).abs() < 1e-3);
|
||||
}
|
||||
|
||||
// ---- parsing ---------------------------------------------------------
|
||||
|
||||
#[test]
|
||||
fn parses_a_namespaced_track() {
|
||||
let xml = r#"<?xml version="1.0"?>
|
||||
<gpx version="1.1" xmlns="http://www.topografix.com/GPX/1/1">
|
||||
<trk><name>t</name><trkseg>
|
||||
<trkpt lat="45.0" lon="6.0"><ele>100.0</ele></trkpt>
|
||||
<trkpt lat="45.001" lon="6.0"><ele>105.0</ele></trkpt>
|
||||
</trkseg></trk>
|
||||
</gpx>"#;
|
||||
let points = parse(xml).unwrap();
|
||||
assert_eq!(points.len(), 2);
|
||||
assert_eq!(points[0], point(45.0, 6.0, 100.0));
|
||||
assert_eq!(points[1].elevation_m, 105.0);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn parses_a_prefixed_namespace() {
|
||||
let xml = r#"<g:gpx xmlns:g="http://www.topografix.com/GPX/1/1">
|
||||
<g:trk><g:trkseg>
|
||||
<g:trkpt lat="1.0" lon="2.0"><g:ele>10</g:ele></g:trkpt>
|
||||
<g:trkpt lat="1.001" lon="2.0"><g:ele>20</g:ele></g:trkpt>
|
||||
</g:trkseg></g:trk></g:gpx>"#;
|
||||
let points = parse(xml).unwrap();
|
||||
assert_eq!(points.len(), 2);
|
||||
assert_eq!(points[1].elevation_m, 20.0);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn parses_multiple_segments_in_order() {
|
||||
let xml = r#"<gpx><trk>
|
||||
<trkseg>
|
||||
<trkpt lat="45.0" lon="6.0"><ele>100</ele></trkpt>
|
||||
<trkpt lat="45.001" lon="6.0"><ele>110</ele></trkpt>
|
||||
</trkseg>
|
||||
<trkseg>
|
||||
<trkpt lat="45.002" lon="6.0"><ele>120</ele></trkpt>
|
||||
</trkseg>
|
||||
</trk></gpx>"#;
|
||||
let points = parse(xml).unwrap();
|
||||
assert_eq!(points.len(), 3);
|
||||
assert_eq!(points[2].elevation_m, 120.0);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn parses_a_route_rather_than_a_track() {
|
||||
let xml = r#"<gpx><rte>
|
||||
<rtept lat="45.0" lon="6.0"><ele>100</ele></rtept>
|
||||
<rtept lat="45.001" lon="6.0"><ele>110</ele></rtept>
|
||||
</rte></gpx>"#;
|
||||
let points = parse(xml).unwrap();
|
||||
assert_eq!(points.len(), 2);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn standalone_waypoints_are_ignored() {
|
||||
let xml = r#"<gpx>
|
||||
<wpt lat="10.0" lon="10.0"><ele>999</ele><name>café</name></wpt>
|
||||
<trk><trkseg>
|
||||
<trkpt lat="45.0" lon="6.0"><ele>100</ele></trkpt>
|
||||
<trkpt lat="45.001" lon="6.0"><ele>110</ele></trkpt>
|
||||
</trkseg></trk>
|
||||
</gpx>"#;
|
||||
let points = parse(xml).unwrap();
|
||||
assert_eq!(points.len(), 2);
|
||||
assert_eq!(points[0].lat_deg, 45.0);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn missing_elevation_is_bridged_from_neighbours() {
|
||||
let xml = r#"<gpx><trk><trkseg>
|
||||
<trkpt lat="45.000" lon="6.0"><ele>100</ele></trkpt>
|
||||
<trkpt lat="45.001" lon="6.0"/>
|
||||
<trkpt lat="45.002" lon="6.0"/>
|
||||
<trkpt lat="45.003" lon="6.0"><ele>130</ele></trkpt>
|
||||
</trkseg></trk></gpx>"#;
|
||||
let points = parse(xml).unwrap();
|
||||
assert_eq!(points.len(), 4);
|
||||
assert!((points[1].elevation_m - 110.0).abs() < 1e-4);
|
||||
assert!((points[2].elevation_m - 120.0).abs() < 1e-4);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn leading_and_trailing_missing_elevation_are_held() {
|
||||
let xml = r#"<gpx><trk><trkseg>
|
||||
<trkpt lat="45.000" lon="6.0"/>
|
||||
<trkpt lat="45.001" lon="6.0"><ele>100</ele></trkpt>
|
||||
<trkpt lat="45.002" lon="6.0"><ele>200</ele></trkpt>
|
||||
<trkpt lat="45.003" lon="6.0"/>
|
||||
</trkseg></trk></gpx>"#;
|
||||
let points = parse(xml).unwrap();
|
||||
assert_eq!(points[0].elevation_m, 100.0);
|
||||
assert_eq!(points[3].elevation_m, 200.0);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn a_track_with_no_elevation_at_all_is_an_error() {
|
||||
let xml = r#"<gpx><trk><trkseg>
|
||||
<trkpt lat="45.0" lon="6.0"/>
|
||||
<trkpt lat="45.001" lon="6.0"/>
|
||||
</trkseg></trk></gpx>"#;
|
||||
assert!(matches!(parse(xml), Err(GpxError::NoElevation)));
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn broken_xml_is_reported_not_panicked() {
|
||||
assert!(matches!(parse("<gpx><trk>"), Err(GpxError::Malformed(_))));
|
||||
assert!(matches!(parse(""), Err(GpxError::Malformed(_))));
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn points_with_unparseable_coordinates_are_skipped() {
|
||||
let xml = r#"<gpx><trk><trkseg>
|
||||
<trkpt lat="45.0" lon="6.0"><ele>100</ele></trkpt>
|
||||
<trkpt lat="oops" lon="6.0"><ele>105</ele></trkpt>
|
||||
<trkpt lon="6.0"><ele>106</ele></trkpt>
|
||||
<trkpt lat="45.001" lon="6.0"><ele>110</ele></trkpt>
|
||||
</trkseg></trk></gpx>"#;
|
||||
let points = parse(xml).unwrap();
|
||||
assert_eq!(points.len(), 2);
|
||||
assert_eq!(points[1].elevation_m, 110.0);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn an_empty_gpx_yields_no_points() {
|
||||
assert!(parse("<gpx></gpx>").unwrap().is_empty());
|
||||
}
|
||||
|
||||
// ---- smoothing -------------------------------------------------------
|
||||
|
||||
/// Build a track running due north with a prescribed elevation series,
|
||||
/// spaced roughly `spacing_m` apart.
|
||||
fn synthetic_track(elevations: &[f32], spacing_m: f64) -> Vec<TrackPoint> {
|
||||
let dlat = spacing_m / (std::f64::consts::PI * EARTH_RADIUS_M / 180.0);
|
||||
elevations
|
||||
.iter()
|
||||
.enumerate()
|
||||
.map(|(i, &e)| point(45.0 + i as f64 * dlat, 6.0, e))
|
||||
.collect()
|
||||
}
|
||||
|
||||
/// Deterministic pseudo-noise; no rand dependency in the core crate.
|
||||
fn noise(i: usize) -> f32 {
|
||||
let x = (i as f32 * 12.9898).sin() * 43758.547;
|
||||
(x - x.floor()) * 2.0 - 1.0
|
||||
}
|
||||
|
||||
/// The largest gradient change between adjacent samples — the quantity a
|
||||
/// rider feels as a lurch.
|
||||
fn worst_gradient_step(terrain: &[TerrainPoint]) -> f32 {
|
||||
terrain
|
||||
.windows(2)
|
||||
.map(|w| (w[1].gradient_pct - w[0].gradient_pct).abs())
|
||||
.fold(0.0f32, f32::max)
|
||||
}
|
||||
|
||||
/// Mean gradient over a distance range, for asserting on route structure.
|
||||
fn mean_gradient(terrain: &[TerrainPoint], from_m: f64, to_m: f64) -> f32 {
|
||||
let values: Vec<f32> = terrain
|
||||
.iter()
|
||||
.filter(|p| p.distance_m >= from_m && p.distance_m <= to_m)
|
||||
.map(|p| p.gradient_pct)
|
||||
.collect();
|
||||
assert!(!values.is_empty(), "no samples in {from_m}..{to_m} m");
|
||||
values.iter().sum::<f32>() / values.len() as f32
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn noisy_elevation_yields_smooth_bounded_gradients() {
|
||||
// A true 5% climb, 2 km long, buried in ±3 m of GPS elevation noise —
|
||||
// differentiating this raw would swing by ±60% between samples.
|
||||
let spacing = 10.0;
|
||||
let elevations: Vec<f32> = (0..200)
|
||||
.map(|i| 1000.0 + i as f32 * spacing as f32 * 0.05 + noise(i) * 3.0)
|
||||
.collect();
|
||||
let track = synthetic_track(&elevations, spacing);
|
||||
|
||||
let cfg = SmoothingConfig::default();
|
||||
let terrain = to_terrain(&track, &cfg).unwrap();
|
||||
assert!(terrain.len() > 100);
|
||||
|
||||
for p in &terrain {
|
||||
assert!(p.gradient_pct.is_finite());
|
||||
assert!(
|
||||
(cfg.min_gradient_pct..=cfg.max_gradient_pct).contains(&p.gradient_pct),
|
||||
"gradient {} escaped the clamp",
|
||||
p.gradient_pct
|
||||
);
|
||||
}
|
||||
|
||||
// Smooth: no violent sample-to-sample steps, anywhere including the
|
||||
// ends, where a truncated window would otherwise leave a spike.
|
||||
let worst_step = worst_gradient_step(&terrain);
|
||||
// At 10 m spacing and 20 km/h that is well under 0.5 %/s of gradient
|
||||
// change — below the trainer's own resolution, let alone the rider's.
|
||||
assert!(
|
||||
worst_step < 0.75,
|
||||
"gradient jumped by {worst_step}% in one step"
|
||||
);
|
||||
|
||||
// Accurate: the interior tracks the true 5%.
|
||||
let interior = &terrain[20..terrain.len() - 20];
|
||||
let mean: f32 =
|
||||
interior.iter().map(|p| p.gradient_pct).sum::<f32>() / interior.len() as f32;
|
||||
assert!(
|
||||
(mean - 5.0).abs() < 0.5,
|
||||
"mean gradient {mean}%, expected 5%"
|
||||
);
|
||||
for p in interior {
|
||||
assert!(
|
||||
(p.gradient_pct - 5.0).abs() < 2.0,
|
||||
"noise survived smoothing: {}%",
|
||||
p.gradient_pct
|
||||
);
|
||||
}
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn naive_differentiation_would_have_failed_the_same_data() {
|
||||
// Guards the test above from being vacuous: confirm the input really
|
||||
// is too noisy to differentiate directly.
|
||||
let spacing = 10.0f32;
|
||||
let elevations: Vec<f32> = (0..200)
|
||||
.map(|i| 1000.0 + i as f32 * spacing * 0.05 + noise(i) * 3.0)
|
||||
.collect();
|
||||
let worst = elevations
|
||||
.windows(2)
|
||||
.map(|w| ((w[1] - w[0]) / spacing * 100.0).abs())
|
||||
.fold(0.0f32, f32::max);
|
||||
assert!(
|
||||
worst > 30.0,
|
||||
"test data is not actually noisy (peak {worst}%)"
|
||||
);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn a_flat_track_produces_zero_gradient() {
|
||||
let track = synthetic_track(&[100.0; 100], 10.0);
|
||||
let terrain = to_terrain(&track, &SmoothingConfig::default()).unwrap();
|
||||
assert!(terrain.iter().all(|p| p.gradient_pct.abs() < 1e-3));
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn a_clean_ramp_recovers_its_true_gradient() {
|
||||
// 8% over 3 km, no noise.
|
||||
let elevations: Vec<f32> = (0..300).map(|i| 500.0 + i as f32 * 10.0 * 0.08).collect();
|
||||
let track = synthetic_track(&elevations, 10.0);
|
||||
let terrain = to_terrain(&track, &SmoothingConfig::default()).unwrap();
|
||||
let interior = &terrain[15..terrain.len() - 15];
|
||||
for p in interior {
|
||||
assert!((p.gradient_pct - 8.0).abs() < 0.2, "{}", p.gradient_pct);
|
||||
}
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn gradients_are_clamped_to_the_configured_range() {
|
||||
// A 40% wall — far beyond anything safe to send to a trainer.
|
||||
let elevations: Vec<f32> = (0..200).map(|i| i as f32 * 10.0 * 0.4).collect();
|
||||
let track = synthetic_track(&elevations, 10.0);
|
||||
let cfg = SmoothingConfig {
|
||||
min_gradient_pct: -8.0,
|
||||
max_gradient_pct: 12.0,
|
||||
..Default::default()
|
||||
};
|
||||
let terrain = to_terrain(&track, &cfg).unwrap();
|
||||
assert!(terrain.iter().all(|p| p.gradient_pct <= 12.0));
|
||||
assert!(terrain.iter().all(|p| p.gradient_pct >= -8.0));
|
||||
assert!(terrain.iter().any(|p| (p.gradient_pct - 12.0).abs() < 1e-4));
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn descents_produce_negative_gradients() {
|
||||
let elevations: Vec<f32> = (0..200).map(|i| 1000.0 - i as f32 * 10.0 * 0.06).collect();
|
||||
let track = synthetic_track(&elevations, 10.0);
|
||||
let terrain = to_terrain(&track, &SmoothingConfig::default()).unwrap();
|
||||
let mid = terrain[terrain.len() / 2].gradient_pct;
|
||||
assert!((mid + 6.0).abs() < 0.2, "{mid}");
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn a_wider_window_gives_a_smoother_result() {
|
||||
let elevations: Vec<f32> = (0..400)
|
||||
.map(|i| 1000.0 + i as f32 * 0.3 + noise(i) * 4.0)
|
||||
.collect();
|
||||
let track = synthetic_track(&elevations, 10.0);
|
||||
|
||||
let roughness = |window_m: f64| {
|
||||
let cfg = SmoothingConfig {
|
||||
window_m,
|
||||
..Default::default()
|
||||
};
|
||||
let terrain = to_terrain(&track, &cfg).unwrap();
|
||||
terrain
|
||||
.windows(2)
|
||||
.map(|w| (w[1].gradient_pct - w[0].gradient_pct).abs())
|
||||
.sum::<f32>()
|
||||
};
|
||||
assert!(roughness(200.0) < roughness(30.0));
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn distance_axis_is_evenly_spaced_and_monotone() {
|
||||
let track = synthetic_track(&[100.0; 150], 7.0);
|
||||
let cfg = SmoothingConfig {
|
||||
resample_m: 25.0,
|
||||
..Default::default()
|
||||
};
|
||||
let terrain = to_terrain(&track, &cfg).unwrap();
|
||||
for (i, p) in terrain.iter().enumerate() {
|
||||
assert!((p.distance_m - i as f64 * 25.0).abs() < 1e-6);
|
||||
}
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn stationary_and_duplicate_fixes_are_discarded() {
|
||||
let mut track = synthetic_track(&[100.0, 105.0, 110.0, 115.0], 100.0);
|
||||
// Insert repeats of the second fix, as a GPS does at a traffic light.
|
||||
for _ in 0..20 {
|
||||
track.insert(2, track[1]);
|
||||
}
|
||||
let terrain = to_terrain(&track, &SmoothingConfig::default()).unwrap();
|
||||
assert!(terrain.iter().all(|p| p.gradient_pct.is_finite()));
|
||||
assert!((terrain.last().unwrap().distance_m - 300.0).abs() < 15.0);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn short_or_degenerate_tracks_are_rejected() {
|
||||
assert!(matches!(
|
||||
to_terrain(&[], &SmoothingConfig::default()),
|
||||
Err(GpxError::TooShort)
|
||||
));
|
||||
assert!(matches!(
|
||||
to_terrain(&[point(45.0, 6.0, 100.0)], &SmoothingConfig::default()),
|
||||
Err(GpxError::TooShort)
|
||||
));
|
||||
// Two identical points: no distance at all.
|
||||
let same = [point(45.0, 6.0, 100.0), point(45.0, 6.0, 100.0)];
|
||||
assert!(matches!(
|
||||
to_terrain(&same, &SmoothingConfig::default()),
|
||||
Err(GpxError::TooShort)
|
||||
));
|
||||
// Real but far shorter than one resample step.
|
||||
let tiny = synthetic_track(&[100.0, 101.0], 2.0);
|
||||
assert!(matches!(
|
||||
to_terrain(&tiny, &SmoothingConfig::default()),
|
||||
Err(GpxError::TooShort)
|
||||
));
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn a_degenerate_config_falls_back_rather_than_dividing_by_zero() {
|
||||
let track = synthetic_track(&[100.0, 110.0, 120.0, 130.0, 140.0], 100.0);
|
||||
let cfg = SmoothingConfig {
|
||||
resample_m: 0.0,
|
||||
window_m: -5.0,
|
||||
min_gradient_pct: 15.0,
|
||||
max_gradient_pct: -10.0,
|
||||
};
|
||||
let terrain = to_terrain(&track, &cfg).unwrap();
|
||||
assert!(!terrain.is_empty());
|
||||
assert!(terrain
|
||||
.iter()
|
||||
.all(|p| p.gradient_pct.is_finite() && (-10.0..=15.0).contains(&p.gradient_pct)));
|
||||
}
|
||||
|
||||
// ---- end to end ------------------------------------------------------
|
||||
|
||||
#[test]
|
||||
fn the_shipped_sample_climb_imports_cleanly() {
|
||||
let points = parse(SAMPLE_CLIMB).unwrap();
|
||||
assert!(points.len() > 100, "{} points", points.len());
|
||||
|
||||
let cfg = SmoothingConfig::default();
|
||||
let profile = import(SAMPLE_CLIMB, "Sample climb", &cfg).unwrap();
|
||||
assert_eq!(profile.name, "Sample climb");
|
||||
assert!(!profile.looping);
|
||||
assert_eq!(profile.blocks.len(), 1);
|
||||
profile.validate().unwrap();
|
||||
|
||||
let Block::Terrain { points: terrain } = &profile.blocks[0] else {
|
||||
panic!("expected a terrain block");
|
||||
};
|
||||
assert!(terrain.len() > 10);
|
||||
for p in terrain {
|
||||
assert!(p.gradient_pct.is_finite());
|
||||
assert!((cfg.min_gradient_pct..=cfg.max_gradient_pct).contains(&p.gradient_pct));
|
||||
}
|
||||
// The fixture is noisy but is a genuine climb, so the mean must be up.
|
||||
let mean: f32 = terrain.iter().map(|p| p.gradient_pct).sum::<f32>() / terrain.len() as f32;
|
||||
assert!(mean > 0.0, "sample climb averaged {mean}%");
|
||||
|
||||
// And the profile it produces is rideable.
|
||||
let extent = profile.total_extent();
|
||||
assert!(extent.metres.unwrap_or(0.0) > 100.0);
|
||||
assert!(profile
|
||||
.sample(crate::profile::Position {
|
||||
elapsed_s: 0.0,
|
||||
distance_m: 50.0,
|
||||
})
|
||||
.is_some());
|
||||
}
|
||||
|
||||
/// The fixture carries ±1.5 m of elevation noise on every point over a
|
||||
/// route with known structure: ~500 m flat, ~1.8 km climbing at 6–8%
|
||||
/// (sinusoidally varying), then ~700 m descending at about −4.5%. The
|
||||
/// pipeline has to recover that structure, not the noise.
|
||||
#[test]
|
||||
fn the_shipped_sample_climb_recovers_its_real_structure() {
|
||||
let cfg = SmoothingConfig::default();
|
||||
let terrain = to_terrain(&parse(SAMPLE_CLIMB).unwrap(), &cfg).unwrap();
|
||||
let total = terrain.last().unwrap().distance_m;
|
||||
assert!((total - 3040.0).abs() < 100.0, "route measured {total} m");
|
||||
|
||||
// No lurches anywhere on the route, ends included.
|
||||
let worst_step = worst_gradient_step(&terrain);
|
||||
assert!(
|
||||
worst_step < 1.0,
|
||||
"gradient jumped by {worst_step}% in one step"
|
||||
);
|
||||
|
||||
// Opening flat.
|
||||
let flat = mean_gradient(&terrain, 0.0, 400.0);
|
||||
assert!(flat.abs() < 1.0, "flat section read {flat}%");
|
||||
|
||||
// The climb, sampled clear of the transitions at either end.
|
||||
let climb = mean_gradient(&terrain, 700.0, 2200.0);
|
||||
assert!((3.0..8.0).contains(&climb), "climb averaged {climb}%");
|
||||
for p in terrain
|
||||
.iter()
|
||||
.filter(|p| (700.0..=2200.0).contains(&p.distance_m))
|
||||
{
|
||||
assert!(
|
||||
(2.0..10.0).contains(&p.gradient_pct),
|
||||
"climb sample at {} m read {}%",
|
||||
p.distance_m,
|
||||
p.gradient_pct
|
||||
);
|
||||
}
|
||||
|
||||
// The closing descent.
|
||||
let descent = mean_gradient(&terrain, 2500.0, 2900.0);
|
||||
assert!(
|
||||
(-6.0..-3.0).contains(&descent),
|
||||
"descent averaged {descent}%"
|
||||
);
|
||||
|
||||
// Net ascent, integrated from the smoothed gradient, matches the route.
|
||||
let spacing = cfg.resample_m as f32;
|
||||
let ascent: f32 = terrain
|
||||
.iter()
|
||||
.map(|p| (p.gradient_pct / 100.0 * spacing).max(0.0))
|
||||
.sum();
|
||||
assert!((60.0..110.0).contains(&ascent), "net ascent {ascent} m");
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn the_shipped_sample_climb_survives_a_tight_smoothing_window() {
|
||||
// Even at a third of the default window the result must stay usable:
|
||||
// noisier, but still free of step changes a rider would feel.
|
||||
let cfg = SmoothingConfig {
|
||||
window_m: 30.0,
|
||||
..Default::default()
|
||||
};
|
||||
let terrain = to_terrain(&parse(SAMPLE_CLIMB).unwrap(), &cfg).unwrap();
|
||||
let worst_step = worst_gradient_step(&terrain);
|
||||
assert!(
|
||||
worst_step < 4.0,
|
||||
"gradient jumped by {worst_step}% in one step"
|
||||
);
|
||||
assert!(terrain.iter().all(|p| p.gradient_pct.is_finite()));
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn import_propagates_parse_errors() {
|
||||
assert!(matches!(
|
||||
import("<gpx>", "n", &SmoothingConfig::default()),
|
||||
Err(GpxError::Malformed(_))
|
||||
));
|
||||
assert!(matches!(
|
||||
import("<gpx></gpx>", "n", &SmoothingConfig::default()),
|
||||
Err(GpxError::TooShort)
|
||||
));
|
||||
}
|
||||
}
|
||||
|
||||
+429
-4
@@ -23,6 +23,24 @@ pub const GRAVITY: f32 = 9.80665;
|
||||
/// below which the rider is considered stopped.
|
||||
pub const MIN_SPEED_MPS: f32 = 0.5;
|
||||
|
||||
/// Absolute ceiling on virtual speed, ~144 km/h. Aerodynamic drag bounds the
|
||||
/// model well below this for any plausible input; the cap exists so that
|
||||
/// absurd configuration (CdA of zero, a 90% descent) still cannot run away.
|
||||
pub const MAX_SPEED_MPS: f32 = 40.0;
|
||||
|
||||
/// Longest tick the integrator will honour. A caller that stalls for a minute
|
||||
/// must not be allowed to teleport the rider down a mountain.
|
||||
const MAX_DT_S: f32 = 10.0;
|
||||
|
||||
/// The integrator sub-divides the caller's `dt` to this resolution. Forward
|
||||
/// Euler on `P/v` is stiff at low speed, so the result would otherwise depend
|
||||
/// on how often the caller happens to tick; sub-stepping makes a 1 Hz tick and
|
||||
/// a 4 Hz tick agree.
|
||||
const SUBSTEP_S: f32 = 0.02;
|
||||
|
||||
/// Gradients beyond this are not physical roads and only appear as bad input.
|
||||
const MAX_ABS_GRADIENT_PCT: f32 = 100.0;
|
||||
|
||||
/// Evolving physical state of the virtual rider.
|
||||
#[derive(Debug, Clone, Copy, PartialEq, Default)]
|
||||
pub struct PhysicsState {
|
||||
@@ -41,8 +59,50 @@ impl PhysicsState {
|
||||
/// rather than snapping to it — and must never produce negative speed,
|
||||
/// NaN, or unbounded values for any finite input.
|
||||
pub fn step(&mut self, power_w: f32, gradient_pct: f32, cfg: &RiderConfig, dt: f32) {
|
||||
let _ = (power_w, gradient_pct, cfg, dt);
|
||||
todo!("implemented in crates/core/src/physics.rs — see AGENT task A")
|
||||
let dt = sanitise(dt, 0.0).clamp(0.0, MAX_DT_S);
|
||||
if dt <= 0.0 {
|
||||
return;
|
||||
}
|
||||
|
||||
let forces = Forces::new(power_w, gradient_pct, cfg);
|
||||
|
||||
// Recover from a poisoned state rather than propagating it: a single
|
||||
// bad tick must not permanently wedge the ride.
|
||||
if !self.speed_mps.is_finite() {
|
||||
self.speed_mps = 0.0;
|
||||
}
|
||||
if !self.distance_m.is_finite() {
|
||||
self.distance_m = 0.0;
|
||||
}
|
||||
if !self.elevation_gain_m.is_finite() {
|
||||
self.elevation_gain_m = 0.0;
|
||||
}
|
||||
|
||||
let steps = (dt / SUBSTEP_S).ceil().max(1.0);
|
||||
let h = dt / steps;
|
||||
let steps = steps as u32;
|
||||
|
||||
for _ in 0..steps {
|
||||
let v0 = self.speed_mps.clamp(0.0, MAX_SPEED_MPS);
|
||||
let v1 = (v0 + forces.acceleration(v0) * h).clamp(0.0, MAX_SPEED_MPS);
|
||||
self.speed_mps = v1;
|
||||
|
||||
// Trapezoidal: with forward Euler on velocity this is the exact
|
||||
// integral of the linear velocity ramp over the sub-step.
|
||||
let ds = (0.5 * (v0 + v1) * h) as f64;
|
||||
self.distance_m += ds;
|
||||
|
||||
// `ds` is measured along the road surface, so the vertical
|
||||
// component is sin(θ). Only ascent counts (FR-7.6).
|
||||
let climb = ds as f32 * forces.sin_theta;
|
||||
if climb > 0.0 {
|
||||
self.elevation_gain_m += climb;
|
||||
}
|
||||
}
|
||||
|
||||
if !self.speed_mps.is_finite() {
|
||||
self.speed_mps = 0.0;
|
||||
}
|
||||
}
|
||||
|
||||
pub fn speed_kph(&self) -> f32 {
|
||||
@@ -54,10 +114,375 @@ impl PhysicsState {
|
||||
}
|
||||
}
|
||||
|
||||
/// The speed-independent parts of the force balance, computed once per tick.
|
||||
struct Forces {
|
||||
/// `P × efficiency`; divided by speed to give propulsive force.
|
||||
wheel_power_w: f32,
|
||||
sin_theta: f32,
|
||||
/// Gravity plus rolling resistance, newtons. Constant in speed.
|
||||
resistive_n: f32,
|
||||
/// `½ρ·CdA`; multiplied by v² to give drag.
|
||||
drag_k: f32,
|
||||
mass_kg: f32,
|
||||
}
|
||||
|
||||
impl Forces {
|
||||
fn new(power_w: f32, gradient_pct: f32, cfg: &RiderConfig) -> Self {
|
||||
// Braking is not modelled, so negative power is treated as coasting.
|
||||
let power = sanitise(power_w, 0.0).max(0.0);
|
||||
let gradient =
|
||||
sanitise(gradient_pct, 0.0).clamp(-MAX_ABS_GRADIENT_PCT, MAX_ABS_GRADIENT_PCT);
|
||||
let theta = (gradient / 100.0).atan();
|
||||
|
||||
// A zero or negative mass would divide by zero; a config that broken
|
||||
// should degrade rather than produce NaN.
|
||||
let mass = sanitise(cfg.total_mass_kg(), 83.0).max(1.0);
|
||||
let efficiency = sanitise(cfg.drivetrain_efficiency, 1.0).clamp(0.0, 1.0);
|
||||
let crr = sanitise(cfg.crr, 0.0).max(0.0);
|
||||
let cda = sanitise(cfg.cda, 0.0).max(0.0);
|
||||
let rho = sanitise(cfg.air_density, 0.0).max(0.0);
|
||||
|
||||
Self {
|
||||
wheel_power_w: power * efficiency,
|
||||
sin_theta: theta.sin(),
|
||||
resistive_n: mass * GRAVITY * (theta.sin() + crr * theta.cos()),
|
||||
drag_k: 0.5 * rho * cda,
|
||||
mass_kg: mass,
|
||||
}
|
||||
}
|
||||
|
||||
fn acceleration(&self, v: f32) -> f32 {
|
||||
let propulsive = self.wheel_power_w / v.max(MIN_SPEED_MPS);
|
||||
// Rolling resistance and gravity are folded together, so at a
|
||||
// standstill on the flat the net is a small negative that the ≥0 clamp
|
||||
// absorbs — the rider does not roll backwards.
|
||||
let net = propulsive - self.resistive_n - self.drag_k * v * v;
|
||||
let a = net / self.mass_kg;
|
||||
if a.is_finite() {
|
||||
a
|
||||
} else {
|
||||
0.0
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
fn sanitise(value: f32, fallback: f32) -> f32 {
|
||||
if value.is_finite() {
|
||||
value
|
||||
} else {
|
||||
fallback
|
||||
}
|
||||
}
|
||||
|
||||
/// Steady-state speed for a given power and gradient — the speed at which
|
||||
/// propulsive and resistive forces balance. Useful for tests and for sanity
|
||||
/// checks on the resistance curve later.
|
||||
///
|
||||
/// The balance is a cubic in `v` (`P·η = F_const·v + k·v³`) with no clean
|
||||
/// closed form once the `max(v, v_min)` floor is included, so it is solved by
|
||||
/// bisection. Net force is non-increasing in `v`, which makes the bracket
|
||||
/// unambiguous.
|
||||
pub fn equilibrium_speed_mps(power_w: f32, gradient_pct: f32, cfg: &RiderConfig) -> f32 {
|
||||
let _ = (power_w, gradient_pct, cfg);
|
||||
todo!("implemented in crates/core/src/physics.rs — see AGENT task A")
|
||||
let forces = Forces::new(power_w, gradient_pct, cfg);
|
||||
|
||||
// Cannot get moving at all: the rider stalls on the climb.
|
||||
if forces.acceleration(0.0) <= 0.0 {
|
||||
return 0.0;
|
||||
}
|
||||
if forces.acceleration(MAX_SPEED_MPS) > 0.0 {
|
||||
return MAX_SPEED_MPS;
|
||||
}
|
||||
|
||||
let mut lo = 0.0f32;
|
||||
let mut hi = MAX_SPEED_MPS;
|
||||
// 60 halvings takes the bracket far below f32 resolution.
|
||||
for _ in 0..60 {
|
||||
let mid = 0.5 * (lo + hi);
|
||||
if mid <= lo || mid >= hi {
|
||||
break;
|
||||
}
|
||||
if forces.acceleration(mid) > 0.0 {
|
||||
lo = mid;
|
||||
} else {
|
||||
hi = mid;
|
||||
}
|
||||
}
|
||||
0.5 * (lo + hi)
|
||||
}
|
||||
|
||||
#[cfg(test)]
|
||||
mod tests {
|
||||
use super::*;
|
||||
|
||||
fn cfg() -> RiderConfig {
|
||||
RiderConfig::default()
|
||||
}
|
||||
|
||||
/// Run the integrator to steady state and return the state.
|
||||
fn settle(power_w: f32, gradient_pct: f32, seconds: f32) -> PhysicsState {
|
||||
let mut s = PhysicsState::default();
|
||||
let cfg = cfg();
|
||||
let dt = 0.25;
|
||||
let ticks = (seconds / dt) as u32;
|
||||
for _ in 0..ticks {
|
||||
s.step(power_w, gradient_pct, &cfg, dt);
|
||||
}
|
||||
s
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn equilibrium_is_a_fixed_point_of_the_integrator() {
|
||||
for (power, gradient) in [(200.0, 0.0), (300.0, 5.0), (150.0, -2.0), (400.0, 8.0)] {
|
||||
let target = equilibrium_speed_mps(power, gradient, &cfg());
|
||||
let settled = settle(power, gradient, 900.0).speed_mps;
|
||||
assert!(
|
||||
(settled - target).abs() < 0.05,
|
||||
"P={power} g={gradient}: integrator settled at {settled}, equilibrium says {target}"
|
||||
);
|
||||
}
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn equilibrium_matches_hand_computed_flat_case() {
|
||||
// 250 W on the flat with the default rider: solve P·η = F_roll·v + k·v³.
|
||||
let c = cfg();
|
||||
let v = equilibrium_speed_mps(250.0, 0.0, &c);
|
||||
let m = c.total_mass_kg();
|
||||
let f_roll = m * GRAVITY * c.crr;
|
||||
let drag = 0.5 * c.air_density * c.cda;
|
||||
let balance = 250.0 * c.drivetrain_efficiency - (f_roll * v + drag * v * v * v);
|
||||
assert!(balance.abs() < 0.5, "residual force {balance} N at v={v}");
|
||||
// Sanity: a 75 kg rider at 250 W on the flat sits around 40 km/h.
|
||||
assert!((35.0..45.0).contains(&(v * 3.6)), "{} km/h", v * 3.6);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn speed_approaches_equilibrium_rather_than_snapping() {
|
||||
let c = cfg();
|
||||
let target = equilibrium_speed_mps(250.0, 0.0, &c);
|
||||
let mut s = PhysicsState::default();
|
||||
|
||||
s.step(250.0, 0.0, &c, 1.0);
|
||||
let after_one_second = s.speed_mps;
|
||||
assert!(
|
||||
after_one_second < target * 0.75,
|
||||
"one second reached {after_one_second} of {target} — no inertia"
|
||||
);
|
||||
assert!(after_one_second > 0.0);
|
||||
|
||||
for _ in 0..600 {
|
||||
s.step(250.0, 0.0, &c, 1.0);
|
||||
}
|
||||
assert!((s.speed_mps - target).abs() < 0.05);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn tick_rate_does_not_change_the_outcome() {
|
||||
let c = cfg();
|
||||
let mut coarse = PhysicsState::default();
|
||||
let mut fine = PhysicsState::default();
|
||||
for _ in 0..60 {
|
||||
coarse.step(300.0, 3.0, &c, 1.0);
|
||||
}
|
||||
for _ in 0..600 {
|
||||
fine.step(300.0, 3.0, &c, 0.1);
|
||||
}
|
||||
assert!((coarse.speed_mps - fine.speed_mps).abs() < 0.02);
|
||||
assert!((coarse.distance_m - fine.distance_m).abs() < 1.0);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn zero_power_coasts_to_a_stop_on_the_flat() {
|
||||
let c = cfg();
|
||||
let mut s = PhysicsState {
|
||||
speed_mps: 11.0,
|
||||
..Default::default()
|
||||
};
|
||||
let start = s.speed_mps;
|
||||
s.step(0.0, 0.0, &c, 1.0);
|
||||
assert!(s.speed_mps < start, "coasting must decelerate");
|
||||
|
||||
for _ in 0..600 {
|
||||
s.step(0.0, 0.0, &c, 1.0);
|
||||
}
|
||||
assert_eq!(s.speed_mps, 0.0, "should have come to rest");
|
||||
assert!(!s.is_moving());
|
||||
assert!(s.distance_m > 0.0 && s.distance_m < 2000.0);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn stationary_with_no_power_never_goes_backwards() {
|
||||
let c = cfg();
|
||||
let mut s = PhysicsState::default();
|
||||
for _ in 0..100 {
|
||||
s.step(0.0, 0.0, &c, 1.0);
|
||||
assert_eq!(s.speed_mps, 0.0);
|
||||
}
|
||||
assert_eq!(s.distance_m, 0.0);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn steep_climb_stalls_but_stays_non_negative() {
|
||||
let c = cfg();
|
||||
let mut s = PhysicsState::default();
|
||||
for _ in 0..300 {
|
||||
s.step(60.0, 20.0, &c, 1.0);
|
||||
assert!(s.speed_mps >= 0.0);
|
||||
}
|
||||
assert!(s.speed_mps < 1.0, "60 W up 20% should barely move");
|
||||
assert_eq!(equilibrium_speed_mps(60.0, 20.0, &c), 0.0);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn steep_descent_accelerates_to_a_bounded_terminal_speed() {
|
||||
let c = cfg();
|
||||
let mut s = PhysicsState::default();
|
||||
for _ in 0..600 {
|
||||
s.step(0.0, -12.0, &c, 1.0);
|
||||
}
|
||||
let terminal = equilibrium_speed_mps(0.0, -12.0, &c);
|
||||
assert!(terminal > 10.0, "should freewheel downhill, got {terminal}");
|
||||
assert!(terminal < MAX_SPEED_MPS);
|
||||
assert!((s.speed_mps - terminal).abs() < 0.1);
|
||||
assert_eq!(s.elevation_gain_m, 0.0, "descending gains no elevation");
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn more_power_always_means_more_speed() {
|
||||
let c = cfg();
|
||||
let mut previous = -1.0;
|
||||
for power in [0.0, 50.0, 100.0, 200.0, 300.0, 500.0, 1000.0] {
|
||||
let v = equilibrium_speed_mps(power, 0.0, &c);
|
||||
assert!(
|
||||
v > previous,
|
||||
"{power} W gave {v} m/s, not more than {previous}"
|
||||
);
|
||||
previous = v;
|
||||
}
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn steeper_gradient_always_means_less_speed() {
|
||||
let c = cfg();
|
||||
let mut previous = f32::INFINITY;
|
||||
for gradient in [-10.0, -5.0, 0.0, 2.0, 5.0, 10.0, 15.0] {
|
||||
let v = equilibrium_speed_mps(300.0, gradient, &c);
|
||||
assert!(
|
||||
v < previous,
|
||||
"{gradient}% gave {v} m/s, not less than {previous}"
|
||||
);
|
||||
previous = v;
|
||||
}
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn distance_and_elevation_accumulate_consistently() {
|
||||
let s = settle(250.0, 5.0, 600.0);
|
||||
assert!(s.distance_m > 0.0);
|
||||
// 5% grade: vertical is sin(atan(0.05)) ≈ 0.0499 of distance travelled.
|
||||
let expected = s.distance_m as f32 * (0.05f32.atan()).sin();
|
||||
assert!(
|
||||
(s.elevation_gain_m - expected).abs() < expected * 0.01,
|
||||
"gain {} vs expected {expected}",
|
||||
s.elevation_gain_m
|
||||
);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn elevation_gain_counts_only_ascent() {
|
||||
let c = cfg();
|
||||
let mut s = PhysicsState::default();
|
||||
for _ in 0..300 {
|
||||
s.step(250.0, 5.0, &c, 1.0);
|
||||
}
|
||||
let after_climb = s.elevation_gain_m;
|
||||
assert!(after_climb > 10.0);
|
||||
for _ in 0..300 {
|
||||
s.step(250.0, -5.0, &c, 1.0);
|
||||
}
|
||||
assert_eq!(
|
||||
s.elevation_gain_m, after_climb,
|
||||
"descent must not reduce gain"
|
||||
);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn hostile_inputs_never_produce_nan_or_negatives() {
|
||||
let mut c = cfg();
|
||||
let hostile = [
|
||||
f32::NAN,
|
||||
f32::INFINITY,
|
||||
f32::NEG_INFINITY,
|
||||
-1.0e30,
|
||||
1.0e30,
|
||||
0.0,
|
||||
-0.0,
|
||||
];
|
||||
for &power in &hostile {
|
||||
for &gradient in &hostile {
|
||||
for &dt in &hostile {
|
||||
let mut s = PhysicsState::default();
|
||||
s.step(power, gradient, &c, dt);
|
||||
s.step(power, gradient, &c, 1.0);
|
||||
assert!(
|
||||
s.speed_mps.is_finite(),
|
||||
"speed NaN for {power}/{gradient}/{dt}"
|
||||
);
|
||||
assert!(s.speed_mps >= 0.0, "negative speed {}", s.speed_mps);
|
||||
assert!(s.speed_mps <= MAX_SPEED_MPS);
|
||||
assert!(s.distance_m.is_finite() && s.distance_m >= 0.0);
|
||||
assert!(s.elevation_gain_m.is_finite() && s.elevation_gain_m >= 0.0);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// A degenerate rider config must degrade, not explode.
|
||||
c.rider_kg = 0.0;
|
||||
c.bike_kg = 0.0;
|
||||
c.cda = 0.0;
|
||||
c.air_density = 0.0;
|
||||
c.crr = f32::NAN;
|
||||
let mut s = PhysicsState::default();
|
||||
for _ in 0..100 {
|
||||
s.step(500.0, -30.0, &c, 1.0);
|
||||
}
|
||||
assert!(s.speed_mps.is_finite() && (0.0..=MAX_SPEED_MPS).contains(&s.speed_mps));
|
||||
assert!(equilibrium_speed_mps(500.0, -30.0, &c).is_finite());
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn poisoned_state_is_recovered() {
|
||||
let c = cfg();
|
||||
let mut s = PhysicsState {
|
||||
speed_mps: f32::NAN,
|
||||
distance_m: f64::NAN,
|
||||
elevation_gain_m: f32::NAN,
|
||||
};
|
||||
s.step(200.0, 0.0, &c, 1.0);
|
||||
assert!(s.speed_mps.is_finite());
|
||||
assert!(s.distance_m.is_finite());
|
||||
assert!(s.elevation_gain_m.is_finite());
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn zero_and_negative_dt_are_no_ops() {
|
||||
let c = cfg();
|
||||
let mut s = PhysicsState {
|
||||
speed_mps: 8.0,
|
||||
..Default::default()
|
||||
};
|
||||
let before = s;
|
||||
s.step(300.0, 0.0, &c, 0.0);
|
||||
s.step(300.0, 0.0, &c, -5.0);
|
||||
assert_eq!(s, before);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn speed_kph_conversion() {
|
||||
let s = PhysicsState {
|
||||
speed_mps: 10.0,
|
||||
..Default::default()
|
||||
};
|
||||
assert!((s.speed_kph() - 36.0).abs() < 1e-5);
|
||||
}
|
||||
}
|
||||
|
||||
+1017
-10
File diff suppressed because it is too large
Load Diff
+752
-5
@@ -32,6 +32,12 @@ pub enum RideStatus {
|
||||
Finished,
|
||||
}
|
||||
|
||||
/// Smallest change worth spending a control-point write on. FR-2.8 caps writes
|
||||
/// at 4 Hz; suppressing no-op targets keeps a 10 Hz tick loop comfortably
|
||||
/// inside that without a timer, and avoids churning the trainer with values it
|
||||
/// cannot resolve anyway.
|
||||
const GRADIENT_EPSILON_PCT: f32 = 0.05;
|
||||
|
||||
pub struct RideSession {
|
||||
pub config: RiderConfig,
|
||||
pub limits: SafetyLimits,
|
||||
@@ -41,6 +47,10 @@ pub struct RideSession {
|
||||
profile: Option<Profile>,
|
||||
/// Manual gradient trim applied on top of the profile's gradient.
|
||||
gradient_offset_pct: f32,
|
||||
/// Level held in [`ControlMode::Resistance`] (FR-4.3).
|
||||
manual_resistance: i16,
|
||||
/// Wattage held in [`ControlMode::Erg`] (FR-4.6).
|
||||
erg_watts: u16,
|
||||
elapsed_ms: u64,
|
||||
last_target: Option<ControlTarget>,
|
||||
}
|
||||
@@ -55,6 +65,8 @@ impl RideSession {
|
||||
physics: PhysicsState::default(),
|
||||
profile: None,
|
||||
gradient_offset_pct: 0.0,
|
||||
manual_resistance: 0,
|
||||
erg_watts: 150,
|
||||
elapsed_ms: 0,
|
||||
last_target: None,
|
||||
}
|
||||
@@ -93,6 +105,50 @@ impl RideSession {
|
||||
self.gradient_offset_pct = 0.0;
|
||||
}
|
||||
|
||||
pub fn gradient_offset_pct(&self) -> f32 {
|
||||
self.gradient_offset_pct
|
||||
}
|
||||
|
||||
/// Set the resistance level held in [`ControlMode::Resistance`] (FR-4.3).
|
||||
///
|
||||
/// Stored unclamped; `SafetyLimits` still has the final say at
|
||||
/// transmission, so the rider's setting is never silently rewritten here.
|
||||
pub fn set_resistance(&mut self, level: i16) {
|
||||
self.manual_resistance = level;
|
||||
}
|
||||
|
||||
pub fn nudge_resistance(&mut self, delta: i16) {
|
||||
self.manual_resistance = self.manual_resistance.saturating_add(delta);
|
||||
}
|
||||
|
||||
pub fn resistance_level(&self) -> i16 {
|
||||
self.manual_resistance
|
||||
}
|
||||
|
||||
/// Set the wattage held in [`ControlMode::Erg`] (FR-4.6).
|
||||
pub fn set_erg_power(&mut self, watts: u16) {
|
||||
self.erg_watts = watts;
|
||||
}
|
||||
|
||||
pub fn nudge_erg_power(&mut self, delta: i16) {
|
||||
self.erg_watts = self.erg_watts.saturating_add_signed(delta);
|
||||
}
|
||||
|
||||
pub fn erg_power_w(&self) -> u16 {
|
||||
self.erg_watts
|
||||
}
|
||||
|
||||
/// Read-only view of the physics model, for diagnostics and recording.
|
||||
pub fn physics(&self) -> &PhysicsState {
|
||||
&self.physics
|
||||
}
|
||||
|
||||
/// The target most recently sent to the trainer, post-clamp (SAF-1: this
|
||||
/// is what should be held when input is lost).
|
||||
pub fn last_target(&self) -> Option<ControlTarget> {
|
||||
self.last_target
|
||||
}
|
||||
|
||||
/// Advance the ride by one tick.
|
||||
///
|
||||
/// Feeds telemetry into the physics model, advances the profile, and
|
||||
@@ -100,18 +156,709 @@ impl RideSession {
|
||||
/// when paused (no distance accrues) and when telemetry is missing power
|
||||
/// (treat as zero rather than panicking).
|
||||
pub fn tick(&mut self, telemetry: Telemetry, dt_s: f32) -> Vec<SessionEvent> {
|
||||
let _ = (telemetry, dt_s);
|
||||
todo!("implemented in crates/core/src/session.rs — see AGENT task A")
|
||||
let mut events = Vec::new();
|
||||
let dt = if dt_s.is_finite() { dt_s.max(0.0) } else { 0.0 };
|
||||
let running = self.status == RideStatus::Running;
|
||||
|
||||
if running {
|
||||
self.elapsed_ms = self
|
||||
.elapsed_ms
|
||||
.saturating_add((dt as f64 * 1000.0).round() as u64);
|
||||
}
|
||||
|
||||
// Resolve the target *before* stepping, so the physics see the same
|
||||
// gradient the trainer is being asked for this tick.
|
||||
let desired = self.desired_target();
|
||||
let exhausted = self.profile.is_some() && desired.is_none();
|
||||
|
||||
if running {
|
||||
// A trainer that reports no power is a trainer the rider is not
|
||||
// pushing; nothing here may panic on a partial FTMS packet.
|
||||
let power_w = f32::from(telemetry.power_w.unwrap_or(0)).max(0.0);
|
||||
self.physics
|
||||
.step(power_w, self.simulated_gradient_pct(), &self.config, dt);
|
||||
}
|
||||
|
||||
// Only a running ride commands the trainer. When paused or finished the
|
||||
// last target simply stands (SAF-1) rather than being re-sent or reset.
|
||||
if running {
|
||||
if let Some(target) = desired {
|
||||
let clamped = self.limits.clamp(target);
|
||||
if changed_meaningfully(self.last_target, clamped) {
|
||||
self.last_target = Some(clamped);
|
||||
events.push(SessionEvent::Command(clamped));
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if exhausted && running {
|
||||
self.status = RideStatus::Finished;
|
||||
events.push(SessionEvent::ProfileFinished);
|
||||
}
|
||||
|
||||
events.push(SessionEvent::Snapshot(self.snapshot(telemetry)));
|
||||
events
|
||||
}
|
||||
|
||||
/// Build the snapshot the UI renders.
|
||||
pub fn snapshot(&self, telemetry: Telemetry) -> RideSnapshot {
|
||||
let _ = telemetry;
|
||||
todo!("implemented in crates/core/src/session.rs — see AGENT task A")
|
||||
RideSnapshot {
|
||||
elapsed_ms: self.elapsed_ms,
|
||||
telemetry,
|
||||
virtual_speed_kph: self.physics.speed_kph(),
|
||||
virtual_distance_m: self.physics.distance_m,
|
||||
gradient_pct: self.simulated_gradient_pct(),
|
||||
elevation_gain_m: self.physics.elevation_gain_m,
|
||||
mode: self.mode,
|
||||
target: self.last_target,
|
||||
profile_progress: self.profile_progress(),
|
||||
}
|
||||
}
|
||||
|
||||
/// Fractional progress through the loaded profile (FR-9.7). `None` for a
|
||||
/// looping profile, which never ends, or when nothing is loaded.
|
||||
pub fn profile_progress(&self) -> Option<f32> {
|
||||
let profile = self.profile.as_ref()?;
|
||||
if profile.looping {
|
||||
return None;
|
||||
}
|
||||
profile.total_extent().progress(self.position())
|
||||
}
|
||||
|
||||
/// The target that should be in force right now, before clamping.
|
||||
fn desired_target(&self) -> Option<ControlTarget> {
|
||||
todo!("implemented in crates/core/src/session.rs — see AGENT task A")
|
||||
match self.mode {
|
||||
// No profile involved: the trim *is* the gradient.
|
||||
ControlMode::ManualGrade => Some(ControlTarget::Gradient {
|
||||
percent: self.gradient_offset_pct,
|
||||
}),
|
||||
ControlMode::Profile => {
|
||||
let sampled = self.profile.as_ref()?.sample(self.position())?;
|
||||
Some(match sampled {
|
||||
// The D-pad trim rides on top of the route (FR-4.2 and
|
||||
// FR-4.4 are simultaneously active, §5.4).
|
||||
ControlTarget::Gradient { percent } => ControlTarget::Gradient {
|
||||
percent: percent + self.gradient_offset_pct,
|
||||
},
|
||||
other => other,
|
||||
})
|
||||
}
|
||||
// These modes hold a value the rider set directly and ignore any
|
||||
// loaded profile — selecting the mode *is* the statement that the
|
||||
// rider is driving the trainer, not the route.
|
||||
ControlMode::Resistance => Some(ControlTarget::Resistance {
|
||||
level: self.manual_resistance,
|
||||
}),
|
||||
ControlMode::Erg => Some(ControlTarget::Power {
|
||||
watts: self.erg_watts,
|
||||
}),
|
||||
}
|
||||
}
|
||||
|
||||
/// The gradient the physics model should simulate this tick: the profile's
|
||||
/// gradient, if it is driving one, plus the manual trim. A profile driving
|
||||
/// power or resistance contributes no slope, so the rider is on the flat
|
||||
/// plus whatever trim they have dialled in.
|
||||
fn simulated_gradient_pct(&self) -> f32 {
|
||||
let base = match self.mode {
|
||||
ControlMode::Profile => match self
|
||||
.profile
|
||||
.as_ref()
|
||||
.and_then(|p| p.sample(self.position()))
|
||||
{
|
||||
Some(ControlTarget::Gradient { percent }) => percent,
|
||||
_ => 0.0,
|
||||
},
|
||||
_ => 0.0,
|
||||
};
|
||||
base + self.gradient_offset_pct
|
||||
}
|
||||
}
|
||||
|
||||
/// Whether a new target differs enough from the last one to be worth sending.
|
||||
/// A change of channel always counts.
|
||||
fn changed_meaningfully(previous: Option<ControlTarget>, next: ControlTarget) -> bool {
|
||||
match (previous, next) {
|
||||
(None, _) => true,
|
||||
(Some(ControlTarget::Gradient { percent: a }), ControlTarget::Gradient { percent: b }) => {
|
||||
(a - b).abs() >= GRADIENT_EPSILON_PCT
|
||||
}
|
||||
(Some(ControlTarget::Resistance { level: a }), ControlTarget::Resistance { level: b }) => {
|
||||
a != b
|
||||
}
|
||||
(Some(ControlTarget::Power { watts: a }), ControlTarget::Power { watts: b }) => a != b,
|
||||
_ => true,
|
||||
}
|
||||
}
|
||||
|
||||
#[cfg(test)]
|
||||
mod tests {
|
||||
use super::*;
|
||||
use crate::profile::{Block, Channel, Extent, Segment, Waveform};
|
||||
|
||||
fn session() -> RideSession {
|
||||
RideSession::new(RiderConfig::default(), SafetyLimits::default())
|
||||
}
|
||||
|
||||
fn powered(watts: i16) -> Telemetry {
|
||||
Telemetry {
|
||||
power_w: Some(watts),
|
||||
..Default::default()
|
||||
}
|
||||
}
|
||||
|
||||
fn commands(events: &[SessionEvent]) -> Vec<ControlTarget> {
|
||||
events
|
||||
.iter()
|
||||
.filter_map(|e| match e {
|
||||
SessionEvent::Command(t) => Some(*t),
|
||||
_ => None,
|
||||
})
|
||||
.collect()
|
||||
}
|
||||
|
||||
fn snapshot_of(events: &[SessionEvent]) -> RideSnapshot {
|
||||
events
|
||||
.iter()
|
||||
.find_map(|e| match e {
|
||||
SessionEvent::Snapshot(s) => Some(*s),
|
||||
_ => None,
|
||||
})
|
||||
.expect("every tick emits a snapshot")
|
||||
}
|
||||
|
||||
fn gradient_of(target: ControlTarget) -> f32 {
|
||||
match target {
|
||||
ControlTarget::Gradient { percent } => percent,
|
||||
other => panic!("expected a gradient target, got {other:?}"),
|
||||
}
|
||||
}
|
||||
|
||||
// ---- basic loop ------------------------------------------------------
|
||||
|
||||
#[test]
|
||||
fn every_tick_emits_exactly_one_snapshot() {
|
||||
let mut s = session();
|
||||
s.start();
|
||||
for _ in 0..10 {
|
||||
let events = s.tick(powered(200), 1.0);
|
||||
let snapshots = events
|
||||
.iter()
|
||||
.filter(|e| matches!(e, SessionEvent::Snapshot(_)))
|
||||
.count();
|
||||
assert_eq!(snapshots, 1);
|
||||
}
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn running_accrues_time_distance_and_speed() {
|
||||
let mut s = session();
|
||||
s.start();
|
||||
for _ in 0..60 {
|
||||
s.tick(powered(250), 1.0);
|
||||
}
|
||||
let snap = snapshot_of(&s.tick(powered(250), 1.0));
|
||||
assert_eq!(snap.elapsed_ms, 61_000);
|
||||
assert!(snap.virtual_distance_m > 300.0);
|
||||
assert!(snap.virtual_speed_kph > 20.0);
|
||||
}
|
||||
|
||||
// ---- pause -----------------------------------------------------------
|
||||
|
||||
#[test]
|
||||
fn pausing_accrues_neither_time_nor_distance() {
|
||||
let mut s = session();
|
||||
s.start();
|
||||
for _ in 0..30 {
|
||||
s.tick(powered(250), 1.0);
|
||||
}
|
||||
let before = snapshot_of(&s.tick(powered(250), 1.0));
|
||||
|
||||
s.pause();
|
||||
for _ in 0..100 {
|
||||
let events = s.tick(powered(250), 1.0);
|
||||
// Paused: nothing new is commanded, the last target stands (SAF-1).
|
||||
assert!(commands(&events).is_empty());
|
||||
}
|
||||
let after = snapshot_of(&s.tick(powered(250), 1.0));
|
||||
assert_eq!(after.virtual_distance_m, before.virtual_distance_m);
|
||||
assert_eq!(after.elapsed_ms, before.elapsed_ms);
|
||||
assert_eq!(after.elevation_gain_m, before.elevation_gain_m);
|
||||
assert_eq!(after.target, before.target);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn an_idle_session_never_commands_the_trainer() {
|
||||
let mut s = session();
|
||||
for _ in 0..5 {
|
||||
assert!(commands(&s.tick(powered(300), 1.0)).is_empty());
|
||||
}
|
||||
assert_eq!(s.last_target(), None);
|
||||
assert_eq!(
|
||||
snapshot_of(&s.tick(powered(300), 1.0)).virtual_distance_m,
|
||||
0.0
|
||||
);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn resuming_after_a_pause_continues_from_where_it_stopped() {
|
||||
let mut s = session();
|
||||
s.start();
|
||||
for _ in 0..30 {
|
||||
s.tick(powered(250), 1.0);
|
||||
}
|
||||
let mid = snapshot_of(&s.tick(powered(250), 1.0)).virtual_distance_m;
|
||||
s.pause();
|
||||
s.tick(powered(250), 1.0);
|
||||
s.start();
|
||||
for _ in 0..10 {
|
||||
s.tick(powered(250), 1.0);
|
||||
}
|
||||
assert!(snapshot_of(&s.tick(powered(250), 1.0)).virtual_distance_m > mid);
|
||||
}
|
||||
|
||||
// ---- missing / hostile telemetry ------------------------------------
|
||||
|
||||
#[test]
|
||||
fn missing_power_is_treated_as_zero() {
|
||||
let mut s = session();
|
||||
s.start();
|
||||
for _ in 0..30 {
|
||||
s.tick(powered(300), 1.0);
|
||||
}
|
||||
let moving = snapshot_of(&s.tick(powered(300), 1.0)).virtual_speed_kph;
|
||||
assert!(moving > 10.0);
|
||||
|
||||
// Empty packets — a real FTMS possibility, not a hypothetical.
|
||||
for _ in 0..300 {
|
||||
s.tick(Telemetry::default(), 1.0);
|
||||
}
|
||||
let snap = snapshot_of(&s.tick(Telemetry::default(), 1.0));
|
||||
assert!(snap.virtual_speed_kph < moving);
|
||||
assert_eq!(snap.virtual_speed_kph, 0.0, "should coast to a stop");
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn negative_power_does_not_drive_the_rider_backwards() {
|
||||
let mut s = session();
|
||||
s.start();
|
||||
for _ in 0..50 {
|
||||
let snap = snapshot_of(&s.tick(powered(-500), 1.0));
|
||||
assert!(snap.virtual_speed_kph >= 0.0);
|
||||
assert_eq!(snap.virtual_distance_m, 0.0);
|
||||
}
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn hostile_dt_does_not_corrupt_the_ride() {
|
||||
let mut s = session();
|
||||
s.start();
|
||||
for dt in [f32::NAN, f32::INFINITY, -1.0, 0.0, 1e20] {
|
||||
let snap = snapshot_of(&s.tick(powered(200), dt));
|
||||
assert!(snap.virtual_speed_kph.is_finite() && snap.virtual_speed_kph >= 0.0);
|
||||
assert!(snap.virtual_distance_m.is_finite() && snap.virtual_distance_m >= 0.0);
|
||||
}
|
||||
}
|
||||
|
||||
// ---- gradient offset -------------------------------------------------
|
||||
|
||||
#[test]
|
||||
fn manual_grade_commands_the_trim_directly() {
|
||||
let mut s = session();
|
||||
s.start();
|
||||
assert_eq!(gradient_of(commands(&s.tick(powered(0), 1.0))[0]), 0.0);
|
||||
|
||||
s.nudge_gradient(0.5);
|
||||
s.nudge_gradient(0.5);
|
||||
let events = s.tick(powered(0), 1.0);
|
||||
assert_eq!(gradient_of(commands(&events)[0]), 1.0);
|
||||
assert_eq!(snapshot_of(&events).gradient_pct, 1.0);
|
||||
|
||||
s.reset_gradient_offset();
|
||||
assert_eq!(gradient_of(commands(&s.tick(powered(0), 1.0))[0]), 0.0);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn the_trim_adds_on_top_of_the_profile_gradient() {
|
||||
let mut s = session();
|
||||
s.load_profile(Profile {
|
||||
name: "flat-then-hill".into(),
|
||||
description: None,
|
||||
looping: false,
|
||||
blocks: vec![Block::Constant {
|
||||
channel: Channel::Gradient,
|
||||
value: 4.0,
|
||||
extent: Extent::Seconds(600.0),
|
||||
}],
|
||||
});
|
||||
s.start();
|
||||
assert_eq!(gradient_of(commands(&s.tick(powered(200), 1.0))[0]), 4.0);
|
||||
|
||||
s.nudge_gradient(-1.5);
|
||||
let events = s.tick(powered(200), 1.0);
|
||||
assert_eq!(gradient_of(commands(&events)[0]), 2.5);
|
||||
// And the physics see the trimmed gradient too, not the raw profile.
|
||||
assert_eq!(snapshot_of(&events).gradient_pct, 2.5);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn the_trim_does_not_disturb_a_power_profile_target() {
|
||||
let mut s = session();
|
||||
s.load_profile(Profile {
|
||||
name: "erg".into(),
|
||||
description: None,
|
||||
looping: false,
|
||||
blocks: vec![Block::Constant {
|
||||
channel: Channel::Power,
|
||||
value: 220.0,
|
||||
extent: Extent::Seconds(600.0),
|
||||
}],
|
||||
});
|
||||
s.start();
|
||||
s.nudge_gradient(3.0);
|
||||
let events = s.tick(powered(220), 1.0);
|
||||
assert_eq!(commands(&events)[0], ControlTarget::Power { watts: 220 });
|
||||
// The trim still tilts the virtual road, which is what drives speed.
|
||||
assert_eq!(snapshot_of(&events).gradient_pct, 3.0);
|
||||
}
|
||||
|
||||
// ---- safety clamping -------------------------------------------------
|
||||
|
||||
#[test]
|
||||
fn out_of_range_gradients_are_clamped_before_transmission() {
|
||||
let mut s = session();
|
||||
s.start();
|
||||
s.nudge_gradient(90.0);
|
||||
let target = commands(&s.tick(powered(0), 1.0))[0];
|
||||
assert_eq!(gradient_of(target), s.limits.max_gradient_pct);
|
||||
|
||||
s.reset_gradient_offset();
|
||||
s.nudge_gradient(-90.0);
|
||||
// Two ticks: the first re-emits after the reset.
|
||||
s.tick(powered(0), 1.0);
|
||||
assert!(gradient_of(s.last_target().unwrap()) >= s.limits.min_gradient_pct);
|
||||
assert_eq!(
|
||||
gradient_of(s.last_target().unwrap()),
|
||||
s.limits.min_gradient_pct
|
||||
);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn an_absurd_profile_cannot_command_an_unsafe_target() {
|
||||
// SAF-6: parameter errors must be caught by SAF-3, not by the profile.
|
||||
let mut s = session();
|
||||
s.load_profile(Profile {
|
||||
name: "runaway".into(),
|
||||
description: None,
|
||||
looping: false,
|
||||
blocks: vec![
|
||||
Block::Constant {
|
||||
channel: Channel::Power,
|
||||
value: 5000.0,
|
||||
extent: Extent::Seconds(10.0),
|
||||
},
|
||||
Block::Constant {
|
||||
channel: Channel::Gradient,
|
||||
value: -400.0,
|
||||
extent: Extent::Seconds(10.0),
|
||||
},
|
||||
Block::Constant {
|
||||
channel: Channel::Resistance,
|
||||
value: 9000.0,
|
||||
extent: Extent::Seconds(10.0),
|
||||
},
|
||||
],
|
||||
});
|
||||
s.start();
|
||||
let mut seen = Vec::new();
|
||||
for _ in 0..29 {
|
||||
seen.extend(commands(&s.tick(powered(200), 1.0)));
|
||||
}
|
||||
assert!(!seen.is_empty());
|
||||
for target in seen {
|
||||
match target {
|
||||
ControlTarget::Power { watts } => {
|
||||
assert!((s.limits.min_power_w..=s.limits.max_power_w).contains(&watts))
|
||||
}
|
||||
ControlTarget::Gradient { percent } => assert!((s.limits.min_gradient_pct
|
||||
..=s.limits.max_gradient_pct)
|
||||
.contains(&percent)),
|
||||
ControlTarget::Resistance { level } => {
|
||||
assert!((s.limits.min_resistance..=s.limits.max_resistance).contains(&level))
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn custom_limits_are_honoured() {
|
||||
let mut s = RideSession::new(
|
||||
RiderConfig::default(),
|
||||
SafetyLimits {
|
||||
min_gradient_pct: -2.0,
|
||||
max_gradient_pct: 3.0,
|
||||
..Default::default()
|
||||
},
|
||||
);
|
||||
s.start();
|
||||
s.nudge_gradient(10.0);
|
||||
assert_eq!(gradient_of(commands(&s.tick(powered(0), 1.0))[0]), 3.0);
|
||||
}
|
||||
|
||||
// ---- rate limiting ---------------------------------------------------
|
||||
|
||||
#[test]
|
||||
fn an_unchanged_target_is_not_resent() {
|
||||
let mut s = session();
|
||||
s.start();
|
||||
assert_eq!(commands(&s.tick(powered(200), 1.0)).len(), 1);
|
||||
for _ in 0..50 {
|
||||
assert!(
|
||||
commands(&s.tick(powered(200), 1.0)).is_empty(),
|
||||
"a steady target must not be re-sent (FR-2.8)"
|
||||
);
|
||||
}
|
||||
s.nudge_gradient(1.0);
|
||||
assert_eq!(commands(&s.tick(powered(200), 1.0)).len(), 1);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn sub_threshold_gradient_drift_is_suppressed() {
|
||||
let mut s = session();
|
||||
s.start();
|
||||
s.tick(powered(0), 1.0);
|
||||
s.nudge_gradient(0.01);
|
||||
assert!(commands(&s.tick(powered(0), 1.0)).is_empty());
|
||||
for _ in 0..10 {
|
||||
s.nudge_gradient(0.01);
|
||||
}
|
||||
assert_eq!(commands(&s.tick(powered(0), 1.0)).len(), 1);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn a_continuously_varying_profile_stays_well_inside_the_write_budget() {
|
||||
// A 10 Hz tick loop over a gradient ramp must not produce 10 writes a
|
||||
// second; FR-2.8 caps them at four.
|
||||
let mut s = session();
|
||||
s.load_profile(Profile {
|
||||
name: "ramp".into(),
|
||||
description: None,
|
||||
looping: false,
|
||||
blocks: vec![Block::Ramp {
|
||||
channel: Channel::Gradient,
|
||||
from: 0.0,
|
||||
to: 6.0,
|
||||
extent: Extent::Seconds(600.0),
|
||||
}],
|
||||
});
|
||||
s.start();
|
||||
let mut writes = 0;
|
||||
for _ in 0..6000 {
|
||||
writes += commands(&s.tick(powered(200), 0.1)).len();
|
||||
}
|
||||
// 6 % of gradient at a 0.05 % threshold is ~120 writes over 600 s.
|
||||
assert!(writes <= 130, "{writes} writes in 600 s");
|
||||
assert!(writes > 100);
|
||||
}
|
||||
|
||||
// ---- profile lifecycle ----------------------------------------------
|
||||
|
||||
#[test]
|
||||
fn a_non_looping_profile_finishes_once() {
|
||||
let mut s = session();
|
||||
s.load_profile(Profile {
|
||||
name: "short".into(),
|
||||
description: None,
|
||||
looping: false,
|
||||
blocks: vec![Block::Constant {
|
||||
channel: Channel::Gradient,
|
||||
value: 2.0,
|
||||
extent: Extent::Seconds(5.0),
|
||||
}],
|
||||
});
|
||||
s.start();
|
||||
let mut finishes = 0;
|
||||
for _ in 0..20 {
|
||||
finishes += s
|
||||
.tick(powered(200), 1.0)
|
||||
.iter()
|
||||
.filter(|e| matches!(e, SessionEvent::ProfileFinished))
|
||||
.count();
|
||||
}
|
||||
assert_eq!(finishes, 1);
|
||||
assert_eq!(s.status, RideStatus::Finished);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn a_looping_profile_never_finishes() {
|
||||
let mut s = session();
|
||||
s.load_profile(Profile {
|
||||
name: "loop".into(),
|
||||
description: None,
|
||||
looping: true,
|
||||
blocks: vec![Block::Segments {
|
||||
segments: vec![
|
||||
Segment {
|
||||
distance_m: 400.0,
|
||||
gradient_pct: 0.0,
|
||||
},
|
||||
Segment {
|
||||
distance_m: 400.0,
|
||||
gradient_pct: 5.0,
|
||||
},
|
||||
],
|
||||
}],
|
||||
});
|
||||
s.start();
|
||||
for _ in 0..1200 {
|
||||
let events = s.tick(powered(250), 1.0);
|
||||
assert!(!events
|
||||
.iter()
|
||||
.any(|e| matches!(e, SessionEvent::ProfileFinished)));
|
||||
}
|
||||
assert_eq!(s.status, RideStatus::Running);
|
||||
assert!(s.physics().distance_m > 2000.0, "should have lapped");
|
||||
assert_eq!(s.profile_progress(), None);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn progress_advances_from_zero_to_one() {
|
||||
let mut s = session();
|
||||
s.load_profile(Profile {
|
||||
name: "p".into(),
|
||||
description: None,
|
||||
looping: false,
|
||||
blocks: vec![Block::Constant {
|
||||
channel: Channel::Gradient,
|
||||
value: 0.0,
|
||||
extent: Extent::Seconds(100.0),
|
||||
}],
|
||||
});
|
||||
s.start();
|
||||
assert_eq!(
|
||||
snapshot_of(&s.tick(powered(200), 0.0)).profile_progress,
|
||||
Some(0.0)
|
||||
);
|
||||
for _ in 0..50 {
|
||||
s.tick(powered(200), 1.0);
|
||||
}
|
||||
let mid = snapshot_of(&s.tick(powered(200), 0.0))
|
||||
.profile_progress
|
||||
.unwrap();
|
||||
assert!((mid - 0.5).abs() < 0.02, "{mid}");
|
||||
for _ in 0..60 {
|
||||
s.tick(powered(200), 1.0);
|
||||
}
|
||||
assert_eq!(
|
||||
snapshot_of(&s.tick(powered(200), 0.0)).profile_progress,
|
||||
Some(1.0)
|
||||
);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn a_distance_profile_advances_only_as_the_rider_rides() {
|
||||
let mut s = session();
|
||||
s.load_profile(Profile {
|
||||
name: "hill".into(),
|
||||
description: None,
|
||||
looping: false,
|
||||
blocks: vec![Block::Segments {
|
||||
segments: vec![
|
||||
Segment {
|
||||
distance_m: 200.0,
|
||||
gradient_pct: 0.0,
|
||||
},
|
||||
Segment {
|
||||
distance_m: 200.0,
|
||||
gradient_pct: 8.0,
|
||||
},
|
||||
],
|
||||
}],
|
||||
});
|
||||
s.start();
|
||||
// No power, so no distance, so no progress no matter how long it runs.
|
||||
for _ in 0..600 {
|
||||
s.tick(Telemetry::default(), 1.0);
|
||||
}
|
||||
assert_eq!(s.profile_progress(), Some(0.0));
|
||||
assert!(s.status == RideStatus::Running);
|
||||
|
||||
for _ in 0..600 {
|
||||
s.tick(powered(250), 1.0);
|
||||
}
|
||||
assert_eq!(s.status, RideStatus::Finished);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn a_wave_profile_drives_the_power_channel() {
|
||||
let mut s = session();
|
||||
s.load_profile(Profile {
|
||||
name: "over-unders".into(),
|
||||
description: None,
|
||||
looping: false,
|
||||
blocks: vec![Block::Wave {
|
||||
channel: Channel::Power,
|
||||
shape: Waveform::Sine,
|
||||
midpoint: 240.0,
|
||||
amplitude: 40.0,
|
||||
period: Extent::Seconds(120.0),
|
||||
repeats: 2.0,
|
||||
phase: 0.0,
|
||||
}],
|
||||
});
|
||||
s.start();
|
||||
let mut watts = Vec::new();
|
||||
for _ in 0..240 {
|
||||
for target in commands(&s.tick(powered(240), 1.0)) {
|
||||
match target {
|
||||
ControlTarget::Power { watts: w } => watts.push(w),
|
||||
other => panic!("unexpected {other:?}"),
|
||||
}
|
||||
}
|
||||
}
|
||||
assert!(watts.contains(&280), "peak never reached: {watts:?}");
|
||||
assert!(watts.contains(&200), "trough never reached");
|
||||
assert!(watts.iter().all(|w| (200..=280).contains(w)));
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn loading_a_profile_switches_into_profile_mode() {
|
||||
let mut s = session();
|
||||
assert_eq!(s.mode, ControlMode::ManualGrade);
|
||||
s.load_profile(Profile {
|
||||
name: "p".into(),
|
||||
description: None,
|
||||
looping: false,
|
||||
blocks: vec![Block::Constant {
|
||||
channel: Channel::Gradient,
|
||||
value: 1.0,
|
||||
extent: Extent::Seconds(10.0),
|
||||
}],
|
||||
});
|
||||
assert_eq!(s.mode, ControlMode::Profile);
|
||||
assert!(s.profile().is_some());
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn a_gradient_profile_accumulates_elevation() {
|
||||
let mut s = session();
|
||||
s.load_profile(Profile {
|
||||
name: "climb".into(),
|
||||
description: None,
|
||||
looping: false,
|
||||
blocks: vec![Block::Constant {
|
||||
channel: Channel::Gradient,
|
||||
value: 6.0,
|
||||
extent: Extent::Seconds(1200.0),
|
||||
}],
|
||||
});
|
||||
s.start();
|
||||
for _ in 0..600 {
|
||||
s.tick(powered(250), 1.0);
|
||||
}
|
||||
let snap = snapshot_of(&s.tick(powered(250), 0.0));
|
||||
let expected = snap.virtual_distance_m as f32 * (0.06f32.atan()).sin();
|
||||
assert!((snap.elevation_gain_m - expected).abs() < expected * 0.02);
|
||||
assert!(snap.elevation_gain_m > 50.0);
|
||||
}
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user