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

V2.x (#5)

Browse Source 
* added support of scout and hunter for protocol v2.0
This commit is contained in:
agilexrobotics
2020-11-03 16:32:51 +08:00
committed by GitHub
parent f2fa06cd4c
commit 4d4b5baf73
29 changed files with 2980 additions and 626 deletions
Download Patch File Download Diff File Expand all files Collapse all files

5
ugv_sdk/CMakeLists.txt
View File

@@ -88,6 +88,11 @@ add_library(${PROJECT_NAME}
src/scout_base.cpp
src/scout_can_parser.c
src/scout_uart_parser.c
src/tracer_base.cpp
src/tracer_can_parser.c
src/tracer_uart_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 Normal file
View File

@@ -0,0 +1,67 @@
/*
* 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 Normal file
View File

@@ -0,0 +1,42 @@
/*
* 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 */

47
ugv_sdk/include/ugv_sdk/scout/scout_protocol.h → ugv_sdk/include/ugv_sdk/bunker/bunker_protocol.h
View File

@@ -1,14 +1,13 @@
/*
* scout_protocol.h
* bunker_protocol.h
*
* Created on: Aug 07, 2019 21:49
* Description:
*
* Copyright (c) 2019 Ruixiang Du (rdu)
*/
#ifndef SCOUT_PROTOCOL_H
#define SCOUT_PROTOCOL_H
#ifndef BUNKER_PROTOCOL_H
#define BUNKER_PROTOCOL_H
#ifdef __cplusplus
extern "C" {
@@ -16,14 +15,14 @@ extern "C" {
#include <stdint.h>
#define SCOUT_CMD_BUF_LEN 32
#define SCOUT_STATUS_BUF_LEN 32
#define SCOUT_FRAME_SIZE 13
#define BUNKER_CMD_BUF_LEN 32
#define BUNKER_STATUS_BUF_LEN 32
#define BUNKER_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)
#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)
@@ -45,8 +44,8 @@ extern "C" {
#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)
//#define CAN_MSG_MOTOR3_DRIVER_STATUS_ID ((uint32_t)0x202)
//#define CAN_MSG_MOTOR4_DRIVER_STATUS_ID ((uint32_t)0x203)
/*--------------------- Control/State Constants ------------------------*/
@@ -240,20 +239,20 @@ typedef struct
// For convenience to access status/control message
typedef enum
{
ScoutMsgNone = 0x00,
BunkerMsgNone = 0x00,
// status messages
ScoutMotionStatusMsg = 0x01,
ScoutLightStatusMsg = 0x02,
ScoutSystemStatusMsg = 0x03,
ScoutMotorDriverStatusMsg = 0x04,
BunkerMotionStatusMsg = 0x01,
BunkerLightStatusMsg = 0x02,
BunkerSystemStatusMsg = 0x03,
BunkerMotorDriverStatusMsg = 0x04,
// control messages
ScoutMotionControlMsg = 0x21,
ScoutLightControlMsg = 0x22
} ScoutMsgType;
BunkerMotionControlMsg = 0x21,
BunkerLightControlMsg = 0x22
} BunkerMsgType;
typedef struct
{
ScoutMsgType type;
BunkerMsgType type;
union {
// status messages
MotionStatusMessage motion_status_msg;
@@ -264,7 +263,7 @@ typedef struct
MotionControlMessage motion_control_msg;
LightControlMessage light_control_msg;
} body;
} ScoutMessage;
} BunkerMessage;
#pragma pack(pop)
@@ -272,4 +271,4 @@ typedef struct
}
#endif
#endif /* SCOUT_PROTOCOL_H */
#endif /* BUNKER_PROTOCOL_H */

24
ugv_sdk/include/ugv_sdk/scout/scout_types.hpp → ugv_sdk/include/ugv_sdk/bunker/bunker_types.hpp
View File

@@ -1,5 +1,5 @@
/*
* scout_state.hpp
* bunker_state.hpp
*
* Created on: Jun 11, 2019 08:48
* Description:
@@ -7,15 +7,15 @@
* Copyright (c) 2019 Ruixiang Du (rdu)
*/
#ifndef SCOUT_STATE_HPP
#define SCOUT_STATE_HPP
#ifndef BUNKER_STATE_HPP
#define BUNKER_STATE_HPP
#include <cstdint>
#include <iostream>
namespace westonrobot
{
struct ScoutState
struct BunkerState
{
enum MotorID
{
@@ -58,7 +58,7 @@ struct ScoutState
double angular_velocity = 0;
};
struct ScoutMotionCmd
struct BunkerMotionCmd
{
enum class FaultClearFlag
{
@@ -73,7 +73,7 @@ struct ScoutMotionCmd
MOTOR_OVERCURRENT = 0x08
};
ScoutMotionCmd(int8_t linear = 0, int8_t angular = 0,
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) {}
@@ -84,11 +84,11 @@ struct ScoutMotionCmd
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 = 0.5235; // 0.5235 rad/s
static constexpr double min_angular_velocity = -0.5235; // -0.5235 rad/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 ScoutLightCmd
struct BunkerLightCmd
{
enum class LightMode
{
@@ -98,8 +98,8 @@ struct ScoutLightCmd
CUSTOM = 0x03
};
ScoutLightCmd() = default;
ScoutLightCmd(LightMode f_mode, uint8_t f_value, LightMode r_mode, uint8_t r_value) : front_mode(f_mode), front_custom_value(f_value),
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;
@@ -109,4 +109,4 @@ struct ScoutLightCmd
};
} // namespace westonrobot
#endif /* SCOUT_STATE_HPP */
#endif /* BUNKER_STATE_HPP */

12
ugv_sdk/include/ugv_sdk/hunter/hunter_base.hpp → ugv_sdk/include/ugv_sdk/hunter_v2/hunter_base.hpp
View File

@@ -17,9 +17,9 @@
#include "ugv_sdk/mobile_base.hpp"
#include "ugv_sdk/hunter/hunter_protocol.h"
#include "ugv_sdk/hunter/hunter_can_parser.h"
#include "ugv_sdk/hunter/hunter_types.hpp"
#include "ugv_sdk/hunter_v2/hunter_protocol.h"
#include "ugv_sdk/hunter_v2/hunter_can_parser.h"
#include "ugv_sdk/hunter_v2/hunter_types.hpp"
namespace westonrobot {
class HunterBase : public MobileBase {
@@ -31,7 +31,7 @@ class HunterBase : public MobileBase {
HunterState GetHunterState();
// motion control
void SetMotionCommand(double linear_vel, double steering_angle,
void SetMotionCommand(double linear_vel, double angular_vel, double steering_angle,
HunterMotionCmd::FaultClearFlag fault_clr_flag =
HunterMotionCmd::FaultClearFlag::NO_FAULT);
@@ -39,6 +39,8 @@ class HunterBase : public MobileBase {
// cmd/status update related variables
std::mutex hunter_state_mutex_;
std::mutex motion_cmd_mutex_;
std::mutex mode_cmd_mutex_;
std::mutex pack_mode_cmd_mutex_;
HunterState hunter_state_;
HunterMotionCmd current_motion_cmd_;
@@ -50,6 +52,8 @@ class HunterBase : public MobileBase {
size_t bytes_received) override{};
void SendMotionCmd(uint8_t count);
void SendModeCtl();
void SetParkMode();
void NewStatusMsgReceivedCallback(const HunterMessage &msg);
public:

2
ugv_sdk/include/ugv_sdk/hunter/hunter_can_parser.h → ugv_sdk/include/ugv_sdk/hunter_v2/hunter_can_parser.h
View File

@@ -17,7 +17,7 @@ extern "C" {
#include <stdint.h>
#include <stdbool.h>
#include "ugv_sdk/hunter/hunter_protocol.h"
#include "ugv_sdk/hunter_v2/hunter_protocol.h"
#ifdef __linux__
#include <linux/can.h>

223
ugv_sdk/include/ugv_sdk/hunter/hunter_protocol.h → ugv_sdk/include/ugv_sdk/hunter_v2/hunter_protocol.h
View File

@@ -25,14 +25,17 @@ extern "C" {
#define HUNTER_MOTOR3_ID ((uint8_t)0x02)
// CAN Definitions
#define CAN_MSG_MOTION_CMD_ID ((uint32_t)0x130)
#define CAN_MSG_MOTION_STATUS_ID ((uint32_t)0x131)
#define CAN_MSG_CONFIG_CMD_ID ((uint32_t)0x210)
#define CAN_MSG_CONFIG_STATUS_ID ((uint32_t)0x211)
#define CAN_MSG_SYSTEM_STATUS_STATUS_ID ((uint32_t)0x151)
#define CAN_MSG_MOTOR1_DRIVER_STATUS_ID ((uint32_t)0x201)
#define CAN_MSG_MOTOR2_DRIVER_STATUS_ID ((uint32_t)0x202)
#define CAN_MSG_MOTOR3_DRIVER_STATUS_ID ((uint32_t)0x203)
#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_SYSTEM_STATUS_STATUS_ID ((uint32_t)0x211)
#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_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_PARK_CONTROL_ID ((uint32_t)0x131)
/*--------------------- Control/State Constants ------------------------*/
@@ -41,7 +44,6 @@ extern "C" {
#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)
@@ -87,25 +89,7 @@ extern "C" {
// 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;
} MotionCmdMessage;
// Motion Status Feedback
typedef struct {
union
{
@@ -116,15 +100,15 @@ typedef struct {
uint8_t high_byte;
uint8_t low_byte;
} linear_velocity;
uint8_t reserved0;
uint8_t reserved1;
uint8_t reserved2;
uint8_t reserved3;
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;
@@ -148,58 +132,25 @@ typedef struct {
uint8_t high_byte;
uint8_t low_byte;
} fault_code;
uint8_t park_mode;
uint8_t count;
uint8_t checksum;
} status;
uint8_t raw[8];
} data;
} SystemStatusMessage;
// System Configuration
// Motor Driver Height Speed Status Feedback
typedef struct {
union
{
struct
{
uint8_t set_zero_steering;
uint8_t reserved0;
uint8_t reserved1;
uint8_t reserved2;
uint8_t reserved3;
uint8_t reserved4;
uint8_t count;
uint8_t checksum;
} status;
uint8_t raw[8];
} data;
} ConfigCmdMessage;
// System Configuration Status Feedback
typedef struct {
union
{
struct
{
uint8_t set_zero_steering;
uint8_t reserved0;
uint8_t reserved1;
uint8_t reserved2;
uint8_t reserved3;
uint8_t reserved4;
uint8_t count;
uint8_t checksum;
} status;
uint8_t raw[8];
} data;
} ConfigStatusMessage;
// Motor Driver Feedback
typedef struct
{
uint8_t motor_id;
union {
union
{
struct
{
struct
{
uint8_t high_byte;
uint8_t low_byte;
} rpm;
struct
{
uint8_t high_byte;
@@ -207,17 +158,105 @@ typedef struct
} current;
struct
{
uint8_t high_byte;
uint8_t low_byte;
} rpm;
int8_t temperature;
uint8_t reserved0;
uint8_t count;
uint8_t checksum;
uint8_t heighest;
uint8_t sec_heighest;
uint8_t sec_lowest;
uint8_t lowest;
} moter_pose;
} status;
uint8_t raw[8];
} data;
} MotorDriverStatusMessage;
} MotorDriverHeightSpeedStatusMessage;
// Motor Driver Low Speed Status Feedback
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;
uint8_t motor_temperature;
uint8_t driver_status;
uint8_t reserved0;
uint8_t reserved1;
} status;
uint8_t raw[8];
} data;
} MotorDriverLowSpeedStatusMessage;
// Motion Control
typedef struct {
union
{
struct
{
struct
{
uint8_t high_byte;
uint8_t low_byte;
} linear_velocity_cmd;
uint8_t reserved0;
uint8_t reserved1;
uint8_t reserved2;
uint8_t reserved3;
struct
{
uint8_t high_byte;
uint8_t low_byte;
} angular_velocity_cmd;
} cmd;
uint8_t raw[8];
} data;
} MotionControlMessage;
// Parking Mode Control
typedef struct {
union
{
struct
{
uint8_t packing_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;
} ParkControlMessage;
// Motion Mode Control
typedef struct {
union
{
struct
{
uint8_t mode_control;
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;
// For convenience to access status/control message
typedef enum
@@ -225,12 +264,13 @@ typedef enum
HunterMsgNone = 0x00,
// status messages
HunterMotionStatusMsg = 0x01,
HunterSystemStatusMsg = 0x03,
HunterMotorDriverStatusMsg = 0x04,
HunterConfigStatusMsg = 0x05,
HunterSystemStatusMsg = 0x02,
HunterMotorDriverHeightSpeedStatusMsg = 0x03,
HunterMotorDriverLowSpeedStatusMsg = 0x04,
// control messages
HunterMotionCmdMsg = 0x21,
HunterConfigCmdMsg = 0x22
HunterMotionControlMsg = 0x21,
HunterParkControlMsg = 0x22,
HunterControlModeMsg = 0x23
} HunterMsgType;
typedef struct
@@ -240,11 +280,12 @@ typedef struct
// status messages
MotionStatusMessage motion_status_msg;
SystemStatusMessage system_status_msg;
ConfigStatusMessage config_status_msg;
MotorDriverStatusMessage motor_driver_status_msg;
MotorDriverHeightSpeedStatusMessage motor_driver_height_speed_status_msg;
MotorDriverLowSpeedStatusMessage motor_driver_low_speed_status_msg;
// control messages
MotionCmdMessage motion_cmd_msg;
ConfigCmdMessage config_cmd_msg;
MotionControlMessage motion_control_msg;
ParkControlMessage park_control_msg;
ModSelectMessage mode_cmd_msg;
} body;
} HunterMessage;

30
ugv_sdk/include/ugv_sdk/hunter/hunter_types.hpp → ugv_sdk/include/ugv_sdk/hunter_v2/hunter_types.hpp
View File

@@ -17,16 +17,24 @@ namespace westonrobot
{
struct HunterState
{
struct MotorState
struct MotorHeightSpeedState
{
double current = 0; // in A
double rpm = 0;
double temperature = 0;
double motor_pose = 0;
};
struct MotorLowSpeedState
{
double driver_voltage = 0;
double driver_temperature = 0;
double motor_temperature = 0;
uint8_t driver_state = 0;
};
// base state
uint8_t base_state = 0;
uint8_t control_mode = 0;
uint8_t park_mode = 1;
uint16_t fault_code = 0;
double battery_voltage = 0.0;
@@ -34,7 +42,8 @@ struct HunterState
// motor state
static constexpr uint8_t motor_num = 3;
MotorState motor_states[motor_num];
MotorHeightSpeedState motor_H_state[motor_num];
MotorLowSpeedState motor_L_state[motor_num];
// motion state
double linear_velocity = 0;
@@ -56,19 +65,22 @@ struct HunterMotionCmd
MOTOR_OVERCURRENT = 0x08
};
HunterMotionCmd(int8_t linear = 0, int8_t angular = 0,
HunterMotionCmd(int8_t linear_height_byte = 0, int8_t linear_low_byte = 0, int8_t angular_height_byte = 0, int8_t angular_low_byte = 0,
FaultClearFlag fault_clr_flag = FaultClearFlag::NO_FAULT)
: linear_velocity(linear), angular_velocity(angular),
: linear_velocity_height_byte(linear_height_byte),linear_velocity_low_byte(linear_low_byte), angular_velocity_height_byte(angular_height_byte),angular_velocity_low_byte(angular_low_byte),
fault_clear_flag(fault_clr_flag) {}
int8_t linear_velocity;
int8_t angular_velocity;
int8_t linear_velocity_height_byte;
int8_t linear_velocity_low_byte;
int8_t angular_velocity_height_byte;
int8_t angular_velocity_low_byte;
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_steering_angle = 0.4622; // 0.4622 rad ~= 26.5 degree
static constexpr double min_steering_angle = -0.4622; // -0.4622 rad
static constexpr double max_steering_angle = 0.576; // 0.576 rad ~= 30.00 degree
static constexpr double min_steering_angle = -0.576; // -0.576 rad
};
} // namespace westonrobot

14
ugv_sdk/include/ugv_sdk/scout/scout_base.hpp → ugv_sdk/include/ugv_sdk/scout_v2/scout_base.hpp
View File

@@ -18,10 +18,10 @@
#include "ugv_sdk/mobile_base.hpp"
#include "ugv_sdk/scout/scout_protocol.h"
#include "ugv_sdk/scout/scout_can_parser.h"
#include "ugv_sdk/scout/scout_uart_parser.h"
#include "ugv_sdk/scout/scout_types.hpp"
#include "ugv_sdk/scout_v2/scout_protocol.h"
#include "ugv_sdk/scout_v2/scout_can_parser.h"
#include "ugv_sdk/scout_v2/scout_uart_parser.h"
#include "ugv_sdk/scout_v2/scout_types.hpp"
namespace westonrobot {
class ScoutBase : public MobileBase {
@@ -48,14 +48,19 @@ class ScoutBase : public MobileBase {
uint8_t tx_buffer_[SCOUT_CMD_BUF_LEN];
// cmd/status update related variables
std::thread cmd_thread_;
std::mutex scout_state_mutex_;
std::mutex motion_cmd_mutex_;
std::mutex light_cmd_mutex_;
std::mutex mode_cmd_mutex_;
ScoutState scout_state_;
ScoutMotionCmd current_motion_cmd_;
ScoutLightCmd current_light_cmd_;
bool light_ctrl_enabled_ = false;
bool light_ctrl_requested_ = false;
@@ -67,6 +72,7 @@ class ScoutBase : public MobileBase {
void SendMotionCmd(uint8_t count);
void SendLightCmd(uint8_t count);
void SendModeCtl();
void NewStatusMsgReceivedCallback(const ScoutMessage &msg);
public:

2
ugv_sdk/include/ugv_sdk/scout/scout_can_parser.h → ugv_sdk/include/ugv_sdk/scout_v2/scout_can_parser.h
View File

@@ -17,7 +17,7 @@ extern "C" {
#include <stdint.h>
#include <stdbool.h>
#include "ugv_sdk/scout/scout_protocol.h"
#include "ugv_sdk/scout_v2/scout_protocol.h"
#ifdef __linux__
#include <linux/can.h>

393
ugv_sdk/include/ugv_sdk/scout_v2/scout_protocol.h Normal file
View File

@@ -0,0 +1,393 @@
/*
* 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)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;
} ScoutMessage;
#pragma pack(pop)
#ifdef __cplusplus
}
#endif
#endif /* SCOUT_PROTOCOL_H */

140
ugv_sdk/include/ugv_sdk/scout_v2/scout_types.hpp Normal file
View File

@@ -0,0 +1,140 @@
/*
* scout_state.hpp
*
* Created on: Jun 11, 2019 08:48
* Description:
*
* Copyright (c) 2019 Ruixiang Du (rdu)
*/
#ifndef SCOUT_STATE_HPP
#define SCOUT_STATE_HPP
#include <cstdint>
#include <iostream>
namespace westonrobot
{
struct ScoutState
{
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;
double motor_pose=0;
};
struct DriverState
{
double driver_voltage = 0;
double driver_temperature = 0;
uint8_t driver_state = 0;
};
struct MotorHeightSpeedState
{
double current = 0; // in A
double rpm = 0;
double motor_pose = 0;
};
struct MotorLowSpeedState
{
double driver_voltage = 0;
double driver_temperature = 0;
double motor_temperature = 0;
uint8_t driver_state = 0;
};
struct LightState
{
uint8_t mode = 0;
uint8_t custom_value = 0;
};
// base state
uint8_t base_state = 0;
uint8_t control_mode = 0;
uint8_t fault_code = 0;
double battery_voltage = 0.0;
// motor state
static constexpr uint8_t motor_num = 4;
// MotorState motor_states[motor_num];
// DriverState driver_states[motor_num];
MotorHeightSpeedState motor_H_state[motor_num];
MotorLowSpeedState motor_L_state[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;
//odometer state
double left_odomter=0;
double right_odomter=0;
};
struct ScoutMotionCmd
{
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
};
ScoutMotionCmd(int8_t linear_height_byte = 0, int8_t linear_low_byte = 0,int8_t angular_height_byte = 0,int8_t angular_low_byte = 0,
FaultClearFlag fault_clr_flag = FaultClearFlag::NO_FAULT)
: linear_velocity_height_byte(linear_height_byte),linear_velocity_low_byte(linear_low_byte), angular_velocity_height_byte(angular_height_byte),angular_velocity_low_byte(angular_low_byte),
fault_clear_flag(fault_clr_flag) {}
int8_t linear_velocity_height_byte;
int8_t linear_velocity_low_byte;
int8_t angular_velocity_height_byte;
int8_t angular_velocity_low_byte;
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 = 0.5235; // 0.5235 rad/s
static constexpr double min_angular_velocity = -0.5235; // -0.5235 rad/s
};
struct ScoutLightCmd
{
enum class LightMode
{
CONST_OFF = 0x00,
CONST_ON = 0x01,
BREATH = 0x02,
CUSTOM = 0x03
};
ScoutLightCmd() = default;
ScoutLightCmd(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 /* SCOUT_STATE_HPP */

2
ugv_sdk/include/ugv_sdk/scout/scout_uart_parser.h → ugv_sdk/include/ugv_sdk/scout_v2/scout_uart_parser.h
View File

@@ -18,7 +18,7 @@ extern "C" {
#include <string.h>
#include <stdbool.h>
#include "ugv_sdk/scout/scout_protocol.h"
#include "ugv_sdk/scout_v2/scout_protocol.h"
bool DecodeScoutMsgFromUART(uint8_t c, ScoutMessage *msg);
void EncodeScoutMsgToUART(const ScoutMessage *msg, uint8_t *buf, uint8_t *len);

72
ugv_sdk/include/ugv_sdk/tracer/tracer_base.hpp
View File

@@ -14,38 +14,28 @@
#include <cstdint>
#include <thread>
#include <mutex>
#include <functional>
#include "async_io/async_can.hpp"
#include "async_io/async_serial.hpp"
//#include "wrp_sdk/asyncio/async_can.hpp"
//#include "wrp_sdk/asyncio/async_serial.hpp"
#include "ugv_sdk/mobile_base.hpp"
#include "tracer_protocol/tracer_protocol.h"
#include "tracer_protocol/tracer_can_parser.h"
#include "ugv_sdk/tracer/tracer_protocol.h"
#include "ugv_sdk/tracer/tracer_can_parser.h"
#include "ugv_sdk/tracer/tracer_uart_parser.h"
#include "ugv_sdk/tracer/tracer_types.hpp"
#include "tracer_base/tracer_types.hpp"
namespace westonrobot
{
class TracerBase
class TracerBase :public MobileBase
{
public:
TracerBase() = default;
~TracerBase();
// do not allow copy
TracerBase(const TracerBase &tracer) = delete;
TracerBase &operator=(const TracerBase &tracer) = delete;
public:
// connect to roboot from CAN
void Connect(std::string dev_name);
// disconnect from roboot, only valid for serial port
void Disconnect();
// cmd thread runs at 100Hz (10ms) by default
void SetCmdThreadPeriodMs(int32_t period_ms) { cmd_thread_period_ms_ = period_ms; };
TracerBase() : MobileBase(){};
~TracerBase()= default;
// get robot state
TracerState GetTracerState();
UartTracerState GetUartTracerState();
// motion control
void SetMotionCommand(double linear_vel, double angular_vel,
TracerMotionCmd::FaultClearFlag fault_clr_flag = TracerMotionCmd::FaultClearFlag::NO_FAULT);
@@ -54,55 +44,53 @@ public:
void SetLightCommand(TracerLightCmd cmd);
void DisableLightCmdControl();
// get robot state
TracerState GetTracerState();
private:
// hardware communication interface
std::shared_ptr<AsyncCAN> can_if_;
std::shared_ptr<AsyncSerial> serial_if_;
// CAN priority higher than serial if both connected
bool can_connected_ = false;
bool serial_connected_ = false;
// bool can_connected_ = false;
// bool serial_connected_ = false;
// serial port related variables
uint8_t tx_cmd_len_;
uint8_t tx_buffer_[TRACER_CMD_BUF_LEN];
// cmd/status update related variables
std::thread cmd_thread_;
//std::thread cmd_thread_;
std::mutex tracer_state_mutex_;
std::mutex uart_tracer_state_mutex_;
std::mutex motion_cmd_mutex_;
std::mutex light_cmd_mutex_;
std::mutex mode_cmd_mutex_;
TracerState tracer_state_;
UartTracerState uart_tracer_state_;
TracerMotionCmd current_motion_cmd_;
UartTracerMotionCmd uart_current_motion_cmd_;
TracerLightCmd current_light_cmd_;
int32_t cmd_thread_period_ms_ = 10;
bool cmd_thread_started_ = false;
// int32_t cmd_thread_period_ms_ = 10;
// bool cmd_thread_started_ = false;
bool light_ctrl_enabled_ = false;
bool light_ctrl_requested_ = false;
// internal functions
void ConfigureCANBus(const std::string &can_if_name = "can1");
void ConfigureSerial(const std::string uart_name = "/dev/ttyUSB0", int32_t baud_rate = 115200);
void StartCmdThread();
void ControlLoop(int32_t period_ms);
void SendMotionCmd(uint8_t count);
void SendRobotCmd() override;
void SendLightCmd(uint8_t count);
void ParseCANFrame(can_frame *rx_frame);
void SendControlCmd();
void ParseCANFrame(can_frame *rx_frame) override;
void ParseUARTBuffer(uint8_t *buf, const size_t bufsize, size_t bytes_received);
// override{};
void NewStatusMsgReceivedCallback(const TracerMessage &msg);
void UartNewStatusMsgReceivedCallback(const UartTracerMessage &msg);
public:
static void UpdateTracerState(const TracerMessage &status_msg, TracerState &state);
static void UartUpdateTracerState(const UartTracerMessage &status_msg, UartTracerState &state);
};
} // namespace westonrobot

2
ugv_sdk/include/ugv_sdk/tracer/tracer_can_parser.h
View File

@@ -17,7 +17,7 @@ extern "C" {
#include <stdint.h>
#include <stdbool.h>
#include "tracer_protocol/tracer_protocol.h"
#include "ugv_sdk/tracer/tracer_protocol.h"
#ifdef __linux__
#include <linux/can.h>

300
ugv_sdk/include/ugv_sdk/tracer/tracer_protocol.h
View File

@@ -23,14 +23,26 @@ extern "C" {
#define TRACER_MOTOR1_ID ((uint8_t)0x00)
#define TRACER_MOTOR2_ID ((uint8_t)0x01)
// 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_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_CMD_ID ((uint32_t)0x130)
#define CAN_MSG_MOTION_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)0x201)
#define CAN_MSG_MOTOR2_DRIVER_STATUS_ID ((uint32_t)0x202)
#define CAN_MSG_MOTION_CMD_ID ((uint32_t)0x111)
#define CAN_MSG_MOTION_STATUS_ID ((uint32_t)0x221)
#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_COMTROL_MODE_ID ((uint32_t)0x421)
#define CAN_MSG_ODOMETER_ID ((uint32_t)0x311)
/*--------------------- Control/State Constants ------------------------*/
@@ -90,7 +102,32 @@ extern "C" {
// Note: id could be different for UART and CAN protocol
// Motion Control
// Motion Control(can)
typedef struct {
union
{
struct
{
struct
{
int8_t H_byte;
int8_t L_byte;
}linear_velocity;
struct
{
int8_t H_byte;
int8_t L_byte;
}angular_velocity;
uint8_t reserved0;
uint8_t reserved1;
uint8_t reserved2;
uint8_t reserved3;
} cmd;
uint8_t raw[8];
} data;
} MotionCmdMessage;
// Motion Control(uart)
typedef struct {
union
{
@@ -107,7 +144,49 @@ typedef struct {
} cmd;
uint8_t raw[8];
} data;
} MotionCmdMessage;
} UartMotionControlMessage;
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;
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;
} status;
uint8_t raw[8];
} data;
} MotionStatusMessage;
typedef struct {
union
@@ -131,10 +210,32 @@ typedef struct {
} status;
uint8_t raw[8];
} data;
} MotionStatusMessage;
} UartMotionStatusMessage;
// 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 count;
//uint8_t checksum;
} status;
uint8_t raw[8];
} data;
} SystemStatusMessage;
typedef struct {
union
{
struct
@@ -156,7 +257,7 @@ typedef struct {
} status;
uint8_t raw[8];
} data;
} SystemStatusMessage;
} UartSystemStatusMessage;
// Light Control
typedef struct {
@@ -167,11 +268,12 @@ typedef 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 reserved2;
uint8_t reserved3;
uint8_t count;
uint8_t checksum;
} cmd;
uint8_t raw[8];
} data;
@@ -190,11 +292,47 @@ typedef struct {
uint8_t reserved0;
uint8_t count;
uint8_t checksum;
} cmd;
uint8_t raw[8];
} data;
} UartLightControlMessage;
typedef struct {
union
{
struct
{
uint8_t light_ctrl_enable;
uint8_t front_light_mode;
uint8_t front_light_custom;
uint8_t reserved0;
uint8_t reserved1;
uint8_t reserved2;
uint8_t reserved3;
uint8_t count;
} status;
uint8_t raw[8];
} data;
} LightStatusMessage;
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;
} UartLightStatusMessage;
// Motor Driver Feedback
typedef struct
{
@@ -221,6 +359,97 @@ typedef struct
} data;
} MotorDriverStatusMessage;
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;
} UartMotorDriverStatusMessage;
typedef struct
{
uint8_t motor_id;
union {
struct
{
struct
{
uint8_t high_byte;
uint8_t low_byte;
} rpm;
int8_t reserved0;
int8_t reserved1;
int8_t reserved2;
int8_t reserved3;
int8_t reserved4;
int8_t reserved5;
} status;
uint8_t raw[8];
} data;
} MotorHeightSpeedStatusMessage;
typedef struct
{
uint8_t motor_id;
union {
struct
{
int8_t reserved0;
int8_t reserved1;
int8_t reserved2;
int8_t reserved3;
int8_t reserved4;
int8_t driver_state;
int8_t reserved5;
int8_t reserved6;
} status;
uint8_t raw[8];
} data;
} MotorLowSpeedStatusMessage;
typedef struct
{
uint8_t motor_id;
union {
struct
{
struct
{
uint8_t highest;
uint8_t sec_highest;
uint8_t sec_lowest;
uint8_t lowest;
}leftodometer;
struct
{
uint8_t highest;
uint8_t sec_highest;
uint8_t sec_lowest;
uint8_t lowest;
}rightodometer;
} status;
uint8_t raw[8];
} data;
} OdometerMessage;
// For convenience to access status/control message
typedef enum
{
@@ -230,12 +459,30 @@ typedef enum
TracerLightStatusMsg = 0x02,
TracerSystemStatusMsg = 0x03,
TracerMotorDriverStatusMsg = 0x04,
TracerOdometerMsg = 0x05,
TracerHeighSpeedMsg = 0x06,
TracerLowSpeedMsg = 0x07,
// control messages
TracerMotionCmdMsg = 0x21,
TracerLightControlMsg = 0x22
TracerLightControlMsg = 0x22,
TracerModeControlMsg = 0x23
} TracerMsgType;
typedef struct
typedef enum
{
UartTracerMsgNone = 0x00,
// status messages
UartTracerMotionStatusMsg = 0x01,
UartTracerLightStatusMsg = 0x02,
UartTracerSystemStatusMsg = 0x03,
UartTracerMotorDriverStatusMsg = 0x04,
// control messages
UartTracerMotionControlMsg = 0x21,
UartTracerLightControlMsg = 0x22
} UartTracerMsgType;
typedef struct
{
TracerMsgType type;
union {
@@ -244,12 +491,33 @@ typedef struct
LightStatusMessage light_status_msg;
SystemStatusMessage system_status_msg;
MotorDriverStatusMessage motor_driver_status_msg;
OdometerMessage odom_msg;
MotorHeightSpeedStatusMessage motor_heigh_speed_msg;
MotorLowSpeedStatusMessage motor_low_speed_msg;
// control messages
MotionCmdMessage motion_cmd_msg;
LightControlMessage light_control_msg;
ModSelectMessage mode_cmd_msg;
} body;
} TracerMessage;
typedef struct
{
UartTracerMsgType type;
union {
// status messages
UartMotionStatusMessage motion_status_msg;
UartLightStatusMessage light_status_msg;
UartSystemStatusMessage system_status_msg;
UartMotorDriverStatusMessage motor_driver_status_msg;
// control messages
UartMotionControlMessage motion_control_msg;
UartLightControlMessage light_control_msg;
} body;
} UartTracerMessage;
#pragma pack(pop)
#ifdef __cplusplus

99
ugv_sdk/include/ugv_sdk/tracer/tracer_types.hpp
View File

@@ -17,19 +17,11 @@ namespace westonrobot
{
struct TracerState
{
enum MotorID
{
FRONT_RIGHT = 0,
FRONT_LEFT = 1,
REAR_LEFT = 2,
REAR_RIGHT = 3
};
struct MotorState
{
double current = 0; // in A
//double current = 0; // in A
double rpm = 0;
double temperature = 0;
//double temperature = 0;
};
struct LightState
@@ -41,20 +33,65 @@ struct TracerState
// base state
uint8_t base_state = 0;
uint8_t control_mode = 0;
uint16_t fault_code = 0;
uint8_t fault_code = 0;
double battery_voltage = 0.0;
// motor state
MotorState motor_states[4];
MotorState motor_states[2];
// 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;
//odometer state
double left_odomter = 0;
double right_odomter = 0;
};
struct UartTracerState
{
enum MotorID
{
FRONT_RIGHT = 0,
FRONT_LEFT = 1,
REAR_LEFT = 2,
REAR_RIGHT = 3
};
struct MotorState
{
double current = 9; // in A
double rpm = 0;
double temperature = 9;
};
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 = 2;
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 TracerMotionCmd
@@ -72,14 +109,46 @@ struct TracerMotionCmd
MOTOR_OVERCURRENT = 0x08
};
TracerMotionCmd(int8_t linear = 0, int8_t angular = 0,
TracerMotionCmd(int8_t linear_H = 0, int8_t angular_H = 0,int8_t linear_L = 0, int8_t angular_L = 0,
FaultClearFlag fault_clr_flag = FaultClearFlag::NO_FAULT)
: linear_velocity_H(linear_H), angular_velocity_H(angular_H),linear_velocity_L(linear_L), angular_velocity_L(angular_L),
fault_clear_flag(fault_clr_flag) {}
int8_t linear_velocity_H;
int8_t linear_velocity_L;
int8_t angular_velocity_H;
int8_t angular_velocity_L;
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 = 0.7853; // 0.5235 rad/s
static constexpr double min_angular_velocity = -0.7853; // -0.5235 rad/s
};
struct UartTracerMotionCmd
{
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
};
UartTracerMotionCmd(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;
FaultClearFlag fault_clear_flag = FaultClearFlag::NO_FAULT;
static constexpr double max_linear_velocity = 1.5; // 1.5 m/s
static constexpr double min_linear_velocity = -1.5; // -1.5 m/s

24
ugv_sdk/include/ugv_sdk/tracer/tracer_uart_parser.h Normal file
View File

@@ -0,0 +1,24 @@
#ifndef TRACE_UART_PARSER_H
#define TRACE_UART_PARSER_H
#ifdef __cplusplus
extern "C" {
#endif
#include <stdint.h>
#include <string.h>
#include <stdbool.h>
#include "ugv_sdk/tracer/tracer_protocol.h"
bool DecodeTracerMsgFromUART(uint8_t c, UartTracerMessage *msg);
void EncodeTracerMsgToUART(const UartTracerMessage *msg, uint8_t *buf, uint8_t *len);
uint8_t CalcTracerUARTChecksum(uint8_t *buf, uint8_t len);
#ifdef __cplusplus
}
#endif
#endif /* TRACE_UART_PARSER_H */

200
ugv_sdk/src/bunker_base.cpp Normal file
View File

@@ -0,0 +1,200 @@
#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 Normal file
View File

@@ -0,0 +1,161 @@
/*
* 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;
}

162
ugv_sdk/src/hunter_base.cpp
View File

@@ -1,4 +1,4 @@
#include "ugv_sdk/hunter/hunter_base.hpp"
#include "ugv_sdk/hunter_v2/hunter_base.hpp"
#include <algorithm>
#include <array>
@@ -16,35 +16,35 @@ namespace westonrobot {
void HunterBase::SendRobotCmd() {
static uint8_t cmd_count = 0;
SendModeCtl();
SetParkMode();
SendMotionCmd(cmd_count++);
}
void HunterBase::SendMotionCmd(uint8_t count) {
// motion control message
HunterMessage m_msg;
m_msg.type = HunterMotionCmdMsg;
if (can_connected_)
m_msg.body.motion_cmd_msg.data.cmd.control_mode = CTRL_MODE_CMD_CAN;
m_msg.type = HunterMotionControlMsg;
/*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_cmd_msg.data.cmd.control_mode = CTRL_MODE_CMD_UART;
m_msg.body.motion_cmd_msg.data.cmd.control_mode = CTRL_MODE_CMD_UART*/;
motion_cmd_mutex_.lock();
m_msg.body.motion_cmd_msg.data.cmd.fault_clear_flag =
static_cast<uint8_t>(current_motion_cmd_.fault_clear_flag);
m_msg.body.motion_cmd_msg.data.cmd.linear_velocity_cmd =
current_motion_cmd_.linear_velocity;
m_msg.body.motion_cmd_msg.data.cmd.angular_velocity_cmd =
current_motion_cmd_.angular_velocity;
m_msg.body.motion_control_msg.data.cmd.linear_velocity_cmd.high_byte =
current_motion_cmd_.linear_velocity_height_byte;
m_msg.body.motion_control_msg.data.cmd.linear_velocity_cmd.low_byte =
current_motion_cmd_.linear_velocity_low_byte;
m_msg.body.motion_control_msg.data.cmd.angular_velocity_cmd.high_byte =
current_motion_cmd_.angular_velocity_height_byte;
m_msg.body.motion_control_msg.data.cmd.angular_velocity_cmd.low_byte =
current_motion_cmd_.angular_velocity_low_byte;
motion_cmd_mutex_.unlock();
m_msg.body.motion_cmd_msg.data.cmd.reserved0 = 0;
m_msg.body.motion_cmd_msg.data.cmd.reserved1 = 0;
m_msg.body.motion_cmd_msg.data.cmd.count = count;
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.reserved2 = 0;
m_msg.body.motion_control_msg.data.cmd.reserved3 = 0;
if (can_connected_)
m_msg.body.motion_cmd_msg.data.cmd.checksum = CalcHunterCANChecksum(
CAN_MSG_MOTION_CMD_ID, m_msg.body.motion_cmd_msg.data.raw, 8);
// send to can bus
if (can_connected_) {
@@ -60,7 +60,7 @@ HunterState HunterBase::GetHunterState() {
}
void HunterBase::SetMotionCommand(
double linear_vel, double steering_angle,
double linear_vel, double angular_vel,double steering_angle,
HunterMotionCmd::FaultClearFlag fault_clr_flag) {
// make sure cmd thread is started before attempting to send commands
if (!cmd_thread_started_) StartCmdThread();
@@ -75,26 +75,68 @@ void HunterBase::SetMotionCommand(
steering_angle = HunterMotionCmd::max_steering_angle;
std::lock_guard<std::mutex> guard(motion_cmd_mutex_);
current_motion_cmd_.linear_velocity = static_cast<int8_t>(
linear_vel / HunterMotionCmd::max_linear_velocity * 100.0);
current_motion_cmd_.angular_velocity = static_cast<int8_t>(
steering_angle / HunterMotionCmd::max_steering_angle * 100.0);
current_motion_cmd_.linear_velocity_height_byte = static_cast<int16_t>(linear_vel*1000)>>8;
current_motion_cmd_.linear_velocity_low_byte = static_cast<int16_t>(linear_vel*1000)&0xff;
current_motion_cmd_.angular_velocity_height_byte = static_cast<int16_t>(angular_vel*1000)>>8;
current_motion_cmd_.angular_velocity_low_byte = static_cast<int16_t>(angular_vel*1000)&0xff;
current_motion_cmd_.fault_clear_flag = fault_clr_flag;
FeedCmdTimeoutWatchdog();
}
void HunterBase::ParseCANFrame(can_frame *rx_frame) {
// validate checksum, discard frame if fails
if (!rx_frame->data[7] == CalcHunterCANChecksum(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;
}
void HunterBase::SendModeCtl(){
HunterMessage m_msg;
m_msg.type = HunterControlModeMsg;
mode_cmd_mutex_.lock();
m_msg.body.mode_cmd_msg.data.cmd.mode_control=0x01;
mode_cmd_mutex_.unlock();
m_msg.body.mode_cmd_msg.data.cmd.reserved0=0;
m_msg.body.mode_cmd_msg.data.cmd.reserved1=0;
m_msg.body.mode_cmd_msg.data.cmd.reserved2=0;
m_msg.body.mode_cmd_msg.data.cmd.reserved3=0;
m_msg.body.mode_cmd_msg.data.cmd.reserved4=0;
m_msg.body.mode_cmd_msg.data.cmd.reserved5=0;
m_msg.body.mode_cmd_msg.data.cmd.reserved6=0;
if (can_connected_) {
// send to can bus
can_frame m_frame;
EncodeHunterMsgToCAN(&m_msg, &m_frame);
can_if_->SendFrame(m_frame);
}else {}
}
// otherwise, update robot state with new frame
void HunterBase::SetParkMode(){
HunterMessage m_msg;
m_msg.type = HunterParkControlMsg;
bool flag = current_motion_cmd_.linear_velocity_height_byte ||
current_motion_cmd_.linear_velocity_low_byte;
if(flag)
{
pack_mode_cmd_mutex_.lock();
m_msg.body.park_control_msg.data.cmd.packing_mode=0x00;
pack_mode_cmd_mutex_.unlock();
}else {
pack_mode_cmd_mutex_.lock();
m_msg.body.park_control_msg.data.cmd.packing_mode=0x01;
pack_mode_cmd_mutex_.unlock();
}
m_msg.body.park_control_msg.data.cmd.reserved0=0;
m_msg.body.park_control_msg.data.cmd.reserved1=0;
m_msg.body.park_control_msg.data.cmd.reserved2=0;
m_msg.body.park_control_msg.data.cmd.reserved3=0;
m_msg.body.park_control_msg.data.cmd.reserved4=0;
m_msg.body.park_control_msg.data.cmd.reserved5=0;
m_msg.body.park_control_msg.data.cmd.reserved6=0;
if (can_connected_) {
// send to can bus
can_frame m_frame;
EncodeHunterMsgToCAN(&m_msg, &m_frame);
can_if_->SendFrame(m_frame);
}else {}
}
void HunterBase::ParseCANFrame(can_frame *rx_frame) {
// update robot state with new frame
HunterMessage status_msg;
DecodeHunterMsgFromCAN(rx_frame, &status_msg);
NewStatusMsgReceivedCallback(status_msg);
@@ -130,6 +172,7 @@ void HunterBase::UpdateHunterState(const HunterMessage &status_msg,
// 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.park_mode = msg.data.status.park_mode;
state.base_state = msg.data.status.base_state;
state.battery_voltage =
(static_cast<uint16_t>(msg.data.status.battery_voltage.low_byte) |
@@ -141,29 +184,46 @@ void HunterBase::UpdateHunterState(const HunterMessage &status_msg,
static_cast<uint16_t>(msg.data.status.fault_code.high_byte) << 8);
break;
}
case HunterMotorDriverStatusMsg: {
// std::cout << "motor 1 driver feedback received" << std::endl;
const MotorDriverStatusMessage &msg =
status_msg.body.motor_driver_status_msg;
case HunterMotorDriverHeightSpeedStatusMsg: {
// std::cout << "motor driver height speed feedback received" << std::endl;
const MotorDriverHeightSpeedStatusMessage &msg =
status_msg.body.motor_driver_height_speed_status_msg;
for (int i = 0; i < HunterState::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) |
state.motor_H_state[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;
state.motor_H_state[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_H_state[msg.motor_id].motor_pose
= static_cast<int32_t>(
static_cast<uint32_t>(msg.data.status.moter_pose.lowest) |
static_cast<uint32_t>(msg.data.status.moter_pose.sec_lowest) << 8 |
static_cast<uint32_t>(msg.data.status.moter_pose.sec_heighest)<< 16|
static_cast<uint32_t>(msg.data.status.moter_pose.heighest)<< 24 );
}
break;
}
case HunterConfigStatusMsg: {
const ConfigStatusMessage &msg = status_msg.body.config_status_msg;
state.set_zero_steering = msg.data.status.set_zero_steering;
break;
case HunterMotorDriverLowSpeedStatusMsg: {
// std::cout << "motor driver low speed feedback received" << std::endl;
const MotorDriverLowSpeedStatusMessage &msg =
status_msg.body.motor_driver_low_speed_status_msg;
for (int i = 0; i < HunterState::motor_num; ++i) {
state.motor_L_state[msg.motor_id].driver_voltage
=(static_cast<uint16_t>(msg.data.status.driver_voltage.low_byte) |
static_cast<uint16_t>(msg.data.status.driver_voltage.high_byte) << 8) /
10.0;
state.motor_L_state[msg.motor_id].driver_temperature
= static_cast<int16_t>(
static_cast<uint16_t>(msg.data.status.driver_temperature.low_byte) |
static_cast<uint16_t>(msg.data.status.driver_temperature.high_byte) << 8);
state.motor_L_state[msg.motor_id].motor_temperature
= msg.data.status.motor_temperature;
state.motor_L_state[msg.motor_id].driver_state
= msg.data.status.driver_status;
}
}
default:
break;

118
ugv_sdk/src/hunter_can_parser.c
View File

@@ -7,11 +7,12 @@
* Copyright (c) 2019 Ruixiang Du (rdu)
*/
#include "ugv_sdk/hunter/hunter_can_parser.h"
#include "ugv_sdk/hunter_v2/hunter_can_parser.h"
#include "string.h"
static void EncodeHunterMotionControlMsgToCAN(const MotionCmdMessage *msg, struct can_frame *tx_frame);
static void EncodeHunterMotionControlMsgToCAN(const MotionControlMessage *msg, struct can_frame *tx_frame);
static void EncodeHunterControlModeMsgToCAN(const ModSelectMessage *msg,struct can_frame *tx_frame);
static void EncodeHunterControlParkMsgToCAN(const ParkControlMessage *msg,struct can_frame *tx_frame);
bool DecodeHunterMsgFromCAN(const struct can_frame *rx_frame, HunterMessage *msg)
{
@@ -20,7 +21,7 @@ bool DecodeHunterMsgFromCAN(const struct can_frame *rx_frame, HunterMessage *msg
switch (rx_frame->can_id)
{
// in the current implementation, both MsgType and can_frame include 8 * uint8_t
case CAN_MSG_MOTION_STATUS_ID:
case CAN_MSG_MOTION_CONTROL_STATUS_ID:
{
msg->type = HunterMotionStatusMsg;
memcpy(msg->body.motion_status_msg.data.raw, rx_frame->data, rx_frame->can_dlc * sizeof(uint8_t));
@@ -32,46 +33,66 @@ bool DecodeHunterMsgFromCAN(const struct can_frame *rx_frame, HunterMessage *msg
memcpy(msg->body.system_status_msg.data.raw, rx_frame->data, rx_frame->can_dlc * sizeof(uint8_t));
break;
}
case CAN_MSG_CONFIG_STATUS_ID:
case CAN_MSG_MOTOR1_HEIGHT_DRIVER_STATUS_ID:
{
msg->type = HunterSystemStatusMsg;
memcpy(msg->body.config_status_msg.data.raw, rx_frame->data, rx_frame->can_dlc * sizeof(uint8_t));
msg->type = HunterMotorDriverHeightSpeedStatusMsg;
msg->body.motor_driver_height_speed_status_msg.motor_id = HUNTER_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_MOTOR1_DRIVER_STATUS_ID:
case CAN_MSG_MOTOR2_HEIGHT_DRIVER_STATUS_ID:
{
msg->type = HunterMotorDriverStatusMsg;
msg->body.motor_driver_status_msg.motor_id = HUNTER_MOTOR1_ID;
memcpy(msg->body.motor_driver_status_msg.data.raw, rx_frame->data, rx_frame->can_dlc * sizeof(uint8_t));
msg->type = HunterMotorDriverHeightSpeedStatusMsg;
msg->body.motor_driver_height_speed_status_msg.motor_id = HUNTER_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_MOTOR2_DRIVER_STATUS_ID:
case CAN_MSG_MOTOR3_HEIGHT_DRIVER_STATUS_ID:
{
msg->type = HunterMotorDriverStatusMsg;
msg->body.motor_driver_status_msg.motor_id = HUNTER_MOTOR2_ID;
memcpy(msg->body.motor_driver_status_msg.data.raw, rx_frame->data, rx_frame->can_dlc * sizeof(uint8_t));
msg->type = HunterMotorDriverHeightSpeedStatusMsg;
msg->body.motor_driver_height_speed_status_msg.motor_id = HUNTER_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_MOTOR3_DRIVER_STATUS_ID:
case CAN_MSG_MOTOR1_LOW_DRIVER_STATUS_ID:
{
msg->type = HunterMotorDriverStatusMsg;
msg->body.motor_driver_status_msg.motor_id = HUNTER_MOTOR3_ID;
memcpy(msg->body.motor_driver_status_msg.data.raw, rx_frame->data, rx_frame->can_dlc * sizeof(uint8_t));
msg->type = HunterMotorDriverLowSpeedStatusMsg;
msg->body.motor_driver_low_speed_status_msg.motor_id = HUNTER_MOTOR1_ID;
memcpy(msg->body.motor_driver_low_speed_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_CMD_ID:
case CAN_MSG_MOTOR2_LOW_DRIVER_STATUS_ID:
{
msg->type = HunterMotionCmdMsg;
memcpy(msg->body.motion_cmd_msg.data.raw, rx_frame->data, rx_frame->can_dlc * sizeof(uint8_t));
msg->type = HunterMotorDriverLowSpeedStatusMsg;
msg->body.motor_driver_low_speed_status_msg.motor_id = HUNTER_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_CONFIG_CMD_ID:
case CAN_MSG_MOTOR3_LOW_DRIVER_STATUS_ID:
{
msg->type = HunterConfigCmdMsg;
memcpy(msg->body.config_status_msg.data.raw, rx_frame->data, rx_frame->can_dlc * sizeof(uint8_t));
msg->type = HunterMotorDriverLowSpeedStatusMsg;
msg->body.motor_driver_low_speed_status_msg.motor_id = HUNTER_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_MOTION_CONTROL_CMD_ID:
{
msg->type = HunterMotionControlMsg;
memcpy(msg->body.motion_control_msg.data.raw, rx_frame->data, rx_frame->can_dlc * sizeof(uint8_t));
break;
}
case CAN_MSG_PARK_CONTROL_ID:
{
msg->type = HunterParkControlMsg;
memcpy(msg->body.park_control_msg.data.raw, rx_frame->data, rx_frame->can_dlc * sizeof(uint8_t));
break;
}
case CAN_MSG_SELECT_CONTROL_MODE_ID:
{
msg->type = HunterControlModeMsg;
memcpy(msg->body.mode_cmd_msg.data.raw, rx_frame->data, rx_frame->can_dlc * sizeof(uint8_t));
break;
}
default:
break;
}
@@ -86,7 +107,7 @@ void EncodeHunterMsgToCAN(const HunterMessage *msg, struct can_frame *tx_frame)
// in the current implementation, both MsgType and can_frame include 8 * uint8_t
case HunterMotionStatusMsg:
{
tx_frame->can_id = CAN_MSG_MOTION_STATUS_ID;
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;
@@ -98,37 +119,44 @@ void EncodeHunterMsgToCAN(const HunterMessage *msg, struct can_frame *tx_frame)
memcpy(tx_frame->data, msg->body.system_status_msg.data.raw, tx_frame->can_dlc);
break;
}
case HunterMotorDriverStatusMsg:
case HunterMotionControlMsg:
{
if (msg->body.motor_driver_status_msg.motor_id == HUNTER_MOTOR1_ID)
tx_frame->can_id = CAN_MSG_MOTOR1_DRIVER_STATUS_ID;
else if (msg->body.motor_driver_status_msg.motor_id == HUNTER_MOTOR2_ID)
tx_frame->can_id = CAN_MSG_MOTOR2_DRIVER_STATUS_ID;
else if (msg->body.motor_driver_status_msg.motor_id == HUNTER_MOTOR3_ID)
tx_frame->can_id = CAN_MSG_MOTOR3_DRIVER_STATUS_ID;
// else if (msg->body.motor_driver_status_msg.motor_id == HUNTER_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);
EncodeHunterMotionControlMsgToCAN(&(msg->body.motion_control_msg), tx_frame);
break;
}
case HunterMotionCmdMsg:
case HunterControlModeMsg:
{
EncodeHunterMotionControlMsgToCAN(&(msg->body.motion_cmd_msg), tx_frame);
EncodeHunterControlModeMsgToCAN(&(msg->body.mode_cmd_msg), tx_frame);
break;
}
case HunterParkControlMsg:
{
EncodeHunterControlParkMsgToCAN(&(msg->body.park_control_msg), tx_frame);
break;
}
default:
break;
}
tx_frame->data[7] = CalcHunterCANChecksum(tx_frame->can_id, tx_frame->data, tx_frame->can_dlc);
}
void EncodeHunterMotionControlMsgToCAN(const MotionCmdMessage *msg, struct can_frame *tx_frame)
void EncodeHunterMotionControlMsgToCAN(const MotionControlMessage *msg, struct can_frame *tx_frame)
{
tx_frame->can_id = CAN_MSG_MOTION_CMD_ID;
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);
}
void EncodeHunterControlModeMsgToCAN(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);
}
void EncodeHunterControlParkMsgToCAN(const ParkControlMessage *msg,struct can_frame *tx_frame)
{
tx_frame->can_id = CAN_MSG_PARK_CONTROL_ID;
tx_frame->can_dlc = 8;
memcpy(tx_frame->data, msg->data.raw, tx_frame->can_dlc);
tx_frame->data[7] = CalcHunterCANChecksum(tx_frame->can_id, tx_frame->data, tx_frame->can_dlc);
}
uint8_t CalcHunterCANChecksum(uint16_t id, uint8_t *data, uint8_t dlc)
@@ -138,4 +166,4 @@ uint8_t CalcHunterCANChecksum(uint16_t id, uint8_t *data, uint8_t dlc)
for (int i = 0; i < (dlc - 1); ++i)
checksum += data[i];
return checksum;
}
}

185
ugv_sdk/src/scout_base.cpp
View File

@@ -1,5 +1,5 @@
#include "ugv_sdk/scout/scout_base.hpp"
#include "ugv_sdk/scout_v2/scout_base.hpp"
#include <stdio.h>
#include <string>
#include <cstring>
#include <iostream>
@@ -16,6 +16,7 @@ namespace westonrobot {
void ScoutBase::SendRobotCmd() {
static uint8_t cmd_count = 0;
static uint8_t light_cmd_count = 0;
SendModeCtl();
SendMotionCmd(cmd_count++);
if (light_ctrl_requested_) SendLightCmd(light_cmd_count++);
}
@@ -25,49 +26,68 @@ void ScoutBase::SendMotionCmd(uint8_t count) {
ScoutMessage m_msg;
m_msg.type = ScoutMotionControlMsg;
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;
// 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;
m_msg.body.motion_control_msg.data.cmd.linear_velocity.high_byte =
current_motion_cmd_.linear_velocity_height_byte;
m_msg.body.motion_control_msg.data.cmd.linear_velocity.low_byte =
current_motion_cmd_.linear_velocity_low_byte;
m_msg.body.motion_control_msg.data.cmd.angular_velocity.high_byte =
current_motion_cmd_.angular_velocity_height_byte;
m_msg.body.motion_control_msg.data.cmd.angular_velocity.low_byte =
current_motion_cmd_.angular_velocity_low_byte;
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 =
CalcScoutCANChecksum(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
m_msg.body.motion_control_msg.data.cmd.reserved2 = 0;
m_msg.body.motion_control_msg.data.cmd.reserved3 = 0;
if (can_connected_) {
// send to can bus
can_frame m_frame;
EncodeScoutMsgToCAN(&m_msg, &m_frame);
can_if_->SendFrame(m_frame);
// std::cout << "CAN cmd sent" << std::endl;
} else {
// send to serial port
EncodeScoutMsgToUART(&m_msg, tx_buffer_, &tx_cmd_len_);
serial_if_->SendBytes(tx_buffer_, tx_cmd_len_);
// std::cout << "serial cmd sent" << std::endl;
}
}
void ScoutBase::SendModeCtl(){
ScoutMessage m_msg;
m_msg.type = ScoutControlModeMsg;
mode_cmd_mutex_.lock();
m_msg.body.mode_cmd_msg.data.cmd.control_mode=0x01;
mode_cmd_mutex_.unlock();
m_msg.body.mode_cmd_msg.data.cmd.reserved0=0;
m_msg.body.mode_cmd_msg.data.cmd.reserved1=0;
m_msg.body.mode_cmd_msg.data.cmd.reserved2=0;
m_msg.body.mode_cmd_msg.data.cmd.reserved3=0;
m_msg.body.mode_cmd_msg.data.cmd.reserved4=0;
m_msg.body.mode_cmd_msg.data.cmd.reserved5=0;
m_msg.body.mode_cmd_msg.data.cmd.reserved6=0;
if (can_connected_) {
// send to can bus
can_frame m_frame;
EncodeScoutMsgToCAN(&m_msg, &m_frame);
can_if_->SendFrame(m_frame);
} else {
// send to serial port
EncodeScoutMsgToUART(&m_msg, tx_buffer_, &tx_cmd_len_);
serial_if_->SendBytes(tx_buffer_, tx_cmd_len_);
}
}
void ScoutBase::SendLightCmd(uint8_t count) {
ScoutMessage l_msg;
l_msg.type = ScoutLightControlMsg;
light_cmd_mutex_.lock();
if (light_ctrl_enabled_) {
l_msg.body.light_control_msg.data.cmd.light_ctrl_enable = LIGHT_ENABLE_CTRL;
@@ -103,9 +123,9 @@ void ScoutBase::SendLightCmd(uint8_t count) {
l_msg.body.light_control_msg.data.cmd.reserved0 = 0;
l_msg.body.light_control_msg.data.cmd.count = count;
if (can_connected_)
l_msg.body.light_control_msg.data.cmd.checksum = CalcScoutCANChecksum(
CAN_MSG_LIGHT_CONTROL_CMD_ID, l_msg.body.light_control_msg.data.raw, 8);
// if (can_connected_)
// l_msg.body.light_control_msg.data.cmd.checksum = CalcScoutCANChecksum(
// CAN_MSG_LIGHT_CONTROL_CMD_ID, l_msg.body.light_control_msg.data.raw, 8);
// serial_connected_: checksum will be calculated later when packed into a
// complete serial frame
@@ -155,13 +175,11 @@ void ScoutBase::SetMotionCommand(
angular_vel = ScoutMotionCmd::max_angular_velocity;
std::lock_guard<std::mutex> guard(motion_cmd_mutex_);
current_motion_cmd_.linear_velocity = static_cast<int8_t>(
linear_vel / ScoutMotionCmd::max_linear_velocity * 100.0);
current_motion_cmd_.angular_velocity = static_cast<int8_t>(
angular_vel / ScoutMotionCmd::max_angular_velocity * 100.0);
current_motion_cmd_.linear_velocity_height_byte = static_cast<int16_t>(linear_vel*1000)>>8;
current_motion_cmd_.linear_velocity_low_byte = static_cast<int16_t>(linear_vel*1000)&0xff;
current_motion_cmd_.angular_velocity_height_byte = static_cast<int16_t>(angular_vel*1000)>>8;
current_motion_cmd_.angular_velocity_low_byte = static_cast<int16_t>(angular_vel*1000)&0xff;
current_motion_cmd_.fault_clear_flag = fault_clr_flag;
FeedCmdTimeoutWatchdog();
}
void ScoutBase::SetLightCommand(ScoutLightCmd cmd) {
@@ -181,17 +199,21 @@ void ScoutBase::DisableLightCmdControl() {
void ScoutBase::ParseCANFrame(can_frame *rx_frame) {
// validate checksum, discard frame if fails
if (!rx_frame->data[7] == CalcScoutCANChecksum(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;
}
// if (!rx_frame->data[7] == CalcScoutCANChecksum(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
ScoutMessage status_msg;
DecodeScoutMsgFromCAN(rx_frame, &status_msg);
// printf("%x\t",status_msg.body.odom_msg.data.status.left.heighest);
// printf("%x\t",status_msg.body.odom_msg.data.status.left.sec_heighest);
// printf("%x\t",status_msg.body.odom_msg.data.status.left.sec_lowest);
// printf("%x\r\n",status_msg.body.odom_msg.data.status.left.lowest);
NewStatusMsgReceivedCallback(status_msg);
}
@@ -219,18 +241,8 @@ void ScoutBase::UpdateScoutState(const ScoutMessage &status_msg,
case ScoutMotionStatusMsg: {
// 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;
state.linear_velocity =static_cast<int16_t>(static_cast<uint16_t>(msg.data.cmd.linear_velocity.low_byte) |static_cast<uint16_t>(msg.data.cmd.linear_velocity.high_byte)<< 8)/1000.0;
state.angular_velocity =static_cast<int16_t>(static_cast<uint16_t>(msg.data.cmd.angular_velocity.low_byte) |static_cast<uint16_t>(msg.data.cmd.angular_velocity.high_byte)<< 8)/1000.0;
break;
}
case ScoutLightStatusMsg: {
@@ -256,30 +268,79 @@ void ScoutBase::UpdateScoutState(const ScoutMessage &status_msg,
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);
state.fault_code =msg.data.status.fault_code;
break;
}
case ScoutMotorDriverStatusMsg: {
// case ScoutMotorDriverStatusMsg: {
// // std::cout << "motor 1 driver feedback received" << std::endl;
// const MotorDriverStatusMessage &msg =
// status_msg.body.motor_driver_status_msg;
// for (int i = 0; i < ScoutState::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;
// }
case ScoutMotorDriverHeightSpeedStatusMsg: {
// std::cout << "motor 1 driver feedback received" << std::endl;
const MotorDriverStatusMessage &msg =
status_msg.body.motor_driver_status_msg;
const MotorDriverHeightSpeedStatusMessage &msg =status_msg.body.motor_driver_height_speed_status_msg;
for (int i = 0; i < ScoutState::motor_num; ++i) {
state.motor_states[status_msg.body.motor_driver_status_msg.motor_id]
state.motor_H_state[status_msg.body.motor_driver_height_speed_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]
state.motor_H_state[status_msg.body.motor_driver_height_speed_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;
static_cast<int16_t>(msg.data.status.rpm.low_byte) |
static_cast<int16_t>(msg.data.status.rpm.high_byte) << 8);
state.motor_H_state[status_msg.body.motor_driver_height_speed_status_msg.motor_id]
.motor_pose = static_cast<int32_t>(static_cast<uint32_t>(msg.data.status.moter_pose.lowest)|static_cast<uint32_t>(msg.data.status.moter_pose.sec_lowest
)<<8|static_cast<uint32_t>(msg.data.status.moter_pose.sec_heighest)<<16|static_cast<uint32_t>(msg.data.status.moter_pose.heighest)<<24);
}
break;
}
case ScoutMotorDriverLowSpeedStatusMsg: {
// std::cout << "motor 1 driver feedback received" << std::endl;
const MotorDriverLowSpeedStatusMessage &msg =status_msg.body.motor_driver_low_speed_status_msg;
for (int i = 0; i < ScoutState::motor_num; ++i) {
state.motor_L_state[status_msg.body.motor_driver_low_speed_status_msg.motor_id]
.driver_voltage =
(static_cast<uint16_t>(msg.data.status.driver_voltage.low_byte) |
static_cast<uint16_t>(msg.data.status.driver_voltage.high_byte) << 8) /
10.0;
state.motor_L_state[status_msg.body.motor_driver_low_speed_status_msg.motor_id]
.driver_temperature = static_cast<int16_t>(
static_cast<uint16_t>(msg.data.status.driver_temperature.low_byte) |
static_cast<uint16_t>(msg.data.status.driver_temperature.high_byte) << 8);
state.motor_L_state[status_msg.body.motor_driver_low_speed_status_msg.motor_id]
.motor_temperature = msg.data.status.motor_temperature;
state.motor_L_state[status_msg.body.motor_driver_low_speed_status_msg.motor_id]
.driver_state = msg.data.status.driver_state;
}
break;
}
case ScoutodometerMsg:{
const OdomterMessage &msg=status_msg.body.odom_msg;
state.left_odomter=static_cast<int32_t>(static_cast<uint32_t>(msg.data.status.left.lowest)|static_cast<uint32_t>(msg.data.status.left.sec_lowest)<<8|(static_cast<uint32_t>(msg.data.status.left.sec_heighest))<<16|static_cast<uint32_t>(msg.data.status.left.heighest)<<24);
state.right_odomter=static_cast<int32_t>((static_cast<uint32_t>(msg.data.status.right.lowest))|(static_cast<uint32_t>(msg.data.status.right.sec_lowest)<<8)|(static_cast<uint32_t>(msg.data.status.right.sec_heighest)<<16)|(static_cast<uint32_t>(msg.data.status.right.heighest)<<24));
// printf("%x\t",status_msg.body.odom_msg.data.status.left.heighest);
// printf("%x\t",status_msg.body.odom_msg.data.status.left.sec_heighest);
// printf("%x\t",status_msg.body.odom_msg.data.status.left.sec_lowest);
// printf("%x\r\n",status_msg.body.odom_msg.data.status.left.lowest);
break;
}
}
}
} // namespace westonrobot

150
ugv_sdk/src/scout_can_parser.c
View File

@@ -7,12 +7,14 @@
* Copyright (c) 2019 Ruixiang Du (rdu)
*/
#include "ugv_sdk/scout/scout_can_parser.h"
#include "ugv_sdk/scout_v2/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);
static void EncodeScoutnControlModeMsgToCAN(const ModSelectMessage *msg, struct can_frame *tx_frame);
bool DecodeScoutMsgFromCAN(const struct can_frame *rx_frame, ScoutMessage *msg)
{
@@ -42,38 +44,6 @@ bool DecodeScoutMsgFromCAN(const struct can_frame *rx_frame, ScoutMessage *msg)
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:
{
@@ -89,6 +59,75 @@ bool DecodeScoutMsgFromCAN(const struct can_frame *rx_frame, ScoutMessage *msg)
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;
}
@@ -122,25 +161,30 @@ void EncodeScoutMsgToCAN(const ScoutMessage *msg, struct can_frame *tx_frame)
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 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 ScoutControlModeMsg:
{
EncodeScoutnControlModeMsgToCAN(&(msg->body.motion_control_msg), tx_frame);
break;
}
case ScoutLightControlMsg:
{
EncodeScoutLightControlMsgToCAN(&(msg->body.light_control_msg), tx_frame);
@@ -149,7 +193,7 @@ void EncodeScoutMsgToCAN(const ScoutMessage *msg, struct can_frame *tx_frame)
default:
break;
}
tx_frame->data[7] = CalcScoutCANChecksum(tx_frame->can_id, tx_frame->data, tx_frame->can_dlc);
// 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)
@@ -159,7 +203,13 @@ void EncodeScoutMotionControlMsgToCAN(const MotionControlMessage *msg, struct ca
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;
@@ -175,4 +225,4 @@ uint8_t CalcScoutCANChecksum(uint16_t id, uint8_t *data, uint8_t dlc)
for (int i = 0; i < (dlc - 1); ++i)
checksum += data[i];
return checksum;
}
}

70
ugv_sdk/src/scout_uart_parser.c
View File

@@ -7,7 +7,7 @@
* Copyright (c) 2019 Ruixiang Du (rdu)
*/
#include "ugv_sdk/scout/scout_uart_parser.h"
#include "ugv_sdk/scout_v2/scout_uart_parser.h"
// #define USE_XOR_CHECKSUM
@@ -96,13 +96,13 @@ void EncodeScoutMsgToUART(const ScoutMessage *msg, uint8_t *buf, uint8_t *len)
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[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] = msg->body.motion_status_msg.data.status.count;
buf[11] = 0;
break;
}
case ScoutLightStatusMsg:
@@ -124,9 +124,9 @@ void EncodeScoutMsgToUART(const ScoutMessage *msg, uint8_t *buf, uint8_t *len)
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;
buf[9] = msg->body.system_status_msg.data.status.fault_code;
buf[10] = 0;
buf[11] = 0;
break;
}
case ScoutMotorDriverStatusMsg:
@@ -190,16 +190,16 @@ void EncodeMotionControlMsgToUART(const MotionControlMessage *msg, uint8_t *buf,
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[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] = msg->data.cmd.count;
buf[12] = CalcScoutUARTChecksum(buf, buf[2] + FRAME_SOF_LEN);
buf[11] = 0x00;
buf[12] = 0x00;
// length: SOF + Frame + Checksum
*len = buf[2] + FRAME_SOF_LEN + 1;
@@ -453,14 +453,14 @@ bool ConstructControlMessage(ScoutMessage *msg)
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.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.count = uart_parsing_data.frame_cnt;
msg->body.motion_control_msg.data.cmd.checksum = uart_parsing_data.frame_checksum;
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:
@@ -473,7 +473,7 @@ bool ConstructControlMessage(ScoutMessage *msg)
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;
// msg->body.light_control_msg.data.cmd.checksum = uart_parsing_data.frame_checksum;
break;
}
}
@@ -494,23 +494,23 @@ bool ConstructStatusMessage(ScoutMessage *msg)
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;
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.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;
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:
@@ -579,7 +579,7 @@ bool ConstructStatusMessage(ScoutMessage *msg)
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.checksum = uart_parsing_data.frame_checksum;
msg->body.light_status_msg.data.status.reserved1 = 0x00;
break;
}
}

441
ugv_sdk/src/tracer_base.cpp
View File

@@ -1,4 +1,4 @@
#include "tracer_base/tracer_base.hpp"
#include "ugv_sdk/tracer/tracer_base.hpp"
#include <string>
#include <cstring>
@@ -10,117 +10,124 @@
#include <ratio>
#include <thread>
#include "stopwatch/stopwatch.h"
#include "stopwatch.hpp"
namespace westonrobot
{
TracerBase::~TracerBase()
{
if (serial_connected_)
serial_if_->close();
if (cmd_thread_.joinable())
cmd_thread_.join();
void TracerBase::SendRobotCmd() {
static uint8_t cmd_count = 0;
static uint8_t light_cmd_count = 0;
if(can_connected_) SendControlCmd();
SendMotionCmd(cmd_count++);
if (light_ctrl_requested_) SendLightCmd(light_cmd_count++);
}
void TracerBase::Connect(std::string dev_name)
{
// if (baud_rate == 0) {
ConfigureCANBus(dev_name);
// } else {
// ConfigureSerial(dev_name, baud_rate);
// if (!serial_connected_)
// std::cerr << "ERROR: Failed to connect to serial port" << std::endl;
// }
}
void TracerBase::Disconnect()
{
if (serial_connected_)
{
if (serial_if_->is_open())
serial_if_->close();
}
}
void TracerBase::ConfigureCANBus(const std::string &can_if_name)
{
can_if_ = std::make_shared<AsyncCAN>(can_if_name);
can_if_->set_receive_callback(std::bind(&TracerBase::ParseCANFrame, this, std::placeholders::_1));
can_connected_ = true;
}
void TracerBase::ConfigureSerial(const std::string uart_name, int32_t baud_rate)
{
serial_if_ = std::make_shared<AsyncSerial>(uart_name, baud_rate);
serial_if_->open();
if (serial_if_->is_open())
serial_connected_ = true;
serial_if_->set_receive_callback(std::bind(&TracerBase::ParseUARTBuffer, this,
std::placeholders::_1,
std::placeholders::_2,
std::placeholders::_3));
}
void TracerBase::StartCmdThread()
{
current_motion_cmd_.linear_velocity = 0;
current_motion_cmd_.angular_velocity = 0;
current_motion_cmd_.fault_clear_flag = TracerMotionCmd::FaultClearFlag::NO_FAULT;
cmd_thread_ = std::thread(std::bind(&TracerBase::ControlLoop, this, cmd_thread_period_ms_));
cmd_thread_started_ = true;
}
void TracerBase::SendMotionCmd(uint8_t count)
{
// motion control message
TracerMessage m_msg;
m_msg.type = TracerMotionCmdMsg;
if (can_connected_)
m_msg.body.motion_cmd_msg.data.cmd.control_mode = CTRL_MODE_CMD_CAN;
else if (serial_connected_)
m_msg.body.motion_cmd_msg.data.cmd.control_mode = CTRL_MODE_CMD_UART;
motion_cmd_mutex_.lock();
m_msg.body.motion_cmd_msg.data.cmd.fault_clear_flag = static_cast<uint8_t>(current_motion_cmd_.fault_clear_flag);
m_msg.body.motion_cmd_msg.data.cmd.linear_velocity_cmd = current_motion_cmd_.linear_velocity;
m_msg.body.motion_cmd_msg.data.cmd.angular_velocity_cmd = current_motion_cmd_.angular_velocity;
motion_cmd_mutex_.unlock();
m_msg.body.motion_cmd_msg.data.cmd.reserved0 = 0;
m_msg.body.motion_cmd_msg.data.cmd.reserved1 = 0;
m_msg.body.motion_cmd_msg.data.cmd.count = count;
if (can_connected_)
m_msg.body.motion_cmd_msg.data.cmd.checksum = CalcTracerCANChecksum(CAN_MSG_MOTION_CMD_ID, m_msg.body.motion_cmd_msg.data.raw, 8);
// serial_connected_: checksum will be calculated later when packed into a complete serial frame
if (can_connected_)
if(can_connected_)
{
// send to can bus
can_frame m_frame;
EncodeTracerMsgToCAN(&m_msg, &m_frame);
can_if_->send_frame(m_frame);
// motion control message
TracerMessage m_msg;
m_msg.type = TracerMotionCmdMsg;
//SendControlCmd();
motion_cmd_mutex_.lock();
m_msg.body.motion_cmd_msg.data.cmd.linear_velocity.H_byte = current_motion_cmd_.linear_velocity_H;
m_msg.body.motion_cmd_msg.data.cmd.linear_velocity.L_byte = current_motion_cmd_.linear_velocity_L;
m_msg.body.motion_cmd_msg.data.cmd.angular_velocity.H_byte = current_motion_cmd_.angular_velocity_H;
m_msg.body.motion_cmd_msg.data.cmd.angular_velocity.L_byte = current_motion_cmd_.angular_velocity_L;
motion_cmd_mutex_.unlock();
m_msg.body.motion_cmd_msg.data.cmd.reserved0 = 0;
m_msg.body.motion_cmd_msg.data.cmd.reserved1 = 0;
m_msg.body.motion_cmd_msg.data.cmd.reserved2 = 0;
m_msg.body.motion_cmd_msg.data.cmd.reserved3 = 0;
if (can_connected_)
{
// send to can bus
can_frame m_frame;
EncodeTracerMsgToCAN(&m_msg, &m_frame);
can_if_->SendFrame(m_frame);
}
// else
// {
// // TODO
// // send to serial port
// // EncodeTracerMsgToUART(&m_msg, tx_buffer_, &tx_cmd_len_);
// // serial_if_->send_bytes(tx_buffer_, tx_cmd_len_);
// }
// if (can_connected_)
// m_msg.body.motion_cmd_msg.data.cmd.checksum = CalcTracerCANChecksum(CAN_MSG_MOTION_CMD_ID, m_msg.body.motion_cmd_msg.data.raw, 8);
// serial_connected_: checksum will be calculated later when packed into a complete serial frame
}
else
{
// TODO
// send to serial port
// EncodeTracerMsgToUART(&m_msg, tx_buffer_, &tx_cmd_len_);
// serial_if_->send_bytes(tx_buffer_, tx_cmd_len_);
else if (serial_connected_){
UartTracerMessage m_msg;
m_msg.type = UartTracerMotionControlMsg;
m_msg.body.motion_control_msg.data.cmd.control_mode = CTRL_MODE_CMD_UART;
//SendControlCmd();
motion_cmd_mutex_.lock();
m_msg.body.motion_control_msg.data.cmd.fault_clear_flag =
static_cast<uint8_t>(uart_current_motion_cmd_.fault_clear_flag);
m_msg.body.motion_control_msg.data.cmd.linear_velocity_cmd =
uart_current_motion_cmd_.linear_velocity;
m_msg.body.motion_control_msg.data.cmd.angular_velocity_cmd =
uart_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;
// serial_connected_: checksum will be calculated later when packed into a
// complete serial frame
EncodeTracerMsgToUART(&m_msg, tx_buffer_, &tx_cmd_len_);
serial_if_->SendBytes(tx_buffer_, tx_cmd_len_);
}
}
void TracerBase::SendLightCmd(uint8_t count)
{
TracerMessage l_msg;
UartTracerMessage l_msg;
l_msg.type = UartTracerLightControlMsg;
light_cmd_mutex_.lock();
if (light_ctrl_enabled_) {
l_msg.body.light_control_msg.data.cmd.light_ctrl_enable = LIGHT_ENABLE_CTRL;
l_msg.body.light_control_msg.data.cmd.front_light_mode =
static_cast<uint8_t>(current_light_cmd_.front_mode);
l_msg.body.light_control_msg.data.cmd.front_light_custom =
current_light_cmd_.front_custom_value;
l_msg.body.light_control_msg.data.cmd.rear_light_mode =
static_cast<uint8_t>(current_light_cmd_.rear_mode);
l_msg.body.light_control_msg.data.cmd.rear_light_custom =
current_light_cmd_.rear_custom_value;
} else {
l_msg.body.light_control_msg.data.cmd.light_ctrl_enable =
LIGHT_DISABLE_CTRL;
l_msg.body.light_control_msg.data.cmd.front_light_mode =
LIGHT_MODE_CONST_OFF;
l_msg.body.light_control_msg.data.cmd.front_light_custom = 0;
l_msg.body.light_control_msg.data.cmd.rear_light_mode =
LIGHT_MODE_CONST_OFF;
l_msg.body.light_control_msg.data.cmd.rear_light_custom = 0;
}
light_ctrl_requested_ = false;
light_cmd_mutex_.unlock();
l_msg.body.light_control_msg.data.cmd.reserved0 = 0;
l_msg.body.light_control_msg.data.cmd.count = count;
// serial_connected_: checksum will be calculated later when packed into a
// complete serial frame
// send to serial port
EncodeTracerMsgToUART(&l_msg, tx_buffer_, &tx_cmd_len_);
serial_if_->SendBytes(tx_buffer_, tx_cmd_len_);
/*TracerMessage l_msg;
l_msg.type = TracerLightControlMsg;
light_cmd_mutex_.lock();
@@ -130,8 +137,8 @@ void TracerBase::SendLightCmd(uint8_t count)
l_msg.body.light_control_msg.data.cmd.front_light_mode = static_cast<uint8_t>(current_light_cmd_.front_mode);
l_msg.body.light_control_msg.data.cmd.front_light_custom = current_light_cmd_.front_custom_value;
l_msg.body.light_control_msg.data.cmd.rear_light_mode = static_cast<uint8_t>(current_light_cmd_.rear_mode);
l_msg.body.light_control_msg.data.cmd.rear_light_custom = current_light_cmd_.rear_custom_value;
//l_msg.body.light_control_msg.data.cmd.rear_light_mode = static_cast<uint8_t>(current_light_cmd_.rear_mode);
//l_msg.body.light_control_msg.data.cmd.rear_light_custom = current_light_cmd_.rear_custom_value;
// std::cout << "cmd: " << l_msg.data.cmd.front_light_mode << " , " << l_msg.data.cmd.front_light_custom << " , "
// << l_msg.data.cmd.rear_light_mode << " , " << l_msg.data.cmd.rear_light_custom << std::endl;
@@ -143,8 +150,8 @@ void TracerBase::SendLightCmd(uint8_t count)
l_msg.body.light_control_msg.data.cmd.front_light_mode = LIGHT_MODE_CONST_OFF;
l_msg.body.light_control_msg.data.cmd.front_light_custom = 0;
l_msg.body.light_control_msg.data.cmd.rear_light_mode = LIGHT_MODE_CONST_OFF;
l_msg.body.light_control_msg.data.cmd.rear_light_custom = 0;
//l_msg.body.light_control_msg.data.cmd.rear_light_mode = LIGHT_MODE_CONST_OFF;
//l_msg.body.light_control_msg.data.cmd.rear_light_custom = 0;
}
light_ctrl_requested_ = false;
light_cmd_mutex_.unlock();
@@ -153,7 +160,7 @@ void TracerBase::SendLightCmd(uint8_t count)
l_msg.body.light_control_msg.data.cmd.count = count;
if (can_connected_)
l_msg.body.light_control_msg.data.cmd.checksum = CalcTracerCANChecksum(CAN_MSG_LIGHT_CONTROL_CMD_ID, l_msg.body.light_control_msg.data.raw, 8);
// l_msg.body.light_control_msg.data.cmd.checksum = CalcTracerCANChecksum(CAN_MSG_LIGHT_CONTROL_CMD_ID, l_msg.body.light_control_msg.data.raw, 8);
// serial_connected_: checksum will be calculated later when packed into a complete serial frame
if (can_connected_)
@@ -162,7 +169,7 @@ void TracerBase::SendLightCmd(uint8_t count)
can_frame l_frame;
EncodeTracerMsgToCAN(&l_msg, &l_frame);
can_if_->send_frame(l_frame);
can_if_->SendFrame(l_frame);
}
// else
// {
@@ -179,28 +186,39 @@ void TracerBase::SendLightCmd(uint8_t count)
// std::cout << "uart: ";
// for (int i = 0; i < tx_cmd_len_; ++i)
// std::cout << static_cast<int>(tx_buffer_[i]) << " ";
// std::cout << std::endl;
// std::cout << std::endl;*/
}
void TracerBase::ControlLoop(int32_t period_ms)
void TracerBase::SendControlCmd()
{
StopWatch ctrl_sw;
uint8_t cmd_count = 0;
uint8_t light_cmd_count = 0;
while (true)
{
ctrl_sw.tic();
TracerMessage c_msg;
c_msg.type = TracerModeControlMsg;
// motion control message
SendMotionCmd(cmd_count++);
mode_cmd_mutex_.lock();
c_msg.body.mode_cmd_msg.data.cmd.control_mode=0x01;
mode_cmd_mutex_.unlock();
c_msg.body.mode_cmd_msg.data.cmd.reserved0=0;
c_msg.body.mode_cmd_msg.data.cmd.reserved1=0;
c_msg.body.mode_cmd_msg.data.cmd.reserved2=0;
c_msg.body.mode_cmd_msg.data.cmd.reserved3=0;
c_msg.body.mode_cmd_msg.data.cmd.reserved4=0;
c_msg.body.mode_cmd_msg.data.cmd.reserved5=0;
c_msg.body.mode_cmd_msg.data.cmd.reserved6=0;
if (can_connected_)
{
// send to can bus
can_frame c_frame;
EncodeTracerMsgToCAN(&c_msg, &c_frame);
can_if_->SendFrame(c_frame);
}
else
{
// TODO
// send to serial port
// EncodeTracerMsgToUART(&m_msg, tx_buffer_, &tx_cmd_len_);
// serial_if_->send_bytes(tx_buffer_, tx_cmd_len_);
}
// check if there is request for light control
if (light_ctrl_requested_)
SendLightCmd(light_cmd_count++);
ctrl_sw.sleep_until_ms(period_ms);
// std::cout << "control loop update frequency: " << 1.0 / ctrl_sw.toc() << std::endl;
}
}
TracerState TracerBase::GetTracerState()
@@ -209,25 +227,54 @@ TracerState TracerBase::GetTracerState()
return tracer_state_;
}
UartTracerState TracerBase::GetUartTracerState()
{
std::lock_guard<std::mutex> guard(uart_tracer_state_mutex_);
return uart_tracer_state_;
}
void TracerBase::SetMotionCommand(double linear_vel, double angular_vel, TracerMotionCmd::FaultClearFlag fault_clr_flag)
{
// make sure cmd thread is started before attempting to send commands
if (!cmd_thread_started_)
StartCmdThread();
if (!cmd_thread_started_) StartCmdThread();
if(can_connected_)
{
if (linear_vel < TracerMotionCmd::min_linear_velocity)
linear_vel = TracerMotionCmd::min_linear_velocity;
if (linear_vel > TracerMotionCmd::max_linear_velocity)
linear_vel = TracerMotionCmd::max_linear_velocity;
if (angular_vel < TracerMotionCmd::min_angular_velocity)
angular_vel = TracerMotionCmd::min_angular_velocity;
if (angular_vel > TracerMotionCmd::max_angular_velocity)
angular_vel = TracerMotionCmd::max_angular_velocity;
if (linear_vel < TracerMotionCmd::min_linear_velocity)
linear_vel = TracerMotionCmd::min_linear_velocity;
if (linear_vel > TracerMotionCmd::max_linear_velocity)
linear_vel = TracerMotionCmd::max_linear_velocity;
if (angular_vel < TracerMotionCmd::min_angular_velocity)
angular_vel = TracerMotionCmd::min_angular_velocity;
if (angular_vel > TracerMotionCmd::max_angular_velocity)
angular_vel = TracerMotionCmd::max_angular_velocity;
std::lock_guard<std::mutex> guard(motion_cmd_mutex_);
current_motion_cmd_.linear_velocity_H = static_cast<int16_t>(linear_vel*1000)>>8;
current_motion_cmd_.linear_velocity_L = static_cast<int16_t>(linear_vel*1000)&0xff;
current_motion_cmd_.angular_velocity_H = static_cast<int16_t>(angular_vel*1000)>>8;
current_motion_cmd_.angular_velocity_L = static_cast<int16_t>(angular_vel*1000)&0xff;
current_motion_cmd_.fault_clear_flag = fault_clr_flag;
}
else
{
if (linear_vel < UartTracerMotionCmd::min_linear_velocity)
linear_vel = UartTracerMotionCmd::min_linear_velocity;
if (linear_vel > UartTracerMotionCmd::max_linear_velocity)
linear_vel = UartTracerMotionCmd::max_linear_velocity;
if (angular_vel < UartTracerMotionCmd::min_angular_velocity)
angular_vel = UartTracerMotionCmd::min_angular_velocity;
if (angular_vel > UartTracerMotionCmd::max_angular_velocity)
angular_vel = UartTracerMotionCmd::max_angular_velocity;
std::lock_guard<std::mutex> guard(motion_cmd_mutex_);
current_motion_cmd_.linear_velocity = static_cast<int8_t>(linear_vel / TracerMotionCmd::max_linear_velocity * 100.0);
current_motion_cmd_.angular_velocity = static_cast<int8_t>(angular_vel / TracerMotionCmd::max_angular_velocity * 100.0);
current_motion_cmd_.fault_clear_flag = fault_clr_flag;
std::lock_guard<std::mutex> guard(motion_cmd_mutex_);
uart_current_motion_cmd_.linear_velocity = static_cast<int8_t>(
linear_vel / UartTracerMotionCmd::max_linear_velocity * 100.0);
uart_current_motion_cmd_.angular_velocity = static_cast<int8_t>(
angular_vel / UartTracerMotionCmd::max_angular_velocity * 100.0);
//uart_current_motion_cmd_.fault_clear_flag = FaultClearFlag::NO_FAULT;
//std::cout<<"linear_vel:"<<linear_vel<<std::endl;
}
FeedCmdTimeoutWatchdog();
}
void TracerBase::SetLightCommand(TracerLightCmd cmd)
@@ -239,6 +286,7 @@ void TracerBase::SetLightCommand(TracerLightCmd cmd)
current_light_cmd_ = cmd;
light_ctrl_enabled_ = true;
light_ctrl_requested_ = true;
FeedCmdTimeoutWatchdog();
}
void TracerBase::DisableLightCmdControl()
@@ -250,16 +298,9 @@ void TracerBase::DisableLightCmdControl()
void TracerBase::ParseCANFrame(can_frame *rx_frame)
{
// validate checksum, discard frame if fails
if (!rx_frame->data[7] == CalcTracerCANChecksum(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
TracerMessage status_msg;
DecodeTracerMsgFromCAN(rx_frame, &status_msg);
DecodeTracerMsgFromCAN(rx_frame, &status_msg);//assigned a value to status_msg from can include status_msg->type
NewStatusMsgReceivedCallback(status_msg);
}
@@ -269,13 +310,12 @@ void TracerBase::ParseUARTBuffer(uint8_t *buf, const size_t bufsize, size_t byte
// serial_parser_.PrintStatistics();
// serial_parser_.ParseBuffer(buf, bytes_received);
// TODO
// TracerMessage status_msg;
// for (int i = 0; i < bytes_received; ++i)
// {
// if (DecodeTracerMsgFromUART(buf[i], &status_msg))
// NewStatusMsgReceivedCallback(status_msg);
// }
UartTracerMessage status_msg;
for (int i = 0; i < bytes_received; ++i)
{
if (DecodeTracerMsgFromUART(buf[i], &status_msg))
UartNewStatusMsgReceivedCallback(status_msg);
}
}
void TracerBase::NewStatusMsgReceivedCallback(const TracerMessage &msg)
@@ -285,6 +325,13 @@ void TracerBase::NewStatusMsgReceivedCallback(const TracerMessage &msg)
UpdateTracerState(msg, tracer_state_);
}
void TracerBase::UartNewStatusMsgReceivedCallback(const UartTracerMessage &msg)
{
// std::cout << "new status msg received" << std::endl;
std::lock_guard<std::mutex> guard(tracer_state_mutex_);
UartUpdateTracerState(msg, uart_tracer_state_);
}
void TracerBase::UpdateTracerState(const TracerMessage &status_msg, TracerState &state)
{
switch (status_msg.type)
@@ -307,8 +354,8 @@ void TracerBase::UpdateTracerState(const TracerMessage &status_msg, TracerState
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;
//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 TracerSystemStatusMsg:
@@ -318,21 +365,101 @@ void TracerBase::UpdateTracerState(const TracerMessage &status_msg, TracerState
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);
state.fault_code = msg.data.status.fault_code;
break;
}
case TracerMotorDriverStatusMsg:
{
// std::cout << "motor 1 driver feedback received" << std::endl;
const MotorDriverStatusMessage &msg = status_msg.body.motor_driver_status_msg;
for (int i = 0; i < 4; ++i)
//const MotorDriverStatusMessage &msg = status_msg.body.motor_driver_status_msg;
const MotorHeightSpeedStatusMessage &msg = status_msg.body.motor_heigh_speed_msg;
for (int i = 0; i < 2; ++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;
//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_heigh_speed_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;
}
case TracerOdometerMsg:
{
// std::cout << "Odometer msg feedback received" << std::endl;
const OdometerMessage &msg = status_msg.body.odom_msg;
state.right_odomter=static_cast<int32_t>((static_cast<uint32_t>(msg.data.status.rightodometer.lowest))|(static_cast<uint32_t>(msg.data.status.rightodometer.sec_lowest)<<8)|(static_cast<uint32_t>(msg.data.status.rightodometer.sec_highest)<<16)|(static_cast<uint32_t>(msg.data.status.rightodometer.highest)<<24));
state.left_odomter=static_cast<int32_t>((static_cast<uint32_t>(msg.data.status.leftodometer.lowest))|(static_cast<uint32_t>(msg.data.status.leftodometer.sec_lowest)<<8)|(static_cast<uint32_t>(msg.data.status.leftodometer.sec_highest)<<16)|(static_cast<uint32_t>(msg.data.status.leftodometer.highest)<<24));
}
}
}
void TracerBase::UartUpdateTracerState(const UartTracerMessage &status_msg, UartTracerState &state)
{
switch (status_msg.type) {
case UartTracerMotionStatusMsg: {
// std::cout << "motion control feedback received" << std::endl;
const UartMotionStatusMessage &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 UartTracerLightStatusMsg: {
// std::cout << "light control feedback received" << std::endl;
const UartLightStatusMessage &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 UartTracerSystemStatusMsg: {
// std::cout << "system status feedback received" << std::endl;
const UartSystemStatusMessage &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 UartTracerMotorDriverStatusMsg: {
// std::cout << "motor 1 driver feedback received" << std::endl;
const UartMotorDriverStatusMessage &msg =
status_msg.body.motor_driver_status_msg;
for (int i = 0; i < UartTracerState::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;
}
default:break;
}
}
} // namespace westonrobot

32
ugv_sdk/src/tracer_can_parser.c
View File

@@ -7,11 +7,12 @@
* Copyright (c) 2020 Ruixiang Du (rdu)
*/
#include "tracer_protocol/tracer_can_parser.h"
#include "ugv_sdk/tracer/tracer_can_parser.h"
#include "string.h"
static void EncodeTracerMotionControlMsgToCAN(const MotionCmdMessage *msg, struct can_frame *tx_frame);
static void EncodeTracerModeControlMsgToCAN(const ModSelectMessage *msg, struct can_frame *tx_frame);
bool DecodeTracerMsgFromCAN(const struct can_frame *rx_frame, TracerMessage *msg)
{
@@ -36,14 +37,18 @@ bool DecodeTracerMsgFromCAN(const struct can_frame *rx_frame, TracerMessage *msg
{
msg->type = TracerMotorDriverStatusMsg;
msg->body.motor_driver_status_msg.motor_id = TRACER_MOTOR1_ID;
msg->body.motor_heigh_speed_msg.motor_id=TRACER_MOTOR1_ID;
memcpy(msg->body.motor_driver_status_msg.data.raw, rx_frame->data, rx_frame->can_dlc * sizeof(uint8_t));
memcpy(msg->body.motor_heigh_speed_msg.data.raw, rx_frame->data, rx_frame->can_dlc * sizeof(uint8_t));
break;
}
case CAN_MSG_MOTOR2_DRIVER_STATUS_ID:
{
msg->type = TracerMotorDriverStatusMsg;
msg->body.motor_driver_status_msg.motor_id = TRACER_MOTOR2_ID;
msg->body.motor_heigh_speed_msg.motor_id = TRACER_MOTOR2_ID;
memcpy(msg->body.motor_driver_status_msg.data.raw, rx_frame->data, rx_frame->can_dlc * sizeof(uint8_t));
memcpy(msg->body.motor_heigh_speed_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
@@ -53,6 +58,12 @@ bool DecodeTracerMsgFromCAN(const struct can_frame *rx_frame, TracerMessage *msg
memcpy(msg->body.motion_cmd_msg.data.raw, rx_frame->data, rx_frame->can_dlc * sizeof(uint8_t));
break;
}
case CAN_MSG_ODOMETER_ID:
{
msg->type = TracerOdometerMsg;
memcpy(msg->body.odom_msg.data.raw, rx_frame->data, rx_frame->can_dlc * sizeof(uint8_t));
break;
}
default:
break;
}
@@ -94,10 +105,15 @@ void EncodeTracerMsgToCAN(const TracerMessage *msg, struct can_frame *tx_frame)
EncodeTracerMotionControlMsgToCAN(&(msg->body.motion_cmd_msg), tx_frame);
break;
}
case TracerModeControlMsg:
{
EncodeTracerModeControlMsgToCAN(&(msg->body.mode_cmd_msg), tx_frame);
break;
}
default:
break;
}
tx_frame->data[7] = CalcTracerCANChecksum(tx_frame->can_id, tx_frame->data, tx_frame->can_dlc);
//tx_frame->data[7] = CalcTracerCANChecksum(tx_frame->can_id, tx_frame->data, tx_frame->can_dlc);
}
void EncodeTracerMotionControlMsgToCAN(const MotionCmdMessage *msg, struct can_frame *tx_frame)
@@ -105,9 +121,15 @@ void EncodeTracerMotionControlMsgToCAN(const MotionCmdMessage *msg, struct can_f
tx_frame->can_id = CAN_MSG_MOTION_CMD_ID;
tx_frame->can_dlc = 8;
memcpy(tx_frame->data, msg->data.raw, tx_frame->can_dlc);
tx_frame->data[7] = CalcTracerCANChecksum(tx_frame->can_id, tx_frame->data, tx_frame->can_dlc);
//tx_frame->data[7] = CalcTracerCANChecksum(tx_frame->can_id, tx_frame->data, tx_frame->can_dlc);
}
void EncodeTracerModeControlMsgToCAN(const ModSelectMessage *msg, struct can_frame *tx_frame)
{
tx_frame->can_id = CAN_MSG_COMTROL_MODE_ID;
tx_frame->can_dlc = 8;
memcpy(tx_frame->data, msg->data.raw, tx_frame->can_dlc);
//tx_frame->data[7] = CalcTracerCANChecksum(tx_frame->can_id, tx_frame->data, tx_frame->can_dlc);
}
uint8_t CalcTracerCANChecksum(uint16_t id, uint8_t *data, uint8_t dlc)
{
uint8_t checksum = 0x00;
@@ -115,4 +137,4 @@ uint8_t CalcTracerCANChecksum(uint16_t id, uint8_t *data, uint8_t dlc)
for (int i = 0; i < (dlc - 1); ++i)
checksum += data[i];
return checksum;
}
}

587
ugv_sdk/src/tracer_uart_parser.c Normal file
View File

@@ -0,0 +1,587 @@
#include "ugv_sdk/tracer/tracer_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
} TracerSerialDecodeState;
#define PAYLOAD_BUFFER_SIZE (TRACER_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, UartTracerMessage *msg);
static uint8_t CalcBufferedFrameChecksum();
static bool ConstructStatusMessage(UartTracerMessage *msg);
static bool ConstructControlMessage(UartTracerMessage *msg);
static void EncodeMotionControlMsgToUART(const UartMotionControlMessage *msg, uint8_t *buf, uint8_t *len);
static void EncodeLightControlMsgToUART(const UartLightControlMessage *msg, uint8_t *buf, uint8_t *len);
void EncodeTracerMsgToUART(const UartTracerMessage *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 UartTracerMotionStatusMsg:
{
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 UartTracerLightStatusMsg:
{
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 UartTracerSystemStatusMsg:
{
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 UartTracerMotorDriverStatusMsg:
{
if (msg->body.motor_driver_status_msg.motor_id == TRACER_MOTOR1_ID)
buf[4] = UART_FRAME_MOTOR1_DRIVER_STATUS_ID;
else if (msg->body.motor_driver_status_msg.motor_id == TRACER_MOTOR2_ID)
buf[4] = UART_FRAME_MOTOR2_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 UartTracerMotionControlMsg:
{
EncodeMotionControlMsgToUART(&(msg->body.motion_control_msg), buf, len);
break;
}
case UartTracerLightControlMsg:
{
EncodeLightControlMsgToUART(&(msg->body.light_control_msg), buf, len);
break;
}
default:
break;
}
buf[12] = CalcTracerUARTChecksum(buf, buf[2] + FRAME_SOF_LEN);
// length: SOF + Frame + Checksum
*len = buf[2] + FRAME_SOF_LEN + 1;
}
bool DecodeTracerMsgFromUART(uint8_t c, UartTracerMessage *msg)
{
static UartTracerMessage decoded_msg;
bool result = ParseChar(c, &decoded_msg);
if (result)
*msg = decoded_msg;
return result;
}
void EncodeMotionControlMsgToUART(const UartMotionControlMessage *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] = CalcTracerUARTChecksum(buf, buf[2] + FRAME_SOF_LEN);
// length: SOF + Frame + Checksum
*len = buf[2] + FRAME_SOF_LEN + 1;
}
void EncodeLightControlMsgToUART(const UartLightControlMessage *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_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] = CalcTracerUARTChecksum(buf, buf[2] + FRAME_SOF_LEN);
// length: SOF + Frame + Checksum
*len = buf[2] + FRAME_SOF_LEN + 1;
}
bool ParseChar(uint8_t c, UartTracerMessage *msg)
{
static TracerSerialDecodeState 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_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(UartTracerMessage *msg)
{
if (msg == NULL)
return false;
switch (uart_parsing_data.frame_id)
{
case UART_FRAME_MOTION_CONTROL_ID:
{
msg->type = UartTracerMotionControlMsg;
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 = UartTracerLightControlMsg;
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(UartTracerMessage *msg)
{
if (msg == NULL)
return false;
switch (uart_parsing_data.frame_id)
{
case UART_FRAME_SYSTEM_STATUS_ID:
{
msg->type = UartTracerSystemStatusMsg;
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 = UartTracerMotionStatusMsg;
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 = UartTracerMotorDriverStatusMsg;
msg->body.motor_driver_status_msg.motor_id = TRACER_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 = UartTracerMotorDriverStatusMsg;
msg->body.motor_driver_status_msg.motor_id = TRACER_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_LIGHT_STATUS_ID:
{
msg->type = UartTracerLightStatusMsg;
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.checksum = uart_parsing_data.frame_checksum;
break;
}
}
return true;
}
uint8_t CalcTracerUARTChecksum(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;
}