Ported over odometry and control using new serial communication

firmware/base/src/msgs.h

@@ -0,0 +1,92 @@
+#pragma once
+
+#include "crc.h"
+#include "stdio.h"
+
+typedef enum tCmdId{
+    TMSG_REQ_STATE = 0x7F,
+    TMSG_SET_WIDTH,
+    TMSG_SET_RATIO,
+    TMSG_END_CALIB,
+    TMSG_SET_SPEED,
+    TMSG_ZERO,
+    TMSG_DELIM,
+} tCmdId;
+
+typedef struct tMsgRatio {
+    double ratio;
+    unsigned char crc;
+}__attribute__((packed)) tMsgRatio;
+
+typedef struct tMsgWidth {
+    double width;
+    unsigned char crc;
+}__attribute__((packed)) tMsgWidth;
+
+typedef struct tMsgSpeed {
+    double vl;
+    double vr;
+    unsigned char crc;
+}__attribute__((packed)) tMsgSpeed;
+
+typedef struct tRespState {
+    double x;
+    double y;
+    double theta;
+    double vl;
+    double vr;
+    unsigned char crc;
+}__attribute__((packed)) tRespState;
+
+typedef struct tRespEndCalib {
+    double left_gain;
+    double right_gain;
+    unsigned char crc;
+}__attribute__((packed)) tRespEndCalib;
+
+typedef union tMsgBuf {
+    tMsgRatio msg_ratio;
+    tMsgWidth msg_width;
+    tMsgSpeed msg_speed;
+}__attribute__((packed)) tMsgBuf;
+
+#define TCMD_ID(id) ((uint16_t)((id << 8) | TMSG_DELIM))
+#define TGET_CMD(cmd) ((cmd >> 8) & 0xFF)
+
+enum {
+    TSTATE_IDLE,
+    TSTATE_BUILD_MSG,
+};
+
+inline void send_tRespState(tRespState *resp) {
+    resp->crc = crcFooter(*resp);
+    fwrite(resp, 1, sizeof(tRespState), stdout);
+    fflush(stdout);
+}
+
+inline void send_tRespEndCalib(tRespEndCalib *state) {
+    state->crc = crcFooter(*state);
+    fwrite(state, 1, sizeof(tRespEndCalib), stdout);
+    fflush(stdout);
+}
+
+inline unsigned push_tMsgBuf(tMsgBuf *buf, unsigned cnt, unsigned char byte, tCmdId id) {
+    unsigned size = 0;
+    switch (id) {
+        case TMSG_SET_RATIO:
+            size = sizeof(tMsgRatio);
+            break;
+        case TMSG_SET_WIDTH:
+            size = sizeof(tMsgRatio);
+            break;
+        case TMSG_SET_SPEED:
+            size = sizeof(tMsgRatio);
+            break;
+        default:
+            return 0;
+    }
+    if(cnt < size) {
+        ((unsigned char*)buf)[cnt] = byte;
+    }
+    return cnt == size-1;
+}