feat(f722): enforce board policy and emit diagnostics
This commit is contained in:
1 parent
a4ef0f0740
commit
f4f47d7302
16 files changed
+563
-162
No files matched your search
@@ -24,6 +24,8 @@ kaptan-data-channel = { path = "../../libs/kaptan-data-channel" }
|
|||||||
kaptan-dma = { path = "../../libs/kaptan-dma" }
|
kaptan-dma = { path = "../../libs/kaptan-dma" }
|
||||||
kaptan-drivers = { path = "../../libs/kaptan-drivers" }
|
kaptan-drivers = { path = "../../libs/kaptan-drivers" }
|
||||||
kaptan-i2c = { path = "../../libs/kaptan-i2c" }
|
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-mavlink = { path = "../../libs/kaptan-mavlink" }
|
||||||
kaptan-sensor = { path = "../../libs/kaptan-sensor" }
|
kaptan-sensor = { path = "../../libs/kaptan-sensor" }
|
||||||
kaptan-spi = { path = "../../libs/kaptan-spi" }
|
kaptan-spi = { path = "../../libs/kaptan-spi" }
|
||||||
|
|||||||
@@ -17,6 +17,11 @@ use kaptan_drivers::{
|
|||||||
lidar_lite_v3::{AcquisitionMode, LidarLiteV3, LidarLiteV3Service, LidarLiteV3ServiceEvent},
|
lidar_lite_v3::{AcquisitionMode, LidarLiteV3, LidarLiteV3Service, LidarLiteV3ServiceEvent},
|
||||||
mtf01::Mtf01Service,
|
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 kaptan_spi::SpiCompletion;
|
||||||
use stm32f7::stm32f722;
|
use stm32f7::stm32f722;
|
||||||
|
|
||||||
@@ -37,7 +42,47 @@ const MTF01_BAUD: u32 = 115_200;
|
|||||||
const I2C1_CONFIG: I2c1Config = I2c1Config::from_timing_register(0x2040_4768);
|
const I2C1_CONFIG: I2c1Config = I2c1Config::from_timing_register(0x2040_4768);
|
||||||
const LIDAR_MODE: AcquisitionMode = AcquisitionMode::WithReceiverBiasCorrection;
|
const LIDAR_MODE: AcquisitionMode = AcquisitionMode::WithReceiverBiasCorrection;
|
||||||
const LIDAR_STARTUP_TICKS: u8 = 3;
|
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);
|
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 SAMPLES: LatestImuMailbox = LatestImuMailbox::new();
|
||||||
static RANGES: LatestRangeMailbox = LatestRangeMailbox::new();
|
static RANGES: LatestRangeMailbox = LatestRangeMailbox::new();
|
||||||
static OPTICAL_FLOW: LatestOpticalFlowMailbox = LatestOpticalFlowMailbox::new();
|
static OPTICAL_FLOW: LatestOpticalFlowMailbox = LatestOpticalFlowMailbox::new();
|
||||||
@@ -56,6 +101,10 @@ pub type F722ServiceTimer = Tim2Periodic;
|
|||||||
/// rather than the IMU or vertical-sensor IRQ ceiling.
|
/// rather than the IMU or vertical-sensor IRQ ceiling.
|
||||||
pub struct F722MavlinkState {
|
pub struct F722MavlinkState {
|
||||||
data_channel: DataChannel<'static, F722MavlinkTransmitter, SERVICE_HZ>,
|
data_channel: DataChannel<'static, F722MavlinkTransmitter, SERVICE_HZ>,
|
||||||
|
logs: EventHub<2, 4>,
|
||||||
|
budget_ticks: u8,
|
||||||
|
error_budget: u8,
|
||||||
|
log_budget: u8,
|
||||||
led: F722StatusLed,
|
led: F722StatusLed,
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -83,7 +132,7 @@ pub struct F722InitResources {
|
|||||||
|
|
||||||
/// Validates the relationship between portable metadata and selected sensor
|
/// Validates the relationship between portable metadata and selected sensor
|
||||||
/// route before platform initialization claims a peripheral.
|
/// route before platform initialization claims a peripheral.
|
||||||
pub(crate) fn validate_vertical_route(
|
pub(crate) fn validate_board_routes(
|
||||||
config: BoardConfig,
|
config: BoardConfig,
|
||||||
routes: F722BoardRoutes,
|
routes: F722BoardRoutes,
|
||||||
) -> Result<(), BoardError> {
|
) -> Result<(), BoardError> {
|
||||||
@@ -99,6 +148,18 @@ pub(crate) fn validate_vertical_route(
|
|||||||
if telemetry.vertical_sensing != expected {
|
if telemetry.vertical_sensing != expected {
|
||||||
return Err(BoardError::VerticalSensorCapabilityMismatch);
|
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(())
|
Ok(())
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -145,14 +206,17 @@ fn initialize_mpu6000(
|
|||||||
clock.fork(),
|
clock.fork(),
|
||||||
vertical,
|
vertical,
|
||||||
)?;
|
)?;
|
||||||
|
let telemetry = config.telemetry().ok_or(BoardError::Initialization)?;
|
||||||
let imu = F722Imu::mpu6000(
|
let imu = F722Imu::mpu6000(
|
||||||
&device.RCC,
|
&device.RCC,
|
||||||
device.SPI1,
|
device.SPI1,
|
||||||
device.DMA2,
|
device.DMA2,
|
||||||
device.GPIOB,
|
device.GPIOB,
|
||||||
clock.fork(),
|
clock.fork(),
|
||||||
|
telemetry.imu_mount,
|
||||||
);
|
);
|
||||||
let transmitter = F722MavlinkTransmitter::usart1(&device.RCC, device.USART1)
|
let transmitter =
|
||||||
|
F722MavlinkTransmitter::usart1(&device.RCC, device.USART1, config.mavlink.baud)
|
||||||
.map_err(|_| BoardError::Initialization)?;
|
.map_err(|_| BoardError::Initialization)?;
|
||||||
let led = F722StatusLed::Kakute(super::peripherals::ActiveLowLedPd2::new(
|
let led = F722StatusLed::Kakute(super::peripherals::ActiveLowLedPd2::new(
|
||||||
&device.RCC,
|
&device.RCC,
|
||||||
@@ -199,14 +263,17 @@ fn initialize_icm42688(
|
|||||||
clock.fork(),
|
clock.fork(),
|
||||||
vertical,
|
vertical,
|
||||||
)?;
|
)?;
|
||||||
|
let telemetry = config.telemetry().ok_or(BoardError::Initialization)?;
|
||||||
let imu = F722Imu::icm42688(
|
let imu = F722Imu::icm42688(
|
||||||
&device.RCC,
|
&device.RCC,
|
||||||
device.SPI3,
|
device.SPI3,
|
||||||
device.DMA1,
|
device.DMA1,
|
||||||
&device.GPIOA,
|
&device.GPIOA,
|
||||||
clock.fork(),
|
clock.fork(),
|
||||||
|
telemetry.imu_mount,
|
||||||
);
|
);
|
||||||
let transmitter = F722MavlinkTransmitter::usart2(&device.RCC, device.USART2)
|
let transmitter =
|
||||||
|
F722MavlinkTransmitter::usart2(&device.RCC, device.USART2, config.mavlink.baud)
|
||||||
.map_err(|_| BoardError::Initialization)?;
|
.map_err(|_| BoardError::Initialization)?;
|
||||||
let led = F722StatusLed::Geprc(GeprcStatusLed::new(&device.RCC, device.GPIOA));
|
let led = F722StatusLed::Geprc(GeprcStatusLed::new(&device.RCC, device.GPIOA));
|
||||||
finish(
|
finish(
|
||||||
@@ -256,7 +323,19 @@ fn finish(
|
|||||||
range,
|
range,
|
||||||
startup_ticks: 0,
|
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 => {}
|
ReceiveInterrupt::NotPending => {}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
@@ -370,8 +452,10 @@ impl F722LidarState {
|
|||||||
if let Some(range) = self.range.as_mut() {
|
if let Some(range) = self.range.as_mut() {
|
||||||
if self.startup_ticks < LIDAR_STARTUP_TICKS {
|
if self.startup_ticks < LIDAR_STARTUP_TICKS {
|
||||||
self.startup_ticks += 1;
|
self.startup_ticks += 1;
|
||||||
if self.startup_ticks == LIDAR_STARTUP_TICKS {
|
if self.startup_ticks == LIDAR_STARTUP_TICKS
|
||||||
let _ = range.finish_startup();
|
&& let Err(error) = range.finish_startup()
|
||||||
|
{
|
||||||
|
record_lidar_fault(error.error_message());
|
||||||
}
|
}
|
||||||
} else {
|
} else {
|
||||||
publish_lidar_event(range.on_periodic_tick());
|
publish_lidar_event(range.on_periodic_tick());
|
||||||
@@ -410,7 +494,7 @@ fn accept_imu_event(event: ImuEvent) {
|
|||||||
ImuEvent::Sample(sample) => {
|
ImuEvent::Sample(sample) => {
|
||||||
SAMPLES.publish(sample);
|
SAMPLES.publish(sample);
|
||||||
}
|
}
|
||||||
ImuEvent::Fault => {}
|
ImuEvent::Fault(message) => record_imu_fault(message),
|
||||||
ImuEvent::Idle | ImuEvent::Initialized => {}
|
ImuEvent::Idle | ImuEvent::Initialized => {}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
@@ -423,7 +507,7 @@ impl F722MavlinkState {
|
|||||||
|
|
||||||
if events & DMA != 0 {
|
if events & DMA != 0 {
|
||||||
let completion = take_mavlink_completion();
|
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 {
|
if completion == kaptan_dma::TxCompletion::Complete {
|
||||||
self.led.toggle();
|
self.led.toggle();
|
||||||
}
|
}
|
||||||
@@ -436,6 +520,94 @@ impl F722MavlinkState {
|
|||||||
// would incorrectly claim initialization never completed.
|
// would incorrectly claim initialization never completed.
|
||||||
self.data_channel
|
self.data_channel
|
||||||
.on_service_tick(kaptan_mavlink::message::MavState::MAV_STATE_STANDBY, false);
|
.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>) {
|
fn publish_lidar_event<E: ErrorReport>(event: LidarLiteV3ServiceEvent<E>) {
|
||||||
if let LidarLiteV3ServiceEvent::Measurement(snapshot) = event {
|
match event {
|
||||||
RANGES.publish(snapshot);
|
LidarLiteV3ServiceEvent::Measurement(snapshot) => RANGES.publish(snapshot),
|
||||||
|
LidarLiteV3ServiceEvent::Fault(error) => record_lidar_fault(error.error_message()),
|
||||||
|
LidarLiteV3ServiceEvent::Idle => {}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
#[cfg(test)]
|
fn record_imu_fault(message: &'static StaticErrorMessage) {
|
||||||
mod tests {
|
IMU_FAULTS.record(message);
|
||||||
use super::validate_vertical_route;
|
}
|
||||||
use crate::{F7BoardBuilder, Mavlink, Mpu6000, Pa4, VerticalSensor};
|
|
||||||
use kaptan_board::{
|
|
||||||
BoardConfig, BoardProfile, DataChannelRates, MavlinkEndpoint, TelemetryProfile,
|
|
||||||
VerticalSensingKind,
|
|
||||||
};
|
|
||||||
|
|
||||||
#[test]
|
fn record_lidar_fault(message: &'static StaticErrorMessage) {
|
||||||
fn rejects_metadata_that_claims_flow_for_a_lidar_route() {
|
LIDAR_FAULTS.record(message);
|
||||||
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_mtf01_fault(message: &'static StaticErrorMessage) {
|
||||||
|
MTF01_FAULTS.record(message);
|
||||||
}
|
}
|
||||||
@@ -1,12 +1,14 @@
|
|||||||
//! STM32F722 Pll216 clock-source assembly.
|
//! STM32F722 Pll216 clock-source assembly.
|
||||||
|
|
||||||
use super::peripherals::{DwtMicrosecondClock, select_pll216_profile};
|
use super::peripherals::{DwtMicrosecondClock, select_pll216_profile};
|
||||||
|
use kaptan_log::KaptanError;
|
||||||
use stm32f7::stm32f722;
|
use stm32f7::stm32f722;
|
||||||
|
|
||||||
/// Core frequency in whole megahertz after selecting the PLL216 board profile.
|
/// Core frequency in whole megahertz after selecting the PLL216 board profile.
|
||||||
const CORE_CLOCK_MHZ: u32 = 216;
|
const CORE_CLOCK_MHZ: u32 = 216;
|
||||||
|
|
||||||
/// A failure while establishing the board's clock tree or monotonic clock.
|
/// A failure while establishing the board's clock tree or monotonic clock.
|
||||||
|
#[derive(Clone, Copy, Debug, Eq, PartialEq, KaptanError)]
|
||||||
pub(crate) enum ClockInitializationError {
|
pub(crate) enum ClockInitializationError {
|
||||||
Profile,
|
Profile,
|
||||||
Monotonic,
|
Monotonic,
|
||||||
|
|||||||
@@ -1,22 +1,19 @@
|
|||||||
//! F722 transport and vertical-route adapters for the portable data channel.
|
//! F722 transport and vertical-route adapters for the portable data channel.
|
||||||
|
|
||||||
use super::{VerticalRoute, peripherals};
|
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_dma::TxCompletion;
|
||||||
use kaptan_mavlink::{
|
use kaptan_mavlink::{
|
||||||
MavlinkFrame,
|
MavlinkFrame,
|
||||||
encoder::DistanceSensorConfig,
|
encoder::DistanceSensorConfig,
|
||||||
transmitter::{FrameClass, MavlinkTransmitter},
|
transmitter::{FrameClass, MavlinkTransmitter, TransmitFaultKind},
|
||||||
};
|
};
|
||||||
|
|
||||||
// TODO(configuration): transport baud rate, sensor orientation/range, and
|
// Board endpoint, baud, sensor topology, and mounting remain immutable
|
||||||
// freshness are currently development profile constants. A ground-station
|
// build-time policy. They are not ground-station-editable in this telemetry
|
||||||
// configuration flow must stage them with sensor calibration and link
|
// image: the F722 has no authenticated config ingress or activation runtime.
|
||||||
// authority, apply them only with output inhibited, then reset estimator,
|
|
||||||
// controller, allocator, and safety prearm state as one revision.
|
|
||||||
pub(crate) const UART1_CLOCK_HZ: u32 = 108_000_000;
|
pub(crate) const UART1_CLOCK_HZ: u32 = 108_000_000;
|
||||||
pub(crate) const UART2_CLOCK_HZ: u32 = 54_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;
|
pub(crate) const SERVICE_HZ: usize = 100;
|
||||||
|
|
||||||
/// Fixed service-timer configuration for the validated Pll216 profile.
|
/// 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.
|
/// The two F722 DMA engines behind the portable data-channel transport.
|
||||||
pub(crate) enum F722MavlinkTransmitter {
|
enum F722MavlinkTransport {
|
||||||
Usart1(MavlinkTransmitter<peripherals::DcacheDisabledDmaMemory, peripherals::Usart1TxDma, 4>),
|
Usart1(MavlinkTransmitter<peripherals::DcacheDisabledDmaMemory, peripherals::Usart1TxDma, 4>),
|
||||||
Usart2(MavlinkTransmitter<peripherals::DcacheDisabledDmaMemory, peripherals::Usart2TxDma, 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 {
|
impl F722MavlinkTransmitter {
|
||||||
pub(crate) fn usart1(
|
pub(crate) fn usart1(
|
||||||
rcc: &stm32f7::stm32f722::RCC,
|
rcc: &stm32f7::stm32f722::RCC,
|
||||||
uart: stm32f7::stm32f722::USART1,
|
uart: stm32f7::stm32f722::USART1,
|
||||||
|
baud: u32,
|
||||||
) -> Result<Self, ()> {
|
) -> Result<Self, ()> {
|
||||||
let uart =
|
let uart = peripherals::Usart1Tx::new(rcc, uart, UART1_CLOCK_HZ, baud).map_err(|_| ())?;
|
||||||
peripherals::Usart1Tx::new(rcc, uart, UART1_CLOCK_HZ, MAVLINK_BAUD).map_err(|_| ())?;
|
|
||||||
// SAFETY: platform initialization disables D-cache before this object
|
// SAFETY: platform initialization disables D-cache before this object
|
||||||
// is created, and the active MAVLink frame remains queue-owned until
|
// is created, and the active MAVLink frame remains queue-owned until
|
||||||
// this platform receives a terminal DMA completion notification.
|
// this platform receives a terminal DMA completion notification.
|
||||||
let memory = unsafe { peripherals::DcacheDisabledDmaMemory::new() };
|
let memory = unsafe { peripherals::DcacheDisabledDmaMemory::new() };
|
||||||
Ok(Self::Usart1(MavlinkTransmitter::new(
|
Ok(Self {
|
||||||
|
inner: F722MavlinkTransport::Usart1(MavlinkTransmitter::new(
|
||||||
memory,
|
memory,
|
||||||
peripherals::Usart1TxDma::new(rcc, uart),
|
peripherals::Usart1TxDma::new(rcc, uart),
|
||||||
)))
|
)),
|
||||||
|
health: TxHealth::default(),
|
||||||
|
pending_faults: 0,
|
||||||
|
})
|
||||||
}
|
}
|
||||||
|
|
||||||
pub(crate) fn usart2(
|
pub(crate) fn usart2(
|
||||||
rcc: &stm32f7::stm32f722::RCC,
|
rcc: &stm32f7::stm32f722::RCC,
|
||||||
uart: stm32f7::stm32f722::USART2,
|
uart: stm32f7::stm32f722::USART2,
|
||||||
|
baud: u32,
|
||||||
) -> Result<Self, ()> {
|
) -> Result<Self, ()> {
|
||||||
let uart =
|
let uart = peripherals::Usart2Tx::new(rcc, uart, UART2_CLOCK_HZ, baud).map_err(|_| ())?;
|
||||||
peripherals::Usart2Tx::new(rcc, uart, UART2_CLOCK_HZ, MAVLINK_BAUD).map_err(|_| ())?;
|
|
||||||
// SAFETY: identical DMA-memory invariant to the USART1 route above;
|
// SAFETY: identical DMA-memory invariant to the USART1 route above;
|
||||||
// only the fixed platform DMA endpoint differs.
|
// only the fixed platform DMA endpoint differs.
|
||||||
let memory = unsafe { peripherals::DcacheDisabledDmaMemory::new() };
|
let memory = unsafe { peripherals::DcacheDisabledDmaMemory::new() };
|
||||||
Ok(Self::Usart2(MavlinkTransmitter::new(
|
Ok(Self {
|
||||||
|
inner: F722MavlinkTransport::Usart2(MavlinkTransmitter::new(
|
||||||
memory,
|
memory,
|
||||||
peripherals::Usart2TxDma::new(rcc, uart),
|
peripherals::Usart2TxDma::new(rcc, uart),
|
||||||
)))
|
)),
|
||||||
|
health: TxHealth::default(),
|
||||||
|
pending_faults: 0,
|
||||||
|
})
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
impl FrameSink for F722MavlinkTransmitter {
|
impl FrameSink for F722MavlinkTransmitter {
|
||||||
fn send(&mut self, class: FrameClass, frame: MavlinkFrame) {
|
fn send(&mut self, class: FrameClass, frame: MavlinkFrame) -> FrameAdmission {
|
||||||
match self {
|
let (admission, start_fault) = match &mut self.inner {
|
||||||
Self::Usart1(transmitter) => {
|
F722MavlinkTransport::Usart1(transmitter) => {
|
||||||
let _ = transmitter.send_frame(class, frame);
|
let outcome = transmitter.admit_and_start(class, frame);
|
||||||
|
(
|
||||||
|
outcome.admission,
|
||||||
|
outcome.start.err().map(|fault| fault.kind()),
|
||||||
|
)
|
||||||
}
|
}
|
||||||
Self::Usart2(transmitter) => {
|
F722MavlinkTransport::Usart2(transmitter) => {
|
||||||
let _ = transmitter.send_frame(class, frame);
|
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) {
|
fn health(&self) -> TxHealth {
|
||||||
match self {
|
self.health
|
||||||
Self::Usart1(transmitter) => {
|
|
||||||
let _ = transmitter.on_dma_complete(completion);
|
|
||||||
}
|
}
|
||||||
Self::Usart2(transmitter) => {
|
|
||||||
let _ = transmitter.on_dma_complete(completion);
|
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
|
||||||
|
}
|
||||||
|
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::{
|
pub(crate) use board::{
|
||||||
F722InitResources, F722LidarState, F722MavlinkState, F722Mtf01, F722Mtf01Uart,
|
F722InitResources, F722LidarState, F722MavlinkState, F722Mtf01, F722Mtf01Uart,
|
||||||
F722ServiceTimer, on_uart4_interrupt, process_imu, record_mavlink_completion,
|
F722ServiceTimer, on_uart4_interrupt, process_imu, record_mavlink_completion,
|
||||||
|
validate_board_routes,
|
||||||
};
|
};
|
||||||
pub(crate) use interrupts::{
|
pub(crate) use interrupts::{
|
||||||
acknowledge_exti3, acknowledge_exti4, acknowledge_imu_rx_dma1, acknowledge_imu_rx_dma2,
|
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
|
// validation, clocks, sensor routes, interrupts, or UART setup. It keeps
|
||||||
// the former four motor pins low even for malformed configuration.
|
// the former four motor pins low even for malformed configuration.
|
||||||
motor_inhibit::inhibit_motor_pins(&device.RCC, &device.GPIOB);
|
motor_inhibit::inhibit_motor_pins(&device.RCC, &device.GPIOB);
|
||||||
config.validate().map_err(BoardError::InvalidConfig)?;
|
crate::validate_board_config(config, routes)?;
|
||||||
board::validate_vertical_route(config, routes)?;
|
|
||||||
board::initialize(core, device, 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
|
// This is one source-level task graph. Its compile-time bindings are the
|
||||||
// platform-owned finite interrupt superset, independent of the board crate.
|
// platform-owned finite interrupt superset, independent of the board crate.
|
||||||
|
#[cfg(target_os = "none")]
|
||||||
firmware::f722_flight_application!(
|
firmware::f722_flight_application!(
|
||||||
crate::f722::registered_routes,
|
crate::f722::registered_routes,
|
||||||
crate::f722::registered_config
|
crate::f722::registered_config
|
||||||
|
|||||||
@@ -1,11 +1,12 @@
|
|||||||
//! Deterministic STM32F722 PLL clock-source selection.
|
//! Deterministic STM32F722 PLL clock-source selection.
|
||||||
|
|
||||||
|
use kaptan_log::KaptanError;
|
||||||
use stm32f7::stm32f722;
|
use stm32f7::stm32f722;
|
||||||
|
|
||||||
const CLOCK_SWITCH_POLL_LIMIT: u16 = 1_024;
|
const CLOCK_SWITCH_POLL_LIMIT: u16 = 1_024;
|
||||||
|
|
||||||
/// Failure to establish the 216 MHz PLL clock profile.
|
/// 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 {
|
pub enum ClockProfileError {
|
||||||
HsiNotReady,
|
HsiNotReady,
|
||||||
SystemClockNotHsi,
|
SystemClockNotHsi,
|
||||||
|
|||||||
@@ -1,6 +1,7 @@
|
|||||||
//! STM32F722 I²C1 interrupt-driven controller.
|
//! STM32F722 I²C1 interrupt-driven controller.
|
||||||
|
|
||||||
use kaptan_i2c::{I2cBus, I2cCompletion};
|
use kaptan_i2c::{I2cBus, I2cCompletion};
|
||||||
|
use kaptan_log::KaptanError;
|
||||||
use stm32f7::stm32f722;
|
use stm32f7::stm32f722;
|
||||||
|
|
||||||
const MAX_BYTES: usize = 255;
|
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 ICR_FAULTS: u32 = ICR_NACKCF | ICR_ERRORS;
|
||||||
const CR2_STOP: u32 = 1 << 14;
|
const CR2_STOP: u32 = 1 << 14;
|
||||||
|
|
||||||
#[derive(Clone, Copy, Debug, Eq, PartialEq)]
|
#[derive(Clone, Copy, Debug, Eq, PartialEq, KaptanError)]
|
||||||
pub enum I2c1Error {
|
pub enum I2c1Error {
|
||||||
InvalidLength,
|
InvalidLength,
|
||||||
InvalidAddress,
|
InvalidAddress,
|
||||||
|
|||||||
@@ -5,13 +5,14 @@
|
|||||||
//! is generated after the last serial frame; transmit completion can occur
|
//! is generated after the last serial frame; transmit completion can occur
|
||||||
//! while the final serial clock is still in progress.
|
//! while the final serial clock is still in progress.
|
||||||
|
|
||||||
|
use kaptan_log::KaptanError;
|
||||||
use kaptan_spi::SpiCompletion;
|
use kaptan_spi::SpiCompletion;
|
||||||
use stm32f7::stm32f722;
|
use stm32f7::stm32f722;
|
||||||
|
|
||||||
const MAX_TRANSFER_LENGTH: usize = u16::MAX as usize;
|
const MAX_TRANSFER_LENGTH: usize = u16::MAX as usize;
|
||||||
|
|
||||||
/// Failure while submitting one SPI1 DMA transaction.
|
/// Failure while submitting one SPI1 DMA transaction.
|
||||||
#[derive(Clone, Copy, Debug, Eq, PartialEq)]
|
#[derive(Clone, Copy, Debug, Eq, PartialEq, KaptanError)]
|
||||||
pub enum Spi1Error {
|
pub enum Spi1Error {
|
||||||
/// The transaction is malformed or exceeds the DMA counter range.
|
/// The transaction is malformed or exceeds the DMA counter range.
|
||||||
InvalidLength,
|
InvalidLength,
|
||||||
|
|||||||
@@ -5,13 +5,14 @@
|
|||||||
//! is generated after the last serial frame; transmit completion can occur
|
//! is generated after the last serial frame; transmit completion can occur
|
||||||
//! while the final serial clock is still in progress.
|
//! while the final serial clock is still in progress.
|
||||||
|
|
||||||
|
use kaptan_log::KaptanError;
|
||||||
use kaptan_spi::SpiCompletion;
|
use kaptan_spi::SpiCompletion;
|
||||||
use stm32f7::stm32f722;
|
use stm32f7::stm32f722;
|
||||||
|
|
||||||
const MAX_TRANSFER_LENGTH: usize = u16::MAX as usize;
|
const MAX_TRANSFER_LENGTH: usize = u16::MAX as usize;
|
||||||
|
|
||||||
/// Failure while submitting one SPI3 DMA transaction.
|
/// Failure while submitting one SPI3 DMA transaction.
|
||||||
#[derive(Clone, Copy, Debug, Eq, PartialEq)]
|
#[derive(Clone, Copy, Debug, Eq, PartialEq, KaptanError)]
|
||||||
pub enum Spi3Error {
|
pub enum Spi3Error {
|
||||||
/// The transaction is malformed or exceeds the DMA counter range.
|
/// The transaction is malformed or exceeds the DMA counter range.
|
||||||
InvalidLength,
|
InvalidLength,
|
||||||
|
|||||||
@@ -1,6 +1,7 @@
|
|||||||
//! DWT-backed monotonic microsecond clock for STM32F722.
|
//! DWT-backed monotonic microsecond clock for STM32F722.
|
||||||
|
|
||||||
use cortex_m::peripheral::{DCB, DWT};
|
use cortex_m::peripheral::{DCB, DWT};
|
||||||
|
use kaptan_log::KaptanError;
|
||||||
use kaptan_time::{MonotonicClock, Timestamp};
|
use kaptan_time::{MonotonicClock, Timestamp};
|
||||||
|
|
||||||
/// A DWT cycle counter extended into monotonic microseconds.
|
/// A DWT cycle counter extended into monotonic microseconds.
|
||||||
@@ -15,7 +16,7 @@ pub struct DwtMicrosecondClock {
|
|||||||
}
|
}
|
||||||
|
|
||||||
/// Invalid fixed clock configuration for the DWT timestamp source.
|
/// 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 {
|
pub enum DwtClockStartError {
|
||||||
/// A zero-megahertz core clock cannot produce microsecond timestamps.
|
/// A zero-megahertz core clock cannot produce microsecond timestamps.
|
||||||
ZeroFrequency,
|
ZeroFrequency,
|
||||||
|
|||||||
@@ -1,9 +1,10 @@
|
|||||||
//! STM32F722 USART routes used by supported boards.
|
//! STM32F722 USART routes used by supported boards.
|
||||||
|
|
||||||
|
use kaptan_log::KaptanError;
|
||||||
use stm32f7::stm32f722;
|
use stm32f7::stm32f722;
|
||||||
|
|
||||||
/// UART baud-rate configuration error.
|
/// UART baud-rate configuration error.
|
||||||
#[derive(Clone, Copy, Debug, Eq, PartialEq)]
|
#[derive(Clone, Copy, Debug, Eq, PartialEq, KaptanError)]
|
||||||
pub enum BaudRateError {
|
pub enum BaudRateError {
|
||||||
/// Zero is not a valid baud rate.
|
/// Zero is not a valid baud rate.
|
||||||
Zero,
|
Zero,
|
||||||
|
|||||||
@@ -5,6 +5,7 @@
|
|||||||
//! cannot be selected at construction.
|
//! cannot be selected at construction.
|
||||||
|
|
||||||
use kaptan_dma::{DmaReadableBuffer, DmaTxEngine, TxCompletion};
|
use kaptan_dma::{DmaReadableBuffer, DmaTxEngine, TxCompletion};
|
||||||
|
use kaptan_log::KaptanError;
|
||||||
use stm32f7::stm32f722;
|
use stm32f7::stm32f722;
|
||||||
|
|
||||||
use super::Usart1Tx;
|
use super::Usart1Tx;
|
||||||
@@ -20,7 +21,7 @@ pub struct Usart1TxDma {
|
|||||||
}
|
}
|
||||||
|
|
||||||
/// Failure to start one DMA frame transfer.
|
/// Failure to start one DMA frame transfer.
|
||||||
#[derive(Clone, Copy, Debug, Eq, PartialEq)]
|
#[derive(Clone, Copy, Debug, Eq, PartialEq, KaptanError)]
|
||||||
pub enum StartError {
|
pub enum StartError {
|
||||||
/// DMA cannot transfer an empty buffer or one exceeding the NDTR limit.
|
/// DMA cannot transfer an empty buffer or one exceeding the NDTR limit.
|
||||||
InvalidBufferLength,
|
InvalidBufferLength,
|
||||||
|
|||||||
@@ -5,6 +5,7 @@
|
|||||||
//! cannot be selected at construction.
|
//! cannot be selected at construction.
|
||||||
|
|
||||||
use kaptan_dma::{DmaReadableBuffer, DmaTxEngine, TxCompletion};
|
use kaptan_dma::{DmaReadableBuffer, DmaTxEngine, TxCompletion};
|
||||||
|
use kaptan_log::KaptanError;
|
||||||
use stm32f7::stm32f722;
|
use stm32f7::stm32f722;
|
||||||
|
|
||||||
use super::Usart2Tx;
|
use super::Usart2Tx;
|
||||||
@@ -15,7 +16,7 @@ pub struct Usart2TxDma {
|
|||||||
}
|
}
|
||||||
|
|
||||||
/// Failure to start one DMA frame transfer on USART2.
|
/// 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 {
|
pub enum Usart2TxDmaStartError {
|
||||||
/// DMA cannot transfer an empty buffer or one exceeding the NDTR limit.
|
/// DMA cannot transfer an empty buffer or one exceeding the NDTR limit.
|
||||||
InvalidBufferLength,
|
InvalidBufferLength,
|
||||||
|
|||||||
@@ -1,6 +1,7 @@
|
|||||||
//! Platform-owned F722 sensor-service assembly.
|
//! Platform-owned F722 sensor-service assembly.
|
||||||
|
|
||||||
use super::peripherals::{DwtMicrosecondClock, Spi1, Spi3};
|
use super::peripherals::{DwtMicrosecondClock, Spi1, Spi3};
|
||||||
|
use kaptan_board::ImuMount;
|
||||||
use kaptan_drivers::{
|
use kaptan_drivers::{
|
||||||
icm42688::{Icm42688, Icm42688Service, Icm42688ServiceEvent},
|
icm42688::{Icm42688, Icm42688Service, Icm42688ServiceEvent},
|
||||||
mpu6000::{
|
mpu6000::{
|
||||||
@@ -8,18 +9,15 @@ use kaptan_drivers::{
|
|||||||
Mpu6000Service, Mpu6000ServiceEvent,
|
Mpu6000Service, Mpu6000ServiceEvent,
|
||||||
},
|
},
|
||||||
};
|
};
|
||||||
|
use kaptan_log::{ErrorReport, StaticErrorMessage};
|
||||||
use kaptan_sensor::imu::ImuSnapshot;
|
use kaptan_sensor::imu::ImuSnapshot;
|
||||||
use kaptan_spi::SpiCompletion;
|
use kaptan_spi::SpiCompletion;
|
||||||
use stm32f7::stm32f722;
|
use stm32f7::stm32f722;
|
||||||
|
|
||||||
use super::routes::{Icm42688Spi3Route, Mpu6000Spi1Route};
|
use super::routes::{Icm42688Spi3Route, Mpu6000Spi1Route};
|
||||||
|
|
||||||
// TODO(configuration): this is an immutable board-development value, not an
|
// This is fixed driver setup for the selected board route. It is distinct from
|
||||||
// airframe calibration. A ground configuration flow must stage the complete
|
// the immutable board mount and from a future safety-gated calibration flow.
|
||||||
// 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.
|
|
||||||
const MPU6000_CONFIG: Mpu6000Config = Mpu6000Config {
|
const MPU6000_CONFIG: Mpu6000Config = Mpu6000Config {
|
||||||
accelerometer: AccelerometerRange::PlusMinus4G,
|
accelerometer: AccelerometerRange::PlusMinus4G,
|
||||||
gyroscope: GyroscopeRange::PlusMinus500DegreesPerSecond,
|
gyroscope: GyroscopeRange::PlusMinus500DegreesPerSecond,
|
||||||
@@ -33,7 +31,7 @@ pub(crate) enum ImuEvent {
|
|||||||
Idle,
|
Idle,
|
||||||
Initialized,
|
Initialized,
|
||||||
Sample(ImuSnapshot),
|
Sample(ImuSnapshot),
|
||||||
Fault,
|
Fault(&'static StaticErrorMessage),
|
||||||
}
|
}
|
||||||
|
|
||||||
/// Opaque owner of the selected F722 IMU service.
|
/// Opaque owner of the selected F722 IMU service.
|
||||||
@@ -43,6 +41,7 @@ pub(crate) enum ImuEvent {
|
|||||||
/// platform, preserving the board-facing abstraction boundary.
|
/// platform, preserving the board-facing abstraction boundary.
|
||||||
pub struct F722Imu {
|
pub struct F722Imu {
|
||||||
inner: F722ImuKind,
|
inner: F722ImuKind,
|
||||||
|
mount: ImuMount,
|
||||||
}
|
}
|
||||||
|
|
||||||
/// The two concrete IMU services supported by the F722 platform.
|
/// The two concrete IMU services supported by the F722 platform.
|
||||||
@@ -58,6 +57,7 @@ impl F722Imu {
|
|||||||
dma: stm32f722::DMA2,
|
dma: stm32f722::DMA2,
|
||||||
chip_select: stm32f722::GPIOB,
|
chip_select: stm32f722::GPIOB,
|
||||||
clock: DwtMicrosecondClock,
|
clock: DwtMicrosecondClock,
|
||||||
|
mount: ImuMount,
|
||||||
) -> Self {
|
) -> Self {
|
||||||
Self {
|
Self {
|
||||||
inner: F722ImuKind::Mpu6000(Mpu6000Service::new(
|
inner: F722ImuKind::Mpu6000(Mpu6000Service::new(
|
||||||
@@ -67,6 +67,7 @@ impl F722Imu {
|
|||||||
MPU6000_CONFIG,
|
MPU6000_CONFIG,
|
||||||
),
|
),
|
||||||
)),
|
)),
|
||||||
|
mount,
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -76,6 +77,7 @@ impl F722Imu {
|
|||||||
dma: stm32f722::DMA1,
|
dma: stm32f722::DMA1,
|
||||||
chip_select: &stm32f722::GPIOA,
|
chip_select: &stm32f722::GPIOA,
|
||||||
clock: DwtMicrosecondClock,
|
clock: DwtMicrosecondClock,
|
||||||
|
mount: ImuMount,
|
||||||
) -> Self {
|
) -> Self {
|
||||||
Self {
|
Self {
|
||||||
inner: F722ImuKind::Icm42688(Icm42688Service::new(
|
inner: F722ImuKind::Icm42688(Icm42688Service::new(
|
||||||
@@ -86,87 +88,104 @@ impl F722Imu {
|
|||||||
chip_select,
|
chip_select,
|
||||||
)),
|
)),
|
||||||
)),
|
)),
|
||||||
|
mount,
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
pub(crate) fn begin(&mut self) -> ImuEvent {
|
pub(crate) fn begin(&mut self) -> ImuEvent {
|
||||||
let ok = match &mut self.inner {
|
match &mut self.inner {
|
||||||
F722ImuKind::Mpu6000(imu) => imu.begin().is_ok(),
|
F722ImuKind::Mpu6000(imu) => match imu.begin() {
|
||||||
F722ImuKind::Icm42688(imu) => imu.begin().is_ok(),
|
Ok(()) => ImuEvent::Idle,
|
||||||
};
|
Err(error) => ImuEvent::Fault(error.error_message()),
|
||||||
if ok { ImuEvent::Idle } else { ImuEvent::Fault }
|
},
|
||||||
|
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 {
|
pub(crate) fn on_periodic_tick(&mut self) -> ImuEvent {
|
||||||
match &mut self.inner {
|
match &mut self.inner {
|
||||||
F722ImuKind::Mpu6000(imu) => mpu_event(imu.on_periodic_tick()),
|
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 {
|
pub(crate) fn on_data_ready(&mut self) -> ImuEvent {
|
||||||
match &mut self.inner {
|
match &mut self.inner {
|
||||||
F722ImuKind::Mpu6000(imu) => mpu_event(imu.on_data_ready()),
|
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 {
|
pub(crate) fn on_transfer_complete(&mut self, completion: SpiCompletion) -> ImuEvent {
|
||||||
match &mut self.inner {
|
match &mut self.inner {
|
||||||
F722ImuKind::Mpu6000(imu) => mpu_event(imu.on_transfer_complete(completion)),
|
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 {
|
match event {
|
||||||
Mpu6000ServiceEvent::Idle => ImuEvent::Idle,
|
Mpu6000ServiceEvent::Idle => ImuEvent::Idle,
|
||||||
Mpu6000ServiceEvent::Initialized => ImuEvent::Initialized,
|
Mpu6000ServiceEvent::Initialized => ImuEvent::Initialized,
|
||||||
Mpu6000ServiceEvent::Sample(sample) => ImuEvent::Sample(sample),
|
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 {
|
match event {
|
||||||
Icm42688ServiceEvent::Idle => ImuEvent::Idle,
|
Icm42688ServiceEvent::Idle => ImuEvent::Idle,
|
||||||
Icm42688ServiceEvent::Initialized => ImuEvent::Initialized,
|
Icm42688ServiceEvent::Initialized => ImuEvent::Initialized,
|
||||||
Icm42688ServiceEvent::Sample(sample) => ImuEvent::Sample(align_icm42688(sample)),
|
Icm42688ServiceEvent::Sample(sample) => ImuEvent::Sample(align_mount(sample, mount)),
|
||||||
Icm42688ServiceEvent::Fault(_) => ImuEvent::Fault,
|
Icm42688ServiceEvent::Fault(error) => ImuEvent::Fault(error.error_message()),
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
/// Aligns the development GEP-F722-HD V2 ICM-42688-P yaw-180° mounting to the
|
/// Applies the board-declared fixed IMU-to-body transform.
|
||||||
/// flight-controller body frame. The MPU6000 profile is already aligned.
|
|
||||||
///
|
///
|
||||||
/// TODO(configuration): replace this build-time transform only as part of the
|
/// This is immutable board metadata validated against the selected electrical
|
||||||
/// same atomic sensor-profile activation described at `MPU6000_CONFIG`; it is
|
/// route before initialization. It is not an airframe calibration setting.
|
||||||
/// not a user-adjustable live setting.
|
fn align_mount(mut sample: ImuSnapshot, mount: ImuMount) -> ImuSnapshot {
|
||||||
fn align_icm42688(mut sample: ImuSnapshot) -> ImuSnapshot {
|
match mount {
|
||||||
sample.acceleration_milligravity[0] = sample.acceleration_milligravity[0].saturating_neg();
|
ImuMount::BodyAligned => sample,
|
||||||
sample.acceleration_milligravity[1] = sample.acceleration_milligravity[1].saturating_neg();
|
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] =
|
||||||
sample.angular_rate_milliradians_per_second[0].saturating_neg();
|
sample.angular_rate_milliradians_per_second[0].saturating_neg();
|
||||||
sample.angular_rate_milliradians_per_second[1] =
|
sample.angular_rate_milliradians_per_second[1] =
|
||||||
sample.angular_rate_milliradians_per_second[1].saturating_neg();
|
sample.angular_rate_milliradians_per_second[1].saturating_neg();
|
||||||
sample
|
sample
|
||||||
}
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
#[cfg(test)]
|
#[cfg(test)]
|
||||||
mod tests {
|
mod tests {
|
||||||
use super::align_icm42688;
|
use super::align_mount;
|
||||||
|
use kaptan_board::ImuMount;
|
||||||
use kaptan_sensor::imu::ImuSnapshot;
|
use kaptan_sensor::imu::ImuSnapshot;
|
||||||
use kaptan_time::Timestamp;
|
use kaptan_time::Timestamp;
|
||||||
|
|
||||||
#[test]
|
#[test]
|
||||||
fn geprc_profile_applies_its_documented_yaw_alignment() {
|
fn geprc_profile_applies_its_documented_yaw_alignment() {
|
||||||
let sample = align_icm42688(ImuSnapshot::new(
|
let sample = align_mount(
|
||||||
|
ImuSnapshot::new(
|
||||||
Timestamp::from_microseconds(0),
|
Timestamp::from_microseconds(0),
|
||||||
[10, 20, 30],
|
[10, 20, 30],
|
||||||
[40, 50, 60],
|
[40, 50, 60],
|
||||||
0,
|
0,
|
||||||
));
|
),
|
||||||
|
ImuMount::Yaw180,
|
||||||
|
);
|
||||||
assert_eq!(sample.acceleration_milligravity, [-10, -20, 30]);
|
assert_eq!(sample.acceleration_milligravity, [-10, -20, 30]);
|
||||||
assert_eq!(sample.angular_rate_milliradians_per_second, [-40, -50, 60]);
|
assert_eq!(sample.angular_rate_milliradians_per_second, [-40, -50, 60]);
|
||||||
}
|
}
|
||||||
|
|||||||
@@ -10,40 +10,67 @@
|
|||||||
#![no_std]
|
#![no_std]
|
||||||
|
|
||||||
use core::marker::PhantomData;
|
use core::marker::PhantomData;
|
||||||
|
use kaptan_log::KaptanError;
|
||||||
|
|
||||||
pub use kaptan_board::BoardConfig;
|
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;
|
mod f722;
|
||||||
|
|
||||||
/// Dependencies used only while the platform expands its private RTIC macro.
|
/// Dependencies used only while the platform expands its private RTIC macro.
|
||||||
/// They stay crate-private: board binaries never name them.
|
/// 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) mod __platform_internal {
|
||||||
pub(crate) use cortex_m as cm;
|
pub(crate) use cortex_m as cm;
|
||||||
pub(crate) use cortex_m_rt as cmrt;
|
pub(crate) use cortex_m_rt as cmrt;
|
||||||
pub(crate) use kaptan_dma as dma;
|
pub(crate) use kaptan_dma as dma;
|
||||||
#[cfg(not(test))]
|
#[cfg(target_os = "none")]
|
||||||
pub(crate) use panic_halt as panic;
|
pub(crate) use panic_halt as panic;
|
||||||
pub(crate) use rtic as rtic_app;
|
pub(crate) use rtic as rtic_app;
|
||||||
pub(crate) use stm32f7 as pac;
|
pub(crate) use stm32f7 as pac;
|
||||||
#[cfg(test)]
|
#[cfg(not(target_os = "none"))]
|
||||||
pub(crate) mod panic {}
|
pub(crate) mod panic {}
|
||||||
}
|
}
|
||||||
|
|
||||||
/// Construction or validation failure for a declarative F722 board.
|
/// 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 {
|
pub enum BoardError {
|
||||||
/// Portable board metadata is self-inconsistent.
|
/// Portable board metadata is self-inconsistent.
|
||||||
InvalidConfig(BoardConfigError),
|
InvalidConfig(BoardConfigError),
|
||||||
/// Metadata and the selected vertical-sensor route disagree.
|
/// Metadata and the selected vertical-sensor route disagree.
|
||||||
VerticalSensorCapabilityMismatch,
|
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 declaration requests a service-timer frequency not implemented by
|
||||||
/// the selected F722 platform profile.
|
/// the selected F722 platform profile.
|
||||||
UnsupportedServiceFrequency,
|
UnsupportedServiceFrequency,
|
||||||
|
/// The selected baud cannot carry the fixed telemetry/log schedule.
|
||||||
|
TelemetryLinkOvercommitted,
|
||||||
/// The platform could not establish a clock, peripheral, or DMA owner.
|
/// The platform could not establish a clock, peripheral, or DMA owner.
|
||||||
Initialization,
|
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.
|
/// MPU6000 component selector.
|
||||||
#[derive(Clone, Copy, Debug, Eq, PartialEq)]
|
#[derive(Clone, Copy, Debug, Eq, PartialEq)]
|
||||||
pub struct Mpu6000;
|
pub struct Mpu6000;
|
||||||
@@ -235,9 +262,31 @@ pub struct F722BoardRoutes {
|
|||||||
}
|
}
|
||||||
|
|
||||||
impl F722BoardRoutes {
|
impl F722BoardRoutes {
|
||||||
|
#[cfg_attr(not(target_os = "none"), allow(dead_code))]
|
||||||
pub(crate) const fn profile(self) -> F722Profile {
|
pub(crate) const fn profile(self) -> F722Profile {
|
||||||
self.profile
|
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 {
|
macro_rules! impl_mpu6000_routes {
|
||||||
@@ -303,17 +352,6 @@ impl_icm42688_routes!(LidarLiteV3I2c1, f722::VerticalRoute::LidarLiteV3);
|
|||||||
/// .build();
|
/// .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.
|
/// Contract implemented by a declarative STM32F722 board crate.
|
||||||
pub trait F7Board {
|
pub trait F7Board {
|
||||||
/// Portable identity, capabilities and outbound data-channel policy.
|
/// 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.
|
/// Link anchor for the platform-owned F722 board-I/O application.
|
||||||
#[doc(hidden)]
|
#[doc(hidden)]
|
||||||
pub fn firmware_anchor() {}
|
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 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 {
|
fn platform_source(relative: &str) -> String {
|
||||||
let root = PathBuf::from(env!("CARGO_MANIFEST_DIR"));
|
let root = PathBuf::from(env!("CARGO_MANIFEST_DIR"));
|
||||||
fs::read_to_string(root.join(relative)).expect("platform source must exist")
|
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")
|
.find("motor_inhibit::inhibit_motor_pins")
|
||||||
.expect("F722 initialization must claim the former motor pins first");
|
.expect("F722 initialization must claim the former motor pins first");
|
||||||
let validation = module
|
let validation = module
|
||||||
.find("config.validate()")
|
.find("validate_board_config")
|
||||||
.expect("F722 initialization must validate its board configuration");
|
.expect("F722 initialization must validate its board configuration");
|
||||||
let board_initialization = module
|
let board_initialization = module
|
||||||
.find("board::initialize")
|
.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]
|
#[test]
|
||||||
fn telemetry_platform_has_no_physical_actuator_peripheral_path() {
|
fn telemetry_platform_has_no_physical_actuator_peripheral_path() {
|
||||||
let sources = [
|
let sources = [
|
||||||
@@ -117,3 +209,38 @@ fn telemetry_has_no_inbound_command_authority() {
|
|||||||
"telemetry must not schedule a worker from an inbound command"
|
"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