eive/eive-obsw
13
4
Fork 0
Code Issues 4 Pull Requests 1 Releases 121 Wiki Activity

syrlinks handler ready for testing

Browse Source
This commit is contained in:
Jakob Meier
2021-02-22 09:24:42 +01:00
parent b4d74b4a4f
commit 7710fda755
18 changed files with 842 additions and 247 deletions
Download Patch File Download Diff File Expand all files Collapse all files

217
mission/devices/SyrlinksHkHandler.cpp
View File

@ -1,9 +1,10 @@
#include <mission/devices/SyrlinksHkHandler.h>
#include <fsfwconfig/OBSWConfig.h>
#include <fsfw/globalfunctions/CRC.h>
#include <fsfw/datapool/PoolReadHelper.h>
SyrlinksHkHandler::SyrlinksHkHandler(object_id_t objectId, object_id_t comIF, CookieIF * comCookie) :
DeviceHandlerBase(objectId, comIF, comCookie), rxStatusRegistersSet(this) {
DeviceHandlerBase(objectId, comIF, comCookie), rxDataset(this), txDataset(this) {
if (comCookie == NULL) {
sif::error << "SyrlinksHkHandler: Invalid com cookie" << std::endl;
}
@ -25,7 +26,7 @@ void SyrlinksHkHandler::doShutDown(){
ReturnValue_t SyrlinksHkHandler::buildNormalDeviceCommand(
DeviceCommandId_t * id) {
*id = READ_RX_STATUS_REGISTERS;
*id = SYRLINKS::READ_RX_STATUS_REGISTERS;
return buildCommandFromCommand(*id, NULL, 0);
}
@ -39,33 +40,40 @@ ReturnValue_t SyrlinksHkHandler::buildCommandFromCommand(
size_t commandDataLen) {
switch(deviceCommand) {
case(SYRLINKS::RESET_UNIT): {
resetCommand.copy(rawPacket, resetCommand.size(), 0);
resetCommand.copy(reinterpret_cast<char*>(commandBuffer), resetCommand.size(), 0);
rawPacketLen = resetCommand.size();
rememberCommandId = SYRLINKS::RESET_UNIT;
rawPacket = commandBuffer;
return RETURN_OK;
}
case(SYRLINKS::SET_TX_MODE_STANDBY): {
setTxModeStandby.copy(rawPacket, setTxModeStandby.size(), 0);
setTxModeStandby.copy(reinterpret_cast<char*>(commandBuffer), setTxModeStandby.size(), 0);
rawPacketLen = setTxModeStandby.size();
rememberCommandId = SYRLINKS::SET_TX_MODE_STANDBY;
rawPacket = commandBuffer;
return RETURN_OK;
}
case(SYRLINKS::SET_TX_MODE_MODULATION): {
setTxModeModulation.copy(rawPacket, setTxModeModulation.size(), 0);
setTxModeModulation.copy(reinterpret_cast<char*>(commandBuffer), setTxModeModulation.size(), 0);
rawPacketLen = setTxModeModulation.size();
rememberCommandId = SYRLINKS::SET_TX_MODE_MODULATION;
rawPacket = commandBuffer;
return RETURN_OK;
}
case(SYRLINKS::SET_TX_MODE_CW): {
setTxModeCw.copy(rawPacket, setTxModeCw.size(), 0);
setTxModeCw.copy(reinterpret_cast<char*>(commandBuffer), setTxModeCw.size(), 0);
rawPacketLen = setTxModeCw.size();
rememberCommandId = SYRLINKS::SET_TX_MODE_CW;
rawPacket = commandBuffer;
return RETURN_OK;
}
case(SYRLINKS::READ_RX_STATUS_REGISTERS): {
readRxStatusRegCommand.copy(rawPacket, readRxStatusRegCommand.size(), 0);
readRxStatusRegCommand.copy(reinterpret_cast<char*>(commandBuffer), readRxStatusRegCommand.size(), 0);
rawPacketLen = readRxStatusRegCommand.size();
rememberCommandId = SYRLINKS::READ_RX_STATUS_REGISTERS;
rawPacket = commandBuffer;
return RETURN_OK;
}
case(SYRLINKS::READ_TX_STATUS): {
readTxStatus.copy(reinterpret_cast<char*>(commandBuffer), readTxStatus.size(), 0);
rawPacketLen = readTxStatus.size();
rememberCommandId = SYRLINKS::READ_TX_STATUS;
rawPacket = commandBuffer;
return RETURN_OK;
}
default:
@ -75,11 +83,17 @@ ReturnValue_t SyrlinksHkHandler::buildCommandFromCommand(
}
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,
this->insertInCommandAndReplyMap(SYRLINKS::RESET_UNIT, 1, nullptr, SYRLINKS::ACK_SIZE, false,
true, SYRLINKS::ACK_REPLY);
this->insertInCommandAndReplyMap(SYRLINKS::SET_TX_MODE_STANDBY, 1, nullptr, SYRLINKS::ACK_SIZE,
false, true, SYRLINKS::ACK_REPLY);
this->insertInCommandAndReplyMap(SYRLINKS::SET_TX_MODE_MODULATION, 1, nullptr,
SYRLINKS::ACK_SIZE, false, true, SYRLINKS::ACK_REPLY);
this->insertInCommandAndReplyMap(SYRLINKS::SET_TX_MODE_CW, 1, nullptr, SYRLINKS::ACK_SIZE,
false, true, SYRLINKS::ACK_REPLY);
this->insertInCommandAndReplyMap(SYRLINKS::READ_TX_STATUS, 1, &txDataset,
SYRLINKS::READ_TX_STATUS);
this->insertInCommandAndReplyMap(SYRLINKS::READ_RX_STATUS_REGISTERS, 1, &rxDataset,
SYRLINKS::RX_STATUS_REGISTERS_REPLY_SIZE);
}
@ -88,59 +102,33 @@ ReturnValue_t SyrlinksHkHandler::scanForReply(const uint8_t *start,
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;
if(*start != '<') {
sif::error << "SyrlinksHkHandler::scanForReply: Missing start frame character" << std::endl;
return MISSING_START_FRAME_CHARACTER;
}
switch(*(start + 1)) {
case('A'):
*foundLen = SYRLINKS::ACK_SIZE;
*foundId = SYRLINKS::ACK_REPLY;
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;
}
case('E'):
*foundLen = SYRLINKS::RX_STATUS_REGISTERS_REPLY_SIZE;
*foundId = SYRLINKS::READ_RX_STATUS_REGISTERS;
rememberCommandId = SYRLINKS::NONE;
break;
case('R'):
if (rememberCommandId == SYRLINKS::READ_TX_STATUS) {
*foundLen = SYRLINKS::READ_TX_STATUS_REPLY_SIZE;
*foundId = SYRLINKS::READ_TX_STATUS;
rememberCommandId = SYRLINKS::NONE;
}
break;
default:
sif::error << "SyrlinksHkHandler::scanForReply: Unknown reply identifier" << std::endl;
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;
}
@ -150,53 +138,44 @@ ReturnValue_t SyrlinksHkHandler::interpretDeviceReply(DeviceCommandId_t id,
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);
case (SYRLINKS::ACK_REPLY):
result = verifyReply(packet, SYRLINKS::ACK_SIZE);
if (result != RETURN_OK) {
sif::debug << "SyrlinksHkHandler::interpretDeviceReply: Status reply has invalid crc"
<< std::endl;
sif::error << "SyrlinksHkHandler::interpretDeviceReply: Acknowledgement reply has "
"invalid crc" << std::endl;
return CRC_FAILURE;
}
result = parseReplyStatus(static_cast<char*>(packet + SYRLINKS::MESSAGE_HEADER_SIZE));
result = parseReplyStatus(reinterpret_cast<const char*>(packet + SYRLINKS::MESSAGE_HEADER_SIZE));
if (result != RETURN_OK) {
return result;
}
break;
case(SYRLINKS::READ_RX_STATUS_REGISTERS):
result = verifyReadRxStatusRegistersReply(packet);
result = verifyReply(packet, SYRLINKS::RX_STATUS_REGISTERS_REPLY_SIZE);
if (result != RETURN_OK) {
sif::debug << "SyrlinksHkHandler::interpretDeviceReply: Read rx status registers reply "
sif::error << "SyrlinksHkHandler::interpretDeviceReply: Read rx status registers reply "
<< "has invalid crc" << std::endl;
return CRC_FAILURE;
}
parseRxStatusRegistersReply(packet);
break;
case(SYRLINKS::READ_TX_STATUS):
result = verifyReply(packet, SYRLINKS::READ_TX_STATUS_REPLY_SIZE);
if (result != RETURN_OK) {
sif::error << "SyrlinksHkHandler::interpretDeviceReply: Read tx status reply "
<< "has invalid crc" << std::endl;
return CRC_FAILURE;
}
parseTxStatusReply(packet);
break;
default: {
sif::debug << "SyrlinksHkHandler::interpretDeviceReply: Unknown device reply id"
<< std::endl;
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;
return RETURN_OK;
}
std::string SyrlinksHkHandler::convertIntToHexString(uint16_t intValue) {
@ -205,7 +184,7 @@ std::string SyrlinksHkHandler::convertIntToHexString(uint16_t intValue) {
return stream.str();
}
ReturnValue_t SyrlinksHkHandler::parseReplyStatus(char* status) {
ReturnValue_t SyrlinksHkHandler::parseReplyStatus(const char* status) {
switch (*status) {
case '0':
return RETURN_OK;
@ -231,29 +210,54 @@ ReturnValue_t SyrlinksHkHandler::parseReplyStatus(char* status) {
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;
sif::debug << "SyrlinksHkHandler::parseReplyStatus: Status reply contains an invalid "
<< "status id" << std::endl;
return RETURN_FAILED;
}
}
ReturnValue_t SyrlinksHkHandler::verifyReadRxStatusRegistersReply(uint8_t* packet) {
ReturnValue_t SyrlinksHkHandler::verifyReply(const uint8_t* packet, uint8_t size) {
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,
uint16_t crc = CRC::crc16ccitt(packet, 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;
const char* replyCrc = reinterpret_cast<const char*>(packet) + size - SYRLINKS::SIZE_CRC_AND_TERMINATION;
size_t startPosition = 0;
result = recalculatedCrc.compare(startPosition, SYRLINKS_CRC_FIELD_SIZE, replyCrc);
result = recalculatedCrc.compare(startPosition, SYRLINKS::CRC_FIELD_SIZE, replyCrc);
if (result != 0) {
return RETURN_FAILED;
}
return RETURN_OK;
}
void SyrlinksHkHandler::parseRxStatusRegistersReply(const uint8_t* packet) {
PoolReadHelper readHelper(&rxDataset);
uint16_t offset = SYRLINKS::MESSAGE_HEADER_SIZE;
rxDataset.rxStatus = *(packet + offset);
packet += 1;
rxDataset.rxSensitivity = *(packet + offset) << 16 | *(packet + offset + 1) << 8 | *(packet + offset + 2);
packet += 3;
rxDataset.rxFrequencyShift = *(packet + offset) << 16 | *(packet + offset + 1) << 8 | *(packet + offset + 2);
packet += 3;
rxDataset.rxIqPower = *(packet + offset) << 8 | *(packet + offset + 1);
packet += 2;
rxDataset.rxAgcValue = *(packet + offset + 1) << 8 | *(packet + offset);
packet += 2;
packet += 1; // reserved register
rxDataset.rxDemodEb= *(packet + offset) << 16 | *(packet + offset + 1) << 8 | *(packet + offset + 2);
packet += 3;
rxDataset.rxDemodN0= *(packet + offset) << 16 | *(packet + offset + 1) << 8 | *(packet + offset + 2);
packet += 3;
rxDataset.rxDataRate = *(packet + offset);
}
void SyrlinksHkHandler::parseTxStatusReply(const uint8_t* packet) {
PoolReadHelper readHelper(&txDataset);
uint16_t offset = SYRLINKS::MESSAGE_HEADER_SIZE;
rxDataset.rxStatus = *(packet + offset);
}
void SyrlinksHkHandler::setNormalDatapoolEntriesInvalid(){
}
@ -264,7 +268,20 @@ uint32_t SyrlinksHkHandler::getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo)
ReturnValue_t SyrlinksHkHandler::initializeLocalDataPool(localpool::DataPool& localDataPoolMap,
LocalDataPoolManager& poolManager) {
localDataPoolMap.emplace(TMP1075::TEMPERATURE_C_TMP1075_1, new PoolEntry<float>( { 0.0 }));
localDataPoolMap.emplace(SYRLINKS::RX_STATUS, new PoolEntry<uint8_t>( { 0 }));
localDataPoolMap.emplace(SYRLINKS::RX_SENSITIVITY, new PoolEntry<uint8_t>( { 0 }));
localDataPoolMap.emplace(SYRLINKS::RX_FREQUENCY_SHIFT, new PoolEntry<uint32_t>( { 0 }));
localDataPoolMap.emplace(SYRLINKS::RX_IQ_POWER, new PoolEntry<uint16_t>( { 0 }));
localDataPoolMap.emplace(SYRLINKS::RX_AGC_VALUE, new PoolEntry<uint16_t>( { 0 }));
localDataPoolMap.emplace(SYRLINKS::RX_DEMOD_EB, new PoolEntry<uint32_t>( { 0 }));
localDataPoolMap.emplace(SYRLINKS::RX_DEMOD_N0, new PoolEntry<uint32_t>( { 0 }));
localDataPoolMap.emplace(SYRLINKS::RX_DATA_RATE, new PoolEntry<uint8_t>( { 0 }));
return HasReturnvaluesIF::RETURN_OK;
}
void SyrlinksHkHandler::setModeNormal() {
mode = MODE_NORMAL;
}