2023-02-01 17:07:38 +01:00
|
|
|
#include <fsfw/events/EventManagerIF.h>
|
2023-02-01 17:33:24 +01:00
|
|
|
#include <fsfw/pus/CServiceHealthCommanding.h>
|
2023-02-01 17:07:38 +01:00
|
|
|
|
2021-07-13 20:58:45 +02:00
|
|
|
#include "fsfw/health/HasHealthIF.h"
|
|
|
|
#include "fsfw/health/HealthMessage.h"
|
2022-02-02 10:29:30 +01:00
|
|
|
#include "fsfw/objectmanager/ObjectManager.h"
|
|
|
|
#include "fsfw/pus/servicepackets/Service201Packets.h"
|
|
|
|
#include "fsfw/serviceinterface/ServiceInterface.h"
|
2021-06-05 19:52:38 +02:00
|
|
|
|
2023-02-01 17:33:24 +01:00
|
|
|
CServiceHealthCommanding::CServiceHealthCommanding(HealthServiceCfg args)
|
|
|
|
: CommandingServiceBase(args.objectId, args.apid, "PUS 201 Health MGMT", args.service,
|
|
|
|
args.numParallelCommands, args.commandTimeoutSeconds),
|
2023-02-23 12:44:42 +01:00
|
|
|
healthTableId(args.table),
|
2023-02-01 17:33:24 +01:00
|
|
|
maxNumHealthInfoPerCycle(args.maxNumHealthInfoPerCycle) {}
|
2020-12-01 14:59:35 +01:00
|
|
|
|
2023-02-23 12:44:42 +01:00
|
|
|
ReturnValue_t CServiceHealthCommanding::initialize() {
|
|
|
|
ReturnValue_t result = CommandingServiceBase::initialize();
|
|
|
|
if (result != returnvalue::OK) {
|
|
|
|
return result;
|
|
|
|
}
|
|
|
|
|
|
|
|
healthTable = ObjectManager::instance()->get<HealthTable>(healthTableId);
|
2023-02-23 13:38:24 +01:00
|
|
|
if (healthTable == nullptr) {
|
2023-02-23 12:44:42 +01:00
|
|
|
return returnvalue::FAILED;
|
|
|
|
}
|
|
|
|
|
|
|
|
return returnvalue::OK;
|
|
|
|
}
|
|
|
|
|
2023-02-01 17:33:24 +01:00
|
|
|
ReturnValue_t CServiceHealthCommanding::isValidSubservice(uint8_t subservice) {
|
2022-02-02 10:29:30 +01:00
|
|
|
switch (subservice) {
|
|
|
|
case (Subservice::COMMAND_SET_HEALTH):
|
|
|
|
case (Subservice::COMMAND_ANNOUNCE_HEALTH):
|
|
|
|
case (Subservice::COMMAND_ANNOUNCE_HEALTH_ALL):
|
2022-08-16 01:08:26 +02:00
|
|
|
return returnvalue::OK;
|
2022-02-02 10:29:30 +01:00
|
|
|
default:
|
2021-01-03 14:16:52 +01:00
|
|
|
#if FSFW_CPP_OSTREAM_ENABLED == 1
|
2022-02-02 10:29:30 +01:00
|
|
|
sif::error << "Invalid Subservice" << std::endl;
|
2021-01-03 13:58:18 +01:00
|
|
|
#endif
|
2022-02-02 10:29:30 +01:00
|
|
|
return AcceptsTelecommandsIF::INVALID_SUBSERVICE;
|
|
|
|
}
|
2020-12-01 14:59:35 +01:00
|
|
|
}
|
|
|
|
|
2023-02-01 17:33:24 +01:00
|
|
|
ReturnValue_t CServiceHealthCommanding::getMessageQueueAndObject(uint8_t subservice,
|
|
|
|
const uint8_t *tcData,
|
|
|
|
size_t tcDataLen,
|
|
|
|
MessageQueueId_t *id,
|
|
|
|
object_id_t *objectId) {
|
|
|
|
switch (subservice) {
|
|
|
|
case (Subservice::COMMAND_SET_HEALTH):
|
|
|
|
case (Subservice::COMMAND_ANNOUNCE_HEALTH): {
|
|
|
|
if (tcDataLen < sizeof(object_id_t)) {
|
|
|
|
return CommandingServiceBase::INVALID_TC;
|
|
|
|
}
|
|
|
|
SerializeAdapter::deSerialize(objectId, &tcData, &tcDataLen, SerializeIF::Endianness::BIG);
|
2020-12-01 14:59:35 +01:00
|
|
|
|
2023-02-01 17:33:24 +01:00
|
|
|
return checkInterfaceAndAcquireMessageQueue(id, objectId);
|
|
|
|
}
|
|
|
|
case (Subservice::COMMAND_ANNOUNCE_HEALTH_ALL): {
|
|
|
|
return returnvalue::OK;
|
|
|
|
}
|
|
|
|
default: {
|
|
|
|
return returnvalue::FAILED;
|
|
|
|
}
|
2023-02-01 17:07:38 +01:00
|
|
|
}
|
2020-12-01 14:59:35 +01:00
|
|
|
}
|
|
|
|
|
2023-02-01 17:33:24 +01:00
|
|
|
ReturnValue_t CServiceHealthCommanding::checkInterfaceAndAcquireMessageQueue(
|
2022-05-09 10:47:23 +02:00
|
|
|
MessageQueueId_t *messageQueueToSet, const object_id_t *objectId) {
|
|
|
|
auto *destination = ObjectManager::instance()->get<HasHealthIF>(*objectId);
|
2022-02-02 10:29:30 +01:00
|
|
|
if (destination == nullptr) {
|
|
|
|
return CommandingServiceBase::INVALID_OBJECT;
|
|
|
|
}
|
2020-12-01 14:59:35 +01:00
|
|
|
|
2022-02-02 10:29:30 +01:00
|
|
|
*messageQueueToSet = destination->getCommandQueue();
|
2022-08-16 01:08:26 +02:00
|
|
|
return returnvalue::OK;
|
2020-12-01 14:59:35 +01:00
|
|
|
}
|
|
|
|
|
2023-02-01 17:33:24 +01:00
|
|
|
ReturnValue_t CServiceHealthCommanding::prepareCommand(CommandMessage *message, uint8_t subservice,
|
|
|
|
const uint8_t *tcData, size_t tcDataLen,
|
|
|
|
uint32_t *state, object_id_t objectId) {
|
2022-08-16 01:08:26 +02:00
|
|
|
ReturnValue_t result = returnvalue::OK;
|
2022-02-02 10:29:30 +01:00
|
|
|
switch (subservice) {
|
|
|
|
case (Subservice::COMMAND_SET_HEALTH): {
|
|
|
|
HealthSetCommand healthCommand;
|
|
|
|
result = healthCommand.deSerialize(&tcData, &tcDataLen, SerializeIF::Endianness::BIG);
|
2022-08-16 01:08:26 +02:00
|
|
|
if (result != returnvalue::OK) {
|
2020-12-01 14:59:35 +01:00
|
|
|
break;
|
2022-02-02 10:29:30 +01:00
|
|
|
}
|
|
|
|
HealthMessage::setHealthMessage(message, HealthMessage::HEALTH_SET,
|
|
|
|
healthCommand.getHealth());
|
|
|
|
break;
|
2020-12-01 14:59:35 +01:00
|
|
|
}
|
2022-02-02 10:29:30 +01:00
|
|
|
case (Subservice::COMMAND_ANNOUNCE_HEALTH): {
|
|
|
|
HealthMessage::setHealthMessage(message, HealthMessage::HEALTH_ANNOUNCE);
|
|
|
|
break;
|
2020-12-01 14:59:35 +01:00
|
|
|
}
|
2022-02-02 10:29:30 +01:00
|
|
|
case (Subservice::COMMAND_ANNOUNCE_HEALTH_ALL): {
|
2023-02-01 17:07:38 +01:00
|
|
|
ReturnValue_t result = iterateHealthTable(true);
|
2023-02-01 17:33:24 +01:00
|
|
|
if (result == returnvalue::OK) {
|
|
|
|
reportAllHealth = true;
|
|
|
|
return EXECUTION_COMPLETE;
|
|
|
|
}
|
|
|
|
return result;
|
2020-12-01 14:59:35 +01:00
|
|
|
}
|
2022-05-09 10:47:23 +02:00
|
|
|
default: {
|
|
|
|
// Should never happen, subservice was already checked
|
2022-08-16 01:08:26 +02:00
|
|
|
result = returnvalue::FAILED;
|
2022-05-09 10:47:23 +02:00
|
|
|
}
|
2022-02-02 10:29:30 +01:00
|
|
|
}
|
|
|
|
return result;
|
2020-12-01 14:59:35 +01:00
|
|
|
}
|
|
|
|
|
2023-02-01 17:33:24 +01:00
|
|
|
ReturnValue_t CServiceHealthCommanding::handleReply(const CommandMessage *reply,
|
|
|
|
Command_t previousCommand, uint32_t *state,
|
|
|
|
CommandMessage *optionalNextCommand,
|
|
|
|
object_id_t objectId, bool *isStep) {
|
2022-02-02 10:29:30 +01:00
|
|
|
Command_t replyId = reply->getCommand();
|
|
|
|
if (replyId == HealthMessage::REPLY_HEALTH_SET) {
|
|
|
|
return EXECUTION_COMPLETE;
|
|
|
|
} else if (replyId == CommandMessageIF::REPLY_REJECTED) {
|
|
|
|
return reply->getReplyRejectedReason();
|
|
|
|
}
|
|
|
|
return CommandingServiceBase::INVALID_REPLY;
|
2020-12-01 14:59:35 +01:00
|
|
|
}
|
|
|
|
|
2023-02-01 17:33:24 +01:00
|
|
|
void CServiceHealthCommanding::doPeriodicOperation() {
|
|
|
|
if (reportAllHealth) {
|
|
|
|
for (uint8_t i = 0; i < maxNumHealthInfoPerCycle; i++) {
|
2023-02-01 19:58:27 +01:00
|
|
|
ReturnValue_t result = iterateHealthTable(false);
|
2023-02-01 17:33:24 +01:00
|
|
|
if (result != returnvalue::OK) {
|
|
|
|
reportAllHealth = false;
|
|
|
|
break;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
2020-12-01 15:05:54 +01:00
|
|
|
// Not used for now, health state already reported by event
|
2023-02-01 17:33:24 +01:00
|
|
|
[[maybe_unused]] ReturnValue_t CServiceHealthCommanding::prepareHealthSetReply(
|
2022-05-13 13:21:01 +02:00
|
|
|
const CommandMessage *reply) {
|
2022-05-09 10:47:23 +02:00
|
|
|
auto health = static_cast<uint8_t>(HealthMessage::getHealth(reply));
|
|
|
|
auto oldHealth = static_cast<uint8_t>(HealthMessage::getOldHealth(reply));
|
2022-02-02 10:29:30 +01:00
|
|
|
HealthSetReply healthSetReply(health, oldHealth);
|
2022-07-26 13:59:09 +02:00
|
|
|
return sendTmPacket(Subservice::REPLY_HEALTH_SET, healthSetReply);
|
2020-12-01 14:59:35 +01:00
|
|
|
}
|
2023-02-01 17:07:38 +01:00
|
|
|
|
2023-02-01 17:33:24 +01:00
|
|
|
ReturnValue_t CServiceHealthCommanding::iterateHealthTable(bool reset) {
|
2023-02-01 17:07:38 +01:00
|
|
|
std::pair<object_id_t, HasHealthIF::HealthState> pair;
|
|
|
|
|
2023-02-23 12:44:42 +01:00
|
|
|
ReturnValue_t result = healthTable->iterate(&pair, reset);
|
2023-02-01 17:07:38 +01:00
|
|
|
if (result != returnvalue::OK) {
|
|
|
|
return result;
|
|
|
|
} else {
|
|
|
|
EventManagerIF::triggerEvent(pair.first, HasHealthIF::HEALTH_INFO, pair.second, pair.second);
|
|
|
|
return returnvalue::OK;
|
|
|
|
}
|
|
|
|
}
|