/* Function: mdlOutputs =======================================================
*
*/
static void mdlOutputs(SimStruct *S, int_T tid)
{
uint8_T *y0 = (uint8_T *)ssGetOutputPortRealSignal(S,0);
const int_T y_width = ssGetOutputPortWidth(S,0);
int i;
char* buffer[1];
int connected=ssGetIWorkValue(S, 0);
int curCon = ssGetIWorkValue(S, 2);
char* last = (void*) ssGetPWorkValue(S, 1);
if(connected==1)
{
fcntl(curCon, F_SETFL, O_NONBLOCK);
if(recv(curCon,buffer, 1, MSG_PEEK)>0)
{
read(curCon,last,y_width);
/*printf("%.*s\n",y_width,last);*/
}
}
else
{
connected=tryConnect(S);
if (connected)
printf("Connected To Client\n");
}
/*fflush(stdout);*/
for(i=0;i<y_width;i++)
y0[i]=last[i];
ssSetIWorkValue(S, 0, connected);
/*ssSetPWorkValue(S, 0, (void*) newsockfd);*/
}
/* 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)
{
int curCon = ssGetIWorkValue(S, 1);
int accCon = ssGetIWorkValue(S, 2);
close(curCon);
close(accCon);
}
static void mdlOutputs(SimStruct * S, int_T tid) {
int_T id = ssGetIWorkValue(S, 0);
int_T noBytes = ssGetIWorkValue(S, 1);
int_T count;
uint8_T *buffer = (uint8_T *)ssGetPWorkValue(S, 0);
count = udpReceive(id, buffer, noBytes);
*(ssGetOutputPortRealSignal(S, 1)) = !count;
if (count == 0)
memcpy((void *) ssGetOutputPortSignal(S, 0),
(void *) (uint8_T *) buffer, noBytes);
}
static void mdlOutputs(SimStruct *S, int_T tid)
{
#ifndef MATLAB_MEX_FILE
int_T nChannels=ssGetIWorkValue(S,CHANNELS_I_IND);
uint_T base=ssGetIWorkValue(S,BASE_ADDR_I_IND);
int_T i,j,channel;
uint_T tempData;
real_T *output;
volatile uint32_T *ioaddress;
//start A/D conversion (ADCNTL5)
ioaddress=(void *)(base+0x2C0);
ioaddress[0]=0x1;
//wait for the start of A/D conversion
delay(60e-6);
//wait for the finish of all channels
delay(nChannels*(120e-6));
//start A/D conversion (ADCNTL5)
ioaddress=(void *)(base+0x2C0);
ioaddress[0]=0x0;
delay(16e-6);
for(i=0;i<nChannels;i++)
{
//read data from ADCNTL8
ioaddress=(void *)(base+0x2F0);
tempData=ioaddress[0];
delay(16e-6);
//get channel number from tempData[13][12]
channel=(tempData&0xf000)>>12;
output=ssGetOutputPortSignal(S,channel);
tempData=tempData&0xfff;
output[0]=(real_T)(tempData*ssGetRWorkValue(S,GAIN_R_IND)+ssGetRWorkValue(S,OFFSET_R_IND));
}
//clear data on FIFO (ADCNTL7)
ioaddress=(void *)(base+0x2E0);
ioaddress[0]=0xfff;
delay(16e-6);
//clear FIFO again (read ADCNTL8,there is "752" store in FIFO even after clear FIFO once,only on PCI2013)
ioaddress=(void *)(base+0x2F0);
tempData=ioaddress[0];
delay(16e-6);
#endif
}
static void mdlOutputs(SimStruct *S, int_T tid)
{
int_T i;
real_T *y = ssGetOutputPortRealSignal(S,0);
int_T num_channels = ssGetOutputPortWidth(S,0);
uint_T base_dio=ssGetIWorkValue(S,BASE_ADDR_I_IND);
int_T port=ssGetIWorkValue(S,OUTPORT_I_IND);
int_T input;
#ifndef MATLAB_MEX_FILE
input=inb(base_dio+port);
for(i=0; i<num_channels; i++){
*y++=(input & (1 << i)) >> i;
}
#endif
}
static void mdlOutputs(SimStruct *S, int_T tid)
{
real_T *y = ssGetOutputPortRealSignal(S,0);
int_T num_channels = ssGetOutputPortWidth(S,0);
uint_T base_adc=ssGetIWorkValue(S,BASE_ADDR_I_IND);
uint_T base_status=ssGetIWorkValue(S,BASE_ADDR_I_IND+1);
int_T i,res;
#ifndef MATLAB_MEX_FILE
for(i=0; i<num_channels; i++){
outw(0x00,base_adc);
while((inw(base_status) & 0x1000)==0x0);
res=inw(base_adc);
*y++=20.0*res/4096-10.0;
}
#endif
}
static void mdlTerminate(SimStruct * S) {
void *buffer = ssGetPWorkValue(S, 0);
int id = ssGetIWorkValue(S, 0);
udpClose(id);
if (buffer != NULL) {
free(buffer);
}
}
/* 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)
{
#ifndef MATALB_MEX_FILE
volatile uint32_T *ioaddress;
uint_T base=ssGetIWorkValue(S,BASE_ADDR_I_IND);
//clear device (write sth in ADCNTL7)
ioaddress=(void *)(base+0x2E0);
ioaddress[0]=0xfff;
delay(16e-6);
#endif
}
static void mdlTerminate(SimStruct * S) {
void *buffer = ssGetPWorkValue(S, 0);
int id = ssGetIWorkValue(S, 0);
/* stop the thread (FIRST) */
run_pthread = 0;
#ifdef __linux__
usleep(thread_usleep);
if (dc1394_stop_iso_transmission(raw1394Handle,dc1394Camera.node) != DC1394_SUCCESS)
return;
cleanup();
#endif
if (buffer != NULL)
free(buffer);
}
static void mdlOutputs(SimStruct *S, int_T tid)
{
real_T *y = ssGetOutputPortRealSignal(S,0);
int_T num_channels = ssGetOutputPortWidth(S,0);
int_T first_chan = ssGetIWorkValue(S,BASE_ADDR_I_IND);
int i;
int val[8];
#ifndef MATLAB_MEX_FILE
read_adc(first_chan, num_channels, val);
for(i=0; i<num_channels; i++){
*y++=(real_T) i2v(val[i]);
}
#endif
}
static int tryConnect(SimStruct *S)
{
struct sockaddr_in* cli_addr=(void*)ssGetPWorkValue(S, 0);
int curCon,accCon=ssGetIWorkValue(S, 1),clilen = sizeof(cli_addr);
listen(accCon,5);
curCon = accept(accCon,(struct sockaddr *) &cli_addr,&clilen);
ssSetIWorkValue(S, 2, curCon);
if (curCon < 0)
return 0;
else
{
return 1;
}
}
static void mdlOutputs(SimStruct * S, int_T tid) {
uint8_T *buffer;
int_T noBytes;
real_T *count = ssGetOutputPortRealSignal(S,1);
buffer = (uint8_T *)ssGetPWorkValue(S,0);
noBytes = ssGetIWorkValue(S,0);
switch(frame_flag)
{
case 0:
*count = (real_T)0;
break;
case 1:
*count = (real_T)(640*480);
memcpy((void *) ssGetOutputPortSignal(S, 0), (void *) (uint8_T *) image_buffer1, 640*480);
break;
case 2:
*count = (real_T)(640*480);
memcpy((void *) ssGetOutputPortSignal(S, 0), (void *) (uint8_T *) image_buffer2, 640*480);
break;
}
}
static void mdlTerminate(SimStruct *S) {
#ifndef MATLAB_MEX_FILE
int num = ssGetIWorkValue(S,0);
cleanRosBlock(num);
#endif
}
static void mdlOutputs(SimStruct *S, int_T tid)
{
PCONFIG_DATA config;
const real_T *in_xdot;
const real_T *in_x0;
InputRealPtrsType pin_params;
const boolean_T *in_reset;
real_T *out_x;
real_T *out_t;
real_T *xd;
real_T *xd_temp;
real_T *xd_temp2;
time_T initial_time;
time_T final_time;
quaternion phi;
quaternion q;
vector omegadot_temp;
const real_T *pomegadot;
int_T i;
/* Retrieve C object from the pointers vector */
config = ssGetPWork(S)[0];
xd = (real_T*) ssGetDWork(S,0);
xd_temp = (real_T*) ssGetDWork(S,1);
if( config->nq ) xd_temp2 = (real_T*) ssGetDWork(S,2);
in_xdot = ssGetInputPortRealSignal(S, config->idxin_xdot);
if( config->idxin_x0 ) in_x0 = ssGetInputPortRealSignal(S, config->idxin_x0);
if( config->idxin_params ) pin_params = ssGetInputPortRealSignalPtrs(S, config->idxin_params);
if( config->idxin_reset ) in_reset = ((InputBooleanPtrsType) ssGetInputPortSignalPtrs(S, config->idxin_reset))[0];
out_x = ssGetOutputPortRealSignal(S, 1);
if( config->idxout_time ) out_t = ssGetOutputPortRealSignal(S, config->idxout_time);
switch( intval(mxGetScalar(paramSpecificationsSource)) )
{
case 1:
initial_time = config->initial_time;
final_time = ssGetTaskTime(S,0);
break;
case 2:
initial_time = mxGetScalar(paramInitialTime);
final_time = mxGetScalar(paramFinalTime);
break;
case 3:
initial_time = *(pin_params[0]);
final_time = *(pin_params[1]);
break;
default:
ssSetErrorStatus(S,"Wrong integration algorithm selected");
return;
}
/* ssPrintf("ti=%f, tf=%f\r\n", initial_time, final_time); */
/* Reset the states */
if( ssGetIWorkValue(S, 0) || (config->idxin_reset && *in_reset) || (intval(mxGetScalar(paramSpecificationsSource)) > 1) )
{
ssSetIWorkValue(S, 0, 0);
if( intval(mxGetScalar(paramInitialConditionSource)) == 1 )
{
/* Internal initial conditions */
for( i=0; i<config->nstates; i++ )
{
xd[i] = mxGetPr(paramInitialCondition)[(mxGetNumberOfElements(paramInitialCondition) == 1 ? 0 : i)];
}
}
else
{
/* External initial conditions */
memcpy(xd, in_x0, config->nstates*sizeof(real_T));
}
memcpy(out_x, xd, config->nstates*sizeof(real_T));
if( config->idxout_time ) out_t[0] = initial_time;
}
if( final_time > initial_time )
{
if( intval(mxGetScalar(paramIntegrationAlgorithm)) == 1 )
{
/* Euler algorithm */
if( !ssCallSystemWithTid(S,0,tid) ) return;
i = 0;
while( i<config->nstates )
{
if( config->nq && (i >= config->start_idx_q) && (i < config->end_idx_q) )
{
pomegadot = ( config->use_omegadot ? &in_xdot[i] : config->omegadot_zero );
QuaternionIntegralEulerChange( final_time-initial_time, &in_xdot[i], pomegadot, &xd[i], phi );
QuaternionProd(&xd[i], phi, &xd[i]);
QuaternionNormalize(&xd[i]);
i += 4;
}
else
{
xd[i] += in_xdot[i] * (final_time-initial_time);
i++;
}
}
}
if( intval(mxGetScalar(paramIntegrationAlgorithm)) == 2 )
{
/* Runge-Kutta algorithm */
/* f1 */
if( !ssCallSystemWithTid(S,0,tid) ) return;
i = 0;
while( i<config->nstates )
{
if( config->nq && (i >= config->start_idx_q) && (i < config->end_idx_q) )
{
pomegadot = ( config->use_omegadot ? &in_xdot[i] : config->omegadot_zero );
omegadot_temp[0] = pomegadot[0]*6; omegadot_temp[1] = pomegadot[1]*6; omegadot_temp[2] = pomegadot[2]*6;
QuaternionIntegralEulerChange( (final_time-initial_time)/6, &in_xdot[i], omegadot_temp, &xd[i], &xd_temp[i] );
QuaternionProd(&xd_temp[i], &xd_temp[i], q);
QuaternionProd(&xd_temp[i], q, phi);
QuaternionProd(&xd[i], phi, &out_x[i]);
i += 4;
}
else
{
xd_temp[i] = in_xdot[i];
out_x[i] = xd[i] + 0.5*(final_time-initial_time)*in_xdot[i];
i++;
}
}
if( config->idxout_time ) out_t[0] = initial_time + 0.5*(final_time-initial_time);
/* f2 */
if( !ssCallSystemWithTid(S,0,tid) ) return;
i = 0;
while( i<config->nstates )
{
if( config->nq && (i >= config->start_idx_q) && (i < config->end_idx_q) )
{
pomegadot = ( config->use_omegadot ? &in_xdot[i] : config->omegadot_zero );
omegadot_temp[0] = pomegadot[0]*6; omegadot_temp[1] = pomegadot[1]*6; omegadot_temp[2] = pomegadot[2]*6;
QuaternionIntegralEulerChange( (final_time-initial_time)/6, &in_xdot[i], omegadot_temp, &xd[i], q );
QuaternionProd(q, q, phi);
QuaternionProd(&xd_temp[i], phi, &xd_temp[i]);
QuaternionProd(phi, q, phi);
QuaternionProd(&xd[i], phi, &out_x[i]);
i += 4;
}
else
{
xd_temp[i] += 2*in_xdot[i];
out_x[i] = xd[i] + 0.5*(final_time-initial_time)*in_xdot[i];
i++;
}
}
if( config->idxout_time ) out_t[0] = initial_time + 0.5*(final_time-initial_time);
/* f3 */
if( !ssCallSystemWithTid(S,0,tid) ) return;
i = 0;
while( i<config->nstates )
{
if( config->nq && (i >= config->start_idx_q) && (i < config->end_idx_q) )
{
pomegadot = ( config->use_omegadot ? &in_xdot[i] : config->omegadot_zero );
omegadot_temp[0] = pomegadot[0]*6; omegadot_temp[1] = pomegadot[1]*6; omegadot_temp[2] = pomegadot[2]*6;
QuaternionIntegralEulerChange( (final_time-initial_time)/6, &in_xdot[i], omegadot_temp, &xd[i], q );
QuaternionProd(q, q, phi);
QuaternionProd(&xd_temp[i], phi, &xd_temp[i]);
QuaternionProd(phi, q, phi);
QuaternionProd(phi, phi, phi);
QuaternionProd(&xd[i], phi, &out_x[i]);
i += 4;
}
else
{
xd_temp[i] += 2*in_xdot[i];
out_x[i] = xd[i] + (final_time-initial_time)*in_xdot[i];
i++;
}
}
if( config->idxout_time ) out_t[0] = final_time;
/* f4 */
if( !ssCallSystemWithTid(S,0,tid) ) return;
i = 0;
while( i<config->nstates )
{
if( config->nq && (i >= config->start_idx_q) && (i < config->end_idx_q) )
{
pomegadot = ( config->use_omegadot ? &in_xdot[i] : config->omegadot_zero );
omegadot_temp[0] = pomegadot[0]*6; omegadot_temp[1] = pomegadot[1]*6; omegadot_temp[2] = pomegadot[2]*6;
QuaternionIntegralEulerChange( (final_time-initial_time)/6, &in_xdot[i], omegadot_temp, &xd[i], q );
QuaternionProd(&xd_temp[i], q, &xd_temp[i]);
QuaternionProd(&xd[i], &xd_temp[i], &xd[i]);
QuaternionNormalize(&xd[i]);
i += 4;
}
else
{
xd_temp[i] += in_xdot[i];
xd[i] += 1.0/6*(final_time-initial_time)*xd_temp[i];
i++;
}
}
}
}
else
{
if( !ssCallSystemWithTid(S,0,tid) ) return;
}
config->initial_time = final_time;
/* Update the outputs */
memcpy(out_x, xd, config->nstates*sizeof(real_T));
if( config->idxout_time ) out_t[0] = final_time;
}
