refactor(firmware): simplify configuration and diagnostics

This commit is contained in:
Onur Özkan committed 2026-08-23 00:40:36 +03:00
1 parent ba9c97ed3b
commit fdf82a5114
96 files changed
+1612 -2772

No files matched your search

+106 -67
View File
@@ -3,7 +3,7 @@
use core::sync::atomic::{AtomicU8, Ordering};
use super::peripherals::{
DwtMicrosecondClock, I2c1, I2c1Config, ReceiveInterrupt, Tim2Periodic, Uart4,
DwtMicrosecondClock, I2c1, I2c1Config, I2c1Error, 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,
@@ -14,15 +14,16 @@ use kaptan_data_channel::{
DataChannel, DataChannelRates, DataSources, LatestImuMailbox, LatestOpticalFlowMailbox,
LatestRangeMailbox,
};
use kaptan_diagnostics_mavlink::{encode_error, encode_log};
use kaptan_drivers::{
lidar_lite_v3::{AcquisitionMode, LidarLiteV3, LidarLiteV3Service, LidarLiteV3ServiceEvent},
lidar_lite_v3::{
AcquisitionMode, Error as LidarError, LidarLiteV3, LidarLiteV3Service,
LidarLiteV3ServiceEvent, StartupError,
},
mtf01::Mtf01Service,
};
use kaptan_log::{
DeliveryClass, ErrorReport, KaptanError, LogFilter, LogSinkQueue, StaticError,
StaticErrorMailbox, TransportAdmission,
};
use kaptan_log_mavlink::{DiagnosticPayload, encode_diagnostic};
use kaptan_err_propagate::{ErrorId, ErrorSink};
use kaptan_log::{DeliveryClass, LogSinkQueue, TransportAdmission};
use kaptan_spi::SpiCompletion;
use stm32f7::stm32f722;
@@ -70,16 +71,6 @@ pub(crate) const fn scheduled_link_budget_fits(baud: u32) -> bool {
SCHEDULED_TELEMETRY_BYTES_PER_SECOND < baud as usize / 10
}
static UART_COMPLETION: AtomicU8 = AtomicU8::new(0);
#[derive(Clone, Copy, Debug, KaptanError)]
enum F722TelemetryError {
Mtf01Uart,
}
// 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();
@@ -106,8 +97,8 @@ where
config_protocol: ConfigProtocol,
receiver: IntegrityMavlinkReceiver<2>,
config_lease_ticks: u16,
mavlink_errors: ErrorSink,
mavlink_logs: LogSinkQueue<1, 1>,
fault_mailbox_cursor: u8,
led: F722StatusLed,
}
@@ -367,11 +358,7 @@ where
optical_flow_hz: runtime.optical_flow_hz.map(usize::from),
heartbeat_hz: usize::from(runtime.heartbeat_hz),
};
let mavlink_log_filter = if runtime.mavlink_logging_enabled {
LogFilter::up_to(runtime.mavlink_maximum_log_severity)
} else {
LogFilter::disabled()
};
let mavlink_log_filter = runtime.mavlink_log_level_filter;
let data_channel = DataChannel::new(
transmitter,
system_id,
@@ -400,8 +387,8 @@ where
config_protocol: ConfigProtocol::new(Target::new(system_id, component_id)),
receiver: IntegrityMavlinkReceiver::new(),
config_lease_ticks: 0,
mavlink_errors: ErrorSink::new(),
mavlink_logs: LogSinkQueue::new(mavlink_log_filter),
fault_mailbox_cursor: 0,
led,
},
})
@@ -517,7 +504,7 @@ pub fn on_uart4_interrupt(mtf01: &mut Option<F722Mtf01>) {
}
ReceiveInterrupt::Fault => {
mtf01.reset();
record_mtf01_fault(F722TelemetryError::Mtf01Uart.static_error());
kaptan_log::error!("mtf01 uart receive fault");
}
ReceiveInterrupt::NotPending => {}
}
@@ -535,22 +522,23 @@ impl F722LidarState {
i2c.acknowledge_event_interrupt()
}
});
publish_lidar_event(event);
let _ = publish_lidar_event(event);
}
pub fn on_timer(&mut self) {
#[kaptan_err_propagate::propagate]
pub fn on_timer(&mut self) -> Result<(), StartupError> {
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());
if self.startup_ticks == LIDAR_STARTUP_TICKS {
range.finish_startup()?;
kaptan_log::info!("lidar-lite v3 initialized");
}
} else {
publish_lidar_event(range.on_periodic_tick());
let _ = publish_lidar_event(range.on_periodic_tick());
}
}
Ok(())
}
}
@@ -561,32 +549,38 @@ pub fn process_imu(imu: &mut F722Imu, events: u8) {
const TRANSFER_COMPLETE: u8 = 1 << 1;
const TIMER: u8 = 1 << 3;
const DRDY: u8 = 1 << 0;
let transfer_terminal = events & (TRANSFER_FAULT | TRANSFER_COMPLETE) != 0;
if events & START != 0 {
accept_imu_event(imu.begin());
let _ = accept_imu_event(imu.begin());
}
if events & TRANSFER_FAULT != 0 {
accept_imu_event(imu.on_transfer_complete(SpiCompletion::TransferFault));
let _ = accept_imu_event(imu.on_transfer_complete(SpiCompletion::TransferFault));
}
if events & TRANSFER_COMPLETE != 0 {
accept_imu_event(imu.on_transfer_complete(SpiCompletion::Complete));
let _ = accept_imu_event(imu.on_transfer_complete(SpiCompletion::Complete));
}
if events & TIMER != 0 {
accept_imu_event(imu.on_periodic_tick());
if events & TIMER != 0 && !transfer_terminal {
let _ = accept_imu_event(imu.on_periodic_tick());
}
if events & DRDY != 0 {
accept_imu_event(imu.on_data_ready());
if events & DRDY != 0 && !transfer_terminal {
let _ = accept_imu_event(imu.on_data_ready());
}
}
fn accept_imu_event(event: ImuEvent) {
#[kaptan_err_propagate::propagate]
fn accept_imu_event(event: ImuEvent) -> Result<(), ErrorId> {
match event {
ImuEvent::Sample(sample) => {
SAMPLES.publish(sample);
}
ImuEvent::Fault(message) => record_imu_fault(message),
ImuEvent::Idle | ImuEvent::Initialized => {}
ImuEvent::Fault(error) => return Err(error),
ImuEvent::Initialized => {
kaptan_log::info!("imu initialized");
}
ImuEvent::Idle => {}
}
Ok(())
}
impl<C, I, const MAX_PAYLOAD_BYTES: usize> F722MavlinkState<C, I, MAX_PAYLOAD_BYTES>
@@ -620,8 +614,7 @@ where
// 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.drain_one_mavlink_log_record();
self.drain_one_mavlink_diagnostic();
}
}
@@ -632,6 +625,7 @@ where
self.receiver.reset();
self.config_lease_ticks = 0;
self.config_protocol.expire(&mut self.configuration);
kaptan_log::warn!("mavlink receive fault; configuration session expired");
false
}
ReceiveInterrupt::NotPending => false,
@@ -683,28 +677,45 @@ where
self.config_lease_ticks -= 1;
if self.config_lease_ticks == 0 {
self.config_protocol.expire(&mut self.configuration);
kaptan_log::debug!("configuration session expired");
}
}
fn drain_driver_faults(&mut self) {
let _ = self.mavlink_logs.drain_next_error_mailbox(
[&IMU_FAULTS, &LIDAR_FAULTS, &MTF01_FAULTS],
&mut self.fault_mailbox_cursor,
);
fn drain_one_mavlink_diagnostic(&mut self) {
if self.mavlink_errors.is_empty()
&& let Some(record) = kaptan_err_propagate::take()
{
let _ = self.mavlink_errors.publish_record(record);
}
if self.submit_pending_mavlink_error() {
return;
}
if self.mavlink_logs.peek().is_none() {
let mavlink_logs = &mut self.mavlink_logs;
let _ = kaptan_log::drain_one(|record| mavlink_logs.publish_record(*record));
}
self.submit_queued_mavlink_log();
}
fn drain_one_mavlink_log_record(&mut self) {
fn submit_pending_mavlink_error(&mut self) -> bool {
let data_channel = &mut self.data_channel;
self.mavlink_errors.submit_next(|record| {
let payload = encode_error(record);
data_channel
.send_custom(kaptan_mavlink::transmitter::FrameClass::Critical, &payload)
.is_retained()
})
}
fn submit_queued_mavlink_log(&mut self) {
let data_channel = &mut self.data_channel;
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_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 payload = encode_log(record);
let admission = data_channel.send_custom(class, &payload);
let outcome = if admission.is_retained() {
TransportAdmission::Accepted
} else {
@@ -778,22 +789,50 @@ pub fn record_mavlink_completion(completion: kaptan_dma::TxCompletion) {
}
}
fn publish_lidar_event<E: ErrorReport>(event: LidarLiteV3ServiceEvent<E>) {
#[kaptan_err_propagate::propagate]
fn publish_lidar_event(
event: LidarLiteV3ServiceEvent<I2c1Error>,
) -> Result<(), LidarError<I2c1Error>> {
match event {
LidarLiteV3ServiceEvent::Measurement(snapshot) => RANGES.publish(snapshot),
LidarLiteV3ServiceEvent::Fault(error) => record_lidar_fault(error.static_error()),
LidarLiteV3ServiceEvent::Idle => {}
LidarLiteV3ServiceEvent::Measurement(snapshot) => {
RANGES.publish(snapshot);
Ok(())
}
LidarLiteV3ServiceEvent::Fault(LidarError::Start(I2c1Error::Busy)) => Ok(()),
LidarLiteV3ServiceEvent::Fault(error) => Err(error),
LidarLiteV3ServiceEvent::Idle => Ok(()),
}
}
fn record_imu_fault(message: &'static StaticError) {
IMU_FAULTS.record(message);
}
#[cfg(test)]
mod tests {
use super::{I2c1Error, LidarError, LidarLiteV3ServiceEvent, publish_lidar_event};
fn record_lidar_fault(message: &'static StaticError) {
LIDAR_FAULTS.record(message);
}
#[test]
fn transient_i2c_busy_is_retry_not_a_typed_lidar_fault() {
while kaptan_err_propagate::take().is_some() {}
assert_eq!(
publish_lidar_event(LidarLiteV3ServiceEvent::Fault(LidarError::Start(
I2c1Error::Busy,
))),
Ok(())
);
assert!(kaptan_err_propagate::take().is_none());
fn record_mtf01_fault(message: &'static StaticError) {
MTF01_FAULTS.record(message);
assert!(matches!(
publish_lidar_event(LidarLiteV3ServiceEvent::Fault(LidarError::Start(
I2c1Error::InvalidLength,
))),
Err(LidarError::Start(I2c1Error::InvalidLength))
));
assert!(kaptan_err_propagate::take().is_some());
assert!(matches!(
publish_lidar_event(LidarLiteV3ServiceEvent::Fault(LidarError::Start(
I2c1Error::InvalidAddress,
))),
Err(LidarError::Start(I2c1Error::InvalidAddress))
));
assert!(kaptan_err_propagate::take().is_some());
}
}
+3 -1
View File
@@ -1,7 +1,7 @@
//! STM32F722 Pll216 clock-source assembly.
use super::peripherals::{DwtMicrosecondClock, select_pll216_profile};
use kaptan_log::KaptanError;
use kaptan_err_propagate::KaptanError;
use stm32f7::stm32f722;
/// Core frequency in whole megahertz after selecting the PLL216 board profile.
@@ -10,7 +10,9 @@ 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 {
#[kaptan_error(id = 11801)]
Profile,
#[kaptan_error(id = 11802)]
Monotonic,
}
+8 -21
View File
@@ -7,24 +7,22 @@ use kaptan_config::{
};
use stm32f7::stm32f722;
use crate::{F7ImmutableConfig, Severity};
use crate::{F7ImmutableConfig, LevelFilter};
const MAVLINK_SYSTEM_ID: FieldId = FieldId::new(101);
const MAVLINK_COMPONENT_ID: FieldId = FieldId::new(102);
const RAW_IMU_HZ: FieldId = FieldId::new(103);
const HEARTBEAT_HZ: FieldId = FieldId::new(104);
const MAVLINK_MAXIMUM_LOG_SEVERITY: FieldId = FieldId::new(105);
const MAVLINK_LOG_LEVEL_FILTER: FieldId = FieldId::new(105);
const RANGE_HZ: FieldId = FieldId::new(106);
const OPTICAL_FLOW_HZ: FieldId = FieldId::new(107);
const MAVLINK_LOGGING_ENABLED: FieldId = FieldId::new(108);
/// Narrow boot projection consumed only by fixed F722 services.
#[derive(Clone, Copy, Debug, Eq, PartialEq)]
pub(super) struct F722UserRuntime {
pub(super) system_id: u8,
pub(super) component_id: u8,
pub(super) mavlink_logging_enabled: bool,
pub(super) mavlink_maximum_log_severity: Severity,
pub(super) mavlink_log_level_filter: LevelFilter,
pub(super) raw_imu_hz: u8,
pub(super) range_hz: Option<u8>,
pub(super) optical_flow_hz: Option<u8>,
@@ -124,13 +122,10 @@ where
}
let raw_imu_hz = required_u8(candidate, RAW_IMU_HZ)?;
let heartbeat_hz = required_u8(candidate, HEARTBEAT_HZ)?;
let mavlink_logging_enabled = required_bool(candidate, MAVLINK_LOGGING_ENABLED)?;
let severity = required_u8(candidate, MAVLINK_MAXIMUM_LOG_SEVERITY)?;
let mavlink_maximum_log_severity =
Severity::try_from_wire(severity).ok_or(ConfigViolation::field(
MAVLINK_MAXIMUM_LOG_SEVERITY,
ValidationRule::InvalidEnumValue,
))?;
let level_filter = required_u8(candidate, MAVLINK_LOG_LEVEL_FILTER)?;
let mavlink_log_level_filter = LevelFilter::try_from_wire(level_filter).ok_or(
ConfigViolation::field(MAVLINK_LOG_LEVEL_FILTER, ValidationRule::InvalidEnumValue),
)?;
let range_hz = if immutable.hardware.has_range() {
Some(required_u8(candidate, RANGE_HZ)?)
} else {
@@ -155,8 +150,7 @@ where
Ok(F722UserRuntime {
system_id,
component_id,
mavlink_logging_enabled,
mavlink_maximum_log_severity,
mavlink_log_level_filter,
raw_imu_hz,
range_hz,
optical_flow_hz,
@@ -172,13 +166,6 @@ fn required_u8<C: Config>(candidate: &C, field: FieldId) -> Result<u8, ConfigVio
}
}
fn required_bool<C: Config>(candidate: &C, field: FieldId) -> Result<bool, ConfigViolation> {
match candidate.field_value(field) {
Some(ConfigValue::Bool(value)) => Ok(value),
Some(_) | None => Err(ConfigViolation::field(field, ValidationRule::Inconsistent)),
}
}
fn checked_rate(rate: u8, field: FieldId, service_hz: usize) -> Result<usize, ConfigViolation> {
let rate = usize::from(rate);
if rate == 0 {
@@ -102,6 +102,7 @@ macro_rules! __f722_flight_application {
<$board as $crate::F7Board>::IMMUTABLE_CONFIG,
)
.unwrap_or_else(|_| safe_idle());
$crate::__platform_internal::logging::info!("flight controller initialized");
schedule_imu(IMU_EVENT_START);
(
Shared {
@@ -250,7 +251,7 @@ macro_rules! __f722_flight_application {
#[task(binds = TIM2, priority = 4, local = [timer], shared = [lidar])]
fn tim2(mut context: tim2::Context) {
context.local.timer.acknowledge_update();
context.shared.lidar.lock(F722LidarState::on_timer);
let _ = context.shared.lidar.lock(F722LidarState::on_timer);
schedule_imu(IMU_EVENT_TIMER);
schedule_mavlink(MAVLINK_EVENT_TIMER);
}
@@ -1,6 +1,6 @@
//! Deterministic STM32F722 PLL clock-source selection.
use kaptan_log::KaptanError;
use kaptan_err_propagate::KaptanError;
use stm32f7::stm32f722;
const CLOCK_SWITCH_POLL_LIMIT: u16 = 1_024;
@@ -8,12 +8,19 @@ const CLOCK_SWITCH_POLL_LIMIT: u16 = 1_024;
/// Failure to establish the 216 MHz PLL clock profile.
#[derive(Clone, Copy, Debug, Eq, PartialEq, KaptanError)]
pub enum ClockProfileError {
#[kaptan_error(id = 11001)]
HsiNotReady,
#[kaptan_error(id = 11002)]
SystemClockNotHsi,
#[kaptan_error(id = 11003)]
PllNotStopped,
#[kaptan_error(id = 11004)]
PllNotReady,
#[kaptan_error(id = 11005)]
OverDriveNotReady,
#[kaptan_error(id = 11006)]
OverDriveSwitchNotReady,
#[kaptan_error(id = 11007)]
SystemClockNotPll,
}
@@ -1,7 +1,7 @@
//! STM32F722 I²C1 interrupt-driven controller.
use kaptan_err_propagate::KaptanError;
use kaptan_i2c::{I2cBus, I2cCompletion};
use kaptan_log::KaptanError;
use stm32f7::stm32f722;
const MAX_BYTES: usize = 255;
@@ -23,8 +23,11 @@ const CR2_STOP: u32 = 1 << 14;
#[derive(Clone, Copy, Debug, Eq, PartialEq, KaptanError)]
pub enum I2c1Error {
#[kaptan_error(id = 11701)]
InvalidLength,
#[kaptan_error(id = 11702)]
InvalidAddress,
#[kaptan_error(id = 11703)]
Busy,
}
@@ -23,7 +23,7 @@ pub use gpio::{
configure_uart4_pins_pa0_pa1, configure_usart1_rx_pin_pa10, configure_usart1_tx_pin_pa9,
configure_usart2_pins_pa2_pa3,
};
pub use i2c1::{I2c1, I2c1Config};
pub use i2c1::{I2c1, I2c1Config, I2c1Error};
pub use spi1::{Spi1, Spi1Error};
pub use spi3::{Spi3, Spi3Error};
pub use tim2::{Tim2Periodic, Tim2PeriodicConfig};
@@ -5,7 +5,7 @@
//! 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_err_propagate::KaptanError;
use kaptan_spi::SpiCompletion;
use stm32f7::stm32f722;
@@ -15,8 +15,10 @@ const MAX_TRANSFER_LENGTH: usize = u16::MAX as usize;
#[derive(Clone, Copy, Debug, Eq, PartialEq, KaptanError)]
pub enum Spi1Error {
/// The transaction is malformed or exceeds the DMA counter range.
#[kaptan_error(id = 11501)]
InvalidLength,
/// A previous DMA transaction still owns SPI1.
#[kaptan_error(id = 11502)]
Busy,
}
@@ -5,7 +5,7 @@
//! 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_err_propagate::KaptanError;
use kaptan_spi::SpiCompletion;
use stm32f7::stm32f722;
@@ -15,8 +15,10 @@ const MAX_TRANSFER_LENGTH: usize = u16::MAX as usize;
#[derive(Clone, Copy, Debug, Eq, PartialEq, KaptanError)]
pub enum Spi3Error {
/// The transaction is malformed or exceeds the DMA counter range.
#[kaptan_error(id = 11601)]
InvalidLength,
/// A previous DMA transaction still owns SPI3.
#[kaptan_error(id = 11602)]
Busy,
}
@@ -1,7 +1,7 @@
//! DWT-backed monotonic microsecond clock for STM32F722.
use cortex_m::peripheral::{DCB, DWT};
use kaptan_log::KaptanError;
use kaptan_err_propagate::KaptanError;
use kaptan_time::{MonotonicClock, Timestamp};
/// A DWT cycle counter extended into monotonic microseconds.
@@ -19,6 +19,7 @@ pub struct DwtMicrosecondClock {
#[derive(Clone, Copy, Debug, Eq, PartialEq, KaptanError)]
pub enum DwtClockStartError {
/// A zero-megahertz core clock cannot produce microsecond timestamps.
#[kaptan_error(id = 11101)]
ZeroFrequency,
}
@@ -1,14 +1,16 @@
//! STM32F722 USART routes used by supported boards.
use kaptan_log::KaptanError;
use kaptan_err_propagate::KaptanError;
use stm32f7::stm32f722;
/// UART baud-rate configuration error.
#[derive(Clone, Copy, Debug, Eq, PartialEq, KaptanError)]
pub enum BaudRateError {
/// Zero is not a valid baud rate.
#[kaptan_error(id = 11201)]
Zero,
/// The requested divisor cannot fit USART BRR.
#[kaptan_error(id = 11202)]
OutOfRange,
}
@@ -5,7 +5,7 @@
//! cannot be selected at construction.
use kaptan_dma::{DmaReadableBuffer, DmaTxEngine, TxCompletion};
use kaptan_log::KaptanError;
use kaptan_err_propagate::KaptanError;
use stm32f7::stm32f722;
use super::Usart1Tx;
@@ -24,8 +24,10 @@ pub struct Usart1TxDma {
#[derive(Clone, Copy, Debug, Eq, PartialEq, KaptanError)]
pub enum StartError {
/// DMA cannot transfer an empty buffer or one exceeding the NDTR limit.
#[kaptan_error(id = 11301)]
InvalidBufferLength,
/// DMA2 stream 7 still owns an earlier transfer.
#[kaptan_error(id = 11302)]
Busy,
}
@@ -5,7 +5,7 @@
//! cannot be selected at construction.
use kaptan_dma::{DmaReadableBuffer, DmaTxEngine, TxCompletion};
use kaptan_log::KaptanError;
use kaptan_err_propagate::KaptanError;
use stm32f7::stm32f722;
use super::Usart2Tx;
@@ -23,8 +23,10 @@ pub struct Usart2TxDma {
#[derive(Clone, Copy, Debug, Eq, PartialEq, KaptanError)]
pub enum Usart2TxDmaStartError {
/// DMA cannot transfer an empty buffer or one exceeding the NDTR limit.
#[kaptan_error(id = 11401)]
InvalidBufferLength,
/// DMA1 stream 6 still owns an earlier transfer.
#[kaptan_error(id = 11402)]
Busy,
}
+50 -12
View File
@@ -1,11 +1,11 @@
//! Platform-owned F722 sensor-service assembly.
use super::peripherals::{DwtMicrosecondClock, Spi1, Spi3};
use super::peripherals::{DwtMicrosecondClock, Spi1, Spi1Error, Spi3, Spi3Error};
use kaptan_drivers::{
icm42688::{Icm42688, Icm42688Service, Icm42688ServiceEvent},
mpu6000::{Mpu6000, Mpu6000Config, Mpu6000Service, Mpu6000ServiceEvent},
icm42688::{Error as Icm42688Error, Icm42688, Icm42688Service, Icm42688ServiceEvent},
mpu6000::{Error as Mpu6000Error, Mpu6000, Mpu6000Config, Mpu6000Service, Mpu6000ServiceEvent},
};
use kaptan_log::{ErrorReport, StaticError};
use kaptan_err_propagate::{ErrorId, ErrorReport};
use kaptan_sensor::imu::ImuSnapshot;
use kaptan_spi::SpiCompletion;
use stm32f7::stm32f722;
@@ -20,7 +20,7 @@ pub(crate) enum ImuEvent {
Idle,
Initialized,
Sample(ImuSnapshot),
Fault(&'static StaticError),
Fault(ErrorId),
}
/// Opaque owner of the selected F722 IMU service.
@@ -86,11 +86,11 @@ impl F722Imu {
match &mut self.inner {
F722ImuKind::Mpu6000(imu) => match imu.begin() {
Ok(()) => ImuEvent::Idle,
Err(error) => ImuEvent::Fault(error.static_error()),
Err(error) => ImuEvent::Fault(error.error_id()),
},
F722ImuKind::Icm42688(imu) => match imu.begin() {
Ok(()) => ImuEvent::Idle,
Err(error) => ImuEvent::Fault(error.static_error()),
Err(error) => ImuEvent::Fault(error.error_id()),
},
}
}
@@ -119,21 +119,23 @@ impl F722Imu {
}
}
fn mpu_event<E: ErrorReport>(event: Mpu6000ServiceEvent<E>) -> ImuEvent {
fn mpu_event(event: Mpu6000ServiceEvent<Spi1Error>) -> ImuEvent {
match event {
Mpu6000ServiceEvent::Idle => ImuEvent::Idle,
Mpu6000ServiceEvent::Initialized => ImuEvent::Initialized,
Mpu6000ServiceEvent::Sample(sample) => ImuEvent::Sample(sample),
Mpu6000ServiceEvent::Fault(error) => ImuEvent::Fault(error.static_error()),
Mpu6000ServiceEvent::Fault(Mpu6000Error::Start(Spi1Error::Busy)) => ImuEvent::Idle,
Mpu6000ServiceEvent::Fault(error) => ImuEvent::Fault(error.error_id()),
}
}
fn icm_event<E: ErrorReport>(event: Icm42688ServiceEvent<E>, mount: ImuMount) -> ImuEvent {
fn icm_event(event: Icm42688ServiceEvent<Spi3Error>, mount: ImuMount) -> ImuEvent {
match event {
Icm42688ServiceEvent::Idle => ImuEvent::Idle,
Icm42688ServiceEvent::Initialized => ImuEvent::Initialized,
Icm42688ServiceEvent::Sample(sample) => ImuEvent::Sample(align_mount(sample, mount)),
Icm42688ServiceEvent::Fault(error) => ImuEvent::Fault(error.static_error()),
Icm42688ServiceEvent::Fault(Icm42688Error::Start(Spi3Error::Busy)) => ImuEvent::Idle,
Icm42688ServiceEvent::Fault(error) => ImuEvent::Fault(error.error_id()),
}
}
@@ -160,8 +162,13 @@ fn align_mount(mut sample: ImuSnapshot, mount: ImuMount) -> ImuSnapshot {
#[cfg(test)]
mod tests {
use super::align_mount;
use super::{ImuEvent, align_mount, icm_event, mpu_event};
use crate::ImuMount;
use crate::f722::peripherals::{Spi1Error, Spi3Error};
use kaptan_drivers::{
icm42688::{Error as Icm42688Error, Icm42688ServiceEvent},
mpu6000::{Error as Mpu6000Error, Mpu6000ServiceEvent},
};
use kaptan_sensor::imu::ImuSnapshot;
use kaptan_time::Timestamp;
@@ -179,4 +186,35 @@ mod tests {
assert_eq!(sample.acceleration_milligravity, [-10, -20, 30]);
assert_eq!(sample.angular_rate_milliradians_per_second, [-40, -50, 60]);
}
#[test]
fn transient_spi_busy_is_retry_not_a_typed_imu_fault() {
assert!(matches!(
mpu_event(Mpu6000ServiceEvent::Fault(Mpu6000Error::Start(
Spi1Error::Busy
))),
ImuEvent::Idle
));
assert!(matches!(
icm_event(
Icm42688ServiceEvent::Fault(Icm42688Error::Start(Spi3Error::Busy)),
ImuMount::BodyAligned,
),
ImuEvent::Idle
));
assert!(matches!(
mpu_event(Mpu6000ServiceEvent::Fault(Mpu6000Error::Start(
Spi1Error::InvalidLength
))),
ImuEvent::Fault(_)
));
assert!(matches!(
icm_event(
Icm42688ServiceEvent::Fault(Icm42688Error::Start(Spi3Error::InvalidLength)),
ImuMount::BodyAligned,
),
ImuEvent::Fault(_)
));
}
}
+8 -2
View File
@@ -9,12 +9,12 @@
#![no_std]
use kaptan_config::Config;
use kaptan_log::KaptanError;
use kaptan_err_propagate::KaptanError;
pub use kaptan_drivers::mpu6000::{
AccelerometerRange, DigitalLowPassFilter, GyroscopeRange, Mpu6000Config,
};
pub use kaptan_log::Severity;
pub use kaptan_log::LevelFilter;
/// Fixed sensor-to-body mounting transform.
#[derive(Clone, Copy, Debug, Eq, PartialEq)]
@@ -50,6 +50,7 @@ pub mod __platform_internal {
pub use cortex_m as cm;
pub use cortex_m_rt as cmrt;
pub use kaptan_dma as dma;
pub use kaptan_log as logging;
#[cfg(target_os = "none")]
pub use panic_halt as panic;
pub use rtic as rtic_app;
@@ -62,15 +63,20 @@ pub mod __platform_internal {
#[derive(Clone, Copy, Debug, Eq, PartialEq, KaptanError)]
pub enum BoardError {
/// The immutable vertical route and its electrical parameters disagree.
#[kaptan_error(id = 11901)]
VerticalSensorCapabilityMismatch,
/// The declaration requests a service-timer frequency not implemented by
/// the selected F722 platform profile.
#[kaptan_error(id = 11902)]
UnsupportedServiceFrequency,
/// The selected baud cannot carry the deterministic telemetry schedule.
#[kaptan_error(id = 11903)]
TelemetryLinkOvercommitted,
/// The platform could not establish a clock, peripheral, or DMA owner.
#[kaptan_error(id = 11904)]
Initialization,
/// The target configuration could not be recovered from its flash journal.
#[kaptan_error(id = 11905)]
Configuration,
}