Esempio n. 1
0
bool callback(const PWM2Msg &msg) {
	int16_t pwm;

	pwm = msg.value[MOTOR_ID];

	chSysLock();

	if (pwm > 0) {
		pwm_lld_enable_channel(&PWM_DRIVER, 0, pwm);
		pwm_lld_enable_channel(&PWM_DRIVER, 1, 0);
	} else {
		pwm_lld_enable_channel(&PWM_DRIVER, 0, 0);
		pwm_lld_enable_channel(&PWM_DRIVER, 1, -pwm);
	}

	chSysUnlock();

	return true;
}
Esempio n. 2
0
bool enc_callback(const r2p::tEncoderMsg &msg) {

	pwm = speed_pid.update(msg.delta);

	chSysLock();

	if (pwm > 0) {
		pwm_lld_enable_channel(&PWM_DRIVER, 1, pwm);
		pwm_lld_enable_channel(&PWM_DRIVER, 0, 0);
	} else {
		pwm_lld_enable_channel(&PWM_DRIVER, 1, 0);
		pwm_lld_enable_channel(&PWM_DRIVER, 0, -pwm);
	}
	chSysUnlock();

	palTogglePad(LED2_GPIO, LED2);

	return true;
}
Esempio n. 3
0
/**
 * @brief   Changes the running thread priority level then reschedules if
 *          necessary.
 * @note    The function returns the real thread priority regardless of the
 *          current priority that could be higher than the real priority
 *          because the priority inheritance mechanism.
 *
 * @param[in] newprio   the new priority level of the running thread
 * @return              The old priority level.
 *
 * @api
 */
tprio_t chThdSetPriority(tprio_t newprio) {
  tprio_t oldprio;

  chDbgCheck(newprio <= HIGHPRIO, "chThdSetPriority");

  chSysLock();
#if CH_USE_MUTEXES
  oldprio = currp->p_realprio;
  if ((currp->p_prio == currp->p_realprio) || (newprio > currp->p_prio))
    currp->p_prio = newprio;
  currp->p_realprio = newprio;
#else
  oldprio = currp->p_prio;
  currp->p_prio = newprio;
#endif
  chSchRescheduleS();
  chSysUnlock();
  return oldprio;
}
Esempio n. 4
0
void l3g4200d_update(SPIDriver *spip) {
  int16_t data[3];

  gyro_data.t = chTimeNow();

  //XXX da fare lettura sequenziale!
  data[0] = (l3g4200dReadRegister(spip, L3G4200D_OUT_X_H) & 0xFF) << 8;
  data[0] |= l3g4200dReadRegister(spip, L3G4200D_OUT_X_L) & 0xFF;
  data[1] = (l3g4200dReadRegister(spip, L3G4200D_OUT_Y_H) & 0xFF) << 8;
  data[1] |= l3g4200dReadRegister(spip, L3G4200D_OUT_Y_L) & 0xFF;
  data[2] = (l3g4200dReadRegister(spip, L3G4200D_OUT_Z_H) & 0xFF) << 8;
  data[2] |= l3g4200dReadRegister(spip, L3G4200D_OUT_Z_L) & 0xFF;

  chSysLock();
  gyro_data.x = data[0];
  gyro_data.y = data[1];
  gyro_data.z = data[2];
  chSysUnlock();
}
Esempio n. 5
0
/**
 * @brief   Transmits data via the I2C bus as master.
 * @details Number of receiving bytes must be 0 or more than 1 on STM32F1x.
 *          This is hardware restriction.
 *
 * @param[in] i2cp      pointer to the @p I2CDriver object
 * @param[in] addr      slave device address
 * @param[in] txbuf     pointer to the transmit buffer
 * @param[in] txbytes   number of bytes to be transmitted
 * @param[out] rxbuf    pointer to the receive buffer
 * @param[in] rxbytes   number of bytes to be received
 * @param[in] timeout   the number of ticks before the operation timeouts,
 *                      the following special values are allowed:
 *                      - @a TIME_INFINITE no timeout.
 *                      .
 * @return              The operation status.
 * @retval RDY_OK       if the function succeeded.
 * @retval RDY_RESET    if one or more I2C errors occurred, the errors can
 *                      be retrieved using @p i2cGetErrors().
 * @retval RDY_TIMEOUT  if a timeout occurred before operation end. <b>After a
 *                      timeout the driver must be stopped and restarted
 *                      because the bus is in an uncertain state</b>.
 *
 * @notapi
 */
