void MdlStart(void)
{
{
/* user code (Start function Header) */
testSHM_InitializeBlockIOMap();
testSHM_InitializeParametersMap();
/* Level2 S-Function Block: '<Root>/S-Function' (sSHM) */
{
SimStruct *rts = testSHM_M->childSfunctions[0];
sfcnStart(rts);
if (ssGetErrorStatus(rts) != (NULL))
return;
}
/* Level2 S-Function Block: '<Root>/RTAI_SCOPE' (sfun_rtai_scope) */
{
SimStruct *rts = testSHM_M->childSfunctions[1];
sfcnStart(rts);
if (ssGetErrorStatus(rts) != (NULL))
return;
}
}
MdlInitialize();
}
/* Outputs for root system: '<Root>' */
static void mdlOutputs(SimStruct *S, int_T tid)
{
BlockIO_Hammerstein *_rtB;
_rtB = ((BlockIO_Hammerstein *) ssGetLocalBlockIO(S));
/* DiscreteStateSpace: '<S1>/LinearModel' */
{
((BlockIO_Hammerstein *) ssGetLocalBlockIO(S))->LinearModel = 1.0*((real_T*)
ssGetDWork(S, 0))[0];
}
/* Level2 S-Function Block: '<S1>/Pwlinear1' (sfunpwlinear) */
{
SimStruct *rts = ssGetSFunction(S, 0);
sfcnOutputs(rts, 0);
if (ssGetErrorStatus(rts) != (NULL))
return;
}
/* Outport: '<Root>/Out1' */
((real_T *)ssGetOutputPortSignal(S, 0))[0] = _rtB->Pwlinear1;
/* Level2 S-Function Block: '<S1>/Pwlinear' (sfunpwlinear) */
{
SimStruct *rts = ssGetSFunction(S, 1);
sfcnOutputs(rts, 0);
if (ssGetErrorStatus(rts) != (NULL))
return;
}
/* tid is required for a uniform function interface.
* Argument tid is not used in the function. */
UNUSED_PARAMETER(tid);
}
/* Model initialize function */
void RA4_student_initialize(void)
{
/* RTW Generated Level2 S-Function Block: '<S2>/Robot Arm_sfcn' (Robot_sf) */
{
SimStruct *rts = RA4_student_M->childSfunctions[1];
sfcnStart(rts);
if (ssGetErrorStatus(rts) != (NULL))
return;
}
/* VirtualOutportStart for Outport: '<S1>/Solenoid' */
RA4_student_B.ReferenceSolenoid = RA4_student_P.Solenoid_Y0;
/* End of Start for SubSystem: '<Root>/Controller' */
/* InitializeConditions for UnitDelay: '<Root>/Unit Delay2' */
RA4_student_DW.UnitDelay2_DSTATE[0] =
RA4_student_P.UnitDelay2_InitialCondition;
RA4_student_DW.UnitDelay2_DSTATE[1] =
RA4_student_P.UnitDelay2_InitialCondition;
RA4_student_DW.UnitDelay2_DSTATE[2] =
RA4_student_P.UnitDelay2_InitialCondition;
/* InitializeConditions for UnitDelay: '<Root>/Unit Delay1' */
RA4_student_DW.UnitDelay1_DSTATE = RA4_student_P.UnitDelay1_InitialCondition;
/* RTW Generated Level2 S-Function Block: '<S2>/Robot Arm_sfcn' (Robot_sf) */
{
SimStruct *rts = RA4_student_M->childSfunctions[1];
sfcnInitializeConditions(rts);
if (ssGetErrorStatus(rts) != (NULL))
return;
}
}
void MdlStart(void)
{
/* Level2 S-Function Block: '<Root>/PCI-6221 AD' (adnipcim) */
{
SimStruct *rts = xpcosc_rtM->childSfunctions[0];
sfcnStart(rts);
if (ssGetErrorStatus(rts) != (NULL))
return;
}
/* Level2 S-Function Block: '<Root>/PCI-6713 DA' (danipci671x) */
{
SimStruct *rts = xpcosc_rtM->childSfunctions[1];
sfcnStart(rts);
if (ssGetErrorStatus(rts) != (NULL))
return;
}
/* S-Function Block: <S1>/S-Function (scblock) */
{
int i;
if ((i = rl32eScopeExists(1)) == 0) {
if ((i = rl32eDefScope(1,2)) != 0) {
printf("Error creating scope 1\n");
} else {
rl32eAddSignal(1, rl32eGetSignalNo("Integrator1"));
rl32eAddSignal(1, rl32eGetSignalNo("Rate Transition1"));
rl32eSetScope(1, 4, 250);
rl32eSetScope(1, 40, 0);
rl32eSetScope(1, 7, 1);
rl32eSetScope(1, 0, 0);
rl32eSetScope(1, 3, rl32eGetSignalNo("Integrator1"));
rl32eSetScope(1, 1, 0.0);
rl32eSetScope(1, 2, 0);
rl32eSetScope(1, 10, 0);
rl32eSetTargetScope(1, 11, 0.0);
rl32eSetTargetScope(1, 10, 0.0);
xpceScopeAcqOK(1, &xpcosc_DWork.SFunction_IWORK.AcquireOK);
}
}
if (i) {
rl32eRestartAcquisition(1);
}
}
MdlInitialize();
}
static void mdlInitializeSizes(SimStruct *S)
{
uint_T i;
ssSetNumSFcnParams(S, NUMBER_OF_PARAMS);
#if defined(MATLAB_MEX_FILE)
if (ssGetNumSFcnParams(S) == ssGetSFcnParamsCount(S)) {
mdlCheckParameters(S);
if (ssGetErrorStatus(S) != NULL) {
return;
}
} else {
return;
}
#endif
for (i = 0; i < NUMBER_OF_PARAMS; i++) {
ssSetSFcnParamNotTunable(S, i);
}
ssSetNumInputPorts(S, 0);
ssSetNumOutputPorts(S, 1);
ssSetOutputPortWidth(S, 0, 1);
ssSetNumContStates(S, 0);
ssSetNumDiscStates(S, 0);
ssSetNumSampleTimes(S, 1);
ssSetNumPWork(S,1);
ssSetNumIWork(S,1);
ssSetNumRWork(S,2);
}
/* Function: mdlInitializeSizes ===============================================
* Abstract:
* The sizes information is used by Simulink to determine the S-function
* block's characteristics (number of inputs, outputs, states, etc.).
*/
static void mdlInitializeSizes(SimStruct *S)
{
/* See sfuntmpl_doc.c for more details on the macros below */
ssSetNumSFcnParams(S, NUM_PARAMS); /* Number of expected parameters */
if (ssGetNumSFcnParams(S) == ssGetSFcnParamsCount(S))
{
mdlCheckParameters(S);
if (ssGetErrorStatus(S) != NULL)
/* Return if number of expected != number of actual parameters */
return;
}
else
return;
ssSetNumContStates(S, 0);
ssSetNumDiscStates(S, 0);
ssSetNumInputPorts(S, 0);
ssSetNumOutputPorts(S, 0);
ssSetNumSampleTimes(S, 1);
ssSetSFcnParamTunable(S, 0, 0);
ssSetSFcnParamTunable(S, 1, 0);
ssSetSFcnParamTunable(S, 2, 0);
ssSetOptions(S, SS_OPTION_EXCEPTION_FREE_CODE | SS_OPTION_CAN_BE_CALLED_CONDITIONALLY);
}
/* Function: rsimOneStepST =====================================================
*
* Perform one step of the model.
* Errors are set in the SimStruct's ErrorStatus, NULL means no errors.
*/
static void rsimOneStepST(SimStruct *S)
{
ssSetSimTimeStep(S, MAJOR_TIME_STEP);
/* Check if the solver found ZC event.
* if yes, this is a major step at right post */
if ( ssGetSolverFoundContZcEvents(S) ){
ssSetSolverIsAtRightPostOfContZcEvent(S, true);
}
/* Clear the flag that blocks set in their major time step methods (output,
update, etc.) to flag that they have changed their state. If this flag is
true then we need to run model outputs again at the first minor step
(i.e., at the same time as the last major step).*/
ssClearBlkStateChange(S);
rsimOutputLogUpdate(S);
/* Done with the step. Set the right post flag to false
* It is immaterial whether this step was really a right post step */
ssSetSolverIsAtRightPostOfContZcEvent(S, false);
if (ssGetErrorStatus(S) != NULL) return;
/* Call rsimAdvanceSolver only if no stop was requested (540586) */
if (!ssGetStopRequested(S)) rsimAdvanceSolver(S);
} /* rsimOneStepST */
/*=====================================*
* Configuration and execution methods *
*=====================================*/
static void mdlInitializeSizes(SimStruct *S)
{
#ifdef DEBUG
debug("mdlInitializeSizes entered.\n");
#endif
ssSetNumSFcnParams(S, NPAR); /* Number of expected parameters */
if (ssGetNumSFcnParams(S) != ssGetSFcnParamsCount(S)) {
mdlCheckParameters(S);
if (ssGetErrorStatus(S) != NULL) return;
}
ssSetNumContStates(S, 0);
ssSetNumDiscStates(S, 0);
if (!ssSetNumInputPorts(S, 1)) return;
ssSetInputPortWidth(S, 0, NI);
ssSetInputPortDirectFeedThrough(S, 0, 1);
if (!ssSetNumOutputPorts(S, 1)) return;
ssSetOutputPortWidth(S, 0, NO);
ssSetNumSampleTimes(S, 1);
ssSetNumRWork(S, 0);
ssSetNumIWork(S, 0);
ssSetNumPWork(S, 0);
ssSetNumModes(S, 0);
ssSetNumNonsampledZCs(S, 0);
/* Take care when specifying exception free code - see sfuntmpl_doc.c */
ssSetOptions(S, SS_OPTION_EXCEPTION_FREE_CODE);
#ifdef DEBUG
debug("mdlInitializeSizes left.\n");
#endif
} /* mdlInitializeSizes */
static void mdlInitializeSizes( SimStruct *S )
/* ======================================================================== */
{
int_T port;
/* set number of expected parameters and check for a mismatch. */
ssSetNumSFcnParams( S, NUM_PARAMS );
#if defined(MATLAB_MEX_FILE)
if ( ssGetNumSFcnParams( S ) == ssGetSFcnParamsCount( S ) )
{
mdlCheckParameters( S );
if ( ssGetErrorStatus( S ) != NULL )
return;
}else {
return;
}
#endif
/* sampling */
ssSetNumSampleTimes( S, PORT_BASED_SAMPLE_TIMES );
/* Set number of input ports and tunability */
ssSetSFcnParamTunable( S, DEVICE_INDEX, SS_PRM_NOT_TUNABLE );
/* set the resulting number of ports */
if ( !ssSetNumInputPorts( S, 1 ) )
return;
port = 0;
{
const Frame_T inputsFrames = ( (double) mxGetScalar( ssGetSFcnParam( S, USE_FRAMES ) ) > 0.0) ? FRAME_YES : FRAME_NO;
double sample_time = 1 / mxGetScalar( ssGetSFcnParam( S, SAMPLE_RATE ) );
const int_T buf_length = (int_T) (double) mxGetScalar( ssGetSFcnParam( S, FRAME_LENGTH ) );
const time_T period = (time_T) (sample_time * buf_length);
ssSetInputPortMatrixDimensions( S, port, buf_length, 1 );
ssSetInputPortComplexSignal( S, port, COMPLEX_YES );
ssSetInputPortDataType( S, port, SS_DOUBLE );
ssSetInputPortFrameData( S, port, inputsFrames );
ssSetInputPortDirectFeedThrough( S, port, 1 );
ssSetInputPortSampleTime( S, port, period );
ssSetInputPortOffsetTime( S, port, 0.0 );
}
/* Set number of output ports */
if ( !ssSetNumOutputPorts( S, 0 ) )
return;
/* data port properties */
/* Prepare work Vectors */
ssSetNumPWork( S, P_WORK_LENGTH );
ssSetNumIWork( S, I_WORK_LENGTH );
ssSetNumRWork( S, R_WORK_LENGTH );
ssSetNumModes( S, 0 );
ssSetNumNonsampledZCs( S, 0 );
/* Specify the sim state compliance to be same as a built-in block */
ssSetSimStateCompliance( S, USE_DEFAULT_SIM_STATE );
ssSetOptions( S, 0 );
}
void mdlInitializeSizes(SimStruct *S)
/* ======================================================================== */
{
/* parameters */
ssSetNumSFcnParams(S, NUM_PARAMS);
#if defined(MATLAB_MEX_FILE)
if (ssGetNumSFcnParams(S) == ssGetSFcnParamsCount(S)) {
mdlCheckParameters(S);
if (ssGetErrorStatus(S) != NULL) return;
} else
return;
#endif
ssSetSFcnParamTunable(S, FREQUENCY, SS_PRM_SIM_ONLY_TUNABLE);
ssSetSFcnParamTunable(S, BANDWIDTH, SS_PRM_NOT_TUNABLE);
ssSetSFcnParamTunable(S, TXVGA_GAIN, SS_PRM_SIM_ONLY_TUNABLE);
/* ports */
ssSetNumSampleTimes(S, 1);
if (!ssSetNumOutputPorts(S, 0) || !ssSetNumInputPorts(S, 1)) return;
ssSetInputPortWidth(S, 0, DYNAMICALLY_SIZED);
ssSetInputPortComplexSignal(S, 0, COMPLEX_YES);
ssSetInputPortDataType(S, 0, SS_INT8);
ssSetInputPortDirectFeedThrough(S, 0, true);
ssSetInputPortOptimOpts(S, 0, SS_REUSABLE_AND_LOCAL);
/* work Vectors */
ssSetNumPWork(S, P_WORK_LENGTH);
ssSetNumIWork(S, 0);
ssSetNumRWork(S, NUM_PARAMS);
ssSetNumModes(S, 0);
ssSetNumNonsampledZCs(S, 0);
ssSetSimStateCompliance(S, USE_DEFAULT_SIM_STATE);
ssSetOptions(S, 0);
}
/* Model initialize function */
void AD_model_initialize(void)
{
/* Level2 S-Function Block: '<Root>/Get_Parameter ' (AD_v2) */
{
SimStruct *rts = AD_model_M->childSfunctions[0];
sfcnStart(rts);
if (ssGetErrorStatus(rts) != (NULL))
return;
}
}
/* Helper function to make function calls from non-inlined S-functions. */
int_T rt_CallSys(SimStruct *S, int_T element, int_T tid)
{
(*(S)->callSys.fcns[element])((S)->callSys.args1[element],
(S)->callSys.args2[element], tid);
if (ssGetErrorStatus(S) != (NULL)) {
return 0;
} else {
return 1;
}
}
/* Model initialize function */
void DI_model_initialize(void)
{
/* Level2 S-Function Block: '<Root>/S-Function' (DI_v1) */
{
SimStruct *rts = DI_model_M->childSfunctions[0];
sfcnStart(rts);
if (ssGetErrorStatus(rts) != (NULL))
return;
}
}
/* Model initialize function */
void test_mdl_initialize(void)
{
/* Level2 S-Function Block: '<Root>/S-Function' (phy_to_lnr) */
{
SimStruct *rts = test_mdl_M->childSfunctions[0];
sfcnStart(rts);
if (ssGetErrorStatus(rts) != (NULL))
return;
}
}
static void mdlInitializeSizes(SimStruct *S)
{
const real_T *P1 = mxGetData(PARAM_DEF3(S));
const real_T *P2 = mxGetData(PARAM_DEF2(S));
DECL_AND_INIT_DIMSINFO(outputDimsInfo);
ssSetNumSFcnParams(S, NPARAMS); /* Number of expected parameters */
#if defined(MATLAB_MEX_FILE)
if (ssGetNumSFcnParams(S) == ssGetSFcnParamsCount(S)) {
if (ssGetErrorStatus(S) != NULL) {
return;
}
} else {
return; /* Parameter mismatch will be reported by Simulink */
}
#endif
ssSetNumContStates(S, NUM_CONT_STATES);
ssSetNumDiscStates(S, NUM_DISC_STATES);
if (!ssSetNumInputPorts(S, NUM_INPUTS)) return;
if (!ssSetNumOutputPorts(S, NUM_OUTPUTS)) return;
/* Output Port 0 */
outputDimsInfo.width = *P1;
ssSetOutputPortDimensionInfo(S, 0, &outputDimsInfo);
ssSetOutputPortMatrixDimensions( S ,0, *P1, *P2);
ssSetOutputPortFrameData(S, 0, OUT_0_FRAME_BASED);
ssSetOutputPortDataType(S, 0, SS_UINT8);
ssSetOutputPortComplexSignal(S, 0, OUTPUT_0_COMPLEX);
/* Output Port 1 */
outputDimsInfo.width = *P1;
ssSetOutputPortDimensionInfo(S, 1, &outputDimsInfo);
ssSetOutputPortMatrixDimensions( S ,1, *P1, *P2);
ssSetOutputPortFrameData(S, 1, OUT_1_FRAME_BASED);
ssSetOutputPortDataType(S, 1, SS_UINT8);
ssSetOutputPortComplexSignal(S, 1, OUTPUT_1_COMPLEX);
/* Output Port 2 */
outputDimsInfo.width = *P1;
ssSetOutputPortDimensionInfo(S, 2, &outputDimsInfo);
ssSetOutputPortMatrixDimensions( S ,2, *P1, *P2);
ssSetOutputPortFrameData(S, 2, OUT_2_FRAME_BASED);
ssSetOutputPortDataType(S, 2, SS_UINT8);
ssSetOutputPortComplexSignal(S, 2, OUTPUT_2_COMPLEX);
ssSetNumSampleTimes(S, 1);
ssSetNumRWork(S, 0);
ssSetNumIWork(S, 0);
ssSetNumPWork(S, 0);
ssSetNumModes(S, 0);
ssSetNumNonsampledZCs(S, 0);
/* Take care when specifying exception free code - see sfuntmpl_doc.c */
ssSetOptions(S, (SS_OPTION_EXCEPTION_FREE_CODE |
SS_OPTION_WORKS_WITH_CODE_REUSE));
}
/* Model update function */
void RA4_student_update(void)
{
/* Update for UnitDelay: '<Root>/Unit Delay2' */
RA4_student_DW.UnitDelay2_DSTATE[0] = RA4_student_B.Sum4;
RA4_student_DW.UnitDelay2_DSTATE[1] = RA4_student_B.Sum5;
RA4_student_DW.UnitDelay2_DSTATE[2] = RA4_student_B.Sum6;
/* Update for UnitDelay: '<Root>/Unit Delay1' */
RA4_student_DW.UnitDelay1_DSTATE = RA4_student_B.ReferenceSolenoid;
/* RTW Generated Level2 S-Function Block: '<S2>/Robot Arm_sfcn' (Robot_sf) */
{
SimStruct *rts = RA4_student_M->childSfunctions[1];
sfcnUpdate(rts, 0);
if (ssGetErrorStatus(rts) != (NULL))
return;
}
/* 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 (!(++RA4_student_M->Timing.clockTick0)) {
++RA4_student_M->Timing.clockTickH0;
}
RA4_student_M->Timing.t[0] = RA4_student_M->Timing.clockTick0 *
RA4_student_M->Timing.stepSize0 + RA4_student_M->Timing.clockTickH0 *
RA4_student_M->Timing.stepSize0 * 4294967296.0;
{
/* Update absolute timer for sample time: [0.000244140625s, 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 (!(++RA4_student_M->Timing.clockTick1)) {
++RA4_student_M->Timing.clockTickH1;
}
RA4_student_M->Timing.t[1] = RA4_student_M->Timing.clockTick1 *
RA4_student_M->Timing.stepSize1 + RA4_student_M->Timing.clockTickH1 *
RA4_student_M->Timing.stepSize1 * 4294967296.0;
}
}
/* Helper function to make function disables from non-inlined S-functions. */
int_T rt_DisableSys(SimStruct *S, int_T element, int_T tid)
{
(*(S)->callSys.fcns[3*ssGetOutputPortWidth(S,0)+element])
((S)->callSys.args1[element],
(S)->callSys.args2[element], tid);
if (ssGetErrorStatus(S) != (NULL)) {
return 0;
} else {
return 1;
}
}
/* Function: mdlInitializeSizes ===============================================
* Abstract:
* The sizes information is used by Simulink to determine the S-function
* block's characteristics (number of inputs, outputs, states, etc.).
*/
static void mdlInitializeSizes(SimStruct *S) {
/* See sfuntmpl_doc.c for more details on the macros below */
ssSetNumSFcnParams(S, 2); /* Number of expected parameters */
#if defined(MATLAB_MEX_FILE)
if (ssGetNumSFcnParams(S) == ssGetSFcnParamsCount(S)) {
mdlCheckParameters(S);
if (ssGetErrorStatus(S) != NULL) {
return;
}
} else {
return; /* Parameter mismatch will be reported by Simulink. */
}
#endif
ssSetNumContStates(S, 0);
ssSetNumDiscStates(S, 0);
if (!ssSetNumInputPorts(S, 2))
return;
ssSetInputPortMatrixDimensions(S, 0, DYNAMICALLY_SIZED, DYNAMICALLY_SIZED); // triangles
ssSetInputPortDataType(S, 0, SS_UINT32);
ssSetInputPortMatrixDimensions(S, 1, DYNAMICALLY_SIZED, DYNAMICALLY_SIZED); // vertices
for (int_T i = 0; i < ssGetNumInputPorts(S); ++i) {
/*direct input signal access*/
ssSetInputPortRequiredContiguous(S, i, true);
/*
* Set direct feedthrough flag (1=yes, 0=no).
* A port has direct feedthrough if the input is used in either
* the mdlOutputs or mdlGetTimeOfNextVarHit functions.
* See matlabroot/simulink/src/sfuntmpl_directfeed.txt.
*/
ssSetInputPortDirectFeedThrough(S, i, 1);
}
if (!ssSetNumOutputPorts(S, 0))
return;
ssSetNumSampleTimes(S, 1);
ssSetNumRWork(S, 0);
ssSetNumIWork(S, 0);
ssSetNumPWork(S, 2); //GenericPub and frame id
ssSetNumModes(S, 0);
ssSetNumNonsampledZCs(S, 0);
/* Specify the sim state compliance to be same as a built-in block */
ssSetSimStateCompliance(S, USE_DEFAULT_SIM_STATE);
ssSetOptions(S, 0);
}
void MdlStart(void)
{
/* Level2 S-Function Block: '<Root>/Arduino' (QueryInstrument) */
{
SimStruct *rts = Mechanics_M->childSfunctions[0];
sfcnStart(rts);
if (ssGetErrorStatus(rts) != NULL)
return;
}
MdlInitialize();
}
/* Function: mdlInitializeSizes ===============================================
* Abstract:
* The sizes information is used by Simulink to determine the S-function
* block's characteristics (number of inputs, outputs, states, etc.).
*/
static void mdlInitializeSizes(SimStruct *S)
{
/* See sfuntmpl_doc.c for more details on the macros below */
ssSetNumSFcnParams(S, 5); /* Number of expected parameters */
#if defined(MATLAB_MEX_FILE)
if (ssGetNumSFcnParams(S) == ssGetSFcnParamsCount(S)) {
mdlCheckParameters(S);
if (ssGetErrorStatus(S) != NULL) {
return;
}
} else {
return; /* Parameter mismatch will be reported by Simulink. */
}
#endif
int_T nRobots = mxGetNumberOfElements(ssGetSFcnParam(S,2));
ssSetNumContStates(S, 0);
ssSetNumDiscStates(S, 0);
if (!ssSetNumInputPorts(S, 0)) return;
//ssSetInputPortWidth(S, 0, 1);
//ssSetInputPortRequiredContiguous(S, 0, true); /*direct input signal access*/
/*
* Set direct feedthrough flag (1=yes, 0=no).
* A port has direct feedthrough if the input is used in either
* the mdlOutputs or mdlGetTimeOfNextVarHit functions.
* See matlabroot/simulink/src/sfuntmpl_directfeed.txt.
*/
//ssSetInputPortDirectFeedThrough(S, 0, 1);
if (!ssSetNumOutputPorts(S, 4)) return;
const int_T nDofs = *((mxChar*)mxGetData(ssGetSFcnParam(S, 4)));
ssSetOutputPortMatrixDimensions(S, 0, nDofs, nRobots); // position
ssSetOutputPortMatrixDimensions(S, 1, nDofs, nRobots); // velocity
ssSetOutputPortMatrixDimensions(S, 2, nDofs, nRobots); // effort
ssSetOutputPortMatrixDimensions(S, 3, 1, nRobots); // SentTime in s;
ssSetNumSampleTimes(S, 1);
ssSetNumRWork(S, 0);
ssSetNumIWork(S, 2); // nRobots, nDofs
ssSetNumPWork(S, nRobots + 1); //nRobots x GenericSub, AsyncSpinner
ssSetNumModes(S, 0);
ssSetNumNonsampledZCs(S, 0);
/* Specify the sim state compliance to be same as a built-in block */
ssSetSimStateCompliance(S, USE_DEFAULT_SIM_STATE);
ssSetOptions(S, 0);
}
// Function: mdlInitializeSizes ===============================================
// Abstract:
// The sizes information is used by Simulink to determine the S-function
// block's characteristics (number of inputs, s, states, etc.).
static void mdlInitializeSizes(SimStruct *S)
{
ssSetNumSFcnParams(S, NPARAMS);
#if defined(MATLAB_MEX_FILE)
if(ssGetNumSFcnParams(S) == ssGetSFcnParamsCount(S)){
mdlCheckParameters(S);
if(ssGetErrorStatus(S)!=NULL){
return;
}
else{
cout<<"All parameters have been checked and passed correctly"<<endl;
}
} else{
return; // Parameter mismatch reported by Simulink
}
#endif
// Parameter mismatch will be reported by Simulink
if (ssGetNumSFcnParams(S) != ssGetSFcnParamsCount(S)) {
return;
}
// Specify I/O
// INPUTS
if(!ssSetNumInputPorts(S,0)) return;
// OUTPUTS
if (!ssSetNumOutputPorts(S,SIZE_READING_PORT)) return;
for (int i = 0; i < SIZE_READING_PORT; i++)
{
ssSetOutputPortWidth (S, i, 1);
ssSetOutputPortDataType(S, i, 0);
}
ssSetNumSampleTimes(S, 1);
// Reserve place for C++ object
ssSetNumPWork(S, 1);
// DWork vectors
ssSetNumDWork(S, 1);
ssSetDWorkWidth(S, 0, 1);
ssSetDWorkDataType(S, 0, SS_DOUBLE);
ssSetSimStateCompliance(S, USE_CUSTOM_SIM_STATE);
ssSetOptions(S,
SS_OPTION_WORKS_WITH_CODE_REUSE |
SS_OPTION_EXCEPTION_FREE_CODE |
SS_OPTION_ALLOW_INPUT_SCALAR_EXPANSION |
SS_OPTION_USE_TLC_WITH_ACCELERATOR);
}
static void mdlStart(SimStruct *S)
{
/* instance underlying S-Function data */
#if defined(RT_MALLOC) || defined(MATLAB_MEX_FILE)
# if defined(MATLAB_MEX_FILE)
/* non-finites */
rt_InitInfAndNaN(sizeof(real_T));
# endif
Hammerstein_malloc(S);
if (ssGetErrorStatus(S) != (NULL) ) {
return;
}
#endif
{
BlockIO_Hammerstein *_rtB;
_rtB = ((BlockIO_Hammerstein *) ssGetLocalBlockIO(S));
/* Level2 S-Function Block: '<S1>/Pwlinear1' (sfunpwlinear) */
{
SimStruct *rts = ssGetSFunction(S, 0);
sfcnStart(rts);
if (ssGetErrorStatus(rts) != (NULL))
return;
}
/* Level2 S-Function Block: '<S1>/Pwlinear' (sfunpwlinear) */
{
SimStruct *rts = ssGetSFunction(S, 1);
sfcnStart(rts);
if (ssGetErrorStatus(rts) != (NULL))
return;
}
}
}
/* Function: mdlInitializeSizes ===============================================
* Abstract:
* The sizes information is used by Simulink to determine the S-function
* block's characteristics (number of inputs, outputs, states, etc.).
*/
static void mdlInitializeSizes(SimStruct *S)
{
ssSetNumSFcnParams(S, NPARAMS); /* Number of expected parameters */
#if defined(MATLAB_MEX_FILE)
if (ssGetNumSFcnParams(S) == ssGetSFcnParamsCount(S)) {
mdlCheckParameters(S);
if (ssGetErrorStatus(S) != NULL) {
return;
}
} else {
return; /* Parameter mismatch will be reported by Simulink */
}
#endif
{
int iParam = 0;
int nParam = ssGetNumSFcnParams(S);
for ( iParam = 0; iParam < nParam; iParam++ )
{
ssSetSFcnParamTunable( S, iParam, SS_PRM_SIM_ONLY_TUNABLE );
}
}
ssSetNumContStates(S, NSTATES);
ssSetNumDiscStates(S, 0);
if (!ssSetNumInputPorts(S, NINPUTS)) return;
ssSetInputPortWidth(S, 0, 1);
ssSetInputPortDirectFeedThrough(S, 0, TRUE);
ssSetInputPortWidth(S, 1, 1);
ssSetInputPortDirectFeedThrough(S, 1, TRUE);
ssSetInputPortWidth(S, 2, 1);
ssSetInputPortDirectFeedThrough(S, 2, TRUE);
if (!ssSetNumOutputPorts(S, NOUTPUTS)) return;
//ssSetOutputPortWidth(S, 0, NOUTPUTS);
{
ssSetOutputPortWidth(S, 0, 1);
}
ssSetNumSampleTimes(S, 1);
ssSetNumRWork(S, 0);
ssSetNumIWork(S, 0);
ssSetNumPWork(S, 0);
ssSetNumModes(S, 0);
ssSetNumNonsampledZCs(S, 0);
ssSetSimStateCompliance(S, USE_DEFAULT_SIM_STATE);
/* Take care when specifying exception free code - see sfuntmpl_doc.c */
ssSetOptions(S, SS_OPTION_EXCEPTION_FREE_CODE);
}
static void mdlInitializeConditions(SimStruct *S)
{
BlockIO_Hammerstein *_rtB;
_rtB = ((BlockIO_Hammerstein *) ssGetLocalBlockIO(S));
/* InitializeConditions for DiscreteStateSpace: '<S1>/LinearModel' */
((real_T*) ssGetDWork(S, 0))[0] = 0.0;
/* Level2 S-Function Block: '<S1>/Pwlinear1' (sfunpwlinear) */
{
SimStruct *rts = ssGetSFunction(S, 0);
sfcnInitializeConditions(rts);
if (ssGetErrorStatus(rts) != (NULL))
return;
}
/* Level2 S-Function Block: '<S1>/Pwlinear' (sfunpwlinear) */
{
SimStruct *rts = ssGetSFunction(S, 1);
sfcnInitializeConditions(rts);
if (ssGetErrorStatus(rts) != (NULL))
return;
}
}
/* ======================================================================== */
static void startHackrfTx(SimStruct *S, bool print_info)
/* ======================================================================== */
{
double sample_rate = (1.0 / ssGetSampleTime(S, 0)) * ssGetInputPortDimensions(S, 0)[0];
hackrf_device *device = startHackrf(S, sample_rate, GetParam(BANDWIDTH), print_info);
ssSetPWorkValue(S, DEVICE, device);
if (ssGetErrorStatus(S)) return;
int i = 0; for (; i < NUM_PARAMS; i++) ssSetRWorkValue(S, i, NAN);
mdlProcessParameters(S);
sample_buffer_reset((SampleBuffer*) ssGetPWorkValue(S, SBUF));
int ret = hackrf_start_tx(device, hackrf_tx_callback, S);
Hackrf_assert(S, ret, "Failed to start RX streaming");
}
/* Function: mdlInitializeSizes ===============================================
* Abstract:
* Setup sizes of the various vectors.
*/
static void mdlInitializeSizes(SimStruct *S)
{
DECL_AND_INIT_DIMSINFO(inputDimsInfo);
DECL_AND_INIT_DIMSINFO(outputDimsInfo);
ssSetNumSFcnParams(S, NPARAMS); /* Number of expected parameters */
#if defined(MATLAB_MEX_FILE)
if (ssGetNumSFcnParams(S) == ssGetSFcnParamsCount(S)) {
mdlCheckParameters(S);
if (ssGetErrorStatus(S) != NULL) {
return;
}
} else {
return; /* Parameter mismatch will be reported by Simulink */
}
#endif
ssSetNumContStates(S, NUM_CONT_STATES);
ssSetNumDiscStates(S, NUM_DISC_STATES);
if (!ssSetNumInputPorts(S, NUM_INPUTS)) return;
inputDimsInfo.width = INPUT_0_WIDTH;
ssSetInputPortDimensionInfo(S, 0, &inputDimsInfo);
ssSetInputPortFrameData(S, 0, IN_0_FRAME_BASED);
ssSetInputPortDataType(S, 0, SS_UINT32);
ssSetInputPortComplexSignal(S, 0, INPUT_0_COMPLEX);
ssSetInputPortDirectFeedThrough(S, 0, INPUT_0_FEEDTHROUGH);
ssSetInputPortRequiredContiguous(S, 0, 1); /*direct input signal access*/
if (!ssSetNumOutputPorts(S, NUM_OUTPUTS)) return;
outputDimsInfo.width = OUTPUT_0_WIDTH;
ssSetOutputPortDimensionInfo(S, 0, &outputDimsInfo);
ssSetOutputPortFrameData(S, 0, OUT_0_FRAME_BASED);
ssSetOutputPortDataType(S, 0, SS_DOUBLE);
ssSetOutputPortComplexSignal(S, 0, OUTPUT_0_COMPLEX);
ssSetOutputPortComplexSignal(S, 0, OUTPUT_0_COMPLEX);
ssSetNumSampleTimes(S, 1);
ssSetNumRWork(S, 0);
ssSetNumIWork(S, 0);
ssSetNumPWork(S, 0);
ssSetNumModes(S, 0);
ssSetNumNonsampledZCs(S, 0);
/* Take care when specifying exception free code - see sfuntmpl_doc.c */
ssSetOptions(S, (SS_OPTION_EXCEPTION_FREE_CODE |
SS_OPTION_WORKS_WITH_CODE_REUSE));
}
/* Function: mdlInitializeSizes ===============================================
* Abstract:
* The sizes information is used by Simulink to determine the S-function
* block's characteristics (number of inputs, outputs, states, etc.).
*/
static void mdlInitializeSizes(SimStruct *S)
{
/* See sfuntmpl_doc.c for more details on the macros below */
ssSetNumSFcnParams(S, 5); /* Number of expected parameters */
#if defined(MATLAB_MEX_FILE)
if (ssGetNumSFcnParams(S) == ssGetSFcnParamsCount(S)) {
mdlCheckParameters(S);
if (ssGetErrorStatus(S) != NULL) {
return;
}
} else {
return; /* Parameter mismatch will be reported by Simulink. */
}
#endif
ssSetNumContStates(S, 0);
ssSetNumDiscStates(S, 0);
if (!ssSetNumInputPorts(S, 0)) return;
if (!ssSetNumOutputPorts(S, 0)) return;
ssSetNumSampleTimes(S, 1);
ssSetNumRWork(S, 0);
ssSetNumIWork(S, 1); // nWaitTopic
size_t wait_buflen = mxGetN((ssGetSFcnParam(S, 2)))*sizeof(mxChar)+1;
char* wait_name = (char*)mxMalloc(wait_buflen);
mxGetString((ssGetSFcnParam(S, 2)), wait_name, wait_buflen);
char * topic = strtok(wait_name, TOPIC_SEPARATORS);
unsigned int nWaitTopic = 0;
while (topic != NULL){
nWaitTopic++;
topic = strtok(NULL, TOPIC_SEPARATORS);
}
mxFree(wait_name);
ssSetNumPWork(S, 4+nWaitTopic+1); //Start + Stop + Synchronous + Trigger services + wait_topic Subscriber(s) + AsyncSpinner
ssSetNumModes(S, 0);
ssSetNumNonsampledZCs(S, 0);
/* Specify the sim state compliance to be same as a built-in block */
ssSetSimStateCompliance(S, USE_DEFAULT_SIM_STATE);
ssSetOptions(S, 0);
}
/* Model update function */
static void testSHM_update(int_T tid)
{
/* Level2 S-Function Block: '<Root>/S-Function' (sSHM) */
{
SimStruct *rts = testSHM_M->childSfunctions[0];
sfcnUpdate(rts, 0);
if (ssGetErrorStatus(rts) != (NULL))
return;
}
/* Update absolute time for base rate */
if (!(++testSHM_M->Timing.clockTick0))
++testSHM_M->Timing.clockTickH0;
testSHM_M->Timing.t[0] = testSHM_M->Timing.clockTick0 *
testSHM_M->Timing.stepSize0 + testSHM_M->Timing.clockTickH0 *
testSHM_M->Timing.stepSize0 * 4294967296.0;
/* tid is required for a uniform function interface.
* Argument tid is not used in the function. */
UNUSED_PARAMETER(tid);
}
static void mdlTerminate(SimStruct *S)
{
#ifndef MATLAB_MEX_FILE
int index = (int)COMEDI_DEVICE - 1;
void *dev = (void *)ssGetPWork(S)[0];
int subdev = ssGetIWork(S)[0];
char *devname[4] = {"/dev/comedi0","/dev/comedi1","/dev/comedi2","/dev/comedi3"};
if (ssGetErrorStatus(S) == NULL) {
ComediDev_InUse[index]--;
ComediDev_AIInUse[index]--;
if (!ComediDev_AIInUse[index]) {
comedi_unlock(dev, subdev);
}
if (!ComediDev_InUse[index]) {
comedi_close(dev);
printf("\nCOMEDI %s closed.\n\n", devname[index]);
ComediDev[index] = NULL;
}
}
#endif
}
/* Function: mdlInitializeSizes ===============================================
* Abstract:
* The sizes information is used by Simulink to determine the S-function
* block's characteristics (number of inputs, outputs, states, etc.).
*/
static void mdlInitializeSizes(SimStruct *S) {
ssSetNumSFcnParams(S, 3); /* Number of expected parameters */
// Parameter mismatch will be reported by Simulink
if (ssGetNumSFcnParams(S) == ssGetSFcnParamsCount(S)) {
mdlCheckParameters(S);
if (ssGetErrorStatus(S) != NULL) {
return;
}
} else {
return; /* Parameter mismatch will be reported by Simulink */
}
ssSetSFcnParamTunable(S, 0, 0);
ssSetNumContStates(S, 0);
ssSetNumDiscStates(S, 0);
if (!ssSetNumInputPorts(S, 0)) return;
// ssSetInputPortDirectFeedThrough(S, 0, 1);
if (!ssSetNumOutputPorts(S, 1)) return;
ssSetOutputPortWidth(S, 0, DYNAMICALLY_SIZED);
ssSetNumSampleTimes(S, 1);
ssSetNumPWork(S, 2); // reserve element in the pointers vector
ssSetNumModes(S, 0); // to store a C++ object
ssSetNumNonsampledZCs(S, 0);
ssSetSimStateCompliance(S, USE_CUSTOM_SIM_STATE);
ssSetOptions(S, 0);
}