feat(f722): enforce board policy and emit diagnostics
This commit is contained in:
1 parent
a4ef0f0740
commit
f4f47d7302
16 files changed
+580
-179
No files matched your search
@@ -24,6 +24,8 @@ 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-log = { path = "../../libs/kaptan-log" }
|
||||
kaptan-log-mavlink = { path = "../../libs/kaptan-log-mavlink" }
|
||||
kaptan-mavlink = { path = "../../libs/kaptan-mavlink" }
|
||||
kaptan-sensor = { path = "../../libs/kaptan-sensor" }
|
||||
kaptan-spi = { path = "../../libs/kaptan-spi" }
|
||||
|
||||
@@ -17,6 +17,11 @@ use kaptan_drivers::{
|
||||
lidar_lite_v3::{AcquisitionMode, LidarLiteV3, LidarLiteV3Service, LidarLiteV3ServiceEvent},
|
||||
mtf01::Mtf01Service,
|
||||
};
|
||||
use kaptan_log::{
|
||||
DeliveryClass, ErrorReport, EventHub, KaptanError, LogFilter, LogFlags, StaticErrorMailbox,
|
||||
StaticErrorMessage, TransportAdmission,
|
||||
};
|
||||
use kaptan_log_mavlink::{DiagnosticPayload, encode_diagnostic};
|
||||
use kaptan_spi::SpiCompletion;
|
||||
use stm32f7::stm32f722;
|
||||
|
||||
@@ -37,7 +42,47 @@ 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 ERROR_RECORDS_PER_SECOND: u8 = 10;
|
||||
const LOG_RECORDS_PER_SECOND: u8 = 5;
|
||||
const SCHEDULED_FRAMES_PER_SECOND: usize = 62;
|
||||
const SCHEDULED_FRAME_MAXIMUM_BYTES: usize = 48;
|
||||
const MAVLINK_V2_UNSIGNED_OVERHEAD_BYTES: usize = 12;
|
||||
const KAPTAN_ERROR_FRAME_MAXIMUM_BYTES: usize =
|
||||
kaptan_mavlink::message::kaptan::KAPTAN_ERROR_DATA::ENCODED_LEN
|
||||
+ MAVLINK_V2_UNSIGNED_OVERHEAD_BYTES;
|
||||
const KAPTAN_LOG_FRAME_MAXIMUM_BYTES: usize =
|
||||
kaptan_mavlink::message::kaptan::KAPTAN_LOG_DATA::ENCODED_LEN
|
||||
+ MAVLINK_V2_UNSIGNED_OVERHEAD_BYTES;
|
||||
// The static F722 telemetry profile is bounded at 62 scheduled frames/s.
|
||||
// It permits ten `KAPTAN_ERROR` and five `KAPTAN_LOG` frames/s. Their maxima
|
||||
// are respectively 162 and 163 payload bytes plus 12 unsigned MAVLink V2
|
||||
// framing bytes, so 62×48 + 10×174 + 5×175 = 5,591 bytes/s. MAVLink signing
|
||||
// is not enabled in this telemetry-only image; any future signing profile
|
||||
// must re-budget its signature overhead before it can enable the endpoint.
|
||||
pub(crate) const WORST_CASE_OBSERVABILITY_BYTES_PER_SECOND: usize = SCHEDULED_FRAMES_PER_SECOND
|
||||
* SCHEDULED_FRAME_MAXIMUM_BYTES
|
||||
+ ERROR_RECORDS_PER_SECOND as usize * KAPTAN_ERROR_FRAME_MAXIMUM_BYTES
|
||||
+ LOG_RECORDS_PER_SECOND as usize * KAPTAN_LOG_FRAME_MAXIMUM_BYTES;
|
||||
|
||||
/// Returns whether the immutable board UART configuration can carry the fixed
|
||||
/// telemetry and bounded diagnostic schedule at ten UART bits per byte.
|
||||
pub(crate) const fn link_budget_fits(baud: u32) -> bool {
|
||||
WORST_CASE_OBSERVABILITY_BYTES_PER_SECOND < baud as usize / 10
|
||||
}
|
||||
static UART_COMPLETION: AtomicU8 = AtomicU8::new(0);
|
||||
#[derive(Clone, Copy, Debug, KaptanError)]
|
||||
enum F722TelemetryError {
|
||||
Mtf01Uart,
|
||||
TransmitMemory,
|
||||
TransmitEngine,
|
||||
TransmitTerminal,
|
||||
}
|
||||
// High-priority/ISR-adjacent producers never encode, lock, allocate, or send
|
||||
// MAVLink. They retain the first fault text and coalesce later reports for
|
||||
// the low-priority MAVLink worker to publish.
|
||||
static IMU_FAULTS: StaticErrorMailbox = StaticErrorMailbox::new();
|
||||
static LIDAR_FAULTS: StaticErrorMailbox = StaticErrorMailbox::new();
|
||||
static MTF01_FAULTS: StaticErrorMailbox = StaticErrorMailbox::new();
|
||||
static SAMPLES: LatestImuMailbox = LatestImuMailbox::new();
|
||||
static RANGES: LatestRangeMailbox = LatestRangeMailbox::new();
|
||||
static OPTICAL_FLOW: LatestOpticalFlowMailbox = LatestOpticalFlowMailbox::new();
|
||||
@@ -56,6 +101,10 @@ pub type F722ServiceTimer = Tim2Periodic;
|
||||
/// rather than the IMU or vertical-sensor IRQ ceiling.
|
||||
pub struct F722MavlinkState {
|
||||
data_channel: DataChannel<'static, F722MavlinkTransmitter, SERVICE_HZ>,
|
||||
logs: EventHub<2, 4>,
|
||||
budget_ticks: u8,
|
||||
error_budget: u8,
|
||||
log_budget: u8,
|
||||
led: F722StatusLed,
|
||||
}
|
||||
|
||||
@@ -83,7 +132,7 @@ pub struct F722InitResources {
|
||||
|
||||
/// Validates the relationship between portable metadata and selected sensor
|
||||
/// route before platform initialization claims a peripheral.
|
||||
pub(crate) fn validate_vertical_route(
|
||||
pub(crate) fn validate_board_routes(
|
||||
config: BoardConfig,
|
||||
routes: F722BoardRoutes,
|
||||
) -> Result<(), BoardError> {
|
||||
@@ -99,6 +148,18 @@ pub(crate) fn validate_vertical_route(
|
||||
if telemetry.vertical_sensing != expected {
|
||||
return Err(BoardError::VerticalSensorCapabilityMismatch);
|
||||
}
|
||||
if telemetry.imu != routes.imu() {
|
||||
return Err(BoardError::ImuRouteMismatch);
|
||||
}
|
||||
if telemetry.imu_mount != routes.imu_mount() {
|
||||
return Err(BoardError::ImuMountMismatch);
|
||||
}
|
||||
if config.mavlink.port != routes.mavlink_port() {
|
||||
return Err(BoardError::MavlinkRouteMismatch);
|
||||
}
|
||||
if !link_budget_fits(config.mavlink.baud) {
|
||||
return Err(BoardError::TelemetryLinkOvercommitted);
|
||||
}
|
||||
Ok(())
|
||||
}
|
||||
|
||||
@@ -145,15 +206,18 @@ fn initialize_mpu6000(
|
||||
clock.fork(),
|
||||
vertical,
|
||||
)?;
|
||||
let telemetry = config.telemetry().ok_or(BoardError::Initialization)?;
|
||||
let imu = F722Imu::mpu6000(
|
||||
&device.RCC,
|
||||
device.SPI1,
|
||||
device.DMA2,
|
||||
device.GPIOB,
|
||||
clock.fork(),
|
||||
telemetry.imu_mount,
|
||||
);
|
||||
let transmitter = F722MavlinkTransmitter::usart1(&device.RCC, device.USART1)
|
||||
.map_err(|_| BoardError::Initialization)?;
|
||||
let transmitter =
|
||||
F722MavlinkTransmitter::usart1(&device.RCC, device.USART1, config.mavlink.baud)
|
||||
.map_err(|_| BoardError::Initialization)?;
|
||||
let led = F722StatusLed::Kakute(super::peripherals::ActiveLowLedPd2::new(
|
||||
&device.RCC,
|
||||
device.GPIOD,
|
||||
@@ -199,15 +263,18 @@ fn initialize_icm42688(
|
||||
clock.fork(),
|
||||
vertical,
|
||||
)?;
|
||||
let telemetry = config.telemetry().ok_or(BoardError::Initialization)?;
|
||||
let imu = F722Imu::icm42688(
|
||||
&device.RCC,
|
||||
device.SPI3,
|
||||
device.DMA1,
|
||||
&device.GPIOA,
|
||||
clock.fork(),
|
||||
telemetry.imu_mount,
|
||||
);
|
||||
let transmitter = F722MavlinkTransmitter::usart2(&device.RCC, device.USART2)
|
||||
.map_err(|_| BoardError::Initialization)?;
|
||||
let transmitter =
|
||||
F722MavlinkTransmitter::usart2(&device.RCC, device.USART2, config.mavlink.baud)
|
||||
.map_err(|_| BoardError::Initialization)?;
|
||||
let led = F722StatusLed::Geprc(GeprcStatusLed::new(&device.RCC, device.GPIOA));
|
||||
finish(
|
||||
&device.RCC,
|
||||
@@ -256,7 +323,19 @@ fn finish(
|
||||
range,
|
||||
startup_ticks: 0,
|
||||
},
|
||||
mavlink: F722MavlinkState { data_channel, led },
|
||||
mavlink: F722MavlinkState {
|
||||
data_channel,
|
||||
logs: EventHub::new(LogFilter::new(
|
||||
config
|
||||
.telemetry()
|
||||
.ok_or(BoardError::Initialization)?
|
||||
.maximum_log_level,
|
||||
)),
|
||||
budget_ticks: 0,
|
||||
error_budget: ERROR_RECORDS_PER_SECOND,
|
||||
log_budget: LOG_RECORDS_PER_SECOND,
|
||||
led,
|
||||
},
|
||||
})
|
||||
}
|
||||
|
||||
@@ -346,7 +425,10 @@ pub(crate) fn on_uart4_interrupt(mtf01: &mut Option<F722Mtf01>) {
|
||||
}
|
||||
}
|
||||
}
|
||||
ReceiveInterrupt::Fault => mtf01.reset(),
|
||||
ReceiveInterrupt::Fault => {
|
||||
mtf01.reset();
|
||||
record_mtf01_fault(F722TelemetryError::Mtf01Uart.error_message());
|
||||
}
|
||||
ReceiveInterrupt::NotPending => {}
|
||||
}
|
||||
}
|
||||
@@ -370,8 +452,10 @@ impl F722LidarState {
|
||||
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();
|
||||
if self.startup_ticks == LIDAR_STARTUP_TICKS
|
||||
&& let Err(error) = range.finish_startup()
|
||||
{
|
||||
record_lidar_fault(error.error_message());
|
||||
}
|
||||
} else {
|
||||
publish_lidar_event(range.on_periodic_tick());
|
||||
@@ -410,7 +494,7 @@ fn accept_imu_event(event: ImuEvent) {
|
||||
ImuEvent::Sample(sample) => {
|
||||
SAMPLES.publish(sample);
|
||||
}
|
||||
ImuEvent::Fault => {}
|
||||
ImuEvent::Fault(message) => record_imu_fault(message),
|
||||
ImuEvent::Idle | ImuEvent::Initialized => {}
|
||||
}
|
||||
}
|
||||
@@ -423,7 +507,7 @@ impl F722MavlinkState {
|
||||
|
||||
if events & DMA != 0 {
|
||||
let completion = take_mavlink_completion();
|
||||
self.data_channel.on_dma_complete(completion);
|
||||
let _ = self.data_channel.on_dma_complete(completion);
|
||||
if completion == kaptan_dma::TxCompletion::Complete {
|
||||
self.led.toggle();
|
||||
}
|
||||
@@ -436,6 +520,94 @@ impl F722MavlinkState {
|
||||
// would incorrectly claim initialization never completed.
|
||||
self.data_channel
|
||||
.on_service_tick(kaptan_mavlink::message::MavState::MAV_STATE_STANDBY, false);
|
||||
self.drain_driver_faults();
|
||||
self.budget_ticks = self.budget_ticks.saturating_add(1);
|
||||
if self.budget_ticks == SERVICE_HZ as u8 {
|
||||
self.budget_ticks = 0;
|
||||
self.error_budget = ERROR_RECORDS_PER_SECOND;
|
||||
self.log_budget = LOG_RECORDS_PER_SECOND;
|
||||
}
|
||||
self.drain_one_log_record();
|
||||
}
|
||||
}
|
||||
|
||||
fn drain_driver_faults(&mut self) {
|
||||
self.drain_fault_mailbox(&IMU_FAULTS);
|
||||
self.drain_fault_mailbox(&LIDAR_FAULTS);
|
||||
self.drain_fault_mailbox(&MTF01_FAULTS);
|
||||
self.drain_transport_faults();
|
||||
}
|
||||
|
||||
fn drain_transport_faults(&mut self) {
|
||||
while let Some(fault) = self.data_channel.take_transport_fault() {
|
||||
let message = match fault {
|
||||
kaptan_mavlink::transmitter::TransmitFaultKind::Memory => {
|
||||
F722TelemetryError::TransmitMemory.error_message()
|
||||
}
|
||||
kaptan_mavlink::transmitter::TransmitFaultKind::Engine => {
|
||||
F722TelemetryError::TransmitEngine.error_message()
|
||||
}
|
||||
kaptan_mavlink::transmitter::TransmitFaultKind::Terminal => {
|
||||
F722TelemetryError::TransmitTerminal.error_message()
|
||||
}
|
||||
};
|
||||
let _ = self.logs.publish_error_static(LogFlags::NONE, message);
|
||||
}
|
||||
}
|
||||
|
||||
fn drain_fault_mailbox(&mut self, mailbox: &StaticErrorMailbox) {
|
||||
let (report, coalesced) = mailbox.take();
|
||||
let Some(message) = report else {
|
||||
return;
|
||||
};
|
||||
let flags = if coalesced {
|
||||
LogFlags::COALESCED
|
||||
} else {
|
||||
LogFlags::NONE
|
||||
};
|
||||
let _ = self.logs.publish_error_static(flags, message);
|
||||
}
|
||||
|
||||
fn drain_one_log_record(&mut self) {
|
||||
let Some(record) = self.logs.peek() else {
|
||||
return;
|
||||
};
|
||||
let delivery = record.delivery();
|
||||
if (delivery == DeliveryClass::Critical && self.error_budget == 0)
|
||||
|| (delivery == DeliveryClass::BestEffort && self.log_budget == 0)
|
||||
{
|
||||
return;
|
||||
}
|
||||
let data_channel = &mut self.data_channel;
|
||||
let mut accepted = None;
|
||||
let _ = self.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_diagnostic(record);
|
||||
let admission = match &payload {
|
||||
DiagnosticPayload::Log(payload) => data_channel.send_custom(class, payload),
|
||||
DiagnosticPayload::Error(payload) => data_channel.send_custom(class, payload),
|
||||
};
|
||||
let outcome = if admission.is_retained() {
|
||||
accepted = Some(record.delivery());
|
||||
TransportAdmission::Accepted
|
||||
} else if record.delivery() == DeliveryClass::BestEffort {
|
||||
TransportAdmission::DropBestEffort
|
||||
} else {
|
||||
TransportAdmission::Retry
|
||||
};
|
||||
(outcome, ())
|
||||
});
|
||||
match accepted {
|
||||
Some(DeliveryClass::Critical) => {
|
||||
self.error_budget = self.error_budget.saturating_sub(1);
|
||||
}
|
||||
Some(DeliveryClass::BestEffort) => {
|
||||
self.log_budget = self.log_budget.saturating_sub(1);
|
||||
}
|
||||
None => {}
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -459,49 +631,22 @@ pub fn record_mavlink_completion(completion: kaptan_dma::TxCompletion) {
|
||||
}
|
||||
}
|
||||
|
||||
fn publish_lidar_event<E>(event: LidarLiteV3ServiceEvent<E>) {
|
||||
if let LidarLiteV3ServiceEvent::Measurement(snapshot) = event {
|
||||
RANGES.publish(snapshot);
|
||||
fn publish_lidar_event<E: ErrorReport>(event: LidarLiteV3ServiceEvent<E>) {
|
||||
match event {
|
||||
LidarLiteV3ServiceEvent::Measurement(snapshot) => RANGES.publish(snapshot),
|
||||
LidarLiteV3ServiceEvent::Fault(error) => record_lidar_fault(error.error_message()),
|
||||
LidarLiteV3ServiceEvent::Idle => {}
|
||||
}
|
||||
}
|
||||
|
||||
#[cfg(test)]
|
||||
mod tests {
|
||||
use super::validate_vertical_route;
|
||||
use crate::{F7BoardBuilder, Mavlink, Mpu6000, Pa4, VerticalSensor};
|
||||
use kaptan_board::{
|
||||
BoardConfig, BoardProfile, DataChannelRates, MavlinkEndpoint, TelemetryProfile,
|
||||
VerticalSensingKind,
|
||||
};
|
||||
fn record_imu_fault(message: &'static StaticErrorMessage) {
|
||||
IMU_FAULTS.record(message);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn rejects_metadata_that_claims_flow_for_a_lidar_route() {
|
||||
let routes = F7BoardBuilder::new()
|
||||
.imu(Mpu6000::on_spi1().with_drdy(Pa4))
|
||||
.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,
|
||||
},
|
||||
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),
|
||||
Err(crate::BoardError::VerticalSensorCapabilityMismatch)
|
||||
);
|
||||
}
|
||||
fn record_lidar_fault(message: &'static StaticErrorMessage) {
|
||||
LIDAR_FAULTS.record(message);
|
||||
}
|
||||
|
||||
fn record_mtf01_fault(message: &'static StaticErrorMessage) {
|
||||
MTF01_FAULTS.record(message);
|
||||
}
|
||||
@@ -1,12 +1,14 @@
|
||||
//! STM32F722 Pll216 clock-source assembly.
|
||||
|
||||
use super::peripherals::{DwtMicrosecondClock, select_pll216_profile};
|
||||
use kaptan_log::KaptanError;
|
||||
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.
|
||||
#[derive(Clone, Copy, Debug, Eq, PartialEq, KaptanError)]
|
||||
pub(crate) enum ClockInitializationError {
|
||||
Profile,
|
||||
Monotonic,
|
||||
|
||||
@@ -1,22 +1,19 @@
|
||||
//! F722 transport and vertical-route adapters for the portable data channel.
|
||||
|
||||
use super::{VerticalRoute, peripherals};
|
||||
use kaptan_data_channel::{FrameSink, VerticalDataConfig};
|
||||
use kaptan_data_channel::{DmaCompletion, FrameAdmission, FrameSink, TxHealth, VerticalDataConfig};
|
||||
use kaptan_dma::TxCompletion;
|
||||
use kaptan_mavlink::{
|
||||
MavlinkFrame,
|
||||
encoder::DistanceSensorConfig,
|
||||
transmitter::{FrameClass, MavlinkTransmitter},
|
||||
transmitter::{FrameClass, MavlinkTransmitter, TransmitFaultKind},
|
||||
};
|
||||
|
||||
// TODO(configuration): transport baud rate, sensor orientation/range, and
|
||||
// freshness are currently development profile constants. A ground-station
|
||||
// configuration flow must stage them with sensor calibration and link
|
||||
// authority, apply them only with output inhibited, then reset estimator,
|
||||
// controller, allocator, and safety prearm state as one revision.
|
||||
// Board endpoint, baud, sensor topology, and mounting remain immutable
|
||||
// build-time policy. They are not ground-station-editable in this telemetry
|
||||
// image: the F722 has no authenticated config ingress or activation runtime.
|
||||
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.
|
||||
@@ -60,65 +57,154 @@ pub(crate) fn vertical_data_config(route: VerticalRoute) -> VerticalDataConfig {
|
||||
}
|
||||
|
||||
/// The two F722 DMA engines behind the portable data-channel transport.
|
||||
pub(crate) enum F722MavlinkTransmitter {
|
||||
enum F722MavlinkTransport {
|
||||
Usart1(MavlinkTransmitter<peripherals::DcacheDisabledDmaMemory, peripherals::Usart1TxDma, 4>),
|
||||
Usart2(MavlinkTransmitter<peripherals::DcacheDisabledDmaMemory, peripherals::Usart2TxDma, 4>),
|
||||
}
|
||||
|
||||
/// Sole F722 MAVLink DMA owner plus unconfigurable transport-loss accounting.
|
||||
pub(crate) struct F722MavlinkTransmitter {
|
||||
inner: F722MavlinkTransport,
|
||||
health: TxHealth,
|
||||
pending_faults: u8,
|
||||
}
|
||||
|
||||
impl F722MavlinkTransmitter {
|
||||
pub(crate) fn usart1(
|
||||
rcc: &stm32f7::stm32f722::RCC,
|
||||
uart: stm32f7::stm32f722::USART1,
|
||||
baud: u32,
|
||||
) -> Result<Self, ()> {
|
||||
let uart =
|
||||
peripherals::Usart1Tx::new(rcc, uart, UART1_CLOCK_HZ, MAVLINK_BAUD).map_err(|_| ())?;
|
||||
let uart = peripherals::Usart1Tx::new(rcc, uart, UART1_CLOCK_HZ, 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),
|
||||
)))
|
||||
Ok(Self {
|
||||
inner: F722MavlinkTransport::Usart1(MavlinkTransmitter::new(
|
||||
memory,
|
||||
peripherals::Usart1TxDma::new(rcc, uart),
|
||||
)),
|
||||
health: TxHealth::default(),
|
||||
pending_faults: 0,
|
||||
})
|
||||
}
|
||||
|
||||
pub(crate) fn usart2(
|
||||
rcc: &stm32f7::stm32f722::RCC,
|
||||
uart: stm32f7::stm32f722::USART2,
|
||||
baud: u32,
|
||||
) -> Result<Self, ()> {
|
||||
let uart =
|
||||
peripherals::Usart2Tx::new(rcc, uart, UART2_CLOCK_HZ, MAVLINK_BAUD).map_err(|_| ())?;
|
||||
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::Usart2(MavlinkTransmitter::new(
|
||||
memory,
|
||||
peripherals::Usart2TxDma::new(rcc, uart),
|
||||
)))
|
||||
Ok(Self {
|
||||
inner: F722MavlinkTransport::Usart2(MavlinkTransmitter::new(
|
||||
memory,
|
||||
peripherals::Usart2TxDma::new(rcc, uart),
|
||||
)),
|
||||
health: TxHealth::default(),
|
||||
pending_faults: 0,
|
||||
})
|
||||
}
|
||||
}
|
||||
|
||||
impl FrameSink for F722MavlinkTransmitter {
|
||||
fn send(&mut self, class: FrameClass, frame: MavlinkFrame) {
|
||||
match self {
|
||||
Self::Usart1(transmitter) => {
|
||||
let _ = transmitter.send_frame(class, frame);
|
||||
fn send(&mut self, class: FrameClass, frame: MavlinkFrame) -> FrameAdmission {
|
||||
let (admission, start_fault) = match &mut self.inner {
|
||||
F722MavlinkTransport::Usart1(transmitter) => {
|
||||
let outcome = transmitter.admit_and_start(class, frame);
|
||||
(
|
||||
outcome.admission,
|
||||
outcome.start.err().map(|fault| fault.kind()),
|
||||
)
|
||||
}
|
||||
Self::Usart2(transmitter) => {
|
||||
let _ = transmitter.send_frame(class, frame);
|
||||
F722MavlinkTransport::Usart2(transmitter) => {
|
||||
let outcome = transmitter.admit_and_start(class, frame);
|
||||
(
|
||||
outcome.admission,
|
||||
outcome.start.err().map(|fault| fault.kind()),
|
||||
)
|
||||
}
|
||||
};
|
||||
if !matches!(
|
||||
admission,
|
||||
kaptan_mavlink::transmitter::EnqueueResult::Enqueued
|
||||
) {
|
||||
self.health.admission_loss_count = self.health.admission_loss_count.saturating_add(1);
|
||||
}
|
||||
if let Some(fault) = start_fault {
|
||||
self.record_fault(fault);
|
||||
}
|
||||
FrameAdmission::Retained(admission)
|
||||
}
|
||||
|
||||
fn on_dma_complete(&mut self, completion: TxCompletion) -> DmaCompletion {
|
||||
let start_fault = match &mut self.inner {
|
||||
F722MavlinkTransport::Usart1(transmitter) => transmitter
|
||||
.on_dma_complete(completion)
|
||||
.err()
|
||||
.map(|error| error.kind()),
|
||||
F722MavlinkTransport::Usart2(transmitter) => transmitter
|
||||
.on_dma_complete(completion)
|
||||
.err()
|
||||
.map(|error| error.kind()),
|
||||
};
|
||||
let terminal = matches!(
|
||||
completion,
|
||||
TxCompletion::TransferFault | TxCompletion::Halted
|
||||
);
|
||||
if let Some(fault) = start_fault {
|
||||
self.record_fault(fault);
|
||||
}
|
||||
if terminal {
|
||||
self.record_fault(TransmitFaultKind::Terminal);
|
||||
}
|
||||
if start_fault.is_some() || terminal {
|
||||
DmaCompletion::Fault
|
||||
} else {
|
||||
DmaCompletion::Complete
|
||||
}
|
||||
}
|
||||
|
||||
fn on_dma_complete(&mut self, completion: TxCompletion) {
|
||||
match self {
|
||||
Self::Usart1(transmitter) => {
|
||||
let _ = transmitter.on_dma_complete(completion);
|
||||
fn health(&self) -> TxHealth {
|
||||
self.health
|
||||
}
|
||||
|
||||
fn take_transport_fault(&mut self) -> Option<TransmitFaultKind> {
|
||||
let (bit, fault) = if self.pending_faults & 1 != 0 {
|
||||
(1, TransmitFaultKind::Memory)
|
||||
} else if self.pending_faults & 2 != 0 {
|
||||
(2, TransmitFaultKind::Engine)
|
||||
} else if self.pending_faults & 4 != 0 {
|
||||
(4, TransmitFaultKind::Terminal)
|
||||
} else {
|
||||
return None;
|
||||
};
|
||||
self.pending_faults &= !bit;
|
||||
Some(fault)
|
||||
}
|
||||
}
|
||||
|
||||
impl F722MavlinkTransmitter {
|
||||
fn record_fault(&mut self, fault: TransmitFaultKind) {
|
||||
self.health.dma_fault_count = self.health.dma_fault_count.saturating_add(1);
|
||||
self.pending_faults |= match fault {
|
||||
TransmitFaultKind::Memory => {
|
||||
self.health.memory_fault_count = self.health.memory_fault_count.saturating_add(1);
|
||||
1
|
||||
}
|
||||
Self::Usart2(transmitter) => {
|
||||
let _ = transmitter.on_dma_complete(completion);
|
||||
TransmitFaultKind::Engine => {
|
||||
self.health.engine_fault_count = self.health.engine_fault_count.saturating_add(1);
|
||||
2
|
||||
}
|
||||
}
|
||||
TransmitFaultKind::Terminal => {
|
||||
self.health.terminal_fault_count =
|
||||
self.health.terminal_fault_count.saturating_add(1);
|
||||
4
|
||||
}
|
||||
};
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@@ -46,6 +46,7 @@ pub(crate) enum VerticalRoute {
|
||||
pub(crate) use board::{
|
||||
F722InitResources, F722LidarState, F722MavlinkState, F722Mtf01, F722Mtf01Uart,
|
||||
F722ServiceTimer, on_uart4_interrupt, process_imu, record_mavlink_completion,
|
||||
validate_board_routes,
|
||||
};
|
||||
pub(crate) use interrupts::{
|
||||
acknowledge_exti3, acknowledge_exti4, acknowledge_imu_rx_dma1, acknowledge_imu_rx_dma2,
|
||||
@@ -68,8 +69,7 @@ pub fn initialize(
|
||||
// validation, clocks, sensor routes, interrupts, or UART setup. It keeps
|
||||
// the former four motor pins low even for malformed configuration.
|
||||
motor_inhibit::inhibit_motor_pins(&device.RCC, &device.GPIOB);
|
||||
config.validate().map_err(BoardError::InvalidConfig)?;
|
||||
board::validate_vertical_route(config, routes)?;
|
||||
crate::validate_board_config(config, routes)?;
|
||||
board::initialize(core, device, config, routes)
|
||||
}
|
||||
|
||||
@@ -94,6 +94,7 @@ fn registered_routes() -> Result<F722BoardRoutes, BoardError> {
|
||||
|
||||
// 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_os = "none")]
|
||||
firmware::f722_flight_application!(
|
||||
crate::f722::registered_routes,
|
||||
crate::f722::registered_config
|
||||
|
||||
@@ -1,11 +1,12 @@
|
||||
//! Deterministic STM32F722 PLL clock-source selection.
|
||||
|
||||
use kaptan_log::KaptanError;
|
||||
use stm32f7::stm32f722;
|
||||
|
||||
const CLOCK_SWITCH_POLL_LIMIT: u16 = 1_024;
|
||||
|
||||
/// Failure to establish the 216 MHz PLL clock profile.
|
||||
#[derive(Clone, Copy, Debug, Eq, PartialEq)]
|
||||
#[derive(Clone, Copy, Debug, Eq, PartialEq, KaptanError)]
|
||||
pub enum ClockProfileError {
|
||||
HsiNotReady,
|
||||
SystemClockNotHsi,
|
||||
|
||||
@@ -1,6 +1,7 @@
|
||||
//! STM32F722 I²C1 interrupt-driven controller.
|
||||
|
||||
use kaptan_i2c::{I2cBus, I2cCompletion};
|
||||
use kaptan_log::KaptanError;
|
||||
use stm32f7::stm32f722;
|
||||
|
||||
const MAX_BYTES: usize = 255;
|
||||
@@ -20,7 +21,7 @@ const ICR_TERMINAL: u32 = ICR_NACKCF | ICR_STOPCF | ICR_ERRORS;
|
||||
const ICR_FAULTS: u32 = ICR_NACKCF | ICR_ERRORS;
|
||||
const CR2_STOP: u32 = 1 << 14;
|
||||
|
||||
#[derive(Clone, Copy, Debug, Eq, PartialEq)]
|
||||
#[derive(Clone, Copy, Debug, Eq, PartialEq, KaptanError)]
|
||||
pub enum I2c1Error {
|
||||
InvalidLength,
|
||||
InvalidAddress,
|
||||
|
||||
@@ -5,13 +5,14 @@
|
||||
//! is generated after the last serial frame; transmit completion can occur
|
||||
//! while the final serial clock is still in progress.
|
||||
|
||||
use kaptan_log::KaptanError;
|
||||
use kaptan_spi::SpiCompletion;
|
||||
use stm32f7::stm32f722;
|
||||
|
||||
const MAX_TRANSFER_LENGTH: usize = u16::MAX as usize;
|
||||
|
||||
/// Failure while submitting one SPI1 DMA transaction.
|
||||
#[derive(Clone, Copy, Debug, Eq, PartialEq)]
|
||||
#[derive(Clone, Copy, Debug, Eq, PartialEq, KaptanError)]
|
||||
pub enum Spi1Error {
|
||||
/// The transaction is malformed or exceeds the DMA counter range.
|
||||
InvalidLength,
|
||||
|
||||
@@ -5,13 +5,14 @@
|
||||
//! is generated after the last serial frame; transmit completion can occur
|
||||
//! while the final serial clock is still in progress.
|
||||
|
||||
use kaptan_log::KaptanError;
|
||||
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)]
|
||||
#[derive(Clone, Copy, Debug, Eq, PartialEq, KaptanError)]
|
||||
pub enum Spi3Error {
|
||||
/// The transaction is malformed or exceeds the DMA counter range.
|
||||
InvalidLength,
|
||||
|
||||
@@ -1,6 +1,7 @@
|
||||
//! DWT-backed monotonic microsecond clock for STM32F722.
|
||||
|
||||
use cortex_m::peripheral::{DCB, DWT};
|
||||
use kaptan_log::KaptanError;
|
||||
use kaptan_time::{MonotonicClock, Timestamp};
|
||||
|
||||
/// A DWT cycle counter extended into monotonic microseconds.
|
||||
@@ -15,7 +16,7 @@ pub struct DwtMicrosecondClock {
|
||||
}
|
||||
|
||||
/// Invalid fixed clock configuration for the DWT timestamp source.
|
||||
#[derive(Clone, Copy, Debug, Eq, PartialEq)]
|
||||
#[derive(Clone, Copy, Debug, Eq, PartialEq, KaptanError)]
|
||||
pub enum DwtClockStartError {
|
||||
/// A zero-megahertz core clock cannot produce microsecond timestamps.
|
||||
ZeroFrequency,
|
||||
|
||||
@@ -1,9 +1,10 @@
|
||||
//! STM32F722 USART routes used by supported boards.
|
||||
|
||||
use kaptan_log::KaptanError;
|
||||
use stm32f7::stm32f722;
|
||||
|
||||
/// UART baud-rate configuration error.
|
||||
#[derive(Clone, Copy, Debug, Eq, PartialEq)]
|
||||
#[derive(Clone, Copy, Debug, Eq, PartialEq, KaptanError)]
|
||||
pub enum BaudRateError {
|
||||
/// Zero is not a valid baud rate.
|
||||
Zero,
|
||||
|
||||
@@ -5,6 +5,7 @@
|
||||
//! cannot be selected at construction.
|
||||
|
||||
use kaptan_dma::{DmaReadableBuffer, DmaTxEngine, TxCompletion};
|
||||
use kaptan_log::KaptanError;
|
||||
use stm32f7::stm32f722;
|
||||
|
||||
use super::Usart1Tx;
|
||||
@@ -20,7 +21,7 @@ pub struct Usart1TxDma {
|
||||
}
|
||||
|
||||
/// Failure to start one DMA frame transfer.
|
||||
#[derive(Clone, Copy, Debug, Eq, PartialEq)]
|
||||
#[derive(Clone, Copy, Debug, Eq, PartialEq, KaptanError)]
|
||||
pub enum StartError {
|
||||
/// DMA cannot transfer an empty buffer or one exceeding the NDTR limit.
|
||||
InvalidBufferLength,
|
||||
|
||||
@@ -5,6 +5,7 @@
|
||||
//! cannot be selected at construction.
|
||||
|
||||
use kaptan_dma::{DmaReadableBuffer, DmaTxEngine, TxCompletion};
|
||||
use kaptan_log::KaptanError;
|
||||
use stm32f7::stm32f722;
|
||||
|
||||
use super::Usart2Tx;
|
||||
@@ -15,7 +16,7 @@ pub struct Usart2TxDma {
|
||||
}
|
||||
|
||||
/// Failure to start one DMA frame transfer on USART2.
|
||||
#[derive(Clone, Copy, Debug, Eq, PartialEq)]
|
||||
#[derive(Clone, Copy, Debug, Eq, PartialEq, KaptanError)]
|
||||
pub enum Usart2TxDmaStartError {
|
||||
/// DMA cannot transfer an empty buffer or one exceeding the NDTR limit.
|
||||
InvalidBufferLength,
|
||||
|
||||
@@ -1,6 +1,7 @@
|
||||
//! Platform-owned F722 sensor-service assembly.
|
||||
|
||||
use super::peripherals::{DwtMicrosecondClock, Spi1, Spi3};
|
||||
use kaptan_board::ImuMount;
|
||||
use kaptan_drivers::{
|
||||
icm42688::{Icm42688, Icm42688Service, Icm42688ServiceEvent},
|
||||
mpu6000::{
|
||||
@@ -8,18 +9,15 @@ use kaptan_drivers::{
|
||||
Mpu6000Service, Mpu6000ServiceEvent,
|
||||
},
|
||||
};
|
||||
use kaptan_log::{ErrorReport, StaticErrorMessage};
|
||||
use kaptan_sensor::imu::ImuSnapshot;
|
||||
use kaptan_spi::SpiCompletion;
|
||||
use stm32f7::stm32f722;
|
||||
|
||||
use super::routes::{Icm42688Spi3Route, Mpu6000Spi1Route};
|
||||
|
||||
// TODO(configuration): this is an immutable board-development value, not an
|
||||
// airframe calibration. A ground configuration flow must stage the complete
|
||||
// IMU profile (device settings, mounting transform, bias/scale calibration,
|
||||
// noise model, source/revision) while output is inhibited, then reset the
|
||||
// estimator/controller/allocation state before activation. Never apply one
|
||||
// register/calibration field to a live flight runtime.
|
||||
// This is fixed driver setup for the selected board route. It is distinct from
|
||||
// the immutable board mount and from a future safety-gated calibration flow.
|
||||
const MPU6000_CONFIG: Mpu6000Config = Mpu6000Config {
|
||||
accelerometer: AccelerometerRange::PlusMinus4G,
|
||||
gyroscope: GyroscopeRange::PlusMinus500DegreesPerSecond,
|
||||
@@ -33,7 +31,7 @@ pub(crate) enum ImuEvent {
|
||||
Idle,
|
||||
Initialized,
|
||||
Sample(ImuSnapshot),
|
||||
Fault,
|
||||
Fault(&'static StaticErrorMessage),
|
||||
}
|
||||
|
||||
/// Opaque owner of the selected F722 IMU service.
|
||||
@@ -43,6 +41,7 @@ pub(crate) enum ImuEvent {
|
||||
/// platform, preserving the board-facing abstraction boundary.
|
||||
pub struct F722Imu {
|
||||
inner: F722ImuKind,
|
||||
mount: ImuMount,
|
||||
}
|
||||
|
||||
/// The two concrete IMU services supported by the F722 platform.
|
||||
@@ -58,6 +57,7 @@ impl F722Imu {
|
||||
dma: stm32f722::DMA2,
|
||||
chip_select: stm32f722::GPIOB,
|
||||
clock: DwtMicrosecondClock,
|
||||
mount: ImuMount,
|
||||
) -> Self {
|
||||
Self {
|
||||
inner: F722ImuKind::Mpu6000(Mpu6000Service::new(
|
||||
@@ -67,6 +67,7 @@ impl F722Imu {
|
||||
MPU6000_CONFIG,
|
||||
),
|
||||
)),
|
||||
mount,
|
||||
}
|
||||
}
|
||||
|
||||
@@ -76,6 +77,7 @@ impl F722Imu {
|
||||
dma: stm32f722::DMA1,
|
||||
chip_select: &stm32f722::GPIOA,
|
||||
clock: DwtMicrosecondClock,
|
||||
mount: ImuMount,
|
||||
) -> Self {
|
||||
Self {
|
||||
inner: F722ImuKind::Icm42688(Icm42688Service::new(
|
||||
@@ -86,87 +88,104 @@ impl F722Imu {
|
||||
chip_select,
|
||||
)),
|
||||
)),
|
||||
mount,
|
||||
}
|
||||
}
|
||||
|
||||
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 }
|
||||
match &mut self.inner {
|
||||
F722ImuKind::Mpu6000(imu) => match imu.begin() {
|
||||
Ok(()) => ImuEvent::Idle,
|
||||
Err(error) => ImuEvent::Fault(error.error_message()),
|
||||
},
|
||||
F722ImuKind::Icm42688(imu) => match imu.begin() {
|
||||
Ok(()) => ImuEvent::Idle,
|
||||
Err(error) => ImuEvent::Fault(error.error_message()),
|
||||
},
|
||||
}
|
||||
}
|
||||
|
||||
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()),
|
||||
F722ImuKind::Icm42688(imu) => icm_event(imu.on_periodic_tick(), self.mount),
|
||||
}
|
||||
}
|
||||
|
||||
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()),
|
||||
F722ImuKind::Icm42688(imu) => icm_event(imu.on_data_ready(), self.mount),
|
||||
}
|
||||
}
|
||||
|
||||
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)),
|
||||
F722ImuKind::Icm42688(imu) => {
|
||||
icm_event(imu.on_transfer_complete(completion), self.mount)
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
fn mpu_event<E>(event: Mpu6000ServiceEvent<E>) -> ImuEvent {
|
||||
fn mpu_event<E: ErrorReport>(event: Mpu6000ServiceEvent<E>) -> ImuEvent {
|
||||
match event {
|
||||
Mpu6000ServiceEvent::Idle => ImuEvent::Idle,
|
||||
Mpu6000ServiceEvent::Initialized => ImuEvent::Initialized,
|
||||
Mpu6000ServiceEvent::Sample(sample) => ImuEvent::Sample(sample),
|
||||
Mpu6000ServiceEvent::Fault(_) => ImuEvent::Fault,
|
||||
Mpu6000ServiceEvent::Fault(error) => ImuEvent::Fault(error.error_message()),
|
||||
}
|
||||
}
|
||||
|
||||
fn icm_event<E>(event: Icm42688ServiceEvent<E>) -> ImuEvent {
|
||||
fn icm_event<E: ErrorReport>(event: Icm42688ServiceEvent<E>, mount: ImuMount) -> ImuEvent {
|
||||
match event {
|
||||
Icm42688ServiceEvent::Idle => ImuEvent::Idle,
|
||||
Icm42688ServiceEvent::Initialized => ImuEvent::Initialized,
|
||||
Icm42688ServiceEvent::Sample(sample) => ImuEvent::Sample(align_icm42688(sample)),
|
||||
Icm42688ServiceEvent::Fault(_) => ImuEvent::Fault,
|
||||
Icm42688ServiceEvent::Sample(sample) => ImuEvent::Sample(align_mount(sample, mount)),
|
||||
Icm42688ServiceEvent::Fault(error) => ImuEvent::Fault(error.error_message()),
|
||||
}
|
||||
}
|
||||
|
||||
/// Aligns the development GEP-F722-HD V2 ICM-42688-P yaw-180° mounting to the
|
||||
/// flight-controller body frame. The MPU6000 profile is already aligned.
|
||||
/// Applies the board-declared fixed IMU-to-body transform.
|
||||
///
|
||||
/// TODO(configuration): replace this build-time transform only as part of the
|
||||
/// same atomic sensor-profile activation described at `MPU6000_CONFIG`; it is
|
||||
/// not a user-adjustable live setting.
|
||||
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
|
||||
/// This is immutable board metadata validated against the selected electrical
|
||||
/// route before initialization. It is not an airframe calibration setting.
|
||||
fn align_mount(mut sample: ImuSnapshot, mount: ImuMount) -> ImuSnapshot {
|
||||
match mount {
|
||||
ImuMount::BodyAligned => sample,
|
||||
ImuMount::Yaw180 => {
|
||||
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 super::align_mount;
|
||||
use kaptan_board::ImuMount;
|
||||
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,
|
||||
));
|
||||
let sample = align_mount(
|
||||
ImuSnapshot::new(
|
||||
Timestamp::from_microseconds(0),
|
||||
[10, 20, 30],
|
||||
[40, 50, 60],
|
||||
0,
|
||||
),
|
||||
ImuMount::Yaw180,
|
||||
);
|
||||
assert_eq!(sample.acceleration_milligravity, [-10, -20, 30]);
|
||||
assert_eq!(sample.angular_rate_milliradians_per_second, [-40, -50, 60]);
|
||||
}
|
||||
|
||||
@@ -10,40 +10,67 @@
|
||||
#![no_std]
|
||||
|
||||
use core::marker::PhantomData;
|
||||
use kaptan_log::KaptanError;
|
||||
|
||||
pub use kaptan_board::BoardConfig;
|
||||
use kaptan_board::BoardConfigError;
|
||||
use kaptan_board::{BoardConfigError, ImuDevice, ImuMount, MavlinkPort};
|
||||
|
||||
#[cfg_attr(
|
||||
not(target_os = "none"),
|
||||
allow(dead_code, unused_imports, unused_macros)
|
||||
)]
|
||||
mod f722;
|
||||
|
||||
/// Dependencies used only while the platform expands its private RTIC macro.
|
||||
/// They stay crate-private: board binaries never name them.
|
||||
#[cfg_attr(not(target_os = "none"), allow(unused_imports))]
|
||||
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;
|
||||
#[cfg(not(test))]
|
||||
#[cfg(target_os = "none")]
|
||||
pub(crate) use panic_halt as panic;
|
||||
pub(crate) use rtic as rtic_app;
|
||||
pub(crate) use stm32f7 as pac;
|
||||
#[cfg(test)]
|
||||
#[cfg(not(target_os = "none"))]
|
||||
pub(crate) mod panic {}
|
||||
}
|
||||
|
||||
/// Construction or validation failure for a declarative F722 board.
|
||||
#[derive(Clone, Copy, Debug, Eq, PartialEq)]
|
||||
#[derive(Clone, Copy, Debug, Eq, PartialEq, KaptanError)]
|
||||
pub enum BoardError {
|
||||
/// Portable board metadata is self-inconsistent.
|
||||
InvalidConfig(BoardConfigError),
|
||||
/// Metadata and the selected vertical-sensor route disagree.
|
||||
VerticalSensorCapabilityMismatch,
|
||||
/// Metadata and the selected immutable IMU route disagree.
|
||||
ImuRouteMismatch,
|
||||
/// Metadata and the selected immutable MAVLink route disagree.
|
||||
MavlinkRouteMismatch,
|
||||
/// Metadata and the selected immutable IMU mounting transform disagree.
|
||||
ImuMountMismatch,
|
||||
/// The declaration requests a service-timer frequency not implemented by
|
||||
/// the selected F722 platform profile.
|
||||
UnsupportedServiceFrequency,
|
||||
/// The selected baud cannot carry the fixed telemetry/log schedule.
|
||||
TelemetryLinkOvercommitted,
|
||||
/// The platform could not establish a clock, peripheral, or DMA owner.
|
||||
Initialization,
|
||||
}
|
||||
|
||||
/// Validates immutable board metadata against a selected F722 route graph.
|
||||
///
|
||||
/// This is pure validation: it does not claim a pin, clock, interrupt, DMA
|
||||
/// stream, or peripheral. The runtime invokes it immediately after the motor
|
||||
/// inhibit boundary and before every other hardware action.
|
||||
pub fn validate_board_config(
|
||||
config: BoardConfig,
|
||||
routes: F722BoardRoutes,
|
||||
) -> Result<(), BoardError> {
|
||||
config.validate().map_err(BoardError::InvalidConfig)?;
|
||||
f722::validate_board_routes(config, routes)
|
||||
}
|
||||
|
||||
/// MPU6000 component selector.
|
||||
#[derive(Clone, Copy, Debug, Eq, PartialEq)]
|
||||
pub struct Mpu6000;
|
||||
@@ -235,9 +262,31 @@ pub struct F722BoardRoutes {
|
||||
}
|
||||
|
||||
impl F722BoardRoutes {
|
||||
#[cfg_attr(not(target_os = "none"), allow(dead_code))]
|
||||
pub(crate) const fn profile(self) -> F722Profile {
|
||||
self.profile
|
||||
}
|
||||
|
||||
pub(crate) const fn imu(self) -> ImuDevice {
|
||||
match self.profile {
|
||||
F722Profile::Mpu6000Spi1Usart1 => ImuDevice::Mpu6000,
|
||||
F722Profile::Icm42688Spi3Usart2 => ImuDevice::Icm42688,
|
||||
}
|
||||
}
|
||||
|
||||
pub(crate) const fn imu_mount(self) -> ImuMount {
|
||||
match self.profile {
|
||||
F722Profile::Mpu6000Spi1Usart1 => ImuMount::BodyAligned,
|
||||
F722Profile::Icm42688Spi3Usart2 => ImuMount::Yaw180,
|
||||
}
|
||||
}
|
||||
|
||||
pub(crate) const fn mavlink_port(self) -> MavlinkPort {
|
||||
match self.profile {
|
||||
F722Profile::Mpu6000Spi1Usart1 => MavlinkPort::Serial1,
|
||||
F722Profile::Icm42688Spi3Usart2 => MavlinkPort::Serial2,
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
macro_rules! impl_mpu6000_routes {
|
||||
@@ -303,17 +352,6 @@ impl_icm42688_routes!(LidarLiteV3I2c1, f722::VerticalRoute::LidarLiteV3);
|
||||
/// .build();
|
||||
/// ```
|
||||
///
|
||||
/// ```compile_fail
|
||||
/// use platform_stm32f7::{
|
||||
/// F7BoardBuilder, Icm42688, Mavlink, Pc3, VerticalSensor,
|
||||
/// };
|
||||
///
|
||||
/// let _ = F7BoardBuilder::new()
|
||||
/// .imu(Icm42688::on_spi3().with_drdy(Pc3))
|
||||
/// .mavlink(Mavlink::on_usart2())
|
||||
/// .vertical_sensor(VerticalSensor::none())
|
||||
/// .build(); // MPU6000/USART1 is the only other supported profile.
|
||||
/// ```
|
||||
/// Contract implemented by a declarative STM32F722 board crate.
|
||||
pub trait F7Board {
|
||||
/// Portable identity, capabilities and outbound data-channel policy.
|
||||
@@ -366,30 +404,3 @@ macro_rules! flight_firmware {
|
||||
/// Link anchor for the platform-owned F722 board-I/O 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))
|
||||
.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))
|
||||
.mavlink(Mavlink::on_usart2())
|
||||
.vertical_sensor(VerticalSensor::none())
|
||||
.build()
|
||||
.unwrap();
|
||||
assert_eq!(routes.profile(), F722Profile::Icm42688Spi3Usart2);
|
||||
}
|
||||
}
|
||||
@@ -1,5 +1,16 @@
|
||||
use std::{fs, path::PathBuf};
|
||||
|
||||
use kaptan_board::{
|
||||
BoardConfig, BoardProfile, DataChannelRates, ImuDevice, ImuMount, MavlinkEndpoint, MavlinkPort,
|
||||
TelemetryProfile, VerticalSensingKind,
|
||||
};
|
||||
use kaptan_log::{LogFlags, LogRecord, LogText, Severity};
|
||||
use kaptan_log_mavlink::{DiagnosticPayload, encode_diagnostic};
|
||||
use kaptan_mavlink::encoder::MavlinkEncoder;
|
||||
use platform_stm32f7::{
|
||||
BoardError, F7BoardBuilder, Mavlink, Mpu6000, Pa4, VerticalSensor, validate_board_config,
|
||||
};
|
||||
|
||||
fn platform_source(relative: &str) -> String {
|
||||
let root = PathBuf::from(env!("CARGO_MANIFEST_DIR"));
|
||||
fs::read_to_string(root.join(relative)).expect("platform source must exist")
|
||||
@@ -12,7 +23,7 @@ fn former_motor_pins_are_inhibited_before_all_other_initialization() {
|
||||
.find("motor_inhibit::inhibit_motor_pins")
|
||||
.expect("F722 initialization must claim the former motor pins first");
|
||||
let validation = module
|
||||
.find("config.validate()")
|
||||
.find("validate_board_config")
|
||||
.expect("F722 initialization must validate its board configuration");
|
||||
let board_initialization = module
|
||||
.find("board::initialize")
|
||||
@@ -55,6 +66,87 @@ fn former_motor_pins_are_inhibited_before_all_other_initialization() {
|
||||
}
|
||||
}
|
||||
|
||||
fn mpu_config() -> BoardConfig {
|
||||
BoardConfig {
|
||||
name: "test",
|
||||
mavlink: MavlinkEndpoint {
|
||||
system_id: 1,
|
||||
component_id: 1,
|
||||
port: MavlinkPort::Serial1,
|
||||
baud: 115_200,
|
||||
},
|
||||
profile: BoardProfile::TelemetryOnly(TelemetryProfile {
|
||||
imu: ImuDevice::Mpu6000,
|
||||
imu_mount: ImuMount::BodyAligned,
|
||||
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,
|
||||
},
|
||||
maximum_log_level: Severity::Info,
|
||||
}),
|
||||
}
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn immutable_board_policy_must_match_the_executed_route_graph() {
|
||||
let routes = F7BoardBuilder::new()
|
||||
.imu(Mpu6000::on_spi1().with_drdy(Pa4))
|
||||
.mavlink(Mavlink::on_usart1())
|
||||
.vertical_sensor(VerticalSensor::mtf01_on_uart4())
|
||||
.build()
|
||||
.unwrap();
|
||||
assert_eq!(validate_board_config(mpu_config(), routes), Ok(()));
|
||||
|
||||
let mut wrong_imu = mpu_config();
|
||||
let BoardProfile::TelemetryOnly(profile) = &mut wrong_imu.profile else {
|
||||
unreachable!();
|
||||
};
|
||||
profile.imu = ImuDevice::Icm42688;
|
||||
assert_eq!(
|
||||
validate_board_config(wrong_imu, routes),
|
||||
Err(BoardError::ImuRouteMismatch)
|
||||
);
|
||||
|
||||
let mut wrong_mount = mpu_config();
|
||||
let BoardProfile::TelemetryOnly(profile) = &mut wrong_mount.profile else {
|
||||
unreachable!();
|
||||
};
|
||||
profile.imu_mount = ImuMount::Yaw180;
|
||||
assert_eq!(
|
||||
validate_board_config(wrong_mount, routes),
|
||||
Err(BoardError::ImuMountMismatch)
|
||||
);
|
||||
|
||||
let mut wrong_vertical = mpu_config();
|
||||
let BoardProfile::TelemetryOnly(profile) = &mut wrong_vertical.profile else {
|
||||
unreachable!();
|
||||
};
|
||||
profile.vertical_sensing = VerticalSensingKind::Range;
|
||||
profile.data_channel.optical_flow_hz = None;
|
||||
assert_eq!(
|
||||
validate_board_config(wrong_vertical, routes),
|
||||
Err(BoardError::VerticalSensorCapabilityMismatch)
|
||||
);
|
||||
|
||||
let mut wrong_port = mpu_config();
|
||||
wrong_port.mavlink.port = MavlinkPort::Serial2;
|
||||
assert_eq!(
|
||||
validate_board_config(wrong_port, routes),
|
||||
Err(BoardError::MavlinkRouteMismatch)
|
||||
);
|
||||
|
||||
let mut overcommitted_link = mpu_config();
|
||||
overcommitted_link.mavlink.baud = 9_600;
|
||||
assert_eq!(
|
||||
validate_board_config(overcommitted_link, routes),
|
||||
Err(BoardError::TelemetryLinkOvercommitted)
|
||||
);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn telemetry_platform_has_no_physical_actuator_peripheral_path() {
|
||||
let sources = [
|
||||
@@ -117,3 +209,38 @@ fn telemetry_has_no_inbound_command_authority() {
|
||||
"telemetry must not schedule a worker from an inbound command"
|
||||
);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn f722_link_budget_uses_both_maximum_generated_diagnostic_frames() {
|
||||
let text = LogText::from_message(&"x".repeat(160));
|
||||
let log_record = LogRecord::new(Severity::Info, LogFlags::NONE, text);
|
||||
let error_record = LogRecord::new(Severity::Error, LogFlags::NONE, text);
|
||||
let mut encoder = MavlinkEncoder::new(1, 1);
|
||||
let DiagnosticPayload::Log(log_payload) = encode_diagnostic(&log_record) else {
|
||||
panic!("info record must use KAPTAN_LOG");
|
||||
};
|
||||
let log_frame = encoder.custom(&log_payload);
|
||||
let DiagnosticPayload::Error(error_payload) = encode_diagnostic(&error_record) else {
|
||||
panic!("error record must use KAPTAN_ERROR");
|
||||
};
|
||||
let error_frame = encoder.custom(&error_payload);
|
||||
|
||||
assert_eq!(log_frame.raw_bytes().len(), 175);
|
||||
assert_eq!(
|
||||
kaptan_mavlink::message::kaptan::KAPTAN_LOG_DATA::ENCODED_LEN,
|
||||
163
|
||||
);
|
||||
assert_eq!(error_frame.raw_bytes().len(), 174);
|
||||
assert_eq!(
|
||||
kaptan_mavlink::message::kaptan::KAPTAN_ERROR_DATA::ENCODED_LEN,
|
||||
162
|
||||
);
|
||||
assert_eq!(
|
||||
62 * 48 + 10 * error_frame.raw_bytes().len() + 5 * log_frame.raw_bytes().len(),
|
||||
5_591
|
||||
);
|
||||
let board = platform_source("src/f722/board.rs");
|
||||
assert!(board.contains("KAPTAN_ERROR_FRAME_MAXIMUM_BYTES"));
|
||||
assert!(board.contains("KAPTAN_LOG_FRAME_MAXIMUM_BYTES"));
|
||||
assert!(board.contains("5,591 bytes/s"));
|
||||
}
|
||||
Reference in new issue
Block a user