添加舵机校验计算工具代码

testtool/舵机校验计算工具/main.c

@@ -0,0 +1,75 @@
+#include <stdint.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+
+#define DATA_ID   2
+#define DATA_LEN  3
+#define DATA_TYPE 4
+
+#define DATA_CHECKSUM_START 4
+
+/*
+ * 如：FF FF FE 07 03 2A 08 00 03 E8 DA
+ * 字头：0xFF 0xFF ，连续收到两个 0xFF 表示有数据包到达。
+ * ID 号：指定舵机 ID（1~250），广播 ID（254），其他无效。
+ * 数据长度：参数值=【ID 号+指令类型+参数】的字节总数，即=N+2。
+ * 指令类型：数据包类型，如读指令、写指令等，详见指令类型列表。
+ * 参数：指令包中需要向舵机传达的有效数据，字节数可以为 0~N。
+ * ①、无参数指令：参数字节数为 0
+ * ②、写数据指令：参数的第一个字节为需要发送数据的首地址，之后则是需要发送的
+ * 数据，数据长度根据不同的地址而定。
+ * ③、读数据指令：参数字节数固定为 2 字节，第一个字节为需要读取数据的首地址，
+ * 第二个字节是需要读取数据的字节数。
+ * 数据校验：采用单字节和校验方式，计算方法如下
+ * Checksum= (~ (ID 号 + 数据长度 + 指令类型+ 参数 1+ ...+ 参数 N)) & 0xFF
+ **/
+uint8_t steering_gear_check_sum(int *buffer, int buffer_size) {
+  uint8_t checksum;
+  if (buffer[3] != (buffer_size - 3)) {  // 这buffer_size减3是为了减去两个固定位以及数据长度位
+    printf("error\n");
+    return -1;
+  }
+
+  checksum = (buffer[DATA_ID] + buffer[DATA_LEN] + buffer[DATA_TYPE]);  // 固定加这三位
+  // checksum把数据长度算里面了，但是数据长度位没把自己算里面
+  for (int i = 0; i < (buffer[DATA_LEN] - 2); i++) {
+    checksum += buffer[DATA_CHECKSUM_START + i];
+  }
+
+  return ((~checksum) & 0xff);
+}
+
+int get_int_buffer_len(int *buffer, int buffer_size) {
+  int buffer_len = 0;
+  int count      = 0;
+
+  for (int i = 0; i < buffer_size; i++) {
+    if (0 == buffer[i]) {
+      if (count == 0) {
+        buffer_len = i;
+      }
+      count = count + 1;
+    } else {
+      count = 0;
+    }
+
+    if (count >= 3) {
+      break;
+    }
+  }
+
+  return buffer_len;
+}
+
+/*
+ * 恢复出厂设置:
+ * 0xFF, 0xFF, 0x01, 0x02, 0x06
+ */
+int main(int argc, char *argv[]) {
+  int buffer[128] = {0xFF, 0xFF, 0x01, 0x04, 0x02, 0x38, 0x02};  // 恢复出厂设置
+
+  printf("check sum:0X%x\n", steering_gear_check_sum(buffer, get_int_buffer_len(buffer, (sizeof(buffer) / sizeof(int)))));
+
+  return 0;
+}