msg_t i2c_lld_master_transmit_timeout(I2CDriver *i2cp, i2caddr_t addr,
                                      const uint8_t *txbuf, size_t txbytes,
                                      uint8_t *rxbuf, size_t rxbytes,
                                      systime_t timeout) {
  LPC_I2C_TypeDef *dp = i2cp->i2c;
  VirtualTimer vt;

  i2cp->addr = addr << 1;
  /* Global timeout for the whole operation.*/
  if (timeout != TIME_INFINITE)
    chVTSetI(&vt, timeout, i2c_lld_safety_timeout, (void *)i2cp);

  /* Releases the lock from high level driver.*/
  chSysUnlock();

  /* Initializes driver fields */
  i2cp->errors = 0;
  i2cp->txbuf = txbuf;
  i2cp->txbytes = txbytes;
  i2cp->rxbuf = rxbuf;
  i2cp->rxbytes = rxbytes;

  /* This lock will be released in high level driver.*/
  chSysLock();

  /* Atomic check on the timer in order to make sure that a timeout didn't
     happen outside the critical zone.*/
  if ((timeout != TIME_INFINITE) && !chVTIsArmedI(&vt))
    return RDY_TIMEOUT;

  /* Starts the operation.*/
  dp->CONSET = I2C_CONSET_STA;

  /* Waits for the operation completion or a timeout.*/
  i2cp->thread = chThdSelf();
  chSchGoSleepS(THD_STATE_SUSPENDED);

  if ((timeout != TIME_INFINITE) && chVTIsArmedI(&vt))
    chVTResetI(&vt);

  return chThdSelf()->p_u.rdymsg;
}
Esempio n. 6
0
uint8_t i2c_t::WriteWrite(uint32_t Addr, uint8_t *WPtr1, uint32_t WLength1, uint8_t *WPtr2, uint32_t WLength2) {
    if(chBSemWait(&BSemaphore) != MSG_OK) return FAILURE;
    uint8_t Rslt;
    msg_t r;
    I2C_TypeDef *pi2c = PParams->pi2c;  // To make things shorter
    if(WLength1 == 0 or WPtr1 == nullptr) { Rslt = CMD_ERROR; goto WriteWriteEnd; }
    if(IBusyWait() != OK) { Rslt = BUSY; goto WriteWriteEnd; }
    IReset(); // Reset I2C
    // Prepare TX DMA
    dmaStreamSetMode(PParams->PDmaTx, DMA_MODE_TX);
    dmaStreamSetMemory0(PParams->PDmaTx, WPtr1);
    dmaStreamSetTransactionSize(PParams->PDmaTx, WLength1);
    // Prepare transmission
    if(WLength2 != 0 and WPtr2 != nullptr) {
        IState = istWriteWrite;
        IPtr = WPtr2;
        ILen = WLength2;
        pi2c->CR2 = (Addr << 1) | (WLength1 << 16) | I2C_CR2_RELOAD;
    }
    else { // No second write
        IState = istWrite;
        pi2c->CR2 = (Addr << 1) | (WLength1 << 16);
    }
    dmaStreamEnable(PParams->PDmaTx);   // Enable TX DMA
    // Enable IRQs: TX completed, error, NAck
    pi2c->CR1 |= (I2C_CR1_TCIE | I2C_CR1_ERRIE | I2C_CR1_NACKIE);
    pi2c->CR2 |= I2C_CR2_START;         // Start transmission
    // Wait completion
    chSysLock();
    r = chThdSuspendTimeoutS(&PThd, MS2ST(I2C_TIMEOUT_MS));
    chSysUnlock();
    // Disable IRQs
    pi2c->CR1 &= ~(I2C_CR1_TCIE | I2C_CR1_ERRIE | I2C_CR1_NACKIE);
    if(r == MSG_TIMEOUT) {
        pi2c->CR2 |= I2C_CR2_STOP;
        Rslt = TIMEOUT;
    }
    else Rslt = (IState == istFailure)? FAILURE : OK;
    WriteWriteEnd:
    chBSemSignal(&BSemaphore);
    return Rslt;
}
Esempio n. 7
0
static void writeOscQueue( InputQueue * q )
{
	msg_t msg;
	uint8_t noData;
	size_t cnt, i;
	chSysLock();
		cnt = (chQSpaceI( q ) / 2) * 2;
	chSysUnlock();
	// And just limit send data size to ensure USB chip will not overfull.
	cnt = ( cnt <= USB_FIFO_SZ ) ? cnt : USB_FIFO_SZ;
	for ( i=0; i<cnt; i++ )
	{
		msg = chIQGetTimeout( q, TIME_IMMEDIATE );
		noData = ( ( msg == Q_TIMEOUT ) || ( msg == Q_RESET ) ) ? 1 : 0;
		uint8_t v;
		v = ( noData ) ? 0 : (uint8_t)msg;
		writeResult( v );
	}
	writeEom();
}
Esempio n. 8
0
/**
 * @brief   Returns the thread next to the specified one.
 * @details The reference counter of the specified thread is decremented and
 *          the reference counter of the returned thread is incremented.
 *
 * @param[in] tp        pointer to the thread
 * @return              A reference to the next thread.
 * @retval NULL         if there is no next thread.
 *
 * @api
 */
