Luka Danilovic / Mbed OS PROJ515_GPS

GPS NMEA through NavSatFix

Dependencies:   TinyGPSPlus

PROJ515.cpp@14:90590736e700, 2019-03-19 (annotated)

Committer:
Luka_Danilovic
Date:
Tue Mar 19 12:57:45 2019 +0000
Revision:
14:90590736e700
Parent:
13:755a3cd9d35d
Child:
17:264eb15c8be2
NavSatFix version (Commented)

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 14:90590736e700 6 DigitalIn btn(USER_BUTTON); // Onboard blue button
Luka_Danilovic 14:90590736e700 7 BusOut leds(LED1, LED2, LED3); // Onboard LEDs bus
Luka_Danilovic 0:448e0e74e00f 8
Luka_Danilovic 0:448e0e74e00f 9 // Threads and Signals
Luka_Danilovic 14:90590736e700 10 Thread serThread (osPriorityNormal); // Create thread for serial comms
Luka_Danilovic 14:90590736e700 11 EventFlags thread_com; // Event flag for thread comms
Luka_Danilovic 0:448e0e74e00f 12
Luka_Danilovic 0:448e0e74e00f 13 // Namespaces
Luka_Danilovic 14:90590736e700 14 using namespace std; // Make all std symbols visible
Luka_Danilovic 14:90590736e700 15 using namespace ros; // Make all ros symbols visible
Luka_Danilovic 14:90590736e700 16 using namespace sensor_msgs; // Make all sensor_msgs symbols visible
Luka_Danilovic 14:90590736e700 17 using namespace std_msgs; // Make all sdt_msgs symbols visible
Luka_Danilovic 0:448e0e74e00f 18
Luka_Danilovic 0:448e0e74e00f 19 // Instantiations
Luka_Danilovic 14:90590736e700 20 TinyGPSPlus gpsModule; // Instance of GPS class
Luka_Danilovic 14:90590736e700 21 Serial gpsSer(GPS_TX, GPS_RX); // Instance of Serial class to gps
Luka_Danilovic 14:90590736e700 22 Serial usbSer(USB_TX, USB_RX); // Instance of Serial class to usb
Luka_Danilovic 14:90590736e700 23 NodeHandle nh; // Instance for ROS Node Handler
Luka_Danilovic 14:90590736e700 24 NavSatFix gps_odom_msg; // Instance for ROS NavSatFix message
Luka_Danilovic 14:90590736e700 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 14:90590736e700 30 char gps_c = 0; // GPS serial character
Luka_Danilovic 14:90590736e700 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 14:90590736e700 35 void streamF() // Function for Serial to PC
Luka_Danilovic 0:448e0e74e00f 36 {
Luka_Danilovic 14:90590736e700 37 while(true) { // Do forever
Luka_Danilovic 14:90590736e700 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 13:755a3cd9d35d 40 gps_odom_msg.latitude = gpsModule.location.lng(); // Get Longtitude
Luka_Danilovic 13:755a3cd9d35d 41 gps_odom_msg.longitude = gpsModule.location.lat(); // Get Latitude
Luka_Danilovic 0:448e0e74e00f 42
Luka_Danilovic 14:90590736e700 43 gps_odom_msg.header.stamp = nh.now(); // Get current time
Luka_Danilovic 14:90590736e700 44
Luka_Danilovic 14:90590736e700 45 gps_odom_pub.publish(&gps_odom_msg); // Publish the Odometry message
Luka_Danilovic 14:90590736e700 46 usbSer.printf("\n>Valid Loc<"); // Print out valid location
Luka_Danilovic 0:448e0e74e00f 47 }
Luka_Danilovic 0:448e0e74e00f 48 }
Luka_Danilovic 0:448e0e74e00f 49
Luka_Danilovic 0:448e0e74e00f 50 void setupRosMsg()
Luka_Danilovic 0:448e0e74e00f 51 {
Luka_Danilovic 0:448e0e74e00f 52
Luka_Danilovic 14:90590736e700 53 gps_odom_msg.header.frame_id = frameID; // Pack ROS frame ID
Luka_Danilovic 0:448e0e74e00f 54
Luka_Danilovic 14:90590736e700 55 gps_odom_msg.status.status = UNAGUMENTED; // Location fix method
Luka_Danilovic 14:90590736e700 56 gps_odom_msg.status.service = GPS_CONSTELL; // Satelite constellation used
Luka_Danilovic 13:755a3cd9d35d 57
Luka_Danilovic 14:90590736e700 58 gps_odom_msg.altitude = 0; // Altitude (Always fixed)
Luka_Danilovic 0:448e0e74e00f 59
Luka_Danilovic 14:90590736e700 60 gps_odom_msg.position_covariance[ 0] = CVX; // X pos covariance
Luka_Danilovic 14:90590736e700 61 gps_odom_msg.position_covariance[ 1] = CVO; // Zero
Luka_Danilovic 14:90590736e700 62 gps_odom_msg.position_covariance[ 2] = CVO; // Zero
Luka_Danilovic 14:90590736e700 63 gps_odom_msg.position_covariance[ 3] = CVO; // Zero
Luka_Danilovic 14:90590736e700 64 gps_odom_msg.position_covariance[ 4] = CVY; // Y pos covariance
Luka_Danilovic 14:90590736e700 65 gps_odom_msg.position_covariance[ 5] = CVO; // Zero
Luka_Danilovic 14:90590736e700 66 gps_odom_msg.position_covariance[ 6] = CVO; // Zero
Luka_Danilovic 14:90590736e700 67 gps_odom_msg.position_covariance[ 7] = CVO; // Zero
Luka_Danilovic 14:90590736e700 68 gps_odom_msg.position_covariance[ 8] = CVZ; // Z pos covariance
Luka_Danilovic 13:755a3cd9d35d 69
Luka_Danilovic 14:90590736e700 70 gps_odom_msg.position_covariance_type = UNNWN; // Covariance type
Luka_Danilovic 13:755a3cd9d35d 71
Luka_Danilovic 0:448e0e74e00f 72 }
Luka_Danilovic 0:448e0e74e00f 73
Luka_Danilovic 0:448e0e74e00f 74 /*================================== MAIN ====================================*/
Luka_Danilovic 14:90590736e700 75 int main() // Entry Point
Luka_Danilovic 0:448e0e74e00f 76 {
Luka_Danilovic 14:90590736e700 77 nh.getHardware()->setBaud(ROS_Baud); // Set Baud Rate for ROS Serial
Luka_Danilovic 14:90590736e700 78 nh.initNode(); // Initialise ROS Node Handler
Luka_Danilovic 14:90590736e700 79 nh.advertise(gps_odom_pub); // Adverstise Odometry topic
Luka_Danilovic 14:90590736e700 80 nh.advertise(gps_stat_pub); // Adverstise String topic
Luka_Danilovic 14:90590736e700 81 setupRosMsg(); // Set up ROS message parts
Luka_Danilovic 14:90590736e700 82 gpsSer.baud(GPS_Baud); // Set Baud rate for GPS Serial
Luka_Danilovic 14:90590736e700 83 usbSer.baud(USB_Baud); // Set Baud rate for USB Serial
Luka_Danilovic 14:90590736e700 84 serThread.start(streamF); // Start serial comms thread
Luka_Danilovic 0:448e0e74e00f 85
Luka_Danilovic 14:90590736e700 86 while(!nh.connected()) { // While node handler is not connected
Luka_Danilovic 14:90590736e700 87 nh.spinOnce(); // Attempt to connect and synchronise
Luka_Danilovic 0:448e0e74e00f 88 }
Luka_Danilovic 0:448e0e74e00f 89
Luka_Danilovic 14:90590736e700 90 while (true) { // When node handler is connected, do forever
Luka_Danilovic 0:448e0e74e00f 91
Luka_Danilovic 14:90590736e700 92 nh.spinOnce(); // Reccuring connect and synchronise
Luka_Danilovic 0:448e0e74e00f 93
Luka_Danilovic 14:90590736e700 94 if(gpsSer.readable()) { // If serial buffer has character
Luka_Danilovic 14:90590736e700 95 gps_c = gpsSer.getc(); // Read serial buffer and store character
Luka_Danilovic 14:90590736e700 96 gpsModule.encode(gps_c); // Encode character from GPS
Luka_Danilovic 4:df54ebe69b38 97
Luka_Danilovic 1:b610535e5879 98 } else {
Luka_Danilovic 14:90590736e700 99 leds = leds & 0b100; // Flash LED[0,1] bus OFF. Leave LED[2] on if its on to indicate that the signal was present
Luka_Danilovic 3:30c3f20c2387 100 }
Luka_Danilovic 3:30c3f20c2387 101
Luka_Danilovic 14:90590736e700 102 if (gpsModule.location.isValid()) { // If GPS location is Valid
Luka_Danilovic 14:90590736e700 103 thread_com.set(PRINT_DATA); // Set EventFlag to Print Data
Luka_Danilovic 14:90590736e700 104 leds = LEDS_ON; // Flash LED bus ON
Luka_Danilovic 3:30c3f20c2387 105 }
Luka_Danilovic 1:b610535e5879 106
Luka_Danilovic 14:90590736e700 107 if (btn) { // If blue button is pressed
Luka_Danilovic 5:a201e5377e90 108 sats_n = gpsModule.satellites.value(); // Aquire satelite number
Luka_Danilovic 14:90590736e700 109 usbSer.printf("\nstatChck: %d Satelites", sats_n); // Print Status
Luka_Danilovic 0:448e0e74e00f 110 }
Luka_Danilovic 3:30c3f20c2387 111
Luka_Danilovic 0:448e0e74e00f 112 }
Luka_Danilovic 0:448e0e74e00f 113 }