Diff: main.cpp

Revision:

1:dd8228f2263c

Parent:

0:1e026f2c8707

--- a/main.cpp	Fri May 01 15:57:01 2015 +0000
+++ b/main.cpp	Tue Aug 18 00:06:08 2015 +0000
@@ -1,86 +1,184 @@
+/*
+/ Sentinel software for the base station node.
+/
+/ Design work done by:
+/ Dominic Ottaviano
+/ Sheldon Fernandes
+/ Thien L. Nguyen
+*/
+
 #include "mbed.h"
 #include "xbee900hp.h"
 #include "rtos.h"
 
-// Serial output to PC
+/*===========================================================/
+/ Primary Configuration Area                                 /
+/ Global Variables and Pin Assigments Declared Within        /
+/===========================================================*/
+
+// Serial Port Mutex to prevent multiple transmissions
+Mutex serial_mutex;
+
+// Declare serial output to the host PC over USB
 Serial pc(USBTX,USBRX);
 
-// GPS to pc
+// Connected serial GPS pins
 Serial gps(p28,p27);
 
-// setup xbee
+// Declare pins of connected xbee module using spi (EXPERIMENTAL DRIVER)
 xbee900hp xbee(p11,p12,p13, p8, p9);
 
+// Led outputs on the MBED
 DigitalOut led1(LED1);
 DigitalOut led2(LED2);
 
+// Global Variables
 // Buffer for reading in from xbee
 char buffer[256];
-
-// Predefine scanner thread
-void xbeeScanner(void const *args);
-void hostScanner(void const *args);
-void getline();
-
+// Buffer for storing GPS data
 char gpsmsg[256];
 
+// Predefine threads running here
+// Thread to handle data inputs and outputs to the XBee
+void xbeeScanner(void const *args);
+// Thread to handle data inputs from the GPS module
+void hostScanner(void const *args);
+
+// Function prototypes
+// Function to query the GPS
+int getGPS(char* data);
+
+/*===========================================================/
+/ END OF Primary Configuration Area                          /
+/===========================================================*/
+
+/*===========================================================/
+/ Code to setup the watchdog timer on the MBED to handle-    /
+/ auto resets                                                /
+/===========================================================*/
+
+class Watchdog
+{
+public:
+    // Load timeout value in watchdog timer and enable
+    void kick(float s) {
+        LPC_WDT->WDCLKSEL = 0x1;                // Set CLK src to PCLK
+        uint32_t clk = SystemCoreClock / 16;    // WD has a fixed /4 prescaler, PCLK default is /4
+        LPC_WDT->WDTC = s * (float)clk;
+        LPC_WDT->WDMOD = 0x3;                   // Enabled and Reset
+        kick();
+    }
+    // "kick" or "feed" the dog - reset the watchdog timer
+    // by writing this required bit pattern
+    void kick() {
+        LPC_WDT->WDFEED = 0xAA;
+        LPC_WDT->WDFEED = 0x55;
+    }
+};
+
+// Declare watchdog timer
+Watchdog wdt;
+
+/*===========================================================/
+/ End of watchdog timer code                                 /
+/===========================================================*/
+
+/*===========================================================/
+/ Begin main program code                                    /
+/===========================================================*/
 
 int main()
 {
-    /*Configuration Area
-    */
+    /*===========================/
+    / Configuration Section      /
+    /===========================*/
 
-    // Set pc baud rate to pretty high for some speed
+    // Set PC parameters
+    // Baud rate
     pc.baud(57600);
+    // Other params (8 data bits, no parity, 1 stop bit)
+    pc.format(8,SerialBase::None,1);
+
+    // Set GPS paramters
+    // Baud rate
     gps.baud(4800);
-    
-    // Configure gps with default values.
+    // Other params (8 data bits, no parity, 1 stop bit)
+    gps.format(8,SerialBase::None,1);
+
+    // 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 Area
-    */
+    // Enable watchdog with a reasonable half second timeout
+    wdt.kick(1000);
+
+    /*===========================/
+    / END Configuration Section  /
+    /===========================*/
+    
+
     while (true) {
         // Define Keys
         char startkey[12] = "SentinelOn";
         char endkey[13] = "SentinelOff";
-        char buffer[30];
+        char cmdbuff[30];
 
+        // LED 1 on shows ready status.
+        led1 = 1;
+        // Start loop waiting for start message
         do {
             if (pc.readable() == true) {
                 // Get bytes from computer
-                pc.scanf("%s",buffer);
+                pc.scanf("%s",cmdbuff);
             }
-
-            led1 = 1;
-        } while (strcmp( startkey, buffer ) != 0);
+            wdt.kick();
+        } while (strcmp( startkey, cmdbuff ) != 0);
+        // Clear string in buffer
+        cmdbuff[0] = '\0';
+        // LED 1 off as we enter next segment
         led1 = 0;
 
-        // Code to respond saying its started.
+        // Let host know that we are starting.
         pc.printf("SLON\r\n");
 
         xbee.clearBuff();
@@ -90,15 +188,20 @@
         Thread hostscan(hostScanner);
 
         // Main runloop
+        led2 = 1;
         do {
+            serial_mutex.lock();
             if (pc.readable() == true) {
                 // Get bytes from computer
-                pc.scanf("%s",buffer);
+                pc.scanf("%s",cmdbuff);
             }
+            serial_mutex.unlock();
+            wdt.kick();
+        } while (strcmp( endkey, cmdbuff ) != 0);
+        led2 = 0;
 
-            led2 = 1;
-        } while (strcmp( endkey, buffer ) != 0);
-        led2 = 0;
+        // Clear string in buffer
+        cmdbuff[0] = '\0';
 
         xbscan.terminate();
         hostscan.terminate();