Thread *chRegNextThread(Thread *tp) {
  Thread *ntp;

  chSysLock();
  ntp = tp->p_newer;
  if (ntp == (Thread *)&rlist)
    ntp = NULL;
#if CH_USE_DYNAMIC
  else {
    chDbgAssert(ntp->p_refs < 255, "chRegNextThread(), #1",
                "too many references");
    ntp->p_refs++;
  }
#endif
  chSysUnlock();
#if CH_USE_DYNAMIC
  chThdRelease(tp);
#endif
  return ntp;
}
Esempio n. 9
0
/** button interupt universal **/
void buttonInterupt(){
    static virtual_timer_t vt4;

    chSysLock();
        thread_t *ntp = tp_button;
        if(ntp){
            tp_button = NULL;
            chSchWakeupS(ntp, MSG_OK);
        }
        chSysUnlock();

  palSetPad(GPIOD, GPIOD_LED6);

  chSysLockFromISR();
  chVTResetI(&vt4);

  /* LED4 set to OFF after 200mS.*/
  chVTSetI(&vt4, MS2ST(200), led5off, NULL);
  chSysUnlockFromISR();
}
Esempio n. 10
0
/**
 * @brief   Unregisters an Event Listener from its Event Source.
 * @note    If the event listener is not registered on the specified event
 *          source then the function does nothing.
 * @note    For optimal performance it is better to perform the unregister
 *          operations in inverse order of the register operations (elements
 *          are found on top of the list).
 *
 * @param[in] esp       pointer to the  @p event_source_t structure
 * @param[in] elp       pointer to the @p event_listener_t structure
 *
 * @api
 */
