79 lines
2.1 KiB
C++
79 lines
2.1 KiB
C++
#include "ArcsecDatalinkLayer.h"
|
|
|
|
ArcsecDatalinkLayer::ArcsecDatalinkLayer() {
|
|
slipInit();
|
|
}
|
|
|
|
ArcsecDatalinkLayer::~ArcsecDatalinkLayer() {
|
|
}
|
|
|
|
void ArcsecDatalinkLayer::slipInit() {
|
|
slipInfo.buffer = rxBuffer;
|
|
slipInfo.maxlength = StarTracker::MAX_FRAME_SIZE;
|
|
slipInfo.length = 0;
|
|
slipInfo.unescape_next = 0;
|
|
slipInfo.prev_state = SLIP_COMPLETE;
|
|
}
|
|
|
|
ReturnValue_t ArcsecDatalinkLayer::decodeFrame(const uint8_t* rawData, size_t rawDataSize,
|
|
size_t* bytesLeft) {
|
|
size_t bytePos = 0;
|
|
for (bytePos = 0; bytePos < rawDataSize; bytePos++) {
|
|
enum arc_dec_result decResult = arc_transport_decode_body(*(rawData + bytePos), &slipInfo,
|
|
decodedFrame, &decFrameSize);
|
|
*bytesLeft = rawDataSize - bytePos - 1;
|
|
switch (decResult) {
|
|
case ARC_DEC_INPROGRESS: {
|
|
if (bytePos == rawDataSize - 1) {
|
|
return DEC_IN_PROGRESS;
|
|
}
|
|
continue;
|
|
}
|
|
case ARC_DEC_ERROR_FRAME_SHORT:
|
|
return REPLY_TOO_SHORT;
|
|
case ARC_DEC_ERROR_CHECKSUM:
|
|
return CRC_FAILURE;
|
|
case ARC_DEC_ASYNC:
|
|
case ARC_DEC_SYNC: {
|
|
// Reset length of SLIP struct for next frame
|
|
slipInfo.length = 0;
|
|
return RETURN_OK;
|
|
}
|
|
default:
|
|
sif::debug << "ArcsecDatalinkLayer::decodeFrame: Unknown result code" << std::endl;
|
|
break;
|
|
return RETURN_FAILED;
|
|
}
|
|
}
|
|
return RETURN_FAILED;
|
|
}
|
|
|
|
uint8_t ArcsecDatalinkLayer::getReplyFrameType() {
|
|
return decodedFrame[0];
|
|
}
|
|
|
|
const uint8_t* ArcsecDatalinkLayer::getReply() {
|
|
return &decodedFrame[1];
|
|
}
|
|
|
|
void ArcsecDatalinkLayer::encodeFrame(const uint8_t* data, uint32_t length) {
|
|
arc_transport_encode_body(data, length, encBuffer, &encFrameSize);
|
|
}
|
|
|
|
uint8_t* ArcsecDatalinkLayer::getEncodedFrame() {
|
|
return encBuffer;
|
|
}
|
|
|
|
uint32_t ArcsecDatalinkLayer::getEncodedLength() {
|
|
return encFrameSize;
|
|
}
|
|
|
|
uint8_t ArcsecDatalinkLayer::getStatusField() {
|
|
return *(decodedFrame + STATUS_OFFSET);
|
|
}
|
|
|
|
uint8_t ArcsecDatalinkLayer::getId() {
|
|
return *(decodedFrame + ID_OFFSET);
|
|
}
|
|
|