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

updated v1 protocol

Browse Source
This commit is contained in:
Ruixiang Du
2021-07-09 21:22:56 +08:00
parent 0f6ce71313
commit 0565639c8f
19 changed files with 227420 additions and 290 deletions
Download Patch File Download Diff File Expand all files Collapse all files

197
src/protocol_v1/agx_protocol_v1.h Normal file
View File

@@ -0,0 +1,197 @@
/*
* agx_protocol_v1.h
*
* Created on: Jul 09, 2021 20:34
* Description:
*
* Copyright (c) 2021 Ruixiang Du (rdu)
*/
#ifndef AGX_PROTOCOL_V1_H
#define AGX_PROTOCOL_V1_H
#ifdef __cplusplus
extern "C" {
#endif
#include <stdint.h>
// define endianess of the platform
#if (!defined(USE_LITTLE_ENDIAN) && !defined(USE_BIG_ENDIAN))
#define USE_LITTLE_ENDIAN
#endif
#ifdef USE_BIG_ENDIAN
#error "BIG ENDIAN IS CURRENTLY NOT SUPPORTED"
#endif
/*---------------------------- Motor IDs -------------------------------*/
#define ACTUATOR1_ID ((uint8_t)0x00)
#define ACTUATOR2_ID ((uint8_t)0x01)
#define ACTUATOR3_ID ((uint8_t)0x02)
#define ACTUATOR4_ID ((uint8_t)0x03)
/*--------------------------- Message IDs ------------------------------*/
// CAN: control group
#define CAN_MSG_MOTION_COMMAND_ID ((uint32_t)0x130)
#define CAN_MSG_LIGHT_COMMAND_ID ((uint32_t)0x140)
#define CAN_MSG_VALUE_SET_COMMAND_ID ((uint32_t)0x211)
// CAN: state feedback group
#define CAN_MSG_MOTION_STATE_ID ((uint32_t)0x131)
#define CAN_MSG_LIGHT_STATE_ID ((uint32_t)0x141)
#define CAN_MSG_SYSTEM_STATE_ID ((uint32_t)0x151)
#define CAN_MSG_ACTUATOR1_STATE_ID ((uint32_t)0x200)
#define CAN_MSG_ACTUATOR2_STATE_ID ((uint32_t)0x201)
#define CAN_MSG_ACTUATOR3_STATE_ID ((uint32_t)0x202)
#define CAN_MSG_ACTUATOR4_STATE_ID ((uint32_t)0x203)
/*------------------------ Frame Memory Layout -------------------------*/
/* No padding in the struct */
// reference: https://stackoverflow.com/questions/3318410/pragma-pack-effect
#pragma pack(push, 1)
#ifdef USE_LITTLE_ENDIAN
typedef struct {
uint8_t high_byte;
uint8_t low_byte;
} struct16_t;
typedef struct {
uint8_t msb;
uint8_t high_byte;
uint8_t low_byte;
uint8_t lsb;
} struct32_t;
#elif defined(USE_BIG_ENDIAN)
typedef struct {
uint8_t low_byte;
uint8_t high_byte;
} struct16_t;
typedef struct {
uint8_t lsb;
uint8_t low_byte;
uint8_t high_byte;
uint8_t msb;
} struct32_t;
#endif
// Control messages
#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 ERROR_CLR_NONE ((uint8_t)0x00)
#define ERROR_CLR_BAT_UNDER_VOL ((uint8_t)0x01)
#define ERROR_CLR_BAT_OVER_VOL ((uint8_t)0x02)
#define ERROR_CLR_MOTOR1_COMM ((uint8_t)0x03)
#define ERROR_CLR_MOTOR2_COMM ((uint8_t)0x04)
#define ERROR_CLR_MOTOR3_COMM ((uint8_t)0x05)
#define ERROR_CLR_MOTOR4_COMM ((uint8_t)0x06)
#define ERROR_CLR_MOTOR_DRV_OVERHEAT ((uint8_t)0x07)
#define ERROR_CLR_MOTOR_OVERCURRENT ((uint8_t)0x08)
typedef struct {
uint8_t control_mode;
uint8_t error_clear_byte;
int8_t linear_percentage;
int8_t angular_percentage;
uint8_t reserved0;
uint8_t reserved1;
uint8_t count;
uint8_t checksum;
} MotionCommandFrame;
#define LIGHT_ENABLE_CMD_CTRL ((uint8_t)0x01)
#define LIGHT_DISABLE_CMD_CTRL ((uint8_t)0x00)
#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)
typedef struct {
uint8_t enable_cmd_ctrl;
uint8_t front_mode;
uint8_t front_custom;
uint8_t rear_mode;
uint8_t rear_custom;
uint8_t reserved0;
uint8_t count;
uint8_t checksum;
} LightCommandFrame;
typedef struct {
uint8_t set_neutral;
uint8_t reserved0;
uint8_t reserved1;
uint8_t reserved2;
uint8_t reserved3;
uint8_t reserved4;
uint8_t count;
uint8_t checksum;
} ValueSetCommandFrame;
// State feedback messages
#define VEHICLE_STATE_NORMAL ((uint8_t)0x00)
#define VEHICLE_STATE_ESTOP ((uint8_t)0x01)
#define VEHICLE_STATE_EXCEPTION ((uint8_t)0x02)
#define ERROR_CAN_CHECKSUM_ERROR ((uint16_t)0x0100)
#define ERROR_MOTOR_DRV_OVERHEAT_W ((uint16_t)0x0200)
#define ERROR_MOTOR_OVERCURRENT_W ((uint16_t)0x0400)
#define ERROR_BAT_UNDER_VOL_W ((uint16_t)0x0800)
#define ERROR_RC_SIGNAL_LOSS ((uint16_t)0x1000)
#define ERROR_HIGH_BYTE_RESERVED2 ((uint16_t)0x2000)
#define ERROR_HIGH_BYTE_RESERVED3 ((uint16_t)0x4000)
#define ERROR_HIGH_BYTE_RESERVED4 ((uint16_t)0x8000)
#define ERROR_BAT_UNDER_VOL_F ((uint16_t)0x0001)
#define ERROR_BAT_OVER_VOL_F ((uint16_t)0x0002)
#define ERROR_MOTOR1_COMM_F ((uint16_t)0x0004)
#define ERROR_MOTOR2_COMM_F ((uint16_t)0x0008)
#define ERROR_MOTOR3_COMM_F ((uint16_t)0x0010)
#define ERROR_MOTOR4_COMM_F ((uint16_t)0x0020)
#define ERROR_MOTOR_DRV_OVERHEAT_F ((uint16_t)0x0040)
#define ERROR_MOTOR_OVERCURRENT_F ((uint16_t)0x0080)
typedef struct {
uint8_t vehicle_state;
uint8_t control_mode;
struct16_t battery_voltage;
struct16_t error_code;
uint8_t count;
uint8_t checksum;
} SystemStateFrame;
typedef struct {
struct16_t linear;
struct16_t angular;
uint8_t reserved0;
uint8_t reserved1;
uint8_t count;
uint8_t checksum;
} MotionStateFrame;
typedef LightCommandFrame LightStateFrame;
typedef struct {
struct16_t current;
struct16_t rpm;
int8_t temperature;
uint8_t reserved0;
uint8_t count;
uint8_t checksum;
} ActuatorStateFrame;
#pragma pack(pop)
#ifdef __cplusplus
}
#endif
#endif /* AGX_PROTOCOL_V1_H */

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