void chEvtUnregister(event_source_t *esp, event_listener_t *elp) {
  event_listener_t *p;

  chDbgCheck((esp != NULL) && (elp != NULL));

  /*lint -save -e9087 -e740 [11.3, 1.3] Cast required by list handling.*/
  p = (event_listener_t *)esp;
  /*lint -restore*/
  chSysLock();
  /*lint -save -e9087 -e740 [11.3, 1.3] Cast required by list handling.*/
  while (p->next != (event_listener_t *)esp) {
  /*lint -restore*/
    if (p->next == elp) {
      p->next = elp->next;
      break;
    }
    p = p->next;
  }
  chSysUnlock();
}
Esempio n. 11
0
void RemotePublisher::subscribe(LocalSubscriber * sub, void * buffer,
		size_t size) {

	chPoolLoadArray(&_pool, buffer, size);

	chSysLock();
	if (this->_subscribers == NULL) {
		this->_subscribers = sub;
	} else {
		sub->link(_subscribers);
		_subscribers = sub;
	}

	_rtcan_msg.data = (uint8_t *) allocI();
	if (_rtcan_msg.data) {
		rtcanRegister(&RTCAND, &_rtcan_msg);
	}

	chSysUnlock();
}
Esempio n. 12
0
msg_t pwm_node(void * arg) {
	uint8_t index = *(reinterpret_cast<uint8_t *>(arg));
	r2p::Node node("pwm2sub");
	r2p::Subscriber<r2p::PWM2Msg, 5> pwm_sub;
	r2p::PWM2Msg * msgp;

	(void) arg;

	chRegSetThreadName("pwm_node");

	/* Enable the h-bridge. */
	palSetPad(GPIOB, GPIOB_MOTOR_ENABLE); palClearPad(GPIOA, GPIOA_MOTOR_D1);
	chThdSleepMilliseconds(500);
	pwmStart(&PWM_DRIVER, &pwmcfg);

	node.subscribe(pwm_sub, "pwm");

	for (;;) {
		if (node.spin(r2p::Time::ms(1000))) {
			if (pwm_sub.fetch(msgp)) {
				pwm = msgp->value[index];
				chSysLock()
				;
				if (pwm >= 0) {
					pwm_lld_enable_channel(&PWMD1, 0, msgp->value[index]);
					pwm_lld_enable_channel(&PWMD1, 1, 0);
				} else {
					pwm_lld_enable_channel(&PWMD1, 0, 0);
					pwm_lld_enable_channel(&PWMD1, 1, -msgp->value[index]);
				}
				chSysUnlock();
				pwm_sub.release(*msgp);
			}
		} else {
			// Stop motor if no messages for 1000 ms
			pwm_lld_disable_channel(&PWM_DRIVER, 0);
			pwm_lld_disable_channel(&PWM_DRIVER, 1);
		}
	}
	return CH_SUCCESS;
}
Esempio n. 13
0
/**
 * @brief   Transmits data via the I2C bus as master.
 *
 * @param[in] i2cp      pointer to the @p I2CDriver object
 * @param[in] addr      slave device address
 * @param[in] txbuf     pointer to the transmit buffer
 * @param[in] txbytes   number of bytes to be transmitted
 * @param[out] rxbuf    pointer to the receive buffer
 * @param[in] rxbytes   number of bytes to be received
 * @param[in] timeout   the number of ticks before the operation timeouts,
 *                      the following special values are allowed:
 *                      - @a TIME_INFINITE no timeout.
 *                      .
 * @return              The operation status.
 * @retval RDY_OK       if the function succeeded.
 * @retval RDY_RESET    if one or more I2C errors occurred, the errors can
 *                      be retrieved using @p i2cGetErrors().
 * @retval RDY_TIMEOUT  if a timeout occurred before operation end. <b>After a
 *                      timeout the driver must be stopped and restarted
 *                      because the bus is in an uncertain state</b>.
 *
 * @notapi
 */
msg_t i2c_lld_master_transmit_timeout(I2CDriver *i2cp, i2caddr_t addr,
                                      const uint8_t *txbuf, size_t txbytes,
                                      uint8_t *rxbuf, size_t rxbytes,
                                      systime_t timeout) {
    i2cp->addr = addr;
    i2cp->txbuf = txbuf;
    i2cp->txbytes = txbytes;
    i2cp->txidx = 0;
    i2cp->rxbuf = rxbuf;
    i2cp->rxbytes = rxbytes;
    i2cp->rxidx = 0;

    TWCR = ((1 << TWSTA) | (1 << TWINT) | (1 << TWEN) | (1 << TWIE));

    chSysLock();
    i2cp->thread = chThdSelf();
    chSchGoSleepS(THD_STATE_SUSPENDED);
    chSysUnlock();

    return chThdSelf()->p_u.rdymsg;
}
Esempio n. 14
0
//-----------------------------------------------------------------------------
int
kuroBoxWriterStop(void)
{
	chThdTerminate(loggerThread);
	chThdTerminate(writerThread);

	chThdWait(loggerThread);
	// this thread may be in its own sleep
	chSysLock();
	if (writerThreadForSleep)
	{
		writerThreadForSleep->p_u.rdymsg = (msg_t)10; // just a random non-0
		chSchReadyI(writerThreadForSleep);
		writerThreadForSleep = NULL;
	}
	chSysUnlock();

	chThdWait(writerThread);

	return KB_OK;
}
Esempio n. 15
0
/**
 * @brief   Blocks the execution of the invoking thread until the specified
 *          thread terminates then the exit code is returned.
 * @details This function waits for the specified thread to terminate then
 *          decrements its reference counter, if the counter reaches zero then
 *          the thread working area is returned to the proper allocator.<br>
 *          The memory used by the exited thread is handled in different ways
 *          depending on the API that spawned the thread:
 *          - If the thread was spawned by @p chThdCreateStatic() or by
 *            @p chThdInit() then nothing happens and the thread working area
 *            is not released or modified in any way. This is the default,
 *            totally static, behavior.
 *          - If the thread was spawned by @p chThdCreateFromHeap() then
 *            the working area is returned to the system heap.
 *          - If the thread was spawned by @p chThdCreateFromMemoryPool()
 *            then the working area is returned to the owning memory pool.
 *          .
 * @pre     The configuration option @p CH_USE_WAITEXIT must be enabled in
 *          order to use this function.
 * @post    Enabling @p chThdWait() requires 2-4 (depending on the
 *          architecture) extra bytes in the @p Thread structure.
 * @post    After invoking @p chThdWait() the thread pointer becomes invalid
 *          and must not be used as parameter for further system calls.
 * @note    If @p CH_USE_DYNAMIC is not specified this function just waits for
 *          the thread termination, no memory allocators are involved.
 *
 * @param[in] tp        pointer to the thread
 * @return              The exit code from the terminated thread.
 *
 * @api
 */
