eive-obsw/mission/tmtc/CcsdsIpCoreHandler.cpp

316 lines
9.8 KiB
C++
Raw Normal View History

#include "CcsdsIpCoreHandler.h"
2021-11-22 18:01:16 +01:00
2023-01-27 15:10:26 +01:00
#include <fsfw/subsystem/helper.h>
#include <linux/ipcore/PtmeConfig.h>
#include <mission/config/comCfg.h>
2021-09-19 12:27:48 +02:00
2023-01-23 11:52:46 +01:00
#include "eive/definitions.h"
2022-01-17 15:58:27 +01:00
#include "fsfw/ipc/QueueFactory.h"
#include "fsfw/objectmanager/ObjectManager.h"
#include "fsfw/serialize/SerializeAdapter.h"
2022-01-17 15:58:27 +01:00
#include "fsfw/serviceinterface/ServiceInterface.h"
#include "fsfw/serviceinterface/serviceInterfaceDefintions.h"
#include "mission/devices/devicedefinitions/SyrlinksDefinitions.h"
CcsdsIpCoreHandler::CcsdsIpCoreHandler(object_id_t objectId, object_id_t tcDestination,
2023-03-09 17:44:05 +01:00
PtmeConfig& ptmeConfig, std::atomic_bool& linkState,
2023-03-18 14:34:09 +01:00
GpioIF* gpioIF, PtmeGpios gpioIds)
2022-01-17 15:58:27 +01:00
: SystemObject(objectId),
2023-03-09 17:44:05 +01:00
linkState(linkState),
2022-01-17 15:58:27 +01:00
tcDestination(tcDestination),
parameterHelper(this),
actionHelper(this, nullptr),
2023-01-24 19:07:37 +01:00
modeHelper(this),
ptmeConfig(ptmeConfig),
2023-03-18 14:34:09 +01:00
ptmeGpios(gpioIds),
gpioIF(gpioIF) {
2022-01-17 15:58:27 +01:00
commandQueue = QueueFactory::instance()->createMessageQueue(QUEUE_SIZE);
2022-02-19 16:42:22 +01:00
auto mqArgs = MqArgs(objectId, static_cast<void*>(this));
eventQueue =
QueueFactory::instance()->createMessageQueue(10, EventMessage::EVENT_MESSAGE_SIZE, &mqArgs);
2021-09-19 12:27:48 +02:00
}
2023-03-09 17:44:05 +01:00
CcsdsIpCoreHandler::~CcsdsIpCoreHandler() = default;
2021-09-19 12:27:48 +02:00
ReturnValue_t CcsdsIpCoreHandler::performOperation(uint8_t operationCode) {
2022-01-17 15:58:27 +01:00
readCommandQueue();
2022-08-24 17:27:47 +02:00
return returnvalue::OK;
2021-09-19 12:27:48 +02:00
}
ReturnValue_t CcsdsIpCoreHandler::initialize() {
2022-08-24 17:27:47 +02:00
ReturnValue_t result = returnvalue::OK;
2022-01-17 15:58:27 +01:00
AcceptsTelecommandsIF* tcDistributor =
ObjectManager::instance()->get<AcceptsTelecommandsIF>(tcDestination);
if (tcDistributor == nullptr) {
2021-11-01 12:41:20 +01:00
#if FSFW_CPP_OSTREAM_ENABLED == 1
sif::error << "CcsdsHandler::initialize: Invalid TC Distributor object" << std::endl;
2021-11-01 12:41:20 +01:00
#endif
2022-01-17 15:58:27 +01:00
return ObjectManagerIF::CHILD_INIT_FAILED;
}
2021-11-01 12:41:20 +01:00
2022-01-17 15:58:27 +01:00
tcDistributorQueueId = tcDistributor->getRequestQueue();
2021-11-01 12:41:20 +01:00
2022-01-17 15:58:27 +01:00
result = parameterHelper.initialize();
2022-08-24 17:27:47 +02:00
if (result != returnvalue::OK) {
2022-01-17 15:58:27 +01:00
return result;
}
2021-09-29 14:47:20 +02:00
2022-01-17 15:58:27 +01:00
result = actionHelper.initialize(commandQueue);
2022-08-24 17:27:47 +02:00
if (result != returnvalue::OK) {
2022-01-17 15:58:27 +01:00
return result;
}
2021-11-21 18:07:05 +01:00
2023-01-26 12:04:58 +01:00
result = modeHelper.initialize();
if (result != returnvalue::OK) {
return result;
}
result = ptmeConfig.initialize();
2022-08-24 17:27:47 +02:00
if (result != returnvalue::OK) {
return ObjectManagerIF::CHILD_INIT_FAILED;
}
2023-03-18 14:34:09 +01:00
if (batPriorityParam == 0) {
disablePrioritySelectMode();
} else {
enablePrioritySelectMode();
}
#if OBSW_SYRLINKS_SIMULATED == 1
2022-06-23 12:05:56 +02:00
// Update data on rising edge
2023-03-09 20:38:50 +01:00
ptmeConfig.invertTxClock(false);
2023-03-09 17:44:05 +01:00
linkState = LINK_UP;
#endif /* OBSW_SYRLINKS_SIMULATED == 1*/
2022-01-17 15:58:27 +01:00
return result;
2021-09-19 12:27:48 +02:00
}
void CcsdsIpCoreHandler::readCommandQueue(void) {
2022-01-17 15:58:27 +01:00
CommandMessage commandMessage;
2022-08-24 17:27:47 +02:00
ReturnValue_t result = returnvalue::FAILED;
2021-09-19 12:27:48 +02:00
2022-01-17 15:58:27 +01:00
result = commandQueue->receiveMessage(&commandMessage);
2022-08-24 17:27:47 +02:00
if (result == returnvalue::OK) {
2022-01-17 15:58:27 +01:00
result = parameterHelper.handleParameterMessage(&commandMessage);
2022-08-24 17:27:47 +02:00
if (result == returnvalue::OK) {
2022-01-17 15:58:27 +01:00
return;
}
result = actionHelper.handleActionMessage(&commandMessage);
2022-08-24 17:27:47 +02:00
if (result == returnvalue::OK) {
2022-01-17 15:58:27 +01:00
return;
2021-09-22 16:54:55 +02:00
}
2023-01-24 19:07:37 +01:00
result = modeHelper.handleModeCommand(&commandMessage);
if (result == returnvalue::OK) {
return;
}
2022-01-17 15:58:27 +01:00
CommandMessage reply;
reply.setReplyRejected(CommandMessage::UNKNOWN_COMMAND, commandMessage.getCommand());
commandQueue->reply(&reply);
return;
}
2021-09-19 12:27:48 +02:00
}
MessageQueueId_t CcsdsIpCoreHandler::getCommandQueue() const { return commandQueue->getId(); }
2021-09-19 12:27:48 +02:00
ReturnValue_t CcsdsIpCoreHandler::getParameter(uint8_t domainId, uint8_t uniqueIdentifier,
ParameterWrapper* parameterWrapper,
const ParameterWrapper* newValues,
uint16_t startAtIndex) {
2023-03-18 14:34:09 +01:00
if ((domainId == 0) and (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(batPriorityParam);
if (mode == MODE_ON) {
updateBatPriorityOnTxOff = true;
} else if (mode == MODE_OFF) {
updateBatPriorityFromParam();
}
return returnvalue::OK;
}
return HasParametersIF::INVALID_IDENTIFIER_ID;
2021-09-19 12:27:48 +02:00
}
2021-11-01 12:41:20 +01:00
uint32_t CcsdsIpCoreHandler::getIdentifier() const { return 0; }
2021-11-01 12:41:20 +01:00
MessageQueueId_t CcsdsIpCoreHandler::getRequestQueue() const {
// Forward packets directly to the CCSDS TC distributor
2022-01-17 15:58:27 +01:00
return tcDistributorQueueId;
2021-11-01 12:41:20 +01:00
}
2021-11-21 18:07:05 +01:00
ReturnValue_t CcsdsIpCoreHandler::executeAction(ActionId_t actionId, MessageQueueId_t commandedBy,
const uint8_t* data, size_t size) {
2022-08-24 17:27:47 +02:00
ReturnValue_t result = returnvalue::OK;
2022-01-17 15:58:27 +01:00
switch (actionId) {
2021-11-22 18:01:16 +01:00
case SET_LOW_RATE: {
2023-01-27 14:44:40 +01:00
submode = static_cast<Submode_t>(com::CcsdsSubmode::DATARATE_LOW);
result = ptmeConfig.setRate(RATE_100KBPS);
break;
2021-11-22 18:01:16 +01:00
}
case SET_HIGH_RATE: {
2023-01-27 14:44:40 +01:00
submode = static_cast<Submode_t>(com::CcsdsSubmode::DATARATE_HIGH);
result = ptmeConfig.setRate(RATE_500KBPS);
break;
}
case ARBITRARY_RATE: {
uint32_t bitrate = 0;
SerializeAdapter::deSerialize(&bitrate, &data, &size, SerializeIF::Endianness::BIG);
result = ptmeConfig.setRate(bitrate);
break;
2021-11-22 18:01:16 +01:00
}
case EN_TRANSMITTER: {
2022-01-17 15:58:27 +01:00
enableTransmit();
2023-01-24 19:07:37 +01:00
if (mode == HasModesIF::MODE_OFF) {
mode = HasModesIF::MODE_ON;
}
2022-01-17 15:58:27 +01:00
return EXECUTION_FINISHED;
2021-11-22 18:01:16 +01:00
}
case DISABLE_TRANSMITTER: {
2022-01-17 15:58:27 +01:00
disableTransmit();
2023-01-24 19:07:37 +01:00
if (mode == HasModesIF::MODE_ON) {
mode = HasModesIF::MODE_OFF;
}
2022-01-17 15:58:27 +01:00
return EXECUTION_FINISHED;
2021-11-22 18:01:16 +01:00
}
case ENABLE_TX_CLK_MANIPULATOR: {
result = ptmeConfig.configTxManipulator(true);
break;
}
case DISABLE_TX_CLK_MANIPULATOR: {
result = ptmeConfig.configTxManipulator(false);
break;
}
case UPDATE_ON_RISING_EDGE: {
result = ptmeConfig.invertTxClock(false);
break;
}
case UPDATE_ON_FALLING_EDGE: {
result = ptmeConfig.invertTxClock(true);
break;
}
2021-11-21 18:07:05 +01:00
default:
2022-01-17 15:58:27 +01:00
return COMMAND_NOT_IMPLEMENTED;
}
2022-08-24 17:27:47 +02:00
if (result != returnvalue::OK) {
2022-01-26 17:59:31 +01:00
return result;
}
return EXECUTION_FINISHED;
2021-11-21 18:07:05 +01:00
}
2021-11-22 18:01:16 +01:00
2023-03-09 17:44:05 +01:00
void CcsdsIpCoreHandler::updateLinkState() { linkState = LINK_UP; }
2021-11-22 18:01:16 +01:00
void CcsdsIpCoreHandler::enableTransmit() {
2022-03-30 18:11:21 +02:00
#ifndef TE0720_1CFA
2023-03-18 14:34:09 +01:00
gpioIF->pullHigh(ptmeGpios.enableTxClock);
gpioIF->pullHigh(ptmeGpios.enableTxData);
2022-06-23 20:12:56 +02:00
#endif
2023-03-09 17:44:05 +01:00
linkState = LINK_UP;
2021-11-22 18:01:16 +01:00
}
2023-01-24 19:07:37 +01:00
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) {
2023-01-27 14:44:40 +01:00
if (submode != static_cast<Submode_t>(com::CcsdsSubmode::DATARATE_HIGH) and
2023-01-27 20:23:55 +01:00
submode != static_cast<Submode_t>(com::CcsdsSubmode::DATARATE_LOW) and
submode != static_cast<Submode_t>(com::CcsdsSubmode::DATARATE_DEFAULT)) {
2023-01-24 19:07:37 +01:00
return HasModesIF::INVALID_SUBMODE;
}
2023-01-26 12:04:58 +01:00
} else if (mode != HasModesIF::MODE_OFF) {
return returnvalue::FAILED;
2023-01-24 19:07:37 +01:00
}
*msToReachTheMode = 2000;
2023-01-26 12:04:58 +01:00
return returnvalue::OK;
2023-01-24 19:07:37 +01:00
}
void CcsdsIpCoreHandler::startTransition(Mode_t mode, Submode_t submode) {
auto rateSet = [&](uint32_t rate) {
ReturnValue_t result = ptmeConfig.setRate(rate);
if (result == returnvalue::OK) {
2023-01-27 20:23:55 +01:00
this->mode = HasModesIF::MODE_ON;
}
};
2023-01-24 19:07:37 +01:00
if (mode == HasModesIF::MODE_ON) {
enableTransmit();
if (submode == static_cast<Submode_t>(com::CcsdsSubmode::DATARATE_DEFAULT)) {
com::Datarate currentDatarate = com::getCurrentDatarate();
if (currentDatarate == com::Datarate::LOW_RATE_MODULATION_BPSK) {
rateSet(RATE_100KBPS);
} else if (currentDatarate == com::Datarate::HIGH_RATE_MODULATION_0QPSK) {
rateSet(RATE_500KBPS);
2023-01-24 19:07:37 +01:00
}
} else if (submode == static_cast<Submode_t>(com::CcsdsSubmode::DATARATE_HIGH)) {
rateSet(RATE_500KBPS);
2023-01-27 14:44:40 +01:00
} else if (submode == static_cast<Submode_t>(com::CcsdsSubmode::DATARATE_LOW)) {
rateSet(RATE_100KBPS);
2023-01-24 19:07:37 +01:00
}
2023-01-27 20:23:55 +01:00
2023-01-24 19:07:37 +01:00
} else if (mode == HasModesIF::MODE_OFF) {
disableTransmit();
2023-01-27 20:23:55 +01:00
this->mode = HasModesIF::MODE_OFF;
2023-01-24 19:07:37 +01:00
}
2023-01-27 20:23:55 +01:00
this->submode = submode;
2023-01-26 12:04:58 +01:00
modeHelper.modeChanged(mode, submode);
2023-01-27 17:34:22 +01:00
announceMode(false);
2023-01-24 19:07:37 +01:00
}
void CcsdsIpCoreHandler::announceMode(bool recursive) { triggerEvent(MODE_INFO, mode, submode); }
2023-01-24 19:07:37 +01:00
void CcsdsIpCoreHandler::disableTransmit() {
ptmeConfig.enableBatPriorityBit(false);
2022-06-23 20:12:56 +02:00
#ifndef TE0720_1CFA
2023-03-18 14:34:09 +01:00
gpioIF->pullLow(ptmeGpios.enableTxClock);
gpioIF->pullLow(ptmeGpios.enableTxData);
2022-06-23 20:12:56 +02:00
#endif
2023-03-09 17:44:05 +01:00
linkState = LINK_DOWN;
2023-03-18 14:34:09 +01:00
if (updateBatPriorityOnTxOff) {
updateBatPriorityFromParam();
updateBatPriorityOnTxOff = false;
}
2021-11-22 18:01:16 +01:00
}
2022-09-16 11:43:11 +02:00
const char* CcsdsIpCoreHandler::getName() const { return "CCSDS Handler"; }
2023-01-27 15:10:26 +01:00
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(); }
2023-03-18 14:34:09 +01:00
void CcsdsIpCoreHandler::enablePrioritySelectMode() {
ptmeConfig.enableBatPriorityBit(true);
// Reset the PTME
gpioIF->pullLow(ptmeGpios.ptmeResetn);
gpioIF->pullHigh(ptmeGpios.ptmeResetn);
}
void CcsdsIpCoreHandler::disablePrioritySelectMode() {
ptmeConfig.enableBatPriorityBit(false);
// Reset the PTME
gpioIF->pullLow(ptmeGpios.ptmeResetn);
gpioIF->pullHigh(ptmeGpios.ptmeResetn);
}
void CcsdsIpCoreHandler::updateBatPriorityFromParam() {
if (batPriorityParam == 0) {
disablePrioritySelectMode();
} else {
enablePrioritySelectMode();
}
}