ISComm.c
Go to the documentation of this file.
1 /*
2 MIT LICENSE
3 
4 Copyright (c) 2014-2020 Inertial Sense, Inc. - http://inertialsense.com
5 
6 Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files(the "Software"), to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions :
7 
8 The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software.
9 
10 THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
11 */
12 
13 #include "ISComm.h"
14 
21 unsigned int calculate24BitCRCQ(unsigned char* buffer, unsigned int len)
22 {
23  static const unsigned int TABLE_CRC24Q[] =
24  {
25  0x000000,0x864CFB,0x8AD50D,0x0C99F6,0x93E6E1,0x15AA1A,0x1933EC,0x9F7F17,
26  0xA18139,0x27CDC2,0x2B5434,0xAD18CF,0x3267D8,0xB42B23,0xB8B2D5,0x3EFE2E,
27  0xC54E89,0x430272,0x4F9B84,0xC9D77F,0x56A868,0xD0E493,0xDC7D65,0x5A319E,
28  0x64CFB0,0xE2834B,0xEE1ABD,0x685646,0xF72951,0x7165AA,0x7DFC5C,0xFBB0A7,
29  0x0CD1E9,0x8A9D12,0x8604E4,0x00481F,0x9F3708,0x197BF3,0x15E205,0x93AEFE,
30  0xAD50D0,0x2B1C2B,0x2785DD,0xA1C926,0x3EB631,0xB8FACA,0xB4633C,0x322FC7,
31  0xC99F60,0x4FD39B,0x434A6D,0xC50696,0x5A7981,0xDC357A,0xD0AC8C,0x56E077,
32  0x681E59,0xEE52A2,0xE2CB54,0x6487AF,0xFBF8B8,0x7DB443,0x712DB5,0xF7614E,
33  0x19A3D2,0x9FEF29,0x9376DF,0x153A24,0x8A4533,0x0C09C8,0x00903E,0x86DCC5,
34  0xB822EB,0x3E6E10,0x32F7E6,0xB4BB1D,0x2BC40A,0xAD88F1,0xA11107,0x275DFC,
35  0xDCED5B,0x5AA1A0,0x563856,0xD074AD,0x4F0BBA,0xC94741,0xC5DEB7,0x43924C,
36  0x7D6C62,0xFB2099,0xF7B96F,0x71F594,0xEE8A83,0x68C678,0x645F8E,0xE21375,
37  0x15723B,0x933EC0,0x9FA736,0x19EBCD,0x8694DA,0x00D821,0x0C41D7,0x8A0D2C,
38  0xB4F302,0x32BFF9,0x3E260F,0xB86AF4,0x2715E3,0xA15918,0xADC0EE,0x2B8C15,
39  0xD03CB2,0x567049,0x5AE9BF,0xDCA544,0x43DA53,0xC596A8,0xC90F5E,0x4F43A5,
40  0x71BD8B,0xF7F170,0xFB6886,0x7D247D,0xE25B6A,0x641791,0x688E67,0xEEC29C,
41  0x3347A4,0xB50B5F,0xB992A9,0x3FDE52,0xA0A145,0x26EDBE,0x2A7448,0xAC38B3,
42  0x92C69D,0x148A66,0x181390,0x9E5F6B,0x01207C,0x876C87,0x8BF571,0x0DB98A,
43  0xF6092D,0x7045D6,0x7CDC20,0xFA90DB,0x65EFCC,0xE3A337,0xEF3AC1,0x69763A,
44  0x578814,0xD1C4EF,0xDD5D19,0x5B11E2,0xC46EF5,0x42220E,0x4EBBF8,0xC8F703,
45  0x3F964D,0xB9DAB6,0xB54340,0x330FBB,0xAC70AC,0x2A3C57,0x26A5A1,0xA0E95A,
46  0x9E1774,0x185B8F,0x14C279,0x928E82,0x0DF195,0x8BBD6E,0x872498,0x016863,
47  0xFAD8C4,0x7C943F,0x700DC9,0xF64132,0x693E25,0xEF72DE,0xE3EB28,0x65A7D3,
48  0x5B59FD,0xDD1506,0xD18CF0,0x57C00B,0xC8BF1C,0x4EF3E7,0x426A11,0xC426EA,
49  0x2AE476,0xACA88D,0xA0317B,0x267D80,0xB90297,0x3F4E6C,0x33D79A,0xB59B61,
50  0x8B654F,0x0D29B4,0x01B042,0x87FCB9,0x1883AE,0x9ECF55,0x9256A3,0x141A58,
51  0xEFAAFF,0x69E604,0x657FF2,0xE33309,0x7C4C1E,0xFA00E5,0xF69913,0x70D5E8,
52  0x4E2BC6,0xC8673D,0xC4FECB,0x42B230,0xDDCD27,0x5B81DC,0x57182A,0xD154D1,
53  0x26359F,0xA07964,0xACE092,0x2AAC69,0xB5D37E,0x339F85,0x3F0673,0xB94A88,
54  0x87B4A6,0x01F85D,0x0D61AB,0x8B2D50,0x145247,0x921EBC,0x9E874A,0x18CBB1,
55  0xE37B16,0x6537ED,0x69AE1B,0xEFE2E0,0x709DF7,0xF6D10C,0xFA48FA,0x7C0401,
56  0x42FA2F,0xC4B6D4,0xC82F22,0x4E63D9,0xD11CCE,0x575035,0x5BC9C3,0xDD8538
57  };
58 
59  unsigned int crc = 0;
60  for (uint32_t i = 0; i != len; i++)
61  {
62  crc = ((crc << 8) & 0xFFFFFF) ^ TABLE_CRC24Q[(crc >> 16) ^ buffer[i]];
63  }
64  return crc;
65 }
66 
74 unsigned int getBitsAsUInt32(const unsigned char* buffer, unsigned int pos, unsigned int len)
75 {
76  unsigned int bits = 0;
77  for (unsigned int i = pos; i < pos + len; i++)
78  {
79  bits = (bits << 1) + ((buffer[i / 8] >> (7 - i % 8)) & 1u);
80  }
81  return bits;
82 }
83 
85 static int s_packetEncodingEnabled = 1;
86 
87 // Replace special character with encoded equivalent and add to buffer
88 static uint8_t* encodeByteAddToBuffer(uint32_t val, uint8_t* ptrDest)
89 {
90  switch (val)
91  {
93  case PSC_ASCII_END_BYTE:
94  case PSC_START_BYTE:
95  case PSC_END_BYTE:
96  case PSC_RESERVED_KEY:
97  case UBLOX_START_BYTE1:
98  case RTCM3_START_BYTE:
100  {
101  *ptrDest++ = PSC_RESERVED_KEY;
102  *ptrDest++ = (uint8_t)~val;
103  }
104  else
105  {
106  *ptrDest++ = (uint8_t)val;
107  }
108  break;
109  default:
110  *ptrDest++ = (uint8_t)val;
111  break;
112  }
113 
114  return ptrDest;
115 }
116 
117 static int dataIdShouldSwap(uint32_t dataId)
118 {
119  switch (dataId)
120  {
121  case DID_GPS1_VERSION:
122  case DID_GPS2_VERSION:
123  return 0;
124  }
125  return 1;
126 }
127 
128 static void swapPacket(packet_t* pkt)
129 {
131  {
132  if ((pkt->hdr.pid == PID_DATA || pkt->hdr.pid == PID_SET_DATA) && pkt->body.size >= sizeof(p_data_hdr_t))
133  {
134  // swap the data header only
135  flipEndianess32(pkt->body.ptr, sizeof(p_data_hdr_t));
136  }
137  }
138  else if (pkt->body.size < sizeof(p_data_hdr_t) || (pkt->hdr.pid != PID_DATA && pkt->hdr.pid != PID_SET_DATA))
139  {
140  // swap entire packet, not a data packet
141  flipEndianess32(pkt->body.ptr, pkt->body.size);
142  }
143  else
144  {
145  // swap header
146  flipEndianess32(pkt->body.ptr, sizeof(p_data_hdr_t));
147 
148  // get header
149  p_data_hdr_t* dataHdr = (p_data_hdr_t*)pkt->body.ptr;
150 
151  // if dev_info_t, swap only the uint32 fields, this data structure is handled special as it contains char[] arrays and uint32_t in the same struct
152  if (dataHdr->id == DID_DEV_INFO && pkt->body.size == sizeof(p_data_hdr_t) + sizeof(dev_info_t))
153  {
154  // swap only the pieces that need swapping
155  dev_info_t* devInfo = (dev_info_t*)(pkt->body.ptr + sizeof(p_data_hdr_t));
156  devInfo->buildNumber = SWAP32(devInfo->buildNumber);
157  devInfo->repoRevision = SWAP32(devInfo->repoRevision);
158  devInfo->serialNumber = SWAP32(devInfo->serialNumber);
159  }
160  else if (dataIdShouldSwap(dataHdr->id))
161  {
162  // swap entire packet body
163  flipEndianess32(pkt->body.ptr + sizeof(p_data_hdr_t), pkt->body.size - sizeof(p_data_hdr_t));
164 
165  // flip doubles
166  uint16_t* offsets;
167  uint16_t offsetsLength;
168  uint8_t* dataBuf = pkt->body.ptr + sizeof(p_data_hdr_t);
169 
170  // flip doubles back if needed
171  if ((offsets = getDoubleOffsets(dataHdr->id, &offsetsLength)))
172  {
173  flipDoubles(dataBuf, dataHdr->size, dataHdr->offset, offsets, offsetsLength);
174  }
175 
176  // flip strings back if needed
177  if ((offsets = getStringOffsetsLengths(dataHdr->id, &offsetsLength)))
178  {
179  flipStrings(dataBuf, dataHdr->size, dataHdr->offset, offsets, offsetsLength);
180  }
181  }
182  }
183 }
184 
185 void is_comm_init(is_comm_instance_t* instance, uint8_t *buffer, int bufferSize)
186 {
187  // Clear buffer and initialize buffer pointers
188  memset(buffer, 0, bufferSize);
189  instance->buf.size = bufferSize;
190  instance->buf.start = buffer;
191  instance->buf.end = buffer + bufferSize;
192  instance->buf.head = instance->buf.tail = instance->buf.scan = buffer;
193 
194  // Set parse enable flags
195  instance->config.enableISB = 1;
196  instance->config.enableASCII = 1;
197  instance->config.enableUblox = 1;
198  instance->config.enableRTCM3 = 1;
199 
200  instance->txPktCount = 0;
201  instance->rxErrorCount = 0;
202  instance->hasStartByte = 0;
203  memset(&instance->dataHdr, 0, sizeof(p_data_hdr_t));
204  instance->dataPtr = instance->buf.start;
205  instance->ackNeeded = 0;
206  memset(&instance->pkt, 0, sizeof(packet_t));
207  instance->pkt.body.ptr = instance->buf.start;
208  instance->pkt.body.size = 0;
209  instance->altDecodeBuf = NULL;
210 }
211 
212 static __inline void reset_parser(is_comm_instance_t *instance)
213 {
214  instance->hasStartByte = 0;
215  instance->buf.head = instance->buf.scan;
216 }
217 
219 {
220  // Packet to decode into
221  packet_t *pkt = &(instance->pkt);
222  int pktSize = (int)(instance->buf.scan - instance->buf.head);
223  uint8_t* head;
224 
225  // Set location where to decode packet.
226  if(instance->altDecodeBuf)
227  { // Decode to alternate buffer
228  memcpy(instance->altDecodeBuf, instance->buf.head, pktSize);
229  head = pkt->body.ptr = instance->altDecodeBuf;
230  }
231  else
232  { // Decode packet in place on top of the receive buffer to save memory
233  head = instance->buf.head;
234  instance->pkt.body.ptr = instance->buf.start;
235  }
236 
237  instance->pktPtr = instance->buf.head;
238  reset_parser(instance);
239 
240  if (is_decode_binary_packet(pkt, head, pktSize) == 0)
241  {
242  instance->ackNeeded = 0;
243  instance->dataPtr = NULL;
244 
245  switch(pkt->hdr.pid)
246  {
247  case PID_SET_DATA:
248  case PID_DATA:
249  instance->dataHdr = *((p_data_hdr_t*)pkt->body.ptr);
250 
251  // ensure offset and size are in bounds - check the size independent of offset because the size could be a
252  // negative number in case of corrupt data
253  if (instance->dataHdr.id > DID_NULL &&
254  instance->dataHdr.id < DID_COUNT &&
255  instance->dataHdr.size <= MAX_DATASET_SIZE //&&
256 // instance->dataHdr.offset <= MAX_DATASET_SIZE &&
257 // instance->dataHdr.offset + instance->dataHdr.size <= MAX_DATASET_SIZE
258  )
259  {
260  if(pkt->hdr.pid==PID_SET_DATA)
261  { // acknowledge valid data received
262  instance->ackNeeded = PID_ACK;
263  }
264 
265  // Update data pointer
266  instance->dataPtr = pkt->body.ptr + sizeof(p_data_hdr_t);
268  }
269  else
270  { // negative acknowledge data received
271  instance->ackNeeded = PID_NACK;
272  }
273  break;
274 
275  case PID_GET_DATA:
276  // copy the requested data set info
277  instance->dataHdr = *((p_data_hdr_t*)pkt->body.ptr);
283  case PID_ACK:
284  case PID_NACK:
286  }
287  }
288 
289  // Invalid data or checksum failure.
290  instance->rxErrorCount++;
291  return _PTYPE_PARSE_ERROR;
292 }
293 
295 {
296  uint8_t* head = instance->buf.head;
297  reset_parser(instance);
298 
299  // calculate checksum, if pass return special data id
300  if (instance->buf.scan - head > 7)
301  {
302  // parse out checksum, put in temp null terminator
303  uint8_t tmp = *(instance->buf.scan - 2);
304  *(instance->buf.scan - 2) = 0;
305  int actualCheckSum = (int)strtol((const char*)instance->buf.scan - 4, 0, 16);
306  *(instance->buf.scan - 2) = tmp;
307  int dataCheckSum = 0;
308  for (uint8_t* ptr = head + 1, *ptrEnd = instance->buf.scan - 5; ptr < ptrEnd; ptr++)
309  {
310  dataCheckSum ^= (int)*ptr;
311  }
312  if (actualCheckSum == dataCheckSum)
313  { // valid ASCII Data
314  // Update data pointer and info
315  instance->dataPtr = instance->pktPtr = head;
316  instance->dataHdr.id = 0;
317  instance->dataHdr.size = (uint32_t)(instance->buf.scan - head);
318  instance->dataHdr.offset = 0;
319  return _PTYPE_ASCII_NMEA;
320  }
321  }
322 
323  // Invalid data or checksum failure.
324  instance->rxErrorCount++;
325  return _PTYPE_PARSE_ERROR;
326 }
327 
329 {
330  switch (instance->parseState)
331  {
332  case 1: // preamble 2
333  if (*(instance->buf.scan - 1) != UBLOX_START_BYTE2)
334  {
335  // corrupt data
336  instance->rxErrorCount++;
337  reset_parser(instance);
338  return _PTYPE_PARSE_ERROR;
339  }
340  // fall through
341  case 2: // class id
342  // fall through
343  case 3: // message id
344  // fall through
345  case 4: // length byte 1
346  // fall through
347  case 0:
348  instance->parseState++;
349  break;
350 
351  case 5: // length byte 2
352  {
353  uint32_t len = BE_SWAP16(*((uint16_t*)(void*)(instance->buf.scan - 2)));
354 
355  // if length is greater than available buffer, we cannot parse this ublox packet - ublox header is 6 bytes
356  if (len > instance->buf.size - 6)
357  {
358  instance->rxErrorCount++;
359  reset_parser(instance);
360  return _PTYPE_PARSE_ERROR;
361  }
362  instance->parseState = -((int32_t)len + 2);
363  }
364  break;
365 
366  default:
367  if (++instance->parseState == 0)
368  {
369  // end of ublox packet, if checksum passes, send the external id
370  instance->hasStartByte = 0;
371  uint8_t actualChecksum1 = *(instance->buf.scan - 2);
372  uint8_t actualChecksum2 = *(instance->buf.scan - 1);
373  uint8_t calcChecksum1 = 0;
374  uint8_t calcChecksum2 = 0;
375 
376  // calculate checksum, skipping the first two preamble bytes and the last two bytes which are the checksum
377  for (uint8_t* ptr = instance->buf.head + 2, *ptrEnd = instance->buf.scan - 2; ptr < ptrEnd; ptr++)
378  {
379  calcChecksum1 += *ptr;
380  calcChecksum2 += calcChecksum1;
381  }
382  if (actualChecksum1 == calcChecksum1 && actualChecksum2 == calcChecksum2)
383  { // Checksum passed - Valid ublox packet
384  // Update data pointer and info
385  instance->dataPtr = instance->buf.head;
386  instance->dataHdr.id = 0;
387  instance->dataHdr.size = (uint32_t)(instance->buf.scan - instance->buf.head);
388  instance->dataHdr.offset = 0;
389  instance->pktPtr = instance->buf.head;
390  reset_parser(instance);
391  return _PTYPE_UBLOX;
392  }
393  else
394  { // Checksum failure
395  instance->rxErrorCount++;
396  reset_parser(instance);
397  return _PTYPE_PARSE_ERROR;
398  }
399  }
400  }
401 
402  return _PTYPE_NONE;
403 }
404 
406 {
407  switch (instance->parseState)
408  {
409  case 0:
410  case 1:
411  instance->parseState++;
412  break;
413 
414  case 2:
415  {
416  uint32_t msgLength = getBitsAsUInt32(instance->buf.head, 14, 10);
417 
418  // if message is too small or too big for rtcm3 or too big for buffer, fail
419  if (msgLength > 1023 || msgLength > instance->buf.size - 6)
420  {
421  // corrupt data
422  instance->rxErrorCount++;
423  reset_parser(instance);
424  return _PTYPE_PARSE_ERROR;
425  }
426 
427  // parse the message plus 3 crc24 bytes
428  instance->parseState = -((int32_t)msgLength + 3);
429  } break;
430 
431  default:
432  if (++instance->parseState == 0)
433  {
434  // get len without 3 crc bytes
435  int lenWithoutCrc = (int)((instance->buf.scan - instance->buf.head) - 3);
436  uint32_t actualCRC = calculate24BitCRCQ(instance->buf.head, lenWithoutCrc);
437  uint32_t correctCRC = getBitsAsUInt32(instance->buf.head + lenWithoutCrc, 0, 24);
438 
439  if (actualCRC == correctCRC)
440  { // Checksum passed - Valid RTCM3 packet
441  // Update data pointer and info
442  instance->dataPtr = instance->buf.head;
443  instance->dataHdr.id = 0;
444  instance->dataHdr.size = (uint32_t)(instance->buf.scan - instance->buf.head);
445  instance->dataHdr.offset = 0;
446  instance->pktPtr = instance->buf.head;
447  reset_parser(instance);
448  return _PTYPE_RTCM3;
449  }
450  else
451  { // Checksum failure
452  instance->rxErrorCount++;
453  reset_parser(instance);
454  return _PTYPE_PARSE_ERROR;
455  }
456  }
457  }
458 
459  return _PTYPE_NONE;
460 }
461 
463 {
464 // if (instance == 0 || instance->buf.start == 0)
465 // {
466 // return -1;
467 // }
468 
469  is_comm_buffer_t *buf = &(instance->buf);
470 
471  int bytesFree = (int)(buf->end - buf->tail);
472 
473  // if we are out of free space, we need to either move bytes over or start over
474  if (bytesFree == 0)
475  {
476  if ((int)(buf->head - buf->start) < (int)(buf->size / 3)) // if ring buffer start index is less than this and no space is left, clear the entire ring buffer
477  { // we will be hung unless we flush the ring buffer, we have to drop bytes in this case and the caller
478  // will need to resend the data
479  buf->head = buf->start;
480  buf->tail = buf->start;
481  buf->scan = buf->start;
482  }
483  else
484  { // shift over the remaining data in the hopes that we will get a valid packet by appending the next read call
485  memmove(buf->start, buf->head, buf->tail - buf->head);
486  int shift = (int)(buf->head - buf->start);
487  buf->head -= shift;
488  buf->tail -= shift;
489  buf->scan -= shift;
490  }
491 
492  // re-calculate free byte count
493  bytesFree = (int)(buf->end - buf->tail);
494  }
495 
496  return bytesFree;
497 }
498 
500 {
501  // Reset buffer if needed
502  is_comm_free(instance);
503 
504  // Add byte to buffer
505  *(instance->buf.tail) = byte;
506  instance->buf.tail++;
507 
508  return is_comm_parse(instance);
509 }
510 
511 #define FOUND_START_BYTE(init) if(init){ instance->hasStartByte = byte; instance->buf.head = instance->buf.scan-1; }
512 #define START_BYTE_SEARCH_ERROR()
513 
515 {
516  is_comm_buffer_t *buf = &(instance->buf);
517 
518  // Search for packet
519  while (buf->scan < buf->tail)
520  {
521  uint8_t byte = *(buf->scan++);
522 
523  // Check for start byte if we haven't found it yet
524  if (instance->hasStartByte == 0)
525  {
526  if ((byte == PSC_START_BYTE && instance->config.enableISB) ||
527  (byte == PSC_ASCII_START_BYTE && instance->config.enableASCII) ||
528  (byte == UBLOX_START_BYTE1 && instance->config.enableUblox) ||
529  (byte == RTCM3_START_BYTE && instance->config.enableRTCM3) )
530  { // Found start byte. Initialize states (set flag and reset pos to beginning)
531  instance->hasStartByte = byte;
532  instance->buf.head = instance->buf.scan-1;
533  instance->parseState = 0;
534  }
535  else
536  { // Searching for start byte
537  if (instance->parseState != -1)
538  {
539  instance->parseState = -1;
540  instance->rxErrorCount++;
541  return _PTYPE_PARSE_ERROR; // Return to notify of error
542  }
543  continue; // Continue to scan for data
544  }
545  }
546 
547  // If we have a start byte, process the data type
548  switch (instance->hasStartByte)
549  {
550  case PSC_START_BYTE:
551  if (byte == PSC_END_BYTE)
552  {
553  return processInertialSensePkt(instance);
554  }
555  break;
557  if (byte == PSC_ASCII_END_BYTE)
558  {
559  return processAsciiPkt(instance);
560  }
561  //Check for invalid bytes in ASCII string and exit if found.
562  if (byte == PSC_START_BYTE || byte == PSC_END_BYTE || byte == 0)
563  {
564  instance->hasStartByte = 0;
565  instance->parseState = -1;
566  instance->rxErrorCount++;
567  return _PTYPE_PARSE_ERROR; // Return to notify of error
568  }
569  break;
570  case UBLOX_START_BYTE1:
571  if (processUbloxByte(instance) != _PTYPE_NONE)
572  {
573  return _PTYPE_UBLOX;
574  }
575  break;
576  case RTCM3_START_BYTE:
577  if (processRtcm3Byte(instance) != _PTYPE_NONE)
578  {
579  return _PTYPE_RTCM3;
580  }
581  }
582  }
583 
584  // No valid data yet...
585  return _PTYPE_NONE;
586 }
587 
588 int is_comm_get_data(is_comm_instance_t* instance, uint32_t dataId, uint32_t offset, uint32_t size, uint32_t periodMultiple)
589 {
590  p_data_get_t request;
591 
592  request.id = dataId;
593  request.offset = offset;
594  request.size = size;
595  request.bc_period_multiple = periodMultiple;
596 
597  packet_hdr_t hdr;
598  hdr.flags = 0;
599  hdr.pid = PID_GET_DATA;
600  hdr.counter = (uint8_t)instance->txPktCount++;
601 
602  return is_encode_binary_packet(&request, sizeof(request), &hdr, 0, instance->buf.start, instance->buf.size);
603 }
604 
605 int is_comm_get_data_rmc(is_comm_instance_t* instance, uint64_t rmcBits)
606 {
607  return is_comm_set_data(instance, DID_RMC, offsetof(rmc_t,bits), sizeof(uint64_t), (void*)&rmcBits);
608 }
609 
610 static int sendData(is_comm_instance_t* instance, uint32_t dataId, uint32_t offset, uint32_t size, void* data, uint32_t pid)
611 {
612  int dataSize = size + sizeof(p_data_hdr_t);
613  uint8_t toSend[MAX_DATASET_SIZE];
614  if(dataSize > MAX_DATASET_SIZE)
615  {
616  return -1;
617  }
618 
619  memcpy(toSend + sizeof(p_data_hdr_t), data, size);
620  p_data_hdr_t* dataHdr = (p_data_hdr_t*)toSend;
621  dataHdr->id = dataId;
622  dataHdr->size = size;
623  dataHdr->offset = offset;
624 
625  packet_hdr_t hdr;
626  hdr.flags = 0;
627  hdr.pid = (uint8_t)pid;
628  hdr.counter = (uint8_t)instance->txPktCount++;
629 
630  int result = is_encode_binary_packet(toSend, dataSize, &hdr, 0, instance->buf.start, instance->buf.size);
631  return result;
632 }
633 
634 int is_comm_set_data(is_comm_instance_t* instance, uint32_t dataId, uint32_t offset, uint32_t size, void* data)
635 {
636  return sendData(instance, dataId, offset, size, data, PID_SET_DATA);
637 }
638 
639 int is_comm_data(is_comm_instance_t* instance, uint32_t dataId, uint32_t offset, uint32_t size, void* data)
640 {
641  return sendData(instance, dataId, offset, size, data, PID_DATA);
642 }
643 
645 {
646  packet_hdr_t hdr;
647  hdr.flags = 0;
649  hdr.counter = (uint8_t)instance->txPktCount++;
650 
651  return is_encode_binary_packet(0, 0, &hdr, 0, instance->buf.start, instance->buf.size);
652 }
653 
655 {
656  packet_hdr_t hdr;
657  hdr.flags = 0;
659  hdr.counter = (uint8_t)instance->txPktCount++;
660 
661  return is_encode_binary_packet(0, 0, &hdr, 0, instance->buf.start, instance->buf.size);
662 }
663 
664 #if 0
665 
670 int is_comm_ack(is_comm_instance_t* instance, uint32_t did)
671 {
672  if(did!=0 && instance->ackNeeded==0)
673  {
674  return 0;
675  }
676 
677  int ackSize;
678  bufPtr_t data;
679 
680  // Create and Send request packet
681  p_ack_t ack;
682  ack.hdr.pktInfo = instance->ackNeeded;
683  ack.hdr.pktCounter = instance->pktCounter;
684  ackSize = sizeof(p_ack_hdr_t);
685 
686  // Set ack body
687  // switch (pid)
688  // {
689  // case PID_SET_DATA:
690  // memcpy(&(ack.buf), (p_data_hdr_t*)(instance->buffer), sizeof(p_data_hdr_t));
691  // ackSize += sizeof(p_data_hdr_t);
692  // break;
693  // }
694 
695  data.ptr = (unsigned char*)&ack;
696  data.size = ackSize;
697 
698  int result = is_encode_binary_packet(&ack, ackSize, &hdr, 0, instance->buffer, instance->bufferSize);
699 
700  instance->ackNeeded = 0;
701 }
702 #endif
703 
704 void is_decode_binary_packet_footer(packet_ftr_t* ftr, uint8_t* ptrSrc, uint8_t** ptrSrcEnd, uint32_t* checksum)
705 {
706  int state = 0;
707  uint8_t* currentPtr = (*ptrSrcEnd) - 1;
708  memset(ftr, 0, sizeof(uint32_t));
709 
710  // we need a state machine to ensure we don't overrun ptrSrcEnd
711  while (state != 7 && currentPtr > ptrSrc)
712  {
713  switch (state)
714  {
715  case 0: // packet end byte
716  ftr->stopByte = *currentPtr--;
717  state = 1;
718  break;
719 
720  case 1: // packet checksum 1
721  ftr->cksum1 = *currentPtr--;
722  state = (3 - (*currentPtr == PSC_RESERVED_KEY));
723  break;
724 
725  case 2: // packet checksum 1 is encoded
726  ftr->cksum1 = ~ftr->cksum1;
727  currentPtr--;
728  state = 3;
729  break;
730 
731  case 3: // packet checksum 2
732  ftr->cksum2 = *currentPtr--;
733  state = (5 - (*currentPtr == PSC_RESERVED_KEY));
734  break;
735 
736  case 4: // packet checksum 2 is encoded
737  ftr->cksum2 = ~ftr->cksum2;
738  currentPtr--;
739  state = 5;
740  break;
741 
742  case 5: // packet checksum 3
743  ftr->cksum3 = *currentPtr;
744  state = (7 - (*(currentPtr - 1) == PSC_RESERVED_KEY));
745  break;
746 
747  case 6: // packet checksum 3 is encoded
748  ftr->cksum3 = ~ftr->cksum3;
749  currentPtr--;
750  state = 7;
751  break;
752 
753  default:
754  break;
755  }
756  }
757  *ptrSrcEnd = currentPtr;
758  *checksum = ((uint32_t)ftr->cksum1) | (0x0000FF00 & ((uint32_t)ftr->cksum2 << 8)) | (0x00FF0000 & ((uint32_t)ftr->cksum3 << 16));
759 }
760 
761 int is_decode_binary_packet_byte(uint8_t** _ptrSrc, uint8_t** _ptrDest, uint32_t* checksum, uint32_t shift)
762 {
763  uint8_t* ptrSrc = *_ptrSrc;
764 
765  // packet id byte
766  uint32_t val = *ptrSrc++;
767  switch (val)
768  {
770  case PSC_ASCII_END_BYTE:
771  case PSC_START_BYTE:
772  case PSC_END_BYTE:
773  case RTCM3_START_BYTE:
774  case UBLOX_START_BYTE1:
775  // corrupt data
776  return -1;
777 
778  case PSC_RESERVED_KEY:
779  // skip special byte
780  val = (~(*ptrSrc++) & 0x000000FF);
781  // fall through
782  default:
783  *checksum ^= (val << shift);
784  *((*_ptrDest)++) = (uint8_t)val;
785  }
786  *_ptrSrc = ptrSrc;
787 
788  return 0;
789 }
790 
791 int is_encode_binary_packet(void* srcBuffer, unsigned int srcBufferLength, packet_hdr_t* hdr, uint8_t additionalPktFlags, void* encodedPacket, int encodedPacketLength)
792 {
793  // Ensure data size is small enough, assuming packet size could double after encoding.
794  if (srcBufferLength > MAX_PKT_BODY_SIZE)
795  {
796  return -1;
797  }
798 
799  // Update Packet Counter
800  uint8_t* ptrSrc;
801  uint8_t* ptrSrcEnd;
802  uint8_t* ptrDest = (uint8_t*)encodedPacket;
803  uint8_t* ptrDestEnd = ptrDest + encodedPacketLength;
804  uint32_t shifter = 0;
805  uint32_t checkSumValue = CHECKSUM_SEED;
806  uint32_t val;
807 
808  if (ptrDest >= ptrDestEnd)
809  {
810  return -1;
811  }
812  // Packet header -------------------------------------------------------------------------------------------
813  *ptrDest++ = PSC_START_BYTE;
814 
815  // PID
816  if (ptrDest >= ptrDestEnd)
817  {
818  return -1;
819  }
820  val = hdr->pid;
821  ptrDest = encodeByteAddToBuffer(val, ptrDest);
822  checkSumValue ^= val;
823 
824  // Counter
825  if (ptrDest >= ptrDestEnd)
826  {
827  return -1;
828  }
829  val = hdr->counter;
830  ptrDest = encodeByteAddToBuffer(val, ptrDest);
831  checkSumValue ^= (val << 8);
832 
833  // Flags
834  if (ptrDest >= ptrDestEnd)
835  {
836  return -1;
837  }
838  val = hdr->flags | additionalPktFlags | CPU_IS_LITTLE_ENDIAN | CM_PKT_FLAGS_CHECKSUM_24_BIT;
839  ptrDest = encodeByteAddToBuffer(val, ptrDest);
840  checkSumValue ^= (val << 16);
841 
842  // Packet body ----------------------------------------------------------------------------------------------
843  if (srcBufferLength > 0)
844  {
845  ptrSrc = (uint8_t*)srcBuffer;
846  ptrSrcEnd = ptrSrc + srcBufferLength;
847 
848  // copy body bytes, doing encoding and checksum
849  while (ptrSrc != ptrSrcEnd && ptrDest < ptrDestEnd)
850  {
851  val = *ptrSrc++;
852  checkSumValue ^= (val << shifter);
853 
854  // increment shifter
855  shifter += 8;
856 
857  // reset if shifter equals 24
858  shifter *= (shifter != 24);
859 
860  ptrDest = encodeByteAddToBuffer(val, ptrDest);
861  }
862  }
863 
864  // footer ----------------------------------------------------------------------------------------------------
865 
866  // checksum byte 3
867  if (ptrDest >= ptrDestEnd)
868  {
869  return -1;
870  }
871  val = (uint8_t)((checkSumValue >> 16) & 0xFF);
872  ptrDest = encodeByteAddToBuffer(val, ptrDest);
873 
874  // checksum byte 2
875  if (ptrDest >= ptrDestEnd)
876  {
877  return -1;
878  }
879  val = (uint8_t)(checkSumValue >> 8) & 0xFF;
880  ptrDest = encodeByteAddToBuffer(val, ptrDest);
881 
882  // checksum byte 1
883  if (ptrDest >= ptrDestEnd)
884  {
885  return -1;
886  }
887  val = (uint8_t)(checkSumValue & 0xFF);
888  ptrDest = encodeByteAddToBuffer(val, ptrDest);
889 
890  // packet end byte
891  if (ptrDest >= ptrDestEnd)
892  {
893  return -1;
894  }
895  *ptrDest++ = PSC_END_BYTE;
896  return (int)(ptrDest - (uint8_t*)encodedPacket);
897 }
898 
899 // This function will decode a packet in place if altBuf is NULL.
900 int is_decode_binary_packet(packet_t* pkt, unsigned char* pbuf, int pbufSize)
901 {
902  // before we even get in this method, we can be assured that pbuf starts with a packet start byte and ends with a packet end byte
903  // all other data can potentially be garbage
904  if (pbufSize < 8)
905  {
906  // corrupt data
907  return -1;
908  }
909 
910  // decode the body and calculate checksum
911  uint8_t* ptrSrc = pbuf;
912  uint8_t* ptrSrcEnd = pbuf + pbufSize;
913  packet_ftr_t ftr;
914  uint32_t actualCheckSumValue;
915 
916  is_decode_binary_packet_footer(&ftr, ptrSrc, &ptrSrcEnd, &actualCheckSumValue);
917  uint32_t shifter = 0;
918  uint32_t checkSumValue = CHECKSUM_SEED;
919 
920  // start packet byte
921  uint8_t* ptrDest = (uint8_t*)&pkt->hdr;
922  *ptrDest++ = *ptrSrc++;
923 
924  if
925  (
926  // packet id
927  is_decode_binary_packet_byte(&ptrSrc, &ptrDest, &checkSumValue, 0) ||
928 
929  // packet counter
930  is_decode_binary_packet_byte(&ptrSrc, &ptrDest, &checkSumValue, 8) ||
931 
932  // packet flags
933  is_decode_binary_packet_byte(&ptrSrc, &ptrDest, &checkSumValue, 16)
934  )
935  {
936  return -1;
937  }
938 
939  // decode the body - start shift 0
940  ptrDest = pkt->body.ptr;
941  while (ptrSrc < ptrSrcEnd)
942  {
943  if (is_decode_binary_packet_byte(&ptrSrc, &ptrDest, &checkSumValue, shifter))
944  {
945  return -1;
946  }
947 
948  shifter += 8;
949 
950  // reset if shifter equals 24
951  shifter *= (shifter != 24);
952  }
953 
954  if (actualCheckSumValue != checkSumValue)
955  {
956  // corrupt data
957  return -1;
958  }
959 
960  pkt->body.size = (uint32_t)(ptrDest - pkt->body.ptr);
961  if (pkt->body.size > MAX_PKT_BODY_SIZE)
962  {
963  return -1;
964  }
965 
966  // if the endianness of the packet doesn't match our CPU, we need to flip the data so it will be correct for our CPU architecture
967  else if (pkt->body.size != 0 && (pkt->hdr.flags & CM_PKT_FLAGS_ENDIANNESS_MASK) != CPU_IS_LITTLE_ENDIAN)
968  {
969  swapPacket(pkt);
970  }
971 
972  return 0;
973 }
974 
975 char copyDataPToStructP(void *sptr, const p_data_t *data, const unsigned int maxsize)
976 {
977  if ((data->hdr.size + data->hdr.offset) <= maxsize)
978  {
979  memcpy((uint8_t*)sptr + data->hdr.offset, data->buf, data->hdr.size);
980  return 0;
981  }
982  else
983  {
984  return -1;
985  }
986 }
987 
989 char copyDataPToStructP2(void *sptr, const p_data_hdr_t *dataHdr, const uint8_t *dataBuf, const unsigned int maxsize)
990 {
991  if ((dataHdr->size + dataHdr->offset) <= maxsize)
992  {
993  memcpy((uint8_t*)sptr + dataHdr->offset, dataBuf, dataHdr->size);
994  return 0;
995  }
996  else
997  {
998  return -1;
999  }
1000 }
1001 
1002 void is_enable_packet_encoding(int enabled)
1003 {
1004  s_packetEncodingEnabled = enabled;
1005 }
1006 
1008 char is_comm_copy_to_struct(void *sptr, const is_comm_instance_t *instance, const unsigned int maxsize)
1009 {
1010  if ((instance->dataHdr.size + instance->dataHdr.offset) <= maxsize)
1011  {
1012  memcpy((uint8_t*)sptr + instance->dataHdr.offset, instance->dataPtr, instance->dataHdr.size);
1013  return 0;
1014  }
1015  else
1016  {
1017  return -1;
1018  }
1019 }
uint16_t * getDoubleOffsets(eDataIDs dataId, uint16_t *offsetsLength)
Definition: data_sets.c:141
uint8_t enableISB
Definition: ISComm.h:469
void is_decode_binary_packet_footer(packet_ftr_t *ftr, uint8_t *ptrSrc, uint8_t **ptrSrcEnd, uint32_t *checksum)
Definition: ISComm.c:704
bufPtr_t body
Definition: ISComm.h:360
static int dataIdShouldSwap(uint32_t dataId)
Definition: ISComm.c:117
static __inline void reset_parser(is_comm_instance_t *instance)
Definition: ISComm.c:212
uint32_t repoRevision
Definition: data_sets.h:457
#define MAX_PKT_BODY_SIZE
Definition: ISComm.h:105
void is_enable_packet_encoding(int enabled)
Definition: ISComm.c:1002
#define PID_STOP_BROADCASTS_ALL_PORTS
Definition: ISComm.h:185
static protocol_type_t processUbloxByte(is_comm_instance_t *instance)
Definition: ISComm.c:328
uint8_t * end
Definition: ISComm.h:450
uint32_t id
Definition: ISComm.h:375
uint32_t size
Definition: ISComm.h:378
is_comm_config_t config
Definition: ISComm.h:491
uint8_t enableASCII
Definition: ISComm.h:472
static void swapPacket(packet_t *pkt)
Definition: ISComm.c:128
static int sendData(is_comm_instance_t *instance, uint32_t dataId, uint32_t offset, uint32_t size, void *data, uint32_t pid)
Definition: ISComm.c:610
uint16_t * getStringOffsetsLengths(eDataIDs dataId, uint16_t *offsetsLength)
Definition: data_sets.c:382
uint8_t cksum1
Definition: ISComm.h:347
uint32_t size
Definition: ISComm.h:208
uint8_t enableRTCM3
Definition: ISComm.h:478
if(udd_ctrl_interrupt())
Definition: usbhs_device.c:688
uint8_t enableUblox
Definition: ISComm.h:475
int32_t parseState
Definition: ISComm.h:503
#define DID_COUNT
Definition: data_sets.h:138
packet_hdr_t hdr
Definition: ISComm.h:357
#define DID_DEV_INFO
Definition: data_sets.h:35
#define DID_RMC
Definition: data_sets.h:43
protocol_type_t is_comm_parse_byte(is_comm_instance_t *instance, uint8_t byte)
Definition: ISComm.c:499
int is_comm_get_data(is_comm_instance_t *instance, uint32_t dataId, uint32_t offset, uint32_t size, uint32_t periodMultiple)
Definition: ISComm.c:588
#define NULL
Definition: nm_bsp.h:52
char copyDataPToStructP(void *sptr, const p_data_t *data, const unsigned int maxsize)
Definition: ISComm.c:975
unsigned int getBitsAsUInt32(const unsigned char *buffer, unsigned int pos, unsigned int len)
Definition: ISComm.c:74
char is_comm_copy_to_struct(void *sptr, const is_comm_instance_t *instance, const unsigned int maxsize)
Definition: ISComm.c:1008
#define DID_GPS2_VERSION
Definition: data_sets.h:52
uint32_t pktInfo
Definition: ISComm.h:424
#define PID_SET_DATA
Definition: ISComm.h:184
uint8_t stopByte
Definition: ISComm.h:350
static int s_packetEncodingEnabled
Definition: ISComm.c:85
uint32_t offset
Definition: ISComm.h:404
uint32_t pktCounter
Definition: ISComm.h:427
p_ack_hdr_t hdr
Definition: ISComm.h:434
void flipDoubles(uint8_t *data, int dataLength, int offset, uint16_t *offsets, uint16_t offsetsLength)
Definition: data_sets.c:91
int is_decode_binary_packet_byte(uint8_t **_ptrSrc, uint8_t **_ptrDest, uint32_t *checksum, uint32_t shift)
Definition: ISComm.c:761
void flipEndianess32(uint8_t *data, int dataLength)
Definition: data_sets.c:73
is_comm_buffer_t buf
Definition: ISComm.h:488
int is_comm_set_data(is_comm_instance_t *instance, uint32_t dataId, uint32_t offset, uint32_t size, void *data)
Definition: ISComm.c:634
#define PID_STOP_BROADCASTS_CURRENT_PORT
Definition: ISComm.h:187
#define DID_GPS1_VERSION
Definition: data_sets.h:51
unsigned int calculate24BitCRCQ(unsigned char *buffer, unsigned int len)
Definition: ISComm.c:21
int is_encode_binary_packet(void *srcBuffer, unsigned int srcBufferLength, packet_hdr_t *hdr, uint8_t additionalPktFlags, void *encodedPacket, int encodedPacketLength)
Definition: ISComm.c:791
uint32_t size
Definition: ISComm.h:453
static uint8_t * encodeByteAddToBuffer(uint32_t val, uint8_t *ptrDest)
Definition: ISComm.c:88
int is_comm_free(is_comm_instance_t *instance)
Definition: ISComm.c:462
uint32_t buildNumber
Definition: data_sets.h:451
uint8_t * pktPtr
Definition: ISComm.h:512
uint8_t cksum2
Definition: ISComm.h:344
#define PID_GET_DATA
Definition: ISComm.h:182
#define BE_SWAP16(_i)
Definition: data_sets.h:3535
static protocol_type_t processRtcm3Byte(is_comm_instance_t *instance)
Definition: ISComm.c:405
#define PID_NACK
Definition: ISComm.h:181
#define PID_ACK
Definition: ISComm.h:180
uint8_t * start
Definition: ISComm.h:447
const unsigned int g_validBaudRates[IS_BAUDRATE_COUNT]
Definition: ISComm.c:84
uint32_t hasStartByte
Definition: ISComm.h:500
p_data_hdr_t hdr
Definition: ISComm.h:388
uint32_t ackNeeded
Definition: ISComm.h:518
uint32_t rxErrorCount
Definition: ISComm.h:497
uint8_t * tail
Definition: ISComm.h:459
void flipStrings(uint8_t *data, int dataLength, int offset, uint16_t *offsets, uint16_t offsetsLength)
Definition: data_sets.c:117
uint32_t id
Definition: ISComm.h:398
USBInterfaceDescriptor data
uint8_t * scan
Definition: ISComm.h:462
#define SWAP32(v)
Definition: ISConstants.h:282
uint8_t pid
Definition: ISComm.h:322
void is_comm_init(is_comm_instance_t *instance, uint8_t *buffer, int bufferSize)
Definition: ISComm.c:185
int is_decode_binary_packet(packet_t *pkt, unsigned char *pbuf, int pbufSize)
Definition: ISComm.c:900
uint32_t txPktCount
Definition: ISComm.h:494
#define CHECKSUM_SEED
Definition: ISComm.h:114
uint32_t serialNumber
Definition: data_sets.h:442
p_data_hdr_t dataHdr
Definition: ISComm.h:506
#define PID_DATA
Definition: ISComm.h:183
uint8_t cksum3
Definition: ISComm.h:341
#define DID_NULL
Definition: data_sets.h:34
uint8_t * head
Definition: ISComm.h:456
protocol_type_t
Definition: ISComm.h:75
uint8_t * ptr
Definition: ISComm.h:205
uint8_t * altDecodeBuf
Definition: ISComm.h:515
#define PID_STOP_DID_BROADCAST
Definition: ISComm.h:186
protocol_type_t is_comm_parse(is_comm_instance_t *instance)
Definition: ISComm.c:514
int is_comm_data(is_comm_instance_t *instance, uint32_t dataId, uint32_t offset, uint32_t size, void *data)
Definition: ISComm.c:639
uint8_t buf[MAX_DATASET_SIZE]
Definition: ISComm.h:391
packet_t pkt
Definition: ISComm.h:521
static protocol_type_t processAsciiPkt(is_comm_instance_t *instance)
Definition: ISComm.c:294
uint8_t flags
Definition: ISComm.h:334
int is_comm_stop_broadcasts_all_ports(is_comm_instance_t *instance)
Definition: ISComm.c:644
struct PACKED dev_info_t
uint32_t bc_period_multiple
Definition: ISComm.h:410
static protocol_type_t processInertialSensePkt(is_comm_instance_t *instance)
Definition: ISComm.c:218
uint32_t offset
Definition: ISComm.h:381
int is_comm_stop_broadcasts_current_port(is_comm_instance_t *instance)
Definition: ISComm.c:654
#define MAX_DATASET_SIZE
Definition: ISComm.h:91
uint8_t counter
Definition: ISComm.h:325
uint32_t size
Definition: ISComm.h:401
char copyDataPToStructP2(void *sptr, const p_data_hdr_t *dataHdr, const uint8_t *dataBuf, const unsigned int maxsize)
Definition: ISComm.c:989
int is_comm_get_data_rmc(is_comm_instance_t *instance, uint64_t rmcBits)
Definition: ISComm.c:605
uint8_t * dataPtr
Definition: ISComm.h:509


inertial_sense_ros
Author(s):
autogenerated on Sat Sep 19 2020 03:19:04