mirror of
https://github.com/westonrobot/ugv_sdk
synced 2023-04-08 06:32:14 +08:00
added parser for scout
This commit is contained in:
Ruixiang Du
@@ -7,12 +7,130 @@
|
||||
* Copyright (c) 2021 Ruixiang Du (rdu)
|
||||
*/
|
||||
|
||||
#include "agilex_protocol_v1.h"
|
||||
#include "protocol_v1/agilex_msg_parser_v1.h"
|
||||
#include "ugv_sdk/details/protocol_v1/agilex_protocol_v1.h"
|
||||
#include "ugv_sdk/details/protocol_v1//agilex_msg_parser_v1.h"
|
||||
|
||||
#include "stdio.h"
|
||||
#include "string.h"
|
||||
|
||||
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) {}
|
||||
bool DecodeCanFrameV1(const struct can_frame *rx_frame, AgxMessage *msg) {
|
||||
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));
|
||||
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));
|
||||
break;
|
||||
}
|
||||
case CAN_MSG_LIGHT_STATE_ID: {
|
||||
msg->type = AgxMsgLightState;
|
||||
// msg->light_status_msg.id = CAN_MSG_LIGHT_CONTROL_STATUS_ID;
|
||||
// memcpy(msg->body.light_state_msg.data.raw, rx_frame->data,
|
||||
// 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));
|
||||
break;
|
||||
}
|
||||
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 = SCOUT_MOTOR4_ID;
|
||||
// memcpy(msg->body.motor_driver_status_msg.data.raw, rx_frame->data,
|
||||
// rx_frame->can_dlc * sizeof(uint8_t));
|
||||
break;
|
||||
}
|
||||
default:
|
||||
break;
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
void EncodeCanFrameV1(const AgxMessage *msg, struct can_frame *tx_frame) {
|
||||
switch (msg->type) {
|
||||
case AgxMsgMotionCommandV1: {
|
||||
static uint8_t count = 0;
|
||||
tx_frame->can_id = CAN_MSG_MOTION_COMMAND_ID;
|
||||
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[4] = 0;
|
||||
tx_frame->data[5] = 0;
|
||||
tx_frame->data[6] = count++;
|
||||
tx_frame->data[7] = CalcCanFrameChecksumV1(
|
||||
tx_frame->can_id, tx_frame->data, tx_frame->can_dlc);
|
||||
break;
|
||||
}
|
||||
case AgxMsgValueSetCommandV1: {
|
||||
static uint8_t count = 0;
|
||||
tx_frame->can_id = CAN_MSG_LIGHT_COMMAND_ID;
|
||||
tx_frame->can_dlc = 8;
|
||||
LightCommandFrame frame;
|
||||
if (msg->body.light_command_msg.enable_cmd_ctrl) {
|
||||
frame.enable_cmd_ctrl = LIGHT_ENABLE_CMD_CTRL;
|
||||
frame.front_mode = msg->body.light_command_msg.front_light.mode;
|
||||
frame.front_custom =
|
||||
msg->body.light_command_msg.front_light.custom_value;
|
||||
frame.rear_mode = msg->body.light_command_msg.rear_light.mode;
|
||||
frame.rear_custom = msg->body.light_command_msg.rear_light.custom_value;
|
||||
} else {
|
||||
frame.enable_cmd_ctrl = LIGHT_DISABLE_CMD_CTRL;
|
||||
frame.front_mode = 0;
|
||||
frame.front_custom = 0;
|
||||
frame.rear_mode = 0;
|
||||
frame.rear_custom = 0;
|
||||
}
|
||||
frame.reserved0 = 0;
|
||||
frame.count = count++;
|
||||
memcpy(tx_frame->data, (uint8_t *)(&frame), tx_frame->can_dlc);
|
||||
tx_frame->data[7] = CalcCanFrameChecksumV1(
|
||||
tx_frame->can_id, tx_frame->data, tx_frame->can_dlc);
|
||||
break;
|
||||
}
|
||||
case AgxMsgLightCommand: {
|
||||
break;
|
||||
}
|
||||
default:
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
uint8_t CalcCanFrameChecksumV1(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,41 +0,0 @@
|
||||
/*
|
||||
* 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 "ugv_sdk/details/interface/agilex_message.h"
|
||||
|
||||
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
|
||||
}
|
||||
#endif
|
||||
|
||||
#endif /* AGILEX_MSG_PARSER_H */
|
||||
@@ -1,197 +0,0 @@
|
||||
/*
|
||||
* 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)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 /* AGILEX_PROTOCOL_V1_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"
|
||||
|
||||
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
|
||||
Reference in New Issue
Block a user