fsfw/tmtcservices/CommandingServiceBase.h
2016-06-15 23:48:49 +02:00

487 lines
16 KiB
C++

#ifndef COMMANDINGSERVICEBASE_H_
#define COMMANDINGSERVICEBASE_H_
#include <framework/container/FixedMap.h>
#include <framework/container/FIFO.h>
#include <framework/ipc/CommandMessage.h>
#include <framework/ipc/MessageQueue.h>
#include <framework/objectmanager/ObjectManagerIF.h>
#include <framework/objectmanager/SystemObject.h>
#include <framework/serialize/SerializeAdapter.h>
#include <framework/storagemanager/StorageManagerIF.h>
#include <framework/tasks/ExecutableObjectIF.h>
#include <framework/tcdistribution/PUSDistributorIF.h>
#include <framework/tmtcpacket/pus/TcPacketStored.h>
#include <framework/tmtcpacket/pus/TmPacketStored.h>
#include <framework/tmtcservices/AcceptsTelecommandsIF.h>
#include <framework/tmtcservices/AcceptsTelemetryIF.h>
#include <framework/tmtcservices/TmTcMessage.h>
#include <framework/tmtcservices/VerificationReporter.h>
template<typename STATE_T>
class CommandingServiceBase: public SystemObject,
public AcceptsTelecommandsIF,
public ExecutableObjectIF,
public HasReturnvaluesIF {
public:
static const uint8_t INTERFACE_ID = COMMAND_SERVICE_BASE;
static const ReturnValue_t EXECUTION_COMPLETE = MAKE_RETURN_CODE(1);
static const ReturnValue_t NO_STEP_MESSAGE = MAKE_RETURN_CODE(2);
static const ReturnValue_t OBJECT_BUSY = MAKE_RETURN_CODE(3);
static const ReturnValue_t BUSY = MAKE_RETURN_CODE(4);
static const ReturnValue_t INVALID_TC = MAKE_RETURN_CODE(5);
static const ReturnValue_t INVALID_OBJECT = MAKE_RETURN_CODE(6);
static const ReturnValue_t INVALID_REPLY = MAKE_RETURN_CODE(7);
CommandingServiceBase(object_id_t setObjectId, uint16_t apid,
uint8_t service, uint8_t numberOfParallelCommands,
uint16_t commandTimeout_seconds, size_t queueDepth = 20);
virtual ~CommandingServiceBase();
virtual ReturnValue_t performOperation();
virtual uint16_t getIdentifier();
virtual MessageQueueId_t getRequestQueue();
virtual ReturnValue_t initialize();
protected:
struct CommandInfo {
struct tcInfo {
uint8_t ackFlags;
uint16_t tcPacketId;
uint16_t tcSequenceControl;
} tcInfo;
uint32_t uptimeOfStart;
uint8_t step;
uint8_t subservice;
STATE_T state;
Command_t command;
object_id_t objectId;
FIFO<store_address_t, 3> fifo;
};
const uint16_t apid;
const uint8_t service;
const uint16_t timeout_seconds;
uint8_t tmPacketCounter;
StorageManagerIF *IPCStore;
StorageManagerIF *TCStore;
MessageQueue commandQueue;
MessageQueue requestQueue;
VerificationReporter verificationReporter;
FixedMap<MessageQueueId_t, CommandInfo> commandMap;
uint32_t failureParameter1; //!< May be set be children to return a more precise failure condition.
uint32_t failureParameter2; //!< May be set be children to return a more precise failure condition.
void sendTmPacket(uint8_t subservice, const uint8_t *data,
uint32_t dataLen);
void sendTmPacket(uint8_t subservice, object_id_t objectId,
const uint8_t *data, uint32_t dataLen);
void sendTmPacket(uint8_t subservice, SerializeIF* content);
virtual ReturnValue_t isValidSubservice(uint8_t subservice) = 0;
virtual ReturnValue_t prepareCommand(CommandMessage *message,
uint8_t subservice, const uint8_t *tcData, uint32_t tcDataLen,
STATE_T *state, object_id_t objectId) = 0;
virtual ReturnValue_t handleReply(const CommandMessage *reply,
Command_t previousCommand, STATE_T *state,
CommandMessage *optionalNextCommand, object_id_t objectId,
bool *isStep) = 0;
virtual ReturnValue_t getMessageQueueAndObject(uint8_t subservice,
const uint8_t *tcData, uint32_t tcDataLen, MessageQueueId_t *id,
object_id_t *objectId) = 0;
virtual void handleUnrequestedReply(CommandMessage *reply);
virtual void doPeriodicOperation();
private:
void handleCommandQueue();
void handleRequestQueue();
void rejectPacket(uint8_t reportId, TcPacketStored* packet,
ReturnValue_t errorCode);
void acceptPacket(uint8_t reportId, TcPacketStored* packet);
void startExecution(TcPacketStored *storedPacket,
typename FixedMap<MessageQueueId_t, CommandInfo>::Iterator *iter);
void checkAndExecuteFifo(
typename FixedMap<MessageQueueId_t, CommandInfo>::Iterator *iter);
void checkTimeout();
};
template<typename STATE_T>
CommandingServiceBase<STATE_T>::CommandingServiceBase(object_id_t setObjectId,
uint16_t apid, uint8_t service, uint8_t numberOfParallelCommands,
uint16_t commandTimeout_seconds, size_t queueDepth) :
SystemObject(setObjectId), apid(apid), service(service), timeout_seconds(
commandTimeout_seconds), tmPacketCounter(0), IPCStore(NULL), TCStore(
NULL), commandQueue(queueDepth), requestQueue(20), commandMap(
numberOfParallelCommands), failureParameter1(0), failureParameter2(
0) {
}
template<typename STATE_T>
CommandingServiceBase<STATE_T>::~CommandingServiceBase() {
}
template<typename STATE_T>
ReturnValue_t CommandingServiceBase<STATE_T>::performOperation() {
handleCommandQueue();
handleRequestQueue();
checkTimeout();
doPeriodicOperation();
return RETURN_OK;
}
template<typename STATE_T>
uint16_t CommandingServiceBase<STATE_T>::getIdentifier() {
return service;
}
template<typename STATE_T>
MessageQueueId_t CommandingServiceBase<STATE_T>::getRequestQueue() {
return requestQueue.getId();
}
template<typename STATE_T>
ReturnValue_t CommandingServiceBase<STATE_T>::initialize() {
ReturnValue_t result = SystemObject::initialize();
if (result != HasReturnvaluesIF::RETURN_OK) {
return result;
}
AcceptsTelemetryIF* packetForwarding =
objectManager->get<AcceptsTelemetryIF>(
objects::PUS_PACKET_FORWARDING);
PUSDistributorIF* distributor = objectManager->get<PUSDistributorIF>(
objects::PUS_PACKET_DISTRIBUTOR);
if ((packetForwarding == NULL) && (distributor == NULL)) {
return RETURN_FAILED;
}
distributor->registerService(this);
requestQueue.setDefaultDestination(
packetForwarding->getReportReceptionQueue());
IPCStore = objectManager->get<StorageManagerIF>(objects::IPC_STORE);
TCStore = objectManager->get<StorageManagerIF>(objects::TC_STORE);
if ((IPCStore == NULL) || (TCStore == NULL)) {
return RETURN_FAILED;
}
return RETURN_OK;
}
//Whole method works like this, but I don't like it. Leave it anyway.
template<typename STATE_T>
void CommandingServiceBase<STATE_T>::handleCommandQueue() {
CommandMessage reply, nextCommand;
ReturnValue_t result, sendResult = RETURN_OK;
bool isStep = false;
for (result = commandQueue.receiveMessage(&reply); result == RETURN_OK;
result = commandQueue.receiveMessage(&reply)) {
isStep = false;
typename FixedMap<MessageQueueId_t,
CommandingServiceBase<STATE_T>::CommandInfo>::Iterator iter;
if ((iter = commandMap.find(reply.getSender())) == commandMap.end()) {
handleUnrequestedReply(&reply);
continue;
}
nextCommand.setCommand(CommandMessage::CMD_NONE);
result = handleReply(&reply, iter->command, &iter->state, &nextCommand,
iter->objectId, &isStep);
switch (result) {
case EXECUTION_COMPLETE:
case RETURN_OK:
case NO_STEP_MESSAGE:
iter->command = nextCommand.getCommand();
if (nextCommand.getCommand() != CommandMessage::CMD_NONE) {
sendResult = commandQueue.sendMessage(reply.getSender(),
&nextCommand);
}
if (sendResult == RETURN_OK) {
if (isStep) {
if (result != NO_STEP_MESSAGE) {
verificationReporter.sendSuccessReport(
TC_VERIFY::PROGRESS_SUCCESS,
iter->tcInfo.ackFlags, iter->tcInfo.tcPacketId,
iter->tcInfo.tcSequenceControl, ++iter->step);
}
} else {
verificationReporter.sendSuccessReport(
TC_VERIFY::COMPLETION_SUCCESS,
iter->tcInfo.ackFlags, iter->tcInfo.tcPacketId,
iter->tcInfo.tcSequenceControl, 0);
checkAndExecuteFifo(&iter);
}
} else {
if (isStep) {
nextCommand.clearCommandMessage();
verificationReporter.sendFailureReport(
TC_VERIFY::PROGRESS_FAILURE, iter->tcInfo.ackFlags,
iter->tcInfo.tcPacketId,
iter->tcInfo.tcSequenceControl, sendResult,
++iter->step, failureParameter1, failureParameter2);
} else {
nextCommand.clearCommandMessage();
verificationReporter.sendFailureReport(
TC_VERIFY::COMPLETION_FAILURE,
iter->tcInfo.ackFlags, iter->tcInfo.tcPacketId,
iter->tcInfo.tcSequenceControl, sendResult,
0, failureParameter1, failureParameter2);
}
failureParameter1 = 0;
failureParameter2 = 0;
checkAndExecuteFifo(&iter);
}
break;
case INVALID_REPLY:
//might be just an unrequested reply at a bad moment
handleUnrequestedReply(&reply);
break;
default:
if (isStep) {
verificationReporter.sendFailureReport(
TC_VERIFY::PROGRESS_FAILURE, iter->tcInfo.ackFlags,
iter->tcInfo.tcPacketId, iter->tcInfo.tcSequenceControl,
result, ++iter->step, failureParameter1, failureParameter2);
} else {
verificationReporter.sendFailureReport(
TC_VERIFY::COMPLETION_FAILURE, iter->tcInfo.ackFlags,
iter->tcInfo.tcPacketId, iter->tcInfo.tcSequenceControl,
result, 0, failureParameter1, failureParameter2);
}
failureParameter1 = 0;
failureParameter2 = 0;
checkAndExecuteFifo(&iter);
break;
}
}
}
template<typename STATE_T>
void CommandingServiceBase<STATE_T>::handleRequestQueue() {
TmTcMessage message;
ReturnValue_t result;
store_address_t address;
TcPacketStored packet;
MessageQueueId_t queue;
object_id_t objectId;
for (result = requestQueue.receiveMessage(&message); result == RETURN_OK;
result = requestQueue.receiveMessage(&message)) {
address = message.getStorageId();
packet.setStoreAddress(address);
if ((packet.getSubService() == 0)
|| (isValidSubservice(packet.getSubService()) != RETURN_OK)) {
rejectPacket(TC_VERIFY::START_FAILURE, &packet, INVALID_SUBSERVICE);
continue;
}
result = getMessageQueueAndObject(packet.getSubService(),
packet.getApplicationData(), packet.getApplicationDataSize(),
&queue, &objectId);
if (result != HasReturnvaluesIF::RETURN_OK) {
rejectPacket(TC_VERIFY::START_FAILURE, &packet, result);
continue;
}
//is a command already active for the target object?
typename FixedMap<MessageQueueId_t,
CommandingServiceBase<STATE_T>::CommandInfo>::Iterator iter;
iter = commandMap.find(queue);
if (iter != commandMap.end()) {
result = iter->fifo.insert(address);
if (result != RETURN_OK) {
rejectPacket(TC_VERIFY::START_FAILURE, &packet, OBJECT_BUSY);
}
} else {
CommandInfo newInfo; //Info will be set by startExecution if neccessary
newInfo.objectId = objectId;
result = commandMap.insert(queue, newInfo, &iter);
if (result != RETURN_OK) {
rejectPacket(TC_VERIFY::START_FAILURE, &packet, BUSY);
} else {
startExecution(&packet, &iter);
}
}
}
}
template<typename STATE_T>
void CommandingServiceBase<STATE_T>::sendTmPacket(uint8_t subservice,
const uint8_t* data, uint32_t dataLen) {
TmPacketStored tmPacketStored(this->apid, this->service, subservice,
this->tmPacketCounter++, data, dataLen);
TmTcMessage tmMessage(tmPacketStored.getStoreAddress());
if (requestQueue.sendToDefault(&tmMessage) != RETURN_OK) {
tmPacketStored.deletePacket();
}
}
template<typename STATE_T>
void CommandingServiceBase<STATE_T>::sendTmPacket(uint8_t subservice,
object_id_t objectId, const uint8_t *data, uint32_t dataLen) {
uint8_t buffer[sizeof(object_id_t)];
uint8_t* pBuffer = buffer;
uint32_t size = 0;
SerializeAdapter<object_id_t>::serialize(&objectId, &pBuffer, &size,
sizeof(object_id_t), true);
TmPacketStored tmPacketStored(this->apid, this->service, subservice,
this->tmPacketCounter++, data, dataLen, buffer, size);
TmTcMessage tmMessage(tmPacketStored.getStoreAddress());
if (requestQueue.sendToDefault(&tmMessage) != RETURN_OK) {
tmPacketStored.deletePacket();
}
}
template<typename STATE_T>
void CommandingServiceBase<STATE_T>::sendTmPacket(uint8_t subservice,
SerializeIF* content) {
TmPacketStored tmPacketStored(this->apid, this->service, subservice,
this->tmPacketCounter++, content);
TmTcMessage tmMessage(tmPacketStored.getStoreAddress());
if (requestQueue.sendToDefault(&tmMessage) != RETURN_OK) {
tmPacketStored.deletePacket();
}
}
template<typename STATE_T>
void CommandingServiceBase<STATE_T>::startExecution(
TcPacketStored *storedPacket,
typename FixedMap<MessageQueueId_t,
CommandingServiceBase<STATE_T>::CommandInfo>::Iterator *iter) {
ReturnValue_t result, sendResult = RETURN_OK;
CommandMessage message;
(*iter)->subservice = storedPacket->getSubService();
result = prepareCommand(&message, (*iter)->subservice,
storedPacket->getApplicationData(),
storedPacket->getApplicationDataSize(), &(*iter)->state,
(*iter)->objectId);
switch (result) {
case RETURN_OK:
if (message.getCommand() != CommandMessage::CMD_NONE) {
sendResult = commandQueue.sendMessage((*iter).value->first,
&message);
}
if (sendResult == RETURN_OK) {
OSAL::getUptime(&(*iter)->uptimeOfStart);
(*iter)->step = 0;
// (*iter)->state = 0;
(*iter)->subservice = storedPacket->getSubService();
(*iter)->command = message.getCommand();
(*iter)->tcInfo.ackFlags = storedPacket->getAcknowledgeFlags();
(*iter)->tcInfo.tcPacketId = storedPacket->getPacketId();
(*iter)->tcInfo.tcSequenceControl =
storedPacket->getPacketSequenceControl();
acceptPacket(TC_VERIFY::START_SUCCESS, storedPacket);
} else {
message.clearCommandMessage();
rejectPacket(TC_VERIFY::START_FAILURE, storedPacket, sendResult);
checkAndExecuteFifo(iter);
}
break;
case EXECUTION_COMPLETE:
if (message.getCommand() != CommandMessage::CMD_NONE) {
//Fire-and-forget command.
sendResult = commandQueue.sendMessage((*iter).value->first,
&message);
}
if (sendResult == RETURN_OK) {
verificationReporter.sendSuccessReport(TC_VERIFY::START_SUCCESS,
storedPacket);
acceptPacket(TC_VERIFY::COMPLETION_SUCCESS, storedPacket);
checkAndExecuteFifo(iter);
} else {
message.clearCommandMessage();
rejectPacket(TC_VERIFY::START_FAILURE, storedPacket, sendResult);
checkAndExecuteFifo(iter);
}
break;
default:
rejectPacket(TC_VERIFY::START_FAILURE, storedPacket, result);
checkAndExecuteFifo(iter);
break;
}
}
template<typename STATE_T>
void CommandingServiceBase<STATE_T>::rejectPacket(uint8_t report_id,
TcPacketStored* packet, ReturnValue_t error_code) {
verificationReporter.sendFailureReport(report_id, packet, error_code);
packet->deletePacket();
}
template<typename STATE_T>
void CommandingServiceBase<STATE_T>::acceptPacket(uint8_t reportId,
TcPacketStored* packet) {
verificationReporter.sendSuccessReport(reportId, packet);
packet->deletePacket();
}
template<typename STATE_T>
void CommandingServiceBase<STATE_T>::checkAndExecuteFifo(
typename FixedMap<MessageQueueId_t,
CommandingServiceBase<STATE_T>::CommandInfo>::Iterator *iter) {
store_address_t address;
if ((*iter)->fifo.retrieve(&address) != RETURN_OK) {
commandMap.erase(iter);
} else {
TcPacketStored newPacket(address);
startExecution(&newPacket, iter);
}
}
template<typename STATE_T>
void CommandingServiceBase<STATE_T>::handleUnrequestedReply(
CommandMessage* reply) {
reply->clearCommandMessage();
}
template<typename STATE_T>
inline void CommandingServiceBase<STATE_T>::doPeriodicOperation() {
}
template<typename STATE_T>
void CommandingServiceBase<STATE_T>::checkTimeout() {
uint32_t uptime;
OSAL::getUptime(&uptime);
typename FixedMap<MessageQueueId_t,
CommandingServiceBase<STATE_T>::CommandInfo>::Iterator iter;
for (iter = commandMap.begin(); iter != commandMap.end(); ++iter) {
if ((iter->uptimeOfStart + (timeout_seconds * 1000)) < uptime) {
verificationReporter.sendFailureReport(
TC_VERIFY::COMPLETION_FAILURE, iter->tcInfo.ackFlags,
iter->tcInfo.tcPacketId, iter->tcInfo.tcSequenceControl,
TIMEOUT);
checkAndExecuteFifo(&iter);
}
}
}
#endif /* COMMANDINGSERVICEBASE_H_ */