within AIDAModelica;

model AttitudeControl "Attitude Control Model"
  // CP: 65001
  // SimulationX Version: 3.8.2.45319 x64
  parameter Boolean Test_CstMomentumActiv = false;
  parameter Boolean Test_open_loop[3] = {false, false, false} "parameter";
  parameter Real Test_CstMomentumValues[3] = {0.0, 0.0, 0.0};
  Modelica.Blocks.Interfaces.RealInput AttitudeConsignAP[2](quantity = "Mechanics.Rotation.Angle", displayUnit = "rad") "Desired phi and teta angles input" annotation(
    Placement(visible = true, transformation(extent = {{-166, 23}, {-126, 63}}, rotation = 0), iconTransformation(origin = {-50, 100}, extent = {{-20, -20}, {20, 20}}, rotation = 270)));
  Modelica.Blocks.Interfaces.RealInput AttitudeRCConsign[2](quantity = "Mechanics.Rotation.Angle", displayUnit = "rad") "Desired phi and teta angles input sac" annotation(
    Placement(visible = true,transformation(extent = {{-163, -45}, {-123, -5}}, rotation = 0), iconTransformation(extent = {{-120, -20}, {-80, 20}}, rotation = 0)));
  Modelica.Blocks.Interfaces.RealInput YawConsign(quantity = "Mechanics.Rotation.Angle", displayUnit = "rad") "Desired yaw angle input" annotation(
    Placement(transformation(extent = {{-80, -70}, {-40, -30}}), iconTransformation(extent = {{-120, -70}, {-80, -30}})));
  Modelica.Blocks.Interfaces.RealInput AngularSpeed[3](quantity = "Mechanics.Rotation.RotVelocity", displayUnit = "rad/s") "Real angular velocities feedback input" annotation(
    Placement(transformation(extent = {{-89.7, 58}, {-49.7, 98}}), iconTransformation(origin = {-50, -100}, extent = {{-20, -20}, {20, 20}}, rotation = 90)));
  Modelica.Blocks.Interfaces.RealInput Attitude[3](quantity = "Mechanics.Rotation.Angle", displayUnit = "rad") "Real attitude feedback input" annotation(
    Placement(transformation(extent = {{-40, 45}, {0, 85}}), iconTransformation(origin = {0, -100}, extent = {{-20, -20}, {20, 20}}, rotation = 90)));
  Modelica.Blocks.Interfaces.RealOutput MomentumsConsign[3](quantity = "Mechanics.Rotation.Torque", displayUnit = "Nm") "Desired moments output" annotation(
    Placement(transformation(extent = {{90, -15}, {110, 5}}), iconTransformation(extent = {{90, -10}, {110, 10}})));
  Modelica.Blocks.Interfaces.RealInput RCYawCommand(quantity = "Mechanics.Rotation.Angle", displayUnit = "rad") "RC yaw angle input" annotation(
    Placement(transformation(extent = {{-80, -100}, {-40, -60}}), iconTransformation(origin = {-100, 50}, extent = {{-20, -20}, {20, 20}})));
  Modelica.Blocks.Interfaces.BooleanInput SelectedControlMode "'input Boolean' as connector" annotation(
    Placement(visible = true, transformation(extent = {{-164, -11}, {-124, 29}}, rotation = 0), iconTransformation(origin = {50, -100}, extent = {{-20, -20}, {20, 20}}, rotation = -270)));
  AIDAModelica.ControlRollAngle controlRollAngle1(xAngularSpeedErrorModel1(Kwphi = 3), PID(k = 0.09, Ti = 1.5, Td = 0.03, Add(k1 = 1, k2 = 1, k3 = 1))) annotation(
    Placement(visible = true, transformation(extent = {{49, -8}, {69, 12}}, rotation = 0)));
  AIDAModelica.ControlPitchAngle controlPitchAngle1(yAngularSpeedErrorModel1(Kwteta = 1.5), PID1(Add(k1 = 1, k2 = 1, k3 = 1))) annotation(
    Placement(visible = true, transformation(extent = {{49, 20}, {69, 40}}, rotation = 0)));
  AIDAModelica.ControlYawAngle controlYawAngle1(PID2(k = 0.05, Ti = 2, Td = 0.03, Add(k1 = 1, k2 = 1, k3 = 1))) annotation(
    Placement(visible = true, transformation(extent = {{48, -37}, {68, -17}}, rotation = 0)));
  Modelica.Blocks.Logical.Switch SelectRollConsign annotation(
    Placement(visible = true, transformation(extent = {{-56, -20}, {-36, 0}}, rotation = 0)));
  Modelica.Blocks.Logical.Switch SelectPitchConsign annotation(
    Placement(visible = true, transformation(extent = {{-56, 17}, {-36, 37}}, rotation = 0)));
  Modelica.Blocks.Math.Add computeYawConsign annotation(
    Placement(transformation(extent = {{-35, -75}, {-15, -55}})));
