within AIDAModelica;
// CP: 65001
// SimulationX Version: 3.8.2.45319 x64
model ComputeMotorRate "[SF2.5] Compute thrust"
	import Modelica.Math;
	Modelica.Blocks.Interfaces.RealInput TotalThrust(
		quantity="Mechanics.Translation.Force",
		displayUnit="N") "Desired total thrust " annotation(Placement(
		transformation(extent={{-20,-20},{20,20}}),
		iconTransformation(extent={{-120,55},{-80,95}})));
	Modelica.Blocks.Interfaces.RealInput MomentumConsign[3](
		quantity="Mechanics.Rotation.Torque",
		displayUnit="Nm") "Desired momentums consign" annotation(Placement(
		transformation(extent={{-20,-20},{20,20}}),
		iconTransformation(extent={{-120,-95},{-80,-55}})));
	Modelica.Blocks.Interfaces.RealOutput Motor1Rate(
		quantity="Mechanics.Rotation.RotVelocity",
		displayUnit="rpm") "Desired angular speed of 1 motor" annotation(Placement(
		transformation(extent={{-10,-10},{10,10}}),
		iconTransformation(extent={{90,65},{110,85}})));
	Modelica.Blocks.Interfaces.RealOutput Motor2Rate(
		quantity="Mechanics.Rotation.RotVelocity",
		displayUnit="rpm") "Desired angular speed of 2 motor" annotation(Placement(
		transformation(extent={{-10,-10},{10,10}}),
		iconTransformation(extent={{90,15},{110,35}})));
	Modelica.Blocks.Interfaces.RealOutput Motor3Rate(
		quantity="Mechanics.Rotation.RotVelocity",
		displayUnit="rpm") "Desired angular speed of 3 motor " annotation(Placement(
		transformation(extent={{-10,-10},{10,10}}),
		iconTransformation(extent={{90,-35},{110,-15}})));
	Modelica.Blocks.Interfaces.RealOutput Motor4Rate(
		quantity="Mechanics.Rotation.RotVelocity",
		displayUnit="rpm") "Desired angular speed of 4 motor" annotation(Placement(
		transformation(extent={{-10,-10},{10,10}}),
		iconTransformation(extent={{90,-85},{110,-65}})));
	Real M4[4,4] "Control effectiveness model matrix M";
	parameter Real d(
		quantity="Basics.Length",
		displayUnit="m")=0.33 "Parameter d";
	parameter Real cT(
		quantity="Mechanics.Translation.Force",
		displayUnit="N")=3.438e-7 "Parameter cT";
	parameter Real coef_cT(
		quantity="Mechanics.Rotation.RotVelocity",
		displayUnit="rpm")=0.10471975511965977 "coefficient  cT";
	parameter Real cM(
		quantity="Mechanics.Rotation.Torque",
		displayUnit="Nm")=5.7e-9 "Parameter cM";
	parameter Real coef_cM(
		quantity="Mechanics.Rotation.RotVelocity",
		displayUnit="rpm")=0.10471975511965977 "coefficient cM";
	Real AngVelVector[4](
		quantity="Mechanics.Rotation.RotVelocity",
		displayUnit="rpm") "Vector of desired angular velocities";
	Real TMomVector[4] "Totat thust and moments vector";
	Real ParamCosSin "Parameter Cos Sin 45";
	equation
		// enter your equations here
		  ParamCosSin = sqrt(2) / 2;
		  TMomVector = {TotalThrust, MomentumConsign[1], MomentumConsign[2], MomentumConsign[3]};
		  M4 = {{cT / coef_cT ^ 2, cT / coef_cT ^ 2, cT / coef_cT ^ 2, cT / coef_cT ^ 2}, {d * (cT / coef_cT ^ 2) * ParamCosSin, -d * (cT / coef_cT ^ 2) * ParamCosSin, -d * (cT / coef_cT ^ 2) * ParamCosSin, d * (cT / coef_cT ^ 2) * ParamCosSin}, {d * (cT / coef_cT ^ 2) * ParamCosSin, d * (cT / coef_cT ^ 2) * ParamCosSin, -d * (cT / coef_cT ^ 2) * ParamCosSin, -d * (cT / coef_cT ^ 2) * ParamCosSin}, {cM / coef_cM ^ 2, -cM / coef_cM ^ 2, cM / coef_cM ^ 2, -cM / coef_cM ^ 2}};
		  M4*AngVelVector = TMomVector;
		  Motor1Rate = sqrt(abs(AngVelVector[1]));
		  Motor2Rate = sqrt(abs(AngVelVector[2]));
		  Motor3Rate = sqrt(abs(AngVelVector[3]));
		  Motor4Rate = sqrt(abs(AngVelVector[4]));
	annotation(Icon(
		coordinateSystem(extent={{-100,-125},{100,125}}),
		graphics={
			Rectangle(
				fillColor={255,255,255},
				fillPattern=FillPattern.Solid,
				extent={{-100,126.7},{103.3,-126.7}}),
			Text(
				textString="%name",
				fillPattern=FillPattern.None,
				extent={{-39,15},{39,-15}},
				origin={-9,5})}));
end ComputeMotorRate;