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) - self.pwr_prop_fwd_max;
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        };
83        let pwr_err_per_speed_guess_fn = |speed_guess: si::Velocity| {
84            3.0 * t3 * speed_guess.powi(typenum::P2::new()) + 2.0 * t2 * speed_guess + t1
85        };
86        let pwr_err = pwr_err_fn(speed_guess);
87        if almost_eq_uom(&pwr_err, &(0. * uc::W), Some(1e-6)) {
88            return self.cyc_speed;
89        }
90        let pwr_err_per_speed_guess = pwr_err_per_speed_guess_fn(speed_guess);
91        let new_speed_guess = pwr_err - speed_guess * pwr_err_per_speed_guess;
92        let mut speed_guesses = vec![speed_guess];
93        let mut pwr_errs = vec![pwr_err];
94        let mut d_pwr_err_per_d_speed_guesses = vec![pwr_err_per_speed_guess];
95        let mut new_speed_guesses = vec![new_speed_guess];
96        // speed achieved iteration counter
97        let mut spd_ach_iter_counter = 1;
98        let mut converged = false;
99        let mut speed_ach: si::Velocity = Default::default();
100        while spd_ach_iter_counter < max_iter && !converged {
101            let speed_guess = *speed_guesses.iter().last().unwrap() * (1.0 - g)
102                - g * *new_speed_guesses.iter().last().unwrap()
103                    / d_pwr_err_per_d_speed_guesses[speed_guesses.len() - 1];
104            let pwr_err = pwr_err_fn(speed_guess);
105            let pwr_err_per_speed_guess = pwr_err_per_speed_guess_fn(speed_guess);
106            let new_speed_guess = pwr_err - speed_guess * pwr_err_per_speed_guess;
107            speed_guesses.push(speed_guess);
108            pwr_errs.push(pwr_err);
109            d_pwr_err_per_d_speed_guesses.push(pwr_err_per_speed_guess);
110            new_speed_guesses.push(new_speed_guess);
111            // is the fractional change between previous and current speed guess smaller than `xtol`
112            converged = ((*speed_guesses.iter().last().unwrap()
113                - speed_guesses[speed_guesses.len() - 2])
114                / speed_guesses[speed_guesses.len() - 2])
115                .abs()
116                < xtol;
117            spd_ach_iter_counter += 1;
118
119            // TODO: verify that assuming `speed_guesses.iter().last()` is the correct solution
120            speed_ach = speed_guesses.last().unwrap().max(0.0 * uc::MPS);
121        }
122        speed_ach
123    }
124}
125
126#[cfg(test)]
127mod tests {
128    use crate::imports::*;
129
130    use super::StepInfo;
131
132    #[test]
133    pub fn test_solver_works_from_0_speed() {
134        let step_info = StepInfo {
135            speed_prev: si::Velocity::ZERO,
136            dt: 1.0 * uc::S,
137            cyc_speed: 8.0 * uc::MPS,
138            grade_curr: 0.0 * uc::R,
139            air_density: 1.2 * uc::KGPM3,
140            mass: 1644.2724500334996 * uc::KG,
141            drag_coef: 0.393 * uc::R,
142            frontal_area: 2.12 * uc::M2,
143            wheel_inertia: 0.82 * uc::KGM2,
144            num_wheels: 4,
145            wheel_radius: 0.326 * uc::M,
146            wheel_rr_coef: 0.007 * uc::R,
147            pwr_prop_fwd_max: 21750.0 * uc::W,
148        };
149        let ach_speed_max_iter = 30;
150        let ach_speed_tol = 1.0e-6 * uc::R;
151        let ach_speed_solver_gain = 0.9;
152        let speed_ach =
153            step_info.solve_for_speed(ach_speed_max_iter, ach_speed_tol, ach_speed_solver_gain);
154        assert!(speed_ach > si::Velocity::ZERO);
155    }
156}
fastsim_core/simdrive/roadload.rs

fastsim_core/simdrive/
roadload.rs
