Jamal

Dependencies:   GPS MODSERIAL mbed-src

Fork of GPS_U-blox_NEO-6M_Test_Code by Edoardo De Marchi

Files at this revision

API Documentation at this revision

Comitter:
jelalaoui
Date:
Thu Apr 02 01:37:49 2015 +0000
Parent:
1:acd907fbcbae
Commit message:
Jamal

Changed in this revision

GPS.lib Show annotated file Show diff for this revision Revisions of this file
HdgDist.cpp Show annotated file Show diff for this revision Revisions of this file
HdgDist.h Show annotated file Show diff for this revision Revisions of this file
main.cpp Show diff for this revision Revisions of this file
main.h Show diff for this revision Revisions of this file
diff -r acd907fbcbae -r f10daa35eb51 GPS.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/GPS.lib	Thu Apr 02 01:37:49 2015 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/chris215/code/GPS/#0c1aa5906cef
diff -r acd907fbcbae -r f10daa35eb51 HdgDist.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/HdgDist.cpp	Thu Apr 02 01:37:49 2015 +0000
@@ -0,0 +1,256 @@
+/*
+ * Notes: Firmware for GPS U-Blox NEO-6M
+*/
+
+#include "main.h"
+
+
+void Init()
+{
+    gps.baud(9600);
+    pc.baud(115200);
+
+    pc.printf("Init OK\n");
+}
+
+
+
+int main() 
+{   
+    Init();
+    char c;
+    float latitude;
+
+
+    while(true) 
+    {
+        if(gps.readable())
+        { 
+            if(gps.getc() == '$');           // wait a $
+            {
+                for(int i=0; i<sizeof(cDataBuffer); i++)
+                {
+                    c = gps.getc();
+                    if( c == '\r' )
+                    {
+                        //pc.printf("%s\n", cDataBuffer);
+                        parse(cDataBuffer, i);
+                        i = sizeof(cDataBuffer);
+                        
+                        
+            
+                    }
+                    
+                        
+                    else
+                    {
+                        cDataBuffer[i] = c;
+                    }                 
+                }
+            }
+         } 
+    }
+}
+
+
+void parse(char *cmd, int n)
+{
+    
+    char ns, ew, tf, status;
+    int fq, nst, fix, date;                                     // fix quality, Number of satellites being tracked, 3D fix
+    float timefix, speed, altitude,Lat, Lon;
+    float lat1,lat2,lon1,lon2,lats,lons,flat1, flon1,losx,losy,los,dist_calc;
+    float latitude, longitude;
+   
+    
+    
+    // Global Positioning System Fix Data
+    if(strncmp(cmd,"$GPGGA", 6) == 0) 
+    {
+        sscanf(cmd, "$GPGGA,%f,%f,%c,%f,%c,%d,%d,%*f,%f", &timefix, &latitude, &ns, &longitude, &ew, &fq, &nst, &altitude);
+        //pc.printf("GPGGA Fix taken at: %f, Latitude: %f, %c, Longitude: %f %c, Fix quality: %d, Number of sat: %d, Altitude: %f M\n", timefix, latitude, ns, longitude, ew, fq, nst, altitude);
+        //pc.printf("latitude: %f, longitude: %f", latitude, longitude);
+        
+        float value=latitude;
+        
+        //string decimalToDMS(double value) 
+        //string result = null;
+        double degValue = value / 100;
+        int degrees = (int) degValue;
+        double decMinutesSeconds = ((degValue - degrees)) / .60;
+        //double minuteValue = decMinutesSeconds * 60;
+        //int minutes = (int) minuteValue;
+        //double secsValue = (minuteValue - minutes) * 60;
+        
+        flat1 = degrees + decMinutesSeconds ;
+        char np=ew;
+        if(np== 'E')
+           {
+            flat1*=-1;
+            }
+        
+        //return result;
+        value=longitude;
+        //string decimalToDMS(double value) 
+        //string result = null;
+        degValue = value / 100;
+        degrees = (int) degValue;
+        decMinutesSeconds = ((degValue - degrees)) / .60;
+        //minuteValue = decMinutesSeconds * 60;
+        //minutes = (int) minuteValue;
+        //secsValue = (minuteValue - minutes) * 60;
+        flon1 = degrees + decMinutesSeconds ;
+        char pn=ns;
+        if(pn== 'N'){
+            flon1*=-1;
+            }
+        pc.printf("Current Lat in Deg: %f       Current Lon in Deg: %f \r\n\n\n", flat1,flon1);
+        
+//Distance and bearing calculation
+
+
+        //float flat1=flat;     // flat1 = our current latitude. flat is from the gps data. 
+        //float flon1=flon;  // flon1 = our current longitude. flon is from the fps data.
+        double dist_calc=0;
+        double angle_calc=0;
+        double dist_calc2=0;
+        double diflat=0;
+        double diflon=0;
+        double heading=0;
+        double flat1_rad;
+        double x2lat_rad;
+        double PiRad = 3.1415926535897931/0.180;
+        double x2lat= -29.681411      ;  // latitude point of charging platform
+        double x2lon= -95.251111    ;  //   longitude point of charging platform
+       
+        pc.printf("Charger Lat in Deg: %f       Charger Lon in Deg: %f \r\n\n\n\n\n", x2lat,x2lon);
+  
+  //- distance formula below. Calculates distance to charging platform (1 nautical mile = 1.85 km)
+///Pythagorean error did not seem negligible to me
+     //   losy = (x2lat - flat1);
+       // losx = (x2lon - flon1);
+
+  
+  //if (EXTEND_RANGE)
+   // if (losy > 320000 || losx > 320000) {
+     // losy/=100;
+      //losx/=100; 
+     // los=sqrt((losy*losy)+(losx*losx))*100;
+    //  losy*=100;
+    //  losx*=100;
+    //}
+    
+    //else {
+    // los=sqrt((losy*losy)+(losx*losx)); 
+       //  }
+         
+     // los*= 110567 ; //Converting to meters 
+     //great circle distance
+     
+     //// function below. Calculates distance from current location to waypoint
+     diflat= (x2lat-flat1)* PiRad/1000;  // in radians
+     flat1_rad= flat1* PiRad/1000;    //current latitude to radians
+     x2lat_rad= x2lat* PiRad/1000;  // waypoint latitude to radians
+     diflon=(x2lon-flon1)* PiRad/1000;   //subtract and convert longitudes to radians
+     dist_calc = (sin(diflat/2.0)*sin(diflat/2.0));
+     dist_calc2= cos(flat1_rad);
+     dist_calc2*=cos(x2lat_rad);
+     dist_calc2*=sin(diflon/2.0);                                       
+     dist_calc2*=sin(diflon/2.0);
+     dist_calc +=dist_calc2;
+     dist_calc=(2*atan2(sqrt(dist_calc),sqrt(1.0-dist_calc)));
+     dist_calc*=6371000.0; //Converting to meters
+
+
+
+    
+
+        //dist_calc=sqrt((((flon1)-(x2lon))*((flon1)-(x2lon)))+(((x2lat-flat1)*(x2lat-flat1))));
+        //dist_calc*=110567 ; //Converting to meters
+      //  pc.printf("Dest Lat in Deg: %f,      Dest Lon in Deg: %f \r\n\n", x2lat,x2lon);        
+        pc.printf("Distance to waypoint : %f  m\r\n\n",dist_calc);
+  //=======================angle==========================================
+   
+heading = atan2((sin(diflon)*cos(x2lat_rad)),((cos(flat1_rad)*sin(x2lat_rad))-(sin(flat1_rad)*cos(x2lat_rad)*cos(diflon))));
+heading = heading*180/3.1415926535897931;  // convert from radians to degrees
+int head =heading; //make it a integer now
+if(head<0){
+ heading+=360;   //if the heading is negative then add 360 to make it positive
+}
+
+  //      angle_calc=atan2(diflon,diflat);
+        
+        
+
+        //float declinationAngle2 = 57.29577951;
+        //angle_calc*= declinationAngle2;
+        //feedgps();
+        //getDistance();
+
+       // if(angle_calc < 0){
+   // angle_calc = 360 + angle_calc;
+    //feedgps();
+    //getDistance();
+  //}
+  // Check for wrap due to addition of declination.
+  //if(angle_calc >0){
+    //angle_calc= angle_calc;
+    //feedgps();
+    //getDistance();
+          
+    pc.printf("Bearing angle between current and dest waypoint: %f deg\r\n\n", heading);
+    pc.printf("\n\n\======================================================================\r\n\n");
+        
+    }
+
+
+    }
+      
+/*dist_calc=sqrt((((flon1)-(x2lon))*((flon1)-(x2lon)))+(((x2lat-flat1)*(x2lat-flat1))));
+  dist_calc*=110567 ; //Converting to meters
+  //=======================angle==========================================
+  angle_calc=atan2((x2lon-flon1),(x2lat-flat1));
+
+  float declinationAngle2 = 57.29577951;
+  angle_calc*= declinationAngle2;
+  feedgps();
+  getDistance();
+
+  if(angle_calc < 0){
+    angle_calc = 360 + angle_calc;
+    feedgps();
+    getDistance();
+  }
+  // Check for wrap due to addition of declination.
+  if(angle_calc >0){
+    angle_calc= angle_calc;
+    feedgps();
+    getDistance();
+  }
+
+  float angleDegrees = angle_calc;
+  feedgps();
+
+
+    
+    // Satellite status
+    //if(strncmp(cmd,"$GPGSA", 6) == 0) 
+    /*{
+        sscanf(cmd, "$GPGSA,%c,%d,%d", &tf, &fix, &nst);
+        pc.printf("GPGSA Type fix: %c, 3D fix: %d, number of sat: %d\r\n", tf, fix, nst);
+    }
+    
+    // Geographic position, Latitude and Longitude
+    if(strncmp(cmd,"$GPGLL", 6) == 0) 
+    {
+        sscanf(cmd, "$GPGLL,%f,%c,%f,%c,%f", &latitude, &ns, &longitude, &ew, &timefix);
+        pc.printf("GPGLL Latitude: %f %c, Longitude: %f %c, Fix taken at: %f\n", latitude, ns, longitude, ew, timefix);
+    }
+    
+    // Geographic position, Latitude and Longitude
+    if(strncmp(cmd,"$GPRMC", 6) == 0) 
+    {
+        sscanf(cmd, "$GPRMC,%f,%c,%f,%c,%f,%c,%f,,%d", &timefix, &status, &latitude, &ns, &longitude, &ew, &speed, &date);
+        pc.printf("GPRMC Fix taken at: %f, Status: %c, Latitude: %f %c, Longitude: %f %c, Speed: %f, Date: %d\n", timefix, status, latitude, ns, longitude, ew, speed, date);
+    }
+*/
diff -r acd907fbcbae -r f10daa35eb51 HdgDist.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/HdgDist.h	Thu Apr 02 01:37:49 2015 +0000
@@ -0,0 +1,19 @@
+#pragma once 
+#include "mbed.h"
+#include "MODSERIAL.h"
+
+MODSERIAL pc(USBTX,USBRX);
+
+#if   defined(TARGET_LPC1768)
+MODSERIAL gps(p13, p14);
+#elif defined(TARGET_LPC4330_M4)
+MODSERIAL gps(UART0_TX, UART0_RX);
+#endif
+
+
+char cDataBuffer[500];
+int i = 0;
+
+
+void Init();
+void parse(char *cmd, int n);
diff -r acd907fbcbae -r f10daa35eb51 main.cpp
--- a/main.cpp	Fri Aug 22 12:43:55 2014 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,90 +0,0 @@
-/*
- * Author: Edoardo De Marchi
- * Date: 22-08-14
- * Notes: Firmware for GPS U-Blox NEO-6M
-*/
-
-#include "main.h"
-
-
-void Init()
-{
-    gps.baud(9600);
-    pc.baud(115200);
-
-    pc.printf("Init OK\n");
-}
-
-
-
-int main() 
-{   
-    Init();
-    char c;
-
-    while(true) 
-    {
-        if(gps.readable())
-        { 
-            if(gps.getc() == '$');           // wait a $
-            {
-                for(int i=0; i<sizeof(cDataBuffer); i++)
-                {
-                    c = gps.getc();
-                    if( c == '\r' )
-                    {
-                        //pc.printf("%s\n", cDataBuffer);
-                        parse(cDataBuffer, i);
-                        i = sizeof(cDataBuffer);
-                    }
-                    else
-                    {
-                        cDataBuffer[i] = c;
-                    }                 
-                }
-            }
-         } 
-    }
-}
-
-
-void parse(char *cmd, int n)
-{
-    
-    char ns, ew, tf, status;
-    int fq, nst, fix, date;                                     // fix quality, Number of satellites being tracked, 3D fix
-    float latitude, longitude, timefix, speed, altitude;
-    
-    
-    // Global Positioning System Fix Data
-    if(strncmp(cmd,"$GPGGA", 6) == 0) 
-    {
-        sscanf(cmd, "$GPGGA,%f,%f,%c,%f,%c,%d,%d,%*f,%f", &timefix, &latitude, &ns, &longitude, &ew, &fq, &nst, &altitude);
-        pc.printf("GPGGA Fix taken at: %f, Latitude: %f %c, Longitude: %f %c, Fix quality: %d, Number of sat: %d, Altitude: %f M\n", timefix, latitude, ns, longitude, ew, fq, nst, altitude);
-    }
-    
-    // Satellite status
-    if(strncmp(cmd,"$GPGSA", 6) == 0) 
-    {
-        sscanf(cmd, "$GPGSA,%c,%d,%d", &tf, &fix, &nst);
-        pc.printf("GPGSA Type fix: %c, 3D fix: %d, number of sat: %d\r\n", tf, fix, nst);
-    }
-    
-    // Geographic position, Latitude and Longitude
-    if(strncmp(cmd,"$GPGLL", 6) == 0) 
-    {
-        sscanf(cmd, "$GPGLL,%f,%c,%f,%c,%f", &latitude, &ns, &longitude, &ew, &timefix);
-        pc.printf("GPGLL Latitude: %f %c, Longitude: %f %c, Fix taken at: %f\n", latitude, ns, longitude, ew, timefix);
-    }
-    
-    // Geographic position, Latitude and Longitude
-    if(strncmp(cmd,"$GPRMC", 6) == 0) 
-    {
-        sscanf(cmd, "$GPRMC,%f,%c,%f,%c,%f,%c,%f,,%d", &timefix, &status, &latitude, &ns, &longitude, &ew, &speed, &date);
-        pc.printf("GPRMC Fix taken at: %f, Status: %c, Latitude: %f %c, Longitude: %f %c, Speed: %f, Date: %d\n", timefix, status, latitude, ns, longitude, ew, speed, date);
-    }
-}
-
-
-
-
diff -r acd907fbcbae -r f10daa35eb51 main.h
--- a/main.h	Fri Aug 22 12:43:55 2014 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,19 +0,0 @@
-#pragma once 
-#include "mbed.h"
-#include "MODSERIAL.h"
-
-MODSERIAL pc(USBTX,USBRX);
-
-#if   defined(TARGET_LPC1768)
-MODSERIAL gps(p13, p14);
-#elif defined(TARGET_LPC4330_M4)
-MODSERIAL gps(UART0_TX, UART0_RX);
-#endif
-
-
-char cDataBuffer[500];
-int i = 0;
-
-
-void Init();
-void parse(char *cmd, int n);