From: Keith Packard Date: Tue, 14 May 2013 05:31:31 +0000 (-0700) Subject: altos: Add U-Blox GPS driver X-Git-Tag: 1.2.1~14 X-Git-Url: https://git.gag.com/?p=fw%2Faltos;a=commitdiff_plain;h=50457f9983ec0a432f1050464382749436e3da94 altos: Add U-Blox GPS driver Uses binary mode. Signed-off-by: Keith Packard --- diff --git a/src/drivers/ao_gps_ublox.c b/src/drivers/ao_gps_ublox.c new file mode 100644 index 00000000..32405ea5 --- /dev/null +++ b/src/drivers/ao_gps_ublox.c @@ -0,0 +1,626 @@ +/* + * Copyright © 2013 Keith Packard + * + * 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. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. + */ + +#ifndef AO_GPS_TEST +#include "ao.h" +#endif + +#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; + +static const char ao_gps_set_nmea[] = "\r\n$PUBX,41,1,3,1,57600,0*2d\r\n"; + +const char ao_gps_config[] = { + +}; + +struct ao_ublox_cksum { + uint8_t a, b; +}; + +static __pdata struct ao_ublox_cksum ao_ublox_cksum; +static __pdata uint16_t ao_ublox_len; + +#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 +#endif + +#define ao_ublox_byte() ((uint8_t) ao_ublox_getchar()) + +static inline void add_cksum(struct ao_ublox_cksum *cksum, uint8_t c) +{ + cksum->a += c; + cksum->b += cksum->a; +} + +static void ao_ublox_init_cksum(void) +{ + ao_ublox_cksum.a = ao_ublox_cksum.b = 0; +} + +static void ao_ublox_putchar_cksum(uint8_t c) +{ + add_cksum(&ao_ublox_cksum, c); + ao_ublox_putchar(c); +} + +static uint8_t header_byte(void) +{ + uint8_t c = ao_ublox_byte(); + add_cksum(&ao_ublox_cksum, c); + return c; +} + +static uint8_t data_byte(void) +{ + --ao_ublox_len; + return header_byte(); +} + +static char __xdata *ublox_target; + +static void ublox_u16(uint8_t offset) +{ + uint16_t __xdata *ptr = (uint16_t __xdata *) (ublox_target + offset); + uint16_t val; + + val = data_byte(); + val |= data_byte () << 8; + *ptr = val; +} + +static void ublox_u8(uint8_t offset) +{ + uint8_t __xdata *ptr = (uint8_t __xdata *) (ublox_target + offset); + uint8_t val; + + val = data_byte (); + *ptr = val; +} + +static void ublox_u32(uint8_t offset) __reentrant +{ + uint32_t __xdata *ptr = (uint32_t __xdata *) (ublox_target + offset); + uint32_t val; + + val = ((uint32_t) data_byte ()); + val |= ((uint32_t) data_byte ()) << 8; + val |= ((uint32_t) data_byte ()) << 16; + val |= ((uint32_t) data_byte ()) << 24; + *ptr = val; +} + +static void ublox_discard(uint8_t len) +{ + while (len--) + data_byte(); +} + +#define UBLOX_END 0 +#define UBLOX_DISCARD 1 +#define UBLOX_U8 2 +#define UBLOX_U16 3 +#define UBLOX_U32 4 + +struct ublox_packet_parse { + uint8_t type; + uint8_t offset; +}; + +static void +ao_ublox_parse(void __xdata *target, const struct ublox_packet_parse *parse) __reentrant +{ + uint8_t i, offset; + + ublox_target = target; + for (i = 0; ; i++) { + offset = parse[i].offset; + switch (parse[i].type) { + case UBLOX_END: + return; + case UBLOX_DISCARD: + ublox_discard(offset); + break; + case UBLOX_U8: + ublox_u8(offset); + break; + case UBLOX_U16: + ublox_u16(offset); + break; + case UBLOX_U32: + ublox_u32(offset); + break; + } + } +} + +/* + * NAV-DOP message parsing + */ + +static struct nav_dop { + uint16_t pdop; + uint16_t hdop; + uint16_t vdop; +} nav_dop; + +static const struct ublox_packet_parse nav_dop_packet[] = { + { UBLOX_DISCARD, 6 }, /* 0 GPS Millisecond Time of Week, gDOP */ + { UBLOX_U16, offsetof(struct nav_dop, pdop) }, /* 6 pDOP */ + { UBLOX_DISCARD, 2 }, /* 8 tDOP */ + { UBLOX_U16, offsetof(struct nav_dop, vdop) }, /* 10 vDOP */ + { UBLOX_U16, offsetof(struct nav_dop, hdop) }, /* 12 hDOP */ + { UBLOX_DISCARD, 4 }, /* 14 nDOP, eDOP */ + { UBLOX_END, 0 } +}; + +static void +ao_ublox_parse_nav_dop(void) +{ + ao_ublox_parse(&nav_dop, nav_dop_packet); +} + +/* + * NAV-POSLLH message parsing + */ +static struct nav_posllh { + int32_t lat; + int32_t lon; + int32_t alt_msl; +} nav_posllh; + +static const struct ublox_packet_parse nav_posllh_packet[] = { + { UBLOX_DISCARD, 4 }, /* 0 GPS Millisecond Time of Week */ + { UBLOX_U32, offsetof(struct nav_posllh, lon) }, /* 4 Longitude */ + { UBLOX_U32, offsetof(struct nav_posllh, lat) }, /* 8 Latitude */ + { UBLOX_DISCARD, 4 }, /* 12 Height above Ellipsoid */ + { UBLOX_U32, offsetof(struct nav_posllh, alt_msl) }, /* 16 Height above mean sea level */ + { UBLOX_DISCARD, 8 }, /* 20 hAcc, vAcc */ + { UBLOX_END, 0 }, /* 28 */ +}; + +static void +ao_ublox_parse_nav_posllh(void) +{ + ao_ublox_parse(&nav_posllh, nav_posllh_packet); +} + +/* + * NAV-SOL message parsing + */ +static struct nav_sol { + uint8_t gps_fix; + uint8_t flags; + uint8_t nsat; +} nav_sol; + +static const struct ublox_packet_parse nav_sol_packet[] = { + { UBLOX_DISCARD, 10 }, /* 0 iTOW, fTOW, week */ + { UBLOX_U8, offsetof(struct nav_sol, gps_fix) }, /* 10 gpsFix */ + { UBLOX_U8, offsetof(struct nav_sol, flags) }, /* 11 flags */ + { UBLOX_DISCARD, 35 }, /* 12 ecefX, ecefY, ecefZ, pAcc, ecefVX, ecefVY, ecefVZ, sAcc, pDOP, reserved1 */ + { UBLOX_U8, offsetof(struct nav_sol, nsat) }, /* 47 numSV */ + { UBLOX_DISCARD, 4 }, /* 48 reserved2 */ + { UBLOX_END, 0 } +}; + +#define NAV_SOL_FLAGS_GPSFIXOK 0 +#define NAV_SOL_FLAGS_DIFFSOLN 1 +#define NAV_SOL_FLAGS_WKNSET 2 +#define NAV_SOL_FLAGS_TOWSET 3 + +static void +ao_ublox_parse_nav_sol(void) +{ + ao_ublox_parse(&nav_sol, nav_sol_packet); +} + +/* + * NAV-SVINFO message parsing + */ + +static struct nav_svinfo { + uint8_t num_ch; + uint8_t flags; +} nav_svinfo; + +static const struct ublox_packet_parse nav_svinfo_packet[] = { + { UBLOX_DISCARD, 4 }, /* 0 iTOW */ + { UBLOX_U8, offsetof(struct nav_svinfo, num_ch) }, /* 4 numCh */ + { UBLOX_U8, offsetof(struct nav_svinfo, flags) }, /* 5 globalFlags */ + { UBLOX_DISCARD, 2 }, /* 6 reserved2 */ + { UBLOX_END, 0 } +}; + +#define NAV_SVINFO_MAX_SAT 16 + +static struct nav_svinfo_sat { + uint8_t chn; + uint8_t svid; + uint8_t flags; + uint8_t quality; + uint8_t cno; +} nav_svinfo_sat[NAV_SVINFO_MAX_SAT]; + +static uint8_t nav_svinfo_nsat; + +static const struct ublox_packet_parse nav_svinfo_sat_packet[] = { + { UBLOX_U8, offsetof(struct nav_svinfo_sat, chn) }, /* 8 + 12*N chn */ + { UBLOX_U8, offsetof(struct nav_svinfo_sat, svid) }, /* 9 + 12*N svid */ + { UBLOX_U8, offsetof(struct nav_svinfo_sat, flags) }, /* 10 + 12*N flags */ + { UBLOX_U8, offsetof(struct nav_svinfo_sat, quality) }, /* 11 + 12*N quality */ + { UBLOX_U8, offsetof(struct nav_svinfo_sat, cno) }, /* 12 + 12*N cno */ + { UBLOX_DISCARD, 7 }, /* 13 + 12*N elev, azim, prRes */ + { UBLOX_END, 0 } +}; + +#define NAV_SVINFO_SAT_FLAGS_SVUSED 0 +#define NAV_SVINFO_SAT_FLAGS_DIFFCORR 1 +#define NAV_SVINFO_SAT_FLAGS_ORBITAVAIL 2 +#define NAV_SVINFO_SAT_FLAGS_ORBITEPH 3 +#define NAV_SVINFO_SAT_FLAGS_UNHEALTHY 4 +#define NAV_SVINFO_SAT_FLAGS_ORBITALM 5 +#define NAV_SVINFO_SAT_FLAGS_ORBITAOP 6 +#define NAV_SVINFO_SAT_FLAGS_SMOOTHED 7 + +#define NAV_SVINFO_SAT_QUALITY_IDLE 0 +#define NAV_SVINFO_SAT_QUALITY_SEARCHING 1 +#define NAV_SVINFO_SAT_QUALITY_ACQUIRED 2 +#define NAV_SVINFO_SAT_QUALITY_UNUSABLE 3 +#define NAV_SVINFO_SAT_QUALITY_LOCKED 4 +#define NAV_SVINFO_SAT_QUALITY_RUNNING 5 + +static void +ao_ublox_parse_nav_svinfo(void) +{ + 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) { + ao_ublox_parse(&nav_svinfo_sat[nav_svinfo_nsat++], nav_svinfo_sat_packet); + } else { + ublox_discard(12); + } + } +} + +/* + * NAV-TIMEUTC message parsing + */ +static struct nav_timeutc { + uint16_t year; + uint8_t month; + uint8_t day; + uint8_t hour; + uint8_t min; + uint8_t sec; + uint8_t valid; +} nav_timeutc; + +#define NAV_TIMEUTC_VALID_TOW 0 +#define NAV_TIMEUTC_VALID_WKN 1 +#define NAV_TIMEUTC_VALID_UTC 2 + +static const struct ublox_packet_parse nav_timeutc_packet[] = { + { UBLOX_DISCARD, 12 }, /* 0 iTOW, tAcc, 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 */ + { UBLOX_U8, offsetof(struct nav_timeutc, hour) }, /* 16 hour */ + { UBLOX_U8, offsetof(struct nav_timeutc, min) }, /* 17 min */ + { UBLOX_U8, offsetof(struct nav_timeutc, sec) }, /* 18 sec */ + { UBLOX_U8, offsetof(struct nav_timeutc, valid) }, /* 19 valid */ + { UBLOX_END, 0 } +}; + +static void +ao_ublox_parse_nav_timeutc(void) +{ + ao_ublox_parse(&nav_timeutc, nav_timeutc_packet); +} + +/* + * NAV-VELNED message parsing + */ + +static struct nav_velned { + int32_t vel_d; + uint32_t g_speed; + int32_t heading; +} nav_velned; + +static const struct ublox_packet_parse nav_velned_packet[] = { + { UBLOX_DISCARD, 12 }, /* 0 iTOW, velN, velE */ + { UBLOX_U32, offsetof(struct nav_velned, vel_d) }, /* 12 velD */ + { UBLOX_DISCARD, 4 }, /* 16 speed */ + { UBLOX_U32, offsetof(struct nav_velned, g_speed) }, /* 20 gSpeed */ + { UBLOX_U32, offsetof(struct nav_velned, heading) }, /* 24 heading */ + { UBLOX_DISCARD, 8 }, /* 28 sAcc, cAcc */ + { UBLOX_END, 0 } +}; + +static void +ao_ublox_parse_nav_velned(void) +{ + ao_ublox_parse(&nav_velned, nav_velned_packet); +} + +/* + * Set the protocol mode and baud rate + */ + +static void +ao_gps_setup(void) +{ + uint8_t i, k; + ao_ublox_set_speed(AO_SERIAL_SPEED_9600); + + /* + * A bunch of nulls so the start bit + * is clear + */ + for (i = 0; i < 64; i++) + ao_ublox_putchar(0x00); + + /* + * Send the baud-rate setting and protocol-setting + * command three times + */ + for (k = 0; k < 3; k++) + for (i = 0; i < sizeof (ao_gps_set_nmea); i++) + ao_ublox_putchar(ao_gps_set_nmea[i]); + + /* + * Increase the baud rate + */ + ao_ublox_set_speed(AO_SERIAL_SPEED_57600); + + /* + * Pad with nulls to give the chip + * time to see the baud rate switch + */ + for (i = 0; i < 64; i++) + ao_ublox_putchar(0x00); +} + +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); +} + +void +ao_ublox_putend(void) +{ + ao_ublox_putchar(ao_ublox_cksum.a); + ao_ublox_putchar(ao_ublox_cksum.b); +} + +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_putend(); +} + +/* + * Disable all MON message + */ +static const uint8_t ublox_disable_mon[] = { + 0x0b, 0x09, 0x02, 0x06, 0x07, 0x21, 0x08, 0x04 +}; + +/* + * Disable all NAV messages. The desired + * ones will be explicitly re-enabled + */ + +static const uint8_t ublox_disable_nav[] = { + 0x60, 0x22, 0x31, 0x04, 0x40, 0x01, 0x02, 0x32, + 0x06, 0x03, 0x30, 0x20, 0x21, 0x11, 0x12 +}; + +void +ao_gps(void) __reentrant +{ + uint8_t class, id; + struct ao_ublox_cksum cksum; + uint8_t i; + + ao_gps_setup(); + + for (i = 0; i < sizeof (ublox_disable_mon); i++) + ao_ublox_set_message_rate(0x0a, ublox_disable_mon[i], 0); + for (i = 0; i < sizeof (ublox_disable_nav); i++) + ao_ublox_set_message_rate(0x01, ublox_disable_nav[i], 0); + + /* Enable all of the messages we want */ + + /* DOP */ + ao_ublox_set_message_rate(0x01, 0x04, 1); + + /* POSLLH */ + ao_ublox_set_message_rate(0x01, 0x02, 1); + + /* SOL */ + ao_ublox_set_message_rate(0x01, 0x06, 1); + + /* SVINFO */ + ao_ublox_set_message_rate(0x01, 0x30, 1); + + /* VELNED */ + ao_ublox_set_message_rate(0x01, 0x12, 1); + + /* TIMEUTC */ + ao_ublox_set_message_rate(0x01, 0x21, 1); + + for (;;) { + /* Locate the begining of the next record */ + while (ao_ublox_byte() != (uint8_t) 0xb5) + ; + if (ao_ublox_byte() != (uint8_t) 0x62) + continue; + + ao_ublox_init_cksum(); + + class = header_byte(); + id = header_byte(); + + /* Length */ + ao_ublox_len = header_byte(); + ao_ublox_len |= header_byte() << 8; + + if (ao_ublox_len > 1023) + continue; + + switch (class) { + case UBLOX_NAV: + switch (id) { + case UBLOX_NAV_DOP: + if (ao_ublox_len != 18) + break; + ao_ublox_parse_nav_dop(); + break; + case UBLOX_NAV_POSLLH: + if (ao_ublox_len != 28) + break; + ao_ublox_parse_nav_posllh(); + break; + case UBLOX_NAV_SOL: + if (ao_ublox_len != 52) + break; + ao_ublox_parse_nav_sol(); + break; + case UBLOX_NAV_SVINFO: + if (ao_ublox_len < 8) + break; + ao_ublox_parse_nav_svinfo(); + break; + case UBLOX_NAV_VELNED: + if (ao_ublox_len != 36) + break; + ao_ublox_parse_nav_velned(); + break; + case UBLOX_NAV_TIMEUTC: + if (ao_ublox_len != 20) + break; + ao_ublox_parse_nav_timeutc(); + break; + } + break; + } + + if (ao_ublox_len != 0) + continue; + + /* verify checksum and end sequence */ + cksum.a = ao_ublox_byte(); + cksum.b = 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++; + } + } + + ao_mutex_put(&ao_gps_mutex); + ao_wakeup(&ao_gps_data); + ao_wakeup(&ao_gps_tracking_data); + break; + } + break; + } + } +} + +__xdata struct ao_task ao_gps_task; + +void +ao_gps_init(void) +{ + ao_add_task(&ao_gps_task, ao_gps, "gps"); +} diff --git a/src/drivers/ao_gps_ublox.h b/src/drivers/ao_gps_ublox.h new file mode 100644 index 00000000..13bf6955 --- /dev/null +++ b/src/drivers/ao_gps_ublox.h @@ -0,0 +1,241 @@ +/* + * Copyright © 2013 Keith Packard + * + * 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. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. + */ + +#ifndef _AO_GPS_UBLOX_H_ +#define _AO_GPS_UBLOX_H_ + +struct ublox_hdr { + uint8_t class, message; + uint16_t length; +}; + +#define UBLOX_NAV 0x01 + +#define UBLOX_NAV_DOP 0x04 + +struct ublox_nav_dop { + uint8_t class; /* 0x01 */ + uint8_t message; /* 0x04 */ + uint16_t length; /* 18 */ + + uint32_t itow; /* ms */ + uint16_t gdop; + uint16_t ddop; + uint16_t tdop; + uint16_t vdop; + uint16_t hdop; + uint16_t ndop; + uint16_t edop; +}; + +#define UBLOX_NAV_POSLLH 0x02 + +struct ublox_nav_posllh { + uint8_t class; /* 0x01 */ + uint8_t message; /* 0x02 */ + uint16_t length; /* 28 */ + + uint32_t itow; /* ms */ + int32_t lat; /* deg * 1e7 */ + int32_t lon; /* deg * 1e7 */ + int32_t height; /* mm */ + int32_t hmsl; /* mm */ + uint32_t hacc; /* mm */ + uint32_t vacc; /* mm */ +}; + +#define UBLOX_NAV_SOL 0x06 + +struct ublox_nav_sol { + uint8_t class; /* 0x01 */ + uint8_t message; /* 0x06 */ + uint16_t length; /* 52 */ + + uint32_t itow; /* ms */ + int32_t ftow; /* ns */ + int16_t week; + int8_t gpsfix; + uint8_t flags; + int32_t exefx; /* cm */ + int32_t exefy; /* cm */ + int32_t exefz; /* cm */ + uint32_t pacc; /* cm */ + int32_t exefvx; /* cm/s */ + int32_t exefvy; /* cm/s */ + int32_t exefvz; /* cm/s */ + uint32_t sacc; /* cm/s */ + uint16_t pdop; /* * 100 */ + uint8_t reserved1; + uint8_t numsv; + uint32_t reserved2; +}; + +#define UBLOX_NAV_SOL_GPSFIX_NO_FIX 0 +#define UBLOX_NAV_SOL_GPSFIX_DEAD_RECKONING 1 +#define UBLOX_NAV_SOL_GPSFIX_2D 2 +#define UBLOX_NAV_SOL_GPSFIX_3D 3 +#define UBLOX_NAV_SOL_GPSFIX_GPS_DEAD_RECKONING 4 +#define UBLOX_NAV_SOL_GPSFIX_TIME_ONLY 5 + +#define UBLOX_NAV_SOL_FLAGS_GPSFIXOK 0 +#define UBLOX_NAV_SOL_FLAGS_DIFFSOLN 1 +#define UBLOX_NAV_SOL_FLAGS_WKNSET 2 +#define UBLOX_NAV_SOL_FLAGS_TOWSET 3 + +#define UBLOX_NAV_STATUS 0x03 + +struct ublox_nav_status { + uint8_t class; /* 0x01 */ + uint8_t message; /* 0x03 */ + uint16_t length; /* 16 */ + + uint8_t gpsfix; + uint8_t flags; + uint8_t fixstat; + uint8_t flags2; + + uint32_t ttff; /* ms */ + uint32_t msss; /* ms */ +}; + +#define UBLOX_NAV_STATUS_GPSFIX_NO_FIX 0 +#define UBLOX_NAV_STATUS_GPSFIX_DEAD_RECKONING 1 +#define UBLOX_NAV_STATUS_GPSFIX_2D 2 +#define UBLOX_NAV_STATUS_GPSFIX_3D 3 +#define UBLOX_NAV_STATUS_GPSFIX_GPS_DEAD_RECKONING 4 +#define UBLOX_NAV_STATUS_GPSFIX_TIME_ONLY 5 + +#define UBLOX_NAV_STATUS_FLAGS_GPSFIXOK 0 +#define UBLOX_NAV_STATUS_FLAGS_DIFFSOLN 1 +#define UBLOX_NAV_STATUS_FLAGS_WKNSET 2 +#define UBLOX_NAV_STATUS_FLAGS_TOWSET 3 + +#define UBLOX_NAV_STATUS_FIXSTAT_DGPSISTAT 0 +#define UBLOX_NAV_STATUS_FIXSTAT_MAPMATCHING 6 +#define UBLOX_NAV_STATUS_FIXSTAT_MAPMATCHING_NONE 0 +#define UBLOX_NAV_STATUS_FIXSTAT_MAPMATCHING_VALID 1 +#define UBLOX_NAV_STATUS_FIXSTAT_MAPMATCHING_USED 2 +#define UBLOX_NAV_STATUS_FIXSTAT_MAPMATCHING_DR 3 +#define UBLOX_NAV_STATUS_FIXSTAT_MAPMATCHING_MASK 3 + +#define UBLOX_NAV_STATUS_FLAGS2_PSMSTATE 0 +#define UBLOX_NAV_STATUS_FLAGS2_PSMSTATE_ACQUISITION 0 +#define UBLOX_NAV_STATUS_FLAGS2_PSMSTATE_TRACKING 1 +#define UBLOX_NAV_STATUS_FLAGS2_PSMSTATE_POWER_OPTIMIZED_TRACKING 2 +#define UBLOX_NAV_STATUS_FLAGS2_PSMSTATE_INACTIVE 3 +#define UBLOX_NAV_STATUS_FLAGS2_PSMSTATE_MASK 3 + +#define UBLOX_NAV_SVINFO 0x30 + +struct ublox_nav_svinfo { + uint8_t class; /* 0x01 */ + uint8_t message; /* 0x30 */ + uint16_t length; /* 8 + 12 * numch */ + + uint32_t itow; /* ms */ + + uint8_t numch; + uint8_t globalflags; + uint16_t reserved; +}; + +#define UBLOX_NAV_SVINFO_GLOBAL_FLAGS_CHIPGEN 0 +#define UBLOX_NAV_SVINFO_GLOBAL_FLAGS_CHIPGEN_ANTARIS 0 +#define UBLOX_NAV_SVINFO_GLOBAL_FLAGS_CHIPGEN_U_BLOX_5 1 +#define UBLOX_NAV_SVINFO_GLOBAL_FLAGS_CHIPGEN_U_BLOX_6 2 +#define UBLOX_NAV_SVINFO_GLOBAL_FLAGS_CHIPGEN_MASK 7 + +struct ublox_nav_svinfo_block { + uint8_t chn; + uint8_t svid; + uint8_t flags; + uint8_t quality; + + uint8_t cno; /* dbHz */ + int8_t elev; /* deg */ + int16_t azim; /* deg */ + + int32_t prres; /* cm */ +}; + +#define UBLOX_NAV_SVINFO_BLOCK_FLAGS_SVUSED 0 +#define UBLOX_NAV_SVINFO_BLOCK_FLAGS_DIFFCORR 1 +#define UBLOX_NAV_SVINFO_BLOCK_FLAGS_ORBITAVAIL 2 +#define UBLOX_NAV_SVINFO_BLOCK_FLAGS_ORBITEPH 3 +#define UBLOX_NAV_SVINFO_BLOCK_FLAGS_UNHEALTHY 4 +#define UBLOX_NAV_SVINFO_BLOCK_FLAGS_ORBITALM 5 +#define UBLOX_NAV_SVINFO_BLOCK_FLAGS_ORBITAOP 6 +#define UBLOX_NAV_SVINFO_BLOCK_FLAGS_SMOOTHED 7 + +#define UBLOX_NAV_SVINFO_BLOCK_QUALITY_QUALITYIND 0 +#define UBLOX_NAV_SVINFO_BLOCK_QUALITY_QUALITYIND_IDLE 0 +#define UBLOX_NAV_SVINFO_BLOCK_QUALITY_QUALITYIND_SEARCHING 1 +#define UBLOX_NAV_SVINFO_BLOCK_QUALITY_QUALITYIND_ACQUIRED 2 +#define UBLOX_NAV_SVINFO_BLOCK_QUALITY_QUALITYIND_UNUSABLE 3 +#define UBLOX_NAV_SVINFO_BLOCK_QUALITY_QUALITYIND_CODE_LOCK 4 +#define UBLOX_NAV_SVINFO_BLOCK_QUALITY_QUALITYIND_CARRIER_LOCKED_5 5 +#define UBLOX_NAV_SVINFO_BLOCK_QUALITY_QUALITYIND_CARRIER_LOCKED_6 6 +#define UBLOX_NAV_SVINFO_BLOCK_QUALITY_QUALITYIND_CARRIER_LOCKED_7 7 +#define UBLOX_NAV_SVINFO_BLOCK_QUALITY_QUALITYIND_MASK 7 + +#define UBLOX_NAV_TIMEUTC 0x21 + +struct ublox_nav_timeutc { + uint8_t class; /* 0x01 */ + uint8_t message; /* 0x21 */ + uint16_t length; /* 20 */ + + uint32_t itow; /* ms */ + uint32_t tacc; /* ns */ + int32_t nano; /* ns */ + + uint16_t year; + uint8_t month; + uint8_t day; + + uint8_t hour; + uint8_t min; + uint8_t sec; + uint8_t valid; +}; + +#define UBLOX_NAV_TIMEUTC_VALID_VALIDTOW 0 +#define UBLOX_NAV_TIMEUTC_VALID_VALIDWKN 1 +#define UBLOX_NAV_TIMEUTC_VALID_VALIDUTC 2 + +#define UBLOX_NAV_VELNED 0x12 + +struct ublox_nav_velned { + uint8_t class; /* 0x01 */ + uint8_t message; /* 0x12 */ + uint16_t length; /* 36 */ + + uint32_t itow; /* ms */ + + int32_t veln; /* cm/s */ + int32_t vele; /* cm/s */ + int32_t veld; /* cm/s */ + + uint32_t speed; /* cm/s */ + uint32_t gspeed; /* cm/s */ + + int32_t heading; /* deg */ + uint32_t sacc; /* cm/s */ + uint32_t cacc; /* deg */ +}; + +#endif /* _AO_GPS_UBLOX_H_ */ diff --git a/src/test/Makefile b/src/test/Makefile index d4d98e54..08aa8cd5 100644 --- a/src/test/Makefile +++ b/src/test/Makefile @@ -1,7 +1,7 @@ vpath % ..:../core:../drivers:../util:../micropeak:../aes PROGS=ao_flight_test ao_flight_test_baro ao_flight_test_accel ao_flight_test_noisy_accel ao_flight_test_mm \ - ao_gps_test ao_gps_test_skytraq ao_convert_test ao_convert_pa_test ao_fec_test \ + ao_gps_test ao_gps_test_skytraq ao_gps_test_ublox ao_convert_test ao_convert_pa_test ao_fec_test \ ao_aprs_test ao_micropeak_test ao_fat_test ao_aes_test INCS=ao_kalman.h ao_ms5607.h ao_log.h ao_data.h altitude-pa.h altitude.h @@ -38,6 +38,9 @@ ao_gps_test: ao_gps_test.c ao_gps_sirf.c ao_gps_print.c ao_host.h ao_gps_test_skytraq: ao_gps_test_skytraq.c ao_gps_skytraq.c ao_gps_print.c ao_host.h cc $(CFLAGS) -o $@ $< +ao_gps_test_ublox: ao_gps_test_ublox.c ao_gps_ublox.c ao_gps_print.c ao_host.h + cc $(CFLAGS) -o $@ $< + ao_convert_test: ao_convert_test.c ao_convert.c altitude.h cc $(CFLAGS) -o $@ $< diff --git a/src/test/ao_gps_test_ublox.c b/src/test/ao_gps_test_ublox.c new file mode 100644 index 00000000..80671735 --- /dev/null +++ b/src/test/ao_gps_test_ublox.c @@ -0,0 +1,409 @@ +/* + * Copyright © 2009 Keith Packard + * + * 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. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. + */ + +#define AO_GPS_TEST +#include "ao_host.h" +#include +#include +#include +#include +#include +#include +#define AO_GPS_NUM_SAT_MASK (0xf << 0) +#define AO_GPS_NUM_SAT_SHIFT (0) + +#define AO_GPS_VALID (1 << 4) +#define AO_GPS_RUNNING (1 << 5) +#define AO_GPS_DATE_VALID (1 << 6) +#define AO_GPS_COURSE_VALID (1 << 7) + +struct ao_telemetry_location { + uint8_t year; + uint8_t month; + uint8_t day; + uint8_t hour; + uint8_t minute; + uint8_t second; + uint8_t flags; + int32_t latitude; /* degrees * 10⁷ */ + int32_t longitude; /* degrees * 10⁷ */ + int16_t altitude; /* m */ + uint16_t ground_speed; /* cm/s */ + uint8_t course; /* degrees / 2 */ + uint8_t pdop; /* * 5 */ + uint8_t hdop; /* * 5 */ + uint8_t vdop; /* * 5 */ + int16_t climb_rate; /* cm/s */ + uint16_t h_error; /* m */ + uint16_t v_error; /* m */ +}; + +#define UBLOX_SAT_STATE_ACQUIRED (1 << 0) +#define UBLOX_SAT_STATE_CARRIER_PHASE_VALID (1 << 1) +#define UBLOX_SAT_BIT_SYNC_COMPLETE (1 << 2) +#define UBLOX_SAT_SUBFRAME_SYNC_COMPLETE (1 << 3) +#define UBLOX_SAT_CARRIER_PULLIN_COMPLETE (1 << 4) +#define UBLOX_SAT_CODE_LOCKED (1 << 5) +#define UBLOX_SAT_ACQUISITION_FAILED (1 << 6) +#define UBLOX_SAT_EPHEMERIS_AVAILABLE (1 << 7) + +struct ao_telemetry_satellite_info { + uint8_t svid; + uint8_t c_n_1; +}; + +#define AO_TELEMETRY_SATELLITE_MAX_SAT 12 + +struct ao_telemetry_satellite { + uint8_t channels; + struct ao_telemetry_satellite_info sats[AO_TELEMETRY_SATELLITE_MAX_SAT]; +}; + +#define ao_gps_orig ao_telemetry_location +#define ao_gps_tracking_orig ao_telemetry_satellite +#define ao_gps_sat_orig ao_telemetry_satellite_info + +void +ao_mutex_get(uint8_t *mutex) +{ +} + +void +ao_mutex_put(uint8_t *mutex) +{ +} + +static int ao_gps_fd; +static FILE *ao_gps_file; + +#if 0 +static void +ao_dbg_char(char c) +{ + char line[128]; + line[0] = '\0'; + if (c < ' ') { + if (c == '\n') + sprintf (line, "\n"); + else + sprintf (line, "\\%02x", ((int) c) & 0xff); + } else { + sprintf (line, "%c", c); + } + write(1, line, strlen(line)); +} +#endif + +#include + +int +get_millis(void) +{ + struct timeval tv; + gettimeofday(&tv, NULL); + return tv.tv_sec * 1000 + tv.tv_usec / 1000; +} + +static uint8_t in_message[4096]; +static int in_len; +static uint16_t recv_len; + +static void check_ublox_message(char *which, uint8_t *msg); + +char +ao_serial1_getchar(void) +{ + char c; + uint8_t uc; + int i; + + i = getc(ao_gps_file); + if (i == EOF) { + perror("getchar"); + exit(1); + } + c = i; + uc = (uint8_t) c; + if (in_len || uc == 0xb5) { + in_message[in_len++] = c; + if (in_len == 6) { + recv_len = in_message[4] | (in_message[5] << 8); + } else if (in_len > 6 && in_len == recv_len + 8) { + check_ublox_message("recv", in_message + 2); + in_len = 0; + } + + } + return c; +} + +#define MESSAGE_LEN 4096 + +static uint8_t message[MESSAGE_LEN]; +static int message_len; +static uint16_t send_len; + +void +ao_serial1_putchar(char c) +{ + int i; + uint8_t uc = (uint8_t) c; + + if (message_len || uc == 0xb5) { + if (message_len < MESSAGE_LEN) + message[message_len++] = uc; + if (message_len == 6) { + send_len = message[4] | (message[5] << 8); + } else if (message_len > 6 && message_len == send_len + 8) { + check_ublox_message("send", message + 2); + message_len = 0; + } + } + + for (;;) { + i = write(ao_gps_fd, &c, 1); + if (i == 1) + break; + if (i < 0 && (errno == EINTR || errno == EAGAIN)) + continue; + perror("putchar"); + exit(1); + } +} + +#define AO_SERIAL_SPEED_4800 0 +#define AO_SERIAL_SPEED_9600 1 +#define AO_SERIAL_SPEED_57600 2 +#define AO_SERIAL_SPEED_115200 3 + +static void +ao_serial1_set_speed(uint8_t speed) +{ + int fd = ao_gps_fd; + struct termios termios; + + printf ("\t\tset speed %d\n", speed); + tcdrain(fd); + tcgetattr(fd, &termios); + switch (speed) { + case AO_SERIAL_SPEED_4800: + cfsetspeed(&termios, B4800); + break; + case AO_SERIAL_SPEED_9600: + cfsetspeed(&termios, B9600); + break; + case AO_SERIAL_SPEED_57600: + cfsetspeed(&termios, B57600); + break; + case AO_SERIAL_SPEED_115200: + cfsetspeed(&termios, B115200); + break; + } + tcsetattr(fd, TCSAFLUSH, &termios); + tcflush(fd, TCIFLUSH); +} + +#define ao_time() 0 + +uint8_t ao_task_minimize_latency; + +#define ao_usb_getchar() 0 + +#include "ao_gps_print.c" +#include "ao_gps_ublox.c" + +static void +check_ublox_message(char *which, uint8_t *msg) +{ + uint8_t class = msg[0]; + uint8_t id = msg[1]; + uint16_t len = msg[2] | (msg[3] << 8); + uint16_t i; + struct ao_ublox_cksum cksum_msg = { .a = msg[4 + len], + .b = msg[4 + len + 1] }; + struct ao_ublox_cksum cksum= { 0, 0 }; + + for (i = 0; i < 4 + len; i++) { + add_cksum(&cksum, msg[i]); + } + if (cksum.a != cksum_msg.a || cksum.b != cksum_msg.b) { + printf ("\t%s: cksum mismatch %02x,%02x != %02x,%02x\n", + which, + cksum_msg.a & 0xff, + cksum_msg.b & 0xff, + cksum.a & 0xff, + cksum.b & 0xff); + return; + } + switch (class) { + case UBLOX_NAV: + switch (id) { + case UBLOX_NAV_DOP: ; + struct ublox_nav_dop *nav_dop = (void *) msg; + printf ("\tnav-dop iTOW %9u gDOP %5u dDOP %5u tDOP %5u vDOP %5u hDOP %5u nDOP %5u eDOP %5u\n", + nav_dop->itow, + nav_dop->gdop, + nav_dop->ddop, + nav_dop->tdop, + nav_dop->vdop, + nav_dop->hdop, + nav_dop->ndop, + nav_dop->edop); + return; + case UBLOX_NAV_POSLLH: ; + struct ublox_nav_posllh *nav_posllh = (void *) msg; + printf ("\tnav-posllh iTOW %9u lon %12.7f lat %12.7f height %10.3f hMSL %10.3f hAcc %10.3f vAcc %10.3f\n", + nav_posllh->itow, + nav_posllh->lon / 1e7, + nav_posllh->lat / 1e7, + nav_posllh->height / 1e3, + nav_posllh->hmsl / 1e3, + nav_posllh->hacc / 1e3, + nav_posllh->vacc / 1e3); + return; + case UBLOX_NAV_SOL: ; + struct ublox_nav_sol *nav_sol = (struct ublox_nav_sol *) msg; + printf ("\tnav-sol iTOW %9u fTOW %9d week %5d gpsFix %2d flags %02x\n", + nav_sol->itow, nav_sol->ftow, nav_sol->week, + nav_sol->gpsfix, nav_sol->flags); + return; + case UBLOX_NAV_SVINFO: ; + struct ublox_nav_svinfo *nav_svinfo = (struct ublox_nav_svinfo *) msg; + printf ("\tnav-svinfo iTOW %9u numCH %3d globalFlags %02x\n", + nav_svinfo->itow, nav_svinfo->numch, nav_svinfo->globalflags); + int i; + for (i = 0; i < nav_svinfo->numch; i++) { + struct ublox_nav_svinfo_block *nav_svinfo_block = (void *) (msg + 12 + 12 * i); + printf ("\t\tchn %3u svid %3u flags %02x quality %3u cno %3u elev %3d azim %6d prRes %9d\n", + nav_svinfo_block->chn, + nav_svinfo_block->svid, + nav_svinfo_block->flags, + nav_svinfo_block->quality, + nav_svinfo_block->cno, + nav_svinfo_block->elev, + nav_svinfo_block->azim, + nav_svinfo_block->prres); + } + return; + case UBLOX_NAV_VELNED: ; + struct ublox_nav_velned *nav_velned = (void *) msg; + printf ("\tnav-velned iTOW %9u velN %10.2f velE %10.2f velD %10.2f speed %10.2f gSpeed %10.2f heading %10.5f sAcc %10.2f cAcc %10.5f\n", + nav_velned->itow, + nav_velned->veln / 1e2, + nav_velned->vele / 1e2, + nav_velned->veld / 1e2, + nav_velned->speed / 1e2, + nav_velned->gspeed / 1e2, + nav_velned->heading / 1e5, + nav_velned->sacc / 1e5, + nav_velned->cacc / 1e6); + return; + case UBLOX_NAV_TIMEUTC:; + struct ublox_nav_timeutc *nav_timeutc = (void *) msg; + printf ("\tnav-timeutc iTOW %9u tAcc %5u nano %5d %4u-%2d-%2d %2d:%02d:%02d flags %02x\n", + nav_timeutc->itow, + nav_timeutc->tacc, + nav_timeutc->nano, + nav_timeutc->year, + nav_timeutc->month, + nav_timeutc->day, + nav_timeutc->hour, + nav_timeutc->min, + nav_timeutc->sec, + nav_timeutc->valid); + return; + } + break; + } +#if 1 + printf ("\t%s: class %02x id %02x len %d:", which, class & 0xff, id & 0xff, len & 0xffff); + for (i = 0; i < len; i++) + printf (" %02x", msg[4 + i]); + printf (" cksum %02x %02x", cksum_msg.a & 0xff, cksum_msg.b & 0xff); +#endif + printf ("\n"); +} + +void +ao_dump_state(void *wchan) +{ + if (wchan == &ao_gps_data) + ao_gps_print(&ao_gps_data); + else + ao_gps_tracking_print(&ao_gps_tracking_data); + putchar('\n'); + return; +} + +int +ao_gps_open(const char *tty) +{ + struct termios termios; + int fd; + + fd = open (tty, O_RDWR); + if (fd < 0) + return -1; + + tcgetattr(fd, &termios); + cfmakeraw(&termios); + cfsetspeed(&termios, B4800); + tcsetattr(fd, TCSAFLUSH, &termios); + + tcdrain(fd); + tcflush(fd, TCIFLUSH); + return fd; +} + +#include + +static const struct option options[] = { + { .name = "tty", .has_arg = 1, .val = 'T' }, + { 0, 0, 0, 0}, +}; + +static void usage(char *program) +{ + fprintf(stderr, "usage: %s [--tty ]\n", program); + exit(1); +} + +int +main (int argc, char **argv) +{ + char *tty = "/dev/ttyUSB0"; + int c; + + while ((c = getopt_long(argc, argv, "T:", options, NULL)) != -1) { + switch (c) { + case 'T': + tty = optarg; + break; + default: + usage(argv[0]); + break; + } + } + ao_gps_fd = ao_gps_open(tty); + if (ao_gps_fd < 0) { + perror (tty); + exit (1); + } + ao_gps_file = fdopen(ao_gps_fd, "r"); + ao_gps(); + return 0; +}