static void mdlSetInputPortDataType(SimStruct *S, int_T port, DTypeId dataType) {
int portWidth;
if ( port == 0 ) {
portWidth = ssGetInputPortWidth(S,0);
if( isAcceptableDataType( S, dataType,portWidth ) ) {
/*
* Accept proposed data type if it is an unsigned integer type
* force all data ports to use this data type.
*/
ssSetInputPortDataType( S, 0, dataType );
} else {
/* Reject proposed data type */
ssSetErrorStatus(S,"Invalid input signal width, data type or input port is not connected. See mask help for valid data types.");
goto EXIT_POINT;
}
} else {
/*
* Should not end up here. Simulink will only call this function
* for existing input ports whose data types are unknown.
*/
ssSetErrorStatus(S, "Error setting input port data type.");
goto EXIT_POINT;
}
EXIT_POINT:
return;
}
static void mdlStart(SimStruct * S) {
uchar_T ipAddress[16];
int_T ipPort = (int_T) mxGetPr(IP_PORT_ARG)[0];
int_T noBytes = (int_T) mxGetPr(NO_BYTES_ARG)[0];
int_T id = 0;
uint8_T *buffer = (uint8_T *) calloc(noBytes, sizeof(uint8_T));
if (buffer == NULL) {
sprintf(msg, "Error UDP Receive: Memory allocation failed");
ssSetErrorStatus(S, msg);
return;
}
ssSetPWorkValue(S, 0, (void *) buffer);
/* initialize buffer with zeros */
memset((void *) buffer, 0, noBytes * sizeof(uint8_T));
mxGetString(IP_ADDRESS_ARG, ipAddress, 16);
id = udpOpenReceive(ipPort, ipAddress, noBytes);
if(id < 0){
sprintf(msg, "Error: No UDP channel free or UDP error");
ssSetErrorStatus(S, msg);
}
ssSetIWorkValue(S, 0, id);
ssSetIWorkValue(S, 1, noBytes);
}
/* Function: mdlCheckParameters =============================================
* Abstract:
* Validate our parameters to verify they are okay.
*/
static void mdlCheckParameters(SimStruct *S)
{
int paramIndex = 0;
bool validParam = false;
/* All parameters must match the S-function Builder Dialog */
{
const mxArray *pVal0 = ssGetSFcnParam(S,0);
if (!mxIsDouble(pVal0)) {
ssSetErrorStatus(S,"Sample time parameter SampleTime must be of type double");
return;
}
}
{
const mxArray *pVal0 = ssGetSFcnParam(S,0);
if (!IS_PARAM_DOUBLE(pVal0)) {
validParam = true;
paramIndex = 0;
goto EXIT_POINT;
}
}
EXIT_POINT:
if (validParam) {
char parameterErrorMsg[1024];
sprintf(parameterErrorMsg, "The data type and or complexity of parameter %d does not match the "
"information specified in the S-function Builder dialog. "
"For non-double parameters you will need to cast them using int8, int16, "
"int32, uint8, uint16, uint32 or boolean.", paramIndex + 1);
ssSetErrorStatus(S,parameterErrorMsg);
}
return;
}
/* Function CheckInputSampleTimes ==============================================
* Abstract:
* We know both input sample times, now verify that the make sense
* (see top of file).
*/
static void CheckInputSampleTimes(SimStruct *S)
{
real_T enableTs = ssGetInputPortSampleTime(S, ENABLE_IPORT);
real_T signalTs = ssGetInputPortSampleTime(S, SIGNAL_IPORT);
int enableTid = ssGetInputPortSampleTimeIndex(S,ENABLE_IPORT);
if (enableTs != CONTINUOUS_SAMPLE_TIME) {
/* signal must be a multiple of the enable */
real_T k = floor(signalTs/enableTs+0.5);
if (k < 1.0 ||
fabs(k - signalTs/enableTs) > mxGetEps()*128.0) {
ssSetErrorStatus(S, "Sample time of source signal (port 2) "
"must be a positive multiple of enable "
"(port 1) signal");
return;
}
}
if (enableTid == CONSTANT_TID) {
ssSetErrorStatus(S, "Constant sample time is not allowed "
"at enable port (port 1)");
return;
}
} /* end CheckInputSampleTimes */
static void mdlSetOutputPortWidth(SimStruct *S, int_T port, int_T outputPortWidth)
{
if( ((ssGetInputPortWidth(S, 0) != DYNAMICALLY_SIZED) && (ssGetInputPortWidth(S, 0) != outputPortWidth)) || \
((ssGetOutputPortWidth(S, 1) != DYNAMICALLY_SIZED) && (ssGetOutputPortWidth(S, 1) != outputPortWidth)) )
{
ssSetErrorStatus(S,"xdot and x must have the same size");
return;
}
if( intval(mxGetScalar(paramInitialConditionSource)) > 1 )
{
if( (ssGetInputPortWidth(S, 1) != DYNAMICALLY_SIZED) && (ssGetInputPortWidth(S, 1) != outputPortWidth) )
{
ssSetErrorStatus(S,"xdot, x0 and x must have the same size");
return;
}
ssSetInputPortWidth(S, 1, outputPortWidth);
}
else
{
if( (mxGetNumberOfElements(paramInitialCondition) != 1) && (mxGetNumberOfElements(paramInitialCondition) != outputPortWidth) )
{
ssSetErrorStatus(S,"xdot, x0 and x must have the same size");
return;
}
}
ssSetInputPortWidth(S, 0, outputPortWidth);
ssSetOutputPortWidth(S, 1, outputPortWidth);
}
static void mdlSetInputPortComplexSignal(SimStruct *S,
int_T port,
int_T iPortComplexSignal)
{
int_T oPortComplexSignal = ssGetOutputPortComplexSignal(S,0);
/* Set the complex signal of the input ports */
ssSetInputPortComplexSignal(S, port, iPortComplexSignal);
if(iPortComplexSignal == COMPLEX_YES){
/* Output port must be a complex signal */
if(oPortComplexSignal == COMPLEX_INHERITED){
ssSetOutputPortComplexSignal(S, 0, COMPLEX_YES);
}else if(oPortComplexSignal == COMPLEX_NO){
ssSetErrorStatus(S, "Output port must be complex.");
}
}else if(oPortComplexSignal != COMPLEX_NO){
/*
* The current input port is a real signal. If the other input port
* is a real signal, the output port must be a real signal.
*/
int_T otherPort = (port == 0)? 1 : 0;
int_T otherPortComplexSignal =
ssGetInputPortComplexSignal(S, otherPort);
if(otherPortComplexSignal == COMPLEX_NO){
/* Both input ports are real signals */
if(oPortComplexSignal == COMPLEX_INHERITED){
ssSetOutputPortComplexSignal(S, 0, COMPLEX_NO);
}else if(oPortComplexSignal == COMPLEX_YES){
ssSetErrorStatus(S, "Output port must be real.");
}
}
}
}
/* Function to initialize sizes */
static void mdlInitializeSizes(SimStruct *S)
{
/* checksum */
ssSetChecksumVal(S, 0, 4008323717U);
ssSetChecksumVal(S, 1, 2543693537U);
ssSetChecksumVal(S, 2, 4289145620U);
ssSetChecksumVal(S, 3, 3389381616U);
/* options */
ssSetOptions(S, SS_OPTION_EXCEPTION_FREE_CODE);
/* Accelerator check memory map size match for DWork */
if (ssGetSizeofDWork(S) != sizeof(D_Work)) {
ssSetErrorStatus(S,"Unexpected error: Internal DWork sizes do "
"not match for accelerator mex file.");
}
/* Accelerator check memory map size match for BlockIO */
if (ssGetSizeofGlobalBlockIO(S) != sizeof(BlockIO)) {
ssSetErrorStatus(S,"Unexpected error: Internal BlockIO sizes do "
"not match for accelerator mex file.");
}
/* model parameters */
_ssSetDefaultParam(S, (real_T *) &rtDefaultParameters);
}
static void mdlInitializeConditions(SimStruct *S)
{
/* Now we can be sure that the network blocks have been initialized
and that network pointers have been written to workspace.
Do the remaining initialization */
RMsys *rmsys = (RMsys*) ssGetUserData(S);
// Get the network pointer from the MATLAB workspace
char nwsysbuf[MAXCHARS];
sprintf(nwsysbuf, "_nwsys_%d", rmsys->networkNbr);
mxArray *var = (mxArray*)mexGetVariablePtr("global", nwsysbuf);
if (var == NULL) {
mexPrintf("Network %d not found!\n", rmsys->networkNbr);
ssSetErrorStatus(S, "ttreceive: cannot connect to network block");
return;
}
rmsys->nwsys = (RTnetwork *)(*((long long *)mxGetPr(var)));
// Check the mask input arguments
if (rmsys->receiver < 1 || rmsys->receiver > rmsys->nwsys->nbrOfNodes) {
mexPrintf("Receiver number %d out of bounds\n", rmsys->receiver);
ssSetErrorStatus(S, "ttreceive: receiver number out of bounds");
return;
}
}
nneyi35s1a * ) & _rtP -> nfugx5ih43 , & _rtZCSV -> nfugx5ih43 ) ; } static
void mdlInitializeSizes ( SimStruct * S ) { ssSetChecksumVal ( S , 0 ,
2573679304U ) ; ssSetChecksumVal ( S , 1 , 1732126455U ) ; ssSetChecksumVal (
S , 2 , 2644021823U ) ; ssSetChecksumVal ( S , 3 , 2167879752U ) ; { mxArray
* slVerStructMat = NULL ; mxArray * slStrMat = mxCreateString ( "simulink" )
; char slVerChar [ 10 ] ; int status = mexCallMATLAB ( 1 , & slVerStructMat ,
1 , & slStrMat , "ver" ) ; if ( status == 0 ) { mxArray * slVerMat =
mxGetField ( slVerStructMat , 0 , "Version" ) ; if ( slVerMat == NULL ) {
status = 1 ; } else { status = mxGetString ( slVerMat , slVerChar , 10 ) ; }
} mxDestroyArray ( slStrMat ) ; mxDestroyArray ( slVerStructMat ) ; if ( (
status == 1 ) || ( strcmp ( slVerChar , "8.7" ) != 0 ) ) { return ; } }
ssSetOptions ( S , SS_OPTION_EXCEPTION_FREE_CODE ) ; if ( ssGetSizeofDWork (
S ) != sizeof ( pklu3vjy1t ) ) { ssSetErrorStatus ( S ,
"Unexpected error: Internal DWork sizes do "
"not match for accelerator mex file." ) ; } if ( ssGetSizeofGlobalBlockIO ( S
) != sizeof ( nmsgyp54ig ) ) { ssSetErrorStatus ( S ,
"Unexpected error: Internal BlockIO sizes do "
"not match for accelerator mex file." ) ; } { int ssSizeofParams ;
ssGetSizeofParams ( S , & ssSizeofParams ) ; if ( ssSizeofParams != sizeof (
bbqvqz25ov ) ) { static char msg [ 256 ] ; sprintf ( msg ,
"Unexpected error: Internal Parameters sizes do "
"not match for accelerator mex file." ) ; } } _ssSetModelRtp ( S , ( real_T *
) & aaesvjrkn2 ) ; _ssSetConstBlockIO ( S , & j0x3c3z5bk ) ; rt_InitInfAndNaN
( sizeof ( real_T ) ) ; ( ( bbqvqz25ov * ) ssGetModelRtp ( S ) ) ->
nfugx5ih43 . P_6 = rtInf ; ( ( bbqvqz25ov * ) ssGetModelRtp ( S ) ) ->
cqabclhqip4 . P_6 = rtInf ; } static void mdlInitializeSampleTimes (
/* Function: mdlInitializeConditions ========================================
* Abstract:
* Initialize states to zeros.
*/
static void mdlInitializeConditions(SimStruct *S)
{
int_T i;
InputRealPtrsType M, q;
/* checks the input matrices for NaN and Inf */
/* checking M */
M = ssGetInputPortRealSignalPtrs(S,0);
for (i=0; i<NSTATES(S)*NSTATES(S); i++) {
if ( !mxIsFinite(*M[i]) ) {
ssSetErrorStatus(S, "lcp_sfun: No 'NaN' or 'Inf' terms are allowed in input matrix M.");
return;
}
}
/* checking q */
q = ssGetInputPortRealSignalPtrs(S,1);
for (i=0; i<NSTATES(S); i++) {
if ( !mxIsFinite(*q[i]) ) {
ssSetErrorStatus(S, "lcp_sfun: No 'NaN' or 'Inf' terms are allowed in input vector q.");
return;
}
}
}
void mdlSetInputPortDimensionInfo(SimStruct *S, int_T port, const DimsInfo_T *dimsInfo)
/* ========================================================================*/
{
if (dimsInfo->numDims >= 2 && dimsInfo->dims[1] > 1)
ssSetErrorStatus(S, "Wrong port dimensions")
else if (BUFFER_SIZE / BYTES_PER_SAMPLE % dimsInfo->dims[0])
ssSetErrorStatus(S, "Frame size must be a power of two (<= 2^18)")
else
ssSetInputPortDimensionInfo(S, port, dimsInfo);
}
/* Function: mdlCheckParameters =============================================
* Abstract:
* Validate our parameters to verify they are okay.
*/
static void mdlCheckParameters(SimStruct *S)
{
unsigned int i;
if ( !mxIsChar(paramHostName) || (mxGetNumberOfElements(paramHostName) < 1) || (mxGetNumberOfElements(paramHostName) > 100) )
{
ssSetErrorStatus(S, "The Host Name parameter must be a nonempty string with max. 100 elements.");
return;
}
if ( (!IS_PARAM_DOUBLE(paramImageSizeXYN)) || (mxGetNumberOfElements(paramImageSizeXYN) < 2) || (mxGetNumberOfElements(paramImageSizeXYN) > 3) )
{
ssSetErrorStatus(S, "The image size must have two elements [X Y] or three elements [X Y frames].");
return;
}
else
{
for( i=0; i<mxGetNumberOfElements(paramImageSizeXYN); i++ )
{
if( intval(mxGetPr(paramImageSizeXYN)[i]) <= 0 )
{
ssSetErrorStatus(S, "The image dimensions have to be positive integers.");
return;
}
}
}
if ( (!IS_PARAM_DOUBLE(paramPixelFormat)) || (mxGetNumberOfElements(paramPixelFormat) != 1) )
{
ssSetErrorStatus(S, "Pixel format must be a scalar");
return;
}
else if (intval(mxGetPr(paramPixelFormat)[0]) != 1 && intval(mxGetPr(paramPixelFormat)[0]) != 5)
{
ssSetErrorStatus(S, "Pixel format must be Mono8(1) or Rgb24(5)");
return;
}
if ( (!IS_PARAM_DOUBLE(paramTimeout)) || (mxGetNumberOfElements(paramTimeout) != 1) )
{
ssSetErrorStatus(S, "Timeout must be a scalar");
return;
}
else if (mxGetPr(paramTimeout)[0] < 0)
{
ssSetErrorStatus(S, "Timeout must be non-negative");
return;
}
if ( (!IS_PARAM_DOUBLE(paramSampleTime)) || (mxGetNumberOfElements(paramSampleTime) != 1) )
{
ssSetErrorStatus(S, "Wrong sample time.");
return;
}
if ( !((mxGetPr(paramSampleTime)[0] == -1) || (mxGetPr(paramSampleTime)[0] > 0)) )
{
ssSetErrorStatus(S, "The sample time must be inherited (-1) or discrete (a positive number).");
return;
}
}
/* Function: propPortComplexity ===========================================
*/
void propPortComplexity(SimStruct *S)
{
CSignal_T cY = ssGetOutputPortComplexSignal( S, 0);
CSignal_T cU = ssGetInputPortComplexSignal( S, 0);
/* if input is complex, then output must be complex
*/
if ( cU == COMPLEX_YES )
{
/* if output complexity is not known then set it
*/
if ( cY == COMPLEX_INHERITED )
{
ssSetOutputPortComplexSignal(S, 0, COMPLEX_YES);
}
/* if the output is real, then an error has occurred
*/
else if ( cY == COMPLEX_NO )
{
ssSetErrorStatus(S,"Output is REAL, but input is COMPLEX.");
return;
}
}
/* if input is real then output must be real
*/
else if( cU == COMPLEX_NO )
{
/* if output complexity is not known then set it
*/
if ( cY == COMPLEX_INHERITED )
{
ssSetOutputPortComplexSignal(S, 0, COMPLEX_NO);
}
/* if the output is complex, then an error has occurred
*/
else if ( cY == COMPLEX_YES )
{
ssSetErrorStatus(S,"Output is COMPLEX, but input is REAL.");
return;
}
}
else /* Input is COMPLEX_INHERITED) */
{
if ( cY == COMPLEX_NO )
{
ssSetInputPortComplexSignal(S, 0, COMPLEX_NO );
}
else if ( cY == COMPLEX_YES )
{
ssSetInputPortComplexSignal(S, 0, COMPLEX_YES );
}
}
}
/* Function: mdlCheckParameters =============================================
* Abstract:
* Validate our parameters to verify they are okay.
*/
static void mdlCheckParameters(SimStruct *S)
{
/* Check 1st parameter: A-Matrix */
{
if ( mxGetN(A_PARAM(S)) != NSTATES || !IS_PARAM_DOUBLE(A_PARAM(S)) ) {
ssSetErrorStatus(S,"1st parameter to S-function "
"\"A-Matrix\" must be square and double");
return;
}
}
/* Check 2nd parameter: B-Matrix */
{
if (mxGetM(B_PARAM(S)) != NSTATES || !IS_PARAM_DOUBLE(B_PARAM(S)) ) {
ssSetErrorStatus(S,"2nd parameter to S-function "
"\"B-Matrix\" is not dimensioned "
"correctly");
return;
}
}
/* Check 3rd parameter: C-Matrix */
{
if (mxGetN(C_PARAM(S)) != NSTATES || !IS_PARAM_DOUBLE(C_PARAM(S)) ) {
ssSetErrorStatus(S,"3rd parameter to S-function "
"\"C-Matrix\" is not dimensioned "
"correctly");
return;
}
}
/* Check 4th parameter: D-Matrix */
{
if (mxGetM(D_PARAM(S)) != NOUTPUTS ||
mxGetN(D_PARAM(S)) != NINPUTS || !IS_PARAM_DOUBLE(D_PARAM(S)) ) {
ssSetErrorStatus(S,"4th parameter to S-function "
"\"D-Matrix\" is not dimensioned "
"correctly");
return;
}
}
/* Check 5th parameter: X0 */
{
if ( ((mxGetM(X0_PARAM(S)) != 0) &&
(mxGetM(X0_PARAM(S)) != NSTATES)) || !OK_EMPTY_DOUBLE_PARAM(X0_PARAM(S)) ) {
ssSetErrorStatus(S,"5th parameter to S-function "
"\"X0-Matrix\" is not dimensioned "
"correctly");
return;
}
}
}
/* Function: mdlCheckParameters =============================================
* Abstract:
* Validate our parameters to verify:
* o The numerator must be of a lower order than the denominator.
* o The sample time must be a real positive nonzero value.
*/
static void mdlCheckParameters(SimStruct *S)
{
// SFUNPRINTF("Calling mdlCheckParameters");
// Tsim
if (mxIsEmpty( ssGetSFcnParam(S,0)) ||
mxIsSparse( ssGetSFcnParam(S,0)) ||
mxIsComplex( ssGetSFcnParam(S,0)) ||
mxIsLogical( ssGetSFcnParam(S,0)) ||
!mxIsNumeric( ssGetSFcnParam(S,0)) ||
!mxIsDouble( ssGetSFcnParam(S,0)) ||
mxGetNumberOfElements(ssGetSFcnParam(S,0)) != 1) {
ssSetErrorStatus(S,"Simulation time must be a single double Value");
return;
}
// Vrep services base name
if (!mxIsChar( ssGetSFcnParam(S,1)) ) {
ssSetErrorStatus(S,"Vrep services base name must be a char array (string)");
return;
}
// wait topic name
if (!mxIsChar( ssGetSFcnParam(S,2)) ) {
ssSetErrorStatus(S,"Wait topic name must be a char array (string)");
return;
}
// wait timeout
if (mxIsEmpty( ssGetSFcnParam(S,3)) ||
mxIsSparse( ssGetSFcnParam(S,3)) ||
mxIsComplex( ssGetSFcnParam(S,3)) ||
mxIsLogical( ssGetSFcnParam(S,3)) ||
!mxIsNumeric( ssGetSFcnParam(S,3)) ||
!mxIsDouble( ssGetSFcnParam(S,3)) ||
mxGetNumberOfElements(ssGetSFcnParam(S,3)) != 1) {
ssSetErrorStatus(S,"Wait timeout must be a single double Value");
return;
}
// wait extra_wait
if (mxIsEmpty( ssGetSFcnParam(S,4)) ||
mxIsSparse( ssGetSFcnParam(S,4)) ||
mxIsComplex( ssGetSFcnParam(S,4)) ||
mxIsLogical( ssGetSFcnParam(S,4)) ||
!mxIsNumeric( ssGetSFcnParam(S,4)) ||
!mxIsDouble( ssGetSFcnParam(S,4)) ||
mxGetNumberOfElements(ssGetSFcnParam(S,4)) != 1) {
ssSetErrorStatus(S,"Extra wait time must be a single double Value");
return;
}
}
static void mdlStart(SimStruct *S)
{
// ######### YARP INITIALIZATION STUFF ##################
Network::init();
fprintf(stderr,"YARP NETWORK INITIALIZED\n");
if (!Network::checkNetwork() || !Network::initialized()){
ssSetErrorStatus(S,"YARP server wasn't found active!! \n");
return;
}
else
cout<<"YARP is running!!\n"<<endl;
int_T buflen, status;
char *String;
buflen = mxGetN((ssGetSFcnParam(S, PARAM_IDX_1)))*sizeof(mxChar)+1;
String = static_cast<char*>(mxMalloc(buflen));
status = mxGetString((ssGetSFcnParam(S, PARAM_IDX_1)),String,buflen);
if (status) {
ssSetErrorStatus(S,"Cannot retrieve string from parameter 1!! \n");
return;
}
//string port_name = String;
char *port_name = String; //FROM port name
buflen = mxGetN((ssGetSFcnParam(S, PARAM_IDX_2)))*sizeof(mxChar)+1;
String = static_cast<char*>(mxMalloc(buflen));
status = mxGetString((ssGetSFcnParam(S, PARAM_IDX_2)),String,buflen);
if (status) {
ssSetErrorStatus(S,"Cannot retrieve string from parameter 2!! \n");
return;
}
char *toPort_name = String;
// ######## CHECKING INPUT PARAMETERS ############
BufferedPort<Vector> *toPort;
toPort = new BufferedPort<Vector>;
toPort->open(toPort_name);
ConstString toPortName = toPort->getName();
cout<<"[From] Port name will be: "<<port_name<<endl;
cout<<"[To] Port name will be: "<<toPort->getName()<<endl;
ssGetPWork(S)[0] = toPort;
Network::connect(port_name,toPortName);
}
/* Function: mdlOutputs =======================================================
* Abstract:
* In this function, you compute the outputs of your S-function
* block. Generally outputs are placed in the output vector, ssGetY(S).
*/
static void mdlOutputs(SimStruct *S, int_T tid){
#if defined(RT)
TmcdDeviceBallActual* actualBall = 0;
TmcdDeviceActual* actual = 0;
# endif
Context* context = (Context*)(ssGetPWork(S)[0]);
#if defined(RT)
if (context->enableSpacemouse)
{
actualBall = tmcdDeviceBallGetActual(context->tmcd2005.deviceBall.handle);
if ( !actualBall )
ssSetErrorStatus(S,"could not get state from TMCD2005");
// copy events to simulink outputs
*((boolean_T*)ssGetOutputPortSignal(S, 0)) = actualBall->on;
((boolean_T*)ssGetOutputPortSignal(S, 1))[0] = actualBall->keys[0];
((boolean_T*)ssGetOutputPortSignal(S, 1))[1] = actualBall->keys[1];
((boolean_T*)ssGetOutputPortSignal(S, 1))[2] = actualBall->keys[2];
((real_T*)ssGetOutputPortSignal(S, 2))[0] = actualBall->ballData[0];
((real_T*)ssGetOutputPortSignal(S, 2))[1] = actualBall->ballData[1];
((real_T*)ssGetOutputPortSignal(S, 2))[2] = actualBall->ballData[2];
((real_T*)ssGetOutputPortSignal(S, 2))[3] = actualBall->ballData[3];
((real_T*)ssGetOutputPortSignal(S, 2))[4] = actualBall->ballData[4];
((real_T*)ssGetOutputPortSignal(S, 2))[5] = actualBall->ballData[5];
*((boolean_T*)ssGetOutputPortSignal(S, 3)) = actualBall->isBallActive;
}
else
{
actual = tmcdDeviceGetActual(context->tmcd2005.device.handle);
if ( !actual )
ssSetErrorStatus(S,"could not get state from TMCD2005");
// copy events to simulink outputs
*((boolean_T*)ssGetOutputPortSignal(S, 0)) = actual->on;
((boolean_T*)ssGetOutputPortSignal(S, 1))[0] = actual->keys[0];
((boolean_T*)ssGetOutputPortSignal(S, 1))[1] = actual->keys[1];
((boolean_T*)ssGetOutputPortSignal(S, 1))[2] = actual->keys[2];
((boolean_T*)ssGetOutputPortSignal(S, 1))[3] = actual->keys[3];
}
#endif
}
/* Function: mdlCheckParameters =============================================
* Abstract:
* Validate our parameters to verify:
* o The numerator must be of a lower order than the denominator.
* o The sample time must be a real positive nonzero value.
*/
static void mdlCheckParameters(SimStruct *S)
{
// SFUNPRINTF("Calling mdlCheckParameters");
// Tsim
if (mxIsEmpty( ssGetSFcnParam(S,0)) ||
mxIsSparse( ssGetSFcnParam(S,0)) ||
mxIsComplex( ssGetSFcnParam(S,0)) ||
mxIsLogical( ssGetSFcnParam(S,0)) ||
!mxIsNumeric( ssGetSFcnParam(S,0)) ||
!mxIsDouble( ssGetSFcnParam(S,0)) ||
mxGetNumberOfElements(ssGetSFcnParam(S,0)) != 1) {
ssSetErrorStatus(S,"Simulation time must be a single double Value");
return;
}
// Prefix Topic
if (!mxIsChar( ssGetSFcnParam(S,1)) ) {
ssSetErrorStatus(S,"Prefix value must be char array (string)");
return;
}
// Robot Array
if (mxIsEmpty( ssGetSFcnParam(S,2)) ||
mxIsSparse( ssGetSFcnParam(S,2)) ||
mxIsComplex( ssGetSFcnParam(S,2)) ||
mxIsLogical( ssGetSFcnParam(S,2)) ||
!mxIsChar( ssGetSFcnParam(S,2)) ) {
ssSetErrorStatus(S,"Robot Vector must be a char vector of robot ids");
return;
}
// Postfix Topic
if (!mxIsChar( ssGetSFcnParam(S,3)) ) {
ssSetErrorStatus(S,"Postfix value must be char array (string)");
return;
}
// N. of dofs
if (mxIsEmpty( ssGetSFcnParam(S,4)) ||
mxIsSparse( ssGetSFcnParam(S,4)) ||
mxIsComplex( ssGetSFcnParam(S,4)) ||
mxIsLogical( ssGetSFcnParam(S,4)) ||
!mxIsChar( ssGetSFcnParam(S,4)) ||
mxGetNumberOfElements(ssGetSFcnParam(S,4)) != 1) {
ssSetErrorStatus(S,"N. of dofs must be a char");
return;
}
}
static void mdlInitializeSizes(SimStruct *S) {
int_T msgType;
int_T initValLen;
ssSetNumSFcnParams(S,5);
if (ssGetNumSFcnParams(S) != ssGetSFcnParamsCount(S)) {
return; /* Parameter mismatch will be reported by Simulink */
}
ssSetNumContStates(S,0);
ssSetNumDiscStates(S,0);
if (!ssSetNumInputPorts(S,0)) return;
msgType = (int_T)*mxGetPr(ssGetSFcnParam(S,0));
switch (msgType) {
case SUBSCRIBER_TWIST_STAMPED:
case PUBLISHER_POINT_STAMPED:
if (!ssSetNumOutputPorts(S,2)) return;
ssSetOutputPortWidth(S,1,2);
break;
default:
if (!ssSetNumOutputPorts(S,1)) return;
}
switch (msgType) {
case SUBSCRIBER_FLOAT64:
case SUBSCRIBER_INT32:
case SUBSCRIBER_BOOL:
case SUBSCRIBER_TIME:
ssSetOutputPortWidth(S,0,1);
break;
case SUBSCRIBER_FLOAT64ARRAY:
ssSetOutputPortWidth(S,0,DYNAMICALLY_SIZED);
break;
case SUBSCRIBER_TWIST:
case SUBSCRIBER_TWIST_STAMPED:
ssSetOutputPortWidth(S,0,6);
break;
case PUBLISHER_POINT:
case PUBLISHER_POINT_STAMPED:
case PUBLISHER_POSE2D:
ssSetOutputPortWidth(S,0,3);
break;
default:
ssSetErrorStatus(S, "Unknown message type!");
return;
}
initValLen = mxGetNumberOfElements(ssGetSFcnParam(S,2));
if (initValLen > 1) {
ssSetOutputPortWidth(S,0,initValLen);
}
ssSetNumSampleTimes(S,1);
ssSetNumRWork(S,0);
ssSetNumIWork(S,1);
ssSetNumPWork(S,2);
ssSetNumModes(S,0);
ssSetNumNonsampledZCs(S,0);
ssSetOptions(S,0);
}
/* Function: mdlCheckParameters =============================================
* Abstract:
* Validate our parameters to verify they are okay.
*/
static void mdlCheckParameters(SimStruct *S)
{
#define PrmNumPos 46
int paramIndex = 0;
bool validParam = false;
char paramVector[] ={'1'};
static char parameterErrorMsg[] ="The data type and/or complexity of parameter does not match the information "
"specified in the S-function Builder dialog. For non-double parameters you will need to cast them using int8, int16,"
"int32, uint8, uint16, uint32 or boolean.";
/* All parameters must match the S-function Builder Dialog */
{
const mxArray *pVal0 = ssGetSFcnParam(S,0);
if (!IS_PARAM_DOUBLE(pVal0)) {
validParam = true;
paramIndex = 0;
goto EXIT_POINT;
}
}
EXIT_POINT:
if (validParam) {
parameterErrorMsg[PrmNumPos] = paramVector[paramIndex];
ssSetErrorStatus(S,parameterErrorMsg);
}
return;
}
static void mdlSetInputPortDimensionInfo(SimStruct *S,
int_T port,
const DimsInfo_T *dimsInfo)
{
boolean_T isNotVector = ((dimsInfo->numDims == 2 ) &&
(dimsInfo->dims[0] > 1 && dimsInfo->dims[1] > 1)) ;
if(isNotVector){
ssSetErrorStatus(S, "The block only accepts vector signals. "
"It does not accept a [mxn] matrix signal "
"where m > 1 and n > 1.");
}else{
int otherPort = (port == 0) ? 1 : 0;
if(!ssSetInputPortDimensionInfo(S, port, dimsInfo)) return;
/*
* If other port width is unknown, set the other port width.
* Note1: we cannot update other port dimension info.
* Note2: For simplicity, this block cannot accept partial dimension,
* however, it may partially set other port dimension info.
*/
if(ssGetInputPortWidth(S, otherPort) == DYNAMICALLY_SIZED &&
ssGetInputPortWidth(S, port) != DYNAMICALLY_SIZED){
DECL_AND_INIT_DIMSINFO(dimsInfo);
dimsInfo.width = ssGetInputPortWidth (S, port);
dimsInfo.numDims = ssGetInputPortNumDimensions(S, otherPort);
dimsInfo.dims = ssGetInputPortDimensions (S, otherPort);
if(!ssSetInputPortDimensionInfo(S, otherPort, &dimsInfo)) return;
}
}
}
static void mdlSetOutputPortDimensionInfo(SimStruct *S,
int_T port,
const DimsInfo_T *dimsInfo)
{
/* This should never occur! */
ssSetErrorStatus(S, "Error setting output port width.");
}
static void mdlSetOutputPortDimensionInfo(SimStruct *S,
int_T port,
const DimsInfo_T *dimsInfo)
{
# ifdef DEBUG
printf(" co_udp_receiver :: Now in mdlSetOutputPortDimensionInfo.\n");
# endif
/*
* If the block has scalar parameter, the output dimensions are unknown.
* Set the input and output port to have the same dimensions.
*/
if(!ssSetOutputPortDimensionInfo(S, port, dimsInfo)) return;
/* The block only accepts 2-D signals. Check number of dimensions. */
if(dimsInfo->numDims != 1) {
ssSetErrorStatus(S, "Invalid output port dimensions. The output signal "
"must be a vector signal.");
return;
} else {
/* Set the input port dimensions */
if(!ssSetInputPortDimensionInfo(S, port, dimsInfo)) return;
}
}
/* Function: mdlStart =======================================================
* Abstract:
* This function is called once at start of model execution. If you
* have states that should be initialized once, this is the place
* to do it.
*/
static void mdlStart(SimStruct *S)
{
int i;
/* Bus Information */
slDataTypeAccess *dta = ssGetDataTypeAccess(S);
const char *bpath = ssGetPath(S);
DTypeId u_busId = ssGetDataTypeId(S, "u_bus");
DTypeId x_busId = ssGetDataTypeId(S, "x_bus");
int_T *busInfo = (int_T *)malloc((ACADO_NX+ACADO_NU)*2*sizeof(int_T));
if(busInfo==NULL) {
ssSetErrorStatus(S, "Memory allocation failure");
return;
}
/* Calculate offsets of all primitive elements of the bus */
for( i = 0; i < ACADO_NX; i++ ) {
busInfo[2*i] = dtaGetDataTypeElementOffset(dta, bpath,x_busId,i);
busInfo[2*i+1] = dtaGetDataTypeSize(dta, bpath, ssGetDataTypeId(S, "double"));
}
for( i = 0; i < ACADO_NU; i++ ) {
busInfo[2*(ACADO_NX+i)] = dtaGetDataTypeElementOffset(dta, bpath,u_busId,i);
busInfo[2*(ACADO_NX+i)+1] = dtaGetDataTypeSize(dta, bpath, ssGetDataTypeId(S, "double"));
}
ssSetUserData(S, busInfo);
}
static void mdlUpdate(SimStruct *S, int_T tid) {
UNUSED(tid);
TimePoint *wall_clock_start_time =
static_cast<TimePoint *>(ssGetPWorkValue(S, 0));
if (!wall_clock_start_time) {
// t0 not set yet, set it now. (note: used to set this in mdlStart, but
// there can be a big pause between mdlStart and the full simulation start)
wall_clock_start_time = new TimePoint();
*wall_clock_start_time = TimeClock::now();
ssSetPWorkValue(S, 0, wall_clock_start_time);
}
double sim_time = ssGetT(S);
double realtime_factor = mxGetScalar(ssGetSFcnParam(S, 1));
TimePoint wall_time = TimeClock::now();
TimePoint desired_time =
*wall_clock_start_time + TimeDuration(sim_time / realtime_factor);
if (desired_time >
wall_time) { // could probably just call sleep_until, but just in case
std::this_thread::sleep_until(desired_time);
} else if (wall_time > desired_time + std::chrono::duration<double>(
1.0 / realtime_factor)) {
mexPrintf("at time %f, I'm behind by more than 1 (scaled) second\n",
sim_time);
ssSetErrorStatus(S,
"Simulink is not keeping up with real time. Consider "
"reducing demands on your ODE solver, or optimizing your "
"code.");
}
}
static void mdlUpdate(SimStruct *S, int_T tid)
{
UNUSED(tid);
struct timeval tv;
gettimeofday(&tv, NULL);
double *wall_t0 = (double*) ssGetDWork(S,0);
if (*wall_t0 == TIME_NOT_YET_SET) {
// t0 not set yet, set it now. (note: used to set this in mdlStart, but there can be a big pause between mdlStart and the full simulation start)
*wall_t0 = timevalToDouble(tv);
return;
}
double wall_t = timevalToDouble(tv),
simtime = ssGetT(S),
realtime_factor = mxGetScalar(ssGetSFcnParam(S, 1));
double t_diff = (wall_t - *wall_t0)*realtime_factor - simtime;
// mexPrintf("wall time: %f, sim time: %f, (scaled) diff: %f\n", wall_t-*wall_t0, simtime, t_diff);
if (t_diff<0.0) { // then simtime > scaled wall_time
timespec tosleep;
tv = doubleToTimeval(-t_diff);
tosleep.tv_sec = tv.tv_sec;
tosleep.tv_nsec = tv.tv_usec * 1000;
// mexPrintf("sleeping... %d sec, %d nsec\n", tosleep.tv_sec, tosleep.tv_nsec);
nanosleep(&tosleep, NULL);
} else if (t_diff>1.0) { // then I'm behind by more than 1 second
mexPrintf("at time %f, I'm behind by %f sec\n", simtime, t_diff);
ssSetErrorStatus(S, "Simulink is not keeping up with real time. Consider reducing demands on your ODE solver, or optimizing your code.");
}
}
static void mdlInitializeSizes(SimStruct *S)
{
ssSetNumSFcnParams(S, 5); /* Number of expected parameters */
#ifndef TRES_SIMULINK_DISABLE_MASK_PROTECTION
// Perform mask params validity check
// TODO very basic error check (to be improved).
const mxArray *mxMsdVarName = ssGetSFcnParam(S,MSG_DESCR_VARNAME);
if ((mxGetM(mxMsdVarName) != 1) || (mxGetN(mxMsdVarName) == 0))
{
ssSetErrorStatus(S, "The message-set description variable cannot be empty");
return;
}
const mxArray *mxNdVarName = ssGetSFcnParam(S,NTWK_DESCR_VARNAME);
if ((mxGetM(mxNdVarName) != 1) || (mxGetN(mxNdVarName) == 0))
{
ssSetErrorStatus(S, "The network description variable cannot be empty (you must specify at least the network topology!)");
return;
}
const mxArray *mxAddLibsPath = ssGetSFcnParam(S,OTHER_DEPS);
if ((mxGetM(mxAddLibsPath) != 1) || (mxGetN(mxAddLibsPath) == 0))
{
ssSetErrorStatus(S, "The Additional Libraries (see the Simulator tab) field cannot be empty");
return;
}
#endif
ssSetNumContStates(S, 0);
ssSetNumDiscStates(S, 0);
// Set the number of input ports to 0
if (!ssSetNumInputPorts(S, 0)) return;
// Set the output port to have a dynamic dimension
if (!ssSetNumOutputPorts(S, 1)) return;
ssSetOutputPortWidth(S, 0, DYNAMICALLY_SIZED);
ssSetNumSampleTimes(S, 1);
ssSetNumDWork(S, 1); // store the `New pending activations available' flag
ssSetDWorkWidth(S, 0, 1);
ssSetDWorkDataType(S, 0, SS_BOOLEAN);
ssSetNumPWork(S, 1); // store the tres::Network
ssSetNumRWork(S, 1); // store the time_resolution
ssSetNumNonsampledZCs(S, 1); // next hit
}
void TT_CALLBACK_ERROR(SimStruct *S, const char *error_msg) {
mxArray *rhs[1];
rhs[0] = mxCreateString(error_msg);
mexCallMATLAB(0, NULL, 1, rhs, "error");
mexPrintf("??? %s\n\nIn block ==> %s\nSimulation aborted!\n", error_msg, ssGetBlockName(S));
ssSetErrorStatus(S, "");
}
static void mdlInitializeSizes(SimStruct *S)
{
int i,nChannels;
static char msg[256];
ssSetNumSFcnParams(S, NUMBER_OF_ARGS); /* Number of expected parameters */
if (ssGetNumSFcnParams(S) != ssGetSFcnParamsCount(S)) {
/* Return if number of expected != number of actual parameters */
sprintf(msg,"Wrong Number of input arguments passed.\n%d arguments are expected\n",NUMBER_OF_ARGS);
ssSetErrorStatus(S,msg);
return;
}
ssSetNumContStates(S, 0);
ssSetNumDiscStates(S, 0);
ssSetNumInputPorts(S, 0);//there is no input
//if (!ssSetNumInputPorts(S, 1)) return;
nChannels=(uint_T)mxGetN(CHANNEL_ARG);
//if (!ssSetNumOutputPorts(S, 1)) return;
ssSetNumOutputPorts(S,nChannels);//set number of outputs ports
for(i=0;i<nChannels;i++)
{
ssSetOutputPortWidth(S, i, 1);//set width of each port to 1
}
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.
*/
ssSetInputPortDirectFeedThrough(S, 0, 1);
ssSetNumSampleTimes(S, 1);//set number of sample time to 1,which means all input ports share same Ts
ssSetNumRWork(S, NO_R_WORKS);//number of float in work vector
ssSetNumIWork(S, NO_I_WORKS);//number of int in work vector
ssSetNumPWork(S, 0);//number of pointer in work vector
ssSetNumModes(S, 0);
ssSetNumNonsampledZCs(S, 0);//no need for pass-zero check
ssSetSFcnParamNotTunable(S,0);
ssSetSFcnParamNotTunable(S,1);
ssSetSFcnParamNotTunable(S,2);
ssSetSFcnParamNotTunable(S,3);
ssSetSFcnParamNotTunable(S,4);
ssSetOptions(S,SS_OPTION_EXCEPTION_FREE_CODE|SS_OPTION_PLACE_ASAP);
/* Specify the sim state compliance to be same as a built-in block */
ssSetSimStateCompliance(S, USE_DEFAULT_SIM_STATE);
}
/* 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)
{
char host[] = "localhost";
int port = 9002;
trigger = 0;
/* See sfuntmpl_doc.c for more details on the macros below */
ssSetNumSFcnParams(S, 1); /* Number of expected parameters */
if (ssGetNumSFcnParams(S) != ssGetSFcnParamsCount(S)) {
/* Return if number of expected != number of actual parameters */
return;
}
ssSetNumContStates(S, 0);
ssSetNumDiscStates(S, 0);
/* This functions receives no inputs, but it could use inputs instead of hard-coded parameters (e.g. port and host) */
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, 2)) return;
ssSetOutputPortWidth(S, 0, PAYLOAD_LEN/2);
ssSetOutputPortDataType(S, 0, SS_UINT16);
ssSetOutputPortWidth(S, 1, 1);
ssSetNumSampleTimes(S, 1);
ssSetNumRWork(S, 0);
ssSetNumIWork(S, 0);
ssSetNumPWork(S, 0);
ssSetNumModes(S, 0);
ssSetNumNonsampledZCs(S, 0);
ssSetOptions(S, SS_OPTION_EXCEPTION_FREE_CODE);
/* Connect to the SerialForwarder server using functions linked in sfsource.o */
fd = open_sf_source(host, port);
if (fd < 0){
ssSetErrorStatus(S,"SerialForwarder not running in the specified host and port");
return;
}
}
