|
|
|
|
|
within AIDAModelica;
|
|
|
|
|
|
|
|
|
|
|
|
model ControlRollAngle "[SF2.3.4] Control roll angle"
|
|
|
|
|
|
// CP: 65001
|
|
|
|
|
|
// SimulationX Version: 3.8.2.45319 x64
|
|
|
|
|
|
Modelica.Blocks.Interfaces.RealInput RollConsign(quantity = "Mechanics.Rotation.Angle", displayUnit = "rad") "Choosed roll consign" annotation(
|
|
|
|
|
|
Placement(transformation(extent = {{-130, 45}, {-90, 85}}), iconTransformation(extent = {{-120, 30}, {-80, 70}})));
|
|
|
|
|
|
Modelica.Blocks.Interfaces.RealInput Roll(quantity = "Mechanics.Rotation.Angle", displayUnit = "rad") "Real drone roll angle feedback" annotation(
|
|
|
|
|
|
Placement(transformation(extent = {{-125, 10}, {-85, 50}}), iconTransformation(extent = {{-120, -20}, {-80, 20}})));
|
|
|
|
|
|
Modelica.Blocks.Interfaces.RealInput AngularSpeedX(quantity = "Mechanics.Rotation.RotVelocity", displayUnit = "rad/s") "Real drone angular X speed feedback" annotation(
|
|
|
|
|
|
Placement(transformation(extent = {{-125, -15}, {-85, 25}}), iconTransformation(extent = {{-120, -70}, {-80, -30}})));
|
|
|
|
|
|
Modelica.Blocks.Interfaces.RealOutput MomentumX(quantity = "Mechanics.Rotation.Torque", displayUnit = "Nm") "Desired momentum at axe X" annotation(
|
|
|
|
|
|
Placement(transformation(extent = {{60, 20}, {80, 40}}), iconTransformation(extent = {{86.7, -10}, {106.7, 10}})));
|
|
|
|
|
|
XAngularSpeedErrorModel xAngularSpeedErrorModel1(Kwphi = 1) "X angular speed error calculation model" annotation(
|
|
|
|
|
|
Placement(transformation(extent = {{-30, 20}, {-10, 40}})));
|
|
|
|
|
|
Modelica.Blocks.Continuous.PID PID(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 = {{20, 20}, {40, 40}})));
|
|
|
|
|
|
equation
|
|
|
|
|
|
connect(PID.y, MomentumX) annotation(
|
|
|
|
|
|
Line(points = {{41, 30}, {46, 30}, {65, 30}, {70, 30}}, color = {0, 0, 127}, thickness = 0.0625));
|
|
|
|
|
|
connect(xAngularSpeedErrorModel1.RollConsign, RollConsign) annotation(
|
|
|
|
|
|
Line(points = {{-30, 35}, {-35, 35}, {-105, 35}, {-105, 65}, {-110, 65}}, color = {0, 0, 127}, thickness = 0.0625));
|
|
|
|
|
|
connect(xAngularSpeedErrorModel1.Roll, Roll) annotation(
|
|
|
|
|
|
Line(points = {{-30, 30}, {-35, 30}, {-100, 30}, {-105, 30}}, color = {0, 0, 127}, thickness = 0.0625));
|
|
|
|
|
|
connect(xAngularSpeedErrorModel1.AngularSpeedX, AngularSpeedX) annotation(
|
|
|
|
|
|
Line(points = {{-30, 25}, {-35, 25}, {-100, 25}, {-100, 5}, {-105, 5}}, color = {0, 0, 127}, thickness = 0.0625));
|
|
|
|
|
|
connect(xAngularSpeedErrorModel1.AngularSpeedXError, PID.u) annotation(
|
|
|
|
|
|
Line(points = {{-10.3, 30}, {-5.3, 30}, {13, 30}, {18, 30}}, color = {0, 0, 127}, thickness = 0.0625));
|
|
|
|
|
|
annotation(
|
|
|
|
|
|
RollConsign(flags = 2),
|
|
|
|
|
|
Roll(flags = 2),
|
|
|
|
|
|
AngularSpeedX(flags = 2),
|
|
|
|
|
|
MomentumX(flags = 2),
|
|
|
|
|
|
xAngularSpeedErrorModel1(RollConsign(flags = 2), Roll(flags = 2), AngularSpeedX(flags = 2), AngularSpeedXError(flags = 2), ephi(flags = 2), wxd(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))),
|
|
|
|
|
|
Icon(graphics = {Rectangle(fillColor = {255, 255, 255}, fillPattern = FillPattern.Solid, extent = {{-100, 100}, {100, -100}}), Text(origin = {19, -5}, extent = {{-45, 19}, {1, 1}}, textString = "%name")}, coordinateSystem(initialScale = 0.1)),
|
|
|
|
|
|
experiment(StopTime = 1, StartTime = 0, Interval = 0.002, MaxInterval = "0.001"));
|
|
|
|
|
|
end ControlRollAngle;
|
