// 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;