自学教程：C++ ssSetUserData函数代码示例

static void sf_opaque_terminate_c1_a3(void *chartInstanceVar){  if (chartInstanceVar!=NULL) {    SimStruct *S = ((SFc1_a3InstanceStruct*) chartInstanceVar)->S;    if (sim_mode_is_rtw_gen(S) || sim_mode_is_external(S)) {      sf_clear_rtw_identifier(S);      unload_a3_optimization_info();    }    finalize_c1_a3((SFc1_a3InstanceStruct*) chartInstanceVar);    utFree((void *)chartInstanceVar);    ssSetUserData(S,NULL);  }}

static void mdlStart_c9_testing_control_with_estimate(SimStruct *S){  SFc9_testing_control_with_estimateInstanceStruct *chartInstance;  chartInstance = (SFc9_testing_control_with_estimateInstanceStruct *)malloc    (sizeof(SFc9_testing_control_with_estimateInstanceStruct));  memset(chartInstance, 0, sizeof         (SFc9_testing_control_with_estimateInstanceStruct));  if (chartInstance==NULL) {    sf_mex_error_message("Could not allocate memory for chart instance.");  }  chartInstance->chartInfo.chartInstance = chartInstance;  chartInstance->chartInfo.isEMLChart = 1;  chartInstance->chartInfo.chartInitialized = 0;  chartInstance->chartInfo.sFunctionGateway =    sf_opaque_gateway_c9_testing_control_with_estimate;  chartInstance->chartInfo.initializeChart =    sf_opaque_initialize_c9_testing_control_with_estimate;  chartInstance->chartInfo.terminateChart =    sf_opaque_terminate_c9_testing_control_with_estimate;  chartInstance->chartInfo.enableChart =    sf_opaque_enable_c9_testing_control_with_estimate;  chartInstance->chartInfo.disableChart =    sf_opaque_disable_c9_testing_control_with_estimate;  chartInstance->chartInfo.getSimState =    sf_opaque_get_sim_state_c9_testing_control_with_estimate;  chartInstance->chartInfo.setSimState =    sf_opaque_set_sim_state_c9_testing_control_with_estimate;  chartInstance->chartInfo.getSimStateInfo =    sf_get_sim_state_info_c9_testing_control_with_estimate;  chartInstance->chartInfo.zeroCrossings = NULL;  chartInstance->chartInfo.outputs = NULL;  chartInstance->chartInfo.derivatives = NULL;  chartInstance->chartInfo.mdlRTW = mdlRTW_c9_testing_control_with_estimate;  chartInstance->chartInfo.mdlStart = mdlStart_c9_testing_control_with_estimate;  chartInstance->chartInfo.mdlSetWorkWidths =    mdlSetWorkWidths_c9_testing_control_with_estimate;  chartInstance->chartInfo.extModeExec = NULL;  chartInstance->chartInfo.restoreLastMajorStepConfiguration = NULL;  chartInstance->chartInfo.restoreBeforeLastMajorStepConfiguration = NULL;  chartInstance->chartInfo.storeCurrentConfiguration = NULL;  chartInstance->S = S;  ssSetUserData(S,(void *)(&(chartInstance->chartInfo)));/* register the chart instance with simstruct */  init_dsm_address_info(chartInstance);  if (!sim_mode_is_rtw_gen(S)) {  }  sf_opaque_init_subchart_simstructs(chartInstance->chartInfo.chartInstance);  chart_debug_initialization(S,1);}

