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

saved work on v1 protocol parser

Browse Source
This commit is contained in:
Ruixiang Du
2021-07-13 15:04:44 +08:00
parent 6cbdebc38a
commit 8b606b7520
6 changed files with 80891 additions and 22 deletions
Download Patch File Download Diff File Expand all files Collapse all files

0
docs/protocol_v1/SCOUT mini用户手册1.0协议.pdf → docs/protocol_v1/SCOUT MINI用户手册1.0协议.pdf
View File

80842
docs/protocol_v1/SCOUT MINI麦轮用户手册1.0协议.pdf Normal file
View File

File diff suppressed because one or more lines are too long

4
include/ugv_sdk/details/interface/agilex_message.h
View File

@@ -50,6 +50,7 @@ typedef struct {
bool clear_all_error; bool clear_all_error;
float linear; float linear;
float angular; float angular;
float lateral;
} MotionCommandMessageV1; } MotionCommandMessageV1;
typedef struct { typedef struct {
@@ -133,7 +134,8 @@ typedef struct {
uint8_t motor_id; uint8_t motor_id;
float current; float current;
int16_t rpm; int16_t rpm;
float temperature; float driver_temp;
float motor_temp;
} ActuatorStateMessageV1; } ActuatorStateMessageV1;
/***************** Sensor messages ******************/ /***************** Sensor messages ******************/

6
include/ugv_sdk/details/robot_base/scout_base.hpp
View File

@@ -97,6 +97,12 @@ class ScoutBase : public AgilexBase<Parser>, public ScoutInterface {
.actuator_ls_state[status_msg.body.actuator_ls_state_msg.motor_id] = .actuator_ls_state[status_msg.body.actuator_ls_state_msg.motor_id] =
status_msg.body.actuator_ls_state_msg; status_msg.body.actuator_ls_state_msg;
break; break;
}
case AgxMsgActuatorStateV1: {
// std::cout << "actuator v1 feedback received" << std::endl;
state.actuator_state[status_msg.body.v1_actuator_state_msg.motor_id] =
status_msg.body.v1_actuator_state_msg;
break;
} }
// case AgxMsgRcState: { // case AgxMsgRcState: {
// state.rc_state = status_msg.body.rc_state_msg; // state.rc_state = status_msg.body.rc_state_msg;

59
src/protocol_v1/agilex_msg_parser_v1.c
View File

