platforms/stm32f7: add reusable F722 firmware
This commit is contained in:
Onur Özkan committed
1 parent
2ae66c49a8
commit
ac54c8308d
32 files changed
+4006
-248
No files matched your search
@@ -8,7 +8,7 @@
|
||||
/// A borrowed byte range prepared for a device-to-read DMA transfer.
|
||||
///
|
||||
/// This is a capability rather than a general byte-slice wrapper. It exists
|
||||
/// only after a board-selected policy proves DMA reachability and CPU-to-device
|
||||
/// only after a platform-selected policy proves DMA reachability and CPU-to-device
|
||||
/// visibility for the selected controller.
|
||||
pub struct DmaReadableBuffer<'a> {
|
||||
bytes: &'a [u8],
|
||||
|
||||
@@ -42,7 +42,7 @@ pub trait I2cBus {
|
||||
/// [`Self::start_read`] for the completed read transaction.
|
||||
fn complete_transaction(&mut self, completion: I2cCompletion, read: Option<&mut [u8]>);
|
||||
|
||||
/// Cancels an in-flight transaction after a board-selected timeout.
|
||||
/// Cancels an in-flight transaction after a platform-selected timeout.
|
||||
///
|
||||
/// Implementations must release controller-owned transaction state and
|
||||
/// arrange for any required STOP or peripheral recovery without waiting.
|
||||
|
||||
@@ -1,22 +0,0 @@
|
||||
[package]
|
||||
name = "kaptan-stm32f7"
|
||||
edition.workspace = true
|
||||
license.workspace = true
|
||||
repository.workspace = true
|
||||
rust-version.workspace = true
|
||||
version.workspace = true
|
||||
|
||||
[lints]
|
||||
workspace = true
|
||||
|
||||
[features]
|
||||
default = []
|
||||
stm32f722 = ["stm32f7/stm32f722"]
|
||||
|
||||
[dependencies]
|
||||
cortex-m = "0.7"
|
||||
kaptan-dma = { path = "../kaptan-dma" }
|
||||
kaptan-i2c = { path = "../kaptan-i2c" }
|
||||
kaptan-spi = { path = "../kaptan-spi" }
|
||||
kaptan-time = { path = "../kaptan-time" }
|
||||
stm32f7 = { version = "0.16", default-features = false }
|
||||
@@ -1,14 +0,0 @@
|
||||
//! STM32F7 peripheral drivers used by Kaptan boards.
|
||||
//!
|
||||
//! This crate is organized by MCU variant and peripheral route. A board chooses
|
||||
//! an MCU feature, owns its pin muxing and RTIC interrupt graph, then composes
|
||||
//! the typed peripheral drivers from the selected module. Generic telemetry
|
||||
//! policy and frame ownership deliberately do not belong here.
|
||||
|
||||
#![no_std]
|
||||
|
||||
#[cfg(not(feature = "stm32f722"))]
|
||||
compile_error!("select one supported STM32F7 variant feature, such as `stm32f722`");
|
||||
|
||||
#[cfg(feature = "stm32f722")]
|
||||
pub mod stm32f722;
|
||||
@@ -1,27 +0,0 @@
|
||||
//! STM32F722 peripheral implementations selected by supported boards.
|
||||
//!
|
||||
//! This module exports only typed peripheral building blocks. A board selects
|
||||
//! pins, clocks, interrupt priorities, and static ownership.
|
||||
|
||||
mod clock;
|
||||
mod dma_buffer;
|
||||
mod gpio;
|
||||
mod i2c1;
|
||||
mod spi1;
|
||||
mod tim2;
|
||||
mod time;
|
||||
mod usart;
|
||||
mod usart1_tx_dma2_stream7;
|
||||
|
||||
pub use clock::{ClockProfileError, select_pll216_profile};
|
||||
pub use dma_buffer::DcacheDisabledDmaMemory;
|
||||
pub use gpio::{
|
||||
ActiveLowLedPd2, configure_exti4_pin_pa4, configure_i2c1_pins_pb8_pb9,
|
||||
configure_spi1_pins_pa5_pa6_pa7, configure_usart1_rx_pin_pa10, configure_usart1_tx_pin_pa9,
|
||||
};
|
||||
pub use i2c1::{I2c1, I2c1Config, I2c1Error};
|
||||
pub use spi1::{Spi1, Spi1Error};
|
||||
pub use tim2::{Tim2Periodic, Tim2PeriodicConfig};
|
||||
pub use time::{DwtClockStartError, DwtMicrosecondClock};
|
||||
pub use usart::{BaudRateError, ReceiveInterrupt, Usart1Tx};
|
||||
pub use usart1_tx_dma2_stream7::{StartError, Usart1TxDma};
|
||||
@@ -1,148 +0,0 @@
|
||||
//! USART1 transmit configuration for STM32F722.
|
||||
|
||||
use stm32f7::stm32f722;
|
||||
|
||||
/// UART baud-rate configuration error.
|
||||
#[derive(Clone, Copy, Debug, Eq, PartialEq)]
|
||||
pub enum BaudRateError {
|
||||
/// Zero is not a valid baud rate.
|
||||
Zero,
|
||||
/// The requested divisor cannot fit USART BRR.
|
||||
OutOfRange,
|
||||
}
|
||||
|
||||
/// Terminal result from acknowledging the USART1 receive interrupt.
|
||||
#[derive(Clone, Copy, Debug, Eq, PartialEq)]
|
||||
pub enum ReceiveInterrupt {
|
||||
/// One received byte was removed from the UART data register.
|
||||
Byte(u8),
|
||||
/// A serial transport fault invalidated the current receive stream.
|
||||
Fault,
|
||||
/// This USART1 interrupt did not have a receive event pending.
|
||||
NotPending,
|
||||
}
|
||||
|
||||
/// Owns configured full-duplex USART1 hardware.
|
||||
///
|
||||
/// Pin alternate-function setup remains board-owned because it depends on the
|
||||
/// schematic. Configure pins before constructing this type.
|
||||
pub struct Usart1Tx {
|
||||
peripheral: stm32f722::USART1,
|
||||
}
|
||||
|
||||
impl Usart1Tx {
|
||||
/// Enables USART1 and configures 8N1 transmit and receive operation.
|
||||
pub fn new(
|
||||
rcc: &stm32f722::RCC,
|
||||
peripheral: stm32f722::USART1,
|
||||
peripheral_clock_hz: u32,
|
||||
baud: u32,
|
||||
) -> Result<Self, BaudRateError> {
|
||||
let brr = calculate_brr(peripheral_clock_hz, baud)?;
|
||||
|
||||
rcc.apb2rstr()
|
||||
.modify(|_, write| write.usart1rst().set_bit());
|
||||
rcc.apb2rstr()
|
||||
.modify(|_, write| write.usart1rst().clear_bit());
|
||||
rcc.apb2enr().modify(|_, write| write.usart1en().set_bit());
|
||||
peripheral.brr().write(|write| {
|
||||
// SAFETY: `brr` is validated to fit the complete BRR register.
|
||||
unsafe { write.bits(u32::from(brr)) }
|
||||
});
|
||||
peripheral.cr1().write(|write| {
|
||||
write
|
||||
.te()
|
||||
.set_bit()
|
||||
.re()
|
||||
.set_bit()
|
||||
.rxneie()
|
||||
.set_bit()
|
||||
.ue()
|
||||
.set_bit()
|
||||
});
|
||||
peripheral.cr2().write(|write| write.stop().stop1());
|
||||
|
||||
Ok(Self { peripheral })
|
||||
}
|
||||
|
||||
pub(crate) fn enable_dma_transmit(&self) {
|
||||
self.peripheral.cr3().write(|write| write.dmat().set_bit());
|
||||
}
|
||||
|
||||
pub(crate) fn transmit_data_register_address(&self) -> u32 {
|
||||
self.peripheral.tdr().as_ptr() as u32
|
||||
}
|
||||
|
||||
/// Acknowledges one USART1 receive interrupt without accessing TX state.
|
||||
///
|
||||
/// The board invokes this only from the `USART1` interrupt. USART1 TX DMA
|
||||
/// owns its disjoint data register and DMA stream, so this static route may
|
||||
/// safely acknowledge RX while that transfer is active.
|
||||
pub fn acknowledge_receive_interrupt() -> ReceiveInterrupt {
|
||||
// SAFETY: the board binds USART1 exactly once and this function touches
|
||||
// only its receive/error registers, never the TX DMA configuration.
|
||||
let peripheral = unsafe { &*stm32f722::USART1::ptr() };
|
||||
let status = peripheral.isr().read();
|
||||
let fault = status.pe().is_error()
|
||||
|| status.fe().is_error()
|
||||
|| status.nf().is_noise()
|
||||
|| status.ore().is_overrun();
|
||||
|
||||
if status.rxne().is_data_ready() {
|
||||
let byte = peripheral.rdr().read().rdr().bits() as u8;
|
||||
if fault {
|
||||
clear_receive_errors(peripheral);
|
||||
ReceiveInterrupt::Fault
|
||||
} else {
|
||||
ReceiveInterrupt::Byte(byte)
|
||||
}
|
||||
} else if fault {
|
||||
clear_receive_errors(peripheral);
|
||||
ReceiveInterrupt::Fault
|
||||
} else {
|
||||
ReceiveInterrupt::NotPending
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
fn clear_receive_errors(peripheral: &stm32f722::usart1::RegisterBlock) {
|
||||
peripheral.icr().write(|write| {
|
||||
write
|
||||
.pecf()
|
||||
.clear()
|
||||
.fecf()
|
||||
.clear()
|
||||
.ncf()
|
||||
.clear()
|
||||
.orecf()
|
||||
.clear()
|
||||
});
|
||||
}
|
||||
|
||||
const fn calculate_brr(peripheral_clock_hz: u32, baud: u32) -> Result<u16, BaudRateError> {
|
||||
if baud == 0 {
|
||||
return Err(BaudRateError::Zero);
|
||||
}
|
||||
|
||||
let divisor = (peripheral_clock_hz + (baud / 2)) / baud;
|
||||
if divisor == 0 || divisor > u16::MAX as u32 {
|
||||
Err(BaudRateError::OutOfRange)
|
||||
} else {
|
||||
Ok(divisor as u16)
|
||||
}
|
||||
}
|
||||
|
||||
#[cfg(test)]
|
||||
mod tests {
|
||||
use super::{BaudRateError, calculate_brr};
|
||||
|
||||
#[test]
|
||||
fn calculates_reset_clock_brr() {
|
||||
assert_eq!(calculate_brr(16_000_000, 115_200), Ok(139));
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn rejects_zero_baud() {
|
||||
assert_eq!(calculate_brr(16_000_000, 0), Err(BaudRateError::Zero));
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,32 @@
|
||||
[package]
|
||||
name = "platform-stm32f7"
|
||||
edition.workspace = true
|
||||
license.workspace = true
|
||||
repository.workspace = true
|
||||
rust-version.workspace = true
|
||||
version.workspace = true
|
||||
|
||||
[lints]
|
||||
workspace = true
|
||||
|
||||
[features]
|
||||
default = []
|
||||
|
||||
[dependencies]
|
||||
kaptan-actuator = { path = "../../libs/kaptan-actuator" }
|
||||
kaptan-board = { path = "../../libs/kaptan-board" }
|
||||
kaptan-data-channel = { path = "../../libs/kaptan-data-channel" }
|
||||
kaptan-dma = { path = "../../libs/kaptan-dma" }
|
||||
kaptan-drivers = { path = "../../libs/kaptan-drivers" }
|
||||
kaptan-i2c = { path = "../../libs/kaptan-i2c" }
|
||||
kaptan-mavlink = { path = "../../libs/kaptan-mavlink" }
|
||||
kaptan-sensor = { path = "../../libs/kaptan-sensor" }
|
||||
kaptan-spi = { path = "../../libs/kaptan-spi" }
|
||||
kaptan-time = { path = "../../libs/kaptan-time" }
|
||||
cortex-m = "0.7"
|
||||
cortex-m-rt = "0.7"
|
||||
rtic = { version = "2.3", default-features = false, features = ["thumbv7-backend"] }
|
||||
stm32f7 = { version = "0.16", default-features = false, features = ["rt", "stm32f722"] }
|
||||
|
||||
[target.'cfg(target_arch = "arm")'.dependencies]
|
||||
panic-halt = "1"
|
||||
@@ -0,0 +1,135 @@
|
||||
#![cfg_attr(not(target_arch = "arm"), allow(dead_code))]
|
||||
|
||||
mod tim3_dshot;
|
||||
mod tim3_pwm;
|
||||
|
||||
use kaptan_actuator::{MotorOutput, MotorProtocol};
|
||||
use stm32f7::stm32f722;
|
||||
|
||||
use self::{tim3_dshot::Tim3Dshot, tim3_pwm::Tim3Pwm};
|
||||
use crate::f722::MotorRoute;
|
||||
|
||||
/// STM32F722 TIM3/TIM5 protocol-neutral M1–M4 output route.
|
||||
///
|
||||
/// TIM3 channels 1–4 use PB4, PB5, PB0, and PB1 respectively. Kakute's
|
||||
/// M1–M4 connector order is PB0, PB1, PB4, PB5, while GEPRC's is PB4, PB5,
|
||||
/// PB0, PB1. The route descriptor therefore supplies the logical-to-channel
|
||||
/// map; no board inherits another board's motor numbering.
|
||||
/// Opaque owner of the F722 motor peripherals used by the RTIC application.
|
||||
///
|
||||
/// The type is public only because RTIC exposes resource metadata in generated
|
||||
/// public items. Its fields and construction remain platform-private.
|
||||
pub struct F722MotorOutput {
|
||||
transport: Transport,
|
||||
}
|
||||
|
||||
enum Transport {
|
||||
Pwm(Tim3Pwm),
|
||||
Dshot(Tim3Dshot),
|
||||
}
|
||||
|
||||
impl F722MotorOutput {
|
||||
pub(crate) fn new(
|
||||
rcc: &stm32f722::RCC,
|
||||
tim3: stm32f722::TIM3,
|
||||
tim5: stm32f722::TIM5,
|
||||
protocol: MotorProtocol,
|
||||
route: MotorRoute,
|
||||
) -> Self {
|
||||
let logical_to_tim3_channel = logical_to_tim3_channel(route);
|
||||
let transport = match protocol {
|
||||
MotorProtocol::Pwm(config) => {
|
||||
let _ = tim5;
|
||||
Transport::Pwm(Tim3Pwm::new(rcc, tim3, config))
|
||||
}
|
||||
MotorProtocol::Dshot(config) => Transport::Dshot(Tim3Dshot::new(
|
||||
rcc,
|
||||
tim3,
|
||||
tim5,
|
||||
config,
|
||||
logical_to_tim3_channel,
|
||||
)),
|
||||
};
|
||||
Self { transport }
|
||||
}
|
||||
|
||||
/// Services TIM3 while RTIC holds this motor owner's resource lock.
|
||||
pub(crate) fn on_tim3_interrupt(&mut self) {
|
||||
if matches!(self.transport, Transport::Pwm(_)) {
|
||||
Tim3Pwm::on_tim3_update();
|
||||
}
|
||||
}
|
||||
|
||||
/// Services TIM5 while RTIC holds this motor owner's resource lock.
|
||||
pub(crate) fn on_tim5_interrupt(&mut self) {
|
||||
if matches!(self.transport, Transport::Dshot(_)) {
|
||||
Tim3Dshot::on_tim5_update();
|
||||
}
|
||||
}
|
||||
|
||||
/// Begins the configured arming sequence.
|
||||
pub(crate) fn arm(&mut self) {
|
||||
<Self as MotorOutput>::arm(self);
|
||||
}
|
||||
|
||||
/// Immediately returns the outputs to their disarmed state.
|
||||
pub(crate) fn disarm(&mut self) {
|
||||
<Self as MotorOutput>::disarm(self);
|
||||
}
|
||||
|
||||
/// Returns the arming state of this uniquely owned output route.
|
||||
pub(crate) fn is_armed(&self) -> bool {
|
||||
<Self as MotorOutput>::is_armed(self)
|
||||
}
|
||||
}
|
||||
|
||||
/// Maps logical M1–M4 indices to zero-based TIM3 channel indices.
|
||||
fn logical_to_tim3_channel(route: MotorRoute) -> [usize; 4] {
|
||||
match route {
|
||||
// Kakute: M1=PB0/CH3, M2=PB1/CH4, M3=PB4/CH1, M4=PB5/CH2.
|
||||
MotorRoute::KakuteTim3Tim5 => [2, 3, 0, 1],
|
||||
// GEPRC: M1=PB4/CH1, M2=PB5/CH2, M3=PB0/CH3, M4=PB1/CH4.
|
||||
MotorRoute::GeprcTim3Tim5 => [0, 1, 2, 3],
|
||||
}
|
||||
}
|
||||
|
||||
impl MotorOutput for F722MotorOutput {
|
||||
fn arm(&mut self) {
|
||||
match &mut self.transport {
|
||||
Transport::Pwm(output) => output.arm(),
|
||||
Transport::Dshot(output) => output.arm(),
|
||||
}
|
||||
}
|
||||
|
||||
fn disarm(&mut self) {
|
||||
match &mut self.transport {
|
||||
Transport::Pwm(output) => output.disarm(),
|
||||
Transport::Dshot(output) => output.disarm(),
|
||||
}
|
||||
}
|
||||
|
||||
fn is_armed(&self) -> bool {
|
||||
match &self.transport {
|
||||
Transport::Pwm(output) => output.is_armed(),
|
||||
Transport::Dshot(output) => output.is_armed(),
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
#[cfg(test)]
|
||||
mod tests {
|
||||
use super::logical_to_tim3_channel;
|
||||
use crate::f722::MotorRoute;
|
||||
|
||||
#[test]
|
||||
fn keeps_each_supported_board_motor_order_explicit() {
|
||||
assert_eq!(
|
||||
logical_to_tim3_channel(MotorRoute::KakuteTim3Tim5),
|
||||
[2, 3, 0, 1]
|
||||
);
|
||||
assert_eq!(
|
||||
logical_to_tim3_channel(MotorRoute::GeprcTim3Tim5),
|
||||
[0, 1, 2, 3]
|
||||
);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,466 @@
|
||||
use core::sync::atomic::{AtomicBool, Ordering};
|
||||
|
||||
use kaptan_actuator::dshot::{
|
||||
ArmSequence, Config, FRAME_SYMBOLS, FrameValue, Throttle, Timing, symbols,
|
||||
};
|
||||
use stm32f7::stm32f722;
|
||||
|
||||
const APB1_TIMER_CLOCK_HZ: u32 = 108_000_000;
|
||||
const DSHOT_CHANNELS: usize = 4;
|
||||
const DSHOT_BUFFER_LENGTH: usize = FRAME_SYMBOLS * DSHOT_CHANNELS;
|
||||
const TIM3_CHANNEL_ENABLE: u32 = 0x1111;
|
||||
const PWM_MODE_1_WITH_PRELOAD: u32 = 0x6868;
|
||||
const TIM3_DMAR_CCR1_OFFSET: u8 = 13;
|
||||
const DEFAULT_ARMED_IDLE_THROTTLE: Throttle = match Throttle::new(48) {
|
||||
Some(throttle) => throttle,
|
||||
None => panic!("valid DShot throttle"),
|
||||
};
|
||||
|
||||
// These static buffers live in DMA-visible SRAM. They are initialized before
|
||||
// DMA is enabled and are subsequently read-only to the DMA controller.
|
||||
static mut DSHOT_ZERO_FRAME: [u32; DSHOT_BUFFER_LENGTH] = [0; DSHOT_BUFFER_LENGTH];
|
||||
static mut DSHOT_ARMED_IDLE_FRAME: [u32; DSHOT_BUFFER_LENGTH] = [0; DSHOT_BUFFER_LENGTH];
|
||||
static ARM_SEQUENCE: ArmSequence = ArmSequence::new(DEFAULT_ARMED_IDLE_THROTTLE, 0);
|
||||
static DMA_RUNNING: AtomicBool = AtomicBool::new(false);
|
||||
|
||||
pub(super) struct Tim3Dshot {
|
||||
timer: stm32f722::TIM3,
|
||||
dma_available: bool,
|
||||
}
|
||||
|
||||
impl Tim3Dshot {
|
||||
pub(super) fn new(
|
||||
rcc: &stm32f722::RCC,
|
||||
timer: stm32f722::TIM3,
|
||||
scheduler: stm32f722::TIM5,
|
||||
config: Config,
|
||||
logical_to_tim3_channel: [usize; DSHOT_CHANNELS],
|
||||
) -> Self {
|
||||
let rate = config.rate();
|
||||
let dshot_timer_hz = rate.timer_base_hz();
|
||||
let timing = match Timing::from_timer_clock(dshot_timer_hz, rate) {
|
||||
Some(timing) => timing,
|
||||
None => unreachable!(),
|
||||
};
|
||||
let timer_prescaler = APB1_TIMER_CLOCK_HZ / dshot_timer_hz;
|
||||
assert!(APB1_TIMER_CLOCK_HZ.is_multiple_of(dshot_timer_hz));
|
||||
ARM_SEQUENCE
|
||||
.configure_zero_frames(rate.frames_for_millis(config.arming_duration().milliseconds()));
|
||||
ARM_SEQUENCE.configure_armed_throttle(config.armed_idle_throttle());
|
||||
|
||||
rcc.ahb1enr().modify(|_, write| write.gpioben().set_bit());
|
||||
rcc.ahb1enr().modify(|_, write| write.dma1en().set_bit());
|
||||
rcc.apb1rstr().modify(|_, write| write.tim3rst().set_bit());
|
||||
rcc.apb1rstr()
|
||||
.modify(|_, write| write.tim3rst().clear_bit());
|
||||
rcc.apb1enr().modify(|_, write| write.tim3en().set_bit());
|
||||
|
||||
timer
|
||||
.cr1()
|
||||
.modify(|_, write| write.cen().clear_bit().arpe().enabled());
|
||||
timer.dier().modify(|_, write| write.ude().disabled());
|
||||
timer.ccer().write(|write| {
|
||||
// SAFETY: zero is the documented all-channels-disabled CCER encoding while TIM3 is stopped.
|
||||
unsafe { write.bits(0) }
|
||||
});
|
||||
timer.psc().write(|write| {
|
||||
// SAFETY: `timer_prescaler` divides the fixed APB1 timer clock and fits the PSC field.
|
||||
unsafe { write.psc().bits((timer_prescaler - 1) as u16) }
|
||||
});
|
||||
timer.arr().write(|write| {
|
||||
// SAFETY: `Timing` validates a non-zero ARR value representable by TIM3.
|
||||
unsafe { write.bits(u32::from(timing.bit_ticks() - 1)) }
|
||||
});
|
||||
timer.ccmr1_output().write(|write| {
|
||||
// SAFETY: this constant selects documented PWM mode 1 with preload for channels 1 and 2.
|
||||
unsafe { write.bits(PWM_MODE_1_WITH_PRELOAD) }
|
||||
});
|
||||
timer.ccmr2_output().write(|write| {
|
||||
// SAFETY: this constant selects documented PWM mode 1 with preload for channels 3 and 4.
|
||||
unsafe { write.bits(PWM_MODE_1_WITH_PRELOAD) }
|
||||
});
|
||||
timer.dcr().write(|write| {
|
||||
// SAFETY: the DMA burst starts at CCR1 and covers exactly four contiguous compare registers.
|
||||
unsafe {
|
||||
write
|
||||
.dba()
|
||||
.bits(TIM3_DMAR_CCR1_OFFSET)
|
||||
.dbl()
|
||||
.bits((DSHOT_CHANNELS - 1) as u8)
|
||||
}
|
||||
});
|
||||
timer.egr().write(|write| write.ug().set_bit());
|
||||
let dma_available = disable_dma_stream();
|
||||
initialize_dma_frames(
|
||||
timing,
|
||||
config.armed_idle_throttle(),
|
||||
logical_to_tim3_channel,
|
||||
);
|
||||
|
||||
rcc.apb1rstr().modify(|_, write| write.tim5rst().set_bit());
|
||||
rcc.apb1rstr()
|
||||
.modify(|_, write| write.tim5rst().clear_bit());
|
||||
rcc.apb1enr().modify(|_, write| write.tim5en().set_bit());
|
||||
scheduler.psc().write(|write| {
|
||||
// SAFETY: zero is a valid TIM5 prescaler and keeps the scheduler at the APB1 timer clock.
|
||||
unsafe { write.psc().bits(0) }
|
||||
});
|
||||
scheduler.arr().write(|write| {
|
||||
// SAFETY: each supported DShot rate divides APB1_TIMER_CLOCK_HZ, yielding a valid ARR value.
|
||||
unsafe { write.bits(APB1_TIMER_CLOCK_HZ / rate.output_rate_hz() - 1) }
|
||||
});
|
||||
scheduler.egr().write(|write| write.ug().set_bit());
|
||||
scheduler.sr().modify(|_, write| write.uif().clear());
|
||||
scheduler.dier().write(|write| write.uie().disabled());
|
||||
scheduler.cr1().write(|write| write.cen().clear_bit());
|
||||
configure_gpio_low();
|
||||
|
||||
Self {
|
||||
timer,
|
||||
dma_available,
|
||||
}
|
||||
}
|
||||
|
||||
pub(super) fn arm(&mut self) {
|
||||
if ARM_SEQUENCE.is_armed() {
|
||||
return;
|
||||
}
|
||||
if !self.dma_available {
|
||||
self.dma_available = disable_dma_stream();
|
||||
if !self.dma_available {
|
||||
return;
|
||||
}
|
||||
}
|
||||
ARM_SEQUENCE.arm();
|
||||
if !self.start_stream(zero_frame()) {
|
||||
ARM_SEQUENCE.disarm();
|
||||
return;
|
||||
}
|
||||
// SAFETY: TIM5 was uniquely taken during initialization and remains owned by this
|
||||
// platform resource; RTIC serializes this method with `on_tim5_update`.
|
||||
let scheduler = unsafe { &*stm32f722::TIM5::ptr() };
|
||||
scheduler.sr().modify(|_, write| write.uif().clear());
|
||||
scheduler.dier().modify(|_, write| write.uie().enabled());
|
||||
scheduler.cr1().modify(|_, write| write.cen().set_bit());
|
||||
}
|
||||
|
||||
pub(super) fn disarm(&mut self) {
|
||||
ARM_SEQUENCE.disarm();
|
||||
// SAFETY: TIM5 is owned by this motor resource and cannot be accessed concurrently
|
||||
// because RTIC locks that resource for both the service IRQ and this method.
|
||||
let scheduler = unsafe { &*stm32f722::TIM5::ptr() };
|
||||
scheduler.cr1().modify(|_, write| write.cen().clear_bit());
|
||||
scheduler.dier().modify(|_, write| write.uie().disabled());
|
||||
self.stop_stream();
|
||||
self.timer.ccer().write(|write| {
|
||||
// SAFETY: zero disables all TIM3 channels after its stream and counter have stopped.
|
||||
unsafe { write.bits(0) }
|
||||
});
|
||||
configure_gpio_low();
|
||||
}
|
||||
|
||||
pub(super) fn is_armed(&self) -> bool {
|
||||
ARM_SEQUENCE.is_armed()
|
||||
}
|
||||
|
||||
fn start_stream(&mut self, frame: *const u32) -> bool {
|
||||
self.timer.cr1().modify(|_, write| write.cen().clear_bit());
|
||||
DMA_RUNNING.store(false, Ordering::Release);
|
||||
if !configure_dma_stream(frame) {
|
||||
return false;
|
||||
}
|
||||
self.timer.ccer().write(|write| {
|
||||
// SAFETY: this enables exactly TIM3 channels 1 through 4 in output mode.
|
||||
unsafe { write.bits(TIM3_CHANNEL_ENABLE) }
|
||||
});
|
||||
configure_gpio_alternate_function();
|
||||
self.timer.egr().write(|write| write.ug().set_bit());
|
||||
DMA_RUNNING.store(true, Ordering::Release);
|
||||
dma_stream().cr().modify(|_, write| write.en().enabled());
|
||||
self.timer.dier().modify(|_, write| write.ude().enabled());
|
||||
self.timer.cr1().modify(|_, write| write.cen().set_bit());
|
||||
true
|
||||
}
|
||||
|
||||
fn stop_stream(&mut self) {
|
||||
self.timer.cr1().modify(|_, write| write.cen().clear_bit());
|
||||
DMA_RUNNING.store(false, Ordering::Release);
|
||||
self.timer.dier().modify(|_, write| write.ude().disabled());
|
||||
let _ = disable_dma_stream();
|
||||
}
|
||||
|
||||
pub(super) fn on_tim5_update() {
|
||||
// SAFETY: TIM5 is owned by the motor resource; its RTIC lock excludes arm/disarm while
|
||||
// the update handler clears its flag.
|
||||
let scheduler = unsafe { &*stm32f722::TIM5::ptr() };
|
||||
scheduler.sr().modify(|_, write| write.uif().clear());
|
||||
// SAFETY: TIM3 is uniquely owned by the same platform resource and not otherwise stolen.
|
||||
let timer = unsafe { &*stm32f722::TIM3::ptr() };
|
||||
timer.dier().modify(|_, write| write.ude().disabled());
|
||||
if !disable_dma_stream() {
|
||||
return;
|
||||
}
|
||||
if !DMA_RUNNING.load(Ordering::Acquire) {
|
||||
return;
|
||||
}
|
||||
let frame = match ARM_SEQUENCE.next_frame() {
|
||||
Some(FrameValue::Stop) => zero_frame(),
|
||||
Some(FrameValue::Throttle(_)) => armed_idle_frame(),
|
||||
None => return,
|
||||
};
|
||||
dma_stream().ndtr().write(|write| {
|
||||
// SAFETY: the fixed frame length is the exact initialized DMA buffer length.
|
||||
unsafe { write.bits(DSHOT_BUFFER_LENGTH as u32) }
|
||||
});
|
||||
dma_stream().m0ar().write(|write| {
|
||||
// SAFETY: `frame` is one of the static, aligned DMA-visible frame buffers.
|
||||
unsafe { write.bits(frame as u32) }
|
||||
});
|
||||
dma_stream().cr().modify(|_, write| write.en().enabled());
|
||||
timer.dier().modify(|_, write| write.ude().enabled());
|
||||
}
|
||||
}
|
||||
|
||||
fn initialize_dma_frames(
|
||||
timing: Timing,
|
||||
armed_idle: Throttle,
|
||||
logical_to_tim3_channel: [usize; DSHOT_CHANNELS],
|
||||
) {
|
||||
write_dma_frame(
|
||||
core::ptr::addr_of_mut!(DSHOT_ZERO_FRAME).cast(),
|
||||
FrameValue::Stop,
|
||||
timing,
|
||||
logical_to_tim3_channel,
|
||||
);
|
||||
write_dma_frame(
|
||||
core::ptr::addr_of_mut!(DSHOT_ARMED_IDLE_FRAME).cast(),
|
||||
FrameValue::Throttle(armed_idle),
|
||||
timing,
|
||||
logical_to_tim3_channel,
|
||||
);
|
||||
}
|
||||
|
||||
fn write_dma_frame(
|
||||
destination: *mut u32,
|
||||
value: FrameValue,
|
||||
timing: Timing,
|
||||
logical_to_tim3_channel: [usize; DSHOT_CHANNELS],
|
||||
) {
|
||||
let symbols = symbols(
|
||||
value.packet(),
|
||||
timing.zero_high_ticks(),
|
||||
timing.one_high_ticks(),
|
||||
);
|
||||
let mut bit = 0;
|
||||
while bit < FRAME_SYMBOLS {
|
||||
let mut logical_motor = 0;
|
||||
while logical_motor < DSHOT_CHANNELS {
|
||||
let channel = logical_to_tim3_channel[logical_motor];
|
||||
// SAFETY: `destination` identifies one of the exclusive static frame buffers during
|
||||
// initialization. The selected platform route is a permutation of
|
||||
// the four TIM3 channels, so the bounded indices stay within
|
||||
// DSHOT_BUFFER_LENGTH and each DMA word is initialized once.
|
||||
unsafe {
|
||||
destination
|
||||
.add(bit * DSHOT_CHANNELS + channel)
|
||||
.write(symbols[bit] as u32)
|
||||
};
|
||||
logical_motor += 1;
|
||||
}
|
||||
bit += 1;
|
||||
}
|
||||
}
|
||||
|
||||
fn zero_frame() -> *const u32 {
|
||||
core::ptr::addr_of!(DSHOT_ZERO_FRAME).cast()
|
||||
}
|
||||
fn armed_idle_frame() -> *const u32 {
|
||||
core::ptr::addr_of!(DSHOT_ARMED_IDLE_FRAME).cast()
|
||||
}
|
||||
|
||||
fn dma() -> &'static stm32f722::dma2::RegisterBlock {
|
||||
// SAFETY: DMA1 stream 2 is exclusively claimed by Tim3Dshot; all access to this register
|
||||
// block is serialized through the platform's RTIC-owned motor state.
|
||||
unsafe { &*stm32f722::DMA1::ptr() }
|
||||
}
|
||||
fn dma_stream() -> &'static stm32f722::dma2::ST {
|
||||
dma().st(2)
|
||||
}
|
||||
|
||||
fn disable_dma_stream() -> bool {
|
||||
dma_stream().cr().modify(|_, write| write.en().disabled());
|
||||
let mut spins = 1_024;
|
||||
while dma_stream().cr().read().en().is_enabled() && spins != 0 {
|
||||
spins -= 1;
|
||||
}
|
||||
if dma_stream().cr().read().en().is_enabled() {
|
||||
return false;
|
||||
}
|
||||
dma().lifcr().write(|write| {
|
||||
write
|
||||
.cfeif2()
|
||||
.clear()
|
||||
.cdmeif2()
|
||||
.clear()
|
||||
.cteif2()
|
||||
.clear()
|
||||
.chtif2()
|
||||
.clear()
|
||||
.ctcif2()
|
||||
.clear()
|
||||
});
|
||||
true
|
||||
}
|
||||
|
||||
fn configure_dma_stream(frame: *const u32) -> bool {
|
||||
if !disable_dma_stream() {
|
||||
return false;
|
||||
}
|
||||
dma_stream().cr().write(|write| {
|
||||
// SAFETY: stream 2 is disabled above; channel 5 and the width/direction settings are
|
||||
// the documented DMA1/TIM3_UP route for this fixed u32 frame buffer.
|
||||
unsafe {
|
||||
write
|
||||
.chsel()
|
||||
.bits(5)
|
||||
.dir()
|
||||
.memory_to_peripheral()
|
||||
.pl()
|
||||
.very_high()
|
||||
.minc()
|
||||
.incremented()
|
||||
.pinc()
|
||||
.fixed()
|
||||
.msize()
|
||||
.bits32()
|
||||
.psize()
|
||||
.bits32()
|
||||
.circ()
|
||||
.disabled()
|
||||
.tcie()
|
||||
.disabled()
|
||||
.teie()
|
||||
.disabled()
|
||||
.dmeie()
|
||||
.disabled()
|
||||
}
|
||||
});
|
||||
dma_stream().ndtr().write(|write| {
|
||||
// SAFETY: transfer count equals the fixed initialized DMA frame length.
|
||||
unsafe { write.bits(DSHOT_BUFFER_LENGTH as u32) }
|
||||
});
|
||||
dma_stream().par().write(|write| {
|
||||
// SAFETY: the address is TIM3's DMA-burst register, the fixed peripheral endpoint.
|
||||
unsafe { write.bits(tim3_dmar_address()) }
|
||||
});
|
||||
dma_stream().m0ar().write(|write| {
|
||||
// SAFETY: `frame` is a static aligned buffer retained for the entire DMA transfer.
|
||||
unsafe { write.bits(frame as u32) }
|
||||
});
|
||||
dma_stream().fcr().write(|write| {
|
||||
// SAFETY: FIFO threshold 3 and direct-mode disable are documented legal DMA settings.
|
||||
unsafe { write.fth().bits(3).dmdis().set_bit() }
|
||||
});
|
||||
true
|
||||
}
|
||||
|
||||
fn tim3_dmar_address() -> u32 {
|
||||
// SAFETY: TIM3 is uniquely owned by Tim3Dshot; this only obtains the stable MMIO address of
|
||||
// its DMAR register and does not create mutable Rust access to the peripheral.
|
||||
unsafe { (&*stm32f722::TIM3::ptr()).dmar().as_ptr() as u32 }
|
||||
}
|
||||
|
||||
fn port() -> &'static stm32f722::gpiob::RegisterBlock {
|
||||
// SAFETY: PB0/PB1/PB4/PB5 are exclusively configured as TIM3 motor pins by this platform
|
||||
// resource; no board code receives GPIOB or this raw register block.
|
||||
unsafe { &*stm32f722::GPIOB::ptr() }
|
||||
}
|
||||
|
||||
fn configure_gpio_low() {
|
||||
let port = port();
|
||||
port.moder().modify(|_, write| {
|
||||
write
|
||||
.moder0()
|
||||
.output()
|
||||
.moder1()
|
||||
.output()
|
||||
.moder4()
|
||||
.output()
|
||||
.moder5()
|
||||
.output()
|
||||
});
|
||||
port.otyper().modify(|_, write| {
|
||||
write
|
||||
.ot0()
|
||||
.push_pull()
|
||||
.ot1()
|
||||
.push_pull()
|
||||
.ot4()
|
||||
.push_pull()
|
||||
.ot5()
|
||||
.push_pull()
|
||||
});
|
||||
port.pupdr().modify(|_, write| {
|
||||
write
|
||||
.pupdr0()
|
||||
.floating()
|
||||
.pupdr1()
|
||||
.floating()
|
||||
.pupdr4()
|
||||
.floating()
|
||||
.pupdr5()
|
||||
.floating()
|
||||
});
|
||||
port.bsrr().write(|write| {
|
||||
write
|
||||
.br0()
|
||||
.set_bit()
|
||||
.br1()
|
||||
.set_bit()
|
||||
.br4()
|
||||
.set_bit()
|
||||
.br5()
|
||||
.set_bit()
|
||||
});
|
||||
}
|
||||
|
||||
fn configure_gpio_alternate_function() {
|
||||
let port = port();
|
||||
port.afrl().modify(|_, write| {
|
||||
// SAFETY: AF2 is TIM3 on PB0/PB1/PB4/PB5; these fields are disjoint from board-owned
|
||||
// pins and have been exclusively reserved for motor output by the route profile.
|
||||
unsafe {
|
||||
write
|
||||
.afrl0()
|
||||
.bits(2)
|
||||
.afrl1()
|
||||
.bits(2)
|
||||
.afrl4()
|
||||
.bits(2)
|
||||
.afrl5()
|
||||
.bits(2)
|
||||
}
|
||||
});
|
||||
port.moder().modify(|_, write| {
|
||||
write
|
||||
.moder0()
|
||||
.alternate()
|
||||
.moder1()
|
||||
.alternate()
|
||||
.moder4()
|
||||
.alternate()
|
||||
.moder5()
|
||||
.alternate()
|
||||
});
|
||||
port.ospeedr().modify(|_, write| {
|
||||
write
|
||||
.ospeedr0()
|
||||
.high_speed()
|
||||
.ospeedr1()
|
||||
.high_speed()
|
||||
.ospeedr4()
|
||||
.high_speed()
|
||||
.ospeedr5()
|
||||
.high_speed()
|
||||
});
|
||||
}
|
||||
@@ -0,0 +1,206 @@
|
||||
use core::sync::atomic::{AtomicU16, Ordering};
|
||||
|
||||
use kaptan_actuator::pwm::{ArmSequence, Config, FrameValue, PulseWidth};
|
||||
use stm32f7::stm32f722;
|
||||
|
||||
const APB1_TIMER_CLOCK_HZ: u32 = 108_000_000;
|
||||
const TIMER_TICK_HZ: u32 = 1_000_000;
|
||||
const PWM_CHANNEL_ENABLE: u32 = 0x1111;
|
||||
const PWM_MODE_1_WITH_PRELOAD: u32 = 0x6868;
|
||||
const DEFAULT_MINIMUM_PULSE: PulseWidth = match PulseWidth::new(1_000) {
|
||||
Some(pulse) => pulse,
|
||||
None => panic!("valid PWM pulse"),
|
||||
};
|
||||
const DEFAULT_ARMED_IDLE_PULSE: PulseWidth = match PulseWidth::new(1_000) {
|
||||
Some(pulse) => pulse,
|
||||
None => panic!("valid PWM pulse"),
|
||||
};
|
||||
|
||||
static ARM_SEQUENCE: ArmSequence = ArmSequence::new(DEFAULT_ARMED_IDLE_PULSE, 0);
|
||||
static MINIMUM_PULSE: AtomicU16 = AtomicU16::new(DEFAULT_MINIMUM_PULSE.microseconds());
|
||||
|
||||
pub(super) struct Tim3Pwm {
|
||||
timer: stm32f722::TIM3,
|
||||
minimum_pulse: PulseWidth,
|
||||
}
|
||||
|
||||
impl Tim3Pwm {
|
||||
pub(super) fn new(rcc: &stm32f722::RCC, timer: stm32f722::TIM3, config: Config) -> Self {
|
||||
let period_us = match config.frame_rate().period_microseconds() {
|
||||
Some(period) => period,
|
||||
None => unreachable!(),
|
||||
};
|
||||
ARM_SEQUENCE.configure_minimum_frames(
|
||||
config
|
||||
.frame_rate()
|
||||
.frames_for_millis(config.arming_duration().milliseconds()),
|
||||
);
|
||||
ARM_SEQUENCE.configure_armed_idle(config.armed_idle_pulse());
|
||||
MINIMUM_PULSE.store(config.minimum_pulse().microseconds(), Ordering::Release);
|
||||
|
||||
rcc.ahb1enr().modify(|_, write| write.gpioben().set_bit());
|
||||
rcc.apb1rstr().modify(|_, write| write.tim3rst().set_bit());
|
||||
rcc.apb1rstr()
|
||||
.modify(|_, write| write.tim3rst().clear_bit());
|
||||
rcc.apb1enr().modify(|_, write| write.tim3en().set_bit());
|
||||
|
||||
configure_gpio_alternate_function();
|
||||
timer
|
||||
.cr1()
|
||||
.write(|write| write.cen().clear_bit().arpe().enabled());
|
||||
timer.psc().write(|write| {
|
||||
// SAFETY: the fixed APB1 clock divides exactly to the documented 1 MHz timer tick.
|
||||
unsafe {
|
||||
write
|
||||
.psc()
|
||||
.bits((APB1_TIMER_CLOCK_HZ / TIMER_TICK_HZ - 1) as u16)
|
||||
}
|
||||
});
|
||||
timer.arr().write(|write| {
|
||||
// SAFETY: `frame_rate` guarantees a non-zero period fitting TIM3's ARR register.
|
||||
unsafe { write.bits(u32::from(period_us - 1)) }
|
||||
});
|
||||
timer.ccmr1_output().write(|write| {
|
||||
// SAFETY: this is the documented PWM mode 1 plus preload encoding for channels 1 and 2.
|
||||
unsafe { write.bits(PWM_MODE_1_WITH_PRELOAD) }
|
||||
});
|
||||
timer.ccmr2_output().write(|write| {
|
||||
// SAFETY: this is the documented PWM mode 1 plus preload encoding for channels 3 and 4.
|
||||
unsafe { write.bits(PWM_MODE_1_WITH_PRELOAD) }
|
||||
});
|
||||
write_pulse(&timer, config.minimum_pulse());
|
||||
timer.ccer().write(|write| {
|
||||
// SAFETY: this enables only the four TIM3 output channels configured above.
|
||||
unsafe { write.bits(PWM_CHANNEL_ENABLE) }
|
||||
});
|
||||
timer.egr().write(|write| write.ug().set_bit());
|
||||
timer.sr().modify(|_, write| write.uif().clear());
|
||||
timer.dier().write(|write| write.uie().enabled());
|
||||
timer.cr1().modify(|_, write| write.cen().set_bit());
|
||||
|
||||
Self {
|
||||
timer,
|
||||
minimum_pulse: config.minimum_pulse(),
|
||||
}
|
||||
}
|
||||
|
||||
pub(super) fn arm(&mut self) {
|
||||
ARM_SEQUENCE.arm();
|
||||
}
|
||||
|
||||
pub(super) fn disarm(&mut self) {
|
||||
ARM_SEQUENCE.disarm();
|
||||
write_pulse(&self.timer, self.minimum_pulse);
|
||||
}
|
||||
|
||||
pub(super) fn is_armed(&self) -> bool {
|
||||
ARM_SEQUENCE.is_armed()
|
||||
}
|
||||
|
||||
pub(super) fn on_tim3_update() {
|
||||
// SAFETY: TIM3 is exclusively owned by the platform motor resource; RTIC locks that
|
||||
// resource across the service task and this update handler.
|
||||
let timer = unsafe { &*stm32f722::TIM3::ptr() };
|
||||
timer.sr().modify(|_, write| write.uif().clear());
|
||||
let pulse = match ARM_SEQUENCE.next_frame() {
|
||||
Some(FrameValue::ArmedIdle(pulse)) => pulse,
|
||||
Some(FrameValue::Minimum) | None => configured_minimum_pulse(),
|
||||
};
|
||||
write_pulse(timer, pulse);
|
||||
}
|
||||
}
|
||||
|
||||
fn configured_minimum_pulse() -> PulseWidth {
|
||||
match PulseWidth::new(MINIMUM_PULSE.load(Ordering::Acquire)) {
|
||||
Some(pulse) => pulse,
|
||||
None => unreachable!(),
|
||||
}
|
||||
}
|
||||
|
||||
fn write_pulse(timer: &stm32f722::tim3::RegisterBlock, pulse: PulseWidth) {
|
||||
let ticks = u32::from(pulse.microseconds());
|
||||
timer.ccr1().write(|write| {
|
||||
// SAFETY: `PulseWidth` bounds ticks to the configured PWM frame interval.
|
||||
unsafe { write.bits(ticks) }
|
||||
});
|
||||
timer.ccr2().write(|write| {
|
||||
// SAFETY: `PulseWidth` bounds ticks to the configured PWM frame interval.
|
||||
unsafe { write.bits(ticks) }
|
||||
});
|
||||
timer.ccr3().write(|write| {
|
||||
// SAFETY: `PulseWidth` bounds ticks to the configured PWM frame interval.
|
||||
unsafe { write.bits(ticks) }
|
||||
});
|
||||
timer.ccr4().write(|write| {
|
||||
// SAFETY: `PulseWidth` bounds ticks to the configured PWM frame interval.
|
||||
unsafe { write.bits(ticks) }
|
||||
});
|
||||
}
|
||||
|
||||
fn port() -> &'static stm32f722::gpiob::RegisterBlock {
|
||||
// SAFETY: PB0/PB1/PB4/PB5 are exclusively claimed by the selected TIM3 motor route; board
|
||||
// code never receives GPIOB and RTIC serializes platform writes to these registers.
|
||||
unsafe { &*stm32f722::GPIOB::ptr() }
|
||||
}
|
||||
|
||||
fn configure_gpio_alternate_function() {
|
||||
let port = port();
|
||||
port.afrl().modify(|_, write| {
|
||||
// SAFETY: AF2 selects TIM3 on these four reserved GPIOB fields, leaving all other pins intact.
|
||||
unsafe {
|
||||
write
|
||||
.afrl0()
|
||||
.bits(2)
|
||||
.afrl1()
|
||||
.bits(2)
|
||||
.afrl4()
|
||||
.bits(2)
|
||||
.afrl5()
|
||||
.bits(2)
|
||||
}
|
||||
});
|
||||
port.moder().modify(|_, write| {
|
||||
write
|
||||
.moder0()
|
||||
.alternate()
|
||||
.moder1()
|
||||
.alternate()
|
||||
.moder4()
|
||||
.alternate()
|
||||
.moder5()
|
||||
.alternate()
|
||||
});
|
||||
port.otyper().modify(|_, write| {
|
||||
write
|
||||
.ot0()
|
||||
.push_pull()
|
||||
.ot1()
|
||||
.push_pull()
|
||||
.ot4()
|
||||
.push_pull()
|
||||
.ot5()
|
||||
.push_pull()
|
||||
});
|
||||
port.ospeedr().modify(|_, write| {
|
||||
write
|
||||
.ospeedr0()
|
||||
.high_speed()
|
||||
.ospeedr1()
|
||||
.high_speed()
|
||||
.ospeedr4()
|
||||
.high_speed()
|
||||
.ospeedr5()
|
||||
.high_speed()
|
||||
});
|
||||
port.pupdr().modify(|_, write| {
|
||||
write
|
||||
.pupdr0()
|
||||
.floating()
|
||||
.pupdr1()
|
||||
.floating()
|
||||
.pupdr4()
|
||||
.floating()
|
||||
.pupdr5()
|
||||
.floating()
|
||||
});
|
||||
}
|
||||
@@ -0,0 +1,558 @@
|
||||
//! Profile-independent STM32F722 flight state and ownership graph.
|
||||
|
||||
use core::sync::atomic::{AtomicBool, AtomicU8, Ordering};
|
||||
|
||||
use super::peripherals::{
|
||||
DwtMicrosecondClock, I2c1, I2c1Config, ReceiveInterrupt, Tim2Periodic, Uart4,
|
||||
configure_exti3_pin_pc3, configure_exti4_pin_pa4, configure_i2c1_pins_pb8_pb9,
|
||||
configure_spi1_pins_pa5_pa6_pa7, configure_spi3_pins_pc10_pc11_pc12,
|
||||
configure_uart4_pins_pa0_pa1, configure_usart1_rx_pin_pa10, configure_usart1_tx_pin_pa9,
|
||||
configure_usart2_pins_pa2_pa3,
|
||||
};
|
||||
use kaptan_actuator::{ArmingDuration, MotorProtocol};
|
||||
use kaptan_board::{BoardConfig, VerticalSensingKind};
|
||||
use kaptan_data_channel::{
|
||||
DataChannel, DataSources, LatestImuMailbox, LatestOpticalFlowMailbox, LatestRangeMailbox,
|
||||
};
|
||||
use kaptan_drivers::{
|
||||
lidar_lite_v3::{AcquisitionMode, LidarLiteV3, LidarLiteV3Service, LidarLiteV3ServiceEvent},
|
||||
mtf01::Mtf01Service,
|
||||
};
|
||||
use kaptan_mavlink::{
|
||||
commands::arm::{ArmRequest, decode_arm_request},
|
||||
receiver::MavlinkReceiver,
|
||||
};
|
||||
use kaptan_spi::SpiCompletion;
|
||||
use stm32f7::stm32f722;
|
||||
|
||||
use crate::{BoardError, F722BoardRoutes, F722Profile};
|
||||
|
||||
use super::{
|
||||
MotorRoute, VerticalRoute, clock,
|
||||
data_channel::{
|
||||
F722MavlinkTransmitter, F722StatusLed, GeprcStatusLed, SERVICE_HZ, SERVICE_TIMER_CONFIG,
|
||||
vertical_data_config,
|
||||
},
|
||||
interrupts,
|
||||
sensors::{F722Imu, ImuEvent},
|
||||
};
|
||||
|
||||
const UART4_CLOCK_HZ: u32 = 54_000_000;
|
||||
const MTF01_BAUD: u32 = 115_200;
|
||||
const I2C1_CONFIG: I2c1Config = I2c1Config::from_timing_register(0x2040_4768);
|
||||
const LIDAR_MODE: AcquisitionMode = AcquisitionMode::WithReceiverBiasCorrection;
|
||||
const LIDAR_STARTUP_TICKS: u8 = 3;
|
||||
const MOTOR_PROTOCOL: MotorProtocol = MotorProtocol::Dshot(
|
||||
match kaptan_actuator::dshot::Config::new(
|
||||
kaptan_actuator::dshot::Rate::Dshot600,
|
||||
match kaptan_actuator::dshot::Throttle::new(300) {
|
||||
Some(throttle) => throttle,
|
||||
None => panic!("armed idle throttle must be valid"),
|
||||
},
|
||||
match ArmingDuration::from_millis(2_000) {
|
||||
Some(duration) => duration,
|
||||
None => panic!("arming duration must be valid"),
|
||||
},
|
||||
) {
|
||||
Some(config) => config,
|
||||
None => panic!("DShot motor protocol must be valid"),
|
||||
},
|
||||
);
|
||||
|
||||
static IMU_HEALTHY: AtomicBool = AtomicBool::new(false);
|
||||
static UART_COMPLETION: AtomicU8 = AtomicU8::new(0);
|
||||
static SAMPLES: LatestImuMailbox = LatestImuMailbox::new();
|
||||
static RANGES: LatestRangeMailbox = LatestRangeMailbox::new();
|
||||
static OPTICAL_FLOW: LatestOpticalFlowMailbox = LatestOpticalFlowMailbox::new();
|
||||
|
||||
type F722Range = LidarLiteV3Service<DwtMicrosecondClock, I2c1>;
|
||||
/// Opaque RTIC-local MTF-01 service type; its route is platform-owned.
|
||||
pub type F722Mtf01 = Mtf01Service<DwtMicrosecondClock>;
|
||||
/// Opaque RTIC-local UART4 owner; its construction is platform-owned.
|
||||
pub type F722Mtf01Uart = Uart4;
|
||||
/// Opaque RTIC-local service timer; its construction is platform-owned.
|
||||
pub type F722ServiceTimer = Tim2Periodic;
|
||||
|
||||
/// RTIC-shared MAVLink state.
|
||||
///
|
||||
/// Only USART receive and the MAVLink worker access this state, so its
|
||||
/// priority ceiling is 9 rather than the IMU or vertical-sensor IRQ ceiling.
|
||||
pub struct F722MavlinkState {
|
||||
receiver: MavlinkReceiver<4>,
|
||||
data_channel: DataChannel<'static, F722MavlinkTransmitter, SERVICE_HZ>,
|
||||
led: F722StatusLed,
|
||||
config: BoardConfig,
|
||||
}
|
||||
|
||||
/// RTIC-shared LIDAR state.
|
||||
///
|
||||
/// I2C event/error IRQs and the service timer are its only users. MTF-01 is a
|
||||
/// separate UART-only local resource, so it cannot raise this ceiling.
|
||||
pub struct F722LidarState {
|
||||
range: Option<F722Range>,
|
||||
startup_ticks: u8,
|
||||
}
|
||||
|
||||
/// Explicit handoff from platform initialization to the RTIC resource graph.
|
||||
///
|
||||
/// This is not runtime state indirection: each field is moved exactly once
|
||||
/// into the single RTIC owner that accesses it.
|
||||
pub struct F722InitResources {
|
||||
pub(crate) imu: F722Imu,
|
||||
pub(crate) mtf01: Option<F722Mtf01>,
|
||||
pub(crate) mtf01_uart: Option<F722Mtf01Uart>,
|
||||
pub(crate) timer: F722ServiceTimer,
|
||||
pub(crate) lidar: F722LidarState,
|
||||
pub(crate) mavlink: F722MavlinkState,
|
||||
pub(crate) motors: crate::actuator::F722MotorOutput,
|
||||
}
|
||||
|
||||
/// Validates the relationship between portable metadata and selected sensor
|
||||
/// route before platform initialization claims a peripheral.
|
||||
pub(crate) fn validate_vertical_route(
|
||||
config: BoardConfig,
|
||||
routes: F722BoardRoutes,
|
||||
) -> Result<(), BoardError> {
|
||||
if config.data_channel.service_hz != SERVICE_HZ {
|
||||
return Err(BoardError::UnsupportedServiceFrequency);
|
||||
}
|
||||
let expected = match routes.vertical {
|
||||
VerticalRoute::None => VerticalSensingKind::None,
|
||||
VerticalRoute::Mtf01 => VerticalSensingKind::RangeAndOpticalFlow,
|
||||
VerticalRoute::LidarLiteV3 => VerticalSensingKind::Range,
|
||||
};
|
||||
if config.vertical_sensing != expected {
|
||||
return Err(BoardError::VerticalSensorCapabilityMismatch);
|
||||
}
|
||||
Ok(())
|
||||
}
|
||||
|
||||
pub(crate) fn initialize(
|
||||
mut core: cortex_m::Peripherals,
|
||||
device: stm32f722::Peripherals,
|
||||
config: BoardConfig,
|
||||
routes: F722BoardRoutes,
|
||||
) -> Result<F722InitResources, BoardError> {
|
||||
let clock = clock::initialize(&mut core, &device).map_err(|_| BoardError::Initialization)?;
|
||||
|
||||
// The profile is written once before any interrupt is enabled. ISR helpers
|
||||
// read it with Acquire ordering and only access a route initialized by the
|
||||
// matching branch below.
|
||||
interrupts::activate_profile(routes.profile());
|
||||
|
||||
match routes.profile() {
|
||||
F722Profile::Mpu6000Spi1Usart1 => {
|
||||
initialize_mpu6000(device, clock, config, routes.motors, routes.vertical)
|
||||
}
|
||||
F722Profile::Icm42688Spi3Usart2 => {
|
||||
initialize_icm42688(device, clock, config, routes.motors, routes.vertical)
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
fn initialize_mpu6000(
|
||||
device: stm32f722::Peripherals,
|
||||
clock: DwtMicrosecondClock,
|
||||
config: BoardConfig,
|
||||
motors: MotorRoute,
|
||||
vertical: VerticalRoute,
|
||||
) -> Result<F722InitResources, BoardError> {
|
||||
configure_spi1_pins_pa5_pa6_pa7(&device.RCC, &device.GPIOA);
|
||||
configure_exti4_pin_pa4(&device.GPIOA);
|
||||
configure_exti4(&device.RCC, device.SYSCFG, device.EXTI);
|
||||
configure_usart1_tx_pin_pa9(&device.GPIOA);
|
||||
configure_usart1_rx_pin_pa10(&device.GPIOA);
|
||||
configure_vertical_pins(&device.RCC, &device.GPIOA, &device.GPIOB, vertical);
|
||||
|
||||
let (range, mtf01, mtf01_uart) = initialize_vertical(
|
||||
&device.RCC,
|
||||
device.I2C1,
|
||||
device.UART4,
|
||||
clock.fork(),
|
||||
clock.fork(),
|
||||
vertical,
|
||||
)?;
|
||||
let imu = F722Imu::mpu6000(&device.RCC, device.SPI1, device.DMA2, device.GPIOB, clock);
|
||||
let transmitter = F722MavlinkTransmitter::usart1(&device.RCC, device.USART1)
|
||||
.map_err(|_| BoardError::Initialization)?;
|
||||
let led = F722StatusLed::Kakute(super::peripherals::ActiveLowLedPd2::new(
|
||||
&device.RCC,
|
||||
device.GPIOD,
|
||||
));
|
||||
finish(
|
||||
&device.RCC,
|
||||
device.TIM2,
|
||||
device.TIM3,
|
||||
device.TIM5,
|
||||
config,
|
||||
motors,
|
||||
vertical,
|
||||
imu,
|
||||
range,
|
||||
mtf01,
|
||||
mtf01_uart,
|
||||
transmitter,
|
||||
led,
|
||||
)
|
||||
}
|
||||
|
||||
fn initialize_icm42688(
|
||||
device: stm32f722::Peripherals,
|
||||
clock: DwtMicrosecondClock,
|
||||
config: BoardConfig,
|
||||
motors: MotorRoute,
|
||||
vertical: VerticalRoute,
|
||||
) -> Result<F722InitResources, BoardError> {
|
||||
configure_spi3_pins_pc10_pc11_pc12(&device.RCC, &device.GPIOC);
|
||||
configure_exti3_pin_pc3(&device.GPIOC);
|
||||
configure_exti3(&device.RCC, device.SYSCFG, device.EXTI);
|
||||
configure_usart2_pins_pa2_pa3(&device.GPIOA);
|
||||
configure_vertical_pins(&device.RCC, &device.GPIOA, &device.GPIOB, vertical);
|
||||
|
||||
let (range, mtf01, mtf01_uart) = initialize_vertical(
|
||||
&device.RCC,
|
||||
device.I2C1,
|
||||
device.UART4,
|
||||
clock.fork(),
|
||||
clock.fork(),
|
||||
vertical,
|
||||
)?;
|
||||
let imu = F722Imu::icm42688(&device.RCC, device.SPI3, device.DMA1, &device.GPIOA, clock);
|
||||
let transmitter = F722MavlinkTransmitter::usart2(&device.RCC, device.USART2)
|
||||
.map_err(|_| BoardError::Initialization)?;
|
||||
let led = F722StatusLed::Geprc(GeprcStatusLed::new(&device.RCC, device.GPIOA));
|
||||
finish(
|
||||
&device.RCC,
|
||||
device.TIM2,
|
||||
device.TIM3,
|
||||
device.TIM5,
|
||||
config,
|
||||
motors,
|
||||
vertical,
|
||||
imu,
|
||||
range,
|
||||
mtf01,
|
||||
mtf01_uart,
|
||||
transmitter,
|
||||
led,
|
||||
)
|
||||
}
|
||||
|
||||
#[allow(clippy::too_many_arguments)]
|
||||
fn finish(
|
||||
rcc: &stm32f722::RCC,
|
||||
tim2: stm32f722::TIM2,
|
||||
tim3: stm32f722::TIM3,
|
||||
tim5: stm32f722::TIM5,
|
||||
config: BoardConfig,
|
||||
motors: MotorRoute,
|
||||
vertical: VerticalRoute,
|
||||
imu: F722Imu,
|
||||
range: Option<F722Range>,
|
||||
mtf01: Option<F722Mtf01>,
|
||||
mtf01_uart: Option<Uart4>,
|
||||
transmitter: F722MavlinkTransmitter,
|
||||
led: F722StatusLed,
|
||||
) -> Result<F722InitResources, BoardError> {
|
||||
let data_channel = DataChannel::new(
|
||||
transmitter,
|
||||
config,
|
||||
vertical_data_config(vertical),
|
||||
DataSources {
|
||||
imu: &SAMPLES,
|
||||
range: &RANGES,
|
||||
optical_flow: &OPTICAL_FLOW,
|
||||
},
|
||||
)
|
||||
.map_err(|_| BoardError::Initialization)?;
|
||||
Ok(F722InitResources {
|
||||
imu,
|
||||
mtf01,
|
||||
mtf01_uart,
|
||||
timer: Tim2Periodic::new(rcc, tim2, SERVICE_TIMER_CONFIG),
|
||||
lidar: F722LidarState {
|
||||
range,
|
||||
startup_ticks: 0,
|
||||
},
|
||||
mavlink: F722MavlinkState {
|
||||
receiver: MavlinkReceiver::new(),
|
||||
data_channel,
|
||||
led,
|
||||
config,
|
||||
},
|
||||
motors: crate::actuator::F722MotorOutput::new(rcc, tim3, tim5, MOTOR_PROTOCOL, motors),
|
||||
})
|
||||
}
|
||||
|
||||
fn configure_vertical_pins(
|
||||
rcc: &stm32f722::RCC,
|
||||
gpioa: &stm32f722::GPIOA,
|
||||
gpiob: &stm32f722::GPIOB,
|
||||
vertical: VerticalRoute,
|
||||
) {
|
||||
match vertical {
|
||||
VerticalRoute::None => {}
|
||||
VerticalRoute::Mtf01 => configure_uart4_pins_pa0_pa1(gpioa),
|
||||
VerticalRoute::LidarLiteV3 => configure_i2c1_pins_pb8_pb9(rcc, gpiob),
|
||||
}
|
||||
}
|
||||
|
||||
type VerticalResources = (Option<F722Range>, Option<F722Mtf01>, Option<Uart4>);
|
||||
|
||||
fn initialize_vertical(
|
||||
rcc: &stm32f722::RCC,
|
||||
i2c: stm32f722::I2C1,
|
||||
uart4: stm32f722::UART4,
|
||||
mtf01_clock: DwtMicrosecondClock,
|
||||
range_clock: DwtMicrosecondClock,
|
||||
vertical: VerticalRoute,
|
||||
) -> Result<VerticalResources, BoardError> {
|
||||
match vertical {
|
||||
VerticalRoute::None => Ok((None, None, None)),
|
||||
VerticalRoute::Mtf01 => Ok((
|
||||
None,
|
||||
Some(Mtf01Service::new(mtf01_clock)),
|
||||
Some(
|
||||
Uart4::new(rcc, uart4, UART4_CLOCK_HZ, MTF01_BAUD)
|
||||
.map_err(|_| BoardError::Initialization)?,
|
||||
),
|
||||
)),
|
||||
VerticalRoute::LidarLiteV3 => Ok((
|
||||
Some(LidarLiteV3Service::new(
|
||||
range_clock,
|
||||
LidarLiteV3::new(I2c1::new(rcc, i2c, I2C1_CONFIG)),
|
||||
LIDAR_MODE,
|
||||
)),
|
||||
None,
|
||||
None,
|
||||
)),
|
||||
}
|
||||
}
|
||||
|
||||
fn configure_exti4(rcc: &stm32f722::RCC, syscfg: stm32f722::SYSCFG, exti: stm32f722::EXTI) {
|
||||
rcc.apb2enr().modify(|_, write| write.syscfgen().set_bit());
|
||||
syscfg.exticr2().modify(|_, write| {
|
||||
// SAFETY: selector zero is GPIOA for EXTI4 on STM32F722.
|
||||
unsafe { write.exti4().bits(0) }
|
||||
});
|
||||
exti.imr().modify(|_, write| write.im4().set_bit());
|
||||
exti.rtsr().modify(|_, write| write.tr4().set_bit());
|
||||
exti.ftsr().modify(|_, write| write.tr4().clear_bit());
|
||||
exti.pr().write(|write| write.pr4().clear());
|
||||
}
|
||||
|
||||
fn configure_exti3(rcc: &stm32f722::RCC, syscfg: stm32f722::SYSCFG, exti: stm32f722::EXTI) {
|
||||
rcc.apb2enr().modify(|_, write| write.syscfgen().set_bit());
|
||||
syscfg.exticr1().modify(|_, write| {
|
||||
// SAFETY: selector two is GPIOC for EXTI3 on STM32F722.
|
||||
unsafe { write.exti3().bits(2) }
|
||||
});
|
||||
exti.imr().modify(|_, write| write.im3().set_bit());
|
||||
exti.rtsr().modify(|_, write| write.tr3().set_bit());
|
||||
exti.ftsr().modify(|_, write| write.tr3().clear_bit());
|
||||
exti.pr().write(|write| write.pr3().clear());
|
||||
}
|
||||
|
||||
/// Services the MTF-01 UART route. It is a task-local resource, so it needs
|
||||
/// no RTIC lock and cannot extend another resource's priority ceiling.
|
||||
pub(crate) fn on_uart4_interrupt(mtf01: &mut Option<F722Mtf01>) {
|
||||
let Some(mtf01) = mtf01.as_mut() else {
|
||||
return;
|
||||
};
|
||||
match Uart4::acknowledge_receive_interrupt() {
|
||||
ReceiveInterrupt::Byte(byte) => {
|
||||
if let Some(report) = mtf01.on_received_byte(byte) {
|
||||
if let Some(range) = report.range_snapshot() {
|
||||
RANGES.publish(range);
|
||||
}
|
||||
if let Some(flow) = report.optical_flow_snapshot() {
|
||||
OPTICAL_FLOW.publish(flow);
|
||||
}
|
||||
}
|
||||
}
|
||||
ReceiveInterrupt::Fault => mtf01.reset(),
|
||||
ReceiveInterrupt::NotPending => {}
|
||||
}
|
||||
}
|
||||
|
||||
impl F722LidarState {
|
||||
pub(crate) fn on_i2c_event(&mut self, error: bool) {
|
||||
let Some(range) = self.range.as_mut() else {
|
||||
return;
|
||||
};
|
||||
let event = range.on_controller_interrupt(|i2c| {
|
||||
if error {
|
||||
i2c.acknowledge_error_interrupt()
|
||||
} else {
|
||||
i2c.acknowledge_event_interrupt()
|
||||
}
|
||||
});
|
||||
publish_lidar_event(event);
|
||||
}
|
||||
|
||||
pub(crate) fn on_timer(&mut self) {
|
||||
if let Some(range) = self.range.as_mut() {
|
||||
if self.startup_ticks < LIDAR_STARTUP_TICKS {
|
||||
self.startup_ticks += 1;
|
||||
if self.startup_ticks == LIDAR_STARTUP_TICKS {
|
||||
let _ = range.finish_startup();
|
||||
}
|
||||
} else {
|
||||
publish_lidar_event(range.on_periodic_tick());
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/// Advances the IMU state machine in its dedicated worker-local resource.
|
||||
pub(crate) fn process_imu(imu: &mut F722Imu, events: u8) {
|
||||
const START: u8 = 1 << 4;
|
||||
const TRANSFER_FAULT: u8 = 1 << 2;
|
||||
const TRANSFER_COMPLETE: u8 = 1 << 1;
|
||||
const TIMER: u8 = 1 << 3;
|
||||
const DRDY: u8 = 1 << 0;
|
||||
|
||||
if events & START != 0 {
|
||||
accept_imu_event(imu.begin());
|
||||
}
|
||||
if events & TRANSFER_FAULT != 0 {
|
||||
accept_imu_event(imu.on_transfer_complete(SpiCompletion::TransferFault));
|
||||
}
|
||||
if events & TRANSFER_COMPLETE != 0 {
|
||||
accept_imu_event(imu.on_transfer_complete(SpiCompletion::Complete));
|
||||
}
|
||||
if events & TIMER != 0 {
|
||||
accept_imu_event(imu.on_periodic_tick());
|
||||
}
|
||||
if events & DRDY != 0 {
|
||||
accept_imu_event(imu.on_data_ready());
|
||||
}
|
||||
}
|
||||
|
||||
fn accept_imu_event(event: ImuEvent) {
|
||||
match event {
|
||||
ImuEvent::Sample(sample) => {
|
||||
IMU_HEALTHY.store(true, Ordering::Release);
|
||||
SAMPLES.publish(sample);
|
||||
}
|
||||
ImuEvent::Fault => IMU_HEALTHY.store(false, Ordering::Release),
|
||||
ImuEvent::Idle | ImuEvent::Initialized => {}
|
||||
}
|
||||
}
|
||||
|
||||
pub(crate) fn imu_is_healthy() -> bool {
|
||||
IMU_HEALTHY.load(Ordering::Acquire)
|
||||
}
|
||||
|
||||
impl F722MavlinkState {
|
||||
pub(crate) fn on_mavlink_byte(&mut self, receive: ReceiveInterrupt) -> bool {
|
||||
match receive {
|
||||
ReceiveInterrupt::Byte(byte) => self.receiver.receive_byte(byte),
|
||||
ReceiveInterrupt::Fault => {
|
||||
self.receiver.reset();
|
||||
false
|
||||
}
|
||||
ReceiveInterrupt::NotPending => false,
|
||||
}
|
||||
}
|
||||
|
||||
/// Handles the bounded DMA and timer work that does not need the motor
|
||||
/// resource. `motors_armed` is sampled under its separate RTIC lock.
|
||||
pub(crate) fn process_worker_events(&mut self, events: u8, motors_armed: bool) {
|
||||
const TIMER: u8 = 1 << 0;
|
||||
const DMA: u8 = 1 << 2;
|
||||
|
||||
if events & DMA != 0 {
|
||||
let completion = match UART_COMPLETION.swap(0, Ordering::AcqRel) {
|
||||
1 => kaptan_dma::TxCompletion::Complete,
|
||||
2 => kaptan_dma::TxCompletion::TransferFault,
|
||||
_ => kaptan_dma::TxCompletion::NotPending,
|
||||
};
|
||||
self.data_channel.on_dma_complete(completion);
|
||||
if completion == kaptan_dma::TxCompletion::Complete {
|
||||
self.led.toggle();
|
||||
}
|
||||
}
|
||||
|
||||
if events & TIMER != 0 {
|
||||
self.data_channel
|
||||
.on_service_tick(IMU_HEALTHY.load(Ordering::Acquire), motors_armed);
|
||||
}
|
||||
}
|
||||
|
||||
/// Removes already-decoded non-arm frames and returns one arm command at a
|
||||
/// time. The worker releases this resource before taking the motor lock.
|
||||
pub(crate) fn next_arm_request(&mut self) -> Option<ArmRequest> {
|
||||
while let Some(frame) = self.receiver.take_received() {
|
||||
if let Some(request) = decode_arm_request(
|
||||
&frame,
|
||||
self.config.mavlink.system_id,
|
||||
self.config.mavlink.component_id,
|
||||
) {
|
||||
return Some(request);
|
||||
}
|
||||
}
|
||||
None
|
||||
}
|
||||
|
||||
pub(crate) fn acknowledge_arm_request(&mut self, result: kaptan_mavlink::message::MavResult) {
|
||||
self.data_channel.acknowledge_command(
|
||||
kaptan_mavlink::message::MavCmd::MAV_CMD_COMPONENT_ARM_DISARM,
|
||||
result,
|
||||
);
|
||||
}
|
||||
}
|
||||
|
||||
pub fn record_mavlink_completion(completion: kaptan_dma::TxCompletion) {
|
||||
let code = match completion {
|
||||
kaptan_dma::TxCompletion::Complete => 1,
|
||||
kaptan_dma::TxCompletion::TransferFault | kaptan_dma::TxCompletion::Halted => 2,
|
||||
kaptan_dma::TxCompletion::NotPending => 0,
|
||||
};
|
||||
if code != 0 {
|
||||
UART_COMPLETION.store(code, Ordering::Release);
|
||||
}
|
||||
}
|
||||
|
||||
fn publish_lidar_event<E>(event: LidarLiteV3ServiceEvent<E>) {
|
||||
if let LidarLiteV3ServiceEvent::Measurement(snapshot) = event {
|
||||
RANGES.publish(snapshot);
|
||||
}
|
||||
}
|
||||
|
||||
#[cfg(test)]
|
||||
mod tests {
|
||||
use super::validate_vertical_route;
|
||||
use crate::{F7BoardBuilder, Mavlink, MotorOutputs, Mpu6000, Pa4, VerticalSensor};
|
||||
use kaptan_board::{BoardConfig, DataChannelRates, MavlinkEndpoint, VerticalSensingKind};
|
||||
|
||||
#[test]
|
||||
fn rejects_metadata_that_claims_flow_for_a_lidar_route() {
|
||||
let routes = F7BoardBuilder::new()
|
||||
.imu(Mpu6000::on_spi1().with_drdy(Pa4))
|
||||
.motors(MotorOutputs::kakute_tim3_tim5().dshot600())
|
||||
.mavlink(Mavlink::on_usart1())
|
||||
.vertical_sensor(VerticalSensor::lidar_lite_v3_on_i2c1())
|
||||
.build()
|
||||
.unwrap();
|
||||
let config = BoardConfig {
|
||||
name: "invalid",
|
||||
mavlink: MavlinkEndpoint {
|
||||
system_id: 1,
|
||||
component_id: 1,
|
||||
},
|
||||
vertical_sensing: VerticalSensingKind::RangeAndOpticalFlow,
|
||||
data_channel: DataChannelRates {
|
||||
service_hz: 100,
|
||||
raw_imu_hz: 20,
|
||||
range_hz: Some(20),
|
||||
optical_flow_hz: Some(20),
|
||||
heartbeat_hz: 2,
|
||||
},
|
||||
};
|
||||
assert_eq!(
|
||||
validate_vertical_route(config, routes),
|
||||
Err(crate::BoardError::VerticalSensorCapabilityMismatch)
|
||||
);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,25 @@
|
||||
//! STM32F722 Pll216 clock-source assembly.
|
||||
|
||||
use super::peripherals::{DwtMicrosecondClock, select_pll216_profile};
|
||||
use stm32f7::stm32f722;
|
||||
|
||||
/// Core frequency in whole megahertz after selecting the PLL216 board profile.
|
||||
const CORE_CLOCK_MHZ: u32 = 216;
|
||||
|
||||
/// A failure while establishing the board's clock tree or monotonic clock.
|
||||
pub(crate) enum ClockInitializationError {
|
||||
Profile,
|
||||
Monotonic,
|
||||
}
|
||||
|
||||
/// Selects the F722 Pll216 profile and starts its monotonic microsecond clock.
|
||||
pub(crate) fn initialize(
|
||||
core: &mut cortex_m::Peripherals,
|
||||
device: &stm32f722::Peripherals,
|
||||
) -> Result<DwtMicrosecondClock, ClockInitializationError> {
|
||||
select_pll216_profile(&device.RCC, &device.PWR, &device.FLASH)
|
||||
.map_err(|_| ClockInitializationError::Profile)?;
|
||||
core.SCB.disable_dcache(&mut core.CPUID);
|
||||
DwtMicrosecondClock::start::<CORE_CLOCK_MHZ>(&mut core.DCB, &mut core.DWT)
|
||||
.map_err(|_| ClockInitializationError::Monotonic)
|
||||
}
|
||||
@@ -0,0 +1,159 @@
|
||||
//! F722 transport and vertical-route adapters for the portable data channel.
|
||||
|
||||
use super::{VerticalRoute, peripherals};
|
||||
use kaptan_data_channel::{FrameSink, VerticalDataConfig};
|
||||
use kaptan_dma::TxCompletion;
|
||||
use kaptan_mavlink::{
|
||||
MavlinkFrame,
|
||||
encoder::DistanceSensorConfig,
|
||||
transmitter::{FrameClass, MavlinkTransmitter},
|
||||
};
|
||||
|
||||
pub(crate) const UART1_CLOCK_HZ: u32 = 108_000_000;
|
||||
pub(crate) const UART2_CLOCK_HZ: u32 = 54_000_000;
|
||||
pub(crate) const MAVLINK_BAUD: u32 = 115_200;
|
||||
pub(crate) const SERVICE_HZ: usize = 100;
|
||||
|
||||
/// Fixed service-timer configuration for the validated Pll216 profile.
|
||||
pub(crate) const SERVICE_TIMER_CONFIG: peripherals::Tim2PeriodicConfig =
|
||||
peripherals::Tim2PeriodicConfig {
|
||||
prescaler: 10_799,
|
||||
auto_reload: 99,
|
||||
};
|
||||
|
||||
/// Maps a selected vertical electrical route to complete data-channel metadata.
|
||||
pub(crate) fn vertical_data_config(route: VerticalRoute) -> VerticalDataConfig {
|
||||
let distance =
|
||||
|minimum_distance_centimetres, maximum_distance_centimetres| DistanceSensorConfig {
|
||||
sensor_id: 0,
|
||||
orientation:
|
||||
kaptan_mavlink::message::MavSensorOrientation::MAV_SENSOR_ROTATION_PITCH_270,
|
||||
minimum_distance_centimetres,
|
||||
maximum_distance_centimetres,
|
||||
};
|
||||
|
||||
match route {
|
||||
VerticalRoute::None => VerticalDataConfig {
|
||||
distance: None,
|
||||
optical_flow_sensor_id: None,
|
||||
maximum_age_ticks: 10,
|
||||
},
|
||||
VerticalRoute::Mtf01 => VerticalDataConfig {
|
||||
distance: Some(distance(2, 800)),
|
||||
optical_flow_sensor_id: Some(0),
|
||||
maximum_age_ticks: 10,
|
||||
},
|
||||
VerticalRoute::LidarLiteV3 => VerticalDataConfig {
|
||||
distance: Some(distance(20, 4_000)),
|
||||
optical_flow_sensor_id: None,
|
||||
maximum_age_ticks: 10,
|
||||
},
|
||||
}
|
||||
}
|
||||
|
||||
/// The two F722 DMA engines behind the portable data-channel transport.
|
||||
pub(crate) enum F722MavlinkTransmitter {
|
||||
Usart1(MavlinkTransmitter<peripherals::DcacheDisabledDmaMemory, peripherals::Usart1TxDma, 4>),
|
||||
Usart2(MavlinkTransmitter<peripherals::DcacheDisabledDmaMemory, peripherals::Usart2TxDma, 4>),
|
||||
}
|
||||
|
||||
impl F722MavlinkTransmitter {
|
||||
pub(crate) fn usart1(
|
||||
rcc: &stm32f7::stm32f722::RCC,
|
||||
uart: stm32f7::stm32f722::USART1,
|
||||
) -> Result<Self, ()> {
|
||||
let uart =
|
||||
peripherals::Usart1Tx::new(rcc, uart, UART1_CLOCK_HZ, MAVLINK_BAUD).map_err(|_| ())?;
|
||||
// SAFETY: platform initialization disables D-cache before this object
|
||||
// is created, and the active MAVLink frame remains queue-owned until
|
||||
// this platform receives a terminal DMA completion notification.
|
||||
let memory = unsafe { peripherals::DcacheDisabledDmaMemory::new() };
|
||||
Ok(Self::Usart1(MavlinkTransmitter::new(
|
||||
memory,
|
||||
peripherals::Usart1TxDma::new(rcc, uart),
|
||||
)))
|
||||
}
|
||||
|
||||
pub(crate) fn usart2(
|
||||
rcc: &stm32f7::stm32f722::RCC,
|
||||
uart: stm32f7::stm32f722::USART2,
|
||||
) -> Result<Self, ()> {
|
||||
let uart =
|
||||
peripherals::Usart2Tx::new(rcc, uart, UART2_CLOCK_HZ, MAVLINK_BAUD).map_err(|_| ())?;
|
||||
// SAFETY: identical DMA-memory invariant to the USART1 route above;
|
||||
// only the fixed platform DMA endpoint differs.
|
||||
let memory = unsafe { peripherals::DcacheDisabledDmaMemory::new() };
|
||||
Ok(Self::Usart2(MavlinkTransmitter::new(
|
||||
memory,
|
||||
peripherals::Usart2TxDma::new(rcc, uart),
|
||||
)))
|
||||
}
|
||||
}
|
||||
|
||||
impl FrameSink for F722MavlinkTransmitter {
|
||||
fn send(&mut self, class: FrameClass, frame: MavlinkFrame) {
|
||||
match self {
|
||||
Self::Usart1(transmitter) => {
|
||||
let _ = transmitter.send_frame(class, frame);
|
||||
}
|
||||
Self::Usart2(transmitter) => {
|
||||
let _ = transmitter.send_frame(class, frame);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
fn on_dma_complete(&mut self, completion: TxCompletion) {
|
||||
match self {
|
||||
Self::Usart1(transmitter) => transmitter.on_dma_complete(completion),
|
||||
Self::Usart2(transmitter) => transmitter.on_dma_complete(completion),
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/// Board-route status light; it is not part of a board declaration API.
|
||||
pub(crate) enum F722StatusLed {
|
||||
Kakute(peripherals::ActiveLowLedPd2),
|
||||
Geprc(GeprcStatusLed),
|
||||
}
|
||||
|
||||
impl F722StatusLed {
|
||||
pub(crate) fn toggle(&mut self) {
|
||||
match self {
|
||||
Self::Kakute(led) => led.toggle(),
|
||||
Self::Geprc(led) => led.toggle(),
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/// GEPRC GEP-F722-HD V2 blue status LED on PA13.
|
||||
pub(crate) struct GeprcStatusLed {
|
||||
port: stm32f7::stm32f722::GPIOA,
|
||||
is_high: bool,
|
||||
}
|
||||
|
||||
impl GeprcStatusLed {
|
||||
pub(crate) fn new(rcc: &stm32f7::stm32f722::RCC, port: stm32f7::stm32f722::GPIOA) -> Self {
|
||||
rcc.ahb1enr().modify(|_, write| write.gpioaen().set_bit());
|
||||
port.moder().modify(|_, write| write.moder13().output());
|
||||
port.otyper().modify(|_, write| write.ot13().push_pull());
|
||||
port.ospeedr()
|
||||
.modify(|_, write| write.ospeedr13().low_speed());
|
||||
port.pupdr().modify(|_, write| write.pupdr13().floating());
|
||||
port.bsrr().write(|write| write.br13().set_bit());
|
||||
Self {
|
||||
port,
|
||||
is_high: false,
|
||||
}
|
||||
}
|
||||
|
||||
fn toggle(&mut self) {
|
||||
self.is_high = !self.is_high;
|
||||
self.port.bsrr().write(|write| {
|
||||
if self.is_high {
|
||||
write.bs13().set_bit()
|
||||
} else {
|
||||
write.br13().set_bit()
|
||||
}
|
||||
});
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,59 @@
|
||||
//! Focused physical-route diagnostics for supported F722 boards.
|
||||
|
||||
use super::peripherals::{Usart2Tx, configure_usart2_pins_pa2_pa3, select_pll216_profile};
|
||||
use cortex_m_rt as _;
|
||||
use panic_halt as _;
|
||||
use stm32f7::stm32f722;
|
||||
|
||||
const UART2_CLOCK_HZ: u32 = 54_000_000;
|
||||
const UART2_BAUD: u32 = 115_200;
|
||||
const SQUARE_WAVE_BYTE: u8 = 0x55;
|
||||
const LED_TOGGLE_INTERVAL_BYTES: u16 = 11_520;
|
||||
|
||||
/// Continuously transmits `0x55` on the GEP-F722-HD V2's T2 pad.
|
||||
///
|
||||
/// This intentionally initializes only the clock tree, PA2/PA3, and USART2.
|
||||
/// It does not enable RTIC, DMA, MAVLink, sensors, timers, or motor outputs.
|
||||
/// The PA13 blue LED also changes state roughly once per second, proving the
|
||||
/// test passed clock and UART initialization. A host at 115200 8N1 must
|
||||
/// receive a continuous sequence of `55` bytes.
|
||||
pub fn geprc_uart2_wire_test() -> ! {
|
||||
// SAFETY: this diagnostic installs the sole reset entry point and takes no
|
||||
// interrupt or RTIC path, so no other code can own these peripherals.
|
||||
let device = unsafe { stm32f722::Peripherals::steal() };
|
||||
|
||||
select_pll216_profile(&device.RCC, &device.PWR, &device.FLASH)
|
||||
.expect("could not select F722 PLL216 clock profile");
|
||||
device
|
||||
.RCC
|
||||
.ahb1enr()
|
||||
.modify(|_, write| write.gpioaen().set_bit());
|
||||
configure_usart2_pins_pa2_pa3(&device.GPIOA);
|
||||
device
|
||||
.GPIOA
|
||||
.moder()
|
||||
.modify(|_, write| write.moder13().output());
|
||||
// The V2's blue LED is active low. Start it off so each following toggle
|
||||
// is visible; PA13 is otherwise unused by this standalone diagnostic.
|
||||
device.GPIOA.bsrr().write(|write| write.bs13().set_bit());
|
||||
let uart = Usart2Tx::new(&device.RCC, device.USART2, UART2_CLOCK_HZ, UART2_BAUD)
|
||||
.expect("could not initialize USART2");
|
||||
|
||||
let mut bytes_since_led_toggle = 0;
|
||||
let mut led_is_on = false;
|
||||
loop {
|
||||
uart.write_blocking(SQUARE_WAVE_BYTE);
|
||||
bytes_since_led_toggle += 1;
|
||||
if bytes_since_led_toggle == LED_TOGGLE_INTERVAL_BYTES {
|
||||
bytes_since_led_toggle = 0;
|
||||
led_is_on = !led_is_on;
|
||||
device.GPIOA.bsrr().write(|write| {
|
||||
if led_is_on {
|
||||
write.br13().set_bit()
|
||||
} else {
|
||||
write.bs13().set_bit()
|
||||
}
|
||||
});
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,328 @@
|
||||
//! The one platform-owned RTIC task graph for all supported F722 profiles.
|
||||
|
||||
/// Defines the F722 application in the platform crate. All interrupt bindings
|
||||
/// are the finite platform-owned superset. Profile activation determines which
|
||||
/// acknowledged route has initialized hardware.
|
||||
#[cfg(target_arch = "arm")]
|
||||
macro_rules! f722_flight_application {
|
||||
($routes:path, $config:path) => {
|
||||
#[allow(unused_imports)]
|
||||
use $crate::__platform_internal::cmrt as _;
|
||||
#[allow(unused_imports)]
|
||||
use $crate::__platform_internal::panic as _;
|
||||
|
||||
// RTIC 2.3 expands hardware-vector shims that call unsafe PAC token
|
||||
// constructors inside generated unsafe functions. Those shims are
|
||||
// generated by RTIC, not hand-written platform accesses; all platform
|
||||
// unsafe register operations remain documented at their source.
|
||||
#[allow(unsafe_op_in_unsafe_fn)]
|
||||
#[$crate::__platform_internal::rtic_app::app(
|
||||
device = $crate::__platform_internal::pac::stm32f722,
|
||||
peripherals = true,
|
||||
dispatchers = [TIM6_DAC, TIM7]
|
||||
)]
|
||||
mod kaptan_f722_platform_app {
|
||||
use core::sync::atomic::{AtomicBool, AtomicU8, Ordering};
|
||||
|
||||
use $crate::f722::{
|
||||
F722Imu, F722InitResources, F722LidarState, F722MavlinkState, F722Mtf01,
|
||||
F722Mtf01Uart, F722ServiceTimer, acknowledge_exti3, acknowledge_exti4,
|
||||
acknowledge_imu_rx_dma1, acknowledge_imu_rx_dma2, acknowledge_imu_spi1_error,
|
||||
acknowledge_imu_spi3_error, acknowledge_imu_tx_dma1, acknowledge_imu_tx_dma2,
|
||||
acknowledge_mavlink_dma1, acknowledge_mavlink_dma2, acknowledge_mavlink_rx_usart1,
|
||||
acknowledge_mavlink_rx_usart2, imu_is_healthy, initialize, on_uart4_interrupt,
|
||||
process_imu, record_mavlink_completion,
|
||||
};
|
||||
use $crate::actuator::F722MotorOutput;
|
||||
use $crate::__platform_internal::spi::SpiCompletion;
|
||||
#[allow(unused_imports)]
|
||||
use $crate::__platform_internal::rtic_app::Mutex;
|
||||
|
||||
const IMU_EVENT_DRDY: u8 = 1 << 0;
|
||||
const IMU_EVENT_TRANSFER_COMPLETE: u8 = 1 << 1;
|
||||
const IMU_EVENT_TRANSFER_FAULT: u8 = 1 << 2;
|
||||
const IMU_EVENT_TIMER: u8 = 1 << 3;
|
||||
const IMU_EVENT_START: u8 = 1 << 4;
|
||||
const MAVLINK_EVENT_TIMER: u8 = 1 << 0;
|
||||
const MAVLINK_EVENT_DMA: u8 = 1 << 2;
|
||||
const MAVLINK_EVENT_RECEIVED: u8 = 1 << 3;
|
||||
|
||||
// Hardware IRQs and both software workers coalesce event bits.
|
||||
// Release/Acquire pairs make each terminal acknowledgement visible
|
||||
// to the worker that consumes it; the queued bit prevents unbounded
|
||||
// software-task backlog while preserving the latest pending work.
|
||||
static IMU_EVENTS: AtomicU8 = AtomicU8::new(0);
|
||||
static MAVLINK_EVENTS: AtomicU8 = AtomicU8::new(0);
|
||||
static IMU_QUEUED: AtomicBool = AtomicBool::new(false);
|
||||
static MAVLINK_QUEUED: AtomicBool = AtomicBool::new(false);
|
||||
|
||||
#[shared]
|
||||
struct Shared {
|
||||
mavlink: F722MavlinkState,
|
||||
motors: F722MotorOutput,
|
||||
lidar: F722LidarState,
|
||||
}
|
||||
|
||||
#[local]
|
||||
struct Local {
|
||||
imu: F722Imu,
|
||||
mtf01: Option<F722Mtf01>,
|
||||
_mtf01_uart: Option<F722Mtf01Uart>,
|
||||
timer: F722ServiceTimer,
|
||||
}
|
||||
|
||||
#[init]
|
||||
fn init(context: init::Context) -> (Shared, Local) {
|
||||
let routes = $routes().unwrap_or_else(|_| safe_idle());
|
||||
let F722InitResources {
|
||||
imu,
|
||||
mtf01,
|
||||
mtf01_uart,
|
||||
timer,
|
||||
lidar,
|
||||
mavlink,
|
||||
motors,
|
||||
} = initialize(context.core, context.device, $config(), routes)
|
||||
.unwrap_or_else(|_| safe_idle());
|
||||
schedule_imu(IMU_EVENT_START);
|
||||
(
|
||||
Shared {
|
||||
mavlink,
|
||||
motors,
|
||||
lidar,
|
||||
},
|
||||
Local {
|
||||
imu,
|
||||
mtf01,
|
||||
_mtf01_uart: mtf01_uart,
|
||||
timer,
|
||||
},
|
||||
)
|
||||
}
|
||||
|
||||
#[idle]
|
||||
fn idle(_: idle::Context) -> ! {
|
||||
loop {
|
||||
$crate::__platform_internal::cm::asm::wfi();
|
||||
}
|
||||
}
|
||||
|
||||
#[task(binds = EXTI4, priority = 15)]
|
||||
fn exti4(_: exti4::Context) {
|
||||
if acknowledge_exti4() {
|
||||
schedule_imu(IMU_EVENT_DRDY);
|
||||
}
|
||||
}
|
||||
|
||||
#[task(binds = EXTI3, priority = 15)]
|
||||
fn exti3(_: exti3::Context) {
|
||||
if acknowledge_exti3() {
|
||||
schedule_imu(IMU_EVENT_DRDY);
|
||||
}
|
||||
}
|
||||
|
||||
#[task(binds = DMA2_STREAM0, priority = 14)]
|
||||
fn imu_rx_dma2(_: imu_rx_dma2::Context) {
|
||||
match acknowledge_imu_rx_dma2() {
|
||||
SpiCompletion::Complete => schedule_imu(IMU_EVENT_TRANSFER_COMPLETE),
|
||||
SpiCompletion::TransferFault => schedule_imu(IMU_EVENT_TRANSFER_FAULT),
|
||||
SpiCompletion::NotPending => {}
|
||||
}
|
||||
}
|
||||
|
||||
#[task(binds = DMA2_STREAM3, priority = 14)]
|
||||
fn imu_tx_dma2(_: imu_tx_dma2::Context) {
|
||||
if acknowledge_imu_tx_dma2() == SpiCompletion::TransferFault {
|
||||
schedule_imu(IMU_EVENT_TRANSFER_FAULT);
|
||||
}
|
||||
}
|
||||
|
||||
#[task(binds = SPI1, priority = 14)]
|
||||
fn imu_spi1_error(_: imu_spi1_error::Context) {
|
||||
if acknowledge_imu_spi1_error() {
|
||||
schedule_imu(IMU_EVENT_TRANSFER_FAULT);
|
||||
}
|
||||
}
|
||||
|
||||
#[task(binds = DMA1_STREAM0, priority = 14)]
|
||||
fn imu_rx_dma1(_: imu_rx_dma1::Context) {
|
||||
match acknowledge_imu_rx_dma1() {
|
||||
SpiCompletion::Complete => schedule_imu(IMU_EVENT_TRANSFER_COMPLETE),
|
||||
SpiCompletion::TransferFault => schedule_imu(IMU_EVENT_TRANSFER_FAULT),
|
||||
SpiCompletion::NotPending => {}
|
||||
}
|
||||
}
|
||||
|
||||
#[task(binds = DMA1_STREAM5, priority = 14)]
|
||||
fn imu_tx_dma1(_: imu_tx_dma1::Context) {
|
||||
if acknowledge_imu_tx_dma1() == SpiCompletion::TransferFault {
|
||||
schedule_imu(IMU_EVENT_TRANSFER_FAULT);
|
||||
}
|
||||
}
|
||||
|
||||
#[task(binds = SPI3, priority = 14)]
|
||||
fn imu_spi3_error(_: imu_spi3_error::Context) {
|
||||
if acknowledge_imu_spi3_error() {
|
||||
schedule_imu(IMU_EVENT_TRANSFER_FAULT);
|
||||
}
|
||||
}
|
||||
|
||||
#[task(binds = UART4, priority = 14, local = [mtf01])]
|
||||
fn uart4(context: uart4::Context) {
|
||||
on_uart4_interrupt(context.local.mtf01);
|
||||
}
|
||||
|
||||
#[task(binds = TIM3, priority = 12, shared = [motors])]
|
||||
fn motor_update(mut context: motor_update::Context) {
|
||||
context.shared.motors.lock(F722MotorOutput::on_tim3_interrupt);
|
||||
}
|
||||
|
||||
#[task(binds = TIM5, priority = 12, shared = [motors])]
|
||||
fn motor_scheduler(mut context: motor_scheduler::Context) {
|
||||
context.shared.motors.lock(F722MotorOutput::on_tim5_interrupt);
|
||||
}
|
||||
|
||||
#[task(binds = I2C1_EV, priority = 11, shared = [lidar])]
|
||||
fn i2c1_event(mut context: i2c1_event::Context) {
|
||||
context
|
||||
.shared
|
||||
.lidar
|
||||
.lock(|lidar| lidar.on_i2c_event(false));
|
||||
}
|
||||
|
||||
#[task(binds = I2C1_ER, priority = 11, shared = [lidar])]
|
||||
fn i2c1_error(mut context: i2c1_error::Context) {
|
||||
context
|
||||
.shared
|
||||
.lidar
|
||||
.lock(|lidar| lidar.on_i2c_event(true));
|
||||
}
|
||||
|
||||
#[task(binds = DMA2_STREAM7, priority = 10)]
|
||||
fn mavlink_dma2(_: mavlink_dma2::Context) {
|
||||
let completion = acknowledge_mavlink_dma2();
|
||||
if completion != $crate::__platform_internal::dma::TxCompletion::NotPending {
|
||||
record_mavlink_completion(completion);
|
||||
schedule_mavlink(MAVLINK_EVENT_DMA);
|
||||
}
|
||||
}
|
||||
|
||||
#[task(binds = DMA1_STREAM6, priority = 10)]
|
||||
fn mavlink_dma1(_: mavlink_dma1::Context) {
|
||||
let completion = acknowledge_mavlink_dma1();
|
||||
if completion != $crate::__platform_internal::dma::TxCompletion::NotPending {
|
||||
record_mavlink_completion(completion);
|
||||
schedule_mavlink(MAVLINK_EVENT_DMA);
|
||||
}
|
||||
}
|
||||
|
||||
#[task(binds = USART1, priority = 8, shared = [mavlink])]
|
||||
fn mavlink_rx_usart1(mut context: mavlink_rx_usart1::Context) {
|
||||
let admitted = context
|
||||
.shared
|
||||
.mavlink
|
||||
.lock(|mavlink| mavlink.on_mavlink_byte(acknowledge_mavlink_rx_usart1()));
|
||||
if admitted {
|
||||
schedule_mavlink(MAVLINK_EVENT_RECEIVED);
|
||||
}
|
||||
}
|
||||
|
||||
#[task(binds = USART2, priority = 8, shared = [mavlink])]
|
||||
fn mavlink_rx_usart2(mut context: mavlink_rx_usart2::Context) {
|
||||
let admitted = context
|
||||
.shared
|
||||
.mavlink
|
||||
.lock(|mavlink| mavlink.on_mavlink_byte(acknowledge_mavlink_rx_usart2()));
|
||||
if admitted {
|
||||
schedule_mavlink(MAVLINK_EVENT_RECEIVED);
|
||||
}
|
||||
}
|
||||
|
||||
#[task(binds = TIM2, priority = 4, local = [timer], shared = [lidar])]
|
||||
fn tim2(mut context: tim2::Context) {
|
||||
context.local.timer.acknowledge_update();
|
||||
context.shared.lidar.lock(F722LidarState::on_timer);
|
||||
schedule_imu(IMU_EVENT_TIMER);
|
||||
schedule_mavlink(MAVLINK_EVENT_TIMER);
|
||||
}
|
||||
|
||||
#[task(priority = 13, local = [imu])]
|
||||
async fn imu_worker(context: imu_worker::Context) {
|
||||
let events = IMU_EVENTS.swap(0, Ordering::AcqRel);
|
||||
process_imu(context.local.imu, events);
|
||||
IMU_QUEUED.store(false, Ordering::Release);
|
||||
if IMU_EVENTS.load(Ordering::Acquire) != 0
|
||||
&& !IMU_QUEUED.swap(true, Ordering::AcqRel)
|
||||
&& imu_worker::spawn().is_err()
|
||||
{
|
||||
IMU_QUEUED.store(false, Ordering::Release);
|
||||
}
|
||||
}
|
||||
|
||||
#[task(priority = 9, shared = [mavlink, motors])]
|
||||
async fn mavlink_worker(mut context: mavlink_worker::Context) {
|
||||
let events = MAVLINK_EVENTS.swap(0, Ordering::AcqRel);
|
||||
let motors_armed = context.shared.motors.lock(|motors| motors.is_armed());
|
||||
context
|
||||
.shared
|
||||
.mavlink
|
||||
.lock(|mavlink| mavlink.process_worker_events(events, motors_armed));
|
||||
if events & MAVLINK_EVENT_RECEIVED != 0 {
|
||||
while let Some(request) =
|
||||
context.shared.mavlink.lock(F722MavlinkState::next_arm_request)
|
||||
{
|
||||
let result = match request {
|
||||
$crate::__platform_internal::mavlink::commands::arm::ArmRequest::Arm if !imu_is_healthy() =>
|
||||
{
|
||||
$crate::__platform_internal::mavlink::message::MavResult::MAV_RESULT_DENIED
|
||||
}
|
||||
$crate::__platform_internal::mavlink::commands::arm::ArmRequest::Arm => {
|
||||
context.shared.motors.lock(F722MotorOutput::arm);
|
||||
$crate::__platform_internal::mavlink::message::MavResult::MAV_RESULT_ACCEPTED
|
||||
}
|
||||
$crate::__platform_internal::mavlink::commands::arm::ArmRequest::Disarm => {
|
||||
context.shared.motors.lock(F722MotorOutput::disarm);
|
||||
$crate::__platform_internal::mavlink::message::MavResult::MAV_RESULT_ACCEPTED
|
||||
}
|
||||
};
|
||||
context
|
||||
.shared
|
||||
.mavlink
|
||||
.lock(|mavlink| mavlink.acknowledge_arm_request(result));
|
||||
}
|
||||
}
|
||||
MAVLINK_QUEUED.store(false, Ordering::Release);
|
||||
if MAVLINK_EVENTS.load(Ordering::Acquire) != 0
|
||||
&& !MAVLINK_QUEUED.swap(true, Ordering::AcqRel)
|
||||
&& mavlink_worker::spawn().is_err()
|
||||
{
|
||||
MAVLINK_QUEUED.store(false, Ordering::Release);
|
||||
}
|
||||
}
|
||||
|
||||
fn schedule_imu(event: u8) {
|
||||
IMU_EVENTS.fetch_or(event, Ordering::Release);
|
||||
if !IMU_QUEUED.swap(true, Ordering::AcqRel) && imu_worker::spawn().is_err() {
|
||||
IMU_QUEUED.store(false, Ordering::Release);
|
||||
}
|
||||
}
|
||||
|
||||
fn schedule_mavlink(event: u8) {
|
||||
MAVLINK_EVENTS.fetch_or(event, Ordering::Release);
|
||||
if !MAVLINK_QUEUED.swap(true, Ordering::AcqRel) && mavlink_worker::spawn().is_err()
|
||||
{
|
||||
MAVLINK_QUEUED.store(false, Ordering::Release);
|
||||
}
|
||||
}
|
||||
|
||||
fn safe_idle() -> ! {
|
||||
loop {
|
||||
$crate::__platform_internal::cm::asm::wfi();
|
||||
}
|
||||
}
|
||||
}
|
||||
};
|
||||
}
|
||||
|
||||
#[cfg(target_arch = "arm")]
|
||||
pub(crate) use f722_flight_application;
|
||||
@@ -0,0 +1,132 @@
|
||||
//! Platform-owned interrupt acknowledgement for all supported F722 profiles.
|
||||
|
||||
use core::sync::atomic::{AtomicU8, Ordering};
|
||||
|
||||
use super::peripherals::{
|
||||
ReceiveInterrupt, Spi1, Spi3, Usart1Tx, Usart1TxDma, Usart2Tx, Usart2TxDma,
|
||||
};
|
||||
use kaptan_dma::TxCompletion;
|
||||
use kaptan_spi::SpiCompletion;
|
||||
use stm32f7::stm32f722;
|
||||
|
||||
use crate::F722Profile;
|
||||
|
||||
const PROFILE_NONE: u8 = 0;
|
||||
const PROFILE_MPU6000_SPI1_USART1: u8 = 1;
|
||||
const PROFILE_ICM42688_SPI3_USART2: u8 = 2;
|
||||
|
||||
static ACTIVE_PROFILE: AtomicU8 = AtomicU8::new(PROFILE_NONE);
|
||||
|
||||
pub(crate) fn activate_profile(profile: F722Profile) {
|
||||
let encoded = match profile {
|
||||
F722Profile::Mpu6000Spi1Usart1 => PROFILE_MPU6000_SPI1_USART1,
|
||||
F722Profile::Icm42688Spi3Usart2 => PROFILE_ICM42688_SPI3_USART2,
|
||||
};
|
||||
ACTIVE_PROFILE.store(encoded, Ordering::Release);
|
||||
}
|
||||
|
||||
fn active_profile() -> u8 {
|
||||
ACTIVE_PROFILE.load(Ordering::Acquire)
|
||||
}
|
||||
|
||||
pub fn acknowledge_exti4() -> bool {
|
||||
if active_profile() != PROFILE_MPU6000_SPI1_USART1 {
|
||||
return false;
|
||||
}
|
||||
// SAFETY: the selected profile configured EXTI4 once before interrupts.
|
||||
// This ISR writes only the W1C pending bit for PA4 and does not alias the
|
||||
// worker-owned SPI or DMA registers.
|
||||
unsafe {
|
||||
(&*stm32f722::EXTI::ptr())
|
||||
.pr()
|
||||
.write(|write| write.pr4().clear());
|
||||
}
|
||||
true
|
||||
}
|
||||
|
||||
pub fn acknowledge_exti3() -> bool {
|
||||
if active_profile() != PROFILE_ICM42688_SPI3_USART2 {
|
||||
return false;
|
||||
}
|
||||
// SAFETY: the selected profile configured EXTI3 once before interrupts.
|
||||
// This ISR writes only the W1C pending bit for PC3, disjoint from every
|
||||
// peripheral state owned by the deferred IMU worker.
|
||||
unsafe {
|
||||
(&*stm32f722::EXTI::ptr())
|
||||
.pr()
|
||||
.write(|write| write.pr3().clear());
|
||||
}
|
||||
true
|
||||
}
|
||||
|
||||
pub fn acknowledge_imu_rx_dma2() -> SpiCompletion {
|
||||
if active_profile() == PROFILE_MPU6000_SPI1_USART1 {
|
||||
Spi1::acknowledge_rx_interrupt()
|
||||
} else {
|
||||
SpiCompletion::NotPending
|
||||
}
|
||||
}
|
||||
|
||||
pub fn acknowledge_imu_tx_dma2() -> SpiCompletion {
|
||||
if active_profile() == PROFILE_MPU6000_SPI1_USART1 {
|
||||
Spi1::acknowledge_tx_interrupt()
|
||||
} else {
|
||||
SpiCompletion::NotPending
|
||||
}
|
||||
}
|
||||
|
||||
pub fn acknowledge_imu_spi1_error() -> bool {
|
||||
active_profile() == PROFILE_MPU6000_SPI1_USART1 && Spi1::acknowledge_error_interrupt()
|
||||
}
|
||||
|
||||
pub fn acknowledge_imu_rx_dma1() -> SpiCompletion {
|
||||
if active_profile() == PROFILE_ICM42688_SPI3_USART2 {
|
||||
Spi3::acknowledge_rx_interrupt()
|
||||
} else {
|
||||
SpiCompletion::NotPending
|
||||
}
|
||||
}
|
||||
|
||||
pub fn acknowledge_imu_tx_dma1() -> SpiCompletion {
|
||||
if active_profile() == PROFILE_ICM42688_SPI3_USART2 {
|
||||
Spi3::acknowledge_tx_interrupt()
|
||||
} else {
|
||||
SpiCompletion::NotPending
|
||||
}
|
||||
}
|
||||
|
||||
pub fn acknowledge_imu_spi3_error() -> bool {
|
||||
active_profile() == PROFILE_ICM42688_SPI3_USART2 && Spi3::acknowledge_error_interrupt()
|
||||
}
|
||||
|
||||
pub fn acknowledge_mavlink_dma2() -> TxCompletion {
|
||||
if active_profile() == PROFILE_MPU6000_SPI1_USART1 {
|
||||
Usart1TxDma::acknowledge_interrupt()
|
||||
} else {
|
||||
TxCompletion::NotPending
|
||||
}
|
||||
}
|
||||
|
||||
pub fn acknowledge_mavlink_dma1() -> TxCompletion {
|
||||
if active_profile() == PROFILE_ICM42688_SPI3_USART2 {
|
||||
Usart2TxDma::acknowledge_interrupt()
|
||||
} else {
|
||||
TxCompletion::NotPending
|
||||
}
|
||||
}
|
||||
|
||||
pub fn acknowledge_mavlink_rx_usart1() -> ReceiveInterrupt {
|
||||
if active_profile() == PROFILE_MPU6000_SPI1_USART1 {
|
||||
Usart1Tx::acknowledge_receive_interrupt()
|
||||
} else {
|
||||
ReceiveInterrupt::NotPending
|
||||
}
|
||||
}
|
||||
|
||||
pub fn acknowledge_mavlink_rx_usart2() -> ReceiveInterrupt {
|
||||
if active_profile() == PROFILE_ICM42688_SPI3_USART2 {
|
||||
Usart2Tx::acknowledge_receive_interrupt()
|
||||
} else {
|
||||
ReceiveInterrupt::NotPending
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,104 @@
|
||||
//! Finite STM32F722 execution profiles.
|
||||
//!
|
||||
//! This module is the sole owner of STM32F722 register routes and execution
|
||||
//! lifecycle. Its public-in-crate profile names describe useful electrical
|
||||
//! meaning; the RTIC bindings, PAC names, DMA streams and synchronization are
|
||||
//! intentionally contained here and in the macro template.
|
||||
|
||||
#![cfg_attr(not(target_arch = "arm"), allow(dead_code, unused_imports))]
|
||||
|
||||
mod board;
|
||||
mod clock;
|
||||
mod data_channel;
|
||||
#[cfg(target_arch = "arm")]
|
||||
mod diagnostics;
|
||||
mod firmware;
|
||||
mod interrupts;
|
||||
mod peripherals;
|
||||
mod routes;
|
||||
mod sensors;
|
||||
|
||||
use crate::{BoardError, F722BoardRoutes};
|
||||
use kaptan_board::BoardConfig;
|
||||
|
||||
#[derive(Clone, Copy, Debug, Eq, PartialEq)]
|
||||
pub(crate) enum VerticalRoute {
|
||||
None,
|
||||
Mtf01,
|
||||
LidarLiteV3,
|
||||
}
|
||||
|
||||
/// Logical-M1–M4 ordering for the fixed TIM3 physical channels.
|
||||
///
|
||||
/// Both supported boards use PB4/PB5/PB0/PB1 electrically, but their ESC
|
||||
/// connectors assign those pins to logical motors differently. This remains a
|
||||
/// platform-private route choice rather than a board-side execution detail.
|
||||
#[derive(Clone, Copy, Debug, Eq, PartialEq)]
|
||||
pub(crate) enum MotorRoute {
|
||||
KakuteTim3Tim5,
|
||||
GeprcTim3Tim5,
|
||||
}
|
||||
|
||||
pub(crate) use board::{
|
||||
F722InitResources, F722LidarState, F722MavlinkState, F722Mtf01, F722Mtf01Uart,
|
||||
F722ServiceTimer, imu_is_healthy, on_uart4_interrupt, process_imu, record_mavlink_completion,
|
||||
};
|
||||
#[cfg(target_arch = "arm")]
|
||||
pub use diagnostics::geprc_uart2_wire_test;
|
||||
#[cfg(not(target_arch = "arm"))]
|
||||
pub fn geprc_uart2_wire_test() -> ! {
|
||||
loop {
|
||||
core::hint::spin_loop();
|
||||
}
|
||||
}
|
||||
pub(crate) use interrupts::{
|
||||
acknowledge_exti3, acknowledge_exti4, acknowledge_imu_rx_dma1, acknowledge_imu_rx_dma2,
|
||||
acknowledge_imu_spi1_error, acknowledge_imu_spi3_error, acknowledge_imu_tx_dma1,
|
||||
acknowledge_imu_tx_dma2, acknowledge_mavlink_dma1, acknowledge_mavlink_dma2,
|
||||
acknowledge_mavlink_rx_usart1, acknowledge_mavlink_rx_usart2,
|
||||
};
|
||||
pub(crate) use sensors::F722Imu;
|
||||
|
||||
/// Materializes a board declaration after RTIC has exclusively taken the PAC
|
||||
/// peripherals. The route/config check happens before any pin or peripheral
|
||||
/// is initialized, so a malformed board declaration cannot drive hardware.
|
||||
pub fn initialize(
|
||||
core: cortex_m::Peripherals,
|
||||
device: stm32f7::stm32f722::Peripherals,
|
||||
config: BoardConfig,
|
||||
routes: F722BoardRoutes,
|
||||
) -> Result<F722InitResources, BoardError> {
|
||||
config.validate().map_err(BoardError::InvalidConfig)?;
|
||||
board::validate_vertical_route(config, routes)?;
|
||||
board::initialize(core, device, config, routes)
|
||||
}
|
||||
|
||||
#[cfg(target_arch = "arm")]
|
||||
unsafe extern "Rust" {
|
||||
static KAPTAN_F722_BOARD_CONFIG: BoardConfig;
|
||||
fn kaptan_f722_board_routes() -> Result<F722BoardRoutes, BoardError>;
|
||||
}
|
||||
|
||||
#[cfg(target_arch = "arm")]
|
||||
fn registered_config() -> BoardConfig {
|
||||
// SAFETY: `flight_firmware!` emits this immutable symbol exactly once in
|
||||
// the selected board binary. The platform reads it only during RTIC init,
|
||||
// before any interrupt or worker can observe configuration state.
|
||||
unsafe { KAPTAN_F722_BOARD_CONFIG }
|
||||
}
|
||||
|
||||
#[cfg(target_arch = "arm")]
|
||||
fn registered_routes() -> Result<F722BoardRoutes, BoardError> {
|
||||
// SAFETY: `flight_firmware!` emits this function exactly once in the board
|
||||
// binary. It only constructs a safe descriptor; it cannot touch PAC state
|
||||
// because the board builder contains no peripherals.
|
||||
unsafe { kaptan_f722_board_routes() }
|
||||
}
|
||||
|
||||
// This is one source-level task graph. Its compile-time bindings are the
|
||||
// platform-owned finite interrupt superset, independent of the board crate.
|
||||
#[cfg(target_arch = "arm")]
|
||||
firmware::f722_flight_application!(
|
||||
crate::f722::registered_routes,
|
||||
crate::f722::registered_config
|
||||
);
|
||||
+25
-14
@@ -16,7 +16,7 @@ pub enum ClockProfileError {
|
||||
SystemClockNotPll,
|
||||
}
|
||||
|
||||
/// Selects the board's HSI PLL profile.
|
||||
/// Selects the F722 platform's HSI PLL profile.
|
||||
///
|
||||
/// SYSCLK/HCLK run at 216 MHz; APB1 is 54 MHz (timers run at 108 MHz), and
|
||||
/// APB2 is 108 MHz. HSI is the PLL source, so no external-crystal assumption
|
||||
@@ -43,8 +43,10 @@ pub fn select_pll216_profile(
|
||||
|
||||
// Scale 1, over-drive and seven flash wait states are required at 216 MHz.
|
||||
rcc.apb1enr().modify(|_, write| write.pwren().set_bit());
|
||||
pwr.cr1().modify(|read, write| unsafe {
|
||||
write.bits((read.bits() & !(0b11 << 14)) | (0b11 << 14) | (1 << 16))
|
||||
pwr.cr1().modify(|read, write| {
|
||||
// SAFETY: this read-modify-write changes only the documented voltage-scale and
|
||||
// over-drive-enable fields; it preserves every other PWR_CR1 bit owned by boot.
|
||||
unsafe { write.bits((read.bits() & !(0b11 << 14)) | (0b11 << 14) | (1 << 16)) }
|
||||
});
|
||||
if !wait_until(|| pwr.csr1().read().odrdy().bit_is_set()) {
|
||||
return Err(ClockProfileError::OverDriveNotReady);
|
||||
@@ -53,25 +55,34 @@ pub fn select_pll216_profile(
|
||||
if !wait_until(|| pwr.csr1().read().odswrdy().bit_is_set()) {
|
||||
return Err(ClockProfileError::OverDriveSwitchNotReady);
|
||||
}
|
||||
flash.acr().modify(|read, write| unsafe {
|
||||
write.bits((read.bits() & !0x0f) | 7 | (1 << 8) | (1 << 9))
|
||||
flash.acr().modify(|read, write| {
|
||||
// SAFETY: these are the STM32F7-documented latency, instruction-cache and
|
||||
// data-cache fields for 216 MHz; the read-modify-write retains other ACR bits.
|
||||
unsafe { write.bits((read.bits() & !0x0f) | 7 | (1 << 8) | (1 << 9)) }
|
||||
});
|
||||
|
||||
// HSI / 16 * 432 / 2 = 216 MHz; Q=9 supplies a 48 MHz domain.
|
||||
rcc.pllcfgr()
|
||||
.write(|write| unsafe { write.bits(16 | (432 << 6) | (9 << 24)) });
|
||||
rcc.pllcfgr().write(|write| {
|
||||
// SAFETY: 16, 432, P=/2 (the reset encoding), and Q=9 are the valid PLL
|
||||
// divisors described above; the PLL is disabled before this full-register write.
|
||||
unsafe { write.bits(16 | (432 << 6) | (9 << 24)) }
|
||||
});
|
||||
rcc.cr().modify(|_, write| write.pllon().on());
|
||||
if !wait_until(|| rcc.cr().read().pllrdy().is_ready()) {
|
||||
return Err(ClockProfileError::PllNotReady);
|
||||
}
|
||||
rcc.cfgr().modify(|read, write| unsafe {
|
||||
rcc.cfgr().modify(|read, write| {
|
||||
// AHB /1, APB1 /4, APB2 /2, SYSCLK = PLL.
|
||||
write.bits(
|
||||
(read.bits() & !((0x0f << 4) | (0x07 << 10) | (0x07 << 13) | 0x03))
|
||||
| (0b101 << 10)
|
||||
| (0b100 << 13)
|
||||
| 0b10,
|
||||
)
|
||||
// SAFETY: this preserves every CFGR field except the documented prescaler and
|
||||
// system-clock-source fields, which are written with encodings valid for F722.
|
||||
unsafe {
|
||||
write.bits(
|
||||
(read.bits() & !((0x0f << 4) | (0x07 << 10) | (0x07 << 13) | 0x03))
|
||||
| (0b101 << 10)
|
||||
| (0b100 << 13)
|
||||
| 0b10,
|
||||
)
|
||||
}
|
||||
});
|
||||
if !wait_until(|| rcc.cfgr().read().sws().is_pll()) {
|
||||
return Err(ClockProfileError::SystemClockNotPll);
|
||||
+5
-5
@@ -2,17 +2,17 @@
|
||||
|
||||
use kaptan_dma::{DmaReadableBuffer, DmaTxBufferFactory};
|
||||
///
|
||||
/// DMA transmit memory policy for a board with D-cache disabled.
|
||||
/// DMA transmit memory policy for a platform with D-cache disabled.
|
||||
///
|
||||
/// The board's linker script must also place transfer frames in SRAM reachable
|
||||
/// The platform linker policy must also place transfer frames in SRAM reachable
|
||||
/// by the selected DMA controller. This policy does not support a cached board;
|
||||
/// such a board requires a distinct policy that performs cache maintenance.
|
||||
/// such a platform requires a distinct policy that performs cache maintenance.
|
||||
pub struct DcacheDisabledDmaMemory {
|
||||
_private: (),
|
||||
}
|
||||
|
||||
impl DcacheDisabledDmaMemory {
|
||||
/// Declares that the selected board disabled D-cache before DMA setup.
|
||||
/// Declares that the selected platform disabled D-cache before DMA setup.
|
||||
///
|
||||
/// # Safety
|
||||
///
|
||||
@@ -27,7 +27,7 @@ impl DmaTxBufferFactory for DcacheDisabledDmaMemory {
|
||||
type Error = core::convert::Infallible;
|
||||
|
||||
fn prepare<'a>(&mut self, bytes: &'a [u8]) -> Result<DmaReadableBuffer<'a>, Self::Error> {
|
||||
// SAFETY: `Self` can exist only after the board proved its D-cache and
|
||||
// SAFETY: `Self` can exist only after the platform proved its D-cache and
|
||||
// linker-memory policy. The runtime retains frame ownership until a
|
||||
// terminal DMA outcome or successful recovery.
|
||||
Ok(unsafe { DmaReadableBuffer::new_unchecked(bytes) })
|
||||
+90
-1
@@ -48,6 +48,59 @@ pub fn configure_spi1_pins_pa5_pa6_pa7(rcc: &stm32f722::RCC, port: &stm32f722::G
|
||||
});
|
||||
}
|
||||
|
||||
/// Configures PC10, PC11, and PC12 for SPI3 alternate function 6.
|
||||
pub fn configure_spi3_pins_pc10_pc11_pc12(rcc: &stm32f722::RCC, port: &stm32f722::GPIOC) {
|
||||
rcc.ahb1enr().modify(|_, write| write.gpiocen().set_bit());
|
||||
port.moder().modify(|_, write| {
|
||||
write
|
||||
.moder10()
|
||||
.alternate()
|
||||
.moder11()
|
||||
.alternate()
|
||||
.moder12()
|
||||
.alternate()
|
||||
});
|
||||
port.afrh().modify(|_, write| {
|
||||
// SAFETY: AF6 selects SPI3 on PC10, PC11, and PC12 on STM32F722.
|
||||
unsafe {
|
||||
write
|
||||
.afrh10()
|
||||
.bits(0b0110)
|
||||
.afrh11()
|
||||
.bits(0b0110)
|
||||
.afrh12()
|
||||
.bits(0b0110)
|
||||
}
|
||||
});
|
||||
port.otyper().modify(|_, write| {
|
||||
write
|
||||
.ot10()
|
||||
.push_pull()
|
||||
.ot11()
|
||||
.push_pull()
|
||||
.ot12()
|
||||
.push_pull()
|
||||
});
|
||||
port.ospeedr().modify(|_, write| {
|
||||
write
|
||||
.ospeedr10()
|
||||
.high_speed()
|
||||
.ospeedr11()
|
||||
.high_speed()
|
||||
.ospeedr12()
|
||||
.high_speed()
|
||||
});
|
||||
port.pupdr().modify(|_, write| {
|
||||
write
|
||||
.pupdr10()
|
||||
.floating()
|
||||
.pupdr11()
|
||||
.floating()
|
||||
.pupdr12()
|
||||
.floating()
|
||||
});
|
||||
}
|
||||
|
||||
/// Configures PB8/PB9 for I²C1 alternate function 4.
|
||||
pub fn configure_i2c1_pins_pb8_pb9(rcc: &stm32f722::RCC, port: &stm32f722::GPIOB) {
|
||||
rcc.ahb1enr().modify(|_, w| w.gpioben().set_bit());
|
||||
@@ -66,13 +119,19 @@ pub fn configure_i2c1_pins_pb8_pb9(rcc: &stm32f722::RCC, port: &stm32f722::GPIOB
|
||||
|
||||
/// Configures PA4 as the input pin for an EXTI4 route.
|
||||
///
|
||||
/// A board must select this explicitly rather than inheriting the pin mode
|
||||
/// A platform route must select this explicitly rather than inheriting the pin mode
|
||||
/// left by a bootloader or an earlier firmware image.
|
||||
pub fn configure_exti4_pin_pa4(port: &stm32f722::GPIOA) {
|
||||
port.moder().modify(|_, write| write.moder4().input());
|
||||
port.pupdr().modify(|_, write| write.pupdr4().floating());
|
||||
}
|
||||
|
||||
/// Configures PC3 as the input pin for an EXTI3 route.
|
||||
pub fn configure_exti3_pin_pc3(port: &stm32f722::GPIOC) {
|
||||
port.moder().modify(|_, write| write.moder3().input());
|
||||
port.pupdr().modify(|_, write| write.pupdr3().floating());
|
||||
}
|
||||
|
||||
/// Configures PA9 for USART1 transmit alternate function 7.
|
||||
pub fn configure_usart1_tx_pin_pa9(port: &stm32f722::GPIOA) {
|
||||
port.moder().modify(|_, write| write.moder9().alternate());
|
||||
@@ -96,6 +155,36 @@ pub fn configure_usart1_rx_pin_pa10(port: &stm32f722::GPIOA) {
|
||||
port.pupdr().modify(|_, write| write.pupdr10().floating());
|
||||
}
|
||||
|
||||
/// Configures PA2/PA3 for USART2 transmit/receive alternate function 7.
|
||||
pub fn configure_usart2_pins_pa2_pa3(port: &stm32f722::GPIOA) {
|
||||
port.moder()
|
||||
.modify(|_, write| write.moder2().alternate().moder3().alternate());
|
||||
port.afrl().modify(|_, write| {
|
||||
// SAFETY: AF7 selects USART2 TX/RX on PA2/PA3 on STM32F722.
|
||||
unsafe { write.afrl2().bits(0b0111).afrl3().bits(0b0111) }
|
||||
});
|
||||
port.otyper().modify(|_, write| write.ot2().push_pull());
|
||||
port.ospeedr()
|
||||
.modify(|_, write| write.ospeedr2().high_speed());
|
||||
port.pupdr()
|
||||
.modify(|_, write| write.pupdr2().floating().pupdr3().floating());
|
||||
}
|
||||
|
||||
/// Configures PA0/PA1 for UART4 alternate function 8.
|
||||
pub fn configure_uart4_pins_pa0_pa1(port: &stm32f722::GPIOA) {
|
||||
port.moder()
|
||||
.modify(|_, write| write.moder0().alternate().moder1().alternate());
|
||||
port.afrl().modify(|_, write| {
|
||||
// SAFETY: AF8 selects UART4 TX/RX on PA0/PA1 on STM32F722.
|
||||
unsafe { write.afrl0().bits(0b1000).afrl1().bits(0b1000) }
|
||||
});
|
||||
port.otyper().modify(|_, write| write.ot0().push_pull());
|
||||
port.ospeedr()
|
||||
.modify(|_, write| write.ospeedr0().high_speed());
|
||||
port.pupdr()
|
||||
.modify(|_, write| write.pupdr0().floating().pupdr1().floating());
|
||||
}
|
||||
|
||||
/// Active-low status LED connected to PD2.
|
||||
pub struct ActiveLowLedPd2 {
|
||||
port: stm32f722::GPIOD,
|
||||
+3
-3
@@ -27,7 +27,7 @@ pub enum I2c1Error {
|
||||
Busy,
|
||||
}
|
||||
|
||||
/// Board-selected I²C1 electrical timing.
|
||||
/// Platform-selected I²C1 electrical timing.
|
||||
#[derive(Clone, Copy)]
|
||||
pub struct I2c1Config {
|
||||
timing: u32,
|
||||
@@ -102,14 +102,14 @@ pub struct I2c1 {
|
||||
}
|
||||
|
||||
impl I2c1 {
|
||||
/// Configures I²C1 using board-selected pin electrical timing.
|
||||
/// Configures I²C1 using platform-selected pin electrical timing.
|
||||
pub fn new(rcc: &stm32f722::RCC, peripheral: stm32f722::I2C1, config: I2c1Config) -> Self {
|
||||
rcc.apb1rstr().modify(|_, w| w.i2c1rst().set_bit());
|
||||
rcc.apb1rstr().modify(|_, w| w.i2c1rst().clear_bit());
|
||||
rcc.apb1enr().modify(|_, w| w.i2c1en().set_bit());
|
||||
// SAFETY: zero is the documented reset configuration for CR1.
|
||||
peripheral.cr1().write(|w| unsafe { w.bits(0) });
|
||||
// SAFETY: `timing` is a board-selected I2C_TIMINGR bit pattern.
|
||||
// SAFETY: `timing` is a platform-selected I2C_TIMINGR bit pattern.
|
||||
peripheral
|
||||
.timingr()
|
||||
.write(|w| unsafe { w.bits(config.timing) });
|
||||
@@ -0,0 +1,33 @@
|
||||
//! STM32F722 peripheral implementations selected by supported boards.
|
||||
//!
|
||||
//! This module exports only typed peripheral building blocks. The family
|
||||
//! platform selects pins, clocks, interrupt priorities, and static ownership.
|
||||
|
||||
mod clock;
|
||||
mod dma_buffer;
|
||||
mod gpio;
|
||||
mod i2c1;
|
||||
mod spi1;
|
||||
mod spi3;
|
||||
mod tim2;
|
||||
mod time;
|
||||
mod usart;
|
||||
mod usart1_tx_dma2_stream7;
|
||||
mod usart2_tx_dma1_stream6;
|
||||
|
||||
pub use clock::select_pll216_profile;
|
||||
pub use dma_buffer::DcacheDisabledDmaMemory;
|
||||
pub use gpio::{
|
||||
ActiveLowLedPd2, configure_exti3_pin_pc3, configure_exti4_pin_pa4, configure_i2c1_pins_pb8_pb9,
|
||||
configure_spi1_pins_pa5_pa6_pa7, configure_spi3_pins_pc10_pc11_pc12,
|
||||
configure_uart4_pins_pa0_pa1, configure_usart1_rx_pin_pa10, configure_usart1_tx_pin_pa9,
|
||||
configure_usart2_pins_pa2_pa3,
|
||||
};
|
||||
pub use i2c1::{I2c1, I2c1Config};
|
||||
pub use spi1::{Spi1, Spi1Error};
|
||||
pub use spi3::{Spi3, Spi3Error};
|
||||
pub use tim2::{Tim2Periodic, Tim2PeriodicConfig};
|
||||
pub use time::DwtMicrosecondClock;
|
||||
pub use usart::{ReceiveInterrupt, Uart4, Usart1Tx, Usart2Tx};
|
||||
pub use usart1_tx_dma2_stream7::Usart1TxDma;
|
||||
pub use usart2_tx_dma1_stream6::Usart2TxDma;
|
||||
File renamed without changes.
@@ -0,0 +1,335 @@
|
||||
//! STM32F722 SPI3 full-duplex DMA controller.
|
||||
//!
|
||||
//! SPI3 uses DMA1 Stream0/Channel0 for receive and DMA1 Stream5/Channel0 for
|
||||
//! transmit. The receive terminal event is the transaction boundary because it
|
||||
//! is generated after the last serial frame; transmit completion can occur
|
||||
//! while the final serial clock is still in progress.
|
||||
|
||||
use kaptan_spi::SpiCompletion;
|
||||
use stm32f7::stm32f722;
|
||||
|
||||
const MAX_TRANSFER_LENGTH: usize = u16::MAX as usize;
|
||||
|
||||
/// Failure while submitting one SPI3 DMA transaction.
|
||||
#[derive(Clone, Copy, Debug, Eq, PartialEq)]
|
||||
pub enum Spi3Error {
|
||||
/// The transaction is malformed or exceeds the DMA counter range.
|
||||
InvalidLength,
|
||||
/// A previous DMA transaction still owns SPI3.
|
||||
Busy,
|
||||
}
|
||||
|
||||
/// Owns SPI3 and its fixed full-duplex DMA1 request route.
|
||||
pub struct Spi3 {
|
||||
peripheral: stm32f722::SPI3,
|
||||
controller: stm32f722::DMA1,
|
||||
active: bool,
|
||||
}
|
||||
|
||||
impl Spi3 {
|
||||
/// Configures SPI3 mode 0 master operation at APB2/128 and reserves DMA1.
|
||||
///
|
||||
/// This keeps the ICM-42688-P route below 1 MHz with the board's 108 MHz
|
||||
/// APB2 clock while retaining substantial margin for board-level wiring.
|
||||
pub fn new(
|
||||
rcc: &stm32f722::RCC,
|
||||
peripheral: stm32f722::SPI3,
|
||||
controller: stm32f722::DMA1,
|
||||
) -> Self {
|
||||
rcc.apb1rstr().modify(|_, write| write.spi3rst().set_bit());
|
||||
rcc.apb1rstr()
|
||||
.modify(|_, write| write.spi3rst().clear_bit());
|
||||
rcc.apb1enr().modify(|_, write| write.spi3en().set_bit());
|
||||
rcc.ahb1enr().modify(|_, write| write.dma1en().set_bit());
|
||||
|
||||
peripheral.cr1().write(|write| {
|
||||
write
|
||||
.mstr()
|
||||
.master()
|
||||
.br()
|
||||
.div128()
|
||||
.ssm()
|
||||
.set_bit()
|
||||
.ssi()
|
||||
.set_bit()
|
||||
});
|
||||
peripheral
|
||||
.cr2()
|
||||
.write(|write| write.ds().eight_bit().frxth().set_bit().errie().set_bit());
|
||||
|
||||
Self {
|
||||
peripheral,
|
||||
controller,
|
||||
active: false,
|
||||
}
|
||||
}
|
||||
|
||||
/// Starts a full-duplex DMA transaction without polling a peripheral flag.
|
||||
pub fn start_transfer(&mut self, transmit: &[u8], receive: &mut [u8]) -> Result<(), Spi3Error> {
|
||||
if transmit.is_empty()
|
||||
|| transmit.len() != receive.len()
|
||||
|| transmit.len() > MAX_TRANSFER_LENGTH
|
||||
{
|
||||
return Err(Spi3Error::InvalidLength);
|
||||
}
|
||||
if self.active
|
||||
|| self.rx_stream().cr().read().en().is_enabled()
|
||||
|| self.tx_stream().cr().read().en().is_enabled()
|
||||
{
|
||||
return Err(Spi3Error::Busy);
|
||||
}
|
||||
|
||||
// Arm the complete DMA receive path before SPI starts generating
|
||||
// clocks. This avoids an RX FIFO overrun between transactions.
|
||||
self.peripheral
|
||||
.cr1()
|
||||
.modify(|_, write| write.spe().clear_bit());
|
||||
self.clear_spi_overrun();
|
||||
self.clear_rx_flags();
|
||||
self.clear_tx_flags();
|
||||
self.configure_rx(receive);
|
||||
self.configure_tx(transmit);
|
||||
self.peripheral.cr2().modify(|_, write| {
|
||||
let write = write.rxdmaen().set_bit().txdmaen().set_bit();
|
||||
if transmit.len() & 1 == 0 {
|
||||
write.ldma_rx().even().ldma_tx().even()
|
||||
} else {
|
||||
// With 8-bit frames, F7 SPI needs this for the final
|
||||
// byte of an odd-length DMA transaction. IMU samples are
|
||||
// one command byte plus fourteen data bytes.
|
||||
write.ldma_rx().odd().ldma_tx().odd()
|
||||
}
|
||||
});
|
||||
self.rx_stream()
|
||||
.cr()
|
||||
.modify(|_, write| write.en().enabled());
|
||||
self.tx_stream()
|
||||
.cr()
|
||||
.modify(|_, write| write.en().enabled());
|
||||
self.peripheral
|
||||
.cr1()
|
||||
.modify(|_, write| write.spe().set_bit());
|
||||
self.active = true;
|
||||
Ok(())
|
||||
}
|
||||
|
||||
/// Releases controller-side ownership after a DMA terminal event.
|
||||
pub fn complete_transfer(&mut self, _: SpiCompletion) {
|
||||
// TODO: Abort both DMA streams on peripheral faults.
|
||||
self.peripheral
|
||||
.cr2()
|
||||
.modify(|_, write| write.rxdmaen().clear_bit().txdmaen().clear_bit());
|
||||
self.peripheral
|
||||
.cr1()
|
||||
.modify(|_, write| write.spe().clear_bit());
|
||||
self.active = false;
|
||||
}
|
||||
|
||||
/// Acknowledges an SPI peripheral overrun interrupt.
|
||||
///
|
||||
/// An overrun is not a DMA-stream error, so it must be observed through
|
||||
/// SPI3's own IRQ. The reference manual clears OVR by reading DR then SR.
|
||||
pub fn acknowledge_error_interrupt() -> bool {
|
||||
// SAFETY: SPI3 is initialized once by `Spi3::new`; this ISR only
|
||||
// clears its peripheral OVR condition.
|
||||
let peripheral = unsafe { &*stm32f722::SPI3::ptr() };
|
||||
if !peripheral.sr().read().ovr().is_overrun() {
|
||||
return false;
|
||||
}
|
||||
let _ = peripheral.dr().read().bits();
|
||||
let _ = peripheral.sr().read().bits();
|
||||
true
|
||||
}
|
||||
|
||||
/// Acknowledges the RX terminal IRQ. Invoke only from DMA1 Stream0.
|
||||
pub fn acknowledge_rx_interrupt() -> SpiCompletion {
|
||||
// SAFETY: DMA1 is initialized once by `Spi3::new`; this ISR touches
|
||||
// only Stream0's status and clear registers, never worker state.
|
||||
let controller = unsafe { &*stm32f722::DMA1::ptr() };
|
||||
let status = controller.lisr().read();
|
||||
let fault =
|
||||
status.teif0().is_error() || status.dmeif0().is_error() || status.feif0().is_error();
|
||||
let complete = status.tcif0().is_complete();
|
||||
controller.lifcr().write(|write| {
|
||||
write
|
||||
.cfeif0()
|
||||
.clear()
|
||||
.cdmeif0()
|
||||
.clear()
|
||||
.cteif0()
|
||||
.clear()
|
||||
.chtif0()
|
||||
.clear()
|
||||
.ctcif0()
|
||||
.clear()
|
||||
});
|
||||
if fault {
|
||||
SpiCompletion::TransferFault
|
||||
} else if complete {
|
||||
SpiCompletion::Complete
|
||||
} else {
|
||||
SpiCompletion::NotPending
|
||||
}
|
||||
}
|
||||
|
||||
/// Acknowledges TX errors; TX completion alone is not a transaction event.
|
||||
pub fn acknowledge_tx_interrupt() -> SpiCompletion {
|
||||
// SAFETY: DMA1 Stream5 is initialized once by `Spi3::new`; this ISR
|
||||
// touches only Stream5's status and clear registers.
|
||||
let controller = unsafe { &*stm32f722::DMA1::ptr() };
|
||||
let status = controller.hisr().read();
|
||||
let fault =
|
||||
status.teif5().is_error() || status.dmeif5().is_error() || status.feif5().is_error();
|
||||
controller.hifcr().write(|write| {
|
||||
write
|
||||
.cfeif5()
|
||||
.clear()
|
||||
.cdmeif5()
|
||||
.clear()
|
||||
.cteif5()
|
||||
.clear()
|
||||
.chtif5()
|
||||
.clear()
|
||||
.ctcif5()
|
||||
.clear()
|
||||
});
|
||||
if fault {
|
||||
SpiCompletion::TransferFault
|
||||
} else {
|
||||
SpiCompletion::NotPending
|
||||
}
|
||||
}
|
||||
|
||||
fn configure_rx(&self, receive: &mut [u8]) {
|
||||
self.rx_stream().cr().write(|write| {
|
||||
// SAFETY: Channel 0 is SPI3_RX on DMA1 Stream0; checked slice
|
||||
// length fits NDTR, and all values map directly to documented bits.
|
||||
unsafe {
|
||||
write
|
||||
.chsel()
|
||||
.bits(0)
|
||||
.dir()
|
||||
.peripheral_to_memory()
|
||||
.pl()
|
||||
.very_high()
|
||||
.minc()
|
||||
.incremented()
|
||||
.pinc()
|
||||
.fixed()
|
||||
.msize()
|
||||
.bits8()
|
||||
.psize()
|
||||
.bits8()
|
||||
.circ()
|
||||
.disabled()
|
||||
.tcie()
|
||||
.enabled()
|
||||
.teie()
|
||||
.enabled()
|
||||
.dmeie()
|
||||
.enabled()
|
||||
}
|
||||
});
|
||||
self.rx_stream()
|
||||
.ndtr()
|
||||
// SAFETY: `start_transfer` bounds the length to the 16-bit NDTR field.
|
||||
.write(|write| unsafe { write.bits(receive.len() as u32) });
|
||||
self.rx_stream()
|
||||
.par()
|
||||
// SAFETY: `DR` is the memory-mapped SPI3 DMA peripheral endpoint.
|
||||
.write(|write| unsafe { write.bits(self.peripheral.dr().as_ptr() as u32) });
|
||||
self.rx_stream()
|
||||
.m0ar()
|
||||
// SAFETY: the caller retains `receive` until terminal completion.
|
||||
.write(|write| unsafe { write.bits(receive.as_mut_ptr() as u32) });
|
||||
}
|
||||
|
||||
fn configure_tx(&self, transmit: &[u8]) {
|
||||
self.tx_stream().cr().write(|write| {
|
||||
// SAFETY: Channel 0 is SPI3_TX on DMA1 Stream5; checked slice
|
||||
// length fits NDTR, and all values map directly to documented bits.
|
||||
unsafe {
|
||||
write
|
||||
.chsel()
|
||||
.bits(0)
|
||||
.dir()
|
||||
.memory_to_peripheral()
|
||||
.pl()
|
||||
.very_high()
|
||||
.minc()
|
||||
.incremented()
|
||||
.pinc()
|
||||
.fixed()
|
||||
.msize()
|
||||
.bits8()
|
||||
.psize()
|
||||
.bits8()
|
||||
.circ()
|
||||
.disabled()
|
||||
.teie()
|
||||
.enabled()
|
||||
.dmeie()
|
||||
.enabled()
|
||||
}
|
||||
});
|
||||
self.tx_stream()
|
||||
.ndtr()
|
||||
// SAFETY: `start_transfer` bounds the length to the 16-bit NDTR field.
|
||||
.write(|write| unsafe { write.bits(transmit.len() as u32) });
|
||||
self.tx_stream()
|
||||
.par()
|
||||
// SAFETY: `DR` is the memory-mapped SPI3 DMA peripheral endpoint.
|
||||
.write(|write| unsafe { write.bits(self.peripheral.dr().as_ptr() as u32) });
|
||||
self.tx_stream()
|
||||
.m0ar()
|
||||
// SAFETY: the caller retains `transmit` until terminal completion.
|
||||
.write(|write| unsafe { write.bits(transmit.as_ptr() as u32) });
|
||||
}
|
||||
|
||||
fn rx_stream(&self) -> &stm32f722::dma1::ST {
|
||||
self.controller.st(0)
|
||||
}
|
||||
|
||||
fn tx_stream(&self) -> &stm32f722::dma1::ST {
|
||||
self.controller.st(5)
|
||||
}
|
||||
|
||||
fn clear_rx_flags(&self) {
|
||||
self.controller.lifcr().write(|write| {
|
||||
write
|
||||
.cfeif0()
|
||||
.clear()
|
||||
.cdmeif0()
|
||||
.clear()
|
||||
.cteif0()
|
||||
.clear()
|
||||
.chtif0()
|
||||
.clear()
|
||||
.ctcif0()
|
||||
.clear()
|
||||
});
|
||||
}
|
||||
|
||||
fn clear_tx_flags(&self) {
|
||||
self.controller.hifcr().write(|write| {
|
||||
write
|
||||
.cfeif5()
|
||||
.clear()
|
||||
.cdmeif5()
|
||||
.clear()
|
||||
.cteif5()
|
||||
.clear()
|
||||
.chtif5()
|
||||
.clear()
|
||||
.ctcif5()
|
||||
.clear()
|
||||
});
|
||||
}
|
||||
|
||||
fn clear_spi_overrun(&self) {
|
||||
if self.peripheral.sr().read().ovr().is_overrun() {
|
||||
let _ = self.peripheral.dr().read().bits();
|
||||
let _ = self.peripheral.sr().read().bits();
|
||||
}
|
||||
}
|
||||
}
|
||||
+11
-7
@@ -13,11 +13,11 @@ pub struct Tim2PeriodicConfig {
|
||||
|
||||
/// TIM2 configured to issue periodic update interrupts.
|
||||
pub struct Tim2Periodic {
|
||||
peripheral: stm32f722::TIM2,
|
||||
_peripheral: stm32f722::TIM2,
|
||||
}
|
||||
|
||||
impl Tim2Periodic {
|
||||
/// Enables TIM2 with the supplied board-derived period configuration.
|
||||
/// Enables TIM2 with the supplied platform-derived period configuration.
|
||||
pub fn new(
|
||||
rcc: &stm32f722::RCC,
|
||||
peripheral: stm32f722::TIM2,
|
||||
@@ -37,17 +37,21 @@ impl Tim2Periodic {
|
||||
unsafe { write.arr().bits(config.auto_reload) }
|
||||
});
|
||||
peripheral.egr().write(|write| write.ug().set_bit());
|
||||
peripheral.sr().modify(|_, write| write.uif().clear_bit());
|
||||
// TIM2 status flags are write-zero-to-clear. Use the PAC's explicit
|
||||
// W0C operation rather than a read-modify-write after `EGR.UG`.
|
||||
peripheral.sr().write(|write| write.uif().clear());
|
||||
peripheral.dier().modify(|_, write| write.uie().set_bit());
|
||||
peripheral.cr1().modify(|_, write| write.cen().set_bit());
|
||||
|
||||
Self { peripheral }
|
||||
Self {
|
||||
_peripheral: peripheral,
|
||||
}
|
||||
}
|
||||
|
||||
/// Acknowledges a TIM2 update interrupt.
|
||||
#[cfg(target_arch = "arm")]
|
||||
pub fn acknowledge_update(&mut self) {
|
||||
self.peripheral
|
||||
.sr()
|
||||
.modify(|_, write| write.uif().clear_bit());
|
||||
// `UIF` is write-zero-to-clear (W0C).
|
||||
self._peripheral.sr().write(|write| write.uif().clear());
|
||||
}
|
||||
}
|
||||
+3
-3
@@ -7,7 +7,7 @@ use kaptan_time::{MonotonicClock, Timestamp};
|
||||
///
|
||||
/// `now` must be called at least once during every 32-bit cycle-counter wrap
|
||||
/// interval. At the Kakute reset clock this is approximately 268 seconds; the
|
||||
/// board periodic service calls it much more frequently.
|
||||
/// platform periodic service calls it much more frequently.
|
||||
pub struct DwtMicrosecondClock {
|
||||
cycles_per_microsecond: u32,
|
||||
previous_cycles: u32,
|
||||
@@ -24,8 +24,8 @@ pub enum DwtClockStartError {
|
||||
impl DwtMicrosecondClock {
|
||||
/// Enables the DWT cycle counter for a fixed integer-MHz core clock.
|
||||
///
|
||||
/// `MEGAHERTZ` must match the board's core clock. This function is called
|
||||
/// once during board initialization before interrupts are enabled.
|
||||
/// `MEGAHERTZ` must match the platform's core clock. This function is
|
||||
/// called once during platform initialization before interrupts are enabled.
|
||||
pub fn start<const MEGAHERTZ: u32>(
|
||||
dcb: &mut DCB,
|
||||
dwt: &mut DWT,
|
||||
@@ -0,0 +1,285 @@
|
||||
//! STM32F722 USART routes used by supported boards.
|
||||
|
||||
use stm32f7::stm32f722;
|
||||
|
||||
/// UART baud-rate configuration error.
|
||||
#[derive(Clone, Copy, Debug, Eq, PartialEq)]
|
||||
pub enum BaudRateError {
|
||||
/// Zero is not a valid baud rate.
|
||||
Zero,
|
||||
/// The requested divisor cannot fit USART BRR.
|
||||
OutOfRange,
|
||||
}
|
||||
|
||||
/// Terminal result from acknowledging a USART receive interrupt.
|
||||
#[derive(Clone, Copy, Debug, Eq, PartialEq)]
|
||||
pub enum ReceiveInterrupt {
|
||||
/// One received byte was removed from the UART data register.
|
||||
Byte(u8),
|
||||
/// A serial transport fault invalidated the current receive stream.
|
||||
Fault,
|
||||
/// This USART interrupt did not have a receive event pending.
|
||||
NotPending,
|
||||
}
|
||||
|
||||
/// Owns configured full-duplex USART1 hardware.
|
||||
///
|
||||
/// The family platform owns schematic-selected pin setup. Configure pins
|
||||
/// before constructing this type.
|
||||
pub struct Usart1Tx {
|
||||
peripheral: stm32f722::USART1,
|
||||
}
|
||||
|
||||
/// Owns configured full-duplex USART2 hardware.
|
||||
///
|
||||
/// The family platform owns schematic-selected pin setup. Configure pins
|
||||
/// before constructing this type.
|
||||
pub struct Usart2Tx {
|
||||
peripheral: stm32f722::USART2,
|
||||
}
|
||||
|
||||
impl Usart2Tx {
|
||||
/// Enables USART2 and configures 8N1 transmit and receive operation.
|
||||
pub fn new(
|
||||
rcc: &stm32f722::RCC,
|
||||
peripheral: stm32f722::USART2,
|
||||
peripheral_clock_hz: u32,
|
||||
baud: u32,
|
||||
) -> Result<Self, BaudRateError> {
|
||||
let brr = calculate_brr(peripheral_clock_hz, baud)?;
|
||||
|
||||
// Do not inherit a bootloader's USART2 kernel-clock selection. The
|
||||
// F722 flight clock profile supplies `peripheral_clock_hz` from PCLK1,
|
||||
// so select PCLK1 before programming BRR.
|
||||
rcc.dckcfgr2().modify(|_, write| write.usart2sel().apb1());
|
||||
rcc.apb1rstr()
|
||||
.modify(|_, write| write.usart2rst().set_bit());
|
||||
rcc.apb1rstr()
|
||||
.modify(|_, write| write.usart2rst().clear_bit());
|
||||
rcc.apb1enr().modify(|_, write| write.usart2en().set_bit());
|
||||
peripheral.brr().write(|write| {
|
||||
// SAFETY: `brr` is validated to fit the complete BRR register.
|
||||
unsafe { write.bits(u32::from(brr)) }
|
||||
});
|
||||
peripheral.cr1().write(|write| {
|
||||
write
|
||||
.te()
|
||||
.set_bit()
|
||||
.re()
|
||||
.set_bit()
|
||||
.rxneie()
|
||||
.set_bit()
|
||||
.ue()
|
||||
.set_bit()
|
||||
});
|
||||
peripheral.cr2().write(|write| write.stop().stop1());
|
||||
|
||||
Ok(Self { peripheral })
|
||||
}
|
||||
|
||||
pub(crate) fn enable_dma_transmit(&self) {
|
||||
self.peripheral.cr3().write(|write| write.dmat().set_bit());
|
||||
}
|
||||
|
||||
pub(crate) fn transmit_data_register_address(&self) -> u32 {
|
||||
self.peripheral.tdr().as_ptr() as u32
|
||||
}
|
||||
|
||||
/// Blocks until USART2 accepts one byte for transmission.
|
||||
///
|
||||
/// This is used by the platform's UART wire diagnostic. Flight data-channel traffic
|
||||
/// uses [`Usart2TxDma`](super::Usart2TxDma) instead.
|
||||
#[cfg(target_arch = "arm")]
|
||||
pub fn write_blocking(&self, byte: u8) {
|
||||
while !self.peripheral.isr().read().txe().is_not_full() {}
|
||||
self.peripheral.tdr().write(|write| {
|
||||
// SAFETY: `byte` is eight bits and therefore fits the USART's
|
||||
// documented nine-bit TDR data field.
|
||||
unsafe { write.tdr().bits(u16::from(byte)) }
|
||||
});
|
||||
}
|
||||
|
||||
/// Acknowledges one USART2 receive interrupt without accessing TX state.
|
||||
pub fn acknowledge_receive_interrupt() -> ReceiveInterrupt {
|
||||
// SAFETY: the platform binds USART2 exactly once and this function touches
|
||||
// only its receive/error registers, never the TX DMA configuration.
|
||||
let peripheral = unsafe { &*stm32f722::USART2::ptr() };
|
||||
acknowledge_receive_interrupt(peripheral)
|
||||
}
|
||||
}
|
||||
|
||||
impl Usart1Tx {
|
||||
/// Enables USART1 and configures 8N1 transmit and receive operation.
|
||||
pub fn new(
|
||||
rcc: &stm32f722::RCC,
|
||||
peripheral: stm32f722::USART1,
|
||||
peripheral_clock_hz: u32,
|
||||
baud: u32,
|
||||
) -> Result<Self, BaudRateError> {
|
||||
let brr = calculate_brr(peripheral_clock_hz, baud)?;
|
||||
|
||||
rcc.apb2rstr()
|
||||
.modify(|_, write| write.usart1rst().set_bit());
|
||||
rcc.apb2rstr()
|
||||
.modify(|_, write| write.usart1rst().clear_bit());
|
||||
rcc.apb2enr().modify(|_, write| write.usart1en().set_bit());
|
||||
peripheral.brr().write(|write| {
|
||||
// SAFETY: `brr` is validated to fit the complete BRR register.
|
||||
unsafe { write.bits(u32::from(brr)) }
|
||||
});
|
||||
peripheral.cr1().write(|write| {
|
||||
write
|
||||
.te()
|
||||
.set_bit()
|
||||
.re()
|
||||
.set_bit()
|
||||
.rxneie()
|
||||
.set_bit()
|
||||
.ue()
|
||||
.set_bit()
|
||||
});
|
||||
peripheral.cr2().write(|write| write.stop().stop1());
|
||||
|
||||
Ok(Self { peripheral })
|
||||
}
|
||||
|
||||
pub(crate) fn enable_dma_transmit(&self) {
|
||||
self.peripheral.cr3().write(|write| write.dmat().set_bit());
|
||||
}
|
||||
|
||||
pub(crate) fn transmit_data_register_address(&self) -> u32 {
|
||||
self.peripheral.tdr().as_ptr() as u32
|
||||
}
|
||||
|
||||
/// Acknowledges one USART1 receive interrupt without accessing TX state.
|
||||
///
|
||||
/// The platform invokes this only from the `USART1` interrupt. USART1 TX DMA
|
||||
/// owns its disjoint data register and DMA stream, so this static route may
|
||||
/// safely acknowledge RX while that transfer is active.
|
||||
pub fn acknowledge_receive_interrupt() -> ReceiveInterrupt {
|
||||
// SAFETY: the platform binds USART1 exactly once and this function touches
|
||||
// only its receive/error registers, never the TX DMA configuration.
|
||||
let peripheral = unsafe { &*stm32f722::USART1::ptr() };
|
||||
acknowledge_receive_interrupt(peripheral)
|
||||
}
|
||||
}
|
||||
|
||||
/// Owns configured UART4 hardware.
|
||||
///
|
||||
/// This route is intended for peripherals that continuously stream data, such
|
||||
/// as the MTF-01. It does not enable DMA state.
|
||||
pub struct Uart4 {
|
||||
_peripheral: stm32f722::UART4,
|
||||
}
|
||||
|
||||
impl Uart4 {
|
||||
/// Enables UART4 and configures 8N1 transmit and receive operation.
|
||||
pub fn new(
|
||||
rcc: &stm32f722::RCC,
|
||||
peripheral: stm32f722::UART4,
|
||||
peripheral_clock_hz: u32,
|
||||
baud: u32,
|
||||
) -> Result<Self, BaudRateError> {
|
||||
let brr = calculate_brr(peripheral_clock_hz, baud)?;
|
||||
|
||||
rcc.apb1rstr().modify(|_, write| write.uart4rst().set_bit());
|
||||
rcc.apb1rstr()
|
||||
.modify(|_, write| write.uart4rst().clear_bit());
|
||||
rcc.apb1enr().modify(|_, write| write.uart4en().set_bit());
|
||||
peripheral.brr().write(|write| {
|
||||
// SAFETY: `brr` is validated to fit the complete BRR register.
|
||||
unsafe { write.bits(u32::from(brr)) }
|
||||
});
|
||||
peripheral.cr1().write(|write| {
|
||||
write
|
||||
.te()
|
||||
.set_bit()
|
||||
.re()
|
||||
.set_bit()
|
||||
.rxneie()
|
||||
.set_bit()
|
||||
.ue()
|
||||
.set_bit()
|
||||
});
|
||||
peripheral.cr2().write(|write| write.stop().stop1());
|
||||
|
||||
Ok(Self {
|
||||
_peripheral: peripheral,
|
||||
})
|
||||
}
|
||||
|
||||
/// Acknowledges one UART4 receive interrupt.
|
||||
pub fn acknowledge_receive_interrupt() -> ReceiveInterrupt {
|
||||
// SAFETY: the platform binds UART4 exactly once and this function
|
||||
// accesses only its receive and error registers.
|
||||
let peripheral = unsafe { &*stm32f722::UART4::ptr() };
|
||||
acknowledge_receive_interrupt(peripheral)
|
||||
}
|
||||
}
|
||||
|
||||
fn acknowledge_receive_interrupt(
|
||||
peripheral: &stm32f722::usart1::RegisterBlock,
|
||||
) -> ReceiveInterrupt {
|
||||
let status = peripheral.isr().read();
|
||||
let fault = status.pe().is_error()
|
||||
|| status.fe().is_error()
|
||||
|| status.nf().is_noise()
|
||||
|| status.ore().is_overrun();
|
||||
|
||||
if status.rxne().is_data_ready() {
|
||||
let byte = peripheral.rdr().read().rdr().bits() as u8;
|
||||
if fault {
|
||||
clear_receive_errors(peripheral);
|
||||
ReceiveInterrupt::Fault
|
||||
} else {
|
||||
ReceiveInterrupt::Byte(byte)
|
||||
}
|
||||
} else if fault {
|
||||
clear_receive_errors(peripheral);
|
||||
ReceiveInterrupt::Fault
|
||||
} else {
|
||||
ReceiveInterrupt::NotPending
|
||||
}
|
||||
}
|
||||
|
||||
fn clear_receive_errors(peripheral: &stm32f722::usart1::RegisterBlock) {
|
||||
peripheral.icr().write(|write| {
|
||||
write
|
||||
.pecf()
|
||||
.clear()
|
||||
.fecf()
|
||||
.clear()
|
||||
.ncf()
|
||||
.clear()
|
||||
.orecf()
|
||||
.clear()
|
||||
});
|
||||
}
|
||||
|
||||
const fn calculate_brr(peripheral_clock_hz: u32, baud: u32) -> Result<u16, BaudRateError> {
|
||||
if baud == 0 {
|
||||
return Err(BaudRateError::Zero);
|
||||
}
|
||||
|
||||
let divisor = (peripheral_clock_hz + (baud / 2)) / baud;
|
||||
if divisor == 0 || divisor > u16::MAX as u32 {
|
||||
Err(BaudRateError::OutOfRange)
|
||||
} else {
|
||||
Ok(divisor as u16)
|
||||
}
|
||||
}
|
||||
|
||||
#[cfg(test)]
|
||||
mod tests {
|
||||
use super::{BaudRateError, calculate_brr};
|
||||
|
||||
#[test]
|
||||
fn calculates_reset_clock_brr() {
|
||||
assert_eq!(calculate_brr(16_000_000, 115_200), Ok(139));
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn rejects_zero_baud() {
|
||||
assert_eq!(calculate_brr(16_000_000, 0), Err(BaudRateError::Zero));
|
||||
}
|
||||
}
|
||||
+2
-2
@@ -1,7 +1,7 @@
|
||||
//! Fixed STM32F722 DMA route for USART1 transmit.
|
||||
//!
|
||||
//! USART1 TX uses DMA2 stream 7, channel 4 on STM32F722. The route is a type,
|
||||
//! rather than board-provided integer parameters, so invalid request mappings
|
||||
//! rather than platform-provided integer parameters, so invalid request mappings
|
||||
//! cannot be selected at construction.
|
||||
|
||||
use kaptan_dma::{DmaReadableBuffer, DmaTxEngine, TxCompletion};
|
||||
@@ -38,7 +38,7 @@ impl Usart1TxDma {
|
||||
///
|
||||
/// The caller must retain `buffer`'s backing storage until
|
||||
/// [`Self::acknowledge_interrupt`] returns a terminal result.
|
||||
/// - The board must bind `DMA2_STREAM7`, call `acknowledge_interrupt` from
|
||||
/// - The platform must bind `DMA2_STREAM7`, call `acknowledge_interrupt` from
|
||||
/// that interrupt, and defer lifecycle handling to its single owner.
|
||||
pub fn start(&mut self, buffer: DmaReadableBuffer<'_>) -> Result<(), StartError> {
|
||||
let bytes = buffer.bytes();
|
||||
@@ -0,0 +1,219 @@
|
||||
//! Fixed STM32F722 DMA route for USART2 transmit.
|
||||
//!
|
||||
//! USART2 TX uses DMA1 Stream6, channel 4 on STM32F722. The route is a type,
|
||||
//! rather than platform-provided integer parameters, so invalid request mappings
|
||||
//! cannot be selected at construction.
|
||||
|
||||
use kaptan_dma::{DmaReadableBuffer, DmaTxEngine, TxCompletion};
|
||||
use stm32f7::stm32f722;
|
||||
|
||||
use super::Usart2Tx;
|
||||
|
||||
/// Owns the fixed USART2 TX DMA1 Stream6 route.
|
||||
pub struct Usart2TxDma {
|
||||
usart: Usart2Tx,
|
||||
}
|
||||
|
||||
/// Failure to start one DMA frame transfer on USART2.
|
||||
#[derive(Clone, Copy, Debug, Eq, PartialEq)]
|
||||
pub enum Usart2TxDmaStartError {
|
||||
/// DMA cannot transfer an empty buffer or one exceeding the NDTR limit.
|
||||
InvalidBufferLength,
|
||||
/// DMA1 stream 6 still owns an earlier transfer.
|
||||
Busy,
|
||||
}
|
||||
|
||||
impl Usart2TxDma {
|
||||
/// Enables DMA1 and connects USART2 TX to its fixed request route.
|
||||
pub fn new(rcc: &stm32f722::RCC, usart: Usart2Tx) -> Self {
|
||||
rcc.ahb1enr().modify(|_, write| write.dma1en().set_bit());
|
||||
Self { usart }
|
||||
}
|
||||
|
||||
/// Starts one memory-to-USART DMA transfer.
|
||||
///
|
||||
/// The caller retains the backing storage until
|
||||
/// [`Self::acknowledge_interrupt`] reports a terminal result.
|
||||
pub fn start(&mut self, buffer: DmaReadableBuffer<'_>) -> Result<(), Usart2TxDmaStartError> {
|
||||
let bytes = buffer.bytes();
|
||||
if bytes.is_empty() || bytes.len() > u16::MAX as usize {
|
||||
return Err(Usart2TxDmaStartError::InvalidBufferLength);
|
||||
}
|
||||
if self.is_active() {
|
||||
return Err(Usart2TxDmaStartError::Busy);
|
||||
}
|
||||
|
||||
self.clear_interrupt_flags();
|
||||
// Idle USART2 has TXE set. Enabling DMAT *after* the stream is armed
|
||||
// creates the initial request for byte zero without manually writing
|
||||
// TDR, so the whole frame is owned by DMA.
|
||||
self.configure_stream(bytes);
|
||||
self.stream().cr().modify(|_, write| write.en().enabled());
|
||||
self.usart.enable_dma_transmit();
|
||||
Ok(())
|
||||
}
|
||||
|
||||
/// Acknowledges a DMA1 Stream6 terminal IRQ without accessing engine state.
|
||||
pub fn acknowledge_interrupt() -> TxCompletion {
|
||||
// SAFETY: this handler accesses only the fixed DMA1 Stream6 route.
|
||||
let controller = unsafe { &*stm32f722::DMA1::ptr() };
|
||||
let status = controller.hisr().read();
|
||||
let completion = DmaTerminalStatus {
|
||||
transfer_complete: status.tcif6().is_complete(),
|
||||
transfer_error: status.teif6().is_error(),
|
||||
direct_mode_error: status.dmeif6().is_error(),
|
||||
fifo_error: status.feif6().is_error(),
|
||||
}
|
||||
.classify();
|
||||
if completion == TxCompletion::NotPending {
|
||||
return completion;
|
||||
}
|
||||
|
||||
controller
|
||||
.st(6)
|
||||
.cr()
|
||||
.modify(|_, write| write.en().disabled());
|
||||
// SAFETY: USART2 is initialized once by `Usart2Tx::new`; this ISR
|
||||
// clears only its DMA transmit-request enable after the stream's
|
||||
// terminal status has been observed.
|
||||
unsafe {
|
||||
(&*stm32f722::USART2::ptr())
|
||||
.cr3()
|
||||
.modify(|_, write| write.dmat().clear_bit())
|
||||
};
|
||||
controller.hifcr().write(|write| {
|
||||
write
|
||||
.ctcif6()
|
||||
.clear()
|
||||
.chtif6()
|
||||
.clear()
|
||||
.cteif6()
|
||||
.clear()
|
||||
.cdmeif6()
|
||||
.clear()
|
||||
.cfeif6()
|
||||
.clear()
|
||||
});
|
||||
completion
|
||||
}
|
||||
|
||||
/// Returns whether DMA1 Stream6 owns a transfer.
|
||||
pub fn is_active(&self) -> bool {
|
||||
self.stream().cr().read().en().is_enabled()
|
||||
}
|
||||
|
||||
fn configure_stream(&mut self, buffer: &[u8]) {
|
||||
self.stream().cr().write(|write| {
|
||||
// SAFETY: Channel 4 and these transfer fields are the documented
|
||||
// USART2 TX mapping for DMA1 Stream6 on STM32F722.
|
||||
unsafe {
|
||||
write
|
||||
.chsel()
|
||||
.bits(4)
|
||||
.dir()
|
||||
.memory_to_peripheral()
|
||||
.pl()
|
||||
.very_high()
|
||||
.minc()
|
||||
.incremented()
|
||||
.pinc()
|
||||
.fixed()
|
||||
.msize()
|
||||
.bits8()
|
||||
.psize()
|
||||
.bits8()
|
||||
.circ()
|
||||
.disabled()
|
||||
.tcie()
|
||||
.enabled()
|
||||
.teie()
|
||||
.enabled()
|
||||
.dmeie()
|
||||
.enabled()
|
||||
}
|
||||
});
|
||||
self.stream().ndtr().write(|write| {
|
||||
// SAFETY: `start` checked the slice length against the NDTR limit.
|
||||
unsafe { write.bits(buffer.len() as u32) }
|
||||
});
|
||||
self.stream().par().write(|write| {
|
||||
// SAFETY: This is the memory-mapped USART2 transmit-data register.
|
||||
unsafe { write.bits(self.usart.transmit_data_register_address()) }
|
||||
});
|
||||
self.stream().m0ar().write(|write| {
|
||||
// SAFETY: The caller of `start` upholds the buffer lifetime contract.
|
||||
unsafe { write.bits(buffer.as_ptr() as u32) }
|
||||
});
|
||||
}
|
||||
|
||||
fn stream(&self) -> &stm32f722::dma2::ST {
|
||||
self.controller().st(6)
|
||||
}
|
||||
|
||||
fn clear_interrupt_flags(&self) {
|
||||
self.controller().hifcr().write(|write| {
|
||||
write
|
||||
.ctcif6()
|
||||
.clear()
|
||||
.chtif6()
|
||||
.clear()
|
||||
.cteif6()
|
||||
.clear()
|
||||
.cdmeif6()
|
||||
.clear()
|
||||
.cfeif6()
|
||||
.clear()
|
||||
});
|
||||
}
|
||||
|
||||
fn controller(&self) -> &stm32f722::dma2::RegisterBlock {
|
||||
// SAFETY: SPI3 owns DMA1 initialization; USART2 TX is permanently
|
||||
// assigned to disjoint Stream6 and accesses no other stream.
|
||||
unsafe { &*stm32f722::DMA1::ptr() }
|
||||
}
|
||||
}
|
||||
|
||||
#[derive(Clone, Copy, Debug, Eq, PartialEq)]
|
||||
struct DmaTerminalStatus {
|
||||
transfer_complete: bool,
|
||||
transfer_error: bool,
|
||||
direct_mode_error: bool,
|
||||
fifo_error: bool,
|
||||
}
|
||||
|
||||
impl DmaTerminalStatus {
|
||||
const fn classify(self) -> TxCompletion {
|
||||
if self.transfer_error || self.direct_mode_error || self.fifo_error {
|
||||
TxCompletion::TransferFault
|
||||
} else if self.transfer_complete {
|
||||
TxCompletion::Complete
|
||||
} else {
|
||||
TxCompletion::NotPending
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
impl DmaTxEngine for Usart2TxDma {
|
||||
type StartError = Usart2TxDmaStartError;
|
||||
|
||||
fn start(&mut self, buffer: DmaReadableBuffer<'_>) -> Result<(), Self::StartError> {
|
||||
Self::start(self, buffer)
|
||||
}
|
||||
}
|
||||
|
||||
#[cfg(test)]
|
||||
mod tests {
|
||||
use super::DmaTerminalStatus;
|
||||
use kaptan_dma::TxCompletion;
|
||||
|
||||
#[test]
|
||||
fn terminal_error_has_precedence_over_completion() {
|
||||
let status = DmaTerminalStatus {
|
||||
transfer_complete: true,
|
||||
transfer_error: false,
|
||||
direct_mode_error: false,
|
||||
fifo_error: true,
|
||||
};
|
||||
assert_eq!(status.classify(), TxCompletion::TransferFault);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,142 @@
|
||||
//! Platform-owned STM32F722 electrical route implementations.
|
||||
|
||||
use super::peripherals::{Spi1, Spi1Error, Spi3, Spi3Error};
|
||||
use kaptan_spi::{SpiCompletion, SpiDevice};
|
||||
use stm32f7::stm32f722;
|
||||
|
||||
/// Owns the MPU6000 SPI1 transaction path and its PB2 chip select.
|
||||
pub(crate) struct Mpu6000Spi1Route {
|
||||
spi: Spi1,
|
||||
chip_select: stm32f722::GPIOB,
|
||||
}
|
||||
|
||||
impl Mpu6000Spi1Route {
|
||||
pub(crate) fn new(rcc: &stm32f722::RCC, spi: Spi1, chip_select: stm32f722::GPIOB) -> Self {
|
||||
rcc.ahb1enr().modify(|_, write| write.gpioben().set_bit());
|
||||
chip_select
|
||||
.moder()
|
||||
.modify(|_, write| write.moder2().output());
|
||||
chip_select
|
||||
.otyper()
|
||||
.modify(|_, write| write.ot2().push_pull());
|
||||
chip_select
|
||||
.ospeedr()
|
||||
.modify(|_, write| write.ospeedr2().low_speed());
|
||||
chip_select
|
||||
.pupdr()
|
||||
.modify(|_, write| write.pupdr2().floating());
|
||||
chip_select.bsrr().write(|write| write.bs2().set_bit());
|
||||
Self { spi, chip_select }
|
||||
}
|
||||
|
||||
fn select(&self) {
|
||||
self.chip_select.bsrr().write(|write| write.br2().set_bit());
|
||||
}
|
||||
|
||||
fn deselect(&self) {
|
||||
self.chip_select.bsrr().write(|write| write.bs2().set_bit());
|
||||
}
|
||||
}
|
||||
|
||||
impl SpiDevice for Mpu6000Spi1Route {
|
||||
type StartError = Spi1Error;
|
||||
|
||||
fn start_transfer(
|
||||
&mut self,
|
||||
transmit: &[u8],
|
||||
receive: &mut [u8],
|
||||
) -> Result<(), Self::StartError> {
|
||||
self.select();
|
||||
if let Err(error) = self.spi.start_transfer(transmit, receive) {
|
||||
self.deselect();
|
||||
return Err(error);
|
||||
}
|
||||
Ok(())
|
||||
}
|
||||
|
||||
fn complete_transfer(&mut self, completion: SpiCompletion) {
|
||||
self.spi.complete_transfer(completion);
|
||||
if matches!(
|
||||
completion,
|
||||
SpiCompletion::Complete | SpiCompletion::TransferFault
|
||||
) {
|
||||
self.deselect();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/// Owns the ICM-42688-P SPI3 transaction path. PA15 is its chip select.
|
||||
///
|
||||
/// GPIOA itself stays owned by the platform data-channel/LED state. The route
|
||||
/// uses only PA15 BSRR writes, disjoint from USART2 (PA2/PA3) and the blue
|
||||
/// LED (PA13), so no read-modify-write alias exists between these owners.
|
||||
pub(crate) struct Icm42688Spi3Route {
|
||||
spi: Spi3,
|
||||
}
|
||||
|
||||
impl Icm42688Spi3Route {
|
||||
pub(crate) fn new(rcc: &stm32f722::RCC, spi: Spi3, chip_select: &stm32f722::GPIOA) -> Self {
|
||||
rcc.ahb1enr().modify(|_, write| write.gpioaen().set_bit());
|
||||
chip_select
|
||||
.moder()
|
||||
.modify(|_, write| write.moder15().output());
|
||||
chip_select
|
||||
.otyper()
|
||||
.modify(|_, write| write.ot15().push_pull());
|
||||
chip_select
|
||||
.ospeedr()
|
||||
.modify(|_, write| write.ospeedr15().low_speed());
|
||||
chip_select
|
||||
.pupdr()
|
||||
.modify(|_, write| write.pupdr15().floating());
|
||||
chip_select.bsrr().write(|write| write.bs15().set_bit());
|
||||
Self { spi }
|
||||
}
|
||||
|
||||
fn chip_select_port() -> &'static stm32f722::gpioa::RegisterBlock {
|
||||
// SAFETY: RTIC initialization configures GPIOA once before interrupts
|
||||
// are enabled. This route subsequently writes only PA15 through BSRR;
|
||||
// USART2 uses PA2/PA3 and the LED uses PA13 through disjoint BSRR bits.
|
||||
// No task performs a GPIOA read-modify-write on those shared outputs.
|
||||
unsafe { &*stm32f722::GPIOA::ptr() }
|
||||
}
|
||||
|
||||
fn select(&self) {
|
||||
Self::chip_select_port()
|
||||
.bsrr()
|
||||
.write(|write| write.br15().set_bit());
|
||||
}
|
||||
|
||||
fn deselect(&self) {
|
||||
Self::chip_select_port()
|
||||
.bsrr()
|
||||
.write(|write| write.bs15().set_bit());
|
||||
}
|
||||
}
|
||||
|
||||
impl SpiDevice for Icm42688Spi3Route {
|
||||
type StartError = Spi3Error;
|
||||
|
||||
fn start_transfer(
|
||||
&mut self,
|
||||
transmit: &[u8],
|
||||
receive: &mut [u8],
|
||||
) -> Result<(), Self::StartError> {
|
||||
self.select();
|
||||
if let Err(error) = self.spi.start_transfer(transmit, receive) {
|
||||
self.deselect();
|
||||
return Err(error);
|
||||
}
|
||||
Ok(())
|
||||
}
|
||||
|
||||
fn complete_transfer(&mut self, completion: SpiCompletion) {
|
||||
self.spi.complete_transfer(completion);
|
||||
if matches!(
|
||||
completion,
|
||||
SpiCompletion::Complete | SpiCompletion::TransferFault
|
||||
) {
|
||||
self.deselect();
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,163 @@
|
||||
//! Platform-owned F722 sensor-service assembly.
|
||||
|
||||
use super::peripherals::{DwtMicrosecondClock, Spi1, Spi3};
|
||||
use kaptan_drivers::{
|
||||
icm42688::{Icm42688, Icm42688Service, Icm42688ServiceEvent},
|
||||
mpu6000::{
|
||||
AccelerometerRange, DigitalLowPassFilter, GyroscopeRange, Mpu6000, Mpu6000Config,
|
||||
Mpu6000Service, Mpu6000ServiceEvent,
|
||||
},
|
||||
};
|
||||
use kaptan_sensor::imu::ImuSnapshot;
|
||||
use kaptan_spi::SpiCompletion;
|
||||
use stm32f7::stm32f722;
|
||||
|
||||
use super::routes::{Icm42688Spi3Route, Mpu6000Spi1Route};
|
||||
|
||||
const MPU6000_CONFIG: Mpu6000Config = Mpu6000Config {
|
||||
accelerometer: AccelerometerRange::PlusMinus4G,
|
||||
gyroscope: GyroscopeRange::PlusMinus500DegreesPerSecond,
|
||||
digital_low_pass_filter: DigitalLowPassFilter::Bandwidth42Hz,
|
||||
sample_rate_divider: 9,
|
||||
};
|
||||
|
||||
/// A profile-neutral IMU worker result. Device protocol errors are consumed
|
||||
/// here because flight policy only needs the terminal healthy/fault outcome.
|
||||
pub(crate) enum ImuEvent {
|
||||
Idle,
|
||||
Initialized,
|
||||
Sample(ImuSnapshot),
|
||||
Fault,
|
||||
}
|
||||
|
||||
/// Opaque owner of the selected F722 IMU service.
|
||||
///
|
||||
/// RTIC places local resource types in generated public metadata, so this
|
||||
/// wrapper is public. The device services and routes remain private to the
|
||||
/// platform, preserving the board-facing abstraction boundary.
|
||||
pub struct F722Imu {
|
||||
inner: F722ImuKind,
|
||||
}
|
||||
|
||||
/// The two concrete IMU services supported by the F722 platform.
|
||||
enum F722ImuKind {
|
||||
Mpu6000(Mpu6000Service<DwtMicrosecondClock, Mpu6000Spi1Route>),
|
||||
Icm42688(Icm42688Service<DwtMicrosecondClock, Icm42688Spi3Route>),
|
||||
}
|
||||
|
||||
impl F722Imu {
|
||||
pub(crate) fn mpu6000(
|
||||
rcc: &stm32f722::RCC,
|
||||
spi: stm32f722::SPI1,
|
||||
dma: stm32f722::DMA2,
|
||||
chip_select: stm32f722::GPIOB,
|
||||
clock: DwtMicrosecondClock,
|
||||
) -> Self {
|
||||
Self {
|
||||
inner: F722ImuKind::Mpu6000(Mpu6000Service::new(
|
||||
clock,
|
||||
Mpu6000::new(
|
||||
Mpu6000Spi1Route::new(rcc, Spi1::new(rcc, spi, dma), chip_select),
|
||||
MPU6000_CONFIG,
|
||||
),
|
||||
)),
|
||||
}
|
||||
}
|
||||
|
||||
pub(crate) fn icm42688(
|
||||
rcc: &stm32f722::RCC,
|
||||
spi: stm32f722::SPI3,
|
||||
dma: stm32f722::DMA1,
|
||||
chip_select: &stm32f722::GPIOA,
|
||||
clock: DwtMicrosecondClock,
|
||||
) -> Self {
|
||||
Self {
|
||||
inner: F722ImuKind::Icm42688(Icm42688Service::new(
|
||||
clock,
|
||||
Icm42688::new(Icm42688Spi3Route::new(
|
||||
rcc,
|
||||
Spi3::new(rcc, spi, dma),
|
||||
chip_select,
|
||||
)),
|
||||
)),
|
||||
}
|
||||
}
|
||||
|
||||
pub(crate) fn begin(&mut self) -> ImuEvent {
|
||||
let ok = match &mut self.inner {
|
||||
F722ImuKind::Mpu6000(imu) => imu.begin().is_ok(),
|
||||
F722ImuKind::Icm42688(imu) => imu.begin().is_ok(),
|
||||
};
|
||||
if ok { ImuEvent::Idle } else { ImuEvent::Fault }
|
||||
}
|
||||
|
||||
pub(crate) fn on_periodic_tick(&mut self) -> ImuEvent {
|
||||
match &mut self.inner {
|
||||
F722ImuKind::Mpu6000(imu) => mpu_event(imu.on_periodic_tick()),
|
||||
F722ImuKind::Icm42688(imu) => icm_event(imu.on_periodic_tick()),
|
||||
}
|
||||
}
|
||||
|
||||
pub(crate) fn on_data_ready(&mut self) -> ImuEvent {
|
||||
match &mut self.inner {
|
||||
F722ImuKind::Mpu6000(imu) => mpu_event(imu.on_data_ready()),
|
||||
F722ImuKind::Icm42688(imu) => icm_event(imu.on_data_ready()),
|
||||
}
|
||||
}
|
||||
|
||||
pub(crate) fn on_transfer_complete(&mut self, completion: SpiCompletion) -> ImuEvent {
|
||||
match &mut self.inner {
|
||||
F722ImuKind::Mpu6000(imu) => mpu_event(imu.on_transfer_complete(completion)),
|
||||
F722ImuKind::Icm42688(imu) => icm_event(imu.on_transfer_complete(completion)),
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
fn mpu_event<E>(event: Mpu6000ServiceEvent<E>) -> ImuEvent {
|
||||
match event {
|
||||
Mpu6000ServiceEvent::Idle => ImuEvent::Idle,
|
||||
Mpu6000ServiceEvent::Initialized => ImuEvent::Initialized,
|
||||
Mpu6000ServiceEvent::Sample(sample) => ImuEvent::Sample(sample),
|
||||
Mpu6000ServiceEvent::Fault(_) => ImuEvent::Fault,
|
||||
}
|
||||
}
|
||||
|
||||
fn icm_event<E>(event: Icm42688ServiceEvent<E>) -> ImuEvent {
|
||||
match event {
|
||||
Icm42688ServiceEvent::Idle => ImuEvent::Idle,
|
||||
Icm42688ServiceEvent::Initialized => ImuEvent::Initialized,
|
||||
Icm42688ServiceEvent::Sample(sample) => ImuEvent::Sample(align_icm42688(sample)),
|
||||
Icm42688ServiceEvent::Fault(_) => ImuEvent::Fault,
|
||||
}
|
||||
}
|
||||
|
||||
/// Aligns the verified GEP-F722-HD V2 ICM-42688-P yaw-180° mounting to the
|
||||
/// flight-controller body frame. The MPU6000 profile is already aligned.
|
||||
fn align_icm42688(mut sample: ImuSnapshot) -> ImuSnapshot {
|
||||
sample.acceleration_milligravity[0] = sample.acceleration_milligravity[0].saturating_neg();
|
||||
sample.acceleration_milligravity[1] = sample.acceleration_milligravity[1].saturating_neg();
|
||||
sample.angular_rate_milliradians_per_second[0] =
|
||||
sample.angular_rate_milliradians_per_second[0].saturating_neg();
|
||||
sample.angular_rate_milliradians_per_second[1] =
|
||||
sample.angular_rate_milliradians_per_second[1].saturating_neg();
|
||||
sample
|
||||
}
|
||||
|
||||
#[cfg(test)]
|
||||
mod tests {
|
||||
use super::align_icm42688;
|
||||
use kaptan_sensor::imu::ImuSnapshot;
|
||||
use kaptan_time::Timestamp;
|
||||
|
||||
#[test]
|
||||
fn geprc_profile_applies_its_documented_yaw_alignment() {
|
||||
let sample = align_icm42688(ImuSnapshot::new(
|
||||
Timestamp::from_microseconds(0),
|
||||
[10, 20, 30],
|
||||
[40, 50, 60],
|
||||
0,
|
||||
));
|
||||
assert_eq!(sample.acceleration_milligravity, [-10, -20, 30]);
|
||||
assert_eq!(sample.angular_rate_milliradians_per_second, [-40, -50, 60]);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,484 @@
|
||||
//! STM32F7 execution platform for declarative Kaptan boards.
|
||||
//!
|
||||
//! A board crate selects the safe route descriptors below and invokes
|
||||
//! [`flight_firmware!`]. The macro emits only a declarative link-time board
|
||||
//! registration; the one RTIC application for every supported STM32F722
|
||||
//! profile is defined and compiled in this crate. Consequently RTIC, PAC
|
||||
//! ownership, interrupt acknowledgement, DMA and atomics remain platform
|
||||
//! implementation details.
|
||||
|
||||
#![no_std]
|
||||
|
||||
use core::marker::PhantomData;
|
||||
|
||||
pub use kaptan_board::BoardConfig;
|
||||
use kaptan_board::BoardConfigError;
|
||||
|
||||
mod actuator;
|
||||
mod f722;
|
||||
|
||||
/// Dependencies used only while the platform expands its private RTIC macro.
|
||||
/// They stay crate-private: board binaries never name them.
|
||||
#[cfg(target_arch = "arm")]
|
||||
pub(crate) mod __platform_internal {
|
||||
pub(crate) use cortex_m as cm;
|
||||
pub(crate) use cortex_m_rt as cmrt;
|
||||
pub(crate) use kaptan_dma as dma;
|
||||
pub(crate) use kaptan_mavlink as mavlink;
|
||||
pub(crate) use kaptan_spi as spi;
|
||||
pub(crate) use panic_halt as panic;
|
||||
pub(crate) use rtic as rtic_app;
|
||||
pub(crate) use stm32f7 as pac;
|
||||
}
|
||||
|
||||
/// The isolated diagnostic macro expands an entry attribute in a board binary.
|
||||
/// Rust macro visibility requires this one dependency to be reachable; it is
|
||||
/// not a platform configuration or execution API.
|
||||
#[cfg(target_arch = "arm")]
|
||||
#[doc(hidden)]
|
||||
pub mod __macro_support {
|
||||
pub use cortex_m_rt;
|
||||
}
|
||||
|
||||
/// Construction or validation failure for a declarative F722 board.
|
||||
#[derive(Clone, Copy, Debug, Eq, PartialEq)]
|
||||
pub enum BoardError {
|
||||
/// Portable board metadata is self-inconsistent.
|
||||
InvalidConfig(BoardConfigError),
|
||||
/// Metadata and the selected vertical-sensor route disagree.
|
||||
VerticalSensorCapabilityMismatch,
|
||||
/// The declaration requests a service-timer frequency not implemented by
|
||||
/// the selected F722 platform profile.
|
||||
UnsupportedServiceFrequency,
|
||||
/// The platform could not establish a clock, peripheral, or DMA owner.
|
||||
Initialization,
|
||||
}
|
||||
|
||||
/// MPU6000 component selector.
|
||||
#[derive(Clone, Copy, Debug, Eq, PartialEq)]
|
||||
pub struct Mpu6000;
|
||||
|
||||
/// The supported MPU6000 SPI1 electrical route before selecting DRDY.
|
||||
#[derive(Clone, Copy, Debug, Eq, PartialEq)]
|
||||
pub struct Mpu6000Spi1;
|
||||
|
||||
/// The complete MPU6000 SPI1 / PA4 data-ready route.
|
||||
#[derive(Clone, Copy, Debug, Eq, PartialEq)]
|
||||
pub struct Mpu6000Spi1Exti4;
|
||||
|
||||
impl Mpu6000 {
|
||||
/// Selects the supported SPI1 MPU6000 connection.
|
||||
pub const fn on_spi1() -> Mpu6000Spi1 {
|
||||
Mpu6000Spi1
|
||||
}
|
||||
}
|
||||
|
||||
impl Mpu6000Spi1 {
|
||||
/// Completes the supported PA4 data-ready connection.
|
||||
pub const fn with_drdy(self, _: Pa4) -> Mpu6000Spi1Exti4 {
|
||||
Mpu6000Spi1Exti4
|
||||
}
|
||||
}
|
||||
|
||||
/// ICM-42688-P component selector.
|
||||
#[derive(Clone, Copy, Debug, Eq, PartialEq)]
|
||||
pub struct Icm42688;
|
||||
|
||||
/// The supported ICM-42688-P SPI3 electrical route before selecting DRDY.
|
||||
#[derive(Clone, Copy, Debug, Eq, PartialEq)]
|
||||
pub struct Icm42688Spi3;
|
||||
|
||||
/// The complete ICM-42688-P SPI3 / PC3 data-ready route.
|
||||
#[derive(Clone, Copy, Debug, Eq, PartialEq)]
|
||||
pub struct Icm42688Spi3Exti3;
|
||||
|
||||
impl Icm42688 {
|
||||
/// Selects the supported SPI3 ICM-42688-P connection.
|
||||
pub const fn on_spi3() -> Icm42688Spi3 {
|
||||
Icm42688Spi3
|
||||
}
|
||||
}
|
||||
|
||||
impl Icm42688Spi3 {
|
||||
/// Completes the supported PC3 data-ready connection.
|
||||
pub const fn with_drdy(self, _: Pc3) -> Icm42688Spi3Exti3 {
|
||||
Icm42688Spi3Exti3
|
||||
}
|
||||
}
|
||||
|
||||
/// MPU6000 data-ready pin selector.
|
||||
#[derive(Clone, Copy, Debug, Eq, PartialEq)]
|
||||
pub struct Pa4;
|
||||
/// ICM-42688-P data-ready pin selector.
|
||||
#[derive(Clone, Copy, Debug, Eq, PartialEq)]
|
||||
pub struct Pc3;
|
||||
|
||||
/// Motor-output component selector.
|
||||
#[derive(Clone, Copy, Debug, Eq, PartialEq)]
|
||||
pub struct MotorOutputs;
|
||||
/// Kakute TIM3/TIM5 motor route before selecting a waveform protocol.
|
||||
#[derive(Clone, Copy, Debug, Eq, PartialEq)]
|
||||
pub struct KakuteTim3Tim5Motors;
|
||||
/// GEPRC TIM3/TIM5 motor route before selecting a waveform protocol.
|
||||
#[derive(Clone, Copy, Debug, Eq, PartialEq)]
|
||||
pub struct GeprcTim3Tim5Motors;
|
||||
/// Validated Kakute TIM3/TIM5 DShot600 motor route.
|
||||
#[derive(Clone, Copy, Debug, Eq, PartialEq)]
|
||||
pub struct KakuteTim3Tim5Dshot600;
|
||||
/// Validated GEPRC TIM3/TIM5 DShot600 motor route.
|
||||
#[derive(Clone, Copy, Debug, Eq, PartialEq)]
|
||||
pub struct GeprcTim3Tim5Dshot600;
|
||||
|
||||
impl MotorOutputs {
|
||||
/// Selects Kakute's M1–M4 mapping: PB0, PB1, PB4, PB5.
|
||||
pub const fn kakute_tim3_tim5() -> KakuteTim3Tim5Motors {
|
||||
KakuteTim3Tim5Motors
|
||||
}
|
||||
|
||||
/// Selects GEPRC's M1–M4 mapping: PB4, PB5, PB0, PB1.
|
||||
pub const fn geprc_tim3_tim5() -> GeprcTim3Tim5Motors {
|
||||
GeprcTim3Tim5Motors
|
||||
}
|
||||
}
|
||||
|
||||
impl KakuteTim3Tim5Motors {
|
||||
/// Selects the platform-validated DShot600 waveform configuration.
|
||||
pub const fn dshot600(self) -> KakuteTim3Tim5Dshot600 {
|
||||
KakuteTim3Tim5Dshot600
|
||||
}
|
||||
}
|
||||
|
||||
impl GeprcTim3Tim5Motors {
|
||||
/// Selects the platform-validated DShot600 waveform configuration.
|
||||
pub const fn dshot600(self) -> GeprcTim3Tim5Dshot600 {
|
||||
GeprcTim3Tim5Dshot600
|
||||
}
|
||||
}
|
||||
|
||||
/// MAVLink component selector.
|
||||
#[derive(Clone, Copy, Debug, Eq, PartialEq)]
|
||||
pub struct Mavlink;
|
||||
/// Validated MAVLink route using USART1.
|
||||
#[derive(Clone, Copy, Debug, Eq, PartialEq)]
|
||||
pub struct MavlinkUsart1;
|
||||
/// Validated MAVLink route using USART2.
|
||||
#[derive(Clone, Copy, Debug, Eq, PartialEq)]
|
||||
pub struct MavlinkUsart2;
|
||||
|
||||
impl Mavlink {
|
||||
/// Selects the supported USART1 MAVLink route.
|
||||
pub const fn on_usart1() -> MavlinkUsart1 {
|
||||
MavlinkUsart1
|
||||
}
|
||||
|
||||
/// Selects the supported USART2 MAVLink route.
|
||||
pub const fn on_usart2() -> MavlinkUsart2 {
|
||||
MavlinkUsart2
|
||||
}
|
||||
}
|
||||
|
||||
/// Vertical-sensor component selector.
|
||||
#[derive(Clone, Copy, Debug, Eq, PartialEq)]
|
||||
pub struct VerticalSensor;
|
||||
/// No vertical-sensor connection is selected.
|
||||
#[derive(Clone, Copy, Debug, Eq, PartialEq)]
|
||||
pub struct NoVerticalSensor;
|
||||
/// Validated MTF-01 Microlink route.
|
||||
#[derive(Clone, Copy, Debug, Eq, PartialEq)]
|
||||
pub struct Mtf01Uart4;
|
||||
/// Validated LIDAR-Lite v3 route.
|
||||
#[derive(Clone, Copy, Debug, Eq, PartialEq)]
|
||||
pub struct LidarLiteV3I2c1;
|
||||
|
||||
impl VerticalSensor {
|
||||
/// Declares no vertical-sensor peripheral.
|
||||
pub const fn none() -> NoVerticalSensor {
|
||||
NoVerticalSensor
|
||||
}
|
||||
|
||||
/// Selects the supported MTF-01 Microlink connection.
|
||||
pub const fn mtf01_on_uart4() -> Mtf01Uart4 {
|
||||
Mtf01Uart4
|
||||
}
|
||||
|
||||
/// Selects the supported LIDAR-Lite v3 connection.
|
||||
pub const fn lidar_lite_v3_on_i2c1() -> LidarLiteV3I2c1 {
|
||||
LidarLiteV3I2c1
|
||||
}
|
||||
}
|
||||
|
||||
/// Unselected typestate used only while a board declaration is incomplete.
|
||||
#[derive(Clone, Copy, Debug, Eq, PartialEq)]
|
||||
pub struct Missing;
|
||||
/// Safe typestate builder for a declarative F722 board.
|
||||
///
|
||||
/// This type has no peripherals, PAC types, interrupt names, DMA identifiers,
|
||||
/// atomics, or unsafe operations. The F722 platform has one fixed validated
|
||||
/// clock tree, so only board-selectable electrical routes participate in
|
||||
/// typestate. A final `build()` exists only for complete supported route
|
||||
/// combinations, so omitted or cross-wired essential routes fail to compile at
|
||||
/// the board declaration.
|
||||
pub struct F7BoardBuilder<
|
||||
Imu = Missing,
|
||||
Motors = Missing,
|
||||
MavlinkRoute = Missing,
|
||||
Vertical = Missing,
|
||||
> {
|
||||
_state: PhantomData<(Imu, Motors, MavlinkRoute, Vertical)>,
|
||||
}
|
||||
|
||||
impl F7BoardBuilder {
|
||||
/// Starts an empty board declaration.
|
||||
pub const fn new() -> Self {
|
||||
Self {
|
||||
_state: PhantomData,
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
impl Default for F7BoardBuilder {
|
||||
fn default() -> Self {
|
||||
Self::new()
|
||||
}
|
||||
}
|
||||
|
||||
impl<I, M, T, V> F7BoardBuilder<I, M, T, V> {
|
||||
/// Selects an IMU route descriptor.
|
||||
///
|
||||
/// Only a complete, profile-compatible descriptor has a final
|
||||
/// [`F7BoardBuilder::build`] implementation. Keeping this one selection
|
||||
/// operation uniform avoids teaching a board author platform profile
|
||||
/// mechanics merely because the selected IMU model differs.
|
||||
pub fn imu<Selected>(self, _: Selected) -> F7BoardBuilder<Selected, M, T, V> {
|
||||
F7BoardBuilder {
|
||||
_state: PhantomData,
|
||||
}
|
||||
}
|
||||
|
||||
/// Selects the TIM3/TIM5 DShot600 output route.
|
||||
pub fn motors<Selected>(self, _: Selected) -> F7BoardBuilder<I, Selected, T, V> {
|
||||
F7BoardBuilder {
|
||||
_state: PhantomData,
|
||||
}
|
||||
}
|
||||
|
||||
/// Selects a validated MAVLink route. Only profile-compatible route types
|
||||
/// have a final [`F7BoardBuilder::build`] implementation.
|
||||
pub fn mavlink<Selected>(self, _: Selected) -> F7BoardBuilder<I, M, Selected, V> {
|
||||
F7BoardBuilder {
|
||||
_state: PhantomData,
|
||||
}
|
||||
}
|
||||
|
||||
/// Selects a validated vertical-sensor route. Its capability is checked
|
||||
/// against [`F7Board::CONFIG`] before any peripheral is initialized.
|
||||
pub fn vertical_sensor<Selected>(self, _: Selected) -> F7BoardBuilder<I, M, T, Selected> {
|
||||
F7BoardBuilder {
|
||||
_state: PhantomData,
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/// Finite STM32F722 execution profile selected from safe route descriptors.
|
||||
#[derive(Clone, Copy, Debug, Eq, PartialEq)]
|
||||
pub(crate) enum F722Profile {
|
||||
Mpu6000Spi1Usart1,
|
||||
Icm42688Spi3Usart2,
|
||||
}
|
||||
|
||||
/// Fully validated, non-executing F722 board declaration.
|
||||
///
|
||||
/// It is deliberately only a compact route description. Peripheral tokens
|
||||
/// are taken later by the platform's RTIC initializer and never reach a board
|
||||
/// crate.
|
||||
#[doc(hidden)]
|
||||
#[derive(Clone, Copy, Debug, Eq, PartialEq)]
|
||||
pub struct F722BoardRoutes {
|
||||
profile: F722Profile,
|
||||
motors: f722::MotorRoute,
|
||||
vertical: f722::VerticalRoute,
|
||||
}
|
||||
|
||||
impl F722BoardRoutes {
|
||||
pub(crate) const fn profile(self) -> F722Profile {
|
||||
self.profile
|
||||
}
|
||||
}
|
||||
|
||||
macro_rules! impl_mpu6000_routes {
|
||||
($vertical:ty, $route:expr) => {
|
||||
impl F7BoardBuilder<Mpu6000Spi1Exti4, KakuteTim3Tim5Dshot600, MavlinkUsart1, $vertical> {
|
||||
/// Finishes a complete supported MPU6000 F722 declaration.
|
||||
pub const fn build(self) -> Result<F722BoardRoutes, BoardError> {
|
||||
Ok(F722BoardRoutes {
|
||||
profile: F722Profile::Mpu6000Spi1Usart1,
|
||||
motors: f722::MotorRoute::KakuteTim3Tim5,
|
||||
vertical: $route,
|
||||
})
|
||||
}
|
||||
}
|
||||
};
|
||||
}
|
||||
|
||||
impl_mpu6000_routes!(Mtf01Uart4, f722::VerticalRoute::Mtf01);
|
||||
impl_mpu6000_routes!(LidarLiteV3I2c1, f722::VerticalRoute::LidarLiteV3);
|
||||
impl_mpu6000_routes!(NoVerticalSensor, f722::VerticalRoute::None);
|
||||
|
||||
macro_rules! impl_icm42688_routes {
|
||||
($vertical:ty, $route:expr) => {
|
||||
impl F7BoardBuilder<Icm42688Spi3Exti3, GeprcTim3Tim5Dshot600, MavlinkUsart2, $vertical> {
|
||||
/// Finishes a complete supported ICM-42688-P F722 declaration.
|
||||
pub const fn build(self) -> Result<F722BoardRoutes, BoardError> {
|
||||
Ok(F722BoardRoutes {
|
||||
profile: F722Profile::Icm42688Spi3Usart2,
|
||||
motors: f722::MotorRoute::GeprcTim3Tim5,
|
||||
vertical: $route,
|
||||
})
|
||||
}
|
||||
}
|
||||
};
|
||||
}
|
||||
|
||||
impl_icm42688_routes!(NoVerticalSensor, f722::VerticalRoute::None);
|
||||
impl_icm42688_routes!(Mtf01Uart4, f722::VerticalRoute::Mtf01);
|
||||
impl_icm42688_routes!(LidarLiteV3I2c1, f722::VerticalRoute::LidarLiteV3);
|
||||
|
||||
/// Missing and cross-profile route selections are rejected before firmware is
|
||||
/// linked, because no matching `build` implementation exists.
|
||||
///
|
||||
/// ```compile_fail
|
||||
/// use platform_stm32f7::{
|
||||
/// F7BoardBuilder, Mavlink, MotorOutputs, Mpu6000, Pa4,
|
||||
/// };
|
||||
///
|
||||
/// let _ = F7BoardBuilder::new()
|
||||
/// .imu(Mpu6000::on_spi1().with_drdy(Pa4))
|
||||
/// .motors(MotorOutputs::kakute_tim3_tim5().dshot600())
|
||||
/// .mavlink(Mavlink::on_usart1())
|
||||
/// .build(); // vertical route is missing
|
||||
/// ```
|
||||
///
|
||||
/// ```compile_fail
|
||||
/// use platform_stm32f7::{
|
||||
/// F7BoardBuilder, Icm42688, Mavlink, MotorOutputs, Pc3,
|
||||
/// VerticalSensor,
|
||||
/// };
|
||||
///
|
||||
/// let _ = F7BoardBuilder::new()
|
||||
/// .imu(Icm42688::on_spi3().with_drdy(Pc3))
|
||||
/// .motors(MotorOutputs::geprc_tim3_tim5().dshot600())
|
||||
/// .mavlink(Mavlink::on_usart1()) // ICM route requires USART2
|
||||
/// .vertical_sensor(VerticalSensor::none())
|
||||
/// .build();
|
||||
/// ```
|
||||
///
|
||||
/// ```compile_fail
|
||||
/// use platform_stm32f7::{
|
||||
/// F7BoardBuilder, Icm42688, Mavlink, MotorOutputs, Pc3, VerticalSensor,
|
||||
/// };
|
||||
///
|
||||
/// let _ = F7BoardBuilder::new()
|
||||
/// .imu(Icm42688::on_spi3().with_drdy(Pc3))
|
||||
/// .motors(MotorOutputs::kakute_tim3_tim5().dshot600())
|
||||
/// .mavlink(Mavlink::on_usart2())
|
||||
/// .vertical_sensor(VerticalSensor::none())
|
||||
/// .build(); // ICM/USART2 uses the GEPRC logical motor order.
|
||||
/// ```
|
||||
/// Contract implemented by a declarative STM32F722 board crate.
|
||||
pub trait F7Board {
|
||||
/// Portable identity, capabilities and outbound data-channel policy.
|
||||
const CONFIG: BoardConfig;
|
||||
|
||||
/// Selects one complete supported F722 route graph.
|
||||
fn build(platform: F7BoardBuilder) -> Result<F722BoardRoutes, BoardError>;
|
||||
}
|
||||
|
||||
/// Checks portable metadata at compile time.
|
||||
#[macro_export]
|
||||
macro_rules! enforce_f7_board_contract {
|
||||
($board:ty) => {
|
||||
const _: () = match <$board as $crate::F7Board>::CONFIG.validate() {
|
||||
Ok(()) => (),
|
||||
Err(_) => panic!("invalid STM32F7 board configuration"),
|
||||
};
|
||||
};
|
||||
}
|
||||
|
||||
/// Installs the platform-owned RTIC flight firmware for a declarative board.
|
||||
///
|
||||
/// The application binds the finite superset of interrupt roles needed by the
|
||||
/// currently supported F722 profiles. The route declaration selects which
|
||||
/// roles are initialized; inactive bindings acknowledge nothing. Adding an
|
||||
/// interrupt role is therefore a platform-only change, not a board task graph.
|
||||
#[macro_export]
|
||||
macro_rules! flight_firmware {
|
||||
($board:ty) => {
|
||||
$crate::enforce_f7_board_contract!($board);
|
||||
// The platform RTIC application calls these symbols during `init`.
|
||||
// They carry only the declarative board contract; RTIC, PAC, DMA,
|
||||
// atomics, and unsafe operations remain inside this platform crate.
|
||||
#[unsafe(no_mangle)]
|
||||
pub static KAPTAN_F722_BOARD_CONFIG: $crate::BoardConfig =
|
||||
<$board as $crate::F7Board>::CONFIG;
|
||||
#[unsafe(no_mangle)]
|
||||
pub extern "Rust" fn kaptan_f722_board_routes()
|
||||
-> Result<$crate::F722BoardRoutes, $crate::BoardError> {
|
||||
<$board as $crate::F7Board>::build($crate::F7BoardBuilder::new())
|
||||
}
|
||||
#[used]
|
||||
static KAPTAN_F722_PLATFORM_FIRMWARE: fn() = $crate::firmware_anchor;
|
||||
};
|
||||
}
|
||||
|
||||
/// Installs the isolated GEPRC UART2 physical-route diagnostic image.
|
||||
///
|
||||
/// This diagnostic never installs the flight application. It remains a
|
||||
/// platform-owned image and is intentionally separate from board declarations.
|
||||
#[macro_export]
|
||||
macro_rules! geprc_uart2_wire_test {
|
||||
() => {
|
||||
#[$crate::__macro_support::cortex_m_rt::entry]
|
||||
fn main() -> ! {
|
||||
$crate::__private::geprc_uart2_wire_test()
|
||||
}
|
||||
};
|
||||
}
|
||||
|
||||
/// Implementation support consumed only by the exported firmware macros.
|
||||
#[doc(hidden)]
|
||||
pub mod __private {
|
||||
pub use crate::f722::geprc_uart2_wire_test;
|
||||
}
|
||||
|
||||
/// Link anchor for the platform-owned F722 RTIC application.
|
||||
#[doc(hidden)]
|
||||
pub fn firmware_anchor() {}
|
||||
|
||||
#[cfg(test)]
|
||||
mod tests {
|
||||
use super::*;
|
||||
|
||||
#[test]
|
||||
fn route_descriptors_select_the_mpu6000_profile() {
|
||||
let routes = F7BoardBuilder::new()
|
||||
.imu(Mpu6000::on_spi1().with_drdy(Pa4))
|
||||
.motors(MotorOutputs::kakute_tim3_tim5().dshot600())
|
||||
.mavlink(Mavlink::on_usart1())
|
||||
.vertical_sensor(VerticalSensor::mtf01_on_uart4())
|
||||
.build()
|
||||
.unwrap();
|
||||
assert_eq!(routes.profile(), F722Profile::Mpu6000Spi1Usart1);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn route_descriptors_select_the_icm42688_profile() {
|
||||
let routes = F7BoardBuilder::new()
|
||||
.imu(Icm42688::on_spi3().with_drdy(Pc3))
|
||||
.motors(MotorOutputs::geprc_tim3_tim5().dshot600())
|
||||
.mavlink(Mavlink::on_usart2())
|
||||
.vertical_sensor(VerticalSensor::none())
|
||||
.build()
|
||||
.unwrap();
|
||||
assert_eq!(routes.profile(), F722Profile::Icm42688Spi3Usart2);
|
||||
}
|
||||
}
|
||||
Reference in new issue
Block a user