diff --git a/bsp_linux/comIF/CspComIF.cpp b/bsp_linux/comIF/CspComIF.cpp index 6716c530..e7616bba 100644 --- a/bsp_linux/comIF/CspComIF.cpp +++ b/bsp_linux/comIF/CspComIF.cpp @@ -59,7 +59,7 @@ ReturnValue_t CspComIF::initializeInterface(CookieIF *cookie) { uint8_t cspAddress = cspCookie->getCspAddress(); uint16_t maxReplyLength = cspCookie->getMaxReplyLength(); - if(cspDeviceMap.find(cspAddress) != cspDeviceMap.end()){ + if(cspDeviceMap.find(cspAddress) == cspDeviceMap.end()){ /* Insert device information in CSP map */ cspDeviceMap.emplace(cspAddress, vectorBuffer(maxReplyLength)); } @@ -145,14 +145,19 @@ ReturnValue_t CspComIF::cspTransfer(uint8_t cspAddress, uint8_t cspPort, const uint8_t* cmdBuffer, int cmdBufferLen, uint16_t querySize) { uint32_t timeout_ms = 1000; - uint8_t* replyBuffer = cspDeviceMap[cspAddress].data(); + vectorBufferIter iter = cspDeviceMap.find(cspAddress); + if(iter == cspDeviceMap.end()){ + sif::error << "CSP device with address " << cspAddress << " no found in" + << " device map" << std::endl; + } + uint8_t* replyBuffer = iter->second.data(); uint8_t tmpCmdBuffer[cmdBufferLen]; memcpy(tmpCmdBuffer, cmdBuffer, cmdBufferLen); csp_conn_t * conn = csp_connect(CSP_PRIO_HIGH, cspAddress, cspPort, 0, CSP_O_NONE); - querySize = 12; + querySize = 14; int receivedBytes = csp_transaction_persistent(conn, timeout_ms, tmpCmdBuffer, cmdBufferLen, replyBuffer, querySize); if(receivedBytes != querySize){ diff --git a/mission/devices/P60DockHandler.cpp b/mission/devices/P60DockHandler.cpp index ab914681..f1950f50 100644 --- a/mission/devices/P60DockHandler.cpp +++ b/mission/devices/P60DockHandler.cpp @@ -43,14 +43,15 @@ ReturnValue_t P60DockHandler::buildCommandFromCommand( /* Unpack the received action message */ GetParamMessageUnpacker getParamMessage(commandData, commandDataLen); uint8_t tableId = getParamMessage.getTableId(); - uint16_t address = getParamMessage.getAddress(); + uint16_t address = EndianConverter::convertLittleEndian( + getParamMessage.getAddress()); uint16_t length = EndianConverter::convertLittleEndian( sizeof(address)); uint16_t checksum = GOMSPACE::IGNORE_CHECKSUM; uint16_t seq = 0; uint16_t total = 0; - uint16_t querySize = getParamMessage.getQuerySize(); - + uint16_t querySize = getParamMessage.getQuerySize() + + CspGetParamCommand::GS_HDR_LENGTH; /* Generate the CSP command to send to the P60 Dock */ CspGetParamCommand getParamCmd(querySize, PARAM_GET, tableId, length, checksum, seq, total, address); diff --git a/mission/devices/devicedefinitions/GomSpacePackets.h b/mission/devices/devicedefinitions/GomSpacePackets.h index 5fab5a8b..025beeb8 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 = 0x0bb0; + static const uint16_t IGNORE_CHECKSUM = 0xb00b; /* CSP port to ping gomspace devices. */ static const uint8_t PING_PORT = 1; static const uint8_t REBOOT_PORT = 4; @@ -111,6 +111,8 @@ private: */ class CspGetParamCommand : public SerialLinkedListAdapter { public: + /* The size of the header of a gomspace CSP packet. */ + static const uint8_t GS_HDR_LENGTH = 12; CspGetParamCommand(uint16_t querySize_, uint8_t action_, uint8_t tableId_, uint16_t addresslength_, uint16_t checksum_, uint16_t seq_,