[gpsd-dev] [PATCH 3/3] TSIP: support multi-constelation chip packets

[Nuno Gonçalves]

Signed-off-by: Nuno Goncalves <address@hidden>
---
 driver_tsip.c | 116 +++++++++++++++++++++++++++++++++++++++++++++++++++++++++-
 packet.c      |  32 ++++++++++++----
 2 files changed, 139 insertions(+), 9 deletions(-)

diff --git a/driver_tsip.c b/driver_tsip.c
index f397271..293f62b 100644
--- a/driver_tsip.c
+++ b/driver_tsip.c
@@ -34,7 +34,7 @@ void configuration_packets_accutime_gold(struct
gps_device_t *session);
 void configuration_packets_generic(struct gps_device_t *session);

 #ifdef TSIP_ENABLE
-#define TSIP_CHANNELS 12
+#define TSIP_CHANNELS 15

 static int tsip_write(struct gps_device_t *session,
       unsigned int id, unsigned char *buf, size_t len)
@@ -120,7 +120,7 @@ static gps_mask_t tsip_parse_input(struct
gps_device_t *session)
     int i, j, len, count;
     gps_mask_t mask = 0;
     unsigned int id;
-    uint8_t u1, u2, u3, u4, u5;
+    uint8_t u1, u2, u3, u4, u5, u6, u7, u8, u9, u10;
     int16_t s1, s2, s3, s4;
     int32_t sl1, sl2, sl3;
     uint32_t ul1, ul2;
@@ -513,8 +513,111 @@ static gps_mask_t tsip_parse_input(struct
gps_device_t *session)
  session->gpsdata.satellites_visible = i;
  }
  break;
+     case 0x5d: /* GNSS Satellite Tracking Status (multi-GNSS operation) */
+ if (len != 26)
+    break;
+ u1 = getub(buf, 0); /* PRN */
+ u2 = getub(buf, 1); /* chan */
+ u3 = getub(buf, 2); /* Acquisition flag */
+ u4 = getub(buf, 3); /* SV used in Position or Time calculation*/
+ f1 = getbef32((char *)buf, 4); /* Signal level */
+ f2 = getbef32((char *)buf, 8); /* time of Last measurement */
+ d1 = getbef32((char *)buf, 12) * RAD_2_DEG; /* Elevation */
+ d2 = getbef32((char *)buf, 16) * RAD_2_DEG; /* Azimuth */
+ u5 = getub(buf, 20); /* old measurement flag */
+ u6 = getub(buf, 21); /* integer msec flag */
+ u7 = getub(buf, 22); /* bad data flag */
+ u8 = getub(buf, 23); /* data collection flag */
+ u9 = getub(buf, 24); /* Used flags */
+ u10 = getub(buf, 25); /* SV Type */
+
+ i = u2; /* channel number */
+ gpsd_log(&session->context->errout, LOG_INF,
+ "Satellite Tracking Status: Ch %2d Con %d PRN %3d Acq %d Use %d SNR
%4.1f LMT %.04f El %4.1f Az %5.1f Old %d Int %d Bad %d Col %d TPF %d
SVT %d\n",
+ i, u10, u1, u3, u4, f1, f2, d1, d2, u5, u6, u7, u8, u9, u10);
+ if (i < TSIP_CHANNELS) {
+    if (d1 >= 0.0) {
+ session->gpsdata.skyview[i].PRN = (short)u1;
+ session->gpsdata.skyview[i].ss = (double)f1;
+ session->gpsdata.skyview[i].elevation = (short)round(d1);
+ session->gpsdata.skyview[i].azimuth = (short)round(d2);
+ session->gpsdata.skyview[i].used = (bool)u4;
+    } else {
+ session->gpsdata.skyview[i].PRN = (short)u1;
+ session->gpsdata.skyview[i].elevation =
+    session->gpsdata.skyview[i].azimuth = 0;
+ session->gpsdata.skyview[i].ss = 0.0;
+ session->gpsdata.skyview[i].used = false;
+    }
+    if (++i == session->gpsdata.satellites_visible) {
+ session->gpsdata.skyview_time = NAN;
+ mask |= SATELLITE_SET; /* last of the series */
+    }
+    if (i > session->gpsdata.satellites_visible)
+ session->gpsdata.satellites_visible = i;
+ }
+ break;
     case 0x5e: /* Additional Fix Status Report */
  break;
