165 lines
6.8 KiB
Plaintext
165 lines
6.8 KiB
Plaintext
|
within AIDAModelica;
|
||
|
|
|
||
|
|
model RemoteControl "RemoteControl"
|
||
|
|
// CP: 65001
|
||
|
|
// SimulationX Version: 3.8.2.45319 x64
|
||
|
|
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}}, initialScale = 0.1), graphics = {Rectangle(fillColor = {255, 255, 255}, fillPattern = FillPattern.Solid, extent = {{-123.3, 100}, {123.3, -100}}), Text(origin = {-12, 10}, extent = {{-78, 18}, {86, -20}}, textString = "RemoteControl")}),
|
||
|
|
experiment(StopTime = 10, StartTime = 0, Interval = 0.02, MaxInterval = "0.001"));
|
||
|
|
end RemoteControl;
|