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, updated code organization

Browse Source
This commit is contained in:
Ruixiang Du
2021-07-09 22:19:21 +08:00
parent 0565639c8f
commit 41f598c65c
20 changed files with 253 additions and 1389 deletions
Download Patch File Download Diff File Expand all files Collapse all files

17
CMakeLists.txt
View File

@@ -80,19 +80,12 @@ add_library(${PROJECT_NAME}
src/mobile_robot/scout_robot.cpp src/mobile_robot/scout_robot.cpp
######################## ########################
## protocol v2 support ## protocol v2 support
# parser src/protocol_v2/agilex_msg_parser_v2.c
src/protocol_v2/agx_msg_parser.c src/protocol_v2/parser_impl/protocol_v2_parser.cpp
src/protocol_v2/protocol_v2_parser.cpp
# robots
# src/protocol_v2/scout_base.cpp
# src/protocol_v2/tracer_base.cpp
# src/protocol_v2/ranger_base.cpp
######################## ########################
## legacy protocol v1 support ## legacy protocol v1 support (transparent to user)
src/protocol_v1/scout/scout_uart_parser.c src/protocol_v1/agilex_msg_parser_v1.c
src/protocol_v1/scout/scout_can_parser.c src/protocol_v1/parser_impl/scout_protocol_v1_parser.cpp
src/protocol_v1/scout/scout_protocol_v1_parser.cpp
) )
target_link_libraries(${PROJECT_NAME} PUBLIC Threads::Threads) target_link_libraries(${PROJECT_NAME} PUBLIC Threads::Threads)
target_compile_definitions(${PROJECT_NAME} PUBLIC ASIO_ENABLE_OLD_SERVICES) target_compile_definitions(${PROJECT_NAME} PUBLIC ASIO_ENABLE_OLD_SERVICES)

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

