|
|
|
|
|
// 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;
|
