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

28276
docs/protocol_v1/BUNKER用户手册.pdf Executable file
View File

File diff suppressed because one or more lines are too long

21104
docs/protocol_v1/HUNTER用户手册.pdf Executable file
View File

File diff suppressed because one or more lines are too long

25695
docs/protocol_v1/SCOUT mini用户手册1.0协议.pdf Executable file
View File

File diff suppressed because one or more lines are too long

87035
docs/protocol_v1/SCOUT用户手册1.0协议.pdf Executable file
View File

File diff suppressed because one or more lines are too long

27793
docs/protocol_v2/SCOUT mini用户手册2.0协议.pdf Executable file
View File

File diff suppressed because one or more lines are too long

36814
docs/protocol_v2/scout 2.0用户手册_210630.pdf Executable file
View File

File diff suppressed because one or more lines are too long

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

@@ -43,7 +43,7 @@ typedef struct {
} LightOperation; } LightOperation;
typedef struct { typedef struct {
bool cmd_ctrl_allowed; bool enable_cmd_ctrl;
LightOperation front_light; LightOperation front_light;
LightOperation rear_light; LightOperation rear_light;
} LightCommandMessage; } LightCommandMessage;

4
include/ugv_sdk/details/interface/ranger_interface.hpp
View File

@@ -40,13 +40,13 @@ struct RangerLightCmd {
RangerLightCmd() = default; RangerLightCmd() = default;
RangerLightCmd(LightMode f_mode, uint8_t f_value, LightMode r_mode, RangerLightCmd(LightMode f_mode, uint8_t f_value, LightMode r_mode,
uint8_t r_value) uint8_t r_value)
: cmd_ctrl_allowed(true), : enable_cmd_ctrl(true),
front_mode(f_mode), front_mode(f_mode),
front_custom_value(f_value), front_custom_value(f_value),
rear_mode(r_mode), rear_mode(r_mode),
rear_custom_value(r_value) {} rear_custom_value(r_value) {}
bool cmd_ctrl_allowed = false; bool enable_cmd_ctrl = false;
LightMode front_mode; LightMode front_mode;
uint8_t front_custom_value; uint8_t front_custom_value;
LightMode rear_mode; LightMode rear_mode;

4
include/ugv_sdk/details/interface/tracer_interface.hpp
View File

@@ -38,11 +38,11 @@ struct TracerMotionCmd {
struct TracerLightCmd { struct TracerLightCmd {
TracerLightCmd() = default; TracerLightCmd() = default;
TracerLightCmd(LightMode f_mode, uint8_t f_value) TracerLightCmd(LightMode f_mode, uint8_t f_value)
: cmd_ctrl_allowed(true), : enable_cmd_ctrl(true),
front_mode(f_mode), front_mode(f_mode),
front_custom_value(f_value) {} front_custom_value(f_value) {}
bool cmd_ctrl_allowed = false; bool enable_cmd_ctrl = false;
LightMode front_mode; LightMode front_mode;
uint8_t front_custom_value; uint8_t front_custom_value;
}; };

167
include/ugv_sdk/details/protocol_v1/agilex_message_v1.h Normal file
View File

@@ -0,0 +1,167 @@
/*
* agilex_message_v1.h
*
* Created on: Jul 09, 2021 20:55
* Description:
* all values are using SI units (e.g. meter/second/radian)
*
* Re-implemented as a subset of AgxMessage defined in protocol v2
*
* Copyright (c) 2021 Ruixiang Du (rdu)
*/
#ifndef AGILEX_MESSAGE_V1_H
#define AGILEX_MESSAGE_V1_H
#ifdef __cplusplus
extern "C" {
#endif
#include <stdbool.h>
#include <stdint.h>
#include <string.h>
/***************** Control messages *****************/
typedef enum {
// CONTROL_MODE_STANDBY = 0x00,
CONTROL_MODE_RC = 0x00,
CONTROL_MODE_CAN = 0x01,
CONTROL_MODE_UART = 0x02
} ControlMode;
// 0x130
typedef struct {
ControlMode control_mode;
uint8_t fault_clear;
float linear;
float angular;
} MotionCommandMessage;
// 0x140
typedef enum {
CONST_OFF = 0x00,
CONST_ON = 0x01,
BREATH = 0x02,
CUSTOM = 0x03
} LightMode;
typedef struct {
LightMode mode;
uint8_t custom_value;
} LightOperation;
typedef struct {
bool enable_cmd_ctrl;
LightOperation front_light;
LightOperation rear_light;
} LightCommandMessage;
typedef struct {
bool set_neutral;
} ValueSetCommandMessage;
/**************** Feedback messages *****************/
typedef enum {
VehicleStateNormal = 0x00,
VehicleStateEStop = 0x01,
VehicleStateException = 0x02
} VehicleState;
#define SYSTEM_ERROR_MOTOR_DRIVER_MASK ((uint16_t)0x0100)
#define SYSTEM_ERROR_HL_COMM_MASK ((uint16_t)0x0200)
#define SYSTEM_ERROR_BATTERY_FAULT_MASK ((uint16_t)0x0001)
#define SYSTEM_ERROR_BATTERY_WARNING_MASK ((uint16_t)0x0002)
#define SYSTEM_ERROR_RC_SIGNAL_LOSS_MASK ((uint16_t)0x0004)
#define SYSTEM_ERROR_MOTOR1_COMM_MASK ((uint16_t)0x0008)
#define SYSTEM_ERROR_MOTOR2_COMM_MASK ((uint16_t)0x0010)
#define SYSTEM_ERROR_MOTOR3_COMM_MASK ((uint16_t)0x0020)
#define SYSTEM_ERROR_MOTOR4_COMM_MASK ((uint16_t)0x0040)
#define SYSTEM_ERROR_STEER_ENCODER_MASK ((uint16_t)0x0080)
typedef struct {
VehicleState vehicle_state;
ControlMode control_mode;
float battery_voltage;
uint16_t error_code;
} SystemStateMessage;
// 0x221
typedef struct {
float linear_velocity;
float angular_velocity; // only valid for differential drivering
float lateral_velocity;
float steering_angle; // only valid for ackermann steering
} MotionStateMessage;
// 0x231
typedef LightCommandMessage LightStateMessage;
// 0x251 - 0x258
typedef struct {
uint8_t motor_id;
int16_t rpm;
float current;
int32_t pulse_count;
} ActuatorStateMessage;
// 0x261 - 0x264
#define DRIVER_STATE_INPUT_VOLTAGE_LOW_MASK ((uint8_t)0x01)
#define DRIVER_STATE_MOTOR_OVERHEAT_MASK ((uint8_t)0x02)
#define DRIVER_STATE_DRIVER_OVERLOAD_MASK ((uint8_t)0x04)
#define DRIVER_STATE_DRIVER_OVERHEAT_MASK ((uint8_t)0x08)
#define DRIVER_STATE_SENSOR_FAULT_MASK ((uint8_t)0x10)
#define DRIVER_STATE_DRIVER_FAULT_MASK ((uint8_t)0x20)
#define DRIVER_STATE_DRIVER_ENABLED_MASK ((uint8_t)0x40)
#define DRIVER_STATE_DRIVER_RESET_MASK ((uint8_t)0x80)
// 0x291
typedef struct {
uint8_t motion_mode;
uint8_t mode_changing;
} MotionModeFeedbackMessage;
typedef struct {
uint8_t motor_id;
float driver_voltage;
float driver_temperature;
int8_t motor_temperature;
uint8_t driver_state;
} ActuatorLSStateMessage;
//////////////////////////////////////////////////////
typedef enum {
AgxMsgUnkonwn = 0x00,
// command
AgxMsgMotionCommand,
AgxMsgLightCommand,
AgxMsgValueSetCommand,
// state feedback
AgxMsgSystemState,
AgxMsgMotionState,
AgxMsgLightState,
AgxMsgActuatorState,
} MsgType;
typedef struct {
MsgType type;
union {
// command
MotionCommandMessage motion_command_msg;
LightCommandMessage light_command_msg;
// state feedback
SystemStateMessage system_state_msg;
MotionStateMessage motion_state_msg;
LightStateMessage light_state_msg;
ActuatorStateMessage actuator_state_msg;
} body;
} AgxMessageV1;
#ifdef __cplusplus
}
#endif
#endif /* AGILEX_MESSAGE_V1_H */

4
include/ugv_sdk/details/robot_base/agilex_base.hpp
View File

@@ -75,7 +75,7 @@ class AgilexBase : public RobotInterface {
if (can_connected_) { if (can_connected_) {
AgxMessage msg; AgxMessage msg;
msg.type = AgxMsgLightCommand; msg.type = AgxMsgLightCommand;
msg.body.light_command_msg.cmd_ctrl_allowed = true; msg.body.light_command_msg.enable_cmd_ctrl = true;
msg.body.light_command_msg.front_light.mode = front_mode; msg.body.light_command_msg.front_light.mode = front_mode;
msg.body.light_command_msg.front_light.custom_value = front_custom_value; msg.body.light_command_msg.front_light.custom_value = front_custom_value;
msg.body.light_command_msg.rear_light.mode = rear_mode; msg.body.light_command_msg.rear_light.mode = rear_mode;
@@ -93,7 +93,7 @@ class AgilexBase : public RobotInterface {
AgxMessage msg; AgxMessage msg;
msg.type = AgxMsgLightCommand; msg.type = AgxMsgLightCommand;
msg.body.light_command_msg.cmd_ctrl_allowed = false; msg.body.light_command_msg.enable_cmd_ctrl = false;
// send to can bus // send to can bus
can_frame frame; can_frame frame;

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 * scout_protocol.h
* *
* Created on: Aug 07, 2019 21:49 * Created on: Aug 07, 2019 21:49
* Description: * Description:
* *
* Copyright (c) 2019 Ruixiang Du (rdu) * Copyright (c) 2019 Ruixiang Du (rdu)
*/ */
#ifndef SCOUT_PROTOCOL_H #ifndef SCOUT_PROTOCOL_H
#define SCOUT_PROTOCOL_H #define SCOUT_PROTOCOL_H
@@ -16,87 +16,87 @@ extern "C" {
#include <stdint.h> #include <stdint.h>
#define SCOUT_CMD_BUF_LEN 32 #define SCOUT_CMD_BUF_LEN 32
#define SCOUT_STATUS_BUF_LEN 32 #define SCOUT_STATUS_BUF_LEN 32
#define SCOUT_FRAME_SIZE 13 #define SCOUT_FRAME_SIZE 13
#define SCOUT_MOTOR1_ID ((uint8_t)0x00) #define SCOUT_MOTOR1_ID ((uint8_t)0x00)
#define SCOUT_MOTOR2_ID ((uint8_t)0x01) #define SCOUT_MOTOR2_ID ((uint8_t)0x01)
#define SCOUT_MOTOR3_ID ((uint8_t)0x02) #define SCOUT_MOTOR3_ID ((uint8_t)0x02)
#define SCOUT_MOTOR4_ID ((uint8_t)0x03) #define SCOUT_MOTOR4_ID ((uint8_t)0x03)
// UART Definitions // UART Definitions
#define UART_FRAME_SYSTEM_STATUS_ID ((uint8_t)0x01) #define UART_FRAME_SYSTEM_STATUS_ID ((uint8_t)0x01)
#define UART_FRAME_MOTION_STATUS_ID ((uint8_t)0x02) #define UART_FRAME_MOTION_STATUS_ID ((uint8_t)0x02)
#define UART_FRAME_MOTOR1_DRIVER_STATUS_ID ((uint8_t)0x03) #define UART_FRAME_MOTOR1_DRIVER_STATUS_ID ((uint8_t)0x03)
#define UART_FRAME_MOTOR2_DRIVER_STATUS_ID ((uint8_t)0x04) #define UART_FRAME_MOTOR2_DRIVER_STATUS_ID ((uint8_t)0x04)
#define UART_FRAME_MOTOR3_DRIVER_STATUS_ID ((uint8_t)0x05) #define UART_FRAME_MOTOR3_DRIVER_STATUS_ID ((uint8_t)0x05)
#define UART_FRAME_MOTOR4_DRIVER_STATUS_ID ((uint8_t)0x06) #define UART_FRAME_MOTOR4_DRIVER_STATUS_ID ((uint8_t)0x06)
#define UART_FRAME_LIGHT_STATUS_ID ((uint8_t)0x07) #define UART_FRAME_LIGHT_STATUS_ID ((uint8_t)0x07)
#define UART_FRAME_MOTION_CONTROL_ID ((uint8_t)0x01) #define UART_FRAME_MOTION_CONTROL_ID ((uint8_t)0x01)
#define UART_FRAME_LIGHT_CONTROL_ID ((uint8_t)0x02) #define UART_FRAME_LIGHT_CONTROL_ID ((uint8_t)0x02)
// CAN Definitions // CAN Definitions
#define CAN_MSG_MOTION_CONTROL_CMD_ID ((uint32_t)0x130) #define CAN_MSG_MOTION_CONTROL_CMD_ID ((uint32_t)0x130)
#define CAN_MSG_MOTION_CONTROL_STATUS_ID ((uint32_t)0x131) #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_CMD_ID ((uint32_t)0x140)
#define CAN_MSG_LIGHT_CONTROL_STATUS_ID ((uint32_t)0x141) #define CAN_MSG_LIGHT_CONTROL_STATUS_ID ((uint32_t)0x141)
#define CAN_MSG_SYSTEM_STATUS_STATUS_ID ((uint32_t)0x151) #define CAN_MSG_SYSTEM_STATUS_STATUS_ID ((uint32_t)0x151)
#define CAN_MSG_MOTOR1_DRIVER_STATUS_ID ((uint32_t)0x200) #define CAN_MSG_MOTOR1_DRIVER_STATUS_ID ((uint32_t)0x200)
#define CAN_MSG_MOTOR2_DRIVER_STATUS_ID ((uint32_t)0x201) #define CAN_MSG_MOTOR2_DRIVER_STATUS_ID ((uint32_t)0x201)
#define CAN_MSG_MOTOR3_DRIVER_STATUS_ID ((uint32_t)0x202) #define CAN_MSG_MOTOR3_DRIVER_STATUS_ID ((uint32_t)0x202)
#define CAN_MSG_MOTOR4_DRIVER_STATUS_ID ((uint32_t)0x203) #define CAN_MSG_MOTOR4_DRIVER_STATUS_ID ((uint32_t)0x203)
/*--------------------- Control/State Constants ------------------------*/ /*--------------------- Control/State Constants ------------------------*/
// Motion Control // Motion Control
#define CTRL_MODE_REMOTE ((uint8_t)0x00) // #define CTRL_MODE_REMOTE ((uint8_t)0x00)
#define CTRL_MODE_CMD_CAN ((uint8_t)0x01) // #define CTRL_MODE_CMD_CAN ((uint8_t)0x01)
#define CTRL_MODE_CMD_UART ((uint8_t)0x02) // #define CTRL_MODE_CMD_UART ((uint8_t)0x02)
#define CTRL_MODE_COMMANDED ((uint8_t)0x03) // #define CTRL_MODE_COMMANDED ((uint8_t)0x03)
#define FAULT_CLR_NONE ((uint8_t)0x00) #define FAULT_CLR_NONE ((uint8_t)0x00)
#define FAULT_CLR_BAT_UNDER_VOL ((uint8_t)0x01) #define FAULT_CLR_BAT_UNDER_VOL ((uint8_t)0x01)
#define FAULT_CLR_BAT_OVER_VOL ((uint8_t)0x02) #define FAULT_CLR_BAT_OVER_VOL ((uint8_t)0x02)
#define FAULT_CLR_MOTOR1_COMM ((uint8_t)0x03) #define FAULT_CLR_MOTOR1_COMM ((uint8_t)0x03)
#define FAULT_CLR_MOTOR2_COMM ((uint8_t)0x04) #define FAULT_CLR_MOTOR2_COMM ((uint8_t)0x04)
#define FAULT_CLR_MOTOR3_COMM ((uint8_t)0x05) #define FAULT_CLR_MOTOR3_COMM ((uint8_t)0x05)
#define FAULT_CLR_MOTOR4_COMM ((uint8_t)0x06) #define FAULT_CLR_MOTOR4_COMM ((uint8_t)0x06)
#define FAULT_CLR_MOTOR_DRV_OVERHEAT ((uint8_t)0x07) #define FAULT_CLR_MOTOR_DRV_OVERHEAT ((uint8_t)0x07)
#define FAULT_CLR_MOTOR_OVERCURRENT ((uint8_t)0x08) #define FAULT_CLR_MOTOR_OVERCURRENT ((uint8_t)0x08)
// Light Control // Light Control
#define LIGHT_DISABLE_CTRL ((uint8_t)0x00) #define LIGHT_DISABLE_CTRL ((uint8_t)0x00)
#define LIGHT_ENABLE_CTRL ((uint8_t)0x01) #define LIGHT_ENABLE_CTRL ((uint8_t)0x01)
#define LIGHT_MODE_CONST_OFF ((uint8_t)0x00) #define LIGHT_MODE_CONST_OFF ((uint8_t)0x00)
#define LIGHT_MODE_CONST_ON ((uint8_t)0x01) #define LIGHT_MODE_CONST_ON ((uint8_t)0x01)
#define LIGHT_MODE_BREATH ((uint8_t)0x02) #define LIGHT_MODE_BREATH ((uint8_t)0x02)
#define LIGHT_MODE_CUSTOM ((uint8_t)0x03) #define LIGHT_MODE_CUSTOM ((uint8_t)0x03)
// System Status Feedback // System Status Feedback
#define BASE_STATE_NORMAL ((uint8_t)0x00) #define BASE_STATE_NORMAL ((uint8_t)0x00)
#define BASE_STATE_ESTOP ((uint8_t)0x01) #define BASE_STATE_ESTOP ((uint8_t)0x01)
#define BASE_STATE_EXCEPTION ((uint8_t)0x02) #define BASE_STATE_EXCEPTION ((uint8_t)0x02)
#define FAULT_CAN_CHECKSUM_ERROR ((uint16_t)0x0100) #define FAULT_CAN_CHECKSUM_ERROR ((uint16_t)0x0100)
#define FAULT_MOTOR_DRV_OVERHEAT_W ((uint16_t)0x0200) #define FAULT_MOTOR_DRV_OVERHEAT_W ((uint16_t)0x0200)
#define FAULT_MOTOR_OVERCURRENT_W ((uint16_t)0x0400) #define FAULT_MOTOR_OVERCURRENT_W ((uint16_t)0x0400)
#define FAULT_BAT_UNDER_VOL_W ((uint16_t)0x0800) #define FAULT_BAT_UNDER_VOL_W ((uint16_t)0x0800)
#define FAULT_RC_SIGNAL_LOSS ((uint16_t)0x1000) #define FAULT_RC_SIGNAL_LOSS ((uint16_t)0x1000)
#define FAULT_HIGH_BYTE_RESERVED2 ((uint16_t)0x2000) #define FAULT_HIGH_BYTE_RESERVED2 ((uint16_t)0x2000)
#define FAULT_HIGH_BYTE_RESERVED3 ((uint16_t)0x4000) #define FAULT_HIGH_BYTE_RESERVED3 ((uint16_t)0x4000)
#define FAULT_HIGH_BYTE_RESERVED4 ((uint16_t)0x8000) #define FAULT_HIGH_BYTE_RESERVED4 ((uint16_t)0x8000)
#define FAULT_BAT_UNDER_VOL_F ((uint16_t)0x0001) #define FAULT_BAT_UNDER_VOL_F ((uint16_t)0x0001)
#define FAULT_BAT_OVER_VOL_F ((uint16_t)0x0002) #define FAULT_BAT_OVER_VOL_F ((uint16_t)0x0002)
#define FAULT_MOTOR1_COMM_F ((uint16_t)0x0004) #define FAULT_MOTOR1_COMM_F ((uint16_t)0x0004)
#define FAULT_MOTOR2_COMM_F ((uint16_t)0x0008) #define FAULT_MOTOR2_COMM_F ((uint16_t)0x0008)
#define FAULT_MOTOR3_COMM_F ((uint16_t)0x0010) #define FAULT_MOTOR3_COMM_F ((uint16_t)0x0010)
#define FAULT_MOTOR4_COMM_F ((uint16_t)0x0020) #define FAULT_MOTOR4_COMM_F ((uint16_t)0x0020)
#define FAULT_MOTOR_DRV_OVERHEAT_F ((uint16_t)0x0040) #define FAULT_MOTOR_DRV_OVERHEAT_F ((uint16_t)0x0040)
#define FAULT_MOTOR_OVERCURRENT_F ((uint16_t)0x0080) #define FAULT_MOTOR_OVERCURRENT_F ((uint16_t)0x0080)
/*-------------------- Control/Feedback Messages -----------------------*/ /*-------------------- Control/Feedback Messages -----------------------*/
@@ -105,165 +105,144 @@ extern "C" {
#pragma pack(push, 1) #pragma pack(push, 1)
// Note: id could be different for UART and CAN protocol // Note: id could be different for UART and CAN protocol
// Motion Control // Motion Control
typedef struct { typedef struct {
union union {
{ struct {
struct uint8_t control_mode;
{ uint8_t fault_clear_flag;
uint8_t control_mode; int8_t linear_velocity_cmd;
uint8_t fault_clear_flag; int8_t angular_velocity_cmd;
int8_t linear_velocity_cmd; uint8_t reserved0;
int8_t angular_velocity_cmd; uint8_t reserved1;
uint8_t reserved0; uint8_t count;
uint8_t reserved1; uint8_t checksum;
uint8_t count; } cmd;
uint8_t checksum; uint8_t raw[8];
} cmd; } data;
uint8_t raw[8];
} data;
} MotionControlMessage; } MotionControlMessage;
typedef struct { typedef struct {
union union {
{ struct {
struct struct {
{ uint8_t high_byte;
struct uint8_t low_byte;
{ } linear_velocity;
uint8_t high_byte; struct {
uint8_t low_byte; uint8_t high_byte;
} linear_velocity; uint8_t low_byte;
struct } angular_velocity;
{ uint8_t reserved0;
uint8_t high_byte; uint8_t reserved1;
uint8_t low_byte; uint8_t count;
} angular_velocity; uint8_t checksum;
uint8_t reserved0; } status;
uint8_t reserved1; uint8_t raw[8];
uint8_t count; } data;
uint8_t checksum;
} status;
uint8_t raw[8];
} data;
} MotionStatusMessage; } MotionStatusMessage;
// System Status Feedback // System Status Feedback
typedef struct { typedef struct {
union union {
{ struct {
struct uint8_t base_state;
{ uint8_t control_mode;
uint8_t base_state; struct {
uint8_t control_mode; uint8_t high_byte;
struct uint8_t low_byte;
{ } battery_voltage;
uint8_t high_byte; struct {
uint8_t low_byte; uint8_t high_byte;
} battery_voltage; uint8_t low_byte;
struct } fault_code;
{ uint8_t count;
uint8_t high_byte; uint8_t checksum;
uint8_t low_byte; } status;
} fault_code; uint8_t raw[8];
uint8_t count; } data;
uint8_t checksum;
} status;
uint8_t raw[8];
} data;
} SystemStatusMessage; } SystemStatusMessage;
// Light Control // Light Control
typedef struct { typedef struct {
union union {
{ struct {
struct uint8_t light_ctrl_enable;
{ uint8_t front_mode;
uint8_t light_ctrl_enable; uint8_t front_custom;
uint8_t front_light_mode; uint8_t rear_mode;
uint8_t front_light_custom; uint8_t rear_custom;
uint8_t rear_light_mode; uint8_t reserved0;
uint8_t rear_light_custom; uint8_t count;
uint8_t reserved0; uint8_t checksum;
uint8_t count; } cmd;
uint8_t checksum; uint8_t raw[8];
} cmd; } data;
uint8_t raw[8];
} data;
} LightControlMessage; } LightControlMessage;
typedef struct { typedef struct {
union union {
{ struct {
struct uint8_t light_ctrl_enable;
{ uint8_t front_mode;
uint8_t light_ctrl_enable; uint8_t front_custom;
uint8_t front_light_mode; uint8_t rear_mode;
uint8_t front_light_custom; uint8_t rear_custom;
uint8_t rear_light_mode; uint8_t reserved0;
uint8_t rear_light_custom; uint8_t count;
uint8_t reserved0; uint8_t checksum;
uint8_t count; } status;
uint8_t checksum; uint8_t raw[8];
} status; } data;
uint8_t raw[8];
} data;
} LightStatusMessage; } LightStatusMessage;
// Motor Driver Feedback // Motor Driver Feedback
typedef struct typedef struct {
{ uint8_t motor_id;
uint8_t motor_id; union {
union { struct {
struct struct {
{ uint8_t high_byte;
struct uint8_t low_byte;
{ } current;
uint8_t high_byte; struct {
uint8_t low_byte; uint8_t high_byte;
} current; uint8_t low_byte;
struct } rpm;
{ int8_t temperature;
uint8_t high_byte; uint8_t reserved0;
uint8_t low_byte; uint8_t count;
} rpm; uint8_t checksum;
int8_t temperature; } status;
uint8_t reserved0; uint8_t raw[8];
uint8_t count; } data;
uint8_t checksum;
} status;
uint8_t raw[8];
} data;
} MotorDriverStatusMessage; } MotorDriverStatusMessage;
// For convenience to access status/control message // For convenience to access status/control message
typedef enum typedef enum {
{ ScoutMsgNone = 0x00,
ScoutMsgNone = 0x00, // status messages
// status messages ScoutMotionStatusMsg = 0x01,
ScoutMotionStatusMsg = 0x01, ScoutLightStatusMsg = 0x02,
ScoutLightStatusMsg = 0x02, ScoutSystemStatusMsg = 0x03,
ScoutSystemStatusMsg = 0x03, ScoutMotorDriverStatusMsg = 0x04,
ScoutMotorDriverStatusMsg = 0x04, // control messages
// control messages ScoutMotionControlMsg = 0x21,
ScoutMotionControlMsg = 0x21, ScoutLightControlMsg = 0x22
ScoutLightControlMsg = 0x22
} ScoutMsgType; } ScoutMsgType;
typedef struct typedef struct {
{ ScoutMsgType type;
ScoutMsgType type; union {
union { // status messages
// status messages MotionStatusMessage motion_status_msg;
MotionStatusMessage motion_status_msg; LightStatusMessage light_status_msg;
LightStatusMessage light_status_msg; SystemStatusMessage system_status_msg;
SystemStatusMessage system_status_msg; MotorDriverStatusMessage motor_driver_status_msg;
MotorDriverStatusMessage motor_driver_status_msg; // control messages
// control messages MotionControlMessage motion_control_msg;
MotionControlMessage motion_control_msg; LightControlMessage light_control_msg;
LightControlMessage light_control_msg; } body;
} body;
} ScoutMessage; } ScoutMessage;
#pragma pack(pop) #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" #include "protocol_v1/scout/scout_can_parser.h"
namespace westonrobot { namespace westonrobot {
// CAN support
bool ScoutProtocolV1Parser::DecodeMessage(const struct can_frame *rx_frame, 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, 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 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 // UART support
bool ScoutProtocolV1Parser::DecodeMessage(uint8_t *data, uint8_t dlc, 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[4] = UART_FRAME_LIGHT_STATUS_ID;
buf[5] = msg->body.light_status_msg.data.status.light_ctrl_enable; 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[6] = msg->body.light_status_msg.data.status.front_mode;
buf[7] = msg->body.light_status_msg.data.status.front_light_custom; buf[7] = msg->body.light_status_msg.data.status.front_custom;
buf[8] = msg->body.light_status_msg.data.status.rear_light_mode; buf[8] = msg->body.light_status_msg.data.status.rear_mode;
buf[9] = msg->body.light_status_msg.data.status.rear_light_custom; buf[9] = msg->body.light_status_msg.data.status.rear_custom;
buf[10] = 0; buf[10] = 0;
buf[11] = msg->body.light_status_msg.data.status.count; buf[11] = msg->body.light_status_msg.data.status.count;
break; break;
@@ -218,10 +218,10 @@ void EncodeLightControlMsgToUART(const LightControlMessage *msg, uint8_t *buf, u
// frame payload // frame payload
buf[5] = msg->data.cmd.light_ctrl_enable; buf[5] = msg->data.cmd.light_ctrl_enable;
buf[6] = msg->data.cmd.front_light_mode; buf[6] = msg->data.cmd.front_mode;
buf[7] = msg->data.cmd.front_light_custom; buf[7] = msg->data.cmd.front_custom;
buf[8] = msg->data.cmd.rear_light_mode; buf[8] = msg->data.cmd.rear_mode;
buf[9] = msg->data.cmd.rear_light_custom; buf[9] = msg->data.cmd.rear_custom;
buf[10] = 0x00; buf[10] = 0x00;
// frame count, checksum // frame count, checksum
@@ -467,10 +467,10 @@ bool ConstructControlMessage(ScoutMessage *msg)
{ {
msg->type = ScoutLightControlMsg; 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.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_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.front_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_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.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.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.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;
@@ -573,10 +573,10 @@ bool ConstructStatusMessage(ScoutMessage *msg)
{ {
msg->type = ScoutLightStatusMsg; 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.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_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.front_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_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.rear_custom = uart_parsing_data.payload_buffer[4];
msg->body.light_status_msg.data.status.reserved0 = 0x00; 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.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.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; msg->type = AgxMsgLightCommand;
// parse frame buffer to message // parse frame buffer to message
LightCommandFrame *frame = (LightCommandFrame *)(rx_frame->data); LightCommandFrame *frame = (LightCommandFrame *)(rx_frame->data);
msg->body.light_command_msg.cmd_ctrl_allowed = msg->body.light_command_msg.enable_cmd_ctrl =
(frame->cmd_ctrl_allowed != 0) ? true : false; (frame->enable_cmd_ctrl != 0) ? true : false;
msg->body.light_command_msg.front_light.mode = frame->front_light_mode; msg->body.light_command_msg.front_light.mode = frame->front_mode;
msg->body.light_command_msg.front_light.custom_value = msg->body.light_command_msg.front_light.custom_value =
frame->front_light_custom; frame->front_custom;
msg->body.light_command_msg.rear_light.mode = frame->rear_light_mode; msg->body.light_command_msg.rear_light.mode = frame->rear_mode;
msg->body.light_command_msg.rear_light.custom_value = msg->body.light_command_msg.rear_light.custom_value =
frame->rear_light_custom; frame->rear_custom;
break; break;
} }
case CAN_MSG_BRAKING_COMMAND_ID: { 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: { case CAN_MSG_LIGHT_STATE_ID: {
msg->type = AgxMsgLightState; msg->type = AgxMsgLightState;
LightStateFrame *frame = (LightStateFrame *)(rx_frame->data); LightStateFrame *frame = (LightStateFrame *)(rx_frame->data);
msg->body.light_command_msg.cmd_ctrl_allowed = msg->body.light_command_msg.enable_cmd_ctrl =
(frame->cmd_ctrl_allowed != 0) ? true : false; (frame->enable_cmd_ctrl != 0) ? true : false;
msg->body.light_command_msg.front_light.mode = frame->front_light_mode; msg->body.light_command_msg.front_light.mode = frame->front_mode;
msg->body.light_command_msg.front_light.custom_value = msg->body.light_command_msg.front_light.custom_value =
frame->front_light_custom; frame->front_custom;
msg->body.light_command_msg.rear_light.mode = frame->rear_light_mode; msg->body.light_command_msg.rear_light.mode = frame->rear_mode;
msg->body.light_command_msg.rear_light.custom_value = msg->body.light_command_msg.rear_light.custom_value =
frame->rear_light_custom; frame->rear_custom;
break; break;
} }
case CAN_MSG_RC_STATE_ID: { 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_id = CAN_MSG_LIGHT_COMMAND_ID;
tx_frame->can_dlc = 8; tx_frame->can_dlc = 8;
LightCommandFrame frame; LightCommandFrame frame;
if (msg->body.light_command_msg.cmd_ctrl_allowed) { if (msg->body.light_command_msg.enable_cmd_ctrl) {
frame.cmd_ctrl_allowed = LIGHT_CMD_CTRL_ALLOWED; frame.enable_cmd_ctrl = LIGHT_ENABLE_CMD_CTRL;
frame.front_light_mode = msg->body.light_command_msg.front_light.mode; frame.front_mode = msg->body.light_command_msg.front_light.mode;
frame.front_light_custom = frame.front_custom =
msg->body.light_command_msg.front_light.custom_value; msg->body.light_command_msg.front_light.custom_value;
frame.rear_light_mode = msg->body.light_command_msg.rear_light.mode; frame.rear_mode = msg->body.light_command_msg.rear_light.mode;
frame.rear_light_custom = frame.rear_custom =
msg->body.light_command_msg.rear_light.custom_value; msg->body.light_command_msg.rear_light.custom_value;
} else { } else {
frame.cmd_ctrl_allowed = LIGHT_CMD_CTRL_DISALLOWED; frame.enable_cmd_ctrl = LIGHT_DISABLE_CMD_CTRL;
frame.front_light_mode = 0; frame.front_mode = 0;
frame.front_light_custom = 0; frame.front_custom = 0;
frame.rear_light_mode = 0; frame.rear_mode = 0;
frame.rear_light_custom = 0; frame.rear_custom = 0;
} }
frame.reserved0 = 0; frame.reserved0 = 0;
frame.reserved1 = 0; frame.reserved1 = 0;
@@ -363,7 +363,7 @@ void EncodeCanFrame(const AgxMessage *msg, struct can_frame *tx_frame) {
} }
case AgxMsgBrakingCommand: { case AgxMsgBrakingCommand: {
tx_frame->can_id = CAN_MSG_BRAKING_COMMAND_ID; tx_frame->can_id = CAN_MSG_BRAKING_COMMAND_ID;
tx_frame->can_dlc = 8; tx_frame->can_dlc = 2;
BrakingCommandFrame frame; BrakingCommandFrame frame;
// TODO // TODO
memcpy(tx_frame->data, (uint8_t *)(&frame), tx_frame->can_dlc); 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_MOTION_COMMAND_ID ((uint32_t)0x111)
#define CAN_MSG_LIGHT_COMMAND_ID ((uint32_t)0x121) #define CAN_MSG_LIGHT_COMMAND_ID ((uint32_t)0x121)
#define CAN_MSG_BRAKING_COMMAND_ID ((uint32_t)0x131) #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 // state feedback group: 0x2
#define CAN_MSG_SYSTEM_STATE_ID ((uint32_t)0x211) #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_ACTUATOR7_LS_STATE_ID ((uint32_t)0x267)
#define CAN_MSG_ACTUATOR8_LS_STATE_ID ((uint32_t)0x268) #define CAN_MSG_ACTUATOR8_LS_STATE_ID ((uint32_t)0x268)
#define CAN_MSG_MOTOR_ANGLE_INFO ((uint32_t)0x271) #define CAN_MSG_MOTOR_ANGLE_INFO ((uint32_t)0x271)
#define CAN_MSG_MOTOR_SPEED_INFO ((uint32_t)0x281) #define CAN_MSG_MOTOR_SPEED_INFO ((uint32_t)0x281)
#define CAN_MSG_CURRENT_CTRL_MODE ((uint32_t)0x291) #define CAN_MSG_CURRENT_CTRL_MODE ((uint32_t)0x291)
// sensor data group: 0x3 // sensor data group: 0x3
#define CAN_MSG_ODOMETRY_ID ((uint32_t)0x311) #define CAN_MSG_ODOMETRY_ID ((uint32_t)0x311)
@@ -109,16 +109,6 @@ extern "C" {
#define CAN_MSG_STATE_RESET_CONFIG_ID ((uint32_t)0x441) #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 -------------------------*/ /*------------------------ Frame Memory Layout -------------------------*/
/* No padding in the struct */ /* No padding in the struct */
@@ -157,30 +147,26 @@ typedef struct {
struct16_t steering_angle; struct16_t steering_angle;
} MotionCommandFrame; } MotionCommandFrame;
#define LIGHT_ENABLE_CMD_CTRL ((uint8_t)0x01)
#define LIGHT_DISABLE_CMD_CTRL ((uint8_t)0x00)
typedef struct { typedef struct {
uint8_t cmd_ctrl_allowed; uint8_t enable_cmd_ctrl;
uint8_t front_light_mode; uint8_t front_mode;
uint8_t front_light_custom; uint8_t front_custom;
uint8_t rear_light_mode; uint8_t rear_mode;
uint8_t rear_light_custom; uint8_t rear_custom;
uint8_t reserved0; uint8_t reserved0;
uint8_t reserved1; uint8_t reserved1;
uint8_t count; uint8_t count;
} LightCommandFrame; } LightCommandFrame;
typedef struct { typedef struct {
uint8_t cmd_ctrl_allowed; uint8_t enable_brake;
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; uint8_t count;
} BrakingCommandFrame; } BrakingCommandFrame;
typedef struct typedef struct {
{
uint8_t motion_mode; uint8_t motion_mode;
uint8_t reserved0; uint8_t reserved0;
uint8_t reserved1; uint8_t reserved1;
@@ -189,7 +175,7 @@ typedef struct
uint8_t reserved4; uint8_t reserved4;
uint8_t reserved5; uint8_t reserved5;
uint8_t reserved6; uint8_t reserved6;
}SetMotionModeFrame; } SetMotionModeFrame;
// State feedback messages // State feedback messages
typedef struct { typedef struct {
@@ -209,11 +195,11 @@ typedef struct {
} MotionStateFrame; } MotionStateFrame;
typedef struct { typedef struct {
uint8_t cmd_ctrl_allowed; uint8_t enable_cmd_ctrl;
uint8_t front_light_mode; uint8_t front_mode;
uint8_t front_light_custom; uint8_t front_custom;
uint8_t rear_light_mode; uint8_t rear_mode;
uint8_t rear_light_custom; uint8_t rear_custom;
uint8_t reserved0; uint8_t reserved0;
uint8_t reserved1; uint8_t reserved1;
uint8_t count; uint8_t count;
@@ -267,7 +253,7 @@ typedef struct {
typedef struct { typedef struct {
uint8_t motion_mode; uint8_t motion_mode;
uint8_t mode_changing; uint8_t mode_changing;
}MotionModeStateFrame; } MotionModeStateFrame;
// sensors // sensors
typedef struct { typedef struct {
@@ -340,6 +326,11 @@ typedef struct {
} BmsExtendedFrame; } BmsExtendedFrame;
// query/config // 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 { typedef struct {
uint8_t request; uint8_t request;
uint8_t reserved0; 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) { 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, AgilexBase::SendLightCommand(cmd.front_mode, cmd.front_custom_value,
LightMode::CONST_OFF, 0); 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) { 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, AgilexBase::SendLightCommand(cmd.front_mode, cmd.front_custom_value,
LightMode::CONST_OFF, 0); LightMode::CONST_OFF, 0);
} }