X-Git-Url: https://git.gag.com/?p=fw%2Faltos;a=blobdiff_plain;f=src%2Fao_gps_skytraq.c;h=7ac269469d31c2bc90c42c6d92c0c9b5adb4b95e;hp=88509a3d6c0da48a7873104520869760e3de6aa7;hb=e268798dc260311f5f0167909481b41c9d27fc1c;hpb=b080e933a65d268aaaec8cfd5f617a13d5babc43 diff --git a/src/ao_gps_skytraq.c b/src/ao_gps_skytraq.c index 88509a3d..7ac26946 100644 --- a/src/ao_gps_skytraq.c +++ b/src/ao_gps_skytraq.c @@ -21,21 +21,21 @@ #define AO_GPS_LEADER 2 -static const char ao_gps_header[] = "GP"; +static __code char ao_gps_header[] = "GP"; __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; +static __data char ao_gps_char; +static __pdata uint8_t ao_gps_cksum; +static __pdata uint8_t ao_gps_error; -__xdata uint16_t ao_gps_tick; -__xdata struct ao_gps_data ao_gps_data; -__xdata struct ao_gps_tracking_data ao_gps_tracking_data; +__pdata uint16_t ao_gps_tick; +__xdata struct ao_telemetry_location ao_gps_data; +__xdata struct ao_telemetry_satellite ao_gps_tracking_data; -static __xdata uint16_t ao_gps_next_tick; -static __xdata struct ao_gps_data ao_gps_next; -static __xdata uint8_t ao_gps_date_flags; -static __xdata struct ao_gps_tracking_data ao_gps_tracking_next; +static __pdata uint16_t ao_gps_next_tick; +static __xdata struct ao_telemetry_location ao_gps_next; +static __pdata uint8_t ao_gps_date_flags; +static __xdata struct ao_telemetry_satellite ao_gps_tracking_next; #define STQ_S 0xa0, 0xa1 #define STQ_E 0x0d, 0x0a @@ -49,14 +49,7 @@ static __xdata struct ao_gps_tracking_data ao_gps_tracking_next; STQ_S, 0,15, id, a,b,c,d,e,f,g,h,i,j,k,l,m,n, \ (id^a^b^c^d^e^f^g^h^i^j^k^l^m^n), STQ_E -static const uint8_t ao_gps_config_serial[] = { - SKYTRAQ_MSG_3(0x05, 0, 4, 0), /* set serial port */ - /* 0 = com1 */ - /* 0 = 4800, 1 = 9600, 2 = 19200, 3 = 38400, - * 4 = 57600, 5 = 115200 */ -}; - -static const uint8_t ao_gps_config[] = { +static __code uint8_t ao_gps_config[] = { SKYTRAQ_MSG_8(0x08, 1, 1, 1, 1, 1, 1, 1, 0), /* configure nmea */ /* gga interval */ /* gsa interval */ @@ -70,10 +63,6 @@ static const uint8_t ao_gps_config[] = { SKYTRAQ_MSG_2(0x3c, 0x00, 0x00), /* configure navigation mode */ /* 0 = car, 1 = pedestrian */ /* 0 = update to sram, 1 = update sram + flash */ - - SKYTRAQ_MSG_2(0x0e, 10, 0), /* config nav interval */ - /* interval */ - /* 0 = update to sram, 1 = update sram */ }; static void @@ -100,13 +89,13 @@ ao_gps_skip_sep(void) ao_gps_lexchar(); } -__xdata static uint8_t ao_gps_num_width; +__pdata static uint8_t ao_gps_num_width; static int16_t ao_gps_decimal(uint8_t max_width) { int16_t v; - __xdata uint8_t neg = 0; + __pdata uint8_t neg = 0; ao_gps_skip_sep(); if (ao_gps_char == '-') { @@ -249,14 +238,14 @@ ao_nmea_gga() ao_gps_next.flags |= i; ao_gps_lexchar(); - ao_gps_next.hdop = ao_gps_decimal(0xff); - if (ao_gps_next.hdop <= 50) { - ao_gps_next.hdop = (uint8_t) 5 * ao_gps_next.hdop; + i = ao_gps_decimal(0xff); + if (i <= 50) { + i = (uint8_t) 5 * i; if (ao_gps_char == '.') - ao_gps_next.hdop = (ao_gps_next.hdop + - ((uint8_t) ao_gps_decimal(1) >> 1)); + i = (i + ((uint8_t) ao_gps_decimal(1) >> 1)); } else - ao_gps_next.hdop = 255; + i = 255; + ao_gps_next.hdop = i; ao_gps_skip_field(); ao_gps_next.altitude = ao_gps_decimal(0xff); @@ -276,7 +265,7 @@ ao_nmea_gga() if (!ao_gps_error) { ao_mutex_get(&ao_gps_mutex); ao_gps_tick = ao_gps_next_tick; - memcpy(&ao_gps_data, &ao_gps_next, sizeof (struct ao_gps_data)); + memcpy(&ao_gps_data, &ao_gps_next, sizeof (ao_gps_data)); ao_mutex_put(&ao_gps_mutex); ao_wakeup(&ao_gps_data); } @@ -323,9 +312,8 @@ ao_nmea_gsv(void) ao_gps_skip_field(); /* azimuth */ c = ao_gps_decimal(2); /* C/N0 */ if (i < AO_MAX_GPS_TRACKING) { - if (!(ao_gps_tracking_next.sats[i].c_n_1 = c)) - ao_gps_tracking_next.sats[i].svid = 0; - ao_gps_tracking_next.channels = i + 1; + if ((ao_gps_tracking_next.sats[i].c_n_1 = c) != 0) + ao_gps_tracking_next.channels = i + 1; } } if (ao_gps_char == '*') { @@ -402,15 +390,16 @@ ao_nmea_rmc(void) } } -#define ao_skytraq_sendstruct(s) ao_skytraq_sendbytes((s), (s)+sizeof(s)) +#define ao_skytraq_sendstruct(s) ao_skytraq_sendbytes((s), sizeof(s)) static void -ao_skytraq_sendbytes(const uint8_t *b, const uint8_t *e) +ao_skytraq_sendbytes(__code uint8_t *b, uint8_t l) { - while (b != e) { - if (*b == 0xa0) + while (l--) { + uint8_t c = *b++; + if (c == 0xa0) ao_delay(AO_MS_TO_TICKS(500)); - ao_serial_putchar(*b++); + ao_serial_putchar(c); } } @@ -455,9 +444,6 @@ ao_gps(void) __reentrant /* give skytraq time to boot in case of cold start */ ao_delay(AO_MS_TO_TICKS(2000)); - ao_skytraq_sendstruct(ao_gps_config_serial); - ao_delay(AO_MS_TO_TICKS(1000)); - ao_serial_set_speed(AO_SERIAL_SPEED_57600); ao_skytraq_sendstruct(ao_gps_config); @@ -475,18 +461,25 @@ __xdata struct ao_task ao_gps_task; static void gps_dump(void) __reentrant { + uint8_t i; ao_mutex_get(&ao_gps_mutex); printf ("Date: %02d/%02d/%02d\n", ao_gps_data.year, ao_gps_data.month, ao_gps_data.day); printf ("Time: %02d:%02d:%02d\n", ao_gps_data.hour, ao_gps_data.minute, ao_gps_data.second); printf ("Lat/Lon: %ld %ld\n", ao_gps_data.latitude, ao_gps_data.longitude); printf ("Alt: %d\n", ao_gps_data.altitude); printf ("Flags: 0x%x\n", ao_gps_data.flags); + printf ("Sats: %d", ao_gps_tracking_data.channels); + for (i = 0; i < ao_gps_tracking_data.channels; i++) + printf (" %d %d", + ao_gps_tracking_data.sats[i].svid, + ao_gps_tracking_data.sats[i].c_n_1); + printf ("\ndone\n"); ao_mutex_put(&ao_gps_mutex); } __code struct ao_cmds ao_gps_cmds[] = { - { 'g', gps_dump, "g Display current GPS values" }, - { 0, gps_dump, NULL }, + { gps_dump, "g\0Display GPS" }, + { 0, NULL }, }; void