Compare commits

...

8 Commits

8 changed files with 474 additions and 11 deletions

View File

@ -332,6 +332,7 @@ void DeviceHandlerBase::callChildStatemachine() {
}
void DeviceHandlerBase::setTransition(Mode_t modeTo, Submode_t submodeTo) {
sif::debug<<"DeviceHandlerBase::setTransition"<<std::endl;
triggerEvent(CHANGING_MODE, modeTo, submodeTo);
childTransitionDelay = getTransitionDelayMs(mode, modeTo);
transitionSourceMode = mode;
@ -428,13 +429,15 @@ void DeviceHandlerBase::replyToReply(DeviceReplyMap::iterator iter,
void DeviceHandlerBase::doSendWrite() {
if (cookieInfo.state == COOKIE_WRITE_READY) {
sif::debug<<" DeviceHandlerBase::doSendWrite: Calling sendMessage"<<std::endl;
ReturnValue_t result = communicationInterface->sendMessage(cookie,
rawPacket, rawPacketLen);
if (result == RETURN_OK) {
cookieInfo.state = COOKIE_WRITE_SENT;
sif::debug<<" DeviceHandlerBase::doSendWrite: Calling sendMessage succeeded"<<std::endl;
} else {
sif::debug<<" DeviceHandlerBase::doSendWrite: Calling sendMessage failed"<<std::endl;
//always generate a failure event, so that FDIR knows what's up
triggerEvent(DEVICE_SENDING_COMMAND_FAILED, result,
cookieInfo.pendingCommand->first);
@ -447,9 +450,11 @@ void DeviceHandlerBase::doSendWrite() {
void DeviceHandlerBase::doGetWrite() {
if (cookieInfo.state != COOKIE_WRITE_SENT) {
// sif::debug<<" DeviceHandlerBase::doGetWrite: COOKIE_WRITE_SENT not set"<<std::endl;
return;
}
cookieInfo.state = COOKIE_UNUSED;
sif::debug<<" DeviceHandlerBase::doGetWrite: Calling getSendSuccess"<<std::endl;
ReturnValue_t result = communicationInterface->getSendSuccess(cookie);
if (result == RETURN_OK) {
if (wiretappingMode == RAW) {
@ -470,7 +475,7 @@ void DeviceHandlerBase::doGetWrite() {
void DeviceHandlerBase::doSendRead() {
ReturnValue_t result;
// sif::debug<<" DeviceHandlerBase::doSendRead: Calling requestReceiveMessage"<<std::endl;
result = communicationInterface->requestReceiveMessage(cookie);
if (result == RETURN_OK) {
cookieInfo.state = COOKIE_READ_SENT;
@ -497,7 +502,7 @@ void DeviceHandlerBase::doGetRead() {
}
cookieInfo.state = COOKIE_UNUSED;
// sif::debug<<" DeviceHandlerBase::doGetRead: Calling readReceivedMessage"<<std::endl;
result = communicationInterface->readReceivedMessage(cookie, &receivedData,
&receivedDataLen);
@ -522,10 +527,12 @@ void DeviceHandlerBase::doGetRead() {
//This approach avoids infinite loops due to buggy scanForReply routines (seen in bug 1077).
uint32_t remainingLength = receivedDataLen;
for (uint32_t count = 0; count < receivedDataLen; count++) {
sif::debug<<" DeviceHandlerBase::doGetRead: Calling scanForReply"<<std::endl;
result = scanForReply(receivedData, remainingLength, &foundId,
&foundLen);
switch (result) {
case RETURN_OK:
sif::debug<<" DeviceHandlerBase::doGetRead: Calling handle reply"<<std::endl;
handleReply(receivedData, foundId, foundLen);
break;
case APERIODIC_REPLY: {
@ -823,6 +830,7 @@ ReturnValue_t DeviceHandlerBase::enableReplyInReplyMap(
void DeviceHandlerBase::doTransition(Mode_t modeFrom, Submode_t subModeFrom) {
setMode(getBaseMode(mode));
sif::debug<<"DeviceHandlerBase::doTransition Mode: "<<mode<<" reached"<<std::endl;
}
uint32_t DeviceHandlerBase::getTransitionDelayMs(Mode_t modeFrom,
@ -900,6 +908,7 @@ ReturnValue_t DeviceHandlerBase::checkModeCommand(Mode_t commandedMode,
void DeviceHandlerBase::startTransition(Mode_t commandedMode,
Submode_t commandedSubmode) {
sif::debug<<" DeviceHandlerBase::startTransition"<<std::endl;
switch (commandedMode) {
case MODE_ON:
if (mode == MODE_OFF) {
@ -1165,8 +1174,8 @@ void DeviceHandlerBase::buildInternalCommand(void) {
if (mode == MODE_NORMAL) {
result = buildNormalDeviceCommand(&deviceCommandId);
if (result == BUSY) {
sif::debug << std::hex << getObjectId()
<< ": DHB::buildInternalCommand busy" << std::endl; //so we can track misconfigurations
// sif::debug << std::hex << getObjectId()
// << ": DeviceHandlerBase::buildInternalCommand busy" << std::endl; //so we can track misconfigurations
result = NOTHING_TO_SEND; //no need to report this
}
} else if (mode == MODE_RAW) {
@ -1186,9 +1195,9 @@ void DeviceHandlerBase::buildInternalCommand(void) {
if (iter == deviceCommandMap.end()) {
result = COMMAND_NOT_SUPPORTED;
} else if (iter->second.isExecuting) {
sif::debug << std::hex << getObjectId()
<< ": DHB::buildInternalCommand: Command "
<< deviceCommandId << " isExecuting" << std::endl; //so we can track misconfigurations
// sif::debug << std::hex << getObjectId()
// << ": DHB::buildInternalCommand: Command "
// << deviceCommandId << " isExecuting" << std::endl; //so we can track misconfigurations
return; //this is an internal command, no need to report a failure here, missed reply will track if a reply is too late, otherwise, it's ok
} else {
iter->second.sendReplyTo = NO_COMMANDER;

View File

@ -2,6 +2,7 @@
#include <framework/serviceinterface/ServiceInterfaceStream.h>
#include <iomanip>
#include <cstdlib>
#include <iomanip>
ObjectManager::ObjectManager( void (*setProducer)() ):
produceObjects(setProducer) {

149
serialize/SerialBufferAdapter2.h Executable file
View File

@ -0,0 +1,149 @@
#ifndef SERIALBUFFERADAPTER2_H_
#define SERIALBUFFERADAPTER2_H_
#include <framework/serialize/SerializeIF.h>
#include <framework/serialize/SerializeAdapter.h>
#include <framework/serviceinterface/ServiceInterfaceStream.h>
#include <cstring>
extern std::ostream error;
extern std::ostream debug;
/**
* This adapter provides an interface for SerializeIF to serialize or deserialize
* buffers with no length header but a known size.
*
* Additionally, the buffer length can be serialized too and will be put in front of the serialized buffer.
*
* Can be used with SerialLinkedListAdapter by declaring a SerializeElement with
* SerialElement<SerialBufferAdapter<bufferLengthType(will be uint8_t mostly)>> serialBufferElement.
* Right now, the SerialBufferAdapter must always be initialized with the buffer and size !
*
* \ingroup serialize
*/
template<typename BUFFER_TYPE = uint8_t, typename count_t = uint8_t>
class SerialBufferAdapter2: public SerializeIF {
public:
/**
* Constructor for constant uint8_t buffer. Length field can be serialized optionally.
* Type of length can be supplied as template type.
* @param buffer
* @param bufferLength
* @param serializeLength
*/
SerialBufferAdapter2(void * buffer_, count_t bufferLength_, bool serializeLength_ = false):
bufferLength(bufferLength_), serializeLength(serializeLength_) {
determineLengthInBytes(sizeof(BUFFER_TYPE));
buffer = reinterpret_cast<const uint8_t *>(buffer_);
constBuffer = NULL;
}
SerialBufferAdapter2(const void * buffer_, count_t bufferLength_, bool serializeLength_ = false):
bufferLength(bufferLength_), serializeLength(serializeLength_) {
determineLengthInBytes(sizeof(BUFFER_TYPE));
constBuffer = reinterpret_cast<const uint8_t *>(buffer_);
buffer = NULL;
}
ReturnValue_t serialize(uint8_t ** buffer, uint32_t* size,
const uint32_t max_size, bool bigEndian) const {
uint32_t serializedLength = bufferLength;
if (serializeLength) {
serializedLength += AutoSerializeAdapter::getSerializedSize(
&bufferLength);
}
if (*size + serializedLength > max_size) {
return BUFFER_TOO_SHORT;
} else {
if (serializeLength) {
AutoSerializeAdapter::serialize(&bufferLength, buffer, size,
max_size, bigEndian);
}
memcpy(*buffer, this->buffer, bufferLength);
*size += bufferLength;
(*buffer) += bufferLength;
return HasReturnvaluesIF::RETURN_OK;
}
}
uint32_t getSerializedSize() const {
if (serializeLength) {
return bufferLength + AutoSerializeAdapter::getSerializedSize(&bufferLength);
} else {
return bufferLength;
}
}
ReturnValue_t deSerialize(const uint8_t** buffer,
int32_t* size, bool bigEndian) {
//TODO Ignores Endian flag!
if (buffer != NULL) {
if(serializeLength){
// Suggestion (would require removing rest of the block inside this if clause !):
//ReturnValue_t result = AutoSerializeAdapter::deSerialize(&bufferLength,buffer,size,bigEndian);
//if (result != HasReturnvaluesIF::RETURN_OK) {
// return result;
//}
count_t serializedSize = AutoSerializeAdapter::getSerializedSize(
&bufferLength);
if((*size - bufferLength - serializedSize) >= 0){
*buffer += serializedSize;
*size -= serializedSize;
}else{
return STREAM_TOO_SHORT;
}
}
//No Else If, go on with buffer
if (*size - bufferLength >= 0) {
*size -= bufferLength;
memcpy(this->buffer, *buffer, bufferLength);
(*buffer) += bufferLength;
return HasReturnvaluesIF::RETURN_OK;
} else {
return STREAM_TOO_SHORT;
}
} else {
return HasReturnvaluesIF::RETURN_FAILED;
}
}
BUFFER_TYPE * getBuffer() {
return reinterpret_cast<BUFFER_TYPE *>(buffer);
}
void setBuffer(void * buffer_, count_t bufferLength_, bool serializeLength_ = false) {
buffer = buffer_;
bufferLength = bufferLength_;
serializeLength = serializeLength_;
determineLengthInBytes(sizeof(BUFFER_TYPE));
}
void setConstBuffer(const void * buffer_, count_t bufferLength_, bool serializeLength_ = false) {
constBuffer = buffer_;
bufferLength = bufferLength_;
serializeLength = serializeLength_;
determineLengthInBytes(sizeof(BUFFER_TYPE));
}
private:
uint8_t * buffer;
const uint8_t * constBuffer;
count_t bufferLength;
bool serializeLength;
void determineLengthInBytes(uint8_t typeSize) {
switch(typeSize) {
case(1): break;
case(2):
bufferLength *= 2; break;
case(4):
bufferLength *= 4; break;
case(8):
bufferLength *= 8; break;
default:
error << "Serial Buffer Adapter 2: Invalid type size, assuming regular uint8_t." << std::endl;
error << "Detected type size: " << (int) typeSize << std::endl;
}
}
};
#endif /* SERIALBUFFERADAPTER2_H_ */

View File

@ -11,7 +11,7 @@ CCSDSDistributor::~CCSDSDistributor() {
}
iterator_t CCSDSDistributor::selectDestination() {
// debug << "CCSDSDistributor::selectDestination received: " << this->currentMessage.getStorageId().pool_index << ", " << this->currentMessage.getStorageId().packet_index << std::endl;
sif::debug << "CCSDSDistributor::selectDestination received: " << this->currentMessage.getStorageId().pool_index << ", " << this->currentMessage.getStorageId().packet_index << std::endl;
const uint8_t* p_packet = NULL;
size_t size = 0;
//TODO check returncode?

View File

@ -15,7 +15,7 @@ PUSDistributor::~PUSDistributor() {
}
iterator_t PUSDistributor::selectDestination() {
// debug << "PUSDistributor::handlePacket received: " << this->current_packet_id.store_index << ", " << this->current_packet_id.packet_index << std::endl;
sif::debug << "PUSDistributor::handlePacket received "<<std::endl;// << this->currentPacket.store_index << ", " << this->current_packet_id.packet_index << std::endl;
iterator_t queueMapIt = this->queueMap.end();
this->currentPacket.setStoreAddress(this->currentMessage.getStorageId());
if (currentPacket.getWholeData() != NULL) {

View File

@ -15,7 +15,7 @@ TcDistributor::~TcDistributor() {
ReturnValue_t TcDistributor::performOperation(uint8_t opCode) {
ReturnValue_t status = RETURN_OK;
// debug << "TcDistributor: performing Operation." << std::endl;
//sif::debug << "TcDistributor: performing Operation." << std::endl;
for (status = tcQueue->receiveMessage(&currentMessage); status == RETURN_OK;
status = tcQueue->receiveMessage(&currentMessage)) {
status = handlePacket();
@ -31,6 +31,7 @@ ReturnValue_t TcDistributor::handlePacket() {
iterator_t queueMapIt = this->selectDestination();
ReturnValue_t returnValue = RETURN_FAILED;
sif::debug << "TcDistributor::handlePacket" << std::endl;
if (queueMapIt != this->queueMap.end()) {
returnValue = this->tcQueue->sendMessage(queueMapIt->second,
&this->currentMessage);

177
tmtcservices/TmTcBridge.cpp Normal file
View File

@ -0,0 +1,177 @@
/**
* @file TmTcBridge.cpp
*
* @date 26.12.2019
* @author R. Mueller
*/
#include <framework/tmtcservices/TmTcBridge.h>
#include <framework/ipc/QueueFactory.h>
#include <framework/tmtcservices/AcceptsTelecommandsIF.h>
#include <framework/serviceinterface/ServiceInterfaceStream.h>
extern std::ostream error;
extern std::ostream debug;
extern std::ostream info;
TmTcBridge::TmTcBridge(object_id_t objectId_, object_id_t ccsdsPacketDistributor_):
SystemObject(objectId_),tcStore(NULL), tmStore(NULL),
ccsdsPacketDistributor(ccsdsPacketDistributor_), communicationLinkUp(false),
tmStored(false),recvBuffer(NULL), size(0) {
TmTcReceptionQueue = QueueFactory::instance()->
createMessageQueue(TMTC_RECEPTION_QUEUE_DEPTH);
}
TmTcBridge::~TmTcBridge() {
}
ReturnValue_t TmTcBridge::initialize() {
tcStore = objectManager->get<StorageManagerIF>(objects::TC_STORE);
if (tcStore == NULL) {
return RETURN_FAILED;
}
tmStore = objectManager->get<StorageManagerIF>(objects::TM_STORE);
if (tmStore == NULL) {
return RETURN_FAILED;
}
AcceptsTelecommandsIF* tcDistributor =
objectManager->get<AcceptsTelecommandsIF>(ccsdsPacketDistributor);
if (tcDistributor == NULL) {
return RETURN_FAILED;
}
TmTcReceptionQueue->setDefaultDestination(tcDistributor->getRequestQueue());
return RETURN_OK;
}
ReturnValue_t TmTcBridge::performOperation(uint8_t operationCode) {
ReturnValue_t result;
result = handleTc();
if(result != RETURN_OK) {
error << "TMTC Bridge: Error handling TCs" << std::endl;
}
result = handleTm();
if (result != RETURN_OK) {
error << "TMTC Bridge: Error handling TMs" << std::endl;
}
return result;
}
ReturnValue_t TmTcBridge::handleTc() {
ReturnValue_t result = receiveTc(&recvBuffer, &size);
return result;
}
ReturnValue_t TmTcBridge::handleTm() {
ReturnValue_t result = readTmQueue();
if(result != RETURN_OK) {
error << "TMTC Bridge: Reading TM Queue failed" << std::endl;
return RETURN_FAILED;
}
if(tmStored && communicationLinkUp) {
result = sendStoredTm();
}
return result;
}
ReturnValue_t TmTcBridge::readTmQueue() {
TmTcMessage message;
const uint8_t* data = NULL;
size_t size = 0;
for (ReturnValue_t result = TmTcReceptionQueue->receiveMessage(&message);
result == RETURN_OK; result = TmTcReceptionQueue->receiveMessage(&message))
{
if(communicationLinkUp == false) {
result = storeDownlinkData(&message);
return result;
}
result = tmStore->getData(message.getStorageId(), &data, &size);
if (result != HasReturnvaluesIF::RETURN_OK) {
continue;
}
result = sendTm(data, size);
if (result != RETURN_OK) {
error << "TMTC Bridge: Could not send TM packet"<< std::endl;
tmStore->deleteData(message.getStorageId());
return result;
}
tmStore->deleteData(message.getStorageId());
}
return RETURN_OK;
}
ReturnValue_t TmTcBridge::storeDownlinkData(TmTcMessage *message) {
sif::info << "TMTC Bridge: Comm Link down. "
"Saving packet ID to be sent later " << std::endl;
store_address_t storeId;
if(fifo.full()) {
sif::info << "TMTC Bridge: TM downlink max. number of stored packet IDs reached."
" Overwriting old data" << std::endl;
fifo.retrieve(&storeId);
tmStore->deleteData(storeId);
}
storeId = message->getStorageId();
fifo.insert(storeId);
tmStored = true;
return RETURN_OK;
}
ReturnValue_t TmTcBridge::sendStoredTm() {
uint8_t counter = 0;
ReturnValue_t result = RETURN_OK;
while(!fifo.empty() && counter < MAX_STORED_DATA_SENT_PER_CYCLE) {
sif::info << "UDP Server: Sending stored TM data. There are "
<< (int) fifo.size() << " left to send" << std::endl;
store_address_t storeId;
const uint8_t* data = NULL;
size_t size = 0;
fifo.retrieve(&storeId);
result = tmStore->getData(storeId, &data, &size);
sendTm(data,size);
if(result != RETURN_OK) {
error << "UDP Server: Could not send stored downlink data" << std::endl;
result = RETURN_FAILED;
}
counter ++;
if(fifo.empty()) {
tmStored = false;
}
tmStore->deleteData(storeId);
}
return result;
}
void TmTcBridge::registerCommConnect() {
if(!communicationLinkUp) {
sif::info << "TMTC Bridge: Registered Comm Link Connect" << std::endl;
communicationLinkUp = true;
}
}
void TmTcBridge::registerCommDisconnect() {
sif::info << "TMTC Bridge: Registered Comm Link Disconnect" << std::endl;
if(communicationLinkUp) {
communicationLinkUp = false;
}
}
MessageQueueId_t TmTcBridge::getReportReceptionQueue(uint8_t virtualChannel) {
return TmTcReceptionQueue->getId();
}
void TmTcBridge::printData(uint8_t * data, uint32_t dataLen) {
sif::info << "TMTC Bridge: Printing data: [";
for(uint32_t i=0;i<dataLen;i++) {
sif::info << std::hex << (int)data[i];
if(i < dataLen-1){
sif::info << " , ";
}
}
sif::info << " ] " << std::endl;
}

126
tmtcservices/TmTcBridge.h Normal file
View File

@ -0,0 +1,126 @@
/**
* @file TmTcBridge.h
*
* @date 26.12.2019
*/
#ifndef FRAMEWORK_TMTCSERVICES_TMTCBRIDGE_H_
#define FRAMEWORK_TMTCSERVICES_TMTCBRIDGE_H_
#include <framework/tmtcservices/AcceptsTelemetryIF.h>
#include <framework/tasks/ExecutableObjectIF.h>
#include <framework/ipc/MessageQueueIF.h>
#include <framework/storagemanager/StorageManagerIF.h>
#include <framework/objectmanager/SystemObject.h>
#include <framework/tmtcservices/TmTcMessage.h>
#include <framework/container/FIFO.h>
class TmTcBridge : public AcceptsTelemetryIF,
public ExecutableObjectIF,
public HasReturnvaluesIF,
public SystemObject {
public:
TmTcBridge(object_id_t objectId_, object_id_t ccsdsPacketDistributor_);
virtual ~TmTcBridge();
/**
* Initializes basic FSFW components for the TMTC Bridge
* @return
*/
virtual ReturnValue_t initialize();
/**
* @brief The performOperation method is executed in a task.
* @details There are no restrictions for calls within this method, so any
* other member of the class can be used.
* @return Currently, the return value is ignored.
*/
virtual ReturnValue_t performOperation(uint8_t operationCode = 0);
/**
* Return TMTC Reception Queue
* @param virtualChannel
* @return
*/
virtual MessageQueueId_t getReportReceptionQueue(uint8_t virtualChannel = 0);
void registerCommConnect();
void registerCommDisconnect();
protected:
MessageQueueIF* TmTcReceptionQueue; //!< Used to send and receive TMTC messages. TmTcMessage is used to transport messages between tasks.
StorageManagerIF* tcStore;
StorageManagerIF* tmStore;
object_id_t ccsdsPacketDistributor;
bool communicationLinkUp; //!< Used to specify whether communication link is up
bool tmStored;
/**
* Handle TC reception. Default implementation provided
* @return
*/
virtual ReturnValue_t handleTc();
/**
* Implemented by child class. Perform receiving of Telecommand, for example by implementing
* specific drivers or wrappers, e.g. UART Communication or lwIP stack
* @param recvBuffer [out] Received data
* @param size [out] Size of received data
* @return
*/
virtual ReturnValue_t receiveTc(uint8_t ** recvBuffer, uint32_t * size) = 0;
/**
* Handle Telemetry. Default implementation provided.
* Calls sendTm()
* @return
*/
virtual ReturnValue_t handleTm();
/**
* Read the TM Queue and send TM if necessary. Default implementation provided
* @return
*/
virtual ReturnValue_t readTmQueue();
/**
* Implemented by child class. Perform sending of Telemetry by implementing
* communication drivers or wrappers, e.g. UART communication or lwIP stack.
* @param data
* @param dataLen
* @return
*/
virtual ReturnValue_t sendTm(const uint8_t * data, uint32_t dataLen) = 0;
/**
* Store data to be sent later if communication link is not up.
* @param message
* @return
*/
ReturnValue_t storeDownlinkData(TmTcMessage * message);
/**
* Send stored data if communication link is active
* @return
*/
ReturnValue_t sendStoredTm();
/**
* Print data as hexidecimal array
* @param data
* @param dataLen
*/
void printData(uint8_t * data, uint32_t dataLen);
private:
static const uint8_t TMTC_RECEPTION_QUEUE_DEPTH = 20;
static const uint8_t MAX_STORED_DATA_SENT_PER_CYCLE = 10;
static const uint8_t MAX_DOWNLINK_PACKETS_STORED = 15;
FIFO<store_address_t, MAX_DOWNLINK_PACKETS_STORED> fifo;
uint8_t * recvBuffer;
uint32_t size;
};
#endif /* FRAMEWORK_TMTCSERVICES_TMTCBRIDGE_H_ */