From 6bf06fa179f6d49700083991d4425ca9a8a2e461 Mon Sep 17 00:00:00 2001 From: "Gary E. Miller" Date: Wed, 16 Jan 2019 18:16:23 -0800 Subject: driver_sirf: Fix MODE in MID 67,1 --- driver_sirf.c | 42 ++++++++++++++++++++++++++++++++++++------ 1 file changed, 36 insertions(+), 6 deletions(-) (limited to 'driver_sirf.c') diff --git a/driver_sirf.c b/driver_sirf.c index 6b5df954..4a940641 100644 --- a/driver_sirf.c +++ b/driver_sirf.c @@ -594,7 +594,7 @@ static gps_mask_t sirf_msg_67_1(struct gps_device_t *session, return 0; gpsd_log(&session->context->errout, LOG_PROG, - "SiRF V: MID 67,1\n"); + "SiRF V: MID 67,1 Multiconstellation Navigation Data Response \n"); solution_validity = getbeu32(buf, 2); if (0 != solution_validity) { @@ -645,15 +645,45 @@ static gps_mask_t sirf_msg_67_1(struct gps_device_t *session, mask |= LATLON_SET; - /* fake a mode for test */ - session->newdata.mode = MODE_3D; + switch (solution_info & 0x07) { + case 0: /* no fix */ + session->newdata.mode = MODE_NO_FIX; + break; + case 1: /* unused */ + session->newdata.mode = MODE_NO_FIX; + break; + case 2: /* unused */ + session->newdata.mode = MODE_NO_FIX; + break; + case 3: /* 3-SV KF Solution */ + session->newdata.mode = MODE_2D; + break; + case 4: /* Four or more SV KF Solution */ + session->newdata.mode = MODE_3D; + break; + case 5: /* 2-D Least-squares Solution */ + session->newdata.mode = MODE_2D; + break; + case 6: /* 3-D Least-squaresSolution */ + session->newdata.mode = MODE_3D; + break; + case 7: /* DR solution, assume 3D */ + session->newdata.mode = MODE_3D; + break; + default: /* can't really happen */ + session->newdata.mode = MODE_NO_FIX; + break; + } mask |= MODE_SET; - /* sog - speed over ground m/s * 100 */ - session->newdata.speed = getbeu16(buf, 70) * 1e-2; + if (!(solution_info & 0x01000)) { + /* sog - speed over ground m/s * 100 */ + session->newdata.speed = getbeu16(buf, 70) * 1e-2; + mask |= SPEED_SET; + } /* cog - course over ground fm true north deg * 100 */ session->newdata.track = getbeu16(buf, 72) * 1e-2; - mask |= SPEED_SET | TRACK_SET; + mask |= TRACK_SET; /* climb_rate - vertical velocity m/s * 100 */ session->newdata.climb = getbes16(buf, 74) * 1e-2; -- cgit v1.2.1