pixracerpro/
board.rs

1// ******************************************************************************
2// * File     : boards/pixracerpro/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::sensors::SensorBus;
41
42use embassy_time::Delay;
43use stm_32::cortex_m::prelude::_embedded_hal_blocking_delay_DelayMs;
44#[cfg(not(feature = "scope-timing-pins"))]
45use stm_32::cortex_m::prelude::_embedded_hal_blocking_delay_DelayUs;
46use stm_32::peripherals;
47use stm_32::peripherals::pwm::PixRacerProServoMonstrosity;
48use stm_32::*;
49
50include!("../../../platforms/stm_32/stm32h7x3_common.rs");
51
52static mut PARAM_STORE: Option<Params> = None;
53
54#[cfg(feature = "sensor-poll-diagnostics")]
55mod sensor_poll_diagnostics {
56    use core::sync::atomic::{AtomicU32, Ordering};
57
58    use veloxity_core::{errors::SensorError, math::FlightFloat};
59
60    #[unsafe(no_mangle)]
61    pub static VELOXITY_PIXRACER_DIAG_IMU: AtomicU32 = AtomicU32::new(0);
62    #[unsafe(no_mangle)]
63    pub static VELOXITY_PIXRACER_DIAG_IMU_ERR: AtomicU32 = AtomicU32::new(0);
64    #[unsafe(no_mangle)]
65    pub static VELOXITY_PIXRACER_DIAG_MAG: AtomicU32 = AtomicU32::new(0);
66    #[unsafe(no_mangle)]
67    pub static VELOXITY_PIXRACER_DIAG_MAG_ERR: AtomicU32 = AtomicU32::new(0);
68    #[unsafe(no_mangle)]
69    pub static VELOXITY_PIXRACER_DIAG_BARO: AtomicU32 = AtomicU32::new(0);
70    #[unsafe(no_mangle)]
71    pub static VELOXITY_PIXRACER_DIAG_BARO_ERR: AtomicU32 = AtomicU32::new(0);
72    #[unsafe(no_mangle)]
73    pub static VELOXITY_PIXRACER_DIAG_PITOT: AtomicU32 = AtomicU32::new(0);
74    #[unsafe(no_mangle)]
75    pub static VELOXITY_PIXRACER_DIAG_PITOT_ERR: AtomicU32 = AtomicU32::new(0);
76    #[unsafe(no_mangle)]
77    pub static VELOXITY_PIXRACER_DIAG_RANGE: AtomicU32 = AtomicU32::new(0);
78    #[unsafe(no_mangle)]
79    pub static VELOXITY_PIXRACER_DIAG_RANGE_ERR: AtomicU32 = AtomicU32::new(0);
80    #[unsafe(no_mangle)]
81    pub static VELOXITY_PIXRACER_DIAG_GNSS: AtomicU32 = AtomicU32::new(0);
82    #[unsafe(no_mangle)]
83    pub static VELOXITY_PIXRACER_DIAG_GNSS_ERR: AtomicU32 = AtomicU32::new(0);
84    #[unsafe(no_mangle)]
85    pub static VELOXITY_PIXRACER_DIAG_RC: AtomicU32 = AtomicU32::new(0);
86    #[unsafe(no_mangle)]
87    pub static VELOXITY_PIXRACER_DIAG_RC_ERR: AtomicU32 = AtomicU32::new(0);
88
89    pub fn record_bus<R: FlightFloat>(sensors: &veloxity_core::sensors::SensorBus<R>) {
90        record(
91            &sensors.imu,
92            &VELOXITY_PIXRACER_DIAG_IMU,
93            &VELOXITY_PIXRACER_DIAG_IMU_ERR,
94        );
95        record(
96            &sensors.mag,
97            &VELOXITY_PIXRACER_DIAG_MAG,
98            &VELOXITY_PIXRACER_DIAG_MAG_ERR,
99        );
100        record(
101            &sensors.baro,
102            &VELOXITY_PIXRACER_DIAG_BARO,
103            &VELOXITY_PIXRACER_DIAG_BARO_ERR,
104        );
105        record(
106            &sensors.pitot,
107            &VELOXITY_PIXRACER_DIAG_PITOT,
108            &VELOXITY_PIXRACER_DIAG_PITOT_ERR,
109        );
110        record(
111            &sensors.range,
112            &VELOXITY_PIXRACER_DIAG_RANGE,
113            &VELOXITY_PIXRACER_DIAG_RANGE_ERR,
114        );
115        record(
116            &sensors.gnss,
117            &VELOXITY_PIXRACER_DIAG_GNSS,
118            &VELOXITY_PIXRACER_DIAG_GNSS_ERR,
119        );
120        record(
121            &sensors.rc,
122            &VELOXITY_PIXRACER_DIAG_RC,
123            &VELOXITY_PIXRACER_DIAG_RC_ERR,
124        );
125    }
126
127    fn record<T>(sample: &Option<Result<T, SensorError>>, count: &AtomicU32, errors: &AtomicU32) {
128        if let Some(result) = sample {
129            count.fetch_add(1, Ordering::Relaxed);
130            if result.is_err() {
131                errors.fetch_add(1, Ordering::Relaxed);
132            }
133        }
134    }
135}
136
137fn spawn_task<S: Send>(
138    spawner: &embassy_executor::SendSpawner,
139    token: Result<embassy_executor::SpawnToken<S>, embassy_executor::SpawnError>,
140) {
141    spawner.spawn(token.expect("failed to allocate Embassy task"));
142}
143
144pub struct Board {
145    _probe: [Output<'static>; 3],
146    pub start_time: embassy_time::Instant,
147    test_pin_1: Output<'static>,
148    test_pin_2: Output<'static>,
149    pending_reset_to_bootloader: Option<bool>,
150    #[cfg(feature = "sensor-poll-diagnostics")]
151    last_sbus_diag_ms: u32,
152    #[cfg(feature = "sensor-poll-diagnostics")]
153    sbus_rc_drains: u32,
154    #[cfg(feature = "sensor-poll-diagnostics")]
155    sbus_last_rc_status: u32,
156    #[cfg(feature = "sensor-poll-diagnostics")]
157    sbus_last_rc_lol: bool,
158}
159
160impl BoardIo for Board {
161    fn set_test_pin_1(&mut self, high: bool) {
162        if high {
163            self.test_pin_1.set_high();
164        } else {
165            self.test_pin_1.set_low();
166        }
167    }
168
169    fn set_test_pin_2(&mut self, high: bool) {
170        if high {
171            self.test_pin_2.set_high();
172        } else {
173            self.test_pin_2.set_low();
174        }
175    }
176
177    fn update_sensor_bus<R: FlightFloat>(&mut self, sensors: &mut SensorBus<R>) {
178        sensors.clear();
179        sensors.imu = peripherals::bmi08x::IMU_SIGNAL
180            .try_take()
181            .map(|result| result.map(|packet| packet.cast()));
182        sensors.mag = peripherals::ist8308::MAG_SIGNAL.try_take();
183        sensors.baro = peripherals::dps310::BARO_SIGNAL.try_take();
184        sensors.pitot = peripherals::ms4525::PITOT_SIGNAL.try_take();
185        sensors.range = peripherals::llv3hp::RANGE_SIGNAL.try_take();
186        sensors.gnss = peripherals::ublox::GNSS_SIGNAL.try_take();
187        sensors.rc = peripherals::sbus::RC_SIGNAL.try_take();
188        #[cfg(feature = "sensor-poll-diagnostics")]
189        self.record_sbus_rc_drain(&sensors.rc);
190        #[cfg(feature = "sensor-poll-diagnostics")]
191        sensor_poll_diagnostics::record_bus(sensors);
192        #[cfg(feature = "sensor-poll-diagnostics")]
193        self.log_sbus_diagnostics_if_due();
194        #[cfg(not(feature = "scope-timing-pins"))]
195        if sensors.imu.is_some() {
196            let mut delay = Delay;
197            self.set_test_pin_1(true);
198            delay.delay_us(1u32);
199            self.set_test_pin_1(false);
200        }
201    }
202
203    fn imu_pending(&self) -> bool {
204        peripherals::bmi08x::IMU_SIGNAL.signaled()
205    }
206
207    fn update_imu_sensor<R: FlightFloat>(&mut self, sensors: &mut SensorBus<R>) {
208        sensors.clear();
209        sensors.imu = peripherals::bmi08x::IMU_SIGNAL
210            .try_take()
211            .map(|result| result.map(|packet| packet.cast()));
212        #[cfg(feature = "sensor-poll-diagnostics")]
213        sensor_poll_diagnostics::record_bus(sensors);
214        #[cfg(not(feature = "scope-timing-pins"))]
215        if sensors.imu.is_some() {
216            let mut delay = Delay;
217            self.set_test_pin_1(true);
218            delay.delay_us(1u32);
219            self.set_test_pin_1(false);
220        }
221    }
222
223    fn update_service_sensor_bus<R: FlightFloat>(&mut self, sensors: &mut SensorBus<R>) {
224        sensors.clear();
225        sensors.mag = peripherals::ist8308::MAG_SIGNAL.try_take();
226        sensors.baro = peripherals::dps310::BARO_SIGNAL.try_take();
227        sensors.pitot = peripherals::ms4525::PITOT_SIGNAL.try_take();
228        sensors.range = peripherals::llv3hp::RANGE_SIGNAL.try_take();
229        sensors.gnss = peripherals::ublox::GNSS_SIGNAL.try_take();
230        sensors.rc = peripherals::sbus::RC_SIGNAL.try_take();
231        #[cfg(feature = "sensor-poll-diagnostics")]
232        self.record_sbus_rc_drain(&sensors.rc);
233        #[cfg(feature = "sensor-poll-diagnostics")]
234        sensor_poll_diagnostics::record_bus(sensors);
235        #[cfg(feature = "sensor-poll-diagnostics")]
236        self.log_sbus_diagnostics_if_due();
237    }
238
239    fn serial_rx_read(&mut self, buf: &mut [u8]) -> Option<Result<usize, errors::TelemError>> {
240        #[cfg(feature = "usb-vcp-serial")]
241        match peripherals::vcp::VCP_RX.try_read(buf) {
242            Ok(n) => {
243                return Some(Ok(n));
244            }
245            Err(embassy_sync::pipe::TryReadError::Empty) => {
246                // This is NORMAL. Do not log an error.
247                // Return Ok(0) to indicate "no bytes right now" without erroring.
248                return Some(Ok(0));
249            }
250        }
251
252        #[cfg(not(feature = "usb-vcp-serial"))]
253        match peripherals::telem::TELEM_RX.try_read(buf) {
254            Ok(n) => {
255                return Some(Ok(n));
256            }
257            Err(embassy_sync::pipe::TryReadError::Empty) => {
258                // This is NORMAL. Do not log an error.
259                // Return Ok(0) to indicate "no bytes right now" without erroring.
260                return Some(Ok(0));
261            }
262        }
263    }
264    fn serial_tx_write(&mut self, bytes: &[u8]) -> Option<Result<usize, errors::TelemError>> {
265        #[cfg(feature = "usb-vcp-serial")]
266        {
267            let mut n = 0;
268            let len = bytes.len();
269            loop {
270                match peripherals::vcp::VCP_TX.try_write(&bytes[n..len]) {
271                    Ok(wrote) => {
272                        if wrote == (len - n) {
273                            break;
274                        } else {
275                            n += wrote;
276                        }
277                    }
278                    Err(_) => {
279                        return Some(Err(errors::TelemError::GenericTelemError(
280                            "Error Writing USB VCP Packet!",
281                        )));
282                    }
283                }
284            }
285            return Some(Ok(len));
286        }
287
288        #[cfg(not(feature = "usb-vcp-serial"))]
289        {
290            let mut n = 0;
291            let len = bytes.len();
292            loop {
293                match peripherals::telem::TELEM_TX.try_write(&bytes[n..len]) {
294                    Ok(wrote) => {
295                        if wrote == (len - n) {
296                            break;
297                        } else {
298                            n += wrote;
299                        }
300                    }
301                    Err(_) => {
302                        return Some(Err(errors::TelemError::GenericTelemError(
303                            "Error Writing Telem Packet!",
304                        )));
305                    }
306                }
307            }
308            Some(Ok(len))
309        }
310    }
311
312    fn clock_millis(&self) -> u32 {
313        self.start_time.elapsed().as_millis() as u32
314    }
315
316    fn clock_micros(&self) -> u64 {
317        self.start_time.elapsed().as_micros() as u64
318    }
319
320    fn read_params(&mut self, params: &mut Params) -> bool {
321        let Some(stored) = (unsafe { PARAM_STORE }) else {
322            return false;
323        };
324        *params = stored;
325        true
326    }
327
328    fn write_params(&mut self, params: &Params) -> bool {
329        unsafe {
330            PARAM_STORE = Some(*params);
331        }
332        true
333    }
334
335    fn reboot(&mut self) -> bool {
336        self.pending_reset_to_bootloader = Some(false);
337        true
338    }
339
340    fn reboot_to_bootloader(&mut self) -> bool {
341        self.pending_reset_to_bootloader = Some(true);
342        true
343    }
344
345    fn run_deferred_board_actions(&mut self) {
346        if self.pending_reset_to_bootloader.take().is_some() {
347            let mut delay = Delay;
348            delay.delay_ms(20u32);
349            stm_32::cortex_m::peripheral::SCB::sys_reset();
350        }
351    }
352}
353
354impl Board {
355    pub fn new() -> (Board, PixRacerProServoMonstrosity) {
356        let p: EMBASSY_Peripherals = embassy_stm32::init(clock_config(24));
357
358        let start_time = embassy_time::Instant::now();
359
360        // SPI1 (ROSflight uses for internal ICM, unused here)
361        let mut spi1_config: embassy_stm32::spi::Config = spi::Config::default();
362        spi1_config.frequency = mhz(16); // Phil recommends not running over 4 Mbps
363        spi1_config.mode = spi::MODE_3;
364        spi1_config.bit_order = spi::BitOrder::MsbFirst;
365        spi1_config.miso_pull = embassy_stm32::gpio::Pull::Up;
366        let spi1 = spi::Spi::new(
367            p.SPI1,
368            p.PA5,
369            p.PA7,
370            p.PA6,
371            p.DMA1_CH0,
372            p.DMA1_CH1,
373            BoardIrqs,
374            spi1_config,
375        );
376        let spi1_bus = Mutex::new(spi1);
377        let _spi1_bus = SPI1_BUS.init(spi1_bus);
378
379        // SPI2 (internal DPS310)
380        let mut spi2_config: embassy_stm32::spi::Config = spi::Config::default();
381        spi2_config.frequency = mhz(16); // Phil recommends not running over 4 Mbps
382        spi2_config.mode = spi::MODE_3;
383        spi2_config.bit_order = spi::BitOrder::MsbFirst;
384        spi2_config.miso_pull = embassy_stm32::gpio::Pull::Up;
385        let spi2 = spi::Spi::new(
386            p.SPI2,
387            p.PB10, // Sck
388            p.PB15, // Mosi
389            p.PB14, // Miso
390            p.DMA1_CH2,
391            p.DMA1_CH3,
392            BoardIrqs,
393            spi2_config,
394        );
395        let spi2_bus = Mutex::new(spi2);
396        let spi2_bus = SPI2_BUS.init(spi2_bus);
397
398        // DPS310 Baro (Internal)
399        let nss2 = Output::new(p.PD7, Level::High, Speed::Low);
400        // these pins are generalized for the IC
401        let drdy2 = ExtiInput::new(p.PD15, p.EXTI15, Pull::Down, BoardIrqs);
402        let dps_dev = SpiDevice::new(spi2_bus, nss2);
403        let dps_sensor = peripherals::dps310::Dps310Sensor {
404            dev: dps_dev,
405            drdy: drdy2,
406            three_wire: false,
407        };
408
409        // I2C1 Bus (ist8308 Mag and MS4525 Pitot on external)
410        let mut i2c_config = i2c::Config::default();
411        i2c_config.scl_pullup = true;
412        i2c_config.sda_pullup = true;
413        i2c_config.frequency = Hertz(100_000);
414        let i2c1 = i2c::I2c::new(
415            p.I2C1, p.PB8, p.PB9, p.DMA2_CH2, p.DMA2_CH3, BoardIrqs, i2c_config,
416        );
417        let i2c1_bus = Mutex::new(i2c1);
418        let i2c1_bus = I2C1_BUS.init(i2c1_bus);
419
420        // IST8308 Magnetometer (External)
421        let ist8303_sensor = peripherals::ist8308::Ist8308Sensor {
422            dev: I2cDevice::new(i2c1_bus),
423        };
424
425        // MS4525 Pitot (External)
426        let ms4525_sensor = peripherals::ms4525::Ms4525Sensor {
427            dev: I2cDevice::new(i2c1_bus),
428        };
429
430        let llv3hp_sensor = peripherals::llv3hp::Llv3hpSensor {
431            dev: I2cDevice::new(i2c1_bus),
432        };
433
434        // Companion Computer UART - Austin's documentation references uart3 instead of 2 for companion computer
435        let mut uart3config = usart::Config::default();
436        uart3config.rx_pull = Pull::Up;
437        uart3config.baudrate = 921600;
438        let uart3 = Uart::new(
439            p.USART3,
440            p.PD9,
441            p.PD8,
442            p.DMA2_CH4,
443            p.DMA2_CH5,
444            BoardIrqs,
445            uart3config,
446        )
447        .unwrap();
448        let (uart3_tx, uart3_rx) = uart3.split();
449
450        let telem3_rx = peripherals::telem::TelemRx {
451            uart_rx: uart3_rx,
452            byte_processor: stm_32::peripherals::telem::BasicProcessor {},
453        };
454
455        let telem3_tx = peripherals::telem::TelemTx { uart_tx: uart3_tx };
456
457        // VCP
458        static EP_BUF_CELL: StaticCell<[u8; 256]> = StaticCell::new();
459        let mut config = embassy_stm32::usb::Config::default();
460        config.vbus_detection = true;
461        let driver = Driver::new_fs(
462            p.USB_OTG_FS,
463            Irqs,
464            p.PA12,
465            p.PA11,
466            EP_BUF_CELL.init([0u8; 256]),
467            config,
468        );
469        let vcp = peripherals::vcp::Vcp {
470            driver,
471            byte_processor: stm_32::peripherals::vcp::BasicProcessor {},
472        };
473
474        // USART4 (external GPS)
475        let mut uart4config = usart::Config::default();
476        uart4config.baudrate = 9600u32;
477        uart4config.rx_pull = Pull::Up;
478        let uart4 = Uart::new(
479            p.UART4,
480            p.PA1,
481            p.PA0,
482            p.DMA2_CH6,
483            p.DMA2_CH7,
484            BoardIrqs,
485            uart4config,
486        )
487        .unwrap();
488
489        // UBlox NEO-M9N GNSS (External)
490        let ublox_sensor = peripherals::ublox::UbloxSensor {
491            uart: uart4,
492            protocol: peripherals::ublox::Protocol::M8,
493            baudrate: peripherals::ublox::Bitrate::Baud230400,
494            nav_period_ms: 100u16,
495        };
496        let drdy_pps = ExtiInput::new(p.PG12, p.EXTI12, Pull::Down, BoardIrqs);
497        let pps_sensor = peripherals::pps::PpsSensor { pps: drdy_pps };
498
499        // S.Bus USART6
500        // Sbus only uses Rx.
501        let mut uart6config = usart::Config::default();
502        uart6config.baudrate = 100000u32;
503        uart6config.parity = usart::Parity::ParityEven;
504        uart6config.stop_bits = usart::StopBits::STOP2;
505        uart6config.invert_rx = true;
506        uart6config.invert_tx = true;
507        uart6config.data_bits = usart::DataBits::DataBits8;
508
509        let usart6 = Uart::new(
510            p.USART6,
511            p.PC7,
512            p.PC6,
513            p.DMA1_CH4,
514            p.DMA1_CH5,
515            BoardIrqs,
516            uart6config,
517        )
518        .unwrap();
519        let (_uart6_tx, uart6_rx) = usart6.split();
520        let sbus_rx = peripherals::sbus::SbusRC { uart: uart6_rx };
521
522        // uSD SDMMC1
523        let sdmmc1 = sdmmc::Sdmmc::new_4bit(
524            p.SDMMC1,
525            BoardIrqs,
526            p.PC12,
527            p.PD2,
528            p.PC8,
529            p.PC9,
530            p.PC10,
531            p.PC11,
532            Default::default(),
533        );
534
535        // SPI5 (Internal BMI085)
536        let mut spi5_config: embassy_stm32::spi::Config = spi::Config::default();
537        spi5_config.frequency = mhz(2); // Phil recommends not running over 4 Mbps
538        spi5_config.mode = spi::MODE_3;
539        spi5_config.bit_order = spi::BitOrder::MsbFirst;
540        spi5_config.miso_pull = embassy_stm32::gpio::Pull::Up;
541        let spi5 = spi::Spi::new(
542            p.SPI5,
543            p.PF7,      // sck
544            p.PF9,      // mosi
545            p.PF8,      // miso
546            p.DMA1_CH6, // tx_dma
547            p.DMA1_CH7, // rx_dma
548            BoardIrqs,
549            spi5_config,
550        );
551        let spi5_ = Mutex::new(spi5);
552        let spi5_bus = SPI5_BUS.init(spi5_);
553
554        // BMI085 (Internal)
555        let nss_bmi08x_a = Output::new(p.PF6, Level::High, Speed::Low); // Accel
556        let drdy_bmi08x_a = ExtiInput::new(p.PF1, p.EXTI1, Pull::Down, BoardIrqs); // Accel
557        let nss_bmi08x_g = Output::new(p.PF10, Level::High, Speed::Low); // Gyro
558        let drdy_bmi08x_g = ExtiInput::new(p.PF3, p.EXTI3, Pull::Down, BoardIrqs); // Gyro
559        let bmi08x_dev_a = SpiDevice::new(spi5_bus, nss_bmi08x_a);
560        let bmi08x_dev_g = SpiDevice::new(spi5_bus, nss_bmi08x_g);
561        let jumper: Output<'static> = Output::new(p.PF2, Level::High, Speed::Low); // Bridge pin
562        let bmi08x_sensor = peripherals::bmi08x::Bmi08xSensor {
563            dev_a: bmi08x_dev_a,
564            dev_g: bmi08x_dev_g,
565            drdy_a: drdy_bmi08x_a,
566            drdy_g: drdy_bmi08x_g,
567            jumper: jumper,
568            range_a: peripherals::bmi08x::AccelRange::Bmi085(
569                peripherals::bmi08x::AccelRange085::Max16g,
570            ),
571            range_g: peripherals::bmi08x::GyroRange::Max500dps,
572            sample_rate: peripherals::bmi08x::SampleRate::Odr400Hz,
573        };
574
575        // Detect GPIO input.
576        let usd_detect = embassy_stm32::gpio::Input::new(p.PG3, Pull::None); // PG3 is not connected
577        let usd_card = peripherals::sd_card::SdCard {
578            sdmmc: sdmmc1,
579            detect: usd_detect,
580        };
581
582        // P1 Priority Task for Rx Telemetry
583        interrupt::SAI1.set_priority(Priority::P0);
584        let spawner1 = P1_EXECUTOR.start(interrupt::SAI1);
585        spawn_task(&spawner1, peripherals::bmi08x::task(bmi08x_sensor));
586
587        // P2 Priority Task for Gyros
588        interrupt::SAI2.set_priority(Priority::P2);
589        let spawner2 = P2_EXECUTOR.start(interrupt::SAI2);
590
591        // P2 VCP Task (Telemetry alternate)
592        spawn_task(&spawner2, peripherals::vcp::task(vcp));
593        spawn_task(&spawner2, peripherals::telem::task_rx(telem3_rx));
594
595        // P3 Priority Task for Polled Peripherals
596        interrupt::SAI3.set_priority(Priority::P3);
597        let spawner3 = P3_EXECUTOR.start(interrupt::SAI3);
598        spawn_task(&spawner3, peripherals::ist8308::task(ist8303_sensor));
599        spawn_task(&spawner3, peripherals::ms4525::task(ms4525_sensor));
600        spawn_task(&spawner3, peripherals::dps310::task(dps_sensor));
601        spawn_task(&spawner3, peripherals::ublox::task(ublox_sensor));
602        spawn_task(&spawner3, peripherals::pps::task(pps_sensor));
603        spawn_task(&spawner3, peripherals::sbus::task(sbus_rx));
604        spawn_task(&spawner3, peripherals::llv3hp::task(llv3hp_sensor));
605
606        // P4 Priority for Tx Telemetry
607        interrupt::SAI4.set_priority(Priority::P4);
608        let spawner4 = P4_EXECUTOR.start(interrupt::SAI4);
609        spawn_task(&spawner4, peripherals::telem::task_tx(telem3_tx));
610        spawn_task(&spawner4, peripherals::sd_card::task(usd_card));
611
612        // Only the four TIM1 motor outputs are mapped. PA15 is the Pixracer Pro
613        // buzzer PWM pin, and TIM4 aux outputs are intentionally left unmapped.
614        let tim1_ch1_pin = PwmPin::<_, embassy_stm32::timer::Ch1>::new(p.PE9, OutputType::PushPull);
615        let tim1_ch2_pin =
616            PwmPin::<_, embassy_stm32::timer::Ch2>::new(p.PE11, OutputType::PushPull);
617        let tim1_ch3_pin =
618            PwmPin::<_, embassy_stm32::timer::Ch3>::new(p.PE13, OutputType::PushPull);
619        let tim1_ch4_pin =
620            PwmPin::<_, embassy_stm32::timer::Ch4>::new(p.PE14, OutputType::PushPull);
621
622        let timer1 = SimplePwm::new(
623            p.TIM1,
624            Some(tim1_ch1_pin),
625            Some(tim1_ch2_pin),
626            Some(tim1_ch3_pin),
627            Some(tim1_ch4_pin),
628            Hertz::hz(400),
629            Default::default(),
630        );
631        let timer4 = SimplePwm::new(
632            p.TIM4,
633            None,
634            None,
635            None,
636            None,
637            Hertz::hz(400),
638            Default::default(),
639        );
640        let timer2 = SimplePwm::new(
641            p.TIM2,
642            None,
643            None,
644            None,
645            None,
646            Hertz::hz(400),
647            Default::default(),
648        );
649
650        let timer1 = peripherals::pwm::TimerEnum::TIM1(timer1);
651        let timer4 = peripherals::pwm::TimerEnum::TIM4(timer4);
652        let timer2 = peripherals::pwm::TimerEnum::TIM2(timer2);
653
654        let timers: [peripherals::pwm::TimerEnum; 3] = [timer1, timer2, timer4];
655
656        let servos = peripherals::pwm::PixRacerProServoMonstrosity::with_timer_kinds_and_dma(
657            timers,
658            [
659                (0, peripherals::pwm::TimerChannel::Ch1), // TIM1, channels 1-4
660                (0, peripherals::pwm::TimerChannel::Ch2), // -
661                (0, peripherals::pwm::TimerChannel::Ch3), // -
662                (0, peripherals::pwm::TimerChannel::Ch4), // -
663                (1, peripherals::pwm::TimerChannel::Ch1), // TIM2, channel 1
664                (2, peripherals::pwm::TimerChannel::Ch2), // TIM4, channels 2 and 3
665                (2, peripherals::pwm::TimerChannel::Ch3), // -
666            ],
667            [
668                peripherals::pwm::PwmTimerBlockKind::StandardOnly,
669                peripherals::pwm::PwmTimerBlockKind::StandardOnly,
670                peripherals::pwm::PwmTimerBlockKind::StandardOnly,
671            ],
672            [None, None, None],
673        );
674
675        // Test PWM pins
676        #[cfg_attr(feature = "scope-timing-pins", allow(unused_mut))]
677        let mut test_pin_1 = Output::new(p.PD11, Level::Low, Speed::VeryHigh);
678        #[cfg(not(feature = "scope-timing-pins"))]
679        test_pin_1.set_high();
680        #[cfg_attr(feature = "scope-timing-pins", allow(unused_mut))]
681        let mut test_pin_2 = Output::new(p.PD12, Level::Low, Speed::VeryHigh);
682        #[cfg(not(feature = "scope-timing-pins"))]
683        test_pin_2.set_high();
684
685        // Setup Probe GPIO's
686        let probe = [
687            Output::new(p.PG13, Level::Low, Speed::Low),
688            Output::new(p.PG9, Level::Low, Speed::Low),
689            Output::new(p.PG14, Level::Low, Speed::Low),
690            // Output::new(p.PG0, Level::Low, Speed::Low), // unknown
691        ];
692        (
693            Board {
694                _probe: probe,
695                start_time,
696                test_pin_1,
697                test_pin_2,
698                pending_reset_to_bootloader: None,
699                #[cfg(feature = "sensor-poll-diagnostics")]
700                last_sbus_diag_ms: 0,
701                #[cfg(feature = "sensor-poll-diagnostics")]
702                sbus_rc_drains: 0,
703                #[cfg(feature = "sensor-poll-diagnostics")]
704                sbus_last_rc_status: 0,
705                #[cfg(feature = "sensor-poll-diagnostics")]
706                sbus_last_rc_lol: false,
707            },
708            servos,
709        )
710    }
711
712    #[cfg(feature = "sensor-poll-diagnostics")]
713    fn record_sbus_rc_drain(
714        &mut self,
715        rc: &Option<Result<veloxity_core::packets::RcPacket, errors::SensorError>>,
716    ) {
717        if let Some(result) = rc {
718            self.sbus_rc_drains = self.sbus_rc_drains.wrapping_add(1);
719            if let Ok(packet) = result {
720                self.sbus_last_rc_status = packet.header.status as u32;
721                self.sbus_last_rc_lol = packet.lol;
722            }
723        }
724    }
725
726    #[cfg(feature = "sensor-poll-diagnostics")]
727    fn log_sbus_diagnostics_if_due(&mut self) {
728        let now_ms = self.clock_millis();
729        if now_ms.wrapping_sub(self.last_sbus_diag_ms) < 1_000 {
730            return;
731        }
732        self.last_sbus_diag_ms = now_ms;
733
734        let sbus = peripherals::sbus::diagnostics();
735        veloxity_core::log_info!(
736            "SBUS rx{} e{} sz{} n{} v{}",
737            sbus.read_ok,
738            sbus.read_err,
739            sbus.last_read_size,
740            sbus.size_25,
741            sbus.valid_frame
742        );
743        veloxity_core::log_info!(
744            "SBUS bh{} bf{} sig{} to{} dr{}",
745            sbus.bad_header,
746            sbus.bad_footer,
747            sbus.signal,
748            sbus.timeout,
749            self.sbus_rc_drains
750        );
751        veloxity_core::log_info!(
752            "SBUS st{} rst{} lol{}",
753            sbus.last_status,
754            self.sbus_last_rc_status,
755            self.sbus_last_rc_lol as u8
756        );
757    }
758}
pixracerpro/board.rs

pixracerpro/
board.rs
