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 2026-11-07 18:12:11 +03:00
1 parent fb1b8af2d1
commit f9a61a086f
211 files changed
+45657 -12740

No files matched your search

@@ -0,0 +1,586 @@
//! Direct scripted sensor trajectories exercise the core flight phases.
mod support;
use kaptan_flight_runtime::*;
use kaptan_navigation::{AccelerationFrd, GRAVITY_MPS2, PositionNed, SensorRecord, VelocityNed};
use support::*;
fn climb(t: u64, start: u64, duration: f64) -> (f64, f64, f64) {
let s = (t.saturating_sub(start) as f64 * 1e-6 / duration).clamp(0., 1.);
(
2. * (10. * s.powi(3) - 15. * s.powi(4) + 6. * s.powi(5)),
2. / duration * (30. * s * s - 60. * s.powi(3) + 30. * s.powi(4)),
2. / (duration * duration) * (60. * s - 180. * s * s + 120. * s.powi(3)),
)
}
fn fly(ep: &mut FlightEndpoint, out: &mut Recording, t: u64, start: u64, land: Option<u64>) {
fly_with_bounce(ep, out, t, start, land, None);
}
fn fly_with_bounce(
ep: &mut FlightEndpoint,
out: &mut Recording,
t: u64,
start: u64,
land: Option<u64>,
bounce: Option<u64>,
) {
let motion = |measured: u64| {
let (mut h, mut v, _) = climb(measured, start, 5.);
if let Some(land) = land
&& measured >= land
{
let (down, dv, _) = climb(measured, land, 6.);
h = 2. - down;
v = -dv;
}
if let Some(bounce) = bounce
&& measured >= bounce
&& measured <= bounce + 1_000_000
{
let phase = (measured - bounce) as f64 * 1e-6 * core::f64::consts::TAU;
h += 0.05 * (1.0 - phase.cos());
v += 0.05 * core::f64::consts::TAU * phase.sin();
}
(h, v)
};
for mut record in records(t) {
let measured = record.context().measurement_time.as_microseconds();
let (h, v) = motion(measured);
match &mut record {
SensorRecord::Imu(r) => {
// Complete declared window, not endpoint acceleration.
let duration = u64::from(r.acquisition_duration_us);
let a = (v - motion(measured - duration).1) / (duration as f64 * 1e-6);
r.acceleration =
AccelerationFrd::metres_per_second_squared(0., 0., -GRAVITY_MPS2 - a as f32)
}
SensorRecord::LocalPositionVelocity(r) => {
r.position = PositionNed::metres(12., -7., 4. - h as f32);
r.velocity = VelocityNed::metres_per_second(0., 0., -v as f32);
}
SensorRecord::Pressure(r) => {
r.pascals =
(101325. * (-h / (287.05 * 288.15 / f64::from(GRAVITY_MPS2))).exp()) as f32
}
SensorRecord::Range(r) => r.metres = 0.04 + h as f32,
SensorRecord::Magnetic(_) => {}
}
assert_eq!(
ep.observe(ep.status().epoch, record, time(t), out).unwrap(),
ObservationAdmission::Queued
);
}
ep.tick(time(t), out).unwrap();
}
#[test]
fn onboard_hover_lands_and_stops() {
let (mut ep, mut out, start) = armed();
let mission = request(
&ep,
2,
FlightCommand::TakeoffHoverLand {
height_above_launch_metres: 2.,
},
start,
);
assert_eq!(
ep.command(mission, time(start), &mut out)
.unwrap()
.lifecycle,
CommandLifecycle::Accepted
);
let flight_deadline = ep.status().flight_deadline;
for t in (start + 10_000..=start + 8_000_000).step_by(10_000) {
fly(&mut ep, &mut out, t, start, None);
}
let hold = ep.status();
assert_eq!(hold.phase, VehiclePhase::Hold);
let land = hold.phase_since.as_microseconds() + 30_000_000;
assert_eq!(hold.phase_deadline, Some(time(land)));
assert_eq!(hold.active_command.unwrap().sequence, RequestSequence(2));
for t in (start + 8_010_000..land).step_by(10_000) {
fly(&mut ep, &mut out, t, start, None);
}
assert_eq!(ep.status().phase_since, hold.phase_since);
assert_eq!(ep.status().phase_deadline, Some(time(land)));
assert!(!out.commands.iter().any(|command| {
command.sequence == RequestSequence(2) && command.lifecycle == CommandLifecycle::Completed
}));
let output = *out.outputs.last().unwrap();
ep.supervise(time(land), &mut out).unwrap();
assert_eq!(ep.status().phase, VehiclePhase::Land);
assert_eq!(ep.status().flight_deadline, flight_deadline);
assert_eq!(
ep.status().phase_deadline,
Some(time(land + LANDING_BUDGET_US))
);
assert_eq!(*out.outputs.last().unwrap(), output);
fly(&mut ep, &mut out, land, start, Some(land));
for t in (land + 10_000..=land + 9_000_000).step_by(10_000) {
fly(&mut ep, &mut out, t, start, Some(land));
}
assert_eq!(ep.status().phase, VehiclePhase::Disarmed);
assert_eq!(ep.status().arm, None);
assert_eq!(ep.status().active_command, None);
assert_eq!(ep.status().flight_deadline, None);
assert!(!out.outputs.last().unwrap().permitted());
let completed = ep.status().last_terminal_command.unwrap();
assert_eq!(completed.kind, CommandKind::TakeoffHoverLand);
assert_eq!(completed.lifecycle, CommandLifecycle::Completed);
assert_eq!(completed.arm, None);
assert!(completed.at > time(land));
}
#[test]
fn takeoff_hold_offboard_loss_latches_lands_and_requires_new_arm() {
let (mut ep, mut out, start) = armed();
ep.command(
request(
&ep,
2,
FlightCommand::Takeoff {
height_above_launch_metres: 2.,
},
start,
),
time(start),
&mut out,
)
.unwrap();
for t in (start + 10_000..=start + 8_000_000).step_by(10_000) {
fly(&mut ep, &mut out, t, start, None);
}
assert_eq!(ep.status().phase, VehiclePhase::Hold);
assert_eq!(ep.status().phase_deadline, None);
let held_since = ep.status().phase_since;
// Autonomous phase outlives the finite command TTL and settling deadline.
for t in (start + 8_010_000..=start + 38_000_000).step_by(10_000) {
fly(&mut ep, &mut out, t, start, None);
}
assert_eq!(ep.status().phase_since, held_since);
assert!(ep.status().flight_deadline.is_some());
let enter = start + 38_000_000;
let origin = ep.status().estimate.unwrap().origin.id;
let initial = LocalSetpoint::PositionYaw {
position: PositionNed::metres(0., 0., -2.),
yaw_radians: 0.,
};
let mut entry = request(
&ep,
3,
FlightCommand::EnterOffboard { origin, initial },
enter - 3_000,
);
entry.context.time_uncertainty_microseconds = 2_000;
assert_eq!(
ep.command(entry, time(enter), &mut out).unwrap().lifecycle,
CommandLifecycle::Completed
);
let sp = SetpointRequest {
epoch: ep.status().epoch,
owner: ep.status().owner,
arm: ep.status().arm.unwrap(),
origin,
offboard: RequestSequence(3),
sequence: RequestSequence(0),
issued_at: time(enter - 3_000),
time_uncertainty_microseconds: 2_000,
setpoint: initial,
};
assert_eq!(
ep.setpoint(sp, time(enter), &mut out).unwrap().reason,
CommandReason::Replay
);
assert!(
ep.setpoint(
SetpointRequest {
sequence: RequestSequence(1),
..sp
},
time(enter),
&mut out
)
.unwrap()
.accepted
);
for t in (enter + 10_000..=enter + 300_000).step_by(10_000) {
fly(&mut ep, &mut out, t, start, None);
if t == enter + 290_000 {
assert!(out.outputs.last().unwrap().valid_until() > time(enter + 300_000));
}
}
assert_eq!(ep.status().phase, VehiclePhase::FallbackHold);
assert_eq!(ep.status().phase_deadline, Some(time(enter + 1_295_000)));
let retry = ep
.command(
request(
&ep,
4,
FlightCommand::EnterOffboard { origin, initial },
enter + 300_000,
),
time(enter + 300_000),
&mut out,
)
.unwrap();
assert_eq!(retry.reason, CommandReason::FallbackLatched);
assert!(
!ep.setpoint(
SetpointRequest {
sequence: RequestSequence(2),
issued_at: time(enter + 298_000),
..sp
},
time(enter + 300_000),
&mut out
)
.unwrap()
.accepted
);
let land = enter + 1_300_000;
for t in (enter + 310_000..=land).step_by(10_000) {
fly(&mut ep, &mut out, t, start, None);
if t == land - 10_000 {
assert!(out.outputs.last().unwrap().valid_until() > time(land));
}
}
assert_eq!(ep.status().phase, VehiclePhase::Land);
let deadline = ep.status().phase_deadline;
assert_eq!(deadline, Some(time(enter + 21_295_000)));
for sequence in [5, 6] {
let repeated = ep
.command(
request(&ep, sequence, FlightCommand::Land, land),
time(land),
&mut out,
)
.unwrap();
assert_eq!(repeated.reason, CommandReason::AlreadyActive);
assert_eq!(repeated.lifecycle, CommandLifecycle::Rejected);
assert_eq!(ep.status().phase_deadline, deadline);
}
for t in (land + 10_000..=land + 9_000_000).step_by(10_000) {
fly(&mut ep, &mut out, t, start, Some(land));
}
assert_eq!(ep.status().phase, VehiclePhase::Disarmed);
assert!(ep.status().fallback_latched);
assert_eq!(ep.status().ground_state, GroundState::Grounded);
let inhibit = out.outputs.last().unwrap();
assert!(!inhibit.permitted());
assert_eq!(inhibit.arm(), None);
let now = land + 9_000_000;
let s = ep
.command(
request(&ep, 7, FlightCommand::Arm, now),
time(now),
&mut out,
)
.unwrap();
assert_eq!(s.arm, Some(ArmId(7)));
assert!(ep.status().fallback_latched);
for t in (now + 10_000..=now + 600_000).step_by(10_000) {
fly(&mut ep, &mut out, t, start, Some(land));
}
assert_eq!(ep.status().phase, VehiclePhase::ArmedIdle);
assert!(!ep.status().fallback_latched);
}
#[test]
fn failed_inhibition_and_terminal_notification_are_both_retained() {
let (mut ep, mut out, now) = armed();
out.fail_inhibit = true;
out.fail_notification = true;
let err = ep
.command(
request(&ep, 2, FlightCommand::Terminate, now),
time(now),
&mut out,
)
.unwrap_err();
assert_eq!(err.primary, EndpointFailure::Notification("notification"));
assert_eq!(err.inhibition, Some("inhibit"));
assert_eq!(ep.status().phase, VehiclePhase::Terminated);
assert!(!out.outputs.last().unwrap().permitted());
assert_eq!(out.commands.last().unwrap().kind, CommandKind::Terminate);
}
#[test]
fn supervision_latches_stream_expiry_and_land_without_consuming_control_release() {
let (mut ep, mut out, start) = armed();
ep.command(
request(
&ep,
2,
FlightCommand::Takeoff {
height_above_launch_metres: 2.0,
},
start,
),
time(start),
&mut out,
)
.unwrap();
for t in (start + 10_000..=start + 8_000_000).step_by(10_000) {
fly(&mut ep, &mut out, t, start, None);
}
assert_eq!(ep.status().phase, VehiclePhase::Hold);
let enter = start + 8_000_000;
let origin = ep.status().estimate.unwrap().origin.id;
ep.command(
request(
&ep,
3,
FlightCommand::EnterOffboard {
origin,
initial: LocalSetpoint::PositionYaw {
position: PositionNed::metres(0.0, 0.0, -2.0),
yaw_radians: 0.0,
},
},
enter,
),
time(enter),
&mut out,
)
.unwrap();
let expiry = enter + 300_000;
for t in (enter + 10_000..expiry).step_by(10_000) {
fly(&mut ep, &mut out, t, start, None);
}
let output = *out.outputs.last().unwrap();
let cached = ep.status().estimate;
ep.supervise(time(expiry), &mut out).unwrap();
assert_eq!(ep.status().phase, VehiclePhase::FallbackHold);
assert!(ep.status().fallback_latched);
assert_eq!(ep.status().phase_deadline, Some(time(expiry + 1_000_000)));
assert_eq!(ep.status().estimate, cached);
assert_eq!(*out.outputs.last().unwrap(), output);
ep.supervise(time(expiry), &mut out).unwrap();
assert_eq!(*out.outputs.last().unwrap(), output);
// The post-ingress tick must still release on the due grid point.
fly(&mut ep, &mut out, expiry, start, None);
assert_eq!(
out.outputs.last().unwrap().sequence(),
output.sequence() + 1
);
assert!(out.outputs.last().unwrap().permitted());
for t in (expiry + 10_000..expiry + 1_000_000).step_by(10_000) {
fly(&mut ep, &mut out, t, start, None);
}
let output = *out.outputs.last().unwrap();
let cached = ep.status().estimate;
ep.supervise(time(expiry + 1_000_000), &mut out).unwrap();
assert_eq!(ep.status().phase, VehiclePhase::Land);
assert_eq!(ep.status().phase_deadline, Some(time(expiry + 21_000_000)));
assert_eq!(ep.status().estimate, cached);
assert_eq!(*out.outputs.last().unwrap(), output);
}
#[test]
fn unsuccessful_hold_enters_land_without_resetting_flight_deadline() {
let (mut ep, mut out, start) = armed();
ep.command(
request(
&ep,
2,
FlightCommand::Takeoff {
height_above_launch_metres: 2.,
},
start,
),
time(start),
&mut out,
)
.unwrap();
for t in (start + 10_000..=start + 2_000_000).step_by(10_000) {
fly(&mut ep, &mut out, t, start, None);
}
let at = start + 2_000_000;
ep.command(request(&ep, 3, FlightCommand::Hold, at), time(at), &mut out)
.unwrap();
let flight_deadline = ep.status().flight_deadline;
for t in (at + 10_000..=at + 10_000_000).step_by(10_000) {
fly(&mut ep, &mut out, t, start, None);
}
assert_eq!(ep.status().phase, VehiclePhase::Land);
assert_eq!(ep.status().flight_deadline, flight_deadline);
let result = ep.status().last_terminal_command.unwrap();
assert_eq!(result.kind, CommandKind::Hold);
assert_eq!(result.lifecycle, CommandLifecycle::Failed);
assert_eq!(result.reason, CommandReason::PhaseDeadline);
}
#[test]
fn streamed_velocity_and_yaw_reach_the_actual_controller() {
let (mut ep, mut out, start) = armed();
ep.command(
request(
&ep,
2,
FlightCommand::Takeoff {
height_above_launch_metres: 2.0,
},
start,
),
time(start),
&mut out,
)
.unwrap();
for t in (start + 10_000..=start + 8_000_000).step_by(10_000) {
fly(&mut ep, &mut out, t, start, None);
}
let now = start + 8_000_000;
let status = ep.status();
let origin = status.estimate.unwrap().origin.id;
let initial = LocalSetpoint::VelocityYaw {
velocity: VelocityNed::metres_per_second(0.3, 0.0, 0.0),
yaw_radians: 0.5,
};
ep.command(
request(
&ep,
3,
FlightCommand::EnterOffboard { origin, initial },
now,
),
time(now),
&mut out,
)
.unwrap();
for t in (now + 10_000..=now + 50_000).step_by(10_000) {
fly(&mut ep, &mut out, t, start, None);
}
let u = out.outputs.last().unwrap().motors();
assert!(u[0] - u[1] + u[2] - u[3] > 0.0001); // positive yaw, mixer reaction signs
assert!(u[0] - u[1] - u[2] + u[3] < -0.001); // nose-down pitch for north velocity
let sp = SetpointRequest {
epoch: status.epoch,
owner: status.owner,
arm: status.arm.unwrap(),
origin,
offboard: RequestSequence(3),
sequence: RequestSequence(1),
issued_at: time(now + 50_000),
time_uncertainty_microseconds: 0,
setpoint: LocalSetpoint::VelocityYaw {
velocity: VelocityNed::metres_per_second(0.3, 0.0, 0.0),
yaw_radians: -0.5,
},
};
assert!(
ep.setpoint(sp, time(now + 50_000), &mut out)
.unwrap()
.accepted
);
let expires = ep.status().setpoint_expires_at;
assert_eq!(
ep.setpoint(sp, time(now + 50_000), &mut out)
.unwrap()
.reason,
CommandReason::Replay
);
assert_eq!(ep.status().setpoint_expires_at, expires);
for t in (now + 60_000..=now + 150_000).step_by(10_000) {
fly(&mut ep, &mut out, t, start, None);
}
let u = out.outputs.last().unwrap().motors();
assert!(u[0] - u[1] + u[2] - u[3] < -0.0001);
}
#[test]
fn sensed_bounce_restarts_contact_dwell_without_extending_landing() {
let (mut ep, mut out, start) = armed();
ep.command(
request(
&ep,
2,
FlightCommand::Takeoff {
height_above_launch_metres: 2.0,
},
start,
),
time(start),
&mut out,
)
.unwrap();
for t in (start + 10_000..=start + 8_000_000).step_by(10_000) {
fly(&mut ep, &mut out, t, start, None);
}
let land = start + 8_000_000;
ep.command(
request(&ep, 3, FlightCommand::Land, land),
time(land),
&mut out,
)
.unwrap();
let deadline = ep.status().phase_deadline;
let mut bounce = None;
let mut observed_motion = false;
for t in (land + 10_000..=land + 11_000_000).step_by(10_000) {
fly_with_bounce(&mut ep, &mut out, t, start, Some(land), bounce);
if bounce.is_none() && ep.status().phase == VehiclePhase::TouchdownConfirm {
bounce = Some(t + 100_000);
}
if let Some(at) = bounce {
if t >= at + 200_000 && t <= at + 800_000 {
assert_ne!(ep.status().ground_state, GroundState::Grounded);
assert_eq!(ep.status().phase_deadline, deadline);
observed_motion = true;
}
if t >= at && t < at + 2_000_000 {
assert_ne!(ep.status().phase, VehiclePhase::Disarmed);
}
}
}
assert!(observed_motion);
assert_eq!(ep.status().phase, VehiclePhase::Disarmed);
assert_eq!(
ep.status().last_terminal_command.unwrap().kind,
CommandKind::Land
);
}
#[test]
fn flight_reserve_fails_pending_hold_before_autonomous_landing() {
let (mut ep, mut out, start) = armed();
ep.command(
request(
&ep,
2,
FlightCommand::Takeoff {
height_above_launch_metres: 2.0,
},
start,
),
time(start),
&mut out,
)
.unwrap();
for t in (start + 10_000..=start + 99_000_000).step_by(10_000) {
fly(&mut ep, &mut out, t, start, None);
}
let hold = start + 99_000_000;
ep.command(
request(&ep, 3, FlightCommand::Hold, hold),
time(hold),
&mut out,
)
.unwrap();
let land = start + 100_000_000;
for t in (hold + 10_000..=land).step_by(10_000) {
fly(&mut ep, &mut out, t, start, None);
}
assert_eq!(ep.status().phase, VehiclePhase::Land);
assert_eq!(ep.status().active_command, None);
let failure = ep.command_outcomes()[0].unwrap().status;
assert_eq!(failure.sequence, RequestSequence(3));
assert_eq!(failure.lifecycle, CommandLifecycle::Failed);
assert_eq!(failure.reason, CommandReason::PhaseDeadline);
for t in (land + 10_000..=land + 9_000_000).step_by(10_000) {
fly(&mut ep, &mut out, t, start, Some(land));
}
assert_eq!(ep.status().phase, VehiclePhase::Disarmed);
assert!(!out.commands.iter().any(|c|c.sequence==RequestSequence(3) && c.lifecycle==CommandLifecycle::Completed));
assert_eq!(ep.status().last_terminal_command, Some(failure));
}
@@ -0,0 +1,164 @@
mod support;
use kaptan_config::ConfigModel;
use kaptan_flight_runtime::*;
use kaptan_math::Vector3;
use kaptan_navigation::{
AccelerationFrd, AngularRateFrd, NavigationFailure, PositionNed, SensorRecord, VelocityNed,
};
use support::*;
#[test]
fn actual_navigation_fixed_reference_origin_and_delayed_aiding() {
let (ep, out, now) = ready();
let estimate = ep.status().estimate.unwrap();
assert!(
estimate.capabilities.flight_ready(),
"{:?}",
estimate.capabilities
);
assert!(estimate.position.vector().norm() < 0.01);
assert!((estimate.origin.launch_in_reference.vector().x() - 12.).abs() < 0.001);
assert!(estimate.fused_through.as_microseconds() <= now - 100_000);
assert_eq!(estimate.at, time(now));
assert!(
estimate
.ground_clearance
.unwrap()
.contact_clearance_metres
.abs()
< 0.002
);
assert!(out.outputs.iter().all(|v| !v.permitted()));
}
#[test]
fn navigation_failures_retain_their_reason() {
for (delay, observe, failure, reason) in [
(
30_000,
true,
NavigationFailure::ImuGap,
TerminalReason::ImuLost,
),
(
100_000,
false,
NavigationFailure::WorkBudget,
TerminalReason::NavigationLost,
),
] {
let (mut ep, mut out, now) = ready();
let now = now + delay;
let error = if observe {
ep.observe(
ep.status().epoch,
SensorRecord::Imu(imu(now)),
time(now),
&mut out,
)
.unwrap_err()
} else {
ep.tick(time(now), &mut out).unwrap_err()
};
assert_eq!(error.primary, EndpointFailure::Navigation(failure));
assert_eq!(ep.status().terminal_reason, Some(reason));
assert_eq!(ep.status().phase, VehiclePhase::Terminated);
assert!(!out.outputs.last().unwrap().permitted());
}
}
#[test]
fn healthy_armed_idle_expires_without_rearming() {
let (mut ep, mut out, now) = armed();
for t in (now + 10_000..=now + 10_500_000).step_by(10_000) {
feed(&mut ep, &mut out, t);
}
assert_eq!(ep.status().phase, VehiclePhase::Disarmed);
assert_eq!(ep.status().arm, None);
assert!(out.outputs.iter().all(|v| !v.permitted()));
}
#[test]
fn declared_noise_realizations_initialize_and_remain_grounded() {
// Box-Muller Gaussian draws, independently implemented at the sensor boundary.
// q/T gives IMU per-sample variance; no packet carries an ImuNoise value.
let mut seed = 0xaca5_8113_78ab_1234_u64;
let mut uniform = || {
seed ^= seed << 13;
seed ^= seed >> 7;
seed ^= seed << 17;
((seed >> 11) as f64 + 0.5) / 9_007_199_254_740_992.
};
let mut normal = || (-2. * uniform().ln()).sqrt() * (core::f64::consts::TAU * uniform()).cos();
let profile = profile_fixture::candidate()
.validate(&profile_fixture::capabilities())
.unwrap();
let mut ep = FlightEndpoint::new(
profile,
RunBinding {
session: TransportSessionId(7),
owner: CommandOwner(1),
},
)
.unwrap();
let mut out = Recording::default();
for m in (10_000..=6_000_000).step_by(10_000) {
let t = m + 1_000;
for mut record in records(t) {
match &mut record {
SensorRecord::Imu(r) => {
let s = (2.5e-5_f64 / 0.01).sqrt();
r.acceleration = AccelerationFrd::metres_per_second_squared(
(normal() * s) as f32,
(normal() * s) as f32,
(-f64::from(kaptan_navigation::GRAVITY_MPS2) + normal() * s) as f32,
);
let s = (2.5e-7_f64 / 0.01).sqrt();
r.angular_rate = AngularRateFrd::radians_per_second(
(normal() * s) as f32,
(normal() * s) as f32,
(normal() * s) as f32,
);
}
SensorRecord::LocalPositionVelocity(r) => {
r.position = PositionNed::metres(
(12. + normal() * 0.1) as f32,
(-7. + normal() * 0.1) as f32,
(4. + normal() * 0.1) as f32,
);
r.velocity = VelocityNed::metres_per_second(
(normal() * 0.05) as f32,
(normal() * 0.05) as f32,
(normal() * 0.05) as f32,
);
}
SensorRecord::Magnetic(r) => {
r.microtesla_frd = Vector3::new(
(20. + normal() * 0.02) as f32,
(normal() * 0.02) as f32,
(45. + normal() * 0.02) as f32,
)
}
SensorRecord::Pressure(r) => r.pascals = (101_325. + normal() * 0.3) as f32,
SensorRecord::Range(r) => {
r.metres = (((0.04 + normal() * 0.002) / 0.001).round() * 0.001) as f32
}
}
let admitted = ep
.observe(ep.status().epoch, record, time(t), &mut out)
.unwrap();
assert!(
admitted == ObservationAdmission::Queued
|| admitted
== ObservationAdmission::Rejected(
ObservationRejection::BehindFusionHorizon
)
);
}
ep.tick(time(t), &mut out).unwrap();
}
let e = ep.status().estimate.unwrap();
assert!(e.capabilities.flight_ready(), "{:?}", e.capabilities);
assert!(e.position.vector().norm() < 0.1, "{:?}", e.position);
assert_eq!(ep.status().ground_state, GroundState::Grounded);
assert!(out.outputs.iter().all(|r| !r.permitted()));
}
@@ -0,0 +1,413 @@
//! Sensor-driven lifecycle and output boundary regressions.
mod support;
use kaptan_flight_runtime::*;
use support::*;
#[test]
fn sensor_startup_arm_binding_and_ground_disarm() {
let (mut ep, mut out, now) = ready();
assert_eq!(ep.status().ground_state, GroundState::Grounded);
let s = ep
.command(
request(&ep, 1, FlightCommand::Arm, now),
time(now),
&mut out,
)
.unwrap();
assert_eq!(s.lifecycle, CommandLifecycle::Accepted);
assert_eq!(s.arm, Some(ArmId(1)));
assert_eq!(ep.status().phase, VehiclePhase::PreArm);
let binding = out.outputs.last().unwrap();
assert_eq!(binding.arm(), s.arm);
assert!(!binding.permitted());
let s = ep
.command(
request(&ep, 2, FlightCommand::GroundDisarm, now),
time(now),
&mut out,
)
.unwrap();
assert_eq!(s.lifecycle, CommandLifecycle::Completed);
assert_eq!(ep.status().phase, VehiclePhase::Disarmed);
assert_eq!(out.outputs.last().unwrap().arm(), None);
}
#[test]
fn command_ttl_replay_and_epoch_do_not_renew_authority() {
let (mut ep, mut out, now) = ready();
let zero = request(&ep, 0, FlightCommand::Arm, now);
assert_eq!(
ep.command(zero, time(now), &mut out).unwrap().reason,
CommandReason::Replay
);
let mut wrong = request(&ep, 50, FlightCommand::Arm, now);
wrong.context.owner = CommandOwner(999);
assert_eq!(
ep.command(wrong, time(now), &mut out).unwrap().reason,
CommandReason::WrongOwner
);
assert_eq!(ep.status().command_highwater, None);
let mut expired = request(&ep, 1, FlightCommand::Arm, now);
expired.context.expires_at = time(now);
assert_eq!(
ep.command(expired, time(now), &mut out).unwrap().lifecycle,
CommandLifecycle::Rejected
);
assert_eq!(ep.status().command_highwater, Some(RequestSequence(1)));
assert_eq!(
ep.command(
request(&ep, 1, FlightCommand::Arm, now),
time(now),
&mut out
)
.unwrap()
.reason,
CommandReason::Replay
);
ep.command(
request(&ep, 2, FlightCommand::Arm, now),
time(now),
&mut out,
)
.unwrap();
for t in (now + 10_000..=now + 600_000).step_by(10_000) {
feed(&mut ep, &mut out, t);
}
assert_eq!(ep.status().phase, VehiclePhase::ArmedIdle); // TTL expired, arm completed.
ep.command(
request(&ep, 3, FlightCommand::GroundDisarm, now + 600_000),
time(now + 600_000),
&mut out,
)
.unwrap();
let old = ep.status().epoch;
let seq = out.outputs.last().unwrap().sequence();
let epoch = ep
.begin_epoch(CommandOwner(8), time(now + 600_000), &mut out)
.unwrap();
assert_ne!(old, epoch);
assert_eq!(out.outputs.last().unwrap().sequence(), seq + 1);
assert_eq!(ep.status().at, time(now + 600_000));
}
#[test]
fn failed_sink_consumes_sequence_and_attempts_new_inhibition() {
let (mut ep, mut out, now) = armed();
ep.command(
request(
&ep,
2,
FlightCommand::Takeoff {
height_above_launch_metres: 2.,
},
now,
),
time(now),
&mut out,
)
.unwrap();
out.fail_enabled = true;
out.fail_inhibit = true;
let attempts = out.outputs.len();
let err = feed_result(&mut ep, &mut out, now + 10_000).unwrap_err();
assert_eq!(out.outputs.len(), attempts + 2);
assert_eq!(err.primary, EndpointFailure::Output("enabled"));
assert_eq!(err.inhibition, Some("inhibit"));
assert_eq!(ep.status().phase, VehiclePhase::Terminated);
let n = out.outputs.len();
assert!(out.outputs[n - 2].permitted());
assert!(!out.outputs[n - 1].permitted());
assert_eq!(
out.outputs[n - 1].sequence(),
out.outputs[n - 2].sequence() + 1
);
assert!(
ep.begin_epoch(CommandOwner(1), time(now + 10_000), &mut out)
.is_err()
);
}
#[test]
fn failed_arm_binding_publication_retains_the_failed_transaction() {
let (mut ep, mut out, now) = ready();
out.fail_inhibit = true;
let error = ep
.command(
request(&ep, 1, FlightCommand::Arm, now),
time(now),
&mut out,
)
.unwrap_err();
assert_eq!(error.primary, EndpointFailure::Output("inhibit"));
assert_eq!(ep.status().phase, VehiclePhase::Terminated);
assert_eq!(ep.status().arm, None);
let failed = ep.status().last_terminal_command.unwrap();
assert_eq!(failed.sequence, RequestSequence(1));
assert_eq!(failed.lifecycle, CommandLifecycle::Failed);
assert_eq!(failed.arm, Some(ArmId(1)));
assert!(!out.outputs.last().unwrap().permitted());
}
#[test]
fn asynchronous_tick_deadline_and_clock_fault_inhibit() {
let (mut ep, mut out, now) = armed();
ep.command(
request(
&ep,
2,
FlightCommand::Takeoff {
height_above_launch_metres: 2.,
},
now,
),
time(now),
&mut out,
)
.unwrap();
feed(&mut ep, &mut out, now + 10_000);
let record = *out.outputs.last().unwrap();
assert!(record.permitted());
assert!(
record.valid_until().as_microseconds() <= record.produced_at().as_microseconds() + 20_000
);
ep.tick(time(now + 10_000), &mut out).unwrap();
assert_eq!(*out.outputs.last().unwrap(), record);
let error = ep.tick(time(now + 30_001), &mut out).unwrap_err();
assert_eq!(error.primary, EndpointFailure::ControlDeadline);
assert!(!out.outputs.last().unwrap().permitted());
let error = ep.tick(time(now), &mut out).unwrap_err();
assert_eq!(error.primary, EndpointFailure::ClockRegression);
assert_eq!(ep.status().phase, VehiclePhase::Terminated);
}
#[test]
fn ordinary_prearm_health_failure_clears_arm_without_terminal_fault() {
let (mut ep, mut out, now) = ready();
ep.command(
request(&ep, 1, FlightCommand::Arm, now),
time(now),
&mut out,
)
.unwrap();
ep.tick(time(now + 25_000), &mut out).unwrap();
assert_eq!(ep.status().phase, VehiclePhase::Disarmed);
assert_eq!(ep.status().arm, None);
assert_eq!(
ep.status().last_terminal_command.unwrap().lifecycle,
CommandLifecycle::Failed
);
assert!(!out.outputs.last().unwrap().permitted());
}
#[test]
fn typed_bad_quality_consumes_sensor_watermark_not_health() {
let (mut ep, mut out, now) = ready();
// The previous complete acquisition ended 1 ms before the service tick.
let mut bad = imu(now + 9_000);
bad.context.quality = 0;
assert_eq!(
ep.observe(
ep.status().epoch,
SensorRecord::Imu(bad),
time(now + 10_000),
&mut out
)
.unwrap(),
ObservationAdmission::Rejected(ObservationRejection::InvalidQuality)
);
bad.context.quality = 255;
assert_eq!(
ep.observe(
ep.status().epoch,
SensorRecord::Imu(bad),
time(now + 10_000),
&mut out
)
.unwrap(),
ObservationAdmission::Rejected(ObservationRejection::Replay)
);
}
#[test]
fn fresh_terminate_does_not_require_current_navigation() {
let (mut ep, mut out, now) = armed();
let now = now + 50_000;
let result = ep
.command(
request(&ep, 2, FlightCommand::Terminate, now),
time(now),
&mut out,
)
.unwrap();
assert_eq!(result.lifecycle, CommandLifecycle::Completed);
assert_eq!(ep.status().phase, VehiclePhase::Terminated);
assert_eq!(ep.status().terminal_reason, Some(TerminalReason::Requested));
assert!(!out.outputs.last().unwrap().permitted());
let sources = ep.status().source_status;
for t in (now + 10_000..=now + 500_000).step_by(10_000) {
for record in records(t) {
assert_eq!(
ep.observe(ep.status().epoch, record, time(t), &mut out)
.unwrap(),
ObservationAdmission::Rejected(ObservationRejection::Terminated)
);
}
ep.tick(time(t), &mut out).unwrap();
}
assert_eq!(ep.status().source_status, sources);
assert_eq!(ep.status().terminal_reason, Some(TerminalReason::Requested));
}
#[test]
fn new_source_isolation_is_seen_before_the_next_control_tick() {
let (mut ep, mut out, now) = ready();
for step in 1..=15 {
let t = now + step * 10_000;
for mut record in records(t) {
let bad = if let SensorRecord::Magnetic(ref mut magnetic) = record {
magnetic.context.quality = 0;
true
} else {
false
};
let outcome = ep
.observe(ep.status().epoch, record, time(t), &mut out)
.unwrap();
assert_eq!(
outcome,
if bad {
ObservationAdmission::Rejected(ObservationRejection::InvalidQuality)
} else {
ObservationAdmission::Queued
}
);
}
if step == 15 {
let arm = ep
.command(request(&ep, 1, FlightCommand::Arm, t), time(t), &mut out)
.unwrap();
assert_eq!(arm.reason, CommandReason::NotReady);
assert_eq!(ep.status().phase, VehiclePhase::Disarmed);
}
ep.tick(time(t), &mut out).unwrap();
}
for t in (now + 160_000..=now + 1_400_000).step_by(10_000) {
feed(&mut ep, &mut out, t);
}
assert_eq!(ep.status().phase, VehiclePhase::Disarmed);
assert_eq!(ep.status().ground_state, GroundState::Grounded);
assert!(ep.status().estimate.unwrap().capabilities.heading.available);
let t = now + 1_400_000;
assert_eq!(
ep.command(request(&ep, 2, FlightCommand::Arm, t), time(t), &mut out)
.unwrap()
.lifecycle,
CommandLifecycle::Accepted
);
}
#[test]
fn sensed_motion_during_prearm_consumes_the_arm_transaction() {
let (mut ep, mut out, now) = ready();
ep.command(
request(&ep, 1, FlightCommand::Arm, now),
time(now),
&mut out,
)
.unwrap();
for step in 1..=3 {
let t = now + step * 10_000;
for mut record in records(t) {
if let SensorRecord::Imu(ref mut imu) = record {
imu.acceleration = kaptan_navigation::AccelerationFrd::metres_per_second_squared(
0.0,
0.0,
-kaptan_navigation::GRAVITY_MPS2 - 5.0,
);
}
ep.observe(ep.status().epoch, record, time(t), &mut out)
.unwrap();
}
ep.tick(time(t), &mut out).unwrap();
}
assert_eq!(ep.status().phase, VehiclePhase::Disarmed);
assert_eq!(ep.status().arm, None);
assert_eq!(ep.status().terminal_reason, None);
assert_eq!(
ep.status().last_terminal_command.unwrap().lifecycle,
CommandLifecycle::Failed
);
assert!(out.outputs.iter().all(|record| !record.permitted()));
}
#[test]
fn full_notification_sink_observes_both_cancelled_arm_and_ground_disarm() {
let (mut ep, mut out, now) = ready();
ep.command(
request(&ep, 1, FlightCommand::Arm, now),
time(now),
&mut out,
)
.unwrap();
out.fail_notification = true;
let error = ep
.command(
request(&ep, 2, FlightCommand::GroundDisarm, now),
time(now),
&mut out,
)
.unwrap_err();
assert_eq!(error.primary, EndpointFailure::Notification("notification"));
assert_eq!(ep.status().phase, VehiclePhase::Disarmed);
assert!(!out.outputs.last().unwrap().permitted());
let outcomes = ep.command_outcomes();
let cancelled = outcomes[0].unwrap();
let disarmed = outcomes[1].unwrap();
assert_eq!(cancelled.status.sequence, RequestSequence(1));
assert_eq!(cancelled.status.lifecycle, CommandLifecycle::Cancelled);
assert_eq!(disarmed.status.sequence, RequestSequence(2));
assert_eq!(disarmed.status.lifecycle, CommandLifecycle::Completed);
assert!(!cancelled.published);
assert!(!disarmed.published);
assert_eq!(
&out.commands[out.commands.len() - 2..],
&[cancelled.status, disarmed.status]
);
assert_eq!(ep.status().last_terminal_command, Some(disarmed.status));
}
#[test]
fn asynchronous_tick_and_observe_faults_deliver_failed_command_outcomes() {
for observe_fault in [false, true] {
let (mut ep, mut out, now) = armed();
ep.command(
request(
&ep,
2,
FlightCommand::Takeoff {
height_above_launch_metres: 2.0,
},
now,
),
time(now),
&mut out,
)
.unwrap();
out.fail_notification = true;
let error = if observe_fault {
let sample = imu(now + 30_000);
ep.observe(
ep.status().epoch,
SensorRecord::Imu(sample),
time(now + 30_000),
&mut out,
)
.unwrap_err()
} else {
ep.tick(time(now + 30_000), &mut out).unwrap_err()
};
assert!(!matches!(error.primary, EndpointFailure::Notification(_)));
let outcome = ep.command_outcomes()[0].unwrap();
assert_eq!(outcome.status.sequence, RequestSequence(2));
assert_eq!(outcome.status.lifecycle, CommandLifecycle::Failed);
assert!(!outcome.published);
assert_eq!(out.commands.last(), Some(&outcome.status));
assert_eq!(ep.status().phase, VehiclePhase::Terminated);
assert!(!out.outputs.last().unwrap().permitted());
}
}
@@ -0,0 +1,173 @@
#[path = "../../../kaptan-flight-config/tests/support/mod.rs"]
pub mod profile_fixture;
use kaptan_config::ConfigModel;
use kaptan_flight_runtime::*;
use kaptan_math::Vector3;
use kaptan_navigation::*;
use kaptan_time::Timestamp;
pub fn time(v: u64) -> Timestamp {
Timestamp::from_microseconds(v)
}
pub fn context(t: u64, receipt: u64, kind: u8, period: u64, frame: FrameId) -> ObservationContext {
ObservationContext {
measurement_time: time(t),
receipt_time: time(receipt),
source: SourceId::new(kind),
frame,
calibration: CalibrationRevision::new(1),
quality: 255,
validity: Validity::Valid,
sequence: ObservationSequence::new((t / period) as u32),
time_uncertainty_microseconds: 0,
}
}
pub fn imu(t: u64) -> FlightImuObservation {
FlightImuObservation {
acquisition_duration_us: 10_000,
context: context(t, t, 1, 10_000, FrameId::BODY_FRD),
acceleration: AccelerationFrd::metres_per_second_squared(0., 0., -GRAVITY_MPS2),
angular_rate: AngularRateFrd::radians_per_second(0., 0., 0.),
}
}
pub fn records(t: u64) -> Vec<SensorRecord> {
let mut result = vec![SensorRecord::Imu(imu(t - 1_000))];
let m = t - 1_000;
if m % 50_000 == 30_000 {
result.push(SensorRecord::LocalPositionVelocity(
LocalPositionVelocityObservation {
context: context(m - 30_000, t, 2, 50_000, FrameId::LOCAL_NED),
position: PositionNed::metres(12., -7., 4.),
velocity: VelocityNed::metres_per_second(0., 0., 0.),
position_variance: [ScalarVariance::new(0.01).unwrap(); 3],
velocity_variance: [ScalarVariance::new(0.0025).unwrap(); 3],
reference: SensorReferenceId(1),
},
));
}
if m.is_multiple_of(50_000) {
result.push(SensorRecord::Magnetic(MagneticObservation {
context: context(m, t, 3, 50_000, FrameId::BODY_FRD),
microtesla_frd: Vector3::new(20., 0., 45.),
variance_microtesla_squared: [ScalarVariance::new(0.0004).unwrap(); 3],
}));
result.push(SensorRecord::Pressure(PressureObservation {
context: context(m, t, 4, 50_000, FrameId::SCALAR),
pascals: 101_325.,
variance_pascals_squared: ScalarVariance::new(0.09).unwrap(),
}));
}
if m.is_multiple_of(20_000) {
result.push(SensorRecord::Range(RangeObservation {
context: context(m, t, 5, 20_000, FrameId::new(3)),
metres: 0.04,
variance: ScalarVariance::new(4e-6).unwrap(),
}));
}
result
}
#[derive(Default)]
pub struct Recording {
pub outputs: Vec<OutputRecord>,
pub commands: Vec<CommandStatus>,
pub fail_enabled: bool,
pub fail_inhibit: bool,
pub fail_notification: bool,
}
impl FlightOutput for Recording {
type Error = &'static str;
fn actuator(&mut self, r: OutputRecord) -> Result<(), Self::Error> {
self.outputs.push(r);
if r.permitted() && self.fail_enabled {
Err("enabled")
} else if !r.permitted() && self.fail_inhibit {
Err("inhibit")
} else {
Ok(())
}
}
fn command_status(&mut self, s: CommandStatus) -> Result<(), Self::Error> {
self.commands.push(s);
if self.fail_notification {
Err("notification")
} else {
Ok(())
}
}
}
pub fn feed_result(
ep: &mut FlightEndpoint,
out: &mut Recording,
t: u64,
) -> Result<(), EndpointError<&'static str>> {
for r in records(t) {
let result = ep.observe(ep.status().epoch, r, time(t), out)?;
if result == ObservationAdmission::Rejected(ObservationRejection::BehindFusionHorizon) {
// The first delayed GNSS delivery after initialization can predate
// t0. It must be refused rather than fused backwards or retimed.
assert!(ep.status().estimate.is_none());
assert_eq!(r.kind(), 2);
assert!(r.context().measurement_time.as_microseconds() <= 2_020_000);
} else {
assert_eq!(result, ObservationAdmission::Queued);
}
}
ep.tick(time(t), out)
}
pub fn feed(ep: &mut FlightEndpoint, out: &mut Recording, t: u64) {
feed_result(ep, out, t).unwrap();
}
pub fn request(ep: &FlightEndpoint, seq: u64, command: FlightCommand, t: u64) -> CommandRequest {
CommandRequest {
context: RequestContext {
epoch: ep.status().epoch,
owner: ep.status().owner,
sequence: RequestSequence(seq),
issued_at: time(t),
expires_at: time(t + 250_000),
time_uncertainty_microseconds: 0,
arm: if command == FlightCommand::Arm {
None
} else {
ep.status().arm
},
},
command,
}
}
pub fn ready() -> (FlightEndpoint, Recording, u64) {
let profile = profile_fixture::candidate()
.validate(&profile_fixture::capabilities())
.unwrap();
let mut ep = FlightEndpoint::new(
profile,
RunBinding {
session: TransportSessionId(123),
owner: CommandOwner(1),
},
)
.unwrap();
let mut out = Recording::default();
for m in (10_000..=4_000_000).step_by(10_000) {
feed(&mut ep, &mut out, m + 1_000);
}
assert!(ep.status().estimate.is_some(), "missing initialization");
(ep, out, 4_001_000)
}
pub fn armed() -> (FlightEndpoint, Recording, u64) {
let (mut ep, mut out, now) = ready();
let result = ep
.command(
request(&ep, 1, FlightCommand::Arm, now),
time(now),
&mut out,
)
.unwrap();
assert_eq!(result.lifecycle, CommandLifecycle::Accepted, "{result:?}");
for t in (now + 10_000..=now + 600_000).step_by(10_000) {
feed(&mut ep, &mut out, t);
}
assert_eq!(ep.status().phase, VehiclePhase::ArmedIdle);
(ep, out, now + 600_000)
}