Exemplo n.º 1
0
static void mdlStart(SimStruct *S)
{
    ssSetRWorkValue(S,0,ssGetTStart(S));
    

   real_T t_pre= getTime();
   real_T t_pre0=t_pre;
   ssSetRWorkValue(S,1,t_pre);
   ssSetRWorkValue(S,2,t_pre0);
   ssSetRWorkValue(S,3,0.02);
}
Exemplo n.º 2
0
static void mdlOutputs(SimStruct *S, int_T tid)
{
   real_T             t_previousSimTime = ssGetRWorkValue(S,0);
   const real_T      *scaleFactor = mxGetPr(TIME_SCALE_FACTOR(S));
   time_T             t_SimTime = ssGetT(S);
   real_T             t_diff = 0.0;
   real_T             dt;
   real_T             t_current;
   real_T             t_0;
   real_T             t_previous;
   real_T             t_elapsed;
   
   /* Desired Delta time */
   dt = (t_SimTime - t_previousSimTime) * (scaleFactor[0]);
   /* Get clock time at the beginning of this step*/   
   t_previous = (real_T)clock()/CLOCKS_PER_SEC;
   t_0 = t_previous;

   /* Wait to reach the desired time */
   while (t_diff<dt){
        t_current = (real_T) clock()/CLOCKS_PER_SEC;
        /* Look for wrapup */
        if (t_current<t_previous){
            t_elapsed = t_previous - t_0;
            t_0 = (real_T) clock()/CLOCKS_PER_SEC - t_elapsed;
        }
        t_diff = t_current - t_0;
        t_previous = t_current;
   }
   
   /* Store current time to be used in next time step*/
   ssSetRWorkValue(S, 0, t_SimTime);
}
Exemplo n.º 3
0
static void mdlOutputs(SimStruct *S,int_T tid) {
	InputRealPtrsType uPtrs0 = ssGetInputPortRealSignalPtrs(S,0);
	InputRealPtrsType uPtrs1 = ssGetInputPortRealSignalPtrs(S,1);
	real_T prev = ssGetRWorkValue(S,0);
	bool dataPort = PARAM(2)[0];
	int_T i;
#ifndef MATLAB_MEX_FILE
	rosShmData_t *shm = (rosShmData_t *)ssGetPWorkValue(S,0);
	SEM *sem = (SEM *)ssGetPWorkValue(S,1);
#endif
	char_T *msg;
	unsigned int strlen = sizeof(char_T)*(PARAM_SIZE(1)+1);

	UNUSED_ARG(tid);	 /* not used in single tasking mode */

	if (U0(0) > 0.5 && U0(0) > prev) {
		msg = (char_T *)malloc(strlen);
		mxGetString(ssGetSFcnParam(S,1), msg, strlen);
#ifndef MATLAB_MEX_FILE
		if (dataPort) {
			for (i = 0; i < ssGetInputPortWidth(S,1); ++i) {
				asprintf(&msg, "%s %f", msg, U1(i));
			}
		}
		if (rt_sem_wait_if(sem) != 0) {
			memcpy(shm->msg.text, msg, MAX_LOG_MSG_SIZE);
			shm->msg.state = NEW_VALUE;
			rt_sem_signal(sem);
		}
#else
		switch ((int)PARAM(0)[0]) {
			case 1: printf("DEBUG"); break;
			case 2: printf("INFO"); break;
			case 3: printf("WARN"); break;
			case 4: printf("ERROR"); break;
			case 5: printf("FATAL"); break;
			default: printf("NONE"); break;
		}
		printf(": %s", msg);
		if (dataPort) {
			for (i = 0; i < ssGetInputPortWidth(S,1); ++i) {
				printf(" %f", U1(i));
			}
		}
		printf("\n");
#endif
		free(msg);
	}
	ssSetRWorkValue(S,0,U0(0));
}
/* ======================================================================== */
static void startHackrfTx(SimStruct *S, bool print_info)
/* ======================================================================== */
{
    double sample_rate = (1.0 / ssGetSampleTime(S, 0)) * ssGetInputPortDimensions(S, 0)[0];
    hackrf_device *device = startHackrf(S, sample_rate, GetParam(BANDWIDTH), print_info);
    ssSetPWorkValue(S, DEVICE, device);
    if (ssGetErrorStatus(S)) return;

    int i = 0; for (; i < NUM_PARAMS; i++) ssSetRWorkValue(S, i, NAN);
    mdlProcessParameters(S);

    sample_buffer_reset((SampleBuffer*) ssGetPWorkValue(S, SBUF));
    int ret = hackrf_start_tx(device, hackrf_tx_callback, S);
    Hackrf_assert(S, ret, "Failed to start RX streaming");
}
Exemplo n.º 5
0
static void mdlStart(SimStruct *S) {
#ifndef MATLAB_MEX_FILE
	int num;
	rosShmData_t *shm;
	SEM *sem;
	rosBlockInitResult_t res;
	unsigned int strlen = sizeof(char_T)*(PARAM_SIZE(1)+1);

	res = registerRosBlock(S, "rosout", LOGGER, 0);
	shm = res.shm;
	sem = res.sem;
	num = res.num;
	shm->msg.level = PARAM(0)[0]-1;

	ssSetIWorkValue(S,0,num);
	ssSetPWorkValue(S,0,(void *)shm);
	ssSetPWorkValue(S,1,(void *)sem);
#endif
	ssSetRWorkValue(S,0,0.0);
}
Exemplo n.º 6
0
/* Function: mdlOutputs =======================================================
 *
*/
static void mdlOutputs(SimStruct *S, int_T tid)
{
    const uint32_T   *set_BaudRate  = (const uint32_T*) ssGetInputPortSignal(S,0);
    const int16_T   *data_send  = (const int16_T*) ssGetInputPortSignal(S,1);
    int16_T        *data_out  = (int16_T *)ssGetOutputPortRealSignal(S,0);
    int16_T        *data_new_flag  = (int16_T *)ssGetOutputPortRealSignal(S,1);
    real_T        *HL_Status  = (real_T *)ssGetOutputPortRealSignal(S,2);
    int16_T        *realTime  = (int16_T *)ssGetOutputPortRealSignal(S,3);
    
   real_T             t_previousSimTime = ssGetRWorkValue(S,0);
   time_T            t_SimTime = ssGetT(S);
   real_T             t_diff = 0.0;
   real_T             dt;
   real_T             t_current=getTime();
   real_T             t_0=t_current;
   real_T             t_previous=t_current;
   real_T             t_elapsed;
   real_T        *y0  = (real_T *)ssGetOutputPortRealSignal(S,0);
   real_T       t_pre=ssGetRWorkValue(S,1);
   real_T       t_pre0=ssGetRWorkValue(S,2);
   real_T       t_term=ssGetRWorkValue(S,3);
   real_T       t_realTime=t_current-t_pre0;
   /* Desired Delta time */
   dt = (t_SimTime - t_previousSimTime) ;
   /* Get clock time at the beginning of this step*/   


   if(t_0-t_pre>(dt+0.001))
  {
	printf("Warning:It is slower than the real time::%f\r\n",t_0-t_pre);
 }
  t_pre= t_0;
   
   /* Store current time to be used in next time step*/
   


    /* %%%-SFUNWIZ_wrapper_Outputs_Changes_BEGIN --- EDIT HERE TO _END */
//static int FTDIopen=0;
if (openFtdi==0)
{
    openFtdi = open_ftdi(*set_BaudRate, "Quad_USB2", 5, 15);
}

if (openFtdi == 1)
{
    
    *data_new_flag = read_ftdi (data_out, HL_Status);
       while (t_realTime<t_term-dt/2){/*(t_diff<dt){*/
        t_current =  getTime();
        t_realTime=t_current-t_pre0;
        
        /* Look for wrapup */
        if (t_current<t_previous){
            t_elapsed = t_previous - t_0;
            t_0 = getTime() - t_elapsed;
        }
        t_diff = t_current - t_0;
        t_previous = t_current;
   }
    send_ftdi (data_send);
}
else
{
    *data_new_flag = -1;
}
     /* Wait to reach the desired time */
   while (t_realTime<t_term){/*(t_diff<dt){*/
        t_current =  getTime();
        t_realTime=t_current-t_pre0;
        
        /* Look for wrapup */
        if (t_current<t_previous){
            t_elapsed = t_previous - t_0;
            t_0 = getTime() - t_elapsed;
        }
        t_diff = t_current - t_0;
        t_previous = t_current;
   }
 ssSetRWorkValue(S, 0, t_SimTime);
 ssSetRWorkValue(S,1,t_pre);
 ssSetRWorkValue(S,3,t_realTime+dt);
 *realTime=t_0-t_pre0;
//    Quadlinux_Com_Outputs_wrapper(set_BaudRate, data_send, data_out, data_new_flag, HL_Status, bytes_sent);
}
Exemplo n.º 7
0
static void mdlStart(SimStruct *S)
{
    ssSetRWorkValue(S,0,ssGetTStart(S));
}
Exemplo n.º 8
0
 /* 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)
 {
   #ifndef MATLAB_MEX_FILE
       int_T nChannels,range,i,channel;
       uint_T base;
       volatile uint32_T *ioaddress;
       
       //
       //These part is omitted.
       //
       ssSetIWorkValue(S,BASE_ADDR_I_IND,base);
       nChannels=(int_T)mxGetN(CHANNEL_ARG);
       ssSetIWorkValue(S,CHANNELS_I_IND,nChannels);
       range=(int_T)mxGetPr(RANGE_ARG)[0];
       switch(range){
           case 1://+/-5V
               ssSetRWorkValue(S,GAIN_R_IND,10.0/4096.0);
               ssSetRWorkValue(S,OFFSET_R_IND,-5.0);
               break;
           case 2://+/-10V
               ssSetRWorkValue(S,GAIN_R_IND,20.0/4096.0);
               ssSetRWorkValue(S,OFFSET_R_IND,-10.0);
               break;
           case 3://0-10V
               ssSetRWorkValue(S,GAIN_R_IND,10.0/4096.0);
               ssSetRWorkValue(S,OFFSET_R_IND,0.0);
               break;
           default:
               sprintf(msg,"Range must between 1 to 3");
               ssSetErrorStatus(S,msg);
               return;
               
       }
       //write the channel number into corresponding register (ADCNTL0)
       for(i=0;i<nChannels;i++)
       {
           channel=(int_T)mxGetPr(CHANNEL_ARG)[i]-1;
           ioaddress=(void *)(base+0x200+(i*0x4));
           ioaddress[0]=channel;
           delay(16e-6);
       }
       
       //set the number of channels (ADCNTL1)
       ioaddress=(void *)(base+0x290);
       ioaddress[0]=nChannels-1;
       delay(16e-16);
       
       //set the frequency of A/D sampling (ADCNTL1)
       ioaddress=(void *)(base+0x280);
       ioaddress[0]=0xFF;
       delay(16e-6);
       
       //clear FIFO (ADCNTL7)
       ioaddress=(void *)(base+0x2E0);
       ioaddress[0]=0;
       delay(16e-6);
       
       //disallow A\D interrupt
       ioaddress=(void *)(base+0x2D0);
       ioaddress[0]=0;
       delay(16e-16);
       
   #endif
 }