/* Function: GetPkt ============================================================
* Abstract:
* Receive nBytes from the host. Return a buffer containing the bytes or
* NULL if an error occurs. Note that the pointer returned is that of the
* global pktBuf. If the buf needs to be grown to accommodate the package,
* it is realloc'd. This function will try to get the requested number
* of bytes indefinitely - it is assumed that the data is either already there,
* or will show up in a "reasonable" amount of time.
*/
PRIVATE const char *GetPkt(const int pktSize)
{
int_T nGot;
boolean_T error = EXT_NO_ERROR;
int_T nGotTotal = 0;
error = GrowRecvBufIfNeeded(pktSize);
if (error != EXT_NO_ERROR) {
fprintf(stderr,"Previous pkt from host thrown away due to lack of memory.\n");
ClearPkt(pktSize);
goto EXIT_POINT;
}
/* Get the data. */
while(nGotTotal < pktSize) {
error = ExtGetHostPkt(extUD,
pktSize - nGotTotal, &nGot, (char_T *)(pktBuf + nGotTotal));
if (error) {
fprintf(stderr,"ExtGetHostPkt() failed.\n");
goto EXIT_POINT;
}
nGotTotal += nGot;
}
EXIT_POINT:
return((error == EXT_NO_ERROR) ? pktBuf : NULL);
} /* end GetPkt */
/* Function: GetPkt ============================================================
* Abstract:
* Receive nBytes from the host. Return a buffer containing the bytes or
* NULL if an error occurs. Note that the pointer returned is that of the
* global pktBuf. If the buf needs to be grown to accommodate the package,
* it is realloc'd. This function will try to get the requested number
* of bytes indefinitely - it is assumed that the data is either already there,
* or will show up in a "reasonable" amount of time.
*/
PRIVATE const char *GetPkt(const int pktSize)
{
int_T nGot;
boolean_T error = EXT_NO_ERROR;
int_T nGotTotal = 0;
int AR_Drone_EXT_Time_Out = 0;
error = GrowRecvBufIfNeeded(pktSize);
if (error != EXT_NO_ERROR) {
#ifndef EXTMODE_DISABLEPRINTF
fprintf(stderr,"Previous pkt from host thrown away due to lack of memory.\n");
#endif
ClearPkt(pktSize);
goto EXIT_POINT;
}
//usleep(AR_DRONE_GET_PKT_DELAY); //375000L
printf("Updating parameters! \n");
//usleep(AR_DRONE_GET_PKT_DELAY); //375000L
/* Get/ the data. */
while(nGotTotal < pktSize) {
AR_Drone_EXT_Time_Out++;
error = ExtGetHostPkt(extUD,
pktSize - nGotTotal, &nGot, (char_T *)(pktBuf + nGotTotal));
if (error) {
#ifndef EXTMODE_DISABLEPRINTF
fprintf(stderr,"ExtGetHostPkt() failed.\n");
#endif
goto EXIT_POINT;
}
nGotTotal += nGot;
if (AR_Drone_EXT_Time_Out>10)
{
printf("Failed to get host packets after 100 ticks \n");
return (NULL);
}
printf("pktSize value is : %d \n", pktSize);
usleep(AR_DRONE_GET_PKT_DELAY); //375000L
}
EXIT_POINT:
return((error == EXT_NO_ERROR) ? pktBuf : NULL);
} /* end GetPkt */
PRIVATE void ClearPkt(const int pktSize)
{
int_T nGot;
boolean_T error = EXT_NO_ERROR;
int_T nGotTotal = 0;
static char buffer;
/* Get and discard the data one char at a time. */
while (nGotTotal < pktSize) {
error = ExtGetHostPkt(extUD, 1, &nGot, (char_T *)&buffer);
if (error) {
fprintf(stderr,"ExtGetHostPkt() failed.\n");
goto EXIT_POINT;
}
nGotTotal += nGot;
}
EXIT_POINT:
return;
} /* end ClearPkt */
/* Function: GetPktHdr =========================================================
* Abstract:
* Attempts to retrieve a packet header from the host. If a header is in
* fact retrieved, the reference arg, 'hdrAvail' will be returned as true.
*
* EXT_NO_ERROR is returned on success, EXT_ERROR is returned on failure.
*
* NOTES:
* o It is not necessarily an error for 'hdrAvail' to be returned as false.
* It typically means that we were polling for packets and none were
* available.
*/
PRIVATE boolean_T GetPktHdr(PktHeader *pktHdr, boolean_T *hdrAvail)
{
int_T nGot = 0; /* assume */
int_T nGotTotal = 0;
int_T pktSize = sizeof(PktHeader);
boolean_T error = EXT_NO_ERROR;
/* Get the header. */
while(nGotTotal < pktSize) {
error = ExtGetHostPkt(extUD,
pktSize - nGotTotal, &nGot, (char_T *)((char_T *)pktHdr + nGotTotal));
if (error) goto EXIT_POINT;
nGotTotal += nGot;
if (nGotTotal == 0) break;
}
assert((nGot == 0) || (nGotTotal == pktSize));
EXIT_POINT:
*hdrAvail = (boolean_T)(nGot > 0);
return(error);
} /* end GetPktHdr */
