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

added get protocol version api

Browse Source
This commit is contained in:
Ruixiang Du
2021-07-13 18:03:50 +08:00
parent 8b606b7520
commit f7224bf56f
14 changed files with 96 additions and 55 deletions
Download Patch File Download Diff File Expand all files Collapse all files

10
src/mobile_robot/scout_robot.cpp
View File

@@ -11,10 +11,10 @@
#include "ugv_sdk/details/robot_base/scout_base.hpp"
namespace westonrobot {
ScoutRobot::ScoutRobot(ProtocolType protocol) {
if (protocol == ProtocolType::AGX_V1) {
ScoutRobot::ScoutRobot(ProtocolVersion protocol) {
if (protocol == ProtocolVersion::AGX_V1) {
robot_ = new ScoutBaseV1();
} else if (protocol == ProtocolType::AGX_V2) {
} else if (protocol == ProtocolVersion::AGX_V2) {
robot_ = new ScoutBaseV2();
}
}
@@ -33,6 +33,10 @@ void ScoutRobot::Connect(std::string uart_name, uint32_t baudrate) {
void ScoutRobot::ResetRobotState() { robot_->ResetRobotState(); }
ProtocolVersion ScoutRobot::GetProtocolVersion() {
return robot_->GetProtocolVersion();
}
void ScoutRobot::SetMotionCommand(double linear_vel, double angular_vel) {
auto scout = dynamic_cast<ScoutInterface*>(robot_);
scout->SetMotionCommand(linear_vel, angular_vel);

18
src/protocol_v1/agilex_msg_parser_v1.c
View File

@@ -18,6 +18,7 @@ bool DecodeCanFrameV1(const struct can_frame *rx_frame, AgxMessage *msg) {
// if checksum not correct
if (!CalcCanFrameChecksumV1(rx_frame->can_id, (uint8_t *)rx_frame->data,
rx_frame->can_dlc)) {
printf("wrong checksum for id: %x-------------->\n", rx_frame->can_id);
return false;
}
@@ -36,13 +37,16 @@ bool DecodeCanFrameV1(const struct can_frame *rx_frame, AgxMessage *msg) {
case CAN_MSG_MOTION_STATE_ID: {
msg->type = AgxMsgMotionState;
msg->body.motion_state_msg.linear_velocity =
((((uint16_t)rx_frame->data[0]) << 8) | (uint16_t)rx_frame->data[1]) /
((int16_t)((((uint16_t)rx_frame->data[0]) << 8) |
(uint16_t)rx_frame->data[1])) /
1000.0f;
msg->body.motion_state_msg.angular_velocity =
((((uint16_t)rx_frame->data[2]) << 8) | (uint16_t)rx_frame->data[3]) /
((int16_t)((((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]) /
((int16_t)((((uint16_t)rx_frame->data[4]) << 8) |
(uint16_t)rx_frame->data[5])) /
1000.0f;
break;
}
@@ -89,7 +93,8 @@ bool DecodeCanFrameV1(const struct can_frame *rx_frame, AgxMessage *msg) {
return true;
}
void EncodeCanFrameV1(const AgxMessage *msg, struct can_frame *tx_frame) {
bool EncodeCanFrameV1(const AgxMessage *msg, struct can_frame *tx_frame) {
bool ret = true;
switch (msg->type) {
case AgxMsgMotionCommandV1: {
static uint8_t count = 0;
@@ -155,9 +160,12 @@ void EncodeCanFrameV1(const AgxMessage *msg, struct can_frame *tx_frame) {
tx_frame->can_id, tx_frame->data, tx_frame->can_dlc);
break;
}
default:
default: {
ret = false;
break;
}
}
return ret;
}
uint8_t CalcCanFrameChecksumV1(uint16_t id, uint8_t *data, uint8_t dlc) {

24
src/protocol_v2/agilex_msg_parser_v2.c
View File

@@ -50,8 +50,7 @@ bool DecodeCanFrameV2(const struct can_frame *rx_frame, AgxMessage *msg) {
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;
msg->body.light_command_msg.rear_light.custom_value = frame->rear_custom;
break;
}
case CAN_MSG_BRAKING_COMMAND_ID: {
@@ -104,8 +103,7 @@ bool DecodeCanFrameV2(const struct can_frame *rx_frame, AgxMessage *msg) {
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;
msg->body.light_command_msg.rear_light.custom_value = frame->rear_custom;
break;
}
case CAN_MSG_RC_STATE_ID: {
@@ -192,10 +190,9 @@ bool DecodeCanFrameV2(const struct can_frame *rx_frame, AgxMessage *msg) {
msg->body.actuator_ls_state_msg.driver_state = frame->driver_state;
break;
}
case CAN_MSG_CURRENT_CTRL_MODE:
{
msg->type=AgxMsgMotionModeState;
MotionModeStateFrame *frame = (MotionModeStateFrame*)(rx_frame->data);
case CAN_MSG_CURRENT_CTRL_MODE: {
msg->type = AgxMsgMotionModeState;
MotionModeStateFrame *frame = (MotionModeStateFrame *)(rx_frame->data);
msg->body.motion_mode_feedback_msg.motion_mode = frame->motion_mode;
msg->body.motion_mode_feedback_msg.mode_changing = frame->mode_changing;
@@ -309,7 +306,8 @@ bool DecodeCanFrameV2(const struct can_frame *rx_frame, AgxMessage *msg) {
return true;
}
void EncodeCanFrameV2(const AgxMessage *msg, struct can_frame *tx_frame) {
bool EncodeCanFrameV2(const AgxMessage *msg, struct can_frame *tx_frame) {
bool ret = true;
switch (msg->type) {
/***************** command frame *****************/
case AgxMsgMotionCommand: {
@@ -346,8 +344,7 @@ void EncodeCanFrameV2(const AgxMessage *msg, struct can_frame *tx_frame) {
frame.front_custom =
msg->body.light_command_msg.front_light.custom_value;
frame.rear_mode = msg->body.light_command_msg.rear_light.mode;
frame.rear_custom =
msg->body.light_command_msg.rear_light.custom_value;
frame.rear_custom = msg->body.light_command_msg.rear_light.custom_value;
} else {
frame.enable_cmd_ctrl = LIGHT_DISABLE_CMD_CTRL;
frame.front_mode = 0;
@@ -590,9 +587,12 @@ void EncodeCanFrameV2(const AgxMessage *msg, struct can_frame *tx_frame) {
memcpy(tx_frame->data, (uint8_t *)(&frame), tx_frame->can_dlc);
break;
}
default:
default: {
ret = false;
break;
}
}
return ret;
}
uint8_t CalcCanFrameChecksumV2(uint16_t id, uint8_t *data, uint8_t dlc) {

2
src/protocol_v2/agilex_msg_parser_v2.h
View File

@@ -31,7 +31,7 @@ struct can_frame {
#include "ugv_sdk/details/interface/agilex_message.h"
bool DecodeCanFrameV2(const struct can_frame *rx_frame, AgxMessage *msg);
void EncodeCanFrameV2(const AgxMessage *msg, struct can_frame *tx_frame);
bool EncodeCanFrameV2(const AgxMessage *msg, struct can_frame *tx_frame);
uint8_t CalcCanFrameChecksumV2(uint16_t id, uint8_t *data, uint8_t dlc);
#ifdef __cplusplus

4
src/protocol_v2/protocol_v2_parser.cpp
View File

@@ -16,9 +16,9 @@ bool ProtocolV2Parser::DecodeMessage(const struct can_frame *rx_frame,
return DecodeCanFrameV2(rx_frame, msg);
}
void ProtocolV2Parser::EncodeMessage(const AgxMessage *msg,
bool ProtocolV2Parser::EncodeMessage(const AgxMessage *msg,
struct can_frame *tx_frame) {
EncodeCanFrameV2(msg, tx_frame);
return EncodeCanFrameV2(msg, tx_frame);
}
uint8_t ProtocolV2Parser::CalculateChecksum(uint16_t id, uint8_t *data,