805 lines
27 KiB
C++
805 lines
27 KiB
C++
|
#include "TestDeviceHandler.h"
|
||
|
|
#include <OBSWConfig.h>
|
||
|
|
|
||
|
|
#include <fsfw/datapool/PoolReadGuard.h>
|
||
|
|
|
||
|
|
#include <cstdlib>
|
||
|
|
|
||
|
|
TestDevice::TestDevice(object_id_t objectId, object_id_t comIF,
|
||
|
|
CookieIF * cookie, testdevice::DeviceIndex deviceIdx, bool fullInfoPrintout,
|
||
|
|
bool changingDataset):
|
||
|
|
DeviceHandlerBase(objectId, comIF, cookie), deviceIdx(deviceIdx),
|
||
|
|
dataset(this), fullInfoPrintout(fullInfoPrintout) {
|
||
|
|
}
|
||
|
|
|
||
|
|
TestDevice::~TestDevice() {}
|
||
|
|
|
||
|
|
void TestDevice::performOperationHook() {
|
||
|
|
if(periodicPrintout) {
|
||
|
|
#if FSFW_CPP_OSTREAM_ENABLED == 1
|
||
|
|
sif::info << "TestDevice" << deviceIdx << "::performOperationHook: Alive!" << std::endl;
|
||
|
|
#else
|
||
|
|
sif::printInfo("TestDevice%d::performOperationHook: Alive!", deviceIdx);
|
||
|
|
#endif
|
||
|
|
}
|
||
|
|
|
||
|
|
if(oneShot) {
|
||
|
|
oneShot = false;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
void TestDevice::doStartUp() {
|
||
|
|
if(fullInfoPrintout) {
|
||
|
|
#if FSFW_CPP_OSTREAM_ENABLED == 1
|
||
|
|
sif::info << "TestDevice" << deviceIdx << "::doStartUp: Switching On" << std::endl;
|
||
|
|
#else
|
||
|
|
sif::printInfo("TestDevice%d::doStartUp: Switching On\n", static_cast<int>(deviceIdx));
|
||
|
|
#endif
|
||
|
|
}
|
||
|
|
|
||
|
|
setMode(_MODE_TO_ON);
|
||
|
|
return;
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
void TestDevice::doShutDown() {
|
||
|
|
if(fullInfoPrintout) {
|
||
|
|
#if FSFW_CPP_OSTREAM_ENABLED == 1
|
||
|
|
sif::info << "TestDevice" << deviceIdx << "::doShutDown: Switching Off" << std::endl;
|
||
|
|
#else
|
||
|
|
sif::printInfo("TestDevice%d::doShutDown: Switching Off\n", static_cast<int>(deviceIdx));
|
||
|
|
#endif
|
||
|
|
}
|
||
|
|
|
||
|
|
setMode(_MODE_SHUT_DOWN);
|
||
|
|
return;
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
ReturnValue_t TestDevice::buildNormalDeviceCommand(DeviceCommandId_t* id) {
|
||
|
|
using namespace testdevice;
|
||
|
|
*id = TEST_NORMAL_MODE_CMD;
|
||
|
|
if(DeviceHandlerBase::isAwaitingReply()) {
|
||
|
|
return NOTHING_TO_SEND;
|
||
|
|
}
|
||
|
|
return buildCommandFromCommand(*id, nullptr, 0);
|
||
|
|
}
|
||
|
|
|
||
|
|
ReturnValue_t TestDevice::buildTransitionDeviceCommand(DeviceCommandId_t* id) {
|
||
|
|
if(mode == _MODE_TO_ON) {
|
||
|
|
if(fullInfoPrintout) {
|
||
|
|
#if FSFW_CPP_OSTREAM_ENABLED == 1
|
||
|
|
sif::info << "TestDevice" << deviceIdx << "::buildTransitionDeviceCommand: Was called"
|
||
|
|
" from _MODE_TO_ON mode" << std::endl;
|
||
|
|
#else
|
||
|
|
sif::printInfo("TestDevice%d::buildTransitionDeviceCommand: "
|
||
|
|
"Was called from _MODE_TO_ON mode\n", deviceIdx);
|
||
|
|
#endif
|
||
|
|
}
|
||
|
|
|
||
|
|
}
|
||
|
|
if(mode == _MODE_TO_NORMAL) {
|
||
|
|
if(fullInfoPrintout) {
|
||
|
|
#if FSFW_CPP_OSTREAM_ENABLED == 1
|
||
|
|
sif::info << "TestDevice" << deviceIdx << "::buildTransitionDeviceCommand: Was called "
|
||
|
|
"from _MODE_TO_NORMAL mode" << std::endl;
|
||
|
|
#else
|
||
|
|
sif::printInfo("TestDevice%d::buildTransitionDeviceCommand: Was called from "
|
||
|
|
" _MODE_TO_NORMAL mode\n", deviceIdx);
|
||
|
|
#endif
|
||
|
|
}
|
||
|
|
|
||
|
|
setMode(MODE_NORMAL);
|
||
|
|
}
|
||
|
|
if(mode == _MODE_SHUT_DOWN) {
|
||
|
|
if(fullInfoPrintout) {
|
||
|
|
#if FSFW_CPP_OSTREAM_ENABLED == 1
|
||
|
|
sif::info << "TestDevice" << deviceIdx << "::buildTransitionDeviceCommand: Was called "
|
||
|
|
"from _MODE_SHUT_DOWN mode" << std::endl;
|
||
|
|
#else
|
||
|
|
sif::printInfo("TestDevice%d::buildTransitionDeviceCommand: Was called from "
|
||
|
|
"_MODE_SHUT_DOWN mode\n", deviceIdx);
|
||
|
|
#endif
|
||
|
|
}
|
||
|
|
|
||
|
|
setMode(MODE_OFF);
|
||
|
|
}
|
||
|
|
return NOTHING_TO_SEND;
|
||
|
|
}
|
||
|
|
|
||
|
|
void TestDevice::doTransition(Mode_t modeFrom, Submode_t submodeFrom) {
|
||
|
|
if(mode == _MODE_TO_NORMAL) {
|
||
|
|
if(fullInfoPrintout) {
|
||
|
|
#if FSFW_CPP_OSTREAM_ENABLED == 1
|
||
|
|
sif::info << "TestDevice" << deviceIdx << "::doTransition: Custom transition to "
|
||
|
|
"normal mode" << std::endl;
|
||
|
|
#else
|
||
|
|
sif::printInfo("TestDevice%d::doTransition: Custom transition to normal mode\n",
|
||
|
|
deviceIdx);
|
||
|
|
#endif
|
||
|
|
}
|
||
|
|
|
||
|
|
}
|
||
|
|
else {
|
||
|
|
DeviceHandlerBase::doTransition(modeFrom, submodeFrom);
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
ReturnValue_t TestDevice::buildCommandFromCommand(
|
||
|
|
DeviceCommandId_t deviceCommand, const uint8_t* commandData,
|
||
|
|
size_t commandDataLen) {
|
||
|
|
using namespace testdevice;
|
||
|
|
ReturnValue_t result = HasReturnvaluesIF::RETURN_OK;
|
||
|
|
switch(deviceCommand) {
|
||
|
|
case(TEST_NORMAL_MODE_CMD): {
|
||
|
|
commandSent = true;
|
||
|
|
result = buildNormalModeCommand(deviceCommand, commandData, commandDataLen);
|
||
|
|
break;
|
||
|
|
}
|
||
|
|
|
||
|
|
case(TEST_COMMAND_0): {
|
||
|
|
commandSent = true;
|
||
|
|
result = buildTestCommand0(deviceCommand, commandData, commandDataLen);
|
||
|
|
break;
|
||
|
|
}
|
||
|
|
|
||
|
|
case(TEST_COMMAND_1): {
|
||
|
|
commandSent = true;
|
||
|
|
result = buildTestCommand1(deviceCommand, commandData, commandDataLen);
|
||
|
|
break;
|
||
|
|
}
|
||
|
|
case(TEST_NOTIF_SNAPSHOT_VAR): {
|
||
|
|
if(changingDatasets) {
|
||
|
|
changingDatasets = false;
|
||
|
|
}
|
||
|
|
|
||
|
|
PoolReadGuard readHelper(&dataset.testUint8Var);
|
||
|
|
if(deviceIdx == testdevice::DeviceIndex::DEVICE_0) {
|
||
|
|
/* This will trigger a variable notification to the demo controller */
|
||
|
|
dataset.testUint8Var = 220;
|
||
|
|
dataset.testUint8Var.setValid(true);
|
||
|
|
}
|
||
|
|
else if(deviceIdx == testdevice::DeviceIndex::DEVICE_1) {
|
||
|
|
/* This will trigger a variable snapshot to the demo controller */
|
||
|
|
dataset.testUint8Var = 30;
|
||
|
|
dataset.testUint8Var.setValid(true);
|
||
|
|
}
|
||
|
|
|
||
|
|
break;
|
||
|
|
}
|
||
|
|
case(TEST_NOTIF_SNAPSHOT_SET): {
|
||
|
|
if(changingDatasets) {
|
||
|
|
changingDatasets = false;
|
||
|
|
}
|
||
|
|
|
||
|
|
PoolReadGuard readHelper(&dataset.testFloat3Vec);
|
||
|
|
|
||
|
|
if(deviceIdx == testdevice::DeviceIndex::DEVICE_0) {
|
||
|
|
/* This will trigger a variable notification to the demo controller */
|
||
|
|
dataset.testFloat3Vec.value[0] = 60;
|
||
|
|
dataset.testFloat3Vec.value[1] = 70;
|
||
|
|
dataset.testFloat3Vec.value[2] = 55;
|
||
|
|
dataset.testFloat3Vec.setValid(true);
|
||
|
|
}
|
||
|
|
else if(deviceIdx == testdevice::DeviceIndex::DEVICE_1) {
|
||
|
|
/* This will trigger a variable notification to the demo controller */
|
||
|
|
dataset.testFloat3Vec.value[0] = -60;
|
||
|
|
dataset.testFloat3Vec.value[1] = -70;
|
||
|
|
dataset.testFloat3Vec.value[2] = -55;
|
||
|
|
dataset.testFloat3Vec.setValid(true);
|
||
|
|
}
|
||
|
|
break;
|
||
|
|
}
|
||
|
|
default:
|
||
|
|
result = DeviceHandlerIF::COMMAND_NOT_SUPPORTED;
|
||
|
|
}
|
||
|
|
return result;
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
ReturnValue_t TestDevice::buildNormalModeCommand(DeviceCommandId_t deviceCommand,
|
||
|
|
const uint8_t* commandData, size_t commandDataLen) {
|
||
|
|
if(fullInfoPrintout) {
|
||
|
|
#if OBSW_VERBOSE_LEVEL >= 3
|
||
|
|
#if FSFW_CPP_OSTREAM_ENABLED == 1
|
||
|
|
sif::info << "TestDevice::buildTestCommand1: Building normal command" << std::endl;
|
||
|
|
#else
|
||
|
|
sif::printInfo("TestDevice::buildTestCommand1: Building command from TEST_COMMAND_1\n");
|
||
|
|
#endif /* FSFW_CPP_OSTREAM_ENABLED == 1 */
|
||
|
|
#endif /* OBSW_VERBOSE_LEVEL >= 3 */
|
||
|
|
}
|
||
|
|
|
||
|
|
if(commandDataLen > MAX_BUFFER_SIZE - sizeof(DeviceCommandId_t)) {
|
||
|
|
return DeviceHandlerIF::INVALID_NUMBER_OR_LENGTH_OF_PARAMETERS;
|
||
|
|
}
|
||
|
|
/* The command is passed on in the command buffer as it is */
|
||
|
|
passOnCommand(deviceCommand, commandData, commandDataLen);
|
||
|
|
return RETURN_OK;
|
||
|
|
}
|
||
|
|
|
||
|
|
ReturnValue_t TestDevice::buildTestCommand0(DeviceCommandId_t deviceCommand,
|
||
|
|
const uint8_t* commandData, size_t commandDataLen) {
|
||
|
|
using namespace testdevice;
|
||
|
|
if(fullInfoPrintout) {
|
||
|
|
#if FSFW_CPP_OSTREAM_ENABLED == 1
|
||
|
|
sif::info << "TestDevice" << deviceIdx << "::buildTestCommand0: Executing simple command "
|
||
|
|
" with completion reply" << std::endl;
|
||
|
|
#else
|
||
|
|
sif::printInfo("TestDevice%d::buildTestCommand0: Executing simple command with "
|
||
|
|
"completion reply\n", deviceIdx);
|
||
|
|
#endif
|
||
|
|
}
|
||
|
|
|
||
|
|
if(commandDataLen > MAX_BUFFER_SIZE - sizeof(DeviceCommandId_t)) {
|
||
|
|
return DeviceHandlerIF::INVALID_NUMBER_OR_LENGTH_OF_PARAMETERS;
|
||
|
|
}
|
||
|
|
|
||
|
|
/* The command is passed on in the command buffer as it is */
|
||
|
|
passOnCommand(deviceCommand, commandData, commandDataLen);
|
||
|
|
return RETURN_OK;
|
||
|
|
}
|
||
|
|
|
||
|
|
ReturnValue_t TestDevice::buildTestCommand1(DeviceCommandId_t deviceCommand,
|
||
|
|
const uint8_t* commandData,
|
||
|
|
size_t commandDataLen) {
|
||
|
|
using namespace testdevice;
|
||
|
|
if(commandDataLen < 7) {
|
||
|
|
return DeviceHandlerIF::INVALID_NUMBER_OR_LENGTH_OF_PARAMETERS;
|
||
|
|
}
|
||
|
|
if(fullInfoPrintout) {
|
||
|
|
#if FSFW_CPP_OSTREAM_ENABLED == 1
|
||
|
|
sif::info << "TestDevice" << deviceIdx << "::buildTestCommand1: Executing command with "
|
||
|
|
"data reply" << std::endl;
|
||
|
|
#else
|
||
|
|
sif::printInfo("TestDevice%d:buildTestCommand1: Executing command with data reply\n",
|
||
|
|
deviceIdx);
|
||
|
|
#endif
|
||
|
|
}
|
||
|
|
|
||
|
|
deviceCommand = EndianConverter::convertBigEndian(deviceCommand);
|
||
|
|
memcpy(commandBuffer, &deviceCommand, sizeof(deviceCommand));
|
||
|
|
|
||
|
|
/* Assign and check parameters */
|
||
|
|
uint16_t parameter1 = 0;
|
||
|
|
size_t size = commandDataLen;
|
||
|
|
ReturnValue_t result = SerializeAdapter::deSerialize(¶meter1,
|
||
|
|
&commandData, &size, SerializeIF::Endianness::BIG);
|
||
|
|
if(result == HasReturnvaluesIF::RETURN_FAILED) {
|
||
|
|
return result;
|
||
|
|
}
|
||
|
|
|
||
|
|
/* Parameter 1 needs to be correct */
|
||
|
|
if(parameter1 != testdevice::COMMAND_1_PARAM1) {
|
||
|
|
return DeviceHandlerIF::INVALID_COMMAND_PARAMETER;
|
||
|
|
}
|
||
|
|
uint64_t parameter2 = 0;
|
||
|
|
result = SerializeAdapter::deSerialize(¶meter2,
|
||
|
|
&commandData, &size, SerializeIF::Endianness::BIG);
|
||
|
|
if(parameter2!= testdevice::COMMAND_1_PARAM2){
|
||
|
|
return DeviceHandlerIF::INVALID_COMMAND_PARAMETER;
|
||
|
|
}
|
||
|
|
|
||
|
|
/* Pass on the parameters to the Echo IF */
|
||
|
|
commandBuffer[4] = (parameter1 & 0xFF00) >> 8;
|
||
|
|
commandBuffer[5] = (parameter1 & 0xFF);
|
||
|
|
parameter2 = EndianConverter::convertBigEndian(parameter2);
|
||
|
|
memcpy(commandBuffer + 6, ¶meter2, sizeof(parameter2));
|
||
|
|
rawPacket = commandBuffer;
|
||
|
|
rawPacketLen = sizeof(deviceCommand) + sizeof(parameter1) +
|
||
|
|
sizeof(parameter2);
|
||
|
|
return RETURN_OK;
|
||
|
|
}
|
||
|
|
|
||
|
|
void TestDevice::passOnCommand(DeviceCommandId_t command, const uint8_t *commandData,
|
||
|
|
size_t commandDataLen) {
|
||
|
|
DeviceCommandId_t deviceCommandBe = EndianConverter::convertBigEndian(command);
|
||
|
|
memcpy(commandBuffer, &deviceCommandBe, sizeof(deviceCommandBe));
|
||
|
|
memcpy(commandBuffer + 4, commandData, commandDataLen);
|
||
|
|
rawPacket = commandBuffer;
|
||
|
|
rawPacketLen = sizeof(deviceCommandBe) + commandDataLen;
|
||
|
|
}
|
||
|
|
|
||
|
|
void TestDevice::fillCommandAndReplyMap() {
|
||
|
|
namespace td = testdevice;
|
||
|
|
insertInCommandAndReplyMap(testdevice::TEST_NORMAL_MODE_CMD, 5, &dataset);
|
||
|
|
insertInCommandAndReplyMap(testdevice::TEST_COMMAND_0, 5);
|
||
|
|
insertInCommandAndReplyMap(testdevice::TEST_COMMAND_1, 5);
|
||
|
|
|
||
|
|
/* No reply expected for these commands */
|
||
|
|
insertInCommandMap(td::TEST_NOTIF_SNAPSHOT_SET);
|
||
|
|
insertInCommandMap(td::TEST_NOTIF_SNAPSHOT_VAR);
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
ReturnValue_t TestDevice::scanForReply(const uint8_t *start, size_t len,
|
||
|
|
DeviceCommandId_t *foundId, size_t *foundLen) {
|
||
|
|
using namespace testdevice;
|
||
|
|
|
||
|
|
/* Unless a command was sent explicitely, we don't expect any replies and ignore this
|
||
|
|
the packet. On a real device, there might be replies which are sent without a previous
|
||
|
|
command. */
|
||
|
|
if(not commandSent) {
|
||
|
|
return DeviceHandlerBase::IGNORE_FULL_PACKET;
|
||
|
|
}
|
||
|
|
else {
|
||
|
|
commandSent = false;
|
||
|
|
}
|
||
|
|
|
||
|
|
if(len < sizeof(object_id_t)) {
|
||
|
|
return DeviceHandlerIF::LENGTH_MISSMATCH;
|
||
|
|
}
|
||
|
|
|
||
|
|
size_t size = len;
|
||
|
|
ReturnValue_t result = SerializeAdapter::deSerialize(foundId, &start, &size,
|
||
|
|
SerializeIF::Endianness::BIG);
|
||
|
|
if (result != RETURN_OK) {
|
||
|
|
return result;
|
||
|
|
}
|
||
|
|
|
||
|
|
DeviceCommandId_t pendingCmd = this->getPendingCommand();
|
||
|
|
|
||
|
|
switch(pendingCmd) {
|
||
|
|
|
||
|
|
case(TEST_NORMAL_MODE_CMD): {
|
||
|
|
if(fullInfoPrintout) {
|
||
|
|
#if OBSW_VERBOSE_LEVEL >= 3
|
||
|
|
#if FSFW_CPP_OSTREAM_ENABLED == 1
|
||
|
|
sif::info << "TestDevice::scanForReply: Reply for normal commnand (ID " <<
|
||
|
|
TEST_NORMAL_MODE_CMD << ") received!" << std::endl;
|
||
|
|
#else
|
||
|
|
sif::printInfo("TestDevice%d::scanForReply: Reply for normal command (ID %d) "
|
||
|
|
"received!\n", deviceIdx, TEST_NORMAL_MODE_CMD);
|
||
|
|
#endif
|
||
|
|
#endif
|
||
|
|
}
|
||
|
|
|
||
|
|
*foundLen = len;
|
||
|
|
*foundId = pendingCmd;
|
||
|
|
return RETURN_OK;
|
||
|
|
}
|
||
|
|
|
||
|
|
case(TEST_COMMAND_0): {
|
||
|
|
if(len < TEST_COMMAND_0_SIZE) {
|
||
|
|
return DeviceHandlerIF::LENGTH_MISSMATCH;
|
||
|
|
}
|
||
|
|
if(fullInfoPrintout) {
|
||
|
|
#if FSFW_CPP_OSTREAM_ENABLED == 1
|
||
|
|
sif::info << "TestDevice" << deviceIdx << "::scanForReply: Reply for simple command "
|
||
|
|
"(ID " << TEST_COMMAND_0 << ") received!" << std::endl;
|
||
|
|
#else
|
||
|
|
sif::printInfo("TestDevice%d::scanForReply: Reply for simple command (ID %d) "
|
||
|
|
"received!\n", deviceIdx, TEST_COMMAND_0);
|
||
|
|
#endif
|
||
|
|
}
|
||
|
|
|
||
|
|
*foundLen = TEST_COMMAND_0_SIZE;
|
||
|
|
*foundId = pendingCmd;
|
||
|
|
return RETURN_OK;
|
||
|
|
}
|
||
|
|
|
||
|
|
case(TEST_COMMAND_1): {
|
||
|
|
if(fullInfoPrintout) {
|
||
|
|
#if FSFW_CPP_OSTREAM_ENABLED == 1
|
||
|
|
sif::info << "TestDevice" << deviceIdx << "::scanForReply: Reply for data command "
|
||
|
|
"(ID " << TEST_COMMAND_1 << ") received!" << std::endl;
|
||
|
|
#else
|
||
|
|
sif::printInfo("TestDevice%d::scanForReply: Reply for data command (ID %d) "
|
||
|
|
"received\n", deviceIdx, TEST_COMMAND_1);
|
||
|
|
#endif
|
||
|
|
}
|
||
|
|
|
||
|
|
*foundLen = len;
|
||
|
|
*foundId = pendingCmd;
|
||
|
|
return RETURN_OK;
|
||
|
|
}
|
||
|
|
|
||
|
|
default:
|
||
|
|
return DeviceHandlerIF::DEVICE_REPLY_INVALID;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
ReturnValue_t TestDevice::interpretDeviceReply(DeviceCommandId_t id,
|
||
|
|
const uint8_t* packet) {
|
||
|
|
ReturnValue_t result = HasReturnvaluesIF::RETURN_OK;
|
||
|
|
switch(id) {
|
||
|
|
/* Periodic replies */
|
||
|
|
case testdevice::TEST_NORMAL_MODE_CMD: {
|
||
|
|
result = interpretingNormalModeReply();
|
||
|
|
break;
|
||
|
|
}
|
||
|
|
/* Simple reply */
|
||
|
|
case testdevice::TEST_COMMAND_0: {
|
||
|
|
result = interpretingTestReply0(id, packet);
|
||
|
|
break;
|
||
|
|
}
|
||
|
|
/* Data reply */
|
||
|
|
case testdevice::TEST_COMMAND_1: {
|
||
|
|
result = interpretingTestReply1(id, packet);
|
||
|
|
break;
|
||
|
|
}
|
||
|
|
default:
|
||
|
|
return DeviceHandlerIF::DEVICE_REPLY_INVALID;
|
||
|
|
}
|
||
|
|
return result;
|
||
|
|
}
|
||
|
|
|
||
|
|
ReturnValue_t TestDevice::interpretingNormalModeReply() {
|
||
|
|
CommandMessage directReplyMessage;
|
||
|
|
if(changingDatasets) {
|
||
|
|
PoolReadGuard readHelper(&dataset);
|
||
|
|
if(dataset.testUint8Var.value == 0) {
|
||
|
|
dataset.testUint8Var.value = 10;
|
||
|
|
dataset.testUint32Var.value = 777;
|
||
|
|
dataset.testFloat3Vec.value[0] = 2.5;
|
||
|
|
dataset.testFloat3Vec.value[1] = -2.5;
|
||
|
|
dataset.testFloat3Vec.value[2] = 2.5;
|
||
|
|
dataset.setValidity(true, true);
|
||
|
|
}
|
||
|
|
else {
|
||
|
|
dataset.testUint8Var.value = 0;
|
||
|
|
dataset.testUint32Var.value = 0;
|
||
|
|
dataset.testFloat3Vec.value[0] = 0.0;
|
||
|
|
dataset.testFloat3Vec.value[1] = 0.0;
|
||
|
|
dataset.testFloat3Vec.value[2] = 0.0;
|
||
|
|
dataset.setValidity(false, true);
|
||
|
|
}
|
||
|
|
return RETURN_OK;
|
||
|
|
}
|
||
|
|
|
||
|
|
PoolReadGuard readHelper(&dataset);
|
||
|
|
if(dataset.testUint8Var.value == 0) {
|
||
|
|
/* Reset state */
|
||
|
|
dataset.testUint8Var.value = 128;
|
||
|
|
}
|
||
|
|
else if(dataset.testUint8Var.value > 200) {
|
||
|
|
if(not resetAfterChange) {
|
||
|
|
/* This will trigger an update notification to the controller */
|
||
|
|
dataset.testUint8Var.setChanged(true);
|
||
|
|
resetAfterChange = true;
|
||
|
|
/* Decrement by 30 automatically. This will prevent any additional notifications. */
|
||
|
|
dataset.testUint8Var.value -= 30;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
/* If the value is greater than 0, it will be decremented in a linear way */
|
||
|
|
else if(dataset.testUint8Var.value > 128) {
|
||
|
|
size_t sizeToDecrement = 0;
|
||
|
|
if(dataset.testUint8Var.value > 128 + 30) {
|
||
|
|
sizeToDecrement = 30;
|
||
|
|
}
|
||
|
|
else {
|
||
|
|
sizeToDecrement = dataset.testUint8Var.value - 128;
|
||
|
|
resetAfterChange = false;
|
||
|
|
}
|
||
|
|
dataset.testUint8Var.value -= sizeToDecrement;
|
||
|
|
}
|
||
|
|
else if(dataset.testUint8Var.value < 50) {
|
||
|
|
if(not resetAfterChange) {
|
||
|
|
/* This will trigger an update snapshot to the controller */
|
||
|
|
dataset.testUint8Var.setChanged(true);
|
||
|
|
resetAfterChange = true;
|
||
|
|
}
|
||
|
|
else {
|
||
|
|
/* Increment by 30 automatically. */
|
||
|
|
dataset.testUint8Var.value += 30;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
/* Increment in linear way */
|
||
|
|
else if(dataset.testUint8Var.value < 128) {
|
||
|
|
size_t sizeToIncrement = 0;
|
||
|
|
if(dataset.testUint8Var.value < 128 - 20) {
|
||
|
|
sizeToIncrement = 20;
|
||
|
|
}
|
||
|
|
else {
|
||
|
|
sizeToIncrement = 128 - dataset.testUint8Var.value;
|
||
|
|
resetAfterChange = false;
|
||
|
|
}
|
||
|
|
dataset.testUint8Var.value += sizeToIncrement;
|
||
|
|
}
|
||
|
|
|
||
|
|
/* TODO: Same for vector */
|
||
|
|
float vectorMean = (dataset.testFloat3Vec.value[0] + dataset.testFloat3Vec.value[1] +
|
||
|
|
dataset.testFloat3Vec.value[2]) / 3.0;
|
||
|
|
|
||
|
|
/* Lambda (private local function) */
|
||
|
|
auto sizeToAdd = [](bool tooHigh, float currentVal) {
|
||
|
|
if(tooHigh) {
|
||
|
|
if(currentVal - 20.0 > 10.0) {
|
||
|
|
return -10.0;
|
||
|
|
}
|
||
|
|
else {
|
||
|
|
return 20.0 - currentVal;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
else {
|
||
|
|
if(std::abs(currentVal + 20.0) > 10.0) {
|
||
|
|
return 10.0;
|
||
|
|
}
|
||
|
|
else {
|
||
|
|
return -20.0 - currentVal;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
};
|
||
|
|
|
||
|
|
if(vectorMean > 20.0 and std::abs(vectorMean - 20.0) > 1.0) {
|
||
|
|
if(not resetAfterChange) {
|
||
|
|
dataset.testFloat3Vec.setChanged(true);
|
||
|
|
resetAfterChange = true;
|
||
|
|
}
|
||
|
|
else {
|
||
|
|
float sizeToDecrementVal0 = 0;
|
||
|
|
float sizeToDecrementVal1 = 0;
|
||
|
|
float sizeToDecrementVal2 = 0;
|
||
|
|
|
||
|
|
sizeToDecrementVal0 = sizeToAdd(true, dataset.testFloat3Vec.value[0]);
|
||
|
|
sizeToDecrementVal1 = sizeToAdd(true, dataset.testFloat3Vec.value[1]);
|
||
|
|
sizeToDecrementVal2 = sizeToAdd(true, dataset.testFloat3Vec.value[2]);
|
||
|
|
|
||
|
|
dataset.testFloat3Vec.value[0] += sizeToDecrementVal0;
|
||
|
|
dataset.testFloat3Vec.value[1] += sizeToDecrementVal1;
|
||
|
|
dataset.testFloat3Vec.value[2] += sizeToDecrementVal2;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
else if (vectorMean < -20.0 and std::abs(vectorMean + 20.0) < 1.0) {
|
||
|
|
if(not resetAfterChange) {
|
||
|
|
dataset.testFloat3Vec.setChanged(true);
|
||
|
|
resetAfterChange = true;
|
||
|
|
}
|
||
|
|
else {
|
||
|
|
float sizeToDecrementVal0 = 0;
|
||
|
|
float sizeToDecrementVal1 = 0;
|
||
|
|
float sizeToDecrementVal2 = 0;
|
||
|
|
|
||
|
|
sizeToDecrementVal0 = sizeToAdd(false, dataset.testFloat3Vec.value[0]);
|
||
|
|
sizeToDecrementVal1 = sizeToAdd(false, dataset.testFloat3Vec.value[1]);
|
||
|
|
sizeToDecrementVal2 = sizeToAdd(false, dataset.testFloat3Vec.value[2]);
|
||
|
|
|
||
|
|
dataset.testFloat3Vec.value[0] += sizeToDecrementVal0;
|
||
|
|
dataset.testFloat3Vec.value[1] += sizeToDecrementVal1;
|
||
|
|
dataset.testFloat3Vec.value[2] += sizeToDecrementVal2;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
else {
|
||
|
|
if(resetAfterChange) {
|
||
|
|
resetAfterChange = false;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
return RETURN_OK;
|
||
|
|
}
|
||
|
|
|
||
|
|
ReturnValue_t TestDevice::interpretingTestReply0(DeviceCommandId_t id, const uint8_t* packet) {
|
||
|
|
CommandMessage commandMessage;
|
||
|
|
if(fullInfoPrintout) {
|
||
|
|
#if FSFW_CPP_OSTREAM_ENABLED == 1
|
||
|
|
sif::info << "TestDevice::interpretingTestReply0: Generating step and finish reply" <<
|
||
|
|
std::endl;
|
||
|
|
#else
|
||
|
|
sif::printInfo("TestDevice::interpretingTestReply0: Generating step and finish reply\n");
|
||
|
|
#endif
|
||
|
|
}
|
||
|
|
|
||
|
|
MessageQueueId_t commander = getCommanderId(id);
|
||
|
|
/* Generate one step reply and the finish reply */
|
||
|
|
actionHelper.step(1, commander, id);
|
||
|
|
actionHelper.finish(true, commander, id);
|
||
|
|
|
||
|
|
return RETURN_OK;
|
||
|
|
}
|
||
|
|
|
||
|
|
ReturnValue_t TestDevice::interpretingTestReply1(DeviceCommandId_t id,
|
||
|
|
const uint8_t* packet) {
|
||
|
|
CommandMessage directReplyMessage;
|
||
|
|
if(fullInfoPrintout) {
|
||
|
|
#if FSFW_CPP_OSTREAM_ENABLED == 1
|
||
|
|
sif::info << "TestDevice" << deviceIdx << "::interpretingReply1: Setting data reply" <<
|
||
|
|
std::endl;
|
||
|
|
#else
|
||
|
|
sif::printInfo("TestDevice%d::interpretingReply1: Setting data reply\n", deviceIdx);
|
||
|
|
#endif
|
||
|
|
}
|
||
|
|
|
||
|
|
MessageQueueId_t commander = getCommanderId(id);
|
||
|
|
/* Send reply with data */
|
||
|
|
ReturnValue_t result = actionHelper.reportData(commander, id, packet,
|
||
|
|
testdevice::TEST_COMMAND_1_SIZE, false);
|
||
|
|
|
||
|
|
if (result != RETURN_OK) {
|
||
|
|
#if FSFW_CPP_OSTREAM_ENABLED == 1
|
||
|
|
sif::error << "TestDevice" << deviceIdx << "::interpretingReply1: Sending data "
|
||
|
|
"reply failed!" << std::endl;
|
||
|
|
#else
|
||
|
|
sif::printError("TestDevice%d::interpretingReply1: Sending data reply failed!\n",
|
||
|
|
deviceIdx);
|
||
|
|
#endif
|
||
|
|
return result;
|
||
|
|
}
|
||
|
|
|
||
|
|
if(result == HasReturnvaluesIF::RETURN_OK) {
|
||
|
|
/* Finish reply */
|
||
|
|
actionHelper.finish(true, commander, id);
|
||
|
|
}
|
||
|
|
else {
|
||
|
|
/* Finish reply */
|
||
|
|
actionHelper.finish(false, commander, id, result);
|
||
|
|
}
|
||
|
|
|
||
|
|
return RETURN_OK;
|
||
|
|
}
|
||
|
|
|
||
|
|
uint32_t TestDevice::getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) {
|
||
|
|
return 5000;
|
||
|
|
}
|
||
|
|
|
||
|
|
void TestDevice::enableFullDebugOutput(bool enable) {
|
||
|
|
this->fullInfoPrintout = enable;
|
||
|
|
}
|
||
|
|
|
||
|
|
ReturnValue_t TestDevice::initializeLocalDataPool(localpool::DataPool &localDataPoolMap,
|
||
|
|
LocalDataPoolManager &poolManager) {
|
||
|
|
namespace td = testdevice;
|
||
|
|
localDataPoolMap.emplace(td::PoolIds::TEST_UINT8_ID, new PoolEntry<uint8_t>({0}));
|
||
|
|
localDataPoolMap.emplace(td::PoolIds::TEST_UINT32_ID, new PoolEntry<uint32_t>({0}));
|
||
|
|
localDataPoolMap.emplace(td::PoolIds::TEST_FLOAT_VEC_3_ID,
|
||
|
|
new PoolEntry<float>({0.0, 0.0, 0.0}));
|
||
|
|
|
||
|
|
sid_t sid;
|
||
|
|
if(deviceIdx == td::DeviceIndex::DEVICE_0) {
|
||
|
|
sid = td::TEST_SET_DEV_0_SID;
|
||
|
|
}
|
||
|
|
else {
|
||
|
|
sid = td::TEST_SET_DEV_1_SID;
|
||
|
|
}
|
||
|
|
/* Subscribe for periodic HK packets but do not enable reporting for now.
|
||
|
|
Non-diangostic with a period of one second */
|
||
|
|
poolManager.subscribeForPeriodicPacket(sid, false, 1.0, false);
|
||
|
|
return HasReturnvaluesIF::RETURN_OK;
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
ReturnValue_t TestDevice::getParameter(uint8_t domainId, uint8_t uniqueId,
|
||
|
|
ParameterWrapper* parameterWrapper, const ParameterWrapper* newValues,
|
||
|
|
uint16_t startAtIndex) {
|
||
|
|
using namespace testdevice;
|
||
|
|
switch (uniqueId) {
|
||
|
|
case ParameterUniqueIds::TEST_UINT32_0: {
|
||
|
|
if(fullInfoPrintout) {
|
||
|
|
uint32_t newValue = 0;
|
||
|
|
ReturnValue_t result = newValues->getElement<uint32_t>(&newValue, 0, 0);
|
||
|
|
if(result == HasReturnvaluesIF::RETURN_OK) {
|
||
|
|
#if FSFW_CPP_OSTREAM_ENABLED == 1
|
||
|
|
sif::info << "TestDevice" << deviceIdx << "::getParameter: Setting parameter 0 to "
|
||
|
|
"new value " << newValue << std::endl;
|
||
|
|
#else
|
||
|
|
sif::printInfo("TestDevice%d::getParameter: Setting parameter 0 to new value %lu\n",
|
||
|
|
deviceIdx, static_cast<unsigned long>(newValue));
|
||
|
|
#endif /* FSFW_CPP_OSTREAM_ENABLED == 1 */
|
||
|
|
}
|
||
|
|
}
|
||
|
|
parameterWrapper->set(testParameter0);
|
||
|
|
break;
|
||
|
|
}
|
||
|
|
case ParameterUniqueIds::TEST_INT32_1: {
|
||
|
|
if(fullInfoPrintout) {
|
||
|
|
int32_t newValue = 0;
|
||
|
|
ReturnValue_t result = newValues->getElement<int32_t>(&newValue, 0, 0);
|
||
|
|
if(result == HasReturnvaluesIF::RETURN_OK) {
|
||
|
|
#if OBSW_DEVICE_HANDLER_PRINTOUT == 1
|
||
|
|
#if FSFW_CPP_OSTREAM_ENABLED == 1
|
||
|
|
sif::info << "TestDevice" << deviceIdx << "::getParameter: Setting parameter 1 to "
|
||
|
|
"new value " << newValue << std::endl;
|
||
|
|
#else
|
||
|
|
sif::printInfo("TestDevice%d::getParameter: Setting parameter 1 to new value %lu\n",
|
||
|
|
deviceIdx, static_cast<unsigned long>(newValue));
|
||
|
|
#endif /* FSFW_CPP_OSTREAM_ENABLED == 1 */
|
||
|
|
#endif /* OBSW_DEVICE_HANDLER_PRINTOUT == 1 */
|
||
|
|
}
|
||
|
|
}
|
||
|
|
parameterWrapper->set(testParameter1);
|
||
|
|
break;
|
||
|
|
}
|
||
|
|
case ParameterUniqueIds::TEST_FLOAT_VEC3_2: {
|
||
|
|
if(fullInfoPrintout) {
|
||
|
|
float newVector[3];
|
||
|
|
if(newValues->getElement<float>(newVector, 0, 0) != RETURN_OK or
|
||
|
|
newValues->getElement<float>(newVector + 1, 0, 1) != RETURN_OK or
|
||
|
|
newValues->getElement<float>(newVector + 2, 0, 2) != RETURN_OK) {
|
||
|
|
return HasReturnvaluesIF::RETURN_FAILED;
|
||
|
|
}
|
||
|
|
#if OBSW_DEVICE_HANDLER_PRINTOUT == 1
|
||
|
|
#if FSFW_CPP_OSTREAM_ENABLED == 1
|
||
|
|
sif::info << "TestDevice" << deviceIdx << "::getParameter: Setting parameter 3 to "
|
||
|
|
"(float vector with 3 entries) to new values [" << newVector[0] << ", " <<
|
||
|
|
newVector[1] << ", " << newVector[2] << "]" << std::endl;
|
||
|
|
#else
|
||
|
|
sif::printInfo("TestDevice%d::getParameter: Setting parameter 3 to new values "
|
||
|
|
"[%f, %f, %f]\n", deviceIdx, newVector[0], newVector[1], newVector[2]);
|
||
|
|
#endif /* FSFW_CPP_OSTREAM_ENABLED == 1 */
|
||
|
|
#endif /* OBSW_DEVICE_HANDLER_PRINTOUT == 1 */
|
||
|
|
}
|
||
|
|
parameterWrapper->setVector(vectorFloatParams2);
|
||
|
|
break;
|
||
|
|
}
|
||
|
|
case(ParameterUniqueIds::PERIODIC_PRINT_ENABLED): {
|
||
|
|
if(fullInfoPrintout) {
|
||
|
|
uint8_t enabled = 0;
|
||
|
|
ReturnValue_t result = newValues->getElement<uint8_t>(&enabled, 0, 0);
|
||
|
|
if(result != HasReturnvaluesIF::RETURN_OK) {
|
||
|
|
return result;
|
||
|
|
}
|
||
|
|
char const* printout = nullptr;
|
||
|
|
if (enabled) {
|
||
|
|
printout = "enabled";
|
||
|
|
}
|
||
|
|
else {
|
||
|
|
printout = "disabled";
|
||
|
|
}
|
||
|
|
#if FSFW_CPP_OSTREAM_ENABLED == 1
|
||
|
|
sif::info << "TestDevice" << deviceIdx << "::getParameter: Periodic printout " <<
|
||
|
|
printout << std::endl;
|
||
|
|
#else
|
||
|
|
sif::printInfo("TestDevice%d::getParameter: Periodic printout %s", printout);
|
||
|
|
#endif /* FSFW_CPP_OSTREAM_ENABLED == 1 */
|
||
|
|
}
|
||
|
|
|
||
|
|
parameterWrapper->set(periodicPrintout);
|
||
|
|
break;
|
||
|
|
}
|
||
|
|
case(ParameterUniqueIds::CHANGING_DATASETS): {
|
||
|
|
|
||
|
|
uint8_t enabled = 0;
|
||
|
|
ReturnValue_t result = newValues->getElement<uint8_t>(&enabled, 0, 0);
|
||
|
|
if(result != HasReturnvaluesIF::RETURN_OK) {
|
||
|
|
return result;
|
||
|
|
}
|
||
|
|
if(not enabled) {
|
||
|
|
PoolReadGuard readHelper(&dataset);
|
||
|
|
dataset.testUint8Var.value = 0;
|
||
|
|
dataset.testUint32Var.value = 0;
|
||
|
|
dataset.testFloat3Vec.value[0] = 0.0;
|
||
|
|
dataset.testFloat3Vec.value[0] = 0.0;
|
||
|
|
dataset.testFloat3Vec.value[1] = 0.0;
|
||
|
|
}
|
||
|
|
|
||
|
|
if(fullInfoPrintout) {
|
||
|
|
char const* printout = nullptr;
|
||
|
|
if (enabled) {
|
||
|
|
printout = "enabled";
|
||
|
|
}
|
||
|
|
else {
|
||
|
|
printout = "disabled";
|
||
|
|
}
|
||
|
|
#if FSFW_CPP_OSTREAM_ENABLED == 1
|
||
|
|
sif::info << "TestDevice" << deviceIdx << "::getParameter: Changing datasets " <<
|
||
|
|
printout << std::endl;
|
||
|
|
#else
|
||
|
|
sif::printInfo("TestDevice%d::getParameter: Changing datasets %s", printout);
|
||
|
|
#endif /* FSFW_CPP_OSTREAM_ENABLED == 1 */
|
||
|
|
}
|
||
|
|
|
||
|
|
parameterWrapper->set(changingDatasets);
|
||
|
|
break;
|
||
|
|
}
|
||
|
|
default:
|
||
|
|
return INVALID_IDENTIFIER_ID;
|
||
|
|
}
|
||
|
|
return HasReturnvaluesIF::RETURN_OK;
|
||
|
|
}
|
||
|
|
|
||
|
|
LocalPoolObjectBase* TestDevice::getPoolObjectHandle(lp_id_t localPoolId) {
|
||
|
|
namespace td = testdevice;
|
||
|
|
if (localPoolId == td::PoolIds::TEST_UINT8_ID) {
|
||
|
|
return &dataset.testUint8Var;
|
||
|
|
}
|
||
|
|
else if (localPoolId == td::PoolIds::TEST_UINT32_ID) {
|
||
|
|
return &dataset.testUint32Var;
|
||
|
|
}
|
||
|
|
else if(localPoolId == td::PoolIds::TEST_FLOAT_VEC_3_ID) {
|
||
|
|
return &dataset.testFloat3Vec;
|
||
|
|
}
|
||
|
|
else {
|
||
|
|
return nullptr;
|
||
|
|
}
|
||
|
|
}
|