AIDASimulation/SimulationModels/AIDAModelica/ProSIVICToSimXInterfaceTest.mo

// CP: 65001
// SimulationX Version: 3.8.2.45319 x64
within AIDAModelica;
model ProSIVICToSimXInterfaceTest "Pro-SIVIC to Simulation X Interface Test"
	Modelica.Blocks.Interfaces.RealInput roll(
		quantity="Mechanics.Rotation.Angle",
		displayUnit="rad") "IMU roll" annotation(Placement(
		transformation(extent={{-140,15},{-100,55}}),
		iconTransformation(
			origin={-275,-100},
			extent={{-20,-20},{20,20}},
			rotation=90)));
	Modelica.Blocks.Interfaces.RealInput pitch(
		quantity="Mechanics.Rotation.Angle",
		displayUnit="rad") "IMU pitch" annotation(Placement(
		transformation(extent={{-105,15},{-65,55}}),
		iconTransformation(
			origin={-225,-100},
			extent={{-20,-20},{20,20}},
			rotation=90)));
	Modelica.Blocks.Interfaces.RealInput yaw(
		quantity="Mechanics.Rotation.Angle",
		displayUnit="rad") "IMU yaw" annotation(Placement(
		transformation(extent={{-135,-10},{-95,30}}),
		iconTransformation(
			origin={-175,-100},
			extent={{-20,-20},{20,20}},
			rotation=90)));
	Modelica.Blocks.Interfaces.RealInput rollRate(
		quantity="Mechanics.Rotation.RotVelocity",
		displayUnit="rad/s") "IMU roll rate" annotation(Placement(
		transformation(extent={{-70,15},{-30,55}}),
		iconTransformation(
			origin={-125,-100},
			extent={{-20,-20},{20,20}},
			rotation=90)));
	Modelica.Blocks.Interfaces.RealInput pitchRate(
		quantity="Mechanics.Rotation.RotVelocity",
		displayUnit="rad/s") "IMU pitch rate" annotation(Placement(
		transformation(extent={{-35,15},{5,55}}),
		iconTransformation(
			origin={-75,-100},
			extent={{-20,-20},{20,20}},
			rotation=90)));
	Modelica.Blocks.Interfaces.RealInput yawRate(
		quantity="Mechanics.Rotation.RotVelocity",
		displayUnit="rad/s") "IMU yaw rate" annotation(Placement(
		transformation(extent={{-65,-10},{-25,30}}),
		iconTransformation(
			origin={-25,-100},
			extent={{-20,-20},{20,20}},
			rotation=90)));
	Modelica.Blocks.Interfaces.RealInput SpeedX(
		quantity="Mechanics.Translation.Accel",
		displayUnit="m/s²") "IMU acceleration X" annotation(Placement(
		transformation(extent={{0,15},{40,55}}),
		iconTransformation(
			origin={25,-100},
			extent={{-20,-20},{20,20}},
			rotation=90)));
	Modelica.Blocks.Interfaces.RealInput SpeedY(
		quantity="Mechanics.Translation.Accel",
		displayUnit="m/s²") "IMU acceleration Y" annotation(Placement(
		transformation(extent={{30,15},{70,55}}),
		iconTransformation(
			origin={75,-100},
			extent={{-20,-20},{20,20}},
			rotation=90)));
	Modelica.Blocks.Interfaces.RealInput SpeedZ(
		quantity="Mechanics.Translation.Accel",
		displayUnit="m/s²") "IMU acceleration Z" annotation(Placement(
		transformation(extent={{5,-10},{45,30}}),
		iconTransformation(
			origin={125,-100},
			extent={{-20,-20},{20,20}},
			rotation=90)));
	Modelica.Blocks.Interfaces.RealInput X(
		quantity="Geometry.Angle",
		displayUnit="deg") "GPS accurate latitude" annotation(Placement(
		transformation(extent={{65,15},{105,55}}),
		iconTransformation(
			origin={175,-100},
			extent={{-20,-20},{20,20}},
			rotation=90)));
	Modelica.Blocks.Interfaces.RealInput Y(
		quantity="Geometry.Angle",
		displayUnit="deg") "GPS accurate longitude" annotation(Placement(
		transformation(extent={{75,-10},{115,30}}),
		iconTransformation(
			origin={225,-100},
			extent={{-20,-20},{20,20}},
			rotation=90)));
	Modelica.Blocks.Interfaces.RealInput Z(
		quantity="Geometry.Length",
		displayUnit="m") "Lidar distance" annotation(Placement(
		transformation(extent={{100,15},{140,55}}),
		iconTransformation(
			origin={275,-100},
			extent={{-20,-20},{20,20}},
			rotation=90)));
	Modelica.Blocks.Interfaces.RealOutput Attitude[3](
		quantity="Mechanics.Rotation.Angle",
		displayUnit="rad") "'output Real' as connector" annotation(Placement(
		transformation(extent={{-80,70},{-60,90}}),
		iconTransformation(
			origin={-225,100},
			extent={{-10,-10},{10,10}},
			rotation=90)));
	Modelica.Blocks.Interfaces.RealOutput AngularVelocities[3](
		quantity="Mechanics.Rotation.RotVelocity",
		displayUnit="rad/s") "'output Real' as connector" annotation(Placement(
		transformation(extent={{-10,70},{10,90}}),
		iconTransformation(
			origin={-75,100},
			extent={{-10,-10},{10,10}},
			rotation=90)));
	Modelica.Blocks.Interfaces.RealOutput Speed[3](
		quantity="Mechanics.Translation.Velocity",
		displayUnit="m/s") "'output Real' as connector" annotation(Placement(
		transformation(extent={{50,70},{70,90}}),
		iconTransformation(
			origin={225,100},
			extent={{-10,-10},{10,10}},
			rotation=90)));
	Modelica.Blocks.Interfaces.RealOutput Position[3](
		quantity="Mechanics.Translation.Displace",
		displayUnit="m") "'output Real' as connector" annotation(Placement(
		transformation(extent={{150,70},{170,90}}),
		iconTransformation(
			origin={75,100},
			extent={{-10,-10},{10,10}},
			rotation=90)));
	parameter Real Longitude0 "Long repère AIDA";
	parameter Real Latitude0 "Lat repère AIDA";
	parameter Real Rt=6378.137 "Rayon terre en m";
	constant Real Pi=3.14159 "Pi";
	parameter Real K;
	Real Xn;
	Real Yw;
	initial equation
		Latitude0=X;
		Longitude0=Y;
		K=Rt*Pi/180;
	equation
		// enter your equations here
		Attitude = {roll,-1*pitch,-1*yaw};
		AngularVelocities = {rollRate,-1*pitchRate,-1*yawRate};
			
		//convertion lat/long
		Xn=(X-Latitude0)*K;
		Yw=cos(X*Pi/180)*(Longitude0-Y)*K;
		Position = {Xn,-Yw,-1*Z};
		Speed = {K*SpeedX,K*SpeedY*cos(X*Pi/180),-1*SpeedZ};
	annotation(
		roll(flags=2),
		pitch(flags=2),
		yaw(flags=2),
		rollRate(flags=2),
		pitchRate(flags=2),
		yawRate(flags=2),
		SpeedX(flags=2),
		SpeedY(flags=2),
		SpeedZ(flags=2),
		X(flags=2),
		Y(flags=2),
		Z(flags=2),
		Attitude(flags=2),
		AngularVelocities(flags=2),
		Speed(flags=2),
		Position(flags=2),
		Icon(
			coordinateSystem(extent={{-325,-100},{325,100}}),
			graphics={
				Rectangle(
					fillColor={255,255,255},
					fillPattern=FillPattern.Solid,
					extent={{-326,100},{326.7,-100}})}),
		experiment(
			StopTime=1,
			StartTime=0,
			Interval=0.002,
			MaxInterval="0.001"));
end ProSIVICToSimXInterfaceTest;