File: CONTROL_SYS.c

    1   /*
    2    * File: CONTROL_SYS.c
    3    *
    4    * Code generated for Simulink model 'CONTROL_SYS'.
    5    *
    6    * Model version                  : 1.379
    7    * Simulink Coder version         : 8.11 (R2016b) 25-Aug-2016
    8    * C/C++ source code generated on : Thu Dec 22 12:00:05 2016
    9    *
   10    * Target selection: ert.tlc
   11    * Embedded hardware selection: Intel->x86-64 (Windows64)
   12    * Code generation objectives:
   13    *    1. Traceability
   14    *    2. Execution efficiency
   15    *    3. ROM efficiency
   16    *    4. RAM efficiency
   17    * Validation result: Not run
   18    */
   19   
   20   #include "CONTROL_SYS.h"
   21   #include "CONTROL_SYS_private.h"
   22   
   23   const THE_MEAS_BUS CONTROL_SYS_rtZTHE_MEAS_BUS = { { 0.0, 0.0, 0.0 },/* Vb */
   24     { 0.0, 0.0, 0.0 },                   /* Xe */
   25     { 0.0, 0.0, 0.0 },                   /* phi_theta_psi */
   26     { 0.0, 0.0, 0.0 },                   /* dphi_dtheta_dpsi */
   27     { 0.0, 0.0, 0.0 }                    /* Ve */
   28   };
   29   
   30   /* Output and update for atomic system: '<S2>/PITCH_and_ROLL_CONT' */
   31   void CONTROL_SYS_PITCH_and_ROLL_CONT(const real_T rtu_XY_CMD[2], const
   32     THE_MEAS_BUS *rtu_MEAS_BUS, B_PITCH_and_ROLL_CONT_CONTROL_T *localB)
   33   {
   34     real_T rtu_XY_CMD_idx_0;
   35     real_T rtu_XY_CMD_idx_1;
   36     real_T u0;
   37   
   38     /* Sum: '<S5>/Sum1' incorporates:
   39      *  MATLAB Function: '<S5>/MATLAB Function'
   40      */
   41     /* MATLAB Function 'CONTROL_SYS/CONT_SYS FM_CMDS/PITCH_and_ROLL_CONT/MATLAB Function': '<S11>:1' */
   42     /* '<S11>:1:4' bRg = [  cos(ang),  sin(ang); */
   43     /* '<S11>:1:5'             -sin(ang),  cos(ang); */
   44     /* '<S11>:1:6'           ]; */
   45     /* '<S11>:1:8' Qr = bRg * Gr; */
   46     rtu_XY_CMD_idx_0 = rtu_XY_CMD[0] - rtu_MEAS_BUS->Xe[0];
   47     rtu_XY_CMD_idx_1 = rtu_XY_CMD[1] - rtu_MEAS_BUS->Xe[1];
   48   
   49     /* Gain: '<S16>/Gain1' incorporates:
   50      *  Gain: '<S17>/Gain'
   51      *  Gain: '<S5>/Gain'
   52      *  Gain: '<S5>/Gain1'
   53      *  MATLAB Function: '<S5>/MATLAB Function'
   54      *  Sum: '<S5>/Sum2'
   55      */
   56     u0 = ((cos(rtu_MEAS_BUS->phi_theta_psi[0]) * rtu_XY_CMD_idx_0 + sin
   57            (rtu_MEAS_BUS->phi_theta_psi[0]) * rtu_XY_CMD_idx_1) * 0.68 -
   58           rtu_MEAS_BUS->Vb[0]) * 0.16428571428571431 * 57.295779513082323;
   59     rtu_XY_CMD_idx_0 = ((-sin(rtu_MEAS_BUS->phi_theta_psi[0]) * rtu_XY_CMD_idx_0 +
   60                          cos(rtu_MEAS_BUS->phi_theta_psi[0]) * rtu_XY_CMD_idx_1) *
   61                         0.68 - rtu_MEAS_BUS->Vb[1]) * 0.16428571428571431 *
   62       57.295779513082323;
   63     if (rtu_XY_CMD_idx_0 > 59.647137433995226) {
   64       rtu_XY_CMD_idx_0 = 59.647137433995226;
   65     } else {
   66       if (rtu_XY_CMD_idx_0 < -59.647137433995226) {
   67         rtu_XY_CMD_idx_0 = -59.647137433995226;
   68       }
   69     }
   70   
   71     /* Gain: '<S5>/Gain3' incorporates:
   72      *  Gain: '<S16>/Gain1'
   73      *  Gain: '<S20>/Proportional Gain'
   74      *  Gain: '<S21>/Proportional Gain'
   75      *  Gain: '<S5>/Gain2'
   76      *  Sum: '<S10>/Sum1'
   77      *  Sum: '<S10>/Sum3'
   78      */
   79     localB->Gain3 = ((-(0.017453292519943295 * rtu_XY_CMD_idx_0) -
   80                       rtu_MEAS_BUS->phi_theta_psi[2]) * 3.111442907298 -
   81                      rtu_MEAS_BUS->dphi_dtheta_dpsi[2]) * 0.050794101555223;
   82   
   83     /* Gain: '<S16>/Gain1' */
   84     if (u0 > 59.647137433995226) {
   85       u0 = 59.647137433995226;
   86     } else {
   87       if (u0 < -59.647137433995226) {
   88         u0 = -59.647137433995226;
   89       }
   90     }
   91   
   92     /* Gain: '<S5>/Gain4' incorporates:
   93      *  Gain: '<S16>/Gain1'
   94      *  Gain: '<S18>/Proportional Gain'
   95      *  Gain: '<S19>/Proportional Gain'
   96      *  Sum: '<S9>/Sum1'
   97      *  Sum: '<S9>/Sum3'
   98      */
   99     localB->Gain4 = ((0.017453292519943295 * u0 - rtu_MEAS_BUS->phi_theta_psi[1]) *
  100                      3.111442907298 - rtu_MEAS_BUS->dphi_dtheta_dpsi[1]) *
  101       0.050794101555223;
  102   }
  103   
  104   /* Output and update for atomic system: '<S2>/YAW_CONT' */
  105   void CONTROL_SYS_YAW_CONT(real_T rtu_Yaw_CMD, const THE_MEAS_BUS *rtu_MEAS_BUS,
  106     B_YAW_CONT_CONTROL_SYS_T *localB)
  107   {
  108     /* Gain: '<S27>/Proportional Gain' incorporates:
  109      *  Gain: '<S26>/Proportional Gain'
  110      *  Sum: '<S25>/Sum1'
  111      *  Sum: '<S25>/Sum3'
  112      */
  113     localB->ProportionalGain = ((rtu_Yaw_CMD - rtu_MEAS_BUS->phi_theta_psi[0]) *
  114       1.61636567778084 - rtu_MEAS_BUS->dphi_dtheta_dpsi[0]) * 0.09744859506471;
  115   }
  116   
  117   /* Output and update for atomic system: '<S2>/Z_CONT' */
  118   void CONTROL_SYS_Z_CONT(real_T rtu_Ze_CMD, const THE_MEAS_BUS *rtu_MEAS_BUS,
  119     B_Z_CONT_CONTROL_SYS_T *localB)
  120   {
  121     /* Gain: '<S30>/Proportional Gain' incorporates:
  122      *  Gain: '<S31>/Proportional Gain'
  123      *  Sum: '<S28>/Sum1'
  124      *  Sum: '<S28>/Sum3'
  125      */
  126     localB->ProportionalGain = ((rtu_Ze_CMD - rtu_MEAS_BUS->Xe[2]) *
  127       0.931782439164433 - rtu_MEAS_BUS->Ve[2]) * 6.7862699178667;
  128   }
  129   
  130   /*
  131    * Output and update for atomic system:
  132    *    '<S33>/MOTOR_1_CONTROL'
  133    *    '<S33>/MOTOR_2_CONTROL'
  134    *    '<S33>/MOTOR_3_CONTROL'
  135    *    '<S33>/MOTOR_4_CONTROL'
  136    */
  137   void CONTROL_SYS_MOTOR_1_CONTROL(real_T rtu_w_CMD, real_T rtu_w_actual,
  138     B_MOTOR_1_CONTROL_CONTROL_SYS_T *localB, DW_MOTOR_1_CONTROL_CONTROL_SY_T
  139     *localDW)
  140   {
  141     real_T rtb_Sum1_j;
  142     real_T u0;
  143   
  144     /* Sum: '<S37>/Sum1' */
  145     rtb_Sum1_j = rtu_w_CMD - rtu_w_actual;
  146   
  147     /* Sum: '<S43>/Sum' incorporates:
  148      *  DiscreteIntegrator: '<S43>/Integrator'
  149      *  Gain: '<S43>/Proportional Gain'
  150      */
  151     u0 = 0.0225173510477141 * rtb_Sum1_j + localDW->Integrator_DSTATE;
  152   
  153     /* Saturate: '<S37>/VOLTAGE Saturation' */
  154     if (u0 > 9.0) {
  155       localB->VOLTS = 9.0;
  156     } else if (u0 < 0.0) {
  157       localB->VOLTS = 0.0;
  158     } else {
  159       localB->VOLTS = u0;
  160     }
  161   
  162     /* End of Saturate: '<S37>/VOLTAGE Saturation' */
  163   
  164     /* Update for DiscreteIntegrator: '<S43>/Integrator' incorporates:
  165      *  Gain: '<S43>/Integral Gain'
  166      */
  167     localDW->Integrator_DSTATE += 0.0872547217562916 * rtb_Sum1_j * 0.02;
  168   }
  169   
  170   /* Model step function */
  171   void CONTROL_SYS_step(RT_MODEL_CONTROL_SYS_T *const CONTROL_SYS_M, THE_MEAS_BUS *
  172                         CONTROL_SYS_U_MEAS_BUS, real_T CONTROL_SYS_U_Ze_cmd,
  173                         real_T CONTROL_SYS_U_XY_cmd[2], real_T
  174                         CONTROL_SYS_U_YAW_cmd, real_T CONTROL_SYS_U_w_mot_UN[4],
  175                         real_T CONTROL_SYS_Y_VOLTS[4], real_T
  176                         CONTROL_SYS_Y_FM_CMD_VEC[4])
  177   {
  178     B_CONTROL_SYS_T *CONTROL_SYS_B = ((B_CONTROL_SYS_T *) CONTROL_SYS_M->blockIO);
  179     DW_CONTROL_SYS_T *CONTROL_SYS_DW = ((DW_CONTROL_SYS_T *) CONTROL_SYS_M->dwork);
  180     real_T rtb_Saturation;
  181     real_T rtb_Saturation3;
  182     real_T rtb_Saturation2;
  183     real_T rtb_Saturation1;
  184     real_T rtb_w2_cmd[4];
  185     real_T tmp[16];
  186     int32_T i;
  187     real_T rtb_w2_cmd_e;
  188   
  189     /* Outputs for Atomic SubSystem: '<S2>/Z_CONT' */
  190   
  191     /* Inport: '<Root>/Ze_cmd' incorporates:
  192      *  Inport: '<Root>/MEAS_BUS'
  193      */
  194     CONTROL_SYS_Z_CONT(CONTROL_SYS_U_Ze_cmd, CONTROL_SYS_U_MEAS_BUS,
  195                        &CONTROL_SYS_B->Z_CONT);
  196   
  197     /* End of Outputs for SubSystem: '<S2>/Z_CONT' */
  198   
  199     /* Sum: '<S2>/Sum2' */
  200     rtb_Saturation = CONTROL_SYS_ConstB.Gain +
  201       CONTROL_SYS_B->Z_CONT.ProportionalGain;
  202   
  203     /* Saturate: '<S4>/Saturation' */
  204     if (rtb_Saturation > 18.0) {
  205       rtb_Saturation = 18.0;
  206     } else {
  207       if (rtb_Saturation < 0.0) {
  208         rtb_Saturation = 0.0;
  209       }
  210     }
  211   
  212     /* End of Saturate: '<S4>/Saturation' */
  213   
  214     /* Outputs for Atomic SubSystem: '<S2>/PITCH_and_ROLL_CONT' */
  215   
  216     /* Inport: '<Root>/XY_cmd' incorporates:
  217      *  Inport: '<Root>/MEAS_BUS'
  218      */
  219     CONTROL_SYS_PITCH_and_ROLL_CONT(CONTROL_SYS_U_XY_cmd, CONTROL_SYS_U_MEAS_BUS,
  220       &CONTROL_SYS_B->PITCH_and_ROLL_CONT);
  221   
  222     /* End of Outputs for SubSystem: '<S2>/PITCH_and_ROLL_CONT' */
  223   
  224     /* Saturate: '<S4>/Saturation3' */
  225     if (CONTROL_SYS_B->PITCH_and_ROLL_CONT.Gain3 > 0.9) {
  226       rtb_Saturation3 = 0.9;
  227     } else if (CONTROL_SYS_B->PITCH_and_ROLL_CONT.Gain3 < -0.9) {
  228       rtb_Saturation3 = -0.9;
  229     } else {
  230       rtb_Saturation3 = CONTROL_SYS_B->PITCH_and_ROLL_CONT.Gain3;
  231     }
  232   
  233     /* End of Saturate: '<S4>/Saturation3' */
  234   
  235     /* Saturate: '<S4>/Saturation2' */
  236     if (CONTROL_SYS_B->PITCH_and_ROLL_CONT.Gain4 > 0.9) {
  237       rtb_Saturation2 = 0.9;
  238     } else if (CONTROL_SYS_B->PITCH_and_ROLL_CONT.Gain4 < -0.9) {
  239       rtb_Saturation2 = -0.9;
  240     } else {
  241       rtb_Saturation2 = CONTROL_SYS_B->PITCH_and_ROLL_CONT.Gain4;
  242     }
  243   
  244     /* End of Saturate: '<S4>/Saturation2' */
  245   
  246     /* Outputs for Atomic SubSystem: '<S2>/YAW_CONT' */
  247   
  248     /* Inport: '<Root>/YAW_cmd' incorporates:
  249      *  Inport: '<Root>/MEAS_BUS'
  250      */
  251     CONTROL_SYS_YAW_CONT(CONTROL_SYS_U_YAW_cmd, CONTROL_SYS_U_MEAS_BUS,
  252                          &CONTROL_SYS_B->YAW_CONT);
  253   
  254     /* End of Outputs for SubSystem: '<S2>/YAW_CONT' */
  255   
  256     /* Saturate: '<S4>/Saturation1' */
  257     if (CONTROL_SYS_B->YAW_CONT.ProportionalGain > 0.12000000000000001) {
  258       rtb_Saturation1 = 0.12000000000000001;
  259     } else if (CONTROL_SYS_B->YAW_CONT.ProportionalGain < -0.12000000000000001) {
  260       rtb_Saturation1 = -0.12000000000000001;
  261     } else {
  262       rtb_Saturation1 = CONTROL_SYS_B->YAW_CONT.ProportionalGain;
  263     }
  264   
  265     /* End of Saturate: '<S4>/Saturation1' */
  266   
  267     /* MATLAB Function: '<S3>/FM_to_w2' incorporates:
  268      *  Constant: '<S3>/Constant'
  269      *  Constant: '<S3>/Constant1'
  270      *  Constant: '<S3>/Constant2'
  271      */
  272     /*  simplify the variable names */
  273     /* MATLAB Function 'CONTROL_SYS/CONT_SYS MOTOR_CMDS/FM_to_w2': '<S32>:1' */
  274     /* '<S32>:1:5' b = Thrust_const; */
  275     /* '<S32>:1:6' k = Twist_const; */
  276     /*  construct the co-efficient matrix: */
  277     /*   F_and_M = C * w^2 */
  278     /*  */
  279     /*  C = [     b,    b,    b,    b; */
  280     /*            0,    b*L,  0,   -b*L; */
  281     /*           -b*L,  0,    b*L,  0; */
  282     /*           -k,    k,   -k,    k; ]; */
  283     /*  used Symbolic toolbox to compute inv(C) */
  284     /* '<S32>:1:17' C_inv = [ */
  285     /* '<S32>:1:18'              1/(4*b),          0, -1/(2*L*b), -1/(4*k); */
  286     /* '<S32>:1:19'              1/(4*b),  1/(2*L*b),          0,  1/(4*k); */
  287     /* '<S32>:1:20'              1/(4*b),          0,  1/(2*L*b), -1/(4*k); */
  288     /* '<S32>:1:21'              1/(4*b), -1/(2*L*b),          0,  1/(4*k); */
  289     /* '<S32>:1:22'             ]; */
  290     /*  solve for the w^2 vec */
  291     /* '<S32>:1:24' F_and_M_col = [Thrust, TQ_roll_X, TQ_pit_Y, TQ_yaw_Z].' ; */
  292     /* '<S32>:1:26' w2_cmd = C_inv * F_and_M_col; */
  293     tmp[0] = 27777.777777777777;
  294     tmp[4] = 0.0;
  295     tmp[8] = -277777.77777777775;
  296     tmp[12] = -2.0833333333333335E+6;
  297     tmp[1] = 27777.777777777777;
  298     tmp[5] = 277777.77777777775;
  299     tmp[9] = 0.0;
  300     tmp[13] = 2.0833333333333335E+6;
  301     tmp[2] = 27777.777777777777;
  302     tmp[6] = 0.0;
  303     tmp[10] = 277777.77777777775;
  304     tmp[14] = -2.0833333333333335E+6;
  305     tmp[3] = 27777.777777777777;
  306     tmp[7] = -277777.77777777775;
  307     tmp[11] = 0.0;
  308     tmp[15] = 2.0833333333333335E+6;
  309   
  310     /* '<S32>:1:28' tf_col = (w2_cmd >= 0); */
  311     /* assert(all(w2_cmd), 'ERROR: some w2 are NEGATIVE !') */
  312     /* assert(false, 'ERROR: some w2 are NEGATIVE !') */
  313     for (i = 0; i < 4; i++) {
  314       rtb_w2_cmd_e = tmp[i + 12] * rtb_Saturation1 + (tmp[i + 8] * rtb_Saturation2
  315         + (tmp[i + 4] * rtb_Saturation3 + tmp[i] * rtb_Saturation));
  316   
  317       /* Sqrt: '<S3>/Sqrt' incorporates:
  318        *  Constant: '<S47>/Constant'
  319        *  DataTypeConversion: '<S34>/Data Type Conversion'
  320        *  Product: '<S34>/Product'
  321        *  RelationalOperator: '<S47>/Compare'
  322        */
  323       rtb_w2_cmd_e = sqrt((real_T)(rtb_w2_cmd_e >= 0.0) * rtb_w2_cmd_e);
  324       rtb_w2_cmd[i] = rtb_w2_cmd_e;
  325     }
  326   
  327     /* End of MATLAB Function: '<S3>/FM_to_w2' */
  328   
  329     /* Outputs for Atomic SubSystem: '<S33>/MOTOR_1_CONTROL' */
  330   
  331     /* Inport: '<Root>/w_mot_UN' */
  332     CONTROL_SYS_MOTOR_1_CONTROL(rtb_w2_cmd[0], CONTROL_SYS_U_w_mot_UN[0],
  333       &CONTROL_SYS_B->MOTOR_1_CONTROL, &CONTROL_SYS_DW->MOTOR_1_CONTROL);
  334   
  335     /* End of Outputs for SubSystem: '<S33>/MOTOR_1_CONTROL' */
  336   
  337     /* Outputs for Atomic SubSystem: '<S33>/MOTOR_2_CONTROL' */
  338     CONTROL_SYS_MOTOR_1_CONTROL(rtb_w2_cmd[1], CONTROL_SYS_U_w_mot_UN[1],
  339       &CONTROL_SYS_B->MOTOR_2_CONTROL, &CONTROL_SYS_DW->MOTOR_2_CONTROL);
  340   
  341     /* End of Outputs for SubSystem: '<S33>/MOTOR_2_CONTROL' */
  342   
  343     /* Outputs for Atomic SubSystem: '<S33>/MOTOR_3_CONTROL' */
  344     CONTROL_SYS_MOTOR_1_CONTROL(rtb_w2_cmd[2], CONTROL_SYS_U_w_mot_UN[2],
  345       &CONTROL_SYS_B->MOTOR_3_CONTROL, &CONTROL_SYS_DW->MOTOR_3_CONTROL);
  346   
  347     /* End of Outputs for SubSystem: '<S33>/MOTOR_3_CONTROL' */
  348   
  349     /* Outputs for Atomic SubSystem: '<S33>/MOTOR_4_CONTROL' */
  350     CONTROL_SYS_MOTOR_1_CONTROL(rtb_w2_cmd[3], CONTROL_SYS_U_w_mot_UN[3],
  351       &CONTROL_SYS_B->MOTOR_4_CONTROL, &CONTROL_SYS_DW->MOTOR_4_CONTROL);
  352   
  353     /* End of Outputs for SubSystem: '<S33>/MOTOR_4_CONTROL' */
  354   
  355     /* Outport: '<Root>/VOLTS' */
  356     CONTROL_SYS_Y_VOLTS[0] = CONTROL_SYS_B->MOTOR_1_CONTROL.VOLTS;
  357     CONTROL_SYS_Y_VOLTS[1] = CONTROL_SYS_B->MOTOR_2_CONTROL.VOLTS;
  358     CONTROL_SYS_Y_VOLTS[2] = CONTROL_SYS_B->MOTOR_3_CONTROL.VOLTS;
  359     CONTROL_SYS_Y_VOLTS[3] = CONTROL_SYS_B->MOTOR_4_CONTROL.VOLTS;
  360   
  361     /* Outport: '<Root>/FM_CMD_VEC' */
  362     CONTROL_SYS_Y_FM_CMD_VEC[0] = rtb_Saturation;
  363     CONTROL_SYS_Y_FM_CMD_VEC[1] = rtb_Saturation3;
  364     CONTROL_SYS_Y_FM_CMD_VEC[2] = rtb_Saturation2;
  365     CONTROL_SYS_Y_FM_CMD_VEC[3] = rtb_Saturation1;
  366   }
  367   
  368   /* Model initialize function */
  369   void CONTROL_SYS_initialize(RT_MODEL_CONTROL_SYS_T *const CONTROL_SYS_M,
  370     THE_MEAS_BUS *CONTROL_SYS_U_MEAS_BUS, real_T *CONTROL_SYS_U_Ze_cmd, real_T
  371     CONTROL_SYS_U_XY_cmd[2], real_T *CONTROL_SYS_U_YAW_cmd, real_T
  372     CONTROL_SYS_U_w_mot_UN[4], real_T CONTROL_SYS_Y_VOLTS[4], real_T
  373     CONTROL_SYS_Y_FM_CMD_VEC[4])
  374   {
  375     B_CONTROL_SYS_T *CONTROL_SYS_B = ((B_CONTROL_SYS_T *) CONTROL_SYS_M->blockIO);
  376     DW_CONTROL_SYS_T *CONTROL_SYS_DW = ((DW_CONTROL_SYS_T *) CONTROL_SYS_M->dwork);
  377   
  378     /* Registration code */
  379   
  380     /* block I/O */
  381     (void) memset(((void *) CONTROL_SYS_B), 0,
  382                   sizeof(B_CONTROL_SYS_T));
  383   
  384     /* states (dwork) */
  385     (void) memset((void *)CONTROL_SYS_DW, 0,
  386                   sizeof(DW_CONTROL_SYS_T));
  387   
  388     /* external inputs */
  389     (void)memset(&CONTROL_SYS_U_w_mot_UN[0], 0, sizeof(real_T) << 2U);
  390     (void)memset(&CONTROL_SYS_U_XY_cmd[0], 0, sizeof(real_T) << 1U);
  391     *CONTROL_SYS_U_MEAS_BUS = CONTROL_SYS_rtZTHE_MEAS_BUS;
  392     *CONTROL_SYS_U_Ze_cmd = 0.0;
  393     *CONTROL_SYS_U_YAW_cmd = 0.0;
  394   
  395     /* external outputs */
  396     (void) memset(&CONTROL_SYS_Y_VOLTS[0], 0,
  397                   4U*sizeof(real_T));
  398     (void) memset(&CONTROL_SYS_Y_FM_CMD_VEC[0], 0,
  399                   4U*sizeof(real_T));
  400   }
  401   
  402   /* Model terminate function */
  403   void CONTROL_SYS_terminate(RT_MODEL_CONTROL_SYS_T *const CONTROL_SYS_M)
  404   {
  405     /* (no terminate code required) */
  406     UNUSED_PARAMETER(CONTROL_SYS_M);
  407   }
  408   
  409   /*
  410    * File trailer for generated code.
  411    *
  412    * [EOF]
  413    */
  414