From 763fda3cdb51e56a31850b9e057b4a76be0e857b Mon Sep 17 00:00:00 2001 From: Philip Heron Date: Tue, 3 Aug 2010 23:42:15 +0100 Subject: [PATCH] Add GPS parser and other bits --- Makefile | 2 +- config.h | 1 - gps.c | 226 +++++++++++++++++++++++++++++++++++++++++++++++++++++++ gps.h | 31 ++++++++ hadie.c | 104 +++++++++++++++++++------ 5 files changed, 340 insertions(+), 24 deletions(-) create mode 100644 gps.c create mode 100644 gps.h diff --git a/Makefile b/Makefile index d1b8a07..83a1548 100644 --- a/Makefile +++ b/Makefile @@ -1,6 +1,6 @@ PROJECT=hadie -OBJECTS=hadie.o rtty.o rs8encode.o c328.o +OBJECTS=hadie.o rtty.o gps.o rs8encode.o c328.o # Serial device used for programming AVR TTYPORT=/dev/ttyACM0 diff --git a/config.h b/config.h index 596c699..36b8cb5 100644 --- a/config.h +++ b/config.h @@ -1,4 +1,3 @@ - /* hadie - High Altitude Balloon flight software */ /*============================================================*/ /* Copyright (C)2010 Philip Heron */ diff --git a/gps.c b/gps.c new file mode 100644 index 0000000..b9da02a --- /dev/null +++ b/gps.c @@ -0,0 +1,226 @@ +/* hadie - High Altitude Balloon flight software */ +/*============================================================*/ +/* Copyright (C)2010 Philip Heron */ +/* */ +/* 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 +#include +#include +#include +#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 */ + if(*r == 'S') gps->latitude_i = -gps->latitude_i; + 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 */ + if(*r == 'W') gps->longitude_i = -gps->longitude_i; + 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); +} + diff --git a/gps.h b/gps.h new file mode 100644 index 0000000..cb0f28b --- /dev/null +++ b/gps.h @@ -0,0 +1,31 @@ +/* hadie - High Altitude Balloon flight software */ +/*============================================================*/ +/* Copyright (C)2010 Philip Heron */ +/* */ +/* 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. */ + +typedef struct { + + uint8_t hour; /* 0-23 */ + uint8_t minute; /* 0-60 */ + uint8_t second; /* 0-60 */ + + int16_t latitude_i; /* -180-180 */ + uint32_t latitude_f; /* 0-999999 */ + + int16_t longitude_i; /* -180-180 */ + uint32_t longitude_f; /* 0-999999 */ + + int32_t altitude; /* 0-99999 */ + + uint8_t fix; /* 0-2 */ + uint8_t sats; /* 0-99 */ + +} gpsfix_t; + +extern void gps_init(); +extern char gps_parse(gpsfix_t *gps); + diff --git a/hadie.c b/hadie.c index 82fc053..68cde02 100644 --- a/hadie.c +++ b/hadie.c @@ -14,6 +14,7 @@ #include #include #include "rtty.h" +#include "gps.h" #include "c328.h" #include "rs8.h" @@ -21,23 +22,29 @@ #define MSG_SIZE (100) char msg[MSG_SIZE]; +#define PREFIX "$$$$" + /* Image TX data */ #define PKT_SIZE (0x100) -#define PKT_SIZE_HEADER (0x06) +#define PKT_SIZE_HEADER (0x0A) #define PKT_SIZE_RSCODES (0x20) #define PKT_SIZE_PAYLOAD (PKT_SIZE - PKT_SIZE_HEADER - PKT_SIZE_RSCODES) uint8_t pkt[PKT_SIZE]; uint16_t pkt_len; uint16_t image_len; -void init_packet(uint8_t *packet, uint8_t imageid, uint8_t pktid, uint16_t filesize) +void init_packet(uint8_t *packet, uint8_t imageid, uint8_t pktid, uint8_t pkts, uint16_t width, uint16_t height) { - packet[0] = 0x55; /* Preamble */ - packet[1] = 0x66; /* Marker */ - packet[2] = imageid; /* Image ID */ - packet[3] = pktid; /* Packet ID */ - packet[4] = filesize & 0XFF; /* Filesize LSB */ - packet[5] = filesize >> 8; /* Filesize MSB */ + packet[0] = 0x55; /* Sync */ + packet[1] = 0x66; /* Type */ + packet[2] = imageid; /* Image ID */ + packet[3] = pktid; /* Packet ID */ + packet[4] = pkts; /* Packets */ + packet[5] = width >> 3; /* Width MCU */ + packet[6] = height >> 3; /* Height MCU */ + packet[7] = 0xFF; /* Next MCU offset */ + packet[8] = 0x00; /* MCU ID MSB */ + packet[9] = 0x00; /* MCU ID LSB */ memset(&packet[PKT_SIZE_HEADER], 0, PKT_SIZE_PAYLOAD); } @@ -45,35 +52,35 @@ char setup_camera(void) { if(c3_sync() != 0) { - rtx_string_P(PSTR("$$" CALLSIGN ":Camera sync failed...\n")); + rtx_string_P(PSTR(PREFIX CALLSIGN ":Camera sync failed...\n")); return(-1); } /* Setup the camera */ if(c3_setup(CT_JPEG, 0, SR_320x240) != 0) { - rtx_string_P(PSTR("$$" CALLSIGN ":Camera setup failed...\n")); + rtx_string_P(PSTR(PREFIX CALLSIGN ":Camera setup failed...\n")); return(-1); } /* Set the package size */ if(c3_set_package_size(PKT_SIZE_PAYLOAD + 6) != 0) { - rtx_string_P(PSTR("$$" CALLSIGN ":Package size set failed!\n")); + rtx_string_P(PSTR(PREFIX CALLSIGN ":Package size set failed!\n")); return(-1); } /* Take the image */ if(c3_snapshot(ST_JPEG, 0) != 0) { - rtx_string_P(PSTR("$$" CALLSIGN ":Snapshot failed!\n")); + rtx_string_P(PSTR(PREFIX CALLSIGN ":Snapshot failed!\n")); return(-1); } /* Get the image size and begin the transfer */ if(c3_get_picture(PT_SNAPSHOT, &image_len) != 0) { - rtx_string_P(PSTR("$$" CALLSIGN ":Get picture failed\n")); + rtx_string_P(PSTR(PREFIX CALLSIGN ":Get picture failed\n")); return(-1); } @@ -91,7 +98,7 @@ char tx_image(void) static uint16_t pkg_len; static uint8_t img_id = 0; - static uint8_t img_tx; + static uint16_t img_tx; static uint8_t pkt_id; if(!setup) @@ -107,7 +114,7 @@ char tx_image(void) } /* Initialise the packet -- make sure previous packet has finished TX'ing! */ - init_packet(pkt, img_id, pkt_id++, image_len); + init_packet(pkt, img_id, pkt_id++, 0xFF, 640, 480); pkt_len = 0; while(pkt_len < PKT_SIZE_PAYLOAD) @@ -115,14 +122,15 @@ char tx_image(void) if(pkg_len == 0) { char msg[100]; - if(c3_get_package(pkg_id++, &pkg, &pkg_len) != 0) + char i; + if((i = c3_get_package(pkg_id++, &pkg, &pkg_len)) != 0) { - snprintf(msg, 100, "$$" CALLSIGN ",Get package %i failed\n", pkg_id - 1); + snprintf(msg, 100, PREFIX CALLSIGN ",Get package %i failed (%i)\n", pkg_id - 1, i); rtx_string(msg); rtx_wait(); setup = 0; - return(-1); + return(setup); } /* Skip the package header */ @@ -135,6 +143,7 @@ char tx_image(void) uint16_t l = PKT_SIZE_PAYLOAD - pkt_len; if(pkg_len < l) l = pkg_len; + /* TODO: Copy with the JPEG filter */ memcpy(pkt + PKT_SIZE_HEADER + pkt_len, pkg, l); pkg += l; @@ -144,7 +153,7 @@ char tx_image(void) } /* Have we reached the end of the image? */ - if(img_tx == image_len) + if(img_tx >= image_len) { c3_finish_picture(); setup = 0; @@ -153,15 +162,57 @@ char tx_image(void) } encode_rs_8(&pkt[1], &pkt[PKT_SIZE_HEADER + PKT_SIZE_PAYLOAD], 0); + rtx_string_P(PSTR("UUU")); /* U = 0x55 */ rtx_data(pkt, PKT_SIZE); + //rtx_wait(); + + //c3_ping(); + + return(setup); +} + +uint16_t crccat(char *msg) +{ + uint16_t x; + for(x = 0xFFFF; *msg; msg++) + x = _crc_xmodem_update(x, *msg); + snprintf(msg, 8, "*%04X\n", x); + return(x); +} + +char tx_telemetry(void) +{ + static unsigned int counter = 0; + gpsfix_t gps; + + /* Read the GPS data */ + gps_parse(&gps); + + rtx_wait(); + snprintf(msg, MSG_SIZE, + PREFIX CALLSIGN ",%u,%02i:%02i:%02i,%i.%06lu,%i.%06lu,%li,%i:%i", + counter++, + gps.hour, gps.minute, gps.second, + gps.latitude_i, gps.latitude_f, + gps.longitude_i, gps.longitude_f, + gps.altitude, gps.fix, gps.sats); + + /* Append the checksum, skipping the first four $'s */ + crccat(msg + 4); + + /* Begin transmitting */ + rtx_string(msg); return(0); } int main(void) { + char r; + /* Initalise the various bits */ rtx_init(); + gps_init(); c3_init(); /* Let the radio settle before beginning */ @@ -172,9 +223,18 @@ int main(void) while(1) { - tx_image(); - rtx_wait(); - //rtx_string_P(PSTR("$$" CALLSIGN ",TEST\n")); + r = 5; + + if(tx_image() == -1) + { + /* The camera goes to sleep while transmitting telemetry, + * sync'ing here seems to prevent it. */ + c3_sync(); + r = 1; + } + + rtx_string_P(PSTR("\n")); + for(; r > 0; r--) tx_telemetry(); } return(0);