AIDASimulation/SimulationModels/AIDAModelica/ControlAltitude.mo

// CP: 65001
// SimulationX Version: 3.8.2.45319 x64
within AIDAModelica;
model ControlAltitude "[SF2.4] Control altitude"
	Modelica.Blocks.Interfaces.RealOutput TotalThrustManual(
		quantity="Mechanics.Translation.Force",
		displayUnit="N") "Desired total thrust autopilot mode output" annotation(Placement(
		transformation(extent={{20,20},{40,40}}),
		iconTransformation(
			origin={100,0},
			extent={{-10,-10},{10,10}})));
	Modelica.Blocks.Interfaces.RealInput VSpeed(
		quantity="Mechanics.Translation.Velocity",
		displayUnit="m/s") "Real drone speed feedback" annotation(Placement(
		transformation(extent={{-105,-5},{-65,35}}),
		iconTransformation(extent={{-120,30},{-80,70}})));
	Modelica.Blocks.Interfaces.RealInput RCVerticalSpeedCommand(
		quantity="Mechanics.Translation.Velocity",
		displayUnit="m/s") "'input Real' as connector" annotation(Placement(
		transformation(extent={{-105,25},{-65,65}}),
		iconTransformation(
			origin={-100,-50},
			extent={{-20,-20},{20,20}})));
	ComputeTotalThrustCA computeTotalThrustCA1 annotation(Placement(transformation(extent={{-10,20},{10,40}})));
	ComputeAltitudeConsign computeAltitudeConsign1 annotation(Placement(transformation(extent={{-55,20},{-35,40}})));
	equation
		connect(computeAltitudeConsign1.VSpeed,VSpeed) annotation(Line(
			points={{-54,26},{-76,26},{-76,16},{-84,16}},
			color={0,0,127}));
				connect(computeTotalThrustCA1.AccelerationConsign,computeAltitudeConsign1.AccelerationConsign) annotation(Line(
			points={{-10,30},{-15,30},{-30,30},{-35,30}},
			color={0,0,127},
			thickness=0.0625));
				connect(computeAltitudeConsign1.RCVerticalSpeedCommand,RCVerticalSpeedCommand) annotation(Line(
			points={{-55,35},{-60,35},{-80,35},{-80,45},{-85,45}},
			color={0,0,127},
			thickness=0.0625));
				
		connect(computeTotalThrustCA1.TotalThrustAP,TotalThrustManual) annotation(Line(
			points={{10,30},{15,30},{25,30},{30,30}},
			color={0,0,127},
			thickness=0.0625));
	annotation(
		Speed(flags=2),
		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))),
		Icon(graphics={
			Rectangle(
				fillColor={255,255,255},
				fillPattern=FillPattern.Solid,
				extent={{-100,100},{100,-100}})}),
		experiment(
			StopTime=1,
			StartTime=0,
			Interval=0.002,
			MaxInterval="0.001"));
end ControlAltitude;