/* This function updates continuous states using the ODE4 fixed-step * solver algorithm */ static void rt_ertODEUpdateContinuousStates(RTWSolverInfo *si , int_T tid) { time_T t = rtsiGetT(si); time_T tnew = rtsiGetSolverStopTime(si); time_T h = rtsiGetStepSize(si); real_T *x = rtsiGetContStates(si); ODE4_IntgData *id = rtsiGetSolverData(si); real_T *y = id->y; real_T *f0 = id->f[0]; real_T *f1 = id->f[1]; real_T *f2 = id->f[2]; real_T *f3 = id->f[3]; real_T temp; int_T i; int_T nXc = 1; rtsiSetSimTimeStep(si,MINOR_TIME_STEP); /* Save the state values at time t in y, we'll use x as ynew. */ (void)memcpy(y, x, nXc*sizeof(real_T)); /* Assumes that rtsiSetT and ModelOutputs are up-to-date */ /* f0 = f(t,y) */ rtsiSetdX(si, f0); m1006_derivatives(); /* f1 = f(t + (h/2), y + (h/2)*f0) */ temp = 0.5 * h; for (i = 0; i < nXc; i++) x[i] = y[i] + (temp*f0[i]); rtsiSetT(si, t + temp); rtsiSetdX(si, f1); m1006_output(0); m1006_derivatives(); /* f2 = f(t + (h/2), y + (h/2)*f1) */ for (i = 0; i < nXc; i++) x[i] = y[i] + (temp*f1[i]); rtsiSetdX(si, f2); m1006_output(0); m1006_derivatives(); /* f3 = f(t + h, y + h*f2) */ for (i = 0; i < nXc; i++) x[i] = y[i] + (h*f2[i]); rtsiSetT(si, tnew); rtsiSetdX(si, f3); m1006_output(0); m1006_derivatives(); /* tnew = t + h ynew = y + (h/6)*(f0 + 2*f1 + 2*f2 + 2*f3) */ temp = h / 6.0; for (i = 0; i < nXc; i++) { x[i] = y[i] + temp*(f0[i] + 2.0*f1[i] + 2.0*f2[i] + f3[i]); } rtsiSetSimTimeStep(si,MAJOR_TIME_STEP); }
/* Model update function */ void xpcosc_update(int_T tid) { if (rtmIsMajorTimeStep(xpcosc_rtM)) { rt_ertODEUpdateContinuousStates(&xpcosc_rtM->solverInfo); } /* Update absolute time for base rate */ /* The "clockTick0" counts the number of times the code of this task has * been executed. The absolute time is the multiplication of "clockTick0" * and "Timing.stepSize0". Size of "clockTick0" ensures timer will not * overflow during the application lifespan selected. * Timer of this task consists of two 32 bit unsigned integers. * The two integers represent the low bits Timing.clockTick0 and the high bits * Timing.clockTickH0. When the low bit overflows to 0, the high bits increment. */ if (!(++xpcosc_rtM->Timing.clockTick0)) { ++xpcosc_rtM->Timing.clockTickH0; } xpcosc_rtM->Timing.t[0] = rtsiGetSolverStopTime(&xpcosc_rtM->solverInfo); { /* Update absolute timer for sample time: [0.001s, 0.0s] */ /* The "clockTick1" counts the number of times the code of this task has * been executed. The absolute time is the multiplication of "clockTick1" * and "Timing.stepSize1". Size of "clockTick1" ensures timer will not * overflow during the application lifespan selected. * Timer of this task consists of two 32 bit unsigned integers. * The two integers represent the low bits Timing.clockTick1 and the high bits * Timing.clockTickH1. When the low bit overflows to 0, the high bits increment. */ if (!(++xpcosc_rtM->Timing.clockTick1)) { ++xpcosc_rtM->Timing.clockTickH1; } xpcosc_rtM->Timing.t[1] = xpcosc_rtM->Timing.clockTick1 * xpcosc_rtM->Timing.stepSize1 + xpcosc_rtM->Timing.clockTickH1 * xpcosc_rtM->Timing.stepSize1 * 4294967296.0; } /* tid is required for a uniform function interface. * Argument tid is not used in the function. */ UNUSED_PARAMETER(tid); }
/* * This function updates continuous states using the ODE2 fixed-step * solver algorithm */ void Position_TiltModelClass::rt_ertODEUpdateContinuousStates(RTWSolverInfo *si ) { time_T tnew = rtsiGetSolverStopTime(si); time_T h = rtsiGetStepSize(si); real_T *x = rtsiGetContStates(si); ODE2_IntgData *id = (ODE2_IntgData *)rtsiGetSolverData(si); real_T *y = id->y; real_T *f0 = id->f[0]; real_T *f1 = id->f[1]; real_T temp; int_T i; int_T nXc = 2; rtsiSetSimTimeStep(si,MINOR_TIME_STEP); /* Save the state values at time t in y, we'll use x as ynew. */ (void) memcpy(y, x, (uint_T)nXc*sizeof(real_T)); /* Assumes that rtsiSetT and ModelOutputs are up-to-date */ /* f0 = f(t,y) */ rtsiSetdX(si, f0); Position_Tilt_derivatives(); /* f1 = f(t + h, y + h*f0) */ for (i = 0; i < nXc; i++) { x[i] = y[i] + (h*f0[i]); } rtsiSetT(si, tnew); rtsiSetdX(si, f1); this->step(); Position_Tilt_derivatives(); /* tnew = t + h ynew = y + (h/2)*(f0 + f1) */ temp = 0.5*h; for (i = 0; i < nXc; i++) { x[i] = y[i] + temp*(f0[i] + f1[i]); } rtsiSetSimTimeStep(si,MAJOR_TIME_STEP); }
/* * This function updates continuous states using the ODE1 fixed-step * solver algorithm */ static void rt_ertODEUpdateContinuousStates(RTWSolverInfo *si ) { time_T tnew = rtsiGetSolverStopTime(si); time_T h = rtsiGetStepSize(si); real_T *x = rtsiGetContStates(si); ODE1_IntgData *id = (ODE1_IntgData *)rtsiGetSolverData(si); real_T *f0 = id->f[0]; int_T i; int_T nXc = 2; rtsiSetSimTimeStep(si,MINOR_TIME_STEP); rtsiSetdX(si, f0); motor_io_position_derivatives(); rtsiSetT(si, tnew); for (i = 0; i < nXc; i++) { *x += h * f0[i]; x++; } rtsiSetSimTimeStep(si,MAJOR_TIME_STEP); }
/* Model update function */ void motor_io_update(void) { if (rtmIsMajorTimeStep(motor_io_M)) { rt_ertODEUpdateContinuousStates(&motor_io_M->solverInfo); } /* Update absolute time for base rate */ /* The "clockTick0" counts the number of times the code of this task has * been executed. The absolute time is the multiplication of "clockTick0" * and "Timing.stepSize0". Size of "clockTick0" ensures timer will not * overflow during the application lifespan selected. * Timer of this task consists of two 32 bit unsigned integers. * The two integers represent the low bits Timing.clockTick0 and the high bits * Timing.clockTickH0. When the low bit overflows to 0, the high bits increment. */ if (!(++motor_io_M->Timing.clockTick0)) { ++motor_io_M->Timing.clockTickH0; } motor_io_M->Timing.t[0] = rtsiGetSolverStopTime(&motor_io_M->solverInfo); { /* Update absolute timer for sample time: [0.01s, 0.0s] */ /* The "clockTick1" counts the number of times the code of this task has * been executed. The absolute time is the multiplication of "clockTick1" * and "Timing.stepSize1". Size of "clockTick1" ensures timer will not * overflow during the application lifespan selected. * Timer of this task consists of two 32 bit unsigned integers. * The two integers represent the low bits Timing.clockTick1 and the high bits * Timing.clockTickH1. When the low bit overflows to 0, the high bits increment. */ if (!(++motor_io_M->Timing.clockTick1)) { ++motor_io_M->Timing.clockTickH1; } motor_io_M->Timing.t[1] = motor_io_M->Timing.clockTick1 * motor_io_M->Timing.stepSize1 + motor_io_M->Timing.clockTickH1 * motor_io_M->Timing.stepSize1 * 4294967296.0; } }
/* * This function updates continuous states using the ODE3 fixed-step * solver algorithm */ static void rt_ertODEUpdateContinuousStates(RTWSolverInfo *si ) { /* Solver Matrices */ static const real_T rt_ODE3_A[3] = { 1.0/2.0, 3.0/4.0, 1.0 }; static const real_T rt_ODE3_B[3][3] = { { 1.0/2.0, 0.0, 0.0 }, { 0.0, 3.0/4.0, 0.0 }, { 2.0/9.0, 1.0/3.0, 4.0/9.0 } }; time_T t = rtsiGetT(si); time_T tnew = rtsiGetSolverStopTime(si); time_T h = rtsiGetStepSize(si); real_T *x = rtsiGetContStates(si); ODE3_IntgData *id = (ODE3_IntgData *)rtsiGetSolverData(si); real_T *y = id->y; real_T *f0 = id->f[0]; real_T *f1 = id->f[1]; real_T *f2 = id->f[2]; real_T hB[3]; int_T i; int_T nXc = 4; rtsiSetSimTimeStep(si,MINOR_TIME_STEP); /* Save the state values at time t in y, we'll use x as ynew. */ (void) memcpy(y, x, (uint_T)nXc*sizeof(real_T)); /* Assumes that rtsiSetT and ModelOutputs are up-to-date */ /* f0 = f(t,y) */ rtsiSetdX(si, f0); trajectoryModel_derivatives(); /* f(:,2) = feval(odefile, t + hA(1), y + f*hB(:,1), args(:)(*)); */ hB[0] = h * rt_ODE3_B[0][0]; for (i = 0; i < nXc; i++) { x[i] = y[i] + (f0[i]*hB[0]); } rtsiSetT(si, t + h*rt_ODE3_A[0]); rtsiSetdX(si, f1); trajectoryModel_step(); trajectoryModel_derivatives(); /* f(:,3) = feval(odefile, t + hA(2), y + f*hB(:,2), args(:)(*)); */ for (i = 0; i <= 1; i++) { hB[i] = h * rt_ODE3_B[1][i]; } for (i = 0; i < nXc; i++) { x[i] = y[i] + (f0[i]*hB[0] + f1[i]*hB[1]); } rtsiSetT(si, t + h*rt_ODE3_A[1]); rtsiSetdX(si, f2); trajectoryModel_step(); trajectoryModel_derivatives(); /* tnew = t + hA(3); ynew = y + f*hB(:,3); */ for (i = 0; i <= 2; i++) { hB[i] = h * rt_ODE3_B[2][i]; } for (i = 0; i < nXc; i++) { x[i] = y[i] + (f0[i]*hB[0] + f1[i]*hB[1] + f2[i]*hB[2]); } rtsiSetT(si, tnew); rtsiSetSimTimeStep(si,MAJOR_TIME_STEP); }
/* Model step function */ void trajectoryModel_step(void) { /* local block i/o variables */ real_T rtb_Sqrt; real_T rtb_Divide1; int8_T rtAction; real_T rtb_Divide; real_T rtb_Add1; if (rtmIsMajorTimeStep(trajectoryModel_M)) { /* set solver stop time */ if (!(trajectoryModel_M->Timing.clockTick0+1)) { rtsiSetSolverStopTime(&trajectoryModel_M->solverInfo, ((trajectoryModel_M->Timing.clockTickH0 + 1) * trajectoryModel_M->Timing.stepSize0 * 4294967296.0)); } else { rtsiSetSolverStopTime(&trajectoryModel_M->solverInfo, ((trajectoryModel_M->Timing.clockTick0 + 1) * trajectoryModel_M->Timing.stepSize0 + trajectoryModel_M->Timing.clockTickH0 * trajectoryModel_M->Timing.stepSize0 * 4294967296.0)); } } /* end MajorTimeStep */ /* Update absolute time of base rate at minor time step */ if (rtmIsMinorTimeStep(trajectoryModel_M)) { trajectoryModel_M->Timing.t[0] = rtsiGetT(&trajectoryModel_M->solverInfo); } /* Integrator: '<Root>/x' */ trajectoryModel_B.x = trajectoryModel_X.x_CSTATE; /* Sum: '<S3>/x-Planet_x' incorporates: * Constant: '<Root>/0 ' */ rtb_Divide1 = trajectoryModel_B.x - trajectoryModel_P._Value; /* Integrator: '<Root>/y ' */ trajectoryModel_B.y = trajectoryModel_X.y_CSTATE; /* Sqrt: '<S3>/Sqrt' incorporates: * Product: '<S3>/(x-Planet_x)^2' * Product: '<S3>/y^2' * Sum: '<S3>/(x-Planet_x)^2+y^2' */ rtb_Sqrt = sqrt(rtb_Divide1 * rtb_Divide1 + trajectoryModel_B.y * trajectoryModel_B.y); /* If: '<Root>/If' incorporates: * Constant: '<Root>/stopRadius' * Constant: '<Root>/terminate' */ if (rtmIsMajorTimeStep(trajectoryModel_M)) { if (rtb_Sqrt < trajectoryModel_P.stopRadius) { rtAction = 0; } else { rtAction = 1; } trajectoryModel_DW.If_ActiveSubsystem = rtAction; } else { rtAction = trajectoryModel_DW.If_ActiveSubsystem; } switch (rtAction) { case 0: /* Outputs for IfAction SubSystem: '<Root>/If Action Subsystem' incorporates: * ActionPort: '<S1>/Action Port' */ trajectoryMod_IfActionSubsystem(rtb_Sqrt, &trajectoryModel_B.IfActionSubsystem); /* End of Outputs for SubSystem: '<Root>/If Action Subsystem' */ break; case 1: /* Outputs for IfAction SubSystem: '<Root>/If Action Subsystem1' incorporates: * ActionPort: '<S2>/Action Port' */ trajectoryMod_IfActionSubsystem(trajectoryModel_P.terminate_Value, &trajectoryModel_B.IfActionSubsystem1); /* End of Outputs for SubSystem: '<Root>/If Action Subsystem1' */ break; } /* End of If: '<Root>/If' */ if (rtmIsMajorTimeStep(trajectoryModel_M)) { /* Stop: '<Root>/Stop Simulation' */ if (trajectoryModel_B.IfActionSubsystem1.In1 != 0.0) { rtmSetStopRequested(trajectoryModel_M, 1); } /* End of Stop: '<Root>/Stop Simulation' */ } /* Integrator: '<Root>/dx' */ trajectoryModel_B.x_dot = trajectoryModel_X.dx_CSTATE; /* Integrator: '<Root>/dy' */ trajectoryModel_B.y_dot = trajectoryModel_X.dy_CSTATE; if (rtmIsMajorTimeStep(trajectoryModel_M)) { } /* Sum: '<S4>/x-Planet_x' incorporates: * Constant: '<Root>/Earth_x' */ rtb_Divide1 = trajectoryModel_B.x - (-trajectoryModel_P.mu); /* Sqrt: '<S4>/Sqrt' incorporates: * Product: '<S4>/(x-Planet_x)^2' * Product: '<S4>/y^2' * Sum: '<S4>/(x-Planet_x)^2+y^2' */ rtb_Divide1 = sqrt(rtb_Divide1 * rtb_Divide1 + trajectoryModel_B.y * trajectoryModel_B.y); /* Product: '<Root>/Divide1' incorporates: * Constant: '<Root>/Moon_x Earth mass' * Product: '<Root>/Divide2' */ rtb_Divide1 = (1.0 - trajectoryModel_P.mu) / (rtb_Divide1 * rtb_Divide1 * rtb_Divide1); /* Sum: '<S5>/x-Planet_x' incorporates: * Constant: '<Root>/Moon_x Earth mass' */ rtb_Divide = trajectoryModel_B.x - (1.0 - trajectoryModel_P.mu); /* Sqrt: '<S5>/Sqrt' incorporates: * Product: '<S5>/(x-Planet_x)^2' * Product: '<S5>/y^2' * Sum: '<S5>/(x-Planet_x)^2+y^2' */ rtb_Divide = sqrt(rtb_Divide * rtb_Divide + trajectoryModel_B.y * trajectoryModel_B.y); /* Product: '<Root>/Divide' incorporates: * Constant: '<Root>/Moon mass' * Product: '<Root>/Divide3' */ rtb_Divide = trajectoryModel_P.mu / (rtb_Divide * rtb_Divide * rtb_Divide); /* Sum: '<Root>/Add1' incorporates: * Constant: '<Root>/2' */ rtb_Add1 = (trajectoryModel_P._Value_f - rtb_Divide1) - rtb_Divide; /* Sum: '<Root>/Add' incorporates: * Gain: '<Root>/Gain' * Product: '<Root>/Product' */ trajectoryModel_B.Add = trajectoryModel_B.y * rtb_Add1 - trajectoryModel_P.Gain_Gain * trajectoryModel_B.x_dot; /* Sum: '<Root>/Add6' incorporates: * Constant: '<Root>/Earth_x' * Constant: '<Root>/Moon_x Earth mass' * Gain: '<Root>/Gain1' * Product: '<Root>/Product5' * Product: '<Root>/Product6' * Product: '<Root>/Product7' * Sum: '<Root>/Add7' */ trajectoryModel_B.Add6 = (((1.0 - trajectoryModel_P.mu) * rtb_Divide + -trajectoryModel_P.mu * rtb_Divide1) + trajectoryModel_B.x * rtb_Add1) + trajectoryModel_P.Gain1_Gain * trajectoryModel_B.y_dot; if (rtmIsMajorTimeStep(trajectoryModel_M)) { /* Matfile logging */ rt_UpdateTXYLogVars(trajectoryModel_M->rtwLogInfo, (trajectoryModel_M->Timing.t)); } /* end MajorTimeStep */ if (rtmIsMajorTimeStep(trajectoryModel_M)) { /* signal main to stop simulation */ { /* Sample time: [0.0s, 0.0s] */ if ((rtmGetTFinal(trajectoryModel_M)!=-1) && !((rtmGetTFinal(trajectoryModel_M)- (((trajectoryModel_M->Timing.clockTick1+ trajectoryModel_M->Timing.clockTickH1* 4294967296.0)) * 0.01)) > (((trajectoryModel_M->Timing.clockTick1+ trajectoryModel_M->Timing.clockTickH1* 4294967296.0)) * 0.01) * (DBL_EPSILON))) { rtmSetErrorStatus(trajectoryModel_M, "Simulation finished"); } } rt_ertODEUpdateContinuousStates(&trajectoryModel_M->solverInfo); /* Update absolute time for base rate */ /* The "clockTick0" counts the number of times the code of this task has * been executed. The absolute time is the multiplication of "clockTick0" * and "Timing.stepSize0". Size of "clockTick0" ensures timer will not * overflow during the application lifespan selected. * Timer of this task consists of two 32 bit unsigned integers. * The two integers represent the low bits Timing.clockTick0 and the high bits * Timing.clockTickH0. When the low bit overflows to 0, the high bits increment. */ if (!(++trajectoryModel_M->Timing.clockTick0)) { ++trajectoryModel_M->Timing.clockTickH0; } trajectoryModel_M->Timing.t[0] = rtsiGetSolverStopTime (&trajectoryModel_M->solverInfo); { /* Update absolute timer for sample time: [0.01s, 0.0s] */ /* The "clockTick1" counts the number of times the code of this task has * been executed. The resolution of this integer timer is 0.01, which is the step size * of the task. Size of "clockTick1" ensures timer will not overflow during the * application lifespan selected. * Timer of this task consists of two 32 bit unsigned integers. * The two integers represent the low bits Timing.clockTick1 and the high bits * Timing.clockTickH1. When the low bit overflows to 0, the high bits increment. */ trajectoryModel_M->Timing.clockTick1++; if (!trajectoryModel_M->Timing.clockTick1) { trajectoryModel_M->Timing.clockTickH1++; } } } /* end MajorTimeStep */ }
/* Model step function */ void Position_TiltModelClass::step() { real_T rtb_Filter; if (rtmIsMajorTimeStep((&Position_Tilt_M))) { /* set solver stop time */ if (!((&Position_Tilt_M)->Timing.clockTick0+1)) { rtsiSetSolverStopTime(&(&Position_Tilt_M)->solverInfo, (((&Position_Tilt_M) ->Timing.clockTickH0 + 1) * (&Position_Tilt_M)->Timing.stepSize0 * 4294967296.0)); } else { rtsiSetSolverStopTime(&(&Position_Tilt_M)->solverInfo, (((&Position_Tilt_M) ->Timing.clockTick0 + 1) * (&Position_Tilt_M)->Timing.stepSize0 + (&Position_Tilt_M)->Timing.clockTickH0 * (&Position_Tilt_M) ->Timing.stepSize0 * 4294967296.0)); } } /* end MajorTimeStep */ /* Update absolute time of base rate at minor time step */ if (rtmIsMinorTimeStep((&Position_Tilt_M))) { (&Position_Tilt_M)->Timing.t[0] = rtsiGetT(&(&Position_Tilt_M)->solverInfo); } /* Sum: '<S1>/Sum' incorporates: * Inport: '<Root>/Pos' * Inport: '<Root>/PosRef' */ rtb_Filter = Position_Tilt_U.PosRef[0] - Position_Tilt_U.Pos[0]; /* Gain: '<S2>/Filter Coefficient' incorporates: * Gain: '<S2>/Derivative Gain' * Integrator: '<S2>/Filter' * Sum: '<S2>/SumD' */ Position_Tilt_B.FilterCoefficient = (Position_Tilt_P.Kd_N * rtb_Filter - Position_Tilt_X.Filter_CSTATE) * Position_Tilt_P.PIDController_N; /* Outport: '<Root>/u_des' incorporates: * Gain: '<S2>/Proportional Gain' * Sum: '<S2>/Sum' */ Position_Tilt_Y.u_des = Position_Tilt_P.Kp_N * rtb_Filter + Position_Tilt_B.FilterCoefficient; /* Sum: '<S1>/Sum1' incorporates: * Inport: '<Root>/Pos' * Inport: '<Root>/PosRef' */ rtb_Filter = Position_Tilt_U.PosRef[1] - Position_Tilt_U.Pos[1]; /* Gain: '<S3>/Filter Coefficient' incorporates: * Gain: '<S3>/Derivative Gain' * Integrator: '<S3>/Filter' * Sum: '<S3>/SumD' */ Position_Tilt_B.FilterCoefficient_f = (Position_Tilt_P.Kd_E * rtb_Filter - Position_Tilt_X.Filter_CSTATE_b) * Position_Tilt_P.PIDController1_N; /* Outport: '<Root>/v_des' incorporates: * Gain: '<S3>/Proportional Gain' * Sum: '<S3>/Sum' */ Position_Tilt_Y.v_des = Position_Tilt_P.Kp_E * rtb_Filter + Position_Tilt_B.FilterCoefficient_f; if (rtmIsMajorTimeStep((&Position_Tilt_M))) { rt_ertODEUpdateContinuousStates(&(&Position_Tilt_M)->solverInfo); /* Update absolute time for base rate */ /* The "clockTick0" counts the number of times the code of this task has * been executed. The absolute time is the multiplication of "clockTick0" * and "Timing.stepSize0". Size of "clockTick0" ensures timer will not * overflow during the application lifespan selected. * Timer of this task consists of two 32 bit unsigned integers. * The two integers represent the low bits Timing.clockTick0 and the high bits * Timing.clockTickH0. When the low bit overflows to 0, the high bits increment. */ if (!(++(&Position_Tilt_M)->Timing.clockTick0)) { ++(&Position_Tilt_M)->Timing.clockTickH0; } (&Position_Tilt_M)->Timing.t[0] = rtsiGetSolverStopTime(&(&Position_Tilt_M )->solverInfo); { /* Update absolute timer for sample time: [0.01s, 0.0s] */ /* The "clockTick1" counts the number of times the code of this task has * been executed. The resolution of this integer timer is 0.01, which is the step size * of the task. Size of "clockTick1" ensures timer will not overflow during the * application lifespan selected. * Timer of this task consists of two 32 bit unsigned integers. * The two integers represent the low bits Timing.clockTick1 and the high bits * Timing.clockTickH1. When the low bit overflows to 0, the high bits increment. */ (&Position_Tilt_M)->Timing.clockTick1++; if (!(&Position_Tilt_M)->Timing.clockTick1) { (&Position_Tilt_M)->Timing.clockTickH1++; } } } /* end MajorTimeStep */ }
/* This function updates continuous states using the ODE3 fixed-step * solver algorithm */ static void rt_ertODEUpdateContinuousStates(RTWSolverInfo *si ) { time_T t = rtsiGetT(si); time_T tnew = rtsiGetSolverStopTime(si); time_T h = rtsiGetStepSize(si); real_T *x = rtsiGetContStates(si); ODE3_IntgData *id = (ODE3_IntgData *)rtsiGetSolverData(si); real_T *y = id->y; real_T *f0 = id->f[0]; real_T *f1 = id->f[1]; real_T *f2 = id->f[2]; real_T hB[3]; int_T i; int_T nXc = 10; rtsiSetSimTimeStep(si,MINOR_TIME_STEP); /* Save the state values at time t in y, we'll use x as ynew. */ (void) memcpy(y,x, nXc*sizeof(real_T)); /* Assumes that rtsiSetT and ModelOutputs are up-to-date */ /* f0 = f(t,y) */ rtsiSetdX(si, f0); Mechanics_derivatives(); /* f(:,2) = feval(odefile, t + hA(1), y + f*hB(:,1), args(:)(*)); */ hB[0] = h * rt_ODE3_B[0][0]; for (i = 0; i < nXc; i++) { x[i] = y[i] + (f0[i]*hB[0]); } rtsiSetT(si, t + h*rt_ODE3_A[0]); rtsiSetdX(si, f1); Mechanics_output(0); Mechanics_derivatives(); /* f(:,3) = feval(odefile, t + hA(2), y + f*hB(:,2), args(:)(*)); */ for (i = 0; i <= 1; i++) hB[i] = h * rt_ODE3_B[1][i]; for (i = 0; i < nXc; i++) { x[i] = y[i] + (f0[i]*hB[0] + f1[i]*hB[1]); } rtsiSetT(si, t + h*rt_ODE3_A[1]); rtsiSetdX(si, f2); Mechanics_output(0); Mechanics_derivatives(); /* tnew = t + hA(3); ynew = y + f*hB(:,3); */ for (i = 0; i <= 2; i++) hB[i] = h * rt_ODE3_B[2][i]; for (i = 0; i < nXc; i++) { x[i] = y[i] + (f0[i]*hB[0] + f1[i]*hB[1] + f2[i]*hB[2]); } rtsiSetT(si, tnew); Mechanics_output(0); Mechanics_projection(); rtsiSetSimTimeStep(si,MAJOR_TIME_STEP); }
void rt_ODEUpdateContinuousStates(RTWSolverInfo *si) { time_T t = rtsiGetT(si); time_T tnew = rtsiGetSolverStopTime(si); time_T h = rtsiGetStepSize(si); real_T *x = rtsiGetContStates(si); IntgData *id = rtsiGetSolverData(si); real_T *y = id->y; real_T *f0 = id->f[0]; real_T *f1 = id->f[1]; real_T *f2 = id->f[2]; real_T hB[3]; int_T i; #ifdef NCSTATES int_T nXc = NCSTATES; #else int_T nXc = rtsiGetNumContStates(si); #endif rtsiSetSimTimeStep(si,MINOR_TIME_STEP); /* Save the state values at time t in y, we'll use x as ynew. */ (void)memcpy(y, x, nXc*sizeof(real_T)); /* Assumes that rtsiSetT and ModelOutputs are up-to-date */ /* f0 = f(t,y) */ rtsiSetdX(si, f0); DERIVATIVES(si); /* f(:,2) = feval(odefile, t + hA(1), y + f*hB(:,1), args(:)(*)); */ hB[0] = h * rt_ODE3_B[0][0]; for (i = 0; i < nXc; i++) { x[i] = y[i] + (f0[i]*hB[0]); } rtsiSetT(si, t + h*rt_ODE3_A[0]); rtsiSetdX(si, f1); OUTPUTS(si,0); DERIVATIVES(si); /* f(:,3) = feval(odefile, t + hA(2), y + f*hB(:,2), args(:)(*)); */ for (i = 0; i <= 1; i++) hB[i] = h * rt_ODE3_B[1][i]; for (i = 0; i < nXc; i++) { x[i] = y[i] + (f0[i]*hB[0] + f1[i]*hB[1]); } rtsiSetT(si, t + h*rt_ODE3_A[1]); rtsiSetdX(si, f2); OUTPUTS(si,0); DERIVATIVES(si); /* tnew = t + hA(3); ynew = y + f*hB(:,3); */ for (i = 0; i <= 2; i++) hB[i] = h * rt_ODE3_B[2][i]; for (i = 0; i < nXc; i++) { x[i] = y[i] + (f0[i]*hB[0] + f1[i]*hB[1] + f2[i]*hB[2]); } rtsiSetT(si, tnew); PROJECTION(si); rtsiSetSimTimeStep(si,MAJOR_TIME_STEP); }
static void rt_UpdateContinuousStates(RT_MODEL * S) { rtmSetT(S, rtsiGetSolverStopTime(rtmGetRTWSolverInfo(S))); }
/* Model step function */ void AttitudeModelClass::step() { real_T Sphi; real_T Cphi; real_T Ctheta; real_T rtb_Sum4; real_T rtb_ProportionalGain; real_T rtb_ProportionalGain_h; real_T rtb_Rates_B[3]; real_T rtb_Sum6; real_T rtb_Saturate_l; real_T rtb_Sum_k; real_T tmp[9]; int32_T i; if (rtmIsMajorTimeStep((&Attitude_M))) { /* set solver stop time */ if (!((&Attitude_M)->Timing.clockTick0+1)) { rtsiSetSolverStopTime(&(&Attitude_M)->solverInfo, (((&Attitude_M) ->Timing.clockTickH0 + 1) * (&Attitude_M)->Timing.stepSize0 * 4294967296.0)); } else { rtsiSetSolverStopTime(&(&Attitude_M)->solverInfo, (((&Attitude_M) ->Timing.clockTick0 + 1) * (&Attitude_M)->Timing.stepSize0 + (&Attitude_M)->Timing.clockTickH0 * (&Attitude_M)->Timing.stepSize0 * 4294967296.0)); } } /* end MajorTimeStep */ /* Update absolute time of base rate at minor time step */ if (rtmIsMinorTimeStep((&Attitude_M))) { (&Attitude_M)->Timing.t[0] = rtsiGetT(&(&Attitude_M)->solverInfo); } /* Saturate: '<S1>/Saturation' incorporates: * Inport: '<Root>/Stick' */ if (Attitude_U.Stick[0] > Attitude_P.Saturation_UpperSat) { rtb_Sum4 = Attitude_P.Saturation_UpperSat; } else if (Attitude_U.Stick[0] < Attitude_P.Saturation_LowerSat) { rtb_Sum4 = Attitude_P.Saturation_LowerSat; } else { rtb_Sum4 = Attitude_U.Stick[0]; } /* Sum: '<S1>/Sum' incorporates: * Gain: '<S1>/Yaw-rate1' * Inport: '<Root>/IMU_Attitude' * Saturate: '<S1>/Saturation' */ rtb_Sum4 = Attitude_P.rollMax * rtb_Sum4 - Attitude_U.IMU_Attitude[0]; /* Gain: '<S3>/Proportional Gain' */ rtb_ProportionalGain = Attitude_P.KRP * rtb_Sum4; /* Gain: '<S3>/Filter Coefficient' incorporates: * Gain: '<S3>/Derivative Gain' * Integrator: '<S3>/Filter' * Sum: '<S3>/SumD' */ Attitude_B.FilterCoefficient = (Attitude_P.KRD * rtb_Sum4 - Attitude_X.Filter_CSTATE) * Attitude_P.N; /* Saturate: '<S1>/Saturation1' incorporates: * Inport: '<Root>/Stick' */ if (Attitude_U.Stick[1] > Attitude_P.Saturation1_UpperSat) { rtb_Sum4 = Attitude_P.Saturation1_UpperSat; } else if (Attitude_U.Stick[1] < Attitude_P.Saturation1_LowerSat) { rtb_Sum4 = Attitude_P.Saturation1_LowerSat; } else { rtb_Sum4 = Attitude_U.Stick[1]; } /* Sum: '<S1>/Sum1' incorporates: * Gain: '<S1>/Yaw-rate2' * Inport: '<Root>/IMU_Attitude' * Saturate: '<S1>/Saturation1' */ rtb_Sum4 = Attitude_P.pitchMax * rtb_Sum4 - Attitude_U.IMU_Attitude[1]; /* Gain: '<S2>/Proportional Gain' */ rtb_ProportionalGain_h = Attitude_P.KPP * rtb_Sum4; /* Gain: '<S2>/Filter Coefficient' incorporates: * Gain: '<S2>/Derivative Gain' * Integrator: '<S2>/Filter' * Sum: '<S2>/SumD' */ Attitude_B.FilterCoefficient_e = (Attitude_P.KPD * rtb_Sum4 - Attitude_X.Filter_CSTATE_m) * Attitude_P.N; /* Sum: '<S1>/Sum2' incorporates: * Inport: '<Root>/IMU_Attitude' * Inport: '<Root>/Stick' */ rtb_Sum4 = Attitude_U.Stick[3] - Attitude_U.IMU_Attitude[2]; /* Gain: '<S4>/Filter Coefficient' incorporates: * Gain: '<S4>/Derivative Gain' * Integrator: '<S4>/Filter' * Sum: '<S4>/SumD' */ Attitude_B.FilterCoefficient_d = (Attitude_P.KYD * rtb_Sum4 - Attitude_X.Filter_CSTATE_mi) * Attitude_P.N; /* Switch: '<S1>/Switch' incorporates: * Gain: '<S1>/Yaw-rate3' * Gain: '<S4>/Proportional Gain' * Inport: '<Root>/Selector' * Inport: '<Root>/Stick' * Saturate: '<S1>/Saturation2' * Sum: '<S4>/Sum' */ if (Attitude_U.Selector >= Attitude_P.Switch_Threshold) { rtb_Sum4 = Attitude_P.KYP * rtb_Sum4 + Attitude_B.FilterCoefficient_d; } else { if (Attitude_U.Stick[2] > Attitude_P.Saturation2_UpperSat) { /* Saturate: '<S1>/Saturation2' */ rtb_Sum4 = Attitude_P.Saturation2_UpperSat; } else if (Attitude_U.Stick[2] < Attitude_P.Saturation2_LowerSat) { /* Saturate: '<S1>/Saturation2' */ rtb_Sum4 = Attitude_P.Saturation2_LowerSat; } else { /* Saturate: '<S1>/Saturation2' incorporates: * Inport: '<Root>/Stick' */ rtb_Sum4 = Attitude_U.Stick[2]; } rtb_Sum4 *= Attitude_P.yawRateMax; } /* End of Switch: '<S1>/Switch' */ /* MATLAB Function: '<S1>/To body from Earth_rates' incorporates: * Inport: '<Root>/IMU_Attitude' */ /* MATLAB Function 'Attitude Controller/To body from Earth_rates': '<S8>:1' */ /* '<S8>:1:3' */ /* '<S8>:1:4' */ /* '<S8>:1:6' */ Sphi = sin(Attitude_U.IMU_Attitude[0]); /* '<S8>:1:7' */ Cphi = cos(Attitude_U.IMU_Attitude[0]); /* '<S8>:1:8' */ /* '<S8>:1:9' */ Ctheta = cos(Attitude_U.IMU_Attitude[1]); /* '<S8>:1:11' */ /* '<S8>:1:15' */ tmp[0] = 1.0; tmp[3] = 0.0; tmp[6] = -sin(Attitude_U.IMU_Attitude[1]); tmp[1] = 0.0; tmp[4] = Cphi; tmp[7] = Sphi * Ctheta; tmp[2] = 0.0; tmp[5] = -Sphi; tmp[8] = Cphi * Ctheta; /* SignalConversion: '<S8>/TmpSignal ConversionAt SFunction Inport2' incorporates: * MATLAB Function: '<S1>/To body from Earth_rates' * Sum: '<S2>/Sum' * Sum: '<S3>/Sum' */ rtb_ProportionalGain += Attitude_B.FilterCoefficient; rtb_ProportionalGain_h += Attitude_B.FilterCoefficient_e; /* MATLAB Function: '<S1>/To body from Earth_rates' incorporates: * SignalConversion: '<S8>/TmpSignal ConversionAt SFunction Inport2' */ for (i = 0; i < 3; i++) { rtb_Rates_B[i] = tmp[i + 6] * rtb_Sum4 + (tmp[i + 3] * rtb_ProportionalGain_h + tmp[i] * rtb_ProportionalGain); } /* Sum: '<S1>/Sum4' incorporates: * Inport: '<Root>/IMU_Rates' */ rtb_Sum4 = rtb_Rates_B[0] - Attitude_U.IMU_Rates[0]; /* Gain: '<S5>/Filter Coefficient' incorporates: * Gain: '<S5>/Derivative Gain' * Integrator: '<S5>/Filter' * Sum: '<S5>/SumD' */ Attitude_B.FilterCoefficient_o = (Attitude_P.Kdp * rtb_Sum4 - Attitude_X.Filter_CSTATE_k) * Attitude_P.N; /* Sum: '<S5>/Sum' incorporates: * Gain: '<S5>/Proportional Gain' * Integrator: '<S5>/Integrator' */ rtb_ProportionalGain = (Attitude_P.Kpp * rtb_Sum4 + Attitude_X.Integrator_CSTATE) + Attitude_B.FilterCoefficient_o; /* Saturate: '<S5>/Saturate' */ if (rtb_ProportionalGain > Attitude_P.satp) { rtb_ProportionalGain_h = Attitude_P.satp; } else if (rtb_ProportionalGain < -Attitude_P.satp) { rtb_ProportionalGain_h = -Attitude_P.satp; } else { rtb_ProportionalGain_h = rtb_ProportionalGain; } /* End of Saturate: '<S5>/Saturate' */ /* Sum: '<S1>/Sum5' incorporates: * Inport: '<Root>/IMU_Rates' */ Sphi = rtb_Rates_B[1] - Attitude_U.IMU_Rates[1]; /* Gain: '<S6>/Filter Coefficient' incorporates: * Gain: '<S6>/Derivative Gain' * Integrator: '<S6>/Filter' * Sum: '<S6>/SumD' */ Attitude_B.FilterCoefficient_b = (Attitude_P.Kdq * Sphi - Attitude_X.Filter_CSTATE_e) * Attitude_P.N; /* Sum: '<S6>/Sum' incorporates: * Gain: '<S6>/Proportional Gain' * Integrator: '<S6>/Integrator' */ Cphi = (Attitude_P.Kpq * Sphi + Attitude_X.Integrator_CSTATE_f) + Attitude_B.FilterCoefficient_b; /* Saturate: '<S6>/Saturate' */ if (Cphi > Attitude_P.satq) { Ctheta = Attitude_P.satq; } else if (Cphi < -Attitude_P.satq) { Ctheta = -Attitude_P.satq; } else { Ctheta = Cphi; } /* End of Saturate: '<S6>/Saturate' */ /* Sum: '<S1>/Sum6' incorporates: * Inport: '<Root>/IMU_Rates' */ rtb_Sum6 = rtb_Rates_B[2] - Attitude_U.IMU_Rates[2]; /* Gain: '<S7>/Filter Coefficient' incorporates: * Gain: '<S7>/Derivative Gain' * Integrator: '<S7>/Filter' * Sum: '<S7>/SumD' */ Attitude_B.FilterCoefficient_oo = (Attitude_P.Kdr * rtb_Sum6 - Attitude_X.Filter_CSTATE_g) * Attitude_P.N; /* Sum: '<S7>/Sum' incorporates: * Gain: '<S7>/Proportional Gain' * Integrator: '<S7>/Integrator' */ rtb_Sum_k = (Attitude_P.Kpr * rtb_Sum6 + Attitude_X.Integrator_CSTATE_h) + Attitude_B.FilterCoefficient_oo; /* Saturate: '<S7>/Saturate' */ if (rtb_Sum_k > Attitude_P.satr) { rtb_Saturate_l = Attitude_P.satr; } else if (rtb_Sum_k < -Attitude_P.satr) { rtb_Saturate_l = -Attitude_P.satr; } else { rtb_Saturate_l = rtb_Sum_k; } /* End of Saturate: '<S7>/Saturate' */ /* Outport: '<Root>/Moments' */ Attitude_Y.Moments[0] = rtb_ProportionalGain_h; Attitude_Y.Moments[1] = Ctheta; Attitude_Y.Moments[2] = rtb_Saturate_l; /* Sum: '<S5>/SumI1' incorporates: * Gain: '<S5>/Integral Gain' * Gain: '<S5>/Kb' * Sum: '<S5>/SumI2' */ Attitude_B.SumI1 = (rtb_ProportionalGain_h - rtb_ProportionalGain) * Attitude_P.Kbp + Attitude_P.Kip * rtb_Sum4; /* Sum: '<S6>/SumI1' incorporates: * Gain: '<S6>/Integral Gain' * Gain: '<S6>/Kb' * Sum: '<S6>/SumI2' */ Attitude_B.SumI1_e = (Ctheta - Cphi) * Attitude_P.Kbq + Attitude_P.Kiq * Sphi; /* Sum: '<S7>/SumI1' incorporates: * Gain: '<S7>/Integral Gain' * Gain: '<S7>/Kb' * Sum: '<S7>/SumI2' */ Attitude_B.SumI1_k = (rtb_Saturate_l - rtb_Sum_k) * Attitude_P.Kbr + Attitude_P.Kir * rtb_Sum6; if (rtmIsMajorTimeStep((&Attitude_M))) { rt_ertODEUpdateContinuousStates(&(&Attitude_M)->solverInfo); /* Update absolute time for base rate */ /* The "clockTick0" counts the number of times the code of this task has * been executed. The absolute time is the multiplication of "clockTick0" * and "Timing.stepSize0". Size of "clockTick0" ensures timer will not * overflow during the application lifespan selected. * Timer of this task consists of two 32 bit unsigned integers. * The two integers represent the low bits Timing.clockTick0 and the high bits * Timing.clockTickH0. When the low bit overflows to 0, the high bits increment. */ if (!(++(&Attitude_M)->Timing.clockTick0)) { ++(&Attitude_M)->Timing.clockTickH0; } (&Attitude_M)->Timing.t[0] = rtsiGetSolverStopTime(&(&Attitude_M) ->solverInfo); { /* Update absolute timer for sample time: [0.01s, 0.0s] */ /* The "clockTick1" counts the number of times the code of this task has * been executed. The resolution of this integer timer is 0.01, which is the step size * of the task. Size of "clockTick1" ensures timer will not overflow during the * application lifespan selected. * Timer of this task consists of two 32 bit unsigned integers. * The two integers represent the low bits Timing.clockTick1 and the high bits * Timing.clockTickH1. When the low bit overflows to 0, the high bits increment. */ (&Attitude_M)->Timing.clockTick1++; if (!(&Attitude_M)->Timing.clockTick1) { (&Attitude_M)->Timing.clockTickH1++; } } } /* end MajorTimeStep */ }