Dragon
Matlab code for quadcopter
Dependents: combination combination
Diff: LAAP.cpp
- Revision:
- 0:25d32fc1c12e
diff -r 000000000000 -r 25d32fc1c12e LAAP.cpp
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/LAAP.cpp Mon Nov 25 10:51:06 2019 +0000
@@ -0,0 +1,1113 @@
+//
+// File: LAAP.cpp
+//
+// Code generated for Simulink model 'LAAP'.
+//
+// Model version : 1.2
+// Simulink Coder version : 9.1 (R2019a) 23-Nov-2018
+// C/C++ source code generated on : Mon Nov 18 17:57:49 2019
+//
+// Target selection: ert.tlc
+// Embedded hardware selection: ARM Compatible->ARM Cortex
+// Code generation objectives:
+// 1. Execution efficiency
+// 2. RAM efficiency
+// Validation result: Not run
+//
+#include "LAAP.h"
+
+// Model step function
+void LAAPModelClass::step()
+{
+ static const int8_T b[3] = { 0, 1, 0 };
+
+ static const real_T b_0[12] = { 0.0028, 0.003, 3.1623, 0.0012, 0.0012, 3.68,
+ 1.6056, 1.6056, 1.0, 0.153, 0.153, 0.1 };
+
+ static const int8_T c[3] = { 0, 1, 0 };
+
+ static const real_T a[16] = { 0.25, 0.25, 0.25, 0.25, 0.0, 2.193, 0.0, -2.193,
+ -2.193, 0.0, 2.193, 0.0, 4.2591, -4.2591, 4.2591, -4.2591 };
+
+ int32_T r1;
+ int32_T i;
+ real_T Fdes_idx_2;
+ real_T Fdes_idx_1;
+ real_T Fdes_idx_0;
+ real_T rtb_TmpSignalConversionAtSFun_0;
+ real_T rtb_TmpSignalConversionAtSFun_1;
+ int32_T R_tmp;
+ int32_T b_I_tmp;
+ int32_T b_I_tmp_0;
+
+ // Outputs for Atomic SubSystem: '<Root>/Adaptive large angle controller'
+ // Delay: '<S1>/Delay1'
+ memcpy(&rtDW.Delay1[0], &rtDW.psy[0], 12U * sizeof(real_T));
+
+ // Outputs for Atomic SubSystem: '<S1>/quadcopter_dynamics_prediction'
+ // MATLAB Function: '<S3>/Euler equations of motion1 ' incorporates:
+ // Constant: '<S1>/mass '
+ // Delay: '<S1>/Delay'
+ // Delay: '<S3>/Delay'
+ // Delay: '<S3>/Delay1'
+
+ rtDW.b_I[0] = 0.01023564759;
+ rtDW.b_I[3] = 0.0;
+ rtDW.b_I[6] = 0.0;
+ rtDW.b_I[1] = 0.0;
+ rtDW.b_I[4] = 0.01023564759;
+ rtDW.b_I[7] = 0.0;
+ rtDW.b_I[2] = 0.0;
+ rtDW.b_I[5] = 0.0;
+ rtDW.b_I[8] = 0.02047129518;
+ rtDW.maxval = std::cos(rtDW.Delay1_DSTATE_l[2]);
+ rtDW.b_I_k[0] = rtDW.maxval;
+ rtDW.Add1_k = std::sin(rtDW.Delay1_DSTATE_l[2]);
+ rtDW.b_I_k[3] = -rtDW.Add1_k;
+ rtDW.b_I_k[6] = 0.0;
+ rtDW.b_I_k[1] = rtDW.Add1_k;
+ rtDW.b_I_k[4] = rtDW.maxval;
+ rtDW.b_I_k[7] = 0.0;
+ rtDW.b_I_k[2] = 0.0;
+ rtDW.d[0] = 1.0;
+ rtDW.b_I_k[5] = 0.0;
+ rtDW.d[3] = 0.0;
+ rtDW.b_I_k[8] = 1.0;
+ rtDW.d[6] = 0.0;
+ rtDW.d[1] = 0.0;
+ Fdes_idx_0 = std::cos(rtDW.Delay1_DSTATE_l[0]);
+ rtDW.d[4] = Fdes_idx_0;
+ Fdes_idx_1 = std::sin(rtDW.Delay1_DSTATE_l[0]);
+ rtDW.d[7] = -Fdes_idx_1;
+ rtDW.d[2] = 0.0;
+ rtDW.d[5] = Fdes_idx_1;
+ rtDW.d[8] = Fdes_idx_0;
+ rtDW.maxval = std::cos(rtDW.Delay1_DSTATE_l[1]);
+ rtDW.dv0[0] = rtDW.maxval;
+ rtDW.dv0[3] = 0.0;
+ rtDW.Add1_k = std::sin(rtDW.Delay1_DSTATE_l[1]);
+ rtDW.dv0[6] = rtDW.Add1_k;
+ for (r1 = 0; r1 < 3; r1++) {
+ for (i = 0; i < 3; i++) {
+ R_tmp = r1 + 3 * i;
+ rtDW.R_c[R_tmp] = 0.0;
+ rtDW.R_c[R_tmp] += rtDW.d[3 * i] * rtDW.b_I_k[r1];
+ rtDW.R_c[R_tmp] += rtDW.d[3 * i + 1] * rtDW.b_I_k[r1 + 3];
+ rtDW.R_c[R_tmp] += rtDW.d[3 * i + 2] * rtDW.b_I_k[r1 + 6];
+ }
+
+ rtDW.dv0[1 + 3 * r1] = b[r1];
+ }
+
+ rtDW.dv0[2] = -rtDW.Add1_k;
+ rtDW.dv0[5] = 0.0;
+ rtDW.dv0[8] = rtDW.maxval;
+ rtDW.w_dot[0] = 0.0;
+ rtDW.w_dot[1] = 0.0;
+ rtDW.w_dot[2] = rtDW.Delay_DSTATE[0];
+ rtDW.dv2[0] = 0.0;
+ rtDW.dv2[1] = 0.0;
+ rtDW.dv2[2] = -7.3575;
+ for (r1 = 0; r1 < 3; r1++) {
+ rtDW.wb[r1] = 0.0;
+ for (i = 0; i < 3; i++) {
+ R_tmp = r1 + 3 * i;
+ rtDW.b_I_k[R_tmp] = 0.0;
+ rtDW.b_I_k[R_tmp] += rtDW.dv0[3 * i] * rtDW.R_c[r1];
+ rtDW.b_I_k[R_tmp] += rtDW.dv0[3 * i + 1] * rtDW.R_c[r1 + 3];
+ rtDW.b_I_k[R_tmp] += rtDW.dv0[3 * i + 2] * rtDW.R_c[r1 + 6];
+ rtDW.wb[r1] += rtDW.b_I_k[R_tmp] * rtDW.w_dot[i];
+ }
+
+ rtDW.a_dotdot[r1] = (rtDW.dv2[r1] + rtDW.wb[r1]) * 1.3333333333333333;
+ }
+
+ rtDW.b_I_k[0] = rtDW.maxval;
+ rtDW.b_I_k[3] = 0.0;
+ rtDW.b_I_k[6] = -Fdes_idx_0 * rtDW.Add1_k;
+ rtDW.b_I_k[1] = 0.0;
+ rtDW.b_I_k[4] = 1.0;
+ rtDW.b_I_k[7] = Fdes_idx_1;
+ rtDW.b_I_k[2] = rtDW.Add1_k;
+ rtDW.b_I_k[5] = 0.0;
+ rtDW.b_I_k[8] = Fdes_idx_0 * rtDW.maxval;
+ for (r1 = 0; r1 < 3; r1++) {
+ rtDW.wb[r1] = rtDW.b_I_k[r1 + 6] * rtDW.Delay_DSTATE_h[2] + (rtDW.b_I_k[r1 +
+ 3] * rtDW.Delay_DSTATE_h[1] + rtDW.b_I_k[r1] * rtDW.Delay_DSTATE_h[0]);
+ }
+
+ rtDW.b_I_k[0] = 0.0;
+ rtDW.b_I_k[3] = -rtDW.wb[2];
+ rtDW.b_I_k[6] = rtDW.wb[1];
+ rtDW.b_I_k[1] = rtDW.wb[2];
+ rtDW.b_I_k[4] = 0.0;
+ rtDW.b_I_k[7] = -rtDW.wb[0];
+ rtDW.b_I_k[2] = -rtDW.wb[1];
+ rtDW.b_I_k[5] = rtDW.wb[0];
+ rtDW.b_I_k[8] = 0.0;
+ rtDW.w_dot[0] = rtDW.Delay_DSTATE[1];
+ rtDW.w_dot[1] = rtDW.Delay_DSTATE[2];
+ rtDW.w_dot[2] = rtDW.Delay_DSTATE[3];
+ for (r1 = 0; r1 < 3; r1++) {
+ rtDW.dv2[r1] = 0.0;
+ for (i = 0; i < 3; i++) {
+ R_tmp = r1 + 3 * i;
+ rtDW.R_c[R_tmp] = 0.0;
+ rtDW.R_c[R_tmp] += rtDW.b_I[3 * i] * rtDW.b_I_k[r1];
+ rtDW.R_c[R_tmp] += rtDW.b_I[3 * i + 1] * rtDW.b_I_k[r1 + 3];
+ rtDW.R_c[R_tmp] += rtDW.b_I[3 * i + 2] * rtDW.b_I_k[r1 + 6];
+ rtDW.dv2[r1] += rtDW.R_c[R_tmp] * rtDW.wb[i];
+ }
+
+ rtDW.Iw_dot[r1] = rtDW.w_dot[r1] - rtDW.dv2[r1];
+ }
+
+ rtDW.w_dot[1] = rtDW.Iw_dot[1] - rtDW.Iw_dot[0] * 0.0;
+ rtDW.w_dot[2] = (rtDW.Iw_dot[2] - rtDW.Iw_dot[0] * 0.0) - rtDW.w_dot[1] * 0.0;
+ rtDW.w_dot[2] /= 0.02047129518;
+ rtDW.w_dot[0] = rtDW.Iw_dot[0] - rtDW.w_dot[2] * 0.0;
+ rtDW.w_dot[1] -= rtDW.w_dot[2] * 0.0;
+ rtDW.w_dot[1] /= 0.01023564759;
+ rtDW.w_dot[0] -= rtDW.w_dot[1] * 0.0;
+ rtDW.w_dot[0] /= 0.01023564759;
+
+ // DiscreteIntegrator: '<S13>/Integrator'
+ for (r1 = 0; r1 < 6; r1++) {
+ if (rtDW.Integrator_PrevResetState != 0) {
+ rtDW.Integrator_DSTATE_ep[r1] = 0.0;
+ if (rtDW.Integrator_DSTATE_ep[r1] >= 10.0) {
+ rtDW.Integrator_DSTATE_ep[r1] = 10.0;
+ } else {
+ if (rtDW.Integrator_DSTATE_ep[r1] <= -10.0) {
+ rtDW.Integrator_DSTATE_ep[r1] = -10.0;
+ }
+ }
+ }
+
+ if (rtDW.Integrator_DSTATE_ep[r1] >= 10.0) {
+ rtDW.Integrator_DSTATE_ep[r1] = 10.0;
+ } else {
+ if (rtDW.Integrator_DSTATE_ep[r1] <= -10.0) {
+ rtDW.Integrator_DSTATE_ep[r1] = -10.0;
+ }
+ }
+
+ rtDW.Integrator[r1] = rtDW.Integrator_DSTATE_ep[r1];
+ }
+
+ // DiscreteIntegrator: '<S14>/Integrator' incorporates:
+ // Inport: '<Root>/phi'
+ // Inport: '<Root>/psi'
+ // Inport: '<Root>/theta'
+ // Inport: '<Root>/x'
+ // Inport: '<Root>/y'
+ // Inport: '<Root>/z'
+
+ if (rtDW.Integrator_IC_LOADING != 0) {
+ rtDW.Integrator_DSTATE_k[0] = rtU.x;
+ rtDW.Integrator_DSTATE_k[1] = rtU.y;
+ rtDW.Integrator_DSTATE_k[2] = rtU.z;
+ rtDW.Integrator_DSTATE_k[3] = rtU.phi;
+ rtDW.Integrator_DSTATE_k[4] = rtU.theta;
+ rtDW.Integrator_DSTATE_k[5] = rtU.psi;
+ for (r1 = 0; r1 < 6; r1++) {
+ if (rtDW.Integrator_DSTATE_k[r1] >= 10.0) {
+ rtDW.Integrator_DSTATE_k[r1] = 10.0;
+ } else {
+ if (rtDW.Integrator_DSTATE_k[r1] <= -10.0) {
+ rtDW.Integrator_DSTATE_k[r1] = -10.0;
+ }
+ }
+ }
+ }
+
+ if (rtDW.Integrator_PrevResetState_n != 0) {
+ rtDW.Integrator_DSTATE_k[0] = rtU.x;
+ rtDW.Integrator_DSTATE_k[1] = rtU.y;
+ rtDW.Integrator_DSTATE_k[2] = rtU.z;
+ rtDW.Integrator_DSTATE_k[3] = rtU.phi;
+ rtDW.Integrator_DSTATE_k[4] = rtU.theta;
+ rtDW.Integrator_DSTATE_k[5] = rtU.psi;
+ for (r1 = 0; r1 < 6; r1++) {
+ if (rtDW.Integrator_DSTATE_k[r1] >= 10.0) {
+ rtDW.Integrator_DSTATE_k[r1] = 10.0;
+ } else {
+ if (rtDW.Integrator_DSTATE_k[r1] <= -10.0) {
+ rtDW.Integrator_DSTATE_k[r1] = -10.0;
+ }
+ }
+ }
+ }
+
+ for (r1 = 0; r1 < 6; r1++) {
+ if (rtDW.Integrator_DSTATE_k[r1] >= 10.0) {
+ rtDW.Integrator_DSTATE_k[r1] = 10.0;
+ } else {
+ if (rtDW.Integrator_DSTATE_k[r1] <= -10.0) {
+ rtDW.Integrator_DSTATE_k[r1] = -10.0;
+ }
+ }
+
+ rtDW.Integrator_p[r1] = rtDW.Integrator_DSTATE_k[r1];
+ }
+
+ // Update for Delay: '<S3>/Delay' incorporates:
+ // DiscreteIntegrator: '<S13>/Integrator'
+
+ rtDW.Delay_DSTATE_h[0] = rtDW.Integrator_DSTATE_ep[3];
+
+ // Update for Delay: '<S3>/Delay1' incorporates:
+ // DiscreteIntegrator: '<S14>/Integrator'
+
+ rtDW.Delay1_DSTATE_l[0] = rtDW.Integrator_DSTATE_k[3];
+
+ // Update for Delay: '<S3>/Delay' incorporates:
+ // DiscreteIntegrator: '<S13>/Integrator'
+
+ rtDW.Delay_DSTATE_h[1] = rtDW.Integrator_DSTATE_ep[4];
+
+ // Update for Delay: '<S3>/Delay1' incorporates:
+ // DiscreteIntegrator: '<S14>/Integrator'
+
+ rtDW.Delay1_DSTATE_l[1] = rtDW.Integrator_DSTATE_k[4];
+
+ // Update for Delay: '<S3>/Delay' incorporates:
+ // DiscreteIntegrator: '<S13>/Integrator'
+
+ rtDW.Delay_DSTATE_h[2] = rtDW.Integrator_DSTATE_ep[5];
+
+ // Update for Delay: '<S3>/Delay1' incorporates:
+ // DiscreteIntegrator: '<S14>/Integrator'
+
+ rtDW.Delay1_DSTATE_l[2] = rtDW.Integrator_DSTATE_k[5];
+
+ // Update for DiscreteIntegrator: '<S13>/Integrator' incorporates:
+ // MATLAB Function: '<S3>/Euler equations of motion1 '
+
+ rtDW.dv1[0] = 0.0001 * rtDW.a_dotdot[0];
+ rtDW.dv1[1] = 0.0001 * rtDW.a_dotdot[1];
+ rtDW.dv1[2] = 0.0001 * rtDW.a_dotdot[2];
+ rtDW.dv1[3] = 0.0001 * rtDW.w_dot[0];
+ rtDW.dv1[4] = 0.0001 * rtDW.w_dot[1];
+ rtDW.dv1[5] = 0.0001 * rtDW.w_dot[2];
+ rtDW.Integrator_PrevResetState = 0;
+
+ // Update for DiscreteIntegrator: '<S14>/Integrator'
+ rtDW.Integrator_IC_LOADING = 0U;
+ for (r1 = 0; r1 < 6; r1++) {
+ // Update for DiscreteIntegrator: '<S13>/Integrator'
+ rtDW.Integrator_DSTATE_ep[r1] += rtDW.dv1[r1];
+ if (rtDW.Integrator_DSTATE_ep[r1] >= 10.0) {
+ rtDW.Integrator_DSTATE_ep[r1] = 10.0;
+ } else {
+ if (rtDW.Integrator_DSTATE_ep[r1] <= -10.0) {
+ rtDW.Integrator_DSTATE_ep[r1] = -10.0;
+ }
+ }
+
+ // Update for DiscreteIntegrator: '<S14>/Integrator'
+ rtDW.Integrator_DSTATE_k[r1] += 0.0001 * rtDW.Integrator[r1];
+ if (rtDW.Integrator_DSTATE_k[r1] >= 10.0) {
+ rtDW.Integrator_DSTATE_k[r1] = 10.0;
+ } else {
+ if (rtDW.Integrator_DSTATE_k[r1] <= -10.0) {
+ rtDW.Integrator_DSTATE_k[r1] = -10.0;
+ }
+ }
+ }
+
+ // Update for DiscreteIntegrator: '<S14>/Integrator'
+ rtDW.Integrator_PrevResetState_n = 0;
+
+ // End of Outputs for SubSystem: '<S1>/quadcopter_dynamics_prediction'
+
+ // Outputs for Atomic SubSystem: '<S1>/largepaa'
+ // Outputs for Enabled SubSystem: '<S2>/large_controller' incorporates:
+ // EnablePort: '<S5>/Enable'
+
+ // MATLAB Function: '<S5>/MATLAB Function3' incorporates:
+ // Constant: '<S2>/mass 1'
+ // Delay: '<S2>/Delay'
+ // Inport: '<Root>/angles'
+ // Inport: '<Root>/r'
+ // Inport: '<Root>/rdot'
+ // Inport: '<Root>/wb'
+ // Inport: '<Root>/x'
+ // Inport: '<Root>/y'
+ // Inport: '<Root>/z'
+
+ rtDW.a_dotdot[0] = rtU.r[0] - rtU.x;
+ rtDW.a_dotdot[1] = rtU.r[1] - rtU.y;
+ rtDW.a_dotdot[2] = rtU.r[2] - rtU.z;
+ Fdes_idx_0 = -rtDW.Delay_DSTATE_hf[0] * rtDW.a_dotdot[0] -
+ rtDW.Delay_DSTATE_hf[3] * rtU.rdot[0];
+ rtDW.Iw_dot[0] = rtU.rdot[0];
+ rtDW.w_dot[0] = 0.0;
+ Fdes_idx_1 = -rtDW.Delay_DSTATE_hf[1] * rtDW.a_dotdot[1] -
+ rtDW.Delay_DSTATE_hf[4] * rtU.rdot[1];
+ rtDW.Iw_dot[1] = rtU.rdot[1];
+ rtDW.w_dot[1] = 0.0;
+ Fdes_idx_2 = ((-rtDW.Delay_DSTATE_hf[2] * rtDW.a_dotdot[2] -
+ rtDW.Delay_DSTATE_hf[5] * rtU.rdot[2]) - 7.3575) + 7.3575;
+ rtDW.Iw_dot[2] = rtU.rdot[2];
+ rtDW.w_dot[2] = 1.0;
+ rtDW.maxval = std::sqrt((Fdes_idx_0 * Fdes_idx_0 + Fdes_idx_1 * Fdes_idx_1) +
+ Fdes_idx_2 * Fdes_idx_2);
+ if (rtDW.maxval == 0.0) {
+ rtDW.maxval = 1.0;
+ }
+
+ rtDW.maxval = 1.0 / rtDW.maxval;
+ rtDW.Zbdes[0] = rtDW.maxval * Fdes_idx_0;
+ rtDW.Zbdes[1] = rtDW.maxval * Fdes_idx_1;
+ rtDW.Zbdes[2] = rtDW.maxval * Fdes_idx_2;
+ rtDW.crossZX[0] = rtDW.Zbdes[1] * 0.0 - rtDW.Zbdes[2] * 0.0;
+ rtDW.crossZX[1] = rtDW.Zbdes[2] - rtDW.Zbdes[0] * 0.0;
+ rtDW.crossZX[2] = rtDW.Zbdes[0] * 0.0 - rtDW.Zbdes[1];
+ rtDW.maxval = std::sqrt((rtDW.crossZX[0] * rtDW.crossZX[0] + rtDW.crossZX[1] *
+ rtDW.crossZX[1]) + rtDW.crossZX[2] * rtDW.crossZX[2]);
+ if (rtDW.maxval == 0.0) {
+ rtDW.maxval = 1.0;
+ }
+
+ rtDW.maxval = 1.0 / rtDW.maxval;
+ rtDW.Add1_k = rtDW.maxval * rtDW.crossZX[0];
+ rtDW.b_I[3] = rtDW.Add1_k;
+ rtDW.b_I[6] = rtDW.Zbdes[0];
+ rtDW.crossZX[0] = rtDW.Add1_k;
+ rtDW.Add1_k = rtDW.maxval * rtDW.crossZX[1];
+ rtDW.b_I[4] = rtDW.Add1_k;
+ rtDW.b_I[7] = rtDW.Zbdes[1];
+ rtDW.crossZX[1] = rtDW.Add1_k;
+ rtDW.Add1_k = rtDW.maxval * rtDW.crossZX[2];
+ rtDW.b_I[5] = rtDW.Add1_k;
+ rtDW.b_I[8] = rtDW.Zbdes[2];
+ rtDW.b_I[0] = rtDW.crossZX[1] * rtDW.Zbdes[2] - rtDW.Add1_k * rtDW.Zbdes[1];
+ rtDW.b_I[1] = rtDW.Add1_k * rtDW.Zbdes[0] - rtDW.crossZX[0] * rtDW.Zbdes[2];
+ rtDW.b_I[2] = rtDW.crossZX[0] * rtDW.Zbdes[1] - rtDW.crossZX[1] * rtDW.Zbdes[0];
+ rtDW.maxval = std::cos(rtU.angles[2]);
+ rtDW.b_I_k[0] = rtDW.maxval;
+ rtDW.Add1_k = std::sin(rtU.angles[2]);
+ rtDW.b_I_k[3] = -rtDW.Add1_k;
+ rtDW.b_I_k[6] = 0.0;
+ rtDW.b_I_k[1] = rtDW.Add1_k;
+ rtDW.b_I_k[4] = rtDW.maxval;
+ rtDW.b_I_k[7] = 0.0;
+ rtDW.b_I_k[2] = 0.0;
+ rtDW.d[0] = 1.0;
+ rtDW.b_I_k[5] = 0.0;
+ rtDW.d[3] = 0.0;
+ rtDW.b_I_k[8] = 1.0;
+ rtDW.d[6] = 0.0;
+ rtDW.d[1] = 0.0;
+ rtDW.maxval = std::cos(rtU.angles[0]);
+ rtDW.d[4] = rtDW.maxval;
+ rtDW.Add1_k = std::sin(rtU.angles[0]);
+ rtDW.d[7] = -rtDW.Add1_k;
+ rtDW.d[2] = 0.0;
+ rtDW.d[5] = rtDW.Add1_k;
+ rtDW.d[8] = rtDW.maxval;
+ rtDW.maxval = std::cos(rtU.angles[1]);
+ rtDW.dv0[0] = rtDW.maxval;
+ rtDW.dv0[3] = 0.0;
+ rtDW.Add1_k = std::sin(rtU.angles[1]);
+ rtDW.dv0[6] = rtDW.Add1_k;
+ for (r1 = 0; r1 < 3; r1++) {
+ for (i = 0; i < 3; i++) {
+ R_tmp = r1 + 3 * i;
+ rtDW.R_c[R_tmp] = 0.0;
+ rtDW.R_c[R_tmp] += rtDW.d[3 * i] * rtDW.b_I_k[r1];
+ rtDW.R_c[R_tmp] += rtDW.d[3 * i + 1] * rtDW.b_I_k[r1 + 3];
+ rtDW.R_c[R_tmp] += rtDW.d[3 * i + 2] * rtDW.b_I_k[r1 + 6];
+ }
+
+ rtDW.dv0[1 + 3 * r1] = c[r1];
+ }
+
+ rtDW.dv0[2] = -rtDW.Add1_k;
+ rtDW.dv0[5] = 0.0;
+ rtDW.dv0[8] = rtDW.maxval;
+ for (r1 = 0; r1 < 3; r1++) {
+ rtDW.Zbdes[r1] = 0.0;
+ for (i = 0; i < 3; i++) {
+ R_tmp = r1 + 3 * i;
+ rtDW.R[R_tmp] = 0.0;
+ rtDW.R[R_tmp] += rtDW.dv0[3 * i] * rtDW.R_c[r1];
+ rtDW.R[R_tmp] += rtDW.dv0[3 * i + 1] * rtDW.R_c[r1 + 3];
+ rtDW.R[R_tmp] += rtDW.dv0[3 * i + 2] * rtDW.R_c[r1 + 6];
+ rtDW.Zbdes[r1] += rtDW.R[R_tmp] * rtDW.w_dot[i];
+ }
+ }
+
+ for (r1 = 0; r1 < 3; r1++) {
+ for (i = 0; i < 3; i++) {
+ R_tmp = r1 + 3 * i;
+ rtDW.b_I_k[R_tmp] = 0.0;
+ rtDW.R_c[R_tmp] = 0.0;
+ rtDW.b_I_k[R_tmp] += rtDW.b_I[3 * r1] * rtDW.R[3 * i];
+ rtDW.R_c[R_tmp] += rtDW.R[3 * r1] * rtDW.b_I[3 * i];
+ b_I_tmp = 3 * r1 + 1;
+ b_I_tmp_0 = 3 * i + 1;
+ rtDW.b_I_k[R_tmp] += rtDW.b_I[b_I_tmp] * rtDW.R[b_I_tmp_0];
+ rtDW.R_c[R_tmp] += rtDW.R[b_I_tmp] * rtDW.b_I[b_I_tmp_0];
+ b_I_tmp = 3 * r1 + 2;
+ b_I_tmp_0 = 3 * i + 2;
+ rtDW.b_I_k[R_tmp] += rtDW.b_I[b_I_tmp] * rtDW.R[b_I_tmp_0];
+ rtDW.R_c[R_tmp] += rtDW.R[b_I_tmp] * rtDW.b_I[b_I_tmp_0];
+ }
+ }
+
+ for (r1 = 0; r1 < 9; r1++) {
+ rtDW.b_I[r1] = (rtDW.b_I_k[r1] - rtDW.R_c[r1]) * 0.5;
+ }
+
+ rtDW.Xc[0] = rtDW.b_I[5];
+ rtDW.Xc[1] = rtDW.b_I[6];
+ rtDW.Xc[2] = rtDW.b_I[1];
+ rtDW.b_I_k[0] = -rtDW.Delay_DSTATE_hf[6];
+ rtDW.b_I_k[3] = -0.0;
+ rtDW.b_I_k[6] = -0.0;
+ rtDW.b_I_k[1] = -0.0;
+ rtDW.b_I_k[4] = -rtDW.Delay_DSTATE_hf[7];
+ rtDW.b_I_k[7] = -0.0;
+ rtDW.b_I_k[2] = -0.0;
+ rtDW.b_I_k[5] = -0.0;
+ rtDW.b_I_k[8] = -rtDW.Delay_DSTATE_hf[8];
+ rtDW.R_c[0] = rtDW.Delay_DSTATE_hf[9];
+ rtDW.R_c[3] = 0.0;
+ rtDW.R_c[6] = 0.0;
+ rtDW.R_c[1] = 0.0;
+ rtDW.R_c[4] = rtDW.Delay_DSTATE_hf[10];
+ rtDW.R_c[7] = 0.0;
+ rtDW.R_c[2] = 0.0;
+ rtDW.R_c[5] = 0.0;
+ rtDW.R_c[8] = rtDW.Delay_DSTATE_hf[11];
+ for (r1 = 0; r1 < 3; r1++) {
+ rtDW.psy[r1] = rtDW.a_dotdot[r1];
+ rtDW.psy[r1 + 3] = rtDW.Iw_dot[r1];
+ rtDW.psy[r1 + 6] = rtDW.Xc[r1];
+ rtDW.psy[r1 + 9] = rtU.wb[r1];
+ rtDW.crossZX[r1] = rtU.wb[r1];
+ rtDW.w_dot[r1] = rtDW.b_I_k[r1 + 6] * rtDW.Xc[2] + (rtDW.b_I_k[r1 + 3] *
+ rtDW.Xc[1] + rtDW.b_I_k[r1] * rtDW.Xc[0]);
+ }
+
+ rtDW.A[0] = ((Fdes_idx_0 * rtDW.Zbdes[0] + Fdes_idx_1 * rtDW.Zbdes[1]) +
+ Fdes_idx_2 * rtDW.Zbdes[2]) + 7.3575;
+ for (r1 = 0; r1 < 3; r1++) {
+ rtDW.A[r1 + 1] = rtDW.w_dot[r1] - (rtDW.R_c[r1 + 6] * rtDW.crossZX[2] +
+ (rtDW.R_c[r1 + 3] * rtDW.crossZX[1] + rtDW.R_c[r1] * rtDW.crossZX[0]));
+ }
+
+ // End of MATLAB Function: '<S5>/MATLAB Function3'
+
+ // Saturate: '<S5>/Saturation'
+ if (rtDW.A[0] > 27.0) {
+ rtDW.maxval = 27.0;
+ } else if (rtDW.A[0] < 0.0) {
+ rtDW.maxval = 0.0;
+ } else {
+ rtDW.maxval = rtDW.A[0];
+ }
+
+ if (rtDW.A[1] > 1.6) {
+ Fdes_idx_0 = 1.6;
+ } else if (rtDW.A[1] < -1.6) {
+ Fdes_idx_0 = -1.6;
+ } else {
+ Fdes_idx_0 = rtDW.A[1];
+ }
+
+ if (rtDW.A[2] > 1.6) {
+ Fdes_idx_1 = 1.6;
+ } else if (rtDW.A[2] < -1.6) {
+ Fdes_idx_1 = -1.6;
+ } else {
+ Fdes_idx_1 = rtDW.A[2];
+ }
+
+ if (rtDW.A[3] > 0.6) {
+ Fdes_idx_2 = 0.6;
+ } else if (rtDW.A[3] < -0.6) {
+ Fdes_idx_2 = -0.6;
+ } else {
+ Fdes_idx_2 = rtDW.A[3];
+ }
+
+ // End of Saturate: '<S5>/Saturation'
+
+ // MATLAB Function: '<S5>/MATLAB Function1'
+ for (r1 = 0; r1 < 4; r1++) {
+ rtDW.Add1_k = a[r1 + 12] * Fdes_idx_2 + (a[r1 + 8] * Fdes_idx_1 + (a[r1 + 4]
+ * Fdes_idx_0 + a[r1] * rtDW.maxval));
+ rtDW.A[r1] = rtDW.Add1_k;
+ }
+
+ if (rtDW.A[0] < 0.0) {
+ rtDW.A[0] = 0.0;
+ }
+
+ if (rtDW.A[1] < 0.0) {
+ rtDW.A[1] = 0.0;
+ }
+
+ if (rtDW.A[2] < 0.0) {
+ rtDW.A[2] = 0.0;
+ }
+
+ if (rtDW.A[3] < 0.0) {
+ rtDW.A[3] = 0.0;
+ }
+
+ rtDW.duty[0] = (3485.3574033876184 * std::sqrt(rtDW.A[0]) * 0.1865 + 768.0) /
+ 20000.0;
+ rtDW.duty[1] = (3485.3574033876184 * std::sqrt(rtDW.A[1]) * 0.1865 + 768.0) /
+ 20000.0;
+ rtDW.duty[2] = (3485.3574033876184 * std::sqrt(rtDW.A[2]) * 0.1865 + 768.0) /
+ 20000.0;
+ rtDW.duty[3] = (3485.3574033876184 * std::sqrt(rtDW.A[3]) * 0.1865 + 768.0) /
+ 20000.0;
+
+ // End of MATLAB Function: '<S5>/MATLAB Function1'
+ // End of Outputs for SubSystem: '<S2>/large_controller'
+
+ // MATLAB Function: '<S6>/MATLAB Function3'
+ rtDW.F[0] = 0.001;
+ rtDW.F[12] = 0.0;
+ rtDW.F[24] = 0.0;
+ rtDW.F[36] = 0.0;
+ rtDW.F[48] = 0.0;
+ rtDW.F[60] = 0.0;
+ rtDW.F[72] = 0.0;
+ rtDW.F[84] = 0.0;
+ rtDW.F[96] = 0.0;
+ rtDW.F[108] = 0.0;
+ rtDW.F[120] = 0.0;
+ rtDW.F[132] = 0.0;
+ rtDW.F[1] = 0.0;
+ rtDW.F[13] = 0.001;
+ rtDW.F[25] = 0.0;
+ rtDW.F[37] = 0.0;
+ rtDW.F[49] = 0.0;
+ rtDW.F[61] = 0.0;
+ rtDW.F[73] = 0.0;
+ rtDW.F[85] = 0.0;
+ rtDW.F[97] = 0.0;
+ rtDW.F[109] = 0.0;
+ rtDW.F[121] = 0.0;
+ rtDW.F[133] = 0.0;
+ rtDW.F[2] = 0.0;
+ rtDW.F[14] = 0.0;
+ rtDW.F[26] = 0.001;
+ rtDW.F[38] = 0.0;
+ rtDW.F[50] = 0.0;
+ rtDW.F[62] = 0.0;
+ rtDW.F[74] = 0.0;
+ rtDW.F[86] = 0.0;
+ rtDW.F[98] = 0.0;
+ rtDW.F[110] = 0.0;
+ rtDW.F[122] = 0.0;
+ rtDW.F[134] = 0.0;
+ rtDW.F[3] = 0.0;
+ rtDW.F[15] = 0.0;
+ rtDW.F[27] = 0.0;
+ rtDW.F[39] = 0.001;
+ rtDW.F[51] = 0.0;
+ rtDW.F[63] = 0.0;
+ rtDW.F[75] = 0.0;
+ rtDW.F[87] = 0.0;
+ rtDW.F[99] = 0.0;
+ rtDW.F[111] = 0.0;
+ rtDW.F[123] = 0.0;
+ rtDW.F[135] = 0.0;
+ rtDW.F[4] = 0.0;
+ rtDW.F[16] = 0.0;
+ rtDW.F[28] = 0.0;
+ rtDW.F[40] = 0.0;
+ rtDW.F[52] = 0.001;
+ rtDW.F[64] = 0.0;
+ rtDW.F[76] = 0.0;
+ rtDW.F[88] = 0.0;
+ rtDW.F[100] = 0.0;
+ rtDW.F[112] = 0.0;
+ rtDW.F[124] = 0.0;
+ rtDW.F[136] = 0.0;
+ rtDW.F[5] = 0.0;
+ rtDW.F[17] = 0.0;
+ rtDW.F[29] = 0.0;
+ rtDW.F[41] = 0.0;
+ rtDW.F[53] = 0.0;
+ rtDW.F[65] = 0.001;
+ rtDW.F[77] = 0.0;
+ rtDW.F[89] = 0.0;
+ rtDW.F[101] = 0.0;
+ rtDW.F[113] = 0.0;
+ rtDW.F[125] = 0.0;
+ rtDW.F[137] = 0.0;
+ rtDW.F[6] = 0.0;
+ rtDW.F[18] = 0.0;
+ rtDW.F[30] = 0.0;
+ rtDW.F[42] = 0.0;
+ rtDW.F[54] = 0.0;
+ rtDW.F[66] = 0.0;
+ rtDW.F[78] = 0.0001;
+ rtDW.F[90] = 0.0;
+ rtDW.F[102] = 0.0;
+ rtDW.F[114] = 0.0;
+ rtDW.F[126] = 0.0;
+ rtDW.F[138] = 0.0;
+ rtDW.F[7] = 0.0;
+ rtDW.F[19] = 0.0;
+ rtDW.F[31] = 0.0;
+ rtDW.F[43] = 0.0;
+ rtDW.F[55] = 0.0;
+ rtDW.F[67] = 0.0;
+ rtDW.F[79] = 0.0;
+ rtDW.F[91] = 0.0001;
+ rtDW.F[103] = 0.0;
+ rtDW.F[115] = 0.0;
+ rtDW.F[127] = 0.0;
+ rtDW.F[139] = 0.0;
+ rtDW.F[8] = 0.0;
+ rtDW.F[20] = 0.0;
+ rtDW.F[32] = 0.0;
+ rtDW.F[44] = 0.0;
+ rtDW.F[56] = 0.0;
+ rtDW.F[68] = 0.0;
+ rtDW.F[80] = 0.0;
+ rtDW.F[92] = 0.0;
+ rtDW.F[104] = 0.0001;
+ rtDW.F[116] = 0.0;
+ rtDW.F[128] = 0.0;
+ rtDW.F[140] = 0.0;
+ rtDW.F[9] = 0.0;
+ rtDW.F[21] = 0.0;
+ rtDW.F[33] = 0.0;
+ rtDW.F[45] = 0.0;
+ rtDW.F[57] = 0.0;
+ rtDW.F[69] = 0.0;
+ rtDW.F[81] = 0.0;
+ rtDW.F[93] = 0.0;
+ rtDW.F[105] = 0.0;
+ rtDW.F[117] = 0.0001;
+ rtDW.F[129] = 0.0;
+ rtDW.F[141] = 0.0;
+ rtDW.F[10] = 0.0;
+ rtDW.F[22] = 0.0;
+ rtDW.F[34] = 0.0;
+ rtDW.F[46] = 0.0;
+ rtDW.F[58] = 0.0;
+ rtDW.F[70] = 0.0;
+ rtDW.F[82] = 0.0;
+ rtDW.F[94] = 0.0;
+ rtDW.F[106] = 0.0;
+ rtDW.F[118] = 0.0;
+ rtDW.F[130] = 0.0001;
+ rtDW.F[142] = 0.0;
+ rtDW.F[11] = 0.0;
+ rtDW.F[23] = 0.0;
+ rtDW.F[35] = 0.0;
+ rtDW.F[47] = 0.0;
+ rtDW.F[59] = 0.0;
+ rtDW.F[71] = 0.0;
+ rtDW.F[83] = 0.0;
+ rtDW.F[95] = 0.0;
+ rtDW.F[107] = 0.0;
+ rtDW.F[119] = 0.0;
+ rtDW.F[131] = 0.0;
+ rtDW.F[143] = 0.0001;
+ rtDW.maxval = 0.0;
+ for (r1 = 0; r1 < 12; r1++) {
+ rtDW.rtb_Delay1_m[r1] = 0.0;
+ for (i = 0; i < 12; i++) {
+ rtDW.rtb_Delay1_m[r1] += rtDW.F[12 * r1 + i] * rtDW.Delay1[i];
+ }
+
+ rtDW.maxval += rtDW.rtb_Delay1_m[r1] * rtDW.Delay1[r1];
+ }
+
+ // SignalConversion: '<S9>/TmpSignal ConversionAt SFunction Inport2' incorporates:
+ // Inport: '<Root>/angles'
+ // Inport: '<Root>/r'
+ // Inport: '<Root>/rdot'
+ // Inport: '<Root>/wb'
+ // MATLAB Function: '<S6>/MATLAB Function3'
+ // Sum: '<S6>/Add1'
+ // Sum: '<S6>/Add2'
+ // Sum: '<S6>/Add3'
+ // Sum: '<S6>/Add4'
+
+ rtDW.rtb_Delay1_m[0] = rtU.r[0] - rtDW.Integrator_p[0];
+ rtDW.rtb_Delay1_m[3] = rtU.rdot[0] - rtDW.Integrator[0];
+ rtDW.rtb_Delay1_m[6] = rtU.angles[0] - rtDW.Integrator_p[3];
+ rtDW.rtb_Delay1_m[9] = rtU.wb[0] - rtDW.wb[0];
+ rtDW.rtb_Delay1_m[1] = rtU.r[1] - rtDW.Integrator_p[1];
+ rtDW.rtb_Delay1_m[4] = rtU.rdot[1] - rtDW.Integrator[1];
+ rtDW.rtb_Delay1_m[7] = rtU.angles[1] - rtDW.Integrator_p[4];
+ rtDW.rtb_Delay1_m[10] = rtU.wb[1] - rtDW.wb[1];
+ rtDW.rtb_Delay1_m[2] = rtU.r[2] - rtDW.Integrator_p[2];
+ rtDW.rtb_Delay1_m[5] = rtU.rdot[2] - rtDW.Integrator[2];
+ rtDW.rtb_Delay1_m[8] = rtU.angles[2] - rtDW.Integrator_p[5];
+ rtDW.rtb_Delay1_m[11] = rtU.wb[2] - rtDW.wb[2];
+ for (r1 = 0; r1 < 12; r1++) {
+ // MATLAB Function: '<S6>/MATLAB Function3'
+ rtDW.F_c[r1] = 0.0;
+ for (i = 0; i < 12; i++) {
+ rtDW.F_c[r1] += rtDW.F[12 * i + r1] * rtDW.Delay1[i];
+ }
+
+ // Update for Delay: '<S2>/Delay' incorporates:
+ // MATLAB Function: '<S6>/MATLAB Function3'
+ // SignalConversion: '<S9>/TmpSignal ConversionAt SFunction Inport2'
+
+ rtDW.Delay_DSTATE_hf[r1] = rtDW.F_c[r1] * rtDW.rtb_Delay1_m[r1] / (1.0 +
+ rtDW.maxval) + b_0[r1];
+ }
+
+ // End of Outputs for SubSystem: '<S1>/largepaa'
+
+ // Outputs for Atomic SubSystem: '<S1>/small_angle_control'
+ // Sum: '<S22>/Add1' incorporates:
+ // Inport: '<Root>/x'
+
+ rtDW.maxval = rtU.x - rtDW.Integrator_p[0];
+
+ // Gain: '<S144>/Filter Coefficient' incorporates:
+ // DiscreteIntegrator: '<S136>/Filter'
+ // Gain: '<S135>/Derivative Gain'
+ // Sum: '<S136>/SumD'
+
+ Fdes_idx_0 = (1.68326013623818 * rtDW.maxval - rtDW.Filter_DSTATE) *
+ 206.264558996617;
+
+ // Sum: '<S22>/Add2' incorporates:
+ // Inport: '<Root>/y'
+
+ Fdes_idx_1 = rtU.y - rtDW.Integrator_p[1];
+
+ // Gain: '<S276>/Filter Coefficient' incorporates:
+ // DiscreteIntegrator: '<S268>/Filter'
+ // Gain: '<S267>/Derivative Gain'
+ // Sum: '<S268>/SumD'
+
+ Fdes_idx_2 = (1.68326013623818 * Fdes_idx_1 - rtDW.Filter_DSTATE_k) *
+ 206.264558996617;
+
+ // SignalConversion: '<S21>/TmpSignal ConversionAt SFunction Inport1' incorporates:
+ // DiscreteIntegrator: '<S141>/Integrator'
+ // DiscreteIntegrator: '<S273>/Integrator'
+ // Gain: '<S146>/Proportional Gain'
+ // Gain: '<S278>/Proportional Gain'
+ // MATLAB Function: '<S4>/MATLAB Function4'
+ // Sum: '<S150>/Sum'
+ // Sum: '<S282>/Sum'
+
+ rtb_TmpSignalConversionAtSFun_1 = (0.576051138105677 * rtDW.maxval +
+ rtDW.Integrator_DSTATE) + Fdes_idx_0;
+ rtb_TmpSignalConversionAtSFun_0 = (0.576051138105677 * Fdes_idx_1 +
+ rtDW.Integrator_DSTATE_a) + Fdes_idx_2;
+
+ // Sum: '<S23>/Add1' incorporates:
+ // Constant: '<S4>/Constant'
+ // MATLAB Function: '<S4>/MATLAB Function4'
+
+ rtDW.Add1_k = (rtb_TmpSignalConversionAtSFun_1 * 0.0 -
+ rtb_TmpSignalConversionAtSFun_0) * 0.1019367991845056 -
+ rtDW.Integrator_p[3];
+
+ // Gain: '<S188>/Filter Coefficient' incorporates:
+ // DiscreteIntegrator: '<S180>/Filter'
+ // Gain: '<S179>/Derivative Gain'
+ // Sum: '<S180>/SumD'
+
+ rtDW.FilterCoefficient_a = (0.171115754916436 * rtDW.Add1_k -
+ rtDW.Filter_DSTATE_m) * 176.087006843524;
+
+ // Sum: '<S23>/Add2' incorporates:
+ // Constant: '<S4>/Constant'
+ // MATLAB Function: '<S4>/MATLAB Function4'
+
+ rtb_TmpSignalConversionAtSFun_1 = (rtb_TmpSignalConversionAtSFun_0 * 0.0 +
+ rtb_TmpSignalConversionAtSFun_1) * 0.1019367991845056 - rtDW.Integrator_p[4];
+
+ // Gain: '<S56>/Filter Coefficient' incorporates:
+ // DiscreteIntegrator: '<S48>/Filter'
+ // Gain: '<S47>/Derivative Gain'
+ // Sum: '<S48>/SumD'
+
+ rtb_TmpSignalConversionAtSFun_0 = (0.171115754916436 *
+ rtb_TmpSignalConversionAtSFun_1 - rtDW.Filter_DSTATE_l) * 176.087006843524;
+
+ // Gain: '<S100>/Filter Coefficient' incorporates:
+ // Constant: '<S4>/Constant'
+ // DiscreteIntegrator: '<S92>/Filter'
+ // Gain: '<S91>/Derivative Gain'
+ // MATLAB Function: '<S4>/MATLAB Function4'
+ // Sum: '<S23>/Add3'
+ // Sum: '<S92>/SumD'
+
+ rtDW.FilterCoefficient_k = ((0.0 - rtDW.Integrator_p[5]) * 0.0490884878345462
+ - rtDW.Filter_DSTATE_lo) * 270.43836739932;
+
+ // SignalConversion: '<S4>/ConcatBufferAtVector ConcatenateIn2' incorporates:
+ // Constant: '<S4>/Constant'
+ // Delay: '<S1>/Delay'
+ // DiscreteIntegrator: '<S185>/Integrator'
+ // DiscreteIntegrator: '<S53>/Integrator'
+ // DiscreteIntegrator: '<S97>/Integrator'
+ // Gain: '<S102>/Proportional Gain'
+ // Gain: '<S190>/Proportional Gain'
+ // Gain: '<S58>/Proportional Gain'
+ // MATLAB Function: '<S4>/MATLAB Function4'
+ // Sum: '<S106>/Sum'
+ // Sum: '<S194>/Sum'
+ // Sum: '<S23>/Add3'
+ // Sum: '<S62>/Sum'
+
+ rtDW.Delay_DSTATE[1] = (0.421511657975665 * rtDW.Add1_k +
+ rtDW.Integrator_DSTATE_m) + rtDW.FilterCoefficient_a;
+ rtDW.Delay_DSTATE[2] = (0.421511657975665 * rtb_TmpSignalConversionAtSFun_1 +
+ rtDW.Integrator_DSTATE_e) + rtb_TmpSignalConversionAtSFun_0;
+ rtDW.Delay_DSTATE[3] = ((0.0 - rtDW.Integrator_p[5]) * 0.00309186213155581 +
+ rtDW.Integrator_DSTATE_o) + rtDW.FilterCoefficient_k;
+
+ // Sum: '<S22>/Add3' incorporates:
+ // Inport: '<Root>/z'
+
+ rtDW.Add3_a = rtU.z - rtDW.Integrator_p[2];
+
+ // Gain: '<S232>/Filter Coefficient' incorporates:
+ // DiscreteIntegrator: '<S224>/Filter'
+ // Gain: '<S223>/Derivative Gain'
+ // Sum: '<S224>/SumD'
+
+ rtDW.FilterCoefficient_i = (1.46547031640698 * rtDW.Add3_a -
+ rtDW.Filter_DSTATE_n) * 19.9677854964375;
+
+ // Sum: '<S4>/Add' incorporates:
+ // Delay: '<S1>/Delay'
+ // DiscreteIntegrator: '<S229>/Integrator'
+ // Gain: '<S234>/Proportional Gain'
+ // Sum: '<S238>/Sum'
+
+ rtDW.Delay_DSTATE[0] = ((0.547568478085603 * rtDW.Add3_a +
+ rtDW.Integrator_DSTATE_ao) + rtDW.FilterCoefficient_i) + 7.3575;
+
+ // Update for DiscreteIntegrator: '<S141>/Integrator' incorporates:
+ // Gain: '<S138>/Integral Gain'
+
+ rtDW.Integrator_DSTATE += 0.0492289691165195 * rtDW.maxval * 0.0001;
+
+ // Update for DiscreteIntegrator: '<S136>/Filter'
+ rtDW.Filter_DSTATE += 0.0001 * Fdes_idx_0;
+
+ // Update for DiscreteIntegrator: '<S273>/Integrator' incorporates:
+ // Gain: '<S270>/Integral Gain'
+
+ rtDW.Integrator_DSTATE_a += 0.0492289691165195 * Fdes_idx_1 * 0.0001;
+
+ // Update for DiscreteIntegrator: '<S268>/Filter'
+ rtDW.Filter_DSTATE_k += 0.0001 * Fdes_idx_2;
+
+ // Update for DiscreteIntegrator: '<S185>/Integrator' incorporates:
+ // Gain: '<S182>/Integral Gain'
+
+ rtDW.Integrator_DSTATE_m += 0.230387898131499 * rtDW.Add1_k * 0.0001;
+
+ // Update for DiscreteIntegrator: '<S180>/Filter'
+ rtDW.Filter_DSTATE_m += 0.0001 * rtDW.FilterCoefficient_a;
+
+ // Update for DiscreteIntegrator: '<S53>/Integrator' incorporates:
+ // Gain: '<S50>/Integral Gain'
+
+ rtDW.Integrator_DSTATE_e += 0.230387898131499 *
+ rtb_TmpSignalConversionAtSFun_1 * 0.0001;
+
+ // Update for DiscreteIntegrator: '<S48>/Filter'
+ rtDW.Filter_DSTATE_l += 0.0001 * rtb_TmpSignalConversionAtSFun_0;
+
+ // Update for DiscreteIntegrator: '<S97>/Integrator' incorporates:
+ // Constant: '<S4>/Constant'
+ // Gain: '<S94>/Integral Gain'
+ // MATLAB Function: '<S4>/MATLAB Function4'
+ // Sum: '<S23>/Add3'
+
+ rtDW.Integrator_DSTATE_o += (0.0 - rtDW.Integrator_p[5]) * 4.32690066926921E-5
+ * 0.0001;
+
+ // Update for DiscreteIntegrator: '<S92>/Filter'
+ rtDW.Filter_DSTATE_lo += 0.0001 * rtDW.FilterCoefficient_k;
+
+ // Update for DiscreteIntegrator: '<S229>/Integrator' incorporates:
+ // Gain: '<S226>/Integral Gain'
+
+ rtDW.Integrator_DSTATE_ao += 0.0506661578399485 * rtDW.Add3_a * 0.0001;
+
+ // Update for DiscreteIntegrator: '<S224>/Filter'
+ rtDW.Filter_DSTATE_n += 0.0001 * rtDW.FilterCoefficient_i;
+
+ // End of Outputs for SubSystem: '<S1>/small_angle_control'
+ // End of Outputs for SubSystem: '<Root>/Adaptive large angle controller'
+
+ // Outport: '<Root>/u1 '
+ rtY.u1 = rtDW.duty[0];
+
+ // Outport: '<Root>/u2 '
+ rtY.u2 = rtDW.duty[1];
+
+ // Outport: '<Root>/u3'
+ rtY.u3 = rtDW.duty[2];
+
+ // Outport: '<Root>/u4'
+ rtY.u4 = rtDW.duty[3];
+}
+
+// Model initialize function
+void LAAPModelClass::initialize()
+{
+ {
+ int32_T i;
+
+ // SystemInitialize for Atomic SubSystem: '<Root>/Adaptive large angle controller'
+ // SystemInitialize for Atomic SubSystem: '<S1>/quadcopter_dynamics_prediction'
+ // InitializeConditions for DiscreteIntegrator: '<S13>/Integrator'
+ for (i = 0; i < 6; i++) {
+ rtDW.Integrator_DSTATE_ep[i] = 0.0;
+ if (rtDW.Integrator_DSTATE_ep[i] >= 10.0) {
+ rtDW.Integrator_DSTATE_ep[i] = 10.0;
+ } else {
+ if (rtDW.Integrator_DSTATE_ep[i] <= -10.0) {
+ rtDW.Integrator_DSTATE_ep[i] = -10.0;
+ }
+ }
+ }
+
+ // End of InitializeConditions for DiscreteIntegrator: '<S13>/Integrator'
+
+ // InitializeConditions for DiscreteIntegrator: '<S14>/Integrator'
+ rtDW.Integrator_IC_LOADING = 1U;
+
+ // End of SystemInitialize for SubSystem: '<S1>/quadcopter_dynamics_prediction'
+ // End of SystemInitialize for SubSystem: '<Root>/Adaptive large angle controller'
+ }
+}
+
+// Constructor
+LAAPModelClass::LAAPModelClass()
+{
+ // Currently there is no constructor body generated.
+}
+
+// Destructor
+LAAPModelClass::~LAAPModelClass()
+{
+ // Currently there is no destructor body generated.
+}
+
+// Root-level input access methods
+
+// Root inport: '<Root>/rdot' set method
+void LAAPModelClass::setrdot(real_T localArgInput[3])
+{
+ rtU.rdot[0] = localArgInput[0];
+ rtU.rdot[1] = localArgInput[1];
+ rtU.rdot[2] = localArgInput[2];
+}
+
+// Root inport: '<Root>/r' set method
+void LAAPModelClass::setr(real_T localArgInput[3])
+{
+ rtU.r[0] = localArgInput[0];
+ rtU.r[1] = localArgInput[1];
+ rtU.r[2] = localArgInput[2];
+}
+
+// Root inport: '<Root>/wb' set method
+void LAAPModelClass::setwb(real_T localArgInput[3])
+{
+ rtU.wb[0] = localArgInput[0];
+ rtU.wb[1] = localArgInput[1];
+ rtU.wb[2] = localArgInput[2];
+}
+
+// Root inport: '<Root>/angles' set method
+void LAAPModelClass::setangles(real_T localArgInput[3])
+{
+ rtU.angles[0] = localArgInput[0];
+ rtU.angles[1] = localArgInput[1];
+ rtU.angles[2] = localArgInput[2];
+}
+
+// Root inport: '<Root>/x' set method
+void LAAPModelClass::setx(real_T localArgInput)
+{
+ rtU.x = localArgInput;
+}
+
+// Root inport: '<Root>/y' set method
+void LAAPModelClass::sety(real_T localArgInput)
+{
+ rtU.y = localArgInput;
+}
+
+// Root inport: '<Root>/z' set method
+void LAAPModelClass::setz(real_T localArgInput)
+{
+ rtU.z = localArgInput;
+}
+
+// Root inport: '<Root>/phi' set method
+void LAAPModelClass::setphi(real_T localArgInput)
+{
+ rtU.phi = localArgInput;
+}
+
+// Root inport: '<Root>/theta' set method
+void LAAPModelClass::settheta(real_T localArgInput)
+{
+ rtU.theta = localArgInput;
+}
+
+// Root inport: '<Root>/psi' set method
+void LAAPModelClass::setpsi(real_T localArgInput)
+{
+ rtU.psi = localArgInput;
+}
+
+// Root-level output access methods
+
+// Root outport: '<Root>/u1 ' get method
+real_T LAAPModelClass::getu1_() const
+{
+ return rtY.u1;
+}
+
+// Root outport: '<Root>/u2 ' get method
+real_T LAAPModelClass::getu2_() const
+{
+ return rtY.u2;
+}
+
+// Root outport: '<Root>/u3' get method
+real_T LAAPModelClass::getu3() const
+{
+ return rtY.u3;
+}
+
+// Root outport: '<Root>/u4' get method
+real_T LAAPModelClass::getu4() const
+{
+ return rtY.u4;
+}
+
+// Real-Time Model get method
+RT_MODEL * LAAPModelClass::getRTM()
+{
+ return (&rtM);
+}
+
+//
+// File trailer for generated code.
+//
+// [EOF]
+//