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)
{
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);
}
}
+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");
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