/** \brief get a packet from the packet pool, but if the
* pool is empty, don't wait, just return NULL
*/
Packet *PacketPoolGetPacket(void) {
if (RingBufferIsEmpty(ringbuffer))
return NULL;
Packet *p = RingBufferMrMwGetNoWait(ringbuffer);
return p;
}
void SendBytesInterrupt(void)
{
ASSERT(IsrIsAtomic(IRQ_LEVEL_SERIAL_WRITE));
while (!RingBufferIsEmpty(&SerialOutputRing)) {
char data;
ASSUME(RingBufferRead(&data, sizeof(data), &SerialOutputRing), 1);
HalSerialWriteChar(data);
}
}
// Try and decode a frame from com state
void CommunicationHandleFrame(CommunicationState *com) {
int keep_decoding = 1;
while(keep_decoding && !RingBufferIsEmpty(&com->uart_in_ringbuffer)) {
BufferClear(&com->deframed_buffer);
keep_decoding = afproto_ringbuffer_pop_frame(&com->uart_in_ringbuffer,
&com->deframed_buffer);
if(com->deframed_buffer.used > 0)
CommandHandleRaw(com->deframed_buffer.data,
com->deframed_buffer.used);
}
}
//
//Public routines
//
COUNT RingBufferRead( char * buff, COUNT size, struct RING_BUFFER * ring )
{
COUNT read=0;
while( read < size && ! RingBufferIsEmpty( ring ) )
{
read += RingBufferReadSmall( buff+read, size-read, ring );
if( ring->ReadIndex == ring->WriteIndex )
{
ring->Empty = TRUE;
}
}
return read;
}
COUNT PipeReadInner( char * buff, COUNT size, PIPE_READ pipe ) {
BOOL wasFull;
BOOL dataLeft;
COUNT read;
//Acquire EmptyLock - No readers can progress until there is data.
SemaphoreDown( & pipe->EmptyLock, NULL );
//Acqure mutex lock - No one can do any io until
//we leave the buffer.
SemaphoreDown( & pipe->Mutex, NULL );
//Check and see if the buffer is full.
//If it is, then the FullLock should
//have been leaked, and writers should be blocking.
wasFull = RingBufferIsFull( & pipe->Ring );
ASSERT( wasFull ? (pipe->FullLock.Count == 0) : TRUE );
//Perform the read.
read = RingBufferRead( buff, size, & pipe->Ring );
// Ring is protected by a read lock which should prevent zero length reads.
ASSERT( read > 0 );
//See if the ring buffer is empty.
//If it is then we need to leak the reader lock.
dataLeft = !RingBufferIsEmpty( & pipe->Ring );
//Release mutex lock - We are out of the ring, so
//let other IO go if its already passed.
SemaphoreUp( & pipe->Mutex );
//If the ring was full while we have exclusive access,
//and we freed up some space then we should release
//the writer lock so writers can go.
if( wasFull && read > 0 ) {
SemaphoreUp( & pipe->FullLock );
}
//If there is data left in the buffer, release the
//empty lock so that other readers can go.
//If there is no data in the buffer, we cant let
//readers progress, so we leak the lock.
if( dataLeft ) {
SemaphoreUp( & pipe->EmptyLock );
}
return read;
}
COUNT PipeWriteInner( char * buff, COUNT size, PIPE_WRITE pipe ) {
BOOL wasEmpty;
BOOL spaceLeft;
COUNT write;
//Acquire FullLock - No writers can progress until we are done.
SemaphoreDown( & pipe->FullLock, NULL );
//Acqure mutex lock - No one can do any io until
//we leave the buffer.
SemaphoreDown( & pipe->Mutex, NULL );
//Check and see if the buffer is empty.
//If it is, then the EmptyLock should
//have been leaked, and readers should be blocking.
wasEmpty = RingBufferIsEmpty( & pipe->Ring );
ASSERT( wasEmpty ? (pipe->EmptyLock.Count == 0) : TRUE );
//Perform the write.
write = RingBufferWrite( buff, size, & pipe->Ring );
// Ring is protected by a write lock which should prevent zero length writes.
ASSERT( write > 0 );
//See if the ring buffer is empty.
//If it is then we need to leak the reader lock.
spaceLeft = !RingBufferIsFull( & pipe->Ring );
//Release mutex lock - We are out of the ring, so
//let other IO go if its already passed.
SemaphoreUp( & pipe->Mutex );
//If the ring was empty while we have exclusive access,
//and we wrote some data, then we should release
//the EmptyLock so readers can go.
if( wasEmpty && write > 0 ) {
SemaphoreUp( & pipe->EmptyLock );
}
//If there is space left in the buffer, release the
//FullLock so that other writers can go.
//If there is no space in the buffer, we cant let
//writers progress, so we leak the lock.
if( spaceLeft ) {
SemaphoreUp( & pipe->FullLock );
}
return write;
}
/*********************************************************************//**
* @brief Client Parer uc/OS II task
* @param[in] p_arg parameter
* @return None
***********************************************************************/
void ClientParserTask(void *p_arg)
{
uint8_t b;
INT8U err = OS_ERR_NONE;
(void) p_arg; // avoid compile warning
// Start Client Parser
for (;;) {
OSMboxPend(client_parser.event, 500, &err);
if (err == OS_ERR_NONE) {
while (!RingBufferIsEmpty(&(client_parser.buffer))) {
RingBufferPop(&(client_parser.buffer), &b);
client_parser.parser[client_parser.mode](&b);
}
}
}
}
int PacketPoolIsEmpty(void) {
return RingBufferIsEmpty(ringbuffer);
}
void OnAxisTimerOverflow ()
{
PORTE.OUTSET = 0x01;
if (time == 0)
{
// Nothing to do
if (!buffer_lock && !RingBufferIsEmpty(&location_buffer))
{
Location_t next = RingBufferGet(&location_buffer);
x_axis.delta = next.x - x_axis.location;
y_axis.delta = next.y - y_axis.location;
z_axis.delta = next.z - z_axis.location;
time = next.time;
ovf_count = 0;
}
//if(location_buffer_size > 0)
//{
// x_axis.delta = nextLocation.x - x_axis.location;
// y_axis.delta = nextLocation.y - y_axis.location;
// z_axis.delta = nextLocation.z - z_axis.location;
// time = nextLocation.time;
// ovf_count = 1;
// location_buffer_size--;
//}
//else
//{
// x_axis.delta = 0;
// y_axis.delta = 0;
// z_axis.delta = 0;
//}
}
else
{
UpdateAxis (&x_axis);
UpdateAxis (&y_axis);
UpdateAxis (&z_axis);
if (ovf_count >= time)
{
time = 0;
if (!buffer_lock && !RingBufferIsEmpty(&location_buffer))
{
Location_t next = RingBufferGet(&location_buffer);
x_axis.delta = next.x - x_axis.location;
y_axis.delta = next.y - y_axis.location;
z_axis.delta = next.z - z_axis.location;
time = next.time;
ovf_count = 0;
}
//if(location_buffer_size > 0)
//{
// x_axis.delta = nextLocation.x - x_axis.location;
// y_axis.delta = nextLocation.y - y_axis.location;
// z_axis.delta = nextLocation.z - z_axis.location;
// time = nextLocation.time;
// ovf_count = 0;
// location_buffer_size--;
//}
//else
//{
// // TODO: If there's a buffer of positions, move to the next one here
// // (set delta, time, and count accordingly)
// x_axis.delta = 0;
// y_axis.delta = 0;
// z_axis.delta = 0;
// time = 0;
// ovf_count = 0;
//}
PORTE.OUTTGL = 0x02;
}
else
{
ovf_count++;
}
}
PORTE.OUTCLR = 0x01;
}