within AIDAModelica;

model ControlSpeed "[SF2.1.3] Control Speed"
  // CP: 65001
  // SimulationX Version: 3.8.2.45319 x64
  Modelica.Blocks.Interfaces.RealInput SpeedConsign[3](quantity = "Mechanics.Translation.Velocity", displayUnit = "m/s") "Choosed drone speed consign " annotation(
    Placement(transformation(extent = {{-120, 30}, {-80, 70}}), iconTransformation(extent = {{-120, 30}, {-80, 70}})));
  Modelica.Blocks.Interfaces.RealInput Speed[3](quantity = "Mechanics.Translation.Velocity", displayUnit = "m/s") "Real drone speed feedback" annotation(
    Placement(visible = true,transformation(extent = {{-122, -28}, {-82, 12}}, rotation = 0), iconTransformation(extent = {{-120, -70}, {-80, -30}}, rotation = 0)));
  Modelica.Blocks.Interfaces.RealOutput AccelerationConsign[3](quantity = "Mechanics.Translation.Accel", displayUnit = "m/s²") "Drone acceleration consign " annotation(
    Placement(transformation(extent = {{65, 20}, {85, 40}}), iconTransformation(extent = {{90, -10}, {110, 10}})));
  AIDAModelica.SpeedErrorModel speedErrorModel1 annotation(
    Placement(visible = true, transformation(extent = {{-60, 22}, {-40, 42}}, rotation = 0)));
  Modelica.Blocks.Continuous.PID PID(Td = 1.5, Ti = 8, initType = Modelica.Blocks.Types.InitPID.InitialState, k = 1.5) "PID-controller in additive description form" annotation(
    Placement(transformation(extent = {{15, 50}, {35, 70}})));
  Modelica.Blocks.Continuous.PID PID2(Td = 1.5, Ti = 8, initType = Modelica.Blocks.Types.InitPID.InitialState, k = 1.5) annotation(
    Placement(transformation(extent = {{15, 20}, {35, 40}})));
  Modelica.Blocks.Continuous.PID PID1(Td = 1, Ti = 3, initType = Modelica.Blocks.Types.InitPID.InitialState, k = 1.8) annotation(
    Placement(transformation(extent = {{15, -10}, {35, 10}})));
equation
  connect(PID1.u, speedErrorModel1.SpeedError[3]) annotation(
    Line(points = {{14, 0}, {-40, 0}, {-40, 32}}, color = {0, 0, 127}));
  connect(PID2.u, speedErrorModel1.SpeedError[2]) annotation(
    Line(points = {{14, 30}, {-15, 30}, {-15, 32}, {-40, 32}}, color = {0, 0, 127}));
  connect(PID.u, speedErrorModel1.SpeedError[1]) annotation(
    Line(points = {{14, 60}, {-40, 60}, {-40, 32}}, color = {0, 0, 127}));
  connect(speedErrorModel1.Speed, Speed) annotation(
    Line(points = {{-50, 22}, {-98, 22}, {-98, -8}, {-102, -8}}, color = {0, 0, 127}));
  connect(speedErrorModel1.SpeedConsign, SpeedConsign) annotation(
    Line(points = {{-60, 32}, {-92, 32}, {-92, 50}, {-100, 50}}, color = {0, 0, 127}));
  connect(PID1.y, AccelerationConsign[3]) annotation(
    Line(points = {{36, 0}, {68, 0}, {68, 30}, {76, 30}}, color = {0, 0, 127}));
  connect(PID2.y, AccelerationConsign[2]) annotation(
    Line(points = {{36, 30}, {70, 30}, {70, 30}, {76, 30}}, color = {0, 0, 127}));
  connect(PID.y, AccelerationConsign[1]) annotation(
    Line(points = {{36, 60}, {68, 60}, {68, 30}, {76, 30}}, color = {0, 0, 127}));
// enter your equations here
equation
   annotation(
    SpeedConsign(flags = 2),
    Speed(flags = 2),
    AccelerationConsign(flags = 2),
    speedErrorModel1(SpeedConsign(flags = 2), Speed(flags = 2), SpeedError(flags = 2), vhd(flags = 2), vh(flags = 2), vzd(flags = 2), vz(flags = 2), HSE(flags = 2), ASE(flags = 2)),
    PID(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))),
    PID2(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))),
    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))),
    Icon(graphics = {Rectangle(fillColor = {255, 255, 255}, fillPattern = FillPattern.Solid, extent = {{-100, 100}, {100, -100}}), Text(origin = {-23, 130}, extent = {{-33, 12}, {95, -22}}, textString = "%name")}, coordinateSystem(initialScale = 0.1)),
    experiment(StopTime = 1, StartTime = 0, Interval = 0.002, MaxInterval = "0.001"));
end ControlSpeed;