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 proto v2

Browse Source
This commit is contained in:
Ruixiang Du
2020-11-04 18:51:21 +08:00
parent d484c75aaa
commit 057696d207
11 changed files with 1290 additions and 858 deletions
Download Patch File Download Diff File Expand all files Collapse all files

11
README.md
View File

@@ -25,6 +25,17 @@ Supported environments
It should also work in other similar Linux environments but only the above listed environments are regularly tested.
Communication protocol
| Robot | Protocol Version |
| :----: | :--------------: |
| Scout | V1, V2 |
| Hunter | V1, V2 |
| Tracer | V2 |
| Bunker | V1 |
Currently we're transitioning the communication protocol from version 1 to version 2. Upgrading protocol won't affect your ROS packages but you will need to make sure the robot is running a compatible firmware.
Generally, you only need to instantiate an object of the robot base class (such as ScoutBase), then use the object to programmatically control the robot. Internally, the base class manages two background threads, one to process CAN/UART messages of the robot state and accordingly update state variables in the robot state data structure, and the other to maintain a 50Hz loop and send the latest command to the robot base. User can iteratively perform tasks in the main thread and check the robot state or set control commands. Advanced users may also use this setup as a reference and maintain the control and monitoring loops in a different way.
## Build SDK

6
ugv_sdk/CMakeLists.txt
View File

