From 720d2d5b7ae3986bc39dcff13d48b70b506a6202 Mon Sep 17 00:00:00 2001 From: Duc-Tien Nguyen Date: Wed, 15 May 2019 14:04:29 -0400 Subject: [PATCH] reformule --- cfg/controllerDyn.cfg | 16 - cfg/controllerGS1Dyn.cfg | 57 +- src/tuning_GS1_grt_rtw/multiword_types.h | 4 +- src/tuning_GS1_grt_rtw/rtmodel.h | 4 +- src/tuning_GS1_grt_rtw/rtwtypes.h | 4 +- src/tuning_GS1_grt_rtw/tuning_GS1.cpp | 1227 ++++++++--------- src/tuning_GS1_grt_rtw/tuning_GS1.h | 239 ++-- src/tuning_GS1_grt_rtw/tuning_GS1_data.cpp | 10 +- src/tuning_GS1_grt_rtw/tuning_GS1_private.h | 4 +- src/tuning_GS1_grt_rtw/tuning_GS1_types.h | 4 +- src/tuning_GS1_node.cpp | 79 +- src/tuning_GS2_node.cpp | 1 + src/tuning_nominal_grt_rtw/multiword_types.h | 4 +- src/tuning_nominal_grt_rtw/rtmodel.h | 4 +- src/tuning_nominal_grt_rtw/rtwtypes.h | 4 +- src/tuning_nominal_grt_rtw/tuning_nominal.cpp | 4 +- src/tuning_nominal_grt_rtw/tuning_nominal.h | 6 +- .../tuning_nominal_data.cpp | 6 +- .../tuning_nominal_private.h | 4 +- .../tuning_nominal_types.h | 4 +- 20 files changed, 761 insertions(+), 924 deletions(-) diff --git a/cfg/controllerDyn.cfg b/cfg/controllerDyn.cfg index 1e2ba94..95488d0 100755 --- a/cfg/controllerDyn.cfg +++ b/cfg/controllerDyn.cfg @@ -72,12 +72,6 @@ group4.add("ky", double_t, CONTROLLER_GAIN["value"],"k-y", group4.add("kvy", double_t, CONTROLLER_GAIN["value"],"k-yd", -0.5505, -2.0, 0.0) group4.add("kiy", double_t, CONTROLLER_GAIN["value"],"k-iy", -0.7657, -2.0, 0.0) -#group4.add("kx", double_t, CONTROLLER_GAIN["value"],"k-x", 1.6651, 0.0, 2.0) -#group4.add("kvx", double_t, CONTROLLER_GAIN["value"],"k-xd", 0.9236, 0.0, 2.0) -#group4.add("kix", double_t, CONTROLLER_GAIN["value"],"k-ix", 1.3486, 0.0, 2.0) -#group4.add("ky", double_t, CONTROLLER_GAIN["value"],"k-y", -1.5145, -2.0, 0.0) -#group4.add("kvy", double_t, CONTROLLER_GAIN["value"],"k-yd", -0.8551, -2.0, 0.0) -#group4.add("kiy", double_t, CONTROLLER_GAIN["value"],"k-iy", -1.1930, -2.0, 0.0) group4.add("kphi", double_t, CONTROLLER_GAIN["value"],"k-phi", 1.7378, 0.0, 10.0) group4.add("kp", double_t, CONTROLLER_GAIN["value"],"k-p", 0.3476, 0.0, 1.0) @@ -92,14 +86,4 @@ group4.add("kpsi", double_t, CONTROLLER_GAIN["value"],"k-psi", group4.add("kr", double_t, CONTROLLER_GAIN["value"],"k-p", 0.3130, 0.0, 0.5) group4.add("kipsi", double_t, CONTROLLER_GAIN["value"],"k-ipsi", 0.3162, 0.0, 0.5) -#group4.add("kphi", double_t, CONTROLLER_GAIN["value"],"k-phi", 2.6042, 0.0, 10.0) -#group4.add("kp", double_t, CONTROLLER_GAIN["value"],"k-p", 0.4431, 0.0, 1.0) -#group4.add("kiphi", double_t, CONTROLLER_GAIN["value"],"k-iphi", 0.0, 0.0, 20.0) -#group4.add("ktheta", double_t, CONTROLLER_GAIN["value"],"k-theta", 2.8913 , 0.0, 10.0) -#group4.add("kq", double_t, CONTROLLER_GAIN["value"],"k-q", 0.5163, 0.0, 1.0) -#group4.add("kitheta", double_t, CONTROLLER_GAIN["value"],"k-itheta", 0.0, 0.0, 20.0) -#group4.add("kpsi", double_t, CONTROLLER_GAIN["value"],"k-psi", 0.7660, 0.0, 1.0) -#group4.add("kr", double_t, CONTROLLER_GAIN["value"],"k-p", 0.4006, 0.0, 1.0) -#group4.add("kipsi", double_t, CONTROLLER_GAIN["value"],"k-ipsi", 0.5774, 0.0, 1.0) - exit(gen.generate(PACKAGE, "gsft_control", "controllerDyn")) diff --git a/cfg/controllerGS1Dyn.cfg b/cfg/controllerGS1Dyn.cfg index db63843..896ec04 100755 --- a/cfg/controllerGS1Dyn.cfg +++ b/cfg/controllerGS1Dyn.cfg @@ -20,7 +20,7 @@ CONTROLLER_GAIN = gen.const("CONTROLLER_GAIN", int_t, 0x00000008, "controller_ group1 = gen.add_group("group_control") group2 = gen.add_group("group_waypoint_references") #group3 = gen.add_group("group3_drone_parameters") -group4 = gen.add_group("group_controller_gains") +#group4 = gen.add_group("group_controller_gains") group5 = gen.add_group("group_LOE") # Name Type Reconfiguration level Description Default Min Max @@ -30,15 +30,15 @@ test_mode_enum = gen.enum([ gen.const("TEST_MANUAL", int_t, 0, "Manual test" gen.const("TEST_WAYPOINT", int_t, 1, "Waypoint test"), gen.const("TEST_TRACKING", int_t, 2, "Circular tracking test")], "An enum to chose test mode") -gen.add("test_mode", int_t, 0, "test mode", 0, 0, 2, edit_method=test_mode_enum) +gen.add("test_mode", int_t, 0.0, "test mode", 0, 0, 2, edit_method=test_mode_enum) -LOE_mode_enum = gen.enum([ gen.const("NO", int_t, 1, "Nominal test"), - gen.const("LOE_TRUE", int_t, 2, "LOE true"), - gen.const("LOE_FICTIVE", int_t, 3, "LOE fictive"), - gen.const("LOE_CALCUL", int_t, 4, "LOE calcul"), - gen.const("LOE_FDD", int_t, 5, "LOE fdd")], +LOE_mode_enum = gen.enum([ gen.const("NO", int_t, 0, "Nominal test"), + gen.const("LOE_TRUE", int_t, 1, "LOE true"), + gen.const("LOE_FICTIVE", int_t, 2, "LOE fictive"), + gen.const("LOE_CALCUL", int_t, 3, "LOE calcul"), + gen.const("LOE_FDD", int_t, 4, "LOE fdd")], "An enum to chose LOE estimation mode") -gen.add("LOE_mode", int_t, 0, "LOE mode", 1, 1, 5, edit_method=LOE_mode_enum) +gen.add("LOE_mode", int_t, 0.0, "LOE mode", 0, 0, 4, edit_method=LOE_mode_enum) group1.add("enable_take_off", bool_t, ENABLE_CTRL["value"], "enable take off", False) # ENABLE_CTRL["value"] = 0x00000001 group1.add("enable_landing", bool_t, ENABLE_CTRL["value"], "enable landing", False) @@ -69,45 +69,4 @@ group5.add("LOE_t6", double_t, 0.0, "fault time 6", #group3.add("kT", double_t, NEW_PARAMETERS["value"], "thrust constant", 8.54858e-6, 1e-7, 1e-5) #group3.add("factor", double_t, NEW_PARAMETERS["value"], "scaling factor", 0.035, 0.0, 0.05) -group4.add("kz", double_t, CONTROLLER_GAIN["value"],"k-z", 14.7226, 0.0, 23.2925) -group4.add("kvz", double_t, CONTROLLER_GAIN["value"],"k-zd", 6.8078, 0.0, 8.47) -group4.add("kiz", double_t, CONTROLLER_GAIN["value"],"k-iz", 12.2474, 0.0, 25.025) - -group4.add("kx", double_t, CONTROLLER_GAIN["value"],"k-x", 1.2426, 0.0, 2.0) -group4.add("kvx", double_t, CONTROLLER_GAIN["value"],"k-xd", 0.7085, 0.0, 2.0) -group4.add("kix", double_t, CONTROLLER_GAIN["value"],"k-ix", 0.9913, 0.0, 2.0) -group4.add("ky", double_t, CONTROLLER_GAIN["value"],"k-y", -0.9258, -2.0, 0.0) -group4.add("kvy", double_t, CONTROLLER_GAIN["value"],"k-yd", -0.5770, -2.0, 0.0) -group4.add("kiy", double_t, CONTROLLER_GAIN["value"],"k-iy", -0.6662, -2.0, 0.0) - -#group4.add("kx", double_t, CONTROLLER_GAIN["value"],"k-x", 1.6651, 0.0, 2.0) -#group4.add("kvx", double_t, CONTROLLER_GAIN["value"],"k-xd", 0.9236, 0.0, 2.0) -#group4.add("kix", double_t, CONTROLLER_GAIN["value"],"k-ix", 1.3486, 0.0, 2.0) -#group4.add("ky", double_t, CONTROLLER_GAIN["value"],"k-y", -1.5145, -2.0, 0.0) -#group4.add("kvy", double_t, CONTROLLER_GAIN["value"],"k-yd", -0.8551, -2.0, 0.0) -#group4.add("kiy", double_t, CONTROLLER_GAIN["value"],"k-iy", -1.1930, -2.0, 0.0) - -group4.add("kphi", double_t, CONTROLLER_GAIN["value"],"k-phi", 2.0050, 0.0, 10.0) -group4.add("kp", double_t, CONTROLLER_GAIN["value"],"k-p", 0.4087, 0.0, 1.0) -group4.add("kiphi", double_t, CONTROLLER_GAIN["value"],"k-iphi", 0.0, 0.0, 20.0) -group4.add("ktheta", double_t, CONTROLLER_GAIN["value"],"k-theta", 2.2946, 0.0, 10.0) -group4.add("kq", double_t, CONTROLLER_GAIN["value"],"k-q", 0.4589, 0.0, 1.0) -group4.add("kitheta", double_t, CONTROLLER_GAIN["value"],"k-itheta", 0.0, 0.0, 20.0) -#group4.add("kpsi", double_t, CONTROLLER_GAIN["value"],"k-psi", 0.3036, 0.0, 0.498) -#group4.add("kr", double_t, CONTROLLER_GAIN["value"],"k-p", 0.2440, 0.0, 0.3130) -#group4.add("kipsi", double_t, CONTROLLER_GAIN["value"],"k-ipsi", 0.1581, 0.0, 0.3162) -group4.add("kpsi", double_t, CONTROLLER_GAIN["value"],"k-psi", 0.498, 0.0, 1.0) -group4.add("kr", double_t, CONTROLLER_GAIN["value"],"k-p", 0.3130, 0.0, 0.5) -group4.add("kipsi", double_t, CONTROLLER_GAIN["value"],"k-ipsi", 0.3162, 0.0, 0.5) - -#group4.add("kphi", double_t, CONTROLLER_GAIN["value"],"k-phi", 2.6042, 0.0, 10.0) -#group4.add("kp", double_t, CONTROLLER_GAIN["value"],"k-p", 0.4431, 0.0, 1.0) -#group4.add("kiphi", double_t, CONTROLLER_GAIN["value"],"k-iphi", 0.0, 0.0, 20.0) -#group4.add("ktheta", double_t, CONTROLLER_GAIN["value"],"k-theta", 2.8913 , 0.0, 10.0) -#group4.add("kq", double_t, CONTROLLER_GAIN["value"],"k-q", 0.5163, 0.0, 1.0) -#group4.add("kitheta", double_t, CONTROLLER_GAIN["value"],"k-itheta", 0.0, 0.0, 20.0) -#group4.add("kpsi", double_t, CONTROLLER_GAIN["value"],"k-psi", 0.7660, 0.0, 1.0) -#group4.add("kr", double_t, CONTROLLER_GAIN["value"],"k-p", 0.4006, 0.0, 1.0) -#group4.add("kipsi", double_t, CONTROLLER_GAIN["value"],"k-ipsi", 0.5774, 0.0, 1.0) - exit(gen.generate(PACKAGE, "gsft_control", "controllerGS1Dyn")) diff --git a/src/tuning_GS1_grt_rtw/multiword_types.h b/src/tuning_GS1_grt_rtw/multiword_types.h index 228f514..aaede3b 100644 --- a/src/tuning_GS1_grt_rtw/multiword_types.h +++ b/src/tuning_GS1_grt_rtw/multiword_types.h @@ -7,9 +7,9 @@ * * Code generation for model "tuning_GS1". * - * Model version : 1.2101 + * Model version : 1.2189 * Simulink Coder version : 8.12 (R2017a) 16-Feb-2017 - * C++ source code generated on : Mon Oct 22 19:08:51 2018 + * C++ source code generated on : Thu Mar 7 15:36:47 2019 * * Target selection: grt.tlc * Note: GRT includes extra infrastructure and instrumentation for prototyping diff --git a/src/tuning_GS1_grt_rtw/rtmodel.h b/src/tuning_GS1_grt_rtw/rtmodel.h index f9db110..b77bdd2 100644 --- a/src/tuning_GS1_grt_rtw/rtmodel.h +++ b/src/tuning_GS1_grt_rtw/rtmodel.h @@ -7,9 +7,9 @@ * * Code generation for model "tuning_GS1". * - * Model version : 1.2101 + * Model version : 1.2189 * Simulink Coder version : 8.12 (R2017a) 16-Feb-2017 - * C++ source code generated on : Mon Oct 22 19:08:51 2018 + * C++ source code generated on : Thu Mar 7 15:36:47 2019 * * Target selection: grt.tlc * Note: GRT includes extra infrastructure and instrumentation for prototyping diff --git a/src/tuning_GS1_grt_rtw/rtwtypes.h b/src/tuning_GS1_grt_rtw/rtwtypes.h index bf9c4fd..f96810d 100644 --- a/src/tuning_GS1_grt_rtw/rtwtypes.h +++ b/src/tuning_GS1_grt_rtw/rtwtypes.h @@ -7,9 +7,9 @@ * * Code generation for model "tuning_GS1". * - * Model version : 1.2101 + * Model version : 1.2189 * Simulink Coder version : 8.12 (R2017a) 16-Feb-2017 - * C++ source code generated on : Mon Oct 22 19:08:51 2018 + * C++ source code generated on : Thu Mar 7 15:36:47 2019 * * Target selection: grt.tlc * Note: GRT includes extra infrastructure and instrumentation for prototyping diff --git a/src/tuning_GS1_grt_rtw/tuning_GS1.cpp b/src/tuning_GS1_grt_rtw/tuning_GS1.cpp index ebcd049..cef58c5 100644 --- a/src/tuning_GS1_grt_rtw/tuning_GS1.cpp +++ b/src/tuning_GS1_grt_rtw/tuning_GS1.cpp @@ -7,9 +7,9 @@ * * Code generation for model "tuning_GS1". * - * Model version : 1.2101 + * Model version : 1.2189 * Simulink Coder version : 8.12 (R2017a) 16-Feb-2017 - * C++ source code generated on : Mon Oct 22 19:08:51 2018 + * C++ source code generated on : Thu Mar 7 15:36:47 2019 * * Target selection: grt.tlc * Note: GRT includes extra infrastructure and instrumentation for prototyping @@ -135,52 +135,199 @@ void tuning_GS1ModelClass::step() } { - real_T un[6]; - static const real_T Kz[12] = { 2.6144, 2.6144, 2.6144, 2.6144, 2.6144, - 2.6144, 1.3106, 1.3106, 1.3106, 1.3106, 1.3106, 1.3106 }; + static const real_T y[12] = { 3.633, 3.633, 3.633, 3.633, 3.633, 3.633, + 2.017, 2.017, 2.017, 2.017, 2.017, 2.017 }; - real_T rtb_Sum1_cy[3]; + static const real_T a[12] = { 0.4546, 1.009, 1.009, 1.009, 1.009, 1.009, + 0.6222, 0.4581, 0.4581, 0.4581, 0.4581, 0.4581 }; + + static const real_T b_a[12] = { 1.009, 0.4546, 1.009, 1.009, 1.009, 1.009, + 0.4581, 0.6222, 0.4581, 0.4581, 0.4581, 0.4581 }; + + static const real_T c_a[12] = { 1.009, 1.009, 0.4546, 1.009, 1.009, 1.009, + 0.4581, 0.4581, 0.6222, 0.4581, 0.4581, 0.4581 }; + + static const real_T d_a[12] = { 1.009, 1.009, 1.009, 0.4546, 1.009, 1.009, + 0.4581, 0.4581, 0.4581, 0.6222, 0.4581, 0.4581 }; + + static const real_T e_a[12] = { 1.009, 1.009, 1.009, 1.009, 0.4546, 1.009, + 0.4581, 0.4581, 0.4581, 0.4581, 0.6222, 0.4581 }; + + static const real_T f_a[12] = { 1.009, 1.009, 1.009, 1.009, 1.009, 0.4546, + 0.4581, 0.4581, 0.4581, 0.4581, 0.4581, 0.6222 }; + + static const real_T g_a[6] = { 0.3052, 0.7377, 0.7377, 0.7377, 0.7377, + 0.7377 }; + + static const real_T h_a[6] = { 0.7377, 0.3052, 0.7377, 0.7377, 0.7377, + 0.7377 }; + + static const real_T i_a[6] = { 0.7377, 0.7377, 0.3052, 0.7377, 0.7377, + 0.7377 }; + + static const real_T j_a[6] = { 0.7377, 0.7377, 0.7377, 0.3052, 0.7377, + 0.7377 }; + + static const real_T k_a[6] = { 0.7377, 0.7377, 0.7377, 0.7377, 0.3052, + 0.7377 }; + + static const real_T l_a[6] = { 0.7377, 0.7377, 0.7377, 0.7377, 0.7377, + 0.3052 }; + + static const real_T y_0[12] = { -2.735, 2.735, -2.735, 2.735, -2.735, 2.735, + -1.888, 1.888, -1.888, 1.888, -1.888, 1.888 }; + + static const real_T b_y[6] = { -1.243, 1.243, -1.243, 1.243, -1.243, 1.243 }; + + static const real_T a_0[12] = { 0.01893, 0.583, -0.583, 0.583, -0.583, 0.583, + -0.253, 0.7419, -0.7419, 0.7419, -0.7419, 0.7419 }; + + static const real_T b_a_0[12] = { -0.583, -0.01893, -0.583, 0.583, -0.583, + 0.583, -0.7419, 0.253, -0.7419, 0.7419, -0.7419, 0.7419 }; + + static const real_T c_a_0[12] = { -0.583, 0.583, 0.01893, 0.583, -0.583, + 0.583, -0.7419, 0.7419, -0.253, 0.7419, -0.7419, 0.7419 }; + + static const real_T d_a_0[12] = { -0.583, 0.583, -0.583, -0.01893, -0.583, + 0.583, -0.7419, 0.7419, -0.7419, 0.253, -0.7419, 0.7419 }; + + static const real_T e_a_0[12] = { -0.583, 0.583, -0.583, 0.583, 0.01893, + 0.583, -0.7419, 0.7419, -0.7419, 0.7419, -0.253, 0.7419 }; + + static const real_T f_a_0[12] = { -0.583, 0.583, -0.583, 0.583, -0.583, + -0.01893, -0.7419, 0.7419, -0.7419, 0.7419, -0.7419, 0.253 }; + + static const real_T g_a_0[6] = { 0.126, 0.261, -0.261, 0.261, -0.261, 0.261 + }; + + static const real_T h_a_0[6] = { -0.261, -0.126, -0.261, 0.261, -0.261, + 0.261 }; + + static const real_T i_a_0[6] = { -0.261, 0.261, 0.126, 0.261, -0.261, 0.261 + }; + + static const real_T j_a_0[6] = { -0.261, 0.261, -0.261, -0.126, -0.261, + 0.261 }; + + static const real_T k_a_0[6] = { -0.261, 0.261, -0.261, 0.261, 0.126, 0.261 + }; + + static const real_T l_a_0[6] = { -0.261, 0.261, -0.261, 0.261, -0.261, + -0.126 }; + + static const real_T a_1[16] = { -0.1661, 0.3195, 0.3195, -0.3195, -0.1374, + 0.3305, 0.3305, -0.3305, -0.6998, 2.039, 2.039, -2.039, -0.4595, 0.5803, + 0.5803, -0.5803 }; + + static const real_T b_a_1[16] = { -0.3195, 0.1661, 0.3195, -0.3195, -0.3305, + 0.1374, 0.3305, -0.3305, -2.039, 0.6998, 2.039, -2.039, -0.5803, 0.4595, + 0.5803, -0.5803 }; + + static const real_T c_a_1[16] = { -0.3195, 0.3195, 0.1661, -0.3195, -0.3305, + 0.3305, 0.1374, -0.3305, -2.039, 2.039, 0.6998, -2.039, -0.5803, 0.5803, + 0.4595, -0.5803 }; + + static const real_T d_a_1[16] = { -0.3195, 0.3195, 0.3195, -0.1661, -0.3305, + 0.3305, 0.3305, -0.1374, -2.039, 2.039, 2.039, -0.6998, -0.5803, 0.5803, + 0.5803, -0.4595 }; + + static const real_T e_a_1[4] = { -0.03956, 0.2068, 0.2068, -0.2068 }; + + static const real_T f_a_1[4] = { -0.2068, 0.03956, 0.2068, -0.2068 }; + + static const real_T g_a_1[4] = { -0.2068, 0.2068, 0.03956, -0.2068 }; + + static const real_T h_a_1[4] = { -0.2068, 0.2068, 0.2068, -0.03956 }; + + static const real_T a_2[24] = { 0.1665, -0.3596, -0.1798, 0.1798, 0.3596, + 0.1798, 0.07562, -0.3218, -0.1609, 0.1609, 0.3218, 0.1609, 0.2108, 1.4594, + 0.7297, -0.7297, -1.4594, -0.7297, 0.3924, 0.2406, 0.1203, -0.1203, + -0.2406, -0.1203 }; + + static const real_T b_a_2[24] = { -0.1798, 0.333, -0.1798, 0.1798, 0.3596, + 0.1798, -0.1609, 0.15124, -0.1609, 0.1609, 0.3218, 0.1609, 0.7297, 0.4216, + 0.7297, -0.7297, -1.4594, -0.7297, 0.1203, 0.7848, 0.1203, -0.1203, + -0.2406, -0.1203 }; + + static const real_T c_a_2[24] = { -0.1798, -0.3596, 0.1665, 0.1798, 0.3596, + 0.1798, -0.1609, -0.3218, 0.07562, 0.1609, 0.3218, 0.1609, 0.7297, 1.4594, + 0.2108, -0.7297, -1.4594, -0.7297, 0.1203, 0.2406, 0.3924, -0.1203, + -0.2406, -0.1203 }; + + static const real_T d_a_2[24] = { -0.1798, -0.3596, -0.1798, -0.1665, 0.3596, + 0.1798, -0.1609, -0.3218, -0.1609, -0.07562, 0.3218, 0.1609, 0.7297, + 1.4594, 0.7297, -0.2108, -1.4594, -0.7297, 0.1203, 0.2406, 0.1203, -0.3924, + -0.2406, -0.1203 }; + + static const real_T e_a_2[24] = { -0.1798, -0.3596, -0.1798, 0.1798, -0.333, + 0.1798, -0.1609, -0.3218, -0.1609, 0.1609, -0.15124, 0.1609, 0.7297, + 1.4594, 0.7297, -0.7297, -0.4216, -0.7297, 0.1203, 0.2406, 0.1203, -0.1203, + -0.7848, -0.1203 }; + + static const real_T f_a_2[24] = { -0.1798, -0.3596, -0.1798, 0.1798, 0.3596, + -0.1665, -0.1609, -0.3218, -0.1609, 0.1609, 0.3218, -0.07562, 0.7297, + 1.4594, 0.7297, -0.7297, -1.4594, -0.2108, 0.1203, 0.2406, 0.1203, -0.1203, + -0.2406, -0.3924 }; + + static const real_T g_a_2[6] = { 0.1855, -0.19436, -0.09718, 0.09718, + 0.19436, 0.09718 }; + + static const real_T h_a_2[6] = { -0.09718, 0.371, -0.09718, 0.09718, 0.19436, + 0.09718 }; + + static const real_T i_a_1[6] = { -0.09718, -0.19436, 0.1855, 0.09718, + 0.19436, 0.09718 }; + + static const real_T j_a_1[6] = { -0.09718, -0.19436, -0.09718, -0.1855, + 0.19436, 0.09718 }; + + static const real_T k_a_1[6] = { -0.09718, -0.19436, -0.09718, 0.09718, + -0.371, 0.09718 }; + + static const real_T l_a_1[6] = { -0.09718, -0.19436, -0.09718, 0.09718, + 0.19436, -0.1855 }; + + real_T rtb_Sum1[3]; real_T rtb_Add_a[6]; real_T rtb_LOE_out[6]; real_T rtb_gamma_m[6]; - real_T rtb_u[6]; + real_T rtb_Kiz[6]; real_T rtb_d_z; + real_T rtb_Kipsi[6]; real_T rtb_d_psi; real_T rtb_Kix[4]; real_T rtb_d_x; - real_T rtb_Kiy[6]; real_T rtb_d_y; + real_T rtb_T_f[6]; real_T rtb_ref[4]; real_T rtb_Clock; real_T rtb_MemoryX[6]; real_T rtb_Kz[12]; + real_T rtb_Kpsi[12]; real_T rtb_Kxtheta[16]; real_T rtb_Kyphi[24]; int32_T i; - static const real_T tmp[6] = { -1.4438, 1.4438, -1.4438, 1.4438, -1.4438, - 1.4438 }; - - static const real_T tmp_0[12] = { -2.274, 2.274, -2.274, 2.274, -2.274, - 2.274, -1.4292, 1.4292, -1.4292, 1.4292, -1.4292, 1.4292 }; - real_T rtb_Add_f[3]; real_T rtb_Add_f_0[3]; - real_T tmp_1[6]; - real_T tmp_2[6]; + real_T rtb_gamma_j; + real_T rtb_gamma_j_0; + real_T rtb_gamma_j_1; + real_T tmp[6]; + real_T rtb_Kipsi_0[6]; + real_T rtb_ref_0[6]; int32_T i_0; real_T rtb_ff_idx_1; real_T rtb_ff_idx_0; real_T rtb_gamma_idx_2; real_T rtb_gamma_idx_1; real_T rtb_gamma_idx_0; - real_T u0; /* Clock: '/Clock' */ rtb_Clock = (&tuning_GS1_M)->Timing.t[0]; - /* MATLAB Function 'Test_config_and_data/LOE_': ':1' */ - /* ':1:2' LOE_out = [0;0;0;0;0;0]; */ - /* ':1:3' for i = 1:6 */ + /* MATLAB Function 'Test_config_and_data/LOE_': ':1' */ + /* ':1:2' LOE_out = [0;0;0;0;0;0]; */ + /* ':1:3' for i = 1:6 */ /* MATLAB Function 'FDD': ':1' */ /* ':1:2' gamma = [0 0 0 0 0 0]'; */ /* ':1:3' for i = 1:6 */ @@ -191,9 +338,9 @@ void tuning_GS1ModelClass::step() */ rtb_LOE_out[i] = 0.0; - /* ':1:4' if t>= LOE_t(i) */ + /* ':1:4' if t>= LOE_t(i) */ if (rtb_Clock >= tuning_GS1_U.LOE_t[i]) { - /* ':1:5' LOE_out(i) = LOE(i); */ + /* ':1:5' LOE_out(i) = LOE(i); */ rtb_LOE_out[i] = tuning_GS1_U.LOE_a[i]; } @@ -233,34 +380,34 @@ void tuning_GS1ModelClass::step() } /* Outputs for Atomic SubSystem: '/UseCurrentEstimator' */ - /* Outputs for Enabled SubSystem: '/Enabled Subsystem' incorporates: - * EnablePort: '/Enable' + /* Outputs for Enabled SubSystem: '/Enabled Subsystem' incorporates: + * EnablePort: '/Enable' */ if (!tuning_GS1_DW.EnabledSubsystem_MODE) { tuning_GS1_DW.EnabledSubsystem_MODE = true; } - /* Sum: '/Add1' incorporates: + /* Sum: '/Add1' incorporates: * Constant: '/C' * Delay: '/MemoryX' * Inport: '/X' - * Product: '/Product' - * Product: '/Product2' + * Product: '/Product' + * Product: '/Product2' */ for (i = 0; i < 3; i++) { - rtb_gamma_idx_1 = 0.0; + rtb_d_z = 0.0; for (i_0 = 0; i_0 < 6; i_0++) { - rtb_gamma_idx_1 += tuning_GS1_ConstP.C_Value[3 * i_0 + i] * + rtb_d_z += tuning_GS1_ConstP.C_Value[3 * i_0 + i] * tuning_GS1_DW.MemoryX_DSTATE[i_0]; } - rtb_Add_f[i] = tuning_GS1_U.X[9 + i] - rtb_gamma_idx_1; + rtb_Add_f[i] = tuning_GS1_U.X[9 + i] - rtb_d_z; } - /* End of Sum: '/Add1' */ + /* End of Sum: '/Add1' */ for (i = 0; i < 6; i++) { - /* Product: '/Product2' incorporates: - * Constant: '/KalmanGainM' + /* Product: '/Product2' incorporates: + * Constant: '/KalmanGainM' */ tuning_GS1_B.Product2[i] = 0.0; tuning_GS1_B.Product2[i] += tuning_GS1_ConstP.KalmanGainM_Value[i] * @@ -270,22 +417,22 @@ void tuning_GS1ModelClass::step() tuning_GS1_B.Product2[i] += tuning_GS1_ConstP.KalmanGainM_Value[i + 12] * rtb_Add_f[2]; - /* Sum: '/Add' incorporates: + /* Sum: '/Add' incorporates: * Delay: '/MemoryX' */ rtb_Add_a[i] = tuning_GS1_B.Product2[i] + tuning_GS1_DW.MemoryX_DSTATE[i]; } - /* End of Outputs for SubSystem: '/Enabled Subsystem' */ + /* End of Outputs for SubSystem: '/Enabled Subsystem' */ /* End of Outputs for SubSystem: '/UseCurrentEstimator' */ /* Product: '/Product1' incorporates: * Constant: '/Constant1' */ for (i = 0; i < 3; i++) { - rtb_Sum1_cy[i] = tuning_GS1_ConstP.Constant1_Value[i + 6] * rtb_Add_a[2] - + (tuning_GS1_ConstP.Constant1_Value[i + 3] * rtb_Add_a[1] + - tuning_GS1_ConstP.Constant1_Value[i] * rtb_Add_a[0]); + rtb_Sum1[i] = tuning_GS1_ConstP.Constant1_Value[i + 6] * rtb_Add_a[2] + + (tuning_GS1_ConstP.Constant1_Value[i + 3] * rtb_Add_a[1] + + tuning_GS1_ConstP.Constant1_Value[i] * rtb_Add_a[0]); } /* End of Product: '/Product1' */ @@ -298,12 +445,12 @@ void tuning_GS1ModelClass::step() * Product: '/j x k' * Product: '/k x i' */ - rtb_Add_f[0] = rtb_Add_a[1] * rtb_Sum1_cy[2]; - rtb_Add_f[1] = rtb_Add_a[2] * rtb_Sum1_cy[0]; - rtb_Add_f[2] = rtb_Add_a[0] * rtb_Sum1_cy[1]; - rtb_Add_f_0[0] = rtb_Add_a[2] * rtb_Sum1_cy[1]; - rtb_Add_f_0[1] = rtb_Add_a[0] * rtb_Sum1_cy[2]; - rtb_Add_f_0[2] = rtb_Add_a[1] * rtb_Sum1_cy[0]; + rtb_Add_f[0] = rtb_Add_a[1] * rtb_Sum1[2]; + rtb_Add_f[1] = rtb_Add_a[2] * rtb_Sum1[0]; + rtb_Add_f[2] = rtb_Add_a[0] * rtb_Sum1[1]; + rtb_Add_f_0[0] = rtb_Add_a[2] * rtb_Sum1[1]; + rtb_Add_f_0[1] = rtb_Add_a[0] * rtb_Sum1[2]; + rtb_Add_f_0[2] = rtb_Add_a[1] * rtb_Sum1[0]; /* Sum: '/Sum3' incorporates: * Constant: '/Constant' @@ -312,11 +459,10 @@ void tuning_GS1ModelClass::step() * Sum: '/Sum1' */ for (i = 0; i < 3; i++) { - rtb_Sum1_cy[i] = (((tuning_GS1_ConstP.Constant1_Value[i + 3] * - rtb_Add_a[4] + tuning_GS1_ConstP.Constant1_Value[i] * - rtb_Add_a[3]) + tuning_GS1_ConstP.Constant1_Value[i - + 6] * rtb_Add_a[5]) + - tuning_GS1_ConstP.Constant_Value[i]) + (rtb_Add_f[i] - + rtb_Sum1[i] = (((tuning_GS1_ConstP.Constant1_Value[i + 3] * rtb_Add_a[4] + + tuning_GS1_ConstP.Constant1_Value[i] * rtb_Add_a[3]) + + tuning_GS1_ConstP.Constant1_Value[i + 6] * rtb_Add_a[5]) + + tuning_GS1_ConstP.Constant_Value[i]) + (rtb_Add_f[i] - rtb_Add_f_0[i]); } @@ -335,9 +481,9 @@ void tuning_GS1ModelClass::step() /* ':1:9' u(2) - u(5) ; */ /* ':1:10' u(3) - u(6) ]; */ /* ':1:11' Residu = diff - M\y; */ - rtb_gamma_idx_1 = rtb_Sum1_cy[0] - rtb_Sum1_cy[1] * -0.57735026918962584; - rtb_gamma_idx_2 = ((rtb_Sum1_cy[2] - rtb_Sum1_cy[1] * 0.19603055651554735) - - rtb_gamma_idx_1 * 0.16976744186046511) / + rtb_gamma_idx_1 = rtb_Sum1[0] - rtb_Sum1[1] * -0.57735026918962584; + rtb_gamma_idx_2 = ((rtb_Sum1[2] - rtb_Sum1[1] * 0.19603055651554735) - + rtb_gamma_idx_1 * 0.16976744186046511) / -0.10949999999999999; rtb_gamma_idx_1 -= rtb_gamma_idx_2 * 0.215; rtb_gamma_idx_1 /= 0.215; @@ -347,9 +493,8 @@ void tuning_GS1ModelClass::step() /* ':1:15' Residu(2)/u(2) */ /* ':1:16' Residu(3)/u(3)]; */ rtb_gamma_idx_0 = ((tuning_GS1_DW.Memory_PreviousInput[0] - - tuning_GS1_DW.Memory_PreviousInput[3]) - - ((rtb_Sum1_cy[1] - rtb_gamma_idx_2 * - 0.18619546181365429) - rtb_gamma_idx_1 * 0.0) / + tuning_GS1_DW.Memory_PreviousInput[3]) - ((rtb_Sum1[1] + - rtb_gamma_idx_2 * 0.18619546181365429) - rtb_gamma_idx_1 * 0.0) / -0.18619546181365429) / tuning_GS1_DW.Memory_PreviousInput[0]; rtb_gamma_idx_1 = ((tuning_GS1_DW.Memory_PreviousInput[1] - @@ -379,28 +524,28 @@ void tuning_GS1ModelClass::step() } /* MultiPortSwitch: '/Multiport Switch' incorporates: - * Inport: '/LOE_calcul' + * Inport: '/LOE_cal' * Inport: '/LOE_mode' */ switch ((int32_T)tuning_GS1_U.LOE_mode) { - case 1: + case 0: for (i = 0; i < 6; i++) { rtb_gamma_m[i] = 0.0; } break; - case 2: + case 1: for (i = 0; i < 6; i++) { rtb_gamma_m[i] = rtb_LOE_out[i]; } break; - case 3: + case 2: break; - case 4: + case 3: for (i = 0; i < 6; i++) { - rtb_gamma_m[i] = tuning_GS1_U.LOE_calcul[i]; + rtb_gamma_m[i] = tuning_GS1_U.LOE_cal[i]; } break; @@ -412,484 +557,275 @@ void tuning_GS1ModelClass::step() } /* End of MultiPortSwitch: '/Multiport Switch' */ - /* MATLAB Function 'GS_controller/z_GS_controller/MATLAB Function': ':1' */ - /* GS_z_gain1_OK.m */ - /* to change 1 */ - /* ':1:4' un = (u - 0.5)./0.5; */ + + /* Saturate: '/ ' */ for (i = 0; i < 6; i++) { - /* Saturate: '/ ' */ + rtb_d_y = rtb_gamma_m[i]; if (rtb_gamma_m[i] > 1.0) { - rtb_d_z = 1.0; - } else if (rtb_gamma_m[i] < 0.0) { - rtb_d_z = 0.0; + rtb_d_y = 1.0; } else { - rtb_d_z = rtb_gamma_m[i]; + if (rtb_gamma_m[i] < 0.0) { + rtb_d_y = 0.0; + } } - /* MATLAB Function: '/MATLAB Function' */ - un[i] = (rtb_d_z - 0.5) / 0.5; - - /* Saturate: '/ ' */ - rtb_u[i] = rtb_d_z; + rtb_gamma_m[i] = rtb_d_y; } - /* MATLAB Function: '/MATLAB Function' */ - /* normalized LOE */ - /* to change 2 */ - /* ':1:7' Kz = ones(6,1)*[2.6144 1.3106]; */ - memcpy(&rtb_Kz[0], &Kz[0], 12U * sizeof(real_T)); + /* End of Saturate: '/ ' */ + + /* MATLAB Function: '/MATLAB Function' */ + /* MATLAB Function 'GS_controller/z_GS_controller/MATLAB Function': ':1' */ + /* Nominal gain */ + /* Kz = [2.4538 1.1346 */ + /* 2.4538 1.1346 */ + /* 2.4538 1.1346]; */ + /* Kiz = [2.0412 */ + /* 2.0412 */ + /* 2.0412]; */ + /* FTC1 gain */ + /* ':1:17' Kx0 = [3.633 2.0170]; */ + /* Kx0 = [a0 b0] */ + /* Kx1 = [a1 b1 */ + /* ':1:18' Kx1 = [0.4546 0.6222 % Kx1 = [a1 b1 */ + /* ':1:19' 1.009 0.4581]; */ + /* a2 b2] */ + /* ':1:20' Ki0 = 2.515; */ + /* Ki0 = i0 */ + /* Ki1 = [i1 i2]' */ + /* ':1:21' Ki1 = [0.3052 % Ki1 = [i1 i2]' */ + /* ':1:22' 0.7377]; */ + /* ':1:24' K_mapping0 = [1 1 1 1 1 1]'; */ + /* ':1:25' K_mapping1 = [1 0 0 0 0 0;0 1 1 1 1 1]'; */ + /* ':1:26' K_mapping2 = [0 1 0 0 0 0;1 0 1 1 1 1]'; */ + /* ':1:27' K_mapping3 = [0 0 1 0 0 0;1 1 0 1 1 1]'; */ + /* ':1:28' K_mapping4 = [0 0 0 1 0 0;1 1 1 0 1 1]'; */ + /* ':1:29' K_mapping5 = [0 0 0 0 1 0;1 1 1 1 0 1]'; */ + /* ':1:30' K_mapping6 = [0 0 0 0 0 1;1 1 1 1 1 0]'; */ + /* ':1:32' Kz = K_mapping0*Kx0 + K_mapping1*Kx1*u(1) + K_mapping2*Kx1*u(2) + K_mapping3*Kx1*u(3) + K_mapping4*Kx1*u(4) + K_mapping5*Kx1*u(5) + K_mapping6*Kx1*u(6); */ + for (i = 0; i < 12; i++) { + rtb_Kz[i] = (((((a[i] * rtb_gamma_m[0] + y[i]) + b_a[i] * rtb_gamma_m[1]) + + c_a[i] * rtb_gamma_m[2]) + d_a[i] * rtb_gamma_m[3]) + + e_a[i] * rtb_gamma_m[4]) + f_a[i] * rtb_gamma_m[5]; + } - /* ':1:8' Kiz = 1.9268.*ones(6,1); */ + /* ':1:33' Kiz = K_mapping0*Ki0 + K_mapping1*Ki1*u(1) + K_mapping2*Ki1*u(2) + K_mapping3*Ki1*u(3) + K_mapping4*Ki1*u(4) + K_mapping5*Ki1*u(5) + K_mapping6*Ki1*u(6); */ for (i = 0; i < 6; i++) { - rtb_gamma_m[i] = 1.9268; + rtb_Kiz[i] = (((((g_a[i] * rtb_gamma_m[0] + 2.515) + h_a[i] * rtb_gamma_m + [1]) + i_a[i] * rtb_gamma_m[2]) + j_a[i] * rtb_gamma_m[3]) + + k_a[i] * rtb_gamma_m[4]) + l_a[i] * rtb_gamma_m[5]; } - /* ':1:10' Kz_i = [0.0345 0.0399]; */ - /* ':1:11' Kz_j = [0.0381 0.0525]; */ - /* ':1:12' Kiz_i = -0.0554; */ - /* ':1:13' Kiz_j = -0.0597; */ - /* */ - /* ':1:15' Kz(1,:) = Kz(1,:) + Kz_i.*un(1) + Kz_j.*un(2) + Kz_j.*un(3); */ - /* ':1:16' Kz(2,:) = Kz(2,:) + Kz_j.*un(1) + Kz_i.*un(2) + Kz_j.*un(3); */ - /* ':1:17' Kz(3,:) = Kz(3,:) + Kz_j.*un(1) + Kz_j.*un(2) + Kz_i.*un(3); */ - /* ':1:18' Kz(4,:) = Kz(4,:) + Kz_i.*un(4) + Kz_j.*un(5) + Kz_j.*un(6); */ - /* ':1:19' Kz(5,:) = Kz(5,:) + Kz_j.*un(4) + Kz_i.*un(5) + Kz_j.*un(6); */ - /* ':1:20' Kz(6,:) = Kz(6,:) + Kz_j.*un(4) + Kz_j.*un(5) + Kz_i.*un(6); */ - rtb_Kz[0] = ((0.0345 * un[0] + 2.6144) + 0.0381 * un[1]) + 0.0381 * un[2]; - rtb_Kz[1] = ((0.0381 * un[0] + rtb_Kz[1]) + 0.0345 * un[1]) + 0.0381 * un[2]; - rtb_Kz[2] = ((0.0381 * un[0] + rtb_Kz[2]) + 0.0381 * un[1]) + 0.0345 * un[2]; - rtb_Kz[3] = ((0.0345 * un[3] + rtb_Kz[3]) + 0.0381 * un[4]) + 0.0381 * un[5]; - rtb_Kz[4] = ((0.0381 * un[3] + rtb_Kz[4]) + 0.0345 * un[4]) + 0.0381 * un[5]; - rtb_Kz[5] = ((0.0381 * un[3] + rtb_Kz[5]) + 0.0381 * un[4]) + 0.0345 * un[5]; - rtb_Kz[6] = ((0.0399 * un[0] + 1.3106) + 0.0525 * un[1]) + 0.0525 * un[2]; - rtb_Kz[7] = ((0.0525 * un[0] + rtb_Kz[7]) + 0.0399 * un[1]) + 0.0525 * un[2]; - rtb_Kz[8] = ((0.0525 * un[0] + rtb_Kz[8]) + 0.0525 * un[1]) + 0.0399 * un[2]; - rtb_Kz[9] = ((0.0399 * un[3] + rtb_Kz[9]) + 0.0525 * un[4]) + 0.0525 * un[5]; - rtb_Kz[10] = ((0.0525 * un[3] + rtb_Kz[10]) + 0.0399 * un[4]) + 0.0525 * un - [5]; - rtb_Kz[11] = ((0.0525 * un[3] + rtb_Kz[11]) + 0.0525 * un[4]) + 0.0399 * un - [5]; - - /* ':1:22' Kiz(1) = Kiz(1) + Kiz_i.*un(1) + Kiz_j.*un(2) + Kiz_j.*un(3); */ - rtb_gamma_m[0] = ((-0.0554 * un[0] + 1.9268) + -0.0597 * un[1]) + -0.0597 * - un[2]; - - /* ':1:23' Kiz(2) = Kiz(2) + Kiz_j.*un(1) + Kiz_i.*un(2) + Kiz_j.*un(3); */ - rtb_gamma_m[1] = ((-0.0597 * un[0] + rtb_gamma_m[1]) + -0.0554 * un[1]) + - -0.0597 * un[2]; - - /* ':1:24' Kiz(3) = Kiz(3) + Kiz_j.*un(1) + Kiz_j.*un(2) + Kiz_i.*un(3); */ - rtb_gamma_m[2] = ((-0.0597 * un[0] + rtb_gamma_m[2]) + -0.0597 * un[1]) + - -0.0554 * un[2]; - - /* ':1:25' Kiz(4) = Kiz(4) + Kiz_i.*un(4) + Kiz_j.*un(5) + Kiz_j.*un(6); */ - rtb_gamma_m[3] = ((-0.0554 * un[3] + rtb_gamma_m[3]) + -0.0597 * un[4]) + - -0.0597 * un[5]; - - /* ':1:26' Kiz(5) = Kiz(5) + Kiz_j.*un(4) + Kiz_i.*un(5) + Kiz_j.*un(6); */ - rtb_gamma_m[4] = ((-0.0597 * un[3] + rtb_gamma_m[4]) + -0.0554 * un[4]) + - -0.0597 * un[5]; - - /* ':1:27' Kiz(6) = Kiz(6) + Kiz_j.*un(4) + Kiz_j.*un(5) + Kiz_i.*un(6); */ - rtb_gamma_m[5] = ((-0.0597 * un[3] + rtb_gamma_m[5]) + -0.0597 * un[4]) + - -0.0554 * un[5]; - /* Sum: '/Sum5' incorporates: * Inport: '/X' * Inport: '/Y0' */ - /* % Nominal gain */ - /* Kz = [ 2.4538 1.1346 */ - /* 2.4538 1.1346 */ - /* 2.4538 1.1346]; */ - /* */ - /* Kiz = [ 2.0412 2.0412 2.0412 2.0412 2.0412 2.0412 ]'; */ - /* ':1:38' gain = [Kz(1,:) Kiz(1)]; */ - /* */ - /* KX = */ - /* 2.6144 1.3106 0.0345 0.0399 0.0381 0.0525 0.0381 0.0525 */ - /* 2.6144 1.3106 0.0381 0.0525 0.0345 0.0399 0.0381 0.0525 */ - /* 2.6144 1.3106 0.0381 0.0525 0.0381 0.0525 0.0345 0.0399 */ - /* 2.7492 1.6026 0.1543 0.1161 0.1543 0.1161 0.1543 0.1161 */ - /* ----------------------------------- */ - /* Ki = */ - /* 1.8709 -0.0554 -0.0597 -0.0597 */ - /* 1.8709 -0.0597 -0.0554 -0.0597 */ - /* 1.8709 -0.0597 -0.0597 -0.0554 */ - /* 1.9268 0.0233 0.0233 0.0233 */ - /* ------------------------------ */ - /* Nominal gain: OK */ - /* Kz = [14.7226 6.8078]; Kiz = 12.2474; */ - /* KX0 = [Kz;Kz;Kz;Kz;Kz;Kz]./6; */ - /* Ki0 = [Kiz;Kiz;Kiz;Kiz;Kiz;Kiz]./6; */ - /* => gain motor 1 = [2.4538 1.1346 2.0412] & gain_z = [14.7226 6.8078 12.2474] % nominal classique */ - /* ------------------------------ */ - /* evalSurf gain: OK [z z_dot z_int] */ - /* u = [0 0 0 ] => gain motor 1 = [2.5037 1.1657 2.0456] => gain_z = [15.0222 6.9942 12.2736] % GS nominal */ - /* u = [0.5 0.5 0.5 ] => gain motor 1 = [2.6144 1.3106 1.8709] => gain_z = [15.6864 7.8636 11.2254] % GS with LOE */ - /* ------------------------------ */ - /* implementation check */ - /* u = [0 0 0 0 0 0] => gain motor 1 = [2.5037 1.1657 2.1016] => gain_z = [15.0222 6.9942 12.6096] % GS nominal implementation */ - /* u = [0.5 0.5 0.5 0.5 0.5 0.5 ] => gain motor 1 = [2.6144 1.3106 1.9268] => gain_z = [15.6864 7.8636 11.5608] % GS with LOE implementation */ - /* 6*[2.6144 1.3106 1.9268] */ + /* ':1:35' gain = [Kz(1,:) Kiz(1)]; */ + /* to display: gain z, zd, int for first rotor */ rtb_d_z = tuning_GS1_U.X[2] - tuning_GS1_U.Y0[2]; + /* MATLAB Function: '/MATLAB Function' */ + /* MATLAB Function 'GS_controller/psi_GS_controller/MATLAB Function': ':1' */ + /* LQR gain */ + /* Kpsi = [-2.2740 -1.4292 */ + /* 2.2740 1.4292 */ + /* -2.2740 -1.4292 */ + /* 2.2740 1.4292 */ + /* -2.2740 -1.4292 */ + /* 2.2740 1.4292]; */ + /* Kipsi = [-1.4438 1.4438 -1.4438 1.4438 -1.4438 1.4438]'; */ + /* FTC1 gain */ + /* ':1:12' Kx0 = [-2.735 -1.888 ]; */ + /* Kx0 = [a0 b0] */ + /* Kx1 = [a1 b1 */ + /* ':1:13' Kx1 = [ 0.01893 -0.253 % Kx1 = [a1 b1 */ + /* ':1:14' -0.583 -0.7419]; */ + /* a2 b2] */ + /* ':1:15' Ki0 = -1.243; */ + /* Ki0 = i0 */ + /* Ki1 = [i1 i2]' */ + /* ':1:16' Ki1 = [ 0.126 % Ki1 = [i1 i2]' */ + /* ':1:17' -0.261]; */ + /* ':1:19' K_mapping0 = [1 -1 1 -1 1 -1]'; */ + /* ':1:20' K_mapping1 = [1 0 0 0 0 0;0 -1 1 -1 1 -1]'; */ + /* ':1:21' K_mapping2 = [0 -1 0 0 0 0;1 0 1 -1 1 -1]'; */ + /* ':1:22' K_mapping3 = [0 0 1 0 0 0;1 -1 0 -1 1 -1]'; */ + /* ':1:23' K_mapping4 = [0 0 0 -1 0 0;1 -1 1 0 1 -1]'; */ + /* ':1:24' K_mapping5 = [0 0 0 0 1 0;1 -1 1 -1 0 -1]'; */ + /* ':1:25' K_mapping6 = [0 0 0 0 0 -1;1 -1 1 -1 1 0]'; */ + /* ':1:27' Kpsi = K_mapping0*Kx0 + K_mapping1*Kx1*u(1) + K_mapping2*Kx1*u(2) + K_mapping3*Kx1*u(3) + K_mapping4*Kx1*u(4) + K_mapping5*Kx1*u(5) + K_mapping6*Kx1*u(6); */ + for (i = 0; i < 12; i++) { + rtb_Kpsi[i] = (((((a_0[i] * rtb_gamma_m[0] + y_0[i]) + b_a_0[i] * + rtb_gamma_m[1]) + c_a_0[i] * rtb_gamma_m[2]) + d_a_0[i] * + rtb_gamma_m[3]) + e_a_0[i] * rtb_gamma_m[4]) + f_a_0[i] * + rtb_gamma_m[5]; + } + + /* ':1:28' Kipsi = K_mapping0*Ki0 + K_mapping1*Ki1*u(1) + K_mapping2*Ki1*u(2) + K_mapping3*Ki1*u(3) + K_mapping4*Ki1*u(4) + K_mapping5*Ki1*u(5) + K_mapping6*Ki1*u(6); */ + for (i = 0; i < 6; i++) { + rtb_Kipsi[i] = (((((g_a_0[i] * rtb_gamma_m[0] + b_y[i]) + h_a_0[i] * + rtb_gamma_m[1]) + i_a_0[i] * rtb_gamma_m[2]) + j_a_0[i] + * rtb_gamma_m[3]) + k_a_0[i] * rtb_gamma_m[4]) + l_a_0[i] + * rtb_gamma_m[5]; + } + /* Sum: '/Sum6' incorporates: * Inport: '/X' * Inport: '/Y0' */ - /* MATLAB Function 'GS_controller/psi_GS_controller/MATLAB Function': ':1' */ - /* GS_psi_3gamma_gain2_OK.m */ - /* to change 1 */ - /* ':1:4' un = (u - 0.5)./0.5; */ - /* normalized LOE */ - /* to change 2 */ - /* Kpsi0 = [-2.2767 -1.6150]; */ - /* Kipsi0 = -1.1607; */ - /* */ - /* Kpsi_i = [0.0543 -0.0262]; */ - /* Kpsi_j = [0.0554 -0.0301]; */ - /* Kipsi_i = 0.0966; */ - /* Kipsi_j = 0.0905; */ - /* */ - /* % [-1 1 -1 1 -1 1] */ - /* Kpsi = zeros(6,2); Kipsi = zeros(6,1); */ - /* Kpsi(1,:) = Kpsi0 + Kpsi_i.*un(1) + Kpsi_j.*un(2) + Kpsi_j.*un(3); */ - /* Kpsi(2,:) = -(Kpsi0 + Kpsi_j.*un(1) + Kpsi_i.*un(2) + Kpsi_j.*un(3)); */ - /* Kpsi(3,:) = Kpsi0 + Kpsi_j.*un(1) + Kpsi_j.*un(2) + Kpsi_i.*un(3); */ - /* Kpsi(4,:) = -(Kpsi0 + Kpsi_i.*un(4) + Kpsi_j.*un(5) + Kpsi_j.*un(6)); */ - /* Kpsi(5,:) = Kpsi0 + Kpsi_j.*un(4) + Kpsi_i.*un(5) + Kpsi_j.*un(6); */ - /* Kpsi(6,:) = -(Kpsi0 + Kpsi_j.*un(4) + Kpsi_j.*un(5) + Kpsi_i.*un(6)); */ - /* */ - /* Kipsi(1) = Kipsi0 + Kipsi_i.*un(1) + Kipsi_j.*un(2) + Kipsi_j.*un(3); */ - /* Kipsi(2) = -(Kipsi0 + Kipsi_j.*un(1) + Kipsi_i.*un(2) + Kipsi_j.*un(3)); */ - /* Kipsi(3) = Kipsi0 + Kipsi_j.*un(1) + Kipsi_j.*un(2) + Kipsi_i.*un(3); */ - /* Kipsi(4) = -(Kipsi0 + Kipsi_i.*un(4) + Kipsi_j.*un(5) + Kipsi_j.*un(6)); */ - /* Kipsi(5) = Kipsi0 + Kipsi_j.*un(4) + Kipsi_i.*un(5) + Kipsi_j.*un(6); */ - /* Kipsi(6) = -(Kipsi0 + Kipsi_j.*un(4) + Kipsi_j.*un(5) + Kipsi_i.*un(6)); */ - /* ':1:30' Kpsi = [-2.2740 -1.4292 */ - /* ':1:31' 2.2740 1.4292 */ - /* ':1:32' -2.2740 -1.4292 */ - /* ':1:33' 2.2740 1.4292 */ - /* ':1:34' -2.2740 -1.4292 */ - /* ':1:35' 2.2740 1.4292]; */ - /* ':1:36' Kipsi = [-1.4438 1.4438 -1.4438 1.4438 -1.4438 1.4438]'; */ - /* ':1:37' gain = [Kpsi(1,:) Kipsi(1)]; */ - /* KX = */ - /* -2.2767 -1.6150 0.0543 -0.0262 0.0554 -0.0301 0.0554 -0.0301 */ - /* 2.2767 1.6150 -0.0554 0.0301 -0.0543 0.0262 -0.0554 0.0301 */ - /* -2.2767 -1.6150 0.0554 -0.0301 0.0554 -0.0301 0.0543 -0.0262 */ - /* 2.4517 1.8414 0.0784 0.1422 0.0784 0.1422 0.0784 0.1422 */ - /* -2.4517 -1.8414 -0.0784 -0.1422 -0.0784 -0.1422 -0.0784 -0.1422 */ - /* 2.4517 1.8414 0.0784 0.1422 0.0784 0.1422 0.0784 0.1422 */ - /* */ - /* Ki = */ - /* -1.1607 0.0966 0.0905 0.0905 */ - /* 1.1607 -0.0905 -0.0966 -0.0905 */ - /* -1.1607 0.0905 0.0905 0.0966 */ - /* 1.2250 -0.0423 -0.0423 -0.0423 */ - /* -1.2250 0.0423 0.0423 0.0423 */ - /* 1.2250 -0.0423 -0.0423 -0.0423 */ - /* ------------------------------ */ - /* Nominal gain: OK */ - /* Kpsi = [0.498 0.313]; Kipsi = 0.3162 */ - /* factor = 0.0365 */ - /* KX0 = 1/6/factor*[-1 1 -1 1 -1 1]'*Kpsi */ - /* Ki0 = 1/6/factor*[-1 1 -1 1 -1 1]'*Kipsi */ - /* => gain motor 1 = [-2.2740 -1.4292 -1.4438] & gain_psi = [0.498 0.3130 0.3162] % nominal classique */ - /* ------------------------------ */ - /* evalSurf gain: OK [psi psi_dot psi_int] */ - /* u = [0 0 0 ] => gain motor 1 = [-2.4419 -1.5286 -1.4382] => gain_psi = [0.5348 0.3348 0.3150 ] % GS nominal */ - /* u = [0.5 0.5 0.5 ] => gain motor 1 = [-2.276 -1.6150 -1.1607] => gain_psi = [0.4984 0.3537 0.2542 ] % GS with LOE */ - /* ------------------------------ */ - /* implementation check */ - /* u = [0 0 0 0 0 0] => gain motor 1 = [-2.4418 -1.5286 -1.4383] => gain_psi = [ ~ ] % GS nominal implementation ~ evalSurf */ - /* u = [0.5 0.5 0.5 0.5 0.5 0.5 ] => gain motor 1 = [-2.2767 -1.6150 -1.1607] => gain_psi = [ ~ ] % GS with LOE implementation ~ evalSurf */ - /* -factor*6*[-2.4418 -1.5286 -1.4383] */ + /* ':1:30' gain = [Kpsi(1,:) Kipsi(1)]; */ + /* to display: gain psi, psid, int for first rotor */ rtb_d_psi = tuning_GS1_U.X[8] - tuning_GS1_U.Y0[3]; /* MATLAB Function: '/MATLAB Function' */ - /* MATLAB Function 'GS_controller/x_GS_controller/MATLAB Function': ':1' */ - /* GS_x_pitch_OK.m */ - /* to change 1 */ - /* ':1:4' un = (u - 0.4)./0.4; */ - for (i = 0; i < 6; i++) { - un[i] = (rtb_u[i] - 0.4) / 0.4; + /* MATLAB Function 'GS_controller/x_GS_controller/MATLAB Function': ':1' */ + /* LQR gain */ + /* K_lqr = [-1.6684 -0.9513 -3.0809 -0.6162 */ + /* 1.6684 0.9513 3.0809 0.6162 */ + /* -1.6684 -0.9513 -3.0809 -0.6162] */ + /* Ki_lqr = [-1.3310 */ + /* 1.3310 */ + /* -1.3310] */ + /* FTC1 gain */ + /* Kx = [ a0 b0 c0 d0| [a1 b1 c1 d1| [a2 b2 c2 d2| [ a2 b2 c2 d2| [a2 b2 c2 d2| */ + /* |-a0 -b0 -c0 -d0| + |-a2 -b2 -c2 -d2| * gamma1 + |-a1 -b1 -c1 -d1| * gamma3 + |-a2 -b2 -c2 -d2| * gamma4 + |-a2 -b2 -c2 -d2| * gamma6 */ + /* |-a0 -b0 -c0 -d0| |-a2 -b2 -c2 -d2| |-a2 -b2 -c2 -d2| |-a1 -b1 -c1 -d1| |-a2 -b2 -c2 -d2| */ + /* | a0 b0 c0 d0] |a2 b2 c2 d2] |a2 b2 c2 d2] | a2 b2 c2 d2] |a1 b1 c1 d1] */ + /* Kix = [i0 [ i1 [ i2 [ i2 [ i2 */ + /* -i0 + -i2 * gamma1 + -i1 * gamma3 + -i2 * gamma4 + -i2 * gamma6 */ + /* -i0 -i2 -i2 -i1 -i2 */ + /* i0] i2] i2] i2] i1] */ + /* ':1:23' Kx0 = [-1.788 -1.367 -4.629 -0.7231]; */ + /* Kx0 = [a0 b0 c0 d0] */ + /* Kx1 = [a1 b1 c1 d1 */ + /* ':1:24' Kx1 = [-0.1661 -0.1374 -0.6998 -0.4595 % Kx1 = [a1 b1 c1 d1 */ + /* ':1:25' -0.3195 -0.3305 -2.039 -0.5803]; */ + /* a2 b2 c2 d2] */ + /* ':1:26' Ki0 = -1.036; */ + /* Ki0 = i0 */ + /* Ki1 = [i1 i2]' */ + /* ':1:27' Ki1 = [-0.03956 % Ki1 = [i1 i2]' */ + /* ':1:28' -0.2068]; */ + /* ':1:30' K_mapping1 = [1 0 0 0;0 -1 -1 1]'; */ + /* ':1:31' K_mapping3 = [0 -1 0 0;1 0 -1 1]'; */ + /* ':1:32' K_mapping4 = [0 0 -1 0;1 -1 0 1]'; */ + /* ':1:33' K_mapping6 = [0 0 0 1;1 -1 -1 0]'; */ + /* ':1:35' Kxtheta = K_mapping1*Kx1*u(1) + K_mapping3*Kx1*u(3) + K_mapping4*Kx1*u(4) + K_mapping6*Kx1*u(6); */ + for (i = 0; i < 16; i++) { + rtb_Kxtheta[i] = ((a_1[i] * rtb_gamma_m[0] + b_a_1[i] * rtb_gamma_m[2]) + + c_a_1[i] * rtb_gamma_m[3]) + d_a_1[i] * rtb_gamma_m[5]; } - /* normalized LOE */ - /* to change 2 */ - /* ':1:7' Kxtheta = zeros(4,4); */ - memset(&rtb_Kxtheta[0], 0, sizeof(real_T) << 4U); - - /* ':1:8' Kix = zeros(4,1); */ - /* */ - /* ':1:10' Kxtheta_i = [0.0260 -0.0179 -0.0407 0.0011]; */ - /* ':1:11' Kxtheta_j = [0.0939 -0.0281 -0.1520 -0.0374]; */ - /* ':1:12' Kix_i = 0.1062; */ - /* ':1:13' Kix_j = 0.1651; */ - /* % [-1 1 1 -1] */ - /* ':1:16' Kxtheta(1,:) = Kxtheta(1,:) + Kxtheta_i.*un(1) + Kxtheta_j.*un(2); */ - /* ':1:17' Kxtheta(2,:) = -(Kxtheta(2,:) + Kxtheta_j.*un(1) + Kxtheta_i.*un(2)); */ - /* ':1:18' Kxtheta(3,:) = -(Kxtheta(3,:) + Kxtheta_i.*un(3) + Kxtheta_j.*un(4)); */ - /* ':1:19' Kxtheta(4,:) = Kxtheta(4,:) + Kxtheta_j.*un(3) + Kxtheta_i.*un(4); */ - rtb_Kxtheta[0] = 0.026 * un[0] + 0.0939 * un[1]; - rtb_Kxtheta[1] = -((0.0939 * un[0] + rtb_Kxtheta[1]) + 0.026 * un[1]); - rtb_Kxtheta[2] = -((0.026 * un[2] + rtb_Kxtheta[2]) + 0.0939 * un[3]); - rtb_Kxtheta[3] = (0.0939 * un[2] + rtb_Kxtheta[3]) + 0.026 * un[3]; - rtb_Kxtheta[4] = -0.0179 * un[0] + -0.0281 * un[1]; - rtb_Kxtheta[5] = -((-0.0281 * un[0] + rtb_Kxtheta[5]) + -0.0179 * un[1]); - rtb_Kxtheta[6] = -((-0.0179 * un[2] + rtb_Kxtheta[6]) + -0.0281 * un[3]); - rtb_Kxtheta[7] = (-0.0281 * un[2] + rtb_Kxtheta[7]) + -0.0179 * un[3]; - rtb_Kxtheta[8] = -0.0407 * un[0] + -0.152 * un[1]; - rtb_Kxtheta[9] = -((-0.152 * un[0] + rtb_Kxtheta[9]) + -0.0407 * un[1]); - rtb_Kxtheta[10] = -((-0.0407 * un[2] + rtb_Kxtheta[10]) + -0.152 * un[3]); - rtb_Kxtheta[11] = (-0.152 * un[2] + rtb_Kxtheta[11]) + -0.0407 * un[3]; - rtb_Kxtheta[12] = 0.0011 * un[0] + -0.0374 * un[1]; - rtb_Kxtheta[13] = -((-0.0374 * un[0] + rtb_Kxtheta[13]) + 0.0011 * un[1]); - rtb_Kxtheta[14] = -((0.0011 * un[2] + rtb_Kxtheta[14]) + -0.0374 * un[3]); - rtb_Kxtheta[15] = (-0.0374 * un[2] + rtb_Kxtheta[15]) + 0.0011 * un[3]; - - /* ':1:21' Kix(1,:) = Kix(1,:) + Kix_i.*un(1) + Kix_j.*un(2); */ - rtb_Kix[0] = 0.1062 * un[0] + 0.1651 * un[1]; - - /* ':1:22' Kix(2,:) = -(Kix(2,:) + Kix_j.*un(1) + Kix_i.*un(2)); */ - rtb_Kix[1] = -(0.1651 * un[0] + 0.1062 * un[1]); - - /* ':1:23' Kix(3,:) = -(Kix(3,:) + Kix_i.*un(3) + Kix_j.*un(4)); */ - rtb_Kix[2] = -(0.1062 * un[2] + 0.1651 * un[3]); - - /* ':1:24' Kix(4,:) = Kix(4,:) + Kix_j.*un(3) + Kix_i.*un(4); */ - rtb_Kix[3] = 0.1651 * un[2] + 0.1062 * un[3]; - /* Sum: '/Sum1' incorporates: * Inport: '/X' * Inport: '/Y0' */ - /* ':1:26' gain = [-1.7167 -1.0582 -1.2273] + [Kxtheta(1,1:2) Kix(1)]; */ - /* */ - /* KX = */ - /* -1.7167 -1.0582 -3.6042 -0.7094 0.0260 -0.0179 -0.0407 0.0011 0.0939 -0.0281 -0.1520 -0.0374 */ - /* 1.7185 1.0547 3.5986 0.7090 -0.0912 0.0228 0.1437 0.0367 -0.0270 0.0157 0.0835 -0.0026 */ - /* 1.7219 1.1497 4.1815 0.8496 -0.0318 0.0304 0.3428 0.1170 -0.0362 0.0392 0.3567 0.1182 */ - /* -1.7219 -1.1497 -4.1815 -0.8496 0.0318 -0.0304 -0.3428 -0.1170 0.0362 -0.0392 -0.3567 -0.1182 */ - /* */ - /* Ki = */ - /* -1.2273 0.1062 0.1651 */ - /* 1.2364 -0.1515 -0.1850 */ - /* 1.1313 -0.0551 -0.0778 */ - /* -1.1313 0.0551 0.0778 */ - /* ------------------------------ */ - /* Nominal gain: OK */ - /* K_x_SFint = [1.2426 0.7085]; Ki_x_SFint = 0.9913; K_pitch_SF = [2.2946 0.4589]; */ - /* % [ x xd theta the_dot integral_x] */ - /* gain_x_theta = [1.2426 0.7085 2.2946 0.4589 0.9913] */ - /* */ - /* arm = 0.215; */ - /* KX0 = 3^(1/2)/(6*arm)*[-1 1 1 -1]'*[K_x_SFint K_pitch_SF] */ - /* Ki0 = 3^(1/2)/(6*arm)*[-1 1 1 -1]'*Ki_x_SFint */ - /* => gain motor 1 = [-1.6684 -0.9513 -3.0809 -0.6162 -1.3310] & gain_x_theta = [1.2426 0.7085 2.2946 0.4589 0.9913] % nominal classique */ - /* ------------------------------ */ - /* evalSurf gain: OK [ x xd theta the_dot integral_x] */ - /* u = [0 0 ] => gain motor 1 = [-1.8366 -1.0123 -3.4114 -0.6731 -1.4986] => gain_x_theta = [1.3679 0.7539 2.5407 0.5013 1.1161 ] % GS nominal */ - /* u = [0.5 0.5] => gain motor 1 = [-1.6868 -1.0697 -3.6523 -0.7185 -1.1595] => gain_x_theta = [1.2563 0.7967 2.7202 0.5351 0.8636 ] % GS with LOE */ - /* ------------------------------ */ - /* implementation check */ - /* u = [0 0 0 0] => gain motor 1 = [-1.8366 -1.0122 -3.4115 -0.6731 -1.4986] => gain_x_theta = [1.3679 0.7539 2.5408 0.5013 1.1161 ] % GS nominal implementation */ - /* u = [0.5 0.5 0.5 0.5] => gain motor 1 = [-1.6867 -1.0697 -3.6524 -0.7185 -1.1595] => gain_x_theta = [1.2562 0.7967 2.7202 0.5351 0.8636 ] % GS with LOE implementation */ - /* */ - /* ll = [-1.7167 -1.0582 -3.6042 -0.7094] */ - /* Kxtheta = [ll; -ll; -ll; ll]+ Kxtheta */ - /* Kix = [-1.2273 1.2273 1.2273 -1.2273]' + Kix */ - /* -(6*arm/sqrt(3)).*[-1.7167 -1.0582 -3.6042 -0.7094 -1.2273] */ - /* =============> [x xd the the_d x_int] = [1.2786 0.7881 2.6843 0.5283 0.9141] =========> dynamic reconfigure */ + /* ':1:36' Kix = K_mapping1*Ki1*u(1) + K_mapping3*Ki1*u(3) + K_mapping4*Ki1*u(4) + K_mapping6*Ki1*u(6); */ + /* ':1:38' gain = [Kx0(1:2) Ki0] + [Kxtheta(1,1:2) Kix(1)]; */ + /* to display: gain x, xd, int for first rotor */ rtb_d_x = tuning_GS1_U.X[0] - tuning_GS1_U.Y0[0]; - - /* Sum: '/Sum1' incorporates: - * Inport: '/X' - * Integrator: '/Integrator1' - * Product: '/Product' - * Product: '/Product1' - * SignalConversion: '/TmpSignal ConversionAtProduct1Inport2' - */ for (i = 0; i < 4; i++) { - rtb_ref[i] = rtb_Kix[i] * tuning_GS1_X.Integrator1_CSTATE_b - + /* MATLAB Function: '/MATLAB Function' */ + rtb_d_y = ((e_a_1[i] * rtb_gamma_m[0] + f_a_1[i] * rtb_gamma_m[2]) + + g_a_1[i] * rtb_gamma_m[3]) + h_a_1[i] * rtb_gamma_m[5]; + + /* Sum: '/Sum1' incorporates: + * Inport: '/X' + * Integrator: '/Integrator1' + * Product: '/Product' + * Product: '/Product1' + * SignalConversion: '/TmpSignal ConversionAtProduct1Inport2' + */ + rtb_ref[i] = rtb_d_y * tuning_GS1_X.Integrator1_CSTATE_p - (((rtb_Kxtheta[i + 4] * tuning_GS1_U.X[3] + rtb_Kxtheta[i] * rtb_d_x) + rtb_Kxtheta[i + 8] * tuning_GS1_U.X[7]) + rtb_Kxtheta[i + 12] * tuning_GS1_U.X[10]); - } - /* End of Sum: '/Sum1' */ + /* MATLAB Function: '/MATLAB Function' */ + rtb_Kix[i] = rtb_d_y; + } /* MATLAB Function: '/MATLAB Function' */ - /* MATLAB Function 'GS_controller/y_GS_controller/MATLAB Function': ':1' */ - /* to change 1 */ - /* ':1:4' un = (u - 0.375)./0.375; */ - for (i = 0; i < 6; i++) { - rtb_u[i] = (rtb_u[i] - 0.375) / 0.375; + /* MATLAB Function 'GS_controller/y_GS_controller/MATLAB Function': ':1' */ + /* LQR gain */ + /* K_lqr = [-0.7348 -0.4267 1.3471 0.2695 */ + /* -1.4696 -0.8535 2.6943 0.5389 */ + /* -0.7348 -0.4267 1.3471 0.2695 */ + /* 0.7348 0.4267 -1.3471 -0.2695 */ + /* 1.4696 0.8535 -2.6943 -0.5389 */ + /* 0.7348 0.4267 -1.3471 -0.2695]; */ + /* Ki_lqr = [ -0.5936 */ + /* -1.1871 */ + /* -0.5936 */ + /* 0.5936 */ + /* 1.1871 */ + /* 0.5936] */ + /* FTC1 gain */ + /* ':1:17' Ky0 = [-0.8246 -0.5442 1.768 0.3045]; */ + /* Ky0 = [a0 b0 c0 d0] */ + /* Ky1 = [a1 b1 c1 d1 */ + /* ':1:18' Ky1 = [ 0.1665 0.07562 0.2108 0.3924 % Ky1 = [a1 b1 c1 d1 */ + /* ':1:19' -0.1798 -0.1609 0.7297 0.1203]; */ + /* a2 b2 c2 d2] */ + /* ':1:20' Kiy0 = -0.577; */ + /* Kiy0 = i0 */ + /* Kiy1 = [i1 i2]' */ + /* ':1:21' Kiy1 = [ 0.1855 % Kiy1 = [i1 i2]' */ + /* ':1:22' -0.09718]; */ + /* ':1:24' K_mapping0 = [1 2 1 -1 -2 -1]'; */ + /* ':1:25' K_mapping1 = [1 0 0 0 0 0;0 2 1 -1 -2 -1]'; */ + /* ':1:26' K_mapping2 = [0 2 0 0 0 0;1 0 1 -1 -2 -1]'; */ + /* ':1:27' K_mapping3 = [0 0 1 0 0 0;1 2 0 -1 -2 -1]'; */ + /* ':1:28' K_mapping4 = [0 0 0 -1 0 0;1 2 1 0 -2 -1]'; */ + /* ':1:29' K_mapping5 = [0 0 0 0 -2 0;1 2 1 -1 0 -1]'; */ + /* ':1:30' K_mapping6 = [0 0 0 0 0 -1;1 2 1 -1 -2 0]'; */ + /* ':1:32' Kyphi = K_mapping1*Ky1*u(1) + K_mapping2*Ky1*u(2) + K_mapping3*Ky1*u(3) + K_mapping4*Ky1*u(4) + K_mapping5*Ky1*u(5) + K_mapping6*Ky1*u(6); */ + for (i = 0; i < 24; i++) { + rtb_Kyphi[i] = ((((a_2[i] * rtb_gamma_m[0] + b_a_2[i] * rtb_gamma_m[1]) + + c_a_2[i] * rtb_gamma_m[2]) + d_a_2[i] * rtb_gamma_m[3]) + + e_a_2[i] * rtb_gamma_m[4]) + f_a_2[i] * rtb_gamma_m[5]; } - /* normalized LOE */ - /* to change 2 */ - /* ':1:7' Kyphi = zeros(6,4); */ - memset(&rtb_Kyphi[0], 0, 24U * sizeof(real_T)); - - /* ':1:8' Kiy = zeros(6,1); */ - /* */ - /* %% GS_y_3gamma_OK.m */ - /* Kyphi_i = [0.0500 0.0327 -0.0649 -0.0583]; */ - /* Kyphi_j = [-0.0406 -0.0468 0.1455 0.0252]; % attention */ - /* Kiy_i = 0.0094; */ - /* Kiy_j = 0.0485; */ - /* %% GS_y_3gamma_OK2.m */ - /* ':1:17' Kyphi_i = [-0.0102 -0.0051 0.0091 0.0038]; */ - /* ':1:18' Kyphi_j = [0.0060 -0.0063 0.0279 0.0113]; */ - /* attention */ - /* ':1:19' Kiy_i = -0.0351; */ - /* ':1:20' Kiy_j = -0.0314; */ - /* %% */ - /* [1 2 1 -1 -2 -1] */ - /* ':1:23' Kyphi(1,:) = Kyphi(1,:) + Kyphi_i.*un(1) + Kyphi_j.*un(2) + Kyphi_j.*un(3); */ - /* ':1:24' Kyphi(2,:) = 2.*(Kyphi(2,:) + Kyphi_j.*un(1) + Kyphi_i.*un(2) + Kyphi_j.*un(3)); */ - /* ':1:25' Kyphi(3,:) = Kyphi(3,:) + Kyphi_j.*un(1) + Kyphi_j.*un(2) + Kyphi_i.*un(3); */ - /* ':1:26' Kyphi(4,:) = -(Kyphi(4,:) + Kyphi_i.*un(4) + Kyphi_j.*un(5) + Kyphi_j.*un(6)); */ - /* ':1:27' Kyphi(5,:) = -2.*(Kyphi(5,:) + Kyphi_j.*un(4) + Kyphi_i.*un(5) + Kyphi_j.*un(6)); */ - /* ':1:28' Kyphi(6,:) = -(Kyphi(6,:) + Kyphi_j.*un(4) + Kyphi_j.*un(5) + Kyphi_i.*un(6)); */ - rtb_Kyphi[0] = (-0.0102 * rtb_u[0] + 0.006 * rtb_u[1]) + 0.006 * rtb_u[2]; - rtb_Kyphi[1] = (((0.006 * rtb_u[0] + rtb_Kyphi[1]) + -0.0102 * rtb_u[1]) + - 0.006 * rtb_u[2]) * 2.0; - rtb_Kyphi[2] = ((0.006 * rtb_u[0] + rtb_Kyphi[2]) + 0.006 * rtb_u[1]) + - -0.0102 * rtb_u[2]; - rtb_Kyphi[3] = -(((-0.0102 * rtb_u[3] + rtb_Kyphi[3]) + 0.006 * rtb_u[4]) + - 0.006 * rtb_u[5]); - rtb_Kyphi[4] = (((0.006 * rtb_u[3] + rtb_Kyphi[4]) + -0.0102 * rtb_u[4]) + - 0.006 * rtb_u[5]) * -2.0; - rtb_Kyphi[5] = -(((0.006 * rtb_u[3] + rtb_Kyphi[5]) + 0.006 * rtb_u[4]) + - -0.0102 * rtb_u[5]); - rtb_Kyphi[6] = (-0.0051 * rtb_u[0] + -0.0063 * rtb_u[1]) + -0.0063 * rtb_u[2]; - rtb_Kyphi[7] = (((-0.0063 * rtb_u[0] + rtb_Kyphi[7]) + -0.0051 * rtb_u[1]) + - -0.0063 * rtb_u[2]) * 2.0; - rtb_Kyphi[8] = ((-0.0063 * rtb_u[0] + rtb_Kyphi[8]) + -0.0063 * rtb_u[1]) + - -0.0051 * rtb_u[2]; - rtb_Kyphi[9] = -(((-0.0051 * rtb_u[3] + rtb_Kyphi[9]) + -0.0063 * rtb_u[4]) - + -0.0063 * rtb_u[5]); - rtb_Kyphi[10] = (((-0.0063 * rtb_u[3] + rtb_Kyphi[10]) + -0.0051 * rtb_u[4]) - + -0.0063 * rtb_u[5]) * -2.0; - rtb_Kyphi[11] = -(((-0.0063 * rtb_u[3] + rtb_Kyphi[11]) + -0.0063 * rtb_u[4]) - + -0.0051 * rtb_u[5]); - rtb_Kyphi[12] = (0.0091 * rtb_u[0] + 0.0279 * rtb_u[1]) + 0.0279 * rtb_u[2]; - rtb_Kyphi[13] = (((0.0279 * rtb_u[0] + rtb_Kyphi[13]) + 0.0091 * rtb_u[1]) + - 0.0279 * rtb_u[2]) * 2.0; - rtb_Kyphi[14] = ((0.0279 * rtb_u[0] + rtb_Kyphi[14]) + 0.0279 * rtb_u[1]) + - 0.0091 * rtb_u[2]; - rtb_Kyphi[15] = -(((0.0091 * rtb_u[3] + rtb_Kyphi[15]) + 0.0279 * rtb_u[4]) - + 0.0279 * rtb_u[5]); - rtb_Kyphi[16] = (((0.0279 * rtb_u[3] + rtb_Kyphi[16]) + 0.0091 * rtb_u[4]) + - 0.0279 * rtb_u[5]) * -2.0; - rtb_Kyphi[17] = -(((0.0279 * rtb_u[3] + rtb_Kyphi[17]) + 0.0279 * rtb_u[4]) - + 0.0091 * rtb_u[5]); - rtb_Kyphi[18] = (0.0038 * rtb_u[0] + 0.0113 * rtb_u[1]) + 0.0113 * rtb_u[2]; - rtb_Kyphi[19] = (((0.0113 * rtb_u[0] + rtb_Kyphi[19]) + 0.0038 * rtb_u[1]) + - 0.0113 * rtb_u[2]) * 2.0; - rtb_Kyphi[20] = ((0.0113 * rtb_u[0] + rtb_Kyphi[20]) + 0.0113 * rtb_u[1]) + - 0.0038 * rtb_u[2]; - rtb_Kyphi[21] = -(((0.0038 * rtb_u[3] + rtb_Kyphi[21]) + 0.0113 * rtb_u[4]) - + 0.0113 * rtb_u[5]); - rtb_Kyphi[22] = (((0.0113 * rtb_u[3] + rtb_Kyphi[22]) + 0.0038 * rtb_u[4]) + - 0.0113 * rtb_u[5]) * -2.0; - rtb_Kyphi[23] = -(((0.0113 * rtb_u[3] + rtb_Kyphi[23]) + 0.0113 * rtb_u[4]) - + 0.0038 * rtb_u[5]); - - /* ':1:30' Kiy(1) = Kiy(1) + Kiy_i.*un(1) + Kiy_j.*un(2) + Kiy_j.*un(3); */ - rtb_Kiy[0] = (-0.0351 * rtb_u[0] + -0.0314 * rtb_u[1]) + -0.0314 * rtb_u[2]; - - /* ':1:31' Kiy(2) = 2*(Kiy(2) + Kiy_j.*un(1) + Kiy_i.*un(2) + Kiy_j.*un(3)); */ - rtb_Kiy[1] = ((-0.0314 * rtb_u[0] + -0.0351 * rtb_u[1]) + -0.0314 * rtb_u[2]) - * 2.0; - - /* ':1:32' Kiy(3) = Kiy(3) + Kiy_j.*un(1) + Kiy_j.*un(2) + Kiy_i.*un(3); */ - rtb_Kiy[2] = (-0.0314 * rtb_u[0] + -0.0314 * rtb_u[1]) + -0.0351 * rtb_u[2]; - - /* ':1:33' Kiy(4) = -(Kiy(4) + Kiy_i.*un(4) + Kiy_j.*un(5) + Kiy_j.*un(6)); */ - rtb_Kiy[3] = -((-0.0351 * rtb_u[3] + -0.0314 * rtb_u[4]) + -0.0314 * rtb_u[5]); - - /* ':1:34' Kiy(5) = -2*(Kiy(5) + Kiy_j.*un(4) + Kiy_i.*un(5) + Kiy_j.*un(6)); */ - rtb_Kiy[4] = ((-0.0314 * rtb_u[3] + -0.0351 * rtb_u[4]) + -0.0314 * rtb_u[5]) - * -2.0; - - /* ':1:35' Kiy(6) = -(Kiy(6) + Kiy_j.*un(4) + Kiy_j.*un(5) + Kiy_i.*un(6)); */ - rtb_Kiy[5] = -((-0.0314 * rtb_u[3] + -0.0314 * rtb_u[4]) + -0.0351 * rtb_u[5]); + /* ':1:33' Kiy = K_mapping1*Kiy1*u(1) + K_mapping2*Kiy1*u(2) + K_mapping3*Kiy1*u(3) + K_mapping4*Kiy1*u(4) + K_mapping5*Kiy1*u(5) + K_mapping6*Kiy1*u(6); */ + rtb_d_y = rtb_gamma_m[0]; + rtb_ff_idx_0 = rtb_gamma_m[1]; + rtb_ff_idx_1 = rtb_gamma_m[2]; + rtb_gamma_j = rtb_gamma_m[3]; + rtb_gamma_j_0 = rtb_gamma_m[4]; + rtb_gamma_j_1 = rtb_gamma_m[5]; + for (i = 0; i < 6; i++) { + rtb_gamma_m[i] = ((((g_a_2[i] * rtb_d_y + h_a_2[i] * rtb_ff_idx_0) + + i_a_1[i] * rtb_ff_idx_1) + j_a_1[i] * rtb_gamma_j) + + k_a_1[i] * rtb_gamma_j_0) + l_a_1[i] * rtb_gamma_j_1; + } /* Sum: '/Sum4' incorporates: * Inport: '/X' * Inport: '/Y0' */ - /* ':1:37' gain = [-0.7177 -0.4473 -0.5164] + [Kyphi(1,1:2) Kiy(1)]; */ - /* Ky, Kyd, Kintegral */ - /* GS_y_3gamma_OK2.m */ - /* KX = */ - /* -0.7177 -0.4473 1.5543 0.3168 -0.0102 -0.0051 0.0091 0.0038 0.0009 -0.0052 0.0471 0.0209 0.0060 -0.0064 0.0278 0.0113 */ - /* -1.4643 -0.8936 3.0410 0.6040 -0.0483 -0.0088 -0.0184 -0.0074 -0.1145 -0.0304 0.0189 -0.0198 -0.0483 -0.0087 -0.0185 -0.0076 */ - /* -0.7177 -0.4473 1.5543 0.3168 0.0060 -0.0063 0.0279 0.0113 0.0009 -0.0051 0.0471 0.0209 -0.0102 -0.0050 0.0091 0.0036 */ - /* -0.7544 -0.4915 1.7728 0.3588 -0.1001 -0.0530 0.1571 0.0227 -0.0576 -0.0544 0.2471 0.0621 -0.1002 -0.0531 0.1569 0.0227 */ - /* -1.5089 -0.9829 3.5456 0.7176 -0.2003 -0.1061 0.3141 0.0455 -0.1152 -0.1087 0.4943 0.1242 -0.2004 -0.1062 0.3138 0.0454 */ - /* -0.7544 -0.4915 1.7728 0.3588 -0.1001 -0.0530 0.1571 0.0227 -0.0576 -0.0544 0.2471 0.0621 -0.1002 -0.0531 0.1569 0.0227 */ - /* ----------------------------------- */ - /* Ki = */ - /* -0.5164 -0.0351 -0.0155 -0.0314 */ - /* -1.0621 -0.0769 -0.0214 -0.0770 */ - /* -0.5164 -0.0313 -0.0155 -0.0352 */ - /* -0.5253 -0.0610 -0.0277 -0.0611 */ - /* -1.0505 -0.1219 -0.0555 -0.1221 */ - /* -0.5253 -0.0610 -0.0277 -0.0611 */ - /* ------------------------------ */ - /* Nominal gain: OK */ - /* K_y_SFint = [-0.9479 -0.5505]; Ki_y_SFint = -0.7657; K_roll_SF = [1.7378 0.3476]; */ - /* [ y yd phi phi_dot integral_y] */ - /* gain_Y_phi = [-0.9479 -0.5505 1.7378 0.3476 -0.7657] */ - /* arm = 0.215 */ - /* KX0 = 1/(6*arm)*[1 2 1 1 2 1]'*[K_y_SFint K_roll_SF] % !!!! not -1 -2 -1 => for better tuning */ - /* Ki0 = 1/(6*arm)*[1 2 1 1 2 1]'*Ki_y_SFint */ - /* gain motor 1 = [-0.7348 -0.4267 1.3471 0.2695 -0.5936] & gain_Y_phi = [-0.9479 -0.5505 1.7378 0.3476 -0.7657] % nominal classique */ - /* ------------------------------ */ - /* evalSurf gain: OK [ y yd phi phi_dot integral_y] */ - /* u = [0 0 0] => gain motor 1 = [-0.7144 -0.4306 1.4702 0.2807 -0.4345] => gain_Y_phi = [-0.9216 -0.5555 1.8966 0.3621 -0.5605] % GS nominal */ - /* u = [0.5 0.5 0.5] => gain motor 1 = [-0.7188 -0.4529 1.5823 0.3288 -0.5438] => gain_Y_phi = [-0.9273 -0.5842 2.0412 0.4242 -0.7015] % GS with LOE */ - /* ------------------------------ */ - /* implementation check */ - /* u = [0 0 0 0 0 0] => gain motor 1 = [-0.7195 -0.4296 1.4894 0.2904 -0.4185] => gain_Y_phi = [-0.9282 -0.5542 1.9213 0.3746 -0.5399] % GS nominal implementation */ - /* u = [0.5 0.5 0.5 0.5 0.5 0.5] => gain motor 1 = [-0.7171 -0.4532 1.5759 0.3256 -0.5490] => gain_Y_phi = [-0.9251 -0.5846 2.0329 0.4200 -0.7082] % GS with LOE implementation */ - /* implementation check: old */ - /* */ - /* k0 = [-0.7177 -0.4473 1.5543 0.3168 ] */ - /* Kyphi = [k0; 2*k0; k0; -k0; -2*k0;-k0]+ Kyphi% */ - /* Kiy = [-0.5164 2*(-0.5164) -0.5164 0.5164 2*0.5164 0.5164]' + Kiy */ - /* (6*arm)*[-0.7177 -0.4473 1.5543 0.3168 -0.5164] */ - /* !============! [y yd phi phid y_int] = [-0.9258 -0.5770 2.0050 0.4087 -0.6662] !=========> dynamic reconfigure */ + /* ':1:35' gain = [Ky0(1:2) Kiy0] + [Kyphi(1,1:2) Kiy(1)]; */ + /* to display: gain y, yd, int for first rotor */ rtb_d_y = tuning_GS1_U.X[1] - tuning_GS1_U.Y0[1]; - /* Sum: '/Sum2' incorporates: + /* Sum: '/Sum2' incorporates: * Inport: '/X' - * Inport: '/gain' - * Integrator: '/Integrator1' - * Product: '/Product2' - * Product: '/Product3' - * SignalConversion: '/TmpSignal ConversionAtProduct3Inport2' + * Integrator: '/Integrator1' + * Product: '/Product2' + * Product: '/Product3' + * SignalConversion: '/TmpSignal ConversionAtProduct3Inport2' */ - tuning_GS1_B.Sum2 = tuning_GS1_U.gain[2] * tuning_GS1_X.Integrator1_CSTATE_p - - (tuning_GS1_U.gain[0] * rtb_d_x + tuning_GS1_U.gain[1] * tuning_GS1_U.X - [3]); + tuning_GS1_B.Sum2 = 0.77159399375578352 * tuning_GS1_X.Integrator1_CSTATE_pc + - (1.3316699428912557 * rtb_d_x + 1.0181167851970618 * tuning_GS1_U.X[3]); - /* Sum: '/Sum3' incorporates: + /* Sum: '/Sum3' incorporates: * Inport: '/X' - * Inport: '/gain' - * Integrator: '/Integrator' - * Product: '/Product1' - * Product: '/Product4' - * SignalConversion: '/TmpSignal ConversionAtProduct1Inport2' + * Integrator: '/Integrator' + * Product: '/Product1' + * Product: '/Product4' + * SignalConversion: '/TmpSignal ConversionAtProduct1Inport2' */ - tuning_GS1_B.Sum3 = tuning_GS1_U.gain[5] * tuning_GS1_X.Integrator_CSTATE - - (tuning_GS1_U.gain[3] * rtb_d_y + tuning_GS1_U.gain[4] * tuning_GS1_U.X[4]); + tuning_GS1_B.Sum3 = -0.74432999999999994 * tuning_GS1_X.Integrator_CSTATE - + (-1.063734 * rtb_d_y + -0.702018 * tuning_GS1_U.X[4]); /* RateTransition: '/T_outer' incorporates: * Inport: '/X' @@ -916,24 +852,24 @@ void tuning_GS1ModelClass::step() * Inport: '/X' * Inport: '/mode' */ - /* MATLAB Function 'Test_config_and_data/FFW': ':1' */ - /* ':1:2' ff = [0;0]; */ + /* MATLAB Function 'Test_config_and_data/FFW': ':1' */ + /* ':1:2' ff = [0;0]; */ rtb_ff_idx_0 = 0.0; rtb_ff_idx_1 = 0.0; - /* ':1:3' g = 9.81; */ + /* ':1:3' g = 9.81; */ /* [x;y] = [cos(t); sin(t)] */ - /* ':1:5' if (test_mode == 2) */ + /* ':1:5' if (test_mode == 2) */ if ((tuning_GS1_U.mode == 2.0) && ((rtb_Clock >= 10.0) && (rtb_Clock <= 50.0))) { - /* ':1:6' if (t >=10) && (t<= 50) */ - /* ':1:7' axref_N = -cos(t); */ - /* ':1:8' ayref_N = -sin(t); */ - /* ':1:10' axref_b = cos(X(8))*cos(X(9))*axref_N + cos(X(8))*sin(X(9))*ayref_N; */ - /* ':1:11' ayref_b = (sin(X(7))*sin(X(8))*cos(X(9)) - cos(X(7))*sin(X(9)))*axref_N + (sin(X(7))*sin(X(8))*sin(X(9)) + cos(X(7))*cos(X(9)))*ayref_N; */ + /* ':1:6' if (t >=10) && (t<= 50) */ + /* ':1:7' axref_N = -cos(t); */ + /* ':1:8' ayref_N = -sin(t); */ + /* ':1:10' axref_b = cos(X(8))*cos(X(9))*axref_N + cos(X(8))*sin(X(9))*ayref_N; */ + /* ':1:11' ayref_b = (sin(X(7))*sin(X(8))*cos(X(9)) - cos(X(7))*sin(X(9)))*axref_N + (sin(X(7))*sin(X(8))*sin(X(9)) + cos(X(7))*cos(X(9)))*ayref_N; */ /* azref_b = (cos(X(7))*sin(X(8))*cos(X(9)) + sin(X(7))*sin(X(9)))*axref_N + (cos(X(7))*sin(X(8))*sin(X(9)) - sin(X(7))*cos(X(9)))*ayref_N; */ /* */ - /* ':1:14' ff = [-ayref_b/g; axref_b/g]; */ + /* ':1:14' ff = [-ayref_b/g; axref_b/g]; */ rtb_ff_idx_0 = -((std::sin(tuning_GS1_U.X[6]) * std::sin(tuning_GS1_U.X[7]) * std::cos(tuning_GS1_U.X[8]) - std::cos(tuning_GS1_U.X [6]) * std::sin(tuning_GS1_U.X[8])) * -std::cos(rtb_Clock) + (std::sin @@ -944,11 +880,28 @@ void tuning_GS1ModelClass::step() -std::cos(rtb_Clock) + std::cos(tuning_GS1_U.X[7]) * std:: sin(tuning_GS1_U.X[8]) * -std::sin(rtb_Clock)) / 9.81; } else { - /* ':1:15' else */ - /* ':1:16' ff = [0;0]; */ + /* ':1:15' else */ + /* ':1:16' ff = [0;0]; */ } /* End of MATLAB Function: '/FFW' */ + if (rtmIsMajorTimeStep((&tuning_GS1_M)) && + (&tuning_GS1_M)->Timing.TaskCounters.TID[1] == 0) { + /* Product: '/Product' incorporates: + * Inport: '/X' + * SignalConversion: '/TmpSignal ConversionAtProductInport2' + */ + tuning_GS1_B.Product = 2.28072 * tuning_GS1_U.X[6] + 0.39280499999999996 * + tuning_GS1_U.X[9]; + + /* Product: '/Product' incorporates: + * Inport: '/X' + * SignalConversion: '/TmpSignal ConversionAtProductInport2' + */ + tuning_GS1_B.Product_d = 3.4475951709416233 * tuning_GS1_U.X[7] + + 0.53855175374981368 * tuning_GS1_U.X[10]; + } + if (rtmIsMajorTimeStep((&tuning_GS1_M)) && (&tuning_GS1_M)->Timing.TaskCounters.TID[2] == 0) { /* Fcn: '/Fcn' */ @@ -960,48 +913,42 @@ void tuning_GS1ModelClass::step() for (i = 0; i < 6; i++) { /* Sum: '/Sum' incorporates: * Inport: '/X' - * Integrator: '/Integrator1' - * Product: '/Product' - * Product: '/Product1' - * SignalConversion: '/TmpSignal ConversionAtProduct1Inport2' - * Sum: '/Sum1' + * Integrator: '/Integrator1' + * Product: '/Product' + * Product: '/Product1' + * SignalConversion: '/TmpSignal ConversionAtProduct1Inport2' + * Sum: '/Sum1' */ - un[i] = (rtb_gamma_m[i] * tuning_GS1_X.Integrator1_CSTATE - (rtb_Kz[i + 6] - * tuning_GS1_U.X[5] + rtb_Kz[i] * rtb_d_z)) + 2.5179000000000005; + tmp[i] = (rtb_Kiz[i] * tuning_GS1_X.Integrator1_CSTATE - (rtb_Kz[i + 6] * + tuning_GS1_U.X[5] + rtb_Kz[i] * rtb_d_z)) + 2.5179000000000005; /* Sum: '/Sum1' incorporates: * Inport: '/X' * Integrator: '/Integrator1' - * MATLAB Function: '/MATLAB Function' * Product: '/Product' + * Product: '/Product1' * SignalConversion: '/TmpSignal ConversionAtProduct1Inport2' * Sum: '/Sum' */ - tmp_1[i] = tmp[i] * tuning_GS1_X.Integrator1_CSTATE_o - (tmp_0[i + 6] * - tuning_GS1_U.X[11] + tmp_0[i] * rtb_d_psi); + rtb_Kipsi_0[i] = rtb_Kipsi[i] * tuning_GS1_X.Integrator1_CSTATE_d - + (rtb_Kpsi[i + 6] * tuning_GS1_U.X[11] + rtb_Kpsi[i] * rtb_d_psi); } - /* Gain: '/Gain' incorporates: + /* Sum: '/Sum' incorporates: * Constant: '/Constant' - * Sum: '/Sum' */ - tmp_2[0] = 0.0 * rtb_ref[0]; - tmp_2[1] = 0.0; - tmp_2[4] = 0.0; - tmp_2[5] = 0.0 * rtb_ref[3]; - tmp_2[2] = 0.0 * rtb_ref[1]; - tmp_2[3] = 0.0 * rtb_ref[2]; + rtb_ref_0[0] = rtb_ref[0]; + rtb_ref_0[1] = 0.0; + rtb_ref_0[2] = rtb_ref[1]; + rtb_ref_0[3] = rtb_ref[2]; + rtb_ref_0[4] = 0.0; + rtb_ref_0[5] = rtb_ref[3]; /* Saturate: '/2Nm ' incorporates: - * Inport: '/X' - * Inport: '/gain' - * Product: '/Product' - * SignalConversion: '/TmpSignal ConversionAtProductInport2' + * Sum: '/Sum1' * Sum: '/Sum7' - * Sum: '/Sum1' */ - rtb_ff_idx_0 = (tuning_GS1_B.Fcn1 + rtb_ff_idx_0) - (tuning_GS1_U.gain[9] * - tuning_GS1_U.X[6] + tuning_GS1_U.gain[10] * tuning_GS1_U.X[9]); + rtb_ff_idx_0 = (tuning_GS1_B.Fcn1 + rtb_ff_idx_0) - tuning_GS1_B.Product; if (rtb_ff_idx_0 > 2.0) { /* SignalConversion: '/TmpSignal ConversionAtControl AllocationInport1' */ rtb_ff_idx_0 = 2.0; @@ -1015,15 +962,10 @@ void tuning_GS1ModelClass::step() /* End of Saturate: '/2Nm ' */ /* Saturate: '/2Nm' incorporates: - * Inport: '/X' - * Inport: '/gain' - * Product: '/Product' - * SignalConversion: '/TmpSignal ConversionAtProductInport2' - * Sum: '/Sum1' + * Sum: '/Sum1' * Sum: '/Sum8' */ - rtb_ff_idx_1 = (tuning_GS1_B.Fcn + rtb_ff_idx_1) - (tuning_GS1_U.gain[12] * - tuning_GS1_U.X[7] + tuning_GS1_U.gain[13] * tuning_GS1_U.X[10]); + rtb_ff_idx_1 = (tuning_GS1_B.Fcn + rtb_ff_idx_1) - tuning_GS1_B.Product_d; if (rtb_ff_idx_1 > 2.0) { /* SignalConversion: '/TmpSignal ConversionAtControl AllocationInport1' */ rtb_ff_idx_1 = 2.0; @@ -1047,18 +989,19 @@ void tuning_GS1ModelClass::step() * Sum: '/Sum1' * Sum: '/Sum' */ - u0 = ((rtb_Kiy[i] * tuning_GS1_X.Integrator1_CSTATE_d - (((rtb_Kyphi[i + 6] - * tuning_GS1_U.X[4] + rtb_Kyphi[i] * rtb_d_y) + rtb_Kyphi[i + 12] - * tuning_GS1_U.X[6]) + rtb_Kyphi[i + 18] * tuning_GS1_U.X[9])) + - ((un[i] + tmp_1[i]) + tmp_2[i])) + + rtb_gamma_j = ((rtb_gamma_m[i] * tuning_GS1_X.Integrator1_CSTATE_n - + (((rtb_Kyphi[i + 6] * tuning_GS1_U.X[4] + rtb_Kyphi[i] * + rtb_d_y) + rtb_Kyphi[i + 12] * tuning_GS1_U.X[6]) + + rtb_Kyphi[i + 18] * tuning_GS1_U.X[9])) + ((tmp[i] + + rtb_Kipsi_0[i]) + rtb_ref_0[i])) + (tuning_GS1_ConstP.ControlAllocation_Gain[i + 6] * rtb_ff_idx_1 + tuning_GS1_ConstP.ControlAllocation_Gain[i] * rtb_ff_idx_0); - if (u0 > 8.54858) { + if (rtb_gamma_j > 8.54858) { tuning_GS1_B.u[i] = 8.54858; - } else if (u0 < 0.0) { + } else if (rtb_gamma_j < 0.0) { tuning_GS1_B.u[i] = 0.0; } else { - tuning_GS1_B.u[i] = u0; + tuning_GS1_B.u[i] = rtb_gamma_j; } /* End of Saturate: '/ ' */ @@ -1073,12 +1016,12 @@ void tuning_GS1ModelClass::step() /* ':1:6' T5 = T(5)*(1-LOE(5)); */ /* ':1:7' T6 = T(6)*(1-LOE(6)); */ /* ':1:8' T_f = [T1;T2;T3;T4;T5;T6]; */ - rtb_u[0] = (1.0 - rtb_LOE_out[0]) * tuning_GS1_B.u[0]; - rtb_u[1] = (1.0 - rtb_LOE_out[1]) * tuning_GS1_B.u[1]; - rtb_u[2] = (1.0 - rtb_LOE_out[2]) * tuning_GS1_B.u[2]; - rtb_u[3] = (1.0 - rtb_LOE_out[3]) * tuning_GS1_B.u[3]; - rtb_u[4] = (1.0 - rtb_LOE_out[4]) * tuning_GS1_B.u[4]; - rtb_u[5] = (1.0 - rtb_LOE_out[5]) * tuning_GS1_B.u[5]; + rtb_T_f[0] = (1.0 - rtb_LOE_out[0]) * tuning_GS1_B.u[0]; + rtb_T_f[1] = (1.0 - rtb_LOE_out[1]) * tuning_GS1_B.u[1]; + rtb_T_f[2] = (1.0 - rtb_LOE_out[2]) * tuning_GS1_B.u[2]; + rtb_T_f[3] = (1.0 - rtb_LOE_out[3]) * tuning_GS1_B.u[3]; + rtb_T_f[4] = (1.0 - rtb_LOE_out[4]) * tuning_GS1_B.u[4]; + rtb_T_f[5] = (1.0 - rtb_LOE_out[5]) * tuning_GS1_B.u[5]; /* Outport: '/motor_command' */ for (i = 0; i < 6; i++) { @@ -1088,8 +1031,8 @@ void tuning_GS1ModelClass::step() * Sqrt: '/Sqrt1' * Sum: '/Sum3' */ - rtb_ff_idx_0 = (std::sqrt(116978.4923343994 * rtb_u[i]) * 9.5493 - 1250.0) - * 0.022857142857142857; + rtb_ff_idx_0 = (std::sqrt(116978.4923343994 * rtb_T_f[i]) * 9.5493 - + 1250.0) * 0.022857142857142857; /* Saturate: '/Saturation' */ if (rtb_ff_idx_0 > 200.0) { @@ -1110,14 +1053,14 @@ void tuning_GS1ModelClass::step() * Inport: '/mode' * Inport: '/ref' */ - /* MATLAB Function 'Test_config_and_data/MATLAB Function': ':1' */ - /* ':1:2' ref = Y0; */ - /* ':1:3' switch test_mode */ + /* MATLAB Function 'Test_config_and_data/MATLAB Function': ':1' */ + /* ':1:2' ref = Y0; */ + /* ':1:3' switch test_mode */ switch ((int32_T)tuning_GS1_U.mode) { case 0: - /* ':1:4' case 0 % manual test */ + /* ':1:4' case 0 % manual test */ /* manual test */ - /* ':1:5' ref = ref_manual; */ + /* ':1:5' ref = ref_manual; */ rtb_ref[0] = tuning_GS1_U.ref[0]; rtb_ref[1] = tuning_GS1_U.ref[1]; rtb_ref[2] = tuning_GS1_U.ref[2]; @@ -1125,46 +1068,46 @@ void tuning_GS1ModelClass::step() break; case 1: - /* ':1:6' case 1 % waypoint */ + /* ':1:6' case 1 % waypoint */ /* waypoint */ - /* ':1:7' if t<=15 */ + /* ':1:7' if t<=15 */ if (rtb_Clock <= 15.0) { - /* ':1:8' ref = [Y0(1); Y0(2); 1; Y0(4)]; */ + /* ':1:8' ref = [Y0(1); Y0(2); 1; Y0(4)]; */ rtb_ref[0] = tuning_GS1_U.Y0[0]; rtb_ref[1] = tuning_GS1_U.Y0[1]; rtb_ref[2] = 1.0; rtb_ref[3] = tuning_GS1_U.Y0[3]; } else if (rtb_Clock <= 25.0) { - /* ':1:9' elseif t <= 25 */ - /* ':1:10' ref = [Y0(1)+1; Y0(2)-1; 1; Y0(4)]; */ + /* ':1:9' elseif t <= 25 */ + /* ':1:10' ref = [Y0(1)+1; Y0(2)-1; 1; Y0(4)]; */ rtb_ref[0] = tuning_GS1_U.Y0[0] + 1.0; rtb_ref[1] = tuning_GS1_U.Y0[1] - 1.0; rtb_ref[2] = 1.0; rtb_ref[3] = tuning_GS1_U.Y0[3]; } else if (rtb_Clock <= 35.0) { - /* ':1:11' elseif t <=35 */ - /* ':1:12' ref = [Y0(1)-1; Y0(2)+1; 1; Y0(4)]; */ + /* ':1:11' elseif t <=35 */ + /* ':1:12' ref = [Y0(1)-1; Y0(2)+1; 1; Y0(4)]; */ rtb_ref[0] = tuning_GS1_U.Y0[0] - 1.0; rtb_ref[1] = tuning_GS1_U.Y0[1] + 1.0; rtb_ref[2] = 1.0; rtb_ref[3] = tuning_GS1_U.Y0[3]; } else if (rtb_Clock <= 45.0) { - /* ':1:13' elseif t <=45 */ - /* ':1:14' ref = [Y0(1)-1; Y0(2)+1; 1; Y0(4)]; */ + /* ':1:13' elseif t <=45 */ + /* ':1:14' ref = [Y0(1)-1; Y0(2)+1; 1; Y0(4)]; */ rtb_ref[0] = tuning_GS1_U.Y0[0] - 1.0; rtb_ref[1] = tuning_GS1_U.Y0[1] + 1.0; rtb_ref[2] = 1.0; rtb_ref[3] = tuning_GS1_U.Y0[3]; } else if (rtb_Clock <= 55.0) { - /* ':1:15' elseif t <=55 */ - /* ':1:16' ref = [Y0(1); Y0(2); 1; Y0(4)]; */ + /* ':1:15' elseif t <=55 */ + /* ':1:16' ref = [Y0(1); Y0(2); 1; Y0(4)]; */ rtb_ref[0] = tuning_GS1_U.Y0[0]; rtb_ref[1] = tuning_GS1_U.Y0[1]; rtb_ref[2] = 1.0; rtb_ref[3] = tuning_GS1_U.Y0[3]; } else { - /* ':1:17' else */ - /* ':1:18' ref = Y0; */ + /* ':1:17' else */ + /* ':1:18' ref = Y0; */ rtb_ref[0] = tuning_GS1_U.Y0[0]; rtb_ref[1] = tuning_GS1_U.Y0[1]; rtb_ref[2] = tuning_GS1_U.Y0[2]; @@ -1185,32 +1128,32 @@ void tuning_GS1ModelClass::step() break; case 2: - /* ':1:31' case 2 % circular tracking */ + /* ':1:31' case 2 % circular tracking */ /* circular tracking */ - /* ':1:32' if t<=10 */ + /* ':1:32' if t<=10 */ if (rtb_Clock <= 10.0) { - /* ':1:33' ref = [Y0(1); Y0(2); 0.75; Y0(4)]; */ + /* ':1:33' ref = [Y0(1); Y0(2); 0.75; Y0(4)]; */ rtb_ref[0] = tuning_GS1_U.Y0[0]; rtb_ref[1] = tuning_GS1_U.Y0[1]; rtb_ref[2] = 0.75; rtb_ref[3] = tuning_GS1_U.Y0[3]; } else if (rtb_Clock <= 50.0) { - /* ':1:34' elseif t <= 50 */ - /* ':1:35' ref = [Y0(1)+cos(t); Y0(2)+sin(t); 0.75; Y0(4)]; */ + /* ':1:34' elseif t <= 50 */ + /* ':1:35' ref = [Y0(1)+cos(t); Y0(2)+sin(t); 0.75; Y0(4)]; */ rtb_ref[0] = tuning_GS1_U.Y0[0] + std::cos(rtb_Clock); rtb_ref[1] = tuning_GS1_U.Y0[1] + std::sin(rtb_Clock); rtb_ref[2] = 0.75; rtb_ref[3] = tuning_GS1_U.Y0[3]; } else if (rtb_Clock <= 60.0) { - /* ':1:36' elseif t <= 60 */ - /* ':1:37' ref = [Y0(1); Y0(2); 0.75; Y0(4)]; */ + /* ':1:36' elseif t <= 60 */ + /* ':1:37' ref = [Y0(1); Y0(2); 0.75; Y0(4)]; */ rtb_ref[0] = tuning_GS1_U.Y0[0]; rtb_ref[1] = tuning_GS1_U.Y0[1]; rtb_ref[2] = 0.75; rtb_ref[3] = tuning_GS1_U.Y0[3]; } else { - /* ':1:38' else */ - /* ':1:39' ref = Y0; */ + /* ':1:38' else */ + /* ':1:39' ref = Y0; */ rtb_ref[0] = tuning_GS1_U.Y0[0]; rtb_ref[1] = tuning_GS1_U.Y0[1]; rtb_ref[2] = tuning_GS1_U.Y0[2]; @@ -1221,8 +1164,8 @@ void tuning_GS1ModelClass::step() break; default: - /* ':1:42' otherwise */ - /* ':1:43' ref = Y0; */ + /* ':1:42' otherwise */ + /* ':1:43' ref = Y0; */ rtb_ref[0] = tuning_GS1_U.Y0[0]; rtb_ref[1] = tuning_GS1_U.Y0[1]; rtb_ref[2] = tuning_GS1_U.Y0[2]; @@ -1257,7 +1200,7 @@ void tuning_GS1ModelClass::step() tuning_GS1_Y.thrust_pre[i] = tuning_GS1_B.u[i]; /* Outport: '/thrust_after' */ - tuning_GS1_Y.thrust_after[i] = rtb_u[i]; + tuning_GS1_Y.thrust_after[i] = rtb_T_f[i]; } if (rtmIsMajorTimeStep((&tuning_GS1_M)) && @@ -1266,7 +1209,7 @@ void tuning_GS1ModelClass::step() tuning_GS1_Y.acc_Kalman[0] = rtb_Add_a[3]; /* Outport: '/M_Kalman' */ - tuning_GS1_Y.M_Kalman[0] = rtb_Sum1_cy[0]; + tuning_GS1_Y.M_Kalman[0] = rtb_Sum1[0]; /* Outport: '/vel_Kalman' */ tuning_GS1_Y.vel_Kalman[0] = rtb_Add_a[0]; @@ -1275,7 +1218,7 @@ void tuning_GS1ModelClass::step() tuning_GS1_Y.acc_Kalman[1] = rtb_Add_a[4]; /* Outport: '/M_Kalman' */ - tuning_GS1_Y.M_Kalman[1] = rtb_Sum1_cy[1]; + tuning_GS1_Y.M_Kalman[1] = rtb_Sum1[1]; /* Outport: '/vel_Kalman' */ tuning_GS1_Y.vel_Kalman[1] = rtb_Add_a[1]; @@ -1284,7 +1227,7 @@ void tuning_GS1ModelClass::step() tuning_GS1_Y.acc_Kalman[2] = rtb_Add_a[5]; /* Outport: '/M_Kalman' */ - tuning_GS1_Y.M_Kalman[2] = rtb_Sum1_cy[2]; + tuning_GS1_Y.M_Kalman[2] = rtb_Sum1[2]; /* Outport: '/vel_Kalman' */ tuning_GS1_Y.vel_Kalman[2] = rtb_Add_a[2]; @@ -1294,20 +1237,20 @@ void tuning_GS1ModelClass::step() * MATLAB Function: '/MATLAB Function' * MATLAB Function: '/MATLAB Function' * MATLAB Function: '/MATLAB Function' - * MATLAB Function: '/MATLAB Function' + * MATLAB Function: '/MATLAB Function' */ tuning_GS1_Y.gain_GS[0] = rtb_Kz[0]; - tuning_GS1_Y.gain_GS[3] = -2.274; - tuning_GS1_Y.gain_GS[6] = -1.7167 + rtb_Kxtheta[0]; - tuning_GS1_Y.gain_GS[9] = -0.7177 + rtb_Kyphi[0]; + tuning_GS1_Y.gain_GS[3] = rtb_Kpsi[0]; + tuning_GS1_Y.gain_GS[6] = -1.788 + rtb_Kxtheta[0]; + tuning_GS1_Y.gain_GS[9] = -0.8246 + rtb_Kyphi[0]; tuning_GS1_Y.gain_GS[1] = rtb_Kz[6]; - tuning_GS1_Y.gain_GS[4] = -1.4292; - tuning_GS1_Y.gain_GS[7] = -1.0582 + rtb_Kxtheta[4]; - tuning_GS1_Y.gain_GS[10] = -0.4473 + rtb_Kyphi[6]; - tuning_GS1_Y.gain_GS[2] = rtb_gamma_m[0]; - tuning_GS1_Y.gain_GS[5] = -1.4438; - tuning_GS1_Y.gain_GS[8] = -1.2273 + rtb_Kix[0]; - tuning_GS1_Y.gain_GS[11] = -0.5164 + rtb_Kiy[0]; + tuning_GS1_Y.gain_GS[4] = rtb_Kpsi[6]; + tuning_GS1_Y.gain_GS[7] = -1.367 + rtb_Kxtheta[4]; + tuning_GS1_Y.gain_GS[10] = -0.5442 + rtb_Kyphi[6]; + tuning_GS1_Y.gain_GS[2] = rtb_Kiz[0]; + tuning_GS1_Y.gain_GS[5] = rtb_Kipsi[0]; + tuning_GS1_Y.gain_GS[8] = -1.036 + rtb_Kix[0]; + tuning_GS1_Y.gain_GS[11] = -0.577 + rtb_gamma_m[0]; /* Sum: '/Sum' incorporates: * Inport: '/Y0' @@ -1329,7 +1272,7 @@ void tuning_GS1ModelClass::step() /* End of Saturate: '/yaw' */ /* Sum: '/Sum3' */ - tuning_GS1_B.Sum3_f = rtb_Clock - rtb_d_psi; + tuning_GS1_B.Sum3_d = rtb_Clock - rtb_d_psi; /* Saturate: '/x' */ if (rtb_ref[0] > 2.0) { @@ -1343,7 +1286,7 @@ void tuning_GS1ModelClass::step() /* End of Saturate: '/x' */ /* Sum: '/Sum3' */ - tuning_GS1_B.Sum3_a = rtb_Clock - rtb_d_x; + tuning_GS1_B.Sum3_n = rtb_Clock - rtb_d_x; /* Saturate: '/y' */ if (rtb_ref[1] > 2.0) { @@ -1357,9 +1300,9 @@ void tuning_GS1ModelClass::step() /* End of Saturate: '/y' */ /* Sum: '/Sum3' */ - tuning_GS1_B.Sum3_i = rtb_Clock - rtb_d_y; + tuning_GS1_B.Sum3_h = rtb_Clock - rtb_d_y; - /* Saturate: '/z' */ + /* Saturate: '/z' */ if (rtb_ref[2] > 1.75) { rtb_Clock = 1.75; } else if (rtb_ref[2] < 0.0) { @@ -1368,14 +1311,14 @@ void tuning_GS1ModelClass::step() rtb_Clock = rtb_ref[2]; } - /* End of Saturate: '/z' */ + /* End of Saturate: '/z' */ - /* Sum: '/Sum3' */ - tuning_GS1_B.Sum3_d = rtb_Clock - rtb_d_z; + /* Sum: '/Sum3' */ + tuning_GS1_B.Sum3_l = rtb_Clock - rtb_d_z; if (rtmIsMajorTimeStep((&tuning_GS1_M)) && (&tuning_GS1_M)->Timing.TaskCounters.TID[1] == 0) { - /* Outputs for Enabled SubSystem: '/MeasurementUpdate' incorporates: - * EnablePort: '/Enable' + /* Outputs for Enabled SubSystem: '/MeasurementUpdate' incorporates: + * EnablePort: '/Enable' */ if (rtmIsMajorTimeStep((&tuning_GS1_M)) && (!tuning_GS1_DW.MeasurementUpdate_MODE)) { @@ -1384,9 +1327,9 @@ void tuning_GS1ModelClass::step() if (tuning_GS1_DW.MeasurementUpdate_MODE) { for (i = 0; i < 3; i++) { - /* Product: '/C[k]*xhat[k|k-1]' incorporates: + /* Product: '/C[k]*xhat[k|k-1]' incorporates: * Constant: '/C' - * Sum: '/Add1' + * Sum: '/Add1' */ rtb_Add_f[i] = 0.0; for (i_0 = 0; i_0 < 6; i_0++) { @@ -1394,18 +1337,18 @@ void tuning_GS1ModelClass::step() rtb_MemoryX[i_0]; } - /* End of Product: '/C[k]*xhat[k|k-1]' */ + /* End of Product: '/C[k]*xhat[k|k-1]' */ - /* Sum: '/Sum' incorporates: + /* Sum: '/Sum' incorporates: * Inport: '/X' - * Product: '/Product3' - * Sum: '/Add1' + * Product: '/Product3' + * Sum: '/Add1' */ rtb_Add_f_0[i] = tuning_GS1_U.X[9 + i] - rtb_Add_f[i]; } - /* Product: '/Product3' incorporates: - * Constant: '/KalmanGainL' + /* Product: '/Product3' incorporates: + * Constant: '/KalmanGainL' */ for (i = 0; i < 6; i++) { tuning_GS1_B.Product3[i] = 0.0; @@ -1418,25 +1361,25 @@ void tuning_GS1ModelClass::step() } } - /* End of Outputs for SubSystem: '/MeasurementUpdate' */ + /* End of Outputs for SubSystem: '/MeasurementUpdate' */ for (i = 0; i < 6; i++) { - /* Product: '/A[k]*xhat[k|k-1]' incorporates: + /* Product: '/A[k]*xhat[k|k-1]' incorporates: * Constant: '/A' - * Sum: '/Add' + * Sum: '/Add' */ - un[i] = 0.0; + tmp[i] = 0.0; for (i_0 = 0; i_0 < 6; i_0++) { - un[i] += tuning_GS1_ConstP.A_Value[6 * i_0 + i] * rtb_MemoryX[i_0]; + tmp[i] += tuning_GS1_ConstP.A_Value[6 * i_0 + i] * rtb_MemoryX[i_0]; } - /* End of Product: '/A[k]*xhat[k|k-1]' */ + /* End of Product: '/A[k]*xhat[k|k-1]' */ - /* Sum: '/Add' */ - rtb_Add[i] = un[i] + tuning_GS1_B.Product3[i]; + /* Sum: '/Add' */ + rtb_Add[i] = tmp[i] + tuning_GS1_B.Product3[i]; } } - /* Saturate: '/x' */ + /* Saturate: '/x' */ if (rtb_ref[0] > 2.0) { rtb_Clock = 2.0; } else if (rtb_ref[0] < -2.0) { @@ -1445,12 +1388,12 @@ void tuning_GS1ModelClass::step() rtb_Clock = rtb_ref[0]; } - /* End of Saturate: '/x' */ + /* End of Saturate: '/x' */ - /* Sum: '/Sum1' */ + /* Sum: '/Sum1' */ tuning_GS1_B.Sum1 = rtb_Clock - rtb_d_x; - /* Saturate: '/y' */ + /* Saturate: '/y' */ if (rtb_ref[1] > 2.0) { rtb_Clock = 2.0; } else if (rtb_ref[1] < -2.0) { @@ -1459,9 +1402,9 @@ void tuning_GS1ModelClass::step() rtb_Clock = rtb_ref[1]; } - /* End of Saturate: '/y' */ + /* End of Saturate: '/y' */ - /* Sum: '/Sum4' */ + /* Sum: '/Sum4' */ tuning_GS1_B.Sum4 = rtb_Clock - rtb_d_y; } @@ -1526,22 +1469,22 @@ void tuning_GS1ModelClass::tuning_GS1_derivatives() XDot_tuning_GS1_T *_rtXdot; _rtXdot = ((XDot_tuning_GS1_T *) (&tuning_GS1_M)->derivs); - /* Derivatives for Integrator: '/Integrator1' */ - _rtXdot->Integrator1_CSTATE = tuning_GS1_B.Sum3_d; + /* Derivatives for Integrator: '/Integrator1' */ + _rtXdot->Integrator1_CSTATE = tuning_GS1_B.Sum3_l; /* Derivatives for Integrator: '/Integrator1' */ - _rtXdot->Integrator1_CSTATE_o = tuning_GS1_B.Sum3_f; + _rtXdot->Integrator1_CSTATE_d = tuning_GS1_B.Sum3_d; /* Derivatives for Integrator: '/Integrator1' */ - _rtXdot->Integrator1_CSTATE_b = tuning_GS1_B.Sum3_a; + _rtXdot->Integrator1_CSTATE_p = tuning_GS1_B.Sum3_n; /* Derivatives for Integrator: '/Integrator1' */ - _rtXdot->Integrator1_CSTATE_d = tuning_GS1_B.Sum3_i; + _rtXdot->Integrator1_CSTATE_n = tuning_GS1_B.Sum3_h; - /* Derivatives for Integrator: '/Integrator1' */ - _rtXdot->Integrator1_CSTATE_p = tuning_GS1_B.Sum1; + /* Derivatives for Integrator: '/Integrator1' */ + _rtXdot->Integrator1_CSTATE_pc = tuning_GS1_B.Sum1; - /* Derivatives for Integrator: '/Integrator' */ + /* Derivatives for Integrator: '/Integrator' */ _rtXdot->Integrator_CSTATE = tuning_GS1_B.Sum4; } @@ -1617,41 +1560,41 @@ void tuning_GS1ModelClass::initialize() /* InitializeConditions for Delay: '/MemoryX' */ tuning_GS1_DW.icLoad = 1U; - /* InitializeConditions for Integrator: '/Integrator1' */ + /* InitializeConditions for Integrator: '/Integrator1' */ tuning_GS1_X.Integrator1_CSTATE = 0.0; /* InitializeConditions for Integrator: '/Integrator1' */ - tuning_GS1_X.Integrator1_CSTATE_o = 0.0; + tuning_GS1_X.Integrator1_CSTATE_d = 0.0; /* InitializeConditions for Integrator: '/Integrator1' */ - tuning_GS1_X.Integrator1_CSTATE_b = 0.0; + tuning_GS1_X.Integrator1_CSTATE_p = 0.0; /* InitializeConditions for Integrator: '/Integrator1' */ - tuning_GS1_X.Integrator1_CSTATE_d = 0.0; + tuning_GS1_X.Integrator1_CSTATE_n = 0.0; - /* InitializeConditions for Integrator: '/Integrator1' */ - tuning_GS1_X.Integrator1_CSTATE_p = 0.0; + /* InitializeConditions for Integrator: '/Integrator1' */ + tuning_GS1_X.Integrator1_CSTATE_pc = 0.0; - /* InitializeConditions for Integrator: '/Integrator' */ + /* InitializeConditions for Integrator: '/Integrator' */ tuning_GS1_X.Integrator_CSTATE = 0.0; - /* SystemInitialize for Enabled SubSystem: '/MeasurementUpdate' */ + /* SystemInitialize for Enabled SubSystem: '/MeasurementUpdate' */ /* SystemInitialize for Atomic SubSystem: '/UseCurrentEstimator' */ - /* SystemInitialize for Enabled SubSystem: '/Enabled Subsystem' */ + /* SystemInitialize for Enabled SubSystem: '/Enabled Subsystem' */ for (i = 0; i < 6; i++) { /* InitializeConditions for Memory: '/Memory' */ tuning_GS1_DW.Memory_PreviousInput[i] = 2.5179000000000005; - /* SystemInitialize for Outport: '/deltax' */ + /* SystemInitialize for Outport: '/deltax' */ tuning_GS1_B.Product2[i] = 0.0; - /* SystemInitialize for Outport: '/L*(y[k]-yhat[k|k-1])' */ + /* SystemInitialize for Outport: '/L*(y[k]-yhat[k|k-1])' */ tuning_GS1_B.Product3[i] = 0.0; } - /* End of SystemInitialize for SubSystem: '/Enabled Subsystem' */ + /* End of SystemInitialize for SubSystem: '/Enabled Subsystem' */ /* End of SystemInitialize for SubSystem: '/UseCurrentEstimator' */ - /* End of SystemInitialize for SubSystem: '/MeasurementUpdate' */ + /* End of SystemInitialize for SubSystem: '/MeasurementUpdate' */ } } diff --git a/src/tuning_GS1_grt_rtw/tuning_GS1.h b/src/tuning_GS1_grt_rtw/tuning_GS1.h index 630073a..0fc4f34 100644 --- a/src/tuning_GS1_grt_rtw/tuning_GS1.h +++ b/src/tuning_GS1_grt_rtw/tuning_GS1.h @@ -7,9 +7,9 @@ * * Code generation for model "tuning_GS1". * - * Model version : 1.2101 + * Model version : 1.2189 * Simulink Coder version : 8.12 (R2017a) 16-Feb-2017 - * C++ source code generated on : Mon Oct 22 19:08:51 2018 + * C++ source code generated on : Thu Mar 7 15:36:47 2019 * * Target selection: grt.tlc * Note: GRT includes extra infrastructure and instrumentation for prototyping @@ -150,20 +150,22 @@ /* Block signals (auto storage) */ typedef struct { real_T LOE_FDD[6]; /* '/Vector Concatenate' */ - real_T Sum2; /* '/Sum2' */ - real_T Sum3; /* '/Sum3' */ + real_T Sum2; /* '/Sum2' */ + real_T Sum3; /* '/Sum3' */ real_T T_outer[3]; /* '/T_outer' */ real_T Fcn1; /* '/Fcn1' */ + real_T Product; /* '/Product' */ real_T Fcn; /* '/Fcn' */ + real_T Product_d; /* '/Product' */ real_T u[6]; /* '/ ' */ - real_T Sum3_f; /* '/Sum3' */ - real_T Sum3_a; /* '/Sum3' */ - real_T Sum3_i; /* '/Sum3' */ - real_T Sum3_d; /* '/Sum3' */ - real_T Sum1; /* '/Sum1' */ - real_T Sum4; /* '/Sum4' */ - real_T Product2[6]; /* '/Product2' */ - real_T Product3[6]; /* '/Product3' */ + real_T Sum3_d; /* '/Sum3' */ + real_T Sum3_n; /* '/Sum3' */ + real_T Sum3_h; /* '/Sum3' */ + real_T Sum3_l; /* '/Sum3' */ + real_T Sum1; /* '/Sum1' */ + real_T Sum4; /* '/Sum4' */ + real_T Product2[6]; /* '/Product2' */ + real_T Product3[6]; /* '/Product3' */ } B_tuning_GS1_T; /* Block states (auto storage) for system '' */ @@ -171,38 +173,38 @@ typedef struct { real_T MemoryX_DSTATE[6]; /* '/MemoryX' */ real_T Memory_PreviousInput[6]; /* '/Memory' */ uint8_T icLoad; /* '/MemoryX' */ - boolean_T MeasurementUpdate_MODE; /* '/MeasurementUpdate' */ - boolean_T EnabledSubsystem_MODE; /* '/Enabled Subsystem' */ + boolean_T MeasurementUpdate_MODE; /* '/MeasurementUpdate' */ + boolean_T EnabledSubsystem_MODE; /* '/Enabled Subsystem' */ } DW_tuning_GS1_T; /* Continuous states (auto storage) */ typedef struct { - real_T Integrator1_CSTATE; /* '/Integrator1' */ - real_T Integrator1_CSTATE_o; /* '/Integrator1' */ - real_T Integrator1_CSTATE_b; /* '/Integrator1' */ - real_T Integrator1_CSTATE_d; /* '/Integrator1' */ - real_T Integrator1_CSTATE_p; /* '/Integrator1' */ - real_T Integrator_CSTATE; /* '/Integrator' */ + real_T Integrator1_CSTATE; /* '/Integrator1' */ + real_T Integrator1_CSTATE_d; /* '/Integrator1' */ + real_T Integrator1_CSTATE_p; /* '/Integrator1' */ + real_T Integrator1_CSTATE_n; /* '/Integrator1' */ + real_T Integrator1_CSTATE_pc; /* '/Integrator1' */ + real_T Integrator_CSTATE; /* '/Integrator' */ } X_tuning_GS1_T; /* State derivatives (auto storage) */ typedef struct { - real_T Integrator1_CSTATE; /* '/Integrator1' */ - real_T Integrator1_CSTATE_o; /* '/Integrator1' */ - real_T Integrator1_CSTATE_b; /* '/Integrator1' */ - real_T Integrator1_CSTATE_d; /* '/Integrator1' */ - real_T Integrator1_CSTATE_p; /* '/Integrator1' */ - real_T Integrator_CSTATE; /* '/Integrator' */ + real_T Integrator1_CSTATE; /* '/Integrator1' */ + real_T Integrator1_CSTATE_d; /* '/Integrator1' */ + real_T Integrator1_CSTATE_p; /* '/Integrator1' */ + real_T Integrator1_CSTATE_n; /* '/Integrator1' */ + real_T Integrator1_CSTATE_pc; /* '/Integrator1' */ + real_T Integrator_CSTATE; /* '/Integrator' */ } XDot_tuning_GS1_T; /* State disabled */ typedef struct { - boolean_T Integrator1_CSTATE; /* '/Integrator1' */ - boolean_T Integrator1_CSTATE_o; /* '/Integrator1' */ - boolean_T Integrator1_CSTATE_b; /* '/Integrator1' */ - boolean_T Integrator1_CSTATE_d; /* '/Integrator1' */ - boolean_T Integrator1_CSTATE_p; /* '/Integrator1' */ - boolean_T Integrator_CSTATE; /* '/Integrator' */ + boolean_T Integrator1_CSTATE; /* '/Integrator1' */ + boolean_T Integrator1_CSTATE_d; /* '/Integrator1' */ + boolean_T Integrator1_CSTATE_p; /* '/Integrator1' */ + boolean_T Integrator1_CSTATE_n; /* '/Integrator1' */ + boolean_T Integrator1_CSTATE_pc; /* '/Integrator1' */ + boolean_T Integrator_CSTATE; /* '/Integrator' */ } XDis_tuning_GS1_T; #ifndef ODE4_INTG @@ -229,7 +231,7 @@ typedef struct { real_T Constant1_Value[9]; /* Expression: pInitialization.M - * Referenced by: '/KalmanGainM' + * Referenced by: '/KalmanGainM' */ real_T KalmanGainM_Value[18]; @@ -238,7 +240,7 @@ typedef struct { */ real_T C_Value[18]; - /* Expression: B_ENU_inv(:,2:3) + /* Expression: B_CA_ENU_inv(:,2:3) * Referenced by: '/Control Allocation' */ real_T ControlAllocation_Gain[12]; @@ -249,7 +251,7 @@ typedef struct { real_T A_Value[36]; /* Expression: pInitialization.L - * Referenced by: '/KalmanGainL' + * Referenced by: '/KalmanGainL' */ real_T KalmanGainL_Value[18]; } ConstP_tuning_GS1_T; @@ -262,8 +264,7 @@ typedef struct { real_T ref[4]; /* '/ref' */ real_T LOE_t[6]; /* '/LOE_t' */ real_T LOE_a[6]; /* '/LOE_a' */ - real_T gain[18]; /* '/gain' */ - real_T LOE_calcul[6]; /* '/LOE_calcul' */ + real_T LOE_cal[6]; /* '/LOE_cal' */ real_T LOE_mode; /* '/LOE_mode' */ } ExtU_tuning_GS1_T; @@ -395,20 +396,24 @@ class tuning_GS1ModelClass { /*- * These blocks were eliminated from the model due to optimizations: * - * Block '/ConstantP' : Unused code path elimination - * Block '/CovarianceZ' : Unused code path elimination + * Block '/ConstantP' : Unused code path elimination + * Block '/CovarianceZ' : Unused code path elimination + * Block '/Data Type Duplicate' : Unused code path elimination + * Block '/Data Type Duplicate' : Unused code path elimination + * Block '/Conversion' : Unused code path elimination * Block '/Data Type Duplicate' : Unused code path elimination + * Block '/Conversion' : Unused code path elimination * Block '/Data Type Duplicate' : Unused code path elimination - * Block '/Conversion' : Unused code path elimination - * Block '/Data Type Duplicate' : Unused code path elimination - * Block '/Conversion' : Unused code path elimination - * Block '/Data Type Duplicate' : Unused code path elimination - * Block '/Add1' : Unused code path elimination - * Block '/Product' : Unused code path elimination - * Block '/Product1' : Unused code path elimination + * Block '/Add1' : Unused code path elimination + * Block '/Product' : Unused code path elimination + * Block '/Product1' : Unused code path elimination + * Block '/Data Type Duplicate' : Unused code path elimination + * Block '/Data Type Duplicate' : Unused code path elimination * Block '/Data Type Duplicate' : Unused code path elimination * Block '/Data Type Duplicate' : Unused code path elimination + * Block '/Conversion' : Unused code path elimination * Block '/Data Type Duplicate' : Unused code path elimination + * Block '/Conversion' : Unused code path elimination * Block '/Data Type Duplicate' : Unused code path elimination * Block '/Conversion' : Unused code path elimination * Block '/Data Type Duplicate' : Unused code path elimination @@ -422,13 +427,9 @@ class tuning_GS1ModelClass { * Block '/Data Type Duplicate' : Unused code path elimination * Block '/Conversion' : Unused code path elimination * Block '/Data Type Duplicate' : Unused code path elimination - * Block '/Conversion' : Unused code path elimination * Block '/Data Type Duplicate' : Unused code path elimination - * Block '/Conversion' : Unused code path elimination * Block '/Data Type Duplicate' : Unused code path elimination * Block '/Data Type Duplicate' : Unused code path elimination - * Block '/Data Type Duplicate' : Unused code path elimination - * Block '/Data Type Duplicate' : Unused code path elimination * Block '/G' : Unused code path elimination * Block '/H' : Unused code path elimination * Block '/ManualSwitchPZ' : Unused code path elimination @@ -436,32 +437,32 @@ class tuning_GS1ModelClass { * Block '/P0' : Unused code path elimination * Block '/Q' : Unused code path elimination * Block '/R' : Unused code path elimination - * Block '/Constant' : Unused code path elimination + * Block '/Constant' : Unused code path elimination * Block '/Reset' : Unused code path elimination * Block '/Reshapeyhat' : Unused code path elimination - * Block '/CheckSignalProperties' : Unused code path elimination + * Block '/CheckSignalProperties' : Unused code path elimination * Block '/ ' : Eliminated since input and output rates are identical * Block '/ ' : Eliminated since input and output rates are identical * Block '/ ' : Eliminated since input and output rates are identical - * Block '/Conversion' : Eliminate redundant data type conversion - * Block '/Conversion' : Eliminate redundant data type conversion + * Block '/Conversion' : Eliminate redundant data type conversion + * Block '/Conversion' : Eliminate redundant data type conversion + * Block '/Conversion' : Eliminate redundant data type conversion + * Block '/Conversion' : Eliminate redundant data type conversion * Block '/Conversion' : Eliminate redundant data type conversion * Block '/Conversion' : Eliminate redundant data type conversion - * Block '/Conversion' : Eliminate redundant data type conversion - * Block '/Conversion' : Eliminate redundant data type conversion * Block '/DataTypeConversionEnable' : Eliminate redundant data type conversion + * Block '/Conversion' : Eliminate redundant data type conversion + * Block '/Conversion' : Eliminate redundant data type conversion * Block '/Conversion' : Eliminate redundant data type conversion - * Block '/Conversion' : Eliminate redundant data type conversion - * Block '/Conversion' : Eliminate redundant data type conversion - * Block '/Reshape' : Reshape block reduction + * Block '/Reshape' : Reshape block reduction * Block '/ReshapeX0' : Reshape block reduction * Block '/Reshapeu' : Reshape block reduction * Block '/Reshapexhat' : Reshape block reduction * Block '/Reshapey' : Reshape block reduction + * Block '/Reshape' : Reshape block reduction + * Block '/Reshape1' : Reshape block reduction + * Block '/Reshape' : Reshape block reduction * Block '/Reshape' : Reshape block reduction - * Block '/Reshape1' : Reshape block reduction - * Block '/Reshape' : Reshape block reduction - * Block '/Reshape' : Reshape block reduction * Block '/Rate Transition' : Eliminated since input and output rates are identical */ @@ -495,63 +496,61 @@ class tuning_GS1ModelClass { * '' : 'tuning_GS1/GS_controller/psi_GS_controller' * '' : 'tuning_GS1/GS_controller/x_GS_controller' * '' : 'tuning_GS1/GS_controller/y_GS_controller' - * '' : 'tuning_GS1/GS_controller/y_GS_controller1' - * '' : 'tuning_GS1/GS_controller/z_GS_controller' - * '' : 'tuning_GS1/GS_controller/psi_GS_controller/MATLAB Function' - * '' : 'tuning_GS1/GS_controller/x_GS_controller/MATLAB Function' - * '' : 'tuning_GS1/GS_controller/y_GS_controller/MATLAB Function' - * '' : 'tuning_GS1/GS_controller/y_GS_controller1/MATLAB Function' - * '' : 'tuning_GS1/GS_controller/z_GS_controller/MATLAB Function' - * '' : 'tuning_GS1/Kalman Filter/CalculatePL' - * '' : 'tuning_GS1/Kalman Filter/CalculateYhat' - * '' : 'tuning_GS1/Kalman Filter/DataTypeConversionA' - * '' : 'tuning_GS1/Kalman Filter/DataTypeConversionB' - * '' : 'tuning_GS1/Kalman Filter/DataTypeConversionC' - * '' : 'tuning_GS1/Kalman Filter/DataTypeConversionD' - * '' : 'tuning_GS1/Kalman Filter/DataTypeConversionG' - * '' : 'tuning_GS1/Kalman Filter/DataTypeConversionH' - * '' : 'tuning_GS1/Kalman Filter/DataTypeConversionN' - * '' : 'tuning_GS1/Kalman Filter/DataTypeConversionP' - * '' : 'tuning_GS1/Kalman Filter/DataTypeConversionP0' - * '' : 'tuning_GS1/Kalman Filter/DataTypeConversionQ' - * '' : 'tuning_GS1/Kalman Filter/DataTypeConversionR' - * '' : 'tuning_GS1/Kalman Filter/DataTypeConversionReset' - * '' : 'tuning_GS1/Kalman Filter/DataTypeConversionX' - * '' : 'tuning_GS1/Kalman Filter/DataTypeConversionX0' - * '' : 'tuning_GS1/Kalman Filter/DataTypeConversionu' - * '' : 'tuning_GS1/Kalman Filter/MemoryP' - * '' : 'tuning_GS1/Kalman Filter/Observer' - * '' : 'tuning_GS1/Kalman Filter/ReducedQRN' - * '' : 'tuning_GS1/Kalman Filter/ScalarExpansionP0' - * '' : 'tuning_GS1/Kalman Filter/ScalarExpansionQ' - * '' : 'tuning_GS1/Kalman Filter/ScalarExpansionR' - * '' : 'tuning_GS1/Kalman Filter/UseCurrentEstimator' - * '' : 'tuning_GS1/Kalman Filter/checkA' - * '' : 'tuning_GS1/Kalman Filter/checkB' - * '' : 'tuning_GS1/Kalman Filter/checkC' - * '' : 'tuning_GS1/Kalman Filter/checkD' - * '' : 'tuning_GS1/Kalman Filter/checkEnable' - * '' : 'tuning_GS1/Kalman Filter/checkG' - * '' : 'tuning_GS1/Kalman Filter/checkH' - * '' : 'tuning_GS1/Kalman Filter/checkN' - * '' : 'tuning_GS1/Kalman Filter/checkP0' - * '' : 'tuning_GS1/Kalman Filter/checkQ' - * '' : 'tuning_GS1/Kalman Filter/checkR' - * '' : 'tuning_GS1/Kalman Filter/checkReset' - * '' : 'tuning_GS1/Kalman Filter/checkX0' - * '' : 'tuning_GS1/Kalman Filter/checku' - * '' : 'tuning_GS1/Kalman Filter/checky' - * '' : 'tuning_GS1/Kalman Filter/CalculatePL/DataTypeConversionL' - * '' : 'tuning_GS1/Kalman Filter/CalculatePL/DataTypeConversionM' - * '' : 'tuning_GS1/Kalman Filter/CalculatePL/DataTypeConversionP' - * '' : 'tuning_GS1/Kalman Filter/CalculatePL/DataTypeConversionZ' - * '' : 'tuning_GS1/Kalman Filter/Observer/MeasurementUpdate' - * '' : 'tuning_GS1/Kalman Filter/UseCurrentEstimator/Enabled Subsystem' - * '' : 'tuning_GS1/Nominal_xy_Controller/XY State Feedback' - * '' : 'tuning_GS1/Nominal_xy_Controller/pitch_controller_SF' - * '' : 'tuning_GS1/Nominal_xy_Controller/roll_controller_SF' - * '' : 'tuning_GS1/Test_config_and_data/FFW' - * '' : 'tuning_GS1/Test_config_and_data/LOE_' - * '' : 'tuning_GS1/Test_config_and_data/MATLAB Function' + * '' : 'tuning_GS1/GS_controller/z_GS_controller' + * '' : 'tuning_GS1/GS_controller/psi_GS_controller/MATLAB Function' + * '' : 'tuning_GS1/GS_controller/x_GS_controller/MATLAB Function' + * '' : 'tuning_GS1/GS_controller/y_GS_controller/MATLAB Function' + * '' : 'tuning_GS1/GS_controller/z_GS_controller/MATLAB Function' + * '' : 'tuning_GS1/Kalman Filter/CalculatePL' + * '' : 'tuning_GS1/Kalman Filter/CalculateYhat' + * '' : 'tuning_GS1/Kalman Filter/DataTypeConversionA' + * '' : 'tuning_GS1/Kalman Filter/DataTypeConversionB' + * '' : 'tuning_GS1/Kalman Filter/DataTypeConversionC' + * '' : 'tuning_GS1/Kalman Filter/DataTypeConversionD' + * '' : 'tuning_GS1/Kalman Filter/DataTypeConversionG' + * '' : 'tuning_GS1/Kalman Filter/DataTypeConversionH' + * '' : 'tuning_GS1/Kalman Filter/DataTypeConversionN' + * '' : 'tuning_GS1/Kalman Filter/DataTypeConversionP' + * '' : 'tuning_GS1/Kalman Filter/DataTypeConversionP0' + * '' : 'tuning_GS1/Kalman Filter/DataTypeConversionQ' + * '' : 'tuning_GS1/Kalman Filter/DataTypeConversionR' + * '' : 'tuning_GS1/Kalman Filter/DataTypeConversionReset' + * '' : 'tuning_GS1/Kalman Filter/DataTypeConversionX' + * '' : 'tuning_GS1/Kalman Filter/DataTypeConversionX0' + * '' : 'tuning_GS1/Kalman Filter/DataTypeConversionu' + * '' : 'tuning_GS1/Kalman Filter/MemoryP' + * '' : 'tuning_GS1/Kalman Filter/Observer' + * '' : 'tuning_GS1/Kalman Filter/ReducedQRN' + * '' : 'tuning_GS1/Kalman Filter/ScalarExpansionP0' + * '' : 'tuning_GS1/Kalman Filter/ScalarExpansionQ' + * '' : 'tuning_GS1/Kalman Filter/ScalarExpansionR' + * '' : 'tuning_GS1/Kalman Filter/UseCurrentEstimator' + * '' : 'tuning_GS1/Kalman Filter/checkA' + * '' : 'tuning_GS1/Kalman Filter/checkB' + * '' : 'tuning_GS1/Kalman Filter/checkC' + * '' : 'tuning_GS1/Kalman Filter/checkD' + * '' : 'tuning_GS1/Kalman Filter/checkEnable' + * '' : 'tuning_GS1/Kalman Filter/checkG' + * '' : 'tuning_GS1/Kalman Filter/checkH' + * '' : 'tuning_GS1/Kalman Filter/checkN' + * '' : 'tuning_GS1/Kalman Filter/checkP0' + * '' : 'tuning_GS1/Kalman Filter/checkQ' + * '' : 'tuning_GS1/Kalman Filter/checkR' + * '' : 'tuning_GS1/Kalman Filter/checkReset' + * '' : 'tuning_GS1/Kalman Filter/checkX0' + * '' : 'tuning_GS1/Kalman Filter/checku' + * '' : 'tuning_GS1/Kalman Filter/checky' + * '' : 'tuning_GS1/Kalman Filter/CalculatePL/DataTypeConversionL' + * '' : 'tuning_GS1/Kalman Filter/CalculatePL/DataTypeConversionM' + * '' : 'tuning_GS1/Kalman Filter/CalculatePL/DataTypeConversionP' + * '' : 'tuning_GS1/Kalman Filter/CalculatePL/DataTypeConversionZ' + * '' : 'tuning_GS1/Kalman Filter/Observer/MeasurementUpdate' + * '' : 'tuning_GS1/Kalman Filter/UseCurrentEstimator/Enabled Subsystem' + * '' : 'tuning_GS1/Nominal_xy_Controller/XY State Feedback' + * '' : 'tuning_GS1/Nominal_xy_Controller/pitch_controller_SF' + * '' : 'tuning_GS1/Nominal_xy_Controller/roll_controller_SF' + * '' : 'tuning_GS1/Test_config_and_data/FFW' + * '' : 'tuning_GS1/Test_config_and_data/LOE_' + * '' : 'tuning_GS1/Test_config_and_data/MATLAB Function' */ #endif /* RTW_HEADER_tuning_GS1_h_ */ diff --git a/src/tuning_GS1_grt_rtw/tuning_GS1_data.cpp b/src/tuning_GS1_grt_rtw/tuning_GS1_data.cpp index 5d1ee5c..a103167 100644 --- a/src/tuning_GS1_grt_rtw/tuning_GS1_data.cpp +++ b/src/tuning_GS1_grt_rtw/tuning_GS1_data.cpp @@ -7,9 +7,9 @@ * * Code generation for model "tuning_GS1". * - * Model version : 1.2101 + * Model version : 1.2189 * Simulink Coder version : 8.12 (R2017a) 16-Feb-2017 - * C++ source code generated on : Mon Oct 22 19:08:51 2018 + * C++ source code generated on : Thu Mar 7 15:36:47 2019 * * Target selection: grt.tlc * Note: GRT includes extra infrastructure and instrumentation for prototyping @@ -34,7 +34,7 @@ const ConstP_tuning_GS1_T tuning_GS1_ConstP = { { 0.0347563, 0.0, 0.0, 0.0, 0.0458929, 0.0, 0.0, 0.0, 0.0977 }, /* Expression: pInitialization.M - * Referenced by: '/KalmanGainM' + * Referenced by: '/KalmanGainM' */ { 0.0013828503617152289, 0.0, 0.0, 0.000956799230965245, 0.0, 0.0, 0.0, 0.001382850361721501, 0.0, 0.0, 0.00095679923096881356, 0.0, 0.0, 0.0, @@ -46,7 +46,7 @@ const ConstP_tuning_GS1_T tuning_GS1_ConstP = { { 1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 }, - /* Expression: B_ENU_inv(:,2:3) + /* Expression: B_CA_ENU_inv(:,2:3) * Referenced by: '/Control Allocation' */ { 0.77519379844962388, 1.5503875968992147, 0.77519379844962344, @@ -62,7 +62,7 @@ const ConstP_tuning_GS1_T tuning_GS1_ConstP = { 0.0, 0.0, 0.0, 0.001, 0.0, 0.0, 1.0 }, /* Expression: pInitialization.L - * Referenced by: '/KalmanGainL' + * Referenced by: '/KalmanGainL' */ { 0.001383807160946194, 0.0, 0.0, 0.000956799230965245, 0.0, 0.0, 0.0, 0.0013838071609524698, 0.0, 0.0, 0.00095679923096881356, 0.0, 0.0, 0.0, diff --git a/src/tuning_GS1_grt_rtw/tuning_GS1_private.h b/src/tuning_GS1_grt_rtw/tuning_GS1_private.h index 8beadab..622714b 100644 --- a/src/tuning_GS1_grt_rtw/tuning_GS1_private.h +++ b/src/tuning_GS1_grt_rtw/tuning_GS1_private.h @@ -7,9 +7,9 @@ * * Code generation for model "tuning_GS1". * - * Model version : 1.2101 + * Model version : 1.2189 * Simulink Coder version : 8.12 (R2017a) 16-Feb-2017 - * C++ source code generated on : Mon Oct 22 19:08:51 2018 + * C++ source code generated on : Thu Mar 7 15:36:47 2019 * * Target selection: grt.tlc * Note: GRT includes extra infrastructure and instrumentation for prototyping diff --git a/src/tuning_GS1_grt_rtw/tuning_GS1_types.h b/src/tuning_GS1_grt_rtw/tuning_GS1_types.h index 1e83c62..500af7c 100644 --- a/src/tuning_GS1_grt_rtw/tuning_GS1_types.h +++ b/src/tuning_GS1_grt_rtw/tuning_GS1_types.h @@ -7,9 +7,9 @@ * * Code generation for model "tuning_GS1". * - * Model version : 1.2101 + * Model version : 1.2189 * Simulink Coder version : 8.12 (R2017a) 16-Feb-2017 - * C++ source code generated on : Mon Oct 22 19:08:51 2018 + * C++ source code generated on : Thu Mar 7 15:36:47 2019 * * Target selection: grt.tlc * Note: GRT includes extra infrastructure and instrumentation for prototyping diff --git a/src/tuning_GS1_node.cpp b/src/tuning_GS1_node.cpp index 14fdbbb..efc053f 100644 --- a/src/tuning_GS1_node.cpp +++ b/src/tuning_GS1_node.cpp @@ -30,7 +30,6 @@ double gPsi; // heading (rad) Eigen::VectorXd gY0(4); // initial position (equilibrium) Eigen::VectorXd gRef(4); // references (x, y, z, yaw) -Eigen::VectorXd gGain(19); Eigen::VectorXd gLOE(6); // LOE trued Eigen::VectorXd gLOE_t(6); // LOE moment Eigen::VectorXd gThrust_measure(6); @@ -43,30 +42,6 @@ void controller_dyn_callback(gsft_control::controllerGS1DynConfig &config, uint3 config.RESET = false; } else if (level & gsft_control::controllerGS1Dyn_ENABLE_CTRL){ - if (config.new_controller_gains){ - gGain[0] = config.kx; // x - gGain[1] = config.kvx; // vx - gGain[2] = config.kix; // integral x - gGain[3] = config.ky; // y - gGain[4] = config.kvy; - gGain[5] = config.kiy; - gGain[6] = config.kz; // z - gGain[7] = config.kvz; - gGain[8] = config.kiz; - gGain[9] = config.kphi; // roll - gGain[10] = config.kp; - gGain[11] = config.kiphi; - gGain[12] = config.ktheta; // pitch - gGain[13] = config.kq; - gGain[14] = config.kitheta; - gGain[15] = config.kpsi; // yaw - gGain[16] = config.kr; - gGain[17] = config.kipsi; - - ROS_INFO("New controller gains"); - config.new_controller_gains = false; - } - // Note: dynamic reconfigure is called when created by server_dyn.setCallback(f_dyn) // i.e. before the first Odometry available // Compare mode: because config.enable_take_off always TRUE @@ -78,30 +53,10 @@ void controller_dyn_callback(gsft_control::controllerGS1DynConfig &config, uint3 gY0[1] = gOdometry.position.y(); gY0[2] = gOdometry.position.z(); gY0[3] = gPsi; - // if (config.test_mode == gsft_control::controllerGS1Dyn_TEST_MANUAL){ gRef[0] = gY0[0]; gRef[1] = gY0[1]; gRef[2] = config.ref_z; gRef[3] = gY0[3]; - // } - gGain[0] = config.kx; // x - gGain[1] = config.kvx; // vx - gGain[2] = config.kix; // integral x - gGain[3] = config.ky; // y - gGain[4] = config.kvy; - gGain[5] = config.kiy; - gGain[6] = config.kz; // z - gGain[7] = config.kvz; - gGain[8] = config.kiz; - gGain[9] = config.kphi; // roll - gGain[10] = config.kp; - gGain[11] = config.kiphi; - gGain[12] = config.ktheta; // pitch - gGain[13] = config.kq; - gGain[14] = config.kitheta; - gGain[15] = config.kpsi; // yaw - gGain[16] = config.kr; - gGain[17] = config.kipsi; gLOE[0] = config.LOE_1; gLOE[1] = config.LOE_2; @@ -251,12 +206,11 @@ int main(int argc, char** argv) { gLanding_flag = false; gEmergency_status = false; gTest_mode = 0; - gLOE_mode = 1; // nominal + gLOE_mode = 0; // nominal gPsi = 0.0; gY0 << 0.0, 0.0, 0.0, 0.0; gRef << 0.0, 0.0, 0.0, 0.0; - gGain = Eigen::VectorXd::Zero(19); gLOE = Eigen::VectorXd::Zero(6); gLOE_t = Eigen::VectorXd::Zero(6); gThrust_measure = Eigen::VectorXd::Zero(6); @@ -310,25 +264,19 @@ int main(int argc, char** argv) { for (unsigned int i=0; i< 6; i++) { gController.tuning_GS1_U.LOE_t[i] = gLOE_t[i]; // fault time } - /*for (unsigned int i=0; i< 19; i++) { - ROS_INFO("Controller gain k[%d] = %f",i,gGain[i]); - }*/ } if (controller_active) { // controller active after take-off request // Initialization before gController.step(); gController.tuning_GS1_U.mode = gTest_mode; // 0 = manual, ... gController.tuning_GS1_U.LOE_mode = gLOE_mode; // 1 = nominal, 2 = LOE true, ... - for (unsigned int i=0; i< 4; i++) { gController.tuning_GS1_U.ref[i] = gRef[i]; } for (unsigned int i=0; i< 4; i++) { gController.tuning_GS1_U.Y0[i] = gY0[i]; } - for (unsigned int i=0; i< 19; i++) { - gController.tuning_GS1_U.gain[i] = gGain[i]; - } + gController.tuning_GS1_U.X[0] = gOdometry.position.x(); gController.tuning_GS1_U.X[1] = gOdometry.position.y(); gController.tuning_GS1_U.X[2] = gOdometry.position.z(); @@ -427,10 +375,10 @@ int main(int argc, char** argv) { uav_state_msg->rotation_speed_B.y = gOdometry.angular_velocity.y(); uav_state_msg->rotation_speed_B.z = gOdometry.angular_velocity.z(); - // uav_state_msg->total_thrust = gController.tuning_GS1_Y.virtual_control[0]; - // uav_state_msg->moment.x = gController.tuning_GS1_Y.virtual_control[1]; - // uav_state_msg->moment.y = gController.tuning_GS1_Y.virtual_control[2]; - // uav_state_msg->moment.z = gController.tuning_GS1_Y.virtual_control[3]; + /* uav_state_msg->total_thrust = gController.tuning_GS1_Y.virtual_control[0]; + uav_state_msg->moment.x = gController.tuning_GS1_Y.virtual_control[1]; + uav_state_msg->moment.y = gController.tuning_GS1_Y.virtual_control[2]; + uav_state_msg->moment.z = gController.tuning_GS1_Y.virtual_control[3]; */ uav_state_msg->thrust_pre.x = gController.tuning_GS1_Y.thrust_pre[0]; uav_state_msg->thrust_pre.y = gController.tuning_GS1_Y.thrust_pre[1]; @@ -454,7 +402,8 @@ int main(int argc, char** argv) { uav_state_msg->Kxyz.x = gController.tuning_GS1_Y.gain_GS[6]; uav_state_msg->Kxyz.y = gController.tuning_GS1_Y.gain_GS[9]; - uav_state_msg->Kxyz.z = gController.tuning_GS1_Y.gain_GS[0]; + uav_state_msg->Kxyz.z = gController.tuning_GS1_Y.gain_GS[0]/10.0; + ROS_INFO("Kz %f",gController.tuning_GS1_Y.gain_GS[0]); uav_state_msg->Kpsi = gController.tuning_GS1_Y.gain_GS[3]; uav_state_msg->header.stamp = ros::Time::now(); @@ -463,23 +412,25 @@ int main(int argc, char** argv) { // LOE message if (controller_active){ gsft_control::LOEPtr LOE_msg(new gsft_control::LOE); + for(unsigned int i=0; i< 6; i++) { + gController.tuning_GS1_U.LOE_cal[i] = 0.0; + LOE_msg->LOE_calcul[i] = 0.0; LOE_msg->LOE_true[i] = gController.tuning_GS1_Y.LOE_true[i]; if (i<3){ LOE_msg->LOE_FDD[i] = gController.tuning_GS1_Y.LOE13_estimated[i]; } + if (thrust_prev_sent[i]!=0){ - temp_LOE_calcul = 1-gThrust_measure[i]/thrust_prev_sent[i]; + temp_LOE_calcul = 1.0-gThrust_measure[i]/thrust_prev_sent[i]; LOE_msg->LOE_calcul[i] = temp_LOE_calcul; if (seq>10){ - // gController.tuning_GS1_U.LOE_calcul[i] = fmin(fmax(temp_LOE_calcul,0.0),1.0); - gController.tuning_GS1_U.LOE_calcul[i] = temp_LOE_calcul; - } else{ - gController.tuning_GS1_U.LOE_calcul[i] = 0.0; + gController.tuning_GS1_U.LOE_cal[i] = temp_LOE_calcul; } } thrust_prev_sent[i] = gController.tuning_GS1_Y.thrust_pre[i]; } + if (seq>5){ // cheat LOE_msg->header.seq = seq; //LOE_msg->header.frame_id = frame_id; diff --git a/src/tuning_GS2_node.cpp b/src/tuning_GS2_node.cpp index 6016d62..520de71 100644 --- a/src/tuning_GS2_node.cpp +++ b/src/tuning_GS2_node.cpp @@ -403,6 +403,7 @@ int main(int argc, char** argv) { uav_state_msg->Kxyz.x = gController.tuning_GS2_Y.gain_GS[6]; uav_state_msg->Kxyz.y = gController.tuning_GS2_Y.gain_GS[9]; uav_state_msg->Kxyz.z = gController.tuning_GS2_Y.gain_GS[0]/10.0; + ROS_INFO("Kz %f",gController.tuning_GS2_Y.gain_GS[0]); uav_state_msg->Kpsi = gController.tuning_GS2_Y.gain_GS[3]; uav_state_msg->header.stamp = ros::Time::now(); diff --git a/src/tuning_nominal_grt_rtw/multiword_types.h b/src/tuning_nominal_grt_rtw/multiword_types.h index 2799799..7f0ffd6 100644 --- a/src/tuning_nominal_grt_rtw/multiword_types.h +++ b/src/tuning_nominal_grt_rtw/multiword_types.h @@ -7,9 +7,9 @@ * * Code generation for model "tuning_nominal". * - * Model version : 1.1506 + * Model version : 1.1507 * Simulink Coder version : 8.12 (R2017a) 16-Feb-2017 - * C++ source code generated on : Tue Jan 22 10:49:00 2019 + * C++ source code generated on : Wed May 15 11:49:32 2019 * * Target selection: grt.tlc * Note: GRT includes extra infrastructure and instrumentation for prototyping diff --git a/src/tuning_nominal_grt_rtw/rtmodel.h b/src/tuning_nominal_grt_rtw/rtmodel.h index abd358a..ecefa99 100644 --- a/src/tuning_nominal_grt_rtw/rtmodel.h +++ b/src/tuning_nominal_grt_rtw/rtmodel.h @@ -7,9 +7,9 @@ * * Code generation for model "tuning_nominal". * - * Model version : 1.1506 + * Model version : 1.1507 * Simulink Coder version : 8.12 (R2017a) 16-Feb-2017 - * C++ source code generated on : Tue Jan 22 10:49:00 2019 + * C++ source code generated on : Wed May 15 11:49:32 2019 * * Target selection: grt.tlc * Note: GRT includes extra infrastructure and instrumentation for prototyping diff --git a/src/tuning_nominal_grt_rtw/rtwtypes.h b/src/tuning_nominal_grt_rtw/rtwtypes.h index f8b50e8..53b6686 100644 --- a/src/tuning_nominal_grt_rtw/rtwtypes.h +++ b/src/tuning_nominal_grt_rtw/rtwtypes.h @@ -7,9 +7,9 @@ * * Code generation for model "tuning_nominal". * - * Model version : 1.1506 + * Model version : 1.1507 * Simulink Coder version : 8.12 (R2017a) 16-Feb-2017 - * C++ source code generated on : Tue Jan 22 10:49:00 2019 + * C++ source code generated on : Wed May 15 11:49:32 2019 * * Target selection: grt.tlc * Note: GRT includes extra infrastructure and instrumentation for prototyping diff --git a/src/tuning_nominal_grt_rtw/tuning_nominal.cpp b/src/tuning_nominal_grt_rtw/tuning_nominal.cpp index a636521..9f71244 100644 --- a/src/tuning_nominal_grt_rtw/tuning_nominal.cpp +++ b/src/tuning_nominal_grt_rtw/tuning_nominal.cpp @@ -7,9 +7,9 @@ * * Code generation for model "tuning_nominal". * - * Model version : 1.1506 + * Model version : 1.1507 * Simulink Coder version : 8.12 (R2017a) 16-Feb-2017 - * C++ source code generated on : Tue Jan 22 10:49:00 2019 + * C++ source code generated on : Wed May 15 11:49:32 2019 * * Target selection: grt.tlc * Note: GRT includes extra infrastructure and instrumentation for prototyping diff --git a/src/tuning_nominal_grt_rtw/tuning_nominal.h b/src/tuning_nominal_grt_rtw/tuning_nominal.h index 5ef0eed..89e058a 100644 --- a/src/tuning_nominal_grt_rtw/tuning_nominal.h +++ b/src/tuning_nominal_grt_rtw/tuning_nominal.h @@ -7,9 +7,9 @@ * * Code generation for model "tuning_nominal". * - * Model version : 1.1506 + * Model version : 1.1507 * Simulink Coder version : 8.12 (R2017a) 16-Feb-2017 - * C++ source code generated on : Tue Jan 22 10:49:00 2019 + * C++ source code generated on : Wed May 15 11:49:32 2019 * * Target selection: grt.tlc * Note: GRT includes extra infrastructure and instrumentation for prototyping @@ -209,7 +209,7 @@ typedef struct { /* Constant parameters (auto storage) */ typedef struct { - /* Expression: B_ENU_inv + /* Expression: B_CA_ENU_inv * Referenced by: '/Control Allocation' */ real_T ControlAllocation_Gain[24]; diff --git a/src/tuning_nominal_grt_rtw/tuning_nominal_data.cpp b/src/tuning_nominal_grt_rtw/tuning_nominal_data.cpp index bd52776..5eb1065 100644 --- a/src/tuning_nominal_grt_rtw/tuning_nominal_data.cpp +++ b/src/tuning_nominal_grt_rtw/tuning_nominal_data.cpp @@ -7,9 +7,9 @@ * * Code generation for model "tuning_nominal". * - * Model version : 1.1506 + * Model version : 1.1507 * Simulink Coder version : 8.12 (R2017a) 16-Feb-2017 - * C++ source code generated on : Tue Jan 22 10:49:00 2019 + * C++ source code generated on : Wed May 15 11:49:32 2019 * * Target selection: grt.tlc * Note: GRT includes extra infrastructure and instrumentation for prototyping @@ -23,7 +23,7 @@ /* Constant parameters (auto storage) */ const ConstP_tuning_nominal_T tuning_nominal_ConstP = { - /* Expression: B_ENU_inv + /* Expression: B_CA_ENU_inv * Referenced by: '/Control Allocation' */ { 0.16666666666666674, 0.16666666666666669, 0.16666666666666671, diff --git a/src/tuning_nominal_grt_rtw/tuning_nominal_private.h b/src/tuning_nominal_grt_rtw/tuning_nominal_private.h index 36af89a..dc63718 100644 --- a/src/tuning_nominal_grt_rtw/tuning_nominal_private.h +++ b/src/tuning_nominal_grt_rtw/tuning_nominal_private.h @@ -7,9 +7,9 @@ * * Code generation for model "tuning_nominal". * - * Model version : 1.1506 + * Model version : 1.1507 * Simulink Coder version : 8.12 (R2017a) 16-Feb-2017 - * C++ source code generated on : Tue Jan 22 10:49:00 2019 + * C++ source code generated on : Wed May 15 11:49:32 2019 * * Target selection: grt.tlc * Note: GRT includes extra infrastructure and instrumentation for prototyping diff --git a/src/tuning_nominal_grt_rtw/tuning_nominal_types.h b/src/tuning_nominal_grt_rtw/tuning_nominal_types.h index 061a9fd..47d8871 100644 --- a/src/tuning_nominal_grt_rtw/tuning_nominal_types.h +++ b/src/tuning_nominal_grt_rtw/tuning_nominal_types.h @@ -7,9 +7,9 @@ * * Code generation for model "tuning_nominal". * - * Model version : 1.1506 + * Model version : 1.1507 * Simulink Coder version : 8.12 (R2017a) 16-Feb-2017 - * C++ source code generated on : Tue Jan 22 10:49:00 2019 + * C++ source code generated on : Wed May 15 11:49:32 2019 * * Target selection: grt.tlc * Note: GRT includes extra infrastructure and instrumentation for prototyping