File content as of revision 3:0ca91f8afec5:

// Class Scheduler
#include "mbed.h"
#include "uLCD_4DGL.h"
#include <string>
#include <vector>
#include "Course.h"
#include "SDFileSystem.h"
#include "LSM9DS1.h"
#include "PinDetect.h"
#define PI 3.14159
#define DECLINATION -4.94 // Declination (degrees) in Atlanta,GA.

SDFileSystem sd(p5, p6, p7, p8, "sd"); // the pinout on the mbed Cool Components workshop board
Serial pc(USBTX, USBRX);
Serial esp(p28, p27); // tx, rx
uLCD_4DGL uLCD(p13,p14,p15); // serial tx, serial rx, reset pin;
PinDetect left(p20);
PinDetect right(p18);
PinDetect center(p19);

char ssid[32] = "";     // enter WiFi router ssid inside the quotes
char pwd [32] = ""; // enter WiFi router password inside the quotes

// for sending/receiving data over serial
volatile int tx_in=0;
volatile int tx_out=0;
volatile int rx_in=0;
volatile int rx_out=0;
const int buffer_size = 4095;
char tx_buffer[buffer_size+1];
char rx_buffer[buffer_size+1];
void Tx_interrupt();
void Rx_interrupt();
void read_line();

int DataRX;
int update;
char cmdbuff[512];
char webdata[4096]; // This may need to be bigger depending on WEB browser used
void SendCMD(),read_line(),ReadWebData(),startserver();
int addCourseToVector(vector<Course>& cVec, Course newCourse);
vector<Course> courseVec;
void readClassFile(vector<Course>& cVec), writeClassFile(vector<Course>& cVec);
void displayCourseVec();
float calculateHeading(float mx, float my), computeAngleToDestination(float diffLat, float diffLong);
int xEnd, yEnd, lastXEnd, lastYEnd = 0.0;
char rx_line[512];
string ip_line;
bool ip_found = false;
bool screen_refreshed = false;
int volatile current_screen = 0;
bool volatile screen_change = false;
int port        =80;  // set server port
int SERVtimeout =5;    // set server timeout in seconds in case link breaks.

void left_callback(void)
{
    if (courseVec.size() != 0) {
        current_screen = (3 + current_screen - 1)%3;
        screen_change = true;
    } else
        current_screen = 0;
}

void right_callback(void)
{
    if (courseVec.size() != 0) {
        current_screen = (3 + current_screen + 1)%3;
        screen_change = true;
    } else
        current_screen = 0;
}

void center_callback(void)
{
    if (current_screen == 0) {
        courseVec.clear();
        update = 1;
    }
}

