/** * @brief Puts the invoking thread into the queue's threads queue. * * @param[out] qp pointer to an @p GenericQueue structure * @param[in] time the number of ticks before the operation timeouts, * the following special values are allowed: * - @a TIME_IMMEDIATE immediate timeout. * - @a TIME_INFINITE no timeout. * . * @return A message specifying how the invoking thread has been * released from threads queue. * @retval Q_OK is the normal exit, thread signaled. * @retval Q_RESET if the queue has been reset. * @retval Q_TIMEOUT if the queue operation timed out. */ static msg_t qwait(GenericQueue *qp, systime_t time) { if (TIME_IMMEDIATE == time) return Q_TIMEOUT; currp->p_u.wtobjp = qp; queue_insert(currp, &qp->q_waiting); return chSchGoSleepTimeoutS(THD_STATE_WTQUEUE, time); }
/** * @brief Enqueues the caller thread on a threads queue object. * @details The caller thread is enqueued and put to sleep until it is * dequeued or the specified timeouts expires. * * @param[in] tqp pointer to the threads queue object * @param[in] timeout the timeout in system ticks, the special values are * handled as follow: * - @a TIME_INFINITE the thread enters an infinite sleep * state. * - @a TIME_IMMEDIATE the thread is not enqueued and * the function returns @p MSG_TIMEOUT as if a timeout * occurred. * . * @return The message from @p osalQueueWakeupOneI() or * @p osalQueueWakeupAllI() functions. * @retval MSG_TIMEOUT if the thread has not been dequeued within the * specified timeout or if the function has been * invoked with @p TIME_IMMEDIATE as timeout * specification. * * @sclass */ msg_t chThdEnqueueTimeoutS(threads_queue_t *tqp, systime_t timeout) { if (TIME_IMMEDIATE == timeout) return MSG_TIMEOUT; queue_insert(currp, tqp); return chSchGoSleepTimeoutS(CH_STATE_QUEUED, timeout); }
/** * @brief Sends the current thread sleeping and sets a reference variable. * @note This function must reschedule, it can only be called from thread * context. * * @param[in] trp a pointer to a thread reference object * @param[in] timeout the number of ticks before the operation timeouts, * the following special values are allowed: * - @a TIME_INFINITE no timeout. * . * @return The wake up message. * * @sclass */ msg_t chThdSuspendTimeoutS(thread_reference_t *trp, systime_t timeout) { chDbgAssert(*trp == NULL, "not NULL"); *trp = nil.current; nil.current->u1.trp = trp; return chSchGoSleepTimeoutS(NIL_STATE_SUSP, timeout); }
/** * @brief Sends the current thread sleeping and sets a reference variable. * @note This function must reschedule, it can only be called from thread * context. * * @param[in] trp a pointer to a thread reference object * @param[in] timeout the timeout in system ticks, the special values are * handled as follow: * - @a TIME_INFINITE the thread enters an infinite sleep * state. * - @a TIME_IMMEDIATE the thread is not enqueued and * the function returns @p MSG_TIMEOUT as if a timeout * occurred. * . * @return The wake up message. * @retval MSG_TIMEOUT if the operation timed out. * * @sclass */ msg_t chThdSuspendTimeoutS(thread_reference_t *trp, systime_t timeout) { thread_t *tp = chThdGetSelfX(); chDbgAssert(*trp == NULL, "not NULL"); if (TIME_IMMEDIATE == timeout) return MSG_TIMEOUT; *trp = tp; tp->p_u.wtobjp = &trp; return chSchGoSleepTimeoutS(CH_STATE_SUSPENDED, timeout); }
// Enter RX mode and wait reception for Timeout_ms. uint8_t cc1101_t::ReceiveSync(uint32_t Timeout_ms, void *Ptr, int8_t *PRssi) { FlushRxFIFO(); chSysLock(); PWaitingThread = chThdSelf(); EnterRX(); msg_t Rslt = chSchGoSleepTimeoutS(THD_STATE_SUSPENDED, MS2ST(Timeout_ms)); chSysUnlock(); // Will be here when IRQ will fire, or timeout occur - with appropriate message if(Rslt == RDY_TIMEOUT) { // Nothing received, timeout occured EnterIdle(); // Get out of RX mode return TIMEOUT; } else return ReadFIFO(Ptr, PRssi); }
/* * Enter RX mode and wait reception for Timeout_ms. */ uint8_t cc1101_t::ReceiveSync(uint32_t Timeout_ms, rPkt_t *pPkt) { FlushRxFIFO(); EnterRX(); // After that, some time will be wasted to recalibrate chSysLock(); PWaitingThread = chThdSelf(); msg_t Rslt = chSchGoSleepTimeoutS(THD_STATE_SUSPENDED, MS2ST(Timeout_ms)); chSysUnlock(); // Will be here when IRQ will fire, or timeout occur - with appropriate message if(Rslt == RDY_TIMEOUT) { // Nothing received, timeout occured EnterIdle(); // Get out of RX mode return TIMEOUT; } // IRQ occured: something received, or CRC error else return ReadFIFO(pPkt); }
/** * @brief Performs a wait operation on a semaphore with timeout specification. * * @param[in] sp pointer to a @p semaphore_t structure * @param[in] timeout the number of ticks before the operation timeouts, * the following special values are allowed: * - @a TIME_IMMEDIATE immediate timeout. * - @a TIME_INFINITE no timeout. * . * @return A message specifying how the invoking thread has been * released from the semaphore. * @retval NIL_MSG_OK if the thread has not stopped on the semaphore or the * semaphore has been signaled. * @retval NIL_MSG_RST if the semaphore has been reset using @p chSemReset(). * @retval NIL_MSG_TMO if the semaphore has not been signaled or reset within * the specified timeout. * * @sclass */ msg_t chSemWaitTimeoutS(semaphore_t *sp, systime_t timeout) { /* Note, the semaphore counter is a volatile variable so accesses are manually optimized.*/ cnt_t cnt = sp->cnt; if (cnt <= (cnt_t)0) { if (TIME_IMMEDIATE == timeout) { return MSG_TIMEOUT; } sp->cnt = cnt - (cnt_t)1; nil.current->u1.semp = sp; return chSchGoSleepTimeoutS(NIL_STATE_WTSEM, timeout); } sp->cnt = cnt - (cnt_t)1; return MSG_OK; }
msg_t hmc5883l_thread(void *arg) { (void)arg; uint8_t buf_data[6]; float field[3]; chRegSetThreadName("HMC5883L"); i2cStart(&I2CD2, &i2cconfig); while (!hmc58831_ID_check()) { tweeter_set_error(ERROR_MAGNO, true); chThdSleepMilliseconds(500); } tweeter_set_error(ERROR_MAGNO, false); /* Initialise the settings. */ while (!hmc5883l_init()) { tweeter_set_error(ERROR_MAGNO, true); chThdSleepMilliseconds(500); } tweeter_set_error(ERROR_MAGNO, false); while (TRUE) { /* Sleep until DRDY */ chSysLock(); tpHMC5883L = chThdSelf(); chSchGoSleepTimeoutS(THD_STATE_SUSPENDED, 100); tpHMC5883L = NULL; chSysUnlock(); /* Pull data from magno into buf_data. */ if (hmc5883l_receive(buf_data)) { tweeter_set_error(ERROR_MAGNO, false); hmc5883l_field_convert(buf_data, field); log_s16(CHAN_IMU_MAGNO, field[0], field[1], field[2], 0); /*define this state estimation function state_estimation_new_magno(field[0], field[1], field[2]); */ } else { chThdSleepMilliseconds(20); tweeter_set_error(ERROR_MAGNO, true); } } }
/** * @brief Waits on the condition variable releasing the mutex lock. * @details Releases the currently owned mutex, waits on the condition * variable, and finally acquires the mutex again. All the sequence * is performed atomically. * @pre The invoking thread <b>must</b> have at least one owned mutex. * @pre The configuration option @p CH_USE_CONDVARS_TIMEOUT must be enabled * in order to use this function. * @post Exiting the function because a timeout does not re-acquire the * mutex, the mutex ownership is lost. * * @param[in] cp pointer to the @p CondVar structure * @param[in] time the number of ticks before the operation timeouts, the * special values are handled as follow: * - @a TIME_INFINITE no timeout. * - @a TIME_IMMEDIATE this value is not allowed. * . * @return A message specifying how the invoking thread has been * released from the condition variable. * @retval RDY_OK if the condvar has been signaled using * @p chCondSignal(). * @retval RDY_RESET if the condvar has been signaled using * @p chCondBroadcast(). * @retval RDY_TIMEOUT if the condvar has not been signaled within the * specified timeout. * * @sclass */ msg_t chCondWaitTimeoutS(CondVar *cp, systime_t time) { Mutex *mp; msg_t msg; chDbgCheck((cp != NULL) && (time != TIME_IMMEDIATE), "chCondWaitTimeoutS"); chDbgAssert(currp->p_mtxlist != NULL, "chCondWaitTimeoutS(), #1", "not owning a mutex"); mp = chMtxUnlockS(); currp->p_u.wtobjp = cp; prio_insert(currp, &cp->c_queue); msg = chSchGoSleepTimeoutS(THD_STATE_WTCOND, time); if (msg != RDY_TIMEOUT) chMtxLockS(mp); return msg; }
/** * @brief Waits on the condition variable releasing the mutex lock. * @details Releases the currently owned mutex, waits on the condition * variable, and finally acquires the mutex again. All the sequence * is performed atomically. * @pre The invoking thread <b>must</b> have at least one owned mutex. * @pre The configuration option @p CH_CFG_USE_CONDVARS_TIMEOUT must be enabled * in order to use this function. * @post Exiting the function because a timeout does not re-acquire the * mutex, the mutex ownership is lost. * * @param[in] cp pointer to the @p condition_variable_t structure * @param[in] time the number of ticks before the operation timeouts, the * special values are handled as follow: * - @a TIME_INFINITE no timeout. * - @a TIME_IMMEDIATE this value is not allowed. * . * @return A message specifying how the invoking thread has been * released from the condition variable. * @retval MSG_OK if the condition variable has been signaled using * @p chCondSignal(). * @retval MSG_RESET if the condition variable has been signaled using * @p chCondBroadcast(). * @retval MSG_TIMEOUT if the condition variable has not been signaled within * the specified timeout. * * @sclass */ msg_t chCondWaitTimeoutS(condition_variable_t *cp, systime_t time) { mutex_t *mp; msg_t msg; chDbgCheckClassS(); chDbgCheck((cp != NULL) && (time != TIME_IMMEDIATE)); chDbgAssert(currp->p_mtxlist != NULL, "not owning a mutex"); mp = chMtxGetNextMutexS(); chMtxUnlockS(mp); currp->p_u.wtobjp = cp; queue_prio_insert(currp, &cp->c_queue); msg = chSchGoSleepTimeoutS(CH_STATE_WTCOND, time); if (msg != MSG_TIMEOUT) chMtxLockS(mp); return msg; }
/** * @brief Performs a wait operation on a semaphore with timeout specification. * * @param[in] sp pointer to a @p semaphore_t structure * @param[in] time the number of ticks before the operation timeouts, * the following special values are allowed: * - @a TIME_IMMEDIATE immediate timeout. * - @a TIME_INFINITE no timeout. * . * @return A message specifying how the invoking thread has been * released from the semaphore. * @retval MSG_OK if the thread has not stopped on the semaphore or the * semaphore has been signaled. * @retval MSG_RESET if the semaphore has been reset using @p chSemReset(). * @retval MSG_TIMEOUT if the semaphore has not been signaled or reset within * the specified timeout. * * @sclass */ msg_t chSemWaitTimeoutS(semaphore_t *sp, systime_t time) { chDbgCheckClassS(); chDbgCheck(sp != NULL); chDbgAssert(((sp->s_cnt >= 0) && queue_isempty(&sp->s_queue)) || ((sp->s_cnt < 0) && queue_notempty(&sp->s_queue)), "inconsistent semaphore"); if (--sp->s_cnt < 0) { if (TIME_IMMEDIATE == time) { sp->s_cnt++; return MSG_TIMEOUT; } currp->p_u.wtobjp = sp; sem_insert(currp, &sp->s_queue); return chSchGoSleepTimeoutS(CH_STATE_WTSEM, time); } return MSG_OK; }
/** * @brief Performs a wait operation on a semaphore with timeout specification. * * @param[in] sp pointer to a @p Semaphore structure * @param[in] time the number of ticks before the operation timeouts, * the following special values are allowed: * - @a TIME_IMMEDIATE immediate timeout. * - @a TIME_INFINITE no timeout. * . * @return A message specifying how the invoking thread has been * released from the semaphore. * @retval RDY_OK if the thread has not stopped on the semaphore or the * semaphore has been signaled. * @retval RDY_RESET if the semaphore has been reset using @p chSemReset(). * @retval RDY_TIMEOUT if the semaphore has not been signaled or reset within * the specified timeout. * * @sclass */ msg_t chSemWaitTimeoutS(Semaphore *sp, systime_t time) { chDbgCheck(sp != NULL, "chSemWaitTimeoutS"); chDbgAssert(((sp->s_cnt >= 0) && isempty(&sp->s_queue)) || ((sp->s_cnt < 0) && notempty(&sp->s_queue)), "chSemWaitTimeoutS(), #1", "inconsistent semaphore"); if (--sp->s_cnt < 0) { if (TIME_IMMEDIATE == time) { sp->s_cnt++; return RDY_TIMEOUT; } currp->p_u.wtobjp = sp; sem_insert(currp, &sp->s_queue); return chSchGoSleepTimeoutS(THD_STATE_WTSEM, time); } return RDY_OK; }
/** * @brief Waits for all the specified events. * @details The function waits for all the events specified in @p events to * become pending then the events are cleared and returned. * * @param[in] events events that the function should wait * for, @p ALL_EVENTS requires all the events * @param[in] time the number of ticks before the operation timeouts, * the following special values are allowed: * - @a TIME_IMMEDIATE immediate timeout. * - @a TIME_INFINITE no timeout. * . * @return The mask of the served and cleared events. * @retval 0 if the operation has timed out. * * @api */ eventmask_t chEvtWaitAllTimeout(eventmask_t events, systime_t time) { thread_t *ctp = currp; chSysLock(); if ((ctp->epending & events) != events) { if (TIME_IMMEDIATE == time) { chSysUnlock(); return (eventmask_t)0; } ctp->u.ewmask = events; if (chSchGoSleepTimeoutS(CH_STATE_WTANDEVT, time) < MSG_OK) { chSysUnlock(); return (eventmask_t)0; } } ctp->epending &= ~events; chSysUnlock(); return events; }
/** * @brief Waits for all the specified events. * @details The function waits for all the events specified in @p mask to * become pending then the events are cleared and returned. * * @param[in] mask mask of the event flags that the function should wait * for, @p ALL_EVENTS requires all the events * @param[in] time the number of ticks before the operation timeouts, * the following special values are allowed: * - @a TIME_IMMEDIATE immediate timeout. * - @a TIME_INFINITE no timeout. * . * @return The mask of the served and cleared events. * @retval 0 if the operation has timed out. * * @api */ eventmask_t chEvtWaitAllTimeout(eventmask_t mask, systime_t time) { Thread *ctp = currp; chSysLock(); if ((ctp->p_epending & mask) != mask) { if (TIME_IMMEDIATE == time) { chSysUnlock(); return (eventmask_t)0; } ctp->p_u.ewmask = mask; if (chSchGoSleepTimeoutS(THD_STATE_WTANDEVT, time) < RDY_OK) { chSysUnlock(); return (eventmask_t)0; } } ctp->p_epending &= ~mask; chSysUnlock(); return mask; }
/** * @brief Waits for any of the specified events. * @details The function waits for any event among those specified in * @p mask to become pending then the events are cleared and * returned. * * @param[in] mask mask of the event flags that the function should wait * for, @p ALL_EVENTS enables all the events * @param[in] timeout the number of ticks before the operation timeouts, * the following special values are allowed: * - @a TIME_IMMEDIATE immediate timeout. * - @a TIME_INFINITE no timeout. * . * @return The mask of the served and cleared events. * @retval 0 if the operation has timed out. * * @sclass */ eventmask_t chEvtWaitAnyTimeoutS(eventmask_t mask, systime_t timeout) { thread_t *ctp = nil.current; eventmask_t m; if ((m = (ctp->epmask & mask)) == (eventmask_t)0) { if (TIME_IMMEDIATE == timeout) { chSysUnlock(); return (eventmask_t)0; } ctp->u1.ewmask = mask; if (chSchGoSleepTimeoutS(NIL_STATE_WTOREVT, timeout) < MSG_OK) { chSysUnlock(); return (eventmask_t)0; } m = ctp->epmask & mask; } ctp->epmask &= ~m; return m; }
/** * @brief Waits for any of the specified events. * @details The function waits for any event among those specified in * @p events to become pending then the events are cleared and * returned. * * @param[in] events events that the function should wait * for, @p ALL_EVENTS enables all the events * @param[in] time the number of ticks before the operation timeouts, * the following special values are allowed: * - @a TIME_IMMEDIATE immediate timeout. * - @a TIME_INFINITE no timeout. * . * @return The mask of the served and cleared events. * @retval 0 if the operation has timed out. * * @api */ eventmask_t chEvtWaitAnyTimeout(eventmask_t events, systime_t time) { thread_t *ctp = currp; eventmask_t m; chSysLock(); if ((m = (ctp->p_epending & events)) == 0) { if (TIME_IMMEDIATE == time) { chSysUnlock(); return (eventmask_t)0; } ctp->p_u.ewmask = events; if (chSchGoSleepTimeoutS(CH_STATE_WTOREVT, time) < MSG_OK) { chSysUnlock(); return (eventmask_t)0; } m = ctp->p_epending & events; } ctp->p_epending &= ~m; chSysUnlock(); return m; }
/** * @brief Waits on the condition variable releasing the mutex lock. * @details Releases the currently owned mutex, waits on the condition * variable, and finally acquires the mutex again. All the sequence * is performed atomically. * @pre The invoking thread <b>must</b> have at least one owned mutex. * @pre The configuration option @p CH_CFG_USE_CONDVARS_TIMEOUT must be enabled * in order to use this function. * @post Exiting the function because a timeout does not re-acquire the * mutex, the mutex ownership is lost. * * @param[in] cp pointer to the @p condition_variable_t structure * @param[in] time the number of ticks before the operation timeouts, the * special values are handled as follow: * - @a TIME_INFINITE no timeout. * - @a TIME_IMMEDIATE this value is not allowed. * . * @return A message specifying how the invoking thread has been * released from the condition variable. * @retval MSG_OK if the condition variable has been signaled using * @p chCondSignal(). * @retval MSG_RESET if the condition variable has been signaled using * @p chCondBroadcast(). * @retval MSG_TIMEOUT if the condition variable has not been signaled within * the specified timeout. * * @sclass */ msg_t chCondWaitTimeoutS(condition_variable_t *cp, systime_t time) { mutex_t *mp; msg_t msg; chDbgCheckClassS(); chDbgCheck((cp != NULL) && (time != TIME_IMMEDIATE)); chDbgAssert(currp->p_mtxlist != NULL, "not owning a mutex"); /* Getting "current" mutex and releasing it.*/ mp = chMtxGetNextMutexS(); chMtxUnlockS(mp); /* Start waiting on the condition variable, on exit the mutex is taken again.*/ currp->p_u.wtobjp = cp; queue_prio_insert(currp, &cp->c_queue); msg = chSchGoSleepTimeoutS(CH_STATE_WTCOND, time); if (msg != MSG_TIMEOUT) { chMtxLockS(mp); } return msg; }
/** * @brief Waits for any of the specified events. * @details The function waits for any event among those specified in * @p mask to become pending then the events are cleared and * returned. * * @param[in] mask mask of the event flags that the function should wait * for, @p ALL_EVENTS enables all the events * @param[in] time the number of ticks before the operation timeouts, * the following special values are allowed: * - @a TIME_IMMEDIATE immediate timeout. * - @a TIME_INFINITE no timeout. * . * @return The mask of the served and cleared events. * @retval 0 if the operation has timed out. * * @api */ eventmask_t chEvtWaitAnyTimeout(eventmask_t mask, systime_t time) { Thread *ctp = currp; eventmask_t m; chSysLock(); if ((m = (ctp->p_epending & mask)) == 0) { if (TIME_IMMEDIATE == time) { chSysUnlock(); return (eventmask_t)0; } ctp->p_u.ewmask = mask; if (chSchGoSleepTimeoutS(THD_STATE_WTOREVT, time) < RDY_OK) { chSysUnlock(); return (eventmask_t)0; } m = ctp->p_epending & mask; } ctp->p_epending &= ~m; chSysUnlock(); return m; }
/* * ADXL345 (low-g accelerometer) main thread. */ msg_t adxl345_thread(void *arg) { (void)arg; int j; const SPIConfig spi_cfg = { NULL, ADXL345_SPI_CS_PORT, ADXL345_SPI_CS_PIN, SPI_CR1_BR_1 | SPI_CR1_BR_0 | SPI_CR1_CPOL | SPI_CR1_CPHA }; int16_t accels[3], axis, g; chRegSetThreadName("ADXL345"); spiStart(&ADXL345_SPID, &spi_cfg); adxl3x5_init(&ADXL345_SPID, 4, &axis, &g); log_s16(CHAN_CAL_LGA, axis, g, 0, 0); while(TRUE) { adxl3x5_read_accel(&ADXL345_SPID, accels); for (j = 0; j < 3; j++){ global_accel[j] = accels[j]; } log_s16(CHAN_IMU_LGA, accels[0], accels[1], accels[2], 0); state_estimation_new_lg_accel( adxl3x5_accels_to_axis(accels, axis, g)); /* Sleep until DRDY */ chSysLock(); tp345 = chThdSelf(); chSchGoSleepTimeoutS(THD_STATE_SUSPENDED, 100); tp345 = NULL; chSysUnlock(); } }
/** * @brief Waits for exactly one of the specified events. * @details The function waits for one event among those specified in * @p events to become pending then the event is cleared and returned. * @note One and only one event is served in the function, the one with the * lowest event id. The function is meant to be invoked into a loop * in order to serve all the pending events.<br> * This means that Event Listeners with a lower event identifier have * an higher priority. * * @param[in] events events that the function should wait * for, @p ALL_EVENTS enables all the events * @param[in] time the number of ticks before the operation timeouts, * the following special values are allowed: * - @a TIME_IMMEDIATE immediate timeout. * - @a TIME_INFINITE no timeout. * . * @return The mask of the lowest event id served and cleared. * @retval 0 if the operation has timed out. * * @api */ eventmask_t chEvtWaitOneTimeout(eventmask_t events, systime_t time) { thread_t *ctp = currp; eventmask_t m; chSysLock(); m = ctp->epending & events; if (m == (eventmask_t)0) { if (TIME_IMMEDIATE == time) { chSysUnlock(); return (eventmask_t)0; } ctp->u.ewmask = events; if (chSchGoSleepTimeoutS(CH_STATE_WTOREVT, time) < MSG_OK) { chSysUnlock(); return (eventmask_t)0; } m = ctp->epending & events; } m ^= m & (m - (eventmask_t)1); ctp->epending &= ~m; chSysUnlock(); return m; }