within AIDAModelica;

model ComputeAltitudeConsign "[SF2.4.1] Compute altitude consign"
  // CP: 65001
  // SimulationX Version: 3.8.2.45319 x64
  Modelica.Blocks.Interfaces.RealInput RCVerticalSpeedCommand(quantity = "Mechanics.Translation.Velocity", displayUnit = "m/s") "RC vertical speed consign" annotation(
    Placement(transformation(extent = {{-150, 25}, {-110, 65}}), iconTransformation(extent = {{-120, 30}, {-80, 70}})));
  Modelica.Blocks.Interfaces.RealInput VSpeed(quantity = "Mechanics.Translation.Velocity", displayUnit = "m/s") "Real drone speed feedback" annotation(
    Placement(transformation(extent = {{-150, -35}, {-110, 5}}), iconTransformation(extent = {{-120, -70}, {-80, -30}})));
  Modelica.Blocks.Interfaces.RealOutput AccelerationConsign(quantity = "Mechanics.Translation.Accel", displayUnit = "m/s²") "Drone acceleration consign " annotation(
    Placement(transformation(extent = {{65, 25}, {85, 45}}), iconTransformation(extent = {{90, -10}, {110, 10}})));
  Modelica.Blocks.Continuous.PID PID1(k = 250, Ti = 1.5, Td = 0.02, initType = Modelica.Blocks.Types.InitPID.NoInit) annotation(
    Placement(transformation(extent = {{15, 25}, {35, 45}})));
  AIDAModelica.SpeedErrorModelSAC speedErrorModelSAC1 annotation(
    Placement(transformation(extent = {{-55, 30}, {-35, 50}})));
equation
  connect(speedErrorModelSAC1.SpeedError, PID1.u) annotation(
    Line(points = {{-35, 40}, {-30, 40}, {8, 40}, {8, 35}, {13, 35}}, color = {0, 0, 127}, thickness = 0.0625));
  connect(RCVerticalSpeedCommand, speedErrorModelSAC1.SpeedConsign) annotation(
    Line(points = {{-130, 45}, {-125, 45}, {-60, 45}, {-60, 40}, {-55, 40}}, color = {0, 0, 127}, thickness = 0.0625));
  connect(VSpeed, speedErrorModelSAC1.Speed) annotation(
    Line(points = {{-130, -15}, {-125, -15}, {-45, -15}, {-45, 25}, {-45, 30}}, color = {0, 0, 127}, thickness = 0.0625));
  connect(PID1.y, AccelerationConsign) annotation(
    Line(points = {{36, 35}, {41, 35}, {70, 35}, {75, 35}}, color = {0, 0, 127}, thickness = 0.0625));
  annotation(
    Speed(flags = 2),
    RCVerticalSpeedCommand(flags = 2),
    AccelerationConsign(flags = 2),
    PID1(u(flags = 2), y(flags = 2), P(u(flags = 2), y(flags = 2)), I(u(flags = 2), y(flags = 2)), D(u(flags = 2), y(flags = 2), x(flags = 2)), Gain(u(flags = 2), y(flags = 2)), Add(u1(flags = 2), u2(flags = 2), u3(flags = 2), y(flags = 2))),
    speedErrorModelSAC1(SpeedConsign(flags = 2), Speed(flags = 2), SpeedError(flags = 2), vzd(flags = 2), vz(flags = 2), ASE(flags = 2)),
    Icon(graphics = {Rectangle(fillColor = {255, 255, 255}, fillPattern = FillPattern.Solid, extent = {{-100, 100}, {100, -100}})}),
    experiment(StopTime = 1, StartTime = 0, Interval = 0.002, MaxInterval = "0.001"),
    Speed(flags = 2),
    RCVerticalSpeedCommand(flags = 2),
    AccelerationConsign(flags = 2),
    PID1(u(flags = 2), y(flags = 2), P(u(flags = 2), y(flags = 2)), I(u(flags = 2), y(flags = 2)), D(u(flags = 2), y(flags = 2), x(flags = 2)), Gain(u(flags = 2), y(flags = 2)), Add(u1(flags = 2), u2(flags = 2), u3(flags = 2), y(flags = 2))),
    speedErrorModelSAC1(SpeedConsign(flags = 2), Speed(flags = 2), SpeedError(flags = 2), vzd(flags = 2), vz(flags = 2), ASE(flags = 2)),
    Icon(graphics = {Rectangle(fillColor = {255, 255, 255}, fillPattern = FillPattern.Solid, extent = {{-100, 100}, {100, -100}}), Text(origin = {-1, 4}, extent = {{-35, 10}, {35, -10}}, textString = "%name")}, coordinateSystem(initialScale = 0.1)),
    experiment(StopTime = 1, StartTime = 0, Interval = 0.002, MaxInterval = "0.001"));
end ComputeAltitudeConsign;