/* Function: mdlOutputs =======================================================
*
*/
static void mdlOutputs(SimStruct *S, int_T tid)
{
uint8_T *r = (uint8_T *)ssGetOutputPortRealSignal(S,0);
uint8_T *g = (uint8_T *)ssGetOutputPortRealSignal(S,1);
uint8_T *b = (uint8_T *)ssGetOutputPortRealSignal(S,2);
uint16_T *depth = (uint16_T *)ssGetOutputPortRealSignal(S,3);
SS_SimMode mode= ssGetSimMode(S);
if(mode==0)
{
int i;
if (newFrame>0)
{
for(i=0;i<307200;i++)
{
//change
r[i]=kinR[i%480][i/480];
g[i]=kinG[i%480][i/480];
b[i]=kinB[i%480][i/480];
depth[i]=kinD[i%480][i/480];
newFrame=0;
}
for(i=1;i<307200;i++)
{
if(depth[i]>=1080)
depth[i]=1080;
if(depth[i]<410)
depth[i]=410;
}
}
}
}
/* Function: mdlUpdate ======================================================
* Abstract:
* This function is called once for every major integration time step.
* Discrete states are typically updated here, but this function is useful
* for performing any tasks that should only take place once per
* integration step.
*/
static void mdlUpdate(SimStruct *S, int_T tid)
{
UNUSED_ARG(tid); /* not used in single tasking mode */
if( ssGetSimMode(S) != SS_SIMMODE_NORMAL )
{
return;
}
}
/* Function: mdlTerminate =====================================================
* Abstract:
* In this function, you should perform any actions that are necessary
* at the termination of a simulation. For example, if memory was
* allocated in mdlStart, this is the place to free it.
*/
static void mdlTerminate(SimStruct *S)
{
SS_SimMode mode= ssGetSimMode(S);
if(mode==0)
{
printf("Term\n");
kinRunning= 0;
freenect_stop_depth(kinDev);
freenect_stop_video(kinDev);
freenect_close_device(kinDev);
freenect_shutdown(kinCtx);
}
}
/* 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)
{
SS_SimMode mode= ssGetSimMode(S);
if(mode==0)
{
kinFail=0;
kinRunning=1;
newFrame=0;
//change
memset (kinR,'1',307200);
memset (kinG,'1',307200);
memset (kinB,'1',307200);
memset (kinD,'1',307200);
/*-----INIT---------------*/
if (freenect_init(&kinCtx, 0)) {
printf("Error: Cannot get context\n");
kinFail=1;
return;
}
if (freenect_open_device(kinCtx, &kinDev, 0)) {
printf("Error: Cannot get device\n");
kinFail=1;
return;
}
freenect_set_depth_format(kinDev, FREENECT_DEPTH_11BIT);
freenect_start_depth(kinDev);
//change
freenect_set_video_format(kinDev, FREENECT_VIDEO_YUV_RGB);
freenect_start_video(kinDev);
freenect_set_depth_callback(kinDev, depth_cb);
freenect_set_video_callback(kinDev, rgb_cb);
/*-----END INIT------------*/
pthread_create( &kinThread, NULL, runLoop, NULL);
}
}
/* Function: mdlInitializeSizes ===============================================
* Abstract:
* Setup sizes of the various vectors.
*/
static void mdlInitializeSizes(SimStruct *S)
{
DECL_AND_INIT_DIMSINFO(inputDimsInfo);
DECL_AND_INIT_DIMSINFO(outputDimsInfo);
ssSetNumSFcnParams(S, NPARAMS);
if (ssGetNumSFcnParams(S) != ssGetSFcnParamsCount(S)) {
return; /* Parameter mismatch will be reported by Simulink */
}
ssSetNumContStates(S, NUM_CONT_STATES);
ssSetNumDiscStates(S, NUM_DISC_STATES);
if (!ssSetNumInputPorts(S, NUM_INPUTS)) return;
/*Input Port 0 */
/* Register xref_bus datatype for Input port 0 */
#if defined(MATLAB_MEX_FILE)
if (ssGetSimMode(S) != SS_SIMMODE_SIZES_CALL_ONLY)
{
DTypeId dataTypeIdReg;
ssRegisterTypeFromNamedObject(S, "xref_bus", &dataTypeIdReg);
if(dataTypeIdReg == INVALID_DTYPE_ID) return;
ssSetInputPortDataType(S,0, dataTypeIdReg);
}
#endif
ssSetInputPortWidth(S, 0, INPUT_0_WIDTH);
ssSetInputPortComplexSignal(S, 0, INPUT_0_COMPLEX);
ssSetInputPortDirectFeedThrough(S, 0, INPUT_0_FEEDTHROUGH);
ssSetInputPortRequiredContiguous(S, 0, 1); /*direct input signal access*/
ssSetBusInputAsStruct(S, 0,IN_0_BUS_BASED);
ssSetInputPortBusMode(S, 0, SL_BUS_MODE); /*Input Port 1 */
/* Register x_bus datatype for Input port 1 */
#if defined(MATLAB_MEX_FILE)
if (ssGetSimMode(S) != SS_SIMMODE_SIZES_CALL_ONLY)
{
DTypeId dataTypeIdReg;
ssRegisterTypeFromNamedObject(S, "x_bus", &dataTypeIdReg);
if(dataTypeIdReg == INVALID_DTYPE_ID) return;
ssSetInputPortDataType(S,1, dataTypeIdReg);
}
#endif
ssSetInputPortWidth(S, 1, INPUT_1_WIDTH);
ssSetInputPortComplexSignal(S, 1, INPUT_1_COMPLEX);
ssSetInputPortDirectFeedThrough(S, 1, INPUT_1_FEEDTHROUGH);
ssSetInputPortRequiredContiguous(S, 1, 1); /*direct input signal access*/
ssSetBusInputAsStruct(S, 1,IN_1_BUS_BASED);
ssSetInputPortBusMode(S, 1, SL_BUS_MODE);
if (!ssSetNumOutputPorts(S, NUM_OUTPUTS)) return;
/* Register u_bus datatype for Output port 0 */
#if defined(MATLAB_MEX_FILE)
if (ssGetSimMode(S) != SS_SIMMODE_SIZES_CALL_ONLY)
{
DTypeId dataTypeIdReg;
ssRegisterTypeFromNamedObject(S, "u_bus", &dataTypeIdReg);
if(dataTypeIdReg == INVALID_DTYPE_ID) return;
ssSetOutputPortDataType(S,0, dataTypeIdReg);
}
#endif
ssSetBusOutputObjectName(S, 0, (void *) "u_bus");
ssSetOutputPortWidth(S, 0, OUTPUT_0_WIDTH);
ssSetOutputPortComplexSignal(S, 0, OUTPUT_0_COMPLEX);
ssSetBusOutputAsStruct(S, 0,OUT_0_BUS_BASED);
ssSetOutputPortBusMode(S, 0, SL_BUS_MODE);
if (ssRTWGenIsCodeGen(S)) {
isSimulationTarget = GetRTWEnvironmentMode(S);
if (isSimulationTarget==-1) {
ssSetErrorStatus(S, " Unable to determine a valid code generation environment mode");
return;
}
isSimulationTarget |= ssRTWGenIsModelReferenceSimTarget(S);
}
/* Set the number of dworks */
if (!isDWorkPresent) {
if (!ssSetNumDWork(S, 0)) return;
} else {
if (!ssSetNumDWork(S, 3)) return;
}
if (isDWorkPresent) {
/*
* Configure the dwork 0 (u0."BUS")
*/
#if defined(MATLAB_MEX_FILE)
if (ssGetSimMode(S) != SS_SIMMODE_SIZES_CALL_ONLY) {
DTypeId dataTypeIdReg;
ssRegisterTypeFromNamedObject(S, "xref_bus", &dataTypeIdReg);
if (dataTypeIdReg == INVALID_DTYPE_ID) return;
ssSetDWorkDataType(S, 0, dataTypeIdReg);
}
#endif
ssSetDWorkUsageType(S, 0, SS_DWORK_USED_AS_DWORK);
ssSetDWorkName(S, 0, "u0BUS");
ssSetDWorkWidth(S, 0, DYNAMICALLY_SIZED);
ssSetDWorkComplexSignal(S, 0, COMPLEX_NO);
/*
* Configure the dwork 1 (u1."BUS")
*/
#if defined(MATLAB_MEX_FILE)
if (ssGetSimMode(S) != SS_SIMMODE_SIZES_CALL_ONLY) {
DTypeId dataTypeIdReg;
ssRegisterTypeFromNamedObject(S, "x_bus", &dataTypeIdReg);
if (dataTypeIdReg == INVALID_DTYPE_ID) return;
ssSetDWorkDataType(S, 1, dataTypeIdReg);
}
#endif
ssSetDWorkUsageType(S, 1, SS_DWORK_USED_AS_DWORK);
ssSetDWorkName(S, 1, "u1BUS");
ssSetDWorkWidth(S, 1, DYNAMICALLY_SIZED);
ssSetDWorkComplexSignal(S, 1, COMPLEX_NO);
/*
* Configure the dwork 2 (y0BUS)
*/
#if defined(MATLAB_MEX_FILE)
if (ssGetSimMode(S) != SS_SIMMODE_SIZES_CALL_ONLY) {
DTypeId dataTypeIdReg;
ssRegisterTypeFromNamedObject(S, "u_bus", &dataTypeIdReg);
if (dataTypeIdReg == INVALID_DTYPE_ID) return;
ssSetDWorkDataType(S, 2, dataTypeIdReg);
}
#endif
ssSetDWorkUsageType(S, 2, SS_DWORK_USED_AS_DWORK);
ssSetDWorkName(S, 2, "y0BUS");
ssSetDWorkWidth(S, 2, DYNAMICALLY_SIZED);
ssSetDWorkComplexSignal(S, 2, COMPLEX_NO);
}
ssSetNumSampleTimes(S, 1);
ssSetNumRWork(S, 0);
ssSetNumIWork(S, 0);
ssSetNumPWork(S, 0);
ssSetNumModes(S, 0);
ssSetNumNonsampledZCs(S, 0);
/* Take care when specifying exception free code - see sfuntmpl_doc.c */
ssSetOptions(S, (SS_OPTION_EXCEPTION_FREE_CODE |
SS_OPTION_USE_TLC_WITH_ACCELERATOR |
SS_OPTION_WORKS_WITH_CODE_REUSE));
}
/* 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)
{
DECL_AND_INIT_DIMSINFO(outputDimsInfo);
ssSetNumSFcnParams(S, NPARAMS);
if (ssGetNumSFcnParams(S) != ssGetSFcnParamsCount(S)) {
return; /* Parameter mismatch will be reported by Simulink */
}
ssSetNumContStates(S, NUM_CONT_STATES);
ssSetNumDiscStates(S, NUM_DISC_STATES);
if (!ssSetNumInputPorts(S, NUM_INPUTS)) return;
if (!ssSetNumOutputPorts(S, NUM_OUTPUTS)) return;
/* Output Port 0 */
ssSetOutputPortWidth(S, 0, OUTPUT_0_WIDTH);
ssSetOutputPortDataType(S, 0, SS_DOUBLE);
ssSetOutputPortComplexSignal(S, 0, OUTPUT_0_COMPLEX);
/* Output Port 1 */
/* Register SL_ROS_SUB_MSG datatype for Output port 1 */
#if defined(MATLAB_MEX_FILE)
if (ssGetSimMode(S) != SS_SIMMODE_SIZES_CALL_ONLY)
{
DTypeId dataTypeIdReg;
ssRegisterTypeFromNamedObject(S, "SL_ROS_SUB_MSG", &dataTypeIdReg);
if(dataTypeIdReg == INVALID_DTYPE_ID) return;
ssSetOutputPortDataType(S,1, dataTypeIdReg);
}
#endif
ssSetBusOutputObjectName(S, 1, (void *) "SL_ROS_SUB_MSG");
ssSetOutputPortWidth(S, 1, OUTPUT_1_WIDTH);
ssSetOutputPortComplexSignal(S, 1, OUTPUT_1_COMPLEX);
ssSetBusOutputAsStruct(S, 1, OUT_1_BUS_BASED);
ssSetOutputPortBusMode(S, 1, SL_BUS_MODE);
if (ssRTWGenIsCodeGen(S)) {
isSimulationTarget = GetRTWEnvironmentMode(S);
if (isSimulationTarget == -1) {
ssSetErrorStatus(S, " Unable to determine a valid code generation environment mode");
return;
}
isSimulationTarget |= ssRTWGenIsModelReferenceSimTarget(S);
}
/* Set the number of dworks */
if (!isDWorkPresent) {
if (!ssSetNumDWork(S, 0)) return;
}
else {
if (!ssSetNumDWork(S, 1)) return;
}
if (isDWorkPresent) {
/*
* Configure the dwork 0 (y1BUS)
*/
#if defined(MATLAB_MEX_FILE)
if (ssGetSimMode(S) != SS_SIMMODE_SIZES_CALL_ONLY) {
DTypeId dataTypeIdReg;
ssRegisterTypeFromNamedObject(S, "SL_ROS_SUB_MSG", &dataTypeIdReg);
if (dataTypeIdReg == INVALID_DTYPE_ID) return;
ssSetDWorkDataType(S, 0, dataTypeIdReg);
}
#endif
ssSetDWorkUsageType(S, 0, SS_DWORK_USED_AS_DWORK);
ssSetDWorkName(S, 0, "y1BUS");
ssSetDWorkWidth(S, 0, DYNAMICALLY_SIZED);
ssSetDWorkComplexSignal(S, 0, COMPLEX_NO);
}
ssSetNumSampleTimes(S, 1);
ssSetNumRWork(S, 0);
ssSetNumIWork(S, 0);
ssSetNumPWork(S, 0);
ssSetNumModes(S, 0);
ssSetNumNonsampledZCs(S, 0);
ssSetSimulinkVersionGeneratedIn(S, "8.7");
/* Take care when specifying exception free code - see sfuntmpl_doc.c */
ssSetOptions(S, 0);
}
static void mdlInitializeSizes(SimStruct *S)
{
ssSetNumSFcnParams(S, 0);
if (S->mdlInfo->genericFcn != NULL) {
_GenericFcn fcn = S->mdlInfo->genericFcn;
(fcn)(S, GEN_FCN_CHK_MODELREF_SOLVER_TYPE_EARLY, 2, NULL);
}
ssSetRTWGeneratedSFcn(S, 2);
ssSetNumContStates(S, 0);
ssSetNumDiscStates(S, 0);
if (!ssSetNumInputPorts(S, 2))
return;
if (!ssSetInputPortVectorDimension(S, 0, 1))
return;
ssSetInputPortFrameData(S, 0, FRAME_NO);
ssSetInputPortBusMode(S, 0, SL_NON_BUS_MODE)
if (ssGetSimMode(S) != SS_SIMMODE_SIZES_CALL_ONLY)
{
ssSetInputPortDataType(S, 0, SS_DOUBLE);
}
ssSetInputPortDirectFeedThrough(S, 0, 1);
ssSetInputPortRequiredContiguous(S, 0, 1);
ssSetInputPortOptimOpts(S, 0, SS_NOT_REUSABLE_AND_GLOBAL);
ssSetInputPortOverWritable(S, 0, FALSE);
ssSetInputPortSampleTime(S, 0, 0.0);
ssSetInputPortOffsetTime(S, 0, 0.0);
if (!ssSetInputPortVectorDimension(S, 1, 1))
return;
ssSetInputPortFrameData(S, 1, FRAME_NO);
ssSetInputPortBusMode(S, 1, SL_NON_BUS_MODE)
if (ssGetSimMode(S) != SS_SIMMODE_SIZES_CALL_ONLY)
{
ssSetInputPortDataType(S, 1, SS_DOUBLE);
}
ssSetInputPortDirectFeedThrough(S, 1, 1);
ssSetInputPortRequiredContiguous(S, 1, 1);
ssSetInputPortOptimOpts(S, 1, SS_NOT_REUSABLE_AND_GLOBAL);
ssSetInputPortOverWritable(S, 1, FALSE);
ssSetInputPortSampleTime(S, 1, 0.0);
ssSetInputPortOffsetTime(S, 1, 0.0);
if (!ssSetNumOutputPorts(S, 1))
return;
if (!ssSetOutputPortVectorDimension(S, 0, 1))
return;
ssSetOutputPortFrameData(S, 0, FRAME_NO);
ssSetOutputPortBusMode(S, 0, SL_NON_BUS_MODE)
if (ssGetSimMode(S) != SS_SIMMODE_SIZES_CALL_ONLY)
{
ssSetOutputPortDataType(S, 0, SS_DOUBLE);
}
ssSetOutputPortSampleTime(S, 0, 0.0);
ssSetOutputPortOffsetTime(S, 0, 0.0);
ssSetOutputPortOkToMerge(S, 0, SS_OK_TO_MERGE);
ssSetOutputPortOptimOpts(S, 0, SS_NOT_REUSABLE_AND_GLOBAL);
rt_InitInfAndNaN(sizeof(real_T));
{
real_T minValue = rtMinusInf;
real_T maxValue = rtInf;
ssSetModelRefInputSignalDesignMin(S,0,&minValue);
ssSetModelRefInputSignalDesignMax(S,0,&maxValue);
}
{
real_T minValue = rtMinusInf;
real_T maxValue = rtInf;
ssSetModelRefInputSignalDesignMin(S,1,&minValue);
ssSetModelRefInputSignalDesignMax(S,1,&maxValue);
}
{
real_T minValue = rtMinusInf;
real_T maxValue = rtInf;
ssSetModelRefOutputSignalDesignMin(S,0,&minValue);
ssSetModelRefOutputSignalDesignMax(S,0,&maxValue);
}
{
static ssRTWStorageType storageClass[3] = { SS_RTW_STORAGE_AUTO,
SS_RTW_STORAGE_AUTO, SS_RTW_STORAGE_AUTO };
ssSetModelRefPortRTWStorageClasses(S, storageClass);
}
ssSetNumSampleTimes(S, PORT_BASED_SAMPLE_TIMES);
ssSetNumRWork(S, 0);
ssSetNumIWork(S, 0);
ssSetNumPWork(S, 0);
ssSetNumModes(S, 0);
ssSetNumZeroCrossingSignals(S, 0);
ssSetOutputPortIsNonContinuous(S, 0, 0);
ssSetOutputPortIsFedByBlockWithModesNoZCs(S, 0, 0);
ssSetInputPortIsNotDerivPort(S, 0, 1);
ssSetInputPortIsNotDerivPort(S, 1, 1);
ssSetModelReferenceSampleTimeInheritanceRule(S,
DISALLOW_SAMPLE_TIME_INHERITANCE);
ssSetOptimizeModelRefInitCode(S, 0);
ssSetModelReferenceNormalModeSupport(S, MDL_START_AND_MDL_PROCESS_PARAMS_OK);
ssSetOptions(S, SS_OPTION_EXCEPTION_FREE_CODE |
SS_OPTION_DISALLOW_CONSTANT_SAMPLE_TIME |
SS_OPTION_SUPPORTS_ALIAS_DATA_TYPES |
SS_OPTION_WORKS_WITH_CODE_REUSE |
SS_OPTION_CALL_TERMINATE_ON_EXIT);
if (S->mdlInfo->genericFcn != NULL) {
ssRegModelRefChildModel(S,1,childModels);
}
#if SS_SFCN_FOR_SIM
if (S->mdlInfo->genericFcn != NULL &&
ssGetSimMode(S) != SS_SIMMODE_SIZES_CALL_ONLY) {
mr_vdmultRM_MdlInfoRegFcn(S, "vdmultRM");
}
#endif
if (!ssSetNumDWork(S, 1)) {
return;
}
#if SS_SFCN_FOR_SIM
{
int mdlrefDWTypeId;
ssRegMdlRefDWorkType(S, &mdlrefDWTypeId);
if (mdlrefDWTypeId == INVALID_DTYPE_ID )
return;
if (!ssSetDataTypeSize(S, mdlrefDWTypeId, sizeof(rtMdlrefDWork_mr_vdmultRM)))
return;
ssSetDWorkDataType(S, 0, mdlrefDWTypeId);
ssSetDWorkWidth(S, 0, 1);
}
#endif
ssSetNeedAbsoluteTime(S, 1);
}
/* 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)
{
/* Number of expected parameters */
ssSetNumSFcnParams(S, 0);
/*
* Set the number of pworks.
*/
ssSetNumPWork(S, 0);
/*
* Set the number of dworks.
*/
if (!ssSetNumDWork(S, 0))
return;
/*
* Set the number of input ports.
*/
if (!ssSetNumInputPorts(S, 0))
return;
/*
* Set the number of output ports.
*/
if (!ssSetNumOutputPorts(S, 1))
return;
/*
* Configure the output port 1
*/
ssSetOutputPortDataType(S, 0, SS_SINGLE);
ssSetOutputPortWidth(S, 0, 1);
ssSetOutputPortComplexSignal(S, 0, COMPLEX_NO);
ssSetOutputPortOptimOpts(S, 0, SS_REUSABLE_AND_LOCAL);
ssSetOutputPortOutputExprInRTW(S, 0, 1);
/*
* Register reserved identifiers to avoid name conflict
*/
if (ssRTWGenIsCodeGen(S) || ssGetSimMode(S)==SS_SIMMODE_EXTERNAL) {
/*
* Register reserved identifier for StartFcnSpec
*/
ssRegMdlInfo(S, "Magneto_Initialization", MDL_INFO_ID_RESERVED, 0, 0,
ssGetPath(S));
/*
* Register reserved identifier for OutputFcnSpec
*/
ssRegMdlInfo(S, "Magneto_Get_X", MDL_INFO_ID_RESERVED, 0, 0, ssGetPath(S));
/*
* Register reserved identifier for wrappers
*/
if (ssRTWGenIsModelReferenceSimTarget(S)) {
/*
* Register reserved identifier for StartFcnSpec for SimulationTarget
*/
ssRegMdlInfo(S, "ARDrone_Magneto_X_wrapper_start", MDL_INFO_ID_RESERVED, 0,
0, ssGetPath(S));
/*
* Register reserved identifier for OutputFcnSpec for SimulationTarget
*/
ssRegMdlInfo(S, "ARDrone_Magneto_X_wrapper_output", MDL_INFO_ID_RESERVED,
0, 0, ssGetPath(S));
}
}
/*
* This S-function can be used in referenced model simulating in normal mode.
*/
ssSetModelReferenceNormalModeSupport(S, MDL_START_AND_MDL_PROCESS_PARAMS_OK);
/*
* Set the number of sample time.
*/
ssSetNumSampleTimes(S, 1);
/*
* All options have the form SS_OPTION_<name> and are documented in
* matlabroot/simulink/include/simstruc.h. The options should be
* bitwise or'd together as in
* ssSetOptions(S, (SS_OPTION_name1 | SS_OPTION_name2))
*/
ssSetOptions(S,
SS_OPTION_USE_TLC_WITH_ACCELERATOR |
SS_OPTION_CAN_BE_CALLED_CONDITIONALLY |
SS_OPTION_EXCEPTION_FREE_CODE |
SS_OPTION_WORKS_WITH_CODE_REUSE |
SS_OPTION_SFUNCTION_INLINED_FOR_RTW |
SS_OPTION_DISALLOW_CONSTANT_SAMPLE_TIME);
}
static void mdlTerminate(SimStruct *S)
{
tCamera* Camera;
unsigned int i;
if( ssGetSimMode(S) != SS_SIMMODE_NORMAL )
{
return;
}
/* Retrieve and destroy C object */
Camera = (tCamera*)ssGetPWork(S)[0];
num_instances--;
if( Camera )
{
if( Camera->Handle )
# ifdef LOAD_PVLIB_AT_RUNTIME
if( PvLib )
# endif
{
ss_PvCommandRun(Camera->Handle,"AcquisitionStop");
ss_PvCaptureEnd(Camera->Handle);
ss_PvCaptureQueueClear(Camera->Handle);
ss_PvCameraClose(Camera->Handle);
}
if( num_instances == 0 )
{
# if defined(_WIN32) || defined(_WIN64) || defined(__LCC__)
if( Camera->csec_init ) DeleteCriticalSection( &(Camera->csec) );
if( Camera->event ) CloseHandle( Camera->event );
# else
if( Camera->mutex_init ) pthread_mutex_destroy( &(Camera->mutex) );
if( Camera->cond_init ) pthread_cond_destroy( &(Camera->cond) );
# endif
}
if( Camera->Frames )
{
for( i=0; i<Camera->frames_count; i++ )
{
if( Camera->Frames[i].ImageBuffer ) free(Camera->Frames[i].ImageBuffer);
}
free(Camera->Frames);
}
if( Camera->frames_queue ) free(Camera->frames_queue);
free(Camera);
ssGetPWork(S)[0] = NULL;
}
if( num_instances == 0 )
{
# ifdef LOAD_PVLIB_AT_RUNTIME
if( PvLib )
{
ss_PvUnInitialize();
close_library(PvLib);
PvLib = NULL;
}
# else
ss_PvUnInitialize();
# endif
# if defined(_WIN32) || defined(_WIN64) || defined(__LCC__)
WSACleanup();
# endif
}
}
static void mdlOutputs(SimStruct *S, int_T tid)
{
/* Retrieve C object from the pointers vector */
tCamera* Camera = (tCamera*) ssGetPWork(S)[0];
uint8_T *py0;
int_T y_len;
int frames_idx_in;
int frames_idx_out;
int frames_idx_out2;
int frames_avail;
int frames_avail2;
int frames_ignore;
int frames_reject;
tPvFrame* Frame;
unsigned long idx_x;
unsigned long idx_y;
unsigned long color_plane_size;
unsigned char* buf;
# if defined(_WIN32) || defined(_WIN64) || defined(__LCC__)
DWORD rc;
# else
struct timespec timeout;
int rc;
# endif
if( ssGetSimMode(S) != SS_SIMMODE_NORMAL )
{
return;
}
py0 = (uint8_T *)ssGetOutputPortSignal(S,0);
y_len = ssGetOutputPortWidth(S, 0);
/* Suspend thread and wait for N frames */
# if defined(_WIN32) || defined(_WIN64) || defined(__LCC__)
if( Camera->timeout_msec )
{
rc = WaitForSingleObject( Camera->event, Camera->timeout_msec );
if( rc != WAIT_OBJECT_0 )
{
ssSetErrorStatus(S,"Timeout occured.");
return;
}
}
EnterCriticalSection( &(Camera->csec) );
frames_idx_in = Camera->frames_idx_in;
frames_idx_out = Camera->frames_idx_out;
LeaveCriticalSection( &(Camera->csec) );
# else
if( Camera->timeout_sec || Camera->timeout_nsec )
{
clock_gettime(CLOCK_REALTIME, &timeout);
timeout.tv_sec += Camera->timeout_sec;
timeout.tv_nsec += Camera->timeout_nsec;
if( timeout.tv_nsec >= 1000000000l )
{
timeout.tv_nsec -= 1000000000l;
timeout.tv_sec++;
}
rc = 0;
pthread_mutex_lock(&(Camera->mutex));
while (!Camera->Avail && rc==0)
{
rc = pthread_cond_timedwait(&(Camera->cond), &(Camera->mutex), &timeout);
}
if( rc == ETIMEDOUT )
{
ssSetErrorStatus(S,"Timeout occured.");
return;
}
Camera->Avail = false;
frames_idx_in = Camera->frames_idx_in;
frames_idx_out = Camera->frames_idx_out;
pthread_mutex_unlock(&(Camera->mutex));
}
else
{
pthread_mutex_lock(&(Camera->mutex));
frames_idx_in = Camera->frames_idx_in;
frames_idx_out = Camera->frames_idx_out;
pthread_mutex_unlock(&(Camera->mutex));
}
# endif
frames_avail = frames_idx_in - frames_idx_out;
if( frames_avail < 0 )
{
frames_avail += Camera->frames_count + 1;
}
frames_ignore = ( frames_avail > (int)Camera->dim_N ? frames_avail-Camera->dim_N : 0 );
frames_reject = 0;
frames_avail2 = frames_avail;
frames_idx_out2 = Camera->frames_idx_out;
while( frames_avail2 )
{
Frame = Camera->frames_queue[frames_idx_out2];
if( Frame->Status != ePvErrSuccess )
{
frames_reject++;
}
frames_avail2--;
}
frames_avail2 = frames_avail - frames_reject - frames_ignore;
*((uint32_T *)ssGetOutputPortSignal(S,1)) = frames_avail2;
/* Software trigger */
if( intval(mxGetPr(paramSoftwareTrigger)[0]) )
{
ss_PvCommandRun(Camera->Handle, "FrameStartTriggerSoftware");
}
frames_avail2 = frames_avail;
while( frames_avail2 )
{
Frame = Camera->frames_queue[Camera->frames_idx_out];
if( Frame->Status == ePvErrSuccess )
{
if( frames_ignore )
{
frames_ignore--;
}
else
{
if( Camera->dim_pixel == 1)
{
for( idx_x = 0; idx_x < Camera->dim_x; idx_x++ )
{
buf = (unsigned char*)Frame->ImageBuffer + idx_x;
for( idx_y = 0; idx_y < Camera->dim_y; idx_y++ )
{
*py0++ = *buf;
buf += Camera->dim_x;
}
}
}
else if( Camera->dim_pixel == 3)
{
color_plane_size = Camera->dim_x * Camera->dim_y;
for( idx_x = 0; idx_x < Camera->dim_x; idx_x++ )
{
buf = (unsigned char*)Frame->ImageBuffer + idx_x*3;
for( idx_y = 0; idx_y < Camera->dim_y; idx_y++ )
{
*py0 = *buf;
*(py0+color_plane_size) = *(buf+1);
*(py0++ +2*color_plane_size) = *(buf+2);
buf += Camera->dim_x*3;
}
}
}
}
}
# if defined(_WIN32) || defined(_WIN64) || defined(__LCC__)
EnterCriticalSection( &(Camera->csec) );
# else
pthread_mutex_lock( &(Camera->mutex) );
# endif
if( ++Camera->frames_idx_out >= Camera->frames_count+1 )
{
Camera->frames_idx_out = 0;
}
# if defined(_WIN32) || defined(_WIN64) || defined(__LCC__)
LeaveCriticalSection( &(Camera->csec) );
# else
pthread_mutex_unlock( &(Camera->mutex) );
# endif
ss_PvCaptureQueueFrame(Camera->Handle, Frame, callback_fn);
frames_avail2--;
}
}
static void mdlStart(SimStruct *S)
{
tCamera* Camera;
char host_name[101];
int host_len;
unsigned long host;
struct hostent *hent;
unsigned int i;
unsigned long FrameSize;
const int pixel_size[9] = {1, 2, 1, 2, 3, 6, 0, 0, 0};
const char* const pixel_format[9] = {"Mono8", "Mono16", "Bayer8", "Bayer16", "Rgb24", "Rgb48", "Yuv411", "Yuv422", "Yuv444"};
# if defined(_WIN32) || defined(_WIN64) || defined(__LCC__)
WSADATA wsa_data;
# endif
if( ssGetSimMode(S) != SS_SIMMODE_NORMAL )
{
return;
}
num_instances++;
if( num_instances == 1 )
{
# if defined(_WIN32) || defined(_WIN64) || defined(__LCC__)
/* Activate the Winsock DLL */
if (WSAStartup(MAKEWORD(2,2),&wsa_data) != 0)
{
ssSetErrorStatus(S, "Can not initialize the Winsock DLL.");
return;
}
# endif
# ifdef LOAD_PVLIB_AT_RUNTIME
/* Load the shared library */
if( !(PvLib = load_library(msg)) )
{
ssSetErrorStatus(S, msg);
return;
}
# endif
/* Initialize the camera */
if( ss_PvInitializeNoDiscovery() != ePvErrSuccess )
{
ssSetErrorStatus(S,"Could not initialize the PvAPI library. \r\nCheck that the shared library is in the same folder with the model.");
return;
}
}
/* Try to find the IP address for the hostname */
host_len = mxGetM(paramHostName) * mxGetN(paramHostName) + 1;
if( host_len > sizeof(host_name) ) host_len = sizeof(host_name);
mxGetString(paramHostName, host_name, host_len);
hent = gethostbyname(host_name);
if (hent != NULL)
{
memcpy((char *)&host, (char *)hent->h_addr_list[0], hent->h_length);
}
else
{
/* If hostname does not exist try as IP address */
host = inet_addr(host_name);
if (host==INADDR_NONE)
{
ssSetErrorStatus(S,"Invalid camera IP or the host name does not exist.");
return;
}
}
/* Store new C object in the pointers vector */
Camera = (tCamera*) calloc(1,sizeof(tCamera));
ssGetPWork(S)[0] = Camera;
/* Initialize the thread synchronization objects */
# if defined(_WIN32) || defined(_WIN64) || defined(__LCC__)
InitializeCriticalSection(&(Camera->csec));
Camera->csec_init = 1;
if( (Camera->event = CreateEvent( NULL, FALSE, FALSE, NULL )) == NULL)
{
ssSetErrorStatus(S, "Can not initialize the event variable.");
return;
}
# else
if( pthread_mutex_init(&(Camera->mutex),NULL) )
{
ssSetErrorStatus(S, "Can not initialize the mutex.");
return;
}
Camera->mutex_init = 1;
if( pthread_cond_init( &(Camera->cond),NULL) )
{
ssSetErrorStatus(S, "Can not initialize the condition variable.");
return;
}
Camera->cond_init = 1;
# endif
/* Populate the Camera structure fields */
Camera->dim_x = intval(mxGetPr(paramImageSizeXYN)[0]);
Camera->dim_y = intval(mxGetPr(paramImageSizeXYN)[1]);
if ((mxGetNumberOfElements(paramImageSizeXYN) == 3) && (intval(mxGetPr(paramImageSizeXYN)[2])) > 1)
{
Camera->dim_N = intval(mxGetPr(paramImageSizeXYN)[2]);
}
else
{
Camera->dim_N = 1;
}
Camera->dim_pixel = pixel_size[intval(mxGetPr(paramPixelFormat)[0])-1];
Camera->frames_count = Camera->dim_N * 2 + 1;
if( mxGetPr(paramTimeout)[0] > 0 )
{
# if defined(_WIN32) || defined(_WIN64) || defined(__LCC__)
Camera->timeout_msec = (DWORD) (mxGetPr(paramTimeout)[0] * 1000);
# else
Camera->timeout_sec = floor(mxGetPr(paramTimeout)[0]);
Camera->timeout_nsec = (mxGetPr(paramTimeout)[0] - floor(mxGetPr(paramTimeout)[0])) * 1000000000.0;
# endif
}
/* Open the camera and configure it */
if( (ss_PvCameraOpenByAddr(host, ePvAccessMaster, &(Camera->Handle)) != ePvErrSuccess) || \
(Camera->Handle == NULL) )
{
Camera->Handle = NULL;
ssSetErrorStatus(S,"Invalid camera IP or host address.");
return;
}
if( (ss_PvAttrUint32Set(Camera->Handle, "Width", Camera->dim_x) != ePvErrSuccess) || \
(ss_PvAttrUint32Set(Camera->Handle, "Height", Camera->dim_y) != ePvErrSuccess) || \
(ss_PvAttrEnumSet(Camera->Handle, "PixelFormat", pixel_format[intval(mxGetPr(paramPixelFormat)[0])-1]) != ePvErrSuccess) || \
(ss_PvAttrEnumSet(Camera->Handle, "AcquisitionMode", "Continuous") != ePvErrSuccess) )
{
ssSetErrorStatus(S,"Can not set the camera attributes.");
return;
}
if( intval(mxGetPr(paramSoftwareTrigger)[0]) )
if( ss_PvAttrEnumSet(Camera->Handle, "FrameStartTriggerMode", "Software") != ePvErrSuccess )
{
ssSetErrorStatus(S,"Can not set the camera attributes.");
return;
}
FrameSize = 0;
ss_PvAttrUint32Get(Camera->Handle, "TotalBytesPerFrame", &FrameSize);
if( FrameSize != Camera->dim_x * Camera->dim_y * Camera->dim_pixel )
{
ssSetErrorStatus(S,"Wrong frame size was read from the camera.");
return;
}
/* Allocate memory for the image buffers */
Camera->Frames = (tPvFrame*) calloc(Camera->frames_count, sizeof(tPvFrame));
Camera->frames_queue = (tPvFrame**) calloc(Camera->frames_count+1, sizeof(tPvFrame*));
for( i=0; i<Camera->frames_count; i++ )
{
Camera->Frames[i].ImageBuffer = calloc(1, FrameSize);
Camera->Frames[i].ImageBufferSize = FrameSize;
Camera->Frames[i].Context[0] = Camera;
}
/* Start the capture */
if (ss_PvCaptureStart(Camera->Handle) != ePvErrSuccess)
{
ssSetErrorStatus(S,"Can not start the camera capture.");
return;
}
if( ss_PvCommandRun(Camera->Handle, "AcquisitionStart") != ePvErrSuccess )
{
ssSetErrorStatus(S,"Can not start the camera acquisition.");
return;
}
/* Queue the image buffers */
# if defined(_WIN32) || defined(_WIN64) || defined(__LCC__)
EnterCriticalSection( &(Camera->csec) );
# else
pthread_mutex_lock( &(Camera->mutex) );
# endif
for( i=0; i<Camera->frames_count-1; i++ )
{
ss_PvCaptureQueueFrame(Camera->Handle, &(Camera->Frames[i]), callback_fn);
}
# if defined(_WIN32) || defined(_WIN64) || defined(__LCC__)
LeaveCriticalSection( &(Camera->csec) );
# else
pthread_mutex_unlock( &(Camera->mutex) );
# endif
/* Software trigger */
if( intval(mxGetPr(paramSoftwareTrigger)[0]) )
{
ss_PvCommandRun(Camera->Handle, "FrameStartTriggerSoftware");
}
}
return ; } static void mdlInitializeSizes ( SimStruct * S ) {
ssSetNumSFcnParams ( S , 0 ) ; ssFxpSetU32BitRegionCompliant ( S , 1 ) ;
rt_InitInfAndNaN ( sizeof ( real_T ) ) ; if ( S -> mdlInfo -> genericFcn != (
NULL ) ) { _GenericFcn fcn = S -> mdlInfo -> genericFcn ; real_T lifeSpan =
rtInf ; real_T startTime = 0.0 ; real_T stopTime = rtInf ; int_T hwSettings [
15 ] ; int_T opSettings [ 1 ] ; boolean_T concurrTaskSupport = 0 ; boolean_T
hasDiscTs = 1 ; real_T fixedStep = 0.001 ; ( fcn ) ( S ,
GEN_FCN_CHK_MODELREF_SOLVER_TYPE_EARLY , 2 , ( NULL ) ) ; ( fcn ) ( S ,
GEN_FCN_MODELREF_RATE_GROUPED , 0 , ( NULL ) ) ; if ( ! ( fcn ) ( S ,
GEN_FCN_CHK_MODELREF_LIFE_SPAN , - 1 , & lifeSpan ) ) return ; if ( ! ( fcn )
( S , GEN_FCN_CHK_MODELREF_START_TIME , - 1 , & startTime ) ) return ; if ( !
( fcn ) ( S , GEN_FCN_CHK_MODELREF_STOP_TIME , - 1 , & stopTime ) ) return ;
hwSettings [ 0 ] = 16 ; hwSettings [ 1 ] = 16 ; hwSettings [ 2 ] = 16 ;
hwSettings [ 3 ] = 32 ; hwSettings [ 4 ] = 32 ; hwSettings [ 5 ] = 64 ;
hwSettings [ 6 ] = 16 ; hwSettings [ 7 ] = 0 ; hwSettings [ 8 ] = 1 ;
hwSettings [ 9 ] = 16 ; hwSettings [ 10 ] = 1 ; hwSettings [ 11 ] = 2 ;
hwSettings [ 12 ] = 2 ; hwSettings [ 13 ] = 64 ; hwSettings [ 14 ] = 0 ; if (
! ( fcn ) ( S , GEN_FCN_CHK_MODELREF_HARDWARE_SETTINGS , 15 , hwSettings ) )
return ; opSettings [ 0 ] = 0 ; if ( ! ( fcn ) ( S ,
GEN_FCN_CHK_MODELREF_OPTIM_SETTINGS , 1 , opSettings ) ) return ; if ( ! (
fcn ) ( S , GEN_FCN_CHK_MODELREF_CONCURRETNT_TASK_SUPPORT , ( int_T )
concurrTaskSupport , ( NULL ) ) ) return ; if ( ! ( fcn ) ( S ,
GEN_FCN_CHK_MODELREF_SOLVER_TYPE , 0 , & hasDiscTs ) ) return ; if ( ! ( fcn
) ( S , GEN_FCN_CHK_MODELREF_SOLVER_NAME , 0 , ( void * ) "FixedStepDiscrete"
) ) return ; if ( ! ( fcn ) ( S , GEN_FCN_CHK_MODELREF_SOLVER_MODE ,
SOLVER_MODE_SINGLETASKING , ( NULL ) ) ) return ; if ( ! ( fcn ) ( S ,
GEN_FCN_CHK_MODELREF_FIXED_STEP , 0 , & fixedStep ) ) return ; ( fcn ) ( S ,
GEN_FCN_CHK_MODELREF_FRAME_UPGRADE_DIAGNOSTICS , 1 , ( NULL ) ) ; } { static
const char * globalVarList [ ] = { "Cntrl_Status" , "DT_PRECISION_HI" ,
"DT_PRECISION_LO" , "EV_Param" , "Motor_Cmds" , "Power_Lims" ,
"brake_cmd_table" , "ctrlConst" , "decel_cmd_vec" , "regen_pwr_vec" } ;
ssRegModelRefGlobalVarUsage ( S , 10 , ( void * ) globalVarList ) ; }
ssSetRTWGeneratedSFcn ( S , 2 ) ; ssSetNumContStates ( S , 0 ) ;
ssSetNumDiscStates ( S , 0 ) ; if ( ! ssSetNumInputPorts ( S , 8 ) ) return ;
if ( ! ssSetInputPortVectorDimension ( S , 0 , 1 ) ) return ;
ssSetInputPortDimensionsMode ( S , 0 , FIXED_DIMS_MODE ) ;
ssSetInputPortFrameData ( S , 0 , FRAME_NO ) ; if ( ssGetSimMode ( S ) !=
SS_SIMMODE_SIZES_CALL_ONLY ) { ssSetInputPortDataType ( S , 0 , SS_SINGLE ) ;
} ssSetInputPortDirectFeedThrough ( S , 0 , 1 ) ;
ssSetInputPortRequiredContiguous ( S , 0 , 1 ) ; ssSetInputPortOptimOpts ( S
, 0 , SS_NOT_REUSABLE_AND_LOCAL ) ; ssSetInputPortOverWritable ( S , 0 ,
FALSE ) ; ssSetInputPortSampleTime ( S , 0 , 0.001 ) ;
ssSetInputPortOffsetTime ( S , 0 , 0.0 ) ; if ( !
ssSetInputPortVectorDimension ( S , 1 , 1 ) ) return ;
ssSetInputPortDimensionsMode ( S , 1 , FIXED_DIMS_MODE ) ;
ssSetInputPortFrameData ( S , 1 , FRAME_NO ) ; if ( ssGetSimMode ( S ) !=
SS_SIMMODE_SIZES_CALL_ONLY ) { ssSetInputPortDataType ( S , 1 , SS_SINGLE ) ;
} ssSetInputPortDirectFeedThrough ( S , 1 , 1 ) ;
ssSetInputPortRequiredContiguous ( S , 1 , 1 ) ; ssSetInputPortOptimOpts ( S
, 1 , SS_NOT_REUSABLE_AND_LOCAL ) ; ssSetInputPortOverWritable ( S , 1 ,
FALSE ) ; ssSetInputPortSampleTime ( S , 1 , 0.001 ) ;
ssSetInputPortOffsetTime ( S , 1 , 0.0 ) ; if ( !
ssSetInputPortVectorDimension ( S , 2 , 1 ) ) return ;
ssSetInputPortDimensionsMode ( S , 2 , FIXED_DIMS_MODE ) ;
ssSetInputPortFrameData ( S , 2 , FRAME_NO ) ; if ( ssGetSimMode ( S ) !=
SS_SIMMODE_SIZES_CALL_ONLY ) { ssSetInputPortDataType ( S , 2 , SS_SINGLE ) ;
} ssSetInputPortDirectFeedThrough ( S , 2 , 0 ) ;
ssSetInputPortRequiredContiguous ( S , 2 , 1 ) ; ssSetInputPortOptimOpts ( S
, 2 , SS_NOT_REUSABLE_AND_LOCAL ) ; ssSetInputPortOverWritable ( S , 2 ,
FALSE ) ; ssSetInputPortSampleTime ( S , 2 , 0.001 ) ;
ssSetInputPortOffsetTime ( S , 2 , 0.0 ) ; if ( !
ssSetInputPortVectorDimension ( S , 3 , 1 ) ) return ;
ssSetInputPortDimensionsMode ( S , 3 , FIXED_DIMS_MODE ) ;
ssSetInputPortFrameData ( S , 3 , FRAME_NO ) ; if ( ssGetSimMode ( S ) !=
SS_SIMMODE_SIZES_CALL_ONLY ) { ssSetInputPortDataType ( S , 3 , SS_SINGLE ) ;
} ssSetInputPortDirectFeedThrough ( S , 3 , 0 ) ;
ssSetInputPortRequiredContiguous ( S , 3 , 1 ) ; ssSetInputPortOptimOpts ( S
, 3 , SS_NOT_REUSABLE_AND_LOCAL ) ; ssSetInputPortOverWritable ( S , 3 ,
FALSE ) ; ssSetInputPortSampleTime ( S , 3 , 0.001 ) ;
ssSetInputPortOffsetTime ( S , 3 , 0.0 ) ; if ( !
ssSetInputPortVectorDimension ( S , 4 , 1 ) ) return ;
ssSetInputPortDimensionsMode ( S , 4 , FIXED_DIMS_MODE ) ;
ssSetInputPortFrameData ( S , 4 , FRAME_NO ) ; if ( ssGetSimMode ( S ) !=
SS_SIMMODE_SIZES_CALL_ONLY ) { ssSetInputPortDataType ( S , 4 , SS_SINGLE ) ;
} ssSetInputPortDirectFeedThrough ( S , 4 , 1 ) ;
ssSetInputPortRequiredContiguous ( S , 4 , 1 ) ; ssSetInputPortOptimOpts ( S
, 4 , SS_NOT_REUSABLE_AND_LOCAL ) ; ssSetInputPortOverWritable ( S , 4 ,
FALSE ) ; ssSetInputPortSampleTime ( S , 4 , 0.001 ) ;
ssSetInputPortOffsetTime ( S , 4 , 0.0 ) ; if ( !
ssSetInputPortVectorDimension ( S , 5 , 1 ) ) return ;
ssSetInputPortDimensionsMode ( S , 5 , FIXED_DIMS_MODE ) ;
ssSetInputPortFrameData ( S , 5 , FRAME_NO ) ; if ( ssGetSimMode ( S ) !=
SS_SIMMODE_SIZES_CALL_ONLY ) { ssSetInputPortDataType ( S , 5 , SS_SINGLE ) ;
} ssSetInputPortDirectFeedThrough ( S , 5 , 1 ) ;
ssSetInputPortRequiredContiguous ( S , 5 , 1 ) ; ssSetInputPortOptimOpts ( S
, 5 , SS_NOT_REUSABLE_AND_LOCAL ) ; ssSetInputPortOverWritable ( S , 5 ,
FALSE ) ; ssSetInputPortSampleTime ( S , 5 , 0.001 ) ;
ssSetInputPortOffsetTime ( S , 5 , 0.0 ) ; if ( !
ssSetInputPortVectorDimension ( S , 6 , 1 ) ) return ;
ssSetInputPortDimensionsMode ( S , 6 , FIXED_DIMS_MODE ) ;
ssSetInputPortFrameData ( S , 6 , FRAME_NO ) ; if ( ssGetSimMode ( S ) !=
SS_SIMMODE_SIZES_CALL_ONLY ) { ssSetInputPortDataType ( S , 6 , SS_SINGLE ) ;
} ssSetInputPortDirectFeedThrough ( S , 6 , 1 ) ;
ssSetInputPortRequiredContiguous ( S , 6 , 1 ) ; ssSetInputPortOptimOpts ( S
, 6 , SS_REUSABLE_AND_LOCAL ) ; ssSetInputPortOverWritable ( S , 6 , FALSE )
; ssSetInputPortSampleTime ( S , 6 , 0.001 ) ; ssSetInputPortOffsetTime ( S ,
6 , 0.0 ) ; if ( ! ssSetInputPortVectorDimension ( S , 7 , 1 ) ) return ;
ssSetInputPortDimensionsMode ( S , 7 , FIXED_DIMS_MODE ) ;
ssSetInputPortFrameData ( S , 7 , FRAME_NO ) ; if ( ssGetSimMode ( S ) !=
SS_SIMMODE_SIZES_CALL_ONLY ) { ssSetInputPortDataType ( S , 7 , SS_SINGLE ) ;
} ssSetInputPortDirectFeedThrough ( S , 7 , 1 ) ;
ssSetInputPortRequiredContiguous ( S , 7 , 1 ) ; ssSetInputPortOptimOpts ( S
, 7 , SS_REUSABLE_AND_LOCAL ) ; ssSetInputPortOverWritable ( S , 7 , FALSE )
; ssSetInputPortSampleTime ( S , 7 , 0.001 ) ; ssSetInputPortOffsetTime ( S ,
7 , 0.0 ) ; if ( ! ssSetNumOutputPorts ( S , 3 ) ) return ; if ( !
ssSetOutputPortVectorDimension ( S , 0 , 1 ) ) return ;
ssSetOutputPortDimensionsMode ( S , 0 , FIXED_DIMS_MODE ) ;
ssSetOutputPortFrameData ( S , 0 , FRAME_NO ) ; if ( ssGetSimMode ( S ) !=
SS_SIMMODE_SIZES_CALL_ONLY ) {
#if defined (MATLAB_MEX_FILE)
{ DTypeId dataTypeIdReg ; ssRegisterTypeFromNamedObject ( S , "Cntrl_Status"
, & dataTypeIdReg ) ; if ( dataTypeIdReg == INVALID_DTYPE_ID ) return ;
ssSetOutputPortDataType ( S , 0 , dataTypeIdReg ) ; }
#endif
} ssSetOutputPortSampleTime ( S , 0 , 0.001 ) ; ssSetOutputPortOffsetTime ( S
, 0 , 0.0 ) ; ssSetOutputPortDiscreteValuedOutput ( S , 0 , 0 ) ;
ssSetOutputPortOkToMerge ( S , 0 , SS_OK_TO_MERGE ) ;
ssSetOutputPortOptimOpts ( S , 0 , SS_NOT_REUSABLE_AND_GLOBAL ) ; if ( !
ssSetOutputPortVectorDimension ( S , 1 , 1 ) ) return ;
ssSetOutputPortDimensionsMode ( S , 1 , FIXED_DIMS_MODE ) ;
ssSetOutputPortFrameData ( S , 1 , FRAME_NO ) ; if ( ssGetSimMode ( S ) !=
SS_SIMMODE_SIZES_CALL_ONLY ) {
#if defined (MATLAB_MEX_FILE)
{ DTypeId dataTypeIdReg ; ssRegisterTypeFromNamedObject ( S , "Motor_Cmds" ,
& dataTypeIdReg ) ; if ( dataTypeIdReg == INVALID_DTYPE_ID ) return ;
ssSetOutputPortDataType ( S , 1 , dataTypeIdReg ) ; }
#endif
} ssSetOutputPortSampleTime ( S , 1 , 0.001 ) ; ssSetOutputPortOffsetTime ( S
, 1 , 0.0 ) ; ssSetOutputPortDiscreteValuedOutput ( S , 1 , 0 ) ;
ssSetOutputPortOkToMerge ( S , 1 , SS_OK_TO_MERGE ) ;
ssSetOutputPortOptimOpts ( S , 1 , SS_NOT_REUSABLE_AND_LOCAL ) ; if ( !
ssSetOutputPortVectorDimension ( S , 2 , 1 ) ) return ;
ssSetOutputPortDimensionsMode ( S , 2 , FIXED_DIMS_MODE ) ;
ssSetOutputPortFrameData ( S , 2 , FRAME_NO ) ; if ( ssGetSimMode ( S ) !=
SS_SIMMODE_SIZES_CALL_ONLY ) { ssSetOutputPortDataType ( S , 2 , SS_SINGLE )
; } ssSetOutputPortSampleTime ( S , 2 , 0.001 ) ; ssSetOutputPortOffsetTime (
S , 2 , 0.0 ) ; ssSetOutputPortDiscreteValuedOutput ( S , 2 , 0 ) ;
ssSetOutputPortOkToMerge ( S , 2 , SS_OK_TO_MERGE ) ;
ssSetOutputPortOptimOpts ( S , 2 , SS_NOT_REUSABLE_AND_LOCAL ) ; { real_T
minValue = rtMinusInf ; real_T maxValue = rtInf ;
ssSetModelRefInputSignalDesignMin ( S , 0 , & minValue ) ;
ssSetModelRefInputSignalDesignMax ( S , 0 , & maxValue ) ; } { real_T
minValue = rtMinusInf ; real_T maxValue = rtInf ;
ssSetModelRefInputSignalDesignMin ( S , 1 , & minValue ) ;
ssSetModelRefInputSignalDesignMax ( S , 1 , & maxValue ) ; } { real_T
minValue = rtMinusInf ; real_T maxValue = rtInf ;
ssSetModelRefInputSignalDesignMin ( S , 2 , & minValue ) ;
ssSetModelRefInputSignalDesignMax ( S , 2 , & maxValue ) ; } { real_T
minValue = rtMinusInf ; real_T maxValue = rtInf ;
ssSetModelRefInputSignalDesignMin ( S , 3 , & minValue ) ;
ssSetModelRefInputSignalDesignMax ( S , 3 , & maxValue ) ; } { real_T
minValue = rtMinusInf ; real_T maxValue = rtInf ;
ssSetModelRefInputSignalDesignMin ( S , 4 , & minValue ) ;
ssSetModelRefInputSignalDesignMax ( S , 4 , & maxValue ) ; } { real_T
minValue = rtMinusInf ; real_T maxValue = rtInf ;
ssSetModelRefInputSignalDesignMin ( S , 5 , & minValue ) ;
ssSetModelRefInputSignalDesignMax ( S , 5 , & maxValue ) ; } { real_T
minValue = rtMinusInf ; real_T maxValue = rtInf ;
ssSetModelRefInputSignalDesignMin ( S , 6 , & minValue ) ;
ssSetModelRefInputSignalDesignMax ( S , 6 , & maxValue ) ; } { real_T
minValue = rtMinusInf ; real_T maxValue = rtInf ;
ssSetModelRefInputSignalDesignMin ( S , 7 , & minValue ) ;
ssSetModelRefInputSignalDesignMax ( S , 7 , & maxValue ) ; } { real_T
minValue = rtMinusInf ; real_T maxValue = rtInf ;
ssSetModelRefOutputSignalDesignMin ( S , 0 , & minValue ) ;
ssSetModelRefOutputSignalDesignMax ( S , 0 , & maxValue ) ; } { real_T
minValue = rtMinusInf ; real_T maxValue = rtInf ;
ssSetModelRefOutputSignalDesignMin ( S , 1 , & minValue ) ;
ssSetModelRefOutputSignalDesignMax ( S , 1 , & maxValue ) ; } { real_T
minValue = rtMinusInf ; real_T maxValue = rtInf ;
ssSetModelRefOutputSignalDesignMin ( S , 2 , & minValue ) ;
ssSetModelRefOutputSignalDesignMax ( S , 2 , & maxValue ) ; } { static
ssRTWStorageType storageClass [ 11 ] = { SS_RTW_STORAGE_AUTO ,
SS_RTW_STORAGE_AUTO , SS_RTW_STORAGE_AUTO , SS_RTW_STORAGE_AUTO ,
SS_RTW_STORAGE_AUTO , SS_RTW_STORAGE_AUTO , SS_RTW_STORAGE_AUTO ,
SS_RTW_STORAGE_AUTO , SS_RTW_STORAGE_AUTO , SS_RTW_STORAGE_AUTO ,
SS_RTW_STORAGE_AUTO } ; ssSetModelRefPortRTWStorageClasses ( S , storageClass
) ; } ssSetModelRefSignalLoggingSaveFormat ( S , SS_DATASET_FORMAT ) ;
ssSetNumSampleTimes ( S , PORT_BASED_SAMPLE_TIMES ) ; ssSetNumRWork ( S , 0 )
; ssSetNumIWork ( S , 0 ) ; ssSetNumPWork ( S , 0 ) ; ssSetNumModes ( S , 0 )
; { int_T zcsIdx = 0 ; } ssSetOutputPortIsNonContinuous ( S , 0 , 0 ) ;
ssSetOutputPortIsFedByBlockWithModesNoZCs ( S , 0 , 0 ) ;
ssSetOutputPortIsNonContinuous ( S , 1 , 0 ) ;
ssSetOutputPortIsFedByBlockWithModesNoZCs ( S , 1 , 0 ) ;
ssSetOutputPortIsNonContinuous ( S , 2 , 0 ) ;
ssSetOutputPortIsFedByBlockWithModesNoZCs ( S , 2 , 0 ) ;
ssSetInputPortIsNotDerivPort ( S , 0 , 1 ) ; ssSetInputPortIsNotDerivPort ( S
, 1 , 1 ) ; ssSetInputPortIsNotDerivPort ( S , 2 , 1 ) ;
ssSetInputPortIsNotDerivPort ( S , 3 , 1 ) ; ssSetInputPortIsNotDerivPort ( S
, 4 , 1 ) ; ssSetInputPortIsNotDerivPort ( S , 5 , 1 ) ;
ssSetInputPortIsNotDerivPort ( S , 6 , 1 ) ; ssSetInputPortIsNotDerivPort ( S
, 7 , 1 ) ; ssSetModelReferenceSampleTimeInheritanceRule ( S ,
DISALLOW_SAMPLE_TIME_INHERITANCE ) ; ssSetOptimizeModelRefInitCode ( S , 1 )
; ssSetAcceptsFcnCallInputs ( S ) ; { static const char * inlinedVars [ ] = {
"EV_Param" , "brake_cmd_table" , "ctrlConst" , "decel_cmd_vec" ,
"regen_pwr_vec" } ; ssSetModelRefInlinedVars ( S , 5 , ( void * ) inlinedVars
) ; } ssSetModelReferenceNormalModeSupport ( S ,
MDL_START_AND_MDL_PROCESS_PARAMS_OK ) ; ssSupportsMultipleExecInstances ( S ,
FALSE ) ; ssRegisterMsgForNotSupportingMultiExecInst ( S ,
"<diag_root><diag id=\"Simulink:blocks:ImplicitIterSS_SigObjExpStorageClassNotSupportedInside\"><arguments><arg type=\"numeric\">1</arg><arg type=\"encoded\">RQBWAF8AUAB3AHIAXwBNAGEAbgBhAGcAZQByAC8AUABvAHcAZQByAF8AQwBvAG4AdAByAG8AbAAvAEUAcgByAG8AcgAgAFMAdQBtAAAA</arg><arg type=\"encoded\">cABvAHcAZQByAF8AZQByAHIAbwByAAAA</arg><arg type=\"encoded\">PABfAF8AaQBpAFMAUwBfAF8APgA8AC8AXwBfAGkAaQBTAFMAXwBfAD4AAAA=</arg><arg type=\"encoded\">PABfAF8AaQB0AGUAcgBCAGwAawBfAF8APgA8AC8AXwBfAGkAdABlAHIAQgBsAGsAXwBfAD4AAAA=</arg></arguments></diag>\n</diag_root>"
) ; ssHasStateInsideForEachSS ( S , FALSE ) ; ssSetModelRefHasParforForEachSS
( S , FALSE ) ; ssSetModelRefHasVariantModelOrSubsystem ( S , FALSE ) ;
ssSetNumAsyncTs ( S , 0 ) ; ssSetOptions ( S , SS_OPTION_EXCEPTION_FREE_CODE
| SS_OPTION_DISALLOW_CONSTANT_SAMPLE_TIME |
SS_OPTION_SUPPORTS_ALIAS_DATA_TYPES | SS_OPTION_WORKS_WITH_CODE_REUSE ) ; if
( S -> mdlInfo -> genericFcn != ( NULL ) ) { ssRegModelRefChildModel ( S , 1
, childModels ) ; }
#if SS_SFCN_FOR_SIM
if ( S -> mdlInfo -> genericFcn != ( NULL ) && ssGetSimMode ( S ) !=
SS_SIMMODE_SIZES_CALL_ONLY ) { int_T retVal = 1 ;
mr_EV_Pwr_Manager_MdlInfoRegFcn ( S , "EV_Pwr_Manager" , & retVal ) ; if ( !
retVal ) return ; }
#endif
ssSetNumDWork ( S , 0 ) ; slmrSetHasNonVirtualConstantTs ( S , true ) ;
ssSetNeedAbsoluteTime ( S , 1 ) ; ssSetModelRefHasEnablePort ( S , 0 ) ; }
/* Function to initialize sizes. */
static void mdlInitializeSizes(SimStruct *S)
{
ssSetNumSampleTimes(S, 1); /* Number of sample times */
ssSetNumContStates(S, 0); /* Number of continuous states */
ssSetNumNonsampledZCs(S, 0); /* Number of nonsampled ZCs */
/* Number of output ports */
if (!ssSetNumOutputPorts(S, 1))
return;
/* outport number: 0 */
if (!ssSetOutputPortVectorDimension(S, 0, 1))
return;
if (ssGetSimMode(S) != SS_SIMMODE_SIZES_CALL_ONLY) {
ssSetOutputPortDataType(S, 0, SS_DOUBLE);
}
ssSetOutputPortSampleTime(S, 0, 0.06);
ssSetOutputPortOffsetTime(S, 0, 0.0);
ssSetOutputPortOptimOpts(S, 0, SS_REUSABLE_AND_LOCAL);
/* Number of input ports */
if (!ssSetNumInputPorts(S, 1))
return;
/* inport number: 0 */
{
if (!ssSetInputPortVectorDimension(S, 0, 1))
return;
if (ssGetSimMode(S) != SS_SIMMODE_SIZES_CALL_ONLY) {
ssSetInputPortDataType(S, 0, SS_DOUBLE);
}
ssSetInputPortDirectFeedThrough(S, 0, 1);
ssSetInputPortSampleTime(S, 0, 0.06);
ssSetInputPortOffsetTime(S, 0, 0.0);
ssSetInputPortOverWritable(S, 0, 0);
ssSetInputPortOptimOpts(S, 0, SS_NOT_REUSABLE_AND_GLOBAL);
}
ssSetRTWGeneratedSFcn(S, 1); /* Generated S-function */
/* DWork */
if (!ssSetNumDWork(S, 1)) {
return;
}
/* '<S1>/LinearModel': DSTATE */
ssSetDWorkName(S, 0, "DWORK0");
ssSetDWorkWidth(S, 0, 1);
ssSetDWorkUsedAsDState(S, 0, 1);
/* Tunable Parameters */
ssSetNumSFcnParams(S, 0);
/* Number of expected parameters */
#if defined(MATLAB_MEX_FILE)
if (ssGetNumSFcnParams(S) == ssGetSFcnParamsCount(S)) {
#if defined(MDL_CHECK_PARAMETERS)
mdlCheckParameters(S);
#endif /* MDL_CHECK_PARAMETERS */
if (ssGetErrorStatus(S) != (NULL) ) {
return;
}
} else {
return; /* Parameter mismatch will be reported by Simulink */
}
#endif /* MATLAB_MEX_FILE */
/* Options */
ssSetOptions(S, (SS_OPTION_RUNTIME_EXCEPTION_FREE_CODE |
SS_OPTION_PORT_SAMPLE_TIMES_ASSIGNED ));
#if SS_SFCN_FOR_SIM
{
ssSupportsMultipleExecInstances(S, false);
ssRegisterMsgForNotSupportingMultiExecInst(S,
"<diag_root><diag id=\"Simulink:blocks:BlockDoesNotSupportMultiExecInstances\"><arguments><arg type=\"encoded\">SABhAG0AbQBlAHIAcwB0AGUAaQBuAC8ASABhAG0AbQBlAHIAcwB0AGUAaQBuAC0AVwBpAGUAbgBlAHIAIABNAG8AZABlAGwAMQAvAFAAdwBsAGkAbgBlAGEAcgAxAAAA</arg><arg type=\"encoded\">PABfAF8AaQBpAFMAUwBfAF8APgA8AC8AXwBfAGkAaQBTAFMAXwBfAD4AAAA=</arg><arg type=\"encoded\">PABfAF8AaQB0AGUAcgBCAGwAawBfAF8APgA8AC8AXwBfAGkAdABlAHIAQgBsAGsAXwBfAD4AAAA=</arg></arguments></diag>\n</diag_root>");
ssHasStateInsideForEachSS(S, false);
}
#endif
}
static void mdlStart(SimStruct *S)
{
/*char* pp1Orig;=(char*)mxGetPr(ssGetSFcnParam(S,0));*/
int param_width1=mxGetNumberOfElements(ssGetSFcnParam(S,0));
int i,j,mod;
char pp1[200];
/*Get the configuration parameter from simulink*/
memset(pp1, 0,200* sizeof(char));
mxGetString(ssGetSFcnParam(S,0),pp1,param_width1+1);
/* if ((char)pp1Orig[i]=='[')*/
/*{
mod=2;
for (i=0;i<param_width1*mod;i+=mod)
pp1[i/mod]=(char)pp1Orig[i];
}*/
SS_SimMode mode= ssGetSimMode(S);
if(mode!=0)
goto OUT;
/*Setup USB serial connection*/
/*The following lines are done automatically at startup due to installed usbHostConf scrip*/
/*j=system("insmod /media/mmc2/usbserial.ko");
j=system("insmod /media/mmc2/ftdi_sio.ko");
j=system("echo host > /sys/devices/platform/musb_hdrc/mode");*/
fd = open("/dev/ttyUSB0", O_RDWR | O_NOCTTY | O_NDELAY);
if (fd == -1)
{
perror("open_port: Unable to open /dev/ttyUSB0 - ");
fd = open("/dev/ttyUSB1", O_RDWR | O_NOCTTY | O_NDELAY);
if (fd == -1)
{
perror("open_port: Unable to open /dev/ttyUSB1 - ");
}
}
else
fcntl(fd, F_SETFL, 0);
struct termios options;
if (tcgetattr(fd, &options) < 0) {
perror("init_serialport: Couldn't get term attributes");
goto OUT;
}
/*BAUD RATE SET, MUST MATCH ARDUINOS!!!*/
cfsetispeed(&options, B115200);
cfsetospeed(&options, B115200);
/* 8N1*/
options.c_cflag &= ~CSIZE; /* Mask the character size bits */
options.c_cflag |= CS8; /* Select 8 data bits */
options.c_cflag &= ~PARENB;
options.c_cflag &= ~CSTOPB;
/* turn off HUPCL to avoid reset*/
options.c_cflag &= ~HUPCL;
/*no flow control*/
/*options.c_cflag &= ~CRTSCTS;*/
options.c_cflag |= (CLOCAL | CREAD);
options.c_iflag &= ~(IXON | IXOFF | IXANY); /* turn off s/w flow ctrl*/
options.c_lflag &= ~(ICANON | ECHO | ECHOE | ISIG); /* make raw*/
/* see: http://unixwiz.net/techtips/termios-vmin-vtime.html*/
options.c_cc[VMIN] = 0;
options.c_cc[VTIME] = 20;
if( tcsetattr(fd, TCSANOW, &options) < 0) {
perror("init_serialport: Couldn't set term attributes");
goto OUT;
}
usleep(1000000);/*Pause for one second to assure connection*/
/*END SERIAL LINE INTIALIZATION-------------------*/
/*Clear arrays*/
for(i=0;i<20;i++)
{
prefix[i]=0;
lasts[i]=-999;
}
memset(conf, 0,100* sizeof(char));
printf("width=%d\n",param_width1);
printf("writing config-");
for(i=0;i<param_width1;i++)
{
conf[i]=*(pp1+i);
printf("%c",conf[i]);
}
printf("\n",*(pp1+i));
fflush(stdout);
j=write(fd,"!!!!!!X",7);/*Flush out unfinished commands and reset conf*/
if (j < 0)
fputs("write() of bytes failed!\n", stderr);
j=write(fd,conf,param_width1);
if (j < 0)
fputs("write() of bytes failed!\n", stderr);
for(i=0; i<param_width1/4;i++)/*parse through input config (each part is 4 chars)*/
{
int curIndex;
curIndex=(*(pp1+i*4+3))-48 +((*(pp1+i*4+2))=='A')*14 ;
/*printf("ind=%d\n",curIndex);*/
fflush(stdout);
prefix[(int)curIndex]=*(pp1+i*4+4);
prefix[(int)curIndex]=*(pp1+i*4+4);
/*printf("pref=%c\n",prefix[curIndex]);*/
}
OUT:;;
}
/* 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)
{
/* Number of expected parameters */
ssSetNumSFcnParams(S, 0);
/*
* Set the number of pworks.
*/
ssSetNumPWork(S, 0);
/*
* Set the number of dworks.
*/
if (!ssSetNumDWork(S, 0))
return;
/*
* Set the number of input ports.
*/
if (!ssSetNumInputPorts(S, 0))
return;
/*
* Set the number of output ports.
*/
if (!ssSetNumOutputPorts(S, 0))
return;
/*
* Register reserved identifiers to avoid name conflict
*/
if (ssRTWGenIsCodeGen(S) || ssGetSimMode(S)==SS_SIMMODE_EXTERNAL) {
/*
* Register reserved identifier for StartFcnSpec
*/
ssRegMdlInfo(S, "Actuator_Initialization", MDL_INFO_ID_RESERVED, 0, 0,
ssGetPath(S));
/*
* Register reserved identifier for TerminateFcnSpec
*/
ssRegMdlInfo(S, "Actuator_Stop", MDL_INFO_ID_RESERVED, 0, 0, ssGetPath(S));
}
/*
* This S-function can be used in referenced model simulating in normal mode.
*/
ssSetModelReferenceNormalModeSupport(S, MDL_START_AND_MDL_PROCESS_PARAMS_OK);
/*
* Set the number of sample time.
*/
ssSetNumSampleTimes(S, 1);
/*
* All options have the form SS_OPTION_<name> and are documented in
* matlabroot/simulink/include/simstruc.h. The options should be
* bitwise or'd together as in
* ssSetOptions(S, (SS_OPTION_name1 | SS_OPTION_name2))
*/
ssSetOptions(S,
SS_OPTION_USE_TLC_WITH_ACCELERATOR |
SS_OPTION_CAN_BE_CALLED_CONDITIONALLY |
SS_OPTION_EXCEPTION_FREE_CODE |
SS_OPTION_WORKS_WITH_CODE_REUSE |
SS_OPTION_SFUNCTION_INLINED_FOR_RTW |
SS_OPTION_DISALLOW_CONSTANT_SAMPLE_TIME);
ssSetSimulinkVersionGeneratedIn(S, "8.3");
}
