diff --git a/fmu2aadl/.gitignore b/fmu2aadl/.gitignore new file mode 100644 index 0000000..c69f094 --- /dev/null +++ b/fmu2aadl/.gitignore @@ -0,0 +1,3 @@ +build/ +dist/ +fmu2aadl.egg-info/ diff --git a/fmu2aadl/README.rst b/fmu2aadl/README.rst index a0da82d..e0713ed 100644 --- a/fmu2aadl/README.rst +++ b/fmu2aadl/README.rst @@ -15,8 +15,6 @@ Installation The script is provided as a standard Python module. The script can be installed, as user, using default Python install -procedure: +procedure:: -``` -python setup.py install --user -``` + python setup.py install --user diff --git a/fmu2aadl/examples/moonlanding/Makefile b/fmu2aadl/examples/moonlanding/Makefile index 8f460a4..0039ed5 100644 --- a/fmu2aadl/examples/moonlanding/Makefile +++ b/fmu2aadl/examples/moonlanding/Makefile @@ -1,11 +1,14 @@ all: moonlanding_fmu.aadl check_aadl code_gen -moonlanding_fmu.aadl: - fmu2aadl ../../../models/MoonLanding/MoonLanding.fmu +moonlanding_fmu.aadl: MoonLanding.fmu + fmu2aadl MoonLanding.fmu code_gen: moonlanding_fmu.aadl ocarina -aadlv2 -y -g polyorb_hi_c moonlanding.aadl +build: moonlanding_fmu.aadl + ocarina -aadlv2 -y -b -g polyorb_hi_c moonlanding.aadl + check_aadl: moonlanding_fmu.aadl ocarina -aadlv2 -y moonlanding.aadl @@ -13,4 +16,5 @@ clean: -rm -rf *~ distclean: clean - -rm -rf moonlanding_sys_impl + -rm -rf moonlanding_sys_impl fmu_wrapper.* userdefined.mk moonlanding_fmu.aadl \ + moonlanding_fmu_wrapper.c diff --git a/fmu2aadl/examples/moonlanding/README.rst b/fmu2aadl/examples/moonlanding/README.rst new file mode 100644 index 0000000..43af037 --- /dev/null +++ b/fmu2aadl/examples/moonlanding/README.rst @@ -0,0 +1,25 @@ +Moonlanding AADL + FMI +====================== + +About +----- + +This example is adapted from the Moonlanding example from Peter Fritzon, as detailed in the tutorial : https://openmodelica.org/images/docs/userdocs/modprod2012-tutorial1-Peter-Fritzson-ModelicaTutorial.pdf + +We adapted this example so that: + +* the environment is modeled as a `Modelica` model, and turned into a FMU using the OpenModelica toolset +* the controller is implemented as a `C` function, +* the controller is integrated to the environment in an `AADL` model + +From an code generation perpective: + +* The fmu2aadl script generates the corresponding AADL for the environment elements, and the C wrapper function. +* ocarina is used to assemble all elements together. + + + +Note +---- + +The generated FMU targets GNU/Linux 64 bits diff --git a/fmu2aadl/examples/moonlanding/moonlanding_fmu_wrapper.c b/fmu2aadl/examples/moonlanding/moonlanding_fmu_wrapper.c deleted file mode 100644 index c6d5a1d..0000000 --- a/fmu2aadl/examples/moonlanding/moonlanding_fmu_wrapper.c +++ /dev/null @@ -1,71 +0,0 @@ -#include -#include -#include - -#include "fmi2.h" -#include "sim_support.h" -#include "fmu_wrapper.h" - -/******************************************************************************/ -void moonlanding_fmu_entrypoint (fmi2Real *a_out, - fmi2Real thrust_in, - fmi2Real *v_out) -{ - const char *fmuFileName ="MoonLanding.fmu"; - double tEnd = 110.0; - double h = 0.1; - static bool initialized = false; - static FMUContext ctx; - - /* 1/ FMU activation */ - - if (!initialized) { - ctx.fmu = malloc (sizeof (FMU)); - FMU_Activate_Entrypoint (fmuFileName, tEnd, h, &ctx); - initialized = true; - } - - /* 2/ Regular compute entrypoint */ - - fmi2ValueReference vr; - fmi2Real r; - fmi2Status fmi2Flag; - - /* Get the scalar variables */ - ScalarVariable *thrust_sv = getVariable (ctx.fmu->modelDescription, "thrust"); - ScalarVariable *a_sv = getVariable (ctx.fmu->modelDescription, "a"); - ScalarVariable *v_sv = getVariable (ctx.fmu->modelDescription, "v"); - - /* Set the input */ - vr = getValueReference (thrust_sv); - fmi2Flag = ctx.fmu->setReal (ctx.component, &vr, 1, &thrust_in); - - /* Calculate the Step */ - doStep (ctx.fmu, ctx.component, - ctx.currentCommunicationPoint, - ctx.communicationStepSize, - ctx.noSetFMUStatePriorToCurrentPoint); - - /* Dump values */ - outputRow (ctx.fmu, ctx.component, - ctx.currentCommunicationPoint, - ctx.resultFile, ',', fmi2False); - - /* Get the outputs */ - vr = getValueReference (a_sv); - fmi2Flag = ctx.fmu->getReal (ctx.component, &vr, 1, &r); - *a_out = r; - - vr = getValueReference (v_sv); - fmi2Flag = ctx.fmu->getReal (ctx.component, &vr, 1, &r); - *v_out = r; - - ctx.currentCommunicationPoint += ctx.communicationStepSize; - - /* 3) terminate the simulation */ - - if (ctx.currentCommunicationPoint > tEnd) { - freeContext (ctx); - exit (0); - } -} diff --git a/fmu2aadl/examples/moonlanding/userdefined.mk b/fmu2aadl/examples/moonlanding/userdefined.mk deleted file mode 100644 index 8513dc1..0000000 --- a/fmu2aadl/examples/moonlanding/userdefined.mk +++ /dev/null @@ -1,28 +0,0 @@ -################################################################################ -# The following configures the elements to use FMUSDK2 elements - -# Folder of the shared src from the FMUSDK2 submodule -FMUSDK2_PATH = ../../../../../fmusdk2/fmu20/src/shared - -################################################################################ -# DO NOT EDIT PAST THE FOLLOWING - -# FMUSDK2 configuration - -FMUSDK2_DEFINE = -DFMI_COSIMULATION -DSTANDALONE_XML_PARSER \ - -DLIBXML_STATIC - -FMUSDK2_INCLUDE = -I$(FMUSDK2_PATH)/include -I$(FMUSDK2_PATH) \ - -I$(FMUSDK2_PATH)/parser/libxml -I$(FMUSDK2_PATH)/parser - -################################################################################ -# Ocarina/PolyORB-HI/C configuration - -USE_CPP_LINKER = 1 - -USER_CFLAGS = $(FMUSDK2_DEFINE) $(FMUSDK2_INCLUDE) -USER_LDFLAGS = -lexpat -ldl -lxml2 -USER_OBJS += $(FMUSDK2_PATH)/sim_support.o ../../fmu_wrapper.o \ - $(FMUSDK2_PATH)/parser/XmlElement.o \ - $(FMUSDK2_PATH)/parser/XmlParser.o \ - $(FMUSDK2_PATH)/parser/XmlParserCApi.o diff --git a/fmu2aadl/fmu2aadl/__init__.py b/fmu2aadl/fmu2aadl/__init__.py index 1163108..1998535 100755 --- a/fmu2aadl/fmu2aadl/__init__.py +++ b/fmu2aadl/fmu2aadl/__init__.py @@ -145,6 +145,121 @@ def FMU2AADL_Subprogram(root,tree,file): file.write(3 * ' ' + 'end ' + root.get('modelName') + '_spg;\n') file.write('\n') +################################################################################ +def FMU2C_Wrapper(root,tree,file): + + file.write ('#include \n') + file.write ('#include \n') + file.write ('#include \n') + file.write ('#include "fmi2.h"\n') + file.write ('#include "sim_support.h"\n') + file.write ('#include "fmu_wrapper.h"\n') + file.write ('\n') + + is_first_arg = True + file.write('void ' + root.get('modelName').lower() + '_fmu_entrypoint \n') + + for svar in tree.xpath("/fmiModelDescription/ModelVariables/ScalarVariable"): + if svar.get('causality') == 'input': + if is_first_arg: + file.write(6 * ' ' + '(') + is_first_arg = False + else: + file.write (',\n') + file.write(7 * ' ') + + file.write ('fmi2Real ' + svar.get('name') + '_in') + + if svar.get('causality') == 'output': + if is_first_arg: + file.write(6 * ' ' + '(') + is_first_arg = False + else: + file.write (',\n') + file.write(7 * ' ') + + file.write('fmi2Real *' + svar.get('name') + '_out') + + file.write (') \n{\n'); + + file.write(2 * ' ' + 'const char *fmuFileName ="MoonLanding.fmu";\n') + file.write(2 * ' ' + 'double tEnd = 110.0;\n') + file.write(2 * ' ' + 'double h = 0.1;\n') + file.write(2 * ' ' + 'static bool initialized = false;\n') + file.write(2 * ' ' + 'static FMUContext ctx;\n') + file.write('\n'); + file.write(2 * ' ' + '/* 1/ FMU activation */') + file.write('\n'); + + file.write(2 * ' ' + 'if (!initialized) {\n'); + file.write(4 * ' ' + 'ctx.fmu = malloc (sizeof (FMU));\n'); + file.write(4 * ' ' + + 'FMU_Activate_Entrypoint (fmuFileName, tEnd, h, &ctx);\n'); + file.write(4 * ' ' + 'initialized = true;\n'); + file.write(2 * ' ' + '}\n'); + file.write('\n'); + + file.write(2 * ' ' + '/* 2/ Regular compute entrypoint */\n'); + file.write('\n'); + file.write(2 * ' ' + 'fmi2ValueReference vr;\n'); + file.write(2 * ' ' + 'fmi2Real r;\n'); + file.write(2 * ' ' + 'fmi2Status fmi2Flag;\n'); + file.write('\n'); + + file.write(2 * ' ' + '/* Get the scalar variables */\n'); + for svar in tree.xpath("/fmiModelDescription/ModelVariables/ScalarVariable"): + if svar.get('causality') == 'input' or svar.get('causality') == 'output': + file.write(2 * ' ' + 'ScalarVariable *' + svar.get('name') + '_sv' + + ' = getVariable (ctx.fmu->modelDescription, "' + + svar.get('name') + '");\n') + file.write('\n'); + + file.write (2 * ' ' + '/* Set the input */\n') + for svar in tree.xpath("/fmiModelDescription/ModelVariables/ScalarVariable"): + if svar.get('causality') == 'input': + file.write(2 * ' ' + 'vr = getValueReference (' + svar.get('name') + + '_sv);\n') + file.write (2 * ' ' + + 'fmi2Flag = ctx.fmu->setReal (ctx.component, &vr, 1, &' + + svar.get('name') + '_in);\n') + file.write('\n') + + file.write (2 * ' ' + '/* Calculate the Step */\n') + file.write (2 * ' ' + 'doStep (ctx.fmu, ctx.component,\n') + file.write (10 * ' ' + 'ctx.currentCommunicationPoint,\n') + file.write (10 * ' ' + 'ctx.communicationStepSize,\n') + file.write (10 * ' ' + 'ctx.noSetFMUStatePriorToCurrentPoint);\n') + file.write('\n'); + + file.write (2 * ' ' + '/* Dump values */\n') + file.write (2 * ' ' + 'outputRow (ctx.fmu, ctx.component,\n') + file.write (13 * ' ' + 'ctx.currentCommunicationPoint,\n') + file.write (13 * ' ' + "ctx.resultFile, ',', fmi2False);\n") + file.write('\n'); + + file.write (2 * ' ' + '/* Get the outputs */\n') + for svar in tree.xpath("/fmiModelDescription/ModelVariables/ScalarVariable"): + if svar.get('causality') == 'output': + file.write(2 * ' ' + 'vr = getValueReference (' + svar.get('name') + + '_sv);\n') + file.write(2 * ' ' + + 'fmi2Flag = ctx.fmu->getReal (ctx.component, &vr, 1, &r);\n') + file.write (2 * ' ' + '*' + svar.get('name') + '_out = r;\n') + file.write('\n'); + + file.write(2 * ' ' + + 'ctx.currentCommunicationPoint += ctx.communicationStepSize;\n'); + file.write('\n'); + file.write(2 * ' ' + '/* 3) terminate the simulation */\n'); + file.write('\n'); + file.write(2 * ' ' + 'if (ctx.currentCommunicationPoint > tEnd) {\n'); + file.write(4 * ' ' + 'freeContext (ctx);\n'); + file.write(4 * ' ' + 'exit (0);\n'); + file.write(2 * ' ' + '}\n'); + file.write('}\n'); + + file.close() + ################################################################################ def fmu2aadl(fmu_file): ''' @@ -155,8 +270,6 @@ def fmu2aadl(fmu_file): ''' - print os.path.dirname(__file__) - # Unzip FMU _unzipModelDescriptionFile(fmu_file) @@ -178,6 +291,14 @@ def fmu2aadl(fmu_file): FMU2AADL_Thread_Impl(root,tree,file) FMU2AADL_Epilogue(root, file) + # Build C wrapper file + + cFile = root.get('modelName').lower() + '_fmu_wrapper.c' + file = open(cFile,'w') + print "Generating C file: " + cFile + + FMU2C_Wrapper (root, tree, file) + # Copy required runtime files shutil.copy(os.path.dirname(__file__) + "/fmu_wrapper.c", ".") diff --git a/models/CrazyFlie/AA_control.mo b/models/CrazyFlie/AA_control.mo new file mode 100644 index 0000000..8afdc34 --- /dev/null +++ b/models/CrazyFlie/AA_control.mo @@ -0,0 +1,55 @@ +model AA_control +import Modelica.SIunits; + final constant Real g = Modelica.Constants.g_n; //Acceleration of the gravity + parameter SIunits.Mass m = 0.65; + parameter SIunits.Length l = 0.23; // Length + parameter SIunits.MomentOfInertia Ix = 7.5e-3; //Quadrotor moment of inertia around X axis + parameter SIunits.MomentOfInertia Iy = 7.5e-3; //Quadrotor moment of inertia around Y axis + parameter SIunits.MomentOfInertia Iz = 1.3e-2; //Quadrotor moment of inertia around Z axis + parameter SIunits.MomentOfInertia Jr = 6.5e-5; //Total rotational moment of inertia around the propeller axis + parameter Real d = 7.5e-7; // Drag factor + parameter Real Kpz=90, Kdz=13.795, Kpp=50, Kdp=8, Kpt=50, Kdt=8.09, Kpps=50, Kdps=8.3996; + Modelica.Blocks.Interfaces.RealInput Phi annotation( + Placement(visible = true, transformation(origin = {-120, 240}, extent = {{-20, -20}, {20, 20}}, rotation = 0), iconTransformation(origin = {-120, 240}, extent = {{-20, -20}, {20, 20}}, rotation = 0))); + Modelica.Blocks.Interfaces.RealInput Theta annotation( + Placement(visible = true, transformation(origin = {-120, 180}, extent = {{-20, -20}, {20, 20}}, rotation = 0), iconTransformation(origin = {-120, 180}, extent = {{-20, -20}, {20, 20}}, rotation = 0))); + Modelica.Blocks.Interfaces.RealInput Psi annotation( + Placement(visible = true, transformation(origin = {-120, 120}, extent = {{-20, -20}, {20, 20}}, rotation = 0), iconTransformation(origin = {-120, 120}, extent = {{-20, -20}, {20, 20}}, rotation = 0))); + Modelica.Blocks.Interfaces.RealInput Phid annotation( + Placement(visible = true, transformation(origin = {-120, 60}, extent = {{-20, -20}, {20, 20}}, rotation = 0), iconTransformation(origin = {-120, 60}, extent = {{-20, -20}, {20, 20}}, rotation = 0))); + Modelica.Blocks.Interfaces.RealInput Thetad annotation( + Placement(visible = true, transformation(origin = {-120, 0}, extent = {{-20, -20}, {20, 20}}, rotation = 0), iconTransformation(origin = {-120, 0}, extent = {{-20, -20}, {20, 20}}, rotation = 0))); + Modelica.Blocks.Interfaces.RealInput Psid annotation( + Placement(visible = true, transformation(origin = {-120, -60}, extent = {{-20, -20}, {20, 20}}, rotation = 0), iconTransformation(origin = {-120, -60}, extent = {{-20, -20}, {20, 20}}, rotation = 0))); + Modelica.Blocks.Interfaces.RealInput Z annotation( + Placement(visible = true, transformation(origin = {-120, -120}, extent = {{-20, -20}, {20, 20}}, rotation = 0), iconTransformation(origin = {-120, -120}, extent = {{-20, -20}, {20, 20}}, rotation = 0))); + Modelica.Blocks.Interfaces.RealInput Zd annotation( + Placement(visible = true, transformation(origin = {-120, -180}, extent = {{-20, -20}, {20, 20}}, rotation = 0), iconTransformation(origin = {-120, -180}, extent = {{-20, -20}, {20, 20}}, rotation = 0))); + Modelica.Blocks.Interfaces.RealInput Omega annotation( + Placement(visible = true, transformation(origin = {-120, -240}, extent = {{-20, -20}, {20, 20}}, rotation = 0), iconTransformation(origin = {-120, -240}, extent = {{-20, -20}, {20, 20}}, rotation = 0))); + + Modelica.Blocks.Interfaces.RealOutput T annotation( + Placement(visible = true, transformation(origin = {110, 100}, extent = {{-10, -10}, {10, 10}}, rotation = 0), iconTransformation(origin = {110, 100}, extent = {{-10, -10}, {10, 10}}, rotation = 0))); + Modelica.Blocks.Interfaces.RealOutput tau_phi annotation( + Placement(visible = true, transformation(origin = {110, 50}, extent = {{-10, -10}, {10, 10}}, rotation = 0), iconTransformation(origin = {110, 50}, extent = {{-10, -10}, {10, 10}}, rotation = 0))); + Modelica.Blocks.Interfaces.RealOutput tau_theta annotation( + Placement(visible = true, transformation(origin = {110, -50}, extent = {{-10, -10}, {10, 10}}, rotation = 0), iconTransformation(origin = {110, -50}, extent = {{-10, -10}, {10, 10}}, rotation = 0))); + Modelica.Blocks.Interfaces.RealOutput tau_psi annotation( + Placement(visible = true, transformation(origin = {110, -100}, extent = {{-10, -10}, {10, 10}}, rotation = 0), iconTransformation(origin = {110, -100}, extent = {{-10, -10}, {10, 10}}, rotation = 0))); +Real Z_dot, Phi_dot, Theta_dot, Psi_dot; + +equation +Z_dot=der(Z);Phi_dot=der(Phi);Theta_dot=der(Theta);Psi_dot=der(Psi); +T=(Kpz*(Zd-Z)-Kdz*Z_dot+g)*m/(cos(Phi)*cos(Theta))+d*Z_dot; +tau_phi=Kpp*(Phid-Phi)-Kdp*Phi_dot-(Iy-Iz)/l*Theta_dot*Psi_dot-Jr/l*Theta_dot*Omega; +tau_theta=Kpt*(Thetad-Theta)-Kdt*Theta_dot-(Iz-Ix)/l*Phi_dot*Psi_dot+Jr/l*Phi_dot*Omega; +tau_psi=Kpps*(Psid-Psi)-Kdps*Psi_dot-(Ix-Iy)/l*Phi_dot*Theta_dot; + +annotation( + uses(Modelica(version = "3.2.2")), + Icon(coordinateSystem(extent = {{-100, -280}, {100, 280}}),graphics = {Rectangle(extent = {{-100, 280}, {100, -280}})}), + Diagram(coordinateSystem(extent = {{-100, -280}, {100, 280}})), + version = "", + __OpenModelica_commandLineOptions = ""); + +end AA_control; \ No newline at end of file diff --git a/models/CrazyFlie/Angles.mo b/models/CrazyFlie/Angles.mo new file mode 100644 index 0000000..7e6f678 --- /dev/null +++ b/models/CrazyFlie/Angles.mo @@ -0,0 +1,41 @@ +model Angles + + parameter Real l = 0.23; // Length + parameter Real Ix = 0.0075; //Quadrotor moment of inertia around X axis + parameter Real Iy = 0.0075; //Quadrotor moment of inertia around Y axis + parameter Real Iz = 0.013; //Quadrotor moment of inertia around Z axis + parameter Real Jr = 6.5e-5; //Total rotational moment of inertia around the propeller axis + parameter Real b = 3.13e-5; //Thrust factor + parameter Real d = 7.5e-7; // Drag factor + + Modelica.Blocks.Interfaces.RealInput u2 annotation( + Placement(visible = true, transformation(origin = {-120, 20}, extent = {{-20, -20}, {20, 20}}, rotation = 0), iconTransformation(origin = {-120, 22}, extent = {{-20, -20}, {20, 20}}, rotation = 0))); + Modelica.Blocks.Interfaces.RealInput u3 annotation( + Placement(visible = true, transformation(origin = {-120, -20}, extent = {{-20, -20}, {20, 20}}, rotation = 0), iconTransformation(origin = {-120, -22}, extent = {{-20, -20}, {20, 20}}, rotation = 0))); + Modelica.Blocks.Interfaces.RealInput u4 annotation( + Placement(visible = true, transformation(origin = {-120, -60}, extent = {{-20, -20}, {20, 20}}, rotation = 0), iconTransformation(origin = {-120, -66}, extent = {{-20, -20}, {20, 20}}, rotation = 0))); + Modelica.Blocks.Interfaces.RealInput omega annotation( + Placement(visible = true, transformation(origin = {-120, -100}, extent = {{-20, -20}, {20, 20}}, rotation = 0), iconTransformation(origin = {-120, -110}, extent = {{-20, -20}, {20, 20}}, rotation = 0))); + Modelica.Blocks.Interfaces.RealOutput Phi_dot_dot annotation( + Placement(visible = true, transformation(origin = {110, 120}, extent = {{-10, -10}, {10, 10}}, rotation = 0), iconTransformation(origin = {110, 110}, extent = {{-10, -10}, {10, 10}}, rotation = 0))); + Modelica.Blocks.Interfaces.RealOutput Theta_dot_dot annotation( + Placement(visible = true, transformation(origin = {110, 30}, extent = {{-10, -10}, {10, 10}}, rotation = 0), iconTransformation(origin = {110, 20}, extent = {{-10, -10}, {10, 10}}, rotation = 0))); + Modelica.Blocks.Interfaces.RealOutput Psi_dot_dot annotation( + Placement(visible = true, transformation(origin = {110, -80}, extent = {{-10, -10}, {10, 10}}, rotation = 0), iconTransformation(origin = {110, -70}, extent = {{-10, -10}, {10, 10}}, rotation = 0))); + Modelica.Blocks.Interfaces.RealInput Phi_dot annotation( + Placement(visible = true, transformation(origin = {-120, 140}, extent = {{-20, -20}, {20, 20}}, rotation = 0), iconTransformation(origin = {-120, 150}, extent = {{-20, -20}, {20, 20}}, rotation = 0))); + Modelica.Blocks.Interfaces.RealInput Psi_dot annotation( + Placement(visible = true, transformation(origin = {-120, 60}, extent = {{-20, -20}, {20, 20}}, rotation = 0), iconTransformation(origin = {-120, 66}, extent = {{-20, -20}, {20, 20}}, rotation = 0))); + Modelica.Blocks.Interfaces.RealInput Theta_dot annotation( + Placement(visible = true, transformation(origin = {-120, 100}, extent = {{-20, -20}, {20, 20}}, rotation = 0), iconTransformation(origin = {-120, 108}, extent = {{-20, -20}, {20, 20}}, rotation = 0))); +equation + Phi_dot_dot = ((Iy - Iz)/Ix)*Theta_dot*Psi_dot + (Jr/Ix)*Theta_dot*omega + (l/Ix)*u2; + Theta_dot_dot = ((Iz - Ix)/Iy)*Phi_dot*Psi_dot - (Jr/Iy)*Phi_dot*omega + (l/Iy)*u3; + Psi_dot_dot = ((Ix - Iy)/Iz)*Phi_dot*Theta_dot + (1/Iz)*u4; +annotation( + uses(Modelica(version = "3.2.2")), + Icon(coordinateSystem(extent = {{-100, -120}, {100, 160}}),graphics = {Rectangle(origin = {0, 40}, extent = {{-100, 120}, {100, -160}})}), + Diagram(coordinateSystem(extent = {{-100, -120}, {100, 160}})), + version = "", + __OpenModelica_commandLineOptions = ""); +end Angles; diff --git a/models/CrazyFlie/Angles_Subsystem.mo b/models/CrazyFlie/Angles_Subsystem.mo new file mode 100644 index 0000000..cbce8dc --- /dev/null +++ b/models/CrazyFlie/Angles_Subsystem.mo @@ -0,0 +1,66 @@ +model Angles_Subsystem + Angles angles1 annotation( + Placement(visible = true, transformation(origin = {-14, 8}, extent = {{-10, -12}, {10, 16}}, rotation = 0))); + Modelica.Blocks.Interfaces.RealInput u2 annotation( + Placement(visible = true, transformation(origin = {-120, 80}, extent = {{-20, -20}, {20, 20}}, rotation = 0), iconTransformation(origin = {-120, 80}, extent = {{-20, -20}, {20, 20}}, rotation = 0))); + Modelica.Blocks.Interfaces.RealInput u3 annotation( + Placement(visible = true, transformation(origin = {-120, 30}, extent = {{-20, -20}, {20, 20}}, rotation = 0), iconTransformation(origin = {-120, 30}, extent = {{-20, -20}, {20, 20}}, rotation = 0))); + Modelica.Blocks.Interfaces.RealInput u4 annotation( + Placement(visible = true, transformation(origin = {-120, -20}, extent = {{-20, -20}, {20, 20}}, rotation = 0), iconTransformation(origin = {-120, -20}, extent = {{-20, -20}, {20, 20}}, rotation = 0))); + Modelica.Blocks.Interfaces.RealInput omega annotation( + Placement(visible = true, transformation(origin = {-120, -70}, extent = {{-20, -20}, {20, 20}}, rotation = 0), iconTransformation(origin = {-120, -70}, extent = {{-20, -20}, {20, 20}}, rotation = 0))); + Modelica.Blocks.Continuous.Integrator integrator1 annotation( + Placement(visible = true, transformation(origin = {32, 20}, extent = {{-10, -10}, {10, 10}}, rotation = 0))); + Modelica.Blocks.Continuous.Integrator integrator2 annotation( + Placement(visible = true, transformation(origin = {74, 20}, extent = {{-10, -10}, {10, 10}}, rotation = 0))); + Modelica.Blocks.Interfaces.RealOutput phi annotation( + Placement(visible = true, transformation(origin = {110, 60}, extent = {{-10, -10}, {10, 10}}, rotation = 0), iconTransformation(origin = {110, 60}, extent = {{-10, -10}, {10, 10}}, rotation = 0))); + Modelica.Blocks.Interfaces.RealOutput theta annotation( + Placement(visible = true, transformation(origin = {110, 0}, extent = {{-10, -10}, {10, 10}}, rotation = 0), iconTransformation(origin = {110, 0}, extent = {{-10, -10}, {10, 10}}, rotation = 0))); + Modelica.Blocks.Interfaces.RealOutput psi annotation( + Placement(visible = true, transformation(origin = {110, -60}, extent = {{-10, -10}, {10, 10}}, rotation = 0), iconTransformation(origin = {110, -60}, extent = {{-10, -10}, {10, 10}}, rotation = 0))); + Modelica.Blocks.Continuous.Integrator integrator3 annotation( + Placement(visible = true, transformation(origin = {32, -10}, extent = {{-10, -10}, {10, 10}}, rotation = 0))); + Modelica.Blocks.Continuous.Integrator integrator4 annotation( + Placement(visible = true, transformation(origin = {72, -10}, extent = {{-10, -10}, {10, 10}}, rotation = 0))); + Modelica.Blocks.Continuous.Integrator integrator5 annotation( + Placement(visible = true, transformation(origin = {30, -50}, extent = {{-10, -10}, {10, 10}}, rotation = 0))); + Modelica.Blocks.Continuous.Integrator integrator6 annotation( + Placement(visible = true, transformation(origin = {80, -60}, extent = {{-10, -10}, {10, 10}}, rotation = 0))); +equation + connect(integrator6.y, psi) annotation( + Line(points = {{92, -60}, {104, -60}, {104, -60}, {110, -60}}, color = {0, 0, 127})); + connect(integrator5.y, integrator6.u) annotation( + Line(points = {{42, -50}, {48, -50}, {48, -60}, {68, -60}, {68, -60}}, color = {0, 0, 127})); + connect(integrator5.y, angles1.Psi_dot) annotation( + Line(points = {{42, -50}, {56, -50}, {56, 68}, {-46, 68}, {-46, 14}, {-26, 14}, {-26, 14}}, color = {0, 0, 127})); + connect(angles1.Psi_dot_dot, integrator5.u) annotation( + Line(points = {{-2, 2}, {6, 2}, {6, -50}, {18, -50}, {18, -50}}, color = {0, 0, 127})); + connect(integrator3.y, angles1.Theta_dot) annotation( + Line(points = {{44, -10}, {48, -10}, {48, 60}, {-44, 60}, {-44, 18}, {-26, 18}, {-26, 18}}, color = {0, 0, 127})); + connect(integrator4.y, theta) annotation( + Line(points = {{84, -10}, {90, -10}, {90, 0}, {110, 0}, {110, 0}}, color = {0, 0, 127})); + connect(integrator3.y, integrator4.u) annotation( + Line(points = {{44, -10}, {60, -10}, {60, -10}, {60, -10}}, color = {0, 0, 127})); + connect(angles1.Theta_dot_dot, integrator3.u) annotation( + Line(points = {{-2, 10}, {12, 10}, {12, -10}, {20, -10}, {20, -10}}, color = {0, 0, 127})); + connect(integrator2.y, phi) annotation( + Line(points = {{86, 20}, {92, 20}, {92, 60}, {110, 60}, {110, 60}}, color = {0, 0, 127})); + connect(integrator1.y, angles1.Phi_dot) annotation( + Line(points = {{44, 20}, {52, 20}, {52, 56}, {-40, 56}, {-40, 22}, {-26, 22}, {-26, 24}}, color = {0, 0, 127})); + connect(integrator1.y, integrator2.u) annotation( + Line(points = {{44, 20}, {62, 20}, {62, 20}, {62, 20}}, color = {0, 0, 127})); + connect(angles1.Phi_dot_dot, integrator1.u) annotation( + Line(points = {{-2, 20}, {20, 20}, {20, 20}, {20, 20}}, color = {0, 0, 127})); + connect(u2, angles1.u2) annotation( + Line(points = {{-120, 80}, {-50, 80}, {-50, 10}, {-26, 10}, {-26, 10}}, color = {0, 0, 127})); + connect(u3, angles1.u3) annotation( + Line(points = {{-120, 30}, {-58, 30}, {-58, 6}, {-26, 6}, {-26, 6}}, color = {0, 0, 127})); + connect(u4, angles1.u4) annotation( + Line(points = {{-120, -20}, {-88, -20}, {-88, 2}, {-26, 2}, {-26, 2}}, color = {0, 0, 127})); + connect(omega, angles1.omega) annotation( + Line(points = {{-120, -70}, {-74, -70}, {-74, -4}, {-26, -4}, {-26, -3}}, color = {0, 0, 127})); + annotation( + uses(Modelica(version = "3.2.2")), + Icon(graphics = {Rectangle(origin = {-35, 41}, extent = {{-65, 59}, {135, -141}})})); +end Angles_Subsystem; diff --git a/models/CrazyFlie/CrazyFlie.py b/models/CrazyFlie/CrazyFlie.py new file mode 100644 index 0000000..3cf41d9 --- /dev/null +++ b/models/CrazyFlie/CrazyFlie.py @@ -0,0 +1,6 @@ +from pymodelica import compile_fmu +from pyfmi import load_fmu + +def run_demo(): + print("Compiling the FMUs") + system_model = compile_fmu("Quadrotor_all","Quadrotor_all.mo", target='cs', version='2.0'); diff --git a/models/CrazyFlie/CrazyFlie.pyc b/models/CrazyFlie/CrazyFlie.pyc new file mode 100644 index 0000000..2fccad7 Binary files /dev/null and b/models/CrazyFlie/CrazyFlie.pyc differ diff --git a/models/CrazyFlie/CrazyFlie/.project b/models/CrazyFlie/CrazyFlie/.project new file mode 100644 index 0000000..271c365 --- /dev/null +++ b/models/CrazyFlie/CrazyFlie/.project @@ -0,0 +1,12 @@ + + + CrazyFlie + + + + + + + org.eclipse.sirius.nature.modelingproject + + diff --git a/models/CrazyFlie/CrazyFlie/CrazyFlie.csgraph b/models/CrazyFlie/CrazyFlie/CrazyFlie.csgraph new file mode 100644 index 0000000..69f7b7b --- /dev/null +++ b/models/CrazyFlie/CrazyFlie/CrazyFlie.csgraph @@ -0,0 +1,318 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/models/CrazyFlie/CrazyFlie/representations.aird b/models/CrazyFlie/CrazyFlie/representations.aird new file mode 100644 index 0000000..aeda2b9 --- /dev/null +++ b/models/CrazyFlie/CrazyFlie/representations.aird @@ -0,0 +1,5259 @@ + + + + CrazyFlie.csgraph + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + KEEP_LOCATION + KEEP_SIZE + KEEP_RATIO + + + + + + + + + KEEP_LOCATION + KEEP_SIZE + KEEP_RATIO + + + + + + + + + KEEP_LOCATION + KEEP_SIZE + KEEP_RATIO + + + + + + + + + KEEP_LOCATION + KEEP_SIZE + KEEP_RATIO + + + + + + + + + KEEP_LOCATION + KEEP_SIZE + KEEP_RATIO + + + + + + + + + KEEP_LOCATION + KEEP_SIZE + KEEP_RATIO + + + + + + + + + KEEP_LOCATION + KEEP_SIZE + KEEP_RATIO + + + + + + + + + KEEP_LOCATION + KEEP_SIZE + KEEP_RATIO + + + + + + + + + KEEP_LOCATION + KEEP_SIZE + KEEP_RATIO + + + + + + + + + KEEP_LOCATION + KEEP_SIZE + KEEP_RATIO + + + + + + + + + KEEP_LOCATION + KEEP_SIZE + KEEP_RATIO + + + + + + + + + KEEP_LOCATION + KEEP_SIZE + KEEP_RATIO + + + + + + + + + KEEP_LOCATION + KEEP_SIZE + KEEP_RATIO + + + + + + KEEP_LOCATION + KEEP_SIZE + KEEP_RATIO + + + + + + + + + + + + KEEP_LOCATION + KEEP_SIZE + KEEP_RATIO + + + + + + + + + KEEP_LOCATION + KEEP_SIZE + KEEP_RATIO + + + + + + + + + KEEP_LOCATION + KEEP_SIZE + KEEP_RATIO + + + + + + + + + KEEP_LOCATION + KEEP_SIZE + KEEP_RATIO + + + + + + + + + KEEP_LOCATION + KEEP_SIZE + KEEP_RATIO + + + + + + + + + KEEP_LOCATION + KEEP_SIZE + KEEP_RATIO + + + + + + + + + KEEP_LOCATION + KEEP_SIZE + KEEP_RATIO + + + + + + + + + KEEP_LOCATION + KEEP_SIZE + KEEP_RATIO + + + + + + + + + KEEP_LOCATION + KEEP_SIZE + KEEP_RATIO + + + + + + + + + KEEP_LOCATION + KEEP_SIZE + KEEP_RATIO + + + + + + + + + KEEP_LOCATION + KEEP_SIZE + KEEP_RATIO + + + + + + KEEP_LOCATION + KEEP_SIZE + KEEP_RATIO + + + + + + + + + + + + KEEP_LOCATION + KEEP_SIZE + KEEP_RATIO + + + + + + + + + KEEP_LOCATION + KEEP_SIZE + KEEP_RATIO + + + + + + + + + KEEP_LOCATION + KEEP_SIZE + KEEP_RATIO + + + + + + + + + KEEP_LOCATION + KEEP_SIZE + KEEP_RATIO + + + + + + KEEP_LOCATION + KEEP_SIZE + KEEP_RATIO + + + + + + + + + + + + KEEP_LOCATION + KEEP_SIZE + KEEP_RATIO + + + + + + + + + KEEP_LOCATION + KEEP_SIZE + KEEP_RATIO + + + + + + + + + KEEP_LOCATION + KEEP_SIZE + KEEP_RATIO + + + + + + + + + KEEP_LOCATION + KEEP_SIZE + KEEP_RATIO + + + + + + + + + KEEP_LOCATION + KEEP_SIZE + KEEP_RATIO + + + + + + + + + KEEP_LOCATION + KEEP_SIZE + KEEP_RATIO + + + + + + + + + KEEP_LOCATION + KEEP_SIZE + KEEP_RATIO + + + + + + + + + KEEP_LOCATION + KEEP_SIZE + KEEP_RATIO + + + + + + + + + KEEP_LOCATION + KEEP_SIZE + KEEP_RATIO + + + + + + KEEP_LOCATION + KEEP_SIZE + KEEP_RATIO + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + KEEP_LOCATION + KEEP_SIZE + KEEP_RATIO + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + KEEP_LOCATION + KEEP_SIZE + KEEP_RATIO + + + + + + + + + + + + + + + + + KEEP_LOCATION + KEEP_SIZE + KEEP_RATIO + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + KEEP_LOCATION + KEEP_SIZE + KEEP_RATIO + + + + + + + + + + + + + + + + + KEEP_LOCATION + KEEP_SIZE + KEEP_RATIO + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/models/CrazyFlie/Displacement.mo b/models/CrazyFlie/Displacement.mo new file mode 100644 index 0000000..5d35acc --- /dev/null +++ b/models/CrazyFlie/Displacement.mo @@ -0,0 +1,42 @@ +model Displacement +final constant Real g = Modelica.Constants.g_n; //Acceleration of the gravity + parameter Real m = 0.65; // Mass + parameter Real Ix = 7.5e-3; //Quadrotor moment of inertia around X axis + parameter Real Iy = 7.5e-3; //Quadrotor moment of inertia around Y axis + parameter Real Iz = 1.3e-3; //Quadrotor moment of inertia around Z axis + parameter Real Jr = 6.5e-5; //Total rotational moment of inertia around the propeller axis + parameter Real b = 3.13e-5; //Thrust factor + parameter Real d = 7.5e-7; // Drag factor + + Modelica.Blocks.Interfaces.RealInput u1 annotation( + Placement(visible = true, transformation(origin = {-120, 105}, extent = {{-20, -20}, {20, 20}}, rotation = 0), iconTransformation(origin = {-120, 148}, extent = {{-20, -20}, {20, 20}}, rotation = 0))); + Modelica.Blocks.Interfaces.RealInput Phi annotation( + Placement(visible = true, transformation(origin = {-120, 71}, extent = {{-20, -20}, {20, 20}}, rotation = 0), iconTransformation(origin = {-120, 99}, extent = {{-20, -20}, {20, 20}}, rotation = 0))); + Modelica.Blocks.Interfaces.RealInput Theta annotation( + Placement(visible = true, transformation(origin = {-120, 34}, extent = {{-20, -20}, {20, 20}}, rotation = 0), iconTransformation(origin = {-120, 51}, extent = {{-20, -20}, {20, 20}}, rotation = 0))); + Modelica.Blocks.Interfaces.RealInput Psi annotation( + Placement(visible = true, transformation(origin = {-120, 0}, extent = {{-20, -20}, {20, 20}}, rotation = 0), iconTransformation(origin = {-120, -2}, extent = {{-20, -20}, {20, 20}}, rotation = 0))); + Modelica.Blocks.Interfaces.RealOutput X_dot_dot annotation( + Placement(visible = true, transformation(origin = {110, 90}, extent = {{-10, -10}, {10, 10}}, rotation = 0), iconTransformation(origin = {110, 90}, extent = {{-10, -10}, {10, 10}}, rotation = 0))); + Modelica.Blocks.Interfaces.RealOutput Y_dot_dot annotation( + Placement(visible = true, transformation(origin = {110, 0}, extent = {{-10, -10}, {10, 10}}, rotation = 0), iconTransformation(origin = {110, 0}, extent = {{-10, -10}, {10, 10}}, rotation = 0))); + Modelica.Blocks.Interfaces.RealOutput Z_dot_dot annotation( + Placement(visible = true, transformation(origin = {110, -90}, extent = {{-10, -10}, {10, 10}}, rotation = 0), iconTransformation(origin = {110, -90}, extent = {{-10, -10}, {10, 10}}, rotation = 0))); + Modelica.Blocks.Interfaces.RealInput X_dot annotation( + Placement(visible = true, transformation(origin = {-120, -40}, extent = {{-20, -20}, {20, 20}}, rotation = 0), iconTransformation(origin = {-120, -54}, extent = {{-20, -20}, {20, 20}}, rotation = 0))); + Modelica.Blocks.Interfaces.RealInput Y_dot annotation( + Placement(visible = true, transformation(origin = {-120, -80}, extent = {{-20, -20}, {20, 20}}, rotation = 0), iconTransformation(origin = {-122, -109}, extent = {{-20, -20}, {20, 20}}, rotation = 0))); + Modelica.Blocks.Interfaces.RealInput Z_dot annotation( + Placement(visible = true, transformation(origin = {-120, -120}, extent = {{-20, -20}, {20, 20}}, rotation = 0), iconTransformation(origin = {-122, -158}, extent = {{-20, -20}, {20, 20}}, rotation = 0))); +equation +X_dot_dot = (cos(Phi)*sin(Theta)*cos(Psi) + sin(Phi)*sin(Psi))*u1/m - X_dot*d/m; +Y_dot_dot = (cos(Phi)*sin(Theta)*sin(Psi) - sin(Phi)*cos(Psi))*u1/m - Y_dot*d/m; +Z_dot_dot = cos(Phi)*cos(Theta)*u1/m - Z_dot*d/m - g; + + +annotation( + Diagram(coordinateSystem(grid = {2, 1}, extent = {{-100, -160}, {100, 140}})), + Icon(coordinateSystem(grid = {2, 1}, extent = {{-100, -160}, {100, 140}}), graphics = {Rectangle(origin = {0, 0}, extent = {{-100, 160}, {100, -180}})}), + version = "", + uses(Modelica(version = "3.2.2")), + __OpenModelica_commandLineOptions = "");end Displacement; diff --git a/models/CrazyFlie/Displacement_Subsystem.mo b/models/CrazyFlie/Displacement_Subsystem.mo new file mode 100644 index 0000000..79da6ac --- /dev/null +++ b/models/CrazyFlie/Displacement_Subsystem.mo @@ -0,0 +1,66 @@ +model Displacement_Subsystem + Displacement displacement1 annotation( + Placement(visible = true, transformation(origin = {-40, 8}, extent = {{-10, -16}, {10, 14}}, rotation = 0))); + Modelica.Blocks.Interfaces.RealInput phi annotation( + Placement(visible = true, transformation(origin = {-120, 80}, extent = {{-20, -20}, {20, 20}}, rotation = 0), iconTransformation(origin = {-120, 80}, extent = {{-20, -20}, {20, 20}}, rotation = 0))); + Modelica.Blocks.Interfaces.RealInput theta annotation( + Placement(visible = true, transformation(origin = {-120, 30}, extent = {{-20, -20}, {20, 20}}, rotation = 0), iconTransformation(origin = {-120, 30}, extent = {{-20, -20}, {20, 20}}, rotation = 0))); + Modelica.Blocks.Interfaces.RealInput psi annotation( + Placement(visible = true, transformation(origin = {-120, -20}, extent = {{-20, -20}, {20, 20}}, rotation = 0), iconTransformation(origin = {-120, -20}, extent = {{-20, -20}, {20, 20}}, rotation = 0))); + Modelica.Blocks.Interfaces.RealInput u1 annotation( + Placement(visible = true, transformation(origin = {-120, -70}, extent = {{-20, -20}, {20, 20}}, rotation = 0), iconTransformation(origin = {-120, -70}, extent = {{-20, -20}, {20, 20}}, rotation = 0))); + Modelica.Blocks.Interfaces.RealOutput x annotation( + Placement(visible = true, transformation(origin = {110, 60}, extent = {{-10, -10}, {10, 10}}, rotation = 0), iconTransformation(origin = {110, 60}, extent = {{-10, -10}, {10, 10}}, rotation = 0))); + Modelica.Blocks.Interfaces.RealOutput y annotation( + Placement(visible = true, transformation(origin = {110, 0}, extent = {{-10, -10}, {10, 10}}, rotation = 0), iconTransformation(origin = {110, 0}, extent = {{-10, -10}, {10, 10}}, rotation = 0))); + Modelica.Blocks.Interfaces.RealOutput z annotation( + Placement(visible = true, transformation(origin = {110, -60}, extent = {{-10, -10}, {10, 10}}, rotation = 0), iconTransformation(origin = {110, -60}, extent = {{-10, -10}, {10, 10}}, rotation = 0))); + Modelica.Blocks.Continuous.Integrator integrator1 annotation( + Placement(visible = true, transformation(origin = {28, 52}, extent = {{-10, -10}, {10, 10}}, rotation = 0))); + Modelica.Blocks.Continuous.Integrator integrator2 annotation( + Placement(visible = true, transformation(origin = {66, 52}, extent = {{-10, -10}, {10, 10}}, rotation = 0))); + Modelica.Blocks.Continuous.Integrator integrator3 annotation( + Placement(visible = true, transformation(origin = {28, 16}, extent = {{-10, -10}, {10, 10}}, rotation = 0))); + Modelica.Blocks.Continuous.Integrator integrator4 annotation( + Placement(visible = true, transformation(origin = {64, 16}, extent = {{-10, -10}, {10, 10}}, rotation = 0))); + Modelica.Blocks.Continuous.Integrator integrator5 annotation( + Placement(visible = true, transformation(origin = {26, -26}, extent = {{-10, -10}, {10, 10}}, rotation = 0))); + Modelica.Blocks.Continuous.Integrator integrator6 annotation( + Placement(visible = true, transformation(origin = {58, -26}, extent = {{-10, -10}, {10, 10}}, rotation = 0))); +equation + connect(u1, displacement1.u1) annotation( + Line(points = {{-120, -70}, {-74, -70}, {-74, 22}, {-52, 22}, {-52, 22}}, color = {0, 0, 127})); + connect(psi, displacement1.Psi) annotation( + Line(points = {{-120, -20}, {-80, -20}, {-80, 8}, {-52, 8}, {-52, 8}}, color = {0, 0, 127})); + connect(theta, displacement1.Theta) annotation( + Line(points = {{-120, 30}, {-80, 30}, {-80, 12}, {-52, 12}, {-52, 14}}, color = {0, 0, 127})); + connect(phi, displacement1.Phi) annotation( + Line(points = {{-120, 80}, {-78, 80}, {-78, 18}, {-52, 18}, {-52, 18}}, color = {0, 0, 127})); + connect(integrator5.y, displacement1.Z_dot) annotation( + Line(points = {{38, -26}, {42, -26}, {42, 66}, {-66, 66}, {-66, -8}, {-52, -8}, {-52, -8}}, color = {0, 0, 127})); + connect(integrator3.y, displacement1.Y_dot) annotation( + Line(points = {{40, 16}, {48, 16}, {48, 86}, {-62, 86}, {-62, -2}, {-52, -2}, {-52, -2}}, color = {0, 0, 127})); + connect(integrator6.y, z) annotation( + Line(points = {{70, -26}, {82, -26}, {82, -60}, {110, -60}, {110, -60}}, color = {0, 0, 127})); + connect(integrator5.y, integrator6.u) annotation( + Line(points = {{38, -26}, {46, -26}, {46, -26}, {46, -26}}, color = {0, 0, 127})); + connect(displacement1.Z_dot_dot, integrator5.u) annotation( + Line(points = {{-28, -2}, {2, -2}, {2, -28}, {14, -28}, {14, -26}}, color = {0, 0, 127})); + connect(integrator4.y, y) annotation( + Line(points = {{76, 16}, {86, 16}, {86, 0}, {110, 0}, {110, 0}}, color = {0, 0, 127})); + connect(integrator3.y, integrator4.u) annotation( + Line(points = {{40, 16}, {50, 16}, {50, 16}, {52, 16}}, color = {0, 0, 127})); + connect(displacement1.Y_dot_dot, integrator3.u) annotation( + Line(points = {{-28, 8}, {4, 8}, {4, 16}, {16, 16}, {16, 16}}, color = {0, 0, 127})); + connect(integrator1.y, displacement1.X_dot) annotation( + Line(points = {{40, 52}, {50, 52}, {50, 82}, {-70, 82}, {-70, 2}, {-52, 2}, {-52, 2}}, color = {0, 0, 127})); + connect(integrator2.y, x) annotation( + Line(points = {{78, 52}, {90, 52}, {90, 60}, {110, 60}, {110, 60}}, color = {0, 0, 127})); + connect(integrator1.y, integrator2.u) annotation( + Line(points = {{40, 52}, {52, 52}, {52, 52}, {54, 52}}, color = {0, 0, 127})); + connect(displacement1.X_dot_dot, integrator1.u) annotation( + Line(points = {{-28, 18}, {-8, 18}, {-8, 52}, {16, 52}, {16, 52}}, color = {0, 0, 127})); + annotation( + uses(Modelica(version = "3.2.2")), + Icon(graphics = {Rectangle(origin = {-77, 91}, extent = {{-23, 9}, {177, -191}})})); +end Displacement_Subsystem; diff --git a/models/CrazyFlie/Drone_Model.mo b/models/CrazyFlie/Drone_Model.mo new file mode 100644 index 0000000..d67092c --- /dev/null +++ b/models/CrazyFlie/Drone_Model.mo @@ -0,0 +1,47 @@ +model Drone_Model + Speed_calculator speed_calculator1 annotation( + Placement(visible = true, transformation(origin = {-92.408, 29.891}, extent = {{-13.7424, -16.491}, {13.7424, 16.491}}, rotation = 0))); + Modelica.Blocks.Sources.Step step1(height = 1) annotation( + Placement(visible = true, transformation(origin = {-184, 48}, extent = {{-10, -10}, {10, 10}}, rotation = 0))); + Modelica.Blocks.Sources.Step step2(height = 1) annotation( + Placement(visible = true, transformation(origin = {-182, 24}, extent = {{-10, -10}, {10, 10}}, rotation = 0))); + Modelica.Blocks.Sources.Step step3(height = 1) annotation( + Placement(visible = true, transformation(origin = {-184, -4}, extent = {{-10, -10}, {10, 10}}, rotation = 0))); + Modelica.Blocks.Sources.Step step4(height = 1) annotation( + Placement(visible = true, transformation(origin = {-184, -32}, extent = {{-10, -10}, {10, 10}}, rotation = 0))); + Angles_Subsystem angles_Subsystem1 annotation( + Placement(visible = true, transformation(origin = {-10, 30}, extent = {{-10, -10}, {10, 10}}, rotation = 0))); + Displacement_Subsystem displacement_Subsystem1 annotation( + Placement(visible = true, transformation(origin = {50, 30}, extent = {{-10, -10}, {10, 10}}, rotation = 0))); +equation + connect(speed_calculator1.u1, displacement_Subsystem1.u1) annotation( + Line(points = {{-78, 36}, {-42, 36}, {-42, 12}, {30, 12}, {30, 22}, {38, 22}, {38, 24}}, color = {0, 0, 127})); + connect(angles_Subsystem1.psi, displacement_Subsystem1.psi) annotation( + Line(points = {{2, 24}, {26, 24}, {26, 28}, {38, 28}, {38, 28}}, color = {0, 0, 127})); + connect(angles_Subsystem1.theta, displacement_Subsystem1.theta) annotation( + Line(points = {{2, 30}, {28, 30}, {28, 32}, {38, 32}, {38, 34}}, color = {0, 0, 127})); + connect(angles_Subsystem1.phi, displacement_Subsystem1.phi) annotation( + Line(points = {{2, 36}, {26, 36}, {26, 38}, {38, 38}, {38, 38}}, color = {0, 0, 127})); + connect(speed_calculator1.u4, angles_Subsystem1.u4) annotation( + Line(points = {{-78, 16}, {-26, 16}, {-26, 28}, {-22, 28}, {-22, 28}}, color = {0, 0, 127})); + connect(speed_calculator1.u3, angles_Subsystem1.u3) annotation( + Line(points = {{-78, 24}, {-28, 24}, {-28, 34}, {-22, 34}, {-22, 34}}, color = {0, 0, 127})); + connect(speed_calculator1.u2, angles_Subsystem1.u2) annotation( + Line(points = {{-78, 30}, {-30, 30}, {-30, 38}, {-22, 38}, {-22, 38}}, color = {0, 0, 127})); + connect(speed_calculator1.omega, angles_Subsystem1.omega) annotation( + Line(points = {{-78, 44}, {-36, 44}, {-36, 22}, {-22, 22}, {-22, 24}}, color = {0, 0, 127})); + connect(step4.y, speed_calculator1.U4) annotation( + Line(points = {{-173, -32}, {-150, -32}, {-150, 17}, {-109, 17}}, color = {0, 0, 127})); + connect(step1.y, speed_calculator1.U1) annotation( + Line(points = {{-172, 48}, {-152, 48}, {-152, 43}, {-109, 43}}, color = {0, 0, 127})); + connect(step2.y, speed_calculator1.U2) annotation( + Line(points = {{-171, 24}, {-141, 24}, {-141, 35}, {-109, 35}}, color = {0, 0, 127})); + connect(step3.y, speed_calculator1.U3) annotation( + Line(points = {{-173, -4}, {-152, -4}, {-152, 26}, {-109, 26}}, color = {0, 0, 127})); + annotation( + uses(Modelica(version = "3.2.2")), + Diagram(coordinateSystem(extent = {{-200, -100}, {150, 100}})), + Icon(coordinateSystem(extent = {{-200, -100}, {150, 100}})), + version = "", + __OpenModelica_commandLineOptions = ""); +end Drone_Model; diff --git a/models/CrazyFlie/Drone_Model1.mo b/models/CrazyFlie/Drone_Model1.mo new file mode 100644 index 0000000..0d815a0 --- /dev/null +++ b/models/CrazyFlie/Drone_Model1.mo @@ -0,0 +1,83 @@ +model Drone_Model1 + Modelica.Blocks.Sources.Step Xd(height = 1, startTime = 0) annotation( + Placement(visible = true, transformation(origin = {-182, 58}, extent = {{-10, -10}, {10, 10}}, rotation = 0))); + Modelica.Blocks.Sources.Step Yd(height = 1, startTime = 0) annotation( + Placement(visible = true, transformation(origin = {-182, 24}, extent = {{-10, -10}, {10, 10}}, rotation = 0))); + Modelica.Blocks.Sources.Step Zd(height = 1, startTime = 0) annotation( + Placement(visible = true, transformation(origin = {-184, -4}, extent = {{-10, -10}, {10, 10}}, rotation = 0))); + Modelica.Blocks.Sources.Step Psid(height = 1, startTime = 0) annotation( + Placement(visible = true, transformation(origin = {-184, -32}, extent = {{-10, -10}, {10, 10}}, rotation = 0))); + Position_controller position_controller1 annotation( + Placement(visible = true, transformation(origin = {-107, 16.8}, extent = {{-21, -25.2}, {21, 25.2}}, rotation = 0))); + AA_control aA_control1 annotation( + Placement(visible = true, transformation(origin = {-45.5, -1}, extent = {{-14.5, -29}, {14.5, 29}}, rotation = 0))); + Angles angles1 annotation( + Placement(visible = true, transformation(origin = {56, 10}, extent = {{-10, -12}, {10, 12}}, rotation = 0))); + Displacement displacement1 annotation( + Placement(visible = true, transformation(origin = {100, 12}, extent = {{-10, -16}, {10, 14}}, rotation = 0))); + Speed_calculator speed_calculator1 annotation( + Placement(visible = true, transformation(origin = {6, 6}, extent = {{-18, -18}, {18, 18}}, rotation = 0))); +equation + connect(Xd.y, position_controller1.Xd) annotation( + Line(points = {{-170, 58}, {-154, 58}, {-154, 38}, {-132, 38}}, color = {0, 0, 127})); + connect(Yd.y, position_controller1.Yd) annotation( + Line(points = {{-170, 24}, {-132, 24}}, color = {0, 0, 127})); + connect(Zd.y, position_controller1.Zd) annotation( + Line(points = {{-172, -4}, {-132, -4}, {-132, 10}}, color = {0, 0, 127})); + connect(position_controller1.Phid, aA_control1.Phid) annotation( + Line(points = {{-84, 27}, {-76.5, 27}, {-76.5, 16}, {-63, 16}}, color = {0, 0, 127})); + connect(position_controller1.Thetad, aA_control1.Thetad) annotation( + Line(points = {{-84, 6}, {-63, 6}, {-63, -1}}, color = {0, 0, 127})); + connect(displacement1.X, position_controller1.X) annotation( + Line(points = {{112, 22}, {114, 22}, {114, 80}, {-144, 80}, {-144, 28}, {-132, 28}, {-132, 31}}, color = {0, 0, 127})); + connect(displacement1.Y, position_controller1.Y) annotation( + Line(points = {{112, 12}, {116, 12}, {116, 84}, {-148, 84}, {-148, 17}, {-132, 17}}, color = {0, 0, 127})); + connect(position_controller1.Z, displacement1.Z) annotation( + Line(points = {{-132, 3}, {-148, 3}, {-148, -42}, {120, -42}, {120, 2}, {112, 2}}, color = {0, 0, 127})); + connect(position_controller1.Psid, Psid.y) annotation( + Line(points = {{-132, -4}, {-172, -4}, {-172, -32}}, color = {0, 0, 127})); + connect(aA_control1.tau_psi, speed_calculator1.U4) annotation( + Line(points = {{-30, -30}, {-18, -30}, {-18, -8}, {-16, -8}}, color = {0, 0, 127})); + connect(aA_control1.tau_theta, speed_calculator1.U3) annotation( + Line(points = {{-30, -15}, {-25, -15}, {-25, 2}, {-16, 2}}, color = {0, 0, 127})); + connect(aA_control1.tau_phi, speed_calculator1.U2) annotation( + Line(points = {{-30, 13}, {-16, 13}, {-16, 12}}, color = {0, 0, 127})); + connect(aA_control1.T, speed_calculator1.U1) annotation( + Line(points = {{-30, 28}, {-16, 28}, {-16, 20}}, color = {0, 0, 127})); + connect(aA_control1.Omega, angles1.omega) annotation( + Line(points = {{-63, -71}, {-70, -71}, {-70, -38}, {44, -38}, {44, 1}}, color = {0, 0, 127})); + connect(aA_control1.Psid, Psid.y) annotation( + Line(points = {{-63, -18}, {-82, -18}, {-82, -32}, {-172, -32}}, color = {0, 0, 127})); + connect(aA_control1.Z, displacement1.Z) annotation( + Line(points = {{-63, -36}, {-80, -36}, {-80, -44}, {114, -44}, {114, 4}, {112, 4}, {112, 2}}, color = {0, 0, 127})); + connect(aA_control1.Zd, Zd.y) annotation( + Line(points = {{-63, -53}, {-78, -53}, {-78, -38}, {-162, -38}, {-162, -4}, {-172, -4}}, color = {0, 0, 127})); + connect(aA_control1.Psi, angles1.Psi) annotation( + Line(points = {{-63, 34}, {-78, 34}, {-78, 64}, {80, 64}, {80, 1}, {67, 1}}, color = {0, 0, 127})); + connect(aA_control1.Theta, angles1.Theta) annotation( + Line(points = {{-63, 51}, {-76, 51}, {-76, 60}, {76, 60}, {76, 10}, {67, 10}}, color = {0, 0, 127})); + connect(aA_control1.Phi, angles1.Phi) annotation( + Line(points = {{-63, 69}, {-74, 69}, {-74, 56}, {74, 56}, {74, 19}, {67, 19}}, color = {0, 0, 127})); + connect(displacement1.u1, speed_calculator1.u1) annotation( + Line(points = {{88, 24}, {32, 24}, {32, 14}, {26, 14}, {26, 14}}, color = {0, 0, 127})); + connect(speed_calculator1.omega, angles1.omega) annotation( + Line(points = {{26, 20}, {36, 20}, {36, 2}, {44, 2}, {44, 0}}, color = {0, 0, 127})); + connect(angles1.u2, speed_calculator1.u2) annotation( + Line(points = {{44, 20}, {38, 20}, {38, 6}, {26, 6}, {26, 6}}, color = {0, 0, 127})); + connect(angles1.u3, speed_calculator1.u3) annotation( + Line(points = {{44, 14}, {34, 14}, {34, -2}, {26, -2}, {26, -2}}, color = {0, 0, 127})); + connect(speed_calculator1.u4, angles1.u4) annotation( + Line(points = {{26, -8}, {40, -8}, {40, 6}, {44, 6}, {44, 6}}, color = {0, 0, 127})); + connect(angles1.Phi, displacement1.Phi) annotation( + Line(points = {{67, 19}, {86, 19}, {86, 16}, {88, 16}}, color = {0, 0, 127})); + connect(angles1.Theta, displacement1.Theta) annotation( + Line(points = {{67, 10}, {79.5, 10}, {79.5, 8}, {88, 8}}, color = {0, 0, 127})); + connect(angles1.Psi, displacement1.Psi) annotation( + Line(points = {{67, 1}, {78, 1}, {78, 0}, {88, 0}}, color = {0, 0, 127})); + annotation( + uses(Modelica(version = "3.2.2")), + Diagram(coordinateSystem(extent = {{-200, -100}, {150, 100}})), + Icon(coordinateSystem(extent = {{-200, -100}, {150, 100}})), + version = "", + __OpenModelica_commandLineOptions = ""); +end Drone_Model1; \ No newline at end of file diff --git a/models/CrazyFlie/Drone_Model_FMU.mo b/models/CrazyFlie/Drone_Model_FMU.mo new file mode 100644 index 0000000..c3a3dbc --- /dev/null +++ b/models/CrazyFlie/Drone_Model_FMU.mo @@ -0,0 +1,83 @@ +model Drone_Model_FMU + Modelica.Blocks.Sources.Step Xd(height = 1, startTime = 0) annotation( + Placement(visible = true, transformation(origin = {-182, 58}, extent = {{-10, -10}, {10, 10}}, rotation = 0))); + Modelica.Blocks.Sources.Step Yd(height = 1, startTime = 0) annotation( + Placement(visible = true, transformation(origin = {-182, 24}, extent = {{-10, -10}, {10, 10}}, rotation = 0))); + Modelica.Blocks.Sources.Step Zd(height = 1, startTime = 0) annotation( + Placement(visible = true, transformation(origin = {-184, -4}, extent = {{-10, -10}, {10, 10}}, rotation = 0))); + Modelica.Blocks.Sources.Step Psid(height = 1, startTime = 0) annotation( + Placement(visible = true, transformation(origin = {-184, -32}, extent = {{-10, -10}, {10, 10}}, rotation = 0))); + Position_controller position_controller1 annotation( + Placement(visible = true, transformation(origin = {-109, 8.8}, extent = {{-21, -25.2}, {21, 25.2}}, rotation = 0))); + AA_control aA_control1 annotation( + Placement(visible = true, transformation(origin = {-49.5, 9}, extent = {{-14.5, -29}, {14.5, 29}}, rotation = 0))); + Angles angles1 annotation( + Placement(visible = true, transformation(origin = {56, 10}, extent = {{-10, -12}, {10, 12}}, rotation = 0))); + Displacement displacement1 annotation( + Placement(visible = true, transformation(origin = {100, 12}, extent = {{-10, -16}, {10, 14}}, rotation = 0))); + Speed_calculator_me_FMU speed_calculator_me_FMU1 annotation( + Placement(visible = true, transformation(origin = {5, 11}, extent = {{-17, -17}, {17, 17}}, rotation = 0))); +equation + connect(speed_calculator_me_FMU1.u4, angles1.u4) annotation( + Line(points = {{24, 6}, {42, 6}, {42, 8}, {44, 8}}, color = {0, 0, 127})); + connect(speed_calculator_me_FMU1.u3, angles1.u3) annotation( + Line(points = {{24, 10}, {38, 10}, {38, 14}, {44, 14}, {44, 14}}, color = {0, 0, 127})); + connect(speed_calculator_me_FMU1.u2, angles1.u2) annotation( + Line(points = {{24, 14}, {34, 14}, {34, 18}, {44, 18}, {44, 20}}, color = {0, 0, 127})); + connect(speed_calculator_me_FMU1.u1, displacement1.u1) annotation( + Line(points = {{24, 18}, {30, 18}, {30, 30}, {86, 30}, {86, 22}, {88, 22}, {88, 24}}, color = {0, 0, 127})); + connect(speed_calculator_me_FMU1.omega, angles1.omega) annotation( + Line(points = {{24, 22}, {28, 22}, {28, 0}, {44, 0}, {44, 2}}, color = {0, 0, 127})); + connect(aA_control1.tau_psi, speed_calculator_me_FMU1.U4) annotation( + Line(points = {{-34, -6}, {-16, -6}, {-16, 10}, {-14, 10}}, color = {0, 0, 127})); + connect(aA_control1.tau_theta, speed_calculator_me_FMU1.U3) annotation( + Line(points = {{-34, 2}, {-20, 2}, {-20, 14}, {-14, 14}}, color = {0, 0, 127})); + connect(aA_control1.tau_phi, speed_calculator_me_FMU1.U2) annotation( + Line(points = {{-34, 16}, {-22, 16}, {-22, 19}, {-14, 19}}, color = {0, 0, 127})); + connect(aA_control1.T, speed_calculator_me_FMU1.U1) annotation( + Line(points = {{-34, 24}, {-14, 24}, {-14, 23}}, color = {0, 0, 127})); + connect(angles1.Phi, displacement1.Phi) annotation( + Line(points = {{67, 19}, {86, 19}, {86, 16}, {88, 16}}, color = {0, 0, 127})); + connect(angles1.Theta, displacement1.Theta) annotation( + Line(points = {{67, 10}, {79.5, 10}, {79.5, 8}, {88, 8}}, color = {0, 0, 127})); + connect(angles1.Psi, displacement1.Psi) annotation( + Line(points = {{67, 1}, {78, 1}, {78, 0}, {88, 0}}, color = {0, 0, 127})); + connect(aA_control1.Phi, angles1.Phi) annotation( + Line(points = {{-66, 44}, {-74, 44}, {-74, 56}, {74, 56}, {74, 19}, {67, 19}}, color = {0, 0, 127})); + connect(aA_control1.Theta, angles1.Theta) annotation( + Line(points = {{-66, 36}, {-76, 36}, {-76, 60}, {76, 60}, {76, 10}, {67, 10}}, color = {0, 0, 127})); + connect(aA_control1.Psi, angles1.Psi) annotation( + Line(points = {{-66, 26}, {-78, 26}, {-78, 64}, {80, 64}, {80, 1}, {67, 1}}, color = {0, 0, 127})); + connect(aA_control1.Omega, angles1.omega) annotation( + Line(points = {{-66, -26}, {-70, -26}, {-70, -38}, {44, -38}, {44, 1}}, color = {0, 0, 127})); + connect(aA_control1.Zd, Zd.y) annotation( + Line(points = {{-66, -18}, {-78, -18}, {-78, -38}, {-162, -38}, {-162, -4}, {-172, -4}, {-172, -4}}, color = {0, 0, 127})); + connect(position_controller1.Psid, Psid.y) annotation( + Line(points = {{-134, -22}, {-172, -22}, {-172, -32}, {-172, -32}}, color = {0, 0, 127})); + connect(position_controller1.Z, displacement1.Z) annotation( + Line(points = {{-134, -12}, {-148, -12}, {-148, -42}, {120, -42}, {120, 2}, {112, 2}, {112, 2}, {112, 2}}, color = {0, 0, 127})); + connect(displacement1.Y, position_controller1.Y) annotation( + Line(points = {{112, 12}, {116, 12}, {116, 84}, {-148, 84}, {-148, 8}, {-134, 8}, {-134, 8}}, color = {0, 0, 127})); + connect(displacement1.X, position_controller1.X) annotation( + Line(points = {{112, 22}, {114, 22}, {114, 80}, {-144, 80}, {-144, 28}, {-134, 28}, {-134, 30}}, color = {0, 0, 127})); + connect(aA_control1.Z, displacement1.Z) annotation( + Line(points = {{-66, -8}, {-80, -8}, {-80, -44}, {114, -44}, {114, 4}, {112, 4}, {112, 2}, {112, 2}}, color = {0, 0, 127})); + connect(aA_control1.Psid, Psid.y) annotation( + Line(points = {{-66, 0}, {-82, 0}, {-82, -32}, {-174, -32}, {-174, -32}, {-172, -32}}, color = {0, 0, 127})); + connect(position_controller1.Thetad, aA_control1.Thetad) annotation( + Line(points = {{-86, -6}, {-86, -6}, {-86, 10}, {-66, 10}, {-66, 8}}, color = {0, 0, 127})); + connect(position_controller1.Phid, aA_control1.Phid) annotation( + Line(points = {{-86, 24}, {-80, 24}, {-80, 18}, {-66, 18}, {-66, 18}}, color = {0, 0, 127})); + connect(Zd.y, position_controller1.Zd) annotation( + Line(points = {{-172, -4}, {-134, -4}, {-134, -2}, {-134, -2}}, color = {0, 0, 127})); + connect(Yd.y, position_controller1.Yd) annotation( + Line(points = {{-170, 24}, {-160, 24}, {-160, 18}, {-134, 18}, {-134, 20}}, color = {0, 0, 127})); + connect(Xd.y, position_controller1.Xd) annotation( + Line(points = {{-170, 58}, {-154, 58}, {-154, 40}, {-134, 40}, {-134, 40}}, color = {0, 0, 127})); + annotation( + uses(Modelica(version = "3.2.2")), + Diagram(coordinateSystem(extent = {{-200, -100}, {150, 100}})), + Icon(coordinateSystem(extent = {{-200, -100}, {150, 100}})), + version = "", + __OpenModelica_commandLineOptions = ""); +end Drone_Model_FMU; diff --git a/models/CrazyFlie/Position_controller.mo b/models/CrazyFlie/Position_controller.mo new file mode 100644 index 0000000..ca22a1e --- /dev/null +++ b/models/CrazyFlie/Position_controller.mo @@ -0,0 +1,43 @@ +model Position_controller +import Modelica.SIunits; + final constant Real g = Modelica.Constants.g_n; //Acceleration of the gravity + parameter Real Kpx=6, Kdx=4, Kpy=6, Kdy=4, Kpz=6, Kdz=0.4; + Modelica.Blocks.Interfaces.RealInput Xd annotation( + Placement(visible = true, transformation(origin = {-120, 150}, extent = {{-20, -20}, {20, 20}}, rotation = 0), iconTransformation(origin = {-120, 150}, extent = {{-20, -20}, {20, 20}}, rotation = 0))); + Modelica.Blocks.Interfaces.RealInput X annotation( + Placement(visible = true, transformation(origin = {-120, 100}, extent = {{-20, -20}, {20, 20}}, rotation = 0), iconTransformation(origin = {-120, 100}, extent = {{-20, -20}, {20, 20}}, rotation = 0))); + Modelica.Blocks.Interfaces.RealInput Yd annotation( + Placement(visible = true, transformation(origin = {-120, 50}, extent = {{-20, -20}, {20, 20}}, rotation = 0), iconTransformation(origin = {-120, 50}, extent = {{-20, -20}, {20, 20}}, rotation = 0))); + Modelica.Blocks.Interfaces.RealInput Y annotation( + Placement(visible = true, transformation(origin = {-120, 0}, extent = {{-20, -20}, {20, 20}}, rotation = 0), iconTransformation(origin = {-120, 0}, extent = {{-20, -20}, {20, 20}}, rotation = 0))); + Modelica.Blocks.Interfaces.RealInput Zd annotation( + Placement(visible = true, transformation(origin = {-120, -50}, extent = {{-20, -20}, {20, 20}}, rotation = 0), iconTransformation(origin = {-120, -50}, extent = {{-20, -20}, {20, 20}}, rotation = 0))); + Modelica.Blocks.Interfaces.RealInput Z annotation( + Placement(visible = true, transformation(origin = {-120, -100}, extent = {{-20, -20}, {20, 20}}, rotation = 0), iconTransformation(origin = {-120, -100}, extent = {{-20, -20}, {20, 20}}, rotation = 0))); + Modelica.Blocks.Interfaces.RealInput Psid annotation( + Placement(visible = true, transformation(origin = {-120, -150}, extent = {{-20, -20}, {20, 20}}, rotation = 0), iconTransformation(origin = {-120, -150}, extent = {{-20, -20}, {20, 20}}, rotation = 0))); + + Modelica.Blocks.Interfaces.RealOutput Phid annotation( + Placement(visible = true, transformation(origin = {110, 75}, extent = {{-10, -10}, {10, 10}}, rotation = 0), iconTransformation(origin = {110, 75}, extent = {{-10, -10}, {10, 10}}, rotation = 0))); + Modelica.Blocks.Interfaces.RealOutput Thetad annotation( + Placement(visible = true, transformation(origin = {110, -75}, extent = {{-10, -10}, {10, 10}}, rotation = 0), iconTransformation(origin = {110, -75}, extent = {{-10, -10}, {10, 10}}, rotation = 0))); + +Real dx, dy, dz, Xd_dot_dot, Yd_dot_dot, Zd_dot_dot, Xd_dot, Yd_dot, Zd_dot, X_dot, Y_dot, Z_dot; + +equation + Xd_dot=der(Xd);Xd_dot_dot=der(Xd_dot);X_dot=der(X); + Yd_dot=der(Yd);Yd_dot_dot=der(Yd_dot);Y_dot=der(Y); + Zd_dot=der(Zd);Zd_dot_dot=der(Zd_dot);Z_dot=der(Z); + dx=Xd_dot_dot+Kdx*(Xd_dot-X_dot)+Kpx*(Xd-X); + dy=Yd_dot_dot+Kdy*(Yd_dot-Y_dot)+Kpy*(Yd-Y); + dz=Zd_dot_dot+Kdz*(Zd_dot-Z_dot)+Kpz*(Zd-Z); + Thetad=atan(dx*cos(Psid)+dy*sin(Psid))/(dz+g); + Phid=atan(dx*sin(Psid)-dy*cos(Psid))/(sqrt(dx^2+dy^2+(dz+g)^2)); + +annotation( + uses(Modelica(version = "3.2.2")), + Icon(coordinateSystem(extent = {{-100, -180}, {100, 180}}),graphics = {Rectangle(extent = {{-100, 180}, {100, -180}})}), + Diagram(coordinateSystem(extent = {{-100, -180}, {100, 180}})), + version = "", + __OpenModelica_commandLineOptions = ""); +end Position_controller; \ No newline at end of file diff --git a/models/CrazyFlie/Quadrotor.mo b/models/CrazyFlie/Quadrotor.mo new file mode 100644 index 0000000..546109b --- /dev/null +++ b/models/CrazyFlie/Quadrotor.mo @@ -0,0 +1,59 @@ +model Quadrotor + Position_controller Position_controller1 annotation( + Placement(visible = true, transformation(origin = {-40, 32}, extent = {{-10, -12}, {10, 12}}, rotation = 0))); + AA_control aA_control annotation( + Placement(visible = true, transformation(origin = {10, 30}, extent = {{-10, -20}, {10, 20}}, rotation = 0))); + Modelica.Blocks.Sources.Step Xd annotation( + Placement(visible = true, transformation(origin = {-94, 52}, extent = {{-4, -4}, {4, 4}}, rotation = 0))); + Modelica.Blocks.Sources.Step Yd annotation( + Placement(visible = true, transformation(origin = {-94, 38}, extent = {{-4, -4}, {4, 4}}, rotation = 0))); + Modelica.Blocks.Sources.Step Zd annotation( + Placement(visible = true, transformation(origin = {-94, 26}, extent = {{-4, -4}, {4, 4}}, rotation = 0))); + Modelica.Blocks.Sources.Step Psid annotation( + Placement(visible = true, transformation(origin = {-94, 14}, extent = {{-4, -4}, {4, 4}}, rotation = 0))); + System_Model system_Model annotation( + Placement(visible = true, transformation(origin = {54, 32}, extent = {{-10, -12}, {10, 12}}, rotation = 0))); +equation + connect(system_Model.Z, Position_controller1.Z) annotation( + Line(points = {{66, 26}, {80, 26}, {80, 2}, {-60, 2}, {-60, 22}, {-52, 22}, {-52, 22}}, color = {0, 0, 127})); + connect(system_Model.Y, Position_controller1.Y) annotation( + Line(points = {{66, 28}, {78, 28}, {78, 70}, {-64, 70}, {-64, 32}, {-52, 32}, {-52, 32}}, color = {0, 0, 127})); + connect(system_Model.X, Position_controller1.X) annotation( + Line(points = {{66, 32}, {76, 32}, {76, 68}, {-60, 68}, {-60, 42}, {-52, 42}, {-52, 42}}, color = {0, 0, 127})); + connect(system_Model.Psi, aA_control.Psi) annotation( + Line(points = {{66, 36}, {74, 36}, {74, 64}, {-12, 64}, {-12, 42}, {-2, 42}, {-2, 42}}, color = {0, 0, 127})); + connect(system_Model.Theta, aA_control.Theta) annotation( + Line(points = {{66, 38}, {72, 38}, {72, 62}, {-10, 62}, {-10, 48}, {-2, 48}, {-2, 48}}, color = {0, 0, 127})); + connect(system_Model.Phi, aA_control.Phi) annotation( + Line(points = {{66, 42}, {70, 42}, {70, 60}, {-8, 60}, {-8, 54}, {-2, 54}, {-2, 54}}, color = {0, 0, 127})); + connect(aA_control.tau_psi, system_Model.tau_psi) annotation( + Line(points = {{22, 20}, {42, 20}, {42, 22}, {42, 22}}, color = {0, 0, 127})); + connect(aA_control.tau_theta, system_Model.tau_theta) annotation( + Line(points = {{22, 26}, {40, 26}, {40, 28}, {42, 28}}, color = {0, 0, 127})); + connect(aA_control.tau_phi, system_Model.tau_phi) annotation( + Line(points = {{22, 36}, {40, 36}, {40, 36}, {42, 36}}, color = {0, 0, 127})); + connect(aA_control.T, system_Model.T) annotation( + Line(points = {{22, 40}, {40, 40}, {40, 42}, {42, 42}}, color = {0, 0, 127})); + connect(system_Model.Omega, aA_control.Omega) annotation( + Line(points = {{66, 22}, {68, 22}, {68, -2}, {-6, -2}, {-6, 6}, {-2, 6}, {-2, 6}}, color = {0, 0, 127})); + connect(Position_controller1.Z, aA_control.Z) annotation( + Line(points = {{-52, 22}, {-60, 22}, {-60, 2}, {-12, 2}, {-12, 18}, {-2, 18}}, color = {0, 0, 127})); + connect(aA_control.Psid, Psid.y) annotation( + Line(points = {{-2, 24}, {-10, 24}, {-10, 20}, {-28, 20}, {-28, 4}, {-90, 4}, {-90, 14}}, color = {0, 0, 127})); + connect(Psid.y, Position_controller1.Psid) annotation( + Line(points = {{-90, 14}, {-71, 14}, {-71, 16}, {-52, 16}}, color = {0, 0, 127})); + connect(aA_control.Zd, Zd.y) annotation( + Line(points = {{-2, 12}, {-70, 12}, {-70, 26}, {-90, 26}, {-90, 26}}, color = {0, 0, 127})); + connect(Zd.y, Position_controller1.Zd) annotation( + Line(points = {{-90, 26}, {-52, 26}}, color = {0, 0, 127})); + connect(Xd.y, Position_controller1.Xd) annotation( + Line(points = {{-90, 52}, {-70, 52}, {-70, 46}, {-52, 46}, {-52, 48}}, color = {0, 0, 127})); + connect(Yd.y, Position_controller1.Yd) annotation( + Line(points = {{-90, 38}, {-52, 38}}, color = {0, 0, 127})); + connect(Position_controller1.Thetad, aA_control.Thetad) annotation( + Line(points = {{-28, 24}, {-12, 24}, {-12, 30}, {-2, 30}, {-2, 30}}, color = {0, 0, 127})); + connect(Position_controller1.Phid, aA_control.Phid) annotation( + Line(points = {{-28, 40}, {-12, 40}, {-12, 36}, {-2, 36}, {-2, 36}}, color = {0, 0, 127})); + annotation( + uses(Modelica(version = "3.2.2"))); +end Quadrotor; diff --git a/models/CrazyFlie/Quadrotor_FMU.mo b/models/CrazyFlie/Quadrotor_FMU.mo new file mode 100644 index 0000000..9310880 --- /dev/null +++ b/models/CrazyFlie/Quadrotor_FMU.mo @@ -0,0 +1,59 @@ +model Quadrotor_FMU + Position_controller Position_controller1 annotation( + Placement(visible = true, transformation(origin = {-40, 32}, extent = {{-10, -12}, {10, 12}}, rotation = 0))); + AA_control aA_control annotation( + Placement(visible = true, transformation(origin = {12, 30}, extent = {{-10, -20}, {10, 20}}, rotation = 0))); + Modelica.Blocks.Sources.Step Xd annotation( + Placement(visible = true, transformation(origin = {-94, 52}, extent = {{-4, -4}, {4, 4}}, rotation = 0))); + Modelica.Blocks.Sources.Step Yd annotation( + Placement(visible = true, transformation(origin = {-94, 38}, extent = {{-4, -4}, {4, 4}}, rotation = 0))); + Modelica.Blocks.Sources.Step Zd annotation( + Placement(visible = true, transformation(origin = {-94, 26}, extent = {{-4, -4}, {4, 4}}, rotation = 0))); + Modelica.Blocks.Sources.Step Psid annotation( + Placement(visible = true, transformation(origin = {-94, 14}, extent = {{-4, -4}, {4, 4}}, rotation = 0))); + System_Model_me_FMU system_Model_me_FMU2 annotation( + Placement(visible = true, transformation(origin = {66, 8}, extent = {{-10, -10}, {10, 10}}, rotation = 0))); +equation + connect(system_Model_me_FMU2.Omega, aA_control.Phi) annotation( + Line(points = {{78, 16}, {86, 16}, {86, 62}, {-8, 62}, {-8, 54}, {0, 54}, {0, 54}}, color = {0, 0, 127})); + connect(system_Model_me_FMU2.Phi, aA_control.Theta) annotation( + Line(points = {{78, 12}, {82, 12}, {82, 60}, {-14, 60}, {-14, 48}, {0, 48}, {0, 48}}, color = {0, 0, 127})); + connect(system_Model_me_FMU2.Psi, aA_control.Psi) annotation( + Line(points = {{78, 10}, {96, 10}, {96, -18}, {-10, -18}, {-10, 42}, {0, 42}, {0, 42}}, color = {0, 0, 127})); + connect(system_Model_me_FMU2.Theta, Position_controller1.X) annotation( + Line(points = {{78, 8}, {94, 8}, {94, -16}, {-74, -16}, {-74, 42}, {-52, 42}, {-52, 42}}, color = {0, 0, 127})); + connect(system_Model_me_FMU2.X, Position_controller1.Y) annotation( + Line(points = {{78, 4}, {92, 4}, {92, -12}, {-66, -12}, {-66, 32}, {-52, 32}, {-52, 32}}, color = {0, 0, 127})); + connect(system_Model_me_FMU2.Y, Position_controller1.Z) annotation( + Line(points = {{78, 2}, {86, 2}, {86, -4}, {-64, -4}, {-64, 22}, {-52, 22}, {-52, 22}}, color = {0, 0, 127})); + connect(system_Model_me_FMU2.Z, aA_control.Omega) annotation( + Line(points = {{78, 0}, {80, 0}, {80, -4}, {-6, -4}, {-6, 6}, {0, 6}, {0, 6}}, color = {0, 0, 127})); + connect(aA_control.T, system_Model_me_FMU2.T) annotation( + Line(points = {{24, 40}, {46, 40}, {46, 15}, {55, 15}}, color = {0, 0, 127})); + connect(aA_control.tau_phi, system_Model_me_FMU2.tau_phi) annotation( + Line(points = {{24, 36}, {44, 36}, {44, 13}, {55, 13}}, color = {0, 0, 127})); + connect(aA_control.tau_theta, system_Model_me_FMU2.tau_psi) annotation( + Line(points = {{24, 26}, {42, 26}, {42, 10}, {55, 10}}, color = {0, 0, 127})); + connect(aA_control.tau_psi, system_Model_me_FMU2.tau_theta) annotation( + Line(points = {{24, 20}, {40, 20}, {40, 7}, {55, 7}}, color = {0, 0, 127})); + connect(Position_controller1.Phid, aA_control.Phid) annotation( + Line(points = {{-28, 40}, {-12, 40}, {-12, 36}, {0, 36}}, color = {0, 0, 127})); + connect(Position_controller1.Thetad, aA_control.Thetad) annotation( + Line(points = {{-28, 24}, {-12, 24}, {-12, 30}, {0, 30}}, color = {0, 0, 127})); + connect(aA_control.Zd, Zd.y) annotation( + Line(points = {{0, 12}, {-70, 12}, {-70, 26}, {-90, 26}}, color = {0, 0, 127})); + connect(aA_control.Psid, Psid.y) annotation( + Line(points = {{0, 24}, {-10, 24}, {-10, 20}, {-28, 20}, {-28, 4}, {-90, 4}, {-90, 14}}, color = {0, 0, 127})); + connect(Position_controller1.Z, aA_control.Z) annotation( + Line(points = {{-52, 22}, {-60, 22}, {-60, 2}, {-12, 2}, {-12, 18}, {0, 18}}, color = {0, 0, 127})); + connect(Psid.y, Position_controller1.Psid) annotation( + Line(points = {{-90, 14}, {-71, 14}, {-71, 16}, {-52, 16}}, color = {0, 0, 127})); + connect(Zd.y, Position_controller1.Zd) annotation( + Line(points = {{-90, 26}, {-52, 26}}, color = {0, 0, 127})); + connect(Xd.y, Position_controller1.Xd) annotation( + Line(points = {{-90, 52}, {-70, 52}, {-70, 46}, {-52, 46}, {-52, 48}}, color = {0, 0, 127})); + connect(Yd.y, Position_controller1.Yd) annotation( + Line(points = {{-90, 38}, {-52, 38}}, color = {0, 0, 127})); + annotation( + uses(Modelica(version = "3.2.2"))); +end Quadrotor_FMU; diff --git a/models/CrazyFlie/Quadrotor_all.fmu b/models/CrazyFlie/Quadrotor_all.fmu new file mode 100644 index 0000000..53e428b Binary files /dev/null and b/models/CrazyFlie/Quadrotor_all.fmu differ diff --git a/models/CrazyFlie/Quadrotor_all.mo b/models/CrazyFlie/Quadrotor_all.mo new file mode 100644 index 0000000..38cbcfe --- /dev/null +++ b/models/CrazyFlie/Quadrotor_all.mo @@ -0,0 +1,212 @@ +class Quadrotor_all +model Angles + + import Modelica.SIunits; + parameter SIunits.Length l = 0.23; // Length + parameter SIunits.MomentOfInertia Ix = 7.5e-3; //Quadrotor moment of inertia around X axis + parameter SIunits.MomentOfInertia Iy = 7.5e-3; //Quadrotor moment of inertia around Y axis + parameter SIunits.MomentOfInertia Iz = 1.3e-2; //Quadrotor moment of inertia around Z axis + parameter SIunits.MomentOfInertia Jr = 6.5e-5; //Total rotational moment of inertia around the propeller axis + parameter Real b = 3.13e-5; //Thrust factor + parameter Real d = 7.5e-7; // Drag factor + Real Phi_dot, Theta_dot, Psi_dot, Phi_dot_dot, Theta_dot_dot, Psi_dot_dot; + + Modelica.Blocks.Interfaces.RealInput u2 annotation( + Placement(visible = true, transformation(origin = {-120, 78}, extent = {{-20, -20}, {20, 20}}, rotation = 0), iconTransformation(origin = {-120, 90}, extent = {{-20, -20}, {20, 20}}, rotation = 0))); + Modelica.Blocks.Interfaces.RealInput u3 annotation( + Placement(visible = true, transformation(origin = {-120, 26}, extent = {{-20, -20}, {20, 20}}, rotation = 0), iconTransformation(origin = {-120, 30}, extent = {{-20, -20}, {20, 20}}, rotation = 0))); + Modelica.Blocks.Interfaces.RealInput u4 annotation( + Placement(visible = true, transformation(origin = {-120, -32}, extent = {{-20, -20}, {20, 20}}, rotation = 0), iconTransformation(origin = {-120, -30}, extent = {{-20, -20}, {20, 20}}, rotation = 0))); + Modelica.Blocks.Interfaces.RealInput omega annotation( + Placement(visible = true, transformation(origin = {-120, -80}, extent = {{-20, -20}, {20, 20}}, rotation = 0), iconTransformation(origin = {-120, -90}, extent = {{-20, -20}, {20, 20}}, rotation = 0))); + Modelica.Blocks.Interfaces.RealOutput Phi annotation( + Placement(visible = true, transformation(origin = {110, 80}, extent = {{-10, -10}, {10, 10}}, rotation = 0), iconTransformation(origin = {110, 88}, extent = {{-10, -10}, {10, 10}}, rotation = 0))); + Modelica.Blocks.Interfaces.RealOutput Theta annotation( + Placement(visible = true, transformation(origin = {110, 0}, extent = {{-10, -10}, {10, 10}}, rotation = 0), iconTransformation(origin = {110, 0}, extent = {{-10, -10}, {10, 10}}, rotation = 0))); + Modelica.Blocks.Interfaces.RealOutput Psi annotation( + Placement(visible = true, transformation(origin = {110, -80}, extent = {{-10, -10}, {10, 10}}, rotation = 0), iconTransformation(origin = {110, -90}, extent = {{-10, -10}, {10, 10}}, rotation = 0))); +equation + Phi_dot_dot = (Iy-Iz)/Ix*Theta_dot*Psi_dot + Jr/Ix*Theta_dot*omega + l/Ix*u2; + Theta_dot_dot = (Iz-Ix)/Iy*Phi_dot*Psi_dot - Jr/Iy*Phi_dot*omega + l/Iy*u3; + Psi_dot_dot = (Ix-Iy)/Iz*Phi_dot*Theta_dot + 1/Iz*u4; + Phi_dot = der(Phi); + Phi_dot_dot = der(Phi_dot); + Theta_dot = der(Theta); + Theta_dot_dot = der(Theta_dot); + Psi_dot = der(Psi); + Psi_dot_dot = der(Psi_dot); + +annotation( + uses(Modelica(version = "3.2.2")), + Icon(coordinateSystem(extent = {{-100, -120}, {100, 120}}),graphics = {Rectangle(origin = {0, 0}, extent = {{-100, 120}, {100, -120}})}), + Diagram(coordinateSystem(extent = {{-100, -120}, {100, 120}})), + version = "", + __OpenModelica_commandLineOptions = ""); +end Angles; + +model Displacement +import Modelica.SIunits; +final constant Real g = Modelica.Constants.g_n; //Acceleration of the gravity +parameter SIunits.Mass m = 0.65; // Mass +parameter SIunits.MomentOfInertia Ix = 7.5e-3; //Quadrotor moment of inertia around X axis +parameter SIunits.MomentOfInertia Iy = 7.5e-3; //Quadrotor moment of inertia around Y axis +parameter SIunits.MomentOfInertia Iz = 1.3e-3; //Quadrotor moment of inertia around Z axis +parameter SIunits.MomentOfInertia Jr = 6.5e-5; //Total rotational moment of inertia around the propeller axis +parameter Real b = 3.13e-5; //Thrust factor +parameter Real d = 7.5e-7; // Drag factor +Real X_dot, Y_dot, Z_dot, X_dot_dot, Y_dot_dot, Z_dot_dot; + Modelica.Blocks.Interfaces.RealInput u1 annotation( + Placement(visible = true, transformation(origin = {-120, 110}, extent = {{-20, -20}, {20, 20}}, rotation = 0), iconTransformation(origin = {-120, 110}, extent = {{-20, -20}, {20, 20}}, rotation = 0))); + Modelica.Blocks.Interfaces.RealInput Phi annotation( + Placement(visible = true, transformation(origin = {-120, 40}, extent = {{-20, -20}, {20, 20}}, rotation = 0), iconTransformation(origin = {-120, 40}, extent = {{-20, -20}, {20, 20}}, rotation = 0))); + Modelica.Blocks.Interfaces.RealInput Theta annotation( + Placement(visible = true, transformation(origin = {-120, -40}, extent = {{-20, -20}, {20, 20}}, rotation = 0), iconTransformation(origin = {-120, -40}, extent = {{-20, -20}, {20, 20}}, rotation = 0))); + Modelica.Blocks.Interfaces.RealInput Psi annotation( + Placement(visible = true, transformation(origin = {-120, -110}, extent = {{-20, -20}, {20, 20}}, rotation = 0), iconTransformation(origin = {-120, -110}, extent = {{-20, -20}, {20, 20}}, rotation = 0))); + Modelica.Blocks.Interfaces.RealOutput X annotation( + Placement(visible = true, transformation(origin = {110, 90}, extent = {{-10, -10}, {10, 10}}, rotation = 0), iconTransformation(origin = {110, 90}, extent = {{-10, -10}, {10, 10}}, rotation = 0))); + Modelica.Blocks.Interfaces.RealOutput Y annotation( + Placement(visible = true, transformation(origin = {110, 0}, extent = {{-10, -10}, {10, 10}}, rotation = 0), iconTransformation(origin = {110, 0}, extent = {{-10, -10}, {10, 10}}, rotation = 0))); + Modelica.Blocks.Interfaces.RealOutput Z annotation( + Placement(visible = true, transformation(origin = {110, -90}, extent = {{-10, -10}, {10, 10}}, rotation = 0), iconTransformation(origin = {110, -90}, extent = {{-10, -10}, {10, 10}}, rotation = 0))); + +equation +X_dot_dot = (cos(Phi)*sin(Theta)*cos(Psi) + sin(Phi)*sin(Psi))*u1/m - X_dot*d/m; +Y_dot_dot = (cos(Phi)*sin(Theta)*sin(Psi) + sin(Phi)*cos(Psi))*u1/m - Y_dot*d/m; +Z_dot_dot = cos(Phi)*cos(Theta)*u1/m - Z_dot*d/m - g; +X_dot = der(X); +X_dot_dot = der(X_dot); +Y_dot = der(Y); +Y_dot_dot = der(Y_dot); +Z_dot = der(Z); +Z_dot_dot = der(Z_dot); + +annotation( + Diagram(coordinateSystem(grid = {2, 1}, extent = {{-100, -160}, {100, 140}})), + Icon(coordinateSystem(grid = {2, 1}, extent = {{-100, -160}, {100, 140}}),graphics = {Rectangle(origin = {0, 0}, extent = {{-100, 160}, {100, -180}})}), + version = "", + uses, + __OpenModelica_commandLineOptions = ""); +end Displacement; + +model Speed_calculator + import Modelica.SIunits; + parameter SIunits.Length l = 0.23; // Length + parameter Real b = 3.13e-5; //Thrust factor + parameter Real d = 7.5e-7; // Drag factor + Real o1, o2, o3, o4; + Modelica.Blocks.Interfaces.RealInput U1 annotation( + Placement(visible = true, transformation(origin = {-120, 80}, extent = {{-20, -20}, {20, 20}}, rotation = 0), iconTransformation(origin = {-120, 80}, extent = {{-20, -20}, {20, 20}}, rotation = 0))); + Modelica.Blocks.Interfaces.RealInput U2 annotation( + Placement(visible = true, transformation(origin = {-120, 30}, extent = {{-20, -20}, {20, 20}}, rotation = 0), iconTransformation(origin = {-120, 30}, extent = {{-20, -20}, {20, 20}}, rotation = 0))); + Modelica.Blocks.Interfaces.RealInput U3 annotation( + Placement(visible = true, transformation(origin = {-120, -28}, extent = {{-20, -20}, {20, 20}}, rotation = 0), iconTransformation(origin = {-120, -24}, extent = {{-20, -20}, {20, 20}}, rotation = 0))); + Modelica.Blocks.Interfaces.RealInput U4 annotation( + Placement(visible = true, transformation(origin = {-120, -80}, extent = {{-20, -20}, {20, 20}}, rotation = 0), iconTransformation(origin = {-120, -80}, extent = {{-20, -20}, {20, 20}}, rotation = 0))); + Modelica.Blocks.Interfaces.RealOutput u1 annotation( + Placement(visible = true, transformation(origin = {110, 40}, extent = {{-10, -10}, {10, 10}}, rotation = 0), iconTransformation(origin = {110, 40}, extent = {{-10, -10}, {10, 10}}, rotation = 0))); + Modelica.Blocks.Interfaces.RealOutput u2 annotation( + Placement(visible = true, transformation(origin = {110, 0}, extent = {{-10, -10}, {10, 10}}, rotation = 0), iconTransformation(origin = {110, 0}, extent = {{-10, -10}, {10, 10}}, rotation = 0))); + Modelica.Blocks.Interfaces.RealOutput u3 annotation( + Placement(visible = true, transformation(origin = {110, -40}, extent = {{-10, -10}, {10, 10}}, rotation = 0), iconTransformation(origin = {110, -40}, extent = {{-10, -10}, {10, 10}}, rotation = 0))); + Modelica.Blocks.Interfaces.RealOutput u4 annotation( + Placement(visible = true, transformation(origin = {110, -80}, extent = {{-10, -10}, {10, 10}}, rotation = 0), iconTransformation(origin = {110, -80}, extent = {{-10, -10}, {10, 10}}, rotation = 0))); + Modelica.Blocks.Interfaces.RealOutput omega annotation( + Placement(visible = true, transformation(origin = {110, 80}, extent = {{-10, -10}, {10, 10}}, rotation = 0), iconTransformation(origin = {110, 80}, +extent = {{-10, -10}, {10, 10}}, rotation = 0))); + +equation + o1 = 1/(4*b)*U1 + 1/(2*b*l)*U3 - 1/(4*d)*U4; + o2 = 1/(4*b)*U1 - 1/(2*b*l)*U2 + 1/(4*d)*U4; + o3 = 1/(4*b)*U1 - 1/(2*b*l)*U3 - 1/(4*d)*U4; + o4 = 1/(4*b)*U1 + 1/(2*b*l)*U2 + 1/(4*d)*U4; + omega = (-sqrt(abs(o1)) + sqrt(abs(o2)) - sqrt(abs(o3)) + sqrt(abs(o4)))*d; + u1 = U1; + u2 = U2/l; + u3 = U3/l; + u4 = U4; + +annotation( + uses(Modelica(version = "3.2.2")), + Icon(graphics = {Rectangle(origin = {0, 1}, extent = {{-100, 99}, {100, -101}})}), + Diagram(coordinateSystem(extent = {{-100, -120}, {100, 120}})), + version = "", + __OpenModelica_commandLineOptions = ""); +end Speed_calculator; + + Displacement displacement1 annotation( + Placement(visible = true, transformation(origin = {76, 22}, extent = {{-10, -16}, {10, 14}}, rotation = 0))); + Angles angles1 annotation( + Placement(visible = true, transformation(origin = {-4, 24}, extent = {{-10, -12}, {10, 12}}, rotation = 0))); + Speed_calculator speed_calculator1 annotation( + Placement(visible = true, transformation(origin = {-48, 24}, extent = {{-10, -10}, {10, 10}}, rotation = 0))); + Modelica.Blocks.Interfaces.RealInput T annotation( + Placement(visible = true, transformation(origin = {-120, 90}, extent = {{-20, -20}, {20, 20}}, rotation = 0), iconTransformation(origin = {-120, 90}, extent = {{-20, -20}, {20, 20}}, rotation = 0))); + Modelica.Blocks.Interfaces.RealInput tau_phi annotation( + Placement(visible = true, transformation(origin = {-120, 30}, extent = {{-20, -20}, {20, 20}}, rotation = 0), iconTransformation(origin = {-120, 30}, extent = {{-20, -20}, {20, 20}}, rotation = 0))); + Modelica.Blocks.Interfaces.RealInput tau_theta annotation( + Placement(visible = true, transformation(origin = {-120, -30}, extent = {{-20, -20}, {20, 20}}, rotation = 0), iconTransformation(origin = {-120, -30}, extent = {{-20, -20}, {20, 20}}, rotation = 0))); + Modelica.Blocks.Interfaces.RealInput tau_psi annotation( + Placement(visible = true, transformation(origin = {-120, -90}, extent = {{-20, -20}, {20, 20}}, rotation = 0), iconTransformation(origin = {-120, -90}, extent = {{-20, -20}, {20, 20}}, rotation = 0))); + Modelica.Blocks.Interfaces.RealOutput Phi annotation( + Placement(visible = true, transformation(origin = {110, 90}, extent = {{-10, -10}, {10, 10}}, rotation = 0), iconTransformation(origin = {110, 90}, extent = {{-10, -10}, {10, 10}}, rotation = 0))); + Modelica.Blocks.Interfaces.RealOutput Theta annotation( + Placement(visible = true, transformation(origin = {110, 60}, extent = {{-10, -10}, {10, 10}}, rotation = 0), iconTransformation(origin = {110, 60}, extent = {{-10, -10}, {10, 10}}, rotation = 0))); + Modelica.Blocks.Interfaces.RealOutput Psi annotation( + Placement(visible = true, transformation(origin = {110, 30}, extent = {{-10, -10}, {10, 10}}, rotation = 0), iconTransformation(origin = {110, 30}, extent = {{-10, -10}, {10, 10}}, rotation = 0))); + Modelica.Blocks.Interfaces.RealOutput X annotation( + Placement(visible = true, transformation(origin = {110, 0}, extent = {{-10, -10}, {10, 10}}, rotation = 0), iconTransformation(origin = {110, 0}, extent = {{-10, -10}, {10, 10}}, rotation = 0))); + Modelica.Blocks.Interfaces.RealOutput Y annotation( + Placement(visible = true, transformation(origin = {110, -30}, extent = {{-10, -10}, {10, 10}}, rotation = 0), iconTransformation(origin = {110, -30}, extent = {{-10, -10}, {10, 10}}, rotation = 0))); + Modelica.Blocks.Interfaces.RealOutput Z annotation( + Placement(visible = true, transformation(origin = {110, -60}, extent = {{-10, -10}, {10, 10}}, rotation = 0), iconTransformation(origin = {110, -60}, extent = {{-10, -10}, {10, 10}}, rotation = 0))); + Modelica.Blocks.Interfaces.RealOutput Omega annotation( + Placement(visible = true, transformation(origin = {110, -92}, extent = {{-10, -10}, {10, 10}}, rotation = 0), iconTransformation(origin = {110, -90}, extent = {{-10, -10}, {10, 10}}, rotation = 0))); +equation + connect(Omega, angles1.omega) annotation( + Line(points = {{110, -92}, {-30, -92}, {-30, 16}, {-16, 16}}, color = {0, 0, 127})); + connect(displacement1.Z, Z) annotation( + Line(points = {{88, 14}, {90, 14}, {90, -60}, {110, -60}}, color = {0, 0, 127})); + connect(displacement1.Y, Y) annotation( + Line(points = {{88, 22}, {92, 22}, {92, -30}, {110, -30}}, color = {0, 0, 127})); + connect(Phi, angles1.Phi) annotation( + Line(points = {{110, 90}, {24, 90}, {24, 32}, {8, 32}}, color = {0, 0, 127})); + connect(Theta, angles1.Theta) annotation( + Line(points = {{110, 60}, {30, 60}, {30, 24}, {8, 24}}, color = {0, 0, 127})); + connect(Psi, angles1.Psi) annotation( + Line(points = {{110, 30}, {96, 30}, {96, 52}, {16, 52}, {16, 14}, {8, 14}, {8, 16}}, color = {0, 0, 127})); + connect(X, displacement1.X) annotation( + Line(points = {{110, 0}, {94, 0}, {94, 32}, {88, 32}}, color = {0, 0, 127})); + connect(tau_psi, speed_calculator1.U4) annotation( + Line(points = {{-120, -90}, {-80, -90}, {-80, 16}, {-60, 16}, {-60, 16}}, color = {0, 0, 127})); + connect(tau_theta, speed_calculator1.U3) annotation( + Line(points = {{-120, -30}, {-90, -30}, {-90, 22}, {-60, 22}, {-60, 22}}, color = {0, 0, 127})); + connect(tau_phi, speed_calculator1.U2) annotation( + Line(points = {{-120, 30}, {-82, 30}, {-82, 26}, {-60, 26}, {-60, 28}}, color = {0, 0, 127})); + connect(T, speed_calculator1.U1) annotation( + Line(points = {{-120, 90}, {-76, 90}, {-76, 32}, {-60, 32}, {-60, 32}}, color = {0, 0, 127})); + connect(speed_calculator1.u1, displacement1.u1) annotation( + Line(points = {{-36, 28}, {-32, 28}, {-32, 48}, {52, 48}, {52, 36}, {64, 36}, {64, 33}}, color = {0, 0, 127})); + connect(angles1.Psi, displacement1.Psi) annotation( + Line(points = {{8, 14}, {48, 14}, {48, 11}, {64, 11}}, color = {0, 0, 127})); + connect(angles1.Theta, displacement1.Theta) annotation( + Line(points = {{8, 24}, {50, 24}, {50, 18}, {64, 18}}, color = {0, 0, 127})); + connect(angles1.Phi, displacement1.Phi) annotation( + Line(points = {{8, 32}, {52, 32}, {52, 26}, {64, 26}}, color = {0, 0, 127})); + connect(speed_calculator1.u4, angles1.u4) annotation( + Line(points = {{-36, 16}, {-22, 16}, {-22, 20}, {-16, 20}, {-16, 20}}, color = {0, 0, 127})); + connect(speed_calculator1.u3, angles1.u3) annotation( + Line(points = {{-36, 20}, {-24, 20}, {-24, 26}, {-16, 26}, {-16, 28}}, color = {0, 0, 127})); + connect(speed_calculator1.u2, angles1.u2) annotation( + Line(points = {{-36, 24}, {-26, 24}, {-26, 34}, {-16, 34}, {-16, 34}}, color = {0, 0, 127})); + connect(speed_calculator1.omega, angles1.omega) annotation( + Line(points = {{-36, 32}, {-30, 32}, {-30, 14}, {-16, 14}, {-16, 14}}, color = {0, 0, 127})); + annotation( + uses(Modelica(version = "3.2.2")), + Diagram(coordinateSystem(extent = {{-100, -140}, {100, 140}})), + Icon(coordinateSystem(extent = {{-100, -140}, {100, 140}}),graphics = {Rectangle(extent = {{-100, 140}, {100, -140}})}), + version = "", + __OpenModelica_commandLineOptions = ""); + + +end Quadrotor_all; \ No newline at end of file diff --git a/models/CrazyFlie/Simulink/FMUs/Quadrotor/Angles.fmu b/models/CrazyFlie/Simulink/FMUs/Quadrotor/Angles.fmu new file mode 100644 index 0000000..863c8c0 Binary files /dev/null and b/models/CrazyFlie/Simulink/FMUs/Quadrotor/Angles.fmu differ diff --git a/models/CrazyFlie/Simulink/FMUs/Quadrotor/Displacement.fmu b/models/CrazyFlie/Simulink/FMUs/Quadrotor/Displacement.fmu new file mode 100644 index 0000000..30a5212 Binary files /dev/null and b/models/CrazyFlie/Simulink/FMUs/Quadrotor/Displacement.fmu differ diff --git a/models/CrazyFlie/Simulink/FMUs/Quadrotor/Step1.fmu b/models/CrazyFlie/Simulink/FMUs/Quadrotor/Step1.fmu new file mode 100644 index 0000000..426b9ca Binary files /dev/null and b/models/CrazyFlie/Simulink/FMUs/Quadrotor/Step1.fmu differ diff --git a/models/CrazyFlie/Simulink/FMUs/Quadrotor/omegasqr.fmu b/models/CrazyFlie/Simulink/FMUs/Quadrotor/omegasqr.fmu new file mode 100644 index 0000000..885da1c Binary files /dev/null and b/models/CrazyFlie/Simulink/FMUs/Quadrotor/omegasqr.fmu differ diff --git a/models/CrazyFlie/Simulink/FMUs/Quadrotor_and_Controller/PD.fmu b/models/CrazyFlie/Simulink/FMUs/Quadrotor_and_Controller/PD.fmu new file mode 100644 index 0000000..9c7f965 Binary files /dev/null and b/models/CrazyFlie/Simulink/FMUs/Quadrotor_and_Controller/PD.fmu differ diff --git a/models/CrazyFlie/Simulink/FMUs/Quadrotor_and_Controller/Quadrotor.fmu b/models/CrazyFlie/Simulink/FMUs/Quadrotor_and_Controller/Quadrotor.fmu new file mode 100644 index 0000000..4d45b23 Binary files /dev/null and b/models/CrazyFlie/Simulink/FMUs/Quadrotor_and_Controller/Quadrotor.fmu differ diff --git a/models/CrazyFlie/Simulink/FMUs/Quadrotor_and_Controller/Step0.fmu b/models/CrazyFlie/Simulink/FMUs/Quadrotor_and_Controller/Step0.fmu new file mode 100644 index 0000000..a11e64f Binary files /dev/null and b/models/CrazyFlie/Simulink/FMUs/Quadrotor_and_Controller/Step0.fmu differ diff --git a/models/CrazyFlie/Simulink/FMUs/Quadrotor_and_Controller/Subsystem.fmu b/models/CrazyFlie/Simulink/FMUs/Quadrotor_and_Controller/Subsystem.fmu new file mode 100644 index 0000000..3a17569 Binary files /dev/null and b/models/CrazyFlie/Simulink/FMUs/Quadrotor_and_Controller/Subsystem.fmu differ diff --git a/models/CrazyFlie/Simulink/PDQuadrotor2015a.slx b/models/CrazyFlie/Simulink/PDQuadrotor2015a.slx new file mode 100644 index 0000000..3a17a7f Binary files /dev/null and b/models/CrazyFlie/Simulink/PDQuadrotor2015a.slx differ diff --git a/models/CrazyFlie/Simulink/Quadparameters.m b/models/CrazyFlie/Simulink/Quadparameters.m new file mode 100644 index 0000000..08d1202 --- /dev/null +++ b/models/CrazyFlie/Simulink/Quadparameters.m @@ -0,0 +1,42 @@ +clc +clear all +close all + +global Jr Ix Iy Iz b d l m g +% of the PD controller + +kpp = 50; +kdp = 8; + +kpt = 50; +kdt = 8.09; + +kpps = 50; +kdps = 8.3996; + +kpz = 90; +kdz = 13.795; + +kpx = 6; +kdx = 4; + +kpy = 6; +kdy = 4; + +kpz2 = 6; +kdz2 = 0.4; + +Gains = [kpp kdp kpt kdt kpps kdps kpz kdz ... + kpx kdx kpy kdy kpz2 kdz2]; +disp(Gains); +% Quadrotor constants +Ix = 7.5*10^(-3); % Quadrotor moment of inertia around X axis +Iy = 7.5*10^(-3); % Quadrotor moment of inertia around Y axis +Iz = 1.3*10^(-2); % Quadrotor moment of inertia around Z axis +Jr = 6.5*10^(-5); % Total rotational moment of inertia around the propeller axis +b = 3.13*10^(-5); % Thrust factor +d = 7.5*10^(-7); % Drag factor +l = 0.23; % Distance to the center of the Quadrotor +m = 0.65; % Mass of the Quadrotor in Kg +g = 9.807; % Gravitational acceleration + diff --git a/models/CrazyFlie/Simulink/Quadparameters2.m b/models/CrazyFlie/Simulink/Quadparameters2.m new file mode 100644 index 0000000..2d9b0e7 --- /dev/null +++ b/models/CrazyFlie/Simulink/Quadparameters2.m @@ -0,0 +1,48 @@ +% Abdel-Razzak Merheb +% SIMULINK Quadrotor simulation using Bouabdallah's system +% The controller used here is a PD controller +% Values of gains resembles those used by Mr. Bouabdallah +% 29/11/2012 +% % % % % % % % % % % % % % % +clc +clear all +close all + +global Jr Ix Iy Iz b d l m g Kpz Kdz Kpp Kdp Kpt Kdt Kpps Kdps ZdF PhidF ThetadF PsidF ztime phitime thetatime psitime Zinit Phiinit Thetainit Psiinit Uone Utwo Uthree Ufour Ez Ep Et Eps + +% of the PD controller + +kpp = 50; +kdp = 8; + +kpt = 50; +kdt = 8.09; + +kpps = 50; +kdps = 8.3996; + +kpz = 90; +kdz = 13.795; + +kpx = 6; +kdx = 4; + +kpy = 6; +kdy = 4; + +kpz2 = 6; +kdz2 = 0.4; + +Gains = [kpp kdp kpt kdt kpps kdps kpz kdz ... + kpx kdx kpy kdy kpz2 kdz2]; +disp(Gains); +% Quadrotor constants +Ix = 7.5*10^(-3); % Quadrotor moment of inertia around X axis +Iy = 7.5*10^(-3); % Quadrotor moment of inertia around Y axis +Iz = 1.3*10^(-2); % Quadrotor moment of inertia around Z axis +Jr = 6.5*10^(-5); % Total rotational moment of inertia around the propeller axis +b = 3.13*10^(-5); % Thrust factor +d = 7.5*10^(-7); % Drag factor +l = 0.23; % Distance to the center of the Quadrotor +m = 0.65; % Mass of the Quadrotor in Kg +g = 9.81; % Gravitational acceleration \ No newline at end of file diff --git a/models/CrazyFlie/Simulink/quadrotor_system.slx b/models/CrazyFlie/Simulink/quadrotor_system.slx new file mode 100644 index 0000000..9feab3a Binary files /dev/null and b/models/CrazyFlie/Simulink/quadrotor_system.slx differ diff --git a/models/CrazyFlie/Speed_Calculator.mo b/models/CrazyFlie/Speed_Calculator.mo new file mode 100644 index 0000000..87e76f4 --- /dev/null +++ b/models/CrazyFlie/Speed_Calculator.mo @@ -0,0 +1,44 @@ +model Speed_calculator + parameter Real l = 0.23; // Length + parameter Real b = 3.13e-5; //Thrust factor + parameter Real d = 7.5e-7; // Drag factor + Real omegasqr1, omegasqr2, omegasqr3, omegasqr4; + Modelica.Blocks.Interfaces.RealInput U1 annotation( + Placement(visible = true, transformation(origin = {-120, 80}, extent = {{-20, -20}, {20, 20}}, rotation = 0), iconTransformation(origin = {-120, 80}, extent = {{-20, -20}, {20, 20}}, rotation = 0))); + Modelica.Blocks.Interfaces.RealInput U2 annotation( + Placement(visible = true, transformation(origin = {-120, 30}, extent = {{-20, -20}, {20, 20}}, rotation = 0), iconTransformation(origin = {-120, 30}, extent = {{-20, -20}, {20, 20}}, rotation = 0))); + Modelica.Blocks.Interfaces.RealInput U3 annotation( + Placement(visible = true, transformation(origin = {-120, -28}, extent = {{-20, -20}, {20, 20}}, rotation = 0), iconTransformation(origin = {-120, -24}, extent = {{-20, -20}, {20, 20}}, rotation = 0))); + Modelica.Blocks.Interfaces.RealInput U4 annotation( + Placement(visible = true, transformation(origin = {-120, -80}, extent = {{-20, -20}, {20, 20}}, rotation = 0), iconTransformation(origin = {-120, -80}, extent = {{-20, -20}, {20, 20}}, rotation = 0))); + Modelica.Blocks.Interfaces.RealOutput u1 annotation( + Placement(visible = true, transformation(origin = {110, 40}, extent = {{-10, -10}, {10, 10}}, rotation = 0), iconTransformation(origin = {110, 40}, extent = {{-10, -10}, {10, 10}}, rotation = 0))); + Modelica.Blocks.Interfaces.RealOutput u2 annotation( + Placement(visible = true, transformation(origin = {110, 0}, extent = {{-10, -10}, {10, 10}}, rotation = 0), iconTransformation(origin = {110, 0}, extent = {{-10, -10}, {10, 10}}, rotation = 0))); + Modelica.Blocks.Interfaces.RealOutput u3 annotation( + Placement(visible = true, transformation(origin = {110, -40}, extent = {{-10, -10}, {10, 10}}, rotation = 0), iconTransformation(origin = {110, -40}, extent = {{-10, -10}, {10, 10}}, rotation = 0))); + Modelica.Blocks.Interfaces.RealOutput u4 annotation( + Placement(visible = true, transformation(origin = {110, -80}, extent = {{-10, -10}, {10, 10}}, rotation = 0), iconTransformation(origin = {110, -80}, extent = {{-10, -10}, {10, 10}}, rotation = 0))); + Modelica.Blocks.Interfaces.RealOutput omega annotation( + Placement(visible = true, transformation(origin = {110, 80}, extent = {{-10, -10}, {10, 10}}, rotation = 0), iconTransformation(origin = {110, 80}, +extent = {{-10, -10}, {10, 10}}, rotation = 0))); + +equation + omegasqr1 = U1*(1/(4*b)) + U3*(1/(2*b*l)) - U4*(1/(4*d)); + omegasqr2 = U1*(1/(4*b)) - U2*(1/(2*b*l)) + U4*(1/(4*d)); + omegasqr3 = U1*(1/(4*b)) - U3*(1/(2*b*l)) - U4*(1/(4*d)); + omegasqr4 = 1/(4*b)*U1 + U2*(1/(2*b*l)) + U4*(1/(4*d)); + + u1 = b*(omegasqr1 + omegasqr2 + omegasqr3 + omegasqr4); + u2 = b*(omegasqr4 - omegasqr2); + u3 = b*(omegasqr1 - omegasqr3); + u4 = d*(- omegasqr1 + omegasqr2 - omegasqr3 + omegasqr4); + omega = (-sqrt(abs(omegasqr1)) + sqrt(abs(omegasqr2)) - sqrt(abs(omegasqr3)) + sqrt(abs(omegasqr4)))*d; + +annotation( + uses(Modelica(version = "3.2.2")), + Icon(graphics = {Rectangle(origin = {0, 1}, extent = {{-100, 99}, {100, -101}})}), + Diagram(coordinateSystem(extent = {{-100, -120}, {100, 120}})), + version = "", + __OpenModelica_commandLineOptions = ""); + end Speed_calculator; diff --git a/models/CrazyFlie/System_Model.mo b/models/CrazyFlie/System_Model.mo new file mode 100644 index 0000000..2585228 --- /dev/null +++ b/models/CrazyFlie/System_Model.mo @@ -0,0 +1,76 @@ +model System_Model + Displacement displacement1 annotation( + Placement(visible = true, transformation(origin = {76, 22}, extent = {{-10, -16}, {10, 14}}, rotation = 0))); + Angles angles1 annotation( + Placement(visible = true, transformation(origin = {-4, 24}, extent = {{-10, -12}, {10, 12}}, rotation = 0))); + Speed_calculator speed_calculator1 annotation( + Placement(visible = true, transformation(origin = {-48, 24}, extent = {{-10, -10}, {10, 10}}, rotation = 0))); + Modelica.Blocks.Interfaces.RealInput T annotation( + Placement(visible = true, transformation(origin = {-120, 90}, extent = {{-20, -20}, {20, 20}}, rotation = 0), iconTransformation(origin = {-120, 90}, extent = {{-20, -20}, {20, 20}}, rotation = 0))); + Modelica.Blocks.Interfaces.RealInput tau_phi annotation( + Placement(visible = true, transformation(origin = {-120, 30}, extent = {{-20, -20}, {20, 20}}, rotation = 0), iconTransformation(origin = {-120, 30}, extent = {{-20, -20}, {20, 20}}, rotation = 0))); + Modelica.Blocks.Interfaces.RealInput tau_theta annotation( + Placement(visible = true, transformation(origin = {-120, -30}, extent = {{-20, -20}, {20, 20}}, rotation = 0), iconTransformation(origin = {-120, -30}, extent = {{-20, -20}, {20, 20}}, rotation = 0))); + Modelica.Blocks.Interfaces.RealInput tau_psi annotation( + Placement(visible = true, transformation(origin = {-120, -90}, extent = {{-20, -20}, {20, 20}}, rotation = 0), iconTransformation(origin = {-120, -90}, extent = {{-20, -20}, {20, 20}}, rotation = 0))); + Modelica.Blocks.Interfaces.RealOutput Phi annotation( + Placement(visible = true, transformation(origin = {110, 90}, extent = {{-10, -10}, {10, 10}}, rotation = 0), iconTransformation(origin = {110, 90}, extent = {{-10, -10}, {10, 10}}, rotation = 0))); + Modelica.Blocks.Interfaces.RealOutput Theta annotation( + Placement(visible = true, transformation(origin = {110, 60}, extent = {{-10, -10}, {10, 10}}, rotation = 0), iconTransformation(origin = {110, 60}, extent = {{-10, -10}, {10, 10}}, rotation = 0))); + Modelica.Blocks.Interfaces.RealOutput Psi annotation( + Placement(visible = true, transformation(origin = {110, 30}, extent = {{-10, -10}, {10, 10}}, rotation = 0), iconTransformation(origin = {110, 30}, extent = {{-10, -10}, {10, 10}}, rotation = 0))); + Modelica.Blocks.Interfaces.RealOutput X annotation( + Placement(visible = true, transformation(origin = {110, 0}, extent = {{-10, -10}, {10, 10}}, rotation = 0), iconTransformation(origin = {110, 0}, extent = {{-10, -10}, {10, 10}}, rotation = 0))); + Modelica.Blocks.Interfaces.RealOutput Y annotation( + Placement(visible = true, transformation(origin = {110, -30}, extent = {{-10, -10}, {10, 10}}, rotation = 0), iconTransformation(origin = {110, -30}, extent = {{-10, -10}, {10, 10}}, rotation = 0))); + Modelica.Blocks.Interfaces.RealOutput Z annotation( + Placement(visible = true, transformation(origin = {110, -60}, extent = {{-10, -10}, {10, 10}}, rotation = 0), iconTransformation(origin = {110, -60}, extent = {{-10, -10}, {10, 10}}, rotation = 0))); + Modelica.Blocks.Interfaces.RealOutput Omega annotation( + Placement(visible = true, transformation(origin = {110, -92}, extent = {{-10, -10}, {10, 10}}, rotation = 0), iconTransformation(origin = {110, -90}, extent = {{-10, -10}, {10, 10}}, rotation = 0))); +equation + connect(Omega, angles1.omega) annotation( + Line(points = {{110, -92}, {-30, -92}, {-30, 16}, {-16, 16}}, color = {0, 0, 127})); + connect(displacement1.Z, Z) annotation( + Line(points = {{88, 14}, {90, 14}, {90, -60}, {110, -60}}, color = {0, 0, 127})); + connect(displacement1.Y, Y) annotation( + Line(points = {{88, 22}, {92, 22}, {92, -30}, {110, -30}}, color = {0, 0, 127})); + connect(Phi, angles1.Phi) annotation( + Line(points = {{110, 90}, {24, 90}, {24, 32}, {8, 32}}, color = {0, 0, 127})); + connect(Theta, angles1.Theta) annotation( + Line(points = {{110, 60}, {30, 60}, {30, 24}, {8, 24}}, color = {0, 0, 127})); + connect(Psi, angles1.Psi) annotation( + Line(points = {{110, 30}, {96, 30}, {96, 52}, {16, 52}, {16, 14}, {8, 14}, {8, 16}}, color = {0, 0, 127})); + connect(X, displacement1.X) annotation( + Line(points = {{110, 0}, {94, 0}, {94, 32}, {88, 32}}, color = {0, 0, 127})); + connect(tau_psi, speed_calculator1.U4) annotation( + Line(points = {{-120, -90}, {-80, -90}, {-80, 16}, {-60, 16}, {-60, 16}}, color = {0, 0, 127})); + connect(tau_theta, speed_calculator1.U3) annotation( + Line(points = {{-120, -30}, {-90, -30}, {-90, 22}, {-60, 22}, {-60, 22}}, color = {0, 0, 127})); + connect(tau_phi, speed_calculator1.U2) annotation( + Line(points = {{-120, 30}, {-82, 30}, {-82, 26}, {-60, 26}, {-60, 28}}, color = {0, 0, 127})); + connect(T, speed_calculator1.U1) annotation( + Line(points = {{-120, 90}, {-76, 90}, {-76, 32}, {-60, 32}, {-60, 32}}, color = {0, 0, 127})); + connect(speed_calculator1.u1, displacement1.u1) annotation( + Line(points = {{-36, 28}, {-32, 28}, {-32, 48}, {52, 48}, {52, 36}, {64, 36}, {64, 33}}, color = {0, 0, 127})); + connect(angles1.Psi, displacement1.Psi) annotation( + Line(points = {{8, 14}, {48, 14}, {48, 11}, {64, 11}}, color = {0, 0, 127})); + connect(angles1.Theta, displacement1.Theta) annotation( + Line(points = {{8, 24}, {50, 24}, {50, 18}, {64, 18}}, color = {0, 0, 127})); + connect(angles1.Phi, displacement1.Phi) annotation( + Line(points = {{8, 32}, {52, 32}, {52, 26}, {64, 26}}, color = {0, 0, 127})); + connect(speed_calculator1.u4, angles1.u4) annotation( + Line(points = {{-36, 16}, {-22, 16}, {-22, 20}, {-16, 20}, {-16, 20}}, color = {0, 0, 127})); + connect(speed_calculator1.u3, angles1.u3) annotation( + Line(points = {{-36, 20}, {-24, 20}, {-24, 26}, {-16, 26}, {-16, 28}}, color = {0, 0, 127})); + connect(speed_calculator1.u2, angles1.u2) annotation( + Line(points = {{-36, 24}, {-26, 24}, {-26, 34}, {-16, 34}, {-16, 34}}, color = {0, 0, 127})); + connect(speed_calculator1.omega, angles1.omega) annotation( + Line(points = {{-36, 32}, {-30, 32}, {-30, 14}, {-16, 14}, {-16, 14}}, color = {0, 0, 127})); + annotation( + uses(Modelica(version = "3.2.2")), + Diagram(coordinateSystem(extent = {{-100, -140}, {100, 140}})), + Icon(coordinateSystem(extent = {{-100, -140}, {100, 140}}),graphics = {Rectangle(extent = {{-100, 140}, {100, -140}})}), + version = "", + __OpenModelica_commandLineOptions = ""); + +end System_Model; \ No newline at end of file diff --git a/models/MoonLanding/MoonLanding_aadl/.aadlbin-gen/.MoonLanding.aadlbin b/models/MoonLanding/MoonLanding_aadl/.aadlbin-gen/.MoonLanding.aadlbin new file mode 100644 index 0000000..b532364 Binary files /dev/null and b/models/MoonLanding/MoonLanding_aadl/.aadlbin-gen/.MoonLanding.aadlbin differ diff --git a/models/MoonLanding/MoonLanding_aadl/.aadlsettings b/models/MoonLanding/MoonLanding_aadl/.aadlsettings new file mode 100644 index 0000000..f1f7853 --- /dev/null +++ b/models/MoonLanding/MoonLanding_aadl/.aadlsettings @@ -0,0 +1,3 @@ +#Fri Jan 20 15:23:00 CET 2017 +source.directory=/aadl +model.directory=/aaxl diff --git a/models/MoonLanding/MoonLanding_aadl/.project b/models/MoonLanding/MoonLanding_aadl/.project new file mode 100644 index 0000000..9ed615b --- /dev/null +++ b/models/MoonLanding/MoonLanding_aadl/.project @@ -0,0 +1,19 @@ + + + MoonLanding_aadl + + + Plugin_Resources + + + + org.eclipse.xtext.ui.shared.xtextBuilder + + + + + + org.eclipse.xtext.ui.shared.xtextNature + org.osate.core.aadlnature + + diff --git a/models/MoonLanding/MoonLanding_aadl/MoonLanding.aadl b/models/MoonLanding/MoonLanding_aadl/MoonLanding.aadl new file mode 100644 index 0000000..bddfcb7 --- /dev/null +++ b/models/MoonLanding/MoonLanding_aadl/MoonLanding.aadl @@ -0,0 +1,38 @@ +package MoonLanding +public + +system MoonLander_with_Controller +end MoonLander_with_Controller; + +system implementation MoonLander_with_Controller.with_devices + subcomponents + this_controller: process Controller.with_threads; + this_moon_lander_FMU: device MoonLander_FMU.impl; + connections + altitude_conn: port this_moon_lander_FMU.altitude_out -> this_controller.altitude_in; + velocity_conn: port this_moon_lander_FMU.velocity_out -> this_controller.velocity_in; + thrust_conn: port this_controller.needed_thrust_out -> this_moon_lander_FMU.thrust_in; +end MoonLander_with_Controller.with_devices; + + +process Controller + features + altitude_in: in data port; + velocity_in: in data port; + needed_thrust_out: out data port; +end Controller; + +process implementation Controller.with_threads +end Controller.with_threads; + +device MoonLander_FMU + features + thrust_in: in data port; + altitude_out: out data port; + velocity_out: out data port; +end MoonLander_FMU; + +device implementation MoonLander_FMU.impl +end MoonLander_FMU.impl; + +end MoonLanding; \ No newline at end of file diff --git a/models/MoonLanding/MoonLanding_aadl/diagrams/69f60ae2-6a3d-4cfd-aa7d-6cb93478a706.aadl_diagram b/models/MoonLanding/MoonLanding_aadl/diagrams/69f60ae2-6a3d-4cfd-aa7d-6cb93478a706.aadl_diagram new file mode 100644 index 0000000..ba28ab9 --- /dev/null +++ b/models/MoonLanding/MoonLanding_aadl/diagrams/69f60ae2-6a3d-4cfd-aa7d-6cb93478a706.aadl_diagram @@ -0,0 +1,536 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/models/MoonLanding/MoonLanding_aadl/diagrams/8f2e7bf1-c273-484f-ac10-ae88e4ac0322.aadl_diagram b/models/MoonLanding/MoonLanding_aadl/diagrams/8f2e7bf1-c273-484f-ac10-ae88e4ac0322.aadl_diagram new file mode 100644 index 0000000..9ac3f20 --- /dev/null +++ b/models/MoonLanding/MoonLanding_aadl/diagrams/8f2e7bf1-c273-484f-ac10-ae88e4ac0322.aadl_diagram @@ -0,0 +1,207 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/models/MoonLanding/MoonLanding_aadl/diagrams/ba349be1-a86c-40bd-ab7a-bfb15ab963a5.aadl_diagram b/models/MoonLanding/MoonLanding_aadl/diagrams/ba349be1-a86c-40bd-ab7a-bfb15ab963a5.aadl_diagram new file mode 100644 index 0000000..aa5e5e6 --- /dev/null +++ b/models/MoonLanding/MoonLanding_aadl/diagrams/ba349be1-a86c-40bd-ab7a-bfb15ab963a5.aadl_diagram @@ -0,0 +1,206 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/models/Rosace/Rosace_aadl/.aadlbin-gen/.Rosace.aadlbin b/models/Rosace/Rosace_aadl/.aadlbin-gen/.Rosace.aadlbin new file mode 100644 index 0000000..61b8dcd Binary files /dev/null and b/models/Rosace/Rosace_aadl/.aadlbin-gen/.Rosace.aadlbin differ diff --git a/models/Rosace/Rosace_aadl/.aadlsettings b/models/Rosace/Rosace_aadl/.aadlsettings new file mode 100644 index 0000000..a3c30ab --- /dev/null +++ b/models/Rosace/Rosace_aadl/.aadlsettings @@ -0,0 +1,3 @@ +#Wed Jun 07 10:14:36 CEST 2017 +source.directory=/aadl +model.directory=/aaxl diff --git a/models/Rosace/Rosace_aadl/.project b/models/Rosace/Rosace_aadl/.project new file mode 100644 index 0000000..b2d0f73 --- /dev/null +++ b/models/Rosace/Rosace_aadl/.project @@ -0,0 +1,19 @@ + + + Rosace_aadl + + + Plugin_Resources + + + + org.eclipse.xtext.ui.shared.xtextBuilder + + + + + + org.eclipse.xtext.ui.shared.xtextNature + org.osate.core.aadlnature + + diff --git a/models/Rosace/Rosace_aadl/Rosace.aadl b/models/Rosace/Rosace_aadl/Rosace.aadl new file mode 100644 index 0000000..69f9832 --- /dev/null +++ b/models/Rosace/Rosace_aadl/Rosace.aadl @@ -0,0 +1,71 @@ +package Rosace +public + +system Rosace_with_Controller +end Rosace_with_Controller; + +system implementation Rosace_with_Controller.with_devices + subcomponents + this_controller: process Controller.with_threads; + this_Aircraft_Dynamics_FMU: device Aircraft_Dynamics_FMU.impl; + this_Engine_FMU: device Engine_FMU.impl; + this_Elevator_FMU: device Elevator_FMU.impl; + connections + throttle_com_conn: port this_controller.throttle_command_out -> this_Engine_FMU.throttle_command_in; + elevator_com_conn: port this_controller.elevator_deflection_command_out -> this_Elevator_FMU.elevator_deflection_command_in; + engine_thrust_conn: port this_Engine_FMU.engine_thrust_out -> this_Aircraft_Dynamics_FMU.engine_thrust_in; + elevator_def_conn: port this_Elevator_FMU.elevator_deflection_out -> this_Aircraft_Dynamics_FMU.elevator_deflection_in; + altitude_out: port this_Aircraft_Dynamics_FMU.altitude_out -> this_controller.altitude_in; + vertical_acc_conn: port this_Aircraft_Dynamics_FMU.vertical_acceleration_out -> this_controller.vertical_acceleration_in; + vertical_speed_conn: port this_Aircraft_Dynamics_FMU.vertical_speed_out -> this_controller.vertical_speed_in; + pitch_rate_conn: port this_Aircraft_Dynamics_FMU.pitch_rate_out -> this_controller.pitch_rate_in; + true_airspeed_conn: port this_Aircraft_Dynamics_FMU.true_air_speed_out -> this_controller.true_air_speed_in; +end Rosace_with_Controller.with_devices; + +process Controller + features + altitude_in: in data port; + vertical_speed_in: in data port; + vertical_acceleration_in: in data port; + true_air_speed_in: in data port; + pitch_rate_in: in data port; + throttle_command_out: out event port; + elevator_deflection_command_out: out event port; +end Controller; + +process implementation Controller.with_threads +end Controller.with_threads; + +device Aircraft_Dynamics_FMU + features + engine_thrust_in: in data port; + elevator_deflection_in: in data port; + altitude_out: out data port; + vertical_speed_out: out data port; + vertical_acceleration_out: out data port; + true_air_speed_out: out data port; + pitch_rate_out: out data port; +end Aircraft_Dynamics_FMU; + +device Engine_FMU + features + throttle_command_in: in event port; + engine_thrust_out: out data port; +end Engine_FMU; + +device Elevator_FMU + features + elevator_deflection_command_in: in event port; + elevator_deflection_out: out data port; +end Elevator_FMU; + +device implementation Aircraft_Dynamics_FMU.impl +end Aircraft_Dynamics_FMU.impl; + +device implementation Engine_FMU.impl +end Engine_FMU.impl; + +device implementation Elevator_FMU.impl +end Elevator_FMU.impl; + +end Rosace; \ No newline at end of file diff --git a/models/Rosace/Rosace_aadl/diagrams/4b640469-11e1-4f36-b9b8-de2706854778.aadl_diagram b/models/Rosace/Rosace_aadl/diagrams/4b640469-11e1-4f36-b9b8-de2706854778.aadl_diagram new file mode 100644 index 0000000..b8d1e8b --- /dev/null +++ b/models/Rosace/Rosace_aadl/diagrams/4b640469-11e1-4f36-b9b8-de2706854778.aadl_diagram @@ -0,0 +1,1465 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/models/Rosace/Rosace_aadl/diagrams/b877ad7b-1eee-4ffa-bfa8-fa2c417d7492.aadl_diagram b/models/Rosace/Rosace_aadl/diagrams/b877ad7b-1eee-4ffa-bfa8-fa2c417d7492.aadl_diagram new file mode 100644 index 0000000..4920f4a --- /dev/null +++ b/models/Rosace/Rosace_aadl/diagrams/b877ad7b-1eee-4ffa-bfa8-fa2c417d7492.aadl_diagram @@ -0,0 +1,400 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/simulator/.gitignore b/src/simulator/.gitignore index 9f3dcc5..26f8772 100644 --- a/src/simulator/.gitignore +++ b/src/simulator/.gitignore @@ -1,2 +1,3 @@ /Debug/ +/Release/ diff --git a/src/simulator/src/fmi2_api_impl.c b/src/simulator/src/fmi2_api_impl.c index 92ad900..afb8b4e 100644 --- a/src/simulator/src/fmi2_api_impl.c +++ b/src/simulator/src/fmi2_api_impl.c @@ -12,8 +12,6 @@ #include "simulator/external_clock.h" #include "AADL_fmi2CS.h" -//#define FMU_SLAVE 1 - int doOneStep(AADL_fmi2CSComponent* ci, fmi2Real time, fmi2Real ccp){ @@ -21,12 +19,8 @@ int doOneStep(AADL_fmi2CSComponent* ci, fmi2Real time, fmi2Real ccp){ set_sclock_currentTime(time); set_sclock_communicationPoint(ccp); - //kill((int) getpid(), SIGUSR1); - print_logical_clock(); - kill((int) getpid(), SIGUSR2); - //start_scheduler(); - - //kill((int) getpid(), SIGUSR2); + //print_logical_clock(); + start_scheduler(); printf("doStep finished\n"); @@ -74,18 +68,21 @@ int InitializeSlave(fmi2Component c){ initialize_period(); - configure_rr_scheduler(500); - for (i = 0 ; i < 5 ; ++i){ + /*Todo: SWITCH strategy depending on the config at export*/ + + configure_rr_scheduler(0); - ci->tid = um_thread_create(user_thread_fmi, STACKSIZE, 0); + for (i = 0 ; i < 10 ; ++i){ + + ci->tid = um_thread_create(user_thread_fmi, 0, i); } - for (i = 5 ; i < 10 ; ++i){ + /*for (i = 5 ; i < 10 ; ++i){ ci->tid = um_thread_create(user_thread_fmi_2, STACKSIZE, 0); - } + }*/ /*for (i = 0 ; i < 10 ; ++i){ ci->tid = um_thread_create(user_thread_fmi_3, STACKSIZE, 0); diff --git a/src/simulator/src/simulator/constants.h b/src/simulator/src/simulator/constants.h index c07f791..a1f4d18 100644 --- a/src/simulator/src/simulator/constants.h +++ b/src/simulator/src/simulator/constants.h @@ -5,7 +5,7 @@ #define MAX_THREADS 10 /* Maximum number of threads */ #define STACKSIZE (128 * 1024) /* Default stack size */ -#define INTERVAL 700 /* timer interval in microseconds */ +#define INTERVAL 500 /* timer interval in microseconds */ #define MAX_CORE 1 diff --git a/src/simulator/src/simulator/external_clock.c b/src/simulator/src/simulator/external_clock.c index 2afa41a..3885db0 100644 --- a/src/simulator/src/simulator/external_clock.c +++ b/src/simulator/src/simulator/external_clock.c @@ -10,6 +10,10 @@ simulatorClock* sclock; +/* + * TODO: mettre une periode pour faire du temps réel simulé et "accélérer" les calculs? + */ + void init_sclock(){ sclock = calloc(1, sizeof(simulatorClock)); sclock->h = 0.0; diff --git a/src/simulator/src/simulator/timer_interrupt.c b/src/simulator/src/simulator/timer_interrupt.c index 742a258..a198485 100644 --- a/src/simulator/src/simulator/timer_interrupt.c +++ b/src/simulator/src/simulator/timer_interrupt.c @@ -16,10 +16,10 @@ ucontext_t signal_context; /* the interrupt context */ void *signal_stack; /* global interrupt stack */ struct sigaction act; -#ifdef FMU_SLAVE +/*#ifdef FMU_SLAVE struct sigaction act2; struct sigaction act3; -#endif +#endif*/ /******************************************************************************/ /* Timer interrupt handler: @@ -46,21 +46,23 @@ void timer_interrupt(int j, siginfo_t *si, void *old_context) swapcontext(get_current_context(), &signal_context); } -#ifdef FMU_SLAVE -void set_sclockTime(){ +/*#ifdef FMU_SLAVE +void check_sclockTime(){ - /*this simulates real time but is not simulated time*/ - abs_time c_time; - clock_gettime(CLOCK_MONOTONIC, &c_time); - getSClock()->currentTime += c_time.tv_sec - getSClock()->c_time.tv_sec + (float)(c_time.tv_nsec - getSClock()->c_time.tv_nsec) / 1000000000L; - set_sclock_absolute_time(); + //abs_time c_time; + //clock_gettime(CLOCK_MONOTONIC, &c_time); + + //getSClock()->currentTime += c_time.tv_sec - getSClock()->c_time.tv_sec + (float)(c_time.tv_nsec - getSClock()->c_time.tv_nsec) / 1000000000L; + + + //set_sclock_absolute_time(); } void launch_scheduler(){ scheduler(); } -#endif +#endif*/ void init_timer(void) { @@ -72,8 +74,8 @@ void init_timer(void) { act.sa_sigaction = timer_interrupt; /* bind function to the timer */ #ifdef FMU_SLAVE - act2.sa_sigaction = set_sclockTime; - act3.sa_sigaction = launch_scheduler; + //act2.sa_sigaction = check_sclockTime; + //act3.sa_sigaction = launch_scheduler; #endif } @@ -90,13 +92,13 @@ void setup_timer(uint32_t period, bool periodic) act.sa_flags = SA_RESTART /* interruptible functions do not raise [EINTR] */ | SA_SIGINFO; /* to select particular signature signal handler */ -#ifdef FMU_SLAVE +/*#ifdef FMU_SLAVE if(sigaction(SIGUSR1, &act2, NULL) != 0) perror("Signal handler"); if(sigaction(SIGUSR2, &act3, NULL) != 0) - perror("Signal handler"); + perror("Signal handler"); -#else +#else*/ struct itimerval it; if(sigaction(SIGALRM, &act, NULL) != 0) perror("Signal handler"); @@ -113,7 +115,7 @@ void setup_timer(uint32_t period, bool periodic) if (setitimer(ITIMER_REAL, &it, NULL)) perror("setitiimer"); -#endif +//#endif } diff --git a/src/simulator/src/simulator/um_threads.c b/src/simulator/src/simulator/um_threads.c index ae8374c..3c8f0c8 100644 --- a/src/simulator/src/simulator/um_threads.c +++ b/src/simulator/src/simulator/um_threads.c @@ -26,160 +26,153 @@ scheduler_function the_scheduler; waiting_list *w_list = NULL; - - /******************************************************************************/ um_thread_id um_thread_create (void (*function)(void), - stack_size_t stack_size, - priority_t priority) + stack_size_t stack_size, + priority_t priority) { - void *stack; - int err; - - err = getcontext(&(threads[um_thread_index].um_context)); - if (err < 0) - perror("getcontext"); - - if (stack_size == 0) - threads[um_thread_index].stack_size = STACKSIZE; - else - threads[um_thread_index].stack_size = stack_size; - - /* Allocate memory for the thread stack */ - stack = malloc(threads[um_thread_index].stack_size); - if (stack == NULL) { - perror("malloc"); - exit(1); - } - - /* Initialze the ucontext structure: - * - stack and stack size - * - reset sigmask - */ - threads[um_thread_index].um_context.uc_stack.ss_sp = stack; - threads[um_thread_index].um_context.uc_stack.ss_size - = threads[um_thread_index].stack_size; - - threads[um_thread_index].um_context.uc_stack.ss_flags = 0; - sigemptyset(&threads[um_thread_index].um_context.uc_sigmask); - - /* Attach user function */ - makecontext(&threads[um_thread_index].um_context, function, 0); - - /* Update internal arrays of threads */ - threads[um_thread_index].tid = um_thread_index; - threads[um_thread_index].priority = priority; - threads[um_thread_index].state = READY; - - debug_printf("Created thread context: %p, tid %d, function %p\n", - &(threads[um_thread_index].um_context), threads[um_thread_index].tid, - function); - - return um_thread_index++; + void *stack; + int err; + + err = getcontext(&(threads[um_thread_index].um_context)); + if (err < 0) + perror("getcontext"); + + if (stack_size == 0) + threads[um_thread_index].stack_size = STACKSIZE; + else + threads[um_thread_index].stack_size = stack_size; + + /* Allocate memory for the thread stack */ + stack = malloc(threads[um_thread_index].stack_size); + if (stack == NULL) { + perror("malloc"); + exit(1); + } + + /* Initialze the ucontext structure: + * - stack and stack size + * - reset sigmask + */ + threads[um_thread_index].um_context.uc_stack.ss_sp = stack; + threads[um_thread_index].um_context.uc_stack.ss_size + = threads[um_thread_index].stack_size; + + threads[um_thread_index].um_context.uc_stack.ss_flags = 0; + sigemptyset(&threads[um_thread_index].um_context.uc_sigmask); + + /* Attach user function */ + makecontext(&threads[um_thread_index].um_context, function, 0); + + /* Update internal arrays of threads */ + threads[um_thread_index].tid = um_thread_index; + threads[um_thread_index].priority = priority; + threads[um_thread_index].state = READY; + + debug_printf("Created thread context: %p, tid %d, function %p\n", + &(threads[um_thread_index].um_context), threads[um_thread_index].tid, + function); + + return um_thread_index++; } /******************************************************************************/ ucontext_t *get_context (um_thread_id tid) { - return &(threads[tid].um_context); + return &(threads[tid].um_context); } /******************************************************************************/ ucontext_t *get_current_context (void) { - return sched_context; + return sched_context; } /******************************************************************************/ um_thread_id get_current_context_id (void) { - return sched_current_context_id; + return sched_current_context_id; } /******************************************************************************/ void start_scheduler (void) { - sched_current_context_id = 0; - sched_context = get_context(sched_current_context_id); - debug_printf("Starting scheduler @ %p\n", sched_context); + sched_current_context_id = 0; + sched_context = get_context(sched_current_context_id); + debug_printf("Starting scheduler @ %p\n", sched_context); #ifdef FMU_SLAVE - set_sclock_absolute_time(); + //set_sclock_absolute_time(); #endif - scheduler(); + scheduler(); } /******************************************************************************/ /* The scheduling algorithm; selects the next context to run, then starts it. */ void scheduler(void) { - um_thread_id previous = sched_current_context_id; - - if (threads[sched_current_context_id].state == RUNNING) { - threads[sched_current_context_id].state = READY; - do_awake_list(); - } - - sched_current_context_id = the_scheduler (); - - sched_context = get_context (sched_current_context_id); - threads[sched_current_context_id].state = RUNNING; + um_thread_id previous = sched_current_context_id; + + if (threads[sched_current_context_id].state == RUNNING) { + threads[sched_current_context_id].state = READY; + do_awake_list(); + } + + sched_current_context_id = the_scheduler (); + + sched_context = get_context (sched_current_context_id); + threads[sched_current_context_id].state = RUNNING; #ifdef FMU_SLAVE - print_timestamp(); + print_timestamp(); #endif - debug_printf("Switching from %d to %d\n", - previous, sched_current_context_id); - - setcontext(sched_context); /* go */ + debug_printf("Switching from %d to %d\n", + previous, sched_current_context_id); + + setcontext(sched_context); /* go */ } void do_awake_list(void) { abs_time c_time; waiting_list *aux; - - // awake all threads which needed and take them out of the waiting_list Rq : the timer resolution is 1ms so we check within this resolution if (w_list != NULL) { + + #ifdef FMU_SLAVE - //clock_gettime(CLOCK_MONOTONIC, &c_time); - //debug_printf("----> clock = %f\n", getSClock()->c_time.tv_sec + (float)(getSClock()->c_time.tv_nsec) / 1000000000L); - //debug_printf("----> current_abs = %f\n", c_time.tv_sec + (float)(c_time.tv_nsec) / 1000000000L); + clock_gettime(CLOCK_MONOTONIC, &c_time); stop_timer(); - float trigger_time = getSClock()->c_time.tv_sec - (w_list->t).tv_sec + (float)(getSClock()->c_time.tv_nsec - (w_list->t).tv_nsec) / 1000000000L; - //float start_abs = getSClock()->c_time.tv_sec + (float)(getSClock()->c_time.tv_nsec) / 1000000000L; - //float thread_wait_limit = (w_list->t).tv_sec + (float)((w_list->t).tv_nsec) / 1000000000L; - - //If the time is elapsed and if the elapsed is situated during the time of the do step - while(w_list != NULL && trigger_time >= 0.0 && trigger_time <= getSClock()->startTime + getSClock()->h) { - //while(w_list != NULL && thread_wait_limit <= start_abs + getSClock()->startTime) { - debug_printf("----> thread %d limit = %f, current_abs = %f, trigger = %f, end of step = %f\n", w_list->tid, (w_list->t).tv_sec + (float)((w_list->t).tv_nsec) / 1000000000L, getSClock()->c_time.tv_sec + (float)(getSClock()->c_time.tv_nsec) / 1000000000L, trigger_time, getSClock()->startTime + getSClock()->h); - //debug_printf("----> thread %d limit = %f, start_abs = %f, end of step = %f\n", w_list->tid, thread_wait_limit, start_abs, start_abs + getSClock()->startTime + getSClock()->h); + // awake all threads which needed and take them out of the waiting_list Rq : the timer resolution is 1ms so we check within this resolution + while(w_list != NULL && ((w_list->t).tv_sec < c_time.tv_sec || ((w_list->t).tv_sec == c_time.tv_sec && ((w_list->t).tv_nsec <= c_time.tv_nsec + 999999L)))) { + debug_printf("----> %d (%d.%ld)\n", w_list->tid, (int)(w_list->t).tv_sec % 1000, (long)(w_list->t).tv_nsec/CLOCKS_PER_SEC); threads[w_list->tid].state = READY; - aux = w_list->next; - free(w_list); - w_list = aux; + aux = w_list->next; + free(w_list); + w_list = aux; } // set the time for the next thread set_timer_next(); + + #else clock_gettime(CLOCK_MONOTONIC, &c_time); stop_timer(); + // awake all threads which needed and take them out of the waiting_list Rq : the timer resolution is 1ms so we check within this resolution while(w_list != NULL && ((w_list->t).tv_sec < c_time.tv_sec || ((w_list->t).tv_sec == c_time.tv_sec && ((w_list->t).tv_nsec <= c_time.tv_nsec + 999999L)))) { - debug_printf("----> %d (%d.%ld)\n", w_list->tid, (int)(w_list->t).tv_sec % 1000, (long)(w_list->t).tv_nsec/CLOCKS_PER_SEC); - threads[w_list->tid].state = READY; - aux = w_list->next; - free(w_list); - w_list = aux; - } - + debug_printf("----> %d (%d.%ld)\n", w_list->tid, (int)(w_list->t).tv_sec % 1000, (long)(w_list->t).tv_nsec/CLOCKS_PER_SEC); + threads[w_list->tid].state = READY; + aux = w_list->next; + free(w_list); + w_list = aux; + } + // set the time for the next thread set_timer_next(); - #endif } return; + } void set_timer_next() { @@ -196,17 +189,17 @@ ucontext_t yield_context; void* yield_stack; void um_thread_yield (void) { - + /* Create new scheduler context */ - getcontext(&yield_context); - yield_context.uc_stack.ss_sp = yield_stack; - yield_context.uc_stack.ss_size = STACKSIZE; - yield_context.uc_stack.ss_flags = 0; - sigemptyset(&yield_context.uc_sigmask); - makecontext(&yield_context, scheduler, 0); - - /* Save running thread, jump to scheduler */ - swapcontext(get_current_context(), &yield_context); + getcontext(&yield_context); + yield_context.uc_stack.ss_sp = yield_stack; + yield_context.uc_stack.ss_size = STACKSIZE; + yield_context.uc_stack.ss_flags = 0; + sigemptyset(&yield_context.uc_sigmask); + makecontext(&yield_context, scheduler, 0); + + /* Save running thread, jump to scheduler */ + swapcontext(get_current_context(), &yield_context); } /******************************************************************************/ @@ -217,29 +210,23 @@ void configure_scheduler (scheduler_function s) { /******************************************************************************/ void delay_until(abs_time n_time) { - abs_time c_time; - -#ifdef FMU_SLAVE clock_gettime(CLOCK_MONOTONIC, &c_time); -#else - clock_gettime(CLOCK_MONOTONIC, &c_time); -#endif - waiting_list *insert, *prec, *aux; - + // Check if the n_time is positive and at least 1ms if (n_time.tv_sec < c_time.tv_sec || (n_time.tv_sec == c_time.tv_sec && n_time.tv_nsec < c_time.tv_nsec + 1000000L)) return; - + // set the thread to WAITING threads[sched_current_context_id].state = WAITING; - + // Insertion of the thread in the sorted waiting list aux = malloc(sizeof(waiting_list)); + aux->t = n_time; aux->tid = sched_current_context_id; - + stop_timer(); // head insertion if (w_list == NULL || (w_list->t).tv_sec > n_time.tv_sec || (((w_list->t).tv_sec == n_time.tv_sec && ((w_list->t).tv_nsec > n_time.tv_nsec)))) { @@ -253,13 +240,12 @@ void delay_until(abs_time n_time) { insert = insert->next; //debug_printf("loop %d, \n", sched_current_context_id); } - + prec->next = aux; aux->next = insert; } - set_timer_next(); + set_timer_next(); // yield the thread um_thread_yield (); - } diff --git a/src/simulator/src/simulator/user_code_fmi.c b/src/simulator/src/simulator/user_code_fmi.c index b66a7ac..721b3c9 100644 --- a/src/simulator/src/simulator/user_code_fmi.c +++ b/src/simulator/src/simulator/user_code_fmi.c @@ -15,18 +15,11 @@ #include "um_threads.h" #include "external_clock.h" -abs_time shift(int second, long nanosecond) { +abs_time shift(unsigned long ms) { abs_time c_time; clock_gettime(CLOCK_MONOTONIC, &c_time); - //debug_printf("Before Second : %ld, ", c_time.tv_sec); - //debug_printf("Before Nanosecond : %ld \n", c_time.tv_nsec); - - long aux = c_time.tv_nsec + nanosecond; - - c_time.tv_sec += second + aux/1000000000L; - c_time.tv_nsec = aux % 1000000000L; - //debug_printf("After Second : %ld, ", c_time.tv_sec); - //debug_printf("After Nanosecond : %ld \n", c_time.tv_nsec); + c_time.tv_sec = ms / 1000; + c_time.tv_nsec = (ms % 1000) * 1000000; return c_time; } @@ -35,18 +28,30 @@ void user_thread_fmi() { //abs_time c_time; um_thread_id my_id = get_current_context_id(); - debug_printf ("Starting thread %d\n", my_id); while (1) { - debug_printf ("Increment clock and launch scheduler %d\n", my_id); - kill((int) getpid(), SIGUSR1); - print_logical_clock(); - kill((int) getpid(), SIGUSR2); + /*for (i = 0; i< 5; i++) { + + debug_printf ("* %d\n", my_id); + //debug_printf ("Increment clock and launch scheduler %d\n", my_id); + //kill((int) getpid(), SIGUSR1); + //print_logical_clock(); + + compute_during_n_times_100ms (1); + + //kill((int) getpid(), SIGUSR2); + } + //kill((int) getpid(), SIGUSR1); + um_thread_yield ();*/ + + + delay_until(shift(10000)); + printf("o<\n"); } } -void user_thread_fmi_2() { +/*void user_thread_fmi_2() { abs_time c_time; int i = 0; @@ -86,4 +91,4 @@ void user_thread_fmi_3() { delay_until(shift(2, 0L)); } -} +}*/ diff --git a/src/simulator/src/simulator/user_code_fmi.h b/src/simulator/src/simulator/user_code_fmi.h index 3531351..9c972de 100644 --- a/src/simulator/src/simulator/user_code_fmi.h +++ b/src/simulator/src/simulator/user_code_fmi.h @@ -14,6 +14,6 @@ void user_thread_fmi(); void user_thread_fmi_2(); void user_thread_fmi_3(); void initClock(); -abs_time shift(int second, long nanosecond); +abs_time shift(unsigned long ms); #endif /* SIMULATOR_USER_CODE_FMI_H_ */ diff --git a/src/simulator/src/test_fmi2_um_threads.c b/src/simulator/src/test_fmi2_um_threads.c index 14b5518..b54fd84 100644 --- a/src/simulator/src/test_fmi2_um_threads.c +++ b/src/simulator/src/test_fmi2_um_threads.c @@ -12,7 +12,7 @@ main (int argc, char *argv[]) fmi2Component c; fmi2Real time = 0.0; - fmi2Real h=10.0; + fmi2Real h=1.0; fmi2CallbackFunctions* fcbf;