static void mdlStart_c3_aircraft_2(SimStruct *S){  SFc3_aircraft_2InstanceStruct *chartInstance;  ChartRunTimeInfo * crtInfo = (ChartRunTimeInfo *)utMalloc(sizeof    (ChartRunTimeInfo));  chartInstance = (SFc3_aircraft_2InstanceStruct *)utMalloc(sizeof    (SFc3_aircraft_2InstanceStruct));  memset(chartInstance, 0, sizeof(SFc3_aircraft_2InstanceStruct));  if (chartInstance==NULL) {    sf_mex_error_message("Could not allocate memory for chart instance.");  }  chartInstance->chartInfo.chartInstance = chartInstance;  chartInstance->chartInfo.isEMLChart = 1;  chartInstance->chartInfo.chartInitialized = 0;  chartInstance->chartInfo.sFunctionGateway = sf_opaque_gateway_c3_aircraft_2;  chartInstance->chartInfo.initializeChart = sf_opaque_initialize_c3_aircraft_2;  chartInstance->chartInfo.terminateChart = sf_opaque_terminate_c3_aircraft_2;  chartInstance->chartInfo.enableChart = sf_opaque_enable_c3_aircraft_2;  chartInstance->chartInfo.disableChart = sf_opaque_disable_c3_aircraft_2;  chartInstance->chartInfo.getSimState = sf_opaque_get_sim_state_c3_aircraft_2;  chartInstance->chartInfo.setSimState = sf_opaque_set_sim_state_c3_aircraft_2;  chartInstance->chartInfo.getSimStateInfo = sf_get_sim_state_info_c3_aircraft_2;  chartInstance->chartInfo.zeroCrossings = NULL;  chartInstance->chartInfo.outputs = NULL;  chartInstance->chartInfo.derivatives = NULL;  chartInstance->chartInfo.mdlRTW = mdlRTW_c3_aircraft_2;  chartInstance->chartInfo.mdlStart = mdlStart_c3_aircraft_2;  chartInstance->chartInfo.mdlSetWorkWidths = mdlSetWorkWidths_c3_aircraft_2;  chartInstance->chartInfo.extModeExec = NULL;  chartInstance->chartInfo.restoreLastMajorStepConfiguration = NULL;  chartInstance->chartInfo.restoreBeforeLastMajorStepConfiguration = NULL;  chartInstance->chartInfo.storeCurrentConfiguration = NULL;  chartInstance->chartInfo.callAtomicSubchartUserFcn = NULL;  chartInstance->chartInfo.callAtomicSubchartAutoFcn = NULL;  chartInstance->chartInfo.debugInstance = sfGlobalDebugInstanceStruct;  chartInstance->S = S;  crtInfo->checksum = SF_RUNTIME_INFO_CHECKSUM;  crtInfo->instanceInfo = (&(chartInstance->chartInfo));  crtInfo->isJITEnabled = false;  crtInfo->compiledInfo = NULL;  ssSetUserData(S,(void *)(crtInfo));  /* register the chart instance with simstruct */  init_dsm_address_info(chartInstance);  init_simulink_io_address(chartInstance);  if (!sim_mode_is_rtw_gen(S)) {  }  sf_opaque_init_subchart_simstructs(chartInstance->chartInfo.chartInstance);  chart_debug_initialization(S,1);}

static void sf_opaque_terminate_c9_QPSK_Transmit_v12d(void *chartInstanceVar){  if (chartInstanceVar!=NULL) {    SimStruct *S = ((SFc9_QPSK_Transmit_v12dInstanceStruct*) chartInstanceVar)      ->S;    if (sim_mode_is_rtw_gen(S) || sim_mode_is_external(S)) {      sf_clear_rtw_identifier(S);    }    finalize_c9_QPSK_Transmit_v12d((SFc9_QPSK_Transmit_v12dInstanceStruct*)      chartInstanceVar);    free((void *)chartInstanceVar);    ssSetUserData(S,NULL);  }  unload_QPSK_Transmit_v12d_optimization_info();}

static void sf_opaque_terminate_c2_ArregloSeisPorSeis(void *chartInstanceVar){  if (chartInstanceVar!=NULL) {    SimStruct *S = ((SFc2_ArregloSeisPorSeisInstanceStruct*) chartInstanceVar)      ->S;    if (sim_mode_is_rtw_gen(S) || sim_mode_is_external(S)) {      sf_clear_rtw_identifier(S);    }    finalize_c2_ArregloSeisPorSeis((SFc2_ArregloSeisPorSeisInstanceStruct*)      chartInstanceVar);    free((void *)chartInstanceVar);    ssSetUserData(S,NULL);  }  unload_ArregloSeisPorSeis_optimization_info();}

