1use stm_32::peripherals::pwm::{PixRacerProServoMonstrosity, TimerError};
37use veloxity_core::board::BoardIo;
38use veloxity_core::mixer::MixerOutputType;
39use veloxity_core::pwm::{PwmDriver, PwmError, PwmOutputProtocol};
40
41const NUM_HW_CHANNELS: usize = 4;
42
43pub struct BoardPwmDriver<'a> {
44 servos: &'a mut PixRacerProServoMonstrosity,
45 current_values: [f32; NUM_HW_CHANNELS],
46 enabled_chan_mask: u16,
47 max_duty_counts: [u16; NUM_HW_CHANNELS],
48}
49
50impl<'a> BoardPwmDriver<'a> {
51 pub fn new(servos: &'a mut PixRacerProServoMonstrosity) -> Self {
52 let mut max_duty_counts = [0u16; NUM_HW_CHANNELS];
53 for (channel, max_duty) in max_duty_counts.iter_mut().enumerate() {
54 *max_duty = servos.max_duty_cycle(channel);
55 }
56
57 Self {
58 servos,
59 current_values: [1000.0; NUM_HW_CHANNELS],
60 enabled_chan_mask: 0,
61 max_duty_counts,
62 }
63 }
64
65 fn duty_u16_to_pwm_us(duty: u16) -> f32 {
66 let normalized = duty as f32 / u16::MAX as f32;
67 (normalized.clamp(0.0, 1.0) * 1000.0) + 1000.0
68 }
69}
70
71impl<'a> PwmDriver<f64> for BoardPwmDriver<'a> {
72 fn len(&self) -> usize {
73 NUM_HW_CHANNELS
74 }
75
76 fn is_enabled(&self) -> bool {
77 self.enabled_chan_mask == ((1 << NUM_HW_CHANNELS) - 1)
78 }
79
80 fn enable(&mut self, channel: usize) -> Result<(), PwmError> {
81 if channel >= NUM_HW_CHANNELS {
82 return Err(PwmError::ChannelOutOfRange);
83 }
84 self.servos
85 .enable(channel)
86 .map_err(|_| PwmError::GenericError)?;
87
88 self.enabled_chan_mask |= 1 << channel;
89
90 Ok(())
91 }
92
93 fn disable(&mut self, channel: usize) -> Result<(), PwmError> {
94 if channel >= NUM_HW_CHANNELS {
95 return Err(PwmError::ChannelOutOfRange);
96 }
97 self.servos
98 .disable(channel)
99 .map_err(|_| PwmError::GenericError)?;
100
101 self.enabled_chan_mask &= !(1 << channel);
102
103 Ok(())
104 }
105
106 fn enable_all(&mut self) -> Result<(), PwmError> {
107 for i in 0..NUM_HW_CHANNELS {
108 self.enable(i)?;
109 }
110 Ok(())
111 }
112
113 fn disable_all(&mut self) {
114 for i in 0..NUM_HW_CHANNELS {
115 let _ = self.disable(i);
116 }
117 }
118
119 fn set_duty_cycle(&mut self, channel: usize, duty: u16) -> Result<(), PwmError> {
120 if channel >= NUM_HW_CHANNELS {
121 return Err(PwmError::ChannelOutOfRange);
122 }
123 let pwm_us = Self::duty_u16_to_pwm_us(duty);
124 self.current_values[channel] = pwm_us;
125
126 let max_duty = self.max_duty_counts[channel] as f32;
127 let raw_pwm = pwm_us / 2500.0 * max_duty;
128
129 self.servos
130 .set_duty_cycle(channel, raw_pwm as u16)
131 .map_err(|_| PwmError::GenericError)
132 }
133
134 fn configure_output_rates(&mut self, rates_hz: &[f64]) -> Result<(), PwmError> {
135 self.servos
136 .configure_output_rates(&rates_hz[..rates_hz.len().min(NUM_HW_CHANNELS)])
137 .map_err(timer_error_to_pwm_error)?;
138
139 for (channel, max_duty) in self.max_duty_counts.iter_mut().enumerate() {
140 *max_duty = self.servos.max_duty_cycle(channel);
141 }
142
143 Ok(())
144 }
145
146 fn output_protocol(&self, channel: usize) -> Result<PwmOutputProtocol, PwmError> {
147 if channel >= NUM_HW_CHANNELS {
148 return Err(PwmError::ChannelOutOfRange);
149 }
150 self.servos
151 .output_protocol(channel)
152 .map_err(timer_error_to_pwm_error)
153 }
154
155 fn flush<B: BoardIo>(&mut self, _board: &mut B) {
156 }
158
159 fn send_commands<B: BoardIo>(
160 &mut self,
161 board: &mut B,
162 commands_slice: &[f64],
163 ) -> Result<(), PwmError> {
164 self.servos
165 .send_normalized_commands(&commands_slice[..commands_slice.len().min(NUM_HW_CHANNELS)])
166 .map_err(timer_error_to_pwm_error)?;
167 self.flush(board);
168 Ok(())
169 }
170
171 fn send_disarmed_commands<B: BoardIo>(
172 &mut self,
173 board: &mut B,
174 output_types: &[MixerOutputType],
175 ) -> Result<(), PwmError> {
176 self.servos
177 .send_disarmed_commands(&output_types[..output_types.len().min(NUM_HW_CHANNELS)])
178 .map_err(timer_error_to_pwm_error)?;
179 self.flush(board);
180 Ok(())
181 }
182}
183
184fn timer_error_to_pwm_error(error: TimerError) -> PwmError {
185 match error {
186 TimerError::ChanNotSupported => PwmError::ChannelOutOfRange,
187 TimerError::InvalidRate => PwmError::InvalidRate,
188 TimerError::UnsupportedProtocol => PwmError::UnsupportedProtocol,
189 TimerError::TimerNotSupported => PwmError::GenericError,
190 }
191}