// 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;