From 5ca26a72ea2101786cdfbd6a812a2d9913809224 Mon Sep 17 00:00:00 2001 From: "Robin.Mueller" Date: Fri, 28 Aug 2020 17:00:16 +0200 Subject: [PATCH] fixed some shadowing issues --- objectmanager/ObjectManager.cpp | 1 - subsystem/Subsystem.cpp | 1320 +++++++++++++++---------------- subsystem/Subsystem.h | 6 +- timemanager/CCSDSTime.cpp | 2 +- 4 files changed, 662 insertions(+), 667 deletions(-) diff --git a/objectmanager/ObjectManager.cpp b/objectmanager/ObjectManager.cpp index 13d67559e..467eed4bc 100644 --- a/objectmanager/ObjectManager.cpp +++ b/objectmanager/ObjectManager.cpp @@ -100,7 +100,6 @@ void ObjectManager::initialize() { } void ObjectManager::printList() { - std::map::iterator it; sif::debug << "ObjectManager: Object List contains:" << std::endl; for (auto const& it : objectList) { sif::debug << std::hex << it.first << " | " << it.second << std::endl; diff --git a/subsystem/Subsystem.cpp b/subsystem/Subsystem.cpp index a4869eba4..400ecc830 100644 --- a/subsystem/Subsystem.cpp +++ b/subsystem/Subsystem.cpp @@ -1,662 +1,658 @@ -#include "../health/HealthMessage.h" -#include "../objectmanager/ObjectManagerIF.h" -#include "../serialize/SerialArrayListAdapter.h" -#include "../serialize/SerialFixedArrayListAdapter.h" -#include "../serialize/SerializeElement.h" -#include "../serialize/SerialLinkedListAdapter.h" -#include "../subsystem/Subsystem.h" -#include - -Subsystem::Subsystem(object_id_t setObjectId, object_id_t parent, - uint32_t maxNumberOfSequences, uint32_t maxNumberOfTables) : - SubsystemBase(setObjectId, parent, 0), isInTransition(false), childrenChangedHealth( - false), uptimeStartTable(0), currentTargetTable(), targetMode( - 0), targetSubmode(SUBMODE_NONE), initialMode(0), currentSequenceIterator(), modeTables( - maxNumberOfTables), modeSequences(maxNumberOfSequences), IPCStore( - NULL) -#ifdef USE_MODESTORE -,modeStore(NULL) -#endif -{ - -} - -Subsystem::~Subsystem() { - //Auto-generated destructor stub -} - -ReturnValue_t Subsystem::checkSequence(HybridIterator iter, - Mode_t fallbackSequence) { - - //only check for existence, checking the fallback would lead to a (possibly infinite) recursion. - //the fallback sequence will be checked when it is needed. - if (!existsModeSequence(fallbackSequence)) { - return FALLBACK_SEQUENCE_DOES_NOT_EXIST; - } - - if (iter.value == NULL) { - return NO_TARGET_TABLE; - } - - for (; iter.value != NULL; ++iter) { - if (!existsModeTable(iter->getTableId())) { - return TABLE_DOES_NOT_EXIST; - } else { - ReturnValue_t result = checkTable(getTable(iter->getTableId())); - if (result != RETURN_OK) { - return result; - } - } - } - return RETURN_OK; -} - -ReturnValue_t Subsystem::checkSequence(Mode_t sequence) { - if (!existsModeSequence(sequence)) { - return SEQUENCE_DOES_NOT_EXIST; - } - HybridIterator iter = getSequence(sequence); - return checkSequence(iter, getFallbackSequence(sequence)); -} - -bool Subsystem::existsModeSequence(Mode_t id) { - return modeSequences.exists(id) == RETURN_OK; -} - -bool Subsystem::existsModeTable(Mode_t id) { - return modeTables.exists(id) == RETURN_OK; -} - -HybridIterator Subsystem::getCurrentTable() { - return getTable(currentSequenceIterator->getTableId()); -} - -void Subsystem::performChildOperation() { - if (isInTransition) { - if (commandsOutstanding <= 0) { //all children of the current table were commanded and replied - if (currentSequenceIterator.value == NULL) { //we're through with this sequence - if (checkStateAgainstTable(currentTargetTable, targetSubmode) - == RETURN_OK) { - setMode(targetMode, targetSubmode); - isInTransition = false; - return; - } else { - transitionFailed(TARGET_TABLE_NOT_REACHED, - getSequence(targetMode)->getTableId()); - return; - } - } - if (currentSequenceIterator->checkSuccess()) { - if (checkStateAgainstTable(getCurrentTable(), targetSubmode) - != RETURN_OK) { - transitionFailed(TABLE_CHECK_FAILED, - currentSequenceIterator->getTableId()); - return; - } - } - if (currentSequenceIterator->getWaitSeconds() != 0) { - if (uptimeStartTable == 0) { - Clock::getUptime(&uptimeStartTable); - return; - } else { - uint32_t uptimeNow; - Clock::getUptime(&uptimeNow); - if ((uptimeNow - uptimeStartTable) - < (currentSequenceIterator->getWaitSeconds() * 1000)) { - return; - } - } - } - uptimeStartTable = 0; - //next Table, but only if there is one - if ((++currentSequenceIterator).value != NULL) { //we're through with this sequence - executeTable(getCurrentTable(), targetSubmode); - } - } - } else { - if (childrenChangedHealth) { - triggerEvent(CHILD_CHANGED_HEALTH, 0, 0); - childrenChangedHealth = false; - startTransition(mode, submode); - } else if (childrenChangedMode) { - if (checkStateAgainstTable(currentTargetTable, submode) - != RETURN_OK) { - triggerEvent(CANT_KEEP_MODE, mode, submode); - cantKeepMode(); - } - } - } -} - -HybridIterator Subsystem::getSequence(Mode_t id) { - SequenceInfo *sequenceInfo = modeSequences.findValue(id); - if (sequenceInfo->entries.islinked) { - return HybridIterator( - sequenceInfo->entries.firstLinkedElement); - } else { - return HybridIterator( - sequenceInfo->entries.array->front(), - sequenceInfo->entries.array->back()); - } -} - -HybridIterator Subsystem::getTable(Mode_t id) { - EntryPointer *entry = modeTables.findValue(id); - if (entry->islinked) { - return HybridIterator(entry->firstLinkedElement); - } else { - return HybridIterator(entry->array->front(), - entry->array->back()); - } -} - -ReturnValue_t Subsystem::handleCommandMessage(CommandMessage *message) { - ReturnValue_t result; - switch (message->getCommand()) { - case HealthMessage::HEALTH_INFO: { - HealthState health = HealthMessage::getHealth(message); - if (health != EXTERNAL_CONTROL) { - //Ignore external control, as it has an effect only if the mode changes, - //which is communicated with an additional mode info event. - childrenChangedHealth = true; - } - } - break; - case ModeSequenceMessage::ADD_SEQUENCE: { - FixedArrayList sequence; - const uint8_t *pointer; - size_t sizeRead; - result = IPCStore->getData( - ModeSequenceMessage::getStoreAddress(message), &pointer, - &sizeRead); - if (result == RETURN_OK) { - Mode_t fallbackId; - size_t size = sizeRead; - result = SerializeAdapter::deSerialize(&fallbackId, &pointer, &size, - SerializeIF::Endianness::BIG); - if (result == RETURN_OK) { - result = SerialArrayListAdapter::deSerialize( - &sequence, &pointer, &size, - SerializeIF::Endianness::BIG); - if (result == RETURN_OK) { - result = addSequence(&sequence, - ModeSequenceMessage::getSequenceId(message), - fallbackId); - } - } - IPCStore->deleteData(ModeSequenceMessage::getStoreAddress(message)); - } - replyToCommand(result, 0); - } - break; - case ModeSequenceMessage::ADD_TABLE: { - FixedArrayList table; - const uint8_t *pointer; - size_t sizeRead; - result = IPCStore->getData( - ModeSequenceMessage::getStoreAddress(message), &pointer, - &sizeRead); - if (result == RETURN_OK) { - size_t size = sizeRead; - result = SerialArrayListAdapter::deSerialize(&table, - &pointer, &size, SerializeIF::Endianness::BIG); - if (result == RETURN_OK) { - result = addTable(&table, - ModeSequenceMessage::getSequenceId(message)); - } - IPCStore->deleteData(ModeSequenceMessage::getStoreAddress(message)); - } - replyToCommand(result, 0); - - } - break; - case ModeSequenceMessage::DELETE_SEQUENCE: - if (isInTransition) { - replyToCommand(IN_TRANSITION, 0); - break; - } - result = deleteSequence(ModeSequenceMessage::getSequenceId(message)); - replyToCommand(result, 0); - break; - case ModeSequenceMessage::DELETE_TABLE: - if (isInTransition) { - replyToCommand(IN_TRANSITION, 0); - break; - } - result = deleteTable(ModeSequenceMessage::getTableId(message)); - replyToCommand(result, 0); - break; - case ModeSequenceMessage::LIST_SEQUENCES: { - SerialFixedArrayListAdapter sequences; - FixedMap::Iterator iter; - for (iter = modeSequences.begin(); iter != modeSequences.end(); - ++iter) { - sequences.insert(iter.value->first); - } - SerializeIF *pointer = &sequences; - sendSerializablesAsCommandMessage(ModeSequenceMessage::SEQUENCE_LIST, - &pointer, 1); - } - break; - case ModeSequenceMessage::LIST_TABLES: { - SerialFixedArrayListAdapter tables; - FixedMap::Iterator iter; - for (iter = modeTables.begin(); iter != modeTables.end(); ++iter) { - tables.insert(iter.value->first); - } - SerializeIF *pointer = &tables; - sendSerializablesAsCommandMessage(ModeSequenceMessage::TABLE_LIST, - &pointer, 1); - } - break; - case ModeSequenceMessage::READ_SEQUENCE: { - ReturnValue_t result; - Mode_t sequence = ModeSequenceMessage::getSequenceId(message); - SequenceInfo *sequenceInfo = NULL; - result = modeSequences.find(sequence, &sequenceInfo); - if (result != RETURN_OK) { - replyToCommand(result, 0); - } - - SerializeIF *elements[3]; - SerializeElement sequenceId(sequence); - SerializeElement fallbackSequenceId( - getFallbackSequence(sequence)); - - elements[0] = &sequenceId; - elements[1] = &fallbackSequenceId; - - if (sequenceInfo->entries.islinked) { - SerialLinkedListAdapter list( - sequenceInfo->entries.firstLinkedElement, true); - elements[2] = &list; - sendSerializablesAsCommandMessage(ModeSequenceMessage::SEQUENCE, - elements, 3); - } else { - SerialArrayListAdapter serializableArray( - sequenceInfo->entries.array); - - elements[2] = &serializableArray; - sendSerializablesAsCommandMessage(ModeSequenceMessage::SEQUENCE, - elements, 3); - } - } - break; - case ModeSequenceMessage::READ_TABLE: { - ReturnValue_t result; - Mode_t table = ModeSequenceMessage::getSequenceId(message); - EntryPointer *entry = NULL; - result = modeTables.find(table, &entry); - if (result != RETURN_OK) { - replyToCommand(result, 0); - } - - SerializeIF *elements[2]; - SerializeElement tableId(table); - - elements[0] = &tableId; - - if (entry->islinked) { - SerialLinkedListAdapter list( - entry->firstLinkedElement, true); - elements[1] = &list; - sendSerializablesAsCommandMessage(ModeSequenceMessage::TABLE, - elements, 2); - } else { - SerialArrayListAdapter serializableArray( - entry->array); - elements[1] = &serializableArray; - sendSerializablesAsCommandMessage(ModeSequenceMessage::TABLE, - elements, 2); - } - } - break; - case ModeSequenceMessage::READ_FREE_SEQUENCE_SLOTS: { - uint32_t freeSlots = modeSequences.maxSize() - modeSequences.size(); - CommandMessage reply; - ModeSequenceMessage::setModeSequenceMessage(&reply, - ModeSequenceMessage::FREE_SEQUENCE_SLOTS, freeSlots); - commandQueue->reply(&reply); - } - break; - case ModeSequenceMessage::READ_FREE_TABLE_SLOTS: { - uint32_t free = modeTables.maxSize() - modeTables.size(); - CommandMessage reply; - ModeSequenceMessage::setModeSequenceMessage(&reply, - ModeSequenceMessage::FREE_TABLE_SLOTS, free); - commandQueue->reply(&reply); - } - break; - default: - return RETURN_FAILED; - } - return RETURN_OK; -} - -void Subsystem::replyToCommand(ReturnValue_t status, uint32_t parameter) { - if (status == RETURN_OK) { - CommandMessage reply(CommandMessage::REPLY_COMMAND_OK, 0, 0); - commandQueue->reply(&reply); - } else { - CommandMessage reply(CommandMessage::REPLY_REJECTED, status, 0); - commandQueue->reply(&reply); - } -} - -ReturnValue_t Subsystem::addSequence(ArrayList *sequence, - Mode_t id, Mode_t fallbackSequence, bool inStore, bool preInit) { - - ReturnValue_t result; - - //Before initialize() is called, tables must not be checked as the children are not added yet. - //Sequences added before are checked by initialize() - if (!preInit) { - result = checkSequence( - HybridIterator(sequence->front(), - sequence->back()), fallbackSequence); - if (result != RETURN_OK) { - return result; - } - } - - SequenceInfo info; - - info.fallbackSequence = fallbackSequence; - - info.entries.islinked = inStore; - info.entries.array = sequence; - - result = modeSequences.insert(id, info); - - if (result != RETURN_OK) { - return result; - } - - if (inStore) { -#ifdef USE_MODESTORE - result = modeStore->storeArray(sequence, - &(modeSequences.find(id)->entries.firstLinkedElement)); - if (result != RETURN_OK) { - modeSequences.erase(id); - } -#else - modeSequences.erase(id); - return RETURN_FAILED; -#endif - } - - return result; - -} - -ReturnValue_t Subsystem::addTable(ArrayList *table, Mode_t id, - bool inStore, bool preInit) { - - ReturnValue_t result; - - //Before initialize() is called, tables must not be checked as the children are not added yet. - //Tables added before are checked by initialize() - if (!preInit) { - result = checkTable( - HybridIterator(table->front(), table->back())); - if (result != HasReturnvaluesIF::RETURN_OK) { - return result; - } - } - - EntryPointer pointer; - - pointer.islinked = inStore; - pointer.array = table; - - result = modeTables.insert(id, pointer); - - if (result != RETURN_OK) { - return result; - } - - if (inStore) { -#ifdef USE_MODESTORE - result = modeStore->storeArray(table, - &(modeTables.find(id)->firstLinkedElement)); - if (result != RETURN_OK) { - modeTables.erase(id); - } -#else - modeTables.erase(id); - return RETURN_FAILED; -#endif - } - return result; -} - -ReturnValue_t Subsystem::deleteSequence(Mode_t id) { - - if (isFallbackSequence(id)) { - return IS_FALLBACK_SEQUENCE; - } - - SequenceInfo *sequenceInfo; - ReturnValue_t result; - result = modeSequences.find(id, &sequenceInfo); - if (result != HasReturnvaluesIF::RETURN_OK) { - return result; - } - if (!sequenceInfo->entries.islinked) { - return ACCESS_DENIED; - } - -#ifdef USE_MODESTORE - modeStore->deleteList(sequenceInfo->entries.firstLinkedElement); -#endif - modeSequences.erase(id); - return RETURN_OK; -} - -ReturnValue_t Subsystem::deleteTable(Mode_t id) { - - if (isTableUsed(id)) { - return TABLE_IN_USE; - } - - EntryPointer *pointer; - ReturnValue_t result; - result = modeTables.find(id, &pointer); - if (result != HasReturnvaluesIF::RETURN_OK) { - return result; - } - if (!pointer->islinked) { - return ACCESS_DENIED; - } - -#ifdef USE_MODESTORE - modeStore->deleteList(pointer->firstLinkedElement); -#endif - modeSequences.erase(id); - return RETURN_OK; -} - -ReturnValue_t Subsystem::initialize() { - ReturnValue_t result = SubsystemBase::initialize(); - - if (result != RETURN_OK) { - return result; - } - - IPCStore = objectManager->get(objects::IPC_STORE); - if (IPCStore == NULL) { - return RETURN_FAILED; - } - -#ifdef USE_MODESTORE - modeStore = objectManager->get(objects::MODE_STORE); - - if (modeStore == NULL) { - return RETURN_FAILED; - } -#endif - - if ((modeSequences.maxSize() > MAX_NUMBER_OF_TABLES_OR_SEQUENCES) - || (modeTables.maxSize() > MAX_NUMBER_OF_TABLES_OR_SEQUENCES)) { - return TABLE_OR_SEQUENCE_LENGTH_INVALID; - } - - mode = initialMode; - - return RETURN_OK; -} - -MessageQueueId_t Subsystem::getSequenceCommandQueue() const { - return SubsystemBase::getCommandQueue(); -} - -ReturnValue_t Subsystem::checkModeCommand(Mode_t mode, Submode_t submode, - uint32_t *msToReachTheMode) { - //Need to accept all submodes to be able to inherit submodes -// if (submode != SUBMODE_NONE) { -// return INVALID_SUBMODE; -// } - - if (isInTransition && (mode != getFallbackSequence(targetMode))) { - return HasModesIF::IN_TRANSITION; - } else { - return checkSequence(mode); - } -} - -void Subsystem::startTransition(Mode_t sequence, Submode_t submode) { - if (modeHelper.isForced()) { - triggerEvent(FORCING_MODE, sequence, submode); - } else { - triggerEvent(CHANGING_MODE, sequence, submode); - } - targetMode = sequence; - targetSubmode = submode; - isInTransition = true; - commandsOutstanding = 0; - currentSequenceIterator = getSequence(sequence); - - currentTargetTable = getTable(currentSequenceIterator->getTableId()); - - ++currentSequenceIterator; - - if (currentSequenceIterator.value != NULL) { - executeTable(getCurrentTable(), targetSubmode); - } -} - -Mode_t Subsystem::getFallbackSequence(Mode_t sequence) { - for (FixedMap::Iterator iter = modeSequences.begin(); - iter != modeSequences.end(); ++iter) { - if (iter.value->first == sequence) { - return iter->fallbackSequence; - } - } - return -1; -} - -bool Subsystem::isFallbackSequence(Mode_t SequenceId) { - for (FixedMap::Iterator iter = modeSequences.begin(); - iter != modeSequences.end(); iter++) { - if (iter->fallbackSequence == SequenceId) { - return true; - } - } - return false; -} - -bool Subsystem::isTableUsed(Mode_t tableId) { - for (FixedMap::Iterator sequence = - modeSequences.begin(); sequence != modeSequences.end(); - sequence++) { - HybridIterator sequenceIterator = getSequence( - sequence.value->first); - while (sequenceIterator.value != NULL) { - if (sequenceIterator->getTableId() == tableId) { - return true; - } - ++sequenceIterator; - } - } - return false; -} - -void Subsystem::transitionFailed(ReturnValue_t failureCode, - uint32_t parameter) { - triggerEvent(MODE_TRANSITION_FAILED, failureCode, parameter); - if (mode == targetMode) { - //already tried going back to the current mode - //go into fallback mode, also set current mode to fallback mode, so we come here at the next fail - modeHelper.setForced(true); - ReturnValue_t result; - if ((result = checkSequence(getFallbackSequence(mode))) != RETURN_OK) { - triggerEvent(FALLBACK_FAILED, result, getFallbackSequence(mode)); - isInTransition = false; //keep still and allow arbitrary mode commands to recover - return; - } - mode = getFallbackSequence(mode); - startTransition(mode, submode); - } else { - //try to go back to the current mode - startTransition(mode, submode); - } -} - -void Subsystem::sendSerializablesAsCommandMessage(Command_t command, - SerializeIF **elements, uint8_t count) { - ReturnValue_t result; - size_t maxSize = 0; - for (uint8_t i = 0; i < count; i++) { - maxSize += elements[i]->getSerializedSize(); - } - uint8_t *storeBuffer; - store_address_t address; - size_t size = 0; - - result = IPCStore->getFreeElement(&address, maxSize, &storeBuffer); - if (result != HasReturnvaluesIF::RETURN_OK) { - replyToCommand(result, 0); - return; - } - for (uint8_t i = 0; i < count; i++) { - elements[i]->serialize(&storeBuffer, &size, maxSize, - SerializeIF::Endianness::BIG); - } - CommandMessage reply; - ModeSequenceMessage::setModeSequenceMessage(&reply, command, address); - if (commandQueue->reply(&reply) != RETURN_OK) { - IPCStore->deleteData(address); - } -} - -ReturnValue_t Subsystem::checkObjectConnections() { - ReturnValue_t result = RETURN_OK; - for (FixedMap::Iterator iter = modeSequences.begin(); - iter != modeSequences.end(); iter++) { - result = checkSequence(iter.value->first); - if (result != RETURN_OK) { - return result; - } - - } - return RETURN_OK; -} - -void Subsystem::setInitialMode(Mode_t mode) { - initialMode = mode; -} - -void Subsystem::cantKeepMode() { - ReturnValue_t result; - if ((result = checkSequence(getFallbackSequence(mode))) != RETURN_OK) { - triggerEvent(FALLBACK_FAILED, result, getFallbackSequence(mode)); - return; - } - - modeHelper.setForced(true); - - //already set the mode, so that we do not try to go back in our old mode when the transition fails - mode = getFallbackSequence(mode); - //SHOULDDO: We should store submodes for fallback sequence as well, otherwise we should get rid of submodes completely. - startTransition(mode, SUBMODE_NONE); -} +#include "Subsystem.h" +#include "../health/HealthMessage.h" +#include "../objectmanager/ObjectManagerIF.h" +#include "../serialize/SerialArrayListAdapter.h" +#include "../serialize/SerialFixedArrayListAdapter.h" +#include "../serialize/SerializeElement.h" +#include "../serialize/SerialLinkedListAdapter.h" + +#include + +Subsystem::Subsystem(object_id_t setObjectId, object_id_t parent, + uint32_t maxNumberOfSequences, uint32_t maxNumberOfTables) : + SubsystemBase(setObjectId, parent, 0), isInTransition(false), + childrenChangedHealth(false), currentTargetTable(), + targetMode(0), targetSubmode(SUBMODE_NONE), initialMode(0), + currentSequenceIterator(), modeTables(maxNumberOfTables), + modeSequences(maxNumberOfSequences) {} + +Subsystem::~Subsystem() { + //Auto-generated destructor stub +} + +ReturnValue_t Subsystem::checkSequence(HybridIterator iter, + Mode_t fallbackSequence) { + + //only check for existence, checking the fallback would lead to a (possibly infinite) recursion. + //the fallback sequence will be checked when it is needed. + if (!existsModeSequence(fallbackSequence)) { + return FALLBACK_SEQUENCE_DOES_NOT_EXIST; + } + + if (iter.value == NULL) { + return NO_TARGET_TABLE; + } + + for (; iter.value != NULL; ++iter) { + if (!existsModeTable(iter->getTableId())) { + return TABLE_DOES_NOT_EXIST; + } else { + ReturnValue_t result = checkTable(getTable(iter->getTableId())); + if (result != RETURN_OK) { + return result; + } + } + } + return RETURN_OK; +} + +ReturnValue_t Subsystem::checkSequence(Mode_t sequence) { + if (!existsModeSequence(sequence)) { + return SEQUENCE_DOES_NOT_EXIST; + } + HybridIterator iter = getSequence(sequence); + return checkSequence(iter, getFallbackSequence(sequence)); +} + +bool Subsystem::existsModeSequence(Mode_t id) { + return modeSequences.exists(id) == RETURN_OK; +} + +bool Subsystem::existsModeTable(Mode_t id) { + return modeTables.exists(id) == RETURN_OK; +} + +HybridIterator Subsystem::getCurrentTable() { + return getTable(currentSequenceIterator->getTableId()); +} + +void Subsystem::performChildOperation() { + if (isInTransition) { + if (commandsOutstanding <= 0) { //all children of the current table were commanded and replied + if (currentSequenceIterator.value == NULL) { //we're through with this sequence + if (checkStateAgainstTable(currentTargetTable, targetSubmode) + == RETURN_OK) { + setMode(targetMode, targetSubmode); + isInTransition = false; + return; + } else { + transitionFailed(TARGET_TABLE_NOT_REACHED, + getSequence(targetMode)->getTableId()); + return; + } + } + if (currentSequenceIterator->checkSuccess()) { + if (checkStateAgainstTable(getCurrentTable(), targetSubmode) + != RETURN_OK) { + transitionFailed(TABLE_CHECK_FAILED, + currentSequenceIterator->getTableId()); + return; + } + } + if (currentSequenceIterator->getWaitSeconds() != 0) { + if (uptimeStartTable == 0) { + Clock::getUptime(&uptimeStartTable); + return; + } else { + uint32_t uptimeNow; + Clock::getUptime(&uptimeNow); + if ((uptimeNow - uptimeStartTable) + < (currentSequenceIterator->getWaitSeconds() * 1000)) { + return; + } + } + } + uptimeStartTable = 0; + //next Table, but only if there is one + if ((++currentSequenceIterator).value != NULL) { //we're through with this sequence + executeTable(getCurrentTable(), targetSubmode); + } + } + } else { + if (childrenChangedHealth) { + triggerEvent(CHILD_CHANGED_HEALTH, 0, 0); + childrenChangedHealth = false; + startTransition(mode, submode); + } else if (childrenChangedMode) { + if (checkStateAgainstTable(currentTargetTable, submode) + != RETURN_OK) { + triggerEvent(CANT_KEEP_MODE, mode, submode); + cantKeepMode(); + } + } + } +} + +HybridIterator Subsystem::getSequence(Mode_t id) { + SequenceInfo *sequenceInfo = modeSequences.findValue(id); + if (sequenceInfo->entries.islinked) { + return HybridIterator( + sequenceInfo->entries.firstLinkedElement); + } else { + return HybridIterator( + sequenceInfo->entries.array->front(), + sequenceInfo->entries.array->back()); + } +} + +HybridIterator Subsystem::getTable(Mode_t id) { + EntryPointer *entry = modeTables.findValue(id); + if (entry->islinked) { + return HybridIterator(entry->firstLinkedElement); + } else { + return HybridIterator(entry->array->front(), + entry->array->back()); + } +} + +ReturnValue_t Subsystem::handleCommandMessage(CommandMessage *message) { + switch (message->getCommand()) { + case HealthMessage::HEALTH_INFO: { + HealthState health = HealthMessage::getHealth(message); + if (health != EXTERNAL_CONTROL) { + //Ignore external control, as it has an effect only if the mode changes, + //which is communicated with an additional mode info event. + childrenChangedHealth = true; + } + } + break; + case ModeSequenceMessage::ADD_SEQUENCE: { + FixedArrayList sequence; + const uint8_t *pointer; + size_t sizeRead; + ReturnValue_t result = IPCStore->getData( + ModeSequenceMessage::getStoreAddress(message), &pointer, + &sizeRead); + if (result == RETURN_OK) { + Mode_t fallbackId; + size_t size = sizeRead; + result = SerializeAdapter::deSerialize(&fallbackId, &pointer, &size, + SerializeIF::Endianness::BIG); + if (result == RETURN_OK) { + result = SerialArrayListAdapter::deSerialize( + &sequence, &pointer, &size, + SerializeIF::Endianness::BIG); + if (result == RETURN_OK) { + result = addSequence(&sequence, + ModeSequenceMessage::getSequenceId(message), + fallbackId); + } + } + IPCStore->deleteData(ModeSequenceMessage::getStoreAddress(message)); + } + replyToCommand(result, 0); + } + break; + case ModeSequenceMessage::ADD_TABLE: { + FixedArrayList table; + const uint8_t *pointer; + size_t sizeRead; + ReturnValue_t result = IPCStore->getData( + ModeSequenceMessage::getStoreAddress(message), &pointer, + &sizeRead); + if (result == RETURN_OK) { + size_t size = sizeRead; + result = SerialArrayListAdapter::deSerialize(&table, + &pointer, &size, SerializeIF::Endianness::BIG); + if (result == RETURN_OK) { + result = addTable(&table, + ModeSequenceMessage::getSequenceId(message)); + } + IPCStore->deleteData(ModeSequenceMessage::getStoreAddress(message)); + } + replyToCommand(result, 0); + + } + break; + case ModeSequenceMessage::DELETE_SEQUENCE:{ + if (isInTransition) { + replyToCommand(IN_TRANSITION, 0); + break; + } + ReturnValue_t result = deleteSequence(ModeSequenceMessage::getSequenceId(message)); + replyToCommand(result, 0); + } + break; + case ModeSequenceMessage::DELETE_TABLE:{ + if (isInTransition) { + replyToCommand(IN_TRANSITION, 0); + break; + } + ReturnValue_t result = deleteTable(ModeSequenceMessage::getTableId(message)); + replyToCommand(result, 0); + } + break; + case ModeSequenceMessage::LIST_SEQUENCES: { + SerialFixedArrayListAdapter sequences; + FixedMap::Iterator iter; + for (iter = modeSequences.begin(); iter != modeSequences.end(); + ++iter) { + sequences.insert(iter.value->first); + } + SerializeIF *pointer = &sequences; + sendSerializablesAsCommandMessage(ModeSequenceMessage::SEQUENCE_LIST, + &pointer, 1); + } + break; + case ModeSequenceMessage::LIST_TABLES: { + SerialFixedArrayListAdapter tables; + FixedMap::Iterator iter; + for (iter = modeTables.begin(); iter != modeTables.end(); ++iter) { + tables.insert(iter.value->first); + } + SerializeIF *pointer = &tables; + sendSerializablesAsCommandMessage(ModeSequenceMessage::TABLE_LIST, + &pointer, 1); + } + break; + case ModeSequenceMessage::READ_SEQUENCE: { + ReturnValue_t result; + Mode_t sequence = ModeSequenceMessage::getSequenceId(message); + SequenceInfo *sequenceInfo = NULL; + result = modeSequences.find(sequence, &sequenceInfo); + if (result != RETURN_OK) { + replyToCommand(result, 0); + } + + SerializeIF *elements[3]; + SerializeElement sequenceId(sequence); + SerializeElement fallbackSequenceId( + getFallbackSequence(sequence)); + + elements[0] = &sequenceId; + elements[1] = &fallbackSequenceId; + + if (sequenceInfo->entries.islinked) { + SerialLinkedListAdapter list( + sequenceInfo->entries.firstLinkedElement, true); + elements[2] = &list; + sendSerializablesAsCommandMessage(ModeSequenceMessage::SEQUENCE, + elements, 3); + } else { + SerialArrayListAdapter serializableArray( + sequenceInfo->entries.array); + + elements[2] = &serializableArray; + sendSerializablesAsCommandMessage(ModeSequenceMessage::SEQUENCE, + elements, 3); + } + } + break; + case ModeSequenceMessage::READ_TABLE: { + ReturnValue_t result; + Mode_t table = ModeSequenceMessage::getSequenceId(message); + EntryPointer *entry = NULL; + result = modeTables.find(table, &entry); + if (result != RETURN_OK) { + replyToCommand(result, 0); + } + + SerializeIF *elements[2]; + SerializeElement tableId(table); + + elements[0] = &tableId; + + if (entry->islinked) { + SerialLinkedListAdapter list( + entry->firstLinkedElement, true); + elements[1] = &list; + sendSerializablesAsCommandMessage(ModeSequenceMessage::TABLE, + elements, 2); + } else { + SerialArrayListAdapter serializableArray( + entry->array); + elements[1] = &serializableArray; + sendSerializablesAsCommandMessage(ModeSequenceMessage::TABLE, + elements, 2); + } + } + break; + case ModeSequenceMessage::READ_FREE_SEQUENCE_SLOTS: { + uint32_t freeSlots = modeSequences.maxSize() - modeSequences.size(); + CommandMessage reply; + ModeSequenceMessage::setModeSequenceMessage(&reply, + ModeSequenceMessage::FREE_SEQUENCE_SLOTS, freeSlots); + commandQueue->reply(&reply); + } + break; + case ModeSequenceMessage::READ_FREE_TABLE_SLOTS: { + uint32_t free = modeTables.maxSize() - modeTables.size(); + CommandMessage reply; + ModeSequenceMessage::setModeSequenceMessage(&reply, + ModeSequenceMessage::FREE_TABLE_SLOTS, free); + commandQueue->reply(&reply); + } + break; + default: + return RETURN_FAILED; + } + return RETURN_OK; +} + +void Subsystem::replyToCommand(ReturnValue_t status, uint32_t parameter) { + if (status == RETURN_OK) { + CommandMessage reply(CommandMessage::REPLY_COMMAND_OK, 0, 0); + commandQueue->reply(&reply); + } else { + CommandMessage reply(CommandMessage::REPLY_REJECTED, status, 0); + commandQueue->reply(&reply); + } +} + +ReturnValue_t Subsystem::addSequence(ArrayList *sequence, + Mode_t id, Mode_t fallbackSequence, bool inStore, bool preInit) { + + ReturnValue_t result; + + //Before initialize() is called, tables must not be checked as the children are not added yet. + //Sequences added before are checked by initialize() + if (!preInit) { + result = checkSequence( + HybridIterator(sequence->front(), + sequence->back()), fallbackSequence); + if (result != RETURN_OK) { + return result; + } + } + + SequenceInfo info; + + info.fallbackSequence = fallbackSequence; + + info.entries.islinked = inStore; + info.entries.array = sequence; + + result = modeSequences.insert(id, info); + + if (result != RETURN_OK) { + return result; + } + + if (inStore) { +#ifdef USE_MODESTORE + result = modeStore->storeArray(sequence, + &(modeSequences.find(id)->entries.firstLinkedElement)); + if (result != RETURN_OK) { + modeSequences.erase(id); + } +#else + modeSequences.erase(id); + return RETURN_FAILED; +#endif + } + + return result; + +} + +ReturnValue_t Subsystem::addTable(ArrayList *table, Mode_t id, + bool inStore, bool preInit) { + + ReturnValue_t result; + + //Before initialize() is called, tables must not be checked as the children are not added yet. + //Tables added before are checked by initialize() + if (!preInit) { + result = checkTable( + HybridIterator(table->front(), table->back())); + if (result != HasReturnvaluesIF::RETURN_OK) { + return result; + } + } + + EntryPointer pointer; + + pointer.islinked = inStore; + pointer.array = table; + + result = modeTables.insert(id, pointer); + + if (result != RETURN_OK) { + return result; + } + + if (inStore) { +#ifdef USE_MODESTORE + result = modeStore->storeArray(table, + &(modeTables.find(id)->firstLinkedElement)); + if (result != RETURN_OK) { + modeTables.erase(id); + } +#else + modeTables.erase(id); + return RETURN_FAILED; +#endif + } + return result; +} + +ReturnValue_t Subsystem::deleteSequence(Mode_t id) { + + if (isFallbackSequence(id)) { + return IS_FALLBACK_SEQUENCE; + } + + SequenceInfo *sequenceInfo; + ReturnValue_t result; + result = modeSequences.find(id, &sequenceInfo); + if (result != HasReturnvaluesIF::RETURN_OK) { + return result; + } + if (!sequenceInfo->entries.islinked) { + return ACCESS_DENIED; + } + +#ifdef USE_MODESTORE + modeStore->deleteList(sequenceInfo->entries.firstLinkedElement); +#endif + modeSequences.erase(id); + return RETURN_OK; +} + +ReturnValue_t Subsystem::deleteTable(Mode_t id) { + + if (isTableUsed(id)) { + return TABLE_IN_USE; + } + + EntryPointer *pointer; + ReturnValue_t result; + result = modeTables.find(id, &pointer); + if (result != HasReturnvaluesIF::RETURN_OK) { + return result; + } + if (!pointer->islinked) { + return ACCESS_DENIED; + } + +#ifdef USE_MODESTORE + modeStore->deleteList(pointer->firstLinkedElement); +#endif + modeSequences.erase(id); + return RETURN_OK; +} + +ReturnValue_t Subsystem::initialize() { + ReturnValue_t result = SubsystemBase::initialize(); + + if (result != RETURN_OK) { + return result; + } + + IPCStore = objectManager->get(objects::IPC_STORE); + if (IPCStore == NULL) { + return RETURN_FAILED; + } + +#ifdef USE_MODESTORE + modeStore = objectManager->get(objects::MODE_STORE); + + if (modeStore == NULL) { + return RETURN_FAILED; + } +#endif + + if ((modeSequences.maxSize() > MAX_NUMBER_OF_TABLES_OR_SEQUENCES) + || (modeTables.maxSize() > MAX_NUMBER_OF_TABLES_OR_SEQUENCES)) { + return TABLE_OR_SEQUENCE_LENGTH_INVALID; + } + + mode = initialMode; + + return RETURN_OK; +} + +MessageQueueId_t Subsystem::getSequenceCommandQueue() const { + return SubsystemBase::getCommandQueue(); +} + +ReturnValue_t Subsystem::checkModeCommand(Mode_t mode, Submode_t submode, + uint32_t *msToReachTheMode) { + //Need to accept all submodes to be able to inherit submodes +// if (submode != SUBMODE_NONE) { +// return INVALID_SUBMODE; +// } + + if (isInTransition && (mode != getFallbackSequence(targetMode))) { + return HasModesIF::IN_TRANSITION; + } else { + return checkSequence(mode); + } +} + +void Subsystem::startTransition(Mode_t sequence, Submode_t submode) { + if (modeHelper.isForced()) { + triggerEvent(FORCING_MODE, sequence, submode); + } else { + triggerEvent(CHANGING_MODE, sequence, submode); + } + targetMode = sequence; + targetSubmode = submode; + isInTransition = true; + commandsOutstanding = 0; + currentSequenceIterator = getSequence(sequence); + + currentTargetTable = getTable(currentSequenceIterator->getTableId()); + + ++currentSequenceIterator; + + if (currentSequenceIterator.value != NULL) { + executeTable(getCurrentTable(), targetSubmode); + } +} + +Mode_t Subsystem::getFallbackSequence(Mode_t sequence) { + for (FixedMap::Iterator iter = modeSequences.begin(); + iter != modeSequences.end(); ++iter) { + if (iter.value->first == sequence) { + return iter->fallbackSequence; + } + } + return -1; +} + +bool Subsystem::isFallbackSequence(Mode_t SequenceId) { + for (FixedMap::Iterator iter = modeSequences.begin(); + iter != modeSequences.end(); iter++) { + if (iter->fallbackSequence == SequenceId) { + return true; + } + } + return false; +} + +bool Subsystem::isTableUsed(Mode_t tableId) { + for (FixedMap::Iterator sequence = + modeSequences.begin(); sequence != modeSequences.end(); + sequence++) { + HybridIterator sequenceIterator = getSequence( + sequence.value->first); + while (sequenceIterator.value != NULL) { + if (sequenceIterator->getTableId() == tableId) { + return true; + } + ++sequenceIterator; + } + } + return false; +} + +void Subsystem::transitionFailed(ReturnValue_t failureCode, + uint32_t parameter) { + triggerEvent(MODE_TRANSITION_FAILED, failureCode, parameter); + if (mode == targetMode) { + //already tried going back to the current mode + //go into fallback mode, also set current mode to fallback mode, so we come here at the next fail + modeHelper.setForced(true); + ReturnValue_t result; + if ((result = checkSequence(getFallbackSequence(mode))) != RETURN_OK) { + triggerEvent(FALLBACK_FAILED, result, getFallbackSequence(mode)); + isInTransition = false; //keep still and allow arbitrary mode commands to recover + return; + } + mode = getFallbackSequence(mode); + startTransition(mode, submode); + } else { + //try to go back to the current mode + startTransition(mode, submode); + } +} + +void Subsystem::sendSerializablesAsCommandMessage(Command_t command, + SerializeIF **elements, uint8_t count) { + ReturnValue_t result; + size_t maxSize = 0; + for (uint8_t i = 0; i < count; i++) { + maxSize += elements[i]->getSerializedSize(); + } + uint8_t *storeBuffer; + store_address_t address; + size_t size = 0; + + result = IPCStore->getFreeElement(&address, maxSize, &storeBuffer); + if (result != HasReturnvaluesIF::RETURN_OK) { + replyToCommand(result, 0); + return; + } + for (uint8_t i = 0; i < count; i++) { + elements[i]->serialize(&storeBuffer, &size, maxSize, + SerializeIF::Endianness::BIG); + } + CommandMessage reply; + ModeSequenceMessage::setModeSequenceMessage(&reply, command, address); + if (commandQueue->reply(&reply) != RETURN_OK) { + IPCStore->deleteData(address); + } +} + +ReturnValue_t Subsystem::checkObjectConnections() { + ReturnValue_t result = RETURN_OK; + for (FixedMap::Iterator iter = modeSequences.begin(); + iter != modeSequences.end(); iter++) { + result = checkSequence(iter.value->first); + if (result != RETURN_OK) { + return result; + } + + } + return RETURN_OK; +} + +void Subsystem::setInitialMode(Mode_t mode) { + initialMode = mode; +} + +void Subsystem::cantKeepMode() { + ReturnValue_t result; + if ((result = checkSequence(getFallbackSequence(mode))) != RETURN_OK) { + triggerEvent(FALLBACK_FAILED, result, getFallbackSequence(mode)); + return; + } + + modeHelper.setForced(true); + + //already set the mode, so that we do not try to go back in our old mode when the transition fails + mode = getFallbackSequence(mode); + //SHOULDDO: We should store submodes for fallback sequence as well, otherwise we should get rid of submodes completely. + startTransition(mode, SUBMODE_NONE); +} diff --git a/subsystem/Subsystem.h b/subsystem/Subsystem.h index b296a2e88..de4761faa 100644 --- a/subsystem/Subsystem.h +++ b/subsystem/Subsystem.h @@ -92,7 +92,7 @@ protected: bool childrenChangedHealth; - uint32_t uptimeStartTable; + uint32_t uptimeStartTable = 0; HybridIterator currentTargetTable; @@ -108,10 +108,10 @@ protected: FixedMap modeSequences; - StorageManagerIF *IPCStore; + StorageManagerIF *IPCStore = nullptr; #ifdef USE_MODESTORE - ModeStoreIF *modeStore; + ModeStoreIF *modeStore = nullptr; #endif bool existsModeSequence(Mode_t id); diff --git a/timemanager/CCSDSTime.cpp b/timemanager/CCSDSTime.cpp index e073ad860..7124d8bd9 100644 --- a/timemanager/CCSDSTime.cpp +++ b/timemanager/CCSDSTime.cpp @@ -119,7 +119,7 @@ ReturnValue_t CCSDSTime::convertFromCCS(Clock::TimeOfDay_t* to, const uint8_t* f if (temp->pField & (1 << 3)) { //day of year variation uint16_t tempDay = (temp->month << 8) + temp->day; - ReturnValue_t result = convertDaysOfYear(tempDay, to->year, + result = convertDaysOfYear(tempDay, to->year, &(temp->month), &(temp->day)); if (result != RETURN_OK) { return result;