Rev 936 Rev 958
Line 1... Line 1...
1 /**** GPS skrysohledac ****/ 1 /**** GPS skrysohledac ****/
2 #define ID "$Id: gps.c 936 2008-01-02 22:01:37Z kakl $" 2 #define ID "$Id: gps.c 958 2008-01-06 12:42:53Z kakl $"
3   3  
4 #include "gps.h" 4 #include "gps.h"
5   5  
6 #include <math.h> 6 #include <math.h>
7 #include <stdlib.h> 7 #include <stdlib.h>
8 #include <stdlibm.h> 8 #include <stdlibm.h>
9   9  
10 #define LCD_RS PIN_B1 // LCD control 10 #define LCD_RS PIN_B1 // LCD control
11 #define LCD_E PIN_B0 // LCD enable 11 #define LCD_E PIN_B2 // LCD enable
12 #define LCD_DATA_LSB PIN_B4 // LSB data bit LCD 12 #define LCD_DATA_LSB PIN_B4 // LSB data bit LCD
13 #include "MYLCD.C" 13 #include "MYLCD.C"
14   14  
15 // NMEA 15 // NMEA
16 #define TIME 6 // For NMEA parsing 16 #define TIME 6 // For NMEA parsing
17 #define TIME_LEN 10 17 #define TIME_LEN 10
18 #define LATDEG 19 18 #define LATDEG 19
19 #define LATMIN 21 19 #define LATMIN 21
20 #define LONDEG 32 20 #define LONDEG 32
21 #define LONMIN 34 21 #define LONMIN 34
22 #define DEG_LEN 2 22 #define DEG_LEN 2
23 #define MIN_LEN 7 23 #define MIN_LEN 7
24 #define AZIMUTH 46 24 #define AZIMUTH 46
-   25 #define NMEA_LINESIZE 60
-   26  
-   27 #define MAXGC 4
-   28  
-   29 struct geocache
-   30 {
-   31 float lat;
-   32 float lon;
-   33 char name[8];
-   34 };
-   35  
-   36 struct geocache const gc[MAXGC] =
-   37 {
-   38 {49*60+15.5740,14*60+42.3310,"GCCISTI"}, // Testovaci souradnice: cisticka
-   39 {49*60+12.4452,14*60+18.9079,"GCKOMET"}, // Testovaci souradnice: kometa
-   40 {49*60+12.4452,14*60+18.9079,"GCXXX3"},
-   41 {49*60+12.4452,14*60+18.9079,"GCXXX4"}
-   42 };
25   43  
26 float lat,lon; // Latitude, Longitude, Azimuth 44 float lat,lon; // Latitude, Longitude, Azimuth
27 int16 az; 45 int16 az;
-   46 int8 gcnum; // Selection of the GC point
28   47  
29 #define NMEA_LINESIZE 72 48 //$GPRMC,105815.503,V,4915.5877,N,01442.3896,E,0.0,0.00,060108,,,N*44
30   -  
31 inline void read_NMEA() // NMEA parsing 49 inline void read_NMEA() // NMEA parsing
32 { 50 {
-   51 auto char line[NMEA_LINESIZE];
33 auto char item[16]; 52 auto char item[16];
-   53 auto char *ptr;
34 char *ptr,*line; 54 // auto char *line;
35 auto int8 n; 55 auto int8 n;
36 auto char c; 56 auto char c;
37   57  
38 line=malloc(NMEA_LINESIZE); // Space for one line 58 // line=malloc(NMEA_LINESIZE); // Space for one line
39 if (line==NULL) {printf(lcd_putc,"Error"); sleep();}; 59 // if (line==NULL) {printf(lcd_putc,"Error"); sleep();};
40 line[NMEA_LINESIZE-1]=0; 60 line[NMEA_LINESIZE-1]=0;
41   61  
42 while(fgetc(NMEA)!='$'); // Waiting for start character 62 while(fgetc(NMEA)!='$'); // Waiting for start character
43 for(n=0;n<(NMEA_LINESIZE-1);n++) // Read line up to checksum 63 for(n=0;n<(NMEA_LINESIZE-1);n++) // Read line up to checksum
44 { 64 {
Line 49... Line 69...
49 break; 69 break;
50 }; 70 };
51 line[n]=c; 71 line[n]=c;
52 }; 72 };
53   73  
54 lcd_gotoxy(12,4); 74 lcd_gotoxy(12,4);
55 printf(lcd_putc,"%c %06.0g ",line[17],strtod(&line[TIME],&ptr)); 75 printf(lcd_putc,"%c %06.0g ",line[17],strtod(&line[TIME],&ptr));
56   76  
57 lcd_gotoxy(1,3); 77 lcd_gotoxy(1,3);
58 strncpy(item,&line[LATDEG],DEG_LEN+MIN_LEN); item[DEG_LEN+MIN_LEN]=0; 78 strncpy(item,&line[LATDEG],DEG_LEN+MIN_LEN); item[DEG_LEN+MIN_LEN]=0;
59 printf(lcd_putc,"%s*",item); 79 printf(lcd_putc,"%s*",item);
60 strncpy(item,&line[LONDEG],DEG_LEN+MIN_LEN); item[DEG_LEN+MIN_LEN]=0; 80 strncpy(item,&line[LONDEG],DEG_LEN+MIN_LEN); item[DEG_LEN+MIN_LEN]=0;
61 printf(lcd_putc,"%s*",item); 81 printf(lcd_putc,"%s*",item);
62   82  
63 lcd_gotoxy(1,2); 83 // lcd_gotoxy(1,2);
64 lat=strtod(&line[LATMIN],&ptr); 84 lat=strtod(&line[LATMIN],&ptr);
65 line[LATMIN]=0; 85 line[LATMIN]=0;
66 lat+=60*strtod(&line[LATDEG],&ptr); 86 lat+=60*strtod(&line[LATDEG],&ptr);
67 lon=strtod(&line[LONMIN],&ptr); 87 lon=strtod(&line[LONMIN],&ptr);
68 line[LONMIN]=0; 88 line[LONMIN]=0;
69 lon+=60*strtod(&line[LONDEG],&ptr); 89 lon+=60*strtod(&line[LONDEG],&ptr);
70 strtod(&line[AZIMUTH],&ptr); 90 strtod(&line[AZIMUTH],&ptr);
71 az=strtoul(ptr+1,&ptr,10); 91 az=strtoul(ptr+1,&ptr,10);
72 printf(lcd_putc,"%.3g*%.3g*%03Lu",lat,lon,az); 92 // printf(lcd_putc,"%.3g*%.3g*%03Lu",lat,lon,az);
73 free(line); 93 // free(line);
74 } 94 }
75   95  
76 void main() 96 void main()
77 { 97 {
78   98  
Line 84... Line 104...
84 setup_timer_2(T2_DISABLED,0,1); 104 setup_timer_2(T2_DISABLED,0,1);
85 setup_comparator(NC_NC_NC_NC); 105 setup_comparator(NC_NC_NC_NC);
86 setup_vref(VREF_LOW|-2); 106 setup_vref(VREF_LOW|-2);
87 lcd_init(); 107 lcd_init();
88   108  
89 while(TRUE) 109 gcnum++; // Next GC
90 { -  
91 const float gc_lat=49*60+12.4452,gc_lon=14*60+18.9079; // Testovaci souradnice: Cisticka 110 if (gcnum>(MAXGC-1)) gcnum=0;
92   111  
-   112 delay_ms(200);
-   113 lcd_gotoxy(1,2);
93 float bearing, heading, azim; 114 printf(lcd_putc,"%s",gc[gcnum].name); // Print GC name
94   115  
-   116 while(TRUE)
-   117 {
95 read_NMEA(); // Read one NMEA line and parse 118 read_NMEA(); // Read one NMEA line and parse
96   119  
97 lon=(gc_lon-lon)*1214; // Kakona's projection :) 120 lon=(gc[gcnum].lon-lon)*1214; // Kakona's projection :)
98 lat=(gc_lat-lat)*1854; 121 lat=(gc[gcnum].lat-lat)*1854;
99   122  
100 lcd_gotoxy(1,4); 123 lcd_gotoxy(1,4);
101 printf(lcd_putc,"%.0g* ",sqrt((lon*lon) + (lat*lat))); // Distance 124 printf(lcd_putc,"%.0g* ",sqrt((lon*lon) + (lat*lat))); // Distance
102 lcd_gotoxy(1,1); 125 lcd_gotoxy(1,1);
103 if (lon==0) lon=0.001; // Divided by zero cure 126 if (lon==0) lon=0.001; // Divided by zero cure
104 bearing=3-(6/PI*atan2(lat,lon)); // Bearing 127 lon=3-(6/PI*atan2(lat,lon)); // Bearing
105 if (bearing<0) bearing+=12; // overflow cure 128 if (lon<0) lon+=12; // overflow cure
106 if (bearing>12) bearing-=12; 129 if (lon>12) lon-=12;
107 printf(lcd_putc,"BE%2.0g*",bearing); 130 printf(lcd_putc,"BE%2.0g*",lon);
108 azim=12.0/360*az; // Azimuth of the movement 131 lat=12.0/360*az; // Azimuth of the movement
109 heading=bearing-lat; // Heading 132 lon=lon-lat; // Heading
110 if (heading<0) heading+=12; // overflow cure 133 if (lon<0) lon+=12; // overflow cure
111 if (heading>12) heading-=12; 134 if (lon>12) lon-=12;
112 printf(lcd_putc,"HE%2.0g*AZ%2.0g* ",heading,azim); 135 printf(lcd_putc,"HE%2.0g*AZ%2.0g* ",lon,lat);
113 } 136 }
114 } 137 }
-   138