feat(firmware): validate configuration and migrate sensor-driven flight APIs
Validate configuration through its owning schemas and keep board declarations separate from shared F722 resource ownership. Bound USB transitions, isolate transport failures and retain restart-only activation. Migrate navigation and flight consumers with the sensor and schema contracts. Require explicit installation data and synchronous output authority; board motor outputs remain inhibited. Validation: assembled-prefix host tests, strict Clippy, platform/USB tests and both ARM release builds. Final-source coverage and scoped Miri/Kani evidence are retained separately from hardware qualification.
This commit is contained in:
Onur Özkan committed
1 parent
fb1b8af2d1
commit
f9a61a086f
211 files changed
+45657
-12740
No files matched your search
@@ -9,6 +9,7 @@ version.workspace = true
|
||||
workspace = true
|
||||
|
||||
[dependencies]
|
||||
critical-section = "1.2"
|
||||
kaptan-err-reportable = { path = "../kaptan-err-reportable" }
|
||||
kaptan-mavlink = { path = "../kaptan-mavlink", default-features = false, features = [
|
||||
"sensor-telemetry",
|
||||
@@ -17,3 +18,6 @@ kaptan-sensor = { path = "../kaptan-sensor" }
|
||||
kaptan-time = { path = "../kaptan-time" }
|
||||
kaptan-tx = { path = "../kaptan-tx" }
|
||||
mavlink-core = { version = "0.18", default-features = false }
|
||||
|
||||
[target.'cfg(not(target_os = "none"))'.dev-dependencies]
|
||||
critical-section = { version = "1.2", features = ["std"] }
|
||||
@@ -8,19 +8,19 @@
|
||||
|
||||
use kaptan_err_reportable::KaptanError;
|
||||
use kaptan_mavlink::{MavlinkFrame, encoder::MavlinkEncoder, generated::kaptan::MavMessage};
|
||||
use kaptan_sensor::{optical_flow::OpticalFlowSnapshot, range::RangeSnapshot};
|
||||
use kaptan_sensor::{imu::ImuSnapshot, optical_flow::OpticalFlowSnapshot, range::RangeSnapshot};
|
||||
use kaptan_time::Timestamp;
|
||||
use kaptan_tx::{FrameClass, PendingFrame};
|
||||
use mavlink_core::MessageData;
|
||||
|
||||
mod mailbox;
|
||||
|
||||
pub use mailbox::{LatestImuMailbox, LatestOpticalFlowMailbox, LatestRangeMailbox};
|
||||
pub use mailbox::{
|
||||
Consumer, LatestImuMailbox, LatestMailbox, LatestOpticalFlowMailbox, LatestRangeMailbox,
|
||||
Producer,
|
||||
};
|
||||
|
||||
/// Validated publication schedule consumed by the outbound data channel.
|
||||
///
|
||||
/// This is a derived runtime input, not a persisted configuration root. Board
|
||||
/// packages own the factory values and the platform constructs this view only
|
||||
/// after validating the active user configuration against immutable hardware.
|
||||
/// Publication rates in hertz.
|
||||
#[derive(Clone, Copy, Debug, Eq, PartialEq)]
|
||||
pub struct DataChannelRates {
|
||||
/// IMU publication rate.
|
||||
@@ -33,15 +33,55 @@ pub struct DataChannelRates {
|
||||
pub heartbeat_hz: usize,
|
||||
}
|
||||
|
||||
/// Mailboxes read by one data-channel worker.
|
||||
#[derive(Clone, Copy)]
|
||||
/// Mailboxes read by the single telemetry collector.
|
||||
pub struct DataSources<'a> {
|
||||
/// Latest IMU observation from the acquisition worker.
|
||||
pub imu: &'a LatestImuMailbox,
|
||||
pub imu: Consumer<'a, ImuSnapshot>,
|
||||
/// Latest range observation from either supported vertical route.
|
||||
pub range: &'a LatestRangeMailbox,
|
||||
pub range: Consumer<'a, RangeSnapshot>,
|
||||
/// Latest optical-flow observation from MTF-01.
|
||||
pub optical_flow: &'a LatestOpticalFlowMailbox,
|
||||
pub optical_flow: Consumer<'a, OpticalFlowSnapshot>,
|
||||
}
|
||||
|
||||
/// Latest collected observations shared by any number of outbound links.
|
||||
#[derive(Clone, Copy, Debug, Default, Eq, PartialEq)]
|
||||
pub struct TelemetrySamples {
|
||||
/// Latest IMU observation.
|
||||
pub imu: Option<ImuSnapshot>,
|
||||
/// Latest range observation.
|
||||
pub range: Option<RangeSnapshot>,
|
||||
/// Latest optical-flow observation.
|
||||
pub optical_flow: Option<OpticalFlowSnapshot>,
|
||||
}
|
||||
|
||||
/// Sole consumer of a set of SPSC mailboxes, retaining their latest values.
|
||||
pub struct DataCollector<'a> {
|
||||
sources: DataSources<'a>,
|
||||
samples: TelemetrySamples,
|
||||
}
|
||||
|
||||
impl<'a> DataCollector<'a> {
|
||||
/// Creates the single collector for these mailboxes.
|
||||
pub fn new(sources: DataSources<'a>) -> Self {
|
||||
Self {
|
||||
sources,
|
||||
samples: TelemetrySamples::default(),
|
||||
}
|
||||
}
|
||||
|
||||
/// Collects each mailbox once, retaining values without a newer publication.
|
||||
pub fn snapshot(&mut self) -> TelemetrySamples {
|
||||
if let Some(imu) = self.sources.imu.take() {
|
||||
self.samples.imu = Some(imu);
|
||||
}
|
||||
if let Some(range) = self.sources.range.take() {
|
||||
self.samples.range = Some(range);
|
||||
}
|
||||
if let Some(flow) = self.sources.optical_flow.take() {
|
||||
self.samples.optical_flow = Some(flow);
|
||||
}
|
||||
self.samples
|
||||
}
|
||||
}
|
||||
|
||||
/// Data-channel construction failure.
|
||||
@@ -59,80 +99,70 @@ pub enum DataChannelError {
|
||||
}
|
||||
|
||||
/// Scheduled outbound MAVLink frame producer.
|
||||
pub struct DataChannel<'a, const SERVICE_HZ: usize> {
|
||||
pub struct DataChannel<const SERVICE_HZ: usize> {
|
||||
encoder: MavlinkEncoder,
|
||||
schedule: Schedule<SERVICE_HZ>,
|
||||
sources: DataSources<'a>,
|
||||
maximum_vertical_age_ticks: u8,
|
||||
observed_imu: u32,
|
||||
observed_range: u32,
|
||||
last_range: Option<RangeSnapshot>,
|
||||
range_age_ticks: u8,
|
||||
observed_optical_flow: u32,
|
||||
last_optical_flow: Option<OpticalFlowSnapshot>,
|
||||
optical_flow_age_ticks: u8,
|
||||
maximum_vertical_age_microseconds: u64,
|
||||
last_imu_timestamp: Option<Timestamp>,
|
||||
}
|
||||
|
||||
impl<'a, const SERVICE_HZ: usize> DataChannel<'a, SERVICE_HZ> {
|
||||
impl<const SERVICE_HZ: usize> DataChannel<SERVICE_HZ> {
|
||||
/// Creates an outbound data flow from already-validated runtime inputs.
|
||||
pub fn new(
|
||||
system_id: u8,
|
||||
component_id: u8,
|
||||
rates: DataChannelRates,
|
||||
maximum_vertical_age_ticks: u8,
|
||||
sources: DataSources<'a>,
|
||||
maximum_vertical_age_microseconds: u64,
|
||||
) -> Result<Self, DataChannelError> {
|
||||
let schedule = Schedule::from_rates(rates)?;
|
||||
Ok(Self {
|
||||
encoder: MavlinkEncoder::new(system_id, component_id),
|
||||
schedule,
|
||||
sources,
|
||||
maximum_vertical_age_ticks,
|
||||
observed_imu: 0,
|
||||
observed_range: 0,
|
||||
last_range: None,
|
||||
range_age_ticks: u8::MAX,
|
||||
observed_optical_flow: 0,
|
||||
last_optical_flow: None,
|
||||
optical_flow_age_ticks: u8::MAX,
|
||||
maximum_vertical_age_microseconds,
|
||||
last_imu_timestamp: None,
|
||||
})
|
||||
}
|
||||
|
||||
/// Runs all outbound work due for one platform service tick.
|
||||
///
|
||||
/// At most one classified frame is returned for transport admission.
|
||||
pub fn on_service_tick(&mut self) -> Option<PendingFrame<MavlinkFrame>> {
|
||||
self.refresh_vertical_data();
|
||||
|
||||
/// IMU timestamps must increase; recreate the channel for a new clock epoch.
|
||||
pub fn on_service_tick(
|
||||
&mut self,
|
||||
samples: &TelemetrySamples,
|
||||
now: Timestamp,
|
||||
) -> Option<PendingFrame<MavlinkFrame>> {
|
||||
match self.schedule.next_due() {
|
||||
Some(DueData::Imu) => {
|
||||
self.sources
|
||||
.imu
|
||||
.take_newer(&mut self.observed_imu)
|
||||
.map(|sample| PendingFrame {
|
||||
class: FrameClass::BestEffort,
|
||||
frame: self.encoder.imu_snapshot(&sample),
|
||||
})
|
||||
let sample = samples.imu.filter(|sample| {
|
||||
self.last_imu_timestamp
|
||||
.is_none_or(|last| sample.timestamp > last)
|
||||
})?;
|
||||
self.last_imu_timestamp = Some(sample.timestamp);
|
||||
Some(PendingFrame {
|
||||
class: FrameClass::BestEffort,
|
||||
frame: self.encoder.imu_snapshot(&sample),
|
||||
})
|
||||
}
|
||||
Some(DueData::Distance) if self.range_age_ticks <= self.maximum_vertical_age_ticks => {
|
||||
self.last_range.map(|range| PendingFrame {
|
||||
Some(DueData::Distance) => samples
|
||||
.range
|
||||
.filter(|range| self.vertical_is_fresh(range.timestamp, now))
|
||||
.map(|range| PendingFrame {
|
||||
class: FrameClass::BestEffort,
|
||||
frame: self.encoder.range_snapshot(&range),
|
||||
})
|
||||
}
|
||||
Some(DueData::OpticalFlow)
|
||||
if self.optical_flow_age_ticks <= self.maximum_vertical_age_ticks =>
|
||||
{
|
||||
self.last_optical_flow.map(|flow| PendingFrame {
|
||||
}),
|
||||
Some(DueData::OpticalFlow) => samples
|
||||
.optical_flow
|
||||
.filter(|flow| self.vertical_is_fresh(flow.timestamp, now))
|
||||
.map(|flow| PendingFrame {
|
||||
class: FrameClass::BestEffort,
|
||||
frame: self.encoder.optical_flow_snapshot(&flow),
|
||||
})
|
||||
}
|
||||
}),
|
||||
Some(DueData::Heartbeat) => Some(PendingFrame {
|
||||
class: FrameClass::Vital,
|
||||
frame: self.encoder.heartbeat(),
|
||||
}),
|
||||
Some(DueData::Distance | DueData::OpticalFlow) | None => None,
|
||||
None => None,
|
||||
}
|
||||
}
|
||||
|
||||
@@ -149,23 +179,10 @@ impl<'a, const SERVICE_HZ: usize> DataChannel<'a, SERVICE_HZ> {
|
||||
}
|
||||
}
|
||||
|
||||
fn refresh_vertical_data(&mut self) {
|
||||
if let Some(range) = self.sources.range.take_newer(&mut self.observed_range) {
|
||||
self.last_range = Some(range);
|
||||
self.range_age_ticks = 0;
|
||||
} else {
|
||||
self.range_age_ticks = self.range_age_ticks.saturating_add(1);
|
||||
}
|
||||
if let Some(flow) = self
|
||||
.sources
|
||||
.optical_flow
|
||||
.take_newer(&mut self.observed_optical_flow)
|
||||
{
|
||||
self.last_optical_flow = Some(flow);
|
||||
self.optical_flow_age_ticks = 0;
|
||||
} else {
|
||||
self.optical_flow_age_ticks = self.optical_flow_age_ticks.saturating_add(1);
|
||||
}
|
||||
fn vertical_is_fresh(&self, timestamp: Timestamp, now: Timestamp) -> bool {
|
||||
now.as_microseconds()
|
||||
.checked_sub(timestamp.as_microseconds())
|
||||
.is_some_and(|age| age <= self.maximum_vertical_age_microseconds)
|
||||
}
|
||||
}
|
||||
|
||||
@@ -245,70 +262,4 @@ impl<const SERVICE_HZ: usize> Schedule<SERVICE_HZ> {
|
||||
}
|
||||
|
||||
#[cfg(test)]
|
||||
mod tests {
|
||||
use super::{
|
||||
DataChannel, DataChannelRates, DataSources, LatestImuMailbox, LatestOpticalFlowMailbox,
|
||||
LatestRangeMailbox,
|
||||
};
|
||||
use kaptan_mavlink::generated::kaptan::MavMessage;
|
||||
use kaptan_sensor::{
|
||||
imu::ImuSnapshot, optical_flow::OpticalFlowSnapshot, range::RangeSnapshot,
|
||||
};
|
||||
use kaptan_time::Timestamp;
|
||||
use kaptan_tx::FrameClass;
|
||||
|
||||
const RATES: DataChannelRates = DataChannelRates {
|
||||
imu_hz: 20,
|
||||
range_hz: Some(20),
|
||||
optical_flow_hz: Some(20),
|
||||
heartbeat_hz: 2,
|
||||
};
|
||||
|
||||
#[test]
|
||||
fn owns_rate_selection_encoding_and_classification() {
|
||||
let imu = LatestImuMailbox::new();
|
||||
let range = LatestRangeMailbox::new();
|
||||
let flow = LatestOpticalFlowMailbox::new();
|
||||
let mut channel: DataChannel<'_, 100> = DataChannel::new(
|
||||
1,
|
||||
1,
|
||||
RATES,
|
||||
10,
|
||||
DataSources {
|
||||
imu: &imu,
|
||||
range: &range,
|
||||
optical_flow: &flow,
|
||||
},
|
||||
)
|
||||
.unwrap();
|
||||
let mut best_effort = 0;
|
||||
let mut vital = 0;
|
||||
|
||||
for tick in 0..100 {
|
||||
imu.publish(ImuSnapshot::new(
|
||||
Timestamp::from_microseconds(tick),
|
||||
[0; 3],
|
||||
[0; 3],
|
||||
0,
|
||||
));
|
||||
range.publish(RangeSnapshot::new(Timestamp::from_microseconds(tick), 100));
|
||||
flow.publish(OpticalFlowSnapshot::new(
|
||||
Timestamp::from_microseconds(tick),
|
||||
[0; 2],
|
||||
100,
|
||||
100,
|
||||
));
|
||||
if let Some(frame) = channel.on_service_tick() {
|
||||
assert!(frame.frame.has_valid_crc::<MavMessage>());
|
||||
assert_eq!(frame.frame.sequence(), best_effort + vital);
|
||||
match frame.class {
|
||||
FrameClass::Vital => vital += 1,
|
||||
FrameClass::BestEffort => best_effort += 1,
|
||||
FrameClass::Critical => unreachable!(),
|
||||
}
|
||||
}
|
||||
}
|
||||
assert_eq!(best_effort, 60);
|
||||
assert_eq!(vital, 2);
|
||||
}
|
||||
}
|
||||
mod tests;
|
||||
@@ -1,227 +1,220 @@
|
||||
//! Lock-free single-producer, single-consumer latest-value handoff.
|
||||
|
||||
use core::{
|
||||
marker::PhantomData,
|
||||
sync::atomic::{AtomicU32, Ordering},
|
||||
};
|
||||
//! Single-producer, single-consumer latest-value handoff.
|
||||
|
||||
use core::cell::Cell;
|
||||
use critical_section::Mutex;
|
||||
use kaptan_sensor::{imu::ImuSnapshot, optical_flow::OpticalFlowSnapshot, range::RangeSnapshot};
|
||||
use kaptan_time::Timestamp;
|
||||
|
||||
/// Converts an observation to the atomic word representation used by a
|
||||
/// [`LatestMailbox`].
|
||||
/// Fixed-size latest-value mailbox with one producer and one consumer.
|
||||
///
|
||||
/// This is a data-channel handoff contract, not a sensor-domain contract:
|
||||
/// sensor crates own their fields and units, while this crate owns their
|
||||
/// lock-free representation between an acquisition producer and consumer.
|
||||
pub trait MailboxValue<const WORDS: usize>: Copy {
|
||||
/// Converts this observation into its handoff representation.
|
||||
fn into_words(self) -> [u32; WORDS];
|
||||
|
||||
/// Reconstructs an observation from its handoff representation.
|
||||
fn from_words(words: [u32; WORDS]) -> Self;
|
||||
/// A publication replaces any unread value. Each operation acquires the platform
|
||||
/// critical section only to copy or take an `Option<T>`. There are no callbacks,
|
||||
/// allocation, retries, or waits inside that section. The platform must supply a
|
||||
/// critical-section implementation that excludes every participating context.
|
||||
/// On a host this requires the `critical-section` crate's `std` backend.
|
||||
///
|
||||
/// Storage is borrowed by the endpoints and cannot move while they are in use.
|
||||
/// Dropping or forgetting an endpoint does not leave a lock held. After both
|
||||
/// borrows end the mailbox can be split again, retaining any unread value.
|
||||
///
|
||||
/// ```
|
||||
/// use kaptan_data_channel::LatestMailbox;
|
||||
/// let mut mailbox = LatestMailbox::new();
|
||||
/// let (mut producer, mut consumer) = mailbox.split();
|
||||
/// producer.publish([1, 2, 3]);
|
||||
/// producer.publish([4, 5, 6]);
|
||||
/// assert_eq!(consumer.take(), Some([4, 5, 6]));
|
||||
/// assert_eq!(consumer.take(), None);
|
||||
/// ```
|
||||
pub struct LatestMailbox<T: Copy> {
|
||||
slot: Mutex<Cell<Option<T>>>,
|
||||
}
|
||||
|
||||
/// Lock-free latest-value SPSC mailbox.
|
||||
///
|
||||
/// Publishing a newer value intentionally replaces an unread older value: the
|
||||
/// output path is best-effort and must never delay acquisition. The sequence
|
||||
/// counter is a seqlock; odd values indicate a writer and even values identify
|
||||
/// a stable snapshot. Exactly one producer and one consumer may use an
|
||||
/// instance concurrently. Sequence operations are sequentially consistent so
|
||||
/// a reader cannot validate an old even sequence around concurrently written
|
||||
/// atomic payload words; the payload words themselves need only be relaxed.
|
||||
pub struct LatestMailbox<Value, const WORDS: usize> {
|
||||
sequence: AtomicU32,
|
||||
words: [AtomicU32; WORDS],
|
||||
value: PhantomData<Value>,
|
||||
}
|
||||
|
||||
impl<Value: MailboxValue<WORDS>, const WORDS: usize> LatestMailbox<Value, WORDS> {
|
||||
/// Creates an empty mailbox.
|
||||
impl<T: Copy> LatestMailbox<T> {
|
||||
/// Creates an empty mailbox without acquiring a critical section.
|
||||
pub const fn new() -> Self {
|
||||
Self {
|
||||
sequence: AtomicU32::new(0),
|
||||
words: [const { AtomicU32::new(0) }; WORDS],
|
||||
value: PhantomData,
|
||||
slot: Mutex::new(Cell::new(None)),
|
||||
}
|
||||
}
|
||||
|
||||
/// Publishes a snapshot, replacing any unread older snapshot.
|
||||
pub fn publish(&self, value: Value) {
|
||||
let sequence = self.sequence.fetch_add(1, Ordering::SeqCst);
|
||||
debug_assert_eq!(sequence & 1, 0, "SPSC producer must not reenter");
|
||||
|
||||
for (destination, word) in self.words.iter().zip(value.into_words()) {
|
||||
destination.store(word, Ordering::Relaxed);
|
||||
}
|
||||
|
||||
self.sequence
|
||||
.store(sequence.wrapping_add(2), Ordering::SeqCst);
|
||||
}
|
||||
|
||||
/// Returns the newest stable snapshot different from `observed_sequence`.
|
||||
pub fn take_newer(&self, observed_sequence: &mut u32) -> Option<Value> {
|
||||
let before = self.sequence.load(Ordering::SeqCst);
|
||||
if before == *observed_sequence || before & 1 != 0 {
|
||||
return None;
|
||||
}
|
||||
|
||||
let mut words = [0; WORDS];
|
||||
for (destination, source) in words.iter_mut().zip(&self.words) {
|
||||
*destination = source.load(Ordering::Relaxed);
|
||||
}
|
||||
|
||||
if self.sequence.load(Ordering::SeqCst) != before {
|
||||
return None;
|
||||
}
|
||||
|
||||
*observed_sequence = before;
|
||||
Some(Value::from_words(words))
|
||||
/// Borrows the mailbox as exactly one producer and one consumer.
|
||||
///
|
||||
/// A second split cannot overlap either endpoint's use:
|
||||
///
|
||||
/// ```compile_fail,E0499
|
||||
/// use kaptan_data_channel::LatestMailbox;
|
||||
/// let mut mailbox = LatestMailbox::<u32>::new();
|
||||
/// let (mut producer, _consumer) = mailbox.split();
|
||||
/// let (mut duplicate, _) = mailbox.split();
|
||||
/// producer.publish(1);
|
||||
/// duplicate.publish(2);
|
||||
/// ```
|
||||
pub fn split(&mut self) -> (Producer<'_, T>, Consumer<'_, T>) {
|
||||
(Producer { mailbox: self }, Consumer { mailbox: self })
|
||||
}
|
||||
}
|
||||
|
||||
impl<Value: MailboxValue<WORDS>, const WORDS: usize> Default for LatestMailbox<Value, WORDS> {
|
||||
impl<T: Copy> Default for LatestMailbox<T> {
|
||||
fn default() -> Self {
|
||||
Self::new()
|
||||
}
|
||||
}
|
||||
|
||||
/// Exclusive publication endpoint. Moving it transfers publication ownership.
|
||||
///
|
||||
/// ```compile_fail,E0599
|
||||
/// use kaptan_data_channel::LatestMailbox;
|
||||
/// let mut mailbox = LatestMailbox::<u32>::new();
|
||||
/// let (producer, _) = mailbox.split();
|
||||
/// let duplicate = producer.clone();
|
||||
/// ```
|
||||
pub struct Producer<'a, T: Copy> {
|
||||
mailbox: &'a LatestMailbox<T>,
|
||||
}
|
||||
|
||||
impl<T: Copy> Producer<'_, T> {
|
||||
/// Publishes a complete value, replacing any unread older value.
|
||||
///
|
||||
/// ```compile_fail,E0596
|
||||
/// use kaptan_data_channel::LatestMailbox;
|
||||
/// let mut mailbox = LatestMailbox::<u32>::new();
|
||||
/// let (producer, _) = mailbox.split();
|
||||
/// let shared = &producer;
|
||||
/// shared.publish(1);
|
||||
/// ```
|
||||
pub fn publish(&mut self, value: T) {
|
||||
critical_section::with(|cs| self.mailbox.slot.borrow(cs).set(Some(value)));
|
||||
}
|
||||
}
|
||||
|
||||
/// Exclusive consumption endpoint. Values are removed at most once.
|
||||
///
|
||||
/// ```compile_fail,E0599
|
||||
/// use kaptan_data_channel::LatestMailbox;
|
||||
/// let mut mailbox = LatestMailbox::<u32>::new();
|
||||
/// let (_, consumer) = mailbox.split();
|
||||
/// let duplicate = consumer.clone();
|
||||
/// ```
|
||||
pub struct Consumer<'a, T: Copy> {
|
||||
mailbox: &'a LatestMailbox<T>,
|
||||
}
|
||||
|
||||
impl<T: Copy> Consumer<'_, T> {
|
||||
/// Takes the latest unread value, or `None` if no publication is pending.
|
||||
pub fn take(&mut self) -> Option<T> {
|
||||
critical_section::with(|cs| self.mailbox.slot.borrow(cs).take())
|
||||
}
|
||||
}
|
||||
|
||||
/// Latest-value mailbox for IMU data-channel snapshots.
|
||||
pub type LatestImuMailbox = LatestMailbox<ImuSnapshot, 9>;
|
||||
pub type LatestImuMailbox = LatestMailbox<ImuSnapshot>;
|
||||
/// Latest-value mailbox for range data-channel snapshots.
|
||||
pub type LatestRangeMailbox = LatestMailbox<RangeSnapshot, 3>;
|
||||
pub type LatestRangeMailbox = LatestMailbox<RangeSnapshot>;
|
||||
/// Latest-value mailbox for optical-flow data-channel snapshots.
|
||||
pub type LatestOpticalFlowMailbox = LatestMailbox<OpticalFlowSnapshot, 6>;
|
||||
|
||||
impl MailboxValue<9> for ImuSnapshot {
|
||||
fn into_words(self) -> [u32; 9] {
|
||||
let timestamp = self.timestamp.as_microseconds();
|
||||
[
|
||||
timestamp as u32,
|
||||
(timestamp >> 32) as u32,
|
||||
self.acceleration_milligravity[0] as u16 as u32,
|
||||
self.acceleration_milligravity[1] as u16 as u32,
|
||||
self.acceleration_milligravity[2] as u16 as u32,
|
||||
self.angular_rate_milliradians_per_second[0] as u16 as u32,
|
||||
self.angular_rate_milliradians_per_second[1] as u16 as u32,
|
||||
self.angular_rate_milliradians_per_second[2] as u16 as u32,
|
||||
self.temperature_centidegrees_celsius as u16 as u32,
|
||||
]
|
||||
}
|
||||
|
||||
fn from_words(words: [u32; 9]) -> Self {
|
||||
Self::new(
|
||||
timestamp_from_words(words[0], words[1]),
|
||||
[
|
||||
words[2] as u16 as i16,
|
||||
words[3] as u16 as i16,
|
||||
words[4] as u16 as i16,
|
||||
],
|
||||
[
|
||||
words[5] as u16 as i16,
|
||||
words[6] as u16 as i16,
|
||||
words[7] as u16 as i16,
|
||||
],
|
||||
words[8] as u16 as i16,
|
||||
)
|
||||
}
|
||||
}
|
||||
|
||||
impl MailboxValue<3> for RangeSnapshot {
|
||||
fn into_words(self) -> [u32; 3] {
|
||||
let timestamp = self.timestamp.as_microseconds();
|
||||
[
|
||||
timestamp as u32,
|
||||
(timestamp >> 32) as u32,
|
||||
self.distance_millimetres,
|
||||
]
|
||||
}
|
||||
|
||||
fn from_words(words: [u32; 3]) -> Self {
|
||||
Self::new(timestamp_from_words(words[0], words[1]), words[2])
|
||||
}
|
||||
}
|
||||
|
||||
impl MailboxValue<6> for OpticalFlowSnapshot {
|
||||
fn into_words(self) -> [u32; 6] {
|
||||
let timestamp = self.timestamp.as_microseconds();
|
||||
[
|
||||
timestamp as u32,
|
||||
(timestamp >> 32) as u32,
|
||||
self.velocity_millimetres_per_second[0] as u32,
|
||||
self.velocity_millimetres_per_second[1] as u32,
|
||||
self.ground_distance_millimetres,
|
||||
u32::from(self.quality),
|
||||
]
|
||||
}
|
||||
|
||||
fn from_words(words: [u32; 6]) -> Self {
|
||||
Self::new(
|
||||
timestamp_from_words(words[0], words[1]),
|
||||
[words[2] as i32, words[3] as i32],
|
||||
words[4],
|
||||
words[5] as u8,
|
||||
)
|
||||
}
|
||||
}
|
||||
|
||||
const fn timestamp_from_words(low: u32, high: u32) -> Timestamp {
|
||||
Timestamp::from_microseconds((low as u64) | ((high as u64) << 32))
|
||||
}
|
||||
pub type LatestOpticalFlowMailbox = LatestMailbox<OpticalFlowSnapshot>;
|
||||
|
||||
#[cfg(test)]
|
||||
mod tests {
|
||||
use super::{LatestImuMailbox, LatestOpticalFlowMailbox, LatestRangeMailbox};
|
||||
use kaptan_sensor::{
|
||||
imu::ImuSnapshot, optical_flow::OpticalFlowSnapshot, range::RangeSnapshot,
|
||||
};
|
||||
extern crate std;
|
||||
|
||||
use super::*;
|
||||
use kaptan_time::Timestamp;
|
||||
use std::thread;
|
||||
|
||||
#[test]
|
||||
fn publishes_only_the_newest_unread_imu_sample() {
|
||||
let mailbox = LatestImuMailbox::new();
|
||||
let mut observed = 0;
|
||||
mailbox.publish(ImuSnapshot::new(
|
||||
Timestamp::from_microseconds(1),
|
||||
[1; 3],
|
||||
[2; 3],
|
||||
3,
|
||||
));
|
||||
mailbox.publish(ImuSnapshot::new(
|
||||
Timestamp::from_microseconds(2),
|
||||
[4; 3],
|
||||
[5; 3],
|
||||
6,
|
||||
));
|
||||
|
||||
let sample = mailbox.take_newer(&mut observed).unwrap();
|
||||
assert_eq!(sample.timestamp, Timestamp::from_microseconds(2));
|
||||
assert_eq!(sample.acceleration_milligravity, [4; 3]);
|
||||
assert!(mailbox.take_newer(&mut observed).is_none());
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn publishes_only_the_newest_unread_range_sample() {
|
||||
let mailbox = LatestRangeMailbox::new();
|
||||
let mut observed = 0;
|
||||
mailbox.publish(RangeSnapshot::new(Timestamp::from_microseconds(1), 100));
|
||||
mailbox.publish(RangeSnapshot::new(Timestamp::from_microseconds(2), 200));
|
||||
|
||||
assert_eq!(
|
||||
mailbox.take_newer(&mut observed),
|
||||
Some(RangeSnapshot::new(Timestamp::from_microseconds(2), 200))
|
||||
fn latest_publication_overwrites_and_is_taken_once() {
|
||||
let mut mailbox = LatestImuMailbox::new();
|
||||
let (mut producer, mut consumer) = mailbox.split();
|
||||
assert_eq!(consumer.take(), None);
|
||||
let first = ImuSnapshot::new(Timestamp::from_microseconds(1), [1; 3], [2; 3], 3);
|
||||
let latest = ImuSnapshot::new(
|
||||
Timestamp::from_microseconds(u64::MAX),
|
||||
[-32_768, 0, 32_767],
|
||||
[-34_906, 0, 34_906],
|
||||
-2_501,
|
||||
);
|
||||
assert!(mailbox.take_newer(&mut observed).is_none());
|
||||
producer.publish(first);
|
||||
producer.publish(latest);
|
||||
assert_eq!(consumer.take(), Some(latest));
|
||||
assert_eq!(consumer.take(), None);
|
||||
producer.publish(latest);
|
||||
assert_eq!(consumer.take(), Some(latest));
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn preserves_optical_flow_fields() {
|
||||
let mailbox = LatestOpticalFlowMailbox::new();
|
||||
let mut observed = 0;
|
||||
let sample = OpticalFlowSnapshot::new(Timestamp::from_microseconds(1), [-6, 7], 8, 9);
|
||||
mailbox.publish(sample);
|
||||
fn vertical_snapshots_preserve_all_fields() {
|
||||
let mut range = LatestRangeMailbox::new();
|
||||
let mut flow = LatestOpticalFlowMailbox::new();
|
||||
let (mut range_producer, mut range_consumer) = range.split();
|
||||
let (mut flow_producer, mut flow_consumer) = flow.split();
|
||||
let timestamp = Timestamp::from_microseconds(u64::MAX);
|
||||
let range = RangeSnapshot::new(timestamp, u32::MAX);
|
||||
let flow = OpticalFlowSnapshot::new(timestamp, [i32::MIN, i32::MAX], u32::MAX, 255);
|
||||
range_producer.publish(range);
|
||||
flow_producer.publish(flow);
|
||||
assert_eq!(range_consumer.take(), Some(range));
|
||||
assert_eq!(flow_consumer.take(), Some(flow));
|
||||
}
|
||||
|
||||
assert_eq!(mailbox.take_newer(&mut observed), Some(sample));
|
||||
#[test]
|
||||
// Forget is deliberate: endpoint safety must never depend on destruction.
|
||||
#[allow(clippy::forget_non_drop)]
|
||||
fn moving_dropping_and_forgetting_endpoints_needs_no_cleanup() {
|
||||
let mut mailbox = LatestMailbox::new();
|
||||
{
|
||||
let mut consumer = {
|
||||
let (producer, consumer) = mailbox.split();
|
||||
let mut moved = producer;
|
||||
moved.publish(1);
|
||||
consumer
|
||||
};
|
||||
// The producer left scope before its final publication is taken.
|
||||
assert_eq!(consumer.take(), Some(1));
|
||||
assert_eq!(consumer.take(), None);
|
||||
}
|
||||
{
|
||||
let (mut producer, consumer) = mailbox.split();
|
||||
core::mem::forget(consumer);
|
||||
producer.publish(2);
|
||||
producer.publish(3);
|
||||
core::mem::forget(producer);
|
||||
}
|
||||
// Neither endpoint owns a guard or controls the backing storage lifetime.
|
||||
let mut moved_mailbox = mailbox;
|
||||
let (_, mut consumer) = moved_mailbox.split();
|
||||
assert_eq!(consumer.take(), Some(3));
|
||||
assert_eq!(consumer.take(), None);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn owned_endpoints_publish_coherent_values_across_host_threads() {
|
||||
const LAST: u64 = 50_000;
|
||||
let mut mailbox = LatestMailbox::<[u64; 16]>::new();
|
||||
let (mut producer, mut consumer) = mailbox.split();
|
||||
thread::scope(|scope| {
|
||||
let writer = scope.spawn(move || {
|
||||
for value in 1..=LAST {
|
||||
producer.publish([value; 16]);
|
||||
}
|
||||
});
|
||||
let mut last = 0;
|
||||
let mut observe = |value: [u64; 16]| {
|
||||
assert_eq!(value, [value[0]; 16], "torn publication");
|
||||
assert!(value[0] > last, "duplicate or reordered publication");
|
||||
last = value[0];
|
||||
};
|
||||
while !writer.is_finished() {
|
||||
if let Some(value) = consumer.take() {
|
||||
observe(value);
|
||||
}
|
||||
thread::yield_now();
|
||||
}
|
||||
writer.join().unwrap();
|
||||
if let Some(value) = consumer.take() {
|
||||
observe(value);
|
||||
}
|
||||
assert_eq!(last, LAST);
|
||||
assert_eq!(consumer.take(), None);
|
||||
});
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,442 @@
|
||||
use super::{
|
||||
DataChannel, DataChannelError, DataChannelRates, DataCollector, DataSources, LatestImuMailbox,
|
||||
LatestOpticalFlowMailbox, LatestRangeMailbox, Producer, TelemetrySamples,
|
||||
};
|
||||
use kaptan_mavlink::{
|
||||
MavlinkFrame,
|
||||
generated::kaptan::{HEARTBEAT_DATA, HeartbeatState, MavMessage},
|
||||
receiver::IntegrityMavlinkReceiver,
|
||||
};
|
||||
use kaptan_sensor::{imu::ImuSnapshot, optical_flow::OpticalFlowSnapshot, range::RangeSnapshot};
|
||||
use kaptan_time::Timestamp;
|
||||
use kaptan_tx::{EnqueueResult, FrameClass, PendingFrame, TransmitQueue};
|
||||
|
||||
const RATES: DataChannelRates = DataChannelRates {
|
||||
imu_hz: 20,
|
||||
range_hz: Some(20),
|
||||
optical_flow_hz: Some(20),
|
||||
heartbeat_hz: 2,
|
||||
};
|
||||
|
||||
const ALL_STREAMS: DataChannelRates = DataChannelRates {
|
||||
imu_hz: 1,
|
||||
range_hz: Some(1),
|
||||
optical_flow_hz: Some(1),
|
||||
heartbeat_hz: 1,
|
||||
};
|
||||
|
||||
const HEARTBEAT: HEARTBEAT_DATA = HEARTBEAT_DATA {
|
||||
state: HeartbeatState::HEARTBEAT_STATE_INITIALIZED_OUTPUTS_INHIBITED,
|
||||
};
|
||||
|
||||
#[derive(Default)]
|
||||
struct Mailboxes {
|
||||
imu: LatestImuMailbox,
|
||||
range: LatestRangeMailbox,
|
||||
optical_flow: LatestOpticalFlowMailbox,
|
||||
}
|
||||
|
||||
impl Mailboxes {
|
||||
fn split(&mut self) -> (Publishers<'_>, DataCollector<'_>) {
|
||||
let (imu_producer, imu) = self.imu.split();
|
||||
let (range_producer, range) = self.range.split();
|
||||
let (flow_producer, optical_flow) = self.optical_flow.split();
|
||||
(
|
||||
Publishers {
|
||||
imu: imu_producer,
|
||||
range: range_producer,
|
||||
optical_flow: flow_producer,
|
||||
},
|
||||
DataCollector::new(DataSources {
|
||||
imu,
|
||||
range,
|
||||
optical_flow,
|
||||
}),
|
||||
)
|
||||
}
|
||||
}
|
||||
|
||||
struct Publishers<'a> {
|
||||
imu: Producer<'a, ImuSnapshot>,
|
||||
range: Producer<'a, RangeSnapshot>,
|
||||
optical_flow: Producer<'a, OpticalFlowSnapshot>,
|
||||
}
|
||||
|
||||
impl Publishers<'_> {
|
||||
fn publish(&mut self, samples: TelemetrySamples) {
|
||||
if let Some(imu) = samples.imu {
|
||||
self.imu.publish(imu);
|
||||
}
|
||||
if let Some(range) = samples.range {
|
||||
self.range.publish(range);
|
||||
}
|
||||
if let Some(flow) = samples.optical_flow {
|
||||
self.optical_flow.publish(flow);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
fn samples_at(microseconds: u64) -> TelemetrySamples {
|
||||
let timestamp = Timestamp::from_microseconds(microseconds);
|
||||
TelemetrySamples {
|
||||
imu: Some(ImuSnapshot::new(
|
||||
timestamp,
|
||||
[-1_001, 2, 3],
|
||||
[4, -5_006, 7],
|
||||
2_501,
|
||||
)),
|
||||
range: Some(RangeSnapshot::new(timestamp, 2_913)),
|
||||
optical_flow: Some(OpticalFlowSnapshot::new(timestamp, [-123, 456], 2_917, 201)),
|
||||
}
|
||||
}
|
||||
|
||||
fn decode(pending: &PendingFrame<MavlinkFrame>) -> MavMessage {
|
||||
assert!(pending.frame.has_valid_crc::<MavMessage>());
|
||||
let mut receiver = IntegrityMavlinkReceiver::<1>::new();
|
||||
for byte in pending.frame.raw_bytes() {
|
||||
receiver.receive_byte(*byte);
|
||||
}
|
||||
receiver.take_received().unwrap().decode().unwrap()
|
||||
}
|
||||
|
||||
fn assert_sample_frame(pending: &PendingFrame<MavlinkFrame>, samples: &TelemetrySamples) -> usize {
|
||||
let index = match decode(pending) {
|
||||
MavMessage::IMU(data) => {
|
||||
let imu = samples.imu.unwrap();
|
||||
assert_eq!(data.time_us, imu.timestamp.as_microseconds());
|
||||
assert_eq!(
|
||||
data.acceleration_milligravity,
|
||||
imu.acceleration_milligravity
|
||||
);
|
||||
assert_eq!(
|
||||
data.angular_rate_milliradians_per_second,
|
||||
imu.angular_rate_milliradians_per_second
|
||||
);
|
||||
assert_eq!(
|
||||
data.temperature_centidegrees_celsius,
|
||||
imu.temperature_centidegrees_celsius
|
||||
);
|
||||
0
|
||||
}
|
||||
MavMessage::RANGE(data) => {
|
||||
let range = samples.range.unwrap();
|
||||
assert_eq!(data.time_us, range.timestamp.as_microseconds());
|
||||
assert_eq!(data.distance_millimetres, range.distance_millimetres);
|
||||
1
|
||||
}
|
||||
MavMessage::OPTICAL_FLOW(data) => {
|
||||
let flow = samples.optical_flow.unwrap();
|
||||
assert_eq!(data.time_us, flow.timestamp.as_microseconds());
|
||||
assert_eq!(
|
||||
data.velocity_millimetres_per_second,
|
||||
flow.velocity_millimetres_per_second
|
||||
);
|
||||
assert_eq!(
|
||||
data.ground_distance_millimetres,
|
||||
flow.ground_distance_millimetres
|
||||
);
|
||||
assert_eq!(data.quality, flow.quality);
|
||||
2
|
||||
}
|
||||
MavMessage::HEARTBEAT(data) => {
|
||||
assert_eq!(data.state, HEARTBEAT.state);
|
||||
3
|
||||
}
|
||||
_ => panic!("unexpected telemetry message"),
|
||||
};
|
||||
assert_eq!(
|
||||
pending.class,
|
||||
if index == 3 {
|
||||
FrameClass::Vital
|
||||
} else {
|
||||
FrameClass::BestEffort
|
||||
}
|
||||
);
|
||||
index
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn collector_retains_latest_samples_between_publications() {
|
||||
let mut mailboxes = Mailboxes::default();
|
||||
let (mut publishers, mut collector) = mailboxes.split();
|
||||
assert_eq!(collector.snapshot(), TelemetrySamples::default());
|
||||
|
||||
publishers.publish(samples_at(1));
|
||||
publishers.publish(samples_at(2));
|
||||
let first = collector.snapshot();
|
||||
assert_eq!(first, samples_at(2));
|
||||
assert_eq!(collector.snapshot(), first);
|
||||
|
||||
let range = RangeSnapshot::new(Timestamp::from_microseconds(3), 4_567);
|
||||
publishers.range.publish(range);
|
||||
let updated = TelemetrySamples {
|
||||
range: Some(range),
|
||||
..first
|
||||
};
|
||||
assert_eq!(collector.snapshot(), updated);
|
||||
assert_eq!(collector.snapshot(), updated);
|
||||
assert_eq!(first, samples_at(2));
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn owns_rate_selection_encoding_and_classification() {
|
||||
let mut mailboxes = Mailboxes::default();
|
||||
let (mut publishers, mut collector) = mailboxes.split();
|
||||
let mut channel = DataChannel::<100>::new(1, 1, RATES, 100_000).unwrap();
|
||||
let mut counts = [0; 4];
|
||||
let mut sequence = 0;
|
||||
|
||||
for tick in 0..100 {
|
||||
let now = Timestamp::from_microseconds(tick * 10_000);
|
||||
publishers.publish(samples_at(now.as_microseconds()));
|
||||
let samples = collector.snapshot();
|
||||
if let Some(frame) = channel.on_service_tick(&samples, now) {
|
||||
assert_eq!(frame.frame.sequence(), sequence);
|
||||
sequence += 1;
|
||||
counts[assert_sample_frame(&frame, &samples)] += 1;
|
||||
}
|
||||
}
|
||||
assert_eq!(counts, [20, 20, 20, 2]);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn links_share_one_snapshot_with_independent_rates_and_sequences() {
|
||||
let mut mailboxes = Mailboxes::default();
|
||||
let (mut publishers, mut collector) = mailboxes.split();
|
||||
let mut fast = DataChannel::<100>::new(1, 2, RATES, 100_000).unwrap();
|
||||
let mut slow = DataChannel::<100>::new(
|
||||
3,
|
||||
4,
|
||||
DataChannelRates {
|
||||
imu_hz: 10,
|
||||
range_hz: Some(5),
|
||||
optical_flow_hz: None,
|
||||
heartbeat_hz: 1,
|
||||
},
|
||||
100_000,
|
||||
)
|
||||
.unwrap();
|
||||
let external = slow.encode_message(FrameClass::Critical, &HEARTBEAT);
|
||||
assert_eq!(external.class, FrameClass::Critical);
|
||||
assert_eq!(external.frame.sequence(), 0);
|
||||
assert!(matches!(decode(&external), MavMessage::HEARTBEAT(_)));
|
||||
let mut sequences = [0, 1];
|
||||
let mut counts = [[0; 4]; 2];
|
||||
|
||||
for tick in 0..100 {
|
||||
let now = Timestamp::from_microseconds(tick * 10_000);
|
||||
publishers.publish(samples_at(now.as_microseconds()));
|
||||
let samples = collector.snapshot();
|
||||
for (link, channel) in [&mut fast, &mut slow].into_iter().enumerate() {
|
||||
if let Some(frame) = channel.on_service_tick(&samples, now) {
|
||||
assert_eq!(frame.frame.sequence(), sequences[link]);
|
||||
assert_eq!(
|
||||
&frame.frame.raw_bytes()[5..7],
|
||||
if link == 0 { &[1, 2] } else { &[3, 4] }
|
||||
);
|
||||
sequences[link] += 1;
|
||||
counts[link][assert_sample_frame(&frame, &samples)] += 1;
|
||||
}
|
||||
}
|
||||
}
|
||||
assert_eq!(counts, [[20, 20, 20, 2], [10, 5, 0, 1]]);
|
||||
assert_eq!(sequences, [62, 17]);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn stalled_and_backpressured_link_does_not_consume_another_links_data() {
|
||||
let mut mailboxes = Mailboxes::default();
|
||||
let (mut publishers, mut collector) = mailboxes.split();
|
||||
let mut healthy = DataChannel::<4>::new(1, 1, ALL_STREAMS, 100_000).unwrap();
|
||||
let mut blocked = DataChannel::<4>::new(1, 2, ALL_STREAMS, 100_000).unwrap();
|
||||
let mut queue = TransmitQueue::<_, 1>::new();
|
||||
assert_eq!(
|
||||
queue.enqueue(blocked.encode_message(FrameClass::Vital, &HEARTBEAT)),
|
||||
EnqueueResult::Enqueued
|
||||
);
|
||||
let mut counts = [[0; 4]; 2];
|
||||
for tick in 0..8 {
|
||||
let now = Timestamp::from_microseconds(tick * 250_000);
|
||||
publishers.publish(samples_at(now.as_microseconds()));
|
||||
let samples = collector.snapshot();
|
||||
if tick >= 4 {
|
||||
let frame = blocked.on_service_tick(&samples, now).unwrap();
|
||||
assert_eq!(frame.frame.sequence(), (tick - 3) as u8);
|
||||
counts[1][assert_sample_frame(&frame, &samples)] += 1;
|
||||
assert_eq!(
|
||||
queue.enqueue(frame),
|
||||
if frame.class == FrameClass::Vital {
|
||||
EnqueueResult::VitalQueueFull
|
||||
} else {
|
||||
EnqueueResult::BestEffortDropped
|
||||
}
|
||||
);
|
||||
}
|
||||
let frame = healthy.on_service_tick(&samples, now).unwrap();
|
||||
assert_eq!(frame.frame.sequence(), tick as u8);
|
||||
counts[0][assert_sample_frame(&frame, &samples)] += 1;
|
||||
}
|
||||
assert_eq!(counts, [[2; 4], [1; 4]]);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn imu_requires_a_strictly_newer_timestamp_even_after_clock_regression() {
|
||||
let mut channel = DataChannel::<2>::new(
|
||||
1,
|
||||
1,
|
||||
DataChannelRates {
|
||||
range_hz: None,
|
||||
optical_flow_hz: None,
|
||||
..ALL_STREAMS
|
||||
},
|
||||
100_000,
|
||||
)
|
||||
.unwrap();
|
||||
let mut sequence = 0;
|
||||
for (timestamp, expected) in [
|
||||
(Some(0), true),
|
||||
(Some(0), false),
|
||||
(None, false),
|
||||
(Some(0), false),
|
||||
(Some(1), true),
|
||||
(Some(0), false),
|
||||
(Some(2), true),
|
||||
] {
|
||||
let now = Timestamp::from_microseconds(timestamp.unwrap_or(0));
|
||||
let mut samples = samples_at(now.as_microseconds());
|
||||
samples.imu = timestamp.map(|_| ImuSnapshot::new(now, [sequence as i16; 3], [0; 3], 0));
|
||||
let frame = channel.on_service_tick(&samples, now);
|
||||
assert_eq!(frame.is_some(), expected);
|
||||
if let Some(frame) = frame {
|
||||
assert_eq!(frame.frame.sequence(), sequence);
|
||||
sequence += 1;
|
||||
assert_eq!(assert_sample_frame(&frame, &samples), 0);
|
||||
}
|
||||
let heartbeat = channel.on_service_tick(&samples, now).unwrap();
|
||||
assert_eq!(heartbeat.frame.sequence(), sequence);
|
||||
sequence += 1;
|
||||
assert_eq!(assert_sample_frame(&heartbeat, &samples), 3);
|
||||
}
|
||||
}
|
||||
|
||||
fn assert_vertical_cycle(
|
||||
channel: &mut DataChannel<4>,
|
||||
samples: &TelemetrySamples,
|
||||
now: Timestamp,
|
||||
expected: [bool; 2],
|
||||
) {
|
||||
channel.on_service_tick(samples, now);
|
||||
let heartbeat = channel.on_service_tick(samples, now).unwrap();
|
||||
assert_eq!(assert_sample_frame(&heartbeat, samples), 3);
|
||||
for (index, expected) in expected.into_iter().enumerate() {
|
||||
let frame = channel.on_service_tick(samples, now);
|
||||
assert_eq!(frame.is_some(), expected);
|
||||
if let Some(frame) = frame {
|
||||
assert_eq!(assert_sample_frame(&frame, samples), index + 1);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn vertical_freshness_uses_sample_time_on_first_collection() {
|
||||
for (sample_time, now, maximum_age, expected) in [
|
||||
(0, 100_001, 100_000, false),
|
||||
(100_001, 100_000, 100_000, false),
|
||||
(u64::MAX, 0, u64::MAX, false),
|
||||
(0, 100_000, 100_000, true),
|
||||
(42, 42, 0, true),
|
||||
(42, 43, 0, false),
|
||||
] {
|
||||
let mut mailboxes = Mailboxes::default();
|
||||
let (mut publishers, mut collector) = mailboxes.split();
|
||||
publishers.publish(samples_at(sample_time));
|
||||
let samples = collector.snapshot();
|
||||
let mut channel = DataChannel::<4>::new(1, 1, ALL_STREAMS, maximum_age).unwrap();
|
||||
assert_vertical_cycle(
|
||||
&mut channel,
|
||||
&samples,
|
||||
Timestamp::from_microseconds(now),
|
||||
[expected; 2],
|
||||
);
|
||||
}
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn repeated_ticks_and_republication_do_not_change_sample_age() {
|
||||
let mut mailboxes = Mailboxes::default();
|
||||
let (mut publishers, mut collector) = mailboxes.split();
|
||||
let mut channel = DataChannel::<4>::new(1, 1, ALL_STREAMS, 100_000).unwrap();
|
||||
for (now, expected, repetitions) in [
|
||||
(110_000, true, 64),
|
||||
(110_001, false, 1),
|
||||
(1_000_000, false, 1),
|
||||
] {
|
||||
for _ in 0..repetitions {
|
||||
publishers.publish(samples_at(10_000));
|
||||
let samples = collector.snapshot();
|
||||
assert_vertical_cycle(
|
||||
&mut channel,
|
||||
&samples,
|
||||
Timestamp::from_microseconds(now),
|
||||
[expected; 2],
|
||||
);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn vertical_streams_use_their_own_sample_timestamps() {
|
||||
for (range_time, flow_time, expected) in [
|
||||
(99_999, 100_000, [false, true]),
|
||||
(200_000, 200_001, [true, false]),
|
||||
] {
|
||||
let samples = TelemetrySamples {
|
||||
range: samples_at(range_time).range,
|
||||
optical_flow: samples_at(flow_time).optical_flow,
|
||||
..TelemetrySamples::default()
|
||||
};
|
||||
let mut channel = DataChannel::<4>::new(1, 1, ALL_STREAMS, 100_000).unwrap();
|
||||
assert_vertical_cycle(
|
||||
&mut channel,
|
||||
&samples,
|
||||
Timestamp::from_microseconds(200_000),
|
||||
expected,
|
||||
);
|
||||
}
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn invalid_rates_keep_existing_errors() {
|
||||
for (rates, expected) in [
|
||||
(
|
||||
DataChannelRates { imu_hz: 0, ..RATES },
|
||||
DataChannelError::ZeroRate,
|
||||
),
|
||||
(
|
||||
DataChannelRates {
|
||||
range_hz: Some(0),
|
||||
..RATES
|
||||
},
|
||||
DataChannelError::ZeroRate,
|
||||
),
|
||||
(
|
||||
DataChannelRates {
|
||||
imu_hz: 101,
|
||||
..RATES
|
||||
},
|
||||
DataChannelError::RateExceedsServiceFrequency,
|
||||
),
|
||||
(
|
||||
DataChannelRates {
|
||||
imu_hz: 100,
|
||||
..RATES
|
||||
},
|
||||
DataChannelError::ScheduleFull,
|
||||
),
|
||||
] {
|
||||
assert_eq!(
|
||||
DataChannel::<100>::new(1, 1, rates, 100_000).err(),
|
||||
Some(expected)
|
||||
);
|
||||
}
|
||||
}
|
||||
Reference in new issue
Block a user