Raharja Agie
gps.h@0:d463d5c04541, 2011-08-16 (annotated)
- Committer:
- agiembed
- Date:
- Tue Aug 16 05:32:33 2011 +0000
- Revision:
- 0:d463d5c04541
Who changed what in which revision?
| User | Revision | Line number | New contents of line |
|---|---|---|---|
| agiembed | 0:d463d5c04541 | 1 | int gpscnt = 0; |
| agiembed | 0:d463d5c04541 | 2 | |
| agiembed | 0:d463d5c04541 | 3 | |
| agiembed | 0:d463d5c04541 | 4 | |
| agiembed | 0:d463d5c04541 | 5 | void gpscal() |
| agiembed | 0:d463d5c04541 | 6 | { |
| agiembed | 0:d463d5c04541 | 7 | |
| agiembed | 0:d463d5c04541 | 8 | if(gpscnt<10){ |
| agiembed | 0:d463d5c04541 | 9 | lats = gps.lat; //latt=37.541230; //37.540553; |
| agiembed | 0:d463d5c04541 | 10 | lons = gps.lon; // lont=127.079819; // 127.078038; |
| agiembed | 0:d463d5c04541 | 11 | gpscnt++; |
| agiembed | 0:d463d5c04541 | 12 | } |
| agiembed | 0:d463d5c04541 | 13 | |
| agiembed | 0:d463d5c04541 | 14 | latt = lats; //37.5409023;// |
| agiembed | 0:d463d5c04541 | 15 | lont = lons; //127.0800591;// |
| agiembed | 0:d463d5c04541 | 16 | |
| agiembed | 0:d463d5c04541 | 17 | latp = gps.lat; |
| agiembed | 0:d463d5c04541 | 18 | lonp = gps.lon; |
| agiembed | 0:d463d5c04541 | 19 | |
| agiembed | 0:d463d5c04541 | 20 | dlat = latp - latt; |
| agiembed | 0:d463d5c04541 | 21 | dlon = lonp - lont; |
| agiembed | 0:d463d5c04541 | 22 | |
| agiembed | 0:d463d5c04541 | 23 | //GPSASCII(); |
| agiembed | 0:d463d5c04541 | 24 | |
| agiembed | 0:d463d5c04541 | 25 | /* |
| agiembed | 0:d463d5c04541 | 26 | //jarak ke tujuan..... |
| agiembed | 0:d463d5c04541 | 27 | latp=latp*3.14/180; //ftoa(latp, 1, dst); putstr3(dst); putchar3('\n'); |
| agiembed | 0:d463d5c04541 | 28 | latt=latt*3.14/180; //ftoa(latt, 1, dst); putstr3(dst); putchar3('\n'); |
| agiembed | 0:d463d5c04541 | 29 | lonp=lonp*3.14/180; //ftoa(lonp, 1, dst); putstr3(dst); putchar3('\n'); |
| agiembed | 0:d463d5c04541 | 30 | lont=lont*3.14/180; //ftoa(lont, 1, dst); putstr3(dst); putchar3('\n'); |
| agiembed | 0:d463d5c04541 | 31 | d_lat=latp-latt; |
| agiembed | 0:d463d5c04541 | 32 | d_lon=lonp-lont; |
| agiembed | 0:d463d5c04541 | 33 | a=(sin(d_lat/2))*(sin(d_lat/2))+cos(latt)*cos(latp)*(sin(d_lon/2))*(sin(d_lon/2)); |
| agiembed | 0:d463d5c04541 | 34 | //c=2*atan2(sqrt(a),sqrt(1-a)); |
| agiembed | 0:d463d5c04541 | 35 | //d=r*c; |
| agiembed | 0:d463d5c04541 | 36 | c=2*asin(sqrt(a)); |
| agiembed | 0:d463d5c04541 | 37 | d=c*180*60*1000/3.14*1.852; // d = distance to target |
| agiembed | 0:d463d5c04541 | 38 | // if(d > 5000 || d < -5000) d = ld; |
| agiembed | 0:d463d5c04541 | 39 | gps.dist = d; |
| agiembed | 0:d463d5c04541 | 40 | |
| agiembed | 0:d463d5c04541 | 41 | //sudut hadap ke tujuan.... |
| agiembed | 0:d463d5c04541 | 42 | //b=atan2(sin(d_lon)*cos(latp), cos(latt)*sin(latp)-sin(latt)*cos(latp)*cos(d_lon)); |
| agiembed | 0:d463d5c04541 | 43 | b=(acos((sin(latt)-sin(latp)*cos(c))/(sin(c)*cos(latp))))*180/3.14; |
| agiembed | 0:d463d5c04541 | 44 | if (b==360)b=0; |
| agiembed | 0:d463d5c04541 | 45 | gps.bear = b; |
| agiembed | 0:d463d5c04541 | 46 | |
| agiembed | 0:d463d5c04541 | 47 | ld = d; |
| agiembed | 0:d463d5c04541 | 48 | GPSASCII(); |
| agiembed | 0:d463d5c04541 | 49 | */ |
| agiembed | 0:d463d5c04541 | 50 | |
| agiembed | 0:d463d5c04541 | 51 | } |