653 lines
23 KiB
Rust
653 lines
23 KiB
Rust
//! Physical-sensor telemetry STM32F722 board-I/O state and ownership graph.
|
||
|
||
use core::sync::atomic::{AtomicU8, Ordering};
|
||
|
||
use super::peripherals::{
|
||
DwtMicrosecondClock, I2c1, I2c1Config, ReceiveInterrupt, Tim2Periodic, Uart4,
|
||
configure_exti3_pin_pc3, configure_exti4_pin_pa4, configure_i2c1_pins_pb8_pb9,
|
||
configure_spi1_pins_pa5_pa6_pa7, configure_spi3_pins_pc10_pc11_pc12,
|
||
configure_uart4_pins_pa0_pa1, configure_usart1_rx_pin_pa10, configure_usart1_tx_pin_pa9,
|
||
configure_usart2_pins_pa2_pa3,
|
||
};
|
||
use kaptan_board::{BoardConfig, VerticalSensingKind};
|
||
use kaptan_data_channel::{
|
||
DataChannel, DataSources, LatestImuMailbox, LatestOpticalFlowMailbox, LatestRangeMailbox,
|
||
};
|
||
use kaptan_drivers::{
|
||
lidar_lite_v3::{AcquisitionMode, LidarLiteV3, LidarLiteV3Service, LidarLiteV3ServiceEvent},
|
||
mtf01::Mtf01Service,
|
||
};
|
||
use kaptan_log::{
|
||
DeliveryClass, ErrorReport, EventHub, KaptanError, LogFilter, LogFlags, StaticError,
|
||
StaticErrorMailbox, TransportAdmission,
|
||
};
|
||
use kaptan_log_mavlink::{DiagnosticPayload, encode_diagnostic};
|
||
use kaptan_spi::SpiCompletion;
|
||
use stm32f7::stm32f722;
|
||
|
||
use crate::{BoardError, F722BoardRoutes, F722Profile};
|
||
|
||
use super::{
|
||
VerticalRoute, clock,
|
||
data_channel::{
|
||
F722MavlinkTransmitter, F722StatusLed, GeprcStatusLed, SERVICE_HZ, SERVICE_TIMER_CONFIG,
|
||
vertical_data_config,
|
||
},
|
||
interrupts,
|
||
sensors::{F722Imu, ImuEvent},
|
||
};
|
||
|
||
const UART4_CLOCK_HZ: u32 = 54_000_000;
|
||
const MTF01_BAUD: u32 = 115_200;
|
||
const I2C1_CONFIG: I2c1Config = I2c1Config::from_timing_register(0x2040_4768);
|
||
const LIDAR_MODE: AcquisitionMode = AcquisitionMode::WithReceiverBiasCorrection;
|
||
const LIDAR_STARTUP_TICKS: u8 = 3;
|
||
const 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. Both maxima
|
||
// are 163 payload bytes plus 12 unsigned MAVLink V2 framing bytes, so
|
||
// 62×48 + 15×175 = 5,601 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();
|
||
|
||
type F722Range = LidarLiteV3Service<DwtMicrosecondClock, I2c1>;
|
||
/// Opaque RTIC-local MTF-01 service type; its route is platform-owned.
|
||
pub type F722Mtf01 = Mtf01Service<DwtMicrosecondClock>;
|
||
/// Opaque RTIC-local UART4 owner; its construction is platform-owned.
|
||
pub type F722Mtf01Uart = Uart4;
|
||
/// Opaque RTIC-local service timer; its construction is platform-owned.
|
||
pub type F722ServiceTimer = Tim2Periodic;
|
||
|
||
/// RTIC-shared MAVLink state.
|
||
///
|
||
/// Only the MAVLink worker accesses this state, so its priority ceiling is 9
|
||
/// 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,
|
||
}
|
||
|
||
/// RTIC-shared LIDAR state.
|
||
///
|
||
/// I2C event/error IRQs and the service timer are its only users. MTF-01 is a
|
||
/// separate UART-only local resource, so it cannot raise this ceiling.
|
||
pub struct F722LidarState {
|
||
range: Option<F722Range>,
|
||
startup_ticks: u8,
|
||
}
|
||
|
||
/// Explicit handoff from platform initialization to the RTIC resource graph.
|
||
///
|
||
/// This is not runtime state indirection: each field is moved exactly once
|
||
/// into the single RTIC owner that accesses it.
|
||
pub struct F722InitResources {
|
||
pub(crate) imu: F722Imu,
|
||
pub(crate) mtf01: Option<F722Mtf01>,
|
||
pub(crate) mtf01_uart: Option<F722Mtf01Uart>,
|
||
pub(crate) timer: F722ServiceTimer,
|
||
pub(crate) lidar: F722LidarState,
|
||
pub(crate) mavlink: F722MavlinkState,
|
||
}
|
||
|
||
/// Validates the relationship between portable metadata and selected sensor
|
||
/// route before platform initialization claims a peripheral.
|
||
pub(crate) fn validate_board_routes(
|
||
config: BoardConfig,
|
||
routes: F722BoardRoutes,
|
||
) -> Result<(), BoardError> {
|
||
let telemetry = config.telemetry().ok_or(BoardError::Initialization)?;
|
||
if telemetry.data_channel.service_hz != SERVICE_HZ {
|
||
return Err(BoardError::UnsupportedServiceFrequency);
|
||
}
|
||
let expected = match routes.vertical {
|
||
VerticalRoute::None => VerticalSensingKind::None,
|
||
VerticalRoute::Mtf01 => VerticalSensingKind::RangeAndOpticalFlow,
|
||
VerticalRoute::LidarLiteV3 => VerticalSensingKind::Range,
|
||
};
|
||
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(())
|
||
}
|
||
|
||
pub(crate) fn initialize(
|
||
mut core: cortex_m::Peripherals,
|
||
device: stm32f722::Peripherals,
|
||
config: BoardConfig,
|
||
routes: F722BoardRoutes,
|
||
) -> Result<F722InitResources, BoardError> {
|
||
let clock = clock::initialize(&mut core, &device).map_err(|_| BoardError::Initialization)?;
|
||
|
||
// The profile is written once before any interrupt is enabled. ISR helpers
|
||
// read it with Acquire ordering and only access the initialized route.
|
||
interrupts::activate_profile(routes.profile());
|
||
|
||
match routes.profile() {
|
||
F722Profile::Mpu6000Spi1Usart1 => {
|
||
initialize_mpu6000(device, clock, config, routes.vertical)
|
||
}
|
||
F722Profile::Icm42688Spi3Usart2 => {
|
||
initialize_icm42688(device, clock, config, routes.vertical)
|
||
}
|
||
}
|
||
}
|
||
|
||
fn initialize_mpu6000(
|
||
device: stm32f722::Peripherals,
|
||
clock: DwtMicrosecondClock,
|
||
config: BoardConfig,
|
||
vertical: VerticalRoute,
|
||
) -> Result<F722InitResources, BoardError> {
|
||
configure_spi1_pins_pa5_pa6_pa7(&device.RCC, &device.GPIOA);
|
||
configure_exti4_pin_pa4(&device.GPIOA);
|
||
configure_exti4(&device.RCC, device.SYSCFG, device.EXTI);
|
||
configure_usart1_tx_pin_pa9(&device.GPIOA);
|
||
configure_usart1_rx_pin_pa10(&device.GPIOA);
|
||
configure_vertical_pins(&device.RCC, &device.GPIOA, &device.GPIOB, vertical);
|
||
|
||
let (range, mtf01, mtf01_uart) = initialize_vertical(
|
||
&device.RCC,
|
||
device.I2C1,
|
||
device.UART4,
|
||
clock.fork(),
|
||
clock.fork(),
|
||
vertical,
|
||
)?;
|
||
let 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, config.mavlink.baud)
|
||
.map_err(|_| BoardError::Initialization)?;
|
||
let led = F722StatusLed::Kakute(super::peripherals::ActiveLowLedPd2::new(
|
||
&device.RCC,
|
||
device.GPIOD,
|
||
));
|
||
finish(
|
||
&device.RCC,
|
||
device.TIM2,
|
||
config,
|
||
vertical,
|
||
imu,
|
||
range,
|
||
mtf01,
|
||
mtf01_uart,
|
||
transmitter,
|
||
led,
|
||
)
|
||
}
|
||
|
||
fn initialize_icm42688(
|
||
device: stm32f722::Peripherals,
|
||
clock: DwtMicrosecondClock,
|
||
config: BoardConfig,
|
||
vertical: VerticalRoute,
|
||
) -> Result<F722InitResources, BoardError> {
|
||
configure_spi3_pins_pc10_pc11_pc12(&device.RCC, &device.GPIOC);
|
||
configure_exti3_pin_pc3(&device.GPIOC);
|
||
configure_exti3(&device.RCC, device.SYSCFG, device.EXTI);
|
||
// PA2/PA3 writes take effect only after enabling the GPIOA bus clock.
|
||
// The ICM route enables GPIOA later for chip select, which is too late for
|
||
// the USART2 alternate-function configuration below.
|
||
device
|
||
.RCC
|
||
.ahb1enr()
|
||
.modify(|_, write| write.gpioaen().set_bit());
|
||
configure_usart2_pins_pa2_pa3(&device.GPIOA);
|
||
configure_vertical_pins(&device.RCC, &device.GPIOA, &device.GPIOB, vertical);
|
||
|
||
let (range, mtf01, mtf01_uart) = initialize_vertical(
|
||
&device.RCC,
|
||
device.I2C1,
|
||
device.UART4,
|
||
clock.fork(),
|
||
clock.fork(),
|
||
vertical,
|
||
)?;
|
||
let 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, config.mavlink.baud)
|
||
.map_err(|_| BoardError::Initialization)?;
|
||
let led = F722StatusLed::Geprc(GeprcStatusLed::new(&device.RCC, device.GPIOA));
|
||
finish(
|
||
&device.RCC,
|
||
device.TIM2,
|
||
config,
|
||
vertical,
|
||
imu,
|
||
range,
|
||
mtf01,
|
||
mtf01_uart,
|
||
transmitter,
|
||
led,
|
||
)
|
||
}
|
||
|
||
#[allow(clippy::too_many_arguments)]
|
||
fn finish(
|
||
rcc: &stm32f722::RCC,
|
||
tim2: stm32f722::TIM2,
|
||
config: BoardConfig,
|
||
vertical: VerticalRoute,
|
||
imu: F722Imu,
|
||
range: Option<F722Range>,
|
||
mtf01: Option<F722Mtf01>,
|
||
mtf01_uart: Option<Uart4>,
|
||
transmitter: F722MavlinkTransmitter,
|
||
led: F722StatusLed,
|
||
) -> Result<F722InitResources, BoardError> {
|
||
let data_channel = DataChannel::new(
|
||
transmitter,
|
||
config,
|
||
vertical_data_config(vertical),
|
||
DataSources {
|
||
imu: &SAMPLES,
|
||
range: &RANGES,
|
||
optical_flow: &OPTICAL_FLOW,
|
||
},
|
||
)
|
||
.map_err(|_| BoardError::Initialization)?;
|
||
Ok(F722InitResources {
|
||
imu,
|
||
mtf01,
|
||
mtf01_uart,
|
||
timer: Tim2Periodic::new(rcc, tim2, SERVICE_TIMER_CONFIG),
|
||
lidar: F722LidarState {
|
||
range,
|
||
startup_ticks: 0,
|
||
},
|
||
mavlink: F722MavlinkState {
|
||
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,
|
||
},
|
||
})
|
||
}
|
||
|
||
fn configure_vertical_pins(
|
||
rcc: &stm32f722::RCC,
|
||
gpioa: &stm32f722::GPIOA,
|
||
gpiob: &stm32f722::GPIOB,
|
||
vertical: VerticalRoute,
|
||
) {
|
||
match vertical {
|
||
VerticalRoute::None => {}
|
||
VerticalRoute::Mtf01 => configure_uart4_pins_pa0_pa1(gpioa),
|
||
VerticalRoute::LidarLiteV3 => configure_i2c1_pins_pb8_pb9(rcc, gpiob),
|
||
}
|
||
}
|
||
|
||
type VerticalResources = (Option<F722Range>, Option<F722Mtf01>, Option<Uart4>);
|
||
|
||
fn initialize_vertical(
|
||
rcc: &stm32f722::RCC,
|
||
i2c: stm32f722::I2C1,
|
||
uart4: stm32f722::UART4,
|
||
mtf01_clock: DwtMicrosecondClock,
|
||
range_clock: DwtMicrosecondClock,
|
||
vertical: VerticalRoute,
|
||
) -> Result<VerticalResources, BoardError> {
|
||
match vertical {
|
||
VerticalRoute::None => Ok((None, None, None)),
|
||
VerticalRoute::Mtf01 => Ok((
|
||
None,
|
||
Some(Mtf01Service::new(mtf01_clock)),
|
||
Some(
|
||
Uart4::new(rcc, uart4, UART4_CLOCK_HZ, MTF01_BAUD)
|
||
.map_err(|_| BoardError::Initialization)?,
|
||
),
|
||
)),
|
||
VerticalRoute::LidarLiteV3 => Ok((
|
||
Some(LidarLiteV3Service::new(
|
||
range_clock,
|
||
LidarLiteV3::new(I2c1::new(rcc, i2c, I2C1_CONFIG)),
|
||
LIDAR_MODE,
|
||
)),
|
||
None,
|
||
None,
|
||
)),
|
||
}
|
||
}
|
||
|
||
fn configure_exti4(rcc: &stm32f722::RCC, syscfg: stm32f722::SYSCFG, exti: stm32f722::EXTI) {
|
||
rcc.apb2enr().modify(|_, write| write.syscfgen().set_bit());
|
||
syscfg.exticr2().modify(|_, write| {
|
||
// SAFETY: selector zero is GPIOA for EXTI4 on STM32F722.
|
||
unsafe { write.exti4().bits(0) }
|
||
});
|
||
exti.imr().modify(|_, write| write.im4().set_bit());
|
||
exti.rtsr().modify(|_, write| write.tr4().set_bit());
|
||
exti.ftsr().modify(|_, write| write.tr4().clear_bit());
|
||
exti.pr().write(|write| write.pr4().clear());
|
||
}
|
||
|
||
fn configure_exti3(rcc: &stm32f722::RCC, syscfg: stm32f722::SYSCFG, exti: stm32f722::EXTI) {
|
||
rcc.apb2enr().modify(|_, write| write.syscfgen().set_bit());
|
||
syscfg.exticr1().modify(|_, write| {
|
||
// SAFETY: selector two is GPIOC for EXTI3 on STM32F722.
|
||
unsafe { write.exti3().bits(2) }
|
||
});
|
||
exti.imr().modify(|_, write| write.im3().set_bit());
|
||
exti.rtsr().modify(|_, write| write.tr3().set_bit());
|
||
exti.ftsr().modify(|_, write| write.tr3().clear_bit());
|
||
exti.pr().write(|write| write.pr3().clear());
|
||
}
|
||
|
||
/// Services the MTF-01 UART route. It is a task-local resource, so it needs
|
||
/// no RTIC lock and cannot extend another resource's priority ceiling.
|
||
pub(crate) fn on_uart4_interrupt(mtf01: &mut Option<F722Mtf01>) {
|
||
let Some(mtf01) = mtf01.as_mut() else {
|
||
return;
|
||
};
|
||
match Uart4::acknowledge_receive_interrupt() {
|
||
ReceiveInterrupt::Byte(byte) => {
|
||
if let Some(report) = mtf01.on_received_byte(byte) {
|
||
if let Some(range) = report.range_snapshot() {
|
||
RANGES.publish(range);
|
||
}
|
||
if let Some(flow) = report.optical_flow_snapshot() {
|
||
OPTICAL_FLOW.publish(flow);
|
||
}
|
||
}
|
||
}
|
||
ReceiveInterrupt::Fault => {
|
||
mtf01.reset();
|
||
record_mtf01_fault(F722TelemetryError::Mtf01Uart.static_error());
|
||
}
|
||
ReceiveInterrupt::NotPending => {}
|
||
}
|
||
}
|
||
|
||
impl F722LidarState {
|
||
pub(crate) fn on_i2c_event(&mut self, error: bool) {
|
||
let Some(range) = self.range.as_mut() else {
|
||
return;
|
||
};
|
||
let event = range.on_controller_interrupt(|i2c| {
|
||
if error {
|
||
i2c.acknowledge_error_interrupt()
|
||
} else {
|
||
i2c.acknowledge_event_interrupt()
|
||
}
|
||
});
|
||
publish_lidar_event(event);
|
||
}
|
||
|
||
pub(crate) fn on_timer(&mut self) {
|
||
if let Some(range) = self.range.as_mut() {
|
||
if self.startup_ticks < LIDAR_STARTUP_TICKS {
|
||
self.startup_ticks += 1;
|
||
if self.startup_ticks == LIDAR_STARTUP_TICKS
|
||
&& let Err(error) = range.finish_startup()
|
||
{
|
||
record_lidar_fault(error.static_error());
|
||
}
|
||
} else {
|
||
publish_lidar_event(range.on_periodic_tick());
|
||
}
|
||
}
|
||
}
|
||
}
|
||
|
||
/// Advances the IMU state machine in its dedicated worker-local resource.
|
||
pub(crate) fn process_imu(imu: &mut F722Imu, events: u8) {
|
||
const START: u8 = 1 << 4;
|
||
const TRANSFER_FAULT: u8 = 1 << 2;
|
||
const TRANSFER_COMPLETE: u8 = 1 << 1;
|
||
const TIMER: u8 = 1 << 3;
|
||
const DRDY: u8 = 1 << 0;
|
||
|
||
if events & START != 0 {
|
||
accept_imu_event(imu.begin());
|
||
}
|
||
if events & TRANSFER_FAULT != 0 {
|
||
accept_imu_event(imu.on_transfer_complete(SpiCompletion::TransferFault));
|
||
}
|
||
if events & TRANSFER_COMPLETE != 0 {
|
||
accept_imu_event(imu.on_transfer_complete(SpiCompletion::Complete));
|
||
}
|
||
if events & TIMER != 0 {
|
||
accept_imu_event(imu.on_periodic_tick());
|
||
}
|
||
if events & DRDY != 0 {
|
||
accept_imu_event(imu.on_data_ready());
|
||
}
|
||
}
|
||
|
||
fn accept_imu_event(event: ImuEvent) {
|
||
match event {
|
||
ImuEvent::Sample(sample) => {
|
||
SAMPLES.publish(sample);
|
||
}
|
||
ImuEvent::Fault(message) => record_imu_fault(message),
|
||
ImuEvent::Idle | ImuEvent::Initialized => {}
|
||
}
|
||
}
|
||
|
||
impl F722MavlinkState {
|
||
/// Handles bounded DMA and timer work for the telemetry image.
|
||
pub(crate) fn process_worker_events(&mut self, events: u8) {
|
||
const TIMER: u8 = 1 << 0;
|
||
const DMA: u8 = 1 << 2;
|
||
|
||
if events & DMA != 0 {
|
||
let completion = take_mavlink_completion();
|
||
let _ = self.data_channel.on_dma_complete(completion);
|
||
if completion == kaptan_dma::TxCompletion::Complete {
|
||
self.led.toggle();
|
||
}
|
||
}
|
||
|
||
if events & TIMER != 0 {
|
||
// The board is initialized and transmitting physical telemetry,
|
||
// but has no flight runtime or output authority. `STANDBY` with
|
||
// an unarmed base mode communicates that accurately; `UNINIT`
|
||
// would incorrectly claim initialization never completed.
|
||
self.data_channel
|
||
.on_service_tick(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.static_error()
|
||
}
|
||
kaptan_mavlink::transmitter::TransmitFaultKind::Engine => {
|
||
F722TelemetryError::TransmitEngine.static_error()
|
||
}
|
||
kaptan_mavlink::transmitter::TransmitFaultKind::Terminal => {
|
||
F722TelemetryError::TransmitTerminal.static_error()
|
||
}
|
||
};
|
||
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 => {}
|
||
}
|
||
}
|
||
}
|
||
|
||
pub(crate) fn take_mavlink_completion() -> kaptan_dma::TxCompletion {
|
||
match UART_COMPLETION.swap(0, Ordering::AcqRel) {
|
||
1 => kaptan_dma::TxCompletion::Complete,
|
||
2 => kaptan_dma::TxCompletion::TransferFault,
|
||
_ => kaptan_dma::TxCompletion::NotPending,
|
||
}
|
||
}
|
||
|
||
pub fn record_mavlink_completion(completion: kaptan_dma::TxCompletion) {
|
||
let code = match completion {
|
||
kaptan_dma::TxCompletion::Complete => 1,
|
||
kaptan_dma::TxCompletion::TransferFault | kaptan_dma::TxCompletion::Halted => 2,
|
||
kaptan_dma::TxCompletion::NotPending => 0,
|
||
};
|
||
if code != 0 {
|
||
UART_COMPLETION.store(code, Ordering::Release);
|
||
}
|
||
}
|
||
|
||
fn publish_lidar_event<E: ErrorReport>(event: LidarLiteV3ServiceEvent<E>) {
|
||
match event {
|
||
LidarLiteV3ServiceEvent::Measurement(snapshot) => RANGES.publish(snapshot),
|
||
LidarLiteV3ServiceEvent::Fault(error) => record_lidar_fault(error.static_error()),
|
||
LidarLiteV3ServiceEvent::Idle => {}
|
||
}
|
||
}
|
||
|
||
fn record_imu_fault(message: &'static StaticError) {
|
||
IMU_FAULTS.record(message);
|
||
}
|
||
|
||
fn record_lidar_fault(message: &'static StaticError) {
|
||
LIDAR_FAULTS.record(message);
|
||
}
|
||
|
||
fn record_mtf01_fault(message: &'static StaticError) {
|
||
MTF01_FAULTS.record(message);
|
||
}
|