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);
}
/* Function: mdlTerminate =====================================================
* Abstract:
* No termination needed, but we are required to have this routine.
*/
static void mdlTerminate(SimStruct *S)
{
ParStruc *Par = ssGetUserData(S);
if(Par != NULL){
free(Par->Tree);
if (!Par->IsLinear){
free(Par->ttv);
free(Par->ttm);
free(Par->lfmin);
free(Par->lfmax);
}
free(Par);
}
ssSetUserData(S, NULL);
}
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 mdlStart(SimStruct *S)
{
ParStruc *Par = malloc(sizeof *Par); /* Wavenet's parameters */
#if defined(MATLAB_MEX_FILE)
int_T *Ir = ssGetJacobianIr(S);
int_T *Jc = ssGetJacobianJc(S);
#endif
if (Par == NULL){
free(Par);
ssSetErrorStatus(S, "Could not allocate data cache memory.");
return;
}
/* Populate Parameters */
Par->NumberOfUnits = (uint_T) mxGetScalar(NUMUNITS(S));
/*Par->BreakPoints = mxGetData(PAR_BREAKPOINTS(S));*/
Par->LinearCoef = mxGetData(PAR_LINEARCOEF(S));
Par->OutputOffset = mxGetScalar(PAR_OUTPUTOFFSET(S));
Par->OutputCoef = mxGetData(PAR_OUTPUTCOEF(S));
Par->Translation = mxGetData(PAR_TRANSLATION(S));
/* dimensions */
Par->DimXlin = (uint_T) mxGetM(PAR_LINEARCOEF(S));
/* Set the cached data into the user data area for 'this' block. */
ssSetUserData(S, Par);
/* Finish the initialization */
mdlProcessParameters(S);
#if defined(MATLAB_MEX_FILE)
/* Jacobian (dy/dR, R:=regressors) set up */
/* If linearization is disabled, we'll have no storage */
if ((Ir == NULL) || (Jc == NULL)) return;
/* DimInp = 1 for PWLINEAR */
Ir[0] = 0;
Jc[0] = 0;
#endif
}
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);
}
}
/* Function: mdlInitializeSizes ===============================================
* Abstract:
* The sizes information is used by Simulink to determine the S-function
* block's characteristics (number of inputs, outputs, states, etc.).
*/
static void mdlInitializeSizes(SimStruct *S)
{
ssSetNumSFcnParams(S, NUM_PARAMS); /* Number of expected parameters */
#if defined(MATLAB_MEX_FILE)
if (ssGetNumSFcnParams(S) == ssGetSFcnParamsCount(S)) {
mdlCheckParameters(S);
if (ssGetErrorStatus(S) != NULL) return;
} else{
return; /* Simulink will report a parameter mismatch error */
}
#endif
ssSetNumContStates(S, 0);
ssSetNumDiscStates(S, 0);
ssSetSimStateCompliance(S, USE_DEFAULT_SIM_STATE);
if (!ssSetNumInputPorts(S, 1)) return;
ssSetInputPortWidth(S, 0, mxGetScalar(NUMREG(S)));
ssSetInputPortDirectFeedThrough(S, 0, 1); /* this is s static fcn: y = f(u) */
ssSetInputPortRequiredContiguous(S, 0, 1);
/* there is only one scalar output */
if (!ssSetNumOutputPorts(S, 1)) return;
ssSetOutputPortWidth(S, 0, 1);
ssSetNumSampleTimes(S, 1);
ssSetNumRWork(S, 0);
ssSetNumIWork(S, 0);
ssSetNumPWork(S, 0);
ssSetNumModes(S, 0);
ssSetNumNonsampledZCs(S, 0);
ssSetOptions(S, SS_OPTION_EXCEPTION_FREE_CODE);
/* Preemptively clear user data pointer so we don't try to free it */
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);
}
static void mdlInitializeSizes(SimStruct *S)
{
static int printed = 0;
if (!printed) {
printed = 1;
printf("-------------------------------------------------------\n");
printf(" TrueTime, Version 1.5\n");
printf(" Copyright (c) 2007\n");
printf(" Martin Ohlin, Dan Henriksson and Anton Cervin\n");
printf(" Department of Automatic Control LTH\n");
printf(" Lund University, Sweden\n");
printf("-------------------------------------------------------\n");
}
#ifdef KERNEL_MATLAB
char initfun[100];
static mxArray *lhs[1]; // warning: used multiple times
static mxArray *rhs[3]; // warning: used multiple times
segArray = mxCreateDoubleScalar(0.0);
destroyed = false;
mexMakeArrayPersistent(segArray);
#endif
ssSetNumSFcnParams(S, 4); /* Number of expected parameters */
if (ssGetNumSFcnParams(S) != ssGetSFcnParamsCount(S)) {
return; /* Parameter mismatch will be reported by Simulink */
}
#ifdef KERNEL_MATLAB
mxGetString(ssGetSFcnParam(S, 0), initfun, 100);
#endif
rtsys = new RTsys;
/* Assign function pointers */
rtsys->contextSwitchCode = contextSwitchCode;
rtsys->periodicTaskHandlerCode = periodicTaskHandlerCode;
rtsys->timeSort = timeSort;
rtsys->prioSort = prioSort;
rtsys->default_arrival = default_arrival;
rtsys->default_release = default_release;
rtsys->default_start = default_start;
rtsys->default_suspend = default_suspend;
rtsys->default_resume = default_resume;
rtsys->default_finish = default_finish;
rtsys->prioFP = prioFP;
rtsys->prioRM = prioRM;
rtsys->prioEDF = prioEDF;
rtsys->prioDM = prioDM;
#ifdef KERNEL_MATLAB
mexSetTrapFlag(1); // return control to the MEX file after an error
/* Write rtsys pointer to global workspace */
char rtsysbuf[100];
sprintf(rtsysbuf, "%p", rtsys);
mxArray* var = mxCreateDoubleScalar(0.0);
mexMakeArrayPersistent(var);
*((long *)mxGetPr(var)) = (long)rtsys;
mexPutVariable("global", "_rtsys", var);
rtsys->rtsysptr = (mxArray*)mexGetVariablePtr("global", "_rtsys");
/* Evaluating user-defined init function (MATLAB) */
rhs[0] = mxCreateString(initfun);
if (mexCallMATLAB(1, lhs, 1, rhs, "nargin") != 0) {
printf("Call to init function failed!\n");
ssSetErrorStatus(S, "Call to init function failed!");
return;
} else {
if (*mxGetPr(lhs[0]) == 0) {
if (mexCallMATLAB(0, NULL, 0, NULL, initfun) != 0) {
printf("Call to init function failed!\n");
ssSetErrorStatus(S, "Call to init function failed!");
return;
} else {
rtsys->init_phase = false;
}
} else if (*mxGetPr(lhs[0]) == 1) {
rhs[0] = (mxArray *)ssGetSFcnParam(S, 1);
if (mexCallMATLAB(0, NULL, 1, rhs, initfun) != 0) {
printf("Call to init function failed!\n");
ssSetErrorStatus(S, "Call to init function failed!");
return;
} else {
rtsys->init_phase = false;
}
} else {
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 mdlStart(SimStruct *S)
{
ParStruc *Par = malloc(sizeof *Par); /* Tree Partition's parameters */
TreeStruc *Tree = malloc(sizeof *Tree); /* struct for Parameters.Tree struct */
#if defined(MATLAB_MEX_FILE)
int_T *Ir = ssGetJacobianIr(S);
int_T *Jc = ssGetJacobianJc(S);
int_T k;
#endif
/* for some reason, dim is one more than num reg (Anatoli would know why) */
uint_T DimInp = (uint_T) mxGetScalar(NUMREG(S))+1;
boolean_T IsLinear= (boolean_T) mxIsEmpty(TREE_TREELEVELPNTR(S));
uint_T MaxLvl = 0;
if ((Tree == NULL) || (Par == NULL)){
free(Tree);
free(Par);
ssSetErrorStatus(S, "Could not allocate data cache memory.");
return;
}
/* Populate Parameters */
Par->NumberOfUnits = (uint_T) mxGetScalar(NUMUNITS(S));
Par->Threshold = mxGetScalar(OPT_THRESHOLD(S));
Par->RegressorMean = mxGetData(PAR_REGRESSORMEAN(S));
Par->OutputOffset = mxGetScalar(PAR_OUTPUTOFFSET(S));
Par->LinearCoef = mxGetData(PAR_LINEARCOEF(S));
Par->SampleLength = (uint_T) mxGetScalar(PAR_SAMPLELENGTH(S));
Par->NoiseVariance = mxGetScalar(PAR_NOISEVARIANCE(S));
Par->IsLinear = IsLinear;
/* Populate Tree */
if (!IsLinear){
Tree->TreeLevelPntr = mxGetData(TREE_TREELEVELPNTR(S));
MaxLvl = (uint_T) Tree->TreeLevelPntr[Par->NumberOfUnits-1];
Tree->AncestorDescendantPntr = mxGetData(TREE_ANCESTORDESCENDANTPNTR(S));
Tree->LocalizingVectors = mxGetData(TREE_LOCALIZINGVECTORS(S));
Tree->LocalCovMatrix = mxGetData(TREE_LOCALCOVMATRIX(S));
Tree->LocalParVector = mxGetData(TREE_LOCALPARVECTOR(S));
/* allocate memory for temporary arrays required by evaluate_treepartition */
Par->ttv = (real_T *) malloc(DimInp*sizeof(real_T));
Par->ttm = (real_T *) malloc(DimInp*DimInp*sizeof(real_T));
Par->lfmax = (real_T *) malloc(MaxLvl*sizeof(real_T));
Par->lfmin = (real_T *) malloc(MaxLvl*sizeof(real_T));
/* Memory allocation error checking */
if ( (Par->ttv == NULL) || (Par->ttm == NULL) ||
(Par->lfmax == NULL) || (Par->lfmin == NULL) )
{
free(Tree);
free(Par);
ssSetErrorStatus(S, "Could not allocate data cache memory.");
return;
} /* Error checking complete */
}
Par->Tree = Tree;
/* Set the cached data into the user data area for 'this' block. */
ssSetUserData(S, Par);
/* Finish the initialization */
mdlProcessParameters(S);
#if defined(MATLAB_MEX_FILE)
/* Jacobian (dy/dR, R:=regressors) set up */
/* If linearization is disabled, we'll have no storage */
if ((Ir == NULL) || (Jc == NULL)) return;
for (k = 0; k < DimInp-1; k++) {
Ir[k] = 0;
Jc[k] = k;
}
#endif
}
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) {
ssSetErrorStatus(S, "TrueTime Ultrasound Network: The speed of sound must be > 0");
return;
}
//mexPrintf("speedofsound: %f\n", nwsys->speedofsound);
/* Write pointer to Simulink block UserData */
/* mexCallMATLAB(1, lhs, 0, NULL, "gcbh");
sprintf(nwsysp,"%p",nwsys);
rhs[0] = mxCreateDoubleMatrix(1,1,mxREAL);
*mxGetPr(rhs[0]) = *mxGetPr(lhs[0]);
rhs[1] = mxCreateString("UserData");
rhs[2] = mxCreateString(nwsysp);
mexCallMATLAB(0, NULL, 3, rhs, "set_param"); */
/* Write pointer to MATLAB global workspace */
/* FIX: The code above is intended to be removed and replaced by this. */
/* Write rtsys pointer to global workspace */
mxArray* var = mxCreateDoubleScalar(0.0);
mexMakeArrayPersistent(var);
*((void **)mxGetData(var)) = nwsys;
char nwsysbuf[MAXCHARS];
sprintf(nwsysbuf, "_nwsys_%d", nwsys->networkNbr);
mexPutVariable("global", nwsysbuf, var);
nwsys->inputs = new double[nwsys->nbrOfNodes];
nwsys->oldinputs = new double[nwsys->nbrOfNodes];
nwsys->outputs = new double[nwsys->nbrOfNodes];
nwsys->sendschedule = new double[nwsys->nbrOfNodes];
for (i=0; i<nwsys->nbrOfNodes; i++) {
nwsys->inputs[i] = 0.0;
nwsys->oldinputs[i] = 0.0;
nwsys->outputs[i] = 0.0;
nwsys->sendschedule[i] = i+1;
}
nwsys->time = 0.0;
nwsys->prevHit = 0.0;
nwsys->nwnodes = new NWnode*[nwsys->nbrOfNodes];
for (i=0; i<nwsys->nbrOfNodes; i++) {
int j;
nwsys->nwnodes[i] = new NWnode();
//nwsys->nwnodes[i]->transmitPower = transmitPowerWatt;
nwsys->nwnodes[i]->signallevels = new double[nwsys->nbrOfNodes]; //802.11
for (j=0; j<nwsys->nbrOfNodes; j++) {
nwsys->nwnodes[i]->signallevels[j] = 0;
}
}
nwsys->waituntil = 0.0;
nwsys->sending = -1; // Note! -1 means nobody is sending
nwsys->rrturn = nwsys->nbrOfNodes - 1; // want to start at 0
nwsys->lasttime = -1.0;
nwsys->slotcount = nwsys->schedsize - 1; // want to start at 0
nwsys->currslottime = -nwsys->slottime; // same here
// rad, kolum, reella tal
//nwsys->nbrOfTransmissions = mxCreateDoubleMatrix(nwsys->nbrOfNodes, nwsys->nbrOfNodes, mxREAL);
//mexMakeArrayPersistent(nwsys->nbrOfTransmissions);
}
static void mdlInitializeSizes(SimStruct *S)
{
debugPrintf("'%s': mdlInitializeSizes\n", S->path);
int i;
#ifdef KERNEL_MATLAB
char initfun[MAXCHARS];
static mxArray *lhs[1]; // warning: used multiple times
static mxArray *rhs[3]; // warning: used multiple times
mxArray *error_msg_array[1];
char *error_msg;
int nargin;
#endif
ssSetNumSFcnParams(S, 7); /* Number of expected parameters */
if (ssGetNumSFcnParams(S) != ssGetSFcnParamsCount(S)) {
TT_CALLBACK_ERROR(S, "Wrong number of parameters to S-function!");
return; /* Parameter mismatch will be reported by Simulink */
}
rtsys = new RTsys;
strncpy((char*)rtsys->blockName, ssGetBlockName(S), MAXCHARS);
#ifdef KERNEL_MATLAB
rtsys->segArray = mxCreateScalarDouble(0.0);
mexMakeArrayPersistent(rtsys->segArray);
#endif
/* Assign various function pointers */
rtsys->contextSwitchCode = contextSwitchCode;
rtsys->timeCmp = timeCmp;
rtsys->prioCmp = prioCmp;
rtsys->default_arrival = default_arrival;
rtsys->default_release = default_release;
rtsys->default_start = default_start;
rtsys->default_suspend = default_suspend;
rtsys->default_resume = default_resume;
rtsys->default_finish = default_finish;
rtsys->default_runkernel = default_runkernel;
rtsys->prioFP = prioFP;
rtsys->prioEDF = prioEDF;
rtsys->prioDM = prioDM;
/* Create basic kernel data structures */
rtsys->taskList = new List("TaskList", NULL);
rtsys->handlerList = new List("HandlerList", NULL);
rtsys->timerList = new List("TimerList", NULL);
rtsys->monitorList = new List("MonitorList", NULL);
rtsys->eventList = new List("EventList", NULL);
rtsys->mailboxList = new List("MailboxList", NULL);
rtsys->semaphoreList = new List("SemaphoreList", NULL);
rtsys->logList = new List("LogList", NULL);
rtsys->cbsList = new List("CBSList", NULL);
/* Read number of inputs, outputs, and triggers from block mask */
const mxArray *arg;
arg = ssGetSFcnParam(S, 2);
int m = mxGetM(arg);
int n = mxGetN(arg);
if (n != 2 || m != 1) {
TT_CALLBACK_ERROR(S, "Illegal number of analog inputs/outputs!");
return;
}
int ninputs = (int)mxGetPr(arg)[0];
int noutputs = (int)mxGetPr(arg)[1];
if (ninputs < 0 || noutputs < 0) {
TT_CALLBACK_ERROR(S, "Illegal number of analog inputs/outputs!");
return;
}
rtsys->nbrOfInputs = ninputs;
rtsys->nbrOfOutputs = noutputs;
if (ninputs > 0) {
rtsys->inputs = new double[ninputs];
}
if (noutputs > 0) {
rtsys->outputs = new double[noutputs];
}
arg = ssGetSFcnParam(S, 3);
if (!mxIsDoubleScalar(arg)) {
TT_CALLBACK_ERROR(S, "Illegal number of triggers!");
return;
}
int ntriggers = (int)*mxGetPr(arg);
if (ntriggers < 0) {
TT_CALLBACK_ERROR(S, "Illegal number of triggers!");
return;
}
rtsys->nbrOfTriggers = ntriggers;
rtsys->triggers = new Trigger[ntriggers];
arg = ssGetSFcnParam(S, 4);
if (!mxIsDoubleScalar(arg)) {
TT_CALLBACK_ERROR(S, "Illegal trigger type!");
return;
}
int trigType = (int)*mxGetPr(arg);
rtsys->trigType = trigType;
/* Create network interfaces according to the block mask */
arg = ssGetSFcnParam(S, 5);
m = mxGetM(arg); // number of rows = number of network interfaces
n = mxGetN(arg); // number of cols should be 1 or 2
int networkNbr;
int nodeNbr;
char nwname[MAXCHARS];
if ((n == 1 && m != 1) || n > 2) {
TT_CALLBACK_ERROR(S, "Illegal network or node numbers!");
return;
}
if (m > 0) {
rtsys->networkInterfaces = new NetworkInterface[m];
rtsys->nbrOfNetworks = m;
for (i=0; i<m; i++) {
if (n == 1) {
networkNbr = 1;
nodeNbr = (int)mxGetPr(arg)[i];
} else {
networkNbr = (int)mxGetPr(arg)[i];
nodeNbr = (int)mxGetPr(arg)[i+m];
}
NetworkInterface *nwi = &(rtsys->networkInterfaces[i]);
nwi->networkNbr = networkNbr;
nwi->nodeNbr = nodeNbr-1;
nwi->portNbr = i;
sprintf(nwname, "network_%d", networkNbr);
}
}
/* Read clock offset and drift parameters from the block mask */
arg = ssGetSFcnParam(S, 6);
if (!mxIsEmpty(arg)) {
if (mxGetM(arg) == 1 && mxGetN(arg) == 2) {
rtsys->clockOffset = mxGetPr(arg)[0];
rtsys->clockDrift = mxGetPr(arg)[1] + 1.0;
} else {
TT_CALLBACK_ERROR(S, "Illegal offset/drift parameters!");
return;
}
}
#ifdef KERNEL_MATLAB
mexSetTrapFlag(1); // return control to the MEX file after an error
/* Write rtsys pointer to global workspace */
if (mexGetVariablePtr("global", "_rtsys") == 0) {
// pointer variable does not exist - let's create one
debugPrintf("Creating global _rtsys variable\n");
mxArray* var = mxCreateScalarDouble(0.0);
mexMakeArrayPersistent(var);
mexPutVariable("global", "_rtsys", var);
}
rtsys->rtsysptr = (mxArray*)mexGetVariablePtr("global", "_rtsys");
*((long *)mxGetPr(rtsys->rtsysptr)) = (long)rtsys;
/* Evaluating user-defined init function (MATLAB) */
mxGetString(ssGetSFcnParam(S, 0), initfun, MAXCHARS);
rhs[0] = mxCreateString(initfun);
if (mexCallMATLAB(1, lhs, 1, rhs, "nargin") != 0) {
goto error;
}
nargin = (int)*mxGetPr(lhs[0]);
if (nargin == 0) {
if (mexCallMATLAB(0, NULL, 0, NULL, initfun) != 0) {
goto error;
} else {
rtsys->init_phase = false;
}
} else if (nargin == 1) {
rhs[0] = (mxArray *)ssGetSFcnParam(S, 1);
if (mexCallMATLAB(0, NULL, 1, rhs, initfun) != 0) {
goto error;
} else {
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);
}