示例#1
0
bool_t osWaitForEvent(OsEvent *event, systime_t timeout)
{
   msg_t msg;

   //Wait until the specified event is in the signaled
   //state or the timeout interval elapses
   if(timeout == 0)
   {
      //Non-blocking call
      msg = chBSemWaitTimeout(event, TIME_IMMEDIATE);
   }
   else if(timeout == INFINITE_DELAY)
   {
      //Infinite timeout period
      msg = chBSemWaitTimeout(event, TIME_INFINITE);
   }
   else
   {
      //Wait until the specified event becomes set
      msg = chBSemWaitTimeout(event, OS_MS_TO_SYSTICKS(timeout));
   }

   //Check whether the specified event is set
   if(msg == RDY_OK)
      return TRUE;
   else
      return FALSE;
}
示例#2
0
/** Read bytes from the USART RX DMA buffer.
 *
 * \param s The USART DMA state structure.
 * \param data Pointer to a buffer where the received data will be stored.
 * \param len The number of bytes to attempt to read.
 * \param timeout Return if this time passes with no reception.
 * \return The number of bytes successfully read from the DMA receive buffer.
 */
u32 usart_read_dma_timeout(usart_rx_dma_state* s, u8 data[], u32 len, u32 timeout)
{
  u32 n_available;
  do {
    n_available = usart_n_read_dma(s);
  } while ((n_available < len) &&
           (chBSemWaitTimeout(&s->ready_sem, timeout) == RDY_OK));
  n_available = usart_n_read_dma(s);
  u16 n = (len > n_available) ? n_available : len;

  if (s->rd + n < USART_RX_BUFFER_LEN) {
    memcpy(data, &(s->buff[s->rd]), n);
    s->rd += n;
  } else {
    s->rd_wraps++;
    memcpy(&data[0], &(s->buff[s->rd]), USART_RX_BUFFER_LEN - s->rd);
    memcpy(&data[USART_RX_BUFFER_LEN - s->rd],
           &(s->buff[0]), n - USART_RX_BUFFER_LEN + s->rd);
    s->rd = n - USART_RX_BUFFER_LEN + s->rd;
  }

  s->byte_counter += n;

  return n;
}
示例#3
0
u32 usart_support_read_timeout(void *sd, u8 data[], u32 len, u32 timeout)
{
  struct usart_rx_dma_state *s = &((struct usart_support_s *)sd)->rx;

  u32 n_available;
  do {
    n_available = usart_support_n_read(sd);
  } while ((n_available < len) &&
           (chBSemWaitTimeout(&s->ready, timeout) == MSG_OK));
  n_available = usart_support_n_read(sd);
  u16 n = (len > n_available) ? n_available : len;

  if (s->rd + n < USART_RX_BUFFER_LEN) {
    memcpy(data, &(s->buff[s->rd]), n);
    s->rd += n;
  } else {
    s->rd_wraps++;
    memcpy(&data[0], &(s->buff[s->rd]), USART_RX_BUFFER_LEN - s->rd);
    memcpy(&data[USART_RX_BUFFER_LEN - s->rd],
           &(s->buff[0]), n - USART_RX_BUFFER_LEN + s->rd);
    s->rd = n - USART_RX_BUFFER_LEN + s->rd;
  }

  return n;
}
示例#4
0
/*
 * Application entry point.
 */