equation
  connect(SelectRollConsign.u1, AttitudeConsignAP[1]) annotation(
    Line(points = {{-58, -2}, {-106, -2}, {-106, 43}, {-146, 43}}, color = {0, 0, 127}, thickness = 0.0625));
  connect(SelectRollConsign.u2, SelectedControlMode) annotation(
    Line(points = {{-58, -10}, {-78, -10}, {-78, 9}, {-144, 9}}, color = {255, 0, 255}, thickness = 0.0625));
  connect(SelectRollConsign.u3, AttitudeRCConsign[1]) annotation(
    Line(points = {{-58, -18}, {-94, -18}, {-94, -25}, {-143, -25}}, color = {0, 0, 127}, thickness = 0.0625));
  connect(controlRollAngle1.RollConsign, SelectRollConsign.y) annotation(
    Line(points = {{49, 7}, {8, 7}, {8, -10}, {-35, -10}}, color = {0, 0, 127}));
  connect(SelectPitchConsign.u1, AttitudeConsignAP[2]) annotation(
    Line(points = {{-58, 35}, {-106, 35}, {-106, 43}, {-146, 43}}, color = {0, 0, 127}, thickness = 0.0625));
  connect(SelectPitchConsign.u2, SelectedControlMode) annotation(
    Line(points = {{-58, 27}, {-78, 27}, {-78, 9}, {-144, 9}}, color = {255, 0, 255}, thickness = 0.0625));
  connect(SelectPitchConsign.u3, AttitudeRCConsign[2]) annotation(
    Line(points = {{-58, 19}, {-94, 19}, {-94, -25}, {-143, -25}}, color = {0, 0, 127}, thickness = 0.0625));
  connect(controlPitchAngle1.PitchCosign, SelectPitchConsign.y) annotation(
    Line(points = {{49, 35}, {22, 35}, {22, 27}, {-35, 27}}, color = {0, 0, 127}));
  connect(controlYawAngle1.YawConsign, computeYawConsign.y) annotation(
    Line(points = {{48, -22}, {-9, -22}, {-9, -65}, {-14, -65}}, color = {0, 0, 127}));
