eive-obsw/mission/tmtc/CCSDSHandler.cpp

302 lines
9.0 KiB
C++
Raw Normal View History

2022-01-17 15:58:27 +01:00
#include "CCSDSHandler.h"
2021-11-22 18:01:16 +01:00
#include <linux/obc/PdecHandler.h>
2021-09-19 12:27:48 +02:00
2022-01-17 15:58:27 +01:00
#include "fsfw/events/EventManagerIF.h"
#include "fsfw/ipc/QueueFactory.h"
#include "fsfw/objectmanager/ObjectManager.h"
#include "fsfw/serviceinterface/ServiceInterface.h"
#include "fsfw/serviceinterface/serviceInterfaceDefintions.h"
2021-09-19 12:27:48 +02:00
2021-11-21 18:07:05 +01:00
CCSDSHandler::CCSDSHandler(object_id_t objectId, object_id_t ptmeId, object_id_t tcDestination,
2022-01-17 15:58:27 +01:00
TxRateSetterIF* txRateSetterIF, GpioIF* gpioIF, gpioId_t enTxClock,
gpioId_t enTxData)
: SystemObject(objectId),
ptmeId(ptmeId),
tcDestination(tcDestination),
parameterHelper(this),
actionHelper(this, nullptr),
txRateSetterIF(txRateSetterIF),
gpioIF(gpioIF),
enTxClock(enTxClock),
enTxData(enTxData) {
commandQueue = QueueFactory::instance()->createMessageQueue(QUEUE_SIZE);
eventQueue = QueueFactory::instance()->createMessageQueue(EventMessage::EVENT_MESSAGE_SIZE * 2);
2021-09-19 12:27:48 +02:00
}
2022-01-17 15:58:27 +01:00
CCSDSHandler::~CCSDSHandler() {}
2021-09-19 12:27:48 +02:00
2021-09-26 08:29:30 +02:00
ReturnValue_t CCSDSHandler::performOperation(uint8_t operationCode) {
2022-01-17 15:58:27 +01:00
checkEvents();
readCommandQueue();
handleTelemetry();
handleTelecommands();
checkTxTimer();
return RETURN_OK;
2021-09-19 12:27:48 +02:00
}
2021-09-22 16:54:55 +02:00
void CCSDSHandler::handleTelemetry() {
2022-01-17 15:58:27 +01:00
VirtualChannelMapIter iter;
for (iter = virtualChannelMap.begin(); iter != virtualChannelMap.end(); iter++) {
iter->second->performOperation();
}
2021-09-22 16:54:55 +02:00
}
2022-01-17 15:58:27 +01:00
void CCSDSHandler::handleTelecommands() {}
2021-09-22 16:54:55 +02:00
2021-09-19 12:27:48 +02:00
ReturnValue_t CCSDSHandler::initialize() {
2022-01-17 15:58:27 +01:00
ReturnValue_t result = RETURN_OK;
PtmeIF* ptme = ObjectManager::instance()->get<PtmeIF>(ptmeId);
if (ptme == nullptr) {
sif::warning << "Invalid PTME object" << std::endl;
return ObjectManagerIF::CHILD_INIT_FAILED;
}
AcceptsTelecommandsIF* tcDistributor =
ObjectManager::instance()->get<AcceptsTelecommandsIF>(tcDestination);
if (tcDistributor == nullptr) {
2021-11-01 12:41:20 +01:00
#if FSFW_CPP_OSTREAM_ENABLED == 1
2022-01-17 15:58:27 +01:00
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();
if (result != HasReturnvaluesIF::RETURN_OK) {
return result;
}
2021-09-29 14:47:20 +02:00
2022-01-17 15:58:27 +01:00
result = actionHelper.initialize(commandQueue);
if (result != RETURN_OK) {
return result;
}
2021-11-21 18:07:05 +01:00
2022-01-17 15:58:27 +01:00
VirtualChannelMapIter iter;
for (iter = virtualChannelMap.begin(); iter != virtualChannelMap.end(); iter++) {
result = iter->second->initialize();
if (result != RETURN_OK) {
return result;
2021-09-29 14:47:20 +02:00
}
2022-01-17 15:58:27 +01:00
iter->second->setPtmeObject(ptme);
}
2021-09-29 14:47:20 +02:00
2022-01-17 15:58:27 +01:00
EventManagerIF* manager = ObjectManager::instance()->get<EventManagerIF>(objects::EVENT_MANAGER);
if (manager == nullptr) {
2021-11-22 18:01:16 +01:00
#if FSFW_CPP_OSTREAM_ENABLED == 1
2022-01-17 15:58:27 +01:00
sif::error << "CCSDSHandler::initialize: Invalid event manager" << std::endl;
2021-11-22 18:01:16 +01:00
#endif
2022-01-17 15:58:27 +01:00
return ObjectManagerIF::CHILD_INIT_FAILED;
}
result = manager->registerListener(eventQueue->getId());
if (result != HasReturnvaluesIF::RETURN_OK) {
2021-12-02 08:05:33 +01:00
#if FSFW_CPP_OSTREAM_ENABLED == 1
2022-01-17 15:58:27 +01:00
sif::warning << "CCSDSHandler::initialize: Failed to register CCSDS handler as event "
"listener"
<< std::endl;
2021-12-02 08:05:33 +01:00
#endif
2022-01-17 15:58:27 +01:00
return ObjectManagerIF::CHILD_INIT_FAILED;
;
}
result = manager->subscribeToEventRange(eventQueue->getId(),
event::getEventId(PdecHandler::CARRIER_LOCK),
event::getEventId(PdecHandler::BIT_LOCK_PDEC));
if (result != HasReturnvaluesIF::RETURN_OK) {
2021-11-22 18:01:16 +01:00
#if FSFW_CPP_OSTREAM_ENABLED == 1
2022-01-17 15:58:27 +01:00
sif::error << "CCSDSHandler::initialize: Failed to subscribe to events from PDEC "
"handler"
<< std::endl;
2021-11-22 18:01:16 +01:00
#endif
2022-01-17 15:58:27 +01:00
return result;
}
return result;
2021-09-19 12:27:48 +02:00
}
void CCSDSHandler::readCommandQueue(void) {
2022-01-17 15:58:27 +01:00
CommandMessage commandMessage;
ReturnValue_t result = RETURN_FAILED;
2021-09-19 12:27:48 +02:00
2022-01-17 15:58:27 +01:00
result = commandQueue->receiveMessage(&commandMessage);
if (result == RETURN_OK) {
result = parameterHelper.handleParameterMessage(&commandMessage);
if (result == RETURN_OK) {
return;
}
result = actionHelper.handleActionMessage(&commandMessage);
2021-09-22 16:54:55 +02:00
if (result == RETURN_OK) {
2022-01-17 15:58:27 +01:00
return;
2021-09-22 16:54:55 +02:00
}
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
}
2022-01-17 15:58:27 +01:00
MessageQueueId_t CCSDSHandler::getCommandQueue() const { return commandQueue->getId(); }
2021-09-19 12:27:48 +02:00
2021-09-26 08:29:30 +02:00
void CCSDSHandler::addVirtualChannel(VcId_t vcId, VirtualChannel* virtualChannel) {
2022-01-17 15:58:27 +01:00
if (vcId > common::NUMBER_OF_VIRTUAL_CHANNELS) {
sif::warning << "CCSDSHandler::addVirtualChannel: Invalid virtual channel ID" << std::endl;
return;
}
if (virtualChannel == nullptr) {
sif::warning << "CCSDSHandler::addVirtualChannel: Invalid virtual channel interface"
<< std::endl;
return;
}
auto status = virtualChannelMap.emplace(vcId, virtualChannel);
if (status.second == false) {
sif::warning << "CCSDSHandler::addVirtualChannel: Failed to add virtual channel to "
"virtual channel map"
<< std::endl;
return;
}
2021-09-19 12:27:48 +02:00
}
MessageQueueId_t CCSDSHandler::getReportReceptionQueue(uint8_t virtualChannel) {
2022-01-17 15:58:27 +01:00
if (virtualChannel < common::NUMBER_OF_VIRTUAL_CHANNELS) {
VirtualChannelMapIter iter = virtualChannelMap.find(virtualChannel);
if (iter != virtualChannelMap.end()) {
return iter->second->getReportReceptionQueue();
} else {
sif::warning << "CCSDSHandler::getReportReceptionQueue: Virtual channel with ID "
<< static_cast<unsigned int>(virtualChannel) << " not in virtual channel map"
<< std::endl;
return MessageQueueIF::NO_QUEUE;
}
} else {
sif::debug << "CCSDSHandler::getReportReceptionQueue: Invalid virtual channel requested";
}
return MessageQueueIF::NO_QUEUE;
2021-09-19 12:27:48 +02:00
}
2021-09-22 16:54:55 +02:00
ReturnValue_t CCSDSHandler::getParameter(uint8_t domainId, uint8_t uniqueIdentifier,
2022-01-17 15:58:27 +01:00
ParameterWrapper* parameterWrapper,
const ParameterWrapper* newValues, uint16_t startAtIndex) {
return RETURN_OK;
2021-09-19 12:27:48 +02:00
}
2021-11-01 12:41:20 +01:00
2022-01-17 15:58:27 +01:00
uint16_t CCSDSHandler::getIdentifier() { return 0; }
2021-11-01 12:41:20 +01:00
MessageQueueId_t CCSDSHandler::getRequestQueue() {
2022-01-17 15:58:27 +01:00
// Forward packets directly to TC distributor
return tcDistributorQueueId;
2021-11-01 12:41:20 +01:00
}
2021-11-21 18:07:05 +01:00
2022-01-17 15:58:27 +01:00
ReturnValue_t CCSDSHandler::executeAction(ActionId_t actionId, MessageQueueId_t commandedBy,
const uint8_t* data, size_t size) {
switch (actionId) {
2021-11-22 18:01:16 +01:00
case SET_LOW_RATE: {
2022-01-17 15:58:27 +01:00
txRateSetterIF->setRate(BitRates::RATE_400KHZ);
return EXECUTION_FINISHED;
2021-11-22 18:01:16 +01:00
}
case SET_HIGH_RATE: {
2022-01-17 15:58:27 +01:00
txRateSetterIF->setRate(BitRates::RATE_2000KHZ);
return EXECUTION_FINISHED;
2021-11-22 18:01:16 +01:00
}
case EN_TRANSMITTER: {
2022-01-17 15:58:27 +01:00
enableTransmit();
return EXECUTION_FINISHED;
2021-11-22 18:01:16 +01:00
}
case DIS_TRANSMITTER: {
2022-01-17 15:58:27 +01:00
disableTransmit();
return EXECUTION_FINISHED;
2021-11-22 18:01:16 +01:00
}
2021-11-21 18:07:05 +01:00
default:
2022-01-17 15:58:27 +01:00
return COMMAND_NOT_IMPLEMENTED;
}
2021-11-21 18:07:05 +01:00
}
2021-11-22 18:01:16 +01:00
void CCSDSHandler::checkEvents() {
2022-01-17 15:58:27 +01:00
EventMessage event;
for (ReturnValue_t result = eventQueue->receiveMessage(&event); result == RETURN_OK;
result = eventQueue->receiveMessage(&event)) {
switch (event.getMessageId()) {
case EventMessage::EVENT_MESSAGE:
handleEvent(&event);
break;
default:
sif::debug << "CCSDSHandler::checkEvents: Did not subscribe to this event message"
<< std::endl;
break;
2021-11-22 18:01:16 +01:00
}
2022-01-17 15:58:27 +01:00
}
2021-11-22 18:01:16 +01:00
}
void CCSDSHandler::handleEvent(EventMessage* eventMessage) {
2022-01-17 15:58:27 +01:00
Event event = eventMessage->getEvent();
switch (event) {
2021-11-25 14:09:06 +01:00
case PdecHandler::BIT_LOCK_PDEC: {
2022-01-17 15:58:27 +01:00
handleBitLockEvent();
break;
2021-11-22 18:01:16 +01:00
}
case PdecHandler::CARRIER_LOCK: {
2022-01-17 15:58:27 +01:00
handleCarrierLockEvent();
break;
2021-11-22 18:01:16 +01:00
}
default:
2022-01-17 15:58:27 +01:00
sif::debug << "CCSDSHandler::handleEvent: Did not subscribe to this event" << std::endl;
break;
}
2021-11-22 18:01:16 +01:00
}
void CCSDSHandler::handleBitLockEvent() {
2022-01-17 15:58:27 +01:00
if (transmitterCountdown.isBusy()) {
// Transmitter already enabled
return;
}
enableTransmit();
2021-11-22 18:01:16 +01:00
}
void CCSDSHandler::handleCarrierLockEvent() {
2022-01-17 15:58:27 +01:00
if (!enableTxWhenCarrierLock) {
return;
}
enableTransmit();
2021-11-22 18:01:16 +01:00
}
void CCSDSHandler::forwardLinkstate() {
2022-01-17 15:58:27 +01:00
VirtualChannelMapIter iter;
for (iter = virtualChannelMap.begin(); iter != virtualChannelMap.end(); iter++) {
iter->second->setLinkState(linkState);
}
2021-11-22 18:01:16 +01:00
}
void CCSDSHandler::enableTransmit() {
2022-01-17 15:58:27 +01:00
if (transmitterCountdown.isBusy()) {
// Transmitter already enabled
return;
}
transmitterCountdown.setTimeout(TRANSMITTER_TIMEOUT);
2021-11-22 18:01:16 +01:00
#if BOARD_TE0720 == 0
2022-01-17 15:58:27 +01:00
gpioIF->pullHigh(enTxClock);
gpioIF->pullHigh(enTxData);
2021-11-22 18:01:16 +01:00
#endif /* BOARD_TE0720 == 0 */
2022-01-17 15:58:27 +01:00
linkState = UP;
// Set link state of all virtual channels to link up
forwardLinkstate();
2021-11-22 18:01:16 +01:00
}
void CCSDSHandler::checkTxTimer() {
2022-01-17 15:58:27 +01:00
if (linkState == DOWN) {
return;
}
if (transmitterCountdown.hasTimedOut()) {
disableTransmit();
}
2021-11-22 18:01:16 +01:00
}
void CCSDSHandler::disableTransmit() {
#if BOARD_TE0720 == 0
2022-01-17 15:58:27 +01:00
gpioIF->pullLow(enTxClock);
gpioIF->pullLow(enTxData);
2021-11-22 18:01:16 +01:00
#endif /* BOARD_TE0720 == 0 */
2022-01-17 15:58:27 +01:00
linkState = DOWN;
forwardLinkstate();
transmitterCountdown.setTimeout(0);
2021-11-22 18:01:16 +01:00
}