debugFunctions.cpp

Go to the documentation of this file.

/*
 * Copyright (C) 2012 iCub Facility, Istituto Italiano di Tecnologia
 * Authors: Alberto Cardellino
 * CopyPolicy: Released under the terms of the LGPLv2.1 or later, see LGPL.TXT
 *
 */
 
 
#if defined(_MSC_VER) || defined(_MSC_EXTENSIONS)
#include "StdAfx.h"
#endif
#include "debugFunctions.h"
//#include <sys/socket.h>
//#include <netinet/in.h>
//#include <arpa/inet.h>
//#include <netdb.h>
//#include <unistd.h>
 
//typedef struct
//{
//  uint32_t size;
//  uint32_t progNum;
//  struct timeval time;
//} DeepDebugData;
//
//DeepDebugData         DDData[MAX_ACQUISITION] = {0};
//uint32_t          errors[MAX_ACQUISITION] = {0};
 
 
uint32_t            missErr[BOARD_NUM]      = {0};
uint32_t            lateErr[BOARD_NUM]      = {0};
uint32_t            sizeErr[BOARD_NUM]      = {0};
 
static uint32_t     idx[BOARD_NUM]          = {0};
 
struct timeval      recvTime[BOARD_NUM];
struct timeval      prevTime[BOARD_NUM];
struct timeval      diffTime[BOARD_NUM];
 
FILE                *outFile                = stdout;
 
size_t              maxBytes2Read           = 1500;
 
 
 
 
 
uint8_t             board;
uint32_t            prevNum[BOARD_NUM]      = {0};
uint32_t            progNum[BOARD_NUM]      = {0};
 
//eOabstime_t       txtime;
ACE_UINT64          txtime;
ACE_UINT16          size_ini[BOARD_NUM]     = {0};
//EOropframe        *ropFrame               = NULL;
 
bool error = false;
int n = 0;
 
bool check_received_pkt(ACE_INET_Addr *sender_addr, void * pkt, ACE_UINT16 pkt_size)
{
    char  str[64];
 
    // detect board
    if( *sender_addr == eb1)
    {
        board = 1;
    }
    else if( *sender_addr == eb2)
    {
        board = 2;
    }
    else if( *sender_addr == eb3)
    {
        board = 3;
    }
    else if( *sender_addr == eb4)
    {
        board = 4;
    }
    else if( *sender_addr == eb5)
    {
        board = 5;
    }
    else if( *sender_addr == eb6)
    {
        board = 6;
    }
    else
    {
        sender_addr->addr_to_string(str, 64, 1);
        printf(" no board %s\n", str);
        board = 1;
        return false;
    }
    return do_real_check(board, pkt, pkt_size);
}
 
 
bool check_received_pkt(sockaddr *sender_addr, void * pkt, ACE_UINT16 pkt_size)
{
    char  str[64];
 
    sockaddr_in *p = (sockaddr_in *) sender_addr;
    sockaddr_in a;
    a.sin_addr.s_addr = eb2_addr;
 
    // detect board
    if( p->sin_addr.s_addr == a.sin_addr.s_addr )
    {
        board = 2;
    }
    else
    {
        //a.sin_addr.s_addr = eb4_addr;
        if( p->sin_addr.s_addr == eb4_addr )
        {
            board = 4;
        }
        else
        {
            //sender_addr->addr_to_string(str, 64, 1);
            printf(" no board %04X\n", p->sin_addr.s_addr);
            board = 1;
            return false;
        }
    }
 
    return do_real_check(board, pkt, pkt_size);
}
 
 
bool do_real_check(int board, void * pkt, ACE_UINT16 pkt_size)
{
    error = false;
    //  // init ropframe
    //  if(NULL == ropFrame)
    //  {
    //      printf("|\n|\tStarting check on eth packets...\n|\n");
    //      ropFrame = eo_ropframe_New();
    //  }
    //
    //  // Get data from the packet
    //  if( eores_OK != eo_ropframe_Load(ropFrame, (uint8_t*) pkt, pkt_size, maxBytes2Read))
    //  {
    //      printf("Error in the ropFrame!!");
    //      return false;
    //  }
    //
    //  txtime = eo_ropframe_age_Get(ropFrame);
 
    char    *tmp = ((char*) pkt);
    memcpy(&txtime,  (&tmp[8]), 8);
 
    if(idx[board] == 0)
    {
        printf("received packet size %d\n", pkt_size);
 
        size_ini[board] = pkt_size;
        prevNum[board] = (txtime >> 32);
        gettimeofday(&prevTime[board],NULL);
        idx[board]++;
        //AfxMessageBox("Fisrt packet received");
        printf("Fisrt packet from board=%d\n", board);
        progNum[board] = (txtime >> 32);
    }
 
    else //if(idx[board] < max_idx)
    {
        if(size_ini[board] != pkt_size)
        {
            //          fprintf(outFile, "size of packet %d differs from %d\n", size_ini[board], pkt_size);
            sizeErr[board]++;
        }
 
        progNum[board] = (txtime >> 32);
 
        if( (progNum[board] - prevNum[board]) != 1)
        {
            fprintf(outFile,"missing packet: old = %d, new = %d(0x%04X)  - board [%d] \n", prevNum[board], progNum[board], progNum[board], board);
            missErr[board]++;
        }
 
        gettimeofday(&recvTime[board],NULL);
        timeval_subtract(&diffTime[board], &recvTime[board], &prevTime[board]);
        if(diffTime[board].tv_usec >= SOGLIA)
        {
            //          fprintf(outFile, "time between packets %06d.%06d - board [%d] - pkt %d\n", diffTime[board].tv_sec, diffTime[board].tv_usec, board, progNum[board]);
            lateErr[board]++;
        }
 
        //      if (error)
        //      {
        //          nErr[board]++;
        //      }
 
 
        if( (0 == (progNum[board] % 100000)) && (progNum[board] != 0) )
        {
            printf(">periodic check:\n\tpkt_size = %d\n" \
                    "\tprogNum[%d] = %d\n", pkt_size, board, progNum[board]);
            printf("\ttime between packets %06d.%06d - board [%d] - pkt %d\n\n", diffTime[board].tv_sec, diffTime[board].tv_usec, board, progNum[board]);
            print_data();
        }
 
        prevTime[board] = recvTime[board];
        prevNum[board] = progNum[board];
        idx[board]++;
    }
    return true;
}
 
