eive-obsw/mission/devices/SyrlinksHkHandler.cpp

#include <mission/devices/SyrlinksHkHandler.h>
#include <fsfwconfig/OBSWConfig.h>
#include <fsfw/globalfunctions/CRC.h>

SyrlinksHkHandler::SyrlinksHkHandler(object_id_t objectId, object_id_t comIF, CookieIF * comCookie) :
        DeviceHandlerBase(objectId, comIF, comCookie), rxStatusRegistersSet(this) {
    if (comCookie == NULL) {
        sif::error << "SyrlinksHkHandler: Invalid com cookie" << std::endl;
    }
}

SyrlinksHkHandler::~SyrlinksHkHandler() {
}


void SyrlinksHkHandler::doStartUp(){
	if(mode == _MODE_START_UP){
		setMode(MODE_ON);
	}
}

void SyrlinksHkHandler::doShutDown(){

}

ReturnValue_t SyrlinksHkHandler::buildNormalDeviceCommand(
		DeviceCommandId_t * id) {
    *id = READ_RX_STATUS_REGISTERS;
    return buildCommandFromCommand(*id, NULL, 0);
}

ReturnValue_t SyrlinksHkHandler::buildTransitionDeviceCommand(
		DeviceCommandId_t * id){
	return HasReturnvaluesIF::RETURN_OK;
}

ReturnValue_t SyrlinksHkHandler::buildCommandFromCommand(
		DeviceCommandId_t deviceCommand, const uint8_t * commandData,
		size_t commandDataLen) {
	switch(deviceCommand) {
	case(SYRLINKS::RESET_UNIT): {
	    resetCommand.copy(rawPacket, resetCommand.size(), 0);
		rawPacketLen = resetCommand.size();
		rememberCommandId = SYRLINKS::RESET_UNIT;
		return RETURN_OK;
	}
	case(SYRLINKS::SET_TX_MODE_STANDBY): {
	    setTxModeStandby.copy(rawPacket, setTxModeStandby.size(), 0);
		rawPacketLen = setTxModeStandby.size();
		rememberCommandId = SYRLINKS::SET_TX_MODE_STANDBY;
		return RETURN_OK;
	}
	case(SYRLINKS::SET_TX_MODE_MODULATION): {
	    setTxModeModulation.copy(rawPacket, setTxModeModulation.size(), 0);
		rawPacketLen = setTxModeModulation.size();
		rememberCommandId = SYRLINKS::SET_TX_MODE_MODULATION;
		return RETURN_OK;
	}
	case(SYRLINKS::SET_TX_MODE_CW): {
	    setTxModeCw.copy(rawPacket, setTxModeCw.size(), 0);
		rawPacketLen = setTxModeCw.size();
		rememberCommandId = SYRLINKS::SET_TX_MODE_CW;
		return RETURN_OK;
	}
	case(SYRLINKS::READ_RX_STATUS_REGISTERS): {
	    readRxStatusRegCommand.copy(rawPacket, readRxStatusRegCommand.size(), 0);
		rawPacketLen = readRxStatusRegCommand.size();
		rememberCommandId = SYRLINKS::READ_RX_STATUS_REGISTERS;
		return RETURN_OK;
	}
	default:
		return DeviceHandlerIF::COMMAND_NOT_IMPLEMENTED;
	}
	return HasReturnvaluesIF::RETURN_FAILED;
}

