/elekIOaux/shipdata.c
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};