star tracker commands, tracking, mounting, camera, blob

bsp_q7s/devices/startracker/ArcsecDatalinkLayer.cpp

@@ -0,0 +1,78 @@
+#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);
+}
+