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

platform: inhibit motor pins and remove output routes

Browse Source
This commit is contained in:
Onur Özkan committed 2026-08-09 13:57:49 +03:00
1 parent 5cb4b7df68
commit b0d22b61d8
19 files changed
+426 -1126
Download Patch File Download Diff File Expand all files Collapse all files

No files matched your search

platforms/stm32f7/src/f722/board.rs
+56 -114
View File
@@ -1,6 +1,6 @@
//! Profile-independent STM32F722 flight state and ownership graph.
//! Physical-sensor telemetry STM32F722 board-I/O state and ownership graph.
use core::sync::atomic::{AtomicBool, AtomicU8, Ordering};
use core::sync::atomic::{AtomicU8, Ordering};
use super::peripherals::{
DwtMicrosecondClock, I2c1, I2c1Config, ReceiveInterrupt, Tim2Periodic, Uart4,
@@ -9,7 +9,6 @@ use super::peripherals::{
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,
@@ -18,17 +17,13 @@ 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,
VerticalRoute, clock,
data_channel::{
F722MavlinkTransmitter, F722StatusLed, GeprcStatusLed, SERVICE_HZ, SERVICE_TIMER_CONFIG,
vertical_data_config,
@@ -42,24 +37,6 @@ 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();
@@ -75,13 +52,11 @@ 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.
/// Only the MAVLink worker accesses 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.
@@ -104,7 +79,6 @@ pub struct F722InitResources {
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
@@ -113,7 +87,8 @@ pub(crate) fn validate_vertical_route(
config: BoardConfig,
routes: F722BoardRoutes,
) -> Result<(), BoardError> {
if config.data_channel.service_hz != SERVICE_HZ {
let telemetry = config.telemetry().ok_or(BoardError::Initialization)?;
if telemetry.data_channel.service_hz != SERVICE_HZ {
return Err(BoardError::UnsupportedServiceFrequency);
}
let expected = match routes.vertical {
@@ -121,7 +96,7 @@ pub(crate) fn validate_vertical_route(
VerticalRoute::Mtf01 => VerticalSensingKind::RangeAndOpticalFlow,
VerticalRoute::LidarLiteV3 => VerticalSensingKind::Range,
};
if config.vertical_sensing != expected {
if telemetry.vertical_sensing != expected {
return Err(BoardError::VerticalSensorCapabilityMismatch);
}
Ok(())
@@ -136,16 +111,15 @@ pub(crate) fn initialize(
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.
// read it with Acquire ordering and only access the initialized route.
interrupts::activate_profile(routes.profile());
match routes.profile() {
F722Profile::Mpu6000Spi1Usart1 => {
initialize_mpu6000(device, clock, config, routes.motors, routes.vertical)
initialize_mpu6000(device, clock, config, routes.vertical)
}
F722Profile::Icm42688Spi3Usart2 => {
initialize_icm42688(device, clock, config, routes.motors, routes.vertical)
initialize_icm42688(device, clock, config, routes.vertical)
}
}
}
@@ -154,7 +128,6 @@ 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);
@@ -172,7 +145,13 @@ fn initialize_mpu6000(
clock.fork(),
vertical,
)?;
let imu = F722Imu::mpu6000(&device.RCC, device.SPI1, device.DMA2, device.GPIOB, clock);
let imu = F722Imu::mpu6000(
&device.RCC,
device.SPI1,
device.DMA2,
device.GPIOB,
clock.fork(),
);
let transmitter = F722MavlinkTransmitter::usart1(&device.RCC, device.USART1)
.map_err(|_| BoardError::Initialization)?;
let led = F722StatusLed::Kakute(super::peripherals::ActiveLowLedPd2::new(
@@ -182,10 +161,7 @@ fn initialize_mpu6000(
finish(
&device.RCC,
device.TIM2,
device.TIM3,
device.TIM5,
config,
motors,
vertical,
imu,
range,
@@ -200,7 +176,6 @@ 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);
@@ -224,17 +199,20 @@ fn initialize_icm42688(
clock.fork(),
vertical,
)?;
let imu = F722Imu::icm42688(&device.RCC, device.SPI3, device.DMA1, &device.GPIOA, clock);
let imu = F722Imu::icm42688(
&device.RCC,
device.SPI3,
device.DMA1,
&device.GPIOA,
clock.fork(),
);
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,
@@ -249,10 +227,7 @@ fn initialize_icm42688(
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>,
@@ -281,13 +256,7 @@ fn finish(
range,
startup_ticks: 0,
},
mavlink: F722MavlinkState {
receiver: MavlinkReceiver::new(),
data_channel,
led,
config,
},
motors: crate::actuator::F722MotorOutput::new(rcc, tim3, tim5, MOTOR_PROTOCOL, motors),
mavlink: F722MavlinkState { data_channel, led },
})
}
@@ -439,42 +408,21 @@ pub(crate) fn process_imu(imu: &mut F722Imu, events: u8) {
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::Fault => {}
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) {
/// Handles bounded DMA and timer work for the telemetry image.
pub(crate) fn process_worker_events(&mut self, events: u8) {
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,
};
let completion = take_mavlink_completion();
self.data_channel.on_dma_complete(completion);
if completion == kaptan_dma::TxCompletion::Complete {
self.led.toggle();
@@ -482,31 +430,21 @@ impl F722MavlinkState {
}
if events & TIMER != 0 {
// The board is initialized and transmitting physical telemetry,
// but has no flight runtime or output authority. `STANDBY` with
// an unarmed base mode communicates that accurately; `UNINIT`
// would incorrectly claim initialization never completed.
self.data_channel
.on_service_tick(IMU_HEALTHY.load(Ordering::Acquire), motors_armed);
.on_service_tick(kaptan_mavlink::message::MavState::MAV_STATE_STANDBY, false);
}
}
}
/// 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(crate) fn take_mavlink_completion() -> kaptan_dma::TxCompletion {
match UART_COMPLETION.swap(0, Ordering::AcqRel) {
1 => kaptan_dma::TxCompletion::Complete,
2 => kaptan_dma::TxCompletion::TransferFault,
_ => kaptan_dma::TxCompletion::NotPending,
}
}
@@ -530,14 +468,16 @@ fn publish_lidar_event<E>(event: LidarLiteV3ServiceEvent<E>) {
#[cfg(test)]
mod tests {
use super::validate_vertical_route;
use crate::{F7BoardBuilder, Mavlink, MotorOutputs, Mpu6000, Pa4, VerticalSensor};
use kaptan_board::{BoardConfig, DataChannelRates, MavlinkEndpoint, VerticalSensingKind};
use crate::{F7BoardBuilder, Mavlink, Mpu6000, Pa4, VerticalSensor};
use kaptan_board::{
BoardConfig, BoardProfile, DataChannelRates, MavlinkEndpoint, TelemetryProfile,
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()
@@ -548,14 +488,16 @@ mod tests {
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,
},
profile: BoardProfile::TelemetryOnly(TelemetryProfile {
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),