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