AIDASimulation/SimulationModels/AIDAModelica/PositionControl.mo

// CP: 65001
// SimulationX Version: 3.8.2.45319 x64
within AIDAModelica;
model PositionControl "Position Control Model"
	Modelica.Blocks.Interfaces.RealInput DronPositionConsign[3](
		quantity="Mechanics.Translation.Displace",
		displayUnit="m") "Desired drone position consign" annotation(Placement(
		transformation(extent={{-163,56},{-123,96}}),
		iconTransformation(extent={{-120,30},{-80,70}})));
	Modelica.Blocks.Interfaces.RealInput Position[3](
		quantity="Mechanics.Translation.Displace",
		displayUnit="m") "Real drone position feedback" annotation(Placement(
		transformation(extent={{-125,15},{-85,55}}),
		iconTransformation(extent={{-120,-20},{-80,20}})));
	Modelica.Blocks.Interfaces.RealOutput TotalThrustAP(
		quantity="Mechanics.Translation.Force",
		displayUnit="N") "Desired total thrust autopilot mode output" annotation(Placement(
		transformation(
			origin={71,29},
			extent={{-16,-16},{16,16}}),
		iconTransformation(
			origin={100,50},
			extent={{-10,-10},{10,10}})));
	Modelica.Blocks.Interfaces.RealOutput AttitudeConsignAP[2](
		quantity="Mechanics.Rotation.Angle",
		displayUnit="rad") "Desired attitude autopilot mode consign" annotation(Placement(
		transformation(
			origin={81,61},
			extent={{-16,-16},{16,16}}),
		iconTransformation(
			origin={100,-50},
			extent={{-10,-10},{10,10}})));
	Modelica.Blocks.Interfaces.RealInput Attitude[3](
		quantity="Mechanics.Rotation.Angle",
		displayUnit="rad") "Real drone attitude feedback" annotation(Placement(
		transformation(extent={{-15,-25},{25,15}}),
		iconTransformation(
			origin={-50,-100},
			extent={{-20,-20},{20,20}},
			rotation=90)));
	Modelica.Blocks.Interfaces.RealInput Speed[3](
		quantity="Mechanics.Translation.Velocity",
		displayUnit="m/s") "Real drone speed feedback" annotation(Placement(
		transformation(extent={{-60,-25},{-20,15}}),
		iconTransformation(extent={{-120,-70},{-80,-30}})));
	ControlPosition controlPosition1 annotation(Placement(transformation(extent={{-75,60},{-55,80}})));
	ControlSpeed controlSpeed1 annotation(Placement(transformation(extent={{-5,55},{15,75}})));
	ComputeThrustAndAttitudeConsign computeThrustAndAttitudeConsign1 annotation(Placement(transformation(extent={{25,50},{45,70}})));
	SelectSpeedConsign selectSpeedConsign1 annotation(Placement(transformation(extent={{-45,65},{-25,75}})));
	equation
		connect(computeThrustAndAttitudeConsign1.AttitudeAPConsigns,AttitudeConsignAP) annotation(Line(
			points={{44.7,64.7},{49.7,64.7},{76,64.7},{76,61},{81,61}},
			color={0,0,127},
			thickness=0.0625));
		connect(computeThrustAndAttitudeConsign1.TotalThrustAP, TotalThrustAP) annotation(
		  Line(points = {{45, 55}, {60, 55}, {60, 29}, {71, 29}}, color = {0, 0, 127}, thickness = 0.0625));
		connect(controlPosition1.DronePositionConsign,DronPositionConsign) annotation(Line(
			points={{-75,69.3},{-80,69.3},{-138,69.3},{-138,76},{-143,76}},
			color={0,0,127},
			thickness=0.0625));
		connect(selectSpeedConsign1.NavigationVelocityConsign,controlPosition1.NavigationSpeedConsign) annotation(Line(
			points={{-45,69.3},{-50,69.3},{-50,70.3},{-55,70.3}},
			color={0,0,127},
			thickness=0.0625));
		connect(controlSpeed1.SpeedConsign,selectSpeedConsign1.VelocityConsign) annotation(Line(
			points={{-5,69.3},{-10,69.3},{-20,69.3},{-20,69.7},{-25,69.7}},
			color={0,0,127},
			thickness=0.0625));
		connect(controlPosition1.Position,Position) annotation(Line(
			points={{-66,60},{-66,55},{-66,35},{-100,35},{-105,35}},
			color={0,0,127},
			thickness=0.0625));
		connect(controlSpeed1.Speed,Speed) annotation(Line(
			points={{-5,59.3},{-10,59.3},{-35,59.3},{-35,-5},{-40,-5}},
			color={0,0,127},
			thickness=0.0625));
		connect(computeThrustAndAttitudeConsign1.AccelerationConsign,controlSpeed1.AccelerationConsign) annotation(Line(
			points={{25,64.3},{20,64.3},{20,64.7},{15,64.7}},
			color={0,0,127},
			thickness=0.0625));
		connect(computeThrustAndAttitudeConsign1.Attitude,Attitude) annotation(Line(
			points={{25,54.3},{20,54.3},{10,54.3},{10,-5},{5,-5}},
			color={0,0,127},
			thickness=0.0625));
	annotation(
		DronPositionConsign(flags=2),
		Position(flags=2),
		TotalThrustAP(flags=2),
		AttitudeConsignAP(flags=2),
		Attitude(flags=2),
		Speed(flags=2),
		controlPosition1(
			DronePositionConsign(flags=2),
			Position(flags=2),
			NavigationSpeedConsign(flags=2),
			DHP(flags=2),
			HP(flags=2),
			DZP(flags=2),
			ZP(flags=2),
			HNSC(flags=2),
			ASC(flags=2)),
		controlSpeed1(
			SpeedConsign(flags=2),
			Speed(flags=2),
			AccelerationConsign(flags=2),
			speedErrorModel1(
				SpeedConsign(flags=2),
				Speed(flags=2),
				SpeedError(flags=2)),
			PID(
				u(flags=2),
				y(flags=2),
				P(
					u(flags=2),
					y(flags=2)),
				I(
					u(flags=2),
					y(flags=2)),
				D(
					u(flags=2),
					y(flags=2),
					x(flags=2)),
				Gain(
					u(flags=2),
					y(flags=2)),
				Add(
					u1(flags=2),
					u2(flags=2),
					u3(flags=2),
					y(flags=2))),
			PID2(
				u(flags=2),
				y(flags=2),
				P(
					u(flags=2),
					y(flags=2)),
				I(
					u(flags=2),
					y(flags=2)),
				D(
					u(flags=2),
					y(flags=2),
					x(flags=2)),
				Gain(
					u(flags=2),
					y(flags=2)),
				Add(
					u1(flags=2),
					u2(flags=2),
					u3(flags=2),
					y(flags=2))),
			PID1(
				u(flags=2),
				y(flags=2),
				P(
					u(flags=2),
					y(flags=2)),
				I(
					u(flags=2),
					y(flags=2)),
				D(
					u(flags=2),
					y(flags=2),
					x(flags=2)),
				Gain(
					u(flags=2),
					y(flags=2)),
				Add(
					u1(flags=2),
					u2(flags=2),
					u3(flags=2),
					y(flags=2)))),
		computeThrustAndAttitudeConsign1(
			AccelerationConsign(flags=2),
			Attitude(flags=2),
			AttitudeAPConsigns(flags=2),
			TotalThrustAP(flags=2),
			HAC(flags=2),
			Rpsi(flags=2),
			Apsi(flags=2),
			InvApsi(flags=2),
			AAC(flags=2)),
		selectSpeedConsign1(
			NavigationVelocityConsign(flags=2),
			VelocityConsign(flags=2)),
		Icon(graphics={
			Rectangle(
				fillColor={255,255,255},
				fillPattern=FillPattern.Solid,
				extent={{-100,100},{100,-100}}),
			Text(
				textString="%name",
				fillPattern=FillPattern.None,
				extent={{5,-7},{105,-31}},
				origin={-57,21})}),
		experiment(
			StopTime=1,
			StartTime=0,
			Interval=0.002,
			MaxInterval="0.001"));
end PositionControl;