ocean.cpp
Go to the documentation of this file.
00001 
00002 #include <stdio.h>
00003 #include <stdlib.h>
00004 #include <errno.h>
00005 #include <stdarg.h>
00006 #include <unistd.h>
00007 #include <ctype.h>
00008 #include <fcntl.h>
00009 #include <math.h>
00010 #include <poll.h>
00011 #include <limits.h>
00012 #define FILE_LOGGING 0
00013 #include "ocean.h"
00014 #include "ros/time.h"
00015 
00016 using namespace willowgarage::ocean;
00017 
00018 // Define these consts here to make gcc4 happy
00019 const int ocean::NMEA_MAX;
00020 const int ocean::MAXTAGLEN;
00021 const int ocean::MAXCHANNELS;
00022 
00023 const int ocean::INPUT_BUF_SIZE;
00024 const int ocean::OUTPUT_BUF_SIZE;
00025 const unsigned int ocean::MAX_PACKET_LENGTH;
00026 const int ocean::BAD_PACKET;
00027 const int ocean::NO_PACKET;
00028 const int ocean::NMEA_PACKET;
00029 
00030 //Battery Registers
00031 //Takend from the Smart Battery Data Specification Revision 1.1, Dec. 11, 1998
00032 //
00033 const struct ocean::regPair ocean::regList[] = {
00034       {"Manufacturer Access"      ,""      , 0x00},
00035       {"Remaining Capacity Alarm" ,""      , 0x01},
00036       {"Remaining Time Alarm"     ,"min"   , 0x02},
00037       {"Battery Mode"             ,""      , 0x03},
00038       {"At Rate"                  ,"mA"    , 0x04},  //could also be mW
00039       {"At Rate Time To Full"     ,"min"   , 0x05},
00040       {"At Rate Time To Empty"    ,"min"   , 0x06},
00041       {"At Rate OK"               ,"bool"  , 0x07},
00042       {"Temperature"              ,"0.1 K" , 0x08}, 
00043       {"Voltage"                  ,"mV"    , 0x09}, 
00044       {"Current"                  ,"mA"    , 0x0a}, 
00045       {"Average Current"          ,"mA"    , 0x0b}, 
00046       {"Max Error"                ,"%"     , 0x0c}, 
00047       {"Relative State Of Charge" ,"%"     , 0x0d},
00048       {"Absolute State Of Charge" ,"%"     , 0x0e}, 
00049       {"Remaining Capacity"       ,"mAh"   , 0x0f},  //could also be mWh
00050       {"Full Charge Capacity"     ,"mAh"   , 0x10},  //could also be mWh
00051       {"Run Time To Empty"        ,"min"   , 0x11}, 
00052       {"Average Time To Empty"    ,"min"   , 0x12}, 
00053       {"Average Time To Full"     ,"min"   , 0x13}, 
00054       {"Battery Status"           ,""      , 0x16}, 
00055       {"Cycle Count"              ,"cycle" , 0x17}, 
00056       {"Design Capacity"          ,"mAh"   , 0x18}, //could also be mWh
00057       {"Design Voltage"           ,"mV"    , 0x19}, 
00058       {"Specification Info"       ,""      , 0x1a}, 
00059       {"Manufacture Date"         ,"DMY"   , 0x1b}, 
00060       {"Serial Number"            ,"uint"  , 0x1c}, 
00061       {"Manufacture Name"         ,"string", 0x20}, 
00062       {"Device Name"              ,"string", 0x21}, 
00063       {"Device Chemistry"         ,"string", 0x22}, 
00064       {"Manufacture Data"         ,""      , 0x23}
00065     };
00066 const unsigned ocean::regListLength(sizeof(regList)/ sizeof(struct regPair));
00072 ocean::ocean ( int id,  int debug)
00073 {
00074   debuglevel = debug;
00075   server.id = id;
00076   server.battery.resize(4);
00077 
00078   // Mark last update as "-1" for initial values
00079   for (uint i = 0; i < server.battery.size(); ++i)
00080     server.battery[i].last_battery_update = ros::Time(-1);
00081 }
00082 
00083 ocean::~ocean ()
00084 {
00085   if (close (inputDevice) != 0)
00086   {
00087     fprintf (stderr, "failed closing serial device: %s\n", strerror (errno));
00088   }
00089 }
00090 
00091 void
00092 ocean::setDebugLevel (int level)
00093 {
00094   debuglevel = level;
00095 }
00096 
00097 void
00098 ocean::initialize (const std::string &input_dev)
00099 {
00100   inputDevice = open (input_dev.c_str(), O_RDWR | O_NOCTTY);
00101   //printf ("inputDevice %d\n", inputDevice);
00102   if (inputDevice < 0)
00103   {
00104     fprintf (stderr, "failed to open tty device [%s]: %s\n", input_dev.c_str(), strerror (errno));
00105   }
00106   if (isatty (inputDevice) != 1)
00107   {
00108     fprintf (stderr, "Device [%s] not a tty device.\n", input_dev.c_str());
00109   }
00110   report (2, "Device opened dev=%d\n", inputDevice);
00111 
00112   tcgetattr (inputDevice, &ttyset);     //get current port settings
00113 
00114   ttyset.c_cflag &= ~(PARENB | PARODD | CRTSCTS);
00115   ttyset.c_cflag |= CREAD | CLOCAL;
00116   ttyset.c_iflag = ttyset.c_oflag = ttyset.c_lflag = (tcflag_t) 0;
00117 
00118   set_speed (19200);
00119 
00120 #if (FILE_LOGGING > 0)
00121   char logname[128];
00122   sprintf ( logname, "/tmp/oceanServer%c.log", input_dev[input_dev.length()-1]);
00123   report (2, "Logging to file: %s\n", logname);
00124 
00125   outputFile = open( logname, (O_WRONLY | O_APPEND ) );
00126 
00127   if(outputFile < 0)
00128   {
00129     if(errno == ENOENT )
00130     {
00131       outputFile = open( logname, (O_WRONLY | O_CREAT), (S_IRWXU | S_IRWXG) );
00132     }
00133 
00134     if((outputFile < 0))
00135     {
00136       report (2, "Failed to open log file: %d\n", errno);
00137       close(inputDevice);
00138       exit(outputFile);
00139     }
00140   }
00141 #endif
00142 
00143   if(commTest())  //If the Ocean isn't talking then get it into the NMEA mode
00144     resetOcean();
00145 
00146 }
00147 
00148 long int ocean::convertStringBase16( const char* input )
00149 {
00150   char *endptr;
00151   errno = 0;
00152 
00153   if( input == NULL )
00154   {
00155     report (1, "convertStringBase16 input NULL\n");
00156     return 0;
00157   }
00158 
00159   long int result = strtol( input, &endptr, 16 );
00160   if( ((errno == ERANGE) && (( result == LONG_MIN ) || ( result == LONG_MAX ))) || ((errno != 0) && (result == 0)))
00161   {
00162     report (0, "strtol failure\n");
00163     return 0;
00164   }
00165 
00166   if( endptr == input )
00167   {
00168     report (0, "strtol No digits found\n");
00169     return 0;
00170   }
00171 
00172   if( *endptr != '\0' ) //not always and error, but probably not good
00173     report( 1, "strtol characters left after conversion: %s\n", endptr);
00174 
00175   return result;
00176 }
00177 
00178 void
00179 ocean::read_file (const std::string &input)
00180 {
00181   inputDevice = open (input.c_str(), O_RDONLY);
00182   if (inputDevice < 0)
00183   {
00184     fprintf (stderr, "failed to open tty file [%s]: %s\n", input.c_str(), strerror (errno));
00185   }
00186 
00187   packet_reset();
00188 }
00189 
00190 int ocean::commTest()
00191 {
00192   int maxTries = 10;
00193   fd_set rfds;
00194   struct timeval tv;
00195   int retval;
00196 
00197   packet_reset();
00198 
00199   int listenCount = 50;
00200   do
00201   {
00202     maxTries = 10;
00203 
00204     while((packetType != NMEA_PACKET) && (maxTries--))
00205     {
00206       report(5,"commTest: call packet_get\n");
00207       int result = NO_PACKET;
00208 
00209       FD_ZERO(&rfds);
00210       FD_SET(inputDevice, &rfds);
00211       tv.tv_sec = 2;
00212       tv.tv_usec = 0;
00213 
00214       retval = select( inputDevice + 1, &rfds, NULL, NULL, &tv);
00215       if(retval < 0)
00216         report(0, "select error\n");
00217       else if(retval > 0)
00218       {
00219         {
00220           report(5, "calling packet_get\n");
00221           result = packet_get();
00222         }
00223       }
00224       else
00225       {
00226         report(2, "select timeout\n");
00227         listenCount -= 10;
00228         maxTries = 0;
00229       }
00230       usleep(1000);
00231     }
00232 
00233     if(packetType == NMEA_PACKET)
00234     {
00235       report(5,"NMEA packet\n");
00236       if(nmea_parse() > 0)
00237       {
00238         return(0);
00239       }
00240     } else
00241        report(5,"non NMEA packet\n");
00242 
00243     report(6, "listenCount=%d\n", listenCount);
00244   } while( listenCount-- > 0);
00245 
00246   return(-1);
00247 }
00248 
00249 void
00250 ocean::resetOcean()
00251 {
00252   report(5, "Sending ocean reset string\n");
00253   string_send(" ");
00254   usleep(1000);
00255   string_send("x");
00256   usleep(1000);
00257 }
00258 
00259 int
00260 ocean::run (void)
00261 {
00262   int status (0);
00263 
00264   packet_get ();
00265   if (packetType == NMEA_PACKET)
00266   {
00267     status = nmea_parse ();
00268   }
00269   return status;
00270 }
00271 
00275 void
00276 ocean::flush (void)
00277 {
00278   tcflush (inputDevice, TCIOFLUSH);
00279 }
00280 
00281 
00282 int
00283 ocean::get_speed (void)
00284 {
00285   speed_t code = cfgetospeed (&ttyset);
00286   switch (code)
00287   {
00288     case B0:
00289       return (0);
00290     case B300:
00291       return (300);
00292     case B1200:
00293       return (1200);
00294     case B2400:
00295       return (2400);
00296     case B4800:
00297       return (4800);
00298     case B9600:
00299       return (9600);
00300     case B19200:
00301       return (19200);
00302     case B38400:
00303       return (38400);
00304     case B57600:
00305       return (57600);
00306     default:
00307       return (115200);
00308   }
00309 }
00310 
00311 void
00312 ocean::set_speed (int newspeed)
00313 {
00314   unsigned int rate;
00315   if (newspeed < 300)
00316     rate = B0;
00317   else if (newspeed < 1200)
00318     rate = B300;
00319   else if (newspeed < 2400)
00320     rate = B1200;
00321   else if (newspeed < 4800)
00322     rate = B2400;
00323   else if (newspeed < 9600)
00324     rate = B4800;
00325   else if (newspeed < 19200)
00326     rate = B9600;
00327   else if (newspeed < 38400)
00328     rate = B19200;
00329   else if (newspeed < 57600)
00330     rate = B38400;
00331   else if (newspeed < 115200)
00332     rate = B57600;
00333   else
00334     rate = B115200;
00335 
00336   if (cfsetispeed (&ttyset, B0) != 0)
00337     report (0, "Failed setting input speed\n");
00338   if (cfsetospeed (&ttyset, rate) != 0)
00339     report (0, "Failed setting output speed\n");
00340 
00341   ttyset.c_iflag &= ~(PARMRK | INPCK);
00342   ttyset.c_cflag &= ~(CSIZE | CSTOPB | PARENB | PARODD);
00343   //ttyset.c_cflag |= CS8 | PARENB | PARODD;
00344   ttyset.c_cflag |= CS8;
00345 
00346   if (tcsetattr (inputDevice, TCSANOW, &ttyset) != 0)
00347     report (0, "Failed to configure serial device\n");
00348 
00349   flush ();
00350   report (1, "set_speed speed=%d rate=%x\n", newspeed, rate);
00351 
00352   currentBaudRate = (unsigned int) newspeed;
00353   packet_reset ();
00354 }
00355 
00356 void
00357 ocean::report (int errlevel, const char *fmt, ...)
00358 {
00359   if (errlevel <= debuglevel)
00360   {
00361     char buf[BUFSIZ], buf2[BUFSIZ], *sp;
00362     va_list ap;
00363 
00364     (void) strcpy (buf, "ocean: ");
00365     va_start (ap, fmt);
00366     (void) vsnprintf (buf + strlen (buf), sizeof (buf) - strlen (buf), fmt,
00367                       ap);
00368     va_end (ap);
00369 
00370     buf2[0] = '\0';
00371     for (sp = buf; *sp != '\0'; sp++)
00372       if (isprint (*sp)
00373           || (isspace (*sp) && (sp[1] == '\0' || sp[2] == '\0')))
00374         (void) snprintf (buf2 + strlen (buf2), 2, "%c", *sp);
00375       else
00376         (void) snprintf (buf2 + strlen (buf2), 6, "\\x%02x", (unsigned) *sp);
00377 
00378     (void) fputs (buf2, stderr);
00379   }
00380 }
00381 
00382 
00383 enum
00384 {
00385   GROUND_STATE,                 /* we don't know what packet type to expect */
00386   NMEA_DOLLAR,                  /* we've seen first character of NMEA leader */
00387   NMEA_PUB_LEAD,                /* seen second character of NMEA G leader */
00388   NMEA_LEADER_END,              /* seen end char of NMEA leader, in body */
00389   NMEA_CR,                      /* seen terminating \r of NMEA packet */
00390   NMEA_RECOGNIZED               /* saw trailing \n of NMEA packet */
00391 };
00392 
00393 char *
00394 ocean::gpsd_hexdump (void *binbuf, size_t binbuflen)
00395 {
00396   static char hexbuf[MAX_PACKET_LENGTH * 2 + 1];
00397   size_t i;
00398   size_t len =
00399     (size_t) ((binbuflen >
00400                MAX_PACKET_LENGTH) ? MAX_PACKET_LENGTH : binbuflen);
00401   char *ibuf = (char *) binbuf;
00402   memset (hexbuf, 0, sizeof (hexbuf));
00403 
00404   for (i = 0; i < len; i++)
00405   {
00406     (void) snprintf (hexbuf + (2 * i), 3, "%02x",
00407                      (unsigned int) (ibuf[i] & 0xff));
00408   }
00409   return hexbuf;
00410 }
00411 
00417 void
00418 ocean::nextstate (unsigned char c)
00419 {
00420   switch (packetState)
00421   {
00422     case GROUND_STATE:
00423       if (c == '$')
00424       {
00425         packetState = NMEA_DOLLAR;
00426       }
00427       break;
00428 
00429     case NMEA_DOLLAR:
00430       if (c == 'S')        /* vendor sentence */
00431         packetState = NMEA_LEADER_END;
00432       else if (c == 'C')        /* vendor sentence */
00433         packetState = NMEA_LEADER_END;
00434       else if (c == 'B')        /* vendor sentence */
00435         //packetState = NMEA_PUB_LEAD;
00436         packetState = NMEA_LEADER_END;
00437       else
00438         packetState = GROUND_STATE;
00439       break;
00440 
00441 /*
00442     case NMEA_PUB_LEAD:
00443       currentBattery = c - '0'; //convert to battery number
00444       packetState = NMEA_LEADER_END;
00445       break;
00446 */
00447 
00448     case NMEA_LEADER_END:
00449       if (c == '\r')
00450         packetState = NMEA_CR;
00451       else if (c == '\n')
00452         /* not strictly correct, but helps for interpreting logfiles */
00453         packetState = NMEA_RECOGNIZED;
00454       else if (c == '$')
00455         /* faster recovery from missing sentence trailers */
00456         packetState = NMEA_DOLLAR;
00457       else if (!isprint (c))
00458         packetState = GROUND_STATE;
00459       break;
00460 
00461     case NMEA_CR:
00462       if (c == '\n')
00463         packetState = NMEA_RECOGNIZED;
00464       else
00465         packetState = GROUND_STATE;
00466       break;
00467     case NMEA_RECOGNIZED:
00468       if (c == '$')
00469         packetState = NMEA_DOLLAR;
00470       else
00471         packetState = GROUND_STATE;
00472       break;
00473 
00474   }
00475 }
00476 
00477 #define STATE_DEBUG
00478 
00479 /* packet grab succeeded, move to output buffer */
00480 void
00481 ocean::packet_accept (int packet_type)
00482 {
00483   size_t packetlen = inbufptr - inbuffer;
00484   if (packetlen < sizeof (outbuffer))
00485   {
00486     memcpy ((void *) outbuffer, (void *) inbuffer, packetlen);
00487     outbuflen = packetlen;
00488     outbuffer[packetlen] = '\0';
00489     packetType = packet_type;
00490 #ifdef STATE_DEBUG
00491     report (6, "Packet type %d accepted %d = %s\n",
00492             packet_type, packetlen, gpsd_hexdump (outbuffer, outbuflen));
00493 #endif /* STATE_DEBUG */
00494   }
00495   else
00496   {
00497     report (1, "Rejected too long packet type %d len %d\n",
00498             packet_type, packetlen);
00499   }
00500 }
00501 
00502 /* shift the input buffer to discard all data up to current input pointer */
00503 void
00504 ocean::packet_discard ()
00505 {
00506   size_t discard = inbufptr - inbuffer;
00507   size_t remaining = inbuflen - discard;
00508   inbufptr = (unsigned char *) memmove (inbuffer, inbufptr, remaining);
00509   inbuflen = remaining;
00510 #ifdef STATE_DEBUG
00511   report (6, "Packet discard of %d, chars remaining is %d = %s\n",
00512           discard, remaining, gpsd_hexdump (inbuffer, inbuflen));
00513 #endif /* STATE_DEBUG */
00514 }
00515 
00516 /* shift the input buffer to discard one character and reread data */
00517 void
00518 ocean::character_discard ()
00519 {
00520   memmove (inbuffer, inbuffer + 1, (size_t)-- inbuflen);
00521   inbufptr = inbuffer;
00522 #ifdef STATE_DEBUG
00523   report (6, "Character discarded, buffer %d chars = %s\n",
00524           inbuflen, gpsd_hexdump (inbuffer, inbuflen));
00525 #endif /* STATE_DEBUG */
00526 }
00527 
00528 
00529 /* entry points begin here */
00530 
00531 /* get 0-origin big-endian words relative to start of packet buffer */
00532 #define getword(i) (short)(session->inbuffer[2*(i)] | (session->inbuffer[2*(i)+1] << 8))
00533 
00534 
00535 /* grab a packet; returns ether BAD_PACKET or the length */
00536 ssize_t
00537 ocean::packet_parse (size_t newdata)
00538 {
00539 #ifdef STATE_DEBUG
00540   report (6, "Read %d chars to buffer offset %d (total %d): %s\n",
00541           newdata,
00542           inbuflen, inbuflen + newdata, gpsd_hexdump (inbufptr, newdata));
00543 #endif /* STATE_DEBUG */
00544 
00545   outbuflen = 0;
00546   inbuflen += newdata;
00547 #if 0
00548   inbuffer[inbuflen] = '\0';
00549 
00550   report (5, "Input buffer: %s\n", inbuffer);
00551 #endif
00552   while (inbufptr < (inbuffer + inbuflen))
00553   {
00554     unsigned char c = *inbufptr++;
00555     static const char *state_table[] = {
00556       "GROUND_STATE",           /* we don't know what packet type to expect */
00557       "NMEA_DOLLAR",            /* we've seen first character of NMEA leader */
00558       "NMEA_PUB_LEAD",          /* seen second character of NMEA G leader */
00559       "NMEA_LEADER_END",        /* seen end char of NMEA leader, in body */
00560       "NMEA_CR",                /* seen terminating \r of NMEA packet */
00561       "NMEA_RECOGNIZED"         /* saw trailing \n of NMEA packet */
00562     };
00563     nextstate (c);
00564     report (7, "%08ld: character '%c' [%02x], new state: %s\n",
00565             char_counter,
00566             (isprint (c) ? c : '.'), c, state_table[packetState]);
00567     char_counter++;
00568 
00569     if (packetState == GROUND_STATE)
00570     {
00571       character_discard ();
00572     }
00573     else if (packetState == NMEA_RECOGNIZED)
00574     {
00575       bool checksum_ok = true;
00576       char csum[3];
00577       char *trailer = (char *) inbufptr - 5;
00578       //report (6, "Trailing character=%x\n", trailer[0]);
00579       if (*trailer == '%')  //Ocean-Server doesn't follow convention and uses a percent sign
00580       {
00581         unsigned int n, crc = 0;
00582         for (n = 1; (char *) inbuffer + n < trailer; n++)
00583           crc ^= inbuffer[n];
00584         (void) snprintf (csum, sizeof (csum), "%02X", crc);
00585         checksum_ok = (toupper (csum[0]) == toupper (trailer[1])
00586                        && toupper (csum[1]) == toupper (trailer[2]));
00587         //report (6, "checksum_ok=%d\n", checksum_ok);
00588       }
00589       if (checksum_ok)
00590         packet_accept (NMEA_PACKET);
00591 
00592       packetState = GROUND_STATE;
00593       packet_discard ();
00594       break;                    // once we get a packet, get out
00595     }
00596   }                             /* while */
00597 
00598   return (ssize_t) newdata;
00599 }
00600 
00601 #undef getword
00602 
00603 /* grab a packet; returns ether BAD_PACKET or the length */
00604 ssize_t
00605 ocean::packet_get ()
00606 {
00607   ssize_t newdata;
00608   newdata =
00609     read (inputDevice, inbuffer + inbuflen, sizeof (inbuffer) - (inbuflen));
00610   if ((newdata < 0) && (errno != EAGAIN))
00611     return BAD_PACKET;
00612   else if ((newdata == 0) || ((newdata < 0) && (errno == EAGAIN)))
00613     return NO_PACKET;
00614 
00615 #if (FILE_LOGGING > 0)
00616   write( outputFile, inbuffer + inbuflen, newdata );
00617 #endif
00618 
00619   return packet_parse ((size_t) newdata);
00620 }
00621 
00622 /* return the packet machine to the ground state */
00623 void
00624 ocean::packet_reset ()
00625 {
00626   packetState = GROUND_STATE;
00627   inbuflen = 0;
00628   inbufptr = inbuffer;
00629   packetType = NO_PACKET;
00630 }
00631 
00636 /**************************************************************************
00637  *
00638  * NMEA sentence handling begins here
00639  *
00640  **************************************************************************/
00641 unsigned int ocean::processSystem (int count, char *field[])
00642 {
00643   report (2, "processSystem message\n");
00644   /*
00645    * $S,01,270F,02,00,04,60%21
00646    *  1  01 Field
00647    *  2  minutes to discharge in unsigned 16bit Hex
00648    *  3  02 Field
00649    *  4  reserved
00650    *  5  03 Field
00651    *  6  ASCII text message
00652    *
00653    */
00654   server.last_system_update = ros::Time::now();
00655 
00656   for( int index = 1; index < count; )
00657   {
00658     //report (4, "field[1]=%s\n", field[1]);
00659     int tmp = atoi(field[index]);
00660     ++index;
00661     if( field[index] != NULL )
00662     {
00663       switch(tmp)
00664       {
00665         case 1:
00666           server.time_left.fromSec((double)convertStringBase16(field[index]) * 60);
00667           report (5, "timeLeft=%d\n", server.time_left.sec);
00668           break;
00669         case 3:
00670           server.message.assign(field[index]);
00671           //sscanf( field[index], "%s", server.message.c_str());
00672           report (5, "processSystem message=%s\n", server.message.c_str());
00673           break;
00674         case 4:
00675           server.average_charge = (int32_t)convertStringBase16(field[index]);
00676           report (5, "averageCharge=%x\n", server.average_charge);
00677           break;
00678         default:
00679           ;
00680       }
00681     }
00682     ++index;
00683   }
00684 
00685   return 0;
00686 }
00687 
00688 unsigned int ocean::processController (int count, char *field[])
00689 {
00690   report (2, "processController message\n");
00691   /*
00692    * $C1,01,FF,02,00,03,FF,04,00,05,00,06,00,07,00%72
00693    *  1  01 Field
00694    *  2  Batteries present mask
00695    *  3  02 Field
00696    *  4  Batteries charging mask
00697    *  5  03 Field
00698    *  6  Batteries supplying power mask
00699    *
00700    *  8  reserved
00701    *
00702    *  10 charging mask
00703    *
00704    *  12 "Power No Good" status
00705    *
00706    *  14 Charge Inhibited mask
00707    *
00708    */
00709 
00710   server.last_controller_update = ros::Time::now();
00711 
00712   for( int index = 1; index < count; )
00713   {
00714     int tmp = atoi (field[index]);
00715     ++index;
00716     //report (5, "field[%d]=%s  field[%d]=%s\n", index, field[index], index+1, field[index+1]);
00717     long int value = convertStringBase16(field[index]);
00718     report (5, "switch=%d  value=0x%x\n", tmp, value);
00719 
00720     /*
00721     if ( tmp != fieldCount)
00722     {
00723       report (1, "Error processing Controller message\n");
00724     }
00725     else
00726     */
00727     {
00728       switch(tmp)
00729       {
00730         case 1:
00731         {
00732           for(int xx = 0; xx < server.MAX_BAT_COUNT; ++xx)
00733           {
00734             server.battery[xx].present = value & 1;
00735             value = value >> 1;
00736           }
00737         }
00738         break;
00739         case 2:
00740         {
00741           for(int xx = 0; xx < server.MAX_BAT_COUNT; ++xx)
00742           {
00743             server.battery[xx].charging = value & 1;
00744             value = value >> 1;
00745           }
00746         }
00747         break;
00748         case 3:
00749         {
00750           for(int xx = 0; xx < server.MAX_BAT_COUNT; ++xx)
00751           {
00752             server.battery[xx].discharging = value & 1;
00753             value = value >> 1;
00754           }
00755         }
00756         break;
00757         case 4:
00758 #if 0 //we don't care about reserved. --Curt
00759         {
00760           for(int xx = 0; xx < server.MAX_BAT_COUNT; ++xx)
00761           {
00762             server.battery[xx].reserved = value & 1;
00763             value = value >> 1;
00764           }
00765         }
00766 #endif
00767         break;
00768         case 5:
00769         {
00770           for(int xx = 0; xx < server.MAX_BAT_COUNT; ++xx)
00771           {
00772             server.battery[xx].power_present = value & 1;
00773             value = value >> 1;
00774           }
00775         }
00776         break;
00777         case 6:
00778         {
00779           for(int xx = 0; xx < server.MAX_BAT_COUNT; ++xx)
00780           {
00781             server.battery[xx].power_no_good = value & 1;
00782             value = value >> 1;
00783           }
00784         }
00785         break;
00786         case 7:
00787         {
00788           for(int xx = 0; xx < server.MAX_BAT_COUNT; ++xx)
00789           {
00790             server.battery[xx].inhibited = value & 1;
00791             value = value >> 1;
00792           }
00793         }
00794         break;
00795       }
00796     }
00797     ++index;
00798   }
00799 
00800   return 0;
00801 }
00802 
00803 unsigned int ocean::processBattery (int count, char *field[])
00804 {
00805 
00806   report (2, "processBattery %s\n", field[0]);
00807   //report (5, "currentBattery=%d \n", currentBattery);
00808   //
00809   //$B11,02,000A,01,0294,03,0080,08,0B94,09,4115,0A,FFEC,0B,FEC3%3D
00810   //$B11,0C,000A,0D,0060,0E,0062,0F,1952,10,1A45,11,4B67,12,0519%33
00811   //$B11,13,FFFF,14,0000,15,41A0,16,00E0,17,0022,18,19C8,19,3840%3C
00812   //
00813   // 0   Battery number
00814   // 1   register number
00815   // 2   register value
00816   // ..  repeat
00817 
00818   unsigned int battery = (unsigned int)(field[0][2] - '0');
00819   --battery;
00820   report (5, "processBattery count=%d \n", count);
00821   report (5, "currentBattery=%d \n", battery);
00822 
00823   server.battery[battery].last_battery_update = ros::Time::now();
00824   --count;  //get past sentence type
00825 
00826 #if 0
00827   if( (count & 1) == 1 )  //should only have even count
00828   {
00829     report (0, "received odd count=%d \n", count);
00830     count = count & 0xFE;
00831   }
00832 #endif
00833 
00834   int32_t regNumber;
00835   int16_t value;
00836   unsigned int xx = 1;
00837   while(count > 1)  // Must have a pair left to process --Curt
00838   {
00839     regNumber = (unsigned int)convertStringBase16( field[xx] );
00840     ++xx;
00841     value = (unsigned int)convertStringBase16( field[xx] );
00842     ++xx;
00843     report (5, "reg[%u]=%x \n", regNumber, value);
00844     if(regNumber >= server.MAX_BAT_REG)
00845     {
00846       report (2, "Register greater than expected: %x  MAX_BAT_REG=%x\n", regNumber, server.MAX_BAT_REG);
00847     }
00848     else
00849     {
00850       server.battery[battery].battery_register[regNumber] = value;
00851       server.battery[battery].battery_update_flag[regNumber] = 1;
00852       server.battery[battery].battery_register_update[regNumber] = ros::Time::now();
00853     }
00854 
00855     count -= 2;
00856   }
00857   
00858   return 0;
00859 }
00860 
00861 /**************************************************************************
00862  *
00863  * Entry points begin here
00864  *
00865  **************************************************************************/
00866 
00867 /* parse an NMEA sentence, unpack it into a session structure */
00868 unsigned int
00869 ocean::nmea_parse ()
00870 {
00871   static struct
00872   {
00873     const char *name;
00874     int funcNum;
00875   } nmea_phrase[] =
00876   {
00877     {
00878     "S", 1},                    //System
00879     {
00880     "C1", 2},                    //Controller
00881     {
00882     "B1", 3},                    //Battery
00883   };
00884   char buf[NMEA_MAX + 1];
00885   static char zeroStr[] = "0";
00886 
00887   int count;
00888   unsigned int retval = 0;
00889   unsigned int i;
00890   char *p, *field[NMEA_MAX], *s;
00891 
00892   /* make an editable copy of the sentence */
00893   strncpy (buf, (const char *) outbuffer, NMEA_MAX);
00894 
00895   /* discard the checksum part */
00896   for (p = buf; (*p != '%') && (*p >= ' ');)
00897     ++p;
00898 
00899   *p = '\0';
00900   /* split sentence copy on commas, filling the field array */
00901 
00902   for (count = 0, p = (char *) buf; (p != 0) && (*p != 0); p = strchr (p, ','))
00903   {
00904     *p = 0;
00905     ++p;
00906     if ((*p) != ',')
00907     {
00908       field[count] = p;
00909     }
00910     else
00911       field[count] = zeroStr;
00912 
00913     ++count;
00914   }
00915 
00916   /* dispatch on field zero, the sentence tag */
00917   for (i = 0; i < (unsigned) (sizeof (nmea_phrase) / sizeof (nmea_phrase[0])); ++i)
00918   {
00919     s = field[0];
00920     //if (strlen (nmea_phrase[i].name) == 3)
00921       //s += 1;                   /* skip talker ID */
00922     if (strncmp (nmea_phrase[i].name, s, 2) == 0)
00923     {
00924       switch (nmea_phrase[i].funcNum)
00925       {
00926         case 1:
00927           processSystem (count, field);
00928           retval = 1;
00929           break;
00930         case 2:
00931           processController (count, field);
00932           retval = 2;
00933           break;
00934         case 3:
00935           processBattery (count, field);
00936           retval = 3;
00937           break;
00938         default:
00939           retval = 0;
00940           break;
00941       }
00942 
00943       strncpy (tag, nmea_phrase[i].name, MAXTAGLEN);
00944       sentenceLength = strlen ((const char *) outbuffer);
00945       report (5, "Got Packet: type=%s\n", tag);
00946       break;
00947     }
00948   }
00949   packetType = NO_PACKET;
00950   return retval;
00951 }
00952 
00953 /* add NMEA checksum to a possibly  *-terminated sentence */
00954 void
00955 ocean::nmea_add_checksum (char *sentence)
00956 {
00957   unsigned char sum = '\0';
00958   char c, *p = sentence;
00959 
00960   if (*p == '$')
00961   {
00962     p++;
00963   }
00964   else
00965   {
00966     report (1, "Bad NMEA sentence: '%s'\n", sentence);
00967   }
00968   while (((c = *p) != '*') && (c != '\0'))
00969   {
00970     sum ^= c;
00971     p++;
00972   }
00973   *p++ = '*';
00974   (void) snprintf (p, 5, "%02X\r\n", (unsigned) sum);
00975 }
00976 
00977 /* ship a command to the OCEAN, adding * and correct checksum */
00978 int
00979 ocean::nmea_send (const char *fmt, ...)
00980 {
00981   int status;
00982   char buf[BUFSIZ];
00983   va_list ap;
00984 
00985   va_start (ap, fmt);
00986   (void) vsnprintf (buf, sizeof (buf) - 5, fmt, ap);
00987   va_end (ap);
00988   if (fmt[0] == '$')
00989   {
00990     strcat (buf, "*");
00991     nmea_add_checksum (buf);
00992   }
00993   else
00994     strcat (buf, "\r\n");
00995   status = (int) write (inputDevice, buf, strlen (buf));
00996   if (status == (int) strlen (buf))
00997   {
00998     report (3, "=> OCEAN: %s\n", buf);
00999     return status;
01000   }
01001   else
01002   {
01003     report (3, "=> OCEAN: %s FAILED\n", buf);
01004     return -1;
01005   }
01006 }
01007 
01008 int
01009 ocean::string_send (const char *fmt, ...)
01010 {
01011   int status;
01012   char buf[BUFSIZ];
01013   va_list ap;
01014 
01015   va_start (ap, fmt);
01016   (void) vsnprintf (buf, sizeof (buf) - 5, fmt, ap);
01017   va_end (ap);
01018   status = (int) write (inputDevice, buf, strlen (buf));
01019   if (status == (int) strlen (buf))
01020   {
01021     report (3, "=> Ocean: %s\n", buf);
01022     return status;
01023   }
01024   else
01025   {
01026     report (3, "=> Ocean: %s FAILED\n", buf);
01027     return -1;
01028   }
01029 }
01030 


ocean_battery_driver
Author(s): Tully Foote, Curt Meyers
autogenerated on Thu Jun 6 2019 21:10:57