Diff: main.cpp

Revision:

13:1a9c0f9d7128

Parent:

12:49600de70963

Child:

14:4f0ebc5a4f00

--- a/main.cpp	Tue Feb 28 01:32:00 2017 +0000
+++ b/main.cpp	Wed Mar 01 03:39:33 2017 +0000
@@ -28,6 +28,7 @@
 #include "BMP180.h"
 #include "SDFileSystem.h"
 #include "GPSISR.h"
+#include "nav.h"
 
 
 #include "ArialR16x17.h"
@@ -106,6 +107,11 @@
 // Set up serial interrupe service handler for gps characters.
 GPS MyGPS(PIN_TX_GPS,PIN_RX_GPS, 9600);
 
+//Navigation Class.
+NAV nav;
+double plat = 42.826420;
+double plon = -84.219413;
+
 void Draw_Compass_Rose(void);
 void arrow(int x2, int y2, int x1, int y1, int alength, int awidth, int colour);
 
@@ -131,13 +137,13 @@
     TFT.foreground(Blue);
     TFT.background(White);
     wait(0.1);
-   
+       
     Draw_Compass_Rose(); 
         
     while (1) { 
              
-        TFT.set_font((unsigned char*) ArialR20x20);
-        //check if we recieved a new message from GPS, if so, attempt to parse it,
+                TFT.set_font((unsigned char*) ArialR20x20);
+                //check if we recieved a new message from GPS, if so, attempt to parse it,
         if (refresh_Timer.read_ms() >= refresh_Time) {
             refresh_Timer.reset();
                         
@@ -165,7 +171,7 @@
                         if (imu.chip_ready() == 0){
                             pc.printf("Bosch BNO055 is NOT avirable!!\r\n");
                 } else {
-                            printf("Cal %d", imu.read_calib_status());
+                            //printf("Cal %d", imu.read_calib_status());
                             if (imu.read_calib_status() > 0x0){
                                 TFT.foreground(White);
                                 TFT.locate(4, 260) ;
@@ -207,6 +213,9 @@
                 TFT.foreground(Blue);
                 TFT.locate(4, 2) ;
                 TFT.printf("%d-%d-%d", MyGPS.buffer.month, MyGPS.buffer.day, MyGPS.buffer.year);
+                                TFT.locate(4, 50) ;
+                                double waypoint = nav.CalculateDistance(MyGPS.buffer.latitude,MyGPS.buffer.longitude,plat,plon)/double(1609.344);
+                                TFT.printf("%.1fMI From Perry", waypoint);
                 TFT.locate(140, 2) ;
                 TFT.printf("%d:%d:%d", MyGPS.buffer.hours, MyGPS.buffer.minutes, MyGPS.buffer.seconds);
                                 TFT.locate(140, 260) ;
@@ -215,19 +224,24 @@
                                 int degree;
                                 int minutes;
                                 int seconds;
-                                degree = (int)MyGPS.buffer.longitude;
-                                minutes = (int) ( (MyGPS.buffer.longitude - (double)degree) * 60.0);
-                                seconds = (int) ( (MyGPS.buffer.longitude - (double)degree - (double)minutes / 60.0) * 60.0 * 60.0 );
-                TFT.printf("%d %d' %d\"lon", degree, minutes,seconds);
+                                degree = (int)abs(MyGPS.buffer.longitude);
+                minutes = (int) ( (abs(MyGPS.buffer.longitude) - (double)degree) * 60.0);
+                seconds = (int) ( (abs(MyGPS.buffer.longitude) - (double)degree - (double)minutes / 60.0) * 60.0 * 60.0 );
+                TFT.printf("%d %d' %d\" %c lon", degree, minutes,seconds, MyGPS.buffer.lonc);
                                 TFT.locate(4, 300) ;
-                                degree = (int)MyGPS.buffer.latitude;
-                                minutes = (int) ( (MyGPS.buffer.latitude - (double)degree) * 60.0);
-                                seconds = (int) ( (MyGPS.buffer.latitude - (double)degree - (double)minutes / 60.0) * 60.0 * 60.0 );
-                TFT.printf("%d %d' %d\"lat", degree, minutes,seconds);
+                                TFT.locate(4, 300) ;
+                degree = (int)abs(MyGPS.buffer.latitude);
+                minutes = (int) ( (abs(MyGPS.buffer.latitude) - (double)degree) * 60.0);
+                seconds = (int) ( (abs(MyGPS.buffer.latitude) - (double)degree - (double)minutes / 60.0) * 60.0 * 60.0 );
+                TFT.printf("%d %d' %d\" %c lat", degree, minutes,seconds, MyGPS.buffer.latc);
                                 TFT.set_font((unsigned char*) SCProSB31x55);     
                 TFT.locate(120, 200) ;
                 TFT.printf("%.1f", MyGPS.buffer.speed);    
                                 
+                                
+                                
+                                //pc.printf("Dist to Perry %.4f ", nav.CalculateDistance(MyGPS.buffer.latitude,MyGPS.buffer.longitude,plat,plon));
+                                
             }
             else {
                                 TFT.locate(4, 2) ;
@@ -256,10 +270,10 @@
   TFT.line(x3, y3, x4, y4, colour);
   TFT.line(x3, y3, x2, y2, colour);
   TFT.line(x2, y2, x4, y4, colour);
-  TFT.set_font((unsigned char*) Arial12x12);
+    TFT.set_font((unsigned char*) Arial12x12);
   TFT.foreground(Blue);
   TFT.background(White);
-  TFT.locate((centreX-2),(centreY-24));
+    TFT.locate((centreX-2),(centreY-24));
   TFT.printf("N");
   TFT.locate((centreX-2),(centreY+17));
   TFT.printf("S");
@@ -267,7 +281,7 @@
   TFT.printf("E");
   TFT.locate((centreX-23),(centreY-3));
   TFT.printf("W");
-  TFT.set_font((unsigned char*) ArialR20x20);
+    TFT.set_font((unsigned char*) ArialR20x20);
 } 
 
 void Draw_Compass_Rose(void) {