public/kaptan
1
Fork 0
Code Issues 24 Pull Requests Actions Packages Projects Releases Wiki Activity

platforms/stm32f7: add reusable F722 firmware

Browse Source
This commit is contained in:
Onur Özkan committed 2026-08-01 20:54:21 +03:00
1 parent 2ae66c49a8
commit ac54c8308d
32 files changed
+4006 -248
Download Patch File Download Diff File Expand all files Collapse all files

No files matched your search

platforms/stm32f7/src/f722/board.rs
+558
View File
@@ -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)
);
}
}