static void sf_opaque_terminate_c6_motor_control_with_homebrew_SVPWM(void  *chartInstanceVar){  if (chartInstanceVar!=NULL) {    SimStruct *S = ((SFc6_motor_control_with_homebrew_SVPWMInstanceStruct*)                    chartInstanceVar)->S;    if (sim_mode_is_rtw_gen(S) || sim_mode_is_external(S)) {      sf_clear_rtw_identifier(S);    }    finalize_c6_motor_control_with_homebrew_SVPWM      ((SFc6_motor_control_with_homebrew_SVPWMInstanceStruct*) chartInstanceVar);    free((void *)chartInstanceVar);    ssSetUserData(S,NULL);  }  unload_motor_control_with_homebrew_SVPWM_optimization_info();}

static void sf_opaque_terminate_c2_MigrationOWBG_Proto4_1DLinear2DNonLinear(void  *chartInstanceVar){  if (chartInstanceVar!=NULL) {    SimStruct *S = ((SFc2_MigrationOWBG_Proto4_1DLinear2DNonLinearInstanceStruct*)                    chartInstanceVar)->S;    if (sim_mode_is_rtw_gen(S) || sim_mode_is_external(S)) {      sf_clear_rtw_identifier(S);      unload_MigrationOWBG_Proto4_1DLinear2DNonLinear_optimization_info();    }    finalize_c2_MigrationOWBG_Proto4_1DLinear2DNonLinear      ((SFc2_MigrationOWBG_Proto4_1DLinear2DNonLinearInstanceStruct*)       chartInstanceVar);    utFree((void *)chartInstanceVar);    ssSetUserData(S,NULL);  }}

static void sf_opaque_terminate_c2_SALTStabilizationInnerOuterLoopSIM(void  *chartInstanceVar){  if (chartInstanceVar!=NULL) {    SimStruct *S = ((SFc2_SALTStabilizationInnerOuterLoopSIMInstanceStruct*)                    chartInstanceVar)->S;    if (sim_mode_is_rtw_gen(S) || sim_mode_is_external(S)) {      sf_clear_rtw_identifier(S);    }    finalize_c2_SALTStabilizationInnerOuterLoopSIM      ((SFc2_SALTStabilizationInnerOuterLoopSIMInstanceStruct*) chartInstanceVar);    free((void *)chartInstanceVar);    ssSetUserData(S,NULL);  }  unload_SALTStabilizationInnerOuterLoopSIM_optimization_info();}

static void sf_opaque_terminate_c1_VrSubsystem(void *chartInstanceVar){  if (chartInstanceVar!=NULL) {    SimStruct *S = ((SFc1_VrSubsystemInstanceStruct*) chartInstanceVar)->S;    if (sim_mode_is_rtw_gen(S) || sim_mode_is_external(S)) {      sf_clear_rtw_identifier(S);      unload_VrSubsystem_optimization_info();    }    finalize_c1_VrSubsystem((SFc1_VrSubsystemInstanceStruct*) chartInstanceVar);    utFree(chartInstanceVar);    if (ssGetUserData(S)!= NULL) {      sf_free_ChartRunTimeInfo(S);    }    ssSetUserData(S,NULL);  }}

