Vehicles.Cubs2.OuterLoopComponents.AttitudeController

Parameters, inputs, outputs, defaults and equations from the source file.

block AttitudeController
  parameter Real dt(unit="s", min=1.0e-9) = 0.02;
  parameter VehicleParameters vehicle = VehicleParameters();
  parameter AttitudeParameters params = AttitudeParameters();
  parameter Control.PidParameters headingPid =
    Control.PidParameters(samplePeriod=dt, kp=0.5, ki=0.0,
                          kd=0.5, integralLimit=0.0,
                          commandMin=-params.rollLimit,
                          commandMax=params.rollLimit);
  // Closed pitch loop: the plant's stick-to-pitch response is not trusted,
  // so the open-loop gain rides along as feedforward and the PID trims it.
  parameter Control.PidParameters pitchPid =
    Control.PidParameters(samplePeriod=dt, kp=2.3, ki=0.0, kd=0.0,
                          integralLimit=0.0,
                          commandMin=-1.0, commandMax=1.0);

  final parameter Control.PidCoefficients headingCoefficients =
    Control.PidCoefficients(
      derivativeWeight=if headingPid.derivativeCutoffHz > 0.0 then
        1.0 - exp(-2.0 * 3.141592653589793
          * headingPid.derivativeCutoffHz * dt) else 1.0,
      trackingCoefficient=dt / max(headingPid.trackingTime, 1.0e-9));
  final parameter Control.PidCoefficients pitchCoefficients =
    Control.PidCoefficients(
      derivativeWeight=if pitchPid.derivativeCutoffHz > 0.0 then
        1.0 - exp(-2.0 * 3.141592653589793
          * pitchPid.derivativeCutoffHz * dt) else 1.0,
      trackingCoefficient=dt / max(pitchPid.trackingTime, 1.0e-9));

  GuidanceSetpointsInput setpoints;
  FlightStateInput estimate;
  TecsCommandsInput tecsCommands;

  StabilizerCommandsOutput commands;
  discrete output Real rollCommand(start=0.0);
  discrete output Real rollCommandPreview(start=0.0)
    "integral-free bank; what guidance would fly while disengaged";
  output Real courseError;

protected
  discrete Real rollCommandState(start=0.0);
  discrete Real unlimitedRollCommand;
  discrete Real headingIntegral(start=0.0);
  discrete Real headingError(start=0.0);
  discrete Real headingDerivative(start=0.0);
  discrete Real headingUnconstrained(start=0.0);
  discrete Real headingCommand(start=0.0);
  discrete Real headingPreview(start=0.0);
  discrete Real pitchIntegral(start=0.0);
  discrete Real pitchError(start=0.0);
  discrete Real pitchDerivative(start=0.0);
  discrete Real pitchUnconstrained(start=0.0);
  discrete Real pitchCommand(start=0.0);

equation
  courseError = -MathUtilities.wrapAngle(
    setpoints.heading
    - atan2(estimate.velocity_m_s[2], estimate.velocity_m_s[1]));

algorithm
  when sample(0.0, dt) then
    // Keep both PID transitions inside this single guarded producer. Scalar
    // updates avoid a nested multi-output function transaction, which is not
    // representable by the checked eFMI/Solve-IR event contract.
    headingError := courseError;
    headingDerivative := pre(headingDerivative)
      + headingCoefficients.derivativeWeight
        * ((headingError - pre(headingError)) / dt
          - pre(headingDerivative));
    headingUnconstrained := headingPid.kp * headingError
      + pre(headingIntegral) + headingPid.kd * headingDerivative;
    headingCommand := MathUtilities.clip(
      headingUnconstrained, headingPid.commandMin, headingPid.commandMax);
    headingPreview := MathUtilities.clip(
      headingPid.kp * headingError + headingPid.kd * headingDerivative,
      headingPid.commandMin, headingPid.commandMax);
    headingIntegral := MathUtilities.clip(
      pre(headingIntegral) + headingPid.ki * headingError * dt
        + headingCoefficients.trackingCoefficient
          * (pre(headingCommand) - headingUnconstrained),
      -headingPid.integralLimit, headingPid.integralLimit);

    pitchError := tecsCommands.pitch - estimate.euler_rad[2];
    pitchDerivative := pre(pitchDerivative)
      + pitchCoefficients.derivativeWeight
        * ((pitchError - pre(pitchError)) / dt - pre(pitchDerivative));
    pitchUnconstrained := pitchPid.kp * pitchError
      + pre(pitchIntegral) + pitchPid.kd * pitchDerivative;
    pitchCommand := MathUtilities.clip(
      pitchUnconstrained, pitchPid.commandMin, pitchPid.commandMax);
    pitchIntegral := MathUtilities.clip(
      pre(pitchIntegral) + pitchPid.ki * pitchError * dt
        + pitchCoefficients.trackingCoefficient
          * (pre(pitchCommand) - pitchUnconstrained),
      -pitchPid.integralLimit, pitchPid.integralLimit);

    unlimitedRollCommand := headingCommand;
    rollCommandState :=
      MathUtilities.clip(
        MathUtilities.rateLimit(unlimitedRollCommand,
          pre(rollCommandState), params.rollRateLimit * dt),
        -params.rollLimit, params.rollLimit);
    rollCommand := rollCommandState;
    rollCommandPreview :=
      MathUtilities.clip(headingPreview, -params.rollLimit, params.rollLimit);
    // Assign the record array as one guarded producer. Besides expressing the
    // command atomically, this gives event schedulers a complete transaction
    // instead of four element writes to one array-valued variable.
    commands.normalized := {
      MathUtilities.clip(
        params.rollCommandToAileronGain * rollCommand, -1.0, 1.0),
      MathUtilities.clip(
        params.trimElevator
          + params.pitchCommandToElevatorGain * tecsCommands.pitch
          + pitchCommand,
        -1.0, 1.0),
      0.0,
      MathUtilities.clip(
        tecsCommands.thrust / vehicle.thrustMax, 0.0, 1.0)};
  end when;
end AttitudeController;