// CP: 65001
// SimulationX Version: 3.8.2.45319 x64
within AIDAModelica;
model AttitudeControl_PID2 "Attitude Control Model_PID2"
	parameter Real k=1;
	parameter Real Ti=10000;
	parameter Real Td=0.0001;
	Modelica.Blocks.Interfaces.RealInput AttitudeConsignAP[2](
		quantity="Mechanics.Rotation.Angle",
		displayUnit="rad") "Desired phi and teta angles input" annotation(Placement(
		transformation(extent={{-115,5},{-75,45}}),
		iconTransformation(
			origin={-50,100},
			extent={{-20,-20},{20,20}},
			rotation=270)));
	Modelica.Blocks.Interfaces.RealInput AttitudeRCConsign[2](
		quantity="Mechanics.Rotation.Angle",
		displayUnit="rad") "Desired phi and teta angles input sac" annotation(Placement(
		transformation(extent={{-115,-55},{-75,-15}}),
		iconTransformation(extent={{-120,-20},{-80,20}})));
	Modelica.Blocks.Interfaces.RealInput YawConsign(
		quantity="Mechanics.Rotation.Angle",
		displayUnit="rad") "Desired yaw angle input" annotation(Placement(
		transformation(extent={{-80,-70},{-40,-30}}),
		iconTransformation(extent={{-120,-70},{-80,-30}})));
	Modelica.Blocks.Interfaces.RealInput AngularSpeed[3](
		quantity="Mechanics.Rotation.RotVelocity",
		displayUnit="rad/s") "Real angular velocities feedback input" annotation(Placement(
		transformation(extent={{-50,15},{-10,55}}),
		iconTransformation(
			origin={-50,-100},
			extent={{-20,-20},{20,20}},
			rotation=90)));
	Modelica.Blocks.Interfaces.RealInput Attitude[3](
		quantity="Mechanics.Rotation.Angle",
		displayUnit="rad") "Real attitude feedback input" annotation(Placement(
		transformation(extent={{-25,15},{15,55}}),
		iconTransformation(
			origin={0,-100},
			extent={{-20,-20},{20,20}},
			rotation=90)));
	Modelica.Blocks.Interfaces.RealOutput MomentumsConsign[3](
		quantity="Mechanics.Rotation.Torque",
		displayUnit="Nm") "Desired moments output" annotation(Placement(
		transformation(extent={{90,-15},{110,5}}),
		iconTransformation(extent={{90,-10},{110,10}})));
	Modelica.Blocks.Interfaces.RealInput RCYawCommand(
		quantity="Mechanics.Rotation.Angle",
		displayUnit="rad") "RC yaw angle input" annotation(Placement(
		transformation(extent={{-80,-100},{-40,-60}}),
		iconTransformation(
			origin={-100,50},
			extent={{-20,-20},{20,20}})));
	Modelica.Blocks.Interfaces.BooleanInput SelectedControlMode "'input Boolean' as connector" annotation(Placement(
		transformation(extent={{-115,-25},{-75,15}}),
		iconTransformation(
			origin={50,-100},
			extent={{-20,-20},{20,20}},
			rotation=-270)));
	PID_2 controlRollAngle1(
		k=k,
		Ti=Ti,
		Td=Td) annotation(Placement(transformation(extent={{38,13},{58,33}})));
	PID_2 controlPitchAngle1(
		k=k,
		Ti=Ti,
		Td=Td) annotation(Placement(transformation(extent={{48,-15},{68,5}})));
	PID_2 controlYawAngle1(
		k=k,
		Ti=Ti,
		Td=Td) annotation(Placement(transformation(extent={{58,-39},{78,-19}})));
	Modelica.Blocks.Logical.Switch SelectRollConsign annotation(Placement(transformation(extent={{-60,0},{-40,20}})));
	Modelica.Blocks.Logical.Switch SelectPitchConsign annotation(Placement(transformation(extent={{-60,-30},{-40,-10}})));
	Modelica.Blocks.Math.Add add1 annotation(Placement(transformation(extent={{-35,-75},{-15,-55}})));
	equation
		connect(add1.y, controlYawAngle1.Consign) annotation(
		  Line(points = {{-14, -65}, {55, -65}, {55, -27}, {59, -27}}, color = {0, 0, 127}, thickness = 0.0625));
		connect(controlYawAngle1.y, MomentumsConsign[3]) annotation(
		  Line(points = {{79, -29}, {95, -29}, {95, -5}, {100, -5}}, color = {0, 0, 127}, thickness = 0.0625));
		connect(controlYawAngle1.du, AngularSpeed[3]) annotation(
		  Line(points = {{59, -33}, {-25, -33}, {-25, 35}, {-30, 35}}, color = {0, 0, 127}, thickness = 0.0625));
		connect(controlYawAngle1.u, Attitude[3]) annotation(
		  Line(points = {{58, -23}, {0, -23}, {0, 35}, {-5, 35}}, color = {0, 0, 127}, thickness = 0.0625));
		connect(SelectRollConsign.y, controlRollAngle1.Consign) annotation(
		  Line(points = {{-39, 10}, {15, 10}, {15, 25}, {39, 25}}, color = {0, 0, 127}, thickness = 0.0625));
		connect(controlRollAngle1.y, MomentumsConsign[1]) annotation(
		  Line(points = {{59, 23}, {95, 23}, {95, -5}, {100, -5}}, color = {0, 0, 127}, thickness = 0.0625));
		connect(controlRollAngle1.du, AngularSpeed[1]) annotation(
		  Line(points = {{39, 19}, {-25, 19}, {-25, 35}, {-30, 35}}, color = {0, 0, 127}, thickness = 0.0625));
		connect(controlRollAngle1.u, Attitude[1]) annotation(
		  Line(points = {{38, 29}, {0, 29}, {0, 35}, {-5, 35}}, color = {0, 0, 127}, thickness = 0.0625));
		connect(SelectPitchConsign.y, controlPitchAngle1.Consign) annotation(
		  Line(points = {{-39, -20}, {35, -20}, {35, -3}, {49, -3}}, color = {0, 0, 127}, thickness = 0.0625));
		connect(controlPitchAngle1.y, MomentumsConsign[2]) annotation(
		  Line(points = {{69, -5}, {100, -5}}, color = {0, 0, 127}, thickness = 0.0625));
		connect(controlPitchAngle1.du, AngularSpeed[2]) annotation(
		  Line(points = {{49, -9}, {-25, -9}, {-25, 35}, {-30, 35}}, color = {0, 0, 127}, thickness = 0.0625));
		//controlPitchAngle1.u=0; en boucle ouverte : Ki=100 s-1, tau=0.1s
		connect(controlPitchAngle1.u, Attitude[2]) annotation(
		
		 Line(points = {{48, 1}, {0, 1}, {0, 35}, {-5, 35}}, color = {0, 0, 127}, thickness = 0.0625));
		connect(SelectRollConsign.u3, AttitudeRCConsign[1]) annotation(
		  Line(points = {{-62, 2}, {-67, 2}, {-90, 2}, {-90, -35}, {-95, -35}}, color = {0, 0, 127}, thickness = 0.0625));
		connect(SelectRollConsign.u2, SelectedControlMode) annotation(
		  Line(points = {{-62, 10}, {-67, 10}, {-90, 10}, {-90, -5}, {-95, -5}}, color = {255, 0, 255}, thickness = 0.0625));
		connect(SelectRollConsign.u1, AttitudeConsignAP[1]) annotation(
		  Line(points = {{-62, 18}, {-67, 18}, {-90, 18}, {-90, 25}, {-95, 25}}, color = {0, 0, 127}, thickness = 0.0625));
		connect(SelectPitchConsign.u3, AttitudeRCConsign[2]) annotation(
		  Line(points = {{-62, -28}, {-67, -28}, {-90, -28}, {-90, -35}, {-95, -35}}, color = {0, 0, 127}, thickness = 0.0625));
		connect(SelectPitchConsign.u2, SelectedControlMode) annotation(
		  Line(points = {{-62, -20}, {-67, -20}, {-90, -20}, {-90, -5}, {-95, -5}}, color = {255, 0, 255}, thickness = 0.0625));
		connect(SelectPitchConsign.u1, AttitudeConsignAP[2]) annotation(
		  Line(points = {{-62, -12}, {-67, -12}, {-90, -12}, {-90, 25}, {-95, 25}}, color = {0, 0, 127}, thickness = 0.0625));
		connect(add1.u1, YawConsign) annotation(
		  Line(points = {{-37, -59}, {-42, -59}, {-55, -59}, {-55, -50}, {-60, -50}}, color = {0, 0, 127}, thickness = 0.0625));
		connect(add1.u2, RCYawCommand) annotation(
		  Line(points = {{-37, -71}, {-42, -71}, {-55, -71}, {-55, -80}, {-60, -80}}, color = {0, 0, 127}, thickness = 0.0625));
	annotation(
		AttitudeConsignAP(flags=2),
		AttitudeRCConsign(flags=2),
		YawConsign(flags=2),
		AngularSpeed(flags=2),
		Attitude(flags=2),
		MomentumsConsign(flags=2),
		RCYawCommand(flags=2),
		SelectedControlMode(flags=2),
		SelectRollConsign(
			u1(flags=2),
			u2(flags=2),
			u3(flags=2),
			y(flags=2)),
		SelectPitchConsign(
			u1(flags=2),
			u2(flags=2),
			u3(flags=2),
			y(flags=2)),
		add1(
			u1(flags=2),
			u2(flags=2),
			y(flags=2)),
		Icon(graphics={
			Rectangle(
				fillColor={255,255,255},
				fillPattern=FillPattern.Solid,
				extent={{-100,100},{100,-100}})}),
		experiment(
			StopTime=1,
			StartTime=0,
			Interval=0.002,
			MaxInterval="0.001"));
end AttitudeControl_PID2;