eive-obsw/mission/devices/StarTrackerHandler.cpp

#include "StarTrackerHandler.h"
#include "OBSWConfig.h"

#include <fsfw/globalfunctions/CRC.h>

extern "C" {
    #include <thirdparty/arcsec_star_tracker/client/generated/telemetry.h>
    #include <thirdparty/arcsec_star_tracker/client/generated/actionreq.h>
    #include "common/misc.h"
}

StarTrackerHandler::StarTrackerHandler(object_id_t objectId, object_id_t comIF,
        CookieIF * comCookie) :
        DeviceHandlerBase(objectId, comIF, comCookie), temperatureSet(this), versionSet(this), powerSet(
                this), interfaceSet(this), timeSet(this), solutionSet(this) {
    if (comCookie == NULL) {
        sif::error << "StarTrackerHandler: Invalid com cookie" << std::endl;
    }
    slipInit();
}

StarTrackerHandler::~StarTrackerHandler() {
}

void StarTrackerHandler::doStartUp() {

#if OBSW_SWITCH_TO_NORMAL_MODE_AFTER_STARTUP == 1
//    setMode(MODE_NORMAL);
    setMode(_MODE_TO_ON);
#else
    setMode(_MODE_TO_ON);
#endif
}

void StarTrackerHandler::doShutDown() {
    setMode(_MODE_POWER_DOWN);
}

ReturnValue_t StarTrackerHandler::buildNormalDeviceCommand(DeviceCommandId_t * id) {
    switch (internalState) {
    case InternalState::TEMPERATURE_REQUEST:
        *id = StarTracker::REQ_TEMPERATURE;
        break;
    default:
        sif::debug << "StarTrackerHandler::buildNormalDeviceCommand: Invalid internal step"
                << std::endl;
        break;
    }
    return buildCommandFromCommand(*id, NULL, 0);
}

ReturnValue_t StarTrackerHandler::buildTransitionDeviceCommand(DeviceCommandId_t * id) {
    return NOTHING_TO_SEND;
}

ReturnValue_t StarTrackerHandler::buildCommandFromCommand(DeviceCommandId_t deviceCommand,
        const uint8_t * commandData, size_t commandDataLen) {
    ReturnValue_t result = RETURN_OK;
    switch (deviceCommand) {
    case (StarTracker::PING_REQUEST): {
        preparePingRequest();
        return RETURN_OK;
    }
    case (StarTracker::REQ_TIME): {
        prepareTimeRequest();
        return RETURN_OK;
    }
    case (StarTracker::BOOT): {
        prepareBootCommand();
        return RETURN_OK;
    }
    case (StarTracker::REQ_VERSION): {
        prepareVersionRequest();
        return RETURN_OK;
    }
    case (StarTracker::REQ_INTERFACE): {
        prepareInterfaceRequest();
        return RETURN_OK;
    }
    case (StarTracker::UPLOAD_IMAGE): {
        result = prepareImageUploadCommand(commandData, commandDataLen);
        return result;
    }
    case (StarTracker::REQ_POWER): {
        preparePowerRequest();
        return RETURN_OK;
    }
    case (StarTracker::REBOOT): {
        prepareRebootCommand();
        return RETURN_OK;
    }
    case (StarTracker::SUBSCRIBE_TO_TM): {
        prepareSubscriptionCommand(commandData);
        return RETURN_OK;
    }
    case (StarTracker::REQ_SOLUTION): {
        prepareSolutionRequest();
        return RETURN_OK;
    }
    case (StarTracker::REQ_TEMPERATURE): {
        prepareTemperatureRequest();
        return RETURN_OK;
    }
    default:
        return DeviceHandlerIF::COMMAND_NOT_IMPLEMENTED;
    }
    return HasReturnvaluesIF::RETURN_FAILED;
}

void StarTrackerHandler::fillCommandAndReplyMap() {
    /** Reply lengths are unknown because of the slip encoding. Thus always maximum reply size
      * is specified */
	this->insertInCommandAndReplyMap(StarTracker::PING_REQUEST, 3, nullptr,
			StarTracker::MAX_FRAME_SIZE * 2 + 2);
	this->insertInCommandAndReplyMap(StarTracker::BOOT, 3, nullptr,
			StarTracker::MAX_FRAME_SIZE * 2 + 2);
	this->insertInCommandAndReplyMap(StarTracker::REQ_VERSION, 3, &versionSet,
			StarTracker::MAX_FRAME_SIZE * 2 + 2);
	this->insertInCommandAndReplyMap(StarTracker::REQ_TIME, 3, &timeSet,
			StarTracker::MAX_FRAME_SIZE * 2 + 2);
	this->insertInCommandAndReplyMap(StarTracker::UPLOAD_IMAGE, 3, nullptr,
			StarTracker::MAX_FRAME_SIZE * 2 + 2);
	this->insertInCommandAndReplyMap(StarTracker::REQ_POWER, 3, &powerSet,
			StarTracker::MAX_FRAME_SIZE * 2 + 2);
	this->insertInCommandAndReplyMap(StarTracker::REQ_INTERFACE, 3, &interfaceSet,
			StarTracker::MAX_FRAME_SIZE * 2 + 2);
	// Reboot has no reply. Star tracker reboots immediately
	this->insertInCommandMap(StarTracker::REBOOT);
    this->insertInCommandAndReplyMap(StarTracker::SUBSCRIBE_TO_TM, 3, nullptr,
            StarTracker::MAX_FRAME_SIZE * 2 + 2);
	this->insertInCommandAndReplyMap(StarTracker::REQ_SOLUTION, 3, &solutionSet,
			StarTracker::MAX_FRAME_SIZE * 2 + 2);
	this->insertInCommandAndReplyMap(StarTracker::REQ_TEMPERATURE, 3, &temperatureSet,
			StarTracker::MAX_FRAME_SIZE * 2 + 2);
}

