242 lines
7.5 KiB
C++
242 lines
7.5 KiB
C++
#include "piezoresistive_a_protocol.h"
|
|
|
|
#include <QDateTime>
|
|
#include <qcontainerfwd.h>
|
|
#include <qtypes.h>
|
|
|
|
namespace {
|
|
constexpr quint8 kReplyStart0 = 0x55; // 0x55AA little-endian
|
|
constexpr quint8 kReplyStart1 = 0xAA;
|
|
constexpr quint8 kReplyStartAlt0 = 0xAA;
|
|
constexpr quint8 kReplyStartAlt1 = 0x55;
|
|
constexpr quint8 kRequestStart0 = 0x55; // 0x55AA little-endian
|
|
constexpr quint8 kRequestStart1 = 0xAA;
|
|
|
|
quint16 readLe16(const QByteArray& data, int offset) {
|
|
const quint8 b0 = static_cast<quint8>(data[offset]);
|
|
const quint8 b1 = static_cast<quint8>(data[offset + 1]);
|
|
return static_cast<quint16>(b0 | (b1 << 8));
|
|
}
|
|
|
|
quint32 readLe32(const QByteArray& data, int offset) {
|
|
const quint8 b0 = static_cast<quint8>(data[offset]);
|
|
const quint8 b1 = static_cast<quint8>(data[offset + 1]);
|
|
const quint8 b2 = static_cast<quint8>(data[offset + 2]);
|
|
const quint8 b3 = static_cast<quint8>(data[offset + 3]);
|
|
return static_cast<quint32>(b0 | (b1 << 8) | (b2 << 16) | (b3 << 24));
|
|
}
|
|
|
|
void appendLe16(QByteArray& data, quint16 v) {
|
|
data.append(static_cast<char>(v & 0xFF));
|
|
data.append(static_cast<char>((v >> 8) & 0xFF));
|
|
}
|
|
|
|
void appendLe32(QByteArray& data, quint32 v) {
|
|
data.append(static_cast<char>(v & 0xFF));
|
|
data.append(static_cast<char>((v >> 8) & 0xFF));
|
|
data.append(static_cast<char>((v >> 16) & 0xFF));
|
|
data.append(static_cast<char>((v >> 24) & 0xFF));
|
|
}
|
|
}
|
|
|
|
quint8 PiezoresistiveAFormat::crc8ITU(const QByteArray& data, int length) {
|
|
quint8 crc = 0x00;
|
|
const int limit = qMin(length, data.size());
|
|
for (int i = 0; i < limit; ++i) {
|
|
crc ^= static_cast<quint8>(data[i]);
|
|
for (int bit = 0; bit < 8; ++bit) {
|
|
if (crc & 0x80)
|
|
crc = static_cast<quint8>((crc << 1) ^ 0x07);
|
|
else
|
|
crc = static_cast<quint8>(crc << 1);
|
|
}
|
|
}
|
|
return static_cast<quint8>(crc ^ 0x55);
|
|
}
|
|
|
|
ISerialFormat::ParseResult PiezoresistiveAFormat::tryParse(QByteArray* buffer, QByteArray* packet, QString* error) {
|
|
if (!buffer || buffer->isEmpty())
|
|
return ParseResult::NeedMore;
|
|
|
|
int startIndex = -1;
|
|
for (int i = 0; i + 1 < buffer->size(); ++i) {
|
|
if (static_cast<quint8>((*buffer)[i]) == kReplyStart0 &&
|
|
static_cast<quint8>((*buffer)[i + 1]) == kReplyStart1) {
|
|
startIndex = i;
|
|
break;
|
|
}
|
|
if (static_cast<quint8>((*buffer)[i]) == kReplyStartAlt0 &&
|
|
static_cast<quint8>((*buffer)[i + 1]) == kReplyStartAlt1) {
|
|
startIndex = i;
|
|
break;
|
|
}
|
|
}
|
|
|
|
if (startIndex < 0) {
|
|
const quint8 tail = static_cast<quint8>(buffer->back());
|
|
buffer->clear();
|
|
if (tail == kReplyStart0 || tail == kReplyStart1)
|
|
buffer->append(static_cast<char>(tail));
|
|
return ParseResult::NeedMore;
|
|
}
|
|
|
|
if (startIndex > 0)
|
|
buffer->remove(0, startIndex);
|
|
|
|
if (buffer->size() < 5)
|
|
return ParseResult::NeedMore;
|
|
|
|
const quint16 payloadLen = readLe16(*buffer, 2);
|
|
const int totalLen = 4 + payloadLen + 1;
|
|
if (buffer->size() < totalLen)
|
|
return ParseResult::NeedMore;
|
|
|
|
QByteArray candidate = buffer->left(totalLen);
|
|
buffer->remove(0, totalLen);
|
|
|
|
const quint8 crc = static_cast<quint8>(candidate.at(candidate.size() - 1));
|
|
const quint8 calc = crc8ITU(candidate, candidate.size() - 1);
|
|
if (crc != calc) {
|
|
if (error)
|
|
*error = QStringLiteral("CRC mismatch");
|
|
return ParseResult::Invalid;
|
|
}
|
|
|
|
if (packet)
|
|
*packet = candidate;
|
|
if (error)
|
|
error->clear();
|
|
return ParseResult::Ok;
|
|
}
|
|
|
|
QByteArray PiezoresistiveACodec::buildRequest(const SerialConfig& config, const SensorRequest& request) {
|
|
QByteArray packet;
|
|
packet.reserve(15);
|
|
|
|
packet.append(static_cast<char>(kRequestStart0));
|
|
packet.append(static_cast<char>(kRequestStart1));
|
|
|
|
const quint16 payloadLen = 9;
|
|
appendLe16(packet, payloadLen);
|
|
|
|
packet.append(static_cast<char>(config.deviceAddress));
|
|
packet.append(static_cast<char>(0x00));
|
|
packet.append(static_cast<char>(0x80 | request.functionCode));
|
|
|
|
appendLe32(packet, request.startAddress);
|
|
appendLe16(packet, request.dataLength);
|
|
|
|
const quint8 crc = PiezoresistiveAFormat::crc8ITU(packet, packet.size());
|
|
packet.append(static_cast<char>(crc));
|
|
|
|
return packet;
|
|
}
|
|
|
|
QByteArray PiezoresistiveACodec::buildGetVersionRequest(const SerialConfig& config) {
|
|
// TODO:待实现内容(压阻 A 型版本号查询请求帧)
|
|
Q_UNUSED(config)
|
|
return QByteArray();
|
|
}
|
|
|
|
QByteArray PiezoresistiveACodec::buildGetSpecRequest(const SerialConfig& config) {
|
|
// TODO:待实现内容(压阻 A 型规格查询请求帧)
|
|
Q_UNUSED(config)
|
|
return QByteArray();
|
|
}
|
|
|
|
bool PiezoresistiveADecoder::decodeFrame(const QByteArray& packet, DataFrame* frame, QString* error) {
|
|
if (!frame) {
|
|
if (error)
|
|
*error = QStringLiteral("Null frame output");
|
|
return false;
|
|
}
|
|
|
|
if (packet.size() < 15) {
|
|
if (error)
|
|
*error = QStringLiteral("Packet too short");
|
|
return false;
|
|
}
|
|
|
|
const quint8 start0 = static_cast<quint8>(packet[0]);
|
|
const quint8 start1 = static_cast<quint8>(packet[1]);
|
|
const bool startOk = (start0 == kReplyStart0 && start1 == kReplyStart1) ||
|
|
(start0 == kReplyStartAlt0 && start1 == kReplyStartAlt1);
|
|
if (!startOk) {
|
|
if (error)
|
|
*error = QStringLiteral("Bad start bytes");
|
|
return false;
|
|
}
|
|
|
|
const quint16 payloadLen = readLe16(packet, 2);
|
|
const int totalLen = 4 + payloadLen + 1;
|
|
if (packet.size() != totalLen) {
|
|
if (error)
|
|
*error = QStringLiteral("Length mismatch");
|
|
return false;
|
|
}
|
|
|
|
const quint8 crc = static_cast<quint8>(packet.at(packet.size() - 1));
|
|
const quint8 calc = PiezoresistiveAFormat::crc8ITU(packet, packet.size() - 1);
|
|
if (crc != calc) {
|
|
if (error)
|
|
*error = QStringLiteral("CRC mismatch");
|
|
return false;
|
|
}
|
|
|
|
const quint8 funcRaw = static_cast<quint8>(packet[6]);
|
|
const quint16 dataLen = readLe16(packet, 11);
|
|
const quint8 status = static_cast<quint8>(packet[13]);
|
|
|
|
if (payloadLen != static_cast<quint16>(10 + dataLen)) {
|
|
if (error)
|
|
*error = QStringLiteral("Payload length mismatch");
|
|
return false;
|
|
}
|
|
|
|
if (status != 0) {
|
|
if (error)
|
|
*error = QStringLiteral("Device status error");
|
|
return false;
|
|
}
|
|
|
|
if (packet.size() < 15 + dataLen) {
|
|
if (error)
|
|
*error = QStringLiteral("Data length mismatch");
|
|
return false;
|
|
}
|
|
|
|
if ((dataLen % 2) != 0) {
|
|
if (error)
|
|
*error = QStringLiteral("Odd data length");
|
|
return false;
|
|
}
|
|
|
|
const int sampleCount = dataLen / 2;
|
|
QVector<float> values;
|
|
values.reserve(sampleCount);
|
|
|
|
const int dataOffset = 14;
|
|
for (int i = 0; i < sampleCount; ++i) {
|
|
const int offset = dataOffset + i * 2;
|
|
const quint16 raw = readLe16(packet, offset);
|
|
values.push_back(static_cast<float>(raw));
|
|
}
|
|
|
|
frame->pts = DataFrame::makePts(QDateTime::currentDateTime());
|
|
frame->functionCode = (funcRaw >= 0x80) ? static_cast<quint8>(funcRaw - 0x80) : funcRaw;
|
|
frame->data = values;
|
|
|
|
if (error)
|
|
error->clear();
|
|
return true;
|
|
}
|
|
|
|
bool PiezoresistiveADecoder::decodeSpec(const QByteArray& packet, SensorSpec* spec, QString* error) {
|
|
// TODO:待实现内容(解析压阻 A 型规格回复并写入 SensorSpec)
|
|
Q_UNUSED(packet)
|
|
Q_UNUSED(spec)
|
|
if (error)
|
|
*error = QStringLiteral("Not implemented");
|
|
return false;
|
|
}
|