X-Git-Url: https://git.gag.com/?p=fw%2Faltos;a=blobdiff_plain;f=src%2Fao_gps.c;h=78957cb386dc6aeff05f2f19af1971128ff1a0d1;hp=cbde8b48dffce97eb0104ee3707896301b6bfc64;hb=ab40d224b39ba6d29c4056e2d2c365e1eb3d3793;hpb=17d2432a8b9c15963cd3b821f025ad33972ef477 diff --git a/src/ao_gps.c b/src/ao_gps.c index cbde8b48..78957cb3 100644 --- a/src/ao_gps.c +++ b/src/ao_gps.c @@ -15,239 +15,344 @@ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. */ +#ifndef AO_GPS_TEST #include "ao.h" +#endif -#define AO_GPS_LEADER 6 - -static const char ao_gps_header[] = "GPGGA,"; __xdata uint8_t ao_gps_mutex; -static __xdata char ao_gps_char; -static __xdata uint8_t ao_gps_cksum; -static __xdata uint8_t ao_gps_error; - __xdata struct ao_gps_data ao_gps_data; -static __xdata struct ao_gps_data ao_gps_next; -const char ao_gps_config[] = - "$PSRF103,00,00,01,01*25\r\n" /* GGA 1 per sec */ - "$PSRF103,01,00,00,01*25\r\n" /* GLL disable */ - "$PSRF103,02,00,00,01*26\r\n" /* GSA disable */ - "$PSRF103,03,00,00,01*27\r\n" /* GSV disable */ - "$PSRF103,04,00,00,01*20\r\n" /* RMC disable */ - "$PSRF103,05,00,00,01*21\r\n"; /* VTG disable */ +static const char ao_gps_set_nmea[] = "$PSRF100,0,57600,8,1,0*37\r\n"; + +const char ao_gps_config[] = { + 0xa0, 0xa2, 0x00, 0x0e, /* length: 14 bytes */ + 136, /* mode control */ + 0, 0, /* reserved */ + 4, /* degraded mode (disabled) */ + 0, 0, /* reserved */ + 0, 0, /* user specified altitude */ + 2, /* alt hold mode (disabled, require 3d fixes) */ + 0, /* alt hold source (use last computed altitude) */ + 0, /* reserved */ + 0, /* Degraded time out (disabled) */ + 0, /* Dead Reckoning time out (disabled) */ + 0, /* Track smoothing (disabled) */ + 0x00, 0x8e, 0xb0, 0xb3, + + 0xa0, 0xa2, 0x00, 0x08, /* length: 8 bytes */ + 166, /* Set message rate */ + 2, /* enable/disable all messages */ + 0, /* message id (ignored) */ + 0, /* update rate (0 = disable) */ + 0, 0, 0, 0, /* reserved */ + 0x00, 0xa8, 0xb0, 0xb3, + + 0xa0, 0xa2, 0x00, 0x02, /* length: 2 bytes */ + 143, /* static navigation */ + 0, /* disable */ + 0x00, 0x8f, 0xb0, 0xb3, +}; -static void -ao_gps_lexchar(void) +#define NAV_TYPE_GPS_FIX_TYPE_MASK (7 << 0) +#define NAV_TYPE_NO_FIX (0 << 0) +#define NAV_TYPE_SV_KF (1 << 0) +#define NAV_TYPE_2_SV_KF (2 << 0) +#define NAV_TYPE_3_SV_KF (3 << 0) +#define NAV_TYPE_4_SV_KF (4 << 0) +#define NAV_TYPE_2D_LEAST_SQUARES (5 << 0) +#define NAV_TYPE_3D_LEAST_SQUARES (6 << 0) +#define NAV_TYPE_DR (7 << 0) +#define NAV_TYPE_TRICKLE_POWER (1 << 3) +#define NAV_TYPE_ALTITUDE_HOLD_MASK (3 << 4) +#define NAV_TYPE_ALTITUDE_HOLD_NONE (0 << 4) +#define NAV_TYPE_ALTITUDE_HOLD_KF (1 << 4) +#define NAV_TYPE_ALTITUDE_HOLD_USER (2 << 4) +#define NAV_TYPE_ALTITUDE_HOLD_ALWAYS (3 << 4) +#define NAV_TYPE_DOP_LIMIT_EXCEEDED (1 << 6) +#define NAV_TYPE_DGPS_APPLIED (1 << 7) +#define NAV_TYPE_SENSOR_DR (1 << 8) +#define NAV_TYPE_OVERDETERMINED (1 << 9) +#define NAV_TYPE_DR_TIMEOUT_EXCEEDED (1 << 10) +#define NAV_TYPE_FIX_MI_EDIT (1 << 11) +#define NAV_TYPE_INVALID_VELOCITY (1 << 12) +#define NAV_TYPE_ALTITUDE_HOLD_DISABLED (1 << 13) +#define NAV_TYPE_DR_ERROR_STATUS_MASK (3 << 14) +#define NAV_TYPE_DR_ERROR_STATUS_GPS_ONLY (0 << 14) +#define NAV_TYPE_DR_ERROR_STATUS_DR_FROM_GPS (1 << 14) +#define NAV_TYPE_DR_ERROR_STATUS_DR_SENSOR_ERROR (2 << 14) +#define NAV_TYPE_DR_ERROR_STATUS_DR_IN_TEST (3 << 14) + +struct sirf_geodetic_nav_data { + uint16_t nav_type; + uint16_t utc_year; + uint8_t utc_month; + uint8_t utc_day; + uint8_t utc_hour; + uint8_t utc_minute; + uint16_t utc_second; + int32_t lat; + int32_t lon; + int32_t alt_msl; + uint16_t ground_speed; + uint16_t course; + int16_t climb_rate; + uint32_t h_error; + uint32_t v_error; + uint8_t num_sv; + uint8_t hdop; +}; + +static __xdata struct sirf_geodetic_nav_data ao_sirf_data; + +static __pdata uint16_t ao_sirf_cksum; +static __pdata uint16_t ao_sirf_len; + +#define ao_sirf_byte() ((uint8_t) ao_serial_getchar()) + +static uint8_t data_byte(void) { - if (ao_gps_error) - ao_gps_char = '\n'; - else - ao_gps_char = ao_serial_getchar(); - ao_gps_cksum ^= ao_gps_char; + uint8_t c = ao_sirf_byte(); + --ao_sirf_len; + ao_sirf_cksum += c; + return c; } -void -ao_gps_skip(void) +static void sirf_u16(uint8_t offset) { - while (ao_gps_char >= '0') - ao_gps_lexchar(); + uint16_t __xdata *ptr = (uint16_t __xdata *) (((char __xdata *) &ao_sirf_data) + offset); + uint16_t val; + + val = data_byte() << 8; + val |= data_byte (); + *ptr = val; } -void -ao_gps_skip_field(void) +static void sirf_u8(uint8_t offset) { - while (ao_gps_char != ',' && ao_gps_char != '*' && ao_gps_char != '\n') - ao_gps_lexchar(); + uint8_t __xdata *ptr = (uint8_t __xdata *) (((char __xdata *) &ao_sirf_data) + offset); + uint8_t val; + + val = data_byte (); + *ptr = val; } -void -ao_gps_skip_sep(void) +static void sirf_u32(uint8_t offset) { - while (ao_gps_char == ',' || ao_gps_char == '.' || ao_gps_char == '*') - ao_gps_lexchar(); + uint32_t __xdata *ptr = (uint32_t __xdata *) (((char __xdata *) &ao_sirf_data) + offset); + uint32_t val; + + val = ((uint32_t) data_byte ()) << 24; + val |= ((uint32_t) data_byte ()) << 16; + val |= ((uint32_t) data_byte ()) << 8; + val |= ((uint32_t) data_byte ()); + *ptr = val; } -__xdata static uint8_t ao_gps_num_width; - -static int16_t -ao_gps_decimal(uint8_t max_width) +static void sirf_discard(uint8_t len) { - int16_t v; - __xdata uint8_t neg = 0; - - ao_gps_skip_sep(); - if (ao_gps_char == '-') { - neg = 1; - ao_gps_lexchar(); - } - v = 0; - ao_gps_num_width = 0; - while (ao_gps_num_width < max_width) { - if (ao_gps_char < '0' || '9' < ao_gps_char) - break; - v = v * (int16_t) 10 + ao_gps_char - '0'; - ao_gps_num_width++; - ao_gps_lexchar(); - } - if (neg) - v = -v; - return v; + while (len--) + data_byte(); } -static uint8_t -ao_gps_hex(uint8_t max_width) +#define SIRF_END 0 +#define SIRF_DISCARD 1 +#define SIRF_U8 2 +#define SIRF_U16 3 +#define SIRF_U32 4 + +struct sirf_packet_parse { + uint8_t type; + uint8_t offset; +}; + +static const struct sirf_packet_parse geodetic_nav_data_packet[] = { + { SIRF_DISCARD, 2 }, /* 1 nav valid */ + { SIRF_U16, offsetof(struct sirf_geodetic_nav_data, nav_type) }, /* 3 */ + { SIRF_DISCARD, 6 }, /* 5 week number, time of week */ + { SIRF_U16, offsetof(struct sirf_geodetic_nav_data, utc_year) }, /* 11 */ + { SIRF_U8, offsetof(struct sirf_geodetic_nav_data, utc_month) }, /* 13 */ + { SIRF_U8, offsetof(struct sirf_geodetic_nav_data, utc_day) }, /* 14 */ + { SIRF_U8, offsetof(struct sirf_geodetic_nav_data, utc_hour) }, /* 15 */ + { SIRF_U8, offsetof(struct sirf_geodetic_nav_data, utc_minute) }, /* 16 */ + { SIRF_U16, offsetof(struct sirf_geodetic_nav_data, utc_second) }, /* 17 */ + { SIRF_DISCARD, 4 }, /* satellite id list */ /* 19 */ + { SIRF_U32, offsetof(struct sirf_geodetic_nav_data, lat) }, /* 23 */ + { SIRF_U32, offsetof(struct sirf_geodetic_nav_data, lon) }, /* 27 */ + { SIRF_DISCARD, 4 }, /* altitude from ellipsoid */ /* 31 */ + { SIRF_U32, offsetof(struct sirf_geodetic_nav_data, alt_msl) }, /* 35 */ + { SIRF_DISCARD, 1 }, /* map datum */ /* 39 */ + { SIRF_U16, offsetof(struct sirf_geodetic_nav_data, ground_speed) }, /* 40 */ + { SIRF_U16, offsetof(struct sirf_geodetic_nav_data, course) }, /* 42 */ + { SIRF_DISCARD, 2 }, /* magnetic variation */ /* 44 */ + { SIRF_U16, offsetof(struct sirf_geodetic_nav_data, climb_rate) }, /* 46 */ + { SIRF_DISCARD, 2 }, /* turn rate */ /* 48 */ + { SIRF_U32, offsetof(struct sirf_geodetic_nav_data, h_error) }, /* 50 */ + { SIRF_U32, offsetof(struct sirf_geodetic_nav_data, v_error) }, /* 54 */ + { SIRF_DISCARD, 30 }, /* time error, h_vel error, clock_bias, + clock bias error, clock drift, + clock drift error, distance, + distance error, heading error */ /* 58 */ + { SIRF_U8, offsetof(struct sirf_geodetic_nav_data, num_sv) }, /* 88 */ + { SIRF_U8, offsetof(struct sirf_geodetic_nav_data, hdop) }, /* 89 */ + { SIRF_DISCARD, 1 }, /* additional mode info */ /* 90 */ + { SIRF_END, 0 }, /* 91 */ +}; + +static void +ao_sirf_parse_41(void) { - uint8_t v, d; - - ao_gps_skip_sep(); - v = 0; - ao_gps_num_width = 0; - while (ao_gps_num_width < max_width) { - if ('0' <= ao_gps_char && ao_gps_char <= '9') - d = ao_gps_char - '0'; - else if ('A' <= ao_gps_char && ao_gps_char <= 'F') - d = ao_gps_char - 'A' + 10; - else if ('a' <= ao_gps_char && ao_gps_char <= 'f') - d = ao_gps_char - 'a' + 10; - else + uint8_t i, offset; + + for (i = 0; ; i++) { + offset = geodetic_nav_data_packet[i].offset; + switch (geodetic_nav_data_packet[i].type) { + case SIRF_END: + return; + case SIRF_DISCARD: + sirf_discard(offset); + break; + case SIRF_U8: + sirf_u8(offset); + break; + case SIRF_U16: + sirf_u16(offset); + break; + case SIRF_U32: + sirf_u32(offset); break; - v = (v << 4) | d; - ao_gps_num_width++; - ao_gps_lexchar(); + } } - return v; } -static void -ao_gps_parse_pos(__xdata struct ao_gps_pos * pos, uint8_t deg_width) __reentrant +void +ao_gps_setup(void) __reentrant { - pos->degrees = ao_gps_decimal(deg_width); - pos->minutes = ao_gps_decimal(2); - if (ao_gps_char == '.') { - pos->minutes_fraction = ao_gps_decimal(4); - while (ao_gps_num_width < 4) { - pos->minutes_fraction *= 10; - ao_gps_num_width++; - } - } else { - pos->minutes_fraction = 0; - if (ao_gps_char != ',') - ao_gps_error = 1; - } + uint8_t i; + ao_serial_set_speed(AO_SERIAL_SPEED_4800); + for (i = 0; i < 16; i++) + ao_serial_putchar(0x00); + for (i = 0; i < sizeof (ao_gps_set_nmea) - 1; i++) + ao_serial_putchar(ao_gps_set_nmea[i]); + ao_serial_set_speed(AO_SERIAL_SPEED_57600); + for (i = 0; i < 16; i++) + ao_serial_putchar(0x00); } -static void -ao_gps_parse_flag(char yes_c, uint8_t yes, char no_c, uint8_t no) __reentrant +static const char ao_gps_set_message_rate[] = { + 0xa0, 0xa2, 0x00, 0x08, + 166, + 0, +}; + +void +ao_sirf_set_message_rate(uint8_t msg, uint8_t rate) { - ao_gps_skip_sep(); - if (ao_gps_char == yes_c) - ao_gps_next.flags |= yes; - else if (ao_gps_char == no_c) - ao_gps_next.flags |= no; - else - ao_gps_error = 1; - ao_gps_lexchar(); + uint16_t cksum = 0x00a6; + uint8_t i; + + for (i = 0; i < sizeof (ao_gps_set_message_rate); i++) + ao_serial_putchar(ao_gps_set_message_rate[i]); + ao_serial_putchar(msg); + ao_serial_putchar(rate); + cksum = 0xa6 + msg + rate; + for (i = 0; i < 4; i++) + ao_serial_putchar(0); + ao_serial_putchar((cksum >> 8) & 0x7f); + ao_serial_putchar(cksum & 0xff); + ao_serial_putchar(0xb0); + ao_serial_putchar(0xb3); } +static const uint8_t sirf_disable[] = { + 2, + 4, + 9, + 10, + 27, + 50, + 52, +}; void ao_gps(void) __reentrant { - char c; - uint8_t i; - - for (i = 0; (c = ao_gps_config[i]); i++) - ao_serial_putchar(c); + uint8_t i, k; + uint16_t cksum; + + for (k = 0; k < 5; k++) + { + for (i = 0; i < sizeof (ao_gps_config); i++) + ao_serial_putchar(ao_gps_config[i]); + for (i = 0; i < sizeof (sirf_disable); i++) + ao_sirf_set_message_rate(sirf_disable[i], 0); + ao_sirf_set_message_rate(41, 1); + } for (;;) { /* Locate the begining of the next record */ - for (;;) { - c = ao_serial_getchar(); - if (c == '$') - break; - } + while (ao_sirf_byte() != (uint8_t) 0xa0) + ; + if (ao_sirf_byte() != (uint8_t) 0xa2) + continue; - ao_gps_cksum = 0; - ao_gps_error = 0; + /* Length */ + ao_sirf_len = ao_sirf_byte() << 8; + ao_sirf_len |= ao_sirf_byte(); + if (ao_sirf_len > 1023) + continue; - /* Skip anything other than GGA */ - for (i = 0; i < AO_GPS_LEADER; i++) { - ao_gps_lexchar(); - if (ao_gps_char != ao_gps_header[i]) + ao_sirf_cksum = 0; + + /* message ID */ + i = data_byte (); /* 0 */ + + switch (i) { + case 41: + if (ao_sirf_len < 90) break; + ao_sirf_parse_41(); + break; } - if (i != AO_GPS_LEADER) + if (ao_sirf_len != 0) continue; - /* Now read the data into the gps data record - * - * $GPGGA,025149.000,4528.1723,N,12244.2480,W,1,05,2.0,103.5,M,-19.5,M,,0000*66 - * - * Essential fix data - * - * 025149.000 time (02:51:49.000 GMT) - * 4528.1723,N Latitude 45°28.1723' N - * 12244.2480,W Longitude 122°44.2480' W - * 1 Fix quality: - * 0 = invalid - * 1 = GPS fix (SPS) - * 2 = DGPS fix - * 3 = PPS fix - * 4 = Real Time Kinematic - * 5 = Float RTK - * 6 = estimated (dead reckoning) - * 7 = Manual input mode - * 8 = Simulation mode - * 05 Number of satellites (5) - * 2.0 Horizontal dilution - * 103.5,M Altitude, 103.5M above msl - * -19.5,M Height of geoid above WGS84 ellipsoid - * ? time in seconds since last DGPS update - * 0000 DGPS station ID - * *66 checksum - */ - - ao_gps_next.flags = 0; - ao_gps_next.hour = ao_gps_decimal(2); - ao_gps_next.minute = ao_gps_decimal(2); - ao_gps_next.second = ao_gps_decimal(2); - ao_gps_skip_field(); /* skip seconds fraction */ - - ao_gps_parse_pos(&ao_gps_next.latitude, 2); - ao_gps_parse_flag('N', AO_GPS_LATITUDE_NORTH, 'S', AO_GPS_LATITUDE_SOUTH); - ao_gps_parse_pos(&ao_gps_next.longitude, 3); - ao_gps_parse_flag('W', AO_GPS_LONGITUDE_WEST, 'E', AO_GPS_LONGITUDE_EAST); - - i = ao_gps_decimal(0xff); - if (i == 1) - ao_gps_next.flags |= AO_GPS_VALID; - - i = ao_gps_decimal(0xff) << AO_GPS_NUM_SAT_SHIFT; - if (i > AO_GPS_NUM_SAT_MASK) - i = AO_GPS_NUM_SAT_MASK; - ao_gps_next.flags |= i; - - ao_gps_lexchar(); - ao_gps_skip_field(); /* Horizontal dilution */ - - ao_gps_next.altitude = ao_gps_decimal(0xff); - ao_gps_skip_field(); /* skip any fractional portion */ - - /* Skip remaining fields */ - while (ao_gps_char != '*' && ao_gps_char != '\n' && ao_gps_char != '\r') { - ao_gps_lexchar(); - ao_gps_skip_field(); - } - if (ao_gps_char == '*') { - uint8_t cksum = ao_gps_cksum ^ '*'; - if (cksum != ao_gps_hex(2)) - ao_gps_error = 1; - } else - ao_gps_error = 1; - if (!ao_gps_error) { + /* verify checksum and end sequence */ + ao_sirf_cksum &= 0x7fff; + cksum = ao_sirf_byte() << 8; + cksum |= ao_sirf_byte(); + if (ao_sirf_cksum != cksum) + continue; + if (ao_sirf_byte() != (uint8_t) 0xb0) + continue; + if (ao_sirf_byte() != (uint8_t) 0xb3) + continue; + + switch (i) { + case 41: ao_mutex_get(&ao_gps_mutex); - memcpy(&ao_gps_data, &ao_gps_next, sizeof (struct ao_gps_data)); + ao_gps_data.hour = ao_sirf_data.utc_hour; + ao_gps_data.minute = ao_sirf_data.utc_minute; + ao_gps_data.second = ao_sirf_data.utc_second / 1000; + ao_gps_data.flags = ((ao_sirf_data.num_sv << AO_GPS_NUM_SAT_SHIFT) & AO_GPS_NUM_SAT_MASK) | AO_GPS_RUNNING; + if ((ao_sirf_data.nav_type & NAV_TYPE_GPS_FIX_TYPE_MASK) >= NAV_TYPE_4_SV_KF) + ao_gps_data.flags |= AO_GPS_VALID; + ao_gps_data.latitude = ao_sirf_data.lat; + ao_gps_data.longitude = ao_sirf_data.lon; + ao_gps_data.altitude = ao_sirf_data.alt_msl / 100; + ao_gps_data.ground_speed = ao_sirf_data.ground_speed; + ao_gps_data.course = ao_sirf_data.course / 200; + ao_gps_data.hdop = ao_sirf_data.hdop; + ao_gps_data.climb_rate = ao_sirf_data.climb_rate; + if (ao_sirf_data.h_error > 6553500) + ao_gps_data.h_error = 65535; + else + ao_gps_data.h_error = ao_sirf_data.h_error / 100; + if (ao_sirf_data.v_error > 6553500) + ao_gps_data.v_error = 65535; + else + ao_gps_data.v_error = ao_sirf_data.v_error / 100; ao_mutex_put(&ao_gps_mutex); ao_wakeup(&ao_gps_data); + break; } } }