iCub-main
debugFunctions.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2012 iCub Facility, Istituto Italiano di Tecnologia
3  * Authors: Alberto Cardellino
4  * CopyPolicy: Released under the terms of the LGPLv2.1 or later, see LGPL.TXT
5  *
6  */
7 
8 
9 #if defined(_MSC_VER) || defined(_MSC_EXTENSIONS)
10 #include "StdAfx.h"
11 #endif
12 #include "debugFunctions.h"
13 //#include <sys/socket.h>
14 //#include <netinet/in.h>
15 //#include <arpa/inet.h>
16 //#include <netdb.h>
17 //#include <unistd.h>
18 
19 //typedef struct
20 //{
21 // uint32_t size;
22 // uint32_t progNum;
23 // struct timeval time;
24 //} DeepDebugData;
25 //
26 //DeepDebugData DDData[MAX_ACQUISITION] = {0};
27 //uint32_t errors[MAX_ACQUISITION] = {0};
28 
29 
30 uint32_t missErr[BOARD_NUM] = {0};
31 uint32_t lateErr[BOARD_NUM] = {0};
32 uint32_t sizeErr[BOARD_NUM] = {0};
33 
34 static uint32_t idx[BOARD_NUM] = {0};
35 
36 struct timeval recvTime[BOARD_NUM];
37 struct timeval prevTime[BOARD_NUM];
38 struct timeval diffTime[BOARD_NUM];
39 
40 FILE *outFile = stdout;
41 
42 size_t maxBytes2Read = 1500;
43 
44 
45 
46 
47 
48 uint8_t board;
49 uint32_t prevNum[BOARD_NUM] = {0};
50 uint32_t progNum[BOARD_NUM] = {0};
51 
52 //eOabstime_t txtime;
53 ACE_UINT64 txtime;
54 ACE_UINT16 size_ini[BOARD_NUM] = {0};
55 //EOropframe *ropFrame = NULL;
56 
57 bool error = false;
58 int n = 0;
59 
60 bool check_received_pkt(ACE_INET_Addr *sender_addr, void * pkt, ACE_UINT16 pkt_size)
61 {
62  char str[64];
63 
64  // detect board
65  if( *sender_addr == eb1)
66  {
67  board = 1;
68  }
69  else if( *sender_addr == eb2)
70  {
71  board = 2;
72  }
73  else if( *sender_addr == eb3)
74  {
75  board = 3;
76  }
77  else if( *sender_addr == eb4)
78  {
79  board = 4;
80  }
81  else if( *sender_addr == eb5)
82  {
83  board = 5;
84  }
85  else if( *sender_addr == eb6)
86  {
87  board = 6;
88  }
89  else
90  {
91  sender_addr->addr_to_string(str, 64, 1);
92  printf(" no board %s\n", str);
93  board = 1;
94  return false;
95  }
96  return do_real_check(board, pkt, pkt_size);
97 }
98 
99 
100 bool check_received_pkt(sockaddr *sender_addr, void * pkt, ACE_UINT16 pkt_size)
101 {
102  char str[64];
103 
104  sockaddr_in *p = (sockaddr_in *) sender_addr;
105  sockaddr_in a;
106  a.sin_addr.s_addr = eb2_addr;
107 
108  // detect board
109  if( p->sin_addr.s_addr == a.sin_addr.s_addr )
110  {
111  board = 2;
112  }
113  else
114  {
115  //a.sin_addr.s_addr = eb4_addr;
116  if( p->sin_addr.s_addr == eb4_addr )
117  {
118  board = 4;
119  }
120  else
121  {
122  //sender_addr->addr_to_string(str, 64, 1);
123  printf(" no board %04X\n", p->sin_addr.s_addr);
124  board = 1;
125  return false;
126  }
127  }
128 
129  return do_real_check(board, pkt, pkt_size);
130 }
131 
132 
133 bool do_real_check(int board, void * pkt, ACE_UINT16 pkt_size)
134 {
135  error = false;
136  // // init ropframe
137  // if(NULL == ropFrame)
138  // {
139  // printf("|\n|\tStarting check on eth packets...\n|\n");
140  // ropFrame = eo_ropframe_New();
141  // }
142  //
143  // // Get data from the packet
144  // if( eores_OK != eo_ropframe_Load(ropFrame, (uint8_t*) pkt, pkt_size, maxBytes2Read))
145  // {
146  // printf("Error in the ropFrame!!");
147  // return false;
148  // }
149  //
150  // txtime = eo_ropframe_age_Get(ropFrame);
151 
152  char *tmp = ((char*) pkt);
153  memcpy(&txtime, (&tmp[8]), 8);
154 
155  if(idx[board] == 0)
156  {
157  printf("received packet size %d\n", pkt_size);
158 
159  size_ini[board] = pkt_size;
160  prevNum[board] = (txtime >> 32);
161  gettimeofday(&prevTime[board],NULL);
162  idx[board]++;
163  //AfxMessageBox("Fisrt packet received");
164  printf("Fisrt packet from board=%d\n", board);
165  progNum[board] = (txtime >> 32);
166  }
167 
168  else //if(idx[board] < max_idx)
169  {
170  if(size_ini[board] != pkt_size)
171  {
172  // fprintf(outFile, "size of packet %d differs from %d\n", size_ini[board], pkt_size);
173  sizeErr[board]++;
174  }
175 
176  progNum[board] = (txtime >> 32);
177 
178  if( (progNum[board] - prevNum[board]) != 1)
179  {
180  fprintf(outFile,"missing packet: old = %d, new = %d(0x%04X) - board [%d] \n", prevNum[board], progNum[board], progNum[board], board);
181  missErr[board]++;
182  }
183 
184  gettimeofday(&recvTime[board],NULL);
185  timeval_subtract(&diffTime[board], &recvTime[board], &prevTime[board]);
186  if(diffTime[board].tv_usec >= SOGLIA)
187  {
188  // fprintf(outFile, "time between packets %06d.%06d - board [%d] - pkt %d\n", diffTime[board].tv_sec, diffTime[board].tv_usec, board, progNum[board]);
189  lateErr[board]++;
190  }
191 
192  // if (error)
193  // {
194  // nErr[board]++;
196  // }
197 
198 
199  if( (0 == (progNum[board] % 100000)) && (progNum[board] != 0) )
200  {
201  printf(">periodic check:\n\tpkt_size = %d\n" \
202  "\tprogNum[%d] = %d\n", pkt_size, board, progNum[board]);
203  printf("\ttime between packets %06d.%06d - board [%d] - pkt %d\n\n", diffTime[board].tv_sec, diffTime[board].tv_usec, board, progNum[board]);
204  print_data();
205  }
206 
209  idx[board]++;
210  }
211  return true;
212 }
213 
214 void print_data(void)
215 {
216  char output[2048], tmp[64];
217  sprintf(output,"Printing data...\n");
218  for(board = 0; board <BOARD_NUM; board++)
219  {
220  sprintf(tmp,"errors board[%d] \n",board);
221  strcat(output, tmp);
222  sprintf(tmp,"missing = %d over %d\n", missErr[board], idx[board]);
223  strcat(output, tmp);
224  sprintf(tmp,"late = %d over %d\n", lateErr[board], idx[board]);
225  strcat(output, tmp);
226  sprintf(tmp,"size = %d over %d\n", sizeErr[board], idx[board]);
227  strcat(output, tmp);
228  }
229 #if defined(_MSC_VER) || defined(_MSC_EXTENSIONS)
230  AfxMessageBox(output);
231 #else
232  printf(output);
233 #endif
234 }
235 
236 
237 #if defined(_MSC_VER) || defined(_MSC_EXTENSIONS)
238 // Definition of a gettimeofday function
239 
240 int gettimeofday(struct timeval *tv, struct timezone *tz)
241 {
242  // Define a structure to receive the current Windows filetime
243  FILETIME ft;
244 
245  // Initialize the present time to 0 and the timezone to UTC
246  unsigned __int64 tmpres = 0;
247  static int tzflag = 0;
248 
249  if (NULL != tv)
250  {
251  GetSystemTimeAsFileTime(&ft);
252 
253  // The GetSystemTimeAsFileTime returns the number of 100 nanosecond
254  // intervals since Jan 1, 1601 in a structure. Copy the high bits to
255  // the 64 bit tmpres, shift it left by 32 then or in the low 32 bits.
256  tmpres |= ft.dwHighDateTime;
257  tmpres <<= 32;
258  tmpres |= ft.dwLowDateTime;
259 
260  // Convert to microseconds by dividing by 10
261  tmpres /= 10;
262 
263  // The Unix epoch starts on Jan 1 1970. Need to subtract the difference
264  // in seconds from Jan 1 1601.
265  tmpres -= DELTA_EPOCH_IN_MICROSECS;
266 
267  // Finally change microseconds to seconds and place in the seconds value.
268  // The modulus picks up the microseconds.
269  tv->tv_sec = (long)(tmpres / 1000000UL);
270  tv->tv_usec = (long)(tmpres % 1000000UL);
271  }
272 
273  if (NULL != tz)
274  {
275  if (!tzflag)
276  {
277  _tzset();
278  tzflag++;
279  }
280 
281  // Adjust for the timezone west of Greenwich
282  tz->tz_minuteswest = _timezone / 60;
283  tz->tz_dsttime = _daylight;
284  }
285 
286  return 0;
287 }
288 #endif
uint32_t missErr[BOARD_NUM]
struct timeval prevTime[BOARD_NUM]
static uint32_t idx[BOARD_NUM]
bool error
uint32_t sizeErr[BOARD_NUM]
int n
ACE_UINT16 size_ini[BOARD_NUM]
fprintf(fid,'\n')
struct timeval diffTime[BOARD_NUM]
uint32_t prevNum[BOARD_NUM]
void print_data(void)
uint32_t progNum[BOARD_NUM]
ACE_UINT64 txtime
FILE * outFile
uint32_t lateErr[BOARD_NUM]
struct timeval recvTime[BOARD_NUM]
bool check_received_pkt(ACE_INET_Addr *sender_addr, void *pkt, ACE_UINT16 pkt_size)
size_t maxBytes2Read
bool do_real_check(int board, void *pkt, ACE_UINT16 pkt_size)
uint8_t board