mirror of
https://github.com/westonrobot/ugv_sdk
synced 2023-04-08 06:32:14 +08:00
saved work, updated code organization
This commit is contained in:
Ruixiang Du
111
src/protocol_v1/agilex_message_v1.h
Normal file
111
src/protocol_v1/agilex_message_v1.h
Normal file
@@ -0,0 +1,111 @@
|
||||
/*
|
||||
* 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 */
|
||||
18
src/protocol_v1/agilex_msg_parser_v1.c
Normal file
18
src/protocol_v1/agilex_msg_parser_v1.c
Normal file
@@ -0,0 +1,18 @@
|
||||
/*
|
||||
* agilex_msg_parser.c
|
||||
*
|
||||
* Created on: Jul 09, 2021 22:04
|
||||
* Description:
|
||||
*
|
||||
* Copyright (c) 2021 Ruixiang Du (rdu)
|
||||
*/
|
||||
|
||||
#include "agilex_protocol_v1.h"
|
||||
#include "protocol_v1/agilex_msg_parser_v1.h"
|
||||
|
||||
#include "stdio.h"
|
||||
#include "string.h"
|
||||
|
||||
bool DecodeCanFrame(const struct can_frame *rx_frame, AgxMessageV1 *msg) {}
|
||||
void EncodeCanFrame(const AgxMessageV1 *msg, struct can_frame *tx_frame) {}
|
||||
uint8_t CalcCanFrameChecksum(uint16_t id, uint8_t *data, uint8_t dlc) {}
|
||||
41
src/protocol_v1/agilex_msg_parser_v1.h
Normal file
41
src/protocol_v1/agilex_msg_parser_v1.h
Normal file
@@ -0,0 +1,41 @@
|
||||
/*
|
||||
* agilex_msg_parser.h
|
||||
*
|
||||
* Created on: Jul 09, 2021 22:04
|
||||
* Description:
|
||||
*
|
||||
* Copyright (c) 2021 Ruixiang Du (rdu)
|
||||
*/
|
||||
|
||||
#ifndef AGILEX_MSG_PARSER_H
|
||||
#define AGILEX_MSG_PARSER_H
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
#include <stdint.h>
|
||||
#include <stdbool.h>
|
||||
#include <string.h>
|
||||
|
||||
#ifdef __linux__
|
||||
#include <linux/can.h>
|
||||
#else
|
||||
struct can_frame {
|
||||
uint32_t can_id;
|
||||
uint8_t can_dlc;
|
||||
uint8_t data[8] __attribute__((aligned(8)));
|
||||
};
|
||||
#endif
|
||||
|
||||
#include "protocol_v1/agilex_message_v1.h"
|
||||
|
||||
bool DecodeCanFrame(const struct can_frame *rx_frame, AgxMessageV1 *msg);
|
||||
void EncodeCanFrame(const AgxMessageV1 *msg, struct can_frame *tx_frame);
|
||||
uint8_t CalcCanFrameChecksum(uint16_t id, uint8_t *data, uint8_t dlc);
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
|
||||
#endif /* AGILEX_MSG_PARSER_H */
|
||||
@@ -1,5 +1,5 @@
|
||||
/*
|
||||
* agx_protocol_v1.h
|
||||
* agilex_protocol_v1.h
|
||||
*
|
||||
* Created on: Jul 09, 2021 20:34
|
||||
* Description:
|
||||
@@ -7,8 +7,8 @@
|
||||
* Copyright (c) 2021 Ruixiang Du (rdu)
|
||||
*/
|
||||
|
||||
#ifndef AGX_PROTOCOL_V1_H
|
||||
#define AGX_PROTOCOL_V1_H
|
||||
#ifndef AGILEX_PROTOCOL_V1_H
|
||||
#define AGILEX_PROTOCOL_V1_H
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
@@ -194,4 +194,4 @@ typedef struct {
|
||||
}
|
||||
#endif
|
||||
|
||||
#endif /* AGX_PROTOCOL_V1_H */
|
||||
#endif /* AGILEX_PROTOCOL_V1_H */
|
||||
111
src/protocol_v1/parser_impl/scout_protocol_v1_parser.cpp
Normal file
111
src/protocol_v1/parser_impl/scout_protocol_v1_parser.cpp
Normal file
@@ -0,0 +1,111 @@
|
||||
/*
|
||||
* scout_protocol_v1_parser.cpp
|
||||
*
|
||||
* Created on: Jul 08, 2021 22:43
|
||||
* Description:
|
||||
*
|
||||
* Copyright (c) 2021 Ruixiang Du (rdu)
|
||||
*/
|
||||
|
||||
#include "ugv_sdk/details/protocol_v1/scout_protocol_v1_parser.hpp"
|
||||
|
||||
namespace westonrobot {
|
||||
// CAN support
|
||||
bool ScoutProtocolV1Parser::DecodeMessage(const struct can_frame *rx_frame,
|
||||
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) {
|
||||
// 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 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,
|
||||
AgxMessage *msg) {}
|
||||
|
||||
void ScoutProtocolV1Parser::EncodeMessage(const AgxMessage *msg, uint8_t *buf,
|
||||
uint8_t *len) {}
|
||||
|
||||
uint8_t ScoutProtocolV1Parser::CalculateChecksum(uint8_t *buf, uint8_t len) {}
|
||||
} // namespace westonrobot
|
||||
@@ -1,178 +0,0 @@
|
||||
/*
|
||||
* scout_can_parser.c
|
||||
*
|
||||
* Created on: Aug 31, 2019 04:25
|
||||
* Description:
|
||||
*
|
||||
* Copyright (c) 2019 Ruixiang Du (rdu)
|
||||
*/
|
||||
|
||||
#include "protocol_v1/scout/scout_can_parser.h"
|
||||
|
||||
#include "string.h"
|
||||
|
||||
static void EncodeScoutMotionControlMsgToCAN(const MotionControlMessage *msg, struct can_frame *tx_frame);
|
||||
static void EncodeScoutLightControlMsgToCAN(const LightControlMessage *msg, struct can_frame *tx_frame);
|
||||
|
||||
bool DecodeScoutMsgFromCAN(const struct can_frame *rx_frame, ScoutMessage *msg)
|
||||
{
|
||||
msg->type = ScoutMsgNone;
|
||||
|
||||
switch (rx_frame->can_id)
|
||||
{
|
||||
// in the current implementation, both MsgType and can_frame include 8 * uint8_t
|
||||
case CAN_MSG_MOTION_CONTROL_STATUS_ID:
|
||||
{
|
||||
msg->type = ScoutMotionStatusMsg;
|
||||
// msg->motion_status_msg.id = CAN_MSG_MOTION_CONTROL_STATUS_ID;
|
||||
memcpy(msg->body.motion_status_msg.data.raw, rx_frame->data, rx_frame->can_dlc * sizeof(uint8_t));
|
||||
break;
|
||||
}
|
||||
case CAN_MSG_LIGHT_CONTROL_STATUS_ID:
|
||||
{
|
||||
msg->type = ScoutLightStatusMsg;
|
||||
// msg->light_status_msg.id = CAN_MSG_LIGHT_CONTROL_STATUS_ID;
|
||||
memcpy(msg->body.light_status_msg.data.raw, rx_frame->data, rx_frame->can_dlc * sizeof(uint8_t));
|
||||
break;
|
||||
}
|
||||
case CAN_MSG_SYSTEM_STATUS_STATUS_ID:
|
||||
{
|
||||
msg->type = ScoutSystemStatusMsg;
|
||||
// msg->system_status_msg.id = CAN_MSG_SYSTEM_STATUS_STATUS_ID;
|
||||
memcpy(msg->body.system_status_msg.data.raw, rx_frame->data, rx_frame->can_dlc * sizeof(uint8_t));
|
||||
break;
|
||||
}
|
||||
case CAN_MSG_MOTOR1_DRIVER_STATUS_ID:
|
||||
{
|
||||
msg->type = ScoutMotorDriverStatusMsg;
|
||||
// msg->motor_driver_status_msg.id = CAN_MSG_MOTOR1_DRIVER_STATUS_ID;
|
||||
msg->body.motor_driver_status_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_MOTOR2_DRIVER_STATUS_ID:
|
||||
{
|
||||
msg->type = ScoutMotorDriverStatusMsg;
|
||||
// msg->motor_driver_status_msg.id = CAN_MSG_MOTOR2_DRIVER_STATUS_ID;
|
||||
msg->body.motor_driver_status_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_MOTOR3_DRIVER_STATUS_ID:
|
||||
{
|
||||
msg->type = ScoutMotorDriverStatusMsg;
|
||||
// msg->motor_driver_status_msg.id = CAN_MSG_MOTOR3_DRIVER_STATUS_ID;
|
||||
msg->body.motor_driver_status_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));
|
||||
break;
|
||||
}
|
||||
case CAN_MSG_MOTOR4_DRIVER_STATUS_ID:
|
||||
{
|
||||
msg->type = ScoutMotorDriverStatusMsg;
|
||||
// msg->motor_driver_status_msg.id = CAN_MSG_MOTOR4_DRIVER_STATUS_ID;
|
||||
msg->body.motor_driver_status_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));
|
||||
break;
|
||||
}
|
||||
// in the current implementation, both MsgType and can_frame include 8 * uint8_t
|
||||
case CAN_MSG_MOTION_CONTROL_CMD_ID:
|
||||
{
|
||||
msg->type = ScoutMotionControlMsg;
|
||||
// msg->motion_control_msg.id = CAN_MSG_MOTION_CONTROL_CMD_ID;
|
||||
memcpy(msg->body.motion_control_msg.data.raw, rx_frame->data, rx_frame->can_dlc * sizeof(uint8_t));
|
||||
break;
|
||||
}
|
||||
case CAN_MSG_LIGHT_CONTROL_CMD_ID:
|
||||
{
|
||||
msg->type = ScoutLightControlMsg;
|
||||
// msg->light_control_msg.id = CAN_MSG_LIGHT_CONTROL_STATUS_ID;
|
||||
memcpy(msg->body.light_control_msg.data.raw, rx_frame->data, rx_frame->can_dlc * sizeof(uint8_t));
|
||||
break;
|
||||
}
|
||||
default:
|
||||
break;
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
void EncodeScoutMsgToCAN(const ScoutMessage *msg, struct can_frame *tx_frame)
|
||||
{
|
||||
switch (msg->type)
|
||||
{
|
||||
// in the current implementation, both MsgType and can_frame include 8 * uint8_t
|
||||
case ScoutMotionStatusMsg:
|
||||
{
|
||||
tx_frame->can_id = CAN_MSG_MOTION_CONTROL_STATUS_ID;
|
||||
tx_frame->can_dlc = 8;
|
||||
memcpy(tx_frame->data, msg->body.motion_status_msg.data.raw, tx_frame->can_dlc);
|
||||
break;
|
||||
}
|
||||
case ScoutLightStatusMsg:
|
||||
{
|
||||
tx_frame->can_id = CAN_MSG_LIGHT_CONTROL_STATUS_ID;
|
||||
tx_frame->can_dlc = 8;
|
||||
memcpy(tx_frame->data, msg->body.light_status_msg.data.raw, tx_frame->can_dlc);
|
||||
break;
|
||||
}
|
||||
case ScoutSystemStatusMsg:
|
||||
{
|
||||
tx_frame->can_id = CAN_MSG_SYSTEM_STATUS_STATUS_ID;
|
||||
tx_frame->can_dlc = 8;
|
||||
memcpy(tx_frame->data, msg->body.system_status_msg.data.raw, tx_frame->can_dlc);
|
||||
break;
|
||||
}
|
||||
case ScoutMotorDriverStatusMsg:
|
||||
{
|
||||
if (msg->body.motor_driver_status_msg.motor_id == SCOUT_MOTOR1_ID)
|
||||
tx_frame->can_id = CAN_MSG_MOTOR1_DRIVER_STATUS_ID;
|
||||
else if (msg->body.motor_driver_status_msg.motor_id == SCOUT_MOTOR2_ID)
|
||||
tx_frame->can_id = CAN_MSG_MOTOR2_DRIVER_STATUS_ID;
|
||||
else if (msg->body.motor_driver_status_msg.motor_id == SCOUT_MOTOR3_ID)
|
||||
tx_frame->can_id = CAN_MSG_MOTOR3_DRIVER_STATUS_ID;
|
||||
else if (msg->body.motor_driver_status_msg.motor_id == SCOUT_MOTOR4_ID)
|
||||
tx_frame->can_id = CAN_MSG_MOTOR4_DRIVER_STATUS_ID;
|
||||
tx_frame->can_dlc = 8;
|
||||
memcpy(tx_frame->data, msg->body.motor_driver_status_msg.data.raw, tx_frame->can_dlc);
|
||||
break;
|
||||
}
|
||||
case ScoutMotionControlMsg:
|
||||
{
|
||||
EncodeScoutMotionControlMsgToCAN(&(msg->body.motion_control_msg), tx_frame);
|
||||
break;
|
||||
}
|
||||
case ScoutLightControlMsg:
|
||||
{
|
||||
EncodeScoutLightControlMsgToCAN(&(msg->body.light_control_msg), tx_frame);
|
||||
break;
|
||||
}
|
||||
default:
|
||||
break;
|
||||
}
|
||||
tx_frame->data[7] = CalcScoutCANChecksum(tx_frame->can_id, tx_frame->data, tx_frame->can_dlc);
|
||||
}
|
||||
|
||||
void EncodeScoutMotionControlMsgToCAN(const MotionControlMessage *msg, struct can_frame *tx_frame)
|
||||
{
|
||||
tx_frame->can_id = CAN_MSG_MOTION_CONTROL_CMD_ID;
|
||||
tx_frame->can_dlc = 8;
|
||||
memcpy(tx_frame->data, msg->data.raw, tx_frame->can_dlc);
|
||||
tx_frame->data[7] = CalcScoutCANChecksum(tx_frame->can_id, tx_frame->data, tx_frame->can_dlc);
|
||||
}
|
||||
|
||||
void EncodeScoutLightControlMsgToCAN(const LightControlMessage *msg, struct can_frame *tx_frame)
|
||||
{
|
||||
tx_frame->can_id = CAN_MSG_LIGHT_CONTROL_CMD_ID;
|
||||
tx_frame->can_dlc = 8;
|
||||
memcpy(tx_frame->data, msg->data.raw, tx_frame->can_dlc);
|
||||
tx_frame->data[7] = CalcScoutCANChecksum(tx_frame->can_id, tx_frame->data, tx_frame->can_dlc);
|
||||
}
|
||||
|
||||
uint8_t CalcScoutCANChecksum(uint16_t id, uint8_t *data, 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;
|
||||
}
|
||||
@@ -1,42 +0,0 @@
|
||||
/*
|
||||
* scout_can_parser.h
|
||||
*
|
||||
* Created on: Aug 31, 2019 04:23
|
||||
* Description:
|
||||
*
|
||||
* Copyright (c) 2019 Ruixiang Du (rdu)
|
||||
*/
|
||||
|
||||
#ifndef SCOUT_CAN_PARSER_H
|
||||
#define SCOUT_CAN_PARSER_H
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
#include <stdint.h>
|
||||
#include <stdbool.h>
|
||||
|
||||
#include "protocol_v1/scout/scout_protocol.h"
|
||||
|
||||
#ifdef __linux__
|
||||
#include <linux/can.h>
|
||||
#else
|
||||
struct can_frame
|
||||
{
|
||||
uint32_t can_id;
|
||||
uint8_t can_dlc;
|
||||
uint8_t data[8]__attribute__((aligned(8)));
|
||||
};
|
||||
#endif
|
||||
|
||||
bool DecodeScoutMsgFromCAN(const struct can_frame *rx_frame, ScoutMessage *msg);
|
||||
void EncodeScoutMsgToCAN(const ScoutMessage *msg, struct can_frame *tx_frame);
|
||||
|
||||
uint8_t CalcScoutCANChecksum(uint16_t id, uint8_t *data, uint8_t dlc);
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
|
||||
#endif /* SCOUT_CAN_PARSER_H */
|
||||
@@ -1,254 +0,0 @@
|
||||
/*
|
||||
* scout_protocol.h
|
||||
*
|
||||
* Created on: Aug 07, 2019 21:49
|
||||
* Description:
|
||||
*
|
||||
* Copyright (c) 2019 Ruixiang Du (rdu)
|
||||
*/
|
||||
|
||||
#ifndef SCOUT_PROTOCOL_H
|
||||
#define SCOUT_PROTOCOL_H
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
#include <stdint.h>
|
||||
|
||||
#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)
|
||||
|
||||
// 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_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)
|
||||
|
||||
/*--------------------- 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 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_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 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)
|
||||
|
||||
/*-------------------- Control/Feedback Messages -----------------------*/
|
||||
|
||||
/* No padding in the struct */
|
||||
// reference: https://stackoverflow.com/questions/3318410/pragma-pack-effect
|
||||
#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;
|
||||
} 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;
|
||||
} 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;
|
||||
} SystemStatusMessage;
|
||||
|
||||
// Light Control
|
||||
typedef struct {
|
||||
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_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;
|
||||
} 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
|
||||
} 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;
|
||||
} ScoutMessage;
|
||||
|
||||
#pragma pack(pop)
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
|
||||
#endif /* SCOUT_PROTOCOL_H */
|
||||
@@ -1,111 +0,0 @@
|
||||
/*
|
||||
* scout_protocol_v1_parser.cpp
|
||||
*
|
||||
* Created on: Jul 08, 2021 22:43
|
||||
* Description:
|
||||
*
|
||||
* Copyright (c) 2021 Ruixiang Du (rdu)
|
||||
*/
|
||||
|
||||
#include "ugv_sdk/details/protocol_v1/scout_protocol_v1_parser.hpp"
|
||||
|
||||
#include "protocol_v1/scout/scout_can_parser.h"
|
||||
|
||||
namespace westonrobot {
|
||||
// CAN support
|
||||
bool ScoutProtocolV1Parser::DecodeMessage(const struct can_frame *rx_frame,
|
||||
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) {
|
||||
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 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,
|
||||
AgxMessage *msg) {}
|
||||
|
||||
void ScoutProtocolV1Parser::EncodeMessage(const AgxMessage *msg, uint8_t *buf,
|
||||
uint8_t *len) {}
|
||||
|
||||
uint8_t ScoutProtocolV1Parser::CalculateChecksum(uint8_t *buf, uint8_t len) {}
|
||||
} // namespace westonrobot
|
||||
@@ -1,629 +0,0 @@
|
||||
/*
|
||||
* scout_uart_parser.c
|
||||
*
|
||||
* Created on: Aug 14, 2019 12:02
|
||||
* Description:
|
||||
*
|
||||
* Copyright (c) 2019 Ruixiang Du (rdu)
|
||||
*/
|
||||
|
||||
#include "protocol_v1/scout/scout_uart_parser.h"
|
||||
|
||||
// #define USE_XOR_CHECKSUM
|
||||
|
||||
// #define PRINT_CPP_DEBUG_INFO
|
||||
// #define PRINT_JLINK_DEBUG_INFO
|
||||
|
||||
#ifdef PRINT_CPP_DEBUG_INFO
|
||||
#undef PRINT_JLINK_DEBUG_INFO
|
||||
#endif
|
||||
|
||||
#ifdef PRINT_CPP_DEBUG_INFO
|
||||
#define <iostream>
|
||||
#elif (defined(PRINT_JLINK_DEBUG_INFO))
|
||||
#include "segger/jlink_rtt.h"
|
||||
#endif
|
||||
|
||||
typedef enum
|
||||
{
|
||||
WAIT_FOR_SOF1 = 0,
|
||||
WAIT_FOR_SOF2,
|
||||
WAIT_FOR_FRAME_LEN,
|
||||
WAIT_FOR_FRAME_TYPE,
|
||||
WAIT_FOR_FRAME_ID,
|
||||
WAIT_FOR_PAYLOAD,
|
||||
WAIT_FOR_FRAME_COUNT,
|
||||
WAIT_FOR_CHECKSUM
|
||||
} ScoutSerialDecodeState;
|
||||
|
||||
#define PAYLOAD_BUFFER_SIZE (SCOUT_FRAME_SIZE * 2)
|
||||
|
||||
#define FRAME_SOF_LEN ((uint8_t)2)
|
||||
#define FRAME_FIXED_FIELD_LEN ((uint8_t)4)
|
||||
|
||||
#define FRAME_SOF1 ((uint8_t)0x5a)
|
||||
#define FRAME_SOF2 ((uint8_t)0xa5)
|
||||
|
||||
#define FRAME_TYPE_CONTROL ((uint8_t)0x55)
|
||||
#define FRAME_TYPE_STATUS ((uint8_t)0xaa)
|
||||
|
||||
#define FRAME_NONE_ID ((uint8_t)0x00)
|
||||
|
||||
// frame buffer
|
||||
static struct
|
||||
{
|
||||
uint8_t frame_id;
|
||||
uint8_t frame_type;
|
||||
uint8_t frame_len;
|
||||
uint8_t frame_cnt;
|
||||
uint8_t frame_checksum;
|
||||
uint8_t internal_checksum;
|
||||
uint8_t payload_buffer[PAYLOAD_BUFFER_SIZE];
|
||||
size_t payload_data_pos;
|
||||
} uart_parsing_data;
|
||||
|
||||
// statisctics
|
||||
typedef struct
|
||||
{
|
||||
uint32_t frame_parsed;
|
||||
uint32_t frame_with_wrong_checksum;
|
||||
} UARTParsingStats;
|
||||
|
||||
static UARTParsingStats uart_parsing_stats = {.frame_parsed = true, .frame_with_wrong_checksum = 123};
|
||||
|
||||
// internal functions
|
||||
static bool ParseChar(uint8_t c, ScoutMessage *msg);
|
||||
static uint8_t CalcBufferedFrameChecksum();
|
||||
static bool ConstructStatusMessage(ScoutMessage *msg);
|
||||
static bool ConstructControlMessage(ScoutMessage *msg);
|
||||
|
||||
static void EncodeMotionControlMsgToUART(const MotionControlMessage *msg, uint8_t *buf, uint8_t *len);
|
||||
static void EncodeLightControlMsgToUART(const LightControlMessage *msg, uint8_t *buf, uint8_t *len);
|
||||
|
||||
void EncodeScoutMsgToUART(const ScoutMessage *msg, uint8_t *buf, uint8_t *len)
|
||||
{
|
||||
// SOF
|
||||
buf[0] = FRAME_SOF1;
|
||||
buf[1] = FRAME_SOF2;
|
||||
|
||||
// frame len, type, ID
|
||||
buf[2] = 0x0a;
|
||||
buf[3] = FRAME_TYPE_STATUS;
|
||||
|
||||
switch (msg->type)
|
||||
{
|
||||
// in the current implementation, both MsgType and can_frame include 8 * uint8_t
|
||||
case ScoutMotionStatusMsg:
|
||||
{
|
||||
buf[4] = UART_FRAME_MOTION_STATUS_ID;
|
||||
buf[5] = msg->body.motion_status_msg.data.status.linear_velocity.high_byte;
|
||||
buf[6] = msg->body.motion_status_msg.data.status.linear_velocity.low_byte;
|
||||
buf[7] = msg->body.motion_status_msg.data.status.angular_velocity.high_byte;
|
||||
buf[8] = msg->body.motion_status_msg.data.status.angular_velocity.low_byte;
|
||||
buf[9] = 0;
|
||||
buf[10] = 0;
|
||||
buf[11] = msg->body.motion_status_msg.data.status.count;
|
||||
break;
|
||||
}
|
||||
case ScoutLightStatusMsg:
|
||||
{
|
||||
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_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;
|
||||
}
|
||||
case ScoutSystemStatusMsg:
|
||||
{
|
||||
buf[4] = UART_FRAME_SYSTEM_STATUS_ID;
|
||||
buf[5] = msg->body.system_status_msg.data.status.base_state;
|
||||
buf[6] = msg->body.system_status_msg.data.status.control_mode;
|
||||
buf[7] = msg->body.system_status_msg.data.status.battery_voltage.high_byte;
|
||||
buf[8] = msg->body.system_status_msg.data.status.battery_voltage.low_byte;
|
||||
buf[9] = msg->body.system_status_msg.data.status.fault_code.high_byte;
|
||||
buf[10] = msg->body.system_status_msg.data.status.fault_code.low_byte;
|
||||
buf[11] = msg->body.system_status_msg.data.status.count;
|
||||
break;
|
||||
}
|
||||
case ScoutMotorDriverStatusMsg:
|
||||
{
|
||||
if (msg->body.motor_driver_status_msg.motor_id == SCOUT_MOTOR1_ID)
|
||||
buf[4] = UART_FRAME_MOTOR1_DRIVER_STATUS_ID;
|
||||
else if (msg->body.motor_driver_status_msg.motor_id == SCOUT_MOTOR2_ID)
|
||||
buf[4] = UART_FRAME_MOTOR2_DRIVER_STATUS_ID;
|
||||
else if (msg->body.motor_driver_status_msg.motor_id == SCOUT_MOTOR3_ID)
|
||||
buf[4] = UART_FRAME_MOTOR3_DRIVER_STATUS_ID;
|
||||
else if (msg->body.motor_driver_status_msg.motor_id == SCOUT_MOTOR4_ID)
|
||||
buf[4] = UART_FRAME_MOTOR4_DRIVER_STATUS_ID;
|
||||
buf[5] = msg->body.motor_driver_status_msg.data.status.current.high_byte;
|
||||
buf[6] = msg->body.motor_driver_status_msg.data.status.current.low_byte;
|
||||
buf[7] = msg->body.motor_driver_status_msg.data.status.rpm.high_byte;
|
||||
buf[8] = msg->body.motor_driver_status_msg.data.status.rpm.low_byte;
|
||||
buf[9] = msg->body.motor_driver_status_msg.data.status.temperature;
|
||||
buf[10] = 0;
|
||||
buf[11] = msg->body.motor_driver_status_msg.data.status.count;
|
||||
break;
|
||||
}
|
||||
case ScoutMotionControlMsg:
|
||||
{
|
||||
EncodeMotionControlMsgToUART(&(msg->body.motion_control_msg), buf, len);
|
||||
break;
|
||||
}
|
||||
case ScoutLightControlMsg:
|
||||
{
|
||||
EncodeLightControlMsgToUART(&(msg->body.light_control_msg), buf, len);
|
||||
break;
|
||||
}
|
||||
default:
|
||||
break;
|
||||
}
|
||||
|
||||
buf[12] = CalcScoutUARTChecksum(buf, buf[2] + FRAME_SOF_LEN);
|
||||
|
||||
// length: SOF + Frame + Checksum
|
||||
*len = buf[2] + FRAME_SOF_LEN + 1;
|
||||
}
|
||||
|
||||
bool DecodeScoutMsgFromUART(uint8_t c, ScoutMessage *msg)
|
||||
{
|
||||
static ScoutMessage decoded_msg;
|
||||
|
||||
bool result = ParseChar(c, &decoded_msg);
|
||||
if (result)
|
||||
*msg = decoded_msg;
|
||||
return result;
|
||||
}
|
||||
|
||||
void EncodeMotionControlMsgToUART(const MotionControlMessage *msg, uint8_t *buf, uint8_t *len)
|
||||
{
|
||||
// SOF
|
||||
buf[0] = FRAME_SOF1;
|
||||
buf[1] = FRAME_SOF2;
|
||||
|
||||
// frame len, type, ID
|
||||
buf[2] = 0x0a;
|
||||
buf[3] = FRAME_TYPE_CONTROL;
|
||||
buf[4] = UART_FRAME_MOTION_CONTROL_ID;
|
||||
|
||||
// frame payload
|
||||
buf[5] = msg->data.cmd.control_mode;
|
||||
buf[6] = msg->data.cmd.fault_clear_flag;
|
||||
buf[7] = msg->data.cmd.linear_velocity_cmd;
|
||||
buf[8] = msg->data.cmd.angular_velocity_cmd;
|
||||
buf[9] = 0x00;
|
||||
buf[10] = 0x00;
|
||||
|
||||
// frame count, checksum
|
||||
buf[11] = msg->data.cmd.count;
|
||||
buf[12] = CalcScoutUARTChecksum(buf, buf[2] + FRAME_SOF_LEN);
|
||||
|
||||
// length: SOF + Frame + Checksum
|
||||
*len = buf[2] + FRAME_SOF_LEN + 1;
|
||||
}
|
||||
|
||||
void EncodeLightControlMsgToUART(const LightControlMessage *msg, uint8_t *buf, uint8_t *len)
|
||||
{
|
||||
// SOF
|
||||
buf[0] = FRAME_SOF1;
|
||||
buf[1] = FRAME_SOF2;
|
||||
|
||||
// frame len, type, ID
|
||||
buf[2] = 0x0a;
|
||||
buf[3] = FRAME_TYPE_CONTROL;
|
||||
buf[4] = UART_FRAME_LIGHT_CONTROL_ID;
|
||||
|
||||
// frame payload
|
||||
buf[5] = msg->data.cmd.light_ctrl_enable;
|
||||
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
|
||||
buf[11] = msg->data.cmd.count;
|
||||
buf[12] = CalcScoutUARTChecksum(buf, buf[2] + FRAME_SOF_LEN);
|
||||
|
||||
// length: SOF + Frame + Checksum
|
||||
*len = buf[2] + FRAME_SOF_LEN + 1;
|
||||
}
|
||||
|
||||
bool ParseChar(uint8_t c, ScoutMessage *msg)
|
||||
{
|
||||
static ScoutSerialDecodeState decode_state = WAIT_FOR_SOF1;
|
||||
|
||||
bool new_frame_parsed = false;
|
||||
switch (decode_state)
|
||||
{
|
||||
case WAIT_FOR_SOF1:
|
||||
{
|
||||
if (c == FRAME_SOF1)
|
||||
{
|
||||
uart_parsing_data.frame_id = FRAME_NONE_ID;
|
||||
uart_parsing_data.frame_type = 0;
|
||||
uart_parsing_data.frame_len = 0;
|
||||
uart_parsing_data.frame_cnt = 0;
|
||||
uart_parsing_data.frame_checksum = 0;
|
||||
uart_parsing_data.internal_checksum = 0;
|
||||
uart_parsing_data.payload_data_pos = 0;
|
||||
memset(uart_parsing_data.payload_buffer, 0, PAYLOAD_BUFFER_SIZE);
|
||||
|
||||
decode_state = WAIT_FOR_SOF2;
|
||||
#ifdef PRINT_CPP_DEBUG_INFO
|
||||
std::cout << "found sof1" << std::endl;
|
||||
#elif (defined(PRINT_JLINK_DEBUG_INFO))
|
||||
JLinkWriteString(0, "found sof1\n");
|
||||
#endif
|
||||
}
|
||||
break;
|
||||
}
|
||||
case WAIT_FOR_SOF2:
|
||||
{
|
||||
if (c == FRAME_SOF2)
|
||||
{
|
||||
decode_state = WAIT_FOR_FRAME_LEN;
|
||||
#ifdef PRINT_CPP_DEBUG_INFO
|
||||
std::cout << "found sof2" << std::endl;
|
||||
#elif (defined(PRINT_JLINK_DEBUG_INFO))
|
||||
JLinkWriteString(0, "found sof2\n");
|
||||
#endif
|
||||
}
|
||||
else
|
||||
{
|
||||
decode_state = WAIT_FOR_SOF1;
|
||||
#ifdef PRINT_CPP_DEBUG_INFO
|
||||
std::cout << "failed to find sof2" << std::endl;
|
||||
#elif (defined(PRINT_JLINK_DEBUG_INFO))
|
||||
JLinkWriteString(0, "failed to find sof2\n");
|
||||
#endif
|
||||
}
|
||||
break;
|
||||
}
|
||||
case WAIT_FOR_FRAME_LEN:
|
||||
{
|
||||
uart_parsing_data.frame_len = c;
|
||||
decode_state = WAIT_FOR_FRAME_TYPE;
|
||||
#ifdef PRINT_CPP_DEBUG_INFO
|
||||
std::cout << "frame len: " << std::hex << static_cast<int>(frame_len) << std::dec << std::endl;
|
||||
#elif (defined(PRINT_JLINK_DEBUG_INFO))
|
||||
JLinkRTTPrintf(0, "frame len: %d\n", frame_len);
|
||||
#endif
|
||||
break;
|
||||
}
|
||||
case WAIT_FOR_FRAME_TYPE:
|
||||
{
|
||||
switch (c)
|
||||
{
|
||||
case FRAME_TYPE_CONTROL:
|
||||
{
|
||||
uart_parsing_data.frame_type = FRAME_TYPE_CONTROL;
|
||||
decode_state = WAIT_FOR_FRAME_ID;
|
||||
#ifdef PRINT_CPP_DEBUG_INFO
|
||||
std::cout << "control type frame received" << std::endl;
|
||||
#elif (defined(PRINT_JLINK_DEBUG_INFO))
|
||||
JLinkWriteString(0, "control type frame received\n");
|
||||
#endif
|
||||
break;
|
||||
}
|
||||
case FRAME_TYPE_STATUS:
|
||||
{
|
||||
uart_parsing_data.frame_type = FRAME_TYPE_STATUS;
|
||||
decode_state = WAIT_FOR_FRAME_ID;
|
||||
#ifdef PRINT_CPP_DEBUG_INFO
|
||||
std::cout << "status type frame received" << std::endl;
|
||||
#elif (defined(PRINT_JLINK_DEBUG_INFO))
|
||||
JLinkWriteString(0, "status type frame received\n");
|
||||
#endif
|
||||
break;
|
||||
}
|
||||
default:
|
||||
{
|
||||
#ifdef PRINT_CPP_DEBUG_INFO
|
||||
std::cerr << "ERROR: Not expecting frame of a type other than FRAME_TYPE_STATUS" << std::endl;
|
||||
#elif (defined(PRINT_JLINK_DEBUG_INFO))
|
||||
JLinkWriteString(0, "ERROR: Not expecting frame of a type other than FRAME_TYPE_STATUS\n");
|
||||
#endif
|
||||
decode_state = WAIT_FOR_SOF1;
|
||||
}
|
||||
}
|
||||
break;
|
||||
}
|
||||
case WAIT_FOR_FRAME_ID:
|
||||
{
|
||||
switch (c)
|
||||
{
|
||||
case UART_FRAME_SYSTEM_STATUS_ID:
|
||||
case UART_FRAME_MOTION_STATUS_ID:
|
||||
case UART_FRAME_MOTOR1_DRIVER_STATUS_ID:
|
||||
case UART_FRAME_MOTOR2_DRIVER_STATUS_ID:
|
||||
case UART_FRAME_MOTOR3_DRIVER_STATUS_ID:
|
||||
case UART_FRAME_MOTOR4_DRIVER_STATUS_ID:
|
||||
case UART_FRAME_LIGHT_STATUS_ID:
|
||||
{
|
||||
uart_parsing_data.frame_id = c;
|
||||
decode_state = WAIT_FOR_PAYLOAD;
|
||||
#ifdef PRINT_CPP_DEBUG_INFO
|
||||
std::cout << "frame id: " << std::hex << static_cast<int>(frame_id) << std::dec << std::endl;
|
||||
#elif (defined(PRINT_JLINK_DEBUG_INFO))
|
||||
JLinkRTTPrintf(0, "frame id: %d\n", frame_id);
|
||||
#endif
|
||||
break;
|
||||
}
|
||||
default:
|
||||
{
|
||||
#ifdef PRINT_CPP_DEBUG_INFO
|
||||
std::cerr << "ERROR: Unknown frame id" << std::endl;
|
||||
#elif (defined(PRINT_JLINK_DEBUG_INFO))
|
||||
JLinkWriteString(0, "ERROR: Unknown frame id\n");
|
||||
#endif
|
||||
decode_state = WAIT_FOR_SOF1;
|
||||
}
|
||||
}
|
||||
break;
|
||||
}
|
||||
case WAIT_FOR_PAYLOAD:
|
||||
{
|
||||
uart_parsing_data.payload_buffer[uart_parsing_data.payload_data_pos++] = c;
|
||||
#ifdef PRINT_CPP_DEBUG_INFO
|
||||
std::cout << "1 byte added: " << std::hex << static_cast<int>(c) << std::dec << std::endl;
|
||||
#elif (defined(PRINT_JLINK_DEBUG_INFO))
|
||||
JLinkRTTPrintf(0, "1 byte added: %d\n", c);
|
||||
#endif
|
||||
if (uart_parsing_data.payload_data_pos == (uart_parsing_data.frame_len - FRAME_FIXED_FIELD_LEN))
|
||||
decode_state = WAIT_FOR_FRAME_COUNT;
|
||||
break;
|
||||
}
|
||||
case WAIT_FOR_FRAME_COUNT:
|
||||
{
|
||||
uart_parsing_data.frame_cnt = c;
|
||||
decode_state = WAIT_FOR_CHECKSUM;
|
||||
#ifdef PRINT_CPP_DEBUG_INFO
|
||||
std::cout << "frame count: " << std::hex << static_cast<int>(frame_cnt) << std::dec << std::endl;
|
||||
#elif (defined(PRINT_JLINK_DEBUG_INFO))
|
||||
JLinkRTTPrintf(0, "frame count: %d\n", frame_cnt);
|
||||
#endif
|
||||
break;
|
||||
}
|
||||
case WAIT_FOR_CHECKSUM:
|
||||
{
|
||||
uart_parsing_data.frame_checksum = c;
|
||||
uart_parsing_data.internal_checksum = CalcBufferedFrameChecksum();
|
||||
new_frame_parsed = true;
|
||||
decode_state = WAIT_FOR_SOF1;
|
||||
#ifdef PRINT_CPP_DEBUG_INFO
|
||||
std::cout << "--- frame checksum: " << std::hex << static_cast<int>(frame_checksum) << std::dec << std::endl;
|
||||
std::cout << "--- internal frame checksum: " << std::hex << static_cast<int>(internal_checksum) << std::dec << std::endl;
|
||||
#elif (defined(PRINT_JLINK_DEBUG_INFO))
|
||||
JLinkRTTPrintf(0, "--- frame checksum: : %d\n", frame_checksum);
|
||||
JLinkRTTPrintf(0, "--- internal frame checksum: : %d\n", internal_checksum);
|
||||
#endif
|
||||
break;
|
||||
}
|
||||
default:
|
||||
break;
|
||||
}
|
||||
|
||||
if (new_frame_parsed)
|
||||
{
|
||||
if (uart_parsing_data.frame_checksum == uart_parsing_data.internal_checksum)
|
||||
{
|
||||
#ifdef PRINT_CPP_DEBUG_INFO
|
||||
std::cout << "checksum correct" << std::endl;
|
||||
#elif (defined(PRINT_JLINK_DEBUG_INFO))
|
||||
JLinkWriteString(0, "checksum correct\n");
|
||||
#endif
|
||||
if (uart_parsing_data.frame_type == FRAME_TYPE_STATUS)
|
||||
ConstructStatusMessage(msg);
|
||||
else if (uart_parsing_data.frame_type == FRAME_TYPE_CONTROL)
|
||||
ConstructControlMessage(msg);
|
||||
++uart_parsing_stats.frame_parsed;
|
||||
}
|
||||
else
|
||||
{
|
||||
++uart_parsing_stats.frame_with_wrong_checksum;
|
||||
#ifdef PRINT_CPP_DEBUG_INFO
|
||||
std::cout << "checksum is NOT correct" << std::endl;
|
||||
std::cout << std::hex << static_cast<int>(frame_id) << " , " << static_cast<int>(frame_len) << " , " << static_cast<int>(frame_cnt) << " , " << static_cast<int>(frame_checksum) << " : " << std::dec << std::endl;
|
||||
std::cout << "payload: ";
|
||||
for (int i = 0; i < payload_data_pos; ++i)
|
||||
std::cout << std::hex << static_cast<int>(payload_buffer[i]) << std::dec << " ";
|
||||
std::cout << std::endl;
|
||||
std::cout << "--- frame checksum: " << std::hex << static_cast<int>(frame_checksum) << std::dec << std::endl;
|
||||
std::cout << "--- internal frame checksum: " << std::hex << static_cast<int>(internal_checksum) << std::dec << std::endl;
|
||||
#elif (defined(PRINT_JLINK_DEBUG_INFO))
|
||||
JLinkWriteString(0, "checksum is NOT correct\n");
|
||||
#endif
|
||||
}
|
||||
}
|
||||
|
||||
return new_frame_parsed;
|
||||
}
|
||||
|
||||
bool ConstructControlMessage(ScoutMessage *msg)
|
||||
{
|
||||
if (msg == NULL)
|
||||
return false;
|
||||
|
||||
switch (uart_parsing_data.frame_id)
|
||||
{
|
||||
case UART_FRAME_MOTION_CONTROL_ID:
|
||||
{
|
||||
msg->type = ScoutMotionControlMsg;
|
||||
msg->body.motion_control_msg.data.cmd.control_mode = uart_parsing_data.payload_buffer[0];
|
||||
msg->body.motion_control_msg.data.cmd.fault_clear_flag = uart_parsing_data.payload_buffer[1];
|
||||
msg->body.motion_control_msg.data.cmd.linear_velocity_cmd = uart_parsing_data.payload_buffer[2];
|
||||
msg->body.motion_control_msg.data.cmd.angular_velocity_cmd = uart_parsing_data.payload_buffer[3];
|
||||
msg->body.motion_control_msg.data.cmd.reserved0 = uart_parsing_data.payload_buffer[4];
|
||||
msg->body.motion_control_msg.data.cmd.reserved1 = uart_parsing_data.payload_buffer[5];
|
||||
msg->body.motion_control_msg.data.cmd.count = uart_parsing_data.frame_cnt;
|
||||
msg->body.motion_control_msg.data.cmd.checksum = uart_parsing_data.frame_checksum;
|
||||
break;
|
||||
}
|
||||
case UART_FRAME_LIGHT_CONTROL_ID:
|
||||
{
|
||||
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_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;
|
||||
break;
|
||||
}
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
bool ConstructStatusMessage(ScoutMessage *msg)
|
||||
{
|
||||
if (msg == NULL)
|
||||
return false;
|
||||
|
||||
switch (uart_parsing_data.frame_id)
|
||||
{
|
||||
case UART_FRAME_SYSTEM_STATUS_ID:
|
||||
{
|
||||
msg->type = ScoutSystemStatusMsg;
|
||||
msg->body.system_status_msg.data.status.base_state = uart_parsing_data.payload_buffer[0];
|
||||
msg->body.system_status_msg.data.status.control_mode = uart_parsing_data.payload_buffer[1];
|
||||
msg->body.system_status_msg.data.status.battery_voltage.high_byte = uart_parsing_data.payload_buffer[2];
|
||||
msg->body.system_status_msg.data.status.battery_voltage.low_byte = uart_parsing_data.payload_buffer[3];
|
||||
msg->body.system_status_msg.data.status.fault_code.high_byte = uart_parsing_data.payload_buffer[4];
|
||||
msg->body.system_status_msg.data.status.fault_code.low_byte = uart_parsing_data.payload_buffer[5];
|
||||
msg->body.system_status_msg.data.status.count = uart_parsing_data.frame_cnt;
|
||||
msg->body.system_status_msg.data.status.checksum = uart_parsing_data.frame_checksum;
|
||||
break;
|
||||
}
|
||||
case UART_FRAME_MOTION_STATUS_ID:
|
||||
{
|
||||
msg->type = ScoutMotionStatusMsg;
|
||||
msg->body.motion_status_msg.data.status.linear_velocity.high_byte = uart_parsing_data.payload_buffer[0];
|
||||
msg->body.motion_status_msg.data.status.linear_velocity.low_byte = uart_parsing_data.payload_buffer[1];
|
||||
msg->body.motion_status_msg.data.status.angular_velocity.high_byte = uart_parsing_data.payload_buffer[2];
|
||||
msg->body.motion_status_msg.data.status.angular_velocity.low_byte = uart_parsing_data.payload_buffer[3];
|
||||
msg->body.motion_status_msg.data.status.reserved0 = 0x00;
|
||||
msg->body.motion_status_msg.data.status.reserved0 = 0x00;
|
||||
msg->body.motion_status_msg.data.status.count = uart_parsing_data.frame_cnt;
|
||||
msg->body.motion_status_msg.data.status.checksum = uart_parsing_data.frame_checksum;
|
||||
break;
|
||||
}
|
||||
case UART_FRAME_MOTOR1_DRIVER_STATUS_ID:
|
||||
{
|
||||
msg->type = ScoutMotorDriverStatusMsg;
|
||||
msg->body.motor_driver_status_msg.motor_id = SCOUT_MOTOR1_ID;
|
||||
msg->body.motor_driver_status_msg.data.status.current.high_byte = uart_parsing_data.payload_buffer[0];
|
||||
msg->body.motor_driver_status_msg.data.status.current.low_byte = uart_parsing_data.payload_buffer[1];
|
||||
msg->body.motor_driver_status_msg.data.status.rpm.high_byte = uart_parsing_data.payload_buffer[2];
|
||||
msg->body.motor_driver_status_msg.data.status.rpm.low_byte = uart_parsing_data.payload_buffer[3];
|
||||
msg->body.motor_driver_status_msg.data.status.temperature = uart_parsing_data.payload_buffer[4];
|
||||
msg->body.motor_driver_status_msg.data.status.reserved0 = 0x00;
|
||||
msg->body.motor_driver_status_msg.data.status.count = uart_parsing_data.frame_cnt;
|
||||
msg->body.motor_driver_status_msg.data.status.checksum = uart_parsing_data.frame_checksum;
|
||||
break;
|
||||
}
|
||||
case UART_FRAME_MOTOR2_DRIVER_STATUS_ID:
|
||||
{
|
||||
msg->type = ScoutMotorDriverStatusMsg;
|
||||
msg->body.motor_driver_status_msg.motor_id = SCOUT_MOTOR2_ID;
|
||||
msg->body.motor_driver_status_msg.data.status.current.high_byte = uart_parsing_data.payload_buffer[0];
|
||||
msg->body.motor_driver_status_msg.data.status.current.low_byte = uart_parsing_data.payload_buffer[1];
|
||||
msg->body.motor_driver_status_msg.data.status.rpm.high_byte = uart_parsing_data.payload_buffer[2];
|
||||
msg->body.motor_driver_status_msg.data.status.rpm.low_byte = uart_parsing_data.payload_buffer[3];
|
||||
msg->body.motor_driver_status_msg.data.status.temperature = uart_parsing_data.payload_buffer[4];
|
||||
msg->body.motor_driver_status_msg.data.status.reserved0 = 0x00;
|
||||
msg->body.motor_driver_status_msg.data.status.count = uart_parsing_data.frame_cnt;
|
||||
msg->body.motor_driver_status_msg.data.status.checksum = uart_parsing_data.frame_checksum;
|
||||
break;
|
||||
}
|
||||
case UART_FRAME_MOTOR3_DRIVER_STATUS_ID:
|
||||
{
|
||||
msg->type = ScoutMotorDriverStatusMsg;
|
||||
msg->body.motor_driver_status_msg.motor_id = SCOUT_MOTOR3_ID;
|
||||
msg->body.motor_driver_status_msg.data.status.current.high_byte = uart_parsing_data.payload_buffer[0];
|
||||
msg->body.motor_driver_status_msg.data.status.current.low_byte = uart_parsing_data.payload_buffer[1];
|
||||
msg->body.motor_driver_status_msg.data.status.rpm.high_byte = uart_parsing_data.payload_buffer[2];
|
||||
msg->body.motor_driver_status_msg.data.status.rpm.low_byte = uart_parsing_data.payload_buffer[3];
|
||||
msg->body.motor_driver_status_msg.data.status.temperature = uart_parsing_data.payload_buffer[4];
|
||||
msg->body.motor_driver_status_msg.data.status.reserved0 = 0x00;
|
||||
msg->body.motor_driver_status_msg.data.status.count = uart_parsing_data.frame_cnt;
|
||||
msg->body.motor_driver_status_msg.data.status.checksum = uart_parsing_data.frame_checksum;
|
||||
break;
|
||||
}
|
||||
case UART_FRAME_MOTOR4_DRIVER_STATUS_ID:
|
||||
{
|
||||
msg->type = ScoutMotorDriverStatusMsg;
|
||||
msg->body.motor_driver_status_msg.motor_id = SCOUT_MOTOR4_ID;
|
||||
msg->body.motor_driver_status_msg.data.status.current.high_byte = uart_parsing_data.payload_buffer[0];
|
||||
msg->body.motor_driver_status_msg.data.status.current.low_byte = uart_parsing_data.payload_buffer[1];
|
||||
msg->body.motor_driver_status_msg.data.status.rpm.high_byte = uart_parsing_data.payload_buffer[2];
|
||||
msg->body.motor_driver_status_msg.data.status.rpm.low_byte = uart_parsing_data.payload_buffer[3];
|
||||
msg->body.motor_driver_status_msg.data.status.temperature = uart_parsing_data.payload_buffer[4];
|
||||
msg->body.motor_driver_status_msg.data.status.reserved0 = 0x00;
|
||||
msg->body.motor_driver_status_msg.data.status.count = uart_parsing_data.frame_cnt;
|
||||
msg->body.motor_driver_status_msg.data.status.checksum = uart_parsing_data.frame_checksum;
|
||||
break;
|
||||
}
|
||||
case UART_FRAME_LIGHT_STATUS_ID:
|
||||
{
|
||||
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_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;
|
||||
break;
|
||||
}
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
uint8_t CalcScoutUARTChecksum(uint8_t *buf, uint8_t len)
|
||||
{
|
||||
uint8_t checksum = 0;
|
||||
|
||||
#ifdef USE_XOR_CHECKSUM
|
||||
for (int i = 0; i < len; ++i)
|
||||
checksum ^= buf[i];
|
||||
#else
|
||||
for (int i = 0; i < len; ++i)
|
||||
checksum += buf[i];
|
||||
#endif
|
||||
|
||||
return checksum;
|
||||
}
|
||||
|
||||
uint8_t CalcBufferedFrameChecksum()
|
||||
{
|
||||
uint8_t checksum = 0x00;
|
||||
|
||||
#ifdef USE_XOR_CHECKSUM
|
||||
checksum ^= FRAME_SOF1;
|
||||
checksum ^= FRAME_SOF2;
|
||||
checksum ^= uart_parsing_data.frame_len;
|
||||
checksum ^= uart_parsing_data.frame_type;
|
||||
checksum ^= uart_parsing_data.frame_id;
|
||||
for (size_t i = 0; i < uart_parsing_data.payload_data_pos; ++i)
|
||||
checksum ^= uart_parsing_data.payload_buffer[i];
|
||||
checksum ^= uart_parsing_data.frame_cnt;
|
||||
#else
|
||||
checksum += FRAME_SOF1;
|
||||
checksum += FRAME_SOF2;
|
||||
checksum += uart_parsing_data.frame_len;
|
||||
checksum += uart_parsing_data.frame_type;
|
||||
checksum += uart_parsing_data.frame_id;
|
||||
for (size_t i = 0; i < uart_parsing_data.payload_data_pos; ++i)
|
||||
checksum += uart_parsing_data.payload_buffer[i];
|
||||
checksum += uart_parsing_data.frame_cnt;
|
||||
#endif
|
||||
|
||||
return checksum;
|
||||
}
|
||||
@@ -1,32 +0,0 @@
|
||||
/*
|
||||
* scout_uart_parser.h
|
||||
*
|
||||
* Created on: Aug 14, 2019 12:01
|
||||
* Description:
|
||||
*
|
||||
* Copyright (c) 2019 Ruixiang Du (rdu)
|
||||
*/
|
||||
|
||||
#ifndef SCOUT_UART_PARSER_H
|
||||
#define SCOUT_UART_PARSER_H
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
#include <stdint.h>
|
||||
#include <string.h>
|
||||
#include <stdbool.h>
|
||||
|
||||
#include "protocol_v1/scout/scout_protocol.h"
|
||||
|
||||
bool DecodeScoutMsgFromUART(uint8_t c, ScoutMessage *msg);
|
||||
void EncodeScoutMsgToUART(const ScoutMessage *msg, uint8_t *buf, uint8_t *len);
|
||||
|
||||
uint8_t CalcScoutUARTChecksum(uint8_t *buf, uint8_t len);
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
|
||||
#endif /* SCOUT_UART_PARSER_H */
|
||||
Reference in New Issue
Block a user