Improve Subsystem DHB logic #112

Merged
muellerr merged 16 commits from mueller/improve-ss-dhb-logic into develop 2022-11-02 10:32:27 +01:00
29 changed files with 320 additions and 249 deletions

View File

@ -7,6 +7,13 @@
#include "../returnvalues/returnvalue.h"
#include "ArrayList.h"
namespace mapdefs {
static const uint8_t INTERFACE_ID = CLASS_ID::FIXED_MAP;
static const ReturnValue_t KEY_ALREADY_EXISTS = MAKE_RETURN_CODE(0x01);
static const ReturnValue_t MAP_FULL = MAKE_RETURN_CODE(0x02);
static const ReturnValue_t KEY_DOES_NOT_EXIST = MAKE_RETURN_CODE(0x03);
} // namespace mapdefs
/**
* @brief Map implementation for maps with a pre-defined size.
* @details
@ -24,11 +31,6 @@ class FixedMap : public SerializeIF {
"derived class from SerializeIF to be serialize-able");
public:
static const uint8_t INTERFACE_ID = CLASS_ID::FIXED_MAP;
static const ReturnValue_t KEY_ALREADY_EXISTS = MAKE_RETURN_CODE(0x01);
static const ReturnValue_t MAP_FULL = MAKE_RETURN_CODE(0x02);
static const ReturnValue_t KEY_DOES_NOT_EXIST = MAKE_RETURN_CODE(0x03);
private:
static const key_t EMPTY_SLOT = -1;
ArrayList<std::pair<key_t, T>, uint32_t> theMap;
@ -76,10 +78,10 @@ class FixedMap : public SerializeIF {
ReturnValue_t insert(key_t key, T value, Iterator* storedValue = nullptr) {
if (exists(key) == returnvalue::OK) {
return KEY_ALREADY_EXISTS;
return mapdefs::KEY_ALREADY_EXISTS;
}
if (_size == theMap.maxSize()) {
return MAP_FULL;
return mapdefs::MAP_FULL;
}
theMap[_size].first = key;
theMap[_size].second = value;
@ -93,7 +95,7 @@ class FixedMap : public SerializeIF {
ReturnValue_t insert(std::pair<key_t, T> pair) { return insert(pair.first, pair.second); }
ReturnValue_t exists(key_t key) const {
ReturnValue_t result = KEY_DOES_NOT_EXIST;
ReturnValue_t result = mapdefs::KEY_DOES_NOT_EXIST;
if (findIndex(key) < _size) {
result = returnvalue::OK;
}
@ -103,7 +105,7 @@ class FixedMap : public SerializeIF {
ReturnValue_t erase(Iterator* iter) {
uint32_t i;
if ((i = findIndex((*iter).value->first)) >= _size) {
return KEY_DOES_NOT_EXIST;
return mapdefs::KEY_DOES_NOT_EXIST;
}
theMap[i] = theMap[_size - 1];
--_size;
@ -114,7 +116,7 @@ class FixedMap : public SerializeIF {
ReturnValue_t erase(key_t key) {
uint32_t i;
if ((i = findIndex(key)) >= _size) {
return KEY_DOES_NOT_EXIST;
return mapdefs::KEY_DOES_NOT_EXIST;
}
theMap[i] = theMap[_size - 1];
--_size;

View File

@ -4,11 +4,10 @@
#include "fsfw/ipc/QueueFactory.h"
#include "fsfw/objectmanager/ObjectManager.h"
#include "fsfw/subsystem/SubsystemBase.h"
#include "fsfw/subsystem/helper.h"
ControllerBase::ControllerBase(object_id_t setObjectId, object_id_t parentId,
size_t commandQueueDepth)
ControllerBase::ControllerBase(object_id_t setObjectId, size_t commandQueueDepth)
: SystemObject(setObjectId),
parentId(parentId),
mode(MODE_OFF),
submode(SUBMODE_NONE),
modeHelper(this),
@ -21,33 +20,15 @@ ControllerBase::ControllerBase(object_id_t setObjectId, object_id_t parentId,
ControllerBase::~ControllerBase() { QueueFactory::instance()->deleteMessageQueue(commandQueue); }
ReturnValue_t ControllerBase::initialize() {
ReturnValue_t result = SystemObject::initialize();
ReturnValue_t result = modeHelper.initialize();
if (result != returnvalue::OK) {
return result;
}
MessageQueueId_t parentQueue = 0;
if (parentId != objects::NO_OBJECT) {
auto* parent = ObjectManager::instance()->get<SubsystemBase>(parentId);
if (parent == nullptr) {
return returnvalue::FAILED;
}
parentQueue = parent->getCommandQueue();
parent->registerChild(getObjectId());
}
result = healthHelper.initialize(parentQueue);
result = healthHelper.initialize();
if (result != returnvalue::OK) {
return result;
}
result = modeHelper.initialize(parentQueue);
if (result != returnvalue::OK) {
return result;
}
return returnvalue::OK;
return SystemObject::initialize();
}
MessageQueueId_t ControllerBase::getCommandQueue() const { return commandQueue->getId(); }
@ -120,3 +101,13 @@ void ControllerBase::setTaskIF(PeriodicTaskIF* task_) { executingTask = task_; }
void ControllerBase::changeHK(Mode_t mode_, Submode_t submode_, bool enable) {}
ReturnValue_t ControllerBase::initializeAfterTaskCreation() { return returnvalue::OK; }
const HasHealthIF* ControllerBase::getOptHealthIF() const { return this; }
const HasModesIF& ControllerBase::getModeIF() const { return *this; }
ModeTreeChildIF& ControllerBase::getModeTreeChildIF() { return *this; }
ReturnValue_t ControllerBase::connectModeTreeParent(HasModeTreeChildrenIF& parent) {
return modetree::connectModeTreeParent(parent, *this, healthHelper, modeHelper);
}

View File

@ -6,6 +6,9 @@
#include "fsfw/modes/HasModesIF.h"
#include "fsfw/modes/ModeHelper.h"
#include "fsfw/objectmanager/SystemObject.h"
#include "fsfw/subsystem/HasModeTreeChildrenIF.h"
#include "fsfw/subsystem/ModeTreeChildIF.h"
#include "fsfw/subsystem/ModeTreeConnectionIF.h"
#include "fsfw/tasks/ExecutableObjectIF.h"
#include "fsfw/tasks/PeriodicTaskIF.h"
@ -18,13 +21,18 @@
class ControllerBase : public HasModesIF,
public HasHealthIF,
public ExecutableObjectIF,
public ModeTreeChildIF,
public ModeTreeConnectionIF,
public SystemObject {
public:
static const Mode_t MODE_NORMAL = 2;
ControllerBase(object_id_t setObjectId, object_id_t parentId, size_t commandQueueDepth = 3);
ControllerBase(object_id_t setObjectId, size_t commandQueueDepth = 3);
~ControllerBase() override;
ReturnValue_t connectModeTreeParent(HasModeTreeChildrenIF &parent) override;
ModeTreeChildIF &getModeTreeChildIF() override;
/** SystemObject override */
ReturnValue_t initialize() override;
@ -38,6 +46,8 @@ class ControllerBase : public HasModesIF,
ReturnValue_t performOperation(uint8_t opCode) override;
void setTaskIF(PeriodicTaskIF *task) override;
ReturnValue_t initializeAfterTaskCreation() override;
const HasHealthIF *getOptHealthIF() const override;
const HasModesIF &getModeIF() const override;
protected:
/**
@ -56,8 +66,6 @@ class ControllerBase : public HasModesIF,
ReturnValue_t checkModeCommand(Mode_t mode, Submode_t submode,
uint32_t *msToReachTheMode) override = 0;
const object_id_t parentId;
Mode_t mode;
Submode_t submode;

View File

@ -1,8 +1,7 @@
#include "fsfw/controller/ExtendedControllerBase.h"
ExtendedControllerBase::ExtendedControllerBase(object_id_t objectId, object_id_t parentId,
size_t commandQueueDepth)
: ControllerBase(objectId, parentId, commandQueueDepth),
ExtendedControllerBase::ExtendedControllerBase(object_id_t objectId, size_t commandQueueDepth)
: ControllerBase(objectId, commandQueueDepth),
poolManager(this, commandQueue),
actionHelper(this, commandQueue) {}

View File

@ -17,7 +17,7 @@ class ExtendedControllerBase : public ControllerBase,
public HasActionsIF,
public HasLocalDataPoolIF {
public:
ExtendedControllerBase(object_id_t objectId, object_id_t parentId, size_t commandQueueDepth = 3);
ExtendedControllerBase(object_id_t objectId, size_t commandQueueDepth = 3);
~ExtendedControllerBase() override;
/* SystemObjectIF overrides */

