mirrors/ugv_sdk
1
0
Fork 0
mirror of https://github.com/westonrobot/ugv_sdk synced 2023-04-08 06:32:14 +08:00
Code Issues Packages Projects Releases Wiki Activity

saved work

Browse Source
This commit is contained in:
Ruixiang Du
2021-07-10 21:50:27 +08:00
parent 6a8073e90c
commit 4ebef2e906
6 changed files with 55 additions and 144 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -44,6 +44,18 @@ typedef struct {
uint8_t motion_mode;
} MotionModeMessage;
// V1-only messages
typedef struct {
ControlMode control_mode;
bool clear_all_error;
float linear;
float angular;
} MotionCommandMessageV1;
typedef struct {
bool set_neutral;
} ValueSetCommandMessageV1;
/**************** Feedback messages *****************/
#define SYSTEM_ERROR_MOTOR_DRIVER_MASK ((uint16_t)0x0100)
@@ -64,7 +76,6 @@ typedef struct {
uint16_t error_code;
} SystemStateMessage;
// 0x221
typedef struct {
float linear_velocity;
float angular_velocity; // only valid for differential drivering
@@ -72,7 +83,6 @@ typedef struct {
float steering_angle; // only valid for ackermann steering
} MotionStateMessage;
// 0x231
typedef LightCommandMessage LightStateMessage;
typedef struct {
@@ -94,12 +104,6 @@ typedef struct {
int32_t pulse_count;
} ActuatorHSStateMessage;
typedef struct {
uint8_t motion_mode;
uint8_t mode_changing;
} MotionModeFeedbackMessage;
// 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)
@@ -117,6 +121,18 @@ typedef struct {
uint8_t driver_state;
} ActuatorLSStateMessage;
typedef struct {
uint8_t motion_mode;
uint8_t mode_changing;
} MotionModeFeedbackMessage;
// V1-only messages
typedef struct {
float current;
int16_t rpm;
float temperature;
} ActuatorStateMessageV1;
/***************** Sensor messages ******************/
typedef struct {
@@ -167,7 +183,6 @@ typedef struct {
float temperature;
} BmsBasicMessage;
// 0x362
#define BMS_PROT1_CHARGING_CURRENT_NONZERO_MASK ((uint8_t)0x01)
#define BMS_PROT1_CHARGING_OVERCURRENT_SET_MASK ((uint8_t)0x02)
#define BMS_PROT1_DISCHARGING_CURRENT_NONZERO_MASK ((uint8_t)0x10)
@@ -273,7 +288,11 @@ typedef enum {
AgxMsgControlModeConfig,
AgxMsgSteerNeutralRequest,
AgxMsgSteerNeutralResponse,
AgxMsgStateResetConfig
AgxMsgStateResetConfig,
// V1-only messages
AgxMsgMotionCommandV1,
AgxMsgValueSetCommandV1,
AgxMsgActuatorStateV1
} MsgType;
typedef struct {
@@ -309,6 +328,10 @@ typedef struct {
SteerNeutralRequestMessage steer_neutral_request_msg;
SteerNeutralResponseMessage steer_neutral_response_msg;
StateResetConfigMessage state_reset_config_msg;
// V1-only messages
MotionCommandMessageV1 v1_motion_command_msg;
ValueSetCommandMessageV1 v1_value_set_command_msg;
ActuatorStateMessageV1 v1_actuator_stage_msg;
} body;
} AgxMessage;

10
include/ugv_sdk/details/interface/scout_interface.hpp
View File

@@ -16,18 +16,16 @@
namespace westonrobot {
struct ScoutState {
// system state
SystemStateMessage system_state;
MotionStateMessage motion_state;
LightStateMessage light_state;
RcStateMessage rc_state;
// actuator state
// - for v2 robots only
ActuatorHSStateMessage actuator_hs_state[4];
ActuatorLSStateMessage actuator_ls_state[4];
// sensor data
OdometryMessage odometry;
// - for v1 robots only
ActuatorStateMessageV1 actuator_state[4];
};
struct ScoutInterface {

25
include/ugv_sdk/details/robot_base/scout_base.hpp
View File

@@ -61,12 +61,13 @@ class ScoutBase : public AgilexBase<Parser>, public ScoutInterface {
void ParseCANFrame(can_frame *rx_frame) override {
AgxMessage status_msg;
AgilexBase<Parser>::parser_.DecodeMessage(rx_frame, &status_msg);
std::lock_guard<std::mutex> guard(AgilexBase<Parser>::state_mutex_);
UpdateScoutState(status_msg, scout_state_);
if (AgilexBase<Parser>::parser_.DecodeMessage(rx_frame, &status_msg)) {
UpdateScoutState(status_msg, scout_state_);
}
}
void UpdateScoutState(const AgxMessage &status_msg, ScoutState &state) {
std::lock_guard<std::mutex> guard(AgilexBase<Parser>::state_mutex_);
switch (status_msg.type) {
case AgxMsgSystemState: {
// std::cout << "system status feedback received" << std::endl;
@@ -83,10 +84,10 @@ class ScoutBase : public AgilexBase<Parser>, public ScoutInterface {
state.light_state = status_msg.body.light_state_msg;
break;
}
case AgxMsgRcState: {
state.rc_state = status_msg.body.rc_state_msg;
break;
}
// case AgxMsgRcState: {
// state.rc_state = status_msg.body.rc_state_msg;
// break;
// }
case AgxMsgActuatorHSState: {
// std::cout << "actuator hs feedback received" << std::endl;
state
@@ -101,11 +102,11 @@ class ScoutBase : public AgilexBase<Parser>, public ScoutInterface {
status_msg.body.actuator_ls_state_msg;
break;
}
/* sensor feedback */
case AgxMsgOdometry: {
// std::cout << "Odometer msg feedback received" << std::endl;
state.odometry = status_msg.body.odometry_msg;
}
/* sensor feedback */
// case AgxMsgOdometry: {
// // std::cout << "Odometer msg feedback received" << std::endl;
// state.odometry = status_msg.body.odometry_msg;
// }
default:
break;
}

111
src/protocol_v1/agilex_message_v1.h
View File

@@ -1,111 +0,0 @@
/*
* 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>
#include "ugv_sdk/details/interface/agilex_types.h"
/***************** Control messages *****************/
typedef struct {
ControlMode control_mode;
bool clear_all_error;
float linear;
float angular;
} MotionCommandMessage;
typedef struct {
bool enable_cmd_ctrl;
LightOperation front_light;
LightOperation rear_light;
} LightCommandMessage;
typedef struct {
bool set_neutral;
} ValueSetCommandMessage;
/**************** Feedback messages *****************/
#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;
typedef struct {
float linear_velocity;
float angular_velocity;
} MotionStateMessage;
typedef LightCommandMessage LightStateMessage;
typedef struct {
float current;
int16_t rpm;
float temperature;
} ActuatorStateMessage;
//////////////////////////////////////////////////////
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
src/protocol_v1/agilex_msg_parser_v1.c
View File

@@ -13,6 +13,6 @@
#include "stdio.h"
#include "string.h"
bool DecodeCanFrameV1(const struct can_frame *rx_frame, AgxMessageV1 *msg) {}
void EncodeCanFrameV1(const AgxMessageV1 *msg, struct can_frame *tx_frame) {}
bool DecodeCanFrameV1(const struct can_frame *rx_frame, AgxMessage *msg) {}
void EncodeCanFrameV1(const AgxMessage *msg, struct can_frame *tx_frame) {}
uint8_t CalcCanFrameChecksumV1(uint16_t id, uint8_t *data, uint8_t dlc) {}

6
src/protocol_v1/agilex_msg_parser_v1.h
View File

@@ -28,10 +28,10 @@ struct can_frame {
};
#endif
#include "protocol_v1/agilex_message_v1.h"
#include "ugv_sdk/details/interface/agilex_message.h"
bool DecodeCanFrameV1(const struct can_frame *rx_frame, AgxMessageV1 *msg);
void EncodeCanFrameV1(const AgxMessageV1 *msg, struct can_frame *tx_frame);
bool DecodeCanFrameV1(const struct can_frame *rx_frame, AgxMessage *msg);
void EncodeCanFrameV1(const AgxMessage *msg, struct can_frame *tx_frame);
uint8_t CalcCanFrameChecksumV1(uint16_t id, uint8_t *data, uint8_t dlc);
#ifdef __cplusplus