Luka Danilovic / Mbed OS PROJ515_GPS

GPS NMEA through NavSatFix

Dependencies:   TinyGPSPlus

PROJ515.cpp@17:264eb15c8be2, 2019-04-23 (annotated)

Committer:
Luka_Danilovic
Date:
Tue Apr 23 16:02:34 2019 +0000
Revision:
17:264eb15c8be2
Parent:
14:90590736e700 
Fixed the issue of location not updating when "statCheck" print-out is running

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 0:448e0e74e00f 46 }
Luka_Danilovic 0:448e0e74e00f 47 }
Luka_Danilovic 0:448e0e74e00f 48
Luka_Danilovic 0:448e0e74e00f 49 void setupRosMsg()
Luka_Danilovic 0:448e0e74e00f 50 {
Luka_Danilovic 0:448e0e74e00f 51
Luka_Danilovic 14:90590736e700 52 gps_odom_msg.header.frame_id = frameID; // Pack ROS frame ID
Luka_Danilovic 0:448e0e74e00f 53
Luka_Danilovic 14:90590736e700 54 gps_odom_msg.status.status = UNAGUMENTED; // Location fix method
Luka_Danilovic 14:90590736e700 55 gps_odom_msg.status.service = GPS_CONSTELL; // Satelite constellation used
Luka_Danilovic 13:755a3cd9d35d 56
Luka_Danilovic 14:90590736e700 57 gps_odom_msg.altitude = 0; // Altitude (Always fixed)
Luka_Danilovic 0:448e0e74e00f 58
Luka_Danilovic 14:90590736e700 59 gps_odom_msg.position_covariance[ 0] = CVX; // X pos covariance
Luka_Danilovic 14:90590736e700 60 gps_odom_msg.position_covariance[ 1] = CVO; // Zero
Luka_Danilovic 14:90590736e700 61 gps_odom_msg.position_covariance[ 2] = CVO; // Zero
Luka_Danilovic 14:90590736e700 62 gps_odom_msg.position_covariance[ 3] = CVO; // Zero
Luka_Danilovic 14:90590736e700 63 gps_odom_msg.position_covariance[ 4] = CVY; // Y pos covariance
Luka_Danilovic 14:90590736e700 64 gps_odom_msg.position_covariance[ 5] = CVO; // Zero
Luka_Danilovic 14:90590736e700 65 gps_odom_msg.position_covariance[ 6] = CVO; // Zero
Luka_Danilovic 14:90590736e700 66 gps_odom_msg.position_covariance[ 7] = CVO; // Zero
Luka_Danilovic 14:90590736e700 67 gps_odom_msg.position_covariance[ 8] = CVZ; // Z pos covariance
Luka_Danilovic 13:755a3cd9d35d 68
Luka_Danilovic 14:90590736e700 69 gps_odom_msg.position_covariance_type = UNNWN; // Covariance type
Luka_Danilovic 13:755a3cd9d35d 70
Luka_Danilovic 0:448e0e74e00f 71 }
Luka_Danilovic 0:448e0e74e00f 72
Luka_Danilovic 0:448e0e74e00f 73 /*================================== MAIN ====================================*/
Luka_Danilovic 14:90590736e700 74 int main() // Entry Point
Luka_Danilovic 0:448e0e74e00f 75 {
Luka_Danilovic 14:90590736e700 76 nh.getHardware()->setBaud(ROS_Baud); // Set Baud Rate for ROS Serial
Luka_Danilovic 14:90590736e700 77 nh.initNode(); // Initialise ROS Node Handler
Luka_Danilovic 14:90590736e700 78 nh.advertise(gps_odom_pub); // Adverstise Odometry topic
Luka_Danilovic 14:90590736e700 79 nh.advertise(gps_stat_pub); // Adverstise String topic
Luka_Danilovic 14:90590736e700 80 setupRosMsg(); // Set up ROS message parts
Luka_Danilovic 14:90590736e700 81 gpsSer.baud(GPS_Baud); // Set Baud rate for GPS Serial
Luka_Danilovic 14:90590736e700 82 usbSer.baud(USB_Baud); // Set Baud rate for USB Serial
Luka_Danilovic 14:90590736e700 83 serThread.start(streamF); // Start serial comms thread
Luka_Danilovic 0:448e0e74e00f 84
Luka_Danilovic 14:90590736e700 85 while(!nh.connected()) { // While node handler is not connected
Luka_Danilovic 14:90590736e700 86 nh.spinOnce(); // Attempt to connect and synchronise
Luka_Danilovic 0:448e0e74e00f 87 }
Luka_Danilovic 0:448e0e74e00f 88
Luka_Danilovic 14:90590736e700 89 while (true) { // When node handler is connected, do forever
Luka_Danilovic 0:448e0e74e00f 90
Luka_Danilovic 14:90590736e700 91 nh.spinOnce(); // Reccuring connect and synchronise
Luka_Danilovic 0:448e0e74e00f 92
Luka_Danilovic 14:90590736e700 93 if(gpsSer.readable()) { // If serial buffer has character
Luka_Danilovic 14:90590736e700 94 gps_c = gpsSer.getc(); // Read serial buffer and store character
Luka_Danilovic 14:90590736e700 95 gpsModule.encode(gps_c); // Encode character from GPS
Luka_Danilovic 4:df54ebe69b38 96
Luka_Danilovic 1:b610535e5879 97 } else {
Luka_Danilovic 14:90590736e700 98 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 99 }
Luka_Danilovic 3:30c3f20c2387 100
Luka_Danilovic 14:90590736e700 101 if (gpsModule.location.isValid()) { // If GPS location is Valid
Luka_Danilovic 14:90590736e700 102 thread_com.set(PRINT_DATA); // Set EventFlag to Print Data
Luka_Danilovic 3:30c3f20c2387 103 }
Luka_Danilovic 17:264eb15c8be2 104
Luka_Danilovic 17:264eb15c8be2 105 sats_n = gpsModule.satellites.value(); // Aquire satelite number
Luka_Danilovic 17:264eb15c8be2 106 if (sats_n > 0){
Luka_Danilovic 17:264eb15c8be2 107 leds = LEDS_ON;
Luka_Danilovic 17:264eb15c8be2 108 }
Luka_Danilovic 1:b610535e5879 109
Luka_Danilovic 14:90590736e700 110 if (btn) { // If blue button is pressed
Luka_Danilovic 17:264eb15c8be2 111
Luka_Danilovic 17:264eb15c8be2 112 usbSer.printf("\n\nTimestamp: %d ", nh.now()); // Print Status: Time Stamp
Luka_Danilovic 17:264eb15c8be2 113 usbSer.printf("\nstatChck: %d Satelites", sats_n); // Print Status: Satelite Number
Luka_Danilovic 17:264eb15c8be2 114 usbSer.printf("\nstatChck: %f Long", gpsModule.location.lng()); // Print Status: Long
Luka_Danilovic 17:264eb15c8be2 115 usbSer.printf("\nstatChck: %f Lat", gpsModule.location.lat()); // Print Status: Lat
Luka_Danilovic 0:448e0e74e00f 116 }
Luka_Danilovic 3:30c3f20c2387 117
Luka_Danilovic 0:448e0e74e00f 118 }
Luka_Danilovic 0:448e0e74e00f 119 }