int main()
{
    LSM9DS1 IMU(p9, p10, 0xD6, 0x3C);
    float h = 0.0;
    float angleToDest = 0.0;
    IMU.begin();
    if (!IMU.begin()) {
        pc.printf("Failed to communicate with LSM9DS1.\n");
    }
    uLCD.cls();
    uLCD.printf("Place IMU flat");
    IMU.calibrate(1);
    uLCD.cls();
    uLCD.printf("Rotate IMU 360\ndegrees in \nhorizontal plane");
    IMU.calibrateMag(0);

    float myLat = 33.776346;
    float myLong = -84.378927;

    float destinationLat = 33.772778;
    float destinationLong = -84.379685;

    left.mode(PullUp);
    right.mode(PullUp);
    center.mode(PullUp);

    left.attach_deasserted(&left_callback);
    right.attach_deasserted(&right_callback);
    center.attach_deasserted(&center_callback);
    left.setSampleFrequency();
    right.setSampleFrequency();
    center.setSampleFrequency();

    readClassFile(courseVec);

    pc.baud(9600);
    esp.baud(9600);
    // Setup a serial interrupt function to receive data
    esp.attach(&Rx_interrupt, Serial::RxIrq);
    // Setup a serial interrupt function to transmit data
    esp.attach(&Tx_interrupt, Serial::TxIrq);
    startserver();
    DataRX=0;

    while(1) {
        switch(current_screen) {
            case 0:
                if (screen_change) {
                    displayCourseVec();
                    screen_refreshed = true;
                    screen_change = false;
                }
                if(DataRX==1) {
                    ReadWebData();
                    esp.attach(&Rx_interrupt, Serial::RxIrq);
                    if (!screen_refreshed) {
                        displayCourseVec();
                        screen_refreshed = true;
                    }
                }
                if(update==1) {
                    writeClassFile(courseVec);
                    if (!screen_refreshed) {
                        displayCourseVec();
                    }
                    update=0;
                }
                screen_refreshed = false;
                break;
            case 1:
                if (screen_change) {
                    uLCD.printf("Compass screen");
                    screen_change = false;
                }

                IMU.readMag();
                h = calculateHeading(IMU.calcMag(IMU.mx), IMU.calcMag(IMU.my));
                destinationLat = courseVec[0].getLat();
                destinationLong = courseVec[0].getLong();
                float diffLat = destinationLat - myLat;
                float diffLong = destinationLong - myLong;
                angleToDest = computeAngleToDestination(diffLat, diffLong);
                h = angleToDest - h;
                h = h - 90;
                if (h < 0)
                    h = h + 360;
                if (h > 360)
                    h = h - 360;
                xEnd = 0;
                yEnd = 0;
                float rads = 0.0;
                //uLCD.cls();
                if (h < 90) {
                    rads = h * PI / 180;
                    xEnd = floor(63 * sin(rads) + .5);
                    yEnd = floor(63 * cos(rads) + .5);
                    xEnd = 63 + xEnd;
                    yEnd = 63 + yEnd;
                } else if (90 < h < 180) {
                    h = h - 90;
                    rads = h * PI / 180;
                    xEnd = floor(63 * cos(rads) + .5);
                    yEnd = floor(63 * sin(rads) + .5);
                    xEnd = 63 + xEnd;
                    yEnd = 63 - yEnd;
                } else if (180 < h < 270) {
                    h = h - 180;
                    rads = h * PI / 180;
                    xEnd = floor(63 * sin(rads) + .5);
                    yEnd = floor(63 * cos(rads) + .5);
                    xEnd = 63 - xEnd;
                    yEnd = 63 - yEnd;
                } else if (270 < h) {
                    h = h - 270;
                    rads = h * PI / 180;
                    xEnd = floor(63 * cos(rads) + .5);
                    yEnd = floor(63 * sin(rads) + .5);
                    xEnd = 63 - xEnd;
                    yEnd = 63 + yEnd;
                }
                //uLCD.line(63, 63, lastXEnd, lastYEnd, BLACK);
                uLCD.cls();
                uLCD.line(63, 63, xEnd, yEnd, WHITE);
                lastXEnd = xEnd; lastYEnd = yEnd;
                wait(.1);
                break;
            case 2:
                if (screen_change) {
                    uLCD.cls();
                    uLCD.printf("Distance screen");
                    screen_change = false;
                }
                break;

        }
    }
}