+    case 0x6c: /* Satellite Selection List */
+ u1 = getub(buf, 0); /* nsvs/dimension */
+ count = (int)getub(buf, 17);
+ if (len != (18 + count))
+    break;
+ session->driver.tsip.last_6d = now; /* keep timestamp for request */
+#ifdef __UNUSED__
+ /*
+ * This looks right, but it sets a spurious mode value when
+ * the satellite constellation looks good to the chip but no
+ * actual fix has yet been acquired.  We should set the mode
+ * field (which controls gpsd's fix reporting) only from sentences
+ * that convey actual fix information, like 0x20, otherwise we
+ * get results like triggering their error modeler spuriously.
+ */
+ switch (u1 & 7) { /* dimension */
+ case 3:
+    //session->gpsdata.status = STATUS_FIX;
+    session->newdata.mode = MODE_2D;
+    break;
+ case 4:
+    //session->gpsdata.status = STATUS_FIX;
+    session->newdata.mode = MODE_3D;
+    break;
+ default:
+    //session->gpsdata.status = STATUS_NO_FIX;
+    session->newdata.mode = MODE_NO_FIX;
+    break;
+ }
+ mask |= MODE_SET;
+#endif /* __UNUSED__ */
+ session->gpsdata.satellites_used = count;
+ session->gpsdata.dop.pdop = getbef32((char *)buf, 1);
+ session->gpsdata.dop.hdop = getbef32((char *)buf, 5);
+ session->gpsdata.dop.vdop = getbef32((char *)buf, 9);
+ session->gpsdata.dop.tdop = getbef32((char *)buf, 13);
+ session->gpsdata.dop.gdop =
+    sqrt(pow(session->gpsdata.dop.pdop, 2) +
+ pow(session->gpsdata.dop.tdop, 2));
+
+ memset(session->driver.tsip.sats_used, 0,
sizeof(session->driver.tsip.sats_used));
+ buf2[0] = '\0';
+ for (i = 0; i < count; i++)
+    str_appendf(buf2, sizeof(buf2),
+   " %d", session->driver.tsip.sats_used[i] =
+   (short)getub(buf, 18 + i));
+ gpsd_log(&session->context->errout, LOG_DATA,
+ "AIVSS: 0x6d status=%d used=%d "
+ "pdop=%.1f hdop=%.1f vdop=%.1f tdop=%.1f gdup=%.1f\n",
+ session->gpsdata.status,
+ session->gpsdata.satellites_used,
+ session->gpsdata.dop.pdop,
+ session->gpsdata.dop.hdop,
+ session->gpsdata.dop.vdop,
+ session->gpsdata.dop.tdop,
+ session->gpsdata.dop.gdop);
+ mask |= DOP_SET | STATUS_SET | USED_IS;
+ break;
+ break;
     case 0x6d: /* All-In-View Satellite Selection */
  u1 = getub(buf, 0); /* nsvs/dimension */
  count = (int)((u1 >> 4) & 0x0f);
@@ -797,6 +900,15 @@ static gps_mask_t tsip_parse_input(struct
gps_device_t *session)
      session->newdata.mode, session->gpsdata.status);
     break;

