AIDASimulation/SimulationModels/AIDAModelica/modele_complet_RC.mo

within AIDAModelica;

model modele_complet_RC "modele_complet_RC_Cosim.mo"
  AIDAModelica.ControlDroneNavigation controlDroneNavigation1 annotation(
    Placement(visible = true, transformation(origin = {-64, 22}, extent = {{-12, -12}, {12, 12}}, rotation = 0)));
  AIDAModelica.LowLevelFlightControlSystem lowLevelFlightControlSystem1 annotation(
    Placement(transformation(origin = {-22, 22}, extent = {{-12.5, -12.5}, {12.5, 12.5}})));
  AIDAModelica.QuadcopterModel quadcopterModel1 annotation(
    Placement(visible = true, transformation(origin = {32, 22}, extent = {{-10, -12.5}, {10, 12.5}}, rotation = 0)));
  AIDAModelica.RemoteControl remoteControl1(Pitch_cmd = 0.02, Pitch_cmd_t = {9, 15}, Roll_cmd = 0.02, Roll_cmd_t = {6, 12}, VS_cmd = -0.5, VS_cmd_t = {0.1, 3}, Yaw_cmd_t = {18, 22})  annotation(
    Placement(visible = true, transformation(origin = {-22, 78}, extent = {{-12.5, -10}, {12.5, 10}}, rotation = 0)));
  parameter Real Cosim_step_i = 0.02;
  parameter Boolean Cosim_activ_i = false;
equation
  connect(remoteControl1.ControlMode, controlDroneNavigation1.APEngagement) annotation(
    Line(points = {{-34.5, 73}, {-83, 73}, {-83, 28}, {-76, 28}}, color = {255, 0, 255}));
  connect(remoteControl1.IndicatorYawConsign, controlDroneNavigation1.IndicatorYawConsign) annotation(
    Line(points = {{-34.5, 78}, {-89, 78}, {-89, 22}, {-76, 22}}, color = {255, 0, 255}));
  connect(remoteControl1.PitchCommand, lowLevelFlightControlSystem1.RCAttitudeCommands[2]) annotation(
    Line(points = {{-19.5, 68}, {-19.5, 49.7}, {-24.3, 49.7}, {-24.3, 34.7}}, color = {0, 0, 127}));
  connect(remoteControl1.VerticalSpeedCommand, lowLevelFlightControlSystem1.RCVerticalSpeedCommand) annotation(
    Line(points = {{-29.5, 68}, {-29.5, 39.7}, {-29.3, 39.7}, {-29.3, 34.7}}, color = {0, 0, 127}));
  connect(remoteControl1.YawCommand, lowLevelFlightControlSystem1.RCYawCommand) annotation(
    Line(points = {{-14.5, 68}, {-14.5, 39.7}, {-19.7, 39.7}, {-19.7, 34.7}}, color = {0, 0, 127}));
  connect(remoteControl1.RollCommand, lowLevelFlightControlSystem1.RCAttitudeCommands[1]) annotation(
    Line(points = {{-24.5, 68}, {-24.5, 39.7}, {-24.7, 39.7}, {-24.7, 34.7}}, color = {0, 0, 127}));
  connect(quadcopterModel1.Accelerations[:], lowLevelFlightControlSystem1.Accelerations[:]) annotation(
    Line(points = {{42, 14}, {63, 14}, {63, 4}, {-13.75, 4}, {-13.75, 10}, {-14, 10}}, color = {0, 0, 127}));
  //
  connect(controlDroneNavigation1.DronPositionConsign, lowLevelFlightControlSystem1.DronePositionConsign) annotation(
    Line(points = {{-52, 28}, {-39.3, 28}, {-39.3, 29.5}, {-34.5, 29.5}}, color = {0, 0, 127}));
  connect(controlDroneNavigation1.YawConsign, lowLevelFlightControlSystem1.YawConsign) annotation(
    Line(points = {{-52, 22}, {-39.3, 22}, {-39.3, 19.5}, {-34.5, 19.5}}, color = {0, 0, 127}));
  connect(controlDroneNavigation1.SelectedControlMode, lowLevelFlightControlSystem1.SelectedControlMode) annotation(
    Line(points = {{-52, 16}, {-39.3, 16}, {-39.3, 14.5}, {-34.5, 14.5}}, color = {255, 0, 255}));
  //
  connect(lowLevelFlightControlSystem1.ThrottleCommand1, quadcopterModel1.ThrottleCommand1) annotation(
    Line(points = {{-9.5, 29.5}, {17.15, 29.5}, {17.15, 28}, {22, 28}}, color = {0, 0, 127}, thickness = 0.0625));
  connect(lowLevelFlightControlSystem1.ThrottleCommand2, quadcopterModel1.ThrottleCommand2) annotation(
    Line(points = {{-9.5, 24.5}, {17.15, 24.5}, {17.15, 24}, {22, 24}}, color = {0, 0, 127}, thickness = 0.0625));
  connect(lowLevelFlightControlSystem1.ThrottleCommand3, quadcopterModel1.ThrottleCommand3) annotation(
    Line(points = {{-9.5, 19.5}, {17.15, 19.5}, {17.15, 20}, {22, 20}}, color = {0, 0, 127}, thickness = 0.0625));
  connect(lowLevelFlightControlSystem1.ThrottleCommand4, quadcopterModel1.ThrottleCommand4) annotation(
    Line(points = {{-9.5, 14.5}, {17.15, 14.5}, {17.15, 16}, {22, 16}}, color = {0, 0, 127}, thickness = 0.0625));
  //
  connect(quadcopterModel1.AngularVelocities[:], lowLevelFlightControlSystem1.AngularSpeed[:]) annotation(
    Line(points = {{42, 30}, {86, 30}, {86, -15.25}, {-30, -15.25}, {-30, 9.5}}, color = {0, 0, 127}));
  connect(quadcopterModel1.Attitude[:], lowLevelFlightControlSystem1.Attitude[:]) annotation(
    Line(points = {{42, 26}, {80, 26}, {80, -12}, {-26, -12}, {-26, 9.5}}, color = {0, 0, 127}));
  connect(quadcopterModel1.Position[:], lowLevelFlightControlSystem1.Position[:]) annotation(
    Line(points = {{42, 22}, {74, 22}, {74, -8}, {-22, -8}, {-22, 9.5}}, color = {0, 0, 127}));
  connect(quadcopterModel1.Speed[:], lowLevelFlightControlSystem1.Speed[:]) annotation(
    Line(points = {{42, 18}, {68, 18}, {68, -4}, {-18, -4}, {-18, 9.5}}, color = {0, 0, 127}));
  connect(quadcopterModel1.Attitude[3], controlDroneNavigation1.Yaw) annotation(
    Line(points = {{42, 26}, {94, 26}, {94, -30}, {-88, -30}, {-88, 16}, {-76, 16}}, color = {0, 0, 127}));