@@ -19,9 +19,10 @@ extern "C" {
#include <stdint.h> #include <stdint.h>
#include <string.h> #include <string.h>
#include "ugv_sdk/details/interface/agilex_types.h"
/***************** Control messages *****************/ /***************** Control messages *****************/
// 0x111
typedef struct { typedef struct {
float linear_velocity; float linear_velocity;
float angular_velocity; float angular_velocity;
@@ -29,51 +30,22 @@ typedef struct {
float steering_angle; float steering_angle;
} MotionCommandMessage; } MotionCommandMessage;
// 0x121
typedef enum {
CONST_OFF = 0x00,
CONST_ON = 0x01,
BREATH = 0x02,
CUSTOM = 0x03
} LightMode;
typedef struct {
LightMode mode;
uint8_t custom_value;
} LightOperation;
typedef struct { typedef struct {
bool enable_cmd_ctrl; bool enable_cmd_ctrl;
LightOperation front_light; LightOperation front_light;
LightOperation rear_light; LightOperation rear_light;
} LightCommandMessage; } LightCommandMessage;
// 0x131
typedef struct { typedef struct {
bool enable_braking; bool enable_braking;
} BrakingCommandMessage; } BrakingCommandMessage;
// 0x141
typedef struct { typedef struct {
uint8_t motion_mode; uint8_t motion_mode;
} MotionModeMessage; } MotionModeMessage;
/**************** Feedback messages *****************/ /**************** Feedback messages *****************/
// 0x211
typedef enum {
VehicleStateNormal = 0x00,
VehicleStateEStop = 0x01,
VehicleStateException = 0x02
} VehicleState;
typedef enum {
// CONTROL_MODE_STANDBY = 0x00,
CONTROL_MODE_RC = 0x00,
CONTROL_MODE_CAN = 0x01,
CONTROL_MODE_UART = 0x02
} ControlMode;
#define SYSTEM_ERROR_MOTOR_DRIVER_MASK ((uint16_t)0x0100) #define SYSTEM_ERROR_MOTOR_DRIVER_MASK ((uint16_t)0x0100)
#define SYSTEM_ERROR_HL_COMM_MASK ((uint16_t)0x0200) #define SYSTEM_ERROR_HL_COMM_MASK ((uint16_t)0x0200)
#define SYSTEM_ERROR_BATTERY_FAULT_MASK ((uint16_t)0x0001) #define SYSTEM_ERROR_BATTERY_FAULT_MASK ((uint16_t)0x0001)
@@ -103,13 +75,6 @@ typedef struct {
// 0x231 // 0x231
typedef LightCommandMessage LightStateMessage; typedef LightCommandMessage LightStateMessage;
// 0x241
typedef enum {
RC_SWITCH_UP = 0,
RC_SWITCH_MIDDLE,
RC_SWITCH_DOWN
} RcSwitchState;
typedef struct { typedef struct {
RcSwitchState swa; RcSwitchState swa;
RcSwitchState swb; RcSwitchState swb;
@@ -122,7 +87,6 @@ typedef struct {
int8_t var_a; int8_t var_a;
} RcStateMessage; } RcStateMessage;
// 0x251 - 0x258
typedef struct { typedef struct {
uint8_t motor_id; uint8_t motor_id;
int16_t rpm; int16_t rpm;
@@ -130,6 +94,11 @@ typedef struct {
int32_t pulse_count; int32_t pulse_count;
} ActuatorHSStateMessage; } ActuatorHSStateMessage;
typedef struct {
uint8_t motion_mode;
uint8_t mode_changing;
} MotionModeFeedbackMessage;
// 0x261 - 0x264 // 0x261 - 0x264
#define DRIVER_STATE_INPUT_VOLTAGE_LOW_MASK ((uint8_t)0x01) #define DRIVER_STATE_INPUT_VOLTAGE_LOW_MASK ((uint8_t)0x01)
#define DRIVER_STATE_MOTOR_OVERHEAT_MASK ((uint8_t)0x02) #define DRIVER_STATE_MOTOR_OVERHEAT_MASK ((uint8_t)0x02)
@@ -140,12 +109,6 @@ typedef struct {
#define DRIVER_STATE_DRIVER_ENABLED_MASK ((uint8_t)0x40) #define DRIVER_STATE_DRIVER_ENABLED_MASK ((uint8_t)0x40)
#define DRIVER_STATE_DRIVER_RESET_MASK ((uint8_t)0x80) #define DRIVER_STATE_DRIVER_RESET_MASK ((uint8_t)0x80)
// 0x291
typedef struct {
uint8_t motion_mode;
uint8_t mode_changing;
} MotionModeFeedbackMessage;
typedef struct { typedef struct {
uint8_t motor_id; uint8_t motor_id;
float driver_voltage; float driver_voltage;
@@ -156,45 +119,38 @@ typedef struct {
/***************** Sensor messages ******************/ /***************** Sensor messages ******************/
// 0x311
typedef struct { typedef struct {
float left_wheel; float left_wheel;
float right_wheel; float right_wheel;
} OdometryMessage; } OdometryMessage;
// 0x321
typedef struct { typedef struct {
float accel_x; float accel_x;
float accel_y; float accel_y;
float accel_z; float accel_z;
} ImuAccelMessage; } ImuAccelMessage;
// 0x322
typedef struct { typedef struct {
float gyro_x; float gyro_x;
float gyro_y; float gyro_y;
float gyro_z; float gyro_z;
} ImuGyroMessage; } ImuGyroMessage;
// 0x323
typedef struct { typedef struct {
float yaw; float yaw;
float pitch; float pitch;
float roll; float roll;
} ImuEulerMessage; } ImuEulerMessage;
// 0x331
typedef struct { typedef struct {
uint8_t trigger_state; uint8_t trigger_state;
} SafetyBumperMessage; } SafetyBumperMessage;
// 0x340 + num
typedef struct { typedef struct {
uint8_t sensor_id; uint8_t sensor_id;
uint8_t distance[8]; uint8_t distance[8];
} UltrasonicMessage; } UltrasonicMessage;
// 0x350 + num
typedef struct { typedef struct {
uint8_t sensor_id; uint8_t sensor_id;
float relative_distance; float relative_distance;
@@ -203,7 +159,6 @@ typedef struct {
int8_t channels[3]; int8_t channels[3];
} UwbMessage; } UwbMessage;
// 0x361
typedef struct { typedef struct {
uint8_t battery_soc; uint8_t battery_soc;
uint8_t battery_soh; uint8_t battery_soh;
@@ -250,12 +205,10 @@ typedef struct {
/************ Query/config messages ****************/ /************ Query/config messages ****************/
// 0x411
typedef struct { typedef struct {
bool request; bool request;
} VersionRequestMessage; } VersionRequestMessage;
// 0x41a
typedef struct { typedef struct {
uint16_t controller_hw_version; uint16_t controller_hw_version;
uint16_t motor_driver_hw_version; uint16_t motor_driver_hw_version;
@@ -263,22 +216,18 @@ typedef struct {
uint16_t motor_driver_sw_version; uint16_t motor_driver_sw_version;
} VersionResponseMessage; } VersionResponseMessage;
// 0x421
typedef struct { typedef struct {
ControlMode mode; ControlMode mode;
} ControlModeConfigMessage; } ControlModeConfigMessage;
// 0x431
typedef struct { typedef struct {
bool set_as_neutral; bool set_as_neutral;
} SteerNeutralRequestMessage; } SteerNeutralRequestMessage;
// 0x43a
typedef struct { typedef struct {
bool neutral_set_successful; bool neutral_set_successful;
} SteerNeutralResponseMessage; } SteerNeutralResponseMessage;
// 0x441
typedef enum { typedef enum {
CLEAR_ALL_FAULT = 0x00, CLEAR_ALL_FAULT = 0x00,
CLEAR_MOTOR1_FAULT = 0x01, CLEAR_MOTOR1_FAULT = 0x01,

54
include/ugv_sdk/details/interface/agilex_types.h Normal file
View File

@@ -0,0 +1,54 @@
/*
* agilex_types.h
*
* Created on: Jul 09, 2021 21:57
* Description:
*
* Copyright (c) 2021 Ruixiang Du (rdu)
*/
#ifndef AGILEX_TYPES_H
#define AGILEX_TYPES_H
#ifdef __cplusplus
extern "C" {
#endif
#include <stdint.h>
typedef enum {
CONST_OFF = 0x00,
CONST_ON = 0x01,
BREATH = 0x02,
CUSTOM = 0x03
} LightMode;
typedef struct {
LightMode mode;
uint8_t custom_value;
} LightOperation;
typedef enum {
VehicleStateNormal = 0x00,
VehicleStateEStop = 0x01,
VehicleStateException = 0x02
} VehicleState;
typedef enum {
// CONTROL_MODE_STANDBY = 0x00,
CONTROL_MODE_RC = 0x00,
CONTROL_MODE_CAN = 0x01,
CONTROL_MODE_UART = 0x02
} ControlMode;
typedef enum {
RC_SWITCH_UP = 0,
RC_SWITCH_MIDDLE,
RC_SWITCH_DOWN
} RcSwitchState;
#ifdef __cplusplus
}
#endif
#endif /* AGILEX_TYPES_H */

0
src/protocol_v2/ranger_base.cpp → include/ugv_sdk/details/robot_base/ranger_base.cpp
View File

0
src/protocol_v2/tracer_base.cpp → include/ugv_sdk/details/robot_base/tracer_base.cpp
View File

68
include/ugv_sdk/details/protocol_v1/agilex_message_v1.h → src/protocol_v1/agilex_message_v1.h
View File

@@ -21,37 +21,17 @@ extern "C" {
#include <stdint.h> #include <stdint.h>
#include <string.h> #include <string.h>
#include "ugv_sdk/details/interface/agilex_types.h"
/***************** Control messages *****************/ /***************** Control messages *****************/
typedef enum {
// CONTROL_MODE_STANDBY = 0x00,
CONTROL_MODE_RC = 0x00,
CONTROL_MODE_CAN = 0x01,
CONTROL_MODE_UART = 0x02
} ControlMode;
// 0x130
typedef struct { typedef struct {
ControlMode control_mode; ControlMode control_mode;
uint8_t fault_clear; bool clear_all_error;
float linear; float linear;
float angular; float angular;
} MotionCommandMessage; } MotionCommandMessage;
// 0x140
typedef enum {
CONST_OFF = 0x00,
CONST_ON = 0x01,
BREATH = 0x02,
CUSTOM = 0x03
} LightMode;
typedef struct {
LightMode mode;
uint8_t custom_value;
} LightOperation;
typedef struct { typedef struct {
bool enable_cmd_ctrl; bool enable_cmd_ctrl;
LightOperation front_light; LightOperation front_light;
@@ -64,12 +44,6 @@ typedef struct {
/**************** Feedback messages *****************/ /**************** Feedback messages *****************/
typedef enum {
VehicleStateNormal = 0x00,
VehicleStateEStop = 0x01,
VehicleStateException = 0x02
} VehicleState;
#define SYSTEM_ERROR_MOTOR_DRIVER_MASK ((uint16_t)0x0100) #define SYSTEM_ERROR_MOTOR_DRIVER_MASK ((uint16_t)0x0100)
#define SYSTEM_ERROR_HL_COMM_MASK ((uint16_t)0x0200) #define SYSTEM_ERROR_HL_COMM_MASK ((uint16_t)0x0200)
#define SYSTEM_ERROR_BATTERY_FAULT_MASK ((uint16_t)0x0001) #define SYSTEM_ERROR_BATTERY_FAULT_MASK ((uint16_t)0x0001)
@@ -88,49 +62,19 @@ typedef struct {
uint16_t error_code; uint16_t error_code;
} SystemStateMessage; } SystemStateMessage;
// 0x221
typedef struct { typedef struct {
float linear_velocity; float linear_velocity;
float angular_velocity; // only valid for differential drivering float angular_velocity;
float lateral_velocity;
float steering_angle; // only valid for ackermann steering
} MotionStateMessage; } MotionStateMessage;
// 0x231
typedef LightCommandMessage LightStateMessage; typedef LightCommandMessage LightStateMessage;
// 0x251 - 0x258
typedef struct { typedef struct {
uint8_t motor_id;
int16_t rpm;
float current; float current;
int32_t pulse_count; int16_t rpm;
float temperature;
} ActuatorStateMessage; } ActuatorStateMessage;
// 0x261 - 0x264
#define DRIVER_STATE_INPUT_VOLTAGE_LOW_MASK ((uint8_t)0x01)
#define DRIVER_STATE_MOTOR_OVERHEAT_MASK ((uint8_t)0x02)
#define DRIVER_STATE_DRIVER_OVERLOAD_MASK ((uint8_t)0x04)
#define DRIVER_STATE_DRIVER_OVERHEAT_MASK ((uint8_t)0x08)
#define DRIVER_STATE_SENSOR_FAULT_MASK ((uint8_t)0x10)
#define DRIVER_STATE_DRIVER_FAULT_MASK ((uint8_t)0x20)
#define DRIVER_STATE_DRIVER_ENABLED_MASK ((uint8_t)0x40)
#define DRIVER_STATE_DRIVER_RESET_MASK ((uint8_t)0x80)
// 0x291
typedef struct {
uint8_t motion_mode;
uint8_t mode_changing;
} MotionModeFeedbackMessage;
typedef struct {
uint8_t motor_id;
float driver_voltage;
float driver_temperature;
int8_t motor_temperature;
uint8_t driver_state;
} ActuatorLSStateMessage;
////////////////////////////////////////////////////// //////////////////////////////////////////////////////
typedef enum { typedef enum {

18
src/protocol_v1/agilex_msg_parser_v1.c Normal file
View File

@@ -0,0 +1,18 @@
/*
* agilex_msg_parser.c
*
* Created on: Jul 09, 2021 22:04
* Description:
*
* Copyright (c) 2021 Ruixiang Du (rdu)
*/
#include "agilex_protocol_v1.h"
#include "protocol_v1/agilex_msg_parser_v1.h"
#include "stdio.h"
#include "string.h"
bool DecodeCanFrame(const struct can_frame *rx_frame, AgxMessageV1 *msg) {}
void EncodeCanFrame(const AgxMessageV1 *msg, struct can_frame *tx_frame) {}
uint8_t CalcCanFrameChecksum(uint16_t id, uint8_t *data, uint8_t dlc) {}

41
src/protocol_v1/agilex_msg_parser_v1.h Normal file
View File

@@ -0,0 +1,41 @@
/*
* agilex_msg_parser.h
*
* Created on: Jul 09, 2021 22:04
* Description:
*
* Copyright (c) 2021 Ruixiang Du (rdu)
*/
#ifndef AGILEX_MSG_PARSER_H
#define AGILEX_MSG_PARSER_H
#ifdef __cplusplus
extern "C" {
#endif
#include <stdint.h>
#include <stdbool.h>
#include <string.h>
#ifdef __linux__
#include <linux/can.h>
#else
struct can_frame {
uint32_t can_id;
uint8_t can_dlc;
uint8_t data[8] __attribute__((aligned(8)));
};
#endif
#include "protocol_v1/agilex_message_v1.h"
bool DecodeCanFrame(const struct can_frame *rx_frame, AgxMessageV1 *msg);
void EncodeCanFrame(const AgxMessageV1 *msg, struct can_frame *tx_frame);
uint8_t CalcCanFrameChecksum(uint16_t id, uint8_t *data, uint8_t dlc);
#ifdef __cplusplus
}
#endif
#endif /* AGILEX_MSG_PARSER_H */

8
src/protocol_v1/agx_protocol_v1.h → src/protocol_v1/agilex_protocol_v1.h
View File

@@ -1,5 +1,5 @@
/* /*
* agx_protocol_v1.h * agilex_protocol_v1.h
* *
* Created on: Jul 09, 2021 20:34 * Created on: Jul 09, 2021 20:34
* Description: * Description:
@@ -7,8 +7,8 @@
* Copyright (c) 2021 Ruixiang Du (rdu) * Copyright (c) 2021 Ruixiang Du (rdu)
*/ */
#ifndef AGX_PROTOCOL_V1_H #ifndef AGILEX_PROTOCOL_V1_H
#define AGX_PROTOCOL_V1_H #define AGILEX_PROTOCOL_V1_H
#ifdef __cplusplus #ifdef __cplusplus
extern "C" { extern "C" {
@@ -194,4 +194,4 @@ typedef struct {
} }
#endif #endif
#endif /* AGX_PROTOCOL_V1_H */ #endif /* AGILEX_PROTOCOL_V1_H */

111
src/protocol_v1/parser_impl/scout_protocol_v1_parser.cpp Normal file
View File

@@ -0,0 +1,111 @@
/*
* scout_protocol_v1_parser.cpp
*
* Created on: Jul 08, 2021 22:43
* Description:
*
* Copyright (c) 2021 Ruixiang Du (rdu)
*/
#include "ugv_sdk/details/protocol_v1/scout_protocol_v1_parser.hpp"
namespace westonrobot {
// CAN support
bool ScoutProtocolV1Parser::DecodeMessage(const struct can_frame *rx_frame,
AgxMessage *msg) {
// ScoutMessage scout_msg;
// if ScoutMessage found, convert to AgxMessage
// if (DecodeScoutMsgFromCAN(rx_frame, &scout_msg)) {
// switch (scout_msg.type) {
// case ScoutMotionStatusMsg: {
// break;
// }
// case ScoutLightStatusMsg: {
// break;
// }
// case ScoutSystemStatusMsg: {
// break;
// }
// case ScoutMotorDriverStatusMsg: {
// break;
// }
// }
// }
return false;
}
void ScoutProtocolV1Parser::EncodeMessage(const AgxMessage *msg,
struct can_frame *tx_frame) {
// ScoutMessage scout_msg;
// convert to ScoutMessage, then encode to can frame
// switch (msg->type) {
// case AgxMsgMotionCommand: {
// // scout_msg.type = ScoutMotionControlMsg;
// // scout_msg.body.motion_control_msg.data.cmd.control_mode =
// // CTRL_MODE_COMMANDED;
// // scout_msg.body.motion_control_msg.data.cmd.control_mode =
// // CTRL_MODE_CMD_CAN;
// // scout_msg.body.motion_control_msg.data.cmd.fault_clear_flag =
// 0x00;
// /*
// std::lock_guard<std::mutex> guard(motion_cmd_mutex_);
// current_motion_cmd_.linear_velocity = static_cast<int8_t>(
// linear_vel / ScoutCmdLimits::max_linear_velocity * 100.0);
// current_motion_cmd_.angular_velocity = static_cast<int8_t>(
// angular_vel / ScoutCmdLimits::max_angular_velocity * 100.0);
// */
// // scout_msg.body.motion_control_msg.data.cmd.linear_velocity_cmd =
// // current_motion_cmd_.linear_velocity;
// // scout_msg.body.motion_control_msg.data.cmd.angular_velocity_cmd =
// // current_motion_cmd_.angular_velocity;
// // scout_msg.body.motion_control_msg.data.cmd.reserved0 = 0;
// // scout_msg.body.motion_control_msg.data.cmd.reserved1 = 0;
// // scout_msg.body.motion_control_msg.data.cmd.count = count;
// // scout_msg.body.motion_control_msg.data.cmd.checksum =
// // CalculateChecksum(CAN_MSG_MOTION_CONTROL_CMD_ID,
// // scout_msg.body.motion_control_msg.data.raw,
// 8);
// break;
// }
// case AgxMsgLightCommand: {
// scout_msg.body.light_control_msg.data.cmd.light_ctrl_enable =
// LIGHT_ENABLE_CTRL;
// // scout_msg.body.light_control_msg.data.cmd.front_mode =
// // static_cast<uint8_t>(current_light_cmd_.front_mode);
// // scout_msg.body.light_control_msg.data.cmd.front_custom =
// // current_light_cmd_.front_custom_value;
// // scout_msg.body.light_control_msg.data.cmd.rear_mode =
// // static_cast<uint8_t>(current_light_cmd_.rear_mode);
// // scout_msg.body.light_control_msg.data.cmd.rear_custom =
// // current_light_cmd_.rear_custom_value;
// // scout_msg.body.light_control_msg.data.cmd.reserved0 = 0;
// // scout_msg.body.light_control_msg.data.cmd.count = count;
// scout_msg.body.light_control_msg.data.cmd.checksum =
// CalculateChecksum(CAN_MSG_LIGHT_CONTROL_CMD_ID,
// scout_msg.body.light_control_msg.data.raw, 8);
// break;
// }
// }
}
uint8_t ScoutProtocolV1Parser::CalculateChecksum(uint16_t id, uint8_t *data,
uint8_t dlc) {
uint8_t checksum = 0x00;
checksum = (uint8_t)(id & 0x00ff) + (uint8_t)(id >> 8) + dlc;
for (int i = 0; i < (dlc - 1); ++i) checksum += data[i];
return checksum;
}
// UART support
bool ScoutProtocolV1Parser::DecodeMessage(uint8_t *data, uint8_t dlc,
AgxMessage *msg) {}
void ScoutProtocolV1Parser::EncodeMessage(const AgxMessage *msg, uint8_t *buf,
uint8_t *len) {}
uint8_t ScoutProtocolV1Parser::CalculateChecksum(uint8_t *buf, uint8_t len) {}
} // namespace westonrobot

178
src/protocol_v1/scout/scout_can_parser.c
View File

@@ -1,178 +0,0 @@
/*
* scout_can_parser.c
*
* Created on: Aug 31, 2019 04:25
* Description:
*
* Copyright (c) 2019 Ruixiang Du (rdu)
*/
#include "protocol_v1/scout/scout_can_parser.h"
#include "string.h"
static void EncodeScoutMotionControlMsgToCAN(const MotionControlMessage *msg, struct can_frame *tx_frame);
static void EncodeScoutLightControlMsgToCAN(const LightControlMessage *msg, struct can_frame *tx_frame);
bool DecodeScoutMsgFromCAN(const struct can_frame *rx_frame, ScoutMessage *msg)
{
msg->type = ScoutMsgNone;
switch (rx_frame->can_id)
{
// in the current implementation, both MsgType and can_frame include 8 * uint8_t
case CAN_MSG_MOTION_CONTROL_STATUS_ID:
{
msg->type = ScoutMotionStatusMsg;
// msg->motion_status_msg.id = CAN_MSG_MOTION_CONTROL_STATUS_ID;
memcpy(msg->body.motion_status_msg.data.raw, rx_frame->data, rx_frame->can_dlc * sizeof(uint8_t));
break;
}
case CAN_MSG_LIGHT_CONTROL_STATUS_ID:
{
msg->type = ScoutLightStatusMsg;
// msg->light_status_msg.id = CAN_MSG_LIGHT_CONTROL_STATUS_ID;
memcpy(msg->body.light_status_msg.data.raw, rx_frame->data, rx_frame->can_dlc * sizeof(uint8_t));
break;
}
case CAN_MSG_SYSTEM_STATUS_STATUS_ID:
{
msg->type = ScoutSystemStatusMsg;
// msg->system_status_msg.id = CAN_MSG_SYSTEM_STATUS_STATUS_ID;
memcpy(msg->body.system_status_msg.data.raw, rx_frame->data, rx_frame->can_dlc * sizeof(uint8_t));
break;
}
case CAN_MSG_MOTOR1_DRIVER_STATUS_ID:
{
msg->type = ScoutMotorDriverStatusMsg;
// msg->motor_driver_status_msg.id = CAN_MSG_MOTOR1_DRIVER_STATUS_ID;
msg->body.motor_driver_status_msg.motor_id = SCOUT_MOTOR1_ID;
memcpy(msg->body.motor_driver_status_msg.data.raw, rx_frame->data, rx_frame->can_dlc * sizeof(uint8_t));
break;
}
case CAN_MSG_MOTOR2_DRIVER_STATUS_ID:
{
msg->type = ScoutMotorDriverStatusMsg;
// msg->motor_driver_status_msg.id = CAN_MSG_MOTOR2_DRIVER_STATUS_ID;
msg->body.motor_driver_status_msg.motor_id = SCOUT_MOTOR2_ID;
memcpy(msg->body.motor_driver_status_msg.data.raw, rx_frame->data, rx_frame->can_dlc * sizeof(uint8_t));
break;
}
case CAN_MSG_MOTOR3_DRIVER_STATUS_ID:
{
msg->type = ScoutMotorDriverStatusMsg;
// msg->motor_driver_status_msg.id = CAN_MSG_MOTOR3_DRIVER_STATUS_ID;
msg->body.motor_driver_status_msg.motor_id = SCOUT_MOTOR3_ID;
memcpy(msg->body.motor_driver_status_msg.data.raw, rx_frame->data, rx_frame->can_dlc * sizeof(uint8_t));
break;
}
case CAN_MSG_MOTOR4_DRIVER_STATUS_ID:
{
msg->type = ScoutMotorDriverStatusMsg;
// msg->motor_driver_status_msg.id = CAN_MSG_MOTOR4_DRIVER_STATUS_ID;
msg->body.motor_driver_status_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));
break;
}
// in the current implementation, both MsgType and can_frame include 8 * uint8_t
case CAN_MSG_MOTION_CONTROL_CMD_ID:
{
msg->type = ScoutMotionControlMsg;
// msg->motion_control_msg.id = CAN_MSG_MOTION_CONTROL_CMD_ID;
memcpy(msg->body.motion_control_msg.data.raw, rx_frame->data, rx_frame->can_dlc * sizeof(uint8_t));
break;
}
case CAN_MSG_LIGHT_CONTROL_CMD_ID:
{
msg->type = ScoutLightControlMsg;
// msg->light_control_msg.id = CAN_MSG_LIGHT_CONTROL_STATUS_ID;
memcpy(msg->body.light_control_msg.data.raw, rx_frame->data, rx_frame->can_dlc * sizeof(uint8_t));
break;
}
default:
break;
}
return true;
}
void EncodeScoutMsgToCAN(const ScoutMessage *msg, struct can_frame *tx_frame)
{
switch (msg->type)
{
// in the current implementation, both MsgType and can_frame include 8 * uint8_t
case ScoutMotionStatusMsg:
{
tx_frame->can_id = CAN_MSG_MOTION_CONTROL_STATUS_ID;
tx_frame->can_dlc = 8;
memcpy(tx_frame->data, msg->body.motion_status_msg.data.raw, tx_frame->can_dlc);
break;
}
case ScoutLightStatusMsg:
{
tx_frame->can_id = CAN_MSG_LIGHT_CONTROL_STATUS_ID;
tx_frame->can_dlc = 8;
memcpy(tx_frame->data, msg->body.light_status_msg.data.raw, tx_frame->can_dlc);
break;
}
case ScoutSystemStatusMsg:
{
tx_frame->can_id = CAN_MSG_SYSTEM_STATUS_STATUS_ID;
tx_frame->can_dlc = 8;
memcpy(tx_frame->data, msg->body.system_status_msg.data.raw, tx_frame->can_dlc);
break;
}
case ScoutMotorDriverStatusMsg:
{
if (msg->body.motor_driver_status_msg.motor_id == SCOUT_MOTOR1_ID)
tx_frame->can_id = CAN_MSG_MOTOR1_DRIVER_STATUS_ID;
else if (msg->body.motor_driver_status_msg.motor_id == SCOUT_MOTOR2_ID)
tx_frame->can_id = CAN_MSG_MOTOR2_DRIVER_STATUS_ID;
else if (msg->body.motor_driver_status_msg.motor_id == SCOUT_MOTOR3_ID)
tx_frame->can_id = CAN_MSG_MOTOR3_DRIVER_STATUS_ID;
else if (msg->body.motor_driver_status_msg.motor_id == SCOUT_MOTOR4_ID)
tx_frame->can_id = CAN_MSG_MOTOR4_DRIVER_STATUS_ID;
tx_frame->can_dlc = 8;
memcpy(tx_frame->data, msg->body.motor_driver_status_msg.data.raw, tx_frame->can_dlc);
break;
}
case ScoutMotionControlMsg:
{
EncodeScoutMotionControlMsgToCAN(&(msg->body.motion_control_msg), tx_frame);
break;
}
case ScoutLightControlMsg:
{
EncodeScoutLightControlMsgToCAN(&(msg->body.light_control_msg), tx_frame);
break;
}
default:
break;
}
tx_frame->data[7] = CalcScoutCANChecksum(tx_frame->can_id, tx_frame->data, tx_frame->can_dlc);
}
void EncodeScoutMotionControlMsgToCAN(const MotionControlMessage *msg, struct can_frame *tx_frame)
{
tx_frame->can_id = CAN_MSG_MOTION_CONTROL_CMD_ID;
tx_frame->can_dlc = 8;
memcpy(tx_frame->data, msg->data.raw, tx_frame->can_dlc);
tx_frame->data[7] = CalcScoutCANChecksum(tx_frame->can_id, tx_frame->data, tx_frame->can_dlc);
}
void EncodeScoutLightControlMsgToCAN(const LightControlMessage *msg, struct can_frame *tx_frame)
{
tx_frame->can_id = CAN_MSG_LIGHT_CONTROL_CMD_ID;
tx_frame->can_dlc = 8;
memcpy(tx_frame->data, msg->data.raw, tx_frame->can_dlc);
tx_frame->data[7] = CalcScoutCANChecksum(tx_frame->can_id, tx_frame->data, tx_frame->can_dlc);
}
uint8_t CalcScoutCANChecksum(uint16_t id, uint8_t *data, uint8_t dlc)
{
uint8_t checksum = 0x00;
checksum = (uint8_t)(id & 0x00ff) + (uint8_t)(id >> 8) + dlc;
for (int i = 0; i < (dlc - 1); ++i)
checksum += data[i];
return checksum;
}

42
src/protocol_v1/scout/scout_can_parser.h
View File

@@ -1,42 +0,0 @@
/*
* scout_can_parser.h
*
* Created on: Aug 31, 2019 04:23
* Description:
*
* Copyright (c) 2019 Ruixiang Du (rdu)
*/
#ifndef SCOUT_CAN_PARSER_H
#define SCOUT_CAN_PARSER_H
#ifdef __cplusplus
extern "C" {
#endif
#include <stdint.h>
#include <stdbool.h>
#include "protocol_v1/scout/scout_protocol.h"
#ifdef __linux__
#include <linux/can.h>
#else
struct can_frame
{
uint32_t can_id;
uint8_t can_dlc;
uint8_t data[8]__attribute__((aligned(8)));
};
#endif
bool DecodeScoutMsgFromCAN(const struct can_frame *rx_frame, ScoutMessage *msg);
void EncodeScoutMsgToCAN(const ScoutMessage *msg, struct can_frame *tx_frame);
uint8_t CalcScoutCANChecksum(uint16_t id, uint8_t *data, uint8_t dlc);
#ifdef __cplusplus
}
#endif
#endif /* SCOUT_CAN_PARSER_H */

254
src/protocol_v1/scout/scout_protocol.h
View File

@@ -1,254 +0,0 @@
/*
* scout_protocol.h
*
* Created on: Aug 07, 2019 21:49
* Description:
*
* Copyright (c) 2019 Ruixiang Du (rdu)
*/
#ifndef SCOUT_PROTOCOL_H
#define SCOUT_PROTOCOL_H
#ifdef __cplusplus
extern "C" {
#endif
#include <stdint.h>
#define SCOUT_CMD_BUF_LEN 32
#define SCOUT_STATUS_BUF_LEN 32
#define SCOUT_FRAME_SIZE 13
#define SCOUT_MOTOR1_ID ((uint8_t)0x00)
#define SCOUT_MOTOR2_ID ((uint8_t)0x01)
#define SCOUT_MOTOR3_ID ((uint8_t)0x02)
#define SCOUT_MOTOR4_ID ((uint8_t)0x03)
// UART Definitions
#define UART_FRAME_SYSTEM_STATUS_ID ((uint8_t)0x01)
#define UART_FRAME_MOTION_STATUS_ID ((uint8_t)0x02)
#define UART_FRAME_MOTOR1_DRIVER_STATUS_ID ((uint8_t)0x03)
#define UART_FRAME_MOTOR2_DRIVER_STATUS_ID ((uint8_t)0x04)
#define UART_FRAME_MOTOR3_DRIVER_STATUS_ID ((uint8_t)0x05)
#define UART_FRAME_MOTOR4_DRIVER_STATUS_ID ((uint8_t)0x06)
#define UART_FRAME_LIGHT_STATUS_ID ((uint8_t)0x07)
#define UART_FRAME_MOTION_CONTROL_ID ((uint8_t)0x01)
#define UART_FRAME_LIGHT_CONTROL_ID ((uint8_t)0x02)
// CAN Definitions
#define CAN_MSG_MOTION_CONTROL_CMD_ID ((uint32_t)0x130)
#define CAN_MSG_MOTION_CONTROL_STATUS_ID ((uint32_t)0x131)
#define CAN_MSG_LIGHT_CONTROL_CMD_ID ((uint32_t)0x140)
#define CAN_MSG_LIGHT_CONTROL_STATUS_ID ((uint32_t)0x141)
#define CAN_MSG_SYSTEM_STATUS_STATUS_ID ((uint32_t)0x151)
#define CAN_MSG_MOTOR1_DRIVER_STATUS_ID ((uint32_t)0x200)
#define CAN_MSG_MOTOR2_DRIVER_STATUS_ID ((uint32_t)0x201)
#define CAN_MSG_MOTOR3_DRIVER_STATUS_ID ((uint32_t)0x202)
#define CAN_MSG_MOTOR4_DRIVER_STATUS_ID ((uint32_t)0x203)
/*--------------------- Control/State Constants ------------------------*/
// Motion Control
// #define CTRL_MODE_REMOTE ((uint8_t)0x00)
// #define CTRL_MODE_CMD_CAN ((uint8_t)0x01)
// #define CTRL_MODE_CMD_UART ((uint8_t)0x02)
// #define CTRL_MODE_COMMANDED ((uint8_t)0x03)
#define FAULT_CLR_NONE ((uint8_t)0x00)
#define FAULT_CLR_BAT_UNDER_VOL ((uint8_t)0x01)
#define FAULT_CLR_BAT_OVER_VOL ((uint8_t)0x02)
#define FAULT_CLR_MOTOR1_COMM ((uint8_t)0x03)
#define FAULT_CLR_MOTOR2_COMM ((uint8_t)0x04)
#define FAULT_CLR_MOTOR3_COMM ((uint8_t)0x05)
#define FAULT_CLR_MOTOR4_COMM ((uint8_t)0x06)
#define FAULT_CLR_MOTOR_DRV_OVERHEAT ((uint8_t)0x07)
#define FAULT_CLR_MOTOR_OVERCURRENT ((uint8_t)0x08)
// Light Control
#define LIGHT_DISABLE_CTRL ((uint8_t)0x00)
#define LIGHT_ENABLE_CTRL ((uint8_t)0x01)
#define LIGHT_MODE_CONST_OFF ((uint8_t)0x00)
#define LIGHT_MODE_CONST_ON ((uint8_t)0x01)
#define LIGHT_MODE_BREATH ((uint8_t)0x02)
#define LIGHT_MODE_CUSTOM ((uint8_t)0x03)
// System Status Feedback
#define BASE_STATE_NORMAL ((uint8_t)0x00)
#define BASE_STATE_ESTOP ((uint8_t)0x01)
#define BASE_STATE_EXCEPTION ((uint8_t)0x02)
#define FAULT_CAN_CHECKSUM_ERROR ((uint16_t)0x0100)
#define FAULT_MOTOR_DRV_OVERHEAT_W ((uint16_t)0x0200)
#define FAULT_MOTOR_OVERCURRENT_W ((uint16_t)0x0400)
#define FAULT_BAT_UNDER_VOL_W ((uint16_t)0x0800)
#define FAULT_RC_SIGNAL_LOSS ((uint16_t)0x1000)
#define FAULT_HIGH_BYTE_RESERVED2 ((uint16_t)0x2000)
#define FAULT_HIGH_BYTE_RESERVED3 ((uint16_t)0x4000)
#define FAULT_HIGH_BYTE_RESERVED4 ((uint16_t)0x8000)
#define FAULT_BAT_UNDER_VOL_F ((uint16_t)0x0001)
#define FAULT_BAT_OVER_VOL_F ((uint16_t)0x0002)
#define FAULT_MOTOR1_COMM_F ((uint16_t)0x0004)
#define FAULT_MOTOR2_COMM_F ((uint16_t)0x0008)
#define FAULT_MOTOR3_COMM_F ((uint16_t)0x0010)
#define FAULT_MOTOR4_COMM_F ((uint16_t)0x0020)
#define FAULT_MOTOR_DRV_OVERHEAT_F ((uint16_t)0x0040)
#define FAULT_MOTOR_OVERCURRENT_F ((uint16_t)0x0080)
/*-------------------- Control/Feedback Messages -----------------------*/
/* No padding in the struct */
// reference: https://stackoverflow.com/questions/3318410/pragma-pack-effect
#pragma pack(push, 1)
// Note: id could be different for UART and CAN protocol
// Motion Control
typedef struct {
union {
struct {
uint8_t control_mode;
uint8_t fault_clear_flag;
int8_t linear_velocity_cmd;
int8_t angular_velocity_cmd;
uint8_t reserved0;
uint8_t reserved1;
uint8_t count;
uint8_t checksum;
} cmd;
uint8_t raw[8];
} data;
} MotionControlMessage;
typedef struct {
union {
struct {
struct {
uint8_t high_byte;
uint8_t low_byte;
} linear_velocity;
struct {
uint8_t high_byte;
uint8_t low_byte;
} angular_velocity;
uint8_t reserved0;
uint8_t reserved1;
uint8_t count;
uint8_t checksum;
} status;
uint8_t raw[8];
} data;
} MotionStatusMessage;
// System Status Feedback
typedef struct {
union {
struct {
uint8_t base_state;
uint8_t control_mode;
struct {
uint8_t high_byte;
uint8_t low_byte;
} battery_voltage;
struct {
uint8_t high_byte;
uint8_t low_byte;
} fault_code;
uint8_t count;
uint8_t checksum;
} status;
uint8_t raw[8];
} data;
} SystemStatusMessage;
// Light Control
typedef struct {
union {
struct {
uint8_t light_ctrl_enable;
uint8_t front_mode;
uint8_t front_custom;
uint8_t rear_mode;
uint8_t rear_custom;
uint8_t reserved0;
uint8_t count;
uint8_t checksum;
} cmd;
uint8_t raw[8];
} data;
} LightControlMessage;
typedef struct {
union {
struct {
uint8_t light_ctrl_enable;
uint8_t front_mode;
uint8_t front_custom;
uint8_t rear_mode;
uint8_t rear_custom;
uint8_t reserved0;
uint8_t count;
uint8_t checksum;
} status;
uint8_t raw[8];
} data;
} LightStatusMessage;
// Motor Driver Feedback
typedef struct {
uint8_t motor_id;
union {
struct {
struct {
uint8_t high_byte;
uint8_t low_byte;
} current;
struct {
uint8_t high_byte;
uint8_t low_byte;
} rpm;
int8_t temperature;
uint8_t reserved0;
uint8_t count;
uint8_t checksum;
} status;
uint8_t raw[8];
} data;
} MotorDriverStatusMessage;
// For convenience to access status/control message
typedef enum {
ScoutMsgNone = 0x00,
// status messages
ScoutMotionStatusMsg = 0x01,
ScoutLightStatusMsg = 0x02,
ScoutSystemStatusMsg = 0x03,
ScoutMotorDriverStatusMsg = 0x04,
// control messages
ScoutMotionControlMsg = 0x21,
ScoutLightControlMsg = 0x22
} ScoutMsgType;
typedef struct {
ScoutMsgType type;
union {
// status messages
MotionStatusMessage motion_status_msg;
LightStatusMessage light_status_msg;
SystemStatusMessage system_status_msg;
MotorDriverStatusMessage motor_driver_status_msg;
// control messages
MotionControlMessage motion_control_msg;
LightControlMessage light_control_msg;
} body;
} ScoutMessage;
#pragma pack(pop)
#ifdef __cplusplus
}
#endif
#endif /* SCOUT_PROTOCOL_H */

111
src/protocol_v1/scout/scout_protocol_v1_parser.cpp
View File

@@ -1,111 +0,0 @@
/*
* scout_protocol_v1_parser.cpp
*
* Created on: Jul 08, 2021 22:43
* Description:
*
* Copyright (c) 2021 Ruixiang Du (rdu)
*/
#include "ugv_sdk/details/protocol_v1/scout_protocol_v1_parser.hpp"
#include "protocol_v1/scout/scout_can_parser.h"
namespace westonrobot {
// CAN support
bool ScoutProtocolV1Parser::DecodeMessage(const struct can_frame *rx_frame,
AgxMessage *msg) {
ScoutMessage scout_msg;
// if ScoutMessage found, convert to AgxMessage
if (DecodeScoutMsgFromCAN(rx_frame, &scout_msg)) {
switch (scout_msg.type) {
case ScoutMotionStatusMsg: {
break;
}
case ScoutLightStatusMsg: {
break;
}
case ScoutSystemStatusMsg: {
break;
}
case ScoutMotorDriverStatusMsg: {
break;
}
}
}
return false;
}
void ScoutProtocolV1Parser::EncodeMessage(const AgxMessage *msg,
struct can_frame *tx_frame) {
ScoutMessage scout_msg;
// convert to ScoutMessage, then encode to can frame
switch (msg->type) {
case AgxMsgMotionCommand: {
// scout_msg.type = ScoutMotionControlMsg;
// scout_msg.body.motion_control_msg.data.cmd.control_mode =
// CTRL_MODE_COMMANDED;
// scout_msg.body.motion_control_msg.data.cmd.control_mode =
// CTRL_MODE_CMD_CAN;
// scout_msg.body.motion_control_msg.data.cmd.fault_clear_flag = 0x00;
/*
std::lock_guard<std::mutex> guard(motion_cmd_mutex_);
current_motion_cmd_.linear_velocity = static_cast<int8_t>(
linear_vel / ScoutCmdLimits::max_linear_velocity * 100.0);
current_motion_cmd_.angular_velocity = static_cast<int8_t>(
angular_vel / ScoutCmdLimits::max_angular_velocity * 100.0);
*/
// scout_msg.body.motion_control_msg.data.cmd.linear_velocity_cmd =
// current_motion_cmd_.linear_velocity;
// scout_msg.body.motion_control_msg.data.cmd.angular_velocity_cmd =
// current_motion_cmd_.angular_velocity;
// scout_msg.body.motion_control_msg.data.cmd.reserved0 = 0;
// scout_msg.body.motion_control_msg.data.cmd.reserved1 = 0;
// scout_msg.body.motion_control_msg.data.cmd.count = count;
// scout_msg.body.motion_control_msg.data.cmd.checksum =
// CalculateChecksum(CAN_MSG_MOTION_CONTROL_CMD_ID,
// scout_msg.body.motion_control_msg.data.raw, 8);
break;
}
case AgxMsgLightCommand: {
scout_msg.body.light_control_msg.data.cmd.light_ctrl_enable =
LIGHT_ENABLE_CTRL;
// scout_msg.body.light_control_msg.data.cmd.front_mode =
// static_cast<uint8_t>(current_light_cmd_.front_mode);
// scout_msg.body.light_control_msg.data.cmd.front_custom =
// current_light_cmd_.front_custom_value;
// scout_msg.body.light_control_msg.data.cmd.rear_mode =
// static_cast<uint8_t>(current_light_cmd_.rear_mode);
// scout_msg.body.light_control_msg.data.cmd.rear_custom =
// current_light_cmd_.rear_custom_value;
// scout_msg.body.light_control_msg.data.cmd.reserved0 = 0;
// scout_msg.body.light_control_msg.data.cmd.count = count;
scout_msg.body.light_control_msg.data.cmd.checksum =
CalculateChecksum(CAN_MSG_LIGHT_CONTROL_CMD_ID,
scout_msg.body.light_control_msg.data.raw, 8);
break;
}
}
}
uint8_t ScoutProtocolV1Parser::CalculateChecksum(uint16_t id, uint8_t *data,
uint8_t dlc) {
uint8_t checksum = 0x00;
checksum = (uint8_t)(id & 0x00ff) + (uint8_t)(id >> 8) + dlc;
for (int i = 0; i < (dlc - 1); ++i) checksum += data[i];
return checksum;
}
// UART support
bool ScoutProtocolV1Parser::DecodeMessage(uint8_t *data, uint8_t dlc,
AgxMessage *msg) {}
void ScoutProtocolV1Parser::EncodeMessage(const AgxMessage *msg, uint8_t *buf,
uint8_t *len) {}
uint8_t ScoutProtocolV1Parser::CalculateChecksum(uint8_t *buf, uint8_t len) {}
} // namespace westonrobot

629
src/protocol_v1/scout/scout_uart_parser.c
View File

@@ -1,629 +0,0 @@
/*
* scout_uart_parser.c
*
* Created on: Aug 14, 2019 12:02
* Description:
*
* Copyright (c) 2019 Ruixiang Du (rdu)
*/
#include "protocol_v1/scout/scout_uart_parser.h"
// #define USE_XOR_CHECKSUM
// #define PRINT_CPP_DEBUG_INFO
// #define PRINT_JLINK_DEBUG_INFO
#ifdef PRINT_CPP_DEBUG_INFO
#undef PRINT_JLINK_DEBUG_INFO
#endif
#ifdef PRINT_CPP_DEBUG_INFO
#define <iostream>
#elif (defined(PRINT_JLINK_DEBUG_INFO))
#include "segger/jlink_rtt.h"
#endif
typedef enum
{
WAIT_FOR_SOF1 = 0,
WAIT_FOR_SOF2,
WAIT_FOR_FRAME_LEN,
WAIT_FOR_FRAME_TYPE,
WAIT_FOR_FRAME_ID,
WAIT_FOR_PAYLOAD,
WAIT_FOR_FRAME_COUNT,
WAIT_FOR_CHECKSUM
} ScoutSerialDecodeState;
#define PAYLOAD_BUFFER_SIZE (SCOUT_FRAME_SIZE * 2)
#define FRAME_SOF_LEN ((uint8_t)2)
#define FRAME_FIXED_FIELD_LEN ((uint8_t)4)
#define FRAME_SOF1 ((uint8_t)0x5a)
#define FRAME_SOF2 ((uint8_t)0xa5)
#define FRAME_TYPE_CONTROL ((uint8_t)0x55)
#define FRAME_TYPE_STATUS ((uint8_t)0xaa)
#define FRAME_NONE_ID ((uint8_t)0x00)
// frame buffer
static struct
{
uint8_t frame_id;
uint8_t frame_type;
uint8_t frame_len;
uint8_t frame_cnt;
uint8_t frame_checksum;
uint8_t internal_checksum;
uint8_t payload_buffer[PAYLOAD_BUFFER_SIZE];
size_t payload_data_pos;
} uart_parsing_data;
// statisctics
typedef struct
{
uint32_t frame_parsed;
uint32_t frame_with_wrong_checksum;
} UARTParsingStats;
static UARTParsingStats uart_parsing_stats = {.frame_parsed = true, .frame_with_wrong_checksum = 123};
// internal functions
static bool ParseChar(uint8_t c, ScoutMessage *msg);
static uint8_t CalcBufferedFrameChecksum();
static bool ConstructStatusMessage(ScoutMessage *msg);
static bool ConstructControlMessage(ScoutMessage *msg);
static void EncodeMotionControlMsgToUART(const MotionControlMessage *msg, uint8_t *buf, uint8_t *len);
static void EncodeLightControlMsgToUART(const LightControlMessage *msg, uint8_t *buf, uint8_t *len);
void EncodeScoutMsgToUART(const ScoutMessage *msg, uint8_t *buf, uint8_t *len)
{
// SOF
buf[0] = FRAME_SOF1;
buf[1] = FRAME_SOF2;
// frame len, type, ID
buf[2] = 0x0a;
buf[3] = FRAME_TYPE_STATUS;
switch (msg->type)
{
// in the current implementation, both MsgType and can_frame include 8 * uint8_t
case ScoutMotionStatusMsg:
{
buf[4] = UART_FRAME_MOTION_STATUS_ID;
buf[5] = msg->body.motion_status_msg.data.status.linear_velocity.high_byte;
buf[6] = msg->body.motion_status_msg.data.status.linear_velocity.low_byte;
buf[7] = msg->body.motion_status_msg.data.status.angular_velocity.high_byte;
buf[8] = msg->body.motion_status_msg.data.status.angular_velocity.low_byte;
buf[9] = 0;
buf[10] = 0;
buf[11] = msg->body.motion_status_msg.data.status.count;
break;
}
case ScoutLightStatusMsg:
{
buf[4] = UART_FRAME_LIGHT_STATUS_ID;
buf[5] = msg->body.light_status_msg.data.status.light_ctrl_enable;
buf[6] = msg->body.light_status_msg.data.status.front_mode;
buf[7] = msg->body.light_status_msg.data.status.front_custom;
buf[8] = msg->body.light_status_msg.data.status.rear_mode;
buf[9] = msg->body.light_status_msg.data.status.rear_custom;
buf[10] = 0;
buf[11] = msg->body.light_status_msg.data.status.count;
break;
}
case ScoutSystemStatusMsg:
{
buf[4] = UART_FRAME_SYSTEM_STATUS_ID;
buf[5] = msg->body.system_status_msg.data.status.base_state;
buf[6] = msg->body.system_status_msg.data.status.control_mode;
buf[7] = msg->body.system_status_msg.data.status.battery_voltage.high_byte;
buf[8] = msg->body.system_status_msg.data.status.battery_voltage.low_byte;
buf[9] = msg->body.system_status_msg.data.status.fault_code.high_byte;
buf[10] = msg->body.system_status_msg.data.status.fault_code.low_byte;
buf[11] = msg->body.system_status_msg.data.status.count;
break;
}
case ScoutMotorDriverStatusMsg:
{
if (msg->body.motor_driver_status_msg.motor_id == SCOUT_MOTOR1_ID)
buf[4] = UART_FRAME_MOTOR1_DRIVER_STATUS_ID;
else if (msg->body.motor_driver_status_msg.motor_id == SCOUT_MOTOR2_ID)
buf[4] = UART_FRAME_MOTOR2_DRIVER_STATUS_ID;
else if (msg->body.motor_driver_status_msg.motor_id == SCOUT_MOTOR3_ID)
buf[4] = UART_FRAME_MOTOR3_DRIVER_STATUS_ID;
else if (msg->body.motor_driver_status_msg.motor_id == SCOUT_MOTOR4_ID)
buf[4] = UART_FRAME_MOTOR4_DRIVER_STATUS_ID;
buf[5] = msg->body.motor_driver_status_msg.data.status.current.high_byte;
buf[6] = msg->body.motor_driver_status_msg.data.status.current.low_byte;
buf[7] = msg->body.motor_driver_status_msg.data.status.rpm.high_byte;
buf[8] = msg->body.motor_driver_status_msg.data.status.rpm.low_byte;
buf[9] = msg->body.motor_driver_status_msg.data.status.temperature;
buf[10] = 0;
buf[11] = msg->body.motor_driver_status_msg.data.status.count;
break;
}
case ScoutMotionControlMsg:
{
EncodeMotionControlMsgToUART(&(msg->body.motion_control_msg), buf, len);
break;
}
case ScoutLightControlMsg:
{
EncodeLightControlMsgToUART(&(msg->body.light_control_msg), buf, len);
break;
}
default:
break;
}
buf[12] = CalcScoutUARTChecksum(buf, buf[2] + FRAME_SOF_LEN);
// length: SOF + Frame + Checksum
*len = buf[2] + FRAME_SOF_LEN + 1;
}
bool DecodeScoutMsgFromUART(uint8_t c, ScoutMessage *msg)
{
static ScoutMessage decoded_msg;
bool result = ParseChar(c, &decoded_msg);
if (result)
*msg = decoded_msg;
return result;
}
void EncodeMotionControlMsgToUART(const MotionControlMessage *msg, uint8_t *buf, uint8_t *len)
{
// SOF
buf[0] = FRAME_SOF1;
buf[1] = FRAME_SOF2;
// frame len, type, ID
buf[2] = 0x0a;
buf[3] = FRAME_TYPE_CONTROL;
buf[4] = UART_FRAME_MOTION_CONTROL_ID;
// frame payload
buf[5] = msg->data.cmd.control_mode;
buf[6] = msg->data.cmd.fault_clear_flag;
buf[7] = msg->data.cmd.linear_velocity_cmd;
buf[8] = msg->data.cmd.angular_velocity_cmd;
buf[9] = 0x00;
buf[10] = 0x00;
// frame count, checksum
buf[11] = msg->data.cmd.count;
buf[12] = CalcScoutUARTChecksum(buf, buf[2] + FRAME_SOF_LEN);
// length: SOF + Frame + Checksum
*len = buf[2] + FRAME_SOF_LEN + 1;
}
void EncodeLightControlMsgToUART(const LightControlMessage *msg, uint8_t *buf, uint8_t *len)
{
// SOF
buf[0] = FRAME_SOF1;
buf[1] = FRAME_SOF2;
// frame len, type, ID
buf[2] = 0x0a;
buf[3] = FRAME_TYPE_CONTROL;
buf[4] = UART_FRAME_LIGHT_CONTROL_ID;
// frame payload
buf[5] = msg->data.cmd.light_ctrl_enable;
buf[6] = msg->data.cmd.front_mode;
buf[7] = msg->data.cmd.front_custom;
buf[8] = msg->data.cmd.rear_mode;
buf[9] = msg->data.cmd.rear_custom;
buf[10] = 0x00;
// frame count, checksum
buf[11] = msg->data.cmd.count;
buf[12] = CalcScoutUARTChecksum(buf, buf[2] + FRAME_SOF_LEN);
// length: SOF + Frame + Checksum
*len = buf[2] + FRAME_SOF_LEN + 1;
}
bool ParseChar(uint8_t c, ScoutMessage *msg)
{
static ScoutSerialDecodeState decode_state = WAIT_FOR_SOF1;
bool new_frame_parsed = false;
switch (decode_state)
{
case WAIT_FOR_SOF1:
{
if (c == FRAME_SOF1)
{
uart_parsing_data.frame_id = FRAME_NONE_ID;
uart_parsing_data.frame_type = 0;
uart_parsing_data.frame_len = 0;
uart_parsing_data.frame_cnt = 0;
uart_parsing_data.frame_checksum = 0;
uart_parsing_data.internal_checksum = 0;
uart_parsing_data.payload_data_pos = 0;
memset(uart_parsing_data.payload_buffer, 0, PAYLOAD_BUFFER_SIZE);
decode_state = WAIT_FOR_SOF2;
#ifdef PRINT_CPP_DEBUG_INFO
std::cout << "found sof1" << std::endl;
#elif (defined(PRINT_JLINK_DEBUG_INFO))
JLinkWriteString(0, "found sof1\n");
#endif
}
break;
}
case WAIT_FOR_SOF2:
{
if (c == FRAME_SOF2)
{
decode_state = WAIT_FOR_FRAME_LEN;
#ifdef PRINT_CPP_DEBUG_INFO
std::cout << "found sof2" << std::endl;
#elif (defined(PRINT_JLINK_DEBUG_INFO))
JLinkWriteString(0, "found sof2\n");
#endif
}
else
{
decode_state = WAIT_FOR_SOF1;
#ifdef PRINT_CPP_DEBUG_INFO
std::cout << "failed to find sof2" << std::endl;
#elif (defined(PRINT_JLINK_DEBUG_INFO))
JLinkWriteString(0, "failed to find sof2\n");
#endif
}
break;
}
case WAIT_FOR_FRAME_LEN:
{
uart_parsing_data.frame_len = c;
decode_state = WAIT_FOR_FRAME_TYPE;
#ifdef PRINT_CPP_DEBUG_INFO
std::cout << "frame len: " << std::hex << static_cast<int>(frame_len) << std::dec << std::endl;
#elif (defined(PRINT_JLINK_DEBUG_INFO))
JLinkRTTPrintf(0, "frame len: %d\n", frame_len);
#endif
break;
}
case WAIT_FOR_FRAME_TYPE:
{
switch (c)
{
case FRAME_TYPE_CONTROL:
{
uart_parsing_data.frame_type = FRAME_TYPE_CONTROL;
decode_state = WAIT_FOR_FRAME_ID;
#ifdef PRINT_CPP_DEBUG_INFO
std::cout << "control type frame received" << std::endl;
#elif (defined(PRINT_JLINK_DEBUG_INFO))
JLinkWriteString(0, "control type frame received\n");
#endif
break;
}
case FRAME_TYPE_STATUS:
{
uart_parsing_data.frame_type = FRAME_TYPE_STATUS;
decode_state = WAIT_FOR_FRAME_ID;
#ifdef PRINT_CPP_DEBUG_INFO
std::cout << "status type frame received" << std::endl;
#elif (defined(PRINT_JLINK_DEBUG_INFO))
JLinkWriteString(0, "status type frame received\n");
#endif
break;
}
default:
{
#ifdef PRINT_CPP_DEBUG_INFO
std::cerr << "ERROR: Not expecting frame of a type other than FRAME_TYPE_STATUS" << std::endl;
#elif (defined(PRINT_JLINK_DEBUG_INFO))
JLinkWriteString(0, "ERROR: Not expecting frame of a type other than FRAME_TYPE_STATUS\n");
#endif
decode_state = WAIT_FOR_SOF1;
}
}
break;
}
case WAIT_FOR_FRAME_ID:
{
switch (c)
{
case UART_FRAME_SYSTEM_STATUS_ID:
case UART_FRAME_MOTION_STATUS_ID:
case UART_FRAME_MOTOR1_DRIVER_STATUS_ID:
case UART_FRAME_MOTOR2_DRIVER_STATUS_ID:
case UART_FRAME_MOTOR3_DRIVER_STATUS_ID:
case UART_FRAME_MOTOR4_DRIVER_STATUS_ID:
case UART_FRAME_LIGHT_STATUS_ID:
{
uart_parsing_data.frame_id = c;
decode_state = WAIT_FOR_PAYLOAD;
#ifdef PRINT_CPP_DEBUG_INFO
std::cout << "frame id: " << std::hex << static_cast<int>(frame_id) << std::dec << std::endl;
#elif (defined(PRINT_JLINK_DEBUG_INFO))
JLinkRTTPrintf(0, "frame id: %d\n", frame_id);
#endif
break;
}
default:
{
#ifdef PRINT_CPP_DEBUG_INFO
std::cerr << "ERROR: Unknown frame id" << std::endl;
#elif (defined(PRINT_JLINK_DEBUG_INFO))
JLinkWriteString(0, "ERROR: Unknown frame id\n");
#endif
decode_state = WAIT_FOR_SOF1;
}
}
break;
}
case WAIT_FOR_PAYLOAD:
{
uart_parsing_data.payload_buffer[uart_parsing_data.payload_data_pos++] = c;
#ifdef PRINT_CPP_DEBUG_INFO
std::cout << "1 byte added: " << std::hex << static_cast<int>(c) << std::dec << std::endl;
#elif (defined(PRINT_JLINK_DEBUG_INFO))
JLinkRTTPrintf(0, "1 byte added: %d\n", c);
#endif
if (uart_parsing_data.payload_data_pos == (uart_parsing_data.frame_len - FRAME_FIXED_FIELD_LEN))
decode_state = WAIT_FOR_FRAME_COUNT;
break;
}
case WAIT_FOR_FRAME_COUNT:
{
uart_parsing_data.frame_cnt = c;
decode_state = WAIT_FOR_CHECKSUM;
#ifdef PRINT_CPP_DEBUG_INFO
std::cout << "frame count: " << std::hex << static_cast<int>(frame_cnt) << std::dec << std::endl;
#elif (defined(PRINT_JLINK_DEBUG_INFO))
JLinkRTTPrintf(0, "frame count: %d\n", frame_cnt);
#endif
break;
}
case WAIT_FOR_CHECKSUM:
{
uart_parsing_data.frame_checksum = c;
uart_parsing_data.internal_checksum = CalcBufferedFrameChecksum();
new_frame_parsed = true;
decode_state = WAIT_FOR_SOF1;
#ifdef PRINT_CPP_DEBUG_INFO
std::cout << "--- frame checksum: " << std::hex << static_cast<int>(frame_checksum) << std::dec << std::endl;
std::cout << "--- internal frame checksum: " << std::hex << static_cast<int>(internal_checksum) << std::dec << std::endl;
#elif (defined(PRINT_JLINK_DEBUG_INFO))
JLinkRTTPrintf(0, "--- frame checksum: : %d\n", frame_checksum);
JLinkRTTPrintf(0, "--- internal frame checksum: : %d\n", internal_checksum);
#endif
break;
}
default:
break;
}
if (new_frame_parsed)
{
if (uart_parsing_data.frame_checksum == uart_parsing_data.internal_checksum)
{
#ifdef PRINT_CPP_DEBUG_INFO
std::cout << "checksum correct" << std::endl;
#elif (defined(PRINT_JLINK_DEBUG_INFO))
JLinkWriteString(0, "checksum correct\n");
#endif
if (uart_parsing_data.frame_type == FRAME_TYPE_STATUS)
ConstructStatusMessage(msg);
else if (uart_parsing_data.frame_type == FRAME_TYPE_CONTROL)
ConstructControlMessage(msg);
++uart_parsing_stats.frame_parsed;
}
else
{
++uart_parsing_stats.frame_with_wrong_checksum;
#ifdef PRINT_CPP_DEBUG_INFO
std::cout << "checksum is NOT correct" << std::endl;
std::cout << std::hex << static_cast<int>(frame_id) << " , " << static_cast<int>(frame_len) << " , " << static_cast<int>(frame_cnt) << " , " << static_cast<int>(frame_checksum) << " : " << std::dec << std::endl;
std::cout << "payload: ";
for (int i = 0; i < payload_data_pos; ++i)
std::cout << std::hex << static_cast<int>(payload_buffer[i]) << std::dec << " ";
std::cout << std::endl;
std::cout << "--- frame checksum: " << std::hex << static_cast<int>(frame_checksum) << std::dec << std::endl;
std::cout << "--- internal frame checksum: " << std::hex << static_cast<int>(internal_checksum) << std::dec << std::endl;
#elif (defined(PRINT_JLINK_DEBUG_INFO))
JLinkWriteString(0, "checksum is NOT correct\n");
#endif
}
}
return new_frame_parsed;
}
bool ConstructControlMessage(ScoutMessage *msg)
{
if (msg == NULL)
return false;
switch (uart_parsing_data.frame_id)
{
case UART_FRAME_MOTION_CONTROL_ID:
{
msg->type = ScoutMotionControlMsg;
msg->body.motion_control_msg.data.cmd.control_mode = uart_parsing_data.payload_buffer[0];
msg->body.motion_control_msg.data.cmd.fault_clear_flag = uart_parsing_data.payload_buffer[1];
msg->body.motion_control_msg.data.cmd.linear_velocity_cmd = uart_parsing_data.payload_buffer[2];
msg->body.motion_control_msg.data.cmd.angular_velocity_cmd = uart_parsing_data.payload_buffer[3];
msg->body.motion_control_msg.data.cmd.reserved0 = uart_parsing_data.payload_buffer[4];
msg->body.motion_control_msg.data.cmd.reserved1 = uart_parsing_data.payload_buffer[5];
msg->body.motion_control_msg.data.cmd.count = uart_parsing_data.frame_cnt;
msg->body.motion_control_msg.data.cmd.checksum = uart_parsing_data.frame_checksum;
break;
}
case UART_FRAME_LIGHT_CONTROL_ID:
{
msg->type = ScoutLightControlMsg;
msg->body.light_control_msg.data.cmd.light_ctrl_enable = uart_parsing_data.payload_buffer[0];
msg->body.light_control_msg.data.cmd.front_mode = uart_parsing_data.payload_buffer[1];
msg->body.light_control_msg.data.cmd.front_custom = uart_parsing_data.payload_buffer[2];
msg->body.light_control_msg.data.cmd.rear_mode = uart_parsing_data.payload_buffer[3];
msg->body.light_control_msg.data.cmd.rear_custom = uart_parsing_data.payload_buffer[4];
msg->body.light_control_msg.data.cmd.reserved0 = uart_parsing_data.payload_buffer[5];
msg->body.light_control_msg.data.cmd.count = uart_parsing_data.frame_cnt;
msg->body.light_control_msg.data.cmd.checksum = uart_parsing_data.frame_checksum;
break;
}
}
return true;
}
bool ConstructStatusMessage(ScoutMessage *msg)
{
if (msg == NULL)
return false;
switch (uart_parsing_data.frame_id)
{
case UART_FRAME_SYSTEM_STATUS_ID:
{
msg->type = ScoutSystemStatusMsg;
msg->body.system_status_msg.data.status.base_state = uart_parsing_data.payload_buffer[0];
msg->body.system_status_msg.data.status.control_mode = uart_parsing_data.payload_buffer[1];
msg->body.system_status_msg.data.status.battery_voltage.high_byte = uart_parsing_data.payload_buffer[2];
msg->body.system_status_msg.data.status.battery_voltage.low_byte = uart_parsing_data.payload_buffer[3];
msg->body.system_status_msg.data.status.fault_code.high_byte = uart_parsing_data.payload_buffer[4];
msg->body.system_status_msg.data.status.fault_code.low_byte = uart_parsing_data.payload_buffer[5];
msg->body.system_status_msg.data.status.count = uart_parsing_data.frame_cnt;
msg->body.system_status_msg.data.status.checksum = uart_parsing_data.frame_checksum;
break;
}
case UART_FRAME_MOTION_STATUS_ID:
{
msg->type = ScoutMotionStatusMsg;
msg->body.motion_status_msg.data.status.linear_velocity.high_byte = uart_parsing_data.payload_buffer[0];
msg->body.motion_status_msg.data.status.linear_velocity.low_byte = uart_parsing_data.payload_buffer[1];
msg->body.motion_status_msg.data.status.angular_velocity.high_byte = uart_parsing_data.payload_buffer[2];
msg->body.motion_status_msg.data.status.angular_velocity.low_byte = uart_parsing_data.payload_buffer[3];
msg->body.motion_status_msg.data.status.reserved0 = 0x00;
msg->body.motion_status_msg.data.status.reserved0 = 0x00;
msg->body.motion_status_msg.data.status.count = uart_parsing_data.frame_cnt;
msg->body.motion_status_msg.data.status.checksum = uart_parsing_data.frame_checksum;
break;
}
case UART_FRAME_MOTOR1_DRIVER_STATUS_ID:
{
msg->type = ScoutMotorDriverStatusMsg;
msg->body.motor_driver_status_msg.motor_id = SCOUT_MOTOR1_ID;
msg->body.motor_driver_status_msg.data.status.current.high_byte = uart_parsing_data.payload_buffer[0];
msg->body.motor_driver_status_msg.data.status.current.low_byte = uart_parsing_data.payload_buffer[1];
msg->body.motor_driver_status_msg.data.status.rpm.high_byte = uart_parsing_data.payload_buffer[2];
msg->body.motor_driver_status_msg.data.status.rpm.low_byte = uart_parsing_data.payload_buffer[3];
msg->body.motor_driver_status_msg.data.status.temperature = uart_parsing_data.payload_buffer[4];
msg->body.motor_driver_status_msg.data.status.reserved0 = 0x00;
msg->body.motor_driver_status_msg.data.status.count = uart_parsing_data.frame_cnt;
msg->body.motor_driver_status_msg.data.status.checksum = uart_parsing_data.frame_checksum;
break;
}
case UART_FRAME_MOTOR2_DRIVER_STATUS_ID:
{
msg->type = ScoutMotorDriverStatusMsg;
msg->body.motor_driver_status_msg.motor_id = SCOUT_MOTOR2_ID;
msg->body.motor_driver_status_msg.data.status.current.high_byte = uart_parsing_data.payload_buffer[0];
msg->body.motor_driver_status_msg.data.status.current.low_byte = uart_parsing_data.payload_buffer[1];
msg->body.motor_driver_status_msg.data.status.rpm.high_byte = uart_parsing_data.payload_buffer[2];
msg->body.motor_driver_status_msg.data.status.rpm.low_byte = uart_parsing_data.payload_buffer[3];
msg->body.motor_driver_status_msg.data.status.temperature = uart_parsing_data.payload_buffer[4];
msg->body.motor_driver_status_msg.data.status.reserved0 = 0x00;
msg->body.motor_driver_status_msg.data.status.count = uart_parsing_data.frame_cnt;
msg->body.motor_driver_status_msg.data.status.checksum = uart_parsing_data.frame_checksum;
break;
}
case UART_FRAME_MOTOR3_DRIVER_STATUS_ID:
{
msg->type = ScoutMotorDriverStatusMsg;
msg->body.motor_driver_status_msg.motor_id = SCOUT_MOTOR3_ID;
msg->body.motor_driver_status_msg.data.status.current.high_byte = uart_parsing_data.payload_buffer[0];
msg->body.motor_driver_status_msg.data.status.current.low_byte = uart_parsing_data.payload_buffer[1];
msg->body.motor_driver_status_msg.data.status.rpm.high_byte = uart_parsing_data.payload_buffer[2];
msg->body.motor_driver_status_msg.data.status.rpm.low_byte = uart_parsing_data.payload_buffer[3];
msg->body.motor_driver_status_msg.data.status.temperature = uart_parsing_data.payload_buffer[4];
msg->body.motor_driver_status_msg.data.status.reserved0 = 0x00;
msg->body.motor_driver_status_msg.data.status.count = uart_parsing_data.frame_cnt;
msg->body.motor_driver_status_msg.data.status.checksum = uart_parsing_data.frame_checksum;
break;
}
case UART_FRAME_MOTOR4_DRIVER_STATUS_ID:
{
msg->type = ScoutMotorDriverStatusMsg;
msg->body.motor_driver_status_msg.motor_id = SCOUT_MOTOR4_ID;
msg->body.motor_driver_status_msg.data.status.current.high_byte = uart_parsing_data.payload_buffer[0];
msg->body.motor_driver_status_msg.data.status.current.low_byte = uart_parsing_data.payload_buffer[1];
msg->body.motor_driver_status_msg.data.status.rpm.high_byte = uart_parsing_data.payload_buffer[2];
msg->body.motor_driver_status_msg.data.status.rpm.low_byte = uart_parsing_data.payload_buffer[3];
msg->body.motor_driver_status_msg.data.status.temperature = uart_parsing_data.payload_buffer[4];
msg->body.motor_driver_status_msg.data.status.reserved0 = 0x00;
msg->body.motor_driver_status_msg.data.status.count = uart_parsing_data.frame_cnt;
msg->body.motor_driver_status_msg.data.status.checksum = uart_parsing_data.frame_checksum;
break;
}
case UART_FRAME_LIGHT_STATUS_ID:
{
msg->type = ScoutLightStatusMsg;
msg->body.light_status_msg.data.status.light_ctrl_enable = uart_parsing_data.payload_buffer[0];
msg->body.light_status_msg.data.status.front_mode = uart_parsing_data.payload_buffer[1];
msg->body.light_status_msg.data.status.front_custom = uart_parsing_data.payload_buffer[2];
msg->body.light_status_msg.data.status.rear_mode = uart_parsing_data.payload_buffer[3];
msg->body.light_status_msg.data.status.rear_custom = uart_parsing_data.payload_buffer[4];
msg->body.light_status_msg.data.status.reserved0 = 0x00;
msg->body.light_status_msg.data.status.count = uart_parsing_data.frame_cnt;
msg->body.light_status_msg.data.status.checksum = uart_parsing_data.frame_checksum;
break;
}
}
return true;
}
uint8_t CalcScoutUARTChecksum(uint8_t *buf, uint8_t len)
{
uint8_t checksum = 0;
#ifdef USE_XOR_CHECKSUM
for (int i = 0; i < len; ++i)
checksum ^= buf[i];
#else
for (int i = 0; i < len; ++i)
checksum += buf[i];
#endif
return checksum;
}
uint8_t CalcBufferedFrameChecksum()
{
uint8_t checksum = 0x00;
#ifdef USE_XOR_CHECKSUM
checksum ^= FRAME_SOF1;
checksum ^= FRAME_SOF2;
checksum ^= uart_parsing_data.frame_len;
checksum ^= uart_parsing_data.frame_type;
checksum ^= uart_parsing_data.frame_id;
for (size_t i = 0; i < uart_parsing_data.payload_data_pos; ++i)
checksum ^= uart_parsing_data.payload_buffer[i];
checksum ^= uart_parsing_data.frame_cnt;
#else
checksum += FRAME_SOF1;
checksum += FRAME_SOF2;
checksum += uart_parsing_data.frame_len;
checksum += uart_parsing_data.frame_type;
checksum += uart_parsing_data.frame_id;
for (size_t i = 0; i < uart_parsing_data.payload_data_pos; ++i)
checksum += uart_parsing_data.payload_buffer[i];
checksum += uart_parsing_data.frame_cnt;
#endif
return checksum;
}

32
src/protocol_v1/scout/scout_uart_parser.h
View File

@@ -1,32 +0,0 @@
/*
* scout_uart_parser.h
*
* Created on: Aug 14, 2019 12:01
* Description:
*
* Copyright (c) 2019 Ruixiang Du (rdu)
*/
#ifndef SCOUT_UART_PARSER_H
#define SCOUT_UART_PARSER_H
#ifdef __cplusplus
extern "C" {
#endif
#include <stdint.h>
#include <string.h>
#include <stdbool.h>
#include "protocol_v1/scout/scout_protocol.h"
bool DecodeScoutMsgFromUART(uint8_t c, ScoutMessage *msg);
void EncodeScoutMsgToUART(const ScoutMessage *msg, uint8_t *buf, uint8_t *len);
uint8_t CalcScoutUARTChecksum(uint8_t *buf, uint8_t len);
#ifdef __cplusplus
}
#endif
#endif /* SCOUT_UART_PARSER_H */

4
src/protocol_v2/agx_msg_parser.c → src/protocol_v2/agilex_msg_parser_v2.c
View File

@@ -7,8 +7,8 @@
* Copyright (c) 2019 Weston Robot Pte. Ltd. * Copyright (c) 2019 Weston Robot Pte. Ltd.
*/ */
#include "agx_protocol_v2.h" #include "agilex_protocol_v2.h"
#include "protocol_v2/agilex_msg_parser.h" #include "protocol_v2/agilex_msg_parser_v2.h"
#include "stdio.h" #include "stdio.h"
#include "string.h" #include "string.h"

0
src/protocol_v2/agilex_msg_parser.h → src/protocol_v2/agilex_msg_parser_v2.h
View File

8
src/protocol_v2/agx_protocol_v2.h → src/protocol_v2/agilex_protocol_v2.h
View File

@@ -1,5 +1,5 @@
/* /*
* agx_protocol_v2.h * agilex_protocol_v2.h
* *
* Created on: Nov 04, 2020 13:54 * Created on: Nov 04, 2020 13:54
* Description: * Description:
@@ -7,8 +7,8 @@
* Copyright (c) 2020 Weston Robot Pte. Ltd. * Copyright (c) 2020 Weston Robot Pte. Ltd.
*/ */
#ifndef AGX_PROTOCOL_V2_H #ifndef AGILEX_PROTOCOL_V2_H
#define AGX_PROTOCOL_V2_H #define AGILEX_PROTOCOL_V2_H
#ifdef __cplusplus #ifdef __cplusplus
extern "C" { extern "C" {
@@ -399,4 +399,4 @@ typedef struct {
} }
#endif #endif
#endif /* AGX_PROTOCOL_V2_H */ #endif /* AGILEX_PROTOCOL_V2_H */

2
src/protocol_v2/protocol_v2_parser.cpp → src/protocol_v2/parser_impl/protocol_v2_parser.cpp
View File

@@ -8,7 +8,7 @@
*/ */
#include "ugv_sdk/details/protocol_v2/protocol_v2_parser.hpp" #include "ugv_sdk/details/protocol_v2/protocol_v2_parser.hpp"
#include "protocol_v2/agilex_msg_parser.h" #include "protocol_v2/agilex_msg_parser_v2.h"
namespace westonrobot { namespace westonrobot {
bool ProtocolV2Parser::DecodeMessage(const struct can_frame *rx_frame, bool ProtocolV2Parser::DecodeMessage(const struct can_frame *rx_frame,