80 lines
3.7 KiB
Plaintext
80 lines
3.7 KiB
Plaintext
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;
|