// CP: 65001
// SimulationX Version: 3.8.2.45319 x64
within AIDAModelica;
model ComputeThrustAndAttitudeConsign "[SF2.1.4] Compute thrust and attitude consign"
	import Modelica.Math;
	Modelica.Blocks.Interfaces.RealInput AccelerationConsign[3](
		quantity="Mechanics.Translation.Accel",
		displayUnit="m/s²") "Acceleration consign" annotation(Placement(
		transformation(extent={{-20,-20},{20,20}}),
		iconTransformation(extent={{-120,30},{-80,70}})));
	Modelica.Blocks.Interfaces.RealInput Attitude[3](
		quantity="Mechanics.Rotation.Angle",
		displayUnit="rad") "Real drone attitude feedback " annotation(Placement(
		transformation(extent={{-20,-20},{20,20}}),
		iconTransformation(extent={{-120,-70},{-80,-30}})));
	Modelica.Blocks.Interfaces.RealOutput AttitudeAPConsigns[2](
		quantity="Mechanics.Rotation.Angle",
		displayUnit="rad") "Desired drone attitude autopilot mode consign" annotation(Placement(
		transformation(extent={{-10,-10},{10,10}}),
		iconTransformation(extent={{86.7,40},{106.7,60}})));
	Modelica.Blocks.Interfaces.RealOutput TotalThrustAP(
		quantity="Mechanics.Translation.Force",
		displayUnit="N") "Total thrust autopilot mode" annotation(Placement(
		transformation(extent={{-10,-10},{10,10}}),
		iconTransformation(extent={{90,-60},{110,-40}})));
	Real HAC[2](
		quantity="Mechanics.Translation.Velocity",
		displayUnit="m/s") "Horizontal acceleration consign";
	Real Rpsi[2,2] "Rpsi matrix";
	Real Apsi[2,2] "Apsi martix";
	Real InvApsi[2,2] "Inverse Apsi matrix";
	parameter Real ConstMatr[2,2]={{0, -1}, {1, 0}} "Constant Matrix";
	parameter Real g(
		quantity="Mechanics.Translation.Accel",
		displayUnit="m/s²")=9.8 "Parameter g";
	parameter Real m(
		quantity="Basics.Mass",
		displayUnit="kg")=1.8 "Parameter m";
	Real AAC(
		quantity="Mechanics.Translation.Velocity",
		displayUnit="m/s") "Altitude autopilot consign";
	equation
		// enter your equations here
		  HAC = {AccelerationConsign[1], AccelerationConsign[2]};
		  Rpsi = {{cos(Attitude[3]), -sin(Attitude[3])}, {sin(Attitude[3]), cos(Attitude[3])}};
		  Apsi = Rpsi * ConstMatr;
		  InvApsi = Modelica.Math.Matrices.inv(Apsi);
		  AttitudeAPConsigns = 1 / g * InvApsi * HAC;
		  AAC = AccelerationConsign[3];
		  TotalThrustAP = if AAC < 0 then -m * AAC else 0;
	annotation(Icon(graphics={
		Rectangle(
			fillColor={255,255,255},
			fillPattern=FillPattern.Solid,
			extent={{-100,100},{100,-100}}),
		Text(
			textString="%name",
			fillPattern=FillPattern.None,
			extent={{-31,10},{31,-10}},
			origin={-3,4})}));
end ComputeThrustAndAttitudeConsign;