@@ -41,13 +41,21 @@ bool DecodeCanFrameV1(const struct can_frame *rx_frame, AgxMessage *msg) {
msg->body.motion_state_msg.angular_velocity = msg->body.motion_state_msg.angular_velocity =
((((uint16_t)rx_frame->data[2]) << 8) | (uint16_t)rx_frame->data[3]) / ((((uint16_t)rx_frame->data[2]) << 8) | (uint16_t)rx_frame->data[3]) /
1000.0f; 1000.0f;
msg->body.motion_state_msg.lateral_velocity =
((((uint16_t)rx_frame->data[4]) << 8) | (uint16_t)rx_frame->data[5]) /
1000.0f;
break; break;
} }
case CAN_MSG_LIGHT_STATE_ID: { case CAN_MSG_LIGHT_STATE_ID: {
msg->type = AgxMsgLightState; msg->type = AgxMsgLightState;
// msg->light_status_msg.id = CAN_MSG_LIGHT_CONTROL_STATUS_ID; LightStateFrame *frame = (LightStateFrame *)(rx_frame->data);
// memcpy(msg->body.light_state_msg.data.raw, rx_frame->data, msg->body.light_command_msg.enable_cmd_ctrl =
// rx_frame->can_dlc * sizeof(uint8_t)); (frame->enable_cmd_ctrl != 0) ? true : false;
msg->body.light_command_msg.front_light.mode = frame->front_mode;
msg->body.light_command_msg.front_light.custom_value =
frame->front_custom;
msg->body.light_command_msg.rear_light.mode = frame->rear_mode;
msg->body.light_command_msg.rear_light.custom_value = frame->rear_custom;
break; break;
} }
case CAN_MSG_VALUE_SET_STATE_ID: { case CAN_MSG_VALUE_SET_STATE_ID: {
@@ -65,9 +73,13 @@ bool DecodeCanFrameV1(const struct can_frame *rx_frame, AgxMessage *msg) {
msg->type = AgxMsgActuatorStateV1; msg->type = AgxMsgActuatorStateV1;
msg->body.v1_actuator_state_msg.motor_id = msg->body.v1_actuator_state_msg.motor_id =
rx_frame->can_id - CAN_MSG_ACTUATOR1_STATE_ID; rx_frame->can_id - CAN_MSG_ACTUATOR1_STATE_ID;
// msg->body.v1_actuator_state_msg.motor_id = SCOUT_MOTOR4_ID; msg->body.v1_actuator_state_msg.current =
// memcpy(msg->body.motor_driver_status_msg.data.raw, rx_frame->data, ((((uint16_t)rx_frame->data[0]) << 8) | (uint16_t)rx_frame->data[1]) /
// rx_frame->can_dlc * sizeof(uint8_t)); 0.1f;
msg->body.v1_actuator_state_msg.rpm = (int16_t)((
(((uint16_t)rx_frame->data[2]) << 8) | (uint16_t)rx_frame->data[3]));
msg->body.v1_actuator_state_msg.driver_temp = rx_frame->data[4];
msg->body.v1_actuator_state_msg.motor_temp = rx_frame->data[5];
break; break;
} }
default: default:
@@ -83,13 +95,18 @@ void EncodeCanFrameV1(const AgxMessage *msg, struct can_frame *tx_frame) {
static uint8_t count = 0; static uint8_t count = 0;
tx_frame->can_id = CAN_MSG_MOTION_COMMAND_ID; tx_frame->can_id = CAN_MSG_MOTION_COMMAND_ID;
tx_frame->can_dlc = 8; tx_frame->can_dlc = 8;
tx_frame->data[0] = CTRL_MODE_CMD_CAN; MotionCommandFrame frame;
tx_frame->data[1] = ERROR_CLR_NONE; frame.control_mode = CTRL_MODE_CMD_CAN;
tx_frame->data[2] = (int8_t)(msg->body.v1_motion_command_msg.linear); frame.error_clear_byte = ERROR_CLR_NONE;
tx_frame->data[3] = (int8_t)(msg->body.v1_motion_command_msg.angular); frame.linear_percentage =
tx_frame->data[4] = 0; (int8_t)(msg->body.v1_motion_command_msg.linear);
tx_frame->data[5] = 0; frame.angular_percentage =
tx_frame->data[6] = count++; (int8_t)(msg->body.v1_motion_command_msg.angular);
frame.lateral_percentage =
(int8_t)(msg->body.v1_motion_command_msg.lateral);
frame.reserved0 = 0;
frame.count = count++;
memcpy(tx_frame->data, (uint8_t *)(&frame), tx_frame->can_dlc);
tx_frame->data[7] = CalcCanFrameChecksumV1( tx_frame->data[7] = CalcCanFrameChecksumV1(
tx_frame->can_id, tx_frame->data, tx_frame->can_dlc); tx_frame->can_id, tx_frame->data, tx_frame->can_dlc);
break; break;
@@ -98,14 +115,16 @@ void EncodeCanFrameV1(const AgxMessage *msg, struct can_frame *tx_frame) {
static uint8_t count = 0; static uint8_t count = 0;
tx_frame->can_id = CAN_MSG_VALUE_SET_COMMAND_ID; tx_frame->can_id = CAN_MSG_VALUE_SET_COMMAND_ID;
tx_frame->can_dlc = 8; tx_frame->can_dlc = 8;
tx_frame->data[0] = ValueSetCommandFrame frame;
frame.set_neutral =
msg->body.v1_value_set_command_msg.set_neutral ? 0xaa : 0x00; msg->body.v1_value_set_command_msg.set_neutral ? 0xaa : 0x00;
tx_frame->data[1] = 0x00; frame.reserved0 = 0x00;
tx_frame->data[2] = 0x00; frame.reserved1 = 0x00;
tx_frame->data[3] = 0x00; frame.reserved2 = 0x00;
tx_frame->data[4] = 0x00; frame.reserved3 = 0x00;
tx_frame->data[5] = 0x00; frame.reserved4 = 0x00;
tx_frame->data[6] = 0x00; frame.count = count++;
memcpy(tx_frame->data, (uint8_t *)(&frame), tx_frame->can_dlc);
tx_frame->data[7] = CalcCanFrameChecksumV1( tx_frame->data[7] = CalcCanFrameChecksumV1(
tx_frame->can_id, tx_frame->data, tx_frame->can_dlc); tx_frame->can_id, tx_frame->data, tx_frame->can_dlc);
break; break;

2
src/protocol_v1/agilex_protocol_v1.h
View File

@@ -102,8 +102,8 @@ typedef struct {
uint8_t error_clear_byte; uint8_t error_clear_byte;
int8_t linear_percentage; int8_t linear_percentage;
int8_t angular_percentage; int8_t angular_percentage;
int8_t lateral_percentage;
uint8_t reserved0; uint8_t reserved0;
uint8_t reserved1;
uint8_t count; uint8_t count;
uint8_t checksum; uint8_t checksum;
} MotionCommandFrame; } MotionCommandFrame;