Andy K
Satellite Observers Workbench. NOT yet complete, just published for forum posters to \"cherry pick\" pieces of code as requiered as an example.
Diff: gps/gps.c
- Revision:
- 0:0a841b89d614
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/gps/gps.c Mon Oct 11 10:34:55 2010 +0000
@@ -0,0 +1,670 @@
+/****************************************************************************
+ * Copyright 2010 Andy Kirkham, Stellar Technologies Ltd
+ *
+ * This file is part of the Satellite Observers Workbench (SOWB).
+ *
+ * SOWB is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * SOWB is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with SOWB. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * $Id: main.cpp 5 2010-07-12 20:51:11Z ajk $
+ *
+ ***************************************************************************/
+
+/*
+ Implementation notes.
+ The SOWB reads GPS data in on RDX1. We don't use TDX1 as we use it as part
+ of the SSP/SPI for the MAX7456/Flash card reader. However, we are not really
+ interested in writing to the GPS module as the most crucial data is the time
+ rather than the location. We do use the location data but we don't need any
+ WAAS data correction, 10metres is enough accuracy. So we only use Uart1 Rx to
+ get data.
+
+ Additionally, the GPS 1 pulse per second signal is connected to a P29 (P0.5)
+ and the gpioirq.c module routes that interrupt back to us via the _gps_pps_irq()
+ callback function.
+*/
+
+#include "sowb.h"
+#include "rit.h"
+#include "gpio.h"
+#include "gps.h"
+#include "math.h"
+#include "debug.h"
+
+/* Module global variables. */
+double lat_average;
+double lon_average;
+double lat_history[GPS_HISTORY_SIZE];
+double lon_history[GPS_HISTORY_SIZE];
+int history_in_index;
+int history_complete;
+GPS_TIME the_time;
+GPS_LOCATION_RAW the_location;
+
+double lat_acc_average;
+double lon_acc_average;
+double cnt_acc_average;
+
+/* We maintain two serial input buffers so that the
+ _process() function can be processing one buffer
+ while the serial interrupt system can be capturing
+ to the other. */
+char uart1_buffer[2][GPS_BUFFER_SIZE];
+
+#define UART1_BUFFER_A 0
+#define UART1_BUFFER_B 1
+
+char active_buffer, active_buffer_in;
+char passive_buffer_ready;
+
+/* Update flag to signal new data in the active buffer. */
+char have_new_location;
+
+/* Set to non-zero by the updater interrupts. */
+char time_updated;
+char location_updated;
+
+/* Internal function prototypes. */
+void _gps_process_rmc(char passive_buffer);
+void _gps_process_gga(char passive_buffer);
+void _gps_time_inc(GPS_TIME *q);
+void _gps_date_inc(GPS_TIME *q);
+void _gps_timer_tick_cb(int);
+void _gps_pps_alive_cb(int);
+void Uart1_init(void);
+
+/** gps_process
+ */
+void gps_process(void) {
+ int i, j;
+ uint32_t ier_copy;
+ char passive_buffer;
+ double lat_temp, lon_temp;
+
+ if (passive_buffer_ready == 1) {
+ /* Disable serial interrupts on UART1 */
+ ier_copy = LPC_UART1->IER;
+ LPC_UART1->IER = 0x0;
+
+ /* What index is the passive buffer, i.e., what is the opposite
+ of the current active buffer? */
+ passive_buffer = active_buffer == 0 ? 1 : 0;
+
+ if (!strncmp(uart1_buffer[passive_buffer], "$GPRMC", 6)) {
+ _gps_process_rmc(passive_buffer);
+ }
+ else if (!strncmp(uart1_buffer[passive_buffer], "$GPGGA", 6)) {
+ _gps_process_gga(passive_buffer);
+ }
+
+ /* Flag we have completed processing the passive buffer. */
+ passive_buffer_ready = 0;
+
+ /* Enable serial interrupts on UART1 */
+ LPC_UART1->IER = ier_copy;
+ }
+
+ /* Update the latitude/longitude moving average filter. */
+ if (the_location.is_valid && have_new_location != 0) {
+ have_new_location = 0;
+ lat_history[history_in_index] = gps_convert_coord(the_location.lat, GPS_LAT_STR);
+ lon_history[history_in_index] = gps_convert_coord(the_location.lon, GPS_LON_STR);
+ history_in_index++;
+ if (history_in_index >= GPS_HISTORY_SIZE) {
+ history_in_index = 0;
+ history_complete = 1;
+ }
+ j = history_complete == 1 ? GPS_HISTORY_SIZE+1 : history_in_index;
+ for(lat_temp = lon_temp = 0., i = 1; i < j; i++) {
+ lat_temp += lat_history[i - 1];
+ lon_temp += lon_history[i - 1];
+ }
+ if (i) {
+ lat_average = lat_temp / (double)(i - 1);
+ lon_average = lon_temp / (double)(i - 1);
+ location_updated = 1;
+ }
+ }
+}
+
+/** _gps_process_rmc
+ *
+ * Extract the NMEA data from an RMC packet.
+ * Sample:-
+ * $GPRMC,132555.639,A,5611.5374,N,00302.0325,W,000.0,129.3,020910,,,A*75
+ *
+ * @param char passive_buffer Which buffer holds the RMC packet data.
+ */
+void _gps_process_rmc(char passive_buffer) {
+ char *token;
+ int token_counter = 0;
+ char *time = (char *)NULL;
+ char *date = (char *)NULL;
+ char *status = (char *)NULL;
+
+ token = strtok(uart1_buffer[passive_buffer], ",");
+ while (token) {
+ switch (token_counter) {
+ case 9: date = token; break;
+ case 1: time = token; break;
+ case 2: status = token; break;
+ }
+ token = strtok((char *)NULL, ",");
+ token_counter++;
+ }
+
+ if (status && date && time) {
+ the_time.hour = (char)((time[0] - '0') * 10) + (time[1] - '0');
+ the_time.minute = (char)((time[2] - '0') * 10) + (time[3] - '0');
+ the_time.second = (char)((time[4] - '0') * 10) + (time[5] - '0');
+ the_time.day = (char)((date[0] - '0') * 10) + (date[1] - '0');
+ the_time.month = (char)((date[2] - '0') * 10) + (date[3] - '0');
+ the_time.year = (int)((date[4] - '0') * 10) + (date[5] - '0') + 2000;
+ the_time.is_valid = status[0] == 'A' ? 1 : 0;
+ the_time.prev_valid = 1;
+ }
+ else {
+ the_time.is_valid = 0;
+ }
+}
+
+/** _gps_process_gga
+ *
+ * Extract the NMEA data from a GGA packet.
+ * Sample:-
+ * $GPGGA,132526.639,5611.5417,N,00302.0298,W,1,05,7.3,43.4,M,52.0,M,,0000*70
+ *
+ * @param char passive_buffer Which buffer holds the GGA packet data.
+ */
+void _gps_process_gga(char passive_buffer) {
+ char *token;
+ int token_counter = 0;
+ char *latitude = (char *)NULL;
+ char *longitude = (char *)NULL;
+ char *lat_dir = (char *)NULL;
+ char *lon_dir = (char *)NULL;
+ char *qual = (char *)NULL;
+ char *altitude = (char *)NULL;
+ char *sats = (char *)NULL;
+
+ token = strtok(uart1_buffer[passive_buffer], ",");
+ while (token) {
+ switch (token_counter) {
+ case 2: latitude = token; break;
+ case 4: longitude = token; break;
+ case 3: lat_dir = token; break;
+ case 5: lon_dir = token; break;
+ case 6: qual = token; break;
+ case 7: sats = token; break;
+ case 9: altitude = token; break;
+ }
+ token = strtok((char *)NULL, ",");
+ token_counter++;
+ }
+
+ /* If the fix quality is valid set our location information. */
+ if (latitude && longitude && altitude && sats) {
+ memcpy(the_location.lat, latitude, 10); /* Fixed length string. */
+ memcpy(the_location.lon, longitude, 10); /* Fixed length string. */
+ memset(the_location.alt, 0, sizeof(the_location.alt)); /* Clean string out first. */
+ strncpy(the_location.alt, altitude, 10); /* Variable length string. */
+ strncpy(the_location.sats, sats, 3); /* Variable length string. */
+ the_location.north_south = lat_dir[0];
+ the_location.east_west = lon_dir[0];
+ the_location.is_valid = qual[0];
+ the_location.updated++;
+ have_new_location = 1;
+ }
+ else {
+ the_location.is_valid = qual[0];
+ }
+}
+
+/** gps_convert_coord
+ *
+ * Given a string and a type convert the string into a double.
+ *
+ * @param char *s A pointer to the string to convert.
+ * @param int type The conversion type required (lat or lon)
+ * @return double the converted string.
+ */
+double gps_convert_coord(char *s, int type) {
+ int deg, min, sec;
+ double fsec, val;
+
+ if (type == GPS_LAT_STR) {
+ deg = ( (s[0] - '0') * 10) + s[1] - '0';
+ min = ( (s[2] - '0') * 10) + s[3] - '0';
+ sec = ( ((s[5] - '0') * 1000) + ((s[6] - '0') * 100) + ((s[7] - '0') * 10) + (s[8] - '0'));
+ fsec = (double)((double)sec /10000.0);
+ val = (double)deg + ((double)((double)min/60.0)) + (fsec/60.0);
+ return val;
+ }
+ else {
+ deg = ( (s[0] - '0') * 100) + ((s[1] - '0') * 10) + (s[2] - '0');
+ min = ( (s[3] - '0') * 10) + s[4] - '0';
+ sec = ( ((s[6] - '0') * 1000) + ((s[7] - '0') * 100) + ((s[8] - '0') * 10) + (s[9] - '0'));
+ fsec = (double)((double)sec /10000.0);
+ val = (double)deg + ((double)((double)min/60.0)) + (fsec/60.0);
+ return val;
+ }
+}
+
+/** gps_init
+ */
+void gps_init(void) {
+ int i;
+
+ DEBUG_INIT_START;
+
+ /* Set the time to a known starting point in the past. */
+ the_time.year = 2000;
+ the_time.month = 1;
+ the_time.day = 1;
+ the_time.hour = 0;
+ the_time.minute = 0;
+ the_time.second = 0;
+ the_time.tenth = 0;
+ the_time.hundreth = 0;
+ the_time.is_valid = 0;
+ the_time.prev_valid = 0;
+
+ memset(&the_location, 0, sizeof(GPS_LOCATION_RAW));
+
+ /* Initial condition. */
+ time_updated = 0;
+
+ /* Zero out the moving average filter. */
+ lat_average = 0.;
+ lon_average = 0.;
+ history_in_index = 0;
+ history_complete = 0;
+ for(i = 0; i < GPS_HISTORY_SIZE; i++) {
+ lat_history[i] = 0.;
+ lon_history[i] = 0.;
+ }
+
+ lat_acc_average = 0.;
+ lon_acc_average = 0.;
+ cnt_acc_average = 0.;
+
+ /* Init the active buffer and the serial in pointer. */
+ active_buffer = active_buffer_in = 0;
+
+ /* Flag the passive buffer is not ready. The passive buffer
+ is the opposite of the active buffer. When the serial irq
+ system detects the end of a NEMA message it automatically
+ switches buffers and sets passive_buffer_ready non-zero to
+ indicate it's just finished dumping data into it. */
+ passive_buffer_ready = 0;
+
+ /* Updated to non-zero after a new location packet is received. */
+ have_new_location = 0;
+
+ /* Setup the 0.01second timer. */
+ rit_timer_set_reload(RIT_TIMER_CB_GPS, 10); /* Recurring reload. */
+ rit_timer_set_counter(RIT_TIMER_CB_GPS, 10); /* Start timer. */
+
+ DEBUG_INIT_END;
+
+ Uart1_init();
+}
+
+/** gps_get_time
+ *
+ * Copies our internal time data structure to a buffer supplied by the caller.
+ *
+ * Note, the update flag is set to non-zero by the interrupt routines when an
+ * update occurs. The loop is to test if an update occured while the copy was
+ * in progress. If it did, do the copy again as it's most likely corrupted the
+ * orginal copy operation.
+ *
+ * @param GPS_TIME *q A pointer to the GPS_TIME data structure to copy to.
+ * @return GPS_TIME * The supplied pointer.
+ */
+GPS_TIME *gps_get_time(GPS_TIME *q) {
+
+ do {
+ time_updated = 0;
+ memcpy(q, &the_time, sizeof(GPS_TIME));
+ } while (time_updated != 0);
+
+ return q;
+}
+
+/** gps_get_location_raw
+ *
+ * Copies our internal location data structure to a buffer supplied by the caller.
+ *
+ * Note, the update flag is set to non-zero by the interrupt routines when an
+ * update occurs. The loop is to test if an update occured while the copy was
+ * in progress. If it did, do the copy again as it's most likely corrupted the
+ * orginal copy operation.
+ *
+ * @param GPS_LOCATION_RAW *q A pointer to the GPS_LOCATION_RAW data structure to copy to.
+ * @return GPS_LOCATION_RAW * The supplied pointer.
+ */
+GPS_LOCATION_RAW *gps_get_location_raw(GPS_LOCATION_RAW *q) {
+
+ do {
+ location_updated = 0;
+ memcpy(q, &the_location, sizeof(GPS_LOCATION_RAW));
+ } while (location_updated != 0);
+
+ return q;
+}
+
+/** gps_get_location_average
+ *
+ * Places the current average location into the supplied struct buffer.
+ * The caller is responsible for allocating the buffer storage space.
+ *
+ * Note, the update flag is set to non-zero by the interrupt routines when an
+ * update occurs. The loop is to test if an update occured while the copy was
+ * in progress. If it did, do the copy again as it's most likely corrupted the
+ * orginal copy operation.
+ *
+ * @param GPS_LOCATION_AVERAGE *q A pointer to the struct buffer to write data to.
+ * @return GPS_LOCATION_AVERAGE * The supplied pointer returned.
+ */
+GPS_LOCATION_AVERAGE *gps_get_location_average(GPS_LOCATION_AVERAGE *q) {
+ char **p = NULL;
+
+ do {
+ location_updated = 0;
+ q->north_south = the_location.north_south;
+ q->latitude = lat_average;
+ q->east_west = the_location.east_west;
+ q->longitude = lon_average;
+ q->height = strtod(the_location.alt, p);
+ q->sats = the_location.sats;
+ q->is_valid = the_location.is_valid;
+ } while (location_updated != 0);
+
+ /* Test the values to ensure the data is valid. */
+ if (isnan(q->latitude) || isnan(q->longitude) || isnan(q->height)) {
+ q->is_valid = 0;
+ }
+
+ return q;
+}
+
+/** gps_julian_day_number
+ *
+ * Gets the Julian Day Number from the supplied time reference passed.
+ * http://en.wikipedia.org/wiki/Julian_day#Converting_Gregorian_calendar_date_to_Julian_Day_Number
+ *
+ * @param GPS_TIME *t A pointer to a time data structure.
+ * @return double The Julian Day Number.
+ */
+double gps_julian_day_number(GPS_TIME *t) {
+ double wikipedia_jdn = (double)(1461 * ((int)t->year + 4800 + ((int)t->month - 14) / 12)) / 4 + (367 * ((int)t->month - 2 - 12 * (((int)t->month - 14) / 12))) / 12 - (3 * (((int)t->year + 4900 + ((int)t->month - 14) / 12 ) / 100)) / 4 + (int)t->day - 32075;
+
+ /* Not sure why yet but the calculation on the Wikipedia site returns a value that
+ is 0.5 too big. */
+ return wikipedia_jdn;
+}
+
+/** gps_julian_date
+ *
+ * Find the Julian Date based on the supplied args.
+ *
+ * @param GPS_TIME *t A pointer to a time data structure.
+ * @return double The Julian Date.
+ */
+double gps_julian_date(GPS_TIME *t) {
+ double hour, minute, second, jd;
+ hour = (double)t->hour;
+ minute = (double)t->minute;
+ second = (double)t->second + ((double)t->tenth / 10.) + ((double)t->hundreth / 100.);
+ /* Wiki fix, see above. */
+ jd = gps_julian_day_number(t) - 0.5 +
+ ((hour - 12.) / 24.) +
+ (minute / 1440.) +
+ (second / 86400.);
+
+ return jd;
+}
+
+/** gps_siderealDegrees_by_jd
+ *
+ * Calculate the sidereal degree angle based on the
+ * Julian Date supplied.
+ *
+ * @param double jd Julian Date.
+ * @return The sidereal angle in degrees.
+ */
+double gps_siderealDegrees_by_jd(double jd) {
+ double sidereal, gmst, lmst, mul;
+ double T = jd - 2451545.0;
+ double T1 = T / 36525.0;
+ double T2 = T1 * T1;
+ double T3 = T2 * T1;
+
+ /* Calculate gmst angle. */
+ sidereal = 280.46061837 + (360.98564736629 * T) + (0.000387933 * T2) - (T3 / 38710000.0);
+
+ /* Convert to degrees. */
+ sidereal = fmod(sidereal, 360.0);
+ if (sidereal < 0.0) sidereal += 360.0;
+
+ mul = (the_location.east_west == 'W') ? -1.0 : 1.0;
+
+ gmst = sidereal;
+ lmst = gmst + (lon_average * mul);
+ return lmst;
+}
+
+/** gps_siderealDegrees_by_time
+ *
+ * Calculate the sidereal degree angle based on the
+ * time data structure supplied.
+ *
+ * @param GPS_TIME *t A pointer to the time structure.
+ * @return The sidereal angle in degrees.
+ */
+double gps_siderealDegrees_by_time(GPS_TIME *t) {
+ GPS_TIME temp;
+ if (t == (GPS_TIME *)NULL) {
+ t = &temp;
+ gps_get_time(t);
+ }
+ return gps_siderealDegrees_by_jd(gps_julian_date(t));
+}
+
+/** gps_siderealHA_by_jd
+ *
+ * Calculate the HA (hour angle) based on the supplied Julian Date.
+ *
+ * @param double jd The Julian Date.
+ * @return double The Hour Angle.
+ */
+double gps_siderealHA_by_jd(double jd) {
+ double lmst = gps_siderealDegrees_by_jd(jd);
+ return lmst / 360.0 * 24.0;
+}
+
+/** gps_siderealHA_by_time
+ *
+ * Calculate the HA (hour angle) based on the supplied time data structure.
+ *
+ * @param GPS_TIME *t The time data structure.
+ * @return double The Hour Angle.
+ */
+double gps_siderealHA_by_time(GPS_TIME *t) {
+ double lmst = gps_siderealDegrees_by_time(t);
+ return lmst / 360.0 * 24.0;
+}
+
+/** _gps_inc_time
+ *
+ * Used to increment the time structure by 0.01sec.
+ * Called by the RIT timer callback.
+ *
+ * @see gpioirq.c
+ * @param int t_index The timer's index number (handle).
+ */
+void _gps_timer_tick_cb(int t_index) {
+
+ /* We use x so that the_time.hundreth can never be 10 as I have noticed
+ occasionally between the ++ and the == 10 test an interrupt can occur
+ and then the_time.hundreth contains an invalid value. So using x to do
+ the ++ and ==10 test means the_time.hundreth can never itself be 10.
+ We reuse x on the tenths for a similar reason. */
+ char x = the_time.hundreth;
+ x++;
+
+ if (x == 10) {
+ the_time.hundreth = 0;
+ x = the_time.tenth + 1;
+ if (x < 10) {
+ the_time.tenth = x;
+ time_updated = 1;
+ }
+ }
+ else {
+ the_time.hundreth = x;
+ time_updated = 1;
+ }
+}
+
+/** _gps_pps_alive_cb
+ *
+ * Timeout that goes off if the GPS 1PPS signal doesn't
+ * fire within 1.5seconds telling us that the GPS isn't
+ * connected. Not currently used.
+ */
+void _gps_pps_alive_cb(int index) {
+ the_time.is_valid = 0;
+}
+
+/** gps_pps_fall
+ *
+ * Increments the seconds. Called by the GPS 1PP callback interrupt.
+ *
+ * Note, some GPS modules, including the one used in this design,
+ * provide a 1PPS signal. However, it's almost always positive logic
+ * and it doesn't interface directly to an Mbed pin/interrupt. So we
+ * have a simple FET that buffers the signal and in so doing it becomes
+ * an active low signal. Hence why this is a falling edge interrupt.
+ *
+ * @see gpioirq.c
+ */
+void gps_pps_fall(void) {
+ the_time.hundreth = 0;
+ the_time.tenth = 0;
+ _gps_time_inc(&the_time);
+}
+
+/** gps_date_inc
+ *
+ * Increment the time.
+ *
+ * @param GPS_TIME *q Pointer the data struct holding the time.
+ */
+void _gps_time_inc(GPS_TIME *q) {
+
+ time_updated = 1;
+
+ q->second++;
+ if (q->second == 60) {
+ q->second = 0;
+ q->minute++;
+ if (q->minute == 60) {
+ q->minute = 0;
+ q->hour++;
+ if (q->hour == 24) {
+ q->hour = 0;
+ _gps_date_inc(q);
+ }
+ }
+ }
+}
+
+/** _gps_date_inc
+ *
+ * Increment the date.
+ *
+ * @param GPS_TIME *q Pointer the data struct holding the time.
+ */
+void _gps_date_inc(GPS_TIME *q) {
+ const int days[12] = { 31,28,31,30,31,30,31,31,30,31,30,31 } ;
+
+ /* Handle February leap year. */
+ int leap_year = ((the_time.year % 4 == 0 && the_time.year % 100 != 0) || the_time.year % 400 == 0) ? 1 : 0;
+ int days_this_month = days[q->month - 1];
+ if (q->month == 2 && leap_year) days_this_month++;
+
+ q->day++;
+ if (q->day > days_this_month) {
+ q->day = 1;
+ q->month++;
+ if (q->month == 13) {
+ q->year++;
+ }
+ }
+}
+
+/* UART1 functions. */
+
+extern "C" void UART1_IRQHandler(void) __irq {
+ volatile uint32_t iir;
+ volatile char c;
+
+ iir = LPC_UART1->IIR;
+
+ if (iir & 0x1) return;
+
+ /* Do we have a serial character(s) in the fifo? */
+ if (UART_RX_INTERRUPT) {
+ while (UART1_FIFO_NOT_EMPTY) {
+ c = UART1_GETC;
+ uart1_buffer[active_buffer][active_buffer_in++] = c;
+ active_buffer_in &= (GPS_BUFFER_SIZE - 1);
+ if (c == '\n') {
+ /* Swap buffers and clean it out. */
+ active_buffer = active_buffer == 0 ? 1 : 0;
+ memset(uart1_buffer[active_buffer], 0, GPS_BUFFER_SIZE);
+ active_buffer_in = 0;
+ passive_buffer_ready = 1;
+ }
+ }
+ }
+}
+
+/** Uart1_init
+ */
+void Uart1_init(void) {
+
+ DEBUG_INIT_START;
+
+ LPC_SC->PCONP |= (1UL << 4);
+ LPC_SC->PCLKSEL0 &= ~(3UL << 8);
+ LPC_SC->PCLKSEL0 |= (1UL << 8);
+ LPC_PINCON->PINSEL4 &= ~(3UL << 2); /* TXD1 not used. See SSP0_init() in max7456.c */
+ LPC_PINCON->PINSEL4 |= (2UL << 2); /* TXD1 not used. See SSP0_init() in max7456.c */
+ LPC_UART1->LCR = 0x83;
+ LPC_UART1->DLL = 0x71;
+ LPC_UART1->DLM = 0x02;
+ LPC_UART1->LCR = 0x03;
+ LPC_UART1->FCR = 0x07;
+
+ NVIC_SetVector(UART1_IRQn, (uint32_t)UART1_IRQHandler);
+ NVIC_EnableIRQ(UART1_IRQn);
+
+ /* Enable the RDA interrupt. */
+ LPC_UART1->IER = 0x01;
+
+ DEBUG_INIT_END;
+}
+