msg_t chThdWait(Thread *tp) {
  msg_t msg;

  chDbgCheck(tp != NULL, "chThdWait");

  chSysLock();
  chDbgAssert(tp != currp, "chThdWait(), #1", "waiting self");
#if CH_USE_DYNAMIC
  chDbgAssert(tp->p_refs > 0, "chThdWait(), #2", "not referenced");
#endif
  if (tp->p_state != THD_STATE_FINAL) {
    list_insert(currp, &tp->p_waiting);
    chSchGoSleepS(THD_STATE_WTEXIT);
  }
  msg = tp->p_u.exitcode;
  chSysUnlock();
#if CH_USE_DYNAMIC
  chThdRelease(tp);
#endif
  return msg;
}
Esempio n. 16
0
static void rt_test_005_004_execute(void) {

  /* [5.4.1] A thread is created, it goes to wait on the semaphore.*/
  test_set_step(1);
  {
    threads[0] = chThdCreateStatic(wa[0], WA_SIZE, chThdGetPriorityX()+1, thread1, "A");
  }

  /* [5.4.2] The semaphore counter is increased by two, it is then
     tested to be one, the thread must have completed.*/
  test_set_step(2);
  {
    chSysLock();
    chSemAddCounterI(&sem1, 2);
    chSchRescheduleS();
    chSysUnlock();
    test_wait_threads();
    test_assert_lock(chSemGetCounterI(&sem1) == 1, "invalid counter");
    test_assert_sequence("A", "invalid sequence");
  }
}
Esempio n. 17
0
void chVTSetAny(VirtualTimer *vtp, systime_t time, vtfunc_t vtfunc, void *par) {
	if (isIsrContext()) {
		chSysLockFromIsr()
		;

		if (chVTIsArmedI(vtp))
			chVTResetI(vtp);

		chVTSetI(vtp, time, vtfunc, par);
		chSysUnlockFromIsr()
		;
	} else {
		chSysLock()
		;
		if (chVTIsArmedI(vtp))
			chVTResetI(vtp);
		chVTSetI(vtp, time, vtfunc, par);
		chSysUnlock()
		;
	}
}
Esempio n. 18
0
/**
 * @brief Shell termination handler.
 *
 * @param[in] id event id.
 */
