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

mode service fixes

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

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

@ -38,3 +38,5 @@ void ModeMessage::setModeAnnounceMessage(CommandMessage& message, bool recursive
void ModeMessage::setCmdModeModeMessage(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); }

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

@ -46,6 +46,7 @@ class ModeMessage {
static void setCmdModeModeMessage(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);
};

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

@ -54,27 +54,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) {
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;
}
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());
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;
}
}
return result;
}
ReturnValue_t CService200ModeCommanding::handleReply(const CommandMessage *reply,
@ -85,8 +94,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);