PageRenderTime 51ms CodeModel.GetById 7ms app.highlight 39ms RepoModel.GetById 1ms app.codeStats 1ms

/elekIOaux/shipdata.c

http://github.com/Yniold/liftsrc
C | 509 lines | 340 code | 79 blank | 90 comment | 44 complexity | 811a24c5c5b5ffbbcf244e2a87e9c74d MD5 | raw file
  1// ============================================
  2// shipdata.c
  3// ShipData Control Thread
  4// ============================================
  5//
  6// $RCSfile: shipdata.c,v $ last changed on $Date: 2007/07/12 17:18:44 $ by $Author: rudolf $
  7//
  8// History:
  9//
 10// $Log: shipdata.c,v $
 11// Revision 1.7  2007/07/12 17:18:44  rudolf
 12// corrected include file order
 13//
 14// Revision 1.6  2007-03-10 18:09:30  rudolf
 15// fixed lat and longitude swapped
 16//
 17// Revision 1.5  2007-03-08 18:53:23  rudolf
 18// made fields flash green if new data received, cosmetics
 19//
 20// Revision 1.4  2007-03-08 14:01:22  rudolf
 21// cleaned up unused ports
 22//
 23// Revision 1.3  2007-03-07 18:11:28  rudolf
 24// fixed nasty locking bug
 25//
 26// Revision 1.2  2007-03-07 17:14:04  rudolf
 27// more work on parser
 28//
 29// Revision 1.1  2007-03-05 20:48:09  rudolf
 30// added thread for collecting ship's data, more work on parser
 31//
 32//
 33//
 34//
 35//
 36
 37//#define DEBUG_MUTEX
 38//#define DEBUG
 39//#define DEBUG_SETPOS
 40
 41#undef DEBUG
 42#undef DEBUG_SETPOS
 43#undef DEBUG_MUTEX
 44#include <stdint.h>
 45#include <pthread.h>
 46#include <sys/socket.h>
 47#include <sys/types.h>
 48#include <errno.h>
 49#include <signal.h>
 50#include <ncurses.h>
 51
 52
 53#include "../include/elekGeneral.h"
 54#include "../include/elekIO.h"
 55#include "../include/elekIOPorts.h"
 56#include "../commTools/udptools.h"
 57
 58#include "shipdata.h"
 59
 60extern bool bEnableGUI;
 61extern WINDOW* pStatusWin;
 62extern struct MessagePortType MessageOutPortList[];
 63typedef void Sigfunc (int);
 64
 65static void dummy_handler(int signo)
 66{
 67   return;
 68};
 69
 70enum OutPortListEnum
 71{
 72   // this list has to be coherent with MessageOutPortList
 73   ELEK_DEBUG_OUT,                 // port for outgoing messages to debug
 74     ELEK_MANUAL_OUT,                // reverse port for answers to eCmd
 75     ELEK_ELEKIO_AUX_MASTER_OUT,     // port for outgoing data packets from elekAux to master
 76     MAX_MESSAGE_OUTPORTS
 77};
 78
 79// Variables
 80//
 81//typedef void Sigfunc (int);
 82
 83// the thread works on this structure
 84//
 85struct sShipDataType sShipDataThread =
 86{
 87   .iFD = -1,                      /* socket FD */
 88};
 89
 90pthread_mutex_t mShipDataMutex;
 91
 92// bind to socket and create the parser thread
 93//
 94int ShipDataInit(void)
 95{
 96   int iRetCode;
 97   pthread_t ptShipDataThread;
 98
 99   // init mutex before creating thread
100   pthread_mutex_init(&mShipDataMutex,NULL);
101
102   iRetCode = pthread_create(&ptShipDataThread, NULL, (void*)&ShipDataThreadFunc,(void*) &sShipDataThread);
103   if(iRetCode > 0)
104     {
105	extern struct MessagePortType MessageOutPortList[];
106	SendUDPMsg(&MessageOutPortList[ELEK_DEBUG_OUT],"elekIOaux : thread create failed");
107
108	printf("In ShipDataInit(): pthread_create failed!\n\r");
109	return (1);
110     };
111   signal(SIGALRM, dummy_handler);
112
113   return(0);
114};
115
116// thread code
117//
118void ShipDataThreadFunc(void* pArgument)
119{
120   // buffer for one line
121   char aUDPBuffer[1024];
122
123   extern struct MessagePortType MessageOutPortList[];
124   int iRetVal;
125
126   char cTheChar;
127   char *pTheBuffer = aUDPBuffer;
128   int iNumChars;
129
130   // socket structure
131   struct sockaddr_in ServerAddress;
132
133   // shared structure
134   struct sShipDataType *sStructure = (struct sShipDataType *) pArgument;
135
136   while(1)
137     {
138	// try to connect till success
139	while(1)
140	  {
141
142	     // create SocketFD
143	     sStructure->iFD = socket(AF_INET, SOCK_DGRAM, 0);
144	     memset(&ServerAddress,0, sizeof(ServerAddress));
145
146	     // fill out structure for connection to XPORT
147	     ServerAddress.sin_family = AF_INET;
148	     ServerAddress.sin_port = htons(3040);
149	     ServerAddress.sin_addr.s_addr = htonl(INADDR_ANY);
150
151	     // try connection to XPORT using a timeout
152	     if((iRetVal = bind(sStructure->iFD, (const struct sockaddr*)&ServerAddress, sizeof(ServerAddress))) < 0)
153	       {
154		  close(sStructure->iFD);
155		  if(errno == EINTR)
156		    errno = ETIMEDOUT;
157	       }
158	     else
159	       {
160		  break;
161	       };
162	  };
163
164	if(iRetVal < 0)
165	  printf("%s\r\n",strerror(errno));
166	else
167	  {
168	     SendUDPMsg(&MessageOutPortList[ELEK_DEBUG_OUT],"elekIOaux : bound to ship's broadcast data");
169	     if(bEnableGUI)
170	       {
171		  wprintw(pStatusWin,"Bound to ship's broadcast data\n");
172		  wrefresh(pStatusWin);
173	       }
174	     	     else  
175	     printf("elekIOaux : bound to ship's broadcast data\r\n");
176	  };
177
178	while(true)
179	  {
180	     // clear buffer
181             memset(aUDPBuffer,0,1023);
182	     if( (iRetVal = recvfrom(sStructure->iFD,aUDPBuffer,1023,0,NULL,NULL)) < 0)
183	       {
184		  if(errno == EINTR)
185		    printf("elekIOaux : socket timeout\r\n");
186		  else
187		    printf("recvfrom() error\r\n");
188	       }
189	     // we got data in time
190	     else
191	       {
192		  // strncmp may fail with shorter strings
193		  if(iRetVal > 5)
194		    {
195		       if(strncmp(aUDPBuffer,"@GPS1",5) == 0)
196			 {
197			    ShipDataParseGPSBuffer(aUDPBuffer,iRetVal,sStructure);
198			 };
199		       if(strncmp(aUDPBuffer,"@MTS1",5) == 0)
200			 {
201			    ShipDataParseWaterBuffer(aUDPBuffer,iRetVal,sStructure);
202			 };
203		       if(strncmp(aUDPBuffer,"@ESFB",5) == 0)
204			 {
205			    ShipDataParseSonarBuffer(aUDPBuffer,iRetVal,sStructure);
206			 };
207		       if(strncmp(aUDPBuffer,"@GYR1",5) == 0)
208			 {
209			    ShipDataParseGyroBuffer(aUDPBuffer,iRetVal,sStructure);
210			 };
211		       if(strncmp(aUDPBuffer,"@GIL1",5) == 0)
212			 {
213			    ShipDataParseAnemoBuffer(aUDPBuffer,iRetVal,sStructure);
214			 };
215		    };
216		  //		  		  printf("%s\r\n",aUDPBuffer);
217	       };
218	  };
219     };
220   printf("Trying to reconnect!\r\n");
221   close(sStructure->iFD);
222};
223
224void ShipDataParseGPSBuffer(char* pBuffer, int iBuffLen, struct sShipDataType* sDataStructure)
225{
226   char* pRetVal;
227   int iTokenNumber=0;
228   char* pContext;
229
230   // use thread safe version here...
231   pRetVal = strtok_r(pBuffer,",",&pContext);
232
233   //write(2,"ShipDataParseGPSBuffer: before lock\n\r",sizeof("ShipDataParseGPSBuffer: before lock\n\r"));
234   pthread_mutex_lock(&mShipDataMutex);
235   //write(2,"ShipDataParseGPSBuffer: after lock\n\r",sizeof("ShipDataParseGPSBuffer: after lock\n\r"));
236
237   while(true)
238     {
239	if(pRetVal != NULL)
240	  {
241	     iTokenNumber++;
242	     switch(iTokenNumber)
243	       {
244		case 3:
245		    {
246		       unsigned int uiTime = atoi(pRetVal);
247		       sDataStructure->ucUTCHours = uiTime / 10000;
248		       sDataStructure->ucUTCMins = (uiTime - (sDataStructure->ucUTCHours * 10000)) / 100;
249		       sDataStructure->ucUTCSeconds = (uiTime -(sDataStructure->ucUTCHours * 10000)-(sDataStructure->ucUTCMins*100));
250		    }
251
252		case 4:
253		  sDataStructure->ucUTCDay = atoi(pRetVal);
254
255		case 5:
256		  sDataStructure->ucUTCMonth = atoi(pRetVal);
257
258		case 6:
259		  sDataStructure->uiUTCYear = atoi(pRetVal);
260
261		case 7:
262		  sDataStructure->dLatitude= strtod(pRetVal,NULL);
263
264		case 8:
265		  if(pRetVal[0] == 'S')
266		    sDataStructure->dLatitude= -sDataStructure->dLatitude;
267
268		case 9:
269		  sDataStructure->dLongitude= strtod(pRetVal,NULL);
270
271		case 10:
272		  if(pRetVal[0] == 'W')
273		    sDataStructure->dLongitude = -sDataStructure->dLongitude;
274
275		case 11:
276		  sDataStructure->dCourseOverGround= strtod(pRetVal,NULL);
277
278		case 12:
279		  sDataStructure->dGroundSpeed= strtod(pRetVal,NULL);
280
281		default:
282		  break;
283	       };
284	     pRetVal=strtok_r(NULL,",",&pContext);
285	  }
286	else
287	  break;
288
289     };
290
291   sDataStructure->Valid.Field.ShipGPSDataValid = 1;
292   //write(2,"ShipDataParseGPSBuffer: before unlock\n\r",sizeof("ShipDataParseGPSBuffer: before unlock\n\r"));
293   pthread_mutex_unlock(&mShipDataMutex);
294   //write(2,"ShipDataParseGPSBuffer: after unlock\n\r",sizeof("ShipDataParseGPSBuffer: after unlock\n\r"));
295
296   if(gPlotData)
297     {
298	printf("GPS Time:(UTC) %02d:%02d:%02d\r\n",sDataStructure->ucUTCHours,sDataStructure->ucUTCMins,sDataStructure->ucUTCSeconds);
299	printf("GPS Date:      %02d.%02d.%04d\r\n",sDataStructure->ucUTCDay,sDataStructure->ucUTCMonth,sDataStructure->uiUTCYear);
300	printf("GPS Longitude: %09.6f\r\n",sDataStructure->dLongitude);
301	printf("GPS Latitude:  %09.6f\r\n",sDataStructure->dLatitude);
302	printf("GPS Course:    %04.2f\r\n",sDataStructure->dCourseOverGround);
303	printf("GPS Speed:     %04.2f\r\n\r\n",sDataStructure->dGroundSpeed);
304     };
305};
306
307void ShipDataParseWaterBuffer(char* pBuffer, int iBuffLen, struct sShipDataType* sDataStructure)
308{
309   char* pRetVal;
310   int iTokenNumber=0;
311   char* pContext;
312
313   // use thread safe version here...
314   pRetVal = strtok_r(pBuffer,",",&pContext);
315   //write(2,"ShipDataParseWaterBuffer: before lock\n\r",sizeof("ShipDataParseWaterBuffer: before lock\n\r"));
316   pthread_mutex_lock(&mShipDataMutex);
317   //write(2,"ShipDataParseWaterBuffer: after lock\n\r",sizeof("ShipDataParseWaterBuffer: after lock\n\r"));
318
319   while(true)
320     {
321	if(pRetVal != NULL)
322	  {
323	     iTokenNumber++;
324	     switch(iTokenNumber)
325	       {
326		case 2:
327		  if(pRetVal[0] == 'P')
328		    {
329		       //write(2,"ShipDataParseWaterBuffer: before unlock (return)\n\r",sizeof("ShipDataParseWaterBuffer: before unlock (return)\n\r"));
330		       pthread_mutex_unlock(&mShipDataMutex);
331		       //write(2,"ShipDataParseWaterBuffer: after unlock (return)\n\r",sizeof("ShipDataParseWaterBuffer: after unlock (return)\n\r"));
332		       return;
333		    };
334		  
335		case 3:
336		  sDataStructure->dWaterTemp = strtod(pRetVal,NULL);
337
338		case 5:
339		  sDataStructure->dSalinity = strtod(pRetVal,NULL);
340
341		default:
342		  break;
343	       };
344	     pRetVal=strtok_r(NULL,",",&pContext);
345	  }
346	else
347	  break;
348
349     };
350   sDataStructure->Valid.Field.ShipWaterDataValid = 1;
351
352   //write(2,"ShipDataParseWaterBuffer: before unlock\n\r",sizeof("ShipDataParseWaterBuffer: before unlock\n\r"));
353   pthread_mutex_unlock(&mShipDataMutex);
354   //write(2,"ShipDataParseWaterBuffer: after unlock\n\r",sizeof("ShipDataParseWaterBuffer: after unlock\n\r"));
355
356   if(gPlotData)
357     {
358	printf("Water Temperature:  %04.2f\r\n",sDataStructure->dWaterTemp);
359	printf("Water Salinity:     %04.2f\r\n\r\n",sDataStructure->dSalinity);
360     };
361};
362
363void ShipDataParseSonarBuffer(char* pBuffer, int iBuffLen, struct sShipDataType* sDataStructure)
364{
365   char* pRetVal;
366   int iTokenNumber=0;
367   char* pContext;
368
369   //thread safe version here...
370   pRetVal = strtok_r(pBuffer,",",&pContext);
371
372   //write(2,"ShipDataParseSonarBuffer: before lock\n\r",sizeof("ShipDataParseSonarBuffer: before lock\n\r"));
373   pthread_mutex_lock(&mShipDataMutex);
374   //write(2,"ShipDataParseSonarBuffer: after lock\n\r",sizeof("ShipDataParseSonarBuffer: after lock\n\r"));
375
376   while(true)
377     {
378	if(pRetVal != NULL)
379	  {
380	     iTokenNumber++;
381	     switch(iTokenNumber)
382	       {
383		case 3:
384		  if(strncmp(pRetVal, "#SF11SBP",8) != 0)
385		    {
386		       //write(2,"ShipDataParseSonarBuffer: before unlock (return)\n\r",sizeof("ShipDataParseSonarBuffer: before unlock (return)\n\r"));
387		       pthread_mutex_unlock(&mShipDataMutex);
388		       //write(2,"ShipDataParseSonarBuffer: after unlock (return)\n\r",sizeof("ShipDataParseSonarBuffer: after unlock (return)\n\r"));
389
390		       return;
391		    };
392		case 8:
393		  sDataStructure->dFrequency = strtod(pRetVal,NULL);
394
395		case 10:
396		  sDataStructure->dWaterDepth = strtod(pRetVal,NULL);
397
398		default:
399		  break;
400	       };
401	     pRetVal=strtok_r(NULL,",",&pContext);
402	  }
403	else
404	  break;
405
406     };
407   sDataStructure->Valid.Field.ShipSonarDataValid = 1;
408   //write(2,"ShipDataParseSonarBuffer: before unlock\n\r",sizeof("ShipDataParseSonarBuffer: before unlock\n\r"));
409   pthread_mutex_unlock(&mShipDataMutex);
410   //write(2,"ShipDataParseSonarBuffer: after unlock\n\r",sizeof("ShipDataParseSonarBuffer: after unlock\n\r"));
411
412   if(gPlotData)
413     {
414	printf("Water Depth:     %06.2f\r\n\r\n",sDataStructure->dWaterDepth);
415     };
416};
417
418void ShipDataParseGyroBuffer(char* pBuffer, int iBuffLen, struct sShipDataType* sDataStructure)
419{
420   char* pRetVal;
421   int iTokenNumber=0;
422   char* pContext;
423
424   // use thread safe version here...
425   pRetVal = strtok_r(pBuffer,",",&pContext);
426
427   //write(2,"ShipDataParseGyroBuffer: before lock\n\r",sizeof("ShipDataParseGyroBuffer: before lock\n\r"));
428   pthread_mutex_lock(&mShipDataMutex);
429   //write(2,"ShipDataParseGyroBuffer: after lock\n\r",sizeof("ShipDataParseGyroBuffer: after lock\n\r"));
430
431   while(true)
432     {
433	if(pRetVal != NULL)
434	  {
435	     iTokenNumber++;
436	     //printf("Token %02d is '%s'\r\n",iTokenNumber,pRetVal);
437	     switch(iTokenNumber)
438	       {
439		case 3:
440		  sDataStructure->dDirection = strtod(pRetVal,NULL);
441
442		default:
443		  break;
444	       };
445	     pRetVal=strtok_r(NULL,",",&pContext);
446	  }
447	else
448	  break;
449
450     };
451   sDataStructure->Valid.Field.ShipGyroDataValid = 1;
452
453   //write(2,"ShipDataParseGyroBuffer: before unlock\n\r",sizeof("ShipDataParseGyroBuffer: before unlock\n\r"));
454   pthread_mutex_unlock(&mShipDataMutex);
455   //write(2,"ShipDataParseGyroBuffer: after unlock\n\r",sizeof("ShipDataParseGyroBuffer: after unlock\n\r"));
456
457   if(gPlotData)
458     {
459	printf("Gyro Heading:     %06.2f\r\n\r\n",sDataStructure->dDirection);
460     };
461};
462
463void ShipDataParseAnemoBuffer(char* pBuffer, int iBuffLen, struct sShipDataType* sDataStructure)
464{
465   char* pRetVal;
466   int iTokenNumber=0;
467   char* pContext;
468
469   // use thread safe version here...
470   pRetVal = strtok_r(pBuffer,",",&pContext);
471
472   //write(2,"ShipDataParseAnemoBuffer: before lock\n\r",sizeof("ShipDataParseAnemoBuffer: before lock\n\r"));
473   pthread_mutex_lock(&mShipDataMutex);
474   //write(2,"ShipDataParseAnemoBuffer: after lock\n\r",sizeof("ShipDataParseAnemoBuffer: after lock\n\r"));
475
476   while(true)
477     {
478	if(pRetVal != NULL)
479	  {
480	     iTokenNumber++;
481	     switch(iTokenNumber)
482	       {
483		case 3:
484		  sDataStructure->dWindDirection = strtod(pRetVal,NULL);
485
486		case 4:
487		  sDataStructure->dWindSpeed = strtod(pRetVal,NULL);
488
489		default:
490		  break;
491	       };
492	     pRetVal=strtok_r(NULL,",",&pContext);
493	  }
494	else
495	  break;
496
497     };
498   sDataStructure->Valid.Field.ShipMeteoDataValid = 1;
499
500   //write(2,"ShipDataParseAnemoBuffer: before unlock\n\r",sizeof("ShipDataParseAnemoBuffer: before unlock\n\r"));
501   pthread_mutex_unlock(&mShipDataMutex);
502   //write(2,"ShipDataParseAnemoBuffer: after unlock\n\r",sizeof("ShipDataParseAnemoBuffer: after unlock\n\r"));
503
504   if(gPlotData)
505     {
506	printf("Wind Direction:     %06.2f\r\n",sDataStructure->dWindDirection);
507	printf("Wind Speed:         %06.2f\r\n\r\n",sDataStructure->dWindSpeed);
508     };
509};