// la sortie est soit celle caclulée par le contrôleur soit fixée selon la valeur du paramètre Test_CstMomentumActiv et Test_CstMomentumValues
  MomentumsConsign[3] = if Test_CstMomentumActiv then Test_CstMomentumValues[3] else controlYawAngle1.MomentumZ;
  MomentumsConsign[2] = if Test_CstMomentumActiv then Test_CstMomentumValues[2] else controlPitchAngle1.MomentumY;
  MomentumsConsign[1] = if Test_CstMomentumActiv then Test_CstMomentumValues[1] else controlRollAngle1.MomentumX;
  controlRollAngle1.AngularSpeedX = if Test_open_loop[1] then 0 else AngularSpeed[1] annotation(
    Line(points = {{36, 18}, {-25, 18}, {-25, 35}, {-30, 35}}, color = {0, 0, 127}, thickness = 0.0625));
  controlRollAngle1.Roll = if Test_open_loop[1] then 0 else Attitude[1] annotation(
    Line(points = {{36, 23}, {4, 23}, {4, 36}, {-4, 36}}, color = {0, 0, 127}));
  controlYawAngle1.AngularSpeedZ = if Test_open_loop[3] then 0 else AngularSpeed[3] annotation(
    Line(points = {{36, -34}, {-25, -34}, {-25, 35}, {-30, 35}}, color = {0, 0, 127}, thickness = 0.0625));
  controlYawAngle1.Yaw = if Test_open_loop[3] then 0 else Attitude[3] annotation(
    Line(points = {{36, -29}, {2, -29}, {2, 36}, {-4, 36}}, color = {0, 0, 127}));
  controlPitchAngle1.AngularSpeedY = if Test_open_loop[2] then 0 else AngularSpeed[2] annotation(
    Line(points = {{36, -8}, {-25, -8}, {-25, 35}, {-30, 35}}, color = {0, 0, 127}, thickness = 0.0625));
  controlPitchAngle1.Pitch = if Test_open_loop[2] then 0 else Attitude[2] annotation(
    Line(points = {{36, -3}, {2, -3}, {2, 36}, {-4, 36}}, color = {0, 0, 127}));
  connect(computeYawConsign.u2, RCYawCommand) annotation(
    Line(points = {{-37, -71}, {-42, -71}, {-55, -71}, {-55, -80}, {-60, -80}}, color = {0, 0, 127}, thickness = 0.0625));
  connect(computeYawConsign.u1, YawConsign) annotation(
    Line(points = {{-37, -59}, {-42, -59}, {-55, -59}, {-55, -50}, {-60, -50}}, color = {0, 0, 127}, thickness = 0.0625));
  annotation(
    AttitudeConsignAP(flags = 2),
    AttitudeRCConsign(flags = 2),
    YawConsign(flags = 2),
    AngularSpeed(flags = 2),
    Attitude(flags = 2),
    MomentumsConsign(flags = 2),
    RCYawCommand(flags = 2),
    SelectedControlMode(flags = 2),
    controlRollAngle1(RollConsign(flags = 2), Roll(flags = 2), AngularSpeedX(flags = 2), MomentumX(flags = 2), xAngularSpeedErrorModel1(RollConsign(flags = 2), Roll(flags = 2), AngularSpeedX(flags = 2), AngularSpeedXError(flags = 2), ephi(flags = 2), wxd(flags = 2)), PID(u(flags = 2), y(flags = 2), P(u(flags = 2), y(flags = 2)), I(u(flags = 2), y(flags = 2)), D(u(flags = 2), y(flags = 2), x(flags = 2)), Gain(u(flags = 2), y(flags = 2)), Add(u1(flags = 2), u2(flags = 2), u3(flags = 2), y(flags = 2)))),
    controlPitchAngle1(PitchCosign(flags = 2), Pitch(flags = 2), AngularSpeedY(flags = 2), MomentumY(flags = 2), yAngularSpeedErrorModel1(PitchCosign(flags = 2), Pitch(flags = 2), AngularSpeedY(flags = 2), AngularSpeedYError(flags = 2), eteta(flags = 2), wyd(flags = 2)), PID1(u(flags = 2), y(flags = 2), P(u(flags = 2), y(flags = 2)), I(u(flags = 2), y(flags = 2)), D(u(flags = 2), y(flags = 2), x(flags = 2)), Gain(u(flags = 2), y(flags = 2)), Add(u1(flags = 2), u2(flags = 2), u3(flags = 2), y(flags = 2)))),
    controlYawAngle1(YawConsign(flags = 2), Yaw(flags = 2), AngularSpeedZ(flags = 2), MomentumZ(flags = 2), zAngularSpeedErrorModel1(YawCosign(flags = 2), Yaw(flags = 2), AngularSpeedZ(flags = 2), AngularSpeedZError(flags = 2), eyaw(flags = 2), wzd(flags = 2)), PID2(u(flags = 2), y(flags = 2), P(u(flags = 2), y(flags = 2)), I(u(flags = 2), y(flags = 2)), D(u(flags = 2), y(flags = 2), x(flags = 2)), Gain(u(flags = 2), y(flags = 2)), Add(u1(flags = 2), u2(flags = 2), u3(flags = 2), y(flags = 2)))),
    SelectRollConsign(u1(flags = 2), u2(flags = 2), u3(flags = 2), y(flags = 2)),
    SelectPitchConsign(u1(flags = 2), u2(flags = 2), u3(flags = 2), y(flags = 2)),
    computeYawConsign(u1(flags = 2), u2(flags = 2), y(flags = 2)),
    Icon(graphics = {Rectangle(fillColor = {255, 255, 255}, fillPattern = FillPattern.Solid, extent = {{-100, 100}, {100, -100}}), Text(textString = "Control Attitude", fillPattern = FillPattern.None, extent = {{-29, 11}, {29, -11}}, origin = {1, 3})}),
    experiment(StopTime = 30, StartTime = 0, Interval = 0.002, Tolerance = 1e-06),
  __OpenModelica_simulationFlags(jacobian = "coloredNumerical", s = "dassl", lv = "LOG_STATS"));
end AttitudeControl;