Jamal
Dependencies: GPS MODSERIAL mbed-src
Fork of GPS_U-blox_NEO-6M_Test_Code by
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?
User | Revision | Line number | New 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 | */ |