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-12 19:02:41 +08:00
parent 10d9fc734e
commit 6cbdebc38a
14 changed files with 337 additions and 93 deletions
Download Patch File Download Diff File Expand all files Collapse all files

87
src/protocol_v1/agilex_msg_parser_v1.c
View File

@@ -7,26 +7,40 @@
* Copyright (c) 2021 Ruixiang Du (rdu)
*/
#include "ugv_sdk/details/protocol_v1/agilex_protocol_v1.h"
#include "ugv_sdk/details/protocol_v1//agilex_msg_parser_v1.h"
#include "protocol_v1/agilex_protocol_v1.h"
#include "stdio.h"
#include "string.h"
bool DecodeCanFrameV1(const struct can_frame *rx_frame, AgxMessage *msg) {
// if checksum not correct
if (!CalcCanFrameChecksumV1(rx_frame->can_id, (uint8_t *)rx_frame->data,
rx_frame->can_dlc)) {
return false;
}
switch (rx_frame->can_id) {
case CAN_MSG_SYSTEM_STATE_ID: {
msg->type = AgxMsgSystemState;
// msg->system_status_msg.id = CAN_MSG_SYSTEM_STATUS_STATUS_ID;
// memcpy(msg->body.system_state_msg.data.raw, rx_frame->data,
// rx_frame->can_dlc * sizeof(uint8_t));
msg->body.system_state_msg.vehicle_state = rx_frame->data[0];
msg->body.system_state_msg.control_mode = rx_frame->data[1];
msg->body.system_state_msg.battery_voltage =
((((uint16_t)rx_frame->data[2]) << 8) | (uint16_t)rx_frame->data[3]) /
10.0f;
msg->body.system_state_msg.error_code =
((((uint16_t)rx_frame->data[4]) << 8) | (uint16_t)rx_frame->data[5]);
break;
}
case CAN_MSG_MOTION_STATE_ID: {
msg->type = AgxMsgMotionState;
// msg->motion_status_msg.id = CAN_MSG_MOTION_CONTROL_STATUS_ID;
// memcpy(msg->body.motion_state_msg.data.raw, rx_frame->data,
// rx_frame->can_dlc * sizeof(uint8_t));
msg->body.motion_state_msg.linear_velocity =
((((uint16_t)rx_frame->data[0]) << 8) | (uint16_t)rx_frame->data[1]) /
1000.0f;
msg->body.motion_state_msg.angular_velocity =
((((uint16_t)rx_frame->data[2]) << 8) | (uint16_t)rx_frame->data[3]) /
1000.0f;
break;
}
case CAN_MSG_LIGHT_STATE_ID: {
@@ -36,33 +50,21 @@ bool DecodeCanFrameV1(const struct can_frame *rx_frame, AgxMessage *msg) {
// rx_frame->can_dlc * sizeof(uint8_t));
break;
}
case CAN_MSG_ACTUATOR1_STATE_ID: {
msg->type = AgxMsgActuatorStateV1;
// msg->motor_driver_status_msg.id = CAN_MSG_MOTOR1_DRIVER_STATUS_ID;
// msg->body.v1_actuator_state_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_ACTUATOR2_STATE_ID: {
msg->type = AgxMsgActuatorStateV1;
// msg->motor_driver_status_msg.id = CAN_MSG_MOTOR2_DRIVER_STATUS_ID;
// msg->body.v1_actuator_state_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_ACTUATOR3_STATE_ID: {
msg->type = AgxMsgActuatorStateV1;
// msg->motor_driver_status_msg.id = CAN_MSG_MOTOR3_DRIVER_STATUS_ID;
// msg->body.v1_actuator_state_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));
case CAN_MSG_VALUE_SET_STATE_ID: {
msg->type = AgxMsgValueSetStateV1;
if (rx_frame->data[0] == 0xaa)
msg->body.v1_value_set_state_msg.set_neutral = true;
else
msg->body.v1_value_set_state_msg.set_neutral = false;
break;
}
case CAN_MSG_ACTUATOR1_STATE_ID:
case CAN_MSG_ACTUATOR2_STATE_ID:
case CAN_MSG_ACTUATOR3_STATE_ID:
case CAN_MSG_ACTUATOR4_STATE_ID: {
msg->type = AgxMsgActuatorStateV1;
// msg->motor_driver_status_msg.id = CAN_MSG_MOTOR4_DRIVER_STATUS_ID;
msg->body.v1_actuator_state_msg.motor_id =
rx_frame->can_id - CAN_MSG_ACTUATOR1_STATE_ID;
// msg->body.v1_actuator_state_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));
@@ -83,10 +85,8 @@ void EncodeCanFrameV1(const AgxMessage *msg, struct can_frame *tx_frame) {
tx_frame->can_dlc = 8;
tx_frame->data[0] = CTRL_MODE_CMD_CAN;
tx_frame->data[1] = ERROR_CLR_NONE;
tx_frame->data[2] =
(int8_t)(msg->body.motion_command_msg.linear_velocity * 100);
tx_frame->data[3] =
(int8_t)(msg->body.motion_command_msg.angular_velocity * 100);
tx_frame->data[2] = (int8_t)(msg->body.v1_motion_command_msg.linear);
tx_frame->data[3] = (int8_t)(msg->body.v1_motion_command_msg.angular);
tx_frame->data[4] = 0;
tx_frame->data[5] = 0;
tx_frame->data[6] = count++;
@@ -95,6 +95,22 @@ void EncodeCanFrameV1(const AgxMessage *msg, struct can_frame *tx_frame) {
break;
}
case AgxMsgValueSetCommandV1: {
static uint8_t count = 0;
tx_frame->can_id = CAN_MSG_VALUE_SET_COMMAND_ID;
tx_frame->can_dlc = 8;
tx_frame->data[0] =
msg->body.v1_value_set_command_msg.set_neutral ? 0xaa : 0x00;
tx_frame->data[1] = 0x00;
tx_frame->data[2] = 0x00;
tx_frame->data[3] = 0x00;
tx_frame->data[4] = 0x00;
tx_frame->data[5] = 0x00;
tx_frame->data[6] = 0x00;
tx_frame->data[7] = CalcCanFrameChecksumV1(
tx_frame->can_id, tx_frame->data, tx_frame->can_dlc);
break;
}
case AgxMsgLightCommand: {
static uint8_t count = 0;
tx_frame->can_id = CAN_MSG_LIGHT_COMMAND_ID;
tx_frame->can_dlc = 8;
@@ -120,9 +136,6 @@ void EncodeCanFrameV1(const AgxMessage *msg, struct can_frame *tx_frame) {
tx_frame->can_id, tx_frame->data, tx_frame->can_dlc);
break;
}
case AgxMsgLightCommand: {
break;
}
default:
break;
}

199
src/protocol_v1/agilex_protocol_v1.h Normal file
View File

@@ -0,0 +1,199 @@
/*
* agilex_protocol_v1.h
*
* Created on: Jul 09, 2021 20:34
* Description:
*
* Copyright (c) 2021 Ruixiang Du (rdu)
*/
#ifndef AGILEX_PROTOCOL_V1_H
#define AGILEX_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)0x210)
// 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_VALUE_SET_STATE_ID ((uint32_t)0x211)
#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 /* AGILEX_PROTOCOL_V1_H */