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.
50 lines
2.1 KiB
50 lines
2.1 KiB
// CP: 65001 |
|
// SimulationX Version: 3.8.2.45319 x64 |
|
within AIDAModelica; |
|
model ControlPositionSAC "[SF2.1.1] Control position SAC" |
|
Modelica.Blocks.Interfaces.RealInput DronePositionConsign[3]( |
|
quantity="Mechanics.Translation.Displace", |
|
displayUnit="m") "Desired drone position consign" annotation(Placement( |
|
transformation(extent={{-20,-20},{20,20}}), |
|
iconTransformation(extent={{-120,-20},{-80,20}}))); |
|
Modelica.Blocks.Interfaces.RealInput Position[3]( |
|
quantity="Mechanics.Translation.Displace", |
|
displayUnit="m") "Real drone position feedback" annotation(Placement( |
|
transformation(extent={{-20,-20},{20,20}}), |
|
iconTransformation(extent={{-120,-70},{-80,-30}}))); |
|
Modelica.Blocks.Interfaces.RealOutput NavigationSpeedConsign[3]( |
|
quantity="Mechanics.Translation.Velocity", |
|
displayUnit="m/s") "Desired drone speed consign" annotation(Placement( |
|
transformation(extent={{-10,-10},{10,10}}), |
|
iconTransformation(extent={{90,-10},{110,10}}))); |
|
Modelica.Blocks.Interfaces.RealInput RCVErticalSpeedCommand( |
|
quantity="Mechanics.Translation.Velocity", |
|
displayUnit="m/s") "RC vertical speed consign" annotation(Placement( |
|
transformation(extent={{-20,-20},{20,20}}), |
|
iconTransformation(extent={{-120,30},{-80,70}}))); |
|
Real DHP[2]( |
|
quantity="Mechanics.Translation.Displace", |
|
displayUnit="m") "Desired horizontal position of the drone"; |
|
Real HP[2]( |
|
quantity="Mechanics.Translation.Displace", |
|
displayUnit="m") "Real horizontal position of the drone"; |
|
Real HNSC[2]( |
|
quantity="Mechanics.Translation.Velocity", |
|
displayUnit="m/s") "Horizontal Navigation Speed Consign"; |
|
Real ASC( |
|
quantity="Mechanics.Translation.Velocity", |
|
displayUnit="m/s") "AltitudeSpeedConsign"; |
|
parameter Real Kph[2,2]={{1,0},{0,1}} "Proportional regulator gain attitude chanel"; |
|
equation |
|
// enter your equations here |
|
DHP = {DronePositionConsign[1],DronePositionConsign[2]}; |
|
HP = {Position[1],Position[2]}; |
|
HNSC = Kph*(DHP-HP); |
|
ASC = RCVErticalSpeedCommand; |
|
NavigationSpeedConsign = {HNSC[1],HNSC[2],ASC}; |
|
annotation(Icon(graphics={ |
|
Rectangle( |
|
fillColor={255,255,255}, |
|
fillPattern=FillPattern.Solid, |
|
extent={{-100,100},{100,-100}})})); |
|
end ControlPositionSAC;
|
|
|