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;