Diff: main.cpp

Revision:

1:b51fb321d026

Parent:

0:3540612dfbf8

--- a/main.cpp	Fri May 01 15:53:34 2015 +0000
+++ b/main.cpp	Tue Aug 18 00:03:59 2015 +0000
@@ -1,6 +1,19 @@
+/*
+/ Sentinel software for the active nodes.
+/
+/ Design work done by:
+/ Dominic Ottaviano
+/ Sheldon Fernandes
+/ Thien L. Nguyen
+*/
+
 #include "mbed.h"
 #include "xbee900hp.h"
 
+/*===========================================================/
+/ Primary Configuration Area                                 /
+/ Global Variables and Pin Assigments Declared Within        /
+/===========================================================*/
 
 // Define GPS pin connections.
 Serial gps(dp16,dp15);
@@ -8,142 +21,239 @@
 // Define xbee connections. Mosi,Miso,SCK,ATTN,Reset
 xbee900hp xbee(dp2,dp1,dp6, dp9, dp4);
 
-// Function prototyp for gps getline fuction
-void getline();
+// Function prototypes
+// Function to query the GPS
+int getGPS(char* data);
 
-// Container for read GPS message.
+// Global Variables
+// Buffer for reading in from xbee
+char buffer[256];
+// Buffer for storing GPS data
 char gpsmsg[256];
 
