kaptan/firmware/libs/kaptan-flight-config/src/profile/control.rs

use kaptan_config::{Config, ConfigViolation, ValidationRule};

use super::ids;

/// Outer-loop controller policy.
#[derive(Clone, Copy, Debug, PartialEq, Config)]
#[config(post = Self::validate_control)]
pub struct ControlSettings {
    #[config(
        id = 8,
        key = "control.position_gain_per_s2",
        unit = "1/s2",
        finite,
        range(min = 0.0, max = 100.0)
    )]
    pub position_gain_per_second_squared: f32,
    #[config(
        id = 9,
        key = "control.velocity_gain_per_s",
        unit = "1/s",
        finite,
        range(min = 0.0, max = 100.0)
    )]
    pub velocity_gain_per_second: f32,
    #[config(
        id = 10,
        key = "control.maximum_horizontal_acceleration_mps2",
        unit = "m/s2",
        finite,
        range(min = 0.001, max = 100.0)
    )]
    pub maximum_horizontal_acceleration_mps2: f32,
    #[config(
        id = 11,
        key = "control.maximum_upward_acceleration_mps2",
        unit = "m/s2",
        finite,
        range(min = 0.001, max = 100.0)
    )]
    pub maximum_upward_acceleration_mps2: f32,
    #[config(
        id = 12,
        key = "control.maximum_downward_acceleration_mps2",
        unit = "m/s2",
        finite,
        range(min = 0.001, max = 100.0)
    )]
    pub maximum_downward_acceleration_mps2: f32,
    #[config(
        id = 41,
        key = "control.attitude_roll_pitch_gain_per_s",
        unit = "1/s",
        finite,
        range(min = 0.0, max = 10.0)
    )]
    pub attitude_roll_pitch_gain_per_second: f32,
    #[config(
        id = 42,
        key = "control.attitude_yaw_gain_per_s",
        unit = "1/s",
        finite,
        range(min = 0.0, max = 10.0)
    )]
    pub attitude_yaw_gain_per_second: f32,
}

impl ControlSettings {
    fn validate_control(&self) -> Result<(), ConfigViolation> {
        // Positive multicopter thrust requires downward acceleration strictly
        // below gravity. An inclusive scalar maximum cannot express this rule.
        if self.maximum_downward_acceleration_mps2 >= kaptan_navigation::GRAVITY_MPS2 {
            return Err(ConfigViolation::field(
                ids::MAXIMUM_DOWNWARD_ACCELERATION_MPS2,
                ValidationRule::Inconsistent,
            ));
        }
        Ok(())
    }
}

/// Physical thrust envelope.
#[derive(Clone, Copy, Debug, PartialEq, Config)]
#[config(post = Self::validate_thrust)]
pub struct ThrustSettings {
    #[config(
        id = 13,
        key = "thrust.mass_kg",
        unit = "kg",
        finite,
        range(min = 0.01, max = 100.0)
    )]
    pub mass_kg: f32,
    #[config(
        id = 14,
        key = "thrust.maximum_collective_thrust_n",
        unit = "N",
        finite,
        range(min = 0.1, max = 10_000.0)
    )]
    pub maximum_collective_thrust_newtons: f32,
    #[config(
        id = 15,
        key = "thrust.maximum_tilt_tangent",
        unit = "",
        finite,
        range(min = 0.001, max = 10.0)
    )]
    pub maximum_tilt_tangent: f32,
}
impl ThrustSettings {
    fn validate_thrust(&self) -> Result<(), ConfigViolation> {
        if self.maximum_collective_thrust_newtons <= self.mass_kg * kaptan_navigation::GRAVITY_MPS2
            || f64::from(self.maximum_collective_thrust_newtons)
                > f64::from(self.mass_kg)
                    * (f64::from(kaptan_navigation::MAXIMUM_FREE_ACCELERATION_MPS2)
                        - f64::from(kaptan_navigation::GRAVITY_MPS2))
        {
            return Err(ConfigViolation::related(
                ids::MAXIMUM_COLLECTIVE_THRUST_NEWTONS,
                ids::MASS_KG,
                ValidationRule::Inconsistent,
            ));
        }
        Ok(())
    }
}

/// Roll/pitch and yaw P/I gains with shared derivative and contribution limits.
#[derive(Clone, Copy, Debug, PartialEq, Config)]
#[config(post = Self::validate_pid)]
pub struct PidSettings {
    #[config(
        id = 16,
        key = "control.rate_pid.proportional",
        unit = "",
        finite,
        range(min = 0.0, max = 10.0)
    )]
    pub proportional: f32,
    #[config(
        id = 17,
        key = "control.rate_pid.integral",
        unit = "",
        finite,
        range(min = 0.0, max = 10.0)
    )]
    pub integral: f32,
    #[config(
        id = 18,
        key = "control.rate_pid.derivative",
        unit = "",
        finite,
        range(min = 0.0, max = 10.0)
    )]
    pub derivative: f32,
    #[config(
        id = 19,
        key = "control.rate_pid.output_limit",
        unit = "",
        finite,
        range(min = 0.001, max = 1.0)
    )]
    pub output_limit: f32,
    #[config(
        id = 20,
        key = "control.rate_pid.integral_limit",
        unit = "",
        finite,
        range(min = 0.0, max = 1.0)
    )]
    pub integral_limit: f32,
    #[config(
        id = 43,
        key = "control.yaw_rate_pid.proportional",
        unit = "",
        finite,
        range(min = 0.0, max = 10.0)
    )]
    pub yaw_proportional: f32,
    #[config(
        id = 44,
        key = "control.yaw_rate_pid.integral",
        unit = "",
        finite,
        range(min = 0.0, max = 10.0)
    )]
    pub yaw_integral: f32,
}
impl PidSettings {
    fn validate_pid(&self) -> Result<(), ConfigViolation> {
        if self.integral_limit > self.output_limit {
            return Err(ConfigViolation::related(
                ids::RATE_PID_INTEGRAL_LIMIT,
                ids::RATE_PID_OUTPUT_LIMIT,
                ValidationRule::Inconsistent,
            ));
        }
        Ok(())
    }
}

/// Actuator command bounds.
#[derive(Clone, Copy, Debug, PartialEq, Config)]
#[config(post = Self::validate_actuator_limits)]
pub struct ActuatorLimits {
    #[config(
        id = 21,
        key = "actuator.minimum",
        unit = "",
        finite,
        range(min = 0.0, max = 1.0)
    )]
    pub minimum: f32,
    #[config(
        id = 22,
        key = "actuator.maximum",
        unit = "",
        finite,
        range(min = 0.0, max = 1.0)
    )]
    pub maximum: f32,
    #[config(
        id = 23,
        key = "actuator.slew_per_second",
        unit = "1/s",
        finite,
        range(min = 0.001, max = 100.0)
    )]
    pub slew_per_second: f32,
}
impl ActuatorLimits {
    fn validate_actuator_limits(&self) -> Result<(), ConfigViolation> {
        if self.minimum >= self.maximum {
            return Err(ConfigViolation::related(
                ids::ACTUATOR_MINIMUM,
                ids::ACTUATOR_MAXIMUM,
                ValidationRule::Inconsistent,
            ));
        }
        Ok(())
    }
}