static void termination_handler(eventid_t id) {

	chThdSleepMilliseconds(10);

	cputs("Init: shell on SD1 terminated");
	chSysLock();
	chOQResetI(&SD1.oqueue);
	chSysUnlock();

	// todo: 2nd port for TS

//  if (shelltp2 && chThdTerminated(shelltp2)) {
//    chThdWait(shelltp2);
//    shelltp2 = NULL;
//    chThdSleepMilliseconds(10);
//    cputs("Init: shell on SD2 terminated");
//    chSysLock();
//    chOQResetI(&SD2.oqueue);
//    chSysUnlock();
//  }
}
Esempio n. 19
0
static msg_t l3g4200d_update_thread(void *p) {
  SPIDriver *spip = (SPIDriver *)p;

  while (TRUE) {
    msg_t msg;

    /* Waiting for the IRQ to happen.*/
    chSysLock();
    gyrotp = chThdSelf();
    chSchGoSleepS(THD_STATE_SUSPENDED);
    msg = chThdSelf()->p_u.rdymsg;
    chSysUnlock();

    /* If data ready, update all axis.*/
    if (msg == GYRO_DATA_READY) {
      l3g4200d_update(spip);
    }
  }

  return RDY_OK;
}
Esempio n. 20
0
/**
 * @brief   Stops an ongoing conversion.
 *
 * @param[in] adcp      pointer to the @p ADCDriver object
 */
