Jamal El Alaoui / GPS_U-blox_NEO-6M_Code

Jamal

Dependencies:   GPS MODSERIAL mbed-src

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

HdgDist.cpp@2:f10daa35eb51, 2015-04-02 (annotated)

Committer:
jelalaoui
Date:
Thu Apr 02 01:37:49 2015 +0000
Revision:
2:f10daa35eb51
Jamal

Who changed what in which revision?

UserRevisionLine numberNew contents of line
jelalaoui 2:f10daa35eb51 1 /*
jelalaoui 2:f10daa35eb51 2 * Notes: Firmware for GPS U-Blox NEO-6M
jelalaoui 2:f10daa35eb51 3 */
jelalaoui 2:f10daa35eb51 4
jelalaoui 2:f10daa35eb51 5 #include "main.h"
jelalaoui 2:f10daa35eb51 6
jelalaoui 2:f10daa35eb51 7
jelalaoui 2:f10daa35eb51 8 void Init()
jelalaoui 2:f10daa35eb51 9 {
jelalaoui 2:f10daa35eb51 10 gps.baud(9600);
jelalaoui 2:f10daa35eb51 11 pc.baud(115200);
jelalaoui 2:f10daa35eb51 12
jelalaoui 2:f10daa35eb51 13 pc.printf("Init OK\n");
jelalaoui 2:f10daa35eb51 14 }
jelalaoui 2:f10daa35eb51 15
jelalaoui 2:f10daa35eb51 16
jelalaoui 2:f10daa35eb51 17
jelalaoui 2:f10daa35eb51 18 int main()
jelalaoui 2:f10daa35eb51 19 {
jelalaoui 2:f10daa35eb51 20 Init();
jelalaoui 2:f10daa35eb51 21 char c;
jelalaoui 2:f10daa35eb51 22 float latitude;
jelalaoui 2:f10daa35eb51 23
jelalaoui 2:f10daa35eb51 24
jelalaoui 2:f10daa35eb51 25 while(true)
jelalaoui 2:f10daa35eb51 26 {
jelalaoui 2:f10daa35eb51 27 if(gps.readable())
jelalaoui 2:f10daa35eb51 28 {
jelalaoui 2:f10daa35eb51 29 if(gps.getc() == '$'); // wait a $
jelalaoui 2:f10daa35eb51 30 {
jelalaoui 2:f10daa35eb51 31 for(int i=0; i<sizeof(cDataBuffer); i++)
jelalaoui 2:f10daa35eb51 32 {
jelalaoui 2:f10daa35eb51 33 c = gps.getc();
jelalaoui 2:f10daa35eb51 34 if( c == '\r' )
jelalaoui 2:f10daa35eb51 35 {
jelalaoui 2:f10daa35eb51 36 //pc.printf("%s\n", cDataBuffer);
jelalaoui 2:f10daa35eb51 37 parse(cDataBuffer, i);
jelalaoui 2:f10daa35eb51 38 i = sizeof(cDataBuffer);
jelalaoui 2:f10daa35eb51 39
jelalaoui 2:f10daa35eb51 40
jelalaoui 2:f10daa35eb51 41
jelalaoui 2:f10daa35eb51 42 }
jelalaoui 2:f10daa35eb51 43
jelalaoui 2:f10daa35eb51 44
jelalaoui 2:f10daa35eb51 45 else
jelalaoui 2:f10daa35eb51 46 {
jelalaoui 2:f10daa35eb51 47 cDataBuffer[i] = c;
jelalaoui 2:f10daa35eb51 48 }
jelalaoui 2:f10daa35eb51 49 }
jelalaoui 2:f10daa35eb51 50 }
jelalaoui 2:f10daa35eb51 51 }
jelalaoui 2:f10daa35eb51 52 }
jelalaoui 2:f10daa35eb51 53 }
jelalaoui 2:f10daa35eb51 54
jelalaoui 2:f10daa35eb51 55
jelalaoui 2:f10daa35eb51 56 void parse(char *cmd, int n)
jelalaoui 2:f10daa35eb51 57 {
jelalaoui 2:f10daa35eb51 58
jelalaoui 2:f10daa35eb51 59 char ns, ew, tf, status;
jelalaoui 2:f10daa35eb51 60 int fq, nst, fix, date; // fix quality, Number of satellites being tracked, 3D fix
jelalaoui 2:f10daa35eb51 61 float timefix, speed, altitude,Lat, Lon;
jelalaoui 2:f10daa35eb51 62 float lat1,lat2,lon1,lon2,lats,lons,flat1, flon1,losx,losy,los,dist_calc;
jelalaoui 2:f10daa35eb51 63 float latitude, longitude;
jelalaoui 2:f10daa35eb51 64
jelalaoui 2:f10daa35eb51 65
jelalaoui 2:f10daa35eb51 66
jelalaoui 2:f10daa35eb51 67 // Global Positioning System Fix Data
jelalaoui 2:f10daa35eb51 68 if(strncmp(cmd,"$GPGGA", 6) == 0)
jelalaoui 2:f10daa35eb51 69 {
jelalaoui 2:f10daa35eb51 70 sscanf(cmd, "$GPGGA,%f,%f,%c,%f,%c,%d,%d,%*f,%f", &timefix, &latitude, &ns, &longitude, &ew, &fq, &nst, &altitude);
jelalaoui 2:f10daa35eb51 71 //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);
jelalaoui 2:f10daa35eb51 72 //pc.printf("latitude: %f, longitude: %f", latitude, longitude);
jelalaoui 2:f10daa35eb51 73
jelalaoui 2:f10daa35eb51 74 float value=latitude;
jelalaoui 2:f10daa35eb51 75
jelalaoui 2:f10daa35eb51 76 //string decimalToDMS(double value)
jelalaoui 2:f10daa35eb51 77 //string result = null;
jelalaoui 2:f10daa35eb51 78 double degValue = value / 100;
jelalaoui 2:f10daa35eb51 79 int degrees = (int) degValue;
jelalaoui 2:f10daa35eb51 80 double decMinutesSeconds = ((degValue - degrees)) / .60;
jelalaoui 2:f10daa35eb51 81 //double minuteValue = decMinutesSeconds * 60;
jelalaoui 2:f10daa35eb51 82 //int minutes = (int) minuteValue;
jelalaoui 2:f10daa35eb51 83 //double secsValue = (minuteValue - minutes) * 60;
jelalaoui 2:f10daa35eb51 84
jelalaoui 2:f10daa35eb51 85 flat1 = degrees + decMinutesSeconds ;
jelalaoui 2:f10daa35eb51 86 char np=ew;
jelalaoui 2:f10daa35eb51 87 if(np== 'E')
jelalaoui 2:f10daa35eb51 88 {
jelalaoui 2:f10daa35eb51 89 flat1*=-1;
jelalaoui 2:f10daa35eb51 90 }
jelalaoui 2:f10daa35eb51 91
jelalaoui 2:f10daa35eb51 92 //return result;
jelalaoui 2:f10daa35eb51 93 value=longitude;
jelalaoui 2:f10daa35eb51 94 //string decimalToDMS(double value)
jelalaoui 2:f10daa35eb51 95 //string result = null;
jelalaoui 2:f10daa35eb51 96 degValue = value / 100;
jelalaoui 2:f10daa35eb51 97 degrees = (int) degValue;
jelalaoui 2:f10daa35eb51 98 decMinutesSeconds = ((degValue - degrees)) / .60;
jelalaoui 2:f10daa35eb51 99 //minuteValue = decMinutesSeconds * 60;
jelalaoui 2:f10daa35eb51 100 //minutes = (int) minuteValue;
jelalaoui 2:f10daa35eb51 101 //secsValue = (minuteValue - minutes) * 60;
jelalaoui 2:f10daa35eb51 102 flon1 = degrees + decMinutesSeconds ;
jelalaoui 2:f10daa35eb51 103 char pn=ns;
jelalaoui 2:f10daa35eb51 104 if(pn== 'N'){
jelalaoui 2:f10daa35eb51 105 flon1*=-1;
jelalaoui 2:f10daa35eb51 106 }
jelalaoui 2:f10daa35eb51 107 pc.printf("Current Lat in Deg: %f Current Lon in Deg: %f \r\n\n\n", flat1,flon1);
jelalaoui 2:f10daa35eb51 108
jelalaoui 2:f10daa35eb51 109 //Distance and bearing calculation
jelalaoui 2:f10daa35eb51 110
jelalaoui 2:f10daa35eb51 111
jelalaoui 2:f10daa35eb51 112 //float flat1=flat; // flat1 = our current latitude. flat is from the gps data.
jelalaoui 2:f10daa35eb51 113 //float flon1=flon; // flon1 = our current longitude. flon is from the fps data.
jelalaoui 2:f10daa35eb51 114 double dist_calc=0;
jelalaoui 2:f10daa35eb51 115 double angle_calc=0;
jelalaoui 2:f10daa35eb51 116 double dist_calc2=0;
jelalaoui 2:f10daa35eb51 117 double diflat=0;
jelalaoui 2:f10daa35eb51 118 double diflon=0;
jelalaoui 2:f10daa35eb51 119 double heading=0;
jelalaoui 2:f10daa35eb51 120 double flat1_rad;
jelalaoui 2:f10daa35eb51 121 double x2lat_rad;
jelalaoui 2:f10daa35eb51 122 double PiRad = 3.1415926535897931/0.180;
jelalaoui 2:f10daa35eb51 123 double x2lat= -29.681411 ; // latitude point of charging platform
jelalaoui 2:f10daa35eb51 124 double x2lon= -95.251111 ; // longitude point of charging platform
jelalaoui 2:f10daa35eb51 125
jelalaoui 2:f10daa35eb51 126 pc.printf("Charger Lat in Deg: %f Charger Lon in Deg: %f \r\n\n\n\n\n", x2lat,x2lon);
jelalaoui 2:f10daa35eb51 127
jelalaoui 2:f10daa35eb51 128 //- distance formula below. Calculates distance to charging platform (1 nautical mile = 1.85 km)
jelalaoui 2:f10daa35eb51 129 ///Pythagorean error did not seem negligible to me
jelalaoui 2:f10daa35eb51 130 // losy = (x2lat - flat1);
jelalaoui 2:f10daa35eb51 131 // losx = (x2lon - flon1);
jelalaoui 2:f10daa35eb51 132
jelalaoui 2:f10daa35eb51 133
jelalaoui 2:f10daa35eb51 134 //if (EXTEND_RANGE)
jelalaoui 2:f10daa35eb51 135 // if (losy > 320000 || losx > 320000) {
jelalaoui 2:f10daa35eb51 136 // losy/=100;
jelalaoui 2:f10daa35eb51 137 //losx/=100;
jelalaoui 2:f10daa35eb51 138 // los=sqrt((losy*losy)+(losx*losx))*100;
jelalaoui 2:f10daa35eb51 139 // losy*=100;
jelalaoui 2:f10daa35eb51 140 // losx*=100;
jelalaoui 2:f10daa35eb51 141 //}
jelalaoui 2:f10daa35eb51 142
jelalaoui 2:f10daa35eb51 143 //else {
jelalaoui 2:f10daa35eb51 144 // los=sqrt((losy*losy)+(losx*losx));
jelalaoui 2:f10daa35eb51 145 // }
jelalaoui 2:f10daa35eb51 146
jelalaoui 2:f10daa35eb51 147 // los*= 110567 ; //Converting to meters
jelalaoui 2:f10daa35eb51 148 //great circle distance
jelalaoui 2:f10daa35eb51 149
jelalaoui 2:f10daa35eb51 150 //// function below. Calculates distance from current location to waypoint
jelalaoui 2:f10daa35eb51 151 diflat= (x2lat-flat1)* PiRad/1000; // in radians
jelalaoui 2:f10daa35eb51 152 flat1_rad= flat1* PiRad/1000; //current latitude to radians
jelalaoui 2:f10daa35eb51 153 x2lat_rad= x2lat* PiRad/1000; // waypoint latitude to radians
jelalaoui 2:f10daa35eb51 154 diflon=(x2lon-flon1)* PiRad/1000; //subtract and convert longitudes to radians
jelalaoui 2:f10daa35eb51 155 dist_calc = (sin(diflat/2.0)*sin(diflat/2.0));
jelalaoui 2:f10daa35eb51 156 dist_calc2= cos(flat1_rad);
jelalaoui 2:f10daa35eb51 157 dist_calc2*=cos(x2lat_rad);
jelalaoui 2:f10daa35eb51 158 dist_calc2*=sin(diflon/2.0);
jelalaoui 2:f10daa35eb51 159 dist_calc2*=sin(diflon/2.0);
jelalaoui 2:f10daa35eb51 160 dist_calc +=dist_calc2;
jelalaoui 2:f10daa35eb51 161 dist_calc=(2*atan2(sqrt(dist_calc),sqrt(1.0-dist_calc)));
jelalaoui 2:f10daa35eb51 162 dist_calc*=6371000.0; //Converting to meters
jelalaoui 2:f10daa35eb51 163
jelalaoui 2:f10daa35eb51 164
jelalaoui 2:f10daa35eb51 165
jelalaoui 2:f10daa35eb51 166
jelalaoui 2:f10daa35eb51 167
jelalaoui 2:f10daa35eb51 168 //dist_calc=sqrt((((flon1)-(x2lon))*((flon1)-(x2lon)))+(((x2lat-flat1)*(x2lat-flat1))));
jelalaoui 2:f10daa35eb51 169 //dist_calc*=110567 ; //Converting to meters
jelalaoui 2:f10daa35eb51 170 // pc.printf("Dest Lat in Deg: %f, Dest Lon in Deg: %f \r\n\n", x2lat,x2lon);
jelalaoui 2:f10daa35eb51 171 pc.printf("Distance to waypoint : %f m\r\n\n",dist_calc);
jelalaoui 2:f10daa35eb51 172 //=======================angle==========================================
jelalaoui 2:f10daa35eb51 173
jelalaoui 2:f10daa35eb51 174 heading = atan2((sin(diflon)*cos(x2lat_rad)),((cos(flat1_rad)*sin(x2lat_rad))-(sin(flat1_rad)*cos(x2lat_rad)*cos(diflon))));
jelalaoui 2:f10daa35eb51 175 heading = heading*180/3.1415926535897931; // convert from radians to degrees
jelalaoui 2:f10daa35eb51 176 int head =heading; //make it a integer now
jelalaoui 2:f10daa35eb51 177 if(head<0){
jelalaoui 2:f10daa35eb51 178 heading+=360; //if the heading is negative then add 360 to make it positive
jelalaoui 2:f10daa35eb51 179 }
jelalaoui 2:f10daa35eb51 180
jelalaoui 2:f10daa35eb51 181 // angle_calc=atan2(diflon,diflat);
jelalaoui 2:f10daa35eb51 182
jelalaoui 2:f10daa35eb51 183
jelalaoui 2:f10daa35eb51 184
jelalaoui 2:f10daa35eb51 185 //float declinationAngle2 = 57.29577951;
jelalaoui 2:f10daa35eb51 186 //angle_calc*= declinationAngle2;
jelalaoui 2:f10daa35eb51 187 //feedgps();
jelalaoui 2:f10daa35eb51 188 //getDistance();
jelalaoui 2:f10daa35eb51 189
jelalaoui 2:f10daa35eb51 190 // if(angle_calc < 0){
jelalaoui 2:f10daa35eb51 191 // angle_calc = 360 + angle_calc;
jelalaoui 2:f10daa35eb51 192 //feedgps();
jelalaoui 2:f10daa35eb51 193 //getDistance();
jelalaoui 2:f10daa35eb51 194 //}
jelalaoui 2:f10daa35eb51 195 // Check for wrap due to addition of declination.
jelalaoui 2:f10daa35eb51 196 //if(angle_calc >0){
jelalaoui 2:f10daa35eb51 197 //angle_calc= angle_calc;
jelalaoui 2:f10daa35eb51 198 //feedgps();
jelalaoui 2:f10daa35eb51 199 //getDistance();
jelalaoui 2:f10daa35eb51 200
jelalaoui 2:f10daa35eb51 201 pc.printf("Bearing angle between current and dest waypoint: %f deg\r\n\n", heading);
jelalaoui 2:f10daa35eb51 202 pc.printf("\n\n\======================================================================\r\n\n");
jelalaoui 2:f10daa35eb51 203
jelalaoui 2:f10daa35eb51 204 }
jelalaoui 2:f10daa35eb51 205
jelalaoui 2:f10daa35eb51 206
jelalaoui 2:f10daa35eb51 207 }
jelalaoui 2:f10daa35eb51 208
jelalaoui 2:f10daa35eb51 209 /*dist_calc=sqrt((((flon1)-(x2lon))*((flon1)-(x2lon)))+(((x2lat-flat1)*(x2lat-flat1))));
jelalaoui 2:f10daa35eb51 210 dist_calc*=110567 ; //Converting to meters
jelalaoui 2:f10daa35eb51 211 //=======================angle==========================================
jelalaoui 2:f10daa35eb51 212 angle_calc=atan2((x2lon-flon1),(x2lat-flat1));
jelalaoui 2:f10daa35eb51 213
jelalaoui 2:f10daa35eb51 214 float declinationAngle2 = 57.29577951;
jelalaoui 2:f10daa35eb51 215 angle_calc*= declinationAngle2;
jelalaoui 2:f10daa35eb51 216 feedgps();
jelalaoui 2:f10daa35eb51 217 getDistance();
jelalaoui 2:f10daa35eb51 218
jelalaoui 2:f10daa35eb51 219 if(angle_calc < 0){
jelalaoui 2:f10daa35eb51 220 angle_calc = 360 + angle_calc;
jelalaoui 2:f10daa35eb51 221 feedgps();
jelalaoui 2:f10daa35eb51 222 getDistance();
jelalaoui 2:f10daa35eb51 223 }
jelalaoui 2:f10daa35eb51 224 // Check for wrap due to addition of declination.
jelalaoui 2:f10daa35eb51 225 if(angle_calc >0){
jelalaoui 2:f10daa35eb51 226 angle_calc= angle_calc;
jelalaoui 2:f10daa35eb51 227 feedgps();
jelalaoui 2:f10daa35eb51 228 getDistance();
jelalaoui 2:f10daa35eb51 229 }
jelalaoui 2:f10daa35eb51 230
jelalaoui 2:f10daa35eb51 231 float angleDegrees = angle_calc;
jelalaoui 2:f10daa35eb51 232 feedgps();
jelalaoui 2:f10daa35eb51 233
jelalaoui 2:f10daa35eb51 234
jelalaoui 2:f10daa35eb51 235
jelalaoui 2:f10daa35eb51 236 // Satellite status
jelalaoui 2:f10daa35eb51 237 //if(strncmp(cmd,"$GPGSA", 6) == 0)
jelalaoui 2:f10daa35eb51 238 /*{
jelalaoui 2:f10daa35eb51 239 sscanf(cmd, "$GPGSA,%c,%d,%d", &tf, &fix, &nst);
jelalaoui 2:f10daa35eb51 240 pc.printf("GPGSA Type fix: %c, 3D fix: %d, number of sat: %d\r\n", tf, fix, nst);
jelalaoui 2:f10daa35eb51 241 }
jelalaoui 2:f10daa35eb51 242
jelalaoui 2:f10daa35eb51 243 // Geographic position, Latitude and Longitude
jelalaoui 2:f10daa35eb51 244 if(strncmp(cmd,"$GPGLL", 6) == 0)
jelalaoui 2:f10daa35eb51 245 {
jelalaoui 2:f10daa35eb51 246 sscanf(cmd, "$GPGLL,%f,%c,%f,%c,%f", &latitude, &ns, &longitude, &ew, &timefix);
jelalaoui 2:f10daa35eb51 247 pc.printf("GPGLL Latitude: %f %c, Longitude: %f %c, Fix taken at: %f\n", latitude, ns, longitude, ew, timefix);
jelalaoui 2:f10daa35eb51 248 }
jelalaoui 2:f10daa35eb51 249
jelalaoui 2:f10daa35eb51 250 // Geographic position, Latitude and Longitude
jelalaoui 2:f10daa35eb51 251 if(strncmp(cmd,"$GPRMC", 6) == 0)
jelalaoui 2:f10daa35eb51 252 {
jelalaoui 2:f10daa35eb51 253 sscanf(cmd, "$GPRMC,%f,%c,%f,%c,%f,%c,%f,,%d", &timefix, &status, &latitude, &ns, &longitude, &ew, &speed, &date);
jelalaoui 2:f10daa35eb51 254 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);
jelalaoui 2:f10daa35eb51 255 }
jelalaoui 2:f10daa35eb51 256 */