fastsim_core/simdrive/
roadload.rs

1use crate::imports::*;
2
3pub struct StepInfo {
4    pub dt: si::Time,
5    pub speed_prev: si::Velocity,
6    pub cyc_speed: si::Velocity,
7    pub grade_curr: si::Ratio,
8    pub air_density: si::MassDensity,
9    pub mass: si::Mass,
10    pub drag_coef: si::Ratio,
11    pub frontal_area: si::Area,
12    pub wheel_inertia: si::MomentOfInertia,
13    pub num_wheels: u8,
14    pub wheel_radius: si::Length,
15    pub wheel_rr_coef: si::Ratio,
16    pub pwr_prop_fwd_max: si::Power,
17}
18
19impl StepInfo {
20    pub fn solve_for_speed(
21        &self,
22        ach_speed_max_iter: u32,
23        ach_speed_tol: si::Ratio,
24        ach_speed_solver_gain: f64,
25    ) -> si::Velocity {
26        let min_speed = 1e-3 * uc::MPS;
27        let speed_prev = self.speed_prev.max(min_speed);
28        let drag3 = 1.0 / 16.0 * self.air_density * self.drag_coef * self.frontal_area;
29        let accel2 = 0.5 * self.mass / self.dt;
30        let drag2 = 3.0 / 16.0 * self.air_density * self.drag_coef * self.frontal_area * speed_prev;
31        let wheel2 = 0.5 * self.wheel_inertia * self.num_wheels as f64
32            / (self.dt * self.wheel_radius.powi(typenum::P2::new()));
33        let drag1 = 3.0 / 16.0
34            * self.air_density
35            * self.drag_coef
36            * self.frontal_area
37            * speed_prev.powi(typenum::P2::new());
38        let roll1 =
39            0.5 * self.mass * uc::ACC_GRAV * self.wheel_rr_coef * self.grade_curr.atan().cos();
40        let ascent1 = 0.5 * uc::ACC_GRAV * self.grade_curr.atan().sin() * self.mass;
41        let accel0 = -0.5 * self.mass * speed_prev.powi(typenum::P2::new()) / self.dt;
42        let drag0 = 1.0 / 16.0
43            * self.air_density
44            * self.drag_coef
45            * self.frontal_area
46            * speed_prev.powi(typenum::P3::new());
47        let roll0 = 0.5
48            * self.mass
49            * uc::ACC_GRAV
50            * self.wheel_rr_coef
51            * self.grade_curr.atan().cos()
52            * speed_prev;
53        let ascent0 = 0.5 * uc::ACC_GRAV * self.grade_curr.atan().sin() * self.mass * speed_prev;
54        let wheel0 = -0.5
55            * self.wheel_inertia
56            * self.num_wheels as f64
57            * speed_prev.powi(typenum::P2::new())
58            / (self.dt * self.wheel_radius.powi(typenum::P2::new()));
59
60        let t3 = drag3;
61        let t2 = accel2 + drag2 + wheel2;
62        let t1 = drag1 + roll1 + ascent1;
63        let t0 = accel0 + drag0 + roll0 + ascent0 + wheel0;
64
65        // initial guess
66        let speed_guess = if self.speed_prev == si::Velocity::ZERO {
67            // (1e-3 * uc::MPS).max(0.5 * (self.speed_prev + self.cyc_speed))
68            (min_speed).max(self.speed_prev)
69        } else {
70            (min_speed).max(self.speed_prev)
71        };
72        // stop criteria
73        let max_iter = ach_speed_max_iter;
74        let xtol = ach_speed_tol;
75        // solver gain
76        let g = ach_speed_solver_gain;
77        let pwr_err_fn = |speed_guess: si::Velocity| -> si::Power {
78            t3 * speed_guess.powi(typenum::P3::new())
79                + t2 * speed_guess.powi(typenum::P2::new())
80                + t1 * speed_guess
81                + t0
82                - self.pwr_prop_fwd_max
83        };
84        let pwr_err_per_speed_guess_fn = |speed_guess: si::Velocity| {
85            3.0 * t3 * speed_guess.powi(typenum::P2::new()) + 2.0 * t2 * speed_guess + t1
86        };
87        let pwr_err = pwr_err_fn(speed_guess);
88        if almost_eq_uom(&pwr_err, &(0. * uc::W), Some(1e-6)) {
89            return self.cyc_speed;
90        }
91        let pwr_err_per_speed_guess = pwr_err_per_speed_guess_fn(speed_guess);
92        let new_speed_guess = pwr_err - speed_guess * pwr_err_per_speed_guess;
93        let mut speed_guesses = vec![speed_guess];
94        let mut pwr_errs = vec![pwr_err];
95        let mut d_pwr_err_per_d_speed_guesses = vec![pwr_err_per_speed_guess];
96        let mut new_speed_guesses = vec![new_speed_guess];
97        // speed achieved iteration counter
98        let mut spd_ach_iter_counter = 1;
99        let mut converged = false;
100        let mut speed_ach: si::Velocity = Default::default();
101        while spd_ach_iter_counter < max_iter && !converged {
102            let speed_guess = *speed_guesses.iter().last().unwrap() * (1.0 - g)
103                - g * *new_speed_guesses.iter().last().unwrap()
104                    / d_pwr_err_per_d_speed_guesses[speed_guesses.len() - 1];
105            let pwr_err = pwr_err_fn(speed_guess);
106            let pwr_err_per_speed_guess = pwr_err_per_speed_guess_fn(speed_guess);
107            let new_speed_guess = pwr_err - speed_guess * pwr_err_per_speed_guess;
108            speed_guesses.push(speed_guess);
109            pwr_errs.push(pwr_err);
110            d_pwr_err_per_d_speed_guesses.push(pwr_err_per_speed_guess);
111            new_speed_guesses.push(new_speed_guess);
112            // is the fractional change between previous and current speed guess smaller than `xtol`
113            converged = ((*speed_guesses.iter().last().unwrap()
114                - speed_guesses[speed_guesses.len() - 2])
115                / speed_guesses[speed_guesses.len() - 2])
116                .abs()
117                < xtol
118                && almost_le_uom(&pwr_err, &si::Power::ZERO, None);
119            spd_ach_iter_counter += 1;
120
121            // TODO: verify that assuming `speed_guesses.iter().last()` is the correct solution
122            speed_ach = speed_guesses.last().unwrap().max(0.0 * uc::MPS);
123        }
124        speed_ach
125    }
126}
127
128#[cfg(test)]
129mod tests {
130    use crate::imports::*;
131
132    use super::StepInfo;
133
134    #[test]
135    pub fn test_solver_works_from_0_speed() {
136        let step_info = StepInfo {
137            speed_prev: si::Velocity::ZERO,
138            dt: 1.0 * uc::S,
139            cyc_speed: 8.0 * uc::MPS,
140            grade_curr: 0.0 * uc::R,
141            air_density: 1.2 * uc::KGPM3,
142            mass: 1644.2724500334996 * uc::KG,
143            drag_coef: 0.393 * uc::R,
144            frontal_area: 2.12 * uc::M2,
145            wheel_inertia: 0.82 * uc::KGM2,
146            num_wheels: 4,
147            wheel_radius: 0.326 * uc::M,
148            wheel_rr_coef: 0.007 * uc::R,
149            pwr_prop_fwd_max: 21750.0 * uc::W,
150        };
151        let ach_speed_max_iter = 30;
152        let ach_speed_tol = 1.0e-6 * uc::R;
153        let ach_speed_solver_gain = 0.9;
154        let speed_ach =
155            step_info.solve_for_speed(ach_speed_max_iter, ach_speed_tol, ach_speed_solver_gain);
156        assert!(speed_ach > si::Velocity::ZERO);
157    }
158}
fastsim_core/simdrive/roadload.rs

fastsim_core/simdrive/
roadload.rs
