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; } }
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++; } }
/*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; }
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); } }
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"); } }
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); } } } }
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(); } }
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]); }
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]); }
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; } }
/** 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; }
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); } }
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; }
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); } }
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; }
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; } }
/** 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; }
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; }
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; }
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); }
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); }
/* 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; }
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); } }
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"); }
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; } } }