void SyrlinksHkHandler::fillCommandAndReplyMap() {
    this->insertInCommandAndReplyMap(SYRLINKS::RESET_UNIT);
    this->insertInCommandAndReplyMap(SYRLINKS::SET_TX_MODE_STANDBY);
    this->insertInCommandAndReplyMap(SYRLINKS::SET_TX_MODE_MODULATION);
    this->insertInCommandAndReplyMap(SYRLINKS::SET_TX_MODE_CW);
    this->insertInCommandAndReplyMap(SYRLINKS::READ_RX_STATUS_REGISTERS, 1, &rxStatusRegistersSet,
            SYRLINKS::RX_STATUS_REGISTERS_REPLY_SIZE);
}

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

    ReturnValue_t result = RETURN_OK;

	switch(rememberCommandId) {
	case(SYRLINKS::RESET_UNIT):
        if (remainingSize != SYRLINKS::REQUEST_STATUS_REPLY_SIZE) {
            result = REPLY_WRONG_SIZE;
            break;
        }
        *foundLen = SYRLINKS::REQUEST_STATUS_REPLY_SIZE;
        *foundId = SYRLINKS::RESET_UNIT;
        rememberCommandId = SYRLINKS::NONE;
		break;
	case(SYRLINKS::SET_TX_MODE_STANDBY):
        if (remainingSize != SYRLINKS::REQUEST_STATUS_REPLY_SIZE) {
            result = REPLY_WRONG_SIZE;
        }
        *foundLen = SYRLINKS::REQUEST_STATUS_REPLY_SIZE;
        rememberCommandId = SYRLINKS::NONE;
        *foundId = SYRLINKS::SET_TX_MODE_STANDBY;
        break;
	case(SYRLINKS::SET_TX_MODE_MODULATION):
        if (remainingSize != SYRLINKS::REQUEST_STATUS_REPLY_SIZE) {
            result = REPLY_WRONG_SIZE;
        }
        *foundLen = SYRLINKS::REQUEST_STATUS_REPLY_SIZE;
        *foundId = SYRLINKS::SET_TX_MODE_MODULATION;
        rememberCommandId = SYRLINKS::NONE;
        break;
	case(SYRLINKS::SET_TX_MODE_CW):
	    if (remainingSize != SYRLINKS::REQUEST_STATUS_REPLY_SIZE) {
            result = REPLY_WRONG_SIZE;
        }
        *foundLen = SYRLINKS::REQUEST_STATUS_REPLY_SIZE;
        *foundId = SYRLINKS::SET_TX_MODE_CW;
        rememberCommandId = SYRLINKS::NONE;
        break;
	case(SYRLINKS::READ_RX_STATUS_REGISTERS):
        if (remainingSize != SYRLINKS::RX_STATUS_REGISTERS_REPLY_SIZE) {
            result = REPLY_WRONG_SIZE;
        }
        *foundLen = SYRLINKS::RX_STATUS_REGISTERS_REPLY_SIZE;
        *foundId = SYRLINKS::READ_RX_STATUS_REGISTERS;
        rememberCommandId = SYRLINKS::NONE;
        break;
	default:
		result = IGNORE_REPLY_DATA;
		break;
	}

    if (result == REPLY_WRONG_SIZE) {
        sif::debug << "SyrlinksHkHandler::scanForReply: Received reply with invalid length"
            << std::endl;
        return result;
    }

    return result;
}

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

    ReturnValue_t result;

    switch (id) {
    case (SYRLINKS::RESET_UNIT):
    case (SYRLINKS::SET_TX_MODE_STANDBY):
    case (SYRLINKS::SET_TX_MODE_MODULATION):
    case (SYRLINKS::SET_TX_MODE_CW):
        result = verifyTansmissionStatusReply(packet);
        if (result != RETURN_OK) {
            sif::debug << "SyrlinksHkHandler::interpretDeviceReply: Status reply has invalid crc"
                    << std::endl;
            return CRC_FAILURE;
        }
        result = parseReplyStatus(static_cast<char*>(packet + SYRLINKS::MESSAGE_HEADER_SIZE));
        if (result != RETURN_OK) {
            return result;
        }
        break;
    case(SYRLINKS::READ_RX_STATUS_REGISTERS):
        result = verifyReadRxStatusRegistersReply(packet);
        if (result != RETURN_OK) {
            sif::debug << "SyrlinksHkHandler::interpretDeviceReply: Read rx status registers reply "
                    << "has invalid crc" << std::endl;
            return CRC_FAILURE;
        }

        break;
	default: {
		return DeviceHandlerIF::UNKNOWN_DEVICE_REPLY;
	}

	}
	return HasReturnvaluesIF::RETURN_OK;
}


