/* 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); }