Rev 933 Rev 935
Line -... Line 1...
-   1 // GPS skrysohledac
-   2  
1 #include "gps.h" 3 #include "gps.h"
2   4  
3 #include <math.h> 5 #include <math.h>
4 #include <stdlib.h> 6 #include <stdlib.h>
5 #include <stdlibm.h> 7 #include <stdlibm.h>
6 //#include <string.h> -  
7   8  
8 #define LCD_RS PIN_B1 // rizeni registru LCD displeje 9 #define LCD_RS PIN_B1 // LCD control
9 #define LCD_E PIN_B0 // enable LCD displeje 10 #define LCD_E PIN_B0 // LCD enable
10 #define LCD_DATA_LSB PIN_B4 // pripojeni LSB bitu datoveho portu LCD displeje 11 #define LCD_DATA_LSB PIN_B4 // LSB data bit LCD
11 #include "MYLCD.C" 12 #include "MYLCD.C"
12   13  
13 // NMEA 14 // NMEA
14 #define TIME 6 15 #define TIME 6 // For NMEA parsing
15 #define TIME_LEN 10 16 #define TIME_LEN 10
16 #define LATDEG 19 17 #define LATDEG 19
17 #define LATMIN 21 18 #define LATMIN 21
18 #define LONDEG 32 19 #define LONDEG 32
19 #define LONMIN 34 20 #define LONMIN 34
20 #define DEG_LEN 2 21 #define DEG_LEN 2
21 #define MIN_LEN 7 22 #define MIN_LEN 7
22 #define AZIMUTH 46 23 #define AZIMUTH 46
23   24  
24 float lat,lon; 25 float lat,lon; // Latitude, Longitude, Azimuth
25 int16 az; 26 int16 az;
26   27  
-   28 #define NMEA_LINESIZE 72
-   29  
27 inline void read_NMEA() 30 inline void read_NMEA() // NMEA parsing
28 { 31 {
29 // auto char line[70]; -  
30 auto char item[16]; 32 auto char item[16];
31 char *ptr,*line; 33 char *ptr,*line;
32 auto int8 n; 34 auto int8 n;
33 auto char c; 35 auto char c;
34   36  
35 line=malloc(72); 37 line=malloc(NMEA_LINESIZE); // Space for one line
36 if (line==NULL) {printf(lcd_putc,"Error"); sleep();}; 38 if (line==NULL) {printf(lcd_putc,"Error"); sleep();};
37 line[71]=0; 39 line[NMEA_LINESIZE-1]=0;
38   40  
39 while(fgetc(NMEA)!='$'); 41 while(fgetc(NMEA)!='$'); // Waiting for start character
40 for(n=0;n<71;n++) 42 for(n=0;n<(NMEA_LINESIZE-1);n++) // Read line up to checksum
41 { 43 {
42 c=fgetc(NMEA); 44 c=fgetc(NMEA);
43 if(c=='*') 45 if(c=='*')
44 { 46 {
45 line[n]=0; 47 line[n]=0;
Line 54... Line 56...
54 lcd_gotoxy(1,3); 56 lcd_gotoxy(1,3);
55 strncpy(item,&line[LATDEG],DEG_LEN+MIN_LEN); item[DEG_LEN+MIN_LEN]=0; 57 strncpy(item,&line[LATDEG],DEG_LEN+MIN_LEN); item[DEG_LEN+MIN_LEN]=0;
56 printf(lcd_putc,"%s*",item); 58 printf(lcd_putc,"%s*",item);
57 strncpy(item,&line[LONDEG],DEG_LEN+MIN_LEN); item[DEG_LEN+MIN_LEN]=0; 59 strncpy(item,&line[LONDEG],DEG_LEN+MIN_LEN); item[DEG_LEN+MIN_LEN]=0;
58 printf(lcd_putc,"%s*",item); 60 printf(lcd_putc,"%s*",item);
59 lcd_gotoxy(1,4); -  
60 // strncpy(item,&line[AZIMUTH],6); item[6]=0; -  
61 // printf(lcd_putc,"%s*",item); -  
62   61  
63 lcd_gotoxy(1,2); 62 lcd_gotoxy(1,2);
64 lat=strtod(&line[LATMIN],&ptr); 63 lat=strtod(&line[LATMIN],&ptr);
65 line[LATMIN]=0; 64 line[LATMIN]=0;
66 lat+=60*strtod(&line[LATDEG],&ptr); 65 lat+=60*strtod(&line[LATDEG],&ptr);
Line 84... Line 83...
84 setup_timer_2(T2_DISABLED,0,1); 83 setup_timer_2(T2_DISABLED,0,1);
85 setup_comparator(NC_NC_NC_NC); 84 setup_comparator(NC_NC_NC_NC);
86 setup_vref(VREF_LOW|-2); 85 setup_vref(VREF_LOW|-2);
87 lcd_init(); 86 lcd_init();
88   87  
89 // gets(line,NMEA); // Dummy read form the GPS -  
90   -  
91   -  
92 while(TRUE) 88 while(TRUE)
93 { 89 {
94 const float gc_lat=49*60+15.574,gc_lon=14*60+42.331; // Cisticka 90 const float gc_lat=49*60+12.4452,gc_lon=14*60+18.9079; // Testovaci souradnice: Cisticka
95   91  
96 read_NMEA(); 92 float bearing, heading, azim;
-   93
-   94 read_NMEA(); // Read one NMEA line and parse
97   95  
98 lon=(gc_lon-lon)*1214; 96 lon=(gc_lon-lon)*1214; // Kakona's projection :)
99 lat=(gc_lat-lat)*1854; 97 lat=(gc_lat-lat)*1854;
100   98  
101 lcd_gotoxy(1,4); 99 lcd_gotoxy(1,4);
102 printf(lcd_putc,"%.0g* ",sqrt((lon*lon) + (lat*lat))); 100 printf(lcd_putc,"%.0g* ",sqrt((lon*lon) + (lat*lat))); // Distance
103 lcd_gotoxy(1,1); 101 lcd_gotoxy(1,1);
104 if (lat==0) lat=0.001; 102 if (lon==0) lon=0.001; // Divided by zero cure
105 lon=3-(6/PI*atan2(lat,lon)); 103 bearing=3-(6/PI*atan2(lat,lon)); // Bearing
106 if (lon<0) lon+=12; 104 if (bearing<0) bearing+=12; // overflow cure
107 if (lon>12) lon-=12; 105 if (bearing>12) bearing-=12;
108 printf(lcd_putc,"BE%2.0g*",lon); 106 printf(lcd_putc,"BE%2.0g*",bearing);
109 lat=12.0/360*az; 107 azim=12.0/360*az; // Azimuth of the movement
110 lon=lon-lat; 108 heading=bearing-lat; // Heading
111 if (lon<0) lon+=12; 109 if (heading<0) heading+=12; // overflow cure
112 if (lon>12) lon-=12; 110 if (heading>12) heading-=12;
113 printf(lcd_putc,"HE%2.0g*AZ%2.0g* ",lon,lat); 111 printf(lcd_putc,"HE%2.0g*AZ%2.0g* ",heading,azim);
114 } 112 }
115   -  
116 } 113 }