static void mdlStart_c31_adcs_v15_integral_Power_no_charge_in_detumb(SimStruct  *S){  chartInstance.chartInfo.chartInstance = NULL;  chartInstance.chartInfo.isEMLChart = 1;  chartInstance.chartInfo.chartInitialized = 0;  chartInstance.chartInfo.sFunctionGateway =    sf_opaque_gateway_c31_adcs_v15_integral_Power_no_charge_in_detumb;  chartInstance.chartInfo.initializeChart =    sf_opaque_initialize_c31_adcs_v15_integral_Power_no_charge_in_detumb;  chartInstance.chartInfo.terminateChart =    sf_opaque_terminate_c31_adcs_v15_integral_Power_no_charge_in_detumb;  chartInstance.chartInfo.enableChart =    sf_opaque_enable_c31_adcs_v15_integral_Power_no_charge_in_detumb;  chartInstance.chartInfo.disableChart =    sf_opaque_disable_c31_adcs_v15_integral_Power_no_charge_in_detumb;  chartInstance.chartInfo.getSimState =    sf_opaque_get_sim_state_c31_adcs_v15_integral_Power_no_charge_in_detumb;  chartInstance.chartInfo.setSimState =    sf_opaque_set_sim_state_c31_adcs_v15_integral_Power_no_charge_in_detumb;  chartInstance.chartInfo.getSimStateInfo =    sf_get_sim_state_info_c31_adcs_v15_integral_Power_no_charge_in_detumb;  chartInstance.chartInfo.zeroCrossings = NULL;  chartInstance.chartInfo.outputs = NULL;  chartInstance.chartInfo.derivatives = NULL;  chartInstance.chartInfo.mdlRTW =    mdlRTW_c31_adcs_v15_integral_Power_no_charge_in_detumb;  chartInstance.chartInfo.mdlStart =    mdlStart_c31_adcs_v15_integral_Power_no_charge_in_detumb;  chartInstance.chartInfo.mdlSetWorkWidths =    mdlSetWorkWidths_c31_adcs_v15_integral_Power_no_charge_in_detumb;  chartInstance.chartInfo.extModeExec =    sf_opaque_ext_mode_exec_c31_adcs_v15_integral_Power_no_charge_in_detumb;  chartInstance.chartInfo.restoreLastMajorStepConfiguration = NULL;  chartInstance.chartInfo.restoreBeforeLastMajorStepConfiguration = NULL;  chartInstance.chartInfo.storeCurrentConfiguration = NULL;  chartInstance.S = S;  ssSetUserData(S,(void *)(&(chartInstance.chartInfo)));/* register the chart instance with simstruct */  if (!sim_mode_is_rtw_gen(S)) {    init_dsm_address_info();  }  chart_debug_initialization(S,1);}

static void sf_opaque_terminate_c2_old_Demo_RPS(void *chartInstanceVar){  if (chartInstanceVar!=NULL) {    SimStruct *S = ((SFc2_old_Demo_RPSInstanceStruct*) chartInstanceVar)->S;    ChartRunTimeInfo * crtInfo = (ChartRunTimeInfo *)(ssGetUserData(S));    if (sim_mode_is_rtw_gen(S) || sim_mode_is_external(S)) {      sf_clear_rtw_identifier(S);      unload_old_Demo_RPS_optimization_info();    }    finalize_c2_old_Demo_RPS((SFc2_old_Demo_RPSInstanceStruct*) chartInstanceVar);    utFree((void *)chartInstanceVar);    if (crtInfo != NULL) {      utFree((void *)crtInfo);    }    ssSetUserData(S,NULL);  }}

static void sf_opaque_terminate_c1_Engine_Vehicle_CVT_RS_System2(void  *chartInstanceVar){  if (chartInstanceVar!=NULL) {    SimStruct *S = ((SFc1_Engine_Vehicle_CVT_RS_System2InstanceStruct*)                    chartInstanceVar)->S;    ChartRunTimeInfo * crtInfo = (ChartRunTimeInfo *)(ssGetUserData(S));    if (sim_mode_is_rtw_gen(S) || sim_mode_is_external(S)) {      sf_clear_rtw_identifier(S);      unload_Engine_Vehicle_CVT_RS_System2_optimization_info();    }    finalize_c1_Engine_Vehicle_CVT_RS_System2      ((SFc1_Engine_Vehicle_CVT_RS_System2InstanceStruct*) chartInstanceVar);    utFree(chartInstanceVar);    if (crtInfo != NULL) {      utFree(crtInfo);    }    ssSetUserData(S,NULL);  }}

static void sf_opaque_terminate_c3_ARP_02_RPSsmile_Glove_atomicError(void  *chartInstanceVar){  if (chartInstanceVar!=NULL) {    SimStruct *S = ((SFc3_ARP_02_RPSsmile_Glove_atomicErrorInstanceStruct*)                    chartInstanceVar)->S;    ChartRunTimeInfo * crtInfo = (ChartRunTimeInfo *)(ssGetUserData(S));    if (sim_mode_is_rtw_gen(S) || sim_mode_is_external(S)) {      sf_clear_rtw_identifier(S);      unload_ARP_02_RPSsmile_Glove_atomicError_optimization_info();    }    finalize_c3_ARP_02_RPSsmile_Glove_atomicError      ((SFc3_ARP_02_RPSsmile_Glove_atomicErrorInstanceStruct*) chartInstanceVar);    utFree((void *)chartInstanceVar);    if (crtInfo != NULL) {      utFree((void *)crtInfo);    }    ssSetUserData(S,NULL);  }}

