within AIDAModelica;

model ComputeMotorRate "[SF2.5] Compute thrust"
  // CP: 65001
  // SimulationX Version: 3.8.2.45319 x64
  import Modelica.Math;
  Modelica.Blocks.Interfaces.RealInput TotalThrust(quantity = "Mechanics.Translation.Force", displayUnit = "N") "Desired total thrust " annotation(
    Placement(transformation(origin = {-120, 68}, extent = {{-20, -20}, {20, 20}}), iconTransformation(extent = {{-120, 55}, {-80, 95}})));
  Modelica.Blocks.Interfaces.RealInput MomentumConsign[3](each quantity = "Mechanics.Rotation.Torque", each displayUnit = "Nm") "Desired momentums consign" annotation(
    Placement(transformation(origin = {-120, -52}, 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(origin = {110, 84}, 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(origin = {110, 44}, 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(origin = {110, -36}, 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(origin = {110, -76}, 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](each quantity = "Mechanics.Rotation.RotVelocity", each 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;