Ublox6.cpp

00001 #include "mbed.h"
00002 #include "Ublox6.h"
00003 
00004 #define DOP_BIT     0x01
00005 #define POSLLH_BIT  0x02
00006 #define SOL_BIT     0x04
00007 #define VELNED_BIT  0x08
00008 
00009 // TODO 2: parameterize LED/status
00010 
00011 Ublox6::Ublox6(PinName tx, PinName rx):
00012     serial(tx, rx),
00013     _available(false)
00014 {
00015     init();
00016 }
00017 
00018 void Ublox6::init(void)
00019 {
00020     setBaud(38400);
00021     enable();
00022     disableVerbose();
00023 }
00024 
00025 
00026 Serial *Ublox6::getSerial(void)
00027 {
00028     return &serial;
00029 }
00030 
00031 void Ublox6::setBaud(int baud)
00032 {
00033     serial.baud(baud);
00034 }
00035 
00036 void Ublox6::enable(void)
00037 {
00038     reset_available();
00039     serial.attach(callback(this, &Ublox6::recv_handler), Serial::RxIrq);
00040     //attach (T *object, R(T::*member)(A1))
00041 }
00042 
00043 void Ublox6::disable(void)
00044 {
00045     serial.attach(NULL, Serial::RxIrq);
00046 }
00047 
00048 /**
00049  * Enable verbose messages for debugging
00050  */
00051 void Ublox6::enableVerbose(void)
00052 {
00053     // TODO: enable Verbose code
00054 }
00055 
00056 /**
00057  * Disable verbose messages for debugging
00058  */
00059 void Ublox6::disableVerbose(void)
00060 {
00061     // TODO: disable Verbose code
00062 }
00063 
00064 bool Ublox6::available(void)
00065 {
00066     return (_available & (DOP_BIT|POSLLH_BIT|SOL_BIT|VELNED_BIT));
00067 }
00068 
00069 void Ublox6::reset_available(void)
00070 {
00071     //led2 = !led2;
00072     _available = 0;
00073 }
00074 
00075 double Ublox6::latitude(void)
00076 {
00077     return _latitude;
00078 }
00079 
00080 double Ublox6::longitude(void)
00081 {
00082     return _longitude;
00083 }
00084 
00085 float Ublox6::speed_mps(void)
00086 {
00087     return _speed_mps;
00088 }
00089 
00090 float Ublox6::heading_deg(void)
00091 {
00092     return _course_deg;
00093 }
00094 
00095 float Ublox6::hdop(void)
00096 {
00097     return _hdop;
00098 }
00099 
00100 int Ublox6::sat_count(void)
00101 {
00102     return _sat_count;
00103 }
00104 
00105 void Ublox6::parse(unsigned char cc)
00106 {
00107     //unsigned char cc = buf[out++];
00108     //out &= (MAX_LENGTH-1);
00109     static unsigned char ck1, ck2, state, code, id, idx, length, chk1, chk2;
00110     static bool checkOk;
00111     static unsigned char data[MAX_LENGTH];
00112 
00113     switch (state) {
00114         case 0:    // wait for sync 1 (0xB5)
00115             ck1 = ck2 = 0;
00116             checkOk = false;
00117             if (cc == SYNC1)
00118                 state++;
00119             break;
00120         case 1:    // wait for sync 2 (0x62)
00121             if (cc == SYNC2)
00122                 state++;
00123             else
00124                 state = 0;
00125             break;
00126         case 2:    // wait for class code
00127             code = cc;
00128             ck1 += cc;
00129             ck2 += ck1;
00130             state++;
00131             break;
00132         case 3:    // wait for Id
00133             id = cc;
00134             ck1 += cc;
00135             ck2 += ck1;
00136             state++;
00137             break;
00138         case 4:    // wait for length uint8_t 1
00139             length = cc;
00140             ck1 += cc;
00141             ck2 += ck1;
00142             state++;
00143             break;
00144         case 5:    // wait for length uint8_t 2
00145             length |= (unsigned int) cc << 8;
00146             ck1 += cc;
00147             ck2 += ck1;
00148             idx = 0;
00149             state++;
00150             if (length > MAX_LENGTH)
00151                 state= 0;
00152             break;
00153         case 6:    // wait for <length> payload uint8_ts
00154             data[idx++] = cc;
00155             ck1 += cc;
00156             ck2 += ck1;
00157             if (idx >= length) {
00158                 state++;
00159             }
00160             break;
00161         case 7:    // wait for checksum 1
00162             chk1 = cc;
00163             state++;
00164             break;
00165         case 8: {  // wait for checksum 2
00166             chk2 = cc;
00167             checkOk = ck1 == chk1  &&  ck2 == chk2;
00168             if (!checkOk) {
00169                 // do something...?
00170             } else {
00171                 switch (code) {
00172                     case 0x01:      // NAV-
00173                         switch (id) {
00174                             case POSLLH_MSG:  // NAV-POSLLH
00175                                 _longitude = ((float)LONG(4))/10000000.0;
00176                                 _latitude = ((float)LONG(8))/10000000.0;
00177                                 // vAcc = ULONG(24); // mm
00178                                 // hAcc = ULONG(20); // mm
00179                                 _available |= POSLLH_BIT;
00180                                 break;
00181                             case DOP_MSG:  // NAV-DOP
00182                                 //gDOP = ((float) UINT(4))/100.0;
00183                                 //tDOP = ((float) UINT(8))/100.0;
00184                                 //vDOP = ((float) UINT(10))/100.0;
00185                                 _hdop = ((float) UINT(12))/100.0;
00186                                 _available |= DOP_BIT;
00187                                 break;
00188                             case SOL_MSG:  // NAV-SOL
00189                                 //week = UINT(8);
00190                                 //pDOP = ((float) UINT(44))/ 100.0;
00191                                 //pAcc = ULONG(24);
00192                                 _sat_count = data[47];
00193                                 _available |= SOL_BIT;
00194                                 break;
00195                             case VELNED_MSG:  // NAV-VELNED
00196                                 _speed_mps = ULONG(20)/100.0;
00197                                 //sAcc = ULONG(28)/100.0;
00198                                 _course_deg = ((float) LONG(24))/100000.0;
00199                                 //cAcc = ((float) LONG(32))/100000.0;
00200                                 _available |= VELNED_BIT;                                
00201                                 break;
00202                             default:
00203                                 break;
00204                         }
00205                         break;
00206                     case 0x05:      // ACK-
00207                         switch (id) {
00208                             case 0x00:  // ACK-NAK
00209                                 break;
00210                             case 0x01:  // ACK-ACK
00211                                 break;
00212                         }
00213                         break;
00214                 }
00215             }
00216             state = 0;
00217             break;
00218         }
00219         default:
00220             break;
00221     }
00222 }
00223 
00224 
00225 void Ublox6::recv_handler(void)
00226 {
00227     while (serial.readable())
00228         parse(serial.getc());
00229 }