static void sf_opaque_terminate_c8_AllPurposeModel_TyreRelaxation(void  *chartInstanceVar){  if (chartInstanceVar!=NULL) {    SimStruct *S = ((SFc8_AllPurposeModel_TyreRelaxationInstanceStruct*)                    chartInstanceVar)->S;    ChartRunTimeInfo * crtInfo = (ChartRunTimeInfo *)(ssGetUserData(S));    if (sim_mode_is_rtw_gen(S) || sim_mode_is_external(S)) {      sf_clear_rtw_identifier(S);      unload_AllPurposeModel_TyreRelaxation_optimization_info();    }    finalize_c8_AllPurposeModel_TyreRelaxation      ((SFc8_AllPurposeModel_TyreRelaxationInstanceStruct*) chartInstanceVar);    utFree(chartInstanceVar);    if (crtInfo != NULL) {      utFree(crtInfo);    }    ssSetUserData(S,NULL);  }}

static void mdlStart_c3_MPC_framework(SimStruct *S){  chartInstance.chartInfo.chartInstance = NULL;  chartInstance.chartInfo.isEMLChart = 1;  chartInstance.chartInfo.chartInitialized = 0;  chartInstance.chartInfo.sFunctionGateway = sf_opaque_gateway_c3_MPC_framework;  chartInstance.chartInfo.initializeChart =    sf_opaque_initialize_c3_MPC_framework;  chartInstance.chartInfo.terminateChart = sf_opaque_terminate_c3_MPC_framework;  chartInstance.chartInfo.enableChart = sf_opaque_enable_c3_MPC_framework;  chartInstance.chartInfo.disableChart = sf_opaque_disable_c3_MPC_framework;  chartInstance.chartInfo.mdlRTW = mdlRTW_c3_MPC_framework;  chartInstance.chartInfo.mdlStart = mdlStart_c3_MPC_framework;  chartInstance.chartInfo.mdlSetWorkWidths = mdlSetWorkWidths_c3_MPC_framework;  chartInstance.chartInfo.restoreLastMajorStepConfiguration = NULL;  chartInstance.chartInfo.restoreBeforeLastMajorStepConfiguration = NULL;  chartInstance.chartInfo.storeCurrentConfiguration = NULL;  chartInstance.S = S;  ssSetUserData(S,(void *)(&(chartInstance.chartInfo))); /* register the chart instance with simstruct */  ssSetmdlSimulationContextIO(S, sf_c3_MPC_framework_sim_ctx_io);  if(!sim_mode_is_rtw_gen(S)) {    init_dsm_address_info();  }}

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

