within AIDAModelica;
model ControlPositionSAC "[SF2.1.1] Control position SAC"
	Modelica.Blocks.Interfaces.RealInput DronePositionConsign[3](
		each quantity="Mechanics.Translation.Displace",
		each displayUnit="m") "Desired drone position consign" annotation(Placement(
		transformation(origin = {-122, -60}, extent = {{-20, -20}, {20, 20}}),
		iconTransformation(extent={{-120,-20},{-80,20}})));
	Modelica.Blocks.Interfaces.RealInput Position[3](
		each quantity="Mechanics.Translation.Displace",
		each displayUnit="m") "Real drone position feedback" annotation(Placement(
		transformation(origin = {-120, 0}, extent = {{-20, -20}, {20, 20}}),
		iconTransformation(extent={{-120,-70},{-80,-30}})));
	Modelica.Blocks.Interfaces.RealOutput NavigationSpeedConsign[3](
		each quantity="Mechanics.Translation.Velocity",
		each displayUnit="m/s") "Desired drone speed consign" annotation(Placement(
		transformation(origin = {110, 0}, 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(origin = {-120, 60}, extent = {{-20, -20}, {20, 20}}),
		iconTransformation(extent={{-120,30},{-80,70}})));
	Real DHP[2](
		each quantity="Mechanics.Translation.Displace",
		each displayUnit="m") "Desired horizontal position of the drone";
	Real HP[2](
		each quantity="Mechanics.Translation.Displace",
		each displayUnit="m") "Real horizontal position of the drone";
	Real HNSC[2](
		each quantity="Mechanics.Translation.Velocity",
		each 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;