nucleo/
board.rs

1// ******************************************************************************
2// * File     : boards/nucleo/src/board.rs
3// * Date     : June 28, 2026
4// ******************************************************************************
5// *
6// * Copyright (c) 2023, AeroVironment, Inc.
7// * All rights reserved.
8// *
9// * Redistribution and use in source and binary forms, with or without
10// * modification, are permitted provided that the following conditions are met:
11// *
12// * 1.Redistributions of source code must retain the above copyright notice, this
13// * list of conditions and the following disclaimer.
14// *
15// * 2.Redistributions in binary form must reproduce the above copyright notice,
16// * this list of conditions and the following disclaimer in the documentation
17// * and/or other materials provided with the distribution.
18// *
19// * 3.Neither the name of the copyright holder nor the names of its
20// * contributors may be used to endorse or promote products derived from
21// * this software without specific prior written permission.
22// *
23// * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
24// * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
25// * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
26// * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
27// * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
28// * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
29// * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
30// * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
31// * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
32// * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
33// *
34// ******************************************************************************
35
36use veloxity_core::board::BoardIo;
37use veloxity_core::errors;
38use veloxity_core::math::FlightFloat;
39use veloxity_core::params::Params;
40use veloxity_core::pwm::{PwmDriver, PwmError};
41use veloxity_core::sensors::SensorBus;
42
43use embassy_time::Delay;
44use stm_32::cortex_m::prelude::_embedded_hal_blocking_delay_DelayMs;
45use stm_32::peripherals;
46use stm_32::*;
47
48include!("../../../platforms/stm_32/stm32h7x3_common.rs");
49
50static mut PARAM_STORE: Option<Params> = None;
51
52fn spawn_task<S: Send>(
53    spawner: &embassy_executor::SendSpawner,
54    token: Result<embassy_executor::SpawnToken<S>, embassy_executor::SpawnError>,
55) {
56    spawner.spawn(token.expect("failed to allocate Embassy task"));
57}
58
59pub struct Board {
60    _probe: [Output<'static>; 4],
61    pub start_time: embassy_time::Instant,
62    pending_reset_to_bootloader: Option<bool>,
63}
64
65pub struct BoardPwmDriver {
66    servos: peripherals::pwm::ServoMonstrosity,
67    enabled_chan_mask: u16,
68}
69
70impl BoardPwmDriver {
71    pub fn new(servos: peripherals::pwm::ServoMonstrosity) -> Self {
72        Self {
73            servos,
74            enabled_chan_mask: 0,
75        }
76    }
77}
78
79impl PwmDriver<f64> for BoardPwmDriver {
80    fn len(&self) -> usize {
81        self.servos.chan_list.len()
82    }
83
84    fn is_enabled(&self) -> bool {
85        self.enabled_chan_mask == ((1 << self.len()) - 1)
86    }
87
88    fn enable(&mut self, channel: usize) -> Result<(), PwmError> {
89        if channel >= self.len() {
90            return Err(PwmError::ChannelOutOfRange);
91        }
92        self.servos
93            .enable(channel)
94            .map_err(|_| PwmError::GenericError)?;
95        self.enabled_chan_mask |= 1 << channel;
96        Ok(())
97    }
98
99    fn disable(&mut self, channel: usize) -> Result<(), PwmError> {
100        if channel >= self.len() {
101            return Err(PwmError::ChannelOutOfRange);
102        }
103        self.servos
104            .disable(channel)
105            .map_err(|_| PwmError::GenericError)?;
106        self.enabled_chan_mask &= !(1 << channel);
107        Ok(())
108    }
109
110    fn enable_all(&mut self) -> Result<(), PwmError> {
111        for i in 0..self.len() {
112            self.enable(i)?;
113        }
114        Ok(())
115    }
116
117    fn disable_all(&mut self) {
118        for i in 0..self.len() {
119            let _ = self.disable(i);
120        }
121    }
122
123    fn set_duty_cycle(&mut self, channel: usize, duty: u16) -> Result<(), PwmError> {
124        if channel >= self.len() {
125            return Err(PwmError::ChannelOutOfRange);
126        }
127        self.servos
128            .set_duty_cycle(channel, duty)
129            .map_err(|_| PwmError::GenericError)
130    }
131
132    fn configure_output_rates(&mut self, rates_hz: &[f64]) -> Result<(), PwmError> {
133        self.servos
134            .configure_output_rates(rates_hz)
135            .map_err(timer_error_to_pwm_error)
136    }
137
138    fn flush<B: veloxity_core::board::BoardIo>(&mut self, _board: &mut B) {}
139
140    fn send_commands<B: veloxity_core::board::BoardIo>(
141        &mut self,
142        board: &mut B,
143        commands: &[f64],
144    ) -> Result<(), PwmError> {
145        self.servos
146            .send_normalized_commands(commands)
147            .map_err(timer_error_to_pwm_error)?;
148        self.flush(board);
149        Ok(())
150    }
151}
152
153fn timer_error_to_pwm_error(error: peripherals::pwm::TimerError) -> PwmError {
154    match error {
155        peripherals::pwm::TimerError::ChanNotSupported => PwmError::ChannelOutOfRange,
156        peripherals::pwm::TimerError::InvalidRate => PwmError::InvalidRate,
157        peripherals::pwm::TimerError::UnsupportedProtocol => PwmError::UnsupportedProtocol,
158        peripherals::pwm::TimerError::TimerNotSupported => PwmError::GenericError,
159    }
160}
161
162impl BoardIo for Board {
163    fn update_sensor_bus<R: FlightFloat>(&mut self, sensors: &mut SensorBus<R>) {
164        sensors.clear();
165        sensors.imu = peripherals::bmi08x::IMU_SIGNAL
166            .try_take()
167            .map(|result| result.map(|packet| packet.cast()));
168        sensors.mag = peripherals::iis2mdc::MAG_SIGNAL.try_take();
169        sensors.baro = peripherals::dps310::BARO_SIGNAL.try_take();
170        sensors.pitot = peripherals::dlhrl20g::PITOT_SIGNAL.try_take();
171        sensors.gnss = peripherals::ublox::GNSS_SIGNAL.try_take();
172        sensors.rc = peripherals::sbus::RC_SIGNAL.try_take();
173    }
174
175    fn imu_pending(&self) -> bool {
176        peripherals::bmi08x::IMU_SIGNAL.signaled()
177    }
178
179    fn update_imu_sensor<R: FlightFloat>(&mut self, sensors: &mut SensorBus<R>) {
180        sensors.clear();
181        sensors.imu = peripherals::bmi08x::IMU_SIGNAL
182            .try_take()
183            .map(|result| result.map(|packet| packet.cast()));
184    }
185
186    fn update_service_sensor_bus<R: FlightFloat>(&mut self, sensors: &mut SensorBus<R>) {
187        sensors.clear();
188        sensors.mag = peripherals::iis2mdc::MAG_SIGNAL.try_take();
189        sensors.baro = peripherals::dps310::BARO_SIGNAL.try_take();
190        sensors.pitot = peripherals::dlhrl20g::PITOT_SIGNAL.try_take();
191        sensors.gnss = peripherals::ublox::GNSS_SIGNAL.try_take();
192        sensors.rc = peripherals::sbus::RC_SIGNAL.try_take();
193    }
194
195    fn serial_rx_read(&mut self, buf: &mut [u8]) -> Option<Result<usize, errors::TelemError>> {
196        match peripherals::telem::TELEM_RX.try_read(buf) {
197            Ok(n) => return Some(Ok(n)),
198            Err(embassy_sync::pipe::TryReadError::Empty) => {
199                return Some(Ok(0));
200            }
201        }
202    }
203    fn serial_tx_write(&mut self, bytes: &[u8]) -> Option<Result<usize, errors::TelemError>> {
204        let mut n = 0;
205        let len = bytes.len();
206
207        loop {
208            match peripherals::telem::TELEM_TX.try_write(&bytes[n..len]) {
209                Ok(wrote) => {
210                    if wrote == (len - n) {
211                        break;
212                    } else {
213                        n += wrote;
214                    }
215                }
216                Err(_) => {
217                    return Some(Err(errors::TelemError::GenericTelemError(
218                        "Error Writing Telem Packet!",
219                    )));
220                }
221            }
222        }
223        Some(Ok(n))
224    }
225
226    fn clock_millis(&self) -> u32 {
227        self.start_time.elapsed().as_millis() as u32
228    }
229
230    fn clock_micros(&self) -> u64 {
231        self.start_time.elapsed().as_micros() as u64
232    }
233
234    fn read_params(&mut self, params: &mut Params) -> bool {
235        let Some(stored) = (unsafe { PARAM_STORE }) else {
236            return false;
237        };
238        *params = stored;
239        true
240    }
241
242    fn write_params(&mut self, params: &Params) -> bool {
243        unsafe {
244            PARAM_STORE = Some(*params);
245        }
246        true
247    }
248
249    fn reboot(&mut self) -> bool {
250        self.pending_reset_to_bootloader = Some(false);
251        true
252    }
253
254    fn reboot_to_bootloader(&mut self) -> bool {
255        self.pending_reset_to_bootloader = Some(true);
256        true
257    }
258
259    fn run_deferred_board_actions(&mut self) {
260        if self.pending_reset_to_bootloader.take().is_some() {
261            let mut delay = Delay;
262            delay.delay_ms(20u32);
263            stm_32::cortex_m::peripheral::SCB::sys_reset();
264        }
265    }
266}
267
268impl Board {
269    pub fn new() -> (Board, BoardPwmDriver) {
270        let p: EMBASSY_Peripherals = embassy_stm32::init(clock_config(8));
271
272        let start_time = embassy_time::Instant::now();
273
274        // SPI1 Bus ///////////////////////////////////////////
275        let mut spi1_config: embassy_stm32::spi::Config = spi::Config::default();
276        spi1_config.frequency = mhz(1);
277        spi1_config.mode = spi::MODE_3;
278        spi1_config.bit_order = spi::BitOrder::MsbFirst;
279        spi1_config.miso_pull = embassy_stm32::gpio::Pull::Up;
280        let spi1 = spi::Spi::new(
281            p.SPI1,
282            p.PB3,
283            p.PB5,
284            p.PB4,
285            p.DMA1_CH0,
286            p.DMA1_CH1,
287            BoardIrqs,
288            spi1_config,
289        );
290        let spi1_bus = Mutex::new(spi1);
291        let spi1_bus = SPI1_BUS.init(spi1_bus);
292
293        // IIS2MDC Mag
294        let nss1 = Output::new(p.PA4, Level::High, Speed::Low);
295        let drdy1 = ExtiInput::new(p.PF3, p.EXTI3, Pull::Down, BoardIrqs);
296        let iis_dev = SpiDevice::new(spi1_bus, nss1); // Todo implement new funciton
297        let iis_sensor = peripherals::iis2mdc::Iis2mdcSensor {
298            dev: iis_dev,
299            drdy: drdy1,
300        }; // Todo implement new funciton
301
302        // DPS210 Baro
303        let nss2 = Output::new(p.PC7, Level::High, Speed::Low);
304        let drdy2 = ExtiInput::new(p.PG2, p.EXTI2, Pull::Down, BoardIrqs);
305        let dps_dev = SpiDevice::new(spi1_bus, nss2);
306        let dps_sensor = peripherals::dps310::Dps310Sensor {
307            dev: dps_dev,
308            drdy: drdy2,
309            three_wire: true,
310        }; // Todo implement new funciton
311
312        // SPI2 Bus ///////////////////////////////////////////
313        let mut spi2_config: embassy_stm32::spi::Config = spi::Config::default();
314        spi2_config.frequency = mhz(1);
315        spi2_config.mode = spi::MODE_3;
316        spi2_config.bit_order = spi::BitOrder::MsbFirst;
317        spi2_config.miso_pull = embassy_stm32::gpio::Pull::Up;
318        let spi2 = spi::Spi::new(
319            p.SPI2,
320            p.PB10,
321            p.PC3,
322            p.PC2,
323            p.DMA1_CH2,
324            p.DMA1_CH3,
325            BoardIrqs,
326            spi2_config,
327        );
328        let spi2_bus = Mutex::new(spi2);
329        let _spi2_bus = SPI2_BUS.init(spi2_bus);
330
331        // I2C1 Bus  ///////////////////////////////////////////
332        let mut i2c_config = i2c::Config::default();
333        i2c_config.scl_pullup = true;
334        i2c_config.sda_pullup = true;
335        i2c_config.frequency = Hertz(100_000);
336        let i2c1 = i2c::I2c::new(
337            p.I2C1, p.PB8, p.PB9, p.DMA2_CH2, p.DMA2_CH3, BoardIrqs, i2c_config,
338        );
339        let i2c1_bus = Mutex::new(i2c1);
340        let i2c1_bus = I2C1_BUS.init(i2c1_bus);
341
342        // DLHRL20G Pitot
343        let drdy0 = ExtiInput::new(p.PA15, p.EXTI15, Pull::Down, BoardIrqs);
344        let dlhr_dev = I2cDevice::new(i2c1_bus);
345        let _dlhr_sensor = peripherals::dlhrl20g::DlhrL20GSensor {
346            dev: dlhr_dev,
347            drdy: drdy0,
348        };
349
350        // Telemetry UART
351        let mut uart2config = usart::Config::default();
352        uart2config.baudrate = 921600;
353        let usart2 = Uart::new(
354            p.USART2,
355            p.PD6,
356            p.PD5,
357            p.DMA2_CH4,
358            p.DMA2_CH5,
359            BoardIrqs,
360            uart2config,
361        )
362        .unwrap();
363        let (usart2_tx, usart2_rx) = usart2.split();
364
365        let telem2_rx = peripherals::telem::TelemRx {
366            uart_rx: usart2_rx,
367            byte_processor: stm_32::peripherals::telem::BasicProcessor {},
368        };
369
370        let telem2_tx = peripherals::telem::TelemTx { uart_tx: usart2_tx };
371
372        // VCP
373        static EP_BUF_CELL: StaticCell<[u8; 256]> = StaticCell::new();
374        let mut config = embassy_stm32::usb::Config::default();
375        config.vbus_detection = true;
376        let driver = Driver::new_fs(
377            p.USB_OTG_FS,
378            Irqs,
379            p.PA12,
380            p.PA11,
381            EP_BUF_CELL.init([0u8; 256]),
382            config,
383        );
384        let vcp = peripherals::vcp::Vcp {
385            driver,
386            byte_processor: stm_32::peripherals::vcp::BasicProcessor {},
387        };
388
389        // P1 Priority Task for Rx Tememetry
390        interrupt::SAI1.set_priority(Priority::P1);
391        let spawner1 = P1_EXECUTOR.start(interrupt::SAI1);
392        spawn_task(&spawner1, peripherals::telem::task_rx(telem2_rx));
393        spawn_task(&spawner1, peripherals::vcp::task(vcp));
394
395        //GPS USART7
396        let mut uart7config = usart::Config::default();
397        uart7config.baudrate = 9600u32;
398        let uart7 = Uart::new(
399            p.UART7,
400            p.PE7,
401            p.PE8,
402            p.DMA2_CH6,
403            p.DMA2_CH7,
404            BoardIrqs,
405            uart7config,
406        )
407        .unwrap();
408        let ublox_sensor = peripherals::ublox::UbloxSensor {
409            uart: uart7,
410            protocol: peripherals::ublox::Protocol::M8,
411            baudrate: peripherals::ublox::Bitrate::Baud230400,
412            nav_period_ms: 100u16,
413        };
414        let drdy_pps = ExtiInput::new(p.PE0, p.EXTI0, Pull::Down, BoardIrqs); // Gyro
415        let pps_sensor = peripherals::pps::PpsSensor { pps: drdy_pps };
416
417        // S.Bus USART1
418        // Sbus only uses Rx.
419        let mut uart1config = usart::Config::default();
420        uart1config.baudrate = 100000u32;
421        uart1config.parity = usart::Parity::ParityEven;
422        uart1config.stop_bits = usart::StopBits::STOP2;
423        uart1config.invert_rx = true;
424        uart1config.invert_tx = true;
425        uart1config.data_bits = usart::DataBits::DataBits8;
426
427        let usart1 = Uart::new(
428            p.USART1,
429            p.PB7,
430            p.PB6,
431            p.DMA1_CH4,
432            p.DMA1_CH5,
433            BoardIrqs,
434            uart1config,
435        )
436        .unwrap();
437        let (_uart1_tx, uart1_rx) = usart1.split();
438        let sbus_rx = peripherals::sbus::SbusRC { uart: uart1_rx };
439
440        // uSD SDMMC1
441        let sdmmc1 = sdmmc::Sdmmc::new_4bit(
442            p.SDMMC1,
443            BoardIrqs,
444            p.PC12,
445            p.PD2,
446            p.PC8,
447            p.PC9,
448            p.PC10,
449            p.PC11,
450            Default::default(),
451        );
452
453        // SPI4 Bus ///////////////////////////////////////////
454        let mut spi4_config: embassy_stm32::spi::Config = spi::Config::default();
455        spi4_config.frequency = mhz(2);
456        spi4_config.mode = spi::MODE_3;
457        spi4_config.bit_order = spi::BitOrder::MsbFirst;
458        spi4_config.miso_pull = embassy_stm32::gpio::Pull::Up;
459        let spi4 = spi::Spi::new(
460            p.SPI4,
461            p.PE2,
462            p.PE6,
463            p.PE5,
464            p.DMA2_CH0,
465            p.DMA2_CH1,
466            BoardIrqs,
467            spi4_config,
468        );
469        let spi4_ = Mutex::new(spi4);
470        let spi4_bus = SPI4_BUS.init(spi4_);
471
472        // BMI08x
473        let nss_bmi08x_a = Output::new(p.PE3, Level::High, Speed::Low); // Accel
474        let drdy_bmi08x_a = ExtiInput::new(p.PE4, p.EXTI4, Pull::Down, BoardIrqs); // Accel
475        let nss_bmi08x_g = Output::new(p.PF8, Level::High, Speed::Low); // Gyro
476        let drdy_bmi08x_g = ExtiInput::new(p.PF7, p.EXTI7, Pull::Down, BoardIrqs); // Gyro
477        let bmi08x_dev_a = SpiDevice::new(spi4_bus, nss_bmi08x_a);
478        let bmi08x_dev_g = SpiDevice::new(spi4_bus, nss_bmi08x_g);
479        let jumper: Output<'static> = Output::new(p.PF15, Level::High, Speed::Low); // Bridge pin
480
481        let bmi08x_sensor = peripherals::bmi08x::Bmi08xSensor {
482            dev_a: bmi08x_dev_a,
483            dev_g: bmi08x_dev_g,
484            drdy_a: drdy_bmi08x_a,
485            drdy_g: drdy_bmi08x_g,
486            jumper: jumper,
487            range_a: peripherals::bmi08x::AccelRange::Bmi088(
488                peripherals::bmi08x::AccelRange088::Max24G,
489            ),
490            range_g: peripherals::bmi08x::GyroRange::Max500dps,
491            sample_rate: peripherals::bmi08x::SampleRate::Odr400Hz,
492        };
493
494        // P2 Priority Task for Gyros
495        interrupt::SAI2.set_priority(Priority::P2);
496        let spawner2 = P2_EXECUTOR.start(interrupt::SAI2);
497        spawn_task(&spawner2, peripherals::bmi08x::task(bmi08x_sensor));
498
499        // Detect GPIO input.
500        let usd_detect = embassy_stm32::gpio::Input::new(p.PG3, Pull::None);
501        let usd_card = peripherals::sd_card::SdCard {
502            sdmmc: sdmmc1,
503            detect: usd_detect,
504        };
505
506        // P3 Priority Task for Polled Peripherals
507        interrupt::SAI3.set_priority(Priority::P3);
508        let spawner3 = P3_EXECUTOR.start(interrupt::SAI3);
509        //spawner3
510        //    .spawn(peripherals::dlhrl20g::task(dlhr_sensor))
511        //    .unwrap();
512        spawn_task(&spawner3, peripherals::iis2mdc::task(iis_sensor));
513        spawn_task(&spawner3, peripherals::dps310::task(dps_sensor));
514        spawn_task(&spawner3, peripherals::ublox::task(ublox_sensor));
515        spawn_task(&spawner3, peripherals::pps::task(pps_sensor));
516        spawn_task(&spawner3, peripherals::sbus::task(sbus_rx));
517
518        // P4 Priority for Tx Telemetry
519        interrupt::SAI4.set_priority(Priority::P4);
520        let spawner4 = P4_EXECUTOR.start(interrupt::SAI4);
521        spawn_task(&spawner4, peripherals::telem::task_tx(telem2_tx));
522        spawn_task(&spawner4, peripherals::sd_card::task(usd_card));
523
524        // SERVOS + TIMERS
525        // TIM1
526        let ch0_pin = PwmPin::<_, embassy_stm32::timer::Ch1>::new(p.PE9, OutputType::PushPull);
527        let ch1_pin = PwmPin::<_, embassy_stm32::timer::Ch2>::new(p.PE11, OutputType::PushPull);
528        let ch2_pin = PwmPin::<_, embassy_stm32::timer::Ch3>::new(p.PE13, OutputType::PushPull);
529        let ch3_pin = PwmPin::<_, embassy_stm32::timer::Ch4>::new(p.PE14, OutputType::PushPull);
530        // TIM4
531        let ch4_pin = PwmPin::<_, embassy_stm32::timer::Ch1>::new(p.PD12, OutputType::PushPull);
532        let ch5_pin = PwmPin::<_, embassy_stm32::timer::Ch2>::new(p.PD13, OutputType::PushPull);
533        let ch6_pin = PwmPin::<_, embassy_stm32::timer::Ch3>::new(p.PD14, OutputType::PushPull);
534        let ch7_pin = PwmPin::<_, embassy_stm32::timer::Ch4>::new(p.PD15, OutputType::PushPull);
535        // TIM2
536        let ch8_pin = PwmPin::<_, embassy_stm32::timer::Ch1>::new(p.PA0, OutputType::PushPull);
537        let ch9_pin = PwmPin::<_, embassy_stm32::timer::Ch4>::new(p.PB11, OutputType::PushPull);
538        // TIM3
539        let ch10_pin = PwmPin::<_, embassy_stm32::timer::Ch1>::new(p.PC6, OutputType::PushPull);
540        let ch11_pin = PwmPin::<_, embassy_stm32::timer::Ch4>::new(p.PB1, OutputType::PushPull);
541
542        let timer1 = SimplePwm::new(
543            p.TIM1,
544            Some(ch0_pin),
545            Some(ch1_pin),
546            Some(ch2_pin),
547            Some(ch3_pin),
548            Hertz::hz(50),
549            Default::default(),
550        );
551        let timer4 = SimplePwm::new(
552            p.TIM4,
553            Some(ch4_pin),
554            Some(ch5_pin),
555            Some(ch6_pin),
556            Some(ch7_pin),
557            Hertz::hz(50),
558            Default::default(),
559        );
560        let timer2 = SimplePwm::new(
561            p.TIM2,
562            Some(ch8_pin),
563            None,
564            None,
565            Some(ch9_pin),
566            Hertz::hz(50),
567            Default::default(),
568        );
569        let timer3 = SimplePwm::new(
570            p.TIM3,
571            Some(ch10_pin),
572            None,
573            None,
574            Some(ch11_pin),
575            Hertz::hz(50),
576            Default::default(),
577        );
578
579        let timer1 = peripherals::pwm::TimerEnum::TIM1(timer1);
580        let timer4 = peripherals::pwm::TimerEnum::TIM4(timer4);
581        let timer2 = peripherals::pwm::TimerEnum::TIM2(timer2);
582        let timer3 = peripherals::pwm::TimerEnum::TIM3(timer3);
583
584        let timers: [peripherals::pwm::TimerEnum; 4] = [timer1, timer2, timer3, timer4];
585
586        let mut servos = peripherals::pwm::ServoMonstrosity::new(
587            timers,
588            [
589                (0, peripherals::pwm::TimerChannel::Ch1), //TIM1, channels 1-4
590                (0, peripherals::pwm::TimerChannel::Ch2), // -
591                (0, peripherals::pwm::TimerChannel::Ch3), // -
592                (0, peripherals::pwm::TimerChannel::Ch4), // -
593                (1, peripherals::pwm::TimerChannel::Ch1), //TIM2, channels 1, 4
594                (1, peripherals::pwm::TimerChannel::Ch4), // -
595                (2, peripherals::pwm::TimerChannel::Ch1), //TIM3, channels 1, 4
596                (2, peripherals::pwm::TimerChannel::Ch4), // -
597                (3, peripherals::pwm::TimerChannel::Ch1), //TIM4, channels 1-4
598                (3, peripherals::pwm::TimerChannel::Ch2), // -
599                (3, peripherals::pwm::TimerChannel::Ch3), // -
600                (3, peripherals::pwm::TimerChannel::Ch4), // -
601            ],
602        );
603
604        // disable all channels at start
605        for i in 0..servos.len() {
606            let _ = servos.disable(i);
607        }
608
609        // Setup Probe GPIO's
610        let probe = [
611            Output::new(p.PC0, Level::Low, Speed::Low),
612            Output::new(p.PB2, Level::Low, Speed::Low),
613            Output::new(p.PF2, Level::Low, Speed::Low),
614            Output::new(p.PG0, Level::Low, Speed::Low),
615        ];
616
617        (
618            Board {
619                _probe: probe,
620                start_time,
621                pending_reset_to_bootloader: None,
622            },
623            BoardPwmDriver::new(servos),
624        )
625    }
626}
nucleo/board.rs

nucleo/
board.rs