static void mdlInitializeSizes(SimStruct *S){  const mxArray *arg;  ssSetNumSFcnParams(S, 4);  /* Number of expected parameters */  if (ssGetNumSFcnParams(S) != ssGetSFcnParamsCount(S)) {    return; /* Parameter mismatch will be reported by Simulink */  }  if (!ssSetNumInputPorts(S, 5)) return;  if (!ssSetNumOutputPorts(S, 1)) return;  SMsys *smsys = new SMsys;  smsys->trigger = 0.0;  arg = ssGetSFcnParam(S, 0);  if (mxIsDoubleScalar(arg)) {    smsys->networkNbr  = (int)*mxGetPr(arg);  }  arg = ssGetSFcnParam(S, 1);  if (mxIsDoubleScalar(arg)) {    smsys->sender  = (int)*mxGetPr(arg);  }  arg = ssGetSFcnParam(S, 2);  if (mxIsDoubleScalar(arg)) {    smsys->indim  = (int)*mxGetPr(arg);  }  arg = ssGetSFcnParam(S,3);  if(mxIsDoubleScalar(arg)) {    smsys->dynamicSegment = (int)*mxGetPr(arg)-1; // set smsys->dynamicSegment = 0 (static) or 1 (dynamic)  }    /* Input ports */  ssSetInputPortDirectFeedThrough(S, 0, 1);   ssSetInputPortDirectFeedThrough(S, 1, 1);  ssSetInputPortDirectFeedThrough(S, 2, 1);  ssSetInputPortDirectFeedThrough(S, 3, 1);  ssSetInputPortDirectFeedThrough(S, 4, 1);  ssSetInputPortWidth(S, 0, 1);             // receiver  ssSetInputPortWidth(S, 1, smsys->indim);  // data  ssSetInputPortWidth(S, 2, 1);             // length  ssSetInputPortWidth(S, 3, 1);             // prio  ssSetInputPortWidth(S, 4, 1);             // msgID  /* Output Ports */  ssSetOutputPortWidth(S, 0, 1);            // snd trigger  ssSetNumContStates(S, 0);  ssSetNumDiscStates(S, 0);    ssSetNumSampleTimes(S, 1);      ssSetNumRWork(S, 0);  ssSetNumIWork(S, 0);  ssSetNumPWork(S, 0);   ssSetNumModes(S, 0);  ssSetNumNonsampledZCs(S, 0);  ssSetUserData(S, smsys);    ssSetOptions(S, SS_OPTION_CALL_TERMINATE_ON_EXIT); }

//.........这里部分代码省略.........      printf("Init function takes wrong number (> 1) of arguments!/n");      ssSetErrorStatus(S, "Init function takes wrong number (> 1) of arguments!");      return;    }  }#else  /* Save pointer to init args */  rtsys->initarg = (mxArray *)ssGetSFcnParam(S, 1);  /* Evaluating user-defined init function (C++) */  init();  rtsys->init_phase = false;#endif    if (!rtsys->initialized) {    printf("ttInitKernel was not called in init function!/n");    ssSetErrorStatus(S, "ttInitKernel was not called in init function!");    return;  }  // Clock drift parameters  const mxArray *arg;  arg = ssGetSFcnParam(S, 2);  if (mxIsDoubleScalar(arg)) {    rtsys->clockDrift = *mxGetPr(arg) + 1;  }  arg = ssGetSFcnParam(S, 3);  if (mxIsDoubleScalar(arg)) {    rtsys->clockOffset = *mxGetPr(arg);  }  //printf("drift: %f, offset:%f/n", rtsys->clockDrift, rtsys->clockOffset);  if (!ssSetNumInputPorts(S, 4)) return;  ssSetInputPortDirectFeedThrough(S, 0, 0);  ssSetInputPortDirectFeedThrough(S, 1, 0);  ssSetInputPortDirectFeedThrough(S, 2, 0);  ssSetInputPortDirectFeedThrough(S, 3, 0);  if (!ssSetNumOutputPorts(S, 5)) return;  /* Input Ports */  if (rtsys->nbrOfInputs > 0)     ssSetInputPortWidth(S, 0, rtsys->nbrOfInputs);  else    ssSetInputPortWidth(S, 0, 1);    if (rtsys->nbrOfTriggers > 0)     ssSetInputPortWidth(S, 1, rtsys->nbrOfTriggers);  else    ssSetInputPortWidth(S, 1, 1);  if (rtsys->nbrOfNetworks > 0)     ssSetInputPortWidth(S, 2, rtsys->nbrOfNetworks); /* Network receive */  else    ssSetInputPortWidth(S, 2, 1);  ssSetInputPortWidth(S, 3, 1); //battery  /* Output Ports */  if (rtsys->nbrOfOutputs > 0)     ssSetOutputPortWidth(S, 0, rtsys->nbrOfOutputs);  else    ssSetOutputPortWidth(S, 0, 1);  if (rtsys->nbrOfNetworks > 0)     ssSetOutputPortWidth(S, 1, (rtsys->nbrOfNetworks)); /* Network send */  else    ssSetOutputPortWidth(S, 1, 1);  if (rtsys->nbrOfSchedTasks+rtsys->nbrOfSchedHandlers > 0)     ssSetOutputPortWidth(S, 2, rtsys->nbrOfSchedTasks+rtsys->nbrOfSchedHandlers);  else    ssSetOutputPortWidth(S, 2, 1);  if (rtsys->nbrOfSchedMonitors > 0)     ssSetOutputPortWidth(S, 3, rtsys->nbrOfSchedMonitors*rtsys->nbrOfTasks);  else    ssSetOutputPortWidth(S, 3, 1);  ssSetOutputPortWidth(S, 4, 1); //Energy consumption  ssSetNumContStates(S, 0);  ssSetNumDiscStates(S, 0);    ssSetNumSampleTimes(S, 1);      ssSetNumRWork(S, 0);  ssSetNumIWork(S, 0);  ssSetNumPWork(S, 0);   ssSetNumModes(S, 0);  ssSetNumNonsampledZCs(S, 1);  ssSetUserData(S, rtsys);    ssSetOptions(S, SS_OPTION_EXCEPTION_FREE_CODE | SS_OPTION_CALL_TERMINATE_ON_EXIT); }

  static void mdlInitializeSizes(SimStruct *S)  {    const mxArray *arg;    ssSetNumSFcnParams(S, 5);  /* Number of expected parameters */    if (ssGetNumSFcnParams(S) != ssGetSFcnParamsCount(S)) {      return; /* Parameter mismatch will be reported by Simulink */    }      // Parse second argument only, to determine nbrOfNodes    // 2 - Number of nodes    int nbrOfNodes = 0;    arg = ssGetSFcnParam(S, 1);    if (mxIsDoubleScalar(arg)) {      nbrOfNodes = (int) *mxGetPr(arg);    }    if (nbrOfNodes <= 0) {      ssSetErrorStatus(S, "TrueTime Ultrasound Network: The number of nodes must be an integer > 0");      return;    }    ssSetNumContStates(S, 0);    ssSetNumDiscStates(S, 0);      ssSetNumInputPorts(S, 3);    ssSetInputPortDirectFeedThrough(S, 0, 0);    ssSetInputPortWidth(S, 0, nbrOfNodes);    ssSetInputPortWidth(S, 1, nbrOfNodes);    ssSetInputPortWidth(S, 2, nbrOfNodes);      ssSetNumOutputPorts(S, 2);    ssSetOutputPortWidth(S, 0, nbrOfNodes);    ssSetOutputPortWidth(S, 1, nbrOfNodes);    ssSetNumSampleTimes(S, 1);      ssSetNumRWork(S, 0);    ssSetNumIWork(S, 0);    ssSetNumPWork(S, 0);     ssSetNumModes(S, 0);    ssSetNumNonsampledZCs(S, 1);    // Make sure cleanup is performed even if errors occur    ssSetOptions(S, SS_OPTION_CALL_TERMINATE_ON_EXIT);    int i;    // Create new network struct    RTnetwork *nwsys = new RTnetwork;    ssSetUserData(S, nwsys); // save pointer in UserData    // Arg 1 - Network Number    nwsys->networkNbr = 0;    arg = ssGetSFcnParam(S, 0);    if (mxIsDoubleScalar(arg)) {      nwsys->networkNbr = (int) *mxGetPr(arg);    }     if (nwsys->networkNbr <= 0) {      ssSetErrorStatus(S, "TrueTime Ultrasound Network: The network number must be > 0");      return;    }    // Arg 2 - Number of nodes    arg = ssGetSFcnParam(S, 1);    nwsys->nbrOfNodes = (int) *mxGetPr(arg); // we know it's right    //mexPrintf("nbrOfNodes: %d/n", nwsys->nbrOfNodes);        // Arg 3 - Reach    nwsys->reach = 0.0;    arg = ssGetSFcnParam(S, 2);    if (mxIsDoubleScalar(arg)) {      nwsys->reach = *mxGetPr(arg);    }    if (nwsys->reach < 0.0) {      ssSetErrorStatus(S, "TrueTime Ultrasound Network: The reach must be >= 0");      return;    }    //mexPrintf("reach: %f/n", nwsys->reach);    // Arg 4 - Ping length    nwsys->pinglength = 0.0;    arg = ssGetSFcnParam(S, 3);    if (mxIsDoubleScalar(arg)) {      nwsys->pinglength = *mxGetPr(arg);    }    if (nwsys->pinglength < 0.0) {      ssSetErrorStatus(S, "TrueTime Ultrasound Network: The ping length must be >= 0");      return;    }    //mexPrintf("ping length: %f/n", nwsys->pinglength);    // Arg 5 - Speed of sound    nwsys->speedofsound = 0.0;    arg = ssGetSFcnParam(S, 4);    if (mxIsDoubleScalar(arg)) {      nwsys->speedofsound = *mxGetPr(arg);    }    if (nwsys->speedofsound <= 0.0) {//.........这里部分代码省略.........

//.........这里部分代码省略.........      rtsys->init_phase = false;    }  } else {    TT_CALLBACK_ERROR(S, "Init function takes wrong number (> 1) of arguments!");    return;  }  if (rtsys->error) {  error:    mexCallMATLAB(1 ,error_msg_array, 0, NULL, "lasterr");     error_msg = mxArrayToString(error_msg_array[0]);    snprintf(errbuf, MAXERRBUF, "Error in init function '%s'/n%s", initfun, error_msg);    mxFree(error_msg);    TT_CALLBACK_ERROR(S, errbuf);    return;  }  #else  /* Save pointer to init args */  mxArray *initArg = (mxArray *)ssGetSFcnParam(S, 1);  rtsys->initArg = initArg;  /* Evaluating user-defined init function (C++) */  init();  if (rtsys->error) {    TT_RUNKERNEL_ERROR(errbuf);    mexPrintf("??? Error in init() function/n%s/n/n", errbuf);    mexPrintf("In block ==> '%s'/nSimulation aborted!/n", ssGetBlockName(S));    ssSetErrorStatus(S, "");    return;  }  rtsys->init_phase = false;#endif    if (!rtsys->initialized) {    TT_CALLBACK_ERROR(S, "ttInitKernel was not called in init function");    return;  }  if (!ssSetNumInputPorts(S, 4)) return;  ssSetInputPortDirectFeedThrough(S, 0, 0);  ssSetInputPortDirectFeedThrough(S, 1, 0);  ssSetInputPortDirectFeedThrough(S, 2, 0);  ssSetInputPortDirectFeedThrough(S, 3, 0);  if (!ssSetNumOutputPorts(S, 4)) return;  /* Input Ports */  if (rtsys->nbrOfInputs > 0)     ssSetInputPortWidth(S, 0, rtsys->nbrOfInputs);  else    ssSetInputPortWidth(S, 0, 1);    if (rtsys->nbrOfTriggers > 0)     ssSetInputPortWidth(S, 1, rtsys->nbrOfTriggers);  else    ssSetInputPortWidth(S, 1, 1);  if (rtsys->nbrOfNetworks > 0) {    ssSetInputPortWidth(S, 2, rtsys->nbrOfNetworks); /* Network receive */  }  else    ssSetInputPortWidth(S, 2, 1);  ssSetInputPortWidth(S, 3, 1); //battery  /* Output Ports */  if (rtsys->nbrOfOutputs > 0)     ssSetOutputPortWidth(S, 0, rtsys->nbrOfOutputs);  else    ssSetOutputPortWidth(S, 0, 1);  if (rtsys->nbrOfNetworks > 0)     ssSetOutputPortWidth(S, 1, (rtsys->nbrOfNetworks)); /* Network send */  else    ssSetOutputPortWidth(S, 1, 1);  if (rtsys->nbrOfSchedTasks > 0)     ssSetOutputPortWidth(S, 2, rtsys->nbrOfSchedTasks * rtsys->nbrOfCPUs);  else    ssSetOutputPortWidth(S, 2, 1);  ssSetOutputPortWidth(S, 3, 1); //Energy consumption  ssSetNumContStates(S, 0);  ssSetNumDiscStates(S, 0);    ssSetNumSampleTimes(S, 1);      ssSetNumRWork(S, 0);  ssSetNumIWork(S, 0);  ssSetNumPWork(S, 0);   ssSetNumModes(S, 0);  ssSetNumNonsampledZCs(S, 1);  ssSetUserData(S, rtsys);    ssSetOptions(S, SS_OPTION_EXCEPTION_FREE_CODE | SS_OPTION_CALL_TERMINATE_ON_EXIT); }