feat(f722): enforce board policy and emit diagnostics

platforms/stm32f7/src/f722/board.rs

@@ -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);
 }