Migration to OpenModelica 1.25 and FMUs Generation

SimulationModels/OpenModelica 1.11/AIDAModelica/ComputeThrustAndAttitudeConsign.mo

@@ -0,0 +1,61 @@
+// 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;