Exemple #1
0
void sensor_fusion(void *arg)
{
	RTIME now;
	int i,sum, print,k, count;
        /*
         * Arguments: &task (NULL=self), start time, period 
         */
        rt_task_set_periodic(NULL, TM_NOW, 16666666);
	count =0;
	while (isRunning) {
		count++;
                rt_task_wait_period(NULL);
		rt_mutex_acquire(&mutex_acc,TM_INFINITE);
		rt_mutex_acquire(&mutex_gyro,TM_INFINITE);
		k=gyroIndex;
		sum=0; print = 0;
   		for (i=0; i<5; i++){
	 		sum = sum + gyro_sample[k];
			k--;
		}

		print = ((sum/5) + acc_sample)/2;

		rt_mutex_release(&mutex_gyro);
		rt_mutex_release(&mutex_acc);
		
		printlog[logIndex] = print;
		logIndex++;

		if(count == 50)
			isRunning = 0;
	}
}
Exemple #2
0
void addPong(){
    if (nPongs < MAX_PONGS){
        pongs[nPongs].leftCorner.point_x = ((resolution_w-resolution_x)/2 - (pongLength/2));
        pongs[nPongs].leftCorner.point_y = resolution_y;
        oppoentPointLocationX = ((resolution_w-resolution_x)/2 - (pongLength/2));

        rt_mutex_acquire(&txMutex[E_ADD], TM_INFINITE);
	memset(pongData[0], 0, sizeof(pongData[0]));
        sprintf(pongData[0], "pong;%d;",pongs[nPongs].leftCorner.point_x);
        sprintf(pongData[0] + strlen(pongData[0]), 
		"%d;%d;%d;", pongs[nPongs].leftCorner.point_y,pongLength,pongs[nPongs].height);
	padString(pongData[0]);
        rt_mutex_release(&txMutex[E_ADD]);
        
        pongs[nPongs].length = pongLength;
        nPongs++;
        pongs[nPongs].leftCorner.point_x =((resolution_w-resolution_x)/2 - (pongLength/2));
        pongs[nPongs].leftCorner.point_y = DEFAULT_HEIGHT;
        locationPointOfX = ((resolution_w-resolution_x)/2 - (pongLength/2)); 

	rt_mutex_acquire(&txMutex[E_ADD], TM_INFINITE);
	memset(pongData[1], 0, sizeof(pongData[1]));
        sprintf(pongData[1], "pong;%d;",pongs[nPongs].leftCorner.point_x);
        sprintf(pongData[1] + strlen(pongData[1]), 
		"%d;%d;%d;", pongs[nPongs].leftCorner.point_y,pongLength,pongs[nPongs].height);
	padString(pongData[1]);
	rt_mutex_release(&txMutex[E_ADD]);
        nPongs++;
    }
}
Exemple #3
0
/*TODO: If your device need more time to initialize I2C bus or waiting memory write, you can use I2C_AcknowledgePolling avoid I2C bus lose.*/
Status I2C_AcknowledgePolling(I2C_TypeDef* I2Cx ,uint8_t Addr)
{
    uint32_t timeout = 0xFFFF, ret;
    uint16_t tmp;
	ret = rt_mutex_take(i2c_mux, RT_WAITING_FOREVER );
	
	if( ret == RT_EOK )
	{
	    do{
	        if( timeout-- <= 0 )
	        {
	            I2C_ClearFlag(I2Cx,I2C_FLAG_AF);
	            I2Cx->CR1 |= CR1_STOP_Set;
				rt_mutex_release(i2c_mux);
	            return Error;
	        }
	
	        I2Cx->CR1 |= CR1_START_Set;
	        tmp = I2Cx->SR1;//²M°£SB¦ì
	        I2Cx->DR = Addr;
	        
	    }while((I2Cx->SR1&0x0002) != 0x0002);
	  
	    I2C_ClearFlag(I2Cx,I2C_FLAG_AF);
	    I2Cx->CR1 |= CR1_STOP_Set;
	    while ((I2Cx->CR1&0x200) == 0x200);
		rt_kprintf( "AcknowledgePolling OK\n");
		rt_mutex_release(i2c_mux);
		return Success; 
	}
	else
		return Error;
} 
Exemple #4
0
static void thread1_entry(void* parameter)
{
    rt_err_t result;

    
    result = rt_mutex_take(mutex, RT_WAITING_FOREVER);
    result = rt_mutex_take(mutex, RT_WAITING_FOREVER);
    rt_kprintf("thread1: got mutex\n");

    if (result != RT_EOK)
    {
        return;
    }
        
    for(t1_count = 0; t1_count < 5;t1_count ++)
    {
        rt_kprintf("thread1:count: %d\n", t1_count);
    }
    if (t2->current_priority != t1->current_priority)
    {
        rt_kprintf("thread1: released mutex\n");
        rt_mutex_release(mutex);
        rt_mutex_release(mutex);
    }
        
}
Exemple #5
0
static int dfs_jffs2_close(struct dfs_fd* file)
{
	int result;
	cyg_file * jffs2_file;
	
	RT_ASSERT(file->data != NULL);
	jffs2_file = (cyg_file *)(file->data);
	
	if (file->flags & DFS_O_DIRECTORY) /* operations about dir */
	{
		rt_mutex_take(&jffs2_lock, RT_WAITING_FOREVER);
		result = jffs2_dir_colse(jffs2_file);
		rt_mutex_release(&jffs2_lock);
		if (result)
			return jffs2_result_to_dfs(result);	

		rt_free(jffs2_file);
		return 0;
	}
	/* regular file operations */
	rt_mutex_take(&jffs2_lock, RT_WAITING_FOREVER);
	result = jffs2_file_colse(jffs2_file);
	rt_mutex_release(&jffs2_lock);
	if (result)
		return jffs2_result_to_dfs(result);	
	
	/* release memory */
	rt_free(jffs2_file);
	return 0;
}
void computationTask(long arg)
{
	usleep(1);
	while(1){
		int i = 0;
		rt_mutex_acquire(&mutex_donnee,TM_INFINITE);	

		while(tabDonnePris==1){
			rt_mutex_release(&mutex_donnee);
			rt_sem_p(&sem_donnee,TM_INFINITE);
			rt_mutex_acquire(&mutex_donnee,TM_INFINITE);
		}
		tabDonnePris = 1;
		
		rt_mutex_acquire(&mutex_obs,TM_INFINITE);	
		while(tabObstaclePris==1){
		  rt_mutex_release(&mutex_obs);
		  rt_sem_p(&sem_obs,TM_INFINITE);
		  rt_mutex_acquire(&mutex_obs,TM_INFINITE);
		}
		tabObstaclePris = 1;
		printf("***************** COMPUTATION TASK ******************\n");
		
		for(i = 0; i<SENSOR_SIZE; i++){
			//If the obstacle is farther than 7meters
			if(sensorArray[i] > 7){
				
				obstacle* newObstacle = malloc(sizeof(obstacle));
				newObstacle->distance = sensorArray[i];
				
				//Add the new obstacle at the end of the list
				newObstacle->nxt = NULL;
				
				if(obstacleList == NULL) obstacleList = newObstacle;
				else{
					obstacle* temp = obstacleList;
					if(temp->distance==0)temp->distance=newObstacle->distance;
					else{
						while(temp->nxt != NULL)temp = temp->nxt;
						temp->nxt = newObstacle;
					}
				}
			}			
		}
		tabDonnePris = 0;
		tabObstaclePris = 0;
		
		rt_mutex_release(&mutex_obs);
		rt_mutex_release(&mutex_donnee);
		rt_sem_v(&sem_obs);		
		

		usleep(200000);
		printf("fin compute\n");
	}
}
Exemple #7
0
void batterie(void *arg) {
    rt_printf("tconnect : Attente du sémarphore semConnectedRobotbatterie\n");
    rt_sem_p(&semConnectedRobot, TM_INFINITE);
    
    int status=1;
    int niveau_batterie=0;
    DBattery *bat= d_new_battery();
    DMessage *message;
    

    rt_printf("tbatterie : Debut de l'éxecution de periodique à 250ms\n");
    rt_task_set_periodic(NULL, TM_NOW, 250000000);
    
     while (1) {
        rt_task_wait_period(NULL);
        rt_printf("tbatterie : Activation périodique\n");
         
        rt_mutex_acquire(&mutexEtat, TM_INFINITE);
        status = etatCommMoniteur;
        rt_mutex_release(&mutexEtat);
        
         if (status == STATUS_OK) {
             rt_mutex_acquire(&mutexRobot, TM_INFINITE);
             status=d_robot_get_vbat(robot, &niveau_batterie);
             rt_mutex_release(&mutexRobot);
             
             rt_mutex_acquire(&mutexEtat, TM_INFINITE);
             etatCommRobot=status;
             rt_mutex_release(&mutexEtat);
             
             rt_printf("Niveau de la batterie : %d\n", niveau_batterie);
             rt_printf("Status : %d\n", status);
             
             if(status == STATUS_OK) {
                 message=d_new_message();
                 d_battery_set_level(bat,niveau_batterie);
                 d_message_put_battery_level(message, bat);
                 rt_mutex_acquire(&mutexCom, TM_INFINITE);
                 if (write_in_queue(&queueMsgGUI, message, sizeof (DMessage)) < 0) {
                        message->free(message);
                }
                 rt_mutex_release(&mutexCom);
                 
             }
             
 
         }
     }
    
}
Exemple #8
0
void deplacer(void *arg) {
    int status;

    rt_printf("tmove : Attente du sémaphore semDeplacer\n");
    rt_sem_p(&semDeplacer, TM_INFINITE);
    rt_printf("tmove : Debut de l'éxecution periodique à 200ms\n");
    rt_task_set_periodic(NULL, TM_NOW, 200000000);

    while (1) {
        while (cptCommErr < MAX_ECHECS) {
            /* Attente de l'activation périodique */
            rt_task_wait_period(NULL);
            //rt_printf("tmove : Activation périodique\n");

            rt_mutex_acquire(&mutexEtat, TM_INFINITE);
            status = etatCommRobot;
            rt_mutex_release(&mutexEtat);
            if (status == STATUS_OK) {
                rt_mutex_acquire(&mutexMove, TM_INFINITE);
                switch (move->get_direction(move)) {
                case DIRECTION_FORWARD:
                    status = robot->set_motors(robot, MOTEUR_ARRIERE_LENT, MOTEUR_ARRIERE_LENT);
                    break;
                case DIRECTION_LEFT:
                    status = robot->set_motors(robot, MOTEUR_ARRIERE_LENT, MOTEUR_AVANT_LENT);
                    break;
                case DIRECTION_RIGHT:
                    status = robot->set_motors(robot, MOTEUR_AVANT_LENT, MOTEUR_ARRIERE_LENT);
                    break;
                case DIRECTION_STOP:
                    status = robot->set_motors(robot, MOTEUR_STOP, MOTEUR_STOP);
                    break;
                case DIRECTION_STRAIGHT:
                    status = robot->set_motors(robot, MOTEUR_AVANT_LENT, MOTEUR_AVANT_LENT);
                    break;
                }
                rt_mutex_release(&mutexMove);

                if (status == STATUS_OK) {
                    cptCommErr = 0;
                } else {
                    cptCommErr++;
                    rt_printf("tmove : Erreur de communication avec le robot (%d)\n", cptCommErr);
                }
            }
        }
        comm_err_handler(status);
        rt_sem_p(&semDeplacer, TM_INFINITE);
    }
}
/**
 * Timer Task
 */