ReturnValue_t StarTrackerHandler::scanForReply(const uint8_t *start, size_t remainingSize,
        DeviceCommandId_t *foundId, size_t *foundLen) {

    ReturnValue_t result = RETURN_OK;
    uint32_t decodedLength = 0;
    size_t bytePos = 0;
    for (bytePos = 0; bytePos < remainingSize; bytePos++) {
        enum arc_dec_result decResult = arc_transport_decode_body(*(start + bytePos), &slipInfo,
                decodedFrame, &decodedLength);

        switch (decResult) {
        case ARC_DEC_INPROGRESS: {
            if (bytePos == remainingSize - 1) {
                // second doSendread() required to read whole packet
                return IGNORE_FULL_PACKET;
            }
            continue;
        }
        case ARC_DEC_ASYNC: {
            sif::debug << "StarTrackerHandler::scanForReply: Received asynchronous tm" << std::endl;
            /** No asynchronous replies are expected as of now */
            return RETURN_FAILED;
        }
        case ARC_DEC_ERROR_FRAME_SHORT:
            return REPLY_TOO_SHORT;
        case ARC_DEC_ERROR_CHECKSUM:
            return CRC_FAILURE;
        case ARC_DEC_SYNC: {
            /** Reset length of SLIP struct for next frame */
            slipInfo.length = 0;
            break;
        }
        default:
            sif::debug << "StarTrackerHandler::scanForReply: Unknown result code" << std::endl;
            break;
        }
    }

    switch (decodedFrame[0]) {
    case TMTC_ACTIONREPLY: {
        *foundLen = bytePos;
        result = scanForActionReply(foundId);
        break;
    }
    case TMTC_SETPARAMREPLY: {
        *foundLen = bytePos;
        result = scanForSetParameterReply(foundId);
        break;
    }
    case TMTC_TELEMETRYREPLYA:
    case TMTC_TELEMETRYREPLY: {
        *foundLen = bytePos;
        result = scanForTmReply(foundId);
        break;
    }
    default: {
        sif::debug << "StarTrackerHandler::scanForReply: Reply has invalid type id" << std::endl;
        result = RETURN_FAILED;
    }
    }

    return result;
}

ReturnValue_t StarTrackerHandler::interpretDeviceReply(DeviceCommandId_t id, const uint8_t *packet) {

    ReturnValue_t result = RETURN_OK;

    switch (id) {
    case (StarTracker::SUBSCRIBE_TO_TM): {
        result = handleSetParamReply();
        break;
    }
    case (StarTracker::REQ_TIME): {
        result = handleTimeTm();
        break;
    }
    case (StarTracker::PING_REQUEST): {
        result = handlePingReply();
        break;
    }
    case (StarTracker::BOOT): {
        result = handleActionReply();
        break;
    }
    case (StarTracker::REQ_VERSION): {
        result = handleVersionTm();
        break;
    }
    case (StarTracker::REQ_INTERFACE): {
        result = handleInterfaceTm();
        break;
    }
    case (StarTracker::UPLOAD_IMAGE): {
        result = handleUploadImageReply();
        break;
    }
    case (StarTracker::REQ_POWER): {
        result = handlePowerTm();
        break;
    }
    case (StarTracker::REQ_SOLUTION): {
        handleSolutionTm();
        break;
    }
    case (StarTracker::REQ_TEMPERATURE): {
        handleTemperatureTm();
        break;
    }
    default: {
        sif::debug << "StarTrackerHandler::interpretDeviceReply: Unknown device reply id:" << id
                << std::endl;
        result = DeviceHandlerIF::UNKNOWN_DEVICE_REPLY;
    }
    }

    return result;
}


void StarTrackerHandler::setNormalDatapoolEntriesInvalid() {

}

uint32_t StarTrackerHandler::getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) {
    return 5000;
}

