From 32bd57d6d19bdaee50540d157664112c9af9b106 Mon Sep 17 00:00:00 2001 From: "Jakob.Meier" Date: Wed, 16 Dec 2020 15:17:10 +0100 Subject: [PATCH] ping test --- bsp_linux/comIF/CspComIF.cpp | 2 +- mission/devices/P60DockHandler.cpp | 45 ++++++++++-- mission/devices/P60DockHandler.h | 2 + .../devicedefinitions/GomSpacePackets.h | 73 +++++++++++++++++-- 4 files changed, 107 insertions(+), 15 deletions(-) diff --git a/bsp_linux/comIF/CspComIF.cpp b/bsp_linux/comIF/CspComIF.cpp index 00f11c7e..afebeb83 100644 --- a/bsp_linux/comIF/CspComIF.cpp +++ b/bsp_linux/comIF/CspComIF.cpp @@ -86,7 +86,6 @@ ReturnValue_t CspComIF::sendMessage(CookieIF *cookie, SerializeAdapter::deSerialize(&querySize, &sendData, &sendLen, SerializeIF::Endianness::BIG); uint8_t cspAddress = cspCookie->getCspAddress(); - if(cspPort == csp_reserved_ports_e::CSP_PING){ uint32_t timeout = 1000; // ms unsigned int pingSize = 100; // 100 bytes @@ -168,6 +167,7 @@ ReturnValue_t CspComIF::cspTransfer(uint8_t cspAddress, uint8_t cspPort, } else { if(result != 1){ sif::error << "CSP transfer failed" << std::endl; + return HasReturnvaluesIF::RETURN_FAILED; } } diff --git a/mission/devices/P60DockHandler.cpp b/mission/devices/P60DockHandler.cpp index a7f9b820..cf584039 100644 --- a/mission/devices/P60DockHandler.cpp +++ b/mission/devices/P60DockHandler.cpp @@ -34,6 +34,22 @@ ReturnValue_t P60DockHandler::buildCommandFromCommand( size_t commandDataLen) { switch(deviceCommand) { case(PING): { + PingMessageUnpacker pingMessageUnpacker(commandData, commandDataLen); + const uint8_t* pingData = pingMessageUnpacker.getPingData(); + uint8_t pingDataSz = pingMessageUnpacker.getPingDataSz(); + CspPingCommand cspPingCommand(pingDataSz, pingData); + size_t cspPacketLen = 0; + uint8_t* buffer = cspPacket; + cspPingCommand.serialize(&buffer, &cspPacketLen, sizeof(cspPacket), + SerializeIF::Endianness::BIG); + if(cspPacketLen > MAX_PACKET_LEN){ + sif::error << "P60DockHandler: Received invalid ping message" + << std::endl; + return HasReturnvaluesIF::RETURN_FAILED; + } + rawPacket = cspPacket; + rawPacketLen = cspPacketLen; + rememberCommandId = PING; break; } case(PARAM_SET):{ @@ -63,16 +79,16 @@ ReturnValue_t P60DockHandler::buildCommandFromCommand( } rawPacket = cspPacket; rawPacketLen = cspPacketLen; + rememberRequestedSize = querySize; + rememberCommandId = PARAM_SET; break; } case(PARAM_GET):{ /* Unpack the received action message */ GetParamMessageUnpacker getParamMessage(commandData, commandDataLen); uint8_t tableId = getParamMessage.getTableId(); - uint16_t address = EndianConverter::convertLittleEndian( - getParamMessage.getAddress()); - uint16_t length = EndianConverter::convertLittleEndian( - sizeof(address)); + uint16_t address = getParamMessage.getAddress(); + uint16_t length = sizeof(address); uint16_t checksum = GOMSPACE::IGNORE_CHECKSUM; uint16_t seq = 0; uint16_t total = 0; @@ -113,7 +129,7 @@ ReturnValue_t P60DockHandler::scanForReply(const uint8_t *start, switch(rememberCommandId) { case(PING): *foundId = PING; - *foundLen = rememberRequestedSize; + *foundLen = PING_REPLY_SIZE; rememberCommandId = NONE; break; case(PARAM_GET): { @@ -122,6 +138,12 @@ ReturnValue_t P60DockHandler::scanForReply(const uint8_t *start, rememberCommandId = NONE; break; } + case(PARAM_SET): { + *foundId = PARAM_SET; + *foundLen = rememberRequestedSize; + rememberCommandId = NONE; + break; + } default: return IGNORE_REPLY_DATA; } @@ -132,13 +154,15 @@ ReturnValue_t P60DockHandler::interpretDeviceReply(DeviceCommandId_t id, const uint8_t *packet) { switch(id) { case(PING): { - handleDeviceTM((SerializeIF*)packet, id, true, true); + PingReply pingReply(*packet, *(packet + 1)); + handleDeviceTM(&pingReply, id, true, true); break; } case(PARAM_GET): { // -2 to subtract address size from gomspace parameter reply packet uint16_t payloadLength = (*(packet + 2) << 8 | *(packet + 3)) - 2; uint8_t tempPayloadBuffer[payloadLength]; + /* Extract information from received data */ CspGetParamReply cspGetParamReply(tempPayloadBuffer, payloadLength); size_t size = CspGetParamReply::GS_HDR_LENGTH + payloadLength; cspGetParamReply.deSerialize(&packet, &size, @@ -146,11 +170,20 @@ ReturnValue_t P60DockHandler::interpretDeviceReply(DeviceCommandId_t id, uint8_t action = cspGetParamReply.getAction(); uint8_t tableId = cspGetParamReply.getTableId(); uint16_t address = cspGetParamReply.getAddress(); + /* Pack relevant information into a tm reply packet */ ParamReply paramReply(action, tableId, address, payloadLength, tempPayloadBuffer); handleDeviceTM(¶mReply, id, true); break; } + case(PARAM_SET): { + /* When setting a parameter, the p60dock sends back the state of the + * operation */ + if(*packet != PARAM_SET_OK){ + return HasReturnvaluesIF::RETURN_FAILED; + } + break; + } default: break; } diff --git a/mission/devices/P60DockHandler.h b/mission/devices/P60DockHandler.h index abb61ba4..3c4cb3bb 100644 --- a/mission/devices/P60DockHandler.h +++ b/mission/devices/P60DockHandler.h @@ -34,6 +34,8 @@ protected: private: static const uint8_t MAX_PACKET_LEN = 36; + static const uint8_t PARAM_SET_OK = 1; + static const uint8_t PING_REPLY_SIZE = 2; /* Device commands are derived from the rparam.h of the gomspace lib */ static const DeviceCommandId_t PING = 0x1; //!< [EXPORT] : [COMMAND] static const DeviceCommandId_t NONE = 0x2; // Set when no command is pending diff --git a/mission/devices/devicedefinitions/GomSpacePackets.h b/mission/devices/devicedefinitions/GomSpacePackets.h index 4da31b08..88977a86 100644 --- a/mission/devices/devicedefinitions/GomSpacePackets.h +++ b/mission/devices/devicedefinitions/GomSpacePackets.h @@ -6,7 +6,7 @@ #include "fsfw/serialize/SerialLinkedListAdapter.h" namespace GOMSPACE{ - static const uint16_t IGNORE_CHECKSUM = 0xb00b; + static const uint16_t IGNORE_CHECKSUM = 0xbb0; /* CSP port to ping gomspace devices. */ static const uint8_t PING_PORT = 1; static const uint8_t REBOOT_PORT = 4; @@ -16,14 +16,15 @@ namespace GOMSPACE{ /** * @brief A serial linked list adapter implementation to generate ping - * messages for gomspace devices. + * commands for devices supporting the CSP protocol. This command can + * be sent to the CspComIF which will send out the ping request. * * @details A ping request simply sends back the received data provided by the * data buffer. cspPort and querySize are only informations required * by the CspComI and other than the data array not physically * transmitted to the target device. */ -class CspPing : public SerialLinkedListAdapter { +class CspPingCommand : public SerialLinkedListAdapter { public: /** * @brief Constructor @@ -32,16 +33,14 @@ public: * Amounts to the number of bytes send. * @param parameters_ Pointer to data which should be sent to the device. * All data will be sent back by the ping target. - * @param paramterCount_ Number of bytes to send with the ping request. */ - CspPing(uint16_t querySize_, const uint8_t* parameters_, - uint8_t parameterCount_) : - querySize(querySize_), data(parameters_, parameterCount_) { + CspPingCommand(uint16_t querySize_, const uint8_t* parameters_) : + querySize(querySize_), data(parameters_, querySize_) { setLinks(); } private: - CspPing(const CspPing &command); + CspPingCommand(const CspPingCommand &command); void setLinks() { setStart(&cspPort); cspPort.setNext(&querySize); @@ -258,6 +257,28 @@ private: }; +/** + * @brief This class generates telemetry packets containing data from + * CSP get-parameter-replies. + */ +class PingReply : public SerialLinkedListAdapter { +public: + PingReply(uint8_t action_, uint8_t replyTime_) : + action(action_), replyTime(replyTime_) { + setLinks(); + } + +private: + PingReply(const PingReply &reply); + void setLinks() { + setStart(&action); + action.setNext(&replyTime); + } + SerializeElement action; + SerializeElement replyTime; +}; + + /** * @brief This class helps to unpack information from an action messages * to set a parameter in gomspace devices. The action message can be @@ -340,4 +361,40 @@ private: }; +/** + * @brief This class helps to extract the information of a message received + * from an other software component (e.g. the service 8) to generate + * a ping request for gomspace devices. + */ +class PingMessageUnpacker: public SerialLinkedListAdapter { +public: + + PingMessageUnpacker(const uint8_t* commandData, size_t commandDataLen) { + SerializeAdapter::deSerialize(&action, &commandData, &commandDataLen, + SerializeIF::Endianness::BIG); + pingData = commandData; + pingDataSz = commandDataLen; + } + + uint8_t getAction() const { + return action; + } + + const uint8_t* getPingData() { + return pingData; + } + + uint8_t getPingDataSz(){ + return pingDataSz; + } + + +private: + PingMessageUnpacker(const PingMessageUnpacker &message); + uint8_t action; + const uint8_t * pingData; + uint8_t pingDataSz; +}; + + #endif /* MISSION_DEVICES_DEVICEDEFINITIONS_GOMSPACEPACKETS_H_ */