esp_hal_servo/
utils.rs

1//! Utility functions for servo calculations.
2//! These functions are independent of `ServoConfig` and can be tested in isolation.
3
4use core::ops::Range;
5
6const NANOS_IS_SEC: f32 = 1_000_000_000.0;
7
8/// Epsilon value for floating-point comparisons.
9///
10/// Avoid issues with floating-point precision during equality checks.
11pub const EPSILON: f32 = 0.5;
12
13/// Checks if two duty values are approximately equal.
14pub fn approx_eq(left: f32, right: f32) -> bool {
15    (left - right).abs() < EPSILON
16}
17
18/// Transforms absolute duty value to angle in degrees.
19pub fn duty_to_angle(duty: f32, max_angle: f32, duty_range: &Range<f32>) -> f32 {
20    let clamped_duty = duty.clamp(duty_range.start, duty_range.end);
21    ((clamped_duty - duty_range.start) / (duty_range.end - duty_range.start)) * max_angle
22}
23
24/// Transforms angle in degrees to absolute duty value.
25pub fn angle_to_duty(angle: f32, max_angle: f32, duty_range: &Range<f32>) -> f32 {
26    let clamped_angle = angle.clamp(0.0, max_angle);
27    duty_range.start + (clamped_angle / max_angle) * (duty_range.end - duty_range.start)
28}
29
30/// Calculates duty range in absolute values for given servo configuration.
31pub fn calc_duty_range(pulse_width_ns: Range<u32>, frequency_hz: f32, max_duty: f32) -> Range<f32> {
32    pub fn pulse_to_duty(pulse_ns: u32, frequency_hz: f32, max_duty: f32) -> f32 {
33        pulse_ns as f32 * frequency_hz * max_duty / NANOS_IS_SEC
34    }
35
36    let min_pulse = pulse_width_ns.start;
37    let max_pulse = pulse_width_ns.end;
38
39    // Calculate absolute duty values directly from pulse width
40    let min_duty = pulse_to_duty(min_pulse, frequency_hz, max_duty);
41    let max_duty_val = pulse_to_duty(max_pulse, frequency_hz, max_duty);
42
43    // Ensure values are within valid range
44    let mut min_duty = min_duty.min(max_duty);
45    let mut max_duty_val = max_duty_val.min(max_duty);
46
47    // Ensure valid range (min < max)
48    // If min >= max, adjust them to ensure min < max
49    if min_duty >= max_duty_val {
50        let mid = (min_duty + max_duty_val) / 2.0;
51        min_duty = mid.min(max_duty - 1.0);
52        max_duty_val = (min_duty + 1.0).min(max_duty);
53    }
54
55    min_duty..max_duty_val
56}
esp_hal_servo/utils.rs

esp_hal_servo/
utils.rs
