/* 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);
}
/* Function: mdlInitializeSizes ===============================================
* Abstract:
* Setup sizes of the various vectors.
*/
static void mdlInitializeSizes(SimStruct *S)
{
slDataTypeAccess *dta = ssGetDataTypeAccess(S);
int udtId;
ssSetNumSFcnParams(S, 0); /* Number of expected parameters */
if (ssGetNumSFcnParams(S) != ssGetSFcnParamsCount(S)) {
/* Return if number of expected != number of actual parameters */
return;
}
/* Obtain an integer datatype ID for the udt (user-defined type) "Data" */
udtId = ssRegisterDataType(S, "Data");
if ( udtId == INVALID_DTYPE_ID ) return;
/* Register the size of the udt */
if (!ssSetDataTypeSize(S, udtId, sizeof(Data))) return;
/* Register the zero of the udt */
if (!ssSetDataTypeZero(S, udtId, &zero)) return;
/* Register the convert-between datatype function defined above */
if (!dtaSetConvertBetweenFcn(dta, ssGetPath(S), udtId, &DataCnvBtw)) return;
/* Register the is-positive datatype function defined above*/
if (!dtaSetIsPositiveFcn(dta, ssGetPath(S), udtId, &DataIsPos)) return;
/* Set input-port properties */
if (!ssSetNumInputPorts(S, 1)) return;
ssSetInputPortWidth(S, 0, 1);
ssSetInputPortDataType(S, 0, SS_DOUBLE);
ssSetInputPortDirectFeedThrough(S, 0, 1);
/* Set output port properties */
if (!ssSetNumOutputPorts(S, 1)) return;
ssSetOutputPortDataType(S, 0, udtId);
ssSetOutputPortWidth(S, 0, 1);
/* Set miscellaneous properties */
ssSetNumContStates(S, 0);
ssSetNumDiscStates(S, 0);
ssSetNumSampleTimes(S, 1);
ssSetNumRWork(S, 0);
ssSetNumIWork(S, 0);
ssSetNumPWork(S, 0);
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,
SS_OPTION_WORKS_WITH_CODE_REUSE |
SS_OPTION_USE_TLC_WITH_ACCELERATOR |
SS_OPTION_DISALLOW_CONSTANT_SAMPLE_TIME |
SS_OPTION_SFUNCTION_INLINED_FOR_RTW);
}
/* 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)
{
/* 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");
DTypeId xref_busId = ssGetDataTypeId(S, "xref_bus");
int_T *busInfo = (int_T *)malloc(36*sizeof(int_T));
if(busInfo==NULL) {
ssSetErrorStatus(S, "Memory allocation failure");
return;
}
/* Calculate offsets of all primitive elements of the bus */
busInfo[0] = dtaGetDataTypeElementOffset(dta, bpath,xref_busId,0);
busInfo[1] = dtaGetDataTypeSize(dta, bpath, ssGetDataTypeId(S, "double"));
busInfo[2] = dtaGetDataTypeElementOffset(dta, bpath,xref_busId,1);
busInfo[3] = dtaGetDataTypeSize(dta, bpath, ssGetDataTypeId(S, "double"));
busInfo[4] = dtaGetDataTypeElementOffset(dta, bpath,xref_busId,2);
busInfo[5] = dtaGetDataTypeSize(dta, bpath, ssGetDataTypeId(S, "double"));
busInfo[6] = dtaGetDataTypeElementOffset(dta, bpath,xref_busId,3);
busInfo[7] = dtaGetDataTypeSize(dta, bpath, ssGetDataTypeId(S, "double"));
busInfo[8] = dtaGetDataTypeElementOffset(dta, bpath,xref_busId,4);
busInfo[9] = dtaGetDataTypeSize(dta, bpath, ssGetDataTypeId(S, "double"));
busInfo[10] = dtaGetDataTypeElementOffset(dta, bpath,xref_busId,5);
busInfo[11] = dtaGetDataTypeSize(dta, bpath, ssGetDataTypeId(S, "double"));
busInfo[12] = dtaGetDataTypeElementOffset(dta, bpath,xref_busId,6);
busInfo[13] = dtaGetDataTypeSize(dta, bpath, ssGetDataTypeId(S, "double"));
busInfo[14] = dtaGetDataTypeElementOffset(dta, bpath,xref_busId,7);
busInfo[15] = dtaGetDataTypeSize(dta, bpath, ssGetDataTypeId(S, "double"));
busInfo[16] = dtaGetDataTypeElementOffset(dta, bpath,x_busId,0);
busInfo[17] = dtaGetDataTypeSize(dta, bpath, ssGetDataTypeId(S, "double"));
busInfo[18] = dtaGetDataTypeElementOffset(dta, bpath,x_busId,1);
busInfo[19] = dtaGetDataTypeSize(dta, bpath, ssGetDataTypeId(S, "double"));
busInfo[20] = dtaGetDataTypeElementOffset(dta, bpath,x_busId,2);
busInfo[21] = dtaGetDataTypeSize(dta, bpath, ssGetDataTypeId(S, "double"));
busInfo[22] = dtaGetDataTypeElementOffset(dta, bpath,x_busId,3);
busInfo[23] = dtaGetDataTypeSize(dta, bpath, ssGetDataTypeId(S, "double"));
busInfo[24] = dtaGetDataTypeElementOffset(dta, bpath,x_busId,4);
busInfo[25] = dtaGetDataTypeSize(dta, bpath, ssGetDataTypeId(S, "double"));
busInfo[26] = dtaGetDataTypeElementOffset(dta, bpath,x_busId,5);
busInfo[27] = dtaGetDataTypeSize(dta, bpath, ssGetDataTypeId(S, "double"));
busInfo[28] = dtaGetDataTypeElementOffset(dta, bpath,x_busId,6);
busInfo[29] = dtaGetDataTypeSize(dta, bpath, ssGetDataTypeId(S, "double"));
busInfo[30] = dtaGetDataTypeElementOffset(dta, bpath,x_busId,7);
busInfo[31] = dtaGetDataTypeSize(dta, bpath, ssGetDataTypeId(S, "double"));
busInfo[32] = dtaGetDataTypeElementOffset(dta, bpath,u_busId,0);
busInfo[33] = dtaGetDataTypeSize(dta, bpath, ssGetDataTypeId(S, "double"));
busInfo[34] = dtaGetDataTypeElementOffset(dta, bpath,u_busId,1);
busInfo[35] = dtaGetDataTypeSize(dta, bpath, ssGetDataTypeId(S, "double"));
ssSetUserData(S, busInfo);
}
/* Function: mdlInitializeSizes ===============================================
* Abstract:
* Setup sizes of the various vectors.
*/
static void mdlInitializeSizes(SimStruct *S)
{
slDataTypeAccess *dta = ssGetDataTypeAccess(S);
int udtId;
ssSetNumSFcnParams(S, 0); /* Number of expected parameters */
if (ssGetNumSFcnParams(S) != ssGetSFcnParamsCount(S)) {
/* Return if number of expected != number of actual parameters */
return;
}
/* Obtain an integer datatype ID for the udt (user-defined type) "Data" */
udtId = ssRegisterDataType(S, "Data");
if ( udtId == INVALID_DTYPE_ID ) return;
/* Register the size of the udt */
if (!ssSetDataTypeSize(S, udtId, sizeof(Data))) return;
/* Register the zero of the udt */
if (!ssSetDataTypeZero(S, udtId, &zero)) return;
/* Set input-port properties */
if (!ssSetNumInputPorts(S, 1)) return;
ssSetInputPortWidth(S, 0, 1);
ssSetInputPortDataType(S, 0, SS_DOUBLE);
ssSetInputPortDirectFeedThrough(S, 0, 1);
/* Set output port properties */
if (!ssSetNumOutputPorts(S, 1)) return;
ssSetOutputPortDataType(S, 0, udtId);
ssSetOutputPortWidth(S, 0, 1);
/* Set miscellaneous properties */
ssSetNumContStates(S, 0);
ssSetNumDiscStates(S, 0);
ssSetNumSampleTimes(S, 1);
ssSetNumRWork(S, 0);
ssSetNumIWork(S, 0);
ssSetNumPWork(S, 0);
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,
SS_OPTION_WORKS_WITH_CODE_REUSE);
}
/* 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)
{
//ROS stuff
int argc = 0;
char** argv = NULL;
ros::init(argc, argv, "MATLAB_ros_sub");
spinner = new ros::AsyncSpinner(2);
spinner->start();
ros::NodeHandle n;
sub = n.subscribe("/cmu/robot_pose", 1000, robotPoseCallback);
/* Bus Information */
slDataTypeAccess *dta = ssGetDataTypeAccess(S);
const char *bpath = ssGetPath(S);
DTypeId SL_Bus_cpg_optimized_geometry_msgs_PointId = ssGetDataTypeId(S, "SL_Bus_cpg_optimized_geometry_msgs_Point");
DTypeId SL_Bus_cpg_optimized_geometry_msgs_PoseId = ssGetDataTypeId(S, "SL_Bus_cpg_optimized_geometry_msgs_Pose");
DTypeId SL_Bus_cpg_optimized_geometry_msgs_QuaternionId = ssGetDataTypeId(S, "SL_Bus_cpg_optimized_geometry_msgs_Quaternion");
DTypeId SL_Bus_cpg_optimized_geometry_msgs_Vector3Id = ssGetDataTypeId(S, "SL_Bus_cpg_optimized_geometry_msgs_Vector3");
DTypeId SL_ROS_SUB_MSGId = ssGetDataTypeId(S, "SL_ROS_SUB_MSG");
int_T *busInfo = (int_T *)malloc(46 * sizeof(int_T));
if (busInfo == NULL) {
ssSetErrorStatus(S, "Memory allocation failure");
return;
}
/* Calculate offsets of all primitive elements of the bus */
busInfo[0] = dtaGetDataTypeElementOffset(dta, bpath, SL_ROS_SUB_MSGId, 2);
busInfo[1] = 30 * dtaGetDataTypeSize(dta, bpath, ssGetDataTypeId(S, "double"));
busInfo[2] = dtaGetDataTypeElementOffset(dta, bpath, SL_ROS_SUB_MSGId, 3);
busInfo[3] = 3 * dtaGetDataTypeSize(dta, bpath, ssGetDataTypeId(S, "double"));
busInfo[4] = dtaGetDataTypeElementOffset(dta, bpath, SL_ROS_SUB_MSGId, 4);
busInfo[5] = 2 * dtaGetDataTypeSize(dta, bpath, ssGetDataTypeId(S, "double"));
busInfo[6] = dtaGetDataTypeElementOffset(dta, bpath, SL_ROS_SUB_MSGId, 0) + dtaGetDataTypeElementOffset(dta, bpath, SL_Bus_cpg_optimized_geometry_msgs_PoseId, 0) + dtaGetDataTypeElementOffset(dta, bpath, SL_Bus_cpg_optimized_geometry_msgs_PointId, 0);
busInfo[7] = dtaGetDataTypeSize(dta, bpath, ssGetDataTypeId(S, "double"));
busInfo[8] = dtaGetDataTypeElementOffset(dta, bpath, SL_ROS_SUB_MSGId, 0) + dtaGetDataTypeElementOffset(dta, bpath, SL_Bus_cpg_optimized_geometry_msgs_PoseId, 0) + dtaGetDataTypeElementOffset(dta, bpath, SL_Bus_cpg_optimized_geometry_msgs_PointId, 1);
busInfo[9] = dtaGetDataTypeSize(dta, bpath, ssGetDataTypeId(S, "double"));
busInfo[10] = dtaGetDataTypeElementOffset(dta, bpath, SL_ROS_SUB_MSGId, 0) + dtaGetDataTypeElementOffset(dta, bpath, SL_Bus_cpg_optimized_geometry_msgs_PoseId, 0) + dtaGetDataTypeElementOffset(dta, bpath, SL_Bus_cpg_optimized_geometry_msgs_PointId, 2);
busInfo[11] = dtaGetDataTypeSize(dta, bpath, ssGetDataTypeId(S, "double"));
busInfo[12] = dtaGetDataTypeElementOffset(dta, bpath, SL_ROS_SUB_MSGId, 0) + dtaGetDataTypeElementOffset(dta, bpath, SL_Bus_cpg_optimized_geometry_msgs_PoseId, 1) + dtaGetDataTypeElementOffset(dta, bpath, SL_Bus_cpg_optimized_geometry_msgs_QuaternionId, 0);
busInfo[13] = dtaGetDataTypeSize(dta, bpath, ssGetDataTypeId(S, "double"));
busInfo[14] = dtaGetDataTypeElementOffset(dta, bpath, SL_ROS_SUB_MSGId, 0) + dtaGetDataTypeElementOffset(dta, bpath, SL_Bus_cpg_optimized_geometry_msgs_PoseId, 1) + dtaGetDataTypeElementOffset(dta, bpath, SL_Bus_cpg_optimized_geometry_msgs_QuaternionId, 1);
busInfo[15] = dtaGetDataTypeSize(dta, bpath, ssGetDataTypeId(S, "double"));
busInfo[16] = dtaGetDataTypeElementOffset(dta, bpath, SL_ROS_SUB_MSGId, 0) + dtaGetDataTypeElementOffset(dta, bpath, SL_Bus_cpg_optimized_geometry_msgs_PoseId, 1) + dtaGetDataTypeElementOffset(dta, bpath, SL_Bus_cpg_optimized_geometry_msgs_QuaternionId, 2);
busInfo[17] = dtaGetDataTypeSize(dta, bpath, ssGetDataTypeId(S, "double"));
busInfo[18] = dtaGetDataTypeElementOffset(dta, bpath, SL_ROS_SUB_MSGId, 0) + dtaGetDataTypeElementOffset(dta, bpath, SL_Bus_cpg_optimized_geometry_msgs_PoseId, 1) + dtaGetDataTypeElementOffset(dta, bpath, SL_Bus_cpg_optimized_geometry_msgs_QuaternionId, 3);
busInfo[19] = dtaGetDataTypeSize(dta, bpath, ssGetDataTypeId(S, "double"));
busInfo[20] = dtaGetDataTypeElementOffset(dta, bpath, SL_ROS_SUB_MSGId, 1) + dtaGetDataTypeElementOffset(dta, bpath, SL_Bus_cpg_optimized_geometry_msgs_Vector3Id, 0);
busInfo[21] = dtaGetDataTypeSize(dta, bpath, ssGetDataTypeId(S, "double"));
busInfo[22] = dtaGetDataTypeElementOffset(dta, bpath, SL_ROS_SUB_MSGId, 1) + dtaGetDataTypeElementOffset(dta, bpath, SL_Bus_cpg_optimized_geometry_msgs_Vector3Id, 1);
busInfo[23] = dtaGetDataTypeSize(dta, bpath, ssGetDataTypeId(S, "double"));
busInfo[24] = dtaGetDataTypeElementOffset(dta, bpath, SL_ROS_SUB_MSGId, 1) + dtaGetDataTypeElementOffset(dta, bpath, SL_Bus_cpg_optimized_geometry_msgs_Vector3Id, 2);
busInfo[25] = dtaGetDataTypeSize(dta, bpath, ssGetDataTypeId(S, "double"));
busInfo[26] = dtaGetDataTypeElementOffset(dta, bpath, SL_ROS_SUB_MSGId, 5) + dtaGetDataTypeElementOffset(dta, bpath, SL_Bus_cpg_optimized_geometry_msgs_Vector3Id, 0);
busInfo[27] = dtaGetDataTypeSize(dta, bpath, ssGetDataTypeId(S, "double"));
busInfo[28] = dtaGetDataTypeElementOffset(dta, bpath, SL_ROS_SUB_MSGId, 5) + dtaGetDataTypeElementOffset(dta, bpath, SL_Bus_cpg_optimized_geometry_msgs_Vector3Id, 1);
busInfo[29] = dtaGetDataTypeSize(dta, bpath, ssGetDataTypeId(S, "double"));
busInfo[30] = dtaGetDataTypeElementOffset(dta, bpath, SL_ROS_SUB_MSGId, 5) + dtaGetDataTypeElementOffset(dta, bpath, SL_Bus_cpg_optimized_geometry_msgs_Vector3Id, 2);
busInfo[31] = dtaGetDataTypeSize(dta, bpath, ssGetDataTypeId(S, "double"));
busInfo[32] = dtaGetDataTypeElementOffset(dta, bpath, SL_ROS_SUB_MSGId, 6) + dtaGetDataTypeElementOffset(dta, bpath, SL_Bus_cpg_optimized_geometry_msgs_PoseId, 0) + dtaGetDataTypeElementOffset(dta, bpath, SL_Bus_cpg_optimized_geometry_msgs_PointId, 0);
busInfo[33] = dtaGetDataTypeSize(dta, bpath, ssGetDataTypeId(S, "double"));
busInfo[34] = dtaGetDataTypeElementOffset(dta, bpath, SL_ROS_SUB_MSGId, 6) + dtaGetDataTypeElementOffset(dta, bpath, SL_Bus_cpg_optimized_geometry_msgs_PoseId, 0) + dtaGetDataTypeElementOffset(dta, bpath, SL_Bus_cpg_optimized_geometry_msgs_PointId, 1);
busInfo[35] = dtaGetDataTypeSize(dta, bpath, ssGetDataTypeId(S, "double"));
busInfo[36] = dtaGetDataTypeElementOffset(dta, bpath, SL_ROS_SUB_MSGId, 6) + dtaGetDataTypeElementOffset(dta, bpath, SL_Bus_cpg_optimized_geometry_msgs_PoseId, 0) + dtaGetDataTypeElementOffset(dta, bpath, SL_Bus_cpg_optimized_geometry_msgs_PointId, 2);
busInfo[37] = dtaGetDataTypeSize(dta, bpath, ssGetDataTypeId(S, "double"));
busInfo[38] = dtaGetDataTypeElementOffset(dta, bpath, SL_ROS_SUB_MSGId, 6) + dtaGetDataTypeElementOffset(dta, bpath, SL_Bus_cpg_optimized_geometry_msgs_PoseId, 1) + dtaGetDataTypeElementOffset(dta, bpath, SL_Bus_cpg_optimized_geometry_msgs_QuaternionId, 0);
busInfo[39] = dtaGetDataTypeSize(dta, bpath, ssGetDataTypeId(S, "double"));
busInfo[40] = dtaGetDataTypeElementOffset(dta, bpath, SL_ROS_SUB_MSGId, 6) + dtaGetDataTypeElementOffset(dta, bpath, SL_Bus_cpg_optimized_geometry_msgs_PoseId, 1) + dtaGetDataTypeElementOffset(dta, bpath, SL_Bus_cpg_optimized_geometry_msgs_QuaternionId, 1);
busInfo[41] = dtaGetDataTypeSize(dta, bpath, ssGetDataTypeId(S, "double"));
busInfo[42] = dtaGetDataTypeElementOffset(dta, bpath, SL_ROS_SUB_MSGId, 6) + dtaGetDataTypeElementOffset(dta, bpath, SL_Bus_cpg_optimized_geometry_msgs_PoseId, 1) + dtaGetDataTypeElementOffset(dta, bpath, SL_Bus_cpg_optimized_geometry_msgs_QuaternionId, 2);
busInfo[43] = dtaGetDataTypeSize(dta, bpath, ssGetDataTypeId(S, "double"));
busInfo[44] = dtaGetDataTypeElementOffset(dta, bpath, SL_ROS_SUB_MSGId, 6) + dtaGetDataTypeElementOffset(dta, bpath, SL_Bus_cpg_optimized_geometry_msgs_PoseId, 1) + dtaGetDataTypeElementOffset(dta, bpath, SL_Bus_cpg_optimized_geometry_msgs_QuaternionId, 3);
busInfo[45] = dtaGetDataTypeSize(dta, bpath, ssGetDataTypeId(S, "double"));
ssSetUserData(S, busInfo);
}
