rust_gnc/units/
angular.rs

1//! # Angular Units
2//! 
3//! This module provides type-safe representations for angular measurements.
4//! It handles the circular logic required for navigation, specifically 
5//! the transition across the ±π (180°) boundary.
6
7/// A type-safe wrapper for angular measurements in radians.
8/// 
9/// Range: Usually normalized to (-π, π] for flight dynamics.
10#[derive(Debug, Clone, Copy, PartialEq, Default)]
11pub struct Radians(pub f32);
12
13impl Radians {
14    /// Normalizes the angle to the range (-π, π].
15    /// 
16    /// This is essential for preventing "the long way around" maneuvers 
17    /// and ensuring the PID controller receives the smallest possible error.
18    /// 
19    /// ### Performance
20    /// This implementation uses a deterministic approach to ensure consistent 
21    /// execution time in real-time flight loops.
22    pub fn normalize(&self) -> Self {
23        let pi = core::f32::consts::PI;
24        let two_pi = 2.0 * pi;
25
26        // Industry Standard: Use a non-looping normalization for O(1) performance.
27        // This prevents timing jitter on microcontrollers if the input is very large.
28        let mut angle = self.0;
29        if angle <= -pi || angle > pi {
30            angle = angle - two_pi * libm::floorf((angle + pi) / two_pi);
31        }
32        Radians(angle)
33     }
34
35    /// Calculates the shortest angular distance to a target.
36    /// 
37    /// Returns a value in the range (-π, π]. A positive result indicates 
38    /// a clockwise rotation, while a negative result indicates counter-clockwise.
39    /// 
40    /// ### Example
41    /// Moving from 179° to -179° will return a distance of 2° (0.035 rad) 
42    /// instead of -358°.
43     pub fn shortest_distance_to(&self, target: Radians) -> f32 {
44        let delta = target.0 - self.0;
45        Radians(delta).normalize().0
46     }
47}
48
49/// A type-safe wrapper for angular measurements in degrees.
50/// 
51/// Primarily used for human-readable telemetry, logging, and configuration.
52#[derive(Debug, Clone, Copy, PartialEq, Default)]
53pub struct Degrees(pub f32);
54
55impl From<Radians> for Degrees {
56    /// Converts Radians to Degrees using the standard constant π.
57    fn from(radians: Radians) -> Self {
58        Degrees(radians.0 * 180.0 / core::f32::consts::PI)
59    }
60}
61
62impl From<Degrees> for Radians {
63    /// Converts Degrees to Radians. Used for ingesting user configuration 
64    /// into the flight-ready physics engine.
65    fn from(degrees: Degrees) -> Self {
66        Radians(degrees.0 * core::f32::consts::PI / 180.0)
67    }
68}
69
70#[cfg(test)]
71mod tests {
72  use super::*;
73
74    #[test]
75    fn test_normalization_boundaries() {
76        // Test wrap-around at PI
77        assert!((Radians(3.2).normalize().0 - (-3.0831)).abs() < 0.001);
78        // Test wrap-around at -PI
79        assert!((Radians(-3.2).normalize().0 - 3.0831).abs() < 0.001);
80        // Test identity
81        assert_eq!(Radians(1.0).normalize().0, 1.0);
82    }
83}
rust_gnc/units/angular.rs

rust_gnc/units/
angular.rs
