block

Vehicles.Cubs2.OuterLoop

Public declarations

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

Role	Name	Type	Dimensions / attributes / default	Description
parameter	`dt`	`Real`	`(unit="s") = 0.02`	50 Hz outer loop (lockstep: 2 plant steps of 0.01 per packet)
parameter	`vehicle`	`Components.VehicleParameters`	`= Components.VehicleParameters()`
parameter	`route`	`Components.RouteParameters`	`= Components.RouteParameters()`
parameter	`tecsParams`	`Components.TecsParameters`	`= Components.TecsParameters()`
parameter	`attitudeParams`	`Components.AttitudeParameters`	`= Components.AttitudeParameters()`
parameter	`filterCutoffHz`	`Real`	`(unit="Hz") = 10.0`
input	`position_m`	`Real`	`[3](each unit="m")`	current sample [x, y, z] [m]
input	`euler_rad`	`Real`	`[3](each unit="rad")`	current sample [roll, pitch, yaw] [rad]
input	`velocity_m_s`	`Real`	`[3](each unit="m/s")`	current velocity sample [x, y, z] [m/s]
input	`eulerRate_rad_s`	`Real`	`[3](each unit="rad/s")`	current body-rate sample [roll, pitch, yaw] [rad/s]
output	`command`	`Real`	`[4]`	normalized {roll, pitch, yaw, throttle} inputs to the onboard stabilizer
output	`stabilizer`	`Real`		onboard stabilizer PWM [us]
output	`currentWaypoint`	`Integer`
output	`rollCommand`	`Real`
output	`rollCommandPreview`	`Real`
output	`pitchCommandPreview`	`Real`
output	`courseError`	`Real`

Modelica declarations and implementation

Show parameters, interfaces and source

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

block OuterLoop
  import Components = Vehicles.Cubs2.OuterLoopComponents;

  parameter Real dt(unit="s") = 0.02
    "50 Hz outer loop (lockstep: 2 plant steps of 0.01 per packet)";
  parameter Components.VehicleParameters vehicle = Components.VehicleParameters();
  parameter Components.RouteParameters route = Components.RouteParameters();
  parameter Components.TecsParameters tecsParams = Components.TecsParameters();
  parameter Components.AttitudeParameters attitudeParams = Components.AttitudeParameters();
  parameter Real filterCutoffHz(unit="Hz") = 10.0;

  Components.StateEstimator estimator(dt=dt, filterCutoffHz=filterCutoffHz);
  replaceable Components.CrossTrackGuidance guidance(dt=dt, route=route)
    constrainedby Components.RouteGuidanceInterface
    "Replace with a Bry/Dubins polynomial strategy without changing routing"
    annotation(choicesAllMatching = true);
  Components.TECSController tecs(dt=dt, vehicle=vehicle, tecs=tecsParams);
  Components.AttitudeController attitude(dt=dt, vehicle=vehicle, params=attitudeParams);

  input Real position_m[3](each unit="m") "current sample [x, y, z] [m]";
  input Real euler_rad[3](each unit="rad") "current sample [roll, pitch, yaw] [rad]";
  input Real velocity_m_s[3](each unit="m/s") "current velocity sample [x, y, z] [m/s]";
  input Real eulerRate_rad_s[3](each unit="rad/s") "current body-rate sample [roll, pitch, yaw] [rad/s]";

  output Real command[4]
    "normalized {roll, pitch, yaw, throttle} inputs to the onboard stabilizer";
  Components.GuidanceSetpointsOutput setpoints;
  Components.TecsCommandsOutput tecsCommands;
  Components.FlightStateOutput estimate;
  output Real stabilizer "onboard stabilizer PWM [us]";
  output Integer currentWaypoint;
  output Real rollCommand;
  output Real rollCommandPreview;
  output Real pitchCommandPreview;
  output Real courseError;
equation
  estimator.position_m = position_m;
  estimator.euler_rad = euler_rad;
  estimator.velocity_m_s = velocity_m_s;
  estimator.eulerRate_rad_s = eulerRate_rad_s;

  connect(estimator.estimate, guidance.estimate);
  connect(estimator.estimate, tecs.estimate);
  connect(estimator.estimate, attitude.estimate);
  connect(guidance.setpoints, tecs.setpoints);
  connect(guidance.setpoints, attitude.setpoints);
  connect(tecs.commands, attitude.tecsCommands);
  connect(guidance.setpoints, setpoints);
  connect(tecs.commands, tecsCommands);
  connect(estimator.estimate, estimate);

  command = attitude.commands.normalized;
  stabilizer = attitudeParams.stabilizerCommand;
  currentWaypoint = guidance.currentWaypoint;
  rollCommand = attitude.rollCommand;
  rollCommandPreview = attitude.rollCommandPreview;
  pitchCommandPreview = tecs.commands.pitchPreview;
  courseError = attitude.courseError;
end OuterLoop;