eive/fsfw
eive/fsfw
15
1
Fork 0
forked from fsfw/fsfw
Code Issues Pull Requests Releases 1 Wiki Activity

mode service fixes

Browse Source
This commit is contained in:
Robin Müller 2023-02-08 01:20:28 +01:00
parent ab86599db3
commit 8014e4adf9
No known key found for this signature in database
GPG Key ID: 11D4952C8CCEF814
3 changed files with 52 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
src/fsfw/modes/ModeMessage.cpp
View File

@ -24,3 +24,19 @@ 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);
}
void ModeMessage::setCmdModeMessage(CommandMessage& message, Mode_t mode, Submode_t submode) {
setModeMessage(&message, CMD_MODE_COMMAND, mode, submode);
}
void ModeMessage::setModeReadMessage(CommandMessage& message) { message.setCommand(CMD_MODE_READ); }

3
src/fsfw/modes/ModeMessage.h
View File

@ -45,6 +45,9 @@ class ModeMessage {
static void setModeMessage(CommandMessage* message, Command_t command, Mode_t mode,
Submode_t submode);
static void setCmdModeMessage(CommandMessage& message, Mode_t mode, Submode_t submode);
static void setModeAnnounceMessage(CommandMessage& message, bool recursive);
static void setModeReadMessage(CommandMessage& message);
static void setCantReachMode(CommandMessage* message, ReturnValue_t reason);
static void clear(CommandMessage* message);
};

44
src/fsfw/pus/CService200ModeCommanding.cpp
View File

@ -53,16 +53,36 @@ 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;
}
switch (subservice) {
case (Subservice::COMMAND_MODE_COMMAND): {
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());
return result;
ModeMessage::setModeMessage(message, ModeMessage::CMD_MODE_COMMAND,
modeCommandPacket.getMode(), modeCommandPacket.getSubmode());
return returnvalue::OK;
}
case (Subservice::COMMAND_MODE_ANNCOUNCE):
case (Subservice::COMMAND_MODE_ANNOUNCE_RECURSIVELY): {
bool recursive = true;
if (subservice == Subservice::COMMAND_MODE_ANNCOUNCE) {
recursive = false;
}
ModeMessage::setModeAnnounceMessage(*message, recursive);
return EXECUTION_COMPLETE;
}
case (Subservice::COMMAND_MODE_READ): {
ModeMessage::setModeReadMessage(*message);
return returnvalue::OK;
}
default: {
return CommandingServiceBase::INVALID_SUBSERVICE;
}
}
}
ReturnValue_t CService200ModeCommanding::handleReply(const CommandMessage *reply,
@ -73,8 +93,10 @@ ReturnValue_t CService200ModeCommanding::handleReply(const CommandMessage *reply
ReturnValue_t result = returnvalue::FAILED;
switch (replyId) {
case (ModeMessage::REPLY_MODE_REPLY): {
result = prepareModeReply(reply, objectId);
break;
if (previousCommand != ModeMessage::CMD_MODE_COMMAND) {
return prepareModeReply(reply, objectId);
}
return returnvalue::OK;
}
case (ModeMessage::REPLY_WRONG_MODE_REPLY): {
result = prepareWrongModeReply(reply, objectId);