+ case 0x4a: /* Set PPS Characteristics */
+    break;
+
+ case 0x4e: /* PPS Output */
+    break;
+
+ case 0xa2: /* UTC/GPS Timing */
+    break;
+
  case 0xab: /* Thunderbolt Timing Superpacket */
     if (len != 17) {
  gpsd_log(&session->context->errout, 4, "pkt 0xab len=%d\n", len);
diff --git a/packet.c b/packet.c
index 4ab0b5e..0ab4bff 100644
--- a/packet.c
+++ b/packet.c
@@ -1888,6 +1888,7 @@ void packet_parse(struct gps_lexer_t *lexer)
  * 0x43, Velocity Fix, data length 20
  * 0x45, Software Version Information, data length 10
  * 0x46, Health of Receiver, data length 2
+ * 0x47, Signal Level for All Satellites Tracked, data length 1+5*numSV
  * 0x48, GPS System Messages, data length 22
  * 0x49, Almanac Health Page, data length 32
  * 0x4a, LLA Position, data length 20
@@ -1900,8 +1901,10 @@ void packet_parse(struct gps_lexer_t *lexer)
  * 0x5a, Raw Measurements
  * 0x5b, Satellite Ephemeris Status, data length 16
  * 0x5c, Satellite Tracking Status, data length 24
+ * 0x5d, Satellite Tracking Status (multi-gnss), data length 26
  * 0x5e, Additional Fix Status Report
  * 0x5f, 0x5F-01-0B: Reset Error Codes, data length 25 - not
according to the length check below!
+ * 0x6c, Satellite Selection List, data length 18+numSV
  * 0x6d, All-In-View Satellite Selection, data length 17+numSV
  * 0x82, Differential Position Fix Mode, data length 1
  * 0x83, Double Precision XYZ, data length 36
@@ -1924,10 +1927,16 @@ void packet_parse(struct gps_lexer_t *lexer)
                 /* *INDENT-OFF* */
  if (!((0x13 == pkt_id) ||
       (0x1c == pkt_id) ||
+      (0x38 == pkt_id) ||
+      ((0x41 <= pkt_id) && (0x4c >= pkt_id)) ||
+      ((0x54 <= pkt_id) && (0x57 >= pkt_id)) ||
+      ((0x5a <= pkt_id) && (0x5f >= pkt_id)) ||
+      (0x6c == pkt_id) ||
+      (0x6d == pkt_id) ||
+      ((0x82 <= pkt_id) && (0x84 >= pkt_id)) ||
+      (0x8f == pkt_id) ||
       (0xbb == pkt_id) ||
-      (0xbc == pkt_id) ||
-      (0x38 == pkt_id))
-    && ((0x41 > pkt_id) || (0x8f < pkt_id))) {
+      (0xbc == pkt_id))) {
     gpsd_log(&lexer->errout, LOG_IO,
      "Packet ID 0x%02x out of range for TSIP\n",
      pkt_id);
@@ -1952,6 +1961,9 @@ void packet_parse(struct gps_lexer_t *lexer)
     /* pass */ ;
  else if (TSIP_ID_AND_LENGTH(0x46, 2))
     /* pass */ ;
+ /* 0x47 data length 1+5*numSV, total packetlen is 5+5*numSV */
+ else if ((0x47 == pkt_id) && ((packetlen % 5) == 0))
+    /* pass */ ;
  else if (TSIP_ID_AND_LENGTH(0x48, 22))
     /* pass */ ;
  else if (TSIP_ID_AND_LENGTH(0x49, 32))
@@ -1962,7 +1974,7 @@ void packet_parse(struct gps_lexer_t *lexer)
     /* pass */ ;
  else if (TSIP_ID_AND_LENGTH(0x4c, 17))
     /* pass */ ;
- else if (TSIP_ID_AND_LENGTH(0x54, 12))
+ else if (TSIP_ID_AND_LENGTH(0x54, 12)) //length 12 is not according
to the packet list above
     /* pass */ ;
  else if (TSIP_ID_AND_LENGTH(0x55, 4))
     /* pass */ ;
@@ -1976,6 +1988,8 @@ void packet_parse(struct gps_lexer_t *lexer)
     /* pass */ ;
  else if (TSIP_ID_AND_LENGTH(0x5c, 24))
     /* pass */ ;
+ else if (TSIP_ID_AND_LENGTH(0x5d, 26))
+    /* pass */ ;
  else if (TSIP_ID_AND_LENGTH(0x5e, 2))
     /* pass */ ;
  /*
@@ -1984,9 +1998,13 @@ void packet_parse(struct gps_lexer_t *lexer)
  */
  else if (TSIP_ID_AND_LENGTH(0x5f, 66))
     /* pass */ ;
- /* 0x6d is variable length depending on the sat picture */
+ /* 0x6c data length 18+numSV, total packetlen is 22+numSV, numSV up to 224 */
+ else if ((0x6c == pkt_id)
+ && ((22 <= packetlen) && (246 >= packetlen)))
+    /* pass */ ;
+ /* 0x6d data length 17+numSV, total packetlen is 21+numSV, numSV up to 32 */
  else if ((0x6d == pkt_id)
- && ((0x14 <= packetlen) && (0x20 >= packetlen)))
+ && ((21 <= packetlen) && (53 >= packetlen)))
     /* pass */ ;
  else if (TSIP_ID_AND_LENGTH(0x82, 1))
     /* pass */ ;
@@ -1995,7 +2013,7 @@ void packet_parse(struct gps_lexer_t *lexer)
  else if (TSIP_ID_AND_LENGTH(0x84, 36))
     /* pass */ ;
  /* super packets, variable length */
- else if ((0x8e == pkt_id) || (0x8f == pkt_id))
+ else if (0x8f == pkt_id)
     /* pass */ ;
  /*
  * This is according to [TSIP].
-- 
2.7.4

0003-TSIP-support-multi-constelation-chip-packets.patch
Description: Text Data