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