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

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 =
((((uint16_t)rx_frame->data[2]) << 8) | (uint16_t)rx_frame->data[3]) /
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;
}
case CAN_MSG_LIGHT_STATE_ID: {
msg->type = AgxMsgLightState;
// msg->light_status_msg.id = CAN_MSG_LIGHT_CONTROL_STATUS_ID;
// memcpy(msg->body.light_state_msg.data.raw, rx_frame->data,
// rx_frame->can_dlc * sizeof(uint8_t));
LightStateFrame *frame = (LightStateFrame *)(rx_frame->data);
msg->body.light_command_msg.enable_cmd_ctrl =
(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;
}
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->body.v1_actuator_state_msg.motor_id =
rx_frame->can_id - CAN_MSG_ACTUATOR1_STATE_ID;
// msg->body.v1_actuator_state_msg.motor_id = SCOUT_MOTOR4_ID;
// memcpy(msg->body.motor_driver_status_msg.data.raw, rx_frame->data,
// rx_frame->can_dlc * sizeof(uint8_t));
msg->body.v1_actuator_state_msg.current =
((((uint16_t)rx_frame->data[0]) << 8) | (uint16_t)rx_frame->data[1]) /
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;
}
default:
@@ -83,13 +95,18 @@ void EncodeCanFrameV1(const AgxMessage *msg, struct can_frame *tx_frame) {
static uint8_t count = 0;
tx_frame->can_id = CAN_MSG_MOTION_COMMAND_ID;
tx_frame->can_dlc = 8;
tx_frame->data[0] = CTRL_MODE_CMD_CAN;
tx_frame->data[1] = ERROR_CLR_NONE;
tx_frame->data[2] = (int8_t)(msg->body.v1_motion_command_msg.linear);
tx_frame->data[3] = (int8_t)(msg->body.v1_motion_command_msg.angular);
tx_frame->data[4] = 0;
tx_frame->data[5] = 0;
tx_frame->data[6] = count++;
MotionCommandFrame frame;
frame.control_mode = CTRL_MODE_CMD_CAN;
frame.error_clear_byte = ERROR_CLR_NONE;
frame.linear_percentage =
(int8_t)(msg->body.v1_motion_command_msg.linear);
frame.angular_percentage =
(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->can_id, tx_frame->data, tx_frame->can_dlc);
break;
@@ -98,14 +115,16 @@ void EncodeCanFrameV1(const AgxMessage *msg, struct can_frame *tx_frame) {
static uint8_t count = 0;
tx_frame->can_id = CAN_MSG_VALUE_SET_COMMAND_ID;
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;
tx_frame->data[1] = 0x00;
tx_frame->data[2] = 0x00;
tx_frame->data[3] = 0x00;
tx_frame->data[4] = 0x00;
tx_frame->data[5] = 0x00;
tx_frame->data[6] = 0x00;
frame.reserved0 = 0x00;
frame.reserved1 = 0x00;
frame.reserved2 = 0x00;
frame.reserved3 = 0x00;
frame.reserved4 = 0x00;
frame.count = count++;
memcpy(tx_frame->data, (uint8_t *)(&frame), tx_frame->can_dlc);
tx_frame->data[7] = CalcCanFrameChecksumV1(
tx_frame->can_id, tx_frame->data, tx_frame->can_dlc);
break;

2
src/protocol_v1/agilex_protocol_v1.h
View File

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