add motion mode feedback

src/agx_msg_parser.c

@@ -192,6 +192,15 @@ bool DecodeCanFrame(const struct can_frame *rx_frame, AgxMessage *msg) {
       msg->body.actuator_ls_state_msg.driver_state = frame->driver_state;
       break;
     }
+    case CAN_MSG_CURRENT_CTRL_MODE:
+    {
+      msg->type=AgxMsgMotionModeState;
+      MotionModeStateFrame *frame = (MotionModeStateFrame*)(rx_frame->data);
+
+      msg->body.motion_mode_feedback_msg.motion_mode = frame->motion_mode;
+      msg->body.motion_mode_feedback_msg.mode_changing = frame->mode_changing;
+      break;
+    }
     /****************** sensor frame *****************/
     case CAN_MSG_ODOMETRY_ID: {
       msg->type = AgxMsgOdometry;

src/agx_protocol_v2.h

@@ -263,6 +263,12 @@ typedef struct {
   uint8_t reserved1;
 } ActuatorLSStateFrame;
 
+// 0x291
+typedef struct {
+  uint8_t motion_mode;
+  uint8_t mode_changing;
+}MotionModeStateFrame;
+
 // sensors
 typedef struct {
   struct32_t left_wheel;

src/ranger_base.cpp

@@ -95,6 +95,10 @@ void RangerBase::UpdateRangerState(const AgxMessage &status_msg,
           status_msg.body.actuator_ls_state_msg;
       break;
     }
+    case AgxMsgMotionModeState: {
+      state.current_motion_mode = status_msg.body.motion_mode_feedback_msg;
+      break;
+    }
     /* sensor feedback */
     case AgxMsgOdometry: {
       // std::cout << "Odometer msg feedback received" << std::endl;