//
  annotation(
    uses(Modelica(version = "3.2.1")),
    Diagram,
    __iti_controlDroneNavigation1(YawConsign(flags = 2), DronPositionConsign(flags = 2), APEngagement(flags = 2), SelectedControlMode(flags = 2), IndicatorYawConsign(flags = 2), Yaw(flags = 2), runFlightPlan1(DronPositionConsign(flags = 2), YawConsign(flags = 2), X_coordinate_map(u(flags = 2), y(flags = 2)), timeSource1(TimeOutput(flags = 2)), Y_coordinate_map(u(flags = 2), y(flags = 2)), Z_coordinate_map(u(flags = 2), y(flags = 2)), YawAngle(u(flags = 2), y(flags = 2))), switch1(u1(flags = 2), u2(flags = 2), u3(flags = 2), y(flags = 2)), yawConsignProducing1(Yaw(flags = 2), DesiredYaw(flags = 2), YRC(flags = 2), YawOld(flags = 2))),
    __iti_lowLevelFlightControlSystem1(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), TotalThrustSAC(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(TotalThrustSAC(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)), add1(u1(flags = 2), u2(flags = 2), y(flags = 2))), positionControl1(DronPosit
    __iti_quadcopterModel1(ThrottleCommand1(flags = 2), ThrottleCommand2(flags = 2), ThrottleCommand3(flags = 2), ThrottleCommand4(flags = 2), AngularVelocities(flags = 2), Attitude(flags = 2), Position(flags = 2), Speed(flags = 2), rigidBodyKinematicModel1(DroneAngularVelocities(flags = 2), Velocity(flags = 2), AttitudeFB(flags = 2), Attitude(flags = 2), Position(flags = 2), computeDronePosition1(DronVelocity(flags = 2), Position(flags = 2), integrator10(u(flags = 2), y(flags = 2)), integrator11(u(flags = 2), y(flags = 2)), integrator12(u(flags = 2), y(flags = 2))), computeDroneAttitude1(DroneAngularVelocities(flags = 2), AttitudeFB(flags = 2), Attitude(flags = 2), computationChangeAngleVelocity1(DronAngularVelocities(flags = 2), Attitude(flags = 2), ChangeAngleVelocity(flags = 2), W(flags = 2)), integrator7(u(flags = 2), y(flags = 2)), integrator8(u(flags = 2), y(flags = 2)), integrator9(u(flags = 2), y(flags = 2)))), motorPropellerModel1(ThrottleCommandK(flags = 2), MotorKAngularVelocity(flags = 2), createMotion1(ThrottleCommandK(flags = 2), MotorKAngularVelocity(flags = 2), W(u(flags = 2), y(flags = 2)), wSSModel1(CmdKIn(flags = 2), WSSOut(flags = 2)))), motorPropellerModel2(ThrottleCommandK(flags = 2), MotorKAngularVelocity(flags = 2), createMotion1(ThrottleCommandK(flags = 2), MotorKAngularVelocity(flags = 2), W(u(flags = 2), y(flags = 2)), wSSModel1(CmdKIn(flags = 2), WSSOut(flags = 2)))), motorPropellerModel3(ThrottleCommandK(flags = 2), MotorKAngularVelocity(flags = 2), createMotion1(ThrottleCommandK(flags = 2), MotorKAngularVelocity(flags = 2), W(u(flags = 2), y(flags = 2)), wSSModel1(CmdKIn(flags = 2), WSSOut(flags = 2)))), motorPropellerModel4(ThrottleCommandK(flags = 2), MotorKAngularVelocity(flags = 2), createMotion1(ThrottleCommandK(flags = 2), MotorKAngularVelocity(flags = 2), W(u(flags = 2), y(flags = 2)), wSSModel1(CmdKIn(flags = 2), WSSOut(flags = 2)))), rigidBodyDynamicModel1(Moments(flags = 2), TotalThrust(flags = 2), Attitude(flags = 2), DroneAngularVelocities(flags = 2), DroneVelocity(flags = 2), computeDroneVelocity1(TotalThrust(flags = 2), Attitude(flags = 2), DroneVelocity(flags = 2), computationAccelerationModel1(TotalThrust(flags = 2), Attitude(flags = 2), DroneAcceleration(flags = 2), Reb(flags = 2), Rz(flags = 2), Ry(flags = 2), Rx(flags = 2)), integrator4(u(flags = 2), y(flags = 2)), integrator5(u(flags = 2), y(flags = 2)), integrator6(u(flags = 2), y(flags = 2))), computeDroneAngularsVelocities1(Moments(flags = 2), DroneAngularVelocities(flags = 2), computationAngularAccelerationModel1(Moments(flags = 2), DroneAngularAcceleration(flags = 2), invJ(flags = 2)), integrator1(u(flags = 2), y(flags = 2)), integrator2(u(flags = 2), y(flags = 2)), integrator3(u(flags = 2), y(flags = 2)))), cotrolEffectivenessModel1(Motor1AngularVelocity(flags = 2), Motor2AngularVelocity(flags = 2), Motor3AngularVelocity(flags = 2), Motor4AngularVelocity(flags = 2), TotalThrust(flags = 2), Moments(flags = 2), computeMoments1(Motor1AngularVelocity(flags = 2), Motor2AngularVelocity(flags = 2), Motor3AngularVelocity(flags = 2), Motor4AngularVelocity(flags = 2), Moments(flags = 2)), computeTotalThrust1(Motor1AngularVelocity(flags = 2), Motor2AngularVelocity(flags = 2), Motor3AngularVelocity(flags = 2), Motor4AngularVelocity(flags = 2), TotalThrust(flags = 2)))),
    experiment(StopTime = 1, StartTime = 0, Interval = 0.002, __iti_MaxInterval = "0.001"));
end modele_complet_RC;