You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
91 lines
6.8 KiB
91 lines
6.8 KiB
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;
|
|
|