add motion mode feedback

include/ugv_sdk/agilex_message.h

@@ -15,8 +15,8 @@
 extern "C" {
 #endif
 
-#include <stdint.h>
 #include <stdbool.h>
+#include <stdint.h>
 #include <string.h>
 
 /***************** Control messages *****************/
@@ -54,8 +54,7 @@ typedef struct {
 } BrakingCommandMessage;
 
 // 0x141
-typedef  struct
+typedef struct {
-{
   uint8_t motion_mode;
 } MotionModeMessage;
 
@@ -69,7 +68,7 @@ typedef enum {
 } VehicleState;
 
 typedef enum {
-//   CONTROL_MODE_STANDBY = 0x00,
+  //   CONTROL_MODE_STANDBY = 0x00,
   CONTROL_MODE_RC = 0x00,
   CONTROL_MODE_CAN = 0x01,
   CONTROL_MODE_UART = 0x02
@@ -141,6 +140,12 @@ typedef struct {
 #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;
@@ -302,6 +307,7 @@ typedef enum {
   AgxMsgRcState,
   AgxMsgActuatorHSState,
   AgxMsgActuatorLSState,
+  AgxMsgMotionModeState,
   // sensor
   AgxMsgOdometry,
   AgxMsgImuAccel,
@@ -336,6 +342,7 @@ typedef struct {
     RcStateMessage rc_state_msg;
     ActuatorHSStateMessage actuator_hs_state_msg;
     ActuatorLSStateMessage actuator_ls_state_msg;
+    MotionModeFeedbackMessage motion_mode_feedback_msg;
     // sensor
     OdometryMessage odometry_msg;
     ImuAccelMessage imu_accel_msg;

include/ugv_sdk/ranger_base.hpp

@@ -30,6 +30,7 @@ struct RangerState {
 
   ActuatorHSStateMessage actuator_hs_state[8];
   ActuatorLSStateMessage actuator_ls_state[8];
+  MotionModeFeedbackMessage current_motion_mode;
 
   // sensor data
   OdometryMessage odometry;

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;