void adcStopConversion(ADCDriver *adcp) {

    chDbgCheck(adcp != NULL, "adcStopConversion");

    chSysLock();
    chDbgAssert((adcp->ad_state == ADC_READY) ||
                (adcp->ad_state == ADC_RUNNING) ||
                (adcp->ad_state == ADC_COMPLETE),
                "adcStopConversion(), #1",
                "invalid state");
    if (adcp->ad_state == ADC_RUNNING) {
        adc_lld_stop_conversion(adcp);
        adcp->ad_grpp  = NULL;
        adcp->ad_state = ADC_READY;
        chSemResetI(&adcp->ad_sem, 0);
        chSchRescheduleS();
    }
    else
        adcp->ad_state = ADC_READY;
    chSysUnlock();
}
Esempio n. 21
0
static THD_FUNCTION(can_reader_thread, arg)
{
	t_hydra_console *con;
	con = arg;
	chRegSetThreadName("CAN reader");
	chThdSleepMilliseconds(10);
	can_rx_frame rx_msg;
	mode_config_proto_t* proto = &con->mode->proto;

	while (!chThdShouldTerminateX()) {
		if(bsp_can_rxne(proto->dev_num)) {
			chSysLock();
			bsp_can_read(proto->dev_num, &rx_msg);
			can_slcan_out(con, &rx_msg);
			chSysUnlock();
		} else {
			chThdYield();
		}
	}
	chThdExit((msg_t)1);
}
Esempio n. 22
0
void
vexLcdCheckReceiveMessage( LcdData *lcd )
{
    int16_t i;

    // any characters
    if( sdGetWouldBlock(lcd->sdp) )
        return;

    // read up to 16 bytes from serial port
    for(i=0;i<16;i++)
        {
        int16_t c;

        c = sdGetTimeout( lcd->sdp, TIME_IMMEDIATE);
        if( c != Q_TIMEOUT )
            lcd->rxbuf[i] = c;
        else
            break;
        }

    // 6 chars ?
    if( i == 6 )
        {
        // lcd message ?
        if( (lcd->rxbuf[0] == 0xAA) && (lcd->rxbuf[1] == 0x55) && (lcd->rxbuf[2] == 0x16))
            // verify checksum
            if( !((lcd->rxbuf[4] + lcd->rxbuf[5]) & 0xFF) )
                lcd->buttons = lcd->rxbuf[4];
        }

    // flush anything left
    if( !sdGetWouldBlock(lcd->sdp) )
        {
        chSysLock();
        chIQResetI( &(lcd->sdp)->iqueue );
        chSysUnlock();
        }

}
Esempio n. 23
0
static void bmk10_execute(void) {
  static VirtualTimer vt1, vt2;
  uint32_t n = 0;

  test_wait_tick();
  test_start_timer(1000);
  do {
    chSysLock();
    chVTSetI(&vt1, 1, tmo, NULL);
    chVTSetI(&vt2, 10000, tmo, NULL);
    chVTResetI(&vt1);
    chVTResetI(&vt2);
    chSysUnlock();
    n++;
#if defined(SIMULATOR)
    ChkIntSources();
#endif
  } while (!test_timer_done);
  test_print("--- Score : ");
  test_printn(n * 2);
  test_println(" timers/S");
}
Esempio n. 24
0
void lsm303_mag_update(SPIDriver *spip) {
	uint8_t txbuf;
	uint8_t rxbuf[6];
	systime_t timestamp;

	timestamp = chTimeNow();

	txbuf = 0x80 | 0x40 | LSM303D_OUT_X_L_M;
	spiAcquireBus(spip);
	palClearPad(GPIOA, GPIOA_AM_CS);
	spiSend(spip, 1, &txbuf);
	spiReceive(spip, 6, &rxbuf);
	palSetPad(GPIOA, GPIOA_AM_CS);
	spiReleaseBus(spip);

	chSysLock();
	mag_data.t = timestamp;
	mag_data.x = *((int16_t*)&(rxbuf[0])) >> 4;
    mag_data.y = *((int16_t*)&(rxbuf[2])) >> 4;
	mag_data.z = *((int16_t*)&(rxbuf[4])) >> 4;
	chSysUnlock();
}
Esempio n. 25
0
void statusLedPulse(int led, systime_t duration)
{
    chSysLock();
    switch(led)
    {
        case 1:
            if (chVTIsArmedI(&vt1))
                chVTResetI(&vt1);
            else
                palSetPad(IOPORT2, GPIOB_LED1);
            chVTSetI(&vt1, duration, ledoff_1, NULL);
            break;
        case 2:
            if (chVTIsArmedI(&vt2))
                chVTResetI(&vt2);
            else
                palSetPad(IOPORT2, GPIOB_LED2);
            chVTSetI(&vt2, duration, ledoff_2, NULL);
            break;
    }
    chSysUnlock();
}
Esempio n. 26
0
static void test_002_004_execute(void) {
  tprio_t prio, p1;

  /* [2.4.1] Simulating a priority boost situation (prio > realprio).*/
  test_set_step(1);
  {
    prio = chThdGetPriorityX();
    chThdGetSelfX()->prio += 2;
    test_assert(chThdGetPriorityX() == prio + 2, "unexpected priority level");
  }

  /* [2.4.2] Raising thread priority above original priority but below
     the boosted level.*/
  test_set_step(2);
  {
    p1 = chThdSetPriority(prio + 1);
    test_assert(p1 == prio, "unexpected returned priority level");
    test_assert(chThdGetSelfX()->prio == prio + 2, "unexpected priority level");
    test_assert(chThdGetSelfX()->realprio == prio + 1, "unexpected returned real priority level");
  }

  /* [2.4.3] Raising thread priority above the boosted level.*/
  test_set_step(3);
  {
    p1 = chThdSetPriority(prio + 3);
    test_assert(p1 == prio + 1, "unexpected returned priority level");
    test_assert(chThdGetSelfX()->prio == prio + 3, "unexpected priority level");
    test_assert(chThdGetSelfX()->realprio == prio + 3, "unexpected real priority level");
  }

  /* [2.4.4] Restoring original conditions.*/
  test_set_step(4);
  {
    chSysLock();
    chThdGetSelfX()->prio = prio;
    chThdGetSelfX()->realprio = prio;
    chSysUnlock();
  }
}
Esempio n. 27
0
/**
 * @brief   Creates a new thread into a static memory area.
 * @note    A thread can terminate by calling @p chThdExit() or by simply
 *          returning from its main function.
 *
 * @param[out] wsp      pointer to a working area dedicated to the thread stack
 * @param[in] size      size of the working area
 * @param[in] prio      the priority level for the new thread
 * @param[in] pf        the thread function
 * @param[in] arg       an argument passed to the thread function. It can be
 *                      @p NULL.
 * @return              The pointer to the @p thread_t structure allocated for
 *                      the thread into the working space area.
 *
 * @api
 */