View File

@ -1,7 +1,7 @@
#include "fsfw/devicehandlers/AssemblyBase.h"
AssemblyBase::AssemblyBase(object_id_t objectId, object_id_t parentId, uint16_t commandQueueDepth)
: SubsystemBase(objectId, parentId, MODE_OFF, commandQueueDepth),
AssemblyBase::AssemblyBase(object_id_t objectId, uint16_t commandQueueDepth)
: SubsystemBase(objectId, MODE_OFF, commandQueueDepth),
internalState(STATE_NONE),
recoveryState(RECOVERY_IDLE),
recoveringDevice(childrenMap.end()),

View File

@ -41,7 +41,7 @@ class AssemblyBase : public SubsystemBase {
static const ReturnValue_t NEED_TO_CHANGE_HEALTH = MAKE_RETURN_CODE(0x05);
static const ReturnValue_t NOT_ENOUGH_CHILDREN_IN_CORRECT_STATE = MAKE_RETURN_CODE(0xa1);
AssemblyBase(object_id_t objectId, object_id_t parentId, uint16_t commandQueueDepth = 8);
AssemblyBase(object_id_t objectId, uint16_t commandQueueDepth = 8);
virtual ~AssemblyBase();
protected:

View File

@ -3,17 +3,12 @@
#include "fsfw/subsystem/SubsystemBase.h"
ChildHandlerBase::ChildHandlerBase(object_id_t setObjectId, object_id_t deviceCommunication,
CookieIF* cookie, object_id_t hkDestination,
uint32_t thermalStatePoolId, uint32_t thermalRequestPoolId,
object_id_t parent, FailureIsolationBase* customFdir,
size_t cmdQueueSize)
CookieIF* cookie, HasModeTreeChildrenIF& parent,
FailureIsolationBase* customFdir, size_t cmdQueueSize)
: DeviceHandlerBase(setObjectId, deviceCommunication, cookie,
(customFdir == nullptr ? &childHandlerFdir : customFdir), cmdQueueSize),
parentId(parent),
childHandlerFdir(setObjectId) {
this->setHkDestination(hkDestination);
this->setThermalStateRequestPoolIds(thermalStatePoolId, thermalRequestPoolId);
}
parent(parent),
childHandlerFdir(setObjectId) {}
ChildHandlerBase::~ChildHandlerBase() {}
@ -23,21 +18,5 @@ ReturnValue_t ChildHandlerBase::initialize() {
return result;
}
MessageQueueId_t parentQueue = 0;
if (parentId != objects::NO_OBJECT) {
SubsystemBase* parent = ObjectManager::instance()->get<SubsystemBase>(parentId);
if (parent == NULL) {
return returnvalue::FAILED;
}
parentQueue = parent->getCommandQueue();
parent->registerChild(getObjectId());
}
healthHelper.setParentQueue(parentQueue);
modeHelper.setParentQueue(parentQueue);
return returnvalue::OK;
return DeviceHandlerBase::connectModeTreeParent(parent);
}

View File

@ -1,22 +1,23 @@
#ifndef FSFW_DEVICEHANDLER_CHILDHANDLERBASE_H_
#define FSFW_DEVICEHANDLER_CHILDHANDLERBASE_H_
#include <fsfw/subsystem/HasModeTreeChildrenIF.h>
#include "ChildHandlerFDIR.h"
#include "DeviceHandlerBase.h"
class ChildHandlerBase : public DeviceHandlerBase {
public:
ChildHandlerBase(object_id_t setObjectId, object_id_t deviceCommunication, CookieIF* cookie,
object_id_t hkDestination, uint32_t thermalStatePoolId,
uint32_t thermalRequestPoolId, object_id_t parent = objects::NO_OBJECT,
FailureIsolationBase* customFdir = nullptr, size_t cmdQueueSize = 20);
HasModeTreeChildrenIF& parent, FailureIsolationBase* customFdir = nullptr,
size_t cmdQueueSize = 20);
virtual ~ChildHandlerBase();
virtual ReturnValue_t initialize();
protected:
const uint32_t parentId;
HasModeTreeChildrenIF& parent;
ChildHandlerFDIR childHandlerFdir;
};

