eive-obsw/mission/devices/StarTrackerHandler.cpp

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

#include <fsfw/globalfunctions/CRC.h>
#include <fsfw/datapool/PoolReadGuard.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) {
    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) {

    switch (deviceCommand) {
    case (StarTracker::PING_REQUEST): {
        preparePingRequest();
        return RETURN_OK;
    }
    case (StarTracker::REBOOT): {
        prepareRebootCommand();
        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, 1, nullptr,
			StarTracker::MAX_FRAME_SIZE * 2 + 2);
	this->insertInCommandMap(StarTracker::REBOOT);
	this->insertInCommandAndReplyMap(StarTracker::REQ_TEMPERATURE, 1, &temperatureSet,
			StarTracker::MAX_FRAME_SIZE * 2 + 2);
}

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

    uint32_t decodedLength = 0;

    for (size_t idx = 0; idx < remainingSize; idx++) {
        enum arc_dec_result decResult = arc_transport_decode_body(*(start + idx), &slipInfo,
                decodedFrame, &decodedLength);

        switch (decResult) {
        case ARC_DEC_INPROGRESS: {
            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[1]) {
    case (static_cast<uint8_t>(StarTracker::PING_REQUEST)): {
        *foundLen = decodedLength;
        *foundId = StarTracker::PING_REQUEST;
        break;
    }
    case (static_cast<uint8_t>(StarTracker::REQ_TEMPERATURE)): {
        *foundLen = decodedLength;
        *foundId = StarTracker::REQ_TEMPERATURE;
        break;
    }
    default: {
        sif::debug << "StarTrackerHandler::scanForReply: Reply contains invalid reply id"
                << std::endl;
        return RETURN_FAILED;
        break;
    }
    }

    return RETURN_OK;
}

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

    ReturnValue_t result = RETURN_OK;

    switch (id) {
    case (StarTracker::PING_REQUEST): {
        result = handlePingReply();
        break;
    }
    case (StarTracker::REQ_TEMPERATURE): {
        handleTemperatureTm();
        break;
    }
    default: {
        sif::debug << "StarTrackerHandler::interpretDeviceReply: Unknown device reply id:" << id
                << std::endl;
        return 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, new PoolEntry<uint32_t>( { 0 }));
    localDataPoolMap.emplace(StarTracker::TIME, new PoolEntry<uint64_t>( { 0 }));
    localDataPoolMap.emplace(StarTracker::MCU_TEMPERATURE, new PoolEntry<float>( { 0 }));
    localDataPoolMap.emplace(StarTracker::CMOS_TEMPERATURE, new PoolEntry<float>( { 0 }));

    return RETURN_OK;
}

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

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::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::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::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::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) {
        result = PING_FAILED;
    }
    return result;
}

ReturnValue_t StarTrackerHandler::handleTemperatureTm() {
    ReturnValue_t result = RETURN_OK;
    PoolReadGuard rg(&temperatureSet);
    uint32_t offset = 2;
    uint8_t status = *(decodedFrame + offset);
    offset += 1;
    if(status != StarTracker::STATUS_OK) {
        sif::warning << "StarTrackerHandler::handleTemperatureTm: Reply error: "
                << static_cast<unsigned int>(status) << std::endl;
        result = TEMPERATURE_REQUEST_FAILED;
        return result;
    }
    const uint8_t* buffer = decodedFrame + offset;
    size_t size = sizeof(temperatureSet.ticks);
    SerializeAdapter::deSerialize(&temperatureSet.ticks, &buffer, &size, SerializeIF::Endianness::LITTLE);
    offset += size;
    buffer = decodedFrame + offset;
    size = sizeof(temperatureSet.time);
    SerializeAdapter::deSerialize(&temperatureSet.time, &buffer, &size, SerializeIF::Endianness::LITTLE);
    offset += size;
    temperatureSet.mcuTemperature = *(decodedFrame + offset) << 24
            | *(decodedFrame + offset + 1) << 16 | *(decodedFrame + offset + 2) << 8
            | *(decodedFrame + offset + 3);
    offset += sizeof(temperatureSet.mcuTemperature);
    temperatureSet.mcuTemperature = *(decodedFrame + offset) << 24
            | *(decodedFrame + offset + 1) << 16 | *(decodedFrame + offset + 2) << 8
            | *(decodedFrame + offset + 3);
    #if OBSW_VERBOSE_LEVEL >= 1 && OBSW_DEBUG_STARTRACKER == 1
    sif::info << "StarTrackerHandler::handleTemperatureTm: Ticks: "
            << temperatureSet.ticks << std::endl;
    sif::info << "StarTrackerHandler::handleTemperatureTm: Time: "
            << temperatureSet.time << std::endl;
    sif::info << "StarTrackerHandler::handleTemperatureTm: MCU Temperature: "
            << temperatureSet.mcuTemperature << " °C" << std::endl;
    sif::info << "StarTrackerHandler::handleTemperatureTm: CMOS Temperature: "
            << temperatureSet.mcuTemperature << " °C" << std::endl;
#endif
    return result;
}