kopia lustrzana https://github.com/fsphil/hadie
227 wiersze
4.7 KiB
C
227 wiersze
4.7 KiB
C
/* hadie - High Altitude Balloon flight software */
|
|
/*============================================================*/
|
|
/* Copyright (C)2010 Philip Heron <phil@sanslogic.co.uk> */
|
|
/* */
|
|
/* This program is distributed under the terms of the GNU */
|
|
/* General Public License, version 2. You may use, modify, */
|
|
/* and redistribute it under the terms of this license. A */
|
|
/* copy should be included with this source. */
|
|
|
|
#include "config.h"
|
|
#include <stdint.h>
|
|
#include <string.h>
|
|
#include <stdio.h>
|
|
#include <avr/interrupt.h>
|
|
#include "gps.h"
|
|
|
|
/************* UART RX interrupt handler and buffer *************************/
|
|
|
|
/* The maximum length of the NMEA string is 82 characters */
|
|
#define RXBUFSIZE (82 + 1)
|
|
|
|
/* Helper to convert uppercase hex ascii character to integer */
|
|
#define HTOI(c) (((c) - '0' > '9' ? (c) - 'A' + 10 : (c) - '0') & 0x0F)
|
|
|
|
/* Received GPGGA lines are copied into this buffer by the interrupt */
|
|
static volatile char rxline[RXBUFSIZE];
|
|
static volatile char rx_lock = 0;
|
|
|
|
ISR(USART1_RX_vect)
|
|
{
|
|
static char rx[RXBUFSIZE];
|
|
static uint8_t rx_len = 0; /* Number of characters in the buffer */
|
|
static uint8_t rx_checksum = 0; /* Calculated checksum */
|
|
static uint8_t rx_have_checksum = 0; /* If the string has a checksum */
|
|
static uint8_t checksum; /* The checksum at the end of the string */
|
|
uint8_t b;
|
|
|
|
/* Read the received character */
|
|
b = UDR1;
|
|
|
|
switch(b)
|
|
{
|
|
case '$': break;
|
|
case '*': rx_have_checksum = 1; return;
|
|
case '\n':
|
|
case '\r':
|
|
/* A complete line has been received - is the checksum valid? */
|
|
if(!rx_have_checksum) break;
|
|
if(checksum != rx_checksum) break;
|
|
|
|
/* Is it a GPGGA message? */
|
|
rx[rx_len] = '\0';
|
|
if(strncmp(rx, "GPGGA", 5) != 0) break;
|
|
|
|
/* Got a valid GPGGA line, copy it if not locked */
|
|
if(!rx_lock) strncpy((char *) rxline, rx, RXBUFSIZE);
|
|
|
|
break;
|
|
|
|
default:
|
|
if(!rx_have_checksum)
|
|
{
|
|
/* Clear the buffer if full */
|
|
if(rx_len >= RXBUFSIZE) break;
|
|
|
|
/* Store the new character and update checksum */
|
|
rx[rx_len++] = b;
|
|
rx_checksum ^= b;
|
|
}
|
|
else
|
|
{
|
|
checksum <<= 4;
|
|
checksum |= HTOI(b);
|
|
}
|
|
|
|
return;
|
|
}
|
|
|
|
/* Clear the buffer */
|
|
rx_len = 0;
|
|
rx_checksum = 0;
|
|
rx_have_checksum = 0;
|
|
}
|
|
|
|
/* Like atoi but for fixed point numbers. Processes at most n bytes and
|
|
* returns a pointer the next position in the string. Reads dp decimal
|
|
* places */
|
|
int32_t strntofp(const char *s, char **endptr, size_t n, char dp)
|
|
{
|
|
int32_t i = 0;
|
|
char neg = 0, fp = 0;
|
|
|
|
if(n <= 0) goto out;
|
|
|
|
/* Test for a + or - sign */
|
|
switch(*s)
|
|
{
|
|
case '-': neg = !neg;
|
|
case '+': s++; n--;
|
|
}
|
|
|
|
/* Read in the number */
|
|
while(*s && n && (!fp || dp))
|
|
{
|
|
char d = *s;
|
|
|
|
if(d >= '0' && d <= '9')
|
|
{
|
|
/* Add the digit */
|
|
i *= 10;
|
|
i += d - '0';
|
|
|
|
if(fp) dp--;
|
|
}
|
|
else if(dp > 0 && d == '.') fp = 1;
|
|
else break;
|
|
|
|
/* Next... */
|
|
s++;
|
|
n--;
|
|
}
|
|
|
|
while(dp > 0)
|
|
{
|
|
i *= 10;
|
|
dp--;
|
|
}
|
|
|
|
/* Fix result if it's negative */
|
|
if(neg) i = -i;
|
|
|
|
out:
|
|
/* Set the end pointer if needed */
|
|
if(endptr) *endptr = (char *) s;
|
|
|
|
return(i);
|
|
}
|
|
|
|
int32_t strntoi(const char *s, char **endptr, size_t n)
|
|
{
|
|
return(strntofp(s, endptr, n, 0));
|
|
}
|
|
|
|
char *gps_field(char *s, int f)
|
|
{
|
|
while(*s && f > 0) if(*(s++) == ',') f--;
|
|
if(f == 0) return(s);
|
|
return(NULL);
|
|
}
|
|
|
|
char gps_parse(gpsfix_t *gps)
|
|
{
|
|
char i;
|
|
|
|
memset(gps, 0, sizeof(gpsfix_t));
|
|
rx_lock = 1;
|
|
|
|
for(i = 0; i < 10; i++)
|
|
{
|
|
char *r = gps_field((char *) rxline, i);
|
|
if(!r)
|
|
{
|
|
rx_lock = 0;
|
|
return(-1);
|
|
}
|
|
|
|
switch(i)
|
|
{
|
|
case 1: /* Fix Time */
|
|
gps->hour = strntoi(r, &r, 2);
|
|
gps->minute = strntoi(r, &r, 2);
|
|
gps->second = strntoi(r, &r, 2);
|
|
break;
|
|
|
|
case 2: /* Latitude */
|
|
gps->latitude_i = strntoi(r, &r, 2);
|
|
gps->latitude_f = strntofp(r, &r, 7, 4) * 100 / 60;
|
|
break;
|
|
|
|
case 3: /* Latitude hemisphere */
|
|
gps->latitude_h = (*r == 'S' ? 'S' : 'N');
|
|
break;
|
|
|
|
case 4: /* Longitude */
|
|
gps->longitude_i = strntoi(r, &r, 3);
|
|
gps->longitude_f = strntofp(r, &r, 7, 4) * 100 / 60;
|
|
break;
|
|
|
|
case 5: /* Longitude hemisphere */
|
|
gps->longitude_h = (*r == 'W' ? 'W' : 'E');
|
|
break;
|
|
|
|
case 6: /* Fix quality */
|
|
gps->fix = strntoi(r, NULL, 3);
|
|
break;
|
|
|
|
case 7: /* Satellites */
|
|
gps->sats = strntoi(r, NULL, 3);
|
|
break;
|
|
|
|
case 9: /* Altitude */
|
|
gps->altitude = strntoi(r, NULL, 10);
|
|
break;
|
|
}
|
|
}
|
|
|
|
rx_lock = 0;
|
|
|
|
return(0);
|
|
}
|
|
|
|
void gps_init()
|
|
{
|
|
rxline[0] = '\0';
|
|
|
|
/* Do UART1 initialisation, 38400 baud @ 7.3728 MHz */
|
|
UBRR1H = 0;
|
|
UBRR1L = 11;
|
|
|
|
/* Enable RX, TX and RX interrupt */
|
|
UCSR1B = (1 << RXEN1) | (1 << TXEN1) | (1 << RXCIE1);
|
|
|
|
/* 8-bit, no parity and 1 stop bit */
|
|
UCSR1C = (1 << UCSZ11) | (1 << UCSZ10);
|
|
}
|
|
|