+/*===========================================================/
+/ END OF Primary Configuration Area                          /
+/===========================================================*/
+
+/*===========================================================/
+/ Begin main program code                                    /
+/===========================================================*/
+
 int main()
 {
-    // Set gps baud rate to 4800, the default.
+    /*===========================/
+    / Configuration Section      /
+    /===========================*/
+
+    // Set GPS paramters
+    // Baud rate
     gps.baud(4800);
+    // Other params (8 data bits, no parity, 1 stop bit)
+    gps.format(8,SerialBase::None,1);
 
-    // Configure gps with default values.
+    // Configure gps with these parameters:
+    // NMEA, 4800 Baud, 8 Data bits, 1 Stop bit, No Parity
     unsigned int gpsxor = 'P'^'S'^'R'^'F'^'1'^'0'^'0'^','^'1'^','^'4'^'8'^'0'^'0'^','^'8'^','^'1'^','^'0';
-    gps.printf("$PSRF100,1,4800,8,1,0*%i\r\n",gpsxor);
+    // Trim to 8 bits just in case
+    gpsxor = gpsxor & 0xFF;
+    // Send command to the GPS module
+    gps.printf("$PSRF100,1,4800,8,1,0*%u%u\r\n",(gpsxor & 0xF0) >> 4,gpsxor & 0x0F);
 
-    // Enable GGA
-    gpsxor = 'P'^'S'^'R'^'F'^'1'^'0'^'3'^','^'0'^','^'0'^','^'1'^','^'1';
-    gps.printf("$PSRF103,0,0,1,1*%i\r\n",gpsxor);
+    // Disable automatic broadcast of all messages. We are going to poll manually.
+    gpsxor = 'P'^'S'^'R'^'F'^'1'^'0'^'3'^','^'0'^','^'0'^','^'0'^','^'1';
+    // Trim to 8 bits just in case
+    gpsxor = gpsxor & 0xFF;
+    gps.printf("$PSRF103,0,0,0,1*%u%u\r\n",(gpsxor & 0xF0) >> 4,gpsxor & 0x0F);
 
     // Disable GLL
     gpsxor = 'P'^'S'^'R'^'F'^'1'^'0'^'3'^','^'1'^','^'0'^','^'0'^','^'1';
-    gps.printf("$PSRF103,1,0,0,1*%i\r\n",gpsxor);
+    // Trim to 8 bits just in case
+    gpsxor = gpsxor & 0xFF;
+    gps.printf("$PSRF103,1,0,0,1*%u%u\r\n",(gpsxor & 0xF0) >> 4,gpsxor & 0x0F);
 
     // Disable GSA
     gpsxor = 'P'^'S'^'R'^'F'^'1'^'0'^'3'^','^'2'^','^'0'^','^'0'^','^'1';
-    gps.printf("$PSRF103,2,0,0,1*%i\r\n",gpsxor);
+    // Trim to 8 bits just in case
+    gpsxor = gpsxor & 0xFF;
+    gps.printf("$PSRF103,2,0,0,1*%u%u\r\n",(gpsxor & 0xF0) >> 4,gpsxor & 0x0F);
 
     // Disable GSV
     gpsxor = 'P'^'S'^'R'^'F'^'1'^'0'^'3'^','^'3'^','^'0'^','^'0'^','^'1';
-    gps.printf("$PSRF103,3,0,0,1*%i\r\n",gpsxor);
+    // Trim to 8 bits just in case
+    gpsxor = gpsxor & 0xFF;
+    gps.printf("$PSRF103,3,0,0,1*%u%u\r\n",(gpsxor & 0xF0) >> 4,gpsxor & 0x0F);
 
     // Disable RMC
     gpsxor = 'P'^'S'^'R'^'F'^'1'^'0'^'3'^','^'4'^','^'0'^','^'0'^','^'1';
-    gps.printf("$PSRF103,4,0,0,1*%i\r\n",gpsxor);
+    // Trim to 8 bits just in case
+    gpsxor = gpsxor & 0xFF;
+    gps.printf("$PSRF103,4,0,0,1*%u%u\r\n",(gpsxor & 0xF0) >> 4,gpsxor & 0x0F);
 
     // Disable VTG
     gpsxor = 'P'^'S'^'R'^'F'^'1'^'0'^'3'^','^'5'^','^'0'^','^'0'^','^'1';
-    gps.printf("$PSRF103,5,0,0,1*%i\r\n",gpsxor);
+    // Trim to 8 bits just in case
+    gpsxor = gpsxor & 0xFF;
+    gps.printf("$PSRF103,5,0,0,1*%u%u\r\n",(gpsxor & 0xF0) >> 4,gpsxor & 0x0F);
+
+    /*===========================/
+    / END Configuration Section  /
+    /===========================*/
 
 
     // Set the node id of this module. Generated from the serial of the xbee
     char nodeidarray[10];
     int nodeid = 0;
-    
-    // Repeat until gets a value.
-    while (nodeid == 0) {
-        nodeid = 0;
-        // Initialize nodeidarray
+
+    // Initialize nodeidarray
+    for (int i = 0; i<10; i++) {
+        nodeidarray[i] = 0;
+    }
+
+    // Get xbee serial number
+    int serialget = xbee.getSerial(nodeidarray);
+
+    if (serialget == 0) {
         for (int i = 0; i<10; i++) {
-            nodeidarray[i] = 0;
-        }
-    
-        // Get xbee serial number
-        int serialget = xbee.getSerial(nodeidarray);
-        
-        
-        if (serialget == 0) {
-            for (int i = 0; i<10; i++) {
-                // Nonlinear function to expand unique id space
-                nodeid += nodeidarray[i]*nodeidarray[i]+nodeidarray[i];
-            }
+            // Nonlinear function to expand unique id space
+            nodeid += nodeidarray[i]*nodeidarray[i]+nodeidarray[i];
         }
     }
-
     // Node id should now be a unique int
 
-    // Variables for the various important gps command codes
-    float latitude, longitude, altitude, hdop, time;
+    // Set variables, there are more than we need here but it would be useful in the future if we did need any.
     char ns, ew;
-    int lock, satsused;
+    int lock;
+    double satsused;
+    double hdop;
+    double latitude, longitude;
+    double altitude;
+    double time;
+    char altunits;
+    double geoidsep;
+    char geoidunits;
+    int difrefstationid;
+    unsigned int gpschecksum;
+    unsigned int filledbuff;
 
-    // Store message to send out.
-    char buffer[256];
-    
-    // Announce to base platform
-    // Format code line to send out on radio that node has no lock
-    unsigned int filledbuff = snprintf (buffer, sizeof(buffer)-1, "DNLK,%i\n\n",nodeid);
+    // Announce the node to the base station.
+    filledbuff = snprintf (buffer, sizeof(buffer)-1, "ANCE,%i,\r\n",nodeid);
     // Send packet out into the air.
     xbee.sendPacket(buffer, filledbuff);
 
     // Main program run loop.
-    while(1) {
-        // Get the next gps line.
-        getline();
+    while(true) {
+        // Get new data from the GPS and if data is read successfully continue
+        if (!(getGPS(gpsmsg))) {
+            // Parse the recieved data and check if its valid (HINT: lf = double since pointers aren't promoted)
+            if(sscanf(gpsmsg, "$GPGGA,%lf,%lf,%c,%lf,%c,%i,%lf,%lf,%lf,%c,%lf,%c,,%i*%x", &time, &latitude, &ns, &longitude, &ew, &lock, &satsused, &hdop, &altitude, &altunits, &geoidsep, &geoidunits, &difrefstationid, &gpschecksum) == 14) {
+                // Check if the lock is valid
+                if((lock != 1) && (lock != 2) && (lock != 6)) {
+                    // Format code line to send out on radio that node has no lock
+                    filledbuff = snprintf (buffer, sizeof(buffer)-1, "DNLK,%i,\r\n",nodeid);
+                    // Send packet out into the air.
+                    xbee.sendPacket(buffer, filledbuff);
+                } else {
+                    // Convert latitude into proper form for display on a map.
+                    double degrees;
+                    double minutes = modf(latitude/100.0f, &degrees);
+                    minutes = (minutes*100.0f)/60.0f;
+                    latitude = degrees + minutes;
+                    // Convert longitude
+                    minutes = modf(longitude/100.0f, &degrees);
+                    minutes = (minutes*100.0f)/60.0f;
+                    longitude = degrees + minutes;
 
-        // Extract gps information from the GPGGA line.
-        if(sscanf(gpsmsg, "GPGGA,%f,%f,%c,%f,%c,%i,%i,%f,%f", &time, &latitude, &ns, &longitude, &ew, &lock, &satsused, &hdop, &altitude) >= 1) {
+                    // Correct for south and west.
+                    if(ns == 'S') {
+                        latitude  *= -1.0;
+                    }
+                    if(ew == 'W') {
+                        longitude *= -1.0;
+                    }
 
-            // Check if the GPS has a lock or not. (1,2, or 6 indicate lock)
-            if((lock != 1) && (lock != 2)) {
-                // Format code line to send out on radio that node has no lock
-                unsigned int filledbuff = snprintf (buffer, sizeof(buffer)-1, "DNLK,%i\n\n",nodeid);
+                    // Format string for sending out over radio.
+                    filledbuff = snprintf (buffer, sizeof(buffer)-1, "DLIN,%i,%f,%f,%f,%f,\r\n",nodeid, latitude, longitude, altitude, satsused);
+                    // Send packet out.
+                    xbee.sendPacket(buffer, filledbuff);
+                }
+                // Wait a second for GPS data to be fresh again and not waste cycles.
+                wait(1);
+            } else {
+                // GPS hasn't found a good enough lock
+                filledbuff = snprintf (buffer, sizeof(buffer)-1, "DNLK,%i,NOLOCK\r\n",nodeid);
                 // Send packet out into the air.
                 xbee.sendPacket(buffer, filledbuff);
-            } else {
-
-                // Convert latitude from ddmm.mmmm to dd.dddddd
-                double degrees;
-                double minutes = modf(latitude/100.0f, &degrees);
-                minutes = (minutes*100.0f)/60.0f;
-                latitude = degrees + minutes;
-
-                // Convert longitude from ddmm.mmmm to dd.dddddd
-                minutes = modf(longitude/100.0f, &degrees);
-                minutes = (minutes*100.0f)/60.0f;
-                longitude = degrees + minutes;
-
-                // Set signs correctly
-                if(ns == 'S') {
-                    latitude  *= -1.0;
-                }
-                if(ew == 'W') {
-                    longitude *= -1.0;
-                }
-
-                // Format string for sending out over radio.
-                unsigned int filledbuff = snprintf (buffer, sizeof(buffer)-1, "DLIN,%i,%f,%f,%f\n\n",nodeid, latitude, longitude, altitude);
-                // Send packet out.
-                xbee.sendPacket(buffer, filledbuff);
+                // Wait half a second then try again.
+                wait(0.5);
             }
+        } else {
+            // GPS hasn't found lock or sent a corrupted message
+            filledbuff = snprintf (buffer, sizeof(buffer)-1, "DNLK,%i,NOLOCK\r\n",nodeid);
+            // Send packet out into the air.
+            xbee.sendPacket(buffer, filledbuff);
+            // Wait half a second a try again.
+            wait(0.5);
         }
     }
 }
 
-// GPS serial get line function
-void getline()
+int getGPS(char* data)
 {
-    // wait for the start of a line which $ denotes
-    while(gps.getc() != '$');
+    // Request a query of GGA
+    // Precomputed checksum to save cpu cycles
+    gps.printf("$PSRF103,0,1,0,1*%u%u\r\n",0x2,0x5);
 
-    // read the gps line until end of line
-    for(int i=0; i<256; i++) {
-        gpsmsg[i] = gps.getc();
-        if(gpsmsg[i] == '\r') {
-            gpsmsg[i] = 0;
-            return ;
+    // Timer to prevent hangs if gps doesn't respond.
+    Timer gpsTO;
+    gpsTO.start();
+
+    // Wait for gps to becom readable.
+    while (gps.readable() == false) {
+        // Timeout
+        if (gpsTO.read() > 2) {
+           return 1;
         }
     }
+
+    // Wait a tiny bit to allow the gps to send the whole line.
+    wait_ms(50);
+    // Get data from gps
+    gps.scanf("%s",data);
+
+    // Compute checksum of recieved gps data
+    int i = 0;
+    unsigned int calcgpschecksum = 0;
+    // Checksum is calculated between and not including the $ and *
+    while ((data[i] != '\0') && (data[i] != '*')) {
+        // Skip the $
+        if (data[i] != '$') {
+            calcgpschecksum = calcgpschecksum ^ data[i];
+        }
+        i++;
+    }
+    // Shift the checksum to match the format we recieve from the gps
+    calcgpschecksum = calcgpschecksum & 0xFF;
+
+    // Get checksum sent by gps out of string
+    unsigned int realgpschecksum = 0;
+    char checksumarray[3];
+    for (int i = 0; i < 256; i++) {
+        if (data[i] == '*') {
+            // Create little array with ascii hex values.
+            checksumarray[0] = data[i+1];
+            checksumarray[1] = data[i+2];
+            checksumarray[2] = '\0';
+            // Convert ascii values to regular integer
+            sscanf(checksumarray,"%x",&realgpschecksum);
+            break;
+        }
+    }
+
+    // Check for checksum match
+    if( calcgpschecksum == realgpschecksum ) {
+        return 0;
+    }
+
+    // No checksum match
+    return 1;
 }
\ No newline at end of file