eive-obsw/mission/com/CcsdsIpCoreHandler.cpp
Robin Mueller 33985937b7
All checks were successful
EIVE/eive-obsw/pipeline/pr-main This commit looks good
typo
2023-06-29 09:44:09 +02:00

392 lines
13 KiB
C++

#include "CcsdsIpCoreHandler.h"
#include <fsfw/subsystem/helper.h>
#include <linux/ipcore/PtmeConfig.h>
#include <mission/config/comCfg.h>
#include <unistd.h>
#include "eive/definitions.h"
#include "fsfw/ipc/QueueFactory.h"
#include "fsfw/objectmanager/ObjectManager.h"
#include "fsfw/serialize/SerializeAdapter.h"
#include "fsfw/serviceinterface/ServiceInterface.h"
#include "fsfw/serviceinterface/serviceInterfaceDefintions.h"
#include "mission/com/syrlinksDefs.h"
CcsdsIpCoreHandler::CcsdsIpCoreHandler(object_id_t objectId, object_id_t tcDestination,
PtmeConfig& ptmeConfig, std::atomic_bool& linkState,
GpioIF* gpioIF, PtmeGpios gpioIds,
std::atomic_bool& ptmeLocked)
: SystemObject(objectId),
linkState(linkState),
ptmeLocked(ptmeLocked),
tcDestination(tcDestination),
parameterHelper(this),
actionHelper(this, nullptr),
modeHelper(this),
ptmeConfig(ptmeConfig),
ptmeGpios(gpioIds),
gpioIF(gpioIF) {
commandQueue = QueueFactory::instance()->createMessageQueue(QUEUE_SIZE);
auto mqArgs = MqArgs(objectId, static_cast<void*>(this));
eventQueue =
QueueFactory::instance()->createMessageQueue(10, EventMessage::EVENT_MESSAGE_SIZE, &mqArgs);
ptmeLocked = true;
}
CcsdsIpCoreHandler::~CcsdsIpCoreHandler() = default;
ReturnValue_t CcsdsIpCoreHandler::performOperation(uint8_t operationCode) {
readCommandQueue();
performPtmeUpdateWhenApplicable();
return returnvalue::OK;
}
ReturnValue_t CcsdsIpCoreHandler::initialize() {
AcceptsTelecommandsIF* tcDistributor =
ObjectManager::instance()->get<AcceptsTelecommandsIF>(tcDestination);
if (tcDistributor == nullptr) {
#if FSFW_CPP_OSTREAM_ENABLED == 1
sif::error << "CcsdsHandler::initialize: Invalid TC Distributor object" << std::endl;
#endif
return ObjectManagerIF::CHILD_INIT_FAILED;
}
tcDistributorQueueId = tcDistributor->getRequestQueue();
ReturnValue_t result = parameterHelper.initialize();
if (result != returnvalue::OK) {
return result;
}
result = actionHelper.initialize(commandQueue);
if (result != returnvalue::OK) {
return result;
}
result = modeHelper.initialize();
if (result != returnvalue::OK) {
return result;
}
result = ptmeConfig.initialize();
if (result != returnvalue::OK) {
return ObjectManagerIF::CHILD_INIT_FAILED;
}
// This also pulls the PTME out of reset state.
updateBatPriorityFromParam();
ptmeConfig.setPollThreshold(
static_cast<AxiPtmeConfig::IdlePollThreshold>(params.pollThresholdParam));
resetPtme();
ptmeLocked = false;
#if OBSW_SYRLINKS_SIMULATED == 1
// Update data on rising edge
ptmeConfig.invertTxClock(false);
linkState = LINK_UP;
#endif /* OBSW_SYRLINKS_SIMULATED == 1*/
return result;
}
void CcsdsIpCoreHandler::readCommandQueue(void) {
CommandMessage commandMessage;
ReturnValue_t result = returnvalue::FAILED;
result = commandQueue->receiveMessage(&commandMessage);
if (result == returnvalue::OK) {
result = parameterHelper.handleParameterMessage(&commandMessage);
if (result == returnvalue::OK) {
return;
}
result = actionHelper.handleActionMessage(&commandMessage);
if (result == returnvalue::OK) {
return;
}
result = modeHelper.handleModeCommand(&commandMessage);
if (result == returnvalue::OK) {
return;
}
CommandMessage reply;
reply.setReplyRejected(CommandMessage::UNKNOWN_COMMAND, commandMessage.getCommand());
commandQueue->reply(&reply);
return;
}
}
MessageQueueId_t CcsdsIpCoreHandler::getCommandQueue() const { return commandQueue->getId(); }
ReturnValue_t CcsdsIpCoreHandler::getParameter(uint8_t domainId, uint8_t uniqueIdentifier,
ParameterWrapper* parameterWrapper,
const ParameterWrapper* newValues,
uint16_t startAtIndex) {
if (domainId != 0) {
return HasParametersIF::INVALID_DOMAIN_ID;
}
if (uniqueIdentifier == ParamId::BAT_PRIORITY) {
uint8_t newVal = 0;
ReturnValue_t result = newValues->getElement(&newVal);
if (result != returnvalue::OK) {
return result;
}
if (newVal > 1) {
return HasParametersIF::INVALID_VALUE;
}
parameterWrapper->set(params.batPriorityParam);
if (newVal != params.batPriorityParam) {
// This ensures that the BAT priority is updated at some point when an update of the PTME is
// allowed
updateContext.updateBatPrio = true;
// If we are off, we can do the update after X cycles. Otherwise, wait until the transmitter
// goes off.
if (mode == MODE_OFF) {
initPtmeUpdateAfterXCycles();
}
}
return returnvalue::OK;
} else if (uniqueIdentifier == ParamId::POLL_THRESHOLD) {
uint8_t newVal = 0;
ReturnValue_t result = newValues->getElement(&newVal);
if (result != returnvalue::OK) {
return result;
}
if (newVal > static_cast<uint8_t>(AxiPtmeConfig::NEVER)) {
return HasParametersIF::INVALID_VALUE;
}
parameterWrapper->set(newVal);
if (newVal != params.pollThresholdParam) {
updateContext.updatePollThreshold = true;
if (mode == MODE_OFF) {
initPtmeUpdateAfterXCycles();
}
}
return returnvalue::OK;
}
return HasParametersIF::INVALID_IDENTIFIER_ID;
}
uint32_t CcsdsIpCoreHandler::getIdentifier() const { return 0; }
MessageQueueId_t CcsdsIpCoreHandler::getRequestQueue() const {
// Forward packets directly to the CCSDS TC distributor
return tcDistributorQueueId;
}
ReturnValue_t CcsdsIpCoreHandler::executeAction(ActionId_t actionId, MessageQueueId_t commandedBy,
const uint8_t* data, size_t size) {
ReturnValue_t result = returnvalue::OK;
switch (actionId) {
case ARBITRARY_RATE: {
uint32_t bitrate = 0;
result = SerializeAdapter::deSerialize(&bitrate, &data, &size, SerializeIF::Endianness::BIG);
if (result != returnvalue::OK) {
return result;
}
ptmeConfig.setRate(bitrate);
updateContext.updateClockRate = true;
if (mode == MODE_OFF) {
initPtmeUpdateAfterXCycles();
}
break;
}
case ENABLE_TX_CLK_MANIPULATOR: {
ptmeConfig.configTxManipulator(true);
break;
}
case DISABLE_TX_CLK_MANIPULATOR: {
ptmeConfig.configTxManipulator(false);
break;
}
case UPDATE_ON_RISING_EDGE: {
ptmeConfig.invertTxClock(false);
break;
}
case UPDATE_ON_FALLING_EDGE: {
ptmeConfig.invertTxClock(true);
break;
}
default:
return COMMAND_NOT_IMPLEMENTED;
}
if (result != returnvalue::OK) {
return result;
}
return EXECUTION_FINISHED;
}
void CcsdsIpCoreHandler::updateLinkState() { linkState = LINK_UP; }
void CcsdsIpCoreHandler::enableTransmit() {
gpioIF->pullHigh(ptmeGpios.enableTxClock);
gpioIF->pullHigh(ptmeGpios.enableTxData);
linkState = LINK_UP;
}
void CcsdsIpCoreHandler::getMode(Mode_t* mode, Submode_t* submode) {
*mode = this->mode;
*submode = this->submode;
}
ReturnValue_t CcsdsIpCoreHandler::checkModeCommand(Mode_t mode, Submode_t submode,
uint32_t* msToReachTheMode) {
if (mode == HasModesIF::MODE_ON) {
if (submode != static_cast<Submode_t>(com::CcsdsSubmode::DATARATE_HIGH) and
submode != static_cast<Submode_t>(com::CcsdsSubmode::DATARATE_LOW) and
submode != static_cast<Submode_t>(com::CcsdsSubmode::DATARATE_DEFAULT)) {
return HasModesIF::INVALID_SUBMODE;
}
} else if (mode != HasModesIF::MODE_OFF) {
return returnvalue::FAILED;
}
*msToReachTheMode = 2000;
return returnvalue::OK;
}
void CcsdsIpCoreHandler::startTransition(Mode_t mode, Submode_t submode) {
triggerEvent(CHANGING_MODE, mode, submode);
if (mode == HasModesIF::MODE_ON) {
uint32_t currentRate = ptmeConfig.getRate();
// Check whether the rate actually changes.
if ((this->submode != submode) and
(((submode == static_cast<Submode_t>(com::CcsdsSubmode::DATARATE_LOW) and
(currentRate != RATE_100KBPS))) or
((submode == static_cast<Submode_t>(com::CcsdsSubmode::DATARATE_HIGH) and
(currentRate != RATE_500KBPS))))) {
initPtmeUpdateAfterXCycles();
updateContext.enableTransmitAfterPtmeUpdate = true;
updateContext.updateClockRate = true;
this->submode = submode;
this->mode = mode;
updateContext.setModeAfterUpdate = true;
return;
}
// No rate change, so enable transmitter right away.
enableTransmit();
} else if (mode == HasModesIF::MODE_OFF) {
disableTransmit();
}
setMode(mode, submode);
}
void CcsdsIpCoreHandler::announceMode(bool recursive) { triggerEvent(MODE_INFO, mode, submode); }
void CcsdsIpCoreHandler::disableTransmit() {
#ifndef TE0720_1CFA
gpioIF->pullLow(ptmeGpios.enableTxClock);
gpioIF->pullLow(ptmeGpios.enableTxData);
#endif
linkState = LINK_DOWN;
// Some parameters need update and transmitter is off now.
if (updateContext.updateBatPrio or updateContext.updateClockRate) {
initPtmeUpdateAfterXCycles();
}
}
const char* CcsdsIpCoreHandler::getName() const { return "CCSDS Handler"; }
const HasHealthIF* CcsdsIpCoreHandler::getOptHealthIF() const { return nullptr; }
const HasModesIF& CcsdsIpCoreHandler::getModeIF() const { return *this; }
ReturnValue_t CcsdsIpCoreHandler::connectModeTreeParent(HasModeTreeChildrenIF& parent) {
return modetree::connectModeTreeParent(parent, *this, nullptr, modeHelper);
}
ModeTreeChildIF& CcsdsIpCoreHandler::getModeTreeChildIF() { return *this; }
object_id_t CcsdsIpCoreHandler::getObjectId() const { return SystemObject::getObjectId(); }
void CcsdsIpCoreHandler::enablePrioritySelectMode() { ptmeConfig.enableBatPriorityBit(true); }
void CcsdsIpCoreHandler::disablePrioritySelectMode() { ptmeConfig.enableBatPriorityBit(false); }
void CcsdsIpCoreHandler::updateBatPriorityFromParam() {
if (params.batPriorityParam == 0) {
disablePrioritySelectMode();
} else {
enablePrioritySelectMode();
}
}
void CcsdsIpCoreHandler::setMode(Mode_t mode, Submode_t submode) {
this->submode = submode;
this->mode = mode;
modeHelper.modeChanged(mode, submode);
announceMode(false);
}
void CcsdsIpCoreHandler::performPtmeUpdateWhenApplicable() {
if (not updateContext.performPtmeUpdateAfterXCycles) {
return;
}
if (updateContext.ptmeUpdateCycleCount >= 2) {
bool doResetPtme = false;
if (updateContext.updateBatPrio) {
updateBatPriorityFromParam();
updateContext.updateBatPrio = false;
doResetPtme = true;
}
if (updateContext.updatePollThreshold) {
ptmeConfig.setPollThreshold(
static_cast<AxiPtmeConfig::IdlePollThreshold>(params.pollThresholdParam));
updateContext.updatePollThreshold = false;
doResetPtme = true;
}
ReturnValue_t result = returnvalue::OK;
if (updateContext.updateClockRate) {
if (submode == static_cast<Submode_t>(com::CcsdsSubmode::DATARATE_DEFAULT)) {
com::Datarate currentDatarate = com::getCurrentDatarate();
if (currentDatarate == com::Datarate::LOW_RATE_MODULATION_BPSK) {
result = ptmeConfig.setRate(RATE_100KBPS);
} else if (currentDatarate == com::Datarate::HIGH_RATE_MODULATION_0QPSK) {
result = ptmeConfig.setRate(RATE_500KBPS);
}
} else if (submode == static_cast<Submode_t>(com::CcsdsSubmode::DATARATE_HIGH)) {
result = ptmeConfig.setRate(RATE_500KBPS);
} else if (submode == static_cast<Submode_t>(com::CcsdsSubmode::DATARATE_LOW)) {
result = ptmeConfig.setRate(RATE_100KBPS);
}
if (result != returnvalue::OK) {
sif::error << "CcsdsIpCoreHandler: Setting datarate failed" << std::endl;
}
updateContext.updateClockRate = false;
doResetPtme = true;
}
finishPtmeUpdateAfterXCycles(doResetPtme);
return;
}
updateContext.ptmeUpdateCycleCount++;
}
void CcsdsIpCoreHandler::resetPtme() {
gpioIF->pullLow(ptmeGpios.ptmeResetn);
usleep(10);
gpioIF->pullHigh(ptmeGpios.ptmeResetn);
}
void CcsdsIpCoreHandler::initPtmeUpdateAfterXCycles() {
if (not updateContext.performPtmeUpdateAfterXCycles) {
updateContext.performPtmeUpdateAfterXCycles = true;
updateContext.ptmeUpdateCycleCount = 0;
ptmeLocked = true;
}
}
void CcsdsIpCoreHandler::finishPtmeUpdateAfterXCycles(bool doResetPtme) {
if (doResetPtme) {
resetPtme();
}
ptmeLocked = false;
updateContext.performPtmeUpdateAfterXCycles = false;
updateContext.ptmeUpdateCycleCount = 0;
if (updateContext.enableTransmitAfterPtmeUpdate) {
enableTransmit();
updateContext.enableTransmitAfterPtmeUpdate = false;
}
if (updateContext.setModeAfterUpdate) {
setMode(mode, submode);
updateContext.setModeAfterUpdate = false;
}
}