main.cpp
Go to the documentation of this file.
00001 #include <math.h>
00002 #include <time.h>
00003 #include <stdio.h>
00004 #include <stdlib.h>
00005 #include <strings.h>
00006 
00007 #include <sys/stat.h>
00008 #include <sys/time.h>
00009 #include <sys/types.h>
00010 
00011 #include <fcntl.h>
00012 #include <termios.h>
00013 #include <signal.h>
00014 #include <unistd.h>
00015 
00016 #include <ros/ros.h>
00017 #include <sensor_msgs/NavSatFix.h>
00018 
00019 #define BAUDRATE B115200
00020 #define _POSIX_SOURCE 1
00021 
00022 #define FALSE 0
00023 #define TRUE 1
00024 
00025 #define NavDataFieldAt 6
00026 
00027 typedef union _covL4 {
00028   long uLong;
00029   unsigned char bytes[4];
00030 } covL4;
00031 covL4 uLongitude, uLatitude, uHeightAE, uHeightASL;
00032 
00033 typedef union _covU4 {
00034   unsigned long uLong;
00035   unsigned char bytes[4];
00036 } covU4;
00037 covU4 uHoriAccu, uVertAccu;
00038 
00039 int timetodie = 0;
00040 void sigproc(int param)
00041 { 
00042   signal(SIGINT, sigproc);
00043   timetodie = 1;
00044   printf("Got ctrl-c \n");
00045 }
00046 
00047 int main(int argc, char** argv)
00048 {
00049   ros::init(argc, argv, "receive_ublox");
00050   ros::NodeHandle nh;
00051 
00052   int fd, nRead, msgcount = 0;
00053   struct termios oldtio,newtio;
00054   char buf[1024];
00055   struct timeval tv;
00056   struct timeval old;
00057 
00058   signal(SIGINT, sigproc);
00059 
00060   std::string modeDevice;
00061   nh.param("ublox_port", modeDevice, std::string("/dev/ttyACM0"));
00062   fd = open(modeDevice.c_str(), O_RDWR);
00063   if (fd <0) {
00064     printf("\nCannot find GPS device at %s. \n\n", modeDevice.c_str());
00065     return(0); 
00066   }
00067   
00068   printf("\nGPS device opened.\n\n");
00069   tcgetattr(fd, &oldtio);
00070   bzero(&newtio, sizeof(newtio));
00071   
00072   newtio.c_lflag &= ~(ECHO | ICANON | IEXTEN | ISIG);
00073   newtio.c_iflag &= ~(BRKINT | ICRNL | ISTRIP | IXON);
00074   newtio.c_cflag &= ~(CSIZE | PARENB);
00075   newtio.c_cflag |= CS8;
00076   newtio.c_oflag &= ~(OPOST);
00077   newtio.c_cc[VMIN] = 255;
00078   newtio.c_cc[VTIME] = 0;
00079   
00080   cfsetispeed(&newtio, BAUDRATE);
00081   cfsetospeed(&newtio, BAUDRATE); 
00082   
00083   tcflush(fd, TCIFLUSH);
00084   tcsetattr(fd, TCSANOW, &newtio);
00085 
00086   ros::Publisher gpspub = nh.advertise<sensor_msgs::NavSatFix> ("/gps/data", 1);
00087   sensor_msgs::NavSatFix gpsdata;
00088   gpsdata.header.frame_id = "gps";
00089   gpsdata.status.status = 0;
00090   gpsdata.status.service = 1;
00091   gpsdata.position_covariance_type = 1;
00092 
00093   int k = 0, head1 = 0, head2 = 0, classId = 0, pkgId = 0, navDataId = 0;
00094   char HEAD1 = 0xB5, HEAD2 = 0x62, CLASSID = 0x01, PKGID = 0x02;
00095   int toNavData = 0, bCount = 0;
00096   double longitude, latitude, heightAE, heightASL, horiAccu, vertAccu;
00097 
00098   while (!timetodie) {
00099     nRead = read(fd, buf, 255);
00100     if (nRead < 1) {
00101         printf("\nWarning read returned %d.\n\n", nRead);
00102         continue;
00103     } 
00104 
00105     k = 0;
00106 HD: while (head1 != 1 && k < nRead) {
00107       if (buf[k++] == HEAD1) {
00108         head1 = 1;              
00109       }
00110     }
00111 
00112     if (head1 == 1 && head2 != 1 && k < nRead) {
00113       if (buf[k++] == HEAD2) {
00114         head2 = 1;              
00115       } else { 
00116         head1 = 0;
00117         goto HD;
00118       }
00119     }
00120 
00121     if (head1 == 1 && head2 == 1 && classId != 1 && k < nRead) {
00122       if (buf[k++] == CLASSID) {
00123         classId = 1;            
00124       } else {
00125         head1 = head2 = 0;
00126         goto HD;
00127       }
00128     }
00129      
00130     if (head1 == 1 && head2 == 1 && classId == 1 && pkgId != 1 && k < nRead) {
00131       if (buf[k++] == PKGID) {
00132         pkgId = 1;      
00133         toNavData = NavDataFieldAt;
00134       } else {
00135         head1 = head2 = classId = 0;
00136         goto HD;
00137       }
00138     }
00139      
00140     while (head1 == 1 && head2 == 1 && classId == 1 && pkgId == 1 
00141            && navDataId != 1 && k < nRead) {
00142       while (k < nRead && toNavData > 0) {
00143         k++; toNavData--;
00144       }
00145 
00146       if (toNavData == 0) {
00147         navDataId = 1;
00148       } else {
00149         goto CNT;
00150       }
00151     }
00152 
00153     if (head1 == 1 && head2 == 1 && classId == 1 && pkgId == 1 
00154         && navDataId == 1 && bCount != 24 && k < nRead) {
00155       if (bCount < 4) {
00156         do {
00157           uLongitude.bytes[bCount++] = buf[k++];
00158         } while (k < nRead && bCount < 4);
00159       } 
00160   
00161       if (bCount >= 4 && bCount < 8) {
00162         do {
00163           uLatitude.bytes[(bCount++) - 4] = buf[k++];
00164         } while (k < nRead && bCount < 8);
00165       } 
00166 
00167       if (bCount >= 8 && bCount < 12) {
00168         do {
00169           uHeightAE.bytes[(bCount++) - 8] = buf[k++];
00170         } while (k < nRead && bCount < 12);
00171       }
00172 
00173       if (bCount >= 12 && bCount < 16) {
00174         do {
00175           uHeightASL.bytes[(bCount++) - 12] = buf[k++];
00176         } while (k < nRead && bCount < 16);
00177       }
00178 
00179       if (bCount >= 16 && bCount < 20) {
00180         do {
00181           uHoriAccu.bytes[(bCount++) - 16] = buf[k++];
00182         } while (k < nRead && bCount < 20);
00183       } 
00184 
00185       if (bCount >= 20) {
00186         do {
00187           uVertAccu.bytes[(bCount++) - 20] = buf[k++];
00188         } while (k < nRead && bCount < 24);
00189       }
00190 
00191       if (bCount == 24) {
00192         longitude = (double)uLongitude.uLong / 10000000.0;
00193         latitude = (double)uLatitude.uLong / 10000000.0;
00194         heightAE = (double)uHeightAE.uLong / 1000.0;
00195         heightASL = (double)uHeightASL.uLong / 1000.0;
00196         horiAccu = (double)uHoriAccu.uLong / 1000.0;
00197         vertAccu = (double)uVertAccu.uLong / 1000.0;
00198 
00199         gpsdata.header.stamp = ros::Time::now();
00200         gpsdata.longitude = longitude;
00201         gpsdata.latitude = latitude;
00202         gpsdata.altitude = heightAE;
00203 
00204         gpsdata.position_covariance[0] = horiAccu * horiAccu / 2;
00205         gpsdata.position_covariance[4] = horiAccu * horiAccu / 2;
00206         gpsdata.position_covariance[8] = vertAccu * vertAccu;
00207 
00208         gpspub.publish(gpsdata);
00209 
00210         head1 = head2 = classId = pkgId = navDataId = bCount = 0;
00211         goto HD;
00212       }
00213     }
00214 
00215 CNT:gettimeofday(&tv, NULL);
00216     msgcount++;
00217     
00218     if (!(msgcount % 100)) {
00219       old = tv;
00220       fflush(0);
00221     }
00222   }
00223 
00224   tcsetattr(fd, TCSANOW, &oldtio);
00225   return(0);
00226 }
00227 


receive_ublox
Author(s): Ji Zhang, Wenfan Shi, Ben Grocholsky
autogenerated on Wed Aug 26 2015 15:50:44