串口ok

2024-09-26 22:32:20 +08:00
commit 097089ef4e
323 changed files with 135661 additions and 0 deletions
@@ -0,0 +1,118 @@
+/*
+ * ParserPack.c
+ *
+ *  Created on: Jun 24, 2020
+ *      Author: matth
+ */
+
+#include "ParsePackUbx.h"
+#include "ublox.h"
+
+void s_UbxCheckSum(uint8_t *buff, uint16_t len, uint8_t *CheckA, uint8_t *CheckB)
+{
+  unsigned char cka=0,ckb=0;
+  int i;
+
+  for (i = 0; i < len; i++) 
+  {
+    cka += buff[i]; 
+    ckb += cka;
+  }
+
+  if (NULL != CheckA)
+  {
+    *CheckA = cka;
+  }
+  if (NULL != CheckB)
+  {
+    *CheckB = ckb;
+  }
+}
+
+
+void ParserUbx_init(ParserUbx_t *parser)
+{
+  parser->stage = 0;
+}
+
+int ParserUbx_char(ParserUbx_t *parser, uint8_t c)
+{
+	int rslt = -1;
+	uint8_t s_checkA = 0;
+	uint8_t s_checkB = 0;
+
+	switch (parser->stage)
+	{
+	case 0:
+		if (c == 0xB5)
+		{
+			parser->stage++;
+			parser->buff[0] = c;
+		}
+		break;
+	case 1:
+		if (c == 0x62)
+		{
+			parser->stage++;
+			parser->buff[1] = c;
+		}
+		else
+		{
+			parser->stage = 0;
+		}
+		break;
+	case 2:
+		parser->msgClass = c;
+		parser->stage++;
+		parser->buff[2] = c;
+		break;
+	case 3:
+		parser->msgId = c;
+		parser->stage++;
+		parser->buff[3] = c;
+		break;
+	case 4:
+		parser->len = c;
+		parser->stage++;
+		parser->buff[4] = c;
+		break;
+	case 5:
+		parser->len += c<<8;  //payload length
+		if (parser->len <=PARSER_UBX_BUFF_LEN)
+		{
+		  parser->idx = 0;
+		  parser->stage++;
+		  parser->buff[5] = c;
+		}
+		else
+		{
+		  parser->stage = 0;
+		}
+		break;
+	case 6:
+		parser->buff[6+parser->idx++] = c;
+		if (parser->idx == parser->len)
+		{
+			parser->stage++;
+		}
+		break;
+	case 7:
+		parser->checkA = c;
+		parser->stage++;
+		break;
+	case 8:
+		parser->checkB = c;
+		s_UbxCheckSum(&parser->buff[2], parser->len+4, &s_checkA, &s_checkB);
+		if((parser->checkA == s_checkA) && (parser->checkB == s_checkB))
+		{
+			rslt = parser->msgClass;
+		}
+		parser->stage = 0;
+		break;
+	default:
+		parser->stage = 0;
+		break;
+	}
+
+	return rslt;
+}