ReturnValue_t StarTrackerHandler::initializeLocalDataPool(localpool::DataPool& localDataPoolMap,
        LocalDataPoolManager& poolManager) {

    localDataPoolMap.emplace(StarTracker::TICKS_TIME_SET, new PoolEntry<uint32_t>( { 0 }));
    localDataPoolMap.emplace(StarTracker::TIME_TIME_SET, new PoolEntry<uint64_t>( { 0 }));
    localDataPoolMap.emplace(StarTracker::RUN_TIME, new PoolEntry<uint32_t>( { 0 }));
    localDataPoolMap.emplace(StarTracker::UNIX_TIME, new PoolEntry<uint64_t>( { 0 }));

    localDataPoolMap.emplace(StarTracker::TICKS_VERSION_SET, new PoolEntry<uint32_t>( { 0 }));
    localDataPoolMap.emplace(StarTracker::TIME_VERSION_SET, new PoolEntry<uint64_t>( { 0 }));
    localDataPoolMap.emplace(StarTracker::PROGRAM, new PoolEntry<uint8_t>( { 0 }));
    localDataPoolMap.emplace(StarTracker::MAJOR, new PoolEntry<uint8_t>( { 0 }));
    localDataPoolMap.emplace(StarTracker::MINOR, new PoolEntry<uint8_t>( { 0 }));

    localDataPoolMap.emplace(StarTracker::TICKS_INTERFACE_SET, new PoolEntry<uint32_t>( { 0 }));
    localDataPoolMap.emplace(StarTracker::TIME_INTERFACE_SET, new PoolEntry<uint64_t>( { 0 }));
    localDataPoolMap.emplace(StarTracker::FRAME_COUNT, new PoolEntry<uint32_t>( { 0 }));
    localDataPoolMap.emplace(StarTracker::CHECKSUM_ERROR_COUNT, new PoolEntry<uint32_t>( { 0 }));
    localDataPoolMap.emplace(StarTracker::SET_PARAM_COUNT, new PoolEntry<uint32_t>( { 0 }));
    localDataPoolMap.emplace(StarTracker::SET_PARAM_REPLY_COUNT, new PoolEntry<uint32_t>( { 0 }));
    localDataPoolMap.emplace(StarTracker::PARAM_REQUEST_COUNT, new PoolEntry<uint32_t>( { 0 }));
    localDataPoolMap.emplace(StarTracker::PARAM_REPLY_COUNT, new PoolEntry<uint32_t>( { 0 }));
    localDataPoolMap.emplace(StarTracker::REQ_TM_COUNT, new PoolEntry<uint32_t>( { 0 }));
    localDataPoolMap.emplace(StarTracker::TM_REPLY_COUNT, new PoolEntry<uint32_t>( { 0 }));
    localDataPoolMap.emplace(StarTracker::ACTION_REQ_COUNT, new PoolEntry<uint32_t>( { 0 }));
    localDataPoolMap.emplace(StarTracker::ACTION_REPLY_COUNT, new PoolEntry<uint32_t>( { 0 }));

    localDataPoolMap.emplace(StarTracker::TICKS_POWER_SET, new PoolEntry<uint32_t>( { 0 }));
    localDataPoolMap.emplace(StarTracker::TIME_POWER_SET, new PoolEntry<uint64_t>( { 0 }));
    localDataPoolMap.emplace(StarTracker::MCU_CURRENT, new PoolEntry<float>( { 0 }));
    localDataPoolMap.emplace(StarTracker::MCU_VOLTAGE, new PoolEntry<float>( { 0 }));
    localDataPoolMap.emplace(StarTracker::FPGA_CORE_CURRENT, new PoolEntry<float>( { 0 }));
    localDataPoolMap.emplace(StarTracker::FPGA_CORE_VOLTAGE, new PoolEntry<float>( { 0 }));
    localDataPoolMap.emplace(StarTracker::FPGA_18_CURRENT, new PoolEntry<float>( { 0 }));
    localDataPoolMap.emplace(StarTracker::FPGA_18_VOLTAGE, new PoolEntry<float>( { 0 }));
    localDataPoolMap.emplace(StarTracker::FPGA_25_CURRENT, new PoolEntry<float>( { 0 }));
    localDataPoolMap.emplace(StarTracker::FPGA_25_VOLTAGE, new PoolEntry<float>( { 0 }));
    localDataPoolMap.emplace(StarTracker::CMV_21_CURRENT, new PoolEntry<float>( { 0 }));
    localDataPoolMap.emplace(StarTracker::CMV_21_VOLTAGE, new PoolEntry<float>( { 0 }));
    localDataPoolMap.emplace(StarTracker::CMV_PIX_CURRENT, new PoolEntry<float>( { 0 }));
    localDataPoolMap.emplace(StarTracker::CMV_PIX_VOLTAGE, new PoolEntry<float>( { 0 }));
    localDataPoolMap.emplace(StarTracker::CMV_33_CURRENT, new PoolEntry<float>( { 0 }));
    localDataPoolMap.emplace(StarTracker::CMV_33_VOLTAGE, new PoolEntry<float>( { 0 }));
    localDataPoolMap.emplace(StarTracker::CMV_RES_CURRENT, new PoolEntry<float>( { 0 }));
    localDataPoolMap.emplace(StarTracker::CMV_RES_VOLTAGE, new PoolEntry<float>( { 0 }));

    localDataPoolMap.emplace(StarTracker::TICKS_TEMPERATURE_SET, new PoolEntry<uint32_t>( { 0 }));
    localDataPoolMap.emplace(StarTracker::TIME_TEMPERATURE_SET, new PoolEntry<uint64_t>( { 0 }));
    localDataPoolMap.emplace(StarTracker::MCU_TEMPERATURE, new PoolEntry<float>( { 0 }));
    localDataPoolMap.emplace(StarTracker::CMOS_TEMPERATURE, new PoolEntry<float>( { 0 }));

    localDataPoolMap.emplace(StarTracker::TICKS_SOLUTION_SET, new PoolEntry<uint32_t>( { 0 }));
    localDataPoolMap.emplace(StarTracker::TIME_SOLUTION_SET, new PoolEntry<uint64_t>( { 0 }));
    localDataPoolMap.emplace(StarTracker::CALI_QW, new PoolEntry<float>( { 0 }));
    localDataPoolMap.emplace(StarTracker::CALI_QX, new PoolEntry<float>( { 0 }));
    localDataPoolMap.emplace(StarTracker::CALI_QY, new PoolEntry<float>( { 0 }));
    localDataPoolMap.emplace(StarTracker::CALI_QZ, new PoolEntry<float>( { 0 }));
    localDataPoolMap.emplace(StarTracker::TRACK_CONFIDENCE, new PoolEntry<float>( { 0 }));
    localDataPoolMap.emplace(StarTracker::TRACK_QW, new PoolEntry<float>( { 0 }));
    localDataPoolMap.emplace(StarTracker::TRACK_QX, new PoolEntry<float>( { 0 }));
    localDataPoolMap.emplace(StarTracker::TRACK_QY, new PoolEntry<float>( { 0 }));
    localDataPoolMap.emplace(StarTracker::TRACK_QZ, new PoolEntry<float>( { 0 }));
    localDataPoolMap.emplace(StarTracker::TRACK_REMOVED, new PoolEntry<uint8_t>( { 0 }));
    localDataPoolMap.emplace(StarTracker::STARS_CENTROIDED, new PoolEntry<uint8_t>( { 0 }));
    localDataPoolMap.emplace(StarTracker::STARS_MATCHED_DATABASE, new PoolEntry<uint8_t>( { 0 }));
    localDataPoolMap.emplace(StarTracker::LISA_QW, new PoolEntry<float>( { 0 }));
    localDataPoolMap.emplace(StarTracker::LISA_QX, new PoolEntry<float>( { 0 }));
    localDataPoolMap.emplace(StarTracker::LISA_QY, new PoolEntry<float>( { 0 }));
    localDataPoolMap.emplace(StarTracker::LISA_QZ, new PoolEntry<float>( { 0 }));
    localDataPoolMap.emplace(StarTracker::LISA_PERC_CLOSE, new PoolEntry<float>( { 0 }));
    localDataPoolMap.emplace(StarTracker::LISA_NR_CLOSE, new PoolEntry<uint8_t>( { 0 }));
    localDataPoolMap.emplace(StarTracker::TRUST_WORTHY, new PoolEntry<uint8_t>( { 0 }));
    localDataPoolMap.emplace(StarTracker::STABLE_COUNT, new PoolEntry<uint32_t>( { 0 }));
    localDataPoolMap.emplace(StarTracker::SOLUTION_STRATEGY, new PoolEntry<uint8_t>( { 0 }));

    return RETURN_OK;
}

