embedded_flight_control/
lateral_pos.rs

1use crate::pid_config;
2use nalgebra::Vector2;
3
4pub struct LateralPositionController {
5    lateral_k_p: f32,
6    lateral_k_d: f32,
7}
8
9impl Default for LateralPositionController {
10    fn default() -> Self {
11        let (lateral_k_p, lateral_k_d) = pid_config(0.28, 0.95);
12        Self {
13            lateral_k_p,
14            lateral_k_d,
15        }
16    }
17}
18
19impl LateralPositionController {
20    pub fn lateral_position_control(
21        &self,
22        local_position_cmd: Vector2<f32>,
23        local_velocity_cmd: Vector2<f32>,
24        local_position: Vector2<f32>,
25        local_velocity: Vector2<f32>,
26        acceleration_ff: Vector2<f32>,
27    ) -> Vector2<f32> {
28        let err_p = local_position_cmd - local_position;
29        let err_dot = local_velocity_cmd - local_velocity;
30
31        self.lateral_k_p * err_p + self.lateral_k_d * err_dot + acceleration_ff
32    }
33}