AIDA
/
AIDASimulation
mirror of http://172.16.200.102/AIDA/AIDASimulation.git
1
0
Fork 0
Code Issues Projects Releases Wiki Activity
AIDA is a study case for model based system engineering, made by MOISE project. This project contains the simulation model of AIDA (made with SimulationX in Modelica)
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.
4 Commits
1 Branch
0 Tags
3.6 MiB
 
Tree: fdf293ece6
Branches Tags
${ item.name }
Create tag ${ searchTerm }
Create branch ${ searchTerm }
from 'fdf293ece6'
${ noResults }
AIDASimulation/SimulationModels/AIDAModelica/LowLevelFlightControlSystem.mo

127 lines
16 KiB
Raw Blame History

within AIDAModelica;
model LowLevelFlightControlSystem "Low level Flight Control System"
Modelica.Blocks.Interfaces.RealInput DronePositionConsign[3](quantity = "Mechanics.Translation.Displace", displayUnit = "m") "Desired position input" annotation(
Placement(transformation(extent = {{-55, 84}, {-35, 105}}), iconTransformation(extent = {{-160, 64.5}, {-140, 85.5}})));
Modelica.Blocks.Interfaces.RealInput Accelerations[3] "Accelerations in Ground reference" annotation(
Placement(visible = true, transformation(extent = {{-135, 35}, {-115, 60}}, rotation = 0), iconTransformation(origin = {95, -125.333}, extent = {{-10, -10.5}, {10, 10.5}}, rotation = 90)));
Modelica.Blocks.Interfaces.RealInput AngularSpeed[3](quantity = "Mechanics.Rotation.RotVelocity", displayUnit = "rad/s") "Real angular velocities feedback input" annotation(
Placement(transformation(extent = {{20, -71}, {40, -50}}), iconTransformation(origin = {-100, -125}, extent = {{-10, -10.5}, {10, 10.5}}, rotation = 90)));
Modelica.Blocks.Interfaces.RealInput Position[3](quantity = "Mechanics.Translation.Displace", displayUnit = "m") "Real position feedback input" annotation(
Placement(transformation(extent = {{-71, 75}, {-51, 96}}), iconTransformation(origin = {0, -125}, extent = {{-10, -10.5}, {10, 10.5}}, rotation = 90)));
Modelica.Blocks.Interfaces.RealInput YawConsign(quantity = "Mechanics.Rotation.Angle", displayUnit = "rad") "Desired yaw angle input" annotation(
Placement(transformation(extent = {{-10, 4}, {10, 25}}), iconTransformation(extent = {{-160, -35.5}, {-140, -14.5}})));
Modelica.Blocks.Interfaces.RealInput Attitude[3](quantity = "Mechanics.Rotation.Angle", displayUnit = "rad") "Real attitude feedback input" annotation(
Placement(transformation(extent = {{40, -96}, {60, -75}}), iconTransformation(origin = {-50, -125}, extent = {{-10, -10.5}, {10, 10.5}}, rotation = 90)));
Modelica.Blocks.Interfaces.RealInput Speed[3](quantity = "Mechanics.Translation.Velocity", displayUnit = "m/s") "Real velocity feedback input" annotation(
Placement(visible = true, transformation(extent = {{-72, 59}, {-52, 80}}, rotation = 0), iconTransformation(origin = {46, -125}, extent = {{-10, -10.5}, {10, 10.5}}, rotation = 90)));
Modelica.Blocks.Interfaces.RealOutput ThrottleCommand1 "Command 1 output" annotation(
Placement(transformation(extent = {{160, 70}, {180, 90}}), iconTransformation(extent = {{140, 65}, {160, 85}})));
Modelica.Blocks.Interfaces.RealOutput ThrottleCommand2 "Command 2 output" annotation(
Placement(transformation(extent = {{160, 50}, {180, 70}}), iconTransformation(extent = {{140, 15}, {160, 35}})));
Modelica.Blocks.Interfaces.RealOutput ThrottleCommand3 "Command 3 output" annotation(
Placement(transformation(extent = {{160, 30}, {180, 50}}), iconTransformation(extent = {{140, -35}, {160, -15}})));
Modelica.Blocks.Interfaces.RealOutput ThrottleCommand4 "Command 4 output" annotation(
Placement(transformation(extent = {{160, 10}, {180, 30}}), iconTransformation(extent = {{140, -85}, {160, -65}})));
Modelica.Blocks.Interfaces.RealInput RCVerticalSpeedCommand(quantity = "Mechanics.Translation.Velocity", displayUnit = "m/s") "RC vertical speed consign" annotation(
Placement(transformation(extent = {{-125, 79}, {-105, 100}}), iconTransformation(origin = {-75, 125}, extent = {{-10, -10.5}, {10, 10.5}}, rotation = -90)));
Modelica.Blocks.Interfaces.RealInput RCAttitudeCommands[2](quantity = "Mechanics.Rotation.Angle", displayUnit = "rad") "RC attitude consign" annotation(
Placement(transformation(extent = {{-135, 19}, {-115, 40}}), iconTransformation(origin = {-25, 125}, extent = {{-10, -10.5}, {10, 10.5}}, rotation = -90)));
Modelica.Blocks.Interfaces.RealInput RCYawCommand(quantity = "Mechanics.Rotation.Angle", displayUnit = "rad") "RC yaw consign" annotation(
Placement(transformation(extent = {{-120, -41}, {-100, -20}}), iconTransformation(origin = {25, 125}, extent = {{-10, -10.5}, {10, 10.5}}, rotation = -90)));
Modelica.Blocks.Interfaces.BooleanInput SelectedControlMode "Control mode" annotation(
Placement(transformation(extent = {{110, -30}, {130, -10}}), iconTransformation(extent = {{-170, -95}, {-130, -55}})));
MotorControl motorControl1 annotation(
Placement(transformation(extent = {{115, 75}, {135, 85}})));
MotorControl motorControl2 "Motor Control Model" annotation(
Placement(transformation(extent = {{115, 55}, {135, 65}})));
MotorControl motorControl3 annotation(
Placement(transformation(extent = {{115, 35}, {135, 45}})));
MotorControl motorControl4 annotation(
Placement(transformation(extent = {{115, 15}, {135, 25}})));
AIDAModelica.AllocationControl allocationControl1 annotation(
Placement(visible = true, transformation(extent = {{82, 46}, {102, 71}}, rotation = 0)));
AIDAModelica.ControlAltitudeEx controlAltitude1 annotation(
Placement(visible = true, transformation(extent = {{1, 53}, {21, 73}}, rotation = 0)));
AttitudeControl attitudeControl1 annotation(
Placement(transformation(extent = {{45, 20}, {65, 40}})));
PositionControl positionControl1 annotation(
Placement(transformation(extent = {{-20, 80}, {0, 100}})));
equation
connect(allocationControl1.SelectedControlMode, SelectedControlMode) annotation(
Line(points = {{82, 51}, {75, 51}, {75, -20}, {120, -20}}, color = {255, 0, 255}, thickness = 0.0625));
connect(allocationControl1.Motor4Rate, motorControl4.MotorKRate) annotation(
Line(points = {{102, 51}, {110, 51}, {110, 20}, {115, 20}}, color = {0, 0, 127}, thickness = 0.0625));
connect(allocationControl1.Motor1Rate, motorControl1.MotorKRate) annotation(
Line(points = {{102, 66}, {110, 66}, {110, 80}, {115, 80}}, color = {0, 0, 127}, thickness = 0.0625));
connect(allocationControl1.Motor2Rate, motorControl2.MotorKRate) annotation(
Line(points = {{102, 61}, {110, 61}, {110, 60}, {115, 60}}, color = {0, 0, 127}, thickness = 0.0625));
connect(allocationControl1.Motor3Rate, motorControl3.MotorKRate) annotation(
Line(points = {{102, 56}, {108.5, 56}, {108.5, 40}, {115, 40}}, color = {0, 0, 127}, thickness = 0.0625));
connect(attitudeControl1.MomentumsConsign[:], allocationControl1.MomentumsConsign[:]) annotation(
Line(points = {{65, 30}, {75, 30}, {75, 56}, {82, 56}}, color = {0, 0, 127}, thickness = 0.0625));
connect(positionControl1.TotalThrustAP, allocationControl1.TotalThrustAP) annotation(
Line(points = {{0, 95}, {75, 95}, {75, 66}, {82, 66}}, color = {0, 0, 127}, thickness = 0.0625));
connect(controlAltitude1.TotalThrustManual, allocationControl1.TotalThrustManual) annotation(
Line(points = {{21, 63}, {54.5, 63}, {54.5, 61}, {82, 61}}, color = {0, 0, 127}));
connect(attitudeControl1.RCYawCommand, RCYawCommand) annotation(
Line(points = {{46, 36}, {-106, 36}, {-106, -30}, {-110, -30}}, color = {0, 0, 127}));
connect(controlAltitude1.Acceleration, Accelerations[3]) annotation(
Line(points = {{16, 53}, {16, 47.7}, {-125, 47.7}}, color = {0, 0, 127}, thickness = 0.0625));
connect(controlAltitude1.VSpeed, Speed[3]) annotation(
Line(points = {{6, 53}, {6, 50}, {-23.3, 50}, {-23.3, 69.7}, {-62, 69.7}}, color = {0, 0, 127}));
connect(controlAltitude1.RCVerticalSpeedCommand, RCVerticalSpeedCommand) annotation(
Line(points = {{1, 63}, {-110, 63}, {-110, 90}, {-114, 90}}, color = {0, 0, 127}));
connect(attitudeControl1.SelectedControlMode, SelectedControlMode) annotation(
Line(points = {{60, 20}, {60, 15}, {60, -20}, {115, -20}, {120, -20}}, color = {255, 0, 255}, thickness = 0.0625));
connect(positionControl1.Position[:], Position[:]) annotation(
Line(points = {{-20, 90}, {-25, 90}, {-56, 90}, {-56, 85.7}, {-61, 85.7}}, color = {0, 0, 127}, thickness = 0.0625));
connect(positionControl1.Speed[:], Speed[:]) annotation(
Line(points = {{-20, 85}, {-25, 85}, {-57, 85}, {-57, 69.7}, {-62, 69.7}}, color = {0, 0, 127}, thickness = 0.0625));
connect(motorControl1.ThrottleCommandK, ThrottleCommand1) annotation(
Line(points = {{135, 80}, {140, 80}, {165, 80}, {170, 80}}, color = {0, 0, 127}, thickness = 0.0625));
connect(motorControl2.ThrottleCommandK, ThrottleCommand2) annotation(
Line(points = {{135, 60}, {140, 60}, {165, 60}, {170, 60}}, color = {0, 0, 127}, thickness = 0.0625));
connect(motorControl3.ThrottleCommandK, ThrottleCommand3) annotation(
Line(points = {{135, 40}, {140, 40}, {165, 40}, {170, 40}}, color = {0, 0, 127}, thickness = 0.0625));
connect(motorControl4.ThrottleCommandK, ThrottleCommand4) annotation(
Line(points = {{135, 20}, {140, 20}, {165, 20}, {170, 20}}, color = {0, 0, 127}, thickness = 0.0625));
connect(attitudeControl1.AngularSpeed[:], AngularSpeed[:]) annotation(
Line(points = {{50, 20}, {50, 15}, {50, -60.3}, {35, -60.3}, {30, -60.3}}, color = {0, 0, 127}, thickness = 0.0625));
connect(attitudeControl1.Attitude[:], Attitude[:]) annotation(
Line(points = {{55, 20}, {55, 15}, {55, -85.3}, {50, -85.3}}, color = {0, 0, 127}, thickness = 0.0625));
connect(attitudeControl1.YawConsign, YawConsign) annotation(
Line(points = {{45, 25}, {40, 25}, {5, 25}, {5, 14.7}, {0, 14.7}}, color = {0, 0, 127}, thickness = 0.0625));
connect(positionControl1.DronPositionConsign[:], DronePositionConsign[:]) annotation(
Line(points = {{-20, 95}, {-25, 95}, {-40, 95}, {-40, 94.7}, {-45, 94.7}}, color = {0, 0, 127}, thickness = 0.0625));
connect(positionControl1.Attitude[:], Attitude[:]) annotation(
Line(points = {{-15, 80}, {-15, 75}, {-15, -85.3}, {45, -85.3}, {50, -85.3}}, color = {0, 0, 127}, thickness = 0.0625));
connect(positionControl1.AttitudeConsignAP[:], attitudeControl1.AttitudeConsignAP[:]) annotation(
Line(points = {{0, 85}, {5, 85}, {50, 85}, {50, 45}, {50, 40}}, color = {0, 0, 127}, thickness = 0.0625));
connect(RCAttitudeCommands[:], attitudeControl1.AttitudeRCConsign[:]) annotation(
Line(points = {{-125, 29.7}, {-120, 29.7}, {40, 29.7}, {40, 30}, {45, 30}}, color = {0, 0, 127}, thickness = 0.0625));
annotation(
DronePositionConsign(flags = 2),
AngularSpeed(flags = 2),
Position(flags = 2),
YawConsign(flags = 2),
Attitude(flags = 2),
Speed(flags = 2),
ThrottleCommand1(flags = 2),
ThrottleCommand2(flags = 2),
ThrottleCommand3(flags = 2),
ThrottleCommand4(flags = 2),
RCVerticalSpeedCommand(flags = 2),
RCAttitudeCommands(flags = 2),
RCYawCommand(flags = 2),
SelectedControlMode(flags = 2),
motorControl1(MotorKRate(flags = 2), ThrottleCommandK(flags = 2), controlMotor1(DAngVel(flags = 2), ComdKD(flags = 2))),
motorControl2(MotorKRate(flags = 2), ThrottleCommandK(flags = 2), controlMotor1(DAngVel(flags = 2), ComdKD(flags = 2))),
motorControl3(MotorKRate(flags = 2), ThrottleCommandK(flags = 2), controlMotor1(DAngVel(flags = 2), ComdKD(flags = 2))),
motorControl4(MotorKRate(flags = 2), ThrottleCommandK(flags = 2), controlMotor1(DAngVel(flags = 2), ComdKD(flags = 2))),
allocationControl1(TotalThrustAP(flags = 2), TotalThrustManual(flags = 2), MomentumsConsign(flags = 2), Motor1Rate(flags = 2), Motor2Rate(flags = 2), Motor3Rate(flags = 2), Motor4Rate(flags = 2), SelectedControlMode(flags = 2), computeMotorRate1(TotalThrust(flags = 2), MomentumConsign(flags = 2), Motor1Rate(flags = 2), Motor2Rate(flags = 2), Motor3Rate(flags = 2), Motor4Rate(flags = 2), P4(flags = 2), M4(flags = 2), AngVelVector(flags = 2), TMomVector(flags = 2), ParamCosSin(flags = 2)), SelectTotalThrustConsign(u1(flags = 2), u2(flags = 2), u3(flags = 2), y(flags = 2))),
controlAltitude1(TotalThrustManual(flags = 2), RCVerticalSpeedCommand(flags = 2), computeTotalThrustCA1(AccelerationConsign(flags = 2), TotalThrustAP(flags = 2), AC(flags = 2)), computeAltitudeConsign1(RCVerticalSpeedCommand(flags = 2), AccelerationConsign(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))), speedErrorModelSAC1(SpeedConsign(flags = 2), Speed(flags = 2), SpeedError(flags = 2), vzd(flags = 2), vz(flags = 2), ASE(flags = 2)))),
attitudeControl1(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))),
positionControl1(DronPositionConsign(flags = 2), Position(flags = 2), TotalThrustAP(flags = 2), AttitudeConsignAP(flags = 2), Attitude(flags = 2), Speed(flags = 2), controlPosition1(DronePositionConsign(flags = 2), Position(flags = 2), NavigationSpeedConsign(flags = 2), DHP(flags = 2), HP(flags = 2), DZP(flags = 2), ZP(flags = 2), HNSC(flags = 2), ASC(flags = 2)), controlSpeed1(SpeedConsign(flags = 2), Speed(flags = 2), AccelerationConsign(flags = 2), speedErrorModel1(SpeedConsign(flags = 2), Speed(flags = 2), SpeedError(flags = 2), vhd(flags = 2), vh(flags = 2), vzd(flags = 2), vz(flags = 2), HSE(flags = 2), ASE(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))), 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))), 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)))), computeThrustAndAttitudeConsign1(AccelerationConsign(flags = 2), Attitude(flags = 2), AttitudeAPConsigns(flags = 2), TotalThrustAP(flags = 2), HAC(flags = 2), Rpsi(flags = 2), Apsi(flags = 2), InvApsi(flags = 2), AAC(flags = 2)), selectSpeedConsign1(NavigationVelocityConsign(flags = 2), VelocityConsign(flags = 2))),
Icon(coordinateSystem(extent = {{-150, -125}, {150, 125}}, initialScale = 0.1), graphics = {Rectangle(fillColor = {255, 255, 255}, fillPattern = FillPattern.Solid, extent = {{-150, 126.7}, {150, -126.7}}), Text(origin = {0, 18}, extent = {{-88, 26}, {88, -26}}, textString = "%name")}),
experiment(StopTime = 1, StartTime = 0, Interval = 0.002, MaxInterval = "0.001"));
end LowLevelFlightControlSystem;