saved work

2023-04-08 06:32:14 +08:00 · 2021-07-12 19:02:41 +08:00
parent 10d9fc734e
commit 6cbdebc38a
14 changed files with 337 additions and 93 deletions
--- a/src/protocol_v1/agilex_msg_parser_v1.c
+++ b/src/protocol_v1/agilex_msg_parser_v1.c
@@ -7,26 +7,40 @@
 * Copyright (c) 2021 Ruixiang Du (rdu)
 */

-#include "ugv_sdk/details/protocol_v1/agilex_protocol_v1.h"
 #include "ugv_sdk/details/protocol_v1//agilex_msg_parser_v1.h"

+#include "protocol_v1/agilex_protocol_v1.h"
+
 #include "stdio.h"
 #include "string.h"

 bool DecodeCanFrameV1(const struct can_frame *rx_frame, AgxMessage *msg) {
+  // if checksum not correct
+  if (!CalcCanFrameChecksumV1(rx_frame->can_id, (uint8_t *)rx_frame->data,
+                              rx_frame->can_dlc)) {
+    return false;
+  }
+
  switch (rx_frame->can_id) {
    case CAN_MSG_SYSTEM_STATE_ID: {
      msg->type = AgxMsgSystemState;
-      // msg->system_status_msg.id = CAN_MSG_SYSTEM_STATUS_STATUS_ID;
-      //   memcpy(msg->body.system_state_msg.data.raw, rx_frame->data,
-      //          rx_frame->can_dlc * sizeof(uint8_t));
+      msg->body.system_state_msg.vehicle_state = rx_frame->data[0];
+      msg->body.system_state_msg.control_mode = rx_frame->data[1];
+      msg->body.system_state_msg.battery_voltage =
+          ((((uint16_t)rx_frame->data[2]) << 8) | (uint16_t)rx_frame->data[3]) /
+          10.0f;
+      msg->body.system_state_msg.error_code =
+          ((((uint16_t)rx_frame->data[4]) << 8) | (uint16_t)rx_frame->data[5]);
      break;
    }
    case CAN_MSG_MOTION_STATE_ID: {
      msg->type = AgxMsgMotionState;
-      // msg->motion_status_msg.id = CAN_MSG_MOTION_CONTROL_STATUS_ID;
-      //   memcpy(msg->body.motion_state_msg.data.raw, rx_frame->data,
-      //          rx_frame->can_dlc * sizeof(uint8_t));
+      msg->body.motion_state_msg.linear_velocity =
+          ((((uint16_t)rx_frame->data[0]) << 8) | (uint16_t)rx_frame->data[1]) /
+          1000.0f;
+      msg->body.motion_state_msg.angular_velocity =
+          ((((uint16_t)rx_frame->data[2]) << 8) | (uint16_t)rx_frame->data[3]) /
+          1000.0f;
      break;
    }
    case CAN_MSG_LIGHT_STATE_ID: {
@@ -36,33 +50,21 @@ bool DecodeCanFrameV1(const struct can_frame *rx_frame, AgxMessage *msg) {
      //          rx_frame->can_dlc * sizeof(uint8_t));
      break;
    }
-    case CAN_MSG_ACTUATOR1_STATE_ID: {
-      msg->type = AgxMsgActuatorStateV1;
-      // msg->motor_driver_status_msg.id = CAN_MSG_MOTOR1_DRIVER_STATUS_ID;
-      //   msg->body.v1_actuator_state_msg.motor_id = SCOUT_MOTOR1_ID;
-      //   memcpy(msg->body.motor_driver_status_msg.data.raw, rx_frame->data,
-      //          rx_frame->can_dlc * sizeof(uint8_t));
-      break;
-    }
-    case CAN_MSG_ACTUATOR2_STATE_ID: {
-      msg->type = AgxMsgActuatorStateV1;
-      // msg->motor_driver_status_msg.id = CAN_MSG_MOTOR2_DRIVER_STATUS_ID;
-      //   msg->body.v1_actuator_state_msg.motor_id = SCOUT_MOTOR2_ID;
-      //   memcpy(msg->body.motor_driver_status_msg.data.raw, rx_frame->data,
-      //          rx_frame->can_dlc * sizeof(uint8_t));
-      break;
-    }
-    case CAN_MSG_ACTUATOR3_STATE_ID: {
-      msg->type = AgxMsgActuatorStateV1;
-      // msg->motor_driver_status_msg.id = CAN_MSG_MOTOR3_DRIVER_STATUS_ID;
-      //   msg->body.v1_actuator_state_msg.motor_id = SCOUT_MOTOR3_ID;
-      //   memcpy(msg->body.motor_driver_status_msg.data.raw, rx_frame->data,
-      //          rx_frame->can_dlc * sizeof(uint8_t));
+    case CAN_MSG_VALUE_SET_STATE_ID: {
+      msg->type = AgxMsgValueSetStateV1;
+      if (rx_frame->data[0] == 0xaa)
+        msg->body.v1_value_set_state_msg.set_neutral = true;
+      else
+        msg->body.v1_value_set_state_msg.set_neutral = false;
      break;
    }
+    case CAN_MSG_ACTUATOR1_STATE_ID:
+    case CAN_MSG_ACTUATOR2_STATE_ID:
+    case CAN_MSG_ACTUATOR3_STATE_ID:
    case CAN_MSG_ACTUATOR4_STATE_ID: {
      msg->type = AgxMsgActuatorStateV1;
-      // msg->motor_driver_status_msg.id = CAN_MSG_MOTOR4_DRIVER_STATUS_ID;
+      msg->body.v1_actuator_state_msg.motor_id =
+          rx_frame->can_id - CAN_MSG_ACTUATOR1_STATE_ID;
      //   msg->body.v1_actuator_state_msg.motor_id = SCOUT_MOTOR4_ID;
      //   memcpy(msg->body.motor_driver_status_msg.data.raw, rx_frame->data,
      //          rx_frame->can_dlc * sizeof(uint8_t));
@@ -83,10 +85,8 @@ void EncodeCanFrameV1(const AgxMessage *msg, struct can_frame *tx_frame) {
      tx_frame->can_dlc = 8;
      tx_frame->data[0] = CTRL_MODE_CMD_CAN;
      tx_frame->data[1] = ERROR_CLR_NONE;
-      tx_frame->data[2] =
-          (int8_t)(msg->body.motion_command_msg.linear_velocity * 100);
-      tx_frame->data[3] =
-          (int8_t)(msg->body.motion_command_msg.angular_velocity * 100);
+      tx_frame->data[2] = (int8_t)(msg->body.v1_motion_command_msg.linear);
+      tx_frame->data[3] = (int8_t)(msg->body.v1_motion_command_msg.angular);
      tx_frame->data[4] = 0;
      tx_frame->data[5] = 0;
      tx_frame->data[6] = count++;
@@ -95,6 +95,22 @@ void EncodeCanFrameV1(const AgxMessage *msg, struct can_frame *tx_frame) {
      break;
    }
    case AgxMsgValueSetCommandV1: {
+      static uint8_t count = 0;
+      tx_frame->can_id = CAN_MSG_VALUE_SET_COMMAND_ID;
+      tx_frame->can_dlc = 8;
+      tx_frame->data[0] =
+          msg->body.v1_value_set_command_msg.set_neutral ? 0xaa : 0x00;
+      tx_frame->data[1] = 0x00;
+      tx_frame->data[2] = 0x00;
+      tx_frame->data[3] = 0x00;
+      tx_frame->data[4] = 0x00;
+      tx_frame->data[5] = 0x00;
+      tx_frame->data[6] = 0x00;
+      tx_frame->data[7] = CalcCanFrameChecksumV1(
+          tx_frame->can_id, tx_frame->data, tx_frame->can_dlc);
+      break;
+    }
+    case AgxMsgLightCommand: {
      static uint8_t count = 0;
      tx_frame->can_id = CAN_MSG_LIGHT_COMMAND_ID;
      tx_frame->can_dlc = 8;
@@ -120,9 +136,6 @@ void EncodeCanFrameV1(const AgxMessage *msg, struct can_frame *tx_frame) {
          tx_frame->can_id, tx_frame->data, tx_frame->can_dlc);
      break;
    }
-    case AgxMsgLightCommand: {
-      break;
-    }
    default:
      break;
  }
--- a/src/protocol_v1/agilex_protocol_v1.h
+++ b/src/protocol_v1/agilex_protocol_v1.h
@@ -0,0 +1,199 @@
+/*
+ * agilex_protocol_v1.h
+ *
+ * Created on: Jul 09, 2021 20:34
+ * Description:
+ *
+ * Copyright (c) 2021 Ruixiang Du (rdu)
+ */
+
+#ifndef AGILEX_PROTOCOL_V1_H
+#define AGILEX_PROTOCOL_V1_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include <stdint.h>
+
+// define endianess of the platform
+#if (!defined(USE_LITTLE_ENDIAN) && !defined(USE_BIG_ENDIAN))
+#define USE_LITTLE_ENDIAN
+#endif
+
+#ifdef USE_BIG_ENDIAN
+#error "BIG ENDIAN IS CURRENTLY NOT SUPPORTED"
+#endif
+
+/*---------------------------- Motor IDs -------------------------------*/
+
+#define ACTUATOR1_ID ((uint8_t)0x00)
+#define ACTUATOR2_ID ((uint8_t)0x01)
+#define ACTUATOR3_ID ((uint8_t)0x02)
+#define ACTUATOR4_ID ((uint8_t)0x03)
+
+/*--------------------------- Message IDs ------------------------------*/
+
+// CAN: control group
+#define CAN_MSG_MOTION_COMMAND_ID ((uint32_t)0x130)
+#define CAN_MSG_LIGHT_COMMAND_ID ((uint32_t)0x140)
+#define CAN_MSG_VALUE_SET_COMMAND_ID ((uint32_t)0x210)
+
+// CAN: state feedback group
+#define CAN_MSG_MOTION_STATE_ID ((uint32_t)0x131)
+#define CAN_MSG_LIGHT_STATE_ID ((uint32_t)0x141)
+#define CAN_MSG_SYSTEM_STATE_ID ((uint32_t)0x151)
+
+#define CAN_MSG_VALUE_SET_STATE_ID ((uint32_t)0x211)
+
+#define CAN_MSG_ACTUATOR1_STATE_ID ((uint32_t)0x200)
+#define CAN_MSG_ACTUATOR2_STATE_ID ((uint32_t)0x201)
+#define CAN_MSG_ACTUATOR3_STATE_ID ((uint32_t)0x202)
+#define CAN_MSG_ACTUATOR4_STATE_ID ((uint32_t)0x203)
+
+/*------------------------ Frame Memory Layout -------------------------*/
+
+/* No padding in the struct */
+// reference: https://stackoverflow.com/questions/3318410/pragma-pack-effect
+#pragma pack(push, 1)
+
+#ifdef USE_LITTLE_ENDIAN
+typedef struct {
+  uint8_t high_byte;
+  uint8_t low_byte;
+} struct16_t;
+typedef struct {
+  uint8_t msb;
+  uint8_t high_byte;
+  uint8_t low_byte;
+  uint8_t lsb;
+} struct32_t;
+#elif defined(USE_BIG_ENDIAN)
+typedef struct {
+  uint8_t low_byte;
+  uint8_t high_byte;
+} struct16_t;
+typedef struct {
+  uint8_t lsb;
+  uint8_t low_byte;
+  uint8_t high_byte;
+  uint8_t msb;
+} struct32_t;
+#endif
+
+// Control messages
+#define CTRL_MODE_REMOTE ((uint8_t)0x00)
+#define CTRL_MODE_CMD_CAN ((uint8_t)0x01)
+#define CTRL_MODE_CMD_UART ((uint8_t)0x02)
+#define CTRL_MODE_COMMANDED ((uint8_t)0x03)
+
+#define ERROR_CLR_NONE ((uint8_t)0x00)
+#define ERROR_CLR_BAT_UNDER_VOL ((uint8_t)0x01)
+#define ERROR_CLR_BAT_OVER_VOL ((uint8_t)0x02)
+#define ERROR_CLR_MOTOR1_COMM ((uint8_t)0x03)
+#define ERROR_CLR_MOTOR2_COMM ((uint8_t)0x04)
+#define ERROR_CLR_MOTOR3_COMM ((uint8_t)0x05)
+#define ERROR_CLR_MOTOR4_COMM ((uint8_t)0x06)
+#define ERROR_CLR_MOTOR_DRV_OVERHEAT ((uint8_t)0x07)
+#define ERROR_CLR_MOTOR_OVERCURRENT ((uint8_t)0x08)
+
+typedef struct {
+  uint8_t control_mode;
+  uint8_t error_clear_byte;
+  int8_t linear_percentage;
+  int8_t angular_percentage;
+  uint8_t reserved0;
+  uint8_t reserved1;
+  uint8_t count;
+  uint8_t checksum;
+} MotionCommandFrame;
+
+#define LIGHT_ENABLE_CMD_CTRL ((uint8_t)0x01)
+#define LIGHT_DISABLE_CMD_CTRL ((uint8_t)0x00)
+
+#define LIGHT_MODE_CONST_OFF ((uint8_t)0x00)
+#define LIGHT_MODE_CONST_ON ((uint8_t)0x01)
+#define LIGHT_MODE_BREATH ((uint8_t)0x02)
+#define LIGHT_MODE_CUSTOM ((uint8_t)0x03)
+
+typedef struct {
+  uint8_t enable_cmd_ctrl;
+  uint8_t front_mode;
+  uint8_t front_custom;
+  uint8_t rear_mode;
+  uint8_t rear_custom;
+  uint8_t reserved0;
+  uint8_t count;
+  uint8_t checksum;
+} LightCommandFrame;
+
+typedef struct {
+  uint8_t set_neutral;
+  uint8_t reserved0;
+  uint8_t reserved1;
+  uint8_t reserved2;
+  uint8_t reserved3;
+  uint8_t reserved4;
+  uint8_t count;
+  uint8_t checksum;
+} ValueSetCommandFrame;
+
+// State feedback messages
+#define VEHICLE_STATE_NORMAL ((uint8_t)0x00)
+#define VEHICLE_STATE_ESTOP ((uint8_t)0x01)
+#define VEHICLE_STATE_EXCEPTION ((uint8_t)0x02)
+
+#define ERROR_CAN_CHECKSUM_ERROR ((uint16_t)0x0100)
+#define ERROR_MOTOR_DRV_OVERHEAT_W ((uint16_t)0x0200)
+#define ERROR_MOTOR_OVERCURRENT_W ((uint16_t)0x0400)
+#define ERROR_BAT_UNDER_VOL_W ((uint16_t)0x0800)
+#define ERROR_RC_SIGNAL_LOSS ((uint16_t)0x1000)
+#define ERROR_HIGH_BYTE_RESERVED2 ((uint16_t)0x2000)
+#define ERROR_HIGH_BYTE_RESERVED3 ((uint16_t)0x4000)
+#define ERROR_HIGH_BYTE_RESERVED4 ((uint16_t)0x8000)
+
+#define ERROR_BAT_UNDER_VOL_F ((uint16_t)0x0001)
+#define ERROR_BAT_OVER_VOL_F ((uint16_t)0x0002)
+#define ERROR_MOTOR1_COMM_F ((uint16_t)0x0004)
+#define ERROR_MOTOR2_COMM_F ((uint16_t)0x0008)
+#define ERROR_MOTOR3_COMM_F ((uint16_t)0x0010)
+#define ERROR_MOTOR4_COMM_F ((uint16_t)0x0020)
+#define ERROR_MOTOR_DRV_OVERHEAT_F ((uint16_t)0x0040)
+#define ERROR_MOTOR_OVERCURRENT_F ((uint16_t)0x0080)
+
+typedef struct {
+  uint8_t vehicle_state;
+  uint8_t control_mode;
+  struct16_t battery_voltage;
+  struct16_t error_code;
+  uint8_t count;
+  uint8_t checksum;
+} SystemStateFrame;
+
+typedef struct {
+  struct16_t linear;
+  struct16_t angular;
+  uint8_t reserved0;
+  uint8_t reserved1;
+  uint8_t count;
+  uint8_t checksum;
+} MotionStateFrame;
+
+typedef LightCommandFrame LightStateFrame;
+
+typedef struct {
+  struct16_t current;
+  struct16_t rpm;
+  int8_t temperature;
+  uint8_t reserved0;
+  uint8_t count;
+  uint8_t checksum;
+} ActuatorStateFrame;
+
+#pragma pack(pop)
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* AGILEX_PROTOCOL_V1_H */
--- a/src/protocol_v2/agilex_msg_parser_v2.c
+++ b/src/protocol_v2/agilex_msg_parser_v2.c
@@ -369,7 +369,7 @@ void EncodeCanFrameV2(const AgxMessage *msg, struct can_frame *tx_frame) {
      memcpy(tx_frame->data, (uint8_t *)(&frame), tx_frame->can_dlc);
      break;
    }
-    case AgxMsgSetMotionMode: {
+    case AgxMsgSetMotionModeCommand: {
      tx_frame->can_id = CAN_MSG_SET_MOTION_MODE_ID;
      tx_frame->can_dlc = 8;
      SetMotionModeFrame frame;