/**
* @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;
}
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;
}
uint8_t pubA(void){
uint8_t toSend;
if(chMBFetch(&serialMbox, (msg_t *)&toSend, TIME_INFINITE) == MSG_OK)
return toSend;
}
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;
}
/*
* 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);
}
}
}
}
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);
}
}
}
}
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;
}
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);
}
}
// 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);
}
}
}
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;
}
/**
*
* @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;
}
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");
}
}
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");
}
msg_t Mailbox::fetch(msg_t *msgp, systime_t time) {
return chMBFetch(&mb, msgp, time);
}
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
}
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;
}
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");
}
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;
}
/**
* @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 */
}
}