/* Model update function */
void m1006_update(int_T tid)
{
if (rtmIsMajorTimeStep(m1006_M)) {
rt_ertODEUpdateContinuousStates(&m1006_M->solverInfo, 0);
}
/* Update absolute time for base rate */
if(!(++m1006_M->Timing.clockTick0)) ++m1006_M->Timing.clockTickH0;
m1006_M->Timing.t[0] = m1006_M->Timing.clockTick0 * m1006_M->Timing.stepSize0
+ m1006_M->Timing.clockTickH0 * m1006_M->Timing.stepSize0 * 4294967296.0;
if (rtmIsMajorTimeStep(m1006_M) &&
m1006_M->Timing.TaskCounters.TID[1] == 0) {
/* Update absolute timer for sample time: [0.1s, 0.0s] */
if(!(++m1006_M->Timing.clockTick1)) ++m1006_M->Timing.clockTickH1;
m1006_M->Timing.t[1] = m1006_M->Timing.clockTick1 *
m1006_M->Timing.stepSize1 + m1006_M->Timing.clockTickH1 *
m1006_M->Timing.stepSize1 * 4294967296.0;
}
rate_scheduler();
}
/* Model update function */
void Crane_update(int_T tid)
{
if (rtmIsMajorTimeStep(Crane_M) &&
Crane_M->Timing.TaskCounters.TID[1] == 0) {
/* Update for Memory: '<Root>/Memory' */
Crane_DWork.Memory_PreviousInput[0] = Crane_B.Gain3[1];
Crane_DWork.Memory_PreviousInput[1] = Crane_B.Gain3[2];
Crane_DWork.Memory_PreviousInput[2] = Crane_B.Constant;
}
/* Derivative Block: '<S9>/Derivative' */
{
real_T timeStampA = Crane_DWork.Derivative_RWORK.TimeStampA;
real_T timeStampB = Crane_DWork.Derivative_RWORK.TimeStampB;
real_T *lastBank = &Crane_DWork.Derivative_RWORK.TimeStampA;
if (timeStampA != rtInf) {
if (timeStampB == rtInf) {
lastBank += 4;
} else if (timeStampA >= timeStampB) {
lastBank += 4;
}
}
*lastBank++ = Crane_M->Timing.t[0];
*lastBank++ = Crane_B.Sum[0];
*lastBank++ = Crane_B.Sum[1];
*lastBank++ = Crane_B.Sum[2];
}
if (rtmIsMajorTimeStep(Crane_M)) {
rt_ertODEUpdateContinuousStates(&Crane_M->solverInfo);
}
/* Update absolute time for base rate */
if (!(++Crane_M->Timing.clockTick0))
++Crane_M->Timing.clockTickH0;
Crane_M->Timing.t[0] = Crane_M->Timing.clockTick0 * Crane_M->Timing.stepSize0
+ Crane_M->Timing.clockTickH0 * Crane_M->Timing.stepSize0 * 4294967296.0;
if (rtmIsMajorTimeStep(Crane_M) &&
Crane_M->Timing.TaskCounters.TID[1] == 0) {
/* Update absolute timer for sample time: [0.001s, 0.0s] */
if (!(++Crane_M->Timing.clockTick1))
++Crane_M->Timing.clockTickH1;
Crane_M->Timing.t[1] = Crane_M->Timing.clockTick1 *
Crane_M->Timing.stepSize1 + Crane_M->Timing.clockTickH1 *
Crane_M->Timing.stepSize1 * 4294967296.0;
}
UNUSED_PARAMETER(tid);
}
/* 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);
}
/* 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;
}
}
/* Model update function */
void Mechanics_update(int_T tid)
{
if (rtmIsMajorTimeStep(Mechanics_M)) {
rt_ertODEUpdateContinuousStates(&Mechanics_M->solverInfo);
}
/* Update absolute time for base rate */
if (!(++Mechanics_M->Timing.clockTick0))
++Mechanics_M->Timing.clockTickH0;
Mechanics_M->Timing.t[0] = Mechanics_M->Timing.clockTick0 *
Mechanics_M->Timing.stepSize0 + Mechanics_M->Timing.clockTickH0 *
Mechanics_M->Timing.stepSize0 * 4294967296.0;
if (rtmIsMajorTimeStep(Mechanics_M) &&
Mechanics_M->Timing.TaskCounters.TID[1] == 0) {
/* Update absolute timer for sample time: [35.0s, 0.0s] */
if (!(++Mechanics_M->Timing.clockTick1))
++Mechanics_M->Timing.clockTickH1;
Mechanics_M->Timing.t[1] = Mechanics_M->Timing.clockTick1 *
Mechanics_M->Timing.stepSize1 + Mechanics_M->Timing.clockTickH1 *
Mechanics_M->Timing.stepSize1 * 4294967296.0;
}
UNUSED_PARAMETER(tid);
}
/* 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 */
}
/* 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 */
}