size_t StarTrackerHandler::getNextReplyLength(DeviceCommandId_t commandId){
    return StarTracker::MAX_FRAME_SIZE;
}

ReturnValue_t StarTrackerHandler::scanForActionReply(DeviceCommandId_t *foundId) {
    switch (decodedFrame[1]) {
    case (StarTracker::ID::PING): {
        *foundId = StarTracker::PING_REQUEST;
        break;
    }
    case (StarTracker::ID::BOOT): {
        *foundId = StarTracker::BOOT;
        break;
    }
    case (StarTracker::ID::UPLOAD_IMAGE): {
        *foundId = StarTracker::UPLOAD_IMAGE;
        break;
    }
    default:
        sif::debug << "StarTrackerHandler::scanForSetParameterReply: Unknown parameter reply id"
            << std::endl;
        return RETURN_FAILED;
    }
    return RETURN_OK;
}

ReturnValue_t StarTrackerHandler::scanForSetParameterReply(DeviceCommandId_t *foundId) {
    switch (decodedFrame[1]) {
    case (StarTracker::ID::SUBSCRIBE): {
        *foundId = StarTracker::SUBSCRIBE_TO_TM;
        break;
    }
    default:
        sif::debug << "StarTrackerHandler::scanForSetParameterReply: Unknown parameter reply id"
            << std::endl;
        return RETURN_FAILED;
    }
    return RETURN_OK;
}

ReturnValue_t StarTrackerHandler::scanForTmReply(DeviceCommandId_t *foundId) {
    switch (decodedFrame[1]) {
        case (StarTracker::ID::VERSION): {
            *foundId = StarTracker::REQ_VERSION;
            break;
        }
        case (StarTracker::ID::INTERFACE): {
            *foundId = StarTracker::REQ_INTERFACE;
            break;
        }
        case (StarTracker::ID::POWER): {
            *foundId = StarTracker::REQ_POWER;
            break;
        }
        case (StarTracker::ID::TEMPERATURE): {
            *foundId = StarTracker::REQ_TEMPERATURE;
            break;
        }
        case (StarTracker::ID::TIME): {
            *foundId = StarTracker::REQ_TIME;
            break;
        }
        case (StarTracker::ID::SOLUTION): {
            *foundId = StarTracker::REQ_SOLUTION;
            break;
        }
        default: {
            sif::debug << "StarTrackerHandler::scanForReply: Reply contains invalid reply id"
                    << std::endl;
            return RETURN_FAILED;
            break;
        }
    }
    return RETURN_OK;
}

void StarTrackerHandler::slipInit() {
    slipInfo.buffer = rxBuffer;
    slipInfo.maxlength = StarTracker::MAX_FRAME_SIZE;
    slipInfo.length = 0;
    slipInfo.unescape_next = 0;
    slipInfo.prev_state = SLIP_COMPLETE;
}

void StarTrackerHandler::prepareBootCommand() {
    uint32_t length = 0;
    struct BootActionRequest bootRequest = {BOOT_REGION_ID};
    arc_pack_boot_action_req(&bootRequest, commandBuffer, &length);
    uint32_t encLength = 0;
    arc_transport_encode_body(commandBuffer, length, encBuffer, &encLength);
    rawPacket = encBuffer;
    rawPacketLen = encLength;
}

void StarTrackerHandler::prepareTimeRequest() {
    uint32_t length = 0;
    arc_tm_pack_time_req(commandBuffer, &length);
    uint32_t encLength = 0;
    arc_transport_encode_body(commandBuffer, length, encBuffer, &encLength);
    rawPacket = encBuffer;
    rawPacketLen = encLength;
}

