| 1,17 → 1,18 |
|---|
| // GPS skrysohledac |
| |
| #include "gps.h" |
| |
| #include <math.h> |
| #include <stdlib.h> |
| #include <stdlibm.h> |
| //#include <string.h> |
| |
| #define LCD_RS PIN_B1 // rizeni registru LCD displeje |
| #define LCD_E PIN_B0 // enable LCD displeje |
| #define LCD_DATA_LSB PIN_B4 // pripojeni LSB bitu datoveho portu LCD displeje |
| #define LCD_RS PIN_B1 // LCD control |
| #define LCD_E PIN_B0 // LCD enable |
| #define LCD_DATA_LSB PIN_B4 // LSB data bit LCD |
| #include "MYLCD.C" |
| |
| // NMEA |
| #define TIME 6 |
| #define TIME 6 // For NMEA parsing |
| #define TIME_LEN 10 |
| #define LATDEG 19 |
| #define LATMIN 21 |
| 21,23 → 22,24 |
|---|
| #define MIN_LEN 7 |
| #define AZIMUTH 46 |
| |
| float lat,lon; |
| float lat,lon; // Latitude, Longitude, Azimuth |
| int16 az; |
| |
| inline void read_NMEA() |
| #define NMEA_LINESIZE 72 |
| |
| inline void read_NMEA() // NMEA parsing |
| { |
| // auto char line[70]; |
| auto char item[16]; |
| char *ptr,*line; |
| auto int8 n; |
| auto char c; |
| |
| line=malloc(72); |
| line=malloc(NMEA_LINESIZE); // Space for one line |
| if (line==NULL) {printf(lcd_putc,"Error"); sleep();}; |
| line[71]=0; |
| line[NMEA_LINESIZE-1]=0; |
| |
| while(fgetc(NMEA)!='$'); |
| for(n=0;n<71;n++) |
| while(fgetc(NMEA)!='$'); // Waiting for start character |
| for(n=0;n<(NMEA_LINESIZE-1);n++) // Read line up to checksum |
| { |
| c=fgetc(NMEA); |
| if(c=='*') |
| 56,9 → 58,6 |
|---|
| printf(lcd_putc,"%s*",item); |
| strncpy(item,&line[LONDEG],DEG_LEN+MIN_LEN); item[DEG_LEN+MIN_LEN]=0; |
| printf(lcd_putc,"%s*",item); |
| lcd_gotoxy(1,4); |
| // strncpy(item,&line[AZIMUTH],6); item[6]=0; |
| // printf(lcd_putc,"%s*",item); |
| |
| lcd_gotoxy(1,2); |
| lat=strtod(&line[LATMIN],&ptr); |
| 86,31 → 85,29 |
|---|
| setup_vref(VREF_LOW|-2); |
| lcd_init(); |
| |
| // gets(line,NMEA); // Dummy read form the GPS |
| |
| |
| while(TRUE) |
| { |
| const float gc_lat=49*60+15.574,gc_lon=14*60+42.331; // Cisticka |
| const float gc_lat=49*60+12.4452,gc_lon=14*60+18.9079; // Testovaci souradnice: Cisticka |
| |
| read_NMEA(); |
| float bearing, heading, azim; |
| |
| lon=(gc_lon-lon)*1214; |
| read_NMEA(); // Read one NMEA line and parse |
| |
| lon=(gc_lon-lon)*1214; // Kakona's projection :) |
| lat=(gc_lat-lat)*1854; |
| |
| lcd_gotoxy(1,4); |
| printf(lcd_putc,"%.0g* ",sqrt((lon*lon) + (lat*lat))); |
| printf(lcd_putc,"%.0g* ",sqrt((lon*lon) + (lat*lat))); // Distance |
| lcd_gotoxy(1,1); |
| if (lat==0) lat=0.001; |
| lon=3-(6/PI*atan2(lat,lon)); |
| if (lon<0) lon+=12; |
| if (lon>12) lon-=12; |
| printf(lcd_putc,"BE%2.0g*",lon); |
| lat=12.0/360*az; |
| lon=lon-lat; |
| if (lon<0) lon+=12; |
| if (lon>12) lon-=12; |
| printf(lcd_putc,"HE%2.0g*AZ%2.0g* ",lon,lat); |
| if (lon==0) lon=0.001; // Divided by zero cure |
| bearing=3-(6/PI*atan2(lat,lon)); // Bearing |
| if (bearing<0) bearing+=12; // overflow cure |
| if (bearing>12) bearing-=12; |
| printf(lcd_putc,"BE%2.0g*",bearing); |
| azim=12.0/360*az; // Azimuth of the movement |
| heading=bearing-lat; // Heading |
| if (heading<0) heading+=12; // overflow cure |
| if (heading>12) heading-=12; |
| printf(lcd_putc,"HE%2.0g*AZ%2.0g* ",heading,azim); |
| } |
| |
| } |