Beispiel #1
0
/**
 * @brief   Get a message from the queue.
 */
osEvent osMessageGet(osMessageQId queue_id,
                     uint32_t millisec) {

  msg_t msg;
  osEvent event;

  event.def.message_id = queue_id;

  if (port_is_isr_context()) {

    /* Waiting makes no sense in ISRs so any value except "immediate"
       makes no sense.*/
    if (millisec != 0) {
      event.status = osErrorValue;
      return event;
    }

    chSysLockFromISR();
    msg = chMBFetchI((mailbox_t *)queue_id, (msg_t*)&event.value.v);
    chSysUnlockFromISR();
  }
  else {
    msg = chMBFetch((mailbox_t *)queue_id,
                    (msg_t*)&event.value.v,
                    (systime_t)millisec);
  }

  /* Returned event type.*/
  event.status = msg == MSG_OK ? osEventMessage : osEventTimeout;
  return event;
}
Beispiel #2
0
void Infrared_t::IRxTask() {
    msg_t Msg;
    while(1) {
        // Fetch byte from queue
        if(chMBFetch(&imailbox, &Msg, IBitDelay) == RDY_OK) {
            //Uart.Printf("%u\r", Msg);
            PieceType_t Piece = ProcessInterval(Msg);
            switch(Piece) {
                case ptHeader: IStartPkt(); break;
                case ptOne:
                    if(IReceivingData) IAppend(1);
                    else ICancelPkt();
                    break;
                case ptZero:
                    if(IReceivingData) IAppend(0);
                    else ICancelPkt();
                    break;
                default: ICancelPkt(); break;
            } // switch
            // Check if Rx completed
            if(IBitCnt == 14) {
                ICancelPkt();
                RxWord = IRxW << 2;
                chEvtBroadcast(&IEvtSrcIrRx);
            } // if completed
        } // if msg
    } // while 1
}
static msg_t logServerThrd(void *arg)
{
	int i;

	(void) arg;
	chRegSetThreadName("log");

	chprintf((BaseChannel *) &SD2, "Log server started\r\n");

	for (i = 0; i < LOG_MSG_MP_SIZE; i++)
	{
		chPoolFree(&logMP, logMPbuffer[i]);
	}

	msg_t m, res;

	while (TRUE)
	{
		res = chMBFetch(&logMB, &m, TIME_INFINITE );
		if (res == RDY_OK)
		{
			chprintf((BaseChannel *) &SD2, (char *) m);
			chPoolFree(&logMP, (void *) m);
		}
	}

	return -1;
}
Beispiel #4
0
uint8_t pubA(void){

    uint8_t toSend;

    if(chMBFetch(&serialMbox, (msg_t *)&toSend, TIME_INFINITE) == MSG_OK)
    return toSend;
}
Beispiel #5
0
static msg_t tUsbTx(void *arg) {
  (void)arg;
  chRegSetThreadName("usbTx");


  msg_t msg;
  usbPacket *usbBufp;

  enum {UsbTxComleteID = 0, UsbResetID = 1, UsbConfiguredID = 2};

  EventListener elUsbTxComplete;
  EventListener elUsbReset;
  EventListener elUsbConfigured;

  eventmask_t   activeEvents;


  chEvtRegister(&esUsbTxComplete, &elUsbTxComplete, UsbTxComleteID);
  chEvtRegister(&esUsbReset, &elUsbReset, UsbResetID);
  chEvtRegister(&esUsbConfigured, &elUsbConfigured, UsbConfiguredID);

  // Wait for the USB system to be configured. and clear all other event flags.
  chEvtWaitOne(EVENT_MASK(UsbConfiguredID));
  chEvtGetAndClearEvents(EVENT_MASK(UsbTxComleteID) | EVENT_MASK(UsbResetID));

  while (TRUE) {
    chMBFetch (&usbTXMailbox, &msg, TIME_INFINITE);

    // Check if USB has been reconfigured while waiting for message from sysctrl
    activeEvents = chEvtGetAndClearEvents(EVENT_MASK(UsbConfiguredID));
    if (activeEvents == EVENT_MASK(UsbConfiguredID)) {
      // If so, clear the reset event since it is no longer relevant.
      activeEvents = chEvtGetAndClearEvents(EVENT_MASK(UsbResetID));
    }

    // Typecast Mailbox message to command package pointer for readability
    usbBufp = (usbPacket*)msg;

    // Prepare transmit and start the transmission. This operation will return immediately
    usbPrepareTransmit(usbp, EP_IN, usbBufp->packet, (size_t)usbBufp->size);
    chSysLock();
    usbStartTransmitI(usbp, EP_IN);
    chSysUnlock();

    //Check for events from the USB system.
    activeEvents = chEvtWaitAny(EVENT_MASK(UsbTxComleteID) | EVENT_MASK(UsbResetID));

    if (activeEvents == EVENT_MASK(UsbResetID)) {
      chEvtWaitOne(EVENT_MASK(UsbConfiguredID));
      // Clear any events that has occurred while the usb was not configured.
      chEvtGetAndClearEvents(EVENT_MASK(UsbTxComleteID) | EVENT_MASK(UsbResetID));
    }
    usbFreeMailboxBuffer (usbBufp);
  }
  return 0;
}
Beispiel #6
0
/*
 * Test worker threads.
 */
