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