block

Vehicles.Cubs2.OuterLoopComponents.TECSController

Public declarations

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

Role	Name	Type	Dimensions / attributes / default
parameter	`dt`	`Real`	`(unit="s", min=1.0e-9) = 0.02`
parameter	`vehicle`	`VehicleParameters`	`= VehicleParameters()`
parameter	`tecs`	`TecsParameters`	`= TecsParameters()`
parameter	`thrustPid`	`Control.PidParameters`	`= Control.PidParameters( samplePeriod=dt, kp=vehicle.weight * tecs.thrustKp, ki=vehicle.weight * tecs.thrustKi, kd=0.0, integralLimit=tecs.thrustIntegralLimit, commandMin=-vehicle.thrustMax, commandMax=vehicle.thrustMax, trackingTime=tecs.trackingTime)`
parameter	`pitchPid`	`Control.PidParameters`	`= Control.PidParameters( samplePeriod=dt, kp=tecs.pitchKp, ki=tecs.pitchKi, kd=0.0, integralLimit=tecs.pitchIntegralLimit, commandMin=-tecs.pitchCommandLimit, commandMax=tecs.pitchCommandLimit, trackingTime=tecs.trackingTime)`
parameter	`pidCoefficients`	`Control.PidCoefficients`	`= Control.PidCoefficients( derivativeWeight=1.0, trackingCoefficient=dt / max(tecs.trackingTime, 1.0e-9))`
output	`boundedAcceleration`	`Real`	`(unit="m/s2", start=0.0)`
output	`energyRateError`	`Real`	`(start=0.0)`
output	`thrustIntegralContribution`	`Real`	`(unit="N", start=0.0)`
output	`unconstrainedThrustCommand`	`Real`	`(unit="N", start=0.0)`
output	`energyDistributionError`	`Real`	`(start=0.0)`
output	`pitchIntegralContribution`	`Real`	`(unit="rad", start=0.0)`
output	`unconstrainedPitchCommand`	`Real`	`(unit="rad", start=0.0)`

Modelica declarations and implementation

Show parameters, interfaces and source

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

block TECSController
  parameter Real dt(unit="s", min=1.0e-9) = 0.02;
  parameter VehicleParameters vehicle = VehicleParameters();
  parameter TecsParameters tecs = TecsParameters();
  final parameter Control.PidParameters thrustPid = Control.PidParameters(
    samplePeriod=dt,
    kp=vehicle.weight * tecs.thrustKp,
    ki=vehicle.weight * tecs.thrustKi,
    kd=0.0,
    integralLimit=tecs.thrustIntegralLimit,
    commandMin=-vehicle.thrustMax,
    commandMax=vehicle.thrustMax,
    trackingTime=tecs.trackingTime);
  final parameter Control.PidParameters pitchPid = Control.PidParameters(
    samplePeriod=dt,
    kp=tecs.pitchKp,
    ki=tecs.pitchKi,
    kd=0.0,
    integralLimit=tecs.pitchIntegralLimit,
    commandMin=-tecs.pitchCommandLimit,
    commandMax=tecs.pitchCommandLimit,
    trackingTime=tecs.trackingTime);
  final parameter Control.PidCoefficients pidCoefficients =
    Control.PidCoefficients(
      derivativeWeight=1.0,
      trackingCoefficient=dt / max(tecs.trackingTime, 1.0e-9));

  GuidanceSetpointsInput setpoints;
  FlightStateInput estimate;

  discrete output Real boundedAcceleration(unit="m/s2", start=0.0);
  discrete output Real energyRateError(start=0.0);
  discrete output Real thrustIntegralContribution(unit="N", start=0.0);
  discrete output Real unconstrainedThrustCommand(unit="N", start=0.0);
  TecsCommandsOutput commands;
  discrete output Real energyDistributionError(start=0.0);
  discrete output Real pitchIntegralContribution(unit="rad", start=0.0);
  discrete output Real unconstrainedPitchCommand(unit="rad", start=0.0);

protected
  TecsState nextState;
  TecsCommands nextCommands;
  TecsDiagnostics diagnostics;
  discrete Real thrustErrorDerivative(start=0.0);
  discrete Real pitchErrorDerivative(start=0.0);

algorithm
  when sample(0.0, dt) then
    (nextState, nextCommands, diagnostics) := tecsTransition(
      dt,
      vehicle,
      tecs,
      thrustPid,
      pitchPid,
      pidCoefficients,
      setpoints,
      estimate,
      TecsState(
        pre(thrustIntegralContribution),
        pre(energyRateError),
        pre(thrustErrorDerivative),
        pre(pitchIntegralContribution),
        pre(energyDistributionError),
        pre(pitchErrorDerivative)),
      pre(commands.thrust),
      pre(commands.pitch));
    thrustIntegralContribution := nextState.thrustIntegralContribution;
    pitchIntegralContribution := nextState.pitchIntegralContribution;
    thrustErrorDerivative := nextState.thrustDerivative;
    pitchErrorDerivative := nextState.pitchDerivative;
    commands.thrust := nextCommands.thrust;
    commands.pitch := nextCommands.pitch;
    commands.thrustPreview := nextCommands.thrustPreview;
    commands.pitchPreview := nextCommands.pitchPreview;
    boundedAcceleration := diagnostics.boundedAcceleration;
    energyRateError := diagnostics.energyRateError;
    energyDistributionError := diagnostics.energyDistributionError;
    unconstrainedThrustCommand := diagnostics.unconstrainedThrust;
    unconstrainedPitchCommand := diagnostics.unconstrainedPitch;
  end when;
end TECSController;