// Reads and processes GET and POST web data
void ReadWebData()
{
    wait_ms(200);
    esp.attach(NULL,Serial::RxIrq);
    DataRX=0;

    if (!ip_found) {
        char* ip_loc = strstr(rx_buffer, "IP Address:");
        if ((ip_loc != NULL)) {
            ip_loc = strstr(&ip_loc[10], "IP Address:");
            if (ip_loc != NULL) {
                char ip_buf[16];
                memset(ip_buf, '\0', sizeof(ip_buf));
                memcpy(ip_buf, &ip_loc[12], 15);
                ip_line = ip_buf;
                ip_found = true;
            }
        }
    }

    memset(webdata, '\0', sizeof(webdata));
    strcpy(webdata, rx_buffer);
    memset(rx_buffer, '\0', sizeof(rx_buffer));
    rx_in = 0;
    rx_out = 0;

    // check web data for form information
    if (strstr(webdata, "building=none") == NULL) {
        char buildingBuf[4];
        string building;
        char* buildingLoc = strstr(webdata, "building=");
        if (buildingLoc != NULL) {
            memcpy(buildingBuf, &buildingLoc[9], 3);
            buildingBuf[3] = '\0';
            building = buildingBuf;;
        }

        char hourBuf[3];
        int hour = -1;
        char* hourLoc = strstr(webdata, "hour=");
        if (hourLoc != NULL) {
            memcpy(hourBuf, &hourLoc[5], 2);
            hourBuf[2] = '\0';
            hour = atoi(hourBuf);
        }

        char minuteBuf[3];
        int minute = -1;
        char* minuteLoc = strstr(webdata, "minute=");
        if (minuteLoc != NULL) {
            memcpy(minuteBuf, &minuteLoc[7], 2);
            minuteBuf[2] = '\0';
            minute = atoi(minuteBuf);
        }

        char ampmBuf[3];
        string ampm;
        char* ampmLoc = strstr(webdata, "AMPM=");
        if (ampmLoc != NULL) {
            memcpy(ampmBuf, &ampmLoc[5], 2);
            ampmBuf[2] = '\0';
            ampm = ampmBuf;
        }

        if (minute != -1) {
            Course newCourse(building, hour, minute, ampm);
            addCourseToVector(courseVec, newCourse);
            update = 1;
        }
    }
}
// Starts webserver
void startserver()
{
    uLCD.cls();
    uLCD.locate(0,0);
    uLCD.printf("Starting server...");
    pc.printf("++++++++++ Resetting ESP ++++++++++\r\n");
    strcpy(cmdbuff,"node.restart()\r\n");
    SendCMD();
    wait(2);
    read_line();

    pc.printf("\n---------- Connecting to AP ----------\r\n");
    //pc.printf("ssid = %s   pwd = %s\r\n",ssid,pwd);
    strcpy(cmdbuff, "wifi.sta.config(\"");
    strcat(cmdbuff, ssid);
    strcat(cmdbuff, "\",\"");
    strcat(cmdbuff, pwd);
    strcat(cmdbuff, "\")\r\n");
    SendCMD();
    wait(3);
    read_line();

    pc.printf("\n++++++++++ Starting Server ++++++++++\r\n> ");

    //create server
    sprintf(cmdbuff, "srv=net.createServer(net.TCP,%d)\r\n",SERVtimeout);
    SendCMD();
    read_line();
    wait(0.5);
    strcpy(cmdbuff,"srv:listen(80,function(conn)\r\n");
    SendCMD();
    read_line();
    wait(0.3);
    strcpy(cmdbuff,"conn:on(\"receive\",function(conn,payload) \r\n");
    SendCMD();
    read_line();
    wait(0.3);

    //print data to mbed
    strcpy(cmdbuff,"print(payload)\r\n");
    SendCMD();
    read_line();
    wait(0.2);

    //web page data
    strcpy(cmdbuff,"conn:send('<!DOCTYPE html><html><head><title>Class Scheduler!</title></head><body><h1>Class Scheduler</h1>')\r\n");
    SendCMD();
    read_line();
    wait(0.4);
    strcpy(cmdbuff,"conn:send('<p><div class = \"content\">Input your schedule below!</div></p>')\r\n");
    SendCMD();
    read_line();
    wait(0.2);
    strcpy(cmdbuff,"conn:send('<form method=\"POST\">')\r\n");
    SendCMD();
    read_line();
    wait(0.3);
    strcpy(cmdbuff, "conn:send('<select name=\"building\">')\r\n");
    SendCMD();
    read_line();
    wait(0.3);
    strcpy(cmdbuff, "conn:send('<option value =\"none\">None</option>')\r\n");
    SendCMD();
    read_line();
    wait(0.3);
    strcpy(cmdbuff,"conn:send('<option value=\"CLH\">Clough Undergraduate Learning Commons</option>')\r\n");
    SendCMD();
    read_line();
    wait(0.3);
    strcpy(cmdbuff,"conn:send('<option value=\"COC\">College of Computing</option>')\r\n");
    SendCMD();
    read_line();
    wait(0.3);
    strcpy(cmdbuff, "conn:send('<option value=\"KLS\">Klaus Advanced Computing Building</option>')\r\n");
    SendCMD();
    read_line();
    wait(0.3);
    strcpy(cmdbuff, "conn:send('<option value=\"VAN\">Van Leer</option></select>')\r\n");
    SendCMD();
    read_line();
    wait(0.3);
    strcpy(cmdbuff, "conn:send('<select name=\"hour\"><option value=\"01\">1</option><option value=\"02\">2</option>')\r\n");
    SendCMD();
    read_line();
    wait(.3);
    strcpy(cmdbuff, "conn:send('<option value=\"03\">3</option><option value=\"04\">4</option><option value=\"05\">5</option>')\r\n");
    SendCMD();
    read_line();
    wait(.3);
    strcpy(cmdbuff, "conn:send('<option value=\"06\">6</option><option value=\"07\">7</option><option value=\"08\">8</option>')\r\n");
    SendCMD();
    read_line();
    wait(0.3);
    strcpy(cmdbuff, "conn:send('<option value=\"09\">9</option><option value=\"10\">10</option><option value=\"11\">11</option><option value=\"12\">12</option></select>')\r\n");
    SendCMD();
    read_line();
    wait(0.3);
    strcpy(cmdbuff, "conn:send('<select name=\"minute\"><option value=\"00\">00</option><option value=\"05\">05</option>')\r\n");
    SendCMD();
    read_line();
    wait(0.3);
    strcpy(cmdbuff, "conn:send('<option value=\"10\">10</option><option value=\"15\">15</option>')\r\n");
    SendCMD();
    read_line();
    wait(0.3);
    strcpy(cmdbuff, "conn:send('<option value=\"20\">20</option><option value=\"25\">25</option>')\r\n");
    SendCMD();
    read_line();
    wait(0.3);
    strcpy(cmdbuff, "conn:send('<option value=\"30\">30</option><option value=\"35\">35</option>')\r\n");
    SendCMD();
    read_line();
    wait(0.3);
    strcpy(cmdbuff, "conn:send('<option value=\"40\">40</option><option value=\"45\">45</option>')\r\n");
    SendCMD();
    read_line();
    wait(0.3);
    strcpy(cmdbuff, "conn:send('<option value=\"50\">50</option><option value=\"55\">55</option></select>')\r\n");
    SendCMD();
    read_line();
    wait(0.3);
    strcpy(cmdbuff, "conn:send('<select name=\"AMPM\"><option value=\"AM\">AM</option><option value=\"PM\">PM</option></select><br></br>')\r\n");
    SendCMD();
    read_line();
    wait(0.3);

    strcpy(cmdbuff,"conn:send('<p><input type=\"submit\" value=\"Add Class\"></form>')\r\n");
    SendCMD();
    read_line();
    wait(0.3);
    strcpy(cmdbuff, "conn:send('</body></html>')\r\n");
    SendCMD();
    read_line();
    wait(0.5);
    // end web page data
    strcpy(cmdbuff, "conn:on(\"sent\",function(conn) conn:close() end)\r\n"); // close current connection
    SendCMD();
    read_line();
    wait(0.3);
    strcpy(cmdbuff, "end)\r\n");
    SendCMD();
    read_line();
    wait(0.2);
    strcpy(cmdbuff, "end)\r\n");
    SendCMD();
    read_line();
    wait(0.2);

    strcpy(cmdbuff, "tmr.alarm(0, 1000, 1, function()\r\n");
    SendCMD();
    read_line();
    wait(0.2);
    strcpy(cmdbuff, "if wifi.sta.getip() == nil then\r\n");
    SendCMD();
    read_line();
    wait(0.2);
    strcpy(cmdbuff, "print(\"Connecting to AP...\\n\")\r\n");
    SendCMD();
    read_line();
    wait(0.2);
    strcpy(cmdbuff, "else\r\n");
    SendCMD();
    read_line();
    wait(0.2);
    strcpy(cmdbuff, "ip, nm, gw=wifi.sta.getip()\r\n");
    SendCMD();
    read_line();
    wait(0.2);
    strcpy(cmdbuff,"print(\"IP Address: \",ip)\r\n");
    SendCMD();
    read_line();
    wait(0.2);
    strcpy(cmdbuff,"tmr.stop(0)\r\n");
    SendCMD();
    read_line();
    wait(0.2);
    strcpy(cmdbuff,"end\r\n");
    SendCMD();
    read_line();
    wait(0.2);
    strcpy(cmdbuff,"end)\r\n");
    SendCMD();
    read_line();
    wait(0.2);

    pc.printf("\n\n++++++++++ Ready ++++++++++\r\n\n");
}

