2123 lines
75 KiB
Rust
2123 lines
75 KiB
Rust
//! 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<Frd>);
|
|
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<Frd> {
|
|
self.0
|
|
}
|
|
}
|
|
|
|
/// Desired NED force vector in newtons.
|
|
#[derive(Clone, Copy, Debug, PartialEq)]
|
|
pub struct NedForce(Vector3<Ned>);
|
|
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<Ned> {
|
|
self.0
|
|
}
|
|
}
|
|
|
|
/// Desired horizontal heading direction in NED, rather than an ambiguous yaw scalar.
|
|
#[derive(Clone, Copy, Debug, PartialEq)]
|
|
pub struct HeadingNed(Vector3<Ned>);
|
|
impl HeadingNed {
|
|
/// Validates and normalizes a horizontal NED heading.
|
|
pub fn new(north: f32, east: f32) -> Option<Self> {
|
|
let vector = Vector3::new(north, east, 0.0).normalized()?;
|
|
Some(Self(vector))
|
|
}
|
|
/// Returns the normalized NED heading vector.
|
|
pub const fn vector(self) -> Vector3<Ned> {
|
|
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<UnitQuaternion<Frd, Ned>, 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<f32, PidError> {
|
|
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<Vector3<Frd>>,
|
|
}
|
|
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<Vector3<Frd>, 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<Ned>,
|
|
/// Local NED velocity in metres per second.
|
|
pub velocity: Vector3<Ned>,
|
|
/// Local NED acceleration in metres per second squared.
|
|
pub acceleration: Vector3<Ned>,
|
|
}
|
|
|
|
/// 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<Ned>,
|
|
}
|
|
|
|
impl Trajectory {
|
|
/// Starts at a finite local position with zero velocity and acceleration.
|
|
pub fn new(
|
|
initial_position: Vector3<Ned>,
|
|
limits: KinematicLimits,
|
|
) -> Result<Self, TrajectoryError> {
|
|
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<Ned>) -> 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<Ned> {
|
|
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<KinematicReference, TrajectoryError> {
|
|
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::<Ned>::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::<Ned>::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<Ned>,
|
|
maximum_speed: f32,
|
|
maximum_acceleration: f32,
|
|
) -> Option<Vector3<Ned>> {
|
|
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<f32> {
|
|
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<Ned>, maximum: f32) -> Option<Vector3<Ned>> {
|
|
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<Ned>,
|
|
/// Estimated local NED velocity in metres per second.
|
|
pub velocity: Vector3<Ned>,
|
|
}
|
|
|
|
/// 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<Self, TranslationalControlError> {
|
|
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<Vector3<Ned>, 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<Ned>,
|
|
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<const ACTUATORS: usize> {
|
|
outputs: [MixerOutput; ACTUATORS],
|
|
}
|
|
impl<const ACTUATORS: usize> PrecomputedMixer<ACTUATORS> {
|
|
/// 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<const ACTUATORS: usize> {
|
|
/// 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<const ACTUATORS: usize> {
|
|
mixer: PrecomputedMixer<ACTUATORS>,
|
|
previous: [f32; ACTUATORS],
|
|
failed: [bool; ACTUATORS],
|
|
}
|
|
|
|
impl<const ACTUATORS: usize> Allocator<ACTUATORS> {
|
|
/// Validates the reviewed static allocation map.
|
|
pub fn new(mixer: PrecomputedMixer<ACTUATORS>) -> Result<Self, AllocatorError> {
|
|
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<AllocationResult<ACTUATORS>, 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::<Ned>::new(reference.velocity.x(), reference.velocity.y(), 0.0).norm();
|
|
let horizontal_acceleration =
|
|
Vector3::<Ned>::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::<Ned>::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::<Ned>::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::<Ned>::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::<Frd>::new(1.0, 0.0, 0.0));
|
|
let down = attitude.body_to_ned(Vector3::<Frd>::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::<Ned>::new(reference.velocity.x(), reference.velocity.y(), 0.0).norm();
|
|
let horizontal_acceleration =
|
|
Vector3::<Ned>::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);
|
|
}
|
|
}
|