You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
61 lines
2.5 KiB
61 lines
2.5 KiB
// 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;
|
|
|