AIDASimulation/SimulationModels/AIDAModel-integrationProSivic_reference.mo

model AIDA_System "AIDAModel-integrationProSivic_reference.isx"
AIDAModelica.QuadcopterModel quadcopterModel1(
  rigidBodyKinematicModel1(
    computeDronePosition1(
      integrator10(initType = Modelica.Blocks.Types.Init.SteadyState, y_start = -0.29), 
      integrator11(initType = Modelica.Blocks.Types.Init.InitialOutput), 
      integrator12(initType = Modelica.Blocks.Types.Init.InitialOutput))), 
  rigidBodyDynamicModel1(
    computeDroneVelocity1(
      integrator4(initType = Modelica.Blocks.Types.Init.SteadyState)))) annotation(
    Placement(visible = true, transformation(extent = {{29, -8}, {49, 17}}, rotation = 0)));
  
AIDAModelica.LowLevelFlightControlSystem lowLevelFlightControlSystem1(
  controlAltitude1(computeAltitudeConsign1(
    PID1(k = 30, Ti = 1, Td = 0.1, Nd = 10, initType=Modelica.Blocks.Types.InitPID.InitialOutput)), 
      pID_2(k = 25, Ti = 0.5, Td = 0.08, initType=Modelica.Blocks.Types.InitPID.InitialOutput)), 
  attitudeControl1(Test_CstMomentumActiv = false, Test_open_loop = {false, false, false},
    controlRollAngle1(xAngularSpeedErrorModel1(Kwphi = 1.5), 
      PID(k = 0.07, Ti = 30, Td = 0.02, Nd = 0.1,initType=Modelica.Blocks.Types.InitPID.InitialOutput, 
      Add(k1 = 1, k2 = 1, k3 = 1))), 
    controlPitchAngle1(yAngularSpeedErrorModel1(Kwteta = 1.5, RTStepConsign = false), 
      PID1(k = 0.07, Ti = 30, Td = 0.02, Nd = 0.1,initType=Modelica.Blocks.Types.InitPID.InitialOutput, 
      Add(k1 = 1, k2 = 1, k3 = 1))), 
    controlYawAngle1(zAngularSpeedErrorModel1(Kwyaw = 2), 
      PID2(k = 0.04, Ti = 20, Td = 0.5, Nd = 2.5,initType=Modelica.Blocks.Types.InitPID.InitialOutput, 
      Add(k1 = 1, k2 = 1, k3 = 1)))), 

  positionControl1(
    controlSpeed1(
      PID(k = 0.8, Ti = 1,initType=Modelica.Blocks.Types.InitPID.InitialOutput),
      PID2(k = 0.8, Ti = 1,initType=Modelica.Blocks.Types.InitPID.InitialOutput),
      PID1(k = 30, Ti = 1, Td=0.1,initType=Modelica.Blocks.Types.InitPID.InitialOutput)))) annotation(
    Placement(visible = true, transformation(extent = {{-37, -6}, {-12, 19}}, rotation = 0)));
  AIDAModelica.ControlDroneNavigation controlDroneNavigation1 annotation(
    Placement(visible = true, transformation(extent = {{-79, -3}, {-59, 17}}, rotation = 0)));
  AIDAModelica.RemoteControl remoteControl1(
    VS_cmd=-0.5,
	VS_cmd_t={0.1,5},
	Pitch_cmd=0.02,
	Pitch_cmd_t={6,12},
	Yaw_cmd_t={15,19},
	Roll_cmd=0.02,
	Roll_cmd_t={3,9},
	Auto_Ctl_t=0.2) annotation(
    Placement(visible = true, transformation(extent = {{-57, 49}, {-32, 69}}, rotation = 0)));