int main(void) {
  msg_t status = RDY_TIMEOUT;

  halInit();
  chSysInit();
  chBSemInit(&alarm_sem, TRUE);

  rtcGetTime(&RTCD1, &timespec);
  alarmspec.tv_sec = timespec.tv_sec + RTC_ALARMPERIOD;
  rtcSetAlarm(&RTCD1, 0, &alarmspec);

  rtcSetCallback(&RTCD1, rtc_cb);
  while (TRUE) {

    /* Wait until alarm callback signaled semaphore.*/
    status = chBSemWaitTimeout(&alarm_sem, S2ST(RTC_ALARMPERIOD + 10));

    if (status == RDY_TIMEOUT) {
      chSysHalt();
    }
    else {
      rtcGetTime(&RTCD1, &timespec);
      alarmspec.tv_sec = timespec.tv_sec + RTC_ALARMPERIOD;
      rtcSetAlarm(&RTCD1, 0, &alarmspec);
    }
  }
  return 0;
}
示例#5
0
文件: pill.cpp 项目: Kreyl/nute
uint8_t Pill_t::CheckIfConnected() {
    if(chBSemWaitTimeout(&ISem, TIME_INFINITE) != RDY_OK) return FAILURE;
    uint8_t Rslt = i2c.CmdWriteWrite(EEADDR+IAddr, NULL, 0, NULL, 0);
    Connected = (Rslt == OK);
    if(i2c.Error == true) PillReset();
    chBSemSignal(&ISem);
    return Rslt;
}
示例#6
0
文件: pill.cpp 项目: Kreyl/nute
uint8_t Pill_t::Read(uint8_t *Ptr, uint8_t Length) {
    if(chBSemWaitTimeout(&ISem, TIME_INFINITE) != RDY_OK) return FAILURE;
    uint8_t WordAddress = PILL_START_ADDR;
    uint8_t Rslt = i2c.CmdWriteRead(EEADDR+IAddr, &WordAddress, 1, Ptr, Length);
    Connected = (Rslt == OK);
    if(i2c.Error == true) PillReset();
    chBSemSignal(&ISem);
    return Rslt;
}
示例#7
0
/**
 * @brief   Wait on a mutex.
 */
osStatus osMutexWait(osMutexId mutex_id, uint32_t millisec) {

  msg_t msg = chBSemWaitTimeout((binary_semaphore_t *)mutex_id,
                                (systime_t)millisec);
  switch (msg) {
  case MSG_OK:
    return osOK;
  case MSG_TIMEOUT:
    return osErrorTimeoutResource;
  }
  return osErrorResource;
}
示例#8
0
/**
 * @brief   Wait on a mutex.
 */
