feat(firmware): validate configuration and migrate sensor-driven flight APIs
Validate configuration through its owning schemas and keep board declarations separate from shared F722 resource ownership. Bound USB transitions, isolate transport failures and retain restart-only activation. Migrate navigation and flight consumers with the sensor and schema contracts. Require explicit installation data and synchronous output authority; board motor outputs remain inhibited. Validation: assembled-prefix host tests, strict Clippy, platform/USB tests and both ARM release builds. Final-source coverage and scoped Miri/Kani evidence are retained separately from hardware qualification.
This commit is contained in:
Onur Özkan committed
1 parent
fb1b8af2d1
commit
f9a61a086f
211 files changed
+45657
-12740
No files matched your search
@@ -0,0 +1,275 @@
|
||||
mod support;
|
||||
|
||||
use kaptan_config::{ConfigModel, ConfigValue, ConfigViolation, FieldId, ValidationRule};
|
||||
use kaptan_control::{
|
||||
HeadingNed, KinematicLimits, Pid, PidConfig, ThrustEnvelope, Trajectory,
|
||||
TranslationalControlConfig, TranslationalController, TranslationalEstimate, force_to_attitude,
|
||||
};
|
||||
use kaptan_flight_config::{CandidateProfile, VehicleCapabilities};
|
||||
use kaptan_navigation::{GRAVITY_MPS2, MAXIMUM_FREE_ACCELERATION_MPS2, PositionNed, VelocityNed};
|
||||
|
||||
const CAPABILITIES: VehicleCapabilities = support::capabilities();
|
||||
const IMU_INTERVAL: &str = "timing.imu_max_interval_us";
|
||||
const IMU_FRESHNESS: &str = "timing.imu_freshness_us";
|
||||
const DOWNWARD_ACCELERATION: &str = "control.maximum_downward_acceleration_mps2";
|
||||
|
||||
fn id(key: &str) -> FieldId {
|
||||
CandidateProfile::descriptors()
|
||||
.find(|field| field.display_key == key)
|
||||
.unwrap()
|
||||
.id
|
||||
}
|
||||
|
||||
fn set(candidate: &mut CandidateProfile, key: &str, value: ConfigValue) {
|
||||
candidate.set_field(id(key), value).unwrap();
|
||||
}
|
||||
|
||||
fn with_interval(microseconds: u64) -> CandidateProfile {
|
||||
let mut candidate = support::candidate();
|
||||
set(&mut candidate, IMU_INTERVAL, ConfigValue::U64(microseconds));
|
||||
set(&mut candidate, IMU_FRESHNESS, ConfigValue::U64(20_000));
|
||||
candidate
|
||||
}
|
||||
|
||||
fn assert_rejected(candidate: CandidateProfile, issue: ConfigViolation) {
|
||||
// Both whole-tree validation and active-profile construction must reject.
|
||||
assert_eq!(
|
||||
<CandidateProfile as kaptan_config::Config>::validate(&candidate),
|
||||
Err(issue)
|
||||
);
|
||||
assert_eq!(candidate.validate(&CAPABILITIES), Err(issue));
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn imu_interval_cannot_exceed_the_controller_domain() {
|
||||
// The generic primitive still permits 100 ms, but this flight profile's
|
||||
// delayed estimator and consumer require 10..20 ms inertial intervals.
|
||||
assert_eq!(kaptan_control::MAXIMUM_CONTROL_STEP_MICROSECONDS, 100_000);
|
||||
for interval in [10_000, 15_000, 19_999, 20_000] {
|
||||
let active = with_interval(interval).validate(&CAPABILITIES).unwrap();
|
||||
assert_eq!(active.timing().maximum_imu_interval_microseconds, interval);
|
||||
}
|
||||
for interval in [
|
||||
1, 9_999, 20_001, 50_000, 100_000, 100_001, 200_000, 1_000_000,
|
||||
] {
|
||||
let candidate = with_interval(interval);
|
||||
assert!(candidate.validate(&CAPABILITIES).is_err());
|
||||
assert!(<CandidateProfile as kaptan_config::Config>::validate(&candidate).is_err());
|
||||
}
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn downward_acceleration_has_a_strict_gravity_boundary() {
|
||||
let below = f32::from_bits(GRAVITY_MPS2.to_bits() - 1);
|
||||
let above = f32::from_bits(GRAVITY_MPS2.to_bits() + 1);
|
||||
for acceleration in [0.001, 2.0, below] {
|
||||
let mut candidate = support::candidate();
|
||||
set(
|
||||
&mut candidate,
|
||||
DOWNWARD_ACCELERATION,
|
||||
ConfigValue::F32(acceleration),
|
||||
);
|
||||
let active = candidate.validate(&CAPABILITIES).unwrap();
|
||||
assert_eq!(
|
||||
active.control().maximum_downward_acceleration_mps2,
|
||||
acceleration
|
||||
);
|
||||
}
|
||||
for acceleration in [GRAVITY_MPS2, above, 100.0] {
|
||||
let mut candidate = support::candidate();
|
||||
set(
|
||||
&mut candidate,
|
||||
DOWNWARD_ACCELERATION,
|
||||
ConfigValue::F32(acceleration),
|
||||
);
|
||||
assert_rejected(
|
||||
candidate,
|
||||
ConfigViolation::field(id(DOWNWARD_ACCELERATION), ValidationRule::Inconsistent),
|
||||
);
|
||||
}
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn thrust_margin_uses_the_same_gravity_contract() {
|
||||
for thrust in [f32::from_bits(GRAVITY_MPS2.to_bits() - 1), GRAVITY_MPS2] {
|
||||
let mut candidate = support::candidate();
|
||||
set(&mut candidate, "thrust.mass_kg", ConfigValue::F32(1.0));
|
||||
set(
|
||||
&mut candidate,
|
||||
"thrust.maximum_collective_thrust_n",
|
||||
ConfigValue::F32(thrust),
|
||||
);
|
||||
assert_rejected(
|
||||
candidate,
|
||||
ConfigViolation::related(
|
||||
id("thrust.maximum_collective_thrust_n"),
|
||||
id("thrust.mass_kg"),
|
||||
ValidationRule::Inconsistent,
|
||||
),
|
||||
);
|
||||
}
|
||||
let mut candidate = support::candidate();
|
||||
set(&mut candidate, "thrust.mass_kg", ConfigValue::F32(1.0));
|
||||
set(
|
||||
&mut candidate,
|
||||
"thrust.maximum_collective_thrust_n",
|
||||
ConfigValue::F32(f32::from_bits(GRAVITY_MPS2.to_bits() + 1)),
|
||||
);
|
||||
assert!(candidate.validate(&CAPABILITIES).is_ok());
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn thrust_to_mass_certifies_navigation_free_acceleration_without_epsilon() {
|
||||
let issue = ConfigViolation::related(
|
||||
id("thrust.maximum_collective_thrust_n"),
|
||||
id("thrust.mass_kg"),
|
||||
ValidationRule::Inconsistent,
|
||||
);
|
||||
let mut former = support::candidate();
|
||||
set(&mut former, "thrust.mass_kg", ConfigValue::F32(0.01));
|
||||
assert_rejected(former, issue);
|
||||
assert!(support::candidate().validate(&CAPABILITIES).is_ok());
|
||||
|
||||
let acceleration = f64::from(MAXIMUM_FREE_ACCELERATION_MPS2) - f64::from(GRAVITY_MPS2);
|
||||
for mass in [0.01_f32, 1.0, 1.2, 100.0] {
|
||||
let exact = f64::from(mass) * acceleration;
|
||||
let rounded = exact as f32;
|
||||
let floor = if f64::from(rounded) > exact {
|
||||
rounded.next_down()
|
||||
} else {
|
||||
rounded
|
||||
};
|
||||
let mut candidate = support::candidate();
|
||||
set(&mut candidate, "thrust.mass_kg", ConfigValue::F32(mass));
|
||||
for thrust in [floor.next_down(), floor] {
|
||||
set(
|
||||
&mut candidate,
|
||||
"thrust.maximum_collective_thrust_n",
|
||||
ConfigValue::F32(thrust),
|
||||
);
|
||||
assert!(candidate.validate(&CAPABILITIES).is_ok());
|
||||
}
|
||||
set(
|
||||
&mut candidate,
|
||||
"thrust.maximum_collective_thrust_n",
|
||||
ConfigValue::F32(floor.next_up()),
|
||||
);
|
||||
assert_rejected(candidate, issue);
|
||||
}
|
||||
// At m=1 the upper boundary is exactly representable in binary32.
|
||||
let exact_thrust = MAXIMUM_FREE_ACCELERATION_MPS2 - GRAVITY_MPS2;
|
||||
assert_eq!(f64::from(exact_thrust), acceleration);
|
||||
let mut candidate = support::candidate();
|
||||
set(
|
||||
&mut candidate,
|
||||
"thrust.maximum_collective_thrust_n",
|
||||
ConfigValue::F32(exact_thrust),
|
||||
);
|
||||
set(&mut candidate, "thrust.mass_kg", ConfigValue::F32(1.0));
|
||||
assert!(candidate.validate(&CAPABILITIES).is_ok());
|
||||
set(
|
||||
&mut candidate,
|
||||
"thrust.mass_kg",
|
||||
ConfigValue::F32(1.0_f32.next_down()),
|
||||
);
|
||||
assert_rejected(candidate, issue);
|
||||
set(
|
||||
&mut candidate,
|
||||
"thrust.mass_kg",
|
||||
ConfigValue::F32(1.0_f32.next_up()),
|
||||
);
|
||||
assert!(candidate.validate(&CAPABILITIES).is_ok());
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn accepted_boundary_profiles_operate_the_real_control_primitives() {
|
||||
for mass in [0.01, 1.0] {
|
||||
let mut candidate = with_interval(20_000);
|
||||
let downward_limit = f32::from_bits(GRAVITY_MPS2.to_bits() - 1);
|
||||
set(
|
||||
&mut candidate,
|
||||
DOWNWARD_ACCELERATION,
|
||||
ConfigValue::F32(downward_limit),
|
||||
);
|
||||
set(&mut candidate, "thrust.mass_kg", ConfigValue::F32(mass));
|
||||
set(
|
||||
&mut candidate,
|
||||
"thrust.maximum_collective_thrust_n",
|
||||
ConfigValue::F32(15.0 * mass),
|
||||
);
|
||||
for (key, gain) in [
|
||||
("control.rate_pid.proportional", 0.1),
|
||||
("control.rate_pid.integral", 0.02),
|
||||
("control.rate_pid.derivative", 0.05),
|
||||
] {
|
||||
set(&mut candidate, key, ConfigValue::F32(gain));
|
||||
}
|
||||
let active = candidate.validate(&CAPABILITIES).unwrap();
|
||||
assert_eq!(active.timing().maximum_imu_interval_microseconds, 20_000);
|
||||
// Preserve the independent 100-ms known answers of the generic
|
||||
// primitives, without claiming it is a valid flight-estimator cadence.
|
||||
let dt = 0.1;
|
||||
let gains = active.rate_pid();
|
||||
let mut pid = Pid::new(PidConfig {
|
||||
proportional: gains.proportional,
|
||||
integral: gains.integral,
|
||||
derivative: gains.derivative,
|
||||
output_limit: gains.output_limit,
|
||||
integral_limit: gains.integral_limit,
|
||||
})
|
||||
.unwrap();
|
||||
// Known answer with all three terms active at the 100-ms boundary.
|
||||
assert!((pid.update(0.2, 0.3, dt).unwrap() - 0.0354).abs() < 1.0e-7);
|
||||
|
||||
let mut trajectory = Trajectory::new(
|
||||
PositionNed::metres(0.0, 0.0, 0.0).vector(),
|
||||
KinematicLimits {
|
||||
maximum_horizontal_speed_mps: 1.0,
|
||||
maximum_vertical_speed_mps: 1.0,
|
||||
maximum_horizontal_acceleration_mps2: 1.0,
|
||||
maximum_vertical_acceleration_mps2: 1.0,
|
||||
},
|
||||
)
|
||||
.unwrap();
|
||||
trajectory
|
||||
.set_goal(PositionNed::metres(0.0, 0.0, 1.0).vector())
|
||||
.unwrap();
|
||||
let reference = trajectory.update(dt).unwrap();
|
||||
assert!((reference.position.z() - 0.01).abs() < 1.0e-8);
|
||||
|
||||
let control = active.control();
|
||||
let controller = 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,
|
||||
})
|
||||
.unwrap();
|
||||
let acceleration = controller
|
||||
.update(
|
||||
reference,
|
||||
TranslationalEstimate {
|
||||
position: PositionNed::metres(0.0, 0.0, -100.0).vector(),
|
||||
velocity: VelocityNed::metres_per_second(0.0, 0.0, 0.0).vector(),
|
||||
},
|
||||
)
|
||||
.unwrap();
|
||||
assert_eq!(acceleration.z(), downward_limit);
|
||||
|
||||
let thrust = active.thrust();
|
||||
let envelope = ThrustEnvelope {
|
||||
mass_kg: thrust.mass_kg,
|
||||
maximum_collective_thrust_newtons: thrust.maximum_collective_thrust_newtons,
|
||||
maximum_tilt_tangent: thrust.maximum_tilt_tangent,
|
||||
};
|
||||
let (force, saturated) = envelope
|
||||
.force_for_acceleration(acceleration, GRAVITY_MPS2)
|
||||
.unwrap();
|
||||
// Gravity is in [8, 16), so its f32 predecessor is exactly 2^-20 lower.
|
||||
assert_eq!(force.vector().z(), -mass / 1_048_576.0);
|
||||
assert!(!saturated);
|
||||
let attitude = force_to_attitude(force, HeadingNed::new(1.0, 0.0).unwrap()).unwrap();
|
||||
assert_eq!(attitude.w(), 1.0);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,96 @@
|
||||
use kaptan_flight_config::profile::*;
|
||||
use kaptan_math::Vector3;
|
||||
use kaptan_navigation::{
|
||||
BarometricReference, CalibrationRevision, ContactGeometry, FrameId, NavigationInstallation,
|
||||
NavigationSources, RangeInstallation, SensorReferenceId, SourceId,
|
||||
};
|
||||
|
||||
pub const fn candidate() -> CandidateProfile {
|
||||
CandidateProfile {
|
||||
safety: SafetySettings {
|
||||
prearm_dwell_microseconds: 500_000,
|
||||
},
|
||||
timing: TimingSettings {
|
||||
maximum_imu_interval_microseconds: 20_000,
|
||||
imu_freshness_microseconds: 20_000,
|
||||
local_state_freshness_microseconds: 250_000,
|
||||
offboard_timeout_microseconds: 300_000,
|
||||
},
|
||||
control: ControlSettings {
|
||||
position_gain_per_second_squared: 0.8,
|
||||
velocity_gain_per_second: 1.2,
|
||||
maximum_horizontal_acceleration_mps2: 3.0,
|
||||
maximum_upward_acceleration_mps2: 2.0,
|
||||
maximum_downward_acceleration_mps2: 2.0,
|
||||
attitude_roll_pitch_gain_per_second: 4.0,
|
||||
attitude_yaw_gain_per_second: 2.0,
|
||||
},
|
||||
thrust: ThrustSettings {
|
||||
mass_kg: 1.2,
|
||||
maximum_collective_thrust_newtons: 18.0,
|
||||
maximum_tilt_tangent: 0.700_207_53,
|
||||
},
|
||||
rate_pid: PidSettings {
|
||||
proportional: 0.080,
|
||||
integral: 0.006,
|
||||
derivative: 0.0,
|
||||
output_limit: 0.10,
|
||||
integral_limit: 0.025,
|
||||
yaw_proportional: 0.990,
|
||||
yaw_integral: 0.074,
|
||||
},
|
||||
actuator_limits: ActuatorLimits {
|
||||
minimum: 0.0,
|
||||
maximum: 1.0,
|
||||
slew_per_second: 8.0,
|
||||
},
|
||||
flight: FlightSettings {
|
||||
command_ttl_us: 250_000,
|
||||
fallback_hold_us: 1_000_000,
|
||||
output_lifetime_us: 50_000,
|
||||
control_period_us: 10_000,
|
||||
control_deadline_us: 20_000,
|
||||
time_error_bound_us: 2_000,
|
||||
gyro_psd: 2.5e-7,
|
||||
acceleration_psd: 2.5e-5,
|
||||
gyro_bias_psd: 1e-10,
|
||||
acceleration_bias_psd: 1e-8,
|
||||
barometric_datum_psd: 1e-5,
|
||||
},
|
||||
}
|
||||
}
|
||||
|
||||
pub const fn installation() -> NavigationInstallation {
|
||||
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 const fn capabilities() -> VehicleCapabilities {
|
||||
VehicleCapabilities::quad_x(installation())
|
||||
}
|
||||
Reference in new issue
Block a user