271 lines
9.1 KiB
C++
271 lines
9.1 KiB
C++
#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;
|
|
}
|
|
|