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:
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)
|
||||
}
|
||||
Reference in new issue
Block a user