Ejemplo n.º 1
0
/**
 * @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);
}
Ejemplo n.º 2
0
/**
 * @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);
}
Ejemplo n.º 3
0
/**
 * @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);
}
Ejemplo n.º 4
0
/**
 * @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);
}
Ejemplo n.º 5
0
// 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);
}
Ejemplo n.º 6
0
/*
 * 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);
}
Ejemplo n.º 7
0
/**
 * @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;
}
Ejemplo n.º 8
0
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);
        }
    }
}
Ejemplo n.º 9
0
/**
 * @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;
}
Ejemplo n.º 10
0
/**
 * @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;
}
Ejemplo n.º 11
0
/**
 * @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;
}
Ejemplo n.º 12
0
/**
 * @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;
}
Ejemplo n.º 13
0
/**
 * @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;
}
Ejemplo n.º 14
0
/**
 * @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;
}
Ejemplo n.º 15
0
/**
 * @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;
}
Ejemplo n.º 16
0
/**
 * @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;
}
Ejemplo n.º 17
0
/**
 * @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;
}
Ejemplo n.º 18
0
/**
 * @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;
}
Ejemplo n.º 19
0
/*
 * 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();
    }
}
Ejemplo n.º 20
0
/**
 * @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;
}