Rev 933 Rev 935
-   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;
46 break; 48 break;
47 }; 49 };
48 line[n]=c; 50 line[n]=c;
49 }; 51 };
50   52  
51 lcd_gotoxy(12,4); 53 lcd_gotoxy(12,4);
52 printf(lcd_putc,"%c %06.0g ",line[17],strtod(&line[TIME],&ptr)); 54 printf(lcd_putc,"%c %06.0g ",line[17],strtod(&line[TIME],&ptr));
53   55  
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);
67 lon=strtod(&line[LONMIN],&ptr); 66 lon=strtod(&line[LONMIN],&ptr);
68 line[LONMIN]=0; 67 line[LONMIN]=0;
69 lon+=60*strtod(&line[LONDEG],&ptr); 68 lon+=60*strtod(&line[LONDEG],&ptr);
70 strtod(&line[AZIMUTH],&ptr); 69 strtod(&line[AZIMUTH],&ptr);
71 az=strtoul(ptr+1,&ptr,10); 70 az=strtoul(ptr+1,&ptr,10);
72 printf(lcd_putc,"%.3g*%.3g*%03Lu",lat,lon,az); 71 printf(lcd_putc,"%.3g*%.3g*%03Lu",lat,lon,az);
73 free(line); 72 free(line);
74 } 73 }
75   74  
76 void main() 75 void main()
77 { 76 {
78   77  
79 setup_adc_ports(NO_ANALOGS); 78 setup_adc_ports(NO_ANALOGS);
80 setup_adc(ADC_OFF); 79 setup_adc(ADC_OFF);
81 setup_spi(FALSE); 80 setup_spi(FALSE);
82 setup_timer_0(RTCC_INTERNAL|RTCC_DIV_1); 81 setup_timer_0(RTCC_INTERNAL|RTCC_DIV_1);
83 setup_timer_1(T1_DISABLED); 82 setup_timer_1(T1_DISABLED);
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 }