/* Model output function */
void m1006_output(int_T tid)
{
/* Update absolute time of base rate at minor time step */
if (rtmIsMinorTimeStep(m1006_M)) {
m1006_M->Timing.t[0] = rtsiGetT(&m1006_M->solverInfo);
}
if (rtmIsMajorTimeStep(m1006_M)) {
/* set solver stop time */
rtsiSetSolverStopTime(&m1006_M->solverInfo,
((m1006_M->Timing.clockTick0+1)*m1006_M->Timing.stepSize0));
} /* end MajorTimeStep */
/* Sin: '<Root>/Sine Wave' */
m1006_B.SineWave = m1006_P.SineWave_Amp *
sin(m1006_P.SineWave_Freq * m1006_M->Timing.t[0] + m1006_P.SineWave_Phase) +
m1006_P.SineWave_Bias;
/* S-Function "motor_wrapper" Block: <Root>/S-Function Builder */
{
real_T *pxc = m1006_M->ModelData.contStates;
motor_Outputs_wrapper(&m1006_B.SineWave, &m1006_B.SFunctionBuilder, pxc);
}
/* Outport: '<Root>/Out1' */
m1006_Y.Out1 = m1006_B.SFunctionBuilder;
}
/* 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 output function */
void xpcosc_output(int_T tid)
{
real_T temp;
if (rtmIsMajorTimeStep(xpcosc_rtM)) {
/* set solver stop time */
if (!(xpcosc_rtM->Timing.clockTick0+1)) {
rtsiSetSolverStopTime(&xpcosc_rtM->solverInfo,
((xpcosc_rtM->Timing.clockTickH0 + 1) *
xpcosc_rtM->Timing.stepSize0 * 4294967296.0));
} else {
rtsiSetSolverStopTime(&xpcosc_rtM->solverInfo,
((xpcosc_rtM->Timing.clockTick0 + 1) *
xpcosc_rtM->Timing.stepSize0 + xpcosc_rtM->Timing.clockTickH0 *
xpcosc_rtM->Timing.stepSize0 * 4294967296.0));
}
} /* end MajorTimeStep */
/* Update absolute time of base rate at minor time step */
if (rtmIsMinorTimeStep(xpcosc_rtM)) {
xpcosc_rtM->Timing.t[0] = rtsiGetT(&xpcosc_rtM->solverInfo);
}
/* Integrator: '<Root>/Integrator1' */
xpcosc_B.Integrator1 = xpcosc_X.Integrator1_CSTATE;
if (rtmIsMajorTimeStep(xpcosc_rtM)) {
/* Level2 S-Function Block: '<Root>/PCI-6221 AD' (adnipcim) */
{
SimStruct *rts = xpcosc_rtM->childSfunctions[0];
sfcnOutputs(rts, 1);
}
/* RateTransition: '<Root>/Rate Transition1' */
if (rtmIsMajorTimeStep(xpcosc_rtM)) {
xpcosc_B.RateTransition1 = xpcosc_B.PCI6221AD;
}
/* End of RateTransition: '<Root>/Rate Transition1' */
}
/* Outport: '<Root>/Outport' */
xpcosc_Y.Outport[0] = xpcosc_B.Integrator1;
xpcosc_Y.Outport[1] = xpcosc_B.RateTransition1;
/* SignalGenerator: '<Root>/Signal Generator' */
temp = xpcosc_P.SignalGenerator_Frequency * xpcosc_rtM->Timing.t[0];
if (temp - floor(temp) >= 0.5) {
xpcosc_B.SignalGenerator = xpcosc_P.SignalGenerator_Amplitude;
} else {
xpcosc_B.SignalGenerator = -xpcosc_P.SignalGenerator_Amplitude;
}
/* End of SignalGenerator: '<Root>/Signal Generator' */
/* RateTransition: '<Root>/Rate Transition' */
if (rtmIsMajorTimeStep(xpcosc_rtM)) {
xpcosc_B.RateTransition = xpcosc_B.SignalGenerator;
/* Level2 S-Function Block: '<Root>/PCI-6713 DA' (danipci671x) */
{
SimStruct *rts = xpcosc_rtM->childSfunctions[1];
sfcnOutputs(rts, 1);
}
}
/* End of RateTransition: '<Root>/Rate Transition' */
/* ok to acquire for <S1>/S-Function */
xpcosc_DWork.SFunction_IWORK.AcquireOK = 1;
/* Gain: '<Root>/Gain' */
xpcosc_B.Gain = xpcosc_P.Gain_Gain * xpcosc_B.Integrator1;
/* Integrator: '<Root>/Integrator' */
xpcosc_B.Integrator = xpcosc_X.Integrator_CSTATE;
/* Gain: '<Root>/Gain1' */
xpcosc_B.Gain1 = xpcosc_P.Gain1_Gain * xpcosc_B.Integrator;
/* Gain: '<Root>/Gain2' */
xpcosc_B.Gain2 = xpcosc_P.Gain2_Gain * xpcosc_B.SignalGenerator;
/* Sum: '<Root>/Sum' */
xpcosc_B.Sum = (xpcosc_B.Gain2 - xpcosc_B.Gain) - xpcosc_B.Gain1;
/* tid is required for a uniform function interface.
* Argument tid is not used in the function. */
UNUSED_PARAMETER(tid);
}
/* Model output function */
void Crane_output(int_T tid)
{
/* local block i/o variables */
real_T rtb_Falcon_o1[3];
real_T rtb_Gain4[3];
boolean_T rtb_Falcon_o2[4];
/* Update absolute time of base rate at minor time step */
if (rtmIsMinorTimeStep(Crane_M)) {
Crane_M->Timing.t[0] = rtsiGetT(&Crane_M->solverInfo);
}
if (rtmIsMajorTimeStep(Crane_M)) {
/* set solver stop time */
rtsiSetSolverStopTime(&Crane_M->solverInfo,
((Crane_M->Timing.clockTick0+1)*
Crane_M->Timing.stepSize0));
} /* end MajorTimeStep */
{
real_T rtb_Product_idx;
real_T rtb_Product_idx_0;
real_T rtb_Product_idx_1;
real_T rtb_Product1_idx;
real_T rtb_Product1_idx_0;
real_T rtb_Product1_idx_1;
/* S-Function Block: <S10>/Block#1 */
{
_rtMech_PWORK *mechWork = (_rtMech_PWORK *) Crane_DWork.Block1_PWORK;
mechWork->genSimData.time = Crane_M->Timing.t[0];
mechWork->outSimData.majorTimestep = rtmIsMajorTimeStep(Crane_M);
if (kinematicSfcnOutputMethod(mechWork->mechanism, &(mechWork->genSimData),
&(mechWork->outSimData))) {
{
const ErrorRecord *err = getErrorMsg();
static char_T errorMsg[1024];
sprintf(errorMsg,
err->errorMsg,
err->blocks[0],
err->blocks[1],
err->blocks[2],
err->blocks[3],
err->blocks[4]);
rtmSetErrorStatus(Crane_M, errorMsg);
return;
}
}
}
/* Gain: '<S2>/gain_1' */
Crane_B.gain_1[0] = Crane_P.gain_1_Gain * Crane_B.Block1_o1[0];
Crane_B.gain_1[1] = Crane_P.gain_1_Gain * Crane_B.Block1_o1[1];
Crane_B.gain_1[2] = Crane_P.gain_1_Gain * Crane_B.Block1_o1[2];
if (rtmIsMajorTimeStep(Crane_M) &&
Crane_M->Timing.TaskCounters.TID[1] == 0) {
/* Memory: '<Root>/Memory' */
Crane_B.Memory[0] = Crane_DWork.Memory_PreviousInput[0];
Crane_B.Memory[1] = Crane_DWork.Memory_PreviousInput[1];
Crane_B.Memory[2] = Crane_DWork.Memory_PreviousInput[2];
}
/* Sum: '<S9>/Sum' */
Crane_B.Sum[0] = Crane_B.Memory[0] - Crane_B.gain_1[0];
Crane_B.Sum[1] = Crane_B.Memory[1] - Crane_B.gain_1[1];
Crane_B.Sum[2] = Crane_B.Memory[2] - Crane_B.gain_1[2];
/* Product: '<S9>/Product' incorporates:
* Constant: '<Root>/Constant2'
*/
rtb_Product_idx = Crane_P.Constant2_Value * Crane_B.Sum[0];
rtb_Product_idx_0 = Crane_P.Constant2_Value * Crane_B.Sum[1];
rtb_Product_idx_1 = Crane_P.Constant2_Value * Crane_B.Sum[2];
/* Integrator: '<S9>/Integrator' */
rtb_Gain4[0] = Crane_X.Integrator_CSTATE[0];
rtb_Gain4[1] = Crane_X.Integrator_CSTATE[1];
rtb_Gain4[2] = Crane_X.Integrator_CSTATE[2];
/* Product: '<S9>/Product1' incorporates:
* Constant: '<Root>/Constant5'
*/
rtb_Product1_idx = Crane_P.Constant5_Value * rtb_Gain4[0];
rtb_Product1_idx_0 = Crane_P.Constant5_Value * rtb_Gain4[1];
rtb_Product1_idx_1 = Crane_P.Constant5_Value * rtb_Gain4[2];
/* Derivative Block: '<S9>/Derivative' */
{
real_T t = Crane_M->Timing.t[0];
real_T timeStampA = Crane_DWork.Derivative_RWORK.TimeStampA;
real_T timeStampB = Crane_DWork.Derivative_RWORK.TimeStampB;
if (timeStampA >= t && timeStampB >= t) {
rtb_Gain4[0] = 0.0;
rtb_Gain4[1] = 0.0;
rtb_Gain4[2] = 0.0;
} else {
real_T deltaT;
real_T *lastBank = &Crane_DWork.Derivative_RWORK.TimeStampA;
if (timeStampA < timeStampB) {
if (timeStampB < t) {
lastBank += 4;
}
} else if (timeStampA >= t) {
lastBank += 4;
}
deltaT = t - *lastBank++;
rtb_Gain4[0] = (Crane_B.Sum[0] - *lastBank++) / deltaT;
rtb_Gain4[1] = (Crane_B.Sum[1] - *lastBank++) / deltaT;
rtb_Gain4[2] = (Crane_B.Sum[2] - *lastBank++) / deltaT;
}
}
/* Sum: '<S9>/Sum1' incorporates:
* Constant: '<Root>/Constant6'
* Product: '<S9>/Product2'
*/
Crane_B.Sum1[0] = (rtb_Product_idx + rtb_Product1_idx) +
Crane_P.Constant6_Value * rtb_Gain4[0];
Crane_B.Sum1[1] = (rtb_Product_idx_0 + rtb_Product1_idx_0) +
Crane_P.Constant6_Value * rtb_Gain4[1];
Crane_B.Sum1[2] = (rtb_Product_idx_1 + rtb_Product1_idx_1) +
Crane_P.Constant6_Value * rtb_Gain4[2];
/* Gain: '<Root>/Gain4' */
rtb_Gain4[0] = Crane_P.Gain4_Gain * Crane_B.Sum1[0];
rtb_Gain4[1] = Crane_P.Gain4_Gain * Crane_B.Sum1[1];
rtb_Gain4[2] = Crane_P.Gain4_Gain * Crane_B.Sum1[2];
/* Saturate: '<Root>/Saturation' */
Crane_B.Saturation[0] = rt_SATURATE(rtb_Gain4[2],
Crane_P.Saturation_LowerSat, Crane_P.Saturation_UpperSat);
Crane_B.Saturation[1] = rt_SATURATE(rtb_Gain4[0],
Crane_P.Saturation_LowerSat, Crane_P.Saturation_UpperSat);
Crane_B.Saturation[2] = rt_SATURATE(rtb_Gain4[1],
Crane_P.Saturation_LowerSat, Crane_P.Saturation_UpperSat);
if (rtmIsMajorTimeStep(Crane_M) &&
Crane_M->Timing.TaskCounters.TID[1] == 0) {
/* S-Function Block: Crane/Falcon (falcon_block) */
{
t_error result;
double force_vector[3];
double position[3];
t_int read_buttons;
force_vector[0] = Crane_B.Saturation[0];
force_vector[1] = Crane_B.Saturation[1];
force_vector[2] = Crane_B.Saturation[2];
/* read the position and buttons, and output the requested force to the falcon */
result = falcon_read_write(Crane_DWork.Falcon_Falcon, position,
&read_buttons, force_vector);
if (result < 0) {
msg_get_error_messageA(NULL, result, _rt_error_message, sizeof
(_rt_error_message));
rtmSetErrorStatus(Crane_M, _rt_error_message);
return;
}
rtb_Falcon_o1[0] = position[0];
rtb_Falcon_o1[1] = position[1];
rtb_Falcon_o1[2] = position[2];
rtb_Falcon_o2[0] = ((read_buttons & 0x01) != 0);
rtb_Falcon_o2[1] = ((read_buttons & 0x02) != 0);
rtb_Falcon_o2[2] = ((read_buttons & 0x04) != 0);
rtb_Falcon_o2[3] = ((read_buttons & 0x08) != 0);
}
/* Gain: '<Root>/Gain3' */
Crane_B.Gain3[0] = Crane_P.Gain3_Gain * rtb_Falcon_o1[0];
Crane_B.Gain3[1] = Crane_P.Gain3_Gain * rtb_Falcon_o1[1];
Crane_B.Gain3[2] = Crane_P.Gain3_Gain * rtb_Falcon_o1[2];
/* Constant: '<Root>/Constant' */
Crane_B.Constant = Crane_P.Constant_Value;
/* Stop: '<Root>/Stop Simulation' */
if (rtb_Falcon_o2[0] || rtb_Falcon_o2[1] || rtb_Falcon_o2[2] ||
rtb_Falcon_o2[3]) {
rtmSetStopRequested(Crane_M, 1);
}
/* Gain: '<Root>/Gain2' incorporates:
* Constant: '<Root>/Constant1'
*/
Crane_B.Gain2 = Crane_P.Gain2_Gain * Crane_P.Constant1_Value;
/* Gain: '<Root>/Gain1' incorporates:
* Constant: '<Root>/Constant3'
*/
Crane_B.Gain1 = Crane_P.Gain1_Gain * Crane_P.Constant3_Value;
}
/* Gain: '<Root>/Gain' incorporates:
* Sin: '<Root>/Sine Wave'
*/
Crane_B.Gain = (sin(Crane_P.SineWave_Freq * Crane_M->Timing.t[0] +
Crane_P.SineWave_Phase) * Crane_P.SineWave_Amp +
Crane_P.SineWave_Bias) * Crane_P.Gain_Gain;
if (rtmIsMajorTimeStep(Crane_M) &&
Crane_M->Timing.TaskCounters.TID[1] == 0) {
/* Gain: '<S10>/_gravity_conversion' incorporates:
* Constant: '<S8>/SOURCE_BLOCK'
*/
Crane_B._gravity_conversion[0] = Crane_P._gravity_conversion_Gain *
Crane_P.SOURCE_BLOCK_Value[0];
Crane_B._gravity_conversion[1] = Crane_P._gravity_conversion_Gain *
Crane_P.SOURCE_BLOCK_Value[1];
Crane_B._gravity_conversion[2] = Crane_P._gravity_conversion_Gain *
Crane_P.SOURCE_BLOCK_Value[2];
}
/* S-Function Block: <S10>/Block#2 */
{
_rtMech_PWORK *mechWork = (_rtMech_PWORK *) Crane_DWork.Block2_PWORK;
mechWork->genSimData.time = Crane_M->Timing.t[0];
mechWork->outSimData.majorTimestep = rtmIsMajorTimeStep(Crane_M);
if (dynamicSfcnOutputMethod(mechWork->mechanism, &(mechWork->genSimData),
&(mechWork->outSimData))) {
{
const ErrorRecord *err = getErrorMsg();
static char_T errorMsg[1024];
sprintf(errorMsg,
err->errorMsg,
err->blocks[0],
err->blocks[1],
err->blocks[2],
err->blocks[3],
err->blocks[4]);
rtmSetErrorStatus(Crane_M, errorMsg);
return;
}
}
}
/* S-Function Block: <S10>/Block#3 */
{
_rtMech_PWORK *mechWork = (_rtMech_PWORK *) Crane_DWork.Block3_PWORK;
mechWork->genSimData.time = Crane_M->Timing.t[0];
mechWork->outSimData.majorTimestep = rtmIsMajorTimeStep(Crane_M);
if (eventSfcnOutputMethod(mechWork->mechanism, &(mechWork->genSimData),
&(mechWork->outSimData))) {
{
const ErrorRecord *err = getErrorMsg();
static char_T errorMsg[1024];
sprintf(errorMsg,
err->errorMsg,
err->blocks[0],
err->blocks[1],
err->blocks[2],
err->blocks[3],
err->blocks[4]);
rtmSetErrorStatus(Crane_M, errorMsg);
return;
}
}
}
}
UNUSED_PARAMETER(tid);
}
/* Model output function */
void motor_io_position_output(void)
{
real_T temp;
ZCEventType zcEvent;
real_T u1;
real_T u2;
if (rtmIsMajorTimeStep(motor_io_position_M)) {
/* set solver stop time */
if (!(motor_io_position_M->Timing.clockTick0+1)) {
rtsiSetSolverStopTime(&motor_io_position_M->solverInfo,
((motor_io_position_M->Timing.clockTickH0 + 1) *
motor_io_position_M->Timing.stepSize0 * 4294967296.0));
} else {
rtsiSetSolverStopTime(&motor_io_position_M->solverInfo,
((motor_io_position_M->Timing.clockTick0 + 1) *
motor_io_position_M->Timing.stepSize0 +
motor_io_position_M->Timing.clockTickH0 *
motor_io_position_M->Timing.stepSize0 * 4294967296.0));
}
} /* end MajorTimeStep */
/* Update absolute time of base rate at minor time step */
if (rtmIsMinorTimeStep(motor_io_position_M)) {
motor_io_position_M->Timing.t[0] = rtsiGetT(&motor_io_position_M->solverInfo);
}
if (rtmIsMajorTimeStep(motor_io_position_M)) {
/* S-Function (rti_commonblock): '<S13>/S-Function1' */
/* This comment workarounds a code generation problem */
/* Gain: '<S5>/fi1_scaling' */
motor_io_position_B.fi1_scaling = motor_io_position_P.fi1_scaling_Gain *
motor_io_position_B.SFunction1;
/* DiscreteTransferFcn: '<Root>/Gfbreal' */
temp = motor_io_position_B.fi1_scaling;
temp -= motor_io_position_P.Gfbreal_DenCoef[1] *
motor_io_position_DW.Gfbreal_states[0];
temp -= motor_io_position_P.Gfbreal_DenCoef[2] *
motor_io_position_DW.Gfbreal_states[1];
temp /= motor_io_position_P.Gfbreal_DenCoef[0];
motor_io_position_DW.Gfbreal_tmp = temp;
temp = motor_io_position_P.Gfbreal_NumCoef[0] *
motor_io_position_DW.Gfbreal_tmp;
temp += motor_io_position_P.Gfbreal_NumCoef[1] *
motor_io_position_DW.Gfbreal_states[0];
temp += motor_io_position_P.Gfbreal_NumCoef[2] *
motor_io_position_DW.Gfbreal_states[1];
motor_io_position_B.Gfbreal = temp;
}
/* SignalGenerator: '<Root>/SinGenerator' */
motor_io_position_B.SinGenerator = sin
(motor_io_position_P.SinGenerator_Frequency * motor_io_position_M->Timing.t
[0]) * motor_io_position_P.SinGenerator_Amplitude;
/* SignalGenerator: '<Root>/SquareGenerator' */
temp = motor_io_position_P.SquareGenerator_Frequency *
motor_io_position_M->Timing.t[0];
if (temp - floor(temp) >= 0.5) {
motor_io_position_B.SquareGenerator =
motor_io_position_P.SquareGenerator_Amplitude;
} else {
motor_io_position_B.SquareGenerator =
-motor_io_position_P.SquareGenerator_Amplitude;
}
/* End of SignalGenerator: '<Root>/SquareGenerator' */
/* Switch: '<Root>/Switch' incorporates:
* Constant: '<Root>/SignalSelector[0Square,1Sine]'
*/
if (motor_io_position_P.SignalSelector0Square1Sine_Valu != 0.0) {
motor_io_position_B.ref = motor_io_position_B.SinGenerator;
} else {
motor_io_position_B.ref = motor_io_position_B.SquareGenerator;
}
/* End of Switch: '<Root>/Switch' */
if (rtmIsMajorTimeStep(motor_io_position_M)) {
/* DiscreteTransferFcn: '<Root>/Gffreal' */
temp = motor_io_position_B.ref;
temp -= motor_io_position_P.Gffreal_DenCoef[1] *
motor_io_position_DW.Gffreal_states[0];
temp -= motor_io_position_P.Gffreal_DenCoef[2] *
motor_io_position_DW.Gffreal_states[1];
temp /= motor_io_position_P.Gffreal_DenCoef[0];
motor_io_position_DW.Gffreal_tmp = temp;
temp = motor_io_position_P.Gffreal_NumCoef[0] *
motor_io_position_DW.Gffreal_tmp;
temp += motor_io_position_P.Gffreal_NumCoef[1] *
motor_io_position_DW.Gffreal_states[0];
temp += motor_io_position_P.Gffreal_NumCoef[2] *
motor_io_position_DW.Gffreal_states[1];
motor_io_position_B.Gffreal = temp;
/* Sum: '<Root>/Sum' */
motor_io_position_B.Sum = motor_io_position_B.Gffreal -
motor_io_position_B.Gfbreal;
/* Gain: '<Root>/Gain' */
motor_io_position_B.Gain = motor_io_position_P.Gain_Gain *
motor_io_position_B.Sum;
/* Saturate: '<S2>/Saturation' */
temp = motor_io_position_B.Gain;
u1 = motor_io_position_P.Saturation_LowerSat;
u2 = motor_io_position_P.Saturation_UpperSat;
if (temp > u2) {
motor_io_position_B.Volt = u2;
} else if (temp < u1) {
motor_io_position_B.Volt = u1;
} else {
motor_io_position_B.Volt = temp;
}
/* End of Saturate: '<S2>/Saturation' */
/* Gain: '<S2>/pwm_skalning' */
motor_io_position_B.pwm_skalning = motor_io_position_P.pwm_skalning_Gain *
motor_io_position_B.Volt;
/* Sum: '<S2>/Sum' incorporates:
* Constant: '<S2>/pwm_offstet'
*/
motor_io_position_B.Sum_f = motor_io_position_B.pwm_skalning +
motor_io_position_P.pwm_offstet_Value;
/* S-Function (rti_commonblock): '<S9>/S-Function1' */
/* This comment workarounds a code generation problem */
/* dSPACE I/O Board DS1104 #1 Unit:PWM Group:PWM */
ds1104_slave_dsp_pwm_duty_write(0, rti_slv1104_fcn_index[6],
motor_io_position_B.Sum_f);
/* S-Function (rti_commonblock): '<S9>/S-Function2' */
/* This comment workarounds a code generation problem */
/* S-Function (rti_commonblock): '<S9>/S-Function3' */
/* This comment workarounds a code generation problem */
/* S-Function (rti_commonblock): '<S9>/S-Function4' */
/* This comment workarounds a code generation problem */
/* DataTypeConversion: '<S2>/Data Type Conversion' incorporates:
* Constant: '<S2>/Enable[1_Off, 0_On]'
*/
motor_io_position_B.DataTypeConversion =
(motor_io_position_P.Enable1_Off0_On_Value != 0.0);
/* S-Function (rti_commonblock): '<S8>/S-Function1' */
/* This comment workarounds a code generation problem */
/* dSPACE I/O Board DS1104 #1 Unit:BIT_IO Group:BIT_OUT */
if (motor_io_position_B.DataTypeConversion > 0) {
ds1104_bit_io_set(DS1104_DIO0);
} else {
ds1104_bit_io_clear(DS1104_DIO0);
}
/* DiscreteTransferFcn: '<Root>/Gff' */
temp = motor_io_position_B.ref;
temp -= motor_io_position_P.Gff_DenCoef[1] *
motor_io_position_DW.Gff_states[0];
temp -= motor_io_position_P.Gff_DenCoef[2] *
motor_io_position_DW.Gff_states[1];
temp /= motor_io_position_P.Gff_DenCoef[0];
motor_io_position_DW.Gff_tmp = temp;
temp = motor_io_position_P.Gff_NumCoef[0] * motor_io_position_DW.Gff_tmp;
temp += motor_io_position_P.Gff_NumCoef[1] *
motor_io_position_DW.Gff_states[0];
temp += motor_io_position_P.Gff_NumCoef[2] *
motor_io_position_DW.Gff_states[1];
motor_io_position_B.Gff = temp;
}
/* Integrator: '<S1>/Integrator1' */
motor_io_position_B.Integrator1 = motor_io_position_X.Integrator1_CSTATE;
/* Quantizer: '<S4>/Quantizer' */
temp = motor_io_position_B.Integrator1;
motor_io_position_B.Quantizer = rt_roundd_snf(temp / motor_io_position_P.quant)
* motor_io_position_P.quant;
if (rtmIsMajorTimeStep(motor_io_position_M)) {
/* ZeroOrderHold: '<S4>/Zero-Order Hold' */
motor_io_position_B.ZeroOrderHold = motor_io_position_B.Quantizer;
/* DiscreteTransferFcn: '<Root>/Gfb' */
temp = motor_io_position_B.ZeroOrderHold;
temp -= motor_io_position_P.Gfb_DenCoef[1] *
motor_io_position_DW.Gfb_states[0];
temp -= motor_io_position_P.Gfb_DenCoef[2] *
motor_io_position_DW.Gfb_states[1];
temp /= motor_io_position_P.Gfb_DenCoef[0];
motor_io_position_DW.Gfb_tmp = temp;
temp = motor_io_position_P.Gfb_NumCoef[0] * motor_io_position_DW.Gfb_tmp;
temp += motor_io_position_P.Gfb_NumCoef[1] *
motor_io_position_DW.Gfb_states[0];
temp += motor_io_position_P.Gfb_NumCoef[2] *
motor_io_position_DW.Gfb_states[1];
motor_io_position_B.Gfb = temp;
/* Sum: '<Root>/Sum1' */
motor_io_position_B.Sum1 = motor_io_position_B.Gff - motor_io_position_B.Gfb;
/* Saturate: '<Root>/Saturation' */
temp = motor_io_position_B.Sum1;
u1 = motor_io_position_P.Saturation_LowerSat_c;
u2 = motor_io_position_P.Saturation_UpperSat_p;
if (temp > u2) {
motor_io_position_B.Saturation = u2;
} else if (temp < u1) {
motor_io_position_B.Saturation = u1;
} else {
motor_io_position_B.Saturation = temp;
}
/* End of Saturate: '<Root>/Saturation' */
}
/* Integrator: '<S1>/Integrator' */
motor_io_position_B.Integrator = motor_io_position_X.Integrator_CSTATE;
/* Gain: '<S1>/Gain1' */
motor_io_position_B.Gain1 = motor_io_position_P.Gain1_Gain *
motor_io_position_B.Integrator;
/* Sum: '<S1>/Add' */
motor_io_position_B.Add = motor_io_position_B.Saturation -
motor_io_position_B.Gain1;
/* Gain: '<S1>/k//R ' */
motor_io_position_B.kR = motor_io_position_P.kR_Gain * motor_io_position_B.Add;
/* Saturate: '<S6>/Saturate to Fc' */
temp = motor_io_position_B.kR;
u1 = motor_io_position_P.SaturatetoFc_LowerSat;
u2 = motor_io_position_P.F_c;
if (temp > u2) {
motor_io_position_B.Stickslipregion = u2;
} else if (temp < u1) {
motor_io_position_B.Stickslipregion = u1;
} else {
motor_io_position_B.Stickslipregion = temp;
}
/* End of Saturate: '<S6>/Saturate to Fc' */
/* Abs: '<S6>/Abs' */
motor_io_position_B.Abs = fabs(motor_io_position_B.Integrator);
/* RelationalOperator: '<S7>/Compare' incorporates:
* Constant: '<S7>/Constant'
*/
motor_io_position_B.Compare = (motor_io_position_B.Abs <=
motor_io_position_P.Constant_Value);
/* Gain: '<S6>/Vicous friction' */
motor_io_position_B.Vicousfriction = motor_io_position_P.Vicousfriction_Gain *
motor_io_position_B.Integrator;
/* Signum: '<S6>/Sign' */
temp = motor_io_position_B.Integrator;
if (temp < 0.0) {
motor_io_position_B.Sign = -1.0;
} else if (temp > 0.0) {
motor_io_position_B.Sign = 1.0;
} else if (temp == 0.0) {
motor_io_position_B.Sign = 0.0;
} else {
motor_io_position_B.Sign = temp;
}
/* End of Signum: '<S6>/Sign' */
/* Product: '<S6>/Product' incorporates:
* Constant: '<S6>/F_c'
*/
motor_io_position_B.Product = motor_io_position_P.F_c *
motor_io_position_B.Sign;
/* Sum: '<S6>/Add' */
motor_io_position_B.Viscousregion = motor_io_position_B.Vicousfriction +
motor_io_position_B.Product;
/* Switch: '<S6>/Switch' */
if (motor_io_position_B.Compare) {
motor_io_position_B.Friction = motor_io_position_B.Stickslipregion;
} else {
motor_io_position_B.Friction = motor_io_position_B.Viscousregion;
}
/* End of Switch: '<S6>/Switch' */
/* Sum: '<S1>/Add1' */
motor_io_position_B.Add1 = motor_io_position_B.kR -
motor_io_position_B.Friction;
if (rtmIsMajorTimeStep(motor_io_position_M)) {
/* Gain: '<S1>/Gain2' incorporates:
* Constant: '<S1>/Load inertia'
*/
motor_io_position_B.Gain2 = motor_io_position_P.Gain2_Gain *
motor_io_position_P.J1;
/* Sum: '<S1>/Add2' incorporates:
* Constant: '<S1>/Motor inertia'
*/
motor_io_position_B.Add2 = motor_io_position_B.Gain2 +
motor_io_position_P.Motorinertia_Value;
}
/* Product: '<S1>/Inertias 1//J' */
motor_io_position_B.Inertias1J = 1.0 / motor_io_position_B.Add2 *
motor_io_position_B.Add1;
if (rtmIsMajorTimeStep(motor_io_position_M)) {
}
/* Switch: '<S6>/Switch1' incorporates:
* Constant: '<S6>/Constant1'
* Constant: '<S6>/F_c'
*/
if (motor_io_position_B.Integrator > motor_io_position_P.Switch1_Threshold) {
motor_io_position_B.Switch1 = motor_io_position_P.F_c;
} else {
motor_io_position_B.Switch1 = motor_io_position_P.Constant1_Value;
}
/* End of Switch: '<S6>/Switch1' */
if (rtmIsMajorTimeStep(motor_io_position_M)) {
/* S-Function (rti_commonblock): '<S13>/S-Function2' */
/* This comment workarounds a code generation problem */
/* Gain: '<S5>/w1_scaling' */
motor_io_position_B.w1_scaling = motor_io_position_P.w1_scaling_Gain *
motor_io_position_B.SFunction2;
/* Outputs for Triggered SubSystem: '<S5>/DS1104ENC_SET_POS_C1' incorporates:
* TriggerPort: '<S15>/Trigger'
*/
if (rtmIsMajorTimeStep(motor_io_position_M)) {
/* Constant: '<S5>/Reset enc' */
zcEvent = rt_ZCFcn(RISING_ZERO_CROSSING,
&motor_io_position_PrevZCX.DS1104ENC_SET_POS_C1_Trig_ZCE,
(motor_io_position_P.Resetenc_Value));
if (zcEvent != NO_ZCEVENT) {
/* S-Function (rti_commonblock): '<S15>/S-Function1' */
/* This comment workarounds a code generation problem */
/* dSPACE I/O Board DS1104 Unit:ENC_SET */
ds1104_inc_position_write(1, 0, DS1104_INC_LINE_SUBDIV_4);
}
}
/* End of Outputs for SubSystem: '<S5>/DS1104ENC_SET_POS_C1' */
/* S-Function (rti_commonblock): '<S14>/S-Function1' */
/* This comment workarounds a code generation problem */
/* S-Function (rti_commonblock): '<S14>/S-Function2' */
/* This comment workarounds a code generation problem */
}
}
/*
* 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);
}
/* Model output function */
void Mechanics_output(int_T tid)
{
/* Update absolute time of base rate at minor time step */
if (rtmIsMinorTimeStep(Mechanics_M)) {
Mechanics_M->Timing.t[0] = rtsiGetT(&Mechanics_M->solverInfo);
}
if (rtmIsMajorTimeStep(Mechanics_M)) {
/* set solver stop time */
rtsiSetSolverStopTime(&Mechanics_M->solverInfo,
((Mechanics_M->Timing.clockTick0+1)*
Mechanics_M->Timing.stepSize0));
} /* end MajorTimeStep */
if (rtmIsMajorTimeStep(Mechanics_M) &&
Mechanics_M->Timing.TaskCounters.TID[1] == 0) {
/* Level2 S-Function Block: '<Root>/Arduino' (QueryInstrument) */
{
SimStruct *rts = Mechanics_M->childSfunctions[0];
sfcnOutputs(rts, 1);
}
/* Gain: '<Root>/Gain' */
Mechanics_B.Gain = Mechanics_P.Gain_Gain * Mechanics_B.Arduino;
}
/* S-Function Block: <S6>/Block#1 */
{
_rtMech_PWORK *mechWork = (_rtMech_PWORK *) Mechanics_DWork.Block1_PWORK;
mechWork->genSimData.time = Mechanics_M->Timing.t[0];
mechWork->outSimData.majorTimestep = rtmIsMajorTimeStep(Mechanics_M);
if (kinematicSfcnOutputMethod(mechWork->mechanism, &(mechWork->genSimData),
&(mechWork->outSimData))) {
{
const ErrorRecord *err = getErrorMsg();
static char_T errorMsg[1024];
sprintf(errorMsg,
err->errorMsg,
err->blocks[0],
err->blocks[1],
err->blocks[2],
err->blocks[3],
err->blocks[4]);
rtmSetErrorStatus(Mechanics_M, errorMsg);
return;
}
}
}
if (rtmIsMajorTimeStep(Mechanics_M) &&
Mechanics_M->Timing.TaskCounters.TID[1] == 0) {
/* Gain: '<S6>/_gravity_conversion' incorporates:
* Constant: '<S5>/SOURCE_BLOCK'
*/
Mechanics_B._gravity_conversion[0] = Mechanics_P._gravity_conversion_Gain *
Mechanics_P.SOURCE_BLOCK_Value[0];
Mechanics_B._gravity_conversion[1] = Mechanics_P._gravity_conversion_Gain *
Mechanics_P.SOURCE_BLOCK_Value[1];
Mechanics_B._gravity_conversion[2] = Mechanics_P._gravity_conversion_Gain *
Mechanics_P.SOURCE_BLOCK_Value[2];
}
/* S-Function Block: <S6>/Block#2 */
{
_rtMech_PWORK *mechWork = (_rtMech_PWORK *) Mechanics_DWork.Block2_PWORK;
mechWork->genSimData.time = Mechanics_M->Timing.t[0];
mechWork->outSimData.majorTimestep = rtmIsMajorTimeStep(Mechanics_M);
if (dynamicSfcnOutputMethod(mechWork->mechanism, &(mechWork->genSimData),
&(mechWork->outSimData))) {
{
const ErrorRecord *err = getErrorMsg();
static char_T errorMsg[1024];
sprintf(errorMsg,
err->errorMsg,
err->blocks[0],
err->blocks[1],
err->blocks[2],
err->blocks[3],
err->blocks[4]);
rtmSetErrorStatus(Mechanics_M, errorMsg);
return;
}
}
}
/* S-Function Block: <S6>/Block#3 */
{
_rtMech_PWORK *mechWork = (_rtMech_PWORK *) Mechanics_DWork.Block3_PWORK;
mechWork->genSimData.time = Mechanics_M->Timing.t[0];
mechWork->outSimData.majorTimestep = rtmIsMajorTimeStep(Mechanics_M);
if (eventSfcnOutputMethod(mechWork->mechanism, &(mechWork->genSimData),
&(mechWork->outSimData))) {
{
const ErrorRecord *err = getErrorMsg();
static char_T errorMsg[1024];
sprintf(errorMsg,
err->errorMsg,
err->blocks[0],
err->blocks[1],
err->blocks[2],
err->blocks[3],
err->blocks[4]);
rtmSetErrorStatus(Mechanics_M, errorMsg);
return;
}
}
}
UNUSED_PARAMETER(tid);
}
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);
}
/* Model output function */
void motor_io_output(void)
{
ZCEventType zcEvent;
real_T u0;
real_T u1;
real_T u2;
if (rtmIsMajorTimeStep(motor_io_M)) {
/* set solver stop time */
if (!(motor_io_M->Timing.clockTick0+1)) {
rtsiSetSolverStopTime(&motor_io_M->solverInfo,
((motor_io_M->Timing.clockTickH0 + 1) *
motor_io_M->Timing.stepSize0 * 4294967296.0));
} else {
rtsiSetSolverStopTime(&motor_io_M->solverInfo,
((motor_io_M->Timing.clockTick0 + 1) *
motor_io_M->Timing.stepSize0 + motor_io_M->Timing.clockTickH0 *
motor_io_M->Timing.stepSize0 * 4294967296.0));
}
} /* end MajorTimeStep */
/* Update absolute time of base rate at minor time step */
if (rtmIsMinorTimeStep(motor_io_M)) {
motor_io_M->Timing.t[0] = rtsiGetT(&motor_io_M->solverInfo);
}
if (rtmIsMajorTimeStep(motor_io_M)) {
/* S-Function (rti_commonblock): '<S14>/S-Function1' */
/* This comment workarounds a code generation problem */
/* Gain: '<S6>/fi1_scaling' */
motor_io_B.fi1_scaling = motor_io_P.fi1_scaling_Gain * motor_io_B.SFunction1;
/* MATLAB Function: '<Root>/Traject_and_Model_function 1' incorporates:
* Constant: '<Root>/RS'
* Constant: '<Root>/Reset enc'
*/
mot_Traject_and_Model_function1(motor_io_P.Resetenc_Value,
motor_io_P.RS_Value, motor_io_B.fi1_scaling,
&motor_io_B.sf_Traject_and_Model_function1,
&motor_io_DW.sf_Traject_and_Model_function1);
/* Gain: '<Root>/Gain' */
motor_io_B.Gain = motor_io_P.Gain_Gain *
motor_io_B.sf_Traject_and_Model_function1.volt;
/* Saturate: '<S2>/Saturation' */
u0 = motor_io_B.Gain;
u1 = motor_io_P.Saturation_LowerSat;
u2 = motor_io_P.Saturation_UpperSat;
if (u0 > u2) {
motor_io_B.Volt = u2;
} else if (u0 < u1) {
motor_io_B.Volt = u1;
} else {
motor_io_B.Volt = u0;
}
/* End of Saturate: '<S2>/Saturation' */
/* Gain: '<S2>/pwm_skalning' */
motor_io_B.pwm_skalning = motor_io_P.pwm_skalning_Gain * motor_io_B.Volt;
/* Sum: '<S2>/Sum' incorporates:
* Constant: '<S2>/pwm_offstet'
*/
motor_io_B.Sum = motor_io_B.pwm_skalning + motor_io_P.pwm_offstet_Value;
/* S-Function (rti_commonblock): '<S10>/S-Function1' */
/* This comment workarounds a code generation problem */
/* dSPACE I/O Board DS1104 #1 Unit:PWM Group:PWM */
ds1104_slave_dsp_pwm_duty_write(0, rti_slv1104_fcn_index[6], motor_io_B.Sum);
/* S-Function (rti_commonblock): '<S10>/S-Function2' */
/* This comment workarounds a code generation problem */
/* S-Function (rti_commonblock): '<S10>/S-Function3' */
/* This comment workarounds a code generation problem */
/* S-Function (rti_commonblock): '<S10>/S-Function4' */
/* This comment workarounds a code generation problem */
/* DataTypeConversion: '<S2>/Data Type Conversion' incorporates:
* Constant: '<S2>/Enable[1_Off, 0_On]'
*/
motor_io_B.DataTypeConversion = (motor_io_P.Enable1_Off0_On_Value != 0.0);
/* S-Function (rti_commonblock): '<S9>/S-Function1' */
/* This comment workarounds a code generation problem */
/* dSPACE I/O Board DS1104 #1 Unit:BIT_IO Group:BIT_OUT */
if (motor_io_B.DataTypeConversion > 0) {
ds1104_bit_io_set(DS1104_DIO0);
} else {
ds1104_bit_io_clear(DS1104_DIO0);
}
}
/* Integrator: '<S1>/Integrator1' */
motor_io_B.Integrator1 = motor_io_X.Integrator1_CSTATE;
if (rtmIsMajorTimeStep(motor_io_M)) {
/* MATLAB Function: '<Root>/Traject_and_Model_function 3' incorporates:
* Constant: '<Root>/RS'
* Constant: '<Root>/Reset enc'
*/
mot_Traject_and_Model_function1(motor_io_P.Resetenc_Value,
motor_io_P.RS_Value, motor_io_B.Integrator1,
&motor_io_B.sf_Traject_and_Model_function3,
&motor_io_DW.sf_Traject_and_Model_function3);
}
/* Integrator: '<S1>/Integrator' */
motor_io_B.Integrator = motor_io_X.Integrator_CSTATE;
/* Gain: '<S1>/Gain1' */
motor_io_B.Gain1 = motor_io_P.Gain1_Gain * motor_io_B.Integrator;
/* Sum: '<S1>/Add' */
motor_io_B.Add = motor_io_B.sf_Traject_and_Model_function3.volt -
motor_io_B.Gain1;
/* Gain: '<S1>/k//R ' */
motor_io_B.kR = motor_io_P.kR_Gain * motor_io_B.Add;
/* Saturate: '<S7>/Saturate to Fc' */
u0 = motor_io_B.kR;
u1 = motor_io_P.SaturatetoFc_LowerSat;
u2 = motor_io_P.F_c_upper;
if (u0 > u2) {
motor_io_B.Stickslipregion = u2;
} else if (u0 < u1) {
motor_io_B.Stickslipregion = u1;
} else {
motor_io_B.Stickslipregion = u0;
}
/* End of Saturate: '<S7>/Saturate to Fc' */
/* Abs: '<S7>/Abs' */
motor_io_B.Abs = fabs(motor_io_B.Integrator);
/* RelationalOperator: '<S8>/Compare' incorporates:
* Constant: '<S8>/Constant'
*/
motor_io_B.Compare = (motor_io_B.Abs <= motor_io_P.Constant_Value);
/* Gain: '<S7>/Vicous friction' */
motor_io_B.Vicousfriction = motor_io_P.Vicousfriction_Gain *
motor_io_B.Integrator;
/* Signum: '<S7>/Sign' */
u0 = motor_io_B.Integrator;
if (u0 < 0.0) {
motor_io_B.Sign = -1.0;
} else if (u0 > 0.0) {
motor_io_B.Sign = 1.0;
} else if (u0 == 0.0) {
motor_io_B.Sign = 0.0;
} else {
motor_io_B.Sign = u0;
}
/* End of Signum: '<S7>/Sign' */
/* Product: '<S7>/Product' incorporates:
* Constant: '<S7>/F_c'
*/
motor_io_B.Product = motor_io_P.F_c * motor_io_B.Sign;
/* Sum: '<S7>/Add' */
motor_io_B.Viscousregion = motor_io_B.Vicousfriction + motor_io_B.Product;
/* Switch: '<S7>/Switch' */
if (motor_io_B.Compare) {
motor_io_B.Friction = motor_io_B.Stickslipregion;
} else {
motor_io_B.Friction = motor_io_B.Viscousregion;
}
/* End of Switch: '<S7>/Switch' */
/* Sum: '<S1>/Add1' */
motor_io_B.Add1 = motor_io_B.kR - motor_io_B.Friction;
if (rtmIsMajorTimeStep(motor_io_M)) {
/* Gain: '<S1>/Gain2' incorporates:
* Constant: '<S1>/Load inertia'
*/
motor_io_B.Gain2 = motor_io_P.Gain2_Gain * motor_io_P.J1;
/* Sum: '<S1>/Add2' incorporates:
* Constant: '<S1>/Motor inertia'
*/
motor_io_B.Add2 = motor_io_B.Gain2 + motor_io_P.Motorinertia_Value;
}
/* Product: '<S1>/Inertias 1//J' */
motor_io_B.Inertias1J = 1.0 / motor_io_B.Add2 * motor_io_B.Add1;
if (rtmIsMajorTimeStep(motor_io_M)) {
/* S-Function (rti_commonblock): '<S14>/S-Function2' */
/* This comment workarounds a code generation problem */
/* Gain: '<S6>/w1_scaling' */
motor_io_B.w1_scaling = motor_io_P.w1_scaling_Gain * motor_io_B.SFunction2;
/* Outputs for Triggered SubSystem: '<S6>/DS1104ENC_SET_POS_C1' incorporates:
* TriggerPort: '<S16>/Trigger'
*/
if (rtmIsMajorTimeStep(motor_io_M)) {
/* Constant: '<S6>/Reset enc' */
zcEvent = rt_ZCFcn(RISING_ZERO_CROSSING,
&motor_io_PrevZCX.DS1104ENC_SET_POS_C1_Trig_ZCE,
(motor_io_P.Resetenc_Value_k));
if (zcEvent != NO_ZCEVENT) {
/* S-Function (rti_commonblock): '<S16>/S-Function1' */
/* This comment workarounds a code generation problem */
/* dSPACE I/O Board DS1104 Unit:ENC_SET */
ds1104_inc_position_write(1, 0, DS1104_INC_LINE_SUBDIV_4);
}
}
/* End of Outputs for SubSystem: '<S6>/DS1104ENC_SET_POS_C1' */
/* S-Function (rti_commonblock): '<S15>/S-Function1' */
/* This comment workarounds a code generation problem */
/* S-Function (rti_commonblock): '<S15>/S-Function2' */
/* This comment workarounds a code generation problem */
}
/* SignalGenerator: '<Root>/SinGenerator' */
motor_io_B.SinGenerator = sin(motor_io_P.SinGenerator_Frequency *
motor_io_M->Timing.t[0]) * motor_io_P.SinGenerator_Amplitude;
/* SignalGenerator: '<Root>/SquareGenerator' */
u0 = motor_io_P.SquareGenerator_Frequency * motor_io_M->Timing.t[0];
if (u0 - floor(u0) >= 0.5) {
motor_io_B.SquareGenerator = motor_io_P.SquareGenerator_Amplitude;
} else {
motor_io_B.SquareGenerator = -motor_io_P.SquareGenerator_Amplitude;
}
/* End of SignalGenerator: '<Root>/SquareGenerator' */
/* Switch: '<Root>/Switch' incorporates:
* Constant: '<Root>/SignalSelector[0Square,1Sine]'
*/
if (motor_io_P.SignalSelector0Square1Sine_Valu != 0.0) {
motor_io_B.Switch = motor_io_B.SinGenerator;
} else {
motor_io_B.Switch = motor_io_B.SquareGenerator;
}
/* End of Switch: '<Root>/Switch' */
}
/* 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 */
}