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