|
|
|
|
|
// CP: 65001
|
|
|
|
|
|
// SimulationX Version: 3.8.2.45319 x64
|
|
|
|
|
|
within AIDAModelica;
|
|
|
|
|
|
model ControlPositionSAC "[SF2.1.1] Control position SAC"
|
|
|
|
|
|
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(extent={{-120,-70},{-80,-30}})));
|
|
|
|
|
|
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={{90,-10},{110,10}})));
|
|
|
|
|
|
Modelica.Blocks.Interfaces.RealInput RCVErticalSpeedCommand(
|
|
|
|
|
|
quantity="Mechanics.Translation.Velocity",
|
|
|
|
|
|
displayUnit="m/s") "RC vertical speed consign" annotation(Placement(
|
|
|
|
|
|
transformation(extent={{-20,-20},{20,20}}),
|
|
|
|
|
|
iconTransformation(extent={{-120,30},{-80,70}})));
|
|
|
|
|
|
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 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";
|
|
|
|
|
|
equation
|
|
|
|
|
|
// enter your equations here
|
|
|
|
|
|
DHP = {DronePositionConsign[1],DronePositionConsign[2]};
|
|
|
|
|
|
HP = {Position[1],Position[2]};
|
|
|
|
|
|
HNSC = Kph*(DHP-HP);
|
|
|
|
|
|
ASC = RCVErticalSpeedCommand;
|
|
|
|
|
|
NavigationSpeedConsign = {HNSC[1],HNSC[2],ASC};
|
|
|
|
|
|
annotation(Icon(graphics={
|
|
|
|
|
|
Rectangle(
|
|
|
|
|
|
fillColor={255,255,255},
|
|
|
|
|
|
fillPattern=FillPattern.Solid,
|
|
|
|
|
|
extent={{-100,100},{100,-100}})}));
|
|
|
|
|
|
end ControlPositionSAC;
|
