kaptan/firmware/libs/kaptan-flight-runtime/tests/support/mod.rs

#[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)
}