void timerloop_task_proc(void *arg)
{
	int ret = 0;

	getElapsedTime();
	last_timeout_set = 0;
	last_occured_alarm = last_time_read;

	/* trigger first alarm */
	SetAlarm(callback_od, 0, init_callback, 0, 0);
	RTIME current_time;
	RTIME real_alarm;
	do{

		rt_mutex_acquire(&condition_mutex, TM_INFINITE);
		if(last_timeout_set == TIMEVAL_MAX)
		{
			ret = rt_cond_wait(
				&timer_set,
				&condition_mutex,
				TM_INFINITE
				);		/* Then sleep until next message*/
			rt_mutex_release(&condition_mutex);
		}else{
			current_time = rt_timer_read();
			real_alarm = last_time_read + last_timeout_set;
			ret = rt_cond_wait( /* sleep until next deadline */
				&timer_set,
				&condition_mutex,
				(real_alarm - current_time)); /* else alarm consider expired */
			if(ret == -ETIMEDOUT){
				last_occured_alarm = real_alarm;
				rt_mutex_release(&condition_mutex);
				EnterMutex();
				TimeDispatch();
				LeaveMutex();
			}else{
				rt_mutex_release(&condition_mutex);
			}
		}
	}while ((ret == 0 || ret == -EINTR || ret == -ETIMEDOUT) && !stop_timer);

	if(exitall){
		EnterMutex();
		exitall(callback_od, 0);
		LeaveMutex();
	}
}
Exemple #10
0
void opponentMovement(char strOpponent){
    int err;

    if(strOpponent == 'R'){
          if (!(err=illegal_move_check(oppoentPointLocationX, standardPongSpeed))){
              oppoentPointLocationX = oppoentPointLocationX + standardPongSpeed;
              pongs[opponent].leftCorner.point_x = oppoentPointLocationX;
              pongs[opponent].leftCorner.point_y = resolution_y;
          }
	   else {
	        oppoentPointLocationX = oppoentPointLocationX - (err * standardPongSpeed);
	        pongs[opponent].leftCorner.point_x = oppoentPointLocationX;
          }
    }

    else if (strOpponent == 'L'){
        if (!(err=illegal_move_check(oppoentPointLocationX, standardPongSpeed))){
              oppoentPointLocationX = oppoentPointLocationX - standardPongSpeed;
              pongs[opponent].leftCorner.point_x = oppoentPointLocationX;
              pongs[opponent].leftCorner.point_y = resolution_y;
          }
	else {

	      oppoentPointLocationX = oppoentPointLocationX + (-err * standardPongSpeed);
	      pongs[opponent].leftCorner.point_x = oppoentPointLocationX;
	  }
    }

     rt_mutex_acquire(&txMutex[E_MOVE], TM_INFINITE);
     memset(moveData[1], 0, sizeof(moveData[1]));
     sprintf(moveData[1], "player2;%d;%d;%d;%d;",
         pongs[opponent].leftCorner.point_x, pongs[opponent].leftCorner.point_y,pongLength, pongs[opponent].height);
     padString(moveData[1]);
     rt_mutex_release(&txMutex[E_MOVE]);
}
Exemple #11
0
void move(char str){
     
    int err;
    if(str == 'R'){
          if ((err=illegal_move_check(locationPointOfX, standardPongSpeed)) == 0){
              locationPointOfX = locationPointOfX + standardPongSpeed;
              pongs[player].leftCorner.point_x = locationPointOfX;
              pongs[player].leftCorner.point_y = DEFAULT_HEIGHT;
	  }
          else {
	        locationPointOfX = locationPointOfX - (err * standardPongSpeed);
	        pongs[player].leftCorner.point_x = locationPointOfX;
          }
    }

    else if (str == 'L'){
        if ((err=illegal_move_check(locationPointOfX, standardPongSpeed)) == 0){
              locationPointOfX = locationPointOfX - standardPongSpeed;
              pongs[player].leftCorner.point_x = locationPointOfX;
              pongs[player].leftCorner.point_y = DEFAULT_HEIGHT;
          }
	else {
	      locationPointOfX = locationPointOfX + (-err * standardPongSpeed);
	      pongs[player].leftCorner.point_x = locationPointOfX;
	  }
    }

       rt_mutex_acquire(&txMutex[E_MOVE], TM_INFINITE);
       memset(moveData[0], 0, sizeof(moveData[0]));
       sprintf(moveData[0], "player1;%d;%d;%d;%d;",
               pongs[player].leftCorner.point_x, pongs[player].leftCorner.point_y,pongLength, 
               pongs[player].height);
       padString(moveData[0]);
       rt_mutex_release(&txMutex[E_MOVE]);
}
Exemple #12
0
void addBall(){

    if(!ball.created){ //singleton pattern
        ball.centre.point_x = (resolution_w-resolution_x)/2;
        ball.centre.point_y = (resolution_h-resolution_y)/2;
        defaultBalllocationX = (resolution_w-resolution_x)/2;
        defaultBalllocationY =  (resolution_h-resolution_y)/2;

	currentBallOfX = ball.centre.point_x;
        currentBallOfY = ball.centre.point_y;

        // pPanel notify for created pPanel
        balldirectionX = DEFAULT_BALLSPEEDX;
        balldirectionY = DEFAULT_BALLSPEEDY;
        
	rt_mutex_acquire(&txMutex[E_GAME], TM_INFINITE);
	memset(gameData, 0, sizeof(gameData));
	sprintf(gameData, "score;%d;%d;",player_score,opponent_score);
	padString(gameData);
	rt_mutex_release(&txMutex[E_GAME]);
      	
	 memset(ballData, 0, sizeof(ballData));
         sprintf(ballData, "ball;%d;%d;%d;",defaultBalllocationX, defaultBalllocationY, ball.radius);
         padString(ballData);
	 ball.created = true;
    }
}
Exemple #13
0
/** Transmit buffer over socket (non blocking).
 * @param[in] port        = port context struct
 * @param[in] idx = index in tx buffer array
 * @return socket send result
 */