void StarTrackerHandler::preparePingRequest() {
    uint32_t length = 0;
    struct PingActionRequest pingRequest = {PING_ID};
    arc_pack_ping_action_req(&pingRequest, commandBuffer, &length);
    uint32_t encLength = 0;
    arc_transport_encode_body(commandBuffer, length, encBuffer, &encLength);
    rawPacket = encBuffer;
    rawPacketLen = encLength;
}

void StarTrackerHandler::prepareVersionRequest() {
    uint32_t length = 0;
    arc_tm_pack_version_req(commandBuffer, &length);
    uint32_t encLength = 0;
    arc_transport_encode_body(commandBuffer, length, encBuffer, &encLength);
    rawPacket = encBuffer;
    rawPacketLen = encLength;
}

void StarTrackerHandler::prepareInterfaceRequest() {
    uint32_t length = 0;
    arc_tm_pack_interface_req(commandBuffer, &length);
    uint32_t encLength = 0;
    arc_transport_encode_body(commandBuffer, length, encBuffer, &encLength);
    rawPacket = encBuffer;
    rawPacketLen = encLength;
}

ReturnValue_t StarTrackerHandler::prepareImageUploadCommand(const uint8_t* commandData,
        size_t commandDataLen) {
    if (commandDataLen != UPLOAD_COMMAND_LEN) {
        return INVALID_UPLOAD_COMMAND;
    }
    uint32_t length = 0;
    uint32_t position = deserializeUint32(commandData);
    if (position > MAX_POSITION) {
        return MAX_POSITION;
    }
    rememberUploadPosition = position;
    struct UploadActionRequest uploadRequest;
    uploadRequest.position = position;
    std::memcpy(uploadRequest.data, commandData + 4, commandDataLen - 4);
    arc_pack_upload_action_req(&uploadRequest, commandBuffer, &length);
    uint32_t encLength = 0;
    arc_transport_encode_body(commandBuffer, length, encBuffer, &encLength);
    rawPacket = encBuffer;
    rawPacketLen = encLength;
    return RETURN_OK;
}

void StarTrackerHandler::preparePowerRequest() {
    uint32_t length = 0;
    arc_tm_pack_power_req(commandBuffer, &length);
    uint32_t encLength = 0;
    arc_transport_encode_body(commandBuffer, length, encBuffer, &encLength);
    rawPacket = encBuffer;
    rawPacketLen = encLength;
}

void StarTrackerHandler::prepareRebootCommand() {
    uint32_t length = 0;
    struct RebootActionRequest rebootReq;
    arc_pack_reboot_action_req(&rebootReq, commandBuffer, &length);
    uint32_t encLength = 0;
    arc_transport_encode_body(commandBuffer, length, encBuffer, &encLength);
    rawPacket = encBuffer;
    rawPacketLen = encLength;
}

void StarTrackerHandler::prepareSubscriptionCommand(const uint8_t* tmId) {
    uint32_t encLength = 0;
    uint32_t length = 18;
    commandBuffer[0] = TMTC_SETPARAMREQ;
    commandBuffer[1] = StarTracker::ID::SUBSCRIBE;
    // Fill all other fields with invalid tm id
    commandBuffer[2] = *tmId;
    commandBuffer[3] = 0;
    commandBuffer[4] = 0;
    commandBuffer[5] = 0;
    commandBuffer[6] = 0;
    commandBuffer[7] = 0;
    commandBuffer[8] = 0;
    commandBuffer[9] = 0;
    commandBuffer[10] = 0;
    commandBuffer[11] = 0;
    commandBuffer[12] = 0;
    commandBuffer[13] = 0;
    commandBuffer[14] = 0;
    commandBuffer[15] = 0;
    commandBuffer[16] = 0;
    commandBuffer[17] = 0;
    arc_transport_encode_body(commandBuffer, length, encBuffer, &encLength);
    rawPacket = encBuffer;
    rawPacketLen = encLength;
}

void StarTrackerHandler::prepareSolutionRequest() {
    uint32_t length = 0;
    arc_tm_pack_solution_req(commandBuffer, &length);
    uint32_t encLength = 0;
    arc_transport_encode_body(commandBuffer, length, encBuffer, &encLength);
    rawPacket = encBuffer;
    rawPacketLen = encLength;
}

void StarTrackerHandler::prepareTemperatureRequest() {
    uint32_t length = 0;
    arc_tm_pack_temperature_req(commandBuffer, &length);
    uint32_t encLength = 0;
    arc_transport_encode_body(commandBuffer, length, encBuffer, &encLength);
    rawPacket = encBuffer;
    rawPacketLen = encLength;
}

ReturnValue_t StarTrackerHandler::handleSetParamReply() {
    uint8_t status = *(decodedFrame + STATUS_OFFSET);
    if (status != StarTracker::STATUS_OK) {
        sif::warning << "StarTrackerHandler::handleSetParamReply: Failed to execute parameter set "
                " command with parameter ID" <<
                static_cast<unsigned int>(*(decodedFrame + PARAMETER_ID_OFFSET)) << std::endl;
        return SET_PARAM_FAILED;
    }
    return RETURN_OK;
}

ReturnValue_t StarTrackerHandler::handleActionReply() {
    uint8_t status = *(decodedFrame + STATUS_OFFSET);
    if (status != StarTracker::STATUS_OK) {
        sif::warning << "StarTrackerHandler::handleActionReply: Failed to execute action "
                << " command with action ID "
                << static_cast<unsigned int>(*(decodedFrame + ACTION_ID_OFFSET))
                << " and status "<< static_cast<unsigned int>(status) << std::endl;
        return ACTION_FAILED;
    }
    return RETURN_OK;
}

