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