Dragon
Matlab code for quadcopter
Dependents: combination combination
LAAP.cpp
- Committer:
- Glaxys_finest1
- Date:
- 2019-11-25
- Revision:
- 0:25d32fc1c12e
File content as of revision 0:25d32fc1c12e:
//
// 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]
//