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; }
/** 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; }
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; }
/* * Application entry point. */ int main(void) { msg_t status = RDY_TIMEOUT; halInit(); chSysInit(); chBSemInit(&alarm_sem, TRUE); rtcGetTime(&RTCD1, ×pec); 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, ×pec); alarmspec.tv_sec = timespec.tv_sec + RTC_ALARMPERIOD; rtcSetAlarm(&RTCD1, 0, &alarmspec); } } return 0; }
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; }
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; }
/** * @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; }
/** * @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; }
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(); } }
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; }
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; }
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); } }
msg_t BinarySemaphore::waitTimeout(systime_t time) { return chBSemWaitTimeout(&bsem, time); }
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); } }