testArduino/fsfw/subsystem/Subsystem.cpp

664 lines
18 KiB
C++

#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.h"
#include <string>
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<ModeListEntry> 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<ModeListEntry> 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<ModeListEntry> 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<ModeListEntry> Subsystem::getSequence(Mode_t id) {
SequenceInfo *sequenceInfo = modeSequences.findValue(id);
if (sequenceInfo->entries.islinked) {
return HybridIterator<ModeListEntry>(
sequenceInfo->entries.firstLinkedElement);
} else {
return HybridIterator<ModeListEntry>(
sequenceInfo->entries.array->front(),
sequenceInfo->entries.array->back());
}
}
HybridIterator<ModeListEntry> Subsystem::getTable(Mode_t id) {
EntryPointer *entry = modeTables.findValue(id);
if (entry->islinked) {
return HybridIterator<ModeListEntry>(entry->firstLinkedElement);
} else {
return HybridIterator<ModeListEntry>(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<ModeListEntry, MAX_LENGTH_OF_TABLE_OR_SEQUENCE> 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<ModeListEntry>::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<ModeListEntry, MAX_LENGTH_OF_TABLE_OR_SEQUENCE> 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<ModeListEntry>::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<Mode_t, MAX_NUMBER_OF_TABLES_OR_SEQUENCES> sequences;
FixedMap<Mode_t, SequenceInfo>::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<Mode_t, MAX_NUMBER_OF_TABLES_OR_SEQUENCES> tables;
FixedMap<Mode_t, EntryPointer>::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<Mode_t> sequenceId(sequence);
SerializeElement<Mode_t> fallbackSequenceId(
getFallbackSequence(sequence));
elements[0] = &sequenceId;
elements[1] = &fallbackSequenceId;
if (sequenceInfo->entries.islinked) {
SerialLinkedListAdapter<ModeListEntry> list(
sequenceInfo->entries.firstLinkedElement, true);
elements[2] = &list;
sendSerializablesAsCommandMessage(ModeSequenceMessage::SEQUENCE,
elements, 3);
} else {
SerialArrayListAdapter<ModeListEntry> 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<Mode_t> tableId(table);
elements[0] = &tableId;
if (entry->islinked) {
SerialLinkedListAdapter<ModeListEntry> list(
entry->firstLinkedElement, true);
elements[1] = &list;
sendSerializablesAsCommandMessage(ModeSequenceMessage::TABLE,
elements, 2);
} else {
SerialArrayListAdapter<ModeListEntry> 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<ModeListEntry> *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<ModeListEntry>(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<ModeListEntry> *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<ModeListEntry>(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<StorageManagerIF>(objects::IPC_STORE);
if (IPCStore == NULL) {
return RETURN_FAILED;
}
#ifdef USE_MODESTORE
modeStore = objectManager->get<ModeStoreIF>(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<Mode_t, SequenceInfo>::Iterator iter = modeSequences.begin();
iter != modeSequences.end(); ++iter) {
if (iter.value->first == sequence) {
return iter->second.fallbackSequence;
}
}
return -1;
}
bool Subsystem::isFallbackSequence(Mode_t SequenceId) {
for (FixedMap<Mode_t, SequenceInfo>::Iterator iter = modeSequences.begin();
iter != modeSequences.end(); iter++) {
if (iter->second.fallbackSequence == SequenceId) {
return true;
}
}
return false;
}
bool Subsystem::isTableUsed(Mode_t tableId) {
for (FixedMap<Mode_t, SequenceInfo>::Iterator sequence =
modeSequences.begin(); sequence != modeSequences.end();
sequence++) {
HybridIterator<ModeListEntry> 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<Mode_t, SequenceInfo>::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);
}