equation
  connect(lowLevelFlightControlSystem1.AngularSpeed, quadcopterModel1.AngularVelocities) annotation(
    Line(points = {{-33, -6}, {-33, -30}, {81.1667, -30}, {81.1667, 12}, {49.1667, 12}}, color = {0, 0, 127}));
  connect(lowLevelFlightControlSystem1.Attitude, quadcopterModel1.Attitude) annotation(
    Line(points = {{-29, -6}, {-29, -26}, {69.3333, -26}, {69.3333, 8}, {49.3333, 8}}, color = {0, 0, 127}));
  connect(lowLevelFlightControlSystem1.Position, quadcopterModel1.Position) annotation(
    Line(points = {{-24.5, -6}, {-24.5, -22}, {61.5, -22}, {61.5, 4}, {49.5, 4}}, color = {0, 0, 127}));
  connect(lowLevelFlightControlSystem1.Speed, quadcopterModel1.Speed) annotation(
    Line(points = {{-21, -6}, {-17.6667, -6}, {-17.6667, -18}, {57.3333, -18}, {57.3333, 0}, {49.3333, 0}}, color = {0, 0, 127}));
  connect(lowLevelFlightControlSystem1.Accelerations, quadcopterModel1.Accelerations) annotation(
    Line(points = {{-17, -6}, {-13.8333, -6}, {-13.8333, -16.0333}, {54.9167, -16.0333}, {54.9167, -4.0333}, {48.4167, -4.0333}}, color = {0, 0, 127}));
  connect(quadcopterModel1.ThrottleCommand1, lowLevelFlightControlSystem1.ThrottleCommand1) annotation(
    Line(points = {{29, 10.75}, {1, 10.75}, {1, 14}, {-12, 14}}, color = {0, 0, 127}, thickness = 0.015625));
  connect(lowLevelFlightControlSystem1.ThrottleCommand2, quadcopterModel1.ThrottleCommand2) annotation(
    Line(points = {{-12, 9}, {8.5, 9}, {8.5, 7}, {29, 7}}, color = {0, 0, 127}, thickness = 0.015625));
  connect(quadcopterModel1.ThrottleCommand3, lowLevelFlightControlSystem1.ThrottleCommand3) annotation(
    Line(points = {{29, 2.41667}, {12.5, 2.41667}, {12.5, 4}, {-12, 4}}, color = {0, 0, 127}, thickness = 0.015625));
  connect(lowLevelFlightControlSystem1.ThrottleCommand4, quadcopterModel1.ThrottleCommand4) annotation(
    Line(points = {{-12, -1}, {26, -1}, {26, -2}, {29, -2}}, color = {0, 0, 127}, thickness = 0.015625));
  connect(quadcopterModel1.Speed[:], lowLevelFlightControlSystem1.Speed[:]) annotation(
    Line);
  connect(controlDroneNavigation1.YawConsign, lowLevelFlightControlSystem1.YawConsign) annotation(
    Line(points = {{-59, 7}, {-34, 7}, {-34, 4}, {-37, 4}}, color = {0, 0, 127}, thickness = 0.0625));
  connect(controlDroneNavigation1.DronPositionConsign[:], lowLevelFlightControlSystem1.DronePositionConsign[:]) annotation(
    Line(points = {{-59, 12}, {-48, 12}, {-48, 14}, {-37, 14}}, color = {0, 0, 127}, thickness = 0.0625));
  connect(lowLevelFlightControlSystem1.SelectedControlMode, controlDroneNavigation1.SelectedControlMode) annotation(
    Line(points = {{-37, -1}, {-54, -1}, {-54, 2}, {-59, 2}}, color = {255, 0, 255}, thickness = 0.0625));
  connect(lowLevelFlightControlSystem1.RCVerticalSpeedCommand, remoteControl1.VerticalSpeedCommand) annotation(
    Line(points = {{-31, 19}, {-52.75, 19}, {-52.75, 49}}, color = {0, 0, 127}));
  connect(lowLevelFlightControlSystem1.RCYawCommand, remoteControl1.YawCommand) annotation(
    Line(points = {{-22, 19}, {-36.4167, 19}, {-36.4167, 51}, {-35.4167, 51}, {-35.4167, 49}, {-36.4167, 49}}, color = {0, 0, 127}));
  connect(remoteControl1.RollCommand, lowLevelFlightControlSystem1.RCAttitudeCommands[1]) annotation(
    Line(points = {{-47, 49}, {-47, 22}, {-27, 22}, {-27, 19}}, color = {0, 0, 127}, thickness = 0.0625));
  connect(remoteControl1.PitchCommand, lowLevelFlightControlSystem1.RCAttitudeCommands[2]) annotation(
    Line(points = {{-42, 49}, {-42, 22}, {-27, 22}, {-27, 19}}, color = {0, 0, 127}, thickness = 0.0625));
  connect(remoteControl1.IndicatorYawConsign, controlDroneNavigation1.IndicatorYawConsign) annotation(
    Line(points = {{-57, 59}, {-94, 59}, {-94, 6}, {-80, 6}, {-80, 8}, {-78, 8}, {-78, 8}, {-78, 8}}, color = {255, 0, 255}));
  connect(remoteControl1.ControlMode, controlDroneNavigation1.APEngagement) annotation(
    Line(points = {{-57, 54}, {-84, 54}, {-84, 12}, {-79, 12}}, color = {255, 0, 255}, thickness = 0.015625));
  connect(controlDroneNavigation1.Yaw, quadcopterModel1.Attitude[3]) annotation(
    Line(points = {{-79, 2}, {-87, 2}, {-87, -36}, {91, -36}, {91, 8}, {49, 8}, {49, 8}, {49, 8}, {49, 8}}, color = {0, 0, 127}));
  annotation(
    Icon(coordinateSystem(grid = {3, 2})),
    experiment(StartTime = 0, StopTime = 30, Tolerance = 1e-06, Interval = 0.002),
  __OpenModelica_simulationFlags(jacobian = "coloredNumerical", s = "dassl", lv = "LOG_STATS"));
end AIDA_System;