ReturnValue_t StarTrackerHandler::handlePingReply() {
    ReturnValue_t result = RETURN_OK;
    uint32_t pingId = 0;
    uint8_t status = *(decodedFrame + 2);
    const uint8_t* buffer = decodedFrame + 3;
    size_t size = sizeof(pingId);
    SerializeAdapter::deSerialize(&pingId, &buffer, &size, SerializeIF::Endianness::LITTLE);
#if OBSW_VERBOSE_LEVEL >= 1 && OBSW_DEBUG_STARTRACKER == 1
    sif::info << "Ping status: "<< static_cast<unsigned int>(status) << std::endl;
    sif::info << "Ping id: 0x"<< std::hex << pingId << std::endl;
#endif /* OBSW_VERBOSE_LEVEL >= 1 && OBSW_DEBUG_STARTRACKER == 1 */
    if (status != StarTracker::STATUS_OK || pingId != PING_ID) {
        sif::warning << "StarTrackerHandler::handlePingReply: Ping failed" << std::endl;
        result = PING_FAILED;
    }
    return result;
}

ReturnValue_t StarTrackerHandler::handleUploadImageReply() {
    ReturnValue_t result = RETURN_OK;
    result = handleActionReply();
    if (result != RETURN_OK) {
        return result;
    }
    // Position seems to be always 0 (independent of sent position)
//    uint32_t position = deserializeUint32(decodedFrame + ACTION_DATA_OFFSET);
//    if (position != rememberUploadPosition) {
//        sif::warning << "StarTrackerHandler::handleUploadImageReply: Invalid position"
//                << std::endl;
//        return UPLOAD_IMAGE_FAILED;
//    }
    return result;
}

ReturnValue_t StarTrackerHandler::handleTimeTm() {
    ReturnValue_t result = RETURN_OK;
    result = timeSet.read(TIMEOUT_TYPE, MUTEX_TIMEOUT);
    if (result != RETURN_OK) {
        return result;
    }
    uint32_t offset = TM_DATA_FIELD_OFFSET;
    uint8_t status = 0;
    uint32_t ticks = 0;
    uint64_t time = 0;
    getTmHeaderData(&status, &ticks, &time);
    if(status != StarTracker::STATUS_OK) {
        sif::warning << "StarTrackerHandler::handleVersionTm: Reply error: "
                << static_cast<unsigned int>(status) << std::endl;
        result = VERSION_REQ_FAILED;
        return result;
    }
    timeSet.ticks = ticks;
    timeSet.time = time;
    timeSet.runTime = deserializeUint32(decodedFrame + offset);
    offset += sizeof(uint32_t);
    timeSet.unixTime = deserializeUint64(decodedFrame + offset);
    result = timeSet.commit(TIMEOUT_TYPE, MUTEX_TIMEOUT);
    if (result != RETURN_OK) {
        return result;
    }
#if OBSW_VERBOSE_LEVEL >= 1 && OBSW_DEBUG_STARTRACKER == 1
    timeSet.printSet();
#endif /* OBSW_VERBOSE_LEVEL >= 1 && OBSW_DEBUG_STARTRACKER == 1 */
    return result;
}

ReturnValue_t StarTrackerHandler::handleVersionTm() {
    ReturnValue_t result = RETURN_OK;
    uint32_t offset = TM_DATA_FIELD_OFFSET;
    uint8_t status = 0;
    uint32_t ticks = 0;
    uint64_t time = 0;
    getTmHeaderData(&status, &ticks, &time);
    if(status != StarTracker::STATUS_OK) {
        sif::warning << "StarTrackerHandler::handleVersionTm: Reply error: "
                << static_cast<unsigned int>(status) << std::endl;
        result = VERSION_REQ_FAILED;
        return result;
    }
    result = versionSet.read(TIMEOUT_TYPE, MUTEX_TIMEOUT);
    if (result != RETURN_OK) {
        return result;
    }
    versionSet.ticks = ticks;
    versionSet.time = time;
    versionSet.program = *(decodedFrame + offset);
    offset += 1;
    versionSet.major = *(decodedFrame + offset);
    offset += 1;
    versionSet.minor = *(decodedFrame + offset);
    result = versionSet.commit(TIMEOUT_TYPE, MUTEX_TIMEOUT);
    if (result != RETURN_OK) {
        return result;
    }
#if OBSW_VERBOSE_LEVEL >= 1 && OBSW_DEBUG_STARTRACKER == 1
    versionSet.printSet();
#endif /* OBSW_VERBOSE_LEVEL >= 1 && OBSW_DEBUG_STARTRACKER == 1 */
    return result;
}

