File: CONTROL_SYS.c1 /* 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 |