File content as of revision 2:f10daa35eb51:

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