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-config/Cargo.toml
+5 -3
View File
@@ -10,6 +10,8 @@ workspace = true
[dependencies]
kaptan-config = { path = "../kaptan-config" }
kaptan-err-reportable = { path = "../kaptan-err-reportable" }
kaptan-log = { path = "../kaptan-log" }
minicbor = { version = "0.25", features = ["derive"] }
kaptan-control = { path = "../kaptan-control" }
kaptan-navigation = { path = "../kaptan-navigation" }
[dev-dependencies]
kaptan-math = { path = "../kaptan-math" }
firmware/libs/kaptan-flight-config/src/cbor.rs
-382
View File
@@ -1,382 +0,0 @@
//! Bounded CBOR persistence codec for one explicit current record.
use kaptan_config::{
ConfigModel, ConfigValue, ConfigViolation, FieldWriteError, ValidationRule, ValueKind,
};
use kaptan_err_reportable::KaptanError;
use minicbor::{Decoder, Encoder, encode::Write};
use crate::{CandidateProfile, FORMAT_VERSION, SCHEMA_ID, VehicleCapabilities};
/// Maximum complete candidate snapshot accepted by the current vehicle schema.
pub const MAXIMUM_CANDIDATE_BYTES: usize = 4096;
const FORMAT_KEY: u16 = 0;
const SCHEMA_KEY: u16 = u16::MAX;
/// CBOR snapshot decoding failure.
#[derive(Clone, Copy, Debug, Eq, PartialEq, KaptanError)]
pub enum CborError {
/// The supplied payload exceeded the bounded configuration storage budget.
#[kaptan_error(id = 3001)]
TooLarge,
/// CBOR was malformed or did not match a supported versioned wire record.
#[kaptan_error(id = 3002)]
Malformed,
/// A second CBOR item followed the candidate snapshot.
#[kaptan_error(id = 3003)]
TrailingData,
/// The decoded candidate violated complete schema/capability validation.
#[kaptan_error(id = 3004)]
InvalidProfile(ConfigViolation),
}
/// Failure while encoding one complete candidate snapshot.
///
/// The candidate is validated before the first CBOR item is written, so a
/// caller can never export a locally valid but relationally invalid edit.
#[derive(Debug)]
pub enum CborEncodeError<E> {
/// The complete candidate did not validate against immutable board
/// capabilities.
InvalidProfile(ConfigViolation),
/// The caller-owned CBOR writer rejected the encoded snapshot.
Encode(minicbor::encode::Error<E>),
}
/// Decodes exactly one bounded current record and validates it against immutable
/// board capabilities before returning a candidate to its caller.
pub fn decode_candidate(
bytes: &[u8],
capabilities: &VehicleCapabilities,
) -> Result<CandidateProfile, CborError> {
if bytes.len() > MAXIMUM_CANDIDATE_BYTES {
return Err(CborError::TooLarge);
}
let candidate = decode_current(bytes)?;
candidate
.validate(capabilities)
.map_err(CborError::InvalidProfile)?;
Ok(candidate)
}
/// Validates and encodes one complete current snapshot to a caller-owned CBOR
/// writer.
///
/// Validation happens before creating the encoder, ensuring a failed
/// cross-field or capability check emits no partial CBOR record.
pub fn encode_candidate<W>(
candidate: CandidateProfile,
capabilities: &VehicleCapabilities,
writer: W,
) -> Result<(), CborEncodeError<W::Error>>
where
W: Write,
{
candidate
.validate(capabilities)
.map_err(CborEncodeError::InvalidProfile)?;
let mut encoder = Encoder::new(writer);
encoder
.map(CandidateProfile::FIELD_COUNT as u64 + 2)
.map_err(CborEncodeError::Encode)?;
encoder
.u16(FORMAT_KEY)
.and_then(|encoder| encoder.u16(FORMAT_VERSION))
.map_err(CborEncodeError::Encode)?;
encoder
.u16(SCHEMA_KEY)
.and_then(|encoder| encoder.bytes(&SCHEMA_ID))
.map_err(CborEncodeError::Encode)?;
for descriptor in CandidateProfile::descriptors() {
encoder
.u16(descriptor.id.get())
.map_err(CborEncodeError::Encode)?;
encode_value(
&mut encoder,
candidate
.field_value(descriptor.id)
.expect("descriptor field"),
)
.map_err(CborEncodeError::Encode)?;
}
Ok(())
}
fn decode_current(bytes: &[u8]) -> Result<CandidateProfile, CborError> {
let mut decoder = Decoder::new(bytes);
let entry_count = CandidateProfile::FIELD_COUNT as u64 + 2;
if decoder.map().map_err(|_| CborError::Malformed)? != Some(entry_count) {
return Err(CborError::Malformed);
}
let mut format_version = None;
let mut schema_id = None;
let mut values = [None; CandidateProfile::FIELD_COUNT];
for _ in 0..entry_count {
let key = decoder.u16().map_err(|_| CborError::Malformed)?;
match key {
FORMAT_KEY => {
if format_version
.replace(decoder.u16().map_err(|_| CborError::Malformed)?)
.is_some()
{
return Err(CborError::Malformed);
}
}
SCHEMA_KEY => {
let bytes = decoder.bytes().map_err(|_| CborError::Malformed)?;
let value: [u8; 16] = bytes.try_into().map_err(|_| CborError::Malformed)?;
if schema_id.replace(value).is_some() {
return Err(CborError::Malformed);
}
}
_ => {
let (index, descriptor) = CandidateProfile::descriptors()
.enumerate()
.find(|(_, descriptor)| descriptor.id.get() == key)
.ok_or(CborError::Malformed)?;
if values[index].is_some() {
return Err(CborError::Malformed);
}
values[index] = Some(decode_value(&mut decoder, descriptor.kind)?);
}
}
}
if decoder.position() != bytes.len() {
return Err(CborError::TrailingData);
}
if format_version != Some(FORMAT_VERSION) || schema_id != Some(SCHEMA_ID) {
return Err(CborError::InvalidProfile(ConfigViolation::global(
ValidationRule::UnsupportedFormat,
)));
}
if values.iter().any(Option::is_none) {
return Err(CborError::Malformed);
}
let mut candidate = CandidateProfile::factory();
for (index, value) in values.into_iter().enumerate() {
let descriptor = CandidateProfile::descriptors()
.nth(index)
.ok_or(CborError::Malformed)?;
candidate
.set_field(descriptor.id, value.ok_or(CborError::Malformed)?)
.map_err(field_write_error)?;
}
Ok(candidate)
}
fn encode_value<W>(
encoder: &mut Encoder<W>,
value: ConfigValue,
) -> Result<(), minicbor::encode::Error<W::Error>>
where
W: Write,
{
match value {
ConfigValue::Bool(value) => encoder.bool(value)?,
ConfigValue::U8(value) => encoder.u8(value)?,
ConfigValue::U16(value) => encoder.u16(value)?,
ConfigValue::I32(value) => encoder.i32(value)?,
ConfigValue::U32(value) => encoder.u32(value)?,
ConfigValue::U64(value) => encoder.u64(value)?,
ConfigValue::F32(value) => encoder.f32(value)?,
ConfigValue::F64(value) => encoder.f64(value)?,
};
Ok(())
}
fn decode_value(decoder: &mut Decoder<'_>, kind: ValueKind) -> Result<ConfigValue, CborError> {
match kind {
ValueKind::Bool => decoder.bool().map(ConfigValue::Bool),
ValueKind::U8 => decoder.u8().map(ConfigValue::U8),
ValueKind::U16 => decoder.u16().map(ConfigValue::U16),
ValueKind::I32 => decoder.i32().map(ConfigValue::I32),
ValueKind::U32 => decoder.u32().map(ConfigValue::U32),
ValueKind::U64 => decoder.u64().map(ConfigValue::U64),
ValueKind::F32 => decoder.f32().map(ConfigValue::F32),
ValueKind::F64 => decoder.f64().map(ConfigValue::F64),
}
.map_err(|_| CborError::Malformed)
}
fn field_write_error(error: FieldWriteError) -> CborError {
match error {
FieldWriteError::Violation(issue) => CborError::InvalidProfile(issue),
FieldWriteError::UnknownField(_) | FieldWriteError::WrongType { .. } => {
CborError::Malformed
}
}
}
#[cfg(test)]
mod tests {
use super::*;
#[test]
fn round_trip_and_trailing_rejection_are_exact() {
let mut writer = minicbor::encode::write::Cursor::new([0_u8; MAXIMUM_CANDIDATE_BYTES]);
let capabilities = VehicleCapabilities::quad_x();
encode_candidate(CandidateProfile::factory(), &capabilities, &mut writer).unwrap();
let length = writer.position();
let bytes = writer.into_inner();
assert!(decode_candidate(&bytes[..length], &capabilities).is_ok());
let mut trailing = bytes;
trailing[length] = 0xf6;
assert!(matches!(
decode_candidate(&trailing[..=length], &capabilities),
Err(CborError::TrailingData)
));
}
#[test]
fn encoding_rejects_a_cross_field_invalid_candidate_before_writing() {
let capabilities = VehicleCapabilities::quad_x();
let mut candidate = CandidateProfile::factory();
candidate
.set_field(kaptan_config::FieldId::new(2), ConfigValue::U64(50_001))
.expect("the scalar is locally valid");
let mut writer = minicbor::encode::write::Cursor::new([0_u8; MAXIMUM_CANDIDATE_BYTES]);
assert!(matches!(
encode_candidate(candidate, &capabilities, &mut writer),
Err(CborEncodeError::InvalidProfile(violation))
if violation.rule() == ValidationRule::Inconsistent
));
assert_eq!(writer.position(), 0);
}
#[test]
fn rejects_wrong_format_and_schema_before_candidate_conversion() {
let capabilities = VehicleCapabilities::quad_x();
let (bytes, length) = encoded_test_record(FORMAT_VERSION + 1, SCHEMA_ID, |key| key);
assert!(matches!(
decode_candidate(&bytes[..length], &capabilities),
Err(CborError::InvalidProfile(issue))
if issue.rule() == ValidationRule::UnsupportedFormat
));
let (bytes, length) = encoded_test_record(FORMAT_VERSION, [0; 16], |key| key);
assert!(matches!(
decode_candidate(&bytes[..length], &capabilities),
Err(CborError::InvalidProfile(issue))
if issue.rule() == kaptan_config::ValidationRule::UnsupportedFormat
));
}
#[test]
fn exact_map_rejects_unknown_duplicate_and_missing_fields() {
let capabilities = VehicleCapabilities::quad_x();
let (bytes, length) = encoded_test_record(FORMAT_VERSION, SCHEMA_ID, |key| key);
assert!(decode_candidate(&bytes[..length], &capabilities).is_ok());
let (unknown, length) =
encoded_test_record(
FORMAT_VERSION,
SCHEMA_ID,
|key| {
if key == 1 { 25 } else { key }
},
);
assert!(matches!(
decode_candidate(&unknown[..length], &capabilities),
Err(CborError::Malformed)
));
let (duplicate, length) = encoded_test_record(FORMAT_VERSION, SCHEMA_ID, |key| {
if key == 1 { FORMAT_KEY } else { key }
});
assert!(matches!(
decode_candidate(&duplicate[..length], &capabilities),
Err(CborError::Malformed)
));
let mut writer = minicbor::encode::write::Cursor::new([0_u8; MAXIMUM_CANDIDATE_BYTES]);
let mut encoder = Encoder::new(&mut writer);
encoder
.map(CandidateProfile::FIELD_COUNT as u64 + 1)
.unwrap();
encoder
.u16(FORMAT_KEY)
.unwrap()
.u16(FORMAT_VERSION)
.unwrap();
encoder.u16(SCHEMA_KEY).unwrap().bytes(&SCHEMA_ID).unwrap();
let candidate = CandidateProfile::factory();
for descriptor in CandidateProfile::descriptors().skip(1) {
encoder.u16(descriptor.id.get()).unwrap();
encode_value(&mut encoder, candidate.field_value(descriptor.id).unwrap()).unwrap();
}
let length = writer.position();
let missing = writer.into_inner();
assert!(matches!(
decode_candidate(&missing[..length], &capabilities),
Err(CborError::Malformed)
));
}
#[test]
fn cbor_scalar_write_preserves_the_generated_field_and_rule() {
let capabilities = VehicleCapabilities::quad_x();
let mass = CandidateProfile::descriptors()
.find(|descriptor| descriptor.display_key == "thrust.mass_kg")
.expect("declared mass descriptor");
let mut writer = minicbor::encode::write::Cursor::new([0_u8; MAXIMUM_CANDIDATE_BYTES]);
let mut encoder = Encoder::new(&mut writer);
encoder
.map(CandidateProfile::FIELD_COUNT as u64 + 2)
.unwrap();
encoder
.u16(FORMAT_KEY)
.unwrap()
.u16(FORMAT_VERSION)
.unwrap();
encoder.u16(SCHEMA_KEY).unwrap().bytes(&SCHEMA_ID).unwrap();
let candidate = CandidateProfile::factory();
for descriptor in CandidateProfile::descriptors() {
encoder.u16(descriptor.id.get()).unwrap();
let value = if descriptor.id == mass.id {
ConfigValue::F32(0.009)
} else {
candidate.field_value(descriptor.id).unwrap()
};
encode_value(&mut encoder, value).unwrap();
}
let length = writer.position();
let bytes = writer.into_inner();
assert!(matches!(
decode_candidate(&bytes[..length], &capabilities),
Err(CborError::InvalidProfile(issue))
if issue.primary() == Some(mass.id)
&& issue.related_field().is_none()
&& issue.rule() == ValidationRule::BelowMinimum
));
}
fn encoded_test_record<F>(
format_version: u16,
schema_id: [u8; 16],
mut remap_key: F,
) -> ([u8; MAXIMUM_CANDIDATE_BYTES], usize)
where
F: FnMut(u16) -> u16,
{
let mut writer = minicbor::encode::write::Cursor::new([0_u8; MAXIMUM_CANDIDATE_BYTES]);
let mut encoder = Encoder::new(&mut writer);
encoder
.map(CandidateProfile::FIELD_COUNT as u64 + 2)
.unwrap();
encoder
.u16(FORMAT_KEY)
.unwrap()
.u16(format_version)
.unwrap();
encoder.u16(SCHEMA_KEY).unwrap().bytes(&schema_id).unwrap();
let candidate = CandidateProfile::factory();
for descriptor in CandidateProfile::descriptors() {
encoder.u16(remap_key(descriptor.id.get())).unwrap();
encode_value(&mut encoder, candidate.field_value(descriptor.id).unwrap()).unwrap();
}
let length = writer.position();
(writer.into_inner(), length)
}
}
firmware/libs/kaptan-flight-config/src/lib.rs
+8 -6
View File
@@ -1,17 +1,19 @@
//! Vehicle configuration schema and CBOR snapshot codec.
//! Validated configuration for the portable flight runtime.
#![no_std]
pub mod cbor;
pub mod profile;
pub use cbor::{
CborEncodeError, CborError, MAXIMUM_CANDIDATE_BYTES, decode_candidate, encode_candidate,
};
pub use profile::{CandidateProfile, FORMAT_VERSION, VehicleCapabilities, VehicleProfile};
pub use profile::{CandidateProfile, VehicleCapabilities, VehicleProfile};
/// Generated fingerprint of the complete vehicle configuration schema.
pub const SCHEMA_ID: [u8; 16] = <CandidateProfile as kaptan_config::Config>::SCHEMA_ID;
#[cfg(test)]
extern crate std;
#[cfg(test)]
extern crate self as kaptan_flight_config;
#[cfg(test)]
#[path = "../tests/support/mod.rs"]
mod test_support;
firmware/libs/kaptan-flight-config/src/profile/active.rs
+81 -197
View File
@@ -1,30 +1,28 @@
//! Immutable configuration projections consumed by runtime owners.
/// Immutable compiled contract for the physical vehicle/board image.
///
/// A runtime candidate can tune only values inside this contract. It cannot
/// create pins, buses, sensor drivers, or actuator channels.
use kaptan_config::{Config, ConfigValue, ConfigViolation, FieldId, ValidationRule};
use kaptan_navigation::{
ContactGeometry, NavigationConfig, NavigationInstallation, NoiseDensities,
};
use super::{
ActuatorLimits, CandidateProfile, ControlSettings, PidSettings, SafetySettings, ThrustSettings,
TimingSettings,
};
/// Compiled vehicle capabilities.
#[derive(Clone, Copy, Debug, PartialEq)]
pub struct VehicleCapabilities {
/// Number of board-declared logical actuator channels available to the
/// selected vehicle model.
actuator_count: u8,
/// Reviewed physical allocation for this board/airframe image.
///
/// This is capability data rather than a persisted setting: profile edits
/// must never alter motor order, thrust signs, or control authority.
/// Fixed allocation rows. Profile edits cannot change motor order or thrust signs.
mixer: [[f32; 4]; 4],
installation: NavigationInstallation,
}
impl VehicleCapabilities {
/// Returns the reviewed four-actuator Quad-X allocation supported by P1.
///
/// New allocation layouts require a dedicated board-bound airframe review
/// and constructor, rather than accepting an arbitrary coefficient array
/// through a configuration or capability initializer.
pub const fn quad_x() -> Self {
/// Four-actuator Quad-X allocation with an explicit navigation installation.
pub const fn quad_x(installation: NavigationInstallation) -> Self {
Self {
actuator_count: 4,
installation,
mixer: [
[-1.0, 1.0, 1.0, 1.0],
[-1.0, -1.0, -1.0, 1.0],
@@ -33,205 +31,91 @@ impl VehicleCapabilities {
],
}
}
pub(crate) const fn actuator_count(self) -> u8 {
self.actuator_count
}
pub(crate) const fn mixer(self) -> [[f32; 4]; 4] {
self.mixer
}
}
/// Active prearm timing policy.
#[derive(Clone, Copy, Debug, Eq, PartialEq)]
pub struct SafetySettings {
pub(crate) prearm_dwell_microseconds: u64,
}
impl SafetySettings {
pub const fn prearm_dwell_microseconds(self) -> u64 {
self.prearm_dwell_microseconds
}
}
/// Active stream timing policy.
#[derive(Clone, Copy, Debug, Eq, PartialEq)]
pub struct TimingSettings {
pub(crate) maximum_imu_interval_microseconds: u64,
pub(crate) imu_freshness_microseconds: u64,
pub(crate) local_state_freshness_microseconds: u64,
pub(crate) command_link_freshness_microseconds: u64,
}
impl TimingSettings {
pub const fn maximum_imu_interval_microseconds(self) -> u64 {
self.maximum_imu_interval_microseconds
}
pub const fn imu_freshness_microseconds(self) -> u64 {
self.imu_freshness_microseconds
}
pub const fn local_state_freshness_microseconds(self) -> u64 {
self.local_state_freshness_microseconds
}
pub const fn command_link_freshness_microseconds(self) -> u64 {
self.command_link_freshness_microseconds
}
}
/// Active local-aiding uncertainty policy.
#[derive(Clone, Copy, Debug, PartialEq)]
pub struct AidingSettings {
pub(crate) position_variance_m2: f32,
pub(crate) velocity_variance_m2_per_second_squared: f32,
}
impl AidingSettings {
pub const fn position_variance_m2(self) -> f32 {
self.position_variance_m2
}
pub const fn velocity_variance_m2_per_second_squared(self) -> f32 {
self.velocity_variance_m2_per_second_squared
}
}
/// Active outer-loop controller policy.
#[derive(Clone, Copy, Debug, PartialEq)]
pub struct ControlSettings {
pub(crate) position_gain_per_second_squared: f32,
pub(crate) velocity_gain_per_second: f32,
pub(crate) maximum_horizontal_acceleration_mps2: f32,
pub(crate) maximum_upward_acceleration_mps2: f32,
pub(crate) maximum_downward_acceleration_mps2: f32,
}
impl ControlSettings {
pub const fn position_gain_per_second_squared(self) -> f32 {
self.position_gain_per_second_squared
}
pub const fn velocity_gain_per_second(self) -> f32 {
self.velocity_gain_per_second
}
pub const fn maximum_horizontal_acceleration_mps2(self) -> f32 {
self.maximum_horizontal_acceleration_mps2
}
pub const fn maximum_upward_acceleration_mps2(self) -> f32 {
self.maximum_upward_acceleration_mps2
}
pub const fn maximum_downward_acceleration_mps2(self) -> f32 {
self.maximum_downward_acceleration_mps2
}
}
/// Active physical thrust envelope.
#[derive(Clone, Copy, Debug, PartialEq)]
pub struct ThrustSettings {
pub(crate) mass_kg: f32,
pub(crate) maximum_collective_thrust_newtons: f32,
pub(crate) maximum_tilt_tangent: f32,
}
impl ThrustSettings {
pub const fn mass_kg(self) -> f32 {
self.mass_kg
}
pub const fn maximum_collective_thrust_newtons(self) -> f32 {
self.maximum_collective_thrust_newtons
}
pub const fn maximum_tilt_tangent(self) -> f32 {
self.maximum_tilt_tangent
}
}
/// Active scalar PID policy.
#[derive(Clone, Copy, Debug, PartialEq)]
pub struct PidSettings {
pub(crate) proportional: f32,
pub(crate) integral: f32,
pub(crate) derivative: f32,
pub(crate) output_limit: f32,
pub(crate) integral_limit: f32,
}
impl PidSettings {
pub const fn proportional(self) -> f32 {
self.proportional
}
pub const fn integral(self) -> f32 {
self.integral
}
pub const fn derivative(self) -> f32 {
self.derivative
}
pub const fn output_limit(self) -> f32 {
self.output_limit
}
pub const fn integral_limit(self) -> f32 {
self.integral_limit
}
}
/// Active actuator command bounds.
#[derive(Clone, Copy, Debug, PartialEq)]
pub struct ActuatorLimits {
pub(crate) minimum: f32,
pub(crate) maximum: f32,
pub(crate) slew_per_second: f32,
}
impl ActuatorLimits {
pub const fn minimum(self) -> f32 {
self.minimum
}
pub const fn maximum(self) -> f32 {
self.maximum
}
pub const fn slew_per_second(self) -> f32 {
self.slew_per_second
}
}
/// Immutable complete configuration selected at boot.
///
/// An editable candidate must pass validation before it can become active.
/// ```compile_fail,E0308
/// use kaptan_flight_config::{CandidateProfile, VehicleProfile};
/// fn activate(candidate: CandidateProfile) -> VehicleProfile { candidate }
/// ```
/// Callers cannot replace the validated candidate inside an active profile.
/// ```compile_fail,E0616
/// use kaptan_flight_config::{CandidateProfile, VehicleProfile};
/// fn replace(mut active: VehicleProfile, candidate: CandidateProfile) {
/// active.candidate = candidate;
/// }
/// ```
#[derive(Clone, Copy, Debug, PartialEq)]
pub struct VehicleProfile {
pub(crate) safety: SafetySettings,
pub(crate) timing: TimingSettings,
pub(crate) aiding: AidingSettings,
pub(crate) control: ControlSettings,
pub(crate) thrust: ThrustSettings,
pub(crate) rate_pid: PidSettings,
pub(crate) actuator_limits: ActuatorLimits,
pub(crate) mavlink_logging: kaptan_log::LevelFilter,
pub(crate) mixer: [[f32; 4]; 4],
candidate: CandidateProfile,
capabilities: VehicleCapabilities,
}
impl VehicleProfile {
pub(super) fn validate(
candidate: &CandidateProfile,
capabilities: VehicleCapabilities,
) -> Result<Self, ConfigViolation> {
<CandidateProfile as Config>::validate(candidate)?;
let profile = Self {
candidate: *candidate,
capabilities,
};
if !profile.navigation().validate() {
return Err(ConfigViolation::global(ValidationRule::Inconsistent));
}
Ok(profile)
}
pub(super) fn field_value(&self, id: FieldId) -> Option<ConfigValue> {
<CandidateProfile as Config>::field_value(&self.candidate, id)
}
pub const fn safety(self) -> SafetySettings {
self.safety
self.candidate.safety
}
pub const fn timing(self) -> TimingSettings {
self.timing
}
pub const fn aiding(self) -> AidingSettings {
self.aiding
self.candidate.timing
}
pub const fn control(self) -> ControlSettings {
self.control
self.candidate.control
}
pub const fn thrust(self) -> ThrustSettings {
self.thrust
self.candidate.thrust
}
pub const fn rate_pid(self) -> PidSettings {
self.rate_pid
self.candidate.rate_pid
}
pub const fn actuator_limits(self) -> ActuatorLimits {
self.actuator_limits
}
/// Immutable boot-active policy for the MAVLink diagnostic sink.
pub const fn mavlink_logging(self) -> kaptan_log::LevelFilter {
self.mavlink_logging
self.candidate.actuator_limits
}
pub const fn mixer(self) -> [[f32; 4]; 4] {
self.mixer
self.capabilities.mixer
}
pub const fn flight(self) -> super::FlightSettings {
self.candidate.flight
}
pub const fn contact_geometry(self) -> ContactGeometry {
self.capabilities.installation.contact
}
pub fn navigation(self) -> NavigationConfig {
let f = self.candidate.flight;
NavigationConfig {
noise: NoiseDensities {
gyro: f.gyro_psd,
acceleration: f.acceleration_psd,
gyro_bias: f.gyro_bias_psd,
acceleration_bias: f.acceleration_bias_psd,
barometric_datum: f.barometric_datum_psd,
},
maximum_time_uncertainty_microseconds: f.time_error_bound_us as u32,
maximum_imu_interval_microseconds: self.timing().maximum_imu_interval_microseconds,
imu_freshness_microseconds: self.timing().imu_freshness_microseconds,
aiding_freshness_microseconds: self.timing().local_state_freshness_microseconds,
lag_microseconds: 100_000,
installation: self.capabilities.installation,
}
}
}
firmware/libs/kaptan-flight-config/src/profile/candidate.rs
+171 -264
View File
@@ -1,93 +1,55 @@
//! Private vehicle candidate and whole-profile validation boundary.
//! Editable vehicle candidate and whole-profile validation boundary.
use kaptan_config::{
Config, ConfigModel, ConfigValue, ConfigViolation, FieldDescriptor, FieldId, FieldWriteError,
ValidationRule,
};
use kaptan_log::LevelFilter;
use super::{
ActuatorLimits, AidingSettings, ControlSettings, PidSettings, SafetySettings, ThrustSettings,
TimingSettings, VehicleCapabilities, VehicleProfile,
control::{
ActuatorLimitsCandidate, AidingCandidate, ControlCandidate, PidCandidate, ThrustCandidate,
},
mavlink_logging::MavlinkLoggingCandidate,
safety::SafetyCandidate,
timing::TimingCandidate,
ActuatorLimits, ControlSettings, PidSettings, SafetySettings, ThrustSettings, TimingSettings,
VehicleCapabilities, VehicleProfile,
};
/// Current persisted CBOR schema for the vehicle candidate.
pub const FORMAT_VERSION: u16 = 3;
/// Complete editable vehicle candidate.
///
/// Fields are private outside this crate. Local snapshot recovery and MAVLink
/// scalar writes both use the same descriptor/mutation and full-validation
/// path.
#[derive(Clone, Copy, Config)]
/// Descriptor writes require complete validation before activation.
#[derive(Clone, Copy, Debug, PartialEq, Config)]
#[config(post = Self::validate_flight)]
pub struct CandidateProfile {
pub(super) safety: SafetyCandidate,
pub(super) timing: TimingCandidate,
pub(super) aiding: AidingCandidate,
pub(super) control: ControlCandidate,
pub(super) thrust: ThrustCandidate,
pub(super) rate_pid: PidCandidate,
pub(super) actuator_limits: ActuatorLimitsCandidate,
pub(super) mavlink_logging: MavlinkLoggingCandidate,
pub safety: SafetySettings,
pub timing: TimingSettings,
pub control: ControlSettings,
pub thrust: ThrustSettings,
pub rate_pid: PidSettings,
pub actuator_limits: ActuatorLimits,
pub flight: super::FlightSettings,
}
impl CandidateProfile {
/// Explicit factory candidate used by the vehicle-profile examples.
///
/// Board firmware supplies its own factory [`Config`] through the board
/// composition root; configuration types never synthesize values through
/// [`Default`].
pub const fn factory() -> Self {
Self {
safety: SafetyCandidate {
prearm_dwell_microseconds: 100_000,
},
timing: TimingCandidate {
maximum_imu_interval_microseconds: 50_000,
imu_freshness_microseconds: 50_000,
local_state_freshness_microseconds: 150_000,
command_link_freshness_microseconds: 500_000,
},
aiding: AidingCandidate {
position_variance_m2: 0.04,
velocity_variance_m2_per_second_squared: 0.04,
},
control: ControlCandidate {
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,
},
thrust: ThrustCandidate {
mass_kg: 1.2,
maximum_collective_thrust_newtons: 18.0,
maximum_tilt_tangent: 0.700_207_53,
},
rate_pid: PidCandidate {
proportional: 0.035,
integral: 0.006,
derivative: 0.0,
output_limit: 0.10,
integral_limit: 0.025,
},
actuator_limits: ActuatorLimitsCandidate {
minimum: 0.0,
maximum: 1.0,
slew_per_second: 8.0,
},
mavlink_logging: MavlinkLoggingCandidate {
level_filter: LevelFilter::Info,
},
fn validate_flight(&self) -> Result<(), ConfigViolation> {
use super::ids;
use kaptan_config::ValidationRule;
let field = if !(10_000..=20_000).contains(&self.timing.maximum_imu_interval_microseconds) {
Some(ids::MAXIMUM_IMU_INTERVAL_MICROSECONDS)
} else if self.timing.imu_freshness_microseconds != 20_000 {
Some(ids::IMU_FRESHNESS_MICROSECONDS)
} else if !(200_000..=500_000).contains(&self.timing.local_state_freshness_microseconds) {
Some(ids::LOCAL_STATE_FRESHNESS_MICROSECONDS)
} else if self.timing.offboard_timeout_microseconds != 300_000 {
Some(ids::OFFBOARD_TIMEOUT_MICROSECONDS)
} else if self.actuator_limits.minimum != 0. {
Some(ids::ACTUATOR_MINIMUM)
}
// The 0.5-s landing cap ramp can descend by at most 2 command/s.
else if self.actuator_limits.slew_per_second < 2. {
Some(ids::ACTUATOR_SLEW_PER_SECOND)
} else {
None
};
if let Some(id) = field {
return Err(ConfigViolation::field(id, ValidationRule::Inconsistent));
}
Ok(())
}
/// Returns the complete transport-neutral descriptor registry.
pub fn descriptors() -> impl ExactSizeIterator<Item = &'static FieldDescriptor> + Clone {
(0..Self::FIELD_COUNT).map(|index| {
@@ -95,130 +57,11 @@ impl CandidateProfile {
.expect("derived aggregate descriptor count must be exact")
})
}
fn from_active(active: &VehicleProfile) -> Self {
let safety = active.safety();
let timing = active.timing();
let aiding = active.aiding();
let control = active.control();
let thrust = active.thrust();
let rate_pid = active.rate_pid();
let actuator_limits = active.actuator_limits();
let mavlink_logging = active.mavlink_logging();
Self {
safety: SafetyCandidate {
prearm_dwell_microseconds: safety.prearm_dwell_microseconds(),
},
timing: TimingCandidate {
maximum_imu_interval_microseconds: timing.maximum_imu_interval_microseconds(),
imu_freshness_microseconds: timing.imu_freshness_microseconds(),
local_state_freshness_microseconds: timing.local_state_freshness_microseconds(),
command_link_freshness_microseconds: timing.command_link_freshness_microseconds(),
},
aiding: AidingCandidate {
position_variance_m2: aiding.position_variance_m2(),
velocity_variance_m2_per_second_squared: aiding
.velocity_variance_m2_per_second_squared(),
},
control: ControlCandidate {
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(),
},
thrust: ThrustCandidate {
mass_kg: thrust.mass_kg(),
maximum_collective_thrust_newtons: thrust.maximum_collective_thrust_newtons(),
maximum_tilt_tangent: thrust.maximum_tilt_tangent(),
},
rate_pid: PidCandidate {
proportional: rate_pid.proportional(),
integral: rate_pid.integral(),
derivative: rate_pid.derivative(),
output_limit: rate_pid.output_limit(),
integral_limit: rate_pid.integral_limit(),
},
actuator_limits: ActuatorLimitsCandidate {
minimum: actuator_limits.minimum(),
maximum: actuator_limits.maximum(),
slew_per_second: actuator_limits.slew_per_second(),
},
mavlink_logging: MavlinkLoggingCandidate {
level_filter: mavlink_logging,
},
}
}
fn validate_complete(
&self,
capabilities: &VehicleCapabilities,
) -> Result<VehicleProfile, ConfigViolation> {
<Self as Config>::validate(self)?;
self.validate_capabilities(capabilities)?;
Ok(VehicleProfile {
safety: SafetySettings {
prearm_dwell_microseconds: self.safety.prearm_dwell_microseconds,
},
timing: TimingSettings {
maximum_imu_interval_microseconds: self.timing.maximum_imu_interval_microseconds,
imu_freshness_microseconds: self.timing.imu_freshness_microseconds,
local_state_freshness_microseconds: self.timing.local_state_freshness_microseconds,
command_link_freshness_microseconds: self
.timing
.command_link_freshness_microseconds,
},
aiding: AidingSettings {
position_variance_m2: self.aiding.position_variance_m2,
velocity_variance_m2_per_second_squared: self
.aiding
.velocity_variance_m2_per_second_squared,
},
control: ControlSettings {
position_gain_per_second_squared: self.control.position_gain_per_second_squared,
velocity_gain_per_second: self.control.velocity_gain_per_second,
maximum_horizontal_acceleration_mps2: self
.control
.maximum_horizontal_acceleration_mps2,
maximum_upward_acceleration_mps2: self.control.maximum_upward_acceleration_mps2,
maximum_downward_acceleration_mps2: self.control.maximum_downward_acceleration_mps2,
},
thrust: ThrustSettings {
mass_kg: self.thrust.mass_kg,
maximum_collective_thrust_newtons: self.thrust.maximum_collective_thrust_newtons,
maximum_tilt_tangent: self.thrust.maximum_tilt_tangent,
},
rate_pid: PidSettings {
proportional: self.rate_pid.proportional,
integral: self.rate_pid.integral,
derivative: self.rate_pid.derivative,
output_limit: self.rate_pid.output_limit,
integral_limit: self.rate_pid.integral_limit,
},
actuator_limits: ActuatorLimits {
minimum: self.actuator_limits.minimum,
maximum: self.actuator_limits.maximum,
slew_per_second: self.actuator_limits.slew_per_second,
},
mavlink_logging: self.mavlink_logging.level_filter,
mixer: capabilities.mixer(),
})
}
fn validate_capabilities(
&self,
capabilities: &VehicleCapabilities,
) -> Result<(), ConfigViolation> {
if capabilities.actuator_count() != 4 {
return Err(ConfigViolation::global(
ValidationRule::UnsupportedCapability,
));
}
Ok(())
}
}
// ConfigModel exposes the complete registry count as u16 on the wire.
const _: () = assert!(CandidateProfile::FIELD_COUNT <= u16::MAX as usize);
impl ConfigModel for CandidateProfile {
type Active = VehicleProfile;
type Context = VehicleCapabilities;
@@ -237,7 +80,7 @@ impl ConfigModel for CandidateProfile {
}
fn active_field_value(active: &Self::Active, id: FieldId) -> Option<ConfigValue> {
<Self as Config>::field_value(&Self::from_active(active), id)
active.field_value(id)
}
fn set_field(&mut self, id: FieldId, value: ConfigValue) -> Result<(), FieldWriteError> {
@@ -245,7 +88,7 @@ impl ConfigModel for CandidateProfile {
}
fn validate(&self, context: &Self::Context) -> Result<Self::Active, ConfigViolation> {
self.validate_complete(context)
VehicleProfile::validate(self, *context)
}
}
@@ -253,18 +96,43 @@ impl ConfigModel for CandidateProfile {
mod tests {
use kaptan_config::{ConfigModel, ConfigValue, FieldId, FieldWriteError, ValidationRule};
use super::{CandidateProfile, LevelFilter, VehicleCapabilities};
use super::{CandidateProfile, VehicleCapabilities};
use crate::profile::ids;
use crate::test_support::{candidate as fixture_candidate, capabilities};
const CAPABILITIES: VehicleCapabilities = VehicleCapabilities::quad_x();
const CAPABILITIES: VehicleCapabilities = capabilities();
#[test]
fn installation_activation() {
let mut installation = crate::test_support::installation();
installation.calibration = kaptan_navigation::CalibrationRevision::new(29);
installation.reference = kaptan_navigation::SensorReferenceId(42);
installation.atmosphere.pressure_pascals = 80_000.0;
installation.contact.half_span_x_metres = 0.3;
let mut candidate = fixture_candidate();
let active = candidate
.validate(&VehicleCapabilities::quad_x(installation))
.unwrap();
assert_eq!(active.navigation().installation, installation);
assert_eq!(active.contact_geometry(), installation.contact);
candidate.thrust.mass_kg = f32::NAN;
assert!(candidate.validate(&CAPABILITIES).is_err());
assert!(active.thrust().mass_kg.is_finite());
installation.range.direction_frd = kaptan_math::Vector3::zero();
assert_eq!(
fixture_candidate().validate(&VehicleCapabilities::quad_x(installation)),
Err(kaptan_config::ConfigViolation::global(
ValidationRule::Inconsistent
))
);
}
#[test]
fn generated_schema_fingerprint_is_stable() {
assert_eq!(
crate::SCHEMA_ID,
[
0xb9, 0x8b, 0xf9, 0xa0, 0x62, 0x3f, 0x6e, 0xef, 0x4e, 0x88, 0xc6, 0xd8, 0x89, 0x4c,
0x76, 0x8b,
172, 176, 17, 7, 200, 246, 190, 201, 231, 26, 101, 3, 145, 64, 206, 150,
]
);
}
@@ -278,12 +146,72 @@ mod tests {
#[test]
fn every_descriptor_has_a_readable_factory_value() {
let candidate = CandidateProfile::factory();
let candidate = fixture_candidate();
for descriptor in CandidateProfile::descriptors() {
assert!(candidate.field_value(descriptor.id).is_some());
}
}
#[test]
fn active_field_reads_preserve_validated_bits_after_candidate_edits() {
let factory = fixture_candidate();
let mut candidate = factory;
for descriptor in CandidateProfile::descriptors() {
let value = match factory.field_value(descriptor.id).unwrap() {
ConfigValue::U64(value) => ConfigValue::U64(match descriptor.id.get() {
2 => 19_000,
// Fixed engineering cadence and stream policy are still
// checked byte-for-byte in the immutable active snapshot.
3 | 5 | 33 | 34 => value,
_ => value - u64::from(descriptor.id.get()),
}),
ConfigValue::F32(0.0) => ConfigValue::F32(-0.0),
ConfigValue::F32(value) => ConfigValue::F32(value * 0.875),
value => panic!("unexpected profile scalar: {value:?}"),
};
candidate.set_field(descriptor.id, value).unwrap();
}
let validated = candidate;
let active = candidate.validate(&CAPABILITIES).unwrap();
for edit_candidate in [false, true] {
for descriptor in CandidateProfile::descriptors() {
if edit_candidate {
candidate
.set_field(descriptor.id, factory.field_value(descriptor.id).unwrap())
.unwrap();
}
let expected = validated.field_value(descriptor.id).unwrap();
let actual = CandidateProfile::active_field_value(&active, descriptor.id).unwrap();
match (actual, expected) {
(ConfigValue::F32(actual), ConfigValue::F32(expected)) => {
assert_eq!(actual.to_bits(), expected.to_bits(), "{descriptor:?}");
}
_ => assert_eq!(actual, expected, "{descriptor:?}"),
}
}
}
assert_eq!(candidate, factory);
assert_eq!(
CandidateProfile::active_field_value(&active, FieldId::new(24)),
None
);
}
#[test]
fn editing_a_section_copy_cannot_mutate_the_active_profile() {
let active = fixture_candidate().validate(&CAPABILITIES).unwrap();
let mut thrust = active.thrust();
kaptan_config::Config::set_field(&mut thrust, ids::MASS_KG, ConfigValue::F32(100.0))
.unwrap();
assert_eq!(thrust.mass_kg, 100.0);
assert!(kaptan_config::Config::validate(&thrust).is_err());
assert_eq!(active.thrust().mass_kg.to_bits(), 1.2_f32.to_bits());
assert_eq!(
CandidateProfile::active_field_value(&active, ids::MASS_KG),
Some(ConfigValue::F32(1.2))
);
}
#[test]
fn aggregated_descriptor_ids_are_unique() {
for (index, descriptor) in CandidateProfile::descriptors().enumerate() {
@@ -293,11 +221,19 @@ mod tests {
.all(|other| other.id != descriptor.id)
);
}
let mut candidate = fixture_candidate();
for retired in [FieldId::new(6), FieldId::new(7), FieldId::new(25)] {
assert_eq!(candidate.field_value(retired), None);
assert_eq!(
candidate.set_field(retired, ConfigValue::F32(0.01)),
Err(FieldWriteError::UnknownField(retired))
);
}
}
#[test]
fn cross_field_invalid_edit_is_retained_only_in_the_candidate() {
let mut candidate = CandidateProfile::factory();
let mut candidate = fixture_candidate();
candidate
.set_field(
ids::MAXIMUM_IMU_INTERVAL_MICROSECONDS,
@@ -312,13 +248,42 @@ mod tests {
#[test]
fn local_write_error_retains_exact_field_and_rule() {
let mut candidate = CandidateProfile::factory();
let mut candidate = fixture_candidate();
assert_eq!(
candidate.set_field(ids::MASS_KG, ConfigValue::F32(f32::NAN)),
Err(FieldWriteError::non_finite(ids::MASS_KG))
);
}
#[test]
fn pid_gain_domain_includes_derivative_and_preserves_rejected_writes() {
for key in [
"control.rate_pid.proportional",
"control.rate_pid.integral",
"control.rate_pid.derivative",
"control.yaw_rate_pid.proportional",
"control.yaw_rate_pid.integral",
"control.attitude_roll_pitch_gain_per_s",
"control.attitude_yaw_gain_per_s",
] {
let id = descriptor_id(key);
for gain in [0.0, f32::MIN_POSITIVE, 0.001, 0.5, 1.0, 5.0, 10.0] {
let mut candidate = fixture_candidate();
candidate.set_field(id, ConfigValue::F32(gain)).unwrap();
let active = candidate.validate(&CAPABILITIES).unwrap();
assert_eq!(
CandidateProfile::active_field_value(&active, id),
Some(ConfigValue::F32(gain))
);
for invalid in [-1.0, f32::NAN, f32::INFINITY, 10.01] {
let before = candidate;
assert!(candidate.set_field(id, ConfigValue::F32(invalid)).is_err());
assert_eq!(candidate, before);
}
}
}
}
#[test]
fn descriptor_bounds_do_not_conflict_with_derived_field_validation() {
for descriptor in CandidateProfile::descriptors() {
@@ -326,7 +291,7 @@ mod tests {
.into_iter()
.flatten()
{
let mut candidate = CandidateProfile::factory();
let mut candidate = fixture_candidate();
candidate
.set_field(descriptor.id, bound)
.expect("descriptor bound must pass the descriptor-backed setter");
@@ -341,62 +306,4 @@ mod tests {
}
}
}
#[test]
fn mavlink_logging_policy_is_persisted_and_restart_only() {
let mut candidate = CandidateProfile::factory();
let level_descriptor = CandidateProfile::descriptors()
.find(|descriptor| descriptor.display_key == "mavlink.logging.level_filter")
.unwrap();
assert_eq!(
level_descriptor.enum_options,
LevelFilter::CONFIG_ENUM_OPTIONS
);
candidate
.set_field(
descriptor_id("mavlink.logging.level_filter"),
ConfigValue::U8(LevelFilter::Off.wire_value()),
)
.unwrap();
let active = candidate.validate(&CAPABILITIES).unwrap();
assert_eq!(active.mavlink_logging(), LevelFilter::Off);
candidate
.set_field(
descriptor_id("mavlink.logging.level_filter"),
ConfigValue::U8(LevelFilter::Trace.wire_value()),
)
.unwrap();
assert_eq!(
candidate.validate(&CAPABILITIES).unwrap().mavlink_logging(),
LevelFilter::Trace
);
assert!(matches!(
candidate.set_field(
descriptor_id("mavlink.logging.level_filter"),
ConfigValue::U8(6),
),
Err(FieldWriteError::Violation(issue))
if issue.rule() == ValidationRule::InvalidEnumValue
));
}
#[test]
fn every_persisted_log_filter_is_retained_exactly() {
for raw in 0..=5 {
let mut candidate = CandidateProfile::factory();
candidate
.set_field(
descriptor_id("mavlink.logging.level_filter"),
ConfigValue::U8(raw),
)
.expect("descriptor accepts the stable level filter");
assert_eq!(
candidate
.validate(&CAPABILITIES)
.expect("valid complete profile")
.mavlink_logging(),
LevelFilter::try_from_wire(raw).unwrap()
);
}
}
}
firmware/libs/kaptan-flight-config/src/profile/control.rs
+85 -48
View File
@@ -2,28 +2,10 @@ use kaptan_config::{Config, ConfigViolation, ValidationRule};
use super::ids;
#[derive(Clone, Copy, Config)]
pub(super) struct AidingCandidate {
#[config(
id = 6,
key = "aiding.position_variance_m2",
unit = "m2",
finite,
range(min = 0.000_001, max = 10_000.0)
)]
pub(super) position_variance_m2: f32,
#[config(
id = 7,
key = "aiding.velocity_variance_m2ps2",
unit = "m2/s2",
finite,
range(min = 0.000_001, max = 10_000.0)
)]
pub(super) velocity_variance_m2_per_second_squared: f32,
}
#[derive(Clone, Copy, Config)]
pub(super) struct ControlCandidate {
/// Outer-loop controller policy.
#[derive(Clone, Copy, Debug, PartialEq, Config)]
#[config(post = Self::validate_control)]
pub struct ControlSettings {
#[config(
id = 8,
key = "control.position_gain_per_s2",
@@ -31,7 +13,7 @@ pub(super) struct ControlCandidate {
finite,
range(min = 0.0, max = 100.0)
)]
pub(super) position_gain_per_second_squared: f32,
pub position_gain_per_second_squared: f32,
#[config(
id = 9,
key = "control.velocity_gain_per_s",
@@ -39,7 +21,7 @@ pub(super) struct ControlCandidate {
finite,
range(min = 0.0, max = 100.0)
)]
pub(super) velocity_gain_per_second: f32,
pub velocity_gain_per_second: f32,
#[config(
id = 10,
key = "control.maximum_horizontal_acceleration_mps2",
@@ -47,7 +29,7 @@ pub(super) struct ControlCandidate {
finite,
range(min = 0.001, max = 100.0)
)]
pub(super) maximum_horizontal_acceleration_mps2: f32,
pub maximum_horizontal_acceleration_mps2: f32,
#[config(
id = 11,
key = "control.maximum_upward_acceleration_mps2",
@@ -55,7 +37,7 @@ pub(super) struct ControlCandidate {
finite,
range(min = 0.001, max = 100.0)
)]
pub(super) maximum_upward_acceleration_mps2: f32,
pub maximum_upward_acceleration_mps2: f32,
#[config(
id = 12,
key = "control.maximum_downward_acceleration_mps2",
@@ -63,11 +45,43 @@ pub(super) struct ControlCandidate {
finite,
range(min = 0.001, max = 100.0)
)]
pub(super) maximum_downward_acceleration_mps2: f32,
pub maximum_downward_acceleration_mps2: f32,
#[config(
id = 41,
key = "control.attitude_roll_pitch_gain_per_s",
unit = "1/s",
finite,
range(min = 0.0, max = 10.0)
)]
pub attitude_roll_pitch_gain_per_second: f32,
#[config(
id = 42,
key = "control.attitude_yaw_gain_per_s",
unit = "1/s",
finite,
range(min = 0.0, max = 10.0)
)]
pub attitude_yaw_gain_per_second: f32,
}
#[derive(Clone, Copy, Config)]
impl ControlSettings {
fn validate_control(&self) -> Result<(), ConfigViolation> {
// Positive multicopter thrust requires downward acceleration strictly
// below gravity. An inclusive scalar maximum cannot express this rule.
if self.maximum_downward_acceleration_mps2 >= kaptan_navigation::GRAVITY_MPS2 {
return Err(ConfigViolation::field(
ids::MAXIMUM_DOWNWARD_ACCELERATION_MPS2,
ValidationRule::Inconsistent,
));
}
Ok(())
}
}
/// Physical thrust envelope.
#[derive(Clone, Copy, Debug, PartialEq, Config)]
#[config(post = Self::validate_thrust)]
pub(super) struct ThrustCandidate {
pub struct ThrustSettings {
#[config(
id = 13,
key = "thrust.mass_kg",
@@ -75,7 +89,7 @@ pub(super) struct ThrustCandidate {
finite,
range(min = 0.01, max = 100.0)
)]
pub(super) mass_kg: f32,
pub mass_kg: f32,
#[config(
id = 14,
key = "thrust.maximum_collective_thrust_n",
@@ -83,7 +97,7 @@ pub(super) struct ThrustCandidate {
finite,
range(min = 0.1, max = 10_000.0)
)]
pub(super) maximum_collective_thrust_newtons: f32,
pub maximum_collective_thrust_newtons: f32,
#[config(
id = 15,
key = "thrust.maximum_tilt_tangent",
@@ -91,11 +105,16 @@ pub(super) struct ThrustCandidate {
finite,
range(min = 0.001, max = 10.0)
)]
pub(super) maximum_tilt_tangent: f32,
pub maximum_tilt_tangent: f32,
}
impl ThrustCandidate {
impl ThrustSettings {
fn validate_thrust(&self) -> Result<(), ConfigViolation> {
if self.maximum_collective_thrust_newtons <= self.mass_kg * 9.806_65 {
if self.maximum_collective_thrust_newtons <= self.mass_kg * kaptan_navigation::GRAVITY_MPS2
|| f64::from(self.maximum_collective_thrust_newtons)
> f64::from(self.mass_kg)
* (f64::from(kaptan_navigation::MAXIMUM_FREE_ACCELERATION_MPS2)
- f64::from(kaptan_navigation::GRAVITY_MPS2))
{
return Err(ConfigViolation::related(
ids::MAXIMUM_COLLECTIVE_THRUST_NEWTONS,
ids::MASS_KG,
@@ -106,9 +125,10 @@ impl ThrustCandidate {
}
}
#[derive(Clone, Copy, Config)]
/// Roll/pitch and yaw P/I gains with shared derivative and contribution limits.
#[derive(Clone, Copy, Debug, PartialEq, Config)]
#[config(post = Self::validate_pid)]
pub(super) struct PidCandidate {
pub struct PidSettings {
#[config(
id = 16,
key = "control.rate_pid.proportional",
@@ -116,7 +136,7 @@ pub(super) struct PidCandidate {
finite,
range(min = 0.0, max = 10.0)
)]
pub(super) proportional: f32,
pub proportional: f32,
#[config(
id = 17,
key = "control.rate_pid.integral",
@@ -124,7 +144,7 @@ pub(super) struct PidCandidate {
finite,
range(min = 0.0, max = 10.0)
)]
pub(super) integral: f32,
pub integral: f32,
#[config(
id = 18,
key = "control.rate_pid.derivative",
@@ -132,7 +152,7 @@ pub(super) struct PidCandidate {
finite,
range(min = 0.0, max = 10.0)
)]
pub(super) derivative: f32,
pub derivative: f32,
#[config(
id = 19,
key = "control.rate_pid.output_limit",
@@ -140,7 +160,7 @@ pub(super) struct PidCandidate {
finite,
range(min = 0.001, max = 1.0)
)]
pub(super) output_limit: f32,
pub output_limit: f32,
#[config(
id = 20,
key = "control.rate_pid.integral_limit",
@@ -148,9 +168,25 @@ pub(super) struct PidCandidate {
finite,
range(min = 0.0, max = 1.0)
)]
pub(super) integral_limit: f32,
pub integral_limit: f32,
#[config(
id = 43,
key = "control.yaw_rate_pid.proportional",
unit = "",
finite,
range(min = 0.0, max = 10.0)
)]
pub yaw_proportional: f32,
#[config(
id = 44,
key = "control.yaw_rate_pid.integral",
unit = "",
finite,
range(min = 0.0, max = 10.0)
)]
pub yaw_integral: f32,
}
impl PidCandidate {
impl PidSettings {
fn validate_pid(&self) -> Result<(), ConfigViolation> {
if self.integral_limit > self.output_limit {
return Err(ConfigViolation::related(
@@ -163,9 +199,10 @@ impl PidCandidate {
}
}
#[derive(Clone, Copy, Config)]
/// Actuator command bounds.
#[derive(Clone, Copy, Debug, PartialEq, Config)]
#[config(post = Self::validate_actuator_limits)]
pub(super) struct ActuatorLimitsCandidate {
pub struct ActuatorLimits {
#[config(
id = 21,
key = "actuator.minimum",
@@ -173,7 +210,7 @@ pub(super) struct ActuatorLimitsCandidate {
finite,
range(min = 0.0, max = 1.0)
)]
pub(super) minimum: f32,
pub minimum: f32,
#[config(
id = 22,
key = "actuator.maximum",
@@ -181,7 +218,7 @@ pub(super) struct ActuatorLimitsCandidate {
finite,
range(min = 0.0, max = 1.0)
)]
pub(super) maximum: f32,
pub maximum: f32,
#[config(
id = 23,
key = "actuator.slew_per_second",
@@ -189,9 +226,9 @@ pub(super) struct ActuatorLimitsCandidate {
finite,
range(min = 0.001, max = 100.0)
)]
pub(super) slew_per_second: f32,
pub slew_per_second: f32,
}
impl ActuatorLimitsCandidate {
impl ActuatorLimits {
fn validate_actuator_limits(&self) -> Result<(), ConfigViolation> {
if self.minimum >= self.maximum {
return Err(ConfigViolation::related(
firmware/libs/kaptan-flight-config/src/profile/flight.rs
+88
View File
@@ -0,0 +1,88 @@
//! Restart-only flight policy and continuous process-noise densities.
use kaptan_config::Config;
#[derive(Clone, Copy, Debug, PartialEq, Config)]
pub struct FlightSettings {
#[config(
id = 30,
key = "flight.command_ttl_us",
unit = "us",
range(min = 1, max = 250_000)
)]
pub command_ttl_us: u64,
#[config(
id = 31,
key = "flight.fallback_hold_us",
unit = "us",
range(min = 1, max = 1_000_000)
)]
pub fallback_hold_us: u64,
#[config(
id = 32,
key = "flight.output_lifetime_us",
unit = "us",
range(min = 10_000, max = 50_000)
)]
pub output_lifetime_us: u64,
#[config(
id = 33,
key = "flight.control_period_us",
unit = "us",
range(min = 10_000, max = 10_000)
)]
pub control_period_us: u64,
#[config(
id = 34,
key = "flight.control_deadline_us",
unit = "us",
range(min = 20_000, max = 20_000)
)]
pub control_deadline_us: u64,
#[config(
id = 35,
key = "navigation.time_error_bound_us",
unit = "us",
range(min = 0, max = 2_000)
)]
pub time_error_bound_us: u64,
#[config(
id = 36,
key = "navigation.gyro_psd",
unit = "rad2/s",
finite,
range(min = 1e-12, max = 0.01)
)]
pub gyro_psd: f32,
#[config(
id = 37,
key = "navigation.acceleration_psd",
unit = "m2/s3",
finite,
range(min = 1e-12, max = 0.1)
)]
pub acceleration_psd: f32,
#[config(
id = 38,
key = "navigation.gyro_bias_psd",
unit = "rad2/s3",
finite,
range(min = 1e-14, max = 0.01)
)]
pub gyro_bias_psd: f32,
#[config(
id = 39,
key = "navigation.acceleration_bias_psd",
unit = "m2/s5",
finite,
range(min = 1e-14, max = 0.1)
)]
pub acceleration_bias_psd: f32,
#[config(
id = 40,
key = "navigation.barometric_datum_psd",
unit = "m2/s",
finite,
range(min = 1e-12, max = 0.1)
)]
pub barometric_datum_psd: f32,
}
firmware/libs/kaptan-flight-config/src/profile/ids.rs
+16 -16
View File
@@ -1,20 +1,20 @@
//! Symbolic projections used by relation checks and focused tests.
//!
//! These constants never define an ID. Each projects the metadata generated
//! from its owning `#[config(id = ...)]` declaration.
//! Field IDs derived from their owning `#[config(id = ...)]` declarations.
use kaptan_config::FieldId;
use super::{
control::{ActuatorLimitsCandidate, PidCandidate, ThrustCandidate},
timing::TimingCandidate,
};
use super::{ActuatorLimits, ControlSettings, PidSettings, ThrustSettings, TimingSettings};
pub(super) const MAXIMUM_IMU_INTERVAL_MICROSECONDS: FieldId = TimingCandidate::DESCRIPTORS[0].id;
pub(super) const IMU_FRESHNESS_MICROSECONDS: FieldId = TimingCandidate::DESCRIPTORS[1].id;
pub(super) const MASS_KG: FieldId = ThrustCandidate::DESCRIPTORS[0].id;
pub(super) const MAXIMUM_COLLECTIVE_THRUST_NEWTONS: FieldId = ThrustCandidate::DESCRIPTORS[1].id;
pub(super) const RATE_PID_OUTPUT_LIMIT: FieldId = PidCandidate::DESCRIPTORS[3].id;
pub(super) const RATE_PID_INTEGRAL_LIMIT: FieldId = PidCandidate::DESCRIPTORS[4].id;
pub(super) const ACTUATOR_MINIMUM: FieldId = ActuatorLimitsCandidate::DESCRIPTORS[0].id;
pub(super) const ACTUATOR_MAXIMUM: FieldId = ActuatorLimitsCandidate::DESCRIPTORS[1].id;
// Field IDs 6, 7 and 25 remain reserved.
pub(super) const MAXIMUM_IMU_INTERVAL_MICROSECONDS: FieldId = TimingSettings::DESCRIPTORS[0].id;
pub(super) const IMU_FRESHNESS_MICROSECONDS: FieldId = TimingSettings::DESCRIPTORS[1].id;
pub(super) const LOCAL_STATE_FRESHNESS_MICROSECONDS: FieldId = TimingSettings::DESCRIPTORS[2].id;
pub(super) const OFFBOARD_TIMEOUT_MICROSECONDS: FieldId = TimingSettings::DESCRIPTORS[3].id;
pub(super) const MAXIMUM_DOWNWARD_ACCELERATION_MPS2: FieldId = ControlSettings::DESCRIPTORS[4].id;
pub(super) const MASS_KG: FieldId = ThrustSettings::DESCRIPTORS[0].id;
pub(super) const MAXIMUM_COLLECTIVE_THRUST_NEWTONS: FieldId = ThrustSettings::DESCRIPTORS[1].id;
pub(super) const RATE_PID_OUTPUT_LIMIT: FieldId = PidSettings::DESCRIPTORS[3].id;
pub(super) const RATE_PID_INTEGRAL_LIMIT: FieldId = PidSettings::DESCRIPTORS[4].id;
pub(super) const ACTUATOR_MINIMUM: FieldId = ActuatorLimits::DESCRIPTORS[0].id;
pub(super) const ACTUATOR_MAXIMUM: FieldId = ActuatorLimits::DESCRIPTORS[1].id;
pub(super) const ACTUATOR_SLEW_PER_SECOND: FieldId = ActuatorLimits::DESCRIPTORS[2].id;
firmware/libs/kaptan-flight-config/src/profile/mavlink_logging.rs
-16
View File
@@ -1,16 +0,0 @@
//! Persisted, restart-only MAVLink diagnostic-sink policy.
use kaptan_config::Config;
use kaptan_log::LevelFilter;
/// Candidate policy for the MAVLink diagnostic sink.
#[derive(Clone, Copy, Config)]
pub(super) struct MavlinkLoggingCandidate {
#[config(
enum,
id = 25,
key = "mavlink.logging.level_filter",
unit = "log_level"
)]
pub(super) level_filter: LevelFilter,
}
firmware/libs/kaptan-flight-config/src/profile/mod.rs
+7 -6
View File
@@ -3,13 +3,14 @@
mod active;
mod candidate;
mod control;
mod flight;
mod ids;
mod mavlink_logging;
mod safety;
mod timing;
pub use active::{
ActuatorLimits, AidingSettings, ControlSettings, PidSettings, SafetySettings, ThrustSettings,
TimingSettings, VehicleCapabilities, VehicleProfile,
};
pub use candidate::{CandidateProfile, FORMAT_VERSION};
pub use active::{VehicleCapabilities, VehicleProfile};
pub use candidate::CandidateProfile;
pub use control::{ActuatorLimits, ControlSettings, PidSettings, ThrustSettings};
pub use flight::FlightSettings;
pub use safety::SafetySettings;
pub use timing::TimingSettings;
firmware/libs/kaptan-flight-config/src/profile/safety.rs
+4 -3
View File
@@ -1,7 +1,8 @@
use kaptan_config::Config;
#[derive(Clone, Copy, Config)]
pub(super) struct SafetyCandidate {
/// Prearm timing policy, shared by editable candidates and active snapshots.
#[derive(Clone, Copy, Debug, Eq, PartialEq, Config)]
pub struct SafetySettings {
#[config(
id = 1,
key = "safety.prearm_dwell_us",
@@ -9,5 +10,5 @@ pub(super) struct SafetyCandidate {
nonzero,
range(min = 1, max = 10_000_000)
)]
pub(super) prearm_dwell_microseconds: u64,
pub prearm_dwell_microseconds: u64,
}
firmware/libs/kaptan-flight-config/src/profile/timing.rs
+19 -8
View File
@@ -2,9 +2,10 @@ use kaptan_config::{Config, ConfigViolation, ValidationRule};
use super::ids;
#[derive(Clone, Copy, Config)]
/// Stream timing policy, shared by editable candidates and active snapshots.
#[derive(Clone, Copy, Debug, Eq, PartialEq, Config)]
#[config(post = Self::validate_timing)]
pub(super) struct TimingCandidate {
pub struct TimingSettings {
#[config(
id = 2,
key = "timing.imu_max_interval_us",
@@ -12,7 +13,7 @@ pub(super) struct TimingCandidate {
nonzero,
range(min = 1, max = 1_000_000)
)]
pub(super) maximum_imu_interval_microseconds: u64,
pub maximum_imu_interval_microseconds: u64,
#[config(
id = 3,
key = "timing.imu_freshness_us",
@@ -20,7 +21,7 @@ pub(super) struct TimingCandidate {
nonzero,
range(min = 1, max = 1_000_000)
)]
pub(super) imu_freshness_microseconds: u64,
pub imu_freshness_microseconds: u64,
#[config(
id = 4,
key = "timing.local_state_freshness_us",
@@ -28,19 +29,29 @@ pub(super) struct TimingCandidate {
nonzero,
range(min = 1, max = 10_000_000)
)]
pub(super) local_state_freshness_microseconds: u64,
pub local_state_freshness_microseconds: u64,
#[config(
id = 5,
key = "timing.link_freshness_us",
key = "timing.offboard_timeout_us",
unit = "us",
nonzero,
range(min = 1, max = 10_000_000)
)]
pub(super) command_link_freshness_microseconds: u64,
pub offboard_timeout_microseconds: u64,
}
impl TimingCandidate {
impl TimingSettings {
fn validate_timing(&self) -> Result<(), ConfigViolation> {
// The shared primitives retain their numerical upper bound. The flight
// candidate separately validates its tighter IMU and control cadences.
if self.maximum_imu_interval_microseconds
> kaptan_control::MAXIMUM_CONTROL_STEP_MICROSECONDS
{
return Err(ConfigViolation::field(
ids::MAXIMUM_IMU_INTERVAL_MICROSECONDS,
ValidationRule::Inconsistent,
));
}
if self.maximum_imu_interval_microseconds > self.imu_freshness_microseconds {
return Err(ConfigViolation::related(
ids::MAXIMUM_IMU_INTERVAL_MICROSECONDS,
firmware/libs/kaptan-flight-config/tests/control_domain.rs
+275
View File
@@ -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);
}
}
firmware/libs/kaptan-flight-config/tests/support/mod.rs
+96
View File
@@ -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())
}