mirrors/ugv_sdk
1
0
Fork 0
mirror of https://github.com/westonrobot/ugv_sdk synced 2023-04-08 06:32:14 +08:00
Code Issues Packages Projects Releases Wiki Activity

add some ranger protocol definition

Browse Source
This commit is contained in:
wangzheqie
2021-04-21 10:23:02 +08:00
parent 51c4357134
commit 158f62a7f4
3 changed files with 17 additions and 5 deletions
Download Patch File Download Diff File Expand all files Collapse all files

8
src/agx_msg_parser.c
View File

@@ -77,19 +77,19 @@ bool DecodeCanFrame(const struct can_frame *rx_frame, AgxMessage *msg) {
case CAN_MSG_MOTION_STATE_ID: { case CAN_MSG_MOTION_STATE_ID: {
msg->type = AgxMsgMotionState; msg->type = AgxMsgMotionState;
MotionStateFrame *frame = (MotionStateFrame *)(rx_frame->data); 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) | (int16_t)((uint16_t)(frame->linear_velocity.low_byte) |
(uint16_t)(frame->linear_velocity.high_byte) << 8) / (uint16_t)(frame->linear_velocity.high_byte) << 8) /
1000.0; 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) | (int16_t)((uint16_t)(frame->angular_velocity.low_byte) |
(uint16_t)(frame->angular_velocity.high_byte) << 8) / (uint16_t)(frame->angular_velocity.high_byte) << 8) /
1000.0; 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) | (int16_t)((uint16_t)(frame->lateral_velocity.low_byte) |
(uint16_t)(frame->lateral_velocity.high_byte) << 8) / (uint16_t)(frame->lateral_velocity.high_byte) << 8) /
1000.0; 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) | (int16_t)((uint16_t)(frame->steering_angle.low_byte) |
(uint16_t)(frame->steering_angle.high_byte) << 8) / (uint16_t)(frame->steering_angle.high_byte) << 8) /
1000.0; 1000.0;

12
src/agx_protocol_v2.h
View File

@@ -53,11 +53,23 @@ extern "C" {
#define CAN_MSG_ACTUATOR2_HS_STATE_ID ((uint32_t)0x252) #define CAN_MSG_ACTUATOR2_HS_STATE_ID ((uint32_t)0x252)
#define CAN_MSG_ACTUATOR3_HS_STATE_ID ((uint32_t)0x253) #define CAN_MSG_ACTUATOR3_HS_STATE_ID ((uint32_t)0x253)
#define CAN_MSG_ACTUATOR4_HS_STATE_ID ((uint32_t)0x254) #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_ACTUATOR1_LS_STATE_ID ((uint32_t)0x261)
#define CAN_MSG_ACTUATOR2_LS_STATE_ID ((uint32_t)0x262) #define CAN_MSG_ACTUATOR2_LS_STATE_ID ((uint32_t)0x262)
#define CAN_MSG_ACTUATOR3_LS_STATE_ID ((uint32_t)0x263) #define CAN_MSG_ACTUATOR3_LS_STATE_ID ((uint32_t)0x263)
#define CAN_MSG_ACTUATOR4_LS_STATE_ID ((uint32_t)0x264) #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 // sensor data group: 0x3
#define CAN_MSG_ODOMETRY_ID ((uint32_t)0x311) #define CAN_MSG_ODOMETRY_ID ((uint32_t)0x311)

2
src/ranger_base.cpp
View File

@@ -61,7 +61,7 @@ void RangerBase::UpdateRangerState(const AgxMessage &status_msg,
break; break;
} }
case AgxMsgMotionState: { 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; state.motion_state = status_msg.body.motion_state_msg;
break; break;
} }