PROJ515.cpp

00001 #include "PROJ515.hpp" // Contains all Libraries, Definitions & Function Prototypes
00002 
00003 /*================================= SETUP ====================================*/
00004 
00005 // GPIO
00006 DigitalIn btn(USER_BUTTON);                        // Onboard blue button
00007 BusOut leds(LED1, LED2, LED3);                     // Onboard LEDs bus
00008 
00009 // Threads and Signals
00010 Thread serThread (osPriorityNormal);               // Create thread for serial comms
00011 EventFlags thread_com;                             // Event flag for thread comms
00012 
00013 // Namespaces
00014 using namespace std;                               // Make all std symbols visible
00015 using namespace ros;                               // Make all ros symbols visible
00016 using namespace sensor_msgs;                       // Make all sensor_msgs symbols visible
00017 using namespace std_msgs;                          // Make all sdt_msgs symbols visible
00018 
00019 // Instantiations
00020 TinyGPSPlus gpsModule;                             // Instance of GPS class
00021 Serial gpsSer(GPS_TX, GPS_RX);                     // Instance of Serial class to gps
00022 Serial usbSer(USB_TX, USB_RX);                     // Instance of Serial class to usb
00023 NodeHandle nh;                                     // Instance for ROS Node Handler
00024 NavSatFix gps_odom_msg;                            // Instance for ROS NavSatFix message
00025 String gps_stat_msg;                               // Instance for ROS String message
00026 Publisher gps_odom_pub("gps_odom", &gps_odom_msg); // Instance for ROS publisher (Odometry Message)
00027 Publisher gps_stat_pub("gps_stat", &gps_stat_msg); // Instance for ROS publisher (String Message)
00028 
00029 // Definitions
00030 char gps_c = 0;                                    // GPS serial character
00031 int sats_n = 0;                                    // GPS Satelite count
00032 
00033 /*================================ FUNCIONS ==================================*/
00034 
00035 void streamF()                                     // Function for Serial to PC
00036 {
00037     while(true) {                                  // Do forever
00038         thread_com.wait_all(PRINT_DATA);           // Wait on signal to print data (Sleeps if blocking until Flag is set)
00039 
00040         gps_odom_msg.latitude  = gpsModule.location.lng(); // Get Longtitude
00041         gps_odom_msg.longitude = gpsModule.location.lat(); // Get Latitude
00042 
00043         gps_odom_msg.header.stamp = nh.now();      // Get current time
00044         
00045         gps_odom_pub.publish(&gps_odom_msg);       // Publish the Odometry message
00046     }
00047 }
00048 
00049 void setupRosMsg()
00050 {
00051 
00052     gps_odom_msg.header.frame_id = frameID;        // Pack ROS frame ID
00053 
00054     gps_odom_msg.status.status = UNAGUMENTED;      // Location fix method
00055     gps_odom_msg.status.service = GPS_CONSTELL;    // Satelite constellation used
00056     
00057     gps_odom_msg.altitude  = 0;                    // Altitude (Always fixed)
00058 
00059     gps_odom_msg.position_covariance[ 0] = CVX;    // X pos covariance
00060     gps_odom_msg.position_covariance[ 1] = CVO;    // Zero
00061     gps_odom_msg.position_covariance[ 2] = CVO;    // Zero
00062     gps_odom_msg.position_covariance[ 3] = CVO;    // Zero
00063     gps_odom_msg.position_covariance[ 4] = CVY;    // Y pos covariance
00064     gps_odom_msg.position_covariance[ 5] = CVO;    // Zero
00065     gps_odom_msg.position_covariance[ 6] = CVO;    // Zero
00066     gps_odom_msg.position_covariance[ 7] = CVO;    // Zero
00067     gps_odom_msg.position_covariance[ 8] = CVZ;    // Z pos covariance
00068     
00069     gps_odom_msg.position_covariance_type = UNNWN; // Covariance type
00070 
00071 }
00072 
00073 /*================================== MAIN ====================================*/
00074 int main()                                         // Entry Point
00075 {
00076     nh.getHardware()->setBaud(ROS_Baud);           // Set Baud Rate for ROS Serial
00077     nh.initNode();                                 // Initialise ROS Node Handler
00078     nh.advertise(gps_odom_pub);                    // Adverstise Odometry topic
00079     nh.advertise(gps_stat_pub);                    // Adverstise String topic
00080     setupRosMsg();                                 // Set up ROS message parts
00081     gpsSer.baud(GPS_Baud);                         // Set Baud rate for GPS Serial
00082     usbSer.baud(USB_Baud);                         // Set Baud rate for USB Serial
00083     serThread.start(streamF);                      // Start serial comms thread
00084 
00085     while(!nh.connected()) {                       // While node handler is not connected
00086         nh.spinOnce();                             // Attempt to connect and synchronise
00087     }
00088 
00089     while (true) {                                 // When node handler is connected, do forever
00090 
00091         nh.spinOnce();                             // Reccuring connect and synchronise
00092 
00093         if(gpsSer.readable()) {                    // If serial buffer has character
00094             gps_c = gpsSer.getc();                 // Read serial buffer and store character
00095             gpsModule.encode(gps_c);               // Encode character from GPS
00096 
00097         } else {
00098             leds = leds & 0b100;                   // Flash LED[0,1] bus OFF. Leave LED[2] on if its on to indicate that the signal was present
00099         }
00100 
00101         if (gpsModule.location.isValid()) {        // If GPS location is Valid
00102             thread_com.set(PRINT_DATA);            // Set EventFlag to Print Data
00103         }
00104         
00105         sats_n = gpsModule.satellites.value();     // Aquire satelite number
00106         if (sats_n > 0){
00107             leds = LEDS_ON;
00108             }
00109 
00110         if (btn) {                                 // If blue button is pressed
00111             
00112             usbSer.printf("\n\nTimestamp: %d ", nh.now());                  // Print Status: Time Stamp
00113             usbSer.printf("\nstatChck: %d Satelites", sats_n);              // Print Status: Satelite Number
00114             usbSer.printf("\nstatChck: %f Long", gpsModule.location.lng()); // Print Status: Long
00115             usbSer.printf("\nstatChck: %f Lat", gpsModule.location.lat());  // Print Status: Lat
00116         }
00117 
00118     }
00119 }