Luka Danilovic
/
PROJ515_GPS
GPS NMEA through NavSatFix
PROJ515.cpp
- Committer:
- Luka_Danilovic
- Date:
- 2019-03-04
- Revision:
- 5:a201e5377e90
- Parent:
- 4:df54ebe69b38
- Child:
- 6:87f2d46b2f1b
File content as of revision 5:a201e5377e90:
#include "PROJ515.hpp" // Contains all Libraries, Definitions & Function Prototypes /*================================= SETUP ====================================*/ // GPIO 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) // 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 covarianceL 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 } 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 leds = LEDS_ON; // Flash LED bus ON } else { leds = LEDS_OFF; // Flash LED bus OFF } 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 } if (btn) { // If blue button is pressed sats_n = gpsModule.satellites.value(); // Aquire satelite number usbSer.printf("\nstatChck: %d Satelites", sats_n); // Print Status Check } } }