304 lines
9.1 KiB
C++
304 lines
9.1 KiB
C++
#include "fsfw/serviceinterface/ServiceInterface.h"
|
|
#include "fsfw/serviceinterface/serviceInterfaceDefintions.h"
|
|
#include "fsfw/objectmanager/ObjectManager.h"
|
|
#include "fsfw/ipc/QueueFactory.h"
|
|
#include "fsfw/events/EventManagerIF.h"
|
|
|
|
#include <linux/obc/PdecHandler.h>
|
|
|
|
#include "CCSDSHandler.h"
|
|
|
|
CCSDSHandler::CCSDSHandler(object_id_t objectId, object_id_t ptmeId, object_id_t tcDestination,
|
|
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);
|
|
}
|
|
|
|
CCSDSHandler::~CCSDSHandler() {
|
|
}
|
|
|
|
ReturnValue_t CCSDSHandler::performOperation(uint8_t operationCode) {
|
|
checkEvents();
|
|
readCommandQueue();
|
|
handleTelemetry();
|
|
handleTelecommands();
|
|
checkTxTimer();
|
|
return RETURN_OK;
|
|
}
|
|
|
|
void CCSDSHandler::handleTelemetry() {
|
|
VirtualChannelMapIter iter;
|
|
for (iter = virtualChannelMap.begin(); iter != virtualChannelMap.end(); iter++) {
|
|
iter->second->performOperation();
|
|
}
|
|
}
|
|
|
|
void CCSDSHandler::handleTelecommands() {
|
|
|
|
}
|
|
|
|
ReturnValue_t CCSDSHandler::initialize() {
|
|
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) {
|
|
#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();
|
|
|
|
result = parameterHelper.initialize();
|
|
if (result != HasReturnvaluesIF::RETURN_OK) {
|
|
return result;
|
|
}
|
|
|
|
result = actionHelper.initialize(commandQueue);
|
|
if (result != RETURN_OK) {
|
|
return result;
|
|
}
|
|
|
|
VirtualChannelMapIter iter;
|
|
for (iter = virtualChannelMap.begin(); iter != virtualChannelMap.end(); iter++) {
|
|
result = iter->second->initialize();
|
|
if (result != RETURN_OK) {
|
|
return result;
|
|
}
|
|
iter->second->setPtmeObject(ptme);
|
|
}
|
|
|
|
EventManagerIF* manager = ObjectManager::instance()->get<EventManagerIF>(
|
|
objects::EVENT_MANAGER);
|
|
if (manager == nullptr) {
|
|
#if FSFW_CPP_OSTREAM_ENABLED == 1
|
|
sif::error << "CCSDSHandler::initialize: Invalid event manager" << std::endl;
|
|
#endif
|
|
return ObjectManagerIF::CHILD_INIT_FAILED;
|
|
}
|
|
result = manager->registerListener(eventQueue->getId());
|
|
if (result != HasReturnvaluesIF::RETURN_OK) {
|
|
#if FSFW_CPP_OSTREAM_ENABLED == 1
|
|
sif::warning << "CCSDSHandler::initialize: Failed to register CCSDS handler as event "
|
|
"listener" << std::endl;
|
|
#endif
|
|
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) {
|
|
#if FSFW_CPP_OSTREAM_ENABLED == 1
|
|
sif::error << "CCSDSHandler::initialize: Failed to subscribe to events from PDEC "
|
|
"handler" << std::endl;
|
|
#endif
|
|
return result;
|
|
}
|
|
return result;
|
|
}
|
|
|
|
void CCSDSHandler::readCommandQueue(void) {
|
|
CommandMessage commandMessage;
|
|
ReturnValue_t result = RETURN_FAILED;
|
|
|
|
result = commandQueue->receiveMessage(&commandMessage);
|
|
if (result == RETURN_OK) {
|
|
result = parameterHelper.handleParameterMessage(&commandMessage);
|
|
if (result == RETURN_OK) {
|
|
return;
|
|
}
|
|
result = actionHelper.handleActionMessage(&commandMessage);
|
|
if (result == RETURN_OK) {
|
|
return;
|
|
}
|
|
CommandMessage reply;
|
|
reply.setReplyRejected(CommandMessage::UNKNOWN_COMMAND,
|
|
commandMessage.getCommand());
|
|
commandQueue->reply(&reply);
|
|
return;
|
|
}
|
|
}
|
|
|
|
MessageQueueId_t CCSDSHandler::getCommandQueue() const {
|
|
return commandQueue->getId();
|
|
}
|
|
|
|
void CCSDSHandler::addVirtualChannel(VcId_t vcId, VirtualChannel* virtualChannel) {
|
|
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;
|
|
}
|
|
}
|
|
|
|
MessageQueueId_t CCSDSHandler::getReportReceptionQueue(uint8_t virtualChannel) {
|
|
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;
|
|
}
|
|
|
|
ReturnValue_t CCSDSHandler::getParameter(uint8_t domainId, uint8_t uniqueIdentifier,
|
|
ParameterWrapper *parameterWrapper, const ParameterWrapper *newValues,
|
|
uint16_t startAtIndex) {
|
|
return RETURN_OK;
|
|
}
|
|
|
|
uint16_t CCSDSHandler::getIdentifier() {
|
|
return 0;
|
|
}
|
|
|
|
MessageQueueId_t CCSDSHandler::getRequestQueue() {
|
|
// Forward packets directly to TC distributor
|
|
return tcDistributorQueueId;
|
|
}
|
|
|
|
ReturnValue_t CCSDSHandler::executeAction(ActionId_t actionId,
|
|
MessageQueueId_t commandedBy, const uint8_t* data, size_t size) {
|
|
|
|
switch(actionId) {
|
|
case SET_LOW_RATE: {
|
|
txRateSetterIF->setRate(BitRates::RATE_400KHZ);
|
|
return EXECUTION_FINISHED;
|
|
}
|
|
case SET_HIGH_RATE: {
|
|
txRateSetterIF->setRate(BitRates::RATE_2000KHZ);
|
|
return EXECUTION_FINISHED;
|
|
}
|
|
case EN_TRANSMITTER: {
|
|
enableTransmit();
|
|
return EXECUTION_FINISHED;
|
|
}
|
|
case DIS_TRANSMITTER: {
|
|
disableTransmit();
|
|
return EXECUTION_FINISHED;
|
|
}
|
|
default:
|
|
return COMMAND_NOT_IMPLEMENTED;
|
|
}
|
|
}
|
|
|
|
void CCSDSHandler::checkEvents() {
|
|
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;
|
|
}
|
|
}
|
|
}
|
|
|
|
|
|
void CCSDSHandler::handleEvent(EventMessage* eventMessage) {
|
|
Event event = eventMessage->getEvent();
|
|
switch(event){
|
|
case PdecHandler::BIT_LOCK_PDEC: {
|
|
handleBitLockEvent();
|
|
break;
|
|
}
|
|
case PdecHandler::CARRIER_LOCK: {
|
|
handleCarrierLockEvent();
|
|
break;
|
|
}
|
|
default:
|
|
sif::debug << "CCSDSHandler::handleEvent: Did not subscribe to this event"
|
|
<< std::endl;
|
|
break;
|
|
}
|
|
}
|
|
|
|
void CCSDSHandler::handleBitLockEvent() {
|
|
if(transmitterCountdown.isBusy()) {
|
|
// Transmitter already enabled
|
|
return;
|
|
}
|
|
enableTransmit();
|
|
}
|
|
|
|
void CCSDSHandler::handleCarrierLockEvent() {
|
|
if (!enableTxWhenCarrierLock) {
|
|
return;
|
|
}
|
|
enableTransmit();
|
|
}
|
|
|
|
void CCSDSHandler::forwardLinkstate() {
|
|
VirtualChannelMapIter iter;
|
|
for(iter = virtualChannelMap.begin(); iter != virtualChannelMap.end(); iter++) {
|
|
iter->second->setLinkState(linkState);
|
|
}
|
|
}
|
|
|
|
void CCSDSHandler::enableTransmit() {
|
|
if(transmitterCountdown.isBusy()) {
|
|
// Transmitter already enabled
|
|
return;
|
|
}
|
|
transmitterCountdown.setTimeout(TRANSMITTER_TIMEOUT);
|
|
#if BOARD_TE0720 == 0
|
|
gpioIF->pullHigh(enTxClock);
|
|
gpioIF->pullHigh(enTxData);
|
|
#endif /* BOARD_TE0720 == 0 */
|
|
linkState = UP;
|
|
// Set link state of all virtual channels to link up
|
|
forwardLinkstate();
|
|
}
|
|
|
|
void CCSDSHandler::checkTxTimer() {
|
|
if(linkState == DOWN) {
|
|
return;
|
|
}
|
|
if (transmitterCountdown.hasTimedOut()) {
|
|
disableTransmit();
|
|
}
|
|
}
|
|
|
|
void CCSDSHandler::disableTransmit() {
|
|
#if BOARD_TE0720 == 0
|
|
gpioIF->pullLow(enTxClock);
|
|
gpioIF->pullLow(enTxData);
|
|
#endif /* BOARD_TE0720 == 0 */
|
|
linkState = DOWN;
|
|
forwardLinkstate();
|
|
transmitterCountdown.setTimeout(0);
|
|
}
|