Diff: PROJ515.cpp

Revision:

10:46216f5f48a8

Parent:

9:ad8faa306a43

diff -r ad8faa306a43 -r 46216f5f48a8 PROJ515.cpp
--- a/PROJ515.cpp	Thu Mar 07 12:27:26 2019 +0000
+++ b/PROJ515.cpp	Thu Mar 07 13:45:10 2019 +0000
@@ -6,99 +6,35 @@
 DigitalIn btn(USER_BUTTON);                 // Onboard blue button
 BusOut leds(LED1, LED2, LED3);              // Onboard LEDs bus
 
-// Threads and Signals
-Thread serThread (osPriorityNormal);        // Create thread for serial comms
-EventFlags thread_com;                      // Event flag for thread comms
-
 // Namespaces
 using namespace std;                        // Make all std symbols visible
-using namespace ros;                        // Make all ros symbols visible
-using namespace nav_msgs;                   // Make all nav_msgs symbols visible
-using namespace std_msgs;                   // Make all sdt_msgs symbols visible
 
 // Instantiations
 TinyGPSPlus gpsModule;                      // Instance of GPS class
 Serial gpsSer(GPS_TX, GPS_RX);              // Instance of Serial class to gps
-Serial usbSer(USB_TX, USB_RX);              // Instance of Serial class to usb
-NodeHandle nh;                              // Instance for ROS Node Handler
-Odometry gps_odom_msg;                      // Instance for ROS Odometry message
-String gps_stat_msg;                        // Instance for ROS String message
-Publisher gps_odom_pub("gps_odom", &gps_odom_msg); // Instance for ROS publisher (Odometry Message)
-Publisher gps_stat_pub("gps_stat", &gps_stat_msg); // Instance for ROS publisher (String Message)
+Serial usbSer(USBTX, USBRX);
 
 // Definitions
 char gps_c = 0;                             // GPS serial character
 int sats_n = 0;                             // GPS Satelite count
 
-/*================================ FUNCIONS ==================================*/
-
-void streamF()                              // Function for Serial to PC
-{
-    while(true) {                           // Do forever
-        thread_com.wait_all(PRINT_DATA);    // Wait on signal to print data (Sleeps if blocking until Flag is set)
-
-        gps_odom_msg.header.stamp = nh.now(); // Get current time
-
-        gps_odom_msg.pose.pose.position.x = gpsModule.location.lng(); // Get Longtitude
-        gps_odom_msg.pose.pose.position.y = gpsModule.location.lat(); // Get Latitude
-        gps_odom_msg.pose.pose.position.z = 0;  // Get Altitude
-
-        gps_odom_pub.publish(&gps_odom_msg); // Publish the Odometry message
-
-        usbSer.printf("\n>Valid Loc<"); // Print out valid location
-    }
-}
-
-void setupRosMsg()
-{
-    nh.getHardware()->setBaud(ROS_Baud);    // Set Baud Rate for ROS Serial
-
-    gps_odom_msg.header.frame_id = frameID; // Pack ROS frame ID
-    gps_odom_msg.child_frame_id = childID;  // Pack ROS child frame ID
-
-    gps_odom_msg.pose.pose.orientation.x = 1; // Identity quaternion
-    gps_odom_msg.pose.pose.orientation.y = 0; // Identity quaternion
-    gps_odom_msg.pose.pose.orientation.z = 0; // Identity quaternion
-    gps_odom_msg.pose.pose.orientation.w = 0; // Identity quaternion
-
-    gps_odom_msg.pose.covariance[ 0] = CVX; // X pos covariance
-    gps_odom_msg.pose.covariance[ 7] = CVY; // Y pos covariance
-    gps_odom_msg.pose.covariance[14] = CVZ; // Z pos covariance
-    gps_odom_msg.pose.covariance[21] = CVR; // X rot covariance
-    gps_odom_msg.pose.covariance[28] = CVR; // Y rot covariance
-    gps_odom_msg.pose.covariance[35] = CVR; // Z rot covariance
-}
-
 /*================================== MAIN ====================================*/
 int main()                                  // Entry Point
 {
-    nh.initNode();                          // Initialise ROS Node Handler
-    nh.advertise(gps_odom_pub);             // Adverstise Odometry topic
-    nh.advertise(gps_stat_pub);             // Adverstise String topic
-    setupRosMsg();                          // Set up ROS message parts
-    gpsSer.baud(GPS_Baud);                  // Set Baud rate for GPS Serial
-    usbSer.baud(USB_Baud);                  // Set Baud rate for USB Serial
-    serThread.start(streamF);               // Start serial comms thread
-
-    while(!nh.connected()) {                // While node handler is not connected
-        nh.spinOnce();                      // Attempt to connect and synchronise
-    }
+   // gpsSer.baud(GPS_Baud);                  // Set Baud rate for GPS Serial
 
     while (true) {                          // When node handler is connected, do forever
 
-        nh.spinOnce();                      // Reccuring connect and synchronise
-
         if(gpsSer.readable()) {             // If serial buffer has character
             gps_c = gpsSer.getc();          // Read serial buffer and store character
             gpsModule.encode(gps_c);        // Encode character from GPS
-            usbSer.printf("\n %c", gps_c);  // Print out gps character
+            //usbSer.printf("\n %c", gps_c);  // Print out gps character
 
         } else {
             leds = leds & 0b100;                // Flash LED[0,1] bus OFF. Leave LED[2] on if its on to indicate that the signal was present
         }
 
         if (gpsModule.location.isValid()) { // If GPS location is Valid
-            thread_com.set(PRINT_DATA);     // Set EventFlag to Print Data
             usbSer.printf("\n > LOCATION VALID <"); // Print Status Check
             leds = LEDS_ON;                 // Flash LED bus ON
         }
@@ -107,6 +43,6 @@
             sats_n = gpsModule.satellites.value(); // Aquire satelite number
             usbSer.printf("\nstatChck: %d Satelites", sats_n); // Print Status Check
         }
-
+        
     }
 }