embedded_flight_control/
yaw.rs

1use core::f32::consts::PI;
2
3pub struct YawController {
4    yaw_k_p: f32,
5}
6
7impl Default for YawController {
8    fn default() -> Self {
9        Self { yaw_k_p: 4.5 }
10    }
11}
12
13impl YawController {
14    /// Calculate the target yaw-rate in radians/second.
15    pub fn yaw_control(&self, yaw_cmd: f32, yaw: f32) -> f32 {
16        let mut yaw_error = yaw_cmd - yaw;
17        if yaw_error > PI {
18            yaw_error = yaw_error - 2. * PI;
19        } else if yaw_error < -PI {
20            yaw_error = yaw_error + 2. * PI;
21        };
22
23        self.yaw_k_p * yaw_error
24    }
25}