ReturnValue_t StarTrackerHandler::handleInterfaceTm() {
    ReturnValue_t result = RETURN_OK;
    uint32_t offset = TM_DATA_FIELD_OFFSET;
    uint8_t status = 0;
    uint32_t ticks = 0;
    uint64_t time = 0;
    getTmHeaderData(&status, &ticks, &time);
    if(status != StarTracker::STATUS_OK) {
        sif::warning << "StarTrackerHandler::handleInterfaceTm: Reply error: "
                << static_cast<unsigned int>(status) << std::endl;
        result = INTERFACE_REQ_FAILED;
        return result;
    }
    result = interfaceSet.read(TIMEOUT_TYPE, MUTEX_TIMEOUT);
    if (result != RETURN_OK) {
        return result;
    }
    interfaceSet.ticks = ticks;
    interfaceSet.time = time;
    size_t size = sizeof(uint32_t);
    interfaceSet.frameCount = deserializeUint32(decodedFrame + offset);
    offset += size;
    interfaceSet.checksumerrorCount = deserializeUint32(decodedFrame + offset);
    result = interfaceSet.commit(TIMEOUT_TYPE, MUTEX_TIMEOUT);
    if (result != RETURN_OK) {
        return result;
    }
#if OBSW_VERBOSE_LEVEL >= 1 && OBSW_DEBUG_STARTRACKER == 1
    interfaceSet.printSet();
#endif /* OBSW_VERBOSE_LEVEL >= 1 && OBSW_DEBUG_STARTRACKER == 1 */
    return result;
}

ReturnValue_t StarTrackerHandler::handlePowerTm() {
    ReturnValue_t result = RETURN_OK;
    uint32_t offset = TM_DATA_FIELD_OFFSET;
    uint8_t status = 0;
    uint32_t ticks = 0;
    uint64_t time = 0;
    getTmHeaderData(&status, &ticks, &time);
    if(status != StarTracker::STATUS_OK) {
        sif::warning << "StarTrackerHandler::handlePowerTm: Reply error: "
                << static_cast<unsigned int>(status) << std::endl;
        result = POWER_REQ_FAILED;
        return result;
    }
    result = powerSet.read(TIMEOUT_TYPE, MUTEX_TIMEOUT);
    if (result != RETURN_OK) {
        return result;
    }
    powerSet.ticks= ticks;
    powerSet.time= time;
    float value = 0;
    std::memcpy(&value, decodedFrame + offset, sizeof(value));
    powerSet.mcuCurrent = value;
    offset += 4;
    std::memcpy(&value, decodedFrame + offset, sizeof(value));
    powerSet.mcuVoltage = value;
    offset += 4;
    std::memcpy(&value, decodedFrame + offset, sizeof(value));
    powerSet.fpgaCoreCurrent = value;
    offset += 4;
    std::memcpy(&value, decodedFrame + offset, sizeof(value));
    powerSet.fpgaCoreVoltage = value;
    offset += 4;
    std::memcpy(&value, decodedFrame + offset, sizeof(value));
    powerSet.fpga18Current = value;
    offset += 4;
    std::memcpy(&value, decodedFrame + offset, sizeof(value));
    powerSet.fpga18Voltage = value;
    offset += 4;
    std::memcpy(&value, decodedFrame + offset, sizeof(value));
    powerSet.fpga25Current = value;
    offset += 4;
    std::memcpy(&value, decodedFrame + offset, sizeof(value));
    powerSet.fpga25Voltage = value;
    offset += 4;
    std::memcpy(&value, decodedFrame + offset, sizeof(value));
    powerSet.cmv21Current = value;
    offset += 4;
    std::memcpy(&value, decodedFrame + offset, sizeof(value));
    powerSet.cmv21Voltage = value;
    offset += 4;
    std::memcpy(&value, decodedFrame + offset, sizeof(value));
    powerSet.cmvPixCurrent= value;
    offset += 4;
    std::memcpy(&value, decodedFrame + offset, sizeof(value));
    powerSet.cmvPixVoltage = value;
    offset += 4;
    std::memcpy(&value, decodedFrame + offset, sizeof(value));
    powerSet.cmv33Current= value;
    offset += 4;
    std::memcpy(&value, decodedFrame + offset, sizeof(value));
    powerSet.cmv33Voltage = value;
    offset += 4;
    std::memcpy(&value, decodedFrame + offset, sizeof(value));
    powerSet.cmvResCurrent= value;
    offset += 4;
    std::memcpy(&value, decodedFrame + offset, sizeof(value));
    powerSet.cmvResVoltage = value;
    result = powerSet.commit(TIMEOUT_TYPE, MUTEX_TIMEOUT);
    if (result != RETURN_OK) {
        return result;
    }
#if OBSW_VERBOSE_LEVEL >= 1 && OBSW_DEBUG_STARTRACKER == 1
    powerSet.printSet();
#endif /* OBSW_VERBOSE_LEVEL >= 1 && OBSW_DEBUG_STARTRACKER == 1 */
    return result;
}

