summaryrefslogtreecommitdiff
diff options
context:
space:
mode:
Diffstat
-rw-r--r--src/ao_gps.c68
-rw-r--r--src/ao_gps_test.c188
2 files changed, 230 insertions, 26 deletions
diff --git a/src/ao_gps.c b/src/ao_gps.c
index f83361eb..b6d7182d 100644
--- a/src/ao_gps.c
+++ b/src/ao_gps.c
@@ -56,22 +56,6 @@ const char ao_gps_config[] = {
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 */
- 0, /* message id (ignored) */
- 0, /* update rate (0 = disable) */
- 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,
};
#define NAV_TYPE_GPS_FIX_TYPE_MASK (7 << 0)
@@ -224,27 +208,21 @@ 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;
}
@@ -259,11 +237,53 @@ ao_gps_setup(void) __reentrant
#ifdef AO_GPS_TEST
ao_serial_set_speed(j);
#endif
+ for (i = 255; i != 0; i--)
+ ao_serial_putchar(0);
+
for (i = 0; i < sizeof (ao_gps_set_binary); i++)
ao_serial_putchar(ao_gps_set_binary[i]);
}
}
+static const char ao_gps_set_message_rate[] = {
+ 0xa0, 0xa2, 0x00, 0x08,
+ 166,
+ 0,
+#define SET_MESSAGE_RATE_ID 6
+#define SET_MESSAGE_RATE 7
+
+};
+
+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,
+ 4,
+};
+
void
ao_gps(void) __reentrant
{
@@ -272,6 +292,9 @@ ao_gps(void) __reentrant
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)
@@ -336,7 +359,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;
diff --git a/src/ao_gps_test.c b/src/ao_gps_test.c
index 7489e788..35cce7de 100644
--- a/src/ao_gps_test.c
+++ b/src/ao_gps_test.c
@@ -98,6 +98,145 @@ ao_serial_getchar(void)
return c;
}
+static void
+check_sirf_message(char *from, uint8_t *msg, int len)
+{
+ uint16_t encoded_len, encoded_cksum;
+ uint16_t cksum;
+ uint8_t id;
+ int i;
+
+ if (msg[0] != 0xa0 || msg[1] != 0xa2) {
+ printf ("bad header\n");
+ return;
+ }
+ if (len < 7) {
+ printf("short\n");
+ return;
+ }
+ if (msg[len-1] != 0xb3 || msg[len-2] != 0xb0) {
+ printf ("bad trailer\n");
+ return;
+ }
+ encoded_len = (msg[2] << 8) | msg[3];
+ id = msg[4];
+ if (encoded_len != len - 8) {
+ printf ("length mismatch (got %d, wanted %d)\n",
+ len - 8, encoded_len);
+ return;
+ }
+ encoded_cksum = (msg[len - 4] << 8) | msg[len-3];
+ cksum = 0;
+ for (i = 4; i < len - 4; i++)
+ cksum = (cksum + msg[i]) & 0x7fff;
+ if (encoded_cksum != cksum) {
+ printf ("cksum mismatch (got %04x wanted %04x)\n",
+ cksum, encoded_cksum);
+ return;
+ }
+ id = msg[4];
+ if (id == 41) {
+ int off = 4;
+
+ uint8_t id;
+ uint16_t nav_valid;
+ uint16_t nav_type;
+ uint16_t week;
+ uint32_t tow;
+ uint16_t year;
+ uint8_t month;
+ uint8_t day;
+ uint8_t hour;
+ uint8_t minute;
+ uint16_t second;
+ uint32_t sat_list;
+ int32_t lat;
+ int32_t lon;
+ int32_t alt_ell;
+ int32_t alt_msl;
+ int8_t datum;
+ uint16_t sog;
+ uint16_t cog;
+ int16_t mag_var;
+ int16_t climb_rate;
+ int16_t heading_rate;
+ uint32_t h_error;
+ uint32_t v_error;
+ uint32_t t_error;
+ uint16_t h_v_error;
+
+#define get_u8(u) u = (msg[off]); off+= 1
+#define get_u16(u) u = (msg[off] << 8) | (msg[off + 1]); off+= 2
+#define get_u32(u) u = (msg[off] << 24) | (msg[off + 1] << 16) | (msg[off+2] << 8) | (msg[off+3]); off+= 4
+
+ get_u8(id);
+ get_u16(nav_valid);
+ get_u16(nav_type);
+ get_u16(week);
+ get_u32(tow);
+ get_u16(year);
+ get_u8(month);
+ get_u8(day);
+ get_u8(hour);
+ get_u8(minute);
+ get_u16(second);
+ get_u32(sat_list);
+ get_u32(lat);
+ get_u32(lon);
+ get_u32(alt_ell);
+ get_u32(alt_msl);
+ get_u8(datum);
+ get_u16(sog);
+ get_u16(cog);
+ get_u16(mag_var);
+ get_u16(climb_rate);
+ get_u16(heading_rate);
+ get_u32(h_error);
+ get_u32(v_error);
+ get_u32(t_error);
+ get_u16(h_v_error);
+
+
+ printf ("Geodetic Navigation Data (41):\n");
+ printf ("\tNav valid %04x\n", nav_valid);
+ printf ("\tNav type %04x\n", nav_type);
+ printf ("\tWeek %d\n", week);
+ printf ("\tTOW %d\n", tow);
+ printf ("\tyear %d\n", year);
+ printf ("\tmonth %d\n", month);
+ printf ("\tday %d\n", day);
+ printf ("\thour %d\n", hour);
+ printf ("\tminute %d\n", minute);
+ printf ("\tsecond %g\n", second / 1000.0);
+ printf ("\tsats: %08x\n", sat_list);
+ printf ("\tlat: %g\n", lat / 1.0e7);
+ printf ("\tlon: %g\n", lon / 1.0e7);
+ printf ("\talt_ell: %g\n", alt_ell / 100.0);
+ printf ("\talt_msll: %g\n", alt_msl / 100.0);
+ printf ("\tdatum: %d\n", datum);
+ printf ("\tground speed: %g\n", sog / 100.0);
+ printf ("\tcourse: %g\n", cog / 100.0);
+ printf ("\tclimb: %g\n", climb_rate / 100.0);
+ printf ("\theading rate: %g\n", heading_rate / 100.0);
+ printf ("\th error: %g\n", h_error / 100.0);
+ printf ("\tv error: %g\n", v_error / 100.0);
+ printf ("\tt error: %g\n", t_error / 100.0);
+ printf ("\th vel error: %g\n", h_v_error / 100.0);
+ } else {
+ return;
+ printf ("%s %4d:", from, encoded_len);
+ for (i = 4; i < len - 4; i++) {
+ if (((i - 4) & 0xf) == 0)
+ printf("\n ");
+ printf (" %3d", msg[i]);
+ }
+ printf ("\n");
+ }
+}
+
+static uint8_t sirf_in_message[4096];
+static int sirf_in_len;
+
void *
ao_gps_input(void *arg)
{
@@ -109,7 +248,16 @@ ao_gps_input(void *arg)
i = read(ao_gps_fd, &c, 1);
if (i == 1) {
int v;
+ uint8_t uc = c;
+ if (sirf_in_len || uc == 0xa0) {
+ if (sirf_in_len < 4096)
+ sirf_in_message[sirf_in_len++] = uc;
+ if (uc == 0xb3) {
+ check_sirf_message("recv", sirf_in_message, sirf_in_len);
+ sirf_in_len = 0;
+ }
+ }
input_queue[input_tail] = c;
input_tail = (input_tail + 1) % QUEUE_LEN;
sem_post(&input_semaphore);
@@ -125,16 +273,32 @@ ao_gps_input(void *arg)
}
}
+static uint8_t sirf_message[4096];
+static int sirf_message_len;
+
void
ao_serial_putchar(char c)
{
int i;
+ uint8_t uc = (uint8_t) c;
- ao_dbg_char(c);
+ if (sirf_message_len || uc == 0xa0) {
+ if (sirf_message_len < 4096)
+ sirf_message[sirf_message_len++] = uc;
+ if (uc == 0xb3) {
+ check_sirf_message("send", sirf_message, sirf_message_len);
+ sirf_message_len = 0;
+ }
+ }
for (;;) {
i = write(ao_gps_fd, &c, 1);
- if (i == 1)
+ if (i == 1) {
+ if ((uint8_t) c == 0xb3 || c == '\r') {
+ tcdrain(ao_gps_fd);
+ usleep (1000 * 100);
+ }
break;
+ }
if (i < 0 && (errno == EINTR || errno == EAGAIN))
continue;
perror("putchar");
@@ -161,7 +325,25 @@ ao_serial_set_speed(uint8_t fast)
void
ao_dump_state(void)
{
- printf ("dump state\n");
+ double lat, lon;
+ printf ("%02d:%02d:%02d",
+ ao_gps_data.hour, ao_gps_data.minute,
+ ao_gps_data.second);
+ printf (" nsat %d %svalid",
+ ao_gps_data.flags & AO_GPS_NUM_SAT_MASK,
+ ao_gps_data.flags & AO_GPS_VALID ? "" : "not ");
+ printf (" lat %g lon %g alt %d",
+ ao_gps_data.latitude / 1.0e7,
+ ao_gps_data.longitude / 1.0e7,
+ ao_gps_data.altitude);
+ printf (" speed %g climb %g course %d",
+ ao_gps_data.ground_speed / 100.0,
+ ao_gps_data.climb_rate / 100.0,
+ ao_gps_data.course * 2);
+ printf (" hdop %g h_error %d v_error %d",
+ ao_gps_data.hdop / 5.0,
+ ao_gps_data.h_error, ao_gps_data.v_error);
+ printf("\n");
}
int
generated by cgit v1.2.3 (git 2.39.1) at 2025-10-29 23:03:58 +0000