/* 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 */