static THD_FUNCTION(irq_storm_thread, arg) {
  static volatile unsigned x = 0;
  static unsigned cnt = 0;
  unsigned me = (unsigned)arg;
  unsigned target;
  unsigned r;
  msg_t msg;

  chRegSetThreadName("irq_storm");

  /* Thread loop, until terminated.*/
  while (chThdShouldTerminateX() == false) {

    /* Waiting for a message.*/
   chMBFetch(&mb[me], &msg, TIME_INFINITE);

#if IRQ_STORM_CFG_RANDOMIZE != FALSE
   /* Pseudo-random delay.*/
   {
     chSysLock();
     r = rand() & 15;
     chSysUnlock();
     while (r--)
       x++;
   }
#else /* IRQ_STORM_CFG_RANDOMIZE == FALSE */
   /* Fixed delay.*/
   {
     r = me >> 4;
     while (r--)
       x++;
   }
#endif /* IRQ_STORM_CFG_RANDOMIZE == FALSE */

    /* Deciding in which direction to re-send the message.*/
    if (msg == MSG_SEND_LEFT)
      target = me - 1;
    else
      target = me + 1;

    if (target < IRQ_STORM_CFG_NUM_THREADS) {
      /* If this thread is not at the end of a chain re-sending the message,
         note this check works because the variable target is unsigned.*/
      msg = chMBPost(&mb[target], msg, TIME_IMMEDIATE);
      if (msg != MSG_OK)
        saturated = TRUE;
    }
    else {
      /* Provides a visual feedback about the system.*/
      if (++cnt >= 500) {
        cnt = 0;
        palTogglePad(config->port, config->pad);
      }
    }
  }
}
Beispiel #7
0
static msg_t WorkerThread(void *arg) {
  static volatile unsigned x = 0;
  static unsigned cnt = 0;
  unsigned me = (unsigned)arg;
  unsigned target;
  unsigned r;
  msg_t msg;

  chRegSetThreadName("worker");

  /* Work loop.*/
  while (TRUE) {
    /* Waiting for a message.*/
   chMBFetch(&mb[me], &msg, TIME_INFINITE);

#if RANDOMIZE
   /* Pseudo-random delay.*/
   {
     chSysLock();
     r = rand() & 15;
     chSysUnlock();
     while (r--)
       x++;
   }
#else
   /* Fixed delay.*/
   {
     r = me >> 4;
     while (r--)
       x++;
   }
#endif

    /* Deciding in which direction to re-send the message.*/
    if (msg == MSG_SEND_LEFT)
      target = me - 1;
    else
      target = me + 1;

    if (target < NUM_THREADS) {
      /* If this thread is not at the end of a chain re-sending the message,
         note this check works because the variable target is unsigned.*/
      msg = chMBPost(&mb[target], msg, TIME_IMMEDIATE);
      if (msg != RDY_OK)
        saturated = TRUE;
    }
    else {
      /* Provides a visual feedback about the system.*/
      if (++cnt >= 500) {
        cnt = 0;
        palTogglePad(GPIO0, GPIO0_LED2);
      }
    }
  }
}
Beispiel #8
0
static void test_009_003_execute(void) {
  msg_t msg1, msg2;
  unsigned i;

  /* [9.3.1] Filling the mailbox.*/
  test_set_step(1);
  {
    for (i = 0; i < MB_SIZE; i++) {
      msg1 = chMBPost(&mb1, 'B' + i, TIME_INFINITE);
      test_assert(msg1 == MSG_OK, "wrong wake-up message");
    }
  }

  /* [9.3.2] Testing chMBPost(), chMBPostI(), chMBPostAhead() and
     chMBPostAheadI() timeout.*/
  test_set_step(2);
  {
    msg1 = chMBPost(&mb1, 'X', 1);
    test_assert(msg1 == MSG_TIMEOUT, "wrong wake-up message");
    chSysLock();
    msg1 = chMBPostI(&mb1, 'X');
    chSysUnlock();
    test_assert(msg1 == MSG_TIMEOUT, "wrong wake-up message");
    msg1 = chMBPostAhead(&mb1, 'X', 1);
    test_assert(msg1 == MSG_TIMEOUT, "wrong wake-up message");
    chSysLock();
    msg1 = chMBPostAheadI(&mb1, 'X');
    chSysUnlock();
    test_assert(msg1 == MSG_TIMEOUT, "wrong wake-up message");
  }

  /* [9.3.3] Resetting the mailbox. The mailbox is then returned in
     active state.*/
  test_set_step(3);
  {
    chMBReset(&mb1);
    chMBResumeX(&mb1);
  }

  /* [9.3.4] Testing chMBFetch() and chMBFetchI() timeout.*/
  test_set_step(4);
  {
    msg1 = chMBFetch(&mb1, &msg2, 1);
    test_assert(msg1 == MSG_TIMEOUT, "wrong wake-up message");
    chSysLock();
    msg1 = chMBFetchI(&mb1, &msg2);
    chSysUnlock();
    test_assert(msg1 == MSG_TIMEOUT, "wrong wake-up message");
  }
}
/**
 * @brief   Put one 'FrameStruct' type pointer into the mailbox
 * @details This function is one of the APIs between the NetworkLayer and
 *          the DataLinkLayer. When a packet need to be sent the NWL will
 *          call this function individually.
 *
 * @param[in] driver    DataLinkLayer driver structure
 * @param[in] frame     The frame which need to be put into the mailbox
 *
 */