ReturnValue_t SyrlinksHkHandler::verifyTansmissionStatusReply() {
    int result = 0;
    uint16_t crc = CRC::crc16ccitt(packet,
            SYRLINKS::REQUEST_STATUS_REPLY_SIZE - SYRLINKS::SIZE_CRC_AND_TERMINATION,
            CRC_INITIAL_VALUE);
    std::string recalculatedCrc = convertIntToHexString(crc);
    const char* replyCrc = packet + SYRLINKS::REQUEST_STATUS_REPLY_SIZE
            - SYRLINKS::SIZE_CRC_AND_TERMINATION;
    size_t startPosition = 0;
    result = recalculatedCrc.compare(startPosition, SYRLINKS_CRC_FIELD_SIZE, replyCrc);
    if (result != 0) {
        return RETURN_FAILED;
    }
    return RETURN_OK;
}

std::string SyrlinksHkHandler::convertIntToHexString(uint16_t intValue) {
    std::stringstream stream;
    stream << std::hex << intValue;
    return stream.str();
}

ReturnValue_t SyrlinksHkHandler::parseReplyStatus(char* status) {
    switch (*status) {
    case '0':
        return RETURN_OK;
    case '1':
        sif::debug << "SyrlinksHkHandler::parseReplyStatus: Uart faming or parity error"
                << std::endl;
        return UART_FRAMIN_OR_PARITY_ERROR_ACK;
    case '2':
        sif::debug << "SyrlinksHkHandler::parseReplyStatus: Bad character detected" << std::endl;
        return BAD_CHARACTER_ACK;
    case '3':
        sif::debug << "SyrlinksHkHandler::parseReplyStatus: Bad parameter value (unexpected value "
                << "detected" << std::endl;
        return BAD_PARAMETER_VALUE_ACK;
    case '4':
        sif::debug << "SyrlinksHkHandler::parseReplyStatus: Bad end of frame" << std::endl;
        return BAD_END_OF_FRAME_ACK;
    case '5':
        sif::debug << "SyrlinksHkHandler::parseReplyStatus: Unknown command id or attempt to access"
                << " a protected register" << std::endl;
        return UNKNOWN_COMMAND_ID_ACK;
    case '6':
        sif::debug << "SyrlinksHkHandler::parseReplyStatus: Bad CRC" << std::endl;
        return BAD_CRC_ACK;
    default:
        sif::debug << "SyrlinksHkHandler::parseReplyStatus: Status reply holds invalid status "
                << "id" << std::endl;
        return RETURN_FAILED;
    }
}

ReturnValue_t SyrlinksHkHandler::verifyReadRxStatusRegistersReply(uint8_t* packet) {
    int result = 0;
    /* Calculate crc of received packet */
    uint16_t crc = CRC::crc16ccitt(packet,
            SYRLINKS::RX_STATUS_REGISTERS_REPLY_SIZE - SYRLINKS::SIZE_CRC_AND_TERMINATION,
            CRC_INITIAL_VALUE);
    std::string recalculatedCrc = convertIntToHexString(crc);
    const char* replyCrc = packet + SYRLINKS::RX_STATUS_REGISTERS_REPLY_SIZE
            - SYRLINKS::SIZE_CRC_AND_TERMINATION;
    size_t startPosition = 0;
    result = recalculatedCrc.compare(startPosition, SYRLINKS_CRC_FIELD_SIZE, replyCrc);
    if (result != 0) {
        return RETURN_FAILED;
    }
    return RETURN_OK;
}

void SyrlinksHkHandler::setNormalDatapoolEntriesInvalid(){

}

uint32_t SyrlinksHkHandler::getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo){
	return 500;
}

ReturnValue_t SyrlinksHkHandler::initializeLocalDataPool(localpool::DataPool& localDataPoolMap,
        LocalDataPoolManager& poolManager) {
    localDataPoolMap.emplace(TMP1075::TEMPERATURE_C_TMP1075_1, new PoolEntry<float>( { 0.0 }));
    return HasReturnvaluesIF::RETURN_OK;
}