View File

@ -1,4 +1,4 @@
#include "fsfw/devicehandlers/DeviceHandlerBase.h"
#include "DeviceHandlerBase.h"
#include "fsfw/datapoollocal/LocalPoolVariable.h"
#include "fsfw/devicehandlers/AcceptsDeviceResponsesIF.h"
@ -12,6 +12,7 @@
#include "fsfw/serviceinterface/ServiceInterface.h"
#include "fsfw/storagemanager/StorageManagerIF.h"
#include "fsfw/subsystem/SubsystemBase.h"
#include "fsfw/subsystem/helper.h"
#include "fsfw/thermal/ThermalComponentIF.h"
object_id_t DeviceHandlerBase::powerSwitcherId = objects::NO_OBJECT;
@ -132,14 +133,6 @@ ReturnValue_t DeviceHandlerBase::initialize() {
new DeviceHandlerFailureIsolation(this->getObjectId(), defaultFdirParentId);
}
if (this->parent != objects::NO_OBJECT) {
HasModesIF* modeIF = ObjectManager::instance()->get<HasModesIF>(this->parent);
HasHealthIF* healthIF = ObjectManager::instance()->get<HasHealthIF>(this->parent);
if (modeIF != nullptr and healthIF != nullptr) {
setParentQueue(modeIF->getCommandQueue());
}
}
communicationInterface =
ObjectManager::instance()->get<DeviceCommunicationIF>(deviceCommunicationId);
if (communicationInterface == nullptr) {
@ -1578,8 +1571,6 @@ MessageQueueId_t DeviceHandlerBase::getCommanderQueueId(DeviceCommandId_t replyI
void DeviceHandlerBase::setCustomFdir(FailureIsolationBase* fdir) { this->fdirInstance = fdir; }
void DeviceHandlerBase::setParent(object_id_t parent) { this->parent = parent; }
void DeviceHandlerBase::setPowerSwitcher(PowerSwitchIF* switcher) {
this->powerSwitcher = switcher;
}
@ -1602,6 +1593,16 @@ void DeviceHandlerBase::disableCommandsAndReplies() {
}
}
ReturnValue_t DeviceHandlerBase::connectModeTreeParent(HasModeTreeChildrenIF& parent) {
return modetree::connectModeTreeParent(parent, *this, healthHelper, modeHelper);
}
const HasHealthIF* DeviceHandlerBase::getOptHealthIF() const { return this; }
const HasModesIF& DeviceHandlerBase::getModeIF() const { return *this; }
ModeTreeChildIF& DeviceHandlerBase::getModeTreeChildIF() { return *this; }
ReturnValue_t DeviceHandlerBase::finishAction(bool success, DeviceCommandId_t action,
ReturnValue_t result) {
auto commandIter = deviceCommandMap.find(action);

View File

@ -21,6 +21,7 @@
#include "fsfw/returnvalues/returnvalue.h"
#include "fsfw/serviceinterface/ServiceInterface.h"
#include "fsfw/serviceinterface/serviceInterfaceDefintions.h"
#include "fsfw/subsystem/ModeTreeConnectionIF.h"
#include "fsfw/tasks/ExecutableObjectIF.h"
#include "fsfw/tasks/PeriodicTaskIF.h"
#include "fsfw/util/dataWrapper.h"
@ -84,6 +85,8 @@ class DeviceHandlerBase : public DeviceHandlerIF,
public HasModesIF,
public HasHealthIF,
public HasActionsIF,
public ModeTreeChildIF,
public ModeTreeConnectionIF,
public ReceivesParameterMessagesIF,
public HasLocalDataPoolIF {
friend void(Factory::setStaticFrameworkObjectIds)();
@ -104,7 +107,6 @@ class DeviceHandlerBase : public DeviceHandlerIF,
FailureIsolationBase *fdirInstance = nullptr, size_t cmdQueueSize = 20);
void setCustomFdir(FailureIsolationBase *fdir);
void setParent(object_id_t parent);
void setPowerSwitcher(PowerSwitchIF *switcher);
void setHkDestination(object_id_t hkDestination);
@ -121,6 +123,10 @@ class DeviceHandlerBase : public DeviceHandlerIF,
lp_id_t thermalStatePoolId = DeviceHandlerIF::DEFAULT_THERMAL_STATE_POOL_ID,
lp_id_t thermalRequestPoolId = DeviceHandlerIF::DEFAULT_THERMAL_HEATING_REQUEST_POOL_ID,
uint32_t thermalSetId = DeviceHandlerIF::DEFAULT_THERMAL_SET_ID);
ReturnValue_t connectModeTreeParent(HasModeTreeChildrenIF &parent) override;
ModeTreeChildIF &getModeTreeChildIF() override;
/**
* @brief Helper function to ease device handler development.
* This will instruct the transition to MODE_ON immediately
@ -166,7 +172,7 @@ class DeviceHandlerBase : public DeviceHandlerIF,
* @param counter Specifies which Action to perform
* @return returnvalue::OK for successful execution
*/
virtual ReturnValue_t performOperation(uint8_t counter) override;
ReturnValue_t performOperation(uint8_t counter) override;
/**
* @brief Initializes the device handler
@ -176,14 +182,14 @@ class DeviceHandlerBase : public DeviceHandlerIF,
* Calls fillCommandAndReplyMap().
* @return
*/
virtual ReturnValue_t initialize() override;
ReturnValue_t initialize() override;
/**
* @brief Intialization steps performed after all tasks have been created.
* This function will be called by the executing task.
* @return
*/
virtual ReturnValue_t initializeAfterTaskCreation() override;
ReturnValue_t initializeAfterTaskCreation() override;
/** Destructor. */
virtual ~DeviceHandlerBase();
@ -200,6 +206,8 @@ class DeviceHandlerBase : public DeviceHandlerIF,
virtual object_id_t getObjectId() const override;
/**
* This is a helper method for classes which are parent nodes in the mode tree.
* It registers the passed queue as the destination for mode and health messages.
* @param parentQueueId
*/
virtual void setParentQueue(MessageQueueId_t parentQueueId);
@ -961,6 +969,9 @@ class DeviceHandlerBase : public DeviceHandlerIF,
*/
LocalDataPoolManager *getHkManagerHandle() override;
const HasHealthIF *getOptHealthIF() const override;
const HasModesIF &getModeIF() const override;
/**
* Returns the delay cycle count of a reply.
* A count != 0 indicates that the command is already executed.

View File

@ -24,3 +24,13 @@ void ModeMessage::setCantReachMode(CommandMessage* message, ReturnValue_t reason
message->setParameter(reason);
message->setParameter2(0);
}
void ModeMessage::setModeAnnounceMessage(CommandMessage& message, bool recursive) {
Command_t cmd;
if (recursive) {
cmd = CMD_MODE_ANNOUNCE_RECURSIVELY;
} else {
cmd = CMD_MODE_ANNOUNCE;
}
message.setCommand(cmd);
}

View File

@ -1,43 +1,42 @@
#ifndef FSFW_MODES_MODEMESSAGE_H_
#define FSFW_MODES_MODEMESSAGE_H_
#include "../ipc/CommandMessage.h"
#include "fsfw/ipc/CommandMessage.h"
typedef uint32_t Mode_t;
typedef uint8_t Submode_t;
class ModeMessage {
private:
ModeMessage();
public:
static const uint8_t MESSAGE_ID = messagetypes::MODE_COMMAND;
static const Command_t CMD_MODE_COMMAND =
MAKE_COMMAND_ID(0x01); //!> Command to set the specified Mode, replies are: REPLY_MODE_REPLY,
//! REPLY_WRONG_MODE_REPLY, and REPLY_REJECTED; don't add any replies,
//! as this will break the subsystem mode machine!!
static const Command_t CMD_MODE_COMMAND_FORCED = MAKE_COMMAND_ID(
0xF1); //!> Command to set the specified Mode, regardless of external control flag, replies
//! are: REPLY_MODE_REPLY, REPLY_WRONG_MODE_REPLY, and REPLY_REJECTED; don't add any
//! replies, as this will break the subsystem mode machine!!
static const Command_t REPLY_MODE_REPLY =
MAKE_COMMAND_ID(0x02); //!> Reply to a CMD_MODE_COMMAND or CMD_MODE_READ
static const Command_t REPLY_MODE_INFO =
MAKE_COMMAND_ID(0x03); //!> Unrequested info about the current mode (used for composites to
//! inform their container of a changed mode)
static const Command_t REPLY_CANT_REACH_MODE = MAKE_COMMAND_ID(
0x04); //!> Reply in case a mode command can't be executed. Par1: returnCode, Par2: 0
static const Command_t REPLY_WRONG_MODE_REPLY =
MAKE_COMMAND_ID(0x05); //!> Reply to a CMD_MODE_COMMAND, indicating that a mode was commanded
//! and a transition started but was aborted; the parameters contain
//! the mode that was reached
static const Command_t CMD_MODE_READ = MAKE_COMMAND_ID(
0x06); //!> Command to read the current mode and reply with a REPLY_MODE_REPLY
static const Command_t CMD_MODE_ANNOUNCE = MAKE_COMMAND_ID(
0x07); //!> Command to trigger an ModeInfo Event. This command does NOT have a reply.
static const Command_t CMD_MODE_ANNOUNCE_RECURSIVELY =
MAKE_COMMAND_ID(0x08); //!> Command to trigger an ModeInfo Event and to send this command to
//! every child. This command does NOT have a reply.
//!> Command to set the specified Mode, replies are: REPLY_MODE_REPLY,
//! REPLY_WRONG_MODE_REPLY, and REPLY_REJECTED; don't add any replies,
//! as this will break the subsystem mode machine!!
static const Command_t CMD_MODE_COMMAND = MAKE_COMMAND_ID(0x01);
//!> Command to set the specified Mode, regardless of external control flag, replies
//! are: REPLY_MODE_REPLY, REPLY_WRONG_MODE_REPLY, and REPLY_REJECTED; don't add any
//! replies, as this will break the subsystem mode machine!!
static const Command_t CMD_MODE_COMMAND_FORCED = MAKE_COMMAND_ID(0xF1);
//!> Reply to a CMD_MODE_COMMAND or CMD_MODE_READ
static const Command_t REPLY_MODE_REPLY = MAKE_COMMAND_ID(0x02);
//!> Unrequested info about the current mode (used for composites to
//! inform their container of a changed mode)
static const Command_t REPLY_MODE_INFO = MAKE_COMMAND_ID(0x03);
//!> Reply in case a mode command can't be executed. Par1: returnCode, Par2: 0
static const Command_t REPLY_CANT_REACH_MODE = MAKE_COMMAND_ID(0x04);
//!> Reply to a CMD_MODE_COMMAND, indicating that a mode was commanded
//! and a transition started but was aborted; the parameters contain
//! the mode that was reached
static const Command_t REPLY_WRONG_MODE_REPLY = MAKE_COMMAND_ID(0x05);
//!> Command to read the current mode and reply with a REPLY_MODE_REPLY
static const Command_t CMD_MODE_READ = MAKE_COMMAND_ID(0x06);
//!> Command to trigger an ModeInfo Event. This command does NOT have a reply.
static const Command_t CMD_MODE_ANNOUNCE = MAKE_COMMAND_ID(0x07);
//!> Command to trigger an ModeInfo Event and to send this command to
//! every child. This command does NOT have a reply.
static const Command_t CMD_MODE_ANNOUNCE_RECURSIVELY = MAKE_COMMAND_ID(0x08);
ModeMessage() = delete;
static Mode_t getMode(const CommandMessage* message);
static Submode_t getSubmode(const CommandMessage* message);
@ -45,6 +44,7 @@ class ModeMessage {
static void setModeMessage(CommandMessage* message, Command_t command, Mode_t mode,
Submode_t submode);
static void setModeAnnounceMessage(CommandMessage& message, bool recursive);
static void setCantReachMode(CommandMessage* message, ReturnValue_t reason);
static void clear(CommandMessage* message);
};

View File

@ -20,6 +20,7 @@ ReturnValue_t CService200ModeCommanding::isValidSubservice(uint8_t subservice) {
case (Subservice::COMMAND_MODE_COMMAND):
case (Subservice::COMMAND_MODE_READ):
case (Subservice::COMMAND_MODE_ANNCOUNCE):
case (Subservice::COMMAND_MODE_ANNOUNCE_RECURSIVELY):
return returnvalue::OK;
default:
return AcceptsTelecommandsIF::INVALID_SUBSERVICE;
@ -53,15 +54,26 @@ ReturnValue_t CService200ModeCommanding::checkInterfaceAndAcquireMessageQueue(
ReturnValue_t CService200ModeCommanding::prepareCommand(CommandMessage *message, uint8_t subservice,
const uint8_t *tcData, size_t tcDataLen,
uint32_t *state, object_id_t objectId) {
ModePacket modeCommandPacket;
ReturnValue_t result =
modeCommandPacket.deSerialize(&tcData, &tcDataLen, SerializeIF::Endianness::BIG);
if (result != returnvalue::OK) {
return result;
ReturnValue_t result = returnvalue::OK;
if (subservice == Subservice::COMMAND_MODE_ANNCOUNCE or
subservice == Subservice::COMMAND_MODE_ANNOUNCE_RECURSIVELY) {
bool recursive = true;
if (subservice == Subservice::COMMAND_MODE_ANNCOUNCE) {
recursive = false;
}
ModeMessage::setModeAnnounceMessage(*message, recursive);
} else {
ModePacket modeCommandPacket;
ReturnValue_t result =
modeCommandPacket.deSerialize(&tcData, &tcDataLen, SerializeIF::Endianness::BIG);
if (result != returnvalue::OK) {
return result;
}
ModeMessage::setModeMessage(message, ModeMessage::CMD_MODE_COMMAND, modeCommandPacket.getMode(),
modeCommandPacket.getSubmode());
}
ModeMessage::setModeMessage(message, ModeMessage::CMD_MODE_COMMAND, modeCommandPacket.getMode(),
modeCommandPacket.getSubmode());
return result;
}

View File

@ -1,9 +1,9 @@
#ifndef FSFW_PUS_SERVICEPACKETS_SERVICE200PACKETS_H_
#define FSFW_PUS_SERVICEPACKETS_SERVICE200PACKETS_H_
#include "../../modes/ModeMessage.h"
#include "../../serialize/SerialLinkedListAdapter.h"
#include "../../serialize/SerializeIF.h"
#include "fsfw/modes/ModeMessage.h"
#include "fsfw/serialize/SerialLinkedListAdapter.h"
#include "fsfw/serialize/SerializeIF.h"
/**
* @brief Subservice 1, 2, 3, 4, 5

View File

@ -1,3 +1,4 @@
target_sources(${LIB_FSFW_NAME} PRIVATE Subsystem.cpp SubsystemBase.cpp)
target_sources(${LIB_FSFW_NAME} PRIVATE Subsystem.cpp SubsystemBase.cpp
helper.cpp)
add_subdirectory(modes)

View File

@ -0,0 +1,13 @@
#ifndef FSFW_SUBSYSTEM_HASMODETREECHILDRENIF_H_
#define FSFW_SUBSYSTEM_HASMODETREECHILDRENIF_H_
#include "ModeTreeChildIF.h"
class HasModeTreeChildrenIF {
public:
virtual ~HasModeTreeChildrenIF() = default;
virtual ReturnValue_t registerChild(const ModeTreeChildIF& child) = 0;
virtual MessageQueueId_t getCommandQueue() const = 0;
};
#endif // FSFW_SUBSYSTEM_HASMODETREECHILDRENIF_H_

View File

@ -0,0 +1,15 @@
#ifndef FSFW_SUBSYSTEM_MODETREECHILDIF_H_
#define FSFW_SUBSYSTEM_MODETREECHILDIF_H_
#include <fsfw/health/HasHealthIF.h>
#include <fsfw/modes/HasModesIF.h>
class ModeTreeChildIF {
public:
virtual ~ModeTreeChildIF() = default;
virtual object_id_t getObjectId() const = 0;
virtual const HasHealthIF* getOptHealthIF() const = 0;
virtual const HasModesIF& getModeIF() const = 0;
};
#endif /* FSFW_SUBSYSTEM_MODETREECHILDIF_H_ */

View File

@ -0,0 +1,13 @@
#ifndef FSFW_SUBSYSTEM_MODES_MODETREECONNECTIONIF_H_
#define FSFW_SUBSYSTEM_MODES_MODETREECONNECTIONIF_H_
#include "fsfw/subsystem/HasModeTreeChildrenIF.h"
class ModeTreeConnectionIF {
public:
virtual ~ModeTreeConnectionIF() = default;
virtual ReturnValue_t connectModeTreeParent(HasModeTreeChildrenIF& parent) = 0;
virtual ModeTreeChildIF& getModeTreeChildIF() = 0;
};
#endif /* FSFW_SRC_FSFW_SUBSYSTEM_MODES_MODETREECONNECTIONIF_H_ */

View File

@ -9,9 +9,9 @@
#include "fsfw/serialize/SerialLinkedListAdapter.h"
#include "fsfw/serialize/SerializeElement.h"
Subsystem::Subsystem(object_id_t setObjectId, object_id_t parent, uint32_t maxNumberOfSequences,
Subsystem::Subsystem(object_id_t setObjectId, uint32_t maxNumberOfSequences,
uint32_t maxNumberOfTables)
: SubsystemBase(setObjectId, parent, 0),
: SubsystemBase(setObjectId, 0),
isInTransition(false),
childrenChangedHealth(false),
currentTargetTable(),
@ -36,6 +36,13 @@ ReturnValue_t Subsystem::checkSequence(HybridIterator<ModeListEntry> iter,
for (; iter.value != nullptr; ++iter) {
if (!existsModeTable(iter->getTableId())) {
#if FSFW_CPP_OSTREAM_ENABLED == 1
using namespace std;
sif::warning << "Subsystem::checkSequence: "
<< "Object " << setfill('0') << hex << "0x" << setw(8) << getObjectId()
<< setw(0) << ": Mode table for mode ID "
<< "0x" << setw(8) << iter->getTableId() << " does not exist" << dec << endl;
#endif
return TABLE_DOES_NOT_EXIST;
} else {
ReturnValue_t result = checkTable(getTable(iter->getTableId()));

View File

@ -66,8 +66,7 @@ class Subsystem : public SubsystemBase, public HasModeSequenceIF {
* @param maxNumberOfSequences
* @param maxNumberOfTables
*/
Subsystem(object_id_t setObjectId, object_id_t parent, uint32_t maxNumberOfSequences,
uint32_t maxNumberOfTables);
Subsystem(object_id_t setObjectId, uint32_t maxNumberOfSequences, uint32_t maxNumberOfTables);
virtual ~Subsystem();
ReturnValue_t addSequence(SequenceEntry sequence);

View File

@ -1,16 +1,17 @@
#include "fsfw/subsystem/SubsystemBase.h"
#include "fsfw/FSFW.h"
#include "fsfw/ipc/QueueFactory.h"
#include "fsfw/objectmanager/ObjectManager.h"
#include "fsfw/serviceinterface/ServiceInterface.h"
#include "fsfw/serviceinterface.h"
#include "fsfw/subsystem/helper.h"
SubsystemBase::SubsystemBase(object_id_t setObjectId, object_id_t parent, Mode_t initialMode,
SubsystemBase::SubsystemBase(object_id_t setObjectId, Mode_t initialMode,
uint16_t commandQueueDepth)
: SystemObject(setObjectId),
mode(initialMode),
healthHelper(this, setObjectId),
modeHelper(this),
parentId(parent) {
modeHelper(this) {
auto mqArgs = MqArgs(setObjectId, static_cast<void*>(this));
commandQueue = QueueFactory::instance()->createMessageQueue(
commandQueueDepth, CommandMessage::MAX_MESSAGE_SIZE, &mqArgs);
@ -18,36 +19,6 @@ SubsystemBase::SubsystemBase(object_id_t setObjectId, object_id_t parent, Mode_t
SubsystemBase::~SubsystemBase() { QueueFactory::instance()->deleteMessageQueue(commandQueue); }
ReturnValue_t SubsystemBase::registerChild(object_id_t objectId) {
ChildInfo info;
HasModesIF* child = ObjectManager::instance()->get<HasModesIF>(objectId);
// This is a rather ugly hack to have the changedHealth info for all
// children available.
HasHealthIF* healthChild = ObjectManager::instance()->get<HasHealthIF>(objectId);
if (child == nullptr) {
if (healthChild == nullptr) {
return CHILD_DOESNT_HAVE_MODES;
} else {
info.commandQueue = healthChild->getCommandQueue();
info.mode = MODE_OFF;
}
} else {
// intentional to force an initial command during system startup
info.commandQueue = child->getCommandQueue();
info.mode = HasModesIF::MODE_UNDEFINED;
}
info.submode = SUBMODE_NONE;
info.healthChanged = false;
auto resultPair = childrenMap.emplace(objectId, info);
if (not resultPair.second) {
return COULD_NOT_INSERT_CHILD;
}
return returnvalue::OK;
}
ReturnValue_t SubsystemBase::checkStateAgainstTable(HybridIterator<ModeListEntry> tableIter,
Submode_t targetSubmode) {
std::map<object_id_t, ChildInfo>::iterator childIter;
@ -87,7 +58,8 @@ void SubsystemBase::executeTable(HybridIterator<ModeListEntry> tableIter, Submod
if ((iter = childrenMap.find(object)) == childrenMap.end()) {
// illegal table entry, should only happen due to misconfigured mode table
#if FSFW_CPP_OSTREAM_ENABLED == 1
sif::debug << std::hex << getObjectId() << ": invalid mode table entry" << std::endl;
sif::debug << std::hex << SystemObject::getObjectId() << ": invalid mode table entry"
<< std::endl;
#endif
continue;
}
@ -158,36 +130,15 @@ ReturnValue_t SubsystemBase::updateChildChangedHealth(MessageQueueId_t queue, bo
MessageQueueId_t SubsystemBase::getCommandQueue() const { return commandQueue->getId(); }
ReturnValue_t SubsystemBase::initialize() {
MessageQueueId_t parentQueue = MessageQueueIF::NO_QUEUE;
ReturnValue_t result = SystemObject::initialize();
ReturnValue_t result = modeHelper.initialize();
if (result != returnvalue::OK) {
return result;
}
if (parentId != objects::NO_OBJECT) {
SubsystemBase* parent = ObjectManager::instance()->get<SubsystemBase>(parentId);
if (parent == nullptr) {
return returnvalue::FAILED;
}
parentQueue = parent->getCommandQueue();
parent->registerChild(getObjectId());
}
result = healthHelper.initialize(parentQueue);
result = healthHelper.initialize();
if (result != returnvalue::OK) {
return result;
}
result = modeHelper.initialize(parentQueue);
if (result != returnvalue::OK) {
return result;
}
return returnvalue::OK;
return SystemObject::initialize();
}
ReturnValue_t SubsystemBase::performOperation(uint8_t opCode) {
@ -240,8 +191,14 @@ ReturnValue_t SubsystemBase::handleModeReply(CommandMessage* message) {
}
ReturnValue_t SubsystemBase::checkTable(HybridIterator<ModeListEntry> tableIter) {
for (; tableIter.value != NULL; ++tableIter) {
for (; tableIter.value != nullptr; ++tableIter) {
if (childrenMap.find(tableIter.value->getObject()) == childrenMap.end()) {
#if FSFW_CPP_OSTREAM_ENABLED == 1
using namespace std;
sif::warning << "SubsystemBase::checkTable: Could not find Object " << setfill('0') << hex
<< "0x" << setw(8) << tableIter.value->getObject() << " in object " << setw(8)
<< setw(0) << "0x" << setw(8) << SystemObject::getObjectId() << dec << std::endl;
#endif
return TABLE_CONTAINS_INVALID_OBJECT_ID;
}
}
@ -326,4 +283,33 @@ ReturnValue_t SubsystemBase::setHealth(HealthState health) {
HasHealthIF::HealthState SubsystemBase::getHealth() { return healthHelper.getHealth(); }
ReturnValue_t SubsystemBase::connectModeTreeParent(HasModeTreeChildrenIF& parent) {
return modetree::connectModeTreeParent(parent, *this, healthHelper, modeHelper);
}
object_id_t SubsystemBase::getObjectId() const { return SystemObject::getObjectId(); }
void SubsystemBase::modeChanged() {}
ReturnValue_t SubsystemBase::registerChild(const ModeTreeChildIF& child) {
ChildInfo info;
const HasModesIF& modeChild = child.getModeIF();
// intentional to force an initial command during system startup
info.commandQueue = modeChild.getCommandQueue();
info.mode = HasModesIF::MODE_UNDEFINED;
info.submode = SUBMODE_NONE;
info.healthChanged = false;
auto resultPair = childrenMap.emplace(child.getObjectId(), info);
if (not resultPair.second) {
return COULD_NOT_INSERT_CHILD;
}
return returnvalue::OK;
}
const HasHealthIF* SubsystemBase::getOptHealthIF() const { return this; }
const HasModesIF& SubsystemBase::getModeIF() const { return *this; }
ModeTreeChildIF& SubsystemBase::getModeTreeChildIF() { return *this; }

View File

@ -3,14 +3,16 @@
#include <map>
#include "../container/HybridIterator.h"
#include "../health/HasHealthIF.h"
#include "../health/HealthHelper.h"
#include "../ipc/MessageQueueIF.h"
#include "../modes/HasModesIF.h"
#include "../objectmanager/SystemObject.h"
#include "../returnvalues/returnvalue.h"
#include "../tasks/ExecutableObjectIF.h"
#include "fsfw/container/HybridIterator.h"
#include "fsfw/health/HasHealthIF.h"
#include "fsfw/health/HealthHelper.h"
#include "fsfw/ipc/MessageQueueIF.h"
#include "fsfw/modes/HasModesIF.h"
#include "fsfw/objectmanager/SystemObject.h"
#include "fsfw/returnvalues/returnvalue.h"
#include "fsfw/subsystem/HasModeTreeChildrenIF.h"
#include "fsfw/subsystem/ModeTreeConnectionIF.h"
#include "fsfw/tasks/ExecutableObjectIF.h"
#include "modes/HasModeSequenceIF.h"
/**
@ -27,6 +29,9 @@
class SubsystemBase : public SystemObject,
public HasModesIF,
public HasHealthIF,
public HasModeTreeChildrenIF,
public ModeTreeConnectionIF,
public ModeTreeChildIF,
public ExecutableObjectIF {
public:
static const uint8_t INTERFACE_ID = CLASS_ID::SUBSYSTEM_BASE;
@ -36,32 +41,34 @@ class SubsystemBase : public SystemObject,
static const ReturnValue_t COULD_NOT_INSERT_CHILD = MAKE_RETURN_CODE(0x04);
static const ReturnValue_t TABLE_CONTAINS_INVALID_OBJECT_ID = MAKE_RETURN_CODE(0x05);
SubsystemBase(object_id_t setObjectId, object_id_t parent, Mode_t initialMode = 0,
uint16_t commandQueueDepth = 8);
SubsystemBase(object_id_t setObjectId, Mode_t initialMode = 0, uint16_t commandQueueDepth = 8);
virtual ~SubsystemBase();
virtual MessageQueueId_t getCommandQueue() const override;
ReturnValue_t connectModeTreeParent(HasModeTreeChildrenIF &parent) override;
ModeTreeChildIF &getModeTreeChildIF() override;
/**
* Function to register the child objects.
* Performs a checks if the child does implement HasHealthIF and/or HasModesIF
*
* Also adds them to the internal childrenMap.
* Also adds them to the internal childrenMap.
*
* @param objectId
* @return returnvalue::OK if successful
* CHILD_DOESNT_HAVE_MODES if Child is no HasHealthIF and no HasModesIF
* COULD_NOT_INSERT_CHILD If the Child could not be added to the ChildrenMap
* CHILD_DOESNT_HAVE_MODES if Child is no HasHealthIF and no HasModesIF
* COULD_NOT_INSERT_CHILD If the Child could not be added to the ChildrenMap
*/
ReturnValue_t registerChild(object_id_t objectId);
ReturnValue_t registerChild(const ModeTreeChildIF &child) override;
virtual ReturnValue_t initialize() override;
ReturnValue_t initialize() override;
virtual ReturnValue_t performOperation(uint8_t opCode) override;
ReturnValue_t performOperation(uint8_t opCode) override;
virtual ReturnValue_t setHealth(HealthState health) override;
ReturnValue_t setHealth(HealthState health) override;
virtual HasHealthIF::HealthState getHealth() override;
HasHealthIF::HealthState getHealth() override;
protected:
struct ChildInfo {
@ -88,8 +95,6 @@ class SubsystemBase : public SystemObject,
ModeHelper modeHelper;
const object_id_t parentId;
typedef std::map<object_id_t, ChildInfo> ChildrenMap;
ChildrenMap childrenMap;
@ -136,6 +141,10 @@ class SubsystemBase : public SystemObject,
virtual void getMode(Mode_t *mode, Submode_t *submode) override;
object_id_t getObjectId() const override;
const HasHealthIF *getOptHealthIF() const override;
const HasModesIF &getModeIF() const override;
virtual void setToExternalControl() override;
virtual void announceMode(bool recursive) override;

View File

@ -0,0 +1,13 @@
#include "helper.h"
ReturnValue_t modetree::connectModeTreeParent(HasModeTreeChildrenIF& parent,
const ModeTreeChildIF& child,
HealthHelper& healthHelper, ModeHelper& modeHelper) {
ReturnValue_t result = parent.registerChild(child);
if (result != returnvalue::OK) {
return result;
}
healthHelper.setParentQueue(parent.getCommandQueue());
modeHelper.setParentQueue(parent.getCommandQueue());
return returnvalue::OK;
}

View File

@ -0,0 +1,14 @@
#ifndef FSFW_SUBSYSTEM_HELPER_H_
#define FSFW_SUBSYSTEM_HELPER_H_
#include "HasModeTreeChildrenIF.h"
#include "fsfw/health/HealthHelper.h"
namespace modetree {
ReturnValue_t connectModeTreeParent(HasModeTreeChildrenIF& parent, const ModeTreeChildIF& child,
HealthHelper& healthHelper, ModeHelper& modeHelper);
}
#endif /* FSFW_SRC_FSFW_SUBSYSTEM_HELPER_H_ */

View File

@ -2,19 +2,17 @@
#include <fsfw/objectmanager/ObjectManager.h>
TestAssembly::TestAssembly(object_id_t objectId, object_id_t parentId, object_id_t testDevice0,
object_id_t testDevice1)
: AssemblyBase(objectId, parentId),
deviceHandler0Id(testDevice0),
deviceHandler1Id(testDevice1) {
TestAssembly::TestAssembly(object_id_t objectId, object_id_t parentId, ModeTreeChildIF& testDevice0,
ModeTreeChildIF& testDevice1)
: AssemblyBase(objectId, parentId), deviceHandler0(testDevice0), deviceHandler1(testDevice1) {
ModeListEntry newModeListEntry;
newModeListEntry.setObject(testDevice0);
newModeListEntry.setObject(testDevice0.getObjectId());
newModeListEntry.setMode(MODE_OFF);
newModeListEntry.setSubmode(SUBMODE_NONE);
commandTable.insert(newModeListEntry);
newModeListEntry.setObject(testDevice1);
newModeListEntry.setObject(testDevice1.getObjectId());
newModeListEntry.setMode(MODE_OFF);
newModeListEntry.setSubmode(SUBMODE_NONE);
@ -43,8 +41,8 @@ ReturnValue_t TestAssembly::commandChildren(Mode_t mode, Submode_t submode) {
commandTable[1].setMode(MODE_OFF);
commandTable[1].setSubmode(SUBMODE_NONE);
// We try to prefer 0 here but we try to switch to 1 even if it might fail
if (isDeviceAvailable(deviceHandler0Id)) {
if (childrenMap[deviceHandler0Id].mode == MODE_ON) {
if (isDeviceAvailable(deviceHandler0.getObjectId())) {
if (childrenMap[deviceHandler0.getObjectId()].mode == MODE_ON) {
commandTable[0].setMode(mode);
commandTable[0].setSubmode(SUBMODE_NONE);
} else {
@ -53,7 +51,7 @@ ReturnValue_t TestAssembly::commandChildren(Mode_t mode, Submode_t submode) {
result = NEED_SECOND_STEP;
}
} else {
if (childrenMap[deviceHandler1Id].mode == MODE_ON) {
if (childrenMap[deviceHandler1.getObjectId()].mode == MODE_ON) {
commandTable[1].setMode(mode);
commandTable[1].setSubmode(SUBMODE_NONE);
} else {
@ -64,7 +62,7 @@ ReturnValue_t TestAssembly::commandChildren(Mode_t mode, Submode_t submode) {
}
} else {
// Dual Mode Normal
if (childrenMap[deviceHandler0Id].mode == MODE_ON) {
if (childrenMap[deviceHandler0.getObjectId()].mode == MODE_ON) {
commandTable[0].setMode(mode);
commandTable[0].setSubmode(SUBMODE_NONE);
} else {
@ -72,7 +70,7 @@ ReturnValue_t TestAssembly::commandChildren(Mode_t mode, Submode_t submode) {
commandTable[0].setSubmode(SUBMODE_NONE);
result = NEED_SECOND_STEP;
}
if (childrenMap[deviceHandler1Id].mode == MODE_ON) {
if (childrenMap[deviceHandler1.getObjectId()].mode == MODE_ON) {
commandTable[1].setMode(mode);
commandTable[1].setSubmode(SUBMODE_NONE);
} else {
@ -89,7 +87,7 @@ ReturnValue_t TestAssembly::commandChildren(Mode_t mode, Submode_t submode) {
commandTable[1].setMode(MODE_OFF);
commandTable[1].setSubmode(SUBMODE_NONE);
// We try to prefer 0 here but we try to switch to 1 even if it might fail
if (isDeviceAvailable(deviceHandler0Id)) {
if (isDeviceAvailable(deviceHandler0.getObjectId())) {
commandTable[0].setMode(MODE_ON);
commandTable[0].setSubmode(SUBMODE_NONE);
} else {
@ -133,23 +131,14 @@ ReturnValue_t TestAssembly::initialize() {
if (result != returnvalue::OK) {
return result;
}
handler0 = ObjectManager::instance()->get<TestDevice>(deviceHandler0Id);
handler1 = ObjectManager::instance()->get<TestDevice>(deviceHandler1Id);
auto* handler0 = ObjectManager::instance()->get<TestDevice>(deviceHandler0.getObjectId());
auto* handler1 = ObjectManager::instance()->get<TestDevice>(deviceHandler1.getObjectId());
if ((handler0 == nullptr) or (handler1 == nullptr)) {
return returnvalue::FAILED;
}
handler0->setParentQueue(this->getCommandQueue());
handler1->setParentQueue(this->getCommandQueue());
result = registerChild(deviceHandler0Id);
if (result != returnvalue::OK) {
return result;
}
result = registerChild(deviceHandler1Id);
if (result != returnvalue::OK) {
return result;
}
handler0->connectModeTreeParent(*this);
handler1->connectModeTreeParent(*this);
return result;
}

View File

@ -7,8 +7,8 @@
class TestAssembly : public AssemblyBase {
public:
TestAssembly(object_id_t objectId, object_id_t parentId, object_id_t testDevice0,
object_id_t testDevice1);
TestAssembly(object_id_t objectId, object_id_t parentId, ModeTreeChildIF& testDevice0,
ModeTreeChildIF& testDevice1);
virtual ~TestAssembly();
ReturnValue_t initialize() override;
@ -41,10 +41,8 @@ class TestAssembly : public AssemblyBase {
private:
FixedArrayList<ModeListEntry, 2> commandTable;
object_id_t deviceHandler0Id = 0;
object_id_t deviceHandler1Id = 0;
TestDevice* handler0 = nullptr;
TestDevice* handler1 = nullptr;
ModeTreeChildIF& deviceHandler0;
ModeTreeChildIF& deviceHandler1;
bool isDeviceAvailable(object_id_t object);
};

View File

@ -4,8 +4,8 @@
#include <fsfw/objectmanager/ObjectManager.h>
#include <fsfw/serviceinterface/ServiceInterface.h>
TestController::TestController(object_id_t objectId, object_id_t parentId, size_t commandQueueDepth)
: ExtendedControllerBase(objectId, parentId, commandQueueDepth) {}
TestController::TestController(object_id_t objectId, size_t commandQueueDepth)
: ExtendedControllerBase(objectId, commandQueueDepth) {}
TestController::~TestController() {}

View File

@ -7,7 +7,7 @@
class TestController : public ExtendedControllerBase {
public:
TestController(object_id_t objectId, object_id_t parentId, size_t commandQueueDepth = 10);
TestController(object_id_t objectId, size_t commandQueueDepth = 10);
virtual ~TestController();
protected: