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