ARNETWORK_Receiver_t* ARNETWORK_Receiver_New (ARNETWORKAL_Manager_t *networkALManager, unsigned int numberOfOutputBuff, ARNETWORK_IOBuffer_t **outputBufferPtrArr, ARNETWORK_IOBuffer_t **outputBufferPtrMap)
{
/** -- Create a new receiver -- */
/** local declarations */
ARNETWORK_Receiver_t *receiverPtr = NULL;
eARNETWORK_ERROR error = ARNETWORK_OK;
/** Create the receiver */
receiverPtr = malloc (sizeof (ARNETWORK_Receiver_t));
if (receiverPtr)
{
if(networkALManager != NULL)
{
receiverPtr->networkALManager = networkALManager;
}
else
{
error = ARNETWORK_ERROR_BAD_PARAMETER;
}
if(error == ARNETWORK_OK)
{
receiverPtr->isAlive = 1;
receiverPtr->senderPtr = NULL;
receiverPtr->numberOfOutputBuff = numberOfOutputBuff;
receiverPtr->outputBufferPtrArr = outputBufferPtrArr;
receiverPtr->outputBufferPtrMap = outputBufferPtrMap;
#ifdef ENABLE_MONITOR_INCOMING_DATA
receiverPtr->inputEventFd = eventfd(0, EFD_NONBLOCK | EFD_CLOEXEC);
#endif
}
/** delete the receiver if an error occurred */
if (error != ARNETWORK_OK)
{
ARSAL_PRINT (ARSAL_PRINT_ERROR, ARNETWORK_RECEIVER_TAG,"error: %s", ARNETWORK_Error_ToString (error));
ARNETWORK_Receiver_Delete (&receiverPtr);
}
}
return receiverPtr;
}
void* ARSTREAM_Reader_RunDataThread (void *ARSTREAM_Reader_t_Param)
{
uint8_t *recvData = NULL;
int recvSize;
uint16_t previousFNum = UINT16_MAX;
int skipCurrentFrame = 0;
int packetWasAlreadyAck = 0;
ARSTREAM_Reader_t *reader = (ARSTREAM_Reader_t *)ARSTREAM_Reader_t_Param;
ARSTREAM_NetworkHeaders_DataHeader_t *header = NULL;
int recvDataLen = reader->maxFragmentSize + sizeof (ARSTREAM_NetworkHeaders_DataHeader_t);
/* Parameters check */
if (reader == NULL)
{
ARSAL_PRINT (ARSAL_PRINT_ERROR, ARSTREAM_READER_TAG, "Error while starting %s, bad parameters", __FUNCTION__);
return (void *)0;
}
/* Alloc and check */
recvData = malloc (recvDataLen);
if (recvData == NULL)
{
ARSAL_PRINT (ARSAL_PRINT_ERROR, ARSTREAM_READER_TAG, "Error while starting %s, can not alloc memory", __FUNCTION__);
return (void *)0;
}
header = (ARSTREAM_NetworkHeaders_DataHeader_t *)recvData;
ARSAL_PRINT (ARSAL_PRINT_DEBUG, ARSTREAM_READER_TAG, "Stream reader thread running");
reader->dataThreadStarted = 1;
while (reader->threadsShouldStop == 0)
{
eARNETWORK_ERROR err = ARNETWORK_Manager_ReadDataWithTimeout (reader->manager, reader->dataBufferID, recvData, recvDataLen, &recvSize, ARSTREAM_READER_DATAREAD_TIMEOUT_MS);
if (ARNETWORK_OK != err)
{
if (ARNETWORK_ERROR_BUFFER_EMPTY != err)
{
ARSAL_PRINT (ARSAL_PRINT_ERROR, ARSTREAM_READER_TAG, "Error while reading stream data: %s", ARNETWORK_Error_ToString (err));
}
}
else
{
int cpIndex, cpSize, endIndex;
ARSAL_Mutex_Lock (&(reader->ackPacketMutex));
if (header->frameNumber != reader->ackPacket.frameNumber)
{
reader->efficiency_index ++;
reader->efficiency_index %= ARSTREAM_READER_EFFICIENCY_AVERAGE_NB_FRAMES;
reader->efficiency_nbTotal [reader->efficiency_index] = 0;
reader->efficiency_nbUseful [reader->efficiency_index] = 0;
skipCurrentFrame = 0;
reader->currentFrameSize = 0;
reader->ackPacket.frameNumber = header->frameNumber;
#ifdef DEBUG
uint32_t nackPackets = ARSTREAM_NetworkHeaders_AckPacketCountNotSet (&(reader->ackPacket), header->fragmentsPerFrame);
if (nackPackets != 0)
{
ARSAL_PRINT (ARSAL_PRINT_DEBUG, ARSTREAM_READER_TAG, "Dropping a frame (missing %d fragments)", nackPackets);
}
#endif
ARSTREAM_NetworkHeaders_AckPacketResetUpTo (&(reader->ackPacket), header->fragmentsPerFrame);
}
packetWasAlreadyAck = ARSTREAM_NetworkHeaders_AckPacketFlagIsSet (&(reader->ackPacket), header->fragmentNumber);
ARSTREAM_NetworkHeaders_AckPacketSetFlag (&(reader->ackPacket), header->fragmentNumber);
reader->efficiency_nbTotal [reader->efficiency_index] ++;
if (packetWasAlreadyAck == 0)
{
reader->efficiency_nbUseful [reader->efficiency_index] ++;
}
ARSAL_Mutex_Unlock (&(reader->ackPacketMutex));
ARSAL_Mutex_Lock (&(reader->ackSendMutex));
ARSAL_Cond_Signal (&(reader->ackSendCond));
ARSAL_Mutex_Unlock (&(reader->ackSendMutex));
cpIndex = reader->maxFragmentSize * header->fragmentNumber;
cpSize = recvSize - sizeof (ARSTREAM_NetworkHeaders_DataHeader_t);
endIndex = cpIndex + cpSize;
while ((endIndex > reader->currentFrameBufferSize) &&
(skipCurrentFrame == 0) &&
(packetWasAlreadyAck == 0))
{
uint32_t nextFrameBufferSize = reader->maxFragmentSize * header->fragmentsPerFrame;
uint32_t dummy;
uint8_t *nextFrameBuffer = reader->callback (ARSTREAM_READER_CAUSE_FRAME_TOO_SMALL, reader->currentFrameBuffer, reader->currentFrameSize, 0, 0, &nextFrameBufferSize, reader->custom);
if (nextFrameBufferSize >= reader->currentFrameSize && nextFrameBufferSize > 0)
{
memcpy (nextFrameBuffer, reader->currentFrameBuffer, reader->currentFrameSize);
}
else
{
skipCurrentFrame = 1;
}
//TODO: Add "SKIP_FRAME"
reader->callback (ARSTREAM_READER_CAUSE_COPY_COMPLETE, reader->currentFrameBuffer, reader->currentFrameSize, 0, skipCurrentFrame, &dummy, reader->custom);
reader->currentFrameBuffer = nextFrameBuffer;
reader->currentFrameBufferSize = nextFrameBufferSize;
}
if (skipCurrentFrame == 0)
{
if (packetWasAlreadyAck == 0)
{
memcpy (&(reader->currentFrameBuffer)[cpIndex], &recvData[sizeof (ARSTREAM_NetworkHeaders_DataHeader_t)], recvSize - sizeof (ARSTREAM_NetworkHeaders_DataHeader_t));
}
if (endIndex > reader->currentFrameSize)
{
reader->currentFrameSize = endIndex;
}
ARSAL_Mutex_Lock (&(reader->ackPacketMutex));
if (ARSTREAM_NetworkHeaders_AckPacketAllFlagsSet (&(reader->ackPacket), header->fragmentsPerFrame))
{
if (header->frameNumber != previousFNum)
{
int nbMissedFrame = 0;
int isFlushFrame = ((header->frameFlags & ARSTREAM_NETWORK_HEADERS_FLAG_FLUSH_FRAME) != 0) ? 1 : 0;
ARSAL_PRINT (ARSAL_PRINT_DEBUG, ARSTREAM_READER_TAG, "Ack all in frame %d (isFlush : %d)", header->frameNumber, isFlushFrame);
if (header->frameNumber != previousFNum + 1)
{
nbMissedFrame = header->frameNumber - previousFNum - 1;
ARSAL_PRINT (ARSAL_PRINT_DEBUG, ARSTREAM_READER_TAG, "Missed %d frames !", nbMissedFrame);
}
previousFNum = header->frameNumber;
skipCurrentFrame = 1;
reader->currentFrameBuffer = reader->callback (ARSTREAM_READER_CAUSE_FRAME_COMPLETE, reader->currentFrameBuffer, reader->currentFrameSize, nbMissedFrame, isFlushFrame, &(reader->currentFrameBufferSize), reader->custom);
}
}
ARSAL_Mutex_Unlock (&(reader->ackPacketMutex));
}
}
}
free (recvData);
reader->callback (ARSTREAM_READER_CAUSE_CANCEL, reader->currentFrameBuffer, reader->currentFrameSize, 0, 0, &(reader->currentFrameBufferSize), reader->custom);
ARSAL_PRINT (ARSAL_PRINT_DEBUG, ARSTREAM_READER_TAG, "Stream reader thread ended");
reader->dataThreadStarted = 0;
return (void *)0;
}
void *ARCONTROLLER_Network_ReaderRun (void *data)
{
// -- Read and decode one input network buffer --
eARCONTROLLER_ERROR error = ARCONTROLLER_OK;
ARCONTROLLER_Network_t *networkController = NULL;
int readerBufferId = -1;
// Allocate some space for incoming data.
const size_t maxLength = 128 * 1024;
void *readData = malloc (maxLength);
if (readData == NULL)
{
error = ARCONTROLLER_ERROR_ALLOC;
}
if (error == ARCONTROLLER_OK)
{
if (data != NULL)
{
//get thread data
networkController = ((ARCONTROLLER_NETWORK_THREAD_DATA_t *) data)->networkController;
readerBufferId = ((ARCONTROLLER_NETWORK_THREAD_DATA_t *) data)->readerBufferId;
if (networkController == NULL)
{
error = ARCONTROLLER_ERROR_BAD_PARAMETER;
}
}
else
{
error = ARCONTROLLER_ERROR_BAD_PARAMETER;
}
}
if (error == ARCONTROLLER_OK)
{
while (networkController->state != ARCONTROLLER_NETWORK_STATE_STOPPED)
{
if (networkController->state == ARCONTROLLER_NETWORK_STATE_RUNNING)
{
eARNETWORK_ERROR netError = ARNETWORK_OK;
int length = 0;
int skip = 0;
//read data
netError = ARNETWORK_Manager_ReadDataWithTimeout (networkController->networkManager, readerBufferId, readData, maxLength, &length, ARCONTROLLER_NETWORK_READING_TIMEOUT_MS);
if (netError != ARNETWORK_OK)
{
if (netError != ARNETWORK_ERROR_BUFFER_EMPTY)
{
ARSAL_PRINT(ARSAL_PRINT_ERROR, ARCONTROLLER_NETWORK_TAG, "ARNETWORK_Manager_ReadDataWithTimeout () failed : %s", ARNETWORK_Error_ToString(netError));
}
skip = 1;
}
if (!skip)
{
// Forward data to the CommandsManager
eARCOMMANDS_DECODER_ERROR cmdError = ARCOMMANDS_DECODER_OK;
cmdError = ARCOMMANDS_Decoder_DecodeBuffer ((uint8_t *)readData, length);
if ((cmdError != ARCOMMANDS_DECODER_OK) && (cmdError != ARCOMMANDS_DECODER_ERROR_NO_CALLBACK))
{
char msg[128];
ARCOMMANDS_Decoder_DescribeBuffer ((uint8_t *)readData, length, msg, sizeof(msg));
ARSAL_PRINT(ARSAL_PRINT_ERROR, ARCONTROLLER_NETWORK_TAG, "ARCOMMANDS_Decoder_DecodeBuffer () failed : %d %s", cmdError, msg);
}
}
//NO ELSE ; no data read
}
else
{
//sleep
sleep (1); //TODO !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!! replace by signal
}
}
}
// Print Error
if (error != ARCONTROLLER_OK)
{
ARSAL_PRINT(ARSAL_PRINT_ERROR, ARCONTROLLER_NETWORK_TAG, "error: %s", ARCONTROLLER_Error_ToString (error));
}
// cleanup
if (readData != NULL)
{
free (readData);
readData = NULL;
}
return NULL;
}
int ARSTREAM_Reader_TestBenchMain (int argc, char *argv[])
{
int retVal = 0;
appName = argv[0];
if (argc > 3)
{
ARSTREAM_ReaderTb_printUsage ();
return 1;
}
char *ip = __IP;
char *outPath = NULL;
if (argc >= 2)
{
ip = argv[1];
}
if (argc >= 3)
{
outPath = argv[2];
}
ARSAL_PRINT (ARSAL_PRINT_WARNING, __TAG__, "IP = %s", ip);
int nbInBuff = 1;
ARNETWORK_IOBufferParam_t inParams;
ARSTREAM_Reader_InitStreamAckBuffer (&inParams, ACK_BUFFER_ID);
int nbOutBuff = 1;
ARNETWORK_IOBufferParam_t outParams;
ARSTREAM_Reader_InitStreamDataBuffer (&outParams, DATA_BUFFER_ID, ARSTREAM_TB_FRAG_SIZE, ARSTREAM_TB_MAX_NB_FRAG);
eARNETWORK_ERROR error;
eARNETWORKAL_ERROR specificError = ARNETWORKAL_OK;
ARNETWORKAL_Manager_t *osspecificManagerPtr = ARNETWORKAL_Manager_New(&specificError);
if(specificError == ARNETWORKAL_OK)
{
specificError = ARNETWORKAL_Manager_InitWifiNetwork(osspecificManagerPtr, ip, SENDING_PORT, READING_PORT, 1000);
}
else
{
ARSAL_PRINT (ARSAL_PRINT_ERROR, __TAG__, "Error during ARNETWORKAL_Manager_New call : %s", ARNETWORKAL_Error_ToString(specificError));
}
if(specificError == ARNETWORKAL_OK)
{
g_Manager = ARNETWORK_Manager_New(osspecificManagerPtr, nbInBuff, &inParams, nbOutBuff, &outParams, READER_PING_DELAY, NULL, NULL, &error);
}
else
{
ARSAL_PRINT (ARSAL_PRINT_ERROR, __TAG__, "Error during ARNETWORKAL_Manager_InitWifiNetwork call : %s", ARNETWORKAL_Error_ToString(specificError));
error = ARNETWORK_ERROR;
}
if ((g_Manager == NULL) ||
(error != ARNETWORK_OK))
{
ARSAL_PRINT (ARSAL_PRINT_ERROR, __TAG__, "Error during ARNETWORK_Manager_New call : %s", ARNETWORK_Error_ToString(error));
return 1;
}
pthread_t netsend, netread;
pthread_create (&netsend, NULL, ARNETWORK_Manager_SendingThreadRun, g_Manager);
pthread_create (&netread, NULL, ARNETWORK_Manager_ReceivingThreadRun, g_Manager);
retVal = ARSTREAM_ReaderTb_StartStreamTest (g_Manager, outPath);
ARNETWORK_Manager_Stop (g_Manager);
pthread_join (netread, NULL);
pthread_join (netsend, NULL);
ARNETWORK_Manager_Delete (&g_Manager);
ARNETWORKAL_Manager_CloseWifiNetwork(osspecificManagerPtr);
ARNETWORKAL_Manager_Delete(&osspecificManagerPtr);
return retVal;
}
void* ARNETWORK_Receiver_ThreadRun (void *data)
{
/** -- Manage the reception of the data on the Receiver' socket. -- */
/** local declarations */
ARNETWORK_Receiver_t *receiverPtr = data;
ARNETWORKAL_Frame_t frame ;
ARNETWORK_IOBuffer_t* outBufferPtrTemp = NULL;
eARNETWORK_ERROR error = ARNETWORK_OK;
eARNETWORKAL_MANAGER_RETURN result = ARNETWORKAL_MANAGER_RETURN_DEFAULT;
uint8_t ackSeqNumData = 0;
struct timespec now;
while (receiverPtr->isAlive)
{
/** wait a receipt */
if (receiverPtr->networkALManager->receive(receiverPtr->networkALManager) == ARNETWORKAL_MANAGER_RETURN_DEFAULT)
{
/** for each frame present in the receiver buffer */
result = receiverPtr->networkALManager->popFrame(receiverPtr->networkALManager, &frame);
while (result == ARNETWORKAL_MANAGER_RETURN_DEFAULT)
{
/* Special handling of internal frames */
if (frame.id < ARNETWORK_MANAGER_INTERNAL_BUFFER_ID_MAX)
{
switch (frame.id)
{
case ARNETWORK_MANAGER_INTERNAL_BUFFER_ID_PING:
/* Ping, send the corresponding pong */
{
ARNETWORK_Sender_SendPong (receiverPtr->senderPtr, frame.dataPtr, frame.size - offsetof(ARNETWORKAL_Frame_t, dataPtr));
}
break;
case ARNETWORK_MANAGER_INTERNAL_BUFFER_ID_PONG:
/* Pong, tells the sender that we got a response */
{
struct timespec dataTime;
memcpy (&dataTime, frame.dataPtr, sizeof (struct timespec));
ARSAL_Time_GetTime(&now);
ARNETWORK_Sender_GotPingAck (receiverPtr->senderPtr, &dataTime, &now);
}
break;
default:
/* Do nothing as we don't know how to handle it */
break;
}
}
/** management by the command type */
switch (frame.type)
{
case ARNETWORKAL_FRAME_TYPE_ACK:
/** get the acknowledge sequence number from the data */
memcpy (&ackSeqNumData, frame.dataPtr, sizeof(uint8_t));
ARSAL_PRINT (ARSAL_PRINT_VERBOSE, ARNETWORK_RECEIVER_TAG, "[%p] - TYPE: ARNETWORKAL_FRAME_TYPE_ACK | SEQ:%d | ID:%d | SEQ ACK : %d", receiverPtr, receiverPtr, frame.seq, frame.id, ackSeqNumData);
/** transmit the acknowledgement to the sender */
error = ARNETWORK_Sender_AckReceived (receiverPtr->senderPtr, ARNETWORK_Manager_IDAckToIDInput (receiverPtr->networkALManager, frame.id), ackSeqNumData);
if (error != ARNETWORK_OK)
{
switch (error)
{
case ARNETWORK_ERROR_IOBUFFER_BAD_ACK:
ARSAL_PRINT (ARSAL_PRINT_DEBUG, ARNETWORK_RECEIVER_TAG, "[%p] Bad acknowledge, error: %s", receiverPtr, ARNETWORK_Error_ToString (error));
break;
default:
ARSAL_PRINT (ARSAL_PRINT_ERROR, ARNETWORK_RECEIVER_TAG, "[%p] Acknowledge received, error: %s", receiverPtr, ARNETWORK_Error_ToString (error));
break;
}
}
break;
case ARNETWORKAL_FRAME_TYPE_DATA:
ARSAL_PRINT (ARSAL_PRINT_VERBOSE, ARNETWORK_RECEIVER_TAG, "[%p] - TYPE: ARNETWORKAL_FRAME_TYPE_DATA | SEQ:%d | ID:%d", receiverPtr, frame.seq, frame.id);
/** push the data received in the output buffer targeted */
outBufferPtrTemp = receiverPtr->outputBufferPtrMap[frame.id];
if (outBufferPtrTemp != NULL)
{
/** lock the IOBuffer */
error = ARNETWORK_IOBuffer_Lock(outBufferPtrTemp);
if(error == ARNETWORK_OK)
{
int accept = ARNETWORK_IOBuffer_ShouldAcceptData (outBufferPtrTemp, frame.seq);
if (accept > 0)
{
error = ARNETWORK_Receiver_CopyDataRecv(receiverPtr, outBufferPtrTemp, &frame);
}
else if (accept == 0)
{
ARSAL_PRINT (ARSAL_PRINT_DEBUG, ARNETWORK_RECEIVER_TAG, "[%p] Received a retry for buffer %d", receiverPtr, outBufferPtrTemp->ID);
}
else
{
ARSAL_PRINT (ARSAL_PRINT_WARNING, ARNETWORK_RECEIVER_TAG, "[%p] Received an old frame for buffer %d", receiverPtr, outBufferPtrTemp->ID);
}
/** unlock the IOBuffer */
ARNETWORK_IOBuffer_Unlock(outBufferPtrTemp);
if(error != ARNETWORK_OK)
{
ARSAL_PRINT (ARSAL_PRINT_ERROR, ARNETWORK_RECEIVER_TAG, "[%p] data received, error: %s", receiverPtr, ARNETWORK_Error_ToString (error));
}
}
}
break;
case ARNETWORKAL_FRAME_TYPE_DATA_LOW_LATENCY:
ARSAL_PRINT (ARSAL_PRINT_VERBOSE, ARNETWORK_RECEIVER_TAG, "[%p] - TYPE: ARNETWORKAL_FRAME_TYPE_DATA_LOW_LATENCY | SEQ:%d | ID:%d", receiverPtr, frame.seq, frame.id);
/** push the data received in the output buffer targeted */
outBufferPtrTemp = receiverPtr->outputBufferPtrMap[frame.id];
if (outBufferPtrTemp != NULL)
{
/** lock the IOBuffer */
error = ARNETWORK_IOBuffer_Lock(outBufferPtrTemp);
if(error == ARNETWORK_OK)
{
int accept = ARNETWORK_IOBuffer_ShouldAcceptData (outBufferPtrTemp, frame.seq);
if (accept > 0)
{
error = ARNETWORK_Receiver_CopyDataRecv(receiverPtr, outBufferPtrTemp, &frame);
}
else if (accept == 0)
{
ARSAL_PRINT (ARSAL_PRINT_DEBUG, ARNETWORK_RECEIVER_TAG, "[%p] Received a retry for buffer %d", receiverPtr, outBufferPtrTemp->ID);
}
else
{
ARSAL_PRINT (ARSAL_PRINT_WARNING, ARNETWORK_RECEIVER_TAG, "[%p] Received an old frame for buffer %d", receiverPtr, outBufferPtrTemp->ID);
}
/** unlock the IOBuffer */
ARNETWORK_IOBuffer_Unlock(outBufferPtrTemp);
if(error != ARNETWORK_OK)
{
ARSAL_PRINT (ARSAL_PRINT_ERROR, ARNETWORK_RECEIVER_TAG, "[%p] data received, error: %s", receiverPtr, ARNETWORK_Error_ToString (error));
}
}
}
break;
case ARNETWORKAL_FRAME_TYPE_DATA_WITH_ACK:
ARSAL_PRINT (ARSAL_PRINT_VERBOSE, ARNETWORK_RECEIVER_TAG, "[%p] - TYPE: ARNETWORKAL_FRAME_TYPE_DATA_WITH_ACK | SEQ:%d | ID:%d", receiverPtr, frame.seq, frame.id);
/**
* push the data received in the output buffer targeted,
* save the sequence of the command and return an acknowledgement
*/
outBufferPtrTemp = receiverPtr->outputBufferPtrMap[frame.id];
if (outBufferPtrTemp != NULL)
{
/** lock the IOBuffer */
error = ARNETWORK_IOBuffer_Lock(outBufferPtrTemp);
if(error == ARNETWORK_OK)
{
/** OutBuffer->seqWaitAck used to save the last seq */
int accept = ARNETWORK_IOBuffer_ShouldAcceptData (outBufferPtrTemp, frame.seq);
if (accept > 0)
{
error = ARNETWORK_Receiver_CopyDataRecv(receiverPtr, outBufferPtrTemp, &frame);
}
else if (accept == 0)
{
ARSAL_PRINT (ARSAL_PRINT_DEBUG, ARNETWORK_RECEIVER_TAG, "[%p] Received a retry for buffer %d", receiverPtr, outBufferPtrTemp->ID);
}
else
{
ARSAL_PRINT (ARSAL_PRINT_WARNING, ARNETWORK_RECEIVER_TAG, "[%p] Received an old frame for buffer %d", receiverPtr, outBufferPtrTemp->ID);
}
/** unlock the IOBuffer */
ARNETWORK_IOBuffer_Unlock(outBufferPtrTemp);
// data are copied to the IOBuffer, send ACK
if (error == ARNETWORK_OK)
{
/** sending ack even if the seq is not correct */
error = ARNETWORK_Receiver_ReturnACK(receiverPtr, frame.id, frame.seq);
if(error != ARNETWORK_OK)
{
int level = ARSAL_PRINT_ERROR;
if (error == ARNETWORK_ERROR_BUFFER_SIZE)
{
level = ARSAL_PRINT_DEBUG;
}
ARSAL_PRINT(level, ARNETWORK_RECEIVER_TAG, "[%p] ReturnACK, error: %s", receiverPtr, ARNETWORK_Error_ToString(error));
}
}
else
{
ARSAL_PRINT (ARSAL_PRINT_ERROR, ARNETWORK_RECEIVER_TAG, "[%p] data with ack received, error: %s", receiverPtr, ARNETWORK_Error_ToString (error));
}
}
}
break;
default:
ARSAL_PRINT (ARSAL_PRINT_WARNING, ARNETWORK_RECEIVER_TAG, "[%p] !!! command type: %d not known !!!", receiverPtr, frame.type);
break;
}
/** get the next frame*/
result = receiverPtr->networkALManager->popFrame(receiverPtr->networkALManager, &frame);
}
}
}
return NULL;
}
void ARNETWORK_Sender_SendPong (ARNETWORK_Sender_t *senderPtr, uint8_t *data, int dataSize)
{
ARNETWORK_IOBuffer_t *inputBufferPtrTemp;
eARNETWORK_ERROR err = ARNETWORK_OK;
inputBufferPtrTemp = senderPtr->inputBufferPtrMap[ARNETWORK_MANAGER_INTERNAL_BUFFER_ID_PONG];
err = ARNETWORK_IOBuffer_Lock (inputBufferPtrTemp);
if (err != ARNETWORK_OK) {
ARSAL_PRINT (ARSAL_PRINT_ERROR, ARNETWORK_SENDER_TAG, "ARNETWORK_IOBuffer_Lock() failed; error: %s", ARNETWORK_Error_ToString (err));
return;
}
ARNETWORK_IOBuffer_AddData (inputBufferPtrTemp, data, dataSize, NULL, NULL, 1);
ARNETWORK_IOBuffer_Unlock (inputBufferPtrTemp);
}
void* ARNETWORK_Sender_ThreadRun (void* data)
{
/** -- Manage the sending of the data on the sender' -- */
/** local declarations */
ARNETWORK_Sender_t *senderPtr = data;
int inputBufferIndex = 0;
ARNETWORK_IOBuffer_t *inputBufferPtrTemp = NULL; /**< pointer of the input buffer in processing */
eARNETWORK_ERROR error = ARNETWORK_OK;
int waitTimeMs = 0;
struct timespec now;
struct timespec sleepStart;
int sleepDurationMs = 0;
int timeDiffMs;
while (senderPtr->isAlive)
{
waitTimeMs = 500;
for (inputBufferIndex = 0; inputBufferIndex < senderPtr->numberOfInputBuff && waitTimeMs > 0; ++inputBufferIndex)
{
inputBufferPtrTemp = senderPtr->inputBufferPtrArr[inputBufferIndex];
error = ARNETWORK_IOBuffer_Lock(inputBufferPtrTemp);
switch (inputBufferPtrTemp->dataType)
{
// Low latency : no wait if any data available
case ARNETWORKAL_FRAME_TYPE_DATA_LOW_LATENCY:
if ((error == ARNETWORK_OK) &&
(!ARNETWORK_RingBuffer_IsEmpty (inputBufferPtrTemp->dataDescriptorRBuffer)))
{
waitTimeMs = 0;
}
break;
// Acknowledged buffer :
// - If waiting an ack, wait time = time before ack timeout
// - If not waiting an ack and not empty, wait time = time before next send
case ARNETWORKAL_FRAME_TYPE_DATA_WITH_ACK:
if (error == ARNETWORK_OK)
{
if (ARNETWORK_IOBuffer_IsWaitAck(inputBufferPtrTemp))
{
if (inputBufferPtrTemp->ackWaitTimeCount < waitTimeMs)
{
waitTimeMs = inputBufferPtrTemp->ackWaitTimeCount;
}
}
else if (!ARNETWORK_RingBuffer_IsEmpty (inputBufferPtrTemp->dataDescriptorRBuffer))
{
if (inputBufferPtrTemp->waitTimeCount < waitTimeMs)
{
waitTimeMs = inputBufferPtrTemp->waitTimeCount;
}
}
}
break;
// All non Ack buffers
// -
default:
if ((error == ARNETWORK_OK) &&
(!ARNETWORK_RingBuffer_IsEmpty (inputBufferPtrTemp->dataDescriptorRBuffer)))
{
if (inputBufferPtrTemp->waitTimeCount < waitTimeMs)
{
waitTimeMs = inputBufferPtrTemp->waitTimeCount;
}
}
break;
}
ARNETWORK_IOBuffer_Unlock(inputBufferPtrTemp);
}
// Force a minimum wait time after an ARNetworkAL Overflow
if ((senderPtr->hadARNetworkALOverflowOnPreviousRun > 0) &&
(waitTimeMs < ARNETWORK_SENDER_WAIT_TIME_ON_ARNETWORKAL_OVERFLOW_MS))
{
waitTimeMs = ARNETWORK_SENDER_WAIT_TIME_ON_ARNETWORKAL_OVERFLOW_MS;
}
senderPtr->hadARNetworkALOverflowOnPreviousRun = 0;
ARSAL_Time_GetTime(&sleepStart);
if (waitTimeMs > 0)
{
if (waitTimeMs < senderPtr->minimumTimeBetweenSendsMs)
{
waitTimeMs = senderPtr->minimumTimeBetweenSendsMs;
}
ARSAL_Mutex_Lock (&(senderPtr->nextSendMutex));
ARSAL_Cond_Timedwait (&(senderPtr->nextSendCond), &(senderPtr->nextSendMutex), waitTimeMs);
ARSAL_Mutex_Unlock (&(senderPtr->nextSendMutex));
}
/** Process internal input buffers */
ARSAL_Time_GetTime(&now);
sleepDurationMs = ARSAL_Time_ComputeTimespecMsTimeDiff (&sleepStart, &now);
ARSAL_Mutex_Lock (&(senderPtr->pingMutex));
timeDiffMs = ARSAL_Time_ComputeTimespecMsTimeDiff (&(senderPtr->pingStartTime), &now);
/* Send only new pings if ping function is active (min time > 0) */
if (senderPtr->minTimeBetweenPings > 0)
{
int maxWaitTime = senderPtr->minTimeBetweenPings;
if (ARNETWORK_SENDER_PING_TIMEOUT_MS > maxWaitTime)
{
maxWaitTime = ARNETWORK_SENDER_PING_TIMEOUT_MS;
}
/* Send new ping if :
* -> DT > minTimeBetweenPings AND we're not waiting for a ping
* -> DT > maxWaitTime
*/
if (((senderPtr->isPingRunning == 0) &&
(timeDiffMs > senderPtr->minTimeBetweenPings)) ||
(timeDiffMs > maxWaitTime))
{
if (timeDiffMs > ARNETWORK_SENDER_PING_TIMEOUT_MS)
{
senderPtr->lastPingValue = -1;
}
inputBufferPtrTemp = senderPtr->inputBufferPtrMap[ARNETWORK_MANAGER_INTERNAL_BUFFER_ID_PING];
error = ARNETWORK_IOBuffer_Lock (inputBufferPtrTemp);
if (error == ARNETWORK_OK) {
ARNETWORK_IOBuffer_AddData (inputBufferPtrTemp, (uint8_t *)&now, sizeof (now), NULL, NULL, 1);
ARNETWORK_IOBuffer_Unlock (inputBufferPtrTemp);
} else {
ARSAL_PRINT (ARSAL_PRINT_ERROR, ARNETWORK_SENDER_TAG, "ARNETWORK_IOBuffer_Lock() failed; error: %s", ARNETWORK_Error_ToString (error));
}
senderPtr->pingStartTime.tv_sec = now.tv_sec;
senderPtr->pingStartTime.tv_nsec = now.tv_nsec;
senderPtr->isPingRunning = 1;
}
}
ARSAL_Mutex_Unlock (&(senderPtr->pingMutex));
for (inputBufferIndex = 0; inputBufferIndex < senderPtr->networkALManager->maxIds ; inputBufferIndex++)
{
inputBufferPtrTemp = senderPtr->inputBufferPtrMap[inputBufferIndex];
if (inputBufferPtrTemp != NULL)
{
ARNETWORK_Sender_ProcessBufferToSend (senderPtr, inputBufferPtrTemp, (waitTimeMs > 0) ? sleepDurationMs : 0);
}
}
senderPtr->networkALManager->send(senderPtr->networkALManager);
}
return NULL;
}
ARNETWORK_Sender_t* ARNETWORK_Sender_New (ARNETWORKAL_Manager_t *networkALManager, unsigned int numberOfInputBuffer, ARNETWORK_IOBuffer_t **inputBufferPtrArr, unsigned int numberOfInternalInputBuffer, ARNETWORK_IOBuffer_t **internalInputBufferPtrArr, ARNETWORK_IOBuffer_t **inputBufferPtrMap, int pingDelayMs)
{
/** -- Create a new sender -- */
/** local declarations */
ARNETWORK_Sender_t* senderPtr = NULL;
eARNETWORK_ERROR error = ARNETWORK_OK;
/** Create the sender */
senderPtr = malloc (sizeof (ARNETWORK_Sender_t));
if (senderPtr)
{
if(networkALManager != NULL)
{
senderPtr->networkALManager = networkALManager;
}
else
{
error = ARNETWORK_ERROR_BAD_PARAMETER;
}
if(error == ARNETWORK_OK)
{
senderPtr->isAlive = 1;
senderPtr->numberOfInputBuff = numberOfInputBuffer;
senderPtr->inputBufferPtrArr = inputBufferPtrArr;
senderPtr->numberOfInternalInputBuff = numberOfInternalInputBuffer;
senderPtr->internalInputBufferPtrArr = internalInputBufferPtrArr;
senderPtr->inputBufferPtrMap = inputBufferPtrMap;
senderPtr->minimumTimeBetweenSendsMs = ARNETWORK_SENDER_MILLISECOND;
senderPtr->isPingRunning = 0;
senderPtr->hadARNetworkALOverflowOnPreviousRun = 0;
if (pingDelayMs == 0)
{
senderPtr->minTimeBetweenPings = ARNETWORK_SENDER_MINIMUM_TIME_BETWEEN_PINGS_MS;
}
else
{
senderPtr->minTimeBetweenPings = pingDelayMs;
}
ARSAL_Time_GetTime(&(senderPtr->pingStartTime));
}
/* Create the mutex/condition */
if ( (error == ARNETWORK_OK) &&
(ARSAL_Mutex_Init (&(senderPtr->nextSendMutex)) != 0))
{
error = ARNETWORK_ERROR_NEW_BUFFER;
}
if ( (error == ARNETWORK_OK) &&
(ARSAL_Cond_Init (&(senderPtr->nextSendCond)) != 0))
{
error = ARNETWORK_ERROR_NEW_BUFFER;
}
if ( (error == ARNETWORK_OK) &&
(ARSAL_Mutex_Init (&(senderPtr->pingMutex)) != 0))
{
error = ARNETWORK_ERROR_NEW_BUFFER;
}
/** delete the sender if an error occurred */
if (error != ARNETWORK_OK)
{
ARSAL_PRINT (ARSAL_PRINT_ERROR, ARNETWORK_SENDER_TAG, "error: %s", ARNETWORK_Error_ToString (error));
ARNETWORK_Sender_Delete (&senderPtr);
}
}
return senderPtr;
}
int sendLanding(BD_MANAGER_t *deviceManager)
{
int sentStatus = 1;
u_int8_t cmdBuffer[128];
int32_t cmdSize = 0;
eARCOMMANDS_GENERATOR_ERROR cmdError;
eARNETWORK_ERROR netError = ARNETWORK_ERROR;
ARSAL_PRINT(ARSAL_PRINT_INFO, TAG, "- Send landing");
// Send landing command
cmdError = ARCOMMANDS_Generator_GenerateARDrone3PilotingLanding(cmdBuffer, sizeof(cmdBuffer), &cmdSize);
if (cmdError == ARCOMMANDS_GENERATOR_OK)
{
printf("send Landing\n");
netError = ARNETWORK_Manager_SendData(deviceManager->netManager, BD_NET_CD_ACK_ID, cmdBuffer, cmdSize, NULL, &(arnetworkCmdCallback), 1);
printf("Landing sent\n");
}
if ((cmdError != ARCOMMANDS_GENERATOR_OK) || (netError != ARNETWORK_OK))
{
printf("Landing error\n");
ARSAL_PRINT(ARSAL_PRINT_WARNING, TAG, "Failed to send landing command. cmdError:%d netError:%s", cmdError, ARNETWORK_Error_ToString(netError));
sentStatus = 0;
}
return sentStatus;
}
