*
* 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";
+#include <stdarg.h>
-const char ao_gps_config[] = {
+uint8_t ao_gps_new;
+uint8_t ao_gps_mutex;
+AO_TICK_TYPE ao_gps_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
+
+static uint8_t ao_gps_dbg_enable;
+
+#define DBG_PROTO 1
+#define DBG_CHAR 2
+#define DBG_INIT 4
-#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
+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)
{
ao_ublox_cksum.a = ao_ublox_cksum.b = 0;
}
-static void ao_ublox_putchar_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)
+{
+ ao_ublox_put_u8((uint8_t) c);
+}
+
+static void ao_ublox_put_u16(uint16_t c)
+{
+ ao_ublox_put_u8(c);
+ ao_ublox_put_u8(c>>8);
+}
+
+#if 0
+static void ao_ublox_put_i16(int16_t c)
+{
+ ao_ublox_put_u16((uint16_t) c);
+}
+#endif
+
+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);
+}
+
+static void ao_ublox_put_i32(int32_t c)
+{
+ ao_ublox_put_u32((uint32_t) c);
}
static uint8_t header_byte(void)
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();
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");
}
/*
{
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
}
/*
*/
static void
-ao_gps_setup(void)
+ao_gps_delay(void)
{
- uint8_t i, k;
- ao_ublox_set_speed(AO_SERIAL_SPEED_9600);
+ 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();
}
-void
+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_ublox_putchar_cksum(class);
- ao_ublox_putchar_cksum(id);
- ao_ublox_putchar_cksum(len);
- ao_ublox_putchar_cksum(len >> 8);
+ 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);
}
-void
+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);
}
-void
+static void
ao_ublox_set_message_rate(uint8_t class, uint8_t msgid, uint8_t rate)
{
ao_ublox_putstart(0x06, 0x01, 3);
- ao_ublox_putchar_cksum(class);
- ao_ublox_putchar_cksum(msgid);
- ao_ublox_putchar_cksum(rate);
+ ao_ublox_put_u8(class);
+ ao_ublox_put_u8(msgid);
+ ao_ublox_put_u8(rate);
ao_ublox_putend();
}
+static void
+ao_ublox_set_navigation_settings(uint16_t mask,
+ uint8_t dyn_model,
+ uint8_t fix_mode,
+ int32_t fixed_alt,
+ uint32_t fixed_alt_var,
+ int8_t min_elev,
+ uint8_t dr_limit,
+ uint16_t pdop,
+ uint16_t tdop,
+ uint16_t pacc,
+ uint16_t tacc,
+ uint8_t static_hold_thresh,
+ uint8_t dgps_time_out)
+{
+ ao_ublox_putstart(UBLOX_CFG, UBLOX_CFG_NAV5, 36);
+ ao_ublox_put_u16(mask);
+ ao_ublox_put_u8(dyn_model);
+ ao_ublox_put_u8(fix_mode);
+ ao_ublox_put_i32(fixed_alt);
+ ao_ublox_put_u32(fixed_alt_var);
+ ao_ublox_put_i8(min_elev);
+ ao_ublox_put_u8(dr_limit);
+ ao_ublox_put_u16(pdop);
+ ao_ublox_put_u16(tdop);
+ ao_ublox_put_u16(pacc);
+ ao_ublox_put_u16(tacc);
+ ao_ublox_put_u8(static_hold_thresh);
+ ao_ublox_put_u8(dgps_time_out);
+ ao_ublox_put_u32(0);
+ ao_ublox_put_u32(0);
+ ao_ublox_put_u32(0);
+ ao_ublox_putend();
+}
+
+
/*
* Disable all MON message
*/
};
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,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0);
+
for (;;) {
/* 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;
ao_ublox_len = header_byte();
ao_ublox_len |= 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;
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)
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();
continue;
switch (class) {
- case 0x01:
+ case UBLOX_NAV:
switch (id) {
- case 0x21:
+ case UBLOX_NAV_TIMEUTC:
ao_mutex_get(&ao_gps_mutex);
- ao_gps_tick = ao_time();
+ ao_gps_tick = solution_tick;
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;
+ 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_gps_data.altitude = nav_posllh.alt_msl / 1000;
+
+ 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.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 */
+ /* we report dop scaled by 10, but ublox provides dop scaled by 100
+ */
+ ao_gps_data.pdop = nav_dop.pdop / 10;
+ ao_gps_data.hdop = nav_dop.hdop / 10;
+ ao_gps_data.vdop = nav_dop.vdop / 10;
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];
+ struct nav_svinfo_sat *src = &nav_svinfo_sat[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 (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++;
}
ao_mutex_put(&ao_gps_mutex);
- ao_wakeup(&ao_gps_data);
- ao_wakeup(&ao_gps_tracking_data);
+ ao_gps_new = AO_GPS_NEW_DATA | AO_GPS_NEW_TRACKING;
+ ao_wakeup(&ao_gps_new);
break;
}
break;
}
}
-__xdata struct ao_task ao_gps_task;
+#if AO_UBLOX_DEBUG
+static void ao_gps_option(void)
+{
+ uint16_t r = 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 },
+};
+
+struct ao_task ao_gps_task;
void
ao_gps_init(void)
{
+ ao_cmd_register(&ao_gps_cmds[0]);
ao_add_task(&ao_gps_task, ao_gps, "gps");
}
projects / fw / altos / blobdiff