int ecx_outframe_red(ecx_portt *port, int idx)
{
   ec_comt *datagramP;
   ec_etherheadert *ehp;
   int rval;
   
   ehp = (ec_etherheadert *)&(port->txbuf[idx]);
   /* rewrite MAC source address 1 to primary */
   ehp->sa1 = htons(priMAC[1]);
   /* transmit over primary socket*/
   rval = ecx_outframe(port, idx, 0);
   if (port->redstate != ECT_RED_NONE)
   {   
      //pthread_mutex_lock( &(port->tx_mutex) );
      rt_mutex_acquire(&port->tx_mutex, TM_INFINITE);
      ehp = (ec_etherheadert *)&(port->txbuf2);
      /* use dummy frame for secondary socket transmit (BRD) */
      datagramP = (ec_comt*)&(port->txbuf2[ETH_HEADERSIZE]);
      /* write index to frame */
      datagramP->index = idx;
      /* rewrite MAC source address 1 to secondary */
      ehp->sa1 = htons(secMAC[1]);
      /* transmit over secondary socket */
      SEND(port->redport->sockhandle, &(port->txbuf2), port->txbuflength2 , 0);
      //pthread_mutex_unlock( &(port->tx_mutex) );
      rt_mutex_release(&port->tx_mutex);
      port->redport->rxbufstat[idx] = EC_BUF_TX;
   }   
   
   return rval;
}
Exemple #14
0
void connecter(void *arg) {
    DMessage *message = d_new_message();
    int status;

    rt_printf("tconnect : Debut de l'exécution de tconnect\n");

    while (1) {
        rt_printf("tconnect : Attente du sémaphore semConnecterRobot\n");
        rt_sem_p(&semConnecterRobot, TM_INFINITE);
        rt_printf("tconnect : Ouverture de la communication avec le robot\n");
        status = robot->open_device(robot);
        rt_mutex_acquire(&mutexEtat, TM_INFINITE);
        etatCommRobot = status;

        if (status == STATUS_OK) {
            cptCommErr = 0;
            //robot->start_insecurely(robot);
            status = robot->start(robot);
            if (status == STATUS_OK) { /* Demarrage du robot */
                rt_printf("tconnect : Robot démarrer\n");
                rt_sem_v(&semDeplacer);
                rt_sem_v(&semRechargerWatchdog);
                rt_sem_v(&semVerifierBatterie);
            } else { /* Impossible de demarrer le robot, tentative de reinitialisation */
                robot->stop(robot);
                robot->close_com(robot);
            }
        }
        
        rt_mutex_release(&mutexEtat);
        message->put_state(message, status);
        serveur->send(serveur, message);
    }
}
Exemple #15
0
static rt_bool_t ptgui_dc_hw_fini(struct ptgui_dc *dc)
{
	struct ptgui_dc_hw *hw = (struct ptgui_dc_hw *)dc;
	if (dc == RT_NULL || hw->parent.type != PTGUI_DC_HW) {
		return RT_FALSE;
	}

#ifdef __WIN32__
#ifdef PTGUI_USING_MOUSE_CURSOR
	rt_mutex_release(&cursor_mutex);
	/* show cursor */
	ptgui_mouse_show_cursor();
	rt_kprintf("show cursor\n");
#endif
	/* update screen */
	hw->device->screen_update(&(hw->owner->extent));
#else
#ifdef PTGUI_USING_MOUSE_CURSOR
	/* show cursor */
	ptgui_mouse_show_cursor();
#endif

	/* update screen */
	hw->device->screen_update(&(hw->owner->extent));
#endif

	return RT_TRUE;
}
Exemple #16
0
void accelerometer(void *arg)
{
	RTIME now;
	int acceptable;
        /*
         * Arguments: &task (NULL=self), start time, period 
         */
        rt_task_set_periodic(NULL, TM_NOW, 33333333);

	while (isRunning) {
		
                rt_task_wait_period(NULL);
		acceptable = 0;
		//acceptable = rand() % (800 - 0 + 1) + 0; // acceleration between 0-50 
		acceptable = rand_lim(20);
		rt_mutex_acquire(&mutex_acc,TM_INFINITE);
		
		if(acceptable > acc_sample)
			acc_sample = acc_sample + 1;

		else if(acceptable < acc_sample)
			acc_sample = acc_sample - 1;
		
		rt_mutex_release(&mutex_acc);
	}
}
Exemple #17
0
static int dfs_jffs2_write(struct dfs_fd* file, 
                    const void* buf, 
					rt_size_t len)
{
	cyg_file * jffs2_file;
	struct CYG_UIO_TAG uio_s;
	struct CYG_IOVEC_TAG iovec;
	int char_write;
	int result;
	
	RT_ASSERT(file->data != NULL);
	jffs2_file = (cyg_file *)(file->data);	
    uio_s.uio_iov = &iovec;
    uio_s.uio_iov->iov_base = (void *)buf;
    uio_s.uio_iov->iov_len = len;
    uio_s.uio_iovcnt = 1; //must be 1
    //uio_s.uio_offset //not used...
    uio_s.uio_resid = uio_s.uio_iov->iov_len; //seem no use in jffs2;
	
	char_write = jffs2_file->f_offset;
	rt_mutex_take(&jffs2_lock, RT_WAITING_FOREVER);
	result = jffs2_file_write(jffs2_file, &uio_s);
	rt_mutex_release(&jffs2_lock);
	if (result)
		return jffs2_result_to_dfs(result);
	
	/* update position */
	file->pos = jffs2_file->f_offset;	
	char_write = jffs2_file->f_offset - char_write;
	return char_write;
}
Exemple #18
0
void lcd1602_DispProcessing(uint8_t percent)
{
    uint8_t i, BlockNum = 0;
    uint8_t abyString[17];

    rt_err_t rtResult;
    //while (1)
    {
        BlockNum = ((uint32_t)percent * 16) / 99;
        for (i = 0; i < 16; i++)
        {
            if (i < BlockNum)
            {
                abyString [i] = '#';
            }
            else
            {
                abyString [i] = ' ';
            }
        }
        abyString[16] = 0;
        rtResult = rt_mutex_take(LcdMutex, RT_WAITING_FOREVER);
        if (rtResult == RT_EOK)
        {
            LCD_PutStr(abyString, 16, 16);
            rt_mutex_release(LcdMutex);
        }
    }
}
rt_size_t rt_i2c_transfer(struct rt_i2c_bus_device *bus,
                          struct rt_i2c_msg         msgs[],
                          rt_uint32_t               num)
{
    rt_size_t ret;

    if (bus->ops->master_xfer)
    {
#ifdef RT_I2C_DEBUG
        for (ret = 0; ret < num; ret++)
        {
            i2c_dbg("msgs[%d] %c, addr=0x%02x, len=%d%s\n", ret,
                    (msgs[ret].flags & RT_I2C_RD) ? 'R' : 'W',
                    msgs[ret].addr, msgs[ret].len);
        }
#endif

        rt_mutex_take(&bus->lock, RT_WAITING_FOREVER);
        ret = bus->ops->master_xfer(bus, msgs, num);
        rt_mutex_release(&bus->lock);

        return ret;
    }
    else
    {
        i2c_dbg("I2C bus operation not supported\n");

        return 0;
    }
}
Exemple #20
0
/** Get new frame identifier index and allocate corresponding rx buffer.
 * @param[in] port        = port context struct
 * @return new index.
 */
