tmtc merge conflict
This commit is contained in:
commit
f43dbba8e2
23
README.md
23
README.md
@ -277,7 +277,7 @@ Copying a file from Q7S to flatsat PC
|
||||
scp -P 22 root@192.168.133.10:/tmp/kernel-config /tmp
|
||||
````
|
||||
|
||||
From a windows machine files can be copied with putty tools
|
||||
From a windows machine files can be copied with putty tools (note: use IPv4 address)
|
||||
````
|
||||
pscp -scp -P 22 eive@192.168.199.227:</directory-to-example-file/>/example-file </windows-machine-path/>
|
||||
````
|
||||
@ -579,4 +579,23 @@ to install the required GPIO libraries before cloning the system root folder.
|
||||
When using Eclipse, there are two special build variables in the project properties
|
||||
→ C/C++ Build → Build Variables called `Q7S_SYSROOT` or `RPI_SYSROOT`. You can set
|
||||
the sysroot path in those variables to get any additional includes like `gpiod.h` in the
|
||||
Eclipse indexer.
|
||||
Eclipse indexer.
|
||||
|
||||
## Xilinx UARTLIE
|
||||
Get info about ttyUL* devices
|
||||
````
|
||||
cat /proc/tty/driver
|
||||
````
|
||||
|
||||
## I2C
|
||||
Getting information about I2C device
|
||||
````
|
||||
ls /sys/class/i2c-dev/i2c-0/device/device/driver
|
||||
````
|
||||
This shows the memory mapping of /dev/i2c-0
|
||||
|
||||
## Useful Q7S Linux Commands
|
||||
Rebooting currently running image:
|
||||
````
|
||||
xsc_boot_copy -r
|
||||
````
|
@ -135,6 +135,8 @@ void initmission::initTasks() {
|
||||
initmission::printAddObjectError("PUS_17", objects::PUS_SERVICE_17_TEST);
|
||||
}
|
||||
|
||||
#if TE0720 == 0
|
||||
|
||||
//TODO: Add handling of missed deadlines
|
||||
/* Polling Sequence Table Default */
|
||||
#if Q7S_ADD_SPI_TEST == 0
|
||||
@ -147,7 +149,6 @@ void initmission::initTasks() {
|
||||
}
|
||||
#endif
|
||||
|
||||
#if TE0720 == 0
|
||||
FixedTimeslotTaskIF* gomSpacePstTask = factory->
|
||||
createFixedTimeslotTask("GS_PST_TASK", 50,
|
||||
PeriodicTaskIF::MINIMUM_STACK_SIZE*8, 3.0, missedDeadlineFunc);
|
||||
@ -155,6 +156,15 @@ void initmission::initTasks() {
|
||||
if(result != HasReturnvaluesIF::RETURN_OK) {
|
||||
sif::error << "InitMission::initTasks: GomSpace PST initialization failed!" << std::endl;
|
||||
}
|
||||
|
||||
#else
|
||||
FixedTimeslotTaskIF * pollingSequenceTableTE0720 = factory->createFixedTimeslotTask(
|
||||
"PST_TASK_TE0720", 50, PeriodicTaskIF::MINIMUM_STACK_SIZE * 4, 3.0,
|
||||
missedDeadlineFunc);
|
||||
result = pst::pollingSequenceTE0720(pollingSequenceTableTE0720);
|
||||
if (result != HasReturnvaluesIF::RETURN_OK) {
|
||||
sif::error << "InitMission::initTasks: Creating TE0720 PST failed!" << std::endl;
|
||||
}
|
||||
#endif
|
||||
|
||||
#if OBSW_ADD_TEST_CODE == 1
|
||||
@ -184,12 +194,11 @@ void initmission::initTasks() {
|
||||
udpBridgeTask->startTask();
|
||||
udpPollingTask->startTask();
|
||||
|
||||
#if TE0720 == 0
|
||||
#if TE0720 == 0 && Q7S_ADD_SPI_TEST == 0
|
||||
gomSpacePstTask->startTask();
|
||||
#endif
|
||||
|
||||
#if Q7S_ADD_SPI_TEST == 0
|
||||
pollingSequenceTableTaskDefault->startTask();
|
||||
#elif TE0720 == 1 && Q7S_ADD_SPI_TEST == 0
|
||||
pollingSequenceTableTE0720->startTask();
|
||||
#endif
|
||||
|
||||
pusVerification->startTask();
|
||||
|
@ -25,9 +25,11 @@
|
||||
#include <mission/devices/MGMHandlerLIS3MDL.h>
|
||||
#include <mission/devices/MGMHandlerRM3100.h>
|
||||
#include <mission/devices/GyroL3GD20Handler.h>
|
||||
#include <mission/devices/PlocHandler.h>
|
||||
|
||||
#include <mission/devices/devicedefinitions/GomspaceDefinitions.h>
|
||||
#include <mission/devices/devicedefinitions/SyrlinksDefinitions.h>
|
||||
#include <mission/devices/devicedefinitions/PlocDefinitions.h>
|
||||
#include <mission/utility/TmFunnel.h>
|
||||
|
||||
#include <linux/csp/CspCookie.h>
|
||||
@ -406,17 +408,23 @@ void ObjectFactory::produce(){
|
||||
|
||||
#endif /* Q7S_ADD_RTD_DEVICES == 1 */
|
||||
|
||||
I2cCookie* imtqI2cCookie = new I2cCookie(addresses::IMTQ, IMTQ::MAX_REPLY_SIZE,
|
||||
std::string("/dev/i2c-0"));
|
||||
IMTQHandler* imtqHandler = new IMTQHandler(objects::IMTQ_HANDLER, objects::I2C_COM_IF, imtqI2cCookie);
|
||||
imtqHandler->setStartUpImmediately();
|
||||
|
||||
UartCookie* plocUartCookie = new UartCookie(std::string("/dev/ttyUL3"), 115200,
|
||||
PLOC::MAX_REPLY_SIZE);
|
||||
PlocHandler* plocHandler = new PlocHandler(objects::PLOC_HANDLER, objects::UART_COM_IF,
|
||||
plocUartCookie);
|
||||
plocHandler->setStartUpImmediately();
|
||||
|
||||
#endif /* TE0720 == 0 */
|
||||
|
||||
new UdpTmTcBridge(objects::UDP_BRIDGE, objects::CCSDS_PACKET_DISTRIBUTOR, objects::TM_STORE,
|
||||
objects::TC_STORE);
|
||||
new UdpTcPollingTask(objects::UDP_POLLING_TASK, objects::UDP_BRIDGE);
|
||||
|
||||
I2cCookie* imtqI2cCookie = new I2cCookie(addresses::IMTQ, IMTQ::MAX_REPLY_SIZE,
|
||||
std::string("/dev/i2c-0"));
|
||||
IMTQHandler* imtqHandler = new IMTQHandler(objects::IMTQ_HANDLER, objects::I2C_COM_IF, imtqI2cCookie);
|
||||
imtqHandler->setStartUpImmediately();
|
||||
|
||||
#if TE0720 == 1 && TEST_LIBGPIOD == 1
|
||||
/* Configure MIO0 as input */
|
||||
GpiodRegular gpioConfigMio0(std::string("gpiochip0"), 0,
|
||||
@ -425,12 +433,22 @@ void ObjectFactory::produce(){
|
||||
gpioCookie->addGpio(gpioIds::TEST_ID_0, gpioConfigMio0);
|
||||
new LibgpiodTest(objects::LIBGPIOD_TEST, objects::GPIO_IF, gpioCookie);
|
||||
#elif TE0720 == 1
|
||||
|
||||
UartCookie* plocUartCookie = new UartCookie(std::string("/dev/ttyPS1"), 115200,
|
||||
PLOC::MAX_REPLY_SIZE);
|
||||
/* Testing PlocHandler on TE0720-03-1CFA */
|
||||
PlocHandler* plocHandler = new PlocHandler(objects::PLOC_HANDLER, objects::UART_COM_IF,
|
||||
plocUartCookie);
|
||||
plocHandler->setStartUpImmediately();
|
||||
#endif
|
||||
|
||||
#if TE0720 == 1 && TE0720_HEATER_TEST == 1
|
||||
/* Configuration for MIO0 on TE0720-03-1CFA */
|
||||
GpiodRegular gpioConfigForDummyHeater(std::string("gpiochip0"), 0,
|
||||
std::string("Heater0"), gpio::OUT, 0);
|
||||
heaterGpiosCookie->addGpio(gpioIds::HEATER_0, gpioConfigForDummyHeater);
|
||||
new HeaterHandler(objects::HEATER_HANDLER, objects::GPIO_IF, heaterGpiosCookie,
|
||||
objects::PCDU_HANDLER, pcduSwitches::TCS_BOARD_8V_HEATER_IN);
|
||||
GpiodRegular* heaterGpio = new GpiodRegular(std::string("gpiochip0"), 0, std::string("MIO0"), gpio::IN, 0);
|
||||
GpioCookie* gpioCookie = new GpioCookie;
|
||||
gpioCookie->addGpio(gpioIds::HEATER_0, heaterGpio);
|
||||
new HeaterHandler(objects::HEATER_HANDLER, objects::GPIO_IF, gpioCookie, objects::PCDU_HANDLER,
|
||||
pcduSwitches::TCS_BOARD_8V_HEATER_IN);
|
||||
#endif
|
||||
|
||||
#if Q7S_ADD_SPI_TEST == 1
|
||||
|
@ -58,8 +58,9 @@ ReturnValue_t HeaterHandler::initialize() {
|
||||
if(mainLineSwitcherObjectId != objects::NO_OBJECT) {
|
||||
mainLineSwitcher = objectManager->get<PowerSwitchIF>(mainLineSwitcherObjectId);
|
||||
if (mainLineSwitcher == nullptr) {
|
||||
sif::error << "HeaterHandler::initialize: Main line switcher failed to fetch object"
|
||||
<< "from object ID." << std::endl;
|
||||
sif::error
|
||||
<< "HeaterHandler::initialize: Failed to get main line switcher. Make sure "
|
||||
<< "main line switcher object is initialized." << std::endl;
|
||||
return ObjectManagerIF::CHILD_INIT_FAILED;
|
||||
}
|
||||
}
|
||||
|
@ -21,7 +21,8 @@ debugging. */
|
||||
#define OBSW_ADD_TEST_CODE 1
|
||||
#define TEST_LIBGPIOD 0
|
||||
|
||||
#define TE0720 0
|
||||
#define TE0720 1
|
||||
#define TE0720_HEATER_TEST 0
|
||||
|
||||
#define P60DOCK_DEBUG 0
|
||||
#define PDU1_DEBUG 0
|
||||
|
@ -21,7 +21,8 @@ enum: uint8_t {
|
||||
MGM_RM3100,
|
||||
PCDU_HANDLER,
|
||||
HEATER_HANDLER,
|
||||
SA_DEPL_HANDLER
|
||||
SA_DEPL_HANDLER,
|
||||
PLOC_HANDLER
|
||||
};
|
||||
}
|
||||
|
||||
|
@ -52,6 +52,7 @@ namespace objects {
|
||||
GYRO_2_L3G_HANDLER = 0x44000013,
|
||||
|
||||
IMTQ_HANDLER = 0x44000014,
|
||||
PLOC_HANDLER = 0x44000015,
|
||||
|
||||
/* Custom device handler */
|
||||
PCDU_HANDLER = 0x44001000,
|
||||
|
@ -5,13 +5,13 @@
|
||||
#include <fsfw/devicehandlers/DeviceHandlerIF.h>
|
||||
#include <fsfw/tasks/FixedTimeslotTaskIF.h>
|
||||
#include <fsfwconfig/objects/systemObjectList.h>
|
||||
#include <fsfwconfig/OBSWConfig.h>
|
||||
|
||||
|
||||
ReturnValue_t pst::pollingSequenceInitDefault(FixedTimeslotTaskIF *thisSequence)
|
||||
{
|
||||
/* Length of a communication cycle */
|
||||
uint32_t length = thisSequence->getPeriodMs();
|
||||
|
||||
thisSequence->addSlot(objects::TMP1075_HANDLER_1, length * 0,
|
||||
DeviceHandlerIF::PERFORM_OPERATION);
|
||||
thisSequence->addSlot(objects::TMP1075_HANDLER_2, length * 0,
|
||||
@ -40,6 +40,7 @@ ReturnValue_t pst::pollingSequenceInitDefault(FixedTimeslotTaskIF *thisSequence)
|
||||
#endif /* Q7S_ADD_RTD_DEVICES */
|
||||
|
||||
thisSequence->addSlot(objects::IMTQ_HANDLER, length * 0, DeviceHandlerIF::PERFORM_OPERATION);
|
||||
thisSequence->addSlot(objects::PLOC_HANDLER, length * 0, DeviceHandlerIF::PERFORM_OPERATION);
|
||||
|
||||
thisSequence->addSlot(objects::TMP1075_HANDLER_1, length * 0.2, DeviceHandlerIF::SEND_WRITE);
|
||||
thisSequence->addSlot(objects::TMP1075_HANDLER_2, length * 0.2, DeviceHandlerIF::SEND_WRITE);
|
||||
@ -64,6 +65,7 @@ ReturnValue_t pst::pollingSequenceInitDefault(FixedTimeslotTaskIF *thisSequence)
|
||||
#endif /* Q7S_ADD_RTD_DEVICES */
|
||||
|
||||
thisSequence->addSlot(objects::IMTQ_HANDLER, length * 0.2, DeviceHandlerIF::SEND_WRITE);
|
||||
thisSequence->addSlot(objects::PLOC_HANDLER, length * 0.2, DeviceHandlerIF::SEND_WRITE);
|
||||
|
||||
thisSequence->addSlot(objects::TMP1075_HANDLER_1, length * 0.4, DeviceHandlerIF::GET_WRITE);
|
||||
thisSequence->addSlot(objects::TMP1075_HANDLER_2, length * 0.4, DeviceHandlerIF::GET_WRITE);
|
||||
@ -85,9 +87,11 @@ ReturnValue_t pst::pollingSequenceInitDefault(FixedTimeslotTaskIF *thisSequence)
|
||||
thisSequence->addSlot(objects::RTD_IC16, length * 0.4, DeviceHandlerIF::GET_WRITE);
|
||||
thisSequence->addSlot(objects::RTD_IC17, length * 0.4, DeviceHandlerIF::GET_WRITE);
|
||||
thisSequence->addSlot(objects::RTD_IC18, length * 0.4, DeviceHandlerIF::GET_WRITE);
|
||||
thisSequence->addSlot(objects::IMTQ_HANDLER, length * 0.4, DeviceHandlerIF::GET_WRITE);
|
||||
#endif /* Q7S_ADD_RTD_DEVICES */
|
||||
|
||||
thisSequence->addSlot(objects::IMTQ_HANDLER, length * 0.4, DeviceHandlerIF::GET_WRITE);
|
||||
thisSequence->addSlot(objects::PLOC_HANDLER, length * 0.4, DeviceHandlerIF::GET_WRITE);
|
||||
|
||||
thisSequence->addSlot(objects::TMP1075_HANDLER_1, length * 0.6, DeviceHandlerIF::SEND_READ);
|
||||
thisSequence->addSlot(objects::TMP1075_HANDLER_2, length * 0.6, DeviceHandlerIF::SEND_READ);
|
||||
|
||||
@ -111,6 +115,7 @@ ReturnValue_t pst::pollingSequenceInitDefault(FixedTimeslotTaskIF *thisSequence)
|
||||
#endif /* Q7S_ADD_RTD_DEVICES */
|
||||
|
||||
thisSequence->addSlot(objects::IMTQ_HANDLER, length * 0.6, DeviceHandlerIF::SEND_READ);
|
||||
thisSequence->addSlot(objects::PLOC_HANDLER, length * 0.6, DeviceHandlerIF::SEND_READ);
|
||||
|
||||
thisSequence->addSlot(objects::TMP1075_HANDLER_1, length * 0.8, DeviceHandlerIF::GET_READ);
|
||||
thisSequence->addSlot(objects::TMP1075_HANDLER_2, length * 0.8, DeviceHandlerIF::GET_READ);
|
||||
@ -135,6 +140,7 @@ ReturnValue_t pst::pollingSequenceInitDefault(FixedTimeslotTaskIF *thisSequence)
|
||||
#endif /* Q7S_ADD_RTD_DEVICES */
|
||||
|
||||
thisSequence->addSlot(objects::IMTQ_HANDLER, length * 0.8, DeviceHandlerIF::GET_READ);
|
||||
thisSequence->addSlot(objects::PLOC_HANDLER, length * 0.8, DeviceHandlerIF::GET_READ);
|
||||
|
||||
if (thisSequence->checkSequence() == HasReturnvaluesIF::RETURN_OK) {
|
||||
return HasReturnvaluesIF::RETURN_OK;
|
||||
@ -289,3 +295,25 @@ ReturnValue_t pst::pollingSequenceAcsTest(FixedTimeslotTaskIF *thisSequence) {
|
||||
}
|
||||
return HasReturnvaluesIF::RETURN_OK;
|
||||
}
|
||||
|
||||
ReturnValue_t pst::pollingSequenceTE0720(FixedTimeslotTaskIF *thisSequence) {
|
||||
uint32_t length = thisSequence->getPeriodMs();
|
||||
|
||||
thisSequence->addSlot(objects::PLOC_HANDLER, length * 0,
|
||||
DeviceHandlerIF::PERFORM_OPERATION);
|
||||
thisSequence->addSlot(objects::PLOC_HANDLER, length * 0.2,
|
||||
DeviceHandlerIF::SEND_WRITE);
|
||||
thisSequence->addSlot(objects::PLOC_HANDLER, length * 0.4,
|
||||
DeviceHandlerIF::GET_WRITE);
|
||||
thisSequence->addSlot(objects::PLOC_HANDLER, length * 0.6,
|
||||
DeviceHandlerIF::SEND_READ);
|
||||
thisSequence->addSlot(objects::PLOC_HANDLER, length * 0.8,
|
||||
DeviceHandlerIF::GET_READ);
|
||||
|
||||
if (thisSequence->checkSequence() != HasReturnvaluesIF::RETURN_OK) {
|
||||
sif::error << "Initialization of TE0720 PST failed" << std::endl;
|
||||
return HasReturnvaluesIF::RETURN_FAILED;
|
||||
}
|
||||
return HasReturnvaluesIF::RETURN_OK;
|
||||
}
|
||||
|
||||
|
@ -35,6 +35,11 @@ ReturnValue_t pollingSequenceInitDefault(FixedTimeslotTaskIF *thisSequence);
|
||||
ReturnValue_t gomspacePstInit(FixedTimeslotTaskIF *thisSequence);
|
||||
|
||||
ReturnValue_t pollingSequenceAcsTest(FixedTimeslotTaskIF* thisSequence);
|
||||
|
||||
/**
|
||||
* @brief This polling sequence will be created when the software is compiled for the TE0720.
|
||||
*/
|
||||
ReturnValue_t pollingSequenceTE0720(FixedTimeslotTaskIF* thisSequence);
|
||||
}
|
||||
|
||||
|
||||
|
@ -20,6 +20,7 @@ enum {
|
||||
SA_DEPL_HANDLER,
|
||||
SYRLINKS_HANDLER,
|
||||
IMTQ_HANDLER,
|
||||
PLOC_HANDLER
|
||||
};
|
||||
}
|
||||
|
||||
|
@ -363,6 +363,9 @@ ReturnValue_t UartComIF::readReceivedMessage(CookieIF *cookie,
|
||||
*buffer = uartDeviceMapIter->second.replyBuffer.data();
|
||||
*size = uartDeviceMapIter->second.replyLen;
|
||||
|
||||
/* Length is reset to 0 to prevent reading the same data twice */
|
||||
uartDeviceMapIter->second.replyLen = 0;
|
||||
|
||||
return RETURN_OK;
|
||||
}
|
||||
|
||||
|
@ -13,6 +13,7 @@ target_sources(${TARGET_NAME} PUBLIC
|
||||
SyrlinksHkHandler.cpp
|
||||
Max31865PT1000Handler.cpp
|
||||
IMTQHandler.cpp
|
||||
PlocHandler.cpp
|
||||
)
|
||||
|
||||
|
||||
|
@ -6,7 +6,8 @@
|
||||
#include <fsfwconfig/OBSWConfig.h>
|
||||
|
||||
IMTQHandler::IMTQHandler(object_id_t objectId, object_id_t comIF, CookieIF * comCookie) :
|
||||
DeviceHandlerBase(objectId, comIF, comCookie), engHkDataset(this) {
|
||||
DeviceHandlerBase(objectId, comIF, comCookie), engHkDataset(this), calMtmMeasurementSet(
|
||||
this), rawMtmMeasurementSet(this) {
|
||||
if (comCookie == NULL) {
|
||||
sif::error << "IMTQHandler: Invalid com cookie" << std::endl;
|
||||
}
|
||||
@ -20,6 +21,7 @@ void IMTQHandler::doStartUp(){
|
||||
if(mode == _MODE_START_UP){
|
||||
//TODO: Set to MODE_ON again
|
||||
setMode(MODE_NORMAL);
|
||||
communicationStep = CommunicationStep::SELF_TEST;
|
||||
}
|
||||
}
|
||||
|
||||
@ -29,7 +31,32 @@ void IMTQHandler::doShutDown(){
|
||||
|
||||
ReturnValue_t IMTQHandler::buildNormalDeviceCommand(
|
||||
DeviceCommandId_t * id) {
|
||||
*id = IMTQ::GET_ENG_HK_DATA;
|
||||
switch (communicationStep) {
|
||||
case CommunicationStep::SELF_TEST:
|
||||
*id = IMTQ::SELF_TEST;
|
||||
communicationStep = CommunicationStep::GET_ENG_HK_DATA;
|
||||
break;
|
||||
case CommunicationStep::GET_ENG_HK_DATA:
|
||||
*id = IMTQ::GET_ENG_HK_DATA;
|
||||
communicationStep = CommunicationStep::START_MTM_MEASUREMENT;
|
||||
break;
|
||||
case CommunicationStep::START_MTM_MEASUREMENT:
|
||||
*id = IMTQ::START_MTM_MEASUREMENT;
|
||||
communicationStep = CommunicationStep::GET_CAL_MTM_MEASUREMENT;
|
||||
break;
|
||||
case CommunicationStep::GET_CAL_MTM_MEASUREMENT:
|
||||
*id = IMTQ::GET_CAL_MTM_MEASUREMENT;
|
||||
communicationStep = CommunicationStep::GET_RAW_MTM_MEASUREMENT;
|
||||
break;
|
||||
case CommunicationStep::GET_RAW_MTM_MEASUREMENT:
|
||||
*id = IMTQ::GET_RAW_MTM_MEASUREMENT;
|
||||
communicationStep = CommunicationStep::GET_ENG_HK_DATA;
|
||||
break;
|
||||
default:
|
||||
sif::debug << "IMTQHandler::buildNormalDeviceCommand: Invalid communication step"
|
||||
<< std::endl;
|
||||
break;
|
||||
}
|
||||
return buildCommandFromCommand(*id, NULL, 0);
|
||||
}
|
||||
|
||||
@ -42,25 +69,49 @@ ReturnValue_t IMTQHandler::buildCommandFromCommand(
|
||||
DeviceCommandId_t deviceCommand, const uint8_t * commandData,
|
||||
size_t commandDataLen) {
|
||||
switch(deviceCommand) {
|
||||
case(IMTQ::START_ACTUATION_DIPOLE): {
|
||||
/* IMTQ expects low byte first */
|
||||
commandBuffer[0] = IMTQ::CC::START_ACTUATION_DIPOLE;
|
||||
commandBuffer[1] = *(commandData + 1);
|
||||
commandBuffer[2] = *(commandData);
|
||||
commandBuffer[3] = *(commandData + 3);
|
||||
commandBuffer[4] = *(commandData + 2);
|
||||
commandBuffer[5] = *(commandData + 5);
|
||||
commandBuffer[6] = *(commandData + 4);
|
||||
commandBuffer[7] = *(commandData + 7);
|
||||
commandBuffer[8] = *(commandData + 6);
|
||||
rawPacket = commandBuffer;
|
||||
rawPacketLen = 9;
|
||||
return RETURN_OK;
|
||||
}
|
||||
case(IMTQ::GET_ENG_HK_DATA): {
|
||||
commandBuffer[0] = IMTQ::CC::GET_ENG_HK_DATA;
|
||||
rawPacket = commandBuffer;
|
||||
rawPacketLen = 1;
|
||||
return RETURN_OK;
|
||||
}
|
||||
case(IMTQ::START_ACTUATION_DIPOLE): {
|
||||
/* IMTQ expects low byte first */
|
||||
commandBuffer[0] = IMTQ::CC::START_ACTUATION_DIPOLE;
|
||||
commandBuffer[1] = *(commandData + 1);
|
||||
commandBuffer[2] = *(commandData);
|
||||
commandBuffer[3] = *(commandData + 3);
|
||||
commandBuffer[4] = *(commandData + 2);
|
||||
commandBuffer[5] = *(commandData + 5);
|
||||
commandBuffer[6] = *(commandData + 4);
|
||||
commandBuffer[7] = *(commandData + 7);
|
||||
commandBuffer[8] = *(commandData + 6);
|
||||
case(IMTQ::GET_COMMANDED_DIPOLE): {
|
||||
commandBuffer[0] = IMTQ::CC::GET_COMMANDED_DIPOLE;
|
||||
rawPacket = commandBuffer;
|
||||
rawPacketLen = 9;
|
||||
rawPacketLen = 1;
|
||||
return RETURN_OK;
|
||||
}
|
||||
case(IMTQ::START_MTM_MEASUREMENT): {
|
||||
commandBuffer[0] = IMTQ::CC::START_MTM_MEASUREMENT;
|
||||
rawPacket = commandBuffer;
|
||||
rawPacketLen = 1;
|
||||
return RETURN_OK;
|
||||
}
|
||||
case(IMTQ::GET_CAL_MTM_MEASUREMENT): {
|
||||
commandBuffer[0] = IMTQ::CC::GET_CAL_MTM_MEASUREMENT;
|
||||
rawPacket = commandBuffer;
|
||||
rawPacketLen = 1;
|
||||
return RETURN_OK;
|
||||
}
|
||||
case(IMTQ::GET_RAW_MTM_MEASUREMENT): {
|
||||
commandBuffer[0] = IMTQ::CC::GET_RAW_MTM_MEASUREMENT;
|
||||
rawPacket = commandBuffer;
|
||||
rawPacketLen = 1;
|
||||
return RETURN_OK;
|
||||
}
|
||||
default:
|
||||
@ -70,8 +121,18 @@ ReturnValue_t IMTQHandler::buildCommandFromCommand(
|
||||
}
|
||||
|
||||
void IMTQHandler::fillCommandAndReplyMap() {
|
||||
this->insertInCommandAndReplyMap(IMTQ::START_ACTUATION_DIPOLE, 1, nullptr,
|
||||
IMTQ::SIZE_STATUS_REPLY);
|
||||
this->insertInCommandAndReplyMap(IMTQ::GET_ENG_HK_DATA, 1, &engHkDataset,
|
||||
IMTQ::SIZE_ENG_HK_DATA_REPLY, false, true, IMTQ::SIZE_ENG_HK_DATA_REPLY);
|
||||
IMTQ::SIZE_ENG_HK_DATA_REPLY);
|
||||
this->insertInCommandAndReplyMap(IMTQ::GET_COMMANDED_DIPOLE, 1, nullptr,
|
||||
IMTQ::SIZE_GET_COMMANDED_DIPOLE_REPLY);
|
||||
this->insertInCommandAndReplyMap(IMTQ::START_MTM_MEASUREMENT, 1, nullptr,
|
||||
IMTQ::SIZE_STATUS_REPLY);
|
||||
this->insertInCommandAndReplyMap(IMTQ::GET_CAL_MTM_MEASUREMENT, 1, &calMtmMeasurementSet,
|
||||
IMTQ::SIZE_GET_CAL_MTM_MEASUREMENT);
|
||||
this->insertInCommandAndReplyMap(IMTQ::GET_RAW_MTM_MEASUREMENT, 1, &rawMtmMeasurementSet,
|
||||
IMTQ::SIZE_GET_RAW_MTM_MEASUREMENT);
|
||||
}
|
||||
|
||||
ReturnValue_t IMTQHandler::scanForReply(const uint8_t *start,
|
||||
@ -80,10 +141,30 @@ ReturnValue_t IMTQHandler::scanForReply(const uint8_t *start,
|
||||
ReturnValue_t result = RETURN_OK;
|
||||
|
||||
switch(*start) {
|
||||
case(IMTQ::CC::START_ACTUATION_DIPOLE):
|
||||
*foundLen = IMTQ::SIZE_STATUS_REPLY;
|
||||
*foundId = IMTQ::START_ACTUATION_DIPOLE;
|
||||
break;
|
||||
case(IMTQ::CC::START_MTM_MEASUREMENT):
|
||||
*foundLen = IMTQ::SIZE_STATUS_REPLY;
|
||||
*foundId = IMTQ::START_MTM_MEASUREMENT;
|
||||
break;
|
||||
case(IMTQ::CC::GET_ENG_HK_DATA):
|
||||
*foundLen = IMTQ::SIZE_ENG_HK_DATA_REPLY;
|
||||
*foundId = IMTQ::GET_ENG_HK_DATA;
|
||||
break;
|
||||
case(IMTQ::CC::GET_COMMANDED_DIPOLE):
|
||||
*foundLen = IMTQ::SIZE_GET_COMMANDED_DIPOLE_REPLY;
|
||||
*foundId = IMTQ::GET_COMMANDED_DIPOLE;
|
||||
break;
|
||||
case(IMTQ::CC::GET_CAL_MTM_MEASUREMENT):
|
||||
*foundLen = IMTQ::SIZE_GET_CAL_MTM_MEASUREMENT;
|
||||
*foundId = IMTQ::GET_CAL_MTM_MEASUREMENT;
|
||||
break;
|
||||
case(IMTQ::CC::GET_RAW_MTM_MEASUREMENT):
|
||||
*foundLen = IMTQ::SIZE_GET_RAW_MTM_MEASUREMENT;
|
||||
*foundId = IMTQ::GET_RAW_MTM_MEASUREMENT;
|
||||
break;
|
||||
default:
|
||||
sif::debug << "IMTQHandler::scanForReply: Reply contains invalid command code" << std::endl;
|
||||
result = IGNORE_REPLY_DATA;
|
||||
@ -105,9 +186,22 @@ ReturnValue_t IMTQHandler::interpretDeviceReply(DeviceCommandId_t id,
|
||||
}
|
||||
|
||||
switch (id) {
|
||||
case (IMTQ::START_ACTUATION_DIPOLE):
|
||||
case (IMTQ::START_MTM_MEASUREMENT):
|
||||
/* Replies only the status byte which is already handled with parseStatusByte */
|
||||
break;
|
||||
case (IMTQ::GET_ENG_HK_DATA):
|
||||
fillEngHkDataset(packet);
|
||||
break;
|
||||
case (IMTQ::GET_COMMANDED_DIPOLE):
|
||||
handleGetCommandedDipoleReply(packet);
|
||||
break;
|
||||
case (IMTQ::GET_CAL_MTM_MEASUREMENT):
|
||||
fillCalibratedMtmDataset(packet);
|
||||
break;
|
||||
case (IMTQ::GET_RAW_MTM_MEASUREMENT):
|
||||
fillRawMtmDataset(packet);
|
||||
break;
|
||||
default: {
|
||||
sif::debug << "IMTQHandler::interpretDeviceReply: Unknown device reply id" << std::endl;
|
||||
return DeviceHandlerIF::UNKNOWN_DEVICE_REPLY;
|
||||
@ -117,22 +211,68 @@ ReturnValue_t IMTQHandler::interpretDeviceReply(DeviceCommandId_t id,
|
||||
return RETURN_OK;
|
||||
}
|
||||
|
||||
void IMTQHandler::setNormalDatapoolEntriesInvalid(){
|
||||
|
||||
}
|
||||
|
||||
uint32_t IMTQHandler::getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo){
|
||||
return 500;
|
||||
}
|
||||
|
||||
ReturnValue_t IMTQHandler::initializeLocalDataPool(localpool::DataPool& localDataPoolMap,
|
||||
LocalDataPoolManager& poolManager) {
|
||||
|
||||
/** Entries of engineering housekeeping dataset */
|
||||
localDataPoolMap.emplace(IMTQ::DIGITAL_VOLTAGE_MV, new PoolEntry<uint16_t>( { 0 }));
|
||||
localDataPoolMap.emplace(IMTQ::ANALOG_VOLTAGE_MV, new PoolEntry<uint16_t>( { 0 }));
|
||||
localDataPoolMap.emplace(IMTQ::DIGITAL_CURRENT, new PoolEntry<float>( { 0 }));
|
||||
localDataPoolMap.emplace(IMTQ::ANALOG_CURRENT, new PoolEntry<float>( { 0 }));
|
||||
localDataPoolMap.emplace(IMTQ::COIL_X_CURRENT, new PoolEntry<float>( { 0 }));
|
||||
localDataPoolMap.emplace(IMTQ::COIL_Y_CURRENT, new PoolEntry<float>( { 0 }));
|
||||
localDataPoolMap.emplace(IMTQ::COIL_Z_CURRENT, new PoolEntry<float>( { 0 }));
|
||||
localDataPoolMap.emplace(IMTQ::COIL_X_TEMPERATURE, new PoolEntry<uint16_t>( { 0 }));
|
||||
localDataPoolMap.emplace(IMTQ::COIL_Y_TEMPERATURE, new PoolEntry<uint16_t>( { 0 }));
|
||||
localDataPoolMap.emplace(IMTQ::COIL_Z_TEMPERATURE, new PoolEntry<uint16_t>( { 0 }));
|
||||
localDataPoolMap.emplace(IMTQ::MCU_TEMPERATURE, new PoolEntry<uint16_t>( { 0 }));
|
||||
|
||||
/** Entries of calibrated MTM measurement dataset */
|
||||
localDataPoolMap.emplace(IMTQ::MTM_CAL_X, new PoolEntry<uint32_t>( { 0 }));
|
||||
localDataPoolMap.emplace(IMTQ::MTM_CAL_Y, new PoolEntry<uint32_t>( { 0 }));
|
||||
localDataPoolMap.emplace(IMTQ::MTM_CAL_Z, new PoolEntry<uint32_t>( { 0 }));
|
||||
localDataPoolMap.emplace(IMTQ::ACTUATION_CAL_STATUS, new PoolEntry<uint8_t>( { 0 }));
|
||||
|
||||
/** Entries of raw MTM measurement dataset */
|
||||
localDataPoolMap.emplace(IMTQ::MTM_RAW_X, new PoolEntry<float>( { 0 }));
|
||||
localDataPoolMap.emplace(IMTQ::MTM_RAW_Y, new PoolEntry<float>( { 0 }));
|
||||
localDataPoolMap.emplace(IMTQ::MTM_RAW_Z, new PoolEntry<float>( { 0 }));
|
||||
localDataPoolMap.emplace(IMTQ::ACTUATION_RAW_STATUS, new PoolEntry<uint8_t>( { 0 }));
|
||||
|
||||
return HasReturnvaluesIF::RETURN_OK;
|
||||
}
|
||||
|
||||
ReturnValue_t IMTQHandler::parseStatusByte(const uint8_t* packet) {
|
||||
uint8_t cmdErrorField = *(packet + 1) & 0xF;
|
||||
switch (cmdErrorField) {
|
||||
case 0:
|
||||
return RETURN_OK;
|
||||
case 1:
|
||||
sif::error << "IMTQHandler::parseStatusByte: Command rejected without reason" << std::endl;
|
||||
return REJECTED_WITHOUT_REASON;
|
||||
case 2:
|
||||
sif::error << "IMTQHandler::parseStatusByte: Command has invalid command code" << std::endl;
|
||||
return INVALID_COMMAND_CODE;
|
||||
case 3:
|
||||
sif::error << "IMTQHandler::parseStatusByte: Command has missing parameter" << std::endl;
|
||||
return PARAMETER_MISSING;
|
||||
case 4:
|
||||
sif::error << "IMTQHandler::parseStatusByte: Command has invalid parameter" << std::endl;
|
||||
return PARAMETER_INVALID;
|
||||
case 5:
|
||||
sif::error << "IMTQHandler::parseStatusByte: CC unavailable" << std::endl;
|
||||
return CC_UNAVAILABLE;
|
||||
case 7:
|
||||
sif::error << "IMTQHandler::parseStatusByte: IMQT replied internal processing error"
|
||||
<< std::endl;
|
||||
return INTERNAL_PROCESSING_ERROR;
|
||||
default:
|
||||
sif::error << "IMTQHandler::parseStatusByte: CMD Error field contains unknown error code "
|
||||
@ -143,36 +283,36 @@ ReturnValue_t IMTQHandler::parseStatusByte(const uint8_t* packet) {
|
||||
|
||||
void IMTQHandler::fillEngHkDataset(const uint8_t* packet) {
|
||||
uint8_t offset = 2;
|
||||
engHkDataset.digitalVoltageMv = *(packet + offset + 1) | *(packet + offset);
|
||||
engHkDataset.digitalVoltageMv = *(packet + offset + 1) << 8 | *(packet + offset);
|
||||
offset += 2;
|
||||
engHkDataset.analogVoltageMv = *(packet + offset + 1) | *(packet + offset);
|
||||
engHkDataset.analogVoltageMv = *(packet + offset + 1) << 8 | *(packet + offset);
|
||||
offset += 2;
|
||||
engHkDataset.digitalCurrentA = (*(packet + offset + 1) | *(packet + offset)) * 0.0001;
|
||||
engHkDataset.digitalCurrentmA = (*(packet + offset + 1) << 8 | *(packet + offset)) * 0.1;
|
||||
offset += 2;
|
||||
engHkDataset.analogCurrentA = (*(packet + offset + 1) | *(packet + offset)) * 0.0001;
|
||||
engHkDataset.analogCurrentmA = (*(packet + offset + 1) << 8 | *(packet + offset)) * 0.1;
|
||||
offset += 2;
|
||||
engHkDataset.coilXcurrentA = (*(packet + offset + 1) | *(packet + offset)) * 0.0001;
|
||||
engHkDataset.coilXCurrentmA = (*(packet + offset + 1) << 8 | *(packet + offset)) * 0.1;
|
||||
offset += 2;
|
||||
engHkDataset.coilYcurrentA = (*(packet + offset + 1) | *(packet + offset)) * 0.0001;
|
||||
engHkDataset.coilYCurrentmA = (*(packet + offset + 1) << 8 | *(packet + offset)) * 0.1;
|
||||
offset += 2;
|
||||
engHkDataset.coilZcurrentA = (*(packet + offset + 1) | *(packet + offset)) * 0.0001;
|
||||
engHkDataset.coilZCurrentmA = (*(packet + offset + 1) << 8 | *(packet + offset)) * 0.1;
|
||||
offset += 2;
|
||||
engHkDataset.coilXTemperature = (*(packet + offset + 1) | *(packet + offset));
|
||||
engHkDataset.coilXTemperature = (*(packet + offset + 1) << 8 | *(packet + offset));
|
||||
offset += 2;
|
||||
engHkDataset.coilYTemperature = (*(packet + offset + 1) | *(packet + offset));
|
||||
engHkDataset.coilYTemperature = (*(packet + offset + 1) << 8 | *(packet + offset));
|
||||
offset += 2;
|
||||
engHkDataset.coilZTemperature = (*(packet + offset + 1) | *(packet + offset));
|
||||
engHkDataset.coilZTemperature = (*(packet + offset + 1) << 8 | *(packet + offset));
|
||||
offset += 2;
|
||||
engHkDataset.mcuTemperature = (*(packet + offset + 1) | *(packet + offset));
|
||||
engHkDataset.mcuTemperature = (*(packet + offset + 1) << 8 | *(packet + offset));
|
||||
|
||||
#if OBSW_VERBOSE_LEVEL >= 1 && IMQT_DEBUG == 1
|
||||
sif::info << "IMTQ digital voltage: " << engHkDataset.digitalVoltageMv << " mV" << std::endl;
|
||||
sif::info << "IMTQ analog voltage: " << engHkDataset.analogVoltageMv << " mV" << std::endl;
|
||||
sif::info << "IMTQ digital current: " << engHkDataset.digitalCurrentA << " A" << std::endl;
|
||||
sif::info << "IMTQ analog current: " << engHkDataset.analogCurrentA << " A" << std::endl;
|
||||
sif::info << "IMTQ coil X current: " << engHkDataset.coilXcurrentA << " A" << std::endl;
|
||||
sif::info << "IMTQ coil Y current: " << engHkDataset.coilYcurrentA << " A" << std::endl;
|
||||
sif::info << "IMTQ coil Z current: " << engHkDataset.coilZcurrentA << " A" << std::endl;
|
||||
sif::info << "IMTQ digital current: " << engHkDataset.digitalCurrentmA << " mA" << std::endl;
|
||||
sif::info << "IMTQ analog current: " << engHkDataset.analogCurrentmA << " mA" << std::endl;
|
||||
sif::info << "IMTQ coil X current: " << engHkDataset.coilXCurrentmA << " mA" << std::endl;
|
||||
sif::info << "IMTQ coil Y current: " << engHkDataset.coilYCurrentmA << " mA" << std::endl;
|
||||
sif::info << "IMTQ coil Z current: " << engHkDataset.coilZCurrentmA << " mA" << std::endl;
|
||||
sif::info << "IMTQ coil X temperature: " << engHkDataset.coilXTemperature << " °C"
|
||||
<< std::endl;
|
||||
sif::info << "IMTQ coil Y temperature: " << engHkDataset.coilYTemperature << " °C"
|
||||
@ -184,33 +324,93 @@ void IMTQHandler::fillEngHkDataset(const uint8_t* packet) {
|
||||
#endif
|
||||
}
|
||||
|
||||
void IMTQHandler::setNormalDatapoolEntriesInvalid(){
|
||||
|
||||
}
|
||||
|
||||
uint32_t IMTQHandler::getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo){
|
||||
return 500;
|
||||
}
|
||||
|
||||
ReturnValue_t IMTQHandler::initializeLocalDataPool(localpool::DataPool& localDataPoolMap,
|
||||
LocalDataPoolManager& poolManager) {
|
||||
|
||||
localDataPoolMap.emplace(IMTQ::DIGITAL_VOLTAGE_MV, new PoolEntry<uint16_t>( { 0 }));
|
||||
localDataPoolMap.emplace(IMTQ::ANALOG_VOLTAGE_MV, new PoolEntry<uint16_t>( { 0 }));
|
||||
localDataPoolMap.emplace(IMTQ::DIGITAL_CURRENT_A, new PoolEntry<float>( { 0 }));
|
||||
localDataPoolMap.emplace(IMTQ::ANALOG_CURRENT_A, new PoolEntry<float>( { 0 }));
|
||||
localDataPoolMap.emplace(IMTQ::COIL_X_CURRENT_A, new PoolEntry<float>( { 0 }));
|
||||
localDataPoolMap.emplace(IMTQ::COIL_Y_CURRENT_A, new PoolEntry<float>( { 0 }));
|
||||
localDataPoolMap.emplace(IMTQ::COIL_Z_CURRENT_A, new PoolEntry<float>( { 0 }));
|
||||
localDataPoolMap.emplace(IMTQ::COIL_X_TEMPERATURE, new PoolEntry<uint16_t>( { 0 }));
|
||||
localDataPoolMap.emplace(IMTQ::COIL_Y_TEMPERATURE, new PoolEntry<uint16_t>( { 0 }));
|
||||
localDataPoolMap.emplace(IMTQ::COIL_Z_TEMPERATURE, new PoolEntry<uint16_t>( { 0 }));
|
||||
localDataPoolMap.emplace(IMTQ::MCU_TEMPERATURE, new PoolEntry<uint16_t>( { 0 }));
|
||||
|
||||
return HasReturnvaluesIF::RETURN_OK;
|
||||
}
|
||||
|
||||
void IMTQHandler::setModeNormal() {
|
||||
mode = MODE_NORMAL;
|
||||
}
|
||||
|
||||
void IMTQHandler::handleDeviceTM(const uint8_t* data, size_t dataSize, DeviceCommandId_t replyId) {
|
||||
|
||||
if (wiretappingMode == RAW) {
|
||||
/* Data already sent in doGetRead() */
|
||||
return;
|
||||
}
|
||||
|
||||
DeviceReplyMap::iterator iter = deviceReplyMap.find(replyId);
|
||||
if (iter == deviceReplyMap.end()) {
|
||||
sif::debug << "IMTQHandler::handleDeviceTM: Unknown reply id" << std::endl;
|
||||
return;
|
||||
}
|
||||
MessageQueueId_t queueId = iter->second.command->second.sendReplyTo;
|
||||
|
||||
if (queueId == NO_COMMANDER) {
|
||||
return;
|
||||
}
|
||||
|
||||
ReturnValue_t result = actionHelper.reportData(queueId, replyId, data, dataSize);
|
||||
if (result != RETURN_OK) {
|
||||
sif::debug << "IMTQHandler::handleDeviceTM: Failed to report data" << std::endl;
|
||||
return;
|
||||
}
|
||||
}
|
||||
|
||||
void IMTQHandler::handleGetCommandedDipoleReply(const uint8_t* packet) {
|
||||
uint8_t tmData[6];
|
||||
/* Switching endianess of received dipole values */
|
||||
tmData[0] = *(packet + 3);
|
||||
tmData[1] = *(packet + 2);
|
||||
tmData[2] = *(packet + 5);
|
||||
tmData[3] = *(packet + 4);
|
||||
tmData[4] = *(packet + 7);
|
||||
tmData[5] = *(packet + 6);
|
||||
handleDeviceTM(tmData, sizeof(tmData), IMTQ::GET_COMMANDED_DIPOLE);
|
||||
}
|
||||
|
||||
void IMTQHandler::fillCalibratedMtmDataset(const uint8_t* packet) {
|
||||
int8_t offset = 2;
|
||||
calMtmMeasurementSet.mtmXnT = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16
|
||||
| *(packet + offset + 1) << 8 | *(packet + offset);
|
||||
offset += 4;
|
||||
calMtmMeasurementSet.mtmYnT = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16
|
||||
| *(packet + offset + 1) << 8 | *(packet + offset);
|
||||
offset += 4;
|
||||
calMtmMeasurementSet.mtmZnT = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16
|
||||
| *(packet + offset + 1) << 8 | *(packet + offset);
|
||||
offset += 4;
|
||||
calMtmMeasurementSet.coilActuationStatus = (*(packet + offset + 3) << 24)
|
||||
| (*(packet + offset + 2) << 16) | (*(packet + offset + 1) << 8) | (*(packet + offset));
|
||||
#if OBSW_VERBOSE_LEVEL >= 1 && IMQT_DEBUG == 1
|
||||
sif::info << "IMTQ calibrated MTM measurement X: " << calMtmMeasurementSet.mtmXnT << " nT"
|
||||
<< std::endl;
|
||||
sif::info << "IMTQ calibrated MTM measurement Y: " << calMtmMeasurementSet.mtmYnT << " nT"
|
||||
<< std::endl;
|
||||
sif::info << "IMTQ calibrated MTM measurement Z: " << calMtmMeasurementSet.mtmZnT << " nT"
|
||||
<< std::endl;
|
||||
sif::info << "IMTQ coil actuation status during MTM measurement: "
|
||||
<< (unsigned int) calMtmMeasurementSet.coilActuationStatus.value << std::endl;
|
||||
#endif
|
||||
}
|
||||
|
||||
void IMTQHandler::fillRawMtmDataset(const uint8_t* packet) {
|
||||
int8_t offset = 2;
|
||||
rawMtmMeasurementSet.mtmXnT = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16
|
||||
| *(packet + offset + 1) << 8 | *(packet + offset)) * 7.5;
|
||||
offset += 4;
|
||||
rawMtmMeasurementSet.mtmYnT = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16
|
||||
| *(packet + offset + 1) << 8 | *(packet + offset)) * 7.5;
|
||||
offset += 4;
|
||||
rawMtmMeasurementSet.mtmZnT = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16
|
||||
| *(packet + offset + 1) << 8 | *(packet + offset)) * 7.5;
|
||||
offset += 4;
|
||||
rawMtmMeasurementSet.coilActuationStatus = (*(packet + offset + 3) << 24)
|
||||
| (*(packet + offset + 2) << 16) | (*(packet + offset + 1) << 8) | (*(packet + offset));
|
||||
#if OBSW_VERBOSE_LEVEL >= 1 && IMQT_DEBUG == 1
|
||||
sif::info << "IMTQ raw MTM measurement X: " << rawMtmMeasurementSet.mtmXnT << " nT"
|
||||
<< std::endl;
|
||||
sif::info << "IMTQ raw MTM measurement Y: " << rawMtmMeasurementSet.mtmYnT << " nT"
|
||||
<< std::endl;
|
||||
sif::info << "IMTQ raw MTM measurement Z: " << rawMtmMeasurementSet.mtmZnT << " nT"
|
||||
<< std::endl;
|
||||
sif::info << "IMTQ coil actuation status during MTM measurement: "
|
||||
<< (unsigned int) rawMtmMeasurementSet.coilActuationStatus.value << std::endl;
|
||||
#endif
|
||||
}
|
||||
|
@ -52,9 +52,21 @@ private:
|
||||
|
||||
|
||||
IMTQ::EngHkDataset engHkDataset;
|
||||
IMTQ::CalibratedMtmMeasurementSet calMtmMeasurementSet;
|
||||
IMTQ::RawMtmMeasurementSet rawMtmMeasurementSet;
|
||||
|
||||
uint8_t commandBuffer[IMTQ::MAX_COMMAND_SIZE];
|
||||
|
||||
enum class CommunicationStep {
|
||||
SELF_TEST,
|
||||
GET_ENG_HK_DATA,
|
||||
START_MTM_MEASUREMENT,
|
||||
GET_CAL_MTM_MEASUREMENT,
|
||||
GET_RAW_MTM_MEASUREMENT
|
||||
};
|
||||
|
||||
CommunicationStep communicationStep = CommunicationStep::GET_ENG_HK_DATA;
|
||||
|
||||
/**
|
||||
* @brief Each reply contains a status byte giving information about a request. This function
|
||||
* parses this byte and returns the associated failure message.
|
||||
@ -72,6 +84,36 @@ private:
|
||||
*
|
||||
*/
|
||||
void fillEngHkDataset(const uint8_t* packet);
|
||||
|
||||
/**
|
||||
* @brief This function sends a command reply to the requesting queue.
|
||||
*
|
||||
* @param data Pointer to the data to send.
|
||||
* @param dataSize Size of the data to send.
|
||||
* @param relplyId Reply id which will be inserted at the beginning of the action message.
|
||||
*/
|
||||
void handleDeviceTM(const uint8_t* data, size_t dataSize, DeviceCommandId_t replyId);
|
||||
|
||||
/**
|
||||
* @brief This function handles the reply containing the commanded dipole.
|
||||
*
|
||||
* @param packet Pointer to the reply data.
|
||||
*/
|
||||
void handleGetCommandedDipoleReply(const uint8_t* packet);
|
||||
|
||||
/**
|
||||
* @brief This function parses the reply containing the calibrated MTM measurement and writes
|
||||
* the values to the appropriate dataset.
|
||||
* @param packet Pointer to the reply data.
|
||||
*/
|
||||
void fillCalibratedMtmDataset(const uint8_t* packet);
|
||||
|
||||
/**
|
||||
* @brief This function copies the raw MTM measurements to the MTM raw dataset.
|
||||
* @param packet Pointer to the reply data requested with the GET_RAW_MTM_MEASUREMENTS
|
||||
* command.
|
||||
*/
|
||||
void fillRawMtmDataset(const uint8_t* packet);
|
||||
};
|
||||
|
||||
#endif /* MISSION_DEVICES_IMTQHANDLER_H_ */
|
||||
|
498
mission/devices/PlocHandler.cpp
Normal file
498
mission/devices/PlocHandler.cpp
Normal file
@ -0,0 +1,498 @@
|
||||
#include "PlocHandler.h"
|
||||
|
||||
#include <fsfwconfig/OBSWConfig.h>
|
||||
#include <fsfw/globalfunctions/CRC.h>
|
||||
#include <fsfw/datapool/PoolReadGuard.h>
|
||||
#include <fsfwconfig/OBSWConfig.h>
|
||||
|
||||
PlocHandler::PlocHandler(object_id_t objectId, object_id_t comIF, CookieIF * comCookie) :
|
||||
DeviceHandlerBase(objectId, comIF, comCookie) {
|
||||
if (comCookie == NULL) {
|
||||
sif::error << "PlocHandler: Invalid com cookie" << std::endl;
|
||||
}
|
||||
}
|
||||
|
||||
PlocHandler::~PlocHandler() {
|
||||
}
|
||||
|
||||
|
||||
void PlocHandler::doStartUp(){
|
||||
if(mode == _MODE_START_UP){
|
||||
setMode(MODE_ON);
|
||||
}
|
||||
}
|
||||
|
||||
void PlocHandler::doShutDown(){
|
||||
|
||||
}
|
||||
|
||||
ReturnValue_t PlocHandler::buildNormalDeviceCommand(
|
||||
DeviceCommandId_t * id) {
|
||||
return RETURN_OK;
|
||||
}
|
||||
|
||||
ReturnValue_t PlocHandler::buildTransitionDeviceCommand(
|
||||
DeviceCommandId_t * id){
|
||||
return HasReturnvaluesIF::RETURN_OK;
|
||||
}
|
||||
|
||||
ReturnValue_t PlocHandler::buildCommandFromCommand(
|
||||
DeviceCommandId_t deviceCommand, const uint8_t * commandData,
|
||||
size_t commandDataLen) {
|
||||
switch(deviceCommand) {
|
||||
case(PLOC::TC_MEM_WRITE): {
|
||||
return prepareTcMemWriteCommand(commandData, commandDataLen);
|
||||
}
|
||||
case(PLOC::TC_MEM_READ): {
|
||||
return prepareTcMemReadCommand(commandData, commandDataLen);
|
||||
}
|
||||
default:
|
||||
sif::debug << "PlocHandler::buildCommandFromCommand: Command not implemented" << std::endl;
|
||||
return DeviceHandlerIF::COMMAND_NOT_IMPLEMENTED;
|
||||
}
|
||||
return HasReturnvaluesIF::RETURN_FAILED;
|
||||
}
|
||||
|
||||
void PlocHandler::fillCommandAndReplyMap() {
|
||||
this->insertInCommandMap(PLOC::TC_MEM_WRITE);
|
||||
this->insertInCommandMap(PLOC::TC_MEM_READ);
|
||||
this->insertInReplyMap(PLOC::ACK_REPORT, 1, nullptr, PLOC::SIZE_ACK_REPORT);
|
||||
this->insertInReplyMap(PLOC::EXE_REPORT, 3, nullptr, PLOC::SIZE_EXE_REPORT);
|
||||
this->insertInReplyMap(PLOC::TM_MEMORY_READ_REPORT, 2, nullptr, PLOC::SIZE_TM_MEM_READ_REPORT);
|
||||
}
|
||||
|
||||
ReturnValue_t PlocHandler::scanForReply(const uint8_t *start,
|
||||
size_t remainingSize, DeviceCommandId_t *foundId, size_t *foundLen) {
|
||||
|
||||
ReturnValue_t result = RETURN_OK;
|
||||
|
||||
uint16_t apid = (*(start) << 8 | *(start + 1)) & APID_MASK;
|
||||
|
||||
switch(apid) {
|
||||
case(PLOC::APID_ACK_SUCCESS):
|
||||
*foundLen = PLOC::SIZE_ACK_REPORT;
|
||||
*foundId = PLOC::ACK_REPORT;
|
||||
break;
|
||||
case(PLOC::APID_ACK_FAILURE):
|
||||
*foundLen = PLOC::SIZE_ACK_REPORT;
|
||||
*foundId = PLOC::ACK_REPORT;
|
||||
break;
|
||||
case(PLOC::APID_TM_MEMORY_READ_REPORT):
|
||||
*foundLen = PLOC::SIZE_TM_MEM_READ_REPORT;
|
||||
*foundId = PLOC::TM_MEMORY_READ_REPORT;
|
||||
break;
|
||||
case(PLOC::APID_EXE_SUCCESS):
|
||||
*foundLen = PLOC::SIZE_EXE_REPORT;
|
||||
*foundId = PLOC::EXE_REPORT;
|
||||
break;
|
||||
case(PLOC::APID_EXE_FAILURE):
|
||||
*foundLen = PLOC::SIZE_EXE_REPORT;
|
||||
*foundId = PLOC::EXE_REPORT;
|
||||
break;
|
||||
default: {
|
||||
sif::debug << "PlocHandler::scanForReply: Reply has invalid apid" << std::endl;
|
||||
*foundLen = remainingSize;
|
||||
return INVALID_APID;
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* This should normally never fail. However, this function is also responsible for incrementing
|
||||
* the packet sequence count why it is called here.
|
||||
*/
|
||||
result = checkPacketSequenceCount(start);
|
||||
|
||||
return result;
|
||||
}
|
||||
|
||||
ReturnValue_t PlocHandler::interpretDeviceReply(DeviceCommandId_t id,
|
||||
const uint8_t *packet) {
|
||||
|
||||
ReturnValue_t result = RETURN_OK;
|
||||
|
||||
switch (id) {
|
||||
case PLOC::ACK_REPORT: {
|
||||
result = handleAckReport(packet);
|
||||
break;
|
||||
}
|
||||
case (PLOC::TM_MEMORY_READ_REPORT): {
|
||||
result = handleMemoryReadReport(packet);
|
||||
break;
|
||||
}
|
||||
case (PLOC::EXE_REPORT): {
|
||||
result = handleExecutionReport(packet);
|
||||
break;
|
||||
}
|
||||
default: {
|
||||
sif::debug << "PlocHandler::interpretDeviceReply: Unknown device reply id" << std::endl;
|
||||
return DeviceHandlerIF::UNKNOWN_DEVICE_REPLY;
|
||||
}
|
||||
}
|
||||
|
||||
return result;
|
||||
}
|
||||
|
||||
void PlocHandler::setNormalDatapoolEntriesInvalid(){
|
||||
|
||||
}
|
||||
|
||||
uint32_t PlocHandler::getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo){
|
||||
return 500;
|
||||
}
|
||||
|
||||
ReturnValue_t PlocHandler::initializeLocalDataPool(localpool::DataPool& localDataPoolMap,
|
||||
LocalDataPoolManager& poolManager) {
|
||||
|
||||
return HasReturnvaluesIF::RETURN_OK;
|
||||
}
|
||||
|
||||
void PlocHandler::setModeNormal() {
|
||||
mode = MODE_NORMAL;
|
||||
}
|
||||
|
||||
ReturnValue_t PlocHandler::prepareTcMemWriteCommand(const uint8_t * commandData,
|
||||
size_t commandDataLen) {
|
||||
const uint32_t memoryAddress = *(commandData) << 24 | *(commandData + 1) << 16
|
||||
| *(commandData + 2) << 8 | *(commandData + 3);
|
||||
const uint32_t memoryData = *(commandData + 4) << 24 | *(commandData + 5) << 16
|
||||
| *(commandData + 6) << 8 | *(commandData + 7);
|
||||
packetSequenceCount = (packetSequenceCount + 1) & PACKET_SEQUENCE_COUNT_MASK;
|
||||
PLOC::TcMemWrite tcMemWrite(memoryAddress, memoryData, packetSequenceCount);
|
||||
if (tcMemWrite.getFullSize() > PLOC::MAX_COMMAND_SIZE) {
|
||||
sif::debug << "PlocHandler::prepareTcMemWriteCommand: Command too big" << std::endl;
|
||||
return RETURN_FAILED;
|
||||
}
|
||||
memcpy(commandBuffer, tcMemWrite.getWholeData(), tcMemWrite.getFullSize());
|
||||
rawPacket = commandBuffer;
|
||||
rawPacketLen = tcMemWrite.getFullSize();
|
||||
nextReplyId = PLOC::ACK_REPORT;
|
||||
return RETURN_OK;
|
||||
}
|
||||
|
||||
ReturnValue_t PlocHandler::prepareTcMemReadCommand(const uint8_t * commandData,
|
||||
size_t commandDataLen) {
|
||||
const uint32_t memoryAddress = *(commandData) << 24 | *(commandData + 1) << 16
|
||||
| *(commandData + 2) << 8 | *(commandData + 3);
|
||||
packetSequenceCount = (packetSequenceCount + 1) & PACKET_SEQUENCE_COUNT_MASK;
|
||||
PLOC::TcMemRead tcMemRead(memoryAddress, packetSequenceCount);
|
||||
if (tcMemRead.getFullSize() > PLOC::MAX_COMMAND_SIZE) {
|
||||
sif::debug << "PlocHandler::prepareTcMemReadCommand: Command too big" << std::endl;
|
||||
return RETURN_FAILED;
|
||||
}
|
||||
memcpy(commandBuffer, tcMemRead.getWholeData(), tcMemRead.getFullSize());
|
||||
rawPacket = commandBuffer;
|
||||
rawPacketLen = tcMemRead.getFullSize();
|
||||
nextReplyId = PLOC::ACK_REPORT;
|
||||
|
||||
return RETURN_OK;
|
||||
}
|
||||
|
||||
ReturnValue_t PlocHandler::verifyPacket(const uint8_t* start, size_t foundLen) {
|
||||
|
||||
uint16_t receivedCrc = *(start + foundLen - 2) << 8 | *(start + foundLen - 1);
|
||||
|
||||
uint16_t recalculatedCrc = CRC::crc16ccitt(start, foundLen - 2);
|
||||
|
||||
if (receivedCrc != recalculatedCrc) {
|
||||
return CRC_FAILURE;
|
||||
}
|
||||
|
||||
return RETURN_OK;
|
||||
}
|
||||
|
||||
ReturnValue_t PlocHandler::handleAckReport(const uint8_t* data) {
|
||||
|
||||
ReturnValue_t result = RETURN_OK;
|
||||
|
||||
result = verifyPacket(data, PLOC::SIZE_ACK_REPORT);
|
||||
if(result == CRC_FAILURE) {
|
||||
sif::error << "PlocHandler::handleAckReport: CRC failure" << std::endl;
|
||||
nextReplyId = PLOC::NONE;
|
||||
replyRawReplyIfnotWiretapped(data, PLOC::SIZE_ACK_REPORT);
|
||||
triggerEvent(CRC_FAILURE_EVENT);
|
||||
sendFailureReport(PLOC::ACK_REPORT, CRC_FAILURE);
|
||||
disableAllReplies();
|
||||
return IGNORE_REPLY_DATA;
|
||||
}
|
||||
|
||||
uint16_t apid = (*(data) << 8 | *(data + 1)) & APID_MASK;
|
||||
|
||||
switch(apid) {
|
||||
case PLOC::APID_ACK_FAILURE: {
|
||||
//TODO: Interpretation of status field in acknowledgment report
|
||||
sif::debug << "PlocHandler::handleAckReport: Received Ack failure report" << std::endl;
|
||||
DeviceCommandId_t commandId = getPendingCommand();
|
||||
if (commandId != DeviceHandlerIF::NO_COMMAND_ID) {
|
||||
triggerEvent(ACK_FAILURE, commandId);
|
||||
}
|
||||
sendFailureReport(PLOC::ACK_REPORT, RECEIVED_ACK_FAILURE);
|
||||
disableAllReplies();
|
||||
nextReplyId = PLOC::NONE;
|
||||
result = IGNORE_REPLY_DATA;
|
||||
break;
|
||||
}
|
||||
case PLOC::APID_ACK_SUCCESS: {
|
||||
setNextReplyId();
|
||||
break;
|
||||
}
|
||||
default: {
|
||||
sif::debug << "PlocHandler::handleAckReport: Invalid APID in Ack report" << std::endl;
|
||||
result = RETURN_FAILED;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
return result;
|
||||
}
|
||||
|
||||
ReturnValue_t PlocHandler::handleExecutionReport(const uint8_t* data) {
|
||||
|
||||
ReturnValue_t result = RETURN_OK;
|
||||
|
||||
result = verifyPacket(data, PLOC::SIZE_EXE_REPORT);
|
||||
if(result == CRC_FAILURE) {
|
||||
sif::error << "PlocHandler::handleExecutionReport: CRC failure" << std::endl;
|
||||
nextReplyId = PLOC::NONE;
|
||||
return result;
|
||||
}
|
||||
|
||||
uint16_t apid = (*(data) << 8 | *(data + 1)) & APID_MASK;
|
||||
|
||||
switch (apid) {
|
||||
case (PLOC::APID_EXE_SUCCESS): {
|
||||
break;
|
||||
}
|
||||
case (PLOC::APID_EXE_FAILURE): {
|
||||
//TODO: Interpretation of status field in execution report
|
||||
sif::error << "PlocHandler::handleExecutionReport: Received execution failure report"
|
||||
<< std::endl;
|
||||
DeviceCommandId_t commandId = getPendingCommand();
|
||||
if (commandId != DeviceHandlerIF::NO_COMMAND_ID) {
|
||||
triggerEvent(EXE_FAILURE, commandId);
|
||||
}
|
||||
else {
|
||||
sif::debug << "PlocHandler::handleExecutionReport: Unknown command id" << std::endl;
|
||||
}
|
||||
sendFailureReport(PLOC::EXE_REPORT, RECEIVED_EXE_FAILURE);
|
||||
disableExeReportReply();
|
||||
result = IGNORE_REPLY_DATA;
|
||||
break;
|
||||
}
|
||||
default: {
|
||||
sif::error << "PlocHandler::handleExecutionReport: Unknown APID" << std::endl;
|
||||
result = RETURN_FAILED;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
nextReplyId = PLOC::NONE;
|
||||
|
||||
return result;
|
||||
}
|
||||
|
||||
ReturnValue_t PlocHandler::handleMemoryReadReport(const uint8_t* data) {
|
||||
|
||||
ReturnValue_t result = RETURN_OK;
|
||||
|
||||
result = verifyPacket(data, PLOC::SIZE_TM_MEM_READ_REPORT);
|
||||
|
||||
if(result == CRC_FAILURE) {
|
||||
sif::error << "PlocHandler::handleMemoryReadReport: Memory read report has invalid crc"
|
||||
<< std::endl;
|
||||
}
|
||||
/** Send data to commanding queue */
|
||||
handleDeviceTM(data + PLOC::DATA_FIELD_OFFSET, PLOC::SIZE_MEM_READ_REPORT_DATA,
|
||||
PLOC::TM_MEMORY_READ_REPORT);
|
||||
|
||||
nextReplyId = PLOC::EXE_REPORT;
|
||||
|
||||
return result;
|
||||
}
|
||||
|
||||
ReturnValue_t PlocHandler::enableReplyInReplyMap(DeviceCommandMap::iterator command,
|
||||
uint8_t expectedReplies, bool useAlternateId,
|
||||
DeviceCommandId_t alternateReplyID) {
|
||||
|
||||
ReturnValue_t result = RETURN_OK;
|
||||
|
||||
uint8_t enabledReplies = 0;
|
||||
|
||||
switch (command->first) {
|
||||
case PLOC::TC_MEM_WRITE:
|
||||
enabledReplies = 2;
|
||||
break;
|
||||
case PLOC::TC_MEM_READ: {
|
||||
enabledReplies = 3;
|
||||
result = DeviceHandlerBase::enableReplyInReplyMap(command, enabledReplies, true,
|
||||
PLOC::TM_MEMORY_READ_REPORT);
|
||||
if (result != RETURN_OK) {
|
||||
sif::debug << "PlocHandler::enableReplyInReplyMap: Reply with id "
|
||||
<< PLOC::TM_MEMORY_READ_REPORT << " not in replyMap" << std::endl;
|
||||
}
|
||||
break;
|
||||
}
|
||||
default:
|
||||
sif::debug << "PlocHandler::enableReplyInReplyMap: Unknown command id" << std::endl;
|
||||
break;
|
||||
}
|
||||
|
||||
/**
|
||||
* Every command causes at least one acknowledgment and one execution report. Therefore both
|
||||
* replies will be enabled here.
|
||||
*/
|
||||
result = DeviceHandlerBase::enableReplyInReplyMap(command,
|
||||
enabledReplies, true, PLOC::ACK_REPORT);
|
||||
if (result != RETURN_OK) {
|
||||
sif::debug << "PlocHandler::enableReplyInReplyMap: Reply with id " << PLOC::ACK_REPORT
|
||||
<< " not in replyMap" << std::endl;
|
||||
}
|
||||
|
||||
result = DeviceHandlerBase::enableReplyInReplyMap(command,
|
||||
enabledReplies, true, PLOC::EXE_REPORT);
|
||||
if (result != RETURN_OK) {
|
||||
sif::debug << "PlocHandler::enableReplyInReplyMap: Reply with id " << PLOC::EXE_REPORT
|
||||
<< " not in replyMap" << std::endl;
|
||||
}
|
||||
|
||||
return RETURN_OK;
|
||||
}
|
||||
|
||||
void PlocHandler::setNextReplyId() {
|
||||
switch(getPendingCommand()) {
|
||||
case PLOC::TC_MEM_READ:
|
||||
nextReplyId = PLOC::TM_MEMORY_READ_REPORT;
|
||||
break;
|
||||
default:
|
||||
/* If no telemetry is expected the next reply is always the execution report */
|
||||
nextReplyId = PLOC::EXE_REPORT;
|
||||
break;
|
||||
}
|
||||
}
|
||||
size_t PlocHandler::getNextReplyLength(DeviceCommandId_t commandId){
|
||||
|
||||
size_t replyLen = 0;
|
||||
|
||||
if (nextReplyId == PLOC::NONE) {
|
||||
return replyLen;
|
||||
}
|
||||
|
||||
DeviceReplyIter iter = deviceReplyMap.find(nextReplyId);
|
||||
if (iter != deviceReplyMap.end()) {
|
||||
if (iter->second.delayCycles == 0) {
|
||||
/* Reply inactive */
|
||||
return replyLen;
|
||||
}
|
||||
replyLen = iter->second.replyLen;
|
||||
}
|
||||
else {
|
||||
sif::debug << "PlocHandler::getNextReplyLength: No entry for reply with reply id "
|
||||
<< std::hex << nextReplyId << " in deviceReplyMap" << std::endl;
|
||||
}
|
||||
|
||||
return replyLen;
|
||||
}
|
||||
|
||||
void PlocHandler::handleDeviceTM(const uint8_t* data, size_t dataSize, DeviceCommandId_t replyId) {
|
||||
|
||||
ReturnValue_t result = RETURN_OK;
|
||||
|
||||
if (wiretappingMode == RAW) {
|
||||
/* Data already sent in doGetRead() */
|
||||
return;
|
||||
}
|
||||
|
||||
DeviceReplyMap::iterator iter = deviceReplyMap.find(replyId);
|
||||
if (iter == deviceReplyMap.end()) {
|
||||
sif::debug << "PlocHandler::handleDeviceTM: Unknown reply id" << std::endl;
|
||||
return;
|
||||
}
|
||||
MessageQueueId_t queueId = iter->second.command->second.sendReplyTo;
|
||||
|
||||
if (queueId == NO_COMMANDER) {
|
||||
return;
|
||||
}
|
||||
|
||||
result = actionHelper.reportData(queueId, replyId, data, dataSize);
|
||||
if (result != RETURN_OK) {
|
||||
sif::debug << "PlocHandler::handleDeviceTM: Failed to report data" << std::endl;
|
||||
}
|
||||
}
|
||||
|
||||
void PlocHandler::disableAllReplies() {
|
||||
|
||||
DeviceReplyMap::iterator iter;
|
||||
|
||||
/* Disable ack reply */
|
||||
iter = deviceReplyMap.find(PLOC::ACK_REPORT);
|
||||
DeviceReplyInfo *info = &(iter->second);
|
||||
info->delayCycles = 0;
|
||||
info->command = deviceCommandMap.end();
|
||||
|
||||
DeviceCommandId_t commandId = getPendingCommand();
|
||||
|
||||
/* If the command expects a telemetry packet the appropriate tm reply will be disabled here */
|
||||
switch (commandId) {
|
||||
case PLOC::TC_MEM_WRITE:
|
||||
break;
|
||||
case PLOC::TC_MEM_READ: {
|
||||
iter = deviceReplyMap.find(PLOC::TM_MEMORY_READ_REPORT);
|
||||
info = &(iter->second);
|
||||
info->delayCycles = 0;
|
||||
info->command = deviceCommandMap.end();
|
||||
break;
|
||||
}
|
||||
default: {
|
||||
sif::debug << "PlocHandler::disableAllReplies: Unknown command id" << commandId
|
||||
<< std::endl;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
/* We must always disable the execution report reply here */
|
||||
disableExeReportReply();
|
||||
}
|
||||
|
||||
void PlocHandler::sendFailureReport(DeviceCommandId_t replyId, ReturnValue_t status) {
|
||||
|
||||
DeviceReplyIter iter = deviceReplyMap.find(replyId);
|
||||
|
||||
if (iter == deviceReplyMap.end()) {
|
||||
sif::debug << "PlocHandler::sendFailureReport: Reply not in reply map" << std::endl;
|
||||
return;
|
||||
}
|
||||
|
||||
DeviceCommandInfo* info = &(iter->second.command->second);
|
||||
|
||||
if (info == nullptr) {
|
||||
sif::debug << "PlocHandler::sendFailureReport: Reply has no active command" << std::endl;
|
||||
return;
|
||||
}
|
||||
|
||||
if (info->sendReplyTo != NO_COMMANDER) {
|
||||
actionHelper.finish(false, info->sendReplyTo, iter->first, status);
|
||||
}
|
||||
info->isExecuting = false;
|
||||
}
|
||||
|
||||
void PlocHandler::disableExeReportReply() {
|
||||
DeviceReplyIter iter = deviceReplyMap.find(PLOC::EXE_REPORT);
|
||||
DeviceReplyInfo *info = &(iter->second);
|
||||
info->delayCycles = 0;
|
||||
info->command = deviceCommandMap.end();
|
||||
/* Expected replies is set to one here. The value will set to 0 in replyToReply() */
|
||||
info->command->second.expectedReplies = 0;
|
||||
}
|
||||
|
||||
ReturnValue_t PlocHandler::checkPacketSequenceCount(const uint8_t* data) {
|
||||
uint16_t receivedSequenceCount = (*(data + 2) << 8 | *(data + 3)) & PACKET_SEQUENCE_COUNT_MASK;
|
||||
uint16_t expectedPacketSequenceCount = ((packetSequenceCount + 1) & PACKET_SEQUENCE_COUNT_MASK);
|
||||
if (receivedSequenceCount != expectedPacketSequenceCount) {
|
||||
sif::debug
|
||||
<< "PlocHandler::checkPacketSequenceCount: Packet sequence count mismatch. "
|
||||
<< std::endl;
|
||||
sif::debug << "Received sequence count: " << receivedSequenceCount << ". OBSW sequence "
|
||||
<< "count: " << expectedPacketSequenceCount << std::endl;
|
||||
}
|
||||
packetSequenceCount = receivedSequenceCount;
|
||||
return RETURN_OK;
|
||||
}
|
201
mission/devices/PlocHandler.h
Normal file
201
mission/devices/PlocHandler.h
Normal file
@ -0,0 +1,201 @@
|
||||
#ifndef MISSION_DEVICES_PLOCHANDLER_H_
|
||||
#define MISSION_DEVICES_PLOCHANDLER_H_
|
||||
|
||||
#include <fsfw/devicehandlers/DeviceHandlerBase.h>
|
||||
#include <mission/devices/devicedefinitions/PlocDefinitions.h>
|
||||
#include <string.h>
|
||||
|
||||
/**
|
||||
* @brief This is the device handler for the PLOC.
|
||||
*
|
||||
* @details The PLOC uses the space packet protocol for communication. To each command the PLOC
|
||||
* answers with at least one acknowledgment and one execution report.
|
||||
*
|
||||
* @author J. Meier
|
||||
*/
|
||||
class PlocHandler: public DeviceHandlerBase {
|
||||
public:
|
||||
|
||||
PlocHandler(object_id_t objectId, object_id_t comIF, CookieIF * comCookie);
|
||||
virtual ~PlocHandler();
|
||||
|
||||
/**
|
||||
* @brief Sets mode to MODE_NORMAL. Can be used for debugging.
|
||||
*/
|
||||
void setModeNormal();
|
||||
|
||||
protected:
|
||||
void doStartUp() override;
|
||||
void doShutDown() override;
|
||||
ReturnValue_t buildNormalDeviceCommand(DeviceCommandId_t * id) override;
|
||||
ReturnValue_t buildTransitionDeviceCommand(DeviceCommandId_t * id) override;
|
||||
void fillCommandAndReplyMap() override;
|
||||
ReturnValue_t buildCommandFromCommand(DeviceCommandId_t deviceCommand,
|
||||
const uint8_t * commandData,size_t commandDataLen) override;
|
||||
ReturnValue_t scanForReply(const uint8_t *start, size_t remainingSize,
|
||||
DeviceCommandId_t *foundId, size_t *foundLen) override;
|
||||
ReturnValue_t interpretDeviceReply(DeviceCommandId_t id,
|
||||
const uint8_t *packet) override;
|
||||
void setNormalDatapoolEntriesInvalid() override;
|
||||
uint32_t getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) override;
|
||||
ReturnValue_t initializeLocalDataPool(localpool::DataPool& localDataPoolMap,
|
||||
LocalDataPoolManager& poolManager) override;
|
||||
ReturnValue_t enableReplyInReplyMap(DeviceCommandMap::iterator command,
|
||||
uint8_t expectedReplies = 1, bool useAlternateId = false,
|
||||
DeviceCommandId_t alternateReplyID = 0) override;
|
||||
size_t getNextReplyLength(DeviceCommandId_t deviceCommand) override;
|
||||
|
||||
private:
|
||||
|
||||
static const uint8_t INTERFACE_ID = CLASS_ID::PLOC_HANDLER;
|
||||
|
||||
static const ReturnValue_t CRC_FAILURE = MAKE_RETURN_CODE(0xA0); //!> Space Packet received from PLOC has invalid CRC
|
||||
static const ReturnValue_t RECEIVED_ACK_FAILURE = MAKE_RETURN_CODE(0xA1); //!> Received ACK failure reply from PLOC
|
||||
static const ReturnValue_t RECEIVED_EXE_FAILURE = MAKE_RETURN_CODE(0xA2); //!> Received execution failure reply from PLOC
|
||||
static const ReturnValue_t INVALID_APID = MAKE_RETURN_CODE(0xA3); //!> Received space packet with invalid APID from PLOC
|
||||
|
||||
static const uint8_t SUBSYSTEM_ID = SUBSYSTEM_ID::PLOC_HANDLER;
|
||||
|
||||
static const Event MEMORY_READ_RPT_CRC_FAILURE = MAKE_EVENT(1, severity::LOW); //!> PLOC crc failure in telemetry packet
|
||||
static const Event ACK_FAILURE = MAKE_EVENT(2, severity::LOW); //!> PLOC receive acknowledgment failure report
|
||||
static const Event EXE_FAILURE = MAKE_EVENT(3, severity::LOW); //!> PLOC receive execution failure report
|
||||
static const Event CRC_FAILURE_EVENT = MAKE_EVENT(4, severity::LOW); //!> PLOC reply has invalid crc
|
||||
|
||||
static const uint16_t APID_MASK = 0x7FF;
|
||||
static const uint16_t PACKET_SEQUENCE_COUNT_MASK = 0x3FFF;
|
||||
|
||||
uint8_t commandBuffer[PLOC::MAX_COMMAND_SIZE];
|
||||
|
||||
/**
|
||||
* @brief This object is incremented each time a packet is sent or received. By checking the
|
||||
* packet sequence count of a received packet, no packets can be lost without noticing
|
||||
* it. Only the least significant 14 bits represent the packet sequence count in a
|
||||
* space packet. Thus the maximum value amounts to 16383 (0x3FFF).
|
||||
* @note Normally this should never happen because the PLOC replies are always sent in a
|
||||
* fixed order. However, the PLOC software checks this value and will return an ACK
|
||||
* failure report in case the sequence count is not incremented with each transferred
|
||||
* space packet.
|
||||
*/
|
||||
uint16_t packetSequenceCount = 0x3FFF;
|
||||
|
||||
/**
|
||||
* This variable is used to store the id of the next reply to receive. This is necessary
|
||||
* because the PLOC sends as reply to each command at least one acknowledgment and execution
|
||||
* report.
|
||||
*/
|
||||
DeviceCommandId_t nextReplyId = PLOC::NONE;
|
||||
|
||||
/**
|
||||
* @brief This function fills the commandBuffer to initiate the write memory command.
|
||||
*
|
||||
* @param commandData Pointer to action command data.
|
||||
* @param commanDataLen Size of command data in bytes.
|
||||
*
|
||||
* @return RETURN_OK if successful, else RETURN_FAILURE.
|
||||
*/
|
||||
ReturnValue_t prepareTcMemWriteCommand(const uint8_t * commandData, size_t commandDataLen);
|
||||
|
||||
/**
|
||||
* @brief This function fills the commandBuffer to initiate the write reads command.
|
||||
*
|
||||
* @param commandData Pointer to action command data.
|
||||
* @param commanDataLen Size of command data in bytes.
|
||||
*
|
||||
* @return RETURN_OK if successful, else RETURN_FAILURE.
|
||||
*/
|
||||
ReturnValue_t prepareTcMemReadCommand(const uint8_t * commandData, size_t commandDataLen);
|
||||
|
||||
/**
|
||||
* @brief This function checks the crc of the received PLOC reply.
|
||||
*
|
||||
* @param start Pointer to the first byte of the reply.
|
||||
* @param foundLen Pointer to the length of the whole packet.
|
||||
*
|
||||
* @return RETURN_OK if CRC is ok, otherwise CRC_FAILURE.
|
||||
*/
|
||||
ReturnValue_t verifyPacket(const uint8_t* start, size_t foundLen);
|
||||
|
||||
/**
|
||||
* @brief This function handles the acknowledgment report.
|
||||
*
|
||||
* @param data Pointer to the data holding the acknowledgment report.
|
||||
*
|
||||
* @return RETURN_OK if successful, otherwise an error code.
|
||||
*/
|
||||
ReturnValue_t handleAckReport(const uint8_t* data);
|
||||
|
||||
/**
|
||||
* @brief This function handles the data of a execution report.
|
||||
*
|
||||
* @param data Pointer to the received data packet.
|
||||
*
|
||||
* @return RETURN_OK if successful, otherwise an error code.
|
||||
*/
|
||||
ReturnValue_t handleExecutionReport(const uint8_t* data);
|
||||
|
||||
/**
|
||||
* @brief This function handles the memory read report.
|
||||
*
|
||||
* @param data Pointer to the data buffer holding the memory read report.
|
||||
*
|
||||
* @return RETURN_OK if successful, otherwise an error code.
|
||||
*/
|
||||
ReturnValue_t handleMemoryReadReport(const uint8_t* data);
|
||||
|
||||
/**
|
||||
* @brief Depending on the current active command, this function sets the reply id of the
|
||||
* next reply after a successful acknowledgment report has been received. This is
|
||||
* required by the function getNextReplyLength() to identify the length of the next
|
||||
* reply to read.
|
||||
*/
|
||||
void setNextReplyId();
|
||||
|
||||
/**
|
||||
* @brief This function handles action message replies in case the telemetry has been
|
||||
* requested by another object.
|
||||
*
|
||||
* @param data Pointer to the telemetry data.
|
||||
* @param dataSize Size of telemetry in bytes.
|
||||
* @param replyId Id of the reply. This will be added to the ActionMessage.
|
||||
*/
|
||||
void handleDeviceTM(const uint8_t* data, size_t dataSize, DeviceCommandId_t replyId);
|
||||
|
||||
/**
|
||||
* @brief In case an acknowledgment failure reply has been received this function disables
|
||||
* all previously enabled commands and resets the exepected replies variable of an
|
||||
* active command.
|
||||
*/
|
||||
void disableAllReplies();
|
||||
|
||||
/**
|
||||
* @brief This function sends a failure report if the active action was commanded by an other
|
||||
* object.
|
||||
*
|
||||
* @param replyId The id of the reply which signals a failure.
|
||||
* @param status A status byte which gives information about the failure type.
|
||||
*/
|
||||
void sendFailureReport(DeviceCommandId_t replyId, ReturnValue_t status);
|
||||
|
||||
/**
|
||||
* @brief This function disables the execution report reply. Within this function also the
|
||||
* the variable expectedReplies of an active command will be set to 0.
|
||||
*/
|
||||
void disableExeReportReply();
|
||||
|
||||
/**
|
||||
* @brief This function checks and increments the packet sequence count of a received space
|
||||
* packet.
|
||||
*
|
||||
* @param data Pointer to a space packet.
|
||||
*
|
||||
* @return RETURN_OK if successful
|
||||
*
|
||||
* @details There should be never a case in which a wrong packet sequence count is received
|
||||
* because the communication scheme between PLOC and OBC always follows a strict
|
||||
* procedure. Thus this function mainly serves for debugging purposes to detected an
|
||||
* invalid handling of the packet sequence count.
|
||||
*/
|
||||
ReturnValue_t checkPacketSequenceCount(const uint8_t* data);
|
||||
};
|
||||
|
||||
#endif /* MISSION_DEVICES_PLOCHANDLER_H_ */
|
@ -6,46 +6,74 @@ namespace IMTQ {
|
||||
static const DeviceCommandId_t NONE = 0x0;
|
||||
static const DeviceCommandId_t GET_ENG_HK_DATA = 0x1;
|
||||
static const DeviceCommandId_t START_ACTUATION_DIPOLE = 0x2;
|
||||
static const DeviceCommandId_t GET_COMMANDED_DIPOLE = 0x3;
|
||||
/** Generates new measurement of the magnetic field */
|
||||
static const DeviceCommandId_t START_MTM_MEASUREMENT = 0x4;
|
||||
/** Requests the calibrated magnetometer measurement */
|
||||
static const DeviceCommandId_t GET_CAL_MTM_MEASUREMENT = 0x5;
|
||||
/** Requests the raw values measured by the built-in MTM XEN1210 */
|
||||
static const DeviceCommandId_t GET_RAW_MTM_MEASUREMENT = 0x6;
|
||||
static const DeviceCommandId_t SELF_TEST = 0x7;
|
||||
|
||||
static const uint8_t GET_TEMP_REPLY_SIZE = 2;
|
||||
static const uint8_t CFGR_CMD_SIZE = 3;
|
||||
static const uint8_t POINTER_REG_SIZE = 1;
|
||||
|
||||
static const uint32_t ENG_HK_DATA_SET_ID = GET_ENG_HK_DATA;
|
||||
static const uint32_t CAL_MTM_SET = GET_CAL_MTM_MEASUREMENT;
|
||||
|
||||
static const uint8_t SIZE_ENG_HK_COMMAND = 1;
|
||||
static const uint8_t SIZE_STATUS_REPLY = 2;
|
||||
static const uint8_t SIZE_ENG_HK_DATA_REPLY = 24;
|
||||
static const uint8_t SIZE_GET_COMMANDED_DIPOLE_REPLY = 8;
|
||||
static const uint8_t SIZE_GET_CAL_MTM_MEASUREMENT = 15;
|
||||
static const uint8_t SIZE_GET_RAW_MTM_MEASUREMENT = 15;
|
||||
|
||||
static const uint8_t MAX_REPLY_SIZE = SIZE_ENG_HK_DATA_REPLY;
|
||||
static const uint8_t MAX_COMMAND_SIZE = 9;
|
||||
|
||||
static const uint8_t POOL_ENTRIES = 11;
|
||||
/** Define entries in IMTQ specific dataset */
|
||||
static const uint8_t ENG_HK_SET_POOL_ENTRIES = 11;
|
||||
static const uint8_t CAL_MTM_POOL_ENTRIES = 4;
|
||||
|
||||
/**
|
||||
* Command code definitions. Each command or reply of an IMTQ request will begin with one of
|
||||
* the following command codes.
|
||||
*/
|
||||
namespace CC {
|
||||
static const uint8_t START_MTM_MEASUREMENT = 0x4;
|
||||
static const uint8_t START_ACTUATION_DIPOLE = 0x6;
|
||||
static const uint8_t SOFTWARE_RESET = 0xAA;
|
||||
static const uint8_t GET_ENG_HK_DATA = 0x4A;
|
||||
static const uint8_t GET_COMMANDED_DIPOLE = 0x46;
|
||||
static const uint8_t GET_RAW_MTM_MEASUREMENT = 0x42;
|
||||
static const uint8_t GET_CAL_MTM_MEASUREMENT = 0x43;
|
||||
};
|
||||
|
||||
enum IMTQPoolIds: lp_id_t {
|
||||
DIGITAL_VOLTAGE_MV,
|
||||
ANALOG_VOLTAGE_MV,
|
||||
DIGITAL_CURRENT_A,
|
||||
ANALOG_CURRENT_A,
|
||||
COIL_X_CURRENT_A,
|
||||
COIL_Y_CURRENT_A,
|
||||
COIL_Z_CURRENT_A,
|
||||
DIGITAL_CURRENT,
|
||||
ANALOG_CURRENT,
|
||||
COIL_X_CURRENT,
|
||||
COIL_Y_CURRENT,
|
||||
COIL_Z_CURRENT,
|
||||
COIL_X_TEMPERATURE,
|
||||
COIL_Y_TEMPERATURE,
|
||||
COIL_Z_TEMPERATURE,
|
||||
MCU_TEMPERATURE
|
||||
MCU_TEMPERATURE,
|
||||
MTM_CAL_X,
|
||||
MTM_CAL_Y,
|
||||
MTM_CAL_Z,
|
||||
ACTUATION_CAL_STATUS,
|
||||
MTM_RAW_X,
|
||||
MTM_RAW_Y,
|
||||
MTM_RAW_Z,
|
||||
ACTUATION_RAW_STATUS
|
||||
};
|
||||
|
||||
class EngHkDataset:
|
||||
public StaticLocalDataSet<POOL_ENTRIES> {
|
||||
public StaticLocalDataSet<ENG_HK_SET_POOL_ENTRIES> {
|
||||
public:
|
||||
|
||||
EngHkDataset(HasLocalDataPoolIF* owner):
|
||||
@ -60,16 +88,16 @@ public:
|
||||
DIGITAL_VOLTAGE_MV, this);
|
||||
lp_var_t<uint16_t> analogVoltageMv = lp_var_t<uint16_t>(sid.objectId,
|
||||
ANALOG_VOLTAGE_MV, this);
|
||||
lp_var_t<float> digitalCurrentA = lp_var_t<float>(sid.objectId,
|
||||
DIGITAL_CURRENT_A, this);
|
||||
lp_var_t<float> analogCurrentA = lp_var_t<float>(sid.objectId,
|
||||
ANALOG_CURRENT_A, this);
|
||||
lp_var_t<float> coilXcurrentA = lp_var_t<float>(sid.objectId,
|
||||
COIL_X_CURRENT_A, this);
|
||||
lp_var_t<float> coilYcurrentA = lp_var_t<float>(sid.objectId,
|
||||
COIL_Y_CURRENT_A, this);
|
||||
lp_var_t<float> coilZcurrentA = lp_var_t<float>(sid.objectId,
|
||||
COIL_Z_CURRENT_A, this);
|
||||
lp_var_t<float> digitalCurrentmA = lp_var_t<float>(sid.objectId,
|
||||
DIGITAL_CURRENT, this);
|
||||
lp_var_t<float> analogCurrentmA = lp_var_t<float>(sid.objectId,
|
||||
ANALOG_CURRENT, this);
|
||||
lp_var_t<float> coilXCurrentmA = lp_var_t<float>(sid.objectId,
|
||||
COIL_X_CURRENT, this);
|
||||
lp_var_t<float> coilYCurrentmA = lp_var_t<float>(sid.objectId,
|
||||
COIL_Y_CURRENT, this);
|
||||
lp_var_t<float> coilZCurrentmA = lp_var_t<float>(sid.objectId,
|
||||
COIL_Z_CURRENT, this);
|
||||
/** All temperatures in [°C] */
|
||||
lp_var_t<uint16_t> coilXTemperature = lp_var_t<uint16_t>(sid.objectId,
|
||||
COIL_X_TEMPERATURE, this);
|
||||
@ -81,6 +109,60 @@ public:
|
||||
MCU_TEMPERATURE, this);
|
||||
};
|
||||
|
||||
/**
|
||||
* @brief This dataset holds the raw MTM measurements.
|
||||
*/
|
||||
class CalibratedMtmMeasurementSet:
|
||||
public StaticLocalDataSet<CAL_MTM_POOL_ENTRIES> {
|
||||
public:
|
||||
|
||||
CalibratedMtmMeasurementSet(HasLocalDataPoolIF* owner):
|
||||
StaticLocalDataSet(owner, CAL_MTM_SET) {
|
||||
}
|
||||
|
||||
CalibratedMtmMeasurementSet(object_id_t objectId):
|
||||
StaticLocalDataSet(sid_t(objectId, CAL_MTM_SET)) {
|
||||
}
|
||||
|
||||
/** The unit of all measurements is nT */
|
||||
lp_var_t<int32_t> mtmXnT = lp_var_t<int32_t>(sid.objectId,
|
||||
MTM_CAL_X, this);
|
||||
lp_var_t<int32_t> mtmYnT = lp_var_t<int32_t>(sid.objectId,
|
||||
MTM_CAL_Y, this);
|
||||
lp_var_t<int32_t> mtmZnT = lp_var_t<int32_t>(sid.objectId,
|
||||
MTM_CAL_Z, this);
|
||||
/** 1 if coils were actuating during measurement otherwise 0 */
|
||||
lp_var_t<uint8_t> coilActuationStatus = lp_var_t<uint8_t>(sid.objectId,
|
||||
ACTUATION_CAL_STATUS, this);
|
||||
};
|
||||
|
||||
/**
|
||||
* @brief This dataset holds the last calibrated MTM measurement.
|
||||
*/
|
||||
class RawMtmMeasurementSet:
|
||||
public StaticLocalDataSet<CAL_MTM_POOL_ENTRIES> {
|
||||
public:
|
||||
|
||||
RawMtmMeasurementSet(HasLocalDataPoolIF* owner):
|
||||
StaticLocalDataSet(owner, CAL_MTM_SET) {
|
||||
}
|
||||
|
||||
RawMtmMeasurementSet(object_id_t objectId):
|
||||
StaticLocalDataSet(sid_t(objectId, CAL_MTM_SET)) {
|
||||
}
|
||||
|
||||
/** The unit of all measurements is nT */
|
||||
lp_var_t<float> mtmXnT = lp_var_t<float>(sid.objectId,
|
||||
MTM_RAW_X, this);
|
||||
lp_var_t<float> mtmYnT = lp_var_t<float>(sid.objectId,
|
||||
MTM_RAW_Y, this);
|
||||
lp_var_t<float> mtmZnT = lp_var_t<float>(sid.objectId,
|
||||
MTM_RAW_Z, this);
|
||||
/** 1 if coils were actuating during measurement otherwise 0 */
|
||||
lp_var_t<uint8_t> coilActuationStatus = lp_var_t<uint8_t>(sid.objectId,
|
||||
ACTUATION_RAW_STATUS, this);
|
||||
};
|
||||
|
||||
/**
|
||||
* @brief This class can be used to ease the generation of an action message commanding the
|
||||
* IMTQHandler to configure the magnettorquer with the desired dipoles.
|
||||
|
172
mission/devices/devicedefinitions/PlocDefinitions.h
Normal file
172
mission/devices/devicedefinitions/PlocDefinitions.h
Normal file
@ -0,0 +1,172 @@
|
||||
#ifndef MISSION_DEVICES_DEVICEDEFINITIONS_PLOCDEFINITIONS_H_
|
||||
#define MISSION_DEVICES_DEVICEDEFINITIONS_PLOCDEFINITIONS_H_
|
||||
|
||||
#include <fsfw/tmtcpacket/SpacePacket.h>
|
||||
#include <fsfw/globalfunctions/CRC.h>
|
||||
#include <fsfw/serialize/SerializeAdapter.h>
|
||||
|
||||
namespace PLOC {
|
||||
|
||||
static const DeviceCommandId_t NONE = 0x0;
|
||||
static const DeviceCommandId_t TC_MEM_WRITE = 0x1;
|
||||
static const DeviceCommandId_t TC_MEM_READ = 0x2;
|
||||
static const DeviceCommandId_t ACK_REPORT = 0x3;
|
||||
static const DeviceCommandId_t EXE_REPORT = 0x5;
|
||||
static const DeviceCommandId_t TM_MEMORY_READ_REPORT = 0x6;
|
||||
|
||||
static const uint16_t SIZE_ACK_REPORT = 14;
|
||||
static const uint16_t SIZE_EXE_REPORT = 14;
|
||||
static const uint16_t SIZE_TM_MEM_READ_REPORT = 18;
|
||||
|
||||
/**
|
||||
* SpacePacket apids of PLOC telecommands and telemetry.
|
||||
*/
|
||||
static const uint16_t APID_TC_MEM_WRITE = 0x714;
|
||||
static const uint16_t APID_TC_MEM_READ = 0x715;
|
||||
static const uint16_t APID_TM_MEMORY_READ_REPORT = 0x404;
|
||||
static const uint16_t APID_ACK_SUCCESS = 0x400;
|
||||
static const uint16_t APID_ACK_FAILURE = 0x401;
|
||||
static const uint16_t APID_EXE_SUCCESS = 0x402;
|
||||
static const uint16_t APID_EXE_FAILURE = 0x403;
|
||||
|
||||
/** Offset from first byte in Space packet to first byte of data field */
|
||||
static const uint8_t DATA_FIELD_OFFSET = 6;
|
||||
|
||||
/**
|
||||
* The size of payload data which will be forwarded to the requesting object. e.g. PUS Service
|
||||
* 8.
|
||||
*/
|
||||
static const uint8_t SIZE_MEM_READ_REPORT_DATA = 10;
|
||||
|
||||
/**
|
||||
* PLOC space packet length for fixed size packets. This is the size of the whole packet data
|
||||
* field. For the length field in the space packet this size will be substracted by one.
|
||||
*/
|
||||
static const uint16_t LENGTH_TC_MEM_WRITE = 12;
|
||||
static const uint16_t LENGTH_TC_MEM_READ = 8;
|
||||
|
||||
static const size_t MAX_REPLY_SIZE = SIZE_TM_MEM_READ_REPORT;
|
||||
static const size_t MAX_COMMAND_SIZE = 18;
|
||||
|
||||
/**
|
||||
* @brief This class helps to build the memory read command for the PLOC.
|
||||
*
|
||||
* @details The last two bytes of the packet data field contain a CRC calculated over the whole
|
||||
* space packet. This is the CRC-16-CCITT as specified in
|
||||
* ECSS-E-ST-70-41C – Telemetry and telecommand packet utilization.
|
||||
*/
|
||||
class TcMemRead : public SpacePacket {
|
||||
public:
|
||||
|
||||
/**
|
||||
* @brief Constructor
|
||||
*
|
||||
* @param memAddr The memory address to read from.
|
||||
*/
|
||||
TcMemRead(const uint32_t memAddr, uint16_t sequenceCount) :
|
||||
SpacePacket(LENGTH_TC_MEM_READ - 1, true, APID_TC_MEM_READ, sequenceCount) {
|
||||
fillPacketDataField(&memAddr);
|
||||
}
|
||||
|
||||
private:
|
||||
|
||||
/**
|
||||
* @brief This function builds the packet data field for the mem read command.
|
||||
*
|
||||
* @param memAddrPtr Pointer to the memory address to read from.
|
||||
*/
|
||||
void fillPacketDataField(const uint32_t* memAddrPtr) {
|
||||
/* Add memAddr to packet data field */
|
||||
size_t serializedSize = 0;
|
||||
uint8_t* memoryAddressPos = this->localData.fields.buffer;
|
||||
SerializeAdapter::serialize<uint32_t>(memAddrPtr, &memoryAddressPos, &serializedSize,
|
||||
sizeof(*memAddrPtr), SerializeIF::Endianness::LITTLE);
|
||||
|
||||
/* Add memLen to packet data field */
|
||||
this->localData.fields.buffer[OFFSET_MEM_LEN_FIELD] = 1;
|
||||
this->localData.fields.buffer[OFFSET_MEM_LEN_FIELD + 1] = 0;
|
||||
|
||||
/* Calculate crc */
|
||||
uint16_t crc = CRC::crc16ccitt(this->localData.byteStream,
|
||||
sizeof(CCSDSPrimaryHeader) + LENGTH_TC_MEM_READ - CRC_SIZE);
|
||||
|
||||
/* Add crc to packet data field of space packet */
|
||||
serializedSize = 0;
|
||||
uint8_t* crcPos = this->localData.fields.buffer + CRC_OFFSET;
|
||||
SerializeAdapter::serialize<uint16_t>(&crc, &crcPos, &serializedSize,
|
||||
sizeof(crc), SerializeIF::Endianness::BIG);
|
||||
}
|
||||
|
||||
static const uint8_t OFFSET_MEM_LEN_FIELD = 4;
|
||||
static const uint8_t CRC_OFFSET = 6;
|
||||
|
||||
};
|
||||
|
||||
/**
|
||||
* @brief This class helps to generate the space packet to write to a memory address within
|
||||
* the PLOC.
|
||||
* @details The last two bytes of the packet data field contain a CRC calculated over the whole
|
||||
* space packet. This is the CRC-16-CCITT as specified in
|
||||
* ECSS-E-ST-70-41C – Telemetry and telecommand packet utilization.
|
||||
*/
|
||||
class TcMemWrite : public SpacePacket {
|
||||
public:
|
||||
/**
|
||||
* @brief Constructor
|
||||
*
|
||||
* @param memAddr The PLOC memory address where to write to.
|
||||
* @param memoryData The data to write to the specified memory address.
|
||||
* @param sequenceCount The subsequence count. Must be incremented with each new packet.
|
||||
*/
|
||||
TcMemWrite(const uint32_t memAddr, const uint32_t memoryData, uint16_t sequenceCount) :
|
||||
SpacePacket(LENGTH_TC_MEM_WRITE - 1, true, APID_TC_MEM_WRITE, sequenceCount) {
|
||||
fillPacketDataField(&memAddr, &memoryData);
|
||||
}
|
||||
|
||||
private:
|
||||
|
||||
/**
|
||||
* @brief This function builds the packet data field for the mem write command.
|
||||
*
|
||||
* @param memAddrPtr Pointer to the PLOC memory address where to write to.
|
||||
* @param memoryDataPtr Pointer to the memoryData to write
|
||||
*/
|
||||
void fillPacketDataField(const uint32_t* memAddrPtr, const uint32_t* memoryDataPtr) {
|
||||
|
||||
/* Add memAddr to packet data field */
|
||||
size_t serializedSize = 0;
|
||||
uint8_t* memoryAddressPos = this->localData.fields.buffer;
|
||||
SerializeAdapter::serialize<uint32_t>(memAddrPtr, &memoryAddressPos, &serializedSize,
|
||||
sizeof(*memAddrPtr), SerializeIF::Endianness::BIG);
|
||||
|
||||
/* Add memLen to packet data field */
|
||||
this->localData.fields.buffer[OFFSET_MEM_LEN_FIELD] = 1;
|
||||
this->localData.fields.buffer[OFFSET_MEM_LEN_FIELD + 1] = 0;
|
||||
|
||||
/* Add memData to packet data field */
|
||||
serializedSize = 0;
|
||||
uint8_t* memoryDataPos = this->localData.fields.buffer + OFFSET_MEM_DATA_FIELD;
|
||||
SerializeAdapter::serialize<uint32_t>(memoryDataPtr, &memoryDataPos, &serializedSize,
|
||||
sizeof(*memoryDataPtr), SerializeIF::Endianness::BIG);
|
||||
|
||||
/* Calculate crc */
|
||||
uint16_t crc = CRC::crc16ccitt(this->localData.byteStream,
|
||||
sizeof(CCSDSPrimaryHeader) + LENGTH_TC_MEM_WRITE - CRC_SIZE);
|
||||
|
||||
serializedSize = 0;
|
||||
uint8_t* crcPos = this->localData.fields.buffer + CRC_OFFSET;
|
||||
/* Add crc to packet data field of space packet */
|
||||
SerializeAdapter::serialize<uint16_t>(&crc, &crcPos, &serializedSize,
|
||||
sizeof(crc), SerializeIF::Endianness::BIG);
|
||||
}
|
||||
|
||||
/** Offsets from base address of packet data field */
|
||||
static const uint8_t OFFSET_MEM_LEN_FIELD = 4;
|
||||
static const uint8_t OFFSET_MEM_DATA_FIELD = 6;
|
||||
static const uint8_t CRC_OFFSET = 10;
|
||||
};
|
||||
|
||||
}
|
||||
|
||||
|
||||
#endif /* MISSION_DEVICES_DEVICEDEFINITIONS_PLOCDEFINITIONS_H_ */
|
2
tmtc
2
tmtc
@ -1 +1 @@
|
||||
Subproject commit 7cc06ef0e0882d286bab8156ca756e0211e5ebae
|
||||
Subproject commit 06750809cb52044392e0683896538a652f11a512
|
Loading…
Reference in New Issue
Block a user