|
|
|
|
|
within AIDAModelica;
|
|
|
|
|
|
|
|
|
|
|
|
model ControlPitchAngle "[SF2.3.3] Control pitch angle"
|
|
|
|
|
|
// CP: 65001
|
|
|
|
|
|
// SimulationX Version: 3.8.2.45319 x64
|
|
|
|
|
|
Modelica.Blocks.Interfaces.RealInput PitchCosign(quantity = "Mechanics.Rotation.Angle", displayUnit = "rad") "Choosed pitch consign" annotation(
|
|
|
|
|
|
Placement(transformation(extent = {{-105, 60}, {-65, 100}}), iconTransformation(extent = {{-120, 30}, {-80, 70}})));
|
|
|
|
|
|
Modelica.Blocks.Interfaces.RealInput Pitch(quantity = "Mechanics.Rotation.Angle", displayUnit = "rad") "Real drone pitch angle feedback" annotation(
|
|
|
|
|
|
Placement(transformation(extent = {{-105, 15}, {-65, 55}}), iconTransformation(extent = {{-120, -20}, {-80, 20}})));
|
|
|
|
|
|
Modelica.Blocks.Interfaces.RealInput AngularSpeedY(quantity = "Mechanics.Rotation.RotVelocity", displayUnit = "rad/s") "Real drone angular Y speed feedback" annotation(
|
|
|
|
|
|
Placement(transformation(extent = {{-135, -25}, {-95, 15}}), iconTransformation(extent = {{-120, -70}, {-80, -30}})));
|
|
|
|
|
|
Modelica.Blocks.Interfaces.RealOutput MomentumY(quantity = "Mechanics.Rotation.Torque", displayUnit = "Nm") "Desired momentum at axe Y" annotation(
|
|
|
|
|
|
Placement(transformation(extent = {{107, -19}, {127, 1}}), iconTransformation(extent = {{90, -10}, {110, 10}})));
|
|
|
|
|
|
YAngularSpeedErrorModel yAngularSpeedErrorModel1 annotation(
|
|
|
|
|
|
Placement(transformation(extent = {{-25, 25}, {-5, 45}})));
|
|
|
|
|
|
Modelica.Blocks.Continuous.PID PID1(k = 0.09, Ti = 1.5, Td = 0.03, initType = Modelica.Blocks.Types.InitPID.InitialState, Add(k1 = -1, k2 = -1, k3 = -1)) annotation(
|
|
|
|
|
|
Placement(transformation(extent = {{16, 25}, {36, 45}})));
|
|
|
|
|
|
equation
|
|
|
|
|
|
connect(AngularSpeedY, yAngularSpeedErrorModel1.AngularSpeedY) annotation(
|
|
|
|
|
|
Line(points = {{-115, -5}, {-110, -5}, {-30, -5}, {-30, 30}, {-25, 30}}, color = {0, 0, 127}));
|
|
|
|
|
|
connect(PID1.u, yAngularSpeedErrorModel1.AngularSpeedYError) annotation(
|
|
|
|
|
|
Line(points = {{14, 35}, {-5.3, 35}}, color = {0, 0, 127}, thickness = 0.0625));
|
|
|
|
|
|
connect(yAngularSpeedErrorModel1.PitchCosign, PitchCosign) annotation(
|
|
|
|
|
|
Line(points = {{-25, 40}, {-80, 40}, {-80, 80}, {-85, 80}}, color = {0, 0, 127}, thickness = 0.0625));
|
|
|
|
|
|
connect(yAngularSpeedErrorModel1.Pitch, Pitch) annotation(
|
|
|
|
|
|
Line(points = {{-25, 35}, {-30, 35}, {-80, 35}, {-85, 35}}, color = {0, 0, 127}, thickness = 0.0625));
|
|
|
|
|
|
connect(PID1.y, MomentumY) annotation(
|
|
|
|
|
|
Line(points = {{37, 35}, {42, 35}, {112, 35}, {112, -9}, {117, -9}}, color = {0, 0, 127}, thickness = 0.0625));
|
|
|
|
|
|
annotation(
|
|
|
|
|
|
PitchCosign(flags = 2),
|
|
|
|
|
|
Pitch(flags = 2),
|
|
|
|
|
|
AngularSpeedY(flags = 2),
|
|
|
|
|
|
MomentumY(flags = 2),
|
|
|
|
|
|
yAngularSpeedErrorModel1(PitchCosign(flags = 2), Pitch(flags = 2), AngularSpeedY(flags = 2), AngularSpeedYError(flags = 2), eteta(flags = 2), wyd(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(textString = "%name", fillPattern = FillPattern.None, extent = {{-12, 8}, {12, -8}}, origin = {-4, 0})}),
|
|
|
|
|
|
experiment(StopTime = 1, StartTime = 0, Interval = 0.002, MaxInterval = "0.001"));
|
|
|
|
|
|
end ControlPitchAngle;
|