osStatus osMutexWait(osMutexId mutex_id, uint32_t millisec) {
  systime_t timeout = ((millisec == 0) || (millisec == osWaitForever)) ?
                      TIME_INFINITE : MS2ST(millisec);

  msg_t msg = chBSemWaitTimeout((binary_semaphore_t *)mutex_id, timeout);
  switch (msg) {
  case MSG_OK:
    return osOK;
  case MSG_TIMEOUT:
    return osErrorTimeoutResource;
  }
  return osErrorResource;
}
示例#9
0
static void nap_exti_thread(void *arg)
{
  (void)arg;
  chRegSetThreadName("NAP ISR");

  while (TRUE) {
    /* Waiting for the IRQ to happen.*/
    chBSemWaitTimeout(&nap_exti_sem, MS2ST(PROCESS_PERIOD_ms));

    handle_nap_exti();
    tracking_channels_process();
  }
}
示例#10
0
文件: mma8451.c 项目: mcu786/u
static msg_t PollAccelThread(void *semp){
  chRegSetThreadName("PollAccel");

  msg_t sem_status = RDY_OK;

  struct EventListener self_el;
  chEvtRegister(&init_event, &self_el, INIT_FAKE_EVID);

  while (TRUE) {
    sem_status = chBSemWaitTimeout((BinarySemaphore*)semp, MS2ST(20));
    txbuf[0] = ACCEL_STATUS;
    if ((i2c_transmit(mma8451addr, txbuf, 1, rxbuf, 7) == RDY_OK) && (sem_status == RDY_OK)){
      raw_data.xacc = complement2signed(rxbuf[1], rxbuf[2]);
      raw_data.yacc = complement2signed(rxbuf[3], rxbuf[4]);
      raw_data.zacc = complement2signed(rxbuf[5], rxbuf[6]);

      /* there is no need of correcting of placement. Just get milli g */
      mavlink_raw_imu_struct.xacc = raw_data.xacc * *xpol;
      mavlink_raw_imu_struct.yacc = raw_data.yacc * *ypol;
      mavlink_raw_imu_struct.zacc = raw_data.zacc * *zpol;

      comp_data.xacc = 1000 * (((int32_t)raw_data.xacc) * *xpol + *xoffset) / *xsens;
      comp_data.yacc = 1000 * (((int32_t)raw_data.yacc) * *ypol + *yoffset) / *ysens;
      comp_data.zacc = 1000 * (((int32_t)raw_data.zacc) * *zpol + *zoffset) / *zsens;

      /* fill scaled debug struct */
      mavlink_scaled_imu_struct.xacc = comp_data.xacc;
      mavlink_scaled_imu_struct.yacc = comp_data.yacc;
      mavlink_scaled_imu_struct.zacc = comp_data.zacc;
    }
    else{
      raw_data.xacc = -32768;
      raw_data.yacc = -32768;
      raw_data.zacc = -32768;
      mavlink_raw_imu_struct.xacc = -32768;
      mavlink_raw_imu_struct.yacc = -32768;
      mavlink_raw_imu_struct.zacc = -32768;
      mavlink_scaled_imu_struct.xacc = -32768;
      mavlink_scaled_imu_struct.yacc = -32768;
      mavlink_scaled_imu_struct.zacc = -32768;
    }

    if (chThdSelf()->p_epending & EVENT_MASK(SIGHALT_EVID))
      chThdExit(RDY_OK);
  }
  return 0;
}
示例#11
0
文件: pill.cpp 项目: Kreyl/nute
uint8_t Pill_t::Write(uint8_t *Ptr, uint8_t Length) {
    if(chBSemWaitTimeout(&ISem, TIME_INFINITE) != RDY_OK) return FAILURE;
    uint8_t WordAddress = PILL_START_ADDR;
    uint8_t Rslt = OK;
    // Write page by page
    while(Length) {
        uint8_t ToWriteCnt = (Length > PILL_PAGE_SZ)? PILL_PAGE_SZ : Length;
        Rslt = i2c.CmdWriteWrite(EEADDR+IAddr, &WordAddress, 1, Ptr, ToWriteCnt);
        if(Rslt == OK) {
            chThdSleepMilliseconds(5);   // Allow memory to complete writing
            Length -= ToWriteCnt;
            Ptr += ToWriteCnt;
            WordAddress += ToWriteCnt;
        }
        else break;
    }
    Connected = (Rslt == OK);
    if(i2c.Error == true) PillReset();
    chBSemSignal(&ISem);
    return Rslt;
}
示例#12
0
static void nap_exti_thread(void *arg)
{
  (void)arg;
  chRegSetThreadName("NAP ISR");

  while (TRUE) {
    /* Waiting for the IRQ to happen.*/
    chBSemWaitTimeout(&nap_exti_sem, MS2ST(PROCESS_PERIOD_ms));

    /* We need a level (not edge) sensitive interrupt -
     * if there is another interrupt pending on the Swift
     * NAP then the IRQ line will stay high. Therefore if
     * the line is still high, don't suspend the thread.
     */

    spi_lock(SPI_SLAVE_FPGA);
    while (palReadLine(LINE_NAP_IRQ)) {
      handle_nap_exti();
    }
    tracking_channels_process();
    spi_unlock(SPI_SLAVE_FPGA);

  }
}
示例#13
0
msg_t BinarySemaphore::waitTimeout(systime_t time) {

    return chBSemWaitTimeout(&bsem, time);
}
示例#14
0
static msg_t GTimerThreadHandler(void *arg) {
	(void)arg;
	GTimer			*pt;
	systime_t		tm;
	systime_t		nxtTimeout;
	systime_t		lastTime;
	GTimerFunction	fn;
	void			*param;

	#if CH_USE_REGISTRY
		chRegSetThreadName("GTimer");
	#endif

	nxtTimeout = TIME_INFINITE;
	lastTime = 0;
	while(1) {
		/* Wait for work to do. */
		chThdYield();					// Give someone else a go no matter how busy we are
		chBSemWaitTimeout(&waitsem, nxtTimeout);
		
	restartTimerChecks:
	
		// Our reference time
		tm = chTimeNow();
		nxtTimeout = TIME_INFINITE;
		
		/* We need to obtain the mutex */
		chMtxLock(&mutex);

		if (pTimerHead) {
			pt = pTimerHead;
			do {
				// Do we have something to do for this timer?
				if ((pt->flags & GTIMER_FLG_JABBED) || (!(pt->flags & GTIMER_FLG_INFINITE) && TimeIsWithin(pt->when, lastTime, tm))) {
				
					// Is this timer periodic?
					if ((pt->flags & GTIMER_FLG_PERIODIC) && pt->period != TIME_IMMEDIATE) {
						// Yes - Update ready for the next period
						if (!(pt->flags & GTIMER_FLG_INFINITE)) {
							// We may have skipped a period.
							// We use this complicated formulae rather than a loop
							//	because the gcc compiler stuffs up the loop so that it
							//	either loops forever or doesn't get executed at all.
							pt->when += ((tm + pt->period - pt->when) / pt->period) * pt->period;
						}

						// We are definitely no longer jabbed
						pt->flags &= ~GTIMER_FLG_JABBED;
						
					} else {
						// No - get us off the timers list
						if (pt->next == pt->prev)
							pTimerHead = 0;
						else {
							pt->next->prev = pt->prev;
							pt->prev->next = pt->next;
							if (pTimerHead == pt)
								pTimerHead = pt->next;
						}
						pt->flags = 0;
					}
					
					// Call the callback function
					fn = pt->fn;
					param = pt->param;
					chMtxUnlock();
					fn(param);
					
					// We no longer hold the mutex, the callback function may have taken a while
					// and our list may have been altered so start again!
					goto restartTimerChecks;
				}
				
				// Find when we next need to wake up
				if (!(pt->flags & GTIMER_FLG_INFINITE) && pt->when - tm < nxtTimeout)
					nxtTimeout = pt->when - tm;
				pt = pt->next;
			} while(pt != pTimerHead);
		}

		// Ready for the next loop
		lastTime = tm;
		chMtxUnlock();
	}
	return 0;
}
THD_FUNCTION(scMS5611Thread, arg)
{
  systime_t sleep_until;
  systime_t interval_st = MS2ST(ms5611_interval_ms);
  
  (void)arg;
  chRegSetThreadName(__func__);

  SC_LOG_PRINTF("d: ms5611 init\r\n");
  
  // We need to give a bit of time to the chip until we can ask it to shut down
  // i2c not ready initially? (This is inherited from the LSM9DS0 driver)
  chThdSleepMilliseconds(20);

  ms5611_reset();
  chThdSleepMilliseconds(100);
  ms5611_reset();
  chThdSleepMilliseconds(100);
  ms5611_reset();
  chThdSleepMilliseconds(100);
  ms5611_read_prom();
  uint8_t ref_crc = ms5611_prom[MS5611_PROM_SIZE - 1] & 0x0F;
  uint8_t calc_crc = ms5611_calc_crc();
  if (ref_crc != calc_crc) {
    SC_LOG_PRINTF("e: ms5611 crc failure: 0x%x vs 0x%x\r\n", ref_crc, calc_crc);
  }

  sleep_until = chVTGetSystemTime() + interval_st;
  
  // Loop initiating measurements and waiting for results
  while (!chThdShouldTerminateX()) {
    systime_t now = chVTGetSystemTime();
    systime_t wait_time = sleep_until - now;
    uint32_t temp;
    uint32_t pres;
    msg_t msg;

    // Using a semaphore allows to cancel the sleep outside the thread
    if (chBSemWaitTimeout(&ms5611_wait_sem, wait_time) != MSG_TIMEOUT) {
      continue;
    }

    sleep_until += interval_st;
    now = chVTGetSystemTime();
    
    temp = ms5611_read(true);
    
    if (chThdShouldTerminateX()) {
      break;
    }
    
    pres = ms5611_read(false);

    chMtxLock(&data_mtx);
    ms5611_temp = temp;
    ms5611_pres = pres;
    ms5611_time_st = now;
    chMtxUnlock(&data_mtx);

    // Notify main app about new measurements being available
    msg = sc_event_msg_create_type(SC_EVENT_TYPE_MS5611_AVAILABLE);
    sc_event_msg_post(msg, SC_EVENT_MSG_POST_FROM_NORMAL);
  }
}