thread_t *chThdCreateStatic(void *wsp, size_t size,
                            tprio_t prio, tfunc_t pf, void *arg) {
  thread_t *tp;

  chDbgCheck((wsp != NULL) &&
             MEM_IS_ALIGNED(wsp, PORT_WORKING_AREA_ALIGN) &&
             (size >= THD_WORKING_AREA_SIZE(0)) &&
             MEM_IS_ALIGNED(size, PORT_STACK_ALIGN) &&
             (prio <= HIGHPRIO) && (pf != NULL));

#if CH_DBG_FILL_THREADS == TRUE
  _thread_memfill((uint8_t *)wsp,
                  (uint8_t *)wsp + size,
                  CH_DBG_STACK_FILL_VALUE);
#endif

  chSysLock();

  /* The thread structure is laid out in the upper part of the thread
     workspace. The thread position structure is aligned to the required
     stack alignment because it represents the stack top.*/
  tp = (thread_t *)((uint8_t *)wsp + size -
                    MEM_ALIGN_NEXT(sizeof (thread_t), PORT_STACK_ALIGN));

  /* Stack boundary.*/
  tp->stklimit = (stkalign_t *)wsp;

  /* Setting up the port-dependent part of the working area.*/
  PORT_SETUP_CONTEXT(tp, wsp, tp, pf, arg);

  tp = _thread_init(tp, "noname", prio);

  /* Starting the thread immediately.*/
  chSchWakeupS(tp, MSG_OK);
  chSysUnlock();

  return tp;
}
Esempio n. 28
0
static msg_t initUsbTransfer(void *arg) {

  (void)arg;
  uint8_t i;
  chRegSetThreadName("initUsbTransfer");

  while (TRUE) {
    //wait until enough ADC data is aquired or an overflow is detected
    while((((p1+BUFFLEN-p2)%BUFFLEN)<IN_PACKETSIZE) && !overflow){
      chThdSleepMilliseconds(1);
    }

    // copy the ADC data to the USB transfer buffer
    for (i=0;i<IN_PACKETSIZE;i++){
      transferBuf[i]=((uint8_t*) data)[p2];
      //if an overflow is detected, write the overflow count.
      // in a real application this has to be send on another channel
      if(overflow){
          transferBuf[i]= overflow;
          overflow=0;
      }
      p2 = (p2+1)%BUFFLEN;
    }
    //wait for the last transmission to complete
    while(transmitting){
      chThdSleepMilliseconds(1);
    }

    transmitting = 1;
    usbPrepareTransmit(usbp, EP_IN, transferBuf, IN_PACKETSIZE);

    chSysLock();
    usbStartTransmitI(usbp, EP_IN);
    chSysUnlock();

  }
  return 0;
}
Esempio n. 29
0
/**
 * @brief   Returns the thread next to the specified one.
 * @details The reference counter of the specified thread is decremented and
 *          the reference counter of the returned thread is incremented.
 *
 * @param[in] tp        pointer to the thread
 * @return              A reference to the next thread.
 * @retval NULL         if there is no next thread.
 *
 * @api
 */
thread_t *chRegNextThread(thread_t *tp) {
  thread_t *ntp;

  chSysLock();
  ntp = tp->p_newer;
  /*lint -save -e9087 -e740 [11.3, 1.3] Cast required by list handling.*/
  if (ntp == (thread_t *)&ch.rlist) {
  /*lint -restore*/
    ntp = NULL;
  }
#if CH_CFG_USE_DYNAMIC == TRUE
  else {
    chDbgAssert(ntp->p_refs < (trefs_t)255, "too many references");
    ntp->p_refs++;
  }
#endif
  chSysUnlock();
#if CH_CFG_USE_DYNAMIC == TRUE
  chThdRelease(tp);
#endif

  return ntp;
}
Esempio n. 30
0
/**
 * @brief   Brings the driver in a state safe for card removal.
 *
 * @param[in] mmcp      pointer to the @p MMCDriver object
 * @return              The operation status.
 * @retval FALSE        the operation succeeded and the driver is now
 *                      in the @p MMC_INSERTED state.
 * @retval TRUE         the operation failed.
 *
 * @api
 */
bool_t mmcDisconnect(MMCDriver *mmcp) {
  bool_t status;

  chDbgCheck(mmcp != NULL, "mmcDisconnect");

  chDbgAssert((mmcp->state != MMC_UNINIT) && (mmcp->state != MMC_STOP),
              "mmcDisconnect(), #1", "invalid state");
  switch (mmcp->state) {
  case MMC_READY:
    /* Wait for the pending write operations to complete.*/
    sync(mmcp);
    chSysLock();
    if (mmcp->state == MMC_READY)
      mmcp->state = MMC_INSERTED;
    chSysUnlock();
  case MMC_INSERTED:
    status = FALSE;
  default:
    status = TRUE;
  }
  spiStop(mmcp->spip);
  return status;
}