function

Vehicles.Cubs2.OuterLoopComponents.tecsTransition

Pure fixed-dimensional total-energy controller transition

The energy coordinates follow K. R. Bruce, NASA B737 Flight Test Results of the Total Energy Control System, NASA CR-178285, Section 2.1, report page 6. Equation (3) gives E_dot_s/V = gamma + V_dot/g; Equation (4) gives T_req = W (gamma + V_dot/g) + D. The total-energy-rate and energy-distribution PI laws immediately following Equation (4) are represented by the two calls to Control.pidTransition.

The trim/turn terms are explicit feedforward outside those PID maps. Command limits and applied-command tracking are CUBS2 adaptations and are not presented as equations from the NASA report.

Public declarations

Explicit source declarations; dimensions, modifications and defaults are shown as written, without evaluation.

Role	Name	Type	Dimensions / attributes / default
input	`dt`	`Real`	`(unit="s")`
input	`vehicle`	`VehicleParameters`
input	`tecs`	`TecsParameters`
input	`thrustPid`	`Control.PidParameters`
input	`pitchPid`	`Control.PidParameters`
input	`pidCoefficients`	`Control.PidCoefficients`
input	`setpoints`	`GuidanceSetpoints`
input	`estimate`	`FlightState`
input	`previous`	`TecsState`
input	`previousAppliedThrust`	`Real`	`(unit="N")`
input	`previousAppliedPitch`	`Real`	`(unit="rad")`
output	`next`	`TecsState`
output	`commands`	`TecsCommands`
output	`diagnostics`	`TecsDiagnostics`

Modelica declarations and implementation

Show parameters, interfaces and source

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

function tecsTransition
  "Pure fixed-dimensional total-energy controller transition"
  input Real dt(unit="s");
  input VehicleParameters vehicle;
  input TecsParameters tecs;
  input Control.PidParameters thrustPid;
  input Control.PidParameters pitchPid;
  input Control.PidCoefficients pidCoefficients;
  input GuidanceSetpoints setpoints;
  input FlightState estimate;
  input TecsState previous;
  input Real previousAppliedThrust(unit="N");
  input Real previousAppliedPitch(unit="rad");
  output TecsState next;
  output TecsCommands commands;
  output TecsDiagnostics diagnostics;

protected
  Real accelerationMin(unit="m/s2");
  Real accelerationMax(unit="m/s2");
  Real accelerationCommandOverG;
  Real accelerationEstimateOverG;
  Real totalEnergyRateCommand;
  Real totalEnergyRateEstimate;
  Real energyDistributionCommand;
  Real energyDistributionEstimate;
  Real loadFactorExcess;
  Real feedforwardThrust(unit="N");
  Real feedforwardPitch(unit="rad");
  Control.PidState nextThrustState;
  Control.PidState nextPitchState;
  Control.PidResult thrustFeedback;
  Control.PidResult pitchFeedback;

algorithm
  accelerationMin := -vehicle.drag / vehicle.mass;
  accelerationMax := (vehicle.thrustMax - vehicle.drag) / vehicle.mass;
  diagnostics.boundedAcceleration := MathUtilities.clip(
    setpoints.acceleration, accelerationMin, accelerationMax);
  accelerationCommandOverG := diagnostics.boundedAcceleration / vehicle.g;
  accelerationEstimateOverG := estimate.acceleration_m_s2 / vehicle.g;
  // NASA CR-178285, Sec. 2.1, Eq. (3): E_dot_s / V = gamma + V_dot / g.
  totalEnergyRateCommand := setpoints.flightPathAngle + accelerationCommandOverG;
  totalEnergyRateEstimate := estimate.flightPathAngle + accelerationEstimateOverG;
  // The distribution channel subtracts the normalized acceleration term so
  // pitch transfers energy between flight path and speed without changing
  // the total-energy channel.
  energyDistributionCommand := setpoints.flightPathAngle - accelerationCommandOverG;
  energyDistributionEstimate := estimate.flightPathAngle - accelerationEstimateOverG;

  loadFactorExcess := 1.0 / max(cos(estimate.euler_rad[1]), 0.5) - 1.0;
  feedforwardThrust := vehicle.trimThrust
    + vehicle.weight
      * (totalEnergyRateCommand + tecs.turnThrustGain * loadFactorExcess);
  feedforwardPitch := tecs.turnPitchGain * loadFactorExcess;
  diagnostics.energyRateError :=
    totalEnergyRateCommand - totalEnergyRateEstimate;
  diagnostics.energyDistributionError :=
    energyDistributionCommand - energyDistributionEstimate;

  (nextThrustState, thrustFeedback) := Control.pidTransition(
    thrustPid,
    pidCoefficients,
    Control.PidState(
      previous.thrustIntegralContribution,
      previous.thrustError,
      previous.thrustDerivative),
    totalEnergyRateCommand,
    totalEnergyRateEstimate,
    previousAppliedThrust - feedforwardThrust);
  (nextPitchState, pitchFeedback) := Control.pidTransition(
    pitchPid,
    pidCoefficients,
    Control.PidState(
      previous.pitchIntegralContribution,
      previous.pitchError,
      previous.pitchDerivative),
    energyDistributionCommand,
    energyDistributionEstimate,
    previousAppliedPitch - feedforwardPitch);

  next.thrustIntegralContribution := nextThrustState.integralContribution;
  next.thrustError := nextThrustState.error;
  next.thrustDerivative := nextThrustState.derivative;
  next.pitchIntegralContribution := nextPitchState.integralContribution;
  next.pitchError := nextPitchState.error;
  next.pitchDerivative := nextPitchState.derivative;

  commands.thrustPreview := MathUtilities.clip(
    feedforwardThrust + thrustFeedback.preview,
    0.0,
    vehicle.thrustMax);
  commands.pitchPreview := MathUtilities.clip(
    feedforwardPitch + pitchFeedback.preview,
    -tecs.pitchCommandLimit,
    tecs.pitchCommandLimit);

  diagnostics.unconstrainedThrust :=
    feedforwardThrust + thrustFeedback.unconstrainedCommand;
  commands.thrust := MathUtilities.clip(
    diagnostics.unconstrainedThrust, 0.0, vehicle.thrustMax);
  diagnostics.unconstrainedPitch :=
    feedforwardPitch + pitchFeedback.unconstrainedCommand;
  commands.pitch := MathUtilities.clip(
    diagnostics.unconstrainedPitch,
    -tecs.pitchCommandLimit,
    tecs.pitchCommandLimit);
  annotation(Documentation(info="<html>
    <p>The energy coordinates follow K. R. Bruce,
    <a href=\"https://ntrs.nasa.gov/api/citations/19870017485/downloads/19870017485.pdf\">
    NASA B737 Flight Test Results of the Total Energy Control System,
    NASA CR-178285</a>, Section 2.1, report page 6. Equation (3) gives
    <code>E_dot_s/V = gamma + V_dot/g</code>; Equation (4) gives
    <code>T_req = W (gamma + V_dot/g) + D</code>. The total-energy-rate
    and energy-distribution PI laws immediately following Equation (4) are
    represented by the two calls to <code>Control.pidTransition</code>.</p>
    <p>The trim/turn terms are explicit feedforward outside those PID maps.
    Command limits and applied-command tracking are CUBS2 adaptations and are
    not presented as equations from the NASA report.</p>
  </html>"));
end tecsTransition;