You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
75 lines
2.7 KiB
75 lines
2.7 KiB
// 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;
|
|
|