Diff: HdgDist.cpp

Revision:

2:f10daa35eb51

--- /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);
+    }
+*/