ocean.cpp
Go to the documentation of this file.
1 
2 #include <stdio.h>
3 #include <stdlib.h>
4 #include <errno.h>
5 #include <stdarg.h>
6 #include <unistd.h>
7 #include <ctype.h>
8 #include <fcntl.h>
9 #include <math.h>
10 #include <poll.h>
11 #include <limits.h>
12 #define FILE_LOGGING 0
13 #include "ocean.h"
14 #include "ros/time.h"
15 
16 using namespace willowgarage::ocean;
17 
18 // Define these consts here to make gcc4 happy
19 const int ocean::NMEA_MAX;
20 const int ocean::MAXTAGLEN;
21 const int ocean::MAXCHANNELS;
22 
23 const int ocean::INPUT_BUF_SIZE;
24 const int ocean::OUTPUT_BUF_SIZE;
25 const unsigned int ocean::MAX_PACKET_LENGTH;
26 const int ocean::BAD_PACKET;
27 const int ocean::NO_PACKET;
28 const int ocean::NMEA_PACKET;
29 
30 //Battery Registers
31 //Takend from the Smart Battery Data Specification Revision 1.1, Dec. 11, 1998
32 //
33 const struct ocean::regPair ocean::regList[] = {
34  {"Manufacturer Access" ,"" , 0x00},
35  {"Remaining Capacity Alarm" ,"" , 0x01},
36  {"Remaining Time Alarm" ,"min" , 0x02},
37  {"Battery Mode" ,"" , 0x03},
38  {"At Rate" ,"mA" , 0x04}, //could also be mW
39  {"At Rate Time To Full" ,"min" , 0x05},
40  {"At Rate Time To Empty" ,"min" , 0x06},
41  {"At Rate OK" ,"bool" , 0x07},
42  {"Temperature" ,"0.1 K" , 0x08},
43  {"Voltage" ,"mV" , 0x09},
44  {"Current" ,"mA" , 0x0a},
45  {"Average Current" ,"mA" , 0x0b},
46  {"Max Error" ,"%" , 0x0c},
47  {"Relative State Of Charge" ,"%" , 0x0d},
48  {"Absolute State Of Charge" ,"%" , 0x0e},
49  {"Remaining Capacity" ,"mAh" , 0x0f}, //could also be mWh
50  {"Full Charge Capacity" ,"mAh" , 0x10}, //could also be mWh
51  {"Run Time To Empty" ,"min" , 0x11},
52  {"Average Time To Empty" ,"min" , 0x12},
53  {"Average Time To Full" ,"min" , 0x13},
54  {"Battery Status" ,"" , 0x16},
55  {"Cycle Count" ,"cycle" , 0x17},
56  {"Design Capacity" ,"mAh" , 0x18}, //could also be mWh
57  {"Design Voltage" ,"mV" , 0x19},
58  {"Specification Info" ,"" , 0x1a},
59  {"Manufacture Date" ,"DMY" , 0x1b},
60  {"Serial Number" ,"uint" , 0x1c},
61  {"Manufacture Name" ,"string", 0x20},
62  {"Device Name" ,"string", 0x21},
63  {"Device Chemistry" ,"string", 0x22},
64  {"Manufacture Data" ,"" , 0x23}
65  };
66 const unsigned ocean::regListLength(sizeof(regList)/ sizeof(struct regPair));
72 ocean::ocean ( int id, int debug)
73 {
74  debuglevel = debug;
75  server.id = id;
76  server.battery.resize(4);
77 
78  // Mark last update as time "0" for initial values
79  for (uint i = 0; i < server.battery.size(); ++i)
80  server.battery[i].last_battery_update = ros::Time(0);
81 }
82 
84 {
85  if (close (inputDevice) != 0)
86  {
87  fprintf (stderr, "failed closing serial device: %s\n", strerror (errno));
88  }
89 }
90 
91 void
93 {
94  debuglevel = level;
95 }
96 
97 void
98 ocean::initialize (const std::string &input_dev)
99 {
100  inputDevice = open (input_dev.c_str(), O_RDWR | O_NOCTTY);
101  //printf ("inputDevice %d\n", inputDevice);
102  if (inputDevice < 0)
103  {
104  fprintf (stderr, "failed to open tty device [%s]: %s\n", input_dev.c_str(), strerror (errno));
105  }
106  if (isatty (inputDevice) != 1)
107  {
108  fprintf (stderr, "Device [%s] not a tty device.\n", input_dev.c_str());
109  }
110  report (2, "Device opened dev=%d\n", inputDevice);
111 
112  tcgetattr (inputDevice, &ttyset); //get current port settings
113 
114  ttyset.c_cflag &= ~(PARENB | PARODD | CRTSCTS);
115  ttyset.c_cflag |= CREAD | CLOCAL;
116  ttyset.c_iflag = ttyset.c_oflag = ttyset.c_lflag = (tcflag_t) 0;
117 
118  set_speed (19200);
119 
120 #if (FILE_LOGGING > 0)
121  char logname[128];
122  sprintf ( logname, "/tmp/oceanServer%c.log", input_dev[input_dev.length()-1]);
123  report (2, "Logging to file: %s\n", logname);
124 
125  outputFile = open( logname, (O_WRONLY | O_APPEND ) );
126 
127  if(outputFile < 0)
128  {
129  if(errno == ENOENT )
130  {
131  outputFile = open( logname, (O_WRONLY | O_CREAT), (S_IRWXU | S_IRWXG) );
132  }
133 
134  if((outputFile < 0))
135  {
136  report (2, "Failed to open log file: %d\n", errno);
137  close(inputDevice);
138  exit(outputFile);
139  }
140  }
141 #endif
142 
143  if(commTest()) //If the Ocean isn't talking then get it into the NMEA mode
144  resetOcean();
145 
146 }
147 
148 long int ocean::convertStringBase16( const char* input )
149 {
150  char *endptr;
151  errno = 0;
152 
153  if( input == NULL )
154  {
155  report (1, "convertStringBase16 input NULL\n");
156  return 0;
157  }
158 
159  long int result = strtol( input, &endptr, 16 );
160  if( ((errno == ERANGE) && (( result == LONG_MIN ) || ( result == LONG_MAX ))) || ((errno != 0) && (result == 0)))
161  {
162  report (0, "strtol failure\n");
163  return 0;
164  }
165 
166  if( endptr == input )
167  {
168  report (0, "strtol No digits found\n");
169  return 0;
170  }
171 
172  if( *endptr != '\0' ) //not always and error, but probably not good
173  report( 1, "strtol characters left after conversion: %s\n", endptr);
174 
175  return result;
176 }
177 
178 void
179 ocean::read_file (const std::string &input)
180 {
181  inputDevice = open (input.c_str(), O_RDONLY);
182  if (inputDevice < 0)
183  {
184  fprintf (stderr, "failed to open tty file [%s]: %s\n", input.c_str(), strerror (errno));
185  }
186 
187  packet_reset();
188 }
189 
191 {
192  int maxTries = 10;
193  fd_set rfds;
194  struct timeval tv;
195  int retval;
196 
197  packet_reset();
198 
199  int listenCount = 50;
200  do
201  {
202  maxTries = 10;
203 
204  while((packetType != NMEA_PACKET) && (maxTries--))
205  {
206  report(5,"commTest: call packet_get\n");
207  int result = NO_PACKET;
208 
209  FD_ZERO(&rfds);
210  FD_SET(inputDevice, &rfds);
211  tv.tv_sec = 2;
212  tv.tv_usec = 0;
213 
214  retval = select( inputDevice + 1, &rfds, NULL, NULL, &tv);
215  if(retval < 0)
216  report(0, "select error\n");
217  else if(retval > 0)
218  {
219  {
220  report(5, "calling packet_get\n");
221  result = packet_get();
222  }
223  }
224  else
225  {
226  report(2, "select timeout\n");
227  listenCount -= 10;
228  maxTries = 0;
229  }
230  usleep(1000);
231  }
232 
233  if(packetType == NMEA_PACKET)
234  {
235  report(5,"NMEA packet\n");
236  if(nmea_parse() > 0)
237  {
238  return(0);
239  }
240  } else
241  report(5,"non NMEA packet\n");
242 
243  report(6, "listenCount=%d\n", listenCount);
244  } while( listenCount-- > 0);
245 
246  return(-1);
247 }
248 
249 void
251 {
252  report(5, "Sending ocean reset string\n");
253  string_send(" ");
254  usleep(1000);
255  string_send("x");
256  usleep(1000);
257 }
258 
259 int
261 {
262  int status (0);
263 
264  packet_get ();
265  if (packetType == NMEA_PACKET)
266  {
267  status = nmea_parse ();
268  }
269  return status;
270 }
271 
275 void
277 {
278  tcflush (inputDevice, TCIOFLUSH);
279 }
280 
281 
282 int
284 {
285  speed_t code = cfgetospeed (&ttyset);
286  switch (code)
287  {
288  case B0:
289  return (0);
290  case B300:
291  return (300);
292  case B1200:
293  return (1200);
294  case B2400:
295  return (2400);
296  case B4800:
297  return (4800);
298  case B9600:
299  return (9600);
300  case B19200:
301  return (19200);
302  case B38400:
303  return (38400);
304  case B57600:
305  return (57600);
306  default:
307  return (115200);
308  }
309 }
310 
311 void
312 ocean::set_speed (int newspeed)
313 {
314  unsigned int rate;
315  if (newspeed < 300)
316  rate = B0;
317  else if (newspeed < 1200)
318  rate = B300;
319  else if (newspeed < 2400)
320  rate = B1200;
321  else if (newspeed < 4800)
322  rate = B2400;
323  else if (newspeed < 9600)
324  rate = B4800;
325  else if (newspeed < 19200)
326  rate = B9600;
327  else if (newspeed < 38400)
328  rate = B19200;
329  else if (newspeed < 57600)
330  rate = B38400;
331  else if (newspeed < 115200)
332  rate = B57600;
333  else
334  rate = B115200;
335 
336  if (cfsetispeed (&ttyset, B0) != 0)
337  report (0, "Failed setting input speed\n");
338  if (cfsetospeed (&ttyset, rate) != 0)
339  report (0, "Failed setting output speed\n");
340 
341  ttyset.c_iflag &= ~(PARMRK | INPCK);
342  ttyset.c_cflag &= ~(CSIZE | CSTOPB | PARENB | PARODD);
343  //ttyset.c_cflag |= CS8 | PARENB | PARODD;
344  ttyset.c_cflag |= CS8;
345 
346  if (tcsetattr (inputDevice, TCSANOW, &ttyset) != 0)
347  report (0, "Failed to configure serial device\n");
348 
349  flush ();
350  report (1, "set_speed speed=%d rate=%x\n", newspeed, rate);
351 
352  currentBaudRate = (unsigned int) newspeed;
353  packet_reset ();
354 }
355 
356 void
357 ocean::report (int errlevel, const char *fmt, ...)
358 {
359  if (errlevel <= debuglevel)
360  {
361  char buf[BUFSIZ], buf2[BUFSIZ], *sp;
362  va_list ap;
363 
364  (void) strcpy (buf, "ocean: ");
365  va_start (ap, fmt);
366  (void) vsnprintf (buf + strlen (buf), sizeof (buf) - strlen (buf), fmt,
367  ap);
368  va_end (ap);
369 
370  buf2[0] = '\0';
371  for (sp = buf; *sp != '\0'; sp++)
372  if (isprint (*sp)
373  || (isspace (*sp) && (sp[1] == '\0' || sp[2] == '\0')))
374  (void) snprintf (buf2 + strlen (buf2), 2, "%c", *sp);
375  else
376  (void) snprintf (buf2 + strlen (buf2), 6, "\\x%02x", (unsigned) *sp);
377 
378  (void) fputs (buf2, stderr);
379  }
380 }
381 
382 
383 enum
384 {
385  GROUND_STATE, /* we don't know what packet type to expect */
386  NMEA_DOLLAR, /* we've seen first character of NMEA leader */
387  NMEA_PUB_LEAD, /* seen second character of NMEA G leader */
388  NMEA_LEADER_END, /* seen end char of NMEA leader, in body */
389  NMEA_CR, /* seen terminating \r of NMEA packet */
390  NMEA_RECOGNIZED /* saw trailing \n of NMEA packet */
391 };
392 
393 char *
394 ocean::gpsd_hexdump (void *binbuf, size_t binbuflen)
395 {
396  static char hexbuf[MAX_PACKET_LENGTH * 2 + 1];
397  size_t i;
398  size_t len =
399  (size_t) ((binbuflen >
400  MAX_PACKET_LENGTH) ? MAX_PACKET_LENGTH : binbuflen);
401  char *ibuf = (char *) binbuf;
402  memset (hexbuf, 0, sizeof (hexbuf));
403 
404  for (i = 0; i < len; i++)
405  {
406  (void) snprintf (hexbuf + (2 * i), 3, "%02x",
407  (unsigned int) (ibuf[i] & 0xff));
408  }
409  return hexbuf;
410 }
411 
417 void
418 ocean::nextstate (unsigned char c)
419 {
420  switch (packetState)
421  {
422  case GROUND_STATE:
423  if (c == '$')
424  {
426  }
427  break;
428 
429  case NMEA_DOLLAR:
430  if (c == 'S') /* vendor sentence */
432  else if (c == 'C') /* vendor sentence */
434  else if (c == 'B') /* vendor sentence */
435  //packetState = NMEA_PUB_LEAD;
437  else
439  break;
440 
441 /*
442  case NMEA_PUB_LEAD:
443  currentBattery = c - '0'; //convert to battery number
444  packetState = NMEA_LEADER_END;
445  break;
446 */
447 
448  case NMEA_LEADER_END:
449  if (c == '\r')
451  else if (c == '\n')
452  /* not strictly correct, but helps for interpreting logfiles */
454  else if (c == '$')
455  /* faster recovery from missing sentence trailers */
457  else if (!isprint (c))
459  break;
460 
461  case NMEA_CR:
462  if (c == '\n')
464  else
466  break;
467  case NMEA_RECOGNIZED:
468  if (c == '$')
470  else
472  break;
473 
474  }
475 }
476 
477 #define STATE_DEBUG
478 
479 /* packet grab succeeded, move to output buffer */
480 void
481 ocean::packet_accept (int packet_type)
482 {
483  size_t packetlen = inbufptr - inbuffer;
484  if (packetlen < sizeof (outbuffer))
485  {
486  memcpy ((void *) outbuffer, (void *) inbuffer, packetlen);
487  outbuflen = packetlen;
488  outbuffer[packetlen] = '\0';
489  packetType = packet_type;
490 #ifdef STATE_DEBUG
491  report (6, "Packet type %d accepted %d = %s\n",
492  packet_type, packetlen, gpsd_hexdump (outbuffer, outbuflen));
493 #endif /* STATE_DEBUG */
494  }
495  else
496  {
497  report (1, "Rejected too long packet type %d len %d\n",
498  packet_type, packetlen);
499  }
500 }
501 
502 /* shift the input buffer to discard all data up to current input pointer */
503 void
505 {
506  size_t discard = inbufptr - inbuffer;
507  size_t remaining = inbuflen - discard;
508  inbufptr = (unsigned char *) memmove (inbuffer, inbufptr, remaining);
509  inbuflen = remaining;
510 #ifdef STATE_DEBUG
511  report (6, "Packet discard of %d, chars remaining is %d = %s\n",
512  discard, remaining, gpsd_hexdump (inbuffer, inbuflen));
513 #endif /* STATE_DEBUG */
514 }
515 
516 /* shift the input buffer to discard one character and reread data */
517 void
519 {
520  memmove (inbuffer, inbuffer + 1, (size_t)-- inbuflen);
521  inbufptr = inbuffer;
522 #ifdef STATE_DEBUG
523  report (6, "Character discarded, buffer %d chars = %s\n",
525 #endif /* STATE_DEBUG */
526 }
527 
528 
529 /* entry points begin here */
530 
531 /* get 0-origin big-endian words relative to start of packet buffer */
532 #define getword(i) (short)(session->inbuffer[2*(i)] | (session->inbuffer[2*(i)+1] << 8))
533 
534 
535 /* grab a packet; returns ether BAD_PACKET or the length */
536 ssize_t
537 ocean::packet_parse (size_t newdata)
538 {
539 #ifdef STATE_DEBUG
540  report (6, "Read %d chars to buffer offset %d (total %d): %s\n",
541  newdata,
542  inbuflen, inbuflen + newdata, gpsd_hexdump (inbufptr, newdata));
543 #endif /* STATE_DEBUG */
544 
545  outbuflen = 0;
546  inbuflen += newdata;
547 #if 0
548  inbuffer[inbuflen] = '\0';
549 
550  report (5, "Input buffer: %s\n", inbuffer);
551 #endif
552  while (inbufptr < (inbuffer + inbuflen))
553  {
554  unsigned char c = *inbufptr++;
555  static const char *state_table[] = {
556  "GROUND_STATE", /* we don't know what packet type to expect */
557  "NMEA_DOLLAR", /* we've seen first character of NMEA leader */
558  "NMEA_PUB_LEAD", /* seen second character of NMEA G leader */
559  "NMEA_LEADER_END", /* seen end char of NMEA leader, in body */
560  "NMEA_CR", /* seen terminating \r of NMEA packet */
561  "NMEA_RECOGNIZED" /* saw trailing \n of NMEA packet */
562  };
563  nextstate (c);
564  report (7, "%08ld: character '%c' [%02x], new state: %s\n",
565  char_counter,
566  (isprint (c) ? c : '.'), c, state_table[packetState]);
567  char_counter++;
568 
569  if (packetState == GROUND_STATE)
570  {
572  }
573  else if (packetState == NMEA_RECOGNIZED)
574  {
575  bool checksum_ok = true;
576  char csum[3];
577  char *trailer = (char *) inbufptr - 5;
578  //report (6, "Trailing character=%x\n", trailer[0]);
579  if (*trailer == '%') //Ocean-Server doesn't follow convention and uses a percent sign
580  {
581  unsigned int n, crc = 0;
582  for (n = 1; (char *) inbuffer + n < trailer; n++)
583  crc ^= inbuffer[n];
584  (void) snprintf (csum, sizeof (csum), "%02X", crc);
585  checksum_ok = (toupper (csum[0]) == toupper (trailer[1])
586  && toupper (csum[1]) == toupper (trailer[2]));
587  //report (6, "checksum_ok=%d\n", checksum_ok);
588  }
589  if (checksum_ok)
591 
592  packetState = GROUND_STATE;
593  packet_discard ();
594  break; // once we get a packet, get out
595  }
596  } /* while */
597 
598  return (ssize_t) newdata;
599 }
600 
601 #undef getword
602 
603 /* grab a packet; returns ether BAD_PACKET or the length */
604 ssize_t
606 {
607  ssize_t newdata;
608  newdata =
609  read (inputDevice, inbuffer + inbuflen, sizeof (inbuffer) - (inbuflen));
610  if ((newdata < 0) && (errno != EAGAIN))
611  return BAD_PACKET;
612  else if ((newdata == 0) || ((newdata < 0) && (errno == EAGAIN)))
613  return NO_PACKET;
614 
615 #if (FILE_LOGGING > 0)
616  write( outputFile, inbuffer + inbuflen, newdata );
617 #endif
618 
619  return packet_parse ((size_t) newdata);
620 }
621 
622 /* return the packet machine to the ground state */
623 void
625 {
627  inbuflen = 0;
628  inbufptr = inbuffer;
630 }
631 
636 /**************************************************************************
637  *
638  * NMEA sentence handling begins here
639  *
640  **************************************************************************/
641 unsigned int ocean::processSystem (int count, char *field[])
642 {
643  report (2, "processSystem message\n");
644  /*
645  * $S,01,270F,02,00,04,60%21
646  * 1 01 Field
647  * 2 minutes to discharge in unsigned 16bit Hex
648  * 3 02 Field
649  * 4 reserved
650  * 5 03 Field
651  * 6 ASCII text message
652  *
653  */
654  server.last_system_update = ros::Time::now();
655 
656  for( int index = 1; index < count; )
657  {
658  //report (4, "field[1]=%s\n", field[1]);
659  int tmp = atoi(field[index]);
660  ++index;
661  if( field[index] != NULL )
662  {
663  switch(tmp)
664  {
665  case 1:
666  server.time_left.fromSec((double)convertStringBase16(field[index]) * 60);
667  report (5, "timeLeft=%d\n", server.time_left.sec);
668  break;
669  case 3:
670  server.message.assign(field[index]);
671  //sscanf( field[index], "%s", server.message.c_str());
672  report (5, "processSystem message=%s\n", server.message.c_str());
673  break;
674  case 4:
675  server.average_charge = (int32_t)convertStringBase16(field[index]);
676  report (5, "averageCharge=%x\n", server.average_charge);
677  break;
678  default:
679  ;
680  }
681  }
682  ++index;
683  }
684 
685  return 0;
686 }
687 
688 unsigned int ocean::processController (int count, char *field[])
689 {
690  report (2, "processController message\n");
691  /*
692  * $C1,01,FF,02,00,03,FF,04,00,05,00,06,00,07,00%72
693  * 1 01 Field
694  * 2 Batteries present mask
695  * 3 02 Field
696  * 4 Batteries charging mask
697  * 5 03 Field
698  * 6 Batteries supplying power mask
699  *
700  * 8 reserved
701  *
702  * 10 charging mask
703  *
704  * 12 "Power No Good" status
705  *
706  * 14 Charge Inhibited mask
707  *
708  */
709 
710  server.last_controller_update = ros::Time::now();
711 
712  for( int index = 1; index < count; )
713  {
714  int tmp = atoi (field[index]);
715  ++index;
716  //report (5, "field[%d]=%s field[%d]=%s\n", index, field[index], index+1, field[index+1]);
717  long int value = convertStringBase16(field[index]);
718  report (5, "switch=%d value=0x%x\n", tmp, value);
719 
720  /*
721  if ( tmp != fieldCount)
722  {
723  report (1, "Error processing Controller message\n");
724  }
725  else
726  */
727  {
728  switch(tmp)
729  {
730  case 1:
731  {
732  for(int xx = 0; xx < server.MAX_BAT_COUNT; ++xx)
733  {
734  server.battery[xx].present = value & 1;
735  value = value >> 1;
736  }
737  }
738  break;
739  case 2:
740  {
741  for(int xx = 0; xx < server.MAX_BAT_COUNT; ++xx)
742  {
743  server.battery[xx].charging = value & 1;
744  value = value >> 1;
745  }
746  }
747  break;
748  case 3:
749  {
750  for(int xx = 0; xx < server.MAX_BAT_COUNT; ++xx)
751  {
752  server.battery[xx].discharging = value & 1;
753  value = value >> 1;
754  }
755  }
756  break;
757  case 4:
758 #if 0 //we don't care about reserved. --Curt
759  {
760  for(int xx = 0; xx < server.MAX_BAT_COUNT; ++xx)
761  {
762  server.battery[xx].reserved = value & 1;
763  value = value >> 1;
764  }
765  }
766 #endif
767  break;
768  case 5:
769  {
770  for(int xx = 0; xx < server.MAX_BAT_COUNT; ++xx)
771  {
772  server.battery[xx].power_present = value & 1;
773  value = value >> 1;
774  }
775  }
776  break;
777  case 6:
778  {
779  for(int xx = 0; xx < server.MAX_BAT_COUNT; ++xx)
780  {
781  server.battery[xx].power_no_good = value & 1;
782  value = value >> 1;
783  }
784  }
785  break;
786  case 7:
787  {
788  for(int xx = 0; xx < server.MAX_BAT_COUNT; ++xx)
789  {
790  server.battery[xx].inhibited = value & 1;
791  value = value >> 1;
792  }
793  }
794  break;
795  }
796  }
797  ++index;
798  }
799 
800  return 0;
801 }
802 
803 unsigned int ocean::processBattery (int count, char *field[])
804 {
805 
806  report (2, "processBattery %s\n", field[0]);
807  //report (5, "currentBattery=%d \n", currentBattery);
808  //
809  //$B11,02,000A,01,0294,03,0080,08,0B94,09,4115,0A,FFEC,0B,FEC3%3D
810  //$B11,0C,000A,0D,0060,0E,0062,0F,1952,10,1A45,11,4B67,12,0519%33
811  //$B11,13,FFFF,14,0000,15,41A0,16,00E0,17,0022,18,19C8,19,3840%3C
812  //
813  // 0 Battery number
814  // 1 register number
815  // 2 register value
816  // .. repeat
817 
818  unsigned int battery = (unsigned int)(field[0][2] - '0');
819  --battery;
820  report (5, "processBattery count=%d \n", count);
821  report (5, "currentBattery=%d \n", battery);
822 
823  server.battery[battery].last_battery_update = ros::Time::now();
824  --count; //get past sentence type
825 
826 #if 0
827  if( (count & 1) == 1 ) //should only have even count
828  {
829  report (0, "received odd count=%d \n", count);
830  count = count & 0xFE;
831  }
832 #endif
833 
834  int32_t regNumber;
835  int16_t value;
836  unsigned int xx = 1;
837  while(count > 1) // Must have a pair left to process --Curt
838  {
839  regNumber = (unsigned int)convertStringBase16( field[xx] );
840  ++xx;
841  value = (unsigned int)convertStringBase16( field[xx] );
842  ++xx;
843  report (5, "reg[%u]=%x \n", regNumber, value);
844  if(regNumber >= server.MAX_BAT_REG)
845  {
846  report (2, "Register greater than expected: %x MAX_BAT_REG=%x\n", regNumber, server.MAX_BAT_REG);
847  }
848  else
849  {
850  server.battery[battery].battery_register[regNumber] = value;
851  server.battery[battery].battery_update_flag[regNumber] = 1;
852  server.battery[battery].battery_register_update[regNumber] = ros::Time::now();
853  }
854 
855  count -= 2;
856  }
857 
858  return 0;
859 }
860 
861 /**************************************************************************
862  *
863  * Entry points begin here
864  *
865  **************************************************************************/
866 
867 /* parse an NMEA sentence, unpack it into a session structure */
868 unsigned int
870 {
871  static struct
872  {
873  const char *name;
874  int funcNum;
875  } nmea_phrase[] =
876  {
877  {
878  "S", 1}, //System
879  {
880  "C1", 2}, //Controller
881  {
882  "B1", 3}, //Battery
883  };
884  char buf[NMEA_MAX + 1];
885  static char zeroStr[] = "0";
886 
887  int count;
888  unsigned int retval = 0;
889  unsigned int i;
890  char *p, *field[NMEA_MAX], *s;
891 
892  /* make an editable copy of the sentence */
893  strncpy (buf, (const char *) outbuffer, NMEA_MAX);
894 
895  /* discard the checksum part */
896  for (p = buf; (*p != '%') && (*p >= ' ');)
897  ++p;
898 
899  *p = '\0';
900  /* split sentence copy on commas, filling the field array */
901 
902  for (count = 0, p = (char *) buf; (p != 0) && (*p != 0); p = strchr (p, ','))
903  {
904  *p = 0;
905  ++p;
906  if ((*p) != ',')
907  {
908  field[count] = p;
909  }
910  else
911  field[count] = zeroStr;
912 
913  ++count;
914  }
915 
916  /* dispatch on field zero, the sentence tag */
917  for (i = 0; i < (unsigned) (sizeof (nmea_phrase) / sizeof (nmea_phrase[0])); ++i)
918  {
919  s = field[0];
920  //if (strlen (nmea_phrase[i].name) == 3)
921  //s += 1; /* skip talker ID */
922  if (strncmp (nmea_phrase[i].name, s, 2) == 0)
923  {
924  switch (nmea_phrase[i].funcNum)
925  {
926  case 1:
927  processSystem (count, field);
928  retval = 1;
929  break;
930  case 2:
931  processController (count, field);
932  retval = 2;
933  break;
934  case 3:
935  processBattery (count, field);
936  retval = 3;
937  break;
938  default:
939  retval = 0;
940  break;
941  }
942 
943  strncpy (tag, nmea_phrase[i].name, MAXTAGLEN);
944  sentenceLength = strlen ((const char *) outbuffer);
945  report (5, "Got Packet: type=%s\n", tag);
946  break;
947  }
948  }
950  return retval;
951 }
952 
953 /* add NMEA checksum to a possibly *-terminated sentence */
954 void
955 ocean::nmea_add_checksum (char *sentence)
956 {
957  unsigned char sum = '\0';
958  char c, *p = sentence;
959 
960  if (*p == '$')
961  {
962  p++;
963  }
964  else
965  {
966  report (1, "Bad NMEA sentence: '%s'\n", sentence);
967  }
968  while (((c = *p) != '*') && (c != '\0'))
969  {
970  sum ^= c;
971  p++;
972  }
973  *p++ = '*';
974  (void) snprintf (p, 5, "%02X\r\n", (unsigned) sum);
975 }
976 
977 /* ship a command to the OCEAN, adding * and correct checksum */
978 int
979 ocean::nmea_send (const char *fmt, ...)
980 {
981  int status;
982  char buf[BUFSIZ];
983  va_list ap;
984 
985  va_start (ap, fmt);
986  (void) vsnprintf (buf, sizeof (buf) - 5, fmt, ap);
987  va_end (ap);
988  if (fmt[0] == '$')
989  {
990  strcat (buf, "*");
991  nmea_add_checksum (buf);
992  }
993  else
994  strcat (buf, "\r\n");
995  status = (int) write (inputDevice, buf, strlen (buf));
996  if (status == (int) strlen (buf))
997  {
998  report (3, "=> OCEAN: %s\n", buf);
999  return status;
1000  }
1001  else
1002  {
1003  report (3, "=> OCEAN: %s FAILED\n", buf);
1004  return -1;
1005  }
1006 }
1007 
1008 int
1009 ocean::string_send (const char *fmt, ...)
1010 {
1011  int status;
1012  char buf[BUFSIZ];
1013  va_list ap;
1014 
1015  va_start (ap, fmt);
1016  (void) vsnprintf (buf, sizeof (buf) - 5, fmt, ap);
1017  va_end (ap);
1018  status = (int) write (inputDevice, buf, strlen (buf));
1019  if (status == (int) strlen (buf))
1020  {
1021  report (3, "=> Ocean: %s\n", buf);
1022  return status;
1023  }
1024  else
1025  {
1026  report (3, "=> Ocean: %s FAILED\n", buf);
1027  return -1;
1028  }
1029 }
1030 
static const unsigned regListLength
Definition: ocean.h:100
int nmea_send(const char *fmt,...)
Definition: ocean.cpp:979
const std::string name
Definition: ocean.h:32
void set_speed(int speed)
Definition: ocean.cpp:312
char * gpsd_hexdump(void *binbuf, size_t binbuflen)
Definition: ocean.cpp:394
unsigned char outbuffer[OUTPUT_BUF_SIZE+1]
Definition: ocean.h:89
static const int BAD_PACKET
Definition: ocean.h:27
XmlRpcServer s
static const unsigned int MAX_PACKET_LENGTH
Definition: ocean.h:26
unsigned int nmea_parse()
Definition: ocean.cpp:869
unsigned char inbuffer[INPUT_BUF_SIZE+1]
Definition: ocean.h:88
unsigned char * inbufptr
Definition: ocean.h:91
static const int NMEA_PACKET
Definition: ocean.h:29
static const int NO_PACKET
Definition: ocean.h:28
static const int MAXTAGLEN
Definition: ocean.h:21
int string_send(const char *fmt,...)
Definition: ocean.cpp:1009
unsigned long char_counter
Definition: ocean.h:92
void packet_accept(int packet_type)
Definition: ocean.cpp:481
void initialize(const std::string &input_dev)
Definition: ocean.cpp:98
unsigned int processSystem(int count, char *field[])
Definition: ocean.cpp:641
ssize_t packet_parse(size_t newdata)
Definition: ocean.cpp:537
char tag[MAXTAGLEN]
Definition: ocean.h:93
void nextstate(unsigned char c)
Definition: ocean.cpp:418
static const int NMEA_MAX
Definition: ocean.h:20
unsigned int processBattery(int count, char *field[])
Definition: ocean.cpp:803
struct termios ttyset
Definition: ocean.h:76
ocean(int id, int debug=0)
Definition: ocean.cpp:72
unsigned int processController(int count, char *field[])
Definition: ocean.cpp:688
void report(int errlevel, const char *fmt,...)
Definition: ocean.cpp:357
static Time now()
void nmea_add_checksum(char *sentence)
Definition: ocean.cpp:955
void read_file(const std::string &input)
Definition: ocean.cpp:179
long int convertStringBase16(const char *input)
Definition: ocean.cpp:148
static const struct regPair regList[]
Definition: ocean.h:99


ocean_battery_driver
Author(s): Tully Foote, Curt Meyers
autogenerated on Wed Jun 5 2019 20:40:35