742 lines
20 KiB
C++
742 lines
20 KiB
C++
#include "LedDisplayDevice.h"
|
|
|
|
#include <algorithm>
|
|
#include <cerrno>
|
|
#include <cstdint>
|
|
#include <cstdio>
|
|
#include <cstring>
|
|
#include <string>
|
|
#include <vector>
|
|
|
|
#ifdef _WIN32
|
|
#ifndef NOMINMAX
|
|
#define NOMINMAX
|
|
#endif
|
|
#include <winsock2.h>
|
|
#include <ws2tcpip.h>
|
|
#else
|
|
#include <arpa/inet.h>
|
|
#include <fcntl.h>
|
|
#include <netinet/in.h>
|
|
#include <sys/select.h>
|
|
#include <sys/socket.h>
|
|
#include <unistd.h>
|
|
#endif
|
|
|
|
#include "VrLog.h"
|
|
|
|
namespace {
|
|
|
|
constexpr uint8_t kFrameHead = 0xA5;
|
|
constexpr uint8_t kFrameTail = 0x5A;
|
|
constexpr uint16_t kSourceAddress = 0x8000;
|
|
constexpr uint8_t kProtocolVersion = 0x02;
|
|
constexpr uint8_t kCmdGroupRealtime = 0xA3;
|
|
constexpr uint8_t kCmdRealtimeArea = 0x06;
|
|
constexpr uint8_t kCmdGroupAck = 0xA0;
|
|
constexpr size_t kProtocolHeaderSize = 14;
|
|
constexpr size_t kMaxAreaPacketBytes = 1024;
|
|
|
|
#ifdef _WIN32
|
|
using SocketHandle = SOCKET;
|
|
|
|
SocketHandle InvalidSocket()
|
|
{
|
|
return INVALID_SOCKET;
|
|
}
|
|
|
|
bool SocketIsValid(SocketHandle socket)
|
|
{
|
|
return socket != INVALID_SOCKET;
|
|
}
|
|
|
|
void CloseSocket(SocketHandle socket)
|
|
{
|
|
if (SocketIsValid(socket)) {
|
|
closesocket(socket);
|
|
}
|
|
}
|
|
|
|
int LastSocketError()
|
|
{
|
|
return WSAGetLastError();
|
|
}
|
|
|
|
bool InitializeSocketLibrary()
|
|
{
|
|
static bool initialized = []() {
|
|
WSADATA data;
|
|
return WSAStartup(MAKEWORD(2, 2), &data) == 0;
|
|
}();
|
|
return initialized;
|
|
}
|
|
|
|
bool SetSocketBlocking(SocketHandle socket, bool blocking)
|
|
{
|
|
u_long mode = blocking ? 0 : 1;
|
|
return ioctlsocket(socket, FIONBIO, &mode) == 0;
|
|
}
|
|
|
|
#else
|
|
using SocketHandle = int;
|
|
|
|
SocketHandle InvalidSocket()
|
|
{
|
|
return -1;
|
|
}
|
|
|
|
bool SocketIsValid(SocketHandle socket)
|
|
{
|
|
return socket >= 0;
|
|
}
|
|
|
|
void CloseSocket(SocketHandle socket)
|
|
{
|
|
if (SocketIsValid(socket)) {
|
|
close(socket);
|
|
}
|
|
}
|
|
|
|
int LastSocketError()
|
|
{
|
|
return errno;
|
|
}
|
|
|
|
bool InitializeSocketLibrary()
|
|
{
|
|
return true;
|
|
}
|
|
|
|
bool SetSocketBlocking(SocketHandle socket, bool blocking)
|
|
{
|
|
int flags = fcntl(socket, F_GETFL, 0);
|
|
if (flags < 0) {
|
|
return false;
|
|
}
|
|
|
|
if (blocking) {
|
|
flags &= ~O_NONBLOCK;
|
|
} else {
|
|
flags |= O_NONBLOCK;
|
|
}
|
|
|
|
return fcntl(socket, F_SETFL, flags) == 0;
|
|
}
|
|
#endif
|
|
|
|
SocketHandle ToSocketHandle(intptr_t value)
|
|
{
|
|
return static_cast<SocketHandle>(value);
|
|
}
|
|
|
|
intptr_t FromSocketHandle(SocketHandle socket)
|
|
{
|
|
return static_cast<intptr_t>(socket);
|
|
}
|
|
|
|
std::string SocketErrorMessage(const char* operation, int code)
|
|
{
|
|
const char* operationName = "网络操作";
|
|
if (std::strcmp(operation, "socket") == 0) {
|
|
operationName = "创建套接字";
|
|
} else if (std::strcmp(operation, "set nonblocking") == 0) {
|
|
operationName = "设置非阻塞";
|
|
} else if (std::strcmp(operation, "connect") == 0) {
|
|
operationName = "连接";
|
|
} else if (std::strcmp(operation, "send") == 0) {
|
|
operationName = "发送";
|
|
} else if (std::strcmp(operation, "recv") == 0) {
|
|
operationName = "接收";
|
|
}
|
|
|
|
return std::string(operationName) + "失败，错误码：" + std::to_string(code);
|
|
}
|
|
|
|
uint8_t ClampByte(int value, int minValue = 0, int maxValue = 255)
|
|
{
|
|
return static_cast<uint8_t>((std::max)(minValue, (std::min)(maxValue, value)));
|
|
}
|
|
|
|
uint16_t ClampWord(int value, int minValue = 0, int maxValue = 0xFFFF)
|
|
{
|
|
return static_cast<uint16_t>((std::max)(minValue, (std::min)(maxValue, value)));
|
|
}
|
|
|
|
uint16_t PixelWord(int value)
|
|
{
|
|
return static_cast<uint16_t>(0x8000 | ClampWord(value, 0, 0x7FFF));
|
|
}
|
|
|
|
void AppendU16LE(std::vector<uint8_t>& data, uint16_t value)
|
|
{
|
|
data.push_back(static_cast<uint8_t>(value & 0xFF));
|
|
data.push_back(static_cast<uint8_t>((value >> 8) & 0xFF));
|
|
}
|
|
|
|
void AppendU32LE(std::vector<uint8_t>& data, uint32_t value)
|
|
{
|
|
data.push_back(static_cast<uint8_t>(value & 0xFF));
|
|
data.push_back(static_cast<uint8_t>((value >> 8) & 0xFF));
|
|
data.push_back(static_cast<uint8_t>((value >> 16) & 0xFF));
|
|
data.push_back(static_cast<uint8_t>((value >> 24) & 0xFF));
|
|
}
|
|
|
|
uint16_t ReadU16LE(const std::vector<uint8_t>& data, size_t offset)
|
|
{
|
|
return static_cast<uint16_t>(data[offset] | (static_cast<uint16_t>(data[offset + 1]) << 8));
|
|
}
|
|
|
|
uint16_t CalcCrc16(const uint8_t* data, size_t size)
|
|
{
|
|
uint16_t crc = 0;
|
|
for (size_t i = 0; i < size; ++i) {
|
|
crc ^= data[i];
|
|
for (int bit = 0; bit < 8; ++bit) {
|
|
if ((crc & 0x0001) != 0) {
|
|
crc = static_cast<uint16_t>((crc >> 1) ^ 0xA001);
|
|
} else {
|
|
crc = static_cast<uint16_t>(crc >> 1);
|
|
}
|
|
}
|
|
}
|
|
return crc;
|
|
}
|
|
|
|
uint16_t CalcCrc16(const std::vector<uint8_t>& data)
|
|
{
|
|
return CalcCrc16(data.data(), data.size());
|
|
}
|
|
|
|
void AppendEscaped(std::vector<uint8_t>& frame, uint8_t byte)
|
|
{
|
|
if (byte == 0xA5) {
|
|
frame.push_back(0xA6);
|
|
frame.push_back(0x02);
|
|
} else if (byte == 0xA6) {
|
|
frame.push_back(0xA6);
|
|
frame.push_back(0x01);
|
|
} else if (byte == 0x5A) {
|
|
frame.push_back(0x5B);
|
|
frame.push_back(0x02);
|
|
} else if (byte == 0x5B) {
|
|
frame.push_back(0x5B);
|
|
frame.push_back(0x01);
|
|
} else {
|
|
frame.push_back(byte);
|
|
}
|
|
}
|
|
|
|
std::vector<uint8_t> BuildFrame(const LedDisplayConfig& config, const std::vector<uint8_t>& data)
|
|
{
|
|
std::vector<uint8_t> packet;
|
|
packet.reserve(kProtocolHeaderSize + data.size() + 2);
|
|
AppendU16LE(packet, ClampWord(config.slaveId, 0, 0xFFFF));
|
|
AppendU16LE(packet, kSourceAddress);
|
|
packet.push_back(0x00);
|
|
packet.push_back(0x00);
|
|
packet.push_back(0x00);
|
|
packet.push_back(0x00);
|
|
packet.push_back(0x00);
|
|
packet.push_back(0x01);
|
|
packet.push_back(ClampByte(config.controllerType, 0, 0xFF));
|
|
packet.push_back(kProtocolVersion);
|
|
AppendU16LE(packet, ClampWord(static_cast<int>(data.size()), 0, 0xFFFF));
|
|
packet.insert(packet.end(), data.begin(), data.end());
|
|
|
|
const uint16_t crc = CalcCrc16(packet);
|
|
AppendU16LE(packet, crc);
|
|
|
|
std::vector<uint8_t> frame;
|
|
frame.reserve(packet.size() + 16);
|
|
for (int i = 0; i < 8; ++i) {
|
|
frame.push_back(kFrameHead);
|
|
}
|
|
for (uint8_t byte : packet) {
|
|
AppendEscaped(frame, byte);
|
|
}
|
|
frame.push_back(kFrameTail);
|
|
return frame;
|
|
}
|
|
|
|
bool UnescapeFramePayload(const std::vector<uint8_t>& rawFrame, std::vector<uint8_t>& payload)
|
|
{
|
|
if (rawFrame.size() < 10 || rawFrame.back() != kFrameTail) {
|
|
return false;
|
|
}
|
|
|
|
size_t begin = 0;
|
|
const size_t end = rawFrame.size() - 1;
|
|
while (begin < end && rawFrame[begin] == kFrameHead) {
|
|
++begin;
|
|
}
|
|
|
|
if (begin >= end) {
|
|
return false;
|
|
}
|
|
|
|
payload.clear();
|
|
for (size_t i = begin; i < end; ++i) {
|
|
const uint8_t byte = rawFrame[i];
|
|
if ((byte == 0xA6 || byte == 0x5B) && i + 1 < end) {
|
|
const uint8_t next = rawFrame[++i];
|
|
if (byte == 0xA6 && next == 0x02) {
|
|
payload.push_back(0xA5);
|
|
} else if (byte == 0xA6 && next == 0x01) {
|
|
payload.push_back(0xA6);
|
|
} else if (byte == 0x5B && next == 0x02) {
|
|
payload.push_back(0x5A);
|
|
} else if (byte == 0x5B && next == 0x01) {
|
|
payload.push_back(0x5B);
|
|
} else {
|
|
return false;
|
|
}
|
|
} else {
|
|
payload.push_back(byte);
|
|
}
|
|
}
|
|
return true;
|
|
}
|
|
|
|
bool SetSocketTimeout(SocketHandle socket, int timeoutMs)
|
|
{
|
|
#ifdef _WIN32
|
|
const DWORD timeout = static_cast<DWORD>((std::max)(1, timeoutMs));
|
|
return setsockopt(socket, SOL_SOCKET, SO_RCVTIMEO, reinterpret_cast<const char*>(&timeout), sizeof(timeout)) == 0 &&
|
|
setsockopt(socket, SOL_SOCKET, SO_SNDTIMEO, reinterpret_cast<const char*>(&timeout), sizeof(timeout)) == 0;
|
|
#else
|
|
timeval timeout;
|
|
timeout.tv_sec = (std::max)(1, timeoutMs) / 1000;
|
|
timeout.tv_usec = ((std::max)(1, timeoutMs) % 1000) * 1000;
|
|
return setsockopt(socket, SOL_SOCKET, SO_RCVTIMEO, &timeout, sizeof(timeout)) == 0 &&
|
|
setsockopt(socket, SOL_SOCKET, SO_SNDTIMEO, &timeout, sizeof(timeout)) == 0;
|
|
#endif
|
|
}
|
|
|
|
bool WaitForSocket(SocketHandle socket, bool write, int timeoutMs)
|
|
{
|
|
fd_set fds;
|
|
FD_ZERO(&fds);
|
|
FD_SET(socket, &fds);
|
|
|
|
timeval timeout;
|
|
timeout.tv_sec = (std::max)(1, timeoutMs) / 1000;
|
|
timeout.tv_usec = ((std::max)(1, timeoutMs) % 1000) * 1000;
|
|
|
|
#ifdef _WIN32
|
|
const int nfds = 0;
|
|
#else
|
|
const int nfds = socket + 1;
|
|
#endif
|
|
const int ret = select(nfds, write ? nullptr : &fds, write ? &fds : nullptr, nullptr, &timeout);
|
|
return ret > 0 && FD_ISSET(socket, &fds);
|
|
}
|
|
|
|
bool ConnectSocket(const std::string& ip,
|
|
int port,
|
|
int timeoutMs,
|
|
SocketHandle& socket,
|
|
std::string& error)
|
|
{
|
|
socket = InvalidSocket();
|
|
if (!InitializeSocketLibrary()) {
|
|
error = "初始化套接字库失败";
|
|
return false;
|
|
}
|
|
|
|
sockaddr_in address;
|
|
std::memset(&address, 0, sizeof(address));
|
|
address.sin_family = AF_INET;
|
|
address.sin_port = htons(static_cast<uint16_t>(port));
|
|
if (inet_pton(AF_INET, ip.c_str(), &address.sin_addr) != 1) {
|
|
error = "显示屏网络地址无效：" + ip;
|
|
return false;
|
|
}
|
|
|
|
SocketHandle candidate = ::socket(AF_INET, SOCK_STREAM, IPPROTO_TCP);
|
|
if (!SocketIsValid(candidate)) {
|
|
error = SocketErrorMessage("socket", LastSocketError());
|
|
return false;
|
|
}
|
|
|
|
if (!SetSocketBlocking(candidate, false)) {
|
|
error = SocketErrorMessage("set nonblocking", LastSocketError());
|
|
CloseSocket(candidate);
|
|
return false;
|
|
}
|
|
|
|
int ret = ::connect(candidate, reinterpret_cast<sockaddr*>(&address), sizeof(address));
|
|
if (ret != 0) {
|
|
const int code = LastSocketError();
|
|
#ifdef _WIN32
|
|
const bool inProgress = code == WSAEWOULDBLOCK || code == WSAEINPROGRESS || code == WSAEINVAL;
|
|
#else
|
|
const bool inProgress = code == EINPROGRESS || code == EWOULDBLOCK;
|
|
#endif
|
|
if (!inProgress || !WaitForSocket(candidate, true, timeoutMs)) {
|
|
error = SocketErrorMessage("connect", code);
|
|
CloseSocket(candidate);
|
|
return false;
|
|
}
|
|
|
|
int socketError = 0;
|
|
#ifdef _WIN32
|
|
int socketErrorLen = sizeof(socketError);
|
|
#else
|
|
socklen_t socketErrorLen = sizeof(socketError);
|
|
#endif
|
|
if (getsockopt(candidate, SOL_SOCKET, SO_ERROR, reinterpret_cast<char*>(&socketError), &socketErrorLen) != 0 ||
|
|
socketError != 0) {
|
|
error = SocketErrorMessage("connect", socketError == 0 ? LastSocketError() : socketError);
|
|
CloseSocket(candidate);
|
|
return false;
|
|
}
|
|
}
|
|
|
|
SetSocketBlocking(candidate, true);
|
|
SetSocketTimeout(candidate, timeoutMs);
|
|
socket = candidate;
|
|
return true;
|
|
}
|
|
|
|
bool SendAll(SocketHandle socket, const std::vector<uint8_t>& data, std::string& error)
|
|
{
|
|
size_t sent = 0;
|
|
while (sent < data.size()) {
|
|
const int chunk = static_cast<int>(std::min<size_t>(data.size() - sent, 4096));
|
|
const int ret = send(socket, reinterpret_cast<const char*>(data.data() + sent), chunk, 0);
|
|
if (ret <= 0) {
|
|
error = SocketErrorMessage("send", LastSocketError());
|
|
return false;
|
|
}
|
|
sent += static_cast<size_t>(ret);
|
|
}
|
|
return true;
|
|
}
|
|
|
|
bool ReceiveRawFrame(SocketHandle socket, int timeoutMs, std::vector<uint8_t>& rawFrame, std::string& error)
|
|
{
|
|
rawFrame.clear();
|
|
bool started = false;
|
|
while (rawFrame.size() < 4096) {
|
|
if (!WaitForSocket(socket, false, timeoutMs)) {
|
|
error = "接收超时";
|
|
return false;
|
|
}
|
|
|
|
uint8_t byte = 0;
|
|
const int ret = recv(socket, reinterpret_cast<char*>(&byte), 1, 0);
|
|
if (ret <= 0) {
|
|
error = ret == 0 ? "显示屏关闭连接" : SocketErrorMessage("recv", LastSocketError());
|
|
return false;
|
|
}
|
|
|
|
if (!started) {
|
|
if (byte != kFrameHead) {
|
|
continue;
|
|
}
|
|
started = true;
|
|
}
|
|
|
|
rawFrame.push_back(byte);
|
|
if (byte == kFrameTail) {
|
|
return true;
|
|
}
|
|
}
|
|
|
|
error = "响应帧过大";
|
|
return false;
|
|
}
|
|
|
|
const char* AckErrorToString(uint8_t error)
|
|
{
|
|
switch (error) {
|
|
case 0: return "无错误";
|
|
case 1: return "命令组错误";
|
|
case 2: return "命令不存在";
|
|
case 3: return "控制器忙";
|
|
case 4: return "内存容量超限";
|
|
case 5: return "校验错误";
|
|
case 6: return "文件不存在";
|
|
case 7: return "闪存访问错误";
|
|
case 8: return "文件下载错误";
|
|
case 9: return "文件名错误";
|
|
case 10: return "文件类型错误";
|
|
case 11: return "文件校验错误";
|
|
case 12: return "字库不存在";
|
|
case 13: return "固件类型不匹配";
|
|
case 14: return "日期时间格式错误";
|
|
case 15: return "文件已存在";
|
|
case 16: return "文件块编号错误";
|
|
default: return "未知控制器错误";
|
|
}
|
|
}
|
|
|
|
} // namespace
|
|
|
|
int ILedDisplayDevice::CreateObject(ILedDisplayDevice** ppDevice)
|
|
{
|
|
if (!ppDevice) {
|
|
return -1;
|
|
}
|
|
|
|
*ppDevice = new CLedDisplayDevice();
|
|
return *ppDevice ? 0 : -2;
|
|
}
|
|
|
|
CLedDisplayDevice::CLedDisplayDevice() = default;
|
|
|
|
CLedDisplayDevice::~CLedDisplayDevice()
|
|
{
|
|
Close();
|
|
}
|
|
|
|
int CLedDisplayDevice::Open(const LedDisplayConfig& config)
|
|
{
|
|
std::lock_guard<std::mutex> lock(m_mutex);
|
|
m_config = config;
|
|
|
|
CloseSocketLocked();
|
|
|
|
if (!m_config.enabled) {
|
|
NotifyStatus(ELedDisplayStatus::Disconnected, "显示屏未启用");
|
|
return 0;
|
|
}
|
|
|
|
if (m_config.ip.empty() || m_config.port <= 0 || m_config.port > 65535) {
|
|
NotifyStatus(ELedDisplayStatus::Error, "显示屏网络配置无效");
|
|
return -1;
|
|
}
|
|
|
|
if (!ConnectLocked()) {
|
|
NotifyStatus(ELedDisplayStatus::Error, m_lastError);
|
|
return -2;
|
|
}
|
|
|
|
NotifyStatus(ELedDisplayStatus::Connected, "显示屏已连接");
|
|
return 0;
|
|
}
|
|
|
|
void CLedDisplayDevice::Close()
|
|
{
|
|
std::lock_guard<std::mutex> lock(m_mutex);
|
|
CloseSocketLocked();
|
|
}
|
|
|
|
bool CLedDisplayDevice::IsConnected() const
|
|
{
|
|
std::lock_guard<std::mutex> lock(m_mutex);
|
|
return m_connected && SocketIsValid(ToSocketHandle(m_socket));
|
|
}
|
|
|
|
int CLedDisplayDevice::SendResult(const LedDisplayResult& result)
|
|
{
|
|
std::lock_guard<std::mutex> lock(m_mutex);
|
|
if (!m_config.enabled) {
|
|
return 0;
|
|
}
|
|
|
|
if (!ConnectLocked()) {
|
|
NotifyStatus(ELedDisplayStatus::Error, m_lastError);
|
|
return -1;
|
|
}
|
|
|
|
const std::vector<uint8_t> data = BuildRealtimeData(result);
|
|
if (data.empty()) {
|
|
NotifyStatus(ELedDisplayStatus::Error, "生成显示屏实时数据失败");
|
|
return -2;
|
|
}
|
|
|
|
if (!SendFrameLocked(data)) {
|
|
LOG_ERROR("LED display send failed: %s\n", m_lastError.c_str());
|
|
NotifyStatus(ELedDisplayStatus::Error, m_lastError);
|
|
CloseSocketLocked();
|
|
return -3;
|
|
}
|
|
|
|
if (!ReceiveAckLocked()) {
|
|
LOG_ERROR("LED display ack failed: %s\n", m_lastError.c_str());
|
|
NotifyStatus(ELedDisplayStatus::Error, m_lastError);
|
|
CloseSocketLocked();
|
|
return -4;
|
|
}
|
|
|
|
return 0;
|
|
}
|
|
|
|
const LedDisplayConfig& CLedDisplayDevice::GetConfig() const
|
|
{
|
|
return m_config;
|
|
}
|
|
|
|
void CLedDisplayDevice::SetStatusCallback(LedDisplayStatusCallback callback, void* user)
|
|
{
|
|
std::lock_guard<std::mutex> lock(m_mutex);
|
|
m_statusCallback = callback;
|
|
m_statusUser = user;
|
|
}
|
|
|
|
void CLedDisplayDevice::NotifyStatus(ELedDisplayStatus status, const std::string& message)
|
|
{
|
|
if (m_statusCallback) {
|
|
m_statusCallback(status, message, m_statusUser);
|
|
}
|
|
}
|
|
|
|
bool CLedDisplayDevice::ConnectLocked()
|
|
{
|
|
if (m_connected && SocketIsValid(ToSocketHandle(m_socket))) {
|
|
return true;
|
|
}
|
|
|
|
CloseSocketLocked();
|
|
|
|
SocketHandle socket = InvalidSocket();
|
|
std::string error;
|
|
if (!ConnectSocket(m_config.ip, m_config.port, (std::max)(100, m_config.timeoutMs), socket, error)) {
|
|
SetLastError(error);
|
|
return false;
|
|
}
|
|
|
|
m_socket = FromSocketHandle(socket);
|
|
m_connected = true;
|
|
return true;
|
|
}
|
|
|
|
void CLedDisplayDevice::CloseSocketLocked()
|
|
{
|
|
SocketHandle socket = ToSocketHandle(m_socket);
|
|
if (SocketIsValid(socket)) {
|
|
CloseSocket(socket);
|
|
}
|
|
m_socket = FromSocketHandle(InvalidSocket());
|
|
m_connected = false;
|
|
}
|
|
|
|
bool CLedDisplayDevice::SendFrameLocked(const std::vector<uint8_t>& data)
|
|
{
|
|
const std::vector<uint8_t> frame = BuildFrame(m_config, data);
|
|
std::string error;
|
|
if (!SendAll(ToSocketHandle(m_socket), frame, error)) {
|
|
SetLastError(error);
|
|
return false;
|
|
}
|
|
return true;
|
|
}
|
|
|
|
bool CLedDisplayDevice::ReceiveAckLocked()
|
|
{
|
|
std::vector<uint8_t> rawFrame;
|
|
std::string error;
|
|
if (!ReceiveRawFrame(ToSocketHandle(m_socket), (std::max)(100, m_config.timeoutMs), rawFrame, error)) {
|
|
SetLastError(error);
|
|
return false;
|
|
}
|
|
|
|
std::vector<uint8_t> payload;
|
|
if (!UnescapeFramePayload(rawFrame, payload)) {
|
|
SetLastError("显示屏响应帧无效");
|
|
return false;
|
|
}
|
|
|
|
if (payload.size() < kProtocolHeaderSize + 5 + 2) {
|
|
SetLastError("显示屏响应长度过短");
|
|
return false;
|
|
}
|
|
|
|
const uint16_t expectedCrc = ReadU16LE(payload, payload.size() - 2);
|
|
const uint16_t actualCrc = CalcCrc16(payload.data(), payload.size() - 2);
|
|
if (expectedCrc != actualCrc) {
|
|
SetLastError("显示屏响应校验不一致");
|
|
return false;
|
|
}
|
|
|
|
const size_t dataOffset = kProtocolHeaderSize;
|
|
const uint8_t cmdGroup = payload[dataOffset];
|
|
const uint8_t cmd = payload[dataOffset + 1];
|
|
const uint8_t cmdError = payload[dataOffset + 2];
|
|
if (cmdGroup != kCmdGroupAck) {
|
|
SetLastError("显示屏响应命令组异常");
|
|
return false;
|
|
}
|
|
|
|
if (cmd == 0x00 && cmdError == 0x00) {
|
|
return true;
|
|
}
|
|
|
|
SetLastError(std::string("显示屏拒绝命令：") + AckErrorToString(cmdError) +
|
|
" (" + std::to_string(cmdError) + ")");
|
|
return false;
|
|
}
|
|
|
|
std::vector<uint8_t> CLedDisplayDevice::BuildRealtimeData(const LedDisplayResult& result) const
|
|
{
|
|
std::string text = BuildResultText(result);
|
|
std::vector<uint8_t> textData(text.begin(), text.end());
|
|
|
|
const size_t fixedAreaBytes = 27;
|
|
if (textData.size() + fixedAreaBytes > kMaxAreaPacketBytes) {
|
|
textData.resize(kMaxAreaPacketBytes - fixedAreaBytes);
|
|
}
|
|
|
|
std::vector<uint8_t> area;
|
|
area.reserve(fixedAreaBytes + textData.size());
|
|
area.push_back(0x00);
|
|
AppendU16LE(area, PixelWord(m_config.areaX));
|
|
AppendU16LE(area, ClampWord(m_config.areaY, 0, 0x7FFF));
|
|
AppendU16LE(area, PixelWord(m_config.areaWidth > 0 ? m_config.areaWidth : 64));
|
|
AppendU16LE(area, ClampWord(m_config.areaHeight > 0 ? m_config.areaHeight : 32, 1, 0x7FFF));
|
|
area.push_back(ClampByte(m_config.startAddress, 0, 31));
|
|
area.push_back(0x00);
|
|
area.push_back(0x00);
|
|
AppendU16LE(area, ClampWord(m_config.dynamicTimeoutSec, 0, 0xFFFF));
|
|
area.push_back(0x00);
|
|
area.push_back(0x00);
|
|
area.push_back(0x00);
|
|
area.push_back(0x02);
|
|
area.push_back(0x01);
|
|
area.push_back(ClampByte(m_config.displayMode, 0x01, 0x07));
|
|
area.push_back(0x00);
|
|
area.push_back(ClampByte(m_config.speed, 0, 0x18));
|
|
area.push_back(ClampByte(m_config.stayTime, 0, 0xFF));
|
|
AppendU32LE(area, static_cast<uint32_t>(textData.size()));
|
|
area.insert(area.end(), textData.begin(), textData.end());
|
|
|
|
std::vector<uint8_t> data;
|
|
data.reserve(11 + area.size());
|
|
data.push_back(kCmdGroupRealtime);
|
|
data.push_back(kCmdRealtimeArea);
|
|
data.push_back(0x01);
|
|
data.push_back(0x00);
|
|
data.push_back(0x00);
|
|
data.push_back(0x00);
|
|
data.push_back(0x00);
|
|
data.push_back(0x00);
|
|
data.push_back(0x01);
|
|
AppendU16LE(data, ClampWord(static_cast<int>(area.size()), 0, 0xFFFF));
|
|
data.insert(data.end(), area.begin(), area.end());
|
|
return data;
|
|
}
|
|
|
|
std::string CLedDisplayDevice::BuildResultText(const LedDisplayResult& result) const
|
|
{
|
|
if (!result.valid) {
|
|
return "NO DATA";
|
|
}
|
|
|
|
char buffer[128] = {0};
|
|
const char* state = result.guideCode == 2 ? "ERROR" : "OK";
|
|
std::snprintf(buffer,
|
|
sizeof(buffer),
|
|
"%s\nD:%.0f L:%.0f\nA:%.1f",
|
|
state,
|
|
result.distance,
|
|
result.lateralOffset,
|
|
result.angle);
|
|
return buffer;
|
|
}
|
|
|
|
void CLedDisplayDevice::SetLastError(const std::string& error)
|
|
{
|
|
m_lastError = error;
|
|
}
|