X-Git-Url: https://git.gag.com/?p=fw%2Faltos;a=blobdiff_plain;f=src%2Fao_gps.c;h=c06c635fd9fc3d67bf560671cca0572fbde8e974;hp=f83361eb5d9cdcbedaa571059787e1365d7030be;hb=08f37056deec25d77062bc411a04033401b033a5;hpb=0b35447d05a0c7eaf4fefcbcf0065fe3320bba82 diff --git a/src/ao_gps.c b/src/ao_gps.c index f83361eb..c06c635f 100644 --- a/src/ao_gps.c +++ b/src/ao_gps.c @@ -22,22 +22,27 @@ __xdata uint8_t ao_gps_mutex; __xdata struct ao_gps_data ao_gps_data; -const char ao_gps_set_binary[] = { - '$', 'P', 'S', 'R', 'F', '1', '0', '0', ',', '0', ',', - '9', '6', '0', '0', ',', '8', ',', '1', ',', '0', '*', - '0', 'C', '\r','\n', - - 0xa0, 0xa2, 0x00, 0x09, /* length 9 bytes */ - 134, /* Set binary serial port */ - 0, 0, 0x25, 0x80, /* 9600 baud */ - 8, /* data bits */ - 1, /* stop bits */ - 0, /* parity */ - 0, /* pad */ - 0x01, 0x34, 0xb0, 0xb3, -}; +#undef AO_GPS_4800 + +#ifdef AO_GPS_4800 +static const char ao_gps_set_nmea[] = "$PSRF100,0,57600,8,1,0*37\r\n"; +#else +static const char ao_gps_set_nmea[] = "$PSRF100,0,4800,8,1,0*0F\r\n"; +#endif const char ao_gps_config[] = { + 0xa0, 0xa2, 0x00, 0x19, /* length: 25 bytes */ + 128, /* Initialize Data Source */ + 0, 0, 0, 0, /* ECEF X */ + 0, 0, 0, 0, /* ECEF Y */ + 0, 0, 0, 0, /* ECEF Z */ + 0, 0, 0, 0, /* Clock Drift */ + 0, 0, 0, 0, /* Time of Week */ + 0, 0, /* Week Number */ + 0, /* Channels */ + 0xc6, /* Clear user data, RTC not accurate, Clear history, clear ephemeris */ + 0x01, 0x46, 0xb0, 0xb3, + 0xa0, 0xa2, 0x00, 0x0e, /* length: 14 bytes */ 136, /* mode control */ 0, 0, /* reserved */ @@ -52,11 +57,6 @@ const char ao_gps_config[] = { 0, /* Track smoothing (disabled) */ 0x00, 0x8e, 0xb0, 0xb3, - 0xa0, 0xa2, 0x00, 0x02, /* length: 2 bytes */ - 143, /* static navigation */ - 0, /* disable */ - 0x00, 0x8f, 0xb0, 0xb3, - 0xa0, 0xa2, 0x00, 0x08, /* length: 8 bytes */ 166, /* Set message rate */ 2, /* enable/disable all messages */ @@ -65,13 +65,10 @@ const char ao_gps_config[] = { 0, 0, 0, 0, /* reserved */ 0x00, 0xa8, 0xb0, 0xb3, - 0xa0, 0xa2, 0x00, 0x08, /* length: 8 bytes */ - 166, /* Set message rate */ - 0, /* enable/disable one message */ - 41, /* message 41 */ - 1, /* once per second */ - 0, 0, 0, 0, /* reserved */ - 0x00, 0xd0, 0xb0, 0xb3, + 0xa0, 0xa2, 0x00, 0x02, /* length: 2 bytes */ + 143, /* static navigation */ + 0, /* disable */ + 0x00, 0x8f, 0xb0, 0xb3, }; #define NAV_TYPE_GPS_FIX_TYPE_MASK (7 << 0) @@ -224,59 +221,101 @@ ao_sirf_parse_41(void) { uint8_t i, offset; - printf("parse 41\n"); for (i = 0; ; i++) { offset = geodetic_nav_data_packet[i].offset; switch (geodetic_nav_data_packet[i].type) { case SIRF_END: - printf("parse 41 done\n"); return; case SIRF_DISCARD: - printf("parse 41 discard %d\n", offset); sirf_discard(offset); break; case SIRF_U8: - printf("parse 41 u8 %d\n", offset); sirf_u8(offset); break; case SIRF_U16: - printf("parse 41 u16 %d\n", offset); sirf_u16(offset); break; case SIRF_U32: - printf("parse 41 u32 %d\n", offset); sirf_u32(offset); break; } } } -void +static void ao_gps_setup(void) __reentrant { - uint8_t i, j; - for (j = 0; j < 2; j++) { -#ifdef AO_GPS_TEST - ao_serial_set_speed(j); + uint8_t i, k; +#ifdef AO_GPS_4800 + ao_serial_set_speed(AO_SERIAL_SPEED_4800); #endif - for (i = 0; i < sizeof (ao_gps_set_binary); i++) - ao_serial_putchar(ao_gps_set_binary[i]); - } + for (i = 0; i < 64; i++) + ao_serial_putchar(0x00); + for (k = 0; k < 3; k++) + for (i = 0; i < sizeof (ao_gps_set_nmea); i++) + ao_serial_putchar(ao_gps_set_nmea[i]); +#ifdef AO_GPS_4800 + ao_serial_set_speed(AO_SERIAL_SPEED_57600); +#endif + for (i = 0; i < 64; i++) + ao_serial_putchar(0x00); +} + +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) +{ + 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 { - uint8_t i; + uint8_t i, k; uint16_t cksum; - for (i = 0; i < sizeof (ao_gps_config); i++) - ao_serial_putchar(ao_gps_config[i]); + ao_gps_setup(); + 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 */ - while (ao_sirf_byte() != 0xa0) + while (ao_sirf_byte() != (uint8_t) 0xa0) ; - if (ao_sirf_byte() != 0xa2) + if (ao_sirf_byte() != (uint8_t) 0xa2) continue; /* Length */ @@ -289,7 +328,6 @@ ao_gps(void) __reentrant /* message ID */ i = data_byte (); /* 0 */ - printf ("message %d len %d\n", i, ao_sirf_len); switch (i) { case 41: @@ -307,9 +345,9 @@ ao_gps(void) __reentrant cksum |= ao_sirf_byte(); if (ao_sirf_cksum != cksum) continue; - if (ao_sirf_byte() != 0xb0) + if (ao_sirf_byte() != (uint8_t) 0xb0) continue; - if (ao_sirf_byte() != 0xb3) + if (ao_sirf_byte() != (uint8_t) 0xb3) continue; switch (i) { @@ -318,7 +356,7 @@ ao_gps(void) __reentrant 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_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; @@ -336,7 +374,6 @@ ao_gps(void) __reentrant ao_gps_data.v_error = 65535; else ao_gps_data.v_error = ao_sirf_data.v_error / 100; - ao_gps_data.h_error = ao_sirf_data.h_error; ao_mutex_put(&ao_gps_mutex); ao_wakeup(&ao_gps_data); break;