*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; version 2 of the License.
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
*
* This program is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of
#include "ao_gps_ublox.h"
-__xdata uint8_t ao_gps_mutex;
-__pdata uint16_t ao_gps_tick;
-__xdata struct ao_telemetry_location ao_gps_data;
-__xdata struct ao_telemetry_satellite ao_gps_tracking_data;
+#define AO_UBLOX_DEBUG 0
-static const char ao_gps_set_nmea[] = "\r\n$PUBX,41,1,3,1,57600,0*2d\r\n";
+#ifndef AO_UBLOX_VERSION
+#define AO_UBLOX_VERSION 8
+#endif
-const char ao_gps_config[] = {
+#include <stdarg.h>
-};
+uint8_t ao_gps_new;
+uint8_t ao_gps_mutex;
+AO_TICK_TYPE ao_gps_tick;
+AO_TICK_TYPE ao_gps_utc_tick;
+struct ao_telemetry_location ao_gps_data;
+struct ao_telemetry_satellite ao_gps_tracking_data;
+
+#undef AO_SERIAL_SPEED_UBLOX
+
+#ifndef AO_SERIAL_SPEED_UBLOX
+#define AO_SERIAL_SPEED_UBLOX AO_SERIAL_SPEED_9600
+#endif
+
+#if AO_SERIAL_SPEED_UBLOX == AO_SERIAL_SPEED_57600
+#define SERIAL_SPEED_STRING "57600"
+#define SERIAL_SPEED_CHECKSUM "2d"
+#endif
+#if AO_SERIAL_SPEED_UBLOX == AO_SERIAL_SPEED_19200
+#define SERIAL_SPEED_STRING "19200"
+#define SERIAL_SPEED_CHECKSUM "23"
+#endif
+#if AO_SERIAL_SPEED_UBLOX == AO_SERIAL_SPEED_9600
+#define SERIAL_SPEED_STRING "9600"
+#define SERIAL_SPEED_CHECKSUM "16"
+#endif
+
+static const char ao_gps_set_nmea[] =
+ "\r\n$PUBX,41,1,3,1," SERIAL_SPEED_STRING ",0*" SERIAL_SPEED_CHECKSUM "\r\n";
struct ao_ublox_cksum {
uint8_t a, b;
};
-static __pdata struct ao_ublox_cksum ao_ublox_cksum;
-static __pdata uint16_t ao_ublox_len;
+static struct ao_ublox_cksum ao_ublox_cksum;
+static uint16_t ao_ublox_len;
+
+#if AO_UBLOX_DEBUG
-#ifndef ao_ublox_getchar
-#define ao_ublox_getchar ao_serial1_getchar
-#define ao_ublox_putchar ao_serial1_putchar
-#define ao_ublox_set_speed ao_serial1_set_speed
+
+#define DBG_PROTO 1
+#define DBG_CHAR 2
+#define DBG_INIT 4
+
+static uint8_t ao_gps_dbg_enable = 0;
+
+static void ao_gps_dbg(int level, char *format, ...) {
+ va_list a;
+
+ if (level & ao_gps_dbg_enable) {
+ va_start(a, format);
+ vprintf(format, a);
+ va_end(a);
+ flush();
+ }
+}
+
+#else
+#define ao_gps_dbg(fmt, ...)
#endif
-#define ao_ublox_byte() ((uint8_t) ao_ublox_getchar())
+static inline uint8_t ao_ublox_byte(void) {
+ uint8_t c = (uint8_t) ao_gps_getchar();
+ ao_gps_dbg(DBG_CHAR, " %02x", c);
+ return c;
+}
static inline void add_cksum(struct ao_ublox_cksum *cksum, uint8_t c)
{
static void ao_ublox_put_u8(uint8_t c)
{
add_cksum(&ao_ublox_cksum, c);
- ao_ublox_putchar(c);
+ ao_gps_dbg(DBG_CHAR, " (%02x)", c);
+ ao_gps_putchar(c);
}
static void ao_ublox_put_i8(int8_t c)
static void ao_ublox_put_u16(uint16_t c)
{
- ao_ublox_put_u8(c);
- ao_ublox_put_u8(c>>8);
+ ao_ublox_put_u8((uint8_t) c);
+ ao_ublox_put_u8((uint8_t) (c>>8));
}
#if 0
static void ao_ublox_put_u32(uint32_t c)
{
- ao_ublox_put_u8(c);
- ao_ublox_put_u8(c>>8);
- ao_ublox_put_u8(c>>16);
- ao_ublox_put_u8(c>>24);
+ ao_ublox_put_u8((uint8_t) c);
+ ao_ublox_put_u8((uint8_t) (c>>8));
+ ao_ublox_put_u8((uint8_t) (c>>16));
+ ao_ublox_put_u8((uint8_t) (c>>24));
}
static void ao_ublox_put_i32(int32_t c)
return header_byte();
}
-static char __xdata *ublox_target;
+static char *ublox_target;
static void ublox_u16(uint8_t offset)
{
- uint16_t __xdata *ptr = (uint16_t __xdata *) (ublox_target + offset);
+ uint16_t *ptr = (uint16_t *) (void *) (ublox_target + offset);
uint16_t val;
val = data_byte();
- val |= data_byte () << 8;
+ val |= (uint16_t) ((uint16_t) data_byte () << 8);
*ptr = val;
}
static void ublox_u8(uint8_t offset)
{
- uint8_t __xdata *ptr = (uint8_t __xdata *) (ublox_target + offset);
+ uint8_t *ptr = (uint8_t *) (ublox_target + offset);
uint8_t val;
val = data_byte ();
*ptr = val;
}
-static void ublox_u32(uint8_t offset) __reentrant
+static void ublox_u32(uint8_t offset)
{
- uint32_t __xdata *ptr = (uint32_t __xdata *) (ublox_target + offset);
+ uint32_t *ptr = (uint32_t *) (void *) (ublox_target + offset);
uint32_t val;
val = ((uint32_t) data_byte ());
};
static void
-ao_ublox_parse(void __xdata *target, const struct ublox_packet_parse *parse) __reentrant
+ao_ublox_parse(void *target, const struct ublox_packet_parse *parse)
{
uint8_t i, offset;
break;
}
}
+ ao_gps_dbg(DBG_PROTO, "\n");
}
/*
ao_ublox_parse(&nav_dop, nav_dop_packet);
}
+static const struct ublox_packet_parse ack_ack_packet[] = {
+ { UBLOX_DISCARD, 2 }, /* 0 class ID, msg ID */
+ { UBLOX_END, 0 },
+};
+
+static void
+ao_ublox_parse_ack_ack(void)
+{
+ ao_ublox_parse(NULL, ack_ack_packet);
+}
+
+static struct ack_nak {
+ uint8_t class_id;
+ uint8_t msg_id;
+} ack_nak;
+
+static const struct ublox_packet_parse ack_nak_packet[] = {
+ { UBLOX_U8, offsetof(struct ack_nak, class_id) }, /* 0 class ID */
+ { UBLOX_U8, offsetof(struct ack_nak, msg_id) }, /* 1 msg ID */
+ { UBLOX_END, 0 },
+};
+
+static void
+ao_ublox_parse_ack_nak(void)
+{
+ ao_ublox_parse(&ack_nak, ack_nak_packet);
+#if AO_UBLOX_DEBUG
+ ao_gps_dbg(DBG_PROTO, "NAK class 0x%02x msg 0x%02x\n",
+ ack_nak.class_id, ack_nak.msg_id);
+#endif
+}
+
+#if AO_UBLOX_VERSION < 10
/*
* NAV-POSLLH message parsing
*/
{
uint8_t nsat;
nav_svinfo_nsat = 0;
+
ao_ublox_parse(&nav_svinfo, nav_svinfo_packet);
for (nsat = 0; nsat < nav_svinfo.num_ch && ao_ublox_len >= 12; nsat++) {
if (nsat < NAV_SVINFO_MAX_SAT) {
ublox_discard(12);
}
}
+#if AO_UBLOX_DEBUG
+ ao_gps_dbg(DBG_PROTO, "svinfo num_ch %d flags %02x\n", nav_svinfo.num_ch, nav_svinfo.flags);
+ for (nsat = 0; nsat < nav_svinfo.num_ch; nsat++)
+ ao_gps_dbg(DBG_PROTO, "\t%d: chn %d svid %d flags %02x quality %d cno %d\n",
+ nsat,
+ nav_svinfo_sat[nsat].chn,
+ nav_svinfo_sat[nsat].svid,
+ nav_svinfo_sat[nsat].flags,
+ nav_svinfo_sat[nsat].quality,
+ nav_svinfo_sat[nsat].cno);
+#endif
}
/*
* NAV-TIMEUTC message parsing
*/
static struct nav_timeutc {
+ int32_t nano;
uint16_t year;
uint8_t month;
uint8_t day;
#define NAV_TIMEUTC_VALID_UTC 2
static const struct ublox_packet_parse nav_timeutc_packet[] = {
- { UBLOX_DISCARD, 12 }, /* 0 iTOW, tAcc, nano */
+ { UBLOX_DISCARD, 8 }, /* 0 iTOW, tAcc */
+ { UBLOX_U32, offsetof(struct nav_timeutc, nano) }, /* 8 nano */
{ UBLOX_U16, offsetof(struct nav_timeutc, year) }, /* 12 year */
{ UBLOX_U8, offsetof(struct nav_timeutc, month) }, /* 14 month */
{ UBLOX_U8, offsetof(struct nav_timeutc, day) }, /* 15 day */
ao_ublox_parse(&nav_velned, nav_velned_packet);
}
+#else /* AO_UBLOX_VERSION < 10 */
+
/*
- * Set the protocol mode and baud rate
+ * NAV-PVT message parsing
*/
+static struct nav_pvt {
+ uint16_t year;
+ uint8_t month;
+ uint8_t day;
+ uint8_t hour;
+ uint8_t min;
+ uint8_t sec;
+ uint8_t valid;
+ int32_t nano;
+ uint8_t flags;
+ uint8_t num_sv;
+ int32_t lat;
+ int32_t lon;
+ int32_t alt_msl;
+ int32_t vel_d;
+ int32_t g_speed;
+ int32_t heading;
+} nav_pvt;
+
+static const struct ublox_packet_parse nav_pvt_packet[] = {
+ { UBLOX_DISCARD, 4 }, /* 0 iTOW */
+ { UBLOX_U16, offsetof(struct nav_pvt, year) }, /* 4 year */
+ { UBLOX_U8, offsetof(struct nav_pvt, month) }, /* 6 month */
+ { UBLOX_U8, offsetof(struct nav_pvt, day) }, /* 7 day */
+ { UBLOX_U8, offsetof(struct nav_pvt, hour) }, /* 8 hour */
+ { UBLOX_U8, offsetof(struct nav_pvt, min) }, /* 9 min */
+ { UBLOX_U8, offsetof(struct nav_pvt, sec) }, /* 10 sec */
+ { UBLOX_U8, offsetof(struct nav_pvt, valid) }, /* 11 valid */
+ { UBLOX_DISCARD, 4 }, /* 12 tAcc */
+ { UBLOX_U32, offsetof(struct nav_pvt, nano) }, /* 16 nano */
+ { UBLOX_DISCARD, 1 }, /* 20 fixType */
+ { UBLOX_U8, offsetof(struct nav_pvt, flags) }, /* 21 gpsFix */
+ { UBLOX_DISCARD, 1 }, /* 22 flags2 */
+ { UBLOX_U8, offsetof(struct nav_pvt, num_sv) }, /* 23 numSV */
+ { UBLOX_U32, offsetof(struct nav_pvt, lon) }, /* 24 Longitude */
+ { UBLOX_U32, offsetof(struct nav_pvt, lat) }, /* 28 Latitude */
+ { UBLOX_DISCARD, 4 }, /* 32 height above ellipsoid */
+ { UBLOX_U32, offsetof(struct nav_pvt, alt_msl) }, /* 36 Height above mean sea level */
+ { UBLOX_DISCARD, 16 }, /* 40 hAcc, vAcc, velN, velE */
+ { UBLOX_U32, offsetof(struct nav_pvt, vel_d) }, /* 56 velD */
+ { UBLOX_U32, offsetof(struct nav_pvt, g_speed) }, /* 60 gSpeed */
+ { UBLOX_U32, offsetof(struct nav_pvt, heading) }, /* 64 headMot */
+ { UBLOX_DISCARD, 92 - 68 }, /* 68 sAcc .. magAcc */
+ { UBLOX_END, 0 }
+};
+
+#define NAV_PVT_VALID_DATE 0
+#define NAV_PVT_VALID_TIME 1
+#define NAV_PVT_VALID_FULLY_RESOLVED 2
+#define NAV_PVT_VALID_MAG 3
+
+#define NAV_PVT_FLAGS_GNSSFIXOK 0
+#define NAV_PVT_FLAGS_DIFFSOLN 1
+#define NAV_PVT_FLAGS_PSM_STATE 2
+#define NAV_PVT_FLAGS_HEAD_VEH_VALID 5
+#define NAV_PVT_FLAGS_CARR_SOLN 6
+
static void
-ao_gps_setup(void)
+ao_ublox_parse_nav_pvt(void)
{
- uint8_t i, k;
+ ao_ublox_parse(&nav_pvt, nav_pvt_packet);
+#if AO_UBLOX_DEBUG
+ ao_gps_dbg(DBG_PROTO, "\t%d-%d-%d %02d:%02d:%02d %ld (%02x)\n",
+ nav_pvt.year, nav_pvt.month, nav_pvt.day,
+ nav_pvt.hour, nav_pvt.min, nav_pvt.sec,
+ (long) nav_pvt.nano, nav_pvt.valid);
+ ao_gps_dbg(DBG_PROTO, "\tflags %02x numSV %d lon %ld lat %ld alt %ld\n",
+ nav_pvt.flags, nav_pvt.num_sv,
+ (long) nav_pvt.lon, (long) nav_pvt.lat, (long) nav_pvt.alt_msl);
+#endif
+}
- ao_delay(AO_SEC_TO_TICKS(3));
+/*
+ * NAV-SAT message parsing
+ */
+
+static struct nav_sat {
+ uint8_t num_svs;
+} nav_sat;
+
+static const struct ublox_packet_parse nav_sat_packet[] = {
+ { UBLOX_DISCARD, 4 }, /* 0 iTOW */
+ { UBLOX_DISCARD, 1 }, /* 4 version */
+ { UBLOX_U8, offsetof(struct nav_sat, num_svs) }, /* 4 numSvs */
+ { UBLOX_DISCARD, 2 }, /* 6 reserved0 */
+ { UBLOX_END, 0 }
+};
+
+#define NAV_SAT_MAX_SAT AO_TELEMETRY_SATELLITE_MAX_SAT
+
+static struct nav_sat_sat {
+ uint8_t svid;
+ uint8_t cno;
+} nav_sat_sat[NAV_SAT_MAX_SAT];
+
+static uint8_t nav_sat_nsat;
- ao_ublox_set_speed(AO_SERIAL_SPEED_9600);
+static struct nav_sat_real_sat {
+ uint8_t svid;
+ uint8_t cno;
+ uint32_t flags;
+} nav_sat_real_sat;
+
+static const struct ublox_packet_parse nav_sat_sat_packet[] = {
+ { UBLOX_DISCARD, 1 }, /* 8 + 12*N gnssid */
+ { UBLOX_U8, offsetof(struct nav_sat_real_sat, svid) }, /* 9 + 12*N svid */
+ { UBLOX_U8, offsetof(struct nav_sat_real_sat, cno) }, /* 10 + 12*N cno */
+ { UBLOX_DISCARD, 5 }, /* 11 + 12*N elev, azim, prRes */
+ { UBLOX_U32, offsetof(struct nav_sat_real_sat, flags) }, /* 16 + 12*N flags */
+ { UBLOX_END, 0 }
+};
+
+static uint32_t
+ao_ublox_sat_quality(struct nav_sat_real_sat *sat)
+{
+ return (sat->flags >> UBLOX_NAV_SAT_FLAGS_QUALITY) & UBLOX_NAV_SAT_FLAGS_QUALITY_MASK;
+}
+
+static uint32_t
+ao_ublox_sat_health(struct nav_sat_real_sat *sat)
+{
+ return (sat->flags >> UBLOX_NAV_SAT_FLAGS_SV_HEALTH) & UBLOX_NAV_SAT_FLAGS_SV_HEALTH_MASK;
+}
+
+static void
+ao_ublox_parse_nav_sat(void)
+{
+ uint8_t nsat;
+ nav_sat_nsat = 0;
+
+ ao_ublox_parse(&nav_sat, nav_sat_packet);
+ for (nsat = 0; nsat < nav_sat.num_svs && ao_ublox_len >= 12; nsat++) {
+ ao_ublox_parse(&nav_sat_real_sat, nav_sat_sat_packet);
+ if (nav_sat_nsat < NAV_SAT_MAX_SAT &&
+ ao_ublox_sat_health(&nav_sat_real_sat) == UBLOX_NAV_SAT_FLAGS_SV_HEALTH_HEALTHY &&
+ ao_ublox_sat_quality(&nav_sat_real_sat) >= UBLOX_NAV_SAT_FLAGS_QUALITY_ACQUIRED)
+ {
+ nav_sat_sat[nav_sat_nsat].svid = nav_sat_real_sat.svid;
+ nav_sat_sat[nav_sat_nsat].cno = nav_sat_real_sat.cno;
+ nav_sat_nsat++;
+ }
+ }
+#if AO_UBLOX_DEBUG
+ ao_gps_dbg(DBG_PROTO, "sat num_svs %d\n", nav_sat.num_svs);
+ for (nsat = 0; nsat < nav_sat.num_svs; nsat++) {
+ if (nsat < NAV_SAT_MAX_SAT) {
+ ao_gps_dbg(DBG_PROTO, "\t%d: svid %d cno %d\n",
+ nsat,
+ nav_sat_sat[nsat].svid,
+ nav_sat_sat[nsat].cno);
+ } else {
+ ao_gps_dbg(DBG_PROTO, "\t%d: skipped\n", nsat);
+ }
+ }
+#endif
+}
+
+#endif /* else AO_UBLOX_VERSION < 10 */
+
+/*
+ * Set the protocol mode and baud rate
+ */
+
+static void
+ao_gps_delay(void)
+{
+ uint8_t i;
/*
* A bunch of nulls so the start bit
* is clear
*/
+
for (i = 0; i < 64; i++)
- ao_ublox_putchar(0x00);
+ ao_gps_putchar(0x00);
+}
+
+static void
+ao_gps_setup(void)
+{
+ uint8_t i, k;
+
+ ao_delay(AO_SEC_TO_TICKS(3));
+
+ ao_gps_dbg(DBG_INIT, "Set speed 9600\n");
+ ao_gps_set_speed(AO_SERIAL_SPEED_9600);
/*
* Send the baud-rate setting and protocol-setting
* command three times
*/
- for (k = 0; k < 3; k++)
+ for (k = 0; k < 3; k++) {
+ ao_gps_delay();
+
+ ao_gps_dbg(DBG_INIT, "Send initial setting\n");
for (i = 0; i < sizeof (ao_gps_set_nmea); i++)
- ao_ublox_putchar(ao_gps_set_nmea[i]);
+ ao_gps_putchar(ao_gps_set_nmea[i]);
+ }
+
+ ao_gps_delay();
+#if AO_SERIAL_SPEED_UBLOX != AO_SERIAL_SPEED_9600
+ ao_gps_dbg(DBG_INIT, "Set speed high\n");
/*
* Increase the baud rate
*/
- ao_ublox_set_speed(AO_SERIAL_SPEED_57600);
+ ao_gps_set_speed(AO_SERIAL_SPEED_UBLOX);
+#endif
- /*
- * Pad with nulls to give the chip
- * time to see the baud rate switch
- */
- for (i = 0; i < 64; i++)
- ao_ublox_putchar(0x00);
+ ao_gps_delay();
}
static void
ao_ublox_putstart(uint8_t class, uint8_t id, uint16_t len)
{
ao_ublox_init_cksum();
- ao_ublox_putchar(0xb5);
- ao_ublox_putchar(0x62);
+ ao_gps_putchar(0xb5);
+ ao_gps_putchar(0x62);
ao_ublox_put_u8(class);
ao_ublox_put_u8(id);
- ao_ublox_put_u8(len);
- ao_ublox_put_u8(len >> 8);
+ ao_ublox_put_u8((uint8_t) len);
+ ao_ublox_put_u8((uint8_t) (len >> 8));
}
static void
ao_ublox_putend(void)
{
- ao_ublox_putchar(ao_ublox_cksum.a);
- ao_ublox_putchar(ao_ublox_cksum.b);
+ ao_gps_putchar(ao_ublox_cksum.a);
+ ao_gps_putchar(ao_ublox_cksum.b);
}
static void
ao_ublox_set_message_rate(uint8_t class, uint8_t msgid, uint8_t rate)
{
- ao_ublox_putstart(0x06, 0x01, 3);
+ ao_ublox_putstart(UBLOX_CFG, UBLOX_CFG_MSG, 3);
ao_ublox_put_u8(class);
ao_ublox_put_u8(msgid);
ao_ublox_put_u8(rate);
ao_ublox_putend();
}
-
/*
* Disable all MON message
*/
* Enable enough messages to get all of the data we want
*/
static const uint8_t ublox_enable_nav[] = {
- UBLOX_NAV_DOP,
- UBLOX_NAV_POSLLH,
- UBLOX_NAV_SOL,
- UBLOX_NAV_SVINFO,
- UBLOX_NAV_VELNED,
- UBLOX_NAV_TIMEUTC
+ UBLOX_NAV_DOP, /* both */
+#if AO_UBLOX_VERSION >= 10
+ UBLOX_NAV_PVT, /* new */
+ UBLOX_NAV_SAT, /* new */
+#else
+ UBLOX_NAV_POSLLH, /* both, but redundant with PVT */
+ UBLOX_NAV_SOL, /* old */
+ UBLOX_NAV_SVINFO, /* old */
+ UBLOX_NAV_VELNED, /* both, but redundant with PVT */
+ UBLOX_NAV_TIMEUTC /* both, but redundant with PVT */
+#endif
};
void
-ao_gps(void) __reentrant
+ao_gps_set_rate(uint8_t rate)
+{
+ uint8_t i;
+ for (i = 0; i < sizeof (ublox_enable_nav); i++)
+ ao_ublox_set_message_rate(UBLOX_NAV, ublox_enable_nav[i], rate);
+}
+
+void
+ao_gps(void)
{
uint8_t class, id;
struct ao_ublox_cksum cksum;
uint8_t i;
+ AO_TICK_TYPE packet_start_tick;
+ AO_TICK_TYPE solution_tick = 0;
ao_gps_setup();
ao_ublox_set_message_rate(UBLOX_NAV, ublox_disable_nav[i], 0);
/* Enable all of the messages we want */
- for (i = 0; i < sizeof (ublox_enable_nav); i++)
- ao_ublox_set_message_rate(UBLOX_NAV, ublox_enable_nav[i], 1);
-
+ ao_gps_set_rate(1);
+
ao_ublox_set_navigation_settings((1 << UBLOX_CFG_NAV5_MASK_DYN) | (1 << UBLOX_CFG_NAV5_MASK_FIXMODE),
UBLOX_CFG_NAV5_DYNMODEL_AIRBORNE_4G,
UBLOX_CFG_NAV5_FIXMODE_3D,
/* Locate the begining of the next record */
while (ao_ublox_byte() != (uint8_t) 0xb5)
;
+ packet_start_tick = ao_tick_count;
+
if (ao_ublox_byte() != (uint8_t) 0x62)
continue;
/* Length */
ao_ublox_len = header_byte();
- ao_ublox_len |= header_byte() << 8;
+ ao_ublox_len |= (uint16_t) ((uint16_t) header_byte() << 8);
+
+ ao_gps_dbg(DBG_PROTO, "%6u class %02x id %02x len %d\n", packet_start_tick, class, id, ao_ublox_len);
if (ao_ublox_len > 1023)
continue;
+ bool gps_ready = false;
+
switch (class) {
case UBLOX_NAV:
switch (id) {
if (ao_ublox_len != 18)
break;
ao_ublox_parse_nav_dop();
+#if AO_UBLOX_VERSION >= 10
+ gps_ready = true;
+#endif
+ break;
+#if AO_UBLOX_VERSION >= 10
+ case UBLOX_NAV_PVT:
+ if (ao_ublox_len != 92)
+ break;
+ ao_ublox_parse_nav_pvt();
+ solution_tick = packet_start_tick;
break;
+ case UBLOX_NAV_SAT:
+ if (ao_ublox_len < 8)
+ break;
+ ao_ublox_parse_nav_sat();
+ break;
+#else
case UBLOX_NAV_POSLLH:
if (ao_ublox_len != 28)
break;
if (ao_ublox_len != 52)
break;
ao_ublox_parse_nav_sol();
+ solution_tick = packet_start_tick;
break;
case UBLOX_NAV_SVINFO:
if (ao_ublox_len < 8)
if (ao_ublox_len != 20)
break;
ao_ublox_parse_nav_timeutc();
+ gps_ready = true;
break;
+#endif
}
break;
+ case UBLOX_ACK:
+ switch (id) {
+ case UBLOX_ACK_ACK:
+ if (ao_ublox_len != 2)
+ break;
+ ao_ublox_parse_ack_ack();
+ break;
+ case UBLOX_ACK_NAK:
+ if (ao_ublox_len != 2)
+ break;
+ ao_ublox_parse_ack_nak();
+ break;
+ }
}
- if (ao_ublox_len != 0)
+ if (ao_ublox_len != 0) {
+ ao_gps_dbg(DBG_PROTO, "len left %d\n", ao_ublox_len);
continue;
+ }
/* verify checksum and end sequence */
cksum.a = ao_ublox_byte();
if (ao_ublox_cksum.a != cksum.a || ao_ublox_cksum.b != cksum.b)
continue;
- switch (class) {
- case 0x01:
- switch (id) {
- case 0x21:
- ao_mutex_get(&ao_gps_mutex);
- ao_gps_tick = ao_time();
-
- ao_gps_data.flags = 0;
- ao_gps_data.flags |= AO_GPS_RUNNING;
- if (nav_sol.gps_fix & (1 << NAV_SOL_FLAGS_GPSFIXOK)) {
- uint8_t nsat = nav_sol.nsat;
- ao_gps_data.flags |= AO_GPS_VALID;
- if (nsat > 15)
- nsat = 15;
- ao_gps_data.flags |= nsat;
- }
- if (nav_timeutc.valid & (1 << NAV_TIMEUTC_VALID_UTC))
- ao_gps_data.flags |= AO_GPS_DATE_VALID;
-
- ao_gps_data.altitude = nav_posllh.alt_msl / 1000;
- ao_gps_data.latitude = nav_posllh.lat;
- ao_gps_data.longitude = nav_posllh.lon;
-
- ao_gps_data.year = nav_timeutc.year - 2000;
- ao_gps_data.month = nav_timeutc.month;
- ao_gps_data.day = nav_timeutc.day;
-
- ao_gps_data.hour = nav_timeutc.hour;
- ao_gps_data.minute = nav_timeutc.min;
- ao_gps_data.second = nav_timeutc.sec;
-
- ao_gps_data.pdop = nav_dop.pdop;
- ao_gps_data.hdop = nav_dop.hdop;
- ao_gps_data.vdop = nav_dop.vdop;
-
- /* mode is not set */
-
- ao_gps_data.ground_speed = nav_velned.g_speed;
- ao_gps_data.climb_rate = -nav_velned.vel_d;
- ao_gps_data.course = nav_velned.heading / 200000;
-
- ao_gps_tracking_data.channels = 0;
-
- struct ao_telemetry_satellite_info *dst = &ao_gps_tracking_data.sats[0];
-
- for (i = 0; i < nav_svinfo_nsat; i++) {
- struct nav_svinfo_sat *src = &nav_svinfo_sat[i];
-
- if (!(src->flags & (1 << NAV_SVINFO_SAT_FLAGS_UNHEALTHY)) &&
- src->quality >= NAV_SVINFO_SAT_QUALITY_ACQUIRED)
- {
- dst->svid = src->svid;
- dst->c_n_1 = src->cno;
- dst++;
- ao_gps_tracking_data.channels++;
- }
- }
+ if (!gps_ready)
+ continue;
- ao_mutex_put(&ao_gps_mutex);
- ao_wakeup(&ao_gps_data);
- ao_wakeup(&ao_gps_tracking_data);
- break;
+ ao_mutex_get(&ao_gps_mutex);
+ ao_gps_data.flags = 0;
+ ao_gps_data.flags |= AO_GPS_RUNNING;
+ ao_gps_tick = solution_tick;
+
+ /* we report dop scaled by 10, but ublox provides dop scaled by 100
+ */
+ ao_gps_data.pdop = (uint8_t) (nav_dop.pdop / 10);
+ ao_gps_data.hdop = (uint8_t) (nav_dop.hdop / 10);
+ ao_gps_data.vdop = (uint8_t) (nav_dop.vdop / 10);
+
+#if AO_UBLOX_VERSION >= 10
+ ao_gps_utc_tick = packet_start_tick + (AO_TICK_TYPE) AO_NS_TO_TICKS(nav_pvt.nano);
+ if (nav_pvt.flags & (1 << NAV_PVT_FLAGS_GNSSFIXOK)) {
+ uint8_t nsat = nav_pvt.num_sv;
+ ao_gps_data.flags |= AO_GPS_VALID | AO_GPS_COURSE_VALID;
+ if (nsat > 15)
+ nsat = 15;
+ ao_gps_data.flags |= nsat;
+ }
+ if (nav_pvt.valid & (1 << NAV_PVT_VALID_DATE))
+ ao_gps_data.flags |= AO_GPS_DATE_VALID;
+ AO_TELEMETRY_LOCATION_SET_ALTITUDE(&ao_gps_data, nav_pvt.alt_msl / 1000);
+ ao_gps_data.latitude = nav_pvt.lat;
+ ao_gps_data.longitude = nav_pvt.lon;
+
+ ao_gps_data.year = (uint8_t) (nav_pvt.year - 2000);
+ ao_gps_data.month = nav_pvt.month;
+ ao_gps_data.day = nav_pvt.day;
+
+ ao_gps_data.hour = nav_pvt.hour;
+ ao_gps_data.minute = nav_pvt.min;
+ ao_gps_data.second = nav_pvt.sec;
+
+ /* ublox speeds are mm/s */
+ ao_gps_data.ground_speed = (uint16_t) (nav_pvt.g_speed / 10);
+ ao_gps_data.climb_rate = -(int16_t) (nav_pvt.vel_d / 10);
+ ao_gps_data.course = (uint8_t) (nav_pvt.heading / 200000);
+#else
+ ao_gps_utc_tick = packet_start_tick + (AO_TICK_TYPE) AO_NS_TO_TICKS(nav_timeutc.nano);
+ if (nav_sol.gps_fix & (1 << NAV_SOL_FLAGS_GPSFIXOK)) {
+ uint8_t nsat = nav_sol.nsat;
+ ao_gps_data.flags |= AO_GPS_VALID | AO_GPS_COURSE_VALID;
+ if (nsat > 15)
+ nsat = 15;
+ ao_gps_data.flags |= nsat;
+ }
+ if (nav_timeutc.valid & (1 << NAV_TIMEUTC_VALID_UTC))
+ ao_gps_data.flags |= AO_GPS_DATE_VALID;
+
+ AO_TELEMETRY_LOCATION_SET_ALTITUDE(&ao_gps_data, nav_posllh.alt_msl / 1000);
+ ao_gps_data.latitude = nav_posllh.lat;
+ ao_gps_data.longitude = nav_posllh.lon;
+
+ ao_gps_data.year = (uint8_t) (nav_timeutc.year - 2000);
+ ao_gps_data.month = nav_timeutc.month;
+ ao_gps_data.day = nav_timeutc.day;
+
+ ao_gps_data.hour = nav_timeutc.hour;
+ ao_gps_data.minute = nav_timeutc.min;
+ ao_gps_data.second = nav_timeutc.sec;
+
+ /* ublox speeds are cm/s */
+ ao_gps_data.ground_speed = (uint16_t) nav_velned.g_speed;
+ ao_gps_data.climb_rate = -(int16_t) nav_velned.vel_d;
+ ao_gps_data.course = (uint8_t) (nav_velned.heading / 200000);
+#endif
+
+ ao_gps_tracking_data.channels = 0;
+
+ struct ao_telemetry_satellite_info *dst = &ao_gps_tracking_data.sats[0];
+#if AO_UBLOX_VERSION >= 10
+ struct nav_sat_sat *src = &nav_sat_sat[0];
+
+ for (i = 0; i < nav_sat_nsat; i++) {
+ if (ao_gps_tracking_data.channels < AO_TELEMETRY_SATELLITE_MAX_SAT) {
+ dst->svid = src->svid;
+ dst->c_n_1 = src->cno;
+ dst++;
+ ao_gps_tracking_data.channels++;
}
- break;
+ src++;
}
+#else
+ struct nav_svinfo_sat *src = &nav_svinfo_sat[0];
+
+ for (i = 0; i < nav_svinfo_nsat; i++) {
+ if (!(src->flags & (1 << NAV_SVINFO_SAT_FLAGS_UNHEALTHY)) &&
+ src->quality >= NAV_SVINFO_SAT_QUALITY_ACQUIRED)
+ {
+ if (ao_gps_tracking_data.channels < AO_TELEMETRY_SATELLITE_MAX_SAT) {
+ dst->svid = src->svid;
+ dst->c_n_1 = src->cno;
+ dst++;
+ ao_gps_tracking_data.channels++;
+ }
+ }
+ src++;
+ }
+#endif
+ ao_mutex_put(&ao_gps_mutex);
+ ao_gps_new = AO_GPS_NEW_DATA | AO_GPS_NEW_TRACKING;
+ ao_wakeup(&ao_gps_new);
}
}
-__code struct ao_cmds ao_gps_cmds[] = {
- { ao_gps_show, "g\0Display GPS" },
+#if AO_UBLOX_DEBUG
+static void ao_gps_option(void)
+{
+ uint8_t r = (uint8_t) ao_cmd_hex();
+ if (ao_cmd_status != ao_cmd_success) {
+ ao_cmd_status = ao_cmd_success;
+ ao_gps_show();
+ } else {
+ ao_gps_dbg_enable = r;
+ printf ("gps debug set to %d\n", ao_gps_dbg_enable);
+ }
+}
+#else
+#define ao_gps_option ao_gps_show
+#endif
+
+const struct ao_cmds ao_gps_cmds[] = {
+ { ao_gps_option, "g\0Display GPS" },
{ 0, NULL },
};
-__xdata struct ao_task ao_gps_task;
+struct ao_task ao_gps_task;
void
ao_gps_init(void)