updated v1 protocol

include/ugv_sdk/details/interface/agilex_message.h

@@ -43,7 +43,7 @@ typedef struct {
 } LightOperation;
 
 typedef struct {
-  bool cmd_ctrl_allowed;
+  bool enable_cmd_ctrl;
   LightOperation front_light;
   LightOperation rear_light;
 } LightCommandMessage;

include/ugv_sdk/details/interface/ranger_interface.hpp

@@ -40,13 +40,13 @@ struct RangerLightCmd {
   RangerLightCmd() = default;
   RangerLightCmd(LightMode f_mode, uint8_t f_value, LightMode r_mode,
                  uint8_t r_value)
-      : cmd_ctrl_allowed(true),
+      : enable_cmd_ctrl(true),
         front_mode(f_mode),
         front_custom_value(f_value),
         rear_mode(r_mode),
         rear_custom_value(r_value) {}
 
-  bool cmd_ctrl_allowed = false;
+  bool enable_cmd_ctrl = false;
   LightMode front_mode;
   uint8_t front_custom_value;
   LightMode rear_mode;

include/ugv_sdk/details/interface/tracer_interface.hpp

@@ -38,11 +38,11 @@ struct TracerMotionCmd {
 struct TracerLightCmd {
   TracerLightCmd() = default;
   TracerLightCmd(LightMode f_mode, uint8_t f_value)
-      : cmd_ctrl_allowed(true),
+      : enable_cmd_ctrl(true),
         front_mode(f_mode),
         front_custom_value(f_value) {}
 
-  bool cmd_ctrl_allowed = false;
+  bool enable_cmd_ctrl = false;
   LightMode front_mode;
   uint8_t front_custom_value;
 };

include/ugv_sdk/details/protocol_v1/agilex_message_v1.h

@@ -0,0 +1,167 @@
+/*
+ * agilex_message_v1.h
+ *
+ * Created on: Jul 09, 2021 20:55
+ * Description:
+ *  all values are using SI units (e.g. meter/second/radian)
+ *
+ * Re-implemented as a subset of AgxMessage defined in protocol v2
+ *
+ * Copyright (c) 2021 Ruixiang Du (rdu)
+ */
+
+#ifndef AGILEX_MESSAGE_V1_H
+#define AGILEX_MESSAGE_V1_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include <stdbool.h>
+#include <stdint.h>
+#include <string.h>
+
+/***************** Control messages *****************/
+
+typedef enum {
+  //   CONTROL_MODE_STANDBY = 0x00,
+  CONTROL_MODE_RC = 0x00,
+  CONTROL_MODE_CAN = 0x01,
+  CONTROL_MODE_UART = 0x02
+} ControlMode;
+
+// 0x130
+typedef struct {
+  ControlMode control_mode;
+  uint8_t fault_clear;
+
+  float linear;
+  float angular;
+} MotionCommandMessage;
+
+// 0x140
+typedef enum {
+  CONST_OFF = 0x00,
+  CONST_ON = 0x01,
+  BREATH = 0x02,
+  CUSTOM = 0x03
+} LightMode;
+
+typedef struct {
+  LightMode mode;
+  uint8_t custom_value;
+} LightOperation;
+
+typedef struct {
+  bool enable_cmd_ctrl;
+  LightOperation front_light;
+  LightOperation rear_light;
+} LightCommandMessage;
+
+typedef struct {
+  bool set_neutral;
+} ValueSetCommandMessage;
+
+/**************** Feedback messages *****************/
+
+typedef enum {
+  VehicleStateNormal = 0x00,
+  VehicleStateEStop = 0x01,
+  VehicleStateException = 0x02
+} VehicleState;
+
+#define SYSTEM_ERROR_MOTOR_DRIVER_MASK ((uint16_t)0x0100)
+#define SYSTEM_ERROR_HL_COMM_MASK ((uint16_t)0x0200)
+#define SYSTEM_ERROR_BATTERY_FAULT_MASK ((uint16_t)0x0001)
+#define SYSTEM_ERROR_BATTERY_WARNING_MASK ((uint16_t)0x0002)
+#define SYSTEM_ERROR_RC_SIGNAL_LOSS_MASK ((uint16_t)0x0004)
+#define SYSTEM_ERROR_MOTOR1_COMM_MASK ((uint16_t)0x0008)
+#define SYSTEM_ERROR_MOTOR2_COMM_MASK ((uint16_t)0x0010)
+#define SYSTEM_ERROR_MOTOR3_COMM_MASK ((uint16_t)0x0020)
+#define SYSTEM_ERROR_MOTOR4_COMM_MASK ((uint16_t)0x0040)
+#define SYSTEM_ERROR_STEER_ENCODER_MASK ((uint16_t)0x0080)
+
+typedef struct {
+  VehicleState vehicle_state;
+  ControlMode control_mode;
+  float battery_voltage;
+  uint16_t error_code;
+} SystemStateMessage;
+
+// 0x221
+typedef struct {
+  float linear_velocity;
+  float angular_velocity;  // only valid for differential drivering
+  float lateral_velocity;
+  float steering_angle;  // only valid for ackermann steering
+} MotionStateMessage;
+
+// 0x231
+typedef LightCommandMessage LightStateMessage;
+
+// 0x251 - 0x258
+typedef struct {
+  uint8_t motor_id;
+  int16_t rpm;
+  float current;
+  int32_t pulse_count;
+} ActuatorStateMessage;
+
+// 0x261 - 0x264
+#define DRIVER_STATE_INPUT_VOLTAGE_LOW_MASK ((uint8_t)0x01)
+#define DRIVER_STATE_MOTOR_OVERHEAT_MASK ((uint8_t)0x02)
+#define DRIVER_STATE_DRIVER_OVERLOAD_MASK ((uint8_t)0x04)
+#define DRIVER_STATE_DRIVER_OVERHEAT_MASK ((uint8_t)0x08)
+#define DRIVER_STATE_SENSOR_FAULT_MASK ((uint8_t)0x10)
+#define DRIVER_STATE_DRIVER_FAULT_MASK ((uint8_t)0x20)
+#define DRIVER_STATE_DRIVER_ENABLED_MASK ((uint8_t)0x40)
+#define DRIVER_STATE_DRIVER_RESET_MASK ((uint8_t)0x80)
+
+// 0x291
+typedef struct {
+  uint8_t motion_mode;
+  uint8_t mode_changing;
+} MotionModeFeedbackMessage;
+
+typedef struct {
+  uint8_t motor_id;
+  float driver_voltage;
+  float driver_temperature;
+  int8_t motor_temperature;
+  uint8_t driver_state;
+} ActuatorLSStateMessage;
+
+//////////////////////////////////////////////////////
+
+typedef enum {
+  AgxMsgUnkonwn = 0x00,
+  // command
+  AgxMsgMotionCommand,
+  AgxMsgLightCommand,
+  AgxMsgValueSetCommand,
+  // state feedback
+  AgxMsgSystemState,
+  AgxMsgMotionState,
+  AgxMsgLightState,
+  AgxMsgActuatorState,
+} MsgType;
+
+typedef struct {
+  MsgType type;
+  union {
+    // command
+    MotionCommandMessage motion_command_msg;
+    LightCommandMessage light_command_msg;
+    // state feedback
+    SystemStateMessage system_state_msg;
+    MotionStateMessage motion_state_msg;
+    LightStateMessage light_state_msg;
+    ActuatorStateMessage actuator_state_msg;
+  } body;
+} AgxMessageV1;
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* AGILEX_MESSAGE_V1_H */

include/ugv_sdk/details/robot_base/agilex_base.hpp

@@ -75,7 +75,7 @@ class AgilexBase : public RobotInterface {
     if (can_connected_) {
       AgxMessage msg;
       msg.type = AgxMsgLightCommand;
-      msg.body.light_command_msg.cmd_ctrl_allowed = true;
+      msg.body.light_command_msg.enable_cmd_ctrl = true;
       msg.body.light_command_msg.front_light.mode = front_mode;
       msg.body.light_command_msg.front_light.custom_value = front_custom_value;
       msg.body.light_command_msg.rear_light.mode = rear_mode;
@@ -93,7 +93,7 @@ class AgilexBase : public RobotInterface {
       AgxMessage msg;
       msg.type = AgxMsgLightCommand;
 
-      msg.body.light_command_msg.cmd_ctrl_allowed = false;
+      msg.body.light_command_msg.enable_cmd_ctrl = false;
 
       // send to can bus
       can_frame frame;

src/protocol_v1/agx_protocol_v1.h

@@ -0,0 +1,197 @@
+/*
+ * agx_protocol_v1.h
+ *
+ * Created on: Jul 09, 2021 20:34
+ * Description:
+ *
+ * Copyright (c) 2021 Ruixiang Du (rdu)
+ */
+
+#ifndef AGX_PROTOCOL_V1_H
+#define AGX_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)0x211)
+
+// 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_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 /* AGX_PROTOCOL_V1_H */

src/protocol_v1/scout/scout_protocol.h

@@ -51,10 +51,10 @@ extern "C" {
 /*--------------------- Control/State Constants ------------------------*/
 
 // Motion Control
-#define CTRL_MODE_REMOTE                ((uint8_t)0x00)
+// #define CTRL_MODE_REMOTE ((uint8_t)0x00)
-#define CTRL_MODE_CMD_CAN               ((uint8_t)0x01)
+// #define CTRL_MODE_CMD_CAN ((uint8_t)0x01)
-#define CTRL_MODE_CMD_UART              ((uint8_t)0x02)
+// #define CTRL_MODE_CMD_UART ((uint8_t)0x02)
-#define CTRL_MODE_COMMANDED             ((uint8_t)0x03)
+// #define CTRL_MODE_COMMANDED ((uint8_t)0x03)
 
 #define FAULT_CLR_NONE ((uint8_t)0x00)
 #define FAULT_CLR_BAT_UNDER_VOL ((uint8_t)0x01)
@@ -105,13 +105,10 @@ extern "C" {
 #pragma pack(push, 1)
 
 // Note: id could be different for UART and CAN protocol
-
 // Motion Control
 typedef struct {
-    union
+  union {
-    {
+    struct {
-        struct
-        {
       uint8_t control_mode;
       uint8_t fault_clear_flag;
       int8_t linear_velocity_cmd;
@@ -126,17 +123,13 @@ typedef struct {
 } MotionControlMessage;
 
 typedef struct {
-    union
+  union {
-    {
+    struct {
-        struct
+      struct {
-        {
-            struct
-            {
         uint8_t high_byte;
         uint8_t low_byte;
       } linear_velocity;
-            struct
+      struct {
-            {
         uint8_t high_byte;
         uint8_t low_byte;
       } angular_velocity;
@@ -151,19 +144,15 @@ typedef struct {
 
 // System Status Feedback
 typedef struct {
-    union
+  union {
-    {
+    struct {
-        struct
-        {
       uint8_t base_state;
       uint8_t control_mode;
-            struct
+      struct {
-            {
         uint8_t high_byte;
         uint8_t low_byte;
       } battery_voltage;
-            struct
+      struct {
-            {
         uint8_t high_byte;
         uint8_t low_byte;
       } fault_code;
@@ -176,15 +165,13 @@ typedef struct {
 
 // Light Control
 typedef struct {
-    union
+  union {
-    {
+    struct {
-        struct
-        {
       uint8_t light_ctrl_enable;
-            uint8_t front_light_mode;
+      uint8_t front_mode;
-            uint8_t front_light_custom;
+      uint8_t front_custom;
-            uint8_t rear_light_mode;
+      uint8_t rear_mode;
-            uint8_t rear_light_custom;
+      uint8_t rear_custom;
       uint8_t reserved0;
       uint8_t count;
       uint8_t checksum;
@@ -194,15 +181,13 @@ typedef struct {
 } LightControlMessage;
 
 typedef struct {
-    union
+  union {
-    {
+    struct {
-        struct
-        {
       uint8_t light_ctrl_enable;
-            uint8_t front_light_mode;
+      uint8_t front_mode;
-            uint8_t front_light_custom;
+      uint8_t front_custom;
-            uint8_t rear_light_mode;
+      uint8_t rear_mode;
-            uint8_t rear_light_custom;
+      uint8_t rear_custom;
       uint8_t reserved0;
       uint8_t count;
       uint8_t checksum;
@@ -212,19 +197,15 @@ typedef struct {
 } LightStatusMessage;
 
 // Motor Driver Feedback
-typedef struct
+typedef struct {
-{
   uint8_t motor_id;
   union {
-        struct
+    struct {
-        {
+      struct {
-            struct
-            {
         uint8_t high_byte;
         uint8_t low_byte;
       } current;
-            struct
+      struct {
-            {
         uint8_t high_byte;
         uint8_t low_byte;
       } rpm;
@@ -238,8 +219,7 @@ typedef struct
 } MotorDriverStatusMessage;
 
 // For convenience to access status/control message
-typedef enum
+typedef enum {
-{
   ScoutMsgNone = 0x00,
   // status messages
   ScoutMotionStatusMsg = 0x01,
@@ -251,8 +231,7 @@ typedef enum
   ScoutLightControlMsg = 0x22
 } ScoutMsgType;
 
-typedef struct 
+typedef struct {
-{
   ScoutMsgType type;
   union {
     // status messages

src/protocol_v1/scout/scout_protocol_v1_parser.cpp

@@ -12,14 +12,93 @@
 #include "protocol_v1/scout/scout_can_parser.h"
 
 namespace westonrobot {
+// CAN support
 bool ScoutProtocolV1Parser::DecodeMessage(const struct can_frame *rx_frame,
-                                          AgxMessage *msg) {}
+                                          AgxMessage *msg) {
+  ScoutMessage scout_msg;
+  // if ScoutMessage found, convert to AgxMessage
+  if (DecodeScoutMsgFromCAN(rx_frame, &scout_msg)) {
+    switch (scout_msg.type) {
+      case ScoutMotionStatusMsg: {
+        break;
+      }
+      case ScoutLightStatusMsg: {
+        break;
+      }
+      case ScoutSystemStatusMsg: {
+        break;
+      }
+      case ScoutMotorDriverStatusMsg: {
+        break;
+      }
+    }
+  }
+  return false;
+}
 
 void ScoutProtocolV1Parser::EncodeMessage(const AgxMessage *msg,
-                                          struct can_frame *tx_frame) {}
+                                          struct can_frame *tx_frame) {
+  ScoutMessage scout_msg;
+  // convert to ScoutMessage, then encode to can frame
+  switch (msg->type) {
+    case AgxMsgMotionCommand: {
+    //   scout_msg.type = ScoutMotionControlMsg;
+    //   scout_msg.body.motion_control_msg.data.cmd.control_mode =
+    //       CTRL_MODE_COMMANDED;
+    //   scout_msg.body.motion_control_msg.data.cmd.control_mode =
+    //       CTRL_MODE_CMD_CAN;
+    //   scout_msg.body.motion_control_msg.data.cmd.fault_clear_flag = 0x00;
+
+      /*
+      std::lock_guard<std::mutex> guard(motion_cmd_mutex_);
+    current_motion_cmd_.linear_velocity = static_cast<int8_t>(
+        linear_vel / ScoutCmdLimits::max_linear_velocity * 100.0);
+    current_motion_cmd_.angular_velocity = static_cast<int8_t>(
+        angular_vel / ScoutCmdLimits::max_angular_velocity * 100.0);
+      */
+    //   scout_msg.body.motion_control_msg.data.cmd.linear_velocity_cmd =
+    //       current_motion_cmd_.linear_velocity;
+    //   scout_msg.body.motion_control_msg.data.cmd.angular_velocity_cmd =
+    //       current_motion_cmd_.angular_velocity;
+    //   scout_msg.body.motion_control_msg.data.cmd.reserved0 = 0;
+    //   scout_msg.body.motion_control_msg.data.cmd.reserved1 = 0;
+    //   scout_msg.body.motion_control_msg.data.cmd.count = count;
+    //   scout_msg.body.motion_control_msg.data.cmd.checksum =
+    //       CalculateChecksum(CAN_MSG_MOTION_CONTROL_CMD_ID,
+    //                         scout_msg.body.motion_control_msg.data.raw, 8);
+      break;
+    }
+    case AgxMsgLightCommand: {
+      scout_msg.body.light_control_msg.data.cmd.light_ctrl_enable =
+          LIGHT_ENABLE_CTRL;
+
+    //   scout_msg.body.light_control_msg.data.cmd.front_mode =
+    //       static_cast<uint8_t>(current_light_cmd_.front_mode);
+    //   scout_msg.body.light_control_msg.data.cmd.front_custom =
+    //       current_light_cmd_.front_custom_value;
+    //   scout_msg.body.light_control_msg.data.cmd.rear_mode =
+    //       static_cast<uint8_t>(current_light_cmd_.rear_mode);
+    //   scout_msg.body.light_control_msg.data.cmd.rear_custom =
+    //       current_light_cmd_.rear_custom_value;
+    //   scout_msg.body.light_control_msg.data.cmd.reserved0 = 0;
+    //   scout_msg.body.light_control_msg.data.cmd.count = count;
+
+      scout_msg.body.light_control_msg.data.cmd.checksum =
+          CalculateChecksum(CAN_MSG_LIGHT_CONTROL_CMD_ID,
+                            scout_msg.body.light_control_msg.data.raw, 8);
+
+      break;
+    }
+  }
+}
 
 uint8_t ScoutProtocolV1Parser::CalculateChecksum(uint16_t id, uint8_t *data,
-                                                 uint8_t dlc) {}
+                                                 uint8_t dlc) {
+  uint8_t checksum = 0x00;
+  checksum = (uint8_t)(id & 0x00ff) + (uint8_t)(id >> 8) + dlc;
+  for (int i = 0; i < (dlc - 1); ++i) checksum += data[i];
+  return checksum;
+}
 
 // UART support
 bool ScoutProtocolV1Parser::DecodeMessage(uint8_t *data, uint8_t dlc,

src/protocol_v1/scout/scout_uart_parser.c

@@ -109,10 +109,10 @@ void EncodeScoutMsgToUART(const ScoutMessage *msg, uint8_t *buf, uint8_t *len)
     {
         buf[4] = UART_FRAME_LIGHT_STATUS_ID;
         buf[5] = msg->body.light_status_msg.data.status.light_ctrl_enable;
-        buf[6] = msg->body.light_status_msg.data.status.front_light_mode;
+        buf[6] = msg->body.light_status_msg.data.status.front_mode;
-        buf[7] = msg->body.light_status_msg.data.status.front_light_custom;
+        buf[7] = msg->body.light_status_msg.data.status.front_custom;
-        buf[8] = msg->body.light_status_msg.data.status.rear_light_mode;
+        buf[8] = msg->body.light_status_msg.data.status.rear_mode;
-        buf[9] = msg->body.light_status_msg.data.status.rear_light_custom;
+        buf[9] = msg->body.light_status_msg.data.status.rear_custom;
         buf[10] = 0;
         buf[11] = msg->body.light_status_msg.data.status.count;
         break;
@@ -218,10 +218,10 @@ void EncodeLightControlMsgToUART(const LightControlMessage *msg, uint8_t *buf, u
 
     // frame payload
     buf[5] = msg->data.cmd.light_ctrl_enable;
-    buf[6] = msg->data.cmd.front_light_mode;
+    buf[6] = msg->data.cmd.front_mode;
-    buf[7] = msg->data.cmd.front_light_custom;
+    buf[7] = msg->data.cmd.front_custom;
-    buf[8] = msg->data.cmd.rear_light_mode;
+    buf[8] = msg->data.cmd.rear_mode;
-    buf[9] = msg->data.cmd.rear_light_custom;
+    buf[9] = msg->data.cmd.rear_custom;
     buf[10] = 0x00;
 
     // frame count, checksum
@@ -467,10 +467,10 @@ bool ConstructControlMessage(ScoutMessage *msg)
     {
         msg->type = ScoutLightControlMsg;
         msg->body.light_control_msg.data.cmd.light_ctrl_enable = uart_parsing_data.payload_buffer[0];
-        msg->body.light_control_msg.data.cmd.front_light_mode = uart_parsing_data.payload_buffer[1];
+        msg->body.light_control_msg.data.cmd.front_mode = uart_parsing_data.payload_buffer[1];
-        msg->body.light_control_msg.data.cmd.front_light_custom = uart_parsing_data.payload_buffer[2];
+        msg->body.light_control_msg.data.cmd.front_custom = uart_parsing_data.payload_buffer[2];
-        msg->body.light_control_msg.data.cmd.rear_light_mode = uart_parsing_data.payload_buffer[3];
+        msg->body.light_control_msg.data.cmd.rear_mode = uart_parsing_data.payload_buffer[3];
-        msg->body.light_control_msg.data.cmd.rear_light_custom = uart_parsing_data.payload_buffer[4];
+        msg->body.light_control_msg.data.cmd.rear_custom = uart_parsing_data.payload_buffer[4];
         msg->body.light_control_msg.data.cmd.reserved0 = uart_parsing_data.payload_buffer[5];
         msg->body.light_control_msg.data.cmd.count = uart_parsing_data.frame_cnt;
         msg->body.light_control_msg.data.cmd.checksum = uart_parsing_data.frame_checksum;
@@ -573,10 +573,10 @@ bool ConstructStatusMessage(ScoutMessage *msg)
     {
         msg->type = ScoutLightStatusMsg;
         msg->body.light_status_msg.data.status.light_ctrl_enable = uart_parsing_data.payload_buffer[0];
-        msg->body.light_status_msg.data.status.front_light_mode = uart_parsing_data.payload_buffer[1];
+        msg->body.light_status_msg.data.status.front_mode = uart_parsing_data.payload_buffer[1];
-        msg->body.light_status_msg.data.status.front_light_custom = uart_parsing_data.payload_buffer[2];
+        msg->body.light_status_msg.data.status.front_custom = uart_parsing_data.payload_buffer[2];
-        msg->body.light_status_msg.data.status.rear_light_mode = uart_parsing_data.payload_buffer[3];
+        msg->body.light_status_msg.data.status.rear_mode = uart_parsing_data.payload_buffer[3];
-        msg->body.light_status_msg.data.status.rear_light_custom = uart_parsing_data.payload_buffer[4];
+        msg->body.light_status_msg.data.status.rear_custom = uart_parsing_data.payload_buffer[4];
         msg->body.light_status_msg.data.status.reserved0 = 0x00;
         msg->body.light_status_msg.data.status.count = uart_parsing_data.frame_cnt;
         msg->body.light_status_msg.data.status.checksum = uart_parsing_data.frame_checksum;

src/protocol_v2/agx_msg_parser.c

@@ -44,14 +44,14 @@ bool DecodeCanFrame(const struct can_frame *rx_frame, AgxMessage *msg) {
       msg->type = AgxMsgLightCommand;
       // parse frame buffer to message
       LightCommandFrame *frame = (LightCommandFrame *)(rx_frame->data);
-      msg->body.light_command_msg.cmd_ctrl_allowed =
+      msg->body.light_command_msg.enable_cmd_ctrl =
-          (frame->cmd_ctrl_allowed != 0) ? true : false;
+          (frame->enable_cmd_ctrl != 0) ? true : false;
-      msg->body.light_command_msg.front_light.mode = frame->front_light_mode;
+      msg->body.light_command_msg.front_light.mode = frame->front_mode;
       msg->body.light_command_msg.front_light.custom_value =
-          frame->front_light_custom;
+          frame->front_custom;
-      msg->body.light_command_msg.rear_light.mode = frame->rear_light_mode;
+      msg->body.light_command_msg.rear_light.mode = frame->rear_mode;
       msg->body.light_command_msg.rear_light.custom_value =
-          frame->rear_light_custom;
+          frame->rear_custom;
       break;
     }
     case CAN_MSG_BRAKING_COMMAND_ID: {
@@ -98,14 +98,14 @@ bool DecodeCanFrame(const struct can_frame *rx_frame, AgxMessage *msg) {
     case CAN_MSG_LIGHT_STATE_ID: {
       msg->type = AgxMsgLightState;
       LightStateFrame *frame = (LightStateFrame *)(rx_frame->data);
-      msg->body.light_command_msg.cmd_ctrl_allowed =
+      msg->body.light_command_msg.enable_cmd_ctrl =
-          (frame->cmd_ctrl_allowed != 0) ? true : false;
+          (frame->enable_cmd_ctrl != 0) ? true : false;
-      msg->body.light_command_msg.front_light.mode = frame->front_light_mode;
+      msg->body.light_command_msg.front_light.mode = frame->front_mode;
       msg->body.light_command_msg.front_light.custom_value =
-          frame->front_light_custom;
+          frame->front_custom;
-      msg->body.light_command_msg.rear_light.mode = frame->rear_light_mode;
+      msg->body.light_command_msg.rear_light.mode = frame->rear_mode;
       msg->body.light_command_msg.rear_light.custom_value =
-          frame->rear_light_custom;
+          frame->rear_custom;
       break;
     }
     case CAN_MSG_RC_STATE_ID: {
@@ -340,20 +340,20 @@ void EncodeCanFrame(const AgxMessage *msg, struct can_frame *tx_frame) {
       tx_frame->can_id = CAN_MSG_LIGHT_COMMAND_ID;
       tx_frame->can_dlc = 8;
       LightCommandFrame frame;
-      if (msg->body.light_command_msg.cmd_ctrl_allowed) {
+      if (msg->body.light_command_msg.enable_cmd_ctrl) {
-        frame.cmd_ctrl_allowed = LIGHT_CMD_CTRL_ALLOWED;
+        frame.enable_cmd_ctrl = LIGHT_ENABLE_CMD_CTRL;
-        frame.front_light_mode = msg->body.light_command_msg.front_light.mode;
+        frame.front_mode = msg->body.light_command_msg.front_light.mode;
-        frame.front_light_custom =
+        frame.front_custom =
             msg->body.light_command_msg.front_light.custom_value;
-        frame.rear_light_mode = msg->body.light_command_msg.rear_light.mode;
+        frame.rear_mode = msg->body.light_command_msg.rear_light.mode;
-        frame.rear_light_custom =
+        frame.rear_custom =
             msg->body.light_command_msg.rear_light.custom_value;
       } else {
-        frame.cmd_ctrl_allowed = LIGHT_CMD_CTRL_DISALLOWED;
+        frame.enable_cmd_ctrl = LIGHT_DISABLE_CMD_CTRL;
-        frame.front_light_mode = 0;
+        frame.front_mode = 0;
-        frame.front_light_custom = 0;
+        frame.front_custom = 0;
-        frame.rear_light_mode = 0;
+        frame.rear_mode = 0;
-        frame.rear_light_custom = 0;
+        frame.rear_custom = 0;
       }
       frame.reserved0 = 0;
       frame.reserved1 = 0;
@@ -363,7 +363,7 @@ void EncodeCanFrame(const AgxMessage *msg, struct can_frame *tx_frame) {
     }
     case AgxMsgBrakingCommand: {
       tx_frame->can_id = CAN_MSG_BRAKING_COMMAND_ID;
-      tx_frame->can_dlc = 8;
+      tx_frame->can_dlc = 2;
       BrakingCommandFrame frame;
       // TODO
       memcpy(tx_frame->data, (uint8_t *)(&frame), tx_frame->can_dlc);

src/protocol_v2/agx_protocol_v2.h

@@ -109,16 +109,6 @@ extern "C" {
 
 #define CAN_MSG_STATE_RESET_CONFIG_ID ((uint32_t)0x441)
 
-/*--------------------- Control/State Constants ------------------------*/
-
-#define LIGHT_CMD_CTRL_ALLOWED ((uint8_t)0x01)
-#define LIGHT_CMD_CTRL_DISALLOWED ((uint8_t)0x00)
-
-#define VERSION_REQUEST_VALUE ((uint8_t)0x01)
-#define STEER_NEUTRAL_REQUEST_VALUE ((uint8_t)0xee)
-#define STEER_NEUTRAL_RESPONSE_SUCCESS_VALUE ((uint8_t)0xee)
-#define STEER_NEUTRAL_RESPONSE_FAILURE_VALUE ((uint8_t)0xff)
-
 /*------------------------ Frame Memory Layout -------------------------*/
 
 /* No padding in the struct */
@@ -157,30 +147,26 @@ typedef struct {
   struct16_t steering_angle;
 } MotionCommandFrame;
 
+#define LIGHT_ENABLE_CMD_CTRL ((uint8_t)0x01)
+#define LIGHT_DISABLE_CMD_CTRL ((uint8_t)0x00)
+
 typedef struct {
-  uint8_t cmd_ctrl_allowed;
+  uint8_t enable_cmd_ctrl;
-  uint8_t front_light_mode;
+  uint8_t front_mode;
-  uint8_t front_light_custom;
+  uint8_t front_custom;
-  uint8_t rear_light_mode;
+  uint8_t rear_mode;
-  uint8_t rear_light_custom;
+  uint8_t rear_custom;
   uint8_t reserved0;
   uint8_t reserved1;
   uint8_t count;
 } LightCommandFrame;
 
 typedef struct {
-  uint8_t cmd_ctrl_allowed;
+  uint8_t enable_brake;
-  uint8_t front_light_mode;
-  uint8_t front_light_custom;
-  uint8_t rear_light_mode;
-  uint8_t rear_light_custom;
-  uint8_t reserved0;
-  uint8_t reserved1;
   uint8_t count;
 } BrakingCommandFrame;
 
-typedef struct
+typedef struct {
-{
   uint8_t motion_mode;
   uint8_t reserved0;
   uint8_t reserved1;
@@ -189,7 +175,7 @@ typedef struct
   uint8_t reserved4;
   uint8_t reserved5;
   uint8_t reserved6;
-}SetMotionModeFrame;
+} SetMotionModeFrame;
 
 // State feedback messages
 typedef struct {
@@ -209,11 +195,11 @@ typedef struct {
 } MotionStateFrame;
 
 typedef struct {
-  uint8_t cmd_ctrl_allowed;
+  uint8_t enable_cmd_ctrl;
-  uint8_t front_light_mode;
+  uint8_t front_mode;
-  uint8_t front_light_custom;
+  uint8_t front_custom;
-  uint8_t rear_light_mode;
+  uint8_t rear_mode;
-  uint8_t rear_light_custom;
+  uint8_t rear_custom;
   uint8_t reserved0;
   uint8_t reserved1;
   uint8_t count;
@@ -267,7 +253,7 @@ typedef struct {
 typedef struct {
   uint8_t motion_mode;
   uint8_t mode_changing;
-}MotionModeStateFrame;
+} MotionModeStateFrame;
 
 // sensors
 typedef struct {
@@ -340,6 +326,11 @@ typedef struct {
 } BmsExtendedFrame;
 
 // query/config
+#define VERSION_REQUEST_VALUE ((uint8_t)0x01)
+#define STEER_NEUTRAL_REQUEST_VALUE ((uint8_t)0xee)
+#define STEER_NEUTRAL_RESPONSE_SUCCESS_VALUE ((uint8_t)0xee)
+#define STEER_NEUTRAL_RESPONSE_FAILURE_VALUE ((uint8_t)0xff)
+
 typedef struct {
   uint8_t request;
   uint8_t reserved0;

src/protocol_v2/ranger_base.cpp

@@ -36,7 +36,7 @@ void RangerBase::SetMotionCommand(double linear_vel, double steer_angle,
 }
 
 void RangerBase::SetLightCommand(const RangerLightCmd &cmd) {
-  if (cmd.cmd_ctrl_allowed) {
+  if (cmd.enable_cmd_ctrl) {
     AgilexBase::SendLightCommand(cmd.front_mode, cmd.front_custom_value,
                                  LightMode::CONST_OFF, 0);
   }

src/protocol_v2/tracer_base.cpp

@@ -29,7 +29,7 @@ void TracerBaseV2::SetMotionCommand(double linear_vel, double angular_vel) {
 }
 
 void TracerBaseV2::SetLightCommand(const TracerLightCmd &cmd) {
-  if (cmd.cmd_ctrl_allowed) {
+  if (cmd.enable_cmd_ctrl) {
     AgilexBase::SendLightCommand(cmd.front_mode, cmd.front_custom_value,
                                  LightMode::CONST_OFF, 0);
   }