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

Updated FreeRTOS Osal

Browse Source 
CommandingServiceBase is no longer a template
This commit is contained in:
Steffen Gaisser 2019-08-28 14:50:24 +02:00
parent fd782b20c0
commit cd7e47ccbb
42 changed files with 698 additions and 644 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
container/FixedArrayList.h
View File

@ -2,7 +2,9 @@
#define FIXEDARRAYLIST_H_
#include <framework/container/ArrayList.h>
/**
* \ingroup container
*/
template<typename T, uint32_t MAX_SIZE, typename count_t = uint8_t>
class FixedArrayList: public ArrayList<T, count_t> {
private:

4
container/FixedMap.h
View File

@ -5,7 +5,9 @@
#include <framework/returnvalues/HasReturnvaluesIF.h>
#include <utility>
/**
* \ingroup container
*/
template<typename key_t, typename T>
class FixedMap: public SerializeIF {
public:

4
container/FixedOrderedMultimap.h
View File

@ -4,7 +4,9 @@
#include <framework/container/ArrayList.h>
#include <cstring>
#include <set>
/**
* \ingroup container
*/
template<typename key_t, typename T, typename KEY_COMPARE = std::less<key_t>>
class FixedOrderedMultimap {
public:

4
container/SinglyLinkedList.h
View File

@ -3,7 +3,9 @@
#include <stddef.h>
#include <stdint.h>
/**
* \ingroup container
*/
template<typename T>
class LinkedElement {
public:

5
devicehandlers/DeviceHandlerBase.cpp
View File

@ -10,6 +10,7 @@
#include <framework/subsystem/SubsystemBase.h>
#include <framework/thermal/ThermalComponentIF.h>
#include <framework/ipc/QueueFactory.h>
#include <framework/serviceinterface/ServiceInterfaceStream.h>
object_id_t DeviceHandlerBase::powerSwitcherId = 0;
object_id_t DeviceHandlerBase::rawDataReceiverId = 0;
@ -372,7 +373,7 @@ void DeviceHandlerBase::setMode(Mode_t newMode) {
void DeviceHandlerBase::replyReturnvalueToCommand(ReturnValue_t status,
uint32_t parameter) {
//This is actually the reply protocol for raw and misc dh commands.
//This is actually the reply protocol for raw and misc DH commands.
if (status == RETURN_OK) {
CommandMessage reply(CommandMessage::REPLY_COMMAND_OK, 0, parameter);
commandQueue->reply(&reply);
@ -672,7 +673,7 @@ void DeviceHandlerBase::replyRawData(const uint8_t *data, size_t len,
CommandMessage message;
DeviceHandlerMessage::setDeviceHandlerRawReplayMessage(&message,
DeviceHandlerMessage::setDeviceHandlerRawReplyMessage(&message,
getObjectId(), address, isCommand);
// this->DeviceHandlerCommand = CommandMessage::CMD_NONE;

39
devicehandlers/DeviceHandlerBase.h
View File

@ -29,7 +29,15 @@ void setStaticFrameworkObjectIds();
class StorageManagerIF;
/**
* This is the abstract base class for device handlers.
* \defgroup devices Devices
* Contains all devices and the DeviceHandlerBase class.
*/
/**
* \brief This is the abstract base class for device handlers.
*
* Documentation: Dissertation Baetz p.138,139, p.141-149
* SpaceWire Remote Memory Access Protocol (RMAP)
*
* It features handling of @link DeviceHandlerIF::Mode_t Modes @endlink, the RMAP communication and the
* communication with commanding objects.
@ -41,6 +49,9 @@ class StorageManagerIF;
* a default implementation is provided.
*
* Device handler instances should extend this class and implement the abstract functions.
* Components and drivers can send so called cookies which are used for communication
*
* \ingroup devices
*/
class DeviceHandlerBase: public DeviceHandlerIF,
public HasReturnvaluesIF,
@ -68,6 +79,32 @@ public:
virtual MessageQueueId_t getCommandQueue(void) const;
/**
* This function is a core component and is called periodically.
* General sequence:
* If the State is SEND_WRITE:
* 1. Set the cookie state to COOKIE_UNUSED and read the command queue
* 2. Handles Device State Modes by calling doStateMachine().
* This function calls callChildStatemachine() which calls the abstract functions
* doStartUp() and doShutDown()
* 3. Check switch states by calling checkSwitchStates()
* 4. Decrements counter for timeout of replies by calling decrementDeviceReplyMap()
* 5. Performs FDIR check for failures
* 6. Calls hkSwitcher.performOperation()
* 7. If the device mode is MODE_OFF, return RETURN_OK. Otherwise, perform the Action property
* and performs depending on value specified
* by input value counter. The child class tells base class what to do by setting this value.
* - SEND_WRITE: Send data or commands to device by calling doSendWrite()
* Calls abstract funtions buildNomalDeviceCommand()
* or buildTransitionDeviceCommand()
* - GET_WRITE: Get ackknowledgement for sending by calling doGetWrite().
* Calls abstract functions scanForReply() and interpretDeviceReply().
* - SEND_READ: Request reading data from device by calling doSendRead()
* - GET_READ: Access requested reading data by calling doGetRead()
* @param counter Specifies which Action to perform
* @return RETURN_OK for successful execution
*/
virtual ReturnValue_t performOperation(uint8_t counter);
virtual ReturnValue_t initialize();

2
devicehandlers/DeviceHandlerMessage.cpp
View File

@ -56,7 +56,7 @@ object_id_t DeviceHandlerMessage::getDeviceObjectId(
return message->getParameter();
}
void DeviceHandlerMessage::setDeviceHandlerRawReplayMessage(
void DeviceHandlerMessage::setDeviceHandlerRawReplyMessage(
CommandMessage* message, object_id_t deviceObjectid,
store_address_t rawPacketStoreId, bool isCommand) {
if (isCommand) {

2
devicehandlers/DeviceHandlerMessage.h
View File

@ -71,7 +71,7 @@ public:
static void setDeviceHandlerRawCommandMessage(CommandMessage* message,
store_address_t rawPacketStoreId);
static void setDeviceHandlerRawReplayMessage(CommandMessage* message,
static void setDeviceHandlerRawReplyMessage(CommandMessage* message,
object_id_t deviceObjectid, store_address_t rawPacketStoreId,
bool isCommand);

2
events/EventMessage.cpp
View File

@ -51,7 +51,7 @@ EventSeverity_t EventMessage::getSeverity() {
return EVENT::getSeverity(event);
}
void EventMessage::setSeverety(EventSeverity_t severity) {
void EventMessage::setSeverity(EventSeverity_t severity) {
Event event;
memcpy(&event, getData(), sizeof(Event));
event = (event & 0xFF00FFFF) + (severity << 16);

2
events/EventMessage.h
View File

@ -30,7 +30,7 @@ public:
uint8_t getMessageId();
void setMessageId(uint8_t id);
EventSeverity_t getSeverity();
void setSeverety(EventSeverity_t severity);
void setSeverity(EventSeverity_t severity);
EventId_t getEventId();
void setEventId(EventId_t event);
object_id_t getReporter();

1
events/fwSubsystemIdRanges.h
View File

@ -3,6 +3,7 @@
namespace SUBSYSTEM_ID {
enum {
MEMORY = 22,
OBSW = 26,
CDH = 28,
TCS_1 = 59,

6
ipc/MessageQueueIF.h
View File

@ -20,6 +20,10 @@ public:
* No space left for more messages
*/
static const ReturnValue_t FULL = MAKE_RETURN_CODE(2);
/**
* Returned if a reply method was called without partner
*/
static const ReturnValue_t NO_REPLY_PARTNER = MAKE_RETURN_CODE(3);
virtual ~MessageQueueIF() {}
/**
@ -28,6 +32,8 @@ public:
* lastParnter information as destination. If there was no message received yet
* (i.e. lastPartner is zero), an error code is returned.
* @param message A pointer to a previously created message, which is sent.
* \return RETURN_OK if ok
* \return NO_REPLY_PARTNER Should return NO_REPLY_PARTNER if partner was found
*/
virtual ReturnValue_t reply( MessageQueueMessage* message ) = 0;

2
ipc/MessageQueueSenderIF.h
View File

@ -27,7 +27,7 @@ public:
*/
static ReturnValue_t sendMessage(MessageQueueId_t sendTo,
MessageQueueMessage* message, MessageQueueId_t sentFrom =
MessageQueueSenderIF::NO_QUEUE);
MessageQueueSenderIF::NO_QUEUE, bool ignoreFault=false);
private:
MessageQueueSenderIF() {}
};

80
memory/LocalMemory.cpp
View File

@ -1,80 +0,0 @@
#ifdef LEON
#include <bsp_flp/hw_prom/HwProm.h>
#endif
#include <framework/memory/LocalMemory.h>
#include <framework/serialize/SerializeAdapter.h>
#include <framework/ipc/QueueFactory.h>
LocalMemory::LocalMemory(object_id_t setObjectId) :
SystemObject(setObjectId), commandQueue(NULL), memoryHelper(this,
NULL) {
commandQueue = QueueFactory::instance()->createMessageQueue();
}
LocalMemory::~LocalMemory() {
QueueFactory::instance()->deleteMessageQueue(commandQueue);
}
ReturnValue_t LocalMemory::performOperation(uint8_t opCode) {
ReturnValue_t handleResult;
CommandMessage message;
for (ReturnValue_t result = commandQueue->receiveMessage(&message);
result == HasReturnvaluesIF::RETURN_OK;
result = commandQueue->receiveMessage(&message)) {
handleResult = memoryHelper.handleMemoryCommand(&message);
if (handleResult != HasReturnvaluesIF::RETURN_OK) {
message.setToUnknownCommand();
commandQueue->reply(&message);
}
}
return HasReturnvaluesIF::RETURN_OK;
}
ReturnValue_t LocalMemory::handleMemoryLoad(uint32_t address,
const uint8_t* data, uint32_t size, uint8_t** dataPointer) {
ReturnValue_t result = checkWriteAccess(address, size);
if (result == HasReturnvaluesIF::RETURN_OK) {
uint32_t value = 0;
for (uint32_t temp_address = address; temp_address < (address + size);
temp_address += 4, data += 4) {
value = (data[0] << 24) + (data[1] << 16) + (data[2] << 8) + data[3];
*((uint32_t*) temp_address) = value;
}
}
return result;
}
ReturnValue_t LocalMemory::handleMemoryDump(uint32_t address, uint32_t size,
uint8_t** dataPointer, uint8_t* dumpTarget) {
*dataPointer = (uint8_t*) address;
return POINTS_TO_MEMORY;
}
ReturnValue_t LocalMemory::initialize() {
return memoryHelper.initialize(commandQueue);
}
MessageQueueId_t LocalMemory::getCommandQueue() const {
return commandQueue->getId();
}
ReturnValue_t LocalMemory::checkWriteAccess(uint32_t address, uint32_t size) {
if ((address % 4) != 0) {
return UNALIGNED_ACCESS;
}
if ((size % 4) != 0) {
return INVALID_SIZE;
}
#ifdef LEON
if (address < 0x40000000) {
HwProm prom(false);
if (prom.getPromWriteEnabled() != HwProm::WRITE_ENABLED) {
return WRITE_PROTECTED;
}
}
#endif
return HasReturnvaluesIF::RETURN_OK;
}

29
memory/LocalMemory.h
View File

@ -1,29 +0,0 @@
#ifndef LOCALMEMORY_H_
#define LOCALMEMORY_H_
#include <framework/memory/HasMemoryIF.h>
#include <framework/memory/MemoryHelper.h>
#include <framework/objectmanager/SystemObject.h>
#include <framework/tasks/ExecutableObjectIF.h>
#include <framework/ipc/MessageQueueIF.h>
#include <list>
class LocalMemory: public AcceptsMemoryMessagesIF,
public ExecutableObjectIF,
public SystemObject {
private:
MessageQueueIF* commandQueue;
MemoryHelper memoryHelper;
ReturnValue_t checkWriteAccess(uint32_t address, uint32_t size);
public:
LocalMemory(object_id_t setObjectId);
~LocalMemory();
ReturnValue_t performOperation(uint8_t opCode);
ReturnValue_t initialize();
MessageQueueId_t getCommandQueue() const;
ReturnValue_t handleMemoryLoad(uint32_t address, const uint8_t* data,
uint32_t size, uint8_t** dataPointer);
ReturnValue_t handleMemoryDump(uint32_t address, uint32_t size,
uint8_t** dataPointer, uint8_t* dumpTarget);
};
#endif /* LOCALMEMORY_H_ */

1
osal/FreeRTOS/FixedTimeslotTask.cpp
View File

@ -2,6 +2,7 @@
#include "FixedTimeslotTask.h"
uint32_t FixedTimeslotTask::deadlineMissedCount = 0;
const size_t PeriodicTaskIF::MINIMUM_STACK_SIZE = configMINIMAL_STACK_SIZE;
FixedTimeslotTask::FixedTimeslotTask(const char *name, TaskPriority setPriority,
TaskStackSize setStack, TaskPeriod overallPeriod,

41
osal/FreeRTOS/MessageQueue.cpp
View File

@ -5,11 +5,10 @@
// TODO I guess we should have a way of checking if we are in an ISR and then use the "fromISR" versions of all calls
MessageQueue::MessageQueue(size_t message_depth, size_t max_message_size) :
lastPartner(0), defaultDestination(0) {
defaultDestination(0),lastPartner(0) {
handle = xQueueCreate(message_depth, max_message_size);
if (handle == NULL) {
//TODO
;
error << "MessageQueue creation failed" << std::endl;
}
}
@ -32,8 +31,7 @@ ReturnValue_t MessageQueue::reply(MessageQueueMessage* message) {
if (this->lastPartner != 0) {
return sendMessageFrom(this->lastPartner, message, this->getId());
} else {
//TODO: Good returnCode
return HasReturnvaluesIF::RETURN_FAILED;
return NO_REPLY_PARTNER;
}
}
@ -45,8 +43,9 @@ ReturnValue_t MessageQueue::receiveMessage(MessageQueueMessage* message,
}
ReturnValue_t MessageQueue::receiveMessage(MessageQueueMessage* message) {
BaseType_t result = xQueueReceive(handle, message, 0);
BaseType_t result = xQueueReceive(handle,reinterpret_cast<void*>(message->getBuffer()), 0);
if (result == pdPASS){
this->lastPartner = message->getSender();
return HasReturnvaluesIF::RETURN_OK;
} else {
return MessageQueueIF::EMPTY;
@ -75,16 +74,7 @@ void MessageQueue::setDefaultDestination(MessageQueueId_t defaultDestination) {
ReturnValue_t MessageQueue::sendMessageFrom(MessageQueueId_t sendTo,
MessageQueueMessage* message, MessageQueueId_t sentFrom,
bool ignoreFault) {
message->setSender(sentFrom);
BaseType_t result = xQueueSendToBack((void * )sendTo, message, 0);
if (result != pdPASS) {
if (!ignoreFault) {
//TODO errr reporter
}
return MessageQueueIF::FULL;
}
return HasReturnvaluesIF::RETURN_OK;
return sendMessageFromMessageQueue(sendTo,message,sentFrom,ignoreFault);
}
ReturnValue_t MessageQueue::sendToDefaultFrom(MessageQueueMessage* message,
@ -99,4 +89,23 @@ MessageQueueId_t MessageQueue::getDefaultDestination() const {
bool MessageQueue::isDefaultDestinationSet() const {
return 0;
}
ReturnValue_t MessageQueue::sendMessageFromMessageQueue(MessageQueueId_t sendTo,
MessageQueueMessage *message, MessageQueueId_t sentFrom,
bool ignoreFault) {
message->setSender(sentFrom);
BaseType_t result = xQueueSendToBack(reinterpret_cast<void*>(sendTo),reinterpret_cast<const void*>(message->getBuffer()), 0);
if (result != pdPASS) {
if (!ignoreFault) {
InternalErrorReporterIF* internalErrorReporter = objectManager->get<InternalErrorReporterIF>(
objects::INTERNAL_ERROR_REPORTER);
if (internalErrorReporter != NULL) {
internalErrorReporter->queueMessageNotSent();
}
}
return MessageQueueIF::FULL;
}
return HasReturnvaluesIF::RETURN_OK;
}

14
osal/FreeRTOS/MessageQueue.h
View File

@ -28,6 +28,7 @@
* \ingroup message_queue
*/
class MessageQueue : public MessageQueueIF {
friend class MessageQueueSenderIF;
public:
/**
* @brief The constructor initializes and configures the message queue.
@ -136,6 +137,19 @@ public:
MessageQueueId_t getDefaultDestination() const;
bool isDefaultDestinationSet() const;
protected:
/**
* Implementation to be called from any send Call within MessageQueue and MessageQueueSenderIF
* \details This method takes the message provided, adds the sentFrom information and passes
* it on to the destination provided with an operating system call. The OS's return
* value is returned.
* \param sendTo This parameter specifies the message queue id to send the message to.
* \param message This is a pointer to a previously created message, which is sent.
* \param sentFrom The sentFrom information can be set to inject the sender's queue id into the message.
* This variable is set to zero by default.
* \param ignoreFault If set to true, the internal software fault counter is not incremented if queue is full.
*/
static ReturnValue_t sendMessageFromMessageQueue(MessageQueueId_t sendTo,MessageQueueMessage* message, MessageQueueId_t sentFrom = NO_QUEUE,bool ignoreFault=false);
private:
QueueHandle_t handle;
MessageQueueId_t defaultDestination;

5
osal/FreeRTOS/PeriodicTask.cpp
View File

@ -8,7 +8,10 @@ PeriodicTask::PeriodicTask(const char *name, TaskPriority setPriority,
started(false), handle(NULL), period(setPeriod), deadlineMissedFunc(
setDeadlineMissedFunc) {
xTaskCreate(taskEntryPoint, name, setStack, this, setPriority, &handle);
BaseType_t status = xTaskCreate(taskEntryPoint, name, setStack, this, setPriority, &handle);
if(status != pdPASS){
debug << "PeriodicTask Insufficient heap memory remaining. Status: " << status << std::endl;
}
}

5
osal/FreeRTOS/QueueFactory.cpp
View File

@ -7,9 +7,8 @@ QueueFactory* QueueFactory::factoryInstance = NULL;
ReturnValue_t MessageQueueSenderIF::sendMessage(MessageQueueId_t sendTo,
MessageQueueMessage* message, MessageQueueId_t sentFrom) {
return 0;
MessageQueueMessage* message, MessageQueueId_t sentFrom,bool ignoreFault) {
return MessageQueue::sendMessageFromMessageQueue(sendTo,message,sentFrom,ignoreFault);
}
QueueFactory* QueueFactory::instance() {

14
osal/FreeRTOS/README.md Normal file
View File

@ -0,0 +1,14 @@
FreeRTOS Readme
=
## Main.cpp Notices
### Tasks
A FreeRTOS application needs to start
> vTaskStartScheduler()
before creating Tasks.
Keep this in mind for the mission dependent code!
This has to be done before the Task Factory is used.

10
osal/FreeRTOS/TaskFactory.cpp
View File

@ -4,7 +4,7 @@
#include "PeriodicTask.h"
#include "FixedTimeslotTask.h"
//TODO: Different variant than the lazy loading in QueueFactory. What's better and why?
TaskFactory* TaskFactory::factoryInstance = new TaskFactory();
TaskFactory::~TaskFactory() {
@ -13,7 +13,9 @@ TaskFactory::~TaskFactory() {
TaskFactory* TaskFactory::instance() {
return TaskFactory::factoryInstance;
}
/***
* Keep in Mind that you need to call before this vTaskStartScheduler()!
*/
PeriodicTaskIF* TaskFactory::createPeriodicTask(TaskName name_,
TaskPriority taskPriority_, TaskStackSize stackSize_,
TaskPeriod period_,
@ -21,7 +23,9 @@ PeriodicTaskIF* TaskFactory::createPeriodicTask(TaskName name_,
return (PeriodicTaskIF*) (new PeriodicTask(name_, taskPriority_, stackSize_,
period_, deadLineMissedFunction_));
}
/***
* Keep in Mind that you need to call before this vTaskStartScheduler()!
*/
FixedTimeslotTaskIF* TaskFactory::createFixedTimeslotTask(TaskName name_,
TaskPriority taskPriority_, TaskStackSize stackSize_,
TaskPeriod period_,

32
osal/FreeRTOS/main.cpp
View File

@ -1,32 +0,0 @@
//entry point into "bsp"
//TODO This can be done mission dependent and some low level calls before vTaskStartScheduler might be important
//void init(void);
//
//#include <FreeRTOS.h>
//#include <FreeRTOSConfig.h>
//#include "task.h"
//
//
//void initTask(void *parameters) {
// init();
//}
//
//int main(void) {
//
// if ( pdPASS
// != xTaskCreate(initTask, "init", 512, NULL,
// configMAX_PRIORITIES - 1, NULL)) {
// //print_uart0("Could not create task1\r\n");
// }
//
// vTaskStartScheduler();
//
// //Scheduler should never return
//
// //print_uart0("This is bad\n");
//
// for (;;)
// ;
//
// return 0;
//}

3
osal/linux/FixedTimeslotTask.cpp
View File

@ -53,9 +53,6 @@ void FixedTimeslotTask::taskFunctionality() {
if (!started) {
suspend();
}
// A local iterator for the Polling Sequence Table is created to find the start time for the first entry.
std::list<FixedSequenceSlot*>::iterator it = pst.current;
//The start time for the first entry is read.
uint64_t lastWakeTime = getCurrentMonotonicTimeMs();
uint64_t interval = pst.getIntervalToNextSlotMs();

50
osal/linux/MessageQueue.cpp
View File

@ -8,8 +8,7 @@
MessageQueue::MessageQueue(size_t message_depth, size_t max_message_size) :
id(0), lastPartner(0), defaultDestination(NO_QUEUE), internalErrorReporter(
NULL) {
id(0), lastPartner(0), defaultDestination(NO_QUEUE) {
//debug << "MessageQueue::MessageQueue: Creating a queue" << std::endl;
mq_attr attributes;
this->id = 0;
@ -81,8 +80,7 @@ ReturnValue_t MessageQueue::reply(MessageQueueMessage* message) {
if (this->lastPartner != 0) {
return sendMessageFrom(this->lastPartner, message, this->getId());
} else {
//TODO: Good returnCode
return HasReturnvaluesIF::RETURN_FAILED;
return NO_REPLY_PARTNER;
}
}
@ -198,20 +196,41 @@ void MessageQueue::setDefaultDestination(MessageQueueId_t defaultDestination) {
ReturnValue_t MessageQueue::sendMessageFrom(MessageQueueId_t sendTo,
MessageQueueMessage* message, MessageQueueId_t sentFrom,
bool ignoreFault) {
return sendMessageFromMessageQueue(sendTo,message,sentFrom,ignoreFault);
}
ReturnValue_t MessageQueue::sendToDefaultFrom(MessageQueueMessage* message,
MessageQueueId_t sentFrom, bool ignoreFault) {
return sendMessageFrom(defaultDestination, message, sentFrom, ignoreFault);
}
MessageQueueId_t MessageQueue::getDefaultDestination() const {
return this->defaultDestination;
}
bool MessageQueue::isDefaultDestinationSet() const {
return (defaultDestination != NO_QUEUE);
}
uint16_t MessageQueue::queueCounter = 0;
ReturnValue_t MessageQueue::sendMessageFromMessageQueue(MessageQueueId_t sendTo,
MessageQueueMessage *message, MessageQueueId_t sentFrom,
bool ignoreFault) {
message->setSender(sentFrom);
int result = mq_send(sendTo,
reinterpret_cast<const char*>(message->getBuffer()), message->messageSize,0);
//TODO: Check if we're in ISR.
if (result != 0 && !ignoreFault) {
if (internalErrorReporter == NULL) {
internalErrorReporter = objectManager->get<InternalErrorReporterIF>(
if (result != 0) {
if(!ignoreFault){
InternalErrorReporterIF* internalErrorReporter = objectManager->get<InternalErrorReporterIF>(
objects::INTERNAL_ERROR_REPORTER);
}
if (internalErrorReporter != NULL) {
internalErrorReporter->queueMessageNotSent();
}
}
switch(errno){
case EAGAIN:
//The O_NONBLOCK flag was set when opening the queue, or the MQ_NONBLOCK flag was set in its attributes, and the specified queue is full.
@ -241,18 +260,3 @@ ReturnValue_t MessageQueue::sendMessageFrom(MessageQueueId_t sendTo,
}
return HasReturnvaluesIF::RETURN_OK;
}
ReturnValue_t MessageQueue::sendToDefaultFrom(MessageQueueMessage* message,
MessageQueueId_t sentFrom, bool ignoreFault) {
return sendMessageFrom(defaultDestination, message, sentFrom, ignoreFault);
}
MessageQueueId_t MessageQueue::getDefaultDestination() const {
return this->defaultDestination;
}
bool MessageQueue::isDefaultDestinationSet() const {
return (defaultDestination != NO_QUEUE);
}
uint16_t MessageQueue::queueCounter = 0;

24
osal/linux/MessageQueue.h
View File

@ -21,6 +21,7 @@
* \ingroup message_queue
*/
class MessageQueue : public MessageQueueIF {
friend class MessageQueueSenderIF;
public:
/**
* @brief The constructor initializes and configures the message queue.
@ -101,9 +102,6 @@ public:
MessageQueueId_t getId() const;
/**
* \brief With the sendMessage call, a queue message is sent to a receiving queue.
* \details This method takes the message provided, adds the sentFrom information and passes
* it on to the destination provided with an operating system call. The OS's return
* value is returned.
* \param sendTo This parameter specifies the message queue id to send the message to.
* \param message This is a pointer to a previously created message, which is sent.
* \param sentFrom The sentFrom information can be set to inject the sender's queue id into the message.
@ -129,6 +127,19 @@ public:
MessageQueueId_t getDefaultDestination() const;
bool isDefaultDestinationSet() const;
protected:
/**
* Implementation to be called from any send Call within MessageQueue and MessageQueueSenderIF
* \details This method takes the message provided, adds the sentFrom information and passes
* it on to the destination provided with an operating system call. The OS's return
* value is returned.
* \param sendTo This parameter specifies the message queue id to send the message to.
* \param message This is a pointer to a previously created message, which is sent.
* \param sentFrom The sentFrom information can be set to inject the sender's queue id into the message.
* This variable is set to zero by default.
* \param ignoreFault If set to true, the internal software fault counter is not incremented if queue is full.
*/
static ReturnValue_t sendMessageFromMessageQueue(MessageQueueId_t sendTo,MessageQueueMessage* message, MessageQueueId_t sentFrom = NO_QUEUE,bool ignoreFault=false);
private:
/**
* @brief The class stores the queue id it got assigned from the operating system in this attribute.
@ -151,13 +162,6 @@ private:
*/
MessageQueueId_t defaultDestination;
/**
* \brief This attribute stores a reference to the internal error reporter for reporting full queues.
* \details In the event of a full destination queue, the reporter will be notified. The reference is set
* by lazy loading
*/
InternalErrorReporterIF *internalErrorReporter;
/**
* The name of the message queue, stored for unlinking
*/

36
osal/linux/QueueFactory.cpp
View File

@ -9,40 +9,8 @@ QueueFactory* QueueFactory::factoryInstance = NULL;
ReturnValue_t MessageQueueSenderIF::sendMessage(MessageQueueId_t sendTo,
MessageQueueMessage* message, MessageQueueId_t sentFrom) {
message->setSender(sentFrom);
int result = mq_send(sendTo,
reinterpret_cast<const char*>(message->getBuffer()), message->messageSize,0);
//TODO: Check if we're in ISR.
if (result != 0) {
//TODO Translate error
switch(errno){
case EAGAIN:
//The O_NONBLOCK flag was set when opening the queue, or the MQ_NONBLOCK flag was set in its attributes, and the specified queue is full.
return MessageQueueIF::FULL;
case EBADF:
//mq_des doesn't represent a valid message queue descriptor, or mq_des wasn't opened for writing.
error << "MessageQueueSenderIF::sendMessage: Configuration error " << strerror(errno) << " in mq_send mqSendTo: " << sendTo << " sent from " << sentFrom << std::endl;
/*NO BREAK*/
case EINTR:
//The call was interrupted by a signal.
case EINVAL:
/*
* This value indicates one of the following:
* * msg_ptr is NULL.
* * msg_len is negative.
* * msg_prio is greater than MQ_PRIO_MAX.
* * msg_prio is less than 0.
* * MQ_PRIO_RESTRICT is set in the mq_attr of mq_des, and msg_prio is greater than the priority of the calling process.
* */
case EMSGSIZE:
//The msg_len is greater than the msgsize associated with the specified queue.
default:
return HasReturnvaluesIF::RETURN_FAILED;
}
}
return HasReturnvaluesIF::RETURN_OK;
MessageQueueMessage* message, MessageQueueId_t sentFrom,bool ignoreFault) {
return MessageQueue::sendMessageFromMessageQueue(sendTo,message,sentFrom,ignoreFault);
}
QueueFactory* QueueFactory::instance() {

4
osal/rtems/InternalErrorCodes.cpp
View File

@ -14,8 +14,8 @@ ReturnValue_t InternalErrorCodes::translate(uint8_t code) {
return TOO_LITTLE_WORKSPACE;
case INTERNAL_ERROR_WORKSPACE_ALLOCATION:
return WORKSPACE_ALLOCATION;
case INTERNAL_ERROR_INTERRUPT_STACK_TOO_SMALL:
return INTERRUPT_STACK_TOO_SMALL;
// case INTERNAL_ERROR_INTERRUPT_STACK_TOO_SMALL:
// return INTERRUPT_STACK_TOO_SMALL;
case INTERNAL_ERROR_THREAD_EXITTED:
return THREAD_EXITTED;
case INTERNAL_ERROR_INCONSISTENT_MP_INFORMATION:

3
osal/rtems/MessageQueue.cpp
View File

@ -32,8 +32,7 @@ ReturnValue_t MessageQueue::reply(MessageQueueMessage* message) {
if (this->lastPartner != 0) {
return sendMessage(this->lastPartner, message, this->getId());
} else {
//TODO: Good returnCode
return HasReturnvaluesIF::RETURN_FAILED;
return NO_REPLY_PARTNER;
}
}

3
osal/rtems/QueueFactory.cpp
View File

@ -6,7 +6,8 @@ QueueFactory* QueueFactory::factoryInstance = NULL;
ReturnValue_t MessageQueueSenderIF::sendMessage(MessageQueueId_t sendTo,
MessageQueueMessage* message, MessageQueueId_t sentFrom) {
MessageQueueMessage* message, MessageQueueId_t sentFrom,bool ignoreFault) {
//TODO add ignoreFault functionality
message->setSender(sentFrom);
rtems_status_code result = rtems_message_queue_send(sendTo, message->getBuffer(),
message->messageSize);

3
serialize/SerialArrayListAdapter.h
View File

@ -11,6 +11,9 @@
#include <framework/serialize/SerializeIF.h>
#include <utility>
/**
* \ingroup serialize
*/
template<typename T, typename count_t = uint8_t>
class SerialArrayListAdapter : public SerializeIF {
public:

3
serialize/SerialBufferAdapter.h
View File

@ -4,6 +4,9 @@
#include <framework/serialize/SerializeIF.h>
#include <framework/serialize/SerializeAdapter.h>
/**
* \ingroup serialize
*/
template<typename T>
class SerialBufferAdapter: public SerializeIF {
public:

3
serialize/SerialFixedArrayListAdapter.h
View File

@ -4,6 +4,9 @@
#include <framework/container/FixedArrayList.h>
#include <framework/serialize/SerialArrayListAdapter.h>
/**
* \ingroup serialize
*/
template<typename T, uint32_t MAX_SIZE, typename count_t = uint8_t>
class SerialFixedArrayListAdapter : public FixedArrayList<T, MAX_SIZE, count_t>, public SerializeIF {
public:

4
serialize/SerialLinkedListAdapter.h
View File

@ -12,6 +12,10 @@
#include <framework/serialize/SerializeElement.h>
#include <framework/serialize/SerializeIF.h>
//This is where we need the SerializeAdapter!
/**
* \ingroup serialize
*/
template<typename T, typename count_t = uint8_t>
class SerialLinkedListAdapter: public SinglyLinkedList<T>, public SerializeIF {
public:

3
serialize/SerializeAdapter.h
View File

@ -7,6 +7,9 @@
#include <framework/serialize/SerializeIF.h>
#include <string.h>
/**
* \ingroup serialize
*/
template<typename T, int>
class SerializeAdapter_ {
public:

3
serialize/SerializeElement.h
View File

@ -5,6 +5,9 @@
#include <framework/serialize/SerializeAdapter.h>
#include <utility>
/**
* \ingroup serialize
*/
template<typename T>
class SerializeElement : public SerializeIF, public LinkedElement<SerializeIF> {
public:

9
serialize/SerializeIF.h
View File

@ -3,6 +3,15 @@
#include <framework/returnvalues/HasReturnvaluesIF.h>
/**
* \defgroup serialize Serialization
* Contains serialisation services.
*/
/**
* Translation of objects into data streams.
* \ingroup serialize
*/
class SerializeIF {
public:
static const uint8_t INTERFACE_ID = CLASS_ID::SERIALIZE_IF;

2
tmtcpacket/pus/TcPacketStored.cpp
View File

@ -1,5 +1,3 @@
*/
#include <framework/objectmanager/ObjectManagerIF.h>
#include <framework/serviceinterface/ServiceInterfaceStream.h>
#include <framework/tmtcpacket/pus/TcPacketStored.h>

3
tmtcpacket/pus/TmPacketBase.cpp
View File

@ -89,6 +89,9 @@ void TmPacketBase::initializeTmPacket(uint16_t apid, uint8_t service, uint8_t su
//First, set to zero.
memset(&tm_data->data_field, 0, sizeof(tm_data->data_field));
//Set CCSDS_secondary header flag to 0, version number to 001 and ack to 0000
// NOTE: In PUS-C, the PUS Version is 2 and specified for the first 4 bits.
// The other 4 bits of the first byte are the spacecraft time reference status
// To change to PUS-C, set 0b00100000
tm_data->data_field.version_type_ack = 0b00010000;
tm_data->data_field.service_type = service;
tm_data->data_field.service_subtype = subservice;

381
tmtcservices/CommandingServiceBase.cpp Normal file
View File

@ -0,0 +1,381 @@
/*
* CommandingServiceBase.cpp
*
* Created on: 28.08.2019
* Author: gaisser
*/
#include <framework/tmtcservices/CommandingServiceBase.h>
CommandingServiceBase::CommandingServiceBase(object_id_t setObjectId,
uint16_t apid, uint8_t service, uint8_t numberOfParallelCommands,
uint16_t commandTimeout_seconds, object_id_t setPacketSource,
object_id_t setPacketDestination, size_t queueDepth) :
SystemObject(setObjectId), apid(apid), service(service), timeout_seconds(
commandTimeout_seconds), tmPacketCounter(0), IPCStore(NULL), TCStore(
NULL), commandQueue(NULL), requestQueue(NULL), commandMap(
numberOfParallelCommands), failureParameter1(0), failureParameter2(
0), packetSource(setPacketSource), packetDestination(
setPacketDestination),executingTask(NULL) {
commandQueue = QueueFactory::instance()->createMessageQueue(queueDepth);
requestQueue = QueueFactory::instance()->createMessageQueue(queueDepth);
}
CommandingServiceBase::~CommandingServiceBase() {
QueueFactory::instance()->deleteMessageQueue(commandQueue);
QueueFactory::instance()->deleteMessageQueue(requestQueue);
}
ReturnValue_t CommandingServiceBase::performOperation(uint8_t opCode) {
handleCommandQueue();
handleRequestQueue();
checkTimeout();
doPeriodicOperation();
return RETURN_OK;
}
uint16_t CommandingServiceBase::getIdentifier() {
return service;
}
MessageQueueId_t CommandingServiceBase::getRequestQueue() {
return requestQueue->getId();
}
ReturnValue_t CommandingServiceBase::initialize() {
ReturnValue_t result = SystemObject::initialize();
if (result != HasReturnvaluesIF::RETURN_OK) {
return result;
}
AcceptsTelemetryIF* packetForwarding =
objectManager->get<AcceptsTelemetryIF>(packetDestination);
PUSDistributorIF* distributor = objectManager->get<PUSDistributorIF>(
packetSource);
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;
}
void CommandingServiceBase::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::CommandInfo>::Iterator iter;
if (reply.getSender() == MessageQueueIF::NO_QUEUE) {
handleUnrequestedReply(&reply);
continue;
}
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;
}
}
}
void CommandingServiceBase::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::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);
}
}
}
}
void CommandingServiceBase::sendTmPacket(uint8_t subservice,
const uint8_t* data, uint32_t dataLen, const uint8_t* headerData,
uint32_t headerSize) {
TmPacketStored tmPacketStored(this->apid, this->service, subservice,
this->tmPacketCounter, data, dataLen, headerData, headerSize);
ReturnValue_t result = tmPacketStored.sendPacket(
requestQueue->getDefaultDestination(), requestQueue->getId());
if (result == HasReturnvaluesIF::RETURN_OK) {
this->tmPacketCounter++;
}
}
void CommandingServiceBase::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);
ReturnValue_t result = tmPacketStored.sendPacket(
requestQueue->getDefaultDestination(), requestQueue->getId());
if (result == HasReturnvaluesIF::RETURN_OK) {
this->tmPacketCounter++;
}
}
void CommandingServiceBase::sendTmPacket(uint8_t subservice,
SerializeIF* content, SerializeIF* header) {
TmPacketStored tmPacketStored(this->apid, this->service, subservice,
this->tmPacketCounter, content, header);
ReturnValue_t result = tmPacketStored.sendPacket(
requestQueue->getDefaultDestination(), requestQueue->getId());
if (result == HasReturnvaluesIF::RETURN_OK) {
this->tmPacketCounter++;
}
}
void CommandingServiceBase::startExecution(
TcPacketStored *storedPacket,
typename FixedMap<MessageQueueId_t,
CommandingServiceBase::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) {
Clock::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;
}
}
void CommandingServiceBase::rejectPacket(uint8_t report_id,
TcPacketStored* packet, ReturnValue_t error_code) {
verificationReporter.sendFailureReport(report_id, packet, error_code);
packet->deletePacket();
}
void CommandingServiceBase::acceptPacket(uint8_t reportId,
TcPacketStored* packet) {
verificationReporter.sendSuccessReport(reportId, packet);
packet->deletePacket();
}
void CommandingServiceBase::checkAndExecuteFifo(
typename FixedMap<MessageQueueId_t,
CommandingServiceBase::CommandInfo>::Iterator *iter) {
store_address_t address;
if ((*iter)->fifo.retrieve(&address) != RETURN_OK) {
commandMap.erase(iter);
} else {
TcPacketStored newPacket(address);
startExecution(&newPacket, iter);
}
}
void CommandingServiceBase::handleUnrequestedReply(
CommandMessage* reply) {
reply->clearCommandMessage();
}
inline void CommandingServiceBase::doPeriodicOperation() {
}
MessageQueueId_t CommandingServiceBase::getCommandQueue() {
return commandQueue->getId();
}
void CommandingServiceBase::checkTimeout() {
uint32_t uptime;
Clock::getUptime(&uptime);
typename FixedMap<MessageQueueId_t,
CommandingServiceBase::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);
}
}
}

461
tmtcservices/CommandingServiceBase.h
View File

@ -20,7 +20,17 @@
#include <framework/ipc/QueueFactory.h>
#include <framework/timemanager/Clock.h>
template<typename STATE_T>
/**
* \brief This class is the basis for all PUS Services, which have to relay Telecommands to software bus.
*
* It manages Telecommand reception and the generation of Verification Reports like PUSServiceBase.
* Every class that inherits from this abstract class has to implement four adaption points:
* - isValidSubservice
* - getMessageQueueAndObject
* - prepareCommand
* - handleReply
* \ingroup pus_services
*/
class CommandingServiceBase: public SystemObject,
public AcceptsTelecommandsIF,
public ExecutableObjectIF,
@ -35,18 +45,52 @@ public:
static const ReturnValue_t INVALID_OBJECT = MAKE_RETURN_CODE(6);
static const ReturnValue_t INVALID_REPLY = MAKE_RETURN_CODE(7);
/**
* Class constructor. Initializes two important MessageQueues:
* commandQueue for command reception and requestQueue for device reception
* @param setObjectId
* @param apid
* @param service
* @param numberOfParallelCommands
* @param commandTimeout_seconds
* @param setPacketSource
* @param setPacketDestination
* @param queueDepth
*/
CommandingServiceBase(object_id_t setObjectId, uint16_t apid,
uint8_t service, uint8_t numberOfParallelCommands,
uint16_t commandTimeout_seconds, object_id_t setPacketSource,
object_id_t setPacketDestination, size_t queueDepth = 20);
virtual ~CommandingServiceBase();
/***
* This is the periodic called function
* Handle request queue for external commands.
* Handle command Queue for internal commands.
* @param opCode is unused here at the moment
* @return RETURN_OK
*/
virtual ReturnValue_t performOperation(uint8_t opCode);
virtual uint16_t getIdentifier();
/**
* Returns the requestQueue MessageQueueId_t
*
* The requestQueue is the queue for external commands (TC)
*
* @return requestQueue messageQueueId_t
*/
virtual MessageQueueId_t getRequestQueue();
/**
* Returns the commandQueue MessageQueueId_t
*
* Remember the CommandQueue is the queue for internal communication
* @return commandQueue messageQueueId_t
*/
virtual MessageQueueId_t getCommandQueue();
virtual ReturnValue_t initialize();
/**
@ -69,7 +113,7 @@ protected:
uint32_t uptimeOfStart;
uint8_t step;
uint8_t subservice;
STATE_T state;
uint32_t state;
Command_t command;
object_id_t objectId;
FIFO<store_address_t, 3> fifo;
@ -107,21 +151,43 @@ protected:
*/
PeriodicTaskIF* executingTask;
/**
* Send TM data from pointer to data. If a header is supplied it is added before data
* @param subservice Number of subservice
* @param data Pointer to the data in the Packet
* @param dataLen Lenght of data in the Packet
* @param headerData HeaderData will be placed before data
* @param headerSize Size of HeaderData
*/
void sendTmPacket(uint8_t subservice, const uint8_t *data, uint32_t dataLen,
const uint8_t* headerData = NULL, uint32_t headerSize = 0);
/**
* To send TM packets of objects that still need to be serialized and consist of an object ID with appended data
* @param subservice Number of subservice
* @param objectId ObjectId is placed before data
* @param data Data to append to the packet
* @param dataLen Length of Data
*/
void sendTmPacket(uint8_t subservice, object_id_t objectId,
const uint8_t *data, uint32_t dataLen);
/**
* To send packets has data which is in form of a SerializeIF or Adapters implementing it
* @param subservice Number of subservice
* @param content This is a pointer to the serialized packet
* @param header Serialize IF header which will be placed before content
*/
void sendTmPacket(uint8_t subservice, SerializeIF* content,
SerializeIF* header = NULL);
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;
uint32_t *state, object_id_t objectId) = 0;
virtual ReturnValue_t handleReply(const CommandMessage *reply,
Command_t previousCommand, STATE_T *state,
Command_t previousCommand, uint32_t *state,
CommandMessage *optionalNextCommand, object_id_t objectId,
bool *isStep) = 0;
@ -137,8 +203,26 @@ protected:
typename FixedMap<MessageQueueId_t, CommandInfo>::Iterator *iter);
private:
/**
* This method handles internal execution of a command,
* once it has been started by @sa{startExecution()} in the Request Queue handler.
* It handles replies generated by the devices and relayed by the specific service implementation.
* This means that it determines further course of action depending on the return values specified
* in the service implementation.
* This includes the generation of TC verification messages:
* - TM[1,5] Step Successs
* - TM[1,6] Step Failure
* - TM[1,7] Completion Success
* - TM[1,8] Completion Failure
*/
void handleCommandQueue();
/**
* Sequence of request queue handling:
* isValidSubservice -> getMessageQueueAndObject -> startExecution
* Generates Start Success Reports TM[1,3] in subfunction @sa{startExecution()}
* or Start Failure Report TM[1,4] by using the TC Verification Service
*/
void handleRequestQueue();
void rejectPacket(uint8_t reportId, TcPacketStored* packet,
@ -152,373 +236,4 @@ private:
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, object_id_t setPacketSource,
object_id_t setPacketDestination, size_t queueDepth) :
SystemObject(setObjectId), apid(apid), service(service), timeout_seconds(
commandTimeout_seconds), tmPacketCounter(0), IPCStore(NULL), TCStore(
NULL), commandQueue(NULL), requestQueue(NULL), commandMap(
numberOfParallelCommands), failureParameter1(0), failureParameter2(
0), packetSource(setPacketSource), packetDestination(
setPacketDestination),executingTask(NULL) {
commandQueue = QueueFactory::instance()->createMessageQueue(queueDepth);
requestQueue = QueueFactory::instance()->createMessageQueue(20); //TODO: Funny magic number.
}
template<typename STATE_T>
CommandingServiceBase<STATE_T>::~CommandingServiceBase() {
QueueFactory::instance()->deleteMessageQueue(commandQueue);
QueueFactory::instance()->deleteMessageQueue(requestQueue);
}
template<typename STATE_T>
ReturnValue_t CommandingServiceBase<STATE_T>::performOperation(uint8_t opCode) {
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>(packetDestination);
PUSDistributorIF* distributor = objectManager->get<PUSDistributorIF>(
packetSource);
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 (reply.getSender() == MessageQueueIF::NO_QUEUE) {
handleUnrequestedReply(&reply);
continue;
}
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, const uint8_t* headerData,
uint32_t headerSize) {
TmPacketStored tmPacketStored(this->apid, this->service, subservice,
this->tmPacketCounter, data, dataLen, headerData, headerSize);
ReturnValue_t result = tmPacketStored.sendPacket(
requestQueue->getDefaultDestination(), requestQueue->getId());
if (result == HasReturnvaluesIF::RETURN_OK) {
this->tmPacketCounter++;
}
}
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);
ReturnValue_t result = tmPacketStored.sendPacket(
requestQueue->getDefaultDestination(), requestQueue->getId());
if (result == HasReturnvaluesIF::RETURN_OK) {
this->tmPacketCounter++;
}
}
template<typename STATE_T>
void CommandingServiceBase<STATE_T>::sendTmPacket(uint8_t subservice,
SerializeIF* content, SerializeIF* header) {
TmPacketStored tmPacketStored(this->apid, this->service, subservice,
this->tmPacketCounter, content, header);
ReturnValue_t result = tmPacketStored.sendPacket(
requestQueue->getDefaultDestination(), requestQueue->getId());
if (result == HasReturnvaluesIF::RETURN_OK) {
this->tmPacketCounter++;
}
}
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) {
Clock::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;
Clock::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_ */

4
tmtcservices/PusServiceBase.h
View File

@ -23,7 +23,7 @@ void setStaticFrameworkObjectIds();
*/
/**
* This class is the basis for all PUS Services, which can immediately process Telecommand Packets.
* \brief This class is the basis for all PUS Services, which can immediately process Telecommand Packets.
* It manages Telecommand reception and the generation of Verification Reports. Every class that inherits
* from this abstract class has to implement handleRequest and performService. Services that are created with this
* Base class have to handle any kind of request immediately on reception.
@ -68,7 +68,7 @@ public:
* It checks for new requests, and, if found, calls handleRequest, sends completion verification messages and deletes
* the TC requests afterwards.
* performService is always executed afterwards.
* @return - \c RETURN_OK if the periodic performService was successfull.
* @return - \c RETURN_OK if the periodic performService was successful.
* - \c RETURN_FAILED else.
*/
ReturnValue_t performOperation(uint8_t opCode);