File content as of revision 17:264eb15c8be2:

#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 sensor_msgs;                       // Make all sensor_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
NavSatFix gps_odom_msg;                            // Instance for ROS NavSatFix 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.latitude  = gpsModule.location.lng(); // Get Longtitude
        gps_odom_msg.longitude = gpsModule.location.lat(); // Get Latitude

        gps_odom_msg.header.stamp = nh.now();      // Get current time
        
        gps_odom_pub.publish(&gps_odom_msg);       // Publish the Odometry message
    }
}

void setupRosMsg()
{

    gps_odom_msg.header.frame_id = frameID;        // Pack ROS frame ID

    gps_odom_msg.status.status = UNAGUMENTED;      // Location fix method
    gps_odom_msg.status.service = GPS_CONSTELL;    // Satelite constellation used
    
    gps_odom_msg.altitude  = 0;                    // Altitude (Always fixed)

    gps_odom_msg.position_covariance[ 0] = CVX;    // X pos covariance
    gps_odom_msg.position_covariance[ 1] = CVO;    // Zero
    gps_odom_msg.position_covariance[ 2] = CVO;    // Zero
    gps_odom_msg.position_covariance[ 3] = CVO;    // Zero
    gps_odom_msg.position_covariance[ 4] = CVY;    // Y pos covariance
    gps_odom_msg.position_covariance[ 5] = CVO;    // Zero
    gps_odom_msg.position_covariance[ 6] = CVO;    // Zero
    gps_odom_msg.position_covariance[ 7] = CVO;    // Zero
    gps_odom_msg.position_covariance[ 8] = CVZ;    // Z pos covariance
    
    gps_odom_msg.position_covariance_type = UNNWN; // Covariance type

}

/*================================== MAIN ====================================*/
int main()                                         // Entry Point
{
    nh.getHardware()->setBaud(ROS_Baud);           // Set Baud Rate for ROS Serial
    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

        } 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
        }
        
        sats_n = gpsModule.satellites.value();     // Aquire satelite number
        if (sats_n > 0){
            leds = LEDS_ON;
            }

        if (btn) {                                 // If blue button is pressed
            
            usbSer.printf("\n\nTimestamp: %d ", nh.now());                  // Print Status: Time Stamp
            usbSer.printf("\nstatChck: %d Satelites", sats_n);              // Print Status: Satelite Number
            usbSer.printf("\nstatChck: %f Long", gpsModule.location.lng()); // Print Status: Long
            usbSer.printf("\nstatChck: %f Lat", gpsModule.location.lat());  // Print Status: Lat
        }

    }
}