Luka Danilovic / Mbed OS PROJ515_GPS

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   TinyGPSPlus

PROJ515.cpp@5:a201e5377e90, 2019-03-04 (annotated)

Committer:
Luka_Danilovic
Date:
Mon Mar 04 13:03:02 2019 +0000
Revision:
5:a201e5377e90
Parent:
4:df54ebe69b38
Child:
6:87f2d46b2f1b
ROS Working;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Luka_Danilovic 0:448e0e74e00f 1 #include "PROJ515.hpp" // Contains all Libraries, Definitions & Function Prototypes
Luka_Danilovic 0:448e0e74e00f 2
Luka_Danilovic 0:448e0e74e00f 3 /*================================= SETUP ====================================*/
Luka_Danilovic 0:448e0e74e00f 4
Luka_Danilovic 0:448e0e74e00f 5 // GPIO
Luka_Danilovic 3:30c3f20c2387 6 DigitalIn btn(USER_BUTTON); // Onboard blue button
Luka_Danilovic 3:30c3f20c2387 7 BusOut leds(LED1, LED2, LED3); // Onboard LEDs bus
Luka_Danilovic 0:448e0e74e00f 8
Luka_Danilovic 0:448e0e74e00f 9 // Threads and Signals
Luka_Danilovic 3:30c3f20c2387 10 Thread serThread (osPriorityNormal); // Create thread for serial comms
Luka_Danilovic 3:30c3f20c2387 11 EventFlags thread_com; // Event flag for thread comms
Luka_Danilovic 0:448e0e74e00f 12
Luka_Danilovic 0:448e0e74e00f 13 // Namespaces
Luka_Danilovic 0:448e0e74e00f 14 using namespace std; // Make all std symbols visible
Luka_Danilovic 0:448e0e74e00f 15 using namespace ros; // Make all ros symbols visible
Luka_Danilovic 0:448e0e74e00f 16 using namespace nav_msgs; // Make all nav_msgs symbols visible
Luka_Danilovic 0:448e0e74e00f 17 using namespace std_msgs; // Make all sdt_msgs symbols visible
Luka_Danilovic 0:448e0e74e00f 18
Luka_Danilovic 0:448e0e74e00f 19 // Instantiations
Luka_Danilovic 0:448e0e74e00f 20 TinyGPSPlus gpsModule; // Instance of GPS class
Luka_Danilovic 0:448e0e74e00f 21 Serial gpsSer(GPS_TX, GPS_RX); // Instance of Serial class to gps
Luka_Danilovic 4:df54ebe69b38 22 Serial usbSer(USB_TX, USB_RX); // Instance of Serial class to usb
Luka_Danilovic 0:448e0e74e00f 23 NodeHandle nh; // Instance for ROS Node Handler
Luka_Danilovic 0:448e0e74e00f 24 Odometry gps_odom_msg; // Instance for ROS Odometry message
Luka_Danilovic 3:30c3f20c2387 25 String gps_stat_msg; // Instance for ROS String message
Luka_Danilovic 0:448e0e74e00f 26 Publisher gps_odom_pub("gps_odom", &gps_odom_msg); // Instance for ROS publisher (Odometry Message)
Luka_Danilovic 0:448e0e74e00f 27 Publisher gps_stat_pub("gps_stat", &gps_stat_msg); // Instance for ROS publisher (String Message)
Luka_Danilovic 0:448e0e74e00f 28
Luka_Danilovic 0:448e0e74e00f 29 // Definitions
Luka_Danilovic 4:df54ebe69b38 30 char gps_c = 0; // GPS serial character
Luka_Danilovic 4:df54ebe69b38 31 int sats_n = 0; // GPS Satelite count
Luka_Danilovic 0:448e0e74e00f 32
Luka_Danilovic 0:448e0e74e00f 33 /*================================ FUNCIONS ==================================*/
Luka_Danilovic 0:448e0e74e00f 34
Luka_Danilovic 0:448e0e74e00f 35 void streamF() // Function for Serial to PC
Luka_Danilovic 0:448e0e74e00f 36 {
Luka_Danilovic 0:448e0e74e00f 37 while(true) { // Do forever
Luka_Danilovic 0:448e0e74e00f 38 thread_com.wait_all(PRINT_DATA); // Wait on signal to print data (Sleeps if blocking until Flag is set)
Luka_Danilovic 0:448e0e74e00f 39
Luka_Danilovic 5:a201e5377e90 40 gps_odom_msg.header.stamp = nh.now(); // Get current time
Luka_Danilovic 0:448e0e74e00f 41
Luka_Danilovic 5:a201e5377e90 42 gps_odom_msg.pose.pose.position.x = gpsModule.location.lng(); // Get Longtitude
Luka_Danilovic 5:a201e5377e90 43 gps_odom_msg.pose.pose.position.y = gpsModule.location.lat(); // Get Latitude
Luka_Danilovic 5:a201e5377e90 44 gps_odom_msg.pose.pose.position.z = 0; // Get Altitude
Luka_Danilovic 0:448e0e74e00f 45
Luka_Danilovic 5:a201e5377e90 46 gps_odom_pub.publish(&gps_odom_msg); // Publish the Odometry message
Luka_Danilovic 4:df54ebe69b38 47
Luka_Danilovic 5:a201e5377e90 48 usbSer.printf("\n>Valid Loc<"); // Print out valid location
Luka_Danilovic 3:30c3f20c2387 49
Luka_Danilovic 0:448e0e74e00f 50 }
Luka_Danilovic 0:448e0e74e00f 51 }
Luka_Danilovic 0:448e0e74e00f 52
Luka_Danilovic 0:448e0e74e00f 53 void setupRosMsg()
Luka_Danilovic 0:448e0e74e00f 54 {
Luka_Danilovic 4:df54ebe69b38 55 nh.getHardware()->setBaud(ROS_Baud); // Set Baud Rate for ROS Serial
Luka_Danilovic 0:448e0e74e00f 56
Luka_Danilovic 0:448e0e74e00f 57 gps_odom_msg.header.frame_id = frameID; // Pack ROS frame ID
Luka_Danilovic 0:448e0e74e00f 58 gps_odom_msg.child_frame_id = childID; // Pack ROS child frame ID
Luka_Danilovic 0:448e0e74e00f 59
Luka_Danilovic 3:30c3f20c2387 60 gps_odom_msg.pose.pose.orientation.x = 1; // Identity quaternion
Luka_Danilovic 3:30c3f20c2387 61 gps_odom_msg.pose.pose.orientation.y = 0; // Identity quaternion
Luka_Danilovic 3:30c3f20c2387 62 gps_odom_msg.pose.pose.orientation.z = 0; // Identity quaternion
Luka_Danilovic 3:30c3f20c2387 63 gps_odom_msg.pose.pose.orientation.w = 0; // Identity quaternion
Luka_Danilovic 0:448e0e74e00f 64
Luka_Danilovic 3:30c3f20c2387 65 gps_odom_msg.pose.covariance[ 0] = CVX; // X pos covariance
Luka_Danilovic 3:30c3f20c2387 66 gps_odom_msg.pose.covariance[ 7] = CVY; // Y pos covariance
Luka_Danilovic 3:30c3f20c2387 67 gps_odom_msg.pose.covariance[14] = CVZ; // Z pos covariance
Luka_Danilovic 5:a201e5377e90 68 gps_odom_msg.pose.covariance[21] = CVR; // X rot covariance
Luka_Danilovic 5:a201e5377e90 69 gps_odom_msg.pose.covariance[28] = CVR; // Y rot covarianceL
Luka_Danilovic 5:a201e5377e90 70 gps_odom_msg.pose.covariance[35] = CVR; // Z rot covariance
Luka_Danilovic 0:448e0e74e00f 71 }
Luka_Danilovic 0:448e0e74e00f 72
Luka_Danilovic 0:448e0e74e00f 73 /*================================== MAIN ====================================*/
Luka_Danilovic 0:448e0e74e00f 74 int main() // Entry Point
Luka_Danilovic 0:448e0e74e00f 75 {
Luka_Danilovic 0:448e0e74e00f 76 nh.initNode(); // Initialise ROS Node Handler
Luka_Danilovic 0:448e0e74e00f 77 nh.advertise(gps_odom_pub); // Adverstise Odometry topic
Luka_Danilovic 0:448e0e74e00f 78 nh.advertise(gps_stat_pub); // Adverstise String topic
Luka_Danilovic 0:448e0e74e00f 79 setupRosMsg(); // Set up ROS message parts
Luka_Danilovic 0:448e0e74e00f 80 gpsSer.baud(GPS_Baud); // Set Baud rate for GPS Serial
Luka_Danilovic 3:30c3f20c2387 81 usbSer.baud(USB_Baud); // Set Baud rate for USB Serial
Luka_Danilovic 3:30c3f20c2387 82 serThread.start(streamF); // Start serial comms thread
Luka_Danilovic 0:448e0e74e00f 83
Luka_Danilovic 0:448e0e74e00f 84 while(!nh.connected()) { // While node handler is not connected
Luka_Danilovic 3:30c3f20c2387 85 nh.spinOnce(); // Attempt to connect and synchronise
Luka_Danilovic 0:448e0e74e00f 86 }
Luka_Danilovic 0:448e0e74e00f 87
Luka_Danilovic 3:30c3f20c2387 88 while (true) { // When node handler is connected, do forever
Luka_Danilovic 0:448e0e74e00f 89
Luka_Danilovic 3:30c3f20c2387 90 nh.spinOnce(); // Reccuring connect and synchronise
Luka_Danilovic 0:448e0e74e00f 91
Luka_Danilovic 0:448e0e74e00f 92 if(gpsSer.readable()) { // If serial buffer has character
Luka_Danilovic 0:448e0e74e00f 93 gps_c = gpsSer.getc(); // Read serial buffer and store character
Luka_Danilovic 0:448e0e74e00f 94 gpsModule.encode(gps_c); // Encode character from GPS
Luka_Danilovic 3:30c3f20c2387 95 usbSer.printf("\n %c", gps_c); // Print out gps character
Luka_Danilovic 5:a201e5377e90 96 leds = LEDS_ON; // Flash LED bus ON
Luka_Danilovic 4:df54ebe69b38 97
Luka_Danilovic 1:b610535e5879 98 } else {
Luka_Danilovic 4:df54ebe69b38 99 leds = LEDS_OFF; // Flash LED bus OFF
Luka_Danilovic 3:30c3f20c2387 100 }
Luka_Danilovic 3:30c3f20c2387 101
Luka_Danilovic 0:448e0e74e00f 102 if (gpsModule.location.isValid()) { // If GPS location is Valid
Luka_Danilovic 0:448e0e74e00f 103 thread_com.set(PRINT_DATA); // Set EventFlag to Print Data
Luka_Danilovic 4:df54ebe69b38 104 usbSer.printf("\n > LOCATION VALID <"); // Print Status Check
Luka_Danilovic 3:30c3f20c2387 105 }
Luka_Danilovic 1:b610535e5879 106
Luka_Danilovic 3:30c3f20c2387 107 if (btn) { // If blue button is pressed
Luka_Danilovic 5:a201e5377e90 108 sats_n = gpsModule.satellites.value(); // Aquire satelite number
Luka_Danilovic 3:30c3f20c2387 109 usbSer.printf("\nstatChck: %d Satelites", sats_n); // Print Status Check
Luka_Danilovic 0:448e0e74e00f 110 }
Luka_Danilovic 3:30c3f20c2387 111
Luka_Danilovic 0:448e0e74e00f 112 }
Luka_Danilovic 0:448e0e74e00f 113 }