summaryrefslogtreecommitdiffstats
path: root/app/gps.c
diff options
context:
space:
mode:
Diffstat (limited to 'app/gps.c')
-rw-r--r--app/gps.c368
1 files changed, 300 insertions, 68 deletions
diff --git a/app/gps.c b/app/gps.c
index c3ac2d1..101413f 100644
--- a/app/gps.c
+++ b/app/gps.c
@@ -14,6 +14,7 @@
static EPOCH next_sec;
static int ubx_ack = 0;
+static int ubx_nack = 0;
static int ubx_ack_xfer = 0;
static int current_ref_hz = 1;
@@ -32,6 +33,9 @@ static Event_ring gps_ring;
static char gps_info[60];
+static int ubx_cfg_tmode (uint32_t time_mode, int32_t x, int32_t y, int32_t z,
+ uint32_t var, uint32_t min_dur, uint32_t max_var);
+
void exti9_5_isr (void)
{
uint32_t now = HW_CLOCK_REG;
@@ -98,12 +102,44 @@ static int ubx_recv_almanac (uint8_t *ptr, unsigned len)
return 0;
}
+double gps_lat, gps_lon;
+
+static int
+ubx_recv_nav_posllh (uint8_t *ptr, unsigned len)
+{
+ uint32_t d;
+ int32_t ilat, ilon, i;
+
+ if ((!ptr) || (len != 28))
+ return -1;
+
+ ptr += ubx_get_u32 (ptr, &d); //TOW
+ ptr += ubx_get_i32 (ptr, &ilon);
+ ptr += ubx_get_i32 (ptr, &ilat);
+ ptr += ubx_get_i32 (ptr, &i); //geoid height
+ ptr += ubx_get_i32 (ptr, &i); //msl height
+ ptr += ubx_get_u32 (ptr, &d); //hacc
+ ptr += ubx_get_u32 (ptr, &d); //vacc
+
+
+ gps_lat = 1.e-7 * (double) ilat;
+ gps_lon = 1.e-7 * (double) ilon;
+
+
+ printf ("POS: %.4f %.4f\r\n", gps_lat, gps_lon);
+
+ return 0;
+}
+
+
static int
ubx_recv_nav_status (uint8_t *ptr, unsigned len)
{
uint8_t gps_fix, flags, fix_stat, flags2;
uint32_t d;
+ unsigned kick_off_survey_mode=300;
+
if ((!ptr) || (len != 16))
return -1;
@@ -138,7 +174,8 @@ ubx_recv_nav_status (uint8_t *ptr, unsigned len)
case 5:
fix = 'T';
- gps_locked = 0;
+ gps_locked = 1;
+ kick_off_survey_mode=0;
break;
default:
@@ -169,7 +206,7 @@ ubx_recv_nav_status (uint8_t *ptr, unsigned len)
}
- // printf ("fix: %c%c\r\n",fix,fix2);
+ // printf ("fix: %c%c happy %d Fix %02x flags %02x fix_stat %02x flags2 %02x \r\n",fix,fix2,gps_happy, gps_fix, flags,fix_stat,flags2);
if ((gps_locked) && (gps_happy < 10000))
@@ -177,6 +214,16 @@ ubx_recv_nav_status (uint8_t *ptr, unsigned len)
else
gps_happy = 0;
+
+ if (kick_off_survey_mode>0) {
+ kick_off_survey_mode--;
+
+ if (!kick_off_survey_mode) {
+ printf("GPS no time fix - kicking off survey mode\n");
+ ubx_cfg_tmode (1, 0, 0, 0, 0, 3600, 3600); // 1 hour and 6cm
+ }
+ }
+
return 0;
}
@@ -207,20 +254,24 @@ ubx_recv_clock_stats (uint8_t *ptr, unsigned len)
}
+int gps_utc_diff;
+int gps_wday;
+
static int
ubx_recv_utc (uint8_t *ptr, unsigned len)
{
int32_t nano;
uint32_t acc;
+ uint32_t tow;
+ int tod, utc_tod;
uint16_t year;
uint8_t hour, min, sec, day, month, valid;
- uint32_t d;
// char buf[40];
if ((!ptr) || (len != 20))
return -1;
- ptr += ubx_get_u32 (ptr, &d); //TOW
+ ptr += ubx_get_u32 (ptr, &tow); //TOW
ptr += ubx_get_u32 (ptr, &acc); //bias
ptr += ubx_get_i32 (ptr, &nano);
ptr += ubx_get_u16 (ptr, &year);
@@ -270,8 +321,11 @@ ubx_recv_utc (uint8_t *ptr, unsigned len)
u.second = sec;
u.nanosecond = 0;
+
+
gps_time = time_utc_to_epoch (u);
+
next_sec = gps_time;
now = HW_CLOCK_REG;
@@ -281,8 +335,22 @@ ubx_recv_utc (uint8_t *ptr, unsigned len)
pll_set_offset (gps_time, abs);
- }
+ } else
+ printf ("!GPS %s\r\n", gps_info);
+
+#define SECONDS_IN_DAY (24*60*60)
+
+ utc_tod = (((hour * 60) + min) * 60) + sec;
+ tow /= 1000;
+ tod = tow % SECONDS_IN_DAY;
+ gps_wday = tow / SECONDS_IN_DAY;
+ utc_tod += 2 * SECONDS_IN_DAY;
+ utc_tod -= tod;
+
+ while (utc_tod >= (SECONDS_IN_DAY / 2)) utc_tod -= SECONDS_IN_DAY;
+
+ gps_utc_diff = utc_tod;
#if 0
sprintf (buf, "%+6dE-12 %02d%02d%02d", (int) freq,
@@ -298,12 +366,13 @@ ubx_recv_utc (uint8_t *ptr, unsigned len)
static int ubx_recv_nav_sbas (uint8_t *ptr, unsigned len)
{
+ uint32_t d4;
uint8_t prn, mode, service;
int8_t sys;
uint8_t n;
- ptr += 4;
+ ptr += ubx_get_u32 (ptr, &d4);
ptr += ubx_get_u8 (ptr, &prn);
ptr += ubx_get_u8 (ptr, &mode);
ptr += ubx_get_i8 (ptr, &sys);
@@ -312,35 +381,78 @@ static int ubx_recv_nav_sbas (uint8_t *ptr, unsigned len)
ptr += ubx_get_u8 (ptr, &n);
ptr += 3;
- printf ("GPS SBAS PRN:%d M:%d S:%d SVC:%02x\r\n", prn, mode, sys, service);
+// printf ("GPS SBAS PRN:%d M:%d S:%d SVC:%02x SVS:", prn, mode, sys, service);
while (n--) {
ptr += ubx_get_u8 (ptr, &prn);
ptr += 11;
- printf ("GPS SBAS SV:%d\r\n", prn);
+ printf (" %d", prn);
}
+ printf ("\r\n");
return 0;
}
-static int ubx_recv_nav_sat (uint8_t *ptr, unsigned len)
+static int ubx_recv_nav_svinfo (uint8_t *ptr, unsigned len)
{
- uint8_t gnssid, prn, cno, ver;
- uint8_t n;
+ uint32_t d4;
+ uint16_t d2;
+ uint8_t ch, quality, cno, svid;
+ uint8_t n, flags, cflags;
+ const char *st;
- ptr += 4;
+ return 0;
- ptr += ubx_get_u8 (ptr, &ver);
+ ptr += ubx_get_u32 (ptr, &d4);
ptr += ubx_get_u8 (ptr, &n);
- ptr += 2;
+ ptr += ubx_get_u8 (ptr, &flags);
+ ptr += ubx_get_u16 (ptr, &d2);
while (n--) {
- ptr += ubx_get_u8 (ptr, &gnssid);
- ptr += ubx_get_u8 (ptr, &prn);
+ ptr += ubx_get_u8 (ptr, &ch);
+ ptr += ubx_get_u8 (ptr, &svid);
+ ptr += ubx_get_u8 (ptr, &cflags);
+ ptr += ubx_get_u8 (ptr, &quality);
ptr += ubx_get_u8 (ptr, &cno);
- ptr += 9;
+ ptr += 7;
+
+ switch (quality) {
+ case 0:
+ st = "idle";
+ break;
+
+ case 1:
+ st = "search";
+ break;
- printf ("GPS GNSS:%2d PRN:%3d CNO:%3d\r\n", gnssid, prn, cno);
+ case 2:
+ st = "acq";
+ break;
+
+ case 3:
+ st = "inop";
+ break;
+
+ case 4:
+ st = "lock";
+ break;
+
+ case 5:
+ case 6:
+ case 7:
+ st = "good";
+ break;
+
+ default:
+ st = "?";
+ }
+
+ printf ("GPS SVID:%3d C/N:%3d %c%c%c %s\r\n",
+ svid, cno,
+ (cflags & 0x01) ? 'U' : '-',
+ (cflags & 0x02) ? 'D' : '-',
+ (cflags & 0x04) ? 'E' : '-',
+ st);
}
@@ -355,16 +467,41 @@ static int ubx_recv_rinex (uint8_t *payload, unsigned len)
return 0;
}
+static int ubx_recv_tim_svin (uint8_t *ptr, unsigned len)
+{
+ uint8_t valid, active;
+ uint32_t dur, obs, var;
+ int32_t x, y, z;
+
+ ptr += ubx_get_u32 (ptr, &dur);
+ ptr += ubx_get_i32 (ptr, &x);
+ ptr += ubx_get_i32 (ptr, &y);
+ ptr += ubx_get_i32 (ptr, &z);
+ ptr += ubx_get_u32 (ptr, &var);
+ ptr += ubx_get_u32 (ptr, &obs);
+ ptr += ubx_get_u8 (ptr, &valid);
+ ptr += ubx_get_u8 (ptr, &active);
+
+ printf ("TIM-SVIN dur %u var %u obs %u valid %02x active %02x\r\n",
+ (unsigned) dur, (unsigned) var, (unsigned) obs, valid, active);
+
+ return 0;
+}
+
static void
ubx_recv (uint8_t class, uint8_t id, uint8_t *payload, unsigned len)
{
- // printf ("RX> %02x.%02x (%d bytes)\r\n", class, id, len);
+ //printf ("RX> %02x.%02x (%d bytes)\r\n", class, id, len);
switch (class) {
case 1:
switch (id) {
+ case 0x2:
+ ubx_recv_nav_posllh (payload, len);
+ break;
+
case 0x3:
ubx_recv_nav_status (payload, len);
break;
@@ -377,13 +514,14 @@ ubx_recv (uint8_t class, uint8_t id, uint8_t *payload, unsigned len)
ubx_recv_clock_stats (payload, len);
break;
+ case 0x30:
+ ubx_recv_nav_svinfo (payload, len);
+ break;
+
case 0x32:
ubx_recv_nav_sbas (payload, len);
break;
- case 0x35:
- ubx_recv_nav_sat (payload, len);
- break;
default:
printf ("RX> %02x.%02x (%d bytes)\r\n", class, id, len);
@@ -414,10 +552,29 @@ ubx_recv (uint8_t class, uint8_t id, uint8_t *payload, unsigned len)
break;
+ case 0xd:
+ switch (id) {
+ case 0x4:
+ ubx_recv_tim_svin (payload, len);
+
+ break;
+
+ default:
+ printf ("RX> %02x.%02x (%d bytes)\r\n", class, id, len);
+ hexdump (payload, len);
+ }
+
+ break;
+
+
+
case 5:
ubx_ack++;
+
+ if (!id) ubx_nack++;
+
printf (" %s for %02x.%02x\r\n", id ? "ACK" : "NAK", payload[0],
payload[1]);
@@ -651,38 +808,35 @@ ubx_send (uint8_t class, uint8_t id, const void *_payload, unsigned len)
ubx_send_byte (ck_a);
ubx_send_byte (ck_b);
- // printf ("TX> %02x.%02x\r\n", class, id);
+ printf ("TX> %02x.%02x\r\n", class, id);
}
static int
ubx_handshake (uint8_t class, uint8_t id, const void *payload, unsigned len)
{
- uint32_t timeout;
+ uint32_t timeout=4000;
ubx_ack = 0;
+ ubx_nack = 0;
ubx_send (class, id, payload, len);
-
-
- timeout = ticks + TIMEOUT;
-
while (!ubx_ack) {
- if (ticks > timeout) {
+ if (!timeout) {
printf ("GPS timeout resending packet\r\n");
usart1_drain();
ubx_send (class, id, payload, len);
- timeout = ticks + TIMEOUT;
+ timeout = 40000;
}
-
-
+ timeout--;
gps_dispatch();
+ delay_ms(1);
}
- return 0;
+ return !!ubx_nack;
}
static int
@@ -754,6 +908,7 @@ static int
ubx_set_message_rate_port1 (uint8_t class, uint8_t id, uint8_t rate)
{
uint8_t buf[8], *ptr;
+ int ret;
ptr = buf;
@@ -766,7 +921,10 @@ ubx_set_message_rate_port1 (uint8_t class, uint8_t id, uint8_t rate)
ptr += ubx_put_u8 (ptr, 0); //nothing on spi
ptr += ubx_put_u8 (ptr, 0); //nothing on port 5
- return ubx_handshake (0x06, 0x01, buf, (unsigned) (ptr - buf));
+ ret = ubx_handshake (0x06, 0x01, buf, (unsigned) (ptr - buf));
+
+ printf ("GPS set message rate for %02x.%02x %s\r\n", class, id, ret ? "Failed" : "Succeeded");
+ return ret;
}
@@ -785,6 +943,70 @@ ubx_cfg_rst (uint16_t flags)
return 0;
}
+static int
+ubx_cfg_poll (uint8_t class, uint8_t id)
+{
+ return ubx_handshake (class, id, NULL, 0);
+}
+
+static int ubx_cfg_sbas (uint8_t mode, uint8_t usage, uint8_t channels)
+{
+ uint8_t buf[8], *ptr;
+
+ ptr = buf;
+
+ ptr += ubx_put_u8 (ptr, mode);
+ ptr += ubx_put_u8 (ptr, usage);
+ ptr += ubx_put_u8 (ptr, channels);
+ ptr += ubx_put_u8 (ptr, 0x0);
+ ptr += ubx_put_u32 (ptr, /* 0x1 PRN 120 | */ 0x8 /*PRN 123*/ | /* 0x40 PRN 126 | */ 0x10000 /*PRN136*/);
+
+ return ubx_handshake (0x06, 0x16, buf, (unsigned) (ptr - buf));
+}
+
+
+static int ubx_cfg_tmode (uint32_t time_mode, int32_t x, int32_t y, int32_t z,
+ uint32_t var, uint32_t min_dur, uint32_t max_var)
+{
+ uint8_t buf[28], *ptr;
+
+ ptr = buf;
+
+ ptr += ubx_put_u32 (ptr, time_mode);
+ ptr += ubx_put_i32 (ptr, x);
+ ptr += ubx_put_i32 (ptr, y);
+ ptr += ubx_put_i32 (ptr, z);
+ ptr += ubx_put_u32 (ptr, var);
+ ptr += ubx_put_u32 (ptr, min_dur);
+ ptr += ubx_put_u32 (ptr, max_var);
+
+ return ubx_handshake (0x06, 0x1d, buf, (unsigned) (ptr - buf));
+}
+
+static int ubx_aid_ini (int32_t lat, int32_t lon, int32_t alt, uint32_t acc)
+{
+ uint8_t buf[48], *ptr;
+
+ ptr = buf;
+
+ ptr += ubx_put_i32 (ptr, lat);
+ ptr += ubx_put_i32 (ptr, lon);
+ ptr += ubx_put_i32 (ptr, alt);
+ ptr += ubx_put_u32 (ptr, acc);
+ ptr += ubx_put_u16 (ptr, 0); //time mark flags
+ ptr += ubx_put_u16 (ptr, 0); // week number
+ ptr += ubx_put_u32 (ptr, 0); // tow s
+ ptr += ubx_put_i32 (ptr, 0); // tow ns
+ ptr += ubx_put_u32 (ptr, 0); // tow accuracy ms
+ ptr += ubx_put_u32 (ptr, 0); // tow accuracy ns
+ ptr += ubx_put_i32 (ptr, 0); // clock drift or freq
+ ptr += ubx_put_u32 (ptr, 0); // clock drift or freq accuracy
+ ptr += ubx_put_u32 (ptr, 0x21); //flags LLA no time
+
+ return ubx_handshake (0x0b, 0x01, buf, (unsigned) (ptr - buf));
+ //return ubx_send (0x0b, 0x01, NULL, 0);
+
+}
static int
gps_set_ref (int ref_hz)
@@ -794,34 +1016,48 @@ gps_set_ref (int ref_hz)
printf ("setting gps ref to %d hz\r\n", ref_hz);
+ /*Try ublox 5 version first */
+
current_ref_hz = ref_hz;
ptr = buf;
+ ptr += ubx_put_u32 (ptr, 1000000 / ref_hz); //period /us
+ ptr += ubx_put_u32 (ptr, 1000); //duration /us
+ ptr += ubx_put_i8 (ptr, 1); //+ve sense
+ ptr += ubx_put_u8 (ptr, 1); //lock to utc
+ ptr += ubx_put_u8 (ptr, 0); //pulse only when locked
+ ptr += ubx_put_u8 (ptr, 0); //res
+
+ /* 50.52 ns from 10m of RG58 */
+ /* 15.15 ns from 3m of RG174 */
+ /* total 66ns*/
+
+ ptr += ubx_put_i16 (ptr, 66); //antenna cable delay /ns
+ ptr += ubx_put_i16 (ptr, 0); //rf group delay /ns
+ ptr += ubx_put_i32 (ptr, 0); //user delay /ns
+
+ ret = ubx_handshake (0x06, 0x07, buf, (unsigned) (ptr - buf));
+
+ if (!ret) return ret;
+
+ ptr = buf;
+
ptr += ubx_put_u8 (ptr, 0); //timepluse 1
ptr += ubx_put_u8 (ptr, 0); //version 0
ptr += ubx_put_u16 (ptr, 0); //reserved
- // ptr += ubx_put_u16 (ptr, 32); //ant cable delay ns
ptr += ubx_put_u16 (ptr, 0); //ant cable delay ns
ptr += ubx_put_u16 (ptr, 0); //rf group delay ns
- ptr += ubx_put_u32 (ptr, 1000000 / ref_hz); //period unlocked/us
+ ptr += ubx_put_u32 (ptr, 0); //period unlocked/us
ptr += ubx_put_u32 (ptr, 1000000 / ref_hz); //period locked/us
// ptr += ubx_put_u32 (ptr, 0); //pulse width unlocked/us
- ptr += ubx_put_u32 (ptr, 100000 / ref_hz); //pulse width unlocked/us
- ptr += ubx_put_u32 (ptr, 100000 / ref_hz); //pulse width locked/us
+ ptr += ubx_put_u32 (ptr, 0); //pulse width unlocked/us
+ ptr += ubx_put_u32 (ptr, 1000 / ref_hz); //pulse width locked/us
ptr += ubx_put_i32 (ptr, 0); // ? delay
-
-#if 0
- /*Separate numbers for locked/unlocked*/
ptr += ubx_put_u32 (ptr, 0xf7);
-#else
- /*Same numbers for locked/unlocked*/
- ptr += ubx_put_u32 (ptr, 0xf7);
-#endif
-
ret = ubx_handshake (0x06, 0x31, buf, (unsigned) (ptr - buf));
@@ -833,7 +1069,7 @@ int
gps_init (void)
{
uint8_t buf[80], *ptr;
- unsigned len;
+ //unsigned len;
// uint16_t u2;
usart1_drain();
@@ -911,27 +1147,36 @@ gps_init (void)
gps_set_ref (1);
+#if 0
ubx_fetch (0x06, 0x31, NULL, 0, &len);
-
printf ("configured GNSS 6.31\r\n");
+#endif
ubx_set_nav_rate (1000);
#if 1
+ ubx_set_message_rate_port1 (0x01, 0x02, 10);
ubx_set_message_rate_port1 (0x01, 0x03, 1);
ubx_set_message_rate_port1 (0x01, 0x06, 0);
ubx_set_message_rate_port1 (0x01, 0x21, 1);
ubx_set_message_rate_port1 (0x01, 0x22, 1);
- ubx_set_message_rate_port1 (0x01, 0x32, 0);
- ubx_set_message_rate_port1 (0x01, 0x32, 0); /*SBAS*/
- ubx_set_message_rate_port1 (0x01, 0x35, 0); /*SV in view*/
+ ubx_set_message_rate_port1 (0x01, 0x30, 10); /*SV in view*/
+ ubx_set_message_rate_port1 (0x01, 0x32, 10); /*SBAS*/
- ubx_set_message_rate_port1 (0x03, 0x0a, 0);
- ubx_set_message_rate_port1 (0x03, 0x10, 0);
- ubx_set_message_rate_port1 (0x03, 0x21, 0);
- ubx_set_message_rate_port1 (0x04, 0x04, 0);
+ // ubx_set_message_rate_port1 (0x03, 0x0a, 0);
+ // ubx_set_message_rate_port1 (0x03, 0x10, 0);
+ // ubx_set_message_rate_port1 (0x03, 0x21, 0); //?
+ // ubx_set_message_rate_port1 (0x04, 0x04, 0);
+
+ ubx_set_message_rate_port1 (0x0D, 0x04, 10);
+
+ ubx_cfg_sbas (1, 0x7, 2);
+
+ ubx_cfg_poll (0x6, 0x16);
+
+// ubx_aid_ini (522202400, 1279080, 2900, 20000);
printf ("GNSS ready\r\n");
#else
@@ -944,19 +1189,6 @@ gps_init (void)
printf ("GNSS ready\r\n");
#endif
-#if 0
- ptr = buf;
- ptr += ubx_put_u8 (ptr, 0x1);
- ptr += ubx_put_u8 (ptr, 0x7);
- ptr += ubx_put_u8 (ptr, 0x1);
- ptr += ubx_put_u8 (ptr, 0xf);
- ptr += ubx_put_u32 (ptr, 0xffffffff);
- ubx_handshake (0x06, 0x16, buf, (unsigned) (ptr - buf));
-
- printf ("configured SBAS\r\n");
-
- ubx_handshake (0x06, 0x16, buf, 0);
-#endif
// printf ("GPS ready\r\n");
generated by cgit v1.2.3 (git 2.25.1) at 2024-05-05 07:47:20 +0000