msg_t DLLPutFrameInQueue(DLLDriver *dllp, FrameStruct *Frame){
  void *pbuf;
  msg_t ReturnValue = chMBFetch(&dllp->DLLBuffers.DLLFreeOutputBuffer, (msg_t *)&pbuf, TIME_INFINITE);
  if (ReturnValue == MSG_OK) {
    FrameStruct *Temp = pbuf;
    int i;
    for(i = 0; i < FRAME_SIZE_BYTE; i++)
      ((char *)Temp)[i] = ((char *)Frame)[i];

    Temp->CrcHex = CreateCRC(Temp);

    (void)chMBPost(&dllp->DLLBuffers.DLLFilledOutputBuffer, (msg_t)pbuf, TIME_INFINITE);
  }
  return ReturnValue;
}
Beispiel #10
0
static msg_t CmdThread(void* arg){
  chRegSetThreadName("MAV_cmd_exec");
  (void)arg;
  msg_t tmp = 0;
  msg_t status = 0;
  Mail *input_mail = NULL;

  while (TRUE) {
    status = chMBFetch(&mavlink_command_long_mb, &tmp, TIME_INFINITE);
    (void)status;
    input_mail = (Mail*)tmp;
    process_cmd((mavlink_command_long_t *)input_mail->payload);
    input_mail->payload = NULL;
  }

  return 0;
}
static THD_FUNCTION(ThreadSRD1, arg)
{
    SensorReadDriver *srdp = (SensorReadDriver *)arg;
    msg_t message;
    generic_sensor_t *senp;
#if SRD_DEBUG
    chRegSetThreadName("ThreadSRD1");
#endif

    while (1)
    {
        /* Get the latest sensor read request */
        chMBFetch(&srdp->srd_mailbox, &message, TIME_INFINITE);

        /* Convert the message to a sensor pointer */
        senp = (generic_sensor_t *)message;

        /* Call its read function */
        senp->read_sensor(senp->params);
    }
}
Beispiel #12
0
// the stepper thread processes incoming setpoint commands in order of arrival
// there's no lookahead, just take each command and do it as well as possible
static msg_t stepperTh (void*) {
    systime_t timeout = TIME_IMMEDIATE;
    for (;;) {
        msg_t spIndex;
        if (chMBFetch(&setpoint.mailbox, &spIndex, timeout) == RDY_OK) {
            #ifdef GPIO1_OLI_LED1
                palClearPad(GPIO1, GPIO1_OLI_LED1); // active low, on
            #endif
            motionTarget(setpoint.setpoints[spIndex]);
            clearInUse(spIndex);
            timeout = TIME_IMMEDIATE;
        } else {
            // there is no work, stop the stepper and wait for next cmd
            motionStop();
            #ifdef GPIO1_OLI_LED1
                palSetPad(GPIO1, GPIO1_OLI_LED1); // active low, off
            #endif
            timeout = TIME_INFINITE;
        }
    }
    return 0;
}
/**
 * @brief   Continuous serial sending thread.
 * @details The SDSending thread responsible for the continuous frame sending
 *          via serial. It receives the frames from the application through a
 *          mailbox.
 */
static THD_FUNCTION(SDSending, arg) {
  chRegSetThreadName("Sending Thread");
  DLLDriver *dllp = arg;
  void *pbuf;
  FrameStruct *Temp;
  while(true)
  {
      dllp->DLLStats.FreeFilledBuffer = chMBGetFreeCountI(&dllp->DLLBuffers.DLLFilledOutputBuffer);
      dllp->DLLStats.FreeFreeBuffer = chMBGetFreeCountI(&dllp->DLLBuffers.DLLFreeOutputBuffer);
      msg_t msg = chMBFetch(&dllp->DLLBuffers.DLLFilledOutputBuffer, (msg_t *)&pbuf, TIME_INFINITE);

      if(msg == MSG_OK)
      {
        Temp = pbuf;
        if(DLLSendSingleFrameSerial(dllp, Temp))
          dllp->DLLStats.SentFrames++;
        else
          dllp->DLLStats.LostFrames++;
        (void)chMBPost(&dllp->DLLBuffers.DLLFreeOutputBuffer, (msg_t)pbuf, TIME_INFINITE);
      }
  }
}
Beispiel #14
0
	static msg_t GDISPThreadHandler(void *arg) {
		(void)arg;
		gdisp_lld_msg_t	*pmsg;

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

		while(1) {
			/* Wait for msg with work to do. */
			chMBFetch(&gdispMailbox, (msg_t *)&pmsg, TIME_INFINITE);

			/* OK - we need to obtain the mutex in case a synchronous operation is occurring */
			chMtxLock(&gdispMutex);
			gdisp_lld_msg_dispatch(pmsg);
			chMtxUnlock();

			/* Mark the message as free */
			pmsg->action = GDISP_LLD_MSG_NOP;
			chSemSignal(&gdispMsgsSem);
		}
		return 0;
	}
Beispiel #15
0
/**
 *
 * @brief   Retrieves an item from the front of a queue.
 *
 * @param[in] queuep       pointer to instance of @p struct pios_queue
 * @param[in] itemp        pointer to item which will be retrieved
 * @param[in] timeout_ms   timeout for retrieving item from queue in milliseconds
 *
 * @returns true on success or false on timeout or failure
 *
 */
