98 lines
3.4 KiB
C++
98 lines
3.4 KiB
C++
//
|
||
// Created by lgv on 2025/7/17.
|
||
//
|
||
|
||
#pragma once
|
||
|
||
#include "canbus/can_comm/protocol_data.h"
|
||
#include "cmvr/msgs/robot_detail.pb.h"
|
||
#include "sdo_response_protocol.h"
|
||
#include "common/base/logging/logger.h"
|
||
namespace cmvr {
|
||
namespace device {
|
||
template<typename SensorType>
|
||
class SdoResponseProtocol : public ProtocolData<SensorType> {
|
||
public:
|
||
static constexpr uint32_t BASE_ID = 0x580;
|
||
|
||
static uint32_t ID(uint8_t node_id) {
|
||
return BASE_ID + node_id;
|
||
}
|
||
uint32_t ID() const{
|
||
return BASE_ID + node_id_;
|
||
}
|
||
|
||
explicit SdoResponseProtocol(uint8_t node_id) : node_id_(node_id), message_id_(BASE_ID + node_id) {
|
||
}
|
||
|
||
void Parse(const std::uint8_t *bytes, int32_t length, SensorType *sensor_data) const override;
|
||
|
||
virtual void ParseSdoData(const msgs::SdoFrame &sdo_response, SensorType *sensor_data) const {}
|
||
|
||
void UpdateData(uint8_t *data) override {}
|
||
|
||
protected:
|
||
uint8_t node_id_;
|
||
uint32_t message_id_;
|
||
mutable msgs::SdoFrame sdo_response_; // 成员变量
|
||
};
|
||
|
||
|
||
template<typename SensorType>
|
||
void SdoResponseProtocol<SensorType>::Parse(const std::uint8_t *bytes, int32_t length,SensorType *sensor_data) const {
|
||
if (length < 8) {
|
||
CMVR_LOG(WARNING) << "MotorSdoResponseProtocol: data length too short: " << length;
|
||
return;
|
||
}
|
||
|
||
// 解析 command 字段（第0字节）
|
||
auto command = static_cast<msgs::CommandSpecifier>(bytes[0]);
|
||
|
||
// 解析 index（字节1和字节2，低字节优先）
|
||
const uint32_t index = bytes[1] + (bytes[2] << 8);
|
||
|
||
// 解析 subindex（字节3）
|
||
const uint32_t subindex = bytes[3];
|
||
|
||
// 根据 command 解析 data（字节4~7）
|
||
uint32_t data = 0;
|
||
switch (command) {
|
||
case msgs::CS_READ_RESPONSE_ONE_BYTE:
|
||
data = bytes[4];
|
||
break;
|
||
case msgs::CS_READ_RESPONSE_TWO_BYTES:
|
||
data = bytes[5] + (bytes[4] << 8);
|
||
break;
|
||
case msgs::CS_READ_RESPONSE_THREE_BYTES:
|
||
data = bytes[4] + (bytes[5] << 8) + (bytes[6] << 16);
|
||
(static_cast<uint32_t>(bytes[5]) << 8) |
|
||
(static_cast<uint32_t>(bytes[6]) << 16);
|
||
break;
|
||
case msgs::CS_READ_RESPONSE_FOUR_BYTES:
|
||
data = bytes[4] + (bytes[5] << 8) + (bytes[6] << 16) + (bytes[7] << 24);
|
||
break;
|
||
|
||
case msgs::CS_WRITE_SUCCESS_RESPONSE:
|
||
data = 0;
|
||
break;
|
||
case msgs::CS_EXCEPTION_RESPONSE:
|
||
break;
|
||
|
||
default:
|
||
CMVR_LOG(WARNING) << "Unknown command specifier: " << std::hex << static_cast<int>(command);
|
||
data = 0;
|
||
break;
|
||
}
|
||
|
||
sdo_response_.set_node_id(node_id_);
|
||
sdo_response_.set_cs(command);
|
||
sdo_response_.set_index(index);
|
||
sdo_response_.set_sub_index(subindex);
|
||
sdo_response_.set_data(data);
|
||
|
||
// 子类实现的扩展解析data逻辑
|
||
ParseSdoData(sdo_response_, sensor_data);
|
||
}
|
||
}
|
||
}
|