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

feat(f722): add usb mavlink transport

Browse Source
This commit is contained in:
Onur Özkan committed 2026-09-06 23:14:00 +03:00
1 parent 7dc3ccb619
commit 604a7aba86
18 files changed
+1087 -208
Download Patch File Download Diff File Expand all files Collapse all files

No files matched your search

firmware/platforms/stm32f7/Cargo.toml
+4
View File
@@ -28,6 +28,7 @@ kaptan-mavlink = { path = "../../libs/kaptan-mavlink" }
kaptan-sensor = { path = "../../libs/kaptan-sensor" }
kaptan-spi = { path = "../../libs/kaptan-spi" }
kaptan-time = { path = "../../libs/kaptan-time" }
kaptan-tx = { path = "../../libs/kaptan-tx" }
cortex-m = "0.7"
cortex-m-rt = "0.7"
rtic = { version = "2.3", default-features = false, features = [
@@ -37,4 +38,7 @@ stm32f7 = { version = "0.16", default-features = false, features = [
"rt",
"stm32f722",
] }
synopsys-usb-otg = { version = "0.4", features = ["cortex-m", "fs"] }
usb-device = "0.3"
usbd-serial = "0.2"
panic-halt = "1"
firmware/platforms/stm32f7/src/f722/board.rs
+86 -37
View File
@@ -7,7 +7,7 @@ use super::peripherals::{
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,
configure_usart2_pins_pa2_pa3, configure_usb_fs_pins_pa11_pa12,
};
use kaptan_config::Config;
use kaptan_data_channel::{
@@ -40,7 +40,8 @@ use super::config::{
use super::{
VerticalRoute, clock,
data_channel::{
F722MavlinkTransmitter, F722StatusLed, GeprcStatusLed, SERVICE_HZ, SERVICE_TIMER_CONFIG,
F722MavlinkLink, F722StatusLed, GeprcStatusLed, MavlinkIngress, SERVICE_HZ,
SERVICE_TIMER_CONFIG,
},
interrupts,
sensors::{F722Imu, ImuEvent},
@@ -80,17 +81,19 @@ pub type F722ServiceTimer = Tim2Periodic;
/// RTIC-shared MAVLink state.
///
/// Only the MAVLink worker accesses this state, so its priority ceiling is 9
/// rather than the IMU or vertical-sensor IRQ ceiling.
/// The UART and USB handlers do bounded byte/packet admission at priority 8;
/// protocol dispatch and transmission policy remain in the priority-9 worker.
/// This state therefore cannot inherit the IMU or vertical-sensor IRQ ceiling.
pub struct F722MavlinkState<C, I, const MAX_PAYLOAD_BYTES: usize>
where
C: Clone + Config,
I: F7ImmutableConfig,
{
data_channel: DataChannel<'static, F722MavlinkTransmitter, SERVICE_HZ>,
data_channel: DataChannel<'static, SERVICE_HZ>,
link: F722MavlinkLink,
config_file: F722ConfigFile<C, I, MAX_PAYLOAD_BYTES>,
file_service: FileService,
receiver: IntegrityMavlinkReceiver<1>,
uart_receiver: IntegrityMavlinkReceiver<1>,
file_lease_ticks: u16,
mavlink_errors: ErrorSink,
mavlink_logs: LogSinkQueue<1, 1>,
@@ -229,9 +232,16 @@ where
.mpu6000_config()
.ok_or(BoardError::Initialization)?,
);
let transmitter =
F722MavlinkTransmitter::usart1(&device.RCC, device.USART1, hardware.mavlink_baud())
.map_err(|_| BoardError::Initialization)?;
let usb = initialize_usb(
&device.RCC,
&device.GPIOA,
device.OTG_FS_GLOBAL,
device.OTG_FS_DEVICE,
device.OTG_FS_PWRCLK,
immutable.f722().name,
)?;
let link = F722MavlinkLink::usart1(&device.RCC, device.USART1, hardware.mavlink_baud(), usb)
.map_err(|_| BoardError::Initialization)?;
if hardware.status_led() != F722StatusLedRoute::Pd2ActiveLow {
return Err(BoardError::Initialization);
}
@@ -248,7 +258,7 @@ where
range,
mtf01,
mtf01_uart,
transmitter,
link,
led,
configuration,
)
@@ -299,9 +309,16 @@ where
clock.fork(),
hardware.imu_mount(),
);
let transmitter =
F722MavlinkTransmitter::usart2(&device.RCC, device.USART2, hardware.mavlink_baud())
.map_err(|_| BoardError::Initialization)?;
let usb = initialize_usb(
&device.RCC,
&device.GPIOA,
device.OTG_FS_GLOBAL,
device.OTG_FS_DEVICE,
device.OTG_FS_PWRCLK,
immutable.f722().name,
)?;
let link = F722MavlinkLink::usart2(&device.RCC, device.USART2, hardware.mavlink_baud(), usb)
.map_err(|_| BoardError::Initialization)?;
if hardware.status_led() != F722StatusLedRoute::Pa13ActiveHigh {
return Err(BoardError::Initialization);
}
@@ -315,7 +332,7 @@ where
range,
mtf01,
mtf01_uart,
transmitter,
link,
led,
configuration,
)
@@ -331,7 +348,7 @@ fn finish<C, I, const MAX_PAYLOAD_BYTES: usize>(
range: Option<F722Range>,
mtf01: Option<F722Mtf01>,
mtf01_uart: Option<Uart4>,
transmitter: F722MavlinkTransmitter,
link: F722MavlinkLink,
led: F722StatusLed,
configuration: F722ConfigManager<C, I, MAX_PAYLOAD_BYTES>,
) -> Result<F722InitResources<C, I, MAX_PAYLOAD_BYTES>, BoardError>
@@ -350,7 +367,6 @@ where
};
let mavlink_log_filter = runtime.mavlink_log_level_filter;
let data_channel = DataChannel::new(
transmitter,
system_id,
component_id,
rates,
@@ -373,9 +389,10 @@ where
},
mavlink: F722MavlinkState {
data_channel,
link,
config_file: F722ConfigFile::new(configuration),
file_service: FileService::new(Target::new(system_id, component_id)),
receiver: IntegrityMavlinkReceiver::new(),
uart_receiver: IntegrityMavlinkReceiver::new(),
file_lease_ticks: 0,
mavlink_errors: ErrorSink::new(),
mavlink_logs: LogSinkQueue::new(mavlink_log_filter),
@@ -401,6 +418,19 @@ where
.map_err(|_| BoardError::Configuration)
}
fn initialize_usb(
rcc: &stm32f722::RCC,
gpioa: &stm32f722::GPIOA,
global: stm32f722::OTG_FS_GLOBAL,
device: stm32f722::OTG_FS_DEVICE,
power_clock: stm32f722::OTG_FS_PWRCLK,
product: &'static str,
) -> Result<super::peripherals::F722UsbMavlink, BoardError> {
configure_usb_fs_pins_pa11_pa12(rcc, gpioa);
super::peripherals::usb_mavlink(global, device, power_clock, product)
.map_err(|_| BoardError::Initialization)
}
fn configure_vertical_pins(
rcc: &stm32f722::RCC,
gpioa: &stm32f722::GPIOA,
@@ -583,13 +613,16 @@ where
const RECEIVED: u8 = 1 << 1;
const DMA: u8 = 1 << 2;
if events & RECEIVED != 0 {
// Poll at least every service tick, including when endpoint input was
// backpressured by a packet the worker had not yet consumed.
let usb_received = events & TIMER != 0 && self.link.poll_usb();
if events & RECEIVED != 0 || usb_received {
self.drain_file_frames();
}
if events & DMA != 0 {
let completion = take_mavlink_completion();
self.data_channel.on_dma_complete(completion);
self.link.on_dma_complete(completion);
if completion == kaptan_dma::TxCompletion::Complete {
self.led.toggle();
}
@@ -597,27 +630,41 @@ where
if events & TIMER != 0 {
self.tick_file_lease();
self.data_channel.on_service_tick();
if let Some(frame) = self.data_channel.on_service_tick() {
let _ = self.link.send_active(frame);
}
self.drain_one_mavlink_diagnostic();
}
}
pub fn receive_mavlink_interrupt(&mut self, event: ReceiveInterrupt) -> bool {
match event {
ReceiveInterrupt::Byte(byte) => self.receiver.receive_byte(byte),
ReceiveInterrupt::Byte(byte) => self.uart_receiver.receive_byte(byte),
ReceiveInterrupt::Fault => {
self.receiver.reset();
self.file_lease_ticks = 0;
self.file_service.expire(&mut self.config_file);
kaptan_log::warn!("mavlink receive fault; file transfer expired");
self.uart_receiver.reset();
kaptan_log::warn!("mavlink receive fault; framing reset");
false
}
ReceiveInterrupt::NotPending => false,
}
}
/// Acknowledges one OTG FS interrupt and admits USB MAVLink input.
pub fn on_usb_interrupt(&mut self) -> bool {
self.link.poll_usb()
}
fn drain_file_frames(&mut self) {
while let Some(frame) = self.receiver.take_received() {
while let Some((ingress, frame)) = self
.uart_receiver
.take_received()
.map(|frame| (MavlinkIngress::Uart, frame))
.or_else(|| {
self.link
.take_usb_received()
.map(|frame| (MavlinkIngress::Usb, frame))
})
{
let Some(principal) =
Principal::try_new(frame.raw().system_id(), frame.raw().component_id())
else {
@@ -644,7 +691,7 @@ where
&& control.status != FileStatus::RequestConflict
}
};
self.send_file_response(response);
self.send_file_response(ingress, response);
if refresh_lease {
self.file_lease_ticks = FILE_TRANSFER_LEASE_TICKS;
}
@@ -661,15 +708,17 @@ where
}
}
fn send_file_response(&mut self, response: FileResponse) {
fn send_file_response(&mut self, ingress: MavlinkIngress, response: FileResponse) {
let message = wire::encode_response(response);
let class = kaptan_mavlink::transmitter::FrameClass::Critical;
match message {
kaptan::MavMessage::FILE_CONTROL(data) => {
let _ = self.data_channel.send_message(class, &data);
let frame = self.data_channel.encode_message(class, &data);
let _ = self.link.send_to(ingress, frame);
}
kaptan::MavMessage::FILE_DATA(data) => {
let _ = self.data_channel.send_message(class, &data);
let frame = self.data_channel.encode_message(class, &data);
let _ = self.link.send_to(ingress, frame);
}
_ => unreachable!("file response encoder returned another message"),
}
@@ -692,25 +741,25 @@ where
}
fn submit_pending_mavlink_error(&mut self) -> bool {
let data_channel = &mut self.data_channel;
let (data_channel, link) = (&mut self.data_channel, &mut self.link);
self.mavlink_errors.submit_next(|record| {
let payload = encode_error(record);
data_channel
.send_message(kaptan_mavlink::transmitter::FrameClass::Critical, &payload)
.is_retained()
let frame = data_channel
.encode_message(kaptan_mavlink::transmitter::FrameClass::Critical, &payload);
link.send_active(frame)
})
}
fn submit_queued_mavlink_log(&mut self) {
let data_channel = &mut self.data_channel;
let (data_channel, link) = (&mut self.data_channel, &mut self.link);
let _ = self.mavlink_logs.submit_next(|record| {
let class = match record.delivery() {
DeliveryClass::Critical => kaptan_mavlink::transmitter::FrameClass::Critical,
DeliveryClass::BestEffort => kaptan_mavlink::transmitter::FrameClass::BestEffort,
};
let payload = encode_log(record);
let admission = data_channel.send_message(class, &payload);
let outcome = if admission.is_retained() {
let frame = data_channel.encode_message(class, &payload);
let outcome = if link.send_active(frame) {
TransportAdmission::Accepted
} else {
TransportAdmission::Retry
firmware/platforms/stm32f7/src/f722/clock.rs
+2
View File
@@ -6,6 +6,8 @@ use stm32f7::stm32f722;
/// Core frequency in whole megahertz after selecting the PLL216 board profile.
const CORE_CLOCK_MHZ: u32 = 216;
/// Core/AHB frequency used by fixed-frequency peripheral drivers.
pub(crate) const CORE_FREQUENCY_HZ: u32 = CORE_CLOCK_MHZ * 1_000_000;
/// A failure while establishing the board's clock tree or monotonic clock.
#[derive(Clone, Copy, Debug, Eq, PartialEq, KaptanError)]
firmware/platforms/stm32f7/src/f722/data_channel.rs
+66 -28
View File
@@ -1,12 +1,12 @@
//! F722 transport and vertical-route adapters for the portable data channel.
use super::peripherals;
use kaptan_data_channel::FrameSink;
use kaptan_dma::TxCompletion;
use kaptan_mavlink::{
MavlinkFrame,
transmitter::{EnqueueResult, FrameClass, MavlinkTransmitter},
MavlinkFrame, receiver::IntegrityCheckedFrame, transmitter::MavlinkTransmitter,
};
use kaptan_tx::PendingFrame;
use peripherals::F722UsbMavlink;
// Baud, sensor topology, and mounting remain immutable build-time policy. The
// output-inhibited board image may recover endpoint identity and selected
@@ -28,16 +28,25 @@ enum F722MavlinkTransport {
Usart2(MavlinkTransmitter<peripherals::DcacheDisabledDmaMemory, peripherals::Usart2TxDma, 4>),
}
/// Sole F722 MAVLink DMA owner.
pub(crate) struct F722MavlinkTransmitter {
inner: F722MavlinkTransport,
/// Physical link on which an inbound frame arrived.
#[derive(Clone, Copy, Debug, Eq, PartialEq)]
pub(crate) enum MavlinkIngress {
Uart,
Usb,
}
impl F722MavlinkTransmitter {
/// Concrete F722 MAVLink link ownership.
pub(crate) struct F722MavlinkLink {
uart: F722MavlinkTransport,
usb: F722UsbMavlink,
}
impl F722MavlinkLink {
pub(crate) fn usart1(
rcc: &stm32f7::stm32f722::RCC,
uart: stm32f7::stm32f722::USART1,
baud: u32,
usb: F722UsbMavlink,
) -> Result<Self, ()> {
let uart = peripherals::Usart1Tx::new(rcc, uart, UART1_CLOCK_HZ, baud).map_err(|_| ())?;
// SAFETY: platform initialization disables D-cache before this object
@@ -45,10 +54,11 @@ impl F722MavlinkTransmitter {
// this platform receives a terminal DMA completion notification.
let memory = unsafe { peripherals::DcacheDisabledDmaMemory::new() };
Ok(Self {
inner: F722MavlinkTransport::Usart1(MavlinkTransmitter::new(
uart: F722MavlinkTransport::Usart1(MavlinkTransmitter::new(
memory,
peripherals::Usart1TxDma::new(rcc, uart),
)),
usb,
})
}
@@ -56,45 +66,73 @@ impl F722MavlinkTransmitter {
rcc: &stm32f7::stm32f722::RCC,
uart: stm32f7::stm32f722::USART2,
baud: u32,
usb: F722UsbMavlink,
) -> Result<Self, ()> {
let uart = peripherals::Usart2Tx::new(rcc, uart, UART2_CLOCK_HZ, 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 {
inner: F722MavlinkTransport::Usart2(MavlinkTransmitter::new(
uart: F722MavlinkTransport::Usart2(MavlinkTransmitter::new(
memory,
peripherals::Usart2TxDma::new(rcc, uart),
)),
usb,
})
}
}
impl FrameSink for F722MavlinkTransmitter {
fn send(&mut self, class: FrameClass, frame: MavlinkFrame) -> EnqueueResult {
match &mut self.inner {
/// Sends publications on the open USB link, or on UART when USB is closed.
pub(crate) fn send_active(&mut self, pending: PendingFrame<MavlinkFrame>) -> bool {
if self.usb.is_open() {
self.usb.send(pending)
} else {
self.send_uart(pending)
}
}
/// Sends a protocol reply only on the link that supplied its request.
pub(crate) fn send_to(
&mut self,
ingress: MavlinkIngress,
pending: PendingFrame<MavlinkFrame>,
) -> bool {
match ingress {
MavlinkIngress::Uart => self.send_uart(pending),
MavlinkIngress::Usb => self.usb.send(pending),
}
}
fn send_uart(&mut self, pending: PendingFrame<MavlinkFrame>) -> bool {
match &mut self.uart {
F722MavlinkTransport::Usart1(transmitter) => transmitter
.admit_and_start(pending.class, pending.frame)
.admission
.is_retained(),
F722MavlinkTransport::Usart2(transmitter) => transmitter
.admit_and_start(pending.class, pending.frame)
.admission
.is_retained(),
}
}
pub(crate) fn on_dma_complete(&mut self, completion: TxCompletion) {
match &mut self.uart {
F722MavlinkTransport::Usart1(transmitter) => {
let outcome = transmitter.admit_and_start(class, frame);
let _ = outcome.start;
outcome.admission
let _ = transmitter.on_dma_complete(completion);
}
F722MavlinkTransport::Usart2(transmitter) => {
let outcome = transmitter.admit_and_start(class, frame);
let _ = outcome.start;
outcome.admission
let _ = transmitter.on_dma_complete(completion);
}
}
}
fn on_dma_complete(&mut self, completion: TxCompletion) {
match &mut self.inner {
F722MavlinkTransport::Usart1(transmitter) => {
let _ = transmitter.on_dma_complete(completion);
}
F722MavlinkTransport::Usart2(transmitter) => {
let _ = transmitter.on_dma_complete(completion);
}
}
/// Services USB and reports whether the protocol worker has bytes to parse.
pub(crate) fn poll_usb(&mut self) -> bool {
self.usb.poll()
}
pub(crate) fn take_usb_received(&mut self) -> Option<IntegrityCheckedFrame> {
self.usb.take_received()
}
}
firmware/platforms/stm32f7/src/f722/firmware.rs
+11
View File
@@ -248,6 +248,17 @@ macro_rules! __f722_flight_application {
}
}
#[task(binds = OTG_FS, priority = 8, shared = [mavlink])]
fn mavlink_usb(mut context: mavlink_usb::Context) {
if context
.shared
.mavlink
.lock(F722MavlinkState::on_usb_interrupt)
{
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();
firmware/platforms/stm32f7/src/f722/mod.rs
+1
View File
@@ -15,6 +15,7 @@ mod motor_inhibit;
mod peripherals;
mod routes;
mod sensors;
mod usb;
use kaptan_config::Config;
firmware/platforms/stm32f7/src/f722/peripherals/clock.rs
+40 -17
View File
@@ -3,7 +3,10 @@
use kaptan_err_reportable::KaptanError;
use stm32f7::stm32f722;
const CLOCK_SWITCH_POLL_LIMIT: u16 = 1_024;
const CLOCK_SWITCH_POLL_LIMIT: u32 = 1_024;
// Crystal startup is slower than a clock-tree state transition. This remains
// bounded so an absent oscillator reaches safe-idle instead of hanging boot.
const HSE_STARTUP_POLL_LIMIT: u32 = 1_000_000;
/// Failure to establish the 216 MHz PLL clock profile.
#[derive(Clone, Copy, Debug, Eq, PartialEq, KaptanError)]
@@ -22,30 +25,44 @@ pub enum ClockProfileError {
OverDriveSwitchNotReady,
#[kaptan_error(id = 11007)]
SystemClockNotPll,
#[kaptan_error(id = 11008)]
HseNotReady,
}
/// Selects the F722 platform's HSI PLL profile.
/// Selects the F722 platform's 8 MHz HSE 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
/// is introduced.
/// APB2 is 108 MHz. Both supported flight-controller boards have an 8 MHz
/// external oscillator; using it as the PLL source gives USB an accurate
/// 48 MHz clock through PLLQ.
pub fn select_pll216_profile(
rcc: &stm32f722::RCC,
pwr: &stm32f722::PWR,
flash: &stm32f722::FLASH,
) -> Result<(), ClockProfileError> {
rcc.cr().modify(|_, write| write.hsion().on());
if !wait_until(|| rcc.cr().read().hsirdy().is_ready()) {
if !wait_until(CLOCK_SWITCH_POLL_LIMIT, || {
rcc.cr().read().hsirdy().is_ready()
}) {
return Err(ClockProfileError::HsiNotReady);
}
// Return to HSI before changing a PLL left configured by an earlier boot stage.
rcc.cfgr().modify(|_, write| write.sw().hsi());
if !wait_until(|| rcc.cfgr().read().sws().is_hsi()) {
if !wait_until(CLOCK_SWITCH_POLL_LIMIT, || rcc.cfgr().read().sws().is_hsi()) {
return Err(ClockProfileError::SystemClockNotHsi);
}
rcc.cr().modify(|_, write| write.hseon().on());
if !wait_until(HSE_STARTUP_POLL_LIMIT, || {
rcc.cr().read().hserdy().is_ready()
}) {
return Err(ClockProfileError::HseNotReady);
}
rcc.cr().modify(|_, write| write.pllon().off());
if !wait_until(|| !rcc.cr().read().pllrdy().is_ready()) {
if !wait_until(CLOCK_SWITCH_POLL_LIMIT, || {
!rcc.cr().read().pllrdy().is_ready()
}) {
return Err(ClockProfileError::PllNotStopped);
}
@@ -56,11 +73,15 @@ pub fn select_pll216_profile(
// 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()) {
if !wait_until(CLOCK_SWITCH_POLL_LIMIT, || {
pwr.csr1().read().odrdy().bit_is_set()
}) {
return Err(ClockProfileError::OverDriveNotReady);
}
pwr.cr1().modify(|_, write| write.odswen().set_bit());
if !wait_until(|| pwr.csr1().read().odswrdy().bit_is_set()) {
if !wait_until(CLOCK_SWITCH_POLL_LIMIT, || {
pwr.csr1().read().odswrdy().bit_is_set()
}) {
return Err(ClockProfileError::OverDriveSwitchNotReady);
}
flash.acr().modify(|read, write| {
@@ -69,14 +90,16 @@ pub fn select_pll216_profile(
unsafe { write.bits((read.bits() & !0x0f) | 7 | (1 << 8) | (1 << 9)) }
});
// HSI / 16 * 432 / 2 = 216 MHz; Q=9 supplies a 48 MHz domain.
// HSE / 8 * 432 / 2 = 216 MHz; Q=9 supplies the exact 48 MHz USB domain.
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)) }
// SAFETY: M=8, N=432, P=/2 (the reset encoding), Q=9, and the HSE source
// are valid for the 8 MHz board oscillator; the PLL is disabled first.
unsafe { write.bits(8 | (432 << 6) | (1 << 22) | (9 << 24)) }
});
rcc.cr().modify(|_, write| write.pllon().on());
if !wait_until(|| rcc.cr().read().pllrdy().is_ready()) {
if !wait_until(CLOCK_SWITCH_POLL_LIMIT, || {
rcc.cr().read().pllrdy().is_ready()
}) {
return Err(ClockProfileError::PllNotReady);
}
rcc.cfgr().modify(|read, write| {
@@ -92,14 +115,14 @@ pub fn select_pll216_profile(
)
}
});
if !wait_until(|| rcc.cfgr().read().sws().is_pll()) {
if !wait_until(CLOCK_SWITCH_POLL_LIMIT, || rcc.cfgr().read().sws().is_pll()) {
return Err(ClockProfileError::SystemClockNotPll);
}
Ok(())
}
fn wait_until(mut ready: impl FnMut() -> bool) -> bool {
for _ in 0..CLOCK_SWITCH_POLL_LIMIT {
fn wait_until(limit: u32, mut ready: impl FnMut() -> bool) -> bool {
for _ in 0..limit {
if ready() {
return true;
}
firmware/platforms/stm32f7/src/f722/peripherals/gpio.rs
+17
View File
@@ -174,6 +174,23 @@ pub fn configure_usart2_pins_pa2_pa3(port: &stm32f722::GPIOA) {
.modify(|_, write| write.pupdr2().floating().pupdr3().floating());
}
/// Configures PA11/PA12 for the STM32F722 OTG FS data pair.
pub fn configure_usb_fs_pins_pa11_pa12(rcc: &stm32f722::RCC, port: &stm32f722::GPIOA) {
rcc.ahb1enr().modify(|_, write| write.gpioaen().set_bit());
port.moder()
.modify(|_, write| write.moder11().alternate().moder12().alternate());
port.afrh().modify(|_, write| {
// SAFETY: AF10 selects OTG_FS_DM/OTG_FS_DP on PA11/PA12.
unsafe { write.afrh11().bits(0b1010).afrh12().bits(0b1010) }
});
port.otyper()
.modify(|_, write| write.ot11().push_pull().ot12().push_pull());
port.ospeedr()
.modify(|_, write| write.ospeedr11().high_speed().ospeedr12().high_speed());
port.pupdr()
.modify(|_, write| write.pupdr11().floating().pupdr12().floating());
}
/// Configures PA0/PA1 for UART4 alternate function 8.
pub fn configure_uart4_pins_pa0_pa1(port: &stm32f722::GPIOA) {
port.moder()
firmware/platforms/stm32f7/src/f722/peripherals/mod.rs
+3 -1
View File
@@ -14,6 +14,7 @@ mod time;
mod usart;
mod usart1_tx_dma2_stream7;
mod usart2_tx_dma1_stream6;
mod usb;
pub use clock::select_pll216_profile;
pub use dma_buffer::DcacheDisabledDmaMemory;
@@ -21,7 +22,7 @@ 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,
configure_usart2_pins_pa2_pa3, configure_usb_fs_pins_pa11_pa12,
};
pub use i2c1::{I2c1, I2c1Config, I2c1Error};
pub use spi1::{Spi1, Spi1Error};
@@ -31,3 +32,4 @@ 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;
pub(crate) use usb::{F722UsbMavlink, usb_mavlink};
firmware/platforms/stm32f7/src/f722/peripherals/usb.rs
+121
View File
@@ -0,0 +1,121 @@
//! STM32F722 OTG FS ownership and CDC-ACM initialization.
use stm32f7::stm32f722;
use synopsys_usb_otg::{UsbBus, UsbPeripheral};
use usb_device::{
bus::UsbBusAllocator,
device::{StringDescriptors, UsbDeviceBuilder, UsbVidPid},
};
use usbd_serial::CdcAcmClass;
use super::super::{
clock::CORE_FREQUENCY_HZ,
usb::{USB_PACKET_BYTES, UsbMavlink},
};
const USB_VID: u16 = 0x1209;
// pid.codes reserves 1209:0001 as a shared development/test identity. A
// project-specific allocated PID must replace it before product distribution.
const USB_PID: u16 = 0x0001;
// synopsys-usb-otg uses this RAM only for OUT endpoints. CDC-ACM has one
// 64-byte bulk OUT endpoint and EP0 has one 64-byte control OUT endpoint.
const ENDPOINT_MEMORY_WORDS: usize = (USB_PACKET_BYTES / 4) * 2;
const DEVICE_ID_ADDRESS: *const u32 = 0x1ff0_7a10 as *const u32;
const DEVICE_ID_BYTES: usize = 12;
const SERIAL_BYTES: usize = DEVICE_ID_BYTES * 2;
/// Exclusive owner token for the split PAC views of one OTG FS controller.
pub(crate) struct F722UsbPeripheral {
_global: stm32f722::OTG_FS_GLOBAL,
_device: stm32f722::OTG_FS_DEVICE,
_power_clock: stm32f722::OTG_FS_PWRCLK,
}
// SAFETY: the token is constructed once from PAC-owned peripherals and moved
// into one UsbBus. Runtime register access is synchronized by that bus.
unsafe impl Sync for F722UsbPeripheral {}
// SAFETY: the F722 OTG FS register layout, FIFO depth, and endpoint count are
// the values required by synopsys-usb-otg for this device peripheral.
unsafe impl UsbPeripheral for F722UsbPeripheral {
const REGISTERS: *const () = stm32f722::OTG_FS_GLOBAL::ptr() as *const ();
const HIGH_SPEED: bool = false;
const FIFO_DEPTH_WORDS: usize = 320;
const ENDPOINT_COUNT: usize = 6;
fn enable() {
cortex_m::interrupt::free(|_| {
// SAFETY: this static PAC view is used only for the USB clock/reset
// fields while the owned RCC token remains in platform init.
let rcc = unsafe { &*stm32f722::RCC::ptr() };
rcc.dckcfgr2().modify(|_, write| write.ck48msel().pll());
rcc.ahb2enr().modify(|_, write| write.otgfsen().set_bit());
rcc.ahb2rstr().modify(|_, write| write.otgfsrst().set_bit());
rcc.ahb2rstr()
.modify(|_, write| write.otgfsrst().clear_bit());
});
}
fn ahb_frequency_hz(&self) -> u32 {
CORE_FREQUENCY_HZ
}
}
type F722UsbBus = UsbBus<F722UsbPeripheral>;
type F722UsbBusAllocator = UsbBusAllocator<F722UsbBus>;
fn device_serial_number(serial: &'static mut [u8; SERIAL_BYTES]) -> Result<&'static str, ()> {
const HEX: &[u8; 16] = b"0123456789ABCDEF";
for word_index in 0..3 {
// SAFETY: STM32F722 exposes a readable 96-bit factory device identifier
// at DEVICE_ID_ADDRESS. The singleton buffer is exclusively borrowed.
let word = unsafe { core::ptr::read_volatile(DEVICE_ID_ADDRESS.add(word_index)) };
for byte_index in 0..4 {
let byte = (word >> (byte_index * 8)) as u8;
let output = (word_index * 4 + byte_index) * 2;
serial[output] = HEX[usize::from(byte >> 4)];
serial[output + 1] = HEX[usize::from(byte & 0x0f)];
}
}
core::str::from_utf8(serial).map_err(|_| ())
}
pub(crate) type F722UsbMavlink = UsbMavlink<'static, F722UsbBus>;
/// Initializes the board's singleton CDC-ACM MAVLink endpoint.
pub(crate) fn usb_mavlink(
global: stm32f722::OTG_FS_GLOBAL,
device: stm32f722::OTG_FS_DEVICE,
power_clock: stm32f722::OTG_FS_PWRCLK,
product: &'static str,
) -> Result<F722UsbMavlink, ()> {
let peripheral = F722UsbPeripheral {
_global: global,
_device: device,
_power_clock: power_clock,
};
let endpoint_memory = cortex_m::singleton!(
USB_ENDPOINT_MEMORY: [u32; ENDPOINT_MEMORY_WORDS] = [0; ENDPOINT_MEMORY_WORDS]
)
.ok_or(())?;
let allocator = UsbBus::new(peripheral, endpoint_memory);
let allocator = cortex_m::singleton!(
USB_BUS_ALLOCATOR: F722UsbBusAllocator = allocator
)
.ok_or(())?;
let serial_memory =
cortex_m::singleton!(USB_SERIAL: [u8; SERIAL_BYTES] = [0; SERIAL_BYTES]).ok_or(())?;
let serial_number = device_serial_number(serial_memory)?;
let cdc = CdcAcmClass::new(allocator, USB_PACKET_BYTES as u16);
let device = UsbDeviceBuilder::new(allocator, UsbVidPid(USB_VID, USB_PID))
.strings(&[StringDescriptors::default()
.manufacturer("Kaptan")
.product(product)
.serial_number(serial_number)])
.map_err(|_| ())?
.device_class(usbd_serial::USB_CLASS_CDC)
.max_packet_size_0(USB_PACKET_BYTES as u8)
.map_err(|_| ())?
.build();
Ok(UsbMavlink::new(device, cdc))
}
firmware/platforms/stm32f7/src/f722/usb.rs
+135
View File
@@ -0,0 +1,135 @@
//! Bounded MAVLink byte stream over CDC-ACM bulk packets.
use core::ops::Range;
use kaptan_mavlink::{
MavlinkFrame,
receiver::{IntegrityCheckedFrame, IntegrityMavlinkFramer},
};
use kaptan_tx::{PendingFrame, TransmitQueue};
use usb_device::{
UsbError,
bus::UsbBus,
device::{UsbDevice, UsbDeviceState},
};
use usbd_serial::CdcAcmClass;
pub(crate) const USB_PACKET_BYTES: usize = 64;
pub(crate) struct UsbMavlink<'a, B: UsbBus> {
device: UsbDevice<'a, B>,
cdc: CdcAcmClass<'a, B>,
transmit: TransmitQueue<MavlinkFrame, 4>,
tx_offset: usize,
framer: IntegrityMavlinkFramer,
rx_packet: [u8; USB_PACKET_BYTES],
rx_remaining: Range<usize>,
}
impl<'a, B: UsbBus> UsbMavlink<'a, B> {
pub(crate) fn new(device: UsbDevice<'a, B>, cdc: CdcAcmClass<'a, B>) -> Self {
Self {
device,
cdc,
transmit: TransmitQueue::new(),
tx_offset: 0,
framer: IntegrityMavlinkFramer::new(),
rx_packet: [0; USB_PACKET_BYTES],
rx_remaining: 0..0,
}
}
pub(crate) fn is_open(&self) -> bool {
self.device.state() == UsbDeviceState::Configured && self.cdc.dtr()
}
/// Returns whether the frame was retained, not whether the host received it.
pub(crate) fn send(&mut self, pending: PendingFrame<MavlinkFrame>) -> bool {
if !self.is_open() {
return false;
}
let retained = self.transmit.enqueue(pending).is_retained();
self.service_transmit();
retained
}
/// Services USB and buffers at most one packet for the protocol worker.
pub(crate) fn poll(&mut self) -> bool {
let previous_state = self.device.state();
let previous_dtr = self.cdc.dtr();
self.device.poll(&mut [&mut self.cdc]);
if previous_state != UsbDeviceState::Default
&& self.device.state() == UsbDeviceState::Default
{
// Only bus reset flushes endpoint FIFOs. DTR close, suspend, and
// deconfiguration pause TX without invalidating its byte cursor.
self.transmit = TransmitQueue::new();
self.tx_offset = 0;
self.reset_receive();
} else if previous_dtr && !self.cdc.dtr() {
self.reset_receive();
}
if self.rx_remaining.is_empty() && self.device.state() == UsbDeviceState::Configured {
// A previous poll may have left a packet in the bus while our
// buffer was occupied. Reading must not depend on poll's result.
match self.cdc.read_packet(&mut self.rx_packet) {
Ok(length) if self.is_open() => self.rx_remaining = 0..length,
Ok(_) | Err(UsbError::WouldBlock) => {}
Err(error) => panic!("CDC OUT endpoint invariant: {error:?}"),
}
}
self.service_transmit();
!self.rx_remaining.is_empty()
}
/// Parses buffered bytes in the worker, without a second receive queue.
pub(crate) fn take_received(&mut self) -> Option<IntegrityCheckedFrame> {
for index in self.rx_remaining.by_ref() {
if let Some(frame) = self.framer.receive_byte(self.rx_packet[index]) {
return Some(frame);
}
}
None
}
fn reset_receive(&mut self) {
self.framer.reset();
self.rx_remaining = 0..0;
}
fn service_transmit(&mut self) {
if !self.is_open() {
return;
}
let mut complete = false;
let offset = &mut self.tx_offset;
let cdc = &mut self.cdc;
let mut write = |frame: &MavlinkFrame| {
let bytes = frame.raw_bytes();
let end = (*offset + USB_PACKET_BYTES).min(bytes.len());
let packet = &bytes[*offset..end];
let written = cdc.write_packet(packet)?;
assert_eq!(written, packet.len(), "CDC IN endpoint short write");
*offset = end;
// An exact multiple of 64 needs a ZLP: retain offset == len until
// the next empty packet is accepted. The endpoint copies bytes,
// so a final short write ends its borrow of queue-owned storage.
complete = end == bytes.len() && packet.len() < USB_PACKET_BYTES;
Ok(())
};
let result = match self.transmit.active() {
Some(active) => write(&active.frame),
None => self.transmit.start_with(write).map(|_| ()),
};
match result {
Ok(()) if complete => {
self.transmit.complete();
self.tx_offset = 0;
}
Ok(()) | Err(UsbError::WouldBlock) => {}
Err(error) => panic!("CDC IN endpoint invariant: {error:?}"),
}
}
}
firmware/platforms/stm32f7/tests/platform_safety_boundary.rs
+1 -1
View File
@@ -143,7 +143,7 @@ fn ordinary_f722_image_has_direct_configuration_after_output_inhibition() {
for required in [
"config_file: F722ConfigFile",
"file_service: FileService",
"receiver: IntegrityMavlinkReceiver<1>",
"uart_receiver: IntegrityMavlinkReceiver<1>",
"file_lease_ticks: u16",
"TRANSFER_REPLAY_LEASE_SECONDS",
"let Some(message) = frame.decode()",
firmware/platforms/stm32f7/tests/usb_mavlink.rs
+434
View File
@@ -0,0 +1,434 @@
use std::{
collections::VecDeque,
sync::{Arc, Mutex},
};
use kaptan_mavlink::{
MavlinkFrame,
encoder::MavlinkEncoder,
generated::kaptan::{FILE_DATA_DATA, MavMessage},
};
use kaptan_tx::{FrameClass, PendingFrame};
use usb_device::{
UsbDirection, UsbError,
bus::{PollResult, UsbBus, UsbBusAllocator},
device::{UsbDeviceBuilder, UsbVidPid},
endpoint::{EndpointAddress, EndpointType},
};
use usbd_serial::CdcAcmClass;
#[path = "../src/f722/usb.rs"]
mod usb;
use usb::UsbMavlink;
// One pending packet per endpoint, with host-controlled completion. Device and
// CDC control requests run through the real usb-device/usbd-serial code.
#[derive(Default)]
struct BusState {
events: VecDeque<PollResult>,
input: [Option<Vec<u8>>; 4],
output: [Option<Vec<u8>>; 4],
sizes: [[u16; 4]; 2],
stalled: [[bool; 4]; 2],
bulk_in: usize,
bulk_out: usize,
}
#[derive(Clone, Default)]
struct Host(Arc<Mutex<BusState>>);
struct PacketBus(Arc<Mutex<BusState>>);
impl UsbBus for PacketBus {
fn alloc_ep(
&mut self,
direction: UsbDirection,
address: Option<EndpointAddress>,
kind: EndpointType,
size: u16,
_: u8,
) -> Result<EndpointAddress, UsbError> {
let mut state = self.0.lock().unwrap();
let sizes = &mut state.sizes[usize::from(direction == UsbDirection::In)];
let index = match address {
Some(address) => address.index(),
None => (1..sizes.len())
.find(|index| sizes[*index] == 0)
.ok_or(UsbError::EndpointOverflow)?,
};
if index >= sizes.len() || sizes[index] != 0 {
return Err(UsbError::InvalidEndpoint);
}
sizes[index] = size;
if kind == EndpointType::Bulk {
match direction {
UsbDirection::In => state.bulk_in = index,
UsbDirection::Out => state.bulk_out = index,
}
}
Ok(EndpointAddress::from_parts(index, direction))
}
fn enable(&mut self) {}
fn reset(&self) {
let mut state = self.0.lock().unwrap();
state.input.fill(None);
state.output.fill(None);
state.events.clear();
}
fn set_device_address(&self, _: u8) {}
fn write(&self, address: EndpointAddress, bytes: &[u8]) -> Result<usize, UsbError> {
let mut state = self.0.lock().unwrap();
if bytes.len() > usize::from(state.sizes[1][address.index()]) {
return Err(UsbError::BufferOverflow);
}
let packet = &mut state.input[address.index()];
if packet.is_some() {
return Err(UsbError::WouldBlock);
}
*packet = Some(bytes.to_vec());
Ok(bytes.len())
}
fn read(&self, address: EndpointAddress, bytes: &mut [u8]) -> Result<usize, UsbError> {
let mut state = self.0.lock().unwrap();
let packet = state.output[address.index()]
.as_ref()
.ok_or(UsbError::WouldBlock)?;
if packet.len() > bytes.len() {
return Err(UsbError::BufferOverflow);
}
let packet = state.output[address.index()].take().unwrap();
bytes[..packet.len()].copy_from_slice(&packet);
Ok(packet.len())
}
fn set_stalled(&self, address: EndpointAddress, stalled: bool) {
self.0.lock().unwrap().stalled[usize::from(address.is_in())][address.index()] = stalled;
}
fn is_stalled(&self, address: EndpointAddress) -> bool {
self.0.lock().unwrap().stalled[usize::from(address.is_in())][address.index()]
}
fn suspend(&self) {}
fn resume(&self) {}
fn poll(&self) -> PollResult {
self.0
.lock()
.unwrap()
.events
.pop_front()
.unwrap_or(PollResult::None)
}
}
impl Host {
fn event(&self, event: PollResult) {
self.0.lock().unwrap().events.push_back(event);
}
fn control(&self, link: &mut UsbMavlink<'_, PacketBus>, request: [u8; 8]) {
self.0.lock().unwrap().output[0] = Some(request.to_vec());
self.event(PollResult::Data {
ep_out: 0,
ep_in_complete: 0,
ep_setup: 1,
});
link.poll();
assert_eq!(self.take_input(0), Some(Vec::new()));
link.poll();
}
fn configure(&self, link: &mut UsbMavlink<'_, PacketBus>, configured: bool) {
self.control(link, [0, 9, u8::from(configured), 0, 0, 0, 0, 0]);
}
fn dtr(&self, link: &mut UsbMavlink<'_, PacketBus>, open: bool) {
self.control(link, [0x21, 0x22, u8::from(open), 0, 0, 0, 0, 0]);
}
fn write_packet(&self, bytes: &[u8]) {
assert!(bytes.len() <= 64);
let mut state = self.0.lock().unwrap();
let endpoint = state.bulk_out;
assert!(state.output[endpoint].is_none());
state.output[endpoint] = Some(bytes.to_vec());
state.events.push_back(PollResult::Data {
ep_out: 1 << endpoint,
ep_in_complete: 0,
ep_setup: 0,
});
}
fn take_input(&self, endpoint: usize) -> Option<Vec<u8>> {
let mut state = self.0.lock().unwrap();
let packet = state.input[endpoint].take()?;
state.events.push_back(PollResult::Data {
ep_out: 0,
ep_in_complete: 1 << endpoint,
ep_setup: 0,
});
Some(packet)
}
fn collect_packets(&self, link: &mut UsbMavlink<'_, PacketBus>) -> Vec<Vec<u8>> {
let endpoint = self.0.lock().unwrap().bulk_in;
let mut packets = Vec::new();
for _ in 0..16 {
let Some(packet) = self.take_input(endpoint) else {
return packets;
};
packets.push(packet);
link.poll();
}
panic!("transmission did not finish");
}
}
fn with_link(test: impl FnOnce(&Host, &mut UsbMavlink<'_, PacketBus>)) {
let host = Host::default();
let allocator = UsbBusAllocator::new(PacketBus(host.0.clone()));
let class = CdcAcmClass::new(&allocator, 64);
let device = UsbDeviceBuilder::new(&allocator, UsbVidPid(0x1209, 1))
.max_packet_size_0(64)
.unwrap()
.build();
let mut link = UsbMavlink::new(device, class);
host.event(PollResult::Reset);
link.poll();
host.configure(&mut link, true);
host.dtr(&mut link, true);
assert!(link.is_open());
test(&host, &mut link);
}
fn pending(frame: MavlinkFrame) -> PendingFrame<MavlinkFrame> {
PendingFrame {
class: FrameClass::Critical,
frame,
}
}
fn file_frame(length: usize) -> MavlinkFrame {
// FILE_DATA has nine scalar bytes before its trailing data array. Setting
// the final data byte controls MAVLink 2's trailing-zero truncation.
let mut data = FILE_DATA_DATA::DEFAULT;
data.data[..length - 12 - 9].fill(0x5a);
let frame = MavlinkEncoder::new(1, 1).encode(&data);
assert_eq!(frame.raw_bytes().len(), length);
frame
}
#[test]
fn receives_split_frame_followed_by_coalesced_frames() {
with_link(|host, link| {
// HEARTBEAT ID 51027, source 255:1, sequence 0, empty payload,
// CRC-16/MCRF4XX with CRC_EXTRA 161. Integrity admission precedes payload
// validation, so these 12-byte frames must also survive USB batching.
let mut minimum = MavlinkFrame::new();
minimum.as_mut_slice()[..12]
.copy_from_slice(&[0xfd, 0, 0, 0, 0, 255, 1, 0x53, 0xc7, 0, 0x2e, 0xa8]);
assert!(minimum.has_valid_crc::<MavMessage>());
let mut frames = [minimum; 6];
frames[0] = MavlinkEncoder::new(255, 1).heartbeat();
let split = frames[0].raw_bytes().len() - 1;
host.write_packet(&frames[0].raw_bytes()[..split]);
assert!(link.poll());
assert!(link.take_received().is_none());
// One carried-over CRC byte and five complete frames: six completions
// in 61 bytes, exceeding the former five-frame receive queue.
let mut packet = frames[0].raw_bytes()[split..].to_vec();
for frame in &frames[1..] {
packet.extend_from_slice(frame.raw_bytes());
}
host.write_packet(&packet);
assert!(link.poll());
for frame in &frames {
assert_eq!(link.take_received().unwrap().raw(), frame);
}
assert!(link.take_received().is_none());
});
}
#[test]
fn drains_endpoint_backlog_after_its_poll_event_was_consumed() {
with_link(|host, link| {
let mut encoder = MavlinkEncoder::new(255, 1);
let first = encoder.heartbeat();
let second = encoder.heartbeat();
host.write_packet(first.raw_bytes());
assert!(link.poll());
host.write_packet(second.raw_bytes());
assert!(link.poll());
assert_eq!(link.take_received().unwrap().raw(), &first);
assert!(link.take_received().is_none());
assert!(host.0.lock().unwrap().events.is_empty());
assert!(link.poll());
assert_eq!(link.take_received().unwrap().raw(), &second);
});
}
#[test]
fn endpoint_backpressure_neither_repeats_nor_interleaves_frame_bytes() {
with_link(|host, link| {
let frame = file_frame(130);
let next = MavlinkEncoder::new(1, 1).heartbeat();
assert!(link.send(pending(frame)));
assert!(link.send(pending(next)));
for _ in 0..4 {
link.poll();
}
let packets = host.collect_packets(link);
assert_eq!(
packets.iter().map(Vec::len).collect::<Vec<_>>(),
[64, 64, 2, 13]
);
assert_eq!(
packets.concat(),
[frame.raw_bytes(), next.raw_bytes()].concat()
);
});
}
#[test]
fn full_size_final_packet_is_followed_by_one_zlp() {
for length in [64, 128] {
with_link(|host, link| {
let frame = file_frame(length);
let next = MavlinkEncoder::new(1, 1).heartbeat();
assert!(link.send(pending(frame)));
assert!(link.send(pending(next)));
let packets = host.collect_packets(link);
let mut lengths = vec![64; length / 64];
lengths.extend([0, 13]);
assert_eq!(packets.iter().map(Vec::len).collect::<Vec<_>>(), lengths);
assert_eq!(
packets.concat(),
[frame.raw_bytes(), next.raw_bytes()].concat()
);
});
}
}
#[test]
fn bounded_admission_preserves_reserved_frame_and_replaces_only_pending_frames() {
with_link(|host, link| {
let mut encoder = MavlinkEncoder::new(1, 1);
let first = encoder.heartbeat();
let reserved = encoder.heartbeat();
assert!(link.send(pending(first)));
// The endpoint still owns the first packet; this start must retry.
assert!(link.send(pending(reserved)));
for _ in 0..4 {
assert!(link.send(PendingFrame {
class: FrameClass::BestEffort,
frame: encoder.heartbeat(),
}));
}
let critical = std::array::from_fn::<_, 4, _>(|_| encoder.heartbeat());
for frame in critical {
assert!(link.send(pending(frame)));
}
assert!(!link.send(pending(encoder.heartbeat())));
let vital = encoder.heartbeat();
assert!(link.send(PendingFrame {
class: FrameClass::Vital,
frame: vital,
}));
let packets = host.collect_packets(link);
assert_eq!(packets.len(), 6);
assert_eq!(packets[0], first.raw_bytes());
assert_eq!(packets[1], reserved.raw_bytes());
assert_eq!(packets[2], vital.raw_bytes());
let mut pending = critical.map(|frame| frame.raw_bytes().to_vec()).to_vec();
for packet in &packets[3..] {
let index = pending.iter().position(|frame| frame == packet).unwrap();
pending.remove(index);
}
});
}
#[test]
fn dtr_suspend_and_deconfiguration_preserve_unfinished_transmission() {
enum Pause {
Dtr,
Suspend,
Deconfigure,
}
for pause in [Pause::Dtr, Pause::Suspend, Pause::Deconfigure] {
with_link(|host, link| {
let frame = file_frame(130);
assert!(link.send(pending(frame)));
match pause {
Pause::Dtr => host.dtr(link, false),
Pause::Suspend => {
host.event(PollResult::Suspend);
link.poll();
}
Pause::Deconfigure => host.configure(link, false),
}
assert!(!link.is_open());
assert!(!link.send(pending(MavlinkEncoder::new(1, 1).heartbeat())));
match pause {
Pause::Dtr => host.dtr(link, true),
Pause::Suspend => {
host.event(PollResult::Resume);
link.poll();
}
Pause::Deconfigure => host.configure(link, true),
}
assert!(link.is_open());
assert_eq!(host.collect_packets(link).concat(), frame.raw_bytes());
});
}
}
#[test]
fn dtr_close_discards_partial_receive_frame() {
with_link(|host, link| {
let mut encoder = MavlinkEncoder::new(255, 1);
let interrupted = encoder.heartbeat();
let next = encoder.heartbeat();
host.write_packet(&interrupted.raw_bytes()[..8]);
link.poll();
assert!(link.take_received().is_none());
host.dtr(link, false);
host.dtr(link, true);
host.write_packet(next.raw_bytes());
link.poll();
assert_eq!(link.take_received().unwrap().raw(), &next);
assert!(link.take_received().is_none());
});
}
#[test]
fn bus_reset_discards_transmit_and_receive_state() {
with_link(|host, link| {
let frame = file_frame(130);
assert!(link.send(pending(frame)));
host.write_packet(&frame.raw_bytes()[..8]);
link.poll();
assert!(link.take_received().is_none());
host.event(PollResult::Reset);
link.poll();
assert!(!link.is_open());
host.configure(link, true);
host.dtr(link, true);
assert!(host.collect_packets(link).is_empty());
let next = MavlinkEncoder::new(1, 1).heartbeat();
assert!(link.send(pending(next)));
assert_eq!(host.collect_packets(link).concat(), next.raw_bytes());
host.write_packet(next.raw_bytes());
link.poll();
assert_eq!(link.take_received().unwrap().raw(), &next);
});
}