From c19cf7431406fae056f98b0b4dd05a4e76efe460 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Onur=20=C3=96zkan?= Date: Sun, 9 Aug 2026 13:58:14 +0300 Subject: [PATCH] control: add bounded trajectory and allocation core --- libs/kaptan-control/Cargo.toml | 13 + libs/kaptan-control/src/lib.rs | 2122 ++++++++++++++++++++++++++++++++ 2 files changed, 2135 insertions(+) create mode 100644 libs/kaptan-control/Cargo.toml create mode 100644 libs/kaptan-control/src/lib.rs diff --git a/libs/kaptan-control/Cargo.toml b/libs/kaptan-control/Cargo.toml new file mode 100644 index 0000000..51dcd4f --- /dev/null +++ b/libs/kaptan-control/Cargo.toml @@ -0,0 +1,13 @@ +[package] +name = "kaptan-control" +edition.workspace = true +license.workspace = true +repository.workspace = true +rust-version.workspace = true +version.workspace = true + +[lints] +workspace = true + +[dependencies] +kaptan-math = { path = "../kaptan-math" } diff --git a/libs/kaptan-control/src/lib.rs b/libs/kaptan-control/src/lib.rs new file mode 100644 index 0000000..a23005b --- /dev/null +++ b/libs/kaptan-control/src/lib.rs @@ -0,0 +1,2122 @@ +//! Allocation-free flight-control primitives. +//! +//! This crate contains no board layout, peripheral, RTIC, or allocator +//! inversion logic. A vehicle provides reviewed static geometry and +//! precomputed mixer rows; the high-rate path only performs bounded products, +//! saturation, and slew limiting. + +#![no_std] + +use kaptan_math::{Frd, Ned, UnitQuaternion, Vector3, sqrt}; + +/// Body-frame angular rate in radians per second. +#[derive(Clone, Copy, Debug, PartialEq)] +pub struct BodyRate(Vector3); +impl BodyRate { + /// Creates a body rate in rad/s. + pub const fn radians_per_second(roll: f32, pitch: f32, yaw: f32) -> Self { + Self(Vector3::new(roll, pitch, yaw)) + } + /// Returns the FRD vector. + pub const fn vector(self) -> Vector3 { + self.0 + } +} + +/// Desired NED force vector in newtons. +#[derive(Clone, Copy, Debug, PartialEq)] +pub struct NedForce(Vector3); +impl NedForce { + /// Creates a NED force in newtons. + pub const fn newtons(north: f32, east: f32, down: f32) -> Self { + Self(Vector3::new(north, east, down)) + } + /// Returns the NED force vector. + pub const fn vector(self) -> Vector3 { + self.0 + } +} + +/// Desired horizontal heading direction in NED, rather than an ambiguous yaw scalar. +#[derive(Clone, Copy, Debug, PartialEq)] +pub struct HeadingNed(Vector3); +impl HeadingNed { + /// Validates and normalizes a horizontal NED heading. + pub fn new(north: f32, east: f32) -> Option { + let vector = Vector3::new(north, east, 0.0).normalized()?; + Some(Self(vector)) + } + /// Returns the normalized NED heading vector. + pub const fn vector(self) -> Vector3 { + self.0 + } +} + +/// Force-to-attitude conversion failure. +#[derive(Clone, Copy, Debug, Eq, PartialEq)] +pub enum AttitudeError { + /// A zero or non-finite force has no defined thrust direction. + ZeroForce, + /// Requested heading is parallel to the thrust axis. + SingularHeading, +} + +/// Computes a body-to-NED attitude whose body-down axis opposes multicopter +/// thrust force and whose body-forward axis follows the requested heading as +/// closely as geometry allows. +pub fn force_to_attitude( + force: NedForce, + heading: HeadingNed, +) -> Result, AttitudeError> { + let body_down = force + .vector() + .scale(-1.0) + .normalized() + .ok_or(AttitudeError::ZeroForce)?; + let body_right = body_down + .cross(heading.vector()) + .normalized() + .ok_or(AttitudeError::SingularHeading)?; + let body_forward = body_right + .cross(body_down) + .normalized() + .ok_or(AttitudeError::SingularHeading)?; + UnitQuaternion::from_body_axes_in_ned(body_forward, body_right, body_down) + .map_err(|_| AttitudeError::SingularHeading) +} + +/// Bounded scalar PID configuration. +#[derive(Clone, Copy, Debug, PartialEq)] +pub struct PidConfig { + /// Proportional gain. + pub proportional: f32, + /// Integral gain. + pub integral: f32, + /// Derivative gain applied to supplied error derivative. + pub derivative: f32, + /// Absolute output bound. + pub output_limit: f32, + /// Absolute stored integral contribution bound. + pub integral_limit: f32, +} + +/// Invalid PID input/configuration. +#[derive(Clone, Copy, Debug, Eq, PartialEq)] +pub enum PidError { + /// Static PID gains or limits were non-finite or contradictory. + InvalidConfiguration, + /// Time step was non-finite, non-positive, or exceeded the 100-ms integration bound. + InvalidTime, + /// Error input was non-finite. + NonFinite, +} + +/// Scalar PID with conditional-integration anti-windup. +#[derive(Clone, Copy, Debug, PartialEq)] +pub struct Pid { + config: PidConfig, + integral_contribution: f32, +} + +impl Pid { + /// Creates a reset PID. Invalid limits intentionally make all updates fail + /// rather than being silently repaired in the flight path. + pub const fn new(config: PidConfig) -> Self { + Self { + config, + integral_contribution: 0.0, + } + } + /// Clears only the retained integral contribution. + pub fn reset(&mut self) { + self.integral_contribution = 0.0; + } + /// Returns the retained integral output contribution. + pub const fn integral_contribution(&self) -> f32 { + self.integral_contribution + } + + fn validate(&self, error: f32, error_rate: f32, dt_seconds: f32) -> Result<(), PidError> { + if !self.config.proportional.is_finite() + | !self.config.integral.is_finite() + | !self.config.derivative.is_finite() + | !self.config.output_limit.is_finite() + | (self.config.output_limit <= 0.0) + | !self.config.integral_limit.is_finite() + | (self.config.integral_limit < 0.0) + { + return Err(PidError::InvalidConfiguration); + } + if !dt_seconds.is_finite() + | (dt_seconds <= 0.0) + | (dt_seconds > MAXIMUM_CONTROL_STEP_SECONDS) + { + return Err(PidError::InvalidTime); + } + if !error.is_finite() || !error_rate.is_finite() { + return Err(PidError::NonFinite); + } + Ok(()) + } + + /// Updates with error and error derivative. Conditional integration keeps + /// the integrator from increasing saturation in its current direction. + pub fn update( + &mut self, + error: f32, + error_rate: f32, + dt_seconds: f32, + ) -> Result { + self.validate(error, error_rate, dt_seconds)?; + let proportional_derivative = + self.config.proportional * error + self.config.derivative * error_rate; + if !proportional_derivative.is_finite() { + return Err(PidError::NonFinite); + } + let integral_delta = self.config.integral * error * dt_seconds; + if !integral_delta.is_finite() { + return Err(PidError::NonFinite); + } + let tentative_integral = self.integral_contribution + integral_delta; + if !tentative_integral.is_finite() { + return Err(PidError::NonFinite); + } + let candidate = clamp( + tentative_integral, + -self.config.integral_limit, + self.config.integral_limit, + ); + // `tentative_integral` and both clamp bounds were validated finite, so + // the selected operand is finite as well. + debug_assert!(candidate.is_finite()); + let candidate_output = proportional_derivative + candidate; + if !candidate_output.is_finite() { + return Err(PidError::NonFinite); + } + let limited = clamp( + candidate_output, + -self.config.output_limit, + self.config.output_limit, + ); + if candidate_output == limited || candidate_output.signum() != error.signum() { + self.integral_contribution = candidate; + } + let output = clamp( + proportional_derivative + self.integral_contribution, + -self.config.output_limit, + self.config.output_limit, + ); + output + .is_finite() + .then_some(output) + .ok_or(PidError::NonFinite) + } +} + +/// Three independent body-axis PIDs for the rate loop. +pub struct RatePid { + roll: Pid, + pitch: Pid, + yaw: Pid, + previous_error: Option>, +} +impl RatePid { + /// Creates one fixed PID per FRD angular-rate axis. + pub const fn new(roll: PidConfig, pitch: PidConfig, yaw: PidConfig) -> Self { + Self { + roll: Pid::new(roll), + pitch: Pid::new(pitch), + yaw: Pid::new(yaw), + previous_error: None, + } + } + + /// Clears the retained integral and derivative history on every axis. + pub fn reset(&mut self) { + self.roll.reset(); + self.pitch.reset(); + self.yaw.reset(); + self.previous_error = None; + } + + /// Discards only retained integral authority after an actuator limit. + /// + /// Derivative history is intentionally retained: a mixer limit must not + /// manufacture a rate-error impulse on the next controller tick. + pub fn reset_integrators(&mut self) { + self.roll.reset(); + self.pitch.reset(); + self.yaw.reset(); + } + + /// Produces bounded roll/pitch/yaw torque requests from rate error. + pub fn update( + &mut self, + target: BodyRate, + measured: BodyRate, + dt_seconds: f32, + ) -> Result, PidError> { + let error = target.vector().sub(measured.vector()); + if !dt_seconds.is_finite() + | (dt_seconds <= 0.0) + | (dt_seconds > MAXIMUM_CONTROL_STEP_SECONDS) + { + return Err(PidError::InvalidTime); + } + let error_rate = self + .previous_error + .map(|previous| error.sub(previous).scale(1.0 / dt_seconds)) + .unwrap_or_else(Vector3::zero); + // Validate all axes before altering any retained state, so a malformed + // per-axis configuration cannot leave a partial controller update. + self.roll.validate(error.x(), error_rate.x(), dt_seconds)?; + self.pitch.validate(error.y(), error_rate.y(), dt_seconds)?; + self.yaw.validate(error.z(), error_rate.z(), dt_seconds)?; + let mut roll = self.roll; + let mut pitch = self.pitch; + let mut yaw = self.yaw; + let output = Vector3::new( + roll.update(error.x(), error_rate.x(), dt_seconds)?, + pitch.update(error.y(), error_rate.y(), dt_seconds)?, + yaw.update(error.z(), error_rate.z(), dt_seconds)?, + ); + self.roll = roll; + self.pitch = pitch; + self.yaw = yaw; + self.previous_error = Some(error); + Ok(output) + } +} + +/// Position, velocity, and acceleration reference in the local NED frame. +/// +/// The reference is generated continuously. A caller must not replace it with +/// a position step merely because an operator requested a distant goal. +#[derive(Clone, Copy, Debug, PartialEq)] +pub struct KinematicReference { + /// Local NED position in metres. + pub position: Vector3, + /// Local NED velocity in metres per second. + pub velocity: Vector3, + /// Local NED acceleration in metres per second squared. + pub acceleration: Vector3, +} + +/// Kinematic constraints for a local-position reference generator. +#[derive(Clone, Copy, Debug, PartialEq)] +pub struct KinematicLimits { + /// Inclusive magnitude bound for horizontal velocity in m/s. + pub maximum_horizontal_speed_mps: f32, + /// Inclusive magnitude bound for vertical velocity in m/s. + pub maximum_vertical_speed_mps: f32, + /// Inclusive magnitude bound for horizontal acceleration in m/s². + pub maximum_horizontal_acceleration_mps2: f32, + /// Inclusive magnitude bound for vertical acceleration in m/s². + pub maximum_vertical_acceleration_mps2: f32, +} + +/// Rejection returned instead of silently repairing a goal or time interval. +#[derive(Clone, Copy, Debug, Eq, PartialEq)] +pub enum TrajectoryError { + /// A static speed/acceleration constraint is invalid. + InvalidLimits, + /// A goal coordinate is non-finite. + NonFiniteGoal, + /// The service interval was non-finite, non-positive, or exceeded the 100-ms integration bound. + InvalidTime, + /// A finite input produced a non-finite intermediate or candidate state. + NumericalOverflow, +} + +/// A reference generator must not turn a stalled task into a large implicit +/// integration step. The scheduler must reset/reseed it after this bound. +const MAXIMUM_CONTROL_STEP_SECONDS: f32 = 0.1; + +/// Allocation-free velocity-ramped trajectory for one local NED goal. +/// +/// Horizontal motion is constrained as a vector, not independently per axis, +/// so a diagonal command cannot exceed the declared horizontal airspeed or +/// acceleration limit. Vertical motion has its own vehicle limit. +pub struct Trajectory { + limits: KinematicLimits, + reference: KinematicReference, + goal: Vector3, +} + +impl Trajectory { + /// Starts at a finite local position with zero velocity and acceleration. + pub fn new( + initial_position: Vector3, + limits: KinematicLimits, + ) -> Result { + if !valid_limits(limits) { + return Err(TrajectoryError::InvalidLimits); + } + if !initial_position.is_finite() { + return Err(TrajectoryError::NonFiniteGoal); + } + Ok(Self { + limits, + reference: KinematicReference { + position: initial_position, + velocity: Vector3::zero(), + acceleration: Vector3::zero(), + }, + goal: initial_position, + }) + } + + /// Replaces the absolute local NED destination without resetting motion. + pub fn set_goal(&mut self, goal: Vector3) -> Result<(), TrajectoryError> { + if !goal.is_finite() { + return Err(TrajectoryError::NonFiniteGoal); + } + self.goal = goal; + Ok(()) + } + + /// Returns the current absolute local NED destination. + pub const fn goal(&self) -> Vector3 { + self.goal + } + + /// Returns the current continuous reference without advancing it. + pub const fn reference(&self) -> KinematicReference { + self.reference + } + + /// Advances one bounded reference interval. + pub fn update(&mut self, dt_seconds: f32) -> Result { + if !dt_seconds.is_finite() || dt_seconds <= 0.0 || dt_seconds > MAXIMUM_CONTROL_STEP_SECONDS + { + return Err(TrajectoryError::InvalidTime); + } + + let position_error = self.goal.sub(self.reference.position); + if !position_error.is_finite() { + return Err(TrajectoryError::NumericalOverflow); + } + let target_horizontal_velocity = braking_velocity( + Vector3::new(position_error.x(), position_error.y(), 0.0), + self.limits.maximum_horizontal_speed_mps, + self.limits.maximum_horizontal_acceleration_mps2, + ) + .ok_or(TrajectoryError::NumericalOverflow)?; + let target_vertical_velocity = braking_velocity_axis( + position_error.z(), + self.limits.maximum_vertical_speed_mps, + self.limits.maximum_vertical_acceleration_mps2, + ) + .ok_or(TrajectoryError::NumericalOverflow)?; + let current_horizontal_velocity = Vector3::new( + self.reference.velocity.x(), + self.reference.velocity.y(), + 0.0, + ); + let horizontal_delta = limit_magnitude( + target_horizontal_velocity.sub(current_horizontal_velocity), + self.limits.maximum_horizontal_acceleration_mps2 * dt_seconds, + ) + .ok_or(TrajectoryError::NumericalOverflow)?; + let next_horizontal_velocity = current_horizontal_velocity.add(horizontal_delta); + let vertical_delta = clamp( + target_vertical_velocity - self.reference.velocity.z(), + -self.limits.maximum_vertical_acceleration_mps2 * dt_seconds, + self.limits.maximum_vertical_acceleration_mps2 * dt_seconds, + ); + let mut next_velocity = Vector3::new( + next_horizontal_velocity.x(), + next_horizontal_velocity.y(), + self.reference.velocity.z() + vertical_delta, + ); + let mut next_position = self.reference.position.add(next_velocity.scale(dt_seconds)); + if !next_velocity.is_finite() | !next_position.is_finite() { + return Err(TrajectoryError::NumericalOverflow); + } + + // A discrete trajectory can cross the goal by less than one service + // interval. Snap only when the current velocity can be stopped inside + // this interval's declared acceleration envelope; otherwise let the + // bounded braking law decelerate through the crossing normally. + let current_horizontal_speed = current_horizontal_velocity.norm(); + let horizontal_error = + Vector3::::new(position_error.x(), position_error.y(), 0.0).norm(); + if (horizontal_error + <= self.limits.maximum_horizontal_acceleration_mps2 * dt_seconds * dt_seconds) + & (position_error.z().abs() + <= self.limits.maximum_vertical_acceleration_mps2 * dt_seconds * dt_seconds) + & (current_horizontal_speed + <= self.limits.maximum_horizontal_acceleration_mps2 * dt_seconds) + & (self.reference.velocity.z().abs() + <= self.limits.maximum_vertical_acceleration_mps2 * dt_seconds) + { + next_position = self.goal; + next_velocity = Vector3::zero(); + } + + let next_reference = KinematicReference { + position: next_position, + velocity: next_velocity, + acceleration: next_velocity + .sub(self.reference.velocity) + .scale(1.0 / dt_seconds), + }; + if !next_reference.acceleration.is_finite() + | (Vector3::::new( + next_reference.velocity.x(), + next_reference.velocity.y(), + 0.0, + ) + .norm_squared() + > self.limits.maximum_horizontal_speed_mps + * self.limits.maximum_horizontal_speed_mps + + 1.0e-4) + | (next_reference.velocity.z().abs() > self.limits.maximum_vertical_speed_mps + 1.0e-4) + { + return Err(TrajectoryError::NumericalOverflow); + } + self.reference = next_reference; + Ok(self.reference) + } + + /// Returns whether this trajectory has reached its destination and stopped. + pub fn is_complete(&self, position_tolerance_metres: f32, velocity_tolerance_mps: f32) -> bool { + position_tolerance_metres.is_finite() + & (position_tolerance_metres >= 0.0) + & velocity_tolerance_mps.is_finite() + & (velocity_tolerance_mps >= 0.0) + & (self.goal.sub(self.reference.position).norm_squared() + <= position_tolerance_metres * position_tolerance_metres) + & (self.reference.velocity.norm_squared() + <= velocity_tolerance_mps * velocity_tolerance_mps) + } +} + +fn valid_limits(limits: KinematicLimits) -> bool { + limits.maximum_horizontal_speed_mps.is_finite() + & (limits.maximum_horizontal_speed_mps > 0.0) + & limits.maximum_vertical_speed_mps.is_finite() + & (limits.maximum_vertical_speed_mps > 0.0) + & limits.maximum_horizontal_acceleration_mps2.is_finite() + & (limits.maximum_horizontal_acceleration_mps2 > 0.0) + & limits.maximum_vertical_acceleration_mps2.is_finite() + & (limits.maximum_vertical_acceleration_mps2 > 0.0) +} + +fn braking_velocity( + error: Vector3, + maximum_speed: f32, + maximum_acceleration: f32, +) -> Option> { + let distance = error.norm(); + if !distance.is_finite() || distance <= 1.0e-5 { + return distance.is_finite().then_some(Vector3::zero()); + } + let stopping_speed = sqrt(2.0 * maximum_acceleration * distance); + if !stopping_speed.is_finite() { + return None; + } + let velocity = error.scale(maximum_speed.min(stopping_speed) / distance); + velocity.is_finite().then_some(velocity) +} + +fn braking_velocity_axis(error: f32, maximum_speed: f32, maximum_acceleration: f32) -> Option { + if error.abs() <= 1.0e-5 { + error.is_finite().then_some(0.0) + } else { + let stopping_speed = sqrt(2.0 * maximum_acceleration * error.abs()); + if stopping_speed.is_finite() { + Some(error.signum() * maximum_speed.min(stopping_speed)) + } else { + None + } + } +} + +fn limit_magnitude(vector: Vector3, maximum: f32) -> Option> { + if !vector.is_finite() | !maximum.is_finite() | (maximum < 0.0) { + return None; + } + let scale = vector.x().abs().max(vector.y().abs()).max(vector.z().abs()); + if scale == 0.0 { + return Some(vector); + } + let scaled = vector.scale(1.0 / scale); + let magnitude = scale * sqrt(scaled.norm_squared()); + if !magnitude.is_finite() || magnitude > maximum { + return vector + .normalized() + .map(|direction| direction.scale(maximum)); + } + Some(vector) +} + +/// Estimate consumed by the translational controller. +/// +/// This is intentionally a value type. The controller does not accept a +/// simulator object or an aiding measurement; callers pass the estimator's +/// current state after its integrity policy has accepted it. +#[derive(Clone, Copy, Debug, PartialEq)] +pub struct TranslationalEstimate { + /// Estimated local NED position in metres. + pub position: Vector3, + /// Estimated local NED velocity in metres per second. + pub velocity: Vector3, +} + +/// Static limits and gains for the position-to-acceleration loop. +/// +/// TODO(configuration): this is supplied by a reviewed build-time vehicle +/// profile until persistent ground configuration exists. Gains and all limits +/// must be staged and validated as one controller revision, then activated +/// only while disarmed with controller state reset; field-by-field live tuning +/// is not a safe update protocol. +#[derive(Clone, Copy, Debug, PartialEq)] +pub struct TranslationalControlConfig { + /// Position-error gain in s⁻². + pub position_gain_per_second_squared: f32, + /// Velocity-error gain in s⁻¹. + pub velocity_gain_per_second: f32, + /// Maximum horizontal commanded acceleration in m/s². + pub maximum_horizontal_acceleration_mps2: f32, + /// Maximum upward commanded acceleration in m/s² (negative NED down). + pub maximum_upward_acceleration_mps2: f32, + /// Maximum downward commanded acceleration in m/s² (positive NED down). + pub maximum_downward_acceleration_mps2: f32, +} + +/// Position-control configuration or input rejection. +#[derive(Clone, Copy, Debug, Eq, PartialEq)] +pub enum TranslationalControlError { + /// A gain or kinematic limit is invalid. + InvalidConfiguration, + /// A reference or estimator value is non-finite. + NonFiniteState, +} + +/// Stateless outer loop for a cascaded multicopter controller. +/// +/// Integral action is deliberately absent here. Position aiding can disappear +/// or be rejected by the estimator; retaining an unobservable position +/// integral across that transition would be unsafe. Any future integral must +/// have an explicit estimator-validity reset and allocator-feedback policy. +pub struct TranslationalController { + config: TranslationalControlConfig, +} + +impl TranslationalController { + /// Validates one vehicle's static outer-loop configuration. + pub fn new(config: TranslationalControlConfig) -> Result { + if !config.position_gain_per_second_squared.is_finite() + || config.position_gain_per_second_squared < 0.0 + || !config.velocity_gain_per_second.is_finite() + || config.velocity_gain_per_second < 0.0 + || !config.maximum_horizontal_acceleration_mps2.is_finite() + || config.maximum_horizontal_acceleration_mps2 <= 0.0 + || !config.maximum_upward_acceleration_mps2.is_finite() + || config.maximum_upward_acceleration_mps2 <= 0.0 + || !config.maximum_downward_acceleration_mps2.is_finite() + || config.maximum_downward_acceleration_mps2 <= 0.0 + { + return Err(TranslationalControlError::InvalidConfiguration); + } + Ok(Self { config }) + } + + /// Produces a bounded local-NED acceleration request. + pub fn update( + &self, + reference: KinematicReference, + estimate: TranslationalEstimate, + ) -> Result, TranslationalControlError> { + if !reference.position.is_finite() + || !reference.velocity.is_finite() + || !reference.acceleration.is_finite() + || !estimate.position.is_finite() + || !estimate.velocity.is_finite() + { + return Err(TranslationalControlError::NonFiniteState); + } + let position_error = reference.position.sub(estimate.position); + let velocity_error = reference.velocity.sub(estimate.velocity); + let raw = reference + .acceleration + .add(position_error.scale(self.config.position_gain_per_second_squared)) + .add(velocity_error.scale(self.config.velocity_gain_per_second)); + if !raw.is_finite() { + return Err(TranslationalControlError::NonFiniteState); + } + let horizontal = limit_magnitude( + Vector3::new(raw.x(), raw.y(), 0.0), + self.config.maximum_horizontal_acceleration_mps2, + ) + .ok_or(TranslationalControlError::NonFiniteState)?; + Ok(Vector3::new( + horizontal.x(), + horizontal.y(), + clamp( + raw.z(), + -self.config.maximum_upward_acceleration_mps2, + self.config.maximum_downward_acceleration_mps2, + ), + )) + } +} + +/// Physical conversion and attitude-envelope limits for one multirotor. +/// +/// TODO(configuration): mass, thrust capability, and tilt limit are a single +/// vehicle-identification/calibration record. The future configuration path +/// must atomically bind this record to the matching mixer and reject output +/// until it is reviewed for the installed airframe. +#[derive(Clone, Copy, Debug, PartialEq)] +pub struct ThrustEnvelope { + /// Vehicle mass in kilograms. + pub mass_kg: f32, + /// Maximum thrust available from all actuators in newtons. + pub maximum_collective_thrust_newtons: f32, + /// Maximum horizontal-to-vertical thrust ratio. For a 35° tilt limit this + /// is `tan(35°)`, calculated and reviewed with the vehicle configuration. + pub maximum_tilt_tangent: f32, +} + +/// Thrust-envelope rejection. +#[derive(Clone, Copy, Debug, Eq, PartialEq)] +pub enum ThrustEnvelopeError { + /// A static physical parameter is invalid. + InvalidConfiguration, + /// A requested acceleration is non-finite or cannot be produced by + /// positive multicopter thrust. + InvalidAcceleration, +} + +impl ThrustEnvelope { + /// Validates a vehicle's reviewed force limits. + pub fn validate(self) -> Result<(), ThrustEnvelopeError> { + if !self.mass_kg.is_finite() + || self.mass_kg <= 0.0 + || !self.maximum_collective_thrust_newtons.is_finite() + || self.maximum_collective_thrust_newtons <= 0.0 + || !self.maximum_tilt_tangent.is_finite() + || self.maximum_tilt_tangent <= 0.0 + { + Err(ThrustEnvelopeError::InvalidConfiguration) + } else { + Ok(()) + } + } + + /// Converts a bounded NED acceleration request to a feasible thrust force. + /// + /// The returned force is always upward or level in NED. Horizontal force + /// is reduced first for tilt, then the full vector for collective thrust. + /// A caller can expose the reduction as saturation and reset its rate-loop + /// integrators before commanding the mixer. + pub fn force_for_acceleration( + self, + acceleration: Vector3, + gravity_mps2: f32, + ) -> Result<(NedForce, bool), ThrustEnvelopeError> { + self.validate()?; + if !acceleration.is_finite() || !gravity_mps2.is_finite() || gravity_mps2 <= 0.0 { + return Err(ThrustEnvelopeError::InvalidAcceleration); + } + let vertical_force = self.mass_kg * (acceleration.z() - gravity_mps2); + if !vertical_force.is_finite() || vertical_force >= 0.0 { + return Err(ThrustEnvelopeError::InvalidAcceleration); + } + let requested_horizontal = Vector3::new( + self.mass_kg * acceleration.x(), + self.mass_kg * acceleration.y(), + 0.0, + ); + if !vertical_force.is_finite() | !requested_horizontal.is_finite() { + return Err(ThrustEnvelopeError::InvalidAcceleration); + } + let maximum_horizontal = -vertical_force * self.maximum_tilt_tangent; + if !maximum_horizontal.is_finite() | (maximum_horizontal <= 0.0) { + return Err(ThrustEnvelopeError::InvalidAcceleration); + } + let limited_horizontal = limit_magnitude(requested_horizontal, maximum_horizontal) + .ok_or(ThrustEnvelopeError::InvalidAcceleration)?; + let tilt_limited = limited_horizontal != requested_horizontal; + let requested = Vector3::new( + limited_horizontal.x(), + limited_horizontal.y(), + vertical_force, + ); + let magnitude = requested.norm(); + let limited = limit_magnitude(requested, self.maximum_collective_thrust_newtons) + .ok_or(ThrustEnvelopeError::InvalidAcceleration)?; + // `limit_magnitude` returns `Some` only for a finite input vector and + // finite bound, so a returned vector is finite by construction. + if !magnitude.is_finite() { + return Err(ThrustEnvelopeError::InvalidAcceleration); + } + Ok(( + NedForce::newtons(limited.x(), limited.y(), limited.z()), + tilt_limited || magnitude > self.maximum_collective_thrust_newtons, + )) + } +} + +/// Runtime command limits for one statically ordered actuator. +/// +/// Physical positions, thrust axes, and reaction coefficients belong to the +/// vehicle's reviewed offline allocation calculation. Keeping a second copy +/// here would create an authority that the rate-path allocator cannot use. +#[derive(Clone, Copy, Debug, PartialEq)] +pub struct ActuatorLimits { + /// Inclusive lower command bound. + pub minimum: f32, + /// Inclusive upper command bound. + pub maximum: f32, + /// Largest command movement per second. + pub slew_per_second: f32, +} + +/// One reviewed allocation output. Coefficients are ordered as roll torque, +/// pitch torque, yaw torque, then collective thrust. +#[derive(Clone, Copy, Debug, PartialEq)] +pub struct MixerOutput { + /// Precomputed allocation row produced from the vehicle geometry offline. + pub coefficients: [f32; 4], + /// Physical command constraints for this same actuator. + pub limits: ActuatorLimits, +} + +/// Reviewed, precomputed allocation map in static actuator order. +/// +/// Pairing each row with its command limits prevents independently indexed +/// geometry and matrix tables from drifting out of correspondence. +/// +/// TODO(configuration): persist and activate this only as an entire, +/// vehicle-bound allocation profile, with actuator output inhibited and +/// allocation history reset. Individual mixer-row editing would break the +/// static-order and physical-review guarantees of this type. +#[derive(Clone, Copy, Debug, PartialEq)] +pub struct PrecomputedMixer { + outputs: [MixerOutput; ACTUATORS], +} +impl PrecomputedMixer { + /// Creates a reviewed static allocation map. + pub const fn new(outputs: [MixerOutput; ACTUATORS]) -> Self { + Self { outputs } + } +} + +/// Requested body wrench in vehicle-specific normalized allocation units. +/// Units remain explicit in the vehicle's reviewed mixer calibration; generic +/// allocation never pretends torque and thrust share a physical unit. +#[derive(Clone, Copy, Debug, PartialEq)] +pub struct BodyWrench { + /// Roll torque request. + pub roll: f32, + /// Pitch torque request. + pub pitch: f32, + /// Yaw torque request. + pub yaw: f32, + /// Collective thrust request. + pub collective: f32, +} + +/// Allocation construction failure. +#[derive(Clone, Copy, Debug, Eq, PartialEq)] +pub enum AllocatorError { + /// One static allocation output has non-finite or contradictory limits. + InvalidActuator, + /// Mixer contains non-finite values. + InvalidMixer, + /// Time step was non-finite, non-positive, or exceeded the 100-ms integration bound. + InvalidTime, + /// Requested wrench contains a non-finite component. + InvalidWrench, + /// The requested actuator index is outside the static allocation map. + UnknownActuator, +} + +/// Allocation output in the static actuator order. +#[derive(Clone, Copy, Debug, PartialEq)] +pub struct AllocationResult { + /// Command for each statically ordered actuator. + pub commands: [f32; ACTUATORS], + /// At least one command was limited by range, reversal, slew, or failure. + pub saturated: bool, +} + +/// Fixed-map actuator allocator with no dynamic inversion. +pub struct Allocator { + mixer: PrecomputedMixer, + previous: [f32; ACTUATORS], + failed: [bool; ACTUATORS], +} + +impl Allocator { + /// Validates the reviewed static allocation map. + pub fn new(mixer: PrecomputedMixer) -> Result { + validate_mixer_outputs(&mixer.outputs)?; + let mut previous = [0.0; ACTUATORS]; + for (previous, output) in previous.iter_mut().zip(mixer.outputs) { + *previous = output.limits.minimum; + } + Ok(Self { + mixer, + previous, + failed: [false; ACTUATORS], + }) + } + + /// Marks one actuator failed. A failed actuator is forced to its reviewed + /// minimum command on the next allocation; it cannot silently disappear + /// from the mix or retain thrust after a declared fault. + pub fn set_failed(&mut self, index: usize, failed: bool) -> Result<(), AllocatorError> { + if index >= ACTUATORS { + return Err(AllocatorError::UnknownActuator); + } + self.failed[index] = failed; + Ok(()) + } + + /// Resets command history to the reviewed minimum command without clearing + /// any actuator-failure latch. Call on disarm, failsafe, mode changes, or + /// estimator reset before outputs can be authorized again. + pub fn reset_commands(&mut self) { + for (previous, output) in self.previous.iter_mut().zip(self.mixer.outputs) { + *previous = output.limits.minimum; + } + } + + /// Performs fixed-cost mix, limits, and slew control in static motor order. + pub fn allocate( + &mut self, + wrench: BodyWrench, + dt_seconds: f32, + ) -> Result, AllocatorError> { + if !valid_control_step(dt_seconds) { + return Err(AllocatorError::InvalidTime); + } + let requested = [wrench.roll, wrench.pitch, wrench.yaw, wrench.collective]; + if !wrench_components_are_finite(wrench) { + return Err(AllocatorError::InvalidWrench); + } + let mut commands = self.previous; + let mut saturated = false; + for (index, output) in commands.iter_mut().enumerate() { + let mut command = 0.0; + let output_config = self.mixer.outputs[index]; + for (coefficient, request) in output_config.coefficients.iter().zip(requested) { + command += coefficient * request; + } + if !command.is_finite() { + return Err(AllocatorError::InvalidWrench); + } + if self.failed[index] { + command = output_config.limits.minimum; + saturated = true; + } else { + let bounded = clamp( + command, + output_config.limits.minimum, + output_config.limits.maximum, + ); + saturated |= bounded != command; + command = bounded; + let maximum_delta = output_config.limits.slew_per_second * dt_seconds; + // Both factors are finite and bounded: validated control time + // is at most 0.1 s, so a finite reviewed slew limit cannot + // overflow this product. + debug_assert!(maximum_delta.is_finite()); + let slewed = clamp( + command, + self.previous[index] - maximum_delta, + self.previous[index] + maximum_delta, + ); + saturated |= slewed != command; + command = slewed; + } + *output = command; + } + self.previous = commands; + Ok(AllocationResult { + commands, + saturated, + }) + } +} + +fn validate_mixer_outputs(outputs: &[MixerOutput]) -> Result<(), AllocatorError> { + if outputs.is_empty() { + return Err(AllocatorError::InvalidMixer); + } + for output in outputs { + let limits = output.limits; + if !limits.minimum.is_finite() + | !limits.maximum.is_finite() + | (limits.minimum > limits.maximum) + | !limits.slew_per_second.is_finite() + | (limits.slew_per_second < 0.0) + { + return Err(AllocatorError::InvalidActuator); + } + if !mixer_coefficients_are_finite(output.coefficients) { + return Err(AllocatorError::InvalidMixer); + } + } + Ok(()) +} + +fn mixer_coefficients_are_finite(coefficients: [f32; 4]) -> bool { + coefficients[0].is_finite() + & coefficients[1].is_finite() + & coefficients[2].is_finite() + & coefficients[3].is_finite() +} + +fn valid_control_step(dt_seconds: f32) -> bool { + dt_seconds.is_finite() & (dt_seconds > 0.0) & (dt_seconds <= MAXIMUM_CONTROL_STEP_SECONDS) +} + +fn wrench_components_are_finite(wrench: BodyWrench) -> bool { + wrench.roll.is_finite() + & wrench.pitch.is_finite() + & wrench.yaw.is_finite() + & wrench.collective.is_finite() +} + +const fn clamp(value: f32, low: f32, high: f32) -> f32 { + if value < low { + low + } else if value > high { + high + } else { + value + } +} + +#[cfg(test)] +mod tests { + use super::*; + + fn trajectory_limits() -> KinematicLimits { + KinematicLimits { + maximum_horizontal_speed_mps: 4.0, + maximum_vertical_speed_mps: 2.0, + maximum_horizontal_acceleration_mps2: 3.0, + maximum_vertical_acceleration_mps2: 1.5, + } + } + + #[test] + fn distant_goal_stays_bounded() { + let limits = trajectory_limits(); + let mut trajectory = Trajectory::new(Vector3::zero(), limits).unwrap(); + trajectory + .set_goal(Vector3::new(150.0, 200.0, -15.0)) + .unwrap(); + + for _ in 0..10_000 { + let reference = trajectory.update(0.01).unwrap(); + let horizontal_speed = + Vector3::::new(reference.velocity.x(), reference.velocity.y(), 0.0).norm(); + let horizontal_acceleration = + Vector3::::new(reference.acceleration.x(), reference.acceleration.y(), 0.0) + .norm(); + assert!(horizontal_speed <= limits.maximum_horizontal_speed_mps + 1.0e-4); + assert!( + horizontal_acceleration <= limits.maximum_horizontal_acceleration_mps2 + 1.0e-3 + ); + assert!(reference.velocity.z().abs() <= limits.maximum_vertical_speed_mps + 1.0e-4); + assert!( + reference.acceleration.z().abs() + <= limits.maximum_vertical_acceleration_mps2 + 1.0e-3 + ); + if trajectory.is_complete(1.0e-4, 1.0e-4) { + break; + } + } + + assert!(trajectory.is_complete(1.0e-3, 1.0e-3)); + let final_error = trajectory + .reference() + .position + .sub(Vector3::new(150.0, 200.0, -15.0)); + assert!(final_error.x().abs() < 1.0e-3); + assert!(final_error.y().abs() < 1.0e-3); + assert!(final_error.z().abs() < 1.0e-3); + } + + #[test] + fn trajectory_validation() { + let mut bad_limits = trajectory_limits(); + bad_limits.maximum_horizontal_speed_mps = 0.0; + assert!(matches!( + Trajectory::new(Vector3::zero(), bad_limits), + Err(TrajectoryError::InvalidLimits) + )); + + let mut trajectory = Trajectory::new(Vector3::zero(), trajectory_limits()).unwrap(); + assert_eq!( + trajectory.set_goal(Vector3::new(f32::NAN, 0.0, 0.0)), + Err(TrajectoryError::NonFiniteGoal) + ); + assert_eq!(trajectory.update(0.0), Err(TrajectoryError::InvalidTime)); + assert_eq!(trajectory.update(0.2), Err(TrajectoryError::InvalidTime)); + + trajectory + .set_goal(Vector3::new(f32::MAX, 0.0, 0.0)) + .unwrap(); + let before = trajectory.reference(); + assert_eq!( + trajectory.update(0.01), + Err(TrajectoryError::NumericalOverflow) + ); + assert_eq!(trajectory.reference(), before); + } + + #[test] + fn outer_loop_respects_limits() { + let controller = TranslationalController::new(TranslationalControlConfig { + position_gain_per_second_squared: 1.0, + velocity_gain_per_second: 1.0, + maximum_horizontal_acceleration_mps2: 4.0, + maximum_upward_acceleration_mps2: 3.0, + maximum_downward_acceleration_mps2: 2.0, + }) + .unwrap(); + let acceleration = controller + .update( + KinematicReference { + position: Vector3::new(100.0, 100.0, -10.0), + velocity: Vector3::zero(), + acceleration: Vector3::zero(), + }, + TranslationalEstimate { + position: Vector3::zero(), + velocity: Vector3::zero(), + }, + ) + .unwrap(); + assert!(Vector3::::new(acceleration.x(), acceleration.y(), 0.0).norm() <= 4.0001); + assert!((acceleration.z() + 3.0).abs() < 1.0e-6); + + let envelope = ThrustEnvelope { + mass_kg: 1.2, + maximum_collective_thrust_newtons: 18.0, + maximum_tilt_tangent: 0.1, + }; + let (force, saturated) = envelope + .force_for_acceleration(acceleration, 9.806_65) + .unwrap(); + let vector = force.vector(); + let horizontal = Vector3::::new(vector.x(), vector.y(), 0.0).norm(); + assert!(horizontal <= -vector.z() * 0.1001); + assert!(vector.norm() <= 18.0001); + assert!(saturated); + } + + #[test] + fn feed_forward_stays_finite() { + let controller = TranslationalController::new(TranslationalControlConfig { + position_gain_per_second_squared: 0.0, + velocity_gain_per_second: 0.0, + maximum_horizontal_acceleration_mps2: 4.0, + maximum_upward_acceleration_mps2: 3.0, + maximum_downward_acceleration_mps2: 2.0, + }) + .unwrap(); + let acceleration = controller + .update( + KinematicReference { + position: Vector3::zero(), + velocity: Vector3::zero(), + acceleration: Vector3::new(f32::MAX, -f32::MAX, f32::MAX), + }, + TranslationalEstimate { + position: Vector3::zero(), + velocity: Vector3::zero(), + }, + ) + .unwrap(); + assert!(acceleration.is_finite()); + assert!(Vector3::::new(acceleration.x(), acceleration.y(), 0.0).norm() <= 4.0001); + assert_eq!(acceleration.z(), 2.0); + + let envelope = ThrustEnvelope { + mass_kg: 1.0, + maximum_collective_thrust_newtons: 20.0, + maximum_tilt_tangent: 0.7, + }; + let (force, _) = envelope + .force_for_acceleration(Vector3::new(f32::MAX, 0.0, 0.0), 9.806_65) + .unwrap(); + assert!(force.vector().is_finite()); + assert!(force.vector().norm() <= 20.0001); + } + + #[test] + fn pid_anti_windup() { + let mut pid = Pid::new(PidConfig { + proportional: 2.0, + integral: 3.0, + derivative: 0.0, + output_limit: 1.0, + integral_limit: 0.5, + }); + for _ in 0..100 { + assert_eq!(pid.update(1.0, 0.0, 0.01).unwrap(), 1.0); + } + assert!(pid.integral_contribution() <= 0.5); + assert!(pid.update(-1.0, 0.0, 0.01).unwrap() < 0.0); + } + + #[test] + fn pid_overflow_preserves_state() { + let mut pid = Pid::new(PidConfig { + proportional: f32::MAX, + integral: 1.0, + derivative: 0.0, + output_limit: 1.0, + integral_limit: 0.5, + }); + assert_eq!(pid.update(2.0, 0.0, 0.01), Err(PidError::NonFinite)); + assert_eq!(pid.integral_contribution(), 0.0); + } + + #[test] + fn rate_pid_uses_history() { + let config = PidConfig { + proportional: 0.0, + integral: 0.0, + derivative: 1.0, + output_limit: 20.0, + integral_limit: 0.0, + }; + let mut pid = RatePid::new(config, config, config); + assert_eq!( + pid.update( + BodyRate::radians_per_second(0.0, 0.0, 0.0), + BodyRate::radians_per_second(0.0, 0.0, 0.0), + 0.1, + ) + .unwrap() + .x(), + 0.0 + ); + assert_eq!( + pid.update( + BodyRate::radians_per_second(1.0, 0.0, 0.0), + BodyRate::radians_per_second(0.0, 0.0, 0.0), + 0.1, + ) + .unwrap() + .x(), + 10.0 + ); + } + + #[test] + fn deadline_rejection_preserves_state() { + let config = PidConfig { + proportional: 0.0, + integral: 1.0, + derivative: 0.0, + output_limit: 2.0, + integral_limit: 1.0, + }; + let mut pid = Pid::new(config); + pid.update(1.0, 0.0, 0.1).unwrap(); + let pid_before = pid.integral_contribution(); + assert_eq!(pid.update(1.0, 0.0, 0.101), Err(PidError::InvalidTime)); + assert_eq!(pid.integral_contribution(), pid_before); + + let mut rate = RatePid::new(config, config, config); + rate.update( + BodyRate::radians_per_second(1.0, -1.0, 0.5), + BodyRate::radians_per_second(0.0, 0.0, 0.0), + 0.1, + ) + .unwrap(); + let rate_before = ( + rate.roll.integral_contribution(), + rate.pitch.integral_contribution(), + rate.yaw.integral_contribution(), + rate.previous_error, + ); + assert_eq!( + rate.update( + BodyRate::radians_per_second(0.0, 0.0, 0.0), + BodyRate::radians_per_second(0.0, 0.0, 0.0), + 0.101, + ), + Err(PidError::InvalidTime) + ); + assert_eq!( + ( + rate.roll.integral_contribution(), + rate.pitch.integral_contribution(), + rate.yaw.integral_contribution(), + rate.previous_error, + ), + rate_before + ); + + let mut trajectory = Trajectory::new(Vector3::zero(), trajectory_limits()).unwrap(); + trajectory.set_goal(Vector3::new(1.0, 0.0, 0.0)).unwrap(); + trajectory.update(0.1).unwrap(); + let trajectory_before = trajectory.reference(); + assert_eq!(trajectory.update(0.101), Err(TrajectoryError::InvalidTime)); + assert_eq!(trajectory.reference(), trajectory_before); + + let mut allocator = + Allocator::new(PrecomputedMixer::new([output([0.0, 0.0, 0.0, 1.0])])).unwrap(); + allocator + .allocate( + BodyWrench { + roll: 0.0, + pitch: 0.0, + yaw: 0.0, + collective: 0.5, + }, + 0.1, + ) + .unwrap(); + let allocator_before = allocator.previous; + assert_eq!( + allocator.allocate( + BodyWrench { + roll: 0.0, + pitch: 0.0, + yaw: 0.0, + collective: 1.0, + }, + 0.101, + ), + Err(AllocatorError::InvalidTime) + ); + assert_eq!(allocator.previous, allocator_before); + } + + #[test] + fn rate_pid_failure_is_atomic() { + let finite = PidConfig { + proportional: 0.0, + integral: 1.0, + derivative: 0.0, + output_limit: 2.0, + integral_limit: 1.0, + }; + let invalid_yaw = PidConfig { + proportional: f32::MAX, + ..finite + }; + let mut rate = RatePid::new(finite, finite, invalid_yaw); + assert_eq!( + rate.update( + BodyRate::radians_per_second(1.0, 1.0, 2.0), + BodyRate::radians_per_second(0.0, 0.0, 0.0), + 0.1, + ), + Err(PidError::NonFinite) + ); + assert_eq!(rate.roll.integral_contribution(), 0.0); + assert_eq!(rate.pitch.integral_contribution(), 0.0); + assert_eq!(rate.yaw.integral_contribution(), 0.0); + assert!(rate.previous_error.is_none()); + } + + #[test] + fn level_heading() { + let attitude = force_to_attitude( + NedForce::newtons(0.0, 0.0, -10.0), + HeadingNed::new(1.0, 0.0).unwrap(), + ) + .unwrap(); + let forward = attitude.body_to_ned(Vector3::::new(1.0, 0.0, 0.0)); + let down = attitude.body_to_ned(Vector3::::new(0.0, 0.0, 1.0)); + assert!(forward.x() > 0.999); + assert!(down.z() > 0.999); + } + + fn output(coefficients: [f32; 4]) -> MixerOutput { + MixerOutput { + coefficients, + limits: ActuatorLimits { + minimum: 0.0, + maximum: 1.0, + slew_per_second: 100.0, + }, + } + } + + #[test] + fn allocator_order_and_saturation() { + let mixer = + PrecomputedMixer::new([output([0.0, 0.0, 0.0, 1.0]), output([1.0, 0.0, 0.0, 1.0])]); + let mut allocator = Allocator::new(mixer).unwrap(); + let output = allocator + .allocate( + BodyWrench { + roll: 0.5, + pitch: 0.0, + yaw: 0.0, + collective: 0.8, + }, + 0.1, + ) + .unwrap(); + assert_eq!(output.commands, [0.8, 1.0]); + assert!(output.saturated); + allocator.set_failed(1, true).unwrap(); + assert_eq!( + allocator + .allocate( + BodyWrench { + roll: 0.0, + pitch: 0.0, + yaw: 0.0, + collective: 0.8 + }, + 0.1 + ) + .unwrap() + .commands[1], + 0.0 + ); + } + + #[test] + fn allocator_fault_latch() { + let limits = ActuatorLimits { + minimum: 0.0, + maximum: 1.0, + slew_per_second: 100.0, + }; + let mixer = PrecomputedMixer::new([ + MixerOutput { + coefficients: [f32::MAX, 0.0, 0.0, 0.0], + limits, + }, + MixerOutput { + coefficients: [0.0, 0.0, 0.0, 1.0], + limits, + }, + ]); + let mut allocator = Allocator::new(mixer).unwrap(); + assert_eq!( + allocator.allocate( + BodyWrench { + roll: f32::MAX, + pitch: 0.0, + yaw: 0.0, + collective: 0.0, + }, + 0.01, + ), + Err(AllocatorError::InvalidWrench) + ); + allocator.set_failed(1, true).unwrap(); + allocator.reset_commands(); + let output = allocator + .allocate( + BodyWrench { + roll: 0.0, + pitch: 0.0, + yaw: 0.0, + collective: 0.5, + }, + 0.01, + ) + .unwrap(); + assert_eq!(output.commands, [0.0, 0.0]); + assert!(output.saturated); + } + + #[test] + fn replaced_goals_stay_bounded() { + let limits = trajectory_limits(); + let mut trajectory = Trajectory::new(Vector3::zero(), limits).unwrap(); + let mut seed = 0x1234_5678_u32; + for step in 0..2_000 { + seed = seed.wrapping_mul(1_664_525).wrapping_add(1_013_904_223); + if step % 17 == 0 { + let north = (seed as i32 as f32) / i32::MAX as f32 * 500.0; + seed = seed.wrapping_mul(1_664_525).wrapping_add(1_013_904_223); + let east = (seed as i32 as f32) / i32::MAX as f32 * 500.0; + seed = seed.wrapping_mul(1_664_525).wrapping_add(1_013_904_223); + let down = (seed as i32 as f32) / i32::MAX as f32 * 30.0; + trajectory + .set_goal(Vector3::new(north, east, down)) + .unwrap(); + } + let reference = trajectory.update(0.01).unwrap(); + let horizontal_speed = + Vector3::::new(reference.velocity.x(), reference.velocity.y(), 0.0).norm(); + let horizontal_acceleration = + Vector3::::new(reference.acceleration.x(), reference.acceleration.y(), 0.0) + .norm(); + assert!(reference.position.is_finite()); + assert!(horizontal_speed <= limits.maximum_horizontal_speed_mps + 1.0e-4); + assert!( + horizontal_acceleration <= limits.maximum_horizontal_acceleration_mps2 + 1.0e-3 + ); + assert!(reference.velocity.z().abs() <= limits.maximum_vertical_speed_mps + 1.0e-4); + assert!( + reference.acceleration.z().abs() + <= limits.maximum_vertical_acceleration_mps2 + 1.0e-3 + ); + } + } + + #[test] + fn pid_rejects_invalid_input() { + let valid = PidConfig { + proportional: 1.0, + integral: 1.0, + derivative: 1.0, + output_limit: 2.0, + integral_limit: 1.0, + }; + let invalid = [ + PidConfig { + proportional: f32::NAN, + ..valid + }, + PidConfig { + integral: f32::NAN, + ..valid + }, + PidConfig { + derivative: f32::NAN, + ..valid + }, + PidConfig { + output_limit: 0.0, + ..valid + }, + PidConfig { + output_limit: f32::NAN, + ..valid + }, + PidConfig { + integral_limit: -1.0, + ..valid + }, + PidConfig { + integral_limit: f32::NAN, + ..valid + }, + ]; + for config in invalid { + assert_eq!( + Pid::new(config).update(0.0, 0.0, 0.01), + Err(PidError::InvalidConfiguration) + ); + } + let mut pid = Pid::new(valid); + assert_eq!(pid.update(0.0, 0.0, 0.0), Err(PidError::InvalidTime)); + assert_eq!(pid.update(f32::NAN, 0.0, 0.01), Err(PidError::NonFinite)); + assert_eq!(pid.update(0.0, f32::NAN, 0.01), Err(PidError::NonFinite)); + pid.integral_contribution = f32::MAX; + pid.config.integral = f32::MAX; + assert_eq!(pid.update(1.0, 0.0, 0.1), Err(PidError::NonFinite)); + pid.reset(); + assert_eq!(pid.integral_contribution(), 0.0); + let mut opposite_sign_recovery = Pid::new(PidConfig { + proportional: 0.0, + integral: 0.0, + derivative: 0.0, + output_limit: 0.5, + integral_limit: 1.0, + }); + opposite_sign_recovery.integral_contribution = 1.0; + assert_eq!(opposite_sign_recovery.update(-1.0, 0.0, 0.1), Ok(0.5)); + + let mut rate = RatePid::new(valid, valid, valid); + assert_eq!( + rate.update( + BodyRate::radians_per_second(0.0, 0.0, 0.0), + BodyRate::radians_per_second(0.0, 0.0, 0.0), + f32::NAN, + ), + Err(PidError::InvalidTime) + ); + rate.update( + BodyRate::radians_per_second(1.0, 0.0, 0.0), + BodyRate::radians_per_second(0.0, 0.0, 0.0), + 0.1, + ) + .unwrap(); + rate.reset_integrators(); + assert_eq!(rate.roll.integral_contribution(), 0.0); + rate.reset(); + assert!(rate.previous_error.is_none()); + } + + #[test] + fn trajectory_edges() { + assert!(matches!( + Trajectory::new(Vector3::new(f32::NAN, 0.0, 0.0), trajectory_limits()), + Err(TrajectoryError::NonFiniteGoal) + )); + let mut limits = trajectory_limits(); + limits.maximum_vertical_speed_mps = f32::NAN; + assert!(matches!( + Trajectory::new(Vector3::zero(), limits), + Err(TrajectoryError::InvalidLimits) + )); + for limits in [ + KinematicLimits { + maximum_horizontal_speed_mps: f32::NAN, + ..trajectory_limits() + }, + KinematicLimits { + maximum_vertical_speed_mps: 0.0, + ..trajectory_limits() + }, + KinematicLimits { + maximum_horizontal_acceleration_mps2: f32::NAN, + ..trajectory_limits() + }, + KinematicLimits { + maximum_vertical_acceleration_mps2: 0.0, + ..trajectory_limits() + }, + KinematicLimits { + maximum_vertical_acceleration_mps2: f32::NAN, + ..trajectory_limits() + }, + ] { + assert!(matches!( + Trajectory::new(Vector3::zero(), limits), + Err(TrajectoryError::InvalidLimits) + )); + } + + let mut trajectory = Trajectory::new(Vector3::zero(), trajectory_limits()).unwrap(); + assert_eq!(trajectory.goal(), Vector3::zero()); + assert!(!trajectory.is_complete(f32::NAN, 0.0)); + assert!(!trajectory.is_complete(0.0, -1.0)); + assert!(!trajectory.is_complete(0.0, f32::NAN)); + assert!(trajectory.is_complete(0.0, 0.0)); + assert_eq!(trajectory.update(0.01).unwrap().position, Vector3::zero()); + + assert_eq!( + braking_velocity(Vector3::zero(), 1.0, 1.0), + Some(Vector3::zero()) + ); + assert_eq!( + braking_velocity(Vector3::new(f32::NAN, 0.0, 0.0), 1.0, 1.0), + None + ); + assert_eq!(braking_velocity_axis(0.0, 1.0, 1.0), Some(0.0)); + assert_eq!(braking_velocity_axis(f32::NAN, 1.0, 1.0), None); + assert_eq!(limit_magnitude(Vector3::zero(), 1.0), Some(Vector3::zero())); + assert_eq!(limit_magnitude(Vector3::new(f32::NAN, 0.0, 0.0), 1.0), None); + assert_eq!(limit_magnitude(Vector3::new(1.0, 0.0, 0.0), -1.0), None); + } + + #[test] + fn outer_loop_validation() { + let config = TranslationalControlConfig { + position_gain_per_second_squared: 1.0, + velocity_gain_per_second: 1.0, + maximum_horizontal_acceleration_mps2: 4.0, + maximum_upward_acceleration_mps2: 3.0, + maximum_downward_acceleration_mps2: 2.0, + }; + for invalid in [ + TranslationalControlConfig { + position_gain_per_second_squared: -1.0, + ..config + }, + TranslationalControlConfig { + position_gain_per_second_squared: f32::NAN, + ..config + }, + TranslationalControlConfig { + velocity_gain_per_second: -1.0, + ..config + }, + TranslationalControlConfig { + velocity_gain_per_second: f32::NAN, + ..config + }, + TranslationalControlConfig { + maximum_horizontal_acceleration_mps2: 0.0, + ..config + }, + TranslationalControlConfig { + maximum_horizontal_acceleration_mps2: f32::NAN, + ..config + }, + TranslationalControlConfig { + maximum_upward_acceleration_mps2: 0.0, + ..config + }, + TranslationalControlConfig { + maximum_upward_acceleration_mps2: f32::NAN, + ..config + }, + TranslationalControlConfig { + maximum_downward_acceleration_mps2: 0.0, + ..config + }, + TranslationalControlConfig { + maximum_downward_acceleration_mps2: f32::NAN, + ..config + }, + ] { + assert!(matches!( + TranslationalController::new(invalid), + Err(TranslationalControlError::InvalidConfiguration) + )); + } + let controller = TranslationalController::new(config).unwrap(); + let invalid_reference = KinematicReference { + position: Vector3::new(f32::NAN, 0.0, 0.0), + velocity: Vector3::zero(), + acceleration: Vector3::zero(), + }; + let estimate = TranslationalEstimate { + position: Vector3::zero(), + velocity: Vector3::zero(), + }; + assert_eq!( + controller.update(invalid_reference, estimate), + Err(TranslationalControlError::NonFiniteState) + ); + for reference in [ + KinematicReference { + position: Vector3::zero(), + velocity: Vector3::new(f32::NAN, 0.0, 0.0), + acceleration: Vector3::zero(), + }, + KinematicReference { + position: Vector3::zero(), + velocity: Vector3::zero(), + acceleration: Vector3::new(f32::NAN, 0.0, 0.0), + }, + ] { + assert_eq!( + controller.update(reference, estimate), + Err(TranslationalControlError::NonFiniteState) + ); + } + for invalid_estimate in [ + TranslationalEstimate { + position: Vector3::new(f32::NAN, 0.0, 0.0), + velocity: Vector3::zero(), + }, + TranslationalEstimate { + position: Vector3::zero(), + velocity: Vector3::new(f32::NAN, 0.0, 0.0), + }, + ] { + assert_eq!( + controller.update( + KinematicReference { + position: Vector3::zero(), + velocity: Vector3::zero(), + acceleration: Vector3::zero() + }, + invalid_estimate + ), + Err(TranslationalControlError::NonFiniteState) + ); + } + let overflowing = KinematicReference { + position: Vector3::new(f32::MAX, 0.0, 0.0), + velocity: Vector3::zero(), + acceleration: Vector3::zero(), + }; + let huge_gain = TranslationalController::new(TranslationalControlConfig { + position_gain_per_second_squared: f32::MAX, + ..config + }) + .unwrap(); + assert_eq!( + huge_gain.update(overflowing, estimate), + Err(TranslationalControlError::NonFiniteState) + ); + + let envelope = ThrustEnvelope { + mass_kg: 1.0, + maximum_collective_thrust_newtons: 100.0, + maximum_tilt_tangent: 1.0, + }; + assert_eq!( + ThrustEnvelope { + mass_kg: 0.0, + ..envelope + } + .validate(), + Err(ThrustEnvelopeError::InvalidConfiguration) + ); + for invalid in [ + ThrustEnvelope { + mass_kg: f32::NAN, + ..envelope + }, + ThrustEnvelope { + maximum_collective_thrust_newtons: 0.0, + ..envelope + }, + ThrustEnvelope { + maximum_collective_thrust_newtons: f32::NAN, + ..envelope + }, + ThrustEnvelope { + maximum_tilt_tangent: 0.0, + ..envelope + }, + ThrustEnvelope { + maximum_tilt_tangent: f32::NAN, + ..envelope + }, + ] { + assert_eq!( + invalid.validate(), + Err(ThrustEnvelopeError::InvalidConfiguration) + ); + } + assert_eq!( + envelope.force_for_acceleration(Vector3::new(f32::NAN, 0.0, 0.0), 9.8), + Err(ThrustEnvelopeError::InvalidAcceleration) + ); + assert_eq!( + envelope.force_for_acceleration(Vector3::zero(), 0.0), + Err(ThrustEnvelopeError::InvalidAcceleration) + ); + assert_eq!( + envelope.force_for_acceleration(Vector3::zero(), f32::NAN), + Err(ThrustEnvelopeError::InvalidAcceleration) + ); + assert_eq!( + envelope.force_for_acceleration(Vector3::new(0.0, 0.0, 10.0), 9.8), + Err(ThrustEnvelopeError::InvalidAcceleration) + ); + assert!( + !envelope + .force_for_acceleration(Vector3::new(1.0, 0.0, 0.0), 9.8) + .unwrap() + .1 + ); + assert!( + envelope + .force_for_acceleration(Vector3::new(20.0, 0.0, 0.0), 9.8) + .unwrap() + .1 + ); + } + + #[test] + fn allocator_rejects_invalid_data() { + assert!(matches!( + Allocator::<0>::new(PrecomputedMixer::new([])), + Err(AllocatorError::InvalidMixer) + )); + let mut invalid = output([0.0; 4]); + invalid.limits.minimum = 2.0; + invalid.limits.maximum = 1.0; + assert!(matches!( + Allocator::new(PrecomputedMixer::new([invalid])), + Err(AllocatorError::InvalidActuator) + )); + for limits in [ + ActuatorLimits { + minimum: f32::NAN, + ..output([0.0; 4]).limits + }, + ActuatorLimits { + maximum: f32::NAN, + ..output([0.0; 4]).limits + }, + ActuatorLimits { + slew_per_second: f32::NAN, + ..output([0.0; 4]).limits + }, + ActuatorLimits { + slew_per_second: -1.0, + ..output([0.0; 4]).limits + }, + ] { + assert!(matches!( + Allocator::new(PrecomputedMixer::new([MixerOutput { + coefficients: [0.0; 4], + limits + }])), + Err(AllocatorError::InvalidActuator) + )); + } + assert!(matches!( + Allocator::new(PrecomputedMixer::new([output([f32::NAN, 0.0, 0.0, 0.0])])), + Err(AllocatorError::InvalidMixer) + )); + + let mut allocator = + Allocator::new(PrecomputedMixer::new([output([0.0, 0.0, 0.0, 1.0])])).unwrap(); + assert_eq!( + allocator.set_failed(1, true), + Err(AllocatorError::UnknownActuator) + ); + assert_eq!( + allocator.allocate( + BodyWrench { + roll: 0.0, + pitch: 0.0, + yaw: 0.0, + collective: 0.0 + }, + 0.0 + ), + Err(AllocatorError::InvalidTime) + ); + assert_eq!( + allocator.allocate( + BodyWrench { + roll: f32::NAN, + pitch: 0.0, + yaw: 0.0, + collective: 0.0 + }, + 0.1 + ), + Err(AllocatorError::InvalidWrench) + ); + for wrench in [ + BodyWrench { + roll: 0.0, + pitch: f32::NAN, + yaw: 0.0, + collective: 0.0, + }, + BodyWrench { + roll: 0.0, + pitch: 0.0, + yaw: f32::NAN, + collective: 0.0, + }, + BodyWrench { + roll: 0.0, + pitch: 0.0, + yaw: 0.0, + collective: f32::NAN, + }, + ] { + assert_eq!( + allocator.allocate(wrench, 0.1), + Err(AllocatorError::InvalidWrench) + ); + } + assert_eq!( + allocator.allocate( + BodyWrench { + roll: 0.0, + pitch: 0.0, + yaw: 0.0, + collective: 0.5 + }, + 2.0 + ), + Err(AllocatorError::InvalidTime) + ); + allocator.mixer.outputs[0].limits.slew_per_second = 100.0; + allocator + .allocate( + BodyWrench { + roll: 0.0, + pitch: 0.0, + yaw: 0.0, + collective: 0.5, + }, + 0.1, + ) + .unwrap(); + allocator.set_failed(0, true).unwrap(); + assert_eq!( + allocator + .allocate( + BodyWrench { + roll: 0.0, + pitch: 0.0, + yaw: 0.0, + collective: 1.0 + }, + 0.1 + ) + .unwrap() + .commands, + [0.0] + ); + } + + #[test] + fn numerical_guard_paths_reject_without_committing_state() { + let mut pid = Pid::new(PidConfig { + proportional: 0.0, + integral: f32::MAX, + derivative: 0.0, + output_limit: 1.0, + integral_limit: 1.0, + }); + assert_eq!(pid.update(2.0, 0.0, 0.1), Err(PidError::NonFinite)); + assert_eq!(pid.integral_contribution(), 0.0); + + let mut trajectory = Trajectory::new(Vector3::zero(), trajectory_limits()).unwrap(); + trajectory.goal = Vector3::new(f32::MAX, 0.0, 0.0); + trajectory.reference.position = Vector3::new(f32::MAX, 0.0, 0.0); + trajectory.reference.velocity = Vector3::new(f32::MAX, 0.0, 0.0); + assert_eq!( + trajectory.update(0.1), + Err(TrajectoryError::NumericalOverflow) + ); + + let mut bounded_but_inconsistent = + Trajectory::new(Vector3::zero(), trajectory_limits()).unwrap(); + bounded_but_inconsistent.reference.velocity = Vector3::new(5.0, 0.0, 0.0); + assert_eq!( + bounded_but_inconsistent.update(0.01), + Err(TrajectoryError::NumericalOverflow) + ); + + let envelope = ThrustEnvelope { + mass_kg: 1.0, + maximum_collective_thrust_newtons: f32::MAX, + maximum_tilt_tangent: 1.0, + }; + assert_eq!( + envelope.force_for_acceleration(Vector3::new(f32::MAX, 0.0, 0.0), f32::MAX / 2.0), + Err(ThrustEnvelopeError::InvalidAcceleration) + ); + } + + #[test] + fn numeric_faults_fail_closed() { + let valid = PidConfig { + proportional: 0.0, + integral: 0.0, + derivative: 0.0, + output_limit: f32::MAX, + integral_limit: f32::MAX, + }; + let mut derivative_overflow = Pid::new(PidConfig { + derivative: f32::MAX, + ..valid + }); + assert_eq!( + derivative_overflow.update(0.0, 2.0, 0.1), + Err(PidError::NonFinite) + ); + let mut integral_nan = Pid::new(valid); + integral_nan.integral_contribution = f32::NAN; + assert_eq!(integral_nan.update(0.0, 0.0, 0.1), Err(PidError::NonFinite)); + let mut output_overflow = Pid::new(PidConfig { + proportional: f32::MAX, + ..valid + }); + output_overflow.integral_contribution = f32::MAX; + assert_eq!( + output_overflow.update(1.0, 0.0, 0.1), + Err(PidError::NonFinite) + ); + + let mut trajectory = Trajectory::new(Vector3::zero(), trajectory_limits()).unwrap(); + trajectory.goal = Vector3::new(f32::MAX, 0.0, 0.0); + trajectory.reference.position = Vector3::new(-f32::MAX, 0.0, 0.0); + assert_eq!( + trajectory.update(0.01), + Err(TrajectoryError::NumericalOverflow) + ); + assert_eq!( + braking_velocity(Vector3::new(1.0, 0.0, 0.0), f32::MAX, f32::MAX), + None + ); + + let horizontal_overflow = ThrustEnvelope { + mass_kg: 2.0, + maximum_collective_thrust_newtons: 10.0, + maximum_tilt_tangent: 1.0, + }; + assert_eq!( + horizontal_overflow.force_for_acceleration(Vector3::new(f32::MAX, 0.0, 0.0), 9.8), + Err(ThrustEnvelopeError::InvalidAcceleration) + ); + let tilt_overflow = ThrustEnvelope { + mass_kg: 1.0, + maximum_collective_thrust_newtons: 10.0, + maximum_tilt_tangent: f32::MAX, + }; + assert_eq!( + tilt_overflow.force_for_acceleration(Vector3::zero(), 9.8), + Err(ThrustEnvelopeError::InvalidAcceleration) + ); + let vertical_overflow = ThrustEnvelope { + mass_kg: f32::MAX, + maximum_collective_thrust_newtons: 10.0, + maximum_tilt_tangent: 1.0, + }; + assert_eq!( + vertical_overflow.force_for_acceleration(Vector3::zero(), 9.8), + Err(ThrustEnvelopeError::InvalidAcceleration) + ); + } + + #[test] + fn boundary_numeric_guards_reject_each_late_stage_without_state_commit() { + let mut trajectory = Trajectory::new(Vector3::zero(), trajectory_limits()).unwrap(); + let before = trajectory.reference(); + assert_eq!( + trajectory.update(f32::NAN), + Err(TrajectoryError::InvalidTime) + ); + assert_eq!(trajectory.reference(), before); + + // Each force component is finite and passes the tilt limiter, but the + // requested vector norm overflows. The collective limiter must reject + // it rather than manufacturing a finite-looking thrust command. + let collective_norm_overflow = ThrustEnvelope { + mass_kg: 1.0, + maximum_collective_thrust_newtons: f32::MAX, + maximum_tilt_tangent: 1.0, + }; + assert_eq!( + collective_norm_overflow + .force_for_acceleration(Vector3::new(f32::MAX, 0.0, -f32::MAX / 2.0), 1.0), + Err(ThrustEnvelopeError::InvalidAcceleration) + ); + + let mut allocator = + Allocator::new(PrecomputedMixer::new([output([1.0, 0.0, 0.0, 0.0])])).unwrap(); + let history = allocator.previous; + assert_eq!( + allocator.allocate( + BodyWrench { + roll: 0.0, + pitch: 0.0, + yaw: 0.0, + collective: 0.0, + }, + f32::NAN, + ), + Err(AllocatorError::InvalidTime) + ); + assert_eq!(allocator.previous, history); + + // A finite request can still overflow one precomputed mixer product. + // The allocator must reject atomically, preserving its previous output. + allocator.mixer.outputs[0].coefficients[0] = f32::MAX; + assert_eq!( + allocator.allocate( + BodyWrench { + roll: f32::MAX, + pitch: 0.0, + yaw: 0.0, + collective: 0.0, + }, + 0.01, + ), + Err(AllocatorError::InvalidWrench) + ); + assert_eq!(allocator.previous, history); + } +}