From c1b53c167bb183de089d431f5f7c5a5741ed83ba Mon Sep 17 00:00:00 2001
From: wangzheqie <wangzheqie@qq.com>
Date: Sun, 25 Apr 2021 11:42:51 +0800
Subject: [PATCH] add motion mode feedback

---
 include/ugv_sdk/agilex_message.h | 15 +++++++++++----
 include/ugv_sdk/ranger_base.hpp  |  1 +
 src/agx_msg_parser.c             |  9 +++++++++
 src/agx_protocol_v2.h            |  6 ++++++
 src/ranger_base.cpp              |  4 ++++
 5 files changed, 31 insertions(+), 4 deletions(-)

diff --git a/include/ugv_sdk/agilex_message.h b/include/ugv_sdk/agilex_message.h
index 557e6e0..66907dc 100644
--- a/include/ugv_sdk/agilex_message.h
+++ b/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;
diff --git a/include/ugv_sdk/ranger_base.hpp b/include/ugv_sdk/ranger_base.hpp
index f4b7426..2df733e 100644
--- a/include/ugv_sdk/ranger_base.hpp
+++ b/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;
diff --git a/src/agx_msg_parser.c b/src/agx_msg_parser.c
index 406dd3d..a7b5cbe 100644
--- a/src/agx_msg_parser.c
+++ b/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;
diff --git a/src/agx_protocol_v2.h b/src/agx_protocol_v2.h
index bf9ae2b..0f56ffd 100644
--- a/src/agx_protocol_v2.h
+++ b/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;
diff --git a/src/ranger_base.cpp b/src/ranger_base.cpp
index 16abbb5..8d19674 100644
--- a/src/ranger_base.cpp
+++ b/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;