@@ -1,11 +1,11 @@
/*
/*
* scout_protocol.h
*
*
* Created on: Aug 07, 2019 21:49
* Description:
*
* Description:
*
* Copyright (c) 2019 Ruixiang Du (rdu)
*/
*/
#ifndef SCOUT_PROTOCOL_H
#define SCOUT_PROTOCOL_H
@@ -16,87 +16,87 @@ extern "C" {
#include <stdint.h>
#define SCOUT_CMD_BUF_LEN 32
#define SCOUT_STATUS_BUF_LEN 32
#define SCOUT_FRAME_SIZE 13
#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)
#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_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)
#define UART_FRAME_MOTION_CONTROL_ID ((uint8_t)0x01)
#define UART_FRAME_LIGHT_CONTROL_ID ((uint8_t)0x02)
// CAN Definitions
#define CAN_MSG_MOTION_CONTROL_CMD_ID ((uint32_t)0x130)
#define CAN_MSG_MOTION_CONTROL_STATUS_ID ((uint32_t)0x131)
#define CAN_MSG_LIGHT_CONTROL_CMD_ID ((uint32_t)0x140)
#define CAN_MSG_LIGHT_CONTROL_STATUS_ID ((uint32_t)0x141)
#define CAN_MSG_SYSTEM_STATUS_STATUS_ID ((uint32_t)0x151)
#define CAN_MSG_MOTOR1_DRIVER_STATUS_ID ((uint32_t)0x200)
#define CAN_MSG_MOTOR2_DRIVER_STATUS_ID ((uint32_t)0x201)
#define CAN_MSG_MOTOR3_DRIVER_STATUS_ID ((uint32_t)0x202)
#define CAN_MSG_MOTOR4_DRIVER_STATUS_ID ((uint32_t)0x203)
#define CAN_MSG_MOTION_CONTROL_CMD_ID ((uint32_t)0x130)
#define CAN_MSG_MOTION_CONTROL_STATUS_ID ((uint32_t)0x131)
#define CAN_MSG_LIGHT_CONTROL_CMD_ID ((uint32_t)0x140)
#define CAN_MSG_LIGHT_CONTROL_STATUS_ID ((uint32_t)0x141)
#define CAN_MSG_SYSTEM_STATUS_STATUS_ID ((uint32_t)0x151)
#define CAN_MSG_MOTOR1_DRIVER_STATUS_ID ((uint32_t)0x200)
#define CAN_MSG_MOTOR2_DRIVER_STATUS_ID ((uint32_t)0x201)
#define CAN_MSG_MOTOR3_DRIVER_STATUS_ID ((uint32_t)0x202)
#define CAN_MSG_MOTOR4_DRIVER_STATUS_ID ((uint32_t)0x203)
/*--------------------- Control/State Constants ------------------------*/
// Motion Control
#define CTRL_MODE_REMOTE ((uint8_t)0x00)
#define CTRL_MODE_CMD_CAN ((uint8_t)0x01)
#define CTRL_MODE_CMD_UART ((uint8_t)0x02)
#define CTRL_MODE_COMMANDED ((uint8_t)0x03)
// #define 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)
#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_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)
#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 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_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)
#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 -----------------------*/
@@ -105,165 +105,144 @@ extern "C" {
#pragma pack(push, 1)
// Note: id could be different for UART and CAN protocol
// Motion Control
typedef struct {
union
{
struct
{
uint8_t control_mode;
uint8_t fault_clear_flag;
int8_t linear_velocity_cmd;
int8_t angular_velocity_cmd;
uint8_t reserved0;
uint8_t reserved1;
uint8_t count;
uint8_t checksum;
} cmd;
uint8_t raw[8];
} data;
union {
struct {
uint8_t control_mode;
uint8_t fault_clear_flag;
int8_t linear_velocity_cmd;
int8_t angular_velocity_cmd;
uint8_t reserved0;
uint8_t reserved1;
uint8_t count;
uint8_t checksum;
} cmd;
uint8_t raw[8];
} data;
} MotionControlMessage;
typedef struct {
union
{
struct
{
struct
{
uint8_t high_byte;
uint8_t low_byte;
} linear_velocity;
struct
{
uint8_t high_byte;
uint8_t low_byte;
} angular_velocity;
uint8_t reserved0;
uint8_t reserved1;
uint8_t count;
uint8_t checksum;
} status;
uint8_t raw[8];
} data;
union {
struct {
struct {
uint8_t high_byte;
uint8_t low_byte;
} linear_velocity;
struct {
uint8_t high_byte;
uint8_t low_byte;
} angular_velocity;
uint8_t reserved0;
uint8_t reserved1;
uint8_t count;
uint8_t checksum;
} status;
uint8_t raw[8];
} data;
} MotionStatusMessage;
// System Status Feedback
typedef struct {
union
{
struct
{
uint8_t base_state;
uint8_t control_mode;
struct
{
uint8_t high_byte;
uint8_t low_byte;
} battery_voltage;
struct
{
uint8_t high_byte;
uint8_t low_byte;
} fault_code;
uint8_t count;
uint8_t checksum;
} status;
uint8_t raw[8];
} data;
union {
struct {
uint8_t base_state;
uint8_t control_mode;
struct {
uint8_t high_byte;
uint8_t low_byte;
} battery_voltage;
struct {
uint8_t high_byte;
uint8_t low_byte;
} fault_code;
uint8_t count;
uint8_t checksum;
} status;
uint8_t raw[8];
} data;
} SystemStatusMessage;
// Light Control
typedef struct {
union
{
struct
{
uint8_t light_ctrl_enable;
uint8_t front_light_mode;
uint8_t front_light_custom;
uint8_t rear_light_mode;
uint8_t rear_light_custom;
uint8_t reserved0;
uint8_t count;
uint8_t checksum;
} cmd;
uint8_t raw[8];
} data;
union {
struct {
uint8_t light_ctrl_enable;
uint8_t front_mode;
uint8_t front_custom;
uint8_t rear_mode;
uint8_t rear_custom;
uint8_t reserved0;
uint8_t count;
uint8_t checksum;
} cmd;
uint8_t raw[8];
} data;
} LightControlMessage;
typedef struct {
union
{
struct
{
uint8_t light_ctrl_enable;
uint8_t front_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;
union {
struct {
uint8_t light_ctrl_enable;
uint8_t front_mode;
uint8_t front_custom;
uint8_t rear_mode;
uint8_t rear_custom;
uint8_t reserved0;
uint8_t count;
uint8_t checksum;
} status;
uint8_t raw[8];
} data;
} LightStatusMessage;
// Motor Driver Feedback
typedef struct
{
uint8_t motor_id;
union {
struct
{
struct
{
uint8_t high_byte;
uint8_t low_byte;
} current;
struct
{
uint8_t high_byte;
uint8_t low_byte;
} rpm;
int8_t temperature;
uint8_t reserved0;
uint8_t count;
uint8_t checksum;
} status;
uint8_t raw[8];
} data;
typedef struct {
uint8_t motor_id;
union {
struct {
struct {
uint8_t high_byte;
uint8_t low_byte;
} current;
struct {
uint8_t high_byte;
uint8_t low_byte;
} rpm;
int8_t temperature;
uint8_t reserved0;
uint8_t count;
uint8_t checksum;
} status;
uint8_t raw[8];
} data;
} MotorDriverStatusMessage;
// For convenience to access status/control message
typedef enum
{
ScoutMsgNone = 0x00,
// status messages
ScoutMotionStatusMsg = 0x01,
ScoutLightStatusMsg = 0x02,
ScoutSystemStatusMsg = 0x03,
ScoutMotorDriverStatusMsg = 0x04,
// control messages
ScoutMotionControlMsg = 0x21,
ScoutLightControlMsg = 0x22
typedef enum {
ScoutMsgNone = 0x00,
// status messages
ScoutMotionStatusMsg = 0x01,
ScoutLightStatusMsg = 0x02,
ScoutSystemStatusMsg = 0x03,
ScoutMotorDriverStatusMsg = 0x04,
// control messages
ScoutMotionControlMsg = 0x21,
ScoutLightControlMsg = 0x22
} ScoutMsgType;
typedef struct
{
ScoutMsgType type;
union {
// status messages
MotionStatusMessage motion_status_msg;
LightStatusMessage light_status_msg;
SystemStatusMessage system_status_msg;
MotorDriverStatusMessage motor_driver_status_msg;
// control messages
MotionControlMessage motion_control_msg;
LightControlMessage light_control_msg;
} body;
typedef struct {
ScoutMsgType type;
union {
// status messages
MotionStatusMessage motion_status_msg;
LightStatusMessage light_status_msg;
SystemStatusMessage system_status_msg;
MotorDriverStatusMessage motor_driver_status_msg;
// control messages
MotionControlMessage motion_control_msg;
LightControlMessage light_control_msg;
} body;
} ScoutMessage;
#pragma pack(pop)

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

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

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

@@ -109,10 +109,10 @@ void EncodeScoutMsgToUART(const ScoutMessage *msg, uint8_t *buf, uint8_t *len)
{
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[6] = msg->body.light_status_msg.data.status.front_mode;
buf[7] = msg->body.light_status_msg.data.status.front_custom;
buf[8] = msg->body.light_status_msg.data.status.rear_mode;
buf[9] = msg->body.light_status_msg.data.status.rear_custom;
buf[10] = 0;
buf[11] = msg->body.light_status_msg.data.status.count;
break;
@@ -218,10 +218,10 @@ void EncodeLightControlMsgToUART(const LightControlMessage *msg, uint8_t *buf, u
// 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[6] = msg->data.cmd.front_mode;
buf[7] = msg->data.cmd.front_custom;
buf[8] = msg->data.cmd.rear_mode;
buf[9] = msg->data.cmd.rear_custom;
buf[10] = 0x00;
// frame count, checksum
@@ -467,10 +467,10 @@ bool ConstructControlMessage(ScoutMessage *msg)
{
msg->type = ScoutLightControlMsg;
msg->body.light_control_msg.data.cmd.light_ctrl_enable = uart_parsing_data.payload_buffer[0];
msg->body.light_control_msg.data.cmd.front_light_mode = uart_parsing_data.payload_buffer[1];
msg->body.light_control_msg.data.cmd.front_light_custom = uart_parsing_data.payload_buffer[2];
msg->body.light_control_msg.data.cmd.rear_light_mode = uart_parsing_data.payload_buffer[3];
msg->body.light_control_msg.data.cmd.rear_light_custom = uart_parsing_data.payload_buffer[4];
msg->body.light_control_msg.data.cmd.front_mode = uart_parsing_data.payload_buffer[1];
msg->body.light_control_msg.data.cmd.front_custom = uart_parsing_data.payload_buffer[2];
msg->body.light_control_msg.data.cmd.rear_mode = uart_parsing_data.payload_buffer[3];
msg->body.light_control_msg.data.cmd.rear_custom = uart_parsing_data.payload_buffer[4];
msg->body.light_control_msg.data.cmd.reserved0 = uart_parsing_data.payload_buffer[5];
msg->body.light_control_msg.data.cmd.count = uart_parsing_data.frame_cnt;
msg->body.light_control_msg.data.cmd.checksum = uart_parsing_data.frame_checksum;
@@ -573,10 +573,10 @@ bool ConstructStatusMessage(ScoutMessage *msg)
{
msg->type = ScoutLightStatusMsg;
msg->body.light_status_msg.data.status.light_ctrl_enable = uart_parsing_data.payload_buffer[0];
msg->body.light_status_msg.data.status.front_light_mode = uart_parsing_data.payload_buffer[1];
msg->body.light_status_msg.data.status.front_light_custom = uart_parsing_data.payload_buffer[2];
msg->body.light_status_msg.data.status.rear_light_mode = uart_parsing_data.payload_buffer[3];
msg->body.light_status_msg.data.status.rear_light_custom = uart_parsing_data.payload_buffer[4];
msg->body.light_status_msg.data.status.front_mode = uart_parsing_data.payload_buffer[1];
msg->body.light_status_msg.data.status.front_custom = uart_parsing_data.payload_buffer[2];
msg->body.light_status_msg.data.status.rear_mode = uart_parsing_data.payload_buffer[3];
msg->body.light_status_msg.data.status.rear_custom = uart_parsing_data.payload_buffer[4];
msg->body.light_status_msg.data.status.reserved0 = 0x00;
msg->body.light_status_msg.data.status.count = uart_parsing_data.frame_cnt;
msg->body.light_status_msg.data.status.checksum = uart_parsing_data.frame_checksum;

48
src/protocol_v2/agx_msg_parser.c
View File

@@ -44,14 +44,14 @@ bool DecodeCanFrame(const struct can_frame *rx_frame, AgxMessage *msg) {
msg->type = AgxMsgLightCommand;
// parse frame buffer to message
LightCommandFrame *frame = (LightCommandFrame *)(rx_frame->data);
msg->body.light_command_msg.cmd_ctrl_allowed =
(frame->cmd_ctrl_allowed != 0) ? true : false;
msg->body.light_command_msg.front_light.mode = frame->front_light_mode;
msg->body.light_command_msg.enable_cmd_ctrl =
(frame->enable_cmd_ctrl != 0) ? true : false;
msg->body.light_command_msg.front_light.mode = frame->front_mode;
msg->body.light_command_msg.front_light.custom_value =
frame->front_light_custom;
msg->body.light_command_msg.rear_light.mode = frame->rear_light_mode;
frame->front_custom;
msg->body.light_command_msg.rear_light.mode = frame->rear_mode;
msg->body.light_command_msg.rear_light.custom_value =
frame->rear_light_custom;
frame->rear_custom;
break;
}
case CAN_MSG_BRAKING_COMMAND_ID: {
@@ -98,14 +98,14 @@ bool DecodeCanFrame(const struct can_frame *rx_frame, AgxMessage *msg) {
case CAN_MSG_LIGHT_STATE_ID: {
msg->type = AgxMsgLightState;
LightStateFrame *frame = (LightStateFrame *)(rx_frame->data);
msg->body.light_command_msg.cmd_ctrl_allowed =
(frame->cmd_ctrl_allowed != 0) ? true : false;
msg->body.light_command_msg.front_light.mode = frame->front_light_mode;
msg->body.light_command_msg.enable_cmd_ctrl =
(frame->enable_cmd_ctrl != 0) ? true : false;
msg->body.light_command_msg.front_light.mode = frame->front_mode;
msg->body.light_command_msg.front_light.custom_value =
frame->front_light_custom;
msg->body.light_command_msg.rear_light.mode = frame->rear_light_mode;
frame->front_custom;
msg->body.light_command_msg.rear_light.mode = frame->rear_mode;
msg->body.light_command_msg.rear_light.custom_value =
frame->rear_light_custom;
frame->rear_custom;
break;
}
case CAN_MSG_RC_STATE_ID: {
@@ -340,20 +340,20 @@ void EncodeCanFrame(const AgxMessage *msg, struct can_frame *tx_frame) {
tx_frame->can_id = CAN_MSG_LIGHT_COMMAND_ID;
tx_frame->can_dlc = 8;
LightCommandFrame frame;
if (msg->body.light_command_msg.cmd_ctrl_allowed) {
frame.cmd_ctrl_allowed = LIGHT_CMD_CTRL_ALLOWED;
frame.front_light_mode = msg->body.light_command_msg.front_light.mode;
frame.front_light_custom =
if (msg->body.light_command_msg.enable_cmd_ctrl) {
frame.enable_cmd_ctrl = LIGHT_ENABLE_CMD_CTRL;
frame.front_mode = msg->body.light_command_msg.front_light.mode;
frame.front_custom =
msg->body.light_command_msg.front_light.custom_value;
frame.rear_light_mode = msg->body.light_command_msg.rear_light.mode;
frame.rear_light_custom =
frame.rear_mode = msg->body.light_command_msg.rear_light.mode;
frame.rear_custom =
msg->body.light_command_msg.rear_light.custom_value;
} else {
frame.cmd_ctrl_allowed = LIGHT_CMD_CTRL_DISALLOWED;
frame.front_light_mode = 0;
frame.front_light_custom = 0;
frame.rear_light_mode = 0;
frame.rear_light_custom = 0;
frame.enable_cmd_ctrl = LIGHT_DISABLE_CMD_CTRL;
frame.front_mode = 0;
frame.front_custom = 0;
frame.rear_mode = 0;
frame.rear_custom = 0;
}
frame.reserved0 = 0;
frame.reserved1 = 0;
@@ -363,7 +363,7 @@ void EncodeCanFrame(const AgxMessage *msg, struct can_frame *tx_frame) {
}
case AgxMsgBrakingCommand: {
tx_frame->can_id = CAN_MSG_BRAKING_COMMAND_ID;
tx_frame->can_dlc = 8;
tx_frame->can_dlc = 2;
BrakingCommandFrame frame;
// TODO
memcpy(tx_frame->data, (uint8_t *)(&frame), tx_frame->can_dlc);

61
src/protocol_v2/agx_protocol_v2.h
View File

@@ -42,7 +42,7 @@ extern "C" {
#define CAN_MSG_MOTION_COMMAND_ID ((uint32_t)0x111)
#define CAN_MSG_LIGHT_COMMAND_ID ((uint32_t)0x121)
#define CAN_MSG_BRAKING_COMMAND_ID ((uint32_t)0x131)
#define CAN_MSG_SET_MOTION_MODE_ID ((uint32_t)0x141)
#define CAN_MSG_SET_MOTION_MODE_ID ((uint32_t)0x141)
// state feedback group: 0x2
#define CAN_MSG_SYSTEM_STATE_ID ((uint32_t)0x211)
@@ -68,9 +68,9 @@ extern "C" {
#define CAN_MSG_ACTUATOR7_LS_STATE_ID ((uint32_t)0x267)
#define CAN_MSG_ACTUATOR8_LS_STATE_ID ((uint32_t)0x268)
#define CAN_MSG_MOTOR_ANGLE_INFO ((uint32_t)0x271)
#define CAN_MSG_MOTOR_SPEED_INFO ((uint32_t)0x281)
#define CAN_MSG_CURRENT_CTRL_MODE ((uint32_t)0x291)
#define CAN_MSG_MOTOR_ANGLE_INFO ((uint32_t)0x271)
#define CAN_MSG_MOTOR_SPEED_INFO ((uint32_t)0x281)
#define CAN_MSG_CURRENT_CTRL_MODE ((uint32_t)0x291)
// sensor data group: 0x3
#define CAN_MSG_ODOMETRY_ID ((uint32_t)0x311)
@@ -109,16 +109,6 @@ extern "C" {
#define CAN_MSG_STATE_RESET_CONFIG_ID ((uint32_t)0x441)
/*--------------------- Control/State Constants ------------------------*/
#define LIGHT_CMD_CTRL_ALLOWED ((uint8_t)0x01)
#define LIGHT_CMD_CTRL_DISALLOWED ((uint8_t)0x00)
#define VERSION_REQUEST_VALUE ((uint8_t)0x01)
#define STEER_NEUTRAL_REQUEST_VALUE ((uint8_t)0xee)
#define STEER_NEUTRAL_RESPONSE_SUCCESS_VALUE ((uint8_t)0xee)
#define STEER_NEUTRAL_RESPONSE_FAILURE_VALUE ((uint8_t)0xff)
/*------------------------ Frame Memory Layout -------------------------*/
/* No padding in the struct */
@@ -157,30 +147,26 @@ typedef struct {
struct16_t steering_angle;
} MotionCommandFrame;
#define LIGHT_ENABLE_CMD_CTRL ((uint8_t)0x01)
#define LIGHT_DISABLE_CMD_CTRL ((uint8_t)0x00)
typedef struct {
uint8_t cmd_ctrl_allowed;
uint8_t front_light_mode;
uint8_t front_light_custom;
uint8_t rear_light_mode;
uint8_t rear_light_custom;
uint8_t enable_cmd_ctrl;
uint8_t front_mode;
uint8_t front_custom;
uint8_t rear_mode;
uint8_t rear_custom;
uint8_t reserved0;
uint8_t reserved1;
uint8_t count;
} LightCommandFrame;
typedef struct {
uint8_t cmd_ctrl_allowed;
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 enable_brake;
uint8_t count;
} BrakingCommandFrame;
typedef struct
{
typedef struct {
uint8_t motion_mode;
uint8_t reserved0;
uint8_t reserved1;
@@ -189,7 +175,7 @@ typedef struct
uint8_t reserved4;
uint8_t reserved5;
uint8_t reserved6;
}SetMotionModeFrame;
} SetMotionModeFrame;
// State feedback messages
typedef struct {
@@ -209,11 +195,11 @@ typedef struct {
} MotionStateFrame;
typedef struct {
uint8_t cmd_ctrl_allowed;
uint8_t front_light_mode;
uint8_t front_light_custom;
uint8_t rear_light_mode;
uint8_t rear_light_custom;
uint8_t enable_cmd_ctrl;
uint8_t front_mode;
uint8_t front_custom;
uint8_t rear_mode;
uint8_t rear_custom;
uint8_t reserved0;
uint8_t reserved1;
uint8_t count;
@@ -267,7 +253,7 @@ typedef struct {
typedef struct {
uint8_t motion_mode;
uint8_t mode_changing;
}MotionModeStateFrame;
} MotionModeStateFrame;
// sensors
typedef struct {
@@ -340,6 +326,11 @@ typedef struct {
} BmsExtendedFrame;
// query/config
#define VERSION_REQUEST_VALUE ((uint8_t)0x01)
#define STEER_NEUTRAL_REQUEST_VALUE ((uint8_t)0xee)
#define STEER_NEUTRAL_RESPONSE_SUCCESS_VALUE ((uint8_t)0xee)
#define STEER_NEUTRAL_RESPONSE_FAILURE_VALUE ((uint8_t)0xff)
typedef struct {
uint8_t request;
uint8_t reserved0;

2
src/protocol_v2/ranger_base.cpp
View File

@@ -36,7 +36,7 @@ void RangerBase::SetMotionCommand(double linear_vel, double steer_angle,
}
void RangerBase::SetLightCommand(const RangerLightCmd &cmd) {
if (cmd.cmd_ctrl_allowed) {
if (cmd.enable_cmd_ctrl) {
AgilexBase::SendLightCommand(cmd.front_mode, cmd.front_custom_value,
LightMode::CONST_OFF, 0);
}

2
src/protocol_v2/tracer_base.cpp
View File

@@ -29,7 +29,7 @@ void TracerBaseV2::SetMotionCommand(double linear_vel, double angular_vel) {
}
void TracerBaseV2::SetLightCommand(const TracerLightCmd &cmd) {
if (cmd.cmd_ctrl_allowed) {
if (cmd.enable_cmd_ctrl) {
AgilexBase::SendLightCommand(cmd.front_mode, cmd.front_custom_value,
LightMode::CONST_OFF, 0);
}