public/kaptan
1
Fork 0
Code Issues 24 Pull Requests Actions Packages Projects Releases Wiki Activity

feat(firmware): validate configuration and migrate sensor-driven flight APIs

Browse Source 
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
Download Patch File Download Diff File Expand all files Collapse all files

No files matched your search

firmware/libs/kaptan-flight-runtime/Cargo.toml
+8 -2
View File
@@ -9,6 +9,12 @@ version.workspace = true
workspace = true
[dependencies]
kaptan-config = { path = "../kaptan-config" }
kaptan-flight-config = { path = "../kaptan-flight-config" }
kaptan-log = { path = "../kaptan-log" }
kaptan-control = { path = "../kaptan-control" }
kaptan-math = { path = "../kaptan-math" }
kaptan-navigation = { path = "../kaptan-navigation" }
kaptan-safety = { path = "../kaptan-safety" }
kaptan-time = { path = "../kaptan-time" }
[dev-dependencies]
kaptan-config = { path = "../kaptan-config" }
firmware/libs/kaptan-flight-runtime/src/controller.rs
+214
View File
@@ -0,0 +1,214 @@
use kaptan_control::{
ActuatorLimits, Allocator, BodyRate, BodyWrench, HeadingNed, KinematicReference, MixerOutput,
PidConfig, PrecomputedMixer, RatePid, ThrustEnvelope, TranslationalControlConfig,
TranslationalController, TranslationalEstimate, force_to_attitude,
};
use kaptan_flight_config::VehicleProfile;
use kaptan_math::{Frd, Ned, UnitQuaternion, Vector3};
use kaptan_navigation::GRAVITY_MPS2;
use crate::{FlightEstimate, RuntimeConfigError};
pub(crate) struct QuadController {
translational: TranslationalController,
thrust: ThrustEnvelope,
rate: RatePid,
allocator: Allocator<4>,
heading: Vector3<Ned>,
attitude_gain_per_second: [f32; 3],
}
impl QuadController {
pub fn new(config: VehicleProfile) -> Result<Self, RuntimeConfigError> {
let control = config.control();
let thrust = config.thrust();
let rate = config.rate_pid();
let limits = config.actuator_limits();
let translational = TranslationalController::new(TranslationalControlConfig {
position_gain_per_second_squared: control.position_gain_per_second_squared,
velocity_gain_per_second: control.velocity_gain_per_second,
maximum_horizontal_acceleration_mps2: control.maximum_horizontal_acceleration_mps2,
maximum_upward_acceleration_mps2: control.maximum_upward_acceleration_mps2,
maximum_downward_acceleration_mps2: control.maximum_downward_acceleration_mps2,
})
.map_err(|_| RuntimeConfigError::Control)?;
let thrust = ThrustEnvelope {
mass_kg: thrust.mass_kg,
maximum_collective_thrust_newtons: thrust.maximum_collective_thrust_newtons,
maximum_tilt_tangent: thrust.maximum_tilt_tangent,
};
thrust.validate().map_err(|_| RuntimeConfigError::Control)?;
let yaw_gains = (rate.yaw_proportional, rate.yaw_integral);
let rate = PidConfig {
proportional: rate.proportional,
integral: rate.integral,
derivative: rate.derivative,
output_limit: rate.output_limit,
integral_limit: rate.integral_limit,
};
let limits = ActuatorLimits {
minimum: limits.minimum,
maximum: limits.maximum,
slew_per_second: limits.slew_per_second,
};
let mixer = PrecomputedMixer::new(config.mixer().map(|coefficients| MixerOutput {
coefficients,
limits,
}));
Ok(Self {
translational,
thrust,
rate: RatePid::new(
rate,
rate,
PidConfig {
proportional: yaw_gains.0,
integral: yaw_gains.1,
..rate
},
)
.map_err(|_| RuntimeConfigError::Control)?,
allocator: Allocator::new(mixer).map_err(|_| RuntimeConfigError::Control)?,
heading: Vector3::new(1., 0., 0.),
attitude_gain_per_second: [
control.attitude_roll_pitch_gain_per_second,
control.attitude_roll_pitch_gain_per_second,
control.attitude_yaw_gain_per_second,
],
})
}
pub fn update(
&mut self,
state: FlightEstimate,
mut reference: KinematicReference,
dt_seconds: f32,
yaw: Option<f32>,
position_feedback: [bool; 3],
collective_cap: f32,
) -> Result<[f32; 4], ()> {
let measured_position = state.position.vector();
reference.position = Vector3::new(
if position_feedback[0] {
reference.position.x()
} else {
measured_position.x()
},
if position_feedback[1] {
reference.position.y()
} else {
measured_position.y()
},
if position_feedback[2] {
reference.position.z()
} else {
measured_position.z()
},
);
let acceleration = self
.translational
.update(
reference,
TranslationalEstimate {
position: state.position.vector(),
velocity: state.velocity.vector(),
},
)
.map_err(|_| ())?;
let (force, limited) = self
.thrust
.force_for_acceleration(acceleration, GRAVITY_MPS2)
.map_err(|_| ())?;
if let Some(yaw) = yaw {
let half = UnitQuaternion::<Ned, Ned>::from_small_rotation_vector(Vector3::new(
0.,
0.,
yaw * 0.5,
))
.map_err(|_| ())?;
let goal = half
.compose(half)
.map_err(|_| ())?
.rotate(Vector3::new(1., 0., 0.));
let sign = if self.heading.cross(goal).z() < 0. {
-1.
} else {
1.
};
let step = UnitQuaternion::<Ned, Ned>::from_small_rotation_vector(Vector3::new(
0.,
0.,
sign * dt_seconds,
))
.map_err(|_| ())?;
let cosine = step.rotate(Vector3::new(1., 0., 0.)).x();
self.heading = if self.heading.dot(goal) >= cosine {
goal
} else {
step.rotate(self.heading)
};
}
let heading = HeadingNed::new(self.heading.x(), self.heading.y()).ok_or(())?;
let target = force_to_attitude(force, heading).map_err(|_| ())?;
let error = state.attitude.inverse().compose(target).map_err(|_| ())?;
let direction = if error.w() < 0.0 { -1.0 } else { 1.0 };
let target_rate = BodyRate::radians_per_second(
direction * 2.0 * self.attitude_gain_per_second[0] * error.x(),
direction * 2.0 * self.attitude_gain_per_second[1] * error.y(),
direction * 2.0 * self.attitude_gain_per_second[2] * error.z(),
);
let torque = self
.rate
.update(
target_rate,
BodyRate::radians_per_second(
state.angular_rate.vector().x(),
state.angular_rate.vector().y(),
state.angular_rate.vector().z(),
),
dt_seconds,
)
.map_err(|_| ())?;
let allocation = self
.allocator
.allocate_capped(
BodyWrench {
roll: torque.x(),
pitch: torque.y(),
yaw: torque.z(),
collective: (force.vector().norm()
/ self.thrust.maximum_collective_thrust_newtons)
.min(collective_cap),
},
dt_seconds,
collective_cap * 4.,
)
.map_err(|_| ())?;
if limited
|| allocation.saturated
|| force.vector().norm() / self.thrust.maximum_collective_thrust_newtons
> collective_cap
{
self.rate.reset_integrators();
}
Ok(allocation.commands)
}
pub fn reset(&mut self) {
self.rate.reset();
self.allocator.reset_commands();
}
pub fn capture_heading(&mut self, attitude: UnitQuaternion<Frd, Ned>) -> Result<(), ()> {
let forward = attitude.body_to_ned(Vector3::new(1., 0., 0.));
self.heading = Vector3::new(forward.x(), forward.y(), 0.)
.normalized()
.ok_or(())?;
Ok(())
}
pub fn heading_aligned(&self, attitude: UnitQuaternion<Frd, Ned>) -> bool {
let forward = attitude.body_to_ned(Vector3::new(1., 0., 0.));
Vector3::new(forward.x(), forward.y(), 0.)
.normalized()
.is_some_and(|v| v.dot(self.heading) >= 0.996_194_7)
}
}
firmware/libs/kaptan-flight-runtime/src/execution.rs
+2147
View File
File diff suppressed because it is too large. Load diff
firmware/libs/kaptan-flight-runtime/src/lib.rs
+339 -134
View File
@@ -1,148 +1,353 @@
//! Central, transport-neutral flight-runtime composition.
//! Deterministic sensor-driven flight endpoint. The caller owns the clock and
//! borrows its bounded output sink for each effectful call. Local publication
//! never proves remote application. Consumers enforce their own absolute expiry.
//!
//! This crate owns the one configuration manager and the bounded queue for the
//! image's MAVLink diagnostic sink. MAVLink adapters receive mutable borrows
//! for bounded dispatch or draining, but cannot own configuration lifecycle or
//! link state.
//! Callbacks must return errors, not panic. If an unwind crosses this boundary,
//! discard the endpoint and callback owner. A halted CPU does not inhibit remote
//! outputs; independently enforced consumer expiry remains required.
#![no_std]
#![forbid(unsafe_code)]
#[cfg(test)]
extern crate std;
use kaptan_config::{ConfigManager, ConfigStore, StartupError};
use kaptan_flight_config::{CandidateProfile, VehicleCapabilities};
use kaptan_log::{LogRecord, LogSinkQueue};
mod controller;
mod execution;
pub use execution::FlightEndpoint;
/// Default bounded allocation for the MAVLink diagnostic sink. Error records
/// are partitioned from optional detail.
type RuntimeMavlinkLogQueue = LogSinkQueue<8, 16>;
/// Core-owned time reserved for an admitted landing and its completion.
pub const LANDING_BUDGET_US: u64 = 20_000_000;
pub use kaptan_navigation::{
FlightEstimate, ImuCoverage, NavigationFailure, ObservationAdmission, ObservationRejection,
OriginId, SensorRecord,
};
use kaptan_navigation::{PositionNed, SensorStatus, VelocityNed};
use kaptan_time::Timestamp;
/// One boot-active vehicle runtime. It deliberately has no MAVLink dependency.
pub struct FlightRuntime<S> {
configuration: ConfigManager<CandidateProfile, S>,
mavlink_logs: RuntimeMavlinkLogQueue,
#[derive(Clone, Copy, Debug, Eq, PartialEq)]
pub struct CommandOwner(pub u64);
#[derive(Clone, Copy, Debug, Eq, PartialEq)]
pub struct TransportSessionId(pub u128);
#[derive(Clone, Copy, Debug, Eq, PartialEq)]
pub struct RuntimeEpoch {
session: TransportSessionId,
generation: u64,
}
impl<S> FlightRuntime<S>
where
S: ConfigStore<CandidateProfile>,
{
/// Recovers the central manager, validates its boot-active snapshot, and
/// copies the restart-only MAVLink sink policy into its bounded queue.
pub fn recover(
factory: CandidateProfile,
capabilities: VehicleCapabilities,
store: S,
) -> Result<Self, StartupError<S::Error>> {
let configuration = ConfigManager::recover(factory, capabilities, store)?;
let mavlink_logs = RuntimeMavlinkLogQueue::new(configuration.active().mavlink_logging());
Ok(Self {
configuration,
mavlink_logs,
})
}
/// The sole central configuration manager.
pub const fn configuration(&self) -> &ConfigManager<CandidateProfile, S> {
&self.configuration
}
/// Composition-owned admission boundary for one preformatted event.
pub fn publish_log_event(&mut self, record: LogRecord) {
self.mavlink_logs.publish_record(record);
}
/// Submits the next record and removes it only after transport admission.
pub fn submit_next_mavlink<R>(
&mut self,
submit: impl FnOnce(&LogRecord) -> (kaptan_log::TransportAdmission, R),
) -> Option<R> {
self.mavlink_logs.submit_next(submit)
}
}
#[cfg(test)]
mod tests {
use kaptan_config::{ConfigModel, ConfigStore, ConfigValue};
use kaptan_log::TransportAdmission;
use std::sync::Mutex;
use super::FlightRuntime;
static LOG_TEST: Mutex<()> = Mutex::new(());
struct Store;
impl ConfigStore<kaptan_flight_config::CandidateProfile> for Store {
type Error = ();
fn load(
&mut self,
_: &kaptan_flight_config::CandidateProfile,
) -> Result<Option<kaptan_flight_config::CandidateProfile>, Self::Error> {
Ok(None)
}
fn persist(
&mut self,
_: &kaptan_flight_config::CandidateProfile,
) -> Result<(), Self::Error> {
Ok(())
impl RuntimeEpoch {
pub const fn from_parts(session: TransportSessionId, generation: u64) -> Self {
Self {
session,
generation,
}
}
#[test]
fn runtime_uses_the_active_mavlink_policy_and_owns_one_sink_queue() {
let _guard = LOG_TEST.lock().unwrap();
while kaptan_log::drain_one(|_| ()).is_some() {}
let mut runtime = FlightRuntime::recover(
kaptan_flight_config::CandidateProfile::factory(),
kaptan_flight_config::VehicleCapabilities::quad_x(),
Store,
)
.expect("factory configuration validates");
assert_eq!(
runtime.configuration().active().mavlink_logging(),
kaptan_log::LevelFilter::Info
);
kaptan_log::info!("runtime ready");
runtime.publish_log_event(kaptan_log::drain_one(|record| *record).unwrap());
assert_eq!(
runtime.submit_next_mavlink(|record| {
assert_eq!(record.level(), kaptan_log::Level::Info);
(TransportAdmission::Accepted, ())
}),
Some(())
);
pub const fn session(self) -> TransportSessionId {
self.session
}
#[test]
fn disabled_mavlink_sink_filters_every_level() {
let _guard = LOG_TEST.lock().unwrap();
while kaptan_log::drain_one(|_| ()).is_some() {}
let mut factory = kaptan_flight_config::CandidateProfile::factory();
let level_filter = kaptan_flight_config::CandidateProfile::descriptors()
.find(|descriptor| descriptor.display_key == "mavlink.logging.level_filter")
.expect("MAVLink log-level descriptor");
factory
.set_field(level_filter.id, ConfigValue::U8(0))
.unwrap();
let mut runtime = FlightRuntime::recover(
factory,
kaptan_flight_config::VehicleCapabilities::quad_x(),
Store,
)
.expect("disabled MAVLink sink is valid");
assert_eq!(
runtime.configuration().active().mavlink_logging(),
kaptan_log::LevelFilter::Off
);
kaptan_log::error!("not routed to MAVLink");
runtime.publish_log_event(kaptan_log::drain_one(|record| *record).unwrap());
assert_eq!(
runtime.submit_next_mavlink(|_| (TransportAdmission::Accepted, ())),
None
);
pub const fn generation(self) -> u64 {
self.generation
}
}
#[derive(Clone, Copy, Debug, Eq, PartialEq, Ord, PartialOrd)]
pub struct RequestSequence(pub u64);
impl RequestSequence {
pub const fn checked_next(self) -> Option<Self> {
match self.0.checked_add(1) {
Some(v) => Some(Self(v)),
None => None,
}
}
}
#[derive(Clone, Copy, Debug, Eq, PartialEq)]
pub struct ArmId(pub u64);
#[derive(Clone, Copy, Debug, Eq, PartialEq)]
pub struct RunBinding {
pub session: TransportSessionId,
pub owner: CommandOwner,
}
#[derive(Clone, Copy, Debug, Eq, PartialEq)]
pub struct RequestContext {
pub epoch: RuntimeEpoch,
pub owner: CommandOwner,
pub sequence: RequestSequence,
pub issued_at: Timestamp,
pub expires_at: Timestamp,
pub time_uncertainty_microseconds: u32,
pub arm: Option<ArmId>,
}
#[derive(Clone, Copy, Debug, PartialEq)]
pub enum LocalSetpoint {
PositionYaw {
position: PositionNed,
yaw_radians: f32,
},
VelocityYaw {
velocity: VelocityNed,
yaw_radians: f32,
},
}
#[derive(Clone, Copy, Debug, PartialEq)]
pub enum FlightCommand {
Arm,
GroundDisarm,
Takeoff {
height_above_launch_metres: f32,
},
Hold,
EnterOffboard {
origin: OriginId,
initial: LocalSetpoint,
},
Land,
Terminate,
/// Take off, hold for 30 seconds after settling, then land and inhibit output.
TakeoffHoverLand {
height_above_launch_metres: f32,
},
}
impl FlightCommand {
pub const fn kind(self) -> CommandKind {
match self {
Self::Arm => CommandKind::Arm,
Self::GroundDisarm => CommandKind::GroundDisarm,
Self::Takeoff { .. } => CommandKind::Takeoff,
Self::Hold => CommandKind::Hold,
Self::EnterOffboard { .. } => CommandKind::EnterOffboard,
Self::Land => CommandKind::Land,
Self::Terminate => CommandKind::Terminate,
Self::TakeoffHoverLand { .. } => CommandKind::TakeoffHoverLand,
}
}
}
#[derive(Clone, Copy, Debug, PartialEq)]
pub struct CommandRequest {
pub context: RequestContext,
pub command: FlightCommand,
}
#[derive(Clone, Copy, Debug, PartialEq)]
pub struct SetpointRequest {
pub epoch: RuntimeEpoch,
pub owner: CommandOwner,
pub arm: ArmId,
pub origin: OriginId,
pub offboard: RequestSequence,
pub sequence: RequestSequence,
pub issued_at: Timestamp,
pub time_uncertainty_microseconds: u32,
pub setpoint: LocalSetpoint,
}
#[derive(Clone, Copy, Debug, Eq, PartialEq)]
#[repr(u8)]
pub enum CommandKind {
Arm = 1,
GroundDisarm = 2,
Takeoff = 3,
Hold = 4,
EnterOffboard = 5,
Land = 6,
Terminate = 7,
TakeoffHoverLand = 8,
}
#[derive(Clone, Copy, Debug, Eq, PartialEq)]
#[repr(u8)]
pub enum CommandLifecycle {
Rejected,
Accepted,
Completed,
Cancelled,
Failed,
}
#[derive(Clone, Copy, Debug, Eq, PartialEq)]
#[repr(u16)]
pub enum CommandReason {
None = 0,
WrongEpoch = 1,
WrongOwner = 2,
Replay = 3,
SequenceExhausted = 4,
InvalidTime = 5,
Expired = 6,
InvalidValue = 7,
NotReady = 8,
WrongPhase = 9,
WrongArm = 10,
WrongOrigin = 11,
WrongOffboard = 12,
MissingCapability = 13,
FallbackLatched = 14,
AlreadyActive = 15,
Superseded = 16,
PhaseDeadline = 17,
Terminated = 18,
}
#[derive(Clone, Copy, Debug, Eq, PartialEq)]
pub struct CommandStatus {
pub epoch: RuntimeEpoch,
pub owner: CommandOwner,
pub sequence: RequestSequence,
pub kind: CommandKind,
pub lifecycle: CommandLifecycle,
pub at: Timestamp,
pub reason: CommandReason,
pub arm: Option<ArmId>,
}
/// A committed terminal transition from the most recent effectful call.
/// The adapter must reconcile these records even when that call returned Err.
#[derive(Clone, Copy, Debug, Eq, PartialEq)]
pub struct CommandOutcome {
pub status: CommandStatus,
pub published: bool,
}
#[derive(Clone, Copy, Debug, Eq, PartialEq)]
pub struct SetpointStatus {
pub offboard: RequestSequence,
pub sequence: RequestSequence,
pub accepted: bool,
pub reason: CommandReason,
pub expires_at: Option<Timestamp>,
}
#[derive(Clone, Copy, Debug, Eq, PartialEq)]
#[repr(u8)]
pub enum VehiclePhase {
Disarmed,
PreArm,
ArmedIdle,
Takeoff,
Hold,
Offboard,
FallbackHold,
Land,
TouchdownConfirm,
Terminated,
}
#[derive(Clone, Copy, Debug, Eq, PartialEq)]
#[repr(u8)]
pub enum GroundState {
Unknown,
Airborne,
ContactCandidate,
Grounded,
}
#[derive(Clone, Copy, Debug, Eq, PartialEq)]
#[repr(u16)]
pub enum FallbackReason {
OffboardExpired = 1,
}
#[derive(Clone, Copy, Debug, Eq, PartialEq)]
#[repr(u16)]
pub enum TerminalReason {
Requested = 1,
ClockRegression = 2,
SequenceExhausted = 3,
ImuLost = 4,
NavigationLost = 5,
Numerical = 6,
ControlDeadline = 7,
OutputFault = 8,
LandingDeadline = 9,
FlightDeadline = 10,
IngressOverrun = 11,
}
/// Output evidence issued by the runtime. Consumers cannot grant permission
/// by changing the record's private fields.
/// ```compile_fail,E0616
/// use kaptan_flight_runtime::OutputRecord;
/// fn grant_permission(mut record: OutputRecord) {
/// record.permitted = true;
/// }
/// ```
#[derive(Clone, Copy, Debug, PartialEq)]
pub struct OutputRecord {
epoch: RuntimeEpoch,
arm: Option<ArmId>,
sequence: u64,
produced_at: Timestamp,
valid_until: Timestamp,
permitted: bool,
motors: [f32; 4],
}
impl OutputRecord {
pub const fn epoch(self) -> RuntimeEpoch {
self.epoch
}
pub const fn arm(self) -> Option<ArmId> {
self.arm
}
pub const fn sequence(self) -> u64 {
self.sequence
}
pub const fn produced_at(self) -> Timestamp {
self.produced_at
}
pub const fn valid_until(self) -> Timestamp {
self.valid_until
}
pub const fn permitted(self) -> bool {
self.permitted
}
pub const fn motors(self) -> [f32; 4] {
self.motors
}
}
pub trait FlightOutput {
type Error;
fn actuator(&mut self, record: OutputRecord) -> Result<(), Self::Error>;
fn command_status(&mut self, record: CommandStatus) -> Result<(), Self::Error>;
}
#[derive(Clone, Copy, Debug, PartialEq)]
pub struct FlightStatus {
pub epoch: RuntimeEpoch,
pub owner: CommandOwner,
pub at: Timestamp,
pub arm: Option<ArmId>,
pub phase: VehiclePhase,
pub phase_since: Timestamp,
pub phase_deadline: Option<Timestamp>,
pub flight_deadline: Option<Timestamp>,
pub fallback_latched: bool,
pub fallback_reason: Option<FallbackReason>,
pub terminal_reason: Option<TerminalReason>,
pub offboard: Option<RequestSequence>,
pub setpoint_sequence: Option<RequestSequence>,
pub setpoint_expires_at: Option<Timestamp>,
pub command_highwater: Option<RequestSequence>,
pub active_command: Option<CommandStatus>,
pub last_terminal_command: Option<CommandStatus>,
pub estimate: Option<FlightEstimate>,
pub ground_state: GroundState,
pub local_output_healthy: bool,
pub last_output_sequence: Option<u64>,
pub last_local_commit_at: Option<Timestamp>,
pub source_status: [SensorStatus; 5],
}
#[derive(Debug, PartialEq)]
pub struct EndpointError<E> {
pub primary: EndpointFailure<E>,
pub inhibition: Option<E>,
}
#[derive(Debug, PartialEq)]
pub enum EndpointFailure<E> {
ClockRegression,
EpochExhausted,
OutputSequenceExhausted,
NotDisarmed,
ControlDeadline,
Navigation(NavigationFailure),
Control,
Output(E),
Notification(E),
}
impl<E> From<EndpointFailure<E>> for EndpointError<E> {
fn from(primary: EndpointFailure<E>) -> Self {
Self {
primary,
inhibition: None,
}
}
}
#[derive(Clone, Copy, Debug, Eq, PartialEq)]
pub enum RuntimeConfigError {
Navigation,
Control,
Timing,
Safety,
}
firmware/libs/kaptan-flight-runtime/tests/lifecycle.rs
+586
View File
@@ -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));
}
firmware/libs/kaptan-flight-runtime/tests/navigation.rs
+164
View File
@@ -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()));
}
firmware/libs/kaptan-flight-runtime/tests/replay.rs
+413
View File
@@ -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());
}
}
firmware/libs/kaptan-flight-runtime/tests/support/mod.rs
+173
View File
@@ -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)
}