215 lines
6.8 KiB
215 lines
6.8 KiB
within AIDAModelica; |
|
// CP: 65001 |
|
// SimulationX Version: 3.8.2.45319 x64 |
|
|
|
model RemoteControl "RemoteControl" |
|
Real VerticalSpeedCommand_i(start=0) "commande interne non filtree"; |
|
Modelica.Blocks.Interfaces.RealOutput VerticalSpeedCommand( |
|
quantity="Mechanics.Translation.Velocity", |
|
displayUnit="m/s") "'output Real' as connector" annotation(Placement( |
|
transformation(extent={{12,28},{32,48}}), |
|
iconTransformation( |
|
origin={-75,-100}, |
|
extent={{-10,-10},{10,10}}, |
|
rotation=-90))); |
|
Real RollCommand_i(start=0) "commande interne non filtree"; |
|
Modelica.Blocks.Interfaces.RealOutput RollCommand( |
|
quantity="Mechanics.Rotation.Angle", |
|
displayUnit="rad") "'output Real' as connector" annotation(Placement( |
|
transformation(extent={{56,8},{76,28}}), |
|
iconTransformation( |
|
origin={-25,-100}, |
|
extent={{-10,-10},{10,10}}, |
|
rotation=-90))); |
|
Real PitchCommand_i(start=0) "commande interne non filtree"; |
|
Modelica.Blocks.Interfaces.RealOutput PitchCommand( |
|
quantity="Mechanics.Rotation.Angle", |
|
displayUnit="rad") "'output Real' as connector" annotation(Placement( |
|
transformation(extent={{-38,-46},{-18,-26}}), |
|
iconTransformation( |
|
origin={25,-100}, |
|
extent={{-10,-10},{10,10}}, |
|
rotation=-90))); |
|
Real YawCommand_i(start=0) "commande interne non filtree"; |
|
Modelica.Blocks.Interfaces.RealOutput YawCommand( |
|
quantity="Mechanics.Rotation.Angle", |
|
displayUnit="rad") "'output Real' as connector" annotation(Placement( |
|
transformation(extent={{18,-52},{38,-32}}), |
|
iconTransformation( |
|
origin={75,-100}, |
|
extent={{-10,-10},{10,10}}, |
|
rotation=-90))); |
|
Modelica.Blocks.Interfaces.BooleanOutput ControlMode "'output Boolean' as connector" annotation(Placement( |
|
transformation(extent={{-10,-10},{10,10}}), |
|
iconTransformation( |
|
origin={-125,-50}, |
|
extent={{-10,-10},{10,10}}, |
|
rotation=180))); |
|
Modelica.Blocks.Interfaces.BooleanOutput IndicatorYawConsign "'output Boolean' as connector" annotation(Placement( |
|
transformation( |
|
origin={-45,45}, |
|
extent={{-17,-17},{17,17}}), |
|
iconTransformation( |
|
origin={-125,0}, |
|
extent={{-10,-10},{10,10}}, |
|
rotation=-180))); |
|
parameter Real VS_cmd=0.5; |
|
parameter Real[2] VS_cmd_t={3, 5} "Instants de démarrage et fin du step"; |
|
parameter Real Pitch_cmd=0.03; |
|
parameter Real[2] Pitch_cmd_t={1, 4} "Instants de démarrage et fin du step, après stabilisation verticale"; |
|
parameter Real Yaw_cmd=0.5; |
|
parameter Real[2] Yaw_cmd_t={6, 8} "Instants de démarrage et fin du step"; |
|
parameter Real Roll_cmd=0.2 "Amplitude step "; |
|
parameter Real Roll_cmd_t[2]={1, 100} "Instants de démarrage et fin du step"; |
|
parameter Real Tcst=0.1 "constante de temps pour filter les sorties du remote ctl"; |
|
Boolean Auto_Ctl_Enabled(start=true); |
|
parameter Real Auto_Ctl_t=0.1 "Instant de déclenchement du mode auto"; |
|
initial equation |
|
PitchCommand = 0; |
|
RollCommand = 0; |
|
YawCommand = 0; |
|
VerticalSpeedCommand = 0; |
|
PitchCommand_i = 0; |
|
RollCommand_i = 0; |
|
YawCommand_i = 0; |
|
VerticalSpeedCommand_i = 0; |
|
ControlMode = false; |
|
IndicatorYawConsign = false; |
|
Auto_Ctl_Enabled=true; |
|
algorithm |
|
|
|
when time > Auto_Ctl_t and Auto_Ctl_Enabled==true then |
|
ControlMode:=true; |
|
end when; |
|
|
|
when time > VS_cmd_t[1] then |
|
VerticalSpeedCommand_i := VS_cmd; |
|
ControlMode:=false; |
|
Auto_Ctl_Enabled:=false; |
|
end when; |
|
when time > VS_cmd_t[2] then |
|
VerticalSpeedCommand_i := 0; |
|
end when; |
|
when time > VS_cmd_t[2]+ Pitch_cmd_t[1] then |
|
PitchCommand_i := Pitch_cmd; |
|
ControlMode:=false; |
|
end when; |
|
when time > VS_cmd_t[2]+ Pitch_cmd_t[2] then |
|
PitchCommand_i := 0; |
|
end when; |
|
when time > VS_cmd_t[2]+ Roll_cmd_t[1] then |
|
RollCommand_i := Roll_cmd; |
|
ControlMode:=false; |
|
end when; |
|
when time > VS_cmd_t[2]+ Roll_cmd_t[2] then |
|
RollCommand_i := 0; |
|
end when; |
|
//Yaw command |
|
when time > VS_cmd_t[2]+ Yaw_cmd_t[1] then |
|
YawCommand_i := Yaw_cmd; |
|
ControlMode:=false; |
|
IndicatorYawConsign:=true; |
|
end when; |
|
//IndicatorYawConsign := true; |
|
when time > VS_cmd_t[2]+ Yaw_cmd_t[2] then |
|
YawCommand_i := 0; |
|
end when; |
|
//IndicatorYawConsign := false; |
|
/*if time>Yaw_cmd_t[2] then |
|
|
|
int_yaw := 0; |
|
IndicatorYawConsign := false; |
|
|
|
elseif time>Yaw_cmd_t[1] then |
|
|
|
int_yaw := DYaw_cmd; |
|
IndicatorYawConsign := true; |
|
|
|
else |
|
|
|
int_yaw := 0; |
|
IndicatorYawConsign := false; |
|
|
|
end if;*/ |
|
equation |
|
// remote control filter with Tcst time constant |
|
der(VerticalSpeedCommand) = (VerticalSpeedCommand_i - VerticalSpeedCommand) / Tcst; |
|
der(RollCommand) = (RollCommand_i - RollCommand) / Tcst; |
|
der(PitchCommand) = (PitchCommand_i - PitchCommand) / Tcst; |
|
der(YawCommand) = (YawCommand_i - YawCommand) / Tcst; |
|
/* initial code of Andrii VAKULKO, destined to be used with manual button included in the SimulationX interface |
|
if VSCommand1 > 0 then |
|
VerticalSpeedCommand = -1; |
|
elseif VSCommand2 > 0 then |
|
VerticalSpeedCommand = -0.8; |
|
elseif VSCommand3 > 0 then |
|
VerticalSpeedCommand = -0.6; |
|
elseif VSCommand4 > 0 then |
|
VerticalSpeedCommand = -0.4; |
|
elseif VSCommand5 > 0 then |
|
VerticalSpeedCommand = -0.2; |
|
elseif VSCommand6 > 0 then |
|
VerticalSpeedCommand = 0.2; |
|
elseif VSCommand7 > 0 then |
|
VerticalSpeedCommand = 0.4; |
|
elseif VSCommand8 > 0 then |
|
VerticalSpeedCommand = 0.6; |
|
elseif VSCommand9 > 0 then |
|
VerticalSpeedCommand = 0.8; |
|
elseif VSCommand10 > 0 then |
|
VerticalSpeedCommand = 1; |
|
else |
|
VerticalSpeedCommand = 0; |
|
end if; |
|
|
|
if YCommandP > 0 then |
|
der(YawCommand) = 0.01; |
|
//IndicatorYawConsign = true; |
|
elseif YCommandM > 0 then |
|
der(YawCommand) = -0.01; |
|
//IndicatorYawConsign = true; |
|
else |
|
YawCommand = 0; |
|
//IndicatorYawConsign = false; |
|
end if; |
|
|
|
if RCommandP > 0 then |
|
RollCommand = 0.0261799; //15 deg |
|
elseif RCommandM > 0 then |
|
RollCommand = -0.0261799; |
|
else |
|
RollCommand = 0; |
|
end if; |
|
|
|
if PCommandP > 0 then |
|
PitchCommand = 0.0261799; |
|
elseif PCommandM > 0 then |
|
PitchCommand = -0.0261799; |
|
else |
|
PitchCommand = 0; |
|
end if;*/ |
|
annotation( |
|
VerticalSpeedCommand(flags=2), |
|
RollCommand(flags=2), |
|
PitchCommand(flags=2), |
|
YawCommand(flags=2), |
|
ControlMode(flags=2), |
|
IndicatorYawConsign(flags=2), |
|
Icon( |
|
coordinateSystem(extent={{-125,-100},{125,100}}), |
|
graphics={ |
|
Rectangle( |
|
fillColor={255,255,255}, |
|
fillPattern=FillPattern.Solid, |
|
extent={{-123.3,100},{123.3,-100}}), |
|
Text( |
|
textString="%name", |
|
fillPattern=FillPattern.None, |
|
extent={{-86,20},{86,-20}}, |
|
origin={-2,6})}), |
|
experiment( |
|
StopTime=10, |
|
StartTime=0, |
|
Interval=0.02, |
|
MaxInterval="0.001")); |
|
end RemoteControl;
|
|
|