ReturnValue_t StarTrackerHandler::handleSolutionTm() {
    ReturnValue_t result = RETURN_OK;
    result = solutionSet.read(TIMEOUT_TYPE, MUTEX_TIMEOUT);
    if (result != RETURN_OK) {
        return result;
    }
    uint32_t offset = TM_DATA_FIELD_OFFSET;
    uint8_t status = 0;
    uint32_t ticks = 0;
    uint64_t time = 0;
    getTmHeaderData(&status, &ticks, &time);
    if(status != StarTracker::STATUS_OK) {
        sif::warning << "StarTrackerHandler::handleSolutioneTm: Reply error: "
                << static_cast<unsigned int>(status) << std::endl;
        result = TEMPERATURE_REQ_FAILED;
        return result;
    }
    solutionSet.ticks= ticks;
    solutionSet.time= time;
    float word = 0;
    std::memcpy(&word, decodedFrame + offset, sizeof(float));
    solutionSet.caliQw = word;
    offset += sizeof(float);
    std::memcpy(&word, decodedFrame + offset, sizeof(float));
    solutionSet.caliQx = word;
    offset += sizeof(float);
    std::memcpy(&word, decodedFrame + offset, sizeof(float));
    solutionSet.caliQy = word;
    offset += sizeof(float);
    std::memcpy(&word, decodedFrame + offset, sizeof(float));
    solutionSet.caliQz = word;
    offset += sizeof(float);
    std::memcpy(&word, decodedFrame + offset, sizeof(float));
    solutionSet.trackConfidence = word;
    offset += sizeof(float);
    std::memcpy(&word, decodedFrame + offset, sizeof(float));
    solutionSet.trackQw = word;
    offset += sizeof(float);
    std::memcpy(&word, decodedFrame + offset, sizeof(float));
    solutionSet.trackQx = word;
    offset += sizeof(float);
    std::memcpy(&word, decodedFrame + offset, sizeof(float));
    solutionSet.trackQy = word;
    offset += sizeof(float);
    std::memcpy(&word, decodedFrame + offset, sizeof(float));
    solutionSet.trackQz = word;
    offset += sizeof(float);
    solutionSet.trackRemoved = *(decodedFrame + offset);
    offset += sizeof(uint8_t);
    solutionSet.starsCentroided = *(decodedFrame + offset);
    offset += sizeof(uint8_t);
    solutionSet.starsMatchedDatabase = *(decodedFrame + offset);
    offset += sizeof(float);
    std::memcpy(&word, decodedFrame + offset, sizeof(float));
    solutionSet.lisaQw = word;
    offset += sizeof(float);
    std::memcpy(&word, decodedFrame + offset, sizeof(float));
    solutionSet.lisaQx = word;
    offset += sizeof(float);
    std::memcpy(&word, decodedFrame + offset, sizeof(float));
    solutionSet.lisaQy = word;
    offset += sizeof(float);
    std::memcpy(&word, decodedFrame + offset, sizeof(float));
    solutionSet.lisaQz = word;
    offset += sizeof(float);
    std::memcpy(&word, decodedFrame + offset, sizeof(float));
    solutionSet.lisaPercentageClose = word;
    offset += sizeof(float);
    solutionSet.lisaNrClose = *(decodedFrame + offset);
    offset += sizeof(uint8_t);
    solutionSet.isTrustWorthy = *(decodedFrame + offset);
    offset += sizeof(uint8_t);
    solutionSet.stableCount = *(decodedFrame + offset);
    offset += sizeof(uint32_t);
    solutionSet.stableCount = *(decodedFrame + offset);
    result = solutionSet.commit(TIMEOUT_TYPE, MUTEX_TIMEOUT);
    if (result != RETURN_OK) {
        return result;
    }
#if OBSW_VERBOSE_LEVEL >= 1 && OBSW_DEBUG_STARTRACKER == 1
    solutionSet.printSet();
#endif
    return result;
}

ReturnValue_t StarTrackerHandler::handleTemperatureTm() {
    ReturnValue_t result = RETURN_OK;
    uint32_t offset = TM_DATA_FIELD_OFFSET;
    uint8_t status = 0;
    uint32_t ticks = 0;
    uint64_t time = 0;
    getTmHeaderData(&status, &ticks, &time);
    if(status != StarTracker::STATUS_OK) {
        sif::warning << "StarTrackerHandler::handleTemperatureTm: Reply error: "
                << static_cast<unsigned int>(status) << std::endl;
        result = TEMPERATURE_REQ_FAILED;
        return result;
    }
    result = temperatureSet.read(TIMEOUT_TYPE, MUTEX_TIMEOUT);
    if (result != RETURN_OK) {
        return result;
    }
    temperatureSet.ticks= ticks;
    temperatureSet.time= time;
    float temperature = 0;
    std::memcpy(&temperature, decodedFrame + offset, sizeof(temperature));
    temperatureSet.mcuTemperature = temperature;
    offset += sizeof(float);
    std::memcpy(&temperature, decodedFrame + offset, sizeof(temperature));
    temperatureSet.cmosTemperature = temperature;
    result = temperatureSet.commit(TIMEOUT_TYPE, MUTEX_TIMEOUT);
    if (result != RETURN_OK) {
        return result;
    }
#if OBSW_VERBOSE_LEVEL >= 1 && OBSW_DEBUG_STARTRACKER == 1
    temperatureSet.printSet();
#endif
    return result;
}

void StarTrackerHandler::getTmHeaderData(uint8_t* status, uint32_t* ticks, uint64_t* time) {
    *status = *(decodedFrame + STATUS_OFFSET);
    *ticks = deserializeUint32(decodedFrame + TICKS_OFFSET);
    *time = deserializeUint64(decodedFrame + TIME_OFFSET);
}

uint32_t StarTrackerHandler::deserializeUint32(const uint8_t* buffer) {
    uint32_t word = 0;
    word = *(buffer + 3) << 24
        | *(buffer + 2) << 16
        | *(buffer + 1) << 8
        | *(buffer);
    return word;
}

uint64_t StarTrackerHandler::deserializeUint64(uint8_t* buffer) {
    uint64_t word = 0;
    word = static_cast<uint64_t>(*(buffer + 7)) << 56
        | static_cast<uint64_t>(*(buffer + 6)) << 48
        | static_cast<uint64_t>(*(buffer + 5)) << 40
        | static_cast<uint64_t>(*(buffer + 4)) << 32
        | static_cast<uint64_t>(*(buffer + 3)) << 24
        | static_cast<uint64_t>(*(buffer + 2)) << 16
        | static_cast<uint64_t>(*(buffer + 1)) << 8
        | static_cast<uint64_t>(*(buffer));
    return word;
}