@@ -108,39 +211,79 @@
 // Thread to update host position
 void hostScanner(void const *args)
 {
-    char nodeid[4] = "0";
+    // Set variables, there are more than we need here but it would be useful in the future if we did need any.
+    char nodeid[5] = "BASE";
     char ns, ew;
     int lock;
-    int satsused;
-    float hdop;
-    float latitude, longitude;
-    float altitude;
-    float time;
+    double satsused;
+    double hdop;
+    double latitude, longitude;
+    double altitude;
+    double time;
+    char altunits;
+    double geoidsep;
+    char geoidunits;
+    int difrefstationid;
+    unsigned int gpschecksum;
     
-    while (1) {
-        // update.
-        getline();
-        if(sscanf(gpsmsg, "GPGGA,%f,%f,%c,%f,%c,%i,%i,%f,%f", &time, &latitude, &ns, &longitude, &ew, &lock, &satsused, &hdop, &altitude) >= 1) {
-            if((lock != 1) && (lock != 2) && (lock != 6)) {
-                pc.printf("DNLK,%s,NOLOCK\n",nodeid, latitude, longitude, altitude);
+    serial_mutex.lock();
+    // Announce base node to computer application
+    pc.printf("ANCE,%s,\r\n",nodeid);
+    serial_mutex.unlock();
+
+    // Main run loop
+    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)) {
+                    // Lock is not valid, let host know.
+                    serial_mutex.lock();
+                    pc.printf("DNLK,%s,NOLOCK\r\n",nodeid);
+                    serial_mutex.unlock();
+                } 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;
+
+                    // Correct for south and west.
+                    if(ns == 'S') {
+                        latitude  *= -1.0;
+                    }
+                    if(ew == 'W') {
+                        longitude *= -1.0;
+                    }
+                    
+                    serial_mutex.lock();
+                    // Send formatted info to the host
+                    pc.printf("DLIN,%s,%f,%f,%f,%f,\r\n",nodeid, latitude, longitude, altitude, satsused);
+                    serial_mutex.unlock();
+                }
+                // Wait a second for GPS data to be fresh again and not waste cycles.
+                Thread::wait(1000);
             } else {
-                double degrees;
-                double minutes = modf(latitude/100.0f, &degrees);
-                minutes = (minutes*100.0f)/60.0f;
-                latitude = degrees + minutes;
-
-                minutes = modf(longitude/100.0f, &degrees);
-                minutes = (minutes*100.0f)/60.0f;
-                longitude = degrees + minutes;
-
-                if(ns == 'S') {
-                    latitude  *= -1.0;
-                }
-                if(ew == 'W') {
-                    longitude *= -1.0;
-                }
-                pc.printf("DLIN,%s,%f,%f,%f\n",nodeid, latitude, longitude, altitude);
+                // GPS hasn't found a good enough lock
+                serial_mutex.lock();
+                pc.printf("DNLK,%s,NOLOCK\r\n",nodeid);
+                serial_mutex.unlock();
+                // If this check fails we give up cpu time to another thread but only momentarily, we need to get this data successfully.
+                Thread::wait(500);
             }
+        } else {
+            // GPS hasn't found lock or sent a corrupted message
+            serial_mutex.lock();
+            pc.printf("DNLK,%s,NOLOCK\r\n",nodeid);
+            serial_mutex.unlock();
+            // If this check fails we give up cpu time to another thread but only momentarily, we need to get this data successfully.
+            Thread::wait(500);
         }
     }
 }
@@ -154,9 +297,13 @@
         while(xbee.attn() == 0) {
             // Read in data and run all background checksum and validation stuff
             if (xbee.readPacket(buffer) == 0) {
-                pc.printf("%s", buffer);
+                serial_mutex.lock();
+                pc.printf("%s\r\n", buffer);
+                serial_mutex.unlock();
             } else {
-                pc.printf("Packet Skipped\n\r");
+                serial_mutex.lock();
+                pc.printf("Packet Failed Validation\r\n");
+                serial_mutex.unlock();
                 xbee.clearBuff();
             }
         }
@@ -165,15 +312,68 @@
     }
 }
 
-void getline()
+// Function to get the gps message and make sure its valid.
+int getGPS(char* data)
 {
-    while(gps.getc() != '$');    // wait for the start of a line
-    for(int i=0; i<256; i++) {
-        gpsmsg[i] = gps.getc();
-        if(gpsmsg[i] == '\r') {
-            gpsmsg[i] = 0;
-            return ;
+    // Request a query of GGA
+    // Precomputed checksum to save cpu cycles
+    serial_mutex.lock();
+    gps.printf("$PSRF103,0,1,0,1*%u%u\r\n",0x2,0x5);
+    serial_mutex.unlock();
+
+    // 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;
         }
     }
-    error("Overflowed message limit");
+
+    // Wait a tiny bit to allow the gps to send the whole line.
+    Thread::wait(50);
+    // Get data from gps
+    serial_mutex.lock();
+    gps.scanf("%s",data);
+    serial_mutex.unlock();
+
+    // 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