eive-obsw/bsp_q7s/devices/startracker/ArcsecDatalinkLayer.cpp

#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);
}