bool PIOS_Queue_Receive(struct pios_queue *queuep, void *itemp, uint32_t timeout_ms)
{
	msg_t buf;

	systime_t timeout;
	if (timeout_ms == PIOS_QUEUE_TIMEOUT_MAX)
		timeout = TIME_INFINITE;
	else if (timeout_ms == 0)
		timeout = TIME_IMMEDIATE;
	else
		timeout = MS2ST(timeout_ms);

	msg_t result = chMBFetch(&queuep->mb, &buf, timeout);

	if (result != RDY_OK)
		return false;

	memcpy(itemp, (void*)buf, queuep->mp.mp_object_size);

	chPoolFree(&queuep->mp, (void*)buf);

	return true;
}
Beispiel #16
0
static void test_008_001_execute(void) {
  msg_t msg1, msg2;
  unsigned i;

  /* [8.1.1] Testing the mailbox size.*/
  test_set_step(1);
  {
    test_assert_lock(chMBGetFreeCountI(&mb1) == MB_SIZE, "wrong size");
  }

  /* [8.1.2] Resetting the mailbox, conditions are checked, no errors
     expected.*/
  test_set_step(2);
  {
    chMBReset(&mb1);
    test_assert_lock(chMBGetFreeCountI(&mb1) == MB_SIZE, "not empty");
    test_assert_lock(chMBGetUsedCountI(&mb1) == 0, "still full");
    test_assert_lock(mb1.buffer == mb1.wrptr, "write pointer not aligned to base");
    test_assert_lock(mb1.buffer == mb1.rdptr, "read pointer not aligned to base");
  }

  /* [8.1.3] Filling the mailbox using chMBPost() and chMBPostAhead()
     once, no errors expected.*/
  test_set_step(3);
  {
    for (i = 0; i < MB_SIZE - 1; i++) {
      msg1 = chMBPost(&mb1, 'B' + i, TIME_INFINITE);
      test_assert(msg1 == MSG_OK, "wrong wake-up message");
    }
    msg1 = chMBPostAhead(&mb1, 'A', TIME_INFINITE);
    test_assert(msg1 == MSG_OK, "wrong wake-up message");
  }

  /* [8.1.4] Testing intermediate conditions. Data pointers must be
     aligned, semaphore counters are checked.*/
  test_set_step(4);
  {
    test_assert_lock(chMBGetFreeCountI(&mb1) == 0, "still empty");
    test_assert_lock(chMBGetUsedCountI(&mb1) == MB_SIZE, "not full");
    test_assert_lock(mb1.rdptr == mb1.wrptr, "pointers not aligned");
  }

  /* [8.1.5] Emptying the mailbox using chMBFetch(), no errors
     expected.*/
  test_set_step(5);
  {
    for (i = 0; i < MB_SIZE; i++) {
      msg1 = chMBFetch(&mb1, &msg2, TIME_INFINITE);
      test_assert(msg1 == MSG_OK, "wrong wake-up message");
      test_emit_token(msg2);
    }
    test_assert_sequence("ABCD", "wrong get sequence");
  }

  /* [8.1.6] Posting and then fetching one more message, no errors
     expected.*/
  test_set_step(6);
  {
    msg1 = chMBPost(&mb1, 'B' + i, TIME_INFINITE);
    test_assert(msg1 == MSG_OK, "wrong wake-up message");
    msg1 = chMBFetch(&mb1, &msg2, TIME_INFINITE);
    test_assert(msg1 == MSG_OK, "wrong wake-up message");
  }

  /* [8.1.7] Testing final conditions. Data pointers must be aligned to
     buffer start, semaphore counters are checked.*/
  test_set_step(7);
  {
    test_assert_lock(chMBGetFreeCountI(&mb1) == MB_SIZE, "not empty");
    test_assert_lock(chMBGetUsedCountI(&mb1) == 0, "still full");
    test_assert(mb1.buffer == mb1.wrptr, "write pointer not aligned to base");
    test_assert(mb1.buffer == mb1.rdptr, "read pointer not aligned to base");
  }
}
Beispiel #17
0
void Sound_t::ISendNextData() {
//    Uart.Printf("sn\r");
    IDreq.DisableIrq();
    dmaStreamDisable(VS_DMA);
    IDmaIdle = false;
    // If command queue is not empty, send command
    msg_t msg = chMBFetch(&CmdBox, &ICmd.Msg, TIME_IMMEDIATE);
    if(msg == RDY_OK) {
//        Uart.PrintfI("\rvCmd: %A\r", &ICmd, 4, ' ');
        XCS_Lo();   // Start Cmd transmission
        chThdSleepMilliseconds(1);
        dmaStreamSetMemory0(VS_DMA, &ICmd);
        dmaStreamSetTransactionSize(VS_DMA, sizeof(VsCmd_t));
        dmaStreamSetMode(VS_DMA, VS_DMA_MODE | STM32_DMA_CR_MINC);  // Memory pointer increase
        dmaStreamEnable(VS_DMA);
    }
    // Send next chunk of data if any
    else if(State == sndPlaying) {
//        Uart.PrintfI("\rD");
        // Send data if buffer is not empty
        if(PBuf->DataSz != 0) {
            XDCS_Lo();  // Start data transmission
            uint32_t FLength = (PBuf->DataSz > 32)? 32 : PBuf->DataSz;
            dmaStreamSetMemory0(VS_DMA, PBuf->PData);
            dmaStreamSetTransactionSize(VS_DMA, FLength);
            dmaStreamSetMode(VS_DMA, VS_DMA_MODE | STM32_DMA_CR_MINC);  // Memory pointer increase
            dmaStreamEnable(VS_DMA);
            // Process pointers and lengths
            PBuf->DataSz -= FLength;
            PBuf->PData += FLength;
        }
        else IDmaIdle = true;   // Will come true if both buffers are empty

        // Check if buffer is now empty
        if(PBuf->DataSz == 0) {
            // Prepare to read next chunk
//            Uart.Printf("*");
            chSysLock();
            chEvtSignalI(PThread, VS_EVT_READ_NEXT);
            chSysUnlock();
            // Switch to next buf
            PBuf = (PBuf == &Buf1)? &Buf2 : &Buf1;
        }
    }
    else if(State == sndWritingZeroes) {
//        Uart.Printf("\rZ");
        if(ZeroesCount == 0) { // Was writing zeroes, now all over
            State = sndStopped;
            IDmaIdle = true;
//            Uart.Printf("vEnd\r");
            chSysLock();
            chEvtSignalI(PThread, VS_EVT_COMPLETED);
            chSysUnlock();
        }
        else SendZeroes();
    }
    else {
//    	Uart.PrintfI("\rI");
        if(!IDreq.IsHi()) IDreq.EnableIrq(IRQ_PRIO_MEDIUM);
        else IDmaIdle = true;
    }
}
static void mbox1_execute(void) {
  msg_t msg1, msg2;
  unsigned i;

  /*
   * Testing initial space.
   */
  test_assert(1, chMBGetEmpty(&mb1) == MB_SIZE, "wrong size");

  /*
   * Testing enqueuing and backward circularity.
   */
  for (i = 0; i < MB_SIZE - 1; i++) {
    msg1 = chMBPost(&mb1, 'B' + i, TIME_INFINITE);
    test_assert(2, msg1 == RDY_OK, "wrong wake-up message");
  }
  msg1 = chMBPostAhead(&mb1, 'A', TIME_INFINITE);
  test_assert(3, msg1 == RDY_OK, "wrong wake-up message");

  /*
   * Testing post timeout.
   */
  msg1 = chMBPost(&mb1, 'X', 1);
  test_assert(4, msg1 == RDY_TIMEOUT, "wrong wake-up message");

  /*
   * Testing final conditions.
   */
  test_assert(5, chMBGetEmpty(&mb1) == 0, "still empty");
  test_assert(6, chMBGetFull(&mb1) == MB_SIZE, "not full");
  test_assert(7, mb1.mb_rdptr == mb1.mb_wrptr, "pointers not aligned");

  /*
   * Testing dequeuing.
   */
  for (i = 0; i < MB_SIZE; i++) {
  msg1 = chMBFetch(&mb1, &msg2, TIME_INFINITE);
    test_assert(8, msg1 == RDY_OK, "wrong wake-up message");
    test_emit_token(msg2);
  }
  test_assert_sequence(9, "ABCDE");

  /*
   * Testing buffer circularity.
   */
  msg1 = chMBPost(&mb1, 'B' + i, TIME_INFINITE);
  test_assert(10, msg1 == RDY_OK, "wrong wake-up message");
  msg1 = chMBFetch(&mb1, &msg2, TIME_INFINITE);
  test_assert(11, msg1 == RDY_OK, "wrong wake-up message");
  test_assert(12, mb1.mb_buffer == mb1.mb_wrptr, "write pointer not aligned to base");
  test_assert(13, mb1.mb_buffer == mb1.mb_rdptr, "read pointer not aligned to base");

  /*
   * Testing fetch timeout.
   */
  msg1 = chMBFetch(&mb1, &msg2, 1);
  test_assert(14, msg1 == RDY_TIMEOUT, "wrong wake-up message");

  /*
   * Testing final conditions.
   */
  test_assert(15, chMBGetEmpty(&mb1) == MB_SIZE, "not empty");
  test_assert(16, chMBGetFull(&mb1) == 0, "still full");
  test_assert(17, mb1.mb_rdptr == mb1.mb_wrptr, "pointers not aligned");

  /*
   * Testing reset.
   */
  chMBReset(&mb1);

  /*
   * Re-testing final conditions.
   */
  test_assert(18, chMBGetEmpty(&mb1) == MB_SIZE, "not empty");
  test_assert(19, chMBGetFull(&mb1) == 0, "still full");
  test_assert(20, mb1.mb_buffer == mb1.mb_wrptr, "write pointer not aligned to base");
  test_assert(21, mb1.mb_buffer == mb1.mb_rdptr, "read pointer not aligned to base");
}
Beispiel #19
0
msg_t Mailbox::fetch(msg_t *msgp, systime_t time) {

    return chMBFetch(&mb, msgp, time);
}
Beispiel #20
0
void Sound_t::ISendNextData() {
//    Uart.Printf("\rSN");
    dmaStreamDisable(VS_DMA);
    IDmaIdle = false;
    // ==== If command queue is not empty, send command ====
    msg_t msg = chMBFetch(&CmdBox, &ICmd.Msg, TIME_IMMEDIATE);
    if(msg == RDY_OK) {
//        Uart.PrintfI("\rvCmd: %A", &ICmd, 4, ' ');
        XCS_Lo();   // Start Cmd transmission
        dmaStreamSetMemory0(VS_DMA, &ICmd);
        dmaStreamSetTransactionSize(VS_DMA, sizeof(VsCmd_t));
        dmaStreamSetMode(VS_DMA, VS_DMA_MODE | STM32_DMA_CR_MINC);  // Memory pointer increase
        dmaStreamEnable(VS_DMA);
    }
    // ==== Send next chunk of data if any ====
    else switch(State) {
        case sndPlaying: {
//            Uart.PrintfI("\rD");
            // Switch buffer if required
            if(PBuf->DataSz == 0) {
                PBuf = (PBuf == &Buf1)? &Buf2 : &Buf1;      // Switch to next buf
//                Uart.Printf("\rB=%u; Sz=%u", ((PBuf == &Buf1)? 1 : 2), PBuf->DataSz);
                if(PBuf->DataSz == 0) { // Previous attempt to read the file failed
                    IDmaIdle = true;
                    PrepareToStop();
                    break;
                }
                else {
                    chSysLock();
                    chEvtSignalI(PThread, VS_EVT_READ_NEXT);    // Read next chunk of file
                    chSysUnlock();
                }
            }
            // Send next piece of data
            XDCS_Lo();  // Start data transmission
            uint32_t FLength = (PBuf->DataSz > 32)? 32 : PBuf->DataSz;
            dmaStreamSetMemory0(VS_DMA, PBuf->PData);
            dmaStreamSetTransactionSize(VS_DMA, FLength);
            dmaStreamSetMode(VS_DMA, VS_DMA_MODE | STM32_DMA_CR_MINC);  // Memory pointer increase
            dmaStreamEnable(VS_DMA);
//            if(PBuf == &Buf1) Uart.Printf("*"); else Uart.Printf("#");
            // Process pointers and lengths
            PBuf->DataSz -= FLength;
            PBuf->PData += FLength;
        } break;

        case sndWritingZeroes:
//            Uart.Printf("\rZ");
            if(ZeroesCount == 0) { // Was writing zeroes, now all over
                State = sndStopped;
                IDmaIdle = true;
//                Uart.Printf("\rvEnd");
                chSysLock();
                chEvtSignalI(PThread, VS_EVT_COMPLETED);
                chSysUnlock();
            }
            else SendZeroes();
            break;

        case sndStopped:
//            Uart.PrintfI("\rI");
            if(!IDreq.IsHi()) IDreq.EnableIrq(IRQ_PRIO_MEDIUM);
            else IDmaIdle = true;
    } // switch
}
Beispiel #21
0
u32_t sys_arch_mbox_tryfetch(sys_mbox_t *mbox, void **msg) {

  if (chMBFetch(*mbox, (msg_t *)msg, TIME_IMMEDIATE) == MSG_TIMEOUT)
    return SYS_MBOX_EMPTY;
  return 0;
}
Beispiel #22
0
static void mbox1_execute(void) {
  msg_t msg1, msg2;
  unsigned i;

  /*
   * Testing initial space.
   */
  test_assert_lock(1, chMBGetFreeCountI(&mb1) == MB_SIZE, "wrong size");

  /*
   * Testing enqueuing and backward circularity.
   */
  for (i = 0; i < MB_SIZE - 1; i++) {
    msg1 = chMBPost(&mb1, 'B' + i, TIME_INFINITE);
    test_assert(2, msg1 == MSG_OK, "wrong wake-up message");
  }
  msg1 = chMBPostAhead(&mb1, 'A', TIME_INFINITE);
  test_assert(3, msg1 == MSG_OK, "wrong wake-up message");

  /*
   * Testing post timeout.
   */
  msg1 = chMBPost(&mb1, 'X', 1);
  test_assert(4, msg1 == MSG_TIMEOUT, "wrong wake-up message");
  chSysLock();
  msg1 = chMBPostI(&mb1, 'X');
  chSysUnlock();
  test_assert(5, msg1 == MSG_TIMEOUT, "wrong wake-up message");
  msg1 = chMBPostAhead(&mb1, 'X', 1);
  test_assert(6, msg1 == MSG_TIMEOUT, "wrong wake-up message");
  chSysLock();
  msg1 = chMBPostAheadI(&mb1, 'X');
  chSysUnlock();
  test_assert(7, msg1 == MSG_TIMEOUT, "wrong wake-up message");

  /*
   * Testing final conditions.
   */
  test_assert_lock(8, chMBGetFreeCountI(&mb1) == 0, "still empty");
  test_assert_lock(9, chMBGetUsedCountI(&mb1) == MB_SIZE, "not full");
  test_assert_lock(10, mb1.rdptr == mb1.wrptr, "pointers not aligned");

  /*
   * Testing dequeuing.
   */
  for (i = 0; i < MB_SIZE; i++) {
    msg1 = chMBFetch(&mb1, &msg2, TIME_INFINITE);
    test_assert(11, msg1 == MSG_OK, "wrong wake-up message");
    test_emit_token(msg2);
  }
  test_assert_sequence(12, "ABCDE");

  /*
   * Testing buffer circularity.
   */
  msg1 = chMBPost(&mb1, 'B' + i, TIME_INFINITE);
  test_assert(13, msg1 == MSG_OK, "wrong wake-up message");
  msg1 = chMBFetch(&mb1, &msg2, TIME_INFINITE);
  test_assert(14, msg1 == MSG_OK, "wrong wake-up message");
  test_assert(15, mb1.buffer == mb1.wrptr, "write pointer not aligned to base");
  test_assert(16, mb1.buffer == mb1.rdptr, "read pointer not aligned to base");

  /*
   * Testing fetch timeout.
   */
  msg1 = chMBFetch(&mb1, &msg2, 1);
  test_assert(17, msg1 == MSG_TIMEOUT, "wrong wake-up message");
  chSysLock();
  msg1 = chMBFetchI(&mb1, &msg2);
  chSysUnlock();
  test_assert(18, msg1 == MSG_TIMEOUT, "wrong wake-up message");

  /*
   * Testing final conditions.
   */
  test_assert_lock(19, chMBGetFreeCountI(&mb1) == MB_SIZE, "not empty");
  test_assert_lock(20, chMBGetUsedCountI(&mb1) == 0, "still full");
  test_assert_lock(21, mb1.rdptr == mb1.wrptr, "pointers not aligned");

  /*
   * Testing I-Class.
   */
  chSysLock();
  msg1 = chMBPostI(&mb1, 'A');
  test_assert(22, msg1 == MSG_OK, "wrong wake-up message");
  msg1 = chMBPostI(&mb1, 'B');
  test_assert(23, msg1 == MSG_OK, "wrong wake-up message");
  msg1 = chMBPostI(&mb1, 'C');
  test_assert(24, msg1 == MSG_OK, "wrong wake-up message");
  msg1 = chMBPostI(&mb1, 'D');
  test_assert(25, msg1 == MSG_OK, "wrong wake-up message");
  msg1 = chMBPostI(&mb1, 'E');
  chSysUnlock();
  test_assert(26, msg1 == MSG_OK, "wrong wake-up message");
  test_assert(27, mb1.rdptr == mb1.wrptr, "pointers not aligned");
  for (i = 0; i < MB_SIZE; i++) {
    chSysLock();
    msg1 = chMBFetchI(&mb1, &msg2);
    chSysUnlock();
    test_assert(28, msg1 == MSG_OK, "wrong wake-up message");
    test_emit_token(msg2);
  }
  test_assert_sequence(29, "ABCDE");
  test_assert_lock(30, chMBGetFreeCountI(&mb1) == MB_SIZE, "not empty");
  test_assert_lock(31, chMBGetUsedCountI(&mb1) == 0, "still full");
  test_assert(32, mb1.rdptr == mb1.wrptr, "pointers not aligned");

  chSysLock();
  msg1 = chMBPostAheadI(&mb1, 'E');
  test_assert(33, msg1 == MSG_OK, "wrong wake-up message");
  msg1 = chMBPostAheadI(&mb1, 'D');
  test_assert(34, msg1 == MSG_OK, "wrong wake-up message");
  msg1 = chMBPostAheadI(&mb1, 'C');
  test_assert(35, msg1 == MSG_OK, "wrong wake-up message");
  msg1 = chMBPostAheadI(&mb1, 'B');
  test_assert(36, msg1 == MSG_OK, "wrong wake-up message");
  msg1 = chMBPostAheadI(&mb1, 'A');
  chSysUnlock();
  test_assert(37, msg1 == MSG_OK, "wrong wake-up message");
  test_assert(38, mb1.rdptr == mb1.wrptr, "pointers not aligned");
  for (i = 0; i < MB_SIZE; i++) {
    chSysLock();
    msg1 = chMBFetchI(&mb1, &msg2);
    chSysUnlock();
    test_assert(39, msg1 == MSG_OK, "wrong wake-up message");
    test_emit_token(msg2);
  }
  test_assert_sequence(40, "ABCDE");
  test_assert_lock(41, chMBGetFreeCountI(&mb1) == MB_SIZE, "not empty");
  test_assert_lock(42, chMBGetUsedCountI(&mb1) == 0, "still full");
  test_assert(43, mb1.rdptr == mb1.wrptr, "pointers not aligned");

  /*
   * Testing reset.
   */
  chMBReset(&mb1);

  /*
   * Re-testing final conditions.
   */
  test_assert_lock(44, chMBGetFreeCountI(&mb1) == MB_SIZE, "not empty");
  test_assert_lock(45, chMBGetUsedCountI(&mb1) == 0, "still full");
  test_assert_lock(46, mb1.buffer == mb1.wrptr, "write pointer not aligned to base");
  test_assert_lock(47, mb1.buffer == mb1.rdptr, "read pointer not aligned to base");
}
Beispiel #23
0
static msg_t eventLoopThread(void *UNUSED(arg))
{

  while (!chThdShouldTerminate()) {
    msg_t msg;
    msg_t ret;
    SC_EVENT_TYPE type;

    chRegSetThreadName(__func__);

    // Wait for action
    ret = chMBFetch(&event_mb, &msg, TIME_INFINITE);

    if (chThdShouldTerminate()) {
      break;
    }

    chDbgAssert(ret == RDY_OK, "chMBFetch failed", "#1");
    if (ret != RDY_OK) {
      continue;
    }

    // Get event type from the message;
    type = EVENT_MSG_GET_TYPE(msg);

    switch(type) {

      // Application registrable events
#if HAL_USE_UART
    case SC_EVENT_TYPE_PUSH_BYTE:
      if (cb_handle_byte != NULL) {
        SC_UART uart = EVENT_MSG_GET_UART(msg);
        uint8_t byte = EVENT_MSG_GET_BYTE(msg);
        cb_handle_byte(uart, byte);
      }
      break;
#endif
#if HAL_USE_EXT
    case SC_EVENT_TYPE_EXTINT:
      {
        uint8_t pin = EVENT_MSG_GET_PIN(msg);
        if (cb_extint[pin] != NULL) {
          cb_extint[pin]();
        }
      }
      break;
#endif
    case SC_EVENT_TYPE_GSM_STATE_CHANGED:
      if (cb_gsm_state_changed != NULL) {
        cb_gsm_state_changed();
      }
      break;
    case SC_EVENT_TYPE_GSM_CMD_DONE:
      if (cb_gsm_cmd_done != NULL) {
        cb_gsm_cmd_done();
      }
      break;
#if HAL_USE_ADC
    case SC_EVENT_TYPE_ADC_AVAILABLE:
      if (cb_adc_available != NULL) {
        cb_adc_available();
      }
      break;
#endif
    case SC_EVENT_TYPE_TEMP_AVAILABLE:
      if (cb_temp_available != NULL) {
        cb_temp_available();
      }
      break;
    case SC_EVENT_TYPE_9DOF_AVAILABLE:
      if (cb_9dof_available != NULL) {
        cb_9dof_available();
      }
      break;
    case SC_EVENT_TYPE_BLOB_AVAILABLE:
      if (cb_blob_available != NULL) {
        cb_blob_available();
      }
      break;
    case SC_EVENT_TYPE_AHRS_AVAILABLE:
      if (cb_ahrs_available != NULL) {
        cb_ahrs_available();
      }
      break;

      // SC internal types
#if HAL_USE_UART
    case SC_EVENT_TYPE_UART_SEND_FINISHED:
      sc_uart_send_finished();
      break;
#endif
    case SC_EVENT_TYPE_NOP:
      // Do nothing
      break;
    default:
      chDbgAssert(0, "Unhandled event", "sc_event_loop");
      break;
    }
  }

  return RDY_OK;
}
Beispiel #24
0
/**
  * @brief  This is the pressure control thread
  * @param  void* to a PID Loops configuration
  * @retval msg_t status
  */
