|
|
|
|
|
// CP: 65001
|
|
|
|
|
|
// SimulationX Version: 3.8.2.45319 x64
|
|
|
|
|
|
within AIDAModelica;
|
|
|
|
|
|
model ControlPosition "[SF2.1.1] Control position"
|
|
|
|
|
|
Modelica.Blocks.Interfaces.RealInput DronePositionConsign[3](
|
|
|
|
|
|
quantity="Mechanics.Translation.Displace",
|
|
|
|
|
|
displayUnit="m") "Desired drone position consign" annotation(Placement(
|
|
|
|
|
|
transformation(extent={{-20,-20},{20,20}}),
|
|
|
|
|
|
iconTransformation(extent={{-120,-20},{-80,20}})));
|
|
|
|
|
|
Modelica.Blocks.Interfaces.RealInput Position[3](
|
|
|
|
|
|
quantity="Mechanics.Translation.Displace",
|
|
|
|
|
|
displayUnit="m") "Real drone position feedback" annotation(Placement(
|
|
|
|
|
|
transformation(extent={{-20,-20},{20,20}}),
|
|
|
|
|
|
iconTransformation(
|
|
|
|
|
|
origin={0,-100},
|
|
|
|
|
|
extent={{-20,-20},{20,20}},
|
|
|
|
|
|
rotation=90)));
|
|
|
|
|
|
Modelica.Blocks.Interfaces.RealOutput NavigationSpeedConsign[3](
|
|
|
|
|
|
quantity="Mechanics.Translation.Velocity",
|
|
|
|
|
|
displayUnit="m/s") "Desired drone speed consign" annotation(Placement(
|
|
|
|
|
|
transformation(extent={{-10,-10},{10,10}}),
|
|
|
|
|
|
iconTransformation(extent={{85,-15},{115,15}})));
|
|
|
|
|
|
Real DHP[2](
|
|
|
|
|
|
quantity="Mechanics.Translation.Displace",
|
|
|
|
|
|
displayUnit="m") "Desired horizontal position of the drone";
|
|
|
|
|
|
Real HP[2](
|
|
|
|
|
|
quantity="Mechanics.Translation.Displace",
|
|
|
|
|
|
displayUnit="m") "Real horizontal position of the drone";
|
|
|
|
|
|
Real DZP(
|
|
|
|
|
|
quantity="Mechanics.Translation.Displace",
|
|
|
|
|
|
displayUnit="m") "Desired altitude position";
|
|
|
|
|
|
Real ZP(
|
|
|
|
|
|
quantity="Mechanics.Translation.Displace",
|
|
|
|
|
|
displayUnit="m") "Real altitude position";
|
|
|
|
|
|
Real HNSC[2](
|
|
|
|
|
|
quantity="Mechanics.Translation.Velocity",
|
|
|
|
|
|
displayUnit="m/s") "Horizontal Navigation Speed Consign";
|
|
|
|
|
|
Real ASC(
|
|
|
|
|
|
quantity="Mechanics.Translation.Velocity",
|
|
|
|
|
|
displayUnit="m/s") "AltitudeSpeedConsign";
|
|
|
|
|
|
parameter Real Kph[2,2]={{1,0},{0,1}} "Proportional regulator gain attitude chanel";
|
|
|
|
|
|
parameter Real kpz=1.5 "Proportional regulator gain altitude chanel";
|
|
|
|
|
|
equation
|
|
|
|
|
|
// enter your equations here
|
|
|
|
|
|
DHP = {DronePositionConsign[1],DronePositionConsign[2]};
|
|
|
|
|
|
HP = {Position[1],Position[2]};
|
|
|
|
|
|
HNSC = Kph*(DHP-HP);
|
|
|
|
|
|
DZP = DronePositionConsign[3];
|
|
|
|
|
|
ZP = Position[3];
|
|
|
|
|
|
ASC = kpz*(DZP-ZP);
|
|
|
|
|
|
NavigationSpeedConsign = {HNSC[1],HNSC[2],ASC};
|
|
|
|
|
|
annotation(Icon(graphics={
|
|
|
|
|
|
Rectangle(
|
|
|
|
|
|
fillColor={255,255,255},
|
|
|
|
|
|
fillPattern=FillPattern.Solid,
|
|
|
|
|
|
extent={{-100,100},{100,-100}}),
|
|
|
|
|
|
Ellipse(
|
|
|
|
|
|
fillColor={255,255,255},
|
|
|
|
|
|
fillPattern=FillPattern.Solid,
|
|
|
|
|
|
extent={{-35,37},{35,-33}}),
|
|
|
|
|
|
Text(
|
|
|
|
|
|
textString="+",
|
|
|
|
|
|
fillPattern=FillPattern.None,
|
|
|
|
|
|
extent={{-33.8,17},{-0.4,-19.6}}),
|
|
|
|
|
|
Text(
|
|
|
|
|
|
textString="-",
|
|
|
|
|
|
fillPattern=FillPattern.None,
|
|
|
|
|
|
extent={{-15,-6.6},{18.4,-43.2}}),
|
|
|
|
|
|
Text(
|
|
|
|
|
|
textString="kh/kz",
|
|
|
|
|
|
fillPattern=FillPattern.None,
|
|
|
|
|
|
extent={{33.5,30.5},{93.5,-22.9}}),
|
|
|
|
|
|
Line(points={{-36.7,0},{-86.7,0.3}}),
|
|
|
|
|
|
Line(points={{0,-83.3},{0,-31.7}})}));
|
|
|
|
|
|
end ControlPosition;
|
