From fb45d2fa88aff0f31ee0fbb7bc5ecc80959f1aef Mon Sep 17 00:00:00 2001 From: Duc-Tien Nguyen Date: Tue, 25 Sep 2018 10:39:05 -0400 Subject: [PATCH] final test --- CMakeLists.txt | 56 +- cfg/GS1_Dyn.cfg | 59 + package.xml | 1 + .../multiword_types.h | 4 +- .../rtmodel.h | 8 +- .../rtwtypes.h | 4 +- src/tuning_GS1_grt_rtw/tuning_GS1.cpp | 1088 +++++++++++++++ src/tuning_GS1_grt_rtw/tuning_GS1.h | 537 ++++++++ .../tuning_GS1_data.cpp} | 12 +- .../tuning_GS1_private.h} | 14 +- .../tuning_GS1_types.h} | 14 +- src/{gs1_node.cpp => tuning_GS1_node.cpp} | 152 +-- .../multiword_types.h | 0 .../rtmodel.h | 0 .../rtwtypes.h | 0 .../tuning_lqr.cpp} | 552 ++++---- .../tuning_lqr.h} | 86 +- .../tuning_lqr_data.cpp} | 10 +- .../tuning_lqr_private.h} | 12 +- .../tuning_lqr_types.h} | 12 +- ...nning_lqr_node.cpp => tuning_lqr_node.cpp} | 72 +- src/tuning_nominal_grt_rtw/multiword_types.h | 1167 +++++++++++++++++ src/tuning_nominal_grt_rtw/rtmodel.h | 38 + src/tuning_nominal_grt_rtw/rtwtypes.h | 42 + .../tuning_nominal.cpp} | 612 +++++---- .../tuning_nominal.h} | 192 +-- .../tuning_nominal_data.cpp | 74 ++ .../tuning_nominal_private.h | 46 + .../tuning_nominal_types.h | 27 + ...minal_node.cpp => tuning_nominal_node.cpp} | 126 +- 30 files changed, 4053 insertions(+), 964 deletions(-) create mode 100644 cfg/GS1_Dyn.cfg rename src/{tunning_nominal_grt_rtw => tuning_GS1_grt_rtw}/multiword_types.h (99%) rename src/{tunning_nominal_grt_rtw => tuning_GS1_grt_rtw}/rtmodel.h (84%) rename src/{tunning_nominal_grt_rtw => tuning_GS1_grt_rtw}/rtwtypes.h (90%) create mode 100644 src/tuning_GS1_grt_rtw/tuning_GS1.cpp create mode 100644 src/tuning_GS1_grt_rtw/tuning_GS1.h rename src/{tunning_nominal_grt_rtw/tunning_nominal_data.cpp => tuning_GS1_grt_rtw/tuning_GS1_data.cpp} (90%) rename src/{tunning_nominal_grt_rtw/tunning_nominal_private.h => tuning_GS1_grt_rtw/tuning_GS1_private.h} (76%) rename src/{tunning_nominal_grt_rtw/tunning_nominal_types.h => tuning_GS1_grt_rtw/tuning_GS1_types.h} (62%) rename src/{gs1_node.cpp => tuning_GS1_node.cpp} (72%) rename src/{tunning_lqr_grt_rtw => tuning_lqr_grt_rtw}/multiword_types.h (100%) rename src/{tunning_lqr_grt_rtw => tuning_lqr_grt_rtw}/rtmodel.h (100%) rename src/{tunning_lqr_grt_rtw => tuning_lqr_grt_rtw}/rtwtypes.h (100%) rename src/{tunning_lqr_grt_rtw/tunning_lqr.cpp => tuning_lqr_grt_rtw/tuning_lqr.cpp} (55%) rename src/{tunning_lqr_grt_rtw/tunning_lqr.h => tuning_lqr_grt_rtw/tuning_lqr.h} (86%) rename src/{tunning_lqr_grt_rtw/tunning_lqr_data.cpp => tuning_lqr_grt_rtw/tuning_lqr_data.cpp} (88%) rename src/{tunning_lqr_grt_rtw/tunning_lqr_private.h => tuning_lqr_grt_rtw/tuning_lqr_private.h} (81%) rename src/{tunning_lqr_grt_rtw/tunning_lqr_types.h => tuning_lqr_grt_rtw/tuning_lqr_types.h} (70%) rename src/{tunning_lqr_node.cpp => tuning_lqr_node.cpp} (83%) create mode 100644 src/tuning_nominal_grt_rtw/multiword_types.h create mode 100644 src/tuning_nominal_grt_rtw/rtmodel.h create mode 100644 src/tuning_nominal_grt_rtw/rtwtypes.h rename src/{tunning_nominal_grt_rtw/tunning_nominal.cpp => tuning_nominal_grt_rtw/tuning_nominal.cpp} (56%) rename src/{tunning_nominal_grt_rtw/tunning_nominal.h => tuning_nominal_grt_rtw/tuning_nominal.h} (74%) create mode 100644 src/tuning_nominal_grt_rtw/tuning_nominal_data.cpp create mode 100644 src/tuning_nominal_grt_rtw/tuning_nominal_private.h create mode 100644 src/tuning_nominal_grt_rtw/tuning_nominal_types.h rename src/{tunning_nominal_node.cpp => tuning_nominal_node.cpp} (75%) diff --git a/CMakeLists.txt b/CMakeLists.txt index d9bf53b..1d426e6 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -10,6 +10,7 @@ project(gsft_control) find_package(catkin REQUIRED COMPONENTS dynamic_reconfigure mav_msgs + asctec_hl_comm nav_msgs sensor_msgs cmake_modules @@ -114,7 +115,7 @@ generate_dynamic_reconfigure_options( catkin_package( INCLUDE_DIRS include ## LIBRARIES gsft_control - CATKIN_DEPENDS dynamic_reconfigure geometry_msgs mav_msgs nav_msgs roscpp sensor_msgs trajectory_msgs message_generation std_msgs geometry_msgs + CATKIN_DEPENDS dynamic_reconfigure geometry_msgs mav_msgs nav_msgs roscpp sensor_msgs trajectory_msgs message_generation std_msgs geometry_msgs asctec_hl_comm DEPENDS EIGEN3 ) @@ -128,23 +129,27 @@ include_directories( include R2017a/extern/include R2017a/simulink/include - src/tunning_lqr_grt_rtw/ - src/tunning_nominal_grt_rtw/ + src/tuning_lqr_grt_rtw/ + src/tuning_nominal_grt_rtw/ + src/tuning_GS1_grt_rtw/ ${catkin_INCLUDE_DIRS} ${EIGEN3_INCLUDE_DIRS} ) add_library(matlab_simulink - src/tunning_lqr_grt_rtw/tunning_lqr_data.cpp - src/tunning_lqr_grt_rtw/tunning_lqr.cpp + src/tuning_lqr_grt_rtw/tuning_lqr_data.cpp + src/tuning_lqr_grt_rtw/tuning_lqr.cpp - src/tunning_nominal_grt_rtw/tunning_nominal_data.cpp - src/tunning_nominal_grt_rtw/tunning_nominal.cpp + src/tuning_nominal_grt_rtw/tuning_nominal_data.cpp + src/tuning_nominal_grt_rtw/tuning_nominal.cpp - #src/tunning_nominal_grt_rtw/rtGetInf.cpp - #src/tunning_nominal_grt_rtw/rtGetNaN.cpp - #src/tunning_nominal_grt_rtw/rt_nonfinite.cpp + src/tuning_GS1_grt_rtw/tuning_GS1_data.cpp + src/tuning_GS1_grt_rtw/tuning_GS1.cpp + + #src/tuning_nominal_grt_rtw/rtGetInf.cpp + #src/tuning_nominal_grt_rtw/rtGetNaN.cpp + #src/tuning_nominal_grt_rtw/rt_nonfinite.cpp ) add_dependencies(matlab_simulink ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) @@ -153,35 +158,44 @@ add_dependencies(matlab_simulink ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EX ## With catkin_make all packages are built within a single CMake context ## The recommended prefix ensures that target names across packages don't collide -add_executable(tunning_lqr_node - src/tunning_lqr_node.cpp +add_executable(tuning_lqr_node + src/tuning_lqr_node.cpp ) -add_dependencies(tunning_lqr_node +add_dependencies(tuning_lqr_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS} matlab_simulink ) -target_link_libraries(tunning_lqr_node +target_link_libraries(tuning_lqr_node matlab_simulink ${catkin_LIBRARIES} ) - - - -add_executable(tunning_nominal_node - src/tunning_nominal_node.cpp +add_executable(tuning_nominal_node + src/tuning_nominal_node.cpp ) -add_dependencies(tunning_nominal_node +add_dependencies(tuning_nominal_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS} matlab_simulink ) -target_link_libraries(tunning_nominal_node +target_link_libraries(tuning_nominal_node matlab_simulink ${catkin_LIBRARIES} ) +add_executable(tuning_GS1_node + src/tuning_GS1_node.cpp +) +add_dependencies(tuning_GS1_node + ${${PROJECT_NAME}_EXPORTED_TARGETS} + ${catkin_EXPORTED_TARGETS} + matlab_simulink +) +target_link_libraries(tuning_GS1_node + matlab_simulink + ${catkin_LIBRARIES} +) ############# ## Install ## diff --git a/cfg/GS1_Dyn.cfg b/cfg/GS1_Dyn.cfg new file mode 100644 index 0000000..682d1a6 --- /dev/null +++ b/cfg/GS1_Dyn.cfg @@ -0,0 +1,59 @@ +#! /usr/bin/env python + +PACKAGE='gsft_control' +import roslib +roslib.load_manifest(PACKAGE) + +from dynamic_reconfigure.parameter_generator_catkin import * + +gen = ParameterGenerator() + +ENABLE_CTRL = gen.const("ENABLE_CTRL", int_t, 0x00000001, "enable_ctrl") +NEW_REFERENCES = gen.const("NEW_REFERENCES", int_t, 0x00000002, "new_references") +#NEW_PARAMETERS = gen.const("NEW_PARAMETERS", int_t, 0x00000004, "drone_parameter") +CONTROLLER_GAIN = gen.const("CONTROLLER_GAIN", int_t, 0x00000008, "controller_gain") +#LOE = gen.const("LOE", int_t, 0x01000000, "lost_of_control_effectiveness") + +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") +group5 = gen.add_group("group_LOE") + +# Name Type Reconfiguration level Description Default Min Max +gen.add("RESET", bool_t, 0, "reset parameters, gains", False) + +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) + +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) +group1.add("send_waypoint", bool_t, ENABLE_CTRL["value"], "send new waypoint", False) +group1.add("new_controller_gains", bool_t, ENABLE_CTRL["value"], "change controller gains", False) +#group1.add("new_drone_parameters", bool_t, ENABLE_CTRL["value"], "change drone parameters", False) # to do later +#group1.add("enable_yaw", bool_t, ENABLE_CTRL["value"], "enable yaw-control", True) # to do later +#group1.add("enable_gohome", bool_t, ENABLE_CTRL["value"], "enable go home", False) # to do later + +group2.add("ref_x", double_t, NEW_REFERENCES["value"], "X position reference", 0.0, -2.0, 2.0) +group2.add("ref_y", double_t, NEW_REFERENCES["value"], "Y position reference", 0.0, -2.0, 2.0) +group2.add("ref_z", double_t, NEW_REFERENCES["value"], "Z position reference", 0.5, 0.0, 1.5) +group2.add("ref_yaw_deg", double_t, NEW_REFERENCES["value"], "yaw angle reference", 0.0, -180.0, 180.0) + +group5.add("LOE_1", double_t, 0.0, "lost of control 1", 0.0, 0.0, 1.0) +group5.add("LOE_2", double_t, 0.0, "lost of control 2", 0.0, 0.0, 1.0) +group5.add("LOE_3", double_t, 0.0, "lost of control 3", 0.0, 0.0, 1.0) +group5.add("LOE_4", double_t, 0.0, "lost of control 4", 0.0, 0.0, 1.0) +group5.add("LOE_5", double_t, 0.0, "lost of control 5", 0.0, 0.0, 1.0) +group5.add("LOE_6", double_t, 0.0, "lost of control 6", 0.0, 0.0, 1.0) +group5.add("LOE_t1", double_t, 0.0, "occurs time 1", 0.0, 0.0, 100.0) +group5.add("LOE_t2", double_t, 0.0, "occurs time 2", 0.0, 0.0, 100.0) +group5.add("LOE_t3", double_t, 0.0, "occurs time 3", 0.0, 0.0, 100.0) +group5.add("LOE_t4", double_t, 0.0, "occurs time 4", 0.0, 0.0, 100.0) +group5.add("LOE_t5", double_t, 0.0, "occurs time 5", 0.0, 0.0, 100.0) +group5.add("LOE_t6", double_t, 0.0, "occurs time 6", 0.0, 0.0, 100.0) + + +exit(gen.generate(PACKAGE, "gsft_control", "controllerDyn")) diff --git a/package.xml b/package.xml index a9af740..4ef4323 100644 --- a/package.xml +++ b/package.xml @@ -18,6 +18,7 @@ cmake_modules geometry_msgs mav_msgs + asctec_hl_comm nav_msgs sensor_msgs trajectory_msgs diff --git a/src/tunning_nominal_grt_rtw/multiword_types.h b/src/tuning_GS1_grt_rtw/multiword_types.h similarity index 99% rename from src/tunning_nominal_grt_rtw/multiword_types.h rename to src/tuning_GS1_grt_rtw/multiword_types.h index 0488b8d..502cc92 100644 --- a/src/tunning_nominal_grt_rtw/multiword_types.h +++ b/src/tuning_GS1_grt_rtw/multiword_types.h @@ -5,11 +5,11 @@ * perform academic research at degree granting institutions only. Not * for government, commercial, or other organizational use. * - * Code generation for model "tunning_nominal". + * Code generation for model "tuning_GS1". * * Model version : 1.1498 * Simulink Coder version : 8.12 (R2017a) 16-Feb-2017 - * C++ source code generated on : Fri Aug 31 14:28:54 2018 + * C++ source code generated on : Tue Sep 18 10:42:41 2018 * * Target selection: grt.tlc * Note: GRT includes extra infrastructure and instrumentation for prototyping diff --git a/src/tunning_nominal_grt_rtw/rtmodel.h b/src/tuning_GS1_grt_rtw/rtmodel.h similarity index 84% rename from src/tunning_nominal_grt_rtw/rtmodel.h rename to src/tuning_GS1_grt_rtw/rtmodel.h index ab09157..621e8db 100644 --- a/src/tunning_nominal_grt_rtw/rtmodel.h +++ b/src/tuning_GS1_grt_rtw/rtmodel.h @@ -5,11 +5,11 @@ * perform academic research at degree granting institutions only. Not * for government, commercial, or other organizational use. * - * Code generation for model "tunning_nominal". + * Code generation for model "tuning_GS1". * * Model version : 1.1498 * Simulink Coder version : 8.12 (R2017a) 16-Feb-2017 - * C++ source code generated on : Fri Aug 31 14:28:54 2018 + * C++ source code generated on : Tue Sep 18 10:42:41 2018 * * Target selection: grt.tlc * Note: GRT includes extra infrastructure and instrumentation for prototyping @@ -24,7 +24,7 @@ /* * Includes the appropriate headers when we are using rtModel */ -#include "tunning_nominal.h" +#include "tuning_GS1.h" #define GRTINTERFACE 0 /* @@ -33,6 +33,6 @@ * ROOT_IO_FORMAT: 2 (Part of model data structure) */ # define ROOT_IO_FORMAT 2 -#define MODEL_CLASSNAME tunning_nominalModelClass +#define MODEL_CLASSNAME tuning_GS1ModelClass #define MODEL_STEPNAME step #endif /* RTW_HEADER_rtmodel_h_ */ diff --git a/src/tunning_nominal_grt_rtw/rtwtypes.h b/src/tuning_GS1_grt_rtw/rtwtypes.h similarity index 90% rename from src/tunning_nominal_grt_rtw/rtwtypes.h rename to src/tuning_GS1_grt_rtw/rtwtypes.h index fba2a12..f1f484c 100644 --- a/src/tunning_nominal_grt_rtw/rtwtypes.h +++ b/src/tuning_GS1_grt_rtw/rtwtypes.h @@ -5,11 +5,11 @@ * perform academic research at degree granting institutions only. Not * for government, commercial, or other organizational use. * - * Code generation for model "tunning_nominal". + * Code generation for model "tuning_GS1". * * Model version : 1.1498 * Simulink Coder version : 8.12 (R2017a) 16-Feb-2017 - * C++ source code generated on : Fri Aug 31 14:28:54 2018 + * C++ source code generated on : Tue Sep 18 10:42:41 2018 * * 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 new file mode 100644 index 0000000..535fe5f --- /dev/null +++ b/src/tuning_GS1_grt_rtw/tuning_GS1.cpp @@ -0,0 +1,1088 @@ +/* + * tuning_GS1.cpp + * + * Student License - for use by students to meet course requirements and + * perform academic research at degree granting institutions only. Not + * for government, commercial, or other organizational use. + * + * Code generation for model "tuning_GS1". + * + * Model version : 1.1498 + * Simulink Coder version : 8.12 (R2017a) 16-Feb-2017 + * C++ source code generated on : Tue Sep 18 10:42:41 2018 + * + * Target selection: grt.tlc + * Note: GRT includes extra infrastructure and instrumentation for prototyping + * Embedded hardware selection: 32-bit Generic + * Code generation objective: Execution efficiency + * Validation result: Passed (1), Warnings (3), Error (0) + */ + +#include "tuning_GS1.h" +#include "tuning_GS1_private.h" + +static void rate_scheduler(RT_MODEL_tuning_GS1_T *const tuning_GS1_M); + +/* + * This function updates active task flag for each subrate. + * The function is called at model base rate, hence the + * generated code self-manages all its subrates. + */ +static void rate_scheduler(RT_MODEL_tuning_GS1_T *const tuning_GS1_M) +{ + /* Compute which subrates run during the next base time step. Subrates + * are an integer multiple of the base rate counter. Therefore, the subtask + * counter is reset when it reaches its limit (zero means run). + */ + (tuning_GS1_M->Timing.TaskCounters.TID[2])++; + if ((tuning_GS1_M->Timing.TaskCounters.TID[2]) > 4) {/* Sample time: [0.005s, 0.0s] */ + tuning_GS1_M->Timing.TaskCounters.TID[2] = 0; + } +} + +/* + * This function updates continuous states using the ODE4 fixed-step + * solver algorithm + */ +void tuning_GS1ModelClass::rt_ertODEUpdateContinuousStates(RTWSolverInfo *si ) +{ + time_T t = rtsiGetT(si); + time_T tnew = rtsiGetSolverStopTime(si); + time_T h = rtsiGetStepSize(si); + real_T *x = rtsiGetContStates(si); + ODE4_IntgData *id = (ODE4_IntgData *)rtsiGetSolverData(si); + real_T *y = id->y; + real_T *f0 = id->f[0]; + real_T *f1 = id->f[1]; + real_T *f2 = id->f[2]; + real_T *f3 = id->f[3]; + real_T temp; + int_T i; + int_T nXc = 4; + rtsiSetSimTimeStep(si,MINOR_TIME_STEP); + + /* Save the state values at time t in y, we'll use x as ynew. */ + (void) memcpy(y, x, + (uint_T)nXc*sizeof(real_T)); + + /* Assumes that rtsiSetT and ModelOutputs are up-to-date */ + /* f0 = f(t,y) */ + rtsiSetdX(si, f0); + tuning_GS1_derivatives(); + + /* f1 = f(t + (h/2), y + (h/2)*f0) */ + temp = 0.5 * h; + for (i = 0; i < nXc; i++) { + x[i] = y[i] + (temp*f0[i]); + } + + rtsiSetT(si, t + temp); + rtsiSetdX(si, f1); + this->step(); + tuning_GS1_derivatives(); + + /* f2 = f(t + (h/2), y + (h/2)*f1) */ + for (i = 0; i < nXc; i++) { + x[i] = y[i] + (temp*f1[i]); + } + + rtsiSetdX(si, f2); + this->step(); + tuning_GS1_derivatives(); + + /* f3 = f(t + h, y + h*f2) */ + for (i = 0; i < nXc; i++) { + x[i] = y[i] + (h*f2[i]); + } + + rtsiSetT(si, tnew); + rtsiSetdX(si, f3); + this->step(); + tuning_GS1_derivatives(); + + /* tnew = t + h + ynew = y + (h/6)*(f0 + 2*f1 + 2*f2 + 2*f3) */ + temp = h / 6.0; + for (i = 0; i < nXc; i++) { + x[i] = y[i] + temp*(f0[i] + 2.0*f1[i] + 2.0*f2[i] + f3[i]); + } + + rtsiSetSimTimeStep(si,MAJOR_TIME_STEP); +} + +/* Model step function */ +void tuning_GS1ModelClass::step() +{ + /* local block i/o variables */ + real_T rtb_Add[6]; + if (rtmIsMajorTimeStep((&tuning_GS1_M))) { + /* set solver stop time */ + if (!((&tuning_GS1_M)->Timing.clockTick0+1)) { + rtsiSetSolverStopTime(&(&tuning_GS1_M)->solverInfo, (((&tuning_GS1_M) + ->Timing.clockTickH0 + 1) * (&tuning_GS1_M)->Timing.stepSize0 * + 4294967296.0)); + } else { + rtsiSetSolverStopTime(&(&tuning_GS1_M)->solverInfo, (((&tuning_GS1_M) + ->Timing.clockTick0 + 1) * (&tuning_GS1_M)->Timing.stepSize0 + + (&tuning_GS1_M)->Timing.clockTickH0 * (&tuning_GS1_M)->Timing.stepSize0 * + 4294967296.0)); + } + } /* end MajorTimeStep */ + + /* Update absolute time of base rate at minor time step */ + if (rtmIsMinorTimeStep((&tuning_GS1_M))) { + (&tuning_GS1_M)->Timing.t[0] = rtsiGetT(&(&tuning_GS1_M)->solverInfo); + } + + { + real_T rtb_d_z; + real_T rtb_d_x; + real_T rtb_d_y; + real_T rtb_d_psi; + real_T rtb_T_f[6]; + real_T rtb_LOE_out[6]; + real_T rtb_Sum1_l[3]; + real_T rtb_Add_a[6]; + real_T rtb_Sum3_i[3]; + real_T rtb_Clock; + real_T rtb_MemoryX[6]; + int32_T i; + real_T rtb_Add_f[3]; + real_T rtb_Add_f_0[3]; + int32_T i_0; + real_T rtb_ref_idx_3; + real_T rtb_ref_idx_0; + real_T rtb_ff_idx_1; + real_T rtb_ff_idx_0; + real_T u0; + + /* Sum: '/Sum5' incorporates: + * Inport: '/X' + * Inport: '/Y0' + */ + rtb_d_z = tuning_GS1_U.X[2] - tuning_GS1_U.Y0[2]; + + /* Sum: '/Sum1' incorporates: + * Inport: '/X' + * Inport: '/Y0' + */ + rtb_d_x = tuning_GS1_U.X[0] - tuning_GS1_U.Y0[0]; + + /* Sum: '/Sum2' incorporates: + * Inport: '/X' + * Inport: '/gain' + * Integrator: '/Integrator1' + * Product: '/Product2' + * Product: '/Product3' + * SignalConversion: '/TmpSignal ConversionAtProduct3Inport2' + */ + tuning_GS1_B.Sum2 = tuning_GS1_U.gain[2] * tuning_GS1_X.Integrator1_CSTATE_d + - (tuning_GS1_U.gain[0] * rtb_d_x + tuning_GS1_U.gain[1] * tuning_GS1_U.X + [3]); + + /* Sum: '/Sum4' incorporates: + * Inport: '/X' + * Inport: '/Y0' + */ + rtb_d_y = tuning_GS1_U.X[1] - tuning_GS1_U.Y0[1]; + + /* Sum: '/Sum3' incorporates: + * Inport: '/X' + * Inport: '/gain' + * 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]); + + /* RateTransition: '/T_outer' incorporates: + * Inport: '/X' + */ + if ((rtmIsMajorTimeStep((&tuning_GS1_M)) && + (&tuning_GS1_M)->Timing.TaskCounters.TID[1] == 0) && + (rtmIsMajorTimeStep((&tuning_GS1_M)) && + (&tuning_GS1_M)->Timing.TaskCounters.TID[2] == 0)) { + tuning_GS1_B.T_outer[0] = tuning_GS1_B.Sum2; + tuning_GS1_B.T_outer[1] = tuning_GS1_B.Sum3; + tuning_GS1_B.T_outer[2] = tuning_GS1_U.X[8]; + } + + /* End of RateTransition: '/T_outer' */ + if (rtmIsMajorTimeStep((&tuning_GS1_M)) && + (&tuning_GS1_M)->Timing.TaskCounters.TID[2] == 0) { + /* Fcn: '/Fcn1' */ + tuning_GS1_B.Fcn1 = tuning_GS1_B.T_outer[1] * std::cos + (tuning_GS1_B.T_outer[2]) + tuning_GS1_B.T_outer[0] * std::sin + (tuning_GS1_B.T_outer[2]); + } + + /* Clock: '/Clock' */ + rtb_Clock = (&tuning_GS1_M)->Timing.t[0]; + + /* MATLAB Function: '/FFW' incorporates: + * Inport: '/X' + * Inport: '/mode' + */ + /* 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; */ + /* [x;y] = [cos(t); sin(t)] */ + /* ':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; */ + /* 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]; */ + 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 + (tuning_GS1_U.X[6]) * std::sin(tuning_GS1_U.X[7]) * std::sin + (tuning_GS1_U.X[8]) + std::cos(tuning_GS1_U.X[6]) * std::cos + (tuning_GS1_U.X[8])) * -std::sin(rtb_Clock)) / 9.81; + rtb_ff_idx_1 = (std::cos(tuning_GS1_U.X[7]) * std::cos(tuning_GS1_U.X[8]) * + -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]; */ + } + + /* End of MATLAB Function: '/FFW' */ + if (rtmIsMajorTimeStep((&tuning_GS1_M)) && + (&tuning_GS1_M)->Timing.TaskCounters.TID[2] == 0) { + /* Fcn: '/Fcn' */ + tuning_GS1_B.Fcn = -tuning_GS1_B.T_outer[1] * std::sin + (tuning_GS1_B.T_outer[2]) + tuning_GS1_B.T_outer[0] * std::cos + (tuning_GS1_B.T_outer[2]); + } + + /* Sum: '/Sum6' incorporates: + * Inport: '/X' + * Inport: '/Y0' + */ + rtb_d_psi = tuning_GS1_U.X[8] - tuning_GS1_U.Y0[3]; + + /* SignalConversion: '/TmpSignal ConversionAtControl AllocationInport1' incorporates: + * Constant: '/ ' + * Gain: '/Gain1' + * Gain: '/Gain' + * Inport: '/X' + * Inport: '/gain' + * Integrator: '/Integrator1' + * Product: '/Product' + * Product: '/Product1' + * SignalConversion: '/TmpSignal ConversionAtProduct1Inport2' + * Sum: '/Sum1' + * Sum: '/Sum1' + */ + rtb_ref_idx_0 = ((tuning_GS1_U.gain[8] * tuning_GS1_X.Integrator1_CSTATE - + (tuning_GS1_U.gain[6] * rtb_d_z + tuning_GS1_U.gain[7] * + tuning_GS1_U.X[5])) * 0.64935064935064934 + 9.81) * 1.54; + + /* Saturate: '/2Nm ' incorporates: + * Inport: '/X' + * Inport: '/gain' + * Product: '/Product' + * SignalConversion: '/TmpSignal ConversionAtProductInport2' + * 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]); + if (rtb_ff_idx_0 > 2.0) { + /* SignalConversion: '/TmpSignal ConversionAtControl AllocationInport1' */ + rtb_ff_idx_0 = 2.0; + } else { + if (rtb_ff_idx_0 < -2.0) { + /* SignalConversion: '/TmpSignal ConversionAtControl AllocationInport1' */ + rtb_ff_idx_0 = -2.0; + } + } + + /* End of Saturate: '/2Nm ' */ + + /* Saturate: '/2Nm' incorporates: + * Inport: '/X' + * Inport: '/gain' + * Product: '/Product' + * SignalConversion: '/TmpSignal ConversionAtProductInport2' + * Sum: '/Sum8' + * Sum: '/Sum1' + */ + 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]); + if (rtb_ff_idx_1 > 2.0) { + /* SignalConversion: '/TmpSignal ConversionAtControl AllocationInport1' */ + rtb_ff_idx_1 = 2.0; + } else { + if (rtb_ff_idx_1 < -2.0) { + /* SignalConversion: '/TmpSignal ConversionAtControl AllocationInport1' */ + rtb_ff_idx_1 = -2.0; + } + } + + /* End of Saturate: '/2Nm' */ + + /* Saturate: '/1Nm' incorporates: + * Inport: '/X' + * Inport: '/gain' + * Integrator: '/Integrator1' + * Product: '/Product' + * Product: '/Product1' + * SignalConversion: '/TmpSignal ConversionAtProduct1Inport2' + * Sum: '/Sum1' + */ + rtb_ref_idx_3 = tuning_GS1_U.gain[17] * tuning_GS1_X.Integrator1_CSTATE_j - + (tuning_GS1_U.gain[15] * rtb_d_psi + tuning_GS1_U.gain[16] * + tuning_GS1_U.X[11]); + if (rtb_ref_idx_3 > 1.0) { + /* SignalConversion: '/TmpSignal ConversionAtControl AllocationInport1' */ + rtb_ref_idx_3 = 1.0; + } else { + if (rtb_ref_idx_3 < -1.0) { + /* SignalConversion: '/TmpSignal ConversionAtControl AllocationInport1' */ + rtb_ref_idx_3 = -1.0; + } + } + + /* End of Saturate: '/1Nm' */ + /* MATLAB Function 'Test_config_and_data/LOE_': ':1' */ + /* ':1:2' LOE_out = [0;0;0;0;0;0]; */ + /* ':1:3' for i = 1:6 */ + for (i = 0; i < 6; i++) { + /* Gain: '/Control Allocation' incorporates: + * Saturate: '/ ' + */ + u0 = tuning_GS1_ConstP.ControlAllocation_Gain[i + 18] * rtb_ref_idx_3 + + (tuning_GS1_ConstP.ControlAllocation_Gain[i + 12] * rtb_ff_idx_1 + + (tuning_GS1_ConstP.ControlAllocation_Gain[i + 6] * rtb_ff_idx_0 + + tuning_GS1_ConstP.ControlAllocation_Gain[i] * rtb_ref_idx_0)); + + /* Saturate: '/ ' incorporates: + * Gain: '/Control Allocation' + */ + if (u0 > 8.54858) { + tuning_GS1_B.u[i] = 8.54858; + } else if (u0 < 0.0) { + tuning_GS1_B.u[i] = 0.0; + } else { + tuning_GS1_B.u[i] = u0; + } + + /* MATLAB Function: '/LOE_' incorporates: + * Inport: '/LOE_a' + * Inport: '/LOE_t' + */ + rtb_LOE_out[i] = 0.0; + + /* ':1:4' if t>= LOE_t(i) */ + if (rtb_Clock >= tuning_GS1_U.LOE_t[i]) { + /* ':1:5' LOE_out(i) = LOE(i); */ + rtb_LOE_out[i] = tuning_GS1_U.LOE_a[i]; + } + + /* End of MATLAB Function: '/LOE_' */ + } + + /* MATLAB Function: '/Actuator_Fault' */ + /* MATLAB Function 'Actuator_Fault': ':1' */ + /* ':1:2' T1 = T(1)*(1-LOE(1)); */ + /* ':1:3' T2 = T(2)*(1-LOE(2)); */ + /* ':1:4' T3 = T(3)*(1-LOE(3)); */ + /* ':1:5' T4 = T(4)*(1-LOE(4)); */ + /* ':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_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++) { + /* Gain: '/mapping_0_200' incorporates: + * Gain: '/ ' + * Gain: '/rads_to_RPM' + * Sqrt: '/Sqrt1' + * Sum: '/Sum3' + */ + u0 = (std::sqrt(116978.4923343994 * rtb_T_f[i]) * 9.5493 - 1250.0) * + 0.022857142857142857; + + /* Saturate: '/Saturation' */ + if (u0 > 200.0) { + tuning_GS1_Y.motor_command[i] = 200.0; + } else if (u0 < 0.0) { + tuning_GS1_Y.motor_command[i] = 0.0; + } else { + tuning_GS1_Y.motor_command[i] = u0; + } + + /* End of Saturate: '/Saturation' */ + } + + /* End of Outport: '/motor_command' */ + + /* Outport: '/virtual_control' */ + tuning_GS1_Y.virtual_control[0] = rtb_ref_idx_0; + tuning_GS1_Y.virtual_control[1] = rtb_ff_idx_0; + tuning_GS1_Y.virtual_control[2] = rtb_ff_idx_1; + tuning_GS1_Y.virtual_control[3] = rtb_ref_idx_3; + + /* MATLAB Function: '/MATLAB Function' incorporates: + * Inport: '/Y0' + * Inport: '/mode' + * Inport: '/ref' + */ + /* 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 */ + /* manual test */ + /* ':1:5' ref = ref_manual; */ + rtb_ref_idx_0 = tuning_GS1_U.ref[0]; + rtb_ff_idx_0 = tuning_GS1_U.ref[1]; + rtb_ff_idx_1 = tuning_GS1_U.ref[2]; + rtb_ref_idx_3 = tuning_GS1_U.ref[3]; + break; + + case 1: + /* ':1:6' case 1 % waypoint */ + /* waypoint */ + /* ':1:7' if t<=15 */ + if (rtb_Clock <= 15.0) { + /* ':1:8' ref = [Y0(1); Y0(2); 1; Y0(4)]; */ + rtb_ref_idx_0 = tuning_GS1_U.Y0[0]; + rtb_ff_idx_0 = tuning_GS1_U.Y0[1]; + rtb_ff_idx_1 = 1.0; + rtb_ref_idx_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)]; */ + rtb_ref_idx_0 = tuning_GS1_U.Y0[0] + 1.0; + rtb_ff_idx_0 = tuning_GS1_U.Y0[1] - 1.0; + rtb_ff_idx_1 = 1.0; + rtb_ref_idx_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)]; */ + rtb_ref_idx_0 = tuning_GS1_U.Y0[0] - 1.0; + rtb_ff_idx_0 = tuning_GS1_U.Y0[1] + 1.0; + rtb_ff_idx_1 = 1.0; + rtb_ref_idx_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)]; */ + rtb_ref_idx_0 = tuning_GS1_U.Y0[0] - 1.0; + rtb_ff_idx_0 = tuning_GS1_U.Y0[1] + 1.0; + rtb_ff_idx_1 = 1.0; + rtb_ref_idx_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)]; */ + rtb_ref_idx_0 = tuning_GS1_U.Y0[0]; + rtb_ff_idx_0 = tuning_GS1_U.Y0[1]; + rtb_ff_idx_1 = 1.0; + rtb_ref_idx_3 = tuning_GS1_U.Y0[3]; + } else { + /* ':1:17' else */ + /* ':1:18' ref = Y0; */ + rtb_ref_idx_0 = tuning_GS1_U.Y0[0]; + rtb_ff_idx_0 = tuning_GS1_U.Y0[1]; + rtb_ff_idx_1 = tuning_GS1_U.Y0[2]; + rtb_ref_idx_3 = tuning_GS1_U.Y0[3]; + } + + /* if t<=10 */ + /* ref = [Y0(1); Y0(2); 0.5; Y0(4)]; */ + /* elseif t <= 30 */ + /* ref = [Y0(1)-1.5; Y0(2); 0.5; Y0(4)]; */ + /* elseif t <= 50 */ + /* ref = [Y0(1)+1.5; Y0(2); 0.5; Y0(4)]; */ + /* elseif t <= 60 */ + /* ref = [Y0(1); Y0(2); 0.5; Y0(4)]; */ + /* else */ + /* ref = Y0; */ + /* end */ + break; + + case 2: + /* ':1:31' case 2 % circular tracking */ + /* circular tracking */ + /* ':1:32' if t<=10 */ + if (rtb_Clock <= 10.0) { + /* ':1:33' ref = [Y0(1); Y0(2); 0.75; Y0(4)]; */ + rtb_ref_idx_0 = tuning_GS1_U.Y0[0]; + rtb_ff_idx_0 = tuning_GS1_U.Y0[1]; + rtb_ff_idx_1 = 0.75; + rtb_ref_idx_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)]; */ + rtb_ref_idx_0 = tuning_GS1_U.Y0[0] + std::cos(rtb_Clock); + rtb_ff_idx_0 = tuning_GS1_U.Y0[1] + std::sin(rtb_Clock); + rtb_ff_idx_1 = 0.75; + rtb_ref_idx_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)]; */ + rtb_ref_idx_0 = tuning_GS1_U.Y0[0]; + rtb_ff_idx_0 = tuning_GS1_U.Y0[1]; + rtb_ff_idx_1 = 0.75; + rtb_ref_idx_3 = tuning_GS1_U.Y0[3]; + } else { + /* ':1:38' else */ + /* ':1:39' ref = Y0; */ + rtb_ref_idx_0 = tuning_GS1_U.Y0[0]; + rtb_ff_idx_0 = tuning_GS1_U.Y0[1]; + rtb_ff_idx_1 = tuning_GS1_U.Y0[2]; + rtb_ref_idx_3 = tuning_GS1_U.Y0[3]; + } + + /* ref = [cos(t); sin(t);min(t,15);Y0(4)]; */ + break; + + default: + /* ':1:42' otherwise */ + /* ':1:43' ref = Y0; */ + rtb_ref_idx_0 = tuning_GS1_U.Y0[0]; + rtb_ff_idx_0 = tuning_GS1_U.Y0[1]; + rtb_ff_idx_1 = tuning_GS1_U.Y0[2]; + rtb_ref_idx_3 = tuning_GS1_U.Y0[3]; + break; + } + + /* End of MATLAB Function: '/MATLAB Function' */ + + /* Outport: '/ref_out' */ + tuning_GS1_Y.ref_out[0] = rtb_ref_idx_0; + tuning_GS1_Y.ref_out[1] = rtb_ff_idx_0; + tuning_GS1_Y.ref_out[2] = rtb_ff_idx_1; + tuning_GS1_Y.ref_out[3] = rtb_ref_idx_3; + + /* Outport: '/LOE_true' */ + for (i = 0; i < 6; i++) { + tuning_GS1_Y.LOE_true[i] = rtb_LOE_out[i]; + } + + /* End of Outport: '/LOE_true' */ + if (rtmIsMajorTimeStep((&tuning_GS1_M)) && + (&tuning_GS1_M)->Timing.TaskCounters.TID[1] == 0) { + /* Delay: '/MemoryX' */ + for (i = 0; i < 6; i++) { + if (tuning_GS1_DW.icLoad != 0) { + tuning_GS1_DW.MemoryX_DSTATE[i] = 0.0; + } + + rtb_MemoryX[i] = tuning_GS1_DW.MemoryX_DSTATE[i]; + } + + /* Outputs for Atomic SubSystem: '/UseCurrentEstimator' */ + /* Outputs for Enabled SubSystem: '/Enabled Subsystem' incorporates: + * EnablePort: '/Enable' + */ + if (!tuning_GS1_DW.EnabledSubsystem_MODE) { + tuning_GS1_DW.EnabledSubsystem_MODE = true; + } + + /* Sum: '/Add1' incorporates: + * Constant: '/C' + * Delay: '/MemoryX' + * Inport: '/X' + * Product: '/Product' + * Product: '/Product2' + */ + for (i = 0; i < 3; i++) { + u0 = 0.0; + for (i_0 = 0; i_0 < 6; i_0++) { + u0 += tuning_GS1_ConstP.C_Value[3 * i_0 + i] * + tuning_GS1_DW.MemoryX_DSTATE[i_0]; + } + + rtb_Sum1_l[i] = tuning_GS1_U.X[9 + i] - u0; + } + + /* End of Sum: '/Add1' */ + for (i = 0; i < 6; i++) { + /* Product: '/Product2' incorporates: + * Constant: '/KalmanGainM' + */ + tuning_GS1_B.Product2[i] = 0.0; + tuning_GS1_B.Product2[i] += tuning_GS1_ConstP.KalmanGainM_Value[i] * + rtb_Sum1_l[0]; + tuning_GS1_B.Product2[i] += tuning_GS1_ConstP.KalmanGainM_Value[i + 6] * + rtb_Sum1_l[1]; + tuning_GS1_B.Product2[i] += tuning_GS1_ConstP.KalmanGainM_Value[i + 12] * + rtb_Sum1_l[2]; + + /* 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: '/UseCurrentEstimator' */ + + /* Product: '/Product1' incorporates: + * Constant: '/Constant1' + */ + for (i = 0; i < 3; i++) { + rtb_Sum1_l[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' */ + + /* Sum: '/Sum1' incorporates: + * Product: '/i x k' + * Product: '/j x i' + * Product: '/k x j' + * Product: '/i x j' + * Product: '/j x k' + * Product: '/k x i' + */ + rtb_Add_f[0] = rtb_Add_a[1] * rtb_Sum1_l[2]; + rtb_Add_f[1] = rtb_Add_a[2] * rtb_Sum1_l[0]; + rtb_Add_f[2] = rtb_Add_a[0] * rtb_Sum1_l[1]; + rtb_Add_f_0[0] = rtb_Add_a[2] * rtb_Sum1_l[1]; + rtb_Add_f_0[1] = rtb_Add_a[0] * rtb_Sum1_l[2]; + rtb_Add_f_0[2] = rtb_Add_a[1] * rtb_Sum1_l[0]; + + /* Sum: '/Sum3' incorporates: + * Constant: '/Constant' + * Constant: '/Constant1' + * Product: '/Product' + * Sum: '/Sum1' + */ + for (i = 0; i < 3; i++) { + rtb_Sum3_i[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_g[i]) + (rtb_Add_f[i] + - rtb_Add_f_0[i]); + } + + /* End of Sum: '/Sum3' */ + + /* MATLAB Function: '/MATLAB Function1' */ + /* MATLAB Function 'FDD_Kalman/MATLAB Function1': ':1' */ + /* ':1:2' arm = 0.215; */ + /* ':1:2' factor = 0.0365; */ + /* ':1:4' M = [arm/2 arm arm/2; */ + /* ':1:5' -sqrt(3)*arm/2 0 sqrt(3)*arm/2; */ + /* ':1:6' -factor factor -factor]; */ + /* ':1:8' diff = [u(1) - u(4) ; */ + /* ':1:9' u(2) - u(5) ; */ + /* ':1:10' u(3) - u(6) ]; */ + /* ':1:11' Residu = diff - M\y; */ + rtb_Sum1_l[1] = rtb_Sum3_i[0] - rtb_Sum3_i[1] * -0.57735026918962584; + rtb_Sum1_l[2] = (rtb_Sum3_i[2] - rtb_Sum3_i[1] * 0.19603055651554735) - + rtb_Sum1_l[1] * 0.16976744186046511; + rtb_Sum1_l[2] /= -0.10949999999999999; + rtb_Sum1_l[0] = rtb_Sum3_i[1] - rtb_Sum1_l[2] * 0.18619546181365429; + rtb_Sum1_l[1] -= rtb_Sum1_l[2] * 0.215; + rtb_Sum1_l[1] /= 0.215; + rtb_Sum1_l[0] -= rtb_Sum1_l[1] * 0.0; + rtb_Sum1_l[0] /= -0.18619546181365429; + + /* Outport: '/LOE13_estimated' incorporates: + * MATLAB Function: '/MATLAB Function1' + * Memory: '/Memory' + */ + /* Residu_1_4 */ + /* ':1:14' gamma = [Residu(1)/u(1) */ + /* ':1:15' Residu(2)/u(2) */ + /* ':1:16' Residu(3)/u(3)]; */ + tuning_GS1_Y.LOE13_estimated[0] = ((tuning_GS1_DW.Memory_PreviousInput[0] + - tuning_GS1_DW.Memory_PreviousInput[3]) - rtb_Sum1_l[0]) / + tuning_GS1_DW.Memory_PreviousInput[0]; + tuning_GS1_Y.LOE13_estimated[1] = ((tuning_GS1_DW.Memory_PreviousInput[1] + - tuning_GS1_DW.Memory_PreviousInput[4]) - rtb_Sum1_l[1]) / + tuning_GS1_DW.Memory_PreviousInput[1]; + tuning_GS1_Y.LOE13_estimated[2] = ((tuning_GS1_DW.Memory_PreviousInput[2] + - tuning_GS1_DW.Memory_PreviousInput[5]) - rtb_Sum1_l[2]) / + tuning_GS1_DW.Memory_PreviousInput[2]; + } + + for (i = 0; i < 6; i++) { + /* Outport: '/thrust_pre' */ + tuning_GS1_Y.thrust_pre[i] = tuning_GS1_B.u[i]; + + /* Outport: '/thrust_after' */ + tuning_GS1_Y.thrust_after[i] = rtb_T_f[i]; + } + + if (rtmIsMajorTimeStep((&tuning_GS1_M)) && + (&tuning_GS1_M)->Timing.TaskCounters.TID[1] == 0) { + /* Outport: '/acc_Kalman' */ + tuning_GS1_Y.acc_Kalman[0] = rtb_Add_a[3]; + + /* Outport: '/M_Kalman' */ + tuning_GS1_Y.M_Kalman[0] = rtb_Sum3_i[0]; + + /* Outport: '/vel_Kalman' */ + tuning_GS1_Y.vel_Kalman[0] = rtb_Add_a[0]; + + /* Outport: '/acc_Kalman' */ + tuning_GS1_Y.acc_Kalman[1] = rtb_Add_a[4]; + + /* Outport: '/M_Kalman' */ + tuning_GS1_Y.M_Kalman[1] = rtb_Sum3_i[1]; + + /* Outport: '/vel_Kalman' */ + tuning_GS1_Y.vel_Kalman[1] = rtb_Add_a[1]; + + /* Outport: '/acc_Kalman' */ + tuning_GS1_Y.acc_Kalman[2] = rtb_Add_a[5]; + + /* Outport: '/M_Kalman' */ + tuning_GS1_Y.M_Kalman[2] = rtb_Sum3_i[2]; + + /* Outport: '/vel_Kalman' */ + tuning_GS1_Y.vel_Kalman[2] = rtb_Add_a[2]; + + /* Outputs for Enabled SubSystem: '/MeasurementUpdate' incorporates: + * EnablePort: '/Enable' + */ + if (rtmIsMajorTimeStep((&tuning_GS1_M)) && + (!tuning_GS1_DW.MeasurementUpdate_MODE)) { + tuning_GS1_DW.MeasurementUpdate_MODE = true; + } + + if (tuning_GS1_DW.MeasurementUpdate_MODE) { + for (i = 0; i < 3; i++) { + /* Product: '/C[k]*xhat[k|k-1]' incorporates: + * Constant: '/C' + * Sum: '/Add1' + */ + rtb_Sum1_l[i] = 0.0; + for (i_0 = 0; i_0 < 6; i_0++) { + rtb_Sum1_l[i] += tuning_GS1_ConstP.C_Value[3 * i_0 + i] * + rtb_MemoryX[i_0]; + } + + /* End of Product: '/C[k]*xhat[k|k-1]' */ + + /* Sum: '/Sum' incorporates: + * Inport: '/X' + * Product: '/Product3' + * Sum: '/Add1' + */ + rtb_Add_f[i] = tuning_GS1_U.X[9 + i] - rtb_Sum1_l[i]; + } + + /* Product: '/Product3' incorporates: + * Constant: '/KalmanGainL' + */ + for (i = 0; i < 6; i++) { + tuning_GS1_B.Product3[i] = 0.0; + tuning_GS1_B.Product3[i] += tuning_GS1_ConstP.KalmanGainL_Value[i] * + rtb_Add_f[0]; + tuning_GS1_B.Product3[i] += tuning_GS1_ConstP.KalmanGainL_Value[i + 6] + * rtb_Add_f[1]; + tuning_GS1_B.Product3[i] += tuning_GS1_ConstP.KalmanGainL_Value[i + 12] + * rtb_Add_f[2]; + } + } + + /* End of Outputs for SubSystem: '/MeasurementUpdate' */ + for (i = 0; i < 6; i++) { + /* Product: '/A[k]*xhat[k|k-1]' incorporates: + * Constant: '/A' + * Sum: '/Add' + */ + rtb_LOE_out[i] = 0.0; + for (i_0 = 0; i_0 < 6; i_0++) { + rtb_LOE_out[i] += tuning_GS1_ConstP.A_Value[6 * i_0 + i] * + rtb_MemoryX[i_0]; + } + + /* End of Product: '/A[k]*xhat[k|k-1]' */ + + /* Sum: '/Add' */ + rtb_Add[i] = rtb_LOE_out[i] + tuning_GS1_B.Product3[i]; + } + } + + /* Sum: '/Sum' incorporates: + * Inport: '/Y0' + */ + rtb_ref_idx_0 -= tuning_GS1_U.Y0[0]; + rtb_ff_idx_0 -= tuning_GS1_U.Y0[1]; + rtb_ff_idx_1 -= tuning_GS1_U.Y0[2]; + rtb_Clock = rtb_ref_idx_3 - tuning_GS1_U.Y0[3]; + + /* Saturate: '/x' */ + if (rtb_ref_idx_0 > 2.0) { + rtb_ref_idx_0 = 2.0; + } else { + if (rtb_ref_idx_0 < -2.0) { + rtb_ref_idx_0 = -2.0; + } + } + + /* End of Saturate: '/x' */ + + /* Sum: '/Sum1' */ + tuning_GS1_B.Sum1 = rtb_ref_idx_0 - rtb_d_x; + + /* Saturate: '/y' */ + if (rtb_ff_idx_0 > 2.0) { + rtb_ff_idx_0 = 2.0; + } else { + if (rtb_ff_idx_0 < -2.0) { + rtb_ff_idx_0 = -2.0; + } + } + + /* End of Saturate: '/y' */ + + /* Sum: '/Sum4' */ + tuning_GS1_B.Sum4 = rtb_ff_idx_0 - rtb_d_y; + + /* Saturate: '/yaw' */ + if (rtb_Clock > 3.1415926535897931) { + rtb_Clock = 3.1415926535897931; + } else { + if (rtb_Clock < -3.1415926535897931) { + rtb_Clock = -3.1415926535897931; + } + } + + /* End of Saturate: '/yaw' */ + + /* Sum: '/Sum3' */ + tuning_GS1_B.Sum3_n = rtb_Clock - rtb_d_psi; + + /* Saturate: '/z' */ + if (rtb_ff_idx_1 > 1.75) { + rtb_ff_idx_1 = 1.75; + } else { + if (rtb_ff_idx_1 < 0.0) { + rtb_ff_idx_1 = 0.0; + } + } + + /* End of Saturate: '/z' */ + + /* Sum: '/Sum3' */ + tuning_GS1_B.Sum3_h = rtb_ff_idx_1 - rtb_d_z; + } + + if (rtmIsMajorTimeStep((&tuning_GS1_M))) { + int32_T i; + if (rtmIsMajorTimeStep((&tuning_GS1_M)) && + (&tuning_GS1_M)->Timing.TaskCounters.TID[1] == 0) { + /* Update for Delay: '/MemoryX' */ + tuning_GS1_DW.icLoad = 0U; + for (i = 0; i < 6; i++) { + /* Update for Memory: '/Memory' */ + tuning_GS1_DW.Memory_PreviousInput[i] = tuning_GS1_B.u[i]; + + /* Update for Delay: '/MemoryX' */ + tuning_GS1_DW.MemoryX_DSTATE[i] = rtb_Add[i]; + } + } + } /* end MajorTimeStep */ + + if (rtmIsMajorTimeStep((&tuning_GS1_M))) { + rt_ertODEUpdateContinuousStates(&(&tuning_GS1_M)->solverInfo); + + /* Update absolute time for base rate */ + /* The "clockTick0" counts the number of times the code of this task has + * been executed. The absolute time is the multiplication of "clockTick0" + * and "Timing.stepSize0". Size of "clockTick0" ensures timer will not + * overflow during the application lifespan selected. + * Timer of this task consists of two 32 bit unsigned integers. + * The two integers represent the low bits Timing.clockTick0 and the high bits + * Timing.clockTickH0. When the low bit overflows to 0, the high bits increment. + */ + if (!(++(&tuning_GS1_M)->Timing.clockTick0)) { + ++(&tuning_GS1_M)->Timing.clockTickH0; + } + + (&tuning_GS1_M)->Timing.t[0] = rtsiGetSolverStopTime(&(&tuning_GS1_M) + ->solverInfo); + + { + /* Update absolute timer for sample time: [0.001s, 0.0s] */ + /* The "clockTick1" counts the number of times the code of this task has + * been executed. The resolution of this integer timer is 0.001, which is the step size + * of the task. Size of "clockTick1" ensures timer will not overflow during the + * application lifespan selected. + * Timer of this task consists of two 32 bit unsigned integers. + * The two integers represent the low bits Timing.clockTick1 and the high bits + * Timing.clockTickH1. When the low bit overflows to 0, the high bits increment. + */ + (&tuning_GS1_M)->Timing.clockTick1++; + if (!(&tuning_GS1_M)->Timing.clockTick1) { + (&tuning_GS1_M)->Timing.clockTickH1++; + } + } + + rate_scheduler((&tuning_GS1_M)); + } /* end MajorTimeStep */ +} + +/* Derivatives for root system: '' */ +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_h; + + /* Derivatives for Integrator: '/Integrator1' */ + _rtXdot->Integrator1_CSTATE_d = tuning_GS1_B.Sum1; + + /* Derivatives for Integrator: '/Integrator' */ + _rtXdot->Integrator_CSTATE = tuning_GS1_B.Sum4; + + /* Derivatives for Integrator: '/Integrator1' */ + _rtXdot->Integrator1_CSTATE_j = tuning_GS1_B.Sum3_n; +} + +/* Model initialize function */ +void tuning_GS1ModelClass::initialize() +{ + /* Registration code */ + + /* initialize real-time model */ + (void) memset((void *)(&tuning_GS1_M), 0, + sizeof(RT_MODEL_tuning_GS1_T)); + + { + /* Setup solver object */ + rtsiSetSimTimeStepPtr(&(&tuning_GS1_M)->solverInfo, &(&tuning_GS1_M) + ->Timing.simTimeStep); + rtsiSetTPtr(&(&tuning_GS1_M)->solverInfo, &rtmGetTPtr((&tuning_GS1_M))); + rtsiSetStepSizePtr(&(&tuning_GS1_M)->solverInfo, &(&tuning_GS1_M) + ->Timing.stepSize0); + rtsiSetdXPtr(&(&tuning_GS1_M)->solverInfo, &(&tuning_GS1_M)->derivs); + rtsiSetContStatesPtr(&(&tuning_GS1_M)->solverInfo, (real_T **) + &(&tuning_GS1_M)->contStates); + rtsiSetNumContStatesPtr(&(&tuning_GS1_M)->solverInfo, &(&tuning_GS1_M) + ->Sizes.numContStates); + rtsiSetNumPeriodicContStatesPtr(&(&tuning_GS1_M)->solverInfo, + &(&tuning_GS1_M)->Sizes.numPeriodicContStates); + rtsiSetPeriodicContStateIndicesPtr(&(&tuning_GS1_M)->solverInfo, + &(&tuning_GS1_M)->periodicContStateIndices); + rtsiSetPeriodicContStateRangesPtr(&(&tuning_GS1_M)->solverInfo, + &(&tuning_GS1_M)->periodicContStateRanges); + rtsiSetErrorStatusPtr(&(&tuning_GS1_M)->solverInfo, (&rtmGetErrorStatus + ((&tuning_GS1_M)))); + rtsiSetRTModelPtr(&(&tuning_GS1_M)->solverInfo, (&tuning_GS1_M)); + } + + rtsiSetSimTimeStep(&(&tuning_GS1_M)->solverInfo, MAJOR_TIME_STEP); + (&tuning_GS1_M)->intgData.y = (&tuning_GS1_M)->odeY; + (&tuning_GS1_M)->intgData.f[0] = (&tuning_GS1_M)->odeF[0]; + (&tuning_GS1_M)->intgData.f[1] = (&tuning_GS1_M)->odeF[1]; + (&tuning_GS1_M)->intgData.f[2] = (&tuning_GS1_M)->odeF[2]; + (&tuning_GS1_M)->intgData.f[3] = (&tuning_GS1_M)->odeF[3]; + getRTM()->contStates = ((X_tuning_GS1_T *) &tuning_GS1_X); + rtsiSetSolverData(&(&tuning_GS1_M)->solverInfo, (void *)&(&tuning_GS1_M) + ->intgData); + rtsiSetSolverName(&(&tuning_GS1_M)->solverInfo,"ode4"); + rtmSetTPtr(getRTM(), &(&tuning_GS1_M)->Timing.tArray[0]); + (&tuning_GS1_M)->Timing.stepSize0 = 0.001; + + /* block I/O */ + (void) memset(((void *) &tuning_GS1_B), 0, + sizeof(B_tuning_GS1_T)); + + /* states (continuous) */ + { + (void) memset((void *)&tuning_GS1_X, 0, + sizeof(X_tuning_GS1_T)); + } + + /* states (dwork) */ + (void) memset((void *)&tuning_GS1_DW, 0, + sizeof(DW_tuning_GS1_T)); + + /* external inputs */ + (void)memset((void *)&tuning_GS1_U, 0, sizeof(ExtU_tuning_GS1_T)); + + /* external outputs */ + (void) memset((void *)&tuning_GS1_Y, 0, + sizeof(ExtY_tuning_GS1_T)); + + { + int32_T i; + + /* InitializeConditions for Integrator: '/Integrator1' */ + tuning_GS1_X.Integrator1_CSTATE = 0.0; + + /* InitializeConditions for Integrator: '/Integrator1' */ + tuning_GS1_X.Integrator1_CSTATE_d = 0.0; + + /* InitializeConditions for Integrator: '/Integrator' */ + tuning_GS1_X.Integrator_CSTATE = 0.0; + + /* InitializeConditions for Integrator: '/Integrator1' */ + tuning_GS1_X.Integrator1_CSTATE_j = 0.0; + + /* InitializeConditions for Delay: '/MemoryX' */ + tuning_GS1_DW.icLoad = 1U; + + /* SystemInitialize for Enabled SubSystem: '/MeasurementUpdate' */ + /* SystemInitialize for Atomic SubSystem: '/UseCurrentEstimator' */ + /* 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' */ + tuning_GS1_B.Product2[i] = 0.0; + + /* 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: '/UseCurrentEstimator' */ + /* End of SystemInitialize for SubSystem: '/MeasurementUpdate' */ + } +} + +/* Model terminate function */ +void tuning_GS1ModelClass::terminate() +{ + /* (no terminate code required) */ +} + +/* Constructor */ +tuning_GS1ModelClass::tuning_GS1ModelClass() +{ +} + +/* Destructor */ +tuning_GS1ModelClass::~tuning_GS1ModelClass() +{ + /* Currently there is no destructor body generated.*/ +} + +/* Real-Time Model get method */ +RT_MODEL_tuning_GS1_T * tuning_GS1ModelClass::getRTM() +{ + return (&tuning_GS1_M); +} diff --git a/src/tuning_GS1_grt_rtw/tuning_GS1.h b/src/tuning_GS1_grt_rtw/tuning_GS1.h new file mode 100644 index 0000000..3d0ce34 --- /dev/null +++ b/src/tuning_GS1_grt_rtw/tuning_GS1.h @@ -0,0 +1,537 @@ +/* + * tuning_GS1.h + * + * Student License - for use by students to meet course requirements and + * perform academic research at degree granting institutions only. Not + * for government, commercial, or other organizational use. + * + * Code generation for model "tuning_GS1". + * + * Model version : 1.1498 + * Simulink Coder version : 8.12 (R2017a) 16-Feb-2017 + * C++ source code generated on : Tue Sep 18 10:42:41 2018 + * + * Target selection: grt.tlc + * Note: GRT includes extra infrastructure and instrumentation for prototyping + * Embedded hardware selection: 32-bit Generic + * Code generation objective: Execution efficiency + * Validation result: Passed (1), Warnings (3), Error (0) + */ + +#ifndef RTW_HEADER_tuning_GS1_h_ +#define RTW_HEADER_tuning_GS1_h_ +#include +#include +#ifndef tuning_GS1_COMMON_INCLUDES_ +# define tuning_GS1_COMMON_INCLUDES_ +#include "rtwtypes.h" +#include "rtw_continuous.h" +#include "rtw_solver.h" +#endif /* tuning_GS1_COMMON_INCLUDES_ */ + +#include "tuning_GS1_types.h" + +/* Shared type includes */ +#include "multiword_types.h" + +/* Macros for accessing real-time model data structure */ +#ifndef rtmGetBlkStateChangeFlag +# define rtmGetBlkStateChangeFlag(rtm) ((rtm)->blkStateChange) +#endif + +#ifndef rtmSetBlkStateChangeFlag +# define rtmSetBlkStateChangeFlag(rtm, val) ((rtm)->blkStateChange = (val)) +#endif + +#ifndef rtmGetContStateDisabled +# define rtmGetContStateDisabled(rtm) ((rtm)->contStateDisabled) +#endif + +#ifndef rtmSetContStateDisabled +# define rtmSetContStateDisabled(rtm, val) ((rtm)->contStateDisabled = (val)) +#endif + +#ifndef rtmGetContStates +# define rtmGetContStates(rtm) ((rtm)->contStates) +#endif + +#ifndef rtmSetContStates +# define rtmSetContStates(rtm, val) ((rtm)->contStates = (val)) +#endif + +#ifndef rtmGetDerivCacheNeedsReset +# define rtmGetDerivCacheNeedsReset(rtm) ((rtm)->derivCacheNeedsReset) +#endif + +#ifndef rtmSetDerivCacheNeedsReset +# define rtmSetDerivCacheNeedsReset(rtm, val) ((rtm)->derivCacheNeedsReset = (val)) +#endif + +#ifndef rtmGetIntgData +# define rtmGetIntgData(rtm) ((rtm)->intgData) +#endif + +#ifndef rtmSetIntgData +# define rtmSetIntgData(rtm, val) ((rtm)->intgData = (val)) +#endif + +#ifndef rtmGetOdeF +# define rtmGetOdeF(rtm) ((rtm)->odeF) +#endif + +#ifndef rtmSetOdeF +# define rtmSetOdeF(rtm, val) ((rtm)->odeF = (val)) +#endif + +#ifndef rtmGetOdeY +# define rtmGetOdeY(rtm) ((rtm)->odeY) +#endif + +#ifndef rtmSetOdeY +# define rtmSetOdeY(rtm, val) ((rtm)->odeY = (val)) +#endif + +#ifndef rtmGetPeriodicContStateIndices +# define rtmGetPeriodicContStateIndices(rtm) ((rtm)->periodicContStateIndices) +#endif + +#ifndef rtmSetPeriodicContStateIndices +# define rtmSetPeriodicContStateIndices(rtm, val) ((rtm)->periodicContStateIndices = (val)) +#endif + +#ifndef rtmGetPeriodicContStateRanges +# define rtmGetPeriodicContStateRanges(rtm) ((rtm)->periodicContStateRanges) +#endif + +#ifndef rtmSetPeriodicContStateRanges +# define rtmSetPeriodicContStateRanges(rtm, val) ((rtm)->periodicContStateRanges = (val)) +#endif + +#ifndef rtmGetZCCacheNeedsReset +# define rtmGetZCCacheNeedsReset(rtm) ((rtm)->zCCacheNeedsReset) +#endif + +#ifndef rtmSetZCCacheNeedsReset +# define rtmSetZCCacheNeedsReset(rtm, val) ((rtm)->zCCacheNeedsReset = (val)) +#endif + +#ifndef rtmGetdX +# define rtmGetdX(rtm) ((rtm)->derivs) +#endif + +#ifndef rtmSetdX +# define rtmSetdX(rtm, val) ((rtm)->derivs = (val)) +#endif + +#ifndef rtmGetErrorStatus +# define rtmGetErrorStatus(rtm) ((rtm)->errorStatus) +#endif + +#ifndef rtmSetErrorStatus +# define rtmSetErrorStatus(rtm, val) ((rtm)->errorStatus = (val)) +#endif + +#ifndef rtmGetStopRequested +# define rtmGetStopRequested(rtm) ((rtm)->Timing.stopRequestedFlag) +#endif + +#ifndef rtmSetStopRequested +# define rtmSetStopRequested(rtm, val) ((rtm)->Timing.stopRequestedFlag = (val)) +#endif + +#ifndef rtmGetStopRequestedPtr +# define rtmGetStopRequestedPtr(rtm) (&((rtm)->Timing.stopRequestedFlag)) +#endif + +#ifndef rtmGetT +# define rtmGetT(rtm) (rtmGetTPtr((rtm))[0]) +#endif + +/* Block signals (auto storage) */ +typedef struct { + real_T Sum2; /* '/Sum2' */ + real_T Sum3; /* '/Sum3' */ + real_T T_outer[3]; /* '/T_outer' */ + real_T Fcn1; /* '/Fcn1' */ + real_T Fcn; /* '/Fcn' */ + real_T u[6]; /* '/ ' */ + real_T Sum1; /* '/Sum1' */ + real_T Sum4; /* '/Sum4' */ + real_T Sum3_n; /* '/Sum3' */ + real_T Sum3_h; /* '/Sum3' */ + real_T Product2[6]; /* '/Product2' */ + real_T Product3[6]; /* '/Product3' */ +} B_tuning_GS1_T; + +/* Block states (auto storage) for system '' */ +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' */ +} DW_tuning_GS1_T; + +/* Continuous states (auto storage) */ +typedef struct { + real_T Integrator1_CSTATE; /* '/Integrator1' */ + real_T Integrator1_CSTATE_d; /* '/Integrator1' */ + real_T Integrator_CSTATE; /* '/Integrator' */ + real_T Integrator1_CSTATE_j; /* '/Integrator1' */ +} X_tuning_GS1_T; + +/* State derivatives (auto storage) */ +typedef struct { + real_T Integrator1_CSTATE; /* '/Integrator1' */ + real_T Integrator1_CSTATE_d; /* '/Integrator1' */ + real_T Integrator_CSTATE; /* '/Integrator' */ + real_T Integrator1_CSTATE_j; /* '/Integrator1' */ +} XDot_tuning_GS1_T; + +/* State disabled */ +typedef struct { + boolean_T Integrator1_CSTATE; /* '/Integrator1' */ + boolean_T Integrator1_CSTATE_d; /* '/Integrator1' */ + boolean_T Integrator_CSTATE; /* '/Integrator' */ + boolean_T Integrator1_CSTATE_j; /* '/Integrator1' */ +} XDis_tuning_GS1_T; + +#ifndef ODE4_INTG +#define ODE4_INTG + +/* ODE4 Integration Data */ +typedef struct { + real_T *y; /* output */ + real_T *f[4]; /* derivatives */ +} ODE4_IntgData; + +#endif + +/* Constant parameters (auto storage) */ +typedef struct { + /* Expression: B_ENU_inv + * Referenced by: '/Control Allocation' + */ + real_T ControlAllocation_Gain[24]; + + /* Expression: M_bias' + * Referenced by: '/Constant' + */ + real_T Constant_Value_g[3]; + + /* Expression: Ib + * Referenced by: '/Constant1' + */ + real_T Constant1_Value[9]; + + /* Expression: pInitialization.M + * Referenced by: '/KalmanGainM' + */ + real_T KalmanGainM_Value[18]; + + /* Expression: pInitialization.C + * Referenced by: '/C' + */ + real_T C_Value[18]; + + /* Expression: pInitialization.A + * Referenced by: '/A' + */ + real_T A_Value[36]; + + /* Expression: pInitialization.L + * Referenced by: '/KalmanGainL' + */ + real_T KalmanGainL_Value[18]; +} ConstP_tuning_GS1_T; + +/* External inputs (root inport signals with auto storage) */ +typedef struct { + real_T X[12]; /* '/X' */ + real_T Y0[4]; /* '/Y0' */ + real_T mode; /* '/mode' */ + real_T ref[4]; /* '/ref' */ + real_T LOE_t[6]; /* '/LOE_t' */ + real_T LOE_a[6]; /* '/LOE_a' */ + real_T gain[18]; /* '/gain' */ +} ExtU_tuning_GS1_T; + +/* External outputs (root outports fed by signals with auto storage) */ +typedef struct { + real_T motor_command[6]; /* '/motor_command' */ + real_T virtual_control[4]; /* '/virtual_control' */ + real_T ref_out[4]; /* '/ref_out' */ + real_T LOE_true[6]; /* '/LOE_true' */ + real_T LOE13_estimated[3]; /* '/LOE13_estimated' */ + real_T thrust_pre[6]; /* '/thrust_pre' */ + real_T thrust_after[6]; /* '/thrust_after' */ + real_T acc_Kalman[3]; /* '/acc_Kalman' */ + real_T M_Kalman[3]; /* '/M_Kalman' */ + real_T vel_Kalman[3]; /* '/vel_Kalman' */ +} ExtY_tuning_GS1_T; + +/* Real-time Model Data Structure */ +struct tag_RTM_tuning_GS1_T { + const char_T *errorStatus; + RTWSolverInfo solverInfo; + X_tuning_GS1_T *contStates; + int_T *periodicContStateIndices; + real_T *periodicContStateRanges; + real_T *derivs; + boolean_T *contStateDisabled; + boolean_T zCCacheNeedsReset; + boolean_T derivCacheNeedsReset; + boolean_T blkStateChange; + real_T odeY[4]; + real_T odeF[4][4]; + ODE4_IntgData intgData; + + /* + * Sizes: + * The following substructure contains sizes information + * for many of the model attributes such as inputs, outputs, + * dwork, sample times, etc. + */ + struct { + int_T numContStates; + int_T numPeriodicContStates; + int_T numSampTimes; + } Sizes; + + /* + * Timing: + * The following substructure contains information regarding + * the timing information for the model. + */ + struct { + uint32_T clockTick0; + uint32_T clockTickH0; + time_T stepSize0; + uint32_T clockTick1; + uint32_T clockTickH1; + struct { + uint8_T TID[3]; + } TaskCounters; + + SimTimeStep simTimeStep; + boolean_T stopRequestedFlag; + time_T *t; + time_T tArray[3]; + } Timing; +}; + +#ifdef __cplusplus + +extern "C" { + +#endif + +#ifdef __cplusplus + +} +#endif + +/* Constant parameters (auto storage) */ +extern const ConstP_tuning_GS1_T tuning_GS1_ConstP; + +/* Class declaration for model tuning_GS1 */ +class tuning_GS1ModelClass { + /* public data and function members */ + public: + /* External inputs */ + ExtU_tuning_GS1_T tuning_GS1_U; + + /* External outputs */ + ExtY_tuning_GS1_T tuning_GS1_Y; + + /* model initialize function */ + void initialize(); + + /* model step function */ + void step(); + + /* model terminate function */ + void terminate(); + + /* Constructor */ + tuning_GS1ModelClass(); + + /* Destructor */ + ~tuning_GS1ModelClass(); + + /* Real-Time Model get method */ + RT_MODEL_tuning_GS1_T * getRTM(); + + /* private data and function members */ + private: + /* Block signals */ + B_tuning_GS1_T tuning_GS1_B; + + /* Block states */ + DW_tuning_GS1_T tuning_GS1_DW; + X_tuning_GS1_T tuning_GS1_X; /* Block continuous states */ + + /* Real-Time Model */ + RT_MODEL_tuning_GS1_T tuning_GS1_M; + + /* Continuous states update member function*/ + void rt_ertODEUpdateContinuousStates(RTWSolverInfo *si ); + + /* Derivatives member function */ + void tuning_GS1_derivatives(); +}; + +/*- + * These blocks were eliminated from the model due to optimizations: + * + * 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 '/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 + * 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 '/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 + * Block '/N' : Unused code path elimination + * 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 '/Reset' : Unused code path elimination + * Block '/Reshapeyhat' : 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 '/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 '/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 '/Rate Transition' : Eliminated since input and output rates are identical + * Block '/Reshape' : Reshape block reduction + * Block '/Reshape1' : Reshape block reduction + * Block '/Reshape' : Reshape block reduction + * Block '/Reshape' : Reshape block reduction + * Block '/Reshape' : Reshape block reduction + * Block '/Reshape' : Reshape block reduction + */ + +/*- + * The generated code includes comments that allow you to trace directly + * back to the appropriate location in the model. The basic format + * is /block_name, where system is the system number (uniquely + * assigned by Simulink) and block_name is the name of the block. + * + * Use the MATLAB hilite_system command to trace the generated code back + * to the model. For example, + * + * hilite_system('') - opens system 3 + * hilite_system('/Kp') - opens and selects block Kp which resides in S3 + * + * Here is the system hierarchy for this model + * + * '' : 'tuning_GS1' + * '' : 'tuning_GS1/Actuator_Fault' + * '' : 'tuning_GS1/FDD_Kalman' + * '' : 'tuning_GS1/Kalman Filter' + * '' : 'tuning_GS1/Test_config_and_data' + * '' : 'tuning_GS1/Thrust2command' + * '' : 'tuning_GS1/XY State Feedback' + * '' : 'tuning_GS1/pitch_controller_SF' + * '' : 'tuning_GS1/roll_controller_SF' + * '' : 'tuning_GS1/yaw_controller' + * '' : 'tuning_GS1/z_controller' + * '' : 'tuning_GS1/FDD_Kalman/Cross Product' + * '' : 'tuning_GS1/FDD_Kalman/MATLAB Function1' + * '' : 'tuning_GS1/FDD_Kalman/Cross Product/Subsystem' + * '' : 'tuning_GS1/FDD_Kalman/Cross Product/Subsystem2' + * '' : '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/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/tunning_nominal_grt_rtw/tunning_nominal_data.cpp b/src/tuning_GS1_grt_rtw/tuning_GS1_data.cpp similarity index 90% rename from src/tunning_nominal_grt_rtw/tunning_nominal_data.cpp rename to src/tuning_GS1_grt_rtw/tuning_GS1_data.cpp index dde9c05..d9bc483 100644 --- a/src/tunning_nominal_grt_rtw/tunning_nominal_data.cpp +++ b/src/tuning_GS1_grt_rtw/tuning_GS1_data.cpp @@ -1,15 +1,15 @@ /* - * tunning_nominal_data.cpp + * tuning_GS1_data.cpp * * Student License - for use by students to meet course requirements and * perform academic research at degree granting institutions only. Not * for government, commercial, or other organizational use. * - * Code generation for model "tunning_nominal". + * Code generation for model "tuning_GS1". * * Model version : 1.1498 * Simulink Coder version : 8.12 (R2017a) 16-Feb-2017 - * C++ source code generated on : Fri Aug 31 14:28:54 2018 + * C++ source code generated on : Tue Sep 18 10:42:41 2018 * * Target selection: grt.tlc * Note: GRT includes extra infrastructure and instrumentation for prototyping @@ -18,11 +18,11 @@ * Validation result: Passed (1), Warnings (3), Error (0) */ -#include "tunning_nominal.h" -#include "tunning_nominal_private.h" +#include "tuning_GS1.h" +#include "tuning_GS1_private.h" /* Constant parameters (auto storage) */ -const ConstP_tunning_nominal_T tunning_nominal_ConstP = { +const ConstP_tuning_GS1_T tuning_GS1_ConstP = { /* Expression: B_ENU_inv * Referenced by: '/Control Allocation' */ diff --git a/src/tunning_nominal_grt_rtw/tunning_nominal_private.h b/src/tuning_GS1_grt_rtw/tuning_GS1_private.h similarity index 76% rename from src/tunning_nominal_grt_rtw/tunning_nominal_private.h rename to src/tuning_GS1_grt_rtw/tuning_GS1_private.h index 398f862..75e991c 100644 --- a/src/tunning_nominal_grt_rtw/tunning_nominal_private.h +++ b/src/tuning_GS1_grt_rtw/tuning_GS1_private.h @@ -1,15 +1,15 @@ /* - * tunning_nominal_private.h + * tuning_GS1_private.h * * Student License - for use by students to meet course requirements and * perform academic research at degree granting institutions only. Not * for government, commercial, or other organizational use. * - * Code generation for model "tunning_nominal". + * Code generation for model "tuning_GS1". * * Model version : 1.1498 * Simulink Coder version : 8.12 (R2017a) 16-Feb-2017 - * C++ source code generated on : Fri Aug 31 14:28:54 2018 + * C++ source code generated on : Tue Sep 18 10:42:41 2018 * * Target selection: grt.tlc * Note: GRT includes extra infrastructure and instrumentation for prototyping @@ -18,8 +18,8 @@ * Validation result: Passed (1), Warnings (3), Error (0) */ -#ifndef RTW_HEADER_tunning_nominal_private_h_ -#define RTW_HEADER_tunning_nominal_private_h_ +#ifndef RTW_HEADER_tuning_GS1_private_h_ +#define RTW_HEADER_tuning_GS1_private_h_ #include "rtwtypes.h" #include "multiword_types.h" @@ -41,6 +41,6 @@ #endif /* private model entry point functions */ -extern void tunning_nominal_derivatives(); +extern void tuning_GS1_derivatives(); -#endif /* RTW_HEADER_tunning_nominal_private_h_ */ +#endif /* RTW_HEADER_tuning_GS1_private_h_ */ diff --git a/src/tunning_nominal_grt_rtw/tunning_nominal_types.h b/src/tuning_GS1_grt_rtw/tuning_GS1_types.h similarity index 62% rename from src/tunning_nominal_grt_rtw/tunning_nominal_types.h rename to src/tuning_GS1_grt_rtw/tuning_GS1_types.h index 25eb989..3726d33 100644 --- a/src/tunning_nominal_grt_rtw/tunning_nominal_types.h +++ b/src/tuning_GS1_grt_rtw/tuning_GS1_types.h @@ -1,15 +1,15 @@ /* - * tunning_nominal_types.h + * tuning_GS1_types.h * * Student License - for use by students to meet course requirements and * perform academic research at degree granting institutions only. Not * for government, commercial, or other organizational use. * - * Code generation for model "tunning_nominal". + * Code generation for model "tuning_GS1". * * Model version : 1.1498 * Simulink Coder version : 8.12 (R2017a) 16-Feb-2017 - * C++ source code generated on : Fri Aug 31 14:28:54 2018 + * C++ source code generated on : Tue Sep 18 10:42:41 2018 * * Target selection: grt.tlc * Note: GRT includes extra infrastructure and instrumentation for prototyping @@ -18,10 +18,10 @@ * Validation result: Passed (1), Warnings (3), Error (0) */ -#ifndef RTW_HEADER_tunning_nominal_types_h_ -#define RTW_HEADER_tunning_nominal_types_h_ +#ifndef RTW_HEADER_tuning_GS1_types_h_ +#define RTW_HEADER_tuning_GS1_types_h_ /* Forward declaration for rtModel */ -typedef struct tag_RTM_tunning_nominal_T RT_MODEL_tunning_nominal_T; +typedef struct tag_RTM_tuning_GS1_T RT_MODEL_tuning_GS1_T; -#endif /* RTW_HEADER_tunning_nominal_types_h_ */ +#endif /* RTW_HEADER_tuning_GS1_types_h_ */ diff --git a/src/gs1_node.cpp b/src/tuning_GS1_node.cpp similarity index 72% rename from src/gs1_node.cpp rename to src/tuning_GS1_node.cpp index eaf33fd..27279f3 100644 --- a/src/gs1_node.cpp +++ b/src/tuning_GS1_node.cpp @@ -15,12 +15,13 @@ #include #include -#include +#include +#include #include #include -tunning_nominalModelClass gController; +tuning_GS1ModelClass gController; bool gPublish; bool gInit_flag; @@ -36,6 +37,7 @@ Eigen::VectorXd gRef(4); // references (x, y, z, yaw) Eigen::VectorXd gGain(19); Eigen::VectorXd gLOE(6); Eigen::VectorXd gLOE_t(6); +Eigen::VectorXd gMotor_measurement(6); // Eigen::VectorXd gAng_acc_calcul(3); // derivative of angular velocity double gPsi; // heading (rad) @@ -69,6 +71,12 @@ void OdometryCallback(const nav_msgs::Odometry::ConstPtr &odom) { //if (gLanding_flag || gInit) } +void MotorSpeedCallback(const asctec_hl_comm::MotorSpeed::ConstPtr &motor) { + for (unsigned int i = 0; i <6; ++i) { + gMotor_measurement[i] = motor->motor_speed[i]; + } +} + void controller_dyn_callback(gsft_control::controllerDynConfig &config, uint32_t level) { if (config.RESET) { // TO DO : reset parameters, gains @@ -100,7 +108,6 @@ void controller_dyn_callback(gsft_control::controllerDynConfig &config, uint32_t gGain[16] = config.kr; gGain[17] = config.kipsi; - gGain[18] = config.kffx; ROS_INFO("New controller gains"); config.new_controller_gains = false; @@ -144,7 +151,6 @@ void controller_dyn_callback(gsft_control::controllerDynConfig &config, uint32_t gGain[16] = config.kr; gGain[17] = config.kipsi; - gGain[18] = config.kffx; gLOE[0] = config.LOE_1; gLOE[1] = config.LOE_2; @@ -204,14 +210,17 @@ void timmerCallback(const ros::TimerEvent&) } int main(int argc, char** argv) { - ros::init(argc, argv, "tunning_nominal_node"); + ros::init(argc, argv, "tuning_GS1_node"); ros::NodeHandle nh; ros::NodeHandle pnh("~"); - ROS_INFO("tunning_nominal_node main started"); + ROS_INFO("tuning_GS1_node main started"); ros::Subscriber odometry_sub_; odometry_sub_ = nh.subscribe(mav_msgs::default_topics::ODOMETRY, 1, OdometryCallback); + ros::Subscriber motor_speed_measurement_sub_; + motor_speed_measurement_sub_ = nh.subscribe(mav_msgs::default_topics::MOTOR_MEASUREMENT, 1, MotorSpeedCallback); + /*ros::Subscriber lost_control_sub_; lost_control_sub_ = nh.subscribe(gsft_control::default_topics::LOE, 1, LostControlCallback);*/ @@ -277,10 +286,10 @@ int main(int argc, char** argv) { control_actived = true; for (unsigned int i=0; i< 6; i++) { - gController.tunning_nominal_U.LOE_a[i] = gLOE[i]; // fault amplitude + gController.tuning_GS1_U.LOE_a[i] = gLOE[i]; // fault amplitude } for (unsigned int i=0; i< 6; i++) { - gController.tunning_nominal_U.LOE_t[i] = gLOE_t[i]; // fault time + 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]); @@ -289,32 +298,32 @@ int main(int argc, char** argv) { if (control_actived) { // controller active after take-off request // Initialization before Step - gController.tunning_nominal_U.mode = gTest_mode; + gController.tuning_GS1_U.mode = gTest_mode; for (unsigned int i=0; i< 4; i++) { - gController.tunning_nominal_U.ref[i] = gRef[i]; + gController.tuning_GS1_U.ref[i] = gRef[i]; } for (unsigned int i=0; i< 4; i++) { - gController.tunning_nominal_U.Y0[i] = gY0[i]; + gController.tuning_GS1_U.Y0[i] = gY0[i]; } for (unsigned int i=0; i< 19; i++) { - gController.tunning_nominal_U.gain[i] = gGain[i]; + gController.tuning_GS1_U.gain[i] = gGain[i]; } - gController.tunning_nominal_U.X[0] = gOdometry.position_W.x(); - gController.tunning_nominal_U.X[1] = gOdometry.position_W.y(); - gController.tunning_nominal_U.X[2] = gOdometry.position_W.z(); - gController.tunning_nominal_U.X[3 ] = velocity_W.x(); - gController.tunning_nominal_U.X[4 ] = velocity_W.y(); - gController.tunning_nominal_U.X[5 ] = velocity_W.z(); - gController.tunning_nominal_U.X[6 ] = phi; - gController.tunning_nominal_U.X[7 ] = theta; - gController.tunning_nominal_U.X[8 ] = gPsi; - gController.tunning_nominal_U.X[9 ] = gOdometry.angular_velocity_B.x(); - gController.tunning_nominal_U.X[10] = gOdometry.angular_velocity_B.y(); - gController.tunning_nominal_U.X[11] = gOdometry.angular_velocity_B.z(); - /*gController.tunning_nominal_U.X[12] = gAng_acc_calcul[0]; - gController.tunning_nominal_U.X[13] = gAng_acc_calcul[1]; - gController.tunning_nominal_U.X[14] = gAng_acc_calcul[2]; */ + gController.tuning_GS1_U.X[0] = gOdometry.position_W.x(); + gController.tuning_GS1_U.X[1] = gOdometry.position_W.y(); + gController.tuning_GS1_U.X[2] = gOdometry.position_W.z(); + gController.tuning_GS1_U.X[3 ] = velocity_W.x(); + gController.tuning_GS1_U.X[4 ] = velocity_W.y(); + gController.tuning_GS1_U.X[5 ] = velocity_W.z(); + gController.tuning_GS1_U.X[6 ] = phi; + gController.tuning_GS1_U.X[7 ] = theta; + gController.tuning_GS1_U.X[8 ] = gPsi; + gController.tuning_GS1_U.X[9 ] = gOdometry.angular_velocity_B.x(); + gController.tuning_GS1_U.X[10] = gOdometry.angular_velocity_B.y(); + gController.tuning_GS1_U.X[11] = gOdometry.angular_velocity_B.z(); + /*gController.tuning_GS1_U.X[12] = gAng_acc_calcul[0]; + gController.tuning_GS1_U.X[13] = gAng_acc_calcul[1]; + gController.tuning_GS1_U.X[14] = gAng_acc_calcul[2]; */ // Run Matlab controller gController.step(); @@ -329,7 +338,7 @@ int main(int argc, char** argv) { } else { - motor_command[i] = gController.tunning_nominal_Y.motor_command[i]; // normalized [1 .. 200] => Asctec Firefly + motor_command[i] = gController.tuning_GS1_Y.motor_command[i]; // normalized [1 .. 200] => Asctec Firefly motor_RPM[i] = 1250.0 + motor_command[i]*43.75; // real RPM motor_speed[i] = motor_RPM[i]/9.5493; // rad/s => Gazebo } @@ -356,6 +365,10 @@ int main(int argc, char** argv) { actuator_msg->header.stamp = ros::Time::now(); motor_velocity_reference_pub_.publish(actuator_msg); + if (gController.tuning_GS1_Y.ref_out[2] <= 0.25 && gTest_mode > 0){ + gLanding_flag = true; + } + } else { // control_actived = false ( before take-off) mav_msgs::ActuatorsPtr actuator_msg(new mav_msgs::Actuators); actuator_msg->angular_velocities.clear(); @@ -371,7 +384,7 @@ int main(int argc, char** argv) { if (gEmergency_status) { - ROS_ERROR("tunning_nominal_node emergency status"); + ROS_ERROR("tuning_GS1_node emergency status"); ROS_INFO("x = %f, y = %f, z = %f",gOdometry.position_W.x(),gOdometry.position_W.y(),gOdometry.position_W.z()); ros::Duration(0.5).sleep(); gController.terminate(); @@ -390,10 +403,10 @@ int main(int argc, char** argv) { // Publish data: UAV state in World frame if (gPublish){ gsft_control::UAVStatePtr uav_state_msg(new gsft_control::UAVState); - uav_state_msg->position_ref.x = gController.tunning_nominal_Y.ref_out[0]; // gRef only for manual test - uav_state_msg->position_ref.y = gController.tunning_nominal_Y.ref_out[1]; - uav_state_msg->position_ref.z = gController.tunning_nominal_Y.ref_out[2]; - uav_state_msg->heading_ref = gController.tunning_nominal_Y.ref_out[3]; + uav_state_msg->position_ref.x = gController.tuning_GS1_Y.ref_out[0]; // gRef only for manual test + uav_state_msg->position_ref.y = gController.tuning_GS1_Y.ref_out[1]; + uav_state_msg->position_ref.z = gController.tuning_GS1_Y.ref_out[2]; + uav_state_msg->heading_ref = gController.tuning_GS1_Y.ref_out[3]; uav_state_msg->position_W.x = gOdometry.position_W.x(); uav_state_msg->position_W.y = gOdometry.position_W.y(); @@ -407,48 +420,37 @@ int main(int argc, char** argv) { uav_state_msg->rotation_speed_B.x = gOdometry.angular_velocity_B.x(); uav_state_msg->rotation_speed_B.y = gOdometry.angular_velocity_B.y(); uav_state_msg->rotation_speed_B.z = gOdometry.angular_velocity_B.z(); - uav_state_msg->total_thrust = gController.tunning_nominal_Y.virtual_control[0]; - uav_state_msg->moment.x = gController.tunning_nominal_Y.virtual_control[1]; - uav_state_msg->moment.y = gController.tunning_nominal_Y.virtual_control[2]; - uav_state_msg->moment.z = gController.tunning_nominal_Y.virtual_control[3]; - - - uav_state_msg->LOE13_estimated.x = gController.tunning_nominal_Y.LOE13_estimated[0]; - uav_state_msg->LOE13_estimated.y = gController.tunning_nominal_Y.LOE13_estimated[1]; - uav_state_msg->LOE13_estimated.z = gController.tunning_nominal_Y.LOE13_estimated[2]; - uav_state_msg->LOE13_true.x = gController.tunning_nominal_Y.LOE_true[0]; - uav_state_msg->LOE13_true.y = gController.tunning_nominal_Y.LOE_true[1]; - uav_state_msg->LOE13_true.z = gController.tunning_nominal_Y.LOE_true[2]; - - /* uav_state_msg->acc_calcul.x = gAng_acc_calcul[0]; - uav_state_msg->acc_calcul.y = gAng_acc_calcul[1]; - uav_state_msg->acc_calcul.z = gAng_acc_calcul[2]; - uav_state_msg->acc_Kalman.x = gController.tunning_nominal_Y.acc_Kalman[0]; - uav_state_msg->acc_Kalman.y = gController.tunning_nominal_Y.acc_Kalman[1]; - uav_state_msg->acc_Kalman.z = gController.tunning_nominal_Y.acc_Kalman[2]; - - uav_state_msg->M_calcul.x = gController.tunning_nominal_Y.M_calcul[0]; - uav_state_msg->M_calcul.y = gController.tunning_nominal_Y.M_calcul[1]; - uav_state_msg->M_calcul.z = gController.tunning_nominal_Y.M_calcul[2]; - uav_state_msg->M_Kalman.x = gController.tunning_nominal_Y.M_Kalman[0]; - uav_state_msg->M_Kalman.y = gController.tunning_nominal_Y.M_Kalman[1]; - uav_state_msg->M_Kalman.z = gController.tunning_nominal_Y.M_Kalman[2]; - - uav_state_msg->LOE13_calcul.x = gController.tunning_nominal_Y.LOE13_calcul[0]; - uav_state_msg->LOE13_calcul.y = gController.tunning_nominal_Y.LOE13_calcul[1]; - uav_state_msg->LOE13_calcul.z = gController.tunning_nominal_Y.LOE13_calcul[2]; - uav_state_msg->LOE13_Kalman.x = gController.tunning_nominal_Y.LOE13_Kalman[0]; - uav_state_msg->LOE13_Kalman.y = gController.tunning_nominal_Y.LOE13_Kalman[1]; - uav_state_msg->LOE13_Kalman.z = gController.tunning_nominal_Y.LOE13_Kalman[2]; - - - uav_state_msg->thrust_pre.x = gController.tunning_nominal_Y.thrust_pre[0]; - uav_state_msg->thrust_pre.y = gController.tunning_nominal_Y.thrust_pre[1]; - uav_state_msg->thrust_pre.z = gController.tunning_nominal_Y.thrust_pre[2]; - - uav_state_msg->thrust_filtered.x = gController.tunning_nominal_Y.thrust_filtered[0]; - uav_state_msg->thrust_filtered.y = gController.tunning_nominal_Y.thrust_filtered[1]; - uav_state_msg->thrust_filtered.z = gController.tunning_nominal_Y.thrust_filtered[2]; */ + 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->LOE13_estimated.x = gController.tuning_GS1_Y.LOE13_estimated[0]; + uav_state_msg->LOE13_estimated.y = gController.tuning_GS1_Y.LOE13_estimated[1]; + uav_state_msg->LOE13_estimated.z = gController.tuning_GS1_Y.LOE13_estimated[2]; + uav_state_msg->LOE13_true.x = gController.tuning_GS1_Y.LOE_true[0]; + uav_state_msg->LOE13_true.y = gController.tuning_GS1_Y.LOE_true[1]; + uav_state_msg->LOE13_true.z = gController.tuning_GS1_Y.LOE_true[2]; + + 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]; + uav_state_msg->thrust_pre.z = gController.tuning_GS1_Y.thrust_pre[2]; + + uav_state_msg->thrust_after.x = gController.tuning_GS1_Y.thrust_after[0]; + uav_state_msg->thrust_after.y = gController.tuning_GS1_Y.thrust_after[1]; + uav_state_msg->thrust_after.z = gController.tuning_GS1_Y.thrust_after[2]; + + uav_state_msg->vel_Kalman.x = gController.tuning_GS1_Y.vel_Kalman[0]; + uav_state_msg->vel_Kalman.y = gController.tuning_GS1_Y.vel_Kalman[1]; + uav_state_msg->vel_Kalman.z = gController.tuning_GS1_Y.vel_Kalman[2]; + + uav_state_msg->acc_Kalman.x = gController.tuning_GS1_Y.acc_Kalman[0]; + uav_state_msg->acc_Kalman.y = gController.tuning_GS1_Y.acc_Kalman[1]; + uav_state_msg->acc_Kalman.z = gController.tuning_GS1_Y.acc_Kalman[2]; + + uav_state_msg->M_Kalman.x = gController.tuning_GS1_Y.M_Kalman[0]; + uav_state_msg->M_Kalman.y = gController.tuning_GS1_Y.M_Kalman[1]; + uav_state_msg->M_Kalman.z = gController.tuning_GS1_Y.M_Kalman[2]; uav_state_msg->header.stamp = ros::Time::now(); uav_state_pub_.publish(uav_state_msg); diff --git a/src/tunning_lqr_grt_rtw/multiword_types.h b/src/tuning_lqr_grt_rtw/multiword_types.h similarity index 100% rename from src/tunning_lqr_grt_rtw/multiword_types.h rename to src/tuning_lqr_grt_rtw/multiword_types.h diff --git a/src/tunning_lqr_grt_rtw/rtmodel.h b/src/tuning_lqr_grt_rtw/rtmodel.h similarity index 100% rename from src/tunning_lqr_grt_rtw/rtmodel.h rename to src/tuning_lqr_grt_rtw/rtmodel.h diff --git a/src/tunning_lqr_grt_rtw/rtwtypes.h b/src/tuning_lqr_grt_rtw/rtwtypes.h similarity index 100% rename from src/tunning_lqr_grt_rtw/rtwtypes.h rename to src/tuning_lqr_grt_rtw/rtwtypes.h diff --git a/src/tunning_lqr_grt_rtw/tunning_lqr.cpp b/src/tuning_lqr_grt_rtw/tuning_lqr.cpp similarity index 55% rename from src/tunning_lqr_grt_rtw/tunning_lqr.cpp rename to src/tuning_lqr_grt_rtw/tuning_lqr.cpp index b793b60..beee512 100644 --- a/src/tunning_lqr_grt_rtw/tunning_lqr.cpp +++ b/src/tuning_lqr_grt_rtw/tuning_lqr.cpp @@ -1,11 +1,11 @@ /* - * tunning_lqr.cpp + * tuning_lqr.cpp * * Student License - for use by students to meet course requirements and * perform academic research at degree granting institutions only. Not * for government, commercial, or other organizational use. * - * Code generation for model "tunning_lqr". + * Code generation for model "tuning_lqr". * * Model version : 1.1142 * Simulink Coder version : 8.12 (R2017a) 16-Feb-2017 @@ -18,25 +18,25 @@ * Validation result: Passed (1), Warnings (3), Error (0) */ -#include "tunning_lqr.h" -#include "tunning_lqr_private.h" +#include "tuning_lqr.h" +#include "tuning_lqr_private.h" -static void rate_scheduler(RT_MODEL_tunning_lqr_T *const tunning_lqr_M); +static void rate_scheduler(RT_MODEL_tuning_lqr_T *const tuning_lqr_M); /* * This function updates active task flag for each subrate. * The function is called at model base rate, hence the * generated code self-manages all its subrates. */ -static void rate_scheduler(RT_MODEL_tunning_lqr_T *const tunning_lqr_M) +static void rate_scheduler(RT_MODEL_tuning_lqr_T *const tuning_lqr_M) { /* Compute which subrates run during the next base time step. Subrates * are an integer multiple of the base rate counter. Therefore, the subtask * counter is reset when it reaches its limit (zero means run). */ - (tunning_lqr_M->Timing.TaskCounters.TID[2])++; - if ((tunning_lqr_M->Timing.TaskCounters.TID[2]) > 4) {/* Sample time: [0.005s, 0.0s] */ - tunning_lqr_M->Timing.TaskCounters.TID[2] = 0; + (tuning_lqr_M->Timing.TaskCounters.TID[2])++; + if ((tuning_lqr_M->Timing.TaskCounters.TID[2]) > 4) {/* Sample time: [0.005s, 0.0s] */ + tuning_lqr_M->Timing.TaskCounters.TID[2] = 0; } } @@ -44,7 +44,7 @@ static void rate_scheduler(RT_MODEL_tunning_lqr_T *const tunning_lqr_M) * This function updates continuous states using the ODE4 fixed-step * solver algorithm */ -void tunning_lqrModelClass::rt_ertODEUpdateContinuousStates(RTWSolverInfo *si ) +void tuning_lqrModelClass::rt_ertODEUpdateContinuousStates(RTWSolverInfo *si ) { time_T t = rtsiGetT(si); time_T tnew = rtsiGetSolverStopTime(si); @@ -68,7 +68,7 @@ void tunning_lqrModelClass::rt_ertODEUpdateContinuousStates(RTWSolverInfo *si ) /* Assumes that rtsiSetT and ModelOutputs are up-to-date */ /* f0 = f(t,y) */ rtsiSetdX(si, f0); - tunning_lqr_derivatives(); + tuning_lqr_derivatives(); /* f1 = f(t + (h/2), y + (h/2)*f0) */ temp = 0.5 * h; @@ -79,7 +79,7 @@ void tunning_lqrModelClass::rt_ertODEUpdateContinuousStates(RTWSolverInfo *si ) rtsiSetT(si, t + temp); rtsiSetdX(si, f1); this->step(); - tunning_lqr_derivatives(); + tuning_lqr_derivatives(); /* f2 = f(t + (h/2), y + (h/2)*f1) */ for (i = 0; i < nXc; i++) { @@ -88,7 +88,7 @@ void tunning_lqrModelClass::rt_ertODEUpdateContinuousStates(RTWSolverInfo *si ) rtsiSetdX(si, f2); this->step(); - tunning_lqr_derivatives(); + tuning_lqr_derivatives(); /* f3 = f(t + h, y + h*f2) */ for (i = 0; i < nXc; i++) { @@ -98,7 +98,7 @@ void tunning_lqrModelClass::rt_ertODEUpdateContinuousStates(RTWSolverInfo *si ) rtsiSetT(si, tnew); rtsiSetdX(si, f3); this->step(); - tunning_lqr_derivatives(); + tuning_lqr_derivatives(); /* tnew = t + h ynew = y + (h/6)*(f0 + 2*f1 + 2*f2 + 2*f3) */ @@ -111,7 +111,7 @@ void tunning_lqrModelClass::rt_ertODEUpdateContinuousStates(RTWSolverInfo *si ) } /* Model step function */ -void tunning_lqrModelClass::step() +void tuning_lqrModelClass::step() { real_T rtb_Sum3_b; real_T rtb_Sum1_g; @@ -122,40 +122,40 @@ void tunning_lqrModelClass::step() real_T rtb_ff_idx_0; real_T rtb_ff_idx_1; real_T u0; - if (rtmIsMajorTimeStep((&tunning_lqr_M))) { + if (rtmIsMajorTimeStep((&tuning_lqr_M))) { /* set solver stop time */ - if (!((&tunning_lqr_M)->Timing.clockTick0+1)) { - rtsiSetSolverStopTime(&(&tunning_lqr_M)->solverInfo, (((&tunning_lqr_M) - ->Timing.clockTickH0 + 1) * (&tunning_lqr_M)->Timing.stepSize0 * + if (!((&tuning_lqr_M)->Timing.clockTick0+1)) { + rtsiSetSolverStopTime(&(&tuning_lqr_M)->solverInfo, (((&tuning_lqr_M) + ->Timing.clockTickH0 + 1) * (&tuning_lqr_M)->Timing.stepSize0 * 4294967296.0)); } else { - rtsiSetSolverStopTime(&(&tunning_lqr_M)->solverInfo, (((&tunning_lqr_M) - ->Timing.clockTick0 + 1) * (&tunning_lqr_M)->Timing.stepSize0 + - (&tunning_lqr_M)->Timing.clockTickH0 * (&tunning_lqr_M) + rtsiSetSolverStopTime(&(&tuning_lqr_M)->solverInfo, (((&tuning_lqr_M) + ->Timing.clockTick0 + 1) * (&tuning_lqr_M)->Timing.stepSize0 + + (&tuning_lqr_M)->Timing.clockTickH0 * (&tuning_lqr_M) ->Timing.stepSize0 * 4294967296.0)); } } /* end MajorTimeStep */ /* Update absolute time of base rate at minor time step */ - if (rtmIsMinorTimeStep((&tunning_lqr_M))) { - (&tunning_lqr_M)->Timing.t[0] = rtsiGetT(&(&tunning_lqr_M)->solverInfo); + if (rtmIsMinorTimeStep((&tuning_lqr_M))) { + (&tuning_lqr_M)->Timing.t[0] = rtsiGetT(&(&tuning_lqr_M)->solverInfo); } /* Sum: '/Sum5' incorporates: * Inport: '/X' * Inport: '/Y0' */ - tunning_lqr_B.d_z = tunning_lqr_U.X[2] - tunning_lqr_U.Y0[2]; + tuning_lqr_B.d_z = tuning_lqr_U.X[2] - tuning_lqr_U.Y0[2]; /* RateTransition: '/Rate Transition ' incorporates: * Inport: '/X' */ - if ((rtmIsMajorTimeStep((&tunning_lqr_M)) && - (&tunning_lqr_M)->Timing.TaskCounters.TID[1] == 0) && (rtmIsMajorTimeStep - ((&tunning_lqr_M)) && - (&tunning_lqr_M)->Timing.TaskCounters.TID[2] == 0)) { - tunning_lqr_B.d_z_k = tunning_lqr_B.d_z; - tunning_lqr_B.vz = tunning_lqr_U.X[5]; + if ((rtmIsMajorTimeStep((&tuning_lqr_M)) && + (&tuning_lqr_M)->Timing.TaskCounters.TID[1] == 0) && (rtmIsMajorTimeStep + ((&tuning_lqr_M)) && + (&tuning_lqr_M)->Timing.TaskCounters.TID[2] == 0)) { + tuning_lqr_B.d_z_k = tuning_lqr_B.d_z; + tuning_lqr_B.vz = tuning_lqr_U.X[5]; } /* Sum: '/Sum1' incorporates: @@ -165,30 +165,30 @@ void tunning_lqrModelClass::step() * Product: '/Product1' * SignalConversion: '/TmpSignal ConversionAtProduct1Inport2' */ - tunning_lqr_B.Sum1 = tunning_lqr_U.gain[8] * tunning_lqr_X.Integrator1_CSTATE - - (tunning_lqr_U.gain[6] * tunning_lqr_B.d_z_k + tunning_lqr_U.gain[7] * - tunning_lqr_B.vz); - if (rtmIsMajorTimeStep((&tunning_lqr_M)) && - (&tunning_lqr_M)->Timing.TaskCounters.TID[2] == 0) { + tuning_lqr_B.Sum1 = tuning_lqr_U.gain[8] * tuning_lqr_X.Integrator1_CSTATE + - (tuning_lqr_U.gain[6] * tuning_lqr_B.d_z_k + tuning_lqr_U.gain[7] * + tuning_lqr_B.vz); + if (rtmIsMajorTimeStep((&tuning_lqr_M)) && + (&tuning_lqr_M)->Timing.TaskCounters.TID[2] == 0) { /* ZeroOrderHold: '/ ' */ - tunning_lqr_B.dT = tunning_lqr_B.Sum1; + tuning_lqr_B.dT = tuning_lqr_B.Sum1; } /* Sum: '/Sum1' incorporates: * Inport: '/X' * Inport: '/Y0' */ - tunning_lqr_B.d_x = tunning_lqr_U.X[0] - tunning_lqr_U.Y0[0]; + tuning_lqr_B.d_x = tuning_lqr_U.X[0] - tuning_lqr_U.Y0[0]; /* RateTransition: '/Rate Transition ' incorporates: * Inport: '/X' */ - if ((rtmIsMajorTimeStep((&tunning_lqr_M)) && - (&tunning_lqr_M)->Timing.TaskCounters.TID[1] == 0) && (rtmIsMajorTimeStep - ((&tunning_lqr_M)) && - (&tunning_lqr_M)->Timing.TaskCounters.TID[2] == 0)) { - tunning_lqr_B.d_x_b = tunning_lqr_B.d_x; - tunning_lqr_B.vx = tunning_lqr_U.X[3]; + if ((rtmIsMajorTimeStep((&tuning_lqr_M)) && + (&tuning_lqr_M)->Timing.TaskCounters.TID[1] == 0) && (rtmIsMajorTimeStep + ((&tuning_lqr_M)) && + (&tuning_lqr_M)->Timing.TaskCounters.TID[2] == 0)) { + tuning_lqr_B.d_x_b = tuning_lqr_B.d_x; + tuning_lqr_B.vx = tuning_lqr_U.X[3]; } /* Sum: '/Sum1' incorporates: @@ -198,31 +198,31 @@ void tunning_lqrModelClass::step() * Product: '/Product1' * SignalConversion: '/TmpSignal ConversionAtProduct1Inport2' */ - rtb_Sum3_b = tunning_lqr_U.gain[2] * tunning_lqr_X.Integrator1_CSTATE_h - - (tunning_lqr_U.gain[0] * tunning_lqr_B.d_x_b + tunning_lqr_U.gain[1] * - tunning_lqr_B.vx); + rtb_Sum3_b = tuning_lqr_U.gain[2] * tuning_lqr_X.Integrator1_CSTATE_h - + (tuning_lqr_U.gain[0] * tuning_lqr_B.d_x_b + tuning_lqr_U.gain[1] * + tuning_lqr_B.vx); /* Sum: '/Sum4' incorporates: * Inport: '/X' * Inport: '/Y0' */ - tunning_lqr_B.d_y = tunning_lqr_U.X[1] - tunning_lqr_U.Y0[1]; + tuning_lqr_B.d_y = tuning_lqr_U.X[1] - tuning_lqr_U.Y0[1]; /* RateTransition: '/Rate Transition ' incorporates: * Inport: '/X' * RateTransition: '/Rate Transition ' */ - if (rtmIsMajorTimeStep((&tunning_lqr_M)) && - (&tunning_lqr_M)->Timing.TaskCounters.TID[1] == 0) { - if (rtmIsMajorTimeStep((&tunning_lqr_M)) && - (&tunning_lqr_M)->Timing.TaskCounters.TID[2] == 0) { - tunning_lqr_B.d_y_l = tunning_lqr_B.d_y; - tunning_lqr_B.vy = tunning_lqr_U.X[4]; + if (rtmIsMajorTimeStep((&tuning_lqr_M)) && + (&tuning_lqr_M)->Timing.TaskCounters.TID[1] == 0) { + if (rtmIsMajorTimeStep((&tuning_lqr_M)) && + (&tuning_lqr_M)->Timing.TaskCounters.TID[2] == 0) { + tuning_lqr_B.d_y_l = tuning_lqr_B.d_y; + tuning_lqr_B.vy = tuning_lqr_U.X[4]; } - if (rtmIsMajorTimeStep((&tunning_lqr_M)) && - (&tunning_lqr_M)->Timing.TaskCounters.TID[2] == 0) { - tunning_lqr_B.RateTransition = tunning_lqr_U.X[8]; + if (rtmIsMajorTimeStep((&tuning_lqr_M)) && + (&tuning_lqr_M)->Timing.TaskCounters.TID[2] == 0) { + tuning_lqr_B.RateTransition = tuning_lqr_U.X[8]; } } @@ -233,12 +233,12 @@ void tunning_lqrModelClass::step() * Product: '/Product1' * SignalConversion: '/TmpSignal ConversionAtProduct1Inport2' */ - rtb_Sum1_g = tunning_lqr_U.gain[5] * tunning_lqr_X.Integrator1_CSTATE_g - - (tunning_lqr_U.gain[3] * tunning_lqr_B.d_y_l + tunning_lqr_U.gain[4] * - tunning_lqr_B.vy); + rtb_Sum1_g = tuning_lqr_U.gain[5] * tuning_lqr_X.Integrator1_CSTATE_g - + (tuning_lqr_U.gain[3] * tuning_lqr_B.d_y_l + tuning_lqr_U.gain[4] * + tuning_lqr_B.vy); /* Clock: '/Clock' */ - rtb_Clock = (&tunning_lqr_M)->Timing.t[0]; + rtb_Clock = (&tuning_lqr_M)->Timing.t[0]; /* MATLAB Function: '/FFW' incorporates: * Inport: '/X' @@ -252,7 +252,7 @@ void tunning_lqrModelClass::step() /* ':1:3' g = 9.81; */ /* [x;y] = [cos(t); sin(t)] */ /* ':1:5' if (test_mode == 2) */ - if ((tunning_lqr_U.mode == 2.0) && ((rtb_Clock >= 10.0) && (rtb_Clock <= 50.0))) + if ((tuning_lqr_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); */ @@ -261,15 +261,15 @@ void tunning_lqrModelClass::step() /* ':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:13' ff = [-ayref_b/g; axref_b/g]; */ - rtb_ff_idx_0 = -((std::sin(tunning_lqr_U.X[6]) * std::sin(tunning_lqr_U.X[7]) - * std::cos(tunning_lqr_U.X[8]) - std::cos(tunning_lqr_U.X - [6]) * std::sin(tunning_lqr_U.X[8])) * -std::cos(rtb_Clock) + (std::sin - (tunning_lqr_U.X[6]) * std::sin(tunning_lqr_U.X[7]) * std::sin - (tunning_lqr_U.X[8]) + std::cos(tunning_lqr_U.X[6]) * std::cos - (tunning_lqr_U.X[8])) * -std::sin(rtb_Clock)) / 9.81; - rtb_ff_idx_1 = (std::cos(tunning_lqr_U.X[7]) * std::cos(tunning_lqr_U.X[8]) * - -std::cos(rtb_Clock) + std::cos(tunning_lqr_U.X[7]) * std:: - sin(tunning_lqr_U.X[8]) * -std::sin(rtb_Clock)) / 9.81; + rtb_ff_idx_0 = -((std::sin(tuning_lqr_U.X[6]) * std::sin(tuning_lqr_U.X[7]) + * std::cos(tuning_lqr_U.X[8]) - std::cos(tuning_lqr_U.X + [6]) * std::sin(tuning_lqr_U.X[8])) * -std::cos(rtb_Clock) + (std::sin + (tuning_lqr_U.X[6]) * std::sin(tuning_lqr_U.X[7]) * std::sin + (tuning_lqr_U.X[8]) + std::cos(tuning_lqr_U.X[6]) * std::cos + (tuning_lqr_U.X[8])) * -std::sin(rtb_Clock)) / 9.81; + rtb_ff_idx_1 = (std::cos(tuning_lqr_U.X[7]) * std::cos(tuning_lqr_U.X[8]) * + -std::cos(rtb_Clock) + std::cos(tuning_lqr_U.X[7]) * std:: + sin(tuning_lqr_U.X[8]) * -std::sin(rtb_Clock)) / 9.81; } else { /* ':1:14' else */ /* ':1:15' ff = [0;0]; */ @@ -280,43 +280,43 @@ void tunning_lqrModelClass::step() /* Sum: '/Sum7' incorporates: * Fcn: '/Fcn1' */ - tunning_lqr_B.Sum7 = (rtb_Sum1_g * std::cos(tunning_lqr_B.RateTransition) + - rtb_Sum3_b * std::sin(tunning_lqr_B.RateTransition)) + + tuning_lqr_B.Sum7 = (rtb_Sum1_g * std::cos(tuning_lqr_B.RateTransition) + + rtb_Sum3_b * std::sin(tuning_lqr_B.RateTransition)) + rtb_ff_idx_0; /* Fcn: '/Fcn' */ - rtb_Sum3_b = -rtb_Sum1_g * std::sin(tunning_lqr_B.RateTransition) + rtb_Sum3_b - * std::cos(tunning_lqr_B.RateTransition); + rtb_Sum3_b = -rtb_Sum1_g * std::sin(tuning_lqr_B.RateTransition) + rtb_Sum3_b + * std::cos(tuning_lqr_B.RateTransition); /* Sum: '/Sum8' */ - tunning_lqr_B.Sum8 = rtb_Sum3_b + rtb_ff_idx_1; + tuning_lqr_B.Sum8 = rtb_Sum3_b + rtb_ff_idx_1; /* ZeroOrderHold: '/ ' incorporates: * ZeroOrderHold: '/ ' */ - if (rtmIsMajorTimeStep((&tunning_lqr_M)) && - (&tunning_lqr_M)->Timing.TaskCounters.TID[2] == 0) { - tunning_lqr_B.u = tunning_lqr_B.Sum7; + if (rtmIsMajorTimeStep((&tuning_lqr_M)) && + (&tuning_lqr_M)->Timing.TaskCounters.TID[2] == 0) { + tuning_lqr_B.u = tuning_lqr_B.Sum7; /* Saturate: '/roll' */ - if (tunning_lqr_B.u > 0.52359877559829882) { - tunning_lqr_B.roll = 0.52359877559829882; - } else if (tunning_lqr_B.u < -0.52359877559829882) { - tunning_lqr_B.roll = -0.52359877559829882; + if (tuning_lqr_B.u > 0.52359877559829882) { + tuning_lqr_B.roll = 0.52359877559829882; + } else if (tuning_lqr_B.u < -0.52359877559829882) { + tuning_lqr_B.roll = -0.52359877559829882; } else { - tunning_lqr_B.roll = tunning_lqr_B.u; + tuning_lqr_B.roll = tuning_lqr_B.u; } /* End of Saturate: '/roll' */ - tunning_lqr_B.u_c = tunning_lqr_B.Sum8; + tuning_lqr_B.u_c = tuning_lqr_B.Sum8; /* Saturate: '/pitch' */ - if (tunning_lqr_B.u_c > 0.52359877559829882) { - tunning_lqr_B.pitch = 0.52359877559829882; - } else if (tunning_lqr_B.u_c < -0.52359877559829882) { - tunning_lqr_B.pitch = -0.52359877559829882; + if (tuning_lqr_B.u_c > 0.52359877559829882) { + tuning_lqr_B.pitch = 0.52359877559829882; + } else if (tuning_lqr_B.u_c < -0.52359877559829882) { + tuning_lqr_B.pitch = -0.52359877559829882; } else { - tunning_lqr_B.pitch = tunning_lqr_B.u_c; + tuning_lqr_B.pitch = tuning_lqr_B.u_c; } /* End of Saturate: '/pitch' */ @@ -328,7 +328,7 @@ void tunning_lqrModelClass::step() * Inport: '/X' * Inport: '/Y0' */ - rtb_Sum3_b = tunning_lqr_U.X[8] - tunning_lqr_U.Y0[3]; + rtb_Sum3_b = tuning_lqr_U.X[8] - tuning_lqr_U.Y0[3]; /* Sum: '/Sum2' incorporates: * Inport: '/X' @@ -345,13 +345,13 @@ void tunning_lqrModelClass::step() * Sum: '/Sum1' * Sum: '/Sum1' */ - rtb_Sum1_g = tunning_lqr_B.roll - (tunning_lqr_U.gain[9] * tunning_lqr_U.X[6] - + tunning_lqr_U.gain[10] * tunning_lqr_U.X[9]); - rtb_ff_idx_0 = tunning_lqr_B.pitch - (tunning_lqr_U.gain[12] * - tunning_lqr_U.X[7] + tunning_lqr_U.gain[13] * tunning_lqr_U.X[10]); - rtb_ff_idx_1 = tunning_lqr_U.gain[17] * tunning_lqr_X.Integrator1_CSTATE_j - - (tunning_lqr_U.gain[15] * rtb_Sum3_b + tunning_lqr_U.gain[16] * - tunning_lqr_U.X[11]); + rtb_Sum1_g = tuning_lqr_B.roll - (tuning_lqr_U.gain[9] * tuning_lqr_U.X[6] + + tuning_lqr_U.gain[10] * tuning_lqr_U.X[9]); + rtb_ff_idx_0 = tuning_lqr_B.pitch - (tuning_lqr_U.gain[12] * + tuning_lqr_U.X[7] + tuning_lqr_U.gain[13] * tuning_lqr_U.X[10]); + rtb_ff_idx_1 = tuning_lqr_U.gain[17] * tuning_lqr_X.Integrator1_CSTATE_j - + (tuning_lqr_U.gain[15] * rtb_Sum3_b + tuning_lqr_U.gain[16] * + tuning_lqr_U.X[11]); /* MATLAB Function 'Test_config_and_data/LOE_': ':1' */ /* ':1:2' LOE_out = [0;0;0;0;0;0]; */ @@ -361,11 +361,11 @@ void tunning_lqrModelClass::step() * Saturate: '/ ' * Sum: '/Sum2' */ - u0 = tunning_lqr_ConstP.ControlAllocation_Gain[i + 18] * rtb_ff_idx_1 + - (tunning_lqr_ConstP.ControlAllocation_Gain[i + 12] * rtb_ff_idx_0 + - (tunning_lqr_ConstP.ControlAllocation_Gain[i + 6] * rtb_Sum1_g + - (tunning_lqr_B.dT + 15.107400000000002) * - tunning_lqr_ConstP.ControlAllocation_Gain[i])); + u0 = tuning_lqr_ConstP.ControlAllocation_Gain[i + 18] * rtb_ff_idx_1 + + (tuning_lqr_ConstP.ControlAllocation_Gain[i + 12] * rtb_ff_idx_0 + + (tuning_lqr_ConstP.ControlAllocation_Gain[i + 6] * rtb_Sum1_g + + (tuning_lqr_B.dT + 15.107400000000002) * + tuning_lqr_ConstP.ControlAllocation_Gain[i])); /* Saturate: '/ ' incorporates: * Gain: '/Control Allocation' @@ -385,9 +385,9 @@ void tunning_lqrModelClass::step() rtb_LOE_out[i] = 0.0; /* ':1:4' if t>= LOE_t(i) */ - if (rtb_Clock >= tunning_lqr_U.LOE_t[i]) { + if (rtb_Clock >= tuning_lqr_U.LOE_t[i]) { /* ':1:5' LOE_out(i) = LOE(i); */ - rtb_LOE_out[i] = tunning_lqr_U.LOE_a[i]; + rtb_LOE_out[i] = tuning_lqr_U.LOE_a[i]; } /* End of MATLAB Function: '/LOE_' */ @@ -414,13 +414,13 @@ void tunning_lqrModelClass::step() /* Saturate: '/Saturation' */ if (u0 > 200.0) { /* Outport: '/motor_command' */ - tunning_lqr_Y.motor_command[0] = 200.0; + tuning_lqr_Y.motor_command[0] = 200.0; } else if (u0 < 0.0) { /* Outport: '/motor_command' */ - tunning_lqr_Y.motor_command[0] = 0.0; + tuning_lqr_Y.motor_command[0] = 0.0; } else { /* Outport: '/motor_command' */ - tunning_lqr_Y.motor_command[0] = u0; + tuning_lqr_Y.motor_command[0] = u0; } /* Gain: '/mapping_0_200' incorporates: @@ -436,13 +436,13 @@ void tunning_lqrModelClass::step() /* Saturate: '/Saturation' */ if (u0 > 200.0) { /* Outport: '/motor_command' */ - tunning_lqr_Y.motor_command[1] = 200.0; + tuning_lqr_Y.motor_command[1] = 200.0; } else if (u0 < 0.0) { /* Outport: '/motor_command' */ - tunning_lqr_Y.motor_command[1] = 0.0; + tuning_lqr_Y.motor_command[1] = 0.0; } else { /* Outport: '/motor_command' */ - tunning_lqr_Y.motor_command[1] = u0; + tuning_lqr_Y.motor_command[1] = u0; } /* Gain: '/mapping_0_200' incorporates: @@ -458,13 +458,13 @@ void tunning_lqrModelClass::step() /* Saturate: '/Saturation' */ if (u0 > 200.0) { /* Outport: '/motor_command' */ - tunning_lqr_Y.motor_command[2] = 200.0; + tuning_lqr_Y.motor_command[2] = 200.0; } else if (u0 < 0.0) { /* Outport: '/motor_command' */ - tunning_lqr_Y.motor_command[2] = 0.0; + tuning_lqr_Y.motor_command[2] = 0.0; } else { /* Outport: '/motor_command' */ - tunning_lqr_Y.motor_command[2] = u0; + tuning_lqr_Y.motor_command[2] = u0; } /* Gain: '/mapping_0_200' incorporates: @@ -480,13 +480,13 @@ void tunning_lqrModelClass::step() /* Saturate: '/Saturation' */ if (u0 > 200.0) { /* Outport: '/motor_command' */ - tunning_lqr_Y.motor_command[3] = 200.0; + tuning_lqr_Y.motor_command[3] = 200.0; } else if (u0 < 0.0) { /* Outport: '/motor_command' */ - tunning_lqr_Y.motor_command[3] = 0.0; + tuning_lqr_Y.motor_command[3] = 0.0; } else { /* Outport: '/motor_command' */ - tunning_lqr_Y.motor_command[3] = u0; + tuning_lqr_Y.motor_command[3] = u0; } /* Gain: '/mapping_0_200' incorporates: @@ -502,13 +502,13 @@ void tunning_lqrModelClass::step() /* Saturate: '/Saturation' */ if (u0 > 200.0) { /* Outport: '/motor_command' */ - tunning_lqr_Y.motor_command[4] = 200.0; + tuning_lqr_Y.motor_command[4] = 200.0; } else if (u0 < 0.0) { /* Outport: '/motor_command' */ - tunning_lqr_Y.motor_command[4] = 0.0; + tuning_lqr_Y.motor_command[4] = 0.0; } else { /* Outport: '/motor_command' */ - tunning_lqr_Y.motor_command[4] = u0; + tuning_lqr_Y.motor_command[4] = u0; } /* Gain: '/mapping_0_200' incorporates: @@ -524,22 +524,22 @@ void tunning_lqrModelClass::step() /* Saturate: '/Saturation' */ if (u0 > 200.0) { /* Outport: '/motor_command' */ - tunning_lqr_Y.motor_command[5] = 200.0; + tuning_lqr_Y.motor_command[5] = 200.0; } else if (u0 < 0.0) { /* Outport: '/motor_command' */ - tunning_lqr_Y.motor_command[5] = 0.0; + tuning_lqr_Y.motor_command[5] = 0.0; } else { /* Outport: '/motor_command' */ - tunning_lqr_Y.motor_command[5] = u0; + tuning_lqr_Y.motor_command[5] = u0; } /* Outport: '/virtual_control' incorporates: * Sum: '/Sum2' */ - tunning_lqr_Y.virtual_control[0] = tunning_lqr_B.dT + 15.107400000000002; - tunning_lqr_Y.virtual_control[1] = rtb_Sum1_g; - tunning_lqr_Y.virtual_control[2] = rtb_ff_idx_0; - tunning_lqr_Y.virtual_control[3] = rtb_ff_idx_1; + tuning_lqr_Y.virtual_control[0] = tuning_lqr_B.dT + 15.107400000000002; + tuning_lqr_Y.virtual_control[1] = rtb_Sum1_g; + tuning_lqr_Y.virtual_control[2] = rtb_ff_idx_0; + tuning_lqr_Y.virtual_control[3] = rtb_ff_idx_1; /* MATLAB Function: '/MATLAB Function' incorporates: * Inport: '/Y0' @@ -549,15 +549,15 @@ void tunning_lqrModelClass::step() /* MATLAB Function 'Test_config_and_data/MATLAB Function': ':1' */ /* ':1:2' ref = Y0; */ /* ':1:3' switch test_mode */ - switch ((int32_T)tunning_lqr_U.mode) { + switch ((int32_T)tuning_lqr_U.mode) { case 0: /* ':1:4' case 0 % manual test */ /* manual test */ /* ':1:5' ref = ref_manual; */ - rtb_Sum1_g = tunning_lqr_U.ref[0]; - rtb_Clock = tunning_lqr_U.ref[1]; - rtb_ff_idx_0 = tunning_lqr_U.ref[2]; - rtb_ff_idx_1 = tunning_lqr_U.ref[3]; + rtb_Sum1_g = tuning_lqr_U.ref[0]; + rtb_Clock = tuning_lqr_U.ref[1]; + rtb_ff_idx_0 = tuning_lqr_U.ref[2]; + rtb_ff_idx_1 = tuning_lqr_U.ref[3]; break; case 1: @@ -566,45 +566,45 @@ void tunning_lqrModelClass::step() /* ':1:7' if t<=10 */ if (rtb_Clock <= 10.0) { /* ':1:8' ref = [Y0(1); Y0(2); 0.5; Y0(4)]; */ - rtb_Sum1_g = tunning_lqr_U.Y0[0]; - rtb_Clock = tunning_lqr_U.Y0[1]; + rtb_Sum1_g = tuning_lqr_U.Y0[0]; + rtb_Clock = tuning_lqr_U.Y0[1]; rtb_ff_idx_0 = 0.5; - rtb_ff_idx_1 = tunning_lqr_U.Y0[3]; + rtb_ff_idx_1 = tuning_lqr_U.Y0[3]; } else if (rtb_Clock <= 20.0) { /* ':1:9' elseif t <= 20 */ /* ':1:10' ref = [Y0(1)-1.5; Y0(2)+1.5; 0.5; Y0(4)]; */ - rtb_Sum1_g = tunning_lqr_U.Y0[0] - 1.5; - rtb_Clock = tunning_lqr_U.Y0[1] + 1.5; + rtb_Sum1_g = tuning_lqr_U.Y0[0] - 1.5; + rtb_Clock = tuning_lqr_U.Y0[1] + 1.5; rtb_ff_idx_0 = 0.5; - rtb_ff_idx_1 = tunning_lqr_U.Y0[3]; + rtb_ff_idx_1 = tuning_lqr_U.Y0[3]; } else if (rtb_Clock <= 30.0) { /* ':1:11' elseif t <=30 */ /* ':1:12' ref = [Y0(1); Y0(2); 0.5; Y0(4)]; */ - rtb_Sum1_g = tunning_lqr_U.Y0[0]; - rtb_Clock = tunning_lqr_U.Y0[1]; + rtb_Sum1_g = tuning_lqr_U.Y0[0]; + rtb_Clock = tuning_lqr_U.Y0[1]; rtb_ff_idx_0 = 0.5; - rtb_ff_idx_1 = tunning_lqr_U.Y0[3]; + rtb_ff_idx_1 = tuning_lqr_U.Y0[3]; } else if (rtb_Clock <= 40.0) { /* ':1:13' elseif t <=40 */ /* ':1:14' ref = [Y0(1); Y0(2); 0.5; Y0(4)+pi/2]; */ - rtb_Sum1_g = tunning_lqr_U.Y0[0]; - rtb_Clock = tunning_lqr_U.Y0[1]; + rtb_Sum1_g = tuning_lqr_U.Y0[0]; + rtb_Clock = tuning_lqr_U.Y0[1]; rtb_ff_idx_0 = 0.5; - rtb_ff_idx_1 = tunning_lqr_U.Y0[3] + 1.5707963267948966; + rtb_ff_idx_1 = tuning_lqr_U.Y0[3] + 1.5707963267948966; } else if (rtb_Clock <= 50.0) { /* ':1:15' elseif t <=50 */ /* ':1:16' ref = [Y0(1); Y0(2); 0.5; Y0(4)]; */ - rtb_Sum1_g = tunning_lqr_U.Y0[0]; - rtb_Clock = tunning_lqr_U.Y0[1]; + rtb_Sum1_g = tuning_lqr_U.Y0[0]; + rtb_Clock = tuning_lqr_U.Y0[1]; rtb_ff_idx_0 = 0.5; - rtb_ff_idx_1 = tunning_lqr_U.Y0[3]; + rtb_ff_idx_1 = tuning_lqr_U.Y0[3]; } else { /* ':1:17' else */ /* ':1:18' ref = Y0; */ - rtb_Sum1_g = tunning_lqr_U.Y0[0]; - rtb_Clock = tunning_lqr_U.Y0[1]; - rtb_ff_idx_0 = tunning_lqr_U.Y0[2]; - rtb_ff_idx_1 = tunning_lqr_U.Y0[3]; + rtb_Sum1_g = tuning_lqr_U.Y0[0]; + rtb_Clock = tuning_lqr_U.Y0[1]; + rtb_ff_idx_0 = tuning_lqr_U.Y0[2]; + rtb_ff_idx_1 = tuning_lqr_U.Y0[3]; } break; @@ -614,31 +614,31 @@ void tunning_lqrModelClass::step() /* ':1:21' if t<=10 */ if (rtb_Clock <= 10.0) { /* ':1:22' ref = [Y0(1); Y0(2); 0.75; Y0(4)]; */ - rtb_Sum1_g = tunning_lqr_U.Y0[0]; - rtb_Clock = tunning_lqr_U.Y0[1]; + rtb_Sum1_g = tuning_lqr_U.Y0[0]; + rtb_Clock = tuning_lqr_U.Y0[1]; rtb_ff_idx_0 = 0.75; - rtb_ff_idx_1 = tunning_lqr_U.Y0[3]; + rtb_ff_idx_1 = tuning_lqr_U.Y0[3]; } else if (rtb_Clock <= 50.0) { /* ':1:23' elseif t <= 50 */ /* ':1:24' ref = [cos(t); sin(t); 0.75; Y0(4)]; */ rtb_Sum1_g = std::cos(rtb_Clock); rtb_Clock = std::sin(rtb_Clock); rtb_ff_idx_0 = 0.75; - rtb_ff_idx_1 = tunning_lqr_U.Y0[3]; + rtb_ff_idx_1 = tuning_lqr_U.Y0[3]; } else if (rtb_Clock <= 60.0) { /* ':1:25' elseif t <= 60 */ /* ':1:26' ref = [Y0(1); Y0(2); 0.75; Y0(4)]; */ - rtb_Sum1_g = tunning_lqr_U.Y0[0]; - rtb_Clock = tunning_lqr_U.Y0[1]; + rtb_Sum1_g = tuning_lqr_U.Y0[0]; + rtb_Clock = tuning_lqr_U.Y0[1]; rtb_ff_idx_0 = 0.75; - rtb_ff_idx_1 = tunning_lqr_U.Y0[3]; + rtb_ff_idx_1 = tuning_lqr_U.Y0[3]; } else { /* ':1:27' else */ /* ':1:28' ref = Y0; */ - rtb_Sum1_g = tunning_lqr_U.Y0[0]; - rtb_Clock = tunning_lqr_U.Y0[1]; - rtb_ff_idx_0 = tunning_lqr_U.Y0[2]; - rtb_ff_idx_1 = tunning_lqr_U.Y0[3]; + rtb_Sum1_g = tuning_lqr_U.Y0[0]; + rtb_Clock = tuning_lqr_U.Y0[1]; + rtb_ff_idx_0 = tuning_lqr_U.Y0[2]; + rtb_ff_idx_1 = tuning_lqr_U.Y0[3]; } /* ref = [cos(t); sin(t);min(t,15);Y0(4)]; */ @@ -647,24 +647,24 @@ void tunning_lqrModelClass::step() default: /* ':1:31' otherwise */ /* ':1:32' ref = Y0; */ - rtb_Sum1_g = tunning_lqr_U.Y0[0]; - rtb_Clock = tunning_lqr_U.Y0[1]; - rtb_ff_idx_0 = tunning_lqr_U.Y0[2]; - rtb_ff_idx_1 = tunning_lqr_U.Y0[3]; + rtb_Sum1_g = tuning_lqr_U.Y0[0]; + rtb_Clock = tuning_lqr_U.Y0[1]; + rtb_ff_idx_0 = tuning_lqr_U.Y0[2]; + rtb_ff_idx_1 = tuning_lqr_U.Y0[3]; break; } /* End of MATLAB Function: '/MATLAB Function' */ /* Outport: '/ref_out' */ - tunning_lqr_Y.ref_out[0] = rtb_Sum1_g; - tunning_lqr_Y.ref_out[1] = rtb_Clock; - tunning_lqr_Y.ref_out[2] = rtb_ff_idx_0; - tunning_lqr_Y.ref_out[3] = rtb_ff_idx_1; + tuning_lqr_Y.ref_out[0] = rtb_Sum1_g; + tuning_lqr_Y.ref_out[1] = rtb_Clock; + tuning_lqr_Y.ref_out[2] = rtb_ff_idx_0; + tuning_lqr_Y.ref_out[3] = rtb_ff_idx_1; /* Outport: '/LOE' */ for (i = 0; i < 6; i++) { - tunning_lqr_Y.LOE[i] = rtb_LOE_out[i]; + tuning_lqr_Y.LOE[i] = rtb_LOE_out[i]; } /* End of Outport: '/LOE' */ @@ -672,83 +672,83 @@ void tunning_lqrModelClass::step() /* Sum: '/Sum' incorporates: * Inport: '/Y0' */ - tunning_lqr_B.Sum[0] = rtb_Sum1_g - tunning_lqr_U.Y0[0]; - tunning_lqr_B.Sum[1] = rtb_Clock - tunning_lqr_U.Y0[1]; - tunning_lqr_B.Sum[2] = rtb_ff_idx_0 - tunning_lqr_U.Y0[2]; - tunning_lqr_B.Sum[3] = rtb_ff_idx_1 - tunning_lqr_U.Y0[3]; + tuning_lqr_B.Sum[0] = rtb_Sum1_g - tuning_lqr_U.Y0[0]; + tuning_lqr_B.Sum[1] = rtb_Clock - tuning_lqr_U.Y0[1]; + tuning_lqr_B.Sum[2] = rtb_ff_idx_0 - tuning_lqr_U.Y0[2]; + tuning_lqr_B.Sum[3] = rtb_ff_idx_1 - tuning_lqr_U.Y0[3]; /* RateTransition: '/Rate Transition ' */ - if ((rtmIsMajorTimeStep((&tunning_lqr_M)) && - (&tunning_lqr_M)->Timing.TaskCounters.TID[1] == 0) && (rtmIsMajorTimeStep - ((&tunning_lqr_M)) && - (&tunning_lqr_M)->Timing.TaskCounters.TID[2] == 0)) { - tunning_lqr_B.RateTransition_g[0] = tunning_lqr_B.Sum[0]; - tunning_lqr_B.RateTransition_g[1] = tunning_lqr_B.Sum[1]; - tunning_lqr_B.RateTransition_g[2] = tunning_lqr_B.Sum[2]; - tunning_lqr_B.RateTransition_g[3] = tunning_lqr_B.Sum[3]; + if ((rtmIsMajorTimeStep((&tuning_lqr_M)) && + (&tuning_lqr_M)->Timing.TaskCounters.TID[1] == 0) && (rtmIsMajorTimeStep + ((&tuning_lqr_M)) && + (&tuning_lqr_M)->Timing.TaskCounters.TID[2] == 0)) { + tuning_lqr_B.RateTransition_g[0] = tuning_lqr_B.Sum[0]; + tuning_lqr_B.RateTransition_g[1] = tuning_lqr_B.Sum[1]; + tuning_lqr_B.RateTransition_g[2] = tuning_lqr_B.Sum[2]; + tuning_lqr_B.RateTransition_g[3] = tuning_lqr_B.Sum[3]; } /* End of RateTransition: '/Rate Transition ' */ - if (rtmIsMajorTimeStep((&tunning_lqr_M)) && - (&tunning_lqr_M)->Timing.TaskCounters.TID[2] == 0) { + if (rtmIsMajorTimeStep((&tuning_lqr_M)) && + (&tuning_lqr_M)->Timing.TaskCounters.TID[2] == 0) { /* Saturate: '/x' */ - if (tunning_lqr_B.RateTransition_g[0] > 2.0) { + if (tuning_lqr_B.RateTransition_g[0] > 2.0) { u0 = 2.0; - } else if (tunning_lqr_B.RateTransition_g[0] < -2.0) { + } else if (tuning_lqr_B.RateTransition_g[0] < -2.0) { u0 = -2.0; } else { - u0 = tunning_lqr_B.RateTransition_g[0]; + u0 = tuning_lqr_B.RateTransition_g[0]; } /* End of Saturate: '/x' */ /* Sum: '/Sum3' */ - u0 -= tunning_lqr_B.d_x_b; + u0 -= tuning_lqr_B.d_x_b; /* Saturate: '/x_e' */ if (u0 > 2.0) { - tunning_lqr_B.x_e = 2.0; + tuning_lqr_B.x_e = 2.0; } else if (u0 < -2.0) { - tunning_lqr_B.x_e = -2.0; + tuning_lqr_B.x_e = -2.0; } else { - tunning_lqr_B.x_e = u0; + tuning_lqr_B.x_e = u0; } /* End of Saturate: '/x_e' */ /* Saturate: '/y' */ - if (tunning_lqr_B.RateTransition_g[1] > 2.0) { + if (tuning_lqr_B.RateTransition_g[1] > 2.0) { u0 = 2.0; - } else if (tunning_lqr_B.RateTransition_g[1] < -2.0) { + } else if (tuning_lqr_B.RateTransition_g[1] < -2.0) { u0 = -2.0; } else { - u0 = tunning_lqr_B.RateTransition_g[1]; + u0 = tuning_lqr_B.RateTransition_g[1]; } /* End of Saturate: '/y' */ /* Sum: '/Sum3' */ - u0 -= tunning_lqr_B.d_y_l; + u0 -= tuning_lqr_B.d_y_l; /* Saturate: '/y_e' */ if (u0 > 2.0) { - tunning_lqr_B.y_e = 2.0; + tuning_lqr_B.y_e = 2.0; } else if (u0 < -2.0) { - tunning_lqr_B.y_e = -2.0; + tuning_lqr_B.y_e = -2.0; } else { - tunning_lqr_B.y_e = u0; + tuning_lqr_B.y_e = u0; } /* End of Saturate: '/y_e' */ } /* Saturate: '/yaw' */ - if (tunning_lqr_B.Sum[3] > 3.1415926535897931) { + if (tuning_lqr_B.Sum[3] > 3.1415926535897931) { u0 = 3.1415926535897931; - } else if (tunning_lqr_B.Sum[3] < -3.1415926535897931) { + } else if (tuning_lqr_B.Sum[3] < -3.1415926535897931) { u0 = -3.1415926535897931; } else { - u0 = tunning_lqr_B.Sum[3]; + u0 = tuning_lqr_B.Sum[3]; } /* End of Saturate: '/yaw' */ @@ -758,44 +758,44 @@ void tunning_lqrModelClass::step() /* Saturate: '/yaw_e' */ if (rtb_Sum3_b > 1.5707963267948966) { - tunning_lqr_B.yaw_e = 1.5707963267948966; + tuning_lqr_B.yaw_e = 1.5707963267948966; } else if (rtb_Sum3_b < -1.5707963267948966) { - tunning_lqr_B.yaw_e = -1.5707963267948966; + tuning_lqr_B.yaw_e = -1.5707963267948966; } else { - tunning_lqr_B.yaw_e = rtb_Sum3_b; + tuning_lqr_B.yaw_e = rtb_Sum3_b; } /* End of Saturate: '/yaw_e' */ - if (rtmIsMajorTimeStep((&tunning_lqr_M)) && - (&tunning_lqr_M)->Timing.TaskCounters.TID[2] == 0) { + if (rtmIsMajorTimeStep((&tuning_lqr_M)) && + (&tuning_lqr_M)->Timing.TaskCounters.TID[2] == 0) { /* Saturate: '/z' */ - if (tunning_lqr_B.RateTransition_g[2] > 1.75) { + if (tuning_lqr_B.RateTransition_g[2] > 1.75) { u0 = 1.75; - } else if (tunning_lqr_B.RateTransition_g[2] < 0.0) { + } else if (tuning_lqr_B.RateTransition_g[2] < 0.0) { u0 = 0.0; } else { - u0 = tunning_lqr_B.RateTransition_g[2]; + u0 = tuning_lqr_B.RateTransition_g[2]; } /* End of Saturate: '/z' */ /* Sum: '/Sum3' */ - u0 -= tunning_lqr_B.d_z_k; + u0 -= tuning_lqr_B.d_z_k; /* Saturate: '/z_e' */ if (u0 > 1.0) { - tunning_lqr_B.z_e = 1.0; + tuning_lqr_B.z_e = 1.0; } else if (u0 < -1.0) { - tunning_lqr_B.z_e = -1.0; + tuning_lqr_B.z_e = -1.0; } else { - tunning_lqr_B.z_e = u0; + tuning_lqr_B.z_e = u0; } /* End of Saturate: '/z_e' */ } - if (rtmIsMajorTimeStep((&tunning_lqr_M))) { - rt_ertODEUpdateContinuousStates(&(&tunning_lqr_M)->solverInfo); + if (rtmIsMajorTimeStep((&tuning_lqr_M))) { + rt_ertODEUpdateContinuousStates(&(&tuning_lqr_M)->solverInfo); /* Update absolute time for base rate */ /* The "clockTick0" counts the number of times the code of this task has @@ -806,11 +806,11 @@ void tunning_lqrModelClass::step() * The two integers represent the low bits Timing.clockTick0 and the high bits * Timing.clockTickH0. When the low bit overflows to 0, the high bits increment. */ - if (!(++(&tunning_lqr_M)->Timing.clockTick0)) { - ++(&tunning_lqr_M)->Timing.clockTickH0; + if (!(++(&tuning_lqr_M)->Timing.clockTick0)) { + ++(&tuning_lqr_M)->Timing.clockTickH0; } - (&tunning_lqr_M)->Timing.t[0] = rtsiGetSolverStopTime(&(&tunning_lqr_M) + (&tuning_lqr_M)->Timing.t[0] = rtsiGetSolverStopTime(&(&tuning_lqr_M) ->solverInfo); { @@ -823,129 +823,129 @@ void tunning_lqrModelClass::step() * The two integers represent the low bits Timing.clockTick1 and the high bits * Timing.clockTickH1. When the low bit overflows to 0, the high bits increment. */ - (&tunning_lqr_M)->Timing.clockTick1++; - if (!(&tunning_lqr_M)->Timing.clockTick1) { - (&tunning_lqr_M)->Timing.clockTickH1++; + (&tuning_lqr_M)->Timing.clockTick1++; + if (!(&tuning_lqr_M)->Timing.clockTick1) { + (&tuning_lqr_M)->Timing.clockTickH1++; } } - rate_scheduler((&tunning_lqr_M)); + rate_scheduler((&tuning_lqr_M)); } /* end MajorTimeStep */ } /* Derivatives for root system: '' */ -void tunning_lqrModelClass::tunning_lqr_derivatives() +void tuning_lqrModelClass::tuning_lqr_derivatives() { - XDot_tunning_lqr_T *_rtXdot; - _rtXdot = ((XDot_tunning_lqr_T *) (&tunning_lqr_M)->derivs); + XDot_tuning_lqr_T *_rtXdot; + _rtXdot = ((XDot_tuning_lqr_T *) (&tuning_lqr_M)->derivs); /* Derivatives for Integrator: '/Integrator1' */ - _rtXdot->Integrator1_CSTATE = tunning_lqr_B.z_e; + _rtXdot->Integrator1_CSTATE = tuning_lqr_B.z_e; /* Derivatives for Integrator: '/Integrator1' */ - _rtXdot->Integrator1_CSTATE_h = tunning_lqr_B.x_e; + _rtXdot->Integrator1_CSTATE_h = tuning_lqr_B.x_e; /* Derivatives for Integrator: '/Integrator1' */ - _rtXdot->Integrator1_CSTATE_g = tunning_lqr_B.y_e; + _rtXdot->Integrator1_CSTATE_g = tuning_lqr_B.y_e; /* Derivatives for Integrator: '/Integrator1' */ - _rtXdot->Integrator1_CSTATE_j = tunning_lqr_B.yaw_e; + _rtXdot->Integrator1_CSTATE_j = tuning_lqr_B.yaw_e; } /* Model initialize function */ -void tunning_lqrModelClass::initialize() +void tuning_lqrModelClass::initialize() { /* Registration code */ /* initialize real-time model */ - (void) memset((void *)(&tunning_lqr_M), 0, - sizeof(RT_MODEL_tunning_lqr_T)); + (void) memset((void *)(&tuning_lqr_M), 0, + sizeof(RT_MODEL_tuning_lqr_T)); { /* Setup solver object */ - rtsiSetSimTimeStepPtr(&(&tunning_lqr_M)->solverInfo, &(&tunning_lqr_M) + rtsiSetSimTimeStepPtr(&(&tuning_lqr_M)->solverInfo, &(&tuning_lqr_M) ->Timing.simTimeStep); - rtsiSetTPtr(&(&tunning_lqr_M)->solverInfo, &rtmGetTPtr((&tunning_lqr_M))); - rtsiSetStepSizePtr(&(&tunning_lqr_M)->solverInfo, &(&tunning_lqr_M) + rtsiSetTPtr(&(&tuning_lqr_M)->solverInfo, &rtmGetTPtr((&tuning_lqr_M))); + rtsiSetStepSizePtr(&(&tuning_lqr_M)->solverInfo, &(&tuning_lqr_M) ->Timing.stepSize0); - rtsiSetdXPtr(&(&tunning_lqr_M)->solverInfo, &(&tunning_lqr_M)->derivs); - rtsiSetContStatesPtr(&(&tunning_lqr_M)->solverInfo, (real_T **) - &(&tunning_lqr_M)->contStates); - rtsiSetNumContStatesPtr(&(&tunning_lqr_M)->solverInfo, &(&tunning_lqr_M) + rtsiSetdXPtr(&(&tuning_lqr_M)->solverInfo, &(&tuning_lqr_M)->derivs); + rtsiSetContStatesPtr(&(&tuning_lqr_M)->solverInfo, (real_T **) + &(&tuning_lqr_M)->contStates); + rtsiSetNumContStatesPtr(&(&tuning_lqr_M)->solverInfo, &(&tuning_lqr_M) ->Sizes.numContStates); - rtsiSetNumPeriodicContStatesPtr(&(&tunning_lqr_M)->solverInfo, - &(&tunning_lqr_M)->Sizes.numPeriodicContStates); - rtsiSetPeriodicContStateIndicesPtr(&(&tunning_lqr_M)->solverInfo, - &(&tunning_lqr_M)->periodicContStateIndices); - rtsiSetPeriodicContStateRangesPtr(&(&tunning_lqr_M)->solverInfo, - &(&tunning_lqr_M)->periodicContStateRanges); - rtsiSetErrorStatusPtr(&(&tunning_lqr_M)->solverInfo, (&rtmGetErrorStatus - ((&tunning_lqr_M)))); - rtsiSetRTModelPtr(&(&tunning_lqr_M)->solverInfo, (&tunning_lqr_M)); + rtsiSetNumPeriodicContStatesPtr(&(&tuning_lqr_M)->solverInfo, + &(&tuning_lqr_M)->Sizes.numPeriodicContStates); + rtsiSetPeriodicContStateIndicesPtr(&(&tuning_lqr_M)->solverInfo, + &(&tuning_lqr_M)->periodicContStateIndices); + rtsiSetPeriodicContStateRangesPtr(&(&tuning_lqr_M)->solverInfo, + &(&tuning_lqr_M)->periodicContStateRanges); + rtsiSetErrorStatusPtr(&(&tuning_lqr_M)->solverInfo, (&rtmGetErrorStatus + ((&tuning_lqr_M)))); + rtsiSetRTModelPtr(&(&tuning_lqr_M)->solverInfo, (&tuning_lqr_M)); } - rtsiSetSimTimeStep(&(&tunning_lqr_M)->solverInfo, MAJOR_TIME_STEP); - (&tunning_lqr_M)->intgData.y = (&tunning_lqr_M)->odeY; - (&tunning_lqr_M)->intgData.f[0] = (&tunning_lqr_M)->odeF[0]; - (&tunning_lqr_M)->intgData.f[1] = (&tunning_lqr_M)->odeF[1]; - (&tunning_lqr_M)->intgData.f[2] = (&tunning_lqr_M)->odeF[2]; - (&tunning_lqr_M)->intgData.f[3] = (&tunning_lqr_M)->odeF[3]; - getRTM()->contStates = ((X_tunning_lqr_T *) &tunning_lqr_X); - rtsiSetSolverData(&(&tunning_lqr_M)->solverInfo, (void *)&(&tunning_lqr_M) + rtsiSetSimTimeStep(&(&tuning_lqr_M)->solverInfo, MAJOR_TIME_STEP); + (&tuning_lqr_M)->intgData.y = (&tuning_lqr_M)->odeY; + (&tuning_lqr_M)->intgData.f[0] = (&tuning_lqr_M)->odeF[0]; + (&tuning_lqr_M)->intgData.f[1] = (&tuning_lqr_M)->odeF[1]; + (&tuning_lqr_M)->intgData.f[2] = (&tuning_lqr_M)->odeF[2]; + (&tuning_lqr_M)->intgData.f[3] = (&tuning_lqr_M)->odeF[3]; + getRTM()->contStates = ((X_tuning_lqr_T *) &tuning_lqr_X); + rtsiSetSolverData(&(&tuning_lqr_M)->solverInfo, (void *)&(&tuning_lqr_M) ->intgData); - rtsiSetSolverName(&(&tunning_lqr_M)->solverInfo,"ode4"); - rtmSetTPtr(getRTM(), &(&tunning_lqr_M)->Timing.tArray[0]); - (&tunning_lqr_M)->Timing.stepSize0 = 0.001; + rtsiSetSolverName(&(&tuning_lqr_M)->solverInfo,"ode4"); + rtmSetTPtr(getRTM(), &(&tuning_lqr_M)->Timing.tArray[0]); + (&tuning_lqr_M)->Timing.stepSize0 = 0.001; /* block I/O */ - (void) memset(((void *) &tunning_lqr_B), 0, - sizeof(B_tunning_lqr_T)); + (void) memset(((void *) &tuning_lqr_B), 0, + sizeof(B_tuning_lqr_T)); /* states (continuous) */ { - (void) memset((void *)&tunning_lqr_X, 0, - sizeof(X_tunning_lqr_T)); + (void) memset((void *)&tuning_lqr_X, 0, + sizeof(X_tuning_lqr_T)); } /* external inputs */ - (void)memset((void *)&tunning_lqr_U, 0, sizeof(ExtU_tunning_lqr_T)); + (void)memset((void *)&tuning_lqr_U, 0, sizeof(ExtU_tuning_lqr_T)); /* external outputs */ - (void) memset((void *)&tunning_lqr_Y, 0, - sizeof(ExtY_tunning_lqr_T)); + (void) memset((void *)&tuning_lqr_Y, 0, + sizeof(ExtY_tuning_lqr_T)); /* InitializeConditions for Integrator: '/Integrator1' */ - tunning_lqr_X.Integrator1_CSTATE = 0.0; + tuning_lqr_X.Integrator1_CSTATE = 0.0; /* InitializeConditions for Integrator: '/Integrator1' */ - tunning_lqr_X.Integrator1_CSTATE_h = 0.0; + tuning_lqr_X.Integrator1_CSTATE_h = 0.0; /* InitializeConditions for Integrator: '/Integrator1' */ - tunning_lqr_X.Integrator1_CSTATE_g = 0.0; + tuning_lqr_X.Integrator1_CSTATE_g = 0.0; /* InitializeConditions for Integrator: '/Integrator1' */ - tunning_lqr_X.Integrator1_CSTATE_j = 0.0; + tuning_lqr_X.Integrator1_CSTATE_j = 0.0; } /* Model terminate function */ -void tunning_lqrModelClass::terminate() +void tuning_lqrModelClass::terminate() { /* (no terminate code required) */ } /* Constructor */ -tunning_lqrModelClass::tunning_lqrModelClass() +tuning_lqrModelClass::tuning_lqrModelClass() { } /* Destructor */ -tunning_lqrModelClass::~tunning_lqrModelClass() +tuning_lqrModelClass::~tuning_lqrModelClass() { /* Currently there is no destructor body generated.*/ } /* Real-Time Model get method */ -RT_MODEL_tunning_lqr_T * tunning_lqrModelClass::getRTM() +RT_MODEL_tuning_lqr_T * tuning_lqrModelClass::getRTM() { - return (&tunning_lqr_M); + return (&tuning_lqr_M); } diff --git a/src/tunning_lqr_grt_rtw/tunning_lqr.h b/src/tuning_lqr_grt_rtw/tuning_lqr.h similarity index 86% rename from src/tunning_lqr_grt_rtw/tunning_lqr.h rename to src/tuning_lqr_grt_rtw/tuning_lqr.h index ba8638e..22e8998 100644 --- a/src/tunning_lqr_grt_rtw/tunning_lqr.h +++ b/src/tuning_lqr_grt_rtw/tuning_lqr.h @@ -1,11 +1,11 @@ /* - * tunning_lqr.h + * tuning_lqr.h * * Student License - for use by students to meet course requirements and * perform academic research at degree granting institutions only. Not * for government, commercial, or other organizational use. * - * Code generation for model "tunning_lqr". + * Code generation for model "tuning_lqr". * * Model version : 1.1142 * Simulink Coder version : 8.12 (R2017a) 16-Feb-2017 @@ -18,18 +18,18 @@ * Validation result: Passed (1), Warnings (3), Error (0) */ -#ifndef RTW_HEADER_tunning_lqr_h_ -#define RTW_HEADER_tunning_lqr_h_ +#ifndef RTW_HEADER_tuning_lqr_h_ +#define RTW_HEADER_tuning_lqr_h_ #include #include -#ifndef tunning_lqr_COMMON_INCLUDES_ -# define tunning_lqr_COMMON_INCLUDES_ +#ifndef tuning_lqr_COMMON_INCLUDES_ +# define tuning_lqr_COMMON_INCLUDES_ #include "rtwtypes.h" #include "rtw_continuous.h" #include "rtw_solver.h" -#endif /* tunning_lqr_COMMON_INCLUDES_ */ +#endif /* tuning_lqr_COMMON_INCLUDES_ */ -#include "tunning_lqr_types.h" +#include "tuning_lqr_types.h" /* Shared type includes */ #include "multiword_types.h" @@ -173,7 +173,7 @@ typedef struct { real_T y_e; /* '/y_e' */ real_T yaw_e; /* '/yaw_e' */ real_T z_e; /* '/z_e' */ -} B_tunning_lqr_T; +} B_tuning_lqr_T; /* Continuous states (auto storage) */ typedef struct { @@ -181,7 +181,7 @@ typedef struct { real_T Integrator1_CSTATE_h; /* '/Integrator1' */ real_T Integrator1_CSTATE_g; /* '/Integrator1' */ real_T Integrator1_CSTATE_j; /* '/Integrator1' */ -} X_tunning_lqr_T; +} X_tuning_lqr_T; /* State derivatives (auto storage) */ typedef struct { @@ -189,7 +189,7 @@ typedef struct { real_T Integrator1_CSTATE_h; /* '/Integrator1' */ real_T Integrator1_CSTATE_g; /* '/Integrator1' */ real_T Integrator1_CSTATE_j; /* '/Integrator1' */ -} XDot_tunning_lqr_T; +} XDot_tuning_lqr_T; /* State disabled */ typedef struct { @@ -197,7 +197,7 @@ typedef struct { boolean_T Integrator1_CSTATE_h; /* '/Integrator1' */ boolean_T Integrator1_CSTATE_g; /* '/Integrator1' */ boolean_T Integrator1_CSTATE_j; /* '/Integrator1' */ -} XDis_tunning_lqr_T; +} XDis_tuning_lqr_T; #ifndef ODE4_INTG #define ODE4_INTG @@ -216,7 +216,7 @@ typedef struct { * Referenced by: '/Control Allocation' */ real_T ControlAllocation_Gain[24]; -} ConstP_tunning_lqr_T; +} ConstP_tuning_lqr_T; /* External inputs (root inport signals with auto storage) */ typedef struct { @@ -227,7 +227,7 @@ typedef struct { real_T LOE_t[6]; /* '/LOE_t' */ real_T LOE_a[6]; /* '/LOE_a' */ real_T gain[18]; /* '/gain' */ -} ExtU_tunning_lqr_T; +} ExtU_tuning_lqr_T; /* External outputs (root outports fed by signals with auto storage) */ typedef struct { @@ -235,13 +235,13 @@ typedef struct { real_T virtual_control[4]; /* '/virtual_control' */ real_T ref_out[4]; /* '/ref_out' */ real_T LOE[6]; /* '/LOE' */ -} ExtY_tunning_lqr_T; +} ExtY_tuning_lqr_T; /* Real-time Model Data Structure */ -struct tag_RTM_tunning_lqr_T { +struct tag_RTM_tuning_lqr_T { const char_T *errorStatus; RTWSolverInfo solverInfo; - X_tunning_lqr_T *contStates; + X_tuning_lqr_T *contStates; int_T *periodicContStateIndices; real_T *periodicContStateRanges; real_T *derivs; @@ -299,17 +299,17 @@ extern "C" { #endif /* Constant parameters (auto storage) */ -extern const ConstP_tunning_lqr_T tunning_lqr_ConstP; +extern const ConstP_tuning_lqr_T tuning_lqr_ConstP; -/* Class declaration for model tunning_lqr */ -class tunning_lqrModelClass { +/* Class declaration for model tuning_lqr */ +class tuning_lqrModelClass { /* public data and function members */ public: /* External inputs */ - ExtU_tunning_lqr_T tunning_lqr_U; + ExtU_tuning_lqr_T tuning_lqr_U; /* External outputs */ - ExtY_tunning_lqr_T tunning_lqr_Y; + ExtY_tuning_lqr_T tuning_lqr_Y; /* model initialize function */ void initialize(); @@ -321,28 +321,28 @@ class tunning_lqrModelClass { void terminate(); /* Constructor */ - tunning_lqrModelClass(); + tuning_lqrModelClass(); /* Destructor */ - ~tunning_lqrModelClass(); + ~tuning_lqrModelClass(); /* Real-Time Model get method */ - RT_MODEL_tunning_lqr_T * getRTM(); + RT_MODEL_tuning_lqr_T * getRTM(); /* private data and function members */ private: /* Block signals */ - B_tunning_lqr_T tunning_lqr_B; - X_tunning_lqr_T tunning_lqr_X; /* Block continuous states */ + B_tuning_lqr_T tuning_lqr_B; + X_tuning_lqr_T tuning_lqr_X; /* Block continuous states */ /* Real-Time Model */ - RT_MODEL_tunning_lqr_T tunning_lqr_M; + RT_MODEL_tuning_lqr_T tuning_lqr_M; /* Continuous states update member function*/ void rt_ertODEUpdateContinuousStates(RTWSolverInfo *si ); /* Derivatives member function */ - void tunning_lqr_derivatives(); + void tuning_lqr_derivatives(); }; /*- @@ -370,18 +370,18 @@ class tunning_lqrModelClass { * * Here is the system hierarchy for this model * - * '' : 'tunning_lqr' - * '' : 'tunning_lqr/Actuator_Fault' - * '' : 'tunning_lqr/Test_config_and_data' - * '' : 'tunning_lqr/Thrust2command' - * '' : 'tunning_lqr/pitch_controller' - * '' : 'tunning_lqr/roll_controller' - * '' : 'tunning_lqr/x_controller ' - * '' : 'tunning_lqr/y_controller' - * '' : 'tunning_lqr/yaw_controller' - * '' : 'tunning_lqr/z_controller' - * '' : 'tunning_lqr/Test_config_and_data/FFW' - * '' : 'tunning_lqr/Test_config_and_data/LOE_' - * '' : 'tunning_lqr/Test_config_and_data/MATLAB Function' + * '' : 'tuning_lqr' + * '' : 'tuning_lqr/Actuator_Fault' + * '' : 'tuning_lqr/Test_config_and_data' + * '' : 'tuning_lqr/Thrust2command' + * '' : 'tuning_lqr/pitch_controller' + * '' : 'tuning_lqr/roll_controller' + * '' : 'tuning_lqr/x_controller ' + * '' : 'tuning_lqr/y_controller' + * '' : 'tuning_lqr/yaw_controller' + * '' : 'tuning_lqr/z_controller' + * '' : 'tuning_lqr/Test_config_and_data/FFW' + * '' : 'tuning_lqr/Test_config_and_data/LOE_' + * '' : 'tuning_lqr/Test_config_and_data/MATLAB Function' */ -#endif /* RTW_HEADER_tunning_lqr_h_ */ +#endif /* RTW_HEADER_tuning_lqr_h_ */ diff --git a/src/tunning_lqr_grt_rtw/tunning_lqr_data.cpp b/src/tuning_lqr_grt_rtw/tuning_lqr_data.cpp similarity index 88% rename from src/tunning_lqr_grt_rtw/tunning_lqr_data.cpp rename to src/tuning_lqr_grt_rtw/tuning_lqr_data.cpp index dca98f5..14d2427 100644 --- a/src/tunning_lqr_grt_rtw/tunning_lqr_data.cpp +++ b/src/tuning_lqr_grt_rtw/tuning_lqr_data.cpp @@ -1,11 +1,11 @@ /* - * tunning_lqr_data.cpp + * tuning_lqr_data.cpp * * Student License - for use by students to meet course requirements and * perform academic research at degree granting institutions only. Not * for government, commercial, or other organizational use. * - * Code generation for model "tunning_lqr". + * Code generation for model "tuning_lqr". * * Model version : 1.1142 * Simulink Coder version : 8.12 (R2017a) 16-Feb-2017 @@ -18,11 +18,11 @@ * Validation result: Passed (1), Warnings (3), Error (0) */ -#include "tunning_lqr.h" -#include "tunning_lqr_private.h" +#include "tuning_lqr.h" +#include "tuning_lqr_private.h" /* Constant parameters (auto storage) */ -const ConstP_tunning_lqr_T tunning_lqr_ConstP = { +const ConstP_tuning_lqr_T tuning_lqr_ConstP = { /* Expression: B_ENU_inv * Referenced by: '/Control Allocation' */ diff --git a/src/tunning_lqr_grt_rtw/tunning_lqr_private.h b/src/tuning_lqr_grt_rtw/tuning_lqr_private.h similarity index 81% rename from src/tunning_lqr_grt_rtw/tunning_lqr_private.h rename to src/tuning_lqr_grt_rtw/tuning_lqr_private.h index 76d2ca3..3779238 100644 --- a/src/tunning_lqr_grt_rtw/tunning_lqr_private.h +++ b/src/tuning_lqr_grt_rtw/tuning_lqr_private.h @@ -1,11 +1,11 @@ /* - * tunning_lqr_private.h + * tuning_lqr_private.h * * Student License - for use by students to meet course requirements and * perform academic research at degree granting institutions only. Not * for government, commercial, or other organizational use. * - * Code generation for model "tunning_lqr". + * Code generation for model "tuning_lqr". * * Model version : 1.1142 * Simulink Coder version : 8.12 (R2017a) 16-Feb-2017 @@ -18,8 +18,8 @@ * Validation result: Passed (1), Warnings (3), Error (0) */ -#ifndef RTW_HEADER_tunning_lqr_private_h_ -#define RTW_HEADER_tunning_lqr_private_h_ +#ifndef RTW_HEADER_tuning_lqr_private_h_ +#define RTW_HEADER_tuning_lqr_private_h_ #include "rtwtypes.h" #include "multiword_types.h" @@ -41,6 +41,6 @@ #endif /* private model entry point functions */ -extern void tunning_lqr_derivatives(); +extern void tuning_lqr_derivatives(); -#endif /* RTW_HEADER_tunning_lqr_private_h_ */ +#endif /* RTW_HEADER_tuning_lqr_private_h_ */ diff --git a/src/tunning_lqr_grt_rtw/tunning_lqr_types.h b/src/tuning_lqr_grt_rtw/tuning_lqr_types.h similarity index 70% rename from src/tunning_lqr_grt_rtw/tunning_lqr_types.h rename to src/tuning_lqr_grt_rtw/tuning_lqr_types.h index ea5fe87..95cdec3 100644 --- a/src/tunning_lqr_grt_rtw/tunning_lqr_types.h +++ b/src/tuning_lqr_grt_rtw/tuning_lqr_types.h @@ -1,11 +1,11 @@ /* - * tunning_lqr_types.h + * tuning_lqr_types.h * * Student License - for use by students to meet course requirements and * perform academic research at degree granting institutions only. Not * for government, commercial, or other organizational use. * - * Code generation for model "tunning_lqr". + * Code generation for model "tuning_lqr". * * Model version : 1.1142 * Simulink Coder version : 8.12 (R2017a) 16-Feb-2017 @@ -18,10 +18,10 @@ * Validation result: Passed (1), Warnings (3), Error (0) */ -#ifndef RTW_HEADER_tunning_lqr_types_h_ -#define RTW_HEADER_tunning_lqr_types_h_ +#ifndef RTW_HEADER_tuning_lqr_types_h_ +#define RTW_HEADER_tuning_lqr_types_h_ /* Forward declaration for rtModel */ -typedef struct tag_RTM_tunning_lqr_T RT_MODEL_tunning_lqr_T; +typedef struct tag_RTM_tuning_lqr_T RT_MODEL_tuning_lqr_T; -#endif /* RTW_HEADER_tunning_lqr_types_h_ */ +#endif /* RTW_HEADER_tuning_lqr_types_h_ */ diff --git a/src/tunning_lqr_node.cpp b/src/tuning_lqr_node.cpp similarity index 83% rename from src/tunning_lqr_node.cpp rename to src/tuning_lqr_node.cpp index b19056b..cc116ca 100644 --- a/src/tunning_lqr_node.cpp +++ b/src/tuning_lqr_node.cpp @@ -15,12 +15,12 @@ #include #include -#include +#include #include #include -tunning_lqrModelClass gController; +tuning_lqrModelClass gController; bool gPublish; bool gInit_flag; @@ -178,10 +178,10 @@ void timmerCallback(const ros::TimerEvent&) } int main(int argc, char** argv) { - ros::init(argc, argv, "tunning_lqr_node"); + ros::init(argc, argv, "tuning_lqr_node"); ros::NodeHandle nh; ros::NodeHandle pnh("~"); - ROS_INFO("tunning_lqr_node main started"); + ROS_INFO("tuning_lqr_node main started"); ros::Subscriber odometry_sub_; odometry_sub_ = nh.subscribe(mav_msgs::default_topics::ODOMETRY, 1, OdometryCallback); @@ -244,10 +244,10 @@ int main(int argc, char** argv) { control_actived = true; for (unsigned int i=0; i< 6; i++) { - gController.tunning_lqr_U.LOE_a[i] = gLOE[i]; // fault amplitude + gController.tuning_lqr_U.LOE_a[i] = gLOE[i]; // fault amplitude } for (unsigned int i=0; i< 6; i++) { - gController.tunning_lqr_U.LOE_t[i] = gLOE_t[i]; // fault time + gController.tuning_lqr_U.LOE_t[i] = gLOE_t[i]; // fault time } for (unsigned int i=0; i< 18; i++) { ROS_INFO("Controller gain k[%d] = %f",i,gGain[i]); @@ -256,29 +256,29 @@ int main(int argc, char** argv) { if (control_actived) { // controller active after take-off request // Initialization before Step - gController.tunning_lqr_U.mode = gTest_mode; + gController.tuning_lqr_U.mode = gTest_mode; for (unsigned int i=0; i< 4; i++) { - gController.tunning_lqr_U.ref[i] = gRef[i]; + gController.tuning_lqr_U.ref[i] = gRef[i]; } for (unsigned int i=0; i< 4; i++) { - gController.tunning_lqr_U.Y0[i] = gY0[i]; + gController.tuning_lqr_U.Y0[i] = gY0[i]; } for (unsigned int i=0; i< 18; i++) { - gController.tunning_lqr_U.gain[i] = gGain[i]; + gController.tuning_lqr_U.gain[i] = gGain[i]; } - gController.tunning_lqr_U.X[0] = gOdometry.position_W.x(); - gController.tunning_lqr_U.X[1] = gOdometry.position_W.y(); - gController.tunning_lqr_U.X[2] = gOdometry.position_W.z(); - gController.tunning_lqr_U.X[3 ] = velocity_W.x(); - gController.tunning_lqr_U.X[4 ] = velocity_W.y(); - gController.tunning_lqr_U.X[5 ] = velocity_W.z(); - gController.tunning_lqr_U.X[6 ] = phi; - gController.tunning_lqr_U.X[7 ] = theta; - gController.tunning_lqr_U.X[8 ] = gPsi; - gController.tunning_lqr_U.X[9 ] = gOdometry.angular_velocity_B.x(); - gController.tunning_lqr_U.X[10] = gOdometry.angular_velocity_B.y(); - gController.tunning_lqr_U.X[11] = gOdometry.angular_velocity_B.z(); + gController.tuning_lqr_U.X[0] = gOdometry.position_W.x(); + gController.tuning_lqr_U.X[1] = gOdometry.position_W.y(); + gController.tuning_lqr_U.X[2] = gOdometry.position_W.z(); + gController.tuning_lqr_U.X[3 ] = velocity_W.x(); + gController.tuning_lqr_U.X[4 ] = velocity_W.y(); + gController.tuning_lqr_U.X[5 ] = velocity_W.z(); + gController.tuning_lqr_U.X[6 ] = phi; + gController.tuning_lqr_U.X[7 ] = theta; + gController.tuning_lqr_U.X[8 ] = gPsi; + gController.tuning_lqr_U.X[9 ] = gOdometry.angular_velocity_B.x(); + gController.tuning_lqr_U.X[10] = gOdometry.angular_velocity_B.y(); + gController.tuning_lqr_U.X[11] = gOdometry.angular_velocity_B.z(); // Run Matlab controller gController.step(); @@ -293,7 +293,7 @@ int main(int argc, char** argv) { } else { - motor_command[i] = gController.tunning_lqr_Y.motor_command[i]; // normalized [1 .. 200] => Asctec Firefly + motor_command[i] = gController.tuning_lqr_Y.motor_command[i]; // normalized [1 .. 200] => Asctec Firefly motor_RPM[i] = 1250.0 + motor_command[i]*43.75; // real RPM motor_speed[i] = motor_RPM[i]/9.5493; // rad/s => Gazebo } @@ -335,7 +335,7 @@ int main(int argc, char** argv) { if (gEmergency_status) { - ROS_ERROR("tunning_lqr_node emergency status"); + ROS_ERROR("tuning_lqr_node emergency status"); ROS_INFO("x = %f, y = %f, z = %f",gOdometry.position_W.x(),gOdometry.position_W.y(),gOdometry.position_W.z()); ros::Duration(0.5).sleep(); gController.terminate(); @@ -354,10 +354,10 @@ int main(int argc, char** argv) { // Publish data: UAV state in World frame if (gPublish){ gsft_control::UAVStatePtr uav_state_msg(new gsft_control::UAVState); - uav_state_msg->position_ref.x = gController.tunning_lqr_Y.ref_out[0]; // gRef only for manual test - uav_state_msg->position_ref.y = gController.tunning_lqr_Y.ref_out[1]; - uav_state_msg->position_ref.z = gController.tunning_lqr_Y.ref_out[2]; - uav_state_msg->heading_ref = gController.tunning_lqr_Y.ref_out[3]; + uav_state_msg->position_ref.x = gController.tuning_lqr_Y.ref_out[0]; // gRef only for manual test + uav_state_msg->position_ref.y = gController.tuning_lqr_Y.ref_out[1]; + uav_state_msg->position_ref.z = gController.tuning_lqr_Y.ref_out[2]; + uav_state_msg->heading_ref = gController.tuning_lqr_Y.ref_out[3]; uav_state_msg->position_W.x = gOdometry.position_W.x(); uav_state_msg->position_W.y = gOdometry.position_W.y(); @@ -371,14 +371,14 @@ int main(int argc, char** argv) { uav_state_msg->rotation_speed_B.x = gOdometry.angular_velocity_B.x(); uav_state_msg->rotation_speed_B.y = gOdometry.angular_velocity_B.y(); uav_state_msg->rotation_speed_B.z = gOdometry.angular_velocity_B.z(); - uav_state_msg->total_thrust = gController.tunning_lqr_Y.virtual_control[0]; - uav_state_msg->moment.x = gController.tunning_lqr_Y.virtual_control[1]; - uav_state_msg->moment.y = gController.tunning_lqr_Y.virtual_control[2]; - uav_state_msg->moment.z = gController.tunning_lqr_Y.virtual_control[3]; - - /* uav_state_msg->LOE13.x = gController.tunning_lqr_Y.LOE[0]; - uav_state_msg->LOE13.y = gController.tunning_lqr_Y.LOE[1]; - uav_state_msg->LOE13.z = gController.tunning_lqr_Y.LOE[2]; */ + uav_state_msg->total_thrust = gController.tuning_lqr_Y.virtual_control[0]; + uav_state_msg->moment.x = gController.tuning_lqr_Y.virtual_control[1]; + uav_state_msg->moment.y = gController.tuning_lqr_Y.virtual_control[2]; + uav_state_msg->moment.z = gController.tuning_lqr_Y.virtual_control[3]; + + /* uav_state_msg->LOE13.x = gController.tuning_lqr_Y.LOE[0]; + uav_state_msg->LOE13.y = gController.tuning_lqr_Y.LOE[1]; + uav_state_msg->LOE13.z = gController.tuning_lqr_Y.LOE[2]; */ uav_state_msg->header.stamp = ros::Time::now(); uav_state_pub_.publish(uav_state_msg); diff --git a/src/tuning_nominal_grt_rtw/multiword_types.h b/src/tuning_nominal_grt_rtw/multiword_types.h new file mode 100644 index 0000000..815111d --- /dev/null +++ b/src/tuning_nominal_grt_rtw/multiword_types.h @@ -0,0 +1,1167 @@ +/* + * multiword_types.h + * + * Student License - for use by students to meet course requirements and + * perform academic research at degree granting institutions only. Not + * for government, commercial, or other organizational use. + * + * Code generation for model "tuning_nominal". + * + * Model version : 1.1498 + * Simulink Coder version : 8.12 (R2017a) 16-Feb-2017 + * C++ source code generated on : Tue Sep 18 10:50:51 2018 + * + * Target selection: grt.tlc + * Note: GRT includes extra infrastructure and instrumentation for prototyping + * Embedded hardware selection: 32-bit Generic + * Code generation objective: Execution efficiency + * Validation result: Passed (1), Warnings (3), Error (0) + */ + +#ifndef MULTIWORD_TYPES_H +#define MULTIWORD_TYPES_H +#include "rtwtypes.h" + +/* + * MultiWord supporting definitions + */ +typedef long int long_T; + +/* + * MultiWord types + */ +typedef struct { + uint32_T chunks[2]; +} int64m_T; + +typedef struct { + int64m_T re; + int64m_T im; +} cint64m_T; + +typedef struct { + uint32_T chunks[2]; +} uint64m_T; + +typedef struct { + uint64m_T re; + uint64m_T im; +} cuint64m_T; + +typedef struct { + uint32_T chunks[3]; +} int96m_T; + +typedef struct { + int96m_T re; + int96m_T im; +} cint96m_T; + +typedef struct { + uint32_T chunks[3]; +} uint96m_T; + +typedef struct { + uint96m_T re; + uint96m_T im; +} cuint96m_T; + +typedef struct { + uint32_T chunks[4]; +} int128m_T; + +typedef struct { + int128m_T re; + int128m_T im; +} cint128m_T; + +typedef struct { + uint32_T chunks[4]; +} uint128m_T; + +typedef struct { + uint128m_T re; + uint128m_T im; +} cuint128m_T; + +typedef struct { + uint32_T chunks[5]; +} int160m_T; + +typedef struct { + int160m_T re; + int160m_T im; +} cint160m_T; + +typedef struct { + uint32_T chunks[5]; +} uint160m_T; + +typedef struct { + uint160m_T re; + uint160m_T im; +} cuint160m_T; + +typedef struct { + uint32_T chunks[6]; +} int192m_T; + +typedef struct { + int192m_T re; + int192m_T im; +} cint192m_T; + +typedef struct { + uint32_T chunks[6]; +} uint192m_T; + +typedef struct { + uint192m_T re; + uint192m_T im; +} cuint192m_T; + +typedef struct { + uint32_T chunks[7]; +} int224m_T; + +typedef struct { + int224m_T re; + int224m_T im; +} cint224m_T; + +typedef struct { + uint32_T chunks[7]; +} uint224m_T; + +typedef struct { + uint224m_T re; + uint224m_T im; +} cuint224m_T; + +typedef struct { + uint32_T chunks[8]; +} int256m_T; + +typedef struct { + int256m_T re; + int256m_T im; +} cint256m_T; + +typedef struct { + uint32_T chunks[8]; +} uint256m_T; + +typedef struct { + uint256m_T re; + uint256m_T im; +} cuint256m_T; + +typedef struct { + uint32_T chunks[9]; +} int288m_T; + +typedef struct { + int288m_T re; + int288m_T im; +} cint288m_T; + +typedef struct { + uint32_T chunks[9]; +} uint288m_T; + +typedef struct { + uint288m_T re; + uint288m_T im; +} cuint288m_T; + +typedef struct { + uint32_T chunks[10]; +} int320m_T; + +typedef struct { + int320m_T re; + int320m_T im; +} cint320m_T; + +typedef struct { + uint32_T chunks[10]; +} uint320m_T; + +typedef struct { + uint320m_T re; + uint320m_T im; +} cuint320m_T; + +typedef struct { + uint32_T chunks[11]; +} int352m_T; + +typedef struct { + int352m_T re; + int352m_T im; +} cint352m_T; + +typedef struct { + uint32_T chunks[11]; +} uint352m_T; + +typedef struct { + uint352m_T re; + uint352m_T im; +} cuint352m_T; + +typedef struct { + uint32_T chunks[12]; +} int384m_T; + +typedef struct { + int384m_T re; + int384m_T im; +} cint384m_T; + +typedef struct { + uint32_T chunks[12]; +} uint384m_T; + +typedef struct { + uint384m_T re; + uint384m_T im; +} cuint384m_T; + +typedef struct { + uint32_T chunks[13]; +} int416m_T; + +typedef struct { + int416m_T re; + int416m_T im; +} cint416m_T; + +typedef struct { + uint32_T chunks[13]; +} uint416m_T; + +typedef struct { + uint416m_T re; + uint416m_T im; +} cuint416m_T; + +typedef struct { + uint32_T chunks[14]; +} int448m_T; + +typedef struct { + int448m_T re; + int448m_T im; +} cint448m_T; + +typedef struct { + uint32_T chunks[14]; +} uint448m_T; + +typedef struct { + uint448m_T re; + uint448m_T im; +} cuint448m_T; + +typedef struct { + uint32_T chunks[15]; +} int480m_T; + +typedef struct { + int480m_T re; + int480m_T im; +} cint480m_T; + +typedef struct { + uint32_T chunks[15]; +} uint480m_T; + +typedef struct { + uint480m_T re; + uint480m_T im; +} cuint480m_T; + +typedef struct { + uint32_T chunks[16]; +} int512m_T; + +typedef struct { + int512m_T re; + int512m_T im; +} cint512m_T; + +typedef struct { + uint32_T chunks[16]; +} uint512m_T; + +typedef struct { + uint512m_T re; + uint512m_T im; +} cuint512m_T; + +typedef struct { + uint32_T chunks[17]; +} int544m_T; + +typedef struct { + int544m_T re; + int544m_T im; +} cint544m_T; + +typedef struct { + uint32_T chunks[17]; +} uint544m_T; + +typedef struct { + uint544m_T re; + uint544m_T im; +} cuint544m_T; + +typedef struct { + uint32_T chunks[18]; +} int576m_T; + +typedef struct { + int576m_T re; + int576m_T im; +} cint576m_T; + +typedef struct { + uint32_T chunks[18]; +} uint576m_T; + +typedef struct { + uint576m_T re; + uint576m_T im; +} cuint576m_T; + +typedef struct { + uint32_T chunks[19]; +} int608m_T; + +typedef struct { + int608m_T re; + int608m_T im; +} cint608m_T; + +typedef struct { + uint32_T chunks[19]; +} uint608m_T; + +typedef struct { + uint608m_T re; + uint608m_T im; +} cuint608m_T; + +typedef struct { + uint32_T chunks[20]; +} int640m_T; + +typedef struct { + int640m_T re; + int640m_T im; +} cint640m_T; + +typedef struct { + uint32_T chunks[20]; +} uint640m_T; + +typedef struct { + uint640m_T re; + uint640m_T im; +} cuint640m_T; + +typedef struct { + uint32_T chunks[21]; +} int672m_T; + +typedef struct { + int672m_T re; + int672m_T im; +} cint672m_T; + +typedef struct { + uint32_T chunks[21]; +} uint672m_T; + +typedef struct { + uint672m_T re; + uint672m_T im; +} cuint672m_T; + +typedef struct { + uint32_T chunks[22]; +} int704m_T; + +typedef struct { + int704m_T re; + int704m_T im; +} cint704m_T; + +typedef struct { + uint32_T chunks[22]; +} uint704m_T; + +typedef struct { + uint704m_T re; + uint704m_T im; +} cuint704m_T; + +typedef struct { + uint32_T chunks[23]; +} int736m_T; + +typedef struct { + int736m_T re; + int736m_T im; +} cint736m_T; + +typedef struct { + uint32_T chunks[23]; +} uint736m_T; + +typedef struct { + uint736m_T re; + uint736m_T im; +} cuint736m_T; + +typedef struct { + uint32_T chunks[24]; +} int768m_T; + +typedef struct { + int768m_T re; + int768m_T im; +} cint768m_T; + +typedef struct { + uint32_T chunks[24]; +} uint768m_T; + +typedef struct { + uint768m_T re; + uint768m_T im; +} cuint768m_T; + +typedef struct { + uint32_T chunks[25]; +} int800m_T; + +typedef struct { + int800m_T re; + int800m_T im; +} cint800m_T; + +typedef struct { + uint32_T chunks[25]; +} uint800m_T; + +typedef struct { + uint800m_T re; + uint800m_T im; +} cuint800m_T; + +typedef struct { + uint32_T chunks[26]; +} int832m_T; + +typedef struct { + int832m_T re; + int832m_T im; +} cint832m_T; + +typedef struct { + uint32_T chunks[26]; +} uint832m_T; + +typedef struct { + uint832m_T re; + uint832m_T im; +} cuint832m_T; + +typedef struct { + uint32_T chunks[27]; +} int864m_T; + +typedef struct { + int864m_T re; + int864m_T im; +} cint864m_T; + +typedef struct { + uint32_T chunks[27]; +} uint864m_T; + +typedef struct { + uint864m_T re; + uint864m_T im; +} cuint864m_T; + +typedef struct { + uint32_T chunks[28]; +} int896m_T; + +typedef struct { + int896m_T re; + int896m_T im; +} cint896m_T; + +typedef struct { + uint32_T chunks[28]; +} uint896m_T; + +typedef struct { + uint896m_T re; + uint896m_T im; +} cuint896m_T; + +typedef struct { + uint32_T chunks[29]; +} int928m_T; + +typedef struct { + int928m_T re; + int928m_T im; +} cint928m_T; + +typedef struct { + uint32_T chunks[29]; +} uint928m_T; + +typedef struct { + uint928m_T re; + uint928m_T im; +} cuint928m_T; + +typedef struct { + uint32_T chunks[30]; +} int960m_T; + +typedef struct { + int960m_T re; + int960m_T im; +} cint960m_T; + +typedef struct { + uint32_T chunks[30]; +} uint960m_T; + +typedef struct { + uint960m_T re; + uint960m_T im; +} cuint960m_T; + +typedef struct { + uint32_T chunks[31]; +} int992m_T; + +typedef struct { + int992m_T re; + int992m_T im; +} cint992m_T; + +typedef struct { + uint32_T chunks[31]; +} uint992m_T; + +typedef struct { + uint992m_T re; + uint992m_T im; +} cuint992m_T; + +typedef struct { + uint32_T chunks[32]; +} int1024m_T; + +typedef struct { + int1024m_T re; + int1024m_T im; +} cint1024m_T; + +typedef struct { + uint32_T chunks[32]; +} uint1024m_T; + +typedef struct { + uint1024m_T re; + uint1024m_T im; +} cuint1024m_T; + +typedef struct { + uint32_T chunks[33]; +} int1056m_T; + +typedef struct { + int1056m_T re; + int1056m_T im; +} cint1056m_T; + +typedef struct { + uint32_T chunks[33]; +} uint1056m_T; + +typedef struct { + uint1056m_T re; + uint1056m_T im; +} cuint1056m_T; + +typedef struct { + uint32_T chunks[34]; +} int1088m_T; + +typedef struct { + int1088m_T re; + int1088m_T im; +} cint1088m_T; + +typedef struct { + uint32_T chunks[34]; +} uint1088m_T; + +typedef struct { + uint1088m_T re; + uint1088m_T im; +} cuint1088m_T; + +typedef struct { + uint32_T chunks[35]; +} int1120m_T; + +typedef struct { + int1120m_T re; + int1120m_T im; +} cint1120m_T; + +typedef struct { + uint32_T chunks[35]; +} uint1120m_T; + +typedef struct { + uint1120m_T re; + uint1120m_T im; +} cuint1120m_T; + +typedef struct { + uint32_T chunks[36]; +} int1152m_T; + +typedef struct { + int1152m_T re; + int1152m_T im; +} cint1152m_T; + +typedef struct { + uint32_T chunks[36]; +} uint1152m_T; + +typedef struct { + uint1152m_T re; + uint1152m_T im; +} cuint1152m_T; + +typedef struct { + uint32_T chunks[37]; +} int1184m_T; + +typedef struct { + int1184m_T re; + int1184m_T im; +} cint1184m_T; + +typedef struct { + uint32_T chunks[37]; +} uint1184m_T; + +typedef struct { + uint1184m_T re; + uint1184m_T im; +} cuint1184m_T; + +typedef struct { + uint32_T chunks[38]; +} int1216m_T; + +typedef struct { + int1216m_T re; + int1216m_T im; +} cint1216m_T; + +typedef struct { + uint32_T chunks[38]; +} uint1216m_T; + +typedef struct { + uint1216m_T re; + uint1216m_T im; +} cuint1216m_T; + +typedef struct { + uint32_T chunks[39]; +} int1248m_T; + +typedef struct { + int1248m_T re; + int1248m_T im; +} cint1248m_T; + +typedef struct { + uint32_T chunks[39]; +} uint1248m_T; + +typedef struct { + uint1248m_T re; + uint1248m_T im; +} cuint1248m_T; + +typedef struct { + uint32_T chunks[40]; +} int1280m_T; + +typedef struct { + int1280m_T re; + int1280m_T im; +} cint1280m_T; + +typedef struct { + uint32_T chunks[40]; +} uint1280m_T; + +typedef struct { + uint1280m_T re; + uint1280m_T im; +} cuint1280m_T; + +typedef struct { + uint32_T chunks[41]; +} int1312m_T; + +typedef struct { + int1312m_T re; + int1312m_T im; +} cint1312m_T; + +typedef struct { + uint32_T chunks[41]; +} uint1312m_T; + +typedef struct { + uint1312m_T re; + uint1312m_T im; +} cuint1312m_T; + +typedef struct { + uint32_T chunks[42]; +} int1344m_T; + +typedef struct { + int1344m_T re; + int1344m_T im; +} cint1344m_T; + +typedef struct { + uint32_T chunks[42]; +} uint1344m_T; + +typedef struct { + uint1344m_T re; + uint1344m_T im; +} cuint1344m_T; + +typedef struct { + uint32_T chunks[43]; +} int1376m_T; + +typedef struct { + int1376m_T re; + int1376m_T im; +} cint1376m_T; + +typedef struct { + uint32_T chunks[43]; +} uint1376m_T; + +typedef struct { + uint1376m_T re; + uint1376m_T im; +} cuint1376m_T; + +typedef struct { + uint32_T chunks[44]; +} int1408m_T; + +typedef struct { + int1408m_T re; + int1408m_T im; +} cint1408m_T; + +typedef struct { + uint32_T chunks[44]; +} uint1408m_T; + +typedef struct { + uint1408m_T re; + uint1408m_T im; +} cuint1408m_T; + +typedef struct { + uint32_T chunks[45]; +} int1440m_T; + +typedef struct { + int1440m_T re; + int1440m_T im; +} cint1440m_T; + +typedef struct { + uint32_T chunks[45]; +} uint1440m_T; + +typedef struct { + uint1440m_T re; + uint1440m_T im; +} cuint1440m_T; + +typedef struct { + uint32_T chunks[46]; +} int1472m_T; + +typedef struct { + int1472m_T re; + int1472m_T im; +} cint1472m_T; + +typedef struct { + uint32_T chunks[46]; +} uint1472m_T; + +typedef struct { + uint1472m_T re; + uint1472m_T im; +} cuint1472m_T; + +typedef struct { + uint32_T chunks[47]; +} int1504m_T; + +typedef struct { + int1504m_T re; + int1504m_T im; +} cint1504m_T; + +typedef struct { + uint32_T chunks[47]; +} uint1504m_T; + +typedef struct { + uint1504m_T re; + uint1504m_T im; +} cuint1504m_T; + +typedef struct { + uint32_T chunks[48]; +} int1536m_T; + +typedef struct { + int1536m_T re; + int1536m_T im; +} cint1536m_T; + +typedef struct { + uint32_T chunks[48]; +} uint1536m_T; + +typedef struct { + uint1536m_T re; + uint1536m_T im; +} cuint1536m_T; + +typedef struct { + uint32_T chunks[49]; +} int1568m_T; + +typedef struct { + int1568m_T re; + int1568m_T im; +} cint1568m_T; + +typedef struct { + uint32_T chunks[49]; +} uint1568m_T; + +typedef struct { + uint1568m_T re; + uint1568m_T im; +} cuint1568m_T; + +typedef struct { + uint32_T chunks[50]; +} int1600m_T; + +typedef struct { + int1600m_T re; + int1600m_T im; +} cint1600m_T; + +typedef struct { + uint32_T chunks[50]; +} uint1600m_T; + +typedef struct { + uint1600m_T re; + uint1600m_T im; +} cuint1600m_T; + +typedef struct { + uint32_T chunks[51]; +} int1632m_T; + +typedef struct { + int1632m_T re; + int1632m_T im; +} cint1632m_T; + +typedef struct { + uint32_T chunks[51]; +} uint1632m_T; + +typedef struct { + uint1632m_T re; + uint1632m_T im; +} cuint1632m_T; + +typedef struct { + uint32_T chunks[52]; +} int1664m_T; + +typedef struct { + int1664m_T re; + int1664m_T im; +} cint1664m_T; + +typedef struct { + uint32_T chunks[52]; +} uint1664m_T; + +typedef struct { + uint1664m_T re; + uint1664m_T im; +} cuint1664m_T; + +typedef struct { + uint32_T chunks[53]; +} int1696m_T; + +typedef struct { + int1696m_T re; + int1696m_T im; +} cint1696m_T; + +typedef struct { + uint32_T chunks[53]; +} uint1696m_T; + +typedef struct { + uint1696m_T re; + uint1696m_T im; +} cuint1696m_T; + +typedef struct { + uint32_T chunks[54]; +} int1728m_T; + +typedef struct { + int1728m_T re; + int1728m_T im; +} cint1728m_T; + +typedef struct { + uint32_T chunks[54]; +} uint1728m_T; + +typedef struct { + uint1728m_T re; + uint1728m_T im; +} cuint1728m_T; + +typedef struct { + uint32_T chunks[55]; +} int1760m_T; + +typedef struct { + int1760m_T re; + int1760m_T im; +} cint1760m_T; + +typedef struct { + uint32_T chunks[55]; +} uint1760m_T; + +typedef struct { + uint1760m_T re; + uint1760m_T im; +} cuint1760m_T; + +typedef struct { + uint32_T chunks[56]; +} int1792m_T; + +typedef struct { + int1792m_T re; + int1792m_T im; +} cint1792m_T; + +typedef struct { + uint32_T chunks[56]; +} uint1792m_T; + +typedef struct { + uint1792m_T re; + uint1792m_T im; +} cuint1792m_T; + +typedef struct { + uint32_T chunks[57]; +} int1824m_T; + +typedef struct { + int1824m_T re; + int1824m_T im; +} cint1824m_T; + +typedef struct { + uint32_T chunks[57]; +} uint1824m_T; + +typedef struct { + uint1824m_T re; + uint1824m_T im; +} cuint1824m_T; + +typedef struct { + uint32_T chunks[58]; +} int1856m_T; + +typedef struct { + int1856m_T re; + int1856m_T im; +} cint1856m_T; + +typedef struct { + uint32_T chunks[58]; +} uint1856m_T; + +typedef struct { + uint1856m_T re; + uint1856m_T im; +} cuint1856m_T; + +typedef struct { + uint32_T chunks[59]; +} int1888m_T; + +typedef struct { + int1888m_T re; + int1888m_T im; +} cint1888m_T; + +typedef struct { + uint32_T chunks[59]; +} uint1888m_T; + +typedef struct { + uint1888m_T re; + uint1888m_T im; +} cuint1888m_T; + +typedef struct { + uint32_T chunks[60]; +} int1920m_T; + +typedef struct { + int1920m_T re; + int1920m_T im; +} cint1920m_T; + +typedef struct { + uint32_T chunks[60]; +} uint1920m_T; + +typedef struct { + uint1920m_T re; + uint1920m_T im; +} cuint1920m_T; + +typedef struct { + uint32_T chunks[61]; +} int1952m_T; + +typedef struct { + int1952m_T re; + int1952m_T im; +} cint1952m_T; + +typedef struct { + uint32_T chunks[61]; +} uint1952m_T; + +typedef struct { + uint1952m_T re; + uint1952m_T im; +} cuint1952m_T; + +typedef struct { + uint32_T chunks[62]; +} int1984m_T; + +typedef struct { + int1984m_T re; + int1984m_T im; +} cint1984m_T; + +typedef struct { + uint32_T chunks[62]; +} uint1984m_T; + +typedef struct { + uint1984m_T re; + uint1984m_T im; +} cuint1984m_T; + +typedef struct { + uint32_T chunks[63]; +} int2016m_T; + +typedef struct { + int2016m_T re; + int2016m_T im; +} cint2016m_T; + +typedef struct { + uint32_T chunks[63]; +} uint2016m_T; + +typedef struct { + uint2016m_T re; + uint2016m_T im; +} cuint2016m_T; + +typedef struct { + uint32_T chunks[64]; +} int2048m_T; + +typedef struct { + int2048m_T re; + int2048m_T im; +} cint2048m_T; + +typedef struct { + uint32_T chunks[64]; +} uint2048m_T; + +typedef struct { + uint2048m_T re; + uint2048m_T im; +} cuint2048m_T; + +#endif /* MULTIWORD_TYPES_H */ diff --git a/src/tuning_nominal_grt_rtw/rtmodel.h b/src/tuning_nominal_grt_rtw/rtmodel.h new file mode 100644 index 0000000..6e78891 --- /dev/null +++ b/src/tuning_nominal_grt_rtw/rtmodel.h @@ -0,0 +1,38 @@ +/* + * rtmodel.h: + * + * Student License - for use by students to meet course requirements and + * perform academic research at degree granting institutions only. Not + * for government, commercial, or other organizational use. + * + * Code generation for model "tuning_nominal". + * + * Model version : 1.1498 + * Simulink Coder version : 8.12 (R2017a) 16-Feb-2017 + * C++ source code generated on : Tue Sep 18 10:50:51 2018 + * + * Target selection: grt.tlc + * Note: GRT includes extra infrastructure and instrumentation for prototyping + * Embedded hardware selection: 32-bit Generic + * Code generation objective: Execution efficiency + * Validation result: Passed (1), Warnings (3), Error (0) + */ + +#ifndef RTW_HEADER_rtmodel_h_ +#define RTW_HEADER_rtmodel_h_ + +/* + * Includes the appropriate headers when we are using rtModel + */ +#include "tuning_nominal.h" +#define GRTINTERFACE 0 + +/* + * ROOT_IO_FORMAT: 0 (Individual arguments) + * ROOT_IO_FORMAT: 1 (Structure reference) + * ROOT_IO_FORMAT: 2 (Part of model data structure) + */ +# define ROOT_IO_FORMAT 2 +#define MODEL_CLASSNAME tuning_nominalModelClass +#define MODEL_STEPNAME step +#endif /* RTW_HEADER_rtmodel_h_ */ diff --git a/src/tuning_nominal_grt_rtw/rtwtypes.h b/src/tuning_nominal_grt_rtw/rtwtypes.h new file mode 100644 index 0000000..85f7d55 --- /dev/null +++ b/src/tuning_nominal_grt_rtw/rtwtypes.h @@ -0,0 +1,42 @@ +/* + * rtwtypes.h + * + * Student License - for use by students to meet course requirements and + * perform academic research at degree granting institutions only. Not + * for government, commercial, or other organizational use. + * + * Code generation for model "tuning_nominal". + * + * Model version : 1.1498 + * Simulink Coder version : 8.12 (R2017a) 16-Feb-2017 + * C++ source code generated on : Tue Sep 18 10:50:51 2018 + * + * Target selection: grt.tlc + * Note: GRT includes extra infrastructure and instrumentation for prototyping + * Embedded hardware selection: 32-bit Generic + * Code generation objective: Execution efficiency + * Validation result: Passed (1), Warnings (3), Error (0) + */ + +#ifndef RTWTYPES_H +#define RTWTYPES_H +#include "tmwtypes.h" +#include "simstruc_types.h" +#ifndef POINTER_T +# define POINTER_T + +typedef void * pointer_T; + +#endif + +/* Logical type definitions */ +#if (!defined(__cplusplus)) +# ifndef false +# define false (0U) +# endif + +# ifndef true +# define true (1U) +# endif +#endif +#endif /* RTWTYPES_H */ diff --git a/src/tunning_nominal_grt_rtw/tunning_nominal.cpp b/src/tuning_nominal_grt_rtw/tuning_nominal.cpp similarity index 56% rename from src/tunning_nominal_grt_rtw/tunning_nominal.cpp rename to src/tuning_nominal_grt_rtw/tuning_nominal.cpp index 6fc9841..427ada3 100644 --- a/src/tunning_nominal_grt_rtw/tunning_nominal.cpp +++ b/src/tuning_nominal_grt_rtw/tuning_nominal.cpp @@ -1,15 +1,15 @@ /* - * tunning_nominal.cpp + * tuning_nominal.cpp * * Student License - for use by students to meet course requirements and * perform academic research at degree granting institutions only. Not * for government, commercial, or other organizational use. * - * Code generation for model "tunning_nominal". + * Code generation for model "tuning_nominal". * * Model version : 1.1498 * Simulink Coder version : 8.12 (R2017a) 16-Feb-2017 - * C++ source code generated on : Fri Aug 31 14:28:54 2018 + * C++ source code generated on : Tue Sep 18 10:50:51 2018 * * Target selection: grt.tlc * Note: GRT includes extra infrastructure and instrumentation for prototyping @@ -18,25 +18,25 @@ * Validation result: Passed (1), Warnings (3), Error (0) */ -#include "tunning_nominal.h" -#include "tunning_nominal_private.h" +#include "tuning_nominal.h" +#include "tuning_nominal_private.h" -static void rate_scheduler(RT_MODEL_tunning_nominal_T *const tunning_nominal_M); +static void rate_scheduler(RT_MODEL_tuning_nominal_T *const tuning_nominal_M); /* * This function updates active task flag for each subrate. * The function is called at model base rate, hence the * generated code self-manages all its subrates. */ -static void rate_scheduler(RT_MODEL_tunning_nominal_T *const tunning_nominal_M) +static void rate_scheduler(RT_MODEL_tuning_nominal_T *const tuning_nominal_M) { /* Compute which subrates run during the next base time step. Subrates * are an integer multiple of the base rate counter. Therefore, the subtask * counter is reset when it reaches its limit (zero means run). */ - (tunning_nominal_M->Timing.TaskCounters.TID[2])++; - if ((tunning_nominal_M->Timing.TaskCounters.TID[2]) > 4) {/* Sample time: [0.005s, 0.0s] */ - tunning_nominal_M->Timing.TaskCounters.TID[2] = 0; + (tuning_nominal_M->Timing.TaskCounters.TID[2])++; + if ((tuning_nominal_M->Timing.TaskCounters.TID[2]) > 4) {/* Sample time: [0.005s, 0.0s] */ + tuning_nominal_M->Timing.TaskCounters.TID[2] = 0; } } @@ -44,8 +44,8 @@ static void rate_scheduler(RT_MODEL_tunning_nominal_T *const tunning_nominal_M) * This function updates continuous states using the ODE4 fixed-step * solver algorithm */ -void tunning_nominalModelClass::rt_ertODEUpdateContinuousStates(RTWSolverInfo - *si ) +void tuning_nominalModelClass::rt_ertODEUpdateContinuousStates(RTWSolverInfo *si + ) { time_T t = rtsiGetT(si); time_T tnew = rtsiGetSolverStopTime(si); @@ -69,7 +69,7 @@ void tunning_nominalModelClass::rt_ertODEUpdateContinuousStates(RTWSolverInfo /* Assumes that rtsiSetT and ModelOutputs are up-to-date */ /* f0 = f(t,y) */ rtsiSetdX(si, f0); - tunning_nominal_derivatives(); + tuning_nominal_derivatives(); /* f1 = f(t + (h/2), y + (h/2)*f0) */ temp = 0.5 * h; @@ -80,7 +80,7 @@ void tunning_nominalModelClass::rt_ertODEUpdateContinuousStates(RTWSolverInfo rtsiSetT(si, t + temp); rtsiSetdX(si, f1); this->step(); - tunning_nominal_derivatives(); + tuning_nominal_derivatives(); /* f2 = f(t + (h/2), y + (h/2)*f1) */ for (i = 0; i < nXc; i++) { @@ -89,7 +89,7 @@ void tunning_nominalModelClass::rt_ertODEUpdateContinuousStates(RTWSolverInfo rtsiSetdX(si, f2); this->step(); - tunning_nominal_derivatives(); + tuning_nominal_derivatives(); /* f3 = f(t + h, y + h*f2) */ for (i = 0; i < nXc; i++) { @@ -99,7 +99,7 @@ void tunning_nominalModelClass::rt_ertODEUpdateContinuousStates(RTWSolverInfo rtsiSetT(si, tnew); rtsiSetdX(si, f3); this->step(); - tunning_nominal_derivatives(); + tuning_nominal_derivatives(); /* tnew = t + h ynew = y + (h/6)*(f0 + 2*f1 + 2*f2 + 2*f3) */ @@ -112,29 +112,28 @@ void tunning_nominalModelClass::rt_ertODEUpdateContinuousStates(RTWSolverInfo } /* Model step function */ -void tunning_nominalModelClass::step() +void tuning_nominalModelClass::step() { /* local block i/o variables */ real_T rtb_Add[6]; - if (rtmIsMajorTimeStep((&tunning_nominal_M))) { + if (rtmIsMajorTimeStep((&tuning_nominal_M))) { /* set solver stop time */ - if (!((&tunning_nominal_M)->Timing.clockTick0+1)) { - rtsiSetSolverStopTime(&(&tunning_nominal_M)->solverInfo, - (((&tunning_nominal_M)->Timing.clockTickH0 + 1) * - (&tunning_nominal_M)->Timing.stepSize0 * 4294967296.0)); + if (!((&tuning_nominal_M)->Timing.clockTick0+1)) { + rtsiSetSolverStopTime(&(&tuning_nominal_M)->solverInfo, + (((&tuning_nominal_M)->Timing.clockTickH0 + 1) * + (&tuning_nominal_M)->Timing.stepSize0 * 4294967296.0)); } else { - rtsiSetSolverStopTime(&(&tunning_nominal_M)->solverInfo, - (((&tunning_nominal_M)->Timing.clockTick0 + 1) * - (&tunning_nominal_M)->Timing.stepSize0 + (&tunning_nominal_M) - ->Timing.clockTickH0 * (&tunning_nominal_M)->Timing.stepSize0 * + rtsiSetSolverStopTime(&(&tuning_nominal_M)->solverInfo, + (((&tuning_nominal_M)->Timing.clockTick0 + 1) * + (&tuning_nominal_M)->Timing.stepSize0 + (&tuning_nominal_M) + ->Timing.clockTickH0 * (&tuning_nominal_M)->Timing.stepSize0 * 4294967296.0)); } } /* end MajorTimeStep */ /* Update absolute time of base rate at minor time step */ - if (rtmIsMinorTimeStep((&tunning_nominal_M))) { - (&tunning_nominal_M)->Timing.t[0] = rtsiGetT(&(&tunning_nominal_M) - ->solverInfo); + if (rtmIsMinorTimeStep((&tuning_nominal_M))) { + (&tuning_nominal_M)->Timing.t[0] = rtsiGetT(&(&tuning_nominal_M)->solverInfo); } { @@ -163,13 +162,13 @@ void tunning_nominalModelClass::step() * Inport: '/X' * Inport: '/Y0' */ - rtb_d_z = tunning_nominal_U.X[2] - tunning_nominal_U.Y0[2]; + rtb_d_z = tuning_nominal_U.X[2] - tuning_nominal_U.Y0[2]; /* Sum: '/Sum1' incorporates: * Inport: '/X' * Inport: '/Y0' */ - rtb_d_x = tunning_nominal_U.X[0] - tunning_nominal_U.Y0[0]; + rtb_d_x = tuning_nominal_U.X[0] - tuning_nominal_U.Y0[0]; /* Sum: '/Sum2' incorporates: * Inport: '/X' @@ -179,15 +178,15 @@ void tunning_nominalModelClass::step() * Product: '/Product3' * SignalConversion: '/TmpSignal ConversionAtProduct3Inport2' */ - tunning_nominal_B.Sum2 = tunning_nominal_U.gain[2] * - tunning_nominal_X.Integrator1_CSTATE_d - (tunning_nominal_U.gain[0] * - rtb_d_x + tunning_nominal_U.gain[1] * tunning_nominal_U.X[3]); + tuning_nominal_B.Sum2 = tuning_nominal_U.gain[2] * + tuning_nominal_X.Integrator1_CSTATE_d - (tuning_nominal_U.gain[0] * + rtb_d_x + tuning_nominal_U.gain[1] * tuning_nominal_U.X[3]); /* Sum: '/Sum4' incorporates: * Inport: '/X' * Inport: '/Y0' */ - rtb_d_y = tunning_nominal_U.X[1] - tunning_nominal_U.Y0[1]; + rtb_d_y = tuning_nominal_U.X[1] - tuning_nominal_U.Y0[1]; /* Sum: '/Sum3' incorporates: * Inport: '/X' @@ -197,33 +196,33 @@ void tunning_nominalModelClass::step() * Product: '/Product4' * SignalConversion: '/TmpSignal ConversionAtProduct1Inport2' */ - tunning_nominal_B.Sum3 = tunning_nominal_U.gain[5] * - tunning_nominal_X.Integrator_CSTATE - (tunning_nominal_U.gain[3] * rtb_d_y - + tunning_nominal_U.gain[4] * tunning_nominal_U.X[4]); + tuning_nominal_B.Sum3 = tuning_nominal_U.gain[5] * + tuning_nominal_X.Integrator_CSTATE - (tuning_nominal_U.gain[3] * rtb_d_y + + tuning_nominal_U.gain[4] * tuning_nominal_U.X[4]); /* RateTransition: '/T_outer' incorporates: * Inport: '/X' */ - if ((rtmIsMajorTimeStep((&tunning_nominal_M)) && - (&tunning_nominal_M)->Timing.TaskCounters.TID[1] == 0) && - (rtmIsMajorTimeStep((&tunning_nominal_M)) && - (&tunning_nominal_M)->Timing.TaskCounters.TID[2] == 0)) { - tunning_nominal_B.T_outer[0] = tunning_nominal_B.Sum2; - tunning_nominal_B.T_outer[1] = tunning_nominal_B.Sum3; - tunning_nominal_B.T_outer[2] = tunning_nominal_U.X[8]; + if ((rtmIsMajorTimeStep((&tuning_nominal_M)) && + (&tuning_nominal_M)->Timing.TaskCounters.TID[1] == 0) && + (rtmIsMajorTimeStep((&tuning_nominal_M)) && + (&tuning_nominal_M)->Timing.TaskCounters.TID[2] == 0)) { + tuning_nominal_B.T_outer[0] = tuning_nominal_B.Sum2; + tuning_nominal_B.T_outer[1] = tuning_nominal_B.Sum3; + tuning_nominal_B.T_outer[2] = tuning_nominal_U.X[8]; } /* End of RateTransition: '/T_outer' */ - if (rtmIsMajorTimeStep((&tunning_nominal_M)) && - (&tunning_nominal_M)->Timing.TaskCounters.TID[2] == 0) { + if (rtmIsMajorTimeStep((&tuning_nominal_M)) && + (&tuning_nominal_M)->Timing.TaskCounters.TID[2] == 0) { /* Fcn: '/Fcn1' */ - tunning_nominal_B.Fcn1 = tunning_nominal_B.T_outer[1] * std::cos - (tunning_nominal_B.T_outer[2]) + tunning_nominal_B.T_outer[0] * std::sin - (tunning_nominal_B.T_outer[2]); + tuning_nominal_B.Fcn1 = tuning_nominal_B.T_outer[1] * std::cos + (tuning_nominal_B.T_outer[2]) + tuning_nominal_B.T_outer[0] * std::sin + (tuning_nominal_B.T_outer[2]); } /* Clock: '/Clock' */ - rtb_Clock = (&tunning_nominal_M)->Timing.t[0]; + rtb_Clock = (&tuning_nominal_M)->Timing.t[0]; /* MATLAB Function: '/FFW' incorporates: * Inport: '/X' @@ -237,7 +236,7 @@ void tunning_nominalModelClass::step() /* ':1:3' g = 9.81; */ /* [x;y] = [cos(t); sin(t)] */ /* ':1:5' if (test_mode == 2) */ - if ((tunning_nominal_U.mode == 2.0) && ((rtb_Clock >= 10.0) && (rtb_Clock <= + if ((tuning_nominal_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); */ @@ -247,36 +246,36 @@ void tunning_nominalModelClass::step() /* 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]; */ - rtb_ff_idx_0 = -((std::sin(tunning_nominal_U.X[6]) * std::sin - (tunning_nominal_U.X[7]) * std::cos(tunning_nominal_U.X - [8]) - std::cos(tunning_nominal_U.X[6]) * std::sin(tunning_nominal_U.X[8])) - * -std::cos(rtb_Clock) + (std::sin(tunning_nominal_U.X[6]) - * std::sin(tunning_nominal_U.X[7]) * std::sin(tunning_nominal_U.X[8]) + - std::cos(tunning_nominal_U.X[6]) * std::cos(tunning_nominal_U.X[8])) * - -std::sin(rtb_Clock)) / 9.81; - rtb_ff_idx_1 = (std::cos(tunning_nominal_U.X[7]) * std::cos - (tunning_nominal_U.X[8]) * -std::cos(rtb_Clock) + std::cos - (tunning_nominal_U.X[7]) * std::sin(tunning_nominal_U.X[8]) - * -std::sin(rtb_Clock)) / 9.81; + rtb_ff_idx_0 = -((std::sin(tuning_nominal_U.X[6]) * std::sin + (tuning_nominal_U.X[7]) * std::cos(tuning_nominal_U.X[8]) + - std::cos(tuning_nominal_U.X[6]) * std::sin + (tuning_nominal_U.X[8])) * -std::cos(rtb_Clock) + (std:: + sin(tuning_nominal_U.X[6]) * std::sin(tuning_nominal_U.X[7]) * std::sin + (tuning_nominal_U.X[8]) + std::cos(tuning_nominal_U.X[6]) * std::cos + (tuning_nominal_U.X[8])) * -std::sin(rtb_Clock)) / 9.81; + rtb_ff_idx_1 = (std::cos(tuning_nominal_U.X[7]) * std::cos + (tuning_nominal_U.X[8]) * -std::cos(rtb_Clock) + std::cos + (tuning_nominal_U.X[7]) * std::sin(tuning_nominal_U.X[8]) * + -std::sin(rtb_Clock)) / 9.81; } else { /* ':1:15' else */ /* ':1:16' ff = [0;0]; */ } /* End of MATLAB Function: '/FFW' */ - if (rtmIsMajorTimeStep((&tunning_nominal_M)) && - (&tunning_nominal_M)->Timing.TaskCounters.TID[2] == 0) { + if (rtmIsMajorTimeStep((&tuning_nominal_M)) && + (&tuning_nominal_M)->Timing.TaskCounters.TID[2] == 0) { /* Fcn: '/Fcn' */ - tunning_nominal_B.Fcn = -tunning_nominal_B.T_outer[1] * std::sin - (tunning_nominal_B.T_outer[2]) + tunning_nominal_B.T_outer[0] * std::cos - (tunning_nominal_B.T_outer[2]); + tuning_nominal_B.Fcn = -tuning_nominal_B.T_outer[1] * std::sin + (tuning_nominal_B.T_outer[2]) + tuning_nominal_B.T_outer[0] * std::cos + (tuning_nominal_B.T_outer[2]); } /* Sum: '/Sum6' incorporates: * Inport: '/X' * Inport: '/Y0' */ - rtb_d_psi = tunning_nominal_U.X[8] - tunning_nominal_U.Y0[3]; + rtb_d_psi = tuning_nominal_U.X[8] - tuning_nominal_U.Y0[3]; /* SignalConversion: '/TmpSignal ConversionAtControl AllocationInport1' incorporates: * Constant: '/ ' @@ -291,10 +290,10 @@ void tunning_nominalModelClass::step() * Sum: '/Sum1' * Sum: '/Sum1' */ - rtb_ref_idx_0 = ((tunning_nominal_U.gain[8] * - tunning_nominal_X.Integrator1_CSTATE - - (tunning_nominal_U.gain[6] * rtb_d_z + - tunning_nominal_U.gain[7] * tunning_nominal_U.X[5])) * + rtb_ref_idx_0 = ((tuning_nominal_U.gain[8] * + tuning_nominal_X.Integrator1_CSTATE - + (tuning_nominal_U.gain[6] * rtb_d_z + + tuning_nominal_U.gain[7] * tuning_nominal_U.X[5])) * 0.64935064935064934 + 9.81) * 1.54; /* Saturate: '/2Nm ' incorporates: @@ -305,9 +304,9 @@ void tunning_nominalModelClass::step() * Sum: '/Sum7' * Sum: '/Sum1' */ - rtb_ff_idx_0 = (tunning_nominal_B.Fcn1 + rtb_ff_idx_0) - - (tunning_nominal_U.gain[9] * tunning_nominal_U.X[6] + - tunning_nominal_U.gain[10] * tunning_nominal_U.X[9]); + rtb_ff_idx_0 = (tuning_nominal_B.Fcn1 + rtb_ff_idx_0) - + (tuning_nominal_U.gain[9] * tuning_nominal_U.X[6] + tuning_nominal_U.gain + [10] * tuning_nominal_U.X[9]); if (rtb_ff_idx_0 > 2.0) { /* SignalConversion: '/TmpSignal ConversionAtControl AllocationInport1' */ rtb_ff_idx_0 = 2.0; @@ -328,9 +327,9 @@ void tunning_nominalModelClass::step() * Sum: '/Sum8' * Sum: '/Sum1' */ - rtb_ff_idx_1 = (tunning_nominal_B.Fcn + rtb_ff_idx_1) - - (tunning_nominal_U.gain[12] * tunning_nominal_U.X[7] + - tunning_nominal_U.gain[13] * tunning_nominal_U.X[10]); + rtb_ff_idx_1 = (tuning_nominal_B.Fcn + rtb_ff_idx_1) - + (tuning_nominal_U.gain[12] * tuning_nominal_U.X[7] + + tuning_nominal_U.gain[13] * tuning_nominal_U.X[10]); if (rtb_ff_idx_1 > 2.0) { /* SignalConversion: '/TmpSignal ConversionAtControl AllocationInport1' */ rtb_ff_idx_1 = 2.0; @@ -352,9 +351,9 @@ void tunning_nominalModelClass::step() * SignalConversion: '/TmpSignal ConversionAtProduct1Inport2' * Sum: '/Sum1' */ - rtb_ref_idx_3 = tunning_nominal_U.gain[17] * - tunning_nominal_X.Integrator1_CSTATE_j - (tunning_nominal_U.gain[15] * - rtb_d_psi + tunning_nominal_U.gain[16] * tunning_nominal_U.X[11]); + rtb_ref_idx_3 = tuning_nominal_U.gain[17] * + tuning_nominal_X.Integrator1_CSTATE_j - (tuning_nominal_U.gain[15] * + rtb_d_psi + tuning_nominal_U.gain[16] * tuning_nominal_U.X[11]); if (rtb_ref_idx_3 > 1.0) { /* SignalConversion: '/TmpSignal ConversionAtControl AllocationInport1' */ rtb_ref_idx_3 = 1.0; @@ -373,21 +372,20 @@ void tunning_nominalModelClass::step() /* Gain: '/Control Allocation' incorporates: * Saturate: '/ ' */ - u0 = tunning_nominal_ConstP.ControlAllocation_Gain[i + 18] * rtb_ref_idx_3 - + (tunning_nominal_ConstP.ControlAllocation_Gain[i + 12] * rtb_ff_idx_1 - + (tunning_nominal_ConstP.ControlAllocation_Gain[i + 6] * - rtb_ff_idx_0 + tunning_nominal_ConstP.ControlAllocation_Gain[i] * - rtb_ref_idx_0)); + u0 = tuning_nominal_ConstP.ControlAllocation_Gain[i + 18] * rtb_ref_idx_3 + + (tuning_nominal_ConstP.ControlAllocation_Gain[i + 12] * rtb_ff_idx_1 + + (tuning_nominal_ConstP.ControlAllocation_Gain[i + 6] * rtb_ff_idx_0 + + tuning_nominal_ConstP.ControlAllocation_Gain[i] * rtb_ref_idx_0)); /* Saturate: '/ ' incorporates: * Gain: '/Control Allocation' */ if (u0 > 8.54858) { - tunning_nominal_B.u[i] = 8.54858; + tuning_nominal_B.u[i] = 8.54858; } else if (u0 < 0.0) { - tunning_nominal_B.u[i] = 0.0; + tuning_nominal_B.u[i] = 0.0; } else { - tunning_nominal_B.u[i] = u0; + tuning_nominal_B.u[i] = u0; } /* MATLAB Function: '/LOE_' incorporates: @@ -397,9 +395,9 @@ void tunning_nominalModelClass::step() rtb_LOE_out[i] = 0.0; /* ':1:4' if t>= LOE_t(i) */ - if (rtb_Clock >= tunning_nominal_U.LOE_t[i]) { + if (rtb_Clock >= tuning_nominal_U.LOE_t[i]) { /* ':1:5' LOE_out(i) = LOE(i); */ - rtb_LOE_out[i] = tunning_nominal_U.LOE_a[i]; + rtb_LOE_out[i] = tuning_nominal_U.LOE_a[i]; } /* End of MATLAB Function: '/LOE_' */ @@ -414,12 +412,12 @@ void tunning_nominalModelClass::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_T_f[0] = (1.0 - rtb_LOE_out[0]) * tunning_nominal_B.u[0]; - rtb_T_f[1] = (1.0 - rtb_LOE_out[1]) * tunning_nominal_B.u[1]; - rtb_T_f[2] = (1.0 - rtb_LOE_out[2]) * tunning_nominal_B.u[2]; - rtb_T_f[3] = (1.0 - rtb_LOE_out[3]) * tunning_nominal_B.u[3]; - rtb_T_f[4] = (1.0 - rtb_LOE_out[4]) * tunning_nominal_B.u[4]; - rtb_T_f[5] = (1.0 - rtb_LOE_out[5]) * tunning_nominal_B.u[5]; + rtb_T_f[0] = (1.0 - rtb_LOE_out[0]) * tuning_nominal_B.u[0]; + rtb_T_f[1] = (1.0 - rtb_LOE_out[1]) * tuning_nominal_B.u[1]; + rtb_T_f[2] = (1.0 - rtb_LOE_out[2]) * tuning_nominal_B.u[2]; + rtb_T_f[3] = (1.0 - rtb_LOE_out[3]) * tuning_nominal_B.u[3]; + rtb_T_f[4] = (1.0 - rtb_LOE_out[4]) * tuning_nominal_B.u[4]; + rtb_T_f[5] = (1.0 - rtb_LOE_out[5]) * tuning_nominal_B.u[5]; /* Outport: '/motor_command' */ for (i = 0; i < 6; i++) { @@ -434,11 +432,11 @@ void tunning_nominalModelClass::step() /* Saturate: '/Saturation' */ if (u0 > 200.0) { - tunning_nominal_Y.motor_command[i] = 200.0; + tuning_nominal_Y.motor_command[i] = 200.0; } else if (u0 < 0.0) { - tunning_nominal_Y.motor_command[i] = 0.0; + tuning_nominal_Y.motor_command[i] = 0.0; } else { - tunning_nominal_Y.motor_command[i] = u0; + tuning_nominal_Y.motor_command[i] = u0; } /* End of Saturate: '/Saturation' */ @@ -447,10 +445,10 @@ void tunning_nominalModelClass::step() /* End of Outport: '/motor_command' */ /* Outport: '/virtual_control' */ - tunning_nominal_Y.virtual_control[0] = rtb_ref_idx_0; - tunning_nominal_Y.virtual_control[1] = rtb_ff_idx_0; - tunning_nominal_Y.virtual_control[2] = rtb_ff_idx_1; - tunning_nominal_Y.virtual_control[3] = rtb_ref_idx_3; + tuning_nominal_Y.virtual_control[0] = rtb_ref_idx_0; + tuning_nominal_Y.virtual_control[1] = rtb_ff_idx_0; + tuning_nominal_Y.virtual_control[2] = rtb_ff_idx_1; + tuning_nominal_Y.virtual_control[3] = rtb_ref_idx_3; /* MATLAB Function: '/MATLAB Function' incorporates: * Inport: '/Y0' @@ -460,15 +458,15 @@ void tunning_nominalModelClass::step() /* MATLAB Function 'Test_config_and_data/MATLAB Function': ':1' */ /* ':1:2' ref = Y0; */ /* ':1:3' switch test_mode */ - switch ((int32_T)tunning_nominal_U.mode) { + switch ((int32_T)tuning_nominal_U.mode) { case 0: /* ':1:4' case 0 % manual test */ /* manual test */ /* ':1:5' ref = ref_manual; */ - rtb_ref_idx_0 = tunning_nominal_U.ref[0]; - rtb_ff_idx_0 = tunning_nominal_U.ref[1]; - rtb_ff_idx_1 = tunning_nominal_U.ref[2]; - rtb_ref_idx_3 = tunning_nominal_U.ref[3]; + rtb_ref_idx_0 = tuning_nominal_U.ref[0]; + rtb_ff_idx_0 = tuning_nominal_U.ref[1]; + rtb_ff_idx_1 = tuning_nominal_U.ref[2]; + rtb_ref_idx_3 = tuning_nominal_U.ref[3]; break; case 1: @@ -477,45 +475,45 @@ void tunning_nominalModelClass::step() /* ':1:7' if t<=15 */ if (rtb_Clock <= 15.0) { /* ':1:8' ref = [Y0(1); Y0(2); 1; Y0(4)]; */ - rtb_ref_idx_0 = tunning_nominal_U.Y0[0]; - rtb_ff_idx_0 = tunning_nominal_U.Y0[1]; + rtb_ref_idx_0 = tuning_nominal_U.Y0[0]; + rtb_ff_idx_0 = tuning_nominal_U.Y0[1]; rtb_ff_idx_1 = 1.0; - rtb_ref_idx_3 = tunning_nominal_U.Y0[3]; + rtb_ref_idx_3 = tuning_nominal_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)]; */ - rtb_ref_idx_0 = tunning_nominal_U.Y0[0] + 1.0; - rtb_ff_idx_0 = tunning_nominal_U.Y0[1] - 1.0; + rtb_ref_idx_0 = tuning_nominal_U.Y0[0] + 1.0; + rtb_ff_idx_0 = tuning_nominal_U.Y0[1] - 1.0; rtb_ff_idx_1 = 1.0; - rtb_ref_idx_3 = tunning_nominal_U.Y0[3]; + rtb_ref_idx_3 = tuning_nominal_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)]; */ - rtb_ref_idx_0 = tunning_nominal_U.Y0[0] - 1.0; - rtb_ff_idx_0 = tunning_nominal_U.Y0[1] + 1.0; + rtb_ref_idx_0 = tuning_nominal_U.Y0[0] - 1.0; + rtb_ff_idx_0 = tuning_nominal_U.Y0[1] + 1.0; rtb_ff_idx_1 = 1.0; - rtb_ref_idx_3 = tunning_nominal_U.Y0[3]; + rtb_ref_idx_3 = tuning_nominal_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)]; */ - rtb_ref_idx_0 = tunning_nominal_U.Y0[0] - 1.0; - rtb_ff_idx_0 = tunning_nominal_U.Y0[1] + 1.0; + rtb_ref_idx_0 = tuning_nominal_U.Y0[0] - 1.0; + rtb_ff_idx_0 = tuning_nominal_U.Y0[1] + 1.0; rtb_ff_idx_1 = 1.0; - rtb_ref_idx_3 = tunning_nominal_U.Y0[3]; + rtb_ref_idx_3 = tuning_nominal_U.Y0[3]; } else if (rtb_Clock <= 55.0) { /* ':1:15' elseif t <=55 */ /* ':1:16' ref = [Y0(1); Y0(2); 1; Y0(4)]; */ - rtb_ref_idx_0 = tunning_nominal_U.Y0[0]; - rtb_ff_idx_0 = tunning_nominal_U.Y0[1]; + rtb_ref_idx_0 = tuning_nominal_U.Y0[0]; + rtb_ff_idx_0 = tuning_nominal_U.Y0[1]; rtb_ff_idx_1 = 1.0; - rtb_ref_idx_3 = tunning_nominal_U.Y0[3]; + rtb_ref_idx_3 = tuning_nominal_U.Y0[3]; } else { /* ':1:17' else */ /* ':1:18' ref = Y0; */ - rtb_ref_idx_0 = tunning_nominal_U.Y0[0]; - rtb_ff_idx_0 = tunning_nominal_U.Y0[1]; - rtb_ff_idx_1 = tunning_nominal_U.Y0[2]; - rtb_ref_idx_3 = tunning_nominal_U.Y0[3]; + rtb_ref_idx_0 = tuning_nominal_U.Y0[0]; + rtb_ff_idx_0 = tuning_nominal_U.Y0[1]; + rtb_ff_idx_1 = tuning_nominal_U.Y0[2]; + rtb_ref_idx_3 = tuning_nominal_U.Y0[3]; } /* if t<=10 */ @@ -537,31 +535,31 @@ void tunning_nominalModelClass::step() /* ':1:32' if t<=10 */ if (rtb_Clock <= 10.0) { /* ':1:33' ref = [Y0(1); Y0(2); 0.75; Y0(4)]; */ - rtb_ref_idx_0 = tunning_nominal_U.Y0[0]; - rtb_ff_idx_0 = tunning_nominal_U.Y0[1]; + rtb_ref_idx_0 = tuning_nominal_U.Y0[0]; + rtb_ff_idx_0 = tuning_nominal_U.Y0[1]; rtb_ff_idx_1 = 0.75; - rtb_ref_idx_3 = tunning_nominal_U.Y0[3]; + rtb_ref_idx_3 = tuning_nominal_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)]; */ - rtb_ref_idx_0 = tunning_nominal_U.Y0[0] + std::cos(rtb_Clock); - rtb_ff_idx_0 = tunning_nominal_U.Y0[1] + std::sin(rtb_Clock); + rtb_ref_idx_0 = tuning_nominal_U.Y0[0] + std::cos(rtb_Clock); + rtb_ff_idx_0 = tuning_nominal_U.Y0[1] + std::sin(rtb_Clock); rtb_ff_idx_1 = 0.75; - rtb_ref_idx_3 = tunning_nominal_U.Y0[3]; + rtb_ref_idx_3 = tuning_nominal_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)]; */ - rtb_ref_idx_0 = tunning_nominal_U.Y0[0]; - rtb_ff_idx_0 = tunning_nominal_U.Y0[1]; + rtb_ref_idx_0 = tuning_nominal_U.Y0[0]; + rtb_ff_idx_0 = tuning_nominal_U.Y0[1]; rtb_ff_idx_1 = 0.75; - rtb_ref_idx_3 = tunning_nominal_U.Y0[3]; + rtb_ref_idx_3 = tuning_nominal_U.Y0[3]; } else { /* ':1:38' else */ /* ':1:39' ref = Y0; */ - rtb_ref_idx_0 = tunning_nominal_U.Y0[0]; - rtb_ff_idx_0 = tunning_nominal_U.Y0[1]; - rtb_ff_idx_1 = tunning_nominal_U.Y0[2]; - rtb_ref_idx_3 = tunning_nominal_U.Y0[3]; + rtb_ref_idx_0 = tuning_nominal_U.Y0[0]; + rtb_ff_idx_0 = tuning_nominal_U.Y0[1]; + rtb_ff_idx_1 = tuning_nominal_U.Y0[2]; + rtb_ref_idx_3 = tuning_nominal_U.Y0[3]; } /* ref = [cos(t); sin(t);min(t,15);Y0(4)]; */ @@ -570,44 +568,44 @@ void tunning_nominalModelClass::step() default: /* ':1:42' otherwise */ /* ':1:43' ref = Y0; */ - rtb_ref_idx_0 = tunning_nominal_U.Y0[0]; - rtb_ff_idx_0 = tunning_nominal_U.Y0[1]; - rtb_ff_idx_1 = tunning_nominal_U.Y0[2]; - rtb_ref_idx_3 = tunning_nominal_U.Y0[3]; + rtb_ref_idx_0 = tuning_nominal_U.Y0[0]; + rtb_ff_idx_0 = tuning_nominal_U.Y0[1]; + rtb_ff_idx_1 = tuning_nominal_U.Y0[2]; + rtb_ref_idx_3 = tuning_nominal_U.Y0[3]; break; } /* End of MATLAB Function: '/MATLAB Function' */ /* Outport: '/ref_out' */ - tunning_nominal_Y.ref_out[0] = rtb_ref_idx_0; - tunning_nominal_Y.ref_out[1] = rtb_ff_idx_0; - tunning_nominal_Y.ref_out[2] = rtb_ff_idx_1; - tunning_nominal_Y.ref_out[3] = rtb_ref_idx_3; + tuning_nominal_Y.ref_out[0] = rtb_ref_idx_0; + tuning_nominal_Y.ref_out[1] = rtb_ff_idx_0; + tuning_nominal_Y.ref_out[2] = rtb_ff_idx_1; + tuning_nominal_Y.ref_out[3] = rtb_ref_idx_3; /* Outport: '/LOE_true' */ for (i = 0; i < 6; i++) { - tunning_nominal_Y.LOE_true[i] = rtb_LOE_out[i]; + tuning_nominal_Y.LOE_true[i] = rtb_LOE_out[i]; } /* End of Outport: '/LOE_true' */ - if (rtmIsMajorTimeStep((&tunning_nominal_M)) && - (&tunning_nominal_M)->Timing.TaskCounters.TID[1] == 0) { + if (rtmIsMajorTimeStep((&tuning_nominal_M)) && + (&tuning_nominal_M)->Timing.TaskCounters.TID[1] == 0) { /* Delay: '/MemoryX' */ for (i = 0; i < 6; i++) { - if (tunning_nominal_DW.icLoad != 0) { - tunning_nominal_DW.MemoryX_DSTATE[i] = 0.0; + if (tuning_nominal_DW.icLoad != 0) { + tuning_nominal_DW.MemoryX_DSTATE[i] = 0.0; } - rtb_MemoryX[i] = tunning_nominal_DW.MemoryX_DSTATE[i]; + rtb_MemoryX[i] = tuning_nominal_DW.MemoryX_DSTATE[i]; } /* Outputs for Atomic SubSystem: '/UseCurrentEstimator' */ /* Outputs for Enabled SubSystem: '/Enabled Subsystem' incorporates: * EnablePort: '/Enable' */ - if (!tunning_nominal_DW.EnabledSubsystem_MODE) { - tunning_nominal_DW.EnabledSubsystem_MODE = true; + if (!tuning_nominal_DW.EnabledSubsystem_MODE) { + tuning_nominal_DW.EnabledSubsystem_MODE = true; } /* Sum: '/Add1' incorporates: @@ -620,11 +618,11 @@ void tunning_nominalModelClass::step() for (i = 0; i < 3; i++) { u0 = 0.0; for (i_0 = 0; i_0 < 6; i_0++) { - u0 += tunning_nominal_ConstP.C_Value[3 * i_0 + i] * - tunning_nominal_DW.MemoryX_DSTATE[i_0]; + u0 += tuning_nominal_ConstP.C_Value[3 * i_0 + i] * + tuning_nominal_DW.MemoryX_DSTATE[i_0]; } - rtb_Sum1_l[i] = tunning_nominal_U.X[9 + i] - u0; + rtb_Sum1_l[i] = tuning_nominal_U.X[9 + i] - u0; } /* End of Sum: '/Add1' */ @@ -632,19 +630,19 @@ void tunning_nominalModelClass::step() /* Product: '/Product2' incorporates: * Constant: '/KalmanGainM' */ - tunning_nominal_B.Product2[i] = 0.0; - tunning_nominal_B.Product2[i] += - tunning_nominal_ConstP.KalmanGainM_Value[i] * rtb_Sum1_l[0]; - tunning_nominal_B.Product2[i] += - tunning_nominal_ConstP.KalmanGainM_Value[i + 6] * rtb_Sum1_l[1]; - tunning_nominal_B.Product2[i] += - tunning_nominal_ConstP.KalmanGainM_Value[i + 12] * rtb_Sum1_l[2]; + tuning_nominal_B.Product2[i] = 0.0; + tuning_nominal_B.Product2[i] += + tuning_nominal_ConstP.KalmanGainM_Value[i] * rtb_Sum1_l[0]; + tuning_nominal_B.Product2[i] += + tuning_nominal_ConstP.KalmanGainM_Value[i + 6] * rtb_Sum1_l[1]; + tuning_nominal_B.Product2[i] += + tuning_nominal_ConstP.KalmanGainM_Value[i + 12] * rtb_Sum1_l[2]; /* Sum: '/Add' incorporates: * Delay: '/MemoryX' */ - rtb_Add_a[i] = tunning_nominal_B.Product2[i] + - tunning_nominal_DW.MemoryX_DSTATE[i]; + rtb_Add_a[i] = tuning_nominal_B.Product2[i] + + tuning_nominal_DW.MemoryX_DSTATE[i]; } /* End of Outputs for SubSystem: '/Enabled Subsystem' */ @@ -654,11 +652,10 @@ void tunning_nominalModelClass::step() * Constant: '/Constant1' */ for (i = 0; i < 3; i++) { - rtb_Sum1_l[i] = tunning_nominal_ConstP.Constant1_Value[i + 6] * - rtb_Add_a[2] + (tunning_nominal_ConstP.Constant1_Value[i + 3] * - rtb_Add_a[1] + - tunning_nominal_ConstP.Constant1_Value[i] * rtb_Add_a - [0]); + rtb_Sum1_l[i] = tuning_nominal_ConstP.Constant1_Value[i + 6] * + rtb_Add_a[2] + (tuning_nominal_ConstP.Constant1_Value[i + 3] * + rtb_Add_a[1] + tuning_nominal_ConstP.Constant1_Value[i] + * rtb_Add_a[0]); } /* End of Product: '/Product1' */ @@ -685,13 +682,12 @@ void tunning_nominalModelClass::step() * Sum: '/Sum1' */ for (i = 0; i < 3; i++) { - rtb_Sum3_i[i] = (((tunning_nominal_ConstP.Constant1_Value[i + 3] * + rtb_Sum3_i[i] = (((tuning_nominal_ConstP.Constant1_Value[i + 3] * rtb_Add_a[4] + - tunning_nominal_ConstP.Constant1_Value[i] * - rtb_Add_a[3]) + - tunning_nominal_ConstP.Constant1_Value[i + 6] * + tuning_nominal_ConstP.Constant1_Value[i] * rtb_Add_a + [3]) + tuning_nominal_ConstP.Constant1_Value[i + 6] * rtb_Add_a[5]) + - tunning_nominal_ConstP.Constant_Value_g[i]) + + tuning_nominal_ConstP.Constant_Value_g[i]) + (rtb_Add_f[i] - rtb_Add_f_0[i]); } @@ -726,66 +722,66 @@ void tunning_nominalModelClass::step() /* ':1:14' gamma = [Residu(1)/u(1) */ /* ':1:15' Residu(2)/u(2) */ /* ':1:16' Residu(3)/u(3)]; */ - tunning_nominal_Y.LOE13_estimated[0] = - ((tunning_nominal_DW.Memory_PreviousInput[0] - - tunning_nominal_DW.Memory_PreviousInput[3]) - rtb_Sum1_l[0]) / - tunning_nominal_DW.Memory_PreviousInput[0]; - tunning_nominal_Y.LOE13_estimated[1] = - ((tunning_nominal_DW.Memory_PreviousInput[1] - - tunning_nominal_DW.Memory_PreviousInput[4]) - rtb_Sum1_l[1]) / - tunning_nominal_DW.Memory_PreviousInput[1]; - tunning_nominal_Y.LOE13_estimated[2] = - ((tunning_nominal_DW.Memory_PreviousInput[2] - - tunning_nominal_DW.Memory_PreviousInput[5]) - rtb_Sum1_l[2]) / - tunning_nominal_DW.Memory_PreviousInput[2]; + tuning_nominal_Y.LOE13_estimated[0] = + ((tuning_nominal_DW.Memory_PreviousInput[0] - + tuning_nominal_DW.Memory_PreviousInput[3]) - rtb_Sum1_l[0]) / + tuning_nominal_DW.Memory_PreviousInput[0]; + tuning_nominal_Y.LOE13_estimated[1] = + ((tuning_nominal_DW.Memory_PreviousInput[1] - + tuning_nominal_DW.Memory_PreviousInput[4]) - rtb_Sum1_l[1]) / + tuning_nominal_DW.Memory_PreviousInput[1]; + tuning_nominal_Y.LOE13_estimated[2] = + ((tuning_nominal_DW.Memory_PreviousInput[2] - + tuning_nominal_DW.Memory_PreviousInput[5]) - rtb_Sum1_l[2]) / + tuning_nominal_DW.Memory_PreviousInput[2]; } for (i = 0; i < 6; i++) { /* Outport: '/thrust_pre' */ - tunning_nominal_Y.thrust_pre[i] = tunning_nominal_B.u[i]; + tuning_nominal_Y.thrust_pre[i] = tuning_nominal_B.u[i]; /* Outport: '/thrust_after' */ - tunning_nominal_Y.thrust_after[i] = rtb_T_f[i]; + tuning_nominal_Y.thrust_after[i] = rtb_T_f[i]; } - if (rtmIsMajorTimeStep((&tunning_nominal_M)) && - (&tunning_nominal_M)->Timing.TaskCounters.TID[1] == 0) { + if (rtmIsMajorTimeStep((&tuning_nominal_M)) && + (&tuning_nominal_M)->Timing.TaskCounters.TID[1] == 0) { /* Outport: '/acc_Kalman' */ - tunning_nominal_Y.acc_Kalman[0] = rtb_Add_a[3]; + tuning_nominal_Y.acc_Kalman[0] = rtb_Add_a[3]; /* Outport: '/M_Kalman' */ - tunning_nominal_Y.M_Kalman[0] = rtb_Sum3_i[0]; + tuning_nominal_Y.M_Kalman[0] = rtb_Sum3_i[0]; /* Outport: '/vel_Kalman' */ - tunning_nominal_Y.vel_Kalman[0] = rtb_Add_a[0]; + tuning_nominal_Y.vel_Kalman[0] = rtb_Add_a[0]; /* Outport: '/acc_Kalman' */ - tunning_nominal_Y.acc_Kalman[1] = rtb_Add_a[4]; + tuning_nominal_Y.acc_Kalman[1] = rtb_Add_a[4]; /* Outport: '/M_Kalman' */ - tunning_nominal_Y.M_Kalman[1] = rtb_Sum3_i[1]; + tuning_nominal_Y.M_Kalman[1] = rtb_Sum3_i[1]; /* Outport: '/vel_Kalman' */ - tunning_nominal_Y.vel_Kalman[1] = rtb_Add_a[1]; + tuning_nominal_Y.vel_Kalman[1] = rtb_Add_a[1]; /* Outport: '/acc_Kalman' */ - tunning_nominal_Y.acc_Kalman[2] = rtb_Add_a[5]; + tuning_nominal_Y.acc_Kalman[2] = rtb_Add_a[5]; /* Outport: '/M_Kalman' */ - tunning_nominal_Y.M_Kalman[2] = rtb_Sum3_i[2]; + tuning_nominal_Y.M_Kalman[2] = rtb_Sum3_i[2]; /* Outport: '/vel_Kalman' */ - tunning_nominal_Y.vel_Kalman[2] = rtb_Add_a[2]; + tuning_nominal_Y.vel_Kalman[2] = rtb_Add_a[2]; /* Outputs for Enabled SubSystem: '/MeasurementUpdate' incorporates: * EnablePort: '/Enable' */ - if (rtmIsMajorTimeStep((&tunning_nominal_M)) && - (!tunning_nominal_DW.MeasurementUpdate_MODE)) { - tunning_nominal_DW.MeasurementUpdate_MODE = true; + if (rtmIsMajorTimeStep((&tuning_nominal_M)) && + (!tuning_nominal_DW.MeasurementUpdate_MODE)) { + tuning_nominal_DW.MeasurementUpdate_MODE = true; } - if (tunning_nominal_DW.MeasurementUpdate_MODE) { + if (tuning_nominal_DW.MeasurementUpdate_MODE) { for (i = 0; i < 3; i++) { /* Product: '/C[k]*xhat[k|k-1]' incorporates: * Constant: '/C' @@ -793,7 +789,7 @@ void tunning_nominalModelClass::step() */ rtb_Sum1_l[i] = 0.0; for (i_0 = 0; i_0 < 6; i_0++) { - rtb_Sum1_l[i] += tunning_nominal_ConstP.C_Value[3 * i_0 + i] * + rtb_Sum1_l[i] += tuning_nominal_ConstP.C_Value[3 * i_0 + i] * rtb_MemoryX[i_0]; } @@ -804,20 +800,20 @@ void tunning_nominalModelClass::step() * Product: '/Product3' * Sum: '/Add1' */ - rtb_Add_f[i] = tunning_nominal_U.X[9 + i] - rtb_Sum1_l[i]; + rtb_Add_f[i] = tuning_nominal_U.X[9 + i] - rtb_Sum1_l[i]; } /* Product: '/Product3' incorporates: * Constant: '/KalmanGainL' */ for (i = 0; i < 6; i++) { - tunning_nominal_B.Product3[i] = 0.0; - tunning_nominal_B.Product3[i] += - tunning_nominal_ConstP.KalmanGainL_Value[i] * rtb_Add_f[0]; - tunning_nominal_B.Product3[i] += - tunning_nominal_ConstP.KalmanGainL_Value[i + 6] * rtb_Add_f[1]; - tunning_nominal_B.Product3[i] += - tunning_nominal_ConstP.KalmanGainL_Value[i + 12] * rtb_Add_f[2]; + tuning_nominal_B.Product3[i] = 0.0; + tuning_nominal_B.Product3[i] += + tuning_nominal_ConstP.KalmanGainL_Value[i] * rtb_Add_f[0]; + tuning_nominal_B.Product3[i] += + tuning_nominal_ConstP.KalmanGainL_Value[i + 6] * rtb_Add_f[1]; + tuning_nominal_B.Product3[i] += + tuning_nominal_ConstP.KalmanGainL_Value[i + 12] * rtb_Add_f[2]; } } @@ -829,24 +825,24 @@ void tunning_nominalModelClass::step() */ rtb_LOE_out[i] = 0.0; for (i_0 = 0; i_0 < 6; i_0++) { - rtb_LOE_out[i] += tunning_nominal_ConstP.A_Value[6 * i_0 + i] * + rtb_LOE_out[i] += tuning_nominal_ConstP.A_Value[6 * i_0 + i] * rtb_MemoryX[i_0]; } /* End of Product: '/A[k]*xhat[k|k-1]' */ /* Sum: '/Add' */ - rtb_Add[i] = rtb_LOE_out[i] + tunning_nominal_B.Product3[i]; + rtb_Add[i] = rtb_LOE_out[i] + tuning_nominal_B.Product3[i]; } } /* Sum: '/Sum' incorporates: * Inport: '/Y0' */ - rtb_ref_idx_0 -= tunning_nominal_U.Y0[0]; - rtb_ff_idx_0 -= tunning_nominal_U.Y0[1]; - rtb_ff_idx_1 -= tunning_nominal_U.Y0[2]; - rtb_Clock = rtb_ref_idx_3 - tunning_nominal_U.Y0[3]; + rtb_ref_idx_0 -= tuning_nominal_U.Y0[0]; + rtb_ff_idx_0 -= tuning_nominal_U.Y0[1]; + rtb_ff_idx_1 -= tuning_nominal_U.Y0[2]; + rtb_Clock = rtb_ref_idx_3 - tuning_nominal_U.Y0[3]; /* Saturate: '/x' */ if (rtb_ref_idx_0 > 2.0) { @@ -860,7 +856,7 @@ void tunning_nominalModelClass::step() /* End of Saturate: '/x' */ /* Sum: '/Sum1' */ - tunning_nominal_B.Sum1 = rtb_ref_idx_0 - rtb_d_x; + tuning_nominal_B.Sum1 = rtb_ref_idx_0 - rtb_d_x; /* Saturate: '/y' */ if (rtb_ff_idx_0 > 2.0) { @@ -874,7 +870,7 @@ void tunning_nominalModelClass::step() /* End of Saturate: '/y' */ /* Sum: '/Sum4' */ - tunning_nominal_B.Sum4 = rtb_ff_idx_0 - rtb_d_y; + tuning_nominal_B.Sum4 = rtb_ff_idx_0 - rtb_d_y; /* Saturate: '/yaw' */ if (rtb_Clock > 3.1415926535897931) { @@ -888,7 +884,7 @@ void tunning_nominalModelClass::step() /* End of Saturate: '/yaw' */ /* Sum: '/Sum3' */ - tunning_nominal_B.Sum3_n = rtb_Clock - rtb_d_psi; + tuning_nominal_B.Sum3_n = rtb_Clock - rtb_d_psi; /* Saturate: '/z' */ if (rtb_ff_idx_1 > 1.75) { @@ -902,27 +898,27 @@ void tunning_nominalModelClass::step() /* End of Saturate: '/z' */ /* Sum: '/Sum3' */ - tunning_nominal_B.Sum3_h = rtb_ff_idx_1 - rtb_d_z; + tuning_nominal_B.Sum3_h = rtb_ff_idx_1 - rtb_d_z; } - if (rtmIsMajorTimeStep((&tunning_nominal_M))) { + if (rtmIsMajorTimeStep((&tuning_nominal_M))) { int32_T i; - if (rtmIsMajorTimeStep((&tunning_nominal_M)) && - (&tunning_nominal_M)->Timing.TaskCounters.TID[1] == 0) { + if (rtmIsMajorTimeStep((&tuning_nominal_M)) && + (&tuning_nominal_M)->Timing.TaskCounters.TID[1] == 0) { /* Update for Delay: '/MemoryX' */ - tunning_nominal_DW.icLoad = 0U; + tuning_nominal_DW.icLoad = 0U; for (i = 0; i < 6; i++) { /* Update for Memory: '/Memory' */ - tunning_nominal_DW.Memory_PreviousInput[i] = tunning_nominal_B.u[i]; + tuning_nominal_DW.Memory_PreviousInput[i] = tuning_nominal_B.u[i]; /* Update for Delay: '/MemoryX' */ - tunning_nominal_DW.MemoryX_DSTATE[i] = rtb_Add[i]; + tuning_nominal_DW.MemoryX_DSTATE[i] = rtb_Add[i]; } } } /* end MajorTimeStep */ - if (rtmIsMajorTimeStep((&tunning_nominal_M))) { - rt_ertODEUpdateContinuousStates(&(&tunning_nominal_M)->solverInfo); + if (rtmIsMajorTimeStep((&tuning_nominal_M))) { + rt_ertODEUpdateContinuousStates(&(&tuning_nominal_M)->solverInfo); /* Update absolute time for base rate */ /* The "clockTick0" counts the number of times the code of this task has @@ -933,12 +929,12 @@ void tunning_nominalModelClass::step() * The two integers represent the low bits Timing.clockTick0 and the high bits * Timing.clockTickH0. When the low bit overflows to 0, the high bits increment. */ - if (!(++(&tunning_nominal_M)->Timing.clockTick0)) { - ++(&tunning_nominal_M)->Timing.clockTickH0; + if (!(++(&tuning_nominal_M)->Timing.clockTick0)) { + ++(&tuning_nominal_M)->Timing.clockTickH0; } - (&tunning_nominal_M)->Timing.t[0] = rtsiGetSolverStopTime - (&(&tunning_nominal_M)->solverInfo); + (&tuning_nominal_M)->Timing.t[0] = rtsiGetSolverStopTime(&(&tuning_nominal_M) + ->solverInfo); { /* Update absolute timer for sample time: [0.001s, 0.0s] */ @@ -950,133 +946,131 @@ void tunning_nominalModelClass::step() * The two integers represent the low bits Timing.clockTick1 and the high bits * Timing.clockTickH1. When the low bit overflows to 0, the high bits increment. */ - (&tunning_nominal_M)->Timing.clockTick1++; - if (!(&tunning_nominal_M)->Timing.clockTick1) { - (&tunning_nominal_M)->Timing.clockTickH1++; + (&tuning_nominal_M)->Timing.clockTick1++; + if (!(&tuning_nominal_M)->Timing.clockTick1) { + (&tuning_nominal_M)->Timing.clockTickH1++; } } - rate_scheduler((&tunning_nominal_M)); + rate_scheduler((&tuning_nominal_M)); } /* end MajorTimeStep */ } /* Derivatives for root system: '' */ -void tunning_nominalModelClass::tunning_nominal_derivatives() +void tuning_nominalModelClass::tuning_nominal_derivatives() { - XDot_tunning_nominal_T *_rtXdot; - _rtXdot = ((XDot_tunning_nominal_T *) (&tunning_nominal_M)->derivs); + XDot_tuning_nominal_T *_rtXdot; + _rtXdot = ((XDot_tuning_nominal_T *) (&tuning_nominal_M)->derivs); /* Derivatives for Integrator: '/Integrator1' */ - _rtXdot->Integrator1_CSTATE = tunning_nominal_B.Sum3_h; + _rtXdot->Integrator1_CSTATE = tuning_nominal_B.Sum3_h; /* Derivatives for Integrator: '/Integrator1' */ - _rtXdot->Integrator1_CSTATE_d = tunning_nominal_B.Sum1; + _rtXdot->Integrator1_CSTATE_d = tuning_nominal_B.Sum1; /* Derivatives for Integrator: '/Integrator' */ - _rtXdot->Integrator_CSTATE = tunning_nominal_B.Sum4; + _rtXdot->Integrator_CSTATE = tuning_nominal_B.Sum4; /* Derivatives for Integrator: '/Integrator1' */ - _rtXdot->Integrator1_CSTATE_j = tunning_nominal_B.Sum3_n; + _rtXdot->Integrator1_CSTATE_j = tuning_nominal_B.Sum3_n; } /* Model initialize function */ -void tunning_nominalModelClass::initialize() +void tuning_nominalModelClass::initialize() { /* Registration code */ /* initialize real-time model */ - (void) memset((void *)(&tunning_nominal_M), 0, - sizeof(RT_MODEL_tunning_nominal_T)); + (void) memset((void *)(&tuning_nominal_M), 0, + sizeof(RT_MODEL_tuning_nominal_T)); { /* Setup solver object */ - rtsiSetSimTimeStepPtr(&(&tunning_nominal_M)->solverInfo, - &(&tunning_nominal_M)->Timing.simTimeStep); - rtsiSetTPtr(&(&tunning_nominal_M)->solverInfo, &rtmGetTPtr - ((&tunning_nominal_M))); - rtsiSetStepSizePtr(&(&tunning_nominal_M)->solverInfo, &(&tunning_nominal_M - )->Timing.stepSize0); - rtsiSetdXPtr(&(&tunning_nominal_M)->solverInfo, &(&tunning_nominal_M) - ->derivs); - rtsiSetContStatesPtr(&(&tunning_nominal_M)->solverInfo, (real_T **) - &(&tunning_nominal_M)->contStates); - rtsiSetNumContStatesPtr(&(&tunning_nominal_M)->solverInfo, - &(&tunning_nominal_M)->Sizes.numContStates); - rtsiSetNumPeriodicContStatesPtr(&(&tunning_nominal_M)->solverInfo, - &(&tunning_nominal_M)->Sizes.numPeriodicContStates); - rtsiSetPeriodicContStateIndicesPtr(&(&tunning_nominal_M)->solverInfo, - &(&tunning_nominal_M)->periodicContStateIndices); - rtsiSetPeriodicContStateRangesPtr(&(&tunning_nominal_M)->solverInfo, - &(&tunning_nominal_M)->periodicContStateRanges); - rtsiSetErrorStatusPtr(&(&tunning_nominal_M)->solverInfo, (&rtmGetErrorStatus - ((&tunning_nominal_M)))); - rtsiSetRTModelPtr(&(&tunning_nominal_M)->solverInfo, (&tunning_nominal_M)); + rtsiSetSimTimeStepPtr(&(&tuning_nominal_M)->solverInfo, &(&tuning_nominal_M + )->Timing.simTimeStep); + rtsiSetTPtr(&(&tuning_nominal_M)->solverInfo, &rtmGetTPtr((&tuning_nominal_M))); + rtsiSetStepSizePtr(&(&tuning_nominal_M)->solverInfo, &(&tuning_nominal_M) + ->Timing.stepSize0); + rtsiSetdXPtr(&(&tuning_nominal_M)->solverInfo, &(&tuning_nominal_M)->derivs); + rtsiSetContStatesPtr(&(&tuning_nominal_M)->solverInfo, (real_T **) + &(&tuning_nominal_M)->contStates); + rtsiSetNumContStatesPtr(&(&tuning_nominal_M)->solverInfo, + &(&tuning_nominal_M)->Sizes.numContStates); + rtsiSetNumPeriodicContStatesPtr(&(&tuning_nominal_M)->solverInfo, + &(&tuning_nominal_M)->Sizes.numPeriodicContStates); + rtsiSetPeriodicContStateIndicesPtr(&(&tuning_nominal_M)->solverInfo, + &(&tuning_nominal_M)->periodicContStateIndices); + rtsiSetPeriodicContStateRangesPtr(&(&tuning_nominal_M)->solverInfo, + &(&tuning_nominal_M)->periodicContStateRanges); + rtsiSetErrorStatusPtr(&(&tuning_nominal_M)->solverInfo, (&rtmGetErrorStatus + ((&tuning_nominal_M)))); + rtsiSetRTModelPtr(&(&tuning_nominal_M)->solverInfo, (&tuning_nominal_M)); } - rtsiSetSimTimeStep(&(&tunning_nominal_M)->solverInfo, MAJOR_TIME_STEP); - (&tunning_nominal_M)->intgData.y = (&tunning_nominal_M)->odeY; - (&tunning_nominal_M)->intgData.f[0] = (&tunning_nominal_M)->odeF[0]; - (&tunning_nominal_M)->intgData.f[1] = (&tunning_nominal_M)->odeF[1]; - (&tunning_nominal_M)->intgData.f[2] = (&tunning_nominal_M)->odeF[2]; - (&tunning_nominal_M)->intgData.f[3] = (&tunning_nominal_M)->odeF[3]; - getRTM()->contStates = ((X_tunning_nominal_T *) &tunning_nominal_X); - rtsiSetSolverData(&(&tunning_nominal_M)->solverInfo, (void *) - &(&tunning_nominal_M)->intgData); - rtsiSetSolverName(&(&tunning_nominal_M)->solverInfo,"ode4"); - rtmSetTPtr(getRTM(), &(&tunning_nominal_M)->Timing.tArray[0]); - (&tunning_nominal_M)->Timing.stepSize0 = 0.001; + rtsiSetSimTimeStep(&(&tuning_nominal_M)->solverInfo, MAJOR_TIME_STEP); + (&tuning_nominal_M)->intgData.y = (&tuning_nominal_M)->odeY; + (&tuning_nominal_M)->intgData.f[0] = (&tuning_nominal_M)->odeF[0]; + (&tuning_nominal_M)->intgData.f[1] = (&tuning_nominal_M)->odeF[1]; + (&tuning_nominal_M)->intgData.f[2] = (&tuning_nominal_M)->odeF[2]; + (&tuning_nominal_M)->intgData.f[3] = (&tuning_nominal_M)->odeF[3]; + getRTM()->contStates = ((X_tuning_nominal_T *) &tuning_nominal_X); + rtsiSetSolverData(&(&tuning_nominal_M)->solverInfo, (void *) + &(&tuning_nominal_M)->intgData); + rtsiSetSolverName(&(&tuning_nominal_M)->solverInfo,"ode4"); + rtmSetTPtr(getRTM(), &(&tuning_nominal_M)->Timing.tArray[0]); + (&tuning_nominal_M)->Timing.stepSize0 = 0.001; /* block I/O */ - (void) memset(((void *) &tunning_nominal_B), 0, - sizeof(B_tunning_nominal_T)); + (void) memset(((void *) &tuning_nominal_B), 0, + sizeof(B_tuning_nominal_T)); /* states (continuous) */ { - (void) memset((void *)&tunning_nominal_X, 0, - sizeof(X_tunning_nominal_T)); + (void) memset((void *)&tuning_nominal_X, 0, + sizeof(X_tuning_nominal_T)); } /* states (dwork) */ - (void) memset((void *)&tunning_nominal_DW, 0, - sizeof(DW_tunning_nominal_T)); + (void) memset((void *)&tuning_nominal_DW, 0, + sizeof(DW_tuning_nominal_T)); /* external inputs */ - (void)memset((void *)&tunning_nominal_U, 0, sizeof(ExtU_tunning_nominal_T)); + (void)memset((void *)&tuning_nominal_U, 0, sizeof(ExtU_tuning_nominal_T)); /* external outputs */ - (void) memset((void *)&tunning_nominal_Y, 0, - sizeof(ExtY_tunning_nominal_T)); + (void) memset((void *)&tuning_nominal_Y, 0, + sizeof(ExtY_tuning_nominal_T)); { int32_T i; /* InitializeConditions for Integrator: '/Integrator1' */ - tunning_nominal_X.Integrator1_CSTATE = 0.0; + tuning_nominal_X.Integrator1_CSTATE = 0.0; /* InitializeConditions for Integrator: '/Integrator1' */ - tunning_nominal_X.Integrator1_CSTATE_d = 0.0; + tuning_nominal_X.Integrator1_CSTATE_d = 0.0; /* InitializeConditions for Integrator: '/Integrator' */ - tunning_nominal_X.Integrator_CSTATE = 0.0; + tuning_nominal_X.Integrator_CSTATE = 0.0; /* InitializeConditions for Integrator: '/Integrator1' */ - tunning_nominal_X.Integrator1_CSTATE_j = 0.0; + tuning_nominal_X.Integrator1_CSTATE_j = 0.0; /* InitializeConditions for Delay: '/MemoryX' */ - tunning_nominal_DW.icLoad = 1U; + tuning_nominal_DW.icLoad = 1U; /* SystemInitialize for Enabled SubSystem: '/MeasurementUpdate' */ /* SystemInitialize for Atomic SubSystem: '/UseCurrentEstimator' */ /* SystemInitialize for Enabled SubSystem: '/Enabled Subsystem' */ for (i = 0; i < 6; i++) { /* InitializeConditions for Memory: '/Memory' */ - tunning_nominal_DW.Memory_PreviousInput[i] = 2.5179000000000005; + tuning_nominal_DW.Memory_PreviousInput[i] = 2.5179000000000005; /* SystemInitialize for Outport: '/deltax' */ - tunning_nominal_B.Product2[i] = 0.0; + tuning_nominal_B.Product2[i] = 0.0; /* SystemInitialize for Outport: '/L*(y[k]-yhat[k|k-1])' */ - tunning_nominal_B.Product3[i] = 0.0; + tuning_nominal_B.Product3[i] = 0.0; } /* End of SystemInitialize for SubSystem: '/Enabled Subsystem' */ @@ -1086,24 +1080,24 @@ void tunning_nominalModelClass::initialize() } /* Model terminate function */ -void tunning_nominalModelClass::terminate() +void tuning_nominalModelClass::terminate() { /* (no terminate code required) */ } /* Constructor */ -tunning_nominalModelClass::tunning_nominalModelClass() +tuning_nominalModelClass::tuning_nominalModelClass() { } /* Destructor */ -tunning_nominalModelClass::~tunning_nominalModelClass() +tuning_nominalModelClass::~tuning_nominalModelClass() { /* Currently there is no destructor body generated.*/ } /* Real-Time Model get method */ -RT_MODEL_tunning_nominal_T * tunning_nominalModelClass::getRTM() +RT_MODEL_tuning_nominal_T * tuning_nominalModelClass::getRTM() { - return (&tunning_nominal_M); + return (&tuning_nominal_M); } diff --git a/src/tunning_nominal_grt_rtw/tunning_nominal.h b/src/tuning_nominal_grt_rtw/tuning_nominal.h similarity index 74% rename from src/tunning_nominal_grt_rtw/tunning_nominal.h rename to src/tuning_nominal_grt_rtw/tuning_nominal.h index 0fcc771..8a446e8 100644 --- a/src/tunning_nominal_grt_rtw/tunning_nominal.h +++ b/src/tuning_nominal_grt_rtw/tuning_nominal.h @@ -1,15 +1,15 @@ /* - * tunning_nominal.h + * tuning_nominal.h * * Student License - for use by students to meet course requirements and * perform academic research at degree granting institutions only. Not * for government, commercial, or other organizational use. * - * Code generation for model "tunning_nominal". + * Code generation for model "tuning_nominal". * * Model version : 1.1498 * Simulink Coder version : 8.12 (R2017a) 16-Feb-2017 - * C++ source code generated on : Fri Aug 31 14:28:54 2018 + * C++ source code generated on : Tue Sep 18 10:50:51 2018 * * Target selection: grt.tlc * Note: GRT includes extra infrastructure and instrumentation for prototyping @@ -18,18 +18,18 @@ * Validation result: Passed (1), Warnings (3), Error (0) */ -#ifndef RTW_HEADER_tunning_nominal_h_ -#define RTW_HEADER_tunning_nominal_h_ +#ifndef RTW_HEADER_tuning_nominal_h_ +#define RTW_HEADER_tuning_nominal_h_ #include #include -#ifndef tunning_nominal_COMMON_INCLUDES_ -# define tunning_nominal_COMMON_INCLUDES_ +#ifndef tuning_nominal_COMMON_INCLUDES_ +# define tuning_nominal_COMMON_INCLUDES_ #include "rtwtypes.h" #include "rtw_continuous.h" #include "rtw_solver.h" -#endif /* tunning_nominal_COMMON_INCLUDES_ */ +#endif /* tuning_nominal_COMMON_INCLUDES_ */ -#include "tunning_nominal_types.h" +#include "tuning_nominal_types.h" /* Shared type includes */ #include "multiword_types.h" @@ -161,7 +161,7 @@ typedef struct { real_T Sum3_h; /* '/Sum3' */ real_T Product2[6]; /* '/Product2' */ real_T Product3[6]; /* '/Product3' */ -} B_tunning_nominal_T; +} B_tuning_nominal_T; /* Block states (auto storage) for system '' */ typedef struct { @@ -170,7 +170,7 @@ typedef struct { uint8_T icLoad; /* '/MemoryX' */ boolean_T MeasurementUpdate_MODE; /* '/MeasurementUpdate' */ boolean_T EnabledSubsystem_MODE; /* '/Enabled Subsystem' */ -} DW_tunning_nominal_T; +} DW_tuning_nominal_T; /* Continuous states (auto storage) */ typedef struct { @@ -178,7 +178,7 @@ typedef struct { real_T Integrator1_CSTATE_d; /* '/Integrator1' */ real_T Integrator_CSTATE; /* '/Integrator' */ real_T Integrator1_CSTATE_j; /* '/Integrator1' */ -} X_tunning_nominal_T; +} X_tuning_nominal_T; /* State derivatives (auto storage) */ typedef struct { @@ -186,7 +186,7 @@ typedef struct { real_T Integrator1_CSTATE_d; /* '/Integrator1' */ real_T Integrator_CSTATE; /* '/Integrator' */ real_T Integrator1_CSTATE_j; /* '/Integrator1' */ -} XDot_tunning_nominal_T; +} XDot_tuning_nominal_T; /* State disabled */ typedef struct { @@ -194,7 +194,7 @@ typedef struct { boolean_T Integrator1_CSTATE_d; /* '/Integrator1' */ boolean_T Integrator_CSTATE; /* '/Integrator' */ boolean_T Integrator1_CSTATE_j; /* '/Integrator1' */ -} XDis_tunning_nominal_T; +} XDis_tuning_nominal_T; #ifndef ODE4_INTG #define ODE4_INTG @@ -243,7 +243,7 @@ typedef struct { * Referenced by: '/KalmanGainL' */ real_T KalmanGainL_Value[18]; -} ConstP_tunning_nominal_T; +} ConstP_tuning_nominal_T; /* External inputs (root inport signals with auto storage) */ typedef struct { @@ -254,7 +254,7 @@ typedef struct { real_T LOE_t[6]; /* '/LOE_t' */ real_T LOE_a[6]; /* '/LOE_a' */ real_T gain[18]; /* '/gain' */ -} ExtU_tunning_nominal_T; +} ExtU_tuning_nominal_T; /* External outputs (root outports fed by signals with auto storage) */ typedef struct { @@ -268,13 +268,13 @@ typedef struct { real_T acc_Kalman[3]; /* '/acc_Kalman' */ real_T M_Kalman[3]; /* '/M_Kalman' */ real_T vel_Kalman[3]; /* '/vel_Kalman' */ -} ExtY_tunning_nominal_T; +} ExtY_tuning_nominal_T; /* Real-time Model Data Structure */ -struct tag_RTM_tunning_nominal_T { +struct tag_RTM_tuning_nominal_T { const char_T *errorStatus; RTWSolverInfo solverInfo; - X_tunning_nominal_T *contStates; + X_tuning_nominal_T *contStates; int_T *periodicContStateIndices; real_T *periodicContStateRanges; real_T *derivs; @@ -332,17 +332,17 @@ extern "C" { #endif /* Constant parameters (auto storage) */ -extern const ConstP_tunning_nominal_T tunning_nominal_ConstP; +extern const ConstP_tuning_nominal_T tuning_nominal_ConstP; -/* Class declaration for model tunning_nominal */ -class tunning_nominalModelClass { +/* Class declaration for model tuning_nominal */ +class tuning_nominalModelClass { /* public data and function members */ public: /* External inputs */ - ExtU_tunning_nominal_T tunning_nominal_U; + ExtU_tuning_nominal_T tuning_nominal_U; /* External outputs */ - ExtY_tunning_nominal_T tunning_nominal_Y; + ExtY_tuning_nominal_T tuning_nominal_Y; /* model initialize function */ void initialize(); @@ -354,31 +354,31 @@ class tunning_nominalModelClass { void terminate(); /* Constructor */ - tunning_nominalModelClass(); + tuning_nominalModelClass(); /* Destructor */ - ~tunning_nominalModelClass(); + ~tuning_nominalModelClass(); /* Real-Time Model get method */ - RT_MODEL_tunning_nominal_T * getRTM(); + RT_MODEL_tuning_nominal_T * getRTM(); /* private data and function members */ private: /* Block signals */ - B_tunning_nominal_T tunning_nominal_B; + B_tuning_nominal_T tuning_nominal_B; /* Block states */ - DW_tunning_nominal_T tunning_nominal_DW; - X_tunning_nominal_T tunning_nominal_X;/* Block continuous states */ + DW_tuning_nominal_T tuning_nominal_DW; + X_tuning_nominal_T tuning_nominal_X; /* Block continuous states */ /* Real-Time Model */ - RT_MODEL_tunning_nominal_T tunning_nominal_M; + RT_MODEL_tuning_nominal_T tuning_nominal_M; /* Continuous states update member function*/ void rt_ertODEUpdateContinuousStates(RTWSolverInfo *si ); /* Derivatives member function */ - void tunning_nominal_derivatives(); + void tuning_nominal_derivatives(); }; /*- @@ -470,68 +470,68 @@ class tunning_nominalModelClass { * * Here is the system hierarchy for this model * - * '' : 'tunning_nominal' - * '' : 'tunning_nominal/Actuator_Fault' - * '' : 'tunning_nominal/FDD_Kalman' - * '' : 'tunning_nominal/Kalman Filter' - * '' : 'tunning_nominal/Test_config_and_data' - * '' : 'tunning_nominal/Thrust2command' - * '' : 'tunning_nominal/XY State Feedback' - * '' : 'tunning_nominal/pitch_controller_SF' - * '' : 'tunning_nominal/roll_controller_SF' - * '' : 'tunning_nominal/yaw_controller' - * '' : 'tunning_nominal/z_controller' - * '' : 'tunning_nominal/FDD_Kalman/Cross Product' - * '' : 'tunning_nominal/FDD_Kalman/MATLAB Function1' - * '' : 'tunning_nominal/FDD_Kalman/Cross Product/Subsystem' - * '' : 'tunning_nominal/FDD_Kalman/Cross Product/Subsystem2' - * '' : 'tunning_nominal/Kalman Filter/CalculatePL' - * '' : 'tunning_nominal/Kalman Filter/CalculateYhat' - * '' : 'tunning_nominal/Kalman Filter/DataTypeConversionA' - * '' : 'tunning_nominal/Kalman Filter/DataTypeConversionB' - * '' : 'tunning_nominal/Kalman Filter/DataTypeConversionC' - * '' : 'tunning_nominal/Kalman Filter/DataTypeConversionD' - * '' : 'tunning_nominal/Kalman Filter/DataTypeConversionG' - * '' : 'tunning_nominal/Kalman Filter/DataTypeConversionH' - * '' : 'tunning_nominal/Kalman Filter/DataTypeConversionN' - * '' : 'tunning_nominal/Kalman Filter/DataTypeConversionP' - * '' : 'tunning_nominal/Kalman Filter/DataTypeConversionP0' - * '' : 'tunning_nominal/Kalman Filter/DataTypeConversionQ' - * '' : 'tunning_nominal/Kalman Filter/DataTypeConversionR' - * '' : 'tunning_nominal/Kalman Filter/DataTypeConversionReset' - * '' : 'tunning_nominal/Kalman Filter/DataTypeConversionX' - * '' : 'tunning_nominal/Kalman Filter/DataTypeConversionX0' - * '' : 'tunning_nominal/Kalman Filter/DataTypeConversionu' - * '' : 'tunning_nominal/Kalman Filter/MemoryP' - * '' : 'tunning_nominal/Kalman Filter/Observer' - * '' : 'tunning_nominal/Kalman Filter/ReducedQRN' - * '' : 'tunning_nominal/Kalman Filter/ScalarExpansionP0' - * '' : 'tunning_nominal/Kalman Filter/ScalarExpansionQ' - * '' : 'tunning_nominal/Kalman Filter/ScalarExpansionR' - * '' : 'tunning_nominal/Kalman Filter/UseCurrentEstimator' - * '' : 'tunning_nominal/Kalman Filter/checkA' - * '' : 'tunning_nominal/Kalman Filter/checkB' - * '' : 'tunning_nominal/Kalman Filter/checkC' - * '' : 'tunning_nominal/Kalman Filter/checkD' - * '' : 'tunning_nominal/Kalman Filter/checkEnable' - * '' : 'tunning_nominal/Kalman Filter/checkG' - * '' : 'tunning_nominal/Kalman Filter/checkH' - * '' : 'tunning_nominal/Kalman Filter/checkN' - * '' : 'tunning_nominal/Kalman Filter/checkP0' - * '' : 'tunning_nominal/Kalman Filter/checkQ' - * '' : 'tunning_nominal/Kalman Filter/checkR' - * '' : 'tunning_nominal/Kalman Filter/checkReset' - * '' : 'tunning_nominal/Kalman Filter/checkX0' - * '' : 'tunning_nominal/Kalman Filter/checku' - * '' : 'tunning_nominal/Kalman Filter/checky' - * '' : 'tunning_nominal/Kalman Filter/CalculatePL/DataTypeConversionL' - * '' : 'tunning_nominal/Kalman Filter/CalculatePL/DataTypeConversionM' - * '' : 'tunning_nominal/Kalman Filter/CalculatePL/DataTypeConversionP' - * '' : 'tunning_nominal/Kalman Filter/CalculatePL/DataTypeConversionZ' - * '' : 'tunning_nominal/Kalman Filter/Observer/MeasurementUpdate' - * '' : 'tunning_nominal/Kalman Filter/UseCurrentEstimator/Enabled Subsystem' - * '' : 'tunning_nominal/Test_config_and_data/FFW' - * '' : 'tunning_nominal/Test_config_and_data/LOE_' - * '' : 'tunning_nominal/Test_config_and_data/MATLAB Function' + * '' : 'tuning_nominal' + * '' : 'tuning_nominal/Actuator_Fault' + * '' : 'tuning_nominal/FDD_Kalman' + * '' : 'tuning_nominal/Kalman Filter' + * '' : 'tuning_nominal/Test_config_and_data' + * '' : 'tuning_nominal/Thrust2command' + * '' : 'tuning_nominal/XY State Feedback' + * '' : 'tuning_nominal/pitch_controller_SF' + * '' : 'tuning_nominal/roll_controller_SF' + * '' : 'tuning_nominal/yaw_controller' + * '' : 'tuning_nominal/z_controller' + * '' : 'tuning_nominal/FDD_Kalman/Cross Product' + * '' : 'tuning_nominal/FDD_Kalman/MATLAB Function1' + * '' : 'tuning_nominal/FDD_Kalman/Cross Product/Subsystem' + * '' : 'tuning_nominal/FDD_Kalman/Cross Product/Subsystem2' + * '' : 'tuning_nominal/Kalman Filter/CalculatePL' + * '' : 'tuning_nominal/Kalman Filter/CalculateYhat' + * '' : 'tuning_nominal/Kalman Filter/DataTypeConversionA' + * '' : 'tuning_nominal/Kalman Filter/DataTypeConversionB' + * '' : 'tuning_nominal/Kalman Filter/DataTypeConversionC' + * '' : 'tuning_nominal/Kalman Filter/DataTypeConversionD' + * '' : 'tuning_nominal/Kalman Filter/DataTypeConversionG' + * '' : 'tuning_nominal/Kalman Filter/DataTypeConversionH' + * '' : 'tuning_nominal/Kalman Filter/DataTypeConversionN' + * '' : 'tuning_nominal/Kalman Filter/DataTypeConversionP' + * '' : 'tuning_nominal/Kalman Filter/DataTypeConversionP0' + * '' : 'tuning_nominal/Kalman Filter/DataTypeConversionQ' + * '' : 'tuning_nominal/Kalman Filter/DataTypeConversionR' + * '' : 'tuning_nominal/Kalman Filter/DataTypeConversionReset' + * '' : 'tuning_nominal/Kalman Filter/DataTypeConversionX' + * '' : 'tuning_nominal/Kalman Filter/DataTypeConversionX0' + * '' : 'tuning_nominal/Kalman Filter/DataTypeConversionu' + * '' : 'tuning_nominal/Kalman Filter/MemoryP' + * '' : 'tuning_nominal/Kalman Filter/Observer' + * '' : 'tuning_nominal/Kalman Filter/ReducedQRN' + * '' : 'tuning_nominal/Kalman Filter/ScalarExpansionP0' + * '' : 'tuning_nominal/Kalman Filter/ScalarExpansionQ' + * '' : 'tuning_nominal/Kalman Filter/ScalarExpansionR' + * '' : 'tuning_nominal/Kalman Filter/UseCurrentEstimator' + * '' : 'tuning_nominal/Kalman Filter/checkA' + * '' : 'tuning_nominal/Kalman Filter/checkB' + * '' : 'tuning_nominal/Kalman Filter/checkC' + * '' : 'tuning_nominal/Kalman Filter/checkD' + * '' : 'tuning_nominal/Kalman Filter/checkEnable' + * '' : 'tuning_nominal/Kalman Filter/checkG' + * '' : 'tuning_nominal/Kalman Filter/checkH' + * '' : 'tuning_nominal/Kalman Filter/checkN' + * '' : 'tuning_nominal/Kalman Filter/checkP0' + * '' : 'tuning_nominal/Kalman Filter/checkQ' + * '' : 'tuning_nominal/Kalman Filter/checkR' + * '' : 'tuning_nominal/Kalman Filter/checkReset' + * '' : 'tuning_nominal/Kalman Filter/checkX0' + * '' : 'tuning_nominal/Kalman Filter/checku' + * '' : 'tuning_nominal/Kalman Filter/checky' + * '' : 'tuning_nominal/Kalman Filter/CalculatePL/DataTypeConversionL' + * '' : 'tuning_nominal/Kalman Filter/CalculatePL/DataTypeConversionM' + * '' : 'tuning_nominal/Kalman Filter/CalculatePL/DataTypeConversionP' + * '' : 'tuning_nominal/Kalman Filter/CalculatePL/DataTypeConversionZ' + * '' : 'tuning_nominal/Kalman Filter/Observer/MeasurementUpdate' + * '' : 'tuning_nominal/Kalman Filter/UseCurrentEstimator/Enabled Subsystem' + * '' : 'tuning_nominal/Test_config_and_data/FFW' + * '' : 'tuning_nominal/Test_config_and_data/LOE_' + * '' : 'tuning_nominal/Test_config_and_data/MATLAB Function' */ -#endif /* RTW_HEADER_tunning_nominal_h_ */ +#endif /* RTW_HEADER_tuning_nominal_h_ */ diff --git a/src/tuning_nominal_grt_rtw/tuning_nominal_data.cpp b/src/tuning_nominal_grt_rtw/tuning_nominal_data.cpp new file mode 100644 index 0000000..c844780 --- /dev/null +++ b/src/tuning_nominal_grt_rtw/tuning_nominal_data.cpp @@ -0,0 +1,74 @@ +/* + * tuning_nominal_data.cpp + * + * Student License - for use by students to meet course requirements and + * perform academic research at degree granting institutions only. Not + * for government, commercial, or other organizational use. + * + * Code generation for model "tuning_nominal". + * + * Model version : 1.1498 + * Simulink Coder version : 8.12 (R2017a) 16-Feb-2017 + * C++ source code generated on : Tue Sep 18 10:50:51 2018 + * + * Target selection: grt.tlc + * Note: GRT includes extra infrastructure and instrumentation for prototyping + * Embedded hardware selection: 32-bit Generic + * Code generation objective: Execution efficiency + * Validation result: Passed (1), Warnings (3), Error (0) + */ + +#include "tuning_nominal.h" +#include "tuning_nominal_private.h" + +/* Constant parameters (auto storage) */ +const ConstP_tuning_nominal_T tuning_nominal_ConstP = { + /* Expression: B_ENU_inv + * Referenced by: '/Control Allocation' + */ + { 0.16666666666666674, 0.16666666666666669, 0.16666666666666671, + 0.16666666666666671, 0.16666666666666674, 0.16666666666666671, + 0.77519379844962388, 1.5503875968992147, 0.77519379844962344, + -0.77519379844962344, -1.5503875968992149, -0.77519379844962344, + -1.3426750446270113, -2.5529533974250051E-14, 1.3426750446270619, + 1.3426750446270113, 2.5418511671787539E-14, -1.3426750446270626, + -4.5662100456621, 4.5662100456621, -4.5662100456620989, 4.5662100456620971, + -4.5662100456621006, 4.5662100456620989 }, + + /* Expression: M_bias' + * Referenced by: '/Constant' + */ + { -0.005, -0.04, 0.0 }, + + /* Expression: Ib + * Referenced by: '/Constant1' + */ + { 0.0347563, 0.0, 0.0, 0.0, 0.0458929, 0.0, 0.0, 0.0, 0.0977 }, + + /* Expression: pInitialization.M + * 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, + 0.00055009940149860072, 0.0, 0.0, 0.00015134630638216127 }, + + /* Expression: pInitialization.C + * Referenced by: '/C' + */ + { 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: pInitialization.A + * Referenced by: '/A' + */ + { 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, + 0.0, 0.0, 0.0, 0.001, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.001, 0.0, 0.0, 1.0, + 0.0, 0.0, 0.0, 0.001, 0.0, 0.0, 1.0 }, + + /* Expression: pInitialization.L + * 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, + 0.00055025074780498283, 0.0, 0.0, 0.00015134630638216127 } +}; diff --git a/src/tuning_nominal_grt_rtw/tuning_nominal_private.h b/src/tuning_nominal_grt_rtw/tuning_nominal_private.h new file mode 100644 index 0000000..450a7de --- /dev/null +++ b/src/tuning_nominal_grt_rtw/tuning_nominal_private.h @@ -0,0 +1,46 @@ +/* + * tuning_nominal_private.h + * + * Student License - for use by students to meet course requirements and + * perform academic research at degree granting institutions only. Not + * for government, commercial, or other organizational use. + * + * Code generation for model "tuning_nominal". + * + * Model version : 1.1498 + * Simulink Coder version : 8.12 (R2017a) 16-Feb-2017 + * C++ source code generated on : Tue Sep 18 10:50:51 2018 + * + * Target selection: grt.tlc + * Note: GRT includes extra infrastructure and instrumentation for prototyping + * Embedded hardware selection: 32-bit Generic + * Code generation objective: Execution efficiency + * Validation result: Passed (1), Warnings (3), Error (0) + */ + +#ifndef RTW_HEADER_tuning_nominal_private_h_ +#define RTW_HEADER_tuning_nominal_private_h_ +#include "rtwtypes.h" +#include "multiword_types.h" + +/* Private macros used by the generated code to access rtModel */ +#ifndef rtmIsMajorTimeStep +# define rtmIsMajorTimeStep(rtm) (((rtm)->Timing.simTimeStep) == MAJOR_TIME_STEP) +#endif + +#ifndef rtmIsMinorTimeStep +# define rtmIsMinorTimeStep(rtm) (((rtm)->Timing.simTimeStep) == MINOR_TIME_STEP) +#endif + +#ifndef rtmGetTPtr +# define rtmGetTPtr(rtm) ((rtm)->Timing.t) +#endif + +#ifndef rtmSetTPtr +# define rtmSetTPtr(rtm, val) ((rtm)->Timing.t = (val)) +#endif + +/* private model entry point functions */ +extern void tuning_nominal_derivatives(); + +#endif /* RTW_HEADER_tuning_nominal_private_h_ */ diff --git a/src/tuning_nominal_grt_rtw/tuning_nominal_types.h b/src/tuning_nominal_grt_rtw/tuning_nominal_types.h new file mode 100644 index 0000000..50dc7e3 --- /dev/null +++ b/src/tuning_nominal_grt_rtw/tuning_nominal_types.h @@ -0,0 +1,27 @@ +/* + * tuning_nominal_types.h + * + * Student License - for use by students to meet course requirements and + * perform academic research at degree granting institutions only. Not + * for government, commercial, or other organizational use. + * + * Code generation for model "tuning_nominal". + * + * Model version : 1.1498 + * Simulink Coder version : 8.12 (R2017a) 16-Feb-2017 + * C++ source code generated on : Tue Sep 18 10:50:51 2018 + * + * Target selection: grt.tlc + * Note: GRT includes extra infrastructure and instrumentation for prototyping + * Embedded hardware selection: 32-bit Generic + * Code generation objective: Execution efficiency + * Validation result: Passed (1), Warnings (3), Error (0) + */ + +#ifndef RTW_HEADER_tuning_nominal_types_h_ +#define RTW_HEADER_tuning_nominal_types_h_ + +/* Forward declaration for rtModel */ +typedef struct tag_RTM_tuning_nominal_T RT_MODEL_tuning_nominal_T; + +#endif /* RTW_HEADER_tuning_nominal_types_h_ */ diff --git a/src/tunning_nominal_node.cpp b/src/tuning_nominal_node.cpp similarity index 75% rename from src/tunning_nominal_node.cpp rename to src/tuning_nominal_node.cpp index 5d62830..9b551f6 100644 --- a/src/tunning_nominal_node.cpp +++ b/src/tuning_nominal_node.cpp @@ -15,12 +15,12 @@ #include #include -#include +#include #include #include -tunning_nominalModelClass gController; +tuning_nominalModelClass gController; bool gPublish; bool gInit_flag; @@ -202,10 +202,10 @@ void timmerCallback(const ros::TimerEvent&) } int main(int argc, char** argv) { - ros::init(argc, argv, "tunning_nominal_node"); + ros::init(argc, argv, "tuning_nominal_node"); ros::NodeHandle nh; ros::NodeHandle pnh("~"); - ROS_INFO("tunning_nominal_node main started"); + ROS_INFO("tuning_nominal_node main started"); ros::Subscriber odometry_sub_; odometry_sub_ = nh.subscribe(mav_msgs::default_topics::ODOMETRY, 1, OdometryCallback); @@ -275,10 +275,10 @@ int main(int argc, char** argv) { control_actived = true; for (unsigned int i=0; i< 6; i++) { - gController.tunning_nominal_U.LOE_a[i] = gLOE[i]; // fault amplitude + gController.tuning_nominal_U.LOE_a[i] = gLOE[i]; // fault amplitude } for (unsigned int i=0; i< 6; i++) { - gController.tunning_nominal_U.LOE_t[i] = gLOE_t[i]; // fault time + gController.tuning_nominal_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]); @@ -287,32 +287,32 @@ int main(int argc, char** argv) { if (control_actived) { // controller active after take-off request // Initialization before Step - gController.tunning_nominal_U.mode = gTest_mode; + gController.tuning_nominal_U.mode = gTest_mode; for (unsigned int i=0; i< 4; i++) { - gController.tunning_nominal_U.ref[i] = gRef[i]; + gController.tuning_nominal_U.ref[i] = gRef[i]; } for (unsigned int i=0; i< 4; i++) { - gController.tunning_nominal_U.Y0[i] = gY0[i]; + gController.tuning_nominal_U.Y0[i] = gY0[i]; } for (unsigned int i=0; i< 19; i++) { - gController.tunning_nominal_U.gain[i] = gGain[i]; + gController.tuning_nominal_U.gain[i] = gGain[i]; } - gController.tunning_nominal_U.X[0] = gOdometry.position_W.x(); - gController.tunning_nominal_U.X[1] = gOdometry.position_W.y(); - gController.tunning_nominal_U.X[2] = gOdometry.position_W.z(); - gController.tunning_nominal_U.X[3 ] = velocity_W.x(); - gController.tunning_nominal_U.X[4 ] = velocity_W.y(); - gController.tunning_nominal_U.X[5 ] = velocity_W.z(); - gController.tunning_nominal_U.X[6 ] = phi; - gController.tunning_nominal_U.X[7 ] = theta; - gController.tunning_nominal_U.X[8 ] = gPsi; - gController.tunning_nominal_U.X[9 ] = gOdometry.angular_velocity_B.x(); - gController.tunning_nominal_U.X[10] = gOdometry.angular_velocity_B.y(); - gController.tunning_nominal_U.X[11] = gOdometry.angular_velocity_B.z(); - /*gController.tunning_nominal_U.X[12] = gAng_acc_calcul[0]; - gController.tunning_nominal_U.X[13] = gAng_acc_calcul[1]; - gController.tunning_nominal_U.X[14] = gAng_acc_calcul[2]; */ + gController.tuning_nominal_U.X[0] = gOdometry.position_W.x(); + gController.tuning_nominal_U.X[1] = gOdometry.position_W.y(); + gController.tuning_nominal_U.X[2] = gOdometry.position_W.z(); + gController.tuning_nominal_U.X[3 ] = velocity_W.x(); + gController.tuning_nominal_U.X[4 ] = velocity_W.y(); + gController.tuning_nominal_U.X[5 ] = velocity_W.z(); + gController.tuning_nominal_U.X[6 ] = phi; + gController.tuning_nominal_U.X[7 ] = theta; + gController.tuning_nominal_U.X[8 ] = gPsi; + gController.tuning_nominal_U.X[9 ] = gOdometry.angular_velocity_B.x(); + gController.tuning_nominal_U.X[10] = gOdometry.angular_velocity_B.y(); + gController.tuning_nominal_U.X[11] = gOdometry.angular_velocity_B.z(); + /*gController.tuning_nominal_U.X[12] = gAng_acc_calcul[0]; + gController.tuning_nominal_U.X[13] = gAng_acc_calcul[1]; + gController.tuning_nominal_U.X[14] = gAng_acc_calcul[2]; */ // Run Matlab controller gController.step(); @@ -327,7 +327,7 @@ int main(int argc, char** argv) { } else { - motor_command[i] = gController.tunning_nominal_Y.motor_command[i]; // normalized [1 .. 200] => Asctec Firefly + motor_command[i] = gController.tuning_nominal_Y.motor_command[i]; // normalized [1 .. 200] => Asctec Firefly motor_RPM[i] = 1250.0 + motor_command[i]*43.75; // real RPM motor_speed[i] = motor_RPM[i]/9.5493; // rad/s => Gazebo } @@ -354,7 +354,7 @@ int main(int argc, char** argv) { actuator_msg->header.stamp = ros::Time::now(); motor_velocity_reference_pub_.publish(actuator_msg); - if (gController.tunning_nominal_Y.ref_out[2] <= 0.25 && gTest_mode > 0){ + if (gController.tuning_nominal_Y.ref_out[2] <= 0.25 && gTest_mode > 0){ gLanding_flag = true; } @@ -373,7 +373,7 @@ int main(int argc, char** argv) { if (gEmergency_status) { - ROS_ERROR("tunning_nominal_node emergency status"); + ROS_ERROR("tuning_nominal_node emergency status"); ROS_INFO("x = %f, y = %f, z = %f",gOdometry.position_W.x(),gOdometry.position_W.y(),gOdometry.position_W.z()); ros::Duration(0.5).sleep(); gController.terminate(); @@ -392,10 +392,10 @@ int main(int argc, char** argv) { // Publish data: UAV state in World frame if (gPublish){ gsft_control::UAVStatePtr uav_state_msg(new gsft_control::UAVState); - uav_state_msg->position_ref.x = gController.tunning_nominal_Y.ref_out[0]; // gRef only for manual test - uav_state_msg->position_ref.y = gController.tunning_nominal_Y.ref_out[1]; - uav_state_msg->position_ref.z = gController.tunning_nominal_Y.ref_out[2]; - uav_state_msg->heading_ref = gController.tunning_nominal_Y.ref_out[3]; + uav_state_msg->position_ref.x = gController.tuning_nominal_Y.ref_out[0]; // gRef only for manual test + uav_state_msg->position_ref.y = gController.tuning_nominal_Y.ref_out[1]; + uav_state_msg->position_ref.z = gController.tuning_nominal_Y.ref_out[2]; + uav_state_msg->heading_ref = gController.tuning_nominal_Y.ref_out[3]; uav_state_msg->position_W.x = gOdometry.position_W.x(); uav_state_msg->position_W.y = gOdometry.position_W.y(); @@ -409,37 +409,37 @@ int main(int argc, char** argv) { uav_state_msg->rotation_speed_B.x = gOdometry.angular_velocity_B.x(); uav_state_msg->rotation_speed_B.y = gOdometry.angular_velocity_B.y(); uav_state_msg->rotation_speed_B.z = gOdometry.angular_velocity_B.z(); - uav_state_msg->total_thrust = gController.tunning_nominal_Y.virtual_control[0]; - uav_state_msg->moment.x = gController.tunning_nominal_Y.virtual_control[1]; - uav_state_msg->moment.y = gController.tunning_nominal_Y.virtual_control[2]; - uav_state_msg->moment.z = gController.tunning_nominal_Y.virtual_control[3]; - - uav_state_msg->LOE13_estimated.x = gController.tunning_nominal_Y.LOE13_estimated[0]; - uav_state_msg->LOE13_estimated.y = gController.tunning_nominal_Y.LOE13_estimated[1]; - uav_state_msg->LOE13_estimated.z = gController.tunning_nominal_Y.LOE13_estimated[2]; - uav_state_msg->LOE13_true.x = gController.tunning_nominal_Y.LOE_true[0]; - uav_state_msg->LOE13_true.y = gController.tunning_nominal_Y.LOE_true[1]; - uav_state_msg->LOE13_true.z = gController.tunning_nominal_Y.LOE_true[2]; - - uav_state_msg->thrust_pre.x = gController.tunning_nominal_Y.thrust_pre[0]; - uav_state_msg->thrust_pre.y = gController.tunning_nominal_Y.thrust_pre[1]; - uav_state_msg->thrust_pre.z = gController.tunning_nominal_Y.thrust_pre[2]; - - uav_state_msg->thrust_after.x = gController.tunning_nominal_Y.thrust_after[0]; - uav_state_msg->thrust_after.y = gController.tunning_nominal_Y.thrust_after[1]; - uav_state_msg->thrust_after.z = gController.tunning_nominal_Y.thrust_after[2]; - - uav_state_msg->vel_Kalman.x = gController.tunning_nominal_Y.vel_Kalman[0]; - uav_state_msg->vel_Kalman.y = gController.tunning_nominal_Y.vel_Kalman[1]; - uav_state_msg->vel_Kalman.z = gController.tunning_nominal_Y.vel_Kalman[2]; - - uav_state_msg->acc_Kalman.x = gController.tunning_nominal_Y.acc_Kalman[0]; - uav_state_msg->acc_Kalman.y = gController.tunning_nominal_Y.acc_Kalman[1]; - uav_state_msg->acc_Kalman.z = gController.tunning_nominal_Y.acc_Kalman[2]; - - uav_state_msg->M_Kalman.x = gController.tunning_nominal_Y.M_Kalman[0]; - uav_state_msg->M_Kalman.y = gController.tunning_nominal_Y.M_Kalman[1]; - uav_state_msg->M_Kalman.z = gController.tunning_nominal_Y.M_Kalman[2]; + uav_state_msg->total_thrust = gController.tuning_nominal_Y.virtual_control[0]; + uav_state_msg->moment.x = gController.tuning_nominal_Y.virtual_control[1]; + uav_state_msg->moment.y = gController.tuning_nominal_Y.virtual_control[2]; + uav_state_msg->moment.z = gController.tuning_nominal_Y.virtual_control[3]; + + uav_state_msg->LOE13_estimated.x = gController.tuning_nominal_Y.LOE13_estimated[0]; + uav_state_msg->LOE13_estimated.y = gController.tuning_nominal_Y.LOE13_estimated[1]; + uav_state_msg->LOE13_estimated.z = gController.tuning_nominal_Y.LOE13_estimated[2]; + uav_state_msg->LOE13_true.x = gController.tuning_nominal_Y.LOE_true[0]; + uav_state_msg->LOE13_true.y = gController.tuning_nominal_Y.LOE_true[1]; + uav_state_msg->LOE13_true.z = gController.tuning_nominal_Y.LOE_true[2]; + + uav_state_msg->thrust_pre.x = gController.tuning_nominal_Y.thrust_pre[0]; + uav_state_msg->thrust_pre.y = gController.tuning_nominal_Y.thrust_pre[1]; + uav_state_msg->thrust_pre.z = gController.tuning_nominal_Y.thrust_pre[2]; + + uav_state_msg->thrust_after.x = gController.tuning_nominal_Y.thrust_after[0]; + uav_state_msg->thrust_after.y = gController.tuning_nominal_Y.thrust_after[1]; + uav_state_msg->thrust_after.z = gController.tuning_nominal_Y.thrust_after[2]; + + uav_state_msg->vel_Kalman.x = gController.tuning_nominal_Y.vel_Kalman[0]; + uav_state_msg->vel_Kalman.y = gController.tuning_nominal_Y.vel_Kalman[1]; + uav_state_msg->vel_Kalman.z = gController.tuning_nominal_Y.vel_Kalman[2]; + + uav_state_msg->acc_Kalman.x = gController.tuning_nominal_Y.acc_Kalman[0]; + uav_state_msg->acc_Kalman.y = gController.tuning_nominal_Y.acc_Kalman[1]; + uav_state_msg->acc_Kalman.z = gController.tuning_nominal_Y.acc_Kalman[2]; + + uav_state_msg->M_Kalman.x = gController.tuning_nominal_Y.M_Kalman[0]; + uav_state_msg->M_Kalman.y = gController.tuning_nominal_Y.M_Kalman[1]; + uav_state_msg->M_Kalman.z = gController.tuning_nominal_Y.M_Kalman[2]; uav_state_msg->header.stamp = ros::Time::now(); uav_state_pub_.publish(uav_state_msg);