main.cpp

00001 /*
00002 Reads UBX NAV-PVT messages from a u-blox GPS module and displays the distance in
00003 centimeters between the current location and the first location received at power
00004 up. No configuration messages are sent to the GPS so it needs to be prepared to
00005 output NAV-PVT messages ONLY, and have that configuration saved so it will always
00006 do so when powered up.
00007 Screen is a SSD1306 running on I2C connection.
00008 */
00009 #include "mbed.h"
00010 #include "Clock.h"
00011 #include "u8g.h"
00012 
00013 #define OLED_ADDRESS 0x78
00014 
00015 DigitalOut myled(PB_1); 
00016 I2C i2c(PB_7, PB_6); // sda, scl
00017 Serial gps(PA_2, PA_3, 19200); // tx, rx
00018 
00019 // At 19200 baud the buffer gets overwritten too quickly. We need to make our
00020 // own software buffer and update it by interrupts so we don't miss anything.
00021 void Rx_interrupt();
00022 const int buffer_size = 1024;
00023 char rx_buffer[buffer_size+1];
00024 volatile int rx_in=0;
00025 volatile int rx_out=0;
00026 
00027 
00028 void u8g_Delay(uint16_t usec)
00029 {
00030     wait_us((int)usec);
00031 }
00032 
00033 void u8g_MicroDelay()
00034 {
00035     wait_us(1);
00036 }
00037 
00038 void u8g_10MicroDelay()
00039 {
00040     wait_us(10);
00041 }
00042 
00043 uint8_t u8g_com_hw_i2c_fn(u8g_t *u8g, uint8_t msg, uint8_t arg_val, void *arg_ptr)
00044 {
00045     static uint8_t control = 0;
00046 
00047     switch(msg) {
00048         case U8G_COM_MSG_STOP:
00049             break; //do nothing...
00050 
00051         case U8G_COM_MSG_INIT:
00052             u8g_MicroDelay();
00053             break;
00054 
00055         case U8G_COM_MSG_ADDRESS:
00056             //the control byte switches the mode on the device and is set here
00057             u8g_10MicroDelay();
00058             if (arg_val == 0) {
00059                 control = 0;
00060             } else {
00061                 control = 0x40;
00062             }
00063             break;
00064 
00065         case U8G_COM_MSG_WRITE_BYTE: {
00066             //simple: just write one byte
00067             char buffer[2];
00068             buffer[0] = control;
00069             buffer[1] = arg_val;
00070             i2c.write(OLED_ADDRESS, buffer, 2);
00071         }
00072         break;
00073 
00074         case U8G_COM_MSG_WRITE_SEQ:
00075         case U8G_COM_MSG_WRITE_SEQ_P: {
00076             char buffer[1024];
00077             char *ptr = (char*)arg_ptr;
00078             buffer[0] = control;
00079             for (int i = 1; i <= arg_val; i++) {
00080                 buffer[i] = *(ptr++);
00081             }
00082             i2c.write(OLED_ADDRESS, buffer, arg_val);
00083         }
00084         break;
00085 
00086     }
00087     return 1;
00088 }
00089 
00090 #ifndef DEGTORAD
00091 #define DEGTORAD 0.0174532925199432957f
00092 #define RADTODEG 57.295779513082320876f
00093 #endif
00094 
00095 struct geoloc {
00096     double lat;
00097     double lon;
00098     geoloc(double x = 0, double y = 0, int32_t a = 0) {
00099         lat = x;
00100         lon = y;
00101     }
00102 };
00103 
00104 // http://www.movable-type.co.uk/scripts/latlong.html
00105 float geoDistance(struct geoloc &a, struct geoloc &b)   // returns meters
00106 {
00107     const double R = 6371000; // km
00108     double p1 = a.lat * DEGTORAD;
00109     double p2 = b.lat * DEGTORAD;
00110     double dp = (b.lat-a.lat) * DEGTORAD;
00111     double dl = (b.lon-a.lon) * DEGTORAD;
00112 
00113     double x = sin(dp/2) * sin(dp/2) + cos(p1) * cos(p2) * sin(dl/2) * sin(dl/2);
00114     double y = 2 * atan2(sqrt(x), sqrt(1-x));
00115 
00116     return R * y;
00117 }
00118 
00119 geoloc originLocation;
00120 geoloc currentLocation;
00121 
00122 const unsigned char UBX_HEADER[] = { 0xB5, 0x62 };
00123 
00124 struct NAV_PVT {
00125     unsigned char cls;
00126     unsigned char id;
00127     unsigned short len;
00128     unsigned long iTOW;          // GPS time of week of the navigation epoch (ms)
00129 
00130     unsigned short year;         // Year (UTC)
00131     unsigned char month;         // Month, range 1..12 (UTC)
00132     unsigned char day;           // Day of month, range 1..31 (UTC)
00133     unsigned char hour;          // Hour of day, range 0..23 (UTC)
00134     unsigned char minute;        // Minute of hour, range 0..59 (UTC)
00135     unsigned char second;        // Seconds of minute, range 0..60 (UTC)
00136     char valid;                  // Validity Flags (see graphic below)
00137     unsigned long tAcc;          // Time accuracy estimate (UTC) (ns)
00138     long nano;                   // Fraction of second, range -1e9 .. 1e9 (UTC) (ns)
00139     unsigned char fixType;       // GNSSfix Type, range 0..5
00140     char flags;                  // Fix Status Flags
00141     unsigned char reserved1;     // reserved
00142     unsigned char numSV;         // Number of satellites used in Nav Solution
00143 
00144     long lon;                    // Longitude (deg)
00145     long lat;                    // Latitude (deg)
00146     long height;                 // Height above Ellipsoid (mm)
00147     long hMSL;                   // Height above mean sea level (mm)
00148     unsigned long hAcc;          // Horizontal Accuracy Estimate (mm)
00149     unsigned long vAcc;          // Vertical Accuracy Estimate (mm)
00150 
00151     long velN;                   // NED north velocity (mm/s)
00152     long velE;                   // NED east velocity (mm/s)
00153     long velD;                   // NED down velocity (mm/s)
00154     long gSpeed;                 // Ground Speed (2-D) (mm/s)
00155     long heading;                // Heading of motion 2-D (deg)
00156     unsigned long sAcc;          // Speed Accuracy Estimate
00157     unsigned long headingAcc;    // Heading Accuracy Estimate
00158     unsigned short pDOP;         // Position dilution of precision
00159     short reserved2;             // Reserved
00160     unsigned long reserved3;     // Reserved
00161 
00162     // Most receivers prior to the M8 series have an older firmware with a
00163     // smaller size NAV_PVT message. For Neo-7M you should comment this out.
00164     uint8_t dummy[8];
00165 };
00166 
00167 NAV_PVT pvt;
00168 
00169 void Rx_interrupt()
00170 {
00171     while ((gps.readable()) && (((rx_in + 1) % buffer_size) != rx_out)) {
00172         rx_buffer[rx_in] = gps.getc();
00173         rx_in = (rx_in + 1) % buffer_size;
00174     }
00175 }
00176 
00177 bool gpsAvailable()
00178 {
00179     return rx_in != rx_out;
00180 }
00181 
00182 char gpsRead()
00183 {
00184     __disable_irq();
00185     char c = rx_buffer[rx_out];
00186     rx_out = (rx_out + 1) % buffer_size;
00187     __enable_irq();
00188     return c;
00189 }
00190 
00191 void calcChecksum(unsigned char* CK)
00192 {
00193     memset(CK, 0, 2);
00194     for (int i = 0; i < (int)sizeof(NAV_PVT); i++) {
00195         CK[0] += ((unsigned char*)(&pvt))[i];
00196         CK[1] += CK[0];
00197     }
00198 }
00199 
00200 bool processGPS()
00201 {
00202     static int fpos = 0;
00203     static unsigned char checksum[2];
00204     const int payloadSize = sizeof(NAV_PVT);
00205 
00206     while ( gpsAvailable() ) {
00207         char c0 = gpsRead();
00208         unsigned char c = *(unsigned char*)(&c0);
00209         if ( fpos < 2 ) {
00210             if ( c == UBX_HEADER[fpos] )
00211                 fpos++;
00212             else
00213                 fpos = 0;
00214         } else {
00215             if ( (fpos-2) < payloadSize )
00216                 ((unsigned char*)(&pvt))[fpos-2] = c;
00217 
00218             fpos++;
00219 
00220             if ( fpos == (payloadSize+2) ) {
00221                 calcChecksum(checksum);
00222             } else if ( fpos == (payloadSize+3) ) {
00223                 if ( c != checksum[0] )
00224                     fpos = 0;
00225             } else if ( fpos == (payloadSize+4) ) {
00226                 fpos = 0;
00227                 if ( c == checksum[1] ) {
00228                     myled = 1 - myled;
00229                     return true;
00230                 }
00231             } else if ( fpos > (payloadSize+4) ) {
00232                 fpos = 0;
00233             }
00234         }
00235     }
00236     return false;
00237 }
00238 
00239 u8g_t u8g;
00240 
00241 double distance = 0;
00242 int numSV = 0;
00243 float hAcc = 0;
00244 uint64_t lastScreenUpdate = 0;
00245 char spinnerBuf[16];
00246 char satsBuf[32];
00247 char haccBuf[32];
00248 char distBuf[32];
00249 
00250 char* spinnerChars = "/-\\|";
00251 uint8_t screenRefreshSpinnerPos = 0;
00252 uint8_t gpsUpdateSpinnerPos = 0;
00253 
00254 void draw(void)
00255 {
00256     u8g_SetFont(&u8g, u8g_font_9x15B);
00257     u8g_DrawStr(&u8g, 2, 12, spinnerBuf);
00258 
00259     u8g_SetFont(&u8g, u8g_font_helvB08);
00260     u8g_DrawStr(&u8g, 42, 8, satsBuf);
00261     u8g_DrawStr(&u8g, 42, 21, haccBuf);
00262 
00263     u8g_SetFont(&u8g, u8g_font_fub30);
00264     u8g_DrawStr(&u8g, 2, 62, distBuf);
00265 }
00266 
00267 void updateScreen()
00268 {
00269     sprintf(spinnerBuf, "%c %c", spinnerChars[screenRefreshSpinnerPos], spinnerChars[gpsUpdateSpinnerPos]);
00270     sprintf(satsBuf, "Sats: %d", numSV);
00271     sprintf(haccBuf, "hAcc: %d", (int)hAcc);
00272     sprintf(distBuf, "%.1lf", distance);
00273 
00274     u8g_FirstPage(&u8g);
00275     do {
00276         draw();
00277     } while ( u8g_NextPage(&u8g) );
00278     u8g_Delay(10);
00279 }
00280 
00281 void checkScreenUpdate()
00282 {
00283     uint64_t now = clock_ms();
00284     if ( now - lastScreenUpdate > 100 ) {
00285         updateScreen();
00286         lastScreenUpdate = now;
00287         screenRefreshSpinnerPos = (screenRefreshSpinnerPos + 1) % 4;
00288     }
00289 }
00290 
00291 int main()
00292 {
00293     gps.attach(&Rx_interrupt, Serial::RxIrq);
00294     u8g_InitComFn(&u8g, &u8g_dev_ssd1306_128x64_i2c, u8g_com_hw_i2c_fn);
00295 
00296     while ( ! processGPS() )
00297         checkScreenUpdate();
00298 
00299     originLocation.lat = pvt.lat * 0.0000001;
00300     originLocation.lon = pvt.lon * 0.0000001;
00301 
00302     while (true) {
00303 
00304         if ( processGPS() ) {
00305             currentLocation.lat = pvt.lat * 0.0000001;
00306             currentLocation.lon = pvt.lon * 0.0000001;
00307             numSV = pvt.numSV;
00308             hAcc = pvt.hAcc;
00309             distance = geoDistance( currentLocation, originLocation ) * 100;
00310             gpsUpdateSpinnerPos = (gpsUpdateSpinnerPos + 1) % 4;
00311         }
00312 
00313         checkScreenUpdate();
00314     }
00315 }
00316