msg_t Pressure_Thread(void *This_Config) {
	/* This thread is passed a pointer to a PID loop configuration */
	PID_State Pressure_PID_Controllers[((Pressure_Config_Type*)This_Config)->Number_Setpoints];
	memset(Pressure_PID_Controllers,0,((Pressure_Config_Type*)This_Config)->Number_Setpoints*sizeof(PID_State));/* Initialise as zeros */
	float* Last_PID_Out=(float*)chHeapAlloc(NULL,sizeof(float)*((Pressure_Config_Type*)This_Config)->Number_Setpoints);/* PID output for interpol */
	adcsample_t Pressure_Samples[PRESSURE_SAMPLES],Pressure_Sample;/* Use multiple pressure samples to drive down the noise */
	float PID_Out,Pressure;//,step=0.01,sawtooth=0.7;
	uint32_t Setpoint=0;
	uint8_t Old_Setpoint=0, Previous_Setpoint;
	chRegSetThreadName("PID_Pressure");
	//palSetGroupMode(GPIOC, PAL_PORT_BIT(5) | PAL_PORT_BIT(4), 0, PAL_MODE_INPUT_ANALOG);
	palSetPadMode(GPIOE, 9, PAL_MODE_ALTERNATE(1));		/* Only set the pin as AF output here, so as to avoid solenoid getting driven earlier*/
	palSetPadMode(GPIOE, 11, PAL_MODE_ALTERNATE(1));	/* Experimental servo output here */
	#ifndef USE_SERVO
	/*
	* Activates the PWM driver
	*/
	pwmStart(&PWM_Driver_Solenoid, &PWM_Config_Solenoid);	/* Have to define the timer to use for PWM_Driver in hardware config */
	/*
	* Set the solenoid PWM to off
	*/
	pwmEnableChannel(&PWM_Driver_Solenoid, (pwmchannel_t)PWM_CHANNEL_SOLENOID, (pwmcnt_t)0);
	#else
	/*
	* Activates the experimental servo driver
	*/
	pwmStart(&PWM_Driver_Servo, &PWM_Config_Servo);		/* Have to define the timer to use for PWM_Driver in hardware config */
	#endif
	/*
	* Activates the ADC2 driver *and the thermal sensor*.
	*/
	adcStart(&ADCD2, NULL);
	//adcSTM32EnableTSVREFE();
	/*
	/ Now we run the sensor offset calibration loop
	*/
	do {
		adcConvert(&ADCD2, &adcgrpcfg1, &Pressure_Sample, 1);/* This function blocks until it has one sample*/
	} while(Calibrate_Sensor((uint16_t)Pressure_Sample));
	systime_t time = chTimeNow();				/* T0 */
	systime_t Interpolation_Timeout = time;			/* Set to T0 to show there is no current interpolation */
	/* Loop for the pressure control thread */
	while(TRUE) {
		/*
		* Linear conversion.
		*/
		adcConvert(&ADCD2, &adcgrpcfg1, Pressure_Samples, PRESSURE_SAMPLES);/* This function blocks until it has the samples via DMA*/
		/*
		/ Now we process the data and apply the PID controller - we use a median filter to take out the non guassian noise
		*/
		Pressure_Sample = quick_select(Pressure_Samples, PRESSURE_SAMPLES);
		Pressure = Convert_Pressure((uint16_t)Pressure_Sample);/* Converts to PSI as a float */
		/* Retrieve a new setpoint from the setpoint mailbox, only continue if we get it*/
		if(chMBFetch(&Pressures_Setpoint, (msg_t*)&Setpoint, TIME_IMMEDIATE) == RDY_OK) {
			//Pressure=Run_Pressure_Filter(Pressure);/* Square root raised cosine filter for low pass with minimal lag */
			Pressure = Pressure<0?0.0:Pressure;	/* A negative pressure is impossible with current hardware setup - disregard*/
			Setpoint &= 0x000000FF;
			/* The controller is built around an interpolated array of independant PID controllers with seperate setpoints */
			if(Setpoint != Old_Setpoint) {		/* The setpoint has changed */
				Previous_Setpoint = Old_Setpoint;/* This is for use by the interpolator */
				Old_Setpoint = Setpoint;	/* Store the setpoint */
				/* Store the time at which the interpolation to new setpoint completes*/
				Interpolation_Timeout = time + (systime_t)( 4.0 / ((Pressure_Config_Type*)This_Config)->Interpolation_Base );
			}
			if(Interpolation_Timeout > time) {	/* If we have an ongoing interpolation  - note, operates in tick units */
				/* Value goes from 1 to -1 */
				float interpol = erff( (float)(Interpolation_Timeout - time) *\
						 ((Pressure_Config_Type*)This_Config)->Interpolation_Base - 2.0 );/* erf function interpolator */
				interpol = ( (-interpol + 1.0) / 2.0);/* Interpolation value goes from 0 to 1 */
				PID_Out = ( Last_PID_Out[Previous_Setpoint] * (1.0 - interpol) ) + ( Last_PID_Out[Setpoint] * interpol );
				Pressure_PID_Controllers[Setpoint].Last_Input = Pressure;/* Make sure the input to next PID controller is continuous */
			}
			else {
				PID_Out = Run_PID_Loop( ((Pressure_Config_Type*)This_Config)->PID_Loop_Config, &Pressure_PID_Controllers[Setpoint],\
						 (((Pressure_Config_Type*)This_Config)->Setpoints)[Setpoint], \
						 Pressure, (float)PRESSURE_TIME_INTERVAL/1000.0);/* Run PID */
				Last_PID_Out[Setpoint] = PID_Out;/* Store for use by the interpolator */
			}
		}
		else
			PID_Out=0;				/* So we can turn off the solenoid simply by failing to send Setpoints */
		PID_Out=PID_Out>1.0?1.0:PID_Out;
		PID_Out=PID_Out<0.0?0.0:PID_Out;		/* Enforce range limits on the PID output */
		//sawtooth+=step;				/* Test code for debugging mechanics with a sawtooth */
		//if(sawtooth>=1 || sawtooth<=0.65)
		//	step=-step;
		//PID_Out=sawtooth;
		#ifndef USE_SERVO
		/*
		/ Now we apply the PID output to the PWM based solenoid controller, and feed data into the mailbox output - Note fractional input
		*/
		pwmEnableChannel(&PWM_Driver_Solenoid, (pwmchannel_t)PWM_CHANNEL_SOLENOID, (pwmcnt_t)PWM_FRACTION_TO_WIDTH(&PWM_Driver_Solenoid, 1000\
														, (uint32_t)(1000.0*PID_Out)));	
		#else
		pwmEnableChannel(&PWM_Driver_Servo, (pwmchannel_t)PWM_CHANNEL_SERVO, (pwmcnt_t)PWM_FRACTION_TO_WIDTH(&PWM_Driver_Servo, 10000\
														, (uint32_t)(1000.0*(PID_Out+1.0))));	
		#endif
		chMBPost(&Pressures_Reported, *((msg_t*)&Pressure), TIME_IMMEDIATE);/* Non blocking write attempt to the Reported Pressure mailbox FIFO */
		/*
		/ The Thread is syncronised to system time
		*/	
		time += MS2ST(PRESSURE_TIME_INTERVAL);		/* Next deadline */
		chThdSleepUntil(time);				/* Gives us a thread with regular timing */
	}
}