@@ -83,6 +83,8 @@ find_package(Threads REQUIRED)
add_library(${PROJECT_NAME}
src/mobile_base.cpp
src/agx_msg_parser.c
# robots
src/hunter_base.cpp
src/hunter_can_parser.c
src/scout_base.cpp
@@ -91,8 +93,8 @@ add_library(${PROJECT_NAME}
src/tracer_base.cpp
src/tracer_can_parser.c
src/tracer_uart_parser.c
src/bunker_base.cpp
src/bunker_can_parser.c
# src/bunker_base.cpp
# src/bunker_can_parser.c
)
if(BUILD_WITHOUT_ROS)
target_link_libraries(${PROJECT_NAME} wrp_io Threads::Threads)

67
ugv_sdk/include/ugv_sdk/bunker/bunker_base.hpp
View File

@@ -1,67 +0,0 @@
/*
* bunker_base.hpp
*
* Created on: Jun 04, 2019 01:22
* Description:
*
* Copyright (c) 2019 Ruixiang Du (rdu)
*/
#ifndef BUNKER_BASE_HPP
#define BUNKER_BASE_HPP
#include <string>
#include <cstdint>
#include <thread>
#include <mutex>
#include <functional>
#include "ugv_sdk/mobile_base.hpp"
#include "ugv_sdk/bunker/bunker_protocol.h"
#include "ugv_sdk/bunker/bunker_can_parser.h"
#include "ugv_sdk/bunker/bunker_types.hpp"
namespace westonrobot {
class BunkerBase : public MobileBase {
public:
BunkerBase() : MobileBase(){};
~BunkerBase() = default;
public:
// motion control
void SetMotionCommand(double linear_vel, double angular_vel,
BunkerMotionCmd::FaultClearFlag fault_clr_flag =
BunkerMotionCmd::FaultClearFlag::NO_FAULT);
// get robot state
BunkerState GetBunkerState();
private:
// cmd/status update related variables
std::mutex bunker_state_mutex_;
std::mutex motion_cmd_mutex_;
BunkerState bunker_state_;
BunkerMotionCmd current_motion_cmd_;
// internal functions
void SendRobotCmd() override;
void ParseCANFrame(can_frame *rx_frame) override;
void ParseUARTBuffer(uint8_t *buf, const size_t bufsize,
size_t bytes_received) override{};
void SendMotionCmd(uint8_t count);
void NewStatusMsgReceivedCallback(const BunkerMessage &msg);
public:
static void UpdateBunkerState(const BunkerMessage &status_msg,
BunkerState &state);
};
} // namespace westonrobot
#endif /* BUNKER_BASE_HPP */

42
ugv_sdk/include/ugv_sdk/bunker/bunker_can_parser.h
View File

@@ -1,42 +0,0 @@
/*
* bunker_can_parser.h
*
* Created on: Aug 31, 2019 04:23
* Description:
*
* Copyright (c) 2019 Ruixiang Du (rdu)
*/
#ifndef BUNKER_CAN_PARSER_H
#define BUNKER_CAN_PARSER_H
#ifdef __cplusplus
extern "C" {
#endif
#include <stdint.h>
#include <stdbool.h>
#include "ugv_sdk/bunker/bunker_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 DecodeBunkerMsgFromCAN(const struct can_frame *rx_frame, BunkerMessage *msg);
void EncodeBunkerMsgToCAN(const BunkerMessage *msg, struct can_frame *tx_frame);
uint8_t CalcBunkerCANChecksum(uint16_t id, uint8_t *data, uint8_t dlc);
#ifdef __cplusplus
}
#endif
#endif /* BUNKER_CAN_PARSER_H */

274
ugv_sdk/include/ugv_sdk/bunker/bunker_protocol.h
View File

@@ -1,274 +0,0 @@
/*
* bunker_protocol.h
*
* Created on: Aug 07, 2019 21:49
* Description:
*
* Copyright (c) 2019 Ruixiang Du (rdu)
*/
#ifndef BUNKER_PROTOCOL_H
#define BUNKER_PROTOCOL_H
#ifdef __cplusplus
extern "C" {
#endif
#include <stdint.h>
#define BUNKER_CMD_BUF_LEN 32
#define BUNKER_STATUS_BUF_LEN 32
#define BUNKER_FRAME_SIZE 13
#define BUNKER_MOTOR1_ID ((uint8_t)0x00)
#define BUNKER_MOTOR2_ID ((uint8_t)0x01)
//#define BUNKER_MOTOR3_ID ((uint8_t)0x02)
//#define BUNKER_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_light_mode;
uint8_t front_light_custom;
uint8_t rear_light_mode;
uint8_t rear_light_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_light_mode;
uint8_t front_light_custom;
uint8_t rear_light_mode;
uint8_t rear_light_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
{
BunkerMsgNone = 0x00,
// status messages
BunkerMotionStatusMsg = 0x01,
BunkerLightStatusMsg = 0x02,
BunkerSystemStatusMsg = 0x03,
BunkerMotorDriverStatusMsg = 0x04,
// control messages
BunkerMotionControlMsg = 0x21,
BunkerLightControlMsg = 0x22
} BunkerMsgType;
typedef struct
{
BunkerMsgType 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;
} BunkerMessage;
#pragma pack(pop)
#ifdef __cplusplus
}
#endif
#endif /* BUNKER_PROTOCOL_H */

112
ugv_sdk/include/ugv_sdk/bunker/bunker_types.hpp
View File

@@ -1,112 +0,0 @@
/*
* bunker_state.hpp
*
* Created on: Jun 11, 2019 08:48
* Description:
*
* Copyright (c) 2019 Ruixiang Du (rdu)
*/
#ifndef BUNKER_STATE_HPP
#define BUNKER_STATE_HPP
#include <cstdint>
#include <iostream>
namespace westonrobot
{
struct BunkerState
{
enum MotorID
{
FRONT_RIGHT = 0,
FRONT_LEFT = 1,
REAR_LEFT = 2,
REAR_RIGHT = 3
};
struct MotorState
{
double current = 0; // in A
double rpm = 0;
double temperature = 0;
};
struct LightState
{
uint8_t mode = 0;
uint8_t custom_value = 0;
};
// base state
uint8_t base_state = 0;
uint8_t control_mode = 0;
uint16_t fault_code = 0;
double battery_voltage = 0.0;
// motor state
static constexpr uint8_t motor_num = 4;
MotorState motor_states[motor_num];
// light state
bool light_control_enabled = false;
LightState front_light_state;
LightState rear_light_state;
// motion state
double linear_velocity = 0;
double angular_velocity = 0;
};
struct BunkerMotionCmd
{
enum class FaultClearFlag
{
NO_FAULT = 0x00,
BAT_UNDER_VOL = 0x01,
BAT_OVER_VOL = 0x02,
MOTOR1_COMM = 0x03,
MOTOR2_COMM = 0x04,
MOTOR3_COMM = 0x05,
MOTOR4_COMM = 0x06,
MOTOR_DRV_OVERHEAT = 0x07,
MOTOR_OVERCURRENT = 0x08
};
BunkerMotionCmd(int8_t linear = 0, int8_t angular = 0,
FaultClearFlag fault_clr_flag = FaultClearFlag::NO_FAULT)
: linear_velocity(linear), angular_velocity(angular),
fault_clear_flag(fault_clr_flag) {}
int8_t linear_velocity;
int8_t angular_velocity;
FaultClearFlag fault_clear_flag;
static constexpr double max_linear_velocity = 1.5; // 1.5 m/s
static constexpr double min_linear_velocity = -1.5; // -1.5 m/s
static constexpr double max_angular_velocity = 1.0; // 3.1415 rad/s
static constexpr double min_angular_velocity = -1.0; // -3.1415 rad/s
};
struct BunkerLightCmd
{
enum class LightMode
{
CONST_OFF = 0x00,
CONST_ON = 0x01,
BREATH = 0x02,
CUSTOM = 0x03
};
BunkerLightCmd() = default;
BunkerLightCmd(LightMode f_mode, uint8_t f_value, LightMode r_mode, uint8_t r_value) : front_mode(f_mode), front_custom_value(f_value),
rear_mode(r_mode), rear_custom_value(r_value) {}
LightMode front_mode;
uint8_t front_custom_value;
LightMode rear_mode;
uint8_t rear_custom_value;
};
} // namespace westonrobot
#endif /* BUNKER_STATE_HPP */

45
ugv_sdk/include/ugv_sdk/proto/agx_msg_parser.h Normal file
View File

@@ -0,0 +1,45 @@
/*
* agx_msg_parser.h
*
* Created on: Nov 04, 2020 13:40
* Description:
*
* Copyright (c) 2020 Ruixiang Du (rdu)
*/
#ifndef AGX_MSG_PARSER_H
#define AGX_MSG_PARSER_H
// #ifdef __cplusplus
// extern "C" {
// #endif
#include <stdint.h>
#include <stdbool.h>
#include <string.h>
#include "ugv_sdk/proto/agx_protocol_v2.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, AgxMessage *msg);
void EncodeScoutMsgToCAN(const AgxMessage *msg, struct can_frame *tx_frame);
uint8_t CalcScoutCANChecksum(uint16_t id, uint8_t *data, uint8_t dlc);
bool DecodeScoutMsgFromUART(uint8_t c, AgxMessage *msg);
void EncodeScoutMsgToUART(const AgxMessage *msg, uint8_t *buf, uint8_t *len);
uint8_t CalcScoutUARTChecksum(uint8_t *buf, uint8_t len);
// #ifdef __cplusplus
// }
// #endif
#endif /* AGX_PARSER_H */

353
ugv_sdk/include/ugv_sdk/proto/agx_protocol_v2.h Normal file
View File

@@ -0,0 +1,353 @@
/*
* agx_protocol_v2.h
*
* Created on: Nov 04, 2020 13:54
* Description:
*
* Copyright (c) 2020 Ruixiang Du (rdu)
*/
#ifndef AGX_PROTOCOL_V2_H
#define AGX_PROTOCOL_V2_H
// #ifdef __cplusplus
// extern "C" {
// #endif
#include <stdint.h>
#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)0x111)
#define CAN_MSG_MOTION_CONTROL_STATUS_ID ((uint32_t)0x221)
#define CAN_MSG_SELECT_CONTROL_MODE_ID ((uint32_t)0x421)
#define CAN_MSG_LIGHT_CONTROL_CMD_ID ((uint32_t)0x121)
#define CAN_MSG_LIGHT_CONTROL_STATUS_ID ((uint32_t)0x231)
#define CAN_MSG_SYSTEM_STATUS_STATUS_ID ((uint32_t)0x211)
//#define CAN_MSG_MOTOR1_DRIVER_STATUS_ID ((uint32_t)0x251)
//#define CAN_MSG_MOTOR2_DRIVER_STATUS_ID ((uint32_t)0x252)
//#define CAN_MSG_MOTOR3_DRIVER_STATUS_ID ((uint32_t)0x253)
//#define CAN_MSG_MOTOR4_DRIVER_STATUS_ID ((uint32_t)0x254)
#define CAN_MSG_MOTOR1_HEIGHT_DRIVER_STATUS_ID ((uint32_t)0x251)
#define CAN_MSG_MOTOR2_HEIGHT_DRIVER_STATUS_ID ((uint32_t)0x252)
#define CAN_MSG_MOTOR3_HEIGHT_DRIVER_STATUS_ID ((uint32_t)0x253)
#define CAN_MSG_MOTOR4_HEIGHT_DRIVER_STATUS_ID ((uint32_t)0x254)
#define CAN_MSG_MOTOR1_LOW_DRIVER_STATUS_ID ((uint32_t)0x261)
#define CAN_MSG_MOTOR2_LOW_DRIVER_STATUS_ID ((uint32_t)0x262)
#define CAN_MSG_MOTOR3_LOW_DRIVER_STATUS_ID ((uint32_t)0x263)
#define CAN_MSG_MOTOR4_LOW_DRIVER_STATUS_ID ((uint32_t)0x264)
#define CAN_MSG_ODOMETER_ID ((uint32_t)0x311)
/*--------------------- 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 {
struct {
int8_t high_byte;
int8_t low_byte;
} linear_velocity;
struct {
uint8_t high_byte;
uint8_t low_byte;
} angular_velocity;
uint8_t reserved0;
uint8_t reserved1;
uint8_t reserved2;
uint8_t reserved3;
} 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 reserved2;
uint8_t reserved3;
} cmd;
uint8_t raw[8];
} data;
} MotionStatusMessage;
typedef struct {
union {
struct {
uint8_t control_mode;
uint8_t reserved0;
uint8_t reserved1;
uint8_t reserved2;
uint8_t reserved3;
uint8_t reserved4;
uint8_t reserved5;
uint8_t reserved6;
} cmd;
uint8_t raw[8];
} data;
} ModSelectMessage;
// 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;
uint8_t fault_code;
uint8_t reserved0;
uint8_t reserved1;
uint8_t checksum;
} status;
uint8_t raw[8];
} data;
} SystemStatusMessage;
// Light Control
typedef struct {
union {
struct {
uint8_t light_ctrl_enable;
uint8_t front_light_mode;
uint8_t front_light_custom;
uint8_t rear_light_mode;
uint8_t rear_light_custom;
uint8_t reserved0;
uint8_t reserved1;
uint8_t count;
} cmd;
uint8_t raw[8];
} data;
} LightControlMessage;
typedef struct {
union {
struct {
uint8_t light_ctrl_enable;
uint8_t front_light_mode;
uint8_t front_light_custom;
uint8_t rear_light_mode;
uint8_t rear_light_custom;
uint8_t reserved0;
uint8_t reserved1;
uint8_t count;
} 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;
typedef struct {
uint8_t motor_id;
union {
struct {
struct {
uint8_t high_byte;
uint8_t low_byte;
} rpm;
struct {
uint8_t high_byte;
uint8_t low_byte;
} current;
struct {
int8_t heighest;
int8_t sec_heighest;
int8_t sec_lowest;
int8_t lowest;
} moter_pose;
} status;
uint8_t raw[8];
} data;
} MotorDriverHeightSpeedStatusMessage;
typedef struct {
uint8_t motor_id;
union {
struct {
struct {
uint8_t high_byte;
uint8_t low_byte;
} driver_voltage;
struct {
uint8_t high_byte;
uint8_t low_byte;
} driver_temperature;
int8_t motor_temperature;
uint8_t driver_state;
uint8_t reserved0;
uint8_t reserved1;
} status;
uint8_t raw[8];
} data;
} MotorDriverLowSpeedStatusMessage;
typedef struct {
uint8_t motor_id;
union {
struct {
struct {
uint8_t heighest;
uint8_t sec_heighest;
uint8_t sec_lowest;
uint8_t lowest;
} left;
struct {
uint8_t heighest;
uint8_t sec_heighest;
uint8_t sec_lowest;
uint8_t lowest;
} right;
} status;
uint8_t raw[8];
} data;
} OdomterMessage;
// For convenience to access status/control message
typedef enum {
ScoutMsgNone = 0x00,
// status messages
ScoutMotionStatusMsg = 0x01,
ScoutLightStatusMsg = 0x02,
ScoutSystemStatusMsg = 0x03,
ScoutMotorDriverStatusMsg = 0x04,
ScoutMotorDriverHeightSpeedStatusMsg = 0x05,
ScoutMotorDriverLowSpeedStatusMsg = 0x06,
ScoutodometerMsg = 0x07,
// control messages
ScoutMotionControlMsg = 0x21,
ScoutLightControlMsg = 0x22,
ScoutControlModeMsg = 0x23
} 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;
MotorDriverLowSpeedStatusMessage motor_driver_low_speed_status_msg;
MotorDriverHeightSpeedStatusMessage motor_driver_height_speed_status_msg;
OdomterMessage odom_msg;
// control messages
MotionControlMessage motion_control_msg;
LightControlMessage light_control_msg;
ModSelectMessage mode_cmd_msg;
} body;
} AgxMessage;
#pragma pack(pop)
// #ifdef __cplusplus
// }
// #endif
#endif /* AGX_PROTOCOL_V2_H */

877
ugv_sdk/src/agx_msg_parser.c Normal file
View File

@@ -0,0 +1,877 @@
/*
* agx_msg_parser.c
*
* Created on: Aug 31, 2019 04:25
* Description:
*
* Copyright (c) 2019 Ruixiang Du (rdu)
*/
#include "ugv_sdk/proto/agx_msg_parser.h"
#include "string.h"
// #define USE_XOR_CHECKSUM
// #define PRINT_CPP_DEBUG_INFO
// #define PRINT_JLINK_DEBUG_INFO
#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
} UartDecodeState;
#define UART_PAYLOAD_BUFFER_SIZE 64
#define UART_FRAME_SOF_LEN ((uint8_t)2)
#define UART_FRAME_FIXED_FIELD_LEN ((uint8_t)4)
#define UART_FRAME_SOF1 ((uint8_t)0x5a)
#define UART_FRAME_SOF2 ((uint8_t)0xa5)
#define UART_FRAME_TYPE_CONTROL ((uint8_t)0x55)
#define UART_FRAME_TYPE_STATUS ((uint8_t)0xaa)
#define UART_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[UART_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, AgxMessage *msg);
static uint8_t CalcBufferedFrameChecksum();
static bool ConstructStatusMessage(AgxMessage *msg);
static bool ConstructControlMessage(AgxMessage *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);
static void EncodeScoutMotionControlMsgToCAN(const MotionControlMessage *msg,
struct can_frame *tx_frame);
static void EncodeScoutLightControlMsgToCAN(const LightControlMessage *msg,
struct can_frame *tx_frame);
static void EncodeScoutnControlModeMsgToCAN(const ModSelectMessage *msg,
struct can_frame *tx_frame);
bool DecodeScoutMsgFromCAN(const struct can_frame *rx_frame, AgxMessage *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;
}
// 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;
}
case CAN_MSG_MOTOR1_LOW_DRIVER_STATUS_ID: {
msg->type = ScoutMotorDriverLowSpeedStatusMsg;
msg->body.motor_driver_low_speed_status_msg.motor_id = SCOUT_MOTOR1_ID;
memcpy(msg->body.motor_driver_low_speed_status_msg.data.raw,
rx_frame->data, rx_frame->can_dlc * sizeof(uint8_t));
break;
}
case CAN_MSG_MOTOR2_LOW_DRIVER_STATUS_ID: {
msg->type = ScoutMotorDriverLowSpeedStatusMsg;
msg->body.motor_driver_low_speed_status_msg.motor_id = SCOUT_MOTOR2_ID;
memcpy(msg->body.motor_driver_low_speed_status_msg.data.raw,
rx_frame->data, rx_frame->can_dlc * sizeof(uint8_t));
break;
}
case CAN_MSG_MOTOR3_LOW_DRIVER_STATUS_ID: {
msg->type = ScoutMotorDriverLowSpeedStatusMsg;
msg->body.motor_driver_low_speed_status_msg.motor_id = SCOUT_MOTOR3_ID;
memcpy(msg->body.motor_driver_low_speed_status_msg.data.raw,
rx_frame->data, rx_frame->can_dlc * sizeof(uint8_t));
break;
}
case CAN_MSG_MOTOR4_LOW_DRIVER_STATUS_ID: {
msg->type = ScoutMotorDriverLowSpeedStatusMsg;
msg->body.motor_driver_low_speed_status_msg.motor_id = SCOUT_MOTOR4_ID;
memcpy(msg->body.motor_driver_low_speed_status_msg.data.raw,
rx_frame->data, rx_frame->can_dlc * sizeof(uint8_t));
break;
}
case CAN_MSG_MOTOR1_HEIGHT_DRIVER_STATUS_ID: {
msg->type = ScoutMotorDriverHeightSpeedStatusMsg;
msg->body.motor_driver_height_speed_status_msg.motor_id = SCOUT_MOTOR1_ID;
memcpy(msg->body.motor_driver_height_speed_status_msg.data.raw,
rx_frame->data, rx_frame->can_dlc * sizeof(uint8_t));
break;
}
case CAN_MSG_MOTOR2_HEIGHT_DRIVER_STATUS_ID: {
msg->type = ScoutMotorDriverHeightSpeedStatusMsg;
msg->body.motor_driver_height_speed_status_msg.motor_id = SCOUT_MOTOR2_ID;
memcpy(msg->body.motor_driver_height_speed_status_msg.data.raw,
rx_frame->data, rx_frame->can_dlc * sizeof(uint8_t));
break;
}
case CAN_MSG_MOTOR3_HEIGHT_DRIVER_STATUS_ID: {
msg->type = ScoutMotorDriverHeightSpeedStatusMsg;
msg->body.motor_driver_height_speed_status_msg.motor_id = SCOUT_MOTOR3_ID;
memcpy(msg->body.motor_driver_height_speed_status_msg.data.raw,
rx_frame->data, rx_frame->can_dlc * sizeof(uint8_t));
break;
}
case CAN_MSG_MOTOR4_HEIGHT_DRIVER_STATUS_ID: {
msg->type = ScoutMotorDriverHeightSpeedStatusMsg;
msg->body.motor_driver_height_speed_status_msg.motor_id = SCOUT_MOTOR4_ID;
memcpy(msg->body.motor_driver_height_speed_status_msg.data.raw,
rx_frame->data, rx_frame->can_dlc * sizeof(uint8_t));
break;
}
case CAN_MSG_ODOMETER_ID: {
msg->type = ScoutodometerMsg;
// msg->light_control_msg.id = CAN_MSG_LIGHT_CONTROL_STATUS_ID;
memcpy(msg->body.odom_msg.data.raw, rx_frame->data,
rx_frame->can_dlc * sizeof(uint8_t));
// printf("%x\t",msg->body.odom_msg.data.status.left.heighest);
// printf("%x\t",msg->body.odom_msg.data.status.left.sec_heighest);
// printf("%x\t",msg->body.odom_msg.data.status.left.sec_lowest);
// printf("%x\r\n",msg->body.odom_msg.data.status.left.lowest);
// printf("%x\t",msg->body.odom_msg.data.raw);
// printf("%x\r\n",msg->body.odom_msg.data.status);
break;
}
default:
break;
}
return true;
}
void EncodeScoutMsgToCAN(const AgxMessage *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;
}
// TODO check correctiveness
// case ScoutControlModeMsg: {
// EncodeScoutnControlModeMsgToCAN(&(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 EncodeScoutnControlModeMsgToCAN(const ModSelectMessage *msg,
struct can_frame *tx_frame) {
tx_frame->can_id = CAN_MSG_SELECT_CONTROL_MODE_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;
}
void EncodeScoutMsgToUART(const AgxMessage *msg, uint8_t *buf, uint8_t *len) {
// SOF
buf[0] = UART_FRAME_SOF1;
buf[1] = UART_FRAME_SOF2;
// frame len, type, ID
buf[2] = 0x0a;
buf[3] = UART_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.cmd.linear_velocity.high_byte;
buf[6] = msg->body.motion_status_msg.data.cmd.linear_velocity.low_byte;
buf[7] = msg->body.motion_status_msg.data.cmd.angular_velocity.high_byte;
buf[8] = msg->body.motion_status_msg.data.cmd.angular_velocity.low_byte;
buf[9] = 0;
buf[10] = 0;
buf[11] = 0;
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_light_mode;
buf[7] = msg->body.light_status_msg.data.status.front_light_custom;
buf[8] = msg->body.light_status_msg.data.status.rear_light_mode;
buf[9] = msg->body.light_status_msg.data.status.rear_light_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;
buf[10] = 0;
buf[11] = 0;
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] + UART_FRAME_SOF_LEN);
// length: SOF + Frame + Checksum
*len = buf[2] + UART_FRAME_SOF_LEN + 1;
}
bool DecodeScoutMsgFromUART(uint8_t c, AgxMessage *msg) {
static AgxMessage 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] = UART_FRAME_SOF1;
buf[1] = UART_FRAME_SOF2;
// frame len, type, ID
buf[2] = 0x0a;
buf[3] = UART_FRAME_TYPE_CONTROL;
buf[4] = UART_FRAME_MOTION_CONTROL_ID;
// frame payload
buf[5] = msg->data.cmd.linear_velocity.high_byte;
buf[6] = msg->data.cmd.linear_velocity.low_byte;
buf[7] = msg->data.cmd.angular_velocity.high_byte;
buf[8] = msg->data.cmd.angular_velocity.low_byte;
buf[9] = 0x00;
buf[10] = 0x00;
// frame count, checksum
buf[11] = 0x00;
buf[12] = 0x00;
// length: SOF + Frame + Checksum
*len = buf[2] + UART_FRAME_SOF_LEN + 1;
}
void EncodeLightControlMsgToUART(const LightControlMessage *msg, uint8_t *buf,
uint8_t *len) {
// SOF
buf[0] = UART_FRAME_SOF1;
buf[1] = UART_FRAME_SOF2;
// frame len, type, ID
buf[2] = 0x0a;
buf[3] = UART_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_light_mode;
buf[7] = msg->data.cmd.front_light_custom;
buf[8] = msg->data.cmd.rear_light_mode;
buf[9] = msg->data.cmd.rear_light_custom;
buf[10] = 0x00;
// frame count, checksum
buf[11] = msg->data.cmd.count;
buf[12] = CalcScoutUARTChecksum(buf, buf[2] + UART_FRAME_SOF_LEN);
// length: SOF + Frame + Checksum
*len = buf[2] + UART_FRAME_SOF_LEN + 1;
}
bool ParseChar(uint8_t c, AgxMessage *msg) {
static UartDecodeState decode_state = WAIT_FOR_SOF1;
bool new_frame_parsed = false;
switch (decode_state) {
case WAIT_FOR_SOF1: {
if (c == UART_FRAME_SOF1) {
uart_parsing_data.frame_id = UART_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, UART_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 == UART_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 UART_FRAME_TYPE_CONTROL: {
uart_parsing_data.frame_type = UART_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 UART_FRAME_TYPE_STATUS: {
uart_parsing_data.frame_type = UART_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 "
"UART_FRAME_TYPE_STATUS"
<< std::endl;
#elif (defined(PRINT_JLINK_DEBUG_INFO))
JLinkWriteString(0,
"ERROR: Not expecting frame of a type other than "
"UART_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 - UART_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 == UART_FRAME_TYPE_STATUS)
ConstructStatusMessage(msg);
else if (uart_parsing_data.frame_type == UART_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(AgxMessage *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.linear_velocity.high_byte =
uart_parsing_data.payload_buffer[0];
msg->body.motion_control_msg.data.cmd.linear_velocity.low_byte =
uart_parsing_data.payload_buffer[1];
msg->body.motion_control_msg.data.cmd.angular_velocity.high_byte =
uart_parsing_data.payload_buffer[2];
msg->body.motion_control_msg.data.cmd.angular_velocity.low_byte =
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.reserved2 =
uart_parsing_data.payload_buffer[6];
msg->body.motion_control_msg.data.cmd.reserved3 =
uart_parsing_data.payload_buffer[7];
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_light_mode =
uart_parsing_data.payload_buffer[1];
msg->body.light_control_msg.data.cmd.front_light_custom =
uart_parsing_data.payload_buffer[2];
msg->body.light_control_msg.data.cmd.rear_light_mode =
uart_parsing_data.payload_buffer[3];
msg->body.light_control_msg.data.cmd.rear_light_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(AgxMessage *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 =
uart_parsing_data.payload_buffer[4];
msg->body.system_status_msg.data.status.reserved0 =
uart_parsing_data.payload_buffer[5];
msg->body.system_status_msg.data.status.reserved1 =
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.cmd.linear_velocity.high_byte =
uart_parsing_data.payload_buffer[0];
msg->body.motion_status_msg.data.cmd.linear_velocity.low_byte =
uart_parsing_data.payload_buffer[1];
msg->body.motion_status_msg.data.cmd.angular_velocity.high_byte =
uart_parsing_data.payload_buffer[2];
msg->body.motion_status_msg.data.cmd.angular_velocity.low_byte =
uart_parsing_data.payload_buffer[3];
msg->body.motion_status_msg.data.cmd.reserved0 = 0x00;
msg->body.motion_status_msg.data.cmd.reserved1 = 0x00;
msg->body.motion_status_msg.data.cmd.reserved2 = 0x00;
msg->body.motion_status_msg.data.cmd.reserved3 = 0x00;
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_light_mode =
uart_parsing_data.payload_buffer[1];
msg->body.light_status_msg.data.status.front_light_custom =
uart_parsing_data.payload_buffer[2];
msg->body.light_status_msg.data.status.rear_light_mode =
uart_parsing_data.payload_buffer[3];
msg->body.light_status_msg.data.status.rear_light_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.reserved1 = 0x00;
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 ^= UART_FRAME_SOF1;
checksum ^= UART_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 += UART_FRAME_SOF1;
checksum += UART_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;
}

200
ugv_sdk/src/bunker_base.cpp
View File

@@ -1,200 +0,0 @@
#include "ugv_sdk/bunker/bunker_base.hpp"
#include <string>
#include <cstring>
#include <iostream>
#include <algorithm>
#include <array>
#include <chrono>
#include <cstdint>
#include <ratio>
#include <thread>
#include "stopwatch.hpp"
namespace westonrobot {
void BunkerBase::SendRobotCmd() {
static uint8_t cmd_count = 0;
static uint8_t light_cmd_count = 0;
SendMotionCmd(cmd_count++);
}
void BunkerBase::SendMotionCmd(uint8_t count) {
// motion control message
BunkerMessage m_msg;
m_msg.type = BunkerMotionControlMsg;
if (can_connected_)
m_msg.body.motion_control_msg.data.cmd.control_mode = CTRL_MODE_CMD_CAN;
else if (serial_connected_)
m_msg.body.motion_control_msg.data.cmd.control_mode = CTRL_MODE_CMD_UART;
motion_cmd_mutex_.lock();
m_msg.body.motion_control_msg.data.cmd.fault_clear_flag =
static_cast<uint8_t>(current_motion_cmd_.fault_clear_flag);
m_msg.body.motion_control_msg.data.cmd.linear_velocity_cmd =
current_motion_cmd_.linear_velocity;
m_msg.body.motion_control_msg.data.cmd.angular_velocity_cmd =
current_motion_cmd_.angular_velocity;
motion_cmd_mutex_.unlock();
m_msg.body.motion_control_msg.data.cmd.reserved0 = 0;
m_msg.body.motion_control_msg.data.cmd.reserved1 = 0;
m_msg.body.motion_control_msg.data.cmd.count = count;
if (can_connected_)
m_msg.body.motion_control_msg.data.cmd.checksum =
CalcBunkerCANChecksum(CAN_MSG_MOTION_CONTROL_CMD_ID,
m_msg.body.motion_control_msg.data.raw, 8);
// serial_connected_: checksum will be calculated later when packed into a
// complete serial frame
if (can_connected_) {
// send to can bus
can_frame m_frame;
EncodeBunkerMsgToCAN(&m_msg, &m_frame);
can_if_->SendFrame(m_frame);
} else {
// send to serial port
// EncodeBunkerMsgToUART(&m_msg, tx_buffer_, &tx_cmd_len_);
// serial_if_->SendBytes(tx_buffer_, tx_cmd_len_);
}
}
BunkerState BunkerBase::GetBunkerState() {
std::lock_guard<std::mutex> guard(bunker_state_mutex_);
return bunker_state_;
}
void BunkerBase::SetMotionCommand(
double linear_vel, double angular_vel,
BunkerMotionCmd::FaultClearFlag fault_clr_flag) {
// make sure cmd thread is started before attempting to send commands
if (!cmd_thread_started_) StartCmdThread();
if (linear_vel < BunkerMotionCmd::min_linear_velocity)
linear_vel = BunkerMotionCmd::min_linear_velocity;
if (linear_vel > BunkerMotionCmd::max_linear_velocity)
linear_vel = BunkerMotionCmd::max_linear_velocity;
if (angular_vel < BunkerMotionCmd::min_angular_velocity)
angular_vel = BunkerMotionCmd::min_angular_velocity;
if (angular_vel > BunkerMotionCmd::max_angular_velocity)
angular_vel = BunkerMotionCmd::max_angular_velocity;
//std::cout<<angular_vel<<std::endl;
std::lock_guard<std::mutex> guard(motion_cmd_mutex_);
current_motion_cmd_.linear_velocity = static_cast<int8_t>(
linear_vel / BunkerMotionCmd::max_linear_velocity * 100.0);
current_motion_cmd_.angular_velocity = static_cast<int8_t>(
angular_vel / BunkerMotionCmd::max_angular_velocity * 100.0);
current_motion_cmd_.fault_clear_flag = fault_clr_flag;
FeedCmdTimeoutWatchdog();
}
void BunkerBase::ParseCANFrame(can_frame *rx_frame) {
// validate checksum, discard frame if fails
if (!rx_frame->data[7] == CalcBunkerCANChecksum(rx_frame->can_id,
rx_frame->data,
rx_frame->can_dlc)) {
std::cerr << "ERROR: checksum mismatch, discard frame with id "
<< rx_frame->can_id << std::endl;
return;
}
// otherwise, update robot state with new frame
BunkerMessage status_msg;
DecodeBunkerMsgFromCAN(rx_frame, &status_msg);
NewStatusMsgReceivedCallback(status_msg);
}
//void BunkerBase::ParseUARTBuffer(uint8_t *buf, const size_t bufsize,
// size_t bytes_received) {
// // std::cout << "bytes received from serial: " << bytes_received << std::endl;
// // serial_parser_.PrintStatistics();
// // serial_parser_.ParseBuffer(buf, bytes_received);
// BunkerMessage status_msg;
// for (int i = 0; i < bytes_received; ++i) {
// if (DecodeBunkerMsgFromUART(buf[i], &status_msg))
// NewStatusMsgReceivedCallback(status_msg);
// }
//}
void BunkerBase::NewStatusMsgReceivedCallback(const BunkerMessage &msg) {
// std::cout << "new status msg received" << std::endl;
std::lock_guard<std::mutex> guard(bunker_state_mutex_);
UpdateBunkerState(msg, bunker_state_);
}
void BunkerBase::UpdateBunkerState(const BunkerMessage &status_msg,
BunkerState &state) {
switch (status_msg.type) {
case BunkerMotionStatusMsg: {
// std::cout << "motion control feedback received" << std::endl;
const MotionStatusMessage &msg = status_msg.body.motion_status_msg;
state.linear_velocity =
static_cast<int16_t>(
static_cast<uint16_t>(msg.data.status.linear_velocity.low_byte) |
static_cast<uint16_t>(msg.data.status.linear_velocity.high_byte)
<< 8) /
1000.0;
state.angular_velocity =
static_cast<int16_t>(
static_cast<uint16_t>(msg.data.status.angular_velocity.low_byte) |
static_cast<uint16_t>(msg.data.status.angular_velocity.high_byte)
<< 8) /
1000.0;
break;
}
case BunkerLightStatusMsg: {
// std::cout << "light control feedback received" << std::endl;
const LightStatusMessage &msg = status_msg.body.light_status_msg;
if (msg.data.status.light_ctrl_enable == LIGHT_DISABLE_CTRL)
state.light_control_enabled = false;
else
state.light_control_enabled = true;
state.front_light_state.mode = msg.data.status.front_light_mode;
state.front_light_state.custom_value = msg.data.status.front_light_custom;
state.rear_light_state.mode = msg.data.status.rear_light_mode;
state.rear_light_state.custom_value = msg.data.status.rear_light_custom;
break;
}
case BunkerSystemStatusMsg: {
// std::cout << "system status feedback received" << std::endl;
const SystemStatusMessage &msg = status_msg.body.system_status_msg;
state.control_mode = msg.data.status.control_mode;
state.base_state = msg.data.status.base_state;
state.battery_voltage =
(static_cast<uint16_t>(msg.data.status.battery_voltage.low_byte) |
static_cast<uint16_t>(msg.data.status.battery_voltage.high_byte)
<< 8) /
10.0;
state.fault_code =
(static_cast<uint16_t>(msg.data.status.fault_code.low_byte) |
static_cast<uint16_t>(msg.data.status.fault_code.high_byte) << 8);
break;
}
case BunkerMotorDriverStatusMsg: {
// std::cout << "motor 1 driver feedback received" << std::endl;
const MotorDriverStatusMessage &msg =
status_msg.body.motor_driver_status_msg;
for (int i = 0; i < BunkerState::motor_num; ++i) {
state.motor_states[status_msg.body.motor_driver_status_msg.motor_id]
.current =
(static_cast<uint16_t>(msg.data.status.current.low_byte) |
static_cast<uint16_t>(msg.data.status.current.high_byte) << 8) /
10.0;
state.motor_states[status_msg.body.motor_driver_status_msg.motor_id]
.rpm = static_cast<int16_t>(
static_cast<uint16_t>(msg.data.status.rpm.low_byte) |
static_cast<uint16_t>(msg.data.status.rpm.high_byte) << 8);
state.motor_states[status_msg.body.motor_driver_status_msg.motor_id]
.temperature = msg.data.status.temperature;
}
break;
}
}
}
} // namespace westonrobot

161
ugv_sdk/src/bunker_can_parser.c
View File

@@ -1,161 +0,0 @@
/*
* bunker_can_parser.c
*
* Created on: Aug 31, 2019 04:25
* Description:
*
* Copyright (c) 2019 Ruixiang Du (rdu)
*/
#include "ugv_sdk/bunker/bunker_can_parser.h"
#include "string.h"
static void EncodeBunkerMotionControlMsgToCAN(const MotionControlMessage *msg, struct can_frame *tx_frame);
static void EncodeBunkerLightControlMsgToCAN(const LightControlMessage *msg, struct can_frame *tx_frame);
bool DecodeBunkerMsgFromCAN(const struct can_frame *rx_frame, BunkerMessage *msg)
{
msg->type = BunkerMsgNone;
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 = BunkerMotionStatusMsg;
// 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 = BunkerLightStatusMsg;
// 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 = BunkerSystemStatusMsg;
// 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 = BunkerMotorDriverStatusMsg;
// msg->motor_driver_status_msg.id = CAN_MSG_MOTOR1_DRIVER_STATUS_ID;
msg->body.motor_driver_status_msg.motor_id = BUNKER_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 = BunkerMotorDriverStatusMsg;
// msg->motor_driver_status_msg.id = CAN_MSG_MOTOR2_DRIVER_STATUS_ID;
msg->body.motor_driver_status_msg.motor_id = BUNKER_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_MOTION_CONTROL_CMD_ID:
{
msg->type = BunkerMotionControlMsg;
// 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 = BunkerLightControlMsg;
// 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 EncodeBunkerMsgToCAN(const BunkerMessage *msg, struct can_frame *tx_frame)
{
switch (msg->type)
{
// in the current implementation, both MsgType and can_frame include 8 * uint8_t
case BunkerMotionStatusMsg:
{
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 BunkerLightStatusMsg:
{
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 BunkerSystemStatusMsg:
{
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 BunkerMotorDriverStatusMsg:
{
if (msg->body.motor_driver_status_msg.motor_id == BUNKER_MOTOR1_ID)
tx_frame->can_id = CAN_MSG_MOTOR1_DRIVER_STATUS_ID;
else if (msg->body.motor_driver_status_msg.motor_id == BUNKER_MOTOR2_ID)
tx_frame->can_id = CAN_MSG_MOTOR2_DRIVER_STATUS_ID;
// else if (msg->body.motor_driver_status_msg.motor_id == BUNKER_MOTOR3_ID)
// tx_frame->can_id = CAN_MSG_MOTOR3_DRIVER_STATUS_ID;
// else if (msg->body.motor_driver_status_msg.motor_id == BUNKER_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 BunkerMotionControlMsg:
{
EncodeBunkerMotionControlMsgToCAN(&(msg->body.motion_control_msg), tx_frame);
break;
}
case BunkerLightControlMsg:
{
EncodeBunkerLightControlMsgToCAN(&(msg->body.light_control_msg), tx_frame);
break;
}
default:
break;
}
tx_frame->data[7] = CalcBunkerCANChecksum(tx_frame->can_id, tx_frame->data, tx_frame->can_dlc);
}
void EncodeBunkerMotionControlMsgToCAN(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] = CalcBunkerCANChecksum(tx_frame->can_id, tx_frame->data, tx_frame->can_dlc);
}
void EncodeBunkerLightControlMsgToCAN(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] = CalcBunkerCANChecksum(tx_frame->can_id, tx_frame->data, tx_frame->can_dlc);
}
uint8_t CalcBunkerCANChecksum(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;
}