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

Merge pull request #9 from wangzheqie/v2.x

Browse Source 
add motion mode feedback
This commit is contained in:
Du Ruixiang
2021-05-03 09:23:01 +08:00
committed by GitHub
parent 1f8ddc15ca c1b53c167b
commit f78349e2ff
5 changed files with 31 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files

15
include/ugv_sdk/agilex_message.h
View File

@@ -15,8 +15,8 @@
extern "C" { extern "C" {
#endif #endif
#include <stdint.h>
#include <stdbool.h> #include <stdbool.h>
#include <stdint.h>
#include <string.h> #include <string.h>
/***************** Control messages *****************/ /***************** Control messages *****************/
@@ -54,8 +54,7 @@ typedef struct {
} BrakingCommandMessage; } BrakingCommandMessage;
// 0x141 // 0x141
typedef struct typedef struct {
{
uint8_t motion_mode; uint8_t motion_mode;
} MotionModeMessage; } MotionModeMessage;
@@ -69,7 +68,7 @@ typedef enum {
} VehicleState; } VehicleState;
typedef enum { typedef enum {
// CONTROL_MODE_STANDBY = 0x00, // CONTROL_MODE_STANDBY = 0x00,
CONTROL_MODE_RC = 0x00, CONTROL_MODE_RC = 0x00,
CONTROL_MODE_CAN = 0x01, CONTROL_MODE_CAN = 0x01,
CONTROL_MODE_UART = 0x02 CONTROL_MODE_UART = 0x02
@@ -141,6 +140,12 @@ typedef struct {
#define DRIVER_STATE_DRIVER_ENABLED_MASK ((uint8_t)0x40) #define DRIVER_STATE_DRIVER_ENABLED_MASK ((uint8_t)0x40)
#define DRIVER_STATE_DRIVER_RESET_MASK ((uint8_t)0x80) #define DRIVER_STATE_DRIVER_RESET_MASK ((uint8_t)0x80)
// 0x291
typedef struct {
uint8_t motion_mode;
uint8_t mode_changing;
} MotionModeFeedbackMessage;
typedef struct { typedef struct {
uint8_t motor_id; uint8_t motor_id;
float driver_voltage; float driver_voltage;
@@ -302,6 +307,7 @@ typedef enum {
AgxMsgRcState, AgxMsgRcState,
AgxMsgActuatorHSState, AgxMsgActuatorHSState,
AgxMsgActuatorLSState, AgxMsgActuatorLSState,
AgxMsgMotionModeState,
// sensor // sensor
AgxMsgOdometry, AgxMsgOdometry,
AgxMsgImuAccel, AgxMsgImuAccel,
@@ -336,6 +342,7 @@ typedef struct {
RcStateMessage rc_state_msg; RcStateMessage rc_state_msg;
ActuatorHSStateMessage actuator_hs_state_msg; ActuatorHSStateMessage actuator_hs_state_msg;
ActuatorLSStateMessage actuator_ls_state_msg; ActuatorLSStateMessage actuator_ls_state_msg;
MotionModeFeedbackMessage motion_mode_feedback_msg;
// sensor // sensor
OdometryMessage odometry_msg; OdometryMessage odometry_msg;
ImuAccelMessage imu_accel_msg; ImuAccelMessage imu_accel_msg;

1
include/ugv_sdk/ranger_base.hpp
View File

@@ -30,6 +30,7 @@ struct RangerState {
ActuatorHSStateMessage actuator_hs_state[8]; ActuatorHSStateMessage actuator_hs_state[8];
ActuatorLSStateMessage actuator_ls_state[8]; ActuatorLSStateMessage actuator_ls_state[8];
MotionModeFeedbackMessage current_motion_mode;
// sensor data // sensor data
OdometryMessage odometry; OdometryMessage odometry;

9
src/agx_msg_parser.c
View File

@@ -192,6 +192,15 @@ bool DecodeCanFrame(const struct can_frame *rx_frame, AgxMessage *msg) {
msg->body.actuator_ls_state_msg.driver_state = frame->driver_state; msg->body.actuator_ls_state_msg.driver_state = frame->driver_state;
break; break;
} }
case CAN_MSG_CURRENT_CTRL_MODE:
{
msg->type=AgxMsgMotionModeState;
MotionModeStateFrame *frame = (MotionModeStateFrame*)(rx_frame->data);
msg->body.motion_mode_feedback_msg.motion_mode = frame->motion_mode;
msg->body.motion_mode_feedback_msg.mode_changing = frame->mode_changing;
break;
}
/****************** sensor frame *****************/ /****************** sensor frame *****************/
case CAN_MSG_ODOMETRY_ID: { case CAN_MSG_ODOMETRY_ID: {
msg->type = AgxMsgOdometry; msg->type = AgxMsgOdometry;

6
src/agx_protocol_v2.h
View File

@@ -263,6 +263,12 @@ typedef struct {
uint8_t reserved1; uint8_t reserved1;
} ActuatorLSStateFrame; } ActuatorLSStateFrame;
// 0x291
typedef struct {
uint8_t motion_mode;
uint8_t mode_changing;
}MotionModeStateFrame;
// sensors // sensors
typedef struct { typedef struct {
struct32_t left_wheel; struct32_t left_wheel;

4
src/ranger_base.cpp
View File

@@ -95,6 +95,10 @@ void RangerBase::UpdateRangerState(const AgxMessage &status_msg,
status_msg.body.actuator_ls_state_msg; status_msg.body.actuator_ls_state_msg;
break; break;
} }
case AgxMsgMotionModeState: {
state.current_motion_mode = status_msg.body.motion_mode_feedback_msg;
break;
}
/* sensor feedback */ /* sensor feedback */
case AgxMsgOdometry: { case AgxMsgOdometry: {
// std::cout << "Odometer msg feedback received" << std::endl; // std::cout << "Odometer msg feedback received" << std::endl;