Onur Özkan
f9a61a086f
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.
869 lines
31 KiB
Rust
869 lines
31 KiB
Rust
//! Flight observation, uncertainty and bounded execution contracts.
|
|
|
|
use crate::*;
|
|
use kaptan_math::{Frd, Ned, UnitQuaternion, Vector3};
|
|
use kaptan_time::Timestamp;
|
|
|
|
#[derive(Clone, Copy, Debug, Eq, PartialEq)]
|
|
pub struct OriginId(pub u32);
|
|
#[derive(Clone, Copy, Debug, Eq, PartialEq)]
|
|
pub struct SensorReferenceId(pub u32);
|
|
|
|
/// Mean calibrated force/rate over the declared preceding acquisition window.
|
|
/// Interior propagation reconstructs that window with its constant average.
|
|
#[derive(Clone, Copy, Debug, PartialEq)]
|
|
pub struct FlightImuObservation {
|
|
pub context: ObservationContext,
|
|
pub acceleration: AccelerationFrd,
|
|
pub angular_rate: AngularRateFrd,
|
|
/// Positive duration in the endpoint clock, bounded by the configured IMU
|
|
/// interval. Both window endpoints must share the same timing model.
|
|
pub acquisition_duration_us: u32,
|
|
}
|
|
impl FlightImuObservation {
|
|
pub(crate) fn acquisition_start(self) -> Option<u64> {
|
|
if self.acquisition_duration_us == 0 {
|
|
return None;
|
|
}
|
|
self.context
|
|
.measurement_time
|
|
.as_microseconds()
|
|
.checked_sub(u64::from(self.acquisition_duration_us))
|
|
}
|
|
}
|
|
/// Timing of the latest valid admitted IMU window, possibly not yet propagated.
|
|
/// This historical snapshot does not establish freshness, estimator health or
|
|
/// output authority. It may remain available after a numerical failure.
|
|
#[derive(Clone, Copy, Debug, Eq, PartialEq)]
|
|
pub struct ImuCoverage {
|
|
pub end: Timestamp,
|
|
pub acquisition_duration_us: u32,
|
|
pub time_uncertainty_microseconds: u32,
|
|
}
|
|
#[derive(Clone, Copy, Debug, PartialEq)]
|
|
pub struct MagneticObservation {
|
|
pub context: ObservationContext,
|
|
pub microtesla_frd: Vector3<Frd>,
|
|
pub variance_microtesla_squared: [ScalarVariance; 3],
|
|
}
|
|
#[derive(Clone, Copy, Debug, PartialEq)]
|
|
pub struct PressureObservation {
|
|
pub context: ObservationContext,
|
|
pub pascals: f32,
|
|
pub variance_pascals_squared: ScalarVariance,
|
|
}
|
|
#[derive(Clone, Copy, Debug, PartialEq)]
|
|
pub enum SensorRecord {
|
|
Imu(FlightImuObservation),
|
|
LocalPositionVelocity(LocalPositionVelocityObservation),
|
|
Magnetic(MagneticObservation),
|
|
Pressure(PressureObservation),
|
|
Range(RangeObservation),
|
|
}
|
|
impl SensorRecord {
|
|
pub const fn context(self) -> ObservationContext {
|
|
match self {
|
|
Self::Imu(v) => v.context,
|
|
Self::LocalPositionVelocity(v) => v.context,
|
|
Self::Magnetic(v) => v.context,
|
|
Self::Pressure(v) => v.context,
|
|
Self::Range(v) => v.context,
|
|
}
|
|
}
|
|
pub const fn kind(self) -> u8 {
|
|
match self {
|
|
Self::Imu(_) => 1,
|
|
Self::LocalPositionVelocity(_) => 2,
|
|
Self::Magnetic(_) => 3,
|
|
Self::Pressure(_) => 4,
|
|
Self::Range(_) => 5,
|
|
}
|
|
}
|
|
}
|
|
|
|
#[derive(Clone, Copy, Debug, Eq, PartialEq)]
|
|
#[repr(u16)]
|
|
pub enum ObservationRejection {
|
|
WrongEpoch = 1,
|
|
InvalidTime = 2,
|
|
TooOld = 3,
|
|
Replay = 4,
|
|
SourceMismatch = 5,
|
|
FrameMismatch = 6,
|
|
CalibrationMismatch = 7,
|
|
InvalidValue = 8,
|
|
InvalidQuality = 9,
|
|
QueueFull = 10,
|
|
RateExceeded = 11,
|
|
BehindFusionHorizon = 12,
|
|
ReferenceMismatch = 13,
|
|
Terminated = 14,
|
|
}
|
|
#[derive(Clone, Copy, Debug, Eq, PartialEq)]
|
|
pub enum ObservationAdmission {
|
|
Queued,
|
|
Rejected(ObservationRejection),
|
|
}
|
|
#[derive(Clone, Copy, Debug, Eq, PartialEq)]
|
|
pub enum NavigationFailure {
|
|
ImuGap,
|
|
QueueOverrun,
|
|
WorkBudget,
|
|
Numerical,
|
|
RequiredCapabilityLost,
|
|
}
|
|
impl From<kaptan_math::MathError> for NavigationFailure {
|
|
fn from(_: kaptan_math::MathError) -> Self {
|
|
Self::Numerical
|
|
}
|
|
}
|
|
#[derive(Clone, Copy, Debug, Eq, PartialEq)]
|
|
pub enum SourceStatus {
|
|
Unknown,
|
|
Healthy,
|
|
Stale,
|
|
Isolated,
|
|
}
|
|
#[derive(Clone, Copy, Debug, Eq, PartialEq)]
|
|
pub struct SensorStatus {
|
|
pub kind: u8,
|
|
pub source: SourceId,
|
|
pub integrity: SourceStatus,
|
|
pub last_received_measurement: Option<Timestamp>,
|
|
pub last_fused_measurement: Option<Timestamp>,
|
|
pub queued: u16,
|
|
pub rejected: u32,
|
|
pub fused: u32,
|
|
pub dropped: u32,
|
|
}
|
|
|
|
#[derive(Clone, Copy, Debug, PartialEq)]
|
|
pub struct LocalOrigin {
|
|
pub id: OriginId,
|
|
pub established_at: Timestamp,
|
|
pub reference: SensorReferenceId,
|
|
pub launch_in_reference: PositionNed,
|
|
pub launch_variance_m2: [f32; 3],
|
|
}
|
|
#[derive(Clone, Copy, Debug, Eq, PartialEq)]
|
|
pub struct CapabilityEvidence {
|
|
pub available: bool,
|
|
pub last_measurement_at: Option<Timestamp>,
|
|
pub valid_until: Option<Timestamp>,
|
|
}
|
|
#[derive(Clone, Copy, Debug, Eq, PartialEq)]
|
|
pub struct FlightCapabilities {
|
|
pub tilt: CapabilityEvidence,
|
|
pub heading: CapabilityEvidence,
|
|
pub horizontal_position: CapabilityEvidence,
|
|
pub horizontal_velocity: CapabilityEvidence,
|
|
pub height: CapabilityEvidence,
|
|
pub vertical_velocity: CapabilityEvidence,
|
|
pub ground_clearance: CapabilityEvidence,
|
|
}
|
|
impl FlightCapabilities {
|
|
pub fn flight_ready(self) -> bool {
|
|
self.tilt.available
|
|
&& self.heading.available
|
|
&& self.horizontal_position.available
|
|
&& self.horizontal_velocity.available
|
|
&& self.height.available
|
|
&& self.vertical_velocity.available
|
|
&& self.ground_clearance.available
|
|
}
|
|
pub fn valid_until(self) -> Option<Timestamp> {
|
|
if !self.flight_ready() {
|
|
return None;
|
|
}
|
|
[
|
|
self.tilt,
|
|
self.heading,
|
|
self.horizontal_position,
|
|
self.horizontal_velocity,
|
|
self.height,
|
|
self.vertical_velocity,
|
|
self.ground_clearance,
|
|
]
|
|
.into_iter()
|
|
.map(|v| v.valid_until)
|
|
.try_fold(Timestamp::from_microseconds(u64::MAX), |a, b| {
|
|
Some(a.min(b?))
|
|
})
|
|
}
|
|
}
|
|
#[derive(Clone, Copy, Debug, PartialEq)]
|
|
pub struct GroundClearance {
|
|
pub measurement_at: Timestamp,
|
|
pub valid_until: Timestamp,
|
|
pub source: SourceId,
|
|
pub body_origin_height_metres: f32,
|
|
pub contact_clearance_metres: f32,
|
|
pub variance_m2: f32,
|
|
pub measurement_time_uncertainty_microseconds: u32,
|
|
pub upper_error_metres: f32,
|
|
}
|
|
impl GroundClearance {
|
|
/// Bound current clearance using current foot orientation and velocity
|
|
/// confidence. `upper_error_metres` bounds measured COM height, not a foot
|
|
/// projection frozen at the range timestamp.
|
|
pub fn upper_at(
|
|
self,
|
|
now: Timestamp,
|
|
estimate: &FlightEstimate,
|
|
contact: ContactGeometry,
|
|
) -> Option<f32> {
|
|
let speed_bound =
|
|
estimate.velocity_error_bound().norm() + estimate.velocity.vector().norm();
|
|
let angle_bound = estimate.attitude_error_bound().norm();
|
|
if now >= self.valid_until
|
|
|| estimate.at != now
|
|
|| !speed_bound.is_finite()
|
|
|| !angle_bound.is_finite()
|
|
|| !contact.validate()
|
|
{
|
|
return None;
|
|
}
|
|
let earliest = self
|
|
.measurement_at
|
|
.as_microseconds()
|
|
.checked_sub(u64::from(self.measurement_time_uncertainty_microseconds))?;
|
|
let age = now.as_microseconds().checked_sub(earliest)? as f32 * 1e-6;
|
|
// Configuration checks the vehicle's free-flight acceleration bound.
|
|
let feet = contact.depth(estimate.attitude);
|
|
let upper = self.body_origin_height_metres
|
|
+ self.upper_error_metres
|
|
+ speed_bound * age
|
|
+ 0.5 * MAXIMUM_FREE_ACCELERATION_MPS2 * age * age
|
|
- feet
|
|
+ contact.rotation_radius_bound() * angle_bound;
|
|
upper.is_finite().then_some(upper)
|
|
}
|
|
}
|
|
#[derive(Clone, Copy, Debug, PartialEq)]
|
|
pub struct FlightEstimate {
|
|
pub at: Timestamp,
|
|
pub fused_through: Timestamp,
|
|
/// Latest complete IMU acquisition-window end, not its start or receipt.
|
|
pub last_imu_at: Timestamp,
|
|
pub origin: LocalOrigin,
|
|
pub attitude: UnitQuaternion<Frd, Ned>,
|
|
pub position: PositionNed,
|
|
pub velocity: VelocityNed,
|
|
/// Bias-corrected average of the latest complete declared window. This is not
|
|
/// an instantaneous angular-rate measurement at `at`.
|
|
pub angular_rate: AngularRateFrd,
|
|
pub position_variance_m2: [f32; 3],
|
|
pub velocity_variance_m2ps2: [f32; 3],
|
|
pub attitude_variance_rad2: [f32; 3],
|
|
pub timing_error_bound: TimingErrorBounds,
|
|
pub capabilities: FlightCapabilities,
|
|
pub ground_clearance: Option<GroundClearance>,
|
|
}
|
|
/// Deterministic absolute error radii from timing, separate from Gaussian P.
|
|
#[derive(Clone, Copy, Debug, PartialEq)]
|
|
pub struct TimingErrorBounds {
|
|
pub position_metres: [f32; 3],
|
|
pub velocity_mps: [f32; 3],
|
|
pub attitude_radians: [f32; 3],
|
|
}
|
|
impl TimingErrorBounds {
|
|
pub const ZERO: Self = Self {
|
|
position_metres: [0.0; 3],
|
|
velocity_mps: [0.0; 3],
|
|
attitude_radians: [0.0; 3],
|
|
};
|
|
}
|
|
impl FlightEstimate {
|
|
pub fn velocity_error_bound(&self) -> Vector3<Ned> {
|
|
let e: [f32; 3] = core::array::from_fn(|i| {
|
|
3.0 * kaptan_math::sqrt(self.velocity_variance_m2ps2[i])
|
|
+ self.timing_error_bound.velocity_mps[i]
|
|
});
|
|
Vector3::new(e[0], e[1], e[2])
|
|
}
|
|
pub fn attitude_error_bound(&self) -> Vector3<Ned> {
|
|
let e: [f32; 3] = core::array::from_fn(|i| {
|
|
3.0 * kaptan_math::sqrt(self.attitude_variance_rad2[i])
|
|
+ self.timing_error_bound.attitude_radians[i]
|
|
});
|
|
Vector3::new(e[0], e[1], e[2])
|
|
}
|
|
}
|
|
/// Four equal contact spheres at the corners of a body-FRD rectangle.
|
|
#[derive(Clone, Copy, Debug, PartialEq)]
|
|
pub struct ContactGeometry {
|
|
pub half_span_x_metres: f32,
|
|
pub half_span_y_metres: f32,
|
|
pub centre_z_metres: f32,
|
|
pub radius_metres: f32,
|
|
}
|
|
impl ContactGeometry {
|
|
pub(crate) fn validate(self) -> bool {
|
|
self.half_span_x_metres.is_finite()
|
|
&& self.half_span_x_metres > 0.0
|
|
&& self.half_span_y_metres.is_finite()
|
|
&& self.half_span_y_metres > 0.0
|
|
&& self.centre_z_metres.is_finite()
|
|
&& self.radius_metres.is_finite()
|
|
&& self.radius_metres >= 0.0
|
|
&& (self.rotation_radius_bound() + self.radius_metres).is_finite()
|
|
}
|
|
fn rotation_radius_bound(self) -> f32 {
|
|
l1_bound(Vector3::new(
|
|
self.half_span_x_metres,
|
|
self.half_span_y_metres,
|
|
self.centre_z_metres,
|
|
))
|
|
}
|
|
pub(crate) fn depth(self, q: UnitQuaternion<Frd, Ned>) -> f32 {
|
|
let mut depth = f32::NEG_INFINITY;
|
|
for x in [-self.half_span_x_metres, self.half_span_x_metres] {
|
|
for y in [-self.half_span_y_metres, self.half_span_y_metres] {
|
|
let foot = q.body_to_ned(Vector3::new(x, y, self.centre_z_metres)).z()
|
|
+ self.radius_metres;
|
|
if !foot.is_finite() {
|
|
return f32::NAN;
|
|
}
|
|
depth = depth.max(foot);
|
|
}
|
|
}
|
|
depth
|
|
}
|
|
}
|
|
|
|
/// Selected source for each supported observation kind.
|
|
#[derive(Clone, Copy, Debug, Eq, PartialEq)]
|
|
pub struct NavigationSources {
|
|
pub imu: SourceId,
|
|
pub local_position_velocity: SourceId,
|
|
pub magnetic: SourceId,
|
|
pub pressure: SourceId,
|
|
pub range: SourceId,
|
|
}
|
|
impl NavigationSources {
|
|
pub(crate) const fn ordered(self) -> [SourceId; 5] {
|
|
[
|
|
self.imu,
|
|
self.local_position_velocity,
|
|
self.magnetic,
|
|
self.pressure,
|
|
self.range,
|
|
]
|
|
}
|
|
}
|
|
|
|
/// Range-sensor extrinsics and deterministic quantization error.
|
|
#[derive(Clone, Copy, Debug, PartialEq)]
|
|
pub struct RangeInstallation {
|
|
pub frame: FrameId,
|
|
pub offset_frd_metres: Vector3<Frd>,
|
|
/// Finite nonzero beam direction, normalized once by navigation construction.
|
|
pub direction_frd: Vector3<Frd>,
|
|
pub quantization_error_metres: f32,
|
|
}
|
|
impl RangeInstallation {
|
|
fn validate(self) -> bool {
|
|
self.offset_frd_metres.is_finite()
|
|
&& l1_bound(self.offset_frd_metres).is_finite()
|
|
&& self.direction_frd.normalized().is_some()
|
|
&& self.quantization_error_metres.is_finite()
|
|
&& self.quantization_error_metres >= 0.0
|
|
}
|
|
pub(crate) fn lever_bound(self) -> f32 {
|
|
l1_bound(self.offset_frd_metres)
|
|
}
|
|
}
|
|
|
|
// The L1 norm bounds Euclidean radius. Round both additions outward.
|
|
fn l1_bound(v: Vector3<Frd>) -> f32 {
|
|
((v.x().abs() + v.y().abs()).next_up() + v.z().abs()).next_up()
|
|
}
|
|
|
|
/// Local isothermal pressure reference. The conversion accepts 0.95..1.05 of it.
|
|
#[derive(Clone, Copy, Debug, PartialEq)]
|
|
pub struct BarometricReference {
|
|
pub pressure_pascals: f32,
|
|
pub temperature_kelvin: f32,
|
|
}
|
|
impl BarometricReference {
|
|
pub(crate) fn height_scale(self) -> f32 {
|
|
287.05 * self.temperature_kelvin / GRAVITY_MPS2
|
|
}
|
|
fn validate(self) -> bool {
|
|
let lower = 0.95 * self.pressure_pascals;
|
|
let upper = 1.05 * self.pressure_pascals;
|
|
let scale = self.height_scale();
|
|
self.pressure_pascals.is_finite()
|
|
&& self.pressure_pascals > 0.0
|
|
&& self.temperature_kelvin.is_finite()
|
|
&& self.temperature_kelvin > 0.0
|
|
&& (self.pressure_pascals + upper).is_finite()
|
|
&& scale.is_finite()
|
|
&& (scale / lower).is_finite()
|
|
&& scale / upper > 0.0
|
|
}
|
|
}
|
|
|
|
/// Required compiled installation. Calibration identifies one coherent set of
|
|
/// mounting, calibration and uncertainty data, changed only by reinitialization.
|
|
#[derive(Clone, Copy, Debug, PartialEq)]
|
|
pub struct NavigationInstallation {
|
|
pub sources: NavigationSources,
|
|
pub range: RangeInstallation,
|
|
pub contact: ContactGeometry,
|
|
pub atmosphere: BarometricReference,
|
|
pub magnetic_field_ned_microtesla: Vector3<Ned>,
|
|
pub calibration: CalibrationRevision,
|
|
pub reference: SensorReferenceId,
|
|
}
|
|
impl NavigationInstallation {
|
|
fn validate(self) -> bool {
|
|
self.range.validate()
|
|
&& self.contact.validate()
|
|
&& self.atmosphere.validate()
|
|
&& self.magnetic_field_ned_microtesla.is_finite()
|
|
&& Vector3::<Ned>::new(
|
|
self.magnetic_field_ned_microtesla.x(),
|
|
self.magnetic_field_ned_microtesla.y(),
|
|
0.0,
|
|
)
|
|
.norm()
|
|
> 10.0
|
|
&& self.magnetic_field_ned_microtesla.norm() < 100.0
|
|
&& self.reference.0 != 0
|
|
}
|
|
}
|
|
|
|
/// Continuous process-noise densities in SI units.
|
|
#[derive(Clone, Copy, Debug, PartialEq)]
|
|
pub struct NoiseDensities {
|
|
pub gyro: f32, // rad²/s
|
|
pub acceleration: f32, // m²/s³
|
|
pub gyro_bias: f32, // rad²/s³
|
|
pub acceleration_bias: f32, // m²/s⁵
|
|
pub barometric_datum: f32, // m²/s
|
|
}
|
|
#[derive(Clone, Copy, Debug, PartialEq)]
|
|
pub struct NavigationConfig {
|
|
pub noise: NoiseDensities,
|
|
/// Acquisition-window, numerical-step and current-extrapolation ceiling.
|
|
pub maximum_imu_interval_microseconds: u64,
|
|
pub imu_freshness_microseconds: u64,
|
|
pub aiding_freshness_microseconds: u64,
|
|
pub maximum_time_uncertainty_microseconds: u32,
|
|
pub lag_microseconds: u64,
|
|
pub installation: NavigationInstallation,
|
|
}
|
|
impl NavigationConfig {
|
|
#[cfg(test)]
|
|
pub const fn engineering_fixture() -> Self {
|
|
Self {
|
|
noise: NoiseDensities {
|
|
gyro: 2.5e-7,
|
|
acceleration: 2.5e-5,
|
|
gyro_bias: 1e-10,
|
|
acceleration_bias: 1e-8,
|
|
barometric_datum: 1e-5,
|
|
},
|
|
maximum_imu_interval_microseconds: 20_000,
|
|
imu_freshness_microseconds: 20_000,
|
|
aiding_freshness_microseconds: 250_000,
|
|
maximum_time_uncertainty_microseconds: 2_000,
|
|
lag_microseconds: 100_000,
|
|
installation: NavigationInstallation {
|
|
sources: NavigationSources {
|
|
imu: SourceId::new(1),
|
|
local_position_velocity: SourceId::new(2),
|
|
magnetic: SourceId::new(3),
|
|
pressure: SourceId::new(4),
|
|
range: SourceId::new(5),
|
|
},
|
|
range: RangeInstallation {
|
|
frame: FrameId::new(3),
|
|
offset_frd_metres: Vector3::new(0.0, 0.0, 0.04),
|
|
direction_frd: Vector3::new(0.0, 0.0, 1.0),
|
|
quantization_error_metres: 0.0005,
|
|
},
|
|
contact: ContactGeometry {
|
|
half_span_x_metres: 0.12,
|
|
half_span_y_metres: 0.12,
|
|
centre_z_metres: 0.065,
|
|
radius_metres: 0.015,
|
|
},
|
|
atmosphere: BarometricReference {
|
|
pressure_pascals: 101_325.0,
|
|
temperature_kelvin: 288.15,
|
|
},
|
|
magnetic_field_ned_microtesla: Vector3::new(20.0, 0.0, 45.0),
|
|
calibration: CalibrationRevision::new(1),
|
|
reference: SensorReferenceId(1),
|
|
},
|
|
}
|
|
}
|
|
pub fn validate(self) -> bool {
|
|
let n = self.noise;
|
|
[
|
|
n.gyro,
|
|
n.acceleration,
|
|
n.gyro_bias,
|
|
n.acceleration_bias,
|
|
n.barometric_datum,
|
|
]
|
|
.iter()
|
|
.all(|v| v.is_finite() && *v > 0.0 && *v <= 1.0)
|
|
&& (10_000..=20_000).contains(&self.maximum_imu_interval_microseconds)
|
|
&& self.imu_freshness_microseconds >= self.maximum_imu_interval_microseconds
|
|
&& self.imu_freshness_microseconds <= 20_000
|
|
&& self.lag_microseconds == 100_000
|
|
&& (200_000..=500_000).contains(&self.aiding_freshness_microseconds)
|
|
&& self.maximum_time_uncertainty_microseconds <= 2_000
|
|
&& self.installation.validate()
|
|
}
|
|
}
|
|
|
|
pub(crate) fn earliest(c: ObservationContext) -> Option<u64> {
|
|
c.measurement_time
|
|
.as_microseconds()
|
|
.checked_sub(u64::from(c.time_uncertainty_microseconds))
|
|
}
|
|
pub(crate) fn expiry(c: ObservationContext, age: u64) -> Option<Timestamp> {
|
|
Some(Timestamp::from_microseconds(earliest(c)?.checked_add(age)?))
|
|
}
|
|
|
|
#[cfg(test)]
|
|
mod tests {
|
|
use super::*;
|
|
fn contact() -> ContactGeometry {
|
|
NavigationConfig::engineering_fixture().installation.contact
|
|
}
|
|
fn time(v: u64) -> Timestamp {
|
|
Timestamp::from_microseconds(v)
|
|
}
|
|
fn estimate(at: Timestamp) -> FlightEstimate {
|
|
let capability = CapabilityEvidence {
|
|
available: true,
|
|
last_measurement_at: Some(at),
|
|
valid_until: Some(time(u64::MAX)),
|
|
};
|
|
FlightEstimate {
|
|
at,
|
|
fused_through: at,
|
|
last_imu_at: at,
|
|
origin: LocalOrigin {
|
|
id: OriginId(1),
|
|
established_at: at,
|
|
reference: SensorReferenceId(1),
|
|
launch_in_reference: PositionNed::metres(0.0, 0.0, 0.0),
|
|
launch_variance_m2: [0.0; 3],
|
|
},
|
|
attitude: UnitQuaternion::identity(),
|
|
position: PositionNed::metres(0.0, 0.0, 0.0),
|
|
velocity: VelocityNed::metres_per_second(0.1, 0.0, 0.0),
|
|
angular_rate: AngularRateFrd::radians_per_second(0.0, 0.0, 0.0),
|
|
position_variance_m2: [0.0; 3],
|
|
velocity_variance_m2ps2: [0.0; 3],
|
|
attitude_variance_rad2: [0.0; 3],
|
|
timing_error_bound: TimingErrorBounds::ZERO,
|
|
capabilities: FlightCapabilities {
|
|
tilt: capability,
|
|
heading: capability,
|
|
horizontal_position: capability,
|
|
horizontal_velocity: capability,
|
|
height: capability,
|
|
vertical_velocity: capability,
|
|
ground_clearance: capability,
|
|
},
|
|
ground_clearance: None,
|
|
}
|
|
}
|
|
#[test]
|
|
fn capability_expiry_requires_every_requested_capability() {
|
|
let evidence = CapabilityEvidence {
|
|
available: true,
|
|
last_measurement_at: Some(time(10)),
|
|
valid_until: Some(time(100)),
|
|
};
|
|
let healthy = FlightCapabilities {
|
|
tilt: evidence,
|
|
heading: evidence,
|
|
horizontal_position: evidence,
|
|
horizontal_velocity: evidence,
|
|
height: evidence,
|
|
vertical_velocity: evidence,
|
|
ground_clearance: evidence,
|
|
};
|
|
assert_eq!(healthy.valid_until(), Some(time(100)));
|
|
for index in 0..7 {
|
|
let mut c = healthy;
|
|
[
|
|
&mut c.tilt,
|
|
&mut c.heading,
|
|
&mut c.horizontal_position,
|
|
&mut c.horizontal_velocity,
|
|
&mut c.height,
|
|
&mut c.vertical_velocity,
|
|
&mut c.ground_clearance,
|
|
]
|
|
.into_iter()
|
|
.nth(index)
|
|
.unwrap()
|
|
.available = false;
|
|
assert!(!c.flight_ready());
|
|
assert_eq!(c.valid_until(), None);
|
|
}
|
|
let mut c = healthy;
|
|
c.ground_clearance.valid_until = None;
|
|
assert_eq!(c.valid_until(), None);
|
|
c = healthy;
|
|
c.heading.valid_until = Some(time(50));
|
|
assert_eq!(c.valid_until(), Some(time(50)));
|
|
}
|
|
#[test]
|
|
fn clearance_bound_uses_earliest_capture_and_rejects_invalid_arithmetic() {
|
|
let c = GroundClearance {
|
|
measurement_at: time(10_000),
|
|
valid_until: time(60_000),
|
|
source: SourceId::new(5),
|
|
body_origin_height_metres: 0.08,
|
|
contact_clearance_metres: 0.0,
|
|
variance_m2: 4e-6,
|
|
measurement_time_uncertainty_microseconds: 1_000,
|
|
upper_error_metres: 0.01,
|
|
};
|
|
let e = estimate(time(20_000));
|
|
assert!((c.upper_at(time(20_000), &e, contact()).unwrap() - 0.012_612_5).abs() < 1e-7);
|
|
assert_eq!(
|
|
c.upper_at(time(60_000), &estimate(time(60_000)), contact()),
|
|
None
|
|
);
|
|
for speed in [f32::NAN, f32::INFINITY] {
|
|
let mut bad = e;
|
|
bad.velocity = VelocityNed::metres_per_second(speed, 0.0, 0.0);
|
|
assert_eq!(c.upper_at(time(20_000), &bad, contact()), None);
|
|
}
|
|
assert_eq!(
|
|
c.upper_at(time(8_000), &estimate(time(8_000)), contact()),
|
|
None
|
|
);
|
|
assert_eq!(c.upper_at(time(20_001), &e, contact()), None);
|
|
let mut bad_angle = e;
|
|
bad_angle.attitude_variance_rad2[0] = f32::NAN;
|
|
assert_eq!(c.upper_at(time(20_000), &bad_angle, contact()), None);
|
|
let mut invalid_contact = contact();
|
|
invalid_contact.radius_metres = -0.01;
|
|
assert_eq!(c.upper_at(time(20_000), &e, invalid_contact), None);
|
|
assert_eq!(
|
|
GroundClearance {
|
|
measurement_time_uncertainty_microseconds: 20_000,
|
|
..c
|
|
}
|
|
.upper_at(time(20_000), &e, contact()),
|
|
None
|
|
);
|
|
assert_eq!(
|
|
GroundClearance {
|
|
valid_until: time(3_000_000),
|
|
..c
|
|
}
|
|
.upper_at(
|
|
time(2_000_000),
|
|
&FlightEstimate {
|
|
at: time(2_000_000),
|
|
velocity: VelocityNed::metres_per_second(f32::MAX, 0.0, 0.0),
|
|
..e
|
|
},
|
|
contact(),
|
|
),
|
|
None
|
|
);
|
|
}
|
|
#[test]
|
|
fn current_foot_geometry_and_velocity_confidence_bound_landing_clearance() {
|
|
// Independent rigid geometry: fixed COM 0.132 m; foot-sphere centres
|
|
// (+/-0.12,+/-0.12,0.065), radius 0.015. Roll changes 0.2 -> 0.
|
|
let old_depth = 0.12_f64 * 0.2_f64.sin() + 0.065 * 0.2_f64.cos() + 0.015;
|
|
let measured_contact = 0.132 - old_depth;
|
|
let old_bound = measured_contact + 0.0075 + 0.2 * 0.020 + 0.5 * 25.0 * 0.020_f64.powi(2);
|
|
assert!(old_bound < 0.05);
|
|
let actual_now = 0.132 - (0.065 + 0.015);
|
|
let c = GroundClearance {
|
|
measurement_at: time(10_000),
|
|
valid_until: time(70_000),
|
|
source: SourceId::new(5),
|
|
body_origin_height_metres: 0.132,
|
|
contact_clearance_metres: measured_contact as f32,
|
|
variance_m2: 4e-6,
|
|
measurement_time_uncertainty_microseconds: 0,
|
|
upper_error_metres: 0.0075,
|
|
};
|
|
let mut e = estimate(time(30_000));
|
|
e.velocity = VelocityNed::metres_per_second(0.0, 0.0, 0.0);
|
|
let upper = c.upper_at(e.at, &e, contact()).unwrap();
|
|
assert!(f64::from(upper) >= actual_now);
|
|
assert!(upper > 0.05); // neither cap entry nor contact dwell may use the old bound
|
|
e.velocity_variance_m2ps2 = [0.04; 3];
|
|
let uncertain = c.upper_at(e.at, &e, contact()).unwrap();
|
|
assert!(uncertain > upper + 0.020); // 3*sqrt(trace(Pv))*20 ms, not fixed +0.2 m/s
|
|
}
|
|
#[test]
|
|
fn contact_geometry() {
|
|
for geometry in [
|
|
contact(),
|
|
ContactGeometry {
|
|
half_span_x_metres: 0.3,
|
|
half_span_y_metres: 0.2,
|
|
centre_z_metres: 0.15,
|
|
radius_metres: 0.025,
|
|
},
|
|
] {
|
|
let x = f64::from(geometry.half_span_x_metres);
|
|
let y = f64::from(geometry.half_span_y_metres);
|
|
let z = f64::from(geometry.centre_z_metres);
|
|
assert!(f64::from(geometry.rotation_radius_bound()) >= (x * x + y * y + z * z).sqrt());
|
|
for (roll, pitch) in [(-0.5_f64, 0.2_f64), (0.0, 0.0), (0.4, -0.3)] {
|
|
let (sr, cr) = ((roll / 2.0).sin(), (roll / 2.0).cos());
|
|
let (sp, cp) = ((pitch / 2.0).sin(), (pitch / 2.0).cos());
|
|
let q = UnitQuaternion::from_components(
|
|
(cp * cr) as f32,
|
|
(cp * sr) as f32,
|
|
(sp * cr) as f32,
|
|
(-sp * sr) as f32,
|
|
)
|
|
.unwrap();
|
|
let expected = x * pitch.sin().abs()
|
|
+ y * (pitch.cos() * roll.sin()).abs()
|
|
+ z * pitch.cos() * roll.cos()
|
|
+ f64::from(geometry.radius_metres);
|
|
assert!((f64::from(geometry.depth(q)) - expected).abs() < 1e-7);
|
|
let e = FlightEstimate {
|
|
attitude: q,
|
|
..estimate(time(10_000))
|
|
};
|
|
let c = GroundClearance {
|
|
measurement_at: e.at,
|
|
valid_until: time(70_000),
|
|
source: SourceId::new(90),
|
|
body_origin_height_metres: 1.0,
|
|
contact_clearance_metres: 0.0,
|
|
variance_m2: 1e-6,
|
|
measurement_time_uncertainty_microseconds: 0,
|
|
upper_error_metres: 0.001,
|
|
};
|
|
assert!(f64::from(c.upper_at(e.at, &e, geometry).unwrap()) >= 1.0 - expected);
|
|
}
|
|
}
|
|
}
|
|
#[test]
|
|
fn installation_validation() {
|
|
let fixture = NavigationConfig::engineering_fixture();
|
|
for field in 0..9 {
|
|
for bad in [f32::NAN, f32::INFINITY, -1.0] {
|
|
let mut config = fixture;
|
|
let i = &mut config.installation;
|
|
match field {
|
|
0 => i.contact.half_span_x_metres = bad,
|
|
1 => i.contact.half_span_y_metres = bad,
|
|
2 => i.contact.radius_metres = bad,
|
|
3 => i.range.quantization_error_metres = bad,
|
|
4 => i.atmosphere.pressure_pascals = bad,
|
|
5 => i.atmosphere.temperature_kelvin = bad,
|
|
6 if !bad.is_finite() => i.contact.centre_z_metres = bad,
|
|
7 if !bad.is_finite() => {
|
|
i.range.offset_frd_metres = Vector3::new(bad, 0.0, 0.0)
|
|
}
|
|
8 if !bad.is_finite() => i.range.direction_frd = Vector3::new(0.0, 0.0, bad),
|
|
_ => continue,
|
|
}
|
|
assert!(Navigation::new(config).is_err());
|
|
}
|
|
}
|
|
for field in 0..6 {
|
|
let mut config = fixture;
|
|
let i = &mut config.installation;
|
|
match field {
|
|
0 => i.range.direction_frd = Vector3::zero(),
|
|
1 => i.contact.half_span_x_metres = 0.0,
|
|
2 => i.contact.half_span_y_metres = 0.0,
|
|
3 => i.atmosphere.pressure_pascals = 0.0,
|
|
4 => i.atmosphere.temperature_kelvin = 0.0,
|
|
_ => i.atmosphere.pressure_pascals = f32::MAX,
|
|
}
|
|
assert!(Navigation::new(config).is_err());
|
|
}
|
|
let mut config = fixture;
|
|
config.installation.contact.half_span_x_metres = f32::MAX;
|
|
config.installation.contact.half_span_y_metres = f32::MAX;
|
|
assert!(Navigation::new(config).is_err());
|
|
config = fixture;
|
|
config.installation.range.offset_frd_metres = Vector3::new(f32::MAX, f32::MAX, 0.0);
|
|
assert!(Navigation::new(config).is_err());
|
|
for (pressure_pascals, temperature_kelvin) in [
|
|
(101_325.0, f32::MAX),
|
|
(f32::MIN_POSITIVE, 288.15),
|
|
(1e20, f32::from_bits(1)),
|
|
] {
|
|
config = fixture;
|
|
config.installation.atmosphere = BarometricReference {
|
|
pressure_pascals,
|
|
temperature_kelvin,
|
|
};
|
|
assert!(Navigation::new(config).is_err());
|
|
}
|
|
}
|
|
#[test]
|
|
fn navigation_configuration_rejects_each_invalid_domain() {
|
|
let factory = NavigationConfig::engineering_fixture();
|
|
assert!(factory.validate());
|
|
for index in 0..5 {
|
|
for bad in [f32::NAN, 0.0, -1.0, 1.01] {
|
|
let mut c = factory;
|
|
*[
|
|
&mut c.noise.gyro,
|
|
&mut c.noise.acceleration,
|
|
&mut c.noise.gyro_bias,
|
|
&mut c.noise.acceleration_bias,
|
|
&mut c.noise.barometric_datum,
|
|
]
|
|
.into_iter()
|
|
.nth(index)
|
|
.unwrap() = bad;
|
|
assert!(!c.validate());
|
|
assert!(crate::Navigation::new(c).is_err());
|
|
}
|
|
}
|
|
let mut c = factory;
|
|
c.maximum_imu_interval_microseconds = 9_999;
|
|
assert!(!c.validate());
|
|
c = factory;
|
|
c.imu_freshness_microseconds = 19_999;
|
|
assert!(!c.validate());
|
|
c = factory;
|
|
c.imu_freshness_microseconds = 20_001;
|
|
assert!(!c.validate());
|
|
c = factory;
|
|
c.lag_microseconds = 0;
|
|
assert!(!c.validate());
|
|
c = factory;
|
|
c.aiding_freshness_microseconds = 100_000;
|
|
assert!(!c.validate());
|
|
c = factory;
|
|
c.maximum_time_uncertainty_microseconds = 2_001;
|
|
assert!(!c.validate());
|
|
c = factory;
|
|
c.installation.magnetic_field_ned_microtesla = Vector3::new(f32::NAN, 0.0, 45.0);
|
|
assert!(!c.validate());
|
|
c = factory;
|
|
c.installation.magnetic_field_ned_microtesla = Vector3::new(10.0, 0.0, 45.0);
|
|
assert!(!c.validate());
|
|
c = factory;
|
|
c.installation.magnetic_field_ned_microtesla = Vector3::new(20.0, f32::MAX, 45.0);
|
|
assert!(!c.validate());
|
|
c = factory;
|
|
c.installation.reference = SensorReferenceId(0);
|
|
assert!(!c.validate());
|
|
c = factory;
|
|
c.installation.magnetic_field_ned_microtesla = Vector3::new(0.0, 20.0, 45.0);
|
|
assert!(c.validate());
|
|
}
|
|
}
|