GPS.cpp

00001 /*LIB GPS
00002 author: Giovanni Migon
00003 GPS GP 735T
00004 */
00005 
00006 #include "GPS.h"
00007 
00008 GPS::GPS(PinName pinTx, PinName pinRx, int Baud) : _serial( pinTx, pinRx, Baud){
00009     _pinTx = pinTx;
00010     _pinRx = pinRx;
00011     _Baud = Baud;
00012     _count_rx = 0;
00013     memset(&_buf_rx,0,sizeof(_buf_rx));
00014     
00015     lat = 0;
00016     lon = 0;
00017     pdop = 0;
00018 }
00019 
00020 GPS::~GPS() {
00021 }
00022 
00023 /*void GPS::SerialRecvInterrupt (void)
00024 {
00025     if( _serial.readable() ) 
00026         _buf_rx[_count_rx++%sizeof(_buf_rx)] = _serial.getc();
00027 }*/
00028 
00029 void GPS::printRX(void) {
00030     
00031     uint8_t buf_cp[512];
00032     unsigned short i=0;
00033     memcpy(buf_cp,_buf_rx,sizeof(_buf_rx));
00034     //Print all buffer
00035     for(i = 0; i < sizeof(buf_cp); i++) {
00036         //putc(buf_cp[i]);
00037         printf("%c",buf_cp[i]);
00038     }
00039     // Buffer clear
00040     //memset(&buf_cp,0,sizeof(buf_cp));
00041 }
00042 
00043 void GPS::printGPS(void) {
00044     //$GPGLL,3003.66092,S,05110.42152,W,165141.00,A,A*6E  52 caracters
00045     // 1o: split in substrings; 
00046     // 2o: validate a substring; 
00047     // 3o: print correct substring;
00048     
00049     uint8_t buf_cp[512];
00050     uint8_t buf_gpgll[52];      // gps cmd gpgll
00051     uint8_t buf_gpgsa[64];      // gps cmd gpgsa
00052     
00053     unsigned short i = 0;
00054     uint8_t j = 0;
00055     uint8_t count_p = 0;
00056     
00057     // Restart variables
00058     count_p = 0;
00059     lat = 0;
00060     lon = 0;
00061     pdop = 0;
00062     
00063     memcpy(buf_cp,_buf_rx,sizeof(_buf_rx));
00064     
00065     int size_msg = sizeof(buf_cp) - sizeof(buf_gpgsa);
00066     //char * gpgll;
00067     
00068     for(i = 0; i < size_msg ; i++) {
00069         // Detect start command character
00070         if( buf_cp[i] == '$' ) {
00071             // Detect $GPGLL
00072             if( buf_cp[i+4] == 'L' && buf_cp[i+5] == 'L' ) {
00073                 memcpy(buf_gpgll,&buf_cp[i],sizeof(buf_gpgll));
00074                 // Validation Conditions of $GPGLL
00075                 if( (buf_gpgll[18] == 'S' || buf_gpgll[18] == 'N') && (buf_gpgll[32] == 'E' || buf_gpgll[32] == 'W') ) {
00076                     lon = atof((char *)&buf_gpgll[20]);
00077                     lat = atof((char *)&buf_gpgll[7]);
00078                     //printf("%s\n",buf_gpgll);
00079                     //printf("Lat = %f;  Lon = %f;\n",lat, lon);
00080                 } else {
00081                     //printf("Error  GPGLL\n");
00082                     count_p--;
00083                 }
00084             }
00085             // Detect $GPGSA
00086             if( (buf_cp[i+4] == 'S') && (buf_cp[i+5] == 'A') ) {
00087                 memcpy(buf_gpgsa,&buf_cp[i],sizeof(buf_gpgsa));
00088                 //printf("%s@\n",buf_gpgsa);
00089                 // Validation Conditions
00090                 for(j = 20; j < sizeof(buf_gpgsa); j++) {
00091                     if(buf_gpgsa[j] == '.' && count_p == 0) {
00092                         pdop = buf_gpgsa[j-1] - 0x30;
00093                         count_p++;
00094                         //printf("Precision = %d;\n",pdop);
00095                     }
00096                 }
00097             }
00098         }
00099     }
00100     // Verify precision and correct data
00101     if( (((pdop > 0) && (pdop < 5)) && (count_p == 1) ) && lat != 0 ) {
00102         printf("Pre = %d;  Lat = %f;  Lon = %f;\n", pdop, lat, lon);
00103     } else {
00104         printf("Invalid Precision\n");
00105     }
00106 }
00107 
00108