GPS.cpp

00001 // For SiRF III
00002 
00003 #include "GPS.h"
00004 
00005 extern Serial pc;
00006 
00007 
00008 
00009 
00010 GPS::GPS(PinName tx, PinName rx, int type):
00011     hdop(0.0),
00012     serial(tx, rx)
00013 {
00014     setType(SIRF);
00015     setBaud(4800);
00016 } 
00017 
00018 
00019 void GPS::setType(int type)
00020 {
00021     if (type == VENUS || type == MTK || type == SIRF) {
00022         _type = type;
00023     }
00024     
00025     return;
00026 }
00027 
00028 void GPS::setBaud(int baud)
00029 {
00030     serial.baud(baud);
00031     
00032     return;
00033 }
00034 
00035 void GPS::setUpdateRate(int rate)
00036 {
00037     char msg[10] = { 0xA0, 0xA1, 0x00, 0x03,
00038                     0x0E, rate&0xFF, 01,
00039                     0, 0x0D, 0x0A };
00040     for (int i=4; i < 7; i++) {
00041         msg[7] ^= msg[i];
00042     }
00043     switch (rate) {
00044     case 1 :
00045     case 2 :
00046     case 4 :
00047     case 5 :
00048     case 8 :
00049     case 10 :
00050     case 20 :
00051         for (int i=0; i < 10; i++)            
00052             serial.putc(msg[i]);
00053         break;
00054     default :
00055         break;
00056     }
00057 }
00058 
00059 void GPS::setNmeaMessages(bool gga, bool gsa, bool gsv, bool gll, bool rmc, bool vtg)
00060 {
00061     if (_type == VENUS) {
00062         // VENUS Binary MsgID=0x08
00063         // GGA interval
00064         // GSA interval
00065         // GSV interval
00066         // GLL interval
00067         // RMC interval
00068         // VTG interval
00069         // ZDA interval -- hardcode off
00070         char msg[15] = { 0xA0, 0xA1, 0x00, 0x09, 
00071                          0x08, gga, gsa, gsv, gll, rmc, vtg, 0,
00072                          0, 0x0D, 0x0A };
00073         for (int i=4; i < 12; i++) {
00074             msg[12] ^= msg[i];
00075         }
00076         for (int i=0; i < 15; i++)
00077             serial.putc(msg[i]);
00078     }
00079 }
00080     
00081 
00082 void GPS::gsvMessage(bool enable)
00083 {
00084     if (enable) {
00085         if (_type == MTK) {
00086             serial.printf("$PSRF103,03,00,01,01*26\r\n");     // Enable GSV
00087         } else if (_type == VENUS) {
00088         }
00089     } else {
00090         if (_type == MTK) {
00091             serial.printf("$PSRF103,03,00,00,01*27\r\n");     // Disable GSV
00092         } else if (_type == VENUS) {
00093             // ??
00094         }
00095     }
00096 
00097     return;
00098 }
00099 
00100 void GPS::gsaMessage(bool enable)
00101 {
00102     if (enable) {
00103         if (_type == SIRF) {
00104             serial.printf("$PSRF103,02,00,01,01*27\r\n");     // Enable GSA
00105         } else if (_type == VENUS) {
00106             // ??
00107         }
00108     } else {
00109         if (_type == SIRF) {
00110             serial.printf("$PSRF103,02,00,00,01*26\r\n");     // Disable GSA
00111         } else if (_type == VENUS) {
00112             // ??
00113         }
00114     }
00115     
00116     return;
00117 }
00118 
00119 
00120 // Handle data from a GPS (there may be two GPS's so needed to put the code in one routine
00121 //
00122 void GPS::process(GeoPosition &here, char *date, char *time)
00123 {
00124     double lat, lon;
00125     unsigned long age;
00126     
00127     nmea.reset_ready(); // reset the flags
00128     //pc.printf("%d GPS RMC are ready\n", millis);
00129     nmea.f_get_position(&lat, &lon, &age);
00130     nmea.crack_datetime(&year, &month, &day, &hour, &minute, &second, &hundredths, &age);
00131 
00132     sprintf(date, "%02d/%02d/%4d", month, day, year);
00133     sprintf(time, "%02d:%02d:%02d.%03d", hour, minute, second, hundredths);
00134 
00135     hdop = nmea.f_hdop();
00136 
00137     // Bearing and distance to waypoint
00138     here.set(lat, lon);
00139 
00140     //pc.printf("HDOP: %.1f gyro: %d\n", gps1_hdop, gyro);
00141 
00142     return;
00143 }
00144 
00145 
00146 void GPS::init()
00147 {    
00148     // Initialize the GPS comm and handler
00149     //serial.baud(57600); // LOCOSYS LS20031
00150     //DigitalIn myRx(_rx);
00151     //Timer tm;
00152 
00153 
00154 /*    
00155     //pc.printf("gps.init()\n\n");
00156     int rate = 99999999;
00157     for (int i=0; i < 10; i++) {    
00158         //pc.printf("rate: %d\n", rate);
00159         while( myRx ); // wait for low
00160         tm.reset();
00161         tm.start();
00162         while ( !myRx ); // wait for high
00163         if (tm.read_us() < rate) rate = tm.read_us();
00164     }
00165 */
00166     if (_type == MTK) {
00167         gsvMessage(false);      // Disable GSV
00168         gsaMessage(false);      // Disable GSA
00169     } else if (_type == VENUS) {
00170         setNmeaMessages(true, true, false, false, true, false);
00171         setUpdateRate(10);
00172     }
00173     
00174     // Synchronize with GPS
00175     nmea.reset_ready();
00176 }