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