void print_data(void)
{
    char    output[2048], tmp[64];
    sprintf(output,"Printing data...\n");
    for(board = 0; board <BOARD_NUM; board++)
    {
        sprintf(tmp,"errors board[%d] \n",board);
        strcat(output, tmp);
        sprintf(tmp,"missing    = %d over %d\n", missErr[board], idx[board]);
        strcat(output, tmp);
        sprintf(tmp,"late       = %d over %d\n", lateErr[board], idx[board]);
        strcat(output, tmp);
        sprintf(tmp,"size       = %d over %d\n", sizeErr[board], idx[board]);
        strcat(output, tmp);
    }
#if defined(_MSC_VER) || defined(_MSC_EXTENSIONS)
    AfxMessageBox(output);
#else
    printf(output);
#endif
}
 
 
#if defined(_MSC_VER) || defined(_MSC_EXTENSIONS)
// Definition of a gettimeofday function
 
int gettimeofday(struct timeval *tv, struct timezone *tz)
{
    // Define a structure to receive the current Windows filetime
    FILETIME ft;
 
    // Initialize the present time to 0 and the timezone to UTC
    unsigned __int64 tmpres = 0;
    static int tzflag = 0;
 
    if (NULL != tv)
    {
        GetSystemTimeAsFileTime(&ft);
 
        // The GetSystemTimeAsFileTime returns the number of 100 nanosecond
        // intervals since Jan 1, 1601 in a structure. Copy the high bits to
        // the 64 bit tmpres, shift it left by 32 then or in the low 32 bits.
        tmpres |= ft.dwHighDateTime;
        tmpres <<= 32;
        tmpres |= ft.dwLowDateTime;
 
        // Convert to microseconds by dividing by 10
        tmpres /= 10;
 
        // The Unix epoch starts on Jan 1 1970.  Need to subtract the difference
        // in seconds from Jan 1 1601.
        tmpres -= DELTA_EPOCH_IN_MICROSECS;
 
        // Finally change microseconds to seconds and place in the seconds value.
        // The modulus picks up the microseconds.
        tv->tv_sec = (long)(tmpres / 1000000UL);
        tv->tv_usec = (long)(tmpres % 1000000UL);
    }
 
    if (NULL != tz)
    {
        if (!tzflag)
        {
            _tzset();
            tzflag++;
        }
 
        // Adjust for the timezone west of Greenwich
        tz->tz_minuteswest = _timezone / 60;
        tz->tz_dsttime = _daylight;
    }
 
    return 0;
}
#endif