int ecx_getindex(ecx_portt *port)
{
   int idx;
   int cnt;

   rt_mutex_acquire(&port->getindex_mutex, TM_INFINITE);

   idx = port->lastidx + 1;
   /* index can't be larger than buffer array */
   if (idx >= EC_MAXBUF) 
   {
      idx = 0;
   }
   cnt = 0;
   /* try to find unused index */
   while ((port->rxbufstat[idx] != EC_BUF_EMPTY) && (cnt < EC_MAXBUF))
   {
      idx++;
      cnt++;
      if (idx >= EC_MAXBUF) 
      {
         idx = 0;
      }
   }
   port->rxbufstat[idx] = EC_BUF_ALLOC;
   if (port->redstate != ECT_RED_NONE)
      port->redport->rxbufstat[idx] = EC_BUF_ALLOC;
   port->lastidx = idx;

   rt_mutex_release(&port->getindex_mutex);
   
   return idx;
}
Exemple #21
0
int pthread_mutex_unlock(pthread_mutex_t *mutex)
{
	rt_err_t result;

	if (!mutex) return EINVAL;
	if (mutex->attr == -1)
	{
		/* init mutex */
		pthread_mutex_init(mutex, RT_NULL);
	}

	if (mutex->lock.owner != rt_thread_self())
	{
		int mtype;
		mtype = mutex->attr & MUTEXATTR_TYPE_MASK;

		/* error check, return EPERM */
		if (mtype == PTHREAD_MUTEX_ERRORCHECK) return EPERM;

		/* no thread waiting on this mutex */
		if (mutex->lock.owner == RT_NULL) return 0;
	}

	result = rt_mutex_release(&(mutex->lock));
	if (result == RT_EOK) return 0;
	
	return EINVAL;
}
Exemple #22
0
rt_err_t rt_spi_configure(struct rt_spi_device* device, struct rt_spi_configuration* cfg)
{
	rt_err_t result;

	RT_ASSERT(device != RT_NULL);

	/* set configuration */
	device->config.data_width = cfg->data_width;
	device->config.mode = cfg->mode & RT_SPI_MODE_MASK ;
	device->config.max_hz = cfg->max_hz ;

	if (device->bus != RT_NULL)
	{
		result = rt_mutex_take(&(device->bus->lock), RT_WAITING_FOREVER);
		if (result == RT_EOK)
		{
			if (device->bus->owner == device)
			{
				device->bus->ops->configure(device, &device->config);
			}

			/* release lock */
			rt_mutex_release(&(device->bus->lock));
		}
	}

	return RT_EOK;
}
rt_size_t rt_spi_transfer(struct rt_spi_device *device,
                          const void           *send_buf,
                          void                 *recv_buf,
                          rt_size_t             length)
{
    rt_err_t result;
    struct rt_spi_message message;

    RT_ASSERT(device != RT_NULL);
    RT_ASSERT(device->bus != RT_NULL);

    result = rt_mutex_take(&(device->bus->lock), RT_WAITING_FOREVER);
    if (result == RT_EOK)
    {
        if (device->bus->owner != device)
        {
            /* not the same owner as current, re-configure SPI bus */
            result = device->bus->ops->configure(device, &device->config);
            if (result == RT_EOK)
            {
                /* set SPI bus owner */
                device->bus->owner = device;
            }
            else
            {
                /* configure SPI bus failed */
                rt_set_errno(-RT_EIO);
                result = 0;
                goto __exit;
            }
        }

        /* initial message */
        message.send_buf   = send_buf;
        message.recv_buf   = recv_buf;
        message.length     = length;
        message.cs_take    = 1;
        message.cs_release = 1;
        message.next       = RT_NULL;

        /* transfer message */
        result = device->bus->ops->xfer(device, &message);
        if (result == 0)
        {
            rt_set_errno(-RT_EIO);
            goto __exit;
        }
    }
    else
    {
        rt_set_errno(-RT_EIO);

        return 0;
    }

__exit:
    rt_mutex_release(&(device->bus->lock));

    return result;
}
Exemple #24
0
static rt_err_t nand_mtd_write (
		struct rt_mtd_nand_device * dev,
		rt_off_t page,
		const rt_uint8_t * data, rt_uint32_t data_len,//will be 2048 always!
		const rt_uint8_t * spare, rt_uint32_t spare_len)
{	
	rt_err_t result=RT_EOK;
	int i;
	rt_mutex_take(&nand, RT_WAITING_FOREVER);
	if (data != RT_NULL && data_len != 0)
	{	
	    // get offset of sst39vf's write position
		rt_uint32_t page_offs = page*512;
		for(i=0;i<data_len/2;i++)
			Mem_Wr(*(rt_uint16_t *)((rt_uint16_t *)data+i),page_offs+i);
	}

	
	if (spare != RT_NULL && spare_len != 0)
	{
	    // get spare offset of sst39vf's write position
		rt_uint32_t spare_offs = page*16 + NOR_SPARE_BLOCK;
	    	for(i=0;i<spare_len/2;i++)
		    Mem_Wr(*(rt_uint16_t *)((rt_uint16_t *)spare+i),spare_offs+i);

	}
	
	rt_mutex_release(&nand);

	return result;
}
/**
 * Set timer for the next wakeup
 * @param value
 */
void setTimer(TIMEVAL value)
{
	rt_mutex_acquire(&condition_mutex, TM_INFINITE);
	last_timeout_set = value;
	rt_mutex_release(&condition_mutex);
	rt_cond_signal(&timer_set);
}
Exemple #26
0
void semWait2(){
	rt_task_sleep(1000000);
	rt_mutex_acquire(&b, 0);
	rt_printf("High pri task: lock b\n");
	busy_wait_ms(1);
	rt_mutex_acquire(&a, 0);
	rt_printf("High pri task: lock a\n");
	busy_wait_ms(2);
	rt_printf("High pri task: release a\n");
	rt_mutex_release(&a);
	rt_printf("High pri task: release b \n");
	rt_mutex_release(&b);
	busy_wait_ms(1);
	
	
}
Exemple #27
0
/* private functions ======================================================== */
static void task_soft_routine(void * cookie) {
    unsigned long n = 0; /* how many wheel rotations since init? */
    RTIME time_init = 0; /* when did we start? */
    RTIME time_prev = 0; /* when was the previous rotation? */
    RTIME time_curr = 0; /* when was the current rotation? */

    /* we start now! (first wheel rotation) */
    rt_queue_read(&queue, &time_init, sizeof time_curr, TM_INFINITE);
 
    /* previous rotation is now! (init value) */ 
    time_prev = time_init;

    while (1) {
        /* extract the current rotation timestamp from message queue */
        rt_queue_read(&queue, &time_curr, sizeof time_curr, TM_INFINITE);

        /* 
           Let's compute instant speed. We want an Hz value, we've got previous 
           rotation timestamp and current rotation timestamp, substracting them
           give us the time elapsed since previous rotation in nanoseconds. 
           Hz = 1 / s, so Hz = 10^9 / ns, our final value is 10^9 / dt
        */
        rt_mutex_acquire(&mutex_instant, TM_INFINITE);
        instant = 1e9 / (time_curr - time_prev);
        rt_mutex_release(&mutex_instant);

	/*
           Let's compute average speed. We want an Hz value, we've got init
           rotation timestamp and current rotation timestamp, substracting
           them gives us the time elapsed since init rotation in nanoseconds.
           ++n is the number of wheel rotations since init.
           Hz = 1 / s, so Hz = 10^9 / ns, our final value is 10^9 * dx / dt
        */
        rt_mutex_acquire(&mutex_average, TM_INFINITE);
        average = 1e9 * ++n / (time_curr - time_init);
        rt_mutex_release(&mutex_average);

        /* dump current timestamp to file */
        fprintf(ostream, "%llu\n", time_curr);

        /* our job is done, we are now the previous rotation */ 
        time_prev = time_curr;
    }

    (void) cookie;
}
Exemple #28
0
void gyroscope(void *arg)
{
	RTIME now;
	int randG, i, prev;
	gyroIndex = 0;

        /*
         * Arguments: &task (NULL=self), start time, period 
         */
        rt_task_set_periodic(NULL, TM_NOW, 3333333);

	while (isRunning) {
		for(i=0; i<BUFFERSIZE; i++){
                	rt_task_wait_period(NULL);
			//randG = rand() % (361 - 0 + 1) + 0; // angle between 0-361
			randG = rand_lim(360);
			
			rt_mutex_acquire(&mutex_gyro,TM_INFINITE);
			//printf("Random: %d\n", randG);
			prev = gyroIndex;
			gyroIndex++;
			
			if(gyro_sample[prev] >= 360 && randG > gyro_sample[prev])
				gyro_sample[gyroIndex] = 1;

			else if(gyro_sample[prev] <= 1 && randG < gyro_sample[prev])
				gyro_sample[gyroIndex] = 360;

			else if(randG > gyro_sample[prev])
				gyro_sample[gyroIndex] = gyro_sample[prev] + 5;

			else if(randG < gyro_sample[prev])
				gyro_sample[gyroIndex] = gyro_sample[prev] - 5;

			else 
				gyro_sample[gyroIndex] = gyro_sample[prev];
			
			rt_mutex_release(&mutex_gyro);
			
		}
		
		rt_mutex_acquire(&mutex_gyro,TM_INFINITE);
		gyroIndex = 0;
		rt_mutex_release(&mutex_gyro);
	}
}
Exemple #29
0
void semWait1(){
	
	rt_mutex_acquire(&a, 0);
	rt_printf("Low pri task: lock a\n");
	//rt_timer_spin(3000000);
	busy_wait_ms(3);
	rt_mutex_acquire(&b, 0);
	rt_printf("Low pri task: lock b\n");
	busy_wait_ms(3);
	
	rt_printf("Low pri task: unlock b \n");
	rt_mutex_release(&b);
	rt_printf("Low pri task: unlock a \n");
	rt_mutex_release(&a);
	
	rt_printf("Low pri task: stop \n");
}
Exemple #30
0
void bouncingCheck (int x,int y, int radius, int directionX,int directionY){
        if(x + directionX < resolution_x){
            changeBallDirectionHorizontal();
        }
        else if(x +directionX + radius > resolution_w){
            changeBallDirectionHorizontal();
        }
        /* check if we hit the topmost pong */
        if (y+ directionY + (radius/4) <pongs[opponent].leftCorner.point_y + pongs[opponent].height ){
             if(x+directionX + radius > pongs[opponent].leftCorner.point_x &&
                x+directionX + radius < pongs[opponent].leftCorner.point_x+pongLength+radius){
                    bounceOpponent = true;
                    changeBallDirectionVertical();
                }
             else{ // in the vicinity to score a point 
                 player_score++;
		 rt_mutex_acquire(&txMutex[E_GAME],TM_INFINITE);
		 memset(gameData, 0, sizeof(gameData));
		 sprintf(gameData, "score;%d;%d;",player_score,opponent_score);
		 padString(gameData);
		 rt_mutex_release(&txMutex[E_GAME]);                 
                 reset = true;
             }
            
         }
	
        /* check if we hit the bottom-most pong */  
        if (y+ directionY + (radius/4) +pongs[player].height >pongs[player].leftCorner.point_y){
	     if(x+directionX + radius > pongs[player].leftCorner.point_x &&
                x+directionX + radius < pongs[player].leftCorner.point_x+pongLength+radius){
                    bounceplayer = true; 
                    changeBallDirectionVertical();
                }
	     else{                  opponent_score++;
                 rt_mutex_acquire(&txMutex[E_GAME],TM_INFINITE);
 		 memset(gameData, 0, sizeof(gameData));
		 sprintf(gameData, "score;%d;%d;",player_score,opponent_score);
		 padString(gameData);
		 rt_mutex_release(&txMutex[E_GAME]); 
                 reset = true;
             }
             
       }
		
}