/Designs/skrysohledac1/SW/gps.c
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);
}
 
}