From 6304e93ac811ea1c5e3b4e183b8fc0cc3a1b08d6 Mon Sep 17 00:00:00 2001 From: wangzheqie Date: Wed, 21 Apr 2021 10:23:02 +0800 Subject: [PATCH] add some ranger protocol definition --- src/agx_msg_parser.c | 8 ++++---- src/agx_protocol_v2.h | 12 ++++++++++++ src/ranger_base.cpp | 2 +- 3 files changed, 17 insertions(+), 5 deletions(-) diff --git a/src/agx_msg_parser.c b/src/agx_msg_parser.c index 7dce7c3..c6a2226 100644 --- a/src/agx_msg_parser.c +++ b/src/agx_msg_parser.c @@ -77,19 +77,19 @@ bool DecodeCanFrame(const struct can_frame *rx_frame, AgxMessage *msg) { case CAN_MSG_MOTION_STATE_ID: { msg->type = AgxMsgMotionState; MotionStateFrame *frame = (MotionStateFrame *)(rx_frame->data); - msg->body.motion_command_msg.linear_velocity = + msg->body.motion_state_msg.linear_velocity = (int16_t)((uint16_t)(frame->linear_velocity.low_byte) | (uint16_t)(frame->linear_velocity.high_byte) << 8) / 1000.0; - msg->body.motion_command_msg.angular_velocity = + msg->body.motion_state_msg.angular_velocity = (int16_t)((uint16_t)(frame->angular_velocity.low_byte) | (uint16_t)(frame->angular_velocity.high_byte) << 8) / 1000.0; - msg->body.motion_command_msg.lateral_velocity = + msg->body.motion_state_msg.lateral_velocity = (int16_t)((uint16_t)(frame->lateral_velocity.low_byte) | (uint16_t)(frame->lateral_velocity.high_byte) << 8) / 1000.0; - msg->body.motion_command_msg.steering_angle = + msg->body.motion_state_msg.steering_angle = (int16_t)((uint16_t)(frame->steering_angle.low_byte) | (uint16_t)(frame->steering_angle.high_byte) << 8) / 1000.0; diff --git a/src/agx_protocol_v2.h b/src/agx_protocol_v2.h index afbd522..a622dd4 100644 --- a/src/agx_protocol_v2.h +++ b/src/agx_protocol_v2.h @@ -53,11 +53,23 @@ extern "C" { #define CAN_MSG_ACTUATOR2_HS_STATE_ID ((uint32_t)0x252) #define CAN_MSG_ACTUATOR3_HS_STATE_ID ((uint32_t)0x253) #define CAN_MSG_ACTUATOR4_HS_STATE_ID ((uint32_t)0x254) +#define CAN_MSG_ACTUATOR5_HS_STATE_ID ((uint32_t)0x255) +#define CAN_MSG_ACTUATOR6_HS_STATE_ID ((uint32_t)0x256) +#define CAN_MSG_ACTUATOR7_HS_STATE_ID ((uint32_t)0x257) +#define CAN_MSG_ACTUATOR8_HS_STATE_ID ((uint32_t)0x258) #define CAN_MSG_ACTUATOR1_LS_STATE_ID ((uint32_t)0x261) #define CAN_MSG_ACTUATOR2_LS_STATE_ID ((uint32_t)0x262) #define CAN_MSG_ACTUATOR3_LS_STATE_ID ((uint32_t)0x263) #define CAN_MSG_ACTUATOR4_LS_STATE_ID ((uint32_t)0x264) +#define CAN_MSG_ACTUATOR5_LS_STATE_ID ((uint32_t)0x265) +#define CAN_MSG_ACTUATOR6_LS_STATE_ID ((uint32_t)0x266) +#define CAN_MSG_ACTUATOR7_LS_STATE_ID ((uint32_t)0x267) +#define CAN_MSG_ACTUATOR8_LS_STATE_ID ((uint32_t)0x268) + +#define CAN_MSG_MOTOR_ANGLE_INFO ((uint32_t)0x271) +#define CAN_MSG_MOTOR_SPEED_INFO ((uint32_t)0x281) +#define CAN_MSG_CURRENT_CTRL_MODE ((uint32_t)0x291) // sensor data group: 0x3 #define CAN_MSG_ODOMETRY_ID ((uint32_t)0x311) diff --git a/src/ranger_base.cpp b/src/ranger_base.cpp index 55f6855..b5b1e30 100644 --- a/src/ranger_base.cpp +++ b/src/ranger_base.cpp @@ -61,7 +61,7 @@ void RangerBase::UpdateRangerState(const AgxMessage &status_msg, break; } case AgxMsgMotionState: { - // std::cout << "motion control feedback received" << std::endl; + // std::cout << "motion control feedback received" << std::endl; state.motion_state = status_msg.body.motion_state_msg; break; }