dtourolleandClaude Opus 5
f2c4cb2120
Gears are metres per crank revolution rather than gradient offsets, and resistive_force_n exposes what the road is doing at a given speed so load can be computed directly instead of servoed. The load model is tested but not yet commanded: FTMS sim mode has the trainer compute rolling and aero itself, so sending a gradient that already contains them would double-count. Needs Crr/Cw zeroed and a ride to verify. Co-Authored-By: Claude Opus 5 <noreply@anthropic.com>
634 lines
22 KiB
Rust
634 lines
22 KiB
Rust
//! Virtual speed from measured power (§5.7 of REQUIREMENTS.md).
|
||
//!
|
||
//! The app owns the physics rather than trusting the trainer's reported speed
|
||
//! (FR-7.1). This makes ride behaviour reproducible in tests, independent of
|
||
//! the trainer's internal mass assumptions, and is a prerequisite for virtual
|
||
//! gearing later.
|
||
//!
|
||
//! Per tick:
|
||
//! ```text
|
||
//! F_propulsive = (P × drivetrain_efficiency) / max(v, v_min)
|
||
//! F_gravity = m × g × sin(atan(gradient))
|
||
//! F_rolling = m × g × Crr × cos(atan(gradient))
|
||
//! F_aero = ½ × ρ × CdA × v²
|
||
//! a = (F_propulsive − F_gravity − F_rolling − F_aero) / m
|
||
//! v += a × Δt (clamped at ≥ 0)
|
||
//! ```
|
||
|
||
use crate::types::RiderConfig;
|
||
|
||
pub const GRAVITY: f32 = 9.80665;
|
||
|
||
/// Speed floor used to keep `P / v` finite at a standstill. Also the speed
|
||
/// 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;
|
||
|
||
/// Ceiling on *measured* power fed to the model. FTMS Instantaneous Power is a
|
||
/// sint16, so a glitched packet can legitimately decode to 32767 W — which the
|
||
/// force balance faithfully turns into a 144 km/h ride. No human produces more
|
||
/// than ~2500 W even for a single track-sprint pedal stroke, so anything above
|
||
/// this is a bad reading, not a rider.
|
||
///
|
||
/// This is deliberately *not* [`crate::types::SafetyLimits::max_power_w`]: that
|
||
/// one bounds the ERG target we *command*, this one bounds the power we
|
||
/// *believe*.
|
||
pub const MAX_MEASURED_POWER_W: f32 = 2500.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 {
|
||
/// Virtual speed, metres per second.
|
||
pub speed_mps: f32,
|
||
/// Virtual distance travelled, metres.
|
||
pub distance_m: f64,
|
||
/// Cumulative elevation gained, metres.
|
||
pub elevation_gain_m: f32,
|
||
}
|
||
|
||
impl PhysicsState {
|
||
/// Advance the simulation by `dt` seconds under `power_w` at `gradient_pct`.
|
||
///
|
||
/// Must model inertia (FR-7.3) — speed accelerates toward equilibrium
|
||
/// 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 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;
|
||
}
|
||
}
|
||
|
||
/// Pull the modelled speed toward one the trainer actually measured.
|
||
///
|
||
/// The model knows what a bike *would* do for a given power and gradient;
|
||
/// the trainer knows how fast its flywheel is really turning. Neither alone
|
||
/// is right on a single-cog drivetrain: pure physics lets the rider "coast"
|
||
/// downhill at 39 km/h while spinning out against no resistance, and pure
|
||
/// trainer speed would cap descents at whatever cadence the one gear allows.
|
||
///
|
||
/// `weight` is the fraction of the gap closed **per second**, so the result
|
||
/// does not depend on tick rate — a 4 Hz and a 10 Hz loop converge the same.
|
||
pub fn correct_toward(&mut self, measured_mps: f32, weight: f32, dt: f32) {
|
||
if !measured_mps.is_finite() || measured_mps < 0.0 || !dt.is_finite() || dt <= 0.0 {
|
||
return;
|
||
}
|
||
let w = weight.clamp(0.0, 1.0);
|
||
if w == 0.0 {
|
||
return;
|
||
}
|
||
// Fraction of the gap to close this tick, from the per-second rate.
|
||
let alpha = 1.0 - (1.0 - w).powf(dt.min(MAX_DT_S));
|
||
let corrected = self.speed_mps + (measured_mps - self.speed_mps) * alpha;
|
||
if corrected.is_finite() {
|
||
self.speed_mps = corrected.clamp(0.0, MAX_SPEED_MPS);
|
||
}
|
||
}
|
||
|
||
pub fn speed_kph(&self) -> f32 {
|
||
self.speed_mps * 3.6
|
||
}
|
||
|
||
pub fn is_moving(&self) -> bool {
|
||
self.speed_mps > MIN_SPEED_MPS
|
||
}
|
||
}
|
||
|
||
/// 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.
|
||
// The upper clamp is what keeps a glitched FTMS sample from driving the
|
||
// ride at 144 km/h; the integrator is stable and drift-free on its own,
|
||
// but it cannot tell an implausible input from a real one.
|
||
let power = sanitise(power_w, 0.0).clamp(0.0, MAX_MEASURED_POWER_W);
|
||
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 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)
|
||
}
|
||
|
||
/// Total resistive force at a given speed and gradient, newtons.
|
||
///
|
||
/// This is what the road is doing to the rider: gravity down the slope, rolling
|
||
/// resistance, and aerodynamic drag rising with the square of speed. It is the
|
||
/// force a trainer must reproduce at the wheel for the ride to feel real, and
|
||
/// therefore the basis for virtual gearing (see `crate::gearing`).
|
||
pub fn resistive_force_n(speed_mps: f32, gradient_pct: f32, cfg: &RiderConfig) -> f32 {
|
||
let forces = Forces::new(0.0, gradient_pct, cfg);
|
||
let v = if speed_mps.is_finite() {
|
||
speed_mps.clamp(0.0, MAX_SPEED_MPS)
|
||
} else {
|
||
0.0
|
||
};
|
||
let f = forces.resistive_n + forces.drag_k * v * v;
|
||
if f.is_finite() {
|
||
f
|
||
} else {
|
||
0.0
|
||
}
|
||
}
|
||
|
||
#[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);
|
||
}
|
||
|
||
/// The integrator must not creep. Forward Euler's discrete fixed point is
|
||
/// exactly the root of `a(v)`, i.e. the continuous equilibrium, so a steady
|
||
/// effort held for hours must not accumulate its way to a higher speed. A
|
||
/// higher-order scheme would not improve this — it shares the same fixed
|
||
/// point — so this test, not the integration order, is the guarantee.
|
||
#[test]
|
||
fn a_long_steady_ride_does_not_drift_upwards() {
|
||
let c = cfg();
|
||
let target = equilibrium_speed_mps(250.0, 0.0, &c);
|
||
let mut s = PhysicsState::default();
|
||
|
||
// Settle first, then hold for six hours of ride time.
|
||
for _ in 0..2_400 {
|
||
s.step(250.0, 0.0, &c, 0.25);
|
||
}
|
||
let after_settling = s.speed_mps;
|
||
for _ in 0..86_400 {
|
||
s.step(250.0, 0.0, &c, 0.25);
|
||
}
|
||
|
||
assert!(
|
||
(s.speed_mps - after_settling).abs() < 1.0e-3,
|
||
"speed crept from {after_settling} to {} over six hours",
|
||
s.speed_mps
|
||
);
|
||
assert!(
|
||
s.speed_mps <= target + 1.0e-3,
|
||
"settled {} above equilibrium {target}",
|
||
s.speed_mps
|
||
);
|
||
}
|
||
|
||
/// Equilibrium is a fixed point *exactly*, not approximately: stepping from
|
||
/// it must not move. This is the property that makes drift impossible.
|
||
#[test]
|
||
fn stepping_from_equilibrium_does_not_move() {
|
||
let c = cfg();
|
||
for (power, gradient) in [(200.0, 0.0), (300.0, 5.0), (150.0, -2.0)] {
|
||
let v = equilibrium_speed_mps(power, gradient, &c);
|
||
let mut s = PhysicsState {
|
||
speed_mps: v,
|
||
..Default::default()
|
||
};
|
||
s.step(power, gradient, &c, 1.0);
|
||
assert!(
|
||
(s.speed_mps - v).abs() < 1.0e-4,
|
||
"P={power} g={gradient}: {v} -> {}",
|
||
s.speed_mps
|
||
);
|
||
}
|
||
}
|
||
|
||
/// A glitched FTMS sample is a sint16, so it can decode to 32767 W. That
|
||
/// must not become a 144 km/h ride.
|
||
#[test]
|
||
fn implausible_power_cannot_drive_an_implausible_speed() {
|
||
let c = cfg();
|
||
let sane = settle(MAX_MEASURED_POWER_W, 0.0, 300.0).speed_mps;
|
||
|
||
for absurd in [3_000.0, 10_000.0, 32_767.0] {
|
||
let s = settle(absurd, 0.0, 300.0);
|
||
assert!(
|
||
(s.speed_mps - sane).abs() < 1.0e-3,
|
||
"{absurd} W settled at {} m/s, above the {sane} m/s ceiling",
|
||
s.speed_mps
|
||
);
|
||
assert!(
|
||
s.speed_mps < MAX_SPEED_MPS,
|
||
"{absurd} W pinned the speed at the absolute clamp"
|
||
);
|
||
}
|
||
|
||
// Real efforts, including a hard sprint, must be untouched by the clamp.
|
||
for real in [250.0, 600.0, 1_200.0, 2_000.0] {
|
||
let s = settle(real, 0.0, 300.0);
|
||
let expected = equilibrium_speed_mps(real, 0.0, &c);
|
||
assert!(
|
||
(s.speed_mps - expected).abs() < 0.05,
|
||
"{real} W was clamped: {} vs {expected}",
|
||
s.speed_mps
|
||
);
|
||
}
|
||
}
|
||
|
||
#[test]
|
||
fn speed_kph_conversion() {
|
||
let s = PhysicsState {
|
||
speed_mps: 10.0,
|
||
..Default::default()
|
||
};
|
||
assert!((s.speed_kph() - 36.0).abs() < 1e-5);
|
||
}
|
||
}
|