bool DRV_HasTurned(void) { int16_t pos; if (FRTOS1_uxQueueMessagesWaiting(DRV_Queue)>0) { return FALSE; /* still messages in command queue, so there is something pending */ } if (DRV_Status.mode==DRV_MODE_POS) { #define DRV_TURN_SPEED_LOW 50 int32_t speedL, speedR; speedL = TACHO_GetSpeed(TRUE); speedR = TACHO_GetSpeed(FALSE); if (speedL>-DRV_TURN_SPEED_LOW && speedL<DRV_TURN_SPEED_LOW && speedR>-DRV_TURN_SPEED_LOW && speedR<DRV_TURN_SPEED_LOW) { /* speed close to zero */ pos = Q4CLeft_GetPos(); if (match(pos, DRV_Status.pos.left)) { pos = Q4CRight_GetPos(); if (match(pos, DRV_Status.pos.right)) { return TRUE; } } } return FALSE; } /* if */ return TRUE; }
bool DRV_IsStopped(void) { Q4CLeft_QuadCntrType leftPos; Q4CRight_QuadCntrType rightPos; if (FRTOS1_uxQueueMessagesWaiting(DRV_Queue)>0) { return FALSE; /* still messages in command queue, so there is something pending */ } /* do *not* use/calculate speed: too slow! Use position encoder instead */ leftPos = Q4CLeft_GetPos(); rightPos = Q4CRight_GetPos(); if (DRV_Status.mode==DRV_MODE_POS) { if (DRV_Status.pos.left!=(int32_t)leftPos) { return FALSE; } if (DRV_Status.pos.right!=(int32_t)rightPos) { return FALSE; } return TRUE; } else { /* ???? what to do otherwise ???? */ return FALSE; } }
unsigned short SQUEUE_NofElements(void) { return (unsigned short)FRTOS1_uxQueueMessagesWaiting(SQUEUE_Queue); }
unsigned short QUEUE_NofElements(xQueueHandle queue) { return (unsigned short)FRTOS1_uxQueueMessagesWaiting(queue); }
/*! * \brief Peeks a character from the queue without removing it. * \param queue Queue to be used * \return '\0' if the queue is empty, otherwise it contains the character. */ static unsigned short RSTDIO_NofElements(xQueueHandle queue) { return (unsigned short)FRTOS1_uxQueueMessagesWaiting(queue); }