// ESP Command data send
void SendCMD()
{
    int i;
    char temp_char;
    bool empty;
    i = 0;
// Start Critical Section - don't interrupt while changing global buffer variables
    NVIC_DisableIRQ(UART1_IRQn);
    empty = (tx_in == tx_out);
    while ((i==0) || (cmdbuff[i-1] != '\n')) {
// Wait if buffer full
        if (((tx_in + 1) % buffer_size) == tx_out) {
// End Critical Section - need to let interrupt routine empty buffer by sending
            NVIC_EnableIRQ(UART1_IRQn);
            while (((tx_in + 1) % buffer_size) == tx_out) {
            }
// Start Critical Section - don't interrupt while changing global buffer variables
            NVIC_DisableIRQ(UART1_IRQn);
        }
        tx_buffer[tx_in] = cmdbuff[i];
        i++;
        tx_in = (tx_in + 1) % buffer_size;
    }
    if (esp.writeable() && (empty)) {
        temp_char = tx_buffer[tx_out];
        tx_out = (tx_out + 1) % buffer_size;
// Send first character to start tx interrupts, if stopped
        esp.putc(temp_char);
    }
// End Critical Section
    NVIC_EnableIRQ(UART1_IRQn);
    return;
}

// Read a line from the large rx buffer from rx interrupt routine
void read_line()
{
    int i;
    i = 0;
// Start Critical Section - don't interrupt while changing global buffer variables
    NVIC_DisableIRQ(UART1_IRQn);
// Loop reading rx buffer characters until end of line character
    while ((i==0) || (rx_line[i-1] != '\r')) {
// Wait if buffer empty
        if (rx_in == rx_out) {
// End Critical Section - need to allow rx interrupt to get new characters for buffer
            NVIC_EnableIRQ(UART1_IRQn);
            while (rx_in == rx_out) {
            }
// Start Critical Section - don't interrupt while changing global buffer variables
            NVIC_DisableIRQ(UART1_IRQn);
        }
        rx_line[i] = rx_buffer[rx_out];
        i++;
        rx_out = (rx_out + 1) % buffer_size;
    }
// End Critical Section
    NVIC_EnableIRQ(UART1_IRQn);
    rx_line[i-1] = 0;

    return;
}

// Interupt Routine to read in data from serial port
void Rx_interrupt()
{
    DataRX=1;
// Loop just in case more than one character is in UART's receive FIFO buffer
// Stop if buffer full
    while ((esp.readable()) && (((rx_in + 1) % buffer_size) != rx_out)) {
        rx_buffer[rx_in] = esp.getc();
// Uncomment to Echo to USB serial to watch data flow
        pc.putc(rx_buffer[rx_in]);
        rx_in = (rx_in + 1) % buffer_size;
    }
    return;
}

// Interupt Routine to write out data to serial port
void Tx_interrupt()
{
// Loop to fill more than one character in UART's transmit FIFO buffer
// Stop if buffer empty
    while ((esp.writeable()) && (tx_in != tx_out)) {
        esp.putc(tx_buffer[tx_out]);
        tx_out = (tx_out + 1) % buffer_size;
    }
    return;
}

int addCourseToVector(vector<Course>& cVec, Course newCourse)
{
    int numIterations = 0;

    if (cVec.size() == 0) {
        cVec.push_back(newCourse);
        return 1;
    }

    for (int i = 0; i < cVec.size(); i++) {
        numIterations++;
        if (cVec[i].getAMPM_toInt() < newCourse.getAMPM_toInt())
            continue;
        else if (newCourse.getAMPM_toInt() < cVec[i].getAMPM_toInt()) {
            cVec.insert(cVec.begin()+i, newCourse);
            return 1;
        } else if (cVec[i].getAMPM_toInt() == newCourse.getAMPM_toInt()) {
            if (cVec[i].getHour_forCompare() < newCourse.getHour_forCompare())
                continue;
            else if (newCourse.getHour_forCompare() < cVec[i].getHour_forCompare()) {
                cVec.insert(cVec.begin()+i, newCourse);
                return 1;
            } else if (cVec[i].getHour_forCompare() == newCourse.getHour_forCompare()) {
                if (cVec[i].getMinute() < newCourse.getMinute())
                    continue;
                else if (newCourse.getMinute() < cVec[i].getMinute()) {
                    cVec.insert(cVec.begin()+i, newCourse);
                    return 1;
                } else if (cVec[i].getMinute() == newCourse.getMinute()) {
                    uLCD.cls();
                    uLCD.locate(0,0);
                    uLCD.printf("Can not add coure!");
                    uLCD.locate(0,1);
                    uLCD.printf("Course already at");
                    uLCD.locate(0,2);
                    uLCD.printf("%i:%s%s", newCourse.getHour(), newCourse.getMinute_toString(), newCourse.getAMPM());
                    wait(5);
                    return 0;
                }
            }
        }
    }

    if (numIterations == cVec.size()) {
        cVec.push_back(newCourse);
        return 1;
    }

    return 0;
}

void writeClassFile(vector<Course>& cVec)
{
    FILE *writeClass = fopen("/sd/classdir/classes.txt", "w");
    if (writeClass != NULL) {
        string line = "";
        for (int i = 0; i < cVec.size(); i++) {
            if (i != (cVec.size() - 1))
                line = cVec[i].getFileString() + "\n";
            else
                line = cVec[i].getFileString();
            fprintf(writeClass, line.c_str());
        }
        fclose(writeClass);
    }
}

void readClassFile(vector<Course>& cVec)
{
    cVec.clear();

    FILE *readFp = fopen("/sd/classdir/classes.txt", "r");
    char line[15];
    char buildingBuf[4];
    char hourBuf[3];
    int hour;
    char minuteBuf[3];
    int minute;
    char ampmBuf[3];
    uLCD.cls();
    uLCD.locate(0, 1);
    uLCD.printf("Reading class file...");

    memset(buildingBuf, 0, sizeof(buildingBuf));
    memset(hourBuf, 0, sizeof(hourBuf));
    memset(minuteBuf, 0, sizeof(minuteBuf));
    memset(ampmBuf, 0, sizeof(ampmBuf));
    memset(line, 0, sizeof(line));

    if (readFp == NULL)
        return;
    else {
        while (!feof(readFp)) {
            fgets(line, 15, readFp);
            if(line[8] == NULL)
                continue;
            memcpy(buildingBuf, line, 3);
            memcpy(hourBuf, &line[4], 2);
            memcpy(minuteBuf, &line[7], 2);
            memcpy(ampmBuf, &line[10], 2);

            string building = buildingBuf;
            hour = atoi(hourBuf);
            minute = atoi(minuteBuf);
            string ampm = ampmBuf;

            Course temp(building, hour, minute, ampm);
            cVec.push_back(temp);
        }
    }
    fclose(readFp);
    return;
}

void displayCourseVec()
{
    if (courseVec.size() == 0) {
        uLCD.cls();
        uLCD.locate(0,0);
        uLCD.printf("No classes input!");
        uLCD.locate(0,1);
        uLCD.printf("To input classes \nGo to:");
        uLCD.locate(0,4);
        uLCD.printf("%s", ip_line);
    } else {
        uLCD.cls();
        uLCD.locate(0,1);
        for (int i = 0; i < courseVec.size(); i++) {
            uLCD.locate(0, i);
            uLCD.printf("%s", courseVec[i].getDisplayString());
        }
        uLCD.locate(0, 8);
        uLCD.printf("To add courses \ngo to");
        uLCD.locate(0,10);
        uLCD.printf("%s", ip_line);
        uLCD.locate(0,12);
        uLCD.printf("To reset courses\npress center");
    }
}

float calculateHeading(float mx, float my)
{
    float heading = 0.0;
    if (my == 0.0)
        heading = (mx < 0.0) ? 180.0 : 0.0;
    else
        heading = atan2(mx, my)*360.0/(2.0*PI);
    //pc.printf("heading atan=%f \n\r",heading);
    heading -= DECLINATION; //correct for geo location
    if(heading>180.0) heading = heading - 360.0;
    else if(heading<-180.0) heading = 360.0 + heading;
    else if(heading<0.0) heading = 360.0  + heading;

    // Convert everything from radians to degrees:
    //heading *= 180.0 / PI;

    //pc.printf("Magnetic Heading: %f degress\n\r",heading);
    return heading;
}

float computeAngleToDestination(float diffLat, float diffLong)
{
    float angle = 0.0;

    if (diffLat > 0) {
        if (diffLong > 0) {
            // in quadrant 1
            angle = 180*atan2(diffLat,diffLong)/PI;
        } else {
            // in quadrant 2
            angle = 180*atan2(diffLat,-1 * diffLong)/PI;
            angle = 180 - angle;
        }
    } else {
        if (diffLong > 0) {
            // in quadrant 4
            angle = 180*atan2(-1*diffLat, diffLong)/PI;
            angle = 360 - angle;
        } else {
            // in quadrant 3
            angle = 180*atan2(-1*diffLat, -1*diffLong)/PI;
            angle = 180 + angle;
        }
    }

    //pc.printf("Angle to Destination: %f degress\n\r",angle);
    return angle;
}