v2_parser: added encoding impl for motion mode state

westonrobot · Aug 13, 2024 · 70919d4 · 70919d4
1 parent d05a912
commit 70919d4
Showing 1 changed file with 9 additions and 0 deletions.
diff --git a/src/protocol_v2/agilex_msg_parser_v2.c b/src/protocol_v2/agilex_msg_parser_v2.c
@@ -499,6 +499,15 @@ bool EncodeCanFrameV2(const AgxMessage *msg, struct can_frame *tx_frame) {
       memcpy(tx_frame->data, (uint8_t *)(&frame), tx_frame->can_dlc);
       break;
     }
+    case AgxMsgMotionModeState: {
+      tx_frame->can_id = CAN_MSG_CURRENT_CTRL_MODE;
+      tx_frame->can_dlc = 2;
+      MotionModeStateFrame frame;
+      frame.motion_mode = msg->body.motion_mode_state_msg.motion_mode;
+      frame.mode_changing = msg->body.motion_mode_state_msg.mode_changing;
+      memcpy(tx_frame->data, (uint8_t *)(&frame), tx_frame->can_dlc);
+      break;
+    }
     case AgxMsgLightState: {
       tx_frame->can_id = CAN_MSG_LIGHT_STATE_ID;
       tx_frame->can_dlc = 8;