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