From 3d0d10dc3255add47c8576a8b7b7e9ec0bbff6af Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 18 Aug 2022 17:27:39 +0200 Subject: [PATCH 01/52] try IMTQ change --- mission/controller/AcsController.cpp | 2 +- mission/controller/AcsController.h | 3 +-- .../controllerdefinitions/AcsCtrlDefinitions.h | 2 +- mission/devices/IMTQHandler.cpp | 17 ++++++++--------- .../devicedefinitions/IMTQHandlerDefinitions.h | 8 ++------ tmtc | 2 +- 6 files changed, 14 insertions(+), 20 deletions(-) diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp index 48c6f521..c97fd21b 100644 --- a/mission/controller/AcsController.cpp +++ b/mission/controller/AcsController.cpp @@ -89,7 +89,7 @@ void AcsController::copyMgmData() { { PoolReadGuard pg(&imtqMgmSet); if (pg.getReadResult() == RETURN_OK) { - std::memcpy(mgmData.imtqCal.value, imtqMgmSet.mgmXyz.value, 3 * sizeof(int32_t)); + std::memcpy(mgmData.imtqRaw.value, imtqMgmSet.mtmRawNt.value, 3 * sizeof(float)); mgmData.actuationCalStatus.value = imtqMgmSet.coilActuationStatus.value; } } diff --git a/mission/controller/AcsController.h b/mission/controller/AcsController.h index 246506b9..ce1bd7b0 100644 --- a/mission/controller/AcsController.h +++ b/mission/controller/AcsController.h @@ -42,8 +42,7 @@ class AcsController : public ExtendedControllerBase { MGMLIS3MDL::MgmPrimaryDataset(objects::MGM_2_LIS3_HANDLER); RM3100::Rm3100PrimaryDataset mgm3Rm3100Set = RM3100::Rm3100PrimaryDataset(objects::MGM_3_RM3100_HANDLER); - IMTQ::CalibratedMtmMeasurementSet imtqMgmSet = - IMTQ::CalibratedMtmMeasurementSet(objects::IMTQ_HANDLER); + IMTQ::RawMtmMeasurementSet imtqMgmSet = IMTQ::RawMtmMeasurementSet(objects::IMTQ_HANDLER); PoolEntry mgm0PoolVec = PoolEntry(3); PoolEntry mgm1PoolVec = PoolEntry(3); diff --git a/mission/controller/controllerdefinitions/AcsCtrlDefinitions.h b/mission/controller/controllerdefinitions/AcsCtrlDefinitions.h index cec18a63..7c81071a 100644 --- a/mission/controller/controllerdefinitions/AcsCtrlDefinitions.h +++ b/mission/controller/controllerdefinitions/AcsCtrlDefinitions.h @@ -34,7 +34,7 @@ class MgmData : public StaticLocalDataSet { lp_vec_t mgm2Lis3 = lp_vec_t(sid.objectId, MGM_2_LIS3_UT, this); lp_vec_t mgm3Rm3100 = lp_vec_t(sid.objectId, MGM_3_RM3100_UT, this); // The IMTQ measurements are in integer nT - lp_vec_t imtqCal = lp_vec_t(sid.objectId, MGM_IMTQ_CAL_NT, this); + lp_vec_t imtqRaw = lp_vec_t(sid.objectId, MGM_IMTQ_CAL_NT, this); lp_var_t actuationCalStatus = lp_var_t(sid.objectId, MGM_IMTQ_CAL_ACT_STATUS, this); diff --git a/mission/devices/IMTQHandler.cpp b/mission/devices/IMTQHandler.cpp index 91e378ca..a421ae21 100644 --- a/mission/devices/IMTQHandler.cpp +++ b/mission/devices/IMTQHandler.cpp @@ -335,9 +335,7 @@ ReturnValue_t IMTQHandler::initializeLocalDataPool(localpool::DataPool& localDat localDataPoolMap.emplace(IMTQ::ACTUATION_CAL_STATUS, new PoolEntry({0})); /** Entries of raw MTM measurement dataset */ - localDataPoolMap.emplace(IMTQ::MTM_RAW_X, new PoolEntry({0})); - localDataPoolMap.emplace(IMTQ::MTM_RAW_Y, new PoolEntry({0})); - localDataPoolMap.emplace(IMTQ::MTM_RAW_Z, new PoolEntry({0})); + localDataPoolMap.emplace(IMTQ::MTM_RAW, new PoolEntry(3)); localDataPoolMap.emplace(IMTQ::ACTUATION_RAW_STATUS, new PoolEntry({0})); /** INIT measurements for positive X axis test */ @@ -781,28 +779,29 @@ void IMTQHandler::fillCalibratedMtmDataset(const uint8_t* packet) { void IMTQHandler::fillRawMtmDataset(const uint8_t* packet) { PoolReadGuard rg(&rawMtmMeasurementSet); int8_t offset = 2; - rawMtmMeasurementSet.mtmXnT = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 | + rawMtmMeasurementSet.mtmRawNt[0] = (*(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 | + rawMtmMeasurementSet.mtmRawNt[1] = (*(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 | + rawMtmMeasurementSet.mtmRawNt[2] = (*(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)); + rawMtmMeasurementSet.setValidity(true, true); if (debugMode) { #if OBSW_VERBOSE_LEVEL >= 1 - sif::info << "IMTQ raw MTM measurement X: " << rawMtmMeasurementSet.mtmXnT << " nT" + sif::info << "IMTQ raw MTM measurement X: " << rawMtmMeasurementSet.mtmRawNt[0] << " nT" << std::endl; - sif::info << "IMTQ raw MTM measurement Y: " << rawMtmMeasurementSet.mtmYnT << " nT" + sif::info << "IMTQ raw MTM measurement Y: " << rawMtmMeasurementSet.mtmRawNt[1] << " nT" << std::endl; - sif::info << "IMTQ raw MTM measurement Z: " << rawMtmMeasurementSet.mtmZnT << " nT" + sif::info << "IMTQ raw MTM measurement Z: " << rawMtmMeasurementSet.mtmRawNt[2] << " nT" << std::endl; sif::info << "IMTQ coil actuation status during MTM measurement: " << (unsigned int)rawMtmMeasurementSet.coilActuationStatus.value << std::endl; diff --git a/mission/devices/devicedefinitions/IMTQHandlerDefinitions.h b/mission/devices/devicedefinitions/IMTQHandlerDefinitions.h index e6661f84..485be5da 100644 --- a/mission/devices/devicedefinitions/IMTQHandlerDefinitions.h +++ b/mission/devices/devicedefinitions/IMTQHandlerDefinitions.h @@ -117,9 +117,7 @@ enum IMTQPoolIds : lp_id_t { MCU_TEMPERATURE, MGM_CAL_NT, ACTUATION_CAL_STATUS, - MTM_RAW_X, - MTM_RAW_Y, - MTM_RAW_Z, + MTM_RAW, ACTUATION_RAW_STATUS, INIT_POS_X_ERR, @@ -422,9 +420,7 @@ class RawMtmMeasurementSet : public StaticLocalDataSet { RawMtmMeasurementSet(object_id_t objectId) : StaticLocalDataSet(sid_t(objectId, RAW_MTM_SET)) {} /** The unit of all measurements is nT */ - lp_var_t mtmXnT = lp_var_t(sid.objectId, MTM_RAW_X, this); - lp_var_t mtmYnT = lp_var_t(sid.objectId, MTM_RAW_Y, this); - lp_var_t mtmZnT = lp_var_t(sid.objectId, MTM_RAW_Z, this); + lp_vec_t mtmRawNt = lp_vec_t(sid.objectId, MTM_RAW, this); /** 1 if coils were actuating during measurement otherwise 0 */ lp_var_t coilActuationStatus = lp_var_t(sid.objectId, ACTUATION_RAW_STATUS, this); diff --git a/tmtc b/tmtc index 1f5baabf..7a309b4d 160000 --- a/tmtc +++ b/tmtc @@ -1 +1 @@ -Subproject commit 1f5baabf312eb7edc5b0449d443dd5f73054dfa2 +Subproject commit 7a309b4dc1cfb02626bb0b4b3d4e4d06f8192506 From 726e448af982b0f5b698039bc4be7f45c1791a4c Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 18 Aug 2022 18:18:44 +0200 Subject: [PATCH 02/52] fixes for IMTQ mgm reading --- mission/controller/AcsController.h | 2 +- mission/devices/IMTQHandler.cpp | 47 ++++++++++++++++++++---------- tmtc | 2 +- 3 files changed, 33 insertions(+), 18 deletions(-) diff --git a/mission/controller/AcsController.h b/mission/controller/AcsController.h index ce1bd7b0..3f3a4cc2 100644 --- a/mission/controller/AcsController.h +++ b/mission/controller/AcsController.h @@ -48,7 +48,7 @@ class AcsController : public ExtendedControllerBase { PoolEntry mgm1PoolVec = PoolEntry(3); PoolEntry mgm2PoolVec = PoolEntry(3); PoolEntry mgm3PoolVec = PoolEntry(3); - PoolEntry imtqMgmPoolVec = PoolEntry(3); + PoolEntry imtqMgmPoolVec = PoolEntry(3); PoolEntry imtqCalActStatus = PoolEntry(); void copyMgmData(); diff --git a/mission/devices/IMTQHandler.cpp b/mission/devices/IMTQHandler.cpp index a421ae21..b01e64ed 100644 --- a/mission/devices/IMTQHandler.cpp +++ b/mission/devices/IMTQHandler.cpp @@ -778,22 +778,37 @@ void IMTQHandler::fillCalibratedMtmDataset(const uint8_t* packet) { void IMTQHandler::fillRawMtmDataset(const uint8_t* packet) { PoolReadGuard rg(&rawMtmMeasurementSet); - int8_t offset = 2; - rawMtmMeasurementSet.mtmRawNt[0] = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 | - *(packet + offset + 1) << 8 | *(packet + offset)) * - 7.5; - offset += 4; - rawMtmMeasurementSet.mtmRawNt[1] = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 | - *(packet + offset + 1) << 8 | *(packet + offset)) * - 7.5; - offset += 4; - rawMtmMeasurementSet.mtmRawNt[2] = (*(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)); + unsigned int offset = 2; + size_t deSerLen = 16; + const uint8_t* dataStart = packet + offset; + int32_t xRaw = 0; + int32_t yRaw = 0; + int32_t zRaw = 0; + uint32_t coilActStatus = 0; + auto res = SerializeAdapter::deSerialize(&xRaw, &dataStart, &deSerLen, + SerializeIF::Endianness::LITTLE); + if(res != HasReturnvaluesIF::RETURN_OK) { + return; + } + res = SerializeAdapter::deSerialize(&yRaw, &dataStart, &deSerLen, + SerializeIF::Endianness::LITTLE); + if(res != HasReturnvaluesIF::RETURN_OK) { + return; + } + res = SerializeAdapter::deSerialize(&zRaw, &dataStart, &deSerLen, + SerializeIF::Endianness::LITTLE); + if(res != HasReturnvaluesIF::RETURN_OK) { + return; + } + res = SerializeAdapter::deSerialize(&coilActStatus, &dataStart, &deSerLen, + SerializeIF::Endianness::LITTLE); + if(res != HasReturnvaluesIF::RETURN_OK) { + return; + } + rawMtmMeasurementSet.mtmRawNt[0] = xRaw * 7.5; + rawMtmMeasurementSet.mtmRawNt[1] = yRaw * 7.5; + rawMtmMeasurementSet.mtmRawNt[2] = zRaw * 7.5; + rawMtmMeasurementSet.coilActuationStatus = static_cast(coilActStatus); rawMtmMeasurementSet.setValidity(true, true); if (debugMode) { #if OBSW_VERBOSE_LEVEL >= 1 diff --git a/tmtc b/tmtc index 7a309b4d..7e2ea082 160000 --- a/tmtc +++ b/tmtc @@ -1 +1 @@ -Subproject commit 7a309b4dc1cfb02626bb0b4b3d4e4d06f8192506 +Subproject commit 7e2ea08277148873cfa541a872b5cc52bf06ee5e From 70d9f00372c987e50e735d47660a0b987581b67b Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Fri, 19 Aug 2022 16:53:25 +0200 Subject: [PATCH 03/52] bump tmtc --- tmtc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tmtc b/tmtc index 7e2ea082..00e99292 160000 --- a/tmtc +++ b/tmtc @@ -1 +1 @@ -Subproject commit 7e2ea08277148873cfa541a872b5cc52bf06ee5e +Subproject commit 00e99292cc158a347f485507537fa5b63262243b From 9eb210795834f25360a547470de9f91d0d2be1df Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Fri, 19 Aug 2022 21:41:24 +0200 Subject: [PATCH 04/52] fix --- linux/devices/ploc/PlocMPSoCHandler.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/linux/devices/ploc/PlocMPSoCHandler.cpp b/linux/devices/ploc/PlocMPSoCHandler.cpp index b3eea536..1b599ebb 100644 --- a/linux/devices/ploc/PlocMPSoCHandler.cpp +++ b/linux/devices/ploc/PlocMPSoCHandler.cpp @@ -683,7 +683,7 @@ ReturnValue_t PlocMPSoCHandler::handleCamCmdRpt(const uint8_t* data) { reinterpret_cast(dataFieldPtr), tmCamCmdRpt.rememberSpacePacketSize - mpsoc::SPACE_PACKET_HEADER_SIZE - sizeof(uint16_t) - 3); #if OBSW_DEBUG_PLOC_MPSOC == 1 - uint8_t ackValue = *(packet.getPacketData() + packet.getPacketDataLength() - 2); + uint8_t ackValue = *(packetReader.getFullData() + packetReader.getFullPacketLen() - 2); sif::info << "PlocMPSoCHandler: CamCmdRpt message: " << camCmdRptMsg << std::endl; sif::info << "PlocMPSoCHandler: CamCmdRpt Ack value: 0x" << std::hex << static_cast(ackValue) << std::endl; From 2e2afcd6c50375073cb0257fc56fb30de7125cac Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Sat, 20 Aug 2022 01:45:49 +0200 Subject: [PATCH 05/52] moved a size check --- .../PlocSupervisorDefinitions.h | 24 ++++++++++--------- 1 file changed, 13 insertions(+), 11 deletions(-) diff --git a/linux/devices/devicedefinitions/PlocSupervisorDefinitions.h b/linux/devices/devicedefinitions/PlocSupervisorDefinitions.h index d6f74909..8f4e2154 100644 --- a/linux/devices/devicedefinitions/PlocSupervisorDefinitions.h +++ b/linux/devices/devicedefinitions/PlocSupervisorDefinitions.h @@ -1121,19 +1121,8 @@ class WriteMemory : public ploc::SpTcBase { uint8_t* updateData) { size_t serializedSize = 0; uint8_t* data = payloadStart; - SerializeAdapter::serialize(&memoryId, &data, &serializedSize, sizeof(memoryId), - SerializeIF::Endianness::BIG); - SerializeAdapter::serialize(&n, &data, &serializedSize, sizeof(n), - SerializeIF::Endianness::BIG); - SerializeAdapter::serialize(&startAddr, &data, &serializedSize, sizeof(startAddr), - SerializeIF::Endianness::BIG); - SerializeAdapter::serialize(&updateDataLen, &data, &serializedSize, sizeof(updateDataLen), - SerializeIF::Endianness::BIG); if (updateDataLen % 2 != 0) { spParams.setPayloadLen(META_DATA_LENGTH + updateDataLen + 1); - // The data field must be two bytes aligned. Thus, in case the number of bytes to write is odd - // a value of zero is added here - *(data + updateDataLen + 1) = 0; } else { spParams.setPayloadLen(META_DATA_LENGTH + updateDataLen); } @@ -1142,7 +1131,20 @@ class WriteMemory : public ploc::SpTcBase { if (result != HasReturnvaluesIF::RETURN_OK) { return result; } + SerializeAdapter::serialize(&memoryId, &data, &serializedSize, sizeof(memoryId), + SerializeIF::Endianness::BIG); + SerializeAdapter::serialize(&n, &data, &serializedSize, sizeof(n), + SerializeIF::Endianness::BIG); + SerializeAdapter::serialize(&startAddr, &data, &serializedSize, sizeof(startAddr), + SerializeIF::Endianness::BIG); + SerializeAdapter::serialize(&updateDataLen, &data, &serializedSize, sizeof(updateDataLen), + SerializeIF::Endianness::BIG); std::memcpy(data, updateData, updateDataLen); + if (updateDataLen % 2 != 0) { + // The data field must be two bytes aligned. Thus, in case the number of bytes to write is odd + // a value of zero is added here + data[updateDataLen + 1] = 0; + } return HasReturnvaluesIF::RETURN_OK; } }; From c8d38cf8528ec16a024b519dbc046d96294256f9 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Sat, 20 Aug 2022 12:11:08 +0200 Subject: [PATCH 06/52] bugfixes --- .../devicedefinitions/PlocMPSoCDefinitions.h | 8 ++-- .../PlocSupervisorDefinitions.h | 38 +++++++++---------- linux/devices/ploc/PlocSupvHelper.cpp | 9 +++++ linux/devices/ploc/PlocSupvHelper.h | 4 +- mission/devices/IMTQHandler.cpp | 24 ++++++------ tmtc | 2 +- 6 files changed, 46 insertions(+), 39 deletions(-) diff --git a/linux/devices/devicedefinitions/PlocMPSoCDefinitions.h b/linux/devices/devicedefinitions/PlocMPSoCDefinitions.h index a293b1f7..6c72d1d9 100644 --- a/linux/devices/devicedefinitions/PlocMPSoCDefinitions.h +++ b/linux/devices/devicedefinitions/PlocMPSoCDefinitions.h @@ -370,8 +370,8 @@ class TcFlashWrite : public ploc::SpTcBase { if (result != HasReturnvaluesIF::RETURN_OK) { return result; } - size_t serializedSize = 0; - result = SerializeAdapter::serialize(&writeLen, payloadStart, &serializedSize, sizeof(writeLen), + size_t serializedSize = ccsds::HEADER_LEN; + result = SerializeAdapter::serialize(&writeLen, payloadStart, &serializedSize, spParams.maxSize, SerializeIF::Endianness::BIG); if (result != HasReturnvaluesIF::RETURN_OK) { return result; @@ -663,8 +663,8 @@ class TcCamcmdSend : public TcBase { if (res != HasReturnvaluesIF::RETURN_OK) { return res; } - size_t size = sizeof(dataLen); - SerializeAdapter::serialize(&dataLen, payloadStart, &size, sizeof(dataLen), + size_t size = ccsds::HEADER_LEN; + SerializeAdapter::serialize(&dataLen, payloadStart, &size, spParams.maxSize, SerializeIF::Endianness::BIG); std::memcpy(payloadStart + sizeof(dataLen), commandData, commandDataLen); *(payloadStart + sizeof(dataLen) + commandDataLen) = CARRIAGE_RETURN; diff --git a/linux/devices/devicedefinitions/PlocSupervisorDefinitions.h b/linux/devices/devicedefinitions/PlocSupervisorDefinitions.h index 8f4e2154..5aa97e0d 100644 --- a/linux/devices/devicedefinitions/PlocSupervisorDefinitions.h +++ b/linux/devices/devicedefinitions/PlocSupervisorDefinitions.h @@ -721,11 +721,11 @@ class SetAdcWindowAndStride : public ploc::SpTcBase { static const uint16_t CRC_OFFSET = DATA_FIELD_LENGTH - 2; void initPacket(uint16_t windowSize, uint16_t stridingStepSize) { - size_t serializedSize = 0; + size_t serializedSize = 6; uint8_t* data = payloadStart; - SerializeAdapter::serialize(&windowSize, &data, &serializedSize, sizeof(windowSize), + SerializeAdapter::serialize(&windowSize, &data, &serializedSize, spParams.maxSize, SerializeIF::Endianness::BIG); - SerializeAdapter::serialize(&stridingStepSize, &data, &serializedSize, sizeof(stridingStepSize), + SerializeAdapter::serialize(&stridingStepSize, &data, &serializedSize, spParams.maxSize, SerializeIF::Endianness::BIG); } }; @@ -1066,13 +1066,13 @@ class CheckMemory : public ploc::SpTcBase { void initPacket(uint8_t memoryId, uint32_t startAddress, uint32_t length) { uint8_t* data = payloadStart; - size_t serLen = 0; - SerializeAdapter::serialize(&memoryId, &data, &serLen, sizeof(memoryId), + size_t serLen = 6; + SerializeAdapter::serialize(&memoryId, &data, &serLen, spParams.maxSize, SerializeIF::Endianness::BIG); - SerializeAdapter::serialize(&n, &data, &serLen, sizeof(n), SerializeIF::Endianness::BIG); - SerializeAdapter::serialize(&startAddress, &data, &serLen, sizeof(startAddress), + SerializeAdapter::serialize(&n, &data, &serLen, spParams.maxSize, SerializeIF::Endianness::BIG); + SerializeAdapter::serialize(&startAddress, &data, &serLen, spParams.maxSize, SerializeIF::Endianness::BIG); - SerializeAdapter::serialize(&length, &data, &serLen, sizeof(length), + SerializeAdapter::serialize(&length, &data, &serLen, spParams.maxSize, SerializeIF::Endianness::BIG); } }; @@ -1119,7 +1119,6 @@ class WriteMemory : public ploc::SpTcBase { ReturnValue_t initPacket(uint8_t memoryId, uint32_t startAddr, uint16_t updateDataLen, uint8_t* updateData) { - size_t serializedSize = 0; uint8_t* data = payloadStart; if (updateDataLen % 2 != 0) { spParams.setPayloadLen(META_DATA_LENGTH + updateDataLen + 1); @@ -1131,13 +1130,14 @@ class WriteMemory : public ploc::SpTcBase { if (result != HasReturnvaluesIF::RETURN_OK) { return result; } - SerializeAdapter::serialize(&memoryId, &data, &serializedSize, sizeof(memoryId), + size_t serializedSize = 6; + SerializeAdapter::serialize(&memoryId, &data, &serializedSize, spParams.maxSize, SerializeIF::Endianness::BIG); - SerializeAdapter::serialize(&n, &data, &serializedSize, sizeof(n), + SerializeAdapter::serialize(&n, &data, &serializedSize, spParams.maxSize, SerializeIF::Endianness::BIG); - SerializeAdapter::serialize(&startAddr, &data, &serializedSize, sizeof(startAddr), + SerializeAdapter::serialize(&startAddr, &data, &serializedSize, spParams.maxSize, SerializeIF::Endianness::BIG); - SerializeAdapter::serialize(&updateDataLen, &data, &serializedSize, sizeof(updateDataLen), + SerializeAdapter::serialize(&updateDataLen, &data, &serializedSize, spParams.maxSize, SerializeIF::Endianness::BIG); std::memcpy(data, updateData, updateDataLen); if (updateDataLen % 2 != 0) { @@ -1178,17 +1178,15 @@ class EraseMemory : public ploc::SpTcBase { uint32_t length = 0; void initPacket(uint8_t memoryId, uint32_t startAddress, uint32_t length) { - size_t serializedSize = 0; uint8_t* data = payloadStart; - SerializeAdapter::serialize(&memoryId, &data, &serializedSize, sizeof(memoryId), + size_t serializedSize = 6; + SerializeAdapter::serialize(&memoryId, &data, &serializedSize, spParams.maxSize, SerializeIF::Endianness::BIG); - SerializeAdapter::serialize(&n, &data, &serializedSize, sizeof(n), + SerializeAdapter::serialize(&n, &data, &serializedSize, spParams.maxSize, SerializeIF::Endianness::BIG); - serializedSize = 0; - SerializeAdapter::serialize(&startAddress, &data, &serializedSize, sizeof(startAddress), + SerializeAdapter::serialize(&startAddress, &data, &serializedSize, spParams.maxSize, SerializeIF::Endianness::BIG); - serializedSize = 0; - SerializeAdapter::serialize(&length, &data, &serializedSize, sizeof(length), + SerializeAdapter::serialize(&length, &data, &serializedSize, spParams.maxSize, SerializeIF::Endianness::BIG); } }; diff --git a/linux/devices/ploc/PlocSupvHelper.cpp b/linux/devices/ploc/PlocSupvHelper.cpp index 25566b11..de1fae0f 100644 --- a/linux/devices/ploc/PlocSupvHelper.cpp +++ b/linux/devices/ploc/PlocSupvHelper.cpp @@ -161,26 +161,32 @@ void PlocSupvHelper::stopProcess() { terminate = true; } ReturnValue_t PlocSupvHelper::performUpdate() { ReturnValue_t result = RETURN_OK; + sif::info << "PlocSupvHelper::performUpdate: Calculating Image CRC" << std::endl; result = calcImageCrc(); if (result != RETURN_OK) { return result; } + sif::info << "PlocSupvHelper::performUpdate: Selecting Memory" << std::endl; result = selectMemory(); if (result != RETURN_OK) { return result; } + sif::info << "PlocSupvHelper::performUpdate: Preparing Update" << std::endl; result = prepareUpdate(); if (result != RETURN_OK) { return result; } + sif::info << "PlocSupvHelper::performUpdate: Erasing Memory" << std::endl; result = eraseMemory(); if (result != RETURN_OK) { return result; } + sif::info << "PlocSupvHelper::performUpdate: Writing Update Packets" << std::endl; result = writeUpdatePackets(); if (result != RETURN_OK) { return result; } + sif::info << "PlocSupvHelper::performUpdate: Memory Check" << std::endl; result = handleCheckMemoryCommand(); if (result != RETURN_OK) { return result; @@ -317,6 +323,9 @@ ReturnValue_t PlocSupvHelper::prepareUpdate() { resetSpParams(); supv::ApidOnlyPacket packet(spParams, supv::APID_PREPARE_UPDATE); result = packet.buildPacket(); + if (result != RETURN_OK) { + return result; + } result = handlePacketTransmission(packet, PREPARE_UPDATE_EXECUTION_REPORT); if (result != RETURN_OK) { return result; diff --git a/linux/devices/ploc/PlocSupvHelper.h b/linux/devices/ploc/PlocSupvHelper.h index c17521d9..66b5d00a 100644 --- a/linux/devices/ploc/PlocSupvHelper.h +++ b/linux/devices/ploc/PlocSupvHelper.h @@ -178,11 +178,11 @@ class PlocSupvHelper : public SystemObject, public ExecutableObjectIF, public Ha #ifdef XIPHOS_Q7S SdCardManager* sdcMan = nullptr; #endif - uint8_t commandBuffer[supv::MAX_COMMAND_SIZE]; + uint8_t commandBuffer[supv::MAX_COMMAND_SIZE]{}; SpacePacketCreator creator; ploc::SpTcParams spParams = ploc::SpTcParams(creator); - std::array tmBuf; + std::array tmBuf{}; bool terminate = false; diff --git a/mission/devices/IMTQHandler.cpp b/mission/devices/IMTQHandler.cpp index b01e64ed..0d1f72db 100644 --- a/mission/devices/IMTQHandler.cpp +++ b/mission/devices/IMTQHandler.cpp @@ -785,30 +785,30 @@ void IMTQHandler::fillRawMtmDataset(const uint8_t* packet) { int32_t yRaw = 0; int32_t zRaw = 0; uint32_t coilActStatus = 0; - auto res = SerializeAdapter::deSerialize(&xRaw, &dataStart, &deSerLen, - SerializeIF::Endianness::LITTLE); - if(res != HasReturnvaluesIF::RETURN_OK) { + auto res = + SerializeAdapter::deSerialize(&xRaw, &dataStart, &deSerLen, SerializeIF::Endianness::LITTLE); + if (res != HasReturnvaluesIF::RETURN_OK) { return; } - res = SerializeAdapter::deSerialize(&yRaw, &dataStart, &deSerLen, - SerializeIF::Endianness::LITTLE); - if(res != HasReturnvaluesIF::RETURN_OK) { + res = + SerializeAdapter::deSerialize(&yRaw, &dataStart, &deSerLen, SerializeIF::Endianness::LITTLE); + if (res != HasReturnvaluesIF::RETURN_OK) { return; } - res = SerializeAdapter::deSerialize(&zRaw, &dataStart, &deSerLen, - SerializeIF::Endianness::LITTLE); - if(res != HasReturnvaluesIF::RETURN_OK) { + res = + SerializeAdapter::deSerialize(&zRaw, &dataStart, &deSerLen, SerializeIF::Endianness::LITTLE); + if (res != HasReturnvaluesIF::RETURN_OK) { return; } res = SerializeAdapter::deSerialize(&coilActStatus, &dataStart, &deSerLen, - SerializeIF::Endianness::LITTLE); - if(res != HasReturnvaluesIF::RETURN_OK) { + SerializeIF::Endianness::LITTLE); + if (res != HasReturnvaluesIF::RETURN_OK) { return; } rawMtmMeasurementSet.mtmRawNt[0] = xRaw * 7.5; rawMtmMeasurementSet.mtmRawNt[1] = yRaw * 7.5; rawMtmMeasurementSet.mtmRawNt[2] = zRaw * 7.5; - rawMtmMeasurementSet.coilActuationStatus = static_cast(coilActStatus); + rawMtmMeasurementSet.coilActuationStatus = static_cast(coilActStatus); rawMtmMeasurementSet.setValidity(true, true); if (debugMode) { #if OBSW_VERBOSE_LEVEL >= 1 diff --git a/tmtc b/tmtc index 00e99292..1b39bb2a 160000 --- a/tmtc +++ b/tmtc @@ -1 +1 @@ -Subproject commit 00e99292cc158a347f485507537fa5b63262243b +Subproject commit 1b39bb2ad2421db89487b4ea352edbd4d420b9b1 From 447c4d5c886f2826e2a982a935eea10cde288292 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Wed, 24 Aug 2022 17:27:47 +0200 Subject: [PATCH 07/52] all retval replacements --- bsp_egse/InitMission.cpp | 42 +-- bsp_hosted/InitMission.cpp | 44 +-- bsp_hosted/comIF/ArduinoComIF.cpp | 28 +- bsp_hosted/comIF/ArduinoComIF.h | 2 +- .../fsfwconfig/pollingsequence/DummyPst.cpp | 6 +- .../fsfwconfig/pollingsequence/DummyPst.h | 2 +- bsp_linux_board/InitMission.cpp | 54 +-- bsp_linux_board/gpioInit.cpp | 4 +- bsp_q7s/boardtest/Q7STestTask.cpp | 68 ++-- bsp_q7s/callbacks/gnssCallback.cpp | 6 +- bsp_q7s/callbacks/gnssCallback.h | 2 +- bsp_q7s/callbacks/q7sGpioCallbacks.cpp | 2 +- bsp_q7s/callbacks/rwSpiCallback.cpp | 24 +- bsp_q7s/callbacks/rwSpiCallback.h | 2 +- bsp_q7s/core/CoreController.cpp | 142 ++++---- bsp_q7s/core/InitMission.cpp | 94 +++--- bsp_q7s/core/ObjectFactory.cpp | 2 +- bsp_q7s/core/ObjectFactory.h | 2 +- bsp_q7s/memory/FileSystemHandler.cpp | 24 +- bsp_q7s/memory/FilesystemHelper.cpp | 6 +- bsp_q7s/memory/FilesystemHelper.h | 8 +- bsp_q7s/memory/SdCardManager.cpp | 64 ++-- bsp_q7s/memory/SdCardManager.h | 34 +- bsp_q7s/memory/scratchApi.cpp | 14 +- bsp_q7s/memory/scratchApi.h | 18 +- bsp_q7s/xadc/Xadc.cpp | 52 +-- bsp_q7s/xadc/Xadc.h | 2 +- bsp_te0720_1cfa/InitMission.cpp | 48 +-- dummies/AcuDummy.cpp | 8 +- dummies/BpxDummy.cpp | 8 +- dummies/ComIFDummy.cpp | 10 +- dummies/CoreControllerDummy.cpp | 10 +- dummies/GyroAdisDummy.cpp | 8 +- dummies/GyroL3GD20Dummy.cpp | 8 +- dummies/ImtqDummy.cpp | 8 +- dummies/MgmLIS3MDLDummy.cpp | 8 +- dummies/P60DockDummy.cpp | 8 +- dummies/PduDummy.cpp | 8 +- dummies/PlPcduDummy.cpp | 8 +- dummies/RwDummy.cpp | 8 +- dummies/StarTrackerDummy.cpp | 8 +- dummies/SusDummy.cpp | 10 +- dummies/SyrlinksDummy.cpp | 8 +- dummies/TemperatureSensorsDummy.cpp | 12 +- fsfw | 2 +- generators/deps/fsfwgen | 2 +- linux/ObjectFactory.cpp | 2 +- linux/ObjectFactory.h | 2 +- linux/boardtest/I2cTestClass.cpp | 24 +- linux/boardtest/LibgpiodTest.cpp | 34 +- linux/boardtest/SpiTestClass.cpp | 18 +- linux/boardtest/UartTestClass.cpp | 8 +- linux/csp/CspComIF.cpp | 58 ++-- linux/csp/CspComIF.h | 2 +- linux/devices/GPSHyperionLinuxController.cpp | 14 +- linux/devices/Max31865RtdLowlevelHandler.cpp | 98 +++--- .../devicedefinitions/MPSoCReturnValuesIF.h | 2 +- .../devicedefinitions/PlocMPSoCDefinitions.h | 86 ++--- .../PlocSupervisorDefinitions.h | 86 ++--- .../StarTrackerDefinitions.h | 8 +- .../devicedefinitions/SupvReturnValuesIF.h | 2 +- linux/devices/ploc/PlocMPSoCHandler.cpp | 138 ++++---- linux/devices/ploc/PlocMPSoCHandler.h | 8 +- linux/devices/ploc/PlocMPSoCHelper.cpp | 108 +++--- linux/devices/ploc/PlocMPSoCHelper.h | 6 +- linux/devices/ploc/PlocMemoryDumper.cpp | 24 +- linux/devices/ploc/PlocMemoryDumper.h | 3 +- linux/devices/ploc/PlocSupervisorHandler.cpp | 318 +++++++++--------- linux/devices/ploc/PlocSupervisorHandler.h | 8 +- linux/devices/ploc/PlocSupvHelper.cpp | 182 +++++----- linux/devices/ploc/PlocSupvHelper.h | 6 +- .../startracker/ArcsecDatalinkLayer.cpp | 6 +- .../devices/startracker/ArcsecDatalinkLayer.h | 4 +- .../startracker/ArcsecJsonParamBase.cpp | 16 +- .../devices/startracker/ArcsecJsonParamBase.h | 9 +- .../startracker/StarTrackerHandler.cpp | 192 +++++------ .../devices/startracker/StarTrackerHandler.h | 8 +- .../startracker/StarTrackerJsonCommands.cpp | 304 ++++++++--------- linux/devices/startracker/StrHelper.cpp | 134 ++++---- linux/devices/startracker/StrHelper.h | 16 +- .../pollingSequenceFactory.cpp | 28 +- .../pollingsequence/pollingSequenceFactory.h | 2 +- linux/obc/AxiPtmeConfig.cpp | 64 ++-- linux/obc/AxiPtmeConfig.h | 4 +- linux/obc/PapbVcInterface.cpp | 26 +- linux/obc/PapbVcInterface.h | 6 +- linux/obc/PdecConfig.h | 2 +- linux/obc/PdecHandler.cpp | 46 +-- linux/obc/PdecHandler.h | 3 +- linux/obc/Ptme.cpp | 4 +- linux/obc/Ptme.h | 4 +- linux/obc/PtmeConfig.cpp | 10 +- linux/obc/PtmeConfig.h | 4 +- linux/obc/PtmeIF.h | 2 +- linux/obc/VcInterfaceIF.h | 2 +- linux/utility/utility.h | 2 +- mission/controller/AcsController.cpp | 18 +- mission/controller/ThermalController.cpp | 172 +++++----- mission/devices/ACUHandler.cpp | 16 +- mission/devices/BpxBatteryHandler.cpp | 14 +- mission/devices/GPSHyperionHandler.cpp | 12 +- mission/devices/GomspaceDeviceHandler.cpp | 66 ++-- mission/devices/GomspaceDeviceHandler.h | 14 +- mission/devices/GyroADIS1650XHandler.cpp | 34 +- mission/devices/HeaterHandler.cpp | 44 +-- mission/devices/HeaterHandler.h | 2 +- mission/devices/IMTQHandler.cpp | 54 +-- mission/devices/Max31865EiveHandler.cpp | 14 +- mission/devices/Max31865PT1000Handler.cpp | 44 +-- mission/devices/P60DockHandler.cpp | 20 +- mission/devices/PCDUHandler.cpp | 50 +-- mission/devices/PDU1Handler.cpp | 14 +- mission/devices/PDU2Handler.cpp | 14 +- mission/devices/PayloadPcduHandler.cpp | 36 +- mission/devices/RadiationSensorHandler.cpp | 24 +- mission/devices/RwHandler.cpp | 34 +- mission/devices/RwHandler.h | 2 +- .../devices/SolarArrayDeploymentHandler.cpp | 32 +- mission/devices/SolarArrayDeploymentHandler.h | 3 +- mission/devices/SusHandler.cpp | 12 +- mission/devices/SyrlinksHkHandler.cpp | 84 ++--- mission/devices/SyrlinksHkHandler.h | 2 +- mission/devices/Tmp1075Handler.cpp | 18 +- .../devicedefinitions/BpxBatteryDefinitions.h | 22 +- mission/devices/devicedefinitions/SpBase.h | 8 +- .../payloadPcduDefinitions.h | 4 +- mission/memory/NVMParameterBase.cpp | 8 +- mission/memory/NVMParameterBase.h | 12 +- mission/system/AcsBoardAssembly.cpp | 44 +-- mission/system/DualLaneAssemblyBase.cpp | 10 +- mission/system/DualLanePowerStateMachine.cpp | 4 +- mission/system/PowerStateMachineBase.h | 2 +- mission/system/RwAssembly.cpp | 12 +- mission/system/SusAssembly.cpp | 16 +- mission/system/TcsBoardAssembly.cpp | 12 +- mission/system/fdir/GomspacePowerFdir.cpp | 12 +- mission/system/fdir/SyrlinksFdir.cpp | 12 +- mission/tmtc/CCSDSHandler.cpp | 34 +- mission/tmtc/CCSDSHandler.h | 3 +- mission/tmtc/TmFunnel.cpp | 14 +- mission/tmtc/VirtualChannel.cpp | 12 +- mission/tmtc/VirtualChannel.h | 4 +- mission/utility/Timestamp.cpp | 2 +- mission/utility/Timestamp.h | 4 +- test/gpio/DummyGpioIF.cpp | 8 +- test/testtasks/PusTcInjector.cpp | 4 +- test/testtasks/PusTcInjector.h | 2 +- test/testtasks/TestTask.cpp | 10 +- unittest/controller/testThermalController.cpp | 8 +- unittest/mocks/EventManagerMock.cpp | 8 +- 150 files changed, 2109 insertions(+), 2112 deletions(-) diff --git a/bsp_egse/InitMission.cpp b/bsp_egse/InitMission.cpp index d6b192c2..5a72f53c 100644 --- a/bsp_egse/InitMission.cpp +++ b/bsp_egse/InitMission.cpp @@ -2,7 +2,7 @@ #include #include -#include +#include #include #include #include @@ -40,7 +40,7 @@ void initmission::initMission() { void initmission::initTasks() { TaskFactory* factory = TaskFactory::instance(); - ReturnValue_t result = HasReturnvaluesIF::RETURN_OK; + ReturnValue_t result = returnvalue::OK; if (factory == nullptr) { /* Should never happen ! */ return; @@ -55,28 +55,28 @@ void initmission::initTasks() { PeriodicTaskIF* tmtcDistributor = factory->createPeriodicTask( "DIST", 40, PeriodicTaskIF::MINIMUM_STACK_SIZE, 0.2, missedDeadlineFunc); result = tmtcDistributor->addComponent(objects::CCSDS_PACKET_DISTRIBUTOR); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { sif::error << "Object add component failed" << std::endl; } result = tmtcDistributor->addComponent(objects::PUS_PACKET_DISTRIBUTOR); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { sif::error << "Object add component failed" << std::endl; } result = tmtcDistributor->addComponent(objects::TM_FUNNEL); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { sif::error << "Object add component failed" << std::endl; } PeriodicTaskIF* tmtcBridgeTask = factory->createPeriodicTask( "TMTC_BRIDGE", 50, PeriodicTaskIF::MINIMUM_STACK_SIZE, 0.2, missedDeadlineFunc); result = tmtcBridgeTask->addComponent(objects::TMTC_BRIDGE); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { sif::error << "Add component TMTC Bridge failed" << std::endl; } PeriodicTaskIF* tmtcPollingTask = factory->createPeriodicTask( "TMTC_POLLING", 80, PeriodicTaskIF::MINIMUM_STACK_SIZE, 2.0, missedDeadlineFunc); result = tmtcPollingTask->addComponent(objects::TMTC_POLLING_TASK); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { sif::error << "Add component TMTC Polling failed" << std::endl; } @@ -88,7 +88,7 @@ void initmission::initTasks() { FixedTimeslotTaskIF* pst = factory->createFixedTimeslotTask( "STAR_TRACKER_PST", 70, PeriodicTaskIF::MINIMUM_STACK_SIZE * 4, 0.5, missedDeadlineFunc); result = pst::pstUart(pst); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { sif::error << "InitMission::initTasks: Creating PST failed!" << std::endl; } pstTasks.push_back(pst); @@ -96,7 +96,7 @@ void initmission::initTasks() { PeriodicTaskIF* strHelperTask = factory->createPeriodicTask( "STR_HELPER", 20, PeriodicTaskIF::MINIMUM_STACK_SIZE, 0.2, missedDeadlineFunc); result = strHelperTask->addComponent(objects::STR_HELPER); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { initmission::printAddObjectError("STR_HELPER", objects::STR_HELPER); } pstTasks.push_back(strHelperTask); @@ -125,11 +125,11 @@ void initmission::initTasks() { void initmission::createPusTasks(TaskFactory& factory, TaskDeadlineMissedFunction missedDeadlineFunc, std::vector& taskVec) { - ReturnValue_t result = HasReturnvaluesIF::RETURN_OK; + ReturnValue_t result = returnvalue::OK; PeriodicTaskIF* pusVerification = factory.createPeriodicTask( "PUS_VERIF", 40, PeriodicTaskIF::MINIMUM_STACK_SIZE, 0.200, missedDeadlineFunc); result = pusVerification->addComponent(objects::PUS_SERVICE_1_VERIFICATION); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { sif::error << "Object add component failed" << std::endl; } taskVec.push_back(pusVerification); @@ -137,11 +137,11 @@ void initmission::createPusTasks(TaskFactory& factory, PeriodicTaskIF* pusEvents = factory.createPeriodicTask( "PUS_EVENTS", 60, PeriodicTaskIF::MINIMUM_STACK_SIZE, 0.200, missedDeadlineFunc); result = pusEvents->addComponent(objects::PUS_SERVICE_5_EVENT_REPORTING); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { initmission::printAddObjectError("PUS_EVENTS", objects::PUS_SERVICE_5_EVENT_REPORTING); } result = pusEvents->addComponent(objects::EVENT_MANAGER); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { initmission::printAddObjectError("PUS_MGMT", objects::EVENT_MANAGER); } taskVec.push_back(pusEvents); @@ -149,11 +149,11 @@ void initmission::createPusTasks(TaskFactory& factory, PeriodicTaskIF* pusHighPrio = factory.createPeriodicTask( "PUS_HIGH_PRIO", 50, PeriodicTaskIF::MINIMUM_STACK_SIZE, 0.200, missedDeadlineFunc); result = pusHighPrio->addComponent(objects::PUS_SERVICE_2_DEVICE_ACCESS); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { initmission::printAddObjectError("PUS2", objects::PUS_SERVICE_2_DEVICE_ACCESS); } result = pusHighPrio->addComponent(objects::PUS_SERVICE_9_TIME_MGMT); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { initmission::printAddObjectError("PUS9", objects::PUS_SERVICE_9_TIME_MGMT); } taskVec.push_back(pusHighPrio); @@ -161,19 +161,19 @@ void initmission::createPusTasks(TaskFactory& factory, PeriodicTaskIF* pusMedPrio = factory.createPeriodicTask( "PUS_MED_PRIO", 40, PeriodicTaskIF::MINIMUM_STACK_SIZE, 0.8, missedDeadlineFunc); result = pusMedPrio->addComponent(objects::PUS_SERVICE_8_FUNCTION_MGMT); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { initmission::printAddObjectError("PUS8", objects::PUS_SERVICE_8_FUNCTION_MGMT); } result = pusMedPrio->addComponent(objects::PUS_SERVICE_200_MODE_MGMT); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { initmission::printAddObjectError("PUS200", objects::PUS_SERVICE_200_MODE_MGMT); } result = pusMedPrio->addComponent(objects::PUS_SERVICE_20_PARAMETERS); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { initmission::printAddObjectError("PUS20", objects::PUS_SERVICE_20_PARAMETERS); } result = pusMedPrio->addComponent(objects::PUS_SERVICE_3_HOUSEKEEPING); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { initmission::printAddObjectError("PUS3", objects::PUS_SERVICE_3_HOUSEKEEPING); } taskVec.push_back(pusMedPrio); @@ -181,11 +181,11 @@ void initmission::createPusTasks(TaskFactory& factory, PeriodicTaskIF* pusLowPrio = factory.createPeriodicTask( "PUS_LOW_PRIO", 30, PeriodicTaskIF::MINIMUM_STACK_SIZE, 1.6, missedDeadlineFunc); result = pusLowPrio->addComponent(objects::PUS_SERVICE_17_TEST); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { initmission::printAddObjectError("PUS17", objects::PUS_SERVICE_17_TEST); } result = pusLowPrio->addComponent(objects::INTERNAL_ERROR_REPORTER); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { initmission::printAddObjectError("INT_ERR_RPRT", objects::INTERNAL_ERROR_REPORTER); } taskVec.push_back(pusLowPrio); diff --git a/bsp_hosted/InitMission.cpp b/bsp_hosted/InitMission.cpp index 6ec0ed35..4ee90f70 100644 --- a/bsp_hosted/InitMission.cpp +++ b/bsp_hosted/InitMission.cpp @@ -4,7 +4,7 @@ #include #include #include -#include +#include #include #include #include @@ -56,15 +56,15 @@ void initmission::initTasks() { PeriodicTaskIF* tmTcDistributor = factory->createPeriodicTask( "DIST", 40, PeriodicTaskIF::MINIMUM_STACK_SIZE, 0.2, missedDeadlineFunc); ReturnValue_t result = tmTcDistributor->addComponent(objects::CCSDS_PACKET_DISTRIBUTOR); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { sif::error << "Object add component failed" << std::endl; } result = tmTcDistributor->addComponent(objects::PUS_PACKET_DISTRIBUTOR); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { sif::error << "Object add component failed" << std::endl; } result = tmTcDistributor->addComponent(objects::TM_FUNNEL); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { sif::error << "Object add component failed" << std::endl; } @@ -72,13 +72,13 @@ void initmission::initTasks() { PeriodicTaskIF* tmtcBridgeTask = factory->createPeriodicTask( "TMTC_UNIX_BRIDGE", 50, PeriodicTaskIF::MINIMUM_STACK_SIZE, 0.2, missedDeadlineFunc); result = tmtcBridgeTask->addComponent(objects::TMTC_BRIDGE); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { sif::error << "Add component UDP Unix Bridge failed" << std::endl; } PeriodicTaskIF* tmtcPollingTask = factory->createPeriodicTask( "UDP_POLLING", 80, PeriodicTaskIF::MINIMUM_STACK_SIZE, 2.0, missedDeadlineFunc); result = tmtcPollingTask->addComponent(objects::TMTC_POLLING_TASK); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { sif::error << "Add component UDP Polling failed" << std::endl; } @@ -86,89 +86,89 @@ void initmission::initTasks() { PeriodicTaskIF* pusVerification = factory->createPeriodicTask( "PUS_VERIF", 40, PeriodicTaskIF::MINIMUM_STACK_SIZE, 0.200, missedDeadlineFunc); result = pusVerification->addComponent(objects::PUS_SERVICE_1_VERIFICATION); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { sif::error << "Object add component failed" << std::endl; } PeriodicTaskIF* eventHandling = factory->createPeriodicTask( "EVENTS", 60, PeriodicTaskIF::MINIMUM_STACK_SIZE, 0.200, missedDeadlineFunc); result = eventHandling->addComponent(objects::EVENT_MANAGER); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { initmission::printAddObjectError("EVENT_MNGR", objects::EVENT_MANAGER); } result = eventHandling->addComponent(objects::PUS_SERVICE_5_EVENT_REPORTING); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { initmission::printAddObjectError("PUS5", objects::PUS_SERVICE_5_EVENT_REPORTING); } PeriodicTaskIF* pusHighPrio = factory->createPeriodicTask( "PUS_HIGH_PRIO", 50, PeriodicTaskIF::MINIMUM_STACK_SIZE, 0.200, missedDeadlineFunc); result = pusHighPrio->addComponent(objects::PUS_SERVICE_2_DEVICE_ACCESS); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { initmission::printAddObjectError("PUS2", objects::PUS_SERVICE_2_DEVICE_ACCESS); } result = pusHighPrio->addComponent(objects::PUS_SERVICE_9_TIME_MGMT); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { initmission::printAddObjectError("PUS9", objects::PUS_SERVICE_9_TIME_MGMT); } result = pusHighPrio->addComponent(objects::PUS_SERVICE_3_HOUSEKEEPING); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { initmission::printAddObjectError("PUS3", objects::PUS_SERVICE_3_HOUSEKEEPING); } PeriodicTaskIF* pusMedPrio = factory->createPeriodicTask( "PUS_MED_PRIO", 40, PeriodicTaskIF::MINIMUM_STACK_SIZE, 0.8, missedDeadlineFunc); result = pusMedPrio->addComponent(objects::PUS_SERVICE_8_FUNCTION_MGMT); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { initmission::printAddObjectError("PUS8", objects::PUS_SERVICE_8_FUNCTION_MGMT); } result = pusMedPrio->addComponent(objects::PUS_SERVICE_200_MODE_MGMT); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { initmission::printAddObjectError("PUS200", objects::PUS_SERVICE_200_MODE_MGMT); } result = pusMedPrio->addComponent(objects::PUS_SERVICE_20_PARAMETERS); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { initmission::printAddObjectError("PUS20", objects::PUS_SERVICE_20_PARAMETERS); } PeriodicTaskIF* pusLowPrio = factory->createPeriodicTask( "PUS_LOW_PRIO", 30, PeriodicTaskIF::MINIMUM_STACK_SIZE, 1.6, missedDeadlineFunc); result = pusLowPrio->addComponent(objects::PUS_SERVICE_17_TEST); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { initmission::printAddObjectError("PUS17", objects::PUS_SERVICE_17_TEST); } PeriodicTaskIF* thermalTask = factory->createPeriodicTask( "THERMAL_CTL_TASK", 40, PeriodicTaskIF::MINIMUM_STACK_SIZE, 1.0, missedDeadlineFunc); result = thermalTask->addComponent(objects::RTD_0_IC3_PLOC_HEATSPREADER); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { initmission::printAddObjectError("RTD_0_dummy", objects::RTD_0_IC3_PLOC_HEATSPREADER); } result = thermalTask->addComponent(objects::SUS_0_N_LOC_XFYFZM_PT_XF); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { initmission::printAddObjectError("SUS_0_dummy", objects::SUS_0_N_LOC_XFYFZM_PT_XF); } result = thermalTask->addComponent(objects::CORE_CONTROLLER); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { initmission::printAddObjectError("Core controller dummy", objects::CORE_CONTROLLER); } result = thermalTask->addComponent(objects::THERMAL_CONTROLLER); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { initmission::printAddObjectError("THERMAL_CONTROLLER", objects::THERMAL_CONTROLLER); } FixedTimeslotTaskIF* pstTask = factory->createFixedTimeslotTask( "DUMMY_PST", 75, PeriodicTaskIF::MINIMUM_STACK_SIZE * 4, 0.5, missedDeadlineFunc); result = dummy_pst::pst(pstTask); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { sif::error << "Failed to add dummy pst to fixed timeslot task" << std::endl; } #if OBSW_ADD_TEST_CODE == 1 result = testTask->addComponent(objects::TEST_TASK); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { initmission::printAddObjectError("TEST_TASK", objects::TEST_TASK); } #endif /* OBSW_ADD_TEST_CODE == 1 */ diff --git a/bsp_hosted/comIF/ArduinoComIF.cpp b/bsp_hosted/comIF/ArduinoComIF.cpp index be136e45..b669233f 100644 --- a/bsp_hosted/comIF/ArduinoComIF.cpp +++ b/bsp_hosted/comIF/ArduinoComIF.cpp @@ -130,7 +130,7 @@ ArduinoComIF::~ArduinoComIF() { #endif } ReturnValue_t ArduinoComIF::initializeInterface(CookieIF *cookie) { - return HasReturnvaluesIF::RETURN_OK; + return returnvalue::OK; } ReturnValue_t ArduinoComIF::sendMessage(CookieIF *cookie, const uint8_t *data, size_t len) { @@ -142,10 +142,10 @@ ReturnValue_t ArduinoComIF::sendMessage(CookieIF *cookie, const uint8_t *data, s return sendMessage(arduinoCookie->command, arduinoCookie->address, data, len); } -ReturnValue_t ArduinoComIF::getSendSuccess(CookieIF *cookie) { return RETURN_OK; } +ReturnValue_t ArduinoComIF::getSendSuccess(CookieIF *cookie) { return returnvalue::OK; } ReturnValue_t ArduinoComIF::requestReceiveMessage(CookieIF *cookie, size_t requestLen) { - return RETURN_OK; + return returnvalue::OK; } ReturnValue_t ArduinoComIF::readReceivedMessage(CookieIF *cookie, uint8_t **buffer, size_t *size) { @@ -158,7 +158,7 @@ ReturnValue_t ArduinoComIF::readReceivedMessage(CookieIF *cookie, uint8_t **buff *buffer = arduinoCookie->replyBuffer.data(); *size = arduinoCookie->receivedDataLen; - return HasReturnvaluesIF::RETURN_OK; + return returnvalue::OK; } ReturnValue_t ArduinoComIF::sendMessage(uint8_t command, uint8_t address, const uint8_t *data, @@ -178,14 +178,14 @@ ReturnValue_t ArduinoComIF::sendMessage(uint8_t command, uint8_t address, const ReturnValue_t result = DleEncoder::encode(&command, 1, currentPosition, remainingLen, &encodedLen, false); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return result; } currentPosition += encodedLen; remainingLen -= encodedLen; // DleEncoder will never return encodedLen > remainingLen result = DleEncoder::encode(&address, 1, currentPosition, remainingLen, &encodedLen, false); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return result; } currentPosition += encodedLen; @@ -199,7 +199,7 @@ ReturnValue_t ArduinoComIF::sendMessage(uint8_t command, uint8_t address, const result = DleEncoder::encode(temporaryBuffer, 2, currentPosition, remainingLen, &encodedLen, false); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return result; } currentPosition += encodedLen; @@ -207,7 +207,7 @@ ReturnValue_t ArduinoComIF::sendMessage(uint8_t command, uint8_t address, const // encoding the actual data result = DleEncoder::encode(data, dataLen, currentPosition, remainingLen, &encodedLen, false); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return result; } currentPosition += encodedLen; @@ -224,7 +224,7 @@ ReturnValue_t ArduinoComIF::sendMessage(uint8_t command, uint8_t address, const result = DleEncoder::encode(temporaryBuffer, 2, currentPosition, remainingLen, &encodedLen, false); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return result; } currentPosition += encodedLen; @@ -241,16 +241,16 @@ ReturnValue_t ArduinoComIF::sendMessage(uint8_t command, uint8_t address, const ssize_t writtenlen = ::write(serialPort, sendBuffer, encodedLen); if (writtenlen < 0) { // we could try to find out what happened... - return RETURN_FAILED; + return returnvalue::FAILED; } if (writtenlen != encodedLen) { // the OS failed us, we do not try to block until everything is written, as // we can not block the whole system here - return RETURN_FAILED; + return returnvalue::FAILED; } - return RETURN_OK; + return returnvalue::OK; #elif WIN32 - return HasReturnvaluesIF::RETURN_OK; + return returnvalue::OK; #endif } @@ -297,7 +297,7 @@ void ArduinoComIF::handleSerialPortRx() { packet, sizeof(packet), &packetLen); size_t toDelete = firstSTXinRawData; - if (result == HasReturnvaluesIF::RETURN_OK) { + if (result == returnvalue::OK) { handlePacket(packet, packetLen); // after handling the packet, we can delete it from the raw stream, diff --git a/bsp_hosted/comIF/ArduinoComIF.h b/bsp_hosted/comIF/ArduinoComIF.h index 8476b6b5..af849745 100644 --- a/bsp_hosted/comIF/ArduinoComIF.h +++ b/bsp_hosted/comIF/ArduinoComIF.h @@ -5,7 +5,7 @@ #include #include #include -#include +#include #include #include diff --git a/bsp_hosted/fsfwconfig/pollingsequence/DummyPst.cpp b/bsp_hosted/fsfwconfig/pollingsequence/DummyPst.cpp index 18fca7c6..9a92f544 100644 --- a/bsp_hosted/fsfwconfig/pollingsequence/DummyPst.cpp +++ b/bsp_hosted/fsfwconfig/pollingsequence/DummyPst.cpp @@ -129,12 +129,12 @@ ReturnValue_t dummy_pst::pst(FixedTimeslotTaskIF *thisSequence) { thisSequence->addSlot(objects::PLPCDU_HANDLER, length * 0, DeviceHandlerIF::SEND_READ); thisSequence->addSlot(objects::PLPCDU_HANDLER, length * 0, DeviceHandlerIF::GET_READ); - if (thisSequence->checkSequence() == HasReturnvaluesIF::RETURN_OK) { - return HasReturnvaluesIF::RETURN_OK; + if (thisSequence->checkSequence() == returnvalue::OK) { + return returnvalue::OK; } else { #if FSFW_CPP_OSTREAM_ENABLED == 1 sif::error << "pst::pollingSequenceInitDefault: Sequence invalid!" << std::endl; #endif - return HasReturnvaluesIF::RETURN_FAILED; + return returnvalue::FAILED; } } diff --git a/bsp_hosted/fsfwconfig/pollingsequence/DummyPst.h b/bsp_hosted/fsfwconfig/pollingsequence/DummyPst.h index 711e39ba..08bf3ca2 100644 --- a/bsp_hosted/fsfwconfig/pollingsequence/DummyPst.h +++ b/bsp_hosted/fsfwconfig/pollingsequence/DummyPst.h @@ -1,7 +1,7 @@ #ifndef POLLINGSEQUENCEFACTORY_H_ #define POLLINGSEQUENCEFACTORY_H_ -#include +#include class FixedTimeslotTaskIF; diff --git a/bsp_linux_board/InitMission.cpp b/bsp_linux_board/InitMission.cpp index 0fa7a4f6..808fc8be 100644 --- a/bsp_linux_board/InitMission.cpp +++ b/bsp_linux_board/InitMission.cpp @@ -2,7 +2,7 @@ #include #include -#include +#include #include #include #include @@ -36,7 +36,7 @@ void initmission::initMission() { void initmission::initTasks() { TaskFactory* factory = TaskFactory::instance(); - ReturnValue_t result = HasReturnvaluesIF::RETURN_OK; + ReturnValue_t result = returnvalue::OK; if (factory == nullptr) { /* Should never happen ! */ return; @@ -51,15 +51,15 @@ void initmission::initTasks() { PeriodicTaskIF* tmTcDistributor = factory->createPeriodicTask( "DIST", 40, PeriodicTaskIF::MINIMUM_STACK_SIZE, 0.2, missedDeadlineFunc); result = tmTcDistributor->addComponent(objects::CCSDS_PACKET_DISTRIBUTOR); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { sif::error << "Object add component failed" << std::endl; } result = tmTcDistributor->addComponent(objects::PUS_PACKET_DISTRIBUTOR); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { sif::error << "Object add component failed" << std::endl; } result = tmTcDistributor->addComponent(objects::TM_FUNNEL); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { sif::error << "Object add component failed" << std::endl; } @@ -67,13 +67,13 @@ void initmission::initTasks() { PeriodicTaskIF* tmtcBridgeTask = factory->createPeriodicTask( "TMTC_BRIDGE", 50, PeriodicTaskIF::MINIMUM_STACK_SIZE, 0.2, missedDeadlineFunc); result = tmtcBridgeTask->addComponent(objects::TMTC_BRIDGE); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { sif::error << "Add component TMTC Bridge failed" << std::endl; } PeriodicTaskIF* tmtcPollingTask = factory->createPeriodicTask( "TMTC_POLLING", 80, PeriodicTaskIF::MINIMUM_STACK_SIZE, 2.0, missedDeadlineFunc); result = tmtcPollingTask->addComponent(objects::TMTC_POLLING_TASK); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { sif::error << "Add component TMTC Polling failed" << std::endl; } @@ -120,11 +120,11 @@ void initmission::initTasks() { void initmission::createPusTasks(TaskFactory& factory, TaskDeadlineMissedFunction missedDeadlineFunc, std::vector& taskVec) { - ReturnValue_t result = HasReturnvaluesIF::RETURN_OK; + ReturnValue_t result = returnvalue::OK; PeriodicTaskIF* pusVerification = factory.createPeriodicTask( "PUS_VERIF", 40, PeriodicTaskIF::MINIMUM_STACK_SIZE, 0.200, missedDeadlineFunc); result = pusVerification->addComponent(objects::PUS_SERVICE_1_VERIFICATION); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { sif::error << "Object add component failed" << std::endl; } taskVec.push_back(pusVerification); @@ -132,11 +132,11 @@ void initmission::createPusTasks(TaskFactory& factory, PeriodicTaskIF* pusEvents = factory.createPeriodicTask( "PUS_EVENTS", 60, PeriodicTaskIF::MINIMUM_STACK_SIZE, 0.200, missedDeadlineFunc); result = pusEvents->addComponent(objects::PUS_SERVICE_5_EVENT_REPORTING); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { initmission::printAddObjectError("PUS_EVENTS", objects::PUS_SERVICE_5_EVENT_REPORTING); } result = pusEvents->addComponent(objects::EVENT_MANAGER); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { initmission::printAddObjectError("PUS_MGMT", objects::EVENT_MANAGER); } taskVec.push_back(pusEvents); @@ -144,11 +144,11 @@ void initmission::createPusTasks(TaskFactory& factory, PeriodicTaskIF* pusHighPrio = factory.createPeriodicTask( "PUS_HIGH_PRIO", 50, PeriodicTaskIF::MINIMUM_STACK_SIZE, 0.200, missedDeadlineFunc); result = pusHighPrio->addComponent(objects::PUS_SERVICE_2_DEVICE_ACCESS); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { initmission::printAddObjectError("PUS2", objects::PUS_SERVICE_2_DEVICE_ACCESS); } result = pusHighPrio->addComponent(objects::PUS_SERVICE_9_TIME_MGMT); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { initmission::printAddObjectError("PUS9", objects::PUS_SERVICE_9_TIME_MGMT); } taskVec.push_back(pusHighPrio); @@ -156,19 +156,19 @@ void initmission::createPusTasks(TaskFactory& factory, PeriodicTaskIF* pusMedPrio = factory.createPeriodicTask( "PUS_MED_PRIO", 40, PeriodicTaskIF::MINIMUM_STACK_SIZE, 0.8, missedDeadlineFunc); result = pusMedPrio->addComponent(objects::PUS_SERVICE_8_FUNCTION_MGMT); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { initmission::printAddObjectError("PUS8", objects::PUS_SERVICE_8_FUNCTION_MGMT); } result = pusMedPrio->addComponent(objects::PUS_SERVICE_200_MODE_MGMT); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { initmission::printAddObjectError("PUS200", objects::PUS_SERVICE_200_MODE_MGMT); } result = pusMedPrio->addComponent(objects::PUS_SERVICE_20_PARAMETERS); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { initmission::printAddObjectError("PUS20", objects::PUS_SERVICE_20_PARAMETERS); } result = pusMedPrio->addComponent(objects::PUS_SERVICE_3_HOUSEKEEPING); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { initmission::printAddObjectError("PUS3", objects::PUS_SERVICE_3_HOUSEKEEPING); } taskVec.push_back(pusMedPrio); @@ -176,11 +176,11 @@ void initmission::createPusTasks(TaskFactory& factory, PeriodicTaskIF* pusLowPrio = factory.createPeriodicTask( "PUS_LOW_PRIO", 30, PeriodicTaskIF::MINIMUM_STACK_SIZE, 1.6, missedDeadlineFunc); result = pusLowPrio->addComponent(objects::PUS_SERVICE_17_TEST); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { initmission::printAddObjectError("PUS17", objects::PUS_SERVICE_17_TEST); } result = pusLowPrio->addComponent(objects::INTERNAL_ERROR_REPORTER); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { initmission::printAddObjectError("INT_ERR_RPRT", objects::INTERNAL_ERROR_REPORTER); } taskVec.push_back(pusLowPrio); @@ -189,12 +189,12 @@ void initmission::createPusTasks(TaskFactory& factory, void initmission::createPstTasks(TaskFactory& factory, TaskDeadlineMissedFunction missedDeadlineFunc, std::vector& taskVec) { - ReturnValue_t result = HasReturnvaluesIF::RETURN_OK; + ReturnValue_t result = returnvalue::OK; #if OBSW_ADD_SPI_TEST_CODE == 0 FixedTimeslotTaskIF* spiPst = factory.createFixedTimeslotTask( "SPI_PST", 70, PeriodicTaskIF::MINIMUM_STACK_SIZE * 4, 1.0, missedDeadlineFunc); result = pst::pstSpi(spiPst); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { sif::error << "InitMission::initTasks: Creating PST failed!" << std::endl; } taskVec.push_back(spiPst); @@ -204,28 +204,28 @@ void initmission::createPstTasks(TaskFactory& factory, void initmission::createTestTasks(TaskFactory& factory, TaskDeadlineMissedFunction missedDeadlineFunc, std::vector& taskVec) { - ReturnValue_t result = HasReturnvaluesIF::RETURN_OK; + ReturnValue_t result = returnvalue::OK; PeriodicTaskIF* testTask = factory.createPeriodicTask( "TEST_TASK", 40, PeriodicTaskIF::MINIMUM_STACK_SIZE, 2.0, missedDeadlineFunc); result = testTask->addComponent(objects::TEST_TASK); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { initmission::printAddObjectError("TEST_TASK", objects::TEST_TASK); } #if OBSW_ADD_SPI_TEST_CODE == 1 result = testTask->addComponent(objects::SPI_TEST); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { initmission::printAddObjectError("SPI_TEST", objects::SPI_TEST); } #endif /* RPI_ADD_SPI_TEST == 1 */ #if RPI_ADD_GPIO_TEST == 1 result = testTask->addComponent(objects::LIBGPIOD_TEST); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { initmission::printAddObjectError("GPIOD_TEST", objects::LIBGPIOD_TEST); } #endif /* RPI_ADD_GPIO_TEST == 1 */ #if OBSW_ADD_UART_TEST_CODE == 1 result = testTask->addComponent(objects::UART_TEST); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { initmission::printAddObjectError("UART_TEST", objects::UART_TEST); } #endif /* RPI_ADD_GPIO_TEST == 1 */ @@ -237,7 +237,7 @@ void initmission::createTestTasks(TaskFactory& factory, FixedTimeslotTaskIF* pstTestTask = factory->createFixedTimeslotTask( "TEST_PST", 50, PeriodicTaskIF::MINIMUM_STACK_SIZE * 2, 2.0, missedDeadlineFunc); result = pst::pstTest(pstTestTask); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { sif::info << "initmission::initTasks: ACS PST empty or invalid" << std::endl; startTestPst = false; } diff --git a/bsp_linux_board/gpioInit.cpp b/bsp_linux_board/gpioInit.cpp index f913db8a..b9229614 100644 --- a/bsp_linux_board/gpioInit.cpp +++ b/bsp_linux_board/gpioInit.cpp @@ -40,14 +40,14 @@ void rpi::gpio::initSpiCsDecoder(GpioIF* gpioComIF) { for (const auto& info : muxInfo) { result = createRpiGpioConfig(spiMuxGpios, info.gpioId, info.bcmNum, info.consumer, Direction::OUT, Levels::LOW); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { sif::error << "Creating Raspberry Pi SPI Mux GPIO failed with code " << result << std::endl; return; } } result = gpioComIF->addGpios(spiMuxGpios); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { sif::error << "initSpiCsDecoder: Failed to add mux bit gpios to gpioComIF" << std::endl; return; } diff --git a/bsp_q7s/boardtest/Q7STestTask.cpp b/bsp_q7s/boardtest/Q7STestTask.cpp index 6cf4e734..08a980e7 100644 --- a/bsp_q7s/boardtest/Q7STestTask.cpp +++ b/bsp_q7s/boardtest/Q7STestTask.cpp @@ -95,23 +95,23 @@ void Q7STestTask::fileTests() { void Q7STestTask::testScratchApi() { ReturnValue_t result = scratch::writeNumber("TEST", 1); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { sif::debug << "Q7STestTask::scratchApiTest: Writing number failed" << std::endl; } int number = 0; result = scratch::readNumber("TEST", number); sif::info << "Q7STestTask::testScratchApi: Value for key \"TEST\": " << number << std::endl; - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { sif::debug << "Q7STestTask::scratchApiTest: Reading number failed" << std::endl; } result = scratch::writeString("TEST2", "halloWelt"); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { sif::debug << "Q7STestTask::scratchApiTest: Writing string failed" << std::endl; } std::string string; result = scratch::readString("TEST2", string); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { sif::debug << "Q7STestTask::scratchApiTest: Reading number failed" << std::endl; } sif::info << "Q7STestTask::testScratchApi: Value for key \"TEST2\": " << string << std::endl; @@ -143,7 +143,7 @@ void Q7STestTask::testDummyParams() { } ReturnValue_t result = param.readJsonFile(); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { } param.setValue(DummyParameter::DUMMY_KEY_PARAM_1, 3); @@ -154,13 +154,13 @@ void Q7STestTask::testDummyParams() { int test = 0; result = param.getValue(DummyParameter::DUMMY_KEY_PARAM_1, test); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { sif::warning << "Q7STestTask::testDummyParams: Key " << DummyParameter::DUMMY_KEY_PARAM_1 << " does not exist" << std::endl; } std::string test2; result = param.getValue(DummyParameter::DUMMY_KEY_PARAM_2, test2); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { sif::warning << "Q7STestTask::testDummyParams: Key " << DummyParameter::DUMMY_KEY_PARAM_1 << " does not exist" << std::endl; } @@ -179,18 +179,18 @@ ReturnValue_t Q7STestTask::initialize() { void Q7STestTask::testProtHandler() { bool opPerformed = false; - ReturnValue_t result = HasReturnvaluesIF::RETURN_OK; + ReturnValue_t result = returnvalue::OK; // If any chips are unlocked, lock them here result = coreController->setBootCopyProtection(xsc::Chip::ALL_CHIP, xsc::Copy::ALL_COPY, true, opPerformed, true); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { sif::warning << "Q7STestTask::testProtHandler: Op failed" << std::endl; } // unlock own copy result = coreController->setBootCopyProtection(xsc::Chip::SELF_CHIP, xsc::Copy::SELF_COPY, false, opPerformed, true); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { sif::warning << "Q7STestTask::testProtHandler: Op failed" << std::endl; } if (not opPerformed) { @@ -204,7 +204,7 @@ void Q7STestTask::testProtHandler() { // lock own copy result = coreController->setBootCopyProtection(xsc::Chip::SELF_CHIP, xsc::Copy::SELF_COPY, true, opPerformed, true); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { sif::warning << "Q7STestTask::testProtHandler: Op failed" << std::endl; } if (not opPerformed) { @@ -218,7 +218,7 @@ void Q7STestTask::testProtHandler() { // unlock specific copy result = coreController->setBootCopyProtection(xsc::Chip::CHIP_1, xsc::Copy::COPY_1, false, opPerformed, true); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { sif::warning << "Q7STestTask::testProtHandler: Op failed" << std::endl; } if (not opPerformed) { @@ -232,7 +232,7 @@ void Q7STestTask::testProtHandler() { // lock specific copy result = coreController->setBootCopyProtection(xsc::Chip::CHIP_1, xsc::Copy::COPY_1, true, opPerformed, true); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { sif::warning << "Q7STestTask::testProtHandler: Op failed" << std::endl; } if (not opPerformed) { @@ -341,24 +341,24 @@ void Q7STestTask::testFileSystemHandlerDirect(FsOpCodes opCode) { << std::endl; } FileSystemHandler::FsCommandCfg cfg = {}; - ReturnValue_t result = HasReturnvaluesIF::RETURN_OK; + ReturnValue_t result = returnvalue::OK; // Lambda for common code auto createNonEmptyTmpDir = [&]() { if (not std::filesystem::exists("/tmp/test")) { result = fsHandler->createDirectory("/tmp", "test", false, &cfg); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { return result; } } // Creating sample files sif::info << "Creating sample files in directory" << std::endl; result = fsHandler->createFile("/tmp/test", "test1.txt", nullptr, 0, &cfg); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { return result; } result = fsHandler->createFile("/tmp/test", "test2.txt", nullptr, 0, &cfg); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { return result; } return result; @@ -383,7 +383,7 @@ void Q7STestTask::testFileSystemHandlerDirect(FsOpCodes opCode) { fsHandler->createFile("/tmp/", "test.txt", nullptr, 0, &cfg); } result = fsHandler->removeFile("/tmp", "test.txt", &cfg); - if (result == HasReturnvaluesIF::RETURN_OK) { + if (result == returnvalue::OK) { sif::info << "File removed successfully" << std::endl; } else { sif::warning << "File removal failed!" << std::endl; @@ -396,7 +396,7 @@ void Q7STestTask::testFileSystemHandlerDirect(FsOpCodes opCode) { sif::info << "Creating empty file in /tmp folder" << std::endl; // Do not delete file, user can check existence in shell ReturnValue_t result = fsHandler->createDirectory("/tmp/", "test", false, &cfg); - if (result == HasReturnvaluesIF::RETURN_OK) { + if (result == returnvalue::OK) { sif::info << "Directory created successfully" << std::endl; } else { sif::warning << "Directory creation failed!" << std::endl; @@ -413,7 +413,7 @@ void Q7STestTask::testFileSystemHandlerDirect(FsOpCodes opCode) { std::remove("/tmp/test/*"); } result = fsHandler->removeDirectory("/tmp/", "test", false, &cfg); - if (result == HasReturnvaluesIF::RETURN_OK) { + if (result == returnvalue::OK) { sif::info << "Directory removed successfully" << std::endl; } else { sif::warning << "Directory removal failed!" << std::endl; @@ -422,11 +422,11 @@ void Q7STestTask::testFileSystemHandlerDirect(FsOpCodes opCode) { } case (FsOpCodes::REMOVE_FILLED_DIR_IN_TMP): { result = createNonEmptyTmpDir(); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { return; } result = fsHandler->removeDirectory("/tmp/", "test", true, &cfg); - if (result == HasReturnvaluesIF::RETURN_OK) { + if (result == returnvalue::OK) { sif::info << "Directory removed recursively successfully" << std::endl; } else { sif::warning << "Recursive directory removal failed!" << std::endl; @@ -435,11 +435,11 @@ void Q7STestTask::testFileSystemHandlerDirect(FsOpCodes opCode) { } case (FsOpCodes::ATTEMPT_DIR_REMOVAL_NON_EMPTY): { result = createNonEmptyTmpDir(); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { return; } result = fsHandler->removeDirectory("/tmp/", "test", false, &cfg); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { sif::info << "Directory removal attempt failed as expected" << std::endl; } else { sif::warning << "Directory removal worked when it should not have!" << std::endl; @@ -478,7 +478,7 @@ void Q7STestTask::testFileSystemHandlerDirect(FsOpCodes opCode) { } void Q7STestTask::xadcTest() { - ReturnValue_t result = RETURN_OK; + ReturnValue_t result = returnvalue::OK; float temperature = 0; float vccPint = 0; float vccPaux = 0; @@ -490,39 +490,39 @@ void Q7STestTask::xadcTest() { float vrefn = 0; Xadc xadc; result = xadc.getTemperature(temperature); - if (result == HasReturnvaluesIF::RETURN_OK) { + if (result == returnvalue::OK) { sif::info << "Q7STestTask::xadcTest: Chip Temperature: " << temperature << " °C" << std::endl; } result = xadc.getVccPint(vccPint); - if (result == HasReturnvaluesIF::RETURN_OK) { + if (result == returnvalue::OK) { sif::info << "Q7STestTask::xadcTest: VCC PS internal: " << vccPint << " mV" << std::endl; } result = xadc.getVccPaux(vccPaux); - if (result == HasReturnvaluesIF::RETURN_OK) { + if (result == returnvalue::OK) { sif::info << "Q7STestTask::xadcTest: VCC PS auxilliary: " << vccPaux << " mV" << std::endl; } result = xadc.getVccInt(vccInt); - if (result == HasReturnvaluesIF::RETURN_OK) { + if (result == returnvalue::OK) { sif::info << "Q7STestTask::xadcTest: VCC PL internal: " << vccInt << " mV" << std::endl; } result = xadc.getVccAux(vccAux); - if (result == HasReturnvaluesIF::RETURN_OK) { + if (result == returnvalue::OK) { sif::info << "Q7STestTask::xadcTest: VCC PL auxilliary: " << vccAux << " mV" << std::endl; } result = xadc.getVccBram(vccBram); - if (result == HasReturnvaluesIF::RETURN_OK) { + if (result == returnvalue::OK) { sif::info << "Q7STestTask::xadcTest: VCC BRAM: " << vccBram << " mV" << std::endl; } result = xadc.getVccOddr(vccOddr); - if (result == HasReturnvaluesIF::RETURN_OK) { + if (result == returnvalue::OK) { sif::info << "Q7STestTask::xadcTest: VCC PS I/O DDR : " << vccOddr << " mV" << std::endl; } result = xadc.getVrefp(vrefp); - if (result == HasReturnvaluesIF::RETURN_OK) { + if (result == returnvalue::OK) { sif::info << "Q7STestTask::xadcTest: Vrefp : " << vrefp << " mV" << std::endl; } result = xadc.getVrefn(vrefn); - if (result == HasReturnvaluesIF::RETURN_OK) { + if (result == returnvalue::OK) { sif::info << "Q7STestTask::xadcTest: Vrefn : " << vrefn << " mV" << std::endl; } } diff --git a/bsp_q7s/callbacks/gnssCallback.cpp b/bsp_q7s/callbacks/gnssCallback.cpp index 7e854b6d..22ceb3ae 100644 --- a/bsp_q7s/callbacks/gnssCallback.cpp +++ b/bsp_q7s/callbacks/gnssCallback.cpp @@ -11,10 +11,10 @@ ReturnValue_t gps::triggerGpioResetPin(const uint8_t* actionData, size_t len, vo } ResetArgs* resetArgs = reinterpret_cast(args); if (args == nullptr) { - return HasReturnvaluesIF::RETURN_FAILED; + return returnvalue::FAILED; } if (resetArgs->gpioComIF == nullptr) { - return HasReturnvaluesIF::RETURN_FAILED; + return returnvalue::FAILED; } gpioId_t gpioId; if (actionData[0] == 0) { @@ -25,5 +25,5 @@ ReturnValue_t gps::triggerGpioResetPin(const uint8_t* actionData, size_t len, vo resetArgs->gpioComIF->pullLow(gpioId); TaskFactory::delayTask(resetArgs->waitPeriodMs); resetArgs->gpioComIF->pullHigh(gpioId); - return HasReturnvaluesIF::RETURN_OK; + return returnvalue::OK; } diff --git a/bsp_q7s/callbacks/gnssCallback.h b/bsp_q7s/callbacks/gnssCallback.h index cd69f5a6..331f97e7 100644 --- a/bsp_q7s/callbacks/gnssCallback.h +++ b/bsp_q7s/callbacks/gnssCallback.h @@ -1,7 +1,7 @@ #ifndef BSP_Q7S_CALLBACKS_GNSSCALLBACK_H_ #define BSP_Q7S_CALLBACKS_GNSSCALLBACK_H_ -#include "fsfw/returnvalues/HasReturnvaluesIF.h" +#include "fsfw/returnvalues/returnvalue.h" #include "fsfw_hal/linux/gpio/LinuxLibgpioIF.h" struct ResetArgs { diff --git a/bsp_q7s/callbacks/q7sGpioCallbacks.cpp b/bsp_q7s/callbacks/q7sGpioCallbacks.cpp index cf222b03..512050e3 100644 --- a/bsp_q7s/callbacks/q7sGpioCallbacks.cpp +++ b/bsp_q7s/callbacks/q7sGpioCallbacks.cpp @@ -47,7 +47,7 @@ void q7s::gpioCallbacks::initSpiCsDecoder(GpioIF* gpioComIF) { spiMuxGpios->addGpio(gpioIds::EN_RW_CS, enRwDecoder); result = gpioComIF->addGpios(spiMuxGpios); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { sif::error << "initSpiCsDecoder: Failed to add SPI MUX bit GPIOs" << std::endl; return; } diff --git a/bsp_q7s/callbacks/rwSpiCallback.cpp b/bsp_q7s/callbacks/rwSpiCallback.cpp index 0fd2c512..ae23d64d 100644 --- a/bsp_q7s/callbacks/rwSpiCallback.cpp +++ b/bsp_q7s/callbacks/rwSpiCallback.cpp @@ -29,12 +29,12 @@ void closeSpi(int fd, gpioId_t gpioId, GpioIF* gpioIF, MutexIF* mutex); ReturnValue_t spiCallback(SpiComIF* comIf, SpiCookie* cookie, const uint8_t* sendData, size_t sendLen, void* args) { // Stopwatch watch; - ReturnValue_t result = HasReturnvaluesIF::RETURN_OK; + ReturnValue_t result = returnvalue::OK; RwHandler* handler = reinterpret_cast(args); if (handler == nullptr) { sif::error << "rwSpiCallback::spiCallback: Pointer to handler is invalid" << std::endl; - return HasReturnvaluesIF::RETURN_FAILED; + return returnvalue::FAILED; } uint8_t writeBuffer[2] = {}; @@ -48,13 +48,13 @@ ReturnValue_t spiCallback(SpiComIF* comIf, SpiCookie* cookie, const uint8_t* sen cookie->getMutexParams(timeoutType, timeoutMs); if (mutex == nullptr or gpioIF == nullptr) { sif::debug << "rwSpiCallback::spiCallback: Mutex or GPIO interface invalid" << std::endl; - return HasReturnvaluesIF::RETURN_FAILED; + return returnvalue::FAILED; } int fileDescriptor = 0; const std::string& dev = comIf->getSpiDev(); result = openSpi(dev, O_RDWR, gpioIF, gpioId, mutex, timeoutType, timeoutMs, fileDescriptor); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { return result; } @@ -118,7 +118,7 @@ ReturnValue_t spiCallback(SpiComIF* comIf, SpiCookie* cookie, const uint8_t* sen uint8_t* rxBuf = nullptr; result = comIf->getReadBuffer(cookie->getSpiAddress(), &rxBuf); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { closeSpi(fileDescriptor, gpioId, gpioIF, mutex); return result; } @@ -130,7 +130,7 @@ ReturnValue_t spiCallback(SpiComIF* comIf, SpiCookie* cookie, const uint8_t* sen closeSpi(fileDescriptor, gpioId, gpioIF, mutex); usleep(RwDefinitions::SPI_REPLY_DELAY); result = openSpi(dev, O_RDWR, gpioIF, gpioId, mutex, timeoutType, timeoutMs, fileDescriptor); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { return result; } @@ -228,7 +228,7 @@ ReturnValue_t spiCallback(SpiComIF* comIf, SpiCookie* cookie, const uint8_t* sen break; } } - result = HasReturnvaluesIF::RETURN_OK; + result = returnvalue::OK; } cookie->setTransferSize(decodedFrameLen); @@ -244,7 +244,7 @@ ReturnValue_t openSpi(const std::string& devname, int flags, GpioIF* gpioIF, gpi MutexIF* mutex, MutexIF::TimeoutType timeoutType, uint32_t timeoutMs, int& fd) { ReturnValue_t result = mutex->lockMutex(timeoutType, timeoutMs); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { sif::debug << "rwSpiCallback::spiCallback: Failed to lock mutex" << std::endl; return result; } @@ -258,21 +258,21 @@ ReturnValue_t openSpi(const std::string& devname, int flags, GpioIF* gpioIF, gpi // Pull SPI CS low. For now, no support for active high given if (gpioId != gpio::NO_GPIO) { result = gpioIF->pullLow(gpioId); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { sif::error << "rwSpiCallback::spiCallback: Failed to pull chip select low" << std::endl; return result; } } - return HasReturnvaluesIF::RETURN_OK; + return returnvalue::OK; } void closeSpi(int fd, gpioId_t gpioId, GpioIF* gpioIF, MutexIF* mutex) { close(fd); if (gpioId != gpio::NO_GPIO) { - if (gpioIF->pullHigh(gpioId) != HasReturnvaluesIF::RETURN_OK) { + if (gpioIF->pullHigh(gpioId) != returnvalue::OK) { sif::error << "closeSpi: Failed to pull chip select high" << std::endl; } } - if (mutex->unlockMutex() != HasReturnvaluesIF::RETURN_OK) { + if (mutex->unlockMutex() != returnvalue::OK) { sif::error << "rwSpiCallback::closeSpi: Failed to unlock mutex" << std::endl; ; } diff --git a/bsp_q7s/callbacks/rwSpiCallback.h b/bsp_q7s/callbacks/rwSpiCallback.h index 4a5389a3..7390db5b 100644 --- a/bsp_q7s/callbacks/rwSpiCallback.h +++ b/bsp_q7s/callbacks/rwSpiCallback.h @@ -1,7 +1,7 @@ #ifndef BSP_Q7S_RW_SPI_CALLBACK_H_ #define BSP_Q7S_RW_SPI_CALLBACK_H_ -#include "fsfw/returnvalues/HasReturnvaluesIF.h" +#include "fsfw/returnvalues/returnvalue.h" #include "fsfw_hal/common/gpio/GpioCookie.h" #include "fsfw_hal/linux/spi/SpiComIF.h" diff --git a/bsp_q7s/core/CoreController.cpp b/bsp_q7s/core/CoreController.cpp index 25d47b7c..de36b01a 100644 --- a/bsp_q7s/core/CoreController.cpp +++ b/bsp_q7s/core/CoreController.cpp @@ -33,10 +33,10 @@ CoreController::CoreController(object_id_t objectId) opDivider5(5), opDivider10(10), hkSet(this) { - ReturnValue_t result = HasReturnvaluesIF::RETURN_OK; + ReturnValue_t result = returnvalue::OK; try { result = initWatchdogFifo(); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { sif::warning << "CoreController::CoreController: Watchdog FIFO init failed" << std::endl; } sdcMan = SdCardManager::instance(); @@ -50,7 +50,7 @@ CoreController::CoreController(object_id_t objectId) sdStateMachine(); result = initBootCopy(); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { sif::warning << "CoreController::CoreController: Boot copy init" << std::endl; } } catch (const std::filesystem::filesystem_error &e) { @@ -66,7 +66,7 @@ ReturnValue_t CoreController::handleCommandMessage(CommandMessage *message) { void CoreController::performControlOperation() { EventMessage event; - for (ReturnValue_t result = eventQueue->receiveMessage(&event); result == RETURN_OK; + for (ReturnValue_t result = eventQueue->receiveMessage(&event); result == returnvalue::OK; result = eventQueue->receiveMessage(&event)) { switch (event.getEvent()) { case (GpsHyperion::GPS_FIX_CHANGE): { @@ -93,7 +93,7 @@ ReturnValue_t CoreController::initializeLocalDataPool(localpool::DataPool &local localDataPoolMap.emplace(core::PS_VOLTAGE, new PoolEntry({0})); localDataPoolMap.emplace(core::PL_VOLTAGE, new PoolEntry({0})); poolManager.subscribeForRegularPeriodicPacket({hkSet.getSid(), false, 10.0}); - return HasReturnvaluesIF::RETURN_OK; + return returnvalue::OK; } LocalPoolDataSetBase *CoreController::getDataSetHandle(sid_t sid) { @@ -105,11 +105,11 @@ LocalPoolDataSetBase *CoreController::getDataSetHandle(sid_t sid) { ReturnValue_t CoreController::initialize() { ReturnValue_t result = ExtendedControllerBase::initialize(); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { sif::warning << "CoreController::initialize: Base init failed" << std::endl; } result = scratch::writeNumber(scratch::ALLOC_FAILURE_COUNT, 0); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { sif::warning << "CoreController::initialize: Setting up alloc failure " "count failed" << std::endl; @@ -126,31 +126,31 @@ ReturnValue_t CoreController::initialize() { << std::endl; } result = eventManager->registerListener(eventQueue->getId()); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { sif::warning << "CoreController::initialize: Registering as event listener failed" << std::endl; } result = eventManager->subscribeToEvent(eventQueue->getId(), event::getEventId(GpsHyperion::GPS_FIX_CHANGE)); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { sif::warning << "Subscribing for GPS GPS_FIX_CHANGE event failed" << std::endl; } - return RETURN_OK; + return returnvalue::OK; } ReturnValue_t CoreController::initializeAfterTaskCreation() { - ReturnValue_t result = HasReturnvaluesIF::RETURN_OK; + ReturnValue_t result = returnvalue::OK; sdInfo.pref = sdcMan->getPreferredSdCard(); sdcMan->setActiveSdCard(sdInfo.pref); currMntPrefix = sdcMan->getCurrentMountPrefix(); if (BLOCKING_SD_INIT) { ReturnValue_t result = initSdCardBlocking(); - if (result != HasReturnvaluesIF::RETURN_OK and result != SdCardManager::ALREADY_MOUNTED) { + if (result != returnvalue::OK and result != SdCardManager::ALREADY_MOUNTED) { sif::warning << "CoreController::CoreController: SD card init failed" << std::endl; } } sdStateMachine(); performMountedSdCardOperations(); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { sif::warning << "CoreController::initialize: Version initialization failed" << std::endl; } // Add script folder to path @@ -236,22 +236,22 @@ ReturnValue_t CoreController::executeAction(ActionId_t actionId, MessageQueueId_ ReturnValue_t CoreController::checkModeCommand(Mode_t mode, Submode_t submode, uint32_t *msToReachTheMode) { - return HasReturnvaluesIF::RETURN_OK; + return returnvalue::OK; } ReturnValue_t CoreController::initSdCardBlocking() { // Create update status file ReturnValue_t result = sdcMan->updateSdCardStateFile(); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { sif::warning << "CoreController::initialize: Updating SD card state file failed" << std::endl; } #if Q7S_SD_CARD_CONFIG == Q7S_SD_NONE sif::info << "No SD card initialization will be performed" << std::endl; - return HasReturnvaluesIF::RETURN_OK; + return returnvalue::OK; #else result = sdcMan->getSdCardsStatus(sdInfo.currentState); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { sif::warning << "Getting SD card activity status failed" << std::endl; } @@ -269,14 +269,14 @@ ReturnValue_t CoreController::initSdCardBlocking() { sdCardSetup(sd::SdCard::SLOT_1, sd::SdState::MOUNTED, "1", false); // Update status file sdcMan->updateSdCardStateFile(); - return HasReturnvaluesIF::RETURN_OK; + return returnvalue::OK; #endif #endif /* Q7S_SD_CARD_CONFIG != Q7S_SD_NONE */ } ReturnValue_t CoreController::sdStateMachine() { - ReturnValue_t result = HasReturnvaluesIF::RETURN_OK; + ReturnValue_t result = returnvalue::OK; SdCardManager::Operations operation; if (sdInfo.state == SdStates::IDLE) { @@ -324,7 +324,7 @@ ReturnValue_t CoreController::sdStateMachine() { if (not sdInfo.commandExecuted) { // Create update status file result = sdcMan->updateSdCardStateFile(); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { sif::warning << "CoreController::initialize: Updating SD card state file failed" << std::endl; } @@ -343,7 +343,7 @@ ReturnValue_t CoreController::sdStateMachine() { sif::warning << "Preferred SD card invalid. Setting to card 0.." << std::endl; sdInfo.pref = sd::SdCard::SLOT_0; } - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { sif::warning << "Getting SD card activity status failed" << std::endl; } #if Q7S_SD_CARD_CONFIG == Q7S_SD_COLD_REDUNDANT @@ -473,7 +473,7 @@ ReturnValue_t CoreController::sdStateMachine() { sdcMan->setBlocking(true); // Update status file result = sdcMan->updateSdCardStateFile(); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { sif::warning << "CoreController::initialize: Updating SD card state file failed" << std::endl; } sdInfo.commandExecuted = false; @@ -497,7 +497,7 @@ ReturnValue_t CoreController::sdStateMachine() { } sdInfo.cycleCount++; - return HasReturnvaluesIF::RETURN_OK; + return returnvalue::OK; } void CoreController::executeNextExternalSdCommand() { @@ -628,28 +628,28 @@ ReturnValue_t CoreController::sdCardSetup(sd::SdCard sdCard, sd::SdState targetS } else { sif::warning << "CoreController::sdCardSetup: Invalid state for this call" << std::endl; } - return HasReturnvaluesIF::RETURN_OK; + return returnvalue::OK; } ReturnValue_t CoreController::sdColdRedundantBlockingInit() { - ReturnValue_t result = HasReturnvaluesIF::RETURN_OK; + ReturnValue_t result = returnvalue::OK; result = sdCardSetup(sdInfo.pref, sd::SdState::MOUNTED, sdInfo.prefChar); - if (result != SdCardManager::ALREADY_MOUNTED and result != HasReturnvaluesIF::RETURN_OK) { + if (result != SdCardManager::ALREADY_MOUNTED and result != returnvalue::OK) { sif::warning << "Setting up preferred card " << sdInfo.otherChar << " in cold redundant mode failed" << std::endl; // Try other SD card and mark set up operation as failed sdCardSetup(sdInfo.pref, sd::SdState::MOUNTED, sdInfo.prefChar); - result = HasReturnvaluesIF::RETURN_FAILED; + result = returnvalue::FAILED; } - if (result != HasReturnvaluesIF::RETURN_FAILED and sdInfo.otherState != sd::SdState::OFF) { + if (result != returnvalue::FAILED and sdInfo.otherState != sd::SdState::OFF) { sif::info << "Switching off secondary SD card " << sdInfo.otherChar << std::endl; // Switch off other SD card in cold redundant mode if setting up preferred one worked // without issues ReturnValue_t result2 = sdcMan->switchOffSdCard(sdInfo.other, sdInfo.otherState, &sdInfo.currentState); - if (result2 != HasReturnvaluesIF::RETURN_OK and result2 != SdCardManager::ALREADY_OFF) { + if (result2 != returnvalue::OK and result2 != SdCardManager::ALREADY_OFF) { sif::warning << "Switching off secondary SD card " << sdInfo.otherChar << " in cold redundant mode failed" << std::endl; } @@ -660,7 +660,7 @@ ReturnValue_t CoreController::sdColdRedundantBlockingInit() { ReturnValue_t CoreController::incrementAllocationFailureCount() { uint32_t count = 0; ReturnValue_t result = scratch::readNumber(scratch::ALLOC_FAILURE_COUNT, count); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { return result; } count++; @@ -698,7 +698,7 @@ ReturnValue_t CoreController::initVersionFile() { versionFile << fullObswVersionString << std::endl; versionFile << fullFsfwVersionString << std::endl; versionFile << systemString << std::endl; - return HasReturnvaluesIF::RETURN_OK; + return returnvalue::OK; } // Check whether any version has changed @@ -745,7 +745,7 @@ ReturnValue_t CoreController::initVersionFile() { versionFile << systemString << std::endl; } - return HasReturnvaluesIF::RETURN_OK; + return returnvalue::OK; } ReturnValue_t CoreController::actionListDirectoryIntoFile(ActionId_t actionId, @@ -800,7 +800,7 @@ ReturnValue_t CoreController::actionListDirectoryIntoFile(ActionId_t actionId, utility::handleSystemError(result, "CoreController::actionListDirectoryIntoFile"); actionHelper.finish(false, commandedBy, actionId); } - return HasReturnvaluesIF::RETURN_OK; + return returnvalue::OK; } ReturnValue_t CoreController::initBootCopy() { @@ -815,7 +815,7 @@ ReturnValue_t CoreController::initBootCopy() { } getCurrentBootCopy(CURRENT_CHIP, CURRENT_COPY); - return HasReturnvaluesIF::RETURN_OK; + return returnvalue::OK; } void CoreController::getCurrentBootCopy(xsc::Chip &chip, xsc::Copy ©) { @@ -829,10 +829,10 @@ void CoreController::getCurrentBootCopy(xsc::Chip &chip, xsc::Copy ©) { ReturnValue_t CoreController::initWatchdogFifo() { if (not std::filesystem::exists(watchdog::FIFO_NAME)) { - // Still return RETURN_OK for now + // Still return returnvalue::OK for now sif::info << "Watchdog FIFO " << watchdog::FIFO_NAME << " does not exist, can't initiate" << " watchdog" << std::endl; - return HasReturnvaluesIF::RETURN_OK; + return returnvalue::OK; } // Open FIFO write only and non-blocking to prevent SW from killing itself. watchdogFifoFd = open(watchdog::FIFO_NAME.c_str(), O_WRONLY | O_NONBLOCK); @@ -843,10 +843,10 @@ ReturnValue_t CoreController::initWatchdogFifo() { } else { sif::error << "Opening pipe " << watchdog::FIFO_NAME << " write-only failed with " << errno << ": " << strerror(errno) << std::endl; - return HasReturnvaluesIF::RETURN_FAILED; + return returnvalue::FAILED; } } - return HasReturnvaluesIF::RETURN_OK; + return returnvalue::OK; } void CoreController::initPrint() { @@ -872,7 +872,7 @@ ReturnValue_t CoreController::actionXscReboot(const uint8_t *data, size_t size) int result = std::system("xsc_boot_copy -r"); if (result != 0) { utility::handleSystemError(result, "CoreController::executeAction"); - return HasReturnvaluesIF::RETURN_FAILED; + return returnvalue::FAILED; } return HasActionsIF::EXECUTION_FINISHED; } @@ -929,14 +929,14 @@ ReturnValue_t CoreController::actionXscReboot(const uint8_t *data, size_t size) default: break; } - return HasReturnvaluesIF::RETURN_FAILED; + return returnvalue::FAILED; } ReturnValue_t CoreController::actionReboot(const uint8_t *data, size_t size) { bool protOpPerformed = false; gracefulShutdownTasks(xsc::Chip::CHIP_0, xsc::Copy::COPY_0, protOpPerformed); std::system("reboot"); - return RETURN_OK; + return returnvalue::OK; } ReturnValue_t CoreController::gracefulShutdownTasks(xsc::Chip chip, xsc::Copy copy, @@ -948,7 +948,7 @@ ReturnValue_t CoreController::gracefulShutdownTasks(xsc::Chip chip, xsc::Copy co // If any boot copies are unprotected ReturnValue_t result = setBootCopyProtection(xsc::Chip::SELF_CHIP, xsc::Copy::SELF_COPY, true, protOpPerformed, false); - if (result == HasReturnvaluesIF::RETURN_OK and protOpPerformed) { + if (result == returnvalue::OK and protOpPerformed) { // TODO: Would be nice to notify operator. But we can't use the filesystem anymore // and a reboot is imminent. Use scratch buffer? sif::info << "Running slot was writeprotected before reboot" << std::endl; @@ -983,9 +983,9 @@ ReturnValue_t CoreController::generateChipStateFile() { int result = std::system(CHIP_PROT_SCRIPT); if (result != 0) { utility::handleSystemError(result, "CoreController::generateChipStateFile"); - return HasReturnvaluesIF::RETURN_FAILED; + return returnvalue::FAILED; } - return HasReturnvaluesIF::RETURN_OK; + return returnvalue::OK; } ReturnValue_t CoreController::setBootCopyProtection(xsc::Chip targetChip, xsc::Copy targetCopy, @@ -1003,7 +1003,7 @@ ReturnValue_t CoreController::setBootCopyProtection(xsc::Chip targetChip, xsc::C break; } case (xsc::Chip::NO_CHIP): { - return HasReturnvaluesIF::RETURN_OK; + return returnvalue::OK; } case (xsc::Chip::SELF_CHIP): { selfChip = true; @@ -1020,7 +1020,7 @@ ReturnValue_t CoreController::setBootCopyProtection(xsc::Chip targetChip, xsc::C break; } case (xsc::Copy::NO_COPY): { - return HasReturnvaluesIF::RETURN_OK; + return returnvalue::OK; } case (xsc::Copy::SELF_COPY): { selfCopy = true; @@ -1042,7 +1042,7 @@ ReturnValue_t CoreController::setBootCopyProtection(xsc::Chip targetChip, xsc::C if (protOperationPerformed and updateProtFile) { updateProtInfo(); } - return HasReturnvaluesIF::RETURN_OK; + return returnvalue::OK; } int CoreController::handleBootCopyProtAtIndex(xsc::Chip targetChip, xsc::Copy targetCopy, @@ -1130,28 +1130,28 @@ int CoreController::handleBootCopyProtAtIndex(xsc::Chip targetChip, xsc::Copy ta ReturnValue_t CoreController::updateProtInfo(bool regenerateChipStateFile) { using namespace std; - ReturnValue_t result = HasReturnvaluesIF::RETURN_OK; + ReturnValue_t result = returnvalue::OK; if (regenerateChipStateFile) { result = generateChipStateFile(); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { sif::warning << "CoreController::updateProtInfo: Generating chip state file failed" << std::endl; return result; } } if (not filesystem::exists(CHIP_STATE_FILE)) { - return HasReturnvaluesIF::RETURN_FAILED; + return returnvalue::FAILED; } ifstream chipStateFile(CHIP_STATE_FILE); if (not chipStateFile.good()) { - return HasReturnvaluesIF::RETURN_FAILED; + return returnvalue::FAILED; } string nextLine; uint8_t lineCounter = 0; string word; while (getline(chipStateFile, nextLine)) { ReturnValue_t result = handleProtInfoUpdateLine(nextLine); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { sif::warning << "CoreController::updateProtInfo: Protection info update failed!" << std::endl; return result; } @@ -1162,7 +1162,7 @@ ReturnValue_t CoreController::updateProtInfo(bool regenerateChipStateFile) { << std::endl; } } - return HasReturnvaluesIF::RETURN_OK; + return returnvalue::OK; } ReturnValue_t CoreController::handleProtInfoUpdateLine(std::string nextLine) { @@ -1207,7 +1207,7 @@ ReturnValue_t CoreController::handleProtInfoUpdateLine(std::string nextLine) { } wordIdx++; } - return HasReturnvaluesIF::RETURN_OK; + return returnvalue::OK; } void CoreController::performWatchdogControlOperation() { @@ -1274,7 +1274,7 @@ ReturnValue_t CoreController::performSdCardCheck() { sdcMan->getSdCardsStatus(active); auto sdCardCheck = [&](sd::SdCard sdCard) { ReturnValue_t result = sdcMan->isSdCardMountedReadOnly(sdCard, mountedReadOnly); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { sif::error << "CoreController::performSdCardCheck: Could not check " "read-only mount state" << std::endl; @@ -1283,13 +1283,13 @@ ReturnValue_t CoreController::performSdCardCheck() { if (mountedReadOnly) { int linuxErrno = 0; result = sdcMan->performFsck(sdCard, true, linuxErrno); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { sif::error << "CoreController::performSdCardCheck: fsck command on SD Card " << static_cast(sdCard) << " failed with code " << linuxErrno << " | " << strerror(linuxErrno); } result = sdcMan->remountReadWrite(sdCard); - if (result == HasReturnvaluesIF::RETURN_OK) { + if (result == returnvalue::OK) { sif::warning << "CoreController::performSdCardCheck: Remounted SD Card " << static_cast(sdCard) << " read-write"; } else { @@ -1305,7 +1305,7 @@ ReturnValue_t CoreController::performSdCardCheck() { sdCardCheck(sd::SdCard::SLOT_1); } - return RETURN_OK; + return returnvalue::OK; } void CoreController::performRebootFileHandling(bool recreateFile) { @@ -1759,7 +1759,7 @@ ReturnValue_t CoreController::timeFileHandler() { // It is assumed that the system time is set from the GPS time timeval currentTime = {}; ReturnValue_t result = Clock::getClock_timeval(¤tTime); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { return result; } std::string fileName = currMntPrefix + TIME_FILE; @@ -1767,11 +1767,11 @@ ReturnValue_t CoreController::timeFileHandler() { if (not timeFile.good()) { sif::error << "CoreController::timeFileHandler: Error opening time file: " << strerror(errno) << std::endl; - return RETURN_FAILED; + return returnvalue::FAILED; } timeFile << "UNIX SECONDS: " << currentTime.tv_sec << std::endl; } - return RETURN_OK; + return returnvalue::OK; } ReturnValue_t CoreController::initClockFromTimeFile() { @@ -1787,18 +1787,18 @@ ReturnValue_t CoreController::initClockFromTimeFile() { istringstream iss(nextWord); iss >> nextWord; if (iss.bad() or nextWord != "UNIX") { - return RETURN_FAILED; + return returnvalue::FAILED; } iss >> nextWord; if (iss.bad() or nextWord != "SECONDS:") { - return RETURN_FAILED; + return returnvalue::FAILED; } iss >> nextWord; timeval currentTime = {}; char *checkPtr; currentTime.tv_sec = strtol(nextWord.c_str(), &checkPtr, 10); if (iss.bad() or *checkPtr) { - return RETURN_FAILED; + return returnvalue::FAILED; } #if OBSW_VERBOSE_LEVEL >= 1 time_t timeRaw = currentTime.tv_sec; @@ -1808,30 +1808,30 @@ ReturnValue_t CoreController::initClockFromTimeFile() { #endif return Clock::setClock(¤tTime); } - return RETURN_OK; + return returnvalue::OK; } void CoreController::readHkData() { - ReturnValue_t result = RETURN_OK; + ReturnValue_t result = returnvalue::OK; result = hkSet.read(TIMEOUT_TYPE, MUTEX_TIMEOUT); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return; } Xadc xadc; result = xadc.getTemperature(hkSet.temperature.value); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { hkSet.temperature.setValid(false); } else { hkSet.temperature.setValid(true); } result = xadc.getVccPint(hkSet.psVoltage.value); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { hkSet.psVoltage.setValid(false); } else { hkSet.psVoltage.setValid(true); } result = xadc.getVccInt(hkSet.plVoltage.value); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { hkSet.plVoltage.setValid(false); } else { hkSet.plVoltage.setValid(true); @@ -1840,7 +1840,7 @@ void CoreController::readHkData() { hkSet.printSet(); #endif /* OBSW_PRINT_CORE_HK == 1 */ result = hkSet.commit(TIMEOUT_TYPE, MUTEX_TIMEOUT); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return; } } diff --git a/bsp_q7s/core/InitMission.cpp b/bsp_q7s/core/InitMission.cpp index 80f5fc9e..6e0fce0c 100644 --- a/bsp_q7s/core/InitMission.cpp +++ b/bsp_q7s/core/InitMission.cpp @@ -10,7 +10,7 @@ #include "fsfw/objectmanager/ObjectManager.h" #include "fsfw/objectmanager/ObjectManagerIF.h" #include "fsfw/platform.h" -#include "fsfw/returnvalues/HasReturnvaluesIF.h" +#include "fsfw/returnvalues/returnvalue.h" #include "fsfw/serviceinterface/ServiceInterfaceStream.h" #include "fsfw/tasks/FixedTimeslotTaskIF.h" #include "fsfw/tasks/PeriodicTaskIF.h" @@ -55,7 +55,7 @@ void initmission::initMission() { void initmission::initTasks() { TaskFactory* factory = TaskFactory::instance(); - ReturnValue_t result = HasReturnvaluesIF::RETURN_OK; + ReturnValue_t result = returnvalue::OK; if (factory == nullptr) { /* Should never happen ! */ return; @@ -69,7 +69,7 @@ void initmission::initTasks() { PeriodicTaskIF* coreController = factory->createPeriodicTask( "CORE_CTRL", 60, PeriodicTaskIF::MINIMUM_STACK_SIZE, 0.4, missedDeadlineFunc); result = coreController->addComponent(objects::CORE_CONTROLLER); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { initmission::printAddObjectError("CORE_CTRL", objects::CORE_CONTROLLER); } @@ -77,15 +77,15 @@ void initmission::initTasks() { PeriodicTaskIF* tmTcDistributor = factory->createPeriodicTask( "DIST", 40, PeriodicTaskIF::MINIMUM_STACK_SIZE, 0.2, missedDeadlineFunc); result = tmTcDistributor->addComponent(objects::CCSDS_PACKET_DISTRIBUTOR); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { initmission::printAddObjectError("CCSDS_DISTRIB", objects::CCSDS_PACKET_DISTRIBUTOR); } result = tmTcDistributor->addComponent(objects::PUS_PACKET_DISTRIBUTOR); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { initmission::printAddObjectError("PUS_PACKET_DISTRIB", objects::PUS_PACKET_DISTRIBUTOR); } result = tmTcDistributor->addComponent(objects::TM_FUNNEL); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { initmission::printAddObjectError("TM_FUNNEL", objects::TM_FUNNEL); } @@ -94,13 +94,13 @@ void initmission::initTasks() { PeriodicTaskIF* tmtcBridgeTask = factory->createPeriodicTask( "TCPIP_TMTC_BRIDGE", 50, PeriodicTaskIF::MINIMUM_STACK_SIZE, 0.2, missedDeadlineFunc); result = tmtcBridgeTask->addComponent(objects::TMTC_BRIDGE); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { initmission::printAddObjectError("TMTC_BRIDGE", objects::TMTC_BRIDGE); } PeriodicTaskIF* tmtcPollingTask = factory->createPeriodicTask( "TMTC_POLLING", 80, PeriodicTaskIF::MINIMUM_STACK_SIZE, 2.0, missedDeadlineFunc); result = tmtcPollingTask->addComponent(objects::TMTC_POLLING_TASK); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { initmission::printAddObjectError("UDP_POLLING", objects::TMTC_POLLING_TASK); } #endif @@ -109,7 +109,7 @@ void initmission::initTasks() { PeriodicTaskIF* ccsdsHandlerTask = factory->createPeriodicTask( "CCSDS_HANDLER", 50, PeriodicTaskIF::MINIMUM_STACK_SIZE, 2.0, missedDeadlineFunc); result = ccsdsHandlerTask->addComponent(objects::CCSDS_HANDLER); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { initmission::printAddObjectError("CCSDS Handler", objects::CCSDS_HANDLER); } @@ -119,7 +119,7 @@ void initmission::initTasks() { PeriodicTaskIF* pdecHandlerTask = factory->createPeriodicTask( "PDEC_HANDLER", 50, PeriodicTaskIF::MINIMUM_STACK_SIZE, 1.0, missedDeadlineFunc); result = pdecHandlerTask->addComponent(objects::PDEC_HANDLER); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { initmission::printAddObjectError("PDEC Handler", objects::PDEC_HANDLER); } #endif /* OBSW_USE_CCSDS_IP_CORE == 1 */ @@ -128,13 +128,13 @@ void initmission::initTasks() { PeriodicTaskIF* acsTask = factory->createPeriodicTask( "ACS_TASK", 50, PeriodicTaskIF::MINIMUM_STACK_SIZE * 2, 0.4, missedDeadlineFunc); result = acsTask->addComponent(objects::GPS_CONTROLLER); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { initmission::printAddObjectError("GPS_CTRL", objects::GPS_CONTROLLER); } #endif /* OBSW_ADD_ACS_HANDLERS */ acsTask->addComponent(objects::ACS_CONTROLLER); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { initmission::printAddObjectError("ACS_CTRL", objects::ACS_CONTROLLER); } @@ -143,19 +143,19 @@ void initmission::initTasks() { static_cast(sysTask); #if OBSW_ADD_ACS_HANDLERS == 1 result = sysTask->addComponent(objects::ACS_BOARD_ASS); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { initmission::printAddObjectError("ACS_BOARD_ASS", objects::ACS_BOARD_ASS); } #endif /* OBSW_ADD_ACS_HANDLERS */ #if OBSW_ADD_RW == 1 result = sysTask->addComponent(objects::RW_ASS); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { initmission::printAddObjectError("RW_ASS", objects::RW_ASS); } #endif #if OBSW_ADD_SUS_BOARD_ASS == 1 result = sysTask->addComponent(objects::SUS_BOARD_ASS); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { initmission::printAddObjectError("SUS_BOARD_ASS", objects::SUS_BOARD_ASS); } #endif @@ -164,7 +164,7 @@ void initmission::initTasks() { PeriodicTaskIF* tcsPollingTask = factory->createPeriodicTask( "TCS_POLLING_TASK", 70, PeriodicTaskIF::MINIMUM_STACK_SIZE * 2, 0.5, missedDeadlineFunc); result = tcsPollingTask->addComponent(objects::SPI_RTD_COM_IF); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { initmission::printAddObjectError("SPI_RTD_POLLING", objects::SPI_RTD_COM_IF); } PeriodicTaskIF* tcsTask = factory->createPeriodicTask( @@ -203,7 +203,7 @@ void initmission::initTasks() { PeriodicTaskIF* fsTask = factory->createPeriodicTask( "FILE_SYSTEM_TASK", 25, PeriodicTaskIF::MINIMUM_STACK_SIZE, 0.4, missedDeadlineFunc); result = fsTask->addComponent(objects::FILE_SYSTEM_HANDLER); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { initmission::printAddObjectError("FILE_SYSTEM_TASK", objects::FILE_SYSTEM_HANDLER); } @@ -211,7 +211,7 @@ void initmission::initTasks() { PeriodicTaskIF* strHelperTask = factory->createPeriodicTask( "STR_HELPER", 20, PeriodicTaskIF::MINIMUM_STACK_SIZE, 0.2, missedDeadlineFunc); result = strHelperTask->addComponent(objects::STR_HELPER); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { initmission::printAddObjectError("STR_HELPER", objects::STR_HELPER); } #endif /* OBSW_ADD_STAR_TRACKER == 1 */ @@ -220,7 +220,7 @@ void initmission::initTasks() { PeriodicTaskIF* mpsocHelperTask = factory->createPeriodicTask( "PLOC_MPSOC_HELPER", 20, PeriodicTaskIF::MINIMUM_STACK_SIZE, 0.2, missedDeadlineFunc); result = mpsocHelperTask->addComponent(objects::PLOC_MPSOC_HELPER); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { initmission::printAddObjectError("PLOC_MPSOC_HELPER", objects::PLOC_MPSOC_HELPER); } #endif /* OBSW_ADD_PLOC_MPSOC */ @@ -229,7 +229,7 @@ void initmission::initTasks() { PeriodicTaskIF* supvHelperTask = factory->createPeriodicTask( "PLOC_SUPV_HELPER", 10, PeriodicTaskIF::MINIMUM_STACK_SIZE, 1.0, missedDeadlineFunc); result = supvHelperTask->addComponent(objects::PLOC_SUPERVISOR_HELPER); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { initmission::printAddObjectError("PLOC_SUPV_HELPER", objects::PLOC_SUPERVISOR_HELPER); } #endif /* OBSW_ADD_PLOC_SUPERVISOR */ @@ -238,7 +238,7 @@ void initmission::initTasks() { PeriodicTaskIF* ptmeTestTask = factory->createPeriodicTask( "PTME_TEST", 80, PeriodicTaskIF::MINIMUM_STACK_SIZE, 2.0, missedDeadlineFunc); result = ptmeTestTask->addComponent(objects::CCSDS_IP_CORE_BRIDGE); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { initmission::printAddObjectError("PTME_TEST", objects::CCSDS_IP_CORE_BRIDGE); } #endif @@ -310,13 +310,13 @@ void initmission::initTasks() { void initmission::createPstTasks(TaskFactory& factory, TaskDeadlineMissedFunction missedDeadlineFunc, std::vector& taskVec) { - ReturnValue_t result = HasReturnvaluesIF::RETURN_OK; + ReturnValue_t result = returnvalue::OK; /* Polling Sequence Table Default */ #if OBSW_ADD_SPI_TEST_CODE == 0 FixedTimeslotTaskIF* spiPst = factory.createFixedTimeslotTask( "MAIN_SPI", 75, PeriodicTaskIF::MINIMUM_STACK_SIZE * 4, 0.5, missedDeadlineFunc); result = pst::pstSpi(spiPst); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { if (result == FixedTimeslotTaskIF::SLOT_LIST_EMPTY) { sif::warning << "InitMission::initTasks: SPI PST is empty" << std::endl; } else { @@ -331,7 +331,7 @@ void initmission::createPstTasks(TaskFactory& factory, FixedTimeslotTaskIF* rwPstTask = factory.createFixedTimeslotTask( "RW_SPI", 65, PeriodicTaskIF::MINIMUM_STACK_SIZE * 2, 1.0, missedDeadlineFunc); result = pst::pstSpiRw(rwPstTask); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { if (result == FixedTimeslotTaskIF::SLOT_LIST_EMPTY) { sif::warning << "InitMission::initTasks: SPI PST is empty" << std::endl; } else { @@ -345,7 +345,7 @@ void initmission::createPstTasks(TaskFactory& factory, FixedTimeslotTaskIF* uartPst = factory.createFixedTimeslotTask( "UART_PST", 65, PeriodicTaskIF::MINIMUM_STACK_SIZE * 4, 0.2, missedDeadlineFunc); result = pst::pstUart(uartPst); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { if (result == FixedTimeslotTaskIF::SLOT_LIST_EMPTY) { sif::warning << "InitMission::initTasks: UART PST is empty" << std::endl; } else { @@ -358,7 +358,7 @@ void initmission::createPstTasks(TaskFactory& factory, FixedTimeslotTaskIF* gpioPst = factory.createFixedTimeslotTask( "GPIO_PST", 70, PeriodicTaskIF::MINIMUM_STACK_SIZE * 2, 0.2, missedDeadlineFunc); result = pst::pstGpio(gpioPst); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { if (result == FixedTimeslotTaskIF::SLOT_LIST_EMPTY) { sif::warning << "InitMission::initTasks: GPIO PST is empty" << std::endl; } else { @@ -371,7 +371,7 @@ void initmission::createPstTasks(TaskFactory& factory, FixedTimeslotTaskIF* i2cPst = factory.createFixedTimeslotTask( "I2C_PST", 65, PeriodicTaskIF::MINIMUM_STACK_SIZE * 4, 0.2, missedDeadlineFunc); result = pst::pstI2c(i2cPst); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { if (result == FixedTimeslotTaskIF::SLOT_LIST_EMPTY) { sif::warning << "InitMission::initTasks: I2C PST is empty" << std::endl; } else { @@ -386,7 +386,7 @@ void initmission::createPstTasks(TaskFactory& factory, FixedTimeslotTaskIF* gomSpacePstTask = factory.createFixedTimeslotTask( "GS_PST_TASK", 70, PeriodicTaskIF::MINIMUM_STACK_SIZE * 4, 1.0, missedDeadlineFunc); result = pst::pstGompaceCan(gomSpacePstTask); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { if (result != FixedTimeslotTaskIF::SLOT_LIST_EMPTY) { sif::error << "InitMission::initTasks: GomSpace PST initialization failed!" << std::endl; } @@ -398,12 +398,12 @@ void initmission::createPstTasks(TaskFactory& factory, void initmission::createPusTasks(TaskFactory& factory, TaskDeadlineMissedFunction missedDeadlineFunc, std::vector& taskVec) { - ReturnValue_t result = HasReturnvaluesIF::RETURN_OK; + ReturnValue_t result = returnvalue::OK; /* PUS Services */ PeriodicTaskIF* pusVerification = factory.createPeriodicTask( "PUS_VERIF", 40, PeriodicTaskIF::MINIMUM_STACK_SIZE, 0.200, missedDeadlineFunc); result = pusVerification->addComponent(objects::PUS_SERVICE_1_VERIFICATION); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { initmission::printAddObjectError("PUS_VERIF", objects::PUS_SERVICE_1_VERIFICATION); } taskVec.push_back(pusVerification); @@ -411,11 +411,11 @@ void initmission::createPusTasks(TaskFactory& factory, PeriodicTaskIF* pusEvents = factory.createPeriodicTask( "PUS_EVENTS", 60, PeriodicTaskIF::MINIMUM_STACK_SIZE, 0.200, missedDeadlineFunc); result = pusEvents->addComponent(objects::PUS_SERVICE_5_EVENT_REPORTING); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { initmission::printAddObjectError("PUS_EVENTS", objects::PUS_SERVICE_5_EVENT_REPORTING); } result = pusEvents->addComponent(objects::EVENT_MANAGER); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { initmission::printAddObjectError("PUS_MGMT", objects::EVENT_MANAGER); } taskVec.push_back(pusEvents); @@ -423,11 +423,11 @@ void initmission::createPusTasks(TaskFactory& factory, PeriodicTaskIF* pusHighPrio = factory.createPeriodicTask( "PUS_HIGH_PRIO", 50, PeriodicTaskIF::MINIMUM_STACK_SIZE, 0.200, missedDeadlineFunc); result = pusHighPrio->addComponent(objects::PUS_SERVICE_2_DEVICE_ACCESS); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { initmission::printAddObjectError("PUS_2", objects::PUS_SERVICE_2_DEVICE_ACCESS); } result = pusHighPrio->addComponent(objects::PUS_SERVICE_9_TIME_MGMT); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { initmission::printAddObjectError("PUS_9", objects::PUS_SERVICE_9_TIME_MGMT); } taskVec.push_back(pusHighPrio); @@ -436,27 +436,27 @@ void initmission::createPusTasks(TaskFactory& factory, "PUS_MED_PRIO", 40, PeriodicTaskIF::MINIMUM_STACK_SIZE, 0.8, missedDeadlineFunc); result = pusMedPrio->addComponent(objects::PUS_SERVICE_3_HOUSEKEEPING); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { initmission::printAddObjectError("PUS_3", objects::PUS_SERVICE_3_HOUSEKEEPING); } result = pusMedPrio->addComponent(objects::PUS_SERVICE_8_FUNCTION_MGMT); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { initmission::printAddObjectError("PUS_8", objects::PUS_SERVICE_8_FUNCTION_MGMT); } result = pusMedPrio->addComponent(objects::PUS_SERVICE_11_TC_SCHEDULER); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { initmission::printAddObjectError("PUS_11", objects::PUS_SERVICE_11_TC_SCHEDULER); } result = pusMedPrio->addComponent(objects::PUS_SERVICE_20_PARAMETERS); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { initmission::printAddObjectError("PUS_20", objects::PUS_SERVICE_20_PARAMETERS); } result = pusMedPrio->addComponent(objects::PUS_SERVICE_200_MODE_MGMT); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { initmission::printAddObjectError("PUS_200", objects::PUS_SERVICE_200_MODE_MGMT); } result = pusMedPrio->addComponent(objects::PUS_SERVICE_201_HEALTH); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { initmission::printAddObjectError("PUS_201", objects::PUS_SERVICE_201_HEALTH); } taskVec.push_back(pusMedPrio); @@ -464,11 +464,11 @@ void initmission::createPusTasks(TaskFactory& factory, PeriodicTaskIF* pusLowPrio = factory.createPeriodicTask( "PUS_LOW_PRIO", 30, PeriodicTaskIF::MINIMUM_STACK_SIZE, 1.6, missedDeadlineFunc); result = pusLowPrio->addComponent(objects::PUS_SERVICE_17_TEST); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { initmission::printAddObjectError("PUS_17", objects::PUS_SERVICE_17_TEST); } result = pusLowPrio->addComponent(objects::INTERNAL_ERROR_REPORTER); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { initmission::printAddObjectError("ERROR_REPORTER", objects::INTERNAL_ERROR_REPORTER); } taskVec.push_back(pusLowPrio); @@ -478,32 +478,32 @@ void initmission::createTestTasks(TaskFactory& factory, TaskDeadlineMissedFunction missedDeadlineFunc, std::vector& taskVec) { #if OBSW_ADD_TEST_TASK == 1 && OBSW_ADD_TEST_CODE == 1 - ReturnValue_t result = HasReturnvaluesIF::RETURN_OK; + ReturnValue_t result = returnvalue::OK; static_cast(result); // supress warning in case it is not used PeriodicTaskIF* testTask = factory.createPeriodicTask( "TEST_TASK", 60, PeriodicTaskIF::MINIMUM_STACK_SIZE, 1, missedDeadlineFunc); result = testTask->addComponent(objects::TEST_TASK); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { initmission::printAddObjectError("TEST_TASK", objects::TEST_TASK); } #if OBSW_ADD_SPI_TEST_CODE == 1 result = testTask->addComponent(objects::SPI_TEST); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { initmission::printAddObjectError("SPI_TEST", objects::SPI_TEST); } #endif #if OBSW_ADD_I2C_TEST_CODE == 1 result = testTask->addComponent(objects::I2C_TEST); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { initmission::printAddObjectError("I2C_TEST", objects::I2C_TEST); } #endif #if OBSW_ADD_UART_TEST_CODE == 1 result = testTask->addComponent(objects::UART_TEST); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { initmission::printAddObjectError("UART_TEST", objects::UART_TEST); } #endif diff --git a/bsp_q7s/core/ObjectFactory.cpp b/bsp_q7s/core/ObjectFactory.cpp index 3ac765ff..888ec04d 100644 --- a/bsp_q7s/core/ObjectFactory.cpp +++ b/bsp_q7s/core/ObjectFactory.cpp @@ -918,7 +918,7 @@ void ObjectFactory::testAcsBrdAss(AcsBoardAssembly* acsAss) { ModeMessage::setModeMessage(&msg, ModeMessage::CMD_MODE_COMMAND, DeviceHandlerIF::MODE_NORMAL, duallane::A_SIDE); ReturnValue_t result = MessageQueueSenderIF::sendMessage(acsAss->getCommandQueue(), &msg); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { sif::warning << "Sending mode command failed" << std::endl; } } diff --git a/bsp_q7s/core/ObjectFactory.h b/bsp_q7s/core/ObjectFactory.h index e6a68cf2..0b81c235 100644 --- a/bsp_q7s/core/ObjectFactory.h +++ b/bsp_q7s/core/ObjectFactory.h @@ -1,7 +1,7 @@ #ifndef BSP_Q7S_OBJECTFACTORY_H_ #define BSP_Q7S_OBJECTFACTORY_H_ -#include +#include #include diff --git a/bsp_q7s/memory/FileSystemHandler.cpp b/bsp_q7s/memory/FileSystemHandler.cpp index 33849fbe..cb122383 100644 --- a/bsp_q7s/memory/FileSystemHandler.cpp +++ b/bsp_q7s/memory/FileSystemHandler.cpp @@ -35,7 +35,7 @@ ReturnValue_t FileSystemHandler::performOperation(uint8_t unsignedChar) { void FileSystemHandler::fileSystemHandlerLoop() { CommandMessage filemsg; - ReturnValue_t result = HasReturnvaluesIF::RETURN_OK; + ReturnValue_t result = returnvalue::OK; while (true) { if (opCounter % 5 == 0) { if (coreCtrl->sdInitFinished()) { @@ -45,7 +45,7 @@ void FileSystemHandler::fileSystemHandlerLoop() { result = mq->receiveMessage(&filemsg); if (result == MessageQueueIF::EMPTY) { break; - } else if (result != HasReturnvaluesIF::RETURN_FAILED) { + } else if (result != returnvalue::FAILED) { sif::warning << "FileSystemHandler::performOperation: Message reception failed!" << std::endl; break; } @@ -91,7 +91,7 @@ void FileSystemHandler::fileSystemCheckup() { << " but does not appear to be mounted. Attempting fix.." << std::endl; // This function will appear to fix the inconsistent state ReturnValue_t result = sdcMan->sanitizeState(&statusPair, preferredSdCard); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { // Oh no. triggerEvent(SdCardManager::SANITIZATION_FAILED, 0, 0); sif::error << "FileSystemHandler::fileSystemCheckup: Sanitization failed" << std::endl; @@ -114,7 +114,7 @@ ReturnValue_t FileSystemHandler::initialize() { } else if (preferredSdCard == sd::SdCard::SLOT_1) { currentMountPrefix = SdCardManager::SD_1_MOUNT_POINT; } - return HasReturnvaluesIF::RETURN_OK; + return returnvalue::OK; } ReturnValue_t FileSystemHandler::appendToFile(const char* repositoryPath, const char* filename, @@ -129,7 +129,7 @@ ReturnValue_t FileSystemHandler::appendToFile(const char* repositoryPath, const if (not file.good()) { return GENERIC_FILE_ERROR; } - return HasReturnvaluesIF::RETURN_OK; + return returnvalue::OK; } ReturnValue_t FileSystemHandler::createFile(const char* repositoryPath, const char* filename, @@ -144,7 +144,7 @@ ReturnValue_t FileSystemHandler::createFile(const char* repositoryPath, const ch if (not file.good()) { return GENERIC_FILE_ERROR; } - return HasReturnvaluesIF::RETURN_OK; + return returnvalue::OK; } ReturnValue_t FileSystemHandler::removeFile(const char* repositoryPath, const char* filename, @@ -158,7 +158,7 @@ ReturnValue_t FileSystemHandler::removeFile(const char* repositoryPath, const ch sif::warning << "FileSystemHandler::deleteFile: Failed with code " << result << std::endl; return GENERIC_FILE_ERROR; } - return HasReturnvaluesIF::RETURN_OK; + return returnvalue::OK; } ReturnValue_t FileSystemHandler::createDirectory(const char* repositoryPath, const char* dirname, @@ -168,7 +168,7 @@ ReturnValue_t FileSystemHandler::createDirectory(const char* repositoryPath, con return DIRECTORY_ALREADY_EXISTS; } if (std::filesystem::create_directory(path)) { - return HasReturnvaluesIF::RETURN_OK; + return returnvalue::OK; } sif::warning << "Creating directory " << path << " failed" << std::endl; return GENERIC_FILE_ERROR; @@ -183,7 +183,7 @@ ReturnValue_t FileSystemHandler::removeDirectory(const char* repositoryPath, con std::error_code err; if (not deleteRecurively) { if (std::filesystem::remove(path, err)) { - return HasReturnvaluesIF::RETURN_OK; + return returnvalue::OK; } else { // Check error code. Most probably denied permissions because folder is not empty sif::warning << "FileSystemHandler::removeDirectory: Deleting directory failed with " @@ -197,7 +197,7 @@ ReturnValue_t FileSystemHandler::removeDirectory(const char* repositoryPath, con } } else { if (std::filesystem::remove_all(path, err)) { - return HasReturnvaluesIF::RETURN_OK; + return returnvalue::OK; } else { sif::warning << "FileSystemHandler::removeDirectory: Deleting directory failed with " "code " @@ -211,14 +211,14 @@ ReturnValue_t FileSystemHandler::removeDirectory(const char* repositoryPath, con } } - return HasReturnvaluesIF::RETURN_OK; + return returnvalue::OK; } ReturnValue_t FileSystemHandler::renameFile(const char* repositoryPath, const char* oldFilename, const char* newFilename, FileSystemArgsIF* args) { auto basepath = getInitPath(args) / repositoryPath; std::filesystem::rename(basepath / oldFilename, basepath / newFilename); - return HasReturnvaluesIF::RETURN_OK; + return returnvalue::OK; } void FileSystemHandler::parseCfg(FsCommandCfg* cfg, bool& useMountPrefix) { diff --git a/bsp_q7s/memory/FilesystemHelper.cpp b/bsp_q7s/memory/FilesystemHelper.cpp index c4b8fa16..91dfa9c5 100644 --- a/bsp_q7s/memory/FilesystemHelper.cpp +++ b/bsp_q7s/memory/FilesystemHelper.cpp @@ -12,7 +12,7 @@ ReturnValue_t FilesystemHelper::checkPath(std::string path) { SdCardManager* sdcMan = SdCardManager::instance(); if (sdcMan == nullptr) { sif::warning << "FilesystemHelper::checkPath: Invalid SD card manager" << std::endl; - return RETURN_FAILED; + return returnvalue::FAILED; } if (path.substr(0, sizeof(SdCardManager::SD_0_MOUNT_POINT)) == std::string(SdCardManager::SD_0_MOUNT_POINT)) { @@ -27,12 +27,12 @@ ReturnValue_t FilesystemHelper::checkPath(std::string path) { return SD_NOT_MOUNTED; } } - return RETURN_OK; + return returnvalue::OK; } ReturnValue_t FilesystemHelper::fileExists(std::string file) { if (not std::filesystem::exists(file)) { return FILE_NOT_EXISTS; } - return RETURN_OK; + return returnvalue::OK; } diff --git a/bsp_q7s/memory/FilesystemHelper.h b/bsp_q7s/memory/FilesystemHelper.h index efdf5c6a..8233d4db 100644 --- a/bsp_q7s/memory/FilesystemHelper.h +++ b/bsp_q7s/memory/FilesystemHelper.h @@ -4,14 +4,14 @@ #include #include "commonClassIds.h" -#include "fsfw/returnvalues/HasReturnvaluesIF.h" +#include "fsfw/returnvalues/returnvalue.h" /** * @brief This class implements often used functions related to the file system management. * * @author J. Meier */ -class FilesystemHelper : public HasReturnvaluesIF { +class FilesystemHelper { public: static const uint8_t INTERFACE_ID = CLASS_ID::FILE_SYSTEM_HELPER; @@ -26,7 +26,7 @@ class FilesystemHelper : public HasReturnvaluesIF { * * @param path Path to check * - * @return RETURN_OK if path points to SD card and the appropriate SD card is mounted or if + * @return returnvalue::OK if path points to SD card and the appropriate SD card is mounted or if * path does not point to SD card. * Return error code if path points to SD card and the corresponding SD card is not * mounted. @@ -38,7 +38,7 @@ class FilesystemHelper : public HasReturnvaluesIF { * * @param file File to check * - * @return RETURN_OK if file exists, otherwise return error code. + * @return returnvalue::OK if file exists, otherwise return error code. */ static ReturnValue_t fileExists(std::string file); diff --git a/bsp_q7s/memory/SdCardManager.cpp b/bsp_q7s/memory/SdCardManager.cpp index cfd4f490..e2702771 100644 --- a/bsp_q7s/memory/SdCardManager.cpp +++ b/bsp_q7s/memory/SdCardManager.cpp @@ -21,17 +21,17 @@ SdCardManager* SdCardManager::INSTANCE = nullptr; SdCardManager::SdCardManager() : SystemObject(objects::SDC_MANAGER), cmdExecutor(256) { mutex = MutexFactory::instance()->createMutex(); ReturnValue_t result = mutex->lockMutex(); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { sif::error << "SdCardManager::SdCardManager: Mutex lock failed" << std::endl; } uint8_t prefSdRaw = 0; result = scratch::readNumber(scratch::PREFERED_SDC_KEY, prefSdRaw); result = mutex->unlockMutex(); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { sif::error << "SdCardManager::SdCardManager: Mutex unlock failed" << std::endl; } - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { if (result == scratch::KEY_NOT_FOUND) { sif::warning << "CoreController::sdCardInit: " "Preferred SD card not set. Setting to 0" @@ -65,7 +65,7 @@ SdCardManager* SdCardManager::instance() { ReturnValue_t SdCardManager::switchOnSdCard(sd::SdCard sdCard, bool doMountSdCard, SdStatePair* statusPair) { - ReturnValue_t result = HasReturnvaluesIF::RETURN_OK; + ReturnValue_t result = returnvalue::OK; if (doMountSdCard) { if (not blocking) { sif::warning << "SdCardManager::switchOnSdCard: Two-step command but manager is" @@ -80,7 +80,7 @@ ReturnValue_t SdCardManager::switchOnSdCard(sd::SdCard sdCard, bool doMountSdCar sdStatusPtr = std::make_unique(); statusPair = sdStatusPtr.get(); result = getSdCardsStatus(*statusPair); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { return result; } } @@ -89,7 +89,7 @@ ReturnValue_t SdCardManager::switchOnSdCard(sd::SdCard sdCard, bool doMountSdCar if (sdCard == sd::SdCard::BOTH) { sif::warning << "SdCardManager::switchOffSdCard: API does not allow sd::SdStatus::BOTH" << std::endl; - return HasReturnvaluesIF::RETURN_FAILED; + return returnvalue::FAILED; } sd::SdState currentState; @@ -113,10 +113,10 @@ ReturnValue_t SdCardManager::switchOnSdCard(sd::SdCard sdCard, bool doMountSdCar } else if (currentState == sd::SdState::OFF) { result = setSdCardState(sdCard, true); } else { - result = HasReturnvaluesIF::RETURN_FAILED; + result = returnvalue::FAILED; } - if (result != HasReturnvaluesIF::RETURN_OK or not doMountSdCard) { + if (result != returnvalue::OK or not doMountSdCard) { return result; } @@ -127,7 +127,7 @@ ReturnValue_t SdCardManager::switchOffSdCard(sd::SdCard sdCard, bool doUnmountSd SdStatePair* statusPair) { std::pair active; ReturnValue_t result = getSdCardsStatus(active); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { return result; } if (doUnmountSdCard) { @@ -142,7 +142,7 @@ ReturnValue_t SdCardManager::switchOffSdCard(sd::SdCard sdCard, bool doUnmountSd if (sdCard == sd::SdCard::BOTH) { sif::warning << "SdCardManager::switchOffSdCard: API does not allow sd::SdStatus::BOTH" << std::endl; - return HasReturnvaluesIF::RETURN_FAILED; + return returnvalue::FAILED; } if (sdCard == sd::SdCard::SLOT_0) { if (active.first == sd::SdState::OFF) { @@ -156,7 +156,7 @@ ReturnValue_t SdCardManager::switchOffSdCard(sd::SdCard sdCard, bool doUnmountSd if (doUnmountSdCard) { result = unmountSdCard(sdCard); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { return result; } } @@ -187,7 +187,7 @@ ReturnValue_t SdCardManager::setSdCardState(sd::SdCard sdCard, bool on) { command << "q7hw sd set " << sdstring << " " << statestring; cmdExecutor.load(command.str(), blocking, printCmdOutput); ReturnValue_t result = cmdExecutor.execute(); - if (blocking and result != HasReturnvaluesIF::RETURN_OK) { + if (blocking and result != returnvalue::OK) { utility::handleSystemError(cmdExecutor.getLastError(), "SdCardManager::setSdCardState"); } return result; @@ -212,7 +212,7 @@ ReturnValue_t SdCardManager::getSdCardsStatus(SdStatePair& active) { while (std::getline(sdStatus, line)) { processSdStatusLine(active, line, idx, currentSd); } - return HasReturnvaluesIF::RETURN_OK; + return returnvalue::OK; } ReturnValue_t SdCardManager::mountSdCard(sd::SdCard sdCard) { @@ -223,7 +223,7 @@ ReturnValue_t SdCardManager::mountSdCard(sd::SdCard sdCard) { if (sdCard == sd::SdCard::BOTH) { sif::warning << "SdCardManager::mountSdCard: API does not allow sd::SdStatus::BOTH" << std::endl; - return HasReturnvaluesIF::RETURN_FAILED; + return returnvalue::FAILED; } string mountDev; string mountPoint; @@ -247,7 +247,7 @@ ReturnValue_t SdCardManager::mountSdCard(sd::SdCard sdCard) { string sdMountCommand = "mount " + mountDev + " " + mountPoint; cmdExecutor.load(sdMountCommand, blocking, printCmdOutput); ReturnValue_t result = cmdExecutor.execute(); - if (blocking and result != HasReturnvaluesIF::RETURN_OK) { + if (blocking and result != returnvalue::OK) { utility::handleSystemError(cmdExecutor.getLastError(), "SdCardManager::mountSdCard"); } return result; @@ -261,7 +261,7 @@ ReturnValue_t SdCardManager::unmountSdCard(sd::SdCard sdCard) { if (sdCard == sd::SdCard::BOTH) { sif::warning << "SdCardManager::unmountSdCard: API does not allow sd::SdStatus::BOTH" << std::endl; - return HasReturnvaluesIF::RETURN_FAILED; + return returnvalue::FAILED; } string mountPoint; if (sdCard == sd::SdCard::SLOT_0) { @@ -285,7 +285,7 @@ ReturnValue_t SdCardManager::unmountSdCard(sd::SdCard sdCard) { } cmdExecutor.load(sdUnmountCommand, blocking, printCmdOutput); ReturnValue_t result = cmdExecutor.execute(); - if (blocking and result != HasReturnvaluesIF::RETURN_OK) { + if (blocking and result != returnvalue::OK) { utility::handleSystemError(cmdExecutor.getLastError(), "SdCardManager::unmountSdCard"); } return result; @@ -293,7 +293,7 @@ ReturnValue_t SdCardManager::unmountSdCard(sd::SdCard sdCard) { ReturnValue_t SdCardManager::sanitizeState(SdStatePair* statusPair, sd::SdCard prefSdCard) { std::unique_ptr sdStatusPtr; - ReturnValue_t result = HasReturnvaluesIF::RETURN_OK; + ReturnValue_t result = returnvalue::OK; // Enforce blocking operation for now. Be careful to reset it when returning prematurely! bool resetNonBlockingState = false; if (not this->blocking) { @@ -302,7 +302,7 @@ ReturnValue_t SdCardManager::sanitizeState(SdStatePair* statusPair, sd::SdCard p } if (prefSdCard == sd::SdCard::NONE) { result = getPreferredSdCard(); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { } } if (statusPair == nullptr) { @@ -382,7 +382,7 @@ void SdCardManager::processSdStatusLine(std::pair& act sd::SdCard SdCardManager::getPreferredSdCard() const { MutexGuard mg(mutex); auto res = mg.getLockResult(); - if (res != RETURN_OK) { + if (res != returnvalue::OK) { sif::error << "SdCardManager::getPreferredSdCard: Lock error" << std::endl; } return sdInfo.pref; @@ -391,7 +391,7 @@ sd::SdCard SdCardManager::getPreferredSdCard() const { ReturnValue_t SdCardManager::setPreferredSdCard(sd::SdCard sdCard) { MutexGuard mg(mutex); if (sdCard == sd::SdCard::BOTH) { - return HasReturnvaluesIF::RETURN_FAILED; + return returnvalue::FAILED; } sdInfo.pref = sdCard; return scratch::writeNumber(scratch::PREFERED_SDC_KEY, static_cast(sdCard)); @@ -406,7 +406,7 @@ ReturnValue_t SdCardManager::updateSdCardStateFile() { std::string updateCmd = "q7hw sd info all > " + std::string(SD_STATE_FILE); cmdExecutor.load(updateCmd, blocking, printCmdOutput); ReturnValue_t result = cmdExecutor.execute(); - if (blocking and result != HasReturnvaluesIF::RETURN_OK) { + if (blocking and result != returnvalue::OK) { utility::handleSystemError(cmdExecutor.getLastError(), "SdCardManager::mountSdCard"); } return result; @@ -448,10 +448,10 @@ SdCardManager::OpStatus SdCardManager::checkCurrentOp(Operations& currentOp) { case (CommandExecutor::EXECUTION_FINISHED): { return OpStatus::SUCCESS; } - case (HasReturnvaluesIF::RETURN_OK): { + case (returnvalue::OK): { return OpStatus::ONGOING; } - case (HasReturnvaluesIF::RETURN_FAILED): { + case (returnvalue::FAILED): { return OpStatus::FAIL; } default: { @@ -469,7 +469,7 @@ bool SdCardManager::isSdCardMounted(sd::SdCard sdCard) { SdCardManager::SdStatePair active; ReturnValue_t result = this->getSdCardsStatus(active); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { sif::debug << "SdCardManager::isSdCardMounted: Failed to get SD card active state"; return false; } @@ -499,15 +499,15 @@ ReturnValue_t SdCardManager::isSdCardMountedReadOnly(sd::SdCard sdcard, bool& re command << "grep -q '" << SD_1_MOUNT_POINT << " vfat ro,' /proc/mounts"; } ReturnValue_t result = cmdExecutor.load(command.str(), true, false); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { return result; } result = cmdExecutor.execute(); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { int exitStatus = cmdExecutor.getLastError(); if (exitStatus == 1) { readOnly = false; - return RETURN_OK; + return returnvalue::OK; } return result; } @@ -517,7 +517,7 @@ ReturnValue_t SdCardManager::isSdCardMountedReadOnly(sd::SdCard sdcard, bool& re readOnly = false; } readOnly = true; - return HasReturnvaluesIF::RETURN_OK; + return returnvalue::OK; } ReturnValue_t SdCardManager::remountReadWrite(sd::SdCard sdcard) { @@ -528,7 +528,7 @@ ReturnValue_t SdCardManager::remountReadWrite(sd::SdCard sdcard) { command << "mount -o remount,rw " << SD_1_DEV_NAME << " " << SD_1_MOUNT_POINT; } ReturnValue_t result = cmdExecutor.load(command.str(), true, false); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { return result; } return cmdExecutor.execute(); @@ -542,11 +542,11 @@ ReturnValue_t SdCardManager::performFsck(sd::SdCard sdcard, bool printOutput, in command << "fsck -y " << SD_1_DEV_NAME; } ReturnValue_t result = cmdExecutor.load(command.str(), true, printOutput); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { return result; } result = cmdExecutor.execute(); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { linuxError = cmdExecutor.getLastError(); } return result; diff --git a/bsp_q7s/memory/SdCardManager.h b/bsp_q7s/memory/SdCardManager.h index 84d2d97b..796c7ba2 100644 --- a/bsp_q7s/memory/SdCardManager.h +++ b/bsp_q7s/memory/SdCardManager.h @@ -12,7 +12,7 @@ #include "events/subsystemIdRanges.h" #include "fsfw/events/Event.h" -#include "fsfw/returnvalues/HasReturnvaluesIF.h" +#include "fsfw/returnvalues/returnvalue.h" #include "fsfw_hal/linux/CommandExecutor.h" #include "mission/memory/SdCardMountedIF.h" #include "mission/memory/definitions.h" @@ -24,7 +24,7 @@ class MutexIF; * @brief Manages handling of SD cards like switching them on or off or getting the current * state */ -class SdCardManager : public SystemObject, public HasReturnvaluesIF, public SdCardMountedIF { +class SdCardManager : public SystemObject, public SdCardMountedIF { friend class SdCardAccess; public: @@ -44,22 +44,22 @@ class SdCardManager : public SystemObject, public HasReturnvaluesIF, public SdCa static constexpr uint8_t INTERFACE_ID = CLASS_ID::SD_CARD_MANAGER; - static constexpr ReturnValue_t OP_ONGOING = HasReturnvaluesIF::makeReturnCode(INTERFACE_ID, 0); - static constexpr ReturnValue_t ALREADY_ON = HasReturnvaluesIF::makeReturnCode(INTERFACE_ID, 1); + static constexpr ReturnValue_t OP_ONGOING = returnvalue::makeCode(INTERFACE_ID, 0); + static constexpr ReturnValue_t ALREADY_ON = returnvalue::makeCode(INTERFACE_ID, 1); static constexpr ReturnValue_t ALREADY_MOUNTED = - HasReturnvaluesIF::makeReturnCode(INTERFACE_ID, 2); - static constexpr ReturnValue_t ALREADY_OFF = HasReturnvaluesIF::makeReturnCode(INTERFACE_ID, 3); + returnvalue::makeCode(INTERFACE_ID, 2); + static constexpr ReturnValue_t ALREADY_OFF = returnvalue::makeCode(INTERFACE_ID, 3); static constexpr ReturnValue_t STATUS_FILE_NEXISTS = - HasReturnvaluesIF::makeReturnCode(INTERFACE_ID, 10); + returnvalue::makeCode(INTERFACE_ID, 10); static constexpr ReturnValue_t STATUS_FILE_FORMAT_INVALID = - HasReturnvaluesIF::makeReturnCode(INTERFACE_ID, 11); - static constexpr ReturnValue_t MOUNT_ERROR = HasReturnvaluesIF::makeReturnCode(INTERFACE_ID, 12); + returnvalue::makeCode(INTERFACE_ID, 11); + static constexpr ReturnValue_t MOUNT_ERROR = returnvalue::makeCode(INTERFACE_ID, 12); static constexpr ReturnValue_t UNMOUNT_ERROR = - HasReturnvaluesIF::makeReturnCode(INTERFACE_ID, 13); + returnvalue::makeCode(INTERFACE_ID, 13); static constexpr ReturnValue_t SYSTEM_CALL_ERROR = - HasReturnvaluesIF::makeReturnCode(INTERFACE_ID, 14); + returnvalue::makeCode(INTERFACE_ID, 14); static constexpr ReturnValue_t POPEN_CALL_ERROR = - HasReturnvaluesIF::makeReturnCode(INTERFACE_ID, 15); + returnvalue::makeCode(INTERFACE_ID, 15); static constexpr uint8_t SUBSYSTEM_ID = SUBSYSTEM_ID::FILE_SYSTEM; @@ -105,7 +105,7 @@ class SdCardManager : public SystemObject, public HasReturnvaluesIF, public SdCa * @param doMountSdCard Mount the SD card after switching it on, which is necessary * to use it * @param statusPair If the status pair is already available, it can be passed here - * @return - RETURN_OK on success, ALREADY_ON if it is already on, + * @return - returnvalue::OK on success, ALREADY_ON if it is already on, * SYSTEM_CALL_ERROR on system error */ ReturnValue_t switchOnSdCard(sd::SdCard sdCard, bool doMountSdCard = true, @@ -117,7 +117,7 @@ class SdCardManager : public SystemObject, public HasReturnvaluesIF, public SdCa * @param doUnmountSdCard Unmount the SD card before switching the card off, which makes * the operation safer * @param statusPair If the status pair is already available, it can be passed here - * @return - RETURN_OK on success, ALREADY_ON if it is already on, + * @return - returnvalue::OK on success, ALREADY_ON if it is already on, * SYSTEM_CALL_ERROR on system error */ ReturnValue_t switchOffSdCard(sd::SdCard sdCard, bool doUnmountSdCard = true, @@ -127,9 +127,9 @@ class SdCardManager : public SystemObject, public HasReturnvaluesIF, public SdCa * Update the state file or creates one if it does not exist. You need to call this * function before calling #sdCardActive * @return - * - RETURN_OK if the state file was updated successfully + * - returnvalue::OK if the state file was updated successfully * - CommandExecutor::COMMAND_PENDING: Non-blocking command is pending - * - RETURN_FAILED: blocking command failed + * - returnvalue::FAILED: blocking command failed */ ReturnValue_t updateSdCardStateFile(); @@ -139,7 +139,7 @@ class SdCardManager : public SystemObject, public HasReturnvaluesIF, public SdCa * the status of the SD cards and set the field of the provided boolean pair. * @param active Pair of booleans, where the first entry is the state of the first SD card * and the second one the state of the second SD card - * @return - RETURN_OK if the state was read successfully + * @return - returnvalue::OK if the state was read successfully * - STATUS_FILE_FORMAT_INVALID if there was an issue with the state file. The user * should call #updateSdCardStateFile again in that case * - STATUS_FILE_NEXISTS if the status file does not exist diff --git a/bsp_q7s/memory/scratchApi.cpp b/bsp_q7s/memory/scratchApi.cpp index 83bc8239..78f93541 100644 --- a/bsp_q7s/memory/scratchApi.cpp +++ b/bsp_q7s/memory/scratchApi.cpp @@ -6,23 +6,23 @@ ReturnValue_t scratch::writeString(std::string name, std::string string) { int result = std::system(oss.str().c_str()); if (result != 0) { utility::handleSystemError(result, "scratch::writeString"); - return HasReturnvaluesIF::RETURN_FAILED; + return returnvalue::FAILED; } - return HasReturnvaluesIF::RETURN_OK; + return returnvalue::OK; } ReturnValue_t scratch::readString(std::string key, std::string &string) { std::ifstream file; std::string filename; ReturnValue_t result = readToFile(key, file, filename); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { return result; } std::string line; if (not std::getline(file, line)) { std::remove(filename.c_str()); - return HasReturnvaluesIF::RETURN_FAILED; + return returnvalue::FAILED; } size_t pos = line.find("="); @@ -35,7 +35,7 @@ ReturnValue_t scratch::readString(std::string key, std::string &string) { return KEY_NOT_FOUND; } string = line.substr(pos + 1); - return HasReturnvaluesIF::RETURN_OK; + return returnvalue::OK; } ReturnValue_t scratch::clearValue(std::string key) { @@ -44,7 +44,7 @@ ReturnValue_t scratch::clearValue(std::string key) { int result = std::system(oss.str().c_str()); if (result != 0) { utility::handleSystemError(result, "scratch::clearValue"); - return HasReturnvaluesIF::RETURN_FAILED; + return returnvalue::FAILED; } - return HasReturnvaluesIF::RETURN_OK; + return returnvalue::OK; } diff --git a/bsp_q7s/memory/scratchApi.h b/bsp_q7s/memory/scratchApi.h index cd76fca1..a6b99fa0 100644 --- a/bsp_q7s/memory/scratchApi.h +++ b/bsp_q7s/memory/scratchApi.h @@ -7,7 +7,7 @@ #include #include -#include "fsfw/returnvalues/HasReturnvaluesIF.h" +#include "fsfw/returnvalues/returnvalue.h" #include "fsfw/serviceinterface/ServiceInterface.h" #include "linux/utility/utility.h" #include "returnvalues/classIds.h" @@ -21,7 +21,7 @@ static constexpr char PREFERED_SDC_KEY[] = "PREFSD"; static constexpr char ALLOC_FAILURE_COUNT[] = "ALLOCERR"; static constexpr uint8_t INTERFACE_ID = CLASS_ID::SCRATCH_BUFFER; -static constexpr ReturnValue_t KEY_NOT_FOUND = HasReturnvaluesIF::makeReturnCode(INTERFACE_ID, 0); +static constexpr ReturnValue_t KEY_NOT_FOUND = returnvalue::makeCode(INTERFACE_ID, 0); ReturnValue_t clearValue(std::string key); @@ -83,11 +83,11 @@ ReturnValue_t readToFile(std::string name, std::ifstream& file, std::string& fil } else { utility::handleSystemError(result, "scratch::readToFile"); std::remove(filename.c_str()); - return HasReturnvaluesIF::RETURN_FAILED; + return returnvalue::FAILED; } } file.open(filename); - return HasReturnvaluesIF::RETURN_OK; + return returnvalue::OK; } } // End of anonymous namespace @@ -99,9 +99,9 @@ inline ReturnValue_t writeNumber(std::string key, T num) noexcept { int result = std::system(oss.str().c_str()); if (result != 0) { utility::handleSystemError(result, "scratch::writeNumber"); - return HasReturnvaluesIF::RETURN_FAILED; + return returnvalue::FAILED; } - return HasReturnvaluesIF::RETURN_OK; + return returnvalue::OK; } template ::value>::type> @@ -110,7 +110,7 @@ inline ReturnValue_t readNumber(std::string key, T& num) noexcept { ifstream file; std::string filename; ReturnValue_t result = readToFile(key, file, filename); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { std::remove(filename.c_str()); return result; } @@ -118,7 +118,7 @@ inline ReturnValue_t readNumber(std::string key, T& num) noexcept { string line; if (not std::getline(file, line)) { std::remove(filename.c_str()); - return HasReturnvaluesIF::RETURN_FAILED; + return returnvalue::FAILED; } size_t pos = line.find("="); @@ -138,7 +138,7 @@ inline ReturnValue_t readNumber(std::string key, T& num) noexcept { } std::remove(filename.c_str()); - return HasReturnvaluesIF::RETURN_OK; + return returnvalue::OK; } } // namespace scratch diff --git a/bsp_q7s/xadc/Xadc.cpp b/bsp_q7s/xadc/Xadc.cpp index e1f1a505..34a4e159 100644 --- a/bsp_q7s/xadc/Xadc.cpp +++ b/bsp_q7s/xadc/Xadc.cpp @@ -12,20 +12,20 @@ Xadc::Xadc() {} Xadc::~Xadc() {} ReturnValue_t Xadc::getTemperature(float& temperature) { - ReturnValue_t result = HasReturnvaluesIF::RETURN_OK; + ReturnValue_t result = returnvalue::OK; int raw = 0; int offset = 0; float scale = 0; result = readValFromFile(xadc::file::tempRaw.c_str(), raw); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { return result; } result = readValFromFile(xadc::file::tempOffset.c_str(), offset); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { return result; } result = readValFromFile(xadc::file::tempScale.c_str(), scale); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { return result; } temperature = (raw + offset) * scale / 1000; @@ -35,84 +35,84 @@ ReturnValue_t Xadc::getTemperature(float& temperature) { ReturnValue_t Xadc::getVccPint(float& vccPint) { ReturnValue_t result = readVoltageFromSysfs(xadc::file::vccpintRaw, xadc::file::vccpintScale, vccPint); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { return result; } - return HasReturnvaluesIF::RETURN_OK; + return returnvalue::OK; } ReturnValue_t Xadc::getVccPaux(float& vccPaux) { ReturnValue_t result = readVoltageFromSysfs(xadc::file::vccpauxRaw, xadc::file::vccpauxScale, vccPaux); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { return result; } - return HasReturnvaluesIF::RETURN_OK; + return returnvalue::OK; } ReturnValue_t Xadc::getVccInt(float& vccInt) { ReturnValue_t result = readVoltageFromSysfs(xadc::file::vccintRaw, xadc::file::vccintScale, vccInt); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { return result; } - return HasReturnvaluesIF::RETURN_OK; + return returnvalue::OK; } ReturnValue_t Xadc::getVccAux(float& vccAux) { ReturnValue_t result = readVoltageFromSysfs(xadc::file::vccauxRaw, xadc::file::vccauxScale, vccAux); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { return result; } - return HasReturnvaluesIF::RETURN_OK; + return returnvalue::OK; } ReturnValue_t Xadc::getVccBram(float& vccBram) { ReturnValue_t result = readVoltageFromSysfs(xadc::file::vccbramRaw, xadc::file::vccbramScale, vccBram); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { return result; } - return HasReturnvaluesIF::RETURN_OK; + return returnvalue::OK; } ReturnValue_t Xadc::getVccOddr(float& vccOddr) { ReturnValue_t result = readVoltageFromSysfs(xadc::file::vccoddrRaw, xadc::file::vccoddrScale, vccOddr); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { return result; } - return HasReturnvaluesIF::RETURN_OK; + return returnvalue::OK; } ReturnValue_t Xadc::getVrefp(float& vrefp) { ReturnValue_t result = readVoltageFromSysfs(xadc::file::vrefpRaw, xadc::file::vrefpScale, vrefp); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { return result; } - return HasReturnvaluesIF::RETURN_OK; + return returnvalue::OK; } ReturnValue_t Xadc::getVrefn(float& vrefn) { ReturnValue_t result = readVoltageFromSysfs(xadc::file::vrefnRaw, xadc::file::vrefnScale, vrefn); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { return result; } - return HasReturnvaluesIF::RETURN_OK; + return returnvalue::OK; } ReturnValue_t Xadc::readVoltageFromSysfs(std::string rawFile, std::string scaleFile, float& voltage) { - ReturnValue_t result = HasReturnvaluesIF::RETURN_OK; + ReturnValue_t result = returnvalue::OK; float raw = 0; float scale = 0; result = readValFromFile(rawFile.c_str(), raw); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { return result; } result = readValFromFile(scaleFile.c_str(), scale); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { return result; } voltage = calculateVoltage(raw, scale); @@ -127,7 +127,7 @@ ReturnValue_t Xadc::readValFromFile(const char* filename, T& val) { fp = fopen(filename, "r"); if (fp == nullptr) { sif::warning << "Xadc::readValFromFile: Failed to open file " << filename << std::endl; - return HasReturnvaluesIF::RETURN_FAILED; + return returnvalue::FAILED; } char valstring[MAX_STR_LENGTH] = ""; char* returnVal = fgets(valstring, MAX_STR_LENGTH, fp); @@ -135,10 +135,10 @@ ReturnValue_t Xadc::readValFromFile(const char* filename, T& val) { sif::warning << "Xadc::readValFromFile: Failed to read string from file " << filename << std::endl; fclose(fp); - return HasReturnvaluesIF::RETURN_FAILED; + return returnvalue::FAILED; } std::istringstream valSstream(valstring); valSstream >> val; fclose(fp); - return HasReturnvaluesIF::RETURN_OK; + return returnvalue::OK; } diff --git a/bsp_q7s/xadc/Xadc.h b/bsp_q7s/xadc/Xadc.h index 92ec2c0c..be6ab704 100644 --- a/bsp_q7s/xadc/Xadc.h +++ b/bsp_q7s/xadc/Xadc.h @@ -3,7 +3,7 @@ #include -#include "fsfw/returnvalues/HasReturnvaluesIF.h" +#include "fsfw/returnvalues/returnvalue.h" namespace xadc { using namespace std; diff --git a/bsp_te0720_1cfa/InitMission.cpp b/bsp_te0720_1cfa/InitMission.cpp index dd6a3aa0..b7c6b683 100644 --- a/bsp_te0720_1cfa/InitMission.cpp +++ b/bsp_te0720_1cfa/InitMission.cpp @@ -2,7 +2,7 @@ #include #include -#include +#include #include #include #include @@ -36,7 +36,7 @@ void initmission::initMission() { void initmission::initTasks() { TaskFactory* factory = TaskFactory::instance(); - ReturnValue_t result = HasReturnvaluesIF::RETURN_OK; + ReturnValue_t result = returnvalue::OK; if (factory == nullptr) { /* Should never happen ! */ return; @@ -51,28 +51,28 @@ void initmission::initTasks() { PeriodicTaskIF* tmtcDistributor = factory->createPeriodicTask( "DIST", 40, PeriodicTaskIF::MINIMUM_STACK_SIZE, 0.2, missedDeadlineFunc); result = tmtcDistributor->addComponent(objects::CCSDS_PACKET_DISTRIBUTOR); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { sif::error << "Object add component failed" << std::endl; } result = tmtcDistributor->addComponent(objects::PUS_PACKET_DISTRIBUTOR); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { sif::error << "Object add component failed" << std::endl; } result = tmtcDistributor->addComponent(objects::TM_FUNNEL); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { sif::error << "Object add component failed" << std::endl; } PeriodicTaskIF* tmtcBridgeTask = factory->createPeriodicTask( "TMTC_BRIDGE", 50, PeriodicTaskIF::MINIMUM_STACK_SIZE, 0.2, missedDeadlineFunc); result = tmtcBridgeTask->addComponent(objects::TMTC_BRIDGE); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { sif::error << "Add component TMTC Bridge failed" << std::endl; } PeriodicTaskIF* tmtcPollingTask = factory->createPeriodicTask( "TMTC_POLLING", 80, PeriodicTaskIF::MINIMUM_STACK_SIZE, 2.0, missedDeadlineFunc); result = tmtcPollingTask->addComponent(objects::TMTC_POLLING_TASK); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { sif::error << "Add component TMTC Polling failed" << std::endl; } @@ -84,7 +84,7 @@ void initmission::initTasks() { FixedTimeslotTaskIF* pst = factory->createFixedTimeslotTask( "UART_PST", 70, PeriodicTaskIF::MINIMUM_STACK_SIZE * 4, 1.0, missedDeadlineFunc); result = pst::pstUart(pst); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { sif::error << "InitMission::initTasks: Creating PST failed!" << std::endl; } pstTasks.push_back(pst); @@ -93,7 +93,7 @@ void initmission::initTasks() { PeriodicTaskIF* mpsocHelperTask = factory->createPeriodicTask( "PLOC_MPSOC_HELPER", 20, PeriodicTaskIF::MINIMUM_STACK_SIZE, 0.2, missedDeadlineFunc); result = mpsocHelperTask->addComponent(objects::PLOC_MPSOC_HELPER); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { initmission::printAddObjectError("PLOC_MPSOC_HELPER", objects::PLOC_MPSOC_HELPER); } #endif /* OBSW_ADD_PLOC_MPSOC == 1*/ @@ -102,7 +102,7 @@ void initmission::initTasks() { PeriodicTaskIF* supvHelperTask = factory->createPeriodicTask( "PLOC_SUPV_HELPER", 20, PeriodicTaskIF::MINIMUM_STACK_SIZE, 0.2, missedDeadlineFunc); result = supvHelperTask->addComponent(objects::PLOC_SUPERVISOR_HELPER); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { initmission::printAddObjectError("PLOC_SUPV_HELPER", objects::PLOC_SUPERVISOR_HELPER); } #endif /* OBSW_ADD_PLOC_SUPERVISOR == 1 */ @@ -111,7 +111,7 @@ void initmission::initTasks() { PeriodicTaskIF* ccsdsHandlerTask = factory->createPeriodicTask( "CCSDS_HANDLER", 50, PeriodicTaskIF::MINIMUM_STACK_SIZE, 2.0, missedDeadlineFunc); result = ccsdsHandlerTask->addComponent(objects::CCSDS_HANDLER); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { initmission::printAddObjectError("CCSDS Handler", objects::CCSDS_HANDLER); } @@ -121,7 +121,7 @@ void initmission::initTasks() { PeriodicTaskIF* pdecHandlerTask = factory->createPeriodicTask( "PDEC_HANDLER", 50, PeriodicTaskIF::MINIMUM_STACK_SIZE, 1.0, missedDeadlineFunc); result = pdecHandlerTask->addComponent(objects::PDEC_HANDLER); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { initmission::printAddObjectError("PDEC Handler", objects::PDEC_HANDLER); } #endif /* OBSW_USE_CCSDS_IP_CORE == 1 */ @@ -160,11 +160,11 @@ void initmission::initTasks() { void initmission::createPusTasks(TaskFactory& factory, TaskDeadlineMissedFunction missedDeadlineFunc, std::vector& taskVec) { - ReturnValue_t result = HasReturnvaluesIF::RETURN_OK; + ReturnValue_t result = returnvalue::OK; PeriodicTaskIF* pusVerification = factory.createPeriodicTask( "PUS_VERIF", 40, PeriodicTaskIF::MINIMUM_STACK_SIZE, 0.200, missedDeadlineFunc); result = pusVerification->addComponent(objects::PUS_SERVICE_1_VERIFICATION); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { sif::error << "Object add component failed" << std::endl; } taskVec.push_back(pusVerification); @@ -172,11 +172,11 @@ void initmission::createPusTasks(TaskFactory& factory, PeriodicTaskIF* pusEvents = factory.createPeriodicTask( "PUS_EVENTS", 60, PeriodicTaskIF::MINIMUM_STACK_SIZE, 0.200, missedDeadlineFunc); result = pusEvents->addComponent(objects::PUS_SERVICE_5_EVENT_REPORTING); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { initmission::printAddObjectError("PUS_EVENTS", objects::PUS_SERVICE_5_EVENT_REPORTING); } result = pusEvents->addComponent(objects::EVENT_MANAGER); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { initmission::printAddObjectError("PUS_MGMT", objects::EVENT_MANAGER); } taskVec.push_back(pusEvents); @@ -184,11 +184,11 @@ void initmission::createPusTasks(TaskFactory& factory, PeriodicTaskIF* pusHighPrio = factory.createPeriodicTask( "PUS_HIGH_PRIO", 50, PeriodicTaskIF::MINIMUM_STACK_SIZE, 0.200, missedDeadlineFunc); result = pusHighPrio->addComponent(objects::PUS_SERVICE_2_DEVICE_ACCESS); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { initmission::printAddObjectError("PUS2", objects::PUS_SERVICE_2_DEVICE_ACCESS); } result = pusHighPrio->addComponent(objects::PUS_SERVICE_9_TIME_MGMT); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { initmission::printAddObjectError("PUS9", objects::PUS_SERVICE_9_TIME_MGMT); } taskVec.push_back(pusHighPrio); @@ -196,19 +196,19 @@ void initmission::createPusTasks(TaskFactory& factory, PeriodicTaskIF* pusMedPrio = factory.createPeriodicTask( "PUS_MED_PRIO", 40, PeriodicTaskIF::MINIMUM_STACK_SIZE, 0.8, missedDeadlineFunc); result = pusMedPrio->addComponent(objects::PUS_SERVICE_8_FUNCTION_MGMT); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { initmission::printAddObjectError("PUS8", objects::PUS_SERVICE_8_FUNCTION_MGMT); } result = pusMedPrio->addComponent(objects::PUS_SERVICE_200_MODE_MGMT); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { initmission::printAddObjectError("PUS200", objects::PUS_SERVICE_200_MODE_MGMT); } result = pusMedPrio->addComponent(objects::PUS_SERVICE_20_PARAMETERS); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { initmission::printAddObjectError("PUS20", objects::PUS_SERVICE_20_PARAMETERS); } result = pusMedPrio->addComponent(objects::PUS_SERVICE_3_HOUSEKEEPING); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { initmission::printAddObjectError("PUS3", objects::PUS_SERVICE_3_HOUSEKEEPING); } taskVec.push_back(pusMedPrio); @@ -216,11 +216,11 @@ void initmission::createPusTasks(TaskFactory& factory, PeriodicTaskIF* pusLowPrio = factory.createPeriodicTask( "PUS_LOW_PRIO", 30, PeriodicTaskIF::MINIMUM_STACK_SIZE, 1.6, missedDeadlineFunc); result = pusLowPrio->addComponent(objects::PUS_SERVICE_17_TEST); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { initmission::printAddObjectError("PUS17", objects::PUS_SERVICE_17_TEST); } result = pusLowPrio->addComponent(objects::INTERNAL_ERROR_REPORTER); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { initmission::printAddObjectError("INT_ERR_RPRT", objects::INTERNAL_ERROR_REPORTER); } taskVec.push_back(pusLowPrio); diff --git a/dummies/AcuDummy.cpp b/dummies/AcuDummy.cpp index dc0c974d..b1ad6953 100644 --- a/dummies/AcuDummy.cpp +++ b/dummies/AcuDummy.cpp @@ -19,16 +19,16 @@ ReturnValue_t AcuDummy::buildTransitionDeviceCommand(DeviceCommandId_t *id) { ReturnValue_t AcuDummy::buildCommandFromCommand(DeviceCommandId_t deviceCommand, const uint8_t *commandData, size_t commandDataLen) { - return RETURN_OK; + return returnvalue::OK; } ReturnValue_t AcuDummy::scanForReply(const uint8_t *start, size_t len, DeviceCommandId_t *foundId, size_t *foundLen) { - return RETURN_OK; + return returnvalue::OK; } ReturnValue_t AcuDummy::interpretDeviceReply(DeviceCommandId_t id, const uint8_t *packet) { - return RETURN_OK; + return returnvalue::OK; } void AcuDummy::fillCommandAndReplyMap() {} @@ -38,5 +38,5 @@ uint32_t AcuDummy::getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) { return ReturnValue_t AcuDummy::initializeLocalDataPool(localpool::DataPool &localDataPoolMap, LocalDataPoolManager &poolManager) { localDataPoolMap.emplace(P60System::pool::ACU_TEMPERATURES, new PoolEntry(3)); - return HasReturnvaluesIF::RETURN_OK; + return returnvalue::OK; } diff --git a/dummies/BpxDummy.cpp b/dummies/BpxDummy.cpp index 974a6a30..525c5c46 100644 --- a/dummies/BpxDummy.cpp +++ b/dummies/BpxDummy.cpp @@ -19,16 +19,16 @@ ReturnValue_t BpxDummy::buildTransitionDeviceCommand(DeviceCommandId_t *id) { ReturnValue_t BpxDummy::buildCommandFromCommand(DeviceCommandId_t deviceCommand, const uint8_t *commandData, size_t commandDataLen) { - return RETURN_OK; + return returnvalue::OK; } ReturnValue_t BpxDummy::scanForReply(const uint8_t *start, size_t len, DeviceCommandId_t *foundId, size_t *foundLen) { - return RETURN_OK; + return returnvalue::OK; } ReturnValue_t BpxDummy::interpretDeviceReply(DeviceCommandId_t id, const uint8_t *packet) { - return RETURN_OK; + return returnvalue::OK; } void BpxDummy::fillCommandAndReplyMap() {} @@ -51,5 +51,5 @@ ReturnValue_t BpxDummy::initializeLocalDataPool(localpool::DataPool &localDataPo localDataPoolMap.emplace(BpxBattery::BATTERY_HEATER_MODE, &battheatMode); localDataPoolMap.emplace(BpxBattery::BATTHEAT_LOW_LIMIT, &battheatLow); localDataPoolMap.emplace(BpxBattery::BATTHEAT_HIGH_LIMIT, &battheatHigh); - return HasReturnvaluesIF::RETURN_OK; + return returnvalue::OK; } diff --git a/dummies/ComIFDummy.cpp b/dummies/ComIFDummy.cpp index e1552fca..af40fd51 100644 --- a/dummies/ComIFDummy.cpp +++ b/dummies/ComIFDummy.cpp @@ -4,18 +4,18 @@ ComIFDummy::ComIFDummy(object_id_t objectId) : SystemObject(objectId) {} ComIFDummy::~ComIFDummy() {} -ReturnValue_t ComIFDummy::initializeInterface(CookieIF *cookie) { return RETURN_OK; } +ReturnValue_t ComIFDummy::initializeInterface(CookieIF *cookie) { return returnvalue::OK; } ReturnValue_t ComIFDummy::sendMessage(CookieIF *cookie, const uint8_t *sendData, size_t sendLen) { - return RETURN_OK; + return returnvalue::OK; } -ReturnValue_t ComIFDummy::getSendSuccess(CookieIF *cookie) { return RETURN_OK; } +ReturnValue_t ComIFDummy::getSendSuccess(CookieIF *cookie) { return returnvalue::OK; } ReturnValue_t ComIFDummy::requestReceiveMessage(CookieIF *cookie, size_t requestLen) { - return RETURN_OK; + return returnvalue::OK; } ReturnValue_t ComIFDummy::readReceivedMessage(CookieIF *cookie, uint8_t **buffer, size_t *size) { - return RETURN_OK; + return returnvalue::OK; } diff --git a/dummies/CoreControllerDummy.cpp b/dummies/CoreControllerDummy.cpp index b8aa270c..fd2e3f63 100644 --- a/dummies/CoreControllerDummy.cpp +++ b/dummies/CoreControllerDummy.cpp @@ -14,16 +14,16 @@ ReturnValue_t CoreControllerDummy::initialize() { if (not done) { done = true; ReturnValue_t result = ExtendedControllerBase::initialize(); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { return result; } } - return HasReturnvaluesIF::RETURN_OK; + return returnvalue::OK; } ReturnValue_t CoreControllerDummy::handleCommandMessage(CommandMessage* message) { - return RETURN_FAILED; + return returnvalue::FAILED; } void CoreControllerDummy::performControlOperation() { return; } @@ -33,7 +33,7 @@ ReturnValue_t CoreControllerDummy::initializeLocalDataPool(localpool::DataPool& localDataPoolMap.emplace(core::TEMPERATURE, new PoolEntry({0})); localDataPoolMap.emplace(core::PS_VOLTAGE, new PoolEntry({0})); localDataPoolMap.emplace(core::PL_VOLTAGE, new PoolEntry({0})); - return HasReturnvaluesIF::RETURN_OK; + return returnvalue::OK; } LocalPoolDataSetBase* CoreControllerDummy::getDataSetHandle(sid_t sid) { @@ -51,5 +51,5 @@ ReturnValue_t CoreControllerDummy::checkModeCommand(Mode_t mode, Submode_t submo if ((mode != MODE_OFF) && (mode != MODE_ON) && (mode != MODE_NORMAL)) { return INVALID_MODE; } - return RETURN_OK; + return returnvalue::OK; } diff --git a/dummies/GyroAdisDummy.cpp b/dummies/GyroAdisDummy.cpp index 6b760e79..de4e7f91 100644 --- a/dummies/GyroAdisDummy.cpp +++ b/dummies/GyroAdisDummy.cpp @@ -22,16 +22,16 @@ ReturnValue_t GyroAdisDummy::buildTransitionDeviceCommand(DeviceCommandId_t *id) ReturnValue_t GyroAdisDummy::buildCommandFromCommand(DeviceCommandId_t deviceCommand, const uint8_t *commandData, size_t commandDataLen) { - return RETURN_OK; + return returnvalue::OK; } ReturnValue_t GyroAdisDummy::scanForReply(const uint8_t *start, size_t len, DeviceCommandId_t *foundId, size_t *foundLen) { - return RETURN_OK; + return returnvalue::OK; } ReturnValue_t GyroAdisDummy::interpretDeviceReply(DeviceCommandId_t id, const uint8_t *packet) { - return RETURN_OK; + return returnvalue::OK; } void GyroAdisDummy::fillCommandAndReplyMap() {} @@ -41,5 +41,5 @@ uint32_t GyroAdisDummy::getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) { r ReturnValue_t GyroAdisDummy::initializeLocalDataPool(localpool::DataPool &localDataPoolMap, LocalDataPoolManager &poolManager) { localDataPoolMap.emplace(ADIS1650X::TEMPERATURE, new PoolEntry({0.0})); - return HasReturnvaluesIF::RETURN_OK; + return returnvalue::OK; } diff --git a/dummies/GyroL3GD20Dummy.cpp b/dummies/GyroL3GD20Dummy.cpp index 7023870f..935e32f5 100644 --- a/dummies/GyroL3GD20Dummy.cpp +++ b/dummies/GyroL3GD20Dummy.cpp @@ -22,16 +22,16 @@ ReturnValue_t GyroL3GD20Dummy::buildTransitionDeviceCommand(DeviceCommandId_t *i ReturnValue_t GyroL3GD20Dummy::buildCommandFromCommand(DeviceCommandId_t deviceCommand, const uint8_t *commandData, size_t commandDataLen) { - return RETURN_OK; + return returnvalue::OK; } ReturnValue_t GyroL3GD20Dummy::scanForReply(const uint8_t *start, size_t len, DeviceCommandId_t *foundId, size_t *foundLen) { - return RETURN_OK; + return returnvalue::OK; } ReturnValue_t GyroL3GD20Dummy::interpretDeviceReply(DeviceCommandId_t id, const uint8_t *packet) { - return RETURN_OK; + return returnvalue::OK; } void GyroL3GD20Dummy::fillCommandAndReplyMap() {} @@ -44,5 +44,5 @@ ReturnValue_t GyroL3GD20Dummy::initializeLocalDataPool(localpool::DataPool &loca localDataPoolMap.emplace(L3GD20H::ANG_VELOC_Y, new PoolEntry({0.0})); localDataPoolMap.emplace(L3GD20H::ANG_VELOC_Z, new PoolEntry({0.0})); localDataPoolMap.emplace(L3GD20H::TEMPERATURE, new PoolEntry({0.0})); - return HasReturnvaluesIF::RETURN_OK; + return returnvalue::OK; } diff --git a/dummies/ImtqDummy.cpp b/dummies/ImtqDummy.cpp index 0af9ef83..6dc451b3 100644 --- a/dummies/ImtqDummy.cpp +++ b/dummies/ImtqDummy.cpp @@ -20,16 +20,16 @@ ReturnValue_t ImtqDummy::buildTransitionDeviceCommand(DeviceCommandId_t *id) { ReturnValue_t ImtqDummy::buildCommandFromCommand(DeviceCommandId_t deviceCommand, const uint8_t *commandData, size_t commandDataLen) { - return RETURN_OK; + return returnvalue::OK; } ReturnValue_t ImtqDummy::scanForReply(const uint8_t *start, size_t len, DeviceCommandId_t *foundId, size_t *foundLen) { - return RETURN_OK; + return returnvalue::OK; } ReturnValue_t ImtqDummy::interpretDeviceReply(DeviceCommandId_t id, const uint8_t *packet) { - return RETURN_OK; + return returnvalue::OK; } void ImtqDummy::fillCommandAndReplyMap() {} @@ -39,5 +39,5 @@ uint32_t ImtqDummy::getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) { retur ReturnValue_t ImtqDummy::initializeLocalDataPool(localpool::DataPool &localDataPoolMap, LocalDataPoolManager &poolManager) { localDataPoolMap.emplace(IMTQ::MCU_TEMPERATURE, new PoolEntry({0})); - return HasReturnvaluesIF::RETURN_OK; + return returnvalue::OK; } diff --git a/dummies/MgmLIS3MDLDummy.cpp b/dummies/MgmLIS3MDLDummy.cpp index 0fb0edba..ea84c4b2 100644 --- a/dummies/MgmLIS3MDLDummy.cpp +++ b/dummies/MgmLIS3MDLDummy.cpp @@ -22,16 +22,16 @@ ReturnValue_t MgmLIS3MDLDummy::buildTransitionDeviceCommand(DeviceCommandId_t *i ReturnValue_t MgmLIS3MDLDummy::buildCommandFromCommand(DeviceCommandId_t deviceCommand, const uint8_t *commandData, size_t commandDataLen) { - return RETURN_OK; + return returnvalue::OK; } ReturnValue_t MgmLIS3MDLDummy::scanForReply(const uint8_t *start, size_t len, DeviceCommandId_t *foundId, size_t *foundLen) { - return RETURN_OK; + return returnvalue::OK; } ReturnValue_t MgmLIS3MDLDummy::interpretDeviceReply(DeviceCommandId_t id, const uint8_t *packet) { - return RETURN_OK; + return returnvalue::OK; } void MgmLIS3MDLDummy::fillCommandAndReplyMap() {} @@ -41,5 +41,5 @@ uint32_t MgmLIS3MDLDummy::getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) { ReturnValue_t MgmLIS3MDLDummy::initializeLocalDataPool(localpool::DataPool &localDataPoolMap, LocalDataPoolManager &poolManager) { localDataPoolMap.emplace(MGMLIS3MDL::TEMPERATURE_CELCIUS, new PoolEntry({0.0})); - return HasReturnvaluesIF::RETURN_OK; + return returnvalue::OK; } diff --git a/dummies/P60DockDummy.cpp b/dummies/P60DockDummy.cpp index 04f5e5af..32df3882 100644 --- a/dummies/P60DockDummy.cpp +++ b/dummies/P60DockDummy.cpp @@ -22,16 +22,16 @@ ReturnValue_t P60DockDummy::buildTransitionDeviceCommand(DeviceCommandId_t *id) ReturnValue_t P60DockDummy::buildCommandFromCommand(DeviceCommandId_t deviceCommand, const uint8_t *commandData, size_t commandDataLen) { - return RETURN_OK; + return returnvalue::OK; } ReturnValue_t P60DockDummy::scanForReply(const uint8_t *start, size_t len, DeviceCommandId_t *foundId, size_t *foundLen) { - return RETURN_OK; + return returnvalue::OK; } ReturnValue_t P60DockDummy::interpretDeviceReply(DeviceCommandId_t id, const uint8_t *packet) { - return RETURN_OK; + return returnvalue::OK; } void P60DockDummy::fillCommandAndReplyMap() {} @@ -42,5 +42,5 @@ ReturnValue_t P60DockDummy::initializeLocalDataPool(localpool::DataPool &localDa LocalDataPoolManager &poolManager) { localDataPoolMap.emplace(P60System::pool::P60DOCK_TEMPERATURE_1, new PoolEntry({0})); localDataPoolMap.emplace(P60System::pool::P60DOCK_TEMPERATURE_2, new PoolEntry({0})); - return HasReturnvaluesIF::RETURN_OK; + return returnvalue::OK; } diff --git a/dummies/PduDummy.cpp b/dummies/PduDummy.cpp index e955d401..a5cc9c33 100644 --- a/dummies/PduDummy.cpp +++ b/dummies/PduDummy.cpp @@ -19,16 +19,16 @@ ReturnValue_t PduDummy::buildTransitionDeviceCommand(DeviceCommandId_t *id) { ReturnValue_t PduDummy::buildCommandFromCommand(DeviceCommandId_t deviceCommand, const uint8_t *commandData, size_t commandDataLen) { - return RETURN_OK; + return returnvalue::OK; } ReturnValue_t PduDummy::scanForReply(const uint8_t *start, size_t len, DeviceCommandId_t *foundId, size_t *foundLen) { - return RETURN_OK; + return returnvalue::OK; } ReturnValue_t PduDummy::interpretDeviceReply(DeviceCommandId_t id, const uint8_t *packet) { - return RETURN_OK; + return returnvalue::OK; } void PduDummy::fillCommandAndReplyMap() {} @@ -38,5 +38,5 @@ uint32_t PduDummy::getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) { return ReturnValue_t PduDummy::initializeLocalDataPool(localpool::DataPool &localDataPoolMap, LocalDataPoolManager &poolManager) { localDataPoolMap.emplace(P60System::pool::PDU_TEMPERATURE, new PoolEntry({0})); - return HasReturnvaluesIF::RETURN_OK; + return returnvalue::OK; } diff --git a/dummies/PlPcduDummy.cpp b/dummies/PlPcduDummy.cpp index 75f98825..df4acd1b 100644 --- a/dummies/PlPcduDummy.cpp +++ b/dummies/PlPcduDummy.cpp @@ -22,16 +22,16 @@ ReturnValue_t PlPcduDummy::buildTransitionDeviceCommand(DeviceCommandId_t *id) { ReturnValue_t PlPcduDummy::buildCommandFromCommand(DeviceCommandId_t deviceCommand, const uint8_t *commandData, size_t commandDataLen) { - return RETURN_OK; + return returnvalue::OK; } ReturnValue_t PlPcduDummy::scanForReply(const uint8_t *start, size_t len, DeviceCommandId_t *foundId, size_t *foundLen) { - return RETURN_OK; + return returnvalue::OK; } ReturnValue_t PlPcduDummy::interpretDeviceReply(DeviceCommandId_t id, const uint8_t *packet) { - return RETURN_OK; + return returnvalue::OK; } void PlPcduDummy::fillCommandAndReplyMap() {} @@ -41,5 +41,5 @@ uint32_t PlPcduDummy::getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) { ret ReturnValue_t PlPcduDummy::initializeLocalDataPool(localpool::DataPool &localDataPoolMap, LocalDataPoolManager &poolManager) { localDataPoolMap.emplace(plpcdu::PlPcduPoolIds::TEMP, new PoolEntry({0.0})); - return HasReturnvaluesIF::RETURN_OK; + return returnvalue::OK; } diff --git a/dummies/RwDummy.cpp b/dummies/RwDummy.cpp index d41728eb..a21e7ab7 100644 --- a/dummies/RwDummy.cpp +++ b/dummies/RwDummy.cpp @@ -19,16 +19,16 @@ ReturnValue_t RwDummy::buildTransitionDeviceCommand(DeviceCommandId_t *id) { ReturnValue_t RwDummy::buildCommandFromCommand(DeviceCommandId_t deviceCommand, const uint8_t *commandData, size_t commandDataLen) { - return RETURN_OK; + return returnvalue::OK; } ReturnValue_t RwDummy::scanForReply(const uint8_t *start, size_t len, DeviceCommandId_t *foundId, size_t *foundLen) { - return RETURN_OK; + return returnvalue::OK; } ReturnValue_t RwDummy::interpretDeviceReply(DeviceCommandId_t id, const uint8_t *packet) { - return RETURN_OK; + return returnvalue::OK; } void RwDummy::fillCommandAndReplyMap() {} @@ -71,5 +71,5 @@ ReturnValue_t RwDummy::initializeLocalDataPool(localpool::DataPool &localDataPoo localDataPoolMap.emplace(RwDefinitions::SPI_BYTES_READ, new PoolEntry({0})); localDataPoolMap.emplace(RwDefinitions::SPI_REG_OVERRUN_ERRORS, new PoolEntry({0})); localDataPoolMap.emplace(RwDefinitions::SPI_TOTAL_ERRORS, new PoolEntry({0})); - return HasReturnvaluesIF::RETURN_OK; + return returnvalue::OK; } diff --git a/dummies/StarTrackerDummy.cpp b/dummies/StarTrackerDummy.cpp index 4432bb5e..8e2dd507 100644 --- a/dummies/StarTrackerDummy.cpp +++ b/dummies/StarTrackerDummy.cpp @@ -22,16 +22,16 @@ ReturnValue_t StarTrackerDummy::buildTransitionDeviceCommand(DeviceCommandId_t * ReturnValue_t StarTrackerDummy::buildCommandFromCommand(DeviceCommandId_t deviceCommand, const uint8_t *commandData, size_t commandDataLen) { - return RETURN_OK; + return returnvalue::OK; } ReturnValue_t StarTrackerDummy::scanForReply(const uint8_t *start, size_t len, DeviceCommandId_t *foundId, size_t *foundLen) { - return RETURN_OK; + return returnvalue::OK; } ReturnValue_t StarTrackerDummy::interpretDeviceReply(DeviceCommandId_t id, const uint8_t *packet) { - return RETURN_OK; + return returnvalue::OK; } void StarTrackerDummy::fillCommandAndReplyMap() {} @@ -41,5 +41,5 @@ uint32_t StarTrackerDummy::getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) ReturnValue_t StarTrackerDummy::initializeLocalDataPool(localpool::DataPool &localDataPoolMap, LocalDataPoolManager &poolManager) { localDataPoolMap.emplace(startracker::MCU_TEMPERATURE, new PoolEntry({0})); - return HasReturnvaluesIF::RETURN_OK; + return returnvalue::OK; } diff --git a/dummies/SusDummy.cpp b/dummies/SusDummy.cpp index 58b2ac8d..2cddd03e 100644 --- a/dummies/SusDummy.cpp +++ b/dummies/SusDummy.cpp @@ -25,15 +25,15 @@ ReturnValue_t SusDummy::initialize() { if (not done) { done = true; ReturnValue_t result = ExtendedControllerBase::initialize(); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { return result; } } - return HasReturnvaluesIF::RETURN_OK; + return returnvalue::OK; } -ReturnValue_t SusDummy::handleCommandMessage(CommandMessage* message) { return RETURN_FAILED; } +ReturnValue_t SusDummy::handleCommandMessage(CommandMessage* message) { return returnvalue::FAILED; } void SusDummy::performControlOperation() { iteration++; @@ -54,7 +54,7 @@ ReturnValue_t SusDummy::initializeLocalDataPool(localpool::DataPool& localDataPo localDataPoolMap.emplace(SUS::SusPoolIds::TEMPERATURE_C, new PoolEntry({0}, 1, true)); localDataPoolMap.emplace(SUS::SusPoolIds::CHANNEL_VEC, new PoolEntry({0})); - return RETURN_OK; + return returnvalue::OK; } LocalPoolDataSetBase* SusDummy::getDataSetHandle(sid_t sid) { @@ -74,5 +74,5 @@ ReturnValue_t SusDummy::checkModeCommand(Mode_t mode, Submode_t submode, if ((mode != MODE_OFF) && (mode != MODE_ON) && (mode != MODE_NORMAL)) { return INVALID_MODE; } - return RETURN_OK; + return returnvalue::OK; } diff --git a/dummies/SyrlinksDummy.cpp b/dummies/SyrlinksDummy.cpp index 275f194c..49c1319f 100644 --- a/dummies/SyrlinksDummy.cpp +++ b/dummies/SyrlinksDummy.cpp @@ -22,16 +22,16 @@ ReturnValue_t SyrlinksDummy::buildTransitionDeviceCommand(DeviceCommandId_t *id) ReturnValue_t SyrlinksDummy::buildCommandFromCommand(DeviceCommandId_t deviceCommand, const uint8_t *commandData, size_t commandDataLen) { - return RETURN_OK; + return returnvalue::OK; } ReturnValue_t SyrlinksDummy::scanForReply(const uint8_t *start, size_t len, DeviceCommandId_t *foundId, size_t *foundLen) { - return RETURN_OK; + return returnvalue::OK; } ReturnValue_t SyrlinksDummy::interpretDeviceReply(DeviceCommandId_t id, const uint8_t *packet) { - return RETURN_OK; + return returnvalue::OK; } void SyrlinksDummy::fillCommandAndReplyMap() {} @@ -42,5 +42,5 @@ ReturnValue_t SyrlinksDummy::initializeLocalDataPool(localpool::DataPool &localD LocalDataPoolManager &poolManager) { localDataPoolMap.emplace(syrlinks::TEMP_BASEBAND_BOARD, new PoolEntry({0})); localDataPoolMap.emplace(syrlinks::TEMP_POWER_AMPLIFIER, new PoolEntry({0})); - return HasReturnvaluesIF::RETURN_OK; + return returnvalue::OK; } diff --git a/dummies/TemperatureSensorsDummy.cpp b/dummies/TemperatureSensorsDummy.cpp index 129b20bd..49194e91 100644 --- a/dummies/TemperatureSensorsDummy.cpp +++ b/dummies/TemperatureSensorsDummy.cpp @@ -32,16 +32,16 @@ ReturnValue_t TemperatureSensorsDummy::initialize() { if (not done) { done = true; ReturnValue_t result = ExtendedControllerBase::initialize(); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { return result; } } - return HasReturnvaluesIF::RETURN_OK; + return returnvalue::OK; } ReturnValue_t TemperatureSensorsDummy::handleCommandMessage(CommandMessage* message) { - return RETURN_FAILED; + return returnvalue::FAILED; } void TemperatureSensorsDummy::performControlOperation() { @@ -49,7 +49,7 @@ void TemperatureSensorsDummy::performControlOperation() { value = sin(iteration / 80. * M_PI) * 10; ReturnValue_t result = max31865Set.read(); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { sif::warning << "Failed to read temperature from MAX31865 dataset" << std::endl; } max31865Set.rtdValue = value - 5; @@ -73,7 +73,7 @@ ReturnValue_t TemperatureSensorsDummy::initializeLocalDataPool( localDataPoolMap.emplace(static_cast(MAX31865::PoolIds::FAULT_BYTE), new PoolEntry({0})); - return RETURN_OK; + return returnvalue::OK; } LocalPoolDataSetBase* TemperatureSensorsDummy::getDataSetHandle(sid_t sid) { @@ -94,5 +94,5 @@ ReturnValue_t TemperatureSensorsDummy::checkModeCommand(Mode_t mode, Submode_t s if ((mode != MODE_OFF) && (mode != MODE_ON) && (mode != MODE_NORMAL)) { return INVALID_MODE; } - return RETURN_OK; + return returnvalue::OK; } diff --git a/fsfw b/fsfw index 7881f5ba..f5866dda 160000 --- a/fsfw +++ b/fsfw @@ -1 +1 @@ -Subproject commit 7881f5bab86212035b94f4995551d75e843174b5 +Subproject commit f5866ddacee6cd0f381fb1a69f1d0cf22b5b310a diff --git a/generators/deps/fsfwgen b/generators/deps/fsfwgen index e84be4bb..b1e5a2d4 160000 --- a/generators/deps/fsfwgen +++ b/generators/deps/fsfwgen @@ -1 +1 @@ -Subproject commit e84be4bb1710e90e97f8e501565106b9e63ef56b +Subproject commit b1e5a2d40a5f41b9020f2beb0b976035f91c6343 diff --git a/linux/ObjectFactory.cpp b/linux/ObjectFactory.cpp index 494b0baa..a979e4b2 100644 --- a/linux/ObjectFactory.cpp +++ b/linux/ObjectFactory.cpp @@ -328,7 +328,7 @@ void ObjectFactory::createThermalController() { void ObjectFactory::createAcsController() { new AcsController(objects::ACS_CONTROLLER); } void ObjectFactory::gpioChecker(ReturnValue_t result, std::string output) { - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { sif::error << "ObjectFactory: Adding GPIOs failed for " << output << std::endl; } } diff --git a/linux/ObjectFactory.h b/linux/ObjectFactory.h index c88a9eb4..56a5664b 100644 --- a/linux/ObjectFactory.h +++ b/linux/ObjectFactory.h @@ -1,6 +1,6 @@ #pragma once -#include +#include #include #include diff --git a/linux/boardtest/I2cTestClass.cpp b/linux/boardtest/I2cTestClass.cpp index 1bd0aa52..40a66125 100644 --- a/linux/boardtest/I2cTestClass.cpp +++ b/linux/boardtest/I2cTestClass.cpp @@ -17,20 +17,20 @@ ReturnValue_t I2cTestClass::initialize() { if (mode == TestModes::BPX_BATTERY) { battInit(); } - return HasReturnvaluesIF::RETURN_OK; + return returnvalue::OK; } ReturnValue_t I2cTestClass::performPeriodicAction() { if (mode == TestModes::BPX_BATTERY) { battPeriodic(); } - return HasReturnvaluesIF::RETURN_OK; + return returnvalue::OK; } void I2cTestClass::battInit() { sif::info << "I2cTestClass: BPX Initialization" << std::endl; UnixFileGuard fileHelper(i2cdev, &bpxInfo.fd, O_RDWR, "I2cTestClass::sendMessage"); - if (fileHelper.getOpenResult() != HasReturnvaluesIF::RETURN_OK) { + if (fileHelper.getOpenResult() != returnvalue::OK) { sif::error << "Opening I2C device" << i2cdev << " failed" << std::endl; return; } @@ -41,13 +41,13 @@ void I2cTestClass::battInit() { cmdBuf[1] = 0x42; sendLen = 2; ReturnValue_t result = i2cWrite(bpxInfo.fd, cmdBuf.data(), sendLen); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { return; } // Receive back port, error byte and ping reply recvLen = 3; result = i2cRead(bpxInfo.fd, replyBuf.data(), recvLen); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { return; } sif::info << "Ping reply:" << std::endl; @@ -59,7 +59,7 @@ void I2cTestClass::battInit() { void I2cTestClass::battPeriodic() { UnixFileGuard fileHelper(i2cdev, &bpxInfo.fd, O_RDWR, "I2cTestClass::sendMessage"); - if (fileHelper.getOpenResult() != HasReturnvaluesIF::RETURN_OK) { + if (fileHelper.getOpenResult() != returnvalue::OK) { sif::error << "Opening I2C device" << i2cdev << " failed" << std::endl; return; } @@ -69,13 +69,13 @@ void I2cTestClass::battPeriodic() { cmdBuf[0] = BpxBattery::PORT_GET_HK; sendLen = 1; ReturnValue_t result = i2cWrite(bpxInfo.fd, cmdBuf.data(), sendLen); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { return; } // Receive back HK set recvLen = 23; result = i2cRead(bpxInfo.fd, replyBuf.data(), recvLen); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { return; } sif::info << "HK reply:" << std::endl; @@ -86,16 +86,16 @@ ReturnValue_t I2cTestClass::i2cWrite(int fd, uint8_t* data, size_t len) { if (write(fd, data, len) != static_cast(len)) { sif::error << "Failed to write to I2C bus" << std::endl; sif::error << "Error " << errno << ": " << strerror(errno) << std::endl; - return HasReturnvaluesIF::RETURN_FAILED; + return returnvalue::FAILED; } - return HasReturnvaluesIF::RETURN_OK; + return returnvalue::OK; } ReturnValue_t I2cTestClass::i2cRead(int fd, uint8_t* data, size_t len) { if (read(fd, data, len) != static_cast(len)) { sif::error << "Failed to read from I2C bus" << std::endl; sif::error << "Error " << errno << ": " << strerror(errno) << std::endl; - return HasReturnvaluesIF::RETURN_FAILED; + return returnvalue::FAILED; } - return HasReturnvaluesIF::RETURN_OK; + return returnvalue::OK; } diff --git a/linux/boardtest/LibgpiodTest.cpp b/linux/boardtest/LibgpiodTest.cpp index 66e26e3b..a1cbdc11 100644 --- a/linux/boardtest/LibgpiodTest.cpp +++ b/linux/boardtest/LibgpiodTest.cpp @@ -25,9 +25,9 @@ ReturnValue_t LibgpiodTest::performPeriodicAction() { switch (testCase) { case (TestCases::READ): { result = gpioInterface->readGpio(gpioIds::TEST_ID_0, gpioState); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { sif::warning << "LibgpiodTest::performPeriodicAction: Failed to read gpio " << std::endl; - return RETURN_FAILED; + return returnvalue::FAILED; } else { sif::debug << "LibgpiodTest::performPeriodicAction: MIO 0 state = " << static_cast(gpioState) << std::endl; @@ -39,23 +39,23 @@ ReturnValue_t LibgpiodTest::performPeriodicAction() { } case (TestCases::BLINK): { result = gpioInterface->readGpio(gpioIds::TEST_ID_0, gpioState); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { sif::warning << "LibgpiodTest::performPeriodicAction: Failed to read gpio " << std::endl; - return RETURN_FAILED; + return returnvalue::FAILED; } if (gpioState == gpio::Levels::HIGH) { result = gpioInterface->pullLow(gpioIds::TEST_ID_0); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { sif::warning << "LibgpiodTest::performPeriodicAction: Could not pull GPIO low!" << std::endl; - return HasReturnvaluesIF::RETURN_FAILED; + return returnvalue::FAILED; } } else if (gpioState == gpio::Levels::LOW) { result = gpioInterface->pullHigh(gpioIds::TEST_ID_0); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { sif::warning << "LibgpiodTest::performPeriodicAction: Could not pull GPIO high!" << std::endl; - return HasReturnvaluesIF::RETURN_FAILED; + return returnvalue::FAILED; } } else { sif::warning << "LibgpiodTest::performPeriodicAction: Invalid GPIO state" << std::endl; @@ -68,7 +68,7 @@ ReturnValue_t LibgpiodTest::performPeriodicAction() { break; } - return RETURN_OK; + return returnvalue::OK; } ReturnValue_t LibgpiodTest::performOneShotAction() { @@ -84,44 +84,44 @@ ReturnValue_t LibgpiodTest::performOneShotAction() { } case (TestCases::LOOPBACK): { result = gpioInterface->pullHigh(gpioIds::TEST_ID_0); - if (result == HasReturnvaluesIF::RETURN_OK) { + if (result == returnvalue::OK) { sif::info << "LibgpiodTest::performOneShotAction: " "GPIO pulled high successfully for loopback test" << std::endl; } else { sif::warning << "LibgpiodTest::performOneShotAction: Could not pull GPIO high!" << std::endl; - return HasReturnvaluesIF::RETURN_OK; + return returnvalue::OK; } result = gpioInterface->readGpio(gpioIds::TEST_ID_1, gpioState); - if (result == HasReturnvaluesIF::RETURN_OK and gpioState == gpio::Levels::HIGH) { + if (result == returnvalue::OK and gpioState == gpio::Levels::HIGH) { sif::info << "LibgpiodTest::performOneShotAction: " "GPIO state read successfully and is high" << std::endl; } else { sif::warning << "LibgpiodTest::performOneShotAction: GPIO read and is not high!" << std::endl; - return HasReturnvaluesIF::RETURN_OK; + return returnvalue::OK; } result = gpioInterface->pullLow(gpioIds::TEST_ID_0); - if (result == HasReturnvaluesIF::RETURN_OK) { + if (result == returnvalue::OK) { sif::info << "LibgpiodTest::performOneShotAction: " "GPIO pulled low successfully for loopback test" << std::endl; } result = gpioInterface->readGpio(gpioIds::TEST_ID_1, gpioState); - if (result == HasReturnvaluesIF::RETURN_OK and gpioState == gpio::Levels::LOW) { + if (result == returnvalue::OK and gpioState == gpio::Levels::LOW) { sif::info << "LibgpiodTest::performOneShotAction: " "GPIO state read successfully and is low" << std::endl; } else { sif::warning << "LibgpiodTest::performOneShotAction: GPIO read and is not low!" << std::endl; - return HasReturnvaluesIF::RETURN_OK; + return returnvalue::OK; } break; } } - return HasReturnvaluesIF::RETURN_OK; + return returnvalue::OK; } diff --git a/linux/boardtest/SpiTestClass.cpp b/linux/boardtest/SpiTestClass.cpp index d9eb6b5a..dc5a5e3b 100644 --- a/linux/boardtest/SpiTestClass.cpp +++ b/linux/boardtest/SpiTestClass.cpp @@ -53,7 +53,7 @@ ReturnValue_t SpiTestClass::performOneShotAction() { break; } } - return HasReturnvaluesIF::RETURN_OK; + return returnvalue::OK; } ReturnValue_t SpiTestClass::performPeriodicAction() { @@ -65,7 +65,7 @@ ReturnValue_t SpiTestClass::performPeriodicAction() { default: break; } - return HasReturnvaluesIF::RETURN_OK; + return returnvalue::OK; } void SpiTestClass::performRm3100Test(uint8_t mgmId) { @@ -532,11 +532,11 @@ void SpiTestClass::max1227PlPcduTest(int fd) { adcCfg.vbatSwitch) { // This enables the ADC ReturnValue_t result = gpioIF->pullHigh(gpioIds::PLPCDU_ENB_VBAT0); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { return; } result = gpioIF->pullHigh(gpioIds::PLPCDU_ENB_VBAT1); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { return; } adcCfg.vbatSwitch = false; @@ -878,10 +878,10 @@ uint8_t SpiTestClass::readRegister(int fd, gpioId_t chipSelect, uint8_t reg) { ReturnValue_t SpiTestClass::transfer(int fd, gpioId_t chipSelect = gpio::NO_GPIO) { int retval = 0; - ReturnValue_t result = HasReturnvaluesIF::RETURN_OK; + ReturnValue_t result = returnvalue::OK; if (chipSelect != gpio::NO_GPIO) { result = gpioIF->pullLow(chipSelect); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { return result; } } @@ -889,14 +889,14 @@ ReturnValue_t SpiTestClass::transfer(int fd, gpioId_t chipSelect = gpio::NO_GPIO retval = ioctl(fd, SPI_IOC_MESSAGE(1), &spiTransferStruct); if (retval < 0) { utility::handleIoctlError("SpiTestClass::transfer: ioctl failed"); - return HasReturnvaluesIF::RETURN_FAILED; + return returnvalue::FAILED; } if (chipSelect != gpio::NO_GPIO) { result = gpioIF->pullHigh(chipSelect); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { return result; } } - return HasReturnvaluesIF::RETURN_OK; + return returnvalue::OK; } diff --git a/linux/boardtest/UartTestClass.cpp b/linux/boardtest/UartTestClass.cpp index 9c51ed8a..891959da 100644 --- a/linux/boardtest/UartTestClass.cpp +++ b/linux/boardtest/UartTestClass.cpp @@ -26,10 +26,10 @@ ReturnValue_t UartTestClass::initialize() { } else if (mode == TestModes::SCEX) { scexInit(); } - return HasReturnvaluesIF::RETURN_OK; + return returnvalue::OK; } -ReturnValue_t UartTestClass::performOneShotAction() { return HasReturnvaluesIF::RETURN_OK; } +ReturnValue_t UartTestClass::performOneShotAction() { return returnvalue::OK; } ReturnValue_t UartTestClass::performPeriodicAction() { if (mode == TestModes::GPS) { @@ -37,7 +37,7 @@ ReturnValue_t UartTestClass::performPeriodicAction() { } else if (mode == TestModes::SCEX) { scexPeriodic(); } - return HasReturnvaluesIF::RETURN_OK; + return returnvalue::OK; } void UartTestClass::gpsInit() { @@ -217,7 +217,7 @@ int UartTestClass::prepareScexPing() { tmpCmdBuf[6] = crc & 0xff; ReturnValue_t result = dleEncoder.encode(tmpCmdBuf.data(), 7, cmdBuf.data(), cmdBuf.size(), &encodedLen, true); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { sif::warning << "UartTestClass::scexInit: Encoding failed" << std::endl; return -1; } diff --git a/linux/csp/CspComIF.cpp b/linux/csp/CspComIF.cpp index 9f346405..e6283e99 100644 --- a/linux/csp/CspComIF.cpp +++ b/linux/csp/CspComIF.cpp @@ -31,7 +31,7 @@ ReturnValue_t CspComIF::initializeInterface(CookieIF* cookie) { if (csp_init(cspOwnAddress) != CSP_ERR_NONE || csp_buffer_init(buf_count, buf_size) != CSP_ERR_NONE) { sif::error << "Failed to init CSP\r\n" << std::endl; - return HasReturnvaluesIF::RETURN_FAILED; + return returnvalue::FAILED; } int promisc = 0; // Set filter mode on @@ -45,7 +45,7 @@ ReturnValue_t CspComIF::initializeInterface(CookieIF* cookie) { int result = csp_rtable_set(address, netmask, csp_if_ptr, mac); if (result != CSP_ERR_NONE) { sif::error << "Failed to add can interface to router table" << std::endl; - return HasReturnvaluesIF::RETURN_FAILED; + return returnvalue::FAILED; } /* Start the route task */ @@ -54,7 +54,7 @@ ReturnValue_t CspComIF::initializeInterface(CookieIF* cookie) { result = csp_route_start_task(task_stack_size, priority); if (result != CSP_ERR_NONE) { sif::error << "Failed to start csp route task" << std::endl; - return HasReturnvaluesIF::RETURN_FAILED; + return returnvalue::FAILED; } sif::info << canInterface << " initialized successfully" << std::endl; } @@ -66,24 +66,24 @@ ReturnValue_t CspComIF::initializeInterface(CookieIF* cookie) { cspDeviceMap.emplace(cspAddress, vectorBuffer(maxReplyLength)); } - return HasReturnvaluesIF::RETURN_OK; + return returnvalue::OK; } ReturnValue_t CspComIF::sendMessage(CookieIF* cookie, const uint8_t* sendData, size_t sendLen) { int result; if (cookie == NULL) { - return HasReturnvaluesIF::RETURN_FAILED; + return returnvalue::FAILED; } CspCookie* cspCookie = dynamic_cast(cookie); if (cspCookie == NULL) { - return HasReturnvaluesIF::RETURN_FAILED; + return returnvalue::FAILED; } /* Extract csp port and bytes to query from command buffer */ uint8_t cspPort; uint16_t querySize = 0; result = getPortAndQuerySize(&sendData, &sendLen, &cspPort, &querySize); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { return result; } uint8_t cspAddress = cspCookie->getCspAddress(); @@ -101,8 +101,8 @@ ReturnValue_t CspComIF::sendMessage(CookieIF* cookie, const uint8_t* sendData, s /* No CSP fixed port was selected. Send data to the specified port and * wait for querySize number of bytes */ result = cspTransfer(cspAddress, cspPort, sendData, sendLen, querySize); - if (result != HasReturnvaluesIF::RETURN_OK) { - return HasReturnvaluesIF::RETURN_FAILED; + if (result != returnvalue::OK) { + return returnvalue::FAILED; } replySize = querySize; break; @@ -111,22 +111,22 @@ ReturnValue_t CspComIF::sendMessage(CookieIF* cookie, const uint8_t* sendData, s sif::error << "CspComIF: Invalid port specified" << std::endl; break; } - return HasReturnvaluesIF::RETURN_OK; + return returnvalue::OK; } -ReturnValue_t CspComIF::getSendSuccess(CookieIF* cookie) { return HasReturnvaluesIF::RETURN_OK; } +ReturnValue_t CspComIF::getSendSuccess(CookieIF* cookie) { return returnvalue::OK; } ReturnValue_t CspComIF::requestReceiveMessage(CookieIF* cookie, size_t requestLen) { - return HasReturnvaluesIF::RETURN_OK; + return returnvalue::OK; } ReturnValue_t CspComIF::readReceivedMessage(CookieIF* cookie, uint8_t** buffer, size_t* size) { if (cookie == NULL) { - return HasReturnvaluesIF::RETURN_FAILED; + return returnvalue::FAILED; } CspCookie* cspCookie = dynamic_cast(cookie); if (cspCookie == NULL) { - return HasReturnvaluesIF::RETURN_FAILED; + return returnvalue::FAILED; } uint8_t cspAddress = cspCookie->getCspAddress(); @@ -134,7 +134,7 @@ ReturnValue_t CspComIF::readReceivedMessage(CookieIF* cookie, uint8_t** buffer, *buffer = cspDeviceMap[cspAddress].data(); *size = replySize; - return HasReturnvaluesIF::RETURN_OK; + return returnvalue::OK; } ReturnValue_t CspComIF::cspTransfer(uint8_t cspAddress, uint8_t cspPort, const uint8_t* cmdBuffer, @@ -146,7 +146,7 @@ ReturnValue_t CspComIF::cspTransfer(uint8_t cspAddress, uint8_t cspPort, const u if (iter == cspDeviceMap.end()) { sif::error << "CSP device with address " << cspAddress << " no found in" << " device map" << std::endl; - return RETURN_FAILED; + return returnvalue::FAILED; } uint8_t* replyBuffer = iter->second.data(); @@ -157,7 +157,7 @@ ReturnValue_t CspComIF::cspTransfer(uint8_t cspAddress, uint8_t cspPort, const u sif::error << "CspComIF::cspTransfer: Failed to get memory for a csp packet from the csp " << "stack" << std::endl; csp_close(conn); - return RETURN_FAILED; + return returnvalue::FAILED; } memcpy(commandPacket->data, cmdBuffer, cmdLen); @@ -167,12 +167,12 @@ ReturnValue_t CspComIF::cspTransfer(uint8_t cspAddress, uint8_t cspPort, const u csp_buffer_free(commandPacket); sif::error << "CspComIF::cspTransfer: Failed to send csp packet" << std::endl; csp_close(conn); - return RETURN_FAILED; + return returnvalue::FAILED; } /* Return when no reply is expected */ if (expectedSize == 0) { - return RETURN_OK; + return returnvalue::OK; } csp_packet_t* reply; @@ -180,7 +180,7 @@ ReturnValue_t CspComIF::cspTransfer(uint8_t cspAddress, uint8_t cspPort, const u if (reply == NULL) { sif::error << "CspComIF::cspTransfer: Failed to read csp packet" << std::endl; csp_close(conn); - return RETURN_FAILED; + return returnvalue::FAILED; } memcpy(replyBuffer, reply->data, reply->length); expectedSize = expectedSize - reply->length; @@ -191,13 +191,13 @@ ReturnValue_t CspComIF::cspTransfer(uint8_t cspAddress, uint8_t cspPort, const u if (reply == NULL) { sif::error << "CspComIF::cspTransfer: Failed to read csp packet" << std::endl; csp_close(conn); - return RETURN_FAILED; + return returnvalue::FAILED; } if ((reply->length + bytesRead) > iter->second.size()) { sif::error << "CspComIF::cspTransfer: Reply buffer to short" << std::endl; csp_buffer_free(reply); csp_close(conn); - return RETURN_FAILED; + return returnvalue::FAILED; } memcpy(replyBuffer + bytesRead, reply->data, reply->length); expectedSize = expectedSize - reply->length; @@ -209,30 +209,30 @@ ReturnValue_t CspComIF::cspTransfer(uint8_t cspAddress, uint8_t cspPort, const u sif::error << "CspComIF::cspTransfer: Received more bytes than requested" << std::endl; sif::debug << "CspComIF::cspTransfer: Received bytes: " << bytesRead << std::endl; csp_close(conn); - return RETURN_FAILED; + return returnvalue::FAILED; } csp_close(conn); - return HasReturnvaluesIF::RETURN_OK; + return returnvalue::OK; } ReturnValue_t CspComIF::getPortAndQuerySize(const uint8_t** sendData, size_t* sendLen, uint8_t* cspPort, uint16_t* querySize) { ReturnValue_t result = SerializeAdapter::deSerialize(cspPort, sendData, sendLen, SerializeIF::Endianness::BIG); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { sif::error << "CspComIF: Failed to deserialize CSP port from command " << "buffer" << std::endl; - return HasReturnvaluesIF::RETURN_FAILED; + return returnvalue::FAILED; } SerializeAdapter::deSerialize(querySize, sendData, sendLen, SerializeIF::Endianness::BIG); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { sif::error << "CspComIF: Failed to deserialize querySize from command " << "buffer" << std::endl; - return HasReturnvaluesIF::RETURN_FAILED; + return returnvalue::FAILED; } - return HasReturnvaluesIF::RETURN_OK; + return returnvalue::OK; } void CspComIF::initiatePingRequest(uint8_t cspAddress, uint16_t querySize) { diff --git a/linux/csp/CspComIF.h b/linux/csp/CspComIF.h index d36bbf4f..81d95169 100644 --- a/linux/csp/CspComIF.h +++ b/linux/csp/CspComIF.h @@ -4,7 +4,7 @@ #include #include #include -#include +#include #include #include diff --git a/linux/devices/GPSHyperionLinuxController.cpp b/linux/devices/GPSHyperionLinuxController.cpp index 32e3e40d..556db7ef 100644 --- a/linux/devices/GPSHyperionLinuxController.cpp +++ b/linux/devices/GPSHyperionLinuxController.cpp @@ -50,7 +50,7 @@ ReturnValue_t GPSHyperionLinuxController::checkModeCommand(Mode_t mode, Submode_ return HasModesIF::INVALID_MODE; } } - return HasReturnvaluesIF::RETURN_OK; + return returnvalue::OK; } ReturnValue_t GPSHyperionLinuxController::executeAction(ActionId_t actionId, @@ -68,7 +68,7 @@ ReturnValue_t GPSHyperionLinuxController::executeAction(ActionId_t actionId, return DeviceHandlerIF::COMMAND_NOT_IMPLEMENTED; } } - return HasReturnvaluesIF::RETURN_OK; + return returnvalue::OK; } ReturnValue_t GPSHyperionLinuxController::initializeLocalDataPool( @@ -88,7 +88,7 @@ ReturnValue_t GPSHyperionLinuxController::initializeLocalDataPool( localDataPoolMap.emplace(GpsHyperion::SATS_IN_VIEW, new PoolEntry()); localDataPoolMap.emplace(GpsHyperion::FIX_MODE, new PoolEntry()); poolManager.subscribeForRegularPeriodicPacket({gpsSet.getSid(), 30.0}); - return HasReturnvaluesIF::RETURN_OK; + return returnvalue::OK; } void GPSHyperionLinuxController::setResetPinTriggerFunction(gpioResetFunction_t resetCallback, @@ -99,7 +99,7 @@ void GPSHyperionLinuxController::setResetPinTriggerFunction(gpioResetFunction_t ReturnValue_t GPSHyperionLinuxController::initialize() { ReturnValue_t result = ExtendedControllerBase::initialize(); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { return result; } auto openError = [&](const char *type, int error) { @@ -178,11 +178,11 @@ void GPSHyperionLinuxController::readGpsDataFromGpsd() { ReturnValue_t GPSHyperionLinuxController::handleGpsRead() { PoolReadGuard pg(&gpsSet); - if (pg.getReadResult() != HasReturnvaluesIF::RETURN_OK) { + if (pg.getReadResult() != returnvalue::OK) { #if FSFW_VERBOSE_LEVEL >= 1 sif::warning << "GPSHyperionHandler::readGpsDataFromGpsd: Reading dataset failed" << std::endl; #endif - return RETURN_FAILED; + return returnvalue::FAILED; } bool validFix = false; @@ -311,7 +311,7 @@ ReturnValue_t GPSHyperionLinuxController::handleGpsRead() { std::tm tm = *std::gmtime(&t); std::cout << "C Time: " << std::put_time(&tm, "%c") << std::endl; } - return RETURN_OK; + return returnvalue::OK; } #endif diff --git a/linux/devices/Max31865RtdLowlevelHandler.cpp b/linux/devices/Max31865RtdLowlevelHandler.cpp index ea038c8b..4869fe39 100644 --- a/linux/devices/Max31865RtdLowlevelHandler.cpp +++ b/linux/devices/Max31865RtdLowlevelHandler.cpp @@ -24,7 +24,7 @@ Max31865RtdReader::Max31865RtdReader(object_id_t objectId, SpiComIF* lowLevelCom ReturnValue_t Max31865RtdReader::performOperation(uint8_t operationCode) { using namespace MAX31865; - ReturnValue_t result = RETURN_OK; + ReturnValue_t result = returnvalue::OK; static_cast(result); // Stopwatch watch; if (periodicInitHandling()) { @@ -34,12 +34,12 @@ ReturnValue_t Max31865RtdReader::performOperation(uint8_t operationCode) { #endif } else { // No devices usable (e.g. TCS board off) - return RETURN_OK; + return returnvalue::OK; } #if OBSW_RTD_AUTO_MODE == 0 result = periodicReadReqHandling(); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return result; } // After requesting, 65 milliseconds delay required @@ -59,8 +59,8 @@ bool Max31865RtdReader::rtdIsActive(uint8_t idx) { bool Max31865RtdReader::periodicInitHandling() { using namespace MAX31865; MutexGuard mg(readerMutex); - ReturnValue_t result = RETURN_OK; - if (mg.getLockResult() != RETURN_OK) { + ReturnValue_t result = returnvalue::OK; + if (mg.getLockResult() != returnvalue::OK) { sif::warning << "Max31865RtdReader::periodicInitHandling: Mutex lock failed" << std::endl; return false; } @@ -71,28 +71,28 @@ bool Max31865RtdReader::periodicInitHandling() { } if ((rtd->on or rtd->active) and not rtd->configured and rtd->cd.hasTimedOut()) { ManualCsLockWrapper mg(csLock, gpioIF, rtd->spiCookie, csTimeoutType, csTimeoutMs); - if (mg.lockResult != RETURN_OK or mg.gpioResult != RETURN_OK) { + if (mg.lockResult != returnvalue::OK or mg.gpioResult != returnvalue::OK) { sif::error << "Max31865RtdReader::periodicInitHandling: Manual CS lock failed" << std::endl; break; } result = writeCfgReg(rtd->spiCookie, BASE_CFG); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { handleSpiError(rtd, result, "writeCfgReg"); } if (rtd->writeLowThreshold) { result = writeLowThreshold(rtd->spiCookie, rtd->lowThreshold); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { handleSpiError(rtd, result, "writeLowThreshold"); } } if (rtd->writeHighThreshold) { result = writeHighThreshold(rtd->spiCookie, rtd->highThreshold); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { handleSpiError(rtd, result, "writeHighThreshold"); } } result = clearFaultStatus(rtd->spiCookie); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { handleSpiError(rtd, result, "clearFaultStatus"); } rtd->configured = true; @@ -123,9 +123,9 @@ bool Max31865RtdReader::periodicInitHandling() { ReturnValue_t Max31865RtdReader::periodicReadReqHandling() { using namespace MAX31865; MutexGuard mg(readerMutex); - if (mg.getLockResult() != RETURN_OK) { + if (mg.getLockResult() != returnvalue::OK) { sif::warning << "Max31865RtdReader::periodicReadReqHandling: Mutex lock failed" << std::endl; - return RETURN_FAILED; + return returnvalue::FAILED; } // Now request one shot config for all active RTDs for (auto& rtd : rtds) { @@ -134,23 +134,23 @@ ReturnValue_t Max31865RtdReader::periodicReadReqHandling() { } if (rtdIsActive(rtd->idx)) { ReturnValue_t result = writeCfgReg(rtd->spiCookie, BASE_CFG | (1 << CfgBitPos::ONE_SHOT)); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { handleSpiError(rtd, result, "writeCfgReg"); // Release mutex ASAP - return RETURN_FAILED; + return returnvalue::FAILED; } } } - return RETURN_OK; + return returnvalue::OK; } ReturnValue_t Max31865RtdReader::periodicReadHandling() { using namespace MAX31865; - auto result = RETURN_OK; + auto result = returnvalue::OK; MutexGuard mg(readerMutex); - if (mg.getLockResult() != RETURN_OK) { + if (mg.getLockResult() != returnvalue::OK) { sif::warning << "Max31865RtdReader::periodicReadReqHandling: Mutex lock failed" << std::endl; - return RETURN_FAILED; + return returnvalue::FAILED; } // Now read the RTD values for (auto& rtd : rtds) { @@ -161,9 +161,9 @@ ReturnValue_t Max31865RtdReader::periodicReadHandling() { uint16_t rtdVal = 0; bool faultBitSet = false; result = readRtdVal(rtd->spiCookie, rtdVal, faultBitSet); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { handleSpiError(rtd, result, "readRtdVal"); - return RETURN_FAILED; + return returnvalue::FAILED; } if (faultBitSet) { rtd->db.faultBitSet = faultBitSet; @@ -183,7 +183,7 @@ ReturnValue_t Max31865RtdReader::periodicReadHandling() { } } #endif - return RETURN_OK; + return returnvalue::OK; } ReturnValue_t Max31865RtdReader::initializeInterface(CookieIF* cookie) { @@ -192,7 +192,7 @@ ReturnValue_t Max31865RtdReader::initializeInterface(CookieIF* cookie) { } auto* rtdCookie = dynamic_cast(cookie); ReturnValue_t result = comIF->initializeInterface(rtdCookie->spiCookie); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { return result; } if (rtdCookie->idx > EiveMax31855::NUM_RTDS) { @@ -203,28 +203,28 @@ ReturnValue_t Max31865RtdReader::initializeInterface(CookieIF* cookie) { if (dbLen == 0) { dbLen = rtdCookie->db.getSerializedSize(); } - return RETURN_OK; + return returnvalue::OK; } ReturnValue_t Max31865RtdReader::sendMessage(CookieIF* cookie, const uint8_t* sendData, size_t sendLen) { if (cookie == nullptr) { - return RETURN_FAILED; + return returnvalue::FAILED; } // Empty command.. don't fail for now if (sendLen < 1) { - return RETURN_OK; + return returnvalue::OK; } MutexGuard mg(readerMutex); - if (mg.getLockResult() != RETURN_OK) { + if (mg.getLockResult() != returnvalue::OK) { sif::warning << "Max31865RtdReader::sendMessage: Mutex lock failed" << std::endl; - return RETURN_FAILED; + return returnvalue::FAILED; } auto* rtdCookie = dynamic_cast(cookie); uint8_t cmdRaw = sendData[0]; if (cmdRaw > EiveMax31855::RtdCommands::NUM_CMDS) { sif::warning << "Max31865RtdReader::sendMessage: Invalid command" << std::endl; - return RETURN_FAILED; + return returnvalue::FAILED; } auto thresholdHandler = [](Max31865ReaderCookie* rtdCookie, const uint8_t* sendData) { @@ -275,7 +275,7 @@ ReturnValue_t Max31865RtdReader::sendMessage(CookieIF* cookie, const uint8_t* se rtdCookie->highThreshold = (sendData[1] << 8) | sendData[2]; rtdCookie->writeHighThreshold = true; } else { - return RETURN_FAILED; + return returnvalue::FAILED; } break; } @@ -284,7 +284,7 @@ ReturnValue_t Max31865RtdReader::sendMessage(CookieIF* cookie, const uint8_t* se rtdCookie->lowThreshold = (sendData[1] << 8) | sendData[2]; rtdCookie->writeLowThreshold = true; } else { - return RETURN_FAILED; + return returnvalue::FAILED; } break; } @@ -294,34 +294,34 @@ ReturnValue_t Max31865RtdReader::sendMessage(CookieIF* cookie, const uint8_t* se break; } } - return RETURN_OK; + return returnvalue::OK; } -ReturnValue_t Max31865RtdReader::getSendSuccess(CookieIF* cookie) { return RETURN_OK; } +ReturnValue_t Max31865RtdReader::getSendSuccess(CookieIF* cookie) { return returnvalue::OK; } ReturnValue_t Max31865RtdReader::requestReceiveMessage(CookieIF* cookie, size_t requestLen) { - return RETURN_OK; + return returnvalue::OK; } ReturnValue_t Max31865RtdReader::readReceivedMessage(CookieIF* cookie, uint8_t** buffer, size_t* size) { MutexGuard mg(readerMutex); - if (mg.getLockResult() != RETURN_OK) { + if (mg.getLockResult() != returnvalue::OK) { // TODO: Emit warning - return RETURN_FAILED; + return returnvalue::FAILED; } auto* rtdCookie = dynamic_cast(cookie); uint8_t* exchangePtr = rtdCookie->exchangeBuf.data(); size_t serLen = 0; auto result = rtdCookie->db.serialize(&exchangePtr, &serLen, rtdCookie->exchangeBuf.size(), SerializeIF::Endianness::MACHINE); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { // TODO: Emit warning - return RETURN_FAILED; + return returnvalue::FAILED; } *buffer = reinterpret_cast(rtdCookie->exchangeBuf.data()); *size = serLen; - return RETURN_OK; + return returnvalue::OK; } ReturnValue_t Max31865RtdReader::writeCfgReg(SpiCookie* cookie, uint8_t cfg) { @@ -345,7 +345,7 @@ ReturnValue_t Max31865RtdReader::clearFaultStatus(SpiCookie* cookie) { // Read back the current configuration to avoid overwriting it when clearing te fault status uint8_t currentCfg = 0; auto result = readCfgReg(cookie, currentCfg); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return result; } // Clear bytes 5, 3 and 2 which need to be 0 @@ -358,7 +358,7 @@ ReturnValue_t Max31865RtdReader::readCfgReg(SpiCookie* cookie, uint8_t& cfg) { using namespace MAX31865; uint8_t* replyPtr = nullptr; auto result = readNFromReg(cookie, CONFIG, 1, &replyPtr); - if (result == RETURN_OK) { + if (result == returnvalue::OK) { cfg = replyPtr[0]; } return result; @@ -380,7 +380,7 @@ ReturnValue_t Max31865RtdReader::readLowThreshold(SpiCookie* cookie, uint16_t& l using namespace MAX31865; uint8_t* replyPtr = nullptr; auto result = readNFromReg(cookie, LOW_THRESHOLD, 2, &replyPtr); - if (result == RETURN_OK) { + if (result == returnvalue::OK) { lowThreshold = (replyPtr[0] << 8) | replyPtr[1]; } return result; @@ -390,7 +390,7 @@ ReturnValue_t Max31865RtdReader::readHighThreshold(SpiCookie* cookie, uint16_t& using namespace MAX31865; uint8_t* replyPtr = nullptr; auto result = readNFromReg(cookie, HIGH_THRESHOLD, 2, &replyPtr); - if (result == RETURN_OK) { + if (result == returnvalue::OK) { highThreshold = (replyPtr[0] << 8) | replyPtr[1]; } return result; @@ -400,7 +400,7 @@ ReturnValue_t Max31865RtdReader::writeNToReg(SpiCookie* cookie, uint8_t reg, siz uint8_t** reply) { using namespace MAX31865; if (n > cmdBuf.size() - 1) { - return HasReturnvaluesIF::RETURN_FAILED; + return returnvalue::FAILED; } cmdBuf[0] = reg | WRITE_BIT; for (size_t idx = 0; idx < n; idx++) { @@ -413,7 +413,7 @@ ReturnValue_t Max31865RtdReader::readRtdVal(SpiCookie* cookie, uint16_t& val, bo using namespace MAX31865; uint8_t* replyPtr = nullptr; auto result = readNFromReg(cookie, RTD, 2, &replyPtr); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return result; } if (replyPtr[1] & 0b0000'0001) { @@ -428,27 +428,27 @@ ReturnValue_t Max31865RtdReader::readNFromReg(SpiCookie* cookie, uint8_t reg, si uint8_t** reply) { using namespace MAX31865; if (n > 4) { - return HasReturnvaluesIF::RETURN_FAILED; + return returnvalue::FAILED; } // Clear write bit in any case reg &= ~WRITE_BIT; cmdBuf[0] = reg; std::memset(cmdBuf.data() + 1, 0, n); ReturnValue_t result = comIF->sendMessage(cookie, cmdBuf.data(), n + 1); - if (result != RETURN_OK) { - return RETURN_FAILED; + if (result != returnvalue::OK) { + return returnvalue::FAILED; } size_t dummyLen = 0; uint8_t* replyPtr = nullptr; result = comIF->readReceivedMessage(cookie, &replyPtr, &dummyLen); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return result; } if (reply != nullptr) { *reply = replyPtr + 1; } - return RETURN_OK; + return returnvalue::OK; } ReturnValue_t Max31865RtdReader::handleSpiError(Max31865ReaderCookie* cookie, ReturnValue_t result, diff --git a/linux/devices/devicedefinitions/MPSoCReturnValuesIF.h b/linux/devices/devicedefinitions/MPSoCReturnValuesIF.h index ac695ad8..8a1085ca 100644 --- a/linux/devices/devicedefinitions/MPSoCReturnValuesIF.h +++ b/linux/devices/devicedefinitions/MPSoCReturnValuesIF.h @@ -1,7 +1,7 @@ #ifndef MPSOC_RETURN_VALUES_IF_H_ #define MPSOC_RETURN_VALUES_IF_H_ -#include "fsfw/returnvalues/HasReturnvaluesIF.h" +#include "fsfw/returnvalues/returnvalue.h" class MPSoCReturnValuesIF { public: diff --git a/linux/devices/devicedefinitions/PlocMPSoCDefinitions.h b/linux/devices/devicedefinitions/PlocMPSoCDefinitions.h index 180ee907..3423c54c 100644 --- a/linux/devices/devicedefinitions/PlocMPSoCDefinitions.h +++ b/linux/devices/devicedefinitions/PlocMPSoCDefinitions.h @@ -164,21 +164,21 @@ class TcBase : public ploc::SpTcBase, public MPSoCReturnValuesIF { * @param commandData Pointer to command specific data * @param commandDataLen Length of command data * - * @return RETURN_OK if packet creation was successful, otherwise error return value + * @return returnvalue::OK if packet creation was successful, otherwise error return value */ ReturnValue_t buildPacket(const uint8_t* commandData, size_t commandDataLen) { payloadStart = spParams.buf + ccsds::HEADER_LEN; ReturnValue_t res; if (commandData != nullptr and commandDataLen > 0) { res = initPacket(commandData, commandDataLen); - if (res != result::OK) { + if (res != returnvalue::OK) { return res; } } updateSpFields(); res = checkSizeAndSerializeHeader(); - if (res != result::OK) { + if (res != returnvalue::OK) { return res; } return calcCrc(); @@ -192,7 +192,7 @@ class TcBase : public ploc::SpTcBase, public MPSoCReturnValuesIF { * @param commandDataLen Length of received command data */ virtual ReturnValue_t initPacket(const uint8_t* commandData, size_t commandDataLen) { - return HasReturnvaluesIF::RETURN_OK; + return returnvalue::OK; } }; @@ -213,9 +213,9 @@ class TcMemRead : public TcBase { protected: ReturnValue_t initPacket(const uint8_t* commandData, size_t commandDataLen) override { - ReturnValue_t result = HasReturnvaluesIF::RETURN_OK; + ReturnValue_t result = returnvalue::OK; result = lengthCheck(commandDataLen); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { return result; } std::memcpy(payloadStart, commandData, MEM_ADDRESS_SIZE); @@ -224,7 +224,7 @@ class TcMemRead : public TcBase { const uint8_t* memLenPtr = commandData + MEM_ADDRESS_SIZE; result = SerializeAdapter::deSerialize(&memLen, &memLenPtr, &size, SerializeIF::Endianness::BIG); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { return result; } return result; @@ -239,10 +239,10 @@ class TcMemRead : public TcBase { uint16_t memLen = 0; ReturnValue_t lengthCheck(size_t commandDataLen) { - if (commandDataLen != COMMAND_LENGTH or checkPayloadLen() != HasReturnvaluesIF::RETURN_OK) { + if (commandDataLen != COMMAND_LENGTH or checkPayloadLen() != returnvalue::OK) { return INVALID_LENGTH; } - return HasReturnvaluesIF::RETURN_OK; + return returnvalue::OK; } }; @@ -260,16 +260,16 @@ class TcMemWrite : public TcBase { protected: ReturnValue_t initPacket(const uint8_t* commandData, size_t commandDataLen) override { - ReturnValue_t result = HasReturnvaluesIF::RETURN_OK; + ReturnValue_t result = returnvalue::OK; result = lengthCheck(commandDataLen); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { return result; } uint16_t memLen = *(commandData + MEM_ADDRESS_SIZE) << 8 | *(commandData + MEM_ADDRESS_SIZE + 1); spParams.setPayloadLen(MIN_FIXED_PAYLOAD_LENGTH + memLen * 4); result = checkPayloadLen(); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { return result; } std::memcpy(payloadStart, commandData, commandDataLen); @@ -294,7 +294,7 @@ class TcMemWrite : public TcBase { << spParams.maxSize - CRC_SIZE << std::endl; return INVALID_LENGTH; } - return HasReturnvaluesIF::RETURN_OK; + return returnvalue::OK; } }; @@ -315,7 +315,7 @@ class FlashFopen : public ploc::SpTcBase { size_t nameSize = filename.size(); spParams.setPayloadLen(nameSize + sizeof(NULL_TERMINATOR) + sizeof(accessMode)); ReturnValue_t result = checkPayloadLen(); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { return result; } std::memcpy(payloadStart, filename.c_str(), nameSize); @@ -341,7 +341,7 @@ class FlashFclose : public ploc::SpTcBase { size_t nameSize = filename.size(); spParams.setPayloadLen(nameSize + sizeof(NULL_TERMINATOR)); ReturnValue_t result = checkPayloadLen(); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { return result; } std::memcpy(payloadStart, filename.c_str(), nameSize); @@ -359,27 +359,27 @@ class TcFlashWrite : public ploc::SpTcBase { : ploc::SpTcBase(params, apid::TC_FLASHWRITE, sequenceCount) {} ReturnValue_t buildPacket(const uint8_t* writeData, uint32_t writeLen_) { - ReturnValue_t result = HasReturnvaluesIF::RETURN_OK; + ReturnValue_t result = returnvalue::OK; writeLen = writeLen_; if (writeLen > MAX_DATA_SIZE) { sif::debug << "FlashWrite::createPacket: Command data too big" << std::endl; - return HasReturnvaluesIF::RETURN_FAILED; + return returnvalue::FAILED; } spParams.setPayloadLen(static_cast(writeLen) + 4); result = checkPayloadLen(); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { return result; } size_t serializedSize = ccsds::HEADER_LEN; result = SerializeAdapter::serialize(&writeLen, payloadStart, &serializedSize, spParams.maxSize, SerializeIF::Endianness::BIG); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { return result; } std::memcpy(payloadStart + sizeof(writeLen), writeData, writeLen); updateSpFields(); auto res = checkSizeAndSerializeHeader(); - if (res != result::OK) { + if (res != returnvalue::OK) { return res; } return calcCrc(); @@ -401,7 +401,7 @@ class TcFlashDelete : public ploc::SpTcBase { size_t nameSize = filename.size(); spParams.setPayloadLen(nameSize + sizeof(NULL_TERMINATOR)); auto res = checkPayloadLen(); - if (res != HasReturnvaluesIF::RETURN_OK) { + if (res != returnvalue::OK) { return res; } std::memcpy(payloadStart, filename.c_str(), nameSize); @@ -409,7 +409,7 @@ class TcFlashDelete : public ploc::SpTcBase { updateSpFields(); res = checkSizeAndSerializeHeader(); - if (res != result::OK) { + if (res != returnvalue::OK) { return res; } return calcCrc(); @@ -438,14 +438,14 @@ class TcReplayStart : public TcBase { protected: ReturnValue_t initPacket(const uint8_t* commandData, size_t commandDataLen) override { - ReturnValue_t result = HasReturnvaluesIF::RETURN_OK; + ReturnValue_t result = returnvalue::OK; spParams.setPayloadLen(commandDataLen); result = lengthCheck(commandDataLen); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { return result; } result = checkData(*commandData); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { return result; } std::memcpy(payloadStart, commandData, commandDataLen); @@ -459,11 +459,11 @@ class TcReplayStart : public TcBase { ReturnValue_t lengthCheck(size_t commandDataLen) { if (commandDataLen != COMMAND_DATA_LENGTH or - checkPayloadLen() != HasReturnvaluesIF::RETURN_OK) { + checkPayloadLen() != returnvalue::OK) { sif::warning << "TcReplayStart: Command has invalid length " << commandDataLen << std::endl; return INVALID_LENGTH; } - return HasReturnvaluesIF::RETURN_OK; + return returnvalue::OK; } ReturnValue_t checkData(uint8_t replay) { @@ -471,7 +471,7 @@ class TcReplayStart : public TcBase { sif::warning << "TcReplayStart::checkData: Invalid replay value" << std::endl; return INVALID_PARAMETER; } - return HasReturnvaluesIF::RETURN_OK; + return returnvalue::OK; } }; @@ -488,22 +488,22 @@ class TcDownlinkPwrOn : public TcBase { protected: ReturnValue_t initPacket(const uint8_t* commandData, size_t commandDataLen) override { - ReturnValue_t result = HasReturnvaluesIF::RETURN_OK; + ReturnValue_t result = returnvalue::OK; result = lengthCheck(commandDataLen); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { return result; } result = modeCheck(*commandData); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { return result; } result = laneRateCheck(*(commandData + 1)); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { return result; } spParams.setPayloadLen(commandDataLen + sizeof(MAX_AMPLITUDE)); result = checkPayloadLen(); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { return result; } std::memcpy(payloadStart, commandData, commandDataLen); @@ -529,7 +529,7 @@ class TcDownlinkPwrOn : public TcBase { sif::warning << "TcDownlinkPwrOn: Command has invalid length " << commandDataLen << std::endl; return INVALID_LENGTH; } - return HasReturnvaluesIF::RETURN_OK; + return returnvalue::OK; } ReturnValue_t modeCheck(uint8_t mode) { @@ -537,7 +537,7 @@ class TcDownlinkPwrOn : public TcBase { sif::warning << "TcDwonlinkPwrOn::modeCheck: Invalid JESD mode" << std::endl; return INVALID_MODE; } - return HasReturnvaluesIF::RETURN_OK; + return returnvalue::OK; } ReturnValue_t laneRateCheck(uint8_t laneRate) { @@ -545,7 +545,7 @@ class TcDownlinkPwrOn : public TcBase { sif::warning << "TcReplayStart::laneRateCheck: Invalid lane rate" << std::endl; return INVALID_LANE_RATE; } - return HasReturnvaluesIF::RETURN_OK; + return returnvalue::OK; } }; @@ -571,10 +571,10 @@ class TcReplayWriteSeq : public TcBase { protected: ReturnValue_t initPacket(const uint8_t* commandData, size_t commandDataLen) override { - ReturnValue_t result = HasReturnvaluesIF::RETURN_OK; + ReturnValue_t result = returnvalue::OK; spParams.setPayloadLen(commandDataLen + sizeof(NULL_TERMINATOR)); result = lengthCheck(commandDataLen); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { return result; } std::memcpy(payloadStart, commandData, commandDataLen); @@ -587,12 +587,12 @@ class TcReplayWriteSeq : public TcBase { ReturnValue_t lengthCheck(size_t commandDataLen) { if (commandDataLen > USE_DECODING_LENGTH + MAX_FILENAME_SIZE or - checkPayloadLen() != HasReturnvaluesIF::RETURN_OK) { + checkPayloadLen() != returnvalue::OK) { sif::warning << "TcReplayWriteSeq: Command has invalid length " << commandDataLen << std::endl; return INVALID_LENGTH; } - return HasReturnvaluesIF::RETURN_OK; + return returnvalue::OK; } }; @@ -616,7 +616,7 @@ class FlashWritePusCmd : public MPSoCReturnValuesIF { if (mpsocFile.size() > MAX_FILENAME_SIZE) { return MPSOC_FILENAME_TOO_LONG; } - return HasReturnvaluesIF::RETURN_OK; + return returnvalue::OK; } std::string getObcFile() { return obcFile; } @@ -660,7 +660,7 @@ class TcCamcmdSend : public TcBase { uint16_t dataLen = static_cast(commandDataLen + sizeof(CARRIAGE_RETURN)); spParams.setPayloadLen(sizeof(dataLen) + commandDataLen + sizeof(CARRIAGE_RETURN)); auto res = checkPayloadLen(); - if (res != HasReturnvaluesIF::RETURN_OK) { + if (res != returnvalue::OK) { return res; } size_t size = ccsds::HEADER_LEN; @@ -669,7 +669,7 @@ class TcCamcmdSend : public TcBase { std::memcpy(payloadStart + sizeof(dataLen), commandData, commandDataLen); *(payloadStart + sizeof(dataLen) + commandDataLen) = CARRIAGE_RETURN; - return HasReturnvaluesIF::RETURN_OK; + return returnvalue::OK; } private: diff --git a/linux/devices/devicedefinitions/PlocSupervisorDefinitions.h b/linux/devices/devicedefinitions/PlocSupervisorDefinitions.h index bfe23488..1dbb08c0 100644 --- a/linux/devices/devicedefinitions/PlocSupervisorDefinitions.h +++ b/linux/devices/devicedefinitions/PlocSupervisorDefinitions.h @@ -300,7 +300,7 @@ class ApidOnlyPacket : public ploc::SpTcBase { ReturnValue_t buildPacket() { auto res = checkSizeAndSerializeHeader(); - if (res != result::OK) { + if (res != returnvalue::OK) { return res; } return calcCrc(); @@ -336,7 +336,7 @@ class MPSoCBootSelect : public ploc::SpTcBase { ReturnValue_t buildPacket(uint8_t mem = 0, uint8_t bp0 = 0, uint8_t bp1 = 0, uint8_t bp2 = 0) { auto res = checkSizeAndSerializeHeader(); - if (res != result::OK) { + if (res != returnvalue::OK) { return res; } initPacket(mem, bp0, bp1, bp2); @@ -382,7 +382,7 @@ class EnableNvms : public ploc::SpTcBase { ReturnValue_t buildPacket(uint8_t nvm01, uint8_t nvm3) { auto res = checkSizeAndSerializeHeader(); - if (res != result::OK) { + if (res != returnvalue::OK) { return res; } initPacket(nvm01, nvm3); @@ -412,11 +412,11 @@ class SetTimeRef : public ploc::SpTcBase { ReturnValue_t buildPacket(Clock::TimeOfDay_t* time) { auto res = checkSizeAndSerializeHeader(); - if (res != result::OK) { + if (res != returnvalue::OK) { return res; } res = initPacket(time); - if (res != result::OK) { + if (res != returnvalue::OK) { return res; } return calcCrc(); @@ -434,37 +434,37 @@ class SetTimeRef : public ploc::SpTcBase { ReturnValue_t result = SerializeAdapter::serialize(&milliseconds, &dataFieldPtr, &serializedSize, spParams.maxSize, SerializeIF::Endianness::BIG); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { return result; } uint8_t second = static_cast(time->second); result = SerializeAdapter::serialize(&second, &dataFieldPtr, &serializedSize, spParams.maxSize, SerializeIF::Endianness::BIG); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { return result; } uint8_t minute = static_cast(time->minute); result = SerializeAdapter::serialize(&minute, &dataFieldPtr, &serializedSize, spParams.maxSize, SerializeIF::Endianness::BIG); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { return result; } uint8_t hour = static_cast(time->hour); result = SerializeAdapter::serialize(&hour, &dataFieldPtr, &serializedSize, spParams.maxSize, SerializeIF::Endianness::BIG); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { return result; } uint8_t day = static_cast(time->day); result = SerializeAdapter::serialize(&day, &dataFieldPtr, &serializedSize, spParams.maxSize, SerializeIF::Endianness::BIG); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { return result; } uint8_t month = static_cast(time->month); result = SerializeAdapter::serialize(&month, &dataFieldPtr, &serializedSize, spParams.maxSize, SerializeIF::Endianness::BIG); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { return result; } uint8_t year = static_cast(time->year - 1900); @@ -491,7 +491,7 @@ class SetBootTimeout : public ploc::SpTcBase { ReturnValue_t buildPacket(uint32_t timeout) { auto res = checkSizeAndSerializeHeader(); - if (res != result::OK) { + if (res != returnvalue::OK) { return res; } initPacket(timeout); @@ -528,7 +528,7 @@ class SetRestartTries : public ploc::SpTcBase { ReturnValue_t buildPacket(uint8_t restartTries) { auto res = checkSizeAndSerializeHeader(); - if (res != result::OK) { + if (res != returnvalue::OK) { return res; } initPacket(restartTries); @@ -562,7 +562,7 @@ class DisablePeriodicHkTransmission : public ploc::SpTcBase { ReturnValue_t buildPacket() { auto res = checkSizeAndSerializeHeader(); - if (res != result::OK) { + if (res != returnvalue::OK) { return res; } initPacket(); @@ -602,7 +602,7 @@ class LatchupAlert : public ploc::SpTcBase { spParams.creator.setApid(APID_DISABLE_LATCHUP_ALERT); } auto res = checkSizeAndSerializeHeader(); - if (res != result::OK) { + if (res != returnvalue::OK) { return res; } initPacket(latchupId); @@ -636,11 +636,11 @@ class SetAlertlimit : public ploc::SpTcBase { ReturnValue_t buildPacket(uint8_t latchupId, uint32_t dutycycle) { auto res = checkSizeAndSerializeHeader(); - if (res != result::OK) { + if (res != returnvalue::OK) { return res; } res = initPacket(latchupId, dutycycle); - if (res != result::OK) { + if (res != returnvalue::OK) { return res; } return calcCrc(); @@ -679,7 +679,7 @@ class SetAdcEnabledChannels : public ploc::SpTcBase { ReturnValue_t buildPacket(uint16_t ch) { auto res = checkSizeAndSerializeHeader(); - if (res != result::OK) { + if (res != returnvalue::OK) { return res; } initPacket(ch); @@ -718,7 +718,7 @@ class SetAdcWindowAndStride : public ploc::SpTcBase { ReturnValue_t buildPacket(uint16_t windowSize, uint16_t stridingStepSize) { auto res = checkSizeAndSerializeHeader(); - if (res != result::OK) { + if (res != returnvalue::OK) { return res; } initPacket(windowSize, stridingStepSize); @@ -758,7 +758,7 @@ class SetAdcThreshold : public ploc::SpTcBase { ReturnValue_t buildPacket(uint32_t threshold) { auto res = checkSizeAndSerializeHeader(); - if (res != result::OK) { + if (res != returnvalue::OK) { return res; } initPacket(threshold); @@ -796,7 +796,7 @@ class RunAutoEmTests : public ploc::SpTcBase { ReturnValue_t buildPacket(uint8_t test) { auto res = checkSizeAndSerializeHeader(); - if (res != result::OK) { + if (res != returnvalue::OK) { return res; } initPacket(test); @@ -844,7 +844,7 @@ class MramCmd : public ploc::SpTcBase { sif::debug << "WipeMram: Invalid action specified"; } auto res = checkSizeAndSerializeHeader(); - if (res != result::OK) { + if (res != returnvalue::OK) { return res; } initPacket(start, stop); @@ -892,7 +892,7 @@ class SetGpio : public ploc::SpTcBase { ReturnValue_t buildPacket(uint8_t port, uint8_t pin, uint8_t val) { auto res = checkSizeAndSerializeHeader(); - if (res != result::OK) { + if (res != returnvalue::OK) { return res; } initPacket(port, pin, val); @@ -936,7 +936,7 @@ class ReadGpio : public ploc::SpTcBase { ReturnValue_t buildPacket(uint8_t port, uint8_t pin) { auto res = checkSizeAndSerializeHeader(); - if (res != result::OK) { + if (res != returnvalue::OK) { return res; } initPacket(port, pin); @@ -982,7 +982,7 @@ class FactoryReset : public ploc::SpTcBase { ReturnValue_t buildPacket(Op op) { auto res = checkSizeAndSerializeHeader(); - if (res != result::OK) { + if (res != returnvalue::OK) { return res; } initPacket(op); @@ -1020,7 +1020,7 @@ class SetShutdownTimeout : public ploc::SpTcBase { ReturnValue_t buildPacket(uint32_t timeout) { auto res = checkSizeAndSerializeHeader(); - if (res != result::OK) { + if (res != returnvalue::OK) { return res; } initPacket(timeout); @@ -1059,7 +1059,7 @@ class CheckMemory : public ploc::SpTcBase { ReturnValue_t buildPacket(uint8_t memoryId, uint32_t startAddress, uint32_t length) { auto res = checkSizeAndSerializeHeader(); - if (res != result::OK) { + if (res != returnvalue::OK) { return res; } initPacket(memoryId, startAddress, length); @@ -1114,7 +1114,7 @@ class WriteMemory : public ploc::SpTcBase { spParams.creator.setSeqCount(sequenceCount); initPacket(memoryId, startAddress, length, updateData); auto res = checkSizeAndSerializeHeader(); - if (res != result::OK) { + if (res != returnvalue::OK) { return res; } return calcCrc(); @@ -1137,7 +1137,7 @@ class WriteMemory : public ploc::SpTcBase { } // To avoid crashes in this unexpected case ReturnValue_t result = checkPayloadLen(); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { return result; } size_t serializedSize = 6; @@ -1155,7 +1155,7 @@ class WriteMemory : public ploc::SpTcBase { // a value of zero is added here data[updateDataLen + 1] = 0; } - return HasReturnvaluesIF::RETURN_OK; + return returnvalue::OK; } }; @@ -1172,7 +1172,7 @@ class EraseMemory : public ploc::SpTcBase { ReturnValue_t buildPacket(uint8_t memoryId, uint32_t startAddress, uint32_t length) { auto res = checkSizeAndSerializeHeader(); - if (res != result::OK) { + if (res != returnvalue::OK) { return res; } initPacket(memoryId, startAddress, length); @@ -1214,7 +1214,7 @@ class EnableAutoTm : public ploc::SpTcBase { ReturnValue_t buildPacket() { auto res = checkSizeAndSerializeHeader(); - if (res != result::OK) { + if (res != returnvalue::OK) { return res; } payloadStart[0] = ENABLE; @@ -1239,7 +1239,7 @@ class DisableAutoTm : public ploc::SpTcBase { ReturnValue_t buildPacket() { auto res = checkSizeAndSerializeHeader(); - if (res != result::OK) { + if (res != returnvalue::OK) { return res; } payloadStart[0] = DISABLE; @@ -1279,7 +1279,7 @@ class RequestLoggingData : public ploc::SpTcBase { */ ReturnValue_t buildPacket(Sa sa, uint8_t tpc = 0) { auto res = checkSizeAndSerializeHeader(); - if (res != result::OK) { + if (res != returnvalue::OK) { return res; } payloadStart[0] = static_cast(sa); @@ -1305,7 +1305,7 @@ class VerificationReport : public ploc::SpTmReader { const uint8_t* refApidPtr = this->getPacketData(); ReturnValue_t result = SerializeAdapter::deSerialize(&refApid, refApidPtr, &size, SerializeIF::Endianness::BIG); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { sif::debug << "ExecutionReport: Failed to deserialize reference APID field" << std::endl; return result; } @@ -1318,14 +1318,14 @@ class VerificationReport : public ploc::SpTmReader { const uint8_t* statusCodePtr = this->getPacketData() + OFFSET_STATUS_CODE; ReturnValue_t result = SerializeAdapter::deSerialize(&statusCode, statusCodePtr, &size, SerializeIF::Endianness::BIG); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { sif::debug << "ExecutionReport: Failed to deserialize status code field" << std::endl; return result; } return statusCode; } - virtual ReturnValue_t checkApid() { return HasReturnvaluesIF::RETURN_FAILED; } + virtual ReturnValue_t checkApid() { return returnvalue::FAILED; } private: static const uint8_t OFFSET_STATUS_CODE = 4; @@ -1338,7 +1338,7 @@ class AcknowledgmentReport : public VerificationReport { ReturnValue_t checkApid() { uint16_t apid = this->getApid(); if (apid == APID_ACK_SUCCESS) { - return HasReturnvaluesIF::RETURN_OK; + return returnvalue::OK; } else if (apid == APID_ACK_FAILURE) { printStatusInformation(); return SupvReturnValuesIF::RECEIVED_ACK_FAILURE; @@ -1412,7 +1412,7 @@ class ExecutionReport : public VerificationReport { ReturnValue_t checkApid() { uint16_t apid = this->getApid(); if (apid == APID_EXE_SUCCESS) { - return HasReturnvaluesIF::RETURN_OK; + return returnvalue::OK; } else if (apid == APID_EXE_FAILURE) { printStatusInformation(); return SupvReturnValuesIF::RECEIVED_EXE_FAILURE; @@ -1844,7 +1844,7 @@ class UpdateStatusReport : public ploc::SpTmReader { ReturnValue_t parseDataField() { ReturnValue_t result = lengthCheck(); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { return result; } const uint8_t* dataFieldPtr = getFullData() + ccsds::HEADER_LEN; @@ -1855,14 +1855,14 @@ class UpdateStatusReport : public ploc::SpTmReader { SerializeIF::Endianness::BIG); SerializeAdapter::deSerialize(&length, &dataFieldPtr, &size, SerializeIF::Endianness::BIG); SerializeAdapter::deSerialize(&crc, &dataFieldPtr, &size, SerializeIF::Endianness::BIG); - return HasReturnvaluesIF::RETURN_OK; + return returnvalue::OK; } ReturnValue_t verifycrc(uint16_t goodCrc) const { if (crc != goodCrc) { return SupvReturnValuesIF::UPDATE_CRC_FAILURE; } - return HasReturnvaluesIF::RETURN_OK; + return returnvalue::OK; } uint16_t getCrc() const { return crc; } @@ -1883,7 +1883,7 @@ class UpdateStatusReport : public ploc::SpTmReader { if (getFullPacketLen() != FULL_SIZE) { return SupvReturnValuesIF::UPDATE_STATUS_REPORT_INVALID_LENGTH; } - return HasReturnvaluesIF::RETURN_OK; + return returnvalue::OK; } }; diff --git a/linux/devices/devicedefinitions/StarTrackerDefinitions.h b/linux/devices/devicedefinitions/StarTrackerDefinitions.h index f7059c66..be115a78 100644 --- a/linux/devices/devicedefinitions/StarTrackerDefinitions.h +++ b/linux/devices/devicedefinitions/StarTrackerDefinitions.h @@ -821,27 +821,27 @@ class ChecksumReply { * */ ChecksumReply(const uint8_t* datafield) { - ReturnValue_t result = HasReturnvaluesIF::RETURN_OK; + ReturnValue_t result = returnvalue::OK; region = *(datafield); const uint8_t* addressData = datafield + ADDRESS_OFFSET; size_t size = sizeof(address); result = SerializeAdapter::deSerialize(&address, &addressData, &size, SerializeIF::Endianness::LITTLE); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { sif::debug << "ChecksumReply::ChecksumReply: Failed to deserialize address" << std::endl; } const uint8_t* lengthData = datafield + LENGTH_OFFSET; size = sizeof(length); result = SerializeAdapter::deSerialize(&length, &lengthData, &size, SerializeIF::Endianness::LITTLE); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { sif::debug << "ChecksumReply::ChecksumReply: Failed to deserialize length" << std::endl; } const uint8_t* checksumData = datafield + CHECKSUM_OFFSET; size = sizeof(checksum); result = SerializeAdapter::deSerialize(&checksum, &checksumData, &size, SerializeIF::Endianness::LITTLE); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { sif::debug << "ChecksumReply::ChecksumReply: Failed to deserialize checksum" << std::endl; } } diff --git a/linux/devices/devicedefinitions/SupvReturnValuesIF.h b/linux/devices/devicedefinitions/SupvReturnValuesIF.h index 307deb0e..f4557735 100644 --- a/linux/devices/devicedefinitions/SupvReturnValuesIF.h +++ b/linux/devices/devicedefinitions/SupvReturnValuesIF.h @@ -1,7 +1,7 @@ #ifndef SUPV_RETURN_VALUES_IF_H_ #define SUPV_RETURN_VALUES_IF_H_ -#include "fsfw/returnvalues/HasReturnvaluesIF.h" +#include "fsfw/returnvalues/returnvalue.h" class SupvReturnValuesIF { public: diff --git a/linux/devices/ploc/PlocMPSoCHandler.cpp b/linux/devices/ploc/PlocMPSoCHandler.cpp index c56ada07..a192365f 100644 --- a/linux/devices/ploc/PlocMPSoCHandler.cpp +++ b/linux/devices/ploc/PlocMPSoCHandler.cpp @@ -26,9 +26,9 @@ PlocMPSoCHandler::PlocMPSoCHandler(object_id_t objectId, object_id_t uartComIFid PlocMPSoCHandler::~PlocMPSoCHandler() {} ReturnValue_t PlocMPSoCHandler::initialize() { - ReturnValue_t result = RETURN_OK; + ReturnValue_t result = returnvalue::OK; result = DeviceHandlerBase::initialize(); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return result; } uartComIf = dynamic_cast(communicationInterface); @@ -46,13 +46,13 @@ ReturnValue_t PlocMPSoCHandler::initialize() { ; } result = manager->registerListener(eventQueue->getId()); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return result; } result = manager->subscribeToEventRange( eventQueue->getId(), event::getEventId(PlocMPSoCHelper::MPSOC_FLASH_WRITE_FAILED), event::getEventId(PlocMPSoCHelper::MPSOC_FLASH_WRITE_SUCCESSFUL)); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { #if FSFW_CPP_OSTREAM_ENABLED == 1 sif::warning << "PlocMPSoCHandler::initialize: Failed to subscribe to events from " " ploc mpsoc helper" @@ -62,13 +62,13 @@ ReturnValue_t PlocMPSoCHandler::initialize() { } result = plocMPSoCHelper->setComIF(communicationInterface); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return ObjectManagerIF::CHILD_INIT_FAILED; } plocMPSoCHelper->setComCookie(comCookie); plocMPSoCHelper->setSequenceCount(&sequenceCount); result = commandActionHelper.initialize(); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { return ObjectManagerIF::CHILD_INIT_FAILED; } return result; @@ -76,7 +76,7 @@ ReturnValue_t PlocMPSoCHandler::initialize() { void PlocMPSoCHandler::performOperationHook() { EventMessage event; - for (ReturnValue_t result = eventQueue->receiveMessage(&event); result == RETURN_OK; + for (ReturnValue_t result = eventQueue->receiveMessage(&event); result == returnvalue::OK; result = eventQueue->receiveMessage(&event)) { switch (event.getMessageId()) { case EventMessage::EVENT_MESSAGE: @@ -90,9 +90,9 @@ void PlocMPSoCHandler::performOperationHook() { } CommandMessage message; for (ReturnValue_t result = commandActionHelperQueue->receiveMessage(&message); - result == RETURN_OK; result = commandActionHelperQueue->receiveMessage(&message)) { + result == returnvalue::OK; result = commandActionHelperQueue->receiveMessage(&message)) { result = commandActionHelper.handleReply(&message); - if (result == RETURN_OK) { + if (result == returnvalue::OK) { continue; } } @@ -100,7 +100,7 @@ void PlocMPSoCHandler::performOperationHook() { ReturnValue_t PlocMPSoCHandler::executeAction(ActionId_t actionId, MessageQueueId_t commandedBy, const uint8_t* data, size_t size) { - ReturnValue_t result = RETURN_OK; + ReturnValue_t result = returnvalue::OK; switch (actionId) { case mpsoc::SET_UART_TX_TRISTATE: { uartIsolatorSwitch.pullLow(); @@ -127,12 +127,12 @@ ReturnValue_t PlocMPSoCHandler::executeAction(ActionId_t actionId, MessageQueueI } mpsoc::FlashWritePusCmd flashWritePusCmd; result = flashWritePusCmd.extractFields(data, size); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return result; } result = plocMPSoCHelper->startFlashWrite(flashWritePusCmd.getObcFile(), flashWritePusCmd.getMPSoCFile()); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return result; } plocMPSoCHelperExecuting = true; @@ -209,7 +209,7 @@ ReturnValue_t PlocMPSoCHandler::buildCommandFromCommand(DeviceCommandId_t device const uint8_t* commandData, size_t commandDataLen) { spParams.buf = commandBuffer; - ReturnValue_t result = RETURN_OK; + ReturnValue_t result = returnvalue::OK; switch (deviceCommand) { case (mpsoc::TC_MEM_WRITE): { result = prepareTcMemWrite(commandData, commandDataLen); @@ -262,7 +262,7 @@ ReturnValue_t PlocMPSoCHandler::buildCommandFromCommand(DeviceCommandId_t device break; } - if (result == RETURN_OK) { + if (result == returnvalue::OK) { /** * Flushing the receive buffer to make sure there are no data left from a faulty reply. */ @@ -294,15 +294,15 @@ void PlocMPSoCHandler::fillCommandAndReplyMap() { ReturnValue_t PlocMPSoCHandler::scanForReply(const uint8_t* start, size_t remainingSize, DeviceCommandId_t* foundId, size_t* foundLen) { - ReturnValue_t result = RETURN_OK; + ReturnValue_t result = returnvalue::OK; SpacePacketReader spacePacket; spacePacket.setReadOnlyData(start, remainingSize); if (spacePacket.isNull()) { - return RETURN_FAILED; + return returnvalue::FAILED; } auto res = spacePacket.checkSize(); - if (res != RETURN_OK) { + if (res != returnvalue::OK) { return res; } uint16_t apid = spacePacket.getApid(); @@ -350,7 +350,7 @@ ReturnValue_t PlocMPSoCHandler::scanForReply(const uint8_t* start, size_t remain } ReturnValue_t PlocMPSoCHandler::interpretDeviceReply(DeviceCommandId_t id, const uint8_t* packet) { - ReturnValue_t result = RETURN_OK; + ReturnValue_t result = returnvalue::OK; switch (id) { case mpsoc::ACK_REPORT: { @@ -384,7 +384,7 @@ uint32_t PlocMPSoCHandler::getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) ReturnValue_t PlocMPSoCHandler::initializeLocalDataPool(localpool::DataPool& localDataPoolMap, LocalDataPoolManager& poolManager) { - return HasReturnvaluesIF::RETURN_OK; + return returnvalue::OK; } void PlocMPSoCHandler::handleEvent(EventMessage* eventMessage) { @@ -402,31 +402,31 @@ void PlocMPSoCHandler::handleEvent(EventMessage* eventMessage) { ReturnValue_t PlocMPSoCHandler::prepareTcMemWrite(const uint8_t* commandData, size_t commandDataLen) { - ReturnValue_t result = RETURN_OK; + ReturnValue_t result = returnvalue::OK; sequenceCount++; mpsoc::TcMemWrite tcMemWrite(spParams, sequenceCount); result = tcMemWrite.buildPacket(commandData, commandDataLen); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { sequenceCount--; return result; } finishTcPrep(tcMemWrite.getFullPacketLen()); - return RETURN_OK; + return returnvalue::OK; } ReturnValue_t PlocMPSoCHandler::prepareTcMemRead(const uint8_t* commandData, size_t commandDataLen) { - ReturnValue_t result = RETURN_OK; + ReturnValue_t result = returnvalue::OK; sequenceCount++; mpsoc::TcMemRead tcMemRead(spParams, sequenceCount); result = tcMemRead.buildPacket(commandData, commandDataLen); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { sequenceCount--; return result; } finishTcPrep(tcMemRead.getFullPacketLen()); tmMemReadReport.rememberRequestedSize = tcMemRead.getMemLen() * 4 + TmMemReadReport::FIX_SIZE; - return RETURN_OK; + return returnvalue::OK; } ReturnValue_t PlocMPSoCHandler::prepareTcFlashDelete(const uint8_t* commandData, @@ -434,126 +434,126 @@ ReturnValue_t PlocMPSoCHandler::prepareTcFlashDelete(const uint8_t* commandData, if (commandDataLen > config::MAX_PATH_SIZE + config::MAX_FILENAME_SIZE) { return MPSoCReturnValuesIF::NAME_TOO_LONG; } - ReturnValue_t result = RETURN_OK; + ReturnValue_t result = returnvalue::OK; sequenceCount++; mpsoc::TcFlashDelete tcFlashDelete(spParams, sequenceCount); result = tcFlashDelete.buildPacket( std::string(reinterpret_cast(commandData), commandDataLen)); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { sequenceCount--; return result; } finishTcPrep(tcFlashDelete.getFullPacketLen()); - return RETURN_OK; + return returnvalue::OK; } ReturnValue_t PlocMPSoCHandler::prepareTcReplayStart(const uint8_t* commandData, size_t commandDataLen) { - ReturnValue_t result = RETURN_OK; + ReturnValue_t result = returnvalue::OK; sequenceCount++; mpsoc::TcReplayStart tcReplayStart(spParams, sequenceCount); result = tcReplayStart.buildPacket(commandData, commandDataLen); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { sequenceCount--; return result; } finishTcPrep(tcReplayStart.getFullPacketLen()); - return RETURN_OK; + return returnvalue::OK; } ReturnValue_t PlocMPSoCHandler::prepareTcReplayStop() { - ReturnValue_t result = RETURN_OK; + ReturnValue_t result = returnvalue::OK; sequenceCount++; mpsoc::TcReplayStop tcReplayStop(spParams, sequenceCount); result = tcReplayStop.buildPacket(); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { sequenceCount--; return result; } finishTcPrep(tcReplayStop.getFullPacketLen()); - return RETURN_OK; + return returnvalue::OK; } ReturnValue_t PlocMPSoCHandler::prepareTcDownlinkPwrOn(const uint8_t* commandData, size_t commandDataLen) { - ReturnValue_t result = RETURN_OK; + ReturnValue_t result = returnvalue::OK; sequenceCount++; mpsoc::TcDownlinkPwrOn tcDownlinkPwrOn(spParams, sequenceCount); result = tcDownlinkPwrOn.buildPacket(commandData, commandDataLen); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { sequenceCount--; return result; } finishTcPrep(tcDownlinkPwrOn.getFullPacketLen()); - return RETURN_OK; + return returnvalue::OK; } ReturnValue_t PlocMPSoCHandler::prepareTcDownlinkPwrOff() { - ReturnValue_t result = RETURN_OK; + ReturnValue_t result = returnvalue::OK; sequenceCount++; mpsoc::TcDownlinkPwrOff tcDownlinkPwrOff(spParams, sequenceCount); result = tcDownlinkPwrOff.buildPacket(); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { sequenceCount--; return result; } finishTcPrep(tcDownlinkPwrOff.getFullPacketLen()); - return RETURN_OK; + return returnvalue::OK; } ReturnValue_t PlocMPSoCHandler::prepareTcReplayWriteSequence(const uint8_t* commandData, size_t commandDataLen) { - ReturnValue_t result = RETURN_OK; + ReturnValue_t result = returnvalue::OK; sequenceCount++; mpsoc::TcReplayWriteSeq tcReplayWriteSeq(spParams, sequenceCount); result = tcReplayWriteSeq.buildPacket(commandData, commandDataLen); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { sequenceCount--; return result; } finishTcPrep(tcReplayWriteSeq.getFullPacketLen()); - return RETURN_OK; + return returnvalue::OK; } ReturnValue_t PlocMPSoCHandler::prepareTcModeReplay() { - ReturnValue_t result = RETURN_OK; + ReturnValue_t result = returnvalue::OK; sequenceCount++; mpsoc::TcModeReplay tcModeReplay(spParams, sequenceCount); result = tcModeReplay.buildPacket(); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { sequenceCount--; return result; } finishTcPrep(tcModeReplay.getFullPacketLen()); - return RETURN_OK; + return returnvalue::OK; } ReturnValue_t PlocMPSoCHandler::prepareTcModeIdle() { - ReturnValue_t result = RETURN_OK; + ReturnValue_t result = returnvalue::OK; sequenceCount++; mpsoc::TcModeIdle tcModeIdle(spParams, sequenceCount); result = tcModeIdle.buildPacket(); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { sequenceCount--; return result; } finishTcPrep(tcModeIdle.getFullPacketLen()); - return RETURN_OK; + return returnvalue::OK; } ReturnValue_t PlocMPSoCHandler::prepareTcCamCmdSend(const uint8_t* commandData, size_t commandDataLen) { - ReturnValue_t result = RETURN_OK; + ReturnValue_t result = returnvalue::OK; sequenceCount++; mpsoc::TcCamcmdSend tcCamCmdSend(spParams, sequenceCount); result = tcCamCmdSend.buildPacket(commandData, commandDataLen); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { sequenceCount--; return result; } finishTcPrep(tcCamCmdSend.getFullPacketLen()); nextReplyId = mpsoc::TM_CAM_CMD_RPT; - return RETURN_OK; + return returnvalue::OK; } void PlocMPSoCHandler::finishTcPrep(size_t packetLen) { @@ -566,11 +566,11 @@ ReturnValue_t PlocMPSoCHandler::verifyPacket(const uint8_t* start, size_t foundL if (CRC::crc16ccitt(start, foundLen) != 0) { return MPSoCReturnValuesIF::CRC_FAILURE; } - return RETURN_OK; + return returnvalue::OK; } ReturnValue_t PlocMPSoCHandler::handleAckReport(const uint8_t* data) { - ReturnValue_t result = RETURN_OK; + ReturnValue_t result = returnvalue::OK; result = verifyPacket(data, mpsoc::SIZE_ACK_REPORT); if (result == MPSoCReturnValuesIF::CRC_FAILURE) { @@ -606,7 +606,7 @@ ReturnValue_t PlocMPSoCHandler::handleAckReport(const uint8_t* data) { } default: { sif::debug << "PlocMPSoCHandler::handleAckReport: Invalid APID in Ack report" << std::endl; - result = RETURN_FAILED; + result = returnvalue::FAILED; break; } } @@ -615,7 +615,7 @@ ReturnValue_t PlocMPSoCHandler::handleAckReport(const uint8_t* data) { } ReturnValue_t PlocMPSoCHandler::handleExecutionReport(const uint8_t* data) { - ReturnValue_t result = RETURN_OK; + ReturnValue_t result = returnvalue::OK; result = verifyPacket(data, mpsoc::SIZE_EXE_REPORT); if (result == MPSoCReturnValuesIF::CRC_FAILURE) { @@ -649,7 +649,7 @@ ReturnValue_t PlocMPSoCHandler::handleExecutionReport(const uint8_t* data) { } default: { sif::warning << "PlocMPSoCHandler::handleExecutionReport: Unknown APID" << std::endl; - result = RETURN_FAILED; + result = returnvalue::FAILED; break; } } @@ -658,7 +658,7 @@ ReturnValue_t PlocMPSoCHandler::handleExecutionReport(const uint8_t* data) { } ReturnValue_t PlocMPSoCHandler::handleMemoryReadReport(const uint8_t* data) { - ReturnValue_t result = RETURN_OK; + ReturnValue_t result = returnvalue::OK; result = verifyPacket(data, tmMemReadReport.rememberRequestedSize); if (result == MPSoCReturnValuesIF::CRC_FAILURE) { sif::warning << "PlocMPSoCHandler::handleMemoryReadReport: Memory read report has invalid crc" @@ -674,7 +674,7 @@ ReturnValue_t PlocMPSoCHandler::handleMemoryReadReport(const uint8_t* data) { } ReturnValue_t PlocMPSoCHandler::handleCamCmdRpt(const uint8_t* data) { - ReturnValue_t result = RETURN_OK; + ReturnValue_t result = returnvalue::OK; result = verifyPacket(data, tmCamCmdRpt.rememberSpacePacketSize); if (result == MPSoCReturnValuesIF::CRC_FAILURE) { sif::warning << "PlocMPSoCHandler::handleCamCmdRpt: CRC failure" << std::endl; @@ -698,7 +698,7 @@ ReturnValue_t PlocMPSoCHandler::handleCamCmdRpt(const uint8_t* data) { ReturnValue_t PlocMPSoCHandler::enableReplyInReplyMap(DeviceCommandMap::iterator command, uint8_t expectedReplies, bool useAlternateId, DeviceCommandId_t alternateReplyID) { - ReturnValue_t result = RETURN_OK; + ReturnValue_t result = returnvalue::OK; uint8_t enabledReplies = 0; @@ -718,7 +718,7 @@ ReturnValue_t PlocMPSoCHandler::enableReplyInReplyMap(DeviceCommandMap::iterator enabledReplies = 3; result = DeviceHandlerBase::enableReplyInReplyMap(command, enabledReplies, true, mpsoc::TM_MEMORY_READ_REPORT); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { sif::debug << "PlocMPSoCHandler::enableReplyInReplyMap: Reply with id " << mpsoc::TM_MEMORY_READ_REPORT << " not in replyMap" << std::endl; return result; @@ -729,7 +729,7 @@ ReturnValue_t PlocMPSoCHandler::enableReplyInReplyMap(DeviceCommandMap::iterator enabledReplies = 3; result = DeviceHandlerBase::enableReplyInReplyMap(command, enabledReplies, true, mpsoc::TM_CAM_CMD_RPT); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { sif::debug << "PlocMPSoCHandler::enableReplyInReplyMap: Reply with id " << mpsoc::TM_CAM_CMD_RPT << " not in replyMap" << std::endl; return result; @@ -749,14 +749,14 @@ ReturnValue_t PlocMPSoCHandler::enableReplyInReplyMap(DeviceCommandMap::iterator */ result = DeviceHandlerBase::enableReplyInReplyMap(command, enabledReplies, true, mpsoc::ACK_REPORT); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { sif::debug << "PlocMPSoCHandler::enableReplyInReplyMap: Reply with id " << mpsoc::ACK_REPORT << " not in replyMap" << std::endl; } result = DeviceHandlerBase::enableReplyInReplyMap(command, enabledReplies, true, mpsoc::EXE_REPORT); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { sif::debug << "PlocMPSoCHandler::enableReplyInReplyMap: Reply with id " << mpsoc::EXE_REPORT << " not in replyMap" << std::endl; } @@ -779,7 +779,7 @@ ReturnValue_t PlocMPSoCHandler::enableReplyInReplyMap(DeviceCommandMap::iterator break; } - return RETURN_OK; + return returnvalue::OK; } void PlocMPSoCHandler::setNextReplyId() { @@ -833,9 +833,9 @@ size_t PlocMPSoCHandler::getNextReplyLength(DeviceCommandId_t commandId) { ReturnValue_t PlocMPSoCHandler::doSendReadHook() { // Prevent DHB from polling UART during commands executed by the mpsoc helper task if (plocMPSoCHelperExecuting) { - return RETURN_FAILED; + return returnvalue::FAILED; } - return RETURN_OK; + return returnvalue::OK; } MessageQueueIF* PlocMPSoCHandler::getCommandQueuePtr() { return commandActionHelperQueue; } @@ -896,7 +896,7 @@ void PlocMPSoCHandler::completionFailedReceived(ActionId_t actionId, ReturnValue void PlocMPSoCHandler::handleDeviceTM(const uint8_t* data, size_t dataSize, DeviceCommandId_t replyId) { - ReturnValue_t result = RETURN_OK; + ReturnValue_t result = returnvalue::OK; if (wiretappingMode == RAW) { /* Data already sent in doGetRead() */ @@ -915,7 +915,7 @@ void PlocMPSoCHandler::handleDeviceTM(const uint8_t* data, size_t dataSize, } result = actionHelper.reportData(queueId, replyId, data, dataSize); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { sif::debug << "PlocMPSoCHandler::handleDeviceTM: Failed to report data" << std::endl; } } diff --git a/linux/devices/ploc/PlocMPSoCHandler.h b/linux/devices/ploc/PlocMPSoCHandler.h index 05c2e902..fca65c7b 100644 --- a/linux/devices/ploc/PlocMPSoCHandler.h +++ b/linux/devices/ploc/PlocMPSoCHandler.h @@ -180,7 +180,7 @@ class PlocMPSoCHandler : public DeviceHandlerBase, public CommandsActionsIF { * @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. + * @return returnvalue::OK if CRC is ok, otherwise CRC_FAILURE. */ ReturnValue_t verifyPacket(const uint8_t* start, size_t foundLen); @@ -189,7 +189,7 @@ class PlocMPSoCHandler : public DeviceHandlerBase, public CommandsActionsIF { * * @param data Pointer to the data holding the acknowledgment report. * - * @return RETURN_OK if successful, otherwise an error code. + * @return returnvalue::OK if successful, otherwise an error code. */ ReturnValue_t handleAckReport(const uint8_t* data); @@ -198,7 +198,7 @@ class PlocMPSoCHandler : public DeviceHandlerBase, public CommandsActionsIF { * * @param data Pointer to the received data packet. * - * @return RETURN_OK if successful, otherwise an error code. + * @return returnvalue::OK if successful, otherwise an error code. */ ReturnValue_t handleExecutionReport(const uint8_t* data); @@ -207,7 +207,7 @@ class PlocMPSoCHandler : public DeviceHandlerBase, public CommandsActionsIF { * * @param data Pointer to the data buffer holding the memory read report. * - * @return RETURN_OK if successful, otherwise an error code. + * @return returnvalue::OK if successful, otherwise an error code. */ ReturnValue_t handleMemoryReadReport(const uint8_t* data); diff --git a/linux/devices/ploc/PlocMPSoCHelper.cpp b/linux/devices/ploc/PlocMPSoCHelper.cpp index e9a65d64..965c63e6 100644 --- a/linux/devices/ploc/PlocMPSoCHelper.cpp +++ b/linux/devices/ploc/PlocMPSoCHelper.cpp @@ -24,14 +24,14 @@ ReturnValue_t PlocMPSoCHelper::initialize() { sdcMan = SdCardManager::instance(); if (sdcMan == nullptr) { sif::warning << "PlocMPSoCHelper::initialize: Invalid SD Card Manager" << std::endl; - return RETURN_FAILED; + return returnvalue::FAILED; } #endif - return RETURN_OK; + return returnvalue::OK; } ReturnValue_t PlocMPSoCHelper::performOperation(uint8_t operationCode) { - ReturnValue_t result = RETURN_OK; + ReturnValue_t result = returnvalue::OK; semaphore.acquire(); while (true) { switch (internalState) { @@ -41,7 +41,7 @@ ReturnValue_t PlocMPSoCHelper::performOperation(uint8_t operationCode) { } case InternalState::FLASH_WRITE: { result = performFlashWrite(); - if (result == RETURN_OK) { + if (result == returnvalue::OK) { triggerEvent(MPSOC_FLASH_WRITE_SUCCESSFUL); } else { triggerEvent(MPSOC_FLASH_WRITE_FAILED); @@ -60,9 +60,9 @@ ReturnValue_t PlocMPSoCHelper::setComIF(DeviceCommunicationIF* communicationInte uartComIF = dynamic_cast(communicationInterface_); if (uartComIF == nullptr) { sif::warning << "PlocMPSoCHelper::initialize: Invalid uart com if" << std::endl; - return RETURN_FAILED; + return returnvalue::FAILED; } - return RETURN_OK; + return returnvalue::OK; } void PlocMPSoCHelper::setComCookie(CookieIF* comCookie_) { comCookie = comCookie_; } @@ -72,14 +72,14 @@ void PlocMPSoCHelper::setSequenceCount(SourceSequenceCounter* sequenceCount_) { } ReturnValue_t PlocMPSoCHelper::startFlashWrite(std::string obcFile, std::string mpsocFile) { - ReturnValue_t result = RETURN_OK; + ReturnValue_t result = returnvalue::OK; #ifdef XIPHOS_Q7S result = FilesystemHelper::checkPath(obcFile); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return result; } result = FilesystemHelper::fileExists(mpsocFile); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return result; } #endif @@ -87,7 +87,7 @@ ReturnValue_t PlocMPSoCHelper::startFlashWrite(std::string obcFile, std::string if (not std::filesystem::exists(obcFile)) { sif::warning << "PlocMPSoCHelper::startFlashWrite: File " << obcFile << "does not exist" << std::endl; - return RETURN_FAILED; + return returnvalue::FAILED; } #endif @@ -95,14 +95,14 @@ ReturnValue_t PlocMPSoCHelper::startFlashWrite(std::string obcFile, std::string flashWrite.mpsocFile = mpsocFile; internalState = InternalState::FLASH_WRITE; result = resetHelper(); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return result; } return result; } ReturnValue_t PlocMPSoCHelper::resetHelper() { - ReturnValue_t result = RETURN_OK; + ReturnValue_t result = returnvalue::OK; semaphore.release(); spParams.buf = commandBuffer; terminate = false; @@ -113,9 +113,9 @@ ReturnValue_t PlocMPSoCHelper::resetHelper() { void PlocMPSoCHelper::stopProcess() { terminate = true; } ReturnValue_t PlocMPSoCHelper::performFlashWrite() { - ReturnValue_t result = RETURN_OK; + ReturnValue_t result = returnvalue::OK; result = flashfopen(); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return result; } uint8_t tempData[mpsoc::MAX_DATA_SIZE]; @@ -128,7 +128,7 @@ ReturnValue_t PlocMPSoCHelper::performFlashWrite() { size_t bytesRead = 0; while (remainingSize > 0) { if (terminate) { - return RETURN_OK; + return returnvalue::OK; } if (remainingSize > mpsoc::MAX_DATA_SIZE) { dataLength = mpsoc::MAX_DATA_SIZE; @@ -146,74 +146,74 @@ ReturnValue_t PlocMPSoCHelper::performFlashWrite() { (*sequenceCount)++; mpsoc::TcFlashWrite tc(spParams, *sequenceCount); result = tc.buildPacket(tempData, dataLength); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return result; } result = handlePacketTransmission(tc); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return result; } } result = flashfclose(); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return result; } return result; } ReturnValue_t PlocMPSoCHelper::flashfopen() { - ReturnValue_t result = RETURN_OK; + ReturnValue_t result = returnvalue::OK; spParams.buf = commandBuffer; (*sequenceCount)++; mpsoc::FlashFopen flashFopen(spParams, *sequenceCount); result = flashFopen.createPacket(flashWrite.mpsocFile, mpsoc::FlashFopen::APPEND); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return result; } result = handlePacketTransmission(flashFopen); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return result; } - return RETURN_OK; + return returnvalue::OK; } ReturnValue_t PlocMPSoCHelper::flashfclose() { - ReturnValue_t result = RETURN_OK; + ReturnValue_t result = returnvalue::OK; spParams.buf = commandBuffer; (*sequenceCount)++; mpsoc::FlashFclose flashFclose(spParams, *sequenceCount); result = flashFclose.createPacket(flashWrite.mpsocFile); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return result; } result = handlePacketTransmission(flashFclose); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return result; } - return RETURN_OK; + return returnvalue::OK; } ReturnValue_t PlocMPSoCHelper::handlePacketTransmission(ploc::SpTcBase& tc) { - ReturnValue_t result = RETURN_OK; + ReturnValue_t result = returnvalue::OK; result = sendCommand(tc); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return result; } result = handleAck(); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return result; } result = handleExe(); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return result; } - return RETURN_OK; + return returnvalue::OK; } ReturnValue_t PlocMPSoCHelper::sendCommand(ploc::SpTcBase& tc) { - ReturnValue_t result = RETURN_OK; + ReturnValue_t result = returnvalue::OK; result = uartComIF->sendMessage(comCookie, tc.getFullPacket(), tc.getFullPacketLen()); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { sif::warning << "PlocMPSoCHelper::sendCommand: Failed to send command" << std::endl; triggerEvent(MPSOC_SENDING_COMMAND_FAILED, result, static_cast(internalState)); return result; @@ -222,22 +222,22 @@ ReturnValue_t PlocMPSoCHelper::sendCommand(ploc::SpTcBase& tc) { } ReturnValue_t PlocMPSoCHelper::handleAck() { - ReturnValue_t result = RETURN_OK; + ReturnValue_t result = returnvalue::OK; result = handleTmReception(mpsoc::SIZE_ACK_REPORT); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return result; } SpTmReader tmPacket(tmBuf.data(), tmBuf.size()); result = checkReceivedTm(tmPacket); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return result; } uint16_t apid = tmPacket.getApid(); if (apid != mpsoc::apid::ACK_SUCCESS) { handleAckApidFailure(apid); - return RETURN_FAILED; + return returnvalue::FAILED; } - return RETURN_OK; + return returnvalue::OK; } void PlocMPSoCHelper::handleAckApidFailure(uint16_t apid) { @@ -253,23 +253,23 @@ void PlocMPSoCHelper::handleAckApidFailure(uint16_t apid) { } ReturnValue_t PlocMPSoCHelper::handleExe() { - ReturnValue_t result = RETURN_OK; + ReturnValue_t result = returnvalue::OK; result = handleTmReception(mpsoc::SIZE_EXE_REPORT); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return result; } ploc::SpTmReader tmPacket(tmBuf.data(), tmBuf.size()); result = checkReceivedTm(tmPacket); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return result; } uint16_t apid = tmPacket.getApid(); if (apid != mpsoc::apid::EXE_SUCCESS) { handleExeApidFailure(apid); - return RETURN_FAILED; + return returnvalue::FAILED; } - return RETURN_OK; + return returnvalue::OK; } void PlocMPSoCHelper::handleExeApidFailure(uint16_t apid) { @@ -285,12 +285,12 @@ void PlocMPSoCHelper::handleExeApidFailure(uint16_t apid) { } ReturnValue_t PlocMPSoCHelper::handleTmReception(size_t remainingBytes) { - ReturnValue_t result = RETURN_OK; + ReturnValue_t result = returnvalue::OK; size_t readBytes = 0; size_t currentBytes = 0; for (int retries = 0; retries < RETRIES; retries++) { result = receive(tmBuf.data() + readBytes, ¤tBytes, remainingBytes); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return result; } readBytes += currentBytes; @@ -302,21 +302,21 @@ ReturnValue_t PlocMPSoCHelper::handleTmReception(size_t remainingBytes) { if (remainingBytes != 0) { sif::warning << "PlocMPSoCHelper::handleTmReception: Failed to receive reply" << std::endl; triggerEvent(MPSOC_MISSING_EXE, remainingBytes, static_cast(internalState)); - return RETURN_FAILED; + return returnvalue::FAILED; } return result; } ReturnValue_t PlocMPSoCHelper::checkReceivedTm(SpTmReader& reader) { ReturnValue_t result = reader.checkSize(); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { sif::error << "PlocMPSoCHelper::handleTmReception: Size check on received TM failed" << std::endl; triggerEvent(MPSOC_TM_SIZE_ERROR); return result; } reader.checkCrc(); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { sif::warning << "PlocMPSoCHelper::handleTmReception: CRC check failed" << std::endl; triggerEvent(MPSOC_TM_CRC_MISSMATCH, *sequenceCount); return result; @@ -327,24 +327,24 @@ ReturnValue_t PlocMPSoCHelper::checkReceivedTm(SpTmReader& reader) { triggerEvent(MPSOC_HELPER_SEQ_CNT_MISMATCH, *sequenceCount, recvSeqCnt); *sequenceCount = recvSeqCnt; } - return RETURN_OK; + return returnvalue::OK; } ReturnValue_t PlocMPSoCHelper::receive(uint8_t* data, size_t* readBytes, size_t requestBytes) { - ReturnValue_t result = RETURN_OK; + ReturnValue_t result = returnvalue::OK; uint8_t* buffer = nullptr; result = uartComIF->requestReceiveMessage(comCookie, requestBytes); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { sif::warning << "PlocMPSoCHelper::receive: Failed to request reply" << std::endl; triggerEvent(MPSOC_HELPER_REQUESTING_REPLY_FAILED, result, static_cast(static_cast(internalState))); - return RETURN_FAILED; + return returnvalue::FAILED; } result = uartComIF->readReceivedMessage(comCookie, &buffer, readBytes); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { sif::warning << "PlocMPSoCHelper::receive: Failed to read received message" << std::endl; triggerEvent(MPSOC_HELPER_READING_REPLY_FAILED, result, static_cast(internalState)); - return RETURN_FAILED; + return returnvalue::FAILED; } if (*readBytes > 0) { std::memcpy(data, buffer, *readBytes); diff --git a/linux/devices/ploc/PlocMPSoCHelper.h b/linux/devices/ploc/PlocMPSoCHelper.h index eb5b4346..3adffa77 100644 --- a/linux/devices/ploc/PlocMPSoCHelper.h +++ b/linux/devices/ploc/PlocMPSoCHelper.h @@ -6,7 +6,7 @@ #include "fsfw/devicehandlers/CookieIF.h" #include "fsfw/objectmanager/SystemObject.h" #include "fsfw/osal/linux/BinarySemaphore.h" -#include "fsfw/returnvalues/HasReturnvaluesIF.h" +#include "fsfw/returnvalues/returnvalue.h" #include "fsfw/tasks/ExecutableObjectIF.h" #include "fsfw/tmtcservices/SourceSequenceCounter.h" #include "fsfw_hal/linux/uart/UartComIF.h" @@ -20,7 +20,7 @@ * MPSoC and OBC. * @author J. Meier */ -class PlocMPSoCHelper : public SystemObject, public ExecutableObjectIF, public HasReturnvaluesIF { +class PlocMPSoCHelper : public SystemObject, public ExecutableObjectIF { public: static const uint8_t SUBSYSTEM_ID = SUBSYSTEM_ID::PLOC_MPSOC_HELPER; @@ -85,7 +85,7 @@ class PlocMPSoCHelper : public SystemObject, public ExecutableObjectIF, public H * @param obcFile File where to read from the data * @param mpsocFile The file of the MPSoC where should be written to * - * @return RETURN_OK if successful, otherwise error return value + * @return returnvalue::OK if successful, otherwise error return value */ ReturnValue_t startFlashWrite(std::string obcFile, std::string mpsocFile); diff --git a/linux/devices/ploc/PlocMemoryDumper.cpp b/linux/devices/ploc/PlocMemoryDumper.cpp index e1fce10e..a5cd04c4 100644 --- a/linux/devices/ploc/PlocMemoryDumper.cpp +++ b/linux/devices/ploc/PlocMemoryDumper.cpp @@ -19,25 +19,25 @@ PlocMemoryDumper::~PlocMemoryDumper() {} ReturnValue_t PlocMemoryDumper::initialize() { ReturnValue_t result = SystemObject::initialize(); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { return result; } result = commandActionHelper.initialize(); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { return result; } result = actionHelper.initialize(commandQueue); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { return result; } - return HasReturnvaluesIF::RETURN_OK; + return returnvalue::OK; } ReturnValue_t PlocMemoryDumper::performOperation(uint8_t operationCode) { readCommandQueue(); doStateMachine(); - return HasReturnvaluesIF::RETURN_OK; + return returnvalue::OK; } ReturnValue_t PlocMemoryDumper::executeAction(ActionId_t actionId, MessageQueueId_t commandedBy, @@ -78,20 +78,20 @@ MessageQueueIF* PlocMemoryDumper::getCommandQueuePtr() { return commandQueue; } void PlocMemoryDumper::readCommandQueue() { CommandMessage message; - ReturnValue_t result = RETURN_OK; + ReturnValue_t result = returnvalue::OK; - for (result = commandQueue->receiveMessage(&message); result == HasReturnvaluesIF::RETURN_OK; + for (result = commandQueue->receiveMessage(&message); result == returnvalue::OK; result = commandQueue->receiveMessage(&message)) { - if (result != RETURN_OK) { + if (result != returnvalue::OK) { continue; } result = actionHelper.handleActionMessage(&message); - if (result == HasReturnvaluesIF::RETURN_OK) { + if (result == returnvalue::OK) { continue; } result = commandActionHelper.handleReply(&message); - if (result == HasReturnvaluesIF::RETURN_OK) { + if (result == returnvalue::OK) { continue; } @@ -161,7 +161,7 @@ void PlocMemoryDumper::completionFailedReceived(ActionId_t actionId, ReturnValue } void PlocMemoryDumper::commandNextMramDump(ActionId_t dumpCommand) { - ReturnValue_t result = RETURN_OK; + ReturnValue_t result = returnvalue::OK; uint32_t tempStartAddress = 0; uint32_t tempEndAddress = 0; @@ -181,7 +181,7 @@ void PlocMemoryDumper::commandNextMramDump(ActionId_t dumpCommand) { result = commandActionHelper.commandAction(objects::PLOC_SUPERVISOR_HANDLER, dumpCommand, ¶ms); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { sif::warning << "PlocMemoryDumper::commandNextMramDump: Failed to send mram dump command " << "with start address " << tempStartAddress << " and end address " << tempEndAddress << std::endl; diff --git a/linux/devices/ploc/PlocMemoryDumper.h b/linux/devices/ploc/PlocMemoryDumper.h index a1b4f8de..b41ee420 100644 --- a/linux/devices/ploc/PlocMemoryDumper.h +++ b/linux/devices/ploc/PlocMemoryDumper.h @@ -11,7 +11,7 @@ #include "fsfw/action/CommandsActionsIF.h" #include "fsfw/action/HasActionsIF.h" #include "fsfw/objectmanager/SystemObject.h" -#include "fsfw/returnvalues/HasReturnvaluesIF.h" +#include "fsfw/returnvalues/returnvalue.h" #include "fsfw/tasks/ExecutableObjectIF.h" #include "linux/fsfwconfig/objects/systemObjectList.h" @@ -26,7 +26,6 @@ class PlocMemoryDumper : public SystemObject, public HasActionsIF, public ExecutableObjectIF, - public HasReturnvaluesIF, public CommandsActionsIF { public: static const ActionId_t NONE = 0; diff --git a/linux/devices/ploc/PlocSupervisorHandler.cpp b/linux/devices/ploc/PlocSupervisorHandler.cpp index 040fa1de..f683998d 100644 --- a/linux/devices/ploc/PlocSupervisorHandler.cpp +++ b/linux/devices/ploc/PlocSupervisorHandler.cpp @@ -41,9 +41,9 @@ PlocSupervisorHandler::PlocSupervisorHandler(object_id_t objectId, object_id_t u PlocSupervisorHandler::~PlocSupervisorHandler() {} ReturnValue_t PlocSupervisorHandler::initialize() { - ReturnValue_t result = RETURN_OK; + ReturnValue_t result = returnvalue::OK; result = DeviceHandlerBase::initialize(); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return result; } uartComIf = dynamic_cast(communicationInterface); @@ -59,13 +59,13 @@ ReturnValue_t PlocSupervisorHandler::initialize() { return ObjectManagerIF::CHILD_INIT_FAILED; } result = supvHelper->setComIF(uartComIf); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return ObjectManagerIF::CHILD_INIT_FAILED; } supvHelper->setComCookie(comCookie); result = eventSubscription(); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return result; } return result; @@ -73,7 +73,7 @@ ReturnValue_t PlocSupervisorHandler::initialize() { void PlocSupervisorHandler::performOperationHook() { EventMessage event; - for (ReturnValue_t result = eventQueue->receiveMessage(&event); result == RETURN_OK; + for (ReturnValue_t result = eventQueue->receiveMessage(&event); result == returnvalue::OK; result = eventQueue->receiveMessage(&event)) { switch (event.getMessageId()) { case EventMessage::EVENT_MESSAGE: @@ -91,7 +91,7 @@ ReturnValue_t PlocSupervisorHandler::executeAction(ActionId_t actionId, MessageQueueId_t commandedBy, const uint8_t* data, size_t size) { using namespace supv; - ReturnValue_t result = RETURN_OK; + ReturnValue_t result = returnvalue::OK; switch (actionId) { case TERMINATE_SUPV_HELPER: { @@ -107,7 +107,7 @@ ReturnValue_t PlocSupervisorHandler::executeAction(ActionId_t actionId, } result = acceptExternalDeviceCommands(); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { return result; } @@ -118,11 +118,11 @@ ReturnValue_t PlocSupervisorHandler::executeAction(ActionId_t actionId, } UpdateParams params; result = extractUpdateCommand(data, size, params); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return result; } result = supvHelper->performUpdate(params); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return result; } plocSupvHelperExecuting = true; @@ -136,7 +136,7 @@ ReturnValue_t PlocSupervisorHandler::executeAction(ActionId_t actionId, case MEMORY_CHECK_WITH_FILE: { UpdateParams params; ReturnValue_t result = extractBaseParams(&data, size, params); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { return result; } if (not std::filesystem::exists(params.file)) { @@ -152,7 +152,7 @@ ReturnValue_t PlocSupervisorHandler::executeAction(ActionId_t actionId, } result = supvHelper->startEventbBufferRequest( std::string(reinterpret_cast(data), size)); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return result; } plocSupvHelperExecuting = true; @@ -217,32 +217,32 @@ ReturnValue_t PlocSupervisorHandler::buildCommandFromCommand(DeviceCommandId_t d const uint8_t* commandData, size_t commandDataLen) { using namespace supv; - ReturnValue_t result = RETURN_FAILED; + ReturnValue_t result = returnvalue::FAILED; spParams.buf = commandBuffer; switch (deviceCommand) { case GET_HK_REPORT: { prepareEmptyCmd(APID_GET_HK_REPORT); - result = RETURN_OK; + result = returnvalue::OK; break; } case START_MPSOC: { prepareEmptyCmd(APID_START_MPSOC); - result = RETURN_OK; + result = returnvalue::OK; break; } case SHUTDOWN_MPSOC: { prepareEmptyCmd(APID_SHUTWOWN_MPSOC); - result = RETURN_OK; + result = returnvalue::OK; break; } case SEL_MPSOC_BOOT_IMAGE: { prepareSelBootImageCmd(commandData); - result = RETURN_OK; + result = returnvalue::OK; break; } case RESET_MPSOC: { prepareEmptyCmd(APID_RESET_MPSOC); - result = RETURN_OK; + result = returnvalue::OK; break; } case SET_TIME_REF: { @@ -251,22 +251,22 @@ ReturnValue_t PlocSupervisorHandler::buildCommandFromCommand(DeviceCommandId_t d } case SET_BOOT_TIMEOUT: { prepareSetBootTimeoutCmd(commandData); - result = RETURN_OK; + result = returnvalue::OK; break; } case SET_MAX_RESTART_TRIES: { prepareRestartTriesCmd(commandData); - result = RETURN_OK; + result = returnvalue::OK; break; } case DISABLE_PERIOIC_HK_TRANSMISSION: { prepareDisableHk(); - result = RETURN_OK; + result = returnvalue::OK; break; } case GET_BOOT_STATUS_REPORT: { prepareEmptyCmd(APID_GET_BOOT_STATUS_RPT); - result = RETURN_OK; + result = returnvalue::OK; break; } case ENABLE_LATCHUP_ALERT: { @@ -283,32 +283,32 @@ ReturnValue_t PlocSupervisorHandler::buildCommandFromCommand(DeviceCommandId_t d } case SET_ADC_ENABLED_CHANNELS: { prepareSetAdcEnabledChannelsCmd(commandData); - result = RETURN_OK; + result = returnvalue::OK; break; } case SET_ADC_WINDOW_AND_STRIDE: { prepareSetAdcWindowAndStrideCmd(commandData); - result = RETURN_OK; + result = returnvalue::OK; break; } case SET_ADC_THRESHOLD: { prepareSetAdcThresholdCmd(commandData); - result = RETURN_OK; + result = returnvalue::OK; break; } case GET_LATCHUP_STATUS_REPORT: { prepareEmptyCmd(APID_GET_LATCHUP_STATUS_REPORT); - result = RETURN_OK; + result = returnvalue::OK; break; } case COPY_ADC_DATA_TO_MRAM: { prepareEmptyCmd(APID_COPY_ADC_DATA_TO_MRAM); - result = RETURN_OK; + result = returnvalue::OK; break; } case REQUEST_ADC_REPORT: { prepareEmptyCmd(APID_REQUEST_ADC_REPORT); - result = RETURN_OK; + result = returnvalue::OK; break; } case RUN_AUTO_EM_TESTS: { @@ -325,23 +325,23 @@ ReturnValue_t PlocSupervisorHandler::buildCommandFromCommand(DeviceCommandId_t d break; case SET_GPIO: { prepareSetGpioCmd(commandData); - result = RETURN_OK; + result = returnvalue::OK; break; } case READ_GPIO: { prepareReadGpioCmd(commandData); - result = RETURN_OK; + result = returnvalue::OK; break; } case RESTART_SUPERVISOR: { prepareEmptyCmd(APID_RESTART_SUPERVISOR); - result = RETURN_OK; + result = returnvalue::OK; break; } case FACTORY_RESET_CLEAR_ALL: { FactoryReset packet(spParams); result = packet.buildPacket(FactoryReset::Op::CLEAR_ALL); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { break; } finishTcPrep(packet.getFullPacketLen()); @@ -350,7 +350,7 @@ ReturnValue_t PlocSupervisorHandler::buildCommandFromCommand(DeviceCommandId_t d case FACTORY_RESET_CLEAR_MIRROR: { FactoryReset packet(spParams); result = packet.buildPacket(FactoryReset::Op::MIRROR_ENTRIES); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { break; } finishTcPrep(packet.getFullPacketLen()); @@ -359,7 +359,7 @@ ReturnValue_t PlocSupervisorHandler::buildCommandFromCommand(DeviceCommandId_t d case FACTORY_RESET_CLEAR_CIRCULAR: { FactoryReset packet(spParams); result = packet.buildPacket(FactoryReset::Op::CIRCULAR_ENTRIES); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { break; } finishTcPrep(packet.getFullPacketLen()); @@ -367,23 +367,23 @@ ReturnValue_t PlocSupervisorHandler::buildCommandFromCommand(DeviceCommandId_t d } case START_MPSOC_QUIET: { prepareEmptyCmd(APID_START_MPSOC_QUIET); - result = RETURN_OK; + result = returnvalue::OK; break; } case SET_SHUTDOWN_TIMEOUT: { prepareSetShutdownTimeoutCmd(commandData); - result = RETURN_OK; + result = returnvalue::OK; break; } case FACTORY_FLASH: { prepareEmptyCmd(APID_FACTORY_FLASH); - result = RETURN_OK; + result = returnvalue::OK; break; } case ENABLE_AUTO_TM: { EnableAutoTm packet(spParams); result = packet.buildPacket(); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { break; } finishTcPrep(packet.getFullPacketLen()); @@ -392,7 +392,7 @@ ReturnValue_t PlocSupervisorHandler::buildCommandFromCommand(DeviceCommandId_t d case DISABLE_AUTO_TM: { DisableAutoTm packet(spParams); result = packet.buildPacket(); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { break; } finishTcPrep(packet.getFullPacketLen()); @@ -401,7 +401,7 @@ ReturnValue_t PlocSupervisorHandler::buildCommandFromCommand(DeviceCommandId_t d case LOGGING_REQUEST_COUNTERS: { RequestLoggingData packet(spParams); result = packet.buildPacket(RequestLoggingData::Sa::REQUEST_COUNTERS); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { break; } finishTcPrep(packet.getFullPacketLen()); @@ -410,7 +410,7 @@ ReturnValue_t PlocSupervisorHandler::buildCommandFromCommand(DeviceCommandId_t d case LOGGING_CLEAR_COUNTERS: { RequestLoggingData packet(spParams); result = packet.buildPacket(RequestLoggingData::Sa::CLEAR_COUNTERS); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { break; } finishTcPrep(packet.getFullPacketLen()); @@ -420,7 +420,7 @@ ReturnValue_t PlocSupervisorHandler::buildCommandFromCommand(DeviceCommandId_t d uint8_t tpc = *(commandData); RequestLoggingData packet(spParams); result = packet.buildPacket(RequestLoggingData::Sa::SET_LOGGING_TOPIC, tpc); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { break; } finishTcPrep(packet.getFullPacketLen()); @@ -428,7 +428,7 @@ ReturnValue_t PlocSupervisorHandler::buildCommandFromCommand(DeviceCommandId_t d } case RESET_PL: { prepareEmptyCmd(APID_RESET_PL); - result = RETURN_OK; + result = returnvalue::OK; break; } case ENABLE_NVMS: { @@ -442,7 +442,7 @@ ReturnValue_t PlocSupervisorHandler::buildCommandFromCommand(DeviceCommandId_t d break; } - if (result == RETURN_OK) { + if (result == returnvalue::OK) { /** * Flushing the receive buffer to make sure there are no data left from a faulty reply. */ @@ -510,7 +510,7 @@ ReturnValue_t PlocSupervisorHandler::enableReplyInReplyMap(DeviceCommandMap::ite bool useAlternateId, DeviceCommandId_t alternateReplyID) { using namespace supv; - ReturnValue_t result = RETURN_OK; + ReturnValue_t result = returnvalue::OK; uint8_t enabledReplies = 0; @@ -518,7 +518,7 @@ ReturnValue_t PlocSupervisorHandler::enableReplyInReplyMap(DeviceCommandMap::ite case GET_HK_REPORT: { enabledReplies = 3; result = DeviceHandlerBase::enableReplyInReplyMap(command, enabledReplies, true, HK_REPORT); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { sif::debug << "PlocSupervisorHandler::enableReplyInReplyMap: Reply with id " << HK_REPORT << " not in replyMap" << std::endl; } @@ -528,7 +528,7 @@ ReturnValue_t PlocSupervisorHandler::enableReplyInReplyMap(DeviceCommandMap::ite enabledReplies = 3; result = DeviceHandlerBase::enableReplyInReplyMap(command, enabledReplies, true, BOOT_STATUS_REPORT); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { sif::debug << "PlocSupervisorHandler::enableReplyInReplyMap: Reply with id " << BOOT_STATUS_REPORT << " not in replyMap" << std::endl; } @@ -538,7 +538,7 @@ ReturnValue_t PlocSupervisorHandler::enableReplyInReplyMap(DeviceCommandMap::ite enabledReplies = 3; result = DeviceHandlerBase::enableReplyInReplyMap(command, enabledReplies, true, LATCHUP_REPORT); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { sif::debug << "PlocSupervisorHandler::enableReplyInReplyMap: Reply with id " << LATCHUP_REPORT << " not in replyMap" << std::endl; } @@ -548,7 +548,7 @@ ReturnValue_t PlocSupervisorHandler::enableReplyInReplyMap(DeviceCommandMap::ite enabledReplies = 3; result = DeviceHandlerBase::enableReplyInReplyMap(command, enabledReplies, true, LOGGING_REPORT); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { sif::debug << "PlocSupervisorHandler::enableReplyInReplyMap: Reply with id " << LOGGING_REPORT << " not in replyMap" << std::endl; } @@ -557,7 +557,7 @@ ReturnValue_t PlocSupervisorHandler::enableReplyInReplyMap(DeviceCommandMap::ite case REQUEST_ADC_REPORT: { enabledReplies = 3; result = DeviceHandlerBase::enableReplyInReplyMap(command, enabledReplies, true, ADC_REPORT); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { sif::debug << "PlocSupervisorHandler::enableReplyInReplyMap: Reply with id " << ADC_REPORT << " not in replyMap" << std::endl; } @@ -567,7 +567,7 @@ ReturnValue_t PlocSupervisorHandler::enableReplyInReplyMap(DeviceCommandMap::ite enabledReplies = 2; // expected replies will be increased in handleMramDumpPacket result = DeviceHandlerBase::enableReplyInReplyMap(command, enabledReplies, true, FIRST_MRAM_DUMP); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { sif::debug << "PlocSupervisorHandler::enableReplyInReplyMap: Reply with id " << FIRST_MRAM_DUMP << " not in replyMap" << std::endl; } @@ -577,7 +577,7 @@ ReturnValue_t PlocSupervisorHandler::enableReplyInReplyMap(DeviceCommandMap::ite enabledReplies = 2; // expected replies will be increased in handleMramDumpPacket result = DeviceHandlerBase::enableReplyInReplyMap(command, enabledReplies, true, CONSECUTIVE_MRAM_DUMP); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { sif::debug << "PlocSupervisorHandler::enableReplyInReplyMap: Reply with id " << CONSECUTIVE_MRAM_DUMP << " not in replyMap" << std::endl; } @@ -627,7 +627,7 @@ ReturnValue_t PlocSupervisorHandler::enableReplyInReplyMap(DeviceCommandMap::ite * replies will be enabled here. */ result = DeviceHandlerBase::enableReplyInReplyMap(command, enabledReplies, true, ACK_REPORT); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { sif::debug << "PlocSupervisorHandler::enableReplyInReplyMap: Reply with id " << ACK_REPORT << " not in replyMap" << std::endl; } @@ -635,12 +635,12 @@ ReturnValue_t PlocSupervisorHandler::enableReplyInReplyMap(DeviceCommandMap::ite setExecutionTimeout(command->first); result = DeviceHandlerBase::enableReplyInReplyMap(command, enabledReplies, true, EXE_REPORT); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { sif::debug << "PlocSupervisorHandler::enableReplyInReplyMap: Reply with id " << EXE_REPORT << " not in replyMap" << std::endl; } - return RETURN_OK; + return returnvalue::OK; } ReturnValue_t PlocSupervisorHandler::scanForReply(const uint8_t* start, size_t remainingSize, @@ -654,7 +654,7 @@ ReturnValue_t PlocSupervisorHandler::scanForReply(const uint8_t* start, size_t r return parseMramPackets(start, remainingSize, foundLen); } - ReturnValue_t result = RETURN_OK; + ReturnValue_t result = returnvalue::OK; uint16_t apid = (*(start) << 8 | *(start + 1)) & APID_MASK; @@ -712,13 +712,13 @@ ReturnValue_t PlocSupervisorHandler::getSwitches(const uint8_t** switches, } *numberOfSwitches = 1; *switches = &powerSwitch; - return RETURN_OK; + return returnvalue::OK; } ReturnValue_t PlocSupervisorHandler::interpretDeviceReply(DeviceCommandId_t id, const uint8_t* packet) { using namespace supv; - ReturnValue_t result = RETURN_OK; + ReturnValue_t result = returnvalue::OK; switch (id) { case ACK_REPORT: { @@ -861,11 +861,11 @@ ReturnValue_t PlocSupervisorHandler::initializeLocalDataPool(localpool::DataPool localDataPoolMap.emplace(supv::ADC_ENG_14, new PoolEntry({0})); localDataPoolMap.emplace(supv::ADC_ENG_15, new PoolEntry({0})); - return HasReturnvaluesIF::RETURN_OK; + return returnvalue::OK; } void PlocSupervisorHandler::handleEvent(EventMessage* eventMessage) { - ReturnValue_t result = RETURN_OK; + ReturnValue_t result = returnvalue::OK; object_id_t objectId = eventMessage->getReporter(); Event event = eventMessage->getEvent(); switch (objectId) { @@ -880,7 +880,7 @@ void PlocSupervisorHandler::handleEvent(EventMessage* eventMessage) { event == PlocSupvHelper::SUPV_MEM_CHECK_FAIL || event == PlocSupvHelper::SUPV_MEM_CHECK_OK) { result = this->executeAction(supv::SHUTDOWN_MPSOC, NO_COMMANDER, nullptr, 0); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { triggerEvent(SUPV_MPSOC_SHUWDOWN_BUILD_FAILED); sif::warning << "PlocSupervisorHandler::handleEvent: Failed to build MPSoC shutdown " "command" @@ -916,28 +916,28 @@ ReturnValue_t PlocSupervisorHandler::verifyPacket(const uint8_t* start, size_t f if (CRC::crc16ccitt(start, foundLen) != 0) { return SupvReturnValuesIF::CRC_FAILURE; } - return RETURN_OK; + return returnvalue::OK; } ReturnValue_t PlocSupervisorHandler::handleAckReport(const uint8_t* data) { using namespace supv; - ReturnValue_t result = RETURN_OK; + ReturnValue_t result = returnvalue::OK; AcknowledgmentReport ack(data, SIZE_ACK_REPORT); result = ack.checkSize(); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return result; } result = ack.checkCrc(); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { sif::error << "PlocSupervisorHandler::handleAckReport: CRC failure" << std::endl; nextReplyId = supv::NONE; replyRawReplyIfnotWiretapped(data, supv::SIZE_ACK_REPORT); triggerEvent(SUPV_CRC_FAILURE_EVENT); sendFailureReport(supv::ACK_REPORT, SupvReturnValuesIF::CRC_FAILURE); disableAllReplies(); - return RETURN_OK; + return returnvalue::OK; } result = ack.checkApid(); @@ -955,7 +955,7 @@ ReturnValue_t PlocSupervisorHandler::handleAckReport(const uint8_t* data) { result = IGNORE_REPLY_DATA; break; } - case RETURN_OK: { + case returnvalue::OK: { setNextReplyId(); break; } @@ -969,7 +969,7 @@ ReturnValue_t PlocSupervisorHandler::handleAckReport(const uint8_t* data) { break; default: { sif::error << "PlocSupervisorHandler::handleAckReport: APID parsing failed" << std::endl; - result = RETURN_FAILED; + result = returnvalue::FAILED; break; } } @@ -978,16 +978,16 @@ ReturnValue_t PlocSupervisorHandler::handleAckReport(const uint8_t* data) { ReturnValue_t PlocSupervisorHandler::handleExecutionReport(const uint8_t* data) { using namespace supv; - ReturnValue_t result = RETURN_OK; + ReturnValue_t result = returnvalue::OK; ExecutionReport exe(data, SIZE_EXE_REPORT); result = exe.checkSize(); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return result; } result = exe.checkCrc(); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { sif::error << "PlocSupervisorHandler::handleExecutionReport: CRC failure" << std::endl; nextReplyId = supv::NONE; return result; @@ -996,18 +996,18 @@ ReturnValue_t PlocSupervisorHandler::handleExecutionReport(const uint8_t* data) result = exe.checkApid(); switch (result) { - case (RETURN_OK): { + case (returnvalue::OK): { handleExecutionSuccessReport(data); break; } case (SupvReturnValuesIF::RECEIVED_EXE_FAILURE): { handleExecutionFailureReport(exe.getStatusCode()); - result = RETURN_OK; + result = returnvalue::OK; break; } default: { sif::error << "PlocSupervisorHandler::handleExecutionReport: Unknown APID" << std::endl; - result = RETURN_FAILED; + result = returnvalue::FAILED; break; } } @@ -1016,7 +1016,7 @@ ReturnValue_t PlocSupervisorHandler::handleExecutionReport(const uint8_t* data) } ReturnValue_t PlocSupervisorHandler::handleHkReport(const uint8_t* data) { - ReturnValue_t result = RETURN_OK; + ReturnValue_t result = returnvalue::OK; result = verifyPacket(data, supv::SIZE_HK_REPORT); @@ -1092,7 +1092,7 @@ ReturnValue_t PlocSupervisorHandler::handleHkReport(const uint8_t* data) { } ReturnValue_t PlocSupervisorHandler::handleBootStatusReport(const uint8_t* data) { - ReturnValue_t result = RETURN_OK; + ReturnValue_t result = returnvalue::OK; result = verifyPacket(data, supv::SIZE_BOOT_STATUS_REPORT); @@ -1158,7 +1158,7 @@ ReturnValue_t PlocSupervisorHandler::handleBootStatusReport(const uint8_t* data) } ReturnValue_t PlocSupervisorHandler::handleLatchupStatusReport(const uint8_t* data) { - ReturnValue_t result = RETURN_OK; + ReturnValue_t result = returnvalue::OK; result = verifyPacket(data, supv::SIZE_LATCHUP_STATUS_REPORT); @@ -1242,7 +1242,7 @@ ReturnValue_t PlocSupervisorHandler::handleLatchupStatusReport(const uint8_t* da } ReturnValue_t PlocSupervisorHandler::handleLoggingReport(const uint8_t* data) { - ReturnValue_t result = RETURN_OK; + ReturnValue_t result = returnvalue::OK; result = verifyPacket(data, supv::SIZE_LOGGING_REPORT); @@ -1254,20 +1254,20 @@ ReturnValue_t PlocSupervisorHandler::handleLoggingReport(const uint8_t* data) { const uint8_t* dataField = data + supv::DATA_FIELD_OFFSET + sizeof(supv::RequestLoggingData::Sa); result = loggingReport.read(); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return result; } loggingReport.setValidityBufferGeneration(false); size_t size = loggingReport.getSerializedSize(); result = loggingReport.deSerialize(&dataField, &size, SerializeIF::Endianness::BIG); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { sif::warning << "PlocSupervisorHandler::handleLoggingReport: Deserialization failed" << std::endl; } loggingReport.setValidityBufferGeneration(true); loggingReport.setValidity(true, true); result = loggingReport.commit(); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return result; } #if OBSW_VERBOSE_LEVEL >= 1 && OBSW_DEBUG_PLOC_SUPERVISOR == 1 @@ -1278,7 +1278,7 @@ ReturnValue_t PlocSupervisorHandler::handleLoggingReport(const uint8_t* data) { } ReturnValue_t PlocSupervisorHandler::handleAdcReport(const uint8_t* data) { - ReturnValue_t result = RETURN_OK; + ReturnValue_t result = returnvalue::OK; result = verifyPacket(data, supv::SIZE_ADC_REPORT); @@ -1290,19 +1290,19 @@ ReturnValue_t PlocSupervisorHandler::handleAdcReport(const uint8_t* data) { const uint8_t* dataField = data + supv::DATA_FIELD_OFFSET; result = adcReport.read(); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return result; } adcReport.setValidityBufferGeneration(false); size_t size = adcReport.getSerializedSize(); result = adcReport.deSerialize(&dataField, &size, SerializeIF::Endianness::BIG); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { sif::warning << "PlocSupervisorHandler::handleAdcReport: Deserialization failed" << std::endl; } adcReport.setValidityBufferGeneration(true); adcReport.setValidity(true, true); result = adcReport.commit(); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return result; } #if OBSW_VERBOSE_LEVEL >= 1 && OBSW_DEBUG_PLOC_SUPERVISOR == 1 @@ -1378,16 +1378,16 @@ size_t PlocSupervisorHandler::getNextReplyLength(DeviceCommandId_t commandId) { ReturnValue_t PlocSupervisorHandler::doSendReadHook() { // Prevent DHB from polling UART during commands executed by the supervisor helper task if (plocSupvHelperExecuting) { - return RETURN_FAILED; + return returnvalue::FAILED; } - return RETURN_OK; + return returnvalue::OK; } void PlocSupervisorHandler::doOffActivity() { startupState = StartupState::OFF; } void PlocSupervisorHandler::handleDeviceTM(const uint8_t* data, size_t dataSize, DeviceCommandId_t replyId) { - ReturnValue_t result = RETURN_OK; + ReturnValue_t result = returnvalue::OK; if (wiretappingMode == RAW) { /* Data already sent in doGetRead() */ @@ -1406,7 +1406,7 @@ void PlocSupervisorHandler::handleDeviceTM(const uint8_t* data, size_t dataSize, } result = actionHelper.reportData(queueId, replyId, data, dataSize); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { sif::debug << "PlocSupervisorHandler::handleDeviceTM: Failed to report data" << std::endl; } } @@ -1414,49 +1414,49 @@ void PlocSupervisorHandler::handleDeviceTM(const uint8_t* data, size_t dataSize, ReturnValue_t PlocSupervisorHandler::prepareEmptyCmd(uint16_t apid) { supv::ApidOnlyPacket packet(spParams, apid); ReturnValue_t result = packet.buildPacket(); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return result; } finishTcPrep(packet.getFullPacketLen()); - return RETURN_OK; + return returnvalue::OK; } ReturnValue_t PlocSupervisorHandler::prepareSelBootImageCmd(const uint8_t* commandData) { supv::MPSoCBootSelect packet(spParams); ReturnValue_t result = packet.buildPacket(*commandData, *(commandData + 1), *(commandData + 2), *(commandData + 3)); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return result; } finishTcPrep(packet.getFullPacketLen()); - return RETURN_OK; + return returnvalue::OK; } ReturnValue_t PlocSupervisorHandler::prepareSetTimeRefCmd() { Clock::TimeOfDay_t time; ReturnValue_t result = Clock::getDateAndTime(&time); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { sif::warning << "PlocSupervisorHandler::prepareSetTimeRefCmd: Failed to get current time" << std::endl; return SupvReturnValuesIF::GET_TIME_FAILURE; } supv::SetTimeRef packet(spParams); result = packet.buildPacket(&time); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return result; } finishTcPrep(packet.getFullPacketLen()); - return RETURN_OK; + return returnvalue::OK; } ReturnValue_t PlocSupervisorHandler::prepareDisableHk() { supv::DisablePeriodicHkTransmission packet(spParams); ReturnValue_t result = packet.buildPacket(); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return result; } finishTcPrep(packet.getFullPacketLen()); - return RETURN_OK; + return returnvalue::OK; } ReturnValue_t PlocSupervisorHandler::prepareSetBootTimeoutCmd(const uint8_t* commandData) { @@ -1464,27 +1464,27 @@ ReturnValue_t PlocSupervisorHandler::prepareSetBootTimeoutCmd(const uint8_t* com uint32_t timeout = *(commandData) << 24 | *(commandData + 1) << 16 | *(commandData + 2) << 8 | *(commandData + 3); ReturnValue_t result = packet.buildPacket(timeout); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return result; } finishTcPrep(packet.getFullPacketLen()); - return RETURN_OK; + return returnvalue::OK; } ReturnValue_t PlocSupervisorHandler::prepareRestartTriesCmd(const uint8_t* commandData) { uint8_t restartTries = *(commandData); supv::SetRestartTries packet(spParams); ReturnValue_t result = packet.buildPacket(restartTries); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return result; } finishTcPrep(packet.getFullPacketLen()); - return RETURN_OK; + return returnvalue::OK; } ReturnValue_t PlocSupervisorHandler::prepareLatchupConfigCmd(const uint8_t* commandData, DeviceCommandId_t deviceCommand) { - ReturnValue_t result = RETURN_OK; + ReturnValue_t result = returnvalue::OK; uint8_t latchupId = *commandData; if (latchupId > 6) { return SupvReturnValuesIF::INVALID_LATCHUP_ID; @@ -1493,7 +1493,7 @@ ReturnValue_t PlocSupervisorHandler::prepareLatchupConfigCmd(const uint8_t* comm case (supv::ENABLE_LATCHUP_ALERT): { supv::LatchupAlert packet(spParams); result = packet.buildPacket(true, latchupId); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return result; } finishTcPrep(packet.getFullPacketLen()); @@ -1502,7 +1502,7 @@ ReturnValue_t PlocSupervisorHandler::prepareLatchupConfigCmd(const uint8_t* comm case (supv::DISABLE_LATCHUP_ALERT): { supv::LatchupAlert packet(spParams); result = packet.buildPacket(false, latchupId); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return result; } finishTcPrep(packet.getFullPacketLen()); @@ -1511,7 +1511,7 @@ ReturnValue_t PlocSupervisorHandler::prepareLatchupConfigCmd(const uint8_t* comm default: { sif::debug << "PlocSupervisorHandler::prepareLatchupConfigCmd: Invalid command id" << std::endl; - result = RETURN_FAILED; + result = returnvalue::FAILED; break; } } @@ -1529,22 +1529,22 @@ ReturnValue_t PlocSupervisorHandler::prepareSetAlertLimitCmd(const uint8_t* comm } supv::SetAlertlimit packet(spParams); ReturnValue_t result = packet.buildPacket(latchupId, dutycycle); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return result; } finishTcPrep(packet.getFullPacketLen()); - return RETURN_OK; + return returnvalue::OK; } ReturnValue_t PlocSupervisorHandler::prepareSetAdcEnabledChannelsCmd(const uint8_t* commandData) { uint16_t ch = *(commandData) << 8 | *(commandData + 1); supv::SetAdcEnabledChannels packet(spParams); ReturnValue_t result = packet.buildPacket(ch); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return result; } finishTcPrep(packet.getFullPacketLen()); - return RETURN_OK; + return returnvalue::OK; } ReturnValue_t PlocSupervisorHandler::prepareSetAdcWindowAndStrideCmd(const uint8_t* commandData) { @@ -1554,11 +1554,11 @@ ReturnValue_t PlocSupervisorHandler::prepareSetAdcWindowAndStrideCmd(const uint8 uint16_t stridingStepSize = *(commandData + offset) << 8 | *(commandData + offset + 1); supv::SetAdcWindowAndStride packet(spParams); ReturnValue_t result = packet.buildPacket(windowSize, stridingStepSize); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return result; } finishTcPrep(packet.getFullPacketLen()); - return RETURN_OK; + return returnvalue::OK; } ReturnValue_t PlocSupervisorHandler::prepareSetAdcThresholdCmd(const uint8_t* commandData) { @@ -1566,11 +1566,11 @@ ReturnValue_t PlocSupervisorHandler::prepareSetAdcThresholdCmd(const uint8_t* co *(commandData + 3); supv::SetAdcThreshold packet(spParams); ReturnValue_t result = packet.buildPacket(threshold); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return result; } finishTcPrep(packet.getFullPacketLen()); - return RETURN_OK; + return returnvalue::OK; } ReturnValue_t PlocSupervisorHandler::prepareRunAutoEmTest(const uint8_t* commandData) { @@ -1580,11 +1580,11 @@ ReturnValue_t PlocSupervisorHandler::prepareRunAutoEmTest(const uint8_t* command } supv::RunAutoEmTests packet(spParams); ReturnValue_t result = packet.buildPacket(test); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return result; } finishTcPrep(packet.getFullPacketLen()); - return RETURN_OK; + return returnvalue::OK; } ReturnValue_t PlocSupervisorHandler::prepareWipeMramCmd(const uint8_t* commandData) { @@ -1598,11 +1598,11 @@ ReturnValue_t PlocSupervisorHandler::prepareWipeMramCmd(const uint8_t* commandDa } supv::MramCmd packet(spParams); ReturnValue_t result = packet.buildPacket(start, stop, supv::MramCmd::MramAction::WIPE); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return result; } finishTcPrep(packet.getFullPacketLen()); - return RETURN_OK; + return returnvalue::OK; } ReturnValue_t PlocSupervisorHandler::prepareDumpMramCmd(const uint8_t* commandData) { @@ -1616,7 +1616,7 @@ ReturnValue_t PlocSupervisorHandler::prepareDumpMramCmd(const uint8_t* commandDa } supv::MramCmd packet(spParams); ReturnValue_t result = packet.buildPacket(start, stop, supv::MramCmd::MramAction::DUMP); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return result; } expectedMramDumpPackets = (stop - start) / supv::MAX_DATA_CAPACITY; @@ -1626,7 +1626,7 @@ ReturnValue_t PlocSupervisorHandler::prepareDumpMramCmd(const uint8_t* commandDa receivedMramDumpPackets = 0; finishTcPrep(packet.getFullPacketLen()); - return RETURN_OK; + return returnvalue::OK; } ReturnValue_t PlocSupervisorHandler::prepareSetGpioCmd(const uint8_t* commandData) { @@ -1635,11 +1635,11 @@ ReturnValue_t PlocSupervisorHandler::prepareSetGpioCmd(const uint8_t* commandDat uint8_t val = *(commandData + 2); supv::SetGpio packet(spParams); ReturnValue_t result = packet.buildPacket(port, pin, val); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return result; } finishTcPrep(packet.getFullPacketLen()); - return RETURN_OK; + return returnvalue::OK; } ReturnValue_t PlocSupervisorHandler::prepareReadGpioCmd(const uint8_t* commandData) { @@ -1647,11 +1647,11 @@ ReturnValue_t PlocSupervisorHandler::prepareReadGpioCmd(const uint8_t* commandDa uint8_t pin = *(commandData + 1); supv::ReadGpio packet(spParams); ReturnValue_t result = packet.buildPacket(port, pin); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return result; } finishTcPrep(packet.getFullPacketLen()); - return RETURN_OK; + return returnvalue::OK; } void PlocSupervisorHandler::finishTcPrep(size_t packetLen) { @@ -1662,22 +1662,22 @@ void PlocSupervisorHandler::finishTcPrep(size_t packetLen) { ReturnValue_t PlocSupervisorHandler::prepareSetShutdownTimeoutCmd(const uint8_t* commandData) { uint32_t timeout = 0; - ReturnValue_t result = RETURN_OK; + ReturnValue_t result = returnvalue::OK; size_t size = sizeof(timeout); result = SerializeAdapter::deSerialize(&timeout, &commandData, &size, SerializeIF::Endianness::BIG); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { sif::warning << "PlocSupervisorHandler::prepareSetShutdownTimeoutCmd: Failed to deserialize timeout" << std::endl; } supv::SetShutdownTimeout packet(spParams); result = packet.buildPacket(timeout); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return result; } finishTcPrep(packet.getFullPacketLen()); - return RETURN_OK; + return returnvalue::OK; } ReturnValue_t PlocSupervisorHandler::prepareLoggingRequest(const uint8_t* commandData, @@ -1687,11 +1687,11 @@ ReturnValue_t PlocSupervisorHandler::prepareLoggingRequest(const uint8_t* comman uint8_t tpc = *(commandData + 1); RequestLoggingData packet(spParams); ReturnValue_t result = packet.buildPacket(sa, tpc); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return result; } finishTcPrep(packet.getFullPacketLen()); - return RETURN_OK; + return returnvalue::OK; } ReturnValue_t PlocSupervisorHandler::prepareEnableNvmsCommand(const uint8_t* commandData) { @@ -1700,11 +1700,11 @@ ReturnValue_t PlocSupervisorHandler::prepareEnableNvmsCommand(const uint8_t* com uint8_t nvm3 = *(commandData + 1); EnableNvms packet(spParams); ReturnValue_t result = packet.buildPacket(nvm01, nvm3); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return result; } finishTcPrep(packet.getFullPacketLen()); - return RETURN_OK; + return returnvalue::OK; } void PlocSupervisorHandler::disableAllReplies() { @@ -1830,18 +1830,18 @@ ReturnValue_t PlocSupervisorHandler::parseMramPackets(const uint8_t* packet, siz } ReturnValue_t PlocSupervisorHandler::handleMramDumpPacket(DeviceCommandId_t id) { - ReturnValue_t result = RETURN_FAILED; + ReturnValue_t result = returnvalue::FAILED; // Prepare packet for downlink if (packetInBuffer) { uint16_t packetLen = readSpacePacketLength(spacePacketBuffer); result = verifyPacket(spacePacketBuffer, supv::SPACE_PACKET_HEADER_LENGTH + packetLen + 1); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { sif::warning << "PlocSupervisorHandler::handleMramDumpPacket: CRC failure" << std::endl; return result; } result = handleMramDumpFile(id); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { DeviceCommandMap::iterator iter = deviceCommandMap.find(id); actionHelper.finish(false, iter->second.sendReplyTo, id, result); disableAllReplies(); @@ -1854,7 +1854,7 @@ ReturnValue_t PlocSupervisorHandler::handleMramDumpPacket(DeviceCommandId_t id) nextReplyId = supv::EXE_REPORT; } increaseExpectedMramReplies(id); - return RETURN_OK; + return returnvalue::OK; } return result; } @@ -1914,14 +1914,14 @@ ReturnValue_t PlocSupervisorHandler::checkMramPacketApid() { } ReturnValue_t PlocSupervisorHandler::handleMramDumpFile(DeviceCommandId_t id) { - ReturnValue_t result = RETURN_OK; + ReturnValue_t result = returnvalue::OK; uint16_t packetLen = readSpacePacketLength(spacePacketBuffer); uint8_t sequenceFlags = readSequenceFlags(spacePacketBuffer); if (id == supv::FIRST_MRAM_DUMP) { if (sequenceFlags == static_cast(ccsds::SequenceFlags::FIRST_SEGMENT) || (sequenceFlags == static_cast(ccsds::SequenceFlags::UNSEGMENTED))) { result = createMramDumpFile(); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return result; } } @@ -1935,7 +1935,7 @@ ReturnValue_t PlocSupervisorHandler::handleMramDumpFile(DeviceCommandId_t id) { file.write(reinterpret_cast(spacePacketBuffer + supv::SPACE_PACKET_HEADER_LENGTH), packetLen - 1); file.close(); - return RETURN_OK; + return returnvalue::OK; } uint16_t PlocSupervisorHandler::readSpacePacketLength(uint8_t* spacePacket) { @@ -1947,10 +1947,10 @@ uint8_t PlocSupervisorHandler::readSequenceFlags(uint8_t* spacePacket) { } ReturnValue_t PlocSupervisorHandler::createMramDumpFile() { - ReturnValue_t result = RETURN_OK; + ReturnValue_t result = returnvalue::OK; std::string timeStamp; result = getTimeStampString(timeStamp); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return result; } @@ -1973,13 +1973,13 @@ ReturnValue_t PlocSupervisorHandler::createMramDumpFile() { std::ofstream file(activeMramFile, std::ios_base::out); file.close(); - return RETURN_OK; + return returnvalue::OK; } ReturnValue_t PlocSupervisorHandler::getTimeStampString(std::string& timeStamp) { Clock::TimeOfDay_t time; ReturnValue_t result = Clock::getDateAndTime(&time); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { sif::warning << "PlocSupervisorHandler::getTimeStampString: Failed to get current time" << std::endl; return SupvReturnValuesIF::GET_TIME_FAILURE; @@ -1987,7 +1987,7 @@ ReturnValue_t PlocSupervisorHandler::getTimeStampString(std::string& timeStamp) timeStamp = std::to_string(time.year) + "-" + std::to_string(time.month) + "-" + std::to_string(time.day) + "--" + std::to_string(time.hour) + "-" + std::to_string(time.minute) + "-" + std::to_string(time.second); - return RETURN_OK; + return returnvalue::OK; } ReturnValue_t PlocSupervisorHandler::extractUpdateCommand(const uint8_t* commandData, size_t size, @@ -1999,11 +1999,11 @@ ReturnValue_t PlocSupervisorHandler::extractUpdateCommand(const uint8_t* command sif::warning << "PlocSupervisorHandler::extractUpdateCommand: Data size too big" << std::endl; return SupvReturnValuesIF::INVALID_LENGTH; } - ReturnValue_t result = RETURN_OK; + ReturnValue_t result = returnvalue::OK; result = extractBaseParams(&commandData, size, params); result = SerializeAdapter::deSerialize(¶ms.bytesWritten, &commandData, &remSize, SerializeIF::Endianness::BIG); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { sif::warning << "PlocSupervisorHandler::extractUpdateCommand: Failed to deserialize bytes " "already written" << std::endl; @@ -2011,7 +2011,7 @@ ReturnValue_t PlocSupervisorHandler::extractUpdateCommand(const uint8_t* command } result = SerializeAdapter::deSerialize(¶ms.seqCount, &commandData, &remSize, SerializeIF::Endianness::BIG); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { sif::warning << "PlocSupervisorHandler::extractUpdateCommand: Failed to deserialize start sequence count" << std::endl; @@ -2020,7 +2020,7 @@ ReturnValue_t PlocSupervisorHandler::extractUpdateCommand(const uint8_t* command uint8_t delMemRaw = 0; result = SerializeAdapter::deSerialize(&delMemRaw, &commandData, &remSize, SerializeIF::Endianness::BIG); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { sif::warning << "PlocSupervisorHandler::extractUpdateCommand: Failed to deserialize whether to delete " "memory" @@ -2028,7 +2028,7 @@ ReturnValue_t PlocSupervisorHandler::extractUpdateCommand(const uint8_t* command return result; } params.deleteMemory = delMemRaw; - return RETURN_OK; + return returnvalue::OK; } ReturnValue_t PlocSupervisorHandler::extractBaseParams(const uint8_t** commandData, size_t& remSize, @@ -2041,7 +2041,7 @@ ReturnValue_t PlocSupervisorHandler::extractBaseParams(const uint8_t** commandDa } } if (not nullTermFound) { - return HasReturnvaluesIF::RETURN_FAILED; + return returnvalue::FAILED; } params.file = std::string(reinterpret_cast(*commandData)); if (params.file.size() > (config::MAX_FILENAME_SIZE + config::MAX_PATH_SIZE)) { @@ -2055,7 +2055,7 @@ ReturnValue_t PlocSupervisorHandler::extractBaseParams(const uint8_t** commandDa remSize -= 1; ReturnValue_t result = SerializeAdapter::deSerialize(¶ms.startAddr, commandData, &remSize, SerializeIF::Endianness::BIG); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { sif::warning << "PlocSupervisorHandler::extractBaseParams: Failed to deserialize start address" << std::endl; return result; @@ -2064,7 +2064,7 @@ ReturnValue_t PlocSupervisorHandler::extractBaseParams(const uint8_t** commandDa } ReturnValue_t PlocSupervisorHandler::eventSubscription() { - ReturnValue_t result = RETURN_OK; + ReturnValue_t result = returnvalue::OK; EventManagerIF* manager = ObjectManager::instance()->get(objects::EVENT_MANAGER); if (manager == nullptr) { #if FSFW_CPP_OSTREAM_ENABLED == 1 @@ -2074,13 +2074,13 @@ ReturnValue_t PlocSupervisorHandler::eventSubscription() { ; } result = manager->registerListener(eventQueue->getId()); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return result; } result = manager->subscribeToEventRange(eventQueue->getId(), event::getEventId(PlocSupvHelper::SUPV_UPDATE_FAILED), event::getEventId(PlocSupvHelper::SUPV_MEM_CHECK_FAIL)); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { #if FSFW_CPP_OSTREAM_ENABLED == 1 sif::warning << "PlocSupervisorHandler::eventSubscritpion: Failed to subscribe to events from " " ploc supervisor helper" @@ -2097,10 +2097,10 @@ ReturnValue_t PlocSupervisorHandler::handleExecutionSuccessReport(const uint8_t* case supv::READ_GPIO: { supv::ExecutionReport exe(data, supv::SIZE_EXE_REPORT); if (exe.isNull()) { - return RETURN_FAILED; + return returnvalue::FAILED; } ReturnValue_t result = exe.checkSize(); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return result; } uint16_t gpioState = exe.getStatusCode(); @@ -2109,17 +2109,17 @@ ReturnValue_t PlocSupervisorHandler::handleExecutionSuccessReport(const uint8_t* #endif /* OBSW_DEBUG_PLOC_SUPERVISOR == 1 */ DeviceCommandMap::iterator iter = deviceCommandMap.find(commandId); if (iter->second.sendReplyTo == NO_COMMAND_ID) { - return RETURN_OK; + return returnvalue::OK; } uint8_t data[sizeof(gpioState)]; size_t size = 0; result = SerializeAdapter::serialize(&gpioState, data, &size, sizeof(gpioState), SerializeIF::Endianness::BIG); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { sif::debug << "PlocSupervisorHandler: Failed to deserialize GPIO state" << std::endl; } result = actionHelper.reportData(iter->second.sendReplyTo, commandId, data, sizeof(data)); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { sif::warning << "PlocSupervisorHandler: Read GPIO, failed to report data" << std::endl; } break; @@ -2133,7 +2133,7 @@ ReturnValue_t PlocSupervisorHandler::handleExecutionSuccessReport(const uint8_t* default: break; } - return RETURN_OK; + return returnvalue::OK; } void PlocSupervisorHandler::handleExecutionFailureReport(uint16_t statusCode) { diff --git a/linux/devices/ploc/PlocSupervisorHandler.h b/linux/devices/ploc/PlocSupervisorHandler.h index bd4b0b9c..82ab4545 100644 --- a/linux/devices/ploc/PlocSupervisorHandler.h +++ b/linux/devices/ploc/PlocSupervisorHandler.h @@ -171,7 +171,7 @@ class PlocSupervisorHandler : public DeviceHandlerBase { * @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. + * @return returnvalue::OK if CRC is ok, otherwise CRC_FAILURE. */ ReturnValue_t verifyPacket(const uint8_t* start, size_t foundLen); @@ -180,7 +180,7 @@ class PlocSupervisorHandler : public DeviceHandlerBase { * * @param data Pointer to the data holding the acknowledgment report. * - * @return RETURN_OK if successful, otherwise an error code. + * @return returnvalue::OK if successful, otherwise an error code. */ ReturnValue_t handleAckReport(const uint8_t* data); @@ -189,7 +189,7 @@ class PlocSupervisorHandler : public DeviceHandlerBase { * * @param data Pointer to the received data packet. * - * @return RETURN_OK if successful, otherwise an error code. + * @return returnvalue::OK if successful, otherwise an error code. */ ReturnValue_t handleExecutionReport(const uint8_t* data); @@ -199,7 +199,7 @@ class PlocSupervisorHandler : public DeviceHandlerBase { * * @param data Pointer to the data buffer holding the housekeeping read report. * - * @return RETURN_OK if successful, otherwise an error code. + * @return returnvalue::OK if successful, otherwise an error code. */ ReturnValue_t handleHkReport(const uint8_t* data); diff --git a/linux/devices/ploc/PlocSupvHelper.cpp b/linux/devices/ploc/PlocSupvHelper.cpp index c6641f6e..c34fdbe3 100644 --- a/linux/devices/ploc/PlocSupvHelper.cpp +++ b/linux/devices/ploc/PlocSupvHelper.cpp @@ -30,14 +30,14 @@ ReturnValue_t PlocSupvHelper::initialize() { sdcMan = SdCardManager::instance(); if (sdcMan == nullptr) { sif::warning << "PlocSupvHelper::initialize: Invalid SD Card Manager" << std::endl; - return RETURN_FAILED; + return returnvalue::FAILED; } #endif - return RETURN_OK; + return returnvalue::OK; } ReturnValue_t PlocSupvHelper::performOperation(uint8_t operationCode) { - ReturnValue_t result = RETURN_OK; + ReturnValue_t result = returnvalue::OK; semaphore.acquire(); while (true) { switch (internalState) { @@ -47,7 +47,7 @@ ReturnValue_t PlocSupvHelper::performOperation(uint8_t operationCode) { } case InternalState::UPDATE: { result = executeUpdate(); - if (result == RETURN_OK) { + if (result == returnvalue::OK) { triggerEvent(SUPV_UPDATE_SUCCESSFUL, result); } else if (result == PROCESS_TERMINATED) { // Event already triggered @@ -64,7 +64,7 @@ ReturnValue_t PlocSupvHelper::performOperation(uint8_t operationCode) { } case InternalState::CONTINUE_UPDATE: { result = continueUpdate(); - if (result == RETURN_OK) { + if (result == returnvalue::OK) { triggerEvent(SUPV_CONTINUE_UPDATE_SUCCESSFUL, result); } else if (result == PROCESS_TERMINATED) { // Event already triggered @@ -76,7 +76,7 @@ ReturnValue_t PlocSupvHelper::performOperation(uint8_t operationCode) { } case InternalState::REQUEST_EVENT_BUFFER: { result = performEventBufferRequest(); - if (result == RETURN_OK) { + if (result == returnvalue::OK) { triggerEvent(SUPV_EVENT_BUFFER_REQUEST_SUCCESSFUL, result); } else if (result == PROCESS_TERMINATED) { // Event already triggered @@ -97,10 +97,10 @@ ReturnValue_t PlocSupvHelper::performOperation(uint8_t operationCode) { ReturnValue_t PlocSupvHelper::setComIF(UartComIF* uartComIF_) { if (uartComIF_ == nullptr) { sif::warning << "PlocSupvHelper::initialize: Provided invalid uart com if" << std::endl; - return RETURN_FAILED; + return returnvalue::FAILED; } uartComIF = uartComIF_; - return RETURN_OK; + return returnvalue::OK; } void PlocSupvHelper::setComCookie(CookieIF* comCookie_) { comCookie = comCookie_; } @@ -118,16 +118,16 @@ ReturnValue_t PlocSupvHelper::startUpdate(std::string file, uint8_t memoryId, } ReturnValue_t PlocSupvHelper::performUpdate(const supv::UpdateParams& params) { - ReturnValue_t result = RETURN_OK; + ReturnValue_t result = returnvalue::OK; #ifdef XIPHOS_Q7S result = FilesystemHelper::checkPath(params.file); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { sif::warning << "PlocSupvHelper::startUpdate: File " << params.file << " does not exist" << std::endl; return result; } result = FilesystemHelper::fileExists(params.file); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { sif::warning << "PlocSupvHelper::startUpdate: The file " << params.file << " does not exist" << std::endl; return result; @@ -137,7 +137,7 @@ ReturnValue_t PlocSupvHelper::performUpdate(const supv::UpdateParams& params) { if (not std::filesystem::exists(file)) { sif::warning << "PlocSupvHelper::startUpdate: The file " << file << " does not exist" << std::endl; - return RETURN_FAILED; + return returnvalue::FAILED; } #endif update.file = params.file; @@ -145,7 +145,7 @@ ReturnValue_t PlocSupvHelper::performUpdate(const supv::UpdateParams& params) { if (params.bytesWritten > update.fullFileSize) { sif::warning << "Invalid start bytes counter " << params.bytesWritten << ", smaller than full file length" << update.fullFileSize << std::endl; - return HasReturnvaluesIF::RETURN_FAILED; + return returnvalue::FAILED; } update.length = update.fullFileSize - params.bytesWritten; update.memoryId = params.memId; @@ -178,7 +178,7 @@ ReturnValue_t PlocSupvHelper::performMemCheck(uint8_t memoryId, uint32_t startAd internalState = InternalState::CHECK_MEMORY; uartComIF->flushUartTxAndRxBuf(comCookie); semaphore.release(); - return HasReturnvaluesIF::RETURN_OK; + return returnvalue::OK; } void PlocSupvHelper::initiateUpdateContinuation() { @@ -189,7 +189,7 @@ void PlocSupvHelper::initiateUpdateContinuation() { ReturnValue_t PlocSupvHelper::startEventbBufferRequest(std::string path) { #ifdef XIPHOS_Q7S ReturnValue_t result = FilesystemHelper::checkPath(path); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return result; } #endif @@ -200,7 +200,7 @@ ReturnValue_t PlocSupvHelper::startEventbBufferRequest(std::string path) { internalState = InternalState::REQUEST_EVENT_BUFFER; uartComIF->flushUartTxAndRxBuf(comCookie); semaphore.release(); - return RETURN_OK; + return returnvalue::OK; } void PlocSupvHelper::stopProcess() { terminate = true; } @@ -210,26 +210,26 @@ void PlocSupvHelper::executeFullCheckMemoryCommand() { if (update.crcShouldBeChecked) { sif::info << "PLOC SUPV Mem Check: Calculating Image CRC" << std::endl; result = calcImageCrc(); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { triggerEvent(SUPV_MEM_CHECK_FAIL, result); return; } } sif::info << "PLOC SUPV Mem Check: Selecting Memory" << std::endl; result = selectMemory(); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { triggerEvent(SUPV_MEM_CHECK_FAIL, result); return; } sif::info << "PLOC SUPV Mem Check: Preparing Update" << std::endl; result = prepareUpdate(); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { triggerEvent(SUPV_MEM_CHECK_FAIL, result); return; } sif::info << "PLOC SUPV Mem Check: Memory Check" << std::endl; result = handleCheckMemoryCommand(); - if (result == HasReturnvaluesIF::RETURN_OK) { + if (result == returnvalue::OK) { triggerEvent(SUPV_MEM_CHECK_OK, result); } else { triggerEvent(SUPV_MEM_CHECK_FAIL, result); @@ -237,26 +237,26 @@ void PlocSupvHelper::executeFullCheckMemoryCommand() { } ReturnValue_t PlocSupvHelper::executeUpdate() { - ReturnValue_t result = RETURN_OK; + ReturnValue_t result = returnvalue::OK; sif::info << "PLOC SUPV Update MPSoC: Calculating Image CRC" << std::endl; result = calcImageCrc(); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return result; } sif::info << "PLOC SUPV Update MPSoC: Selecting Memory" << std::endl; result = selectMemory(); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return result; } sif::info << "PLOC SUPV Update MPSoC: Preparing Update" << std::endl; result = prepareUpdate(); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return result; } if (update.deleteMemory) { sif::info << "PLOC SUPV Update MPSoC: Erasing Memory" << std::endl; result = eraseMemory(); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return result; } } @@ -265,7 +265,7 @@ ReturnValue_t PlocSupvHelper::executeUpdate() { ReturnValue_t PlocSupvHelper::continueUpdate() { ReturnValue_t result = prepareUpdate(); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return result; } return updateOperation(); @@ -274,7 +274,7 @@ ReturnValue_t PlocSupvHelper::continueUpdate() { ReturnValue_t PlocSupvHelper::updateOperation() { sif::info << "PlocSupvHelper::performUpdate: Writing Update Packets" << std::endl; auto result = writeUpdatePackets(); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return result; } sif::info << "PlocSupvHelper::performUpdate: Memory Check" << std::endl; @@ -282,7 +282,7 @@ ReturnValue_t PlocSupvHelper::updateOperation() { } ReturnValue_t PlocSupvHelper::writeUpdatePackets() { - ReturnValue_t result = RETURN_OK; + ReturnValue_t result = returnvalue::OK; #if OBSW_DEBUG_PLOC_SUPERVISOR == 1 ProgressPrinter progressPrinter("Supervisor update", update.fullFileSize, ProgressPrinter::HALF_PERCENT); @@ -338,13 +338,13 @@ ReturnValue_t PlocSupvHelper::writeUpdatePackets() { supv::WriteMemory packet(spParams); result = packet.buildPacket(seqFlags, update.sequenceCount, update.memoryId, update.startAddress + update.bytesWritten, dataLength, tempData); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { triggerEvent(WRITE_MEMORY_FAILED, buildProgParams1(progPercent, update.sequenceCount), update.bytesWritten); return result; } result = handlePacketTransmission(packet); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { triggerEvent(WRITE_MEMORY_FAILED, buildProgParams1(progPercent, update.sequenceCount), update.bytesWritten); return result; @@ -366,24 +366,24 @@ uint32_t PlocSupvHelper::buildProgParams1(uint8_t percent, uint16_t seqCount) { ReturnValue_t PlocSupvHelper::performEventBufferRequest() { using namespace supv; - ReturnValue_t result = RETURN_OK; + ReturnValue_t result = returnvalue::OK; resetSpParams(); RequestLoggingData packet(spParams); result = packet.buildPacket(RequestLoggingData::Sa::REQUEST_EVENT_BUFFERS); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return result; } result = sendCommand(packet); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return result; } result = handleAck(); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return result; } result = handleTmReception(ccsds::HEADER_LEN, tmBuf.data(), supv::recv_timeout::UPDATE_STATUS_REPORT); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return result; } ploc::SpTmReader spReader(tmBuf.data(), tmBuf.size()); @@ -397,7 +397,7 @@ ReturnValue_t PlocSupvHelper::performEventBufferRequest() { if (not exeAlreadyReceived) { result = handleExe(); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return result; } } @@ -407,85 +407,85 @@ ReturnValue_t PlocSupvHelper::performEventBufferRequest() { ReturnValue_t PlocSupvHelper::handleRemainingExeReport(ploc::SpTmReader& reader) { size_t remBytes = reader.getPacketDataLen() + 1; ReturnValue_t result = handleTmReception(remBytes, tmBuf.data() + ccsds::HEADER_LEN); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { sif::warning << "Reading exe failure report failed" << std::endl; } result = exeReportHandling(); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { sif::warning << "Handling exe report failed" << std::endl; } return result; } ReturnValue_t PlocSupvHelper::selectMemory() { - ReturnValue_t result = RETURN_OK; + ReturnValue_t result = returnvalue::OK; resetSpParams(); supv::MPSoCBootSelect packet(spParams); result = packet.buildPacket(update.memoryId); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return result; } result = handlePacketTransmission(packet); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return result; } - return RETURN_OK; + return returnvalue::OK; } ReturnValue_t PlocSupvHelper::prepareUpdate() { - ReturnValue_t result = RETURN_OK; + ReturnValue_t result = returnvalue::OK; resetSpParams(); supv::ApidOnlyPacket packet(spParams, supv::APID_PREPARE_UPDATE); result = packet.buildPacket(); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return result; } result = handlePacketTransmission(packet, PREPARE_UPDATE_EXECUTION_REPORT); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return result; } - return RETURN_OK; + return returnvalue::OK; } ReturnValue_t PlocSupvHelper::eraseMemory() { - ReturnValue_t result = RETURN_OK; + ReturnValue_t result = returnvalue::OK; resetSpParams(); supv::EraseMemory eraseMemory(spParams); result = eraseMemory.buildPacket(update.memoryId, update.startAddress + update.bytesWritten, update.length); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return result; } result = handlePacketTransmission(eraseMemory, supv::recv_timeout::ERASE_MEMORY); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return result; } - return RETURN_OK; + return returnvalue::OK; } ReturnValue_t PlocSupvHelper::handlePacketTransmission(ploc::SpTcBase& packet, uint32_t timeoutExecutionReport) { - ReturnValue_t result = RETURN_OK; + ReturnValue_t result = returnvalue::OK; result = sendCommand(packet); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return result; } result = handleAck(); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return result; } result = handleExe(timeoutExecutionReport); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return result; } - return RETURN_OK; + return returnvalue::OK; } ReturnValue_t PlocSupvHelper::sendCommand(ploc::SpTcBase& packet) { - ReturnValue_t result = RETURN_OK; + ReturnValue_t result = returnvalue::OK; rememberApid = packet.getApid(); result = uartComIF->sendMessage(comCookie, packet.getFullPacket(), packet.getFullPacketLen()); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { sif::warning << "PlocSupvHelper::sendCommand: Failed to send command" << std::endl; triggerEvent(SUPV_SENDING_COMMAND_FAILED, result, static_cast(internalState)); return result; @@ -494,10 +494,10 @@ ReturnValue_t PlocSupvHelper::sendCommand(ploc::SpTcBase& packet) { } ReturnValue_t PlocSupvHelper::handleAck() { - ReturnValue_t result = RETURN_OK; + ReturnValue_t result = returnvalue::OK; result = handleTmReception(supv::SIZE_ACK_REPORT); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { triggerEvent(ACK_RECEPTION_FAILURE, result, static_cast(rememberApid)); sif::warning << "PlocSupvHelper::handleAck: Error in reception of acknowledgment report" << std::endl; @@ -505,11 +505,11 @@ ReturnValue_t PlocSupvHelper::handleAck() { } supv::AcknowledgmentReport ackReport(tmBuf.data(), tmBuf.size()); result = checkReceivedTm(ackReport); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return result; } result = ackReport.checkApid(); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { if (result == SupvReturnValuesIF::RECEIVED_ACK_FAILURE) { triggerEvent(SUPV_ACK_FAILURE_REPORT, static_cast(ackReport.getRefApid())); } else if (result == SupvReturnValuesIF::INVALID_APID) { @@ -517,14 +517,14 @@ ReturnValue_t PlocSupvHelper::handleAck() { } return result; } - return RETURN_OK; + return returnvalue::OK; } ReturnValue_t PlocSupvHelper::handleExe(uint32_t timeout) { - ReturnValue_t result = RETURN_OK; + ReturnValue_t result = returnvalue::OK; result = handleTmReception(supv::SIZE_EXE_REPORT, tmBuf.data(), timeout); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { triggerEvent(EXE_RECEPTION_FAILURE, result, static_cast(rememberApid)); sif::warning << "PlocSupvHelper::handleExe: Error in reception of execution report" << std::endl; @@ -538,11 +538,11 @@ ReturnValue_t PlocSupvHelper::exeReportHandling() { supv::ExecutionReport exeReport(tmBuf.data(), tmBuf.size()); ReturnValue_t result = checkReceivedTm(exeReport); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return result; } result = exeReport.checkApid(); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { if (result == SupvReturnValuesIF::RECEIVED_EXE_FAILURE) { triggerEvent(SUPV_EXE_FAILURE_REPORT, static_cast(exeReport.getRefApid())); } else if (result == SupvReturnValuesIF::INVALID_APID) { @@ -555,7 +555,7 @@ ReturnValue_t PlocSupvHelper::exeReportHandling() { ReturnValue_t PlocSupvHelper::handleTmReception(size_t remainingBytes, uint8_t* readBuf, uint32_t timeout) { - ReturnValue_t result = RETURN_OK; + ReturnValue_t result = returnvalue::OK; size_t readBytes = 0; size_t currentBytes = 0; Countdown countdown(timeout); @@ -564,7 +564,7 @@ ReturnValue_t PlocSupvHelper::handleTmReception(size_t remainingBytes, uint8_t* } while (!countdown.hasTimedOut()) { result = receive(readBuf + readBytes, ¤tBytes, remainingBytes); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return result; } readBytes += currentBytes; @@ -576,19 +576,19 @@ ReturnValue_t PlocSupvHelper::handleTmReception(size_t remainingBytes, uint8_t* if (remainingBytes != 0) { sif::warning << "PlocSupvHelper::handleTmReception: Failed to read " << std::dec << remainingBytes << " remaining bytes" << std::endl; - return RETURN_FAILED; + return returnvalue::FAILED; } return result; } ReturnValue_t PlocSupvHelper::checkReceivedTm(ploc::SpTmReader& reader) { ReturnValue_t result = reader.checkSize(); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { triggerEvent(SUPV_REPLY_SIZE_MISSMATCH, rememberApid); return result; } result = reader.checkCrc(); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { triggerEvent(SUPV_REPLY_CRC_MISSMATCH, rememberApid); return result; } @@ -596,20 +596,20 @@ ReturnValue_t PlocSupvHelper::checkReceivedTm(ploc::SpTmReader& reader) { } ReturnValue_t PlocSupvHelper::receive(uint8_t* data, size_t* readBytes, size_t requestBytes) { - ReturnValue_t result = RETURN_OK; + ReturnValue_t result = returnvalue::OK; uint8_t* buffer = nullptr; result = uartComIF->requestReceiveMessage(comCookie, requestBytes); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { sif::warning << "PlocSupvHelper::receive: Failed to request reply" << std::endl; triggerEvent(SUPV_HELPER_REQUESTING_REPLY_FAILED, result, static_cast(static_cast(internalState))); - return RETURN_FAILED; + return returnvalue::FAILED; } result = uartComIF->readReceivedMessage(comCookie, &buffer, readBytes); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { sif::warning << "PlocSupvHelper::receive: Failed to read received message" << std::endl; triggerEvent(SUPV_HELPER_READING_REPLY_FAILED, result, static_cast(internalState)); - return RETURN_FAILED; + return returnvalue::FAILED; } if (*readBytes > 0) { std::memcpy(data, buffer, *readBytes); @@ -620,13 +620,13 @@ ReturnValue_t PlocSupvHelper::receive(uint8_t* data, size_t* readBytes, size_t r } ReturnValue_t PlocSupvHelper::calcImageCrc() { - ReturnValue_t result = RETURN_OK; + ReturnValue_t result = returnvalue::OK; if (update.fullFileSize == 0) { - return HasReturnvaluesIF::RETURN_FAILED; + return returnvalue::FAILED; } #ifdef XIPHOS_Q7S result = FilesystemHelper::checkPath(update.file); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { sif::warning << "PlocSupvHelper::calcImageCrc: File " << update.file << " does not exist" << std::endl; return result; @@ -666,7 +666,7 @@ ReturnValue_t PlocSupvHelper::calcImageCrc() { } ReturnValue_t PlocSupvHelper::handleCheckMemoryCommand() { - ReturnValue_t result = RETURN_OK; + ReturnValue_t result = returnvalue::OK; resetSpParams(); // Will hold status report for later processing std::array statusReportBuf{}; @@ -674,15 +674,15 @@ ReturnValue_t PlocSupvHelper::handleCheckMemoryCommand() { // Verification of update write procedure supv::CheckMemory packet(spParams); result = packet.buildPacket(update.memoryId, update.startAddress, update.fullFileSize); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return result; } result = sendCommand(packet); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return result; } result = handleAck(); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return result; } @@ -697,14 +697,14 @@ ReturnValue_t PlocSupvHelper::handleCheckMemoryCommand() { size_t remBytes = spReader.getPacketDataLen() + 1; result = handleTmReception(remBytes, tmBuf.data() + ccsds::HEADER_LEN, supv::recv_timeout::UPDATE_STATUS_REPORT); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { sif::warning << "PlocSupvHelper::handleCheckMemoryCommand: Failed to receive update status report" << std::endl; return result; } result = updateStatusReport.checkCrc(); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { sif::warning << "PlocSupvHelper::handleCheckMemoryCommand: CRC check failed" << std::endl; return result; } @@ -714,7 +714,7 @@ ReturnValue_t PlocSupvHelper::handleCheckMemoryCommand() { if (not exeAlreadyHandled) { result = handleExe(CRC_EXECUTION_TIMEOUT); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return result; } } @@ -722,12 +722,12 @@ ReturnValue_t PlocSupvHelper::handleCheckMemoryCommand() { // Now process the status report updateStatusReport.setData(statusReportBuf.data(), statusReportBuf.size()); result = updateStatusReport.parseDataField(); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return result; } if (update.crcShouldBeChecked) { result = updateStatusReport.verifycrc(update.crc); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { sif::warning << "PlocSupvHelper::handleCheckMemoryCommand: CRC failure. Expected CRC 0x" << std::setfill('0') << std::hex << std::setw(4) << static_cast(update.crc) << " but received CRC 0x" << std::setw(4) @@ -747,7 +747,7 @@ uint32_t PlocSupvHelper::getFileSize(std::string filename) { } ReturnValue_t PlocSupvHelper::handleEventBufferReception(ploc::SpTmReader& reader) { - ReturnValue_t result = RETURN_OK; + ReturnValue_t result = returnvalue::OK; std::string filename = Filenaming::generateAbsoluteFilename( eventBufferReq.path, eventBufferReq.filename, timestamping); std::ofstream file(filename, std::ios_base::app | std::ios_base::out); @@ -770,14 +770,14 @@ ReturnValue_t PlocSupvHelper::handleEventBufferReception(ploc::SpTmReader& reade requestLen -= 6; } result = handleTmReception(requestLen); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { sif::debug << "PlocSupvHelper::handleEventBufferReception: Failed while trying to read packet" << " " << packetsRead + 1 << std::endl; file.close(); return result; } ReturnValue_t result = reader.checkCrc(); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { triggerEvent(SUPV_REPLY_CRC_MISSMATCH, rememberApid); return result; } diff --git a/linux/devices/ploc/PlocSupvHelper.h b/linux/devices/ploc/PlocSupvHelper.h index fb585e12..82ce7522 100644 --- a/linux/devices/ploc/PlocSupvHelper.h +++ b/linux/devices/ploc/PlocSupvHelper.h @@ -7,7 +7,7 @@ #include "fsfw/devicehandlers/CookieIF.h" #include "fsfw/objectmanager/SystemObject.h" #include "fsfw/osal/linux/BinarySemaphore.h" -#include "fsfw/returnvalues/HasReturnvaluesIF.h" +#include "fsfw/returnvalues/returnvalue.h" #include "fsfw/tasks/ExecutableObjectIF.h" #include "fsfw_hal/linux/uart/UartComIF.h" #include "linux/devices/devicedefinitions/PlocSupervisorDefinitions.h" @@ -21,7 +21,7 @@ * the supervisor and the OBC. * @author J. Meier */ -class PlocSupvHelper : public SystemObject, public ExecutableObjectIF, public HasReturnvaluesIF { +class PlocSupvHelper : public SystemObject, public ExecutableObjectIF { public: static const uint8_t SUBSYSTEM_ID = SUBSYSTEM_ID::PLOC_SUPV_HELPER; @@ -114,7 +114,7 @@ class PlocSupvHelper : public SystemObject, public ExecutableObjectIF, public Ha * @param memoryId ID of the memory where to write to * @param startAddress Address where to write data * - * @return RETURN_OK if successful, otherwise error return value + * @return returnvalue::OK if successful, otherwise error return value */ ReturnValue_t performUpdate(const supv::UpdateParams& params); ReturnValue_t startUpdate(std::string file, uint8_t memoryId, uint32_t startAddress); diff --git a/linux/devices/startracker/ArcsecDatalinkLayer.cpp b/linux/devices/startracker/ArcsecDatalinkLayer.cpp index 24eab940..bc45d619 100644 --- a/linux/devices/startracker/ArcsecDatalinkLayer.cpp +++ b/linux/devices/startracker/ArcsecDatalinkLayer.cpp @@ -34,15 +34,15 @@ ReturnValue_t ArcsecDatalinkLayer::decodeFrame(const uint8_t* rawData, size_t ra case ARC_DEC_SYNC: { // Reset length of SLIP struct for next frame slipInfo.length = 0; - return RETURN_OK; + return returnvalue::OK; } default: sif::debug << "ArcsecDatalinkLayer::decodeFrame: Unknown result code" << std::endl; break; - return RETURN_FAILED; + return returnvalue::FAILED; } } - return RETURN_FAILED; + return returnvalue::FAILED; } uint8_t ArcsecDatalinkLayer::getReplyFrameType() { return decodedFrame[0]; } diff --git a/linux/devices/startracker/ArcsecDatalinkLayer.h b/linux/devices/startracker/ArcsecDatalinkLayer.h index e3da9b6a..ec8caed9 100644 --- a/linux/devices/startracker/ArcsecDatalinkLayer.h +++ b/linux/devices/startracker/ArcsecDatalinkLayer.h @@ -1,7 +1,7 @@ #ifndef BSP_Q7S_DEVICES_ARCSECDATALINKLAYER_H_ #define BSP_Q7S_DEVICES_ARCSECDATALINKLAYER_H_ -#include "fsfw/returnvalues/HasReturnvaluesIF.h" +#include "fsfw/returnvalues/returnvalue.h" #include "linux/devices/devicedefinitions/StarTrackerDefinitions.h" extern "C" { @@ -11,7 +11,7 @@ extern "C" { /** * @brief Helper class to handle the datalinklayer of replies from the star tracker of arcsec. */ -class ArcsecDatalinkLayer : public HasReturnvaluesIF { +class ArcsecDatalinkLayer { public: static const uint8_t INTERFACE_ID = CLASS_ID::STR_HANDLER; diff --git a/linux/devices/startracker/ArcsecJsonParamBase.cpp b/linux/devices/startracker/ArcsecJsonParamBase.cpp index cc8bbc35..05864a3f 100644 --- a/linux/devices/startracker/ArcsecJsonParamBase.cpp +++ b/linux/devices/startracker/ArcsecJsonParamBase.cpp @@ -5,15 +5,15 @@ ArcsecJsonParamBase::ArcsecJsonParamBase(std::string setName) : setName(setName) {} ReturnValue_t ArcsecJsonParamBase::create(std::string fullname, uint8_t* buffer) { - ReturnValue_t result = RETURN_OK; + ReturnValue_t result = returnvalue::OK; result = init(fullname); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { sif::warning << "ArcsecJsonParamBase::create: Failed to init parameter command for set " << setName << std::endl; return result; } result = createCommand(buffer); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { sif::warning << "ArcsecJsonParamBase::create: Failed to create parameter command for set " << setName << std::endl; } @@ -25,7 +25,7 @@ ReturnValue_t ArcsecJsonParamBase::getParam(const std::string name, std::string& if ((*it)[arcseckeys::NAME] == name) { value = (*it)[arcseckeys::VALUE]; convertEmpty(value); - return RETURN_OK; + return returnvalue::OK; } } return PARAM_NOT_EXISTS; @@ -68,7 +68,7 @@ void ArcsecJsonParamBase::addSetParamHeader(uint8_t* buffer, uint8_t setId) { } ReturnValue_t ArcsecJsonParamBase::init(const std::string filename) { - ReturnValue_t result = RETURN_OK; + ReturnValue_t result = returnvalue::OK; if (not std::filesystem::exists(filename)) { sif::warning << "ArcsecJsonParamBase::init: JSON file " << filename << " does not exist" << std::endl; @@ -76,10 +76,10 @@ ReturnValue_t ArcsecJsonParamBase::init(const std::string filename) { } createJsonObject(filename); result = initSet(); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return result; } - return RETURN_OK; + return returnvalue::OK; } void ArcsecJsonParamBase::createJsonObject(const std::string fullname) { @@ -94,7 +94,7 @@ ReturnValue_t ArcsecJsonParamBase::initSet() { for (json::iterator it = properties.begin(); it != properties.end(); ++it) { if ((*it)["name"] == setName) { set = (*it)["fields"]; - return RETURN_OK; + return returnvalue::OK; } } sif::warning << "ArcsecJsonParamBase::initSet: Set " << setName << "not present in json file" diff --git a/linux/devices/startracker/ArcsecJsonParamBase.h b/linux/devices/startracker/ArcsecJsonParamBase.h index 2d20fcae..75b160b2 100644 --- a/linux/devices/startracker/ArcsecJsonParamBase.h +++ b/linux/devices/startracker/ArcsecJsonParamBase.h @@ -5,7 +5,7 @@ #include #include -#include "fsfw/returnvalues/HasReturnvaluesIF.h" +#include "fsfw/returnvalues/returnvalue.h" #include "linux/devices/devicedefinitions/StarTrackerDefinitions.h" extern "C" { @@ -22,7 +22,7 @@ using json = nlohmann::json; * * @author J. Meier */ -class ArcsecJsonParamBase : public HasReturnvaluesIF { +class ArcsecJsonParamBase { public: static const uint8_t INTERFACE_ID = CLASS_ID::ARCSEC_JSON_BASE; //! [EXPORT] : [COMMENT] Specified json file does not exist @@ -32,6 +32,7 @@ class ArcsecJsonParamBase : public HasReturnvaluesIF { //! [EXPORT] : [COMMENT] Requested parameter does not exist in json file static const ReturnValue_t PARAM_NOT_EXISTS = MAKE_RETURN_CODE(3); + virtual ~ArcsecJsonParamBase() = default; /** * @brief Constructor * @@ -60,7 +61,7 @@ class ArcsecJsonParamBase : public HasReturnvaluesIF { * @param name The name of the parameter * @param value The string representation of the read value * - * @return RETURN_OK if successful, otherwise PARAM_NOT_EXISTS + * @return returnvalue::OK if successful, otherwise PARAM_NOT_EXISTS */ ReturnValue_t getParam(const std::string name, std::string& value); @@ -129,7 +130,7 @@ class ArcsecJsonParamBase : public HasReturnvaluesIF { * @param setName The name of the set to work on * * @param return JSON_FILE_NOT_EXISTS if specified file does not exist, otherwise - * RETURN_OK + * returnvalue::OK */ ReturnValue_t init(const std::string filename); diff --git a/linux/devices/startracker/StarTrackerHandler.cpp b/linux/devices/startracker/StarTrackerHandler.cpp index 9b3c7721..698887b8 100644 --- a/linux/devices/startracker/StarTrackerHandler.cpp +++ b/linux/devices/startracker/StarTrackerHandler.cpp @@ -53,9 +53,9 @@ StarTrackerHandler::StarTrackerHandler(object_id_t objectId, object_id_t comIF, StarTrackerHandler::~StarTrackerHandler() {} ReturnValue_t StarTrackerHandler::initialize() { - ReturnValue_t result = RETURN_OK; + ReturnValue_t result = returnvalue::OK; result = DeviceHandlerBase::initialize(); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return result; } @@ -68,13 +68,13 @@ ReturnValue_t StarTrackerHandler::initialize() { ; } result = manager->registerListener(eventQueue->getId()); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return result; } result = manager->subscribeToEventRange(eventQueue->getId(), event::getEventId(StrHelper::IMAGE_UPLOAD_FAILED), event::getEventId(StrHelper::FIRMWARE_UPDATE_FAILED)); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { #if FSFW_CPP_OSTREAM_ENABLED == 1 sif::warning << "StarTrackerHandler::initialize: Failed to subscribe to events from " " str helper" @@ -84,16 +84,16 @@ ReturnValue_t StarTrackerHandler::initialize() { } result = strHelper->setComIF(communicationInterface); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return ObjectManagerIF::CHILD_INIT_FAILED; } strHelper->setComCookie(comCookie); - return RETURN_OK; + return returnvalue::OK; } ReturnValue_t StarTrackerHandler::executeAction(ActionId_t actionId, MessageQueueId_t commandedBy, const uint8_t* data, size_t size) { - ReturnValue_t result = RETURN_OK; + ReturnValue_t result = returnvalue::OK; switch (actionId) { case (startracker::STOP_IMAGE_LOADER): { @@ -122,12 +122,12 @@ ReturnValue_t StarTrackerHandler::executeAction(ActionId_t actionId, MessageQueu } result = checkMode(actionId); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return result; } result = checkCommand(actionId); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return result; } @@ -135,14 +135,14 @@ ReturnValue_t StarTrackerHandler::executeAction(ActionId_t actionId, MessageQueu switch (actionId) { case (startracker::UPLOAD_IMAGE): { result = DeviceHandlerBase::acceptExternalDeviceCommands(); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return result; } if (size > MAX_PATH_SIZE + MAX_FILE_NAME) { return FILE_PATH_TOO_LONG; } result = strHelper->startImageUpload(std::string(reinterpret_cast(data), size)); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return result; } strHelperExecuting = true; @@ -150,7 +150,7 @@ ReturnValue_t StarTrackerHandler::executeAction(ActionId_t actionId, MessageQueu } case (startracker::DOWNLOAD_IMAGE): { result = DeviceHandlerBase::acceptExternalDeviceCommands(); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return result; } if (size > MAX_PATH_SIZE) { @@ -158,7 +158,7 @@ ReturnValue_t StarTrackerHandler::executeAction(ActionId_t actionId, MessageQueu } result = strHelper->startImageDownload(std::string(reinterpret_cast(data), size)); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return result; } strHelperExecuting = true; @@ -166,11 +166,11 @@ ReturnValue_t StarTrackerHandler::executeAction(ActionId_t actionId, MessageQueu } case (startracker::FLASH_READ): { result = DeviceHandlerBase::acceptExternalDeviceCommands(); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return result; } result = executeFlashReadCommand(data, size); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return result; } strHelperExecuting = true; @@ -192,7 +192,7 @@ ReturnValue_t StarTrackerHandler::executeAction(ActionId_t actionId, MessageQueu } case (startracker::FIRMWARE_UPDATE): { result = DeviceHandlerBase::acceptExternalDeviceCommands(); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return result; } if (size > MAX_PATH_SIZE + MAX_FILE_NAME) { @@ -200,7 +200,7 @@ ReturnValue_t StarTrackerHandler::executeAction(ActionId_t actionId, MessageQueu } result = strHelper->startFirmwareUpdate(std::string(reinterpret_cast(data), size)); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return result; } strHelperExecuting = true; @@ -214,7 +214,7 @@ ReturnValue_t StarTrackerHandler::executeAction(ActionId_t actionId, MessageQueu void StarTrackerHandler::performOperationHook() { EventMessage event; - for (ReturnValue_t result = eventQueue->receiveMessage(&event); result == RETURN_OK; + for (ReturnValue_t result = eventQueue->receiveMessage(&event); result == returnvalue::OK; result = eventQueue->receiveMessage(&event)) { switch (event.getMessageId()) { case EventMessage::EVENT_MESSAGE: @@ -385,56 +385,56 @@ ReturnValue_t StarTrackerHandler::buildTransitionDeviceCommand(DeviceCommandId_t ReturnValue_t StarTrackerHandler::buildCommandFromCommand(DeviceCommandId_t deviceCommand, const uint8_t* commandData, size_t commandDataLen) { - ReturnValue_t result = RETURN_OK; + ReturnValue_t result = returnvalue::OK; switch (deviceCommand) { case (startracker::PING_REQUEST): { preparePingRequest(); - return RETURN_OK; + return returnvalue::OK; } case (startracker::REQ_TIME): { prepareTimeRequest(); - return RETURN_OK; + return returnvalue::OK; } case (startracker::BOOT): { prepareBootCommand(); - return RETURN_OK; + return returnvalue::OK; } case (startracker::REQ_VERSION): { prepareVersionRequest(); - return RETURN_OK; + return returnvalue::OK; } case (startracker::REQ_INTERFACE): { prepareInterfaceRequest(); - return RETURN_OK; + return returnvalue::OK; } case (startracker::REQ_POWER): { preparePowerRequest(); - return RETURN_OK; + return returnvalue::OK; } case (startracker::SWITCH_TO_BOOTLOADER_PROGRAM): { prepareSwitchToBootloaderCmd(); - return RETURN_OK; + return returnvalue::OK; } case (startracker::TAKE_IMAGE): { prepareTakeImageCommand(commandData); - return RETURN_OK; + return returnvalue::OK; } case (startracker::SUBSCRIPTION): { Subscription subscription; result = prepareParamCommand(commandData, commandDataLen, subscription); - return RETURN_OK; + return returnvalue::OK; } case (startracker::REQ_SOLUTION): { prepareSolutionRequest(); - return RETURN_OK; + return returnvalue::OK; } case (startracker::REQ_TEMPERATURE): { prepareTemperatureRequest(); - return RETURN_OK; + return returnvalue::OK; } case (startracker::REQ_HISTOGRAM): { prepareHistogramRequest(); - return RETURN_OK; + return returnvalue::OK; } case (startracker::LIMITS): { Limits limits; @@ -564,7 +564,7 @@ ReturnValue_t StarTrackerHandler::buildCommandFromCommand(DeviceCommandId_t devi default: return DeviceHandlerIF::COMMAND_NOT_IMPLEMENTED; } - return HasReturnvaluesIF::RETURN_FAILED; + return returnvalue::FAILED; } void StarTrackerHandler::fillCommandAndReplyMap() { @@ -662,13 +662,13 @@ ReturnValue_t StarTrackerHandler::isModeCombinationValid(Mode_t mode, Submode_t case MODE_NORMAL: case MODE_RAW: if (submode == SUBMODE_NONE) { - return RETURN_OK; + return returnvalue::OK; } else { return INVALID_SUBMODE; } case MODE_ON: if (submode == SUBMODE_BOOTLOADER || submode == SUBMODE_FIRMWARE) { - return RETURN_OK; + return returnvalue::OK; } else { return INVALID_SUBMODE; } @@ -759,7 +759,7 @@ void StarTrackerHandler::bootBootloader() { ReturnValue_t StarTrackerHandler::scanForReply(const uint8_t* start, size_t remainingSize, DeviceCommandId_t* foundId, size_t* foundLen) { - ReturnValue_t result = RETURN_OK; + ReturnValue_t result = returnvalue::OK; size_t bytesLeft = 0; result = dataLinkLayer.decodeFrame(start, remainingSize, &bytesLeft); @@ -769,7 +769,7 @@ ReturnValue_t StarTrackerHandler::scanForReply(const uint8_t* start, size_t rema // Need a second doSendRead pass to reaa in whole packet return IGNORE_REPLY_DATA; } - case RETURN_OK: { + case returnvalue::OK: { break; } default: @@ -801,7 +801,7 @@ ReturnValue_t StarTrackerHandler::scanForReply(const uint8_t* start, size_t rema } default: { sif::debug << "StarTrackerHandler::scanForReply: Reply has invalid type id" << std::endl; - result = RETURN_FAILED; + result = returnvalue::FAILED; } } @@ -812,7 +812,7 @@ ReturnValue_t StarTrackerHandler::scanForReply(const uint8_t* start, size_t rema ReturnValue_t StarTrackerHandler::interpretDeviceReply(DeviceCommandId_t id, const uint8_t* packet) { - ReturnValue_t result = RETURN_OK; + ReturnValue_t result = returnvalue::OK; switch (id) { case (startracker::REQ_TIME): { @@ -832,11 +832,11 @@ ReturnValue_t StarTrackerHandler::interpretDeviceReply(DeviceCommandId_t id, } case (startracker::REQ_VERSION): { result = handleTm(versionSet, startracker::VersionSet::SIZE); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return result; } result = checkProgram(); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return result; } break; @@ -1209,7 +1209,7 @@ ReturnValue_t StarTrackerHandler::initializeLocalDataPool(localpool::DataPool& l localDataPoolMap.emplace(startracker::DEBUG_CAMERA_TEST, new PoolEntry({0})); localDataPoolMap.emplace(startracker::CHKSUM, new PoolEntry({0})); - return RETURN_OK; + return returnvalue::OK; } size_t StarTrackerHandler::getNextReplyLength(DeviceCommandId_t commandId) { @@ -1219,9 +1219,9 @@ size_t StarTrackerHandler::getNextReplyLength(DeviceCommandId_t commandId) { ReturnValue_t StarTrackerHandler::doSendReadHook() { // Prevent DHB from polling UART during commands executed by the image loader task if (strHelperExecuting) { - return RETURN_FAILED; + return returnvalue::FAILED; } - return RETURN_OK; + return returnvalue::OK; } ReturnValue_t StarTrackerHandler::getSwitches(const uint8_t** switches, uint8_t* numberOfSwitches) { @@ -1230,7 +1230,7 @@ ReturnValue_t StarTrackerHandler::getSwitches(const uint8_t** switches, uint8_t* } *numberOfSwitches = 1; *switches = &powerSwitch; - return RETURN_OK; + return returnvalue::OK; } ReturnValue_t StarTrackerHandler::checkMode(ActionId_t actionId) { @@ -1244,7 +1244,7 @@ ReturnValue_t StarTrackerHandler::checkMode(ActionId_t actionId) { break; } } - return RETURN_OK; + return returnvalue::OK; } ReturnValue_t StarTrackerHandler::scanForActionReply(DeviceCommandId_t* foundId) { @@ -1273,9 +1273,9 @@ ReturnValue_t StarTrackerHandler::scanForActionReply(DeviceCommandId_t* foundId) default: sif::warning << "StarTrackerHandler::scanForActionReply: Unknown parameter reply id" << std::endl; - return RETURN_FAILED; + return returnvalue::FAILED; } - return RETURN_OK; + return returnvalue::OK; } ReturnValue_t StarTrackerHandler::scanForSetParameterReply(DeviceCommandId_t* foundId) { @@ -1340,9 +1340,9 @@ ReturnValue_t StarTrackerHandler::scanForSetParameterReply(DeviceCommandId_t* fo default: sif::debug << "StarTrackerHandler::scanForParameterReply: Unknown parameter reply id" << std::endl; - return RETURN_FAILED; + return returnvalue::FAILED; } - return RETURN_OK; + return returnvalue::OK; } ReturnValue_t StarTrackerHandler::scanForGetParameterReply(DeviceCommandId_t* foundId) { @@ -1406,11 +1406,11 @@ ReturnValue_t StarTrackerHandler::scanForGetParameterReply(DeviceCommandId_t* fo } default: { sif::warning << "tarTrackerHandler::scanForGetParameterReply: UnkNown ID" << std::endl; - return RETURN_FAILED; + return returnvalue::FAILED; break; } } - return RETURN_OK; + return returnvalue::OK; } ReturnValue_t StarTrackerHandler::scanForTmReply(DeviceCommandId_t* foundId) { @@ -1447,11 +1447,11 @@ ReturnValue_t StarTrackerHandler::scanForTmReply(DeviceCommandId_t* foundId) { default: { sif::debug << "StarTrackerHandler::scanForTmReply: Reply contains invalid reply id: " << static_cast(*reply) << std::endl; - return RETURN_FAILED; + return returnvalue::FAILED; break; } } - return RETURN_OK; + return returnvalue::OK; } void StarTrackerHandler::handleEvent(EventMessage* eventMessage) { @@ -1471,7 +1471,7 @@ void StarTrackerHandler::handleEvent(EventMessage* eventMessage) { ReturnValue_t StarTrackerHandler::executeFlashReadCommand(const uint8_t* commandData, size_t commandDataLen) { - ReturnValue_t result = RETURN_OK; + ReturnValue_t result = returnvalue::OK; if (commandDataLen < FlashReadCmd::MIN_LENGTH) { sif::warning << "StarTrackerHandler::executeFlashReadCommand: Command too short" << std::endl; return COMMAND_TOO_SHORT; @@ -1481,7 +1481,7 @@ ReturnValue_t StarTrackerHandler::executeFlashReadCommand(const uint8_t* command size_t size = sizeof(length); const uint8_t* lengthPtr = commandData + sizeof(startRegion); result = SerializeAdapter::deSerialize(&length, lengthPtr, &size, SerializeIF::Endianness::BIG); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { sif::debug << "StarTrackerHandler::executeFlashReadCommand: Deserialization of length failed" << std::endl; return result; @@ -1495,7 +1495,7 @@ ReturnValue_t StarTrackerHandler::executeFlashReadCommand(const uint8_t* command std::string fullname = std::string(reinterpret_cast(filePtr), commandDataLen - sizeof(startRegion) - sizeof(length)); result = strHelper->startFlashRead(fullname, startRegion, length); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return result; } return result; @@ -1513,7 +1513,7 @@ void StarTrackerHandler::prepareBootCommand() { ReturnValue_t StarTrackerHandler::prepareChecksumCommand(const uint8_t* commandData, size_t commandDataLen) { struct ChecksumActionRequest req; - ReturnValue_t result = RETURN_OK; + ReturnValue_t result = returnvalue::OK; if (commandDataLen != ChecksumCmd::LENGTH) { sif::warning << "StarTrackerHandler::prepareChecksumCommand: Invalid length" << std::endl; return INVALID_LENGTH; @@ -1523,7 +1523,7 @@ ReturnValue_t StarTrackerHandler::prepareChecksumCommand(const uint8_t* commandD const uint8_t* addressPtr = commandData + ChecksumCmd::ADDRESS_OFFSET; result = SerializeAdapter::deSerialize(&req.address, addressPtr, &size, SerializeIF::Endianness::BIG); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { sif::debug << "StarTrackerHandler::prepareChecksumCommand: Deserialization of address " << "failed" << std::endl; return result; @@ -1532,7 +1532,7 @@ ReturnValue_t StarTrackerHandler::prepareChecksumCommand(const uint8_t* commandD const uint8_t* lengthPtr = commandData + ChecksumCmd::LENGTH_OFFSET; result = SerializeAdapter::deSerialize(&req.length, lengthPtr, &size, SerializeIF::Endianness::BIG); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { sif::debug << "StarTrackerHandler::prepareChecksumCommand: Deserialization of length failed" << std::endl; return result; @@ -1635,20 +1635,20 @@ void StarTrackerHandler::prepareHistogramRequest() { ReturnValue_t StarTrackerHandler::prepareParamCommand(const uint8_t* commandData, size_t commandDataLen, ArcsecJsonParamBase& paramSet) { - ReturnValue_t result = RETURN_OK; + ReturnValue_t result = returnvalue::OK; if (commandDataLen > MAX_PATH_SIZE) { return FILE_PATH_TOO_LONG; } std::string fullName(reinterpret_cast(commandData), commandDataLen); result = paramSet.create(fullName, commandBuffer); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return result; } dataLinkLayer.encodeFrame(commandBuffer, paramSet.getSize()); rawPacket = dataLinkLayer.getEncodedFrame(); rawPacketLen = dataLinkLayer.getEncodedLength(); - return RETURN_OK; + return returnvalue::OK; } ReturnValue_t StarTrackerHandler::prepareRequestCameraParams() { @@ -1657,7 +1657,7 @@ ReturnValue_t StarTrackerHandler::prepareRequestCameraParams() { dataLinkLayer.encodeFrame(commandBuffer, length); rawPacket = dataLinkLayer.getEncodedFrame(); rawPacketLen = dataLinkLayer.getEncodedLength(); - return RETURN_OK; + return returnvalue::OK; } ReturnValue_t StarTrackerHandler::prepareRequestLimitsParams() { @@ -1666,7 +1666,7 @@ ReturnValue_t StarTrackerHandler::prepareRequestLimitsParams() { dataLinkLayer.encodeFrame(commandBuffer, length); rawPacket = dataLinkLayer.getEncodedFrame(); rawPacketLen = dataLinkLayer.getEncodedLength(); - return RETURN_OK; + return returnvalue::OK; } ReturnValue_t StarTrackerHandler::prepareRequestLogLevelParams() { @@ -1675,7 +1675,7 @@ ReturnValue_t StarTrackerHandler::prepareRequestLogLevelParams() { dataLinkLayer.encodeFrame(commandBuffer, length); rawPacket = dataLinkLayer.getEncodedFrame(); rawPacketLen = dataLinkLayer.getEncodedLength(); - return RETURN_OK; + return returnvalue::OK; } ReturnValue_t StarTrackerHandler::prepareRequestMountingParams() { @@ -1684,7 +1684,7 @@ ReturnValue_t StarTrackerHandler::prepareRequestMountingParams() { dataLinkLayer.encodeFrame(commandBuffer, length); rawPacket = dataLinkLayer.getEncodedFrame(); rawPacketLen = dataLinkLayer.getEncodedLength(); - return RETURN_OK; + return returnvalue::OK; } ReturnValue_t StarTrackerHandler::prepareRequestImageProcessorParams() { @@ -1693,7 +1693,7 @@ ReturnValue_t StarTrackerHandler::prepareRequestImageProcessorParams() { dataLinkLayer.encodeFrame(commandBuffer, length); rawPacket = dataLinkLayer.getEncodedFrame(); rawPacketLen = dataLinkLayer.getEncodedLength(); - return RETURN_OK; + return returnvalue::OK; } ReturnValue_t StarTrackerHandler::prepareRequestCentroidingParams() { @@ -1702,7 +1702,7 @@ ReturnValue_t StarTrackerHandler::prepareRequestCentroidingParams() { dataLinkLayer.encodeFrame(commandBuffer, length); rawPacket = dataLinkLayer.getEncodedFrame(); rawPacketLen = dataLinkLayer.getEncodedLength(); - return RETURN_OK; + return returnvalue::OK; } ReturnValue_t StarTrackerHandler::prepareRequestLisaParams() { @@ -1711,7 +1711,7 @@ ReturnValue_t StarTrackerHandler::prepareRequestLisaParams() { dataLinkLayer.encodeFrame(commandBuffer, length); rawPacket = dataLinkLayer.getEncodedFrame(); rawPacketLen = dataLinkLayer.getEncodedLength(); - return RETURN_OK; + return returnvalue::OK; } ReturnValue_t StarTrackerHandler::prepareRequestMatchingParams() { @@ -1720,7 +1720,7 @@ ReturnValue_t StarTrackerHandler::prepareRequestMatchingParams() { dataLinkLayer.encodeFrame(commandBuffer, length); rawPacket = dataLinkLayer.getEncodedFrame(); rawPacketLen = dataLinkLayer.getEncodedLength(); - return RETURN_OK; + return returnvalue::OK; } ReturnValue_t StarTrackerHandler::prepareRequestTrackingParams() { @@ -1729,7 +1729,7 @@ ReturnValue_t StarTrackerHandler::prepareRequestTrackingParams() { dataLinkLayer.encodeFrame(commandBuffer, length); rawPacket = dataLinkLayer.getEncodedFrame(); rawPacketLen = dataLinkLayer.getEncodedLength(); - return RETURN_OK; + return returnvalue::OK; } ReturnValue_t StarTrackerHandler::prepareRequestValidationParams() { @@ -1738,7 +1738,7 @@ ReturnValue_t StarTrackerHandler::prepareRequestValidationParams() { dataLinkLayer.encodeFrame(commandBuffer, length); rawPacket = dataLinkLayer.getEncodedFrame(); rawPacketLen = dataLinkLayer.getEncodedLength(); - return RETURN_OK; + return returnvalue::OK; } ReturnValue_t StarTrackerHandler::prepareRequestAlgoParams() { @@ -1747,7 +1747,7 @@ ReturnValue_t StarTrackerHandler::prepareRequestAlgoParams() { dataLinkLayer.encodeFrame(commandBuffer, length); rawPacket = dataLinkLayer.getEncodedFrame(); rawPacketLen = dataLinkLayer.getEncodedLength(); - return RETURN_OK; + return returnvalue::OK; } ReturnValue_t StarTrackerHandler::prepareRequestSubscriptionParams() { @@ -1756,7 +1756,7 @@ ReturnValue_t StarTrackerHandler::prepareRequestSubscriptionParams() { dataLinkLayer.encodeFrame(commandBuffer, length); rawPacket = dataLinkLayer.getEncodedFrame(); rawPacketLen = dataLinkLayer.getEncodedLength(); - return RETURN_OK; + return returnvalue::OK; } ReturnValue_t StarTrackerHandler::prepareRequestLogSubscriptionParams() { @@ -1765,7 +1765,7 @@ ReturnValue_t StarTrackerHandler::prepareRequestLogSubscriptionParams() { dataLinkLayer.encodeFrame(commandBuffer, length); rawPacket = dataLinkLayer.getEncodedFrame(); rawPacketLen = dataLinkLayer.getEncodedLength(); - return RETURN_OK; + return returnvalue::OK; } ReturnValue_t StarTrackerHandler::prepareRequestDebugCameraParams() { @@ -1774,7 +1774,7 @@ ReturnValue_t StarTrackerHandler::prepareRequestDebugCameraParams() { dataLinkLayer.encodeFrame(commandBuffer, length); rawPacket = dataLinkLayer.getEncodedFrame(); rawPacketLen = dataLinkLayer.getEncodedLength(); - return RETURN_OK; + return returnvalue::OK; } ReturnValue_t StarTrackerHandler::handleSetParamReply() { @@ -1792,7 +1792,7 @@ ReturnValue_t StarTrackerHandler::handleSetParamReply() { if (internalState != InternalState::IDLE) { handleStartup(reply + PARAMETER_ID_OFFSET); } - return RETURN_OK; + return returnvalue::OK; } ReturnValue_t StarTrackerHandler::handleActionReply() { @@ -1805,13 +1805,13 @@ ReturnValue_t StarTrackerHandler::handleActionReply() { << static_cast(status) << std::endl; return ACTION_FAILED; } - return RETURN_OK; + return returnvalue::OK; } ReturnValue_t StarTrackerHandler::handleChecksumReply() { - ReturnValue_t result = RETURN_OK; + ReturnValue_t result = returnvalue::OK; result = handleActionReply(); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return result; } const uint8_t* replyData = dataLinkLayer.getReply() + ACTION_DATA_OFFSET; @@ -1834,25 +1834,25 @@ ReturnValue_t StarTrackerHandler::handleChecksumReply() { #if OBSW_VERBOSE_LEVEL >= 1 && OBSW_DEBUG_STARTRACKER == 1 checksumReply.printChecksum(); #endif /* OBSW_VERBOSE_LEVEL >= 1 && OBSW_DEBUG_STARTRACKER == 1 */ - return RETURN_OK; + return returnvalue::OK; } ReturnValue_t StarTrackerHandler::handleParamRequest(LocalPoolDataSetBase& dataset, size_t size) { - ReturnValue_t result = RETURN_OK; + ReturnValue_t result = returnvalue::OK; result = dataset.read(TIMEOUT_TYPE, MUTEX_TIMEOUT); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return result; } const uint8_t* reply = dataLinkLayer.getReply() + PARAMS_OFFSET; dataset.setValidityBufferGeneration(false); result = dataset.deSerialize(&reply, &size, SerializeIF::Endianness::LITTLE); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { sif::warning << "StarTrackerHandler::handleParamRequest Deserialization failed" << std::endl; } dataset.setValidityBufferGeneration(true); dataset.setValidity(true, true); result = dataset.commit(TIMEOUT_TYPE, MUTEX_TIMEOUT); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return result; } #if OBSW_VERBOSE_LEVEL >= 1 && OBSW_DEBUG_STARTRACKER == 1 @@ -1862,7 +1862,7 @@ ReturnValue_t StarTrackerHandler::handleParamRequest(LocalPoolDataSetBase& datas } ReturnValue_t StarTrackerHandler::handlePingReply() { - ReturnValue_t result = RETURN_OK; + ReturnValue_t result = returnvalue::OK; uint32_t pingId = 0; const uint8_t* reply = dataLinkLayer.getReply(); uint8_t status = dataLinkLayer.getStatusField(); @@ -1916,11 +1916,11 @@ ReturnValue_t StarTrackerHandler::checkProgram() { << std::endl; return INVALID_PROGRAM; } - return RETURN_OK; + return returnvalue::OK; } ReturnValue_t StarTrackerHandler::handleTm(LocalPoolDataSetBase& dataset, size_t size) { - ReturnValue_t result = RETURN_OK; + ReturnValue_t result = returnvalue::OK; uint8_t status = *(dataLinkLayer.getReply() + STATUS_OFFSET); if (status != startracker::STATUS_OK) { sif::warning << "StarTrackerHandler::handleTm: Reply error: " @@ -1928,19 +1928,19 @@ ReturnValue_t StarTrackerHandler::handleTm(LocalPoolDataSetBase& dataset, size_t return REPLY_ERROR; } result = dataset.read(TIMEOUT_TYPE, MUTEX_TIMEOUT); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return result; } const uint8_t* reply = dataLinkLayer.getReply() + TICKS_OFFSET; dataset.setValidityBufferGeneration(false); result = dataset.deSerialize(&reply, &size, SerializeIF::Endianness::LITTLE); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { sif::warning << "StarTrackerHandler::handleTm: Deserialization failed" << std::endl; } dataset.setValidityBufferGeneration(true); dataset.setValidity(true, true); result = dataset.commit(TIMEOUT_TYPE, MUTEX_TIMEOUT); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return result; } #if OBSW_VERBOSE_LEVEL >= 1 && OBSW_DEBUG_STARTRACKER == 1 @@ -1950,7 +1950,7 @@ ReturnValue_t StarTrackerHandler::handleTm(LocalPoolDataSetBase& dataset, size_t } ReturnValue_t StarTrackerHandler::handleActionReplySet(LocalPoolDataSetBase& dataset, size_t size) { - ReturnValue_t result = RETURN_OK; + ReturnValue_t result = returnvalue::OK; uint8_t status = *(dataLinkLayer.getReply() + STATUS_OFFSET); if (status != startracker::STATUS_OK) { sif::warning << "StarTrackerHandler::handleActionReplySet: Reply error: " @@ -1958,19 +1958,19 @@ ReturnValue_t StarTrackerHandler::handleActionReplySet(LocalPoolDataSetBase& dat return REPLY_ERROR; } result = dataset.read(TIMEOUT_TYPE, MUTEX_TIMEOUT); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return result; } const uint8_t* reply = dataLinkLayer.getReply() + ACTION_DATA_OFFSET; dataset.setValidityBufferGeneration(false); result = dataset.deSerialize(&reply, &size, SerializeIF::Endianness::LITTLE); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { sif::warning << "StarTrackerHandler::handleActionReplySet Deserialization failed" << std::endl; } dataset.setValidityBufferGeneration(true); dataset.setValidity(true, true); result = dataset.commit(TIMEOUT_TYPE, MUTEX_TIMEOUT); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return result; } #if OBSW_VERBOSE_LEVEL >= 1 && OBSW_DEBUG_STARTRACKER == 1 @@ -2080,5 +2080,5 @@ ReturnValue_t StarTrackerHandler::checkCommand(ActionId_t actionId) { default: break; } - return RETURN_OK; + return returnvalue::OK; } diff --git a/linux/devices/startracker/StarTrackerHandler.h b/linux/devices/startracker/StarTrackerHandler.h index 90a30966..beab9d54 100644 --- a/linux/devices/startracker/StarTrackerHandler.h +++ b/linux/devices/startracker/StarTrackerHandler.h @@ -314,7 +314,7 @@ class StarTrackerHandler : public DeviceHandlerBase { * @param commandData Pointer to received command data * @param commandDataLen Size of received command data * - * @return RETURN_OK if start of execution was successful, otherwise error return value + * @return returnvalue::OK if start of execution was successful, otherwise error return value */ ReturnValue_t executeFlashReadCommand(const uint8_t* commandData, size_t commandDataLen); @@ -385,7 +385,7 @@ class StarTrackerHandler : public DeviceHandlerBase { * @param commandDataLen Length of command * @param paramSet The object defining the command generation * - * @return RETURN_OK if successful, otherwise error return Value + * @return returnvalue::OK if successful, otherwise error return Value */ ReturnValue_t prepareParamCommand(const uint8_t* commandData, size_t commandDataLen, ArcsecJsonParamBase& paramSet); @@ -454,7 +454,7 @@ class StarTrackerHandler : public DeviceHandlerBase { * @param dataset Dataset where reply data will be written to * @param size Size of the dataset * - * @return RETURN_OK if successful, otherwise error return value + * @return returnvalue::OK if successful, otherwise error return value */ ReturnValue_t handleTm(LocalPoolDataSetBase& dataset, size_t size); @@ -463,7 +463,7 @@ class StarTrackerHandler : public DeviceHandlerBase { * * @param actioId Id of received command * - * @return RETURN_OK if star tracker is in valid mode, otherwise error return value + * @return returnvalue::OK if star tracker is in valid mode, otherwise error return value */ ReturnValue_t checkCommand(ActionId_t actionId); diff --git a/linux/devices/startracker/StarTrackerJsonCommands.cpp b/linux/devices/startracker/StarTrackerJsonCommands.cpp index c481c72a..766a1624 100644 --- a/linux/devices/startracker/StarTrackerJsonCommands.cpp +++ b/linux/devices/startracker/StarTrackerJsonCommands.cpp @@ -7,77 +7,77 @@ Limits::Limits() : ArcsecJsonParamBase(arcseckeys::LIMITS) {} size_t Limits::getSize() { return COMMAND_SIZE; } ReturnValue_t Limits::createCommand(uint8_t* buffer) { - ReturnValue_t result = RETURN_OK; + ReturnValue_t result = returnvalue::OK; uint8_t offset = 0; std::string param; addSetParamHeader(buffer, startracker::ID::LIMITS); offset = 2; result = getParam(arcseckeys::ACTION, param); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return result; } adduint8(param, buffer + offset); offset += sizeof(uint8_t); result = getParam(arcseckeys::FPGA18CURRENT, param); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return result; } addfloat(param, buffer + offset); offset += sizeof(float); result = getParam(arcseckeys::FPGA25CURRENT, param); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return result; } addfloat(param, buffer + offset); offset += sizeof(float); result = getParam(arcseckeys::FPGA10CURRENT, param); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return result; } addfloat(param, buffer + offset); offset += sizeof(float); result = getParam(arcseckeys::MCUCURRENT, param); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return result; } addfloat(param, buffer + offset); offset += sizeof(float); result = getParam(arcseckeys::CMOS21CURRENT, param); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return result; } addfloat(param, buffer + offset); offset += sizeof(float); result = getParam(arcseckeys::CMOSPIXCURRENT, param); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return result; } addfloat(param, buffer + offset); offset += sizeof(float); result = getParam(arcseckeys::CMOS33CURRENT, param); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return result; } addfloat(param, buffer + offset); offset += sizeof(float); result = getParam(arcseckeys::CMOSVRESCURRENT, param); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return result; } addfloat(param, buffer + offset); offset += sizeof(float); result = getParam(arcseckeys::CMOS_TEMPERATURE, param); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return result; } addfloat(param, buffer + offset); offset += sizeof(float); result = getParam(arcseckeys::MCU_TEMPERATURE, param); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return result; } addfloat(param, buffer + offset); - return RETURN_OK; + return returnvalue::OK; } Tracking::Tracking() : ArcsecJsonParamBase(arcseckeys::TRACKING) {} @@ -85,35 +85,35 @@ Tracking::Tracking() : ArcsecJsonParamBase(arcseckeys::TRACKING) {} size_t Tracking::getSize() { return COMMAND_SIZE; } ReturnValue_t Tracking::createCommand(uint8_t* buffer) { - ReturnValue_t result = RETURN_OK; + ReturnValue_t result = returnvalue::OK; uint8_t offset = 0; std::string param; addSetParamHeader(buffer, startracker::ID::TRACKING); offset = 2; result = getParam(arcseckeys::THIN_LIMIT, param); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return result; } addfloat(param, buffer + offset); offset += sizeof(float); result = getParam(arcseckeys::OUTLIER_THRESHOLD, param); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return result; } addfloat(param, buffer + offset); offset += sizeof(float); result = getParam(arcseckeys::OUTLIER_THRESHOLD_QUEST, param); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return result; } addfloat(param, buffer + offset); offset += sizeof(float); result = getParam(arcseckeys::TRACKER_CHOICE, param); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return result; } adduint8(param, buffer + offset); - return RETURN_OK; + return returnvalue::OK; } Mounting::Mounting() : ArcsecJsonParamBase(arcseckeys::MOUNTING) {} @@ -121,35 +121,35 @@ Mounting::Mounting() : ArcsecJsonParamBase(arcseckeys::MOUNTING) {} size_t Mounting::getSize() { return COMMAND_SIZE; } ReturnValue_t Mounting::createCommand(uint8_t* buffer) { - ReturnValue_t result = RETURN_OK; + ReturnValue_t result = returnvalue::OK; uint8_t offset = 0; std::string param; addSetParamHeader(buffer, startracker::ID::MOUNTING); offset = 2; result = getParam(arcseckeys::qw, param); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return result; } addfloat(param, buffer + offset); offset += sizeof(float); result = getParam(arcseckeys::qx, param); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return result; } addfloat(param, buffer + offset); offset += sizeof(float); result = getParam(arcseckeys::qy, param); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return result; } addfloat(param, buffer + offset); offset += sizeof(float); result = getParam(arcseckeys::qz, param); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return result; } addfloat(param, buffer + offset); - return RETURN_OK; + return returnvalue::OK; } ImageProcessor::ImageProcessor() : ArcsecJsonParamBase(arcseckeys::IMAGE_PROCESSOR) {} @@ -157,41 +157,41 @@ ImageProcessor::ImageProcessor() : ArcsecJsonParamBase(arcseckeys::IMAGE_PROCESS size_t ImageProcessor::getSize() { return COMMAND_SIZE; } ReturnValue_t ImageProcessor::createCommand(uint8_t* buffer) { - ReturnValue_t result = RETURN_OK; + ReturnValue_t result = returnvalue::OK; uint8_t offset = 0; std::string param; addSetParamHeader(buffer, startracker::ID::IMAGE_PROCESSOR); offset = 2; result = getParam(arcseckeys::IMAGE_PROCESSOR_MODE, param); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return result; } adduint8(param, buffer + offset); offset += sizeof(uint8_t); result = getParam(arcseckeys::STORE, param); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return result; } adduint8(param, buffer + offset); offset += sizeof(uint8_t); result = getParam(arcseckeys::SIGNAL_THRESHOLD, param); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return result; } adduint16(param, buffer + offset); offset += sizeof(uint16_t); result = getParam(arcseckeys::IMAGE_PROCESSOR_DARK_THRESHOLD, param); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return result; } adduint16(param, buffer + offset); offset += sizeof(uint16_t); result = getParam(arcseckeys::BACKGROUND_COMPENSATION, param); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return result; } adduint8(param, buffer + offset); - return RETURN_OK; + return returnvalue::OK; } Camera::Camera() : ArcsecJsonParamBase(arcseckeys::CAMERA) {} @@ -199,155 +199,155 @@ Camera::Camera() : ArcsecJsonParamBase(arcseckeys::CAMERA) {} size_t Camera::getSize() { return COMMAND_SIZE; } ReturnValue_t Camera::createCommand(uint8_t* buffer) { - ReturnValue_t result = RETURN_OK; + ReturnValue_t result = returnvalue::OK; uint8_t offset = 0; std::string param; addSetParamHeader(buffer, startracker::ID::CAMERA); offset = 2; result = getParam(arcseckeys::MODE, param); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return result; } adduint8(param, buffer + offset); offset += sizeof(uint8_t); result = getParam(arcseckeys::FOCALLENGTH, param); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return result; } addfloat(param, buffer + offset); offset += sizeof(float); result = getParam(arcseckeys::EXPOSURE, param); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return result; } addfloat(param, buffer + offset); offset += sizeof(float); result = getParam(arcseckeys::INTERVAL, param); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return result; } addfloat(param, buffer + offset); offset += sizeof(float); result = getParam(arcseckeys::OFFSET, param); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return result; } addint16(param, buffer + offset); offset += sizeof(int16_t); result = getParam(arcseckeys::PGAGAIN, param); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return result; } adduint8(param, buffer + offset); offset += sizeof(uint8_t); result = getParam(arcseckeys::ADCGAIN, param); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return result; } adduint8(param, buffer + offset); offset += sizeof(uint8_t); result = getParam(arcseckeys::REG_1, param); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return result; } adduint8(param, buffer + offset); offset += sizeof(uint8_t); result = getParam(arcseckeys::VAL_1, param); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return result; } adduint8(param, buffer + offset); offset += sizeof(uint8_t); result = getParam(arcseckeys::REG_2, param); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return result; } adduint8(param, buffer + offset); offset += sizeof(uint8_t); result = getParam(arcseckeys::VAL_2, param); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return result; } adduint8(param, buffer + offset); offset += sizeof(uint8_t); result = getParam(arcseckeys::REG_3, param); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return result; } adduint8(param, buffer + offset); offset += sizeof(uint8_t); result = getParam(arcseckeys::VAL_3, param); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return result; } adduint8(param, buffer + offset); offset += sizeof(uint8_t); result = getParam(arcseckeys::REG_4, param); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return result; } adduint8(param, buffer + offset); offset += sizeof(uint8_t); result = getParam(arcseckeys::VAL_4, param); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return result; } adduint8(param, buffer + offset); offset += sizeof(uint8_t); result = getParam(arcseckeys::REG_5, param); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return result; } adduint8(param, buffer + offset); offset += sizeof(uint8_t); result = getParam(arcseckeys::VAL_5, param); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return result; } adduint8(param, buffer + offset); offset += sizeof(uint8_t); result = getParam(arcseckeys::REG_6, param); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return result; } adduint8(param, buffer + offset); offset += sizeof(uint8_t); result = getParam(arcseckeys::VAL_6, param); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return result; } adduint8(param, buffer + offset); offset += sizeof(uint8_t); result = getParam(arcseckeys::REG_7, param); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return result; } adduint8(param, buffer + offset); offset += sizeof(uint8_t); result = getParam(arcseckeys::VAL_7, param); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return result; } adduint8(param, buffer + offset); offset += sizeof(uint8_t); result = getParam(arcseckeys::REG_8, param); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return result; } adduint8(param, buffer + offset); offset += sizeof(uint8_t); result = getParam(arcseckeys::VAL_8, param); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return result; } adduint8(param, buffer + offset); offset += sizeof(uint8_t); result = getParam(arcseckeys::FREQ_1, param); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return result; } adduint32(param, buffer + offset); - return RETURN_OK; + return returnvalue::OK; } Centroiding::Centroiding() : ArcsecJsonParamBase(arcseckeys::CENTROIDING) {} @@ -355,89 +355,89 @@ Centroiding::Centroiding() : ArcsecJsonParamBase(arcseckeys::CENTROIDING) {} size_t Centroiding::getSize() { return COMMAND_SIZE; } ReturnValue_t Centroiding::createCommand(uint8_t* buffer) { - ReturnValue_t result = RETURN_OK; + ReturnValue_t result = returnvalue::OK; uint8_t offset = 0; std::string param; addSetParamHeader(buffer, startracker::ID::CENTROIDING); offset = 2; result = getParam(arcseckeys::ENABLE_FILTER, param); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return result; } adduint8(param, buffer + offset); offset += sizeof(uint8_t); result = getParam(arcseckeys::MAX_QUALITY, param); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return result; } addfloat(param, buffer + offset); offset += sizeof(float); result = getParam(arcseckeys::DARK_THRESHOLD, param); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return result; } addfloat(param, buffer + offset); offset += sizeof(float); result = getParam(arcseckeys::MIN_QUALITY, param); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return result; } addfloat(param, buffer + offset); offset += sizeof(float); result = getParam(arcseckeys::MAX_INTENSITY, param); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return result; } addfloat(param, buffer + offset); offset += sizeof(float); result = getParam(arcseckeys::MIN_INTENSITY, param); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return result; } addfloat(param, buffer + offset); offset += sizeof(float); result = getParam(arcseckeys::MAX_MAGNITUDE, param); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return result; } addfloat(param, buffer + offset); offset += sizeof(float); result = getParam(arcseckeys::GAUSSIAN_CMAX, param); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return result; } addfloat(param, buffer + offset); offset += sizeof(float); result = getParam(arcseckeys::GAUSSIAN_CMIN, param); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return result; } addfloat(param, buffer + offset); offset += sizeof(float); result = getParam(arcseckeys::TRANSMATRIX_00, param); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return result; } addfloat(param, buffer + offset); offset += sizeof(float); result = getParam(arcseckeys::TRANSMATRIX_01, param); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return result; } addfloat(param, buffer + offset); offset += sizeof(float); result = getParam(arcseckeys::TRANSMATRIX_10, param); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return result; } addfloat(param, buffer + offset); offset += sizeof(float); result = getParam(arcseckeys::TRANSMATRIX_11, param); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return result; } addfloat(param, buffer + offset); - return RETURN_OK; + return returnvalue::OK; } Lisa::Lisa() : ArcsecJsonParamBase(arcseckeys::LISA) {} @@ -445,96 +445,96 @@ Lisa::Lisa() : ArcsecJsonParamBase(arcseckeys::LISA) {} size_t Lisa::getSize() { return COMMAND_SIZE; } ReturnValue_t Lisa::createCommand(uint8_t* buffer) { - ReturnValue_t result = RETURN_OK; + ReturnValue_t result = returnvalue::OK; uint8_t offset = 0; std::string param; addSetParamHeader(buffer, startracker::ID::LISA); offset = 2; result = getParam(arcseckeys::LISA_MODE, param); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return result; } adduint32(param, buffer + offset); offset += sizeof(uint32_t); result = getParam(arcseckeys::PREFILTER_DIST_THRESHOLD, param); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return result; } addfloat(param, buffer + offset); offset += sizeof(float); result = getParam(arcseckeys::PREFILTER_ANGLE_THRESHOLD, param); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return result; } addfloat(param, buffer + offset); offset += sizeof(float); result = getParam(arcseckeys::FOV_WIDTH, param); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return result; } addfloat(param, buffer + offset); offset += sizeof(float); result = getParam(arcseckeys::FOV_HEIGHT, param); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return result; } addfloat(param, buffer + offset); offset += sizeof(float); result = getParam(arcseckeys::FLOAT_STAR_LIMIT, param); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return result; } addfloat(param, buffer + offset); offset += sizeof(float); result = getParam(arcseckeys::CLOSE_STAR_LIMIT, param); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return result; } addfloat(param, buffer + offset); offset += sizeof(float); result = getParam(arcseckeys::RATING_WEIGHT_CLOSE_STAR_COUNT, param); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return result; } addfloat(param, buffer + offset); offset += sizeof(float); result = getParam(arcseckeys::RATING_WEIGHT_FRACTION_CLOSE, param); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return result; } addfloat(param, buffer + offset); offset += sizeof(float); result = getParam(arcseckeys::RATING_WEIGHT_MEAN_SUM, param); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return result; } addfloat(param, buffer + offset); offset += sizeof(float); result = getParam(arcseckeys::RATING_WEIGHT_DB_STAR_COUNT, param); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return result; } addfloat(param, buffer + offset); offset += sizeof(float); result = getParam(arcseckeys::MAX_COMBINATIONS, param); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return result; } adduint8(param, buffer + offset); offset += sizeof(uint8_t); result = getParam(arcseckeys::NR_STARS_STOP, param); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return result; } adduint8(param, buffer + offset); offset += sizeof(uint8_t); result = getParam(arcseckeys::FRACTION_CLOSE_STOP, param); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return result; } addfloat(param, buffer + offset); offset += sizeof(float); - return RETURN_OK; + return returnvalue::OK; } Matching::Matching() : ArcsecJsonParamBase(arcseckeys::MATCHING) {} @@ -542,23 +542,23 @@ Matching::Matching() : ArcsecJsonParamBase(arcseckeys::MATCHING) {} size_t Matching::getSize() { return COMMAND_SIZE; } ReturnValue_t Matching::createCommand(uint8_t* buffer) { - ReturnValue_t result = RETURN_OK; + ReturnValue_t result = returnvalue::OK; uint8_t offset = 0; std::string param; addSetParamHeader(buffer, startracker::ID::MATCHING); offset = 2; result = getParam(arcseckeys::SQUARED_DISTANCE_LIMIT, param); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return result; } addfloat(param, buffer + offset); offset += sizeof(float); result = getParam(arcseckeys::SQUARED_SHIFT_LIMIT, param); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return result; } addfloat(param, buffer + offset); - return RETURN_OK; + return returnvalue::OK; } Validation::Validation() : ArcsecJsonParamBase(arcseckeys::VALIDATION) {} @@ -566,35 +566,35 @@ Validation::Validation() : ArcsecJsonParamBase(arcseckeys::VALIDATION) {} size_t Validation::getSize() { return COMMAND_SIZE; } ReturnValue_t Validation::createCommand(uint8_t* buffer) { - ReturnValue_t result = RETURN_OK; + ReturnValue_t result = returnvalue::OK; uint8_t offset = 0; std::string param; addSetParamHeader(buffer, startracker::ID::VALIDATION); offset = 2; result = getParam(arcseckeys::STABLE_COUNT, param); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return result; } adduint8(param, buffer + offset); offset += sizeof(uint8_t); result = getParam(arcseckeys::MAX_DIFFERENCE, param); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return result; } addfloat(param, buffer + offset); offset += sizeof(float); result = getParam(arcseckeys::MIN_TRACKER_CONFIDENCE, param); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return result; } addfloat(param, buffer + offset); offset += sizeof(float); result = getParam(arcseckeys::MIN_MATCHED_STARS, param); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return result; } adduint8(param, buffer + offset); - return RETURN_OK; + return returnvalue::OK; } Algo::Algo() : ArcsecJsonParamBase(arcseckeys::ALGO) {} @@ -602,41 +602,41 @@ Algo::Algo() : ArcsecJsonParamBase(arcseckeys::ALGO) {} size_t Algo::getSize() { return COMMAND_SIZE; } ReturnValue_t Algo::createCommand(uint8_t* buffer) { - ReturnValue_t result = RETURN_OK; + ReturnValue_t result = returnvalue::OK; uint8_t offset = 0; std::string param; addSetParamHeader(buffer, startracker::ID::ALGO); offset = 2; result = getParam(arcseckeys::MODE, param); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return result; } adduint8(param, buffer + offset); offset += sizeof(uint8_t); result = getParam(arcseckeys::L2T_MIN_CONFIDENCE, param); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return result; } addfloat(param, buffer + offset); offset += sizeof(float); result = getParam(arcseckeys::L2T_MIN_MATCHED, param); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return result; } adduint8(param, buffer + offset); offset += sizeof(uint8_t); result = getParam(arcseckeys::T2L_MIN_CONFIDENCE, param); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return result; } addfloat(param, buffer + offset); offset += sizeof(float); result = getParam(arcseckeys::T2L_MIN_MATCHED, param); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return result; } adduint8(param, buffer + offset); - return RETURN_OK; + return returnvalue::OK; } LogLevel::LogLevel() : ArcsecJsonParamBase(arcseckeys::LOGLEVEL) {} @@ -644,108 +644,108 @@ LogLevel::LogLevel() : ArcsecJsonParamBase(arcseckeys::LOGLEVEL) {} size_t LogLevel::getSize() { return COMMAND_SIZE; } ReturnValue_t LogLevel::createCommand(uint8_t* buffer) { - ReturnValue_t result = RETURN_OK; + ReturnValue_t result = returnvalue::OK; uint8_t offset = 0; std::string param; addSetParamHeader(buffer, startracker::ID::LOG_LEVEL); offset = 2; result = getParam(arcseckeys::LOGLEVEL1, param); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return result; } adduint8(param, buffer + offset); offset += sizeof(uint8_t); result = getParam(arcseckeys::LOGLEVEL2, param); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return result; } adduint8(param, buffer + offset); offset += sizeof(uint8_t); result = getParam(arcseckeys::LOGLEVEL3, param); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return result; } adduint8(param, buffer + offset); offset += sizeof(uint8_t); result = getParam(arcseckeys::LOGLEVEL4, param); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return result; } adduint8(param, buffer + offset); offset += sizeof(uint8_t); result = getParam(arcseckeys::LOGLEVEL5, param); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return result; } adduint8(param, buffer + offset); offset += sizeof(uint8_t); result = getParam(arcseckeys::LOGLEVEL6, param); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return result; } adduint8(param, buffer + offset); offset += sizeof(uint8_t); result = getParam(arcseckeys::LOGLEVEL7, param); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return result; } adduint8(param, buffer + offset); offset += sizeof(uint8_t); result = getParam(arcseckeys::LOGLEVEL8, param); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return result; } adduint8(param, buffer + offset); offset += sizeof(uint8_t); result = getParam(arcseckeys::LOGLEVEL9, param); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return result; } adduint8(param, buffer + offset); offset += sizeof(uint8_t); result = getParam(arcseckeys::LOGLEVEL10, param); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return result; } adduint8(param, buffer + offset); offset += sizeof(uint8_t); result = getParam(arcseckeys::LOGLEVEL11, param); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return result; } adduint8(param, buffer + offset); offset += sizeof(uint8_t); result = getParam(arcseckeys::LOGLEVEL12, param); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return result; } adduint8(param, buffer + offset); offset += sizeof(uint8_t); result = getParam(arcseckeys::LOGLEVEL13, param); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return result; } adduint8(param, buffer + offset); offset += sizeof(uint8_t); result = getParam(arcseckeys::LOGLEVEL14, param); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return result; } adduint8(param, buffer + offset); offset += sizeof(uint8_t); result = getParam(arcseckeys::LOGLEVEL15, param); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return result; } adduint8(param, buffer + offset); offset += sizeof(uint8_t); result = getParam(arcseckeys::LOGLEVEL16, param); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return result; } adduint8(param, buffer + offset); - return RETURN_OK; + return returnvalue::OK; } Subscription::Subscription() : ArcsecJsonParamBase(arcseckeys::SUBSCRIPTION) {} @@ -753,107 +753,107 @@ Subscription::Subscription() : ArcsecJsonParamBase(arcseckeys::SUBSCRIPTION) {} size_t Subscription::getSize() { return COMMAND_SIZE; } ReturnValue_t Subscription::createCommand(uint8_t* buffer) { - ReturnValue_t result = RETURN_OK; + ReturnValue_t result = returnvalue::OK; uint8_t offset = 0; std::string param; addSetParamHeader(buffer, startracker::ID::SUBSCRIPTION); offset = 2; result = getParam(arcseckeys::TELEMETRY_1, param); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return result; } adduint8(param, buffer + offset); offset += sizeof(uint8_t); result = getParam(arcseckeys::TELEMETRY_2, param); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return result; } adduint8(param, buffer + offset); offset += sizeof(uint8_t); result = getParam(arcseckeys::TELEMETRY_3, param); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return result; } adduint8(param, buffer + offset); offset += sizeof(uint8_t); result = getParam(arcseckeys::TELEMETRY_4, param); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return result; } adduint8(param, buffer + offset); offset += sizeof(uint8_t); result = getParam(arcseckeys::TELEMETRY_5, param); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return result; } adduint8(param, buffer + offset); offset += sizeof(uint8_t); result = getParam(arcseckeys::TELEMETRY_6, param); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return result; } adduint8(param, buffer + offset); offset += sizeof(uint8_t); result = getParam(arcseckeys::TELEMETRY_7, param); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return result; } adduint8(param, buffer + offset); offset += sizeof(uint8_t); result = getParam(arcseckeys::TELEMETRY_8, param); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return result; } adduint8(param, buffer + offset); offset += sizeof(uint8_t); result = getParam(arcseckeys::TELEMETRY_9, param); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return result; } adduint8(param, buffer + offset); offset += sizeof(uint8_t); result = getParam(arcseckeys::TELEMETRY_10, param); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return result; } adduint8(param, buffer + offset); offset += sizeof(uint8_t); result = getParam(arcseckeys::TELEMETRY_11, param); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return result; } adduint8(param, buffer + offset); offset += sizeof(uint8_t); result = getParam(arcseckeys::TELEMETRY_12, param); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return result; } adduint8(param, buffer + offset); offset += sizeof(uint8_t); result = getParam(arcseckeys::TELEMETRY_13, param); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return result; } adduint8(param, buffer + offset); offset += sizeof(uint8_t); result = getParam(arcseckeys::TELEMETRY_14, param); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return result; } adduint8(param, buffer + offset); offset += sizeof(uint8_t); result = getParam(arcseckeys::TELEMETRY_15, param); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return result; } adduint8(param, buffer + offset); offset += sizeof(uint8_t); result = getParam(arcseckeys::TELEMETRY_16, param); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return result; } adduint8(param, buffer + offset); - return RETURN_OK; + return returnvalue::OK; } LogSubscription::LogSubscription() : ArcsecJsonParamBase(arcseckeys::LOG_SUBSCRIPTION) {} @@ -861,35 +861,35 @@ LogSubscription::LogSubscription() : ArcsecJsonParamBase(arcseckeys::LOG_SUBSCRI size_t LogSubscription::getSize() { return COMMAND_SIZE; } ReturnValue_t LogSubscription::createCommand(uint8_t* buffer) { - ReturnValue_t result = RETURN_OK; + ReturnValue_t result = returnvalue::OK; uint8_t offset = 0; std::string param; addSetParamHeader(buffer, startracker::ID::LOG_SUBSCRIPTION); offset = 2; result = getParam(arcseckeys::LEVEL1, param); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return result; } adduint8(param, buffer + offset); offset += sizeof(uint8_t); result = getParam(arcseckeys::MODULE1, param); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return result; } adduint8(param, buffer + offset); offset += sizeof(uint8_t); result = getParam(arcseckeys::LEVEL2, param); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return result; } adduint8(param, buffer + offset); offset += sizeof(uint8_t); result = getParam(arcseckeys::MODULE2, param); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return result; } adduint8(param, buffer + offset); - return RETURN_OK; + return returnvalue::OK; } DebugCamera::DebugCamera() : ArcsecJsonParamBase(arcseckeys::DEBUG_CAMERA) {} @@ -897,21 +897,21 @@ DebugCamera::DebugCamera() : ArcsecJsonParamBase(arcseckeys::DEBUG_CAMERA) {} size_t DebugCamera::getSize() { return COMMAND_SIZE; } ReturnValue_t DebugCamera::createCommand(uint8_t* buffer) { - ReturnValue_t result = RETURN_OK; + ReturnValue_t result = returnvalue::OK; uint8_t offset = 0; std::string param; addSetParamHeader(buffer, startracker::ID::DEBUG_CAMERA); offset = 2; result = getParam(arcseckeys::TIMING, param); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return result; } adduint32(param, buffer + offset); offset += sizeof(uint32_t); result = getParam(arcseckeys::TEST, param); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return result; } adduint32(param, buffer + offset); - return RETURN_OK; + return returnvalue::OK; } diff --git a/linux/devices/startracker/StrHelper.cpp b/linux/devices/startracker/StrHelper.cpp index b2f02f24..bee15d70 100644 --- a/linux/devices/startracker/StrHelper.cpp +++ b/linux/devices/startracker/StrHelper.cpp @@ -19,14 +19,14 @@ ReturnValue_t StrHelper::initialize() { sdcMan = SdCardManager::instance(); if (sdcMan == nullptr) { sif::warning << "StrHelper::initialize: Invalid SD Card Manager" << std::endl; - return RETURN_FAILED; + return returnvalue::FAILED; } #endif - return RETURN_OK; + return returnvalue::OK; } ReturnValue_t StrHelper::performOperation(uint8_t operationCode) { - ReturnValue_t result = RETURN_OK; + ReturnValue_t result = returnvalue::OK; semaphore.acquire(); while (true) { switch (internalState) { @@ -36,7 +36,7 @@ ReturnValue_t StrHelper::performOperation(uint8_t operationCode) { } case InternalState::UPLOAD_IMAGE: { result = performImageUpload(); - if (result == RETURN_OK) { + if (result == returnvalue::OK) { triggerEvent(IMAGE_UPLOAD_SUCCESSFUL); } else { triggerEvent(IMAGE_UPLOAD_FAILED); @@ -46,7 +46,7 @@ ReturnValue_t StrHelper::performOperation(uint8_t operationCode) { } case InternalState::DOWNLOAD_IMAGE: { result = performImageDownload(); - if (result == RETURN_OK) { + if (result == returnvalue::OK) { triggerEvent(IMAGE_DOWNLOAD_SUCCESSFUL); } else { triggerEvent(IMAGE_DOWNLOAD_FAILED); @@ -56,7 +56,7 @@ ReturnValue_t StrHelper::performOperation(uint8_t operationCode) { } case InternalState::FLASH_READ: { result = performFlashRead(); - if (result == RETURN_OK) { + if (result == returnvalue::OK) { triggerEvent(FLASH_READ_SUCCESSFUL); } else { triggerEvent(FLASH_READ_FAILED); @@ -66,7 +66,7 @@ ReturnValue_t StrHelper::performOperation(uint8_t operationCode) { } case InternalState::FIRMWARE_UPDATE: { result = performFirmwareUpdate(); - if (result == RETURN_OK) { + if (result == returnvalue::OK) { triggerEvent(FIRMWARE_UPDATE_SUCCESSFUL); } else { triggerEvent(FIRMWARE_UPDATE_FAILED); @@ -85,9 +85,9 @@ ReturnValue_t StrHelper::setComIF(DeviceCommunicationIF* communicationInterface_ uartComIF = dynamic_cast(communicationInterface_); if (uartComIF == nullptr) { sif::warning << "StrHelper::initialize: Invalid uart com if" << std::endl; - return RETURN_FAILED; + return returnvalue::FAILED; } - return RETURN_OK; + return returnvalue::OK; } void StrHelper::setComCookie(CookieIF* comCookie_) { comCookie = comCookie_; } @@ -95,7 +95,7 @@ void StrHelper::setComCookie(CookieIF* comCookie_) { comCookie = comCookie_; } ReturnValue_t StrHelper::startImageUpload(std::string fullname) { #ifdef XIPHOS_Q7S ReturnValue_t result = checkPath(fullname); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return result; } #endif @@ -106,13 +106,13 @@ ReturnValue_t StrHelper::startImageUpload(std::string fullname) { internalState = InternalState::UPLOAD_IMAGE; semaphore.release(); terminate = false; - return RETURN_OK; + return returnvalue::OK; } ReturnValue_t StrHelper::startImageDownload(std::string path) { #ifdef XIPHOS_Q7S ReturnValue_t result = checkPath(path); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return result; } #endif @@ -123,7 +123,7 @@ ReturnValue_t StrHelper::startImageDownload(std::string path) { internalState = InternalState::DOWNLOAD_IMAGE; terminate = false; semaphore.release(); - return RETURN_OK; + return returnvalue::OK; } void StrHelper::stopProcess() { terminate = true; } @@ -135,7 +135,7 @@ void StrHelper::setFlashReadFilename(std::string filename) { flashRead.filename ReturnValue_t StrHelper::startFirmwareUpdate(std::string fullname) { #ifdef XIPHOS_Q7S ReturnValue_t result = checkPath(fullname); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return result; } #endif @@ -148,13 +148,13 @@ ReturnValue_t StrHelper::startFirmwareUpdate(std::string fullname) { internalState = InternalState::FIRMWARE_UPDATE; semaphore.release(); terminate = false; - return RETURN_OK; + return returnvalue::OK; } ReturnValue_t StrHelper::startFlashRead(std::string path, uint8_t startRegion, uint32_t length) { #ifdef XIPHOS_Q7S ReturnValue_t result = checkPath(path); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return result; } #endif @@ -167,7 +167,7 @@ ReturnValue_t StrHelper::startFlashRead(std::string path, uint8_t startRegion, u internalState = InternalState::FLASH_READ; semaphore.release(); terminate = false; - return RETURN_OK; + return returnvalue::OK; } void StrHelper::disableTimestamping() { timestamping = false; } @@ -192,11 +192,11 @@ ReturnValue_t StrHelper::performImageDownload() { while (downloadReq.position < ImageDownload::LAST_POSITION) { if (terminate) { file.close(); - return RETURN_OK; + return returnvalue::OK; } arc_pack_download_action_req(&downloadReq, commandBuffer, &size); result = sendAndRead(size, downloadReq.position); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { if (retries < CONFIG_MAX_DOWNLOAD_RETRIES) { uartComIF->flushUartRxBuffer(comCookie); retries++; @@ -206,7 +206,7 @@ ReturnValue_t StrHelper::performImageDownload() { return result; } result = checkActionReply(); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { if (retries < CONFIG_MAX_DOWNLOAD_RETRIES) { uartComIF->flushUartRxBuffer(comCookie); retries++; @@ -216,7 +216,7 @@ ReturnValue_t StrHelper::performImageDownload() { return result; } result = checkReplyPosition(downloadReq.position); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { if (retries < CONFIG_MAX_DOWNLOAD_RETRIES) { uartComIF->flushUartRxBuffer(comCookie); retries++; @@ -234,11 +234,11 @@ ReturnValue_t StrHelper::performImageDownload() { retries = 0; } file.close(); - return RETURN_OK; + return returnvalue::OK; } ReturnValue_t StrHelper::performImageUpload() { - ReturnValue_t result = RETURN_OK; + ReturnValue_t result = returnvalue::OK; uint32_t size = 0; uint32_t imageSize = 0; struct UploadActionRequest uploadReq; @@ -247,7 +247,7 @@ ReturnValue_t StrHelper::performImageUpload() { if (not std::filesystem::exists(uploadImage.uploadFile)) { triggerEvent(STR_HELPER_FILE_NOT_EXISTS, static_cast(internalState)); internalState = InternalState::IDLE; - return RETURN_FAILED; + return returnvalue::FAILED; } std::ifstream file(uploadImage.uploadFile, std::ifstream::binary); // Set position of next character to end of file input stream @@ -260,18 +260,18 @@ ReturnValue_t StrHelper::performImageUpload() { while ((uploadReq.position + 1) * SIZE_IMAGE_PART < imageSize) { if (terminate) { file.close(); - return RETURN_OK; + return returnvalue::OK; } file.seekg(uploadReq.position * SIZE_IMAGE_PART, file.beg); file.read(reinterpret_cast(uploadReq.data), SIZE_IMAGE_PART); arc_pack_upload_action_req(&uploadReq, commandBuffer, &size); result = sendAndRead(size, uploadReq.position); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { file.close(); - return RETURN_FAILED; + return returnvalue::FAILED; } result = checkActionReply(); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { file.close(); return result; } @@ -288,25 +288,25 @@ ReturnValue_t StrHelper::performImageUpload() { uploadReq.position++; arc_pack_upload_action_req(&uploadReq, commandBuffer, &size); result = sendAndRead(size, uploadReq.position); - if (result != RETURN_OK) { - return RETURN_FAILED; + if (result != returnvalue::OK) { + return returnvalue::FAILED; } result = checkActionReply(); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return result; } #if OBSW_DEBUG_STARTRACKER == 1 progressPrinter.print((uploadReq.position + 1) * SIZE_IMAGE_PART); #endif /* OBSW_DEBUG_STARTRACKER == 1 */ - return RETURN_OK; + return returnvalue::OK; } ReturnValue_t StrHelper::performFirmwareUpdate() { using namespace startracker; - ReturnValue_t result = RETURN_OK; + ReturnValue_t result = returnvalue::OK; result = unlockAndEraseRegions(static_cast(startracker::FirmwareRegions::FIRST), static_cast(startracker::FirmwareRegions::LAST)); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return result; } result = performFlashWrite(); @@ -314,7 +314,7 @@ ReturnValue_t StrHelper::performFirmwareUpdate() { } ReturnValue_t StrHelper::performFlashWrite() { - ReturnValue_t result = RETURN_OK; + ReturnValue_t result = returnvalue::OK; uint32_t size = 0; uint32_t bytesWritten = 0; uint32_t fileSize = 0; @@ -322,14 +322,14 @@ ReturnValue_t StrHelper::performFlashWrite() { if (not std::filesystem::exists(flashWrite.fullname)) { triggerEvent(STR_HELPER_FILE_NOT_EXISTS, static_cast(internalState)); internalState = InternalState::IDLE; - return RETURN_FAILED; + return returnvalue::FAILED; } std::ifstream file(flashWrite.fullname, std::ifstream::binary); file.seekg(0, file.end); fileSize = file.tellg(); if (fileSize > FLASH_REGION_SIZE * (flashWrite.lastRegion - flashWrite.firstRegion)) { sif::warning << "StrHelper::performFlashWrite: Invalid file" << std::endl; - return RETURN_FAILED; + return returnvalue::FAILED; } #if OBSW_DEBUG_STARTRACKER == 1 ProgressPrinter progressPrinter("Flash write", fileSize); @@ -341,7 +341,7 @@ ReturnValue_t StrHelper::performFlashWrite() { for (uint32_t idx = 0; idx < fileChunks; idx++) { if (terminate) { file.close(); - return RETURN_OK; + return returnvalue::OK; } file.seekg(idx * CHUNK_SIZE, file.beg); file.read(reinterpret_cast(req.data), CHUNK_SIZE); @@ -352,12 +352,12 @@ ReturnValue_t StrHelper::performFlashWrite() { req.address = bytesWritten; arc_pack_write_action_req(&req, commandBuffer, &size); result = sendAndRead(size, req.address); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { file.close(); return result; } result = checkActionReply(); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { file.close(); return result; } @@ -379,17 +379,17 @@ ReturnValue_t StrHelper::performFlashWrite() { bytesWritten += remainingBytes; arc_pack_write_action_req(&req, commandBuffer, &size); result = sendAndRead(size, req.address); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return result; } result = checkActionReply(); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return result; } #if OBSW_DEBUG_STARTRACKER == 1 progressPrinter.print(fileSize); #endif /* OBSW_DEBUG_STARTRACKER == 1 */ - return RETURN_OK; + return returnvalue::OK; } ReturnValue_t StrHelper::performFlashRead() { @@ -412,7 +412,7 @@ ReturnValue_t StrHelper::performFlashRead() { req.address = 0; while (bytesRead < flashRead.size) { if (terminate) { - return RETURN_OK; + return returnvalue::OK; } if ((flashRead.size - bytesRead) < CHUNK_SIZE) { req.length = flashRead.size - bytesRead; @@ -421,7 +421,7 @@ ReturnValue_t StrHelper::performFlashRead() { } arc_pack_read_action_req(&req, commandBuffer, &size); result = sendAndRead(size, req.address); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { if (retries < CONFIG_MAX_DOWNLOAD_RETRIES) { uartComIF->flushUartRxBuffer(comCookie); retries++; @@ -431,7 +431,7 @@ ReturnValue_t StrHelper::performFlashRead() { return result; } result = checkActionReply(); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { if (retries < CONFIG_MAX_DOWNLOAD_RETRIES) { uartComIF->flushUartRxBuffer(comCookie); retries++; @@ -454,12 +454,12 @@ ReturnValue_t StrHelper::performFlashRead() { #endif /* OBSW_DEBUG_STARTRACKER == 1 */ } file.close(); - return RETURN_OK; + return returnvalue::OK; } ReturnValue_t StrHelper::sendAndRead(size_t size, uint32_t parameter, uint32_t delayMs) { - ReturnValue_t result = RETURN_OK; - ReturnValue_t decResult = RETURN_OK; + ReturnValue_t result = returnvalue::OK; + ReturnValue_t decResult = returnvalue::OK; size_t receivedDataLen = 0; uint8_t* receivedData = nullptr; size_t bytesLeft = 0; @@ -467,10 +467,10 @@ ReturnValue_t StrHelper::sendAndRead(size_t size, uint32_t parameter, uint32_t d datalinkLayer.encodeFrame(commandBuffer, size); result = uartComIF->sendMessage(comCookie, datalinkLayer.getEncodedFrame(), datalinkLayer.getEncodedLength()); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { sif::warning << "StrHelper::sendAndRead: Failed to send packet" << std::endl; triggerEvent(STR_HELPER_SENDING_PACKET_FAILED, result, parameter); - return RETURN_FAILED; + return returnvalue::FAILED; } decResult = ArcsecDatalinkLayer::DEC_IN_PROGRESS; while (decResult == ArcsecDatalinkLayer::DEC_IN_PROGRESS) { @@ -479,23 +479,23 @@ ReturnValue_t StrHelper::sendAndRead(size_t size, uint32_t parameter, uint32_t d while (delay.isBusy()) { } result = uartComIF->requestReceiveMessage(comCookie, startracker::MAX_FRAME_SIZE * 2 + 2); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { sif::warning << "StrHelper::sendAndRead: Failed to request reply" << std::endl; triggerEvent(STR_HELPER_REQUESTING_MSG_FAILED, result, parameter); - return RETURN_FAILED; + return returnvalue::FAILED; } result = uartComIF->readReceivedMessage(comCookie, &receivedData, &receivedDataLen); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { sif::warning << "StrHelper::sendAndRead: Failed to read received message" << std::endl; triggerEvent(STR_HELPER_READING_REPLY_FAILED, result, parameter); - return RETURN_FAILED; + return returnvalue::FAILED; } if (receivedDataLen == 0 && missedReplies < MAX_POLLS) { missedReplies++; continue; } else if ((receivedDataLen == 0) && (missedReplies >= MAX_POLLS)) { triggerEvent(STR_HELPER_NO_REPLY, parameter); - return RETURN_FAILED; + return returnvalue::FAILED; } else { missedReplies = 0; } @@ -504,14 +504,14 @@ ReturnValue_t StrHelper::sendAndRead(size_t size, uint32_t parameter, uint32_t d // This should never happen sif::warning << "StrHelper::sendAndRead: Bytes left after decoding" << std::endl; triggerEvent(STR_HELPER_COM_ERROR, result, parameter); - return RETURN_FAILED; + return returnvalue::FAILED; } } - if (decResult != RETURN_OK) { + if (decResult != returnvalue::OK) { triggerEvent(STR_HELPER_DEC_ERROR, decResult, parameter); - return RETURN_FAILED; + return returnvalue::FAILED; } - return RETURN_OK; + return returnvalue::OK; } ReturnValue_t StrHelper::checkActionReply() { @@ -526,7 +526,7 @@ ReturnValue_t StrHelper::checkActionReply() { << static_cast(status) << std::endl; return STATUS_ERROR; } - return RETURN_OK; + return returnvalue::OK; } ReturnValue_t StrHelper::checkReplyPosition(uint32_t expectedPosition) { @@ -534,9 +534,9 @@ ReturnValue_t StrHelper::checkReplyPosition(uint32_t expectedPosition) { std::memcpy(&receivedPosition, datalinkLayer.getReply() + POS_OFFSET, sizeof(receivedPosition)); if (receivedPosition != expectedPosition) { triggerEvent(POSITION_MISMATCH, receivedPosition); - return RETURN_FAILED; + return returnvalue::FAILED; } - return RETURN_OK; + return returnvalue::OK; } #ifdef XIPHOS_Q7S @@ -554,12 +554,12 @@ ReturnValue_t StrHelper::checkPath(std::string name) { return SD_NOT_MOUNTED; } } - return RETURN_OK; + return returnvalue::OK; } #endif ReturnValue_t StrHelper::unlockAndEraseRegions(uint32_t from, uint32_t to) { - ReturnValue_t result = RETURN_OK; + ReturnValue_t result = returnvalue::OK; #if OBSW_DEBUG_STARTRACKER == 1 ProgressPrinter progressPrinter("Unlock and erase", to - from); #endif /* OBSW_DEBUG_STARTRACKER == 1 */ @@ -572,7 +572,7 @@ ReturnValue_t StrHelper::unlockAndEraseRegions(uint32_t from, uint32_t to) { arc_pack_unlock_action_req(&unlockReq, commandBuffer, &size); sendAndRead(size, unlockReq.region); result = checkActionReply(); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { sif::warning << "StrHelper::unlockAndEraseRegions: Failed to unlock region with id " << static_cast(unlockReq.region) << std::endl; return result; @@ -580,7 +580,7 @@ ReturnValue_t StrHelper::unlockAndEraseRegions(uint32_t from, uint32_t to) { eraseReq.region = idx; arc_pack_erase_action_req(&eraseReq, commandBuffer, &size); result = sendAndRead(size, eraseReq.region, FLASH_ERASE_DELAY); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { sif::warning << "StrHelper::unlockAndEraseRegions: Failed to erase region with id " << static_cast(eraseReq.region) << std::endl; return result; diff --git a/linux/devices/startracker/StrHelper.h b/linux/devices/startracker/StrHelper.h index 63f85233..0e6a912e 100644 --- a/linux/devices/startracker/StrHelper.h +++ b/linux/devices/startracker/StrHelper.h @@ -13,7 +13,7 @@ #include "fsfw/devicehandlers/CookieIF.h" #include "fsfw/objectmanager/SystemObject.h" #include "fsfw/osal/linux/BinarySemaphore.h" -#include "fsfw/returnvalues/HasReturnvaluesIF.h" +#include "fsfw/returnvalues/returnvalue.h" #include "fsfw/tasks/ExecutableObjectIF.h" #include "fsfw_hal/linux/uart/UartComIF.h" @@ -27,7 +27,7 @@ extern "C" { * * @author J. Meier */ -class StrHelper : public SystemObject, public ExecutableObjectIF, public HasReturnvaluesIF { +class StrHelper : public SystemObject, public ExecutableObjectIF { public: static const uint8_t SUBSYSTEM_ID = SUBSYSTEM_ID::STR_HELPER; @@ -270,7 +270,7 @@ class StrHelper : public SystemObject, public ExecutableObjectIF, public HasRetu /** * @brief Performs firmware update * - * @return RETURN_OK if successful, otherwise error return value + * @return returnvalue::OK if successful, otherwise error return value */ ReturnValue_t performFirmwareUpdate(); @@ -289,7 +289,7 @@ class StrHelper : public SystemObject, public ExecutableObjectIF, public HasRetu * * @param ID of first region to write to * - * @return RETURN_OK if successful, otherwise RETURN_FAILED + * @return returnvalue::OK if successful, otherwise returnvalue::FAILED */ ReturnValue_t performFlashWrite(); @@ -307,14 +307,14 @@ class StrHelper : public SystemObject, public ExecutableObjectIF, public HasRetu * @param parameter Parameter 2 of trigger event function * @param delayMs Delay in milliseconds between send and receive call * - * @return RETURN_OK if successful, otherwise RETURN_FAILED + * @return returnvalue::OK if successful, otherwise returnvalue::FAILED */ ReturnValue_t sendAndRead(size_t size, uint32_t parameter, uint32_t delayMs = 0); /** * @brief Checks the header (type id and status fields) of the action reply * - * @return RETURN_OK if reply confirms success of packet transfer, otherwise REUTRN_FAILED + * @return returnvalue::OK if reply confirms success of packet transfer, otherwise REUTRN_FAILED */ ReturnValue_t checkActionReply(); @@ -323,7 +323,7 @@ class StrHelper : public SystemObject, public ExecutableObjectIF, public HasRetu * * @param expectedPosition Value of expected position * - * @return RETURN_OK if received position matches expected position, otherwise RETURN_FAILED + * @return returnvalue::OK if received position matches expected position, otherwise returnvalue::FAILED */ ReturnValue_t checkReplyPosition(uint32_t expectedPosition); @@ -331,7 +331,7 @@ class StrHelper : public SystemObject, public ExecutableObjectIF, public HasRetu /** * @brief Checks if a path points to an sd card and whether the SD card is monuted. * - * @return SD_NOT_MOUNTED id SD card is not mounted, otherwise RETURN_OK + * @return SD_NOT_MOUNTED id SD card is not mounted, otherwise returnvalue::OK */ ReturnValue_t checkPath(std::string name); #endif diff --git a/linux/fsfwconfig/pollingsequence/pollingSequenceFactory.cpp b/linux/fsfwconfig/pollingsequence/pollingSequenceFactory.cpp index 87bd9c51..2535a10e 100644 --- a/linux/fsfwconfig/pollingsequence/pollingSequenceFactory.cpp +++ b/linux/fsfwconfig/pollingsequence/pollingSequenceFactory.cpp @@ -23,12 +23,12 @@ ReturnValue_t pst::pstGpio(FixedTimeslotTaskIF *thisSequence) { thisSequence->addSlot(objects::SOLAR_ARRAY_DEPL_HANDLER, length * 0, DeviceHandlerIF::PERFORM_OPERATION); - if (thisSequence->checkSequence() == HasReturnvaluesIF::RETURN_OK) { - return HasReturnvaluesIF::RETURN_OK; + if (thisSequence->checkSequence() == returnvalue::OK) { + return returnvalue::OK; } sif::error << "PollingSequence::initialize has errors!" << std::endl; - return HasReturnvaluesIF::RETURN_FAILED; + return returnvalue::FAILED; } ReturnValue_t pst::pstSpiRw(FixedTimeslotTaskIF *thisSequence) { @@ -510,13 +510,13 @@ ReturnValue_t pst::pstUart(FixedTimeslotTaskIF *thisSequence) { #endif static_cast(length); if (uartPstEmpty) { - return HasReturnvaluesIF::RETURN_OK; + return returnvalue::OK; } - if (thisSequence->checkSequence() != HasReturnvaluesIF::RETURN_OK) { + if (thisSequence->checkSequence() != returnvalue::OK) { sif::error << "UART PST initialization failed" << std::endl; - return HasReturnvaluesIF::RETURN_FAILED; + return returnvalue::FAILED; } - return HasReturnvaluesIF::RETURN_OK; + return returnvalue::OK; } ReturnValue_t pst::pstGompaceCan(FixedTimeslotTaskIF *thisSequence) { @@ -548,12 +548,12 @@ ReturnValue_t pst::pstGompaceCan(FixedTimeslotTaskIF *thisSequence) { thisSequence->addSlot(objects::PDU1_HANDLER, length * 0.8, DeviceHandlerIF::GET_READ); thisSequence->addSlot(objects::PDU2_HANDLER, length * 0.8, DeviceHandlerIF::GET_READ); thisSequence->addSlot(objects::ACU_HANDLER, length * 0.8, DeviceHandlerIF::GET_READ); - if (thisSequence->checkSequence() != HasReturnvaluesIF::RETURN_OK) { + if (thisSequence->checkSequence() != returnvalue::OK) { sif::error << "GomSpace PST initialization failed" << std::endl; - return HasReturnvaluesIF::RETURN_FAILED; + return returnvalue::FAILED; } static_cast(length); - return HasReturnvaluesIF::RETURN_OK; + return returnvalue::OK; } ReturnValue_t pst::pstTest(FixedTimeslotTaskIF *thisSequence) { @@ -583,11 +583,11 @@ ReturnValue_t pst::pstTest(FixedTimeslotTaskIF *thisSequence) { #endif static_cast(length); if (not notEmpty) { - return HasReturnvaluesIF::RETURN_FAILED; + return returnvalue::FAILED; } - if (thisSequence->checkSequence() != HasReturnvaluesIF::RETURN_OK) { + if (thisSequence->checkSequence() != returnvalue::OK) { sif::error << "Test PST initialization failed" << std::endl; - return HasReturnvaluesIF::RETURN_FAILED; + return returnvalue::FAILED; } - return HasReturnvaluesIF::RETURN_OK; + return returnvalue::OK; } diff --git a/linux/fsfwconfig/pollingsequence/pollingSequenceFactory.h b/linux/fsfwconfig/pollingsequence/pollingSequenceFactory.h index 03b0afb5..960820b5 100644 --- a/linux/fsfwconfig/pollingsequence/pollingSequenceFactory.h +++ b/linux/fsfwconfig/pollingsequence/pollingSequenceFactory.h @@ -8,7 +8,7 @@ #include "q7sConfig.h" #endif -#include "fsfw/returnvalues/HasReturnvaluesIF.h" +#include "fsfw/returnvalues/returnvalue.h" class FixedTimeslotTaskIF; diff --git a/linux/obc/AxiPtmeConfig.cpp b/linux/obc/AxiPtmeConfig.cpp index 26830d05..7c1f90c6 100644 --- a/linux/obc/AxiPtmeConfig.cpp +++ b/linux/obc/AxiPtmeConfig.cpp @@ -14,106 +14,106 @@ AxiPtmeConfig::AxiPtmeConfig(object_id_t objectId, std::string axiUio, int mapNu AxiPtmeConfig::~AxiPtmeConfig() {} ReturnValue_t AxiPtmeConfig::initialize() { - ReturnValue_t result = HasReturnvaluesIF::RETURN_OK; + ReturnValue_t result = returnvalue::OK; UioMapper uioMapper(axiUio, mapNum); result = uioMapper.getMappedAdress(&baseAddress, UioMapper::Permissions::READ_WRITE); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { return result; } - return HasReturnvaluesIF::RETURN_OK; + return returnvalue::OK; } ReturnValue_t AxiPtmeConfig::writeCaduRateReg(uint8_t rateVal) { - ReturnValue_t result = HasReturnvaluesIF::RETURN_OK; + ReturnValue_t result = returnvalue::OK; result = mutex->lockMutex(timeoutType, mutexTimeout); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { sif::warning << "AxiPtmeConfig::writeCaduRateReg: Failed to lock mutex" << std::endl; - return HasReturnvaluesIF::RETURN_FAILED; + return returnvalue::FAILED; } *(baseAddress + CADU_BITRATE_REG) = static_cast(rateVal); result = mutex->unlockMutex(); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { sif::warning << "AxiPtmeConfig::writeCaduRateReg: Failed to unlock mutex" << std::endl; - return HasReturnvaluesIF::RETURN_FAILED; + return returnvalue::FAILED; } - return HasReturnvaluesIF::RETURN_OK; + return returnvalue::OK; } ReturnValue_t AxiPtmeConfig::enableTxclockManipulator() { ReturnValue_t result = writeBit(COMMON_CONFIG_REG, true, BitPos::EN_TX_CLK_MANIPULATOR); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { return result; } - return HasReturnvaluesIF::RETURN_OK; + return returnvalue::OK; } ReturnValue_t AxiPtmeConfig::disableTxclockManipulator() { ReturnValue_t result = writeBit(COMMON_CONFIG_REG, false, BitPos::EN_TX_CLK_MANIPULATOR); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { return result; } - return HasReturnvaluesIF::RETURN_OK; + return returnvalue::OK; } ReturnValue_t AxiPtmeConfig::enableTxclockInversion() { ReturnValue_t result = writeBit(COMMON_CONFIG_REG, true, BitPos::INVERT_CLOCK); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { return result; } - return HasReturnvaluesIF::RETURN_OK; + return returnvalue::OK; } ReturnValue_t AxiPtmeConfig::disableTxclockInversion() { ReturnValue_t result = writeBit(COMMON_CONFIG_REG, false, BitPos::INVERT_CLOCK); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { return result; } - return HasReturnvaluesIF::RETURN_OK; + return returnvalue::OK; } ReturnValue_t AxiPtmeConfig::writeReg(uint32_t regOffset, uint32_t writeVal) { - ReturnValue_t result = HasReturnvaluesIF::RETURN_OK; + ReturnValue_t result = returnvalue::OK; result = mutex->lockMutex(timeoutType, mutexTimeout); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { sif::warning << "AxiPtmeConfig::readReg: Failed to lock mutex" << std::endl; - return HasReturnvaluesIF::RETURN_FAILED; + return returnvalue::FAILED; } *(baseAddress + regOffset / ADRESS_DIVIDER) = writeVal; result = mutex->unlockMutex(); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { sif::warning << "AxiPtmeConfig::readReg: Failed to unlock mutex" << std::endl; - return HasReturnvaluesIF::RETURN_FAILED; + return returnvalue::FAILED; } - return HasReturnvaluesIF::RETURN_OK; + return returnvalue::OK; } ReturnValue_t AxiPtmeConfig::readReg(uint32_t regOffset, uint32_t* readVal) { - ReturnValue_t result = HasReturnvaluesIF::RETURN_OK; + ReturnValue_t result = returnvalue::OK; result = mutex->lockMutex(timeoutType, mutexTimeout); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { sif::warning << "AxiPtmeConfig::readReg: Failed to lock mutex" << std::endl; - return HasReturnvaluesIF::RETURN_FAILED; + return returnvalue::FAILED; } *readVal = *(baseAddress + regOffset / ADRESS_DIVIDER); result = mutex->unlockMutex(); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { sif::warning << "AxiPtmeConfig::readReg: Failed to unlock mutex" << std::endl; - return HasReturnvaluesIF::RETURN_FAILED; + return returnvalue::FAILED; } - return HasReturnvaluesIF::RETURN_OK; + return returnvalue::OK; } ReturnValue_t AxiPtmeConfig::writeBit(uint32_t regOffset, bool bitVal, BitPos bitPos) { uint32_t readVal = 0; ReturnValue_t result = readReg(regOffset, &readVal); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { return result; } uint32_t writeVal = (readVal & ~(1 << static_cast(bitPos))) | bitVal << static_cast(bitPos); result = writeReg(regOffset, writeVal); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { return result; } - return HasReturnvaluesIF::RETURN_OK; + return returnvalue::OK; } diff --git a/linux/obc/AxiPtmeConfig.h b/linux/obc/AxiPtmeConfig.h index 9a048308..6996988c 100644 --- a/linux/obc/AxiPtmeConfig.h +++ b/linux/obc/AxiPtmeConfig.h @@ -5,7 +5,7 @@ #include "fsfw/ipc/MutexIF.h" #include "fsfw/objectmanager/SystemObject.h" -#include "fsfw/returnvalues/HasReturnvaluesIF.h" +#include "fsfw/returnvalues/returnvalue.h" /** * @brief Class providing low level access to the configuration interface of the PTME. @@ -94,7 +94,7 @@ class AxiPtmeConfig : public SystemObject { * @param bitVal The value of the bit to set (1 or 0) * @param bitPos The position of the bit within the register to set * - * @return RETURN_OK if successful, otherwise RETURN_FAILED + * @return returnvalue::OK if successful, otherwise returnvalue::FAILED */ ReturnValue_t writeBit(uint32_t regOffset, bool bitVal, BitPos bitPos); }; diff --git a/linux/obc/PapbVcInterface.cpp b/linux/obc/PapbVcInterface.cpp index 643104d7..fc23fed8 100644 --- a/linux/obc/PapbVcInterface.cpp +++ b/linux/obc/PapbVcInterface.cpp @@ -19,22 +19,22 @@ ReturnValue_t PapbVcInterface::initialize() { } ReturnValue_t PapbVcInterface::write(const uint8_t* data, size_t size) { - if (pollPapbBusySignal() == RETURN_OK) { + if (pollPapbBusySignal() == returnvalue::OK) { startPacketTransfer(); } for (size_t idx = 0; idx < size; idx++) { - if (pollPapbBusySignal() == RETURN_OK) { + if (pollPapbBusySignal() == returnvalue::OK) { *(vcBaseReg + DATA_REG_OFFSET) = static_cast(*(data + idx)); } else { sif::warning << "PapbVcInterface::write: Only written " << idx << " of " << size << " data" << std::endl; - return RETURN_FAILED; + return returnvalue::FAILED; } } - if (pollPapbBusySignal() == RETURN_OK) { + if (pollPapbBusySignal() == returnvalue::OK) { endPacketTransfer(); } - return RETURN_OK; + return returnvalue::OK; } void PapbVcInterface::startPacketTransfer() { *vcBaseReg = CONFIG_START; } @@ -43,30 +43,30 @@ void PapbVcInterface::endPacketTransfer() { *vcBaseReg = CONFIG_END; } ReturnValue_t PapbVcInterface::pollPapbBusySignal() { gpio::Levels papbBusyState = gpio::Levels::LOW; - ReturnValue_t result = RETURN_OK; + ReturnValue_t result = returnvalue::OK; /** Check if PAPB interface is ready to receive data */ result = gpioComIF->readGpio(papbBusyId, papbBusyState); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { sif::warning << "PapbVcInterface::pollPapbBusySignal: Failed to read papb busy signal" << std::endl; - return RETURN_FAILED; + return returnvalue::FAILED; } if (papbBusyState == gpio::Levels::LOW) { sif::warning << "PapbVcInterface::pollPapbBusySignal: PAPB busy" << std::endl; return PAPB_BUSY; } - return RETURN_OK; + return returnvalue::OK; } void PapbVcInterface::isVcInterfaceBufferEmpty() { - ReturnValue_t result = RETURN_OK; + ReturnValue_t result = returnvalue::OK; gpio::Levels papbEmptyState = gpio::Levels::HIGH; result = gpioComIF->readGpio(papbEmptyId, papbEmptyState); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { sif::warning << "PapbVcInterface::isVcInterfaceBufferEmpty: Failed to read papb empty signal" << std::endl; return; @@ -90,9 +90,9 @@ ReturnValue_t PapbVcInterface::sendTestFrame() { } ReturnValue_t result = write(testPacket, 1105); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return result; } - return RETURN_OK; + return returnvalue::OK; } diff --git a/linux/obc/PapbVcInterface.h b/linux/obc/PapbVcInterface.h index 0d6382e3..6162765e 100644 --- a/linux/obc/PapbVcInterface.h +++ b/linux/obc/PapbVcInterface.h @@ -5,7 +5,7 @@ #include #include "OBSWConfig.h" -#include "fsfw/returnvalues/HasReturnvaluesIF.h" +#include "fsfw/returnvalues/returnvalue.h" #include "linux/obc/VcInterfaceIF.h" /** @@ -14,7 +14,7 @@ * * @author J. Meier */ -class PapbVcInterface : public VcInterfaceIF, public HasReturnvaluesIF { +class PapbVcInterface : public VcInterfaceIF { public: /** * @brief Constructor @@ -93,7 +93,7 @@ class PapbVcInterface : public VcInterfaceIF, public HasReturnvaluesIF { * interface is ready to receive more data or not. PAPB is ready when * PAPB_Busy_N == '1'. * - * @return RETURN_OK when ready to receive data else PAPB_BUSY. + * @return returnvalue::OK when ready to receive data else PAPB_BUSY. */ ReturnValue_t pollPapbBusySignal(); diff --git a/linux/obc/PdecConfig.h b/linux/obc/PdecConfig.h index 37dab151..e037dd56 100644 --- a/linux/obc/PdecConfig.h +++ b/linux/obc/PdecConfig.h @@ -3,7 +3,7 @@ #include -#include "fsfw/returnvalues/HasReturnvaluesIF.h" +#include "fsfw/returnvalues/returnvalue.h" /** * @brief This class generates the configuration words for the configuration memory of the PDEC diff --git a/linux/obc/PdecHandler.cpp b/linux/obc/PdecHandler.cpp index 7843ee5c..fcfdd2f4 100644 --- a/linux/obc/PdecHandler.cpp +++ b/linux/obc/PdecHandler.cpp @@ -44,37 +44,37 @@ ReturnValue_t PdecHandler::initialize() { return ObjectManagerIF::CHILD_INIT_FAILED; } - ReturnValue_t result = RETURN_OK; + ReturnValue_t result = returnvalue::OK; UioMapper regMapper(uioRegisters); result = regMapper.getMappedAdress(®isterBaseAddress, UioMapper::Permissions::READ_WRITE); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return ObjectManagerIF::CHILD_INIT_FAILED; } UioMapper configMemMapper(uioConfigMemory); result = configMemMapper.getMappedAdress(&memoryBaseAddress, UioMapper::Permissions::READ_WRITE); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return ObjectManagerIF::CHILD_INIT_FAILED; } UioMapper ramMapper(uioRamMemory); result = ramMapper.getMappedAdress(&ramBaseAddress, UioMapper::Permissions::READ_WRITE); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return ObjectManagerIF::CHILD_INIT_FAILED; } writePdecConfig(); result = releasePdec(); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return ObjectManagerIF::CHILD_INIT_FAILED; } result = actionHelper.initialize(commandQueue); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return result; } - return RETURN_OK; + return returnvalue::OK; } MessageQueueId_t PdecHandler::getCommandQueue() const { return commandQueue->getId(); } @@ -109,33 +109,33 @@ ReturnValue_t PdecHandler::resetFarStatFlag() { sif::warning << "PdecHandler::resetFarStatFlag: FAR register did not match expected value." << " Read value: 0x" << std::hex << static_cast(pdecFar) << std::endl; - return RETURN_FAILED; + return returnvalue::FAILED; } #if OBSW_DEBUG_PDEC_HANDLER == 1 sif::debug << "PdecHandler::resetFarStatFlag: read FAR with value: 0x" << std::hex << pdecFar << std::endl; #endif /* OBSW_DEBUG_PDEC_HANDLER == 1 */ - return RETURN_OK; + return returnvalue::OK; } ReturnValue_t PdecHandler::releasePdec() { - ReturnValue_t result = RETURN_OK; + ReturnValue_t result = returnvalue::OK; result = gpioComIF->pullHigh(pdecReset); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { sif::error << "PdecHandler::releasePdec: Failed to release PDEC reset signal" << std::endl; } return result; } ReturnValue_t PdecHandler::performOperation(uint8_t operationCode) { - ReturnValue_t result = RETURN_OK; + ReturnValue_t result = returnvalue::OK; readCommandQueue(); switch (state) { case State::INIT: resetFarStatFlag(); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { // Requires reconfiguration and reinitialization of PDEC triggerEvent(INVALID_FAR); state = State::WAIT_FOR_RECOVERY; @@ -156,17 +156,17 @@ ReturnValue_t PdecHandler::performOperation(uint8_t operationCode) { break; } - return RETURN_OK; + return returnvalue::OK; } void PdecHandler::readCommandQueue(void) { CommandMessage commandMessage; - ReturnValue_t result = RETURN_FAILED; + ReturnValue_t result = returnvalue::FAILED; result = commandQueue->receiveMessage(&commandMessage); - if (result == RETURN_OK) { + if (result == returnvalue::OK) { result = actionHelper.handleActionMessage(&commandMessage); - if (result == RETURN_OK) { + if (result == returnvalue::OK) { return; } CommandMessage reply; @@ -317,11 +317,11 @@ void PdecHandler::handleIReason(uint32_t pdecFar, ReturnValue_t parameter1) { } void PdecHandler::handleNewTc() { - ReturnValue_t result = RETURN_OK; + ReturnValue_t result = returnvalue::OK; uint32_t tcLength = 0; result = readTc(tcLength); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return; } #if OBSW_DEBUG_PDEC_HANDLER == 1 @@ -333,7 +333,7 @@ void PdecHandler::handleNewTc() { #if OBSW_TC_FROM_PDEC == 1 store_address_t storeId; result = tcStore->addData(&storeId, tcSegment + 1, tcLength - 1); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { sif::warning << "PdecHandler::handleNewTc: Failed to add received space packet to store" << std::endl; return; @@ -342,7 +342,7 @@ void PdecHandler::handleNewTc() { TmTcMessage message(storeId); result = MessageQueueSenderIF::sendMessage(tcDestination->getRequestQueue(), &message); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { sif::warning << "PdecHandler::handleNewTc: Failed to send message to TC destination" << std::endl; tcStore->deleteData(storeId); @@ -369,7 +369,7 @@ ReturnValue_t PdecHandler::readTc(uint32_t& tcLength) { if (tcLength > MAX_TC_SEGMENT_SIZE) { sif::warning << "PdecHandler::handleNewTc: Read invalid TC length from PDEC register" << std::endl; - return RETURN_FAILED; + return returnvalue::FAILED; } uint32_t idx = 0; @@ -400,7 +400,7 @@ ReturnValue_t PdecHandler::readTc(uint32_t& tcLength) { // Backend buffer is handled back to PDEC3 *(registerBaseAddress + PDEC_BFREE_OFFSET) = 0; - return RETURN_OK; + return returnvalue::OK; } void PdecHandler::printTC(uint32_t tcLength) { diff --git a/linux/obc/PdecHandler.h b/linux/obc/PdecHandler.h index aeb571fc..2c6a200e 100644 --- a/linux/obc/PdecHandler.h +++ b/linux/obc/PdecHandler.h @@ -6,7 +6,7 @@ #include "fsfw/action/ActionHelper.h" #include "fsfw/action/HasActionsIF.h" #include "fsfw/objectmanager/SystemObject.h" -#include "fsfw/returnvalues/HasReturnvaluesIF.h" +#include "fsfw/returnvalues/returnvalue.h" #include "fsfw/storagemanager/StorageManagerIF.h" #include "fsfw/tasks/ExecutableObjectIF.h" #include "fsfw/tmtcservices/AcceptsTelecommandsIF.h" @@ -33,7 +33,6 @@ */ class PdecHandler : public SystemObject, public ExecutableObjectIF, - public HasReturnvaluesIF, public HasActionsIF { public: /** diff --git a/linux/obc/Ptme.cpp b/linux/obc/Ptme.cpp index 68ba3924..65705c2e 100644 --- a/linux/obc/Ptme.cpp +++ b/linux/obc/Ptme.cpp @@ -15,11 +15,11 @@ ReturnValue_t Ptme::initialize() { for (iter = vcInterfaceMap.begin(); iter != vcInterfaceMap.end(); iter++) { iter->second->initialize(); } - return RETURN_OK; + return returnvalue::OK; } ReturnValue_t Ptme::writeToVc(uint8_t vcId, const uint8_t* data, size_t size) { - ReturnValue_t result = RETURN_OK; + ReturnValue_t result = returnvalue::OK; VcInterfaceMapIter vcInterfaceMapIter = vcInterfaceMap.find(vcId); if (vcInterfaceMapIter == vcInterfaceMap.end()) { sif::warning << "Ptme::writeToVc: No virtual channel interface found for the virtual " diff --git a/linux/obc/Ptme.h b/linux/obc/Ptme.h index cdb2d6c6..f76f7fd1 100644 --- a/linux/obc/Ptme.h +++ b/linux/obc/Ptme.h @@ -8,7 +8,7 @@ #include #include "OBSWConfig.h" -#include "fsfw/returnvalues/HasReturnvaluesIF.h" +#include "fsfw/returnvalues/returnvalue.h" #include "linux/obc/PtmeIF.h" #include "linux/obc/VcInterfaceIF.h" @@ -18,7 +18,7 @@ * Synchro- nization and channel coding) and CCSDS 132.0-B-2 (TM Space Data Link Protocoll). The IP * cores are implemented on the programmable logic and are accessible through the linux UIO driver. */ -class Ptme : public PtmeIF, public SystemObject, public HasReturnvaluesIF { +class Ptme : public PtmeIF, public SystemObject { public: using VcId_t = uint8_t; diff --git a/linux/obc/PtmeConfig.cpp b/linux/obc/PtmeConfig.cpp index 9cbda7a6..589e5e5f 100644 --- a/linux/obc/PtmeConfig.cpp +++ b/linux/obc/PtmeConfig.cpp @@ -10,9 +10,9 @@ PtmeConfig::~PtmeConfig() {} ReturnValue_t PtmeConfig::initialize() { if (axiPtmeConfig == nullptr) { sif::warning << "PtmeConfig::initialize: Invalid AxiPtmeConfig object" << std::endl; - return RETURN_FAILED; + return returnvalue::FAILED; } - return RETURN_OK; + return returnvalue::OK; } ReturnValue_t PtmeConfig::setRate(uint32_t bitRate) { @@ -27,20 +27,20 @@ ReturnValue_t PtmeConfig::setRate(uint32_t bitRate) { } ReturnValue_t PtmeConfig::invertTxClock(bool invert) { - ReturnValue_t result = RETURN_OK; + ReturnValue_t result = returnvalue::OK; if (invert) { result = axiPtmeConfig->enableTxclockInversion(); } else { result = axiPtmeConfig->disableTxclockInversion(); } - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return CLK_INVERSION_FAILED; } return result; } ReturnValue_t PtmeConfig::configTxManipulator(bool enable) { - ReturnValue_t result = RETURN_OK; + ReturnValue_t result = returnvalue::OK; if (enable) { result = axiPtmeConfig->enableTxclockManipulator(); } else { diff --git a/linux/obc/PtmeConfig.h b/linux/obc/PtmeConfig.h index d6e35b57..06aff2e2 100644 --- a/linux/obc/PtmeConfig.h +++ b/linux/obc/PtmeConfig.h @@ -3,7 +3,7 @@ #include "AxiPtmeConfig.h" #include "fsfw/objectmanager/SystemObject.h" -#include "fsfw/returnvalues/HasReturnvaluesIF.h" +#include "fsfw/returnvalues/returnvalue.h" #include "linux/obc/PtmeConfig.h" /** @@ -11,7 +11,7 @@ * * @author J. Meier */ -class PtmeConfig : public SystemObject, public HasReturnvaluesIF { +class PtmeConfig : public SystemObject { public: /** * @brief Constructor diff --git a/linux/obc/PtmeIF.h b/linux/obc/PtmeIF.h index 7108df05..44aa39f2 100644 --- a/linux/obc/PtmeIF.h +++ b/linux/obc/PtmeIF.h @@ -1,7 +1,7 @@ #ifndef LINUX_OBC_PTMEIF_H_ #define LINUX_OBC_PTMEIF_H_ -#include "fsfw/returnvalues/HasReturnvaluesIF.h" +#include "fsfw/returnvalues/returnvalue.h" /** * @brief Interface class for managing the PTME IP Core implemented in the programmable logic. diff --git a/linux/obc/VcInterfaceIF.h b/linux/obc/VcInterfaceIF.h index 45226e21..de1bb61e 100644 --- a/linux/obc/VcInterfaceIF.h +++ b/linux/obc/VcInterfaceIF.h @@ -3,7 +3,7 @@ #include -#include "fsfw/returnvalues/HasReturnvaluesIF.h" +#include "fsfw/returnvalues/returnvalue.h" /** * @brief Interface class for managing different virtual channels of the PTME IP core implemented diff --git a/linux/utility/utility.h b/linux/utility/utility.h index b4c4fe8b..7f314f88 100644 --- a/linux/utility/utility.h +++ b/linux/utility/utility.h @@ -3,7 +3,7 @@ #include -#include "fsfw/returnvalues/HasReturnvaluesIF.h" +#include "fsfw/returnvalues/returnvalue.h" namespace utility { diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp index c97fd21b..c6b3207e 100644 --- a/mission/controller/AcsController.cpp +++ b/mission/controller/AcsController.cpp @@ -6,7 +6,7 @@ AcsController::AcsController(object_id_t objectId) : ExtendedControllerBase(objectId, objects::NO_OBJECT), mgmData(this) {} ReturnValue_t AcsController::handleCommandMessage(CommandMessage *message) { - return HasReturnvaluesIF::RETURN_OK; + return returnvalue::OK; } void AcsController::performControlOperation() { @@ -31,7 +31,7 @@ void AcsController::performControlOperation() { { PoolReadGuard pg(&mgmData); - if (pg.getReadResult() == RETURN_OK) { + if (pg.getReadResult() == returnvalue::OK) { copyMgmData(); } } @@ -46,7 +46,7 @@ ReturnValue_t AcsController::initializeLocalDataPool(localpool::DataPool &localD localDataPoolMap.emplace(acsctrl::PoolIds::MGM_IMTQ_CAL_NT, &imtqMgmPoolVec); localDataPoolMap.emplace(acsctrl::PoolIds::MGM_IMTQ_CAL_ACT_STATUS, &imtqCalActStatus); poolManager.subscribeForRegularPeriodicPacket({mgmData.getSid(), 5.0}); - return HasReturnvaluesIF::RETURN_OK; + return returnvalue::OK; } LocalPoolDataSetBase *AcsController::getDataSetHandle(sid_t sid) { @@ -58,37 +58,37 @@ LocalPoolDataSetBase *AcsController::getDataSetHandle(sid_t sid) { ReturnValue_t AcsController::checkModeCommand(Mode_t mode, Submode_t submode, uint32_t *msToReachTheMode) { - return HasReturnvaluesIF::RETURN_OK; + return returnvalue::OK; } void AcsController::copyMgmData() { { PoolReadGuard pg(&mgm0Lis3Set); - if (pg.getReadResult() == RETURN_OK) { + if (pg.getReadResult() == returnvalue::OK) { std::memcpy(mgmData.mgm0Lis3.value, mgm0Lis3Set.fieldStrengths.value, 3 * sizeof(float)); } } { PoolReadGuard pg(&mgm1Rm3100Set); - if (pg.getReadResult() == RETURN_OK) { + if (pg.getReadResult() == returnvalue::OK) { std::memcpy(mgmData.mgm1Rm3100.value, mgm1Rm3100Set.fieldStrengths.value, 3 * sizeof(float)); } } { PoolReadGuard pg(&mgm2Lis3Set); - if (pg.getReadResult() == RETURN_OK) { + if (pg.getReadResult() == returnvalue::OK) { std::memcpy(mgmData.mgm2Lis3.value, mgm2Lis3Set.fieldStrengths.value, 3 * sizeof(float)); } } { PoolReadGuard pg(&mgm3Rm3100Set); - if (pg.getReadResult() == RETURN_OK) { + if (pg.getReadResult() == returnvalue::OK) { std::memcpy(mgmData.mgm3Rm3100.value, mgm3Rm3100Set.fieldStrengths.value, 3 * sizeof(float)); } } { PoolReadGuard pg(&imtqMgmSet); - if (pg.getReadResult() == RETURN_OK) { + if (pg.getReadResult() == returnvalue::OK) { std::memcpy(mgmData.imtqRaw.value, imtqMgmSet.mtmRawNt.value, 3 * sizeof(float)); mgmData.actuationCalStatus.value = imtqMgmSet.coilActuationStatus.value; } diff --git a/mission/controller/ThermalController.cpp b/mission/controller/ThermalController.cpp index abb46f48..4800b969 100644 --- a/mission/controller/ThermalController.cpp +++ b/mission/controller/ThermalController.cpp @@ -59,7 +59,7 @@ ReturnValue_t ThermalController::initialize() { } ReturnValue_t ThermalController::handleCommandMessage(CommandMessage* message) { - return RETURN_FAILED; + return returnvalue::FAILED; } void ThermalController::performControlOperation() { @@ -83,19 +83,19 @@ void ThermalController::performControlOperation() { } ReturnValue_t result = sensorTemperatures.read(); - if (result == RETURN_OK) { + if (result == returnvalue::OK) { copySensors(); sensorTemperatures.commit(); } result = susTemperatures.read(); - if (result == RETURN_OK) { + if (result == returnvalue::OK) { copySus(); susTemperatures.commit(); } result = deviceTemperatures.read(); - if (result == RETURN_OK) { + if (result == returnvalue::OK) { copyDevices(); deviceTemperatures.commit(); } @@ -206,7 +206,7 @@ ReturnValue_t ThermalController::initializeLocalDataPool(localpool::DataPool& lo subdp::RegularHkPeriodicParams(susTemperatures.getSid(), false, 10.0)); poolManager.subscribeForRegularPeriodicPacket( subdp::RegularHkPeriodicParams(deviceTemperatures.getSid(), false, 10.0)); - return RETURN_OK; + return returnvalue::OK; } LocalPoolDataSetBase* ThermalController::getDataSetHandle(sid_t sid) { @@ -230,12 +230,12 @@ ReturnValue_t ThermalController::checkModeCommand(Mode_t mode, Submode_t submode if ((mode != MODE_OFF) && (mode != MODE_ON) && (mode != MODE_NORMAL)) { return INVALID_MODE; } - return RETURN_OK; + return returnvalue::OK; } void ThermalController::copySensors() { PoolReadGuard pg0(&max31865Set0); - if (pg0.getReadResult() == HasReturnvaluesIF::RETURN_OK) { + if (pg0.getReadResult() == returnvalue::OK) { sensorTemperatures.sensor_ploc_heatspreader.value = max31865Set0.temperatureCelcius.value; sensorTemperatures.sensor_ploc_heatspreader.setValid(max31865Set0.temperatureCelcius.isValid()); if (not sensorTemperatures.sensor_ploc_heatspreader.isValid()) { @@ -244,7 +244,7 @@ void ThermalController::copySensors() { } PoolReadGuard pg1(&max31865Set1); - if (pg1.getReadResult() == HasReturnvaluesIF::RETURN_OK) { + if (pg1.getReadResult() == returnvalue::OK) { sensorTemperatures.sensor_ploc_missionboard.value = max31865Set1.temperatureCelcius.value; sensorTemperatures.sensor_ploc_missionboard.setValid(max31865Set1.temperatureCelcius.isValid()); if (not sensorTemperatures.sensor_ploc_missionboard.isValid()) { @@ -253,7 +253,7 @@ void ThermalController::copySensors() { } PoolReadGuard pg2(&max31865Set2); - if (pg2.getReadResult() == HasReturnvaluesIF::RETURN_OK) { + if (pg2.getReadResult() == returnvalue::OK) { sensorTemperatures.sensor_4k_camera.value = max31865Set2.temperatureCelcius.value; sensorTemperatures.sensor_4k_camera.setValid(max31865Set2.temperatureCelcius.isValid()); if (not sensorTemperatures.sensor_4k_camera.isValid()) { @@ -262,7 +262,7 @@ void ThermalController::copySensors() { } PoolReadGuard pg3(&max31865Set3); - if (pg3.getReadResult() == HasReturnvaluesIF::RETURN_OK) { + if (pg3.getReadResult() == returnvalue::OK) { sensorTemperatures.sensor_dac_heatspreader.value = max31865Set3.temperatureCelcius.value; sensorTemperatures.sensor_dac_heatspreader.setValid(max31865Set3.temperatureCelcius.isValid()); if (not sensorTemperatures.sensor_dac_heatspreader.isValid()) { @@ -271,7 +271,7 @@ void ThermalController::copySensors() { } PoolReadGuard pg4(&max31865Set4); - if (pg4.getReadResult() == HasReturnvaluesIF::RETURN_OK) { + if (pg4.getReadResult() == returnvalue::OK) { sensorTemperatures.sensor_startracker.value = max31865Set4.temperatureCelcius.value; sensorTemperatures.sensor_startracker.setValid(max31865Set4.temperatureCelcius.isValid()); if (not sensorTemperatures.sensor_startracker.isValid()) { @@ -280,7 +280,7 @@ void ThermalController::copySensors() { } PoolReadGuard pg5(&max31865Set5); - if (pg5.getReadResult() == HasReturnvaluesIF::RETURN_OK) { + if (pg5.getReadResult() == returnvalue::OK) { sensorTemperatures.sensor_rw1.value = max31865Set5.temperatureCelcius.value; sensorTemperatures.sensor_rw1.setValid(max31865Set5.temperatureCelcius.isValid()); if (not sensorTemperatures.sensor_rw1.isValid()) { @@ -289,7 +289,7 @@ void ThermalController::copySensors() { } PoolReadGuard pg6(&max31865Set6); - if (pg6.getReadResult() == HasReturnvaluesIF::RETURN_OK) { + if (pg6.getReadResult() == returnvalue::OK) { sensorTemperatures.sensor_dro.value = max31865Set6.temperatureCelcius.value; sensorTemperatures.sensor_dro.setValid(max31865Set6.temperatureCelcius.isValid()); if (not sensorTemperatures.sensor_dro.isValid()) { @@ -298,7 +298,7 @@ void ThermalController::copySensors() { } PoolReadGuard pg7(&max31865Set7); - if (pg7.getReadResult() == HasReturnvaluesIF::RETURN_OK) { + if (pg7.getReadResult() == returnvalue::OK) { sensorTemperatures.sensor_scex.value = max31865Set7.temperatureCelcius.value; sensorTemperatures.sensor_scex.setValid(max31865Set7.temperatureCelcius.isValid()); if (not sensorTemperatures.sensor_scex.isValid()) { @@ -307,7 +307,7 @@ void ThermalController::copySensors() { } PoolReadGuard pg8(&max31865Set8); - if (pg8.getReadResult() == HasReturnvaluesIF::RETURN_OK) { + if (pg8.getReadResult() == returnvalue::OK) { sensorTemperatures.sensor_x8.value = max31865Set8.temperatureCelcius.value; sensorTemperatures.sensor_x8.setValid(max31865Set8.temperatureCelcius.isValid()); if (not sensorTemperatures.sensor_x8.isValid()) { @@ -316,7 +316,7 @@ void ThermalController::copySensors() { } PoolReadGuard pg9(&max31865Set9); - if (pg9.getReadResult() == HasReturnvaluesIF::RETURN_OK) { + if (pg9.getReadResult() == returnvalue::OK) { sensorTemperatures.sensor_hpa.value = max31865Set9.temperatureCelcius.value; sensorTemperatures.sensor_hpa.setValid(max31865Set9.temperatureCelcius.isValid()); if (not sensorTemperatures.sensor_hpa.isValid()) { @@ -325,7 +325,7 @@ void ThermalController::copySensors() { } PoolReadGuard pg10(&max31865Set10); - if (pg10.getReadResult() == HasReturnvaluesIF::RETURN_OK) { + if (pg10.getReadResult() == returnvalue::OK) { sensorTemperatures.sensor_tx_modul.value = max31865Set10.temperatureCelcius.value; sensorTemperatures.sensor_tx_modul.setValid(max31865Set10.temperatureCelcius.isValid()); if (not sensorTemperatures.sensor_tx_modul.isValid()) { @@ -334,7 +334,7 @@ void ThermalController::copySensors() { } PoolReadGuard pg11(&max31865Set11); - if (pg11.getReadResult() == HasReturnvaluesIF::RETURN_OK) { + if (pg11.getReadResult() == returnvalue::OK) { sensorTemperatures.sensor_mpa.value = max31865Set11.temperatureCelcius.value; sensorTemperatures.sensor_mpa.setValid(max31865Set11.temperatureCelcius.isValid()); if (not sensorTemperatures.sensor_mpa.isValid()) { @@ -343,7 +343,7 @@ void ThermalController::copySensors() { } PoolReadGuard pg12(&max31865Set12); - if (pg12.getReadResult() == HasReturnvaluesIF::RETURN_OK) { + if (pg12.getReadResult() == returnvalue::OK) { sensorTemperatures.sensor_acu.value = max31865Set12.temperatureCelcius.value; sensorTemperatures.sensor_acu.setValid(max31865Set12.temperatureCelcius.isValid()); if (not sensorTemperatures.sensor_acu.isValid()) { @@ -352,7 +352,7 @@ void ThermalController::copySensors() { } PoolReadGuard pg13(&max31865Set13); - if (pg13.getReadResult() == HasReturnvaluesIF::RETURN_OK) { + if (pg13.getReadResult() == returnvalue::OK) { sensorTemperatures.sensor_plpcdu_heatspreader.value = max31865Set13.temperatureCelcius.value; sensorTemperatures.sensor_plpcdu_heatspreader.setValid( max31865Set13.temperatureCelcius.isValid()); @@ -362,7 +362,7 @@ void ThermalController::copySensors() { } PoolReadGuard pg14(&max31865Set14); - if (pg14.getReadResult() == HasReturnvaluesIF::RETURN_OK) { + if (pg14.getReadResult() == returnvalue::OK) { sensorTemperatures.sensor_tcs_board.value = max31865Set14.temperatureCelcius.value; sensorTemperatures.sensor_tcs_board.setValid(max31865Set14.temperatureCelcius.isValid()); if (not sensorTemperatures.sensor_tcs_board.isValid()) { @@ -371,7 +371,7 @@ void ThermalController::copySensors() { } PoolReadGuard pg15(&max31865Set15); - if (pg15.getReadResult() == HasReturnvaluesIF::RETURN_OK) { + if (pg15.getReadResult() == returnvalue::OK) { sensorTemperatures.sensor_magnettorquer.value = max31865Set15.temperatureCelcius.value; sensorTemperatures.sensor_magnettorquer.setValid(max31865Set15.temperatureCelcius.isValid()); if (not sensorTemperatures.sensor_magnettorquer.isValid()) { @@ -379,7 +379,7 @@ void ThermalController::copySensors() { } } PoolReadGuard pg111(&tmp1075Set1); - if (pg1.getReadResult() == HasReturnvaluesIF::RETURN_OK) { + if (pg1.getReadResult() == returnvalue::OK) { sensorTemperatures.sensor_tmp1075_1.value = tmp1075Set1.temperatureCelcius.value; sensorTemperatures.sensor_tmp1075_1.setValid(tmp1075Set1.temperatureCelcius.isValid()); if (not tmp1075Set1.temperatureCelcius.isValid()) { @@ -387,7 +387,7 @@ void ThermalController::copySensors() { } } PoolReadGuard pg112(&tmp1075Set2); - if (pg2.getReadResult() == HasReturnvaluesIF::RETURN_OK) { + if (pg2.getReadResult() == returnvalue::OK) { sensorTemperatures.sensor_tmp1075_2.value = tmp1075Set2.temperatureCelcius.value; sensorTemperatures.sensor_tmp1075_2.setValid(tmp1075Set2.temperatureCelcius.isValid()); if (not tmp1075Set2.temperatureCelcius.isValid()) { @@ -398,7 +398,7 @@ void ThermalController::copySensors() { void ThermalController::copySus() { PoolReadGuard pg0(&susSet0); - if (pg0.getReadResult() == HasReturnvaluesIF::RETURN_OK) { + if (pg0.getReadResult() == returnvalue::OK) { susTemperatures.sus_0_n_loc_xfyfzm_pt_xf.value = susSet0.temperatureCelcius.value; susTemperatures.sus_0_n_loc_xfyfzm_pt_xf.setValid(susSet0.temperatureCelcius.isValid()); if (not susTemperatures.sus_0_n_loc_xfyfzm_pt_xf.isValid()) { @@ -407,7 +407,7 @@ void ThermalController::copySus() { } PoolReadGuard pg1(&susSet1); - if (pg1.getReadResult() == HasReturnvaluesIF::RETURN_OK) { + if (pg1.getReadResult() == returnvalue::OK) { susTemperatures.sus_6_r_loc_xfybzm_pt_xf.value = susSet1.temperatureCelcius.value; susTemperatures.sus_6_r_loc_xfybzm_pt_xf.setValid(susSet1.temperatureCelcius.isValid()); if (not susTemperatures.sus_6_r_loc_xfybzm_pt_xf.isValid()) { @@ -416,7 +416,7 @@ void ThermalController::copySus() { } PoolReadGuard pg2(&susSet2); - if (pg2.getReadResult() == HasReturnvaluesIF::RETURN_OK) { + if (pg2.getReadResult() == returnvalue::OK) { susTemperatures.sus_1_n_loc_xbyfzm_pt_xb.value = susSet2.temperatureCelcius.value; susTemperatures.sus_1_n_loc_xbyfzm_pt_xb.setValid(susSet2.temperatureCelcius.isValid()); if (not susTemperatures.sus_1_n_loc_xbyfzm_pt_xb.isValid()) { @@ -425,7 +425,7 @@ void ThermalController::copySus() { } PoolReadGuard pg3(&susSet3); - if (pg3.getReadResult() == HasReturnvaluesIF::RETURN_OK) { + if (pg3.getReadResult() == returnvalue::OK) { susTemperatures.sus_7_r_loc_xbybzm_pt_xb.value = susSet3.temperatureCelcius.value; susTemperatures.sus_7_r_loc_xbybzm_pt_xb.setValid(susSet3.temperatureCelcius.isValid()); if (not susTemperatures.sus_7_r_loc_xbybzm_pt_xb.isValid()) { @@ -434,7 +434,7 @@ void ThermalController::copySus() { } PoolReadGuard pg4(&susSet4); - if (pg4.getReadResult() == HasReturnvaluesIF::RETURN_OK) { + if (pg4.getReadResult() == returnvalue::OK) { susTemperatures.sus_2_n_loc_xfybzb_pt_yb.value = susSet4.temperatureCelcius.value; susTemperatures.sus_2_n_loc_xfybzb_pt_yb.setValid(susSet4.temperatureCelcius.isValid()); if (not susTemperatures.sus_2_n_loc_xfybzb_pt_yb.isValid()) { @@ -443,7 +443,7 @@ void ThermalController::copySus() { } PoolReadGuard pg5(&susSet5); - if (pg5.getReadResult() == HasReturnvaluesIF::RETURN_OK) { + if (pg5.getReadResult() == returnvalue::OK) { susTemperatures.sus_8_r_loc_xbybzb_pt_yb.value = susSet5.temperatureCelcius.value; susTemperatures.sus_8_r_loc_xbybzb_pt_yb.setValid(susSet5.temperatureCelcius.isValid()); if (not susTemperatures.sus_8_r_loc_xbybzb_pt_yb.isValid()) { @@ -452,7 +452,7 @@ void ThermalController::copySus() { } PoolReadGuard pg6(&susSet6); - if (pg6.getReadResult() == HasReturnvaluesIF::RETURN_OK) { + if (pg6.getReadResult() == returnvalue::OK) { susTemperatures.sus_3_n_loc_xfybzf_pt_yf.value = susSet6.temperatureCelcius.value; susTemperatures.sus_3_n_loc_xfybzf_pt_yf.setValid(susSet6.temperatureCelcius.isValid()); if (not susTemperatures.sus_3_n_loc_xfybzf_pt_yf.isValid()) { @@ -461,7 +461,7 @@ void ThermalController::copySus() { } PoolReadGuard pg7(&susSet7); - if (pg7.getReadResult() == HasReturnvaluesIF::RETURN_OK) { + if (pg7.getReadResult() == returnvalue::OK) { susTemperatures.sus_9_r_loc_xbybzb_pt_yf.value = susSet7.temperatureCelcius.value; susTemperatures.sus_9_r_loc_xbybzb_pt_yf.setValid(susSet7.temperatureCelcius.isValid()); if (not susTemperatures.sus_9_r_loc_xbybzb_pt_yf.isValid()) { @@ -470,7 +470,7 @@ void ThermalController::copySus() { } PoolReadGuard pg8(&susSet8); - if (pg8.getReadResult() == HasReturnvaluesIF::RETURN_OK) { + if (pg8.getReadResult() == returnvalue::OK) { susTemperatures.sus_4_n_loc_xmyfzf_pt_zf.value = susSet8.temperatureCelcius.value; susTemperatures.sus_4_n_loc_xmyfzf_pt_zf.setValid(susSet8.temperatureCelcius.isValid()); if (not susTemperatures.sus_4_n_loc_xmyfzf_pt_zf.isValid()) { @@ -479,7 +479,7 @@ void ThermalController::copySus() { } PoolReadGuard pg9(&susSet9); - if (pg9.getReadResult() == HasReturnvaluesIF::RETURN_OK) { + if (pg9.getReadResult() == returnvalue::OK) { susTemperatures.sus_10_n_loc_xmybzf_pt_zf.value = susSet9.temperatureCelcius.value; susTemperatures.sus_10_n_loc_xmybzf_pt_zf.setValid(susSet9.temperatureCelcius.isValid()); if (not susTemperatures.sus_10_n_loc_xmybzf_pt_zf.isValid()) { @@ -488,7 +488,7 @@ void ThermalController::copySus() { } PoolReadGuard pg10(&susSet10); - if (pg10.getReadResult() == HasReturnvaluesIF::RETURN_OK) { + if (pg10.getReadResult() == returnvalue::OK) { susTemperatures.sus_5_n_loc_xfymzb_pt_zb.value = susSet10.temperatureCelcius.value; susTemperatures.sus_5_n_loc_xfymzb_pt_zb.setValid(susSet10.temperatureCelcius.isValid()); if (not susTemperatures.sus_5_n_loc_xfymzb_pt_zb.isValid()) { @@ -497,7 +497,7 @@ void ThermalController::copySus() { } PoolReadGuard pg11(&susSet11); - if (pg11.getReadResult() == HasReturnvaluesIF::RETURN_OK) { + if (pg11.getReadResult() == returnvalue::OK) { susTemperatures.sus_11_r_loc_xbymzb_pt_zb.value = susSet11.temperatureCelcius.value; susTemperatures.sus_11_r_loc_xbymzb_pt_zb.setValid(susSet11.temperatureCelcius.isValid()); if (not susTemperatures.sus_11_r_loc_xbymzb_pt_zb.isValid()) { @@ -509,7 +509,7 @@ void ThermalController::copySus() { void ThermalController::copyDevices() { lp_var_t tempQ7s = lp_var_t(objects::CORE_CONTROLLER, core::PoolIds::TEMPERATURE); ReturnValue_t result = tempQ7s.read(); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { sif::warning << "ThermalController: Failed to read Q7S temperature" << std::endl; deviceTemperatures.q7s.setValid(false); deviceTemperatures.q7s = static_cast(INVALID_TEMPERATURE); @@ -518,13 +518,13 @@ void ThermalController::copyDevices() { deviceTemperatures.q7s.setValid(tempQ7s.isValid()); } result = tempQ7s.commit(); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { sif::warning << "ThermalController: Failed to commit" << std::endl; } lp_var_t battTemp1 = lp_var_t(objects::BPX_BATT_HANDLER, BpxBattery::BATT_TEMP_1); result = battTemp1.read(); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { sif::warning << "ThermalController: Failed to read battery temperature 1" << std::endl; deviceTemperatures.batteryTemp1.setValid(false); deviceTemperatures.batteryTemp1 = static_cast(INVALID_TEMPERATURE); @@ -533,13 +533,13 @@ void ThermalController::copyDevices() { deviceTemperatures.batteryTemp1.setValid(battTemp1.isValid()); } result = battTemp1.commit(); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { sif::warning << "ThermalController: Failed to commit" << std::endl; } lp_var_t battTemp2 = lp_var_t(objects::BPX_BATT_HANDLER, BpxBattery::BATT_TEMP_2); result = battTemp2.read(); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { sif::warning << "ThermalController: Failed to read battery temperature 2" << std::endl; deviceTemperatures.batteryTemp2.setValid(false); deviceTemperatures.batteryTemp2 = static_cast(INVALID_TEMPERATURE); @@ -548,13 +548,13 @@ void ThermalController::copyDevices() { deviceTemperatures.batteryTemp2.setValid(battTemp2.isValid()); } result = battTemp2.commit(); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { sif::warning << "ThermalController: Failed to commit" << std::endl; } lp_var_t battTemp3 = lp_var_t(objects::BPX_BATT_HANDLER, BpxBattery::BATT_TEMP_3); result = battTemp3.read(); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { sif::warning << "ThermalController: Failed to read battery temperature 3" << std::endl; deviceTemperatures.batteryTemp3.setValid(false); deviceTemperatures.batteryTemp3 = static_cast(INVALID_TEMPERATURE); @@ -563,13 +563,13 @@ void ThermalController::copyDevices() { deviceTemperatures.batteryTemp3.setValid(battTemp3.isValid()); } result = battTemp3.commit(); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { sif::warning << "ThermalController: Failed to commit" << std::endl; } lp_var_t battTemp4 = lp_var_t(objects::BPX_BATT_HANDLER, BpxBattery::BATT_TEMP_4); result = battTemp4.read(); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { sif::warning << "ThermalController: Failed to read battery temperature 4" << std::endl; deviceTemperatures.batteryTemp4.setValid(false); deviceTemperatures.batteryTemp4 = static_cast(INVALID_TEMPERATURE); @@ -578,12 +578,12 @@ void ThermalController::copyDevices() { deviceTemperatures.batteryTemp4.setValid(battTemp4.isValid()); } result = battTemp4.commit(); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { sif::warning << "ThermalController: Failed to commit" << std::endl; } lp_var_t tempRw1 = lp_var_t(objects::RW1, RwDefinitions::TEMPERATURE_C); result = tempRw1.read(); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { sif::warning << "ThermalController: Failed to read reaction wheel 1 temperature" << std::endl; deviceTemperatures.rw1.setValid(false); deviceTemperatures.rw1 = static_cast(INVALID_TEMPERATURE); @@ -592,12 +592,12 @@ void ThermalController::copyDevices() { deviceTemperatures.rw1 = tempRw1; } result = tempRw1.commit(); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { sif::warning << "ThermalController: Failed to commit" << std::endl; } lp_var_t tempRw2 = lp_var_t(objects::RW2, RwDefinitions::TEMPERATURE_C); result = tempRw2.read(); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { sif::warning << "ThermalController: Failed to read reaction wheel 2 temperature" << std::endl; deviceTemperatures.rw2.setValid(false); deviceTemperatures.rw2 = static_cast(INVALID_TEMPERATURE); @@ -606,12 +606,12 @@ void ThermalController::copyDevices() { deviceTemperatures.rw2 = tempRw2; } result = tempRw2.commit(); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { sif::warning << "ThermalController: Failed to commit" << std::endl; } lp_var_t tempRw3 = lp_var_t(objects::RW3, RwDefinitions::TEMPERATURE_C); result = tempRw3.read(); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { sif::warning << "ThermalController: Failed to read reaction wheel 3 temperature" << std::endl; deviceTemperatures.rw3.setValid(false); deviceTemperatures.rw3 = static_cast(INVALID_TEMPERATURE); @@ -620,12 +620,12 @@ void ThermalController::copyDevices() { deviceTemperatures.rw3 = tempRw3; } result = tempRw3.commit(); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { sif::warning << "ThermalController: Failed to commit" << std::endl; } lp_var_t tempRw4 = lp_var_t(objects::RW4, RwDefinitions::TEMPERATURE_C); result = tempRw4.read(); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { sif::warning << "ThermalController: Failed to read reaction wheel 4 temperature" << std::endl; deviceTemperatures.rw4.setValid(false); deviceTemperatures.rw4 = static_cast(INVALID_TEMPERATURE); @@ -634,13 +634,13 @@ void ThermalController::copyDevices() { deviceTemperatures.rw4 = tempRw4; } result = tempRw4.commit(); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { sif::warning << "ThermalController: Failed to commit" << std::endl; } lp_var_t tempStartracker = lp_var_t(objects::STAR_TRACKER, startracker::MCU_TEMPERATURE); result = tempStartracker.read(); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { sif::warning << "ThermalController: Failed to read startracker temperature" << std::endl; deviceTemperatures.startracker.setValid(false); deviceTemperatures.startracker = static_cast(INVALID_TEMPERATURE); @@ -649,13 +649,13 @@ void ThermalController::copyDevices() { deviceTemperatures.startracker = tempStartracker; } result = tempStartracker.commit(); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { sif::warning << "ThermalController: Failed to commit" << std::endl; } lp_var_t tempSyrlinksPowerAmplifier = lp_var_t(objects::SYRLINKS_HK_HANDLER, syrlinks::TEMP_POWER_AMPLIFIER); result = tempSyrlinksPowerAmplifier.read(); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { sif::warning << "ThermalController: Failed to read syrlinks power amplifier temperature" << std::endl; deviceTemperatures.syrlinksPowerAmplifier.setValid(false); @@ -665,13 +665,13 @@ void ThermalController::copyDevices() { deviceTemperatures.syrlinksPowerAmplifier = tempSyrlinksPowerAmplifier; } result = tempSyrlinksPowerAmplifier.commit(); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { sif::warning << "ThermalController: Failed to commit" << std::endl; } lp_var_t tempSyrlinksBasebandBoard = lp_var_t(objects::SYRLINKS_HK_HANDLER, syrlinks::TEMP_BASEBAND_BOARD); result = tempSyrlinksBasebandBoard.read(); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { sif::warning << "ThermalController: Failed to read syrlinks baseband board temperature" << std::endl; deviceTemperatures.syrlinksBasebandBoard.setValid(false); @@ -681,12 +681,12 @@ void ThermalController::copyDevices() { deviceTemperatures.syrlinksBasebandBoard = tempSyrlinksBasebandBoard; } result = tempSyrlinksBasebandBoard.commit(); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { sif::warning << "ThermalController: Failed to commit" << std::endl; } lp_var_t tempMgt = lp_var_t(objects::IMTQ_HANDLER, IMTQ::MCU_TEMPERATURE); result = tempMgt.read(); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { sif::warning << "ThermalController: Failed to read MGT temperature" << std::endl; deviceTemperatures.mgt.setValid(false); deviceTemperatures.mgt = static_cast(INVALID_TEMPERATURE); @@ -695,13 +695,13 @@ void ThermalController::copyDevices() { deviceTemperatures.mgt = tempMgt; } result = tempMgt.commit(); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { sif::warning << "ThermalController: Failed to commit" << std::endl; } lp_vec_t tempAcu = lp_vec_t(objects::ACU_HANDLER, P60System::pool::ACU_TEMPERATURES); result = tempAcu.read(); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { sif::warning << "ThermalController: Failed to read ACU temperatures" << std::endl; deviceTemperatures.acu.setValid(false); deviceTemperatures.acu[0] = static_cast(INVALID_TEMPERATURE); @@ -712,13 +712,13 @@ void ThermalController::copyDevices() { deviceTemperatures.acu = tempAcu; } result = tempAcu.commit(); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { sif::warning << "ThermalController: Failed to commit" << std::endl; } lp_var_t tempPdu1 = lp_var_t(objects::PDU1_HANDLER, P60System::pool::PDU_TEMPERATURE); result = tempPdu1.read(); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { sif::warning << "ThermalController: Failed to read PDU1 temperature" << std::endl; deviceTemperatures.pdu1.setValid(false); deviceTemperatures.pdu1 = static_cast(INVALID_TEMPERATURE); @@ -727,13 +727,13 @@ void ThermalController::copyDevices() { deviceTemperatures.pdu1 = tempPdu1; } result = tempPdu1.commit(); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { sif::warning << "ThermalController: Failed to commit" << std::endl; } lp_var_t tempPdu2 = lp_var_t(objects::PDU2_HANDLER, P60System::pool::PDU_TEMPERATURE); result = tempPdu2.read(); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { sif::warning << "ThermalController: Failed to read PDU2 temperature" << std::endl; deviceTemperatures.pdu2.setValid(false); deviceTemperatures.pdu2 = static_cast(INVALID_TEMPERATURE); @@ -742,13 +742,13 @@ void ThermalController::copyDevices() { deviceTemperatures.pdu2 = tempPdu1; } result = tempPdu2.commit(); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { sif::warning << "ThermalController: Failed to commit" << std::endl; } lp_var_t temp1P60dock = lp_var_t(objects::P60DOCK_HANDLER, P60System::pool::P60DOCK_TEMPERATURE_1); result = temp1P60dock.read(); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { sif::warning << "ThermalController: Failed to read P60 dock temperature 1" << std::endl; deviceTemperatures.temp1P60dock.setValid(false); deviceTemperatures.temp1P60dock = static_cast(INVALID_TEMPERATURE); @@ -757,13 +757,13 @@ void ThermalController::copyDevices() { deviceTemperatures.temp1P60dock = temp1P60dock; } result = temp1P60dock.commit(); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { sif::warning << "ThermalController: Failed to commit" << std::endl; } lp_var_t temp2P60dock = lp_var_t(objects::P60DOCK_HANDLER, P60System::pool::P60DOCK_TEMPERATURE_2); result = temp2P60dock.read(); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { sif::warning << "ThermalController: Failed to read P60 dock temperature 2" << std::endl; deviceTemperatures.temp2P60dock.setValid(false); deviceTemperatures.temp2P60dock = static_cast(INVALID_TEMPERATURE); @@ -772,12 +772,12 @@ void ThermalController::copyDevices() { deviceTemperatures.temp2P60dock = temp2P60dock; } result = temp2P60dock.commit(); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { sif::warning << "ThermalController: Failed to commit" << std::endl; } lp_var_t tempGyro0 = lp_var_t(objects::GYRO_0_ADIS_HANDLER, ADIS1650X::TEMPERATURE); result = tempGyro0.read(); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { sif::warning << "ThermalController: Failed to read gyro 0 temperature" << std::endl; deviceTemperatures.gyro0SideA.setValid(false); deviceTemperatures.gyro0SideA = static_cast(INVALID_TEMPERATURE); @@ -786,12 +786,12 @@ void ThermalController::copyDevices() { deviceTemperatures.gyro0SideA = tempGyro0; } result = tempGyro0.commit(); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { sif::warning << "ThermalController: Failed to commit" << std::endl; } lp_var_t tempGyro1 = lp_var_t(objects::GYRO_1_L3G_HANDLER, L3GD20H::TEMPERATURE); result = tempGyro1.read(); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { sif::warning << "ThermalController: Failed to read gyro 1 temperature" << std::endl; deviceTemperatures.gyro1SideA.setValid(false); deviceTemperatures.gyro1SideA = static_cast(INVALID_TEMPERATURE); @@ -800,12 +800,12 @@ void ThermalController::copyDevices() { deviceTemperatures.gyro1SideA = tempGyro1; } result = tempGyro1.commit(); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { sif::warning << "ThermalController: Failed to commit" << std::endl; } lp_var_t tempGyro2 = lp_var_t(objects::GYRO_2_ADIS_HANDLER, ADIS1650X::TEMPERATURE); result = tempGyro2.read(); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { sif::warning << "ThermalController: Failed to read gyro 2 temperature" << std::endl; deviceTemperatures.gyro2SideB.setValid(false); deviceTemperatures.gyro2SideB = static_cast(INVALID_TEMPERATURE); @@ -814,12 +814,12 @@ void ThermalController::copyDevices() { deviceTemperatures.gyro2SideB = tempGyro2; } result = tempGyro2.commit(); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { sif::warning << "ThermalController: Failed to commit" << std::endl; } lp_var_t tempGyro3 = lp_var_t(objects::GYRO_3_L3G_HANDLER, L3GD20H::TEMPERATURE); result = tempGyro3.read(); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { sif::warning << "ThermalController: Failed to read gyro 3 temperature" << std::endl; deviceTemperatures.gyro3SideB.setValid(false); deviceTemperatures.gyro3SideB = static_cast(INVALID_TEMPERATURE); @@ -828,13 +828,13 @@ void ThermalController::copyDevices() { deviceTemperatures.gyro3SideB = tempGyro3; } result = tempGyro3.commit(); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { sif::warning << "ThermalController: Failed to commit" << std::endl; } lp_var_t tempMgm0 = lp_var_t(objects::MGM_0_LIS3_HANDLER, MGMLIS3MDL::TEMPERATURE_CELCIUS); result = tempMgm0.read(); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { sif::warning << "ThermalController: Failed to read MGM 0 temperature" << std::endl; deviceTemperatures.mgm0SideA.setValid(false); deviceTemperatures.mgm0SideA = static_cast(INVALID_TEMPERATURE); @@ -843,13 +843,13 @@ void ThermalController::copyDevices() { deviceTemperatures.mgm0SideA = tempMgm0; } result = tempMgm0.commit(); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { sif::warning << "ThermalController: Failed to commit" << std::endl; } lp_var_t tempMgm2 = lp_var_t(objects::MGM_2_LIS3_HANDLER, MGMLIS3MDL::TEMPERATURE_CELCIUS); result = tempMgm2.read(); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { sif::warning << "ThermalController: Failed to read MGM 2 temperature" << std::endl; deviceTemperatures.mgm2SideB.setValid(false); deviceTemperatures.mgm2SideB = static_cast(INVALID_TEMPERATURE); @@ -858,12 +858,12 @@ void ThermalController::copyDevices() { deviceTemperatures.mgm2SideB = tempMgm2; } result = tempMgm2.commit(); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { sif::warning << "ThermalController: Failed to commit" << std::endl; } lp_var_t tempAdcPayloadPcdu = lp_var_t(objects::PLPCDU_HANDLER, plpcdu::TEMP); result = tempAdcPayloadPcdu.read(); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { sif::warning << "ThermalController: Failed to read payload PCDU ADC temperature" << std::endl; deviceTemperatures.adcPayloadPcdu.setValid(false); deviceTemperatures.adcPayloadPcdu = static_cast(INVALID_TEMPERATURE); @@ -872,7 +872,7 @@ void ThermalController::copyDevices() { deviceTemperatures.adcPayloadPcdu = tempAdcPayloadPcdu; } result = tempAdcPayloadPcdu.commit(); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { sif::warning << "ThermalController: Failed to commit" << std::endl; } } diff --git a/mission/devices/ACUHandler.cpp b/mission/devices/ACUHandler.cpp index 35f8e2dd..4c711095 100644 --- a/mission/devices/ACUHandler.cpp +++ b/mission/devices/ACUHandler.cpp @@ -24,7 +24,7 @@ void ACUHandler::letChildHandleHkReply(DeviceCommandId_t id, const uint8_t *pack #if OBSW_VERBOSE_LEVEL >= 1 PoolReadGuard pg0(&auxHk); PoolReadGuard pg1(&coreHk); - if (pg0.getReadResult() != RETURN_OK or pg1.getReadResult() != RETURN_OK) { + if (pg0.getReadResult() != returnvalue::OK or pg1.getReadResult() != returnvalue::OK) { return; } for (size_t idx = 0; idx < 3; idx++) { @@ -53,10 +53,10 @@ ReturnValue_t ACUHandler::parseHkTableReply(const uint8_t *packet) { PoolReadGuard pg1(&auxHk); auto res0 = pg0.getReadResult(); auto res1 = pg1.getReadResult(); - if (res0 != RETURN_OK) { + if (res0 != returnvalue::OK) { return res0; } - if (res1 != RETURN_OK) { + if (res1 != returnvalue::OK) { return res1; } dataOffset += 12; @@ -135,7 +135,7 @@ ReturnValue_t ACUHandler::parseHkTableReply(const uint8_t *packet) { dataOffset += 6; coreHk.setValidity(true, true); auxHk.setValidity(true, true); - return RETURN_OK; + return returnvalue::OK; } ReturnValue_t ACUHandler::initializeLocalDataPool(localpool::DataPool &localDataPoolMap, @@ -174,7 +174,7 @@ ReturnValue_t ACUHandler::initializeLocalDataPool(localpool::DataPool &localData subdp::DiagnosticsHkPeriodicParams(coreHk.getSid(), false, 10.0)); poolManager.subscribeForRegularPeriodicPacket( subdp::RegularHkPeriodicParams(auxHk.getSid(), false, 30.0)); - return HasReturnvaluesIF::RETURN_OK; + return returnvalue::OK; } void ACUHandler::printChannelStats() { @@ -190,12 +190,12 @@ void ACUHandler::printChannelStats() { void ACUHandler::setDebugMode(bool enable) { this->debugMode = enable; } ReturnValue_t ACUHandler::printStatus(DeviceCommandId_t cmd) { - ReturnValue_t result = RETURN_OK; + ReturnValue_t result = returnvalue::OK; switch (cmd) { case (GOMSPACE::PRINT_SWITCH_V_I): { PoolReadGuard pg(&coreHk); result = pg.getReadResult(); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { break; } printChannelStats(); @@ -205,7 +205,7 @@ ReturnValue_t ACUHandler::printStatus(DeviceCommandId_t cmd) { return DeviceHandlerIF::COMMAND_NOT_SUPPORTED; } } - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { sif::warning << "Reading PDU1 HK table failed!" << std::endl; } return result; diff --git a/mission/devices/BpxBatteryHandler.cpp b/mission/devices/BpxBatteryHandler.cpp index d121fc8c..02eea4fe 100644 --- a/mission/devices/BpxBatteryHandler.cpp +++ b/mission/devices/BpxBatteryHandler.cpp @@ -38,7 +38,7 @@ ReturnValue_t BpxBatteryHandler::buildTransitionDeviceCommand(DeviceCommandId_t* *id = BpxBattery::PING; return buildCommandFromCommand(*id, nullptr, 0); } - return HasReturnvaluesIF::RETURN_OK; + return returnvalue::OK; } void BpxBatteryHandler::fillCommandAndReplyMap() { @@ -136,7 +136,7 @@ ReturnValue_t BpxBatteryHandler::buildCommandFromCommand(DeviceCommandId_t devic this->rawPacket = cmdBuf.data(); lastCmd = deviceCommand; - return HasReturnvaluesIF::RETURN_OK; + return returnvalue::OK; } ReturnValue_t BpxBatteryHandler::scanForReply(const uint8_t* start, size_t remainingSize, @@ -182,7 +182,7 @@ ReturnValue_t BpxBatteryHandler::scanForReply(const uint8_t* start, size_t remai } *foundLen = remainingSize; *foundId = lastCmd; - return HasReturnvaluesIF::RETURN_OK; + return returnvalue::OK; } ReturnValue_t BpxBatteryHandler::interpretDeviceReply(DeviceCommandId_t id, const uint8_t* packet) { @@ -194,7 +194,7 @@ ReturnValue_t BpxBatteryHandler::interpretDeviceReply(DeviceCommandId_t id, cons PoolReadGuard rg(&hkSet); ReturnValue_t result = hkSet.parseRawHk(packet + 2, 21); hkSet.setValidity(true, true); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { return result; } if (debugMode) { @@ -236,7 +236,7 @@ ReturnValue_t BpxBatteryHandler::interpretDeviceReply(DeviceCommandId_t id, cons case (BpxBattery::CONFIG_GET): { PoolReadGuard rg(&cfgSet); ReturnValue_t result = cfgSet.parseRawHk(packet + 2, 3); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { return result; } cfgSet.setValidity(true, true); @@ -249,7 +249,7 @@ ReturnValue_t BpxBatteryHandler::interpretDeviceReply(DeviceCommandId_t id, cons return DeviceHandlerIF::UNKNOWN_DEVICE_REPLY; } } - return HasReturnvaluesIF::RETURN_OK; + return returnvalue::OK; } uint32_t BpxBatteryHandler::getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) { return 10000; } @@ -272,7 +272,7 @@ ReturnValue_t BpxBatteryHandler::initializeLocalDataPool(localpool::DataPool& lo localDataPoolMap.emplace(BpxBattery::BATTHEAT_HIGH_LIMIT, &battheatHigh); poolManager.subscribeForRegularPeriodicPacket( subdp::RegularHkPeriodicParams(hkSet.getSid(), false, 30.0)); - return HasReturnvaluesIF::RETURN_OK; + return returnvalue::OK; } void BpxBatteryHandler::setToGoToNormalMode(bool enable) { diff --git a/mission/devices/GPSHyperionHandler.cpp b/mission/devices/GPSHyperionHandler.cpp index ac1f99fa..1bc34959 100644 --- a/mission/devices/GPSHyperionHandler.cpp +++ b/mission/devices/GPSHyperionHandler.cpp @@ -66,7 +66,7 @@ ReturnValue_t GPSHyperionHandler::buildCommandFromCommand(DeviceCommandId_t devi return DeviceHandlerIF::COMMAND_NOT_IMPLEMENTED; } } - return HasReturnvaluesIF::RETURN_OK; + return returnvalue::OK; } ReturnValue_t GPSHyperionHandler::scanForReply(const uint8_t *start, size_t len, @@ -85,7 +85,7 @@ ReturnValue_t GPSHyperionHandler::scanForReply(const uint8_t *start, size_t len, // The data from the device will generally be read all at once. Therefore, we // can set all field here PoolReadGuard pg(&gpsSet); - if (pg.getReadResult() != HasReturnvaluesIF::RETURN_OK) { + if (pg.getReadResult() != returnvalue::OK) { #if FSFW_VERBOSE_LEVEL >= 1 sif::warning << "GPSHyperionHandler::scanForReply: Reading dataset failed" << std::endl; #endif @@ -145,12 +145,12 @@ ReturnValue_t GPSHyperionHandler::scanForReply(const uint8_t *start, size_t len, *foundLen = len; *foundId = GpsHyperion::GPS_REPLY; } - return HasReturnvaluesIF::RETURN_OK; + return returnvalue::OK; } ReturnValue_t GPSHyperionHandler::interpretDeviceReply(DeviceCommandId_t id, const uint8_t *packet) { - return HasReturnvaluesIF::RETURN_OK; + return returnvalue::OK; } uint32_t GPSHyperionHandler::getTransitionDelayMs(Mode_t from, Mode_t to) { return 5000; } @@ -170,7 +170,7 @@ ReturnValue_t GPSHyperionHandler::initializeLocalDataPool(localpool::DataPool &l localDataPoolMap.emplace(GpsHyperion::SATS_IN_USE, new PoolEntry()); localDataPoolMap.emplace(GpsHyperion::FIX_MODE, new PoolEntry()); poolManager.subscribeForPeriodicPacket(gpsSet.getSid(), false, 30.0, false); - return HasReturnvaluesIF::RETURN_OK; + return returnvalue::OK; } void GPSHyperionHandler::fillCommandAndReplyMap() { @@ -191,7 +191,7 @@ void GPSHyperionHandler::debugInterface(uint8_t positionTracker, object_id_t obj ReturnValue_t GPSHyperionHandler::initialize() { ReturnValue_t result = DeviceHandlerBase::initialize(); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { return result; } // Enable reply immediately for now diff --git a/mission/devices/GomspaceDeviceHandler.cpp b/mission/devices/GomspaceDeviceHandler.cpp index da54168a..1cf9ea68 100644 --- a/mission/devices/GomspaceDeviceHandler.cpp +++ b/mission/devices/GomspaceDeviceHandler.cpp @@ -26,11 +26,11 @@ void GomspaceDeviceHandler::doStartUp() {} void GomspaceDeviceHandler::doShutDown() {} ReturnValue_t GomspaceDeviceHandler::buildNormalDeviceCommand(DeviceCommandId_t* id) { - return HasReturnvaluesIF::RETURN_OK; + return returnvalue::OK; } ReturnValue_t GomspaceDeviceHandler::buildTransitionDeviceCommand(DeviceCommandId_t* id) { - return HasReturnvaluesIF::RETURN_OK; + return returnvalue::OK; } ReturnValue_t GomspaceDeviceHandler::buildCommandFromCommand(DeviceCommandId_t deviceCommand, @@ -40,7 +40,7 @@ ReturnValue_t GomspaceDeviceHandler::buildCommandFromCommand(DeviceCommandId_t d switch (deviceCommand) { case (GOMSPACE::PING): { result = generatePingCommand(commandData, commandDataLen); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { return result; } break; @@ -51,21 +51,21 @@ ReturnValue_t GomspaceDeviceHandler::buildCommandFromCommand(DeviceCommandId_t d } case (GOMSPACE::PARAM_SET): { result = generateSetParamCommand(commandData, commandDataLen); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { return result; } break; } case (GOMSPACE::PARAM_GET): { result = generateGetParamCommand(commandData, commandDataLen); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { return result; } break; } case (GOMSPACE::GNDWDT_RESET): { result = generateResetWatchdogCmd(); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { return result; } break; @@ -77,7 +77,7 @@ ReturnValue_t GomspaceDeviceHandler::buildCommandFromCommand(DeviceCommandId_t d } case (GOMSPACE::REQUEST_HK_TABLE): { result = generateRequestFullHkTableCmd(hkTableReplySize); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { return result; } break; @@ -85,7 +85,7 @@ ReturnValue_t GomspaceDeviceHandler::buildCommandFromCommand(DeviceCommandId_t d default: return DeviceHandlerIF::COMMAND_NOT_IMPLEMENTED; } - return HasReturnvaluesIF::RETURN_OK; + return returnvalue::OK; } void GomspaceDeviceHandler::fillCommandAndReplyMap() { @@ -128,7 +128,7 @@ ReturnValue_t GomspaceDeviceHandler::scanForReply(const uint8_t* start, size_t r default: return IGNORE_REPLY_DATA; } - return HasReturnvaluesIF::RETURN_OK; + return returnvalue::OK; } ReturnValue_t GomspaceDeviceHandler::interpretDeviceReply(DeviceCommandId_t id, @@ -153,7 +153,7 @@ ReturnValue_t GomspaceDeviceHandler::interpretDeviceReply(DeviceCommandId_t id, size_t size = GOMSPACE::GS_HDR_LENGTH + payloadLength; ReturnValue_t result = cspGetParamReply.deSerialize(&packet, &size, SerializeIF::Endianness::BIG); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { sif::error << "GomspaceDeviceHandler: Failed to deserialize get parameter" << "reply" << std::endl; return result; @@ -170,7 +170,7 @@ ReturnValue_t GomspaceDeviceHandler::interpretDeviceReply(DeviceCommandId_t id, /* When setting a parameter, the p60dock sends back the state of the * operation */ if (*packet != PARAM_SET_OK) { - return HasReturnvaluesIF::RETURN_FAILED; + return returnvalue::FAILED; } setParamCallback(setParamCacher, true); break; @@ -182,7 +182,7 @@ ReturnValue_t GomspaceDeviceHandler::interpretDeviceReply(DeviceCommandId_t id, default: break; } - return HasReturnvaluesIF::RETURN_OK; + return returnvalue::OK; } void GomspaceDeviceHandler::setNormalDatapoolEntriesInvalid() {} @@ -195,16 +195,16 @@ ReturnValue_t GomspaceDeviceHandler::generateSetParamCommand(const uint8_t* comm if (setParamCacher.getAddress() == PDU2::CONFIG_ADDRESS_OUT_EN_Q7S and this->getObjectId() == objects::PDU2_HANDLER) { triggerEvent(power::SWITCHING_Q7S_DENIED, 0, 0); - return HasReturnvaluesIF::RETURN_FAILED; + return returnvalue::FAILED; } - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { sif::error << "GomspaceDeviceHandler: Failed to deserialize set parameter " "message" << std::endl; return result; } result = setParamCallback(setParamCacher, false); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { return result; } /* Get and check address */ @@ -231,7 +231,7 @@ ReturnValue_t GomspaceDeviceHandler::generateSetParamCommand(const uint8_t* comm result = setParamCmd.serialize(&buffer, &cspPacketLen, sizeof(cspPacket), SerializeIF::Endianness::BIG); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { sif::error << "GomspaceDeviceHandler: Failed to serialize command for " << "CspComIF" << std::endl; return result; @@ -246,7 +246,7 @@ ReturnValue_t GomspaceDeviceHandler::generateSetParamCommand(const uint8_t* comm rawPacketLen = cspPacketLen; rememberRequestedSize = querySize; rememberCommandId = GOMSPACE::PARAM_SET; - return HasReturnvaluesIF::RETURN_OK; + return returnvalue::OK; } ReturnValue_t GomspaceDeviceHandler::generateGetParamCommand(const uint8_t* commandData, @@ -255,7 +255,7 @@ ReturnValue_t GomspaceDeviceHandler::generateGetParamCommand(const uint8_t* comm /* Unpack the received action message */ GetParamMessageUnpacker getParamMessage; result = getParamMessage.deSerialize(&commandData, &commandDataLen, SerializeIF::Endianness::BIG); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { sif::error << "Failed to deserialize message to extract information " "from get parameter message" << std::endl; @@ -299,7 +299,7 @@ ReturnValue_t GomspaceDeviceHandler::generateGetParamCommand(const uint8_t* comm uint8_t* buffer = cspPacket; result = getParamCmd.serialize(&buffer, &cspPacketLen, sizeof(cspPacket), SerializeIF::Endianness::BIG); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { sif::error << "GomspaceDeviceHandler: Failed to serialize command to " << "get parameter" << std::endl; } @@ -313,7 +313,7 @@ ReturnValue_t GomspaceDeviceHandler::generateGetParamCommand(const uint8_t* comm rawPacketLen = cspPacketLen; rememberRequestedSize = querySize; rememberCommandId = GOMSPACE::PARAM_GET; - return HasReturnvaluesIF::RETURN_OK; + return returnvalue::OK; } ReturnValue_t GomspaceDeviceHandler::generatePingCommand(const uint8_t* commandData, @@ -323,7 +323,7 @@ ReturnValue_t GomspaceDeviceHandler::generatePingCommand(const uint8_t* commandD uint8_t* buffer = cspPacket; ReturnValue_t result = cspPingCommand.serialize(&buffer, &cspPacketLen, sizeof(cspPacket), SerializeIF::Endianness::BIG); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { sif::error << "GomspaceDeviceHandler: Failed to serialize ping command" << std::endl; return result; } @@ -334,7 +334,7 @@ ReturnValue_t GomspaceDeviceHandler::generatePingCommand(const uint8_t* commandD rawPacket = cspPacket; rawPacketLen = cspPacketLen; rememberCommandId = GOMSPACE::PING; - return HasReturnvaluesIF::RETURN_OK; + return returnvalue::OK; } void GomspaceDeviceHandler::generateRebootCommand() { @@ -355,7 +355,7 @@ ReturnValue_t GomspaceDeviceHandler::childCommandHook(DeviceCommandId_t cmd, ReturnValue_t GomspaceDeviceHandler::setParamCallback(SetParamMessageUnpacker& unpacker, bool afterExecution) { - return HasReturnvaluesIF::RETURN_OK; + return returnvalue::OK; } ReturnValue_t GomspaceDeviceHandler::initializePduPool( @@ -393,7 +393,7 @@ ReturnValue_t GomspaceDeviceHandler::initializePduPool( localDataPoolMap.emplace(P60System::pool::PDU_WDT_CAN_LEFT, new PoolEntry({0})); localDataPoolMap.emplace(P60System::pool::PDU_WDT_CSP_LEFT1, new PoolEntry({0})); localDataPoolMap.emplace(P60System::pool::PDU_WDT_CSP_LEFT2, new PoolEntry({0})); - return RETURN_OK; + return returnvalue::OK; } ReturnValue_t GomspaceDeviceHandler::generateResetWatchdogCmd() { @@ -402,7 +402,7 @@ ReturnValue_t GomspaceDeviceHandler::generateResetWatchdogCmd() { uint8_t* buffer = cspPacket; ReturnValue_t result = watchdogResetCommand.serialize(&buffer, &cspPacketLen, sizeof(cspPacket), SerializeIF::Endianness::BIG); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { sif::error << "GomspaceDeviceHandler: Failed to serialize watchdog reset " << "command" << std::endl; return result; @@ -412,7 +412,7 @@ ReturnValue_t GomspaceDeviceHandler::generateResetWatchdogCmd() { rememberRequestedSize = 0; // No bytes will be queried with the ground // watchdog command. rememberCommandId = GOMSPACE::GNDWDT_RESET; - return HasReturnvaluesIF::RETURN_OK; + return returnvalue::OK; } ReturnValue_t GomspaceDeviceHandler::generateRequestFullHkTableCmd(uint16_t hkTableReplySize) { @@ -424,7 +424,7 @@ ReturnValue_t GomspaceDeviceHandler::generateRequestFullHkTableCmd(uint16_t hkTa uint8_t* buffer = cspPacket; ReturnValue_t result = requestFullTableCommand.serialize( &buffer, &cspPacketLen, sizeof(cspPacket), SerializeIF::Endianness::BIG); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { sif::error << "GomspaceDeviceHandler::generateRequestFullHkTableCmd Failed to serialize " "full table request command " << std::endl; @@ -434,7 +434,7 @@ ReturnValue_t GomspaceDeviceHandler::generateRequestFullHkTableCmd(uint16_t hkTa rawPacketLen = cspPacketLen; rememberRequestedSize = querySize; rememberCommandId = GOMSPACE::REQUEST_HK_TABLE; - return RETURN_OK; + return returnvalue::OK; } uint32_t GomspaceDeviceHandler::getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) { return 0; } @@ -443,7 +443,7 @@ void GomspaceDeviceHandler::setModeNormal() { mode = MODE_NORMAL; } ReturnValue_t GomspaceDeviceHandler::printStatus(DeviceCommandId_t cmd) { sif::info << "No printHkTable implementation given.." << std::endl; - return HasReturnvaluesIF::RETURN_OK; + return returnvalue::OK; } ReturnValue_t GomspaceDeviceHandler::parsePduHkTable(PDU::PduCoreHk& coreHk, PDU::PduAuxHk& auxHk, @@ -451,10 +451,10 @@ ReturnValue_t GomspaceDeviceHandler::parsePduHkTable(PDU::PduCoreHk& coreHk, PDU uint16_t dataOffset = 0; PoolReadGuard pg0(&coreHk); PoolReadGuard pg1(&auxHk); - if (pg0.getReadResult() != HasReturnvaluesIF::RETURN_OK or - pg1.getReadResult() != HasReturnvaluesIF::RETURN_OK) { + if (pg0.getReadResult() != returnvalue::OK or + pg1.getReadResult() != returnvalue::OK) { sif::warning << "Reading PDU1 datasets failed!" << std::endl; - return HasReturnvaluesIF::RETURN_FAILED; + return returnvalue::FAILED; } /* Fist 10 bytes contain the gomspace header. Each variable is preceded by the 16-bit table * address. */ @@ -551,5 +551,5 @@ ReturnValue_t GomspaceDeviceHandler::parsePduHkTable(PDU::PduCoreHk& coreHk, PDU if (not auxHk.isValid()) { auxHk.setValidity(true, true); } - return RETURN_OK; + return returnvalue::OK; } diff --git a/mission/devices/GomspaceDeviceHandler.h b/mission/devices/GomspaceDeviceHandler.h index ea09b374..b564abd4 100644 --- a/mission/devices/GomspaceDeviceHandler.h +++ b/mission/devices/GomspaceDeviceHandler.h @@ -21,12 +21,12 @@ class GomspaceDeviceHandler : public DeviceHandlerBase { public: static constexpr uint8_t CLASS_ID = CLASS_ID::GOM_SPACE_HANDLER; - static const ReturnValue_t PACKET_TOO_LONG = HasReturnvaluesIF::makeReturnCode(CLASS_ID, 0); - static const ReturnValue_t INVALID_TABLE_ID = HasReturnvaluesIF::makeReturnCode(CLASS_ID, 1); - static const ReturnValue_t INVALID_ADDRESS = HasReturnvaluesIF::makeReturnCode(CLASS_ID, 2); - static const ReturnValue_t INVALID_PARAM_SIZE = HasReturnvaluesIF::makeReturnCode(CLASS_ID, 3); - static const ReturnValue_t INVALID_PAYLOAD_SIZE = HasReturnvaluesIF::makeReturnCode(CLASS_ID, 4); - static const ReturnValue_t UNKNOWN_REPLY_ID = HasReturnvaluesIF::makeReturnCode(CLASS_ID, 5); + static const ReturnValue_t PACKET_TOO_LONG = returnvalue::makeCode(CLASS_ID, 0); + static const ReturnValue_t INVALID_TABLE_ID = returnvalue::makeCode(CLASS_ID, 1); + static const ReturnValue_t INVALID_ADDRESS = returnvalue::makeCode(CLASS_ID, 2); + static const ReturnValue_t INVALID_PARAM_SIZE = returnvalue::makeCode(CLASS_ID, 3); + static const ReturnValue_t INVALID_PAYLOAD_SIZE = returnvalue::makeCode(CLASS_ID, 4); + static const ReturnValue_t UNKNOWN_REPLY_ID = returnvalue::makeCode(CLASS_ID, 5); /** * @brief Constructor @@ -105,7 +105,7 @@ class GomspaceDeviceHandler : public DeviceHandlerBase { /** * @brief Can be overriden by child classes to implement device specific commands. * @return Return DeviceHandlerIF::COMMAND_NOT_IMPLEMENTED to let this handler handle - * the command or RETURN_OK if the child handles the command + * the command or returnvalue::OK if the child handles the command */ virtual ReturnValue_t childCommandHook(DeviceCommandId_t cmd, const uint8_t *commandData, size_t commandDataLen); diff --git a/mission/devices/GyroADIS1650XHandler.cpp b/mission/devices/GyroADIS1650XHandler.cpp index 3e798f04..5ff84a96 100644 --- a/mission/devices/GyroADIS1650XHandler.cpp +++ b/mission/devices/GyroADIS1650XHandler.cpp @@ -87,10 +87,10 @@ ReturnValue_t GyroADIS1650XHandler::buildTransitionDeviceCommand(DeviceCommandId sif::debug << "GyroADIS16507Handler::buildTransitionDeviceCommand: " "Unknown internal state!" << std::endl; - return HasReturnvaluesIF::RETURN_OK; + return returnvalue::OK; } } - return HasReturnvaluesIF::RETURN_OK; + return returnvalue::OK; } ReturnValue_t GyroADIS1650XHandler::buildCommandFromCommand(DeviceCommandId_t deviceCommand, @@ -178,7 +178,7 @@ ReturnValue_t GyroADIS1650XHandler::buildCommandFromCommand(DeviceCommandId_t de #endif /* OBSW_VERBOSE_LEVEL >= 1 */ } } - return HasReturnvaluesIF::RETURN_OK; + return returnvalue::OK; } void GyroADIS1650XHandler::fillCommandAndReplyMap() { @@ -198,7 +198,7 @@ ReturnValue_t GyroADIS1650XHandler::scanForReply(const uint8_t *start, size_t re *foundId = this->getPendingCommand(); *foundLen = this->rawPacketLen; - return HasReturnvaluesIF::RETURN_OK; + return returnvalue::OK; } ReturnValue_t GyroADIS1650XHandler::interpretDeviceReply(DeviceCommandId_t id, @@ -215,7 +215,7 @@ ReturnValue_t GyroADIS1650XHandler::interpretDeviceReply(DeviceCommandId_t id, } warningSwitch = false; #endif - return HasReturnvaluesIF::RETURN_FAILED; + return returnvalue::FAILED; } PoolReadGuard rg(&configDataset); configDataset.diagStatReg.value = packet[2] << 8 | packet[3]; @@ -232,7 +232,7 @@ ReturnValue_t GyroADIS1650XHandler::interpretDeviceReply(DeviceCommandId_t id, return handleSensorData(packet); } } - return HasReturnvaluesIF::RETURN_OK; + return returnvalue::OK; } ReturnValue_t GyroADIS1650XHandler::handleSensorData(const uint8_t *packet) { @@ -243,7 +243,7 @@ ReturnValue_t GyroADIS1650XHandler::handleSensorData(const uint8_t *packet) { sif::warning << "GyroADIS1650XHandler::interpretDeviceReply: Analysis with BURST_SEL1" " not implemented!" << std::endl; - return HasReturnvaluesIF::RETURN_OK; + return returnvalue::OK; } case (BurstModes::BURST_16_BURST_SEL_0): { uint16_t checksum = packet[20] << 8 | packet[21]; @@ -258,11 +258,11 @@ ReturnValue_t GyroADIS1650XHandler::handleSensorData(const uint8_t *packet) { "Invalid checksum detected!" << std::endl; #endif - return HasReturnvaluesIF::RETURN_FAILED; + return returnvalue::FAILED; } ReturnValue_t result = configDataset.diagStatReg.read(); - if (result == HasReturnvaluesIF::RETURN_OK) { + if (result == returnvalue::OK) { configDataset.diagStatReg.value = packet[2] << 8 | packet[3]; configDataset.diagStatReg.setValid(true); } @@ -322,7 +322,7 @@ ReturnValue_t GyroADIS1650XHandler::handleSensorData(const uint8_t *packet) { break; } } - return HasReturnvaluesIF::RETURN_OK; + return returnvalue::OK; } uint32_t GyroADIS1650XHandler::getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) { return 6000; } @@ -365,7 +365,7 @@ ReturnValue_t GyroADIS1650XHandler::initializeLocalDataPool(localpool::DataPool localDataPoolMap.emplace(ADIS1650X::DEC_RATE_REGISTER, new PoolEntry()); poolManager.subscribeForRegularPeriodicPacket( subdp::RegularHkPeriodicParams(primaryDataset.getSid(), false, 5.0)); - return HasReturnvaluesIF::RETURN_OK; + return returnvalue::OK; } GyroADIS1650XHandler::BurstModes GyroADIS1650XHandler::getBurstMode() { @@ -396,7 +396,7 @@ ReturnValue_t GyroADIS1650XHandler::spiSendCallback(SpiComIF *comIf, SpiCookie * if (handler == nullptr) { sif::error << "GyroADIS16507Handler::spiSendCallback: Passed handler pointer is invalid!" << std::endl; - return HasReturnvaluesIF::RETURN_FAILED; + return returnvalue::FAILED; } DeviceCommandId_t currentCommand = handler->getPendingCommand(); switch (currentCommand) { @@ -405,13 +405,13 @@ ReturnValue_t GyroADIS1650XHandler::spiSendCallback(SpiComIF *comIf, SpiCookie * } case (ADIS1650X::READ_OUT_CONFIG): default: { - ReturnValue_t result = HasReturnvaluesIF::RETURN_OK; + ReturnValue_t result = returnvalue::OK; int retval = 0; // Prepare transfer int fileDescriptor = 0; std::string device = comIf->getSpiDev(); UnixFileGuard fileHelper(device, &fileDescriptor, O_RDWR, "SpiComIF::sendMessage"); - if (fileHelper.getOpenResult() != HasReturnvaluesIF::RETURN_OK) { + if (fileHelper.getOpenResult() != returnvalue::OK) { return SpiComIF::OPENING_FILE_FAILED; } spi::SpiModes spiMode = spi::SpiModes::MODE_0; @@ -432,13 +432,13 @@ ReturnValue_t GyroADIS1650XHandler::spiSendCallback(SpiComIF *comIf, SpiCookie * sif::warning << "GyroADIS16507Handler::spiSendCallback: " "Mutex or GPIO interface invalid" << std::endl; - return HasReturnvaluesIF::RETURN_FAILED; + return returnvalue::FAILED; #endif } if (gpioId != gpio::NO_GPIO) { result = mutex->lockMutex(timeoutType, timeoutMs); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { #if FSFW_CPP_OSTREAM_ENABLED == 1 sif::error << "SpiComIF::sendMessage: Failed to lock mutex" << std::endl; #endif @@ -486,7 +486,7 @@ ReturnValue_t GyroADIS1650XHandler::spiSendCallback(SpiComIF *comIf, SpiCookie * } } } - return HasReturnvaluesIF::RETURN_OK; + return returnvalue::OK; } void GyroADIS1650XHandler::setToGoToNormalModeImmediately() { goToNormalMode = true; } diff --git a/mission/devices/HeaterHandler.cpp b/mission/devices/HeaterHandler.cpp index 6953dc54..3c25eb6a 100644 --- a/mission/devices/HeaterHandler.cpp +++ b/mission/devices/HeaterHandler.cpp @@ -52,17 +52,17 @@ ReturnValue_t HeaterHandler::performOperation(uint8_t operationCode) { "Out of range error | " << e.what() << std::endl; } - return RETURN_OK; + return returnvalue::OK; } ReturnValue_t HeaterHandler::initialize() { ReturnValue_t result = SystemObject::initialize(); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return ObjectManagerIF::CHILD_INIT_FAILED; } result = initializeHeaterMap(); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return ObjectManagerIF::CHILD_INIT_FAILED; } @@ -73,35 +73,35 @@ ReturnValue_t HeaterHandler::initialize() { } result = actionHelper.initialize(commandQueue); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return ObjectManagerIF::CHILD_INIT_FAILED; } - return RETURN_OK; + return returnvalue::OK; } ReturnValue_t HeaterHandler::initializeHeaterMap() { for (power::Switch_t switchNr = 0; switchNr < heater::NUMBER_OF_SWITCHES; switchNr++) { heaterVec.push_back(HeaterWrapper(helper.heaters[switchNr], SwitchState::OFF)); } - return RETURN_OK; + return returnvalue::OK; } void HeaterHandler::readCommandQueue() { - ReturnValue_t result = RETURN_OK; + ReturnValue_t result = returnvalue::OK; CommandMessage command; do { result = commandQueue->receiveMessage(&command); if (result == MessageQueueIF::EMPTY) { break; - } else if (result != RETURN_OK) { + } else if (result != returnvalue::OK) { sif::warning << "HeaterHandler::readCommandQueue: Message reception error" << std::endl; } result = actionHelper.handleActionMessage(&command); - if (result == RETURN_OK) { + if (result == returnvalue::OK) { continue; } - } while (result == RETURN_OK); + } while (result == returnvalue::OK); } ReturnValue_t HeaterHandler::executeAction(ActionId_t actionId, MessageQueueId_t commandedBy, @@ -147,7 +147,7 @@ ReturnValue_t HeaterHandler::executeAction(ActionId_t actionId, MessageQueueId_t heater.action = action; heater.cmdActive = true; heater.replyQueue = commandedBy; - return RETURN_OK; + return returnvalue::OK; } ReturnValue_t HeaterHandler::sendSwitchCommand(uint8_t switchNr, ReturnValue_t onOff) { @@ -172,12 +172,12 @@ ReturnValue_t HeaterHandler::sendSwitchCommand(uint8_t switchNr, ReturnValue_t o } result = ipcStore->addData(&storeAddress, commandData, sizeof(commandData)); - if (result == RETURN_OK) { + if (result == returnvalue::OK) { CommandMessage message; ActionMessage::setCommand(&message, SWITCH_HEATER, storeAddress); /* Send heater command to own command queue */ result = commandQueue->sendMessage(commandQueue->getId(), &message, 0); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { sif::debug << "HeaterHandler::sendSwitchCommand: Failed to send switch" << "message" << std::endl; } @@ -217,7 +217,7 @@ void HeaterHandler::handleSwitchHandling() { } void HeaterHandler::handleSwitchOnCommand(heater::Switchers heaterIdx) { - ReturnValue_t result = RETURN_OK; + ReturnValue_t result = returnvalue::OK; auto& heater = heaterVec.at(heaterIdx); /* Check if command waits for main switch being set on and whether the timeout has expired */ if (heater.waitMainSwitchOn && heater.mainSwitchCountdown.hasTimedOut()) { @@ -239,7 +239,7 @@ void HeaterHandler::handleSwitchOnCommand(heater::Switchers heaterIdx) { if (checkSwitchState(heaterIdx) == SwitchState::OFF) { gpioId_t gpioId = heater.gpioId; result = gpioInterface->pullHigh(gpioId); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { sif::error << "HeaterHandler::handleSwitchOnCommand: Failed to pull gpio with id " << gpioId << " high" << std::endl; triggerEvent(GPIO_PULL_HIGH_FAILED, result); @@ -253,7 +253,7 @@ void HeaterHandler::handleSwitchOnCommand(heater::Switchers heaterIdx) { // There is no need to send action finish replies if the sender was the // HeaterHandler itself if (heater.replyQueue != commandQueue->getId()) { - if (result == RETURN_OK) { + if (result == returnvalue::OK) { actionHelper.finish(true, heater.replyQueue, heater.action, result); } else { actionHelper.finish(false, heater.replyQueue, heater.action, result); @@ -279,20 +279,20 @@ void HeaterHandler::handleSwitchOnCommand(heater::Switchers heaterIdx) { } void HeaterHandler::handleSwitchOffCommand(heater::Switchers heaterIdx) { - ReturnValue_t result = RETURN_OK; + ReturnValue_t result = returnvalue::OK; auto& heater = heaterVec.at(heaterIdx); // Check whether switch is already off if (checkSwitchState(heaterIdx)) { gpioId_t gpioId = heater.gpioId; result = gpioInterface->pullLow(gpioId); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { sif::error << "HeaterHandler::handleSwitchOffCommand: Failed to pull gpio with id" << gpioId << " low" << std::endl; triggerEvent(GPIO_PULL_LOW_FAILED, result); } else { auto result = heaterMutex->lockMutex(); heater.switchState = OFF; - if (result == HasReturnvaluesIF::RETURN_OK) { + if (result == returnvalue::OK) { heaterMutex->unlockMutex(); } triggerEvent(HEATER_WENT_OFF, heaterIdx, 0); @@ -307,7 +307,7 @@ void HeaterHandler::handleSwitchOffCommand(heater::Switchers heaterIdx) { } if (heater.replyQueue != NO_COMMANDER) { // Report back switch command reply if necessary - if (result == HasReturnvaluesIF::RETURN_OK) { + if (result == returnvalue::OK) { actionHelper.finish(true, heater.replyQueue, heater.action, result); } else { actionHelper.finish(false, heater.replyQueue, heater.action, result); @@ -333,7 +333,7 @@ bool HeaterHandler::allSwitchesOff() { MessageQueueId_t HeaterHandler::getCommandQueue() const { return commandQueue->getId(); } -ReturnValue_t HeaterHandler::sendFuseOnCommand(uint8_t fuseNr) { return RETURN_OK; } +ReturnValue_t HeaterHandler::sendFuseOnCommand(uint8_t fuseNr) { return returnvalue::OK; } ReturnValue_t HeaterHandler::getSwitchState(uint8_t switchNr) const { ReturnValue_t mainSwitchState = mainLineSwitcher->getSwitchState(mainLineSwitch); @@ -341,7 +341,7 @@ ReturnValue_t HeaterHandler::getSwitchState(uint8_t switchNr) const { return PowerSwitchIF::SWITCH_OFF; } if (switchNr > 7) { - return PowerSwitchIF::RETURN_FAILED; + return returnvalue::FAILED; } if (checkSwitchState(static_cast(switchNr)) == SwitchState::ON) { return PowerSwitchIF::SWITCH_ON; diff --git a/mission/devices/HeaterHandler.h b/mission/devices/HeaterHandler.h index 0f4420b4..75badf91 100644 --- a/mission/devices/HeaterHandler.h +++ b/mission/devices/HeaterHandler.h @@ -9,7 +9,7 @@ #include #include #include -#include +#include #include #include #include diff --git a/mission/devices/IMTQHandler.cpp b/mission/devices/IMTQHandler.cpp index 0d1f72db..52df8c97 100644 --- a/mission/devices/IMTQHandler.cpp +++ b/mission/devices/IMTQHandler.cpp @@ -76,48 +76,48 @@ ReturnValue_t IMTQHandler::buildCommandFromCommand(DeviceCommandId_t deviceComma commandBuffer[1] = IMTQ::SELF_TEST_AXIS::X_POSITIVE; rawPacket = commandBuffer; rawPacketLen = 2; - return RETURN_OK; + return returnvalue::OK; } case (IMTQ::NEG_X_SELF_TEST): { commandBuffer[0] = IMTQ::CC::SELF_TEST_CMD; commandBuffer[1] = IMTQ::SELF_TEST_AXIS::X_NEGATIVE; rawPacket = commandBuffer; rawPacketLen = 2; - return RETURN_OK; + return returnvalue::OK; } case (IMTQ::POS_Y_SELF_TEST): { commandBuffer[0] = IMTQ::CC::SELF_TEST_CMD; commandBuffer[1] = IMTQ::SELF_TEST_AXIS::Y_POSITIVE; rawPacket = commandBuffer; rawPacketLen = 2; - return RETURN_OK; + return returnvalue::OK; } case (IMTQ::NEG_Y_SELF_TEST): { commandBuffer[0] = IMTQ::CC::SELF_TEST_CMD; commandBuffer[1] = IMTQ::SELF_TEST_AXIS::Y_NEGATIVE; rawPacket = commandBuffer; rawPacketLen = 2; - return RETURN_OK; + return returnvalue::OK; } case (IMTQ::POS_Z_SELF_TEST): { commandBuffer[0] = IMTQ::CC::SELF_TEST_CMD; commandBuffer[1] = IMTQ::SELF_TEST_AXIS::Z_POSITIVE; rawPacket = commandBuffer; rawPacketLen = 2; - return RETURN_OK; + return returnvalue::OK; } case (IMTQ::NEG_Z_SELF_TEST): { commandBuffer[0] = IMTQ::CC::SELF_TEST_CMD; commandBuffer[1] = IMTQ::SELF_TEST_AXIS::Z_NEGATIVE; rawPacket = commandBuffer; rawPacketLen = 2; - return RETURN_OK; + return returnvalue::OK; } case (IMTQ::GET_SELF_TEST_RESULT): { commandBuffer[0] = IMTQ::CC::GET_SELF_TEST_RESULT; rawPacket = commandBuffer; rawPacketLen = 1; - return RETURN_OK; + return returnvalue::OK; } case (IMTQ::START_ACTUATION_DIPOLE): { /* IMTQ expects low byte first */ @@ -135,42 +135,42 @@ ReturnValue_t IMTQHandler::buildCommandFromCommand(DeviceCommandId_t deviceComma commandBuffer[8] = commandData[6]; rawPacket = commandBuffer; rawPacketLen = 9; - return RETURN_OK; + return returnvalue::OK; } case (IMTQ::GET_ENG_HK_DATA): { commandBuffer[0] = IMTQ::CC::GET_ENG_HK_DATA; rawPacket = commandBuffer; rawPacketLen = 1; - return RETURN_OK; + return returnvalue::OK; } case (IMTQ::GET_COMMANDED_DIPOLE): { commandBuffer[0] = IMTQ::CC::GET_COMMANDED_DIPOLE; rawPacket = commandBuffer; rawPacketLen = 1; - return RETURN_OK; + return returnvalue::OK; } case (IMTQ::START_MTM_MEASUREMENT): { commandBuffer[0] = IMTQ::CC::START_MTM_MEASUREMENT; rawPacket = commandBuffer; rawPacketLen = 1; - return RETURN_OK; + return returnvalue::OK; } case (IMTQ::GET_CAL_MTM_MEASUREMENT): { commandBuffer[0] = IMTQ::CC::GET_CAL_MTM_MEASUREMENT; rawPacket = commandBuffer; rawPacketLen = 1; - return RETURN_OK; + return returnvalue::OK; } case (IMTQ::GET_RAW_MTM_MEASUREMENT): { commandBuffer[0] = IMTQ::CC::GET_RAW_MTM_MEASUREMENT; rawPacket = commandBuffer; rawPacketLen = 1; - return RETURN_OK; + return returnvalue::OK; } default: return DeviceHandlerIF::COMMAND_NOT_IMPLEMENTED; } - return HasReturnvaluesIF::RETURN_FAILED; + return returnvalue::FAILED; } void IMTQHandler::fillCommandAndReplyMap() { @@ -198,7 +198,7 @@ void IMTQHandler::fillCommandAndReplyMap() { ReturnValue_t IMTQHandler::scanForReply(const uint8_t* start, size_t remainingSize, DeviceCommandId_t* foundId, size_t* foundLen) { - ReturnValue_t result = RETURN_OK; + ReturnValue_t result = returnvalue::OK; switch (*start) { case (IMTQ::CC::START_ACTUATION_DIPOLE): @@ -243,11 +243,11 @@ ReturnValue_t IMTQHandler::scanForReply(const uint8_t* start, size_t remainingSi } ReturnValue_t IMTQHandler::interpretDeviceReply(DeviceCommandId_t id, const uint8_t* packet) { - ReturnValue_t result = RETURN_OK; + ReturnValue_t result = returnvalue::OK; result = parseStatusByte(packet); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return result; } @@ -283,7 +283,7 @@ ReturnValue_t IMTQHandler::interpretDeviceReply(DeviceCommandId_t id, const uint } } - return RETURN_OK; + return returnvalue::OK; } void IMTQHandler::setNormalDatapoolEntriesInvalid() {} @@ -608,7 +608,7 @@ ReturnValue_t IMTQHandler::initializeLocalDataPool(localpool::DataPool& localDat subdp::DiagnosticsHkPeriodicParams(calMtmMeasurementSet.getSid(), false, 10.0)); poolManager.subscribeForDiagPeriodicPacket( subdp::DiagnosticsHkPeriodicParams(rawMtmMeasurementSet.getSid(), false, 10.0)); - return HasReturnvaluesIF::RETURN_OK; + return returnvalue::OK; } ReturnValue_t IMTQHandler::getSelfTestCommandId(DeviceCommandId_t* id) { @@ -627,14 +627,14 @@ ReturnValue_t IMTQHandler::getSelfTestCommandId(DeviceCommandId_t* id) { << "command" << std::endl; return UNEXPECTED_SELF_TEST_REPLY; } - return RETURN_OK; + return returnvalue::OK; } ReturnValue_t IMTQHandler::parseStatusByte(const uint8_t* packet) { uint8_t cmdErrorField = *(packet + 1) & 0xF; switch (cmdErrorField) { case 0: - return RETURN_OK; + return returnvalue::OK; case 1: sif::error << "IMTQHandler::parseStatusByte: Command rejected without reason" << std::endl; return REJECTED_WITHOUT_REASON; @@ -728,7 +728,7 @@ void IMTQHandler::handleDeviceTM(const uint8_t* data, size_t dataSize, DeviceCom } ReturnValue_t result = actionHelper.reportData(queueId, replyId, data, dataSize); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { sif::debug << "IMTQHandler::handleDeviceTM: Failed to report data" << std::endl; return; } @@ -787,22 +787,22 @@ void IMTQHandler::fillRawMtmDataset(const uint8_t* packet) { uint32_t coilActStatus = 0; auto res = SerializeAdapter::deSerialize(&xRaw, &dataStart, &deSerLen, SerializeIF::Endianness::LITTLE); - if (res != HasReturnvaluesIF::RETURN_OK) { + if (res != returnvalue::OK) { return; } res = SerializeAdapter::deSerialize(&yRaw, &dataStart, &deSerLen, SerializeIF::Endianness::LITTLE); - if (res != HasReturnvaluesIF::RETURN_OK) { + if (res != returnvalue::OK) { return; } res = SerializeAdapter::deSerialize(&zRaw, &dataStart, &deSerLen, SerializeIF::Endianness::LITTLE); - if (res != HasReturnvaluesIF::RETURN_OK) { + if (res != returnvalue::OK) { return; } res = SerializeAdapter::deSerialize(&coilActStatus, &dataStart, &deSerLen, SerializeIF::Endianness::LITTLE); - if (res != HasReturnvaluesIF::RETURN_OK) { + if (res != returnvalue::OK) { return; } rawMtmMeasurementSet.mtmRawNt[0] = xRaw * 7.5; @@ -2229,7 +2229,7 @@ ReturnValue_t IMTQHandler::getSwitches(const uint8_t** switches, uint8_t* number if (switcher != power::NO_SWITCH) { *numberOfSwitches = 1; *switches = &switcher; - return RETURN_OK; + return returnvalue::OK; } return DeviceHandlerBase::NO_SWITCH; } diff --git a/mission/devices/Max31865EiveHandler.cpp b/mission/devices/Max31865EiveHandler.cpp index 7ad15670..8858cf38 100644 --- a/mission/devices/Max31865EiveHandler.cpp +++ b/mission/devices/Max31865EiveHandler.cpp @@ -86,7 +86,7 @@ ReturnValue_t Max31865EiveHandler::buildCommandFromCommand(DeviceCommandId_t dev default: return NOTHING_TO_SEND; } - return RETURN_OK; + return returnvalue::OK; } void Max31865EiveHandler::setInstantNormal(bool instantNormal) { @@ -135,14 +135,14 @@ ReturnValue_t Max31865EiveHandler::scanForReply(const uint8_t* start, size_t rem } *foundId = EiveMax31855::RtdCommands::EXCHANGE_SET_ID; *foundLen = remainingSize; - return RETURN_OK; + return returnvalue::OK; } ReturnValue_t Max31865EiveHandler::interpretDeviceReply(DeviceCommandId_t id, const uint8_t* packet) { size_t deserTmp = structLen; auto result = exchangeStruct.deSerialize(&packet, &deserTmp, SerializeIF::Endianness::MACHINE); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return result; } if (mode == _MODE_TO_NORMAL and exchangeStruct.active and state == InternalState::ACTIVE) { @@ -157,10 +157,10 @@ ReturnValue_t Max31865EiveHandler::interpretDeviceReply(DeviceCommandId_t id, float approxTemp = exchangeStruct.adcCode / 32.0 - 256.0; PoolReadGuard pg(&sensorDataset); - if (pg.getReadResult() != HasReturnvaluesIF::RETURN_OK) { + if (pg.getReadResult() != returnvalue::OK) { sif::warning << "Max31865EiveHandler: Failed to read sensor dataset" << std::endl; sensorDataset.temperatureCelcius.setValid(false); - return RETURN_OK; + return returnvalue::OK; } sensorDataset.temperatureCelcius = approxTemp; sensorDataset.temperatureCelcius.setValid(true); @@ -171,7 +171,7 @@ ReturnValue_t Max31865EiveHandler::interpretDeviceReply(DeviceCommandId_t id, << " | R[Ohm] " << rtdValue << " Ohms | Approx T[C]: " << approxTemp << std::endl; } } - return RETURN_OK; + return returnvalue::OK; } uint32_t Max31865EiveHandler::getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) { return 2000; } @@ -186,7 +186,7 @@ ReturnValue_t Max31865EiveHandler::initializeLocalDataPool(localpool::DataPool& localDataPoolMap.emplace(static_cast(PoolIds::FAULT_BYTE), new PoolEntry({0})); poolManager.subscribeForRegularPeriodicPacket( subdp::RegularHkPeriodicParams(sensorDataset.getSid(), false, 30.0)); - return RETURN_OK; + return returnvalue::OK; } void Max31865EiveHandler::setDeviceInfo(uint8_t idx_, std::string location_) { diff --git a/mission/devices/Max31865PT1000Handler.cpp b/mission/devices/Max31865PT1000Handler.cpp index a5f6aa4f..30e19d0e 100644 --- a/mission/devices/Max31865PT1000Handler.cpp +++ b/mission/devices/Max31865PT1000Handler.cpp @@ -148,7 +148,7 @@ ReturnValue_t Max31865PT1000Handler::buildTransitionDeviceCommand(DeviceCommandI #else sif::printError("Max31865PT1000Handler: Invalid internal state\n"); #endif - return HasReturnvaluesIF::RETURN_FAILED; + return returnvalue::FAILED; } } @@ -163,7 +163,7 @@ ReturnValue_t Max31865PT1000Handler::buildCommandFromCommand(DeviceCommandId_t d currentCfg = commandData[0]; DeviceHandlerBase::rawPacketLen = 2; DeviceHandlerBase::rawPacket = commandBuffer.data(); - return HasReturnvaluesIF::RETURN_OK; + return returnvalue::OK; } else { return DeviceHandlerIF::NO_COMMAND_DATA; } @@ -173,14 +173,14 @@ ReturnValue_t Max31865PT1000Handler::buildCommandFromCommand(DeviceCommandId_t d commandBuffer[1] = currentCfg | MAX31865::CLEAR_FAULT_BIT_VAL; DeviceHandlerBase::rawPacketLen = 2; DeviceHandlerBase::rawPacket = commandBuffer.data(); - return HasReturnvaluesIF::RETURN_OK; + return returnvalue::OK; } case (MAX31865::REQUEST_CONFIG): { commandBuffer[0] = static_cast(MAX31865::REQUEST_CONFIG); commandBuffer[1] = 0x00; // dummy byte DeviceHandlerBase::rawPacketLen = 2; DeviceHandlerBase::rawPacket = commandBuffer.data(); - return HasReturnvaluesIF::RETURN_OK; + return returnvalue::OK; } case (MAX31865::WRITE_HIGH_THRESHOLD): { commandBuffer[0] = static_cast(MAX31865::WRITE_HIGH_THRESHOLD); @@ -188,7 +188,7 @@ ReturnValue_t Max31865PT1000Handler::buildCommandFromCommand(DeviceCommandId_t d commandBuffer[2] = static_cast(HIGH_THRESHOLD & 0xFF); DeviceHandlerBase::rawPacketLen = 3; DeviceHandlerBase::rawPacket = commandBuffer.data(); - return HasReturnvaluesIF::RETURN_OK; + return returnvalue::OK; } case (MAX31865::REQUEST_HIGH_THRESHOLD): { commandBuffer[0] = static_cast(MAX31865::REQUEST_HIGH_THRESHOLD); @@ -196,7 +196,7 @@ ReturnValue_t Max31865PT1000Handler::buildCommandFromCommand(DeviceCommandId_t d commandBuffer[2] = 0x00; // dummy byte DeviceHandlerBase::rawPacketLen = 3; DeviceHandlerBase::rawPacket = commandBuffer.data(); - return HasReturnvaluesIF::RETURN_OK; + return returnvalue::OK; } case (MAX31865::WRITE_LOW_THRESHOLD): { commandBuffer[0] = static_cast(MAX31865::WRITE_LOW_THRESHOLD); @@ -204,7 +204,7 @@ ReturnValue_t Max31865PT1000Handler::buildCommandFromCommand(DeviceCommandId_t d commandBuffer[2] = static_cast(LOW_THRESHOLD & 0xFF); DeviceHandlerBase::rawPacketLen = 3; DeviceHandlerBase::rawPacket = commandBuffer.data(); - return HasReturnvaluesIF::RETURN_OK; + return returnvalue::OK; } case (MAX31865::REQUEST_LOW_THRESHOLD): { commandBuffer[0] = static_cast(MAX31865::REQUEST_LOW_THRESHOLD); @@ -212,7 +212,7 @@ ReturnValue_t Max31865PT1000Handler::buildCommandFromCommand(DeviceCommandId_t d commandBuffer[2] = 0x00; // dummy byte DeviceHandlerBase::rawPacketLen = 3; DeviceHandlerBase::rawPacket = commandBuffer.data(); - return HasReturnvaluesIF::RETURN_OK; + return returnvalue::OK; } case (MAX31865::REQUEST_RTD): { commandBuffer[0] = static_cast(MAX31865::REQUEST_RTD); @@ -221,14 +221,14 @@ ReturnValue_t Max31865PT1000Handler::buildCommandFromCommand(DeviceCommandId_t d commandBuffer[2] = 0x00; DeviceHandlerBase::rawPacketLen = 3; DeviceHandlerBase::rawPacket = commandBuffer.data(); - return HasReturnvaluesIF::RETURN_OK; + return returnvalue::OK; } case (MAX31865::REQUEST_FAULT_BYTE): { commandBuffer[0] = static_cast(MAX31865::REQUEST_FAULT_BYTE); commandBuffer[1] = 0x00; DeviceHandlerBase::rawPacketLen = 2; DeviceHandlerBase::rawPacket = commandBuffer.data(); - return HasReturnvaluesIF::RETURN_OK; + return returnvalue::OK; } default: // Unknown DeviceCommand @@ -256,7 +256,7 @@ ReturnValue_t Max31865PT1000Handler::scanForReply(const uint8_t *start, size_t r if (remainingSize == rtdReplySize and internalState == InternalState::RUNNING) { *foundId = MAX31865::REQUEST_RTD; *foundLen = rtdReplySize; - return RETURN_OK; + return returnvalue::OK; } if (remainingSize == 3) { @@ -265,27 +265,27 @@ ReturnValue_t Max31865PT1000Handler::scanForReply(const uint8_t *start, size_t r *foundLen = 3; *foundId = MAX31865::WRITE_HIGH_THRESHOLD; commandExecuted = true; - return RETURN_OK; + return returnvalue::OK; } case (InternalState::REQUEST_HIGH_THRESHOLD): { *foundLen = 3; *foundId = MAX31865::REQUEST_HIGH_THRESHOLD; - return RETURN_OK; + return returnvalue::OK; } case (InternalState::CONFIG_LOW_THRESHOLD): { *foundLen = 3; *foundId = MAX31865::WRITE_LOW_THRESHOLD; commandExecuted = true; - return RETURN_OK; + return returnvalue::OK; } case (InternalState::REQUEST_LOW_THRESHOLD): { *foundLen = 3; *foundId = MAX31865::REQUEST_LOW_THRESHOLD; - return RETURN_OK; + return returnvalue::OK; } default: { sif::debug << "Max31865PT1000Handler::scanForReply: Unknown internal state" << std::endl; - return RETURN_OK; + return returnvalue::OK; } } } @@ -313,7 +313,7 @@ ReturnValue_t Max31865PT1000Handler::scanForReply(const uint8_t *start, size_t r } } - return RETURN_OK; + return returnvalue::OK; } ReturnValue_t Max31865PT1000Handler::interpretDeviceReply(DeviceCommandId_t id, @@ -332,7 +332,7 @@ ReturnValue_t Max31865PT1000Handler::interpretDeviceReply(DeviceCommandId_t id, #endif warningSwitch = false; } - return HasReturnvaluesIF::RETURN_OK; + return returnvalue::OK; } // set to true for invalid configs too for now. if (internalState == InternalState::REQUEST_CONFIG) { @@ -415,7 +415,7 @@ ReturnValue_t Max31865PT1000Handler::interpretDeviceReply(DeviceCommandId_t id, } PoolReadGuard pg(&sensorDataset); - if (pg.getReadResult() != HasReturnvaluesIF::RETURN_OK) { + if (pg.getReadResult() != returnvalue::OK) { // Configuration error #if FSFW_CPP_OSTREAM_ENABLED == 1 sif::warning << "Max31865PT1000Handler::interpretDeviceReply: Object ID: " << std::hex @@ -469,7 +469,7 @@ ReturnValue_t Max31865PT1000Handler::interpretDeviceReply(DeviceCommandId_t id, PoolReadGuard pg(&sensorDataset); auto result = pg.getReadResult(); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { // Configuration error #if FSFW_CPP_OSTREAM_ENABLED == 1 sif::warning << "Max31865PT1000Handler::interpretDeviceReply: Object ID: " << std::hex @@ -494,7 +494,7 @@ ReturnValue_t Max31865PT1000Handler::interpretDeviceReply(DeviceCommandId_t id, default: break; } - return HasReturnvaluesIF::RETURN_OK; + return returnvalue::OK; } uint32_t Max31865PT1000Handler::getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) { @@ -516,7 +516,7 @@ ReturnValue_t Max31865PT1000Handler::initializeLocalDataPool(localpool::DataPool localDataPoolMap.emplace(static_cast(PoolIds::FAULT_BYTE), new PoolEntry({0})); poolManager.subscribeForRegularPeriodicPacket( subdp::RegularHkPeriodicParams(sensorDataset.getSid(), false, 30.0)); - return HasReturnvaluesIF::RETURN_OK; + return returnvalue::OK; } void Max31865PT1000Handler::setInstantNormal(bool instantNormal) { diff --git a/mission/devices/P60DockHandler.cpp b/mission/devices/P60DockHandler.cpp index f059a1d7..4c00d09b 100644 --- a/mission/devices/P60DockHandler.cpp +++ b/mission/devices/P60DockHandler.cpp @@ -33,8 +33,8 @@ void P60DockHandler::parseHkTableReply(const uint8_t *packet) { uint16_t dataOffset = 0; PoolReadGuard pg0(&coreHk); PoolReadGuard pg1(&auxHk); - if (pg0.getReadResult() != HasReturnvaluesIF::RETURN_OK or - pg1.getReadResult() != HasReturnvaluesIF::RETURN_OK) { + if (pg0.getReadResult() != returnvalue::OK or + pg1.getReadResult() != returnvalue::OK) { coreHk.setValidity(false, true); auxHk.setValidity(false, true); return; @@ -229,37 +229,37 @@ ReturnValue_t P60DockHandler::initializeLocalDataPool(localpool::DataPool &local subdp::DiagnosticsHkPeriodicParams(coreHk.getSid(), false, 10.0)); poolManager.subscribeForRegularPeriodicPacket( subdp::RegularHkPeriodicParams(auxHk.getSid(), false, 30.0)); - return HasReturnvaluesIF::RETURN_OK; + return returnvalue::OK; } ReturnValue_t P60DockHandler::printStatus(DeviceCommandId_t cmd) { - ReturnValue_t result = RETURN_OK; + ReturnValue_t result = returnvalue::OK; switch (cmd) { case (GOMSPACE::PRINT_SWITCH_V_I): { PoolReadGuard pg0(&coreHk); PoolReadGuard pg1(&auxHk); - if (pg0.getReadResult() != HasReturnvaluesIF::RETURN_OK or - pg1.getReadResult() != HasReturnvaluesIF::RETURN_OK) { + if (pg0.getReadResult() != returnvalue::OK or + pg1.getReadResult() != returnvalue::OK) { break; } printHkTableSwitchIV(); - return HasReturnvaluesIF::RETURN_OK; + return returnvalue::OK; } case (GOMSPACE::PRINT_LATCHUPS): { PoolReadGuard pg(&auxHk); result = pg.getReadResult(); printHkTableLatchups(); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { break; } - return HasReturnvaluesIF::RETURN_OK; + return returnvalue::OK; } default: { return DeviceHandlerIF::COMMAND_NOT_SUPPORTED; } } sif::warning << "Reading P60 Dock HK table failed" << std::endl; - return HasReturnvaluesIF::RETURN_FAILED; + return returnvalue::FAILED; } void P60DockHandler::printHkTableSwitchIV() { diff --git a/mission/devices/PCDUHandler.cpp b/mission/devices/PCDUHandler.cpp index 70952cea..8dc78137 100644 --- a/mission/devices/PCDUHandler.cpp +++ b/mission/devices/PCDUHandler.cpp @@ -26,9 +26,9 @@ PCDUHandler::~PCDUHandler() {} ReturnValue_t PCDUHandler::performOperation(uint8_t counter) { if (counter == DeviceHandlerIF::PERFORM_OPERATION) { readCommandQueue(); - return RETURN_OK; + return returnvalue::OK; } - return RETURN_OK; + return returnvalue::OK; } ReturnValue_t PCDUHandler::initialize() { @@ -40,7 +40,7 @@ ReturnValue_t PCDUHandler::initialize() { } result = poolManager.initialize(commandQueue); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { return result; } @@ -49,12 +49,12 @@ ReturnValue_t PCDUHandler::initialize() { ObjectManager::instance()->get(objects::PDU2_HANDLER); if (pdu2Handler == nullptr) { sif::error << "PCDUHandler::initialize: Invalid pdu2Handler" << std::endl; - return RETURN_FAILED; + return returnvalue::FAILED; } result = pdu2Handler->getSubscriptionInterface()->subscribeForSetUpdateMessage( static_cast(P60System::SetIds::PDU_2_CORE), this->getObjectId(), commandQueue->getId(), true); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { sif::error << "PCDUHandler::initialize: Failed to subscribe for set update messages from " << "PDU2Handler" << std::endl; return result; @@ -65,18 +65,18 @@ ReturnValue_t PCDUHandler::initialize() { ObjectManager::instance()->get(objects::PDU1_HANDLER); if (pdu1Handler == nullptr) { sif::error << "PCDUHandler::initialize: Invalid pdu1Handler" << std::endl; - return RETURN_FAILED; + return returnvalue::FAILED; } result = pdu1Handler->getSubscriptionInterface()->subscribeForSetUpdateMessage( static_cast(P60System::SetIds::PDU_1_CORE), this->getObjectId(), commandQueue->getId(), true); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { sif::error << "PCDUHandler::initialize: Failed to subscribe for set update messages from " << "PDU1Handler" << std::endl; return result; } - return RETURN_OK; + return returnvalue::OK; } void PCDUHandler::initializeSwitchStates() { @@ -95,13 +95,13 @@ void PCDUHandler::initializeSwitchStates() { } void PCDUHandler::readCommandQueue() { - ReturnValue_t result = RETURN_OK; + ReturnValue_t result = returnvalue::OK; CommandMessage command; - for (result = commandQueue->receiveMessage(&command); result == RETURN_OK; + for (result = commandQueue->receiveMessage(&command); result == returnvalue::OK; result = commandQueue->receiveMessage(&command)) { result = poolManager.handleHousekeepingMessage(&command); - if (result == RETURN_OK) { + if (result == returnvalue::OK) { continue; } } @@ -131,18 +131,18 @@ void PCDUHandler::updateHkTableDataset(store_address_t storeId, LocalPoolDataSet const uint8_t* packet_ptr = nullptr; size_t size = 0; result = IPCStore->getData(storeId, &packet_ptr, &size); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { sif::error << "PCDUHandler::updateHkTableDataset: Failed to get data from IPCStore." << std::endl; } result = packetUpdate.deSerialize(&packet_ptr, &size, SerializeIF::Endianness::MACHINE); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { sif::error << "PCDUHandler::updateHkTableDataset: Failed to deserialize received packet " "in hk table dataset" << std::endl; } result = IPCStore->deleteData(storeId); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { sif::error << "PCDUHandler::updateHkTableDataset: Failed to delete data in IPCStore" << std::endl; } @@ -153,7 +153,7 @@ void PCDUHandler::updatePdu2SwitchStates() { using namespace PDU2; GOMSPACE::Pdu pdu = GOMSPACE::Pdu::PDU2; PoolReadGuard rg0(&switcherSet); - if (rg0.getReadResult() == RETURN_OK) { + if (rg0.getReadResult() == returnvalue::OK) { for (uint8_t idx = 0; idx < PDU::CHANNELS_LEN; idx++) { switcherSet.pdu2Switches[idx] = pdu2CoreHk.outputEnables[idx]; } @@ -189,7 +189,7 @@ void PCDUHandler::updatePdu1SwitchStates() { using namespace PDU1; PoolReadGuard rg0(&switcherSet); GOMSPACE::Pdu pdu = GOMSPACE::Pdu::PDU1; - if (rg0.getReadResult() == RETURN_OK) { + if (rg0.getReadResult() == returnvalue::OK) { for (uint8_t idx = 0; idx < PDU::CHANNELS_LEN; idx++) { switcherSet.pdu1Switches[idx] = pdu1CoreHk.outputEnables[idx]; } @@ -277,7 +277,7 @@ ReturnValue_t PCDUHandler::sendSwitchCommand(uint8_t switchNr, ReturnValue_t onO } // This is a dangerous command. Reject/Igore it for now case pcdu::PDU2_CH0_Q7S: { - return RETURN_FAILED; + return returnvalue::FAILED; // memoryAddress = PDU2::CONFIG_ADDRESS_OUT_EN_Q7S; // pdu = ObjectManager::instance()->get(objects::PDU2_HANDLER); // break; @@ -325,7 +325,7 @@ ReturnValue_t PCDUHandler::sendSwitchCommand(uint8_t switchNr, ReturnValue_t onO default: { sif::error << "PCDUHandler::sendSwitchCommand: Invalid switch number " << std::endl; - return RETURN_FAILED; + return returnvalue::FAILED; } } @@ -338,7 +338,7 @@ ReturnValue_t PCDUHandler::sendSwitchCommand(uint8_t switchNr, ReturnValue_t onO break; default: sif::error << "PCDUHandler::sendSwitchCommand: Invalid state commanded" << std::endl; - return RETURN_FAILED; + return returnvalue::FAILED; } GomspaceSetParamMessage setParamMessage(memoryAddress, ¶meterValue, parameterValueSize); @@ -356,7 +356,7 @@ ReturnValue_t PCDUHandler::sendSwitchCommand(uint8_t switchNr, ReturnValue_t onO ActionMessage::setCommand(&message, GOMSPACE::PARAM_SET, storeAddress); result = commandQueue->sendMessage(pdu->getCommandQueue(), &message, 0); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { sif::debug << "PCDUHandler::sendSwitchCommand: Failed to send message to PDU Handler" << std::endl; } else { @@ -366,12 +366,12 @@ ReturnValue_t PCDUHandler::sendSwitchCommand(uint8_t switchNr, ReturnValue_t onO return result; } -ReturnValue_t PCDUHandler::sendFuseOnCommand(uint8_t fuseNr) { return RETURN_OK; } +ReturnValue_t PCDUHandler::sendFuseOnCommand(uint8_t fuseNr) { return returnvalue::OK; } ReturnValue_t PCDUHandler::getSwitchState(uint8_t switchNr) const { if (switchNr >= pcdu::NUMBER_OF_SWITCHES) { sif::debug << "PCDUHandler::getSwitchState: Invalid switch number" << std::endl; - return RETURN_FAILED; + return returnvalue::FAILED; } pwrMutex->lockMutex(); uint8_t currentState = switchStates[switchNr]; @@ -383,7 +383,7 @@ ReturnValue_t PCDUHandler::getSwitchState(uint8_t switchNr) const { } } -ReturnValue_t PCDUHandler::getFuseState(uint8_t fuseNr) const { return RETURN_OK; } +ReturnValue_t PCDUHandler::getFuseState(uint8_t fuseNr) const { return returnvalue::OK; } uint32_t PCDUHandler::getSwitchDelayMs(void) const { return 20000; } @@ -396,7 +396,7 @@ ReturnValue_t PCDUHandler::initializeLocalDataPool(localpool::DataPool& localDat localDataPoolMap.emplace(PoolIds::PDU2_SWITCHES, &pdu2Switches); poolManager.subscribeForRegularPeriodicPacket( subdp::RegularHkPeriodicParams(switcherSet.getSid(), false, 5.0)); - return HasReturnvaluesIF::RETURN_OK; + return returnvalue::OK; } ReturnValue_t PCDUHandler::initializeAfterTaskCreation() { @@ -407,7 +407,7 @@ ReturnValue_t PCDUHandler::initializeAfterTaskCreation() { initializeSwitchStates(); - return HasReturnvaluesIF::RETURN_OK; + return returnvalue::OK; } uint32_t PCDUHandler::getPeriodicOperationFrequency() const { return pstIntervalMs; } diff --git a/mission/devices/PDU1Handler.cpp b/mission/devices/PDU1Handler.cpp index 9c2f845c..04ed10ca 100644 --- a/mission/devices/PDU1Handler.cpp +++ b/mission/devices/PDU1Handler.cpp @@ -34,7 +34,7 @@ ReturnValue_t PDU1Handler::setParamCallback(SetParamMessageUnpacker &unpacker, using namespace PDU1; GOMSPACE::Pdu pdu = GOMSPACE::Pdu::PDU1; if (not afterExecution) { - return HasReturnvaluesIF::RETURN_OK; + return returnvalue::OK; } if (channelSwitchHook != nullptr and unpacker.getParameterSize() == 1) { switch (unpacker.getAddress()) { @@ -76,7 +76,7 @@ ReturnValue_t PDU1Handler::setParamCallback(SetParamMessageUnpacker &unpacker, } } } - return HasReturnvaluesIF::RETURN_OK; + return returnvalue::OK; } void PDU1Handler::parseHkTableReply(const uint8_t *packet) { @@ -90,7 +90,7 @@ ReturnValue_t PDU1Handler::initializeLocalDataPool(localpool::DataPool &localDat subdp::DiagnosticsHkPeriodicParams(coreHk.getSid(), false, 10.0)); poolManager.subscribeForRegularPeriodicPacket( subdp::RegularHkPeriodicParams(auxHk.getSid(), false, 30.0)); - return HasReturnvaluesIF::RETURN_OK; + return returnvalue::OK; } LocalPoolDataSetBase *PDU1Handler::getDataSetHandle(sid_t sid) { @@ -103,12 +103,12 @@ LocalPoolDataSetBase *PDU1Handler::getDataSetHandle(sid_t sid) { } ReturnValue_t PDU1Handler::printStatus(DeviceCommandId_t cmd) { - ReturnValue_t result = RETURN_OK; + ReturnValue_t result = returnvalue::OK; switch (cmd) { case (GOMSPACE::PRINT_SWITCH_V_I): { PoolReadGuard pg(&coreHk); result = pg.getReadResult(); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { break; } printHkTableSwitchVI(); @@ -117,7 +117,7 @@ ReturnValue_t PDU1Handler::printStatus(DeviceCommandId_t cmd) { case (GOMSPACE::PRINT_LATCHUPS): { PoolReadGuard pg(&auxHk); result = pg.getReadResult(); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { break; } printHkTableLatchups(); @@ -127,7 +127,7 @@ ReturnValue_t PDU1Handler::printStatus(DeviceCommandId_t cmd) { return DeviceHandlerIF::COMMAND_NOT_SUPPORTED; } } - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { sif::warning << "Reading PDU1 HK table failed!" << std::endl; } return result; diff --git a/mission/devices/PDU2Handler.cpp b/mission/devices/PDU2Handler.cpp index 073ccb23..2a593237 100644 --- a/mission/devices/PDU2Handler.cpp +++ b/mission/devices/PDU2Handler.cpp @@ -53,16 +53,16 @@ ReturnValue_t PDU2Handler::initializeLocalDataPool(localpool::DataPool &localDat subdp::DiagnosticsHkPeriodicParams(coreHk.getSid(), false, 10.0)); poolManager.subscribeForRegularPeriodicPacket( subdp::RegularHkPeriodicParams(auxHk.getSid(), false, 30.0)); - return HasReturnvaluesIF::RETURN_OK; + return returnvalue::OK; } ReturnValue_t PDU2Handler::printStatus(DeviceCommandId_t cmd) { - ReturnValue_t result = RETURN_OK; + ReturnValue_t result = returnvalue::OK; switch (cmd) { case (GOMSPACE::PRINT_SWITCH_V_I): { PoolReadGuard pg(&coreHk); result = pg.getReadResult(); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { break; } printHkTableSwitchVI(); @@ -71,7 +71,7 @@ ReturnValue_t PDU2Handler::printStatus(DeviceCommandId_t cmd) { case (GOMSPACE::PRINT_LATCHUPS): { PoolReadGuard pg(&auxHk); result = pg.getReadResult(); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { break; } printHkTableLatchups(); @@ -81,7 +81,7 @@ ReturnValue_t PDU2Handler::printStatus(DeviceCommandId_t cmd) { return DeviceHandlerIF::COMMAND_NOT_SUPPORTED; } } - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { sif::warning << "Reading PDU1 HK table failed!" << std::endl; } return result; @@ -134,7 +134,7 @@ ReturnValue_t PDU2Handler::setParamCallback(SetParamMessageUnpacker &unpacker, using namespace PDU2; GOMSPACE::Pdu pdu = GOMSPACE::Pdu::PDU2; if (not afterExecution) { - return HasReturnvaluesIF::RETURN_OK; + return returnvalue::OK; } if (channelSwitchHook != nullptr and unpacker.getParameterSize() == 1) { switch (unpacker.getAddress()) { @@ -176,5 +176,5 @@ ReturnValue_t PDU2Handler::setParamCallback(SetParamMessageUnpacker &unpacker, } } } - return HasReturnvaluesIF::RETURN_OK; + return returnvalue::OK; } diff --git a/mission/devices/PayloadPcduHandler.cpp b/mission/devices/PayloadPcduHandler.cpp index d45b2c95..e643a945 100644 --- a/mission/devices/PayloadPcduHandler.cpp +++ b/mission/devices/PayloadPcduHandler.cpp @@ -75,7 +75,7 @@ ReturnValue_t PayloadPcduHandler::stateMachineToNormal(Mode_t modeFrom, Submode_ sif::error << "PayloadPcduHandler::stateMachineToNormal: Unexpected state PL_PCDU_OFF" << "detected" << std::endl; setMode(MODE_OFF); - return HasReturnvaluesIF::RETURN_FAILED; + return returnvalue::FAILED; } if (state == States::POWER_CHANNELS_ON) { #if OBSW_VERBOSE_LEVEL >= 1 @@ -146,7 +146,7 @@ ReturnValue_t PayloadPcduHandler::stateMachineToNormal(Mode_t modeFrom, Submode_ if (doFinish) { setMode(MODE_NORMAL, submode); } - return RETURN_OK; + return returnvalue::OK; } ReturnValue_t PayloadPcduHandler::buildNormalDeviceCommand(DeviceCommandId_t* id) { @@ -226,7 +226,7 @@ ReturnValue_t PayloadPcduHandler::buildCommandFromCommand(DeviceCommandId_t devi return DeviceHandlerIF::COMMAND_NOT_IMPLEMENTED; } } - return RETURN_OK; + return returnvalue::OK; } ReturnValue_t PayloadPcduHandler::scanForReply(const uint8_t* start, size_t remainingSize, @@ -234,7 +234,7 @@ ReturnValue_t PayloadPcduHandler::scanForReply(const uint8_t* start, size_t rema // SPI is full duplex *foundId = getPendingCommand(); *foundLen = remainingSize; - return HasReturnvaluesIF::RETURN_OK; + return returnvalue::OK; } ReturnValue_t PayloadPcduHandler::interpretDeviceReply(DeviceCommandId_t id, @@ -255,7 +255,7 @@ ReturnValue_t PayloadPcduHandler::interpretDeviceReply(DeviceCommandId_t id, } case (READ_CMD): { PoolReadGuard pg(&adcSet); - if (pg.getReadResult() != HasReturnvaluesIF::RETURN_OK) { + if (pg.getReadResult() != returnvalue::OK) { return pg.getReadResult(); } handleExtConvRead(packet); @@ -266,7 +266,7 @@ ReturnValue_t PayloadPcduHandler::interpretDeviceReply(DeviceCommandId_t id, } case (READ_WITH_TEMP_EXT): { PoolReadGuard pg(&adcSet); - if (pg.getReadResult() != HasReturnvaluesIF::RETURN_OK) { + if (pg.getReadResult() != returnvalue::OK) { return pg.getReadResult(); } handleExtConvRead(packet); @@ -282,7 +282,7 @@ ReturnValue_t PayloadPcduHandler::interpretDeviceReply(DeviceCommandId_t id, break; } } - return HasReturnvaluesIF::RETURN_OK; + return returnvalue::OK; } uint32_t PayloadPcduHandler::getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) { @@ -297,7 +297,7 @@ ReturnValue_t PayloadPcduHandler::initializeLocalDataPool(localpool::DataPool& l localDataPoolMap.emplace(plpcdu::PlPcduPoolIds::TEMP, &tempC); poolManager.subscribeForDiagPeriodicPacket( subdp::DiagnosticsHkPeriodicParams(adcSet.getSid(), false, 5.0)); - return HasReturnvaluesIF::RETURN_OK; + return returnvalue::OK; } void PayloadPcduHandler::setToGoToNormalModeImmediately(bool enable) { @@ -563,7 +563,7 @@ ReturnValue_t PayloadPcduHandler::isModeCombinationValid(Mode_t mode, Submode_t (1 << DRO_ON) | (1 << SOLID_STATE_RELAYS_ADC_ON))))) { return TRANS_NOT_ALLOWED; } - return HasReturnvaluesIF::RETURN_OK; + return returnvalue::OK; } return DeviceHandlerBase::isModeCombinationValid(mode, submode); } @@ -635,7 +635,7 @@ ReturnValue_t PayloadPcduHandler::getParameter(uint8_t domainId, uint8_t uniqueI startAtIndex); } } - return HasReturnvaluesIF::RETURN_OK; + return returnvalue::OK; } void PayloadPcduHandler::handleFailureInjection(std::string output, Event event) { @@ -655,7 +655,7 @@ ReturnValue_t PayloadPcduHandler::handleDoubleParamUpdate(std::string key, const ParameterWrapper* newValues) { double newValue = 0.0; ReturnValue_t result = newValues->getElement(&newValue, 0, 0); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { return result; } params.setValue(key, newValue); @@ -673,7 +673,7 @@ ReturnValue_t PayloadPcduHandler::extConvAsTwoCallback(SpiComIF* comIf, SpiCooki if (handler == nullptr) { sif::error << "GyroADIS16507Handler::spiSendCallback: Passed handler pointer is invalid!" << std::endl; - return HasReturnvaluesIF::RETURN_FAILED; + return returnvalue::FAILED; } DeviceCommandId_t currentCommand = handler->getPendingCommand(); switch (currentCommand) { @@ -687,18 +687,18 @@ ReturnValue_t PayloadPcduHandler::extConvAsTwoCallback(SpiComIF* comIf, SpiCooki return comIf->performRegularSendOperation(cookie, sendData, sendLen); } } - return HasReturnvaluesIF::RETURN_OK; + return returnvalue::OK; } ReturnValue_t PayloadPcduHandler::transferAsTwo(SpiComIF* comIf, SpiCookie* cookie, const uint8_t* sendData, size_t sendLen, bool tempOnly) { - ReturnValue_t result = HasReturnvaluesIF::RETURN_OK; + ReturnValue_t result = returnvalue::OK; int retval = 0; // Prepare transfer int fileDescriptor = 0; UnixFileGuard fileHelper(comIf->getSpiDev(), &fileDescriptor, O_RDWR, "SpiComIF::sendMessage"); - if (fileHelper.getOpenResult() != HasReturnvaluesIF::RETURN_OK) { + if (fileHelper.getOpenResult() != returnvalue::OK) { return SpiComIF::OPENING_FILE_FAILED; } spi::SpiModes spiMode = spi::SpiModes::MODE_0; @@ -722,14 +722,14 @@ ReturnValue_t PayloadPcduHandler::transferAsTwo(SpiComIF* comIf, SpiCookie* cook sif::warning << "GyroADIS16507Handler::spiSendCallback: " "Mutex or GPIO interface invalid" << std::endl; - return HasReturnvaluesIF::RETURN_FAILED; + return returnvalue::FAILED; #endif } if (gpioId != gpio::NO_GPIO) { cookie->getMutexParams(timeoutType, timeoutMs); result = mutex->lockMutex(timeoutType, timeoutMs); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { #if FSFW_CPP_OSTREAM_ENABLED == 1 sif::error << "SpiComIF::sendMessage: Failed to lock mutex" << std::endl; #endif @@ -792,7 +792,7 @@ ReturnValue_t PayloadPcduHandler::transferAsTwo(SpiComIF* comIf, SpiCookie* cook if (gpioId != gpio::NO_GPIO) { mutex->unlockMutex(); } - return HasReturnvaluesIF::RETURN_OK; + return returnvalue::OK; } #endif diff --git a/mission/devices/RadiationSensorHandler.cpp b/mission/devices/RadiationSensorHandler.cpp index 716b54a5..bc11bb45 100644 --- a/mission/devices/RadiationSensorHandler.cpp +++ b/mission/devices/RadiationSensorHandler.cpp @@ -43,7 +43,7 @@ ReturnValue_t RadiationSensorHandler::buildNormalDeviceCommand(DeviceCommandId_t default: { sif::debug << "RadiationSensorHandler::buildNormalDeviceCommand: Unknown communication " << "step" << std::endl; - return HasReturnvaluesIF::RETURN_OK; + return returnvalue::OK; } } return buildCommandFromCommand(*id, nullptr, 0); @@ -67,11 +67,11 @@ ReturnValue_t RadiationSensorHandler::buildCommandFromCommand(DeviceCommandId_t rawPacket = cmdBuffer; rawPacketLen = 1; internalState = InternalState::CONFIGURED; - return RETURN_OK; + return returnvalue::OK; } case (RAD_SENSOR::START_CONVERSION): { ReturnValue_t result = gpioIF->pullHigh(gpioIds::ENABLE_RADFET); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { #if OBSW_VERBOSE_LEVEL >= 1 sif::warning << "RadiationSensorHandler::buildCommandFromCommand; Pulling RADFET Enale pin " "high failed" @@ -83,29 +83,29 @@ ReturnValue_t RadiationSensorHandler::buildCommandFromCommand(DeviceCommandId_t cmdBuffer[1] = RAD_SENSOR::CONVERSION_DEFINITION; rawPacket = cmdBuffer; rawPacketLen = 2; - return RETURN_OK; + return returnvalue::OK; } case (RAD_SENSOR::READ_CONVERSIONS): { cmdBuffer[0] = RAD_SENSOR::DUMMY_BYTE; std::memset(cmdBuffer, RAD_SENSOR::DUMMY_BYTE, RAD_SENSOR::READ_SIZE); rawPacket = cmdBuffer; rawPacketLen = RAD_SENSOR::READ_SIZE; - return RETURN_OK; + return returnvalue::OK; } case RAD_SENSOR::ENABLE_DEBUG_OUTPUT: { printPeriodicData = true; rawPacketLen = 0; - return RETURN_OK; + return returnvalue::OK; } case RAD_SENSOR::DISABLE_DEBUG_OUTPUT: { rawPacketLen = 0; printPeriodicData = false; - return RETURN_OK; + return returnvalue::OK; } default: return DeviceHandlerIF::COMMAND_NOT_IMPLEMENTED; } - return HasReturnvaluesIF::RETURN_FAILED; + return returnvalue::FAILED; } void RadiationSensorHandler::fillCommandAndReplyMap() { @@ -128,7 +128,7 @@ ReturnValue_t RadiationSensorHandler::scanForReply(const uint8_t *start, size_t return IGNORE_REPLY_DATA; case RAD_SENSOR::READ_CONVERSIONS: { ReturnValue_t result = gpioIF->pullLow(gpioIds::ENABLE_RADFET); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { #if OBSW_VERBOSE_LEVEL >= 1 sif::warning << "RadiationSensorHandler::scanForReply; Pulling RADFET Enale pin " "low failed" @@ -147,7 +147,7 @@ ReturnValue_t RadiationSensorHandler::scanForReply(const uint8_t *start, size_t *foundLen = remainingSize; - return HasReturnvaluesIF::RETURN_OK; + return returnvalue::OK; } ReturnValue_t RadiationSensorHandler::interpretDeviceReply(DeviceCommandId_t id, @@ -188,7 +188,7 @@ ReturnValue_t RadiationSensorHandler::interpretDeviceReply(DeviceCommandId_t id, return DeviceHandlerIF::UNKNOWN_DEVICE_REPLY; } } - return HasReturnvaluesIF::RETURN_OK; + return returnvalue::OK; } uint32_t RadiationSensorHandler::getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) { @@ -206,7 +206,7 @@ ReturnValue_t RadiationSensorHandler::initializeLocalDataPool(localpool::DataPoo localDataPoolMap.emplace(RAD_SENSOR::AIN7, new PoolEntry({0})); poolManager.subscribeForRegularPeriodicPacket( subdp::RegularHkPeriodicParams(dataset.getSid(), false, 20.0)); - return HasReturnvaluesIF::RETURN_OK; + return returnvalue::OK; } void RadiationSensorHandler::setToGoToNormalModeImmediately() { this->goToNormalMode = true; } diff --git a/mission/devices/RwHandler.cpp b/mission/devices/RwHandler.cpp index 44dbac0d..b7e26123 100644 --- a/mission/devices/RwHandler.cpp +++ b/mission/devices/RwHandler.cpp @@ -27,14 +27,14 @@ RwHandler::~RwHandler() {} void RwHandler::doStartUp() { internalState = InternalState::GET_RESET_STATUS; - if (gpioComIF->pullHigh(enableGpio) != RETURN_OK) { + if (gpioComIF->pullHigh(enableGpio) != returnvalue::OK) { sif::debug << "RwHandler::doStartUp: Failed to pull enable gpio to high"; } setMode(_MODE_TO_ON); } void RwHandler::doShutDown() { - if (gpioComIF->pullLow(enableGpio) != RETURN_OK) { + if (gpioComIF->pullLow(enableGpio) != returnvalue::OK) { sif::debug << "RwHandler::doStartUp: Failed to pull enable gpio to low"; } setMode(_MODE_POWER_DOWN); @@ -73,28 +73,28 @@ ReturnValue_t RwHandler::buildTransitionDeviceCommand(DeviceCommandId_t* id) { ReturnValue_t RwHandler::buildCommandFromCommand(DeviceCommandId_t deviceCommand, const uint8_t* commandData, size_t commandDataLen) { - ReturnValue_t result = RETURN_OK; + ReturnValue_t result = returnvalue::OK; switch (deviceCommand) { case (RwDefinitions::RESET_MCU): { prepareSimpleCommand(deviceCommand); - return RETURN_OK; + return returnvalue::OK; } case (RwDefinitions::GET_LAST_RESET_STATUS): { prepareSimpleCommand(deviceCommand); - return RETURN_OK; + return returnvalue::OK; } case (RwDefinitions::CLEAR_LAST_RESET_STATUS): { prepareSimpleCommand(deviceCommand); - return RETURN_OK; + return returnvalue::OK; } case (RwDefinitions::GET_RW_STATUS): { prepareSimpleCommand(deviceCommand); - return RETURN_OK; + return returnvalue::OK; } case (RwDefinitions::INIT_RW_CONTROLLER): { prepareSimpleCommand(deviceCommand); - return RETURN_OK; + return returnvalue::OK; } case (RwDefinitions::SET_SPEED): { if (commandDataLen != 6) { @@ -103,7 +103,7 @@ ReturnValue_t RwHandler::buildCommandFromCommand(DeviceCommandId_t deviceCommand return SET_SPEED_COMMAND_INVALID_LENGTH; } result = checkSpeedAndRampTime(commandData, commandDataLen); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return result; } prepareSetSpeedCmd(commandData, commandDataLen); @@ -111,16 +111,16 @@ ReturnValue_t RwHandler::buildCommandFromCommand(DeviceCommandId_t deviceCommand } case (RwDefinitions::GET_TEMPERATURE): { prepareSimpleCommand(deviceCommand); - return RETURN_OK; + return returnvalue::OK; } case (RwDefinitions::GET_TM): { prepareSimpleCommand(deviceCommand); - return RETURN_OK; + return returnvalue::OK; } default: return DeviceHandlerIF::COMMAND_NOT_IMPLEMENTED; } - return HasReturnvaluesIF::RETURN_FAILED; + return returnvalue::FAILED; } void RwHandler::fillCommandAndReplyMap() { @@ -183,13 +183,13 @@ ReturnValue_t RwHandler::scanForReply(const uint8_t* start, size_t remainingSize default: { sif::warning << "RwHandler::scanForReply: Reply contains invalid command code" << std::endl; *foundLen = remainingSize; - return RETURN_FAILED; + return returnvalue::FAILED; } } sizeOfReply = *foundLen; - return RETURN_OK; + return returnvalue::OK; } ReturnValue_t RwHandler::interpretDeviceReply(DeviceCommandId_t id, const uint8_t* packet) { @@ -236,7 +236,7 @@ ReturnValue_t RwHandler::interpretDeviceReply(DeviceCommandId_t id, const uint8_ } } - return RETURN_OK; + return returnvalue::OK; } uint32_t RwHandler::getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) { return 5000; } @@ -283,7 +283,7 @@ ReturnValue_t RwHandler::initializeLocalDataPool(localpool::DataPool& localDataP subdp::RegularHkPeriodicParams(tmDataset.getSid(), false, 30.0)); poolManager.subscribeForRegularPeriodicPacket( subdp::RegularHkPeriodicParams(lastResetStatusSet.getSid(), false, 30.0)); - return RETURN_OK; + return returnvalue::OK; } void RwHandler::prepareSimpleCommand(DeviceCommandId_t id) { @@ -311,7 +311,7 @@ ReturnValue_t RwHandler::checkSpeedAndRampTime(const uint8_t* commandData, size_ return INVALID_RAMP_TIME; } - return RETURN_OK; + return returnvalue::OK; } void RwHandler::prepareSetSpeedCmd(const uint8_t* commandData, size_t commandDataLen) { diff --git a/mission/devices/RwHandler.h b/mission/devices/RwHandler.h index ed5e957c..008d5746 100644 --- a/mission/devices/RwHandler.h +++ b/mission/devices/RwHandler.h @@ -109,7 +109,7 @@ class RwHandler : public DeviceHandlerBase { /** * @brief This function checks if the receiced speed and ramp time to set are in a valid * range. - * @return RETURN_OK if successful, otherwise error code. + * @return returnvalue::OK if successful, otherwise error code. */ ReturnValue_t checkSpeedAndRampTime(const uint8_t* commandData, size_t commandDataLen); diff --git a/mission/devices/SolarArrayDeploymentHandler.cpp b/mission/devices/SolarArrayDeploymentHandler.cpp index 2b24ba07..c4e9a43b 100644 --- a/mission/devices/SolarArrayDeploymentHandler.cpp +++ b/mission/devices/SolarArrayDeploymentHandler.cpp @@ -28,14 +28,14 @@ SolarArrayDeploymentHandler::~SolarArrayDeploymentHandler() {} ReturnValue_t SolarArrayDeploymentHandler::performOperation(uint8_t operationCode) { if (operationCode == DeviceHandlerIF::PERFORM_OPERATION) { handleStateMachine(); - return RETURN_OK; + return returnvalue::OK; } - return RETURN_OK; + return returnvalue::OK; } ReturnValue_t SolarArrayDeploymentHandler::initialize() { ReturnValue_t result = SystemObject::initialize(); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return ObjectManagerIF::CHILD_INIT_FAILED; } @@ -46,7 +46,7 @@ ReturnValue_t SolarArrayDeploymentHandler::initialize() { } result = gpioInterface->addGpios(dynamic_cast(gpioCookie)); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { sif::error << "SolarArrayDeploymentHandler::initialize: Failed to initialize Gpio interface" << std::endl; return ObjectManagerIF::CHILD_INIT_FAILED; @@ -63,11 +63,11 @@ ReturnValue_t SolarArrayDeploymentHandler::initialize() { } result = actionHelper.initialize(commandQueue); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return ObjectManagerIF::CHILD_INIT_FAILED; } - return RETURN_OK; + return returnvalue::OK; } void SolarArrayDeploymentHandler::handleStateMachine() { @@ -119,9 +119,9 @@ void SolarArrayDeploymentHandler::performWaitOn8VActions() { } void SolarArrayDeploymentHandler::switchDeploymentTransistors() { - ReturnValue_t result = RETURN_OK; + ReturnValue_t result = returnvalue::OK; result = gpioInterface->pullHigh(deplSA1); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { sif::debug << "SolarArrayDeploymentHandler::handleStateMachine: Failed to pull solar" " array deployment switch 1 high " << std::endl; @@ -133,7 +133,7 @@ void SolarArrayDeploymentHandler::switchDeploymentTransistors() { mainLineSwitcher->sendSwitchCommand(mainLineSwitch, PowerSwitchIF::SWITCH_OFF); } result = gpioInterface->pullHigh(deplSA2); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { sif::debug << "SolarArrayDeploymentHandler::handleStateMachine: Failed to pull solar" " array deployment switch 2 high " << std::endl; @@ -147,17 +147,17 @@ void SolarArrayDeploymentHandler::switchDeploymentTransistors() { } void SolarArrayDeploymentHandler::handleDeploymentFinish() { - ReturnValue_t result = RETURN_OK; + ReturnValue_t result = returnvalue::OK; if (deploymentCountdown.hasTimedOut()) { - actionHelper.finish(true, rememberCommanderId, DEPLOY_SOLAR_ARRAYS, RETURN_OK); + actionHelper.finish(true, rememberCommanderId, DEPLOY_SOLAR_ARRAYS, returnvalue::OK); result = gpioInterface->pullLow(deplSA1); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { sif::debug << "SolarArrayDeploymentHandler::handleStateMachine: Failed to pull solar" " array deployment switch 1 low " << std::endl; } result = gpioInterface->pullLow(deplSA2); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { sif::debug << "SolarArrayDeploymentHandler::handleStateMachine: Failed to pull solar" " array deployment switch 2 low " << std::endl; @@ -171,12 +171,12 @@ void SolarArrayDeploymentHandler::handleDeploymentFinish() { void SolarArrayDeploymentHandler::readCommandQueue() { CommandMessage command; ReturnValue_t result = commandQueue->receiveMessage(&command); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return; } result = actionHelper.handleActionMessage(&command); - if (result == RETURN_OK) { + if (result == returnvalue::OK) { return; } } @@ -197,7 +197,7 @@ ReturnValue_t SolarArrayDeploymentHandler::executeAction(ActionId_t actionId, } else { stateMachine = SWITCH_8V_ON; rememberCommanderId = commandedBy; - result = RETURN_OK; + result = returnvalue::OK; } return result; } diff --git a/mission/devices/SolarArrayDeploymentHandler.h b/mission/devices/SolarArrayDeploymentHandler.h index d66a0509..691bdf77 100644 --- a/mission/devices/SolarArrayDeploymentHandler.h +++ b/mission/devices/SolarArrayDeploymentHandler.h @@ -7,7 +7,7 @@ #include #include #include -#include +#include #include #include #include @@ -21,7 +21,6 @@ */ class SolarArrayDeploymentHandler : public ExecutableObjectIF, public SystemObject, - public HasReturnvaluesIF, public HasActionsIF { public: static const DeviceCommandId_t DEPLOY_SOLAR_ARRAYS = 0x5; diff --git a/mission/devices/SusHandler.cpp b/mission/devices/SusHandler.cpp index 102613c7..73a8344f 100644 --- a/mission/devices/SusHandler.cpp +++ b/mission/devices/SusHandler.cpp @@ -127,7 +127,7 @@ ReturnValue_t SusHandler::buildCommandFromCommand(DeviceCommandId_t deviceComman default: return DeviceHandlerIF::COMMAND_NOT_IMPLEMENTED; } - return HasReturnvaluesIF::RETURN_OK; + return returnvalue::OK; } void SusHandler::fillCommandAndReplyMap() { @@ -144,7 +144,7 @@ ReturnValue_t SusHandler::scanForReply(const uint8_t *start, size_t remainingSiz DeviceCommandId_t *foundId, size_t *foundLen) { *foundId = this->getPendingCommand(); *foundLen = remainingSize; - return HasReturnvaluesIF::RETURN_OK; + return returnvalue::OK; } ReturnValue_t SusHandler::interpretDeviceReply(DeviceCommandId_t id, const uint8_t *packet) { @@ -153,10 +153,10 @@ ReturnValue_t SusHandler::interpretDeviceReply(DeviceCommandId_t id, const uint8 if (mode == _MODE_START_UP) { commandExecuted = true; } - return HasReturnvaluesIF::RETURN_OK; + return returnvalue::OK; } case SUS::START_INT_TIMED_CONVERSIONS: { - return HasReturnvaluesIF::RETURN_OK; + return returnvalue::OK; } case SUS::READ_INT_TIMED_CONVERSIONS: { PoolReadGuard readSet(&dataset); @@ -193,7 +193,7 @@ ReturnValue_t SusHandler::interpretDeviceReply(DeviceCommandId_t id, const uint8 return DeviceHandlerIF::UNKNOWN_DEVICE_REPLY; } } - return HasReturnvaluesIF::RETURN_OK; + return returnvalue::OK; } uint32_t SusHandler::getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) { return 3000; } @@ -204,7 +204,7 @@ ReturnValue_t SusHandler::initializeLocalDataPool(localpool::DataPool &localData localDataPoolMap.emplace(SUS::CHANNEL_VEC, &channelVec); poolManager.subscribeForDiagPeriodicPacket( subdp::DiagnosticsHkPeriodicParams(dataset.getSid(), false, 5.0)); - return HasReturnvaluesIF::RETURN_OK; + return returnvalue::OK; } void SusHandler::setToGoToNormalMode(bool enable) { this->goToNormalModeImmediately = enable; } diff --git a/mission/devices/SyrlinksHkHandler.cpp b/mission/devices/SyrlinksHkHandler.cpp index 67a015ec..1b4af19f 100644 --- a/mission/devices/SyrlinksHkHandler.cpp +++ b/mission/devices/SyrlinksHkHandler.cpp @@ -99,86 +99,86 @@ ReturnValue_t SyrlinksHkHandler::buildCommandFromCommand(DeviceCommandId_t devic switch (deviceCommand) { case (syrlinks::RESET_UNIT): { prepareCommand(resetCommand, deviceCommand); - return RETURN_OK; + return returnvalue::OK; } case (syrlinks::SET_TX_MODE_STANDBY): { prepareCommand(setTxModeStandby, deviceCommand); - return RETURN_OK; + return returnvalue::OK; } case (syrlinks::SET_TX_MODE_MODULATION): { prepareCommand(setTxModeModulation, deviceCommand); - return RETURN_OK; + return returnvalue::OK; } case (syrlinks::SET_TX_MODE_CW): { prepareCommand(setTxModeCw, deviceCommand); - return RETURN_OK; + return returnvalue::OK; } case (syrlinks::WRITE_LCL_CONFIG): { prepareCommand(writeLclConfig, deviceCommand); - return RETURN_OK; + return returnvalue::OK; } case (syrlinks::READ_RX_STATUS_REGISTERS): { prepareCommand(readRxStatusRegCommand, deviceCommand); - return RETURN_OK; + return returnvalue::OK; } case (syrlinks::READ_LCL_CONFIG): { prepareCommand(readLclConfig, deviceCommand); - return RETURN_OK; + return returnvalue::OK; } case (syrlinks::READ_TX_STATUS): { prepareCommand(readTxStatus, deviceCommand); - return RETURN_OK; + return returnvalue::OK; } case (syrlinks::READ_TX_WAVEFORM): { prepareCommand(readTxWaveform, deviceCommand); - return RETURN_OK; + return returnvalue::OK; } case (syrlinks::READ_TX_AGC_VALUE_HIGH_BYTE): { prepareCommand(readTxAgcValueHighByte, deviceCommand); - return RETURN_OK; + return returnvalue::OK; } case (syrlinks::READ_TX_AGC_VALUE_LOW_BYTE): { prepareCommand(readTxAgcValueLowByte, deviceCommand); - return RETURN_OK; + return returnvalue::OK; } case (syrlinks::TEMP_POWER_AMPLIFIER_HIGH_BYTE): { prepareCommand(tempPowerAmpBoardHighByte, deviceCommand); - return RETURN_OK; + return returnvalue::OK; } case (syrlinks::TEMP_POWER_AMPLIFIER_LOW_BYTE): { prepareCommand(tempPowerAmpBoardLowByte, deviceCommand); - return RETURN_OK; + return returnvalue::OK; } case (syrlinks::TEMP_BASEBAND_BOARD_HIGH_BYTE): { prepareCommand(tempBasebandBoardHighByte, deviceCommand); - return RETURN_OK; + return returnvalue::OK; } case (syrlinks::TEMP_BASEBAND_BOARD_LOW_BYTE): { prepareCommand(tempBasebandBoardLowByte, deviceCommand); - return RETURN_OK; + return returnvalue::OK; } case (syrlinks::CONFIG_BPSK): { prepareCommand(configBPSK, deviceCommand); - return RETURN_OK; + return returnvalue::OK; } case (syrlinks::CONFIG_OQPSK): { prepareCommand(configOQPSK, deviceCommand); - return RETURN_OK; + return returnvalue::OK; } case (syrlinks::ENABLE_DEBUG): { debugMode = true; rawPacketLen = 0; - return RETURN_OK; + return returnvalue::OK; } case (syrlinks::DISABLE_DEBUG): { debugMode = false; rawPacketLen = 0; - return RETURN_OK; + return returnvalue::OK; } default: return DeviceHandlerIF::COMMAND_NOT_IMPLEMENTED; } - return HasReturnvaluesIF::RETURN_FAILED; + return returnvalue::FAILED; } void SyrlinksHkHandler::fillCommandAndReplyMap() { @@ -222,7 +222,7 @@ void SyrlinksHkHandler::fillCommandAndReplyMap() { ReturnValue_t SyrlinksHkHandler::scanForReply(const uint8_t* start, size_t remainingSize, DeviceCommandId_t* foundId, size_t* foundLen) { - ReturnValue_t result = RETURN_OK; + ReturnValue_t result = returnvalue::OK; if (*start != '<') { sif::warning << "SyrlinksHkHandler::scanForReply: Missing start frame character" << std::endl; @@ -257,7 +257,7 @@ ReturnValue_t SyrlinksHkHandler::getSwitches(const uint8_t** switches, uint8_t* } *numberOfSwitches = 1; *switches = &powerSwitch; - return RETURN_OK; + return returnvalue::OK; } ReturnValue_t SyrlinksHkHandler::interpretDeviceReply(DeviceCommandId_t id, const uint8_t* packet) { @@ -266,21 +266,21 @@ ReturnValue_t SyrlinksHkHandler::interpretDeviceReply(DeviceCommandId_t id, cons switch (id) { case (syrlinks::ACK_REPLY): { result = verifyReply(packet, syrlinks::ACK_SIZE); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { sif::warning << "SyrlinksHkHandler::interpretDeviceReply: Acknowledgment reply has " "invalid crc" << std::endl; return CRC_FAILURE; } result = handleAckReply(packet); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return result; } break; } case (syrlinks::READ_RX_STATUS_REGISTERS): { result = verifyReply(packet, syrlinks::RX_STATUS_REGISTERS_REPLY_SIZE); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { sif::warning << "SyrlinksHkHandler::interpretDeviceReply: Read rx status registers reply " << "has invalid crc" << std::endl; return CRC_FAILURE; @@ -290,7 +290,7 @@ ReturnValue_t SyrlinksHkHandler::interpretDeviceReply(DeviceCommandId_t id, cons } case (syrlinks::READ_LCL_CONFIG): { result = verifyReply(packet, syrlinks::READ_ONE_REGISTER_REPLY_SIE); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { sif::warning << "SyrlinksHkHandler::interpretDeviceReply: Read config lcl reply " << "has invalid crc" << std::endl; return CRC_FAILURE; @@ -300,7 +300,7 @@ ReturnValue_t SyrlinksHkHandler::interpretDeviceReply(DeviceCommandId_t id, cons } case (syrlinks::READ_TX_STATUS): { result = verifyReply(packet, syrlinks::READ_ONE_REGISTER_REPLY_SIE); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { sif::warning << "SyrlinksHkHandler::interpretDeviceReply: Read tx status reply " << "has invalid crc" << std::endl; return CRC_FAILURE; @@ -310,7 +310,7 @@ ReturnValue_t SyrlinksHkHandler::interpretDeviceReply(DeviceCommandId_t id, cons } case (syrlinks::READ_TX_WAVEFORM): { result = verifyReply(packet, syrlinks::READ_ONE_REGISTER_REPLY_SIE); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { sif::warning << "SyrlinksHkHandler::interpretDeviceReply: Read tx waveform reply " << "has invalid crc" << std::endl; return CRC_FAILURE; @@ -320,7 +320,7 @@ ReturnValue_t SyrlinksHkHandler::interpretDeviceReply(DeviceCommandId_t id, cons } case (syrlinks::READ_TX_AGC_VALUE_HIGH_BYTE): { result = verifyReply(packet, syrlinks::READ_ONE_REGISTER_REPLY_SIE); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { sif::warning << "SyrlinksHkHandler::interpretDeviceReply: Read tx AGC high byte reply " << "has invalid crc" << std::endl; return CRC_FAILURE; @@ -330,7 +330,7 @@ ReturnValue_t SyrlinksHkHandler::interpretDeviceReply(DeviceCommandId_t id, cons } case (syrlinks::READ_TX_AGC_VALUE_LOW_BYTE): { result = verifyReply(packet, syrlinks::READ_ONE_REGISTER_REPLY_SIE); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { sif::warning << "SyrlinksHkHandler::interpretDeviceReply: Read tx AGC low byte reply " << "has invalid crc" << std::endl; return CRC_FAILURE; @@ -340,7 +340,7 @@ ReturnValue_t SyrlinksHkHandler::interpretDeviceReply(DeviceCommandId_t id, cons } case (syrlinks::TEMP_BASEBAND_BOARD_HIGH_BYTE): { result = verifyReply(packet, syrlinks::READ_ONE_REGISTER_REPLY_SIE); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { sif::warning << "SyrlinksHkHandler::interpretDeviceReply: Read temperature baseband board " << "high byte reply has invalid crc" << std::endl; return CRC_FAILURE; @@ -352,7 +352,7 @@ ReturnValue_t SyrlinksHkHandler::interpretDeviceReply(DeviceCommandId_t id, cons } case (syrlinks::TEMP_BASEBAND_BOARD_LOW_BYTE): { result = verifyReply(packet, syrlinks::READ_ONE_REGISTER_REPLY_SIE); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { sif::warning << "SyrlinksHkHandler::interpretDeviceReply: Read temperature baseband board" " low byte reply has invalid crc" << std::endl; @@ -371,7 +371,7 @@ ReturnValue_t SyrlinksHkHandler::interpretDeviceReply(DeviceCommandId_t id, cons } case (syrlinks::TEMP_POWER_AMPLIFIER_HIGH_BYTE): { result = verifyReply(packet, syrlinks::READ_ONE_REGISTER_REPLY_SIE); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { sif::warning << "SyrlinksHkHandler::interpretDeviceReply: Read temperature power amplifier " << "board high byte reply has invalid crc" << std::endl; return CRC_FAILURE; @@ -384,7 +384,7 @@ ReturnValue_t SyrlinksHkHandler::interpretDeviceReply(DeviceCommandId_t id, cons } case (syrlinks::TEMP_POWER_AMPLIFIER_LOW_BYTE): { result = verifyReply(packet, syrlinks::READ_ONE_REGISTER_REPLY_SIE); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { sif::warning << "SyrlinksHkHandler::interpretDeviceReply: Read temperature power amplifier" << " board low byte reply has invalid crc" << std::endl; return CRC_FAILURE; @@ -406,7 +406,7 @@ ReturnValue_t SyrlinksHkHandler::interpretDeviceReply(DeviceCommandId_t id, cons } } - return RETURN_OK; + return returnvalue::OK; } LocalPoolDataSetBase* SyrlinksHkHandler::getDataSetHandle(sid_t sid) { @@ -469,7 +469,7 @@ uint32_t SyrlinksHkHandler::convertHexStringToUint32(const char* characters, ReturnValue_t SyrlinksHkHandler::parseReplyStatus(const char* status) { switch (*status) { case '0': - return RETURN_OK; + return returnvalue::OK; case '1': sif::debug << "SyrlinksHkHandler::parseReplyStatus: Uart framing or parity error" << std::endl; @@ -494,7 +494,7 @@ ReturnValue_t SyrlinksHkHandler::parseReplyStatus(const char* status) { default: sif::debug << "SyrlinksHkHandler::parseReplyStatus: Status reply contains an invalid " << "status id" << std::endl; - return RETURN_FAILED; + return returnvalue::FAILED; } } @@ -513,9 +513,9 @@ ReturnValue_t SyrlinksHkHandler::verifyReply(const uint8_t* packet, uint8_t size result = recalculatedCrc.compare(replyCrc); if (result != 0) { - return RETURN_FAILED; + return returnvalue::FAILED; } - return RETURN_OK; + return returnvalue::OK; } void SyrlinksHkHandler::parseRxStatusRegistersReply(const uint8_t* packet) { @@ -627,7 +627,7 @@ ReturnValue_t SyrlinksHkHandler::initializeLocalDataPool(localpool::DataPool& lo subdp::DiagnosticsHkPeriodicParams(rxDataset.getSid(), false, 5.0)); poolManager.subscribeForRegularPeriodicPacket( subdp::RegularHkPeriodicParams(temperatureSet.getSid(), false, 10.0)); - return HasReturnvaluesIF::RETURN_OK; + return returnvalue::OK; } void SyrlinksHkHandler::setModeNormal() { mode = MODE_NORMAL; } @@ -637,9 +637,9 @@ float SyrlinksHkHandler::calcTempVal(uint16_t raw) { return 0.126984 * raw - 67. ReturnValue_t SyrlinksHkHandler::handleAckReply(const uint8_t* packet) { ReturnValue_t result = parseReplyStatus(reinterpret_cast(packet + syrlinks::MESSAGE_HEADER_SIZE)); - if (rememberCommandId == syrlinks::WRITE_LCL_CONFIG and result != RETURN_OK) { + if (rememberCommandId == syrlinks::WRITE_LCL_CONFIG and result != returnvalue::OK) { startupState = StartupState::OFF; - } else if (rememberCommandId == syrlinks::WRITE_LCL_CONFIG and result == RETURN_OK) { + } else if (rememberCommandId == syrlinks::WRITE_LCL_CONFIG and result == returnvalue::OK) { startupState = StartupState::DONE; } return result; diff --git a/mission/devices/SyrlinksHkHandler.h b/mission/devices/SyrlinksHkHandler.h index e21cfd3a..44b154f6 100644 --- a/mission/devices/SyrlinksHkHandler.h +++ b/mission/devices/SyrlinksHkHandler.h @@ -172,7 +172,7 @@ class SyrlinksHkHandler : public DeviceHandlerBase { * @param size Size of the whole packet including the crc and the packet termination * character '>'. * - * @return RETURN_OK if successful, otherwise RETURN_FAILED. + * @return returnvalue::OK if successful, otherwise returnvalue::FAILED. */ ReturnValue_t verifyReply(const uint8_t* packet, uint8_t size); diff --git a/mission/devices/Tmp1075Handler.cpp b/mission/devices/Tmp1075Handler.cpp index 995b4f81..83a4643b 100644 --- a/mission/devices/Tmp1075Handler.cpp +++ b/mission/devices/Tmp1075Handler.cpp @@ -29,11 +29,11 @@ ReturnValue_t Tmp1075Handler::buildNormalDeviceCommand(DeviceCommandId_t *id) { communicationStep = CommunicationStep::START_ADC_CONVERSION; return buildCommandFromCommand(*id, NULL, 0); } - return HasReturnvaluesIF::RETURN_OK; + return returnvalue::OK; } ReturnValue_t Tmp1075Handler::buildTransitionDeviceCommand(DeviceCommandId_t *id) { - return HasReturnvaluesIF::RETURN_OK; + return returnvalue::OK; } ReturnValue_t Tmp1075Handler::buildCommandFromCommand(DeviceCommandId_t deviceCommand, @@ -45,7 +45,7 @@ ReturnValue_t Tmp1075Handler::buildCommandFromCommand(DeviceCommandId_t deviceCo prepareAdcConversionCommand(); rawPacket = cmdBuffer; rawPacketLen = TMP1075::CFGR_CMD_SIZE; - return RETURN_OK; + return returnvalue::OK; } case (TMP1075::GET_TEMP): { std::memset(cmdBuffer, 0, sizeof(cmdBuffer)); @@ -53,12 +53,12 @@ ReturnValue_t Tmp1075Handler::buildCommandFromCommand(DeviceCommandId_t deviceCo rawPacket = cmdBuffer; rawPacketLen = TMP1075::POINTER_REG_SIZE; rememberCommandId = TMP1075::GET_TEMP; - return RETURN_OK; + return returnvalue::OK; } default: return DeviceHandlerIF::COMMAND_NOT_IMPLEMENTED; } - return HasReturnvaluesIF::RETURN_FAILED; + return returnvalue::FAILED; } void Tmp1075Handler::fillCommandAndReplyMap() { @@ -77,7 +77,7 @@ ReturnValue_t Tmp1075Handler::scanForReply(const uint8_t *start, size_t remainin default: return IGNORE_REPLY_DATA; } - return HasReturnvaluesIF::RETURN_OK; + return returnvalue::OK; } ReturnValue_t Tmp1075Handler::interpretDeviceReply(DeviceCommandId_t id, const uint8_t *packet) { @@ -91,7 +91,7 @@ ReturnValue_t Tmp1075Handler::interpretDeviceReply(DeviceCommandId_t id, const u << ": Temperature: " << tempValue << " °C" << std::endl; #endif ReturnValue_t result = dataset.read(); - if (result == HasReturnvaluesIF::RETURN_OK) { + if (result == returnvalue::OK) { dataset.temperatureCelcius = tempValue; dataset.setValidity(true, true); dataset.commit(); @@ -105,7 +105,7 @@ ReturnValue_t Tmp1075Handler::interpretDeviceReply(DeviceCommandId_t id, const u return DeviceHandlerIF::UNKNOWN_DEVICE_REPLY; } } - return HasReturnvaluesIF::RETURN_OK; + return returnvalue::OK; } void Tmp1075Handler::setNormalDatapoolEntriesInvalid() {} @@ -125,5 +125,5 @@ ReturnValue_t Tmp1075Handler::initializeLocalDataPool(localpool::DataPool &local localDataPoolMap.emplace(TMP1075::TEMPERATURE_C_TMP1075, new PoolEntry({0.0})); poolManager.subscribeForRegularPeriodicPacket( subdp::RegularHkPeriodicParams(dataset.getSid(), false, 30.0)); - return HasReturnvaluesIF::RETURN_OK; + return returnvalue::OK; } diff --git a/mission/devices/devicedefinitions/BpxBatteryDefinitions.h b/mission/devices/devicedefinitions/BpxBatteryDefinitions.h index 54d4e1f8..eb19529b 100644 --- a/mission/devices/devicedefinitions/BpxBatteryDefinitions.h +++ b/mission/devices/devicedefinitions/BpxBatteryDefinitions.h @@ -133,43 +133,43 @@ class BpxBatteryHk : public StaticLocalDataSet { size_t remSize = size; ReturnValue_t result = chargeCurrent.deSerialize(&data, &remSize, SerializeIF::Endianness::NETWORK); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { return result; } result = dischargeCurrent.deSerialize(&data, &remSize, SerializeIF::Endianness::NETWORK); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { return result; } result = heaterCurrent.deSerialize(&data, &remSize, SerializeIF::Endianness::NETWORK); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { return result; } result = battVoltage.deSerialize(&data, &remSize, SerializeIF::Endianness::NETWORK); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { return result; } result = battTemp1.deSerialize(&data, &remSize, SerializeIF::Endianness::NETWORK); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { return result; } result = battTemp2.deSerialize(&data, &remSize, SerializeIF::Endianness::NETWORK); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { return result; } result = battTemp3.deSerialize(&data, &remSize, SerializeIF::Endianness::NETWORK); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { return result; } result = battTemp4.deSerialize(&data, &remSize, SerializeIF::Endianness::NETWORK); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { return result; } result = rebootCounter.deSerialize(&data, &remSize, SerializeIF::Endianness::NETWORK); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { return result; } result = bootcause.deSerialize(&data, &remSize, SerializeIF::Endianness::NETWORK); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { return result; } return result; @@ -225,7 +225,7 @@ class BpxBatteryCfg : public StaticLocalDataSet { battheatermode.value = data[0]; battheaterLow.value = data[1]; battheaterHigh.value = data[2]; - return HasReturnvaluesIF::RETURN_OK; + return returnvalue::OK; } //! Mode for battheater [0=OFF,1=Auto] diff --git a/mission/devices/devicedefinitions/SpBase.h b/mission/devices/devicedefinitions/SpBase.h index ef1f70b8..2bcaa1dc 100644 --- a/mission/devices/devicedefinitions/SpBase.h +++ b/mission/devices/devicedefinitions/SpBase.h @@ -54,7 +54,7 @@ class SpTcBase { return SerializeIF::BUFFER_TOO_SHORT; } - return result::OK; + return returnvalue::OK; } ReturnValue_t serializeHeader() { @@ -65,7 +65,7 @@ class SpTcBase { ReturnValue_t checkSizeAndSerializeHeader() { ReturnValue_t result = checkPayloadLen(); - if (result != result::OK) { + if (result != returnvalue::OK) { return result; } return serializeHeader(); @@ -108,9 +108,9 @@ class SpTmReader : public SpacePacketReader { ReturnValue_t checkCrc() { if (CRC::crc16ccitt(getFullData(), getFullPacketLen()) != 0) { - return HasReturnvaluesIF::RETURN_FAILED; + return returnvalue::FAILED; } - return HasReturnvaluesIF::RETURN_OK; + return returnvalue::OK; } }; diff --git a/mission/devices/devicedefinitions/payloadPcduDefinitions.h b/mission/devices/devicedefinitions/payloadPcduDefinitions.h index ab387fc7..2730d1cb 100644 --- a/mission/devices/devicedefinitions/payloadPcduDefinitions.h +++ b/mission/devices/devicedefinitions/payloadPcduDefinitions.h @@ -195,7 +195,7 @@ class PlPcduParameter : public NVMParameterBase { ReturnValue_t initialize(std::string mountPrefix) { setFullName(mountPrefix + "/conf/plpcdu.json"); ReturnValue_t result = readJsonFile(); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { // File does not exist or reading JSON failed for various reason. Rewrite the JSON file #if OBSW_VERBOSE_LEVEL >= 1 sif::info << "Creating PL PCDU JSON file at " << getFullName() << std::endl; @@ -203,7 +203,7 @@ class PlPcduParameter : public NVMParameterBase { resetValues(); writeJsonFile(); } - return HasReturnvaluesIF::RETURN_OK; + return returnvalue::OK; } void resetValues() { insertValue(PARAM_KEY_MAP[SSR_TO_DRO_WAIT_TIME], DFT_SSR_TO_DRO_WAIT_TIME); diff --git a/mission/memory/NVMParameterBase.cpp b/mission/memory/NVMParameterBase.cpp index 78f40525..38fa45bb 100644 --- a/mission/memory/NVMParameterBase.cpp +++ b/mission/memory/NVMParameterBase.cpp @@ -15,9 +15,9 @@ ReturnValue_t NVMParameterBase::readJsonFile() { i >> json; } catch (nlohmann::json::exception& e) { sif::warning << "Reading JSON file failed with error " << e.what() << std::endl; - return HasReturnvaluesIF::RETURN_FAILED; + return returnvalue::FAILED; } - return HasReturnvaluesIF::RETURN_OK; + return returnvalue::OK; } return HasFileSystemIF::FILE_DOES_NOT_EXIST; } @@ -28,9 +28,9 @@ ReturnValue_t NVMParameterBase::writeJsonFile() { o << std::setw(4) << json << std::endl; } catch (nlohmann::json::exception& e) { sif::warning << "Writing JSON file failed with error " << e.what() << std::endl; - return HasReturnvaluesIF::RETURN_FAILED; + return returnvalue::FAILED; } - return HasReturnvaluesIF::RETURN_OK; + return returnvalue::OK; } void NVMParameterBase::setFullName(std::string fullName) { this->fullName = fullName; } diff --git a/mission/memory/NVMParameterBase.h b/mission/memory/NVMParameterBase.h index eb839905..4f644fcc 100644 --- a/mission/memory/NVMParameterBase.h +++ b/mission/memory/NVMParameterBase.h @@ -5,9 +5,9 @@ #include #include -#include "fsfw/returnvalues/HasReturnvaluesIF.h" +#include "fsfw/returnvalues/returnvalue.h" -class NVMParameterBase : public HasReturnvaluesIF { +class NVMParameterBase { public: virtual ~NVMParameterBase() {} @@ -16,7 +16,7 @@ class NVMParameterBase : public HasReturnvaluesIF { bool getJsonFileExists(); /** - * Returns RETURN_OK on successfull read and HasFileSystemIF::FILE_DOES_NOT_EXIST if + * Returns returnvalue::OK on successfull read and HasFileSystemIF::FILE_DOES_NOT_EXIST if * file does not exist yet. * @return */ @@ -57,13 +57,13 @@ inline ReturnValue_t NVMParameterBase::insertValue(std::string key, T value) { keys.push_back(key); } json[key] = value; - return HasReturnvaluesIF::RETURN_OK; + return returnvalue::OK; } template inline ReturnValue_t NVMParameterBase::setValue(std::string key, T value) { json[key] = value; - return HasReturnvaluesIF::RETURN_OK; + return returnvalue::OK; } template @@ -72,7 +72,7 @@ inline ReturnValue_t NVMParameterBase::getValue(std::string key, T& value) const return KEY_NOT_EXISTS; } value = json[key]; - return RETURN_OK; + return returnvalue::OK; } #endif /* BSP_Q7S_CORE_NVMPARAMS_NVMPARAMIF_H_ */ diff --git a/mission/system/AcsBoardAssembly.cpp b/mission/system/AcsBoardAssembly.cpp index 5e9076b2..6a59183c 100644 --- a/mission/system/AcsBoardAssembly.cpp +++ b/mission/system/AcsBoardAssembly.cpp @@ -35,7 +35,7 @@ AcsBoardAssembly::AcsBoardAssembly(object_id_t objectId, object_id_t parentId, ReturnValue_t AcsBoardAssembly::commandChildren(Mode_t mode, Submode_t submode) { using namespace duallane; - ReturnValue_t result = RETURN_OK; + ReturnValue_t result = returnvalue::OK; refreshHelperModes(); // Initialize the mode table to ensure all devices are in a defined state modeTable[ModeTableIdx::GYRO_0_A].setMode(MODE_OFF); @@ -75,14 +75,14 @@ ReturnValue_t AcsBoardAssembly::checkChildrenStateOn(Mode_t wantedMode, Submode_ helper.gpsMode != MODE_ON) { return NOT_ENOUGH_CHILDREN_IN_CORRECT_STATE; } - return RETURN_OK; + return returnvalue::OK; } else if (wantedSubmode == B_SIDE) { if ((helper.gyro2SideBMode != wantedMode and helper.gyro3SideBMode != wantedMode) or (helper.mgm2SideBMode != wantedMode and helper.mgm3SideBMode != wantedMode) or helper.gpsMode != MODE_ON) { return NOT_ENOUGH_CHILDREN_IN_CORRECT_STATE; } - return RETURN_OK; + return returnvalue::OK; } else if (wantedSubmode == DUAL_MODE) { if ((helper.gyro0SideAMode != wantedMode and helper.gyro1SideAMode != wantedMode and helper.gyro2AdisIdSideB != wantedMode and helper.gyro3SideBMode != wantedMode) or @@ -94,16 +94,16 @@ ReturnValue_t AcsBoardAssembly::checkChildrenStateOn(Mode_t wantedMode, Submode_ triggerEvent(NOT_ENOUGH_DEVICES_DUAL_MODE, 0, 0); dualModeErrorSwitch = false; } - return RETURN_OK; + return returnvalue::OK; } - return RETURN_OK; + return returnvalue::OK; } - return HasReturnvaluesIF::RETURN_OK; + return returnvalue::OK; } ReturnValue_t AcsBoardAssembly::handleNormalOrOnModeCmd(Mode_t mode, Submode_t submode) { using namespace duallane; - ReturnValue_t result = RETURN_OK; + ReturnValue_t result = returnvalue::OK; bool needsSecondStep = false; auto cmdSeq = [&](object_id_t objectId, Mode_t devMode, ModeTableIdx tableIdx) { if (mode == devMode) { @@ -190,7 +190,7 @@ ReturnValue_t AcsBoardAssembly::handleNormalOrOnModeCmd(Mode_t mode, Submode_t s cmdSeq(helper.gyro3L3gIdSideB, helper.gyro3SideBMode, ModeTableIdx::GYRO_3_B); cmdSeq(helper.mgm2Lis3IdSideB, helper.mgm2SideBMode, ModeTableIdx::MGM_2_B); cmdSeq(helper.mgm3Rm3100IdSideB, helper.mgm3SideBMode, ModeTableIdx::MGM_3_B); - ReturnValue_t status = RETURN_OK; + ReturnValue_t status = returnvalue::OK; if (gpsUsable) { gpioHandler(gpioIds::GNSS_0_NRESET, true, "AcsBoardAssembly::handleNormalOrOnModeCmd: Could not pull nReset pin" @@ -203,7 +203,7 @@ ReturnValue_t AcsBoardAssembly::handleNormalOrOnModeCmd(Mode_t mode, Submode_t s } else { status = gpioIF->pullHigh(gpioIds::GNSS_SELECT); } - if (status != HasReturnvaluesIF::RETURN_OK) { + if (status != returnvalue::OK) { #if OBSW_VERBOSE_LEVEL >= 1 sif::error << "AcsBoardAssembly::handleNormalOrOnModeCmd: Could not pull GNSS select to" "default side for dual mode" @@ -232,13 +232,13 @@ void AcsBoardAssembly::selectGpsInDualMode(duallane::Submodes side) { if (submode != Submodes::DUAL_MODE) { return; } - ReturnValue_t result = RETURN_OK; + ReturnValue_t result = returnvalue::OK; if (side == Submodes::A_SIDE) { result = gpioIF->pullLow(gpioIds::GNSS_SELECT); } else { result = gpioIF->pullHigh(gpioIds::GNSS_SELECT); } - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { #if OBSW_VERBOSE_LEVEL >= 1 sif::error << "AcsBoardAssembly::switchGpsInDualMode: Switching GPS failed" << std::endl; #endif @@ -246,13 +246,13 @@ void AcsBoardAssembly::selectGpsInDualMode(duallane::Submodes side) { } void AcsBoardAssembly::gpioHandler(gpioId_t gpio, bool high, std::string error) { - ReturnValue_t result = RETURN_OK; + ReturnValue_t result = returnvalue::OK; if (high) { result = gpioIF->pullHigh(gpio); } else { result = gpioIF->pullLow(gpio); } - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { #if OBSW_VERBOSE_LEVEL >= 1 sif::error << error << std::endl; #endif @@ -277,39 +277,39 @@ void AcsBoardAssembly::refreshHelperModes() { ReturnValue_t AcsBoardAssembly::initialize() { ReturnValue_t result = registerChild(helper.gyro0AdisIdSideA); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { return result; } result = registerChild(helper.gyro1L3gIdSideA); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { return result; } result = registerChild(helper.gyro2AdisIdSideB); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { return result; } result = registerChild(helper.gyro3L3gIdSideB); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { return result; } result = registerChild(helper.mgm0Lis3IdSideA); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { return result; } result = registerChild(helper.mgm1Rm3100IdSideA); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { return result; } result = registerChild(helper.mgm2Lis3IdSideB); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { return result; } result = registerChild(helper.mgm3Rm3100IdSideB); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { return result; } result = registerChild(helper.gpsId); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { return result; } return AssemblyBase::initialize(); diff --git a/mission/system/DualLaneAssemblyBase.cpp b/mission/system/DualLaneAssemblyBase.cpp index 020ce1ff..f9ca44fe 100644 --- a/mission/system/DualLaneAssemblyBase.cpp +++ b/mission/system/DualLaneAssemblyBase.cpp @@ -68,7 +68,7 @@ ReturnValue_t DualLaneAssemblyBase::pwrStateMachineWrapper() { OpCodes opCode = pwrStateMachine.fsm(); if (customRecoveryStates == RecoveryCustomStates::IDLE) { if (opCode == OpCodes::NONE) { - return RETURN_OK; + return returnvalue::OK; } else if (opCode == OpCodes::TO_OFF_DONE) { // Will be called for transitions to MODE_OFF, where everything is done after power switching finishModeOp(); @@ -85,17 +85,17 @@ ReturnValue_t DualLaneAssemblyBase::pwrStateMachineWrapper() { sif::warning << "Timeout occured in power state machine" << std::endl; #endif triggerEvent(pwrTimeoutEvent, 0, 0); - return RETURN_FAILED; + return returnvalue::FAILED; } } } - return RETURN_OK; + return returnvalue::OK; } ReturnValue_t DualLaneAssemblyBase::isModeCombinationValid(Mode_t mode, Submode_t submode) { using namespace duallane; if (submode != A_SIDE and submode != B_SIDE and submode != DUAL_MODE) { - return HasReturnvaluesIF::RETURN_FAILED; + return returnvalue::FAILED; } if (sideSwitchTransition(mode, submode)) { // I could implement this but this would increase the already high complexity. This is not @@ -104,7 +104,7 @@ ReturnValue_t DualLaneAssemblyBase::isModeCombinationValid(Mode_t mode, Submode_ triggerEvent(SIDE_SWITCH_TRANSITION_NOT_ALLOWED_ID, 0, 0); return TRANS_NOT_ALLOWED; } - return HasReturnvaluesIF::RETURN_OK; + return returnvalue::OK; } void DualLaneAssemblyBase::handleModeReached() { diff --git a/mission/system/DualLanePowerStateMachine.cpp b/mission/system/DualLanePowerStateMachine.cpp index 5669ef7d..2af2fc38 100644 --- a/mission/system/DualLanePowerStateMachine.cpp +++ b/mission/system/DualLanePowerStateMachine.cpp @@ -11,8 +11,8 @@ DualLanePowerStateMachine::DualLanePowerStateMachine(power::Switch_t switchA, power::OpCodes DualLanePowerStateMachine::fsm() { using namespace duallane; - ReturnValue_t switchStateA = RETURN_OK; - ReturnValue_t switchStateB = RETURN_OK; + ReturnValue_t switchStateA = returnvalue::OK; + ReturnValue_t switchStateB = returnvalue::OK; if (state == power::States::IDLE or state == power::States::MODE_COMMANDING) { return opResult; } diff --git a/mission/system/PowerStateMachineBase.h b/mission/system/PowerStateMachineBase.h index 3adeebb6..12e72b60 100644 --- a/mission/system/PowerStateMachineBase.h +++ b/mission/system/PowerStateMachineBase.h @@ -7,7 +7,7 @@ #include "definitions.h" -class PowerStateMachineBase : public HasReturnvaluesIF { +class PowerStateMachineBase { public: PowerStateMachineBase(PowerSwitchIF* pwrSwitcher, dur_millis_t checkTimeout); diff --git a/mission/system/RwAssembly.cpp b/mission/system/RwAssembly.cpp index 7149747e..6e393034 100644 --- a/mission/system/RwAssembly.cpp +++ b/mission/system/RwAssembly.cpp @@ -31,7 +31,7 @@ void RwAssembly::performChildOperation() { } ReturnValue_t RwAssembly::commandChildren(Mode_t mode, Submode_t submode) { - ReturnValue_t result = RETURN_OK; + ReturnValue_t result = returnvalue::OK; modeTransitionFailedSwitch = true; // Initialize the mode table to ensure all devices are in a defined state for (uint8_t idx = 0; idx < NUMBER_RWS; idx++) { @@ -67,12 +67,12 @@ ReturnValue_t RwAssembly::checkChildrenStateOn(Mode_t wantedMode, Submode_t want } return NOT_ENOUGH_CHILDREN_IN_CORRECT_STATE; } - return RETURN_OK; + return returnvalue::OK; } ReturnValue_t RwAssembly::isModeCombinationValid(Mode_t mode, Submode_t submode) { if (mode == MODE_ON or mode == MODE_OFF or mode == DeviceHandlerIF::MODE_NORMAL) { - return RETURN_OK; + return returnvalue::OK; } return HasModesIF::INVALID_MODE; } @@ -112,7 +112,7 @@ void RwAssembly::handleChildrenLostMode(ReturnValue_t result) { } ReturnValue_t RwAssembly::handleNormalOrOnModeCmd(Mode_t mode, Submode_t submode) { - ReturnValue_t result = RETURN_OK; + ReturnValue_t result = returnvalue::OK; bool needsSecondStep = false; Mode_t devMode = 0; object_id_t objId = 0; @@ -168,10 +168,10 @@ bool RwAssembly::isUseable(object_id_t object, Mode_t mode) { } ReturnValue_t RwAssembly::initialize() { - ReturnValue_t result = RETURN_OK; + ReturnValue_t result = returnvalue::OK; for (const auto& obj : helper.rwIds) { result = registerChild(obj); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { return result; } } diff --git a/mission/system/SusAssembly.cpp b/mission/system/SusAssembly.cpp index 56b0dba8..472a6580 100644 --- a/mission/system/SusAssembly.cpp +++ b/mission/system/SusAssembly.cpp @@ -18,7 +18,7 @@ SusAssembly::SusAssembly(object_id_t objectId, object_id_t parentId, PowerSwitch } ReturnValue_t SusAssembly::commandChildren(Mode_t mode, Submode_t submode) { - ReturnValue_t result = RETURN_OK; + ReturnValue_t result = returnvalue::OK; refreshHelperModes(); // Initialize the mode table to ensure all devices are in a defined state for (uint8_t idx = 0; idx < NUMBER_SUN_SENSORS; idx++) { @@ -38,7 +38,7 @@ ReturnValue_t SusAssembly::commandChildren(Mode_t mode, Submode_t submode) { ReturnValue_t SusAssembly::handleNormalOrOnModeCmd(Mode_t mode, Submode_t submode) { using namespace duallane; - ReturnValue_t result = RETURN_OK; + ReturnValue_t result = returnvalue::OK; bool needsSecondStep = false; auto cmdSeq = [&](object_id_t objectId, Mode_t devMode, uint8_t tableIdx) { if (mode == devMode) { @@ -104,28 +104,28 @@ ReturnValue_t SusAssembly::checkChildrenStateOn(Mode_t wantedMode, Submode_t wan return NOT_ENOUGH_CHILDREN_IN_CORRECT_STATE; } } - return RETURN_OK; + return returnvalue::OK; } else if (wantedSubmode == B_SIDE) { for (uint8_t idx = NUMBER_SUN_SENSORS_ONE_SIDE; idx < NUMBER_SUN_SENSORS; idx++) { if (helper.susModes[idx] != wantedMode) { return NOT_ENOUGH_CHILDREN_IN_CORRECT_STATE; } } - return RETURN_OK; + return returnvalue::OK; } else { // Trigger event if devices are faulty? This is the last fallback mode, returning // a failure here would trigger a transition to MODE_OFF unless handleModeTransitionFailed // is overriden - return RETURN_OK; + return returnvalue::OK; } - return RETURN_OK; + return returnvalue::OK; } ReturnValue_t SusAssembly::initialize() { - ReturnValue_t result = RETURN_OK; + ReturnValue_t result = returnvalue::OK; for (const auto& id : helper.susIds) { result = registerChild(id); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { return result; } } diff --git a/mission/system/TcsBoardAssembly.cpp b/mission/system/TcsBoardAssembly.cpp index f973335e..c753504b 100644 --- a/mission/system/TcsBoardAssembly.cpp +++ b/mission/system/TcsBoardAssembly.cpp @@ -36,7 +36,7 @@ void TcsBoardAssembly::performChildOperation() { } ReturnValue_t TcsBoardAssembly::commandChildren(Mode_t mode, Submode_t submode) { - ReturnValue_t result = RETURN_OK; + ReturnValue_t result = returnvalue::OK; // Initialize the mode table to ensure all devices are in a defined state for (uint8_t idx = 0; idx < NUMBER_RTDS; idx++) { modeTable[idx].setMode(MODE_OFF); @@ -80,21 +80,21 @@ ReturnValue_t TcsBoardAssembly::checkChildrenStateOn(Mode_t wantedMode, Submode_ warningSwitch = false; } } - return RETURN_OK; + return returnvalue::OK; } ReturnValue_t TcsBoardAssembly::isModeCombinationValid(Mode_t mode, Submode_t submode) { if (mode == MODE_ON or mode == MODE_OFF or mode == DeviceHandlerIF::MODE_NORMAL) { - return RETURN_OK; + return returnvalue::OK; } return HasModesIF::INVALID_MODE; } ReturnValue_t TcsBoardAssembly::initialize() { - ReturnValue_t result = RETURN_OK; + ReturnValue_t result = returnvalue::OK; for (const auto& obj : helper.rtdInfos) { result = registerChild(obj.first); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { return result; } } @@ -119,7 +119,7 @@ void TcsBoardAssembly::startTransition(Mode_t mode, Submode_t submode) { } ReturnValue_t TcsBoardAssembly::handleNormalOrOnModeCmd(Mode_t mode, Submode_t submode) { - ReturnValue_t result = RETURN_OK; + ReturnValue_t result = returnvalue::OK; bool needsSecondStep = false; Mode_t devMode = 0; object_id_t objId = 0; diff --git a/mission/system/fdir/GomspacePowerFdir.cpp b/mission/system/fdir/GomspacePowerFdir.cpp index f7475491..5d35ca24 100644 --- a/mission/system/fdir/GomspacePowerFdir.cpp +++ b/mission/system/fdir/GomspacePowerFdir.cpp @@ -14,9 +14,9 @@ GomspacePowerFdir::GomspacePowerFdir(object_id_t devId, object_id_t parentId) ReturnValue_t GomspacePowerFdir::eventReceived(EventMessage* event) { if (isFdirInActionOrAreWeFaulty(event)) { - return RETURN_OK; + return returnvalue::OK; } - ReturnValue_t result = RETURN_FAILED; + ReturnValue_t result = returnvalue::FAILED; switch (event->getEvent()) { case HasModesIF::MODE_TRANSITION_FAILED: case HasModesIF::OBJECT_IN_INVALID_MODE: @@ -42,7 +42,7 @@ ReturnValue_t GomspacePowerFdir::eventReceived(EventMessage* event) { // The two above should never be confirmed. case DeviceHandlerIF::DEVICE_MISSED_REPLY: result = sendConfirmationRequest(event); - if (result == HasReturnvaluesIF::RETURN_OK) { + if (result == returnvalue::OK) { break; } // else @@ -67,7 +67,7 @@ ReturnValue_t GomspacePowerFdir::eventReceived(EventMessage* event) { case PowerSwitchIF::SWITCH_WENT_OFF: if (powerConfirmation != MessageQueueIF::NO_QUEUE) { result = sendConfirmationRequest(event, powerConfirmation); - if (result == RETURN_OK) { + if (result == returnvalue::OK) { setFdirState(DEVICE_MIGHT_BE_OFF); } } @@ -102,9 +102,9 @@ ReturnValue_t GomspacePowerFdir::eventReceived(EventMessage* event) { // break; default: // We don't know the event, someone else should handle it. - return RETURN_FAILED; + return returnvalue::FAILED; } - return RETURN_OK; + return returnvalue::OK; } void GomspacePowerFdir::eventConfirmed(EventMessage* event) { diff --git a/mission/system/fdir/SyrlinksFdir.cpp b/mission/system/fdir/SyrlinksFdir.cpp index c35427db..524a94f6 100644 --- a/mission/system/fdir/SyrlinksFdir.cpp +++ b/mission/system/fdir/SyrlinksFdir.cpp @@ -14,9 +14,9 @@ SyrlinksFdir::SyrlinksFdir(object_id_t syrlinksId) ReturnValue_t SyrlinksFdir::eventReceived(EventMessage* event) { if (isFdirInActionOrAreWeFaulty(event)) { - return RETURN_OK; + return returnvalue::OK; } - ReturnValue_t result = RETURN_FAILED; + ReturnValue_t result = returnvalue::FAILED; switch (event->getEvent()) { case HasModesIF::MODE_TRANSITION_FAILED: case HasModesIF::OBJECT_IN_INVALID_MODE: @@ -42,7 +42,7 @@ ReturnValue_t SyrlinksFdir::eventReceived(EventMessage* event) { // The two above should never be confirmed. case DeviceHandlerIF::DEVICE_MISSED_REPLY: result = sendConfirmationRequest(event); - if (result == HasReturnvaluesIF::RETURN_OK) { + if (result == returnvalue::OK) { break; } // else @@ -67,7 +67,7 @@ ReturnValue_t SyrlinksFdir::eventReceived(EventMessage* event) { case PowerSwitchIF::SWITCH_WENT_OFF: if (powerConfirmation != MessageQueueIF::NO_QUEUE) { result = sendConfirmationRequest(event, powerConfirmation); - if (result == RETURN_OK) { + if (result == returnvalue::OK) { setFdirState(DEVICE_MIGHT_BE_OFF); } } @@ -102,9 +102,9 @@ ReturnValue_t SyrlinksFdir::eventReceived(EventMessage* event) { // break; default: // We don't know the event, someone else should handle it. - return RETURN_FAILED; + return returnvalue::FAILED; } - return RETURN_OK; + return returnvalue::OK; } void SyrlinksFdir::eventConfirmed(EventMessage* event) { diff --git a/mission/tmtc/CCSDSHandler.cpp b/mission/tmtc/CCSDSHandler.cpp index 43cdc2bc..06499ff6 100644 --- a/mission/tmtc/CCSDSHandler.cpp +++ b/mission/tmtc/CCSDSHandler.cpp @@ -38,7 +38,7 @@ ReturnValue_t CCSDSHandler::performOperation(uint8_t operationCode) { handleTelemetry(); handleTelecommands(); checkTxTimer(); - return RETURN_OK; + return returnvalue::OK; } void CCSDSHandler::handleTelemetry() { @@ -51,7 +51,7 @@ void CCSDSHandler::handleTelemetry() { void CCSDSHandler::handleTelecommands() {} ReturnValue_t CCSDSHandler::initialize() { - ReturnValue_t result = RETURN_OK; + ReturnValue_t result = returnvalue::OK; PtmeIF* ptme = ObjectManager::instance()->get(ptmeId); if (ptme == nullptr) { sif::warning << "Invalid PTME object" << std::endl; @@ -70,19 +70,19 @@ ReturnValue_t CCSDSHandler::initialize() { tcDistributorQueueId = tcDistributor->getRequestQueue(); result = parameterHelper.initialize(); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { return result; } result = actionHelper.initialize(commandQueue); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return result; } VirtualChannelMapIter iter; for (iter = virtualChannelMap.begin(); iter != virtualChannelMap.end(); iter++) { result = iter->second->initialize(); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return result; } iter->second->setPtmeObject(ptme); @@ -96,7 +96,7 @@ ReturnValue_t CCSDSHandler::initialize() { return ObjectManagerIF::CHILD_INIT_FAILED; } result = manager->registerListener(eventQueue->getId()); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { #if FSFW_CPP_OSTREAM_ENABLED == 1 sif::warning << "CCSDSHandler::initialize: Failed to register CCSDS handler as event " "listener" @@ -108,7 +108,7 @@ ReturnValue_t CCSDSHandler::initialize() { result = manager->subscribeToEventRange(eventQueue->getId(), event::getEventId(PdecHandler::CARRIER_LOCK), event::getEventId(PdecHandler::BIT_LOCK_PDEC)); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { #if FSFW_CPP_OSTREAM_ENABLED == 1 sif::error << "CCSDSHandler::initialize: Failed to subscribe to events from PDEC " "handler" @@ -117,11 +117,11 @@ ReturnValue_t CCSDSHandler::initialize() { return result; } result = ptmeConfig->initialize(); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return ObjectManagerIF::CHILD_INIT_FAILED; } - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { return ObjectManagerIF::CHILD_INIT_FAILED; } @@ -138,16 +138,16 @@ ReturnValue_t CCSDSHandler::initialize() { void CCSDSHandler::readCommandQueue(void) { CommandMessage commandMessage; - ReturnValue_t result = RETURN_FAILED; + ReturnValue_t result = returnvalue::FAILED; result = commandQueue->receiveMessage(&commandMessage); - if (result == RETURN_OK) { + if (result == returnvalue::OK) { result = parameterHelper.handleParameterMessage(&commandMessage); - if (result == RETURN_OK) { + if (result == returnvalue::OK) { return; } result = actionHelper.handleActionMessage(&commandMessage); - if (result == RETURN_OK) { + if (result == returnvalue::OK) { return; } CommandMessage reply; @@ -200,7 +200,7 @@ MessageQueueId_t CCSDSHandler::getReportReceptionQueue(uint8_t virtualChannel) { ReturnValue_t CCSDSHandler::getParameter(uint8_t domainId, uint8_t uniqueIdentifier, ParameterWrapper* parameterWrapper, const ParameterWrapper* newValues, uint16_t startAtIndex) { - return RETURN_OK; + return returnvalue::OK; } uint16_t CCSDSHandler::getIdentifier() { return 0; } @@ -212,7 +212,7 @@ MessageQueueId_t CCSDSHandler::getRequestQueue() { ReturnValue_t CCSDSHandler::executeAction(ActionId_t actionId, MessageQueueId_t commandedBy, const uint8_t* data, size_t size) { - ReturnValue_t result = RETURN_OK; + ReturnValue_t result = returnvalue::OK; switch (actionId) { case SET_LOW_RATE: { result = ptmeConfig->setRate(RATE_100KBPS); @@ -255,7 +255,7 @@ ReturnValue_t CCSDSHandler::executeAction(ActionId_t actionId, MessageQueueId_t default: return COMMAND_NOT_IMPLEMENTED; } - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return result; } return EXECUTION_FINISHED; @@ -263,7 +263,7 @@ ReturnValue_t CCSDSHandler::executeAction(ActionId_t actionId, MessageQueueId_t void CCSDSHandler::checkEvents() { EventMessage event; - for (ReturnValue_t result = eventQueue->receiveMessage(&event); result == RETURN_OK; + for (ReturnValue_t result = eventQueue->receiveMessage(&event); result == returnvalue::OK; result = eventQueue->receiveMessage(&event)) { switch (event.getMessageId()) { case EventMessage::EVENT_MESSAGE: diff --git a/mission/tmtc/CCSDSHandler.h b/mission/tmtc/CCSDSHandler.h index c8afa69a..31a546b5 100644 --- a/mission/tmtc/CCSDSHandler.h +++ b/mission/tmtc/CCSDSHandler.h @@ -10,7 +10,7 @@ #include "fsfw/events/EventMessage.h" #include "fsfw/objectmanager/SystemObject.h" #include "fsfw/parameters/ParameterHelper.h" -#include "fsfw/returnvalues/HasReturnvaluesIF.h" +#include "fsfw/returnvalues/returnvalue.h" #include "fsfw/tasks/ExecutableObjectIF.h" #include "fsfw/timemanager/Countdown.h" #include "fsfw/tmtcservices/AcceptsTelecommandsIF.h" @@ -32,7 +32,6 @@ class CCSDSHandler : public SystemObject, public ExecutableObjectIF, public AcceptsTelemetryIF, public AcceptsTelecommandsIF, - public HasReturnvaluesIF, public ReceivesParameterMessagesIF, public HasActionsIF { public: diff --git a/mission/tmtc/TmFunnel.cpp b/mission/tmtc/TmFunnel.cpp index c23da16d..f3543404 100644 --- a/mission/tmtc/TmFunnel.cpp +++ b/mission/tmtc/TmFunnel.cpp @@ -32,16 +32,16 @@ MessageQueueId_t TmFunnel::getReportReceptionQueue(uint8_t virtualChannel) { ReturnValue_t TmFunnel::performOperation(uint8_t operationCode) { TmTcMessage currentMessage; ReturnValue_t status = tmQueue->receiveMessage(¤tMessage); - while (status == HasReturnvaluesIF::RETURN_OK) { + while (status == returnvalue::OK) { status = handlePacket(¤tMessage); - if (status != HasReturnvaluesIF::RETURN_OK) { + if (status != returnvalue::OK) { break; } status = tmQueue->receiveMessage(¤tMessage); } if (status == MessageQueueIF::EMPTY) { - return HasReturnvaluesIF::RETURN_OK; + return returnvalue::OK; } else { return status; } @@ -51,13 +51,13 @@ ReturnValue_t TmFunnel::handlePacket(TmTcMessage* message) { uint8_t* packetData = nullptr; size_t size = 0; ReturnValue_t result = tmStore->modifyData(message->getStorageId(), &packetData, &size); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { return result; } PusTmZeroCopyWriter packet(timeReader, packetData, size); result = packet.parseDataWithoutCrcCheck(); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { return result; } packet.setSequenceCount(sourceSequenceCount++); @@ -65,7 +65,7 @@ ReturnValue_t TmFunnel::handlePacket(TmTcMessage* message) { packet.updateErrorControl(); result = tmQueue->sendToDefault(message); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { tmStore->deleteData(message->getStorageId()); sif::error << "TmFunnel::handlePacket: Error sending to downlink " "handler" @@ -75,7 +75,7 @@ ReturnValue_t TmFunnel::handlePacket(TmTcMessage* message) { if (storageDestination != objects::NO_OBJECT) { result = storageQueue->sendToDefault(message); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { tmStore->deleteData(message->getStorageId()); sif::error << "TmFunnel::handlePacket: Error sending to storage " "handler" diff --git a/mission/tmtc/VirtualChannel.cpp b/mission/tmtc/VirtualChannel.cpp index 9d413ca7..46ba3fe3 100644 --- a/mission/tmtc/VirtualChannel.cpp +++ b/mission/tmtc/VirtualChannel.cpp @@ -18,21 +18,21 @@ ReturnValue_t VirtualChannel::initialize() { tmStore = ObjectManager::instance()->get(objects::TM_STORE); if (tmStore == nullptr) { sif::error << "VirtualChannel::initialize: Failed to get tm store" << std::endl; - return RETURN_FAILED; + return returnvalue::FAILED; } - return RETURN_OK; + return returnvalue::OK; } ReturnValue_t VirtualChannel::performOperation() { - ReturnValue_t result = RETURN_OK; + ReturnValue_t result = returnvalue::OK; TmTcMessage message; - while (tmQueue->receiveMessage(&message) == RETURN_OK) { + while (tmQueue->receiveMessage(&message) == returnvalue::OK) { store_address_t storeId = message.getStorageId(); const uint8_t* data = nullptr; size_t size = 0; result = tmStore->getData(storeId, &data, &size); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { sif::warning << "VirtualChannel::performOperation: Failed to read data from IPC store" << std::endl; tmStore->deleteData(storeId); @@ -45,7 +45,7 @@ ReturnValue_t VirtualChannel::performOperation() { tmStore->deleteData(storeId); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { return result; } } diff --git a/mission/tmtc/VirtualChannel.h b/mission/tmtc/VirtualChannel.h index 229bcc40..a47a3d59 100644 --- a/mission/tmtc/VirtualChannel.h +++ b/mission/tmtc/VirtualChannel.h @@ -5,7 +5,7 @@ #include #include "OBSWConfig.h" -#include "fsfw/returnvalues/HasReturnvaluesIF.h" +#include "fsfw/returnvalues/returnvalue.h" #include "fsfw/tmtcservices/AcceptsTelemetryIF.h" class StorageManagerIF; @@ -16,7 +16,7 @@ class StorageManagerIF; * * @author J. Meier */ -class VirtualChannel : public AcceptsTelemetryIF, public HasReturnvaluesIF { +class VirtualChannel : public AcceptsTelemetryIF { public: /** * @brief Constructor diff --git a/mission/utility/Timestamp.cpp b/mission/utility/Timestamp.cpp index 095bd28c..030be5c6 100644 --- a/mission/utility/Timestamp.cpp +++ b/mission/utility/Timestamp.cpp @@ -4,7 +4,7 @@ Timestamp::Timestamp() { ReturnValue_t result = Clock::getDateAndTime(&time); - if (result != RETURN_OK) { + if (result != returnvalue::OK) { sif::warning << "Timestamp::Timestamp: Failed to get time" << std::endl; } } diff --git a/mission/utility/Timestamp.h b/mission/utility/Timestamp.h index 4f783437..a83ef34c 100644 --- a/mission/utility/Timestamp.h +++ b/mission/utility/Timestamp.h @@ -5,7 +5,7 @@ #include #include -#include "fsfw/returnvalues/HasReturnvaluesIF.h" +#include "fsfw/returnvalues/returnvalue.h" #include "fsfw/timemanager/Clock.h" /** @@ -13,7 +13,7 @@ * * @author J. Meier */ -class Timestamp : public HasReturnvaluesIF { +class Timestamp { public: Timestamp(); virtual ~Timestamp(); diff --git a/test/gpio/DummyGpioIF.cpp b/test/gpio/DummyGpioIF.cpp index 3938f711..f9c28bf2 100644 --- a/test/gpio/DummyGpioIF.cpp +++ b/test/gpio/DummyGpioIF.cpp @@ -4,13 +4,13 @@ DummyGpioIF::DummyGpioIF() {} DummyGpioIF::~DummyGpioIF() {} -ReturnValue_t DummyGpioIF::addGpios(GpioCookie* cookie) { return RETURN_OK; } +ReturnValue_t DummyGpioIF::addGpios(GpioCookie* cookie) { return returnvalue::OK; } -ReturnValue_t DummyGpioIF::pullHigh(gpioId_t gpioId) { return RETURN_OK; } +ReturnValue_t DummyGpioIF::pullHigh(gpioId_t gpioId) { return returnvalue::OK; } -ReturnValue_t DummyGpioIF::pullLow(gpioId_t gpioId) { return RETURN_OK; } +ReturnValue_t DummyGpioIF::pullLow(gpioId_t gpioId) { return returnvalue::OK; } ReturnValue_t DummyGpioIF::readGpio(gpioId_t gpioId, gpio::Levels& gpioState) { gpioState = gpio::Levels::LOW; - return RETURN_OK; + return returnvalue::OK; } diff --git a/test/testtasks/PusTcInjector.cpp b/test/testtasks/PusTcInjector.cpp index 82b1cb1c..c0bff957 100644 --- a/test/testtasks/PusTcInjector.cpp +++ b/test/testtasks/PusTcInjector.cpp @@ -37,7 +37,7 @@ PusTcInjector::~PusTcInjector() {} // // Send TC packet. // TmTcMessage tcMessage(tcPacket.getStoreAddress()); // ReturnValue_t result = injectionQueue->sendToDefault(&tcMessage); -// if(result != HasReturnvaluesIF::RETURN_OK) { +// if(result != returnvalue::OK) { // sif::warning << "PusTcInjector: Sending TMTC message failed!" << std::endl; // } // return result; @@ -61,5 +61,5 @@ ReturnValue_t PusTcInjector::initialize() { sif::error << "PusTcInjector: TC Store not initialized!" << std::endl; return ObjectManagerIF::CHILD_INIT_FAILED; } - return HasReturnvaluesIF::RETURN_OK; + return returnvalue::OK; } diff --git a/test/testtasks/PusTcInjector.h b/test/testtasks/PusTcInjector.h index 4d1cea7e..217f3b59 100644 --- a/test/testtasks/PusTcInjector.h +++ b/test/testtasks/PusTcInjector.h @@ -27,7 +27,7 @@ class PusTcInjector : public SystemObject { /** * This has to be called before using the PusTcInjector. * Call Not necessary when using a factory and the object manager. - * @return -@c RETURN_OK for successfull init + * @return -@c returnvalue::OK for successfull init * -@c ObjectManagerIF::CHILD_INIT_FAILED otherwise */ ReturnValue_t initialize() override; diff --git a/test/testtasks/TestTask.cpp b/test/testtasks/TestTask.cpp index 911767af..634fdd0d 100644 --- a/test/testtasks/TestTask.cpp +++ b/test/testtasks/TestTask.cpp @@ -18,7 +18,7 @@ EiveTestTask::EiveTestTask(object_id_t objectId_) : TestTask(objectId_), testMod EiveTestTask::~EiveTestTask() {} ReturnValue_t EiveTestTask::performOperation(uint8_t operationCode) { - ReturnValue_t result = RETURN_OK; + ReturnValue_t result = returnvalue::OK; if (oneShotAction) { /* Add code here which should only be run once */ @@ -76,22 +76,22 @@ ReturnValue_t EiveTestTask::performOneShotAction() { #if OBSW_ADD_TEST_CODE == 1 // performLwgpsTest(); #endif - return HasReturnvaluesIF::RETURN_OK; + return returnvalue::OK; } ReturnValue_t EiveTestTask::performPeriodicAction() { - ReturnValue_t result = RETURN_OK; + ReturnValue_t result = returnvalue::OK; return result; } ReturnValue_t EiveTestTask::performActionA() { - ReturnValue_t result = RETURN_OK; + ReturnValue_t result = returnvalue::OK; /* Add periodically executed code here */ return result; } ReturnValue_t EiveTestTask::performActionB() { - ReturnValue_t result = RETURN_OK; + ReturnValue_t result = returnvalue::OK; /* Add periodically executed code here */ return result; } diff --git a/unittest/controller/testThermalController.cpp b/unittest/controller/testThermalController.cpp index c54892e1..a65ec69b 100644 --- a/unittest/controller/testThermalController.cpp +++ b/unittest/controller/testThermalController.cpp @@ -19,14 +19,14 @@ TEST_CASE("Thermal Controller", "[ThermalController]") { ThermalController controller(THERMAL_CONTROLLER_ID, objects::NO_OBJECT); ReturnValue_t result = controller.initialize(); - REQUIRE(result == HasReturnvaluesIF::RETURN_OK); + REQUIRE(result == returnvalue::OK); PeriodicTaskIF* thermalTask = TaskFactory::instance()->createPeriodicTask( "THERMAL_CTL_TASK", 40, PeriodicTaskIF::MINIMUM_STACK_SIZE, 1.0, nullptr); result = thermalTask->addComponent(THERMAL_CONTROLLER_ID); - REQUIRE(result == HasReturnvaluesIF::RETURN_OK); + REQUIRE(result == returnvalue::OK); - REQUIRE(controller.initializeAfterTaskCreation() == HasReturnvaluesIF::RETURN_OK); + REQUIRE(controller.initializeAfterTaskCreation() == returnvalue::OK); testEnvironment::eventManager->clearEventList(); @@ -42,7 +42,7 @@ TEST_CASE("Thermal Controller", "[ThermalController]") { commandQueue->sendMessage(controllerQueue, &modeMessage); - REQUIRE(controller.performOperation(0) == HasReturnvaluesIF::RETURN_OK); + REQUIRE(controller.performOperation(0) == returnvalue::OK); REQUIRE(testEnvironment::eventManager->isEventInEventList( THERMAL_CONTROLLER_ID, HasModesIF::MODE_INFO, ControllerBase::MODE_NORMAL, diff --git a/unittest/mocks/EventManagerMock.cpp b/unittest/mocks/EventManagerMock.cpp index 6956c516..383c755b 100644 --- a/unittest/mocks/EventManagerMock.cpp +++ b/unittest/mocks/EventManagerMock.cpp @@ -5,16 +5,16 @@ EventManagerMock::EventManagerMock() : EventManager(objects::EVENT_MANAGER) {} ReturnValue_t EventManagerMock::performOperation(uint8_t opCode) { - ReturnValue_t result = HasReturnvaluesIF::RETURN_OK; - while (result == HasReturnvaluesIF::RETURN_OK) { + ReturnValue_t result = returnvalue::OK; + while (result == returnvalue::OK) { EventMessage message; result = eventReportQueue->receiveMessage(&message); - if (result == HasReturnvaluesIF::RETURN_OK) { + if (result == returnvalue::OK) { notifyListeners(&message); eventList.emplace_back(message); } } - return HasReturnvaluesIF::RETURN_OK; + return returnvalue::OK; } const std::list* EventManagerMock::getEventList() { From 691505ffa9a5c7c9168f6938f4e0a33948dd07c7 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Wed, 24 Aug 2022 17:35:06 +0200 Subject: [PATCH 08/52] update changelog --- CHANGELOG.md | 2 ++ 1 file changed, 2 insertions(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index 5525389c..9eee73fc 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -12,6 +12,8 @@ list yields a list of all related PRs for each release. # [v1.14.0] +- Update for FSFW: `HasReturnvaluesIF` class replaced by namespace `returnvalue` + # [v1.13.0] 24.08.2022 - Added first version of ACS Controller with gathers MGM data in a set From 86b474724965d2536172a793d2edc58d2262ca1d Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Wed, 24 Aug 2022 19:54:16 +0200 Subject: [PATCH 09/52] apid mask --- linux/devices/devicedefinitions/PlocSupervisorDefinitions.h | 3 +++ 1 file changed, 3 insertions(+) diff --git a/linux/devices/devicedefinitions/PlocSupervisorDefinitions.h b/linux/devices/devicedefinitions/PlocSupervisorDefinitions.h index bfe23488..fa34afee 100644 --- a/linux/devices/devicedefinitions/PlocSupervisorDefinitions.h +++ b/linux/devices/devicedefinitions/PlocSupervisorDefinitions.h @@ -101,6 +101,9 @@ static const uint16_t APID_DATA_LOGGER_DATA = 0x20D; /** * APIDs of telecommand packets */ +// 2 bits APID SRC, 00 for OBC, 2 bits APID DEST, 01 for SUPV, 7 bits CMD ID -> Mask 0x080 +static constexpr uint16_t APID_TC_SUPV_MASK = 0x080; + static const uint16_t APID_START_MPSOC = 0xA1; static const uint16_t APID_SHUTWOWN_MPSOC = 0xA2; static const uint16_t APID_SEL_MPSOC_BOOT_IMAGE = 0xA3; From 8aa65ebbd666b458283de11dea56f423d11a31f7 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 25 Aug 2022 09:55:42 +0200 Subject: [PATCH 10/52] bump tmtc --- tmtc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tmtc b/tmtc index d19cdfa5..4f5559c5 160000 --- a/tmtc +++ b/tmtc @@ -1 +1 @@ -Subproject commit d19cdfa5663966548918091005c7e9bc912e4041 +Subproject commit 4f5559c50fce2f044359df89fff1bcbba1807291 From c0a78e6feffa68b57bcb4c5e08e7c4590db75eb2 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 25 Aug 2022 18:19:33 +0200 Subject: [PATCH 11/52] start extending p60 module code --- mission/devices/GomspaceDeviceHandler.cpp | 41 +++-- mission/devices/GomspaceDeviceHandler.h | 6 +- .../devicedefinitions/GomSpacePackets.h | 145 ++++++------------ .../devicedefinitions/GomspaceDefinitions.h | 28 +++- tmtc | 2 +- 5 files changed, 107 insertions(+), 115 deletions(-) diff --git a/mission/devices/GomspaceDeviceHandler.cpp b/mission/devices/GomspaceDeviceHandler.cpp index da54168a..7606159a 100644 --- a/mission/devices/GomspaceDeviceHandler.cpp +++ b/mission/devices/GomspaceDeviceHandler.cpp @@ -6,6 +6,8 @@ #include "devicedefinitions/GomSpacePackets.h" #include "devicedefinitions/powerDefinitions.h" +using namespace GOMSPACE; + GomspaceDeviceHandler::GomspaceDeviceHandler(object_id_t objectId, object_id_t comIF, CookieIF* comCookie, FailureIsolationBase* customFdir, uint16_t maxConfigTableAddress, @@ -76,7 +78,14 @@ ReturnValue_t GomspaceDeviceHandler::buildCommandFromCommand(DeviceCommandId_t d break; } case (GOMSPACE::REQUEST_HK_TABLE): { - result = generateRequestFullHkTableCmd(hkTableReplySize); + result = generateRequestFullTableCmd(GOMSPACE::TableIds::HK, hkTableReplySize); + if (result != HasReturnvaluesIF::RETURN_OK) { + return result; + } + break; + } + case (GOMSPACE::REQUEST_CONFIG_TABLE): { + result = generateRequestFullTableCmd(GOMSPACE::TableIds::CONFIG, configTableReplySize); if (result != HasReturnvaluesIF::RETURN_OK) { return result; } @@ -214,9 +223,6 @@ ReturnValue_t GomspaceDeviceHandler::generateSetParamCommand(const uint8_t* comm << "action" << std::endl; return INVALID_ADDRESS; } - uint16_t checksum = GOMSPACE::IGNORE_CHECKSUM; - uint16_t seq = 0; - uint16_t total = 0; /* CSP reply only contains the transaction state */ uint16_t querySize = 1; const uint8_t* parameterPtr = setParamCacher.getParameter(); @@ -224,7 +230,7 @@ ReturnValue_t GomspaceDeviceHandler::generateSetParamCommand(const uint8_t* comm uint16_t payloadlength = sizeof(address) + parameterSize; /* Generate command for CspComIF */ - CspSetParamCommand setParamCmd(querySize, payloadlength, checksum, seq, total, address, + CspSetParamCommand setParamCmd(querySize, payloadlength, address, parameterPtr, parameterSize); size_t cspPacketLen = 0; uint8_t* buffer = cspPacket; @@ -263,7 +269,7 @@ ReturnValue_t GomspaceDeviceHandler::generateGetParamCommand(const uint8_t* comm } /* Get an check table id to read from */ uint8_t tableId = getParamMessage.getTableId(); - if (tableId != CONFIG_TABLE_ID && tableId != HK_TABLE_ID) { + if(not validTableId(tableId)) { sif::error << "GomspaceDeviceHandler: Invalid table id in get parameter" " message" << std::endl; @@ -271,20 +277,17 @@ ReturnValue_t GomspaceDeviceHandler::generateGetParamCommand(const uint8_t* comm } /* Get and check address */ uint16_t address = getParamMessage.getAddress(); - if (address > maxHkTableAddress && tableId == HK_TABLE_ID) { + if (address > maxHkTableAddress and tableId == TableIds::HK) { sif::error << "GomspaceDeviceHandler: Invalid address to get parameter from " << "housekeeping table" << std::endl; return INVALID_ADDRESS; } - if (address > maxConfigTableAddress && tableId == CONFIG_TABLE_ID) { + if (address > maxConfigTableAddress and tableId == TableIds::CONFIG) { sif::error << "GomspaceDeviceHandler: Invalid address to get parameter from " << "configuration table" << std::endl; return INVALID_ADDRESS; } uint16_t length = sizeof(address); - uint16_t checksum = GOMSPACE::IGNORE_CHECKSUM; - uint16_t seq = 0; - uint16_t total = 0; uint8_t parameterSize = getParamMessage.getParameterSize(); if (parameterSize > sizeof(uint32_t)) { sif::error << "GomspaceDeviceHandler: GET_PARAM: Invalid parameter " @@ -294,7 +297,7 @@ ReturnValue_t GomspaceDeviceHandler::generateGetParamCommand(const uint8_t* comm uint16_t querySize = parameterSize + GOMSPACE::GS_HDR_LENGTH; /* Generate the CSP command to send to the P60 Dock */ - CspGetParamCommand getParamCmd(querySize, tableId, length, checksum, seq, total, address); + CspGetParamCommand getParamCmd(querySize, tableId, length, address); size_t cspPacketLen = 0; uint8_t* buffer = cspPacket; result = getParamCmd.serialize(&buffer, &cspPacketLen, sizeof(cspPacket), @@ -396,6 +399,14 @@ ReturnValue_t GomspaceDeviceHandler::initializePduPool( return RETURN_OK; } +bool GomspaceDeviceHandler::validTableId(uint8_t id) { + if (id == TableIds::CONFIG or id == TableIds::BOARD_PARAMS or id == TableIds::CALIBRATION or + id == TableIds::HK) { + return true; + } + return false; +} + ReturnValue_t GomspaceDeviceHandler::generateResetWatchdogCmd() { WatchdogResetCommand watchdogResetCommand; size_t cspPacketLen = 0; @@ -415,9 +426,9 @@ ReturnValue_t GomspaceDeviceHandler::generateResetWatchdogCmd() { return HasReturnvaluesIF::RETURN_OK; } -ReturnValue_t GomspaceDeviceHandler::generateRequestFullHkTableCmd(uint16_t hkTableReplySize) { +ReturnValue_t GomspaceDeviceHandler::generateRequestFullTableCmd(uint8_t tableId, + uint16_t hkTableReplySize) { uint16_t querySize = hkTableReplySize; - uint8_t tableId = HK_TABLE_ID; RequestFullTableCommand requestFullTableCommand(querySize, tableId); size_t cspPacketLen = 0; @@ -425,7 +436,7 @@ ReturnValue_t GomspaceDeviceHandler::generateRequestFullHkTableCmd(uint16_t hkTa ReturnValue_t result = requestFullTableCommand.serialize( &buffer, &cspPacketLen, sizeof(cspPacket), SerializeIF::Endianness::BIG); if (result != HasReturnvaluesIF::RETURN_OK) { - sif::error << "GomspaceDeviceHandler::generateRequestFullHkTableCmd Failed to serialize " + sif::error << "GomspaceDeviceHandler::generateRequestFullTableCmd Failed to serialize " "full table request command " << std::endl; return result; diff --git a/mission/devices/GomspaceDeviceHandler.h b/mission/devices/GomspaceDeviceHandler.h index ea09b374..3bf7a600 100644 --- a/mission/devices/GomspaceDeviceHandler.h +++ b/mission/devices/GomspaceDeviceHandler.h @@ -52,8 +52,6 @@ class GomspaceDeviceHandler : public DeviceHandlerBase { static const uint8_t MAX_PACKET_LEN = 36; static const uint8_t PARAM_SET_OK = 1; static const uint8_t PING_REPLY_SIZE = 2; - static const uint8_t CONFIG_TABLE_ID = 1; - static const uint8_t HK_TABLE_ID = 4; uint8_t rememberRequestedSize = 0; uint8_t rememberCommandId = GOMSPACE::NONE; @@ -64,6 +62,7 @@ class GomspaceDeviceHandler : public DeviceHandlerBase { /** The size of the reply following a full hk table request.*/ uint16_t hkTableReplySize; + uint16_t configTableReplySize; LocalPoolDataSetBase *hkTableDataset = nullptr; @@ -83,7 +82,7 @@ class GomspaceDeviceHandler : public DeviceHandlerBase { * @brief The command to generate a request to receive the full housekeeping table is device * specific. Thus the child has to build this command. */ - virtual ReturnValue_t generateRequestFullHkTableCmd(uint16_t hkTableSize); + virtual ReturnValue_t generateRequestFullTableCmd(uint8_t tableId, uint16_t hkTableSize); /** * This command handles printing the HK table to the console. This is useful for debugging @@ -116,6 +115,7 @@ class GomspaceDeviceHandler : public DeviceHandlerBase { LocalDataPoolManager &poolManager, std::array initOutEnb); + static bool validTableId(uint8_t id); private: SetParamMessageUnpacker setParamCacher; /** diff --git a/mission/devices/devicedefinitions/GomSpacePackets.h b/mission/devices/devicedefinitions/GomSpacePackets.h index 9cb25e0c..6a213b69 100644 --- a/mission/devices/devicedefinitions/GomSpacePackets.h +++ b/mission/devices/devicedefinitions/GomSpacePackets.h @@ -7,6 +7,34 @@ #include #include +class CspParamRequestBase : public SerialLinkedListAdapter { +public: + CspParamRequestBase(uint16_t querySize, uint8_t tableId): querySize(querySize), tableId(tableId) { + setLinks(); + } +protected: + + void setLinks() { + setStart(&cspPort); + cspPort.setNext(&querySize); + querySize.setNext(&action); + action.setNext(&tableId); + tableId.setNext(&payloadlength); + payloadlength.setNext(&checksum); + checksum.setNext(&seq); + seq.setNext(&total); + } + SerializeElement cspPort = GOMSPACE::PARAM_PORT; + SerializeElement querySize; + SerializeElement action = GOMSPACE::ParamRequestIds::GET; + SerializeElement tableId; + // Default value 0: Fetch whole table. + SerializeElement payloadlength = 0; + SerializeElement checksum = GOMSPACE::IGNORE_CHECKSUM; + SerializeElement seq = 0; + SerializeElement total = 0; +}; + /** * @brief This class can be used to generated the command for the CspComIF * to reset the watchdog in a gomspace device. @@ -74,46 +102,22 @@ class CspPingCommand : public SerialLinkedListAdapter { * gomspace device but are required for the CspComIF to get the port * and the size to query. */ -class CspSetParamCommand : public SerialLinkedListAdapter { +class CspSetParamCommand : public CspParamRequestBase { public: - CspSetParamCommand(uint16_t querySize_, uint16_t payloadlength_, uint16_t checksum_, - uint16_t seq_, uint16_t total_, uint16_t addr_, const uint8_t *parameter_, - uint8_t parameterCount_) - : querySize(querySize_), - payloadlength(payloadlength_), - checksum(checksum_), - seq(seq_), - total(total_), + CspSetParamCommand(uint16_t querySize_, uint16_t payloadlength_, + uint16_t addr_, const uint8_t *parameter_, + uint8_t parameterCount_, uint8_t tableId = 1) + : CspParamRequestBase(querySize_, tableId), addr(addr_), parameter(parameter_, parameterCount_) { - setLinks(); - } - - private: - CspSetParamCommand(const CspSetParamCommand &command); - void setLinks() { - setStart(&cspPort); - cspPort.setNext(&querySize); - querySize.setNext(&action); - action.setNext(&tableId); - tableId.setNext(&payloadlength); - payloadlength.setNext(&checksum); - checksum.setNext(&seq); - seq.setNext(&total); total.setNext(&addr); addr.setNext(¶meter); + CspParamRequestBase::payloadlength = payloadlength_; + CspParamRequestBase::action = GOMSPACE::ParamRequestIds::SET; } - SerializeElement cspPort = GOMSPACE::PARAM_PORT; - /* Only a parameter will be set. No data will be queried with this command */ - SerializeElement querySize; - SerializeElement action = 0xFF; // param set - /* We will never set a parameter in a table other than the configuration - * table */ - SerializeElement tableId = 1; - SerializeElement payloadlength; - SerializeElement checksum; - SerializeElement seq; - SerializeElement total; + CspSetParamCommand(const CspSetParamCommand &command) = delete; + private: + SerializeElement addr; SerializeElement> parameter; }; @@ -126,48 +130,22 @@ class CspSetParamCommand : public SerialLinkedListAdapter { * @note cspPort and querySize only serve as information for the CspComIF * and will not be transmitted physically to the target device. */ -class CspGetParamCommand : public SerialLinkedListAdapter { +class CspGetParamCommand : public CspParamRequestBase { 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 tableId_, uint16_t addresslength_, - uint16_t checksum_, uint16_t seq_, uint16_t total_, uint16_t addr_) - : querySize(querySize_), - tableId(tableId_), - addresslength(addresslength_), - checksum(checksum_), - seq(seq_), - total(total_), + uint16_t addr_) + : CspParamRequestBase(querySize_, tableId_), addr(addr_) { - fixedValuesInit(); - setLinks(); - } - - private: - CspGetParamCommand(const CspGetParamCommand &command); - void setLinks() { - setStart(&cspPort); - cspPort.setNext(&querySize); - querySize.setNext(&action); - action.setNext(&tableId); - tableId.setNext(&addresslength); - addresslength.setNext(&checksum); - checksum.setNext(&seq); - seq.setNext(&total); total.setNext(&addr); + CspParamRequestBase::tableId = tableId_; + CspParamRequestBase::payloadlength = addresslength_; } - void fixedValuesInit() { cspPort.entry = GOMSPACE::PARAM_PORT; } - SerializeElement cspPort; - SerializeElement querySize; // size of bytes to query - /* Following information will also be physically transmitted to the target - * device*/ - SerializeElement action = 0x00; // get param - SerializeElement tableId; - SerializeElement addresslength; - SerializeElement checksum; - SerializeElement seq; - SerializeElement total; + CspGetParamCommand(const CspGetParamCommand &command) = delete; + private: + SerializeElement addr; }; @@ -179,37 +157,14 @@ class CspGetParamCommand : public SerialLinkedListAdapter { * @note cspPort and querySize only serve as information for the CspComIF * and will not be transmitted physically to the target device. */ -class RequestFullTableCommand : public SerialLinkedListAdapter { +class RequestFullTableCommand : public CspParamRequestBase { public: RequestFullTableCommand(uint16_t querySize_, uint8_t tableId_) - : querySize(querySize_), tableId(tableId_) { - setLinks(); - } + : CspParamRequestBase(querySize_, tableId_) {} + + RequestFullTableCommand(const RequestFullTableCommand &command) = delete; private: - RequestFullTableCommand(const RequestFullTableCommand &command); - void setLinks() { - setStart(&cspPort); - cspPort.setNext(&querySize); - querySize.setNext(&action); - action.setNext(&tableId); - tableId.setNext(&addresslength); - addresslength.setNext(&checksum); - checksum.setNext(&seq); - seq.setNext(&total); - } - SerializeElement cspPort = GOMSPACE::PARAM_PORT; - /** Size of bytes to query (size of csp header + size of table) */ - SerializeElement querySize; - /* Following information will also be physically transmitted to the target - * device*/ - SerializeElement action = 0x00; // get param - SerializeElement tableId; - /* Size of address. Set to 0 to get full table */ - SerializeElement addresslength = 0; - SerializeElement checksum = GOMSPACE::IGNORE_CHECKSUM; - SerializeElement seq = 0; - SerializeElement total = 0; }; /** diff --git a/mission/devices/devicedefinitions/GomspaceDefinitions.h b/mission/devices/devicedefinitions/GomspaceDefinitions.h index 9b79625c..a1f5c6e4 100644 --- a/mission/devices/devicedefinitions/GomspaceDefinitions.h +++ b/mission/devices/devicedefinitions/GomspaceDefinitions.h @@ -37,12 +37,38 @@ static const DeviceCommandId_t GNDWDT_RESET = 9; //!< [EXPORT] : [COMMAND] static const DeviceCommandId_t PARAM_GET = 0; //!< [EXPORT] : [COMMAND] static const DeviceCommandId_t PARAM_SET = 255; //!< [EXPORT] : [COMMAND] static const DeviceCommandId_t REQUEST_HK_TABLE = 16; //!< [EXPORT] : [COMMAND] - +static const DeviceCommandId_t REQUEST_CONFIG_TABLE = 17; //!< [EXPORT] : [COMMAND] +// Not implemented yet +// static const DeviceCommandId_t REQUEST_CALIB_TABLE = 18; //!< [EXPORT] : [COMMAND] //! [EXPORT] : [COMMAND] Print switch states, voltages and currents to the console //! For the ACU device, only print voltages and currents of the 6 ACU channels static const DeviceCommandId_t PRINT_SWITCH_V_I = 32; static const DeviceCommandId_t PRINT_LATCHUPS = 33; +enum ParamRequestIds: uint8_t { + GET = 0x00, + REPLY = 0x55, + SET = 0xFF, + TABLE_SPEC = 0x44, + // Copy memory slot to memory slot + COPY = 0x77, + // Load from file to slot. Load from primary slot + LOAD = 0x88, + // Load by name(s) + LOAD_FROM_STORE = 0x89, + // Save to primary slot + SAVE = 0x99, + // Save by name(s) + SAVE_TO_STORE = 0x9a +}; + +enum TableIds: uint8_t { + BOARD_PARAMS = 0, + CONFIG = 1, + CALIBRATION = 2, + HK = 4 +}; + } // namespace GOMSPACE namespace P60System { diff --git a/tmtc b/tmtc index d19cdfa5..d61af604 160000 --- a/tmtc +++ b/tmtc @@ -1 +1 @@ -Subproject commit d19cdfa5663966548918091005c7e9bc912e4041 +Subproject commit d61af604fec53b6a0af8d54e7c01792fc9a68790 From 7841a5c8c0b0111ecb9f135b5df9e367fb1dde25 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 25 Aug 2022 18:48:07 +0200 Subject: [PATCH 12/52] more complicated than expected --- mission/devices/devicedefinitions/GomspaceDefinitions.h | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/mission/devices/devicedefinitions/GomspaceDefinitions.h b/mission/devices/devicedefinitions/GomspaceDefinitions.h index a1f5c6e4..d7a87b16 100644 --- a/mission/devices/devicedefinitions/GomspaceDefinitions.h +++ b/mission/devices/devicedefinitions/GomspaceDefinitions.h @@ -227,8 +227,11 @@ static const uint16_t MAX_REPLY_LENGTH = 407; static const uint16_t MAX_CONFIGTABLE_ADDRESS = 408; static const uint16_t MAX_HKTABLE_ADDRESS = 187; +// Sources: +// GomSpace library lib/p60-dock_client/include/gs/p60-dock/param static const uint16_t HK_TABLE_SIZE = 188; - +static const uint16_t CAL_TABLE = 0xAE; +static const uint16_t CONFIG_TABLE_SIZE = 0x19C; static const uint8_t HK_TABLE_ENTRIES = 100; /** From 63b8aec89426c169765299382f53ba2eb4795084 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 25 Aug 2022 20:55:56 +0200 Subject: [PATCH 13/52] damn it --- CMakeLists.txt | 46 +- thirdparty/libcsp/CMakeLists.txt | 12 - .../libcsp/bindings/python/libcsp/__init__.py | 6 - thirdparty/libcsp/doc/example.rst | 123 -- thirdparty/libcsp/doc/history.rst | 17 - thirdparty/libcsp/doc/interfaces.rst | 95 -- thirdparty/libcsp/doc/libcsp.rst | 21 - thirdparty/libcsp/doc/memory.rst | 28 - thirdparty/libcsp/doc/mtu.rst | 19 - thirdparty/libcsp/doc/protocolstack.rst | 54 - thirdparty/libcsp/doc/structure.rst | 27 - thirdparty/libcsp/doc/topology.rst | 26 - thirdparty/libcsp/examples/csp_if_fifo.c | 165 --- .../libcsp/examples/csp_if_fifo_windows.c | 225 ---- thirdparty/libcsp/examples/kiss.c | 151 --- .../python_bindings_example_client.py | 42 - .../python_bindings_example_client_can.py | 30 - .../python_bindings_example_server.py | 72 -- thirdparty/libcsp/examples/simple.c | 200 --- thirdparty/libcsp/examples/zmqproxy.c | 82 -- thirdparty/libcsp/include/CMakeLists.txt | 9 - .../libcsp/include/csp/arch/csp_clock.h | 60 - .../libcsp/include/csp/arch/csp_malloc.h | 39 - .../libcsp/include/csp/arch/csp_queue.h | 49 - .../libcsp/include/csp/arch/csp_semaphore.h | 109 -- .../libcsp/include/csp/arch/csp_system.h | 74 -- .../libcsp/include/csp/arch/csp_thread.h | 100 -- thirdparty/libcsp/include/csp/arch/csp_time.h | 57 - .../include/csp/arch/posix/pthread_queue.h | 118 -- .../libcsp/include/csp/crypto/csp_hmac.h | 73 -- .../libcsp/include/csp/crypto/csp_sha1.h | 81 -- .../libcsp/include/csp/crypto/csp_xtea.h | 52 - thirdparty/libcsp/include/csp/csp.h | 545 -------- .../libcsp/include/csp/csp_autoconfig.h | 48 - thirdparty/libcsp/include/csp/csp_buffer.h | 92 -- thirdparty/libcsp/include/csp/csp_cmp.h | 189 --- thirdparty/libcsp/include/csp/csp_crc32.h | 63 - thirdparty/libcsp/include/csp/csp_debug.h | 150 --- thirdparty/libcsp/include/csp/csp_endian.h | 170 --- thirdparty/libcsp/include/csp/csp_error.h | 50 - thirdparty/libcsp/include/csp/csp_iflist.h | 56 - thirdparty/libcsp/include/csp/csp_interface.h | 54 - thirdparty/libcsp/include/csp/csp_platform.h | 56 - thirdparty/libcsp/include/csp/csp_rtable.h | 149 --- thirdparty/libcsp/include/csp/csp_types.h | 235 ---- .../include/csp/drivers/can_socketcan.h | 22 - thirdparty/libcsp/include/csp/drivers/i2c.h | 120 -- thirdparty/libcsp/include/csp/drivers/usart.h | 107 -- .../include/csp/interfaces/csp_if_can.h | 76 -- .../include/csp/interfaces/csp_if_i2c.h | 51 - .../include/csp/interfaces/csp_if_kiss.h | 110 -- .../libcsp/include/csp/interfaces/csp_if_lo.h | 38 - .../include/csp/interfaces/csp_if_zmqhub.h | 26 - thirdparty/libcsp/libcsp.mk | 12 - thirdparty/libcsp/src/CMakeLists.txt | 27 - thirdparty/libcsp/src/arch/CMakeLists.txt | 3 - .../libcsp/src/arch/freertos/csp_malloc.c | 33 - .../libcsp/src/arch/freertos/csp_queue.c | 66 - .../libcsp/src/arch/freertos/csp_semaphore.c | 96 -- .../libcsp/src/arch/freertos/csp_system.c | 139 --- .../libcsp/src/arch/freertos/csp_thread.c | 38 - .../libcsp/src/arch/freertos/csp_time.c | 46 - .../libcsp/src/arch/macosx/csp_malloc.c | 31 - thirdparty/libcsp/src/arch/macosx/csp_queue.c | 64 - .../libcsp/src/arch/macosx/csp_semaphore.c | 105 -- .../libcsp/src/arch/macosx/csp_system.c | 99 -- .../libcsp/src/arch/macosx/csp_thread.c | 31 - thirdparty/libcsp/src/arch/macosx/csp_time.c | 65 - .../libcsp/src/arch/macosx/pthread_queue.c | 179 --- .../libcsp/src/arch/posix/CMakeLists.txt | 9 - thirdparty/libcsp/src/arch/posix/csp_malloc.c | 31 - thirdparty/libcsp/src/arch/posix/csp_queue.c | 64 - .../libcsp/src/arch/posix/csp_semaphore.c | 164 --- thirdparty/libcsp/src/arch/posix/csp_system.c | 131 -- thirdparty/libcsp/src/arch/posix/csp_thread.c | 55 - thirdparty/libcsp/src/arch/posix/csp_time.c | 54 - .../libcsp/src/arch/posix/pthread_queue.c | 243 ---- thirdparty/libcsp/src/arch/windows/README | 18 - .../libcsp/src/arch/windows/csp_malloc.c | 9 - .../libcsp/src/arch/windows/csp_queue.c | 40 - .../libcsp/src/arch/windows/csp_semaphore.c | 74 -- .../libcsp/src/arch/windows/csp_system.c | 60 - .../libcsp/src/arch/windows/csp_thread.c | 11 - thirdparty/libcsp/src/arch/windows/csp_time.c | 20 - .../libcsp/src/arch/windows/windows_glue.h | 23 - .../libcsp/src/arch/windows/windows_queue.c | 91 -- .../libcsp/src/arch/windows/windows_queue.h | 41 - thirdparty/libcsp/src/bindings/python/pycsp.c | 1052 ---------------- thirdparty/libcsp/src/crypto/CMakeLists.txt | 5 - thirdparty/libcsp/src/crypto/csp_hmac.c | 202 --- thirdparty/libcsp/src/crypto/csp_sha1.c | 217 ---- thirdparty/libcsp/src/crypto/csp_xtea.c | 134 -- thirdparty/libcsp/src/csp_bridge.c | 94 -- thirdparty/libcsp/src/csp_buffer.c | 224 ---- thirdparty/libcsp/src/csp_conn.c | 498 -------- thirdparty/libcsp/src/csp_conn.h | 112 -- thirdparty/libcsp/src/csp_crc32.c | 140 --- thirdparty/libcsp/src/csp_debug.c | 133 -- thirdparty/libcsp/src/csp_dedup.c | 66 - thirdparty/libcsp/src/csp_dedup.h | 31 - thirdparty/libcsp/src/csp_endian.c | 204 --- thirdparty/libcsp/src/csp_hex_dump.c | 55 - thirdparty/libcsp/src/csp_iflist.c | 103 -- thirdparty/libcsp/src/csp_io.c | 502 -------- thirdparty/libcsp/src/csp_io.h | 47 - thirdparty/libcsp/src/csp_port.c | 105 -- thirdparty/libcsp/src/csp_port.h | 55 - thirdparty/libcsp/src/csp_promisc.c | 82 -- thirdparty/libcsp/src/csp_promisc.h | 30 - thirdparty/libcsp/src/csp_qfifo.c | 149 --- thirdparty/libcsp/src/csp_qfifo.h | 54 - thirdparty/libcsp/src/csp_route.c | 346 ------ thirdparty/libcsp/src/csp_route.h | 24 - thirdparty/libcsp/src/csp_service_handler.c | 334 ----- thirdparty/libcsp/src/csp_services.c | 233 ---- thirdparty/libcsp/src/csp_sfp.c | 170 --- thirdparty/libcsp/src/drivers/CMakeLists.txt | 1 - .../libcsp/src/drivers/can/CMakeLists.txt | 3 - .../libcsp/src/drivers/can/can_socketcan.c | 201 --- .../libcsp/src/drivers/usart/usart_linux.c | 254 ---- .../libcsp/src/drivers/usart/usart_windows.c | 230 ---- .../libcsp/src/interfaces/CMakeLists.txt | 7 - thirdparty/libcsp/src/interfaces/csp_if_can.c | 279 ----- .../libcsp/src/interfaces/csp_if_can_pbuf.c | 77 -- .../libcsp/src/interfaces/csp_if_can_pbuf.h | 31 - thirdparty/libcsp/src/interfaces/csp_if_i2c.c | 116 -- .../libcsp/src/interfaces/csp_if_kiss.c | 260 ---- thirdparty/libcsp/src/interfaces/csp_if_lo.c | 61 - thirdparty/libcsp/src/rtable/CMakeLists.txt | 3 - .../libcsp/src/rtable/csp_rtable_cidr.c | 233 ---- .../libcsp/src/rtable/csp_rtable_static.c | 128 -- .../libcsp/src/transport/CMakeLists.txt | 4 - thirdparty/libcsp/src/transport/csp_rdp.c | 1102 ----------------- .../libcsp/src/transport/csp_transport.h | 46 - thirdparty/libcsp/src/transport/csp_udp.c | 49 - thirdparty/libcsp/utils/cfpsplit.py | 52 - thirdparty/libcsp/utils/cspsplit.py | 52 - 137 files changed, 26 insertions(+), 15405 deletions(-) delete mode 100644 thirdparty/libcsp/CMakeLists.txt delete mode 100644 thirdparty/libcsp/bindings/python/libcsp/__init__.py delete mode 100644 thirdparty/libcsp/doc/example.rst delete mode 100644 thirdparty/libcsp/doc/history.rst delete mode 100644 thirdparty/libcsp/doc/interfaces.rst delete mode 100644 thirdparty/libcsp/doc/libcsp.rst delete mode 100644 thirdparty/libcsp/doc/memory.rst delete mode 100644 thirdparty/libcsp/doc/mtu.rst delete mode 100644 thirdparty/libcsp/doc/protocolstack.rst delete mode 100644 thirdparty/libcsp/doc/structure.rst delete mode 100644 thirdparty/libcsp/doc/topology.rst delete mode 100644 thirdparty/libcsp/examples/csp_if_fifo.c delete mode 100644 thirdparty/libcsp/examples/csp_if_fifo_windows.c delete mode 100644 thirdparty/libcsp/examples/kiss.c delete mode 100644 thirdparty/libcsp/examples/python_bindings_example_client.py delete mode 100644 thirdparty/libcsp/examples/python_bindings_example_client_can.py delete mode 100644 thirdparty/libcsp/examples/python_bindings_example_server.py delete mode 100644 thirdparty/libcsp/examples/simple.c delete mode 100644 thirdparty/libcsp/examples/zmqproxy.c delete mode 100644 thirdparty/libcsp/include/CMakeLists.txt delete mode 100644 thirdparty/libcsp/include/csp/arch/csp_clock.h delete mode 100644 thirdparty/libcsp/include/csp/arch/csp_malloc.h delete mode 100644 thirdparty/libcsp/include/csp/arch/csp_queue.h delete mode 100644 thirdparty/libcsp/include/csp/arch/csp_semaphore.h delete mode 100644 thirdparty/libcsp/include/csp/arch/csp_system.h delete mode 100644 thirdparty/libcsp/include/csp/arch/csp_thread.h delete mode 100644 thirdparty/libcsp/include/csp/arch/csp_time.h delete mode 100644 thirdparty/libcsp/include/csp/arch/posix/pthread_queue.h delete mode 100644 thirdparty/libcsp/include/csp/crypto/csp_hmac.h delete mode 100644 thirdparty/libcsp/include/csp/crypto/csp_sha1.h delete mode 100644 thirdparty/libcsp/include/csp/crypto/csp_xtea.h delete mode 100644 thirdparty/libcsp/include/csp/csp.h delete mode 100644 thirdparty/libcsp/include/csp/csp_autoconfig.h delete mode 100644 thirdparty/libcsp/include/csp/csp_buffer.h delete mode 100644 thirdparty/libcsp/include/csp/csp_cmp.h delete mode 100644 thirdparty/libcsp/include/csp/csp_crc32.h delete mode 100644 thirdparty/libcsp/include/csp/csp_debug.h delete mode 100644 thirdparty/libcsp/include/csp/csp_endian.h delete mode 100644 thirdparty/libcsp/include/csp/csp_error.h delete mode 100644 thirdparty/libcsp/include/csp/csp_iflist.h delete mode 100644 thirdparty/libcsp/include/csp/csp_interface.h delete mode 100644 thirdparty/libcsp/include/csp/csp_platform.h delete mode 100644 thirdparty/libcsp/include/csp/csp_rtable.h delete mode 100644 thirdparty/libcsp/include/csp/csp_types.h delete mode 100644 thirdparty/libcsp/include/csp/drivers/can_socketcan.h delete mode 100644 thirdparty/libcsp/include/csp/drivers/i2c.h delete mode 100644 thirdparty/libcsp/include/csp/drivers/usart.h delete mode 100644 thirdparty/libcsp/include/csp/interfaces/csp_if_can.h delete mode 100644 thirdparty/libcsp/include/csp/interfaces/csp_if_i2c.h delete mode 100644 thirdparty/libcsp/include/csp/interfaces/csp_if_kiss.h delete mode 100644 thirdparty/libcsp/include/csp/interfaces/csp_if_lo.h delete mode 100644 thirdparty/libcsp/include/csp/interfaces/csp_if_zmqhub.h delete mode 100644 thirdparty/libcsp/libcsp.mk delete mode 100644 thirdparty/libcsp/src/CMakeLists.txt delete mode 100644 thirdparty/libcsp/src/arch/CMakeLists.txt delete mode 100644 thirdparty/libcsp/src/arch/freertos/csp_malloc.c delete mode 100644 thirdparty/libcsp/src/arch/freertos/csp_queue.c delete mode 100644 thirdparty/libcsp/src/arch/freertos/csp_semaphore.c delete mode 100644 thirdparty/libcsp/src/arch/freertos/csp_system.c delete mode 100644 thirdparty/libcsp/src/arch/freertos/csp_thread.c delete mode 100644 thirdparty/libcsp/src/arch/freertos/csp_time.c delete mode 100644 thirdparty/libcsp/src/arch/macosx/csp_malloc.c delete mode 100644 thirdparty/libcsp/src/arch/macosx/csp_queue.c delete mode 100644 thirdparty/libcsp/src/arch/macosx/csp_semaphore.c delete mode 100644 thirdparty/libcsp/src/arch/macosx/csp_system.c delete mode 100644 thirdparty/libcsp/src/arch/macosx/csp_thread.c delete mode 100644 thirdparty/libcsp/src/arch/macosx/csp_time.c delete mode 100644 thirdparty/libcsp/src/arch/macosx/pthread_queue.c delete mode 100644 thirdparty/libcsp/src/arch/posix/CMakeLists.txt delete mode 100644 thirdparty/libcsp/src/arch/posix/csp_malloc.c delete mode 100644 thirdparty/libcsp/src/arch/posix/csp_queue.c delete mode 100644 thirdparty/libcsp/src/arch/posix/csp_semaphore.c delete mode 100644 thirdparty/libcsp/src/arch/posix/csp_system.c delete mode 100644 thirdparty/libcsp/src/arch/posix/csp_thread.c delete mode 100644 thirdparty/libcsp/src/arch/posix/csp_time.c delete mode 100644 thirdparty/libcsp/src/arch/posix/pthread_queue.c delete mode 100644 thirdparty/libcsp/src/arch/windows/README delete mode 100644 thirdparty/libcsp/src/arch/windows/csp_malloc.c delete mode 100644 thirdparty/libcsp/src/arch/windows/csp_queue.c delete mode 100644 thirdparty/libcsp/src/arch/windows/csp_semaphore.c delete mode 100644 thirdparty/libcsp/src/arch/windows/csp_system.c delete mode 100644 thirdparty/libcsp/src/arch/windows/csp_thread.c delete mode 100644 thirdparty/libcsp/src/arch/windows/csp_time.c delete mode 100644 thirdparty/libcsp/src/arch/windows/windows_glue.h delete mode 100644 thirdparty/libcsp/src/arch/windows/windows_queue.c delete mode 100644 thirdparty/libcsp/src/arch/windows/windows_queue.h delete mode 100644 thirdparty/libcsp/src/bindings/python/pycsp.c delete mode 100644 thirdparty/libcsp/src/crypto/CMakeLists.txt delete mode 100644 thirdparty/libcsp/src/crypto/csp_hmac.c delete mode 100644 thirdparty/libcsp/src/crypto/csp_sha1.c delete mode 100644 thirdparty/libcsp/src/crypto/csp_xtea.c delete mode 100644 thirdparty/libcsp/src/csp_bridge.c delete mode 100644 thirdparty/libcsp/src/csp_buffer.c delete mode 100644 thirdparty/libcsp/src/csp_conn.c delete mode 100644 thirdparty/libcsp/src/csp_conn.h delete mode 100644 thirdparty/libcsp/src/csp_crc32.c delete mode 100644 thirdparty/libcsp/src/csp_debug.c delete mode 100644 thirdparty/libcsp/src/csp_dedup.c delete mode 100644 thirdparty/libcsp/src/csp_dedup.h delete mode 100644 thirdparty/libcsp/src/csp_endian.c delete mode 100644 thirdparty/libcsp/src/csp_hex_dump.c delete mode 100644 thirdparty/libcsp/src/csp_iflist.c delete mode 100644 thirdparty/libcsp/src/csp_io.c delete mode 100644 thirdparty/libcsp/src/csp_io.h delete mode 100644 thirdparty/libcsp/src/csp_port.c delete mode 100644 thirdparty/libcsp/src/csp_port.h delete mode 100644 thirdparty/libcsp/src/csp_promisc.c delete mode 100644 thirdparty/libcsp/src/csp_promisc.h delete mode 100644 thirdparty/libcsp/src/csp_qfifo.c delete mode 100644 thirdparty/libcsp/src/csp_qfifo.h delete mode 100644 thirdparty/libcsp/src/csp_route.c delete mode 100644 thirdparty/libcsp/src/csp_route.h delete mode 100644 thirdparty/libcsp/src/csp_service_handler.c delete mode 100644 thirdparty/libcsp/src/csp_services.c delete mode 100644 thirdparty/libcsp/src/csp_sfp.c delete mode 100644 thirdparty/libcsp/src/drivers/CMakeLists.txt delete mode 100644 thirdparty/libcsp/src/drivers/can/CMakeLists.txt delete mode 100644 thirdparty/libcsp/src/drivers/can/can_socketcan.c delete mode 100644 thirdparty/libcsp/src/drivers/usart/usart_linux.c delete mode 100644 thirdparty/libcsp/src/drivers/usart/usart_windows.c delete mode 100644 thirdparty/libcsp/src/interfaces/CMakeLists.txt delete mode 100644 thirdparty/libcsp/src/interfaces/csp_if_can.c delete mode 100644 thirdparty/libcsp/src/interfaces/csp_if_can_pbuf.c delete mode 100644 thirdparty/libcsp/src/interfaces/csp_if_can_pbuf.h delete mode 100644 thirdparty/libcsp/src/interfaces/csp_if_i2c.c delete mode 100644 thirdparty/libcsp/src/interfaces/csp_if_kiss.c delete mode 100644 thirdparty/libcsp/src/interfaces/csp_if_lo.c delete mode 100644 thirdparty/libcsp/src/rtable/CMakeLists.txt delete mode 100644 thirdparty/libcsp/src/rtable/csp_rtable_cidr.c delete mode 100644 thirdparty/libcsp/src/rtable/csp_rtable_static.c delete mode 100644 thirdparty/libcsp/src/transport/CMakeLists.txt delete mode 100644 thirdparty/libcsp/src/transport/csp_rdp.c delete mode 100644 thirdparty/libcsp/src/transport/csp_transport.h delete mode 100644 thirdparty/libcsp/src/transport/csp_udp.c delete mode 100644 thirdparty/libcsp/utils/cfpsplit.py delete mode 100644 thirdparty/libcsp/utils/cspsplit.py diff --git a/CMakeLists.txt b/CMakeLists.txt index fdac38e1..bc1300d0 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -182,6 +182,7 @@ set(FSFW_PATH fsfw) set(TEST_PATH test) set(UNITTEST_PATH unittest) set(LINUX_PATH linux) +set(LIB_GOMSPACE_PATH ${THIRD_PARTY_FOLDER}/gomspace-sw) set(COMMON_PATH common) set(DUMMY_PATH dummies) set(WATCHDOG_PATH watchdog) @@ -218,6 +219,7 @@ if(TGT_BSP) if(NOT BUILD_Q7S_SIMPLE_MODE) set(EIVE_ADD_LINUX_FILES TRUE) set(ADD_CSP_LIB TRUE) + set(ADD_GOMSPACE_LIB TRUE) set(FSFW_HAL_ADD_LINUX ON) set(FSFW_HAL_LINUX_ADD_LIBGPIOD ON) set(FSFW_HAL_LINUX_ADD_PERIPHERAL_DRIVERS ON) @@ -280,7 +282,25 @@ set(FSFW_ADDITIONAL_INC_PATHS "${COMMON_PATH}/config" # global compiler options need to be set before adding executables if(CMAKE_CXX_COMPILER_ID STREQUAL "GNU") - add_compile_options( + # Remove unused sections. + add_compile_options("-ffunction-sections" "-fdata-sections") + + # Removed unused sections. + add_link_options("-Wl,--gc-sections") + +elseif(CMAKE_CXX_COMPILER_ID STREQUAL "MSVC") + set(COMPILER_FLAGS "/permissive-") +endif() + +add_library(${LIB_EIVE_MISSION}) +add_library(${LIB_DUMMIES}) + +# Add main executable +add_executable(${OBSW_NAME}) +set(OBSW_BIN_NAME ${CMAKE_PROJECT_NAME}) + +if(CMAKE_CXX_COMPILER_ID STREQUAL "GNU") + set(WARNING_FLAGS "-Wall" "-Wextra" "-Wimplicit-fallthrough=1" @@ -304,24 +324,11 @@ if(CMAKE_CXX_COMPILER_ID STREQUAL "GNU") # -Wconversion # Creates many false positives -Warith-conversion # Use with # Wconversion to find more implicit conversions -fanalyzer # Should be used # to look through problems - ) - # Remove unused sections. - add_compile_options("-ffunction-sections" "-fdata-sections") - - # Removed unused sections. - add_link_options("-Wl,--gc-sections") - -elseif(CMAKE_CXX_COMPILER_ID STREQUAL "MSVC") - set(COMPILER_FLAGS "/permissive-") + ) + target_compile_options(${OBSW_NAME} PRIVATE ${WARNING_FLAGS}) + target_compile_options(${LIB_EIVE_MISSION} PRIVATE ${WARNING_FLAGS}) endif() -add_library(${LIB_EIVE_MISSION}) -add_library(${LIB_DUMMIES}) - -# Add main executable -add_executable(${OBSW_NAME}) -set(OBSW_BIN_NAME ${CMAKE_PROJECT_NAME}) - set_target_properties(${OBSW_NAME} PROPERTIES OUTPUT_NAME ${OBSW_BIN_NAME}) # Watchdog @@ -357,10 +364,9 @@ if(EIVE_ADD_LINUX_FILES) add_subdirectory(${LINUX_PATH}) endif() add_subdirectory(${BSP_PATH}) -if(ADD_CSP_LIB) - add_subdirectory(${LIB_CSP_PATH}) +if(ADD_GOMSPACE_LIB) + add_subdirectory(${LIB_GOMSPACE_PATH}) endif() - add_subdirectory(${COMMON_PATH}) add_subdirectory(${DUMMY_PATH}) diff --git a/thirdparty/libcsp/CMakeLists.txt b/thirdparty/libcsp/CMakeLists.txt deleted file mode 100644 index 5c9f7677..00000000 --- a/thirdparty/libcsp/CMakeLists.txt +++ /dev/null @@ -1,12 +0,0 @@ -cmake_minimum_required(VERSION 3.13) - -set(LIB_CSP_NAME libcsp) - -add_library(${LIB_CSP_NAME}) - -add_subdirectory(src) -add_subdirectory(include) - -target_include_directories(${LIB_CSP_NAME} PRIVATE - ${CMAKE_CURRENT_SOURCE_DIR} -) \ No newline at end of file diff --git a/thirdparty/libcsp/bindings/python/libcsp/__init__.py b/thirdparty/libcsp/bindings/python/libcsp/__init__.py deleted file mode 100644 index 39de36b5..00000000 --- a/thirdparty/libcsp/bindings/python/libcsp/__init__.py +++ /dev/null @@ -1,6 +0,0 @@ -import sys - -if sys.version_info >= (3, 0): - from libcsp_py3 import * -else: - from libcsp_py2 import * diff --git a/thirdparty/libcsp/doc/example.rst b/thirdparty/libcsp/doc/example.rst deleted file mode 100644 index b82a055e..00000000 --- a/thirdparty/libcsp/doc/example.rst +++ /dev/null @@ -1,123 +0,0 @@ -Client and server example -========================= - -The following examples show the initialization of the protocol stack and examples of client/server code. - -Initialization Sequence ------------------------ - -This code initializes the CSP buffer system, device drivers and router core. The example uses the CAN interface function csp_can_tx but the initialization is similar for other interfaces. The loopback interface does not require any explicit initialization. - -.. code-block:: c - - #include - #include - - /* CAN configuration struct for SocketCAN interface "can0" */ - struct csp_can_config can_conf = {.ifc = "can0"}; - - /* Init buffer system with 10 packets of maximum 320 bytes each */ - csp_buffer_init(10, 320); - - /* Init CSP with address 1 */ - csp_init(1); - - /* Init the CAN interface with hardware filtering */ - csp_can_init(CSP_CAN_MASKED, &can_conf) - - /* Setup default route to CAN interface */ - csp_route_set(CSP_DEFAULT_ROUTE, &csp_can_tx, CSP_HOST_MAC); - - /* Start router task with 500 word stack, OS task priority 1 */ - csp_route_start_task(500, 1); - -Server ------- - -This example shows how to create a server task that listens for incoming connections. CSP should be initialized before starting this task. Note the use of `csp_service_handler()` as the default branch in the port switch case. The service handler will automatically reply to ICMP-like requests, such as pings and buffer status requests. - -.. code-block:: c - - void csp_task(void *parameters) { - /* Create socket without any socket options */ - csp_socket_t *sock = csp_socket(CSP_SO_NONE); - - /* Bind all ports to socket */ - csp_bind(sock, CSP_ANY); - - /* Create 10 connections backlog queue */ - csp_listen(sock, 10); - - /* Pointer to current connection and packet */ - csp_conn_t *conn; - csp_packet_t *packet; - - /* Process incoming connections */ - while (1) { - /* Wait for connection, 10000 ms timeout */ - if ((conn = csp_accept(sock, 10000)) == NULL) - continue; - - /* Read packets. Timout is 1000 ms */ - while ((packet = csp_read(conn, 1000)) != NULL) { - switch (csp_conn_dport(conn)) { - case MY_PORT: - /* Process packet here */ - default: - /* Let the service handler reply pings, buffer use, etc. */ - csp_service_handler(conn, packet); - break; - } - } - - /* Close current connection, and handle next */ - csp_close(conn); - } - } - -Client ------- - -This example shows how to allocate a packet buffer, connect to another host and send the packet. CSP should be initialized before calling this function. RDP, XTEA, HMAC and CRC checksums can be enabled per connection, by setting the connection option to a bitwise OR of any combination of `CSP_O_RDP`, `CSP_O_XTEA`, `CSP_O_HMAC` and `CSP_O_CRC`. - -.. code-block:: c - - int send_packet(void) { - - /* Get packet buffer for data */ - csp_packet_t *packet = csp_buffer_get(data_size); - if (packet == NULL) { - /* Could not get buffer element */ - printf("Failed to get buffer element\\n"); - return -1; - } - - /* Connect to host HOST, port PORT with regular UDP-like protocol and 1000 ms timeout */ - csp_conn_t *conn = csp_connect(CSP_PRIO_NORM, HOST, PORT, 1000, CSP_O_NONE); - if (conn == NULL) { - /* Connect failed */ - printf("Connection failed\\n"); - /* Remember to free packet buffer */ - csp_buffer_free(packet); - return -1; - } - - /* Copy message to packet */ - char *msg = "HELLO"; - strcpy(packet->data, msg); - - /* Set packet length */ - packet->length = strlen(msg); - - /* Send packet */ - if (!csp_send(conn, packet, 1000)) { - /* Send failed */ - printf("Send failed\\n"); - csp_buffer_free(packet); - } - - /* Close connection */ - csp_close(conn); - - return 0 - } diff --git a/thirdparty/libcsp/doc/history.rst b/thirdparty/libcsp/doc/history.rst deleted file mode 100644 index ad064873..00000000 --- a/thirdparty/libcsp/doc/history.rst +++ /dev/null @@ -1,17 +0,0 @@ -History -======= - -The idea was developed by a group of students from Aalborg University in 2008. In 2009 the main developer started working for GomSpace, and CSP became integrated into the GomSpace products. The protocol is based on a 32-bit header containing both transport, network and MAC-layer information. It's implementation is designed for, but not limited to, embedded systems such as the 8-bit AVR microprocessor and the 32-bit ARM and AVR from Atmel. The implementation is written in C and is currently ported to run on FreeRTOS and POSIX and pthreads based operating systems like Linux and BSD. The three letter acronym CSP was originally an abbreviation for CAN Space Protocol because the first MAC-layer driver was written for CAN-bus. Now the physical layer has extended to include spacelink, I2C and RS232, the name was therefore extended to the more general CubeSat Space Protocol without changing the abbreviation. - -Satellites using CSP --------------------- - -This is the known list of satellites or organisations that uses CSP. - - * GomSpace GATOSS GOMX-1 - * AAUSAT-3 - * EgyCubeSat - * EuroLuna - * NUTS - * Hawaiian Space Flight Laboratory - * GomSpace GOMX-3 diff --git a/thirdparty/libcsp/doc/interfaces.rst b/thirdparty/libcsp/doc/interfaces.rst deleted file mode 100644 index 5a80325c..00000000 --- a/thirdparty/libcsp/doc/interfaces.rst +++ /dev/null @@ -1,95 +0,0 @@ -CSP Interfaces -============== - -This is an example of how to implement a new layer-2 interface in CSP. The example is going to show how to create a `csp_if_fifo`, using a set of [named pipes](http://en.wikipedia.org/wiki/Named_pipe). The complete interface example code can be found in `examples/fifo.c`. For an example of a fragmenting interface, see the CAN interface in `src/interfaces/csp_if_can.c`. - -CSP interfaces are declared in a `csp_iface_t` structure, which sets the interface nexthop function and name. A maximum transmission unit can also be set, which forces CSP to drop outgoing packets above a certain size. The fifo interface is defined as: - -.. code-block:: c - - #include - #include - - csp_iface_t csp_if_fifo = { - .name = "fifo", - .nexthop = csp_fifo_tx, - .mtu = BUF_SIZE, - }; - -Outgoing traffic ----------------- - -The nexthop function takes a pointer to a CSP packet and a timeout as parameters. All outgoing packets that are routed to the interface are passed to this function: - -.. code-block:: c - - int csp_fifo_tx(csp_packet_t *packet, uint32_t timeout) { - write(tx_channel, &packet->length, packet->length + sizeof(uint32_t) + sizeof(uint16_t)); - csp_buffer_free(packet); - return 1; - } - -In the fifo interface, we simply transmit the header, length field and data using a write to the fifo. CSP does not dictate the wire format, so other interfaces may decide to e.g. ignore the length field if the physical layer provides start/stop flags. - -_Important notice: If the transmission succeeds, the interface must free the packet and return 1. If transmission fails, the nexthop function should return 0 and not free the packet, to allow retransmissions by the caller._ - -Incoming traffic ----------------- - -The interface also needs to receive incoming packets and pass it to the CSP protocol stack. In the fifo interface, this is handled by a thread that blocks on the incoming fifo and waits for packets: - -.. code-block:: c - - void * fifo_rx(void * parameters) { - csp_packet_t *buf = csp_buffer_get(BUF_SIZE); - /* Wait for packet on fifo */ - while (read(rx_channel, &buf->length, BUF_SIZE) > 0) { - csp_qfifo_write(buf, &csp_if_fifo, NULL); - buf = csp_buffer_get(BUF_SIZE); - } - } - -A new CSP buffer is preallocated with csp_buffer_get(). When data is received, the packet is passed to CSP using `csp_qfifo_write()` and a new buffer is allocated for the next packet. In addition to the received packet, `csp_qfifo_write()` takes two additional arguments: - -.. code-block:: c - - void csp_qfifo_write(csp_packet_t *packet, csp_iface_t *interface, CSP_BASE_TYPE *pxTaskWoken); - -The calling interface must be passed in `interface` to avoid routing loops. Furthermore, `pxTaskWoken` must be set to a non-NULL value if the packet is received in an interrupt service routine. If the packet is received in task context, NULL must be passed. 'pxTaskWoken' only applies to FreeRTOS systems, and POSIX system should always set the value to NULL. - -`csp_qfifo_write` will either accept the packet or free the packet buffer, so the interface must never free the packet after passing it to CSP. - -Initialization --------------- - -In order to initialize the interface, and make it available to the router, use the following function found in `csp/csp_interface.h`: - -.. code-block:: c - - csp_route_add_if(&csp_if_fifo); - -This actually happens automatically if you try to call `csp_route_add()` with an interface that is unknown to the router. This may however be removed in the future, in order to ensure that all interfaces are initialised before configuring the routing table. The reason is, that some products released in the future may ship with an empty routing table, which is then configured by a routing protocol rather than a static configuration. - -In order to setup a manual static route, use the following example where the default route is set to the fifo interface: - -.. code-block:: c - - csp_route_set(CSP_DEFAULT_ROUTE, &csp_if_fifo, CSP_NODE_MAC); - -All outgoing traffic except loopback, is now passed to the fifo interface's nexthop function. - -Building the example --------------------- - -The fifo examples can be compiled with: - -.. code-block:: bash - - % gcc csp_if_fifo.c -o csp_if_fifo -I/include -L/build -lcsp -lpthread -lrt - -The two named pipes are created with: - -.. code-block:: bash - - % mkfifo server_to_client client_to_server - diff --git a/thirdparty/libcsp/doc/libcsp.rst b/thirdparty/libcsp/doc/libcsp.rst deleted file mode 100644 index f866015f..00000000 --- a/thirdparty/libcsp/doc/libcsp.rst +++ /dev/null @@ -1,21 +0,0 @@ -.. CSP Documentation master file. - -.. _libcsp: - -********************** -CubeSat Space Protocol -********************** - -.. toctree:: - :maxdepth: 3 - - ../README - history - structure - interfaces - memory - protocolstack - topology - mtu - example - diff --git a/thirdparty/libcsp/doc/memory.rst b/thirdparty/libcsp/doc/memory.rst deleted file mode 100644 index 4e38d711..00000000 --- a/thirdparty/libcsp/doc/memory.rst +++ /dev/null @@ -1,28 +0,0 @@ -How CSP uses memory -=================== - -CSP has been written for small microprocessor systems. The way memory is handled is therefore a tradeoff between the amount used and the code efficiency. This section tries to give some answers to what the memory is used for and how it it used. The primary memory blocks in use by CSP is: - - * Routing table - * Ports table - * Connection table - * Buffer pool - * Interface list - -Tables ------- -The reason for using tables for the routes, ports and connections is speed. When a new packet arrives the core of CSP needs to do a quick lookup in the connection so see if it can find an existing connection to which the packet matches. If this is not found, it will take a lookup in the ports table to see if there are any applications listening on the incoming port number. Another argument of using tables are pre-allocation. The linker will reserve an area of the memory for which the routes and connections can be stored. This avoid an expensive `malloc()` call during initialization of CSP, and practically costs zero CPU instructions. The downside of using tables are the wasted memory used by unallocated ports and connections. For the routing table the argumentation is the same, pre-allocation is better than calling `malloc()`. - -Buffer Pool ------------ - -The buffer handling system can be compiled for either static allocation or a one-time dynamic allocation of the main memory block. After this, the buffer system is entirely self-contained. All allocated elements are of the same size, so the buffer size must be chosen to be able to handle the maximum possible packet length. The buffer pool uses a queue to store pointers to free buffer elements. First of all, this gives a very quick method to get the next free element since the dequeue is an O(1) operation. Furthermore, since the queue is a protected operating system primitive, it can be accessed from both task-context and interrupt-context. The `csp_buffer_get` version is for task-context and `csp_buffer_get_isr` is for interrupt-context. Using fixed size buffer elements that are preallocated is again a question of speed and safety. - - -A basic concept of the buffer system is called Zero-Copy. This means that from userspace to the kernel-driver, the buffer is never copied from one buffer to another. This is a big deal for a small microprocessor, where a call to `memcpy()` can be very expensive. In practice when data is inserted into a packet, it is shifted a certain number of bytes in order to allow for a packet header to be prepended at the lower layers. This also means that there is a strict contract between the layers, which data can be modified and where. The buffer object is normally casted to a `csp_packet_t`, but when its given to an interface on the MAC layer it's casted to a `csp_i2c_frame_t` for example. - -Interface list --------------- - -The interface list is a simple single-ended linked list of references to the interface specification structures. These structures are static const and allocated by the linker. The pointer to this data is inserted into the list one time during setup of the interface. Each entry in the routing table has a direct pointer to the interface element, thereby avoiding list lookup, but the list is needed in order for the dynamic route configuration to know which interfaces are available. - diff --git a/thirdparty/libcsp/doc/mtu.rst b/thirdparty/libcsp/doc/mtu.rst deleted file mode 100644 index 27753300..00000000 --- a/thirdparty/libcsp/doc/mtu.rst +++ /dev/null @@ -1,19 +0,0 @@ -Maximum Transfer Unit -===================== - -There are two things limiting the MTU of CSP. - - 1. The pre-allocated buffer pool’s allocation size - 2. The link layer protocol. - -So let’s assume that you have made a protocol called KISS with a MTU of 256. The 256 is the total amount of data that you can put into the CSP-packet. However, you need to take the overhead of the link layer into account. Typically this could consist of a length field and/or a start/stop flag. So the actual frame size on the link layer would for example be 256 bytes of data + 2 bytes sync flag + 2 bytes length field. - -This requires a buffer allocation of at lest 256 + 2 + 2. However, the CSP packet itself has some reserved bytes in the beginning of the packet (which you can see in csp.h) - so the recommended buffer allocation size is MAX MTU + 16 bytes. In this case the max MTU would be 256. - -If you try to pass data which is longer than the MTU, the chance is that you will also make a buffer overflow in the CSP buffer pool. However, lets assume that you have two interfaces one with an MTU of 200 bytes and another with an MTU of 100 bytes. In this case you might successfully transfer 150 bytes over the first interface, but the packet will be rejected once it comes to the second interface. - -If you want to increase your MTU of a specific link layer, it is up to the link layer protocol to implement its own fragmentation protocol. A good example is CAN-bus which only allows a frame size of 8 bytes. libcsp have a small protocol for this called the “CAN fragmentation protocol" or CFP for short. This allows data of much larger size to be transferred over the CAN bus. - -Okay, but what if you want to transfer 1000 bytes, and the network maximum MTU is 256? Well, since CSP does not include streaming sockets, only packet’s. Somebody will have to split that data up into chunks. It might be that you application have special knowledge about the datatype you are transmitting, and that it makes sense to split the 1000 byte content into 10 chunks of 100 byte status messages. This, application layer delimitation might be good if you have a situation with packet loss, because your receiver could still make good usage of the partially delivered chunks. - -But, what if you just want 1000 bytes transmitted, and you don’t care about the fragmentation unit, and also don’t want the hassle of writing the fragmentation code yourself? - In this case, libcsp now features a new (still experimental) feature called SFP (small fragmentation protocol) designed to work on the application layer. For this purpose you will not use csp_send and csp_recv, but csp_sfp_send and csp_sfp_recv. This will split your data into chunks of a certain size, enumerate them and transfer over a given connection. If a chunk is missing the SFP client will abort the reception, because SFP does not provide retransmission. If you wish to also have retransmission and orderly delivery you will have to open an RDP connection and send your SFP message to that connection. diff --git a/thirdparty/libcsp/doc/protocolstack.rst b/thirdparty/libcsp/doc/protocolstack.rst deleted file mode 100644 index 365aabbe..00000000 --- a/thirdparty/libcsp/doc/protocolstack.rst +++ /dev/null @@ -1,54 +0,0 @@ -The Protocol Stack -================== - -The CSP protocol stack includes functionality on all layers of the TCP/IP model: - -Layer 1: Drivers ----------------- - -Lib CSP is not designed for any specific processor or hardware peripheral, but yet these drivers are required in order to work. The intention of LibCSP is not to provide CAN, I2C or UART drivers for all platforms, however some drivers has been included for some platforms. If you do not find your platform supported, it is quite simple to add a driver that conforms to the CSP interfaces. For example the I2C driver just requires three functions: `init`, `send` and `recv`. For good stability and performance interrupt driven drivers are preferred in favor of polled drivers. Where applicable also DMA usage is recommended. - -Layer 2: MAC interfaces ------------------------ - -CSP has interfaces for I2C, CAN, RS232 (KISS) and Loopback. The layer 2 protocol software defines a frame-format that is suitable for the media. CSP can be easily extended with implementations for even more links. For example a radio-link and IP-networks. The file `csp_interface.h` declares the rx and tx functions needed in order to define a network interface in CSP. During initialisation of CSP each interface will be inserted into a linked list of interfaces that is available to the router. In cases where link-layer addresses are required, such as I2C, the routing table supports specifying next-hop link-layer address directly. This avoids the need to implement an address resolution protocol to translate CSP addresses to I2C addresses. - -Layer 3: Network Router ------------------------ - -The router core is the backbone of the CSP implementation. The router works by looking at a 32-bit CSP header which contains the delivery and source address together with port numbers for the connection. Each router supports both local delivery and forwarding of frames to another destination. Frames will never exit the router on the same interface that they arrives at, this concept is called split horizon, and helps prevent routing loops. - -The main purpose of the router is to accept incoming packets and deliver them to the right message queue. Therefore, in order to listen on a port-number on the network, a task must create a socket and call the accept() call. This will make the task block and wait for incoming traffic, just like a web-server or similar. When an incoming connection is opened, the task is woken. Depending on the task-priority, the task can even preempt another task and start execution immediately. - -There is no routing protocol for automatic route discovery, all routing tables are pre-programmed into the subsystems. The table itself contains a separate route to each of the possible 32 nodes in the network and the additional default route. This means that the overall topology must be decided before putting sub-systems together, as explained in the `topology.md` file. However CSP has an extension on port zero CMP (CSP management protocol), which allows for over-the-network routing table configuration. This has the advantage that default routes could be changed if for example the primary radio fails, and the secondary should be used instead. - -Layer 4: Transport Layer ------------------------- - -LibCSP implements two different Transport Layer protocols, they are called UDP (unreliable datagram protocol) and RDP (reliable datagram protocol). The name UDP has not been chosen to be an exact replica of the UDP (user datagram protocol) known from the TCP/IP model, but they have certain similarities. - -The most important thing to notice is that CSP is entirely a datagram service. There is no stream based service like TCP. A datagram is defined a block of data with a specified size and structure. This block enters the transport layer as a single datagram and exits the transport layer in the other end as a single datagram. CSP preserves this structure all the way to the physical layer for I2C, KISS and Loopback interfaces are used. The CAN-bus interface has to fragment the datagram into CAN-frames of 8 bytes, however only a fully completed datagram will arrive at the receiver. - -UDP -^^^ - -UDP uses a simple transmission model without implicit hand-shaking dialogues for guaranteeing reliability, ordering, or data integrity. Thus, UDP provides an unreliable service and datagrams may arrive out of order, appear duplicated, or go missing without notice. UDP assumes that error checking and correction is either not necessary or performed in the application, avoiding the overhead of such processing at the network interface level. Time-sensitive applications often use UDP because dropping packets is preferable to waiting for delayed packets, which may not be an option in a real-time system. - -UDP is very practical to implement request/reply based communication where a single packet forms the request and a single packet forms the reply. In this case a typical request and wait protocol is used between the client and server, which will simply return an error if a reply is not received within a specified time limit. An error would normally lead to a retransmission of the request from the user or operator which sent the request. - -While UDP is very simple, it also has some limitations. Normally a human in the loop is a good thing when operating the satellite over UDP. But when it comes to larger file transfers, the human becomes the bottleneck. When a high-speed file transfer is initiated data acknowledgment should be done automatically in order to speed up the transfer. This is where the RDP protocol can help. - -RDP -^^^ -CSP provides a transport layer extension called RDP (reliable datagram protocol) which is an implementation of RFC908 and RFC1151. RDP provides a few additional features: - - * Three-way handshake - * Flow Control - * Data-buffering - * Packet re-ordering - * Retransmission - * Windowing - * Extended Acknowledgment - -For more information on this, please refer to RFC908. - diff --git a/thirdparty/libcsp/doc/structure.rst b/thirdparty/libcsp/doc/structure.rst deleted file mode 100644 index 4c9b515c..00000000 --- a/thirdparty/libcsp/doc/structure.rst +++ /dev/null @@ -1,27 +0,0 @@ -Structure -========= -The Cubesat Space Protocol library is structured as shown in the following table: - -============================= ========================================================================= -**Folder** **Description** -============================= ========================================================================= -libcsp/include/csp Main include files -libcsp/include/csp/arch Architecture include files -libcsp/include/csp/interfaces Interface include files -libcsp/include/csp/drivers Drivers include files -libcsp/src Main modules for CSP: io, router, connections, services -libcsp/src/interfaces Interface modules for CAN, I2C, KISS, LOOP and ZMQHUB -libcsp/src/drivers/can Driver for CAN -libcsp/src/drivers/usart Driver for USART -libcsp/src/arch/freertos FreeRTOS architecture module -libcsp/src/arch/macosx Mac OS X architecture module -libcsp/src/arch/posix Posix architecture module -libcsp/src/arch/windows Windows architecture module -libcsp/src/rtable Routing table module -libcsp/transport Transport module, UDP and RDP -libcsp/crypto Crypto module -libcsp/utils Utilities -libcsp/bindings/python Python wrapper for libcsp -libcsp/examples CSP examples (source code) -libasf/doc The doc folder contains the source code for this documentation -============================= ========================================================================= diff --git a/thirdparty/libcsp/doc/topology.rst b/thirdparty/libcsp/doc/topology.rst deleted file mode 100644 index e629c29e..00000000 --- a/thirdparty/libcsp/doc/topology.rst +++ /dev/null @@ -1,26 +0,0 @@ -Network Topology -================ - -CSP uses a network oriented terminology similar to what is known from the Internet and the TCP/IP model. A CSP network can be configured for several different topologies. The most common topology is to create two segments, one for the Satellite and one for the Ground-Station. - -.. code-block:: none - - I2C BUS - _______________________________ - / | | | \ - +---+ +---+ +---+ +---+ +---+ - |OBC| |COM| |EPS| |PL1| |PL2| Nodes 0 - 7 (Space segment) - +---+ +---+ +---+ +---+ +---+ - ^ - | Radio - v - +---+ +----+ - |TNC| ------- | PC | Nodes 8 - 15 (Ground segment) - +---+ USB +----+ - - Node 9 Node 10 - -The address range, from 0 to 15, has been segmented into two equal size segments. This allows for easy routing in the network. All addresses starting with binary 1 is on the ground-segment, and all addresses starting with 0 is on the space segment. From CSP v1.0 the address space has been increased to 32 addresses, 0 to 31. But for legacy purposes, the old 0 to 15 is still used in most products. - -The network is configured using static routes initialised at boot-up of each sub-system. This means that the basic routing table must be assigned compile-time of each subsystem. However each node supports assigning an individual route to every single node in the network and can be changed run-time. This means that the network topology can be easily reconfigured after startup. - diff --git a/thirdparty/libcsp/examples/csp_if_fifo.c b/thirdparty/libcsp/examples/csp_if_fifo.c deleted file mode 100644 index 136fc3aa..00000000 --- a/thirdparty/libcsp/examples/csp_if_fifo.c +++ /dev/null @@ -1,165 +0,0 @@ -/* -Cubesat Space Protocol - A small network-layer protocol designed for Cubesats -Copyright (C) 2012 GomSpace ApS (http://www.gomspace.com) -Copyright (C) 2012 AAUSAT3 Project (http://aausat3.space.aau.dk) - -This library is free software; you can redistribute it and/or -modify it under the terms of the GNU Lesser General Public -License as published by the Free Software Foundation; either -version 2.1 of the License, or (at your option) any later version. - -This library is distributed in the hope that it will be useful, -but WITHOUT ANY WARRANTY; without even the implied warranty of -MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU -Lesser General Public License for more details. - -You should have received a copy of the GNU Lesser General Public -License along with this library; if not, write to the Free Software -Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA -*/ - -#include -#include -#include -#include -#include -#include - -#include -#include - -#define TYPE_SERVER 1 -#define TYPE_CLIENT 2 -#define PORT 10 -#define BUF_SIZE 250 - -pthread_t rx_thread; -int rx_channel, tx_channel; - -int csp_fifo_tx(csp_iface_t *ifc, csp_packet_t *packet, uint32_t timeout); - -csp_iface_t csp_if_fifo = { - .name = "fifo", - .nexthop = csp_fifo_tx, - .mtu = BUF_SIZE, -}; - -int csp_fifo_tx(csp_iface_t *ifc, csp_packet_t *packet, uint32_t timeout) { - /* Write packet to fifo */ - if (write(tx_channel, &packet->length, packet->length + sizeof(uint32_t) + sizeof(uint16_t)) < 0) - printf("Failed to write frame\r\n"); - csp_buffer_free(packet); - return CSP_ERR_NONE; -} - -void * fifo_rx(void * parameters) { - csp_packet_t *buf = csp_buffer_get(BUF_SIZE); - /* Wait for packet on fifo */ - while (read(rx_channel, &buf->length, BUF_SIZE) > 0) { - csp_qfifo_write(buf, &csp_if_fifo, NULL); - buf = csp_buffer_get(BUF_SIZE); - } - - return NULL; -} - -int main(int argc, char **argv) { - - int me, other, type; - const char *message = "Testing CSP"; - const char *rx_channel_name; - const char *tx_channel_name; - csp_socket_t *sock; - csp_conn_t *conn; - csp_packet_t *packet; - - /* Run as either server or client */ - if (argc != 2) { - printf("usage: %s \r\n", argv[0]); - return -1; - } - - /* Set type */ - if (strcmp(argv[1], "server") == 0) { - me = 1; - other = 2; - tx_channel_name = "server_to_client"; - rx_channel_name = "client_to_server"; - type = TYPE_SERVER; - } else if (strcmp(argv[1], "client") == 0) { - me = 2; - other = 1; - tx_channel_name = "client_to_server"; - rx_channel_name = "server_to_client"; - type = TYPE_CLIENT; - } else { - printf("Invalid type. Must be either 'server' or 'client'\r\n"); - return -1; - } - - /* Init CSP and CSP buffer system */ - if (csp_init(me) != CSP_ERR_NONE || csp_buffer_init(10, 300) != CSP_ERR_NONE) { - printf("Failed to init CSP\r\n"); - return -1; - } - - tx_channel = open(tx_channel_name, O_RDWR); - if (tx_channel < 0) { - printf("Failed to open TX channel\r\n"); - return -1; - } - - rx_channel = open(rx_channel_name, O_RDWR); - if (rx_channel < 0) { - printf("Failed to open RX channel\r\n"); - return -1; - } - - /* Start fifo RX task */ - pthread_create(&rx_thread, NULL, fifo_rx, NULL); - - /* Set default route and start router */ - csp_route_set(CSP_DEFAULT_ROUTE, &csp_if_fifo, CSP_NODE_MAC); - csp_route_start_task(0, 0); - - /* Create socket and listen for incoming connections */ - if (type == TYPE_SERVER) { - sock = csp_socket(CSP_SO_NONE); - csp_bind(sock, PORT); - csp_listen(sock, 5); - } - - /* Super loop */ - while (1) { - if (type == TYPE_SERVER) { - /* Process incoming packet */ - conn = csp_accept(sock, 1000); - if (conn) { - packet = csp_read(conn, 0); - if (packet) - printf("Received: %s\r\n", packet->data); - csp_buffer_free(packet); - csp_close(conn); - } - } else { - /* Send a new packet */ - packet = csp_buffer_get(strlen(message)); - if (packet) { - strcpy((char *) packet->data, message); - packet->length = strlen(message); - - conn = csp_connect(CSP_PRIO_NORM, other, PORT, 1000, CSP_O_NONE); - printf("Sending: %s\r\n", message); - if (!conn || !csp_send(conn, packet, 1000)) - return -1; - csp_close(conn); - } - sleep(1); - } - } - - close(rx_channel); - close(tx_channel); - - return 0; -} diff --git a/thirdparty/libcsp/examples/csp_if_fifo_windows.c b/thirdparty/libcsp/examples/csp_if_fifo_windows.c deleted file mode 100644 index 5b360709..00000000 --- a/thirdparty/libcsp/examples/csp_if_fifo_windows.c +++ /dev/null @@ -1,225 +0,0 @@ -#include -#include -#include -#include -#include -#undef interface - -#include -#include - -#define PIPE_BUFSIZE 1024 - -#define TYPE_SERVER 1 -#define TYPE_CLIENT 2 -#define PORT 10 -#define BUF_SIZE 250 - - -static LPCTSTR pipeName = TEXT("\\\\.\\pipe\\CSP_Pipe"); - -static HANDLE pipe = INVALID_HANDLE_VALUE; - -unsigned WINAPI fifo_rx(void *); -unsigned WINAPI pipe_listener(void *); - -void printError(void); - -int csp_fifo_tx(csp_packet_t *packet, uint32_t timeout); - -csp_iface_t csp_if_fifo = { - .name = "fifo", - .nexthop = csp_fifo_tx, - .mtu = BUF_SIZE, -}; - -int csp_fifo_tx(csp_packet_t *packet, uint32_t timeout) { - printf("csp_fifo_tx tid: %lu\n", GetCurrentThreadId()); - DWORD expectedSent = packet->length + sizeof(uint32_t) + sizeof(uint16_t); - DWORD actualSent; - /* Write packet to fifo */ - if( !WriteFile(pipe, &packet->length, expectedSent, &actualSent, NULL) - || actualSent != expectedSent ) { - printError(); - } - - csp_buffer_free(packet); - return CSP_ERR_NONE; -} - - -int main(int argc, char *argv[]) { - int me, other, type; - char *message = "Testing CSP"; - csp_socket_t *sock = NULL; - csp_conn_t *conn = NULL; - csp_packet_t *packet = NULL; - - /* Run as either server or client */ - if (argc != 2) { - printf("usage: server \r\n"); - return -1; - } - - /* Set type */ - if (strcmp(argv[1], "server") == 0) { - me = 1; - other = 2; - type = TYPE_SERVER; - } else if (strcmp(argv[1], "client") == 0) { - me = 2; - other = 1; - type = TYPE_CLIENT; - } else { - printf("Invalid type. Must be either 'server' or 'client'\r\n"); - return -1; - } - - /* Init CSP and CSP buffer system */ - if (csp_init(me) != CSP_ERR_NONE || csp_buffer_init(10, 300) != CSP_ERR_NONE) { - printf("Failed to init CSP\r\n"); - return -1; - } - - if( type == TYPE_SERVER ) { - _beginthreadex(NULL, 0, pipe_listener, NULL, 0, 0); - } else { - pipe = CreateFile( - pipeName, - GENERIC_READ | GENERIC_WRITE, - 0, - NULL, - OPEN_EXISTING, - 0, - NULL); - if( pipe == INVALID_HANDLE_VALUE ) { - printError(); - return -1; - } - } - - /* Set default route and start router */ - csp_route_set(CSP_DEFAULT_ROUTE, &csp_if_fifo, CSP_NODE_MAC); - csp_route_start_task(0, 0); - - /* Create socket and listen for incoming connections */ - if (type == TYPE_SERVER) { - sock = csp_socket(CSP_SO_NONE); - csp_bind(sock, PORT); - csp_listen(sock, 5); - } - - /* Super loop */ - while (1) { - if (type == TYPE_SERVER) { - /* Process incoming packet */ - conn = csp_accept(sock, 1000); - if (conn) { - packet = csp_read(conn, 0); - if (packet) - printf("Received: %s\r\n", packet->data); - csp_buffer_free(packet); - csp_close(conn); - } - } else { - /* Send a new packet */ - packet = csp_buffer_get(strlen(message)); - if (packet) { - strcpy((char *) packet->data, message); - packet->length = strlen(message); - - conn = csp_connect(CSP_PRIO_NORM, other, PORT, 1000, CSP_O_NONE); - printf("Sending: %s\r\n", message); - if (!conn || !csp_send(conn, packet, 1000)) - return -1; - csp_close(conn); - Sleep(1000); - } - } - } - - return 0; -} - -void printError(void) { - LPTSTR messageBuffer = NULL; - DWORD errorCode = GetLastError(); - DWORD formatMessageRet; - formatMessageRet = FormatMessage( - FORMAT_MESSAGE_ALLOCATE_BUFFER | - FORMAT_MESSAGE_FROM_SYSTEM | - FORMAT_MESSAGE_IGNORE_INSERTS, - NULL, - errorCode, - MAKELANGID(LANG_NEUTRAL, SUBLANG_DEFAULT), - (LPTSTR)&messageBuffer, - 0, - NULL); - - if( !formatMessageRet ) { - wprintf(L"FormatMessage error, code: %lu\n", GetLastError()); - return; - } - - printf("%s\n", messageBuffer); - LocalFree(messageBuffer); -} - -unsigned WINAPI pipe_listener(void *parameters) { - while(1) { - HANDLE pipe = CreateNamedPipe( - pipeName, - PIPE_ACCESS_DUPLEX, - PIPE_TYPE_MESSAGE | PIPE_READMODE_MESSAGE | PIPE_WAIT, - PIPE_UNLIMITED_INSTANCES, - PIPE_BUFSIZE, - PIPE_BUFSIZE, - 0, - NULL); - BOOL clientConnected; - if( pipe == INVALID_HANDLE_VALUE ) { - printf("Error creating named pipe. Code %lu\n", GetLastError()); - return -1; - } - - // True if client connects *after* server called ConnectNamedPipe - // or *between* CreateNamedPipe and ConnectNamedPipe - clientConnected = - ConnectNamedPipe(pipe, NULL) ? TRUE : GetLastError()==ERROR_PIPE_CONNECTED; - printf("Client connected!\n"); - - if( !clientConnected ) { - printf("Failure while listening for clients. Code %lu\n", GetLastError()); - CloseHandle(pipe); - return -1; - } - printf("Create client thread\n"); - _beginthreadex(NULL, 0, fifo_rx, (PVOID)pipe, 0, 0); - } - - return 0; -} - -unsigned WINAPI fifo_rx(void *handle) { - printf("fifo_rx tid: %lu\n", GetCurrentThreadId()); - HANDLE pipe = (HANDLE) handle; - csp_packet_t *buf = csp_buffer_get(BUF_SIZE); - DWORD bytesRead; - BOOL readSuccess; - - while(1) { - readSuccess = - ReadFile(pipe, &buf->length, BUF_SIZE, &bytesRead, NULL); - if( !readSuccess || bytesRead == 0 ) { - csp_buffer_free(buf); - printError(); - break; - } - csp_qfifo_write(buf, &csp_if_fifo, NULL); - buf = csp_buffer_get(BUF_SIZE); - } - printf("Closing pipe to client\n"); - CloseHandle(pipe); - - return 0; -} diff --git a/thirdparty/libcsp/examples/kiss.c b/thirdparty/libcsp/examples/kiss.c deleted file mode 100644 index c95eb2aa..00000000 --- a/thirdparty/libcsp/examples/kiss.c +++ /dev/null @@ -1,151 +0,0 @@ -/** - * Build this example on linux with: - * ./waf configure --enable-examples --enable-if-kiss --with-driver-usart=linux --enable-crc32 clean build - */ - -#include -#include -#include - -#include -#include - -#define PORT 10 -#define MY_ADDRESS 1 - -#define SERVER_TIDX 0 -#define CLIENT_TIDX 1 -#define USART_HANDLE 0 - -CSP_DEFINE_TASK(task_server) { - int running = 1; - csp_socket_t *socket = csp_socket(CSP_SO_NONE); - csp_conn_t *conn; - csp_packet_t *packet; - csp_packet_t *response; - - response = csp_buffer_get(sizeof(csp_packet_t) + 2); - if( response == NULL ) { - fprintf(stderr, "Could not allocate memory for response packet!\n"); - return CSP_TASK_RETURN; - } - response->data[0] = 'O'; - response->data[1] = 'K'; - response->length = 2; - - csp_bind(socket, CSP_ANY); - csp_listen(socket, 5); - - printf("Server task started\r\n"); - - while(running) { - if( (conn = csp_accept(socket, 10000)) == NULL ) { - continue; - } - - while( (packet = csp_read(conn, 100)) != NULL ) { - switch( csp_conn_dport(conn) ) { - case PORT: - if( packet->data[0] == 'q' ) - running = 0; - csp_buffer_free(packet); - csp_send(conn, response, 1000); - break; - default: - csp_service_handler(conn, packet); - break; - } - } - - csp_close(conn); - } - - csp_buffer_free(response); - - return CSP_TASK_RETURN; -} - -CSP_DEFINE_TASK(task_client) { - - char outbuf = 'q'; - char inbuf[3] = {0}; - int pingResult; - - for(int i = 50; i <= 200; i+= 50) { - pingResult = csp_ping(MY_ADDRESS, 1000, 100, CSP_O_NONE); - printf("Ping with payload of %d bytes, took %d ms\n", i, pingResult); - csp_sleep_ms(1000); - } - csp_ps(MY_ADDRESS, 1000); - csp_sleep_ms(1000); - csp_memfree(MY_ADDRESS, 1000); - csp_sleep_ms(1000); - csp_buf_free(MY_ADDRESS, 1000); - csp_sleep_ms(1000); - csp_uptime(MY_ADDRESS, 1000); - csp_sleep_ms(1000); - - csp_transaction(0, MY_ADDRESS, PORT, 1000, &outbuf, 1, inbuf, 2); - printf("Quit response from server: %s\n", inbuf); - - return CSP_TASK_RETURN; -} - -int main(int argc, char **argv) { - csp_debug_toggle_level(CSP_PACKET); - csp_debug_toggle_level(CSP_INFO); - - csp_buffer_init(10, 300); - csp_init(MY_ADDRESS); - - struct usart_conf conf; - -#if defined(CSP_WINDOWS) - conf.device = argc != 2 ? "COM4" : argv[1]; - conf.baudrate = CBR_9600; - conf.databits = 8; - conf.paritysetting = NOPARITY; - conf.stopbits = ONESTOPBIT; - conf.checkparity = FALSE; -#elif defined(CSP_POSIX) - conf.device = argc != 2 ? "/dev/ttyUSB0" : argv[1]; - conf.baudrate = 500000; -#elif defined(CSP_MACOSX) - conf.device = argc != 2 ? "/dev/tty.usbserial-FTSM9EGE" : argv[1]; - conf.baudrate = 115200; -#endif - - /* Run USART init */ - usart_init(&conf); - - /* Setup CSP interface */ - static csp_iface_t csp_if_kiss; - static csp_kiss_handle_t csp_kiss_driver; - csp_kiss_init(&csp_if_kiss, &csp_kiss_driver, usart_putc, usart_insert, "KISS"); - - /* Setup callback from USART RX to KISS RS */ - void my_usart_rx(uint8_t * buf, int len, void * pxTaskWoken) { - csp_kiss_rx(&csp_if_kiss, buf, len, pxTaskWoken); - } - usart_set_callback(my_usart_rx); - - csp_route_set(MY_ADDRESS, &csp_if_kiss, CSP_NODE_MAC); - csp_route_start_task(0, 0); - - csp_conn_print_table(); - csp_route_print_table(); - csp_route_print_interfaces(); - - csp_thread_handle_t handle_server; - csp_thread_create(task_server, "SERVER", 1000, NULL, 0, &handle_server); - csp_thread_handle_t handle_client; - csp_thread_create(task_client, "CLIENT", 1000, NULL, 0, &handle_client); - - /* Wait for program to terminate (ctrl + c) */ - while(1) { - csp_sleep_ms(1000000); - } - - return 0; - -} diff --git a/thirdparty/libcsp/examples/python_bindings_example_client.py b/thirdparty/libcsp/examples/python_bindings_example_client.py deleted file mode 100644 index 123ce36e..00000000 --- a/thirdparty/libcsp/examples/python_bindings_example_client.py +++ /dev/null @@ -1,42 +0,0 @@ -#!/usr/bin/python - -# libcsp must be build with at least these options to run this example client: -# ./waf distclean configure build --enable-bindings --enable-crc32 --enable-rdp --enable-if-zmq --with-driver-usart=linux --enable-if-kiss --enable-xtea --enable-if-can --enable-can-socketcan --enable-hmac --enable-examples - -# Can be run from root of libcsp like this: -# LD_LIBRARY_PATH=build PYTHONPATH=bindings/python:build python examples/python_bindings_example_client.py -# - -import os -import time -import libcsp as csp - - -if __name__ == "__main__": - - csp.buffer_init(10, 300) - csp.init(28) - csp.zmqhub_init(28, "localhost") - csp.rtable_set(27, 5, "ZMQHUB") - csp.route_start_task() - - ## allow router task startup - time.sleep(1) - - ## cmp_ident - (rc, host, model, rev, date, time) = csp.cmp_ident(27) - if rc == csp.CSP_ERR_NONE: - print (host, model, rev, date, time) - else: - print ("error in cmp_ident, rc=%i" % (rc)) - - ## transaction - outbuf = bytearray().fromhex('01') - inbuf = bytearray(1) - print ("using csp_transaction to send a single byte") - if csp.transaction(0, 27, 10, 1000, outbuf, inbuf) < 1: - print ("csp_transaction failed") - else: - print ("got reply, data=" + ''.join('{:02x}'.format(x) for x in inbuf)) - - diff --git a/thirdparty/libcsp/examples/python_bindings_example_client_can.py b/thirdparty/libcsp/examples/python_bindings_example_client_can.py deleted file mode 100644 index ec796572..00000000 --- a/thirdparty/libcsp/examples/python_bindings_example_client_can.py +++ /dev/null @@ -1,30 +0,0 @@ -#!/usr/bin/python - -# libcsp must be build with at least these options to run this example client: -# ./waf distclean configure build --enable-bindings --enable-crc32 --enable-rdp --enable-if-zmq --with-driver-usart=linux --enable-if-kiss --enable-xtea --enable-if-can --enable-can-socketcan --enable-hmac --enable-examples - -# Can be run from root of libcsp like this: -# LD_LIBRARY_PATH=build PYTHONPATH=bindings/python:build python examples/python_bindings_example_client.py -# - -import os -import time -import libcsp as csp - - -if __name__ == "__main__": - - csp.buffer_init(10, 300) - csp.init(28) - csp.can_socketcan_init("can0") - csp.rtable_set(4, 5, "CAN") - csp.route_start_task() - - ## allow router task startup - time.sleep(1) - - - node = 4 - if csp.ping(node) < 0: - print ("Unable to ping node %d"%(node)) - diff --git a/thirdparty/libcsp/examples/python_bindings_example_server.py b/thirdparty/libcsp/examples/python_bindings_example_server.py deleted file mode 100644 index 3cf3f5da..00000000 --- a/thirdparty/libcsp/examples/python_bindings_example_server.py +++ /dev/null @@ -1,72 +0,0 @@ -#!/usr/bin/python - -# libcsp must be build with at least these options to run this example server: -# ./waf distclean configure build --enable-bindings --enable-crc32 --enable-rdp --enable-if-zmq --with-driver-usart=linux --enable-if-kiss --enable-xtea --enable-if-can --enable-can-socketcan --enable-hmac --enable-examples - -# Can be run from root of libcsp like this: -# LD_LIBRARY_PATH=build PYTHONPATH=bindings/python:build python examples/python_bindings_example_server.py -# - -import os -import time -import sys -import libcsp as csp -import subprocess - -if __name__ == "__main__": - - # start a zmqproxy to transport messages to and from the client - zmqp = subprocess.Popen('build/zmqproxy') - - # init csp - csp.buffer_init(10, 300) - csp.init(27) - csp.zmqhub_init(27, "localhost") - csp.rtable_set(28, 5, "ZMQHUB") - csp.route_start_task() - - # set identity - csp.set_hostname("test_service") - csp.set_model("bindings") - csp.set_revision("1.2.3") - - # and read it back - print (csp.get_hostname()) - print (csp.get_model()) - print (csp.get_revision()) - - # start listening for packets... - sock = csp.socket() - csp.bind(sock, csp.CSP_ANY) - csp.listen(sock) - while True: - conn = csp.accept(sock) - if not conn: - continue - - print ("connection: source=%i:%i, dest=%i:%i" % (csp.conn_src(conn), - csp.conn_sport(conn), - csp.conn_dst(conn), - csp.conn_dport(conn))) - - while True: - packet = csp.read(conn) - if not packet: - break - - if csp.conn_dport(conn) == 10: - data = bytearray(csp.packet_get_data(packet)) - length = csp.packet_get_length(packet) - print ("got packet, len=" + str(length) + ", data=" + ''.join('{:02x}'.format(x) for x in data)) - - data[0] = data[0] + 1 - reply_packet = csp.buffer_get(1) - if reply_packet: - csp.packet_set_data(reply_packet, data) - csp.sendto_reply(packet, reply_packet, csp.CSP_O_NONE) - - csp.buffer_free(packet) - else: - csp.service_handler(conn, packet) - csp.close(conn) - diff --git a/thirdparty/libcsp/examples/simple.c b/thirdparty/libcsp/examples/simple.c deleted file mode 100644 index b996f8c1..00000000 --- a/thirdparty/libcsp/examples/simple.c +++ /dev/null @@ -1,200 +0,0 @@ -/* -Cubesat Space Protocol - A small network-layer protocol designed for Cubesats -Copyright (C) 2012 GomSpace ApS (http://www.gomspace.com) -Copyright (C) 2012 AAUSAT3 Project (http://aausat3.space.aau.dk) - -This library is free software; you can redistribute it and/or -modify it under the terms of the GNU Lesser General Public -License as published by the Free Software Foundation; either -version 2.1 of the License, or (at your option) any later version. - -This library is distributed in the hope that it will be useful, -but WITHOUT ANY WARRANTY; without even the implied warranty of -MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU -Lesser General Public License for more details. - -You should have received a copy of the GNU Lesser General Public -License along with this library; if not, write to the Free Software -Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA -*/ - -#include -#include -#include - -#include - -/* Using un-exported header file. - * This is allowed since we are still in libcsp */ -#include - -/** Example defines */ -#define MY_ADDRESS 1 // Address of local CSP node -#define MY_PORT 10 // Port to send test traffic to - -CSP_DEFINE_TASK(task_server) { - - /* Create socket without any socket options */ - csp_socket_t *sock = csp_socket(CSP_SO_NONE); - - /* Bind all ports to socket */ - csp_bind(sock, CSP_ANY); - - /* Create 10 connections backlog queue */ - csp_listen(sock, 10); - - /* Pointer to current connection and packet */ - csp_conn_t *conn; - csp_packet_t *packet; - - /* Process incoming connections */ - while (1) { - - /* Wait for connection, 10000 ms timeout */ - if ((conn = csp_accept(sock, 10000)) == NULL) - continue; - - /* Read packets. Timout is 100 ms */ - while ((packet = csp_read(conn, 100)) != NULL) { - switch (csp_conn_dport(conn)) { - case MY_PORT: - /* Process packet here */ - printf("Packet received on MY_PORT: %s\r\n", (char *) packet->data); - csp_buffer_free(packet); - break; - - default: - /* Let the service handler reply pings, buffer use, etc. */ - csp_service_handler(conn, packet); - break; - } - } - - /* Close current connection, and handle next */ - csp_close(conn); - - } - - return CSP_TASK_RETURN; - -} - -CSP_DEFINE_TASK(task_client) { - - csp_packet_t * packet; - csp_conn_t * conn; - - while (1) { - - /** - * Try ping - */ - - csp_sleep_ms(1000); - - int result = csp_ping(MY_ADDRESS, 100, 100, CSP_O_NONE); - printf("Ping result %d [ms]\r\n", result); - - csp_sleep_ms(1000); - - /** - * Try data packet to server - */ - - /* Get packet buffer for data */ - packet = csp_buffer_get(100); - if (packet == NULL) { - /* Could not get buffer element */ - printf("Failed to get buffer element\n"); - return CSP_TASK_RETURN; - } - - /* Connect to host HOST, port PORT with regular UDP-like protocol and 1000 ms timeout */ - conn = csp_connect(CSP_PRIO_NORM, MY_ADDRESS, MY_PORT, 1000, CSP_O_NONE); - if (conn == NULL) { - /* Connect failed */ - printf("Connection failed\n"); - /* Remember to free packet buffer */ - csp_buffer_free(packet); - return CSP_TASK_RETURN; - } - - /* Copy dummy data to packet */ - const char *msg = "Hello World"; - strcpy((char *) packet->data, msg); - - /* Set packet length */ - packet->length = strlen(msg); - - /* Send packet */ - if (!csp_send(conn, packet, 1000)) { - /* Send failed */ - printf("Send failed\n"); - csp_buffer_free(packet); - } - - /* Close connection */ - csp_close(conn); - - } - - return CSP_TASK_RETURN; -} - -int main(int argc, char * argv[]) { - - /** - * Initialise CSP, - * No physical interfaces are initialised in this example, - * so only the loopback interface is registered. - */ - - /* Init buffer system with 10 packets of maximum 300 bytes each */ - printf("Initialising CSP\r\n"); - csp_buffer_init(5, 300); - - /* Init CSP with address MY_ADDRESS */ - csp_init(MY_ADDRESS); - - /* Start router task with 500 word stack, OS task priority 1 */ - csp_route_start_task(500, 1); - - /* Enable debug output from CSP */ - if ((argc > 1) && (strcmp(argv[1], "-v") == 0)) { - printf("Debug enabed\r\n"); - csp_debug_toggle_level(3); - csp_debug_toggle_level(4); - - printf("Conn table\r\n"); - csp_conn_print_table(); - - printf("Route table\r\n"); - csp_route_print_table(); - - printf("Interfaces\r\n"); - csp_route_print_interfaces(); - - } - - /** - * Initialise example threads, using pthreads. - */ - - /* Server */ - printf("Starting Server task\r\n"); - csp_thread_handle_t handle_server; - csp_thread_create(task_server, "SERVER", 1000, NULL, 0, &handle_server); - - /* Client */ - printf("Starting Client task\r\n"); - csp_thread_handle_t handle_client; - csp_thread_create(task_client, "SERVER", 1000, NULL, 0, &handle_client); - - /* Wait for execution to end (ctrl+c) */ - while(1) { - csp_sleep_ms(100000); - } - - return 0; - -} diff --git a/thirdparty/libcsp/examples/zmqproxy.c b/thirdparty/libcsp/examples/zmqproxy.c deleted file mode 100644 index 5e259579..00000000 --- a/thirdparty/libcsp/examples/zmqproxy.c +++ /dev/null @@ -1,82 +0,0 @@ -#include -#include -#include -#include -#include -#include -#include - -static void * task_capture(void *ctx) { - - /* Subscriber (RX) */ - void *subscriber = zmq_socket(ctx, ZMQ_SUB); - assert(zmq_connect(subscriber, "tcp://localhost:7000") == 0); - assert(zmq_setsockopt(subscriber, ZMQ_SUBSCRIBE, "", 0) == 0); - - while (1) { - zmq_msg_t msg; - zmq_msg_init_size(&msg, 1024); - - /* Receive data */ - if (zmq_msg_recv(&msg, subscriber, 0) < 0) { - zmq_msg_close(&msg); - csp_log_error("ZMQ: %s\r\n", zmq_strerror(zmq_errno())); - continue; - } - - int datalen = zmq_msg_size(&msg); - if (datalen < 5) { - csp_log_warn("ZMQ: Too short datalen: %u\r\n", datalen); - while(zmq_msg_recv(&msg, subscriber, ZMQ_NOBLOCK) > 0) - zmq_msg_close(&msg); - continue; - } - - /* Create new csp packet */ - csp_packet_t * packet = malloc(1024); - if (packet == NULL) { - zmq_msg_close(&msg); - continue; - } - - /* Copy the data from zmq to csp */ - char * satidptr = ((char *) &packet->id) - 1; - memcpy(satidptr, zmq_msg_data(&msg), datalen); - packet->length = datalen - 4 - 1; - - printf("Input: Src %u, Dst %u, Dport %u, Sport %u, Pri %u, Flags 0x%02X, Size %"PRIu16"\r\n", - packet->id.src, packet->id.dst, packet->id.dport, - packet->id.sport, packet->id.pri, packet->id.flags, packet->length); - - free(packet); - zmq_msg_close(&msg); - } -} - -int main(int argc, char ** argv) { - - /** - * ZMQ PROXY - */ - void * ctx = zmq_ctx_new(); - assert(ctx); - - void *frontend = zmq_socket(ctx, ZMQ_XSUB); - assert(frontend); - assert(zmq_bind (frontend, "tcp://*:6000") == 0); - - void *backend = zmq_socket(ctx, ZMQ_XPUB); - assert(backend); - assert(zmq_bind(backend, "tcp://*:7000") == 0); - - pthread_t capworker; - pthread_create(&capworker, NULL, task_capture, ctx); - - printf("Starting ZMQproxy\r\n"); - zmq_proxy(frontend, backend, NULL); - - printf("Closing ZMQproxy\r\n"); - zmq_ctx_destroy(ctx); - return 0; - -} diff --git a/thirdparty/libcsp/include/CMakeLists.txt b/thirdparty/libcsp/include/CMakeLists.txt deleted file mode 100644 index 196e26f3..00000000 --- a/thirdparty/libcsp/include/CMakeLists.txt +++ /dev/null @@ -1,9 +0,0 @@ -target_include_directories(${LIB_CSP_NAME} PRIVATE - ${CMAKE_CURRENT_SOURCE_DIR} -) - -target_include_directories(${LIB_CSP_NAME} INTERFACE - ${CMAKE_CURRENT_SOURCE_DIR} -) - - diff --git a/thirdparty/libcsp/include/csp/arch/csp_clock.h b/thirdparty/libcsp/include/csp/arch/csp_clock.h deleted file mode 100644 index 3c19c887..00000000 --- a/thirdparty/libcsp/include/csp/arch/csp_clock.h +++ /dev/null @@ -1,60 +0,0 @@ -/* -Cubesat Space Protocol - A small network-layer protocol designed for Cubesats -Copyright (C) 2012 Gomspace ApS (http://www.gomspace.com) -Copyright (C) 2012 AAUSAT3 Project (http://aausat3.space.aau.dk) - -This library is free software; you can redistribute it and/or -modify it under the terms of the GNU Lesser General Public -License as published by the Free Software Foundation; either -version 2.1 of the License, or (at your option) any later version. - -This library is distributed in the hope that it will be useful, -but WITHOUT ANY WARRANTY; without even the implied warranty of -MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU -Lesser General Public License for more details. - -You should have received a copy of the GNU Lesser General Public -License along with this library; if not, write to the Free Software -Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA -*/ - -#ifndef _CSP_CLOCK_H_ -#define _CSP_CLOCK_H_ - -/** - @file - - Clock API. -*/ - -#ifdef __cplusplus -extern "C" { -#endif - -#include - -/** - Cross-platform timestamp. -*/ -typedef struct { - //! Seconds - uint32_t tv_sec; - //! Nano-seconds. - uint32_t tv_nsec; -} csp_timestamp_t; - -/** - Get time - must be implemented by the user. -*/ -__attribute__((weak)) extern void clock_get_time(csp_timestamp_t * time); - -/** - Set time - must be implemented by the user. -*/ -__attribute__((weak)) extern void clock_set_time(csp_timestamp_t * time); - -#ifdef __cplusplus -} /* extern "C" */ -#endif - -#endif // _CSP_CLOCK_H_ diff --git a/thirdparty/libcsp/include/csp/arch/csp_malloc.h b/thirdparty/libcsp/include/csp/arch/csp_malloc.h deleted file mode 100644 index 12602d1b..00000000 --- a/thirdparty/libcsp/include/csp/arch/csp_malloc.h +++ /dev/null @@ -1,39 +0,0 @@ -/* -Cubesat Space Protocol - A small network-layer protocol designed for Cubesats -Copyright (C) 2012 Gomspace ApS (http://www.gomspace.com) -Copyright (C) 2012 AAUSAT3 Project (http://aausat3.space.aau.dk) - -This library is free software; you can redistribute it and/or -modify it under the terms of the GNU Lesser General Public -License as published by the Free Software Foundation; either -version 2.1 of the License, or (at your option) any later version. - -This library is distributed in the hope that it will be useful, -but WITHOUT ANY WARRANTY; without even the implied warranty of -MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU -Lesser General Public License for more details. - -You should have received a copy of the GNU Lesser General Public -License along with this library; if not, write to the Free Software -Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA -*/ - -#ifndef _CSP_MALLOC_H_ -#define _CSP_MALLOC_H_ - -#ifdef __cplusplus -extern "C" { -#endif - -#include -#include -#include - -void * csp_malloc(size_t size); -void csp_free(void * ptr); - -#ifdef __cplusplus -} /* extern "C" */ -#endif - -#endif // _CSP_MALLOC_H_ diff --git a/thirdparty/libcsp/include/csp/arch/csp_queue.h b/thirdparty/libcsp/include/csp/arch/csp_queue.h deleted file mode 100644 index 3156c05e..00000000 --- a/thirdparty/libcsp/include/csp/arch/csp_queue.h +++ /dev/null @@ -1,49 +0,0 @@ -/* -Cubesat Space Protocol - A small network-layer protocol designed for Cubesats -Copyright (C) 2012 Gomspace ApS (http://www.gomspace.com) -Copyright (C) 2012 AAUSAT3 Project (http://aausat3.space.aau.dk) - -This library is free software; you can redistribute it and/or -modify it under the terms of the GNU Lesser General Public -License as published by the Free Software Foundation; either -version 2.1 of the License, or (at your option) any later version. - -This library is distributed in the hope that it will be useful, -but WITHOUT ANY WARRANTY; without even the implied warranty of -MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU -Lesser General Public License for more details. - -You should have received a copy of the GNU Lesser General Public -License along with this library; if not, write to the Free Software -Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA -*/ - -#ifndef _CSP_QUEUE_H_ -#define _CSP_QUEUE_H_ - -#ifdef __cplusplus -extern "C" { -#endif - -#define CSP_QUEUE_FULL 0 -#define CSP_QUEUE_ERROR 0 -#define CSP_QUEUE_OK 1 -typedef void * csp_queue_handle_t; - -#include -#include - -csp_queue_handle_t csp_queue_create(int length, size_t item_size); -void csp_queue_remove(csp_queue_handle_t queue); -int csp_queue_enqueue(csp_queue_handle_t handle, void *value, uint32_t timeout); -int csp_queue_enqueue_isr(csp_queue_handle_t handle, void * value, CSP_BASE_TYPE * task_woken); -int csp_queue_dequeue(csp_queue_handle_t handle, void *buf, uint32_t timeout); -int csp_queue_dequeue_isr(csp_queue_handle_t handle, void * buf, CSP_BASE_TYPE * task_woken); -int csp_queue_size(csp_queue_handle_t handle); -int csp_queue_size_isr(csp_queue_handle_t handle); - -#ifdef __cplusplus -} /* extern "C" */ -#endif - -#endif // _CSP_QUEUE_H_ diff --git a/thirdparty/libcsp/include/csp/arch/csp_semaphore.h b/thirdparty/libcsp/include/csp/arch/csp_semaphore.h deleted file mode 100644 index c8068da2..00000000 --- a/thirdparty/libcsp/include/csp/arch/csp_semaphore.h +++ /dev/null @@ -1,109 +0,0 @@ -/* -Cubesat Space Protocol - A small network-layer protocol designed for Cubesats -Copyright (C) 2012 Gomspace ApS (http://www.gomspace.com) -Copyright (C) 2012 AAUSAT3 Project (http://aausat3.space.aau.dk) - -This library is free software; you can redistribute it and/or -modify it under the terms of the GNU Lesser General Public -License as published by the Free Software Foundation; either -version 2.1 of the License, or (at your option) any later version. - -This library is distributed in the hope that it will be useful, -but WITHOUT ANY WARRANTY; without even the implied warranty of -MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU -Lesser General Public License for more details. - -You should have received a copy of the GNU Lesser General Public -License along with this library; if not, write to the Free Software -Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA -*/ - -#ifndef _CSP_SEMAPHORE_H_ -#define _CSP_SEMAPHORE_H_ - -#ifdef __cplusplus -extern "C" { -#endif - -#include - -#include - -/* POSIX interface */ -#if defined(CSP_POSIX) - -#include -#include - -#define CSP_SEMAPHORE_OK 1 -#define CSP_SEMAPHORE_ERROR 2 -#define CSP_MUTEX_OK CSP_SEMAPHORE_OK -#define CSP_MUTEX_ERROR CSP_SEMAPHORE_ERROR - -typedef sem_t csp_bin_sem_handle_t; -typedef pthread_mutex_t csp_mutex_t; - -#endif // CSP_POSIX - -/* MAC OS X interface */ -#if defined(CSP_MACOSX) - -#include -#include "posix/pthread_queue.h" - -#define CSP_SEMAPHORE_OK PTHREAD_QUEUE_OK -#define CSP_SEMAPHORE_ERROR PTHREAD_QUEUE_EMPTY -#define CSP_MUTEX_OK CSP_SEMAPHORE_OK -#define CSP_MUTEX_ERROR CSP_SEMAPHORE_ERROR - -typedef pthread_queue_t * csp_bin_sem_handle_t; -typedef pthread_queue_t * csp_mutex_t; - -#endif // CSP_MACOSX - -#if defined(CSP_WINDOWS) - -#include -#undef interface - -#define CSP_SEMAPHORE_OK 1 -#define CSP_SEMAPHORE_ERROR 2 -#define CSP_MUTEX_OK CSP_SEMAPHORE_OK -#define CSP_MUTEX_ERROR CSP_SEMAPHORE_ERROR - -typedef HANDLE csp_bin_sem_handle_t; -typedef HANDLE csp_mutex_t; - -#endif - -/* FreeRTOS interface */ -#if defined(CSP_FREERTOS) - -#include -#include - -#define CSP_SEMAPHORE_OK pdPASS -#define CSP_SEMAPHORE_ERROR pdFAIL -#define CSP_MUTEX_OK CSP_SEMAPHORE_OK -#define CSP_MUTEX_ERROR CSP_SEMAPHORE_ERROR - -typedef xSemaphoreHandle csp_bin_sem_handle_t; -typedef xSemaphoreHandle csp_mutex_t; - -#endif // CSP_FREERTOS - -int csp_mutex_create(csp_mutex_t * mutex); -int csp_mutex_remove(csp_mutex_t * mutex); -int csp_mutex_lock(csp_mutex_t * mutex, uint32_t timeout); -int csp_mutex_unlock(csp_mutex_t * mutex); -int csp_bin_sem_create(csp_bin_sem_handle_t * sem); -int csp_bin_sem_remove(csp_bin_sem_handle_t * sem); -int csp_bin_sem_wait(csp_bin_sem_handle_t * sem, uint32_t timeout); -int csp_bin_sem_post(csp_bin_sem_handle_t * sem); -int csp_bin_sem_post_isr(csp_bin_sem_handle_t * sem, CSP_BASE_TYPE * task_woken); - -#ifdef __cplusplus -} /* extern "C" */ -#endif - -#endif // _CSP_SEMAPHORE_H_ diff --git a/thirdparty/libcsp/include/csp/arch/csp_system.h b/thirdparty/libcsp/include/csp/arch/csp_system.h deleted file mode 100644 index c6c0e5af..00000000 --- a/thirdparty/libcsp/include/csp/arch/csp_system.h +++ /dev/null @@ -1,74 +0,0 @@ -/* -Cubesat Space Protocol - A small network-layer protocol designed for Cubesats -Copyright (C) 2012 Gomspace ApS (http://www.gomspace.com) -Copyright (C) 2012 AAUSAT3 Project (http://aausat3.space.aau.dk) - -This library is free software; you can redistribute it and/or -modify it under the terms of the GNU Lesser General Public -License as published by the Free Software Foundation; either -version 2.1 of the License, or (at your option) any later version. - -This library is distributed in the hope that it will be useful, -but WITHOUT ANY WARRANTY; without even the implied warranty of -MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU -Lesser General Public License for more details. - -You should have received a copy of the GNU Lesser General Public -License along with this library; if not, write to the Free Software -Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA -*/ - -#ifndef _CSP_SYSTEM_H_ -#define _CSP_SYSTEM_H_ - -#ifdef __cplusplus -extern "C" { -#endif - -#include - -#define COLOR_MASK_COLOR 0x0F -#define COLOR_MASK_MODIFIER 0xF0 - -typedef enum { - /* Colors */ - COLOR_RESET = 0xF0, - COLOR_BLACK = 0x01, - COLOR_RED = 0x02, - COLOR_GREEN = 0x03, - COLOR_YELLOW = 0x04, - COLOR_BLUE = 0x05, - COLOR_MAGENTA = 0x06, - COLOR_CYAN = 0x07, - COLOR_WHITE = 0x08, - /* Modifiers */ - COLOR_NORMAL = 0x0F, - COLOR_BOLD = 0x10, - COLOR_UNDERLINE = 0x20, - COLOR_BLINK = 0x30, - COLOR_HIDE = 0x40, -} csp_color_t; - -/** - * Writes out a task list into a pre-allocate buffer, - * use csp_sys_tasklist_size to get sizeof buffer to allocate - * @param out pointer to output buffer - * @return - */ -int csp_sys_tasklist(char * out); - -/** - * @return Size of tasklist buffer to allocate for the csp_sys_tasklist call - */ -int csp_sys_tasklist_size(void); - -uint32_t csp_sys_memfree(void); -int csp_sys_reboot(void); -int csp_sys_shutdown(void); -void csp_sys_set_color(csp_color_t color); - -#ifdef __cplusplus -} /* extern "C" */ -#endif - -#endif // _CSP_SYSTEM_H_ diff --git a/thirdparty/libcsp/include/csp/arch/csp_thread.h b/thirdparty/libcsp/include/csp/arch/csp_thread.h deleted file mode 100644 index 3c6ea171..00000000 --- a/thirdparty/libcsp/include/csp/arch/csp_thread.h +++ /dev/null @@ -1,100 +0,0 @@ -/* -Cubesat Space Protocol - A small network-layer protocol designed for Cubesats -Copyright (C) 2012 Gomspace ApS (http://www.gomspace.com) -Copyright (C) 2012 AAUSAT3 Project (http://aausat3.space.aau.dk) - -This library is free software; you can redistribute it and/or -modify it under the terms of the GNU Lesser General Public -License as published by the Free Software Foundation; either -version 2.1 of the License, or (at your option) any later version. - -This library is distributed in the hope that it will be useful, -but WITHOUT ANY WARRANTY; without even the implied warranty of -MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU -Lesser General Public License for more details. - -You should have received a copy of the GNU Lesser General Public -License along with this library; if not, write to the Free Software -Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA -*/ - -#ifndef _CSP_THREAD_H_ -#define _CSP_THREAD_H_ - -#ifdef __cplusplus -extern "C" { -#endif - -#include -#include - -/* POSIX interface */ -#if defined(CSP_POSIX) || defined(CSP_MACOSX) - -#include -#include - -#define csp_thread_exit() pthread_exit(NULL) - -typedef pthread_t csp_thread_handle_t; -typedef void * csp_thread_return_t; - -#define CSP_DEFINE_TASK(task_name) csp_thread_return_t task_name(void * param) -#define CSP_TASK_RETURN NULL - -#define csp_sleep_ms(time_ms) usleep(time_ms * 1000); - -#endif // CSP_POSIX - -/* Windows interface */ -#if defined(CSP_WINDOWS) - -#include -#undef interface -#include - -#define csp_thread_exit() _endthreadex(0) - -typedef HANDLE csp_thread_handle_t; -typedef unsigned int csp_thread_return_t; - -#define CSP_DEFINE_TASK(task_name) csp_thread_return_t __attribute__((stdcall)) task_name(void * param) -#define CSP_TASK_RETURN 0 - -#define csp_sleep_ms(time_ms) Sleep(time_ms); - -#endif // CSP_WINDOWS - -/* FreeRTOS interface */ -#if defined(CSP_FREERTOS) - -#include -#include - -#if INCLUDE_vTaskDelete -#define csp_thread_exit() vTaskDelete(NULL) -#else -#define csp_thread_exit() -#endif - -typedef xTaskHandle csp_thread_handle_t; -typedef void csp_thread_return_t; - -#define CSP_DEFINE_TASK(task_name) csp_thread_return_t task_name(void * param) -#define CSP_TASK_RETURN - -#define csp_sleep_ms(time_ms) vTaskDelay(time_ms / portTICK_RATE_MS); - -#endif // CSP_FREERTOS - -#ifndef CSP_WINDOWS -int csp_thread_create(csp_thread_return_t (* routine)(void *), const char * const thread_name, unsigned short stack_depth, void * parameters, unsigned int priority, csp_thread_handle_t * handle); -#else -int csp_thread_create(csp_thread_return_t (* routine)(void *)__attribute__((stdcall)), const char * const thread_name, unsigned short stack_depth, void * parameters, unsigned int priority, csp_thread_handle_t * handle); -#endif - -#ifdef __cplusplus -} /* extern "C" */ -#endif - -#endif // _CSP_THREAD_H_ diff --git a/thirdparty/libcsp/include/csp/arch/csp_time.h b/thirdparty/libcsp/include/csp/arch/csp_time.h deleted file mode 100644 index aa72ab8f..00000000 --- a/thirdparty/libcsp/include/csp/arch/csp_time.h +++ /dev/null @@ -1,57 +0,0 @@ -/* -Cubesat Space Protocol - A small network-layer protocol designed for Cubesats -Copyright (C) 2012 Gomspace ApS (http://www.gomspace.com) -Copyright (C) 2012 AAUSAT3 Project (http://aausat3.space.aau.dk) - -This library is free software; you can redistribute it and/or -modify it under the terms of the GNU Lesser General Public -License as published by the Free Software Foundation; either -version 2.1 of the License, or (at your option) any later version. - -This library is distributed in the hope that it will be useful, -but WITHOUT ANY WARRANTY; without even the implied warranty of -MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU -Lesser General Public License for more details. - -You should have received a copy of the GNU Lesser General Public -License along with this library; if not, write to the Free Software -Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA -*/ - -#ifndef _CSP_TIME_H_ -#define _CSP_TIME_H_ - -#ifdef __cplusplus -extern "C" { -#endif - -#include -#include - -/* Blackfin/x86 on Linux */ -#if defined(CSP_POSIX) - -#include -#include -#include - -#endif // CSP_POSIX - -/* AVR/ARM on FreeRTOS */ -#if defined(CSP_FREERTOS) - -#include -#include - -#endif // CSP_FREERTOS - -uint32_t csp_get_ms(void); -uint32_t csp_get_ms_isr(void); -uint32_t csp_get_s(void); -uint32_t csp_get_s_isr(void); - -#ifdef __cplusplus -} /* extern "C" */ -#endif - -#endif // _CSP_TIME_H_ diff --git a/thirdparty/libcsp/include/csp/arch/posix/pthread_queue.h b/thirdparty/libcsp/include/csp/arch/posix/pthread_queue.h deleted file mode 100644 index 44ef596e..00000000 --- a/thirdparty/libcsp/include/csp/arch/posix/pthread_queue.h +++ /dev/null @@ -1,118 +0,0 @@ -/* -Cubesat Space Protocol - A small network-layer protocol designed for Cubesats -Copyright (C) 2012 Gomspace ApS (http://www.gomspace.com) -Copyright (C) 2012 AAUSAT3 Project (http://aausat3.space.aau.dk) - -This library is free software; you can redistribute it and/or -modify it under the terms of the GNU Lesser General Public -License as published by the Free Software Foundation; either -version 2.1 of the License, or (at your option) any later version. - -This library is distributed in the hope that it will be useful, -but WITHOUT ANY WARRANTY; without even the implied warranty of -MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU -Lesser General Public License for more details. - -You should have received a copy of the GNU Lesser General Public -License along with this library; if not, write to the Free Software -Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA -*/ - -#ifndef _PTHREAD_QUEUE_H_ -#define _PTHREAD_QUEUE_H_ - -/** - @file - - Queue implemented using pthread locks and conds. - - Inspired by c-pthread-queue by Matthew Dickinson: http://code.google.com/p/c-pthread-queue/ -*/ - -#ifdef __cplusplus -extern "C" { -#endif - -#include -#include -#include - -#include - -/** - Queue error codes. - @{ -*/ -/** - General error code - something went wrong. -*/ -#define PTHREAD_QUEUE_ERROR CSP_QUEUE_ERROR -/** - Queue is empty - cannot extract element. -*/ -#define PTHREAD_QUEUE_EMPTY CSP_QUEUE_ERROR -/** - Queue is full - cannot insert element. -*/ -#define PTHREAD_QUEUE_FULL CSP_QUEUE_ERROR -/** - Ok - no error. -*/ -#define PTHREAD_QUEUE_OK CSP_QUEUE_OK -/** @{ */ - -/** - Queue handle. -*/ -typedef struct pthread_queue_s { - //! Memory area. - void * buffer; - //! Memory size. - int size; - //! Item/element size. - int item_size; - //! Items/elements in queue. - int items; - //! Insert point. - int in; - //! Extract point. - int out; - //! Lock. - pthread_mutex_t mutex; - //! Wait because queue is full (insert). - pthread_cond_t cond_full; - //! Wait because queue is empty (extract). - pthread_cond_t cond_empty; -} pthread_queue_t; - -/** - Create queue. -*/ -pthread_queue_t * pthread_queue_create(int length, size_t item_size); - -/** - Delete queue. -*/ -void pthread_queue_delete(pthread_queue_t * q); - -/** - Enqueue/insert element. -*/ -int pthread_queue_enqueue(pthread_queue_t * queue, void * value, uint32_t timeout); - -/** - Dequeue/extract element. -*/ -int pthread_queue_dequeue(pthread_queue_t * queue, void * buf, uint32_t timeout); - -/** - Return number of elements in the queue. -*/ -int pthread_queue_items(pthread_queue_t * queue); - -#ifdef __cplusplus -} /* extern "C" */ -#endif - -#endif // _PTHREAD_QUEUE_H_ - diff --git a/thirdparty/libcsp/include/csp/crypto/csp_hmac.h b/thirdparty/libcsp/include/csp/crypto/csp_hmac.h deleted file mode 100644 index 8c3f5d6a..00000000 --- a/thirdparty/libcsp/include/csp/crypto/csp_hmac.h +++ /dev/null @@ -1,73 +0,0 @@ -/* -Cubesat Space Protocol - A small network-layer protocol designed for Cubesats -Copyright (C) 2012 GomSpace ApS (http://www.gomspace.com) -Copyright (C) 2012 AAUSAT3 Project (http://aausat3.space.aau.dk) - -This library is free software; you can redistribute it and/or -modify it under the terms of the GNU Lesser General Public -License as published by the Free Software Foundation; either -version 2.1 of the License, or (at your option) any later version. - -This library is distributed in the hope that it will be useful, -but WITHOUT ANY WARRANTY; without even the implied warranty of -MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU -Lesser General Public License for more details. - -You should have received a copy of the GNU Lesser General Public -License along with this library; if not, write to the Free Software -Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA -*/ - -#ifndef _CSP_HMAC_H_ -#define _CSP_HMAC_H_ - -#include - -#ifdef __cplusplus -extern "C" { -#endif - -#define CSP_HMAC_LENGTH 4 - -/** - * Append HMAC to packet - * @param packet Pointer to packet - * @param include_header use header in hmac calculation (this will not modify the flags field) - * @return 0 on success, negative on failure - */ -int csp_hmac_append(csp_packet_t * packet, bool include_header); - -/** - * Verify HMAC of packet - * @param packet Pointer to packet - * @param include_header use header in hmac calculation (this will not modify the flags field) - * @return 0 on success, negative on failure - */ -int csp_hmac_verify(csp_packet_t * packet, bool include_header); - -/** - * Calculate HMAC on buffer - * - * This function is used by append/verify but cal also be called separately. - * @param key HMAC key - * @param keylen HMAC key length - * @param data pointer to data - * @param datalen lehgth of data - * @param hmac output HMAC calculation (CSP_HMAC_LENGTH) - * @return 0 on success, negative on failure - */ -int csp_hmac_memory(const uint8_t * key, uint32_t keylen, const uint8_t * data, uint32_t datalen, uint8_t * hmac); - -/** - * Save a copy of the key string for use by the append/verify functions - * @param key HMAC key - * @param keylen HMAC key length - * @return Always returns 0 - */ -int csp_hmac_set_key(char * key, uint32_t keylen); - -#ifdef __cplusplus -} /* extern "C" */ -#endif - -#endif // _CSP_HMAC_H_ diff --git a/thirdparty/libcsp/include/csp/crypto/csp_sha1.h b/thirdparty/libcsp/include/csp/crypto/csp_sha1.h deleted file mode 100644 index aa7e7a0d..00000000 --- a/thirdparty/libcsp/include/csp/crypto/csp_sha1.h +++ /dev/null @@ -1,81 +0,0 @@ -/* -Cubesat Space Protocol - A small network-layer protocol designed for Cubesats -Copyright (C) 2012 GomSpace ApS (http://www.gomspace.com) -Copyright (C) 2012 AAUSAT3 Project (http://aausat3.space.aau.dk) - -This library is free software; you can redistribute it and/or -modify it under the terms of the GNU Lesser General Public -License as published by the Free Software Foundation; either -version 2.1 of the License, or (at your option) any later version. - -This library is distributed in the hope that it will be useful, -but WITHOUT ANY WARRANTY; without even the implied warranty of -MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU -Lesser General Public License for more details. - -You should have received a copy of the GNU Lesser General Public -License along with this library; if not, write to the Free Software -Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA -*/ - -#ifndef _CSP_SHA1_H_ -#define _CSP_SHA1_H_ - -#include - -#ifdef __cplusplus -extern "C" { -#endif - -/* The SHA1 block and message digest size in bytes */ -#define SHA1_BLOCKSIZE 64 -#define SHA1_DIGESTSIZE 20 - -/** - SHA1 state structure -*/ -typedef struct { - //! Internal SHA1 state. - uint64_t length; - //! Internal SHA1 state. - uint32_t state[5]; - //! Internal SHA1 state. - uint32_t curlen; - //! Internal SHA1 state. - uint8_t buf[SHA1_BLOCKSIZE]; -} csp_sha1_state; - -/** - * Initialize the hash state - * @param sha1 The hash state you wish to initialize - */ -void csp_sha1_init(csp_sha1_state * sha1); - -/** - * Process a block of memory though the hash - * @param sha1 The hash state - * @param in The data to hash - * @param inlen The length of the data (octets) - */ -void csp_sha1_process(csp_sha1_state * sha1, const uint8_t * in, uint32_t inlen); - -/** - * Terminate the hash to get the digest - * @param sha1 The hash state - * @param out [out] The destination of the hash (20 bytes) - */ -void csp_sha1_done(csp_sha1_state * sha1, uint8_t * out); - -/** - * Calculate SHA1 hash of block of memory. - * @param msg Pointer to message buffer - * @param len Length of message - * @param sha1 Pointer to SHA1 output buffer. Must be 20 bytes or more! - */ -void csp_sha1_memory(const uint8_t * msg, uint32_t len, uint8_t * hash); - -#ifdef __cplusplus -} /* extern "C" */ -#endif - -#endif // _CSP_SHA1_H_ diff --git a/thirdparty/libcsp/include/csp/crypto/csp_xtea.h b/thirdparty/libcsp/include/csp/crypto/csp_xtea.h deleted file mode 100644 index f740b8d5..00000000 --- a/thirdparty/libcsp/include/csp/crypto/csp_xtea.h +++ /dev/null @@ -1,52 +0,0 @@ -/* -Cubesat Space Protocol - A small network-layer protocol designed for Cubesats -Copyright (C) 2012 GomSpace ApS (http://www.gomspace.com) -Copyright (C) 2012 AAUSAT3 Project (http://aausat3.space.aau.dk) - -This library is free software; you can redistribute it and/or -modify it under the terms of the GNU Lesser General Public -License as published by the Free Software Foundation; either -version 2.1 of the License, or (at your option) any later version. - -This library is distributed in the hope that it will be useful, -but WITHOUT ANY WARRANTY; without even the implied warranty of -MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU -Lesser General Public License for more details. - -You should have received a copy of the GNU Lesser General Public -License along with this library; if not, write to the Free Software -Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA -*/ - -#ifndef _CSP_XTEA_H_ -#define _CSP_XTEA_H_ - -#include - -#ifdef __cplusplus -extern "C" { -#endif - -#define CSP_XTEA_IV_LENGTH 8 - -/** - * XTEA encrypt byte array - * @param plain Pointer to plain text - * @param len Length of plain text - * @param iv Initialization vector - */ -int csp_xtea_encrypt(uint8_t * plain, const uint32_t len, uint32_t iv[2]); - -/** - * Decrypt XTEA encrypted byte array - * @param cipher Pointer to cipher text - * @param len Length of plain text - * @param iv Initialization vector - */ -int csp_xtea_decrypt(uint8_t * cipher, const uint32_t len, uint32_t iv[2]); - -#ifdef __cplusplus -} /* extern "C" */ -#endif - -#endif // _CSP_XTEA_H_ diff --git a/thirdparty/libcsp/include/csp/csp.h b/thirdparty/libcsp/include/csp/csp.h deleted file mode 100644 index 6962195b..00000000 --- a/thirdparty/libcsp/include/csp/csp.h +++ /dev/null @@ -1,545 +0,0 @@ -/* -Cubesat Space Protocol - A small network-layer protocol designed for Cubesats -Copyright (C) 2012 Gomspace ApS (http://www.gomspace.com) -Copyright (C) 2012 AAUSAT3 Project (http://aausat3.space.aau.dk) - -This library is free software; you can redistribute it and/or -modify it under the terms of the GNU Lesser General Public -License as published by the Free Software Foundation; either -version 2.1 of the License, or (at your option) any later version. - -This library is distributed in the hope that it will be useful, -but WITHOUT ANY WARRANTY; without even the implied warranty of -MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU -Lesser General Public License for more details. - -You should have received a copy of the GNU Lesser General Public -License along with this library; if not, write to the Free Software -Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA -*/ - -#ifndef _CSP_H_ -#define _CSP_H_ - -#ifdef __cplusplus -extern "C" { -#endif - -/* Includes */ -#include - -#include - -/* CSP includes */ -#include "csp_types.h" -#include "csp_platform.h" -#include "csp_error.h" -#include "csp_debug.h" -#include "csp_buffer.h" -#include "csp_rtable.h" -#include "csp_iflist.h" - -/** csp_init - * Start up the can-space protocol - * @param my_node_address The CSP node address - */ -int csp_init(uint8_t my_node_address); - -/** csp_set_address - * Set the systems own address - * @param addr The new address of the system - */ -void csp_set_address(uint8_t addr); - -/** csp_get_address - * Get the systems own address - * @return The current address of the system - */ -uint8_t csp_get_address(void); - -/** csp_set_hostname - * Set subsystem hostname. - * This function takes a pointer to a string, which should remain static - * @param hostname Hostname to set - */ -void csp_set_hostname(const char *hostname); - -/** csp_get_hostname - * Get current subsystem hostname. - * @return Pointer to char array with current hostname. - */ -const char *csp_get_hostname(void); - -/** csp_set_model - * Set subsystem model name. - * This function takes a pointer to a string, which should remain static - * @param model Model name to set - */ -void csp_set_model(const char *model); - -/** csp_get_model - * Get current model name. - * @return Pointer to char array with current model name. - */ -const char *csp_get_model(void); - -/** csp_set_revision - * Set subsystem revision. This can be used to override the CMP revision field. - * This function takes a pointer to a string, which should remain static - * @param revision Revision name to set - */ -void csp_set_revision(const char *revision); - -/** csp_get_revision - * Get subsystem revision. - * @return Pointer to char array with software revision. - */ -const char *csp_get_revision(void); - -/** csp_socket - * Create CSP socket endpoint - * @param opts Socket options - * @return Pointer to socket on success, NULL on failure - */ -csp_socket_t *csp_socket(uint32_t opts); - -/** - * Wait for a new connection on a socket created by csp_socket - * @param socket Socket to accept connections on - * @param timeout use CSP_MAX_DELAY for infinite timeout - * @return Return pointer to csp_conn_t or NULL if timeout was reached - */ -csp_conn_t *csp_accept(csp_socket_t *socket, uint32_t timeout); - -/** - * Read data from a connection - * This fuction uses the RX queue of a connection to receive a packet - * If no packet is available and a timeout has been specified - * The call will block. - * Do NOT call this from ISR - * @param conn pointer to connection - * @param timeout timeout in ms, use CSP_MAX_DELAY for infinite blocking time - * @return Returns pointer to csp_packet_t, which you MUST free yourself, either by calling csp_buffer_free() or reusing the buffer for a new csp_send. - */ -csp_packet_t *csp_read(csp_conn_t *conn, uint32_t timeout); - -/** - * Send a packet on an already established connection - * @param conn pointer to connection - * @param packet pointer to packet, - * @param timeout a timeout to wait for TX to complete. NOTE: not all underlying drivers supports flow-control. - * @return returns 1 if successful and 0 otherwise. you MUST free the frame yourself if the transmission was not successful. - */ -int csp_send(csp_conn_t *conn, csp_packet_t *packet, uint32_t timeout); - -/** - * Send a packet on an already established connection, and change the default priority of the connection - * - * @note When using this function, the priority of the connection will change. If you need to change it back - * use another call to csp_send_prio, or ensure that all packets sent on a given connection is using send_prio call. - * - * @param prio csp priority - * @param conn pointer to connection - * @param packet pointer to packet, - * @param timeout a timeout to wait for TX to complete. NOTE: not all underlying drivers supports flow-control. - * @return returns 1 if successful and 0 otherwise. you MUST free the frame yourself if the transmission was not successful. - */ -int csp_send_prio(uint8_t prio, csp_conn_t *conn, csp_packet_t *packet, uint32_t timeout); - -/** - * Perform an entire request/reply transaction - * Copies both input buffer and reply to output buffeer. - * Also makes the connection and closes it again - * @param prio CSP Prio - * @param dest CSP Dest - * @param port CSP Port - * @param timeout timeout in ms - * @param outbuf pointer to outgoing data buffer - * @param outlen length of request to send - * @param inbuf pointer to incoming data buffer - * @param inlen length of expected reply, -1 for unknown size (note inbuf MUST be large enough) - * @return Return 1 or reply size if successful, 0 if error or incoming length does not match or -1 if timeout was reached - */ -int csp_transaction(uint8_t prio, uint8_t dest, uint8_t port, uint32_t timeout, void *outbuf, int outlen, void *inbuf, int inlen); - -/** - * Perform an entire request/reply transaction - * Copies both input buffer and reply to output buffeer. - * Also makes the connection and closes it again - * @param prio CSP Prio - * @param dest CSP Dest - * @param port CSP Port - * @param timeout timeout in ms - * @param outbuf pointer to outgoing data buffer - * @param outlen length of request to send - * @param inbuf pointer to incoming data buffer - * @param inlen length of expected reply, -1 for unknown size (note inbuf MUST be large enough) - * @param opts Connection options. - * @return Return 1 or reply size if successful, 0 if error or incoming length does not match or -1 if timeout was reached - */ -int csp_transaction2(uint8_t prio, uint8_t dest, uint8_t port, uint32_t timeout, void *outbuf, int outlen, void *inbuf, int inlen, uint32_t opts); - -/** - * Use an existing connection to perform a transaction, - * This is only possible if the next packet is on the same port and destination! - * @param conn pointer to connection structure - * @param timeout timeout in ms - * @param outbuf pointer to outgoing data buffer - * @param outlen length of request to send - * @param inbuf pointer to incoming data buffer - * @param inlen length of expected reply, -1 for unknown size (note inbuf MUST be large enough) - * @return - */ -int csp_transaction_persistent(csp_conn_t *conn, uint32_t timeout, void *outbuf, int outlen, void *inbuf, int inlen); - -/** - * Read data from a connection-less server socket - * This fuction uses the socket directly to receive a frame - * If no packet is available and a timeout has been specified the call will block. - * Do NOT call this from ISR - * @return Returns pointer to csp_packet_t, which you MUST free yourself, either by calling csp_buffer_free() or reusing the buffer for a new csp_send. - */ -csp_packet_t *csp_recvfrom(csp_socket_t *socket, uint32_t timeout); - -/** - * Send a packet without previously opening a connection - * @param prio CSP_PRIO_x - * @param dest destination node - * @param dport destination port - * @param src_port source port - * @param opts CSP_O_x - * @param packet pointer to packet - * @param timeout timeout used by interfaces with blocking send - * @return -1 if error (you must free packet), 0 if OK (you must discard pointer) - */ -int csp_sendto(uint8_t prio, uint8_t dest, uint8_t dport, uint8_t src_port, uint32_t opts, csp_packet_t *packet, uint32_t timeout); - -/** - * Send a packet as a direct reply to the source of an incoming packet, - * but still without holding an entire connection - * @param request_packet pointer to packet to reply to - * @param reply_packet actual reply data - * @param opts CSP_O_x - * @param timeout timeout used by interfaces with blocking send - * @return -1 if error (you must free packet), 0 if OK (you must discard pointer) - */ -int csp_sendto_reply(csp_packet_t * request_packet, csp_packet_t * reply_packet, uint32_t opts, uint32_t timeout); - -/** csp_connect - * Used to establish outgoing connections - * This function searches the port table for free slots and finds an unused - * connection from the connection pool - * There is no handshake in the CSP protocol - * @param prio Connection priority. - * @param dest Destination address. - * @param dport Destination port. - * @param timeout Timeout in ms. - * @param opts Connection options. - * @return a pointer to a new connection or NULL - */ -csp_conn_t *csp_connect(uint8_t prio, uint8_t dest, uint8_t dport, uint32_t timeout, uint32_t opts); - -/** csp_close - * Closes a given connection and frees buffers used. - * @param conn pointer to connection structure - * @return CSP_ERR_NONE if connection was closed. Otherwise, an err code is returned. - */ -int csp_close(csp_conn_t *conn); - -/** - * @param conn pointer to connection structure - * @return destination port of an incoming connection - */ -int csp_conn_dport(csp_conn_t *conn); - -/** - * @param conn pointer to connection structure - * @return source port of an incoming connection - */ -int csp_conn_sport(csp_conn_t *conn); - -/** - * @param conn pointer to connection structure - * @return destination address of an incoming connection - */ -int csp_conn_dst(csp_conn_t *conn); - -/** - * @param conn pointer to connection structure - * @return source address of an incoming connection - */ -int csp_conn_src(csp_conn_t *conn); - -/** - * @param conn pointer to connection structure - * @return flags field of an incoming connection - */ -int csp_conn_flags(csp_conn_t *conn); - -/** - * Set socket to listen for incoming connections - * @param socket Socket to enable listening on - * @param conn_queue_length Lenght of backlog connection queue - * @return 0 on success, -1 on error. - */ -int csp_listen(csp_socket_t *socket, size_t conn_queue_length); - -/** - * Bind port to socket - * @param socket Socket to bind port to - * @param port Port number to bind - * @return 0 on success, -1 on error. - */ -int csp_bind(csp_socket_t *socket, uint8_t port); - -/** - * Start the router task. - * @param task_stack_size The number of portStackType to allocate. This only affects FreeRTOS systems. - * @param priority The OS task priority of the router - */ -int csp_route_start_task(unsigned int task_stack_size, unsigned int priority); - -/** - * Call the router worker function manually (without the router task) - * This must be run inside a loop or called periodically for the csp router to work. - * Use this function instead of calling and starting the router task. - * @param timeout max blocking time - * @return -1 if no packet was processed, 0 otherwise - */ -int csp_route_work(uint32_t timeout); - -/** - * Start the bridge task. - * @param task_stack_size The number of portStackType to allocate. This only affects FreeRTOS systems. - * @param priority The OS task priority of the router - * @param _if_a pointer to first side - * @param _if_b pointer to second side - * @return CSP_ERR type - */ -int csp_bridge_start(unsigned int task_stack_size, unsigned int task_priority, csp_iface_t * _if_a, csp_iface_t * _if_b); - -/** - * Enable promiscuous mode packet queue - * This function is used to enable promiscuous mode for the router. - * If enabled, a copy of all incoming packets are placed in a queue - * that can be read with csp_promisc_get(). Not all interface drivers - * support promiscuous mode. - * - * @param buf_size Size of buffer for incoming packets - */ -int csp_promisc_enable(unsigned int buf_size); - -/** - * Disable promiscuous mode. - * If the queue was initialised prior to this, it can be re-enabled - * by calling promisc_enable(0) - */ -void csp_promisc_disable(void); - -/** - * Get packet from promiscuous mode packet queue - * Returns the first packet from the promiscuous mode packet queue. - * The queue is FIFO, so the returned packet is the oldest one - * in the queue. - * - * @param timeout Timeout in ms to wait for a new packet - */ -csp_packet_t *csp_promisc_read(uint32_t timeout); - -/** - * Send multiple packets using the simple fragmentation protocol - * CSP will add total size and offset to all packets - * This can be read by the client using the csp_sfp_recv, if the CSP_FFRAG flag is set - * @param conn pointer to connection - * @param data pointer to data to send - * @param totalsize size of data to send - * @param mtu maximum transfer unit - * @param timeout timeout in ms to wait for csp_send() - * @return 0 if OK, -1 if ERR - */ -int csp_sfp_send(csp_conn_t * conn, const void * data, int totalsize, int mtu, uint32_t timeout); - -/** - * Same as csp_sfp_send but with option to supply your own memcpy function. - * This is usefull if you wish to send data stored in flash memory or another location - * @param conn pointer to connection - * @param data pointer to data to send - * @param totalsize size of data to send - * @param mtu maximum transfer unit - * @param timeout timeout in ms to wait for csp_send() - * @param memcpyfcn, pointer to memcpy function - * @return 0 if OK, -1 if ERR - */ -int csp_sfp_send_own_memcpy(csp_conn_t * conn, const void * data, int totalsize, int mtu, uint32_t timeout, void * (*memcpyfcn)(void *, const void *, size_t)); - -/** - * This is the counterpart to the csp_sfp_send function - * @param conn pointer to active conn, on which you expect to receive sfp packed data - * @param dataout pointer to NULL pointer, whill be overwritten with malloc pointer - * @param datasize actual size of received data - * @param timeout timeout in ms to wait for csp_recv() - * @return 0 if OK, -1 if ERR - */ -int csp_sfp_recv(csp_conn_t * conn, void ** dataout, int * datasize, uint32_t timeout); - -/** - * This is the counterpart to the csp_sfp_send function - * @param conn pointer to active conn, on which you expect to receive sfp packed data - * @param dataout pointer to NULL pointer, whill be overwritten with malloc pointer - * @param datasize actual size of received data - * @param timeout timeout in ms to wait for csp_recv() - * @param first_packet This is a pointer to the first SFP packet (previously received with csp_read) - * @return 0 if OK, -1 if ERR - */ -int csp_sfp_recv_fp(csp_conn_t * conn, void ** dataout, int * datasize, uint32_t timeout, csp_packet_t * first_packet); - -/** - * If the given packet is a service-request (that is uses one of the csp service ports) - * it will be handled according to the CSP service handler. - * This function will either use the packet buffer or delete it, - * so this function is typically called in the last "default" clause of - * a switch/case statement in a csp_listener task. - * In order to listen to csp service ports, bind your listener to the CSP_ANY port. - * This function may only be called from task context. - * @param conn Pointer to the new connection - * @param packet Pointer to the first packet, obtained by using csp_read() - */ -void csp_service_handler(csp_conn_t *conn, csp_packet_t *packet); - -/** - * Send a single ping/echo packet - * @param node node id - * @param timeout timeout in ms - * @param size size of packet to transmit - * @param conn_options csp connection options - * @return >0 = Echo time in ms, -1 = ERR - */ -int csp_ping(uint8_t node, uint32_t timeout, unsigned int size, uint8_t conn_options); - -/** - * Send a single ping/echo packet without waiting for reply - * @param node node id - */ -void csp_ping_noreply(uint8_t node); - -/** - * Request process list. - * @note This is only available for FreeRTOS systems - * @param node node id - * @param timeout timeout in ms - */ -void csp_ps(uint8_t node, uint32_t timeout); - -/** - * Request amount of free memory - * @param node node id - * @param timeout timeout in ms - */ -void csp_memfree(uint8_t node, uint32_t timeout); - -/** - * Request number of free buffer elements - * @param node node id - * @param timeout timeout in ms - */ -void csp_buf_free(uint8_t node, uint32_t timeout); - -/** - * Reboot subsystem - * @param node node id - */ -void csp_reboot(uint8_t node); - -/** - * Shutdown subsystem - * @param node node id - */ -void csp_shutdown(uint8_t node); - -/** - * Request subsystem uptime - * @param node node id - * @param timeout timeout in ms - */ -void csp_uptime(uint8_t node, uint32_t timeout); - -/** - * Set RDP options - * @param window_size Window size - * @param conn_timeout_ms Connection timeout in ms - * @param packet_timeout_ms Packet timeout in ms - * @param delayed_acks Enable/disable delayed acknowledgements - * @param ack_timeout Acknowledgement timeout when delayed ACKs is enabled - * @param ack_delay_count Send acknowledgement for every ack_delay_count packets - */ -void csp_rdp_set_opt(unsigned int window_size, unsigned int conn_timeout_ms, - unsigned int packet_timeout_ms, unsigned int delayed_acks, - unsigned int ack_timeout, unsigned int ack_delay_count); - -/** - * Get RDP options - * @param window_size Window size - * @param conn_timeout_ms Connection timeout in ms - * @param packet_timeout_ms Packet timeout in ms - * @param delayed_acks Enable/disable delayed acknowledgements - * @param ack_timeout Acknowledgement timeout when delayed ACKs is enabled - * @param ack_delay_count Send acknowledgement for every ack_delay_count packets - */ -void csp_rdp_get_opt(unsigned int *window_size, unsigned int *conn_timeout_ms, - unsigned int *packet_timeout_ms, unsigned int *delayed_acks, - unsigned int *ack_timeout, unsigned int *ack_delay_count); - -/** - * Set XTEA key - * @param key Pointer to key array - * @param keylen Length of key - * @return 0 if key was successfully set, -1 otherwise - */ -int csp_xtea_set_key(char *key, uint32_t keylen); - -/** - * Set HMAC key - * @param key Pointer to key array - * @param keylen Length of key - * @return 0 if key was successfully set, -1 otherwise - */ -int csp_hmac_set_key(char *key, uint32_t keylen); - -/** - * Print connection table - */ -void csp_conn_print_table(void); -int csp_conn_print_table_str(char * str_buf, int str_size); - -/** - * Print buffer usage table - */ -void csp_buffer_print_table(void); - -/** - * Hex dump to stdout - */ -void csp_hex_dump(const char *desc, void *addr, int len); - -#ifdef __AVR__ -typedef uint32_t csp_memptr_t; -#else -typedef void * csp_memptr_t; -#endif - -typedef csp_memptr_t (*csp_memcpy_fnc_t)(csp_memptr_t, const csp_memptr_t, size_t); -void csp_cmp_set_memcpy(csp_memcpy_fnc_t fnc); - -/** - * Set csp_debug hook function - * @param f Hook function - */ -#include -typedef void (*csp_debug_hook_func_t)(csp_debug_level_t level, const char *format, va_list args); -void csp_debug_hook_set(csp_debug_hook_func_t f); - -#ifdef __cplusplus -} /* extern "C" */ -#endif - -#endif // _CSP_H_ diff --git a/thirdparty/libcsp/include/csp/csp_autoconfig.h b/thirdparty/libcsp/include/csp/csp_autoconfig.h deleted file mode 100644 index 703f3754..00000000 --- a/thirdparty/libcsp/include/csp/csp_autoconfig.h +++ /dev/null @@ -1,48 +0,0 @@ -/* - * csp_autoconfig.h - * - * Created on: 20.11.2020 - * Author: jakob - */ - -#ifndef GOMSPACE_LIBCSP_INCLUDE_CSP_CSP_AUTOCONFIG_H_ -#define GOMSPACE_LIBCSP_INCLUDE_CSP_CSP_AUTOCONFIG_H_ - -#define ENABLE_NANOPOWER2_CLIENT 1 -#define GIT_REV "unknown" -/* #undef CSP_FREERTOS */ -#define CSP_POSIX 1 -/* #undef CSP_WINDOWS */ -/* #undef CSP_MACOSX */ -#define HAVE_LIBZMQ 1 -#define CSP_DEBUG 1 -#define CSP_USE_RDP 1 -#define CSP_USE_CRC32 1 -#define CSP_USE_HMAC 1 -#define CSP_USE_XTEA 1 -#define CSP_USE_PROMISC 1 -#define CSP_USE_QOS 1 -/* #undef CSP_USE_DEDUP */ -/* #undef CSP_USE_INIT_SHUTDOWN */ -#define CSP_USE_CAN 1 -/* #define CSP_USE_I2C 1 */ -/* #define CSP_USE_KISS 1 */ -/* #define CSP_USE_ZMQHUB 1 */ -#define CSP_CONN_MAX 10 -#define CSP_CONN_QUEUE_LENGTH 100 -#define CSP_FIFO_INPUT 100 -#define CSP_MAX_BIND_PORT 31 -#define CSP_RDP_MAX_WINDOW 20 -#define CSP_PADDING_BYTES 8 -#define CSP_CONNECTION_SO 64 -#define CSP_LOG_LEVEL_DEBUG 1 -#define CSP_LOG_LEVEL_INFO 1 -#define CSP_LOG_LEVEL_WARN 1 -#define CSP_LOG_LEVEL_ERROR 1 -#define CSP_LITTLE_ENDIAN 1 -/* #undef CSP_BIG_ENDIAN */ -#define CSP_HAVE_STDBOOL_H 1 -/* #define CSP_HAVE_LIBSOCKETCAN 1 */ -#define LIBCSP_VERSION "1.5" - -#endif /* GOMSPACE_LIBCSP_INCLUDE_CSP_CSP_AUTOCONFIG_H_ */ diff --git a/thirdparty/libcsp/include/csp/csp_buffer.h b/thirdparty/libcsp/include/csp/csp_buffer.h deleted file mode 100644 index 9ed6df77..00000000 --- a/thirdparty/libcsp/include/csp/csp_buffer.h +++ /dev/null @@ -1,92 +0,0 @@ -/* -Cubesat Space Protocol - A small network-layer protocol designed for Cubesats -Copyright (C) 2012 Gomspace ApS (http://www.gomspace.com) -Copyright (C) 2012 AAUSAT3 Project (http://aausat3.space.aau.dk) - -This library is free software; you can redistribute it and/or -modify it under the terms of the GNU Lesser General Public -License as published by the Free Software Foundation; either -version 2.1 of the License, or (at your option) any later version. - -This library is distributed in the hope that it will be useful, -but WITHOUT ANY WARRANTY; without even the implied warranty of -MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU -Lesser General Public License for more details. - -You should have received a copy of the GNU Lesser General Public -License along with this library; if not, write to the Free Software -Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA -*/ - -#ifndef _CSP_BUFFER_H_ -#define _CSP_BUFFER_H_ - -#ifdef __cplusplus -extern "C" { -#endif - -/** - * Start the buffer handling system - * You must specify the number for buffers and the size. All buffers are fixed - * size so you must specify the size of your largest buffer. - * - * @param count Number of buffers to allocate - * @param size Buffer size in bytes. - * - * @return CSP_ERR_NONE if malloc() succeeded, CSP_ERR message otherwise. - */ -int csp_buffer_init(int count, int size); - -/** - * Get a reference to a free buffer. This function can only be called - * from task context. - * - * @param size Specify what data-size you will put in the buffer - * @return pointer to a free csp_packet_t or NULL if out of memory - */ -void * csp_buffer_get(size_t size); - -/** - * Get a reference to a free buffer. This function can only be called - * from interrupt context. - * - * @param buf_size Specify what data-size you will put in the buffer - * @return pointer to a free csp_packet_t or NULL if out of memory - */ -void * csp_buffer_get_isr(size_t buf_size); - -/** - * Free a buffer after use. - * @param packet pointer to memory area, must be acquired by csp_buffer_get(). - */ -void csp_buffer_free(void *packet); - -/** - * Free a buffer after use in ISR context. - * @param packet pointer to memory area, must be acquired by csp_buffer_get(). - */ -void csp_buffer_free_isr(void *packet); - -/** - * Clone an existing packet and increase/decrease cloned packet size. - * @param buffer Existing buffer to clone. - */ -void * csp_buffer_clone(void *buffer); - -/** - * Return how many buffers that are currently free. - * @return number of free buffers - */ -int csp_buffer_remaining(void); - -/** - * Return the size of the CSP buffers - * @return size of CSP buffers - */ -int csp_buffer_size(void); - -#ifdef __cplusplus -} /* extern "C" */ -#endif - -#endif /* _CSP_BUFFER_H_ */ diff --git a/thirdparty/libcsp/include/csp/csp_cmp.h b/thirdparty/libcsp/include/csp/csp_cmp.h deleted file mode 100644 index 114a8eab..00000000 --- a/thirdparty/libcsp/include/csp/csp_cmp.h +++ /dev/null @@ -1,189 +0,0 @@ -/* -Cubesat Space Protocol - A small network-layer protocol designed for Cubesats -Copyright (C) 2012 Gomspace ApS (http://www.gomspace.com) -Copyright (C) 2012 AAUSAT3 Project (http://aausat3.space.aau.dk) - -This library is free software; you can redistribute it and/or -modify it under the terms of the GNU Lesser General Public -License as published by the Free Software Foundation; either -version 2.1 of the License, or (at your option) any later version. - -This library is distributed in the hope that it will be useful, -but WITHOUT ANY WARRANTY; without even the implied warranty of -MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU -Lesser General Public License for more details. - -You should have received a copy of the GNU Lesser General Public -License along with this library; if not, write to the Free Software -Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA -*/ - -#ifndef _CSP_CMP_H_ -#define _CSP_CMP_H_ - -/** - @file - CSP management protocol (CMP). -*/ - -#ifdef __cplusplus -extern "C" { -#endif - -#include -#include -#include - -/** - CMP type. - @{ -*/ -/** - CMP request. -*/ -#define CSP_CMP_REQUEST 0x00 -/** - CMP reply. -*/ -#define CSP_CMP_REPLY 0xff -/**@}*/ - -/** - CMP requests. - @{ -*/ -/** - CMP request codes. -*/ -/** - Request identification, compile time, revision, name. -*/ -#define CSP_CMP_IDENT 1 -/** - Set/configure routing. -*/ -#define CSP_CMP_ROUTE_SET 2 -/** - Request interface statistics. -*/ -#define CSP_CMP_IF_STATS 3 -/** - Peek/read data from memory. -*/ -#define CSP_CMP_PEEK 4 -/** - Poke/write data from memory. -*/ -#define CSP_CMP_POKE 5 -/** - Get/set clock. -*/ -#define CSP_CMP_CLOCK 6 -/**@}*/ - -/** - CMP identification - max revision length. -*/ -#define CSP_CMP_IDENT_REV_LEN 20 -/** - CMP identification - max date length. -*/ -#define CSP_CMP_IDENT_DATE_LEN 12 -/** - CMP identification - max time length. -*/ -#define CSP_CMP_IDENT_TIME_LEN 9 - -/** - CMP interface statistics - max interface name length. -*/ -#define CSP_CMP_ROUTE_IFACE_LEN 11 - -/** - CMP peek/read memeory - max read length. -*/ -#define CSP_CMP_PEEK_MAX_LEN 200 - -/** - CMP poke/write memeory - max write length. -*/ -#define CSP_CMP_POKE_MAX_LEN 200 - -/** - CSP management protocol description. -*/ -struct csp_cmp_message { - //! CMP request type. - uint8_t type; - //! CMP request code. - uint8_t code; - union { - struct { - char hostname[CSP_HOSTNAME_LEN]; - char model[CSP_MODEL_LEN]; - char revision[CSP_CMP_IDENT_REV_LEN]; - char date[CSP_CMP_IDENT_DATE_LEN]; - char time[CSP_CMP_IDENT_TIME_LEN]; - } ident; - struct { - uint8_t dest_node; - uint8_t next_hop_mac; - char interface[CSP_CMP_ROUTE_IFACE_LEN]; - } route_set; - struct __attribute__((__packed__)) { - char interface[CSP_CMP_ROUTE_IFACE_LEN]; - uint32_t tx; - uint32_t rx; - uint32_t tx_error; - uint32_t rx_error; - uint32_t drop; - uint32_t autherr; - uint32_t frame; - uint32_t txbytes; - uint32_t rxbytes; - uint32_t irq; - } if_stats; - struct { - uint32_t addr; - uint8_t len; - char data[CSP_CMP_PEEK_MAX_LEN]; - } peek; - struct { - uint32_t addr; - uint8_t len; - char data[CSP_CMP_POKE_MAX_LEN]; - } poke; - csp_timestamp_t clock; - }; -} __attribute__ ((packed)); - -/** - Macro for calculating size of management message. -*/ -#define CMP_SIZE(_memb) (sizeof(((struct csp_cmp_message *)0)->_memb) + sizeof(uint8_t) + sizeof(uint8_t)) - -/** - Generic send management message request. -*/ -int csp_cmp(uint8_t node, uint32_t timeout, uint8_t code, int membsize, struct csp_cmp_message *msg); - -/** - Macro for defining management handling function. -*/ -#define CMP_MESSAGE(_code, _memb) \ -static inline int csp_cmp_##_memb(uint8_t node, uint32_t timeout, struct csp_cmp_message *msg) { \ - return csp_cmp(node, timeout, _code, CMP_SIZE(_memb), msg); \ -} - -CMP_MESSAGE(CSP_CMP_IDENT, ident) -CMP_MESSAGE(CSP_CMP_ROUTE_SET, route_set) -CMP_MESSAGE(CSP_CMP_IF_STATS, if_stats) -CMP_MESSAGE(CSP_CMP_PEEK, peek) -CMP_MESSAGE(CSP_CMP_POKE, poke) -CMP_MESSAGE(CSP_CMP_CLOCK, clock) - -#ifdef __cplusplus -} /* extern "C" */ -#endif - -#endif // _CSP_CMP_H_ diff --git a/thirdparty/libcsp/include/csp/csp_crc32.h b/thirdparty/libcsp/include/csp/csp_crc32.h deleted file mode 100644 index a474eaf8..00000000 --- a/thirdparty/libcsp/include/csp/csp_crc32.h +++ /dev/null @@ -1,63 +0,0 @@ -/* -Cubesat Space Protocol - A small network-layer protocol designed for Cubesats -Copyright (C) 2012 GomSpace ApS (http://www.gomspace.com) -Copyright (C) 2012 AAUSAT3 Project (http://aausat3.space.aau.dk) - -This library is free software; you can redistribute it and/or -modify it under the terms of the GNU Lesser General Public -License as published by the Free Software Foundation; either -version 2.1 of the License, or (at your option) any later version. - -This library is distributed in the hope that it will be useful, -but WITHOUT ANY WARRANTY; without even the implied warranty of -MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU -Lesser General Public License for more details. - -You should have received a copy of the GNU Lesser General Public -License along with this library; if not, write to the Free Software -Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA -*/ - -#ifndef _CSP_CRC32_H_ -#define _CSP_CRC32_H_ - -#include - -#ifdef __cplusplus -extern "C" { -#endif - -/** - * Generate precomputed CRC32 table - */ -void csp_crc32_gentab(void); - -/** - * Append CRC32 checksum to packet - * @param packet Packet to append checksum - * @param include_header use header in calculation (this will not modify the flags field) - * @return 0 on success, -1 on error - */ -int csp_crc32_append(csp_packet_t * packet, bool include_header); - -/** - * Verify CRC32 checksum on packet - * @param packet Packet to verify - * @param include_header use header in calculation (this will not modify the flags field) - * @return 0 if checksum is valid, -1 otherwise - */ -int csp_crc32_verify(csp_packet_t * packet, bool include_header); - -/** - * Calculate checksum for a given memory area - * @param data pointer to memory - * @param length length of memory to do checksum on - * @return return uint32_t checksum - */ -uint32_t csp_crc32_memory(const uint8_t * data, uint32_t length); - -#ifdef __cplusplus -} /* extern "C" */ -#endif - -#endif /* _CSP_CRC32_H_ */ diff --git a/thirdparty/libcsp/include/csp/csp_debug.h b/thirdparty/libcsp/include/csp/csp_debug.h deleted file mode 100644 index 64429d49..00000000 --- a/thirdparty/libcsp/include/csp/csp_debug.h +++ /dev/null @@ -1,150 +0,0 @@ -/* -Cubesat Space Protocol - A small network-layer protocol designed for Cubesats -Copyright (C) 2012 Gomspace ApS (http://www.gomspace.com) -Copyright (C) 2012 AAUSAT3 Project (http://aausat3.space.aau.dk) - -This library is free software; you can redistribute it and/or -modify it under the terms of the GNU Lesser General Public -License as published by the Free Software Foundation; either -version 2.1 of the License, or (at your option) any later version. - -This library is distributed in the hope that it will be useful, -but WITHOUT ANY WARRANTY; without even the implied warranty of -MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU -Lesser General Public License for more details. - -You should have received a copy of the GNU Lesser General Public -License along with this library; if not, write to the Free Software -Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA -*/ - -#ifndef _CSP_DEBUG_H_ -#define _CSP_DEBUG_H_ - -#include -#include - -#ifdef __cplusplus -extern "C" { -#endif - -/** Debug levels */ -typedef enum { - CSP_ERROR = 0, - CSP_WARN = 1, - CSP_INFO = 2, - CSP_BUFFER = 3, - CSP_PACKET = 4, - CSP_PROTOCOL = 5, - CSP_LOCK = 6, -} csp_debug_level_t; - -/* Extract filename component from path */ -#define BASENAME(_file) ((strrchr(_file, '/') ? : (strrchr(_file, '\\') ? : _file)) + 1) - -/* Implement csp_assert_fail_action to override default failure action */ -extern void __attribute__((weak)) csp_assert_fail_action(const char *assertion, const char *file, int line); - -#ifndef NDEBUG - #define csp_assert(exp) \ - do { \ - if (!(exp)) { \ - const char *assertion = #exp; \ - const char *file = BASENAME(__FILE__); \ - int line = __LINE__; \ - printf("\E[1;31m[%02" PRIu8 "] Assertion \'%s\' failed in %s:%d\E[0m\r\n", \ - csp_get_address(), assertion, file, line); \ - if (csp_assert_fail_action) \ - csp_assert_fail_action(assertion, file, line); \ - } \ - } while (0) -#else - #define csp_assert(...) do {} while (0) -#endif - -#ifdef __AVR__ - #include - #include - #define CONSTSTR(data) PSTR(data) - #undef printf - #undef sscanf - #undef scanf - #undef sprintf - #undef snprintf - #define printf(s, ...) printf_P(PSTR(s), ## __VA_ARGS__) - #define sscanf(buf, s, ...) sscanf_P(buf, PSTR(s), ## __VA_ARGS__) - #define scanf(s, ...) scanf_P(PSTR(s), ## __VA_ARGS__) - #define sprintf(buf, s, ...) sprintf_P(buf, PSTR(s), ## __VA_ARGS__) - #define snprintf(buf, size, s, ...) snprintf_P(buf, size, PSTR(s), ## __VA_ARGS__) -#else - #define CONSTSTR(data) data -#endif - -#ifdef CSP_DEBUG - #define csp_debug(level, format, ...) do { do_csp_debug(level, CONSTSTR(format), ##__VA_ARGS__); } while(0) -#else - #define csp_debug(...) do {} while (0) -#endif - -#ifdef CSP_LOG_LEVEL_ERROR - #define csp_log_error(format, ...) csp_debug(CSP_ERROR, format, ##__VA_ARGS__) -#else - #define csp_log_error(...) do {} while (0) -#endif - -#ifdef CSP_LOG_LEVEL_WARN - #define csp_log_warn(format, ...) csp_debug(CSP_WARN, format, ##__VA_ARGS__) -#else - #define csp_log_warn(...) do {} while (0) -#endif - -#ifdef CSP_LOG_LEVEL_INFO - #define csp_log_info(format, ...) csp_debug(CSP_INFO, format, ##__VA_ARGS__) -#else - #define csp_log_info(...) do {} while (0) -#endif - -#ifdef CSP_LOG_LEVEL_DEBUG - #define csp_log_buffer(format, ...) csp_debug(CSP_BUFFER, format, ##__VA_ARGS__) - #define csp_log_packet(format, ...) csp_debug(CSP_PACKET, format, ##__VA_ARGS__) - #define csp_log_protocol(format, ...) csp_debug(CSP_PROTOCOL, format, ##__VA_ARGS__) - #define csp_log_lock(format, ...) csp_debug(CSP_LOCK, format, ##__VA_ARGS__) -#else - #define csp_log_buffer(...) do {} while (0) - #define csp_log_packet(...) do {} while (0) - #define csp_log_protocol(...) do {} while (0) - #define csp_log_lock(...) do {} while (0) -#endif - -/** - * This function should not be used directly, use csp_log_() macro instead - * @param level - * @param format - */ -void do_csp_debug(csp_debug_level_t level, const char *format, ...); - -/** - * Toggle debug level on/off - * @param level Level to toggle - */ -void csp_debug_toggle_level(csp_debug_level_t level); - -/** - * Set debug level - * @param level Level to set - * @param value New level value - */ -void csp_debug_set_level(csp_debug_level_t level, bool value); - -/** - * Get current debug level value - * @param level Level value to get - * @return Level value - */ -int csp_debug_get_level(csp_debug_level_t level); - -#ifdef __cplusplus -} /* extern "C" */ -#endif - -#endif // _CSP_DEBUG_H_ diff --git a/thirdparty/libcsp/include/csp/csp_endian.h b/thirdparty/libcsp/include/csp/csp_endian.h deleted file mode 100644 index e63a73c2..00000000 --- a/thirdparty/libcsp/include/csp/csp_endian.h +++ /dev/null @@ -1,170 +0,0 @@ -/* -Cubesat Space Protocol - A small network-layer protocol designed for Cubesats -Copyright (C) 2012 GomSpace ApS (http://www.gomspace.com) -Copyright (C) 2012 AAUSAT3 Project (http://aausat3.space.aau.dk) - -This library is free software; you can redistribute it and/or -modify it under the terms of the GNU Lesser General Public -License as published by the Free Software Foundation; either -version 2.1 of the License, or (at your option) any later version. - -This library is distributed in the hope that it will be useful, -but WITHOUT ANY WARRANTY; without even the implied warranty of -MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU -Lesser General Public License for more details. - -You should have received a copy of the GNU Lesser General Public -License along with this library; if not, write to the Free Software -Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA -*/ - -#ifndef _CSP_ENDIAN_H_ -#define _CSP_ENDIAN_H_ - -#ifdef __cplusplus -extern "C" { -#endif - -#include - -/** - * Convert 16-bit integer from host byte order to network byte order - * @param h16 Host byte order 16-bit integer - */ -uint16_t csp_hton16(uint16_t h16); - -/** - * Convert 16-bit integer from host byte order to host byte order - * @param n16 Network byte order 16-bit integer - */ -uint16_t csp_ntoh16(uint16_t n16); - -/** - * Convert 32-bit integer from host byte order to network byte order - * @param h32 Host byte order 32-bit integer - */ -uint32_t csp_hton32(uint32_t h32); - -/** - * Convert 32-bit integer from host byte order to host byte order - * @param n32 Network byte order 32-bit integer - */ -uint32_t csp_ntoh32(uint32_t n32); - -/** - * Convert 64-bit integer from host byte order to network byte order - * @param h64 Host byte order 64-bit integer - */ -uint64_t csp_hton64(uint64_t h64); - -/** - * Convert 64-bit integer from host byte order to host byte order - * @param n64 Network byte order 64-bit integer - */ -uint64_t csp_ntoh64(uint64_t n64); - -/** - * Convert 16-bit integer from host byte order to big endian byte order - * @param h16 Host byte order 16-bit integer - */ -uint16_t csp_htobe16(uint16_t h16); - -/** - * Convert 16-bit integer from host byte order to little endian byte order - * @param h16 Host byte order 16-bit integer - */ -uint16_t csp_htole16(uint16_t h16); - -/** - * Convert 16-bit integer from big endian byte order to little endian byte order - * @param be16 Big endian byte order 16-bit integer - */ -uint16_t csp_betoh16(uint16_t be16); - -/** - * Convert 16-bit integer from little endian byte order to host byte order - * @param le16 Little endian byte order 16-bit integer - */ -uint16_t csp_letoh16(uint16_t le16); - -/** - * Convert 32-bit integer from host byte order to big endian byte order - * @param h32 Host byte order 32-bit integer - */ -uint32_t csp_htobe32(uint32_t h32); - -/** - * Convert 32-bit integer from little endian byte order to host byte order - * @param h32 Host byte order 32-bit integer - */ -uint32_t csp_htole32(uint32_t h32); - -/** - * Convert 32-bit integer from big endian byte order to host byte order - * @param be32 Big endian byte order 32-bit integer - */ -uint32_t csp_betoh32(uint32_t be32); - -/** - * Convert 32-bit integer from little endian byte order to host byte order - * @param le32 Little endian byte order 32-bit integer - */ -uint32_t csp_letoh32(uint32_t le32); - -/** - * Convert 64-bit integer from host byte order to big endian byte order - * @param h64 Host byte order 64-bit integer - */ -uint64_t csp_htobe64(uint64_t h64); - -/** - * Convert 64-bit integer from host byte order to little endian byte order - * @param h64 Host byte order 64-bit integer - */ -uint64_t csp_htole64(uint64_t h64); - -/** - * Convert 64-bit integer from big endian byte order to host byte order - * @param be64 Big endian byte order 64-bit integer - */ -uint64_t csp_betoh64(uint64_t be64); - -/** - * Convert 64-bit integer from little endian byte order to host byte order - * @param le64 Little endian byte order 64-bit integer - */ -uint64_t csp_letoh64(uint64_t le64); - -/** - * Convert float from host to network byte order - * @param f Float in host order - * @return Float in network order - */ -float csp_htonflt(float f); - -/** - * Convert float from network to host byte order - * @param f Float in network order - * @return Float in host order - */ -float csp_ntohflt(float f); - -/** - * Convert double from host to network byte order - * @param d Double in host order - * @return Double in network order - */ -double csp_htondbl(double d); - -/** - * Convert double from network to host order - * @param d Double in network order - * @return Double in host order - */ -double csp_ntohdbl(double d); - -#ifdef __cplusplus -} /* extern "C" */ -#endif - -#endif // _CSP_ENDIAN_H_ diff --git a/thirdparty/libcsp/include/csp/csp_error.h b/thirdparty/libcsp/include/csp/csp_error.h deleted file mode 100644 index 31866607..00000000 --- a/thirdparty/libcsp/include/csp/csp_error.h +++ /dev/null @@ -1,50 +0,0 @@ -/* -Cubesat Space Protocol - A small network-layer protocol designed for Cubesats -Copyright (C) 2012 GomSpace ApS (http://www.gomspace.com) -Copyright (C) 2012 AAUSAT3 Project (http://aausat3.space.aau.dk) - -This library is free software; you can redistribute it and/or -modify it under the terms of the GNU Lesser General Public -License as published by the Free Software Foundation; either -version 2.1 of the License, or (at your option) any later version. - -This library is distributed in the hope that it will be useful, -but WITHOUT ANY WARRANTY; without even the implied warranty of -MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU -Lesser General Public License for more details. - -You should have received a copy of the GNU Lesser General Public -License along with this library; if not, write to the Free Software -Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA -*/ - -#ifndef _CSP_ERROR_H_ -#define _CSP_ERROR_H_ - -#ifdef __cplusplus -extern "C" { -#endif - -#define CSP_ERR_NONE 0 /* No error */ -#define CSP_ERR_NOMEM -1 /* Not enough memory */ -#define CSP_ERR_INVAL -2 /* Invalid argument */ -#define CSP_ERR_TIMEDOUT -3 /* Operation timed out */ -#define CSP_ERR_USED -4 /* Resource already in use */ -#define CSP_ERR_NOTSUP -5 /* Operation not supported */ -#define CSP_ERR_BUSY -6 /* Device or resource busy */ -#define CSP_ERR_ALREADY -7 /* Connection already in progress */ -#define CSP_ERR_RESET -8 /* Connection reset */ -#define CSP_ERR_NOBUFS -9 /* No more buffer space available */ -#define CSP_ERR_TX -10 /* Transmission failed */ -#define CSP_ERR_DRIVER -11 /* Error in driver layer */ -#define CSP_ERR_AGAIN -12 /* Resource temporarily unavailable */ - -#define CSP_ERR_HMAC -100 /* HMAC failed */ -#define CSP_ERR_XTEA -101 /* XTEA failed */ -#define CSP_ERR_CRC32 -102 /* CRC32 failed */ - -#ifdef __cplusplus -} /* extern "C" */ -#endif - -#endif // _CSP_ERROR_H_ diff --git a/thirdparty/libcsp/include/csp/csp_iflist.h b/thirdparty/libcsp/include/csp/csp_iflist.h deleted file mode 100644 index 55875657..00000000 --- a/thirdparty/libcsp/include/csp/csp_iflist.h +++ /dev/null @@ -1,56 +0,0 @@ -/* -Cubesat Space Protocol - A small network-layer protocol designed for Cubesats -Copyright (C) 2012 GomSpace ApS (http://www.gomspace.com) -Copyright (C) 2012 AAUSAT3 Project (http://aausat3.space.aau.dk) - -This library is free software; you can redistribute it and/or -modify it under the terms of the GNU Lesser General Public -License as published by the Free Software Foundation; either -version 2.1 of the License, or (at your option) any later version. - -This library is distributed in the hope that it will be useful, -but WITHOUT ANY WARRANTY; without even the implied warranty of -MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU -Lesser General Public License for more details. - -You should have received a copy of the GNU Lesser General Public -License along with this library; if not, write to the Free Software -Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA -*/ - -#ifndef CSP_IFLIST_H_ -#define CSP_IFLIST_H_ - -#include - -#ifdef __cplusplus -extern "C" { -#endif - -/** - * Add interface to list - * @param ifc Pointer to interface to add - */ -void csp_iflist_add(csp_iface_t *ifc); - -/** - * Lookup interface by name - * @param name String with interface name - * @return Pointer to interface or NULL if not found - */ -csp_iface_t * csp_iflist_get_by_name(const char *name); - -/** - * Print list of interfaces to stdout - */ -void csp_iflist_print(void); - -/** - * Return list of registered interfaces. - */ -csp_iface_t * csp_iflist_get(void); - -#ifdef __cplusplus -} -#endif -#endif /* CSP_IFLIST_H_ */ diff --git a/thirdparty/libcsp/include/csp/csp_interface.h b/thirdparty/libcsp/include/csp/csp_interface.h deleted file mode 100644 index 8f04bee3..00000000 --- a/thirdparty/libcsp/include/csp/csp_interface.h +++ /dev/null @@ -1,54 +0,0 @@ -/* -Cubesat Space Protocol - A small network-layer protocol designed for Cubesats -Copyright (C) 2012 Gomspace ApS (http://www.gomspace.com) -Copyright (C) 2012 AAUSAT3 Project (http://aausat3.space.aau.dk) - -This library is free software; you can redistribute it and/or -modify it under the terms of the GNU Lesser General Public -License as published by the Free Software Foundation; either -version 2.1 of the License, or (at your option) any later version. - -This library is distributed in the hope that it will be useful, -but WITHOUT ANY WARRANTY; without even the implied warranty of -MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU -Lesser General Public License for more details. - -You should have received a copy of the GNU Lesser General Public -License along with this library; if not, write to the Free Software -Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA -*/ - -#ifndef _CSP_INTERFACE_H_ -#define _CSP_INTERFACE_H_ - -#ifdef __cplusplus -extern "C" { -#endif - -#include - -/** - * Inputs a new packet into the system - * This function is called from interface drivers ISR to route and accept packets. - * But it can also be called from a task, provided that the pxTaskWoken parameter is NULL! - * - * EXTREMELY IMPORTANT: - * pxTaskWoken arg must ALWAYS be NULL if called from task, - * and ALWAYS be NON NULL if called from ISR! - * If this condition is met, this call is completely thread-safe - * - * This function is fire and forget, it returns void, meaning - * that a packet will always be either accepted or dropped - * so the memory will always be freed. - * - * @param packet A pointer to the incoming packet - * @param interface A pointer to the incoming interface TX function. - * @param pxTaskWoken This must be a pointer a valid variable if called from ISR or NULL otherwise! - */ -void csp_qfifo_write(csp_packet_t *packet, csp_iface_t *interface, CSP_BASE_TYPE *pxTaskWoken); - -#ifdef __cplusplus -} /* extern "C" */ -#endif - -#endif // _CSP_INTERFACE_H_ diff --git a/thirdparty/libcsp/include/csp/csp_platform.h b/thirdparty/libcsp/include/csp/csp_platform.h deleted file mode 100644 index b33292e9..00000000 --- a/thirdparty/libcsp/include/csp/csp_platform.h +++ /dev/null @@ -1,56 +0,0 @@ -/* -Cubesat Space Protocol - A small network-layer protocol designed for Cubesats -Copyright (C) 2012 Gomspace ApS (http://www.gomspace.com) -Copyright (C) 2012 AAUSAT3 Project (http://aausat3.space.aau.dk) - -This library is free software; you can redistribute it and/or -modify it under the terms of the GNU Lesser General Public -License as published by the Free Software Foundation; either -version 2.1 of the License, or (at your option) any later version. - -This library is distributed in the hope that it will be useful, -but WITHOUT ANY WARRANTY; without even the implied warranty of -MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU -Lesser General Public License for more details. - -You should have received a copy of the GNU Lesser General Public -License along with this library; if not, write to the Free Software -Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA -*/ - -#ifndef _CSP_PLATFORM_H_ -#define _CSP_PLATFORM_H_ - -#ifdef __cplusplus -extern "C" { -#endif - -#include - -/* Set OS */ -#if defined(CSP_POSIX) || defined(CSP_WINDOWS) || defined(CSP_MACOSX) - #define CSP_BASE_TYPE int - #define CSP_MAX_DELAY (UINT32_MAX) - #define CSP_INFINITY (UINT32_MAX) - #define CSP_DEFINE_CRITICAL(lock) static csp_bin_sem_handle_t lock - #define CSP_INIT_CRITICAL(lock) ({(csp_bin_sem_create(&lock) == CSP_SEMAPHORE_OK) ? CSP_ERR_NONE : CSP_ERR_NOMEM;}) - #define CSP_ENTER_CRITICAL(lock) do { csp_bin_sem_wait(&lock, CSP_MAX_DELAY); } while(0) - #define CSP_EXIT_CRITICAL(lock) do { csp_bin_sem_post(&lock); } while(0) -#elif defined(CSP_FREERTOS) - #include "FreeRTOS.h" - #define CSP_BASE_TYPE portBASE_TYPE - #define CSP_MAX_DELAY portMAX_DELAY - #define CSP_INFINITY portMAX_DELAY - #define CSP_DEFINE_CRITICAL(lock) - #define CSP_INIT_CRITICAL(lock) ({CSP_ERR_NONE;}) - #define CSP_ENTER_CRITICAL(lock) do { portENTER_CRITICAL(); } while (0) - #define CSP_EXIT_CRITICAL(lock) do { portEXIT_CRITICAL(); } while (0) -#else - #error "OS must be either CSP_POSIX, CSP_MACOSX, CSP_FREERTOS OR CSP_WINDOWS" -#endif - -#ifdef __cplusplus -} /* extern "C" */ -#endif - -#endif // _CSP_PLATFORM_H_ diff --git a/thirdparty/libcsp/include/csp/csp_rtable.h b/thirdparty/libcsp/include/csp/csp_rtable.h deleted file mode 100644 index 34cd18e2..00000000 --- a/thirdparty/libcsp/include/csp/csp_rtable.h +++ /dev/null @@ -1,149 +0,0 @@ -/* -Cubesat Space Protocol - A small network-layer protocol designed for Cubesats -Copyright (C) 2012 GomSpace ApS (http://www.gomspace.com) -Copyright (C) 2012 AAUSAT3 Project (http://aausat3.space.aau.dk) - -This library is free software; you can redistribute it and/or -modify it under the terms of the GNU Lesser General Public -License as published by the Free Software Foundation; either -version 2.1 of the License, or (at your option) any later version. - -This library is distributed in the hope that it will be useful, -but WITHOUT ANY WARRANTY; without even the implied warranty of -MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU -Lesser General Public License for more details. - -You should have received a copy of the GNU Lesser General Public -License along with this library; if not, write to the Free Software -Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA -*/ - -#ifndef CSP_RTABLE_H_ -#define CSP_RTABLE_H_ - -#include - -#ifdef __cplusplus -extern "C" { -#endif - -#define CSP_NODE_MAC 0xFF -#define CSP_ROUTE_COUNT (CSP_ID_HOST_MAX + 2) -#define CSP_ROUTE_TABLE_SIZE 5 * CSP_ROUTE_COUNT - -/** - * Find outgoing interface in routing table - * @param id Destination node - * @return pointer to outgoing interface or NULL - */ -csp_iface_t * csp_rtable_find_iface(uint8_t id); - -/** - * Find MAC address associated with node - * @param id Destination node - * @return MAC address - */ -uint8_t csp_rtable_find_mac(uint8_t id); - -/** - * Setup routing entry - * @param node Host - * @param mask Number of bits in netmask - * @param ifc Interface - * @param mac MAC address - * @return CSP error type - */ -int csp_rtable_set(uint8_t node, uint8_t mask, csp_iface_t *ifc, uint8_t mac); - -/** - * Print routing table to stdout - */ -void csp_rtable_print(void); - - -/** - * Load the routing table from a buffer - * (deprecated, please use new csp_rtable_load) - * - * Warning: - * The table will be RAW from memory and contains direct pointers, not interface names. - * Therefore it's very important that a saved routing table is deleted after a firmware update - * - * @param route_table_in pointer to routing table buffer - */ -void csp_route_table_load(uint8_t route_table_in[CSP_ROUTE_TABLE_SIZE]); - -/** - * Save the routing table to a buffer - * (deprecated, please use new csp_rtable_save) - * - * Warning: - * The table will be RAW from memory and contains direct pointers, not interface names. - * Therefore it's very important that a saved routing table is deleted after a firmware update - * - * @param route_table_out pointer to routing table buffer - */ -void csp_route_table_save(uint8_t route_table_out[CSP_ROUTE_TABLE_SIZE]); - -/** - * Save routing table as a string to a buffer, which can be parsed - * again by csp_rtable_load. - * @param buffer pointer to buffer - * @param maxlen length of buffer - * @return length of saved string - */ -int csp_rtable_save(char * buffer, int maxlen); - -/** - * Load routing table from a string in the format - * %u/%u %s %u - * - Address - * - Netmask - * - Ifname - * - Mac Address (this field is optional) - * An example routing string is "0/0 I2C, 8/2 KISS" - * The string must be \0 null terminated - * @param buffer Pointer to string - */ -void csp_rtable_load(const char * buffer); - -/** - * Check string for valid routing table - * @param buffer Pointer to string - * @return number of valid entries found - */ -int csp_rtable_check(const char * buffer); - -/** - * Clear routing table: - * This could be done before load to ensure an entire clean table is loaded. - */ -void csp_rtable_clear(void); - -/** - * Setup routing entry to single node - * (deprecated, please use csp_rtable_set) - * - * @param node Host - * @param ifc Interface - * @param mac MAC address - * @return CSP error type - */ -#define csp_route_set(node, ifc, mac) csp_rtable_set(node, CSP_ID_HOST_SIZE, ifc, mac) - -/** - * Print routing table - * (deprecated, please use csp_rtable_print) - */ -#define csp_route_print_table() csp_rtable_print(); - -/** - * Print list of interfaces - * (deprecated, please use csp_iflist_print) - */ -#define csp_route_print_interfaces() csp_iflist_print(); - -#ifdef __cplusplus -} -#endif -#endif /* CSP_RTABLE_H_ */ diff --git a/thirdparty/libcsp/include/csp/csp_types.h b/thirdparty/libcsp/include/csp/csp_types.h deleted file mode 100644 index a9cc28cd..00000000 --- a/thirdparty/libcsp/include/csp/csp_types.h +++ /dev/null @@ -1,235 +0,0 @@ -/* -Cubesat Space Protocol - A small network-layer protocol designed for Cubesats -Copyright (C) 2012 Gomspace ApS (http://www.gomspace.com) -Copyright (C) 2012 AAUSAT3 Project (http://aausat3.space.aau.dk) - -This library is free software; you can redistribute it and/or -modify it under the terms of the GNU Lesser General Public -License as published by the Free Software Foundation; either -version 2.1 of the License, or (at your option) any later version. - -This library is distributed in the hope that it will be useful, -but WITHOUT ANY WARRANTY; without even the implied warranty of -MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU -Lesser General Public License for more details. - -You should have received a copy of the GNU Lesser General Public -License along with this library; if not, write to the Free Software -Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA -*/ - -#ifndef CSP_TYPES_H_ -#define CSP_TYPES_H_ - -#include -#include // -> CSP_HAVE_X defines -#ifdef CSP_HAVE_STDBOOL_H -#include -#endif - -#ifdef __cplusplus -extern "C" { -#endif - -/* Make bool for compilers without stdbool.h */ -#ifndef CSP_HAVE_STDBOOL_H -#define bool int -#define false 0 -#define true !false -#endif - -/** - * RESERVED PORTS (SERVICES) - */ - -enum csp_reserved_ports_e { - CSP_CMP = 0, - CSP_PING = 1, - CSP_PS = 2, - CSP_MEMFREE = 3, - CSP_REBOOT = 4, - CSP_BUF_FREE = 5, - CSP_UPTIME = 6, - CSP_ANY = (CSP_MAX_BIND_PORT + 1), - CSP_PROMISC = (CSP_MAX_BIND_PORT + 2) -}; - -typedef enum { - CSP_PRIO_CRITICAL = 0, - CSP_PRIO_HIGH = 1, - CSP_PRIO_NORM = 2, - CSP_PRIO_LOW = 3, -} csp_prio_t; - -#define CSP_PRIORITIES (1 << CSP_ID_PRIO_SIZE) - -#ifdef CSP_USE_QOS -#define CSP_RX_QUEUE_LENGTH (CSP_CONN_QUEUE_LENGTH / CSP_PRIORITIES) -#define CSP_ROUTE_FIFOS CSP_PRIORITIES -#define CSP_RX_QUEUES CSP_PRIORITIES -#else -#define CSP_RX_QUEUE_LENGTH CSP_CONN_QUEUE_LENGTH -#define CSP_ROUTE_FIFOS 1 -#define CSP_RX_QUEUES 1 -#endif - -/** Size of bit-fields in CSP header */ -#define CSP_ID_PRIO_SIZE 2 -#define CSP_ID_HOST_SIZE 5 -#define CSP_ID_PORT_SIZE 6 -#define CSP_ID_FLAGS_SIZE 8 - -#define CSP_HEADER_BITS (CSP_ID_PRIO_SIZE + 2 * CSP_ID_HOST_SIZE + 2 * CSP_ID_PORT_SIZE + CSP_ID_FLAGS_SIZE) -#define CSP_HEADER_LENGTH (CSP_HEADER_BITS / 8) - -#if CSP_HEADER_BITS != 32 && __GNUC__ -#error "Header length must be 32 bits" -#endif - -/** Highest number to be entered in field */ -#define CSP_ID_PRIO_MAX ((1 << (CSP_ID_PRIO_SIZE)) - 1) -#define CSP_ID_HOST_MAX ((1 << (CSP_ID_HOST_SIZE)) - 1) -#define CSP_ID_PORT_MAX ((1 << (CSP_ID_PORT_SIZE)) - 1) -#define CSP_ID_FLAGS_MAX ((1 << (CSP_ID_FLAGS_SIZE)) - 1) - -/** Identifier field masks */ -#define CSP_ID_PRIO_MASK ((uint32_t) CSP_ID_PRIO_MAX << (CSP_ID_FLAGS_SIZE + 2 * CSP_ID_PORT_SIZE + 2 * CSP_ID_HOST_SIZE)) -#define CSP_ID_SRC_MASK ((uint32_t) CSP_ID_HOST_MAX << (CSP_ID_FLAGS_SIZE + 2 * CSP_ID_PORT_SIZE + 1 * CSP_ID_HOST_SIZE)) -#define CSP_ID_DST_MASK ((uint32_t) CSP_ID_HOST_MAX << (CSP_ID_FLAGS_SIZE + 2 * CSP_ID_PORT_SIZE)) -#define CSP_ID_DPORT_MASK ((uint32_t) CSP_ID_PORT_MAX << (CSP_ID_FLAGS_SIZE + 1 * CSP_ID_PORT_SIZE)) -#define CSP_ID_SPORT_MASK ((uint32_t) CSP_ID_PORT_MAX << (CSP_ID_FLAGS_SIZE)) -#define CSP_ID_FLAGS_MASK ((uint32_t) CSP_ID_FLAGS_MAX << (0)) - -#define CSP_ID_CONN_MASK (CSP_ID_SRC_MASK | CSP_ID_DST_MASK | CSP_ID_DPORT_MASK | CSP_ID_SPORT_MASK) - -/** - CSP identifier. -*/ -typedef union { - //! Entire identifier. - uint32_t ext; - //! Individual fields. - struct __attribute__((__packed__)) { -#if defined(CSP_BIG_ENDIAN) && !defined(CSP_LITTLE_ENDIAN) - unsigned int pri : CSP_ID_PRIO_SIZE; - unsigned int src : CSP_ID_HOST_SIZE; - unsigned int dst : CSP_ID_HOST_SIZE; - unsigned int dport : CSP_ID_PORT_SIZE; - unsigned int sport : CSP_ID_PORT_SIZE; - unsigned int flags : CSP_ID_FLAGS_SIZE; -#elif defined(CSP_LITTLE_ENDIAN) && !defined(CSP_BIG_ENDIAN) - unsigned int flags : CSP_ID_FLAGS_SIZE; - unsigned int sport : CSP_ID_PORT_SIZE; - unsigned int dport : CSP_ID_PORT_SIZE; - unsigned int dst : CSP_ID_HOST_SIZE; - unsigned int src : CSP_ID_HOST_SIZE; - unsigned int pri : CSP_ID_PRIO_SIZE; -#else -#error "Must define one of CSP_BIG_ENDIAN or CSP_LITTLE_ENDIAN in csp_platform.h" -#endif - }; -} csp_id_t; - -/** Broadcast address */ -#define CSP_BROADCAST_ADDR CSP_ID_HOST_MAX - -/** Default routing address */ -#define CSP_DEFAULT_ROUTE (CSP_ID_HOST_MAX + 1) - -/** CSP Flags */ -#define CSP_FRES1 0x80 // Reserved for future use -#define CSP_FRES2 0x40 // Reserved for future use -#define CSP_FRES3 0x20 // Reserved for future use -#define CSP_FFRAG 0x10 // Use fragmentation -#define CSP_FHMAC 0x08 // Use HMAC verification -#define CSP_FXTEA 0x04 // Use XTEA encryption -#define CSP_FRDP 0x02 // Use RDP protocol -#define CSP_FCRC32 0x01 // Use CRC32 checksum - -/** CSP Socket options */ -#define CSP_SO_NONE 0x0000 // No socket options -#define CSP_SO_RDPREQ 0x0001 // Require RDP -#define CSP_SO_RDPPROHIB 0x0002 // Prohibit RDP -#define CSP_SO_HMACREQ 0x0004 // Require HMAC -#define CSP_SO_HMACPROHIB 0x0008 // Prohibit HMAC -#define CSP_SO_XTEAREQ 0x0010 // Require XTEA -#define CSP_SO_XTEAPROHIB 0x0020 // Prohibit HMAC -#define CSP_SO_CRC32REQ 0x0040 // Require CRC32 -#define CSP_SO_CRC32PROHIB 0x0080 // Prohibit CRC32 -#define CSP_SO_CONN_LESS 0x0100 // Enable Connection Less mode -#define CSP_SO_INTERNAL_LISTEN 0x1000 // Internal flag: listen called on socket - -/** CSP Connect options */ -#define CSP_O_NONE CSP_SO_NONE // No connection options -#define CSP_O_RDP CSP_SO_RDPREQ // Enable RDP -#define CSP_O_NORDP CSP_SO_RDPPROHIB // Disable RDP -#define CSP_O_HMAC CSP_SO_HMACREQ // Enable HMAC -#define CSP_O_NOHMAC CSP_SO_HMACPROHIB // Disable HMAC -#define CSP_O_XTEA CSP_SO_XTEAREQ // Enable XTEA -#define CSP_O_NOXTEA CSP_SO_XTEAPROHIB // Disable XTEA -#define CSP_O_CRC32 CSP_SO_CRC32REQ // Enable CRC32 -#define CSP_O_NOCRC32 CSP_SO_CRC32PROHIB // Disable CRC32 - -/** - * CSP PACKET STRUCTURE - * Note: This structure is constructed to fit - * with all interface frame types in order to - * have buffer reuse - */ -typedef struct __attribute__((__packed__)) { - uint8_t padding[CSP_PADDING_BYTES]; /**< Interface dependent padding */ - uint16_t length; /**< Length field must be just before CSP ID */ - csp_id_t id; /**< CSP id must be just before data */ - union { - uint8_t data[0]; /**< This just points to the rest of the buffer, without a size indication. */ - uint16_t data16[0]; /**< The data 16 and 32 types makes it easy to reference an integer (properly aligned) */ - uint32_t data32[0]; /**< without the compiler warning about strict aliasing rules. */ - }; -} csp_packet_t; - -/** Interface TX function */ -struct csp_iface_s; -typedef int (*nexthop_t)(struct csp_iface_s * interface, csp_packet_t *packet, uint32_t timeout); - -/** Interface struct */ -typedef struct csp_iface_s { - const char *name; /**< Interface name (keep below 10 bytes) */ - void * driver; /**< Pointer to interface handler structure */ - nexthop_t nexthop; /**< Next hop function */ - uint16_t mtu; /**< Maximum Transmission Unit of interface */ - uint8_t split_horizon_off; /**< Disable the route-loop prevention on if */ - uint32_t tx; /**< Successfully transmitted packets */ - uint32_t rx; /**< Successfully received packets */ - uint32_t tx_error; /**< Transmit errors */ - uint32_t rx_error; /**< Receive errors */ - uint32_t drop; /**< Dropped packets */ - uint32_t autherr; /**< Authentication errors */ - uint32_t frame; /**< Frame format errors */ - uint32_t txbytes; /**< Transmitted bytes */ - uint32_t rxbytes; /**< Received bytes */ - uint32_t irq; /**< Interrupts */ - struct csp_iface_s *next; /**< Next interface */ -} csp_iface_t; - -/** - * This define must be equal to the size of the packet overhead in csp_packet_t. - * It is used in csp_buffer_get() to check the allocated buffer size against - * the required buffer size. - */ -#define CSP_BUFFER_PACKET_OVERHEAD (sizeof(csp_packet_t) - sizeof(((csp_packet_t *)0)->data)) - -/** Forward declaration of socket and connection structures */ -typedef struct csp_conn_s csp_socket_t; -typedef struct csp_conn_s csp_conn_t; - -#define CSP_HOSTNAME_LEN 20 -#define CSP_MODEL_LEN 30 - -/* CSP_REBOOT magic values */ -#define CSP_REBOOT_MAGIC 0x80078007 -#define CSP_REBOOT_SHUTDOWN_MAGIC 0xD1E5529A - -#ifdef __cplusplus -} -#endif -#endif /* CSP_TYPES_H_ */ diff --git a/thirdparty/libcsp/include/csp/drivers/can_socketcan.h b/thirdparty/libcsp/include/csp/drivers/can_socketcan.h deleted file mode 100644 index 0963b414..00000000 --- a/thirdparty/libcsp/include/csp/drivers/can_socketcan.h +++ /dev/null @@ -1,22 +0,0 @@ -/* - * can_socketcan.h - * - * Created on: Feb 6, 2017 - * Author: johan - */ - -#ifndef LIB_CSP_INCLUDE_CSP_DRIVERS_CAN_SOCKETCAN_H_ -#define LIB_CSP_INCLUDE_CSP_DRIVERS_CAN_SOCKETCAN_H_ - -#include - -#ifdef __cplusplus -extern "C" { -#endif - -csp_iface_t * csp_can_socketcan_init(const char * ifc, int bitrate, int promisc); - -#ifdef __cplusplus -} -#endif -#endif /* LIB_CSP_INCLUDE_CSP_DRIVERS_CAN_SOCKETCAN_H_ */ diff --git a/thirdparty/libcsp/include/csp/drivers/i2c.h b/thirdparty/libcsp/include/csp/drivers/i2c.h deleted file mode 100644 index 3cf605ee..00000000 --- a/thirdparty/libcsp/include/csp/drivers/i2c.h +++ /dev/null @@ -1,120 +0,0 @@ -/* -Cubesat Space Protocol - A small network-layer protocol designed for Cubesats -Copyright (C) 2012 GomSpace ApS (http://www.gomspace.com) -Copyright (C) 2012 AAUSAT3 Project (http://aausat3.space.aau.dk) - -This library is free software; you can redistribute it and/or -modify it under the terms of the GNU Lesser General Public -License as published by the Free Software Foundation; either -version 2.1 of the License, or (at your option) any later version. - -This library is distributed in the hope that it will be useful, -but WITHOUT ANY WARRANTY; without even the implied warranty of -MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU -Lesser General Public License for more details. - -You should have received a copy of the GNU Lesser General Public -License along with this library; if not, write to the Free Software -Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA -*/ - -/** - * @file - * Common I2C interface, - * This file is derived from the Gomspace I2C driver, - * - */ - -#ifndef I2C_H_ -#define I2C_H_ - -#ifdef __cplusplus -extern "C" { -#endif - -/** - * The return value of the driver is a bit strange, - * It should return E_NO_ERR if successfull and the value is -1 - */ -#define E_NO_ERR -1 - -/** - * Maximum transfer length on I2C - */ -#define I2C_MTU 256 - -/** - I2C device modes - @{ -*/ -/** - I2C Master mode. -*/ -#define I2C_MASTER 0 -/** - I2C Slave mode. -*/ -#define I2C_SLAVE 1 -/**@}*/ - -/** - Data structure for I2C frames. - This structs fits on top of #csp_packet_t, removing the need for copying data. -*/ -typedef struct __attribute__((packed)) i2c_frame_s { - //! Not used by CSP - uint8_t padding; - //! Not used by CSP - cleared before Tx - uint8_t retries; - //! Not used by CSP - uint32_t reserved; - //! Destination address - uint8_t dest; - //! Not used by CSP - cleared before Tx - uint8_t len_rx; - //! Length of \a data part - uint16_t len; - //! CSP data - uint8_t data[I2C_MTU]; -} i2c_frame_t; - -/** - Callback for receiving data. - - @param[in] frame received I2C frame - @param[out] pxTaskWoken can be set, if context switch is required due to received data. -*/ -typedef void (*i2c_callback_t) (i2c_frame_t * frame, void * pxTaskWoken); - -/** - Initialise the I2C driver - - Functions is called by csp_i2c_init(). - - @param handle Which I2C bus (if more than one exists) - @param mode I2C device mode. Must be either I2C_MASTER or I2C_SLAVE - @param addr Own slave address - @param speed Bus speed in kbps - @param queue_len_tx Length of transmit queue - @param queue_len_rx Length of receive queue - @param callback If this value is set, the driver will call this function instead of using an RX queue - @return Error code -*/ -int i2c_init(int handle, int mode, uint8_t addr, uint16_t speed, int queue_len_tx, int queue_len_rx, i2c_callback_t callback); - -/** - User I2C transmit function. - - Called by CSP, when sending message over I2C. - - @param handle Handle to the device - @param frame Pointer to I2C frame - @param timeout Ticks to wait - @return Error code -*/ -int i2c_send(int handle, i2c_frame_t * frame, uint16_t timeout); - -#ifdef __cplusplus -} -#endif -#endif diff --git a/thirdparty/libcsp/include/csp/drivers/usart.h b/thirdparty/libcsp/include/csp/drivers/usart.h deleted file mode 100644 index d2da8448..00000000 --- a/thirdparty/libcsp/include/csp/drivers/usart.h +++ /dev/null @@ -1,107 +0,0 @@ -/* -Cubesat Space Protocol - A small network-layer protocol designed for Cubesats -Copyright (C) 2012 GomSpace ApS (http://www.gomspace.com) -Copyright (C) 2012 AAUSAT3 Project (http://aausat3.space.aau.dk) - -This library is free software; you can redistribute it and/or -modify it under the terms of the GNU Lesser General Public -License as published by the Free Software Foundation; either -version 2.1 of the License, or (at your option) any later version. - -This library is distributed in the hope that it will be useful, -but WITHOUT ANY WARRANTY; without even the implied warranty of -MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU -Lesser General Public License for more details. - -You should have received a copy of the GNU Lesser General Public -License along with this library; if not, write to the Free Software -Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA -*/ - -/** - * @file - * Common USART interface, - * This file is derived from the Gomspace USART driver, - * the main difference is the assumption that only one USART will be present on a PC - */ - -#ifndef USART_H_ -#define USART_H_ - -#include - -#ifdef __cplusplus -extern "C" { -#endif - -/** - Usart configuration, to be used with the usart_init call. -*/ -struct usart_conf { - //! USART device. - const char *device; - //! bits per second. - uint32_t baudrate; - //! Number of data bits. - uint8_t databits; - //! Number of stop bits. - uint8_t stopbits; - //! Parity setting. - uint8_t paritysetting; - //! Enable parity checking (Windows only). - uint8_t checkparity; -}; - -/** - Initialise UART with the usart_conf data structure - @param[in] conf full configuration structure -*/ -void usart_init(struct usart_conf *conf); - -/** - In order to catch incoming chars use the callback. - Only one callback per interface. - @param[in] handle usart[0,1,2,3] - @param[in] callback function pointer -*/ -typedef void (*usart_callback_t) (uint8_t *buf, int len, void *pxTaskWoken); - -/** - Set callback for receiving data. -*/ -void usart_set_callback(usart_callback_t callback); - -/** - Insert a character to the RX buffer of a usart - - @param[in] c character to insert - @param[out] pxTaskWoken can be set, if context switch is required due to received data. -*/ -void usart_insert(char c, void *pxTaskWoken); - -/** - Polling putchar (stdin). - - @param[in] c Character to transmit -*/ -void usart_putc(char c); - -/** - Send char buffer on UART (stdout). - - @param[in] buf Pointer to data - @param[in] len Length of data -*/ -void usart_putstr(char *buf, int len); - -/** - Buffered getchar (stdin). - - @return Character received -*/ -char usart_getc(void); - -#ifdef __cplusplus -} -#endif -#endif /* USART_H_ */ diff --git a/thirdparty/libcsp/include/csp/interfaces/csp_if_can.h b/thirdparty/libcsp/include/csp/interfaces/csp_if_can.h deleted file mode 100644 index 229671f2..00000000 --- a/thirdparty/libcsp/include/csp/interfaces/csp_if_can.h +++ /dev/null @@ -1,76 +0,0 @@ -/* -Cubesat Space Protocol - A small network-layer protocol designed for Cubesats -Copyright (C) 2012 GomSpace ApS (http://www.gomspace.com) -Copyright (C) 2012 AAUSAT3 Project (http://aausat3.space.aau.dk) - -This library is free software; you can redistribute it and/or -modify it under the terms of the GNU Lesser General Public -License as published by the Free Software Foundation; either -version 2.1 of the License, or (at your option) any later version. - -This library is distributed in the hope that it will be useful, -but WITHOUT ANY WARRANTY; without even the implied warranty of -MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU -Lesser General Public License for more details. - -You should have received a copy of the GNU Lesser General Public -License along with this library; if not, write to the Free Software -Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA -*/ - -#ifndef _CSP_IF_CAN_H_ -#define _CSP_IF_CAN_H_ - -#ifdef __cplusplus -extern "C" { -#endif - -#include - -#include -#include - -/* CAN header macros */ -#define CFP_HOST_SIZE 5 -#define CFP_TYPE_SIZE 1 -#define CFP_REMAIN_SIZE 8 -#define CFP_ID_SIZE 10 - -/* Macros for extracting header fields */ -#define CFP_FIELD(id,rsiz,fsiz) ((uint32_t)((uint32_t)((id) >> (rsiz)) & (uint32_t)((1 << (fsiz)) - 1))) -#define CFP_SRC(id) CFP_FIELD(id, CFP_HOST_SIZE + CFP_TYPE_SIZE + CFP_REMAIN_SIZE + CFP_ID_SIZE, CFP_HOST_SIZE) -#define CFP_DST(id) CFP_FIELD(id, CFP_TYPE_SIZE + CFP_REMAIN_SIZE + CFP_ID_SIZE, CFP_HOST_SIZE) -#define CFP_TYPE(id) CFP_FIELD(id, CFP_REMAIN_SIZE + CFP_ID_SIZE, CFP_TYPE_SIZE) -#define CFP_REMAIN(id) CFP_FIELD(id, CFP_ID_SIZE, CFP_REMAIN_SIZE) -#define CFP_ID(id) CFP_FIELD(id, 0, CFP_ID_SIZE) - -/* Macros for building CFP headers */ -#define CFP_MAKE_FIELD(id,fsiz,rsiz) ((uint32_t)(((id) & (uint32_t)((uint32_t)(1 << (fsiz)) - 1)) << (rsiz))) -#define CFP_MAKE_SRC(id) CFP_MAKE_FIELD(id, CFP_HOST_SIZE, CFP_HOST_SIZE + CFP_TYPE_SIZE + CFP_REMAIN_SIZE + CFP_ID_SIZE) -#define CFP_MAKE_DST(id) CFP_MAKE_FIELD(id, CFP_HOST_SIZE, CFP_TYPE_SIZE + CFP_REMAIN_SIZE + CFP_ID_SIZE) -#define CFP_MAKE_TYPE(id) CFP_MAKE_FIELD(id, CFP_TYPE_SIZE, CFP_REMAIN_SIZE + CFP_ID_SIZE) -#define CFP_MAKE_REMAIN(id) CFP_MAKE_FIELD(id, CFP_REMAIN_SIZE, CFP_ID_SIZE) -#define CFP_MAKE_ID(id) CFP_MAKE_FIELD(id, CFP_ID_SIZE, 0) - -/* Mask to uniquely separate connections */ -#define CFP_ID_CONN_MASK (CFP_MAKE_SRC((uint32_t)(1 << CFP_HOST_SIZE) - 1) | \ - CFP_MAKE_DST((uint32_t)(1 << CFP_HOST_SIZE) - 1) | \ - CFP_MAKE_ID((uint32_t)(1 << CFP_ID_SIZE) - 1)) - -/** - Default Maximum Transmission Unit (MTU) for CSP over CAN. - Maximum value is 2042 bytes. -*/ -#define CSP_CAN_MTU 256 - -int csp_can_rx(csp_iface_t *interface, uint32_t id, const uint8_t * data, uint8_t dlc, CSP_BASE_TYPE *task_woken); -int csp_can_tx(csp_iface_t *interface, csp_packet_t *packet, uint32_t timeout); - -/* Must be implemented by the driver */ -int csp_can_tx_frame(csp_iface_t *interface, uint32_t id, const uint8_t * data, uint8_t dlc); - -#ifdef __cplusplus -} /* extern "C" */ -#endif - -#endif /* _CSP_IF_CAN_H_ */ diff --git a/thirdparty/libcsp/include/csp/interfaces/csp_if_i2c.h b/thirdparty/libcsp/include/csp/interfaces/csp_if_i2c.h deleted file mode 100644 index c2169843..00000000 --- a/thirdparty/libcsp/include/csp/interfaces/csp_if_i2c.h +++ /dev/null @@ -1,51 +0,0 @@ -/* -Cubesat Space Protocol - A small network-layer protocol designed for Cubesats -Copyright (C) 2012 GomSpace ApS (http://www.gomspace.com) -Copyright (C) 2012 AAUSAT3 Project (http://aausat3.space.aau.dk) - -This library is free software; you can redistribute it and/or -modify it under the terms of the GNU Lesser General Public -License as published by the Free Software Foundation; either -version 2.1 of the License, or (at your option) any later version. - -This library is distributed in the hope that it will be useful, -but WITHOUT ANY WARRANTY; without even the implied warranty of -MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU -Lesser General Public License for more details. - -You should have received a copy of the GNU Lesser General Public -License along with this library; if not, write to the Free Software -Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA -*/ - -#ifndef _CSP_IF_I2C_H_ -#define _CSP_IF_I2C_H_ - -#ifdef __cplusplus -extern "C" { -#endif - -#include - -#include -#include -#include - -extern csp_iface_t csp_if_i2c; - -/** - * Capture I2C RX events for CSP - * @param opt_addr local i2c address - * @param handle which i2c device to use - * @param speed interface speed in kHz (normally 100 or 400) - * @return csp_error.h code - */ -int csp_i2c_init(uint8_t opt_addr, int handle, int speed); - -void csp_i2c_rx(i2c_frame_t * frame, void * pxTaskWoken); - -#ifdef __cplusplus -} /* extern "C" */ -#endif - -#endif /* _CSP_IF_I2C_H_ */ diff --git a/thirdparty/libcsp/include/csp/interfaces/csp_if_kiss.h b/thirdparty/libcsp/include/csp/interfaces/csp_if_kiss.h deleted file mode 100644 index f164cad1..00000000 --- a/thirdparty/libcsp/include/csp/interfaces/csp_if_kiss.h +++ /dev/null @@ -1,110 +0,0 @@ -/* -Cubesat Space Protocol - A small network-layer protocol designed for Cubesats -Copyright (C) 2012 GomSpace ApS (http://www.gomspace.com) -Copyright (C) 2012 AAUSAT3 Project (http://aausat3.space.aau.dk) - -This library is free software; you can redistribute it and/or -modify it under the terms of the GNU Lesser General Public -License as published by the Free Software Foundation; either -version 2.1 of the License, or (at your option) any later version. - -This library is distributed in the hope that it will be useful, -but WITHOUT ANY WARRANTY; without even the implied warranty of -MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU -Lesser General Public License for more details. - -You should have received a copy of the GNU Lesser General Public -License along with this library; if not, write to the Free Software -Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA -*/ - -#ifndef _CSP_IF_KISS_H_ -#define _CSP_IF_KISS_H_ - -#ifdef __cplusplus -extern "C" { -#endif - -#include - -#include -#include - -/** - * The KISS interface relies on the USART callback in order to parse incoming - * messaged from the serial interface. The USART callback however does not - * support passing the handle number of the responding USART, so you need to implement - * a USART callback for each handle and then call kiss_rx subsequently. - * - * In order to initialize the KISS interface. Fist call kiss_init() and then - * setup your usart to call csp_kiss_rx when new data is available. - * - * When a byte is not a part of a kiss packet, it will be returned to your - * usart driver using the usart_insert funtion that you provide. - * - * @param csp_iface pointer to interface - * @param buf pointer to incoming data - * @param len length of incoming data - * @param pxTaskWoken NULL if task context, pointer to variable if ISR - */ -void csp_kiss_rx(csp_iface_t * interface, uint8_t *buf, int len, void *pxTaskWoken); - -/** - * The putc function is used by the kiss interface to send - * a string of data to the serial port. This function must - * be implemented by the user, and passed to the kiss - * interface through the kiss_init function. - * @param buf byte to push - */ -typedef void (*csp_kiss_putc_f)(char buf); - -/** - * The characters not accepted by the kiss interface, are discarded - * using this function, which must be implemented by the user - * and passed through the kiss_init function. - * - * This reject function is typically used to display ASCII strings - * sent over the serial port, which are not in KISS format. Such as - * debugging information. - * - * @param c rejected character - * @param pxTaskWoken NULL if task context, pointer to variable if ISR - */ -typedef void (*csp_kiss_discard_f)(char c, void *pxTaskWoken); - -typedef enum { - KISS_MODE_NOT_STARTED, - KISS_MODE_STARTED, - KISS_MODE_ESCAPED, - KISS_MODE_SKIP_FRAME, -} kiss_mode_e; - -/** - * This structure should be statically allocated by the user - * and passed to the kiss interface during the init function - * no member information should be changed - */ -typedef struct csp_kiss_handle_s { - //! Put character on usart (tx). - csp_kiss_putc_f kiss_putc; - //! Discard - not KISS data (rx). - csp_kiss_discard_f kiss_discard; - //! Internal KISS state. - unsigned int rx_length; - //! Internal KISS state. - kiss_mode_e rx_mode; - //! Internal KISS state. - unsigned int rx_first; - //! Not used. - volatile unsigned char *rx_cbuf; - //! Internal KISS state. - csp_packet_t * rx_packet; -} csp_kiss_handle_t; - -void csp_kiss_init(csp_iface_t * csp_iface, csp_kiss_handle_t * csp_kiss_handle, csp_kiss_putc_f kiss_putc_f, csp_kiss_discard_f kiss_discard_f, const char * name); - -#ifdef __cplusplus -} /* extern "C" */ -#endif - -#endif /* _CSP_IF_KISS_H_ */ diff --git a/thirdparty/libcsp/include/csp/interfaces/csp_if_lo.h b/thirdparty/libcsp/include/csp/interfaces/csp_if_lo.h deleted file mode 100644 index 793c45ac..00000000 --- a/thirdparty/libcsp/include/csp/interfaces/csp_if_lo.h +++ /dev/null @@ -1,38 +0,0 @@ -/* -Cubesat Space Protocol - A small network-layer protocol designed for Cubesats -Copyright (C) 2012 GomSpace ApS (http://www.gomspace.com) -Copyright (C) 2012 AAUSAT3 Project (http://aausat3.space.aau.dk) - -This library is free software; you can redistribute it and/or -modify it under the terms of the GNU Lesser General Public -License as published by the Free Software Foundation; either -version 2.1 of the License, or (at your option) any later version. - -This library is distributed in the hope that it will be useful, -but WITHOUT ANY WARRANTY; without even the implied warranty of -MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU -Lesser General Public License for more details. - -You should have received a copy of the GNU Lesser General Public -License along with this library; if not, write to the Free Software -Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA -*/ - -#ifndef _CSP_IF_LO_H_ -#define _CSP_IF_LO_H_ - -#ifdef __cplusplus -extern "C" { -#endif - -/* CSP includes */ -#include -#include - -extern csp_iface_t csp_if_lo; - -#ifdef __cplusplus -} /* extern "C" */ -#endif - -#endif // _CSP_IF_LO_H_ diff --git a/thirdparty/libcsp/include/csp/interfaces/csp_if_zmqhub.h b/thirdparty/libcsp/include/csp/interfaces/csp_if_zmqhub.h deleted file mode 100644 index f9ab43bf..00000000 --- a/thirdparty/libcsp/include/csp/interfaces/csp_if_zmqhub.h +++ /dev/null @@ -1,26 +0,0 @@ -#ifndef CSP_IF_ZMQHUB_H_ -#define CSP_IF_ZMQHUB_H_ - -#include - -extern csp_iface_t csp_if_zmqhub; - -/** - * Setup ZMQ interface - * @param addr only receive messages matching this address (255 means all) - * @param host Pointer to string containing zmqproxy host - * @return CSP_ERR - */ -int csp_zmqhub_init(uint8_t addr, const char * host); - -/** - * Setup ZMQ interface - * @param addr only receive messages matching this address (255 means all) - * @param publisher_endpoint Pointer to string containing zmqproxy publisher endpoint - * @param subscriber_endpoint Pointer to string containing zmqproxy subscriber endpoint - * @return CSP_ERR - */ -int csp_zmqhub_init_w_endpoints(uint8_t addr, const char * publisher_url, - const char * subscriber_url); - -#endif /* CSP_IF_ZMQHUB_H_ */ diff --git a/thirdparty/libcsp/libcsp.mk b/thirdparty/libcsp/libcsp.mk deleted file mode 100644 index febffad7..00000000 --- a/thirdparty/libcsp/libcsp.mk +++ /dev/null @@ -1,12 +0,0 @@ -CSRC += $(wildcard $(CURRENTPATH)/src/drivers/can/*.c) -CSRC += $(wildcard $(CURRENTPATH)/src/*.c) -CSRC += $(wildcard $(CURRENTPATH)/src/interfaces/*.c) -CSRC += $(wildcard $(CURRENTPATH)/src/rtable/csp_rtable_cidr.c) -CSRC += $(wildcard $(CURRENTPATH)/src/crypto/*.c) -CSRC += $(wildcard $(CURRENTPATH)/src/arch/posix/*.c) -CSRC += $(wildcard $(CURRENTPATH)/src/transport/*.c) - -INCLUDES += $(CURRENTPATH)/include -INCLUDES += $(CURRENTPATH)/include/csp -INCLUDES += $(CURRENTPATH)/include/csp/crypto -INCLUDES += $(CURRENTPATH) \ No newline at end of file diff --git a/thirdparty/libcsp/src/CMakeLists.txt b/thirdparty/libcsp/src/CMakeLists.txt deleted file mode 100644 index 39c67877..00000000 --- a/thirdparty/libcsp/src/CMakeLists.txt +++ /dev/null @@ -1,27 +0,0 @@ -target_sources(${LIB_CSP_NAME} PRIVATE - csp_bridge.c - csp_buffer.c - csp_conn.c - csp_crc32.c - csp_debug.c - csp_dedup.c - csp_endian.c - csp_hex_dump.c - csp_iflist.c - csp_io.c - csp_port.c - csp_promisc.c - csp_qfifo.c - csp_route.c - csp_service_handler.c - csp_services.c - csp_sfp.c -) - -add_subdirectory(drivers) -add_subdirectory(crypto) -add_subdirectory(interfaces) -add_subdirectory(rtable) -add_subdirectory(transport) -add_subdirectory(arch) - diff --git a/thirdparty/libcsp/src/arch/CMakeLists.txt b/thirdparty/libcsp/src/arch/CMakeLists.txt deleted file mode 100644 index aa0e4ca6..00000000 --- a/thirdparty/libcsp/src/arch/CMakeLists.txt +++ /dev/null @@ -1,3 +0,0 @@ -add_subdirectory(posix) - - diff --git a/thirdparty/libcsp/src/arch/freertos/csp_malloc.c b/thirdparty/libcsp/src/arch/freertos/csp_malloc.c deleted file mode 100644 index 97e5c8f4..00000000 --- a/thirdparty/libcsp/src/arch/freertos/csp_malloc.c +++ /dev/null @@ -1,33 +0,0 @@ -/* -Cubesat Space Protocol - A small network-layer protocol designed for Cubesats -Copyright (C) 2012 Gomspace ApS (http://www.gomspace.com) -Copyright (C) 2012 AAUSAT3 Project (http://aausat3.space.aau.dk) - -This library is free software; you can redistribute it and/or -modify it under the terms of the GNU Lesser General Public -License as published by the Free Software Foundation; either -version 2.1 of the License, or (at your option) any later version. - -This library is distributed in the hope that it will be useful, -but WITHOUT ANY WARRANTY; without even the implied warranty of -MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU -Lesser General Public License for more details. - -You should have received a copy of the GNU Lesser General Public -License along with this library; if not, write to the Free Software -Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA -*/ - -#include -#include - -/* FreeRTOS includes */ -#include - -void * csp_malloc(size_t size) { - return pvPortMalloc(size); -} - -void csp_free(void *ptr) { - vPortFree(ptr); -} diff --git a/thirdparty/libcsp/src/arch/freertos/csp_queue.c b/thirdparty/libcsp/src/arch/freertos/csp_queue.c deleted file mode 100644 index 44efd0eb..00000000 --- a/thirdparty/libcsp/src/arch/freertos/csp_queue.c +++ /dev/null @@ -1,66 +0,0 @@ -/* -Cubesat Space Protocol - A small network-layer protocol designed for Cubesats -Copyright (C) 2012 Gomspace ApS (http://www.gomspace.com) -Copyright (C) 2012 AAUSAT3 Project (http://aausat3.space.aau.dk) - -This library is free software; you can redistribute it and/or -modify it under the terms of the GNU Lesser General Public -License as published by the Free Software Foundation; either -version 2.1 of the License, or (at your option) any later version. - -This library is distributed in the hope that it will be useful, -but WITHOUT ANY WARRANTY; without even the implied warranty of -MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU -Lesser General Public License for more details. - -You should have received a copy of the GNU Lesser General Public -License along with this library; if not, write to the Free Software -Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA -*/ - -#include - -/* FreeRTOS includes */ -#include -#include - -/* CSP includes */ -#include - -#include - -csp_queue_handle_t csp_queue_create(int length, size_t item_size) { - return xQueueCreate(length, item_size); -} - -void csp_queue_remove(csp_queue_handle_t queue) { - vQueueDelete(queue); -} - -int csp_queue_enqueue(csp_queue_handle_t handle, void * value, uint32_t timeout) { - if (timeout != CSP_MAX_DELAY) - timeout = timeout / portTICK_RATE_MS; - return xQueueSendToBack(handle, value, timeout); -} - -int csp_queue_enqueue_isr(csp_queue_handle_t handle, void * value, CSP_BASE_TYPE * task_woken) { - return xQueueSendToBackFromISR(handle, value, task_woken); -} - -int csp_queue_dequeue(csp_queue_handle_t handle, void * buf, uint32_t timeout) { - if (timeout != CSP_MAX_DELAY) - timeout = timeout / portTICK_RATE_MS; - return xQueueReceive(handle, buf, timeout); -} - -int csp_queue_dequeue_isr(csp_queue_handle_t handle, void * buf, CSP_BASE_TYPE * task_woken) { - return xQueueReceiveFromISR(handle, buf, task_woken); -} - -int csp_queue_size(csp_queue_handle_t handle) { - return uxQueueMessagesWaiting(handle); -} - -int csp_queue_size_isr(csp_queue_handle_t handle) { - return uxQueueMessagesWaitingFromISR(handle); -} diff --git a/thirdparty/libcsp/src/arch/freertos/csp_semaphore.c b/thirdparty/libcsp/src/arch/freertos/csp_semaphore.c deleted file mode 100644 index b91757e5..00000000 --- a/thirdparty/libcsp/src/arch/freertos/csp_semaphore.c +++ /dev/null @@ -1,96 +0,0 @@ -/* -Cubesat Space Protocol - A small network-layer protocol designed for Cubesats -Copyright (C) 2012 Gomspace ApS (http://www.gomspace.com) -Copyright (C) 2012 AAUSAT3 Project (http://aausat3.space.aau.dk) - -This library is free software; you can redistribute it and/or -modify it under the terms of the GNU Lesser General Public -License as published by the Free Software Foundation; either -version 2.1 of the License, or (at your option) any later version. - -This library is distributed in the hope that it will be useful, -but WITHOUT ANY WARRANTY; without even the implied warranty of -MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU -Lesser General Public License for more details. - -You should have received a copy of the GNU Lesser General Public -License along with this library; if not, write to the Free Software -Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA -*/ - -#include -#include - -/* FreeRTOS includes */ -#include -#include - -/* CSP includes */ -#include - -#include -#include -#include - -int csp_mutex_create(csp_mutex_t * mutex) { - *mutex = xSemaphoreCreateMutex(); - if (*mutex) { - return CSP_SEMAPHORE_OK; - } else { - return CSP_SEMAPHORE_ERROR; - } -} - -int csp_mutex_remove(csp_mutex_t * mutex) { - return csp_bin_sem_remove(mutex); -} - -int csp_mutex_lock(csp_mutex_t * mutex, uint32_t timeout) { - return csp_bin_sem_wait(mutex, timeout); -} - -int csp_mutex_unlock(csp_mutex_t * mutex) { - return csp_bin_sem_post(mutex); -} - -int csp_bin_sem_create(csp_bin_sem_handle_t * sem) { - vSemaphoreCreateBinary(*sem); - return CSP_SEMAPHORE_OK; -} - -int csp_bin_sem_remove(csp_bin_sem_handle_t * sem) { - if ((sem != NULL) && (*sem != NULL)) { - csp_queue_remove(*sem); - } - return CSP_SEMAPHORE_OK; -} - -int csp_bin_sem_wait(csp_bin_sem_handle_t * sem, uint32_t timeout) { - csp_log_lock("Wait: %p", sem); - if (timeout != CSP_MAX_DELAY) - timeout = timeout / portTICK_RATE_MS; - if (xSemaphoreTake(*sem, timeout) == pdPASS) { - return CSP_SEMAPHORE_OK; - } else { - return CSP_SEMAPHORE_ERROR; - } -} - -int csp_bin_sem_post(csp_bin_sem_handle_t * sem) { - csp_log_lock("Post: %p", sem); - if (xSemaphoreGive(*sem) == pdPASS) { - return CSP_SEMAPHORE_OK; - } else { - return CSP_SEMAPHORE_ERROR; - } -} - -int csp_bin_sem_post_isr(csp_bin_sem_handle_t * sem, CSP_BASE_TYPE * task_woken) { - csp_log_lock("Post: %p", sem); - if (xSemaphoreGiveFromISR(*sem, task_woken) == pdPASS) { - return CSP_SEMAPHORE_OK; - } else { - return CSP_SEMAPHORE_ERROR; - } -} - diff --git a/thirdparty/libcsp/src/arch/freertos/csp_system.c b/thirdparty/libcsp/src/arch/freertos/csp_system.c deleted file mode 100644 index a81c84b4..00000000 --- a/thirdparty/libcsp/src/arch/freertos/csp_system.c +++ /dev/null @@ -1,139 +0,0 @@ -/* -Cubesat Space Protocol - A small network-layer protocol designed for Cubesats -Copyright (C) 2012 Gomspace ApS (http://www.gomspace.com) -Copyright (C) 2012 AAUSAT3 Project (http://aausat3.space.aau.dk) - -This library is free software; you can redistribute it and/or -modify it under the terms of the GNU Lesser General Public -License as published by the Free Software Foundation; either -version 2.1 of the License, or (at your option) any later version. - -This library is distributed in the hope that it will be useful, -but WITHOUT ANY WARRANTY; without even the implied warranty of -MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU -Lesser General Public License for more details. - -You should have received a copy of the GNU Lesser General Public -License along with this library; if not, write to the Free Software -Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA -*/ - -#include -#include -#include -#include - -#include -#include - -#include - -int csp_sys_tasklist(char * out) { -#if (tskKERNEL_VERSION_MAJOR < 8) - vTaskList((signed portCHAR *) out); -#else - vTaskList(out); -#endif - return CSP_ERR_NONE; -} - -int csp_sys_tasklist_size(void) { - return 40 * uxTaskGetNumberOfTasks(); -} - -uint32_t csp_sys_memfree(void) { - - uint32_t total = 0, max = UINT32_MAX, size; - void * pmem; - - /* If size_t is less than 32 bits, start with 10 KiB */ - size = sizeof(uint32_t) > sizeof(size_t) ? 10000 : 1000000; - - while (1) { - pmem = pvPortMalloc(size + total); - if (pmem == NULL) { - max = size + total; - size = size / 2; - } else { - total += size; - if (total + size >= max) - size = size / 2; - vPortFree(pmem); - } - if (size < 32) break; - } - - return total; -} - -int csp_sys_reboot(void) { - - extern void __attribute__((weak)) cpu_set_reset_cause(unsigned int); - if (cpu_set_reset_cause) - cpu_set_reset_cause(1); - - extern void __attribute__((weak)) cpu_reset(void); - if (cpu_reset) { - cpu_reset(); - while (1); - } - - csp_log_error("Failed to reboot"); - - return CSP_ERR_INVAL; -} - -int csp_sys_shutdown(void) { - - extern void __attribute__((weak)) cpu_shutdown(void); - if (cpu_shutdown) { - cpu_shutdown(); - while (1); - } - - csp_log_error("Failed to shutdown"); - - return CSP_ERR_INVAL; -} - -void csp_sys_set_color(csp_color_t color) { - - unsigned int color_code, modifier_code; - switch (color & COLOR_MASK_COLOR) { - case COLOR_BLACK: - color_code = 30; break; - case COLOR_RED: - color_code = 31; break; - case COLOR_GREEN: - color_code = 32; break; - case COLOR_YELLOW: - color_code = 33; break; - case COLOR_BLUE: - color_code = 34; break; - case COLOR_MAGENTA: - color_code = 35; break; - case COLOR_CYAN: - color_code = 36; break; - case COLOR_WHITE: - color_code = 37; break; - case COLOR_RESET: - default: - color_code = 0; break; - } - - switch (color & COLOR_MASK_MODIFIER) { - case COLOR_BOLD: - modifier_code = 1; break; - case COLOR_UNDERLINE: - modifier_code = 2; break; - case COLOR_BLINK: - modifier_code = 3; break; - case COLOR_HIDE: - modifier_code = 4; break; - case COLOR_NORMAL: - default: - modifier_code = 0; break; - } - - printf("\033[%u;%um", modifier_code, color_code); -} diff --git a/thirdparty/libcsp/src/arch/freertos/csp_thread.c b/thirdparty/libcsp/src/arch/freertos/csp_thread.c deleted file mode 100644 index af8296cd..00000000 --- a/thirdparty/libcsp/src/arch/freertos/csp_thread.c +++ /dev/null @@ -1,38 +0,0 @@ -/* -Cubesat Space Protocol - A small network-layer protocol designed for Cubesats -Copyright (C) 2012 Gomspace ApS (http://www.gomspace.com) -Copyright (C) 2012 AAUSAT3 Project (http://aausat3.space.aau.dk) - -This library is free software; you can redistribute it and/or -modify it under the terms of the GNU Lesser General Public -License as published by the Free Software Foundation; either -version 2.1 of the License, or (at your option) any later version. - -This library is distributed in the hope that it will be useful, -but WITHOUT ANY WARRANTY; without even the implied warranty of -MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU -Lesser General Public License for more details. - -You should have received a copy of the GNU Lesser General Public -License along with this library; if not, write to the Free Software -Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA -*/ - -#include -#include - -/* CSP includes */ -#include - -#include - -int csp_thread_create(csp_thread_return_t (* routine)(void *), const char * const thread_name, unsigned short stack_depth, void * parameters, unsigned int priority, csp_thread_handle_t * handle) { -#if (tskKERNEL_VERSION_MAJOR >= 8) - portBASE_TYPE ret = xTaskCreate(routine, thread_name, stack_depth, parameters, priority, handle); -#else - portBASE_TYPE ret = xTaskCreate(routine, (signed char *) thread_name, stack_depth, parameters, priority, handle); -#endif - if (ret != pdTRUE) - return CSP_ERR_NOMEM; - return CSP_ERR_NONE; -} diff --git a/thirdparty/libcsp/src/arch/freertos/csp_time.c b/thirdparty/libcsp/src/arch/freertos/csp_time.c deleted file mode 100644 index fd54a8cb..00000000 --- a/thirdparty/libcsp/src/arch/freertos/csp_time.c +++ /dev/null @@ -1,46 +0,0 @@ -/* -Cubesat Space Protocol - A small network-layer protocol designed for Cubesats -Copyright (C) 2012 Gomspace ApS (http://www.gomspace.com) -Copyright (C) 2012 AAUSAT3 Project (http://aausat3.space.aau.dk) - -This library is free software; you can redistribute it and/or -modify it under the terms of the GNU Lesser General Public -License as published by the Free Software Foundation; either -version 2.1 of the License, or (at your option) any later version. - -This library is distributed in the hope that it will be useful, -but WITHOUT ANY WARRANTY; without even the implied warranty of -MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU -Lesser General Public License for more details. - -You should have received a copy of the GNU Lesser General Public -License along with this library; if not, write to the Free Software -Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA -*/ - -#include - -/* FreeRTOS includes */ -#include -#include - -/* CSP includes */ -#include - -#include - -uint32_t csp_get_ms(void) { - return (uint32_t)(xTaskGetTickCount() * (1000/configTICK_RATE_HZ)); -} - -uint32_t csp_get_ms_isr(void) { - return (uint32_t)(xTaskGetTickCountFromISR() * (1000/configTICK_RATE_HZ)); -} - -uint32_t csp_get_s(void) { - return (uint32_t)(xTaskGetTickCount()/configTICK_RATE_HZ); -} - -uint32_t csp_get_s_isr(void) { - return (uint32_t)(xTaskGetTickCountFromISR()/configTICK_RATE_HZ); -} diff --git a/thirdparty/libcsp/src/arch/macosx/csp_malloc.c b/thirdparty/libcsp/src/arch/macosx/csp_malloc.c deleted file mode 100644 index 95bb8cc7..00000000 --- a/thirdparty/libcsp/src/arch/macosx/csp_malloc.c +++ /dev/null @@ -1,31 +0,0 @@ -/* -Cubesat Space Protocol - A small network-layer protocol designed for Cubesats -Copyright (C) 2012 Gomspace ApS (http://www.gomspace.com) -Copyright (C) 2012 AAUSAT3 Project (http://aausat3.space.aau.dk) - -This library is free software; you can redistribute it and/or -modify it under the terms of the GNU Lesser General Public -License as published by the Free Software Foundation; either -version 2.1 of the License, or (at your option) any later version. - -This library is distributed in the hope that it will be useful, -but WITHOUT ANY WARRANTY; without even the implied warranty of -MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU -Lesser General Public License for more details. - -You should have received a copy of the GNU Lesser General Public -License along with this library; if not, write to the Free Software -Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA -*/ - -#include -#include - -void * csp_malloc(size_t size) { - return malloc(size); -} - -void csp_free(void *ptr) { - free(ptr); -} - diff --git a/thirdparty/libcsp/src/arch/macosx/csp_queue.c b/thirdparty/libcsp/src/arch/macosx/csp_queue.c deleted file mode 100644 index a2fb1b4f..00000000 --- a/thirdparty/libcsp/src/arch/macosx/csp_queue.c +++ /dev/null @@ -1,64 +0,0 @@ -/* -Cubesat Space Protocol - A small network-layer protocol designed for Cubesats -Copyright (C) 2012 Gomspace ApS (http://www.gomspace.com) -Copyright (C) 2012 AAUSAT3 Project (http://aausat3.space.aau.dk) - -This library is free software; you can redistribute it and/or -modify it under the terms of the GNU Lesser General Public -License as published by the Free Software Foundation; either -version 2.1 of the License, or (at your option) any later version. - -This library is distributed in the hope that it will be useful, -but WITHOUT ANY WARRANTY; without even the implied warranty of -MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU -Lesser General Public License for more details. - -You should have received a copy of the GNU Lesser General Public -License along with this library; if not, write to the Free Software -Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA -*/ - -#include -#include - -/* CSP includes */ -#include - -#include -#include - - -csp_queue_handle_t csp_queue_create(int length, size_t item_size) { - return pthread_queue_create(length, item_size); -} - -void csp_queue_remove(csp_queue_handle_t queue) { - return pthread_queue_delete(queue); -} - -int csp_queue_enqueue(csp_queue_handle_t handle, void *value, uint32_t timeout) { - return pthread_queue_enqueue(handle, value, timeout); -} - -int csp_queue_enqueue_isr(csp_queue_handle_t handle, void * value, CSP_BASE_TYPE * task_woken) { - if (task_woken != NULL) - *task_woken = 0; - return csp_queue_enqueue(handle, value, 0); -} - -int csp_queue_dequeue(csp_queue_handle_t handle, void *buf, uint32_t timeout) { - return pthread_queue_dequeue(handle, buf, timeout); -} - -int csp_queue_dequeue_isr(csp_queue_handle_t handle, void *buf, CSP_BASE_TYPE * task_woken) { - *task_woken = 0; - return csp_queue_dequeue(handle, buf, 0); -} - -int csp_queue_size(csp_queue_handle_t handle) { - return pthread_queue_items(handle); -} - -int csp_queue_size_isr(csp_queue_handle_t handle) { - return pthread_queue_items(handle); -} diff --git a/thirdparty/libcsp/src/arch/macosx/csp_semaphore.c b/thirdparty/libcsp/src/arch/macosx/csp_semaphore.c deleted file mode 100644 index 915447f3..00000000 --- a/thirdparty/libcsp/src/arch/macosx/csp_semaphore.c +++ /dev/null @@ -1,105 +0,0 @@ -/* -Cubesat Space Protocol - A small network-layer protocol designed for Cubesats -Copyright (C) 2012 Gomspace ApS (http://www.gomspace.com) -Copyright (C) 2012 AAUSAT3 Project (http://aausat3.space.aau.dk) - -This library is free software; you can redistribute it and/or -modify it under the terms of the GNU Lesser General Public -License as published by the Free Software Foundation; either -version 2.1 of the License, or (at your option) any later version. - -This library is distributed in the hope that it will be useful, -but WITHOUT ANY WARRANTY; without even the implied warranty of -MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU -Lesser General Public License for more details. - -You should have received a copy of the GNU Lesser General Public -License along with this library; if not, write to the Free Software -Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA -*/ - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -/* CSP includes */ -#include - -#include - -int csp_mutex_create(csp_mutex_t * mutex) { - csp_log_lock("Mutex init: %p", mutex); - *mutex = pthread_queue_create(1, sizeof(int)); - if (mutex) { - int dummy = 0; - pthread_queue_enqueue(*mutex, &dummy, 0); - return CSP_SEMAPHORE_OK; - } else { - return CSP_SEMAPHORE_ERROR; - } -} - -int csp_mutex_remove(csp_mutex_t * mutex) { - pthread_queue_delete(*mutex); - return CSP_SEMAPHORE_OK; -} - -int csp_mutex_lock(csp_mutex_t * mutex, uint32_t timeout) { - - int ret; - csp_log_lock("Wait: %p timeout %"PRIu32, mutex, timeout); - - if (timeout == CSP_INFINITY) { - /* TODO: fix this to be infinite */ - int dummy = 0; - if (pthread_queue_dequeue(*mutex, &dummy, timeout) == PTHREAD_QUEUE_OK) - ret = CSP_MUTEX_OK; - else - ret = CSP_MUTEX_ERROR; - } else { - int dummy = 0; - if (pthread_queue_dequeue(*mutex, &dummy, timeout) == PTHREAD_QUEUE_OK) - ret = CSP_MUTEX_OK; - else - ret = CSP_MUTEX_ERROR; - } - - return ret == CSP_MUTEX_ERROR ? CSP_SEMAPHORE_ERROR : CSP_SEMAPHORE_OK; -} - -int csp_mutex_unlock(csp_mutex_t * mutex) { - int dummy = 0; - if (pthread_queue_enqueue(*mutex, &dummy, 0) == PTHREAD_QUEUE_OK) { - return CSP_SEMAPHORE_OK; - } else { - return CSP_SEMAPHORE_ERROR; - } -} - -int csp_bin_sem_create(csp_bin_sem_handle_t * sem) { - return csp_mutex_create(sem); -} - -int csp_bin_sem_remove(csp_bin_sem_handle_t * sem) { - return csp_mutex_remove(sem); -} - -int csp_bin_sem_wait(csp_bin_sem_handle_t * sem, uint32_t timeout) { - return csp_mutex_lock(sem, timeout); -} - -int csp_bin_sem_post(csp_bin_sem_handle_t * sem) { - return csp_mutex_unlock(sem); -} - -int csp_bin_sem_post_isr(csp_bin_sem_handle_t * sem, CSP_BASE_TYPE * task_woken) { - return csp_mutex_unlock(sem); -} diff --git a/thirdparty/libcsp/src/arch/macosx/csp_system.c b/thirdparty/libcsp/src/arch/macosx/csp_system.c deleted file mode 100644 index 834cb210..00000000 --- a/thirdparty/libcsp/src/arch/macosx/csp_system.c +++ /dev/null @@ -1,99 +0,0 @@ -/* -Cubesat Space Protocol - A small network-layer protocol designed for Cubesats -Copyright (C) 2012 Gomspace ApS (http://www.gomspace.com) -Copyright (C) 2012 AAUSAT3 Project (http://aausat3.space.aau.dk) - -This library is free software; you can redistribute it and/or -modify it under the terms of the GNU Lesser General Public -License as published by the Free Software Foundation; either -version 2.1 of the License, or (at your option) any later version. - -This library is distributed in the hope that it will be useful, -but WITHOUT ANY WARRANTY; without even the implied warranty of -MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU -Lesser General Public License for more details. - -You should have received a copy of the GNU Lesser General Public -License along with this library; if not, write to the Free Software -Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA -*/ - -#include -#include -#include - -#include -#include - -#include - -int csp_sys_tasklist(char * out) { - strcpy(out, "Tasklist not available on OSX"); - return CSP_ERR_NONE; -} - -int csp_sys_tasklist_size(void) { - return 100; -} - -uint32_t csp_sys_memfree(void) { - /* TODO: Fix memory free on OSX */ - uint32_t total = 0; - return total; -} - -int csp_sys_reboot(void) { - /* TODO: Fix reboot on OSX */ - csp_log_error("Failed to reboot"); - - return CSP_ERR_INVAL; -} - -int csp_sys_shutdown(void) { - /* TODO: Fix shutdown on OSX */ - csp_log_error("Failed to shutdown"); - - return CSP_ERR_INVAL; -} - -void csp_sys_set_color(csp_color_t color) { - - unsigned int color_code, modifier_code; - switch (color & COLOR_MASK_COLOR) { - case COLOR_BLACK: - color_code = 30; break; - case COLOR_RED: - color_code = 31; break; - case COLOR_GREEN: - color_code = 32; break; - case COLOR_YELLOW: - color_code = 33; break; - case COLOR_BLUE: - color_code = 34; break; - case COLOR_MAGENTA: - color_code = 35; break; - case COLOR_CYAN: - color_code = 36; break; - case COLOR_WHITE: - color_code = 37; break; - case COLOR_RESET: - default: - color_code = 0; break; - } - - switch (color & COLOR_MASK_MODIFIER) { - case COLOR_BOLD: - modifier_code = 1; break; - case COLOR_UNDERLINE: - modifier_code = 2; break; - case COLOR_BLINK: - modifier_code = 3; break; - case COLOR_HIDE: - modifier_code = 4; break; - case COLOR_NORMAL: - default: - modifier_code = 0; break; - } - - printf("\033[%u;%um", modifier_code, color_code); -} diff --git a/thirdparty/libcsp/src/arch/macosx/csp_thread.c b/thirdparty/libcsp/src/arch/macosx/csp_thread.c deleted file mode 100644 index ed64856a..00000000 --- a/thirdparty/libcsp/src/arch/macosx/csp_thread.c +++ /dev/null @@ -1,31 +0,0 @@ -/* -Cubesat Space Protocol - A small network-layer protocol designed for Cubesats -Copyright (C) 2012 Gomspace ApS (http://www.gomspace.com) -Copyright (C) 2012 AAUSAT3 Project (http://aausat3.space.aau.dk) - -This library is free software; you can redistribute it and/or -modify it under the terms of the GNU Lesser General Public -License as published by the Free Software Foundation; either -version 2.1 of the License, or (at your option) any later version. - -This library is distributed in the hope that it will be useful, -but WITHOUT ANY WARRANTY; without even the implied warranty of -MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU -Lesser General Public License for more details. - -You should have received a copy of the GNU Lesser General Public -License along with this library; if not, write to the Free Software -Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA -*/ - -#include -#include - -/* CSP includes */ -#include - -#include - -int csp_thread_create(csp_thread_return_t (* routine)(void *), const char * const thread_name, unsigned short stack_depth, void * parameters, unsigned int priority, csp_thread_handle_t * handle) { - return pthread_create(handle, NULL, routine, parameters); -} diff --git a/thirdparty/libcsp/src/arch/macosx/csp_time.c b/thirdparty/libcsp/src/arch/macosx/csp_time.c deleted file mode 100644 index a53f27e6..00000000 --- a/thirdparty/libcsp/src/arch/macosx/csp_time.c +++ /dev/null @@ -1,65 +0,0 @@ -/* -Cubesat Space Protocol - A small network-layer protocol designed for Cubesats -Copyright (C) 2012 Gomspace ApS (http://www.gomspace.com) -Copyright (C) 2012 AAUSAT3 Project (http://aausat3.space.aau.dk) - -This library is free software; you can redistribute it and/or -modify it under the terms of the GNU Lesser General Public -License as published by the Free Software Foundation; either -version 2.1 of the License, or (at your option) any later version. - -This library is distributed in the hope that it will be useful, -but WITHOUT ANY WARRANTY; without even the implied warranty of -MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU -Lesser General Public License for more details. - -You should have received a copy of the GNU Lesser General Public -License along with this library; if not, write to the Free Software -Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA -*/ - -#include -#include -#include -#include - -/* CSP includes */ -#include - -#include - -uint32_t csp_get_ms(void) { - struct timespec ts; - - clock_serv_t cclock; - mach_timespec_t mts; - host_get_clock_service(mach_host_self(), CALENDAR_CLOCK, &cclock); - clock_get_time(cclock, &mts); - mach_port_deallocate(mach_task_self(), cclock); - ts.tv_sec = mts.tv_sec; - ts.tv_nsec = mts.tv_nsec; - - return (uint32_t)(ts.tv_sec*1000+ts.tv_nsec/1000000); -} - -uint32_t csp_get_ms_isr(void) { - return csp_get_ms(); -} - -uint32_t csp_get_s(void) { - struct timespec ts; - - clock_serv_t cclock; - mach_timespec_t mts; - host_get_clock_service(mach_host_self(), CALENDAR_CLOCK, &cclock); - clock_get_time(cclock, &mts); - mach_port_deallocate(mach_task_self(), cclock); - ts.tv_sec = mts.tv_sec; - ts.tv_nsec = mts.tv_nsec; - - return (uint32_t)ts.tv_sec; -} - -uint32_t csp_get_s_isr(void) { - return csp_get_s(); -} diff --git a/thirdparty/libcsp/src/arch/macosx/pthread_queue.c b/thirdparty/libcsp/src/arch/macosx/pthread_queue.c deleted file mode 100644 index c4ac8c1d..00000000 --- a/thirdparty/libcsp/src/arch/macosx/pthread_queue.c +++ /dev/null @@ -1,179 +0,0 @@ -/* -Cubesat Space Protocol - A small network-layer protocol designed for Cubesats -Copyright (C) 2012 Gomspace ApS (http://www.gomspace.com) -Copyright (C) 2012 AAUSAT3 Project (http://aausat3.space.aau.dk) - -This library is free software; you can redistribute it and/or -modify it under the terms of the GNU Lesser General Public -License as published by the Free Software Foundation; either -version 2.1 of the License, or (at your option) any later version. - -This library is distributed in the hope that it will be useful, -but WITHOUT ANY WARRANTY; without even the implied warranty of -MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU -Lesser General Public License for more details. - -You should have received a copy of the GNU Lesser General Public -License along with this library; if not, write to the Free Software -Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA -*/ - -/* -Inspired by c-pthread-queue by Matthew Dickinson -http://code.google.com/p/c-pthread-queue/ -*/ - -#include -#include -#include -#include -#include -#include -#include - -/* CSP includes */ -#include - -pthread_queue_t * pthread_queue_create(int length, size_t item_size) { - - pthread_queue_t * q = malloc(sizeof(pthread_queue_t)); - - if (q != NULL) { - q->buffer = malloc(length*item_size); - if (q->buffer != NULL) { - q->size = length; - q->item_size = item_size; - q->items = 0; - q->in = 0; - q->out = 0; - if (pthread_mutex_init(&(q->mutex), NULL) || pthread_cond_init(&(q->cond_full), NULL) || pthread_cond_init(&(q->cond_empty), NULL)) { - free(q->buffer); - free(q); - q = NULL; - } - } else { - free(q); - q = NULL; - } - } - - return q; - -} - -void pthread_queue_delete(pthread_queue_t * q) { - - if (q == NULL) - return; - - free(q->buffer); - free(q); - - return; - -} - -int pthread_queue_enqueue(pthread_queue_t * queue, void * value, uint32_t timeout) { - - int ret; - - /* Calculate timeout */ - struct timespec ts; - - clock_serv_t cclock; - mach_timespec_t mts; - host_get_clock_service(mach_host_self(), CALENDAR_CLOCK, &cclock); - clock_get_time(cclock, &mts); - mach_port_deallocate(mach_task_self(), cclock); - ts.tv_sec = mts.tv_sec; - ts.tv_nsec = mts.tv_nsec; - - uint32_t sec = timeout / 1000; - uint32_t nsec = (timeout - 1000 * sec) * 1000000; - - ts.tv_sec += sec; - - if (ts.tv_nsec + nsec > 1000000000) - ts.tv_sec++; - - ts.tv_nsec = (ts.tv_nsec + nsec) % 1000000000; - - /* Get queue lock */ - pthread_mutex_lock(&(queue->mutex)); - while (queue->items == queue->size) { - ret = pthread_cond_timedwait(&(queue->cond_full), &(queue->mutex), &ts); - if (ret != 0) { - pthread_mutex_unlock(&(queue->mutex)); - return PTHREAD_QUEUE_FULL; - } - } - - /* Coby object from input buffer */ - memcpy(queue->buffer+(queue->in * queue->item_size), value, queue->item_size); - queue->items++; - queue->in = (queue->in + 1) % queue->size; - pthread_mutex_unlock(&(queue->mutex)); - - /* Nofify blocked threads */ - pthread_cond_broadcast(&(queue->cond_empty)); - - return PTHREAD_QUEUE_OK; - -} - -int pthread_queue_dequeue(pthread_queue_t * queue, void * buf, uint32_t timeout) { - - int ret; - - /* Calculate timeout */ - struct timespec ts; - clock_serv_t cclock; - mach_timespec_t mts; - host_get_clock_service(mach_host_self(), CALENDAR_CLOCK, &cclock); - clock_get_time(cclock, &mts); - mach_port_deallocate(mach_task_self(), cclock); - ts.tv_sec = mts.tv_sec; - ts.tv_nsec = mts.tv_nsec; - - uint32_t sec = timeout / 1000; - uint32_t nsec = (timeout - 1000 * sec) * 1000000; - - ts.tv_sec += sec; - - if (ts.tv_nsec + nsec > 1000000000) - ts.tv_sec++; - - ts.tv_nsec = (ts.tv_nsec + nsec) % 1000000000; - - /* Get queue lock */ - pthread_mutex_lock(&(queue->mutex)); - while (queue->items == 0) { - ret = pthread_cond_timedwait(&(queue->cond_empty), &(queue->mutex), &ts); - if (ret != 0) { - pthread_mutex_unlock(&(queue->mutex)); - return PTHREAD_QUEUE_EMPTY; - } - } - - /* Coby object to output buffer */ - memcpy(buf, queue->buffer+(queue->out * queue->item_size), queue->item_size); - queue->items--; - queue->out = (queue->out + 1) % queue->size; - pthread_mutex_unlock(&(queue->mutex)); - - /* Nofify blocked threads */ - pthread_cond_broadcast(&(queue->cond_full)); - - return PTHREAD_QUEUE_OK; - -} - -int pthread_queue_items(pthread_queue_t * queue) { - - pthread_mutex_lock(&(queue->mutex)); - int items = queue->items; - pthread_mutex_unlock(&(queue->mutex)); - - return items; - -} diff --git a/thirdparty/libcsp/src/arch/posix/CMakeLists.txt b/thirdparty/libcsp/src/arch/posix/CMakeLists.txt deleted file mode 100644 index 6bf13773..00000000 --- a/thirdparty/libcsp/src/arch/posix/CMakeLists.txt +++ /dev/null @@ -1,9 +0,0 @@ -target_sources(${LIB_CSP_NAME} PRIVATE - csp_malloc.c - csp_queue.c - csp_semaphore.c - csp_system.c - csp_thread.c - csp_time.c - pthread_queue.c -) diff --git a/thirdparty/libcsp/src/arch/posix/csp_malloc.c b/thirdparty/libcsp/src/arch/posix/csp_malloc.c deleted file mode 100644 index 95bb8cc7..00000000 --- a/thirdparty/libcsp/src/arch/posix/csp_malloc.c +++ /dev/null @@ -1,31 +0,0 @@ -/* -Cubesat Space Protocol - A small network-layer protocol designed for Cubesats -Copyright (C) 2012 Gomspace ApS (http://www.gomspace.com) -Copyright (C) 2012 AAUSAT3 Project (http://aausat3.space.aau.dk) - -This library is free software; you can redistribute it and/or -modify it under the terms of the GNU Lesser General Public -License as published by the Free Software Foundation; either -version 2.1 of the License, or (at your option) any later version. - -This library is distributed in the hope that it will be useful, -but WITHOUT ANY WARRANTY; without even the implied warranty of -MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU -Lesser General Public License for more details. - -You should have received a copy of the GNU Lesser General Public -License along with this library; if not, write to the Free Software -Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA -*/ - -#include -#include - -void * csp_malloc(size_t size) { - return malloc(size); -} - -void csp_free(void *ptr) { - free(ptr); -} - diff --git a/thirdparty/libcsp/src/arch/posix/csp_queue.c b/thirdparty/libcsp/src/arch/posix/csp_queue.c deleted file mode 100644 index a2fb1b4f..00000000 --- a/thirdparty/libcsp/src/arch/posix/csp_queue.c +++ /dev/null @@ -1,64 +0,0 @@ -/* -Cubesat Space Protocol - A small network-layer protocol designed for Cubesats -Copyright (C) 2012 Gomspace ApS (http://www.gomspace.com) -Copyright (C) 2012 AAUSAT3 Project (http://aausat3.space.aau.dk) - -This library is free software; you can redistribute it and/or -modify it under the terms of the GNU Lesser General Public -License as published by the Free Software Foundation; either -version 2.1 of the License, or (at your option) any later version. - -This library is distributed in the hope that it will be useful, -but WITHOUT ANY WARRANTY; without even the implied warranty of -MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU -Lesser General Public License for more details. - -You should have received a copy of the GNU Lesser General Public -License along with this library; if not, write to the Free Software -Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA -*/ - -#include -#include - -/* CSP includes */ -#include - -#include -#include - - -csp_queue_handle_t csp_queue_create(int length, size_t item_size) { - return pthread_queue_create(length, item_size); -} - -void csp_queue_remove(csp_queue_handle_t queue) { - return pthread_queue_delete(queue); -} - -int csp_queue_enqueue(csp_queue_handle_t handle, void *value, uint32_t timeout) { - return pthread_queue_enqueue(handle, value, timeout); -} - -int csp_queue_enqueue_isr(csp_queue_handle_t handle, void * value, CSP_BASE_TYPE * task_woken) { - if (task_woken != NULL) - *task_woken = 0; - return csp_queue_enqueue(handle, value, 0); -} - -int csp_queue_dequeue(csp_queue_handle_t handle, void *buf, uint32_t timeout) { - return pthread_queue_dequeue(handle, buf, timeout); -} - -int csp_queue_dequeue_isr(csp_queue_handle_t handle, void *buf, CSP_BASE_TYPE * task_woken) { - *task_woken = 0; - return csp_queue_dequeue(handle, buf, 0); -} - -int csp_queue_size(csp_queue_handle_t handle) { - return pthread_queue_items(handle); -} - -int csp_queue_size_isr(csp_queue_handle_t handle) { - return pthread_queue_items(handle); -} diff --git a/thirdparty/libcsp/src/arch/posix/csp_semaphore.c b/thirdparty/libcsp/src/arch/posix/csp_semaphore.c deleted file mode 100644 index 6829dec2..00000000 --- a/thirdparty/libcsp/src/arch/posix/csp_semaphore.c +++ /dev/null @@ -1,164 +0,0 @@ -/* -Cubesat Space Protocol - A small network-layer protocol designed for Cubesats -Copyright (C) 2012 Gomspace ApS (http://www.gomspace.com) -Copyright (C) 2012 AAUSAT3 Project (http://aausat3.space.aau.dk) - -This library is free software; you can redistribute it and/or -modify it under the terms of the GNU Lesser General Public -License as published by the Free Software Foundation; either -version 2.1 of the License, or (at your option) any later version. - -This library is distributed in the hope that it will be useful, -but WITHOUT ANY WARRANTY; without even the implied warranty of -MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU -Lesser General Public License for more details. - -You should have received a copy of the GNU Lesser General Public -License along with this library; if not, write to the Free Software -Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA -*/ - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -/* CSP includes */ -#include - -#include - -int csp_mutex_create(csp_mutex_t * mutex) { - csp_log_lock("Mutex init: %p", mutex); - if (pthread_mutex_init(mutex, NULL) == 0) { - return CSP_SEMAPHORE_OK; - } else { - return CSP_SEMAPHORE_ERROR; - } -} - -int csp_mutex_remove(csp_mutex_t * mutex) { - if (pthread_mutex_destroy(mutex) == 0) { - return CSP_SEMAPHORE_OK; - } else { - return CSP_SEMAPHORE_ERROR; - } -} - -int csp_mutex_lock(csp_mutex_t * mutex, uint32_t timeout) { - - int ret; - struct timespec ts; - uint32_t sec, nsec; - - csp_log_lock("Wait: %p timeout %"PRIu32, mutex, timeout); - - if (timeout == CSP_INFINITY) { - ret = pthread_mutex_lock(mutex); - } else { - if (clock_gettime(CLOCK_REALTIME, &ts)) - return CSP_SEMAPHORE_ERROR; - - sec = timeout / 1000; - nsec = (timeout - 1000 * sec) * 1000000; - - ts.tv_sec += sec; - - if (ts.tv_nsec + nsec >= 1000000000) - ts.tv_sec++; - - ts.tv_nsec = (ts.tv_nsec + nsec) % 1000000000; - - ret = pthread_mutex_timedlock(mutex, &ts); - } - - if (ret != 0) - return CSP_SEMAPHORE_ERROR; - - return CSP_SEMAPHORE_OK; -} - -int csp_mutex_unlock(csp_mutex_t * mutex) { - if (pthread_mutex_unlock(mutex) == 0) { - return CSP_SEMAPHORE_OK; - } else { - return CSP_SEMAPHORE_ERROR; - } -} - -int csp_bin_sem_create(csp_bin_sem_handle_t * sem) { - csp_log_lock("Semaphore init: %p", sem); - if (sem_init(sem, 0, 1) == 0) { - return CSP_SEMAPHORE_OK; - } else { - return CSP_SEMAPHORE_ERROR; - } -} - -int csp_bin_sem_remove(csp_bin_sem_handle_t * sem) { - if (sem_destroy(sem) == 0) - return CSP_SEMAPHORE_OK; - else - return CSP_SEMAPHORE_ERROR; -} - -int csp_bin_sem_wait(csp_bin_sem_handle_t * sem, uint32_t timeout) { - - int ret; - struct timespec ts; - uint32_t sec, nsec; - - csp_log_lock("Wait: %p timeout %"PRIu32, sem, timeout); - - if (timeout == CSP_INFINITY) { - ret = sem_wait(sem); - } else { - if (clock_gettime(CLOCK_REALTIME, &ts)) - return CSP_SEMAPHORE_ERROR; - - sec = timeout / 1000; - nsec = (timeout - 1000 * sec) * 1000000; - - ts.tv_sec += sec; - - if (ts.tv_nsec + nsec >= 1000000000) - ts.tv_sec++; - - ts.tv_nsec = (ts.tv_nsec + nsec) % 1000000000; - - ret = sem_timedwait(sem, &ts); - } - - if (ret != 0) - return CSP_SEMAPHORE_ERROR; - - return CSP_SEMAPHORE_OK; -} - -int csp_bin_sem_post(csp_bin_sem_handle_t * sem) { - CSP_BASE_TYPE dummy = 0; - return csp_bin_sem_post_isr(sem, &dummy); -} - -int csp_bin_sem_post_isr(csp_bin_sem_handle_t * sem, CSP_BASE_TYPE * task_woken) { - csp_log_lock("Post: %p", sem); - *task_woken = 0; - - int value; - sem_getvalue(sem, &value); - if (value > 0) - return CSP_SEMAPHORE_OK; - - if (sem_post(sem) == 0) { - return CSP_SEMAPHORE_OK; - } else { - return CSP_SEMAPHORE_ERROR; - } -} diff --git a/thirdparty/libcsp/src/arch/posix/csp_system.c b/thirdparty/libcsp/src/arch/posix/csp_system.c deleted file mode 100644 index 6c882c7c..00000000 --- a/thirdparty/libcsp/src/arch/posix/csp_system.c +++ /dev/null @@ -1,131 +0,0 @@ -/* -Cubesat Space Protocol - A small network-layer protocol designed for Cubesats -Copyright (C) 2012 Gomspace ApS (http://www.gomspace.com) -Copyright (C) 2012 AAUSAT3 Project (http://aausat3.space.aau.dk) - -This library is free software; you can redistribute it and/or -modify it under the terms of the GNU Lesser General Public -License as published by the Free Software Foundation; either -version 2.1 of the License, or (at your option) any later version. - -This library is distributed in the hope that it will be useful, -but WITHOUT ANY WARRANTY; without even the implied warranty of -MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU -Lesser General Public License for more details. - -You should have received a copy of the GNU Lesser General Public -License along with this library; if not, write to the Free Software -Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA -*/ - -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include - -#include - -int csp_sys_tasklist(char * out) { - strcpy(out, "Tasklist not available on POSIX"); - return CSP_ERR_NONE; -} - -int csp_sys_tasklist_size(void) { - return 100; -} - -uint32_t csp_sys_memfree(void) { - uint32_t total = 0; - struct sysinfo info; - sysinfo(&info); - total = info.freeram * info.mem_unit; - return total; -} - -int csp_sys_reboot(void) { -#ifdef CSP_USE_INIT_SHUTDOWN - /* Let init(1) handle the reboot */ - int ret = system("reboot"); - (void) ret; /* Silence warning */ -#else - int magic = LINUX_REBOOT_CMD_RESTART; - - /* Sync filesystem before reboot */ - sync(); - reboot(magic); -#endif - - /* If reboot(2) returns, it is an error */ - csp_log_error("Failed to reboot: %s", strerror(errno)); - - return CSP_ERR_INVAL; -} - -int csp_sys_shutdown(void) { -#ifdef CSP_USE_INIT_SHUTDOWN - /* Let init(1) handle the shutdown */ - int ret = system("halt"); - (void) ret; /* Silence warning */ -#else - int magic = LINUX_REBOOT_CMD_HALT; - - /* Sync filesystem before reboot */ - sync(); - reboot(magic); -#endif - - /* If reboot(2) returns, it is an error */ - csp_log_error("Failed to shutdown: %s", strerror(errno)); - - return CSP_ERR_INVAL; -} - -void csp_sys_set_color(csp_color_t color) { - - unsigned int color_code, modifier_code; - switch (color & COLOR_MASK_COLOR) { - case COLOR_BLACK: - color_code = 30; break; - case COLOR_RED: - color_code = 31; break; - case COLOR_GREEN: - color_code = 32; break; - case COLOR_YELLOW: - color_code = 33; break; - case COLOR_BLUE: - color_code = 34; break; - case COLOR_MAGENTA: - color_code = 35; break; - case COLOR_CYAN: - color_code = 36; break; - case COLOR_WHITE: - color_code = 37; break; - case COLOR_RESET: - default: - color_code = 0; break; - } - - switch (color & COLOR_MASK_MODIFIER) { - case COLOR_BOLD: - modifier_code = 1; break; - case COLOR_UNDERLINE: - modifier_code = 2; break; - case COLOR_BLINK: - modifier_code = 3; break; - case COLOR_HIDE: - modifier_code = 4; break; - case COLOR_NORMAL: - default: - modifier_code = 0; break; - } - - printf("\033[%u;%um", modifier_code, color_code); -} diff --git a/thirdparty/libcsp/src/arch/posix/csp_thread.c b/thirdparty/libcsp/src/arch/posix/csp_thread.c deleted file mode 100644 index 3277d35d..00000000 --- a/thirdparty/libcsp/src/arch/posix/csp_thread.c +++ /dev/null @@ -1,55 +0,0 @@ -/* -Cubesat Space Protocol - A small network-layer protocol designed for Cubesats -Copyright (C) 2012 Gomspace ApS (http://www.gomspace.com) -Copyright (C) 2012 AAUSAT3 Project (http://aausat3.space.aau.dk) - -This library is free software; you can redistribute it and/or -modify it under the terms of the GNU Lesser General Public -License as published by the Free Software Foundation; either -version 2.1 of the License, or (at your option) any later version. - -This library is distributed in the hope that it will be useful, -but WITHOUT ANY WARRANTY; without even the implied warranty of -MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU -Lesser General Public License for more details. - -You should have received a copy of the GNU Lesser General Public -License along with this library; if not, write to the Free Software -Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA -*/ - -#include -#include -#include - -/* CSP includes */ -#include - -#include - -int csp_thread_create(csp_thread_return_t (* routine)(void *), const char * const thread_name, unsigned short stack_depth, void * parameters, unsigned int priority, csp_thread_handle_t * handle) { - pthread_attr_t attributes, *attr_ref; - int return_code; - - if( pthread_attr_init(&attributes) == 0 ) - { - unsigned int stack_size = PTHREAD_STACK_MIN;// use at least one memory page - - while(stack_size < stack_depth)// must reach at least the provided size - { - stack_size += PTHREAD_STACK_MIN;// keep memory page boundary (some systems may fail otherwise)) - } - attr_ref = &attributes; - - pthread_attr_setdetachstate(attr_ref, PTHREAD_CREATE_DETACHED);// do not waste memory on each call - pthread_attr_setstacksize(attr_ref, stack_size); - } - else - { - attr_ref = NULL; - } - return_code = pthread_create(handle, attr_ref, routine, parameters); - if( attr_ref != NULL ) pthread_attr_destroy(attr_ref); - - return return_code; -} diff --git a/thirdparty/libcsp/src/arch/posix/csp_time.c b/thirdparty/libcsp/src/arch/posix/csp_time.c deleted file mode 100644 index c9677443..00000000 --- a/thirdparty/libcsp/src/arch/posix/csp_time.c +++ /dev/null @@ -1,54 +0,0 @@ -/* -Cubesat Space Protocol - A small network-layer protocol designed for Cubesats -Copyright (C) 2012 Gomspace ApS (http://www.gomspace.com) -Copyright (C) 2012 AAUSAT3 Project (http://aausat3.space.aau.dk) - -This library is free software; you can redistribute it and/or -modify it under the terms of the GNU Lesser General Public -License as published by the Free Software Foundation; either -version 2.1 of the License, or (at your option) any later version. - -This library is distributed in the hope that it will be useful, -but WITHOUT ANY WARRANTY; without even the implied warranty of -MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU -Lesser General Public License for more details. - -You should have received a copy of the GNU Lesser General Public -License along with this library; if not, write to the Free Software -Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA -*/ - -#include -#include -#include - -/* CSP includes */ -#include - -#include - -uint32_t csp_get_ms(void) { - struct timespec ts; - - if (clock_gettime(CLOCK_MONOTONIC, &ts) == 0) - return (uint32_t)(ts.tv_sec*1000+ts.tv_nsec/1000000); - else - return 0; -} - -uint32_t csp_get_ms_isr(void) { - return csp_get_ms(); -} - -uint32_t csp_get_s(void) { - struct timespec ts; - - if (clock_gettime(CLOCK_MONOTONIC, &ts) == 0) - return (uint32_t)ts.tv_sec; - else - return 0; -} - -uint32_t csp_get_s_isr(void) { - return csp_get_s(); -} diff --git a/thirdparty/libcsp/src/arch/posix/pthread_queue.c b/thirdparty/libcsp/src/arch/posix/pthread_queue.c deleted file mode 100644 index e8b6d4ab..00000000 --- a/thirdparty/libcsp/src/arch/posix/pthread_queue.c +++ /dev/null @@ -1,243 +0,0 @@ -/* -Cubesat Space Protocol - A small network-layer protocol designed for Cubesats -Copyright (C) 2012 Gomspace ApS (http://www.gomspace.com) -Copyright (C) 2012 AAUSAT3 Project (http://aausat3.space.aau.dk) - -This library is free software; you can redistribute it and/or -modify it under the terms of the GNU Lesser General Public -License as published by the Free Software Foundation; either -version 2.1 of the License, or (at your option) any later version. - -This library is distributed in the hope that it will be useful, -but WITHOUT ANY WARRANTY; without even the implied warranty of -MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU -Lesser General Public License for more details. - -You should have received a copy of the GNU Lesser General Public -License along with this library; if not, write to the Free Software -Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA -*/ - -/* -Inspired by c-pthread-queue by Matthew Dickinson -http://code.google.com/p/c-pthread-queue/ -*/ - -#include -#include -#include -#include -#include -#include -#include - -/* CSP includes */ -#include - -static inline int get_deadline(struct timespec *ts, uint32_t timeout_ms) -{ - int ret = clock_gettime(CLOCK_MONOTONIC, ts); - - if (ret < 0) { - return ret; - } - - uint32_t sec = timeout_ms / 1000; - uint32_t nsec = (timeout_ms - 1000 * sec) * 1000000; - - ts->tv_sec += sec; - - if (ts->tv_nsec + nsec >= 1000000000) { - ts->tv_sec++; - } - - ts->tv_nsec = (ts->tv_nsec + nsec) % 1000000000; - - return ret; -} - -static inline int init_cond_clock_monotonic(pthread_cond_t * cond) -{ - - int ret; - pthread_condattr_t attr; - - pthread_condattr_init(&attr); - ret = pthread_condattr_setclock(&attr, CLOCK_MONOTONIC); - - if (ret == 0) { - ret = pthread_cond_init(cond, &attr); - } - - pthread_condattr_destroy(&attr); - return ret; - -} - -pthread_queue_t * pthread_queue_create(int length, size_t item_size) { - - pthread_queue_t * q = malloc(sizeof(pthread_queue_t)); - - if (q != NULL) { - q->buffer = malloc(length*item_size); - if (q->buffer != NULL) { - q->size = length; - q->item_size = item_size; - q->items = 0; - q->in = 0; - q->out = 0; - if (pthread_mutex_init(&(q->mutex), NULL) || init_cond_clock_monotonic(&(q->cond_full)) || init_cond_clock_monotonic(&(q->cond_empty))) { - free(q->buffer); - free(q); - q = NULL; - } - } else { - free(q); - q = NULL; - } - } - - return q; - -} - -void pthread_queue_delete(pthread_queue_t * q) { - - if (q == NULL) - return; - - free(q->buffer); - free(q); - - return; - -} - - -static inline int wait_slot_available(pthread_queue_t * queue, struct timespec *ts) { - - int ret; - - while (queue->items == queue->size) { - - if (ts != NULL) { - ret = pthread_cond_timedwait(&(queue->cond_full), &(queue->mutex), ts); - } else { - ret = pthread_cond_wait(&(queue->cond_full), &(queue->mutex)); - } - - if (ret != 0 && errno != EINTR) { - return PTHREAD_QUEUE_FULL; //Timeout - } - } - - return PTHREAD_QUEUE_OK; - -} - -int pthread_queue_enqueue(pthread_queue_t * queue, void * value, uint32_t timeout) { - - int ret; - struct timespec ts; - struct timespec *pts = NULL; - - /* Calculate timeout */ - if (timeout != CSP_MAX_DELAY) { - if (get_deadline(&ts, timeout) != 0) { - return PTHREAD_QUEUE_ERROR; - } - pts = &ts; - } else { - pts = NULL; - } - - /* Get queue lock */ - pthread_mutex_lock(&(queue->mutex)); - - ret = wait_slot_available(queue, pts); - if (ret == PTHREAD_QUEUE_OK) { - /* Copy object from input buffer */ - memcpy(queue->buffer+(queue->in * queue->item_size), value, queue->item_size); - queue->items++; - queue->in = (queue->in + 1) % queue->size; - } - - pthread_mutex_unlock(&(queue->mutex)); - - if (ret == PTHREAD_QUEUE_OK) { - /* Nofify blocked threads */ - pthread_cond_broadcast(&(queue->cond_empty)); - } - - return ret; - -} - -static inline int wait_item_available(pthread_queue_t * queue, struct timespec *ts) { - - int ret; - - while (queue->items == 0) { - - if (ts != NULL) { - ret = pthread_cond_timedwait(&(queue->cond_empty), &(queue->mutex), ts); - } else { - ret = pthread_cond_wait(&(queue->cond_empty), &(queue->mutex)); - } - - if (ret != 0 && errno != EINTR) { - return PTHREAD_QUEUE_EMPTY; //Timeout - } - } - - return PTHREAD_QUEUE_OK; - -} - -int pthread_queue_dequeue(pthread_queue_t * queue, void * buf, uint32_t timeout) { - - int ret; - struct timespec ts; - struct timespec *pts; - - /* Calculate timeout */ - if (timeout != CSP_MAX_DELAY) { - if (get_deadline(&ts, timeout) != 0) { - return PTHREAD_QUEUE_ERROR; - } - pts = &ts; - } else { - pts = NULL; - } - - /* Get queue lock */ - pthread_mutex_lock(&(queue->mutex)); - - ret = wait_item_available(queue, pts); - if (ret == PTHREAD_QUEUE_OK) { - /* Coby object to output buffer */ - memcpy(buf, queue->buffer+(queue->out * queue->item_size), queue->item_size); - queue->items--; - queue->out = (queue->out + 1) % queue->size; - } - - pthread_mutex_unlock(&(queue->mutex)); - - if (ret == PTHREAD_QUEUE_OK) { - /* Nofify blocked threads */ - pthread_cond_broadcast(&(queue->cond_full)); - } - - return ret; - -} - -int pthread_queue_items(pthread_queue_t * queue) { - - pthread_mutex_lock(&(queue->mutex)); - int items = queue->items; - pthread_mutex_unlock(&(queue->mutex)); - - return items; - -} diff --git a/thirdparty/libcsp/src/arch/windows/README b/thirdparty/libcsp/src/arch/windows/README deleted file mode 100644 index b97ce7f5..00000000 --- a/thirdparty/libcsp/src/arch/windows/README +++ /dev/null @@ -1,18 +0,0 @@ -This directory contains files specific to the windows port of libcsp. - -To compile and create a static library, execute: - - python waf configure --with-os=windows build - -from the root of this project. Note python must be in PATH. - -The build requirements are: - * Windows Vista SP1 - * A recent version of MinGW _or_ MinGW-w64 - * Windows API headers - * cPython 2.5 or newer - -What provides the Windows API headers depends on the development environment: -Using MinGW: Headers provided by w32api package. windows_glue.h header is needed because these headers do not declare condition variables. -Using MinGW-w64: Headers should be available in the default configuration. You may have to compile the distribution from source. windows_glue.h should not be needed. - diff --git a/thirdparty/libcsp/src/arch/windows/csp_malloc.c b/thirdparty/libcsp/src/arch/windows/csp_malloc.c deleted file mode 100644 index 4b301e49..00000000 --- a/thirdparty/libcsp/src/arch/windows/csp_malloc.c +++ /dev/null @@ -1,9 +0,0 @@ -#include - -void * csp_malloc(size_t size) { - return malloc(size); -} - -void csp_free(void * ptr) { - free(ptr); -} diff --git a/thirdparty/libcsp/src/arch/windows/csp_queue.c b/thirdparty/libcsp/src/arch/windows/csp_queue.c deleted file mode 100644 index 177f8fa9..00000000 --- a/thirdparty/libcsp/src/arch/windows/csp_queue.c +++ /dev/null @@ -1,40 +0,0 @@ -#include -#include -#include -#include "windows_queue.h" - -csp_queue_handle_t csp_queue_create(int length, size_t item_size) { - return windows_queue_create(length, item_size); -} - -void csp_queue_remove(csp_queue_handle_t queue) { - windows_queue_delete(queue); -} - -int csp_queue_enqueue(csp_queue_handle_t handle, void *value, uint32_t timeout) { - return windows_queue_enqueue(handle, value, timeout); -} - -int csp_queue_enqueue_isr(csp_queue_handle_t handle, void * value, CSP_BASE_TYPE * task_woken) { - if( task_woken != NULL ) - *task_woken = 0; - return windows_queue_enqueue(handle, value, 0); -} - -int csp_queue_dequeue(csp_queue_handle_t handle, void *buf, uint32_t timeout) { - return windows_queue_dequeue(handle, buf, timeout); -} - -int csp_queue_dequeue_isr(csp_queue_handle_t handle, void * buf, CSP_BASE_TYPE * task_woken) { - if( task_woken != NULL ) - *task_woken = 0; - return windows_queue_dequeue(handle, buf, 0); -} - -int csp_queue_size(csp_queue_handle_t handle) { - return windows_queue_items(handle); -} - -int csp_queue_size_isr(csp_queue_handle_t handle) { - return windows_queue_items(handle); -} diff --git a/thirdparty/libcsp/src/arch/windows/csp_semaphore.c b/thirdparty/libcsp/src/arch/windows/csp_semaphore.c deleted file mode 100644 index aa69251e..00000000 --- a/thirdparty/libcsp/src/arch/windows/csp_semaphore.c +++ /dev/null @@ -1,74 +0,0 @@ -#include -#include -#include - -int csp_mutex_create(csp_mutex_t * mutex) { - HANDLE mutexHandle = CreateMutex(NULL, FALSE, FALSE); - if( mutexHandle == NULL ) { - return CSP_MUTEX_ERROR; - } - *mutex = mutexHandle; - return CSP_MUTEX_OK; -} - -int csp_mutex_remove(csp_mutex_t * mutex) { - if( !CloseHandle(*mutex) ) { - return CSP_MUTEX_ERROR; - } - return CSP_MUTEX_OK; -} - -int csp_mutex_lock(csp_mutex_t * mutex, uint32_t timeout) { - if(WaitForSingleObject(*mutex, timeout) == WAIT_OBJECT_0) { - return CSP_MUTEX_OK; - } - return CSP_MUTEX_ERROR; - -} - -int csp_mutex_unlock(csp_mutex_t * mutex) { - if( !ReleaseMutex(*mutex) ) { - return CSP_MUTEX_ERROR; - } - return CSP_MUTEX_OK; -} - -int csp_bin_sem_create(csp_bin_sem_handle_t * sem) { - HANDLE semHandle = CreateSemaphore(NULL, 1, 1, NULL); - if( semHandle == NULL ) { - return CSP_SEMAPHORE_ERROR; - } - *sem = semHandle; - return CSP_SEMAPHORE_OK; -} - -int csp_bin_sem_remove(csp_bin_sem_handle_t * sem) { - if( !CloseHandle(*sem) ) { - return CSP_SEMAPHORE_ERROR; - } - return CSP_SEMAPHORE_OK; -} - -int csp_bin_sem_wait(csp_bin_sem_handle_t * sem, uint32_t timeout) { - if( WaitForSingleObject(*sem, timeout) == WAIT_OBJECT_0 ) { - return CSP_SEMAPHORE_OK; - } - return CSP_SEMAPHORE_ERROR; - -} - -int csp_bin_sem_post(csp_bin_sem_handle_t * sem) { - if( !ReleaseSemaphore(*sem, 1, NULL) ) { - return CSP_SEMAPHORE_ERROR; - } - return CSP_SEMAPHORE_OK; -} - -int csp_bin_sem_post_isr(csp_bin_sem_handle_t * sem, CSP_BASE_TYPE * task_woken) { - if( task_woken != NULL ) { - *task_woken = 0; - } - return csp_bin_sem_post(sem); -} - - diff --git a/thirdparty/libcsp/src/arch/windows/csp_system.c b/thirdparty/libcsp/src/arch/windows/csp_system.c deleted file mode 100644 index 262c2052..00000000 --- a/thirdparty/libcsp/src/arch/windows/csp_system.c +++ /dev/null @@ -1,60 +0,0 @@ -/* -Cubesat Space Protocol - A small network-layer protocol designed for Cubesats -Copyright (C) 2012 Gomspace ApS (http://www.gomspace.com) -Copyright (C) 2012 AAUSAT3 Project (http://aausat3.space.aau.dk) - -This library is free software; you can redistribute it and/or -modify it under the terms of the GNU Lesser General Public -License as published by the Free Software Foundation; either -version 2.1 of the License, or (at your option) any later version. - -This library is distributed in the hope that it will be useful, -but WITHOUT ANY WARRANTY; without even the implied warranty of -MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU -Lesser General Public License for more details. - -You should have received a copy of the GNU Lesser General Public -License along with this library; if not, write to the Free Software -Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA -*/ - -#include -#include -#include - -#include -#include - -#include - -int csp_sys_tasklist(char * out) { - strcpy(out, "Tasklist not available on Windows"); - return CSP_ERR_NONE; -} - -uint32_t csp_sys_memfree(void) { - MEMORYSTATUSEX statex; - statex.dwLength = sizeof(statex); - GlobalMemoryStatusEx(&statex); - DWORDLONG freePhysicalMem = statex.ullAvailPhys; - size_t total = (size_t) freePhysicalMem; - return (uint32_t)total; -} - -int csp_sys_reboot(void) { - /* TODO: Fix reboot on Windows */ - csp_log_error("Failed to reboot"); - - return CSP_ERR_INVAL; -} - -int csp_sys_shutdown(void) { - /* TODO: Fix shutdown on Windows */ - csp_log_error("Failed to shutdown"); - - return CSP_ERR_INVAL; -} - -void csp_sys_set_color(csp_color_t color) { - /* TODO: Add Windows color output here */ -} diff --git a/thirdparty/libcsp/src/arch/windows/csp_thread.c b/thirdparty/libcsp/src/arch/windows/csp_thread.c deleted file mode 100644 index ef46a948..00000000 --- a/thirdparty/libcsp/src/arch/windows/csp_thread.c +++ /dev/null @@ -1,11 +0,0 @@ -#include -#include -#include - -int csp_thread_create(csp_thread_return_t (* routine)(void *)__attribute__((stdcall)), const char * const thread_name, unsigned short stack_depth, void * parameters, unsigned int priority, csp_thread_handle_t * handle) { - HANDLE taskHandle = (HANDLE) _beginthreadex(NULL, stack_depth, routine, parameters, 0, 0); - if( taskHandle == 0 ) - return CSP_ERR_NOMEM; // Failure - *handle = taskHandle; - return CSP_ERR_NONE; -} diff --git a/thirdparty/libcsp/src/arch/windows/csp_time.c b/thirdparty/libcsp/src/arch/windows/csp_time.c deleted file mode 100644 index 618292ab..00000000 --- a/thirdparty/libcsp/src/arch/windows/csp_time.c +++ /dev/null @@ -1,20 +0,0 @@ -#include -#include -#include - -uint32_t csp_get_ms(void) { - return (uint32_t)GetTickCount(); -} - -uint32_t csp_get_ms_isr(void) { - return csp_get_ms(); -} - -uint32_t csp_get_s(void) { - uint32_t time_ms = csp_get_ms(); - return time_ms/1000; -} - -uint32_t csp_get_s_isr(void) { - return csp_get_s(); -} diff --git a/thirdparty/libcsp/src/arch/windows/windows_glue.h b/thirdparty/libcsp/src/arch/windows/windows_glue.h deleted file mode 100644 index 6e0cf6db..00000000 --- a/thirdparty/libcsp/src/arch/windows/windows_glue.h +++ /dev/null @@ -1,23 +0,0 @@ -#ifndef WINDOWS_GLUE_H -#define WINDOWS_GLUE_H - -#include -#undef interface - -#if (_WIN32_WINNT >= 0x0600) - -#define RTL_CONDITION_VARIABLE_INIT 0 -#define RTL_CONDITION_VARIABLE_LOCKMODE_SHARED 1 -#define CONDITION_VARIABLE_INIT RTL_CONDITION_VARIABLE_INIT -#define CONDITION_VARIABLE_LOCKMODE_SHARED RTL_CONDITION_VARIABLE_LOCKMODE_SHARED - -typedef PVOID RTL_CONDITION_VARIABLE; -typedef RTL_CONDITION_VARIABLE CONDITION_VARIABLE, *PCONDITION_VARIABLE; - -WINBASEAPI VOID WINAPI InitializeConditionVariable(PCONDITION_VARIABLE ConditionVariable); -WINBASEAPI WINBOOL WINAPI SleepConditionVariableCS(PCONDITION_VARIABLE ConditionVariable, PCRITICAL_SECTION CriticalSection, DWORD dwMilliseconds); -WINBASEAPI VOID WINAPI WakeAllConditionVariable(PCONDITION_VARIABLE ConditionVariable); -WINBASEAPI VOID WINAPI WakeConditionVariable(PCONDITION_VARIABLE ConditionVariable); - -#endif // _WIN#"_WINNT -#endif diff --git a/thirdparty/libcsp/src/arch/windows/windows_queue.c b/thirdparty/libcsp/src/arch/windows/windows_queue.c deleted file mode 100644 index aa337dc8..00000000 --- a/thirdparty/libcsp/src/arch/windows/windows_queue.c +++ /dev/null @@ -1,91 +0,0 @@ -#include "windows_queue.h" -#include "windows_glue.h" -#include - -static int queueFull(windows_queue_t * queue) { - return queue->items == queue->size; -} - -static int queueEmpty(windows_queue_t * queue) { - return queue->items == 0; -} - -windows_queue_t * windows_queue_create(int length, size_t item_size) { - windows_queue_t *queue = (windows_queue_t*)malloc(sizeof(windows_queue_t)); - if(queue == NULL) - goto queue_malloc_failed; - - queue->buffer = malloc(length*item_size); - if(queue->buffer == NULL) - goto buffer_malloc_failed; - - queue->size = length; - queue->item_size = item_size; - queue->items = 0; - queue->head_idx = 0; - - InitializeCriticalSection(&(queue->mutex)); - InitializeConditionVariable(&(queue->cond_full)); - InitializeConditionVariable(&(queue->cond_empty)); - goto queue_init_success; - -buffer_malloc_failed: - free(queue); - queue = NULL; -queue_malloc_failed: -queue_init_success: - return queue; -} - -void windows_queue_delete(windows_queue_t * q) { - if(q==NULL) return; - DeleteCriticalSection(&(q->mutex)); - free(q->buffer); - free(q); -} - -int windows_queue_enqueue(windows_queue_t * queue, void * value, int timeout) { - int offset; - EnterCriticalSection(&(queue->mutex)); - while(queueFull(queue)) { - int ret = SleepConditionVariableCS(&(queue->cond_full), &(queue->mutex), timeout); - if( !ret ) { - LeaveCriticalSection(&(queue->mutex)); - return ret == WAIT_TIMEOUT ? WINDOWS_QUEUE_FULL : WINDOWS_QUEUE_ERROR; - } - } - offset = ((queue->head_idx+queue->items) % queue->size) * queue->item_size; - memcpy((unsigned char*)queue->buffer + offset, value, queue->item_size); - queue->items++; - - LeaveCriticalSection(&(queue->mutex)); - WakeAllConditionVariable(&(queue->cond_empty)); - return WINDOWS_QUEUE_OK; -} - -int windows_queue_dequeue(windows_queue_t * queue, void * buf, int timeout) { - EnterCriticalSection(&(queue->mutex)); - while(queueEmpty(queue)) { - int ret = SleepConditionVariableCS(&(queue->cond_empty), &(queue->mutex), timeout); - if( !ret ) { - LeaveCriticalSection(&(queue->mutex)); - return ret == WAIT_TIMEOUT ? WINDOWS_QUEUE_EMPTY : WINDOWS_QUEUE_ERROR; - } - } - memcpy(buf, (unsigned char*)queue->buffer+(queue->head_idx%queue->size*queue->item_size), queue->item_size); - queue->items--; - queue->head_idx = (queue->head_idx + 1) % queue->size; - - LeaveCriticalSection(&(queue->mutex)); - WakeAllConditionVariable(&(queue->cond_full)); - return WINDOWS_QUEUE_OK; -} - -int windows_queue_items(windows_queue_t * queue) { - int items; - EnterCriticalSection(&(queue->mutex)); - items = queue->items; - LeaveCriticalSection(&(queue->mutex)); - - return items; -} diff --git a/thirdparty/libcsp/src/arch/windows/windows_queue.h b/thirdparty/libcsp/src/arch/windows/windows_queue.h deleted file mode 100644 index e6bc5423..00000000 --- a/thirdparty/libcsp/src/arch/windows/windows_queue.h +++ /dev/null @@ -1,41 +0,0 @@ -#ifndef _WINDOWS_QUEUE_H_ -#define _WINDOWS_QUEUE_H_ - -#ifdef __cplusplus -extern "C" { -#endif - -#include -#include "windows_glue.h" -#undef interface - -#include - -#define WINDOWS_QUEUE_ERROR CSP_QUEUE_ERROR -#define WINDOWS_QUEUE_EMPTY CSP_QUEUE_ERROR -#define WINDOWS_QUEUE_FULL CSP_QUEUE_ERROR -#define WINDOWS_QUEUE_OK CSP_QUEUE_OK - -typedef struct windows_queue_s { - void * buffer; - int size; - int item_size; - int items; - int head_idx; - CRITICAL_SECTION mutex; - CONDITION_VARIABLE cond_full; - CONDITION_VARIABLE cond_empty; -} windows_queue_t; - -windows_queue_t * windows_queue_create(int length, size_t item_size); -void windows_queue_delete(windows_queue_t * q); -int windows_queue_enqueue(windows_queue_t * queue, void * value, int timeout); -int windows_queue_dequeue(windows_queue_t * queue, void * buf, int timeout); -int windows_queue_items(windows_queue_t * queue); - -#ifdef __cplusplus -} /* extern "C" */ -#endif - -#endif // _WINDOWS_QUEUE_H_ - diff --git a/thirdparty/libcsp/src/bindings/python/pycsp.c b/thirdparty/libcsp/src/bindings/python/pycsp.c deleted file mode 100644 index f1009d1a..00000000 --- a/thirdparty/libcsp/src/bindings/python/pycsp.c +++ /dev/null @@ -1,1052 +0,0 @@ -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#if PY_MAJOR_VERSION == 3 -#define IS_PY3 -#endif - -static int is_capsule_of_type(PyObject* capsule, const char* expected_type) { - const char* capsule_name = PyCapsule_GetName(capsule); - if (strcmp(capsule_name, expected_type) != 0) { - PyErr_Format( - PyExc_TypeError, - "capsule contains unexpected type, expected=%s, got=%s", - expected_type, capsule_name); // TypeError is thrown - return 0; - } - return 1; -} - -/** - * csp/csp.h - */ - -/* - * void csp_service_handler(csp_conn_t *conn, csp_packet_t *packet); - */ -static PyObject* pycsp_service_handler(PyObject *self, PyObject *args) { - PyObject* conn_capsule; - PyObject* packet_capsule; - if (!PyArg_ParseTuple(args, "OO", &conn_capsule, &packet_capsule)) { - return NULL; // TypeError is thrown - } - - if (!is_capsule_of_type(conn_capsule, "csp_conn_t") || - !is_capsule_of_type(packet_capsule, "csp_packet_t")) { - return NULL; // TypeError is thrown - } - - csp_service_handler( - (csp_conn_t*)PyCapsule_GetPointer(conn_capsule, "csp_conn_t"), - (csp_packet_t*)PyCapsule_GetPointer(packet_capsule, "csp_packet_t")); - - Py_RETURN_NONE; -} - -/* - * int csp_init(uint8_t my_node_address); - */ -static PyObject* pycsp_init(PyObject *self, PyObject *args) { - uint8_t my_node_address; - if (!PyArg_ParseTuple(args, "b", &my_node_address)) { - return NULL; // TypeError is thrown - } - - return Py_BuildValue("i", csp_init(my_node_address)); -} - -/* - * void csp_set_hostname(const char *hostname); - */ -static PyObject* pycsp_set_hostname(PyObject *self, PyObject *args) { - char* hostname; - if (!PyArg_ParseTuple(args, "s", &hostname)) { - return NULL; // TypeError is thrown - } - - csp_set_hostname(hostname); - Py_RETURN_NONE; -} - -/* - * const char *csp_get_hostname(void); - */ -static PyObject* pycsp_get_hostname(PyObject *self, PyObject *args) { - return Py_BuildValue("s", csp_get_hostname()); -} - -/* - * void csp_set_model(const char *model); - */ -static PyObject* pycsp_set_model(PyObject *self, PyObject *args) { - char* model; - if (!PyArg_ParseTuple(args, "s", &model)) { - return NULL; // TypeError is thrown - } - - csp_set_model(model); - Py_RETURN_NONE; -} - -/* - * const char *csp_get_model(void); - */ -static PyObject* pycsp_get_model(PyObject *self, PyObject *args) { - return Py_BuildValue("s", csp_get_model()); -} - -/* - * void csp_set_revision(const char *revision); - */ -static PyObject* pycsp_set_revision(PyObject *self, PyObject *args) { - char* revision; - if (!PyArg_ParseTuple(args, "s", &revision)) { - return NULL; // TypeError is thrown - } - - csp_set_revision(revision); - Py_RETURN_NONE; -} - -/* - * const char *csp_get_revision(void); - */ -static PyObject* pycsp_get_revision(PyObject *self, PyObject *args) { - return Py_BuildValue("s", csp_get_revision()); -} - -/* - * csp_socket_t *csp_socket(uint32_t opts); - */ -static PyObject* pycsp_socket(PyObject *self, PyObject *args) { - uint32_t opts = CSP_SO_NONE; - if (!PyArg_ParseTuple(args, "|I", &opts)) { - return NULL; // TypeError is thrown - } - - return PyCapsule_New(csp_socket(opts), "csp_socket_t", NULL); -} - -/* - * csp_conn_t *csp_accept(csp_socket_t *socket, uint32_t timeout); - */ -static PyObject* pycsp_accept(PyObject *self, PyObject *args) { - PyObject* socket_capsule; - uint32_t timeout = 500; - if (!PyArg_ParseTuple(args, "O|I", &socket_capsule, &timeout)) { - return NULL; // TypeError is thrown - } - - if (!is_capsule_of_type(socket_capsule, "csp_socket_t")) { - return NULL; // TypeError is thrown - } - - void* socket = PyCapsule_GetPointer(socket_capsule, "csp_socket_t"); - csp_conn_t* conn = csp_accept((csp_socket_t*)socket, timeout); - if (conn == NULL) { - Py_RETURN_NONE; // because a capsule cannot contain a NULL-pointer - } - - return PyCapsule_New(conn, "csp_conn_t", NULL); -} - -/* - * csp_packet_t *csp_read(csp_conn_t *conn, uint32_t timeout); - */ -static PyObject* pycsp_read(PyObject *self, PyObject *args) { - PyObject* conn_capsule; - uint32_t timeout = 500; - if (!PyArg_ParseTuple(args, "O|I", &conn_capsule, &timeout)) { - return NULL; // TypeError is thrown - } - - if (!is_capsule_of_type(conn_capsule, "csp_conn_t")) { - return NULL; // TypeError is thrown - } - - void* conn = PyCapsule_GetPointer(conn_capsule, "csp_conn_t"); - csp_packet_t* packet = csp_read((csp_conn_t*)conn, timeout); - if (packet == NULL) { - Py_RETURN_NONE; // because capsule cannot contain a NULL-pointer - } - - return PyCapsule_New(packet, "csp_packet_t", NULL); -} - -/* -* int csp_send(csp_conn_t * conn, csp_packet_t * packet, uint32_t timeout) -*/ -static PyObject* pycsp_send(PyObject *self, PyObject *args) { - PyObject* conn_capsule; - PyObject* packet_capsule; - uint32_t timeout = 500; - if (!PyArg_ParseTuple(args, "OO|I", &conn_capsule, &packet_capsule, &timeout)) { - return NULL; // TypeError is thrown - } - - if (!is_capsule_of_type(conn_capsule, "csp_conn_t")) { - return NULL; // TypeError is thrown - } - - void* packet = PyCapsule_GetPointer(packet_capsule, "csp_packet_t"); - if (packet == NULL) { - Py_RETURN_NONE; - } - - void* conn = PyCapsule_GetPointer(conn_capsule, "csp_conn_t"); - - int result = csp_send(conn, packet, timeout); - - return Py_BuildValue("i", result); -} - -/* - * int csp_transaction(uint8_t prio, uint8_t dest, uint8_t port, - * uint32_t timeout, void *outbuf, int outlen, - * void *inbuf, int inlen); - */ -static PyObject* pycsp_transaction(PyObject *self, PyObject *args) { - uint8_t prio; - uint8_t dest; - uint8_t port; - uint32_t timeout; - Py_buffer inbuf; - Py_buffer outbuf; - if (!PyArg_ParseTuple(args, "bbbIw*w*", &prio, &dest, &port, &timeout, &outbuf, &inbuf)) { - return NULL; // TypeError is thrown - } - - int result = csp_transaction(prio, dest, port, timeout, - outbuf.buf, outbuf.len, - inbuf.buf, inbuf.len); - - return Py_BuildValue("i", result); -} - -/* int csp_sendto(uint8_t prio, uint8_t dest, uint8_t dport, uint8_t src_port, uint32_t opts, csp_packet_t *packet, uint32_t timeout); */ -static PyObject* pycsp_sendto(PyObject *self, PyObject *args) { - uint8_t prio; - uint8_t dest; - uint8_t dport; - uint8_t src_port; - uint32_t opts; - PyObject* packet_capsule; - uint32_t timeout; - if (!PyArg_ParseTuple(args, "bbbbIOI", &prio, &dest, &dport, &src_port, &opts, &packet_capsule, &timeout)) { - Py_RETURN_NONE; - } - - void* packet = PyCapsule_GetPointer(packet_capsule, "csp_packet_t"); - if (packet == NULL) { - Py_RETURN_NONE; - } - - return Py_BuildValue("i", csp_sendto(prio, - dest, - dport, - src_port, - opts, - (csp_packet_t*)packet, - timeout)); -} - - -/* - * int csp_sendto_reply(csp_packet_t * request_packet, - * csp_packet_t * reply_packet, - * uint32_t opts, uint32_t timeout); - */ -static PyObject* pycsp_sendto_reply(PyObject *self, PyObject *args) { - PyObject* request_packet_capsule; - PyObject* reply_packet_capsule; - uint32_t opts = CSP_O_NONE; - uint32_t timeout = 500; - if (!PyArg_ParseTuple(args, "OO|II", &request_packet_capsule, &reply_packet_capsule, &opts, &timeout)) { - return NULL; // TypeError is thrown - } - - if (!is_capsule_of_type(request_packet_capsule, "csp_packet_t") || - !is_capsule_of_type(reply_packet_capsule, "csp_packet_t")) { - return NULL; // TypeError is thrown - } - - void* request_packet = PyCapsule_GetPointer(request_packet_capsule, "csp_packet_t"); - void* reply_packet = PyCapsule_GetPointer(reply_packet_capsule, "csp_packet_t"); - - return Py_BuildValue("i", csp_sendto_reply((csp_packet_t*)request_packet, - (csp_packet_t*)reply_packet, - opts, - timeout)); -} - -/* - * csp_conn_t *csp_connect(uint8_t prio, uint8_t dest, uint8_t dport, uint32_t timeout, uint32_t opts); - */ -static PyObject* pycsp_connect(PyObject *self, PyObject *args) { - uint8_t prio; - uint8_t dest; - uint8_t dport; - uint32_t timeout; - uint32_t opts; - if (!PyArg_ParseTuple(args, "bbbII", &prio, &dest, &dport, &timeout, &opts)) { - return NULL; // TypeError is thrown - } - - csp_conn_t *conn = csp_connect(prio, dest, dport, timeout,opts); - - return PyCapsule_New(conn, "csp_conn_t", NULL); -} - -/* - * int csp_close(csp_conn_t *conn); - */ -static PyObject* pycsp_close(PyObject *self, PyObject *conn_capsule) { - if (!is_capsule_of_type(conn_capsule, "csp_conn_t")) { - return NULL; // TypeError is thrown - } - - void *conn = PyCapsule_GetPointer(conn_capsule, "csp_conn_t"); - return Py_BuildValue("i", csp_close((csp_conn_t*)conn)); -} - -/* - * int csp_conn_dport(csp_conn_t *conn); - */ -static PyObject* pycsp_conn_dport(PyObject *self, PyObject *conn_capsule) { - if (!is_capsule_of_type(conn_capsule, "csp_conn_t")) { - return NULL; // TypeError is thrown - } - - void* conn = PyCapsule_GetPointer(conn_capsule, "csp_conn_t"); - return Py_BuildValue("i", csp_conn_dport((csp_conn_t*)conn)); -} - -/* - * int csp_conn_sport(csp_conn_t *conn); - */ -static PyObject* pycsp_conn_sport(PyObject *self, PyObject *conn_capsule) { - if (!is_capsule_of_type(conn_capsule, "csp_conn_t")) { - return NULL; // TypeError is thrown - } - - void* conn = PyCapsule_GetPointer(conn_capsule, "csp_conn_t"); - return Py_BuildValue("i", csp_conn_sport((csp_conn_t*)conn)); -} - -/* int csp_conn_dst(csp_conn_t *conn); */ -static PyObject* pycsp_conn_dst(PyObject *self, PyObject *conn_capsule) { - if (!is_capsule_of_type(conn_capsule, "csp_conn_t")) { - return NULL; // TypeError is thrown - } - - void* conn = PyCapsule_GetPointer(conn_capsule, "csp_conn_t"); - return Py_BuildValue("i", csp_conn_dst((csp_conn_t*)conn)); -} - -/* - * int csp_conn_src(csp_conn_t *conn); - */ -static PyObject* pycsp_conn_src(PyObject *self, PyObject *conn_capsule) { - if (!is_capsule_of_type(conn_capsule, "csp_conn_t")) { - return NULL; // TypeError is thrown - } - - void* conn = PyCapsule_GetPointer(conn_capsule, "csp_conn_t"); - return Py_BuildValue("i", csp_conn_src((csp_conn_t*)conn)); -} - -/* int csp_listen(csp_socket_t *socket, size_t conn_queue_length); */ -static PyObject* pycsp_listen(PyObject *self, PyObject *args) { - PyObject* socket_capsule; - size_t conn_queue_len = 10; - if (!PyArg_ParseTuple(args, "O|n", &socket_capsule, &conn_queue_len)) { - return NULL; // TypeError is thrown - } - - if (!is_capsule_of_type(socket_capsule, "csp_socket_t")) { - return NULL; // TypeError is thrown - } - - void* sock = PyCapsule_GetPointer(socket_capsule, "csp_socket_t"); - return Py_BuildValue("i", csp_listen((csp_socket_t*)sock, conn_queue_len)); -} - -/* int csp_bind(csp_socket_t *socket, uint8_t port); */ -static PyObject* pycsp_bind(PyObject *self, PyObject *args) { - PyObject* socket_capsule; - uint8_t port; - if (!PyArg_ParseTuple(args, "Ob", &socket_capsule, &port)) { - return NULL; // TypeError is thrown - } - - if (!is_capsule_of_type(socket_capsule, "csp_socket_t")) { - return NULL; // TypeError is thrown - } - - void* sock = PyCapsule_GetPointer(socket_capsule, "csp_socket_t"); - return Py_BuildValue("i", csp_bind((csp_socket_t*)sock, port)); -} - -/* int csp_route_start_task(unsigned int task_stack_size, unsigned int priority); */ -static PyObject* pycsp_route_start_task(PyObject *self, PyObject *args) { - unsigned int priority = CSP_PRIO_NORM; - if (!PyArg_ParseTuple(args, "|I", &priority)) { - return NULL; // TypeError is thrown - } - - return Py_BuildValue("i", csp_route_start_task(0, priority)); -} - -/* - * int csp_ping(uint8_t node, uint32_t timeout, - * unsigned int size, uint8_t conn_options); - */ -static PyObject* pycsp_ping(PyObject *self, PyObject *args) { - uint8_t node; - uint32_t timeout = 500; - unsigned int size = 100; - uint8_t conn_options = CSP_O_NONE; - if (!PyArg_ParseTuple(args, "b|IIb", &node, &timeout, &size, &conn_options)) { - return NULL; // TypeError is thrown - } - - return Py_BuildValue("i", csp_ping(node, timeout, size, conn_options)); -} - -/* - * void csp_reboot(uint8_t node); - */ -static PyObject* pycsp_reboot(PyObject *self, PyObject *args) { - uint8_t node; - if (!PyArg_ParseTuple(args, "b", &node)) { - return NULL; // TypeError is thrown - } - - csp_reboot(node); - Py_RETURN_NONE; -} - -/* - * void csp_shutdown(uint8_t node); - */ -static PyObject* pycsp_shutdown(PyObject *self, PyObject *args) { - uint8_t node; - if (!PyArg_ParseTuple(args, "b", &node)) { - return NULL; // TypeError is thrown - } - - csp_shutdown(node); - Py_RETURN_NONE; -} - -/* - * void csp_rdp_set_opt(unsigned int window_size, - * unsigned int conn_timeout_ms, - * unsigned int packet_timeout_ms, - * unsigned int delayed_acks, - * unsigned int ack_timeout, - * unsigned int ack_delay_count); - */ -static PyObject* pycsp_rdp_set_opt(PyObject *self, PyObject *args) { - unsigned int window_size; - unsigned int conn_timeout_ms; - unsigned int packet_timeout_ms; - unsigned int delayed_acks; - unsigned int ack_timeout; - unsigned int ack_delay_count; - if (!PyArg_ParseTuple(args, "IIIIII", &window_size, &conn_timeout_ms, - &packet_timeout_ms, &delayed_acks, - &ack_timeout, &ack_delay_count)) { - return NULL; // TypeError is thrown - } -#ifdef CSP_USE_RDP - csp_rdp_set_opt(window_size, conn_timeout_ms, packet_timeout_ms, - delayed_acks, ack_timeout, ack_delay_count); -#endif - Py_RETURN_NONE; -} - -/* - * void csp_rdp_get_opt(unsigned int *window_size, - * unsigned int *conn_timeout_ms, - * unsigned int *packet_timeout_ms, - * unsigned int *delayed_acks, - * unsigned int *ack_timeout, - * unsigned int *ack_delay_count); - */ -static PyObject* pycsp_rdp_get_opt(PyObject *self, PyObject *args) { - - unsigned int window_size = 0; - unsigned int conn_timeout_ms = 0; - unsigned int packet_timeout_ms = 0; - unsigned int delayed_acks = 0; - unsigned int ack_timeout = 0; - unsigned int ack_delay_count = 0; -#ifdef CSP_USE_RDP - csp_rdp_get_opt(&window_size, - &conn_timeout_ms, - &packet_timeout_ms, - &delayed_acks, - &ack_timeout, - &ack_delay_count); -#endif - return Py_BuildValue("IIIIII", - window_size, - conn_timeout_ms, - packet_timeout_ms, - delayed_acks, - ack_timeout, - ack_delay_count); -} - - -/* - * - * int csp_xtea_set_key(char *key, uint32_t keylen); - */ -static PyObject* pycsp_xtea_set_key(PyObject *self, PyObject *args) { - char* key; - uint32_t keylen; - if (!PyArg_ParseTuple(args, "si", &key, &keylen)) { - return NULL; // TypeError is thrown - } - return Py_BuildValue("i", csp_xtea_set_key(key, keylen)); -} -/** - * csp/csp_rtable.h - */ - -/* - * int csp_rtable_set(uint8_t node, uint8_t mask, - * csp_iface_t *ifc, uint8_t mac); - */ -static PyObject* pycsp_rtable_set(PyObject *self, PyObject *args) { - uint8_t node; - uint8_t mask; - char* interface_name; - uint8_t mac = CSP_NODE_MAC; - if (!PyArg_ParseTuple(args, "bbs|b", &node, &mask, &interface_name, &mac)) { - return NULL; // TypeError is thrown - } - - return Py_BuildValue("i", csp_rtable_set(node, - mask, - csp_iflist_get_by_name(interface_name), - mac)); -} - -/* - * void csp_rtable_clear(void); - */ -static PyObject* pycsp_rtable_clear(PyObject *self, PyObject *args) { - csp_rtable_clear(); - Py_RETURN_NONE; -} - -/* -* int csp_rtable_check(const char * buffer) -*/ -static PyObject* pycsp_rtable_check(PyObject *self, PyObject *args) { - char* buffer; - if (!PyArg_ParseTuple(args, "s", &buffer)) { - return NULL; // TypeError is thrown - } - - return Py_BuildValue("i", csp_rtable_check(buffer)); -} - -/* -* void csp_rtable_load(const char * buffer) -*/ -static PyObject* pycsp_rtable_load(PyObject *self, PyObject *args) { - char* buffer; - if (!PyArg_ParseTuple(args, "s", &buffer)) { - return NULL; // TypeError is thrown - } - - csp_rtable_load(buffer); - Py_RETURN_NONE; -} - -/** - * csp/csp_buffer.h - */ - -/* - * int csp_buffer_init(int count, int size); - */ -static PyObject* pycsp_buffer_init(PyObject *self, PyObject *args) { - int count; - int size; - if (!PyArg_ParseTuple(args, "ii", &count, &size)) { - return NULL; // TypeError is thrown - } - - return Py_BuildValue("i", csp_buffer_init(count, size)); -} - -/* - * void * csp_buffer_get(size_t size); - */ -static PyObject* pycsp_buffer_get(PyObject *self, PyObject *args) { - size_t size; - if (!PyArg_ParseTuple(args, "n", &size)) { - return NULL; // TypeError is thrown - } - - void* packet = csp_buffer_get(size); - if (packet == NULL) { - Py_RETURN_NONE; - } - - return PyCapsule_New(packet, "csp_packet_t", NULL); -} -/* - * void csp_buffer_free(void *packet); - */ -static PyObject* pycsp_buffer_free(PyObject *self, PyObject *args) { - PyObject* packet_capsule; - if (!PyArg_ParseTuple(args, "O", &packet_capsule)) { - return NULL; // TypeError is thrown - } - - - if (!is_capsule_of_type(packet_capsule, "csp_packet_t")) { - return NULL; // TypeError is thrown - } - - csp_buffer_free(PyCapsule_GetPointer(packet_capsule, "csp_packet_t")); - Py_RETURN_NONE; -} - -/* - * int csp_buffer_remaining(void); - */ -static PyObject* pycsp_buffer_remaining(PyObject *self, PyObject *args) { - return Py_BuildValue("i", csp_buffer_remaining()); -} - -/** - * csp/csp_cmp.h - */ - -/* - * static inline int csp_cmp_ident(uint8_t node, uint32_t timeout, - * struct csp_cmp_message *msg) - */ -static PyObject* pycsp_cmp_ident(PyObject *self, PyObject *args) { - uint8_t node; - uint32_t timeout = 500; - if (!PyArg_ParseTuple(args, "b|i", &node, &timeout)) { - return NULL; // TypeError is thrown - } - - struct csp_cmp_message msg; - int rc = csp_cmp_ident(node, timeout, &msg); - return Py_BuildValue("isssss", - rc, - msg.ident.hostname, - msg.ident.model, - msg.ident.revision, - msg.ident.date, - msg.ident.time); -} - -/* - * static inline int csp_cmp_route_set(uint8_t node, uint32_t timeout, - * struct csp_cmp_message *msg) - */ -static PyObject* pycsp_cmp_route_set(PyObject *self, PyObject *args) { - uint8_t node; - uint32_t timeout = 500; - uint8_t addr; - uint8_t mac; - char* ifstr; - if (!PyArg_ParseTuple(args, "bibbs", &node, &timeout, &addr, &mac, &ifstr)) { - return NULL; // TypeError is thrown - } - - struct csp_cmp_message msg; - msg.route_set.dest_node = addr; - msg.route_set.next_hop_mac = mac; - strncpy(msg.route_set.interface, ifstr, CSP_CMP_ROUTE_IFACE_LEN); - int rc = csp_cmp_route_set(node, timeout, &msg); - return Py_BuildValue("i", - rc); -} - -/* static inline int pycsp_cmp_peek(uint8_t node, uint32_t timeout, struct csp_cmp_message *msg); */ -static PyObject* pycsp_cmp_peek(PyObject *self, PyObject *args) { - uint8_t node; - uint32_t timeout; - uint8_t len; - uint32_t addr; - Py_buffer outbuf; - - if (!PyArg_ParseTuple(args, "biibw*", &node, &timeout, &addr, &len, &outbuf)) { - Py_RETURN_NONE; - } - - if (len > CSP_CMP_PEEK_MAX_LEN) { - len = CSP_CMP_PEEK_MAX_LEN; - } - struct csp_cmp_message msg; - msg.peek.addr = csp_hton32(addr); - msg.peek.len = len; - int rc = csp_cmp_peek(node, timeout, &msg); - if (rc != CSP_ERR_NONE) { - Py_RETURN_NONE; - } - memcpy(outbuf.buf, msg.peek.data, len); - outbuf.len = len; - - return Py_BuildValue("i", rc); -} - -/* static inline int pycsp_cmp_poke(uint8_t node, uint32_t timeout, struct csp_cmp_message *msg); */ -static PyObject* pycsp_cmp_poke(PyObject *self, PyObject *args) { - uint8_t node; - uint32_t timeout; - uint8_t len; - uint32_t addr; - Py_buffer inbuf; - - if (!PyArg_ParseTuple(args, "biibw*", &node, &timeout, &addr, &len, &inbuf)) { - Py_RETURN_NONE; - } - - if (len > CSP_CMP_POKE_MAX_LEN) { - len = CSP_CMP_POKE_MAX_LEN; - } - struct csp_cmp_message msg; - msg.poke.addr = csp_hton32(addr); - msg.poke.len = len; - memcpy(msg.poke.data, inbuf.buf, len); - int rc = csp_cmp_poke(node, timeout, &msg); - if (rc != CSP_ERR_NONE) { - Py_RETURN_NONE; - } - - return Py_BuildValue("i", rc); -} - -/* static inline int csp_cmp_clock(uint8_t node, uint32_t timeout, struct csp_cmp_message *msg); */ -static PyObject* pycsp_cmp_clock(PyObject *self, PyObject *args) { - uint8_t node; - uint32_t timeout; - uint32_t sec; - uint32_t nsec; - if (!PyArg_ParseTuple(args, "bIII", &node, &timeout, &sec, &nsec)) { - Py_RETURN_NONE; - } - - struct csp_cmp_message msg; - msg.clock.tv_sec = csp_hton32(sec); - msg.clock.tv_nsec = csp_hton32(nsec); - return Py_BuildValue("i", csp_cmp_clock(node, timeout, &msg)); -} - -/** - * csp/interfaces/csp_if_zmqhub.h - */ - -/* - * int csp_zmqhub_init(char addr, char * host); - */ -static PyObject* pycsp_zmqhub_init(PyObject *self, PyObject *args) { - char addr; - char* host; - if (!PyArg_ParseTuple(args, "bs", &addr, &host)) { - return NULL; // TypeError is thrown - } - - return Py_BuildValue("i", csp_zmqhub_init(addr, host)); -} - -/** - * csp/drivers/can_socketcan.h - */ - -/* - * csp_iface_t * csp_can_socketcan_init(const char * ifc, int bitrate, int promisc); - */ -static PyObject* pycsp_can_socketcan_init(PyObject *self, PyObject *args) -{ - char* ifc; - int bitrate = 1000000; - int promisc = 0; - - if (!PyArg_ParseTuple(args, "s|ii", &ifc, &bitrate, &promisc)) - { - return NULL; - } - - csp_can_socketcan_init(ifc, bitrate, promisc); - Py_RETURN_NONE; -} - - -/** - * csp/interfaces/csp_if_kiss.h - */ - -/* - * int csp_kiss_init(char addr, char * host); - */ -static PyObject* pycsp_kiss_init(PyObject *self, PyObject *args) { - char* device; - uint32_t baudrate = 500000; - uint32_t mtu = 512; - const char* if_name = "KISS"; - if (!PyArg_ParseTuple(args, "s|IIs", &device, &baudrate, &mtu, &if_name)) { - return NULL; // TypeError is thrown - } - - static csp_iface_t csp_if_kiss; - static csp_kiss_handle_t csp_kiss_driver; - csp_if_kiss.mtu = (uint16_t) mtu; - struct usart_conf conf = {.device = device, .baudrate = baudrate}; - csp_kiss_init(&csp_if_kiss, &csp_kiss_driver, usart_putc, usart_insert, if_name); - usart_init(&conf); - - void my_usart_rx(uint8_t * buf, int len, void * pxTaskWoken) { - csp_kiss_rx(&csp_if_kiss, buf, len, pxTaskWoken); - } - usart_set_callback(my_usart_rx); - - Py_RETURN_NONE; -} - -/** - * Helpers - accessing csp_packet_t members - */ -static PyObject* pycsp_packet_set_data(PyObject *self, PyObject *args) { - PyObject* packet_capsule; - Py_buffer data; - if (!PyArg_ParseTuple(args, "Ow*", &packet_capsule, &data)) { - return NULL; // TypeError is thrown - } - - if (!is_capsule_of_type(packet_capsule, "csp_packet_t")) { - return NULL; // TypeError is thrown - } - - csp_packet_t* packet = PyCapsule_GetPointer(packet_capsule, "csp_packet_t"); - - memcpy((char *)packet->data, data.buf, data.len); - packet->length = data.len; - - Py_RETURN_NONE; -} -static PyObject* pycsp_packet_get_data(PyObject *self, PyObject *packet_capsule) { - if (!is_capsule_of_type(packet_capsule, "csp_packet_t")) { - return NULL; // TypeError is thrown - } - - csp_packet_t* packet = PyCapsule_GetPointer(packet_capsule, "csp_packet_t"); -#ifdef IS_PY3 - return Py_BuildValue("y#", packet->data, packet->length); -#else - return Py_BuildValue("s#", packet->data, packet->length); -#endif -} - -static PyObject* pycsp_packet_get_length(PyObject *self, PyObject *packet_capsule) { - if (!is_capsule_of_type(packet_capsule, "csp_packet_t")) { - return NULL; // TypeError is thrown - } - - csp_packet_t* packet = PyCapsule_GetPointer(packet_capsule, "csp_packet_t"); - return Py_BuildValue("H", packet->length); -} - -static PyMethodDef methods[] = { - - /* csp/csp.h */ - {"service_handler", pycsp_service_handler, METH_VARARGS, ""}, - {"init", pycsp_init, METH_VARARGS, ""}, - {"set_hostname", pycsp_set_hostname, METH_VARARGS, ""}, - {"get_hostname", pycsp_get_hostname, METH_NOARGS, ""}, - {"set_model", pycsp_set_model, METH_VARARGS, ""}, - {"get_model", pycsp_get_model, METH_NOARGS, ""}, - {"set_revision", pycsp_set_revision, METH_VARARGS, ""}, - {"get_revision", pycsp_get_revision, METH_NOARGS, ""}, - {"socket", pycsp_socket, METH_VARARGS, ""}, - {"accept", pycsp_accept, METH_VARARGS, ""}, - {"read", pycsp_read, METH_VARARGS, ""}, - {"send", pycsp_send, METH_VARARGS, ""}, - {"transaction", pycsp_transaction, METH_VARARGS, ""}, - {"sendto_reply", pycsp_sendto_reply, METH_VARARGS, ""}, - {"sendto", pycsp_sendto, METH_VARARGS, ""}, - {"connect", pycsp_connect, METH_VARARGS, ""}, - {"close", pycsp_close, METH_O, ""}, - {"conn_dport", pycsp_conn_dport, METH_O, ""}, - {"conn_sport", pycsp_conn_sport, METH_O, ""}, - {"conn_dst", pycsp_conn_dst, METH_O, ""}, - {"conn_src", pycsp_conn_src, METH_O, ""}, - {"listen", pycsp_listen, METH_VARARGS, ""}, - {"bind", pycsp_bind, METH_VARARGS, ""}, - {"route_start_task", pycsp_route_start_task, METH_VARARGS, ""}, - {"ping", pycsp_ping, METH_VARARGS, ""}, - {"reboot", pycsp_reboot, METH_VARARGS, ""}, - {"shutdown", pycsp_shutdown, METH_VARARGS, ""}, - {"rdp_set_opt", pycsp_rdp_set_opt, METH_VARARGS, ""}, - {"rdp_get_opt", pycsp_rdp_get_opt, METH_NOARGS, ""}, - {"xtea_set_key", pycsp_xtea_set_key, METH_VARARGS, ""}, - - /* csp/csp_rtable.h */ - {"rtable_set", pycsp_rtable_set, METH_VARARGS, ""}, - {"rtable_clear", pycsp_rtable_clear, METH_NOARGS, ""}, - {"rtable_check", pycsp_rtable_check, METH_VARARGS, ""}, - {"rtable_load", pycsp_rtable_load, METH_VARARGS, ""}, - - /* csp/csp_buffer.h */ - {"buffer_init", pycsp_buffer_init, METH_VARARGS, ""}, - {"buffer_free", pycsp_buffer_free, METH_VARARGS, ""}, - {"buffer_get", pycsp_buffer_get, METH_VARARGS, ""}, - {"buffer_remaining", pycsp_buffer_remaining, METH_NOARGS, ""}, - - /* csp/csp_cmp.h */ - {"cmp_ident", pycsp_cmp_ident, METH_VARARGS, ""}, - {"cmp_route_set", pycsp_cmp_route_set, METH_VARARGS, ""}, - {"cmp_peek", pycsp_cmp_peek, METH_VARARGS, ""}, - {"cmp_poke", pycsp_cmp_poke, METH_VARARGS, ""}, - {"cmp_clock", pycsp_cmp_clock, METH_VARARGS, ""}, - - - /* csp/interfaces/csp_if_zmqhub.h */ - {"zmqhub_init", pycsp_zmqhub_init, METH_VARARGS, ""}, - {"kiss_init", pycsp_kiss_init, METH_VARARGS, ""}, - - /* csp/drivers/can_socketcan.h */ - {"can_socketcan_init", pycsp_can_socketcan_init, METH_VARARGS, ""}, - - /* helpers */ - {"packet_get_length", pycsp_packet_get_length, METH_O, ""}, - {"packet_get_data", pycsp_packet_get_data, METH_O, ""}, - {"packet_set_data", pycsp_packet_set_data, METH_VARARGS, ""}, - - /* sentinel */ - {NULL, NULL, 0, NULL} -}; - -#ifdef IS_PY3 -static struct PyModuleDef moduledef = { - PyModuleDef_HEAD_INIT, - "libcsp_py3", - NULL, - -1, - methods, - NULL, - NULL, - NULL, - NULL -}; -#endif - -#ifdef IS_PY3 -PyMODINIT_FUNC PyInit_libcsp_py3(void) { -#else - PyMODINIT_FUNC initlibcsp_py2(void) { -#endif - - PyObject* m; - -#ifdef IS_PY3 - m = PyModule_Create(&moduledef); -#else - m = Py_InitModule("libcsp_py2", methods); -#endif - /** - * csp/csp_types.h - */ - - /* RESERVED PORTS */ - PyModule_AddIntConstant(m, "CSP_CMP", CSP_CMP); - PyModule_AddIntConstant(m, "CSP_PING", CSP_PING); - PyModule_AddIntConstant(m, "CSP_PS", CSP_PS); - PyModule_AddIntConstant(m, "CSP_MEMFREE", CSP_MEMFREE); - PyModule_AddIntConstant(m, "CSP_REBOOT", CSP_REBOOT); - PyModule_AddIntConstant(m, "CSP_BUF_FREE", CSP_BUF_FREE); - PyModule_AddIntConstant(m, "CSP_UPTIME", CSP_UPTIME); - PyModule_AddIntConstant(m, "CSP_ANY", CSP_MAX_BIND_PORT + 1); - PyModule_AddIntConstant(m, "CSP_PROMISC", CSP_MAX_BIND_PORT + 2); - - /* PRIORITIES */ - PyModule_AddIntConstant(m, "CSP_PRIO_CRITICAL", CSP_PRIO_CRITICAL); - PyModule_AddIntConstant(m, "CSP_PRIO_HIGH", CSP_PRIO_HIGH); - PyModule_AddIntConstant(m, "CSP_PRIO_NORM", CSP_PRIO_NORM); - PyModule_AddIntConstant(m, "CSP_PRIO_LOW", CSP_PRIO_LOW); - - /* FLAGS */ - PyModule_AddIntConstant(m, "CSP_FFRAG", CSP_FFRAG); - PyModule_AddIntConstant(m, "CSP_FHMAC", CSP_FHMAC); - PyModule_AddIntConstant(m, "CSP_FXTEA", CSP_FXTEA); - PyModule_AddIntConstant(m, "CSP_FRDP", CSP_FRDP); - PyModule_AddIntConstant(m, "CSP_FCRC32", CSP_FCRC32); - - /* SOCKET OPTIONS */ - PyModule_AddIntConstant(m, "CSP_SO_NONE", CSP_SO_NONE); - PyModule_AddIntConstant(m, "CSP_SO_RDPREQ", CSP_SO_RDPREQ); - PyModule_AddIntConstant(m, "CSP_SO_RDPPROHIB", CSP_SO_RDPPROHIB); - PyModule_AddIntConstant(m, "CSP_SO_HMACREQ", CSP_SO_HMACREQ); - PyModule_AddIntConstant(m, "CSP_SO_HMACPROHIB", CSP_SO_HMACPROHIB); - PyModule_AddIntConstant(m, "CSP_SO_XTEAREQ", CSP_SO_XTEAREQ); - PyModule_AddIntConstant(m, "CSP_SO_XTEAPROHIB", CSP_SO_XTEAPROHIB); - PyModule_AddIntConstant(m, "CSP_SO_CRC32REQ", CSP_SO_CRC32REQ); - PyModule_AddIntConstant(m, "CSP_SO_CRC32PROHIB", CSP_SO_CRC32PROHIB); - PyModule_AddIntConstant(m, "CSP_SO_CONN_LESS", CSP_SO_CONN_LESS); - - /* CONNECT OPTIONS */ - PyModule_AddIntConstant(m, "CSP_O_NONE", CSP_O_NONE); - PyModule_AddIntConstant(m, "CSP_O_RDP", CSP_O_RDP); - PyModule_AddIntConstant(m, "CSP_O_NORDP", CSP_O_NORDP); - PyModule_AddIntConstant(m, "CSP_O_HMAC", CSP_O_HMAC); - PyModule_AddIntConstant(m, "CSP_O_NOHMAC", CSP_O_NOHMAC); - PyModule_AddIntConstant(m, "CSP_O_XTEA", CSP_O_XTEA); - PyModule_AddIntConstant(m, "CSP_O_NOXTEA", CSP_O_NOXTEA); - PyModule_AddIntConstant(m, "CSP_O_CRC32", CSP_O_CRC32); - PyModule_AddIntConstant(m, "CSP_O_NOCRC32", CSP_O_NOCRC32); - - - /** - * csp/csp_error.h - */ - - PyModule_AddIntConstant(m, "CSP_ERR_NONE", CSP_ERR_NONE); - PyModule_AddIntConstant(m, "CSP_ERR_NOMEM", CSP_ERR_NOMEM); - PyModule_AddIntConstant(m, "CSP_ERR_INVAL", CSP_ERR_INVAL); - PyModule_AddIntConstant(m, "CSP_ERR_TIMEDOUT", CSP_ERR_TIMEDOUT); - PyModule_AddIntConstant(m, "CSP_ERR_USED", CSP_ERR_USED); - PyModule_AddIntConstant(m, "CSP_ERR_NOTSUP", CSP_ERR_NOTSUP); - PyModule_AddIntConstant(m, "CSP_ERR_BUSY", CSP_ERR_BUSY); - PyModule_AddIntConstant(m, "CSP_ERR_ALREADY", CSP_ERR_ALREADY); - PyModule_AddIntConstant(m, "CSP_ERR_RESET", CSP_ERR_RESET); - PyModule_AddIntConstant(m, "CSP_ERR_NOBUFS", CSP_ERR_NOBUFS); - PyModule_AddIntConstant(m, "CSP_ERR_TX", CSP_ERR_TX); - PyModule_AddIntConstant(m, "CSP_ERR_DRIVER", CSP_ERR_DRIVER); - PyModule_AddIntConstant(m, "CSP_ERR_AGAIN", CSP_ERR_AGAIN); - PyModule_AddIntConstant(m, "CSP_ERR_HMAC", CSP_ERR_HMAC); - PyModule_AddIntConstant(m, "CSP_ERR_XTEA", CSP_ERR_XTEA); - PyModule_AddIntConstant(m, "CSP_ERR_CRC32", CSP_ERR_CRC32); - - /** - * csp/rtable.h - */ - PyModule_AddIntConstant(m, "CSP_NODE_MAC", CSP_NODE_MAC); - -#ifdef IS_PY3 - return m; -#endif - } - diff --git a/thirdparty/libcsp/src/crypto/CMakeLists.txt b/thirdparty/libcsp/src/crypto/CMakeLists.txt deleted file mode 100644 index 19cb878a..00000000 --- a/thirdparty/libcsp/src/crypto/CMakeLists.txt +++ /dev/null @@ -1,5 +0,0 @@ -target_sources(${LIB_CSP_NAME} PRIVATE - csp_hmac.c - csp_sha1.c - csp_xtea.c -) diff --git a/thirdparty/libcsp/src/crypto/csp_hmac.c b/thirdparty/libcsp/src/crypto/csp_hmac.c deleted file mode 100644 index ae7fbb00..00000000 --- a/thirdparty/libcsp/src/crypto/csp_hmac.c +++ /dev/null @@ -1,202 +0,0 @@ -/* -Cubesat Space Protocol - A small network-layer protocol designed for Cubesats -Copyright (C) 2012 GomSpace ApS (http://www.gomspace.com) -Copyright (C) 2012 AAUSAT3 Project (http://aausat3.space.aau.dk) - -This library is free software; you can redistribute it and/or -modify it under the terms of the GNU Lesser General Public -License as published by the Free Software Foundation; either -version 2.1 of the License, or (at your option) any later version. - -This library is distributed in the hope that it will be useful, -but WITHOUT ANY WARRANTY; without even the implied warranty of -MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU -Lesser General Public License for more details. - -You should have received a copy of the GNU Lesser General Public -License along with this library; if not, write to the Free Software -Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA -*/ - -/* Hash-based Message Authentication Code - based on code from libtom.org */ - -#include -#include -#include - -/* CSP includes */ -#include - -#include -#include - -#ifdef CSP_USE_HMAC - -#define HMAC_KEY_LENGTH 16 - -/* HMAC key */ -static uint8_t csp_hmac_key[HMAC_KEY_LENGTH]; - -/* HMAC state structure */ -typedef struct { - csp_sha1_state md; - uint8_t key[SHA1_BLOCKSIZE]; -} hmac_state; - -static int csp_hmac_init(hmac_state * hmac, const uint8_t * key, uint32_t keylen) { - uint32_t i; - uint8_t buf[SHA1_BLOCKSIZE]; - - /* NULL pointer and key check */ - if (!hmac || !key || keylen < 1) - return CSP_ERR_INVAL; - - /* Make sure we have a large enough key */ - if(keylen > SHA1_BLOCKSIZE) { - csp_sha1_memory(key, keylen, hmac->key); - if(SHA1_DIGESTSIZE < SHA1_BLOCKSIZE) - memset((hmac->key) + SHA1_DIGESTSIZE, 0, (size_t)(SHA1_BLOCKSIZE - SHA1_DIGESTSIZE)); - } else { - memcpy(hmac->key, key, (size_t)keylen); - if(keylen < SHA1_BLOCKSIZE) - memset((hmac->key) + keylen, 0, (size_t)(SHA1_BLOCKSIZE - keylen)); - } - - /* Create the initial vector */ - for(i = 0; i < SHA1_BLOCKSIZE; i++) - buf[i] = hmac->key[i] ^ 0x36; - - /* Prepend to the hash data */ - csp_sha1_init(&hmac->md); - csp_sha1_process(&hmac->md, buf, SHA1_BLOCKSIZE); - - return CSP_ERR_NONE; -} - -static int csp_hmac_process(hmac_state * hmac, const uint8_t * in, uint32_t inlen) { - - /* NULL pointer check */ - if (!hmac || !in) - return CSP_ERR_INVAL; - - /* Process data */ - csp_sha1_process(&hmac->md, in, inlen); - - return CSP_ERR_NONE; -} - -static int csp_hmac_done(hmac_state * hmac, uint8_t * out) { - - uint32_t i; - uint8_t buf[SHA1_BLOCKSIZE]; - uint8_t isha[SHA1_DIGESTSIZE]; - - if (!hmac || !out) - return CSP_ERR_INVAL; - - /* Get the hash of the first HMAC vector plus the data */ - csp_sha1_done(&hmac->md, isha); - - /* Create the second HMAC vector vector */ - for(i = 0; i < SHA1_BLOCKSIZE; i++) - buf[i] = hmac->key[i] ^ 0x5C; - - /* Now calculate the outer hash */ - csp_sha1_init(&hmac->md); - csp_sha1_process(&hmac->md, buf, SHA1_BLOCKSIZE); - csp_sha1_process(&hmac->md, isha, SHA1_DIGESTSIZE); - csp_sha1_done(&hmac->md, buf); - - /* Copy to output */ - for (i = 0; i < SHA1_DIGESTSIZE; i++) - out[i] = buf[i]; - - return CSP_ERR_NONE; -} - -int csp_hmac_memory(const uint8_t * key, uint32_t keylen, const uint8_t * data, uint32_t datalen, uint8_t * hmac) { - hmac_state state; - - /* NULL pointer check */ - if (!key || !data || !hmac) - return CSP_ERR_INVAL; - - /* Init HMAC state */ - if (csp_hmac_init(&state, key, keylen) != 0) - return CSP_ERR_INVAL; - - /* Process data */ - if (csp_hmac_process(&state, data, datalen) != 0) - return CSP_ERR_INVAL; - - /* Output HMAC */ - if (csp_hmac_done(&state, hmac) != 0) - return CSP_ERR_INVAL; - - return CSP_ERR_NONE; -} - -int csp_hmac_set_key(char * key, uint32_t keylen) { - - /* Use SHA1 as KDF */ - uint8_t hash[SHA1_DIGESTSIZE]; - csp_sha1_memory((uint8_t *)key, keylen, hash); - - /* Copy key */ - memcpy(csp_hmac_key, hash, HMAC_KEY_LENGTH); - - return CSP_ERR_NONE; - -} - -int csp_hmac_append(csp_packet_t * packet, bool include_header) { - - /* NULL pointer check */ - if (packet == NULL) - return CSP_ERR_INVAL; - - uint8_t hmac[SHA1_DIGESTSIZE]; - - /* Calculate HMAC */ - if (include_header) { - csp_hmac_memory(csp_hmac_key, HMAC_KEY_LENGTH, (uint8_t *) &packet->id, packet->length + sizeof(packet->id), hmac); - } else { - csp_hmac_memory(csp_hmac_key, HMAC_KEY_LENGTH, packet->data, packet->length, hmac); - } - - /* Truncate hash and copy to packet */ - memcpy(&packet->data[packet->length], hmac, CSP_HMAC_LENGTH); - packet->length += CSP_HMAC_LENGTH; - - return CSP_ERR_NONE; - -} - -int csp_hmac_verify(csp_packet_t * packet, bool include_header) { - - /* NULL pointer check */ - if (packet == NULL) - return CSP_ERR_INVAL; - - uint8_t hmac[SHA1_DIGESTSIZE]; - - /* Calculate HMAC */ - if (include_header) { - csp_hmac_memory(csp_hmac_key, HMAC_KEY_LENGTH, (uint8_t *) &packet->id, packet->length + sizeof(packet->id) - CSP_HMAC_LENGTH, hmac); - } else { - csp_hmac_memory(csp_hmac_key, HMAC_KEY_LENGTH, packet->data, packet->length - CSP_HMAC_LENGTH, hmac); - } - - /* Compare calculated HMAC with packet header */ - if (memcmp(&packet->data[packet->length] - CSP_HMAC_LENGTH, hmac, CSP_HMAC_LENGTH) != 0) { - /* HMAC failed */ - return CSP_ERR_HMAC; - } else { - /* Strip HMAC */ - packet->length -= CSP_HMAC_LENGTH; - return CSP_ERR_NONE; - } - -} - -#endif // CSP_USE_HMAC diff --git a/thirdparty/libcsp/src/crypto/csp_sha1.c b/thirdparty/libcsp/src/crypto/csp_sha1.c deleted file mode 100644 index 6c3920e9..00000000 --- a/thirdparty/libcsp/src/crypto/csp_sha1.c +++ /dev/null @@ -1,217 +0,0 @@ -/* -Cubesat Space Protocol - A small network-layer protocol designed for Cubesats -Copyright (C) 2012 GomSpace ApS (http://www.gomspace.com) -Copyright (C) 2012 AAUSAT3 Project (http://aausat3.space.aau.dk) - -This library is free software; you can redistribute it and/or -modify it under the terms of the GNU Lesser General Public -License as published by the Free Software Foundation; either -version 2.1 of the License, or (at your option) any later version. - -This library is distributed in the hope that it will be useful, -but WITHOUT ANY WARRANTY; without even the implied warranty of -MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU -Lesser General Public License for more details. - -You should have received a copy of the GNU Lesser General Public -License along with this library; if not, write to the Free Software -Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA -*/ - -/* Code originally from Python's SHA1 Module, who based it on libtom.org */ - -#include -#include - -/* CSP includes */ -#include - -#include - -#if defined(CSP_USE_HMAC) || defined(CSP_USE_XTEA) - -/* Rotate left macro */ -#define ROL(x,y) (((x) << (y)) | ((x) >> (32-y))) - -/* Endian Neutral macros that work on all platforms */ -#define STORE32H(x, y) do { (y)[0] = (uint8_t)(((x) >> 24) & 0xff); \ - (y)[1] = (uint8_t)(((x) >> 16) & 0xff); \ - (y)[2] = (uint8_t)(((x) >> 8) & 0xff); \ - (y)[3] = (uint8_t)(((x) >> 0) & 0xff); } while (0) - -#define LOAD32H(x, y) do { (x) = ((uint32_t)((y)[0] & 0xff) << 24) | \ - ((uint32_t)((y)[1] & 0xff) << 16) | \ - ((uint32_t)((y)[2] & 0xff) << 8) | \ - ((uint32_t)((y)[3] & 0xff) << 0); } while (0) - -#define STORE64H(x, y) do { (y)[0] = (uint8_t)(((x) >> 56) & 0xff); \ - (y)[1] = (uint8_t)(((x) >> 48) & 0xff); \ - (y)[2] = (uint8_t)(((x) >> 40) & 0xff); \ - (y)[3] = (uint8_t)(((x) >> 32) & 0xff); \ - (y)[4] = (uint8_t)(((x) >> 24) & 0xff); \ - (y)[5] = (uint8_t)(((x) >> 16) & 0xff); \ - (y)[6] = (uint8_t)(((x) >> 8) & 0xff); \ - (y)[7] = (uint8_t)(((x) >> 0) & 0xff); } while (0) - -#define MIN(x, y) (((x) < (y)) ? (x) : (y)) - -/* SHA1 macros */ -#define F0(x,y,z) (z ^ (x & (y ^ z))) -#define F1(x,y,z) (x ^ y ^ z) -#define F2(x,y,z) ((x & y) | (z & (x | y))) -#define F3(x,y,z) (x ^ y ^ z) - -#define FF_0(a, b, c, d, e, i) do {e = (ROL(a, 5) + F0(b,c,d) + e + W[i] + 0x5a827999UL); b = ROL(b, 30);} while (0) -#define FF_1(a, b, c, d, e, i) do {e = (ROL(a, 5) + F1(b,c,d) + e + W[i] + 0x6ed9eba1UL); b = ROL(b, 30);} while (0) -#define FF_2(a, b, c, d, e, i) do {e = (ROL(a, 5) + F2(b,c,d) + e + W[i] + 0x8f1bbcdcUL); b = ROL(b, 30);} while (0) -#define FF_3(a, b, c, d, e, i) do {e = (ROL(a, 5) + F3(b,c,d) + e + W[i] + 0xca62c1d6UL); b = ROL(b, 30);} while (0) - -static void csp_sha1_compress(csp_sha1_state * sha1, const uint8_t * buf) { - - uint32_t a, b, c, d, e, W[80], i; - - /* Copy the state into 512-bits into W[0..15] */ - for (i = 0; i < 16; i++) - LOAD32H(W[i], buf + (4*i)); - - /* Copy state */ - a = sha1->state[0]; - b = sha1->state[1]; - c = sha1->state[2]; - d = sha1->state[3]; - e = sha1->state[4]; - - /* Expand it */ - for (i = 16; i < 80; i++) - W[i] = ROL(W[i-3] ^ W[i-8] ^ W[i-14] ^ W[i-16], 1); - - /* Compress */ - i = 0; - - /* Round one */ - for (; i < 20;) { - FF_0(a, b, c, d, e, i++); - FF_0(e, a, b, c, d, i++); - FF_0(d, e, a, b, c, i++); - FF_0(c, d, e, a, b, i++); - FF_0(b, c, d, e, a, i++); - } - - /* Round two */ - for (; i < 40;) { - FF_1(a, b, c, d, e, i++); - FF_1(e, a, b, c, d, i++); - FF_1(d, e, a, b, c, i++); - FF_1(c, d, e, a, b, i++); - FF_1(b, c, d, e, a, i++); - } - - /* Round three */ - for (; i < 60;) { - FF_2(a, b, c, d, e, i++); - FF_2(e, a, b, c, d, i++); - FF_2(d, e, a, b, c, i++); - FF_2(c, d, e, a, b, i++); - FF_2(b, c, d, e, a, i++); - } - - /* Round four */ - for (; i < 80;) { - FF_3(a, b, c, d, e, i++); - FF_3(e, a, b, c, d, i++); - FF_3(d, e, a, b, c, i++); - FF_3(c, d, e, a, b, i++); - FF_3(b, c, d, e, a, i++); - } - - /* Store */ - sha1->state[0] += a; - sha1->state[1] += b; - sha1->state[2] += c; - sha1->state[3] += d; - sha1->state[4] += e; - -} - -void csp_sha1_init(csp_sha1_state * sha1) { - - sha1->state[0] = 0x67452301UL; - sha1->state[1] = 0xefcdab89UL; - sha1->state[2] = 0x98badcfeUL; - sha1->state[3] = 0x10325476UL; - sha1->state[4] = 0xc3d2e1f0UL; - sha1->curlen = 0; - sha1->length = 0; - -} - -void csp_sha1_process(csp_sha1_state * sha1, const uint8_t * in, uint32_t inlen) { - - uint32_t n; - while (inlen > 0) { - if (sha1->curlen == 0 && inlen >= SHA1_BLOCKSIZE) { - csp_sha1_compress(sha1, in); - sha1->length += SHA1_BLOCKSIZE * 8; - in += SHA1_BLOCKSIZE; - inlen -= SHA1_BLOCKSIZE; - } else { - n = MIN(inlen, (SHA1_BLOCKSIZE - sha1->curlen)); - memcpy(sha1->buf + sha1->curlen, in, (size_t)n); - sha1->curlen += n; - in += n; - inlen -= n; - if (sha1->curlen == SHA1_BLOCKSIZE) { - csp_sha1_compress(sha1, sha1->buf); - sha1->length += 8*SHA1_BLOCKSIZE; - sha1->curlen = 0; - } - } - } - -} - -void csp_sha1_done(csp_sha1_state * sha1, uint8_t * out) { - - uint32_t i; - - /* Increase the length of the message */ - sha1->length += sha1->curlen * 8; - - /* Append the '1' bit */ - sha1->buf[sha1->curlen++] = 0x80; - - /* If the length is currently above 56 bytes we append zeros - * then compress. Then we can fall back to padding zeros and length - * encoding like normal. - */ - if (sha1->curlen > 56) { - while (sha1->curlen < 64) - sha1->buf[sha1->curlen++] = 0; - csp_sha1_compress(sha1, sha1->buf); - sha1->curlen = 0; - } - - /* Pad up to 56 bytes of zeroes */ - while (sha1->curlen < 56) - sha1->buf[sha1->curlen++] = 0; - - /* Store length */ - STORE64H(sha1->length, sha1->buf + 56); - csp_sha1_compress(sha1, sha1->buf); - - /* Copy output */ - for (i = 0; i < 5; i++) - STORE32H(sha1->state[i], out + (4 * i)); - -} - -void csp_sha1_memory(const uint8_t * msg, uint32_t len, uint8_t * hash) { - - csp_sha1_state md; - csp_sha1_init(&md); - csp_sha1_process(&md, msg, len); - csp_sha1_done(&md, hash); - -} - -#endif // CSP_USE_HMAC diff --git a/thirdparty/libcsp/src/crypto/csp_xtea.c b/thirdparty/libcsp/src/crypto/csp_xtea.c deleted file mode 100644 index 718824d1..00000000 --- a/thirdparty/libcsp/src/crypto/csp_xtea.c +++ /dev/null @@ -1,134 +0,0 @@ -/* -Cubesat Space Protocol - A small network-layer protocol designed for Cubesats -Copyright (C) 2012 GomSpace ApS (http://www.gomspace.com) -Copyright (C) 2012 AAUSAT3 Project (http://aausat3.space.aau.dk) - -This library is free software; you can redistribute it and/or -modify it under the terms of the GNU Lesser General Public -License as published by the Free Software Foundation; either -version 2.1 of the License, or (at your option) any later version. - -This library is distributed in the hope that it will be useful, -but WITHOUT ANY WARRANTY; without even the implied warranty of -MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU -Lesser General Public License for more details. - -You should have received a copy of the GNU Lesser General Public -License along with this library; if not, write to the Free Software -Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA -*/ - -/* Simple implementation of XTEA in CTR mode */ - -#include -#include - -/* CSP includes */ -#include -#include -#include -#include - -#ifdef CSP_USE_XTEA - -#define XTEA_BLOCKSIZE 8 -#define XTEA_ROUNDS 32 -#define XTEA_KEY_LENGTH 16 - -/* XTEA key */ -static uint32_t csp_xtea_key[XTEA_KEY_LENGTH/sizeof(uint32_t)] __attribute__ ((aligned(sizeof(uint32_t)))); - -#define STORE32L(x, y) do { (y)[3] = (uint8_t)(((x) >> 24) & 0xff); \ - (y)[2] = (uint8_t)(((x) >> 16) & 0xff); \ - (y)[1] = (uint8_t)(((x) >> 8) & 0xff); \ - (y)[0] = (uint8_t)(((x) >> 0) & 0xff); } while (0) - -#define LOAD32L(x, y) do { (x) = ((uint32_t)((y)[3] & 0xff) << 24) | \ - ((uint32_t)((y)[2] & 0xff) << 16) | \ - ((uint32_t)((y)[1] & 0xff) << 8) | \ - ((uint32_t)((y)[0] & 0xff) << 0); } while (0) - -/* This function takes 64 bits of data in block and the 128 bits key in key */ -static inline void csp_xtea_encrypt_block(uint8_t *block, uint8_t const *key) { - - uint32_t i, v0, v1, delta = 0x9E3779B9, sum = 0, k[4]; - - LOAD32L(k[0], &key[0]); - LOAD32L(k[1], &key[4]); - LOAD32L(k[2], &key[8]); - LOAD32L(k[3], &key[12]); - - LOAD32L(v0, &block[0]); - LOAD32L(v1, &block[4]); - - for (i = 0; i < XTEA_ROUNDS; i++) { - v0 += (((v1 << 4) ^ (v1 >> 5)) + v1) ^ (sum + k[sum & 3]); - sum += delta; - v1 += (((v0 << 4) ^ (v0 >> 5)) + v0) ^ (sum + k[(sum >> 11) & 3]); - } - - STORE32L(v0, &block[0]); - STORE32L(v1, &block[4]); - -} - -static inline void csp_xtea_xor_byte(uint8_t * dst, uint8_t * src, uint32_t len) { - - unsigned int i; - for (i = 0; i < len; i++) - dst[i] ^= src[i]; - -} - -int csp_xtea_set_key(char * key, uint32_t keylen) { - - /* Use SHA1 as KDF */ - uint8_t hash[SHA1_DIGESTSIZE]; - csp_sha1_memory((uint8_t *)key, keylen, hash); - - /* Copy key */ - memcpy(csp_xtea_key, hash, XTEA_KEY_LENGTH); - - return CSP_ERR_NONE; - -} - -int csp_xtea_encrypt(uint8_t * plain, const uint32_t len, uint32_t iv[2]) { - - unsigned int i; - uint32_t stream[2]; - - uint32_t blocks = (len + XTEA_BLOCKSIZE - 1)/ XTEA_BLOCKSIZE; - uint32_t remain; - - /* Initialize stream */ - stream[0] = csp_htobe32(iv[0]); - stream[1] = csp_htobe32(iv[1]); - - for (i = 0; i < blocks; i++) { - /* Create stream */ - csp_xtea_encrypt_block((uint8_t *)stream, (uint8_t *)csp_xtea_key); - - /* Calculate remaining bytes */ - remain = len - i * XTEA_BLOCKSIZE; - - /* XOR plain text with stream to generate cipher text */ - csp_xtea_xor_byte(&plain[len - remain], (uint8_t *)stream, remain < XTEA_BLOCKSIZE ? remain : XTEA_BLOCKSIZE); - - /* Increment counter */ - stream[0] = csp_htobe32(iv[0]); - stream[1] = csp_htobe32(iv[1]++); - } - - return CSP_ERR_NONE; - -} - -int csp_xtea_decrypt(uint8_t * cipher, const uint32_t len, uint32_t iv[2]) { - - /* Since we use counter mode, we can reuse the encryption function */ - return csp_xtea_encrypt(cipher, len, iv); - -} - -#endif // CSP_USE_XTEA diff --git a/thirdparty/libcsp/src/csp_bridge.c b/thirdparty/libcsp/src/csp_bridge.c deleted file mode 100644 index 1c579a9f..00000000 --- a/thirdparty/libcsp/src/csp_bridge.c +++ /dev/null @@ -1,94 +0,0 @@ -/* -Cubesat Space Protocol - A small network-layer protocol designed for Cubesats -Copyright (C) 2012 GomSpace ApS (http://www.gomspace.com) -Copyright (C) 2012 AAUSAT3 Project (http://aausat3.space.aau.dk) - -This library is free software; you can redistribute it and/or -modify it under the terms of the GNU Lesser General Public -License as published by the Free Software Foundation; either -version 2.1 of the License, or (at your option) any later version. - -This library is distributed in the hope that it will be useful, -but WITHOUT ANY WARRANTY; without even the implied warranty of -MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU -Lesser General Public License for more details. - -You should have received a copy of the GNU Lesser General Public -License along with this library; if not, write to the Free Software -Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA -*/ - -#include -#include -#include -#include "csp_route.h" -#include "csp_qfifo.h" -#include "csp_io.h" -#include "csp_promisc.h" - -static csp_iface_t* if_a = NULL; -static csp_iface_t* if_b = NULL; - -static CSP_DEFINE_TASK(csp_bridge) { - - csp_qfifo_t input; - csp_packet_t * packet; - - /* Here there be bridging */ - while (1) { - - /* Get next packet to route */ - if (csp_qfifo_read(&input) != CSP_ERR_NONE) - continue; - - packet = input.packet; - - csp_log_packet("Input: Src %u, Dst %u, Dport %u, Sport %u, Pri %u, Flags 0x%02X, Size %"PRIu16, - packet->id.src, packet->id.dst, packet->id.dport, - packet->id.sport, packet->id.pri, packet->id.flags, packet->length); - - /* Here there be promiscuous mode */ -#ifdef CSP_USE_PROMISC - csp_promisc_add(packet); -#endif - - /* Find the opposing interface */ - csp_iface_t * ifout; - if (input.interface == if_a) { - ifout = if_b; - } else { - ifout = if_a; - } - - /* Send to the interface directly, no hassle */ - if (csp_send_direct(packet->id, packet, ifout, 0) != CSP_ERR_NONE) { - csp_log_warn("Router failed to send"); - csp_buffer_free(packet); - } - - /* Next message, please */ - continue; - - } - - return CSP_TASK_RETURN; - -} - -int csp_bridge_start(unsigned int task_stack_size, unsigned int task_priority, csp_iface_t * _if_a, csp_iface_t * _if_b) { - - /* Set static references to A/B side of bridge */ - if_a = _if_a; - if_b = _if_b; - - static csp_thread_handle_t handle; - int ret = csp_thread_create(csp_bridge, "BRIDGE", task_stack_size, NULL, task_priority, &handle); - - if (ret != 0) { - csp_log_error("Failed to start task"); - return CSP_ERR_NOMEM; - } - - return CSP_ERR_NONE; - -} diff --git a/thirdparty/libcsp/src/csp_buffer.c b/thirdparty/libcsp/src/csp_buffer.c deleted file mode 100644 index 8947f337..00000000 --- a/thirdparty/libcsp/src/csp_buffer.c +++ /dev/null @@ -1,224 +0,0 @@ -/* -Cubesat Space Protocol - A small network-layer protocol designed for Cubesats -Copyright (C) 2012 GomSpace ApS (http://www.gomspace.com) -Copyright (C) 2012 AAUSAT3 Project (http://aausat3.space.aau.dk) - -This library is free software; you can redistribute it and/or -modify it under the terms of the GNU Lesser General Public -License as published by the Free Software Foundation; either -version 2.1 of the License, or (at your option) any later version. - -This library is distributed in the hope that it will be useful, -but WITHOUT ANY WARRANTY; without even the implied warranty of -MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU -Lesser General Public License for more details. - -You should have received a copy of the GNU Lesser General Public -License along with this library; if not, write to the Free Software -Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA -*/ - -#include -#include -#include - -/* CSP includes */ -#include -#include -#include -#include -#include - -#ifndef CSP_BUFFER_ALIGN -#define CSP_BUFFER_ALIGN (sizeof(int *)) -#endif - -typedef struct csp_skbf_s { - unsigned int refcount; - void * skbf_addr; - char skbf_data[]; -} csp_skbf_t; - -static csp_queue_handle_t csp_buffers; -static char * csp_buffer_pool; -static unsigned int count, size; - -CSP_DEFINE_CRITICAL(csp_critical_lock); - -int csp_buffer_init(int buf_count, int buf_size) { - - unsigned int i; - csp_skbf_t * buf; - - count = buf_count; - size = buf_size + CSP_BUFFER_PACKET_OVERHEAD; - unsigned int skbfsize = (sizeof(csp_skbf_t) + size); - skbfsize = CSP_BUFFER_ALIGN * ((skbfsize + CSP_BUFFER_ALIGN - 1) / CSP_BUFFER_ALIGN); - unsigned int poolsize = count * skbfsize; - - csp_buffer_pool = csp_malloc(poolsize); - if (csp_buffer_pool == NULL) - goto fail_malloc; - - csp_buffers = csp_queue_create(count, sizeof(void *)); - if (!csp_buffers) - goto fail_queue; - - if (CSP_INIT_CRITICAL(csp_critical_lock) != CSP_ERR_NONE) - goto fail_critical; - - memset(csp_buffer_pool, 0, poolsize); - - for (i = 0; i < count; i++) { - - /* We have already taken care of pointer alignment since - * skbfsize is an integer multiple of sizeof(int *) - * but the explicit cast to a void * is still necessary - * to tell the compiler so. - */ - buf = (void *) &csp_buffer_pool[i * skbfsize]; - buf->refcount = 0; - buf->skbf_addr = buf; - - csp_queue_enqueue(csp_buffers, &buf, 0); - - } - - return CSP_ERR_NONE; - -fail_critical: - csp_queue_remove(csp_buffers); -fail_queue: - csp_free(csp_buffer_pool); -fail_malloc: - return CSP_ERR_NOMEM; - -} - -void *csp_buffer_get_isr(size_t buf_size) { - - csp_skbf_t * buffer = NULL; - CSP_BASE_TYPE task_woken = 0; - - if (buf_size + CSP_BUFFER_PACKET_OVERHEAD > size) - return NULL; - - csp_queue_dequeue_isr(csp_buffers, &buffer, &task_woken); - if (buffer == NULL) - return NULL; - - if (buffer != buffer->skbf_addr) - return NULL; - - buffer->refcount++; - return buffer->skbf_data; - -} - -void *csp_buffer_get(size_t buf_size) { - - csp_skbf_t * buffer = NULL; - - if (buf_size + CSP_BUFFER_PACKET_OVERHEAD > size) { - csp_log_error("Attempt to allocate too large block %u", buf_size); - return NULL; - } - - csp_queue_dequeue(csp_buffers, &buffer, 0); - if (buffer == NULL) { - csp_log_error("Out of buffers"); - return NULL; - } - - csp_log_buffer("GET: %p %p", buffer, buffer->skbf_addr); - - if (buffer != buffer->skbf_addr) { - csp_log_error("Corrupt CSP buffer"); - return NULL; - } - - buffer->refcount++; - return buffer->skbf_data; -} - -void csp_buffer_free_isr(void *packet) { - CSP_BASE_TYPE task_woken = 0; - if (!packet) - return; - - csp_skbf_t * buf = packet - sizeof(csp_skbf_t); - - if (((uintptr_t) buf % CSP_BUFFER_ALIGN) > 0) - return; - - if (buf->skbf_addr != buf) - return; - - if (buf->refcount == 0) { - return; - } else if (buf->refcount > 1) { - buf->refcount--; - return; - } else { - buf->refcount = 0; - csp_queue_enqueue_isr(csp_buffers, &buf, &task_woken); - } - -} - -void csp_buffer_free(void *packet) { - if (!packet) { - csp_log_error("Attempt to free null pointer"); - return; - } - - csp_skbf_t * buf = packet - sizeof(csp_skbf_t); - - if (((uintptr_t) buf % CSP_BUFFER_ALIGN) > 0) { - csp_log_error("FREE: Unaligned CSP buffer pointer %p", packet); - return; - } - - if (buf->skbf_addr != buf) { - csp_log_error("FREE: Invalid CSP buffer pointer %p", packet); - return; - } - - if (buf->refcount == 0) { - csp_log_error("FREE: Buffer already free %p", buf); - return; - } else if (buf->refcount > 1) { - buf->refcount--; - csp_log_error("FREE: Buffer %p in use by %u users", buf, buf->refcount); - return; - } else { - buf->refcount = 0; - csp_log_buffer("FREE: %p", buf); - csp_queue_enqueue(csp_buffers, &buf, 0); - } - -} - -void *csp_buffer_clone(void *buffer) { - - csp_packet_t *packet = (csp_packet_t *) buffer; - - if (!packet) - return NULL; - - csp_packet_t *clone = csp_buffer_get(packet->length); - - if (clone) - memcpy(clone, packet, size); - - return clone; - -} - -int csp_buffer_remaining(void) { - return csp_queue_size(csp_buffers); -} - -int csp_buffer_size(void) { - return size; -} diff --git a/thirdparty/libcsp/src/csp_conn.c b/thirdparty/libcsp/src/csp_conn.c deleted file mode 100644 index 7daa569d..00000000 --- a/thirdparty/libcsp/src/csp_conn.c +++ /dev/null @@ -1,498 +0,0 @@ -/* -Cubesat Space Protocol - A small network-layer protocol designed for Cubesats -Copyright (C) 2012 GomSpace ApS (http://www.gomspace.com) -Copyright (C) 2012 AAUSAT3 Project (http://aausat3.space.aau.dk) - -This library is free software; you can redistribute it and/or -modify it under the terms of the GNU Lesser General Public -License as published by the Free Software Foundation; either -version 2.1 of the License, or (at your option) any later version. - -This library is distributed in the hope that it will be useful, -but WITHOUT ANY WARRANTY; without even the implied warranty of -MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU -Lesser General Public License for more details. - -You should have received a copy of the GNU Lesser General Public -License along with this library; if not, write to the Free Software -Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA -*/ - -#include -#include -#include -#include - -/* CSP includes */ -#include -#include - -#include -#include -#include -#include -#include - -#include "csp_conn.h" -#include "transport/csp_transport.h" - -/* Static connection pool */ -static csp_conn_t arr_conn[CSP_CONN_MAX]; - -/* Connection pool lock */ -static csp_bin_sem_handle_t conn_lock; - -/* Source port */ -static uint8_t sport; - -/* Source port lock */ -static csp_bin_sem_handle_t sport_lock; - -void csp_conn_check_timeouts(void) { -#ifdef CSP_USE_RDP - int i; - for (i = 0; i < CSP_CONN_MAX; i++) - if (arr_conn[i].state == CONN_OPEN) - if (arr_conn[i].idin.flags & CSP_FRDP) - csp_rdp_check_timeouts(&arr_conn[i]); -#endif -} - -int csp_conn_get_rxq(int prio) { - -#ifdef CSP_USE_QOS - return prio; -#else - return 0; -#endif - -} - -int csp_conn_lock(csp_conn_t * conn, uint32_t timeout) { - - if (csp_mutex_lock(&conn->lock, timeout) != CSP_MUTEX_OK) - return CSP_ERR_TIMEDOUT; - - return CSP_ERR_NONE; - -} - -int csp_conn_unlock(csp_conn_t * conn) { - - csp_mutex_unlock(&conn->lock); - - return CSP_ERR_NONE; - -} - -int csp_conn_enqueue_packet(csp_conn_t * conn, csp_packet_t * packet) { - - if (!conn) - return CSP_ERR_INVAL; - - int rxq; - if (packet != NULL) { - rxq = csp_conn_get_rxq(packet->id.pri); - } else { - rxq = CSP_RX_QUEUES - 1; - } - - if (csp_queue_enqueue(conn->rx_queue[rxq], &packet, 0) != CSP_QUEUE_OK) { - csp_log_error("RX queue %p full with %u items", conn->rx_queue[rxq], csp_queue_size(conn->rx_queue[rxq])); - return CSP_ERR_NOMEM; - } - -#ifdef CSP_USE_QOS - int event = 0; - if (csp_queue_enqueue(conn->rx_event, &event, 0) != CSP_QUEUE_OK) { - csp_log_error("QOS event queue full"); - return CSP_ERR_NOMEM; - } -#endif - - return CSP_ERR_NONE; -} - -int csp_conn_init(void) { - - /* Initialize source port */ - srand(csp_get_ms()); - sport = (rand() % (CSP_ID_PORT_MAX - CSP_MAX_BIND_PORT)) + (CSP_MAX_BIND_PORT + 1); - - if (csp_bin_sem_create(&sport_lock) != CSP_SEMAPHORE_OK) { - csp_log_error("No more memory for sport semaphore"); - return CSP_ERR_NOMEM; - } - - int i, prio; - for (i = 0; i < CSP_CONN_MAX; i++) { - for (prio = 0; prio < CSP_RX_QUEUES; prio++) - arr_conn[i].rx_queue[prio] = csp_queue_create(CSP_RX_QUEUE_LENGTH, sizeof(csp_packet_t *)); - -#ifdef CSP_USE_QOS - arr_conn[i].rx_event = csp_queue_create(CSP_CONN_QUEUE_LENGTH, sizeof(int)); -#endif - arr_conn[i].state = CONN_CLOSED; - - if (csp_mutex_create(&arr_conn[i].lock) != CSP_MUTEX_OK) { - csp_log_error("Failed to create connection lock"); - return CSP_ERR_NOMEM; - } - -#ifdef CSP_USE_RDP - if (csp_rdp_allocate(&arr_conn[i]) != CSP_ERR_NONE) { - csp_log_error("Failed to create queues for RDP in csp_conn_init"); - return CSP_ERR_NOMEM; - } -#endif - } - - if (csp_bin_sem_create(&conn_lock) != CSP_SEMAPHORE_OK) { - csp_log_error("No more memory for conn semaphore"); - return CSP_ERR_NOMEM; - } - - return CSP_ERR_NONE; - -} - -csp_conn_t * csp_conn_find(uint32_t id, uint32_t mask) { - - /* Search for matching connection */ - int i; - csp_conn_t * conn; - id = (id & mask); - for (i = 0; i < CSP_CONN_MAX; i++) { - conn = &arr_conn[i]; - if ((conn->state != CONN_CLOSED) && (conn->type == CONN_CLIENT) && ((conn->idin.ext & mask) == id)) - return conn; - } - - return NULL; - -} - -static int csp_conn_flush_rx_queue(csp_conn_t * conn) { - - csp_packet_t * packet; - - int prio; - - /* Flush packet queues */ - for (prio = 0; prio < CSP_RX_QUEUES; prio++) { - while (csp_queue_dequeue(conn->rx_queue[prio], &packet, 0) == CSP_QUEUE_OK) - if (packet != NULL) - csp_buffer_free(packet); - } - - /* Flush event queue */ -#ifdef CSP_USE_QOS - int event; - while (csp_queue_dequeue(conn->rx_event, &event, 0) == CSP_QUEUE_OK); -#endif - - return CSP_ERR_NONE; - -} - -csp_conn_t * csp_conn_allocate(csp_conn_type_t type) { - - int i, j; - static uint8_t csp_conn_last_given = 0; - csp_conn_t * conn; - - if (csp_bin_sem_wait(&conn_lock, 100) != CSP_SEMAPHORE_OK) { - csp_log_error("Failed to lock conn array"); - return NULL; - } - - /* Search for free connection */ - i = csp_conn_last_given; - i = (i + 1) % CSP_CONN_MAX; - - for (j = 0; j < CSP_CONN_MAX; j++) { - conn = &arr_conn[i]; - if (conn->state == CONN_CLOSED) - break; - i = (i + 1) % CSP_CONN_MAX; - } - - if (conn->state == CONN_OPEN) { - csp_log_error("No more free connections"); - csp_bin_sem_post(&conn_lock); - return NULL; - } - - conn->idin.ext = 0; - conn->idout.ext = 0; - conn->socket = NULL; - conn->timestamp = 0; - conn->type = type; - conn->state = CONN_OPEN; - - csp_conn_last_given = i; - csp_bin_sem_post(&conn_lock); - - return conn; - -} - -csp_conn_t * csp_conn_new(csp_id_t idin, csp_id_t idout) { - - /* Allocate connection structure */ - csp_conn_t * conn = csp_conn_allocate(CONN_CLIENT); - - if (conn) { - /* No lock is needed here, because nobody else * - * has a reference to this connection yet. */ - conn->idin.ext = idin.ext; - conn->idout.ext = idout.ext; - conn->timestamp = csp_get_ms(); - - /* Ensure connection queue is empty */ - csp_conn_flush_rx_queue(conn); - } - - return conn; - -} - -int csp_close(csp_conn_t * conn) { - - if (conn == NULL) { - csp_log_error("NULL Pointer given to csp_close"); - return CSP_ERR_INVAL; - } - - if (conn->state == CONN_CLOSED) { - csp_log_protocol("Conn already closed"); - return CSP_ERR_NONE; - } - -#ifdef CSP_USE_RDP - /* Ensure RDP knows this connection is closing */ - if (conn->idin.flags & CSP_FRDP || conn->idout.flags & CSP_FRDP) - if (csp_rdp_close(conn) == CSP_ERR_AGAIN) - return CSP_ERR_NONE; -#endif - - /* Lock connection array while closing connection */ - if (csp_bin_sem_wait(&conn_lock, 100) != CSP_SEMAPHORE_OK) { - csp_log_error("Failed to lock conn array"); - return CSP_ERR_TIMEDOUT; - } - - /* Set to closed */ - conn->state = CONN_CLOSED; - - /* Ensure connection queue is empty */ - csp_conn_flush_rx_queue(conn); - - if (conn->socket && (conn->type == CONN_SERVER) && (conn->opts & (CSP_SO_CONN_LESS | CSP_SO_INTERNAL_LISTEN))) { - csp_queue_remove(conn->socket); - conn->socket = NULL; - } - - /* Reset RDP state */ -#ifdef CSP_USE_RDP - if (conn->idin.flags & CSP_FRDP) - csp_rdp_flush_all(conn); -#endif - - /* Unlock connection array */ - csp_bin_sem_post(&conn_lock); - - return CSP_ERR_NONE; -} - -csp_conn_t * csp_connect(uint8_t prio, uint8_t dest, uint8_t dport, uint32_t timeout, uint32_t opts) { - - /* Force options on all connections */ - opts |= CSP_CONNECTION_SO; - - /* Generate identifier */ - csp_id_t incoming_id, outgoing_id; - incoming_id.pri = prio; - incoming_id.dst = csp_get_address(); - incoming_id.src = dest; - incoming_id.sport = dport; - incoming_id.flags = 0; - outgoing_id.pri = prio; - outgoing_id.dst = dest; - outgoing_id.src = csp_get_address(); - outgoing_id.dport = dport; - outgoing_id.flags = 0; - - /* Set connection options */ - if (opts & CSP_O_NOCRC32) { - opts &= ~CSP_O_CRC32; - } - - if (opts & CSP_O_RDP) { -#ifdef CSP_USE_RDP - incoming_id.flags |= CSP_FRDP; - outgoing_id.flags |= CSP_FRDP; -#else - csp_log_error("Attempt to create RDP connection, but CSP was compiled without RDP support"); - return NULL; -#endif - } - - if (opts & CSP_O_HMAC) { -#ifdef CSP_USE_HMAC - outgoing_id.flags |= CSP_FHMAC; - incoming_id.flags |= CSP_FHMAC; -#else - csp_log_error("Attempt to create HMAC authenticated connection, but CSP was compiled without HMAC support"); - return NULL; -#endif - } - - if (opts & CSP_O_XTEA) { -#ifdef CSP_USE_XTEA - outgoing_id.flags |= CSP_FXTEA; - incoming_id.flags |= CSP_FXTEA; -#else - csp_log_error("Attempt to create XTEA encrypted connection, but CSP was compiled without XTEA support"); - return NULL; -#endif - } - - if (opts & CSP_O_CRC32) { -#ifdef CSP_USE_CRC32 - outgoing_id.flags |= CSP_FCRC32; - incoming_id.flags |= CSP_FCRC32; -#else - csp_log_error("Attempt to create CRC32 validated connection, but CSP was compiled without CRC32 support"); - return NULL; -#endif - } - - /* Find an unused ephemeral port */ - csp_conn_t * conn = NULL; - - /* Wait for sport lock - note that csp_conn_new(..) is called inside the lock! */ - if (csp_bin_sem_wait(&sport_lock, 1000) != CSP_SEMAPHORE_OK) - return NULL; - - const uint8_t start = sport; - while (++sport != start) { - if (sport > CSP_ID_PORT_MAX) - sport = CSP_MAX_BIND_PORT + 1; - - outgoing_id.sport = sport; - incoming_id.dport = sport; - - /* Match on destination port of _incoming_ identifier */ - if (csp_conn_find(incoming_id.ext, CSP_ID_DPORT_MASK) == NULL) { - /* Break - we found an unused ephemeral port - allocate connection while locked to mark port in use */ - conn = csp_conn_new(incoming_id, outgoing_id); - break; - } - } - - /* Post sport lock */ - csp_bin_sem_post(&sport_lock); - - if (conn == NULL) - return NULL; - - /* Set connection options */ - conn->opts = opts; - -#ifdef CSP_USE_RDP - /* Call Transport Layer connect */ - if (outgoing_id.flags & CSP_FRDP) { - /* If the transport layer has failed to connect - * deallocate connection structure again and return NULL */ - if (csp_rdp_connect(conn, timeout) != CSP_ERR_NONE) { - csp_close(conn); - return NULL; - } - } -#endif - - /* We have a successful connection */ - return conn; - -} - -inline int csp_conn_dport(csp_conn_t * conn) { - - return conn->idin.dport; - -} - -inline int csp_conn_sport(csp_conn_t * conn) { - - return conn->idin.sport; - -} - -inline int csp_conn_dst(csp_conn_t * conn) { - - return conn->idin.dst; - -} - -inline int csp_conn_src(csp_conn_t * conn) { - - return conn->idin.src; - -} - -inline int csp_conn_flags(csp_conn_t * conn) { - - return conn->idin.flags; - -} - -#ifdef CSP_DEBUG -void csp_conn_print_table(void) { - - int i; - csp_conn_t * conn; - - for (i = 0; i < CSP_CONN_MAX; i++) { - conn = &arr_conn[i]; - printf("[%02u %p] S:%u, %u -> %u, %u -> %u, sock: %p\r\n", - i, conn, conn->state, conn->idin.src, conn->idin.dst, - conn->idin.dport, conn->idin.sport, conn->socket); -#ifdef CSP_USE_RDP - if (conn->idin.flags & CSP_FRDP) - csp_rdp_conn_print(conn); -#endif - } -} - -int csp_conn_print_table_str(char * str_buf, int str_size) { - - int i, start = 0; - csp_conn_t * conn; - char buf[100]; - - /* Display up to 10 connections */ - if (CSP_CONN_MAX - 10 > 0) - start = CSP_CONN_MAX - 10; - - for (i = start; i < CSP_CONN_MAX; i++) { - conn = &arr_conn[i]; - snprintf(buf, sizeof(buf), "[%02u %p] S:%u, %u -> %u, %u -> %u, sock: %p\n", - i, conn, conn->state, conn->idin.src, conn->idin.dst, - conn->idin.dport, conn->idin.sport, conn->socket); - - strncat(str_buf, buf, str_size); - if ((str_size -= strlen(buf)) <= 0) - break; - } - - return CSP_ERR_NONE; -} -#endif - -const csp_conn_t * csp_conn_get_array(size_t * size) -{ - *size = CSP_CONN_MAX; - return arr_conn; -} diff --git a/thirdparty/libcsp/src/csp_conn.h b/thirdparty/libcsp/src/csp_conn.h deleted file mode 100644 index 3fa0ff52..00000000 --- a/thirdparty/libcsp/src/csp_conn.h +++ /dev/null @@ -1,112 +0,0 @@ -/* -Cubesat Space Protocol - A small network-layer protocol designed for Cubesats -Copyright (C) 2012 GomSpace ApS (http://www.gomspace.com) -Copyright (C) 2012 AAUSAT3 Project (http://aausat3.space.aau.dk) - -This library is free software; you can redistribute it and/or -modify it under the terms of the GNU Lesser General Public -License as published by the Free Software Foundation; either -version 2.1 of the License, or (at your option) any later version. - -This library is distributed in the hope that it will be useful, -but WITHOUT ANY WARRANTY; without even the implied warranty of -MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU -Lesser General Public License for more details. - -You should have received a copy of the GNU Lesser General Public -License along with this library; if not, write to the Free Software -Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA -*/ - -#ifndef _CSP_CONN_H_ -#define _CSP_CONN_H_ - -#ifdef __cplusplus -extern "C" { -#endif - -#include - -#include - -#include -#include - -/** @brief Connection states */ -typedef enum { - CONN_CLOSED = 0, - CONN_OPEN = 1, -} csp_conn_state_t; - -/** @brief Connection types */ -typedef enum { - CONN_CLIENT = 0, - CONN_SERVER = 1, -} csp_conn_type_t; - -typedef enum { - RDP_CLOSED = 0, - RDP_SYN_SENT, - RDP_SYN_RCVD, - RDP_OPEN, - RDP_CLOSE_WAIT, -} csp_rdp_state_t; - -/** @brief RDP Connection header - * @note Do not try to pack this struct, the posix sem handle will stop working */ -typedef struct { - csp_rdp_state_t state; /**< Connection state */ - uint16_t snd_nxt; /**< The sequence number of the next segment that is to be sent */ - uint16_t snd_una; /**< The sequence number of the oldest unacknowledged segment */ - uint16_t snd_iss; /**< The initial send sequence number */ - uint16_t rcv_cur; /**< The sequence number of the last segment received correctly and in sequence */ - uint16_t rcv_irs; /**< The initial receive sequence number */ - uint16_t rcv_lsa; /**< The last sequence number acknowledged by the receiver */ - uint32_t window_size; - uint32_t conn_timeout; - uint32_t packet_timeout; - uint32_t delayed_acks; - uint32_t ack_timeout; - uint32_t ack_delay_count; - uint32_t ack_timestamp; - csp_bin_sem_handle_t tx_wait; - csp_queue_handle_t tx_queue; - csp_queue_handle_t rx_queue; -} csp_rdp_t; - -/** @brief Connection struct */ -struct csp_conn_s { - csp_conn_type_t type; /* Connection type (CONN_CLIENT or CONN_SERVER) */ - csp_conn_state_t state; /* Connection state (CONN_OPEN or CONN_CLOSED) */ - csp_mutex_t lock; /* Connection structure lock */ - csp_id_t idin; /* Identifier received */ - csp_id_t idout; /* Identifier transmitted */ -#ifdef CSP_USE_QOS - csp_queue_handle_t rx_event; /* Event queue for RX packets */ -#endif - csp_queue_handle_t rx_queue[CSP_RX_QUEUES]; /* Queue for RX packets */ - csp_queue_handle_t socket; /* Socket to be "woken" when first packet is ready */ - uint32_t timestamp; /* Time the connection was opened */ - uint32_t opts; /* Connection or socket options */ -#ifdef CSP_USE_RDP - csp_rdp_t rdp; /* RDP state */ -#endif -}; - -int csp_conn_lock(csp_conn_t * conn, uint32_t timeout); -int csp_conn_unlock(csp_conn_t * conn); -int csp_conn_enqueue_packet(csp_conn_t * conn, csp_packet_t * packet); -int csp_conn_init(void); -csp_conn_t * csp_conn_allocate(csp_conn_type_t type); -csp_conn_t * csp_conn_find(uint32_t id, uint32_t mask); -csp_conn_t * csp_conn_new(csp_id_t idin, csp_id_t idout); -void csp_conn_check_timeouts(void); -int csp_conn_get_rxq(int prio); - -const csp_conn_t * csp_conn_get_array(size_t * size); // for test purposes only! - -#ifdef __cplusplus -} /* extern "C" */ -#endif - -#endif // _CSP_CONN_H_ diff --git a/thirdparty/libcsp/src/csp_crc32.c b/thirdparty/libcsp/src/csp_crc32.c deleted file mode 100644 index 8bf2145f..00000000 --- a/thirdparty/libcsp/src/csp_crc32.c +++ /dev/null @@ -1,140 +0,0 @@ -/* -Cubesat Space Protocol - A small network-layer protocol designed for Cubesats -Copyright (C) 2012 GomSpace ApS (http://www.gomspace.com) -Copyright (C) 2012 AAUSAT3 Project (http://aausat3.space.aau.dk) - -This library is free software; you can redistribute it and/or -modify it under the terms of the GNU Lesser General Public -License as published by the Free Software Foundation; either -version 2.1 of the License, or (at your option) any later version. - -This library is distributed in the hope that it will be useful, -but WITHOUT ANY WARRANTY; without even the implied warranty of -MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU -Lesser General Public License for more details. - -You should have received a copy of the GNU Lesser General Public -License along with this library; if not, write to the Free Software -Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA -*/ - -#include -#include -#include - -#include -#include - -#include - -#ifdef CSP_USE_CRC32 - -#ifdef __AVR__ -#include -static const uint32_t crc_tab[256] PROGMEM = { -#else -static const uint32_t crc_tab[256] = { -#endif - 0x00000000, 0xF26B8303, 0xE13B70F7, 0x1350F3F4, 0xC79A971F, 0x35F1141C, 0x26A1E7E8, 0xD4CA64EB, - 0x8AD958CF, 0x78B2DBCC, 0x6BE22838, 0x9989AB3B, 0x4D43CFD0, 0xBF284CD3, 0xAC78BF27, 0x5E133C24, - 0x105EC76F, 0xE235446C, 0xF165B798, 0x030E349B, 0xD7C45070, 0x25AFD373, 0x36FF2087, 0xC494A384, - 0x9A879FA0, 0x68EC1CA3, 0x7BBCEF57, 0x89D76C54, 0x5D1D08BF, 0xAF768BBC, 0xBC267848, 0x4E4DFB4B, - 0x20BD8EDE, 0xD2D60DDD, 0xC186FE29, 0x33ED7D2A, 0xE72719C1, 0x154C9AC2, 0x061C6936, 0xF477EA35, - 0xAA64D611, 0x580F5512, 0x4B5FA6E6, 0xB93425E5, 0x6DFE410E, 0x9F95C20D, 0x8CC531F9, 0x7EAEB2FA, - 0x30E349B1, 0xC288CAB2, 0xD1D83946, 0x23B3BA45, 0xF779DEAE, 0x05125DAD, 0x1642AE59, 0xE4292D5A, - 0xBA3A117E, 0x4851927D, 0x5B016189, 0xA96AE28A, 0x7DA08661, 0x8FCB0562, 0x9C9BF696, 0x6EF07595, - 0x417B1DBC, 0xB3109EBF, 0xA0406D4B, 0x522BEE48, 0x86E18AA3, 0x748A09A0, 0x67DAFA54, 0x95B17957, - 0xCBA24573, 0x39C9C670, 0x2A993584, 0xD8F2B687, 0x0C38D26C, 0xFE53516F, 0xED03A29B, 0x1F682198, - 0x5125DAD3, 0xA34E59D0, 0xB01EAA24, 0x42752927, 0x96BF4DCC, 0x64D4CECF, 0x77843D3B, 0x85EFBE38, - 0xDBFC821C, 0x2997011F, 0x3AC7F2EB, 0xC8AC71E8, 0x1C661503, 0xEE0D9600, 0xFD5D65F4, 0x0F36E6F7, - 0x61C69362, 0x93AD1061, 0x80FDE395, 0x72966096, 0xA65C047D, 0x5437877E, 0x4767748A, 0xB50CF789, - 0xEB1FCBAD, 0x197448AE, 0x0A24BB5A, 0xF84F3859, 0x2C855CB2, 0xDEEEDFB1, 0xCDBE2C45, 0x3FD5AF46, - 0x7198540D, 0x83F3D70E, 0x90A324FA, 0x62C8A7F9, 0xB602C312, 0x44694011, 0x5739B3E5, 0xA55230E6, - 0xFB410CC2, 0x092A8FC1, 0x1A7A7C35, 0xE811FF36, 0x3CDB9BDD, 0xCEB018DE, 0xDDE0EB2A, 0x2F8B6829, - 0x82F63B78, 0x709DB87B, 0x63CD4B8F, 0x91A6C88C, 0x456CAC67, 0xB7072F64, 0xA457DC90, 0x563C5F93, - 0x082F63B7, 0xFA44E0B4, 0xE9141340, 0x1B7F9043, 0xCFB5F4A8, 0x3DDE77AB, 0x2E8E845F, 0xDCE5075C, - 0x92A8FC17, 0x60C37F14, 0x73938CE0, 0x81F80FE3, 0x55326B08, 0xA759E80B, 0xB4091BFF, 0x466298FC, - 0x1871A4D8, 0xEA1A27DB, 0xF94AD42F, 0x0B21572C, 0xDFEB33C7, 0x2D80B0C4, 0x3ED04330, 0xCCBBC033, - 0xA24BB5A6, 0x502036A5, 0x4370C551, 0xB11B4652, 0x65D122B9, 0x97BAA1BA, 0x84EA524E, 0x7681D14D, - 0x2892ED69, 0xDAF96E6A, 0xC9A99D9E, 0x3BC21E9D, 0xEF087A76, 0x1D63F975, 0x0E330A81, 0xFC588982, - 0xB21572C9, 0x407EF1CA, 0x532E023E, 0xA145813D, 0x758FE5D6, 0x87E466D5, 0x94B49521, 0x66DF1622, - 0x38CC2A06, 0xCAA7A905, 0xD9F75AF1, 0x2B9CD9F2, 0xFF56BD19, 0x0D3D3E1A, 0x1E6DCDEE, 0xEC064EED, - 0xC38D26C4, 0x31E6A5C7, 0x22B65633, 0xD0DDD530, 0x0417B1DB, 0xF67C32D8, 0xE52CC12C, 0x1747422F, - 0x49547E0B, 0xBB3FFD08, 0xA86F0EFC, 0x5A048DFF, 0x8ECEE914, 0x7CA56A17, 0x6FF599E3, 0x9D9E1AE0, - 0xD3D3E1AB, 0x21B862A8, 0x32E8915C, 0xC083125F, 0x144976B4, 0xE622F5B7, 0xF5720643, 0x07198540, - 0x590AB964, 0xAB613A67, 0xB831C993, 0x4A5A4A90, 0x9E902E7B, 0x6CFBAD78, 0x7FAB5E8C, 0x8DC0DD8F, - 0xE330A81A, 0x115B2B19, 0x020BD8ED, 0xF0605BEE, 0x24AA3F05, 0xD6C1BC06, 0xC5914FF2, 0x37FACCF1, - 0x69E9F0D5, 0x9B8273D6, 0x88D28022, 0x7AB90321, 0xAE7367CA, 0x5C18E4C9, 0x4F48173D, 0xBD23943E, - 0xF36E6F75, 0x0105EC76, 0x12551F82, 0xE03E9C81, 0x34F4F86A, 0xC69F7B69, 0xD5CF889D, 0x27A40B9E, - 0x79B737BA, 0x8BDCB4B9, 0x988C474D, 0x6AE7C44E, 0xBE2DA0A5, 0x4C4623A6, 0x5F16D052, 0xAD7D5351 }; - -uint32_t csp_crc32_memory(const uint8_t * data, uint32_t length) { - uint32_t crc; - - crc = 0xFFFFFFFF; - while (length--) -#ifdef __AVR__ - crc = pgm_read_dword(&crc_tab[(crc ^ *data++) & 0xFFL]) ^ (crc >> 8); -#else - crc = crc_tab[(crc ^ *data++) & 0xFFL] ^ (crc >> 8); -#endif - - return (crc ^ 0xFFFFFFFF); -} - -int csp_crc32_append(csp_packet_t * packet, bool include_header) { - - uint32_t crc; - - /* NULL pointer check */ - if (packet == NULL) - return CSP_ERR_INVAL; - - /* Calculate CRC32, convert to network byte order */ - if (include_header) { - crc = csp_crc32_memory((uint8_t *) &packet->id, packet->length + sizeof(packet->id)); - } else { - crc = csp_crc32_memory(packet->data, packet->length); - } - crc = csp_hton32(crc); - - /* Copy checksum to packet */ - memcpy(&packet->data[packet->length], &crc, sizeof(uint32_t)); - packet->length += sizeof(uint32_t); - - return CSP_ERR_NONE; - -} - -int csp_crc32_verify(csp_packet_t * packet, bool include_header) { - - uint32_t crc; - - /* NULL pointer check */ - if (packet == NULL) - return CSP_ERR_INVAL; - - if (packet->length < sizeof(uint32_t)) - return CSP_ERR_INVAL; - - /* Calculate CRC32, convert to network byte order */ - if (include_header) { - crc = csp_crc32_memory((uint8_t *) &packet->id, packet->length + sizeof(packet->id) - sizeof(uint32_t)); - } else { - crc = csp_crc32_memory(packet->data, packet->length - sizeof(uint32_t)); - } - crc = csp_hton32(crc); - - /* Compare calculated checksum with packet header */ - if (memcmp(&packet->data[packet->length] - sizeof(uint32_t), &crc, sizeof(uint32_t)) != 0) { - /* CRC32 failed */ - return CSP_ERR_INVAL; - } else { - /* Strip CRC32 */ - packet->length -= sizeof(uint32_t); - return CSP_ERR_NONE; - } - -} - -#endif // CSP_USE_CRC32 diff --git a/thirdparty/libcsp/src/csp_debug.c b/thirdparty/libcsp/src/csp_debug.c deleted file mode 100644 index 2e710cb3..00000000 --- a/thirdparty/libcsp/src/csp_debug.c +++ /dev/null @@ -1,133 +0,0 @@ -/* -Cubesat Space Protocol - A small network-layer protocol designed for Cubesats -Copyright (C) 2012 Gomspace ApS (http://www.gomspace.com) -Copyright (C) 2012 AAUSAT3 Project (http://aausat3.space.aau.dk) - -This library is free software; you can redistribute it and/or -modify it under the terms of the GNU Lesser General Public -License as published by the Free Software Foundation; either -version 2.1 of the License, or (at your option) any later version. - -This library is distributed in the hope that it will be useful, -but WITHOUT ANY WARRANTY; without even the implied warranty of -MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU -Lesser General Public License for more details. - -You should have received a copy of the GNU Lesser General Public -License along with this library; if not, write to the Free Software -Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA -*/ - -#include -#include -#include -#include - -#ifdef __AVR__ -#include -#endif - -/* CSP includes */ -#include - -#include - -/* Custom debug function */ -csp_debug_hook_func_t csp_debug_hook_func = NULL; - -/* Debug levels */ -static bool csp_debug_level_enabled[] = { - [CSP_ERROR] = true, - [CSP_WARN] = true, - [CSP_INFO] = false, - [CSP_BUFFER] = false, - [CSP_PACKET] = false, - [CSP_PROTOCOL] = false, - [CSP_LOCK] = false, -}; - -/* Some compilers do not support weak symbols, so this function - * can be used instead to set a custom debug hook */ -void csp_debug_hook_set(csp_debug_hook_func_t f) -{ - csp_debug_hook_func = f; -} - -void do_csp_debug(csp_debug_level_t level, const char *format, ...) -{ - int color = COLOR_RESET; - va_list args; - - /* Don't print anything if log level is disabled */ - if (level > CSP_LOCK || !csp_debug_level_enabled[level]) - return; - - switch(level) { - case CSP_INFO: - color = COLOR_GREEN | COLOR_BOLD; - break; - case CSP_ERROR: - color = COLOR_RED | COLOR_BOLD; - break; - case CSP_WARN: - color = COLOR_YELLOW | COLOR_BOLD; - break; - case CSP_BUFFER: - color = COLOR_MAGENTA; - break; - case CSP_PACKET: - color = COLOR_GREEN; - break; - case CSP_PROTOCOL: - color = COLOR_BLUE; - break; - case CSP_LOCK: - color = COLOR_CYAN; - break; - default: - return; - } - - va_start(args, format); - - /* If csp_debug_hook symbol is defined, pass on the message. - * Otherwise, just print with pretty colors ... */ - if (csp_debug_hook_func) { - csp_debug_hook_func(level, format, args); - } else { - csp_sys_set_color(color); -#ifdef __AVR__ - vfprintf_P(stdout, format, args); -#else - vprintf(format, args); -#endif - printf("\r\n"); - csp_sys_set_color(COLOR_RESET); - } - - va_end(args); -} - -void csp_debug_set_level(csp_debug_level_t level, bool value) -{ - if (level > CSP_LOCK) - return; - csp_debug_level_enabled[level] = value; -} - -int csp_debug_get_level(csp_debug_level_t level) -{ - if (level > CSP_LOCK) - return 0; - return csp_debug_level_enabled[level]; -} - -void csp_debug_toggle_level(csp_debug_level_t level) -{ - if (level > CSP_LOCK) { - printf("Max level is 6\r\n"); - return; - } - csp_debug_level_enabled[level] = (csp_debug_level_enabled[level]) ? false : true; - printf("Level %u: value %u\r\n", level, csp_debug_level_enabled[level]); -} diff --git a/thirdparty/libcsp/src/csp_dedup.c b/thirdparty/libcsp/src/csp_dedup.c deleted file mode 100644 index d263c7a4..00000000 --- a/thirdparty/libcsp/src/csp_dedup.c +++ /dev/null @@ -1,66 +0,0 @@ -/* -Cubesat Space Protocol - A small network-layer protocol designed for Cubesats -Copyright (C) 2012 GomSpace ApS (http://www.gomspace.com) -Copyright (C) 2012 AAUSAT3 Project (http://aausat3.space.aau.dk) - -This library is free software; you can redistribute it and/or -modify it under the terms of the GNU Lesser General Public -License as published by the Free Software Foundation; either -version 2.1 of the License, or (at your option) any later version. - -This library is distributed in the hope that it will be useful, -but WITHOUT ANY WARRANTY; without even the implied warranty of -MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU -Lesser General Public License for more details. - -You should have received a copy of the GNU Lesser General Public -License along with this library; if not, write to the Free Software -Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA -*/ - -#include -#include -#include -#include - -#include -#include -#include - -#include "csp_dedup.h" - -/* Check the last CSP_DEDUP_COUNT packets for duplicates */ -#define CSP_DEDUP_COUNT 16 - -/* Only consider packet a duplicate if received under CSP_DEDUP_WINDOW_MS ago */ -#define CSP_DEDUP_WINDOW_MS 1000 - -/* Store packet CRC's in a ringbuffer */ -static uint32_t csp_dedup_array[CSP_DEDUP_COUNT] = {}; -static uint32_t csp_dedup_timestamp[CSP_DEDUP_COUNT] = {}; -static int csp_dedup_in = 0; - -bool csp_dedup_is_duplicate(csp_packet_t *packet) -{ - /* Calculate CRC32 for packet */ - uint32_t crc = csp_crc32_memory((const uint8_t *) &packet->id, packet->length + sizeof(packet->id)); - - /* Check if we have received this packet before */ - for (int i = 0; i < CSP_DEDUP_COUNT; i++) { - - /* Check for match */ - if (crc == csp_dedup_array[i]) { - - /* Check the timestamp */ - if (csp_get_ms() < csp_dedup_timestamp[i] + CSP_DEDUP_WINDOW_MS) - return true; - } - } - - /* If not, insert packet into duplicate list */ - csp_dedup_array[csp_dedup_in] = crc; - csp_dedup_timestamp[csp_dedup_in] = csp_get_ms(); - csp_dedup_in = (csp_dedup_in + 1) % CSP_DEDUP_COUNT; - - return false; -} diff --git a/thirdparty/libcsp/src/csp_dedup.h b/thirdparty/libcsp/src/csp_dedup.h deleted file mode 100644 index 75a3f124..00000000 --- a/thirdparty/libcsp/src/csp_dedup.h +++ /dev/null @@ -1,31 +0,0 @@ -/* -Cubesat Space Protocol - A small network-layer protocol designed for Cubesats -Copyright (C) 2012 GomSpace ApS (http://www.gomspace.com) -Copyright (C) 2012 AAUSAT3 Project (http://aausat3.space.aau.dk) - -This library is free software; you can redistribute it and/or -modify it under the terms of the GNU Lesser General Public -License as published by the Free Software Foundation; either -version 2.1 of the License, or (at your option) any later version. - -This library is distributed in the hope that it will be useful, -but WITHOUT ANY WARRANTY; without even the implied warranty of -MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU -Lesser General Public License for more details. - -You should have received a copy of the GNU Lesser General Public -License along with this library; if not, write to the Free Software -Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA -*/ - -#ifndef CSP_DEDUP_H_ -#define CSP_DEDUP_H_ - -/** - * Check for a duplicate packet - * @param packet pointer to packet - * @return false if not a duplicate, true if duplicate - */ -bool csp_dedup_is_duplicate(csp_packet_t *packet); - -#endif /* CSP_DEDUP_H_ */ diff --git a/thirdparty/libcsp/src/csp_endian.c b/thirdparty/libcsp/src/csp_endian.c deleted file mode 100644 index 6d0ef226..00000000 --- a/thirdparty/libcsp/src/csp_endian.c +++ /dev/null @@ -1,204 +0,0 @@ -/* -Cubesat Space Protocol - A small network-layer protocol designed for Cubesats -Copyright (C) 2012 GomSpace ApS (http://www.gomspace.com) -Copyright (C) 2012 AAUSAT3 Project (http://aausat3.space.aau.dk) - -This library is free software; you can redistribute it and/or -modify it under the terms of the GNU Lesser General Public -License as published by the Free Software Foundation; either -version 2.1 of the License, or (at your option) any later version. - -This library is distributed in the hope that it will be useful, -but WITHOUT ANY WARRANTY; without even the implied warranty of -MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU -Lesser General Public License for more details. - -You should have received a copy of the GNU Lesser General Public -License along with this library; if not, write to the Free Software -Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA -*/ - -#include - -/* CSP includes */ -#include -#include - -/* Convert 16-bit number from host byte order to network byte order */ -inline uint16_t __attribute__ ((__const__)) csp_hton16(uint16_t h16) { -#ifdef CSP_BIG_ENDIAN - return h16; -#else - return (((h16 & 0xff00) >> 8) | - ((h16 & 0x00ff) << 8)); -#endif -} - -/* Convert 16-bit number from network byte order to host byte order */ -inline uint16_t __attribute__ ((__const__)) csp_ntoh16(uint16_t n16) { - return csp_hton16(n16); -} - -/* Convert 32-bit number from host byte order to network byte order */ -inline uint32_t __attribute__ ((__const__)) csp_hton32(uint32_t h32) { -#ifdef CSP_BIG_ENDIAN - return h32; -#else - return (((h32 & 0xff000000) >> 24) | - ((h32 & 0x000000ff) << 24) | - ((h32 & 0x0000ff00) << 8) | - ((h32 & 0x00ff0000) >> 8)); -#endif -} - -/* Convert 32-bit number from network byte order to host byte order */ -inline uint32_t __attribute__ ((__const__)) csp_ntoh32(uint32_t n32) { - return csp_hton32(n32); -} - -/* Convert 64-bit number from host byte order to network byte order */ -inline uint64_t __attribute__ ((__const__)) csp_hton64(uint64_t h64) { -#ifdef CSP_BIG_ENDIAN - return h64; -#else - return (((h64 & 0xff00000000000000LL) >> 56) | - ((h64 & 0x00000000000000ffLL) << 56) | - ((h64 & 0x00ff000000000000LL) >> 40) | - ((h64 & 0x000000000000ff00LL) << 40) | - ((h64 & 0x0000ff0000000000LL) >> 24) | - ((h64 & 0x0000000000ff0000LL) << 24) | - ((h64 & 0x000000ff00000000LL) >> 8) | - ((h64 & 0x00000000ff000000LL) << 8)); -#endif -} - -/* Convert 64-bit number from host byte order to network byte order */ -inline uint64_t __attribute__ ((__const__)) csp_ntoh64(uint64_t n64) { - return csp_hton64(n64); -} - -/* Convert 16-bit number from host byte order to big endian byte order */ -inline uint16_t __attribute__ ((__const__)) csp_htobe16(uint16_t h16) { - return csp_hton16(h16); -} - -/* Convert 16-bit number from host byte order to little endian byte order */ -inline uint16_t __attribute__ ((__const__)) csp_htole16(uint16_t h16) { -#ifdef CSP_LITTLE_ENDIAN - return h16; -#else - return (((h16 & 0xff00) >> 8) | - ((h16 & 0x00ff) << 8)); -#endif -} - -/* Convert 16-bit number from big endian byte order to little endian byte order */ -inline uint16_t __attribute__ ((__const__)) csp_betoh16(uint16_t be16) { - return csp_ntoh16(be16); -} - -/* Convert 16-bit number from little endian byte order to host byte order */ -inline uint16_t __attribute__ ((__const__)) csp_letoh16(uint16_t le16) { - return csp_htole16(le16); -} - -/* Convert 32-bit number from host byte order to big endian byte order */ -inline uint32_t __attribute__ ((__const__)) csp_htobe32(uint32_t h32) { - return csp_hton32(h32); -} - -/* Convert 32-bit number from little endian byte order to host byte order */ -inline uint32_t __attribute__ ((__const__)) csp_htole32(uint32_t h32) { -#ifdef CSP_LITTLE_ENDIAN - return h32; -#else - return (((h32 & 0xff000000) >> 24) | - ((h32 & 0x000000ff) << 24) | - ((h32 & 0x0000ff00) << 8) | - ((h32 & 0x00ff0000) >> 8)); -#endif -} - -/* Convert 32-bit number from big endian byte order to host byte order */ -inline uint32_t __attribute__ ((__const__)) csp_betoh32(uint32_t be32) { - return csp_ntoh32(be32); -} - -/* Convert 32-bit number from little endian byte order to host byte order */ -inline uint32_t __attribute__ ((__const__)) csp_letoh32(uint32_t le32) { - return csp_htole32(le32); -} - -/* Convert 64-bit number from host byte order to big endian byte order */ -inline uint64_t __attribute__ ((__const__)) csp_htobe64(uint64_t h64) { - return csp_hton64(h64); -} - -/* Convert 64-bit number from host byte order to little endian byte order */ -inline uint64_t __attribute__ ((__const__)) csp_htole64(uint64_t h64) { -#ifdef CSP_LITTLE_ENDIAN - return h64; -#else - return (((h64 & 0xff00000000000000LL) >> 56) | - ((h64 & 0x00000000000000ffLL) << 56) | - ((h64 & 0x00ff000000000000LL) >> 40) | - ((h64 & 0x000000000000ff00LL) << 40) | - ((h64 & 0x0000ff0000000000LL) >> 24) | - ((h64 & 0x0000000000ff0000LL) << 24) | - ((h64 & 0x000000ff00000000LL) >> 8) | - ((h64 & 0x00000000ff000000LL) << 8)); -#endif -} - -/* Convert 64-bit number from big endian byte order to host byte order */ -inline uint64_t __attribute__ ((__const__)) csp_betoh64(uint64_t be64) { - return csp_ntoh64(be64); -} - -/* Convert 64-bit number from little endian byte order to host byte order */ -inline uint64_t __attribute__ ((__const__)) csp_letoh64(uint64_t le64) { - return csp_htole64(le64); -} - - -/* Convert float from host byte order to network byte order */ -inline float __attribute__ ((__const__)) csp_htonflt(float f) { -#ifdef CSP_BIG_ENDIAN - return f; -#else - union v { - float f; - uint32_t i; - }; - union v val; - val.f = f; - val.i = csp_hton32(val.i); - return val.f; -#endif -} - -/* Convert float from host byte order to network byte order */ -inline float __attribute__ ((__const__)) csp_ntohflt(float f) { - return csp_htonflt(f); -} - -/* Convert double from host byte order to network byte order */ -inline double __attribute__ ((__const__)) csp_htondbl(double d) { -#ifdef CSP_BIG_ENDIAN - return d; -#else - union v { - double d; - uint64_t i; - }; - union v val; - val.d = d; - val.i = csp_hton64(val.i); - return val.d; -#endif -} - -/* Convert float from host byte order to network byte order */ -inline double __attribute__ ((__const__)) csp_ntohdbl(double d) { - return csp_htondbl(d); -} diff --git a/thirdparty/libcsp/src/csp_hex_dump.c b/thirdparty/libcsp/src/csp_hex_dump.c deleted file mode 100644 index af0a2660..00000000 --- a/thirdparty/libcsp/src/csp_hex_dump.c +++ /dev/null @@ -1,55 +0,0 @@ -#include -#include - -void csp_hex_dump (const char *desc, void *addr, int len) -{ - int i; - unsigned char buff[17]; - unsigned char *pc = (unsigned char*)addr; - - // Output description if given. - if (desc != NULL) - printf ("%s:\r\n", desc); - - if (len == 0) { - printf(" ZERO LENGTH\r\n"); - return; - } - if (len < 0) { - printf(" NEGATIVE LENGTH: %i\r\n",len); - return; - } - - // Process every byte in the data. - for (i = 0; i < len; i++) { - // Multiple of 16 means new line (with line offset). - - if ((i % 16) == 0) { - // Just don't print ASCII for the zeroth line. - if (i != 0) - printf (" %s\r\n", buff); - - // Output the offset. - printf (" %p ", addr + i); - } - - // Now the hex code for the specific character. - printf (" %02x", pc[i]); - - // And store a printable ASCII character for later. - if ((pc[i] < 0x20) || (pc[i] > 0x7e)) - buff[i % 16] = '.'; - else - buff[i % 16] = pc[i]; - buff[(i % 16) + 1] = '\0'; - } - - // Pad out last line if not exactly 16 characters. - while ((i % 16) != 0) { - printf (" "); - i++; - } - - // And print the final ASCII bit. - printf (" %s\r\n", buff); -} diff --git a/thirdparty/libcsp/src/csp_iflist.c b/thirdparty/libcsp/src/csp_iflist.c deleted file mode 100644 index b17dca44..00000000 --- a/thirdparty/libcsp/src/csp_iflist.c +++ /dev/null @@ -1,103 +0,0 @@ -/* -Cubesat Space Protocol - A small network-layer protocol designed for Cubesats -Copyright (C) 2012 GomSpace ApS (http://www.gomspace.com) -Copyright (C) 2012 AAUSAT3 Project (http://aausat3.space.aau.dk) - -This library is free software; you can redistribute it and/or -modify it under the terms of the GNU Lesser General Public -License as published by the Free Software Foundation; either -version 2.1 of the License, or (at your option) any later version. - -This library is distributed in the hope that it will be useful, -but WITHOUT ANY WARRANTY; without even the implied warranty of -MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU -Lesser General Public License for more details. - -You should have received a copy of the GNU Lesser General Public -License along with this library; if not, write to the Free Software -Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA -*/ - -#include -#include - -/* Interfaces are stored in a linked list*/ -static csp_iface_t * interfaces = NULL; - -csp_iface_t * csp_iflist_get_by_name(const char *name) { - csp_iface_t *ifc = interfaces; - while(ifc) { - if (strncmp(ifc->name, name, 10) == 0) - break; - ifc = ifc->next; - } - return ifc; -} - -void csp_iflist_add(csp_iface_t *ifc) { - - /* Add interface to pool */ - if (interfaces == NULL) { - /* This is the first interface to be added */ - interfaces = ifc; - ifc->next = NULL; - } else { - /* One or more interfaces were already added */ - csp_iface_t * i = interfaces; - while (i != ifc && i->next) - i = i->next; - - /* Insert interface last if not already in pool */ - if (i != ifc && i->next == NULL) { - i->next = ifc; - ifc->next = NULL; - } - } - -} - -csp_iface_t * csp_iflist_get(void) -{ - return interfaces; -} - -#ifdef CSP_DEBUG -static int csp_bytesize(char *buf, int len, unsigned long int n) { - char postfix; - double size; - - if (n >= 1048576) { - size = n/1048576.0; - postfix = 'M'; - } else if (n >= 1024) { - size = n/1024.; - postfix = 'K'; - } else { - size = n; - postfix = 'B'; - } - -#pragma GCC diagnostic push -#pragma GCC diagnostic ignored "-Wformat-truncation" - return snprintf(buf, len, "%.1f%c", size, postfix); -#pragma GCC diagnostic pop -} - -void csp_iflist_print(void) { - csp_iface_t * i = interfaces; - char txbuf[25], rxbuf[25]; - - while (i) { - csp_bytesize(txbuf, 25, i->txbytes); - csp_bytesize(rxbuf, 25, i->rxbytes); - printf("%-5s tx: %05"PRIu32" rx: %05"PRIu32" txe: %05"PRIu32" rxe: %05"PRIu32"\r\n" - " drop: %05"PRIu32" autherr: %05"PRIu32 " frame: %05"PRIu32"\r\n" - " txb: %"PRIu32" (%s) rxb: %"PRIu32" (%s)\r\n\r\n", - i->name, i->tx, i->rx, i->tx_error, i->rx_error, i->drop, - i->autherr, i->frame, i->txbytes, txbuf, i->rxbytes, rxbuf); - i = i->next; - } - -} -#endif - diff --git a/thirdparty/libcsp/src/csp_io.c b/thirdparty/libcsp/src/csp_io.c deleted file mode 100644 index 3d7f614a..00000000 --- a/thirdparty/libcsp/src/csp_io.c +++ /dev/null @@ -1,502 +0,0 @@ -/* -Cubesat Space Protocol - A small network-layer protocol designed for Cubesats -Copyright (C) 2012 GomSpace ApS (http://www.gomspace.com) -Copyright (C) 2012 AAUSAT3 Project (http://aausat3.space.aau.dk) - -This library is free software; you can redistribute it and/or -modify it under the terms of the GNU Lesser General Public -License as published by the Free Software Foundation; either -version 2.1 of the License, or (at your option) any later version. - -This library is distributed in the hope that it will be useful, -but WITHOUT ANY WARRANTY; without even the implied warranty of -MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU -Lesser General Public License for more details. - -You should have received a copy of the GNU Lesser General Public -License along with this library; if not, write to the Free Software -Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA -*/ - -#include -#include -#include -#include - -/* CSP includes */ -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include -#include - -#include -#include - -#include "csp_io.h" -#include "csp_port.h" -#include "csp_conn.h" -#include "csp_route.h" -#include "csp_promisc.h" -#include "csp_qfifo.h" -#include "transport/csp_transport.h" - -/** CSP address of this node */ -static uint8_t csp_my_address; - -/* Hostname, model and build revision */ -static const char *csp_hostname = NULL; -static const char *csp_model = NULL; -static const char *csp_revision = GIT_REV; - -#ifdef CSP_USE_PROMISC -extern csp_queue_handle_t csp_promisc_queue; -#endif - -void csp_set_address(uint8_t addr) -{ - csp_my_address = addr; -} - -uint8_t csp_get_address(void) -{ - return csp_my_address; -} - -void csp_set_hostname(const char *hostname) -{ - csp_hostname = hostname; -} - -const char *csp_get_hostname(void) -{ - return csp_hostname; -} - -void csp_set_model(const char *model) -{ - csp_model = model; -} - -const char *csp_get_model(void) -{ - return csp_model; -} - -void csp_set_revision(const char *revision) -{ - csp_revision = revision; -} - -const char *csp_get_revision(void) -{ - return csp_revision; -} - -int csp_init(unsigned char address) { - - int ret; - - /* Initialize CSP */ - csp_set_address(address); - - ret = csp_conn_init(); - if (ret != CSP_ERR_NONE) - return ret; - - ret = csp_port_init(); - if (ret != CSP_ERR_NONE) - return ret; - - ret = csp_qfifo_init(); - if (ret != CSP_ERR_NONE) - return ret; - - /* Loopback */ - csp_iflist_add(&csp_if_lo); - - /* Register loopback route */ - csp_route_set(csp_get_address(), &csp_if_lo, CSP_NODE_MAC); - - /* Also register loopback as default, until user redefines default route */ - csp_route_set(CSP_DEFAULT_ROUTE, &csp_if_lo, CSP_NODE_MAC); - - return CSP_ERR_NONE; - -} - -csp_socket_t * csp_socket(uint32_t opts) { - - /* Validate socket options */ -#ifndef CSP_USE_RDP - if (opts & CSP_SO_RDPREQ) { - csp_log_error("Attempt to create socket that requires RDP, but CSP was compiled without RDP support"); - return NULL; - } -#endif - -#ifndef CSP_USE_XTEA - if (opts & CSP_SO_XTEAREQ) { - csp_log_error("Attempt to create socket that requires XTEA, but CSP was compiled without XTEA support"); - return NULL; - } -#endif - -#ifndef CSP_USE_HMAC - if (opts & CSP_SO_HMACREQ) { - csp_log_error("Attempt to create socket that requires HMAC, but CSP was compiled without HMAC support"); - return NULL; - } -#endif - -#ifndef CSP_USE_CRC32 - if (opts & CSP_SO_CRC32REQ) { - csp_log_error("Attempt to create socket that requires CRC32, but CSP was compiled without CRC32 support"); - return NULL; - } -#endif - - /* Drop packet if reserved flags are set */ - if (opts & ~(CSP_SO_RDPREQ | CSP_SO_XTEAREQ | CSP_SO_HMACREQ | CSP_SO_CRC32REQ | CSP_SO_CONN_LESS)) { - csp_log_error("Invalid socket option"); - return NULL; - } - - /* Use CSP buffers instead? */ - csp_socket_t * sock = csp_conn_allocate(CONN_SERVER); - if (sock == NULL) - return NULL; - - /* If connectionless, init the queue to a pre-defined size - * if not, the user must init the queue using csp_listen */ - if (opts & CSP_SO_CONN_LESS) { - sock->socket = csp_queue_create(CSP_CONN_QUEUE_LENGTH, sizeof(csp_packet_t *)); - if (sock->socket == NULL) { - csp_close(sock); - return NULL; - } - } else { - sock->socket = NULL; - } - sock->opts = opts; - - return sock; - -} - -csp_conn_t * csp_accept(csp_socket_t * sock, uint32_t timeout) { - - if (sock == NULL) - return NULL; - - if (sock->socket == NULL) - return NULL; - - csp_conn_t * conn; - if (csp_queue_dequeue(sock->socket, &conn, timeout) == CSP_QUEUE_OK) - return conn; - - return NULL; - -} - -csp_packet_t * csp_read(csp_conn_t * conn, uint32_t timeout) { - - csp_packet_t * packet = NULL; - - if (conn == NULL || conn->state != CONN_OPEN) - return NULL; - -#ifdef CSP_USE_QOS - int prio, event; - if (csp_queue_dequeue(conn->rx_event, &event, timeout) != CSP_QUEUE_OK) - return NULL; - - for (prio = 0; prio < CSP_RX_QUEUES; prio++) - if (csp_queue_dequeue(conn->rx_queue[prio], &packet, 0) == CSP_QUEUE_OK) - break; -#else - if (csp_queue_dequeue(conn->rx_queue[0], &packet, timeout) != CSP_QUEUE_OK) - return NULL; -#endif - -#ifdef CSP_USE_RDP - /* Packet read could trigger ACK transmission */ - if (conn->idin.flags & CSP_FRDP && conn->rdp.delayed_acks) - csp_rdp_check_ack(conn); - -#endif - - return packet; - -} - -int csp_send_direct(csp_id_t idout, csp_packet_t * packet, csp_iface_t * ifout, uint32_t timeout) { - - if (packet == NULL) { - csp_log_error("csp_send_direct called with NULL packet"); - goto err; - } - - if ((ifout == NULL) || (ifout->nexthop == NULL)) { - csp_log_error("No route to host: %#08x", idout.ext); - goto err; - } - - csp_log_packet("OUT: S %u, D %u, Dp %u, Sp %u, Pr %u, Fl 0x%02X, Sz %u VIA: %s", - idout.src, idout.dst, idout.dport, idout.sport, idout.pri, idout.flags, packet->length, ifout->name); - - /* Copy identifier to packet (before crc, xtea and hmac) */ - packet->id.ext = idout.ext; - -#ifdef CSP_USE_PROMISC - /* Loopback traffic is added to promisc queue by the router */ - if (idout.dst != csp_get_address() && idout.src == csp_get_address()) { - packet->id.ext = idout.ext; - csp_promisc_add(packet); - } -#endif - - /* Only encrypt packets from the current node */ - if (idout.src == csp_get_address()) { - /* Append HMAC */ - if (idout.flags & CSP_FHMAC) { -#ifdef CSP_USE_HMAC - /* Calculate and add HMAC (does not include header for backwards compatability with csp1.x) */ - if (csp_hmac_append(packet, false) != 0) { - /* HMAC append failed */ - csp_log_warn("HMAC append failed!"); - goto tx_err; - } -#else - csp_log_warn("Attempt to send packet with HMAC, but CSP was compiled without HMAC support. Discarding packet"); - goto tx_err; -#endif - } - - /* Append CRC32 */ - if (idout.flags & CSP_FCRC32) { -#ifdef CSP_USE_CRC32 - /* Calculate and add CRC32 (does not include header for backwards compatability with csp1.x) */ - if (csp_crc32_append(packet, false) != 0) { - /* CRC32 append failed */ - csp_log_warn("CRC32 append failed!"); - goto tx_err; - } -#else - csp_log_warn("Attempt to send packet with CRC32, but CSP was compiled without CRC32 support. Sending without CRC32r"); - idout.flags &= ~(CSP_FCRC32); -#endif - } - - if (idout.flags & CSP_FXTEA) { -#ifdef CSP_USE_XTEA - /* Create nonce */ - uint32_t nonce, nonce_n; - nonce = (uint32_t)rand(); - nonce_n = csp_hton32(nonce); - memcpy(&packet->data[packet->length], &nonce_n, sizeof(nonce_n)); - - /* Create initialization vector */ - uint32_t iv[2] = {nonce, 1}; - - /* Encrypt data */ - if (csp_xtea_encrypt(packet->data, packet->length, iv) != 0) { - /* Encryption failed */ - csp_log_warn("Encryption failed! Discarding packet"); - goto tx_err; - } - - packet->length += sizeof(nonce_n); -#else - csp_log_warn("Attempt to send XTEA encrypted packet, but CSP was compiled without XTEA support. Discarding packet"); - goto tx_err; -#endif - } - } - - /* Store length before passing to interface */ - uint16_t bytes = packet->length; - uint16_t mtu = ifout->mtu; - - if (mtu > 0 && bytes > mtu) - goto tx_err; - - if ((*ifout->nexthop)(ifout, packet, timeout) != CSP_ERR_NONE) - goto tx_err; - - ifout->tx++; - ifout->txbytes += bytes; - return CSP_ERR_NONE; - -tx_err: - ifout->tx_error++; -err: - return CSP_ERR_TX; - -} - -int csp_send(csp_conn_t * conn, csp_packet_t * packet, uint32_t timeout) { - - int ret; - - if ((conn == NULL) || (packet == NULL) || (conn->state != CONN_OPEN)) { - csp_log_error("Invalid call to csp_send"); - return 0; - } - -#ifdef CSP_USE_RDP - if (conn->idout.flags & CSP_FRDP) { - if (csp_rdp_send(conn, packet, timeout) != CSP_ERR_NONE) { - csp_iface_t * ifout = csp_rtable_find_iface(conn->idout.dst); - if (ifout != NULL) - ifout->tx_error++; - csp_log_warn("RDP send failed!"); - return 0; - } - } -#endif - - csp_iface_t * ifout = csp_rtable_find_iface(conn->idout.dst); - ret = csp_send_direct(conn->idout, packet, ifout, timeout); - - return (ret == CSP_ERR_NONE) ? 1 : 0; - -} - -int csp_send_prio(uint8_t prio, csp_conn_t * conn, csp_packet_t * packet, uint32_t timeout) { - conn->idout.pri = prio; - return csp_send(conn, packet, timeout); -} - -int csp_transaction_persistent(csp_conn_t * conn, uint32_t timeout, void * outbuf, int outlen, void * inbuf, int inlen) { - - int size = (inlen > outlen) ? inlen : outlen; - csp_packet_t * packet = csp_buffer_get(size); - if (packet == NULL) - return 0; - - /* Copy the request */ - if (outlen > 0 && outbuf != NULL) - memcpy(packet->data, outbuf, outlen); - packet->length = outlen; - - if (!csp_send(conn, packet, timeout)) { - csp_buffer_free(packet); - return 0; - } - - /* If no reply is expected, return now */ - if (inlen == 0) - return 1; - - packet = csp_read(conn, timeout); - if (packet == NULL) - return 0; - - if ((inlen != -1) && ((int)packet->length != inlen)) { - csp_log_error("Reply length %u expected %u", packet->length, inlen); - csp_buffer_free(packet); - return 0; - } - - memcpy(inbuf, packet->data, packet->length); - int length = packet->length; - csp_buffer_free(packet); - return length; - -} - -int csp_transaction(uint8_t prio, uint8_t dest, uint8_t port, uint32_t timeout, void * outbuf, int outlen, void * inbuf, int inlen) { - return csp_transaction2(prio, dest, port, timeout, outbuf, outlen, inbuf, inlen, 0); -} - -int csp_transaction2(uint8_t prio, uint8_t dest, uint8_t port, uint32_t timeout, void * outbuf, int outlen, void * inbuf, int inlen, uint32_t opts) { - - csp_conn_t * conn = csp_connect(prio, dest, port, 0, opts); - if (conn == NULL) - return 0; - - int status = csp_transaction_persistent(conn, timeout, outbuf, outlen, inbuf, inlen); - - csp_close(conn); - - return status; - -} - -csp_packet_t * csp_recvfrom(csp_socket_t * socket, uint32_t timeout) { - - if ((socket == NULL) || (!(socket->opts & CSP_SO_CONN_LESS))) - return NULL; - - csp_packet_t * packet = NULL; - csp_queue_dequeue(socket->socket, &packet, timeout); - - return packet; - -} - -int csp_sendto(uint8_t prio, uint8_t dest, uint8_t dport, uint8_t src_port, uint32_t opts, csp_packet_t * packet, uint32_t timeout) { - - packet->id.flags = 0; - - if (opts & CSP_O_RDP) { - csp_log_error("Attempt to create RDP packet on connection-less socket"); - return CSP_ERR_INVAL; - } - - if (opts & CSP_O_HMAC) { -#ifdef CSP_USE_HMAC - packet->id.flags |= CSP_FHMAC; -#else - csp_log_error("Attempt to create HMAC authenticated packet, but CSP was compiled without HMAC support"); - return CSP_ERR_NOTSUP; -#endif - } - - if (opts & CSP_O_XTEA) { -#ifdef CSP_USE_XTEA - packet->id.flags |= CSP_FXTEA; -#else - csp_log_error("Attempt to create XTEA encrypted packet, but CSP was compiled without XTEA support"); - return CSP_ERR_NOTSUP; -#endif - } - - if (opts & CSP_O_CRC32) { -#ifdef CSP_USE_CRC32 - packet->id.flags |= CSP_FCRC32; -#else - csp_log_error("Attempt to create CRC32 validated packet, but CSP was compiled without CRC32 support"); - return CSP_ERR_NOTSUP; -#endif - } - - packet->id.dst = dest; - packet->id.dport = dport; - packet->id.src = csp_get_address(); - packet->id.sport = src_port; - packet->id.pri = prio; - - csp_iface_t * ifout = csp_rtable_find_iface(dest); - if (csp_send_direct(packet->id, packet, ifout, timeout) != CSP_ERR_NONE) - return CSP_ERR_NOTSUP; - - return CSP_ERR_NONE; - -} - -int csp_sendto_reply(csp_packet_t * request_packet, csp_packet_t * reply_packet, uint32_t opts, uint32_t timeout) { - if (request_packet == NULL) - return CSP_ERR_INVAL; - - return csp_sendto(request_packet->id.pri, request_packet->id.src, request_packet->id.sport, request_packet->id.dport, opts, reply_packet, timeout); -} diff --git a/thirdparty/libcsp/src/csp_io.h b/thirdparty/libcsp/src/csp_io.h deleted file mode 100644 index 6ea8dfec..00000000 --- a/thirdparty/libcsp/src/csp_io.h +++ /dev/null @@ -1,47 +0,0 @@ -/* -Cubesat Space Protocol - A small network-layer protocol designed for Cubesats -Copyright (C) 2012 GomSpace ApS (http://www.gomspace.com) -Copyright (C) 2012 AAUSAT3 Project (http://aausat3.space.aau.dk) - -This library is free software; you can redistribute it and/or -modify it under the terms of the GNU Lesser General Public -License as published by the Free Software Foundation; either -version 2.1 of the License, or (at your option) any later version. - -This library is distributed in the hope that it will be useful, -but WITHOUT ANY WARRANTY; without even the implied warranty of -MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU -Lesser General Public License for more details. - -You should have received a copy of the GNU Lesser General Public -License along with this library; if not, write to the Free Software -Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA -*/ - -#ifndef _CSP_IO_H_ -#define _CSP_IO_H_ - -#ifdef __cplusplus -extern "C" { -#endif - -#include - -#include - -/** - * Function to transmit a frame without an existing connection structure. - * This function is used for stateless transmissions - * @param idout 32bit CSP identifier - * @param packet pointer to packet, - * @param ifout pointer to output interface - * @param timeout a timeout to wait for TX to complete. NOTE: not all underlying drivers supports flow-control. - * @return returns 1 if successful and 0 otherwise. you MUST free the frame yourself if the transmission was not successful. - */ -int csp_send_direct(csp_id_t idout, csp_packet_t * packet, csp_iface_t * ifout, uint32_t timeout); - -#ifdef __cplusplus -} /* extern "C" */ -#endif - -#endif // _CSP_IO_H_ diff --git a/thirdparty/libcsp/src/csp_port.c b/thirdparty/libcsp/src/csp_port.c deleted file mode 100644 index 2a4ac2a9..00000000 --- a/thirdparty/libcsp/src/csp_port.c +++ /dev/null @@ -1,105 +0,0 @@ -/* -Cubesat Space Protocol - A small network-layer protocol designed for Cubesats -Copyright (C) 2012 GomSpace ApS (http://www.gomspace.com) -Copyright (C) 2012 AAUSAT3 Project (http://aausat3.space.aau.dk) - -This library is free software; you can redistribute it and/or -modify it under the terms of the GNU Lesser General Public -License as published by the Free Software Foundation; either -version 2.1 of the License, or (at your option) any later version. - -This library is distributed in the hope that it will be useful, -but WITHOUT ANY WARRANTY; without even the implied warranty of -MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU -Lesser General Public License for more details. - -You should have received a copy of the GNU Lesser General Public -License along with this library; if not, write to the Free Software -Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA -*/ - -#include -#include -#include - -/* CSP includes */ -#include -#include - -#include -#include -#include - -#include "csp_port.h" -#include "csp_conn.h" - -/* Allocation of ports */ -static csp_port_t ports[CSP_MAX_BIND_PORT + 2]; - -csp_socket_t * csp_port_get_socket(unsigned int port) { - - csp_socket_t * ret = NULL; - - if (port >= CSP_ANY) - return NULL; - - /* Match dport to socket or local "catch all" port number */ - if (ports[port].state == PORT_OPEN) - ret = ports[port].socket; - else if (ports[CSP_ANY].state == PORT_OPEN) - ret = ports[CSP_ANY].socket; - - return ret; - -} - -int csp_port_init(void) { - - memset(ports, PORT_CLOSED, sizeof(csp_port_t) * (CSP_MAX_BIND_PORT + 2)); - - return CSP_ERR_NONE; - -} - -int csp_listen(csp_socket_t * socket, size_t conn_queue_length) { - - if (socket == NULL) - return CSP_ERR_INVAL; - - socket->socket = csp_queue_create(conn_queue_length, sizeof(csp_conn_t *)); - if (socket->socket == NULL) - return CSP_ERR_NOMEM; - - socket->opts |= CSP_SO_INTERNAL_LISTEN; - - return CSP_ERR_NONE; - -} - -int csp_bind(csp_socket_t * socket, uint8_t port) { - - if (socket == NULL) - return CSP_ERR_INVAL; - - if (port > CSP_ANY) { - csp_log_error("Only ports from 0-%u (and CSP_ANY for default) are available for incoming ports", CSP_ANY); - return CSP_ERR_INVAL; - } - - /* Check if port number is valid */ - if (ports[port].state != PORT_CLOSED) { - csp_log_error("Port %d is already in use", port); - return CSP_ERR_USED; - } - - csp_log_info("Binding socket %p to port %u", socket, port); - - /* Save listener */ - ports[port].socket = socket; - ports[port].state = PORT_OPEN; - - return CSP_ERR_NONE; - -} - - diff --git a/thirdparty/libcsp/src/csp_port.h b/thirdparty/libcsp/src/csp_port.h deleted file mode 100644 index d2ec06e9..00000000 --- a/thirdparty/libcsp/src/csp_port.h +++ /dev/null @@ -1,55 +0,0 @@ -/* -Cubesat Space Protocol - A small network-layer protocol designed for Cubesats -Copyright (C) 2012 GomSpace ApS (http://www.gomspace.com) -Copyright (C) 2012 AAUSAT3 Project (http://aausat3.space.aau.dk) - -This library is free software; you can redistribute it and/or -modify it under the terms of the GNU Lesser General Public -License as published by the Free Software Foundation; either -version 2.1 of the License, or (at your option) any later version. - -This library is distributed in the hope that it will be useful, -but WITHOUT ANY WARRANTY; without even the implied warranty of -MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU -Lesser General Public License for more details. - -You should have received a copy of the GNU Lesser General Public -License along with this library; if not, write to the Free Software -Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA -*/ - -#ifndef _CSP_PORT_H_ -#define _CSP_PORT_H_ - -#ifdef __cplusplus -extern "C" { -#endif - -#include - -#include - -/** @brief Port states */ -typedef enum { - PORT_CLOSED = 0, - PORT_OPEN = 1, -} csp_port_state_t; - -/** @brief Port struct */ -typedef struct { - csp_port_state_t state; // Port state - csp_socket_t * socket; // New connections are added to this socket's conn queue -} csp_port_t; - -/** - * Init ports array - */ -int csp_port_init(void); - -csp_socket_t * csp_port_get_socket(unsigned int dport); - -#ifdef __cplusplus -} /* extern "C" */ -#endif - -#endif // _CSP_PORT_H_ diff --git a/thirdparty/libcsp/src/csp_promisc.c b/thirdparty/libcsp/src/csp_promisc.c deleted file mode 100644 index 5f156c33..00000000 --- a/thirdparty/libcsp/src/csp_promisc.c +++ /dev/null @@ -1,82 +0,0 @@ -/* -Cubesat Space Protocol - A small network-layer protocol designed for Cubesats -Copyright (C) 2012 Gomspace ApS (http://www.gomspace.com) -Copyright (C) 2012 AAUSAT3 Project (http://aausat3.space.aau.dk) - -This library is free software; you can redistribute it and/or -modify it under the terms of the GNU Lesser General Public -License as published by the Free Software Foundation; either -version 2.1 of the License, or (at your option) any later version. - -This library is distributed in the hope that it will be useful, -but WITHOUT ANY WARRANTY; without even the implied warranty of -MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU -Lesser General Public License for more details. - -You should have received a copy of the GNU Lesser General Public -License along with this library; if not, write to the Free Software -Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA -*/ - -#include -#include - -#ifdef CSP_USE_PROMISC - -static csp_queue_handle_t csp_promisc_queue = NULL; -static int csp_promisc_enabled = 0; - -int csp_promisc_enable(unsigned int buf_size) { - - /* If queue already initialised */ - if (csp_promisc_queue != NULL) { - csp_promisc_enabled = 1; - return CSP_ERR_NONE; - } - - /* Create packet queue */ - csp_promisc_queue = csp_queue_create(buf_size, sizeof(csp_packet_t *)); - - if (csp_promisc_queue == NULL) - return CSP_ERR_INVAL; - - csp_promisc_enabled = 1; - return CSP_ERR_NONE; - -} - -void csp_promisc_disable(void) { - csp_promisc_enabled = 0; -} - -csp_packet_t * csp_promisc_read(uint32_t timeout) { - - if (csp_promisc_queue == NULL) - return NULL; - - csp_packet_t * packet = NULL; - csp_queue_dequeue(csp_promisc_queue, &packet, timeout); - - return packet; - -} - -void csp_promisc_add(csp_packet_t * packet) { - - if (csp_promisc_enabled == 0) - return; - - if (csp_promisc_queue != NULL) { - /* Make a copy of the message and queue it to the promiscuous task */ - csp_packet_t *packet_copy = csp_buffer_clone(packet); - if (packet_copy != NULL) { - if (csp_queue_enqueue(csp_promisc_queue, &packet_copy, 0) != CSP_QUEUE_OK) { - csp_log_error("Promiscuous mode input queue full"); - csp_buffer_free(packet_copy); - } - } - } - -} - -#endif diff --git a/thirdparty/libcsp/src/csp_promisc.h b/thirdparty/libcsp/src/csp_promisc.h deleted file mode 100644 index be62edda..00000000 --- a/thirdparty/libcsp/src/csp_promisc.h +++ /dev/null @@ -1,30 +0,0 @@ -/* -Cubesat Space Protocol - A small network-layer protocol designed for Cubesats -Copyright (C) 2012 Gomspace ApS (http://www.gomspace.com) -Copyright (C) 2012 AAUSAT3 Project (http://aausat3.space.aau.dk) - -This library is free software; you can redistribute it and/or -modify it under the terms of the GNU Lesser General Public -License as published by the Free Software Foundation; either -version 2.1 of the License, or (at your option) any later version. - -This library is distributed in the hope that it will be useful, -but WITHOUT ANY WARRANTY; without even the implied warranty of -MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU -Lesser General Public License for more details. - -You should have received a copy of the GNU Lesser General Public -License along with this library; if not, write to the Free Software -Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA -*/ - -#ifndef CSP_PROMISC_H_ -#define CSP_PROMISC_H_ - -/** - * Add packet to promiscuous mode packet queue - * @param packet Packet to add to the queue - */ -void csp_promisc_add(csp_packet_t * packet); - -#endif /* CSP_PROMISC_H_ */ diff --git a/thirdparty/libcsp/src/csp_qfifo.c b/thirdparty/libcsp/src/csp_qfifo.c deleted file mode 100644 index 9329b2ca..00000000 --- a/thirdparty/libcsp/src/csp_qfifo.c +++ /dev/null @@ -1,149 +0,0 @@ -/* -Cubesat Space Protocol - A small network-layer protocol designed for Cubesats -Copyright (C) 2012 GomSpace ApS (http://www.gomspace.com) -Copyright (C) 2012 AAUSAT3 Project (http://aausat3.space.aau.dk) - -This library is free software; you can redistribute it and/or -modify it under the terms of the GNU Lesser General Public -License as published by the Free Software Foundation; either -version 2.1 of the License, or (at your option) any later version. - -This library is distributed in the hope that it will be useful, -but WITHOUT ANY WARRANTY; without even the implied warranty of -MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU -Lesser General Public License for more details. - -You should have received a copy of the GNU Lesser General Public -License along with this library; if not, write to the Free Software -Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA -*/ - -#include -#include -#include -#include "csp_qfifo.h" - -static csp_queue_handle_t qfifo[CSP_ROUTE_FIFOS]; -#ifdef CSP_USE_QOS -static csp_queue_handle_t qfifo_events; -#endif - -int csp_qfifo_init(void) { - int prio; - - /* Create router fifos for each priority */ - for (prio = 0; prio < CSP_ROUTE_FIFOS; prio++) { - if (qfifo[prio] == NULL) { - qfifo[prio] = csp_queue_create(CSP_FIFO_INPUT, sizeof(csp_qfifo_t)); - if (!qfifo[prio]) - return CSP_ERR_NOMEM; - } - } - -#ifdef CSP_USE_QOS - /* Create QoS fifo notification queue */ - qfifo_events = csp_queue_create(CSP_FIFO_INPUT, sizeof(int)); - if (!qfifo_events) - return CSP_ERR_NOMEM; -#endif - - return CSP_ERR_NONE; - -} - -int csp_qfifo_read(csp_qfifo_t * input) { - -#ifdef CSP_USE_QOS - int prio, found, event; - - /* Wait for packet in any queue */ - if (csp_queue_dequeue(qfifo_events, &event, FIFO_TIMEOUT) != CSP_QUEUE_OK) - return CSP_ERR_TIMEDOUT; - - /* Find packet with highest priority */ - found = 0; - for (prio = 0; prio < CSP_ROUTE_FIFOS; prio++) { - if (csp_queue_dequeue(qfifo[prio], input, 0) == CSP_QUEUE_OK) { - found = 1; - break; - } - } - - if (!found) { - csp_log_warn("Spurious wakeup: No packet found"); - return CSP_ERR_TIMEDOUT; - } -#else - if (csp_queue_dequeue(qfifo[0], input, FIFO_TIMEOUT) != CSP_QUEUE_OK) - return CSP_ERR_TIMEDOUT; -#endif - - return CSP_ERR_NONE; - -} - -void csp_qfifo_write(csp_packet_t * packet, csp_iface_t * interface, CSP_BASE_TYPE * pxTaskWoken) { - - int result; - - if (packet == NULL) { - if (pxTaskWoken == NULL) { // Only do logging in non-ISR context - csp_log_warn("csp_new packet called with NULL packet"); - } - return; - } else if (interface == NULL) { - if (pxTaskWoken == NULL) { // Only do logging in non-ISR context - csp_log_warn("csp_new packet called with NULL interface"); - } - if (pxTaskWoken == NULL) - csp_buffer_free(packet); - else - csp_buffer_free_isr(packet); - return; - } - - csp_qfifo_t queue_element; - queue_element.interface = interface; - queue_element.packet = packet; - -#ifdef CSP_USE_QOS - int fifo = packet->id.pri; -#else - int fifo = 0; -#endif - - if (pxTaskWoken == NULL) - result = csp_queue_enqueue(qfifo[fifo], &queue_element, 0); - else - result = csp_queue_enqueue_isr(qfifo[fifo], &queue_element, pxTaskWoken); - -#ifdef CSP_USE_QOS - static int event = 0; - - if (result == CSP_QUEUE_OK) { - if (pxTaskWoken == NULL) - csp_queue_enqueue(qfifo_events, &event, 0); - else - csp_queue_enqueue_isr(qfifo_events, &event, pxTaskWoken); - } -#endif - - if (result != CSP_QUEUE_OK) { - if (pxTaskWoken == NULL) { // Only do logging in non-ISR context - csp_log_warn("ERROR: Routing input FIFO is FULL. Dropping packet."); - } - interface->drop++; - if (pxTaskWoken == NULL) - csp_buffer_free(packet); - else - csp_buffer_free_isr(packet); - } - -} - -void csp_qfifo_wake_up(void) { - csp_qfifo_t queue_element; - queue_element.interface = NULL; - queue_element.packet = NULL; - csp_queue_enqueue(qfifo[0], &queue_element, 0); -} diff --git a/thirdparty/libcsp/src/csp_qfifo.h b/thirdparty/libcsp/src/csp_qfifo.h deleted file mode 100644 index 2910c48d..00000000 --- a/thirdparty/libcsp/src/csp_qfifo.h +++ /dev/null @@ -1,54 +0,0 @@ -/* -Cubesat Space Protocol - A small network-layer protocol designed for Cubesats -Copyright (C) 2012 GomSpace ApS (http://www.gomspace.com) -Copyright (C) 2012 AAUSAT3 Project (http://aausat3.space.aau.dk) - -This library is free software; you can redistribute it and/or -modify it under the terms of the GNU Lesser General Public -License as published by the Free Software Foundation; either -version 2.1 of the License, or (at your option) any later version. - -This library is distributed in the hope that it will be useful, -but WITHOUT ANY WARRANTY; without even the implied warranty of -MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU -Lesser General Public License for more details. - -You should have received a copy of the GNU Lesser General Public -License along with this library; if not, write to the Free Software -Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA -*/ - -#ifndef CSP_QFIFO_H_ -#define CSP_QFIFO_H_ - -#ifdef CSP_USE_RDP -#define FIFO_TIMEOUT 100 //! If RDP is enabled, the router needs to awake some times to check timeouts -#else -#define FIFO_TIMEOUT CSP_MAX_DELAY //! If no RDP, the router can sleep untill data arrives -#endif - -/** - * Init FIFO/QOS queues - * @return CSP_ERR type - */ -int csp_qfifo_init(void); - -typedef struct { - csp_iface_t * interface; - csp_packet_t * packet; -} csp_qfifo_t; - -/** - * Read next packet from router input queue - * @param input pointer to router queue item element - * @return CSP_ERR type - */ -int csp_qfifo_read(csp_qfifo_t * input); - -/** - * Wake up any task (e.g. router) waiting on messages. - * For testing. - */ -void csp_qfifo_wake_up(void); - -#endif /* CSP_QFIFO_H_ */ diff --git a/thirdparty/libcsp/src/csp_route.c b/thirdparty/libcsp/src/csp_route.c deleted file mode 100644 index bc843577..00000000 --- a/thirdparty/libcsp/src/csp_route.c +++ /dev/null @@ -1,346 +0,0 @@ -/* -Cubesat Space Protocol - A small network-layer protocol designed for Cubesats -Copyright (C) 2012 GomSpace ApS (http://www.gomspace.com) -Copyright (C) 2012 AAUSAT3 Project (http://aausat3.space.aau.dk) - -This library is free software; you can redistribute it and/or -modify it under the terms of the GNU Lesser General Public -License as published by the Free Software Foundation; either -version 2.1 of the License, or (at your option) any later version. - -This library is distributed in the hope that it will be useful, -but WITHOUT ANY WARRANTY; without even the implied warranty of -MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU -Lesser General Public License for more details. - -You should have received a copy of the GNU Lesser General Public -License along with this library; if not, write to the Free Software -Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA -*/ - -#include -#include -#include - -/* CSP includes */ -#include -#include -#include - -#include -#include - -#include -#include - -#include "csp_port.h" -#include "csp_conn.h" -#include "csp_io.h" -#include "csp_promisc.h" -#include "csp_qfifo.h" -#include "csp_dedup.h" -#include "transport/csp_transport.h" - -/** - * Check supported packet options - * @param interface pointer to incoming interface - * @param packet pointer to packet - * @return CSP_ERR_NONE is all options are supported, CSP_ERR_NOTSUP if not - */ -static int csp_route_check_options(csp_iface_t *interface, csp_packet_t *packet) -{ -#ifndef CSP_USE_XTEA - /* Drop XTEA packets */ - if (packet->id.flags & CSP_FXTEA) { - csp_log_error("Received XTEA encrypted packet, but CSP was compiled without XTEA support. Discarding packet"); - interface->autherr++; - return CSP_ERR_NOTSUP; - } -#endif - -#ifndef CSP_USE_HMAC - /* Drop HMAC packets */ - if (packet->id.flags & CSP_FHMAC) { - csp_log_error("Received packet with HMAC, but CSP was compiled without HMAC support. Discarding packet"); - interface->autherr++; - return CSP_ERR_NOTSUP; - } -#endif - -#ifndef CSP_USE_RDP - /* Drop RDP packets */ - if (packet->id.flags & CSP_FRDP) { - csp_log_error("Received RDP packet, but CSP was compiled without RDP support. Discarding packet"); - interface->rx_error++; - return CSP_ERR_NOTSUP; - } -#endif - return CSP_ERR_NONE; -} - -/** - * Helper function to decrypt, check auth and CRC32 - * @param security_opts either socket_opts or conn_opts - * @param interface pointer to incoming interface - * @param packet pointer to packet - * @return -1 Missing feature, -2 XTEA error, -3 CRC error, -4 HMAC error, 0 = OK. - */ -static int csp_route_security_check(uint32_t security_opts, csp_iface_t * interface, csp_packet_t * packet) { - -#ifdef CSP_USE_XTEA - /* XTEA encrypted packet */ - if (packet->id.flags & CSP_FXTEA) { - /* Read nonce */ - uint32_t nonce; - memcpy(&nonce, &packet->data[packet->length - sizeof(nonce)], sizeof(nonce)); - nonce = csp_ntoh32(nonce); - packet->length -= sizeof(nonce); - - /* Create initialization vector */ - uint32_t iv[2] = {nonce, 1}; - - /* Decrypt data */ - if (csp_xtea_decrypt(packet->data, packet->length, iv) != 0) { - /* Decryption failed */ - csp_log_error("Decryption failed! Discarding packet"); - interface->autherr++; - return CSP_ERR_XTEA; - } - } else if (security_opts & CSP_SO_XTEAREQ) { - csp_log_warn("Received packet without XTEA encryption. Discarding packet"); - interface->autherr++; - return CSP_ERR_XTEA; - } -#endif - - /* CRC32 verified packet */ - if (packet->id.flags & CSP_FCRC32) { -#ifdef CSP_USE_CRC32 - if (packet->length < 4) - csp_log_error("Too short packet for CRC32, %u", packet->length); - /* Verify CRC32 (does not include header for backwards compatability with csp1.x) */ - if (csp_crc32_verify(packet, false) != 0) { - /* Checksum failed */ - csp_log_error("CRC32 verification error! Discarding packet"); - interface->rx_error++; - return CSP_ERR_CRC32; - } - } else if (security_opts & CSP_SO_CRC32REQ) { - csp_log_warn("Received packet without CRC32. Accepting packet"); -#else - /* Strip CRC32 field and accept the packet */ - csp_log_warn("Received packet with CRC32, but CSP was compiled without CRC32 support. Accepting packet"); - packet->length -= sizeof(uint32_t); -#endif - } - -#ifdef CSP_USE_HMAC - /* HMAC authenticated packet */ - if (packet->id.flags & CSP_FHMAC) { - /* Verify HMAC (does not include header for backwards compatability with csp1.x) */ - if (csp_hmac_verify(packet, false) != 0) { - /* HMAC failed */ - csp_log_error("HMAC verification error! Discarding packet"); - interface->autherr++; - return CSP_ERR_HMAC; - } - } else if (security_opts & CSP_SO_HMACREQ) { - csp_log_warn("Received packet without HMAC. Discarding packet"); - interface->autherr++; - return CSP_ERR_HMAC; - } -#endif - -#ifdef CSP_USE_RDP - /* RDP packet */ - if (!(packet->id.flags & CSP_FRDP)) { - if (security_opts & CSP_SO_RDPREQ) { - csp_log_warn("Received packet without RDP header. Discarding packet"); - interface->rx_error++; - return CSP_ERR_INVAL; - } - } -#endif - - return CSP_ERR_NONE; - -} - -int csp_route_work(uint32_t timeout) { - - csp_qfifo_t input; - csp_packet_t * packet; - csp_conn_t * conn; - csp_socket_t * socket; - -#ifdef CSP_USE_RDP - /* Check connection timeouts (currently only for RDP) */ - csp_conn_check_timeouts(); -#endif - - /* Get next packet to route */ - if (csp_qfifo_read(&input) != CSP_ERR_NONE) - return -1; - - packet = input.packet; - if (!packet) - return -1; - - csp_log_packet("INP: S %u, D %u, Dp %u, Sp %u, Pr %u, Fl 0x%02X, Sz %"PRIu16" VIA: %s", - packet->id.src, packet->id.dst, packet->id.dport, - packet->id.sport, packet->id.pri, packet->id.flags, packet->length, input.interface->name); - - /* Here there be promiscuous mode */ -#ifdef CSP_USE_PROMISC - csp_promisc_add(packet); -#endif - -#ifdef CSP_USE_DEDUP - /* Check for duplicates */ - if (csp_dedup_is_duplicate(packet)) { - /* Discard packet */ - csp_log_packet("Duplicate packet discarded"); - input.interface->drop++; - csp_buffer_free(packet); - return 0; - } -#endif - - /* Now we count the message (since its deduplicated) */ - input.interface->rx++; - input.interface->rxbytes += packet->length; - - /* If the message is not to me, route the message to the correct interface */ - if ((packet->id.dst != csp_get_address()) && (packet->id.dst != CSP_BROADCAST_ADDR)) { - - /* Find the destination interface */ - csp_iface_t * dstif = csp_rtable_find_iface(packet->id.dst); - - /* If the message resolves to the input interface, don't loop it back out */ - if ((dstif == NULL) || ((dstif == input.interface) && (input.interface->split_horizon_off == 0))) { - csp_buffer_free(packet); - return 0; - } - - /* Otherwise, actually send the message */ - if (csp_send_direct(packet->id, packet, dstif, 0) != CSP_ERR_NONE) { - csp_log_warn("Router failed to send"); - csp_buffer_free(packet); - } - - /* Next message, please */ - return 0; - } - - /* Discard packets with unsupported options */ - if (csp_route_check_options(input.interface, packet) != CSP_ERR_NONE) { - csp_buffer_free(packet); - return 0; - } - - /* The message is to me, search for incoming socket */ - socket = csp_port_get_socket(packet->id.dport); - - /* If the socket is connection-less, deliver now */ - if (socket && (socket->opts & CSP_SO_CONN_LESS)) { - if (csp_route_security_check(socket->opts, input.interface, packet) < 0) { - csp_buffer_free(packet); - return 0; - } - if (csp_queue_enqueue(socket->socket, &packet, 0) != CSP_QUEUE_OK) { - csp_log_error("Conn-less socket queue full"); - csp_buffer_free(packet); - return 0; - } - return 0; - } - - /* Search for an existing connection */ - conn = csp_conn_find(packet->id.ext, CSP_ID_CONN_MASK); - - /* If this is an incoming packet on a new connection */ - if (conn == NULL) { - - /* Reject packet if no matching socket is found */ - if (!socket) { - csp_buffer_free(packet); - return 0; - } - - /* Run security check on incoming packet */ - if (csp_route_security_check(socket->opts, input.interface, packet) < 0) { - csp_buffer_free(packet); - return 0; - } - - /* New incoming connection accepted */ - csp_id_t idout; - idout.pri = packet->id.pri; - idout.src = csp_get_address(); - - idout.dst = packet->id.src; - idout.dport = packet->id.sport; - idout.sport = packet->id.dport; - idout.flags = packet->id.flags; - - /* Create connection */ - conn = csp_conn_new(packet->id, idout); - - if (!conn) { - csp_log_error("No more connections available"); - csp_buffer_free(packet); - return 0; - } - - /* Store the socket queue and options */ - conn->socket = socket->socket; - conn->opts = socket->opts; - - /* Packet to existing connection */ - } else { - - /* Run security check on incoming packet */ - if (csp_route_security_check(conn->opts, input.interface, packet) < 0) { - csp_buffer_free(packet); - return 0; - } - - } - -#ifdef CSP_USE_RDP - /* Pass packet to RDP module */ - if (packet->id.flags & CSP_FRDP) { - csp_rdp_new_packet(conn, packet); - return 0; - } -#endif - - /* Pass packet to UDP module */ - csp_udp_new_packet(conn, packet); - return 0; -} - -static CSP_DEFINE_TASK(csp_task_router) { - - /* Here there be routing */ - while (1) { - csp_route_work(FIFO_TIMEOUT); - } - - return CSP_TASK_RETURN; - -} - -int csp_route_start_task(unsigned int task_stack_size, unsigned int priority) { - - static csp_thread_handle_t handle_router; - int ret = csp_thread_create(csp_task_router, "RTE", task_stack_size, NULL, priority, &handle_router); - - if (ret != 0) { - csp_log_error("Failed to start router task"); - return CSP_ERR_NOMEM; - } - - return CSP_ERR_NONE; - -} diff --git a/thirdparty/libcsp/src/csp_route.h b/thirdparty/libcsp/src/csp_route.h deleted file mode 100644 index 2a20f49f..00000000 --- a/thirdparty/libcsp/src/csp_route.h +++ /dev/null @@ -1,24 +0,0 @@ -/* -Cubesat Space Protocol - A small network-layer protocol designed for Cubesats -Copyright (C) 2012 GomSpace ApS (http://www.gomspace.com) -Copyright (C) 2012 AAUSAT3 Project (http://aausat3.space.aau.dk) - -This library is free software; you can redistribute it and/or -modify it under the terms of the GNU Lesser General Public -License as published by the Free Software Foundation; either -version 2.1 of the License, or (at your option) any later version. - -This library is distributed in the hope that it will be useful, -but WITHOUT ANY WARRANTY; without even the implied warranty of -MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU -Lesser General Public License for more details. - -You should have received a copy of the GNU Lesser General Public -License along with this library; if not, write to the Free Software -Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA -*/ - -#ifndef _CSP_ROUTE_H_ -#define _CSP_ROUTE_H_ - -#endif // _CSP_ROUTE_H_ diff --git a/thirdparty/libcsp/src/csp_service_handler.c b/thirdparty/libcsp/src/csp_service_handler.c deleted file mode 100644 index 0090afc1..00000000 --- a/thirdparty/libcsp/src/csp_service_handler.c +++ /dev/null @@ -1,334 +0,0 @@ -/* -Cubesat Space Protocol - A small network-layer protocol designed for Cubesats -Copyright (C) 2012 GomSpace ApS (http://www.gomspace.com) -Copyright (C) 2012 AAUSAT3 Project (http://aausat3.space.aau.dk) - -This library is free software; you can redistribute it and/or -modify it under the terms of the GNU Lesser General Public -License as published by the Free Software Foundation; either -version 2.1 of the License, or (at your option) any later version. - -This library is distributed in the hope that it will be useful, -but WITHOUT ANY WARRANTY; without even the implied warranty of -MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU -Lesser General Public License for more details. - -You should have received a copy of the GNU Lesser General Public -License along with this library; if not, write to the Free Software -Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA -*/ - -#include -#include -#include - -/* CSP includes */ -#include -#include -#include -#include -#include - -#include -#include -#include -#include -#include "csp_route.h" - -#define CSP_RPS_MTU 196 - -/** - * The CSP CMP mempy function is used to, override the function used to - * read/write memory by peek and poke. - */ -#ifdef __AVR__ -static uint32_t wrap_32bit_memcpy(uint32_t to, const uint32_t from, size_t size) { - return (uint32_t) (uintptr_t) memcpy((void *) (uintptr_t) to, (const void *) (uintptr_t) from, size); -} -static csp_memcpy_fnc_t csp_cmp_memcpy_fnc = wrap_32bit_memcpy; -#else -static csp_memcpy_fnc_t csp_cmp_memcpy_fnc = (csp_memcpy_fnc_t) memcpy; -#endif - - -void csp_cmp_set_memcpy(csp_memcpy_fnc_t fnc) { - csp_cmp_memcpy_fnc = fnc; -} - -static int do_cmp_ident(struct csp_cmp_message *cmp) { - - /* Copy revision */ - strncpy(cmp->ident.revision, csp_get_revision(), CSP_CMP_IDENT_REV_LEN); - cmp->ident.revision[CSP_CMP_IDENT_REV_LEN - 1] = '\0'; - - /* Copy compilation date */ - strncpy(cmp->ident.date, __DATE__, CSP_CMP_IDENT_DATE_LEN); - cmp->ident.date[CSP_CMP_IDENT_DATE_LEN - 1] = '\0'; - - /* Copy compilation time */ - strncpy(cmp->ident.time, __TIME__, CSP_CMP_IDENT_TIME_LEN); - cmp->ident.time[CSP_CMP_IDENT_TIME_LEN - 1] = '\0'; - - /* Copy hostname */ - strncpy(cmp->ident.hostname, csp_get_hostname(), CSP_HOSTNAME_LEN); - cmp->ident.hostname[CSP_HOSTNAME_LEN - 1] = '\0'; - - /* Copy model name */ - strncpy(cmp->ident.model, csp_get_model(), CSP_MODEL_LEN); - cmp->ident.model[CSP_MODEL_LEN - 1] = '\0'; - - return CSP_ERR_NONE; - -} - -static int do_cmp_route_set(struct csp_cmp_message *cmp) { - - csp_iface_t *ifc = csp_iflist_get_by_name(cmp->route_set.interface); - if (ifc == NULL) - return CSP_ERR_INVAL; - - if (csp_route_set(cmp->route_set.dest_node, ifc, cmp->route_set.next_hop_mac) != CSP_ERR_NONE) - return CSP_ERR_INVAL; - - return CSP_ERR_NONE; - -} - -static int do_cmp_if_stats(struct csp_cmp_message *cmp) { - - csp_iface_t *ifc = csp_iflist_get_by_name(cmp->if_stats.interface); - if (ifc == NULL) - return CSP_ERR_INVAL; - - cmp->if_stats.tx = csp_hton32(ifc->tx); - cmp->if_stats.rx = csp_hton32(ifc->rx); - cmp->if_stats.tx_error = csp_hton32(ifc->tx_error); - cmp->if_stats.rx_error = csp_hton32(ifc->rx_error); - cmp->if_stats.drop = csp_hton32(ifc->drop); - cmp->if_stats.autherr = csp_hton32(ifc->autherr); - cmp->if_stats.frame = csp_hton32(ifc->frame); - cmp->if_stats.txbytes = csp_hton32(ifc->txbytes); - cmp->if_stats.rxbytes = csp_hton32(ifc->rxbytes); - cmp->if_stats.irq = csp_hton32(ifc->irq); - - return CSP_ERR_NONE; -} - -static int do_cmp_peek(struct csp_cmp_message *cmp) { - - cmp->peek.addr = csp_hton32(cmp->peek.addr); - if (cmp->peek.len > CSP_CMP_PEEK_MAX_LEN) - return CSP_ERR_INVAL; - - /* Dangerous, you better know what you are doing */ - csp_cmp_memcpy_fnc((csp_memptr_t) (uintptr_t) cmp->peek.data, (csp_memptr_t) (unsigned long) cmp->peek.addr, cmp->peek.len); - - return CSP_ERR_NONE; - -} - -static int do_cmp_poke(struct csp_cmp_message *cmp) { - - cmp->poke.addr = csp_hton32(cmp->poke.addr); - if (cmp->poke.len > CSP_CMP_POKE_MAX_LEN) - return CSP_ERR_INVAL; - - /* Extremely dangerous, you better know what you are doing */ - csp_cmp_memcpy_fnc((csp_memptr_t) (unsigned long) cmp->poke.addr, (csp_memptr_t) (uintptr_t) cmp->poke.data, cmp->poke.len); - - return CSP_ERR_NONE; - -} - -static int do_cmp_clock(struct csp_cmp_message *cmp) { - - cmp->clock.tv_sec = csp_ntoh32(cmp->clock.tv_sec); - cmp->clock.tv_nsec = csp_ntoh32(cmp->clock.tv_nsec); - - if ((cmp->clock.tv_sec != 0) && (clock_set_time != NULL)) { - clock_set_time(&cmp->clock); - } - - if (clock_get_time != NULL) { - clock_get_time(&cmp->clock); - } - - cmp->clock.tv_sec = csp_hton32(cmp->clock.tv_sec); - cmp->clock.tv_nsec = csp_hton32(cmp->clock.tv_nsec); - return CSP_ERR_NONE; - -} - -/* CSP Management Protocol handler */ -static int csp_cmp_handler(csp_conn_t * conn, csp_packet_t * packet) { - - int ret = CSP_ERR_INVAL; - struct csp_cmp_message * cmp = (struct csp_cmp_message *) packet->data; - - /* Ignore everything but requests */ - if (cmp->type != CSP_CMP_REQUEST) - return ret; - - switch (cmp->code) { - case CSP_CMP_IDENT: - ret = do_cmp_ident(cmp); - packet->length = CMP_SIZE(ident); - break; - - case CSP_CMP_ROUTE_SET: - ret = do_cmp_route_set(cmp); - packet->length = CMP_SIZE(route_set); - break; - - case CSP_CMP_IF_STATS: - ret = do_cmp_if_stats(cmp); - packet->length = CMP_SIZE(if_stats); - break; - - case CSP_CMP_PEEK: - ret = do_cmp_peek(cmp); - break; - - case CSP_CMP_POKE: - ret = do_cmp_poke(cmp); - break; - - case CSP_CMP_CLOCK: - ret = do_cmp_clock(cmp); - break; - - default: - ret = CSP_ERR_INVAL; - break; - } - - cmp->type = CSP_CMP_REPLY; - - return ret; -} - -void csp_service_handler(csp_conn_t * conn, csp_packet_t * packet) { - - switch (csp_conn_dport(conn)) { - - case CSP_CMP: - /* Pass to CMP handler */ - if (csp_cmp_handler(conn, packet) != CSP_ERR_NONE) { - csp_buffer_free(packet); - return; - } - break; - - case CSP_PING: - /* A ping means, just echo the packet, so no changes */ - csp_log_info("SERVICE: Ping received"); - break; - - case CSP_PS: { - /* Sanity check on request */ - if ((packet->length != 1) || (packet->data[0] != 0x55)) { - /* Sanity check failed */ - csp_buffer_free(packet); - /* Clear the packet, it has been freed */ - packet = NULL; - break; - } - /* Start by allocating just the right amount of memory */ - int task_list_size = csp_sys_tasklist_size(); - char * pslist = csp_malloc(task_list_size); - /* Check for malloc fail */ - if (pslist == NULL) { - /* Send out the data */ - strcpy((char *)packet->data, "Not enough memory"); - packet->length = strlen((char *)packet->data); - /* Break and let the default handling send packet */ - break; - } - - /* Retrieve the tasklist */ - csp_sys_tasklist(pslist); - int pslen = strnlen(pslist, task_list_size); - - /* Split the potentially very long string into packets */ - int i = 0; - while(i < pslen) { - - /* Allocate packet buffer, if need be */ - if (packet == NULL) - packet = csp_buffer_get(CSP_RPS_MTU); - if (packet == NULL) - break; - - /* Calculate length, either full MTU or the remainder */ - packet->length = (pslen - i > CSP_RPS_MTU) ? CSP_RPS_MTU : (pslen - i); - - /* Send out the data */ - memcpy(packet->data, &pslist[i], packet->length); - i += packet->length; - if (!csp_send(conn, packet, 0)) - csp_buffer_free(packet); - - /* Clear the packet reference when sent */ - packet = NULL; - - } - csp_free(pslist); - break; - } - - case CSP_MEMFREE: { - uint32_t total = csp_sys_memfree(); - - total = csp_hton32(total); - memcpy(packet->data, &total, sizeof(total)); - packet->length = sizeof(total); - - break; - } - - case CSP_REBOOT: { - uint32_t magic_word; - memcpy(&magic_word, packet->data, sizeof(magic_word)); - - magic_word = csp_ntoh32(magic_word); - - /* If the magic word is valid, reboot */ - if (magic_word == CSP_REBOOT_MAGIC) { - csp_sys_reboot(); - } else if (magic_word == CSP_REBOOT_SHUTDOWN_MAGIC) { - csp_sys_shutdown(); - } - - - - csp_buffer_free(packet); - return; - } - - case CSP_BUF_FREE: { - uint32_t size = csp_buffer_remaining(); - size = csp_hton32(size); - memcpy(packet->data, &size, sizeof(size)); - packet->length = sizeof(size); - break; - } - - case CSP_UPTIME: { - uint32_t time = csp_get_s(); - time = csp_hton32(time); - memcpy(packet->data, &time, sizeof(time)); - packet->length = sizeof(time); - break; - } - - default: - csp_buffer_free(packet); - return; - } - - if (packet != NULL) { - if (!csp_send(conn, packet, 0)) - csp_buffer_free(packet); - } - -} diff --git a/thirdparty/libcsp/src/csp_services.c b/thirdparty/libcsp/src/csp_services.c deleted file mode 100644 index 5392cb82..00000000 --- a/thirdparty/libcsp/src/csp_services.c +++ /dev/null @@ -1,233 +0,0 @@ -/* -Cubesat Space Protocol - A small network-layer protocol designed for Cubesats -Copyright (C) 2012 GomSpace ApS (http://www.gomspace.com) -Copyright (C) 2012 AAUSAT3 Project (http://aausat3.space.aau.dk) - -This library is free software; you can redistribute it and/or -modify it under the terms of the GNU Lesser General Public -License as published by the Free Software Foundation; either -version 2.1 of the License, or (at your option) any later version. - -This library is distributed in the hope that it will be useful, -but WITHOUT ANY WARRANTY; without even the implied warranty of -MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU -Lesser General Public License for more details. - -You should have received a copy of the GNU Lesser General Public -License along with this library; if not, write to the Free Software -Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA -*/ - -#include -#include -#include -#include - -/* CSP includes */ -#include -#include -#include - -#include - -int csp_ping(uint8_t node, uint32_t timeout, unsigned int size, uint8_t conn_options) { - - unsigned int i; - uint32_t start, time, status = 0; - - /* Counter */ - start = csp_get_ms(); - - /* Open connection */ - csp_conn_t * conn = csp_connect(CSP_PRIO_NORM, node, CSP_PING, timeout, conn_options); - if (conn == NULL) - return -1; - - /* Prepare data */ - csp_packet_t * packet; - packet = csp_buffer_get(size); - if (packet == NULL) - goto out; - - /* Set data to increasing numbers */ - packet->length = size; - for (i = 0; i < size; i++) - packet->data[i] = i; - - /* Try to send frame */ - if (!csp_send(conn, packet, 0)) - goto out; - - /* Read incoming frame */ - packet = csp_read(conn, timeout); - if (packet == NULL) - goto out; - - /* Ensure that the data was actually echoed */ - for (i = 0; i < size; i++) - if (packet->data[i] != i % (0xff + 1)) - goto out; - - status = 1; - -out: - /* Clean up */ - if (packet != NULL) - csp_buffer_free(packet); - csp_close(conn); - - /* We have a reply */ - time = (csp_get_ms() - start); - - if (status) { - return time; - } else { - return -1; - } - -} - -void csp_ping_noreply(uint8_t node) { - - /* Prepare data */ - csp_packet_t * packet; - packet = csp_buffer_get(1); - - /* Check malloc */ - if (packet == NULL) - return; - - /* Open connection */ - csp_conn_t * conn = csp_connect(CSP_PRIO_NORM, node, CSP_PING, 0, 0); - if (conn == NULL) { - csp_buffer_free(packet); - return; - } - - packet->data[0] = 0x55; - packet->length = 1; - - printf("Ping ignore reply node %u.\r\n", (unsigned int) node); - - /* Try to send frame */ - if (!csp_send(conn, packet, 0)) - csp_buffer_free(packet); - - csp_close(conn); - -} - -void csp_reboot(uint8_t node) { - uint32_t magic_word = csp_hton32(CSP_REBOOT_MAGIC); - csp_transaction(CSP_PRIO_NORM, node, CSP_REBOOT, 0, &magic_word, sizeof(magic_word), NULL, 0); -} - -void csp_shutdown(uint8_t node) { - uint32_t magic_word = csp_hton32(CSP_REBOOT_SHUTDOWN_MAGIC); - csp_transaction(CSP_PRIO_NORM, node, CSP_REBOOT, 0, &magic_word, sizeof(magic_word), NULL, 0); -} - -void csp_ps(uint8_t node, uint32_t timeout) { - - /* Open connection */ - csp_conn_t * conn = csp_connect(CSP_PRIO_NORM, node, CSP_PS, 0, 0); - if (conn == NULL) - return; - - /* Prepare data */ - csp_packet_t * packet; - packet = csp_buffer_get(95); - - /* Check malloc */ - if (packet == NULL) - goto out; - - packet->data[0] = 0x55; - packet->length = 1; - - printf("PS node %u: \r\n", (unsigned int) node); - - /* Try to send frame */ - if (!csp_send(conn, packet, 0)) - goto out; - - while(1) { - - /* Read incoming frame */ - packet = csp_read(conn, timeout); - if (packet == NULL) - break; - - /* We have a reply, add our own NULL char */ - packet->data[packet->length] = 0; - printf("%s", packet->data); - - /* Each packet from csp_read must to be freed by user */ - csp_buffer_free(packet); - } - - printf("\r\n"); - - /* Clean up */ -out: - if (packet != NULL) - csp_buffer_free(packet); - csp_close(conn); - -} - -void csp_memfree(uint8_t node, uint32_t timeout) { - - uint32_t memfree; - - int status = csp_transaction(CSP_PRIO_NORM, node, CSP_MEMFREE, timeout, NULL, 0, &memfree, sizeof(memfree)); - if (status == 0) { - printf("Network error\r\n"); - return; - } - - /* Convert from network to host order */ - memfree = csp_ntoh32(memfree); - - printf("Free Memory at node %u is %"PRIu32" bytes\r\n", (unsigned int) node, memfree); - -} - -void csp_buf_free(uint8_t node, uint32_t timeout) { - - uint32_t size = 0; - - int status = csp_transaction(CSP_PRIO_NORM, node, CSP_BUF_FREE, timeout, NULL, 0, &size, sizeof(size)); - if (status == 0) { - printf("Network error\r\n"); - return; - } - size = csp_ntoh32(size); - printf("Free buffers at node %u is %"PRIu32"\r\n", (unsigned int) node, size); - -} - -void csp_uptime(uint8_t node, uint32_t timeout) { - - uint32_t uptime = 0; - - int status = csp_transaction(CSP_PRIO_NORM, node, CSP_UPTIME, timeout, NULL, 0, &uptime, sizeof(uptime)); - if (status == 0) { - printf("Network error\r\n"); - return; - } - uptime = csp_ntoh32(uptime); - printf("Uptime of node %u is %"PRIu32" s\r\n", (unsigned int) node, uptime); - -} - -int csp_cmp(uint8_t node, uint32_t timeout, uint8_t code, int membsize, struct csp_cmp_message * msg) { - msg->type = CSP_CMP_REQUEST; - msg->code = code; - int status = csp_transaction(CSP_PRIO_NORM, node, CSP_CMP, timeout, msg, membsize, msg, membsize); - if (status == 0) - return CSP_ERR_TIMEDOUT; - - return CSP_ERR_NONE; -} - diff --git a/thirdparty/libcsp/src/csp_sfp.c b/thirdparty/libcsp/src/csp_sfp.c deleted file mode 100644 index 96ef36e1..00000000 --- a/thirdparty/libcsp/src/csp_sfp.c +++ /dev/null @@ -1,170 +0,0 @@ -/* -Cubesat Space Protocol - A small network-layer protocol designed for Cubesats -Copyright (C) 2012 GomSpace ApS (http://www.gomspace.com) -Copyright (C) 2012 AAUSAT3 Project (http://aausat3.space.aau.dk) - -This library is free software; you can redistribute it and/or -modify it under the terms of the GNU Lesser General Public -License as published by the Free Software Foundation; either -version 2.1 of the License, or (at your option) any later version. - -This library is distributed in the hope that it will be useful, -but WITHOUT ANY WARRANTY; without even the implied warranty of -MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU -Lesser General Public License for more details. - -You should have received a copy of the GNU Lesser General Public -License along with this library; if not, write to the Free Software -Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA -*/ - -#include -#include -#include -#include -#include -#include "csp_conn.h" - -typedef struct __attribute__((__packed__)) { - uint32_t offset; - uint32_t totalsize; -} sfp_header_t; - -/** - * SFP Headers: - * The following functions are helper functions that handles the extra SFP - * information that needs to be appended to all data packets. - */ -static sfp_header_t * csp_sfp_header_add(csp_packet_t * packet) { - sfp_header_t * header = (sfp_header_t *) &packet->data[packet->length]; - packet->length += sizeof(sfp_header_t); - memset(header, 0, sizeof(sfp_header_t)); - return header; -} - -static sfp_header_t * csp_sfp_header_remove(csp_packet_t * packet) { - sfp_header_t * header = (sfp_header_t *) &packet->data[packet->length-sizeof(sfp_header_t)]; - packet->length -= sizeof(sfp_header_t); - return header; -} - -int csp_sfp_send_own_memcpy(csp_conn_t * conn, const void * data, int totalsize, int mtu, uint32_t timeout, void * (*memcpyfcn)(void *, const void *, size_t)) { - - int count = 0; - while(count < totalsize) { - - /* Allocate packet */ - csp_packet_t * packet = csp_buffer_get(mtu); - if (packet == NULL) - return -1; - - /* Calculate sending size */ - int size = totalsize - count; - if (size > mtu) - size = mtu; - - /* Print debug */ - csp_debug(CSP_PROTOCOL, "Sending SFP at %x size %u", data + count, size); - - /* Copy data */ - (*memcpyfcn)(packet->data, data + count, size); - packet->length = size; - - /* Set fragment flag */ - conn->idout.flags |= CSP_FFRAG; - - /* Add SFP header */ - sfp_header_t * sfp_header = csp_sfp_header_add(packet); - sfp_header->totalsize = csp_hton32(totalsize); - sfp_header->offset = csp_hton32(count); - - /* Send data */ - if (!csp_send(conn, packet, timeout)) { - csp_buffer_free(packet); - return -1; - } - - /* Increment count */ - count += size; - - } - - return 0; - -} - -int csp_sfp_send(csp_conn_t * conn, const void * data, int totalsize, int mtu, uint32_t timeout) { - return csp_sfp_send_own_memcpy(conn, data, totalsize, mtu, timeout, &memcpy); -} - -int csp_sfp_recv_fp(csp_conn_t * conn, void ** dataout, int * datasize, uint32_t timeout, csp_packet_t * first_packet) { - - unsigned int last_byte = 0; - - *dataout = NULL; /* Allow caller to assume csp_free() can always be called when dataout is non-NULL */ - - /* Get first packet from user, or from connection */ - csp_packet_t * packet = NULL; - if (first_packet == NULL) { - packet = csp_read(conn, timeout); - if (packet == NULL) - return -1; - } else { - packet = first_packet; - } - - do { - - /* Check that SFP header is present */ - if ((packet->id.flags & CSP_FFRAG) == 0) { - csp_debug(CSP_ERROR, "Missing SFP header"); - csp_buffer_free(packet); - return -1; - } - - /* Read SFP header */ - sfp_header_t * sfp_header = csp_sfp_header_remove(packet); - sfp_header->offset = csp_ntoh32(sfp_header->offset); - sfp_header->totalsize = csp_ntoh32(sfp_header->totalsize); - - csp_debug(CSP_PROTOCOL, "SFP fragment %u/%u", sfp_header->offset + packet->length, sfp_header->totalsize); - - if (sfp_header->offset > last_byte + 1) { - csp_debug(CSP_ERROR, "SFP missing %u bytes", sfp_header->offset - last_byte); - csp_buffer_free(packet); - return -1; - } else { - last_byte = sfp_header->offset + packet->length; - } - - /* Allocate memory */ - if (*dataout == NULL) - *dataout = csp_malloc(sfp_header->totalsize); - if (*dataout == NULL) { - csp_debug(CSP_ERROR, "No dyn-memory for SFP fragment"); - csp_buffer_free(packet); - return -1; - } - - /* Copy data to output */ - *datasize = sfp_header->totalsize; - memcpy(*dataout + sfp_header->offset, packet->data, packet->length); - - if (sfp_header->offset + packet->length >= sfp_header->totalsize) { - csp_debug(CSP_PROTOCOL, "SFP complete"); - csp_buffer_free(packet); - return 0; - } else { - csp_buffer_free(packet); - } - - } while((packet = csp_read(conn, timeout)) != NULL); - - return -1; - -} - -int csp_sfp_recv(csp_conn_t * conn, void ** dataout, int * datasize, uint32_t timeout) { - return csp_sfp_recv_fp(conn, dataout, datasize, timeout, NULL); -} - diff --git a/thirdparty/libcsp/src/drivers/CMakeLists.txt b/thirdparty/libcsp/src/drivers/CMakeLists.txt deleted file mode 100644 index e2dd440b..00000000 --- a/thirdparty/libcsp/src/drivers/CMakeLists.txt +++ /dev/null @@ -1 +0,0 @@ -add_subdirectory(can) diff --git a/thirdparty/libcsp/src/drivers/can/CMakeLists.txt b/thirdparty/libcsp/src/drivers/can/CMakeLists.txt deleted file mode 100644 index d291fccc..00000000 --- a/thirdparty/libcsp/src/drivers/can/CMakeLists.txt +++ /dev/null @@ -1,3 +0,0 @@ -target_sources(${LIB_CSP_NAME} PRIVATE - can_socketcan.c -) diff --git a/thirdparty/libcsp/src/drivers/can/can_socketcan.c b/thirdparty/libcsp/src/drivers/can/can_socketcan.c deleted file mode 100644 index 00d6444e..00000000 --- a/thirdparty/libcsp/src/drivers/can/can_socketcan.c +++ /dev/null @@ -1,201 +0,0 @@ -/* -Cubesat Space Protocol - A small network-layer protocol designed for Cubesats -Copyright (C) 2012 GomSpace ApS (http://www.gomspace.com) -Copyright (C) 2012 AAUSAT3 Project (http://aausat3.space.aau.dk) - -This library is free software; you can redistribute it and/or -modify it under the terms of the GNU Lesser General Public -License as published by the Free Software Foundation; either -version 2.1 of the License, or (at your option) any later version. - -This library is distributed in the hope that it will be useful, -but WITHOUT ANY WARRANTY; without even the implied warranty of -MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU -Lesser General Public License for more details. - -You should have received a copy of the GNU Lesser General Public -License along with this library; if not, write to the Free Software -Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA -*/ - -/* SocketCAN driver */ -#include - -#include -#include - -#include -#include -#include -#include -#include - -#include -#include - -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include - -#include -#include - -#ifdef CSP_HAVE_LIBSOCKETCAN -#include -#endif - -static struct can_socketcan_s { - int socket; - csp_iface_t interface; -} socketcan[1] = { - { - .interface = { - .name = "CAN", - .nexthop = csp_can_tx, - .mtu = CSP_CAN_MTU, - .driver = &socketcan[0], - }, - }, -}; - -static void * socketcan_rx_thread(void * parameters) -{ - struct can_frame frame; - int nbytes; - - while (1) { - /* Read CAN frame */ - nbytes = read(socketcan[0].socket, &frame, sizeof(frame)); - if (nbytes < 0) { - csp_log_error("read: %s", strerror(errno)); - continue; - } - - if (nbytes != sizeof(frame)) { - csp_log_warn("Read incomplete CAN frame"); - continue; - } - - /* Frame type */ - if (frame.can_id & (CAN_ERR_FLAG | CAN_RTR_FLAG) || !(frame.can_id & CAN_EFF_FLAG)) { - /* Drop error and remote frames */ - csp_log_warn("Discarding ERR/RTR/SFF frame"); - continue; - } - - /* Strip flags */ - frame.can_id &= CAN_EFF_MASK; - - /* Call RX callbacsp_can_rx_frameck */ - csp_can_rx(&socketcan[0].interface, frame.can_id, frame.data, frame.can_dlc, NULL); - } - - /* We should never reach this point */ - pthread_exit(NULL); -} - - -int csp_can_tx_frame(csp_iface_t *interface, uint32_t id, const uint8_t * data, uint8_t dlc) -{ - struct can_frame frame; - int i, tries = 0; - memset(&frame, 0, sizeof(frame)); - if (dlc > 8) - return -1; - - /* Copy identifier */ - frame.can_id = id | CAN_EFF_FLAG; - - /* Copy data to frame */ - for (i = 0; i < dlc; i++) - frame.data[i] = data[i]; - - /* Set DLC */ - frame.can_dlc = dlc; - - /* Send frame */ - while (write(socketcan[0].socket, &frame, sizeof(frame)) != sizeof(frame)) { - if (++tries < 1000 && errno == ENOBUFS) { - /* Wait 10 ms and try again */ - usleep(10000); - } else { - csp_log_error("write: %s", strerror(errno)); - break; - } - } - - return 0; -} - -csp_iface_t * csp_can_socketcan_init(const char * ifc, int bitrate, int promisc) -{ - struct ifreq ifr; - struct sockaddr_can addr; - pthread_t rx_thread; - - //printf("-I-: Initiating CAN interface %s\n", ifc); - -#ifdef CSP_HAVE_LIBSOCKETCAN - /* Set interface up */ - if (bitrate > 0) { - can_do_stop(ifc); - can_set_bitrate(ifc, bitrate); - can_set_restart_ms(ifc, 100); - can_do_start(ifc); - } -#endif - - /* Create socket */ - if ((socketcan[0].socket = socket(PF_CAN, SOCK_RAW, CAN_RAW)) < 0) { - csp_log_error("socket: %s", strerror(errno)); - return NULL; - } - - /* Locate interface */ - strncpy(ifr.ifr_name, ifc, IFNAMSIZ - 1); - if (ioctl(socketcan[0].socket, SIOCGIFINDEX, &ifr) < 0) { - csp_log_error("ioctl: %s", strerror(errno)); - return NULL; - } - memset(&addr, 0, sizeof(addr)); - /* Bind the socket to CAN interface */ - addr.can_family = AF_CAN; - addr.can_ifindex = ifr.ifr_ifindex; - if (bind(socketcan[0].socket, (struct sockaddr *)&addr, sizeof(addr)) < 0) { - csp_log_error("bind: %s", strerror(errno)); - return NULL; - } - - /* Set filter mode */ - if (promisc == 0) { - - struct can_filter filter; - filter.can_id = CFP_MAKE_DST(csp_get_address()); - filter.can_mask = CFP_MAKE_DST((1 << CFP_HOST_SIZE) - 1); - - if (setsockopt(socketcan[0].socket, SOL_CAN_RAW, CAN_RAW_FILTER, &filter, sizeof(filter)) < 0) { - csp_log_error("setsockopt: %s", strerror(errno)); - return NULL; - } - - } - - /* Create receive thread */ - if (pthread_create(&rx_thread, NULL, socketcan_rx_thread, NULL) != 0) { - csp_log_error("pthread_create: %s", strerror(errno)); - return NULL; - } - - csp_iflist_add(&socketcan[0].interface); - - return &socketcan[0].interface; -} diff --git a/thirdparty/libcsp/src/drivers/usart/usart_linux.c b/thirdparty/libcsp/src/drivers/usart/usart_linux.c deleted file mode 100644 index c4ceeb27..00000000 --- a/thirdparty/libcsp/src/drivers/usart/usart_linux.c +++ /dev/null @@ -1,254 +0,0 @@ -/* -Cubesat Space Protocol - A small network-layer protocol designed for Cubesats -Copyright (C) 2012 GomSpace ApS (http://www.gomspace.com) -Copyright (C) 2012 AAUSAT3 Project (http://aausat3.space.aau.dk) - -This library is free software; you can redistribute it and/or -modify it under the terms of the GNU Lesser General Public -License as published by the Free Software Foundation; either -version 2.1 of the License, or (at your option) any later version. - -This library is distributed in the hope that it will be useful, -but WITHOUT ANY WARRANTY; without even the implied warranty of -MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU -Lesser General Public License for more details. - -You should have received a copy of the GNU Lesser General Public -License along with this library; if not, write to the Free Software -Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA -*/ - -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include - -int fd; -usart_callback_t usart_callback = NULL; - -static void *serial_rx_thread(void *vptr_args); - -int getbaud(int ifd) { - struct termios termAttr; - int inputSpeed = -1; - speed_t baudRate; - tcgetattr(ifd, &termAttr); - /* Get the input speed. */ - baudRate = cfgetispeed(&termAttr); - switch (baudRate) { - case B0: - inputSpeed = 0; - break; - case B50: - inputSpeed = 50; - break; - case B110: - inputSpeed = 110; - break; - case B134: - inputSpeed = 134; - break; - case B150: - inputSpeed = 150; - break; - case B200: - inputSpeed = 200; - break; - case B300: - inputSpeed = 300; - break; - case B600: - inputSpeed = 600; - break; - case B1200: - inputSpeed = 1200; - break; - case B1800: - inputSpeed = 1800; - break; - case B2400: - inputSpeed = 2400; - break; - case B4800: - inputSpeed = 4800; - break; - case B9600: - inputSpeed = 9600; - break; - case B19200: - inputSpeed = 19200; - break; - case B38400: - inputSpeed = 38400; - break; - case B57600: - inputSpeed = 57600; - break; - case B115200: - inputSpeed = 115200; - break; - case B230400: - inputSpeed = 230400; - break; -#ifndef CSP_MACOSX - case B460800: - inputSpeed = 460800; - break; - case B500000: - inputSpeed = 500000; - break; - case B576000: - inputSpeed = 576000; - break; - case B921600: - inputSpeed = 921600; - break; - case B1000000: - inputSpeed = 1000000; - break; - case B1152000: - inputSpeed = 1152000; - break; - case B1500000: - inputSpeed = 1500000; - break; - case B2000000: - inputSpeed = 2000000; - break; - case B2500000: - inputSpeed = 2500000; - break; - case B3000000: - inputSpeed = 3000000; - break; - case B3500000: - inputSpeed = 3500000; - break; - case B4000000: - inputSpeed = 4000000; - break; -#endif - } - - return inputSpeed; - -} - -void usart_init(struct usart_conf * conf) { - - struct termios options; - pthread_t rx_thread; - - fd = open(conf->device, O_RDWR | O_NOCTTY | O_NONBLOCK); - - if (fd < 0) { - printf("Failed to open %s: %s\r\n", conf->device, strerror(errno)); - return; - } - - int brate = 0; - switch(conf->baudrate) { - case 4800: brate=B4800; break; - case 9600: brate=B9600; break; - case 19200: brate=B19200; break; - case 38400: brate=B38400; break; - case 57600: brate=B57600; break; - case 115200: brate=B115200; break; - case 230400: brate=B230400; break; -#ifndef CSP_MACOSX - case 460800: brate=B460800; break; - case 500000: brate=B500000; break; - case 576000: brate=B576000; break; - case 921600: brate=B921600; break; - case 1000000: brate=B1000000; break; - case 1152000: brate=B1152000; break; - case 1500000: brate=B1500000; break; - case 2000000: brate=B2000000; break; - case 2500000: brate=B2500000; break; - case 3000000: brate=B3000000; break; - case 3500000: brate=B3500000; break; - case 4000000: brate=B4000000; break; -#endif - default: - printf("Unsupported baudrate requested, defaulting to 500000, requested baudrate=%u\n", conf->baudrate); - brate=B500000; - break; - } - - tcgetattr(fd, &options); - cfsetispeed(&options, brate); - cfsetospeed(&options, brate); - options.c_cflag |= (CLOCAL | CREAD); - options.c_cflag &= ~PARENB; - options.c_cflag &= ~CSTOPB; - options.c_cflag &= ~CSIZE; - options.c_cflag |= CS8; - options.c_lflag &= ~(ECHO | ECHONL | ICANON | IEXTEN | ISIG); - options.c_iflag &= ~(IGNBRK | BRKINT | ICRNL | INLCR | PARMRK | INPCK | ISTRIP | IXON); - options.c_oflag &= ~(OCRNL | ONLCR | ONLRET | ONOCR | OFILL | OPOST); - options.c_cc[VTIME] = 0; - options.c_cc[VMIN] = 1; - tcsetattr(fd, TCSANOW, &options); - if (tcgetattr(fd, &options) == -1) - perror("error setting options"); - fcntl(fd, F_SETFL, 0); - - /* Flush old transmissions */ - if (tcflush(fd, TCIOFLUSH) == -1) - printf("Error flushing serial port - %s(%d).\n", strerror(errno), errno); - - if (pthread_create(&rx_thread, NULL, serial_rx_thread, NULL) != 0) - return; - -} - -void usart_set_callback(usart_callback_t callback) { - usart_callback = callback; -} - -void usart_insert(char c, void * pxTaskWoken) { - printf("%c", c); -} - -void usart_putstr(char * buf, int len) { - if (write(fd, buf, len) != len) - return; -} - -void usart_putc(char c) { - if (write(fd, &c, 1) != 1) - return; -} - -char usart_getc(void) { - char c; - if (read(fd, &c, 1) != 1) return 0; - return c; -} - -static void *serial_rx_thread(void *vptr_args) { - unsigned int length; - uint8_t * cbuf = malloc(100000); - - // Receive loop - while (1) { - length = read(fd, cbuf, 300); - if (length <= 0) { - perror("Error: "); - exit(1); - } - if (usart_callback) - usart_callback(cbuf, length, NULL); - } - return NULL; -} diff --git a/thirdparty/libcsp/src/drivers/usart/usart_windows.c b/thirdparty/libcsp/src/drivers/usart/usart_windows.c deleted file mode 100644 index 91ffe87d..00000000 --- a/thirdparty/libcsp/src/drivers/usart/usart_windows.c +++ /dev/null @@ -1,230 +0,0 @@ -#include -#include -#include - -#include -#include - -static HANDLE portHandle = INVALID_HANDLE_VALUE; -static HANDLE rxThread = INVALID_HANDLE_VALUE; -static CRITICAL_SECTION txSection; -static LONG isListening = 0; -static usart_callback_t usart_callback = NULL; - -static void prvSendData(char *buf, int bufsz); -static int prvTryOpenPort(const char* intf); -static int prvTryConfigurePort(const struct usart_conf*); -static int prvTrySetPortTimeouts(void); -static const char* prvParityToStr(BYTE paritySetting); - -#ifdef CSP_DEBUG -static void prvPrintError(void) { - char *messageBuffer = NULL; - DWORD errorCode = GetLastError(); - DWORD formatMessageRet; - formatMessageRet = FormatMessageA( - FORMAT_MESSAGE_ALLOCATE_BUFFER | - FORMAT_MESSAGE_FROM_SYSTEM, - NULL, - errorCode, - MAKELANGID(LANG_NEUTRAL, SUBLANG_DEFAULT), - (char*)&messageBuffer, - 0, - NULL); - - if( !formatMessageRet ) { - csp_log_error("FormatMessage error, code: %lu", GetLastError()); - return; - } - csp_log_error("%s", messageBuffer); - LocalFree(messageBuffer); -} -#endif - -#ifdef CSP_DEBUG -#define printError() prvPrintError() -#else -#define printError() do {} while(0) -#endif - -static int prvTryOpenPort(const char *intf) { - portHandle = CreateFileA( - intf, - GENERIC_READ|GENERIC_WRITE, - 0, - NULL, - OPEN_EXISTING, - 0, - NULL); - - if( portHandle == INVALID_HANDLE_VALUE ) { - DWORD errorCode = GetLastError(); - if( errorCode == ERROR_FILE_NOT_FOUND ) { - csp_log_error("Could not open serial port, because it didn't exist!"); - } - else - csp_log_error("Failure opening serial port! Code: %lu", errorCode); - return 1; - } - return 0; -} - -static int prvTryConfigurePort(const struct usart_conf * conf) { - DCB portSettings = {0}; - portSettings.DCBlength = sizeof(DCB); - if(!GetCommState(portHandle, &portSettings) ) { - csp_log_error("Could not get default settings for open COM port! Code: %lu", GetLastError()); - return -1; - } - portSettings.BaudRate = conf->baudrate; - portSettings.Parity = conf->paritysetting; - portSettings.StopBits = conf->stopbits; - portSettings.fParity = conf->checkparity; - portSettings.fBinary = TRUE; - portSettings.ByteSize = conf->databits; - if( !SetCommState(portHandle, &portSettings) ) { - csp_log_error("Error when setting COM port settings! Code:%lu", GetLastError()); - return 1; - } - - GetCommState(portHandle, &portSettings); - - csp_log_info("Port: %s, Baudrate: %lu, Data bits: %d, Stop bits: %d, Parity: %s", - conf->device, conf->baudrate, conf->databits, conf->stopbits, prvParityToStr(conf->paritysetting)); - return 0; -} - -static const char* prvParityToStr(BYTE paritySetting) { - static const char *parityStr[] = { - "None", - "Odd", - "Even", - "N/A" - }; - char const *resultStr = NULL; - - switch(paritySetting) { - case NOPARITY: - resultStr = parityStr[0]; - break; - case ODDPARITY: - resultStr = parityStr[1]; - break; - case EVENPARITY: - resultStr = parityStr[2]; - break; - default: - resultStr = parityStr[3]; - }; - return resultStr; -} - -static int prvTrySetPortTimeouts(void) { - COMMTIMEOUTS timeouts = {0}; - - if( !GetCommTimeouts(portHandle, &timeouts) ) { - csp_log_error("Error gettings current timeout settings"); - return 1; - } - - timeouts.ReadIntervalTimeout = 5; - timeouts.ReadTotalTimeoutMultiplier = 1; - timeouts.ReadTotalTimeoutConstant = 5; - timeouts.WriteTotalTimeoutMultiplier = 1; - timeouts.WriteTotalTimeoutConstant = 5; - - if(!SetCommTimeouts(portHandle, &timeouts)) { - csp_log_error("Error setting timeouts!"); - return 1; - } - - return 0; -} - -unsigned WINAPI prvRxTask(void* params) { - DWORD bytesRead; - DWORD eventStatus; - uint8_t recvBuffer[24]; - SetCommMask(portHandle, EV_RXCHAR); - - while(isListening) { - WaitCommEvent(portHandle, &eventStatus, NULL); - if( !(eventStatus & EV_RXCHAR) ) { - continue; - } - if( !ReadFile(portHandle, recvBuffer, 24, &bytesRead, NULL)) { - csp_log_warn("Error receiving data! Code: %lu", GetLastError()); - continue; - } - if( usart_callback != NULL ) - usart_callback(recvBuffer, (size_t)bytesRead, NULL); - } - return 0; -} - -static void prvSendData(char *buf, int bufsz) { - DWORD bytesTotal = 0; - DWORD bytesActual; - if( !WriteFile(portHandle, buf, bufsz-bytesTotal, &bytesActual, NULL) ) { - csp_log_error("Could not write data. Code: %lu", GetLastError()); - return; - } - if( !FlushFileBuffers(portHandle) ) { - csp_log_warn("Could not flush write buffer. Code: %lu", GetLastError()); - } -} - -void usart_shutdown(void) { - InterlockedExchange(&isListening, 0); - CloseHandle(portHandle); - portHandle = INVALID_HANDLE_VALUE; - if( rxThread != INVALID_HANDLE_VALUE ) { - WaitForSingleObject(rxThread, INFINITE); - rxThread = INVALID_HANDLE_VALUE; - } - DeleteCriticalSection(&txSection); -} - -void usart_listen(void) { - InterlockedExchange(&isListening, 1); - rxThread = (HANDLE)_beginthreadex(NULL, 0, &prvRxTask, NULL, 0, NULL); -} - -void usart_putstr(char* buf, int bufsz) { - EnterCriticalSection(&txSection); - prvSendData(buf, bufsz); - LeaveCriticalSection(&txSection); -} - -void usart_insert(char c, void *pxTaskWoken) { - /* redirect debug output to stdout */ - printf("%c", c); -} - -void usart_set_callback(usart_callback_t callback) { - usart_callback = callback; -} - -void usart_init(struct usart_conf * conf) { - if( prvTryOpenPort(conf->device) ) { - printError(); - return; - } - - if( prvTryConfigurePort(conf) ) { - printError(); - return; - } - - if( prvTrySetPortTimeouts() ) { - printError(); - return; - } - - InitializeCriticalSection(&txSection); - - /* Start receiver thread */ - usart_listen(); -} - - diff --git a/thirdparty/libcsp/src/interfaces/CMakeLists.txt b/thirdparty/libcsp/src/interfaces/CMakeLists.txt deleted file mode 100644 index 33f779e3..00000000 --- a/thirdparty/libcsp/src/interfaces/CMakeLists.txt +++ /dev/null @@ -1,7 +0,0 @@ -target_sources(${LIB_CSP_NAME} PRIVATE - csp_if_can_pbuf.c - csp_if_can.c - csp_if_i2c.c - csp_if_kiss.c - csp_if_lo.c -) diff --git a/thirdparty/libcsp/src/interfaces/csp_if_can.c b/thirdparty/libcsp/src/interfaces/csp_if_can.c deleted file mode 100644 index 5add8334..00000000 --- a/thirdparty/libcsp/src/interfaces/csp_if_can.c +++ /dev/null @@ -1,279 +0,0 @@ -/* -Cubesat Space Protocol - A small network-layer protocol designed for Cubesats -Copyright (C) 2012 GomSpace ApS (http://www.gomspace.com) -Copyright (C) 2012 AAUSAT3 Project (http://aausat3.space.aau.dk) - -This library is free software; you can redistribute it and/or -modify it under the terms of the GNU Lesser General Public -License as published by the Free Software Foundation; either -version 2.1 of the License, or (at your option) any later version. - -This library is distributed in the hope that it will be useful, -but WITHOUT ANY WARRANTY; without even the implied warranty of -MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU -Lesser General Public License for more details. - -You should have received a copy of the GNU Lesser General Public -License along with this library; if not, write to the Free Software -Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA -*/ - -/* CAN frames contains at most 8 bytes of data, so in order to transmit CSP - * packets larger than this, a fragmentation protocol is required. The CAN - * Fragmentation Protocol (CFP) header is designed to match the 29 bit CAN - * identifier. - * - * The CAN identifier is divided in these fields: - * src: 5 bits - * dst: 5 bits - * type: 1 bit - * remain: 8 bits - * identifier: 10 bits - * - * Source and Destination addresses must match the CSP packet. The type field - * is used to distinguish the first and subsequent frames in a fragmented CSP - * packet. Type is BEGIN (0) for the first fragment and MORE (1) for all other - * fragments. Remain indicates number of remaining fragments, and must be - * decremented by one for each fragment sent. The identifier field serves the - * same purpose as in the Internet Protocol, and should be an auto incrementing - * integer to uniquely separate sessions. - */ - -#include -#include -#include -#include -#include - -#include -#include -#include -#include - -#include -#include -#include -#include - -#include "csp_if_can_pbuf.h" - -/* CFP Frame Types */ -enum cfp_frame_t { - CFP_BEGIN = 0, - CFP_MORE = 1 -}; - -int csp_can_rx(csp_iface_t *interface, uint32_t id, const uint8_t *data, uint8_t dlc, CSP_BASE_TYPE *task_woken) -{ - csp_can_pbuf_element_t *buf; - uint8_t offset; - - /* Random packet loss */ -#if 0 - int random = rand(); - if (random < RAND_MAX * 0.00005) { - csp_log_warn("Dropping frame"); - return; - } -#endif - - /* Bind incoming frame to a packet buffer */ - buf = csp_can_pbuf_find(id, CFP_ID_CONN_MASK); - - /* Check returned buffer */ - if (buf == NULL) { - if (CFP_TYPE(id) == CFP_BEGIN) { - buf = csp_can_pbuf_new(id, task_woken); - if (buf == NULL) { - //csp_log_warn("No available packet buffer for CAN"); - interface->rx_error++; - return CSP_ERR_NOMEM; - } - } else { - //csp_log_warn("Out of order id 0x%X remain %u", CFP_ID(id), CFP_REMAIN(id)); - interface->frame++; - return CSP_ERR_INVAL; - } - } - - /* Reset frame data offset */ - offset = 0; - - switch (CFP_TYPE(id)) { - - case CFP_BEGIN: - - /* Discard packet if DLC is less than CSP id + CSP length fields */ - if (dlc < sizeof(csp_id_t) + sizeof(uint16_t)) { - //csp_log_warn("Short BEGIN frame received"); - interface->frame++; - csp_can_pbuf_free(buf, task_woken); - break; - } - - /* Check for incomplete frame */ - if (buf->packet != NULL) { - /* Reuse the buffer */ - //csp_log_warn("Incomplete frame"); - interface->frame++; - } else { - /* Allocate memory for frame */ - if (task_woken == NULL) { - buf->packet = csp_buffer_get(interface->mtu); - } else { - buf->packet = csp_buffer_get_isr(interface->mtu); - } - if (buf->packet == NULL) { - //csp_log_error("Failed to get buffer for CSP_BEGIN packet"); - interface->frame++; - csp_can_pbuf_free(buf, task_woken); - break; - } - } - - /* Copy CSP identifier and length*/ - memcpy(&(buf->packet->id), data, sizeof(csp_id_t)); - buf->packet->id.ext = csp_ntoh32(buf->packet->id.ext); - memcpy(&(buf->packet->length), data + sizeof(csp_id_t), sizeof(uint16_t)); - buf->packet->length = csp_ntoh16(buf->packet->length); - - /* Reset RX count */ - buf->rx_count = 0; - - /* Set offset to prevent CSP header from being copied to CSP data */ - offset = sizeof(csp_id_t) + sizeof(uint16_t); - - /* Set remain field - increment to include begin packet */ - buf->remain = CFP_REMAIN(id) + 1; - - /* FALLTHROUGH */ - - case CFP_MORE: - - /* Check 'remain' field match */ - if (CFP_REMAIN(id) != buf->remain - 1) { - //csp_log_error("CAN frame lost in CSP packet"); - csp_can_pbuf_free(buf, task_woken); - interface->frame++; - break; - } - - /* Decrement remaining frames */ - buf->remain--; - - /* Check for overflow */ - if ((buf->rx_count + dlc - offset) > buf->packet->length) { - //csp_log_error("RX buffer overflow"); - interface->frame++; - csp_can_pbuf_free(buf, task_woken); - break; - } - - /* Copy dlc bytes into buffer */ - memcpy(&buf->packet->data[buf->rx_count], data + offset, dlc - offset); - buf->rx_count += dlc - offset; - - /* Check if more data is expected */ - if (buf->rx_count != buf->packet->length) - break; - - /* Data is available */ - csp_qfifo_write(buf->packet, interface, task_woken); - - /* Drop packet buffer reference */ - buf->packet = NULL; - - /* Free packet buffer */ - csp_can_pbuf_free(buf, task_woken); - - break; - - default: - //csp_log_warn("Received unknown CFP message type"); - csp_can_pbuf_free(buf, task_woken); - break; - - } - - return CSP_ERR_NONE; -} - -int csp_can_tx(csp_iface_t *interface, csp_packet_t *packet, uint32_t timeout) -{ - - /* CFP Identification number */ - static volatile int csp_can_frame_id = 0; - - /* Get local copy of the static frameid */ - int ident = csp_can_frame_id++; - - uint16_t tx_count; - uint8_t bytes, overhead, avail, dest; - uint8_t frame_buf[8]; - - /* Calculate overhead */ - overhead = sizeof(csp_id_t) + sizeof(uint16_t); - - /* Insert destination node mac address into the CFP destination field */ - dest = csp_rtable_find_mac(packet->id.dst); - if (dest == CSP_NODE_MAC) - dest = packet->id.dst; - - /* Create CAN identifier */ - uint32_t id = 0; - id |= CFP_MAKE_SRC(packet->id.src); - id |= CFP_MAKE_DST(dest); - id |= CFP_MAKE_ID(ident); - id |= CFP_MAKE_TYPE(CFP_BEGIN); - id |= CFP_MAKE_REMAIN((packet->length + overhead - 1) / 8); - - /* Calculate first frame data bytes */ - avail = 8 - overhead; - bytes = (packet->length <= avail) ? packet->length : avail; - - /* Copy CSP headers and data */ - uint32_t csp_id_be = csp_hton32(packet->id.ext); - uint16_t csp_length_be = csp_hton16(packet->length); - - memcpy(frame_buf, &csp_id_be, sizeof(csp_id_be)); - memcpy(frame_buf + sizeof(csp_id_be), &csp_length_be, sizeof(csp_length_be)); - memcpy(frame_buf + overhead, packet->data, bytes); - - /* Increment tx counter */ - tx_count = bytes; - - /* Send first frame */ - if (csp_can_tx_frame(interface, id, frame_buf, overhead + bytes)) { - //csp_log_warn("Failed to send CAN frame in csp_tx_can"); - interface->tx_error++; - return CSP_ERR_DRIVER; - } - - /* Send next frames if not complete */ - while (tx_count < packet->length) { - /* Calculate frame data bytes */ - bytes = (packet->length - tx_count >= 8) ? 8 : packet->length - tx_count; - - /* Prepare identifier */ - id = 0; - id |= CFP_MAKE_SRC(packet->id.src); - id |= CFP_MAKE_DST(dest); - id |= CFP_MAKE_ID(ident); - id |= CFP_MAKE_TYPE(CFP_MORE); - id |= CFP_MAKE_REMAIN((packet->length - tx_count - bytes + 7) / 8); - - /* Increment tx counter */ - tx_count += bytes; - - /* Send frame */ - if (csp_can_tx_frame(interface, id, packet->data + tx_count - bytes, bytes)) { - //csp_log_warn("Failed to send CAN frame in Tx callback"); - interface->tx_error++; - return CSP_ERR_DRIVER; - } - } - - csp_buffer_free(packet); - - return CSP_ERR_NONE; -} diff --git a/thirdparty/libcsp/src/interfaces/csp_if_can_pbuf.c b/thirdparty/libcsp/src/interfaces/csp_if_can_pbuf.c deleted file mode 100644 index 65f18de9..00000000 --- a/thirdparty/libcsp/src/interfaces/csp_if_can_pbuf.c +++ /dev/null @@ -1,77 +0,0 @@ -/* - * csp_if_can_pbuf.c - * - * Created on: Feb 3, 2017 - * Author: johan - */ - -#include -#include "csp_if_can_pbuf.h" - -/* Number of packet buffer elements */ -#define PBUF_ELEMENTS CSP_CONN_MAX - -/* Buffer element timeout in ms */ -#define PBUF_TIMEOUT_MS 1000 - -static csp_can_pbuf_element_t csp_can_pbuf[PBUF_ELEMENTS] = {}; - -int csp_can_pbuf_free(csp_can_pbuf_element_t *buf, CSP_BASE_TYPE *task_woken) -{ - /* Free CSP packet */ - if (buf->packet != NULL) { - if (task_woken == NULL) { - csp_buffer_free(buf->packet); - } else { - csp_buffer_free_isr(buf->packet); - } - } - - /* Mark buffer element free */ - buf->packet = NULL; - buf->rx_count = 0; - buf->cfpid = 0; - buf->last_used = 0; - buf->remain = 0; - buf->state = BUF_FREE; - - return CSP_ERR_NONE; -} - -csp_can_pbuf_element_t *csp_can_pbuf_new(uint32_t id, CSP_BASE_TYPE *task_woken) -{ - uint32_t now = csp_get_ms(); - - for (int i = 0; i < PBUF_ELEMENTS; i++) { - - /* Perform cleanup in used pbufs */ - if (csp_can_pbuf[i].state == BUF_USED) { - if (now - csp_can_pbuf[i].last_used > PBUF_TIMEOUT_MS) - csp_can_pbuf_free(&csp_can_pbuf[i], task_woken); - } - - if (csp_can_pbuf[i].state == BUF_FREE) { - csp_can_pbuf[i].state = BUF_USED; - csp_can_pbuf[i].cfpid = id; - csp_can_pbuf[i].remain = 0; - csp_can_pbuf[i].last_used = now; - return &csp_can_pbuf[i]; - } - - } - - return NULL; - -} - -csp_can_pbuf_element_t *csp_can_pbuf_find(uint32_t id, uint32_t mask) -{ - for (int i = 0; i < PBUF_ELEMENTS; i++) { - if ((csp_can_pbuf[i].state == BUF_USED) && ((csp_can_pbuf[i].cfpid & mask) == (id & mask))) { - csp_can_pbuf[i].last_used = csp_get_ms(); - return &csp_can_pbuf[i]; - } - } - return NULL; -} - diff --git a/thirdparty/libcsp/src/interfaces/csp_if_can_pbuf.h b/thirdparty/libcsp/src/interfaces/csp_if_can_pbuf.h deleted file mode 100644 index 3e71c26c..00000000 --- a/thirdparty/libcsp/src/interfaces/csp_if_can_pbuf.h +++ /dev/null @@ -1,31 +0,0 @@ -/* - * csp_if_can_pbuf.h - * - * Created on: Feb 3, 2017 - * Author: johan - */ - -#ifndef LIB_CSP_SRC_INTERFACES_CSP_IF_CAN_PBUF_H_ -#define LIB_CSP_SRC_INTERFACES_CSP_IF_CAN_PBUF_H_ - -/* Packet buffers */ -typedef enum { - BUF_FREE = 0, /* Buffer element free */ - BUF_USED = 1, /* Buffer element used */ -} csp_can_pbuf_state_t; - -typedef struct { - uint16_t rx_count; /* Received bytes */ - uint32_t remain; /* Remaining packets */ - uint32_t cfpid; /* Connection CFP identification number */ - csp_packet_t *packet; /* Pointer to packet buffer */ - csp_can_pbuf_state_t state; /* Element state */ - uint32_t last_used; /* Timestamp in ms for last use of buffer */ -} csp_can_pbuf_element_t; - -int csp_can_pbuf_free(csp_can_pbuf_element_t *buf, CSP_BASE_TYPE *task_woken); -csp_can_pbuf_element_t *csp_can_pbuf_new(uint32_t id, CSP_BASE_TYPE *task_woken); -csp_can_pbuf_element_t *csp_can_pbuf_find(uint32_t id, uint32_t mask); -void csp_can_pbuf_cleanup(CSP_BASE_TYPE *task_woken); - -#endif /* LIB_CSP_SRC_INTERFACES_CSP_IF_CAN_PBUF_H_ */ diff --git a/thirdparty/libcsp/src/interfaces/csp_if_i2c.c b/thirdparty/libcsp/src/interfaces/csp_if_i2c.c deleted file mode 100644 index c5d105df..00000000 --- a/thirdparty/libcsp/src/interfaces/csp_if_i2c.c +++ /dev/null @@ -1,116 +0,0 @@ -/* -Cubesat Space Protocol - A small network-layer protocol designed for Cubesats -Copyright (C) 2012 GomSpace ApS (http://www.gomspace.com) -Copyright (C) 2012 AAUSAT3 Project (http://aausat3.space.aau.dk) - -This library is free software; you can redistribute it and/or -modify it under the terms of the GNU Lesser General Public -License as published by the Free Software Foundation; either -version 2.1 of the License, or (at your option) any later version. - -This library is distributed in the hope that it will be useful, -but WITHOUT ANY WARRANTY; without even the implied warranty of -MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU -Lesser General Public License for more details. - -You should have received a copy of the GNU Lesser General Public -License along with this library; if not, write to the Free Software -Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA -*/ - -#include -#include - -#include -#include -#include -#include -#include -#include - -static int csp_i2c_handle = 0; - -int csp_i2c_tx(csp_iface_t * interface, csp_packet_t * packet, uint32_t timeout) { - - /* Cast the CSP packet buffer into an i2c frame */ - i2c_frame_t * frame = (i2c_frame_t *) packet; - - /* Insert destination node into the i2c destination field */ - if (csp_rtable_find_mac(packet->id.dst) == CSP_NODE_MAC) { - frame->dest = packet->id.dst; - } else { - frame->dest = csp_rtable_find_mac(packet->id.dst); - } - - /* Save the outgoing id in the buffer */ - packet->id.ext = csp_hton32(packet->id.ext); - - /* Add the CSP header to the I2C length field */ - frame->len += sizeof(packet->id); - frame->len_rx = 0; - - /* Some I2C drivers support X number of retries - * CSP don't care about this. If it doesn't work the first - * time, don'y use time on it. - */ - frame->retries = 0; - - /* enqueue the frame */ - if (i2c_send(csp_i2c_handle, frame, timeout) != E_NO_ERR) - return CSP_ERR_DRIVER; - - return CSP_ERR_NONE; - -} - -/** - * When a frame is received, cast it to a csp_packet - * and send it directly to the CSP new packet function. - * Context: ISR only - * @param frame - */ -void csp_i2c_rx(i2c_frame_t * frame, void * pxTaskWoken) { - - static csp_packet_t * packet; - - /* Validate input */ - if (frame == NULL) - return; - - if ((frame->len < 4) || (frame->len > I2C_MTU)) { - csp_if_i2c.frame++; - csp_buffer_free_isr(frame); - return; - } - - /* Strip the CSP header off the length field before converting to CSP packet */ - frame->len -= sizeof(csp_id_t); - - /* Convert the packet from network to host order */ - packet = (csp_packet_t *) frame; - packet->id.ext = csp_ntoh32(packet->id.ext); - - /* Receive the packet in CSP */ - csp_qfifo_write(packet, &csp_if_i2c, pxTaskWoken); - -} - -int csp_i2c_init(uint8_t addr, int handle, int speed) { - - /* Create i2c_handle */ - csp_i2c_handle = handle; - if (i2c_init(csp_i2c_handle, I2C_MASTER, addr, speed, 10, 10, csp_i2c_rx) != E_NO_ERR) - return CSP_ERR_DRIVER; - - /* Register interface */ - csp_iflist_add(&csp_if_i2c); - - return CSP_ERR_NONE; - -} - -/** Interface definition */ -csp_iface_t csp_if_i2c = { - .name = "I2C", - .nexthop = csp_i2c_tx, -}; diff --git a/thirdparty/libcsp/src/interfaces/csp_if_kiss.c b/thirdparty/libcsp/src/interfaces/csp_if_kiss.c deleted file mode 100644 index fe5707f6..00000000 --- a/thirdparty/libcsp/src/interfaces/csp_if_kiss.c +++ /dev/null @@ -1,260 +0,0 @@ -/* -Cubesat Space Protocol - A small network-layer protocol designed for Cubesats -Copyright (C) 2012 GomSpace ApS (http://www.gomspace.com) -Copyright (C) 2012 AAUSAT3 Project (http://aausat3.space.aau.dk) - -This library is free software; you can redistribute it and/or -modify it under the terms of the GNU Lesser General Public -License as published by the Free Software Foundation; either -version 2.1 of the License, or (at your option) any later version. - -This library is distributed in the hope that it will be useful, -but WITHOUT ANY WARRANTY; without even the implied warranty of -MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU -Lesser General Public License for more details. - -You should have received a copy of the GNU Lesser General Public -License along with this library; if not, write to the Free Software -Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA -*/ - -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include - -#define KISS_MTU 256 - -#define FEND 0xC0 -#define FESC 0xDB -#define TFEND 0xDC -#define TFESC 0xDD - -#define TNC_DATA 0x00 -#define TNC_SET_HARDWARE 0x06 -#define TNC_RETURN 0xFF - -static int kiss_lock_init = 0; -static csp_bin_sem_handle_t kiss_lock; - -/* Send a CSP packet over the KISS RS232 protocol */ -static int csp_kiss_tx(csp_iface_t * interface, csp_packet_t * packet, uint32_t timeout) { - - if (interface == NULL || interface->driver == NULL) - return CSP_ERR_DRIVER; - - /* Add CRC32 checksum */ - csp_crc32_append(packet, false); - - /* Save the outgoing id in the buffer */ - packet->id.ext = csp_hton32(packet->id.ext); - packet->length += sizeof(packet->id.ext); - - /* Lock */ - csp_bin_sem_wait(&kiss_lock, 1000); - - /* Transmit data */ - csp_kiss_handle_t * driver = interface->driver; - driver->kiss_putc(FEND); - driver->kiss_putc(TNC_DATA); - for (unsigned int i = 0; i < packet->length; i++) { - if (((unsigned char *) &packet->id.ext)[i] == FEND) { - ((unsigned char *) &packet->id.ext)[i] = TFEND; - driver->kiss_putc(FESC); - } else if (((unsigned char *) &packet->id.ext)[i] == FESC) { - ((unsigned char *) &packet->id.ext)[i] = TFESC; - driver->kiss_putc(FESC); - } - driver->kiss_putc(((unsigned char *) &packet->id.ext)[i]); - } - driver->kiss_putc(FEND); - - /* Free data */ - csp_buffer_free(packet); - - /* Unlock */ - csp_bin_sem_post(&kiss_lock); - - return CSP_ERR_NONE; -} - -/** - * When a frame is received, decode the kiss-stuff - * and eventually send it directly to the CSP new packet function. - */ -void csp_kiss_rx(csp_iface_t * interface, uint8_t * buf, int len, void * pxTaskWoken) { - - /* Driver handle */ - csp_kiss_handle_t * driver = interface->driver; - - while (len--) { - - /* Input */ - unsigned char inputbyte = *buf++; - - /* If packet was too long */ - if (driver->rx_length > interface->mtu) { - //csp_log_warn("KISS RX overflow"); - interface->rx_error++; - driver->rx_mode = KISS_MODE_NOT_STARTED; - driver->rx_length = 0; - } - - switch (driver->rx_mode) { - - case KISS_MODE_NOT_STARTED: - - /* Send normal chars back to usart driver */ - if (inputbyte != FEND) { - if (driver->kiss_discard != NULL) - driver->kiss_discard(inputbyte, pxTaskWoken); - break; - } - - /* Try to allocate new buffer */ - if (driver->rx_packet == NULL) { - if (pxTaskWoken == NULL) { - driver->rx_packet = csp_buffer_get(interface->mtu); - } else { - driver->rx_packet = csp_buffer_get_isr(interface->mtu); - } - } - - /* If no more memory, skip frame */ - if (driver->rx_packet == NULL) { - driver->rx_mode = KISS_MODE_SKIP_FRAME; - break; - } - - /* Start transfer */ - driver->rx_length = 0; - driver->rx_mode = KISS_MODE_STARTED; - driver->rx_first = 1; - break; - - case KISS_MODE_STARTED: - - /* Escape char */ - if (inputbyte == FESC) { - driver->rx_mode = KISS_MODE_ESCAPED; - break; - } - - /* End Char */ - if (inputbyte == FEND) { - - /* Accept message */ - if (driver->rx_length > 0) { - - /* Check for valid length */ - if (driver->rx_length < CSP_HEADER_LENGTH + sizeof(uint32_t)) { - //csp_log_warn("KISS short frame skipped, len: %u", driver->rx_length); - interface->rx_error++; - driver->rx_mode = KISS_MODE_NOT_STARTED; - break; - } - - /* Count received frame */ - interface->frame++; - - /* The CSP packet length is without the header */ - driver->rx_packet->length = driver->rx_length - CSP_HEADER_LENGTH; - - /* Convert the packet from network to host order */ - driver->rx_packet->id.ext = csp_ntoh32(driver->rx_packet->id.ext); - - /* Validate CRC */ - if (csp_crc32_verify(driver->rx_packet, false) != CSP_ERR_NONE) { - //csp_log_warn("KISS invalid crc frame skipped, len: %u", driver->rx_packet->length); - interface->rx_error++; - driver->rx_mode = KISS_MODE_NOT_STARTED; - break; - } - - /* Send back into CSP, notice calling from task so last argument must be NULL! */ - csp_qfifo_write(driver->rx_packet, interface, pxTaskWoken); - driver->rx_packet = NULL; - driver->rx_mode = KISS_MODE_NOT_STARTED; - break; - - } - - /* Break after the end char */ - break; - } - - /* Skip the first char after FEND which is TNC_DATA (0x00) */ - if (driver->rx_first) { - driver->rx_first = 0; - break; - } - - /* Valid data char */ - ((char *) &driver->rx_packet->id.ext)[driver->rx_length++] = inputbyte; - - break; - - case KISS_MODE_ESCAPED: - - /* Escaped escape char */ - if (inputbyte == TFESC) - ((char *) &driver->rx_packet->id.ext)[driver->rx_length++] = FESC; - - /* Escaped fend char */ - if (inputbyte == TFEND) - ((char *) &driver->rx_packet->id.ext)[driver->rx_length++] = FEND; - - /* Go back to started mode */ - driver->rx_mode = KISS_MODE_STARTED; - break; - - case KISS_MODE_SKIP_FRAME: - - /* Just wait for end char */ - if (inputbyte == FEND) - driver->rx_mode = KISS_MODE_NOT_STARTED; - - break; - - } - - } - -} - -void csp_kiss_init(csp_iface_t * csp_iface, csp_kiss_handle_t * csp_kiss_handle, csp_kiss_putc_f kiss_putc_f, csp_kiss_discard_f kiss_discard_f, const char * name) { - - /* Init lock only once */ - if (kiss_lock_init == 0) { - csp_bin_sem_create(&kiss_lock); - kiss_lock_init = 1; - } - - /* Register device handle as member of interface */ - csp_iface->driver = csp_kiss_handle; - csp_kiss_handle->kiss_discard = kiss_discard_f; - csp_kiss_handle->kiss_putc = kiss_putc_f; - csp_kiss_handle->rx_packet = NULL; - csp_kiss_handle->rx_mode = KISS_MODE_NOT_STARTED; - - /* Set default MTU if not given */ - if (csp_iface->mtu == 0) { - csp_iface->mtu = KISS_MTU; - } - - /* Setup other mandatories */ - csp_iface->nexthop = csp_kiss_tx; - csp_iface->name = name; - - /* Regsiter interface */ - csp_iflist_add(csp_iface); - -} diff --git a/thirdparty/libcsp/src/interfaces/csp_if_lo.c b/thirdparty/libcsp/src/interfaces/csp_if_lo.c deleted file mode 100644 index f3e81b15..00000000 --- a/thirdparty/libcsp/src/interfaces/csp_if_lo.c +++ /dev/null @@ -1,61 +0,0 @@ -/* -Cubesat Space Protocol - A small network-layer protocol designed for Cubesats -Copyright (C) 2012 GomSpace ApS (http://www.gomspace.com) -Copyright (C) 2012 AAUSAT3 Project (http://aausat3.space.aau.dk) - -This library is free software; you can redistribute it and/or -modify it under the terms of the GNU Lesser General Public -License as published by the Free Software Foundation; either -version 2.1 of the License, or (at your option) any later version. - -This library is distributed in the hope that it will be useful, -but WITHOUT ANY WARRANTY; without even the implied warranty of -MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU -Lesser General Public License for more details. - -You should have received a copy of the GNU Lesser General Public -License along with this library; if not, write to the Free Software -Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA -*/ - -/* CSP includes */ -#include -#include -#include -#include - -#include -#include - -#include "../csp_route.h" - -/** - * Loopback interface transmit function - * @param packet Packet to transmit - * @param timeout Timout in ms - * @return 1 if packet was successfully transmitted, 0 on error - */ -static int csp_lo_tx(csp_iface_t * interface, csp_packet_t * packet, uint32_t timeout) { - - /* Drop packet silently if not destined for us. This allows - * blackhole routing addresses by setting their nexthop to - * the loopback interface. - */ - if (packet->id.dst != csp_get_address()) { - /* Consume and drop packet */ - csp_buffer_free(packet); - return CSP_ERR_NONE; - } - - /* Send back into CSP, notice calling from task so last argument must be NULL! */ - csp_qfifo_write(packet, &csp_if_lo, NULL); - - return CSP_ERR_NONE; - -} - -/* Interface definition */ -csp_iface_t csp_if_lo = { - .name = "LOOP", - .nexthop = csp_lo_tx, -}; diff --git a/thirdparty/libcsp/src/rtable/CMakeLists.txt b/thirdparty/libcsp/src/rtable/CMakeLists.txt deleted file mode 100644 index 101f4fb9..00000000 --- a/thirdparty/libcsp/src/rtable/CMakeLists.txt +++ /dev/null @@ -1,3 +0,0 @@ -target_sources(${LIB_CSP_NAME} PRIVATE - csp_rtable_cidr.c -) diff --git a/thirdparty/libcsp/src/rtable/csp_rtable_cidr.c b/thirdparty/libcsp/src/rtable/csp_rtable_cidr.c deleted file mode 100644 index 5758dc3c..00000000 --- a/thirdparty/libcsp/src/rtable/csp_rtable_cidr.c +++ /dev/null @@ -1,233 +0,0 @@ -/* -Cubesat Space Protocol - A small network-layer protocol designed for Cubesats -Copyright (C) 2012 GomSpace ApS (http://www.gomspace.com) -Copyright (C) 2012 AAUSAT3 Project (http://aausat3.space.aau.dk) - -This library is free software; you can redistribute it and/or -modify it under the terms of the GNU Lesser General Public -License as published by the Free Software Foundation; either -version 2.1 of the License, or (at your option) any later version. - -This library is distributed in the hope that it will be useful, -but WITHOUT ANY WARRANTY; without even the implied warranty of -MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU -Lesser General Public License for more details. - -You should have received a copy of the GNU Lesser General Public -License along with this library; if not, write to the Free Software -Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA -*/ - -#include -#include -#include -#include -#include -#include - -/* Local typedef for routing table */ -typedef struct __attribute__((__packed__)) csp_rtable_s { - uint8_t address; - uint8_t netmask; - uint8_t mac; - csp_iface_t * interface; - struct csp_rtable_s * next; -} csp_rtable_t; - -/* Routing entries are stored in a linked list*/ -static csp_rtable_t * rtable = NULL; - -static csp_rtable_t * csp_rtable_find(uint8_t addr, uint8_t netmask, uint8_t exact) { - - /* Remember best result */ - csp_rtable_t * best_result = NULL; - uint8_t best_result_mask = 0; - - /* Start search */ - csp_rtable_t * i = rtable; - while(i) { - - /* Look for exact match */ - if (i->address == addr && i->netmask == netmask) { - best_result = i; - break; - } - - /* Try a CIDR netmask match */ - if (!exact) { - uint8_t hostbits = (1 << (CSP_ID_HOST_SIZE - i->netmask)) - 1; - uint8_t netbits = ~hostbits; - //printf("Netbits %x Hostbits %x\r\n", netbits, hostbits); - - /* Match network addresses */ - uint8_t net_a = i->address & netbits; - uint8_t net_b = addr & netbits; - //printf("A: %hhx, B: %hhx\r\n", net_a, net_b); - - /* We have a match */ - if (net_a == net_b) { - if (i->netmask >= best_result_mask) { - //printf("Match best result %u %u\r\n", best_result_mask, i->netmask); - best_result = i; - best_result_mask = i->netmask; - } - } - - } - - i = i->next; - - } - -#if 0 - if (best_result) - csp_debug(CSP_PACKET, "Using routing entry: %u/%u dev %s m:%u\r\n", best_result->address, best_result->netmask, best_result->interface->name, best_result->mac); -#endif - - return best_result; - -} - -void csp_rtable_clear(void) { - for (csp_rtable_t * i = rtable; (i);) { - void * freeme = i; - i = i->next; - csp_free(freeme); - } - rtable = NULL; - - /* Set loopback up again */ - csp_rtable_set(csp_get_address(), CSP_ID_HOST_SIZE, &csp_if_lo, CSP_NODE_MAC); - -} - -static int csp_rtable_parse(const char * buffer, int dry_run) { - - int valid_entries = 0; - - /* Copy string before running strtok */ - char * str = alloca(strlen(buffer) + 1); - memcpy(str, buffer, strlen(buffer) + 1); - - /* Get first token */ - str = strtok(str, ","); - - while ((str) && (strlen(str) > 1)) { - int address = 0, netmask = 0, mac = 255; - char name[10] = {}; - if (sscanf(str, "%u/%u %s %u", &address, &netmask, name, &mac) != 4) { - if (sscanf(str, "%u/%u %s", &address, &netmask, name) != 3) { - csp_log_error("Parse error %s", str); - return -1; - } - } - //printf("Parsed %u/%u %u %s\r\n", address, netmask, mac, name); - csp_iface_t * ifc = csp_iflist_get_by_name(name); - if (ifc) { - if (dry_run == 0) - csp_rtable_set(address, netmask, ifc, mac); - } else { - csp_log_error("Unknown interface %s", name); - return -1; - } - valid_entries++; - str = strtok(NULL, ","); - } - - return valid_entries; -} - -void csp_rtable_load(const char * buffer) { - csp_rtable_parse(buffer, 0); -} - -int csp_rtable_check(const char * buffer) { - return csp_rtable_parse(buffer, 1); -} - -int csp_rtable_save(char * buffer, int maxlen) { - int len = 0; - for (csp_rtable_t * i = rtable; (i); i = i->next) { - if (i->mac != CSP_NODE_MAC) { - len += snprintf(buffer + len, maxlen - len, "%u/%u %s %u, ", i->address, i->netmask, i->interface->name, i->mac); - } else { - len += snprintf(buffer + len, maxlen - len, "%u/%u %s, ", i->address, i->netmask, i->interface->name); - } - } - return len; -} - -csp_iface_t * csp_rtable_find_iface(uint8_t id) { - csp_rtable_t * entry = csp_rtable_find(id, CSP_ID_HOST_SIZE, 0); - if (entry == NULL) - return NULL; - return entry->interface; -} - -uint8_t csp_rtable_find_mac(uint8_t id) { - csp_rtable_t * entry = csp_rtable_find(id, CSP_ID_HOST_SIZE, 0); - if (entry == NULL) - return 255; - return entry->mac; -} - -int csp_rtable_set(uint8_t _address, uint8_t _netmask, csp_iface_t *ifc, uint8_t mac) { - - if (ifc == NULL) - return CSP_ERR_INVAL; - - /* Set default route in the old way */ - int address, netmask; - if (_address == CSP_DEFAULT_ROUTE) { - netmask = 0; - address = 0; - } else { - netmask = _netmask; - address = _address; - } - - /* Fist see if the entry exists */ - csp_rtable_t * entry = csp_rtable_find(address, netmask, 1); - - /* If not, create a new one */ - if (!entry) { - entry = csp_malloc(sizeof(csp_rtable_t)); - if (entry == NULL) - return CSP_ERR_NOMEM; - - entry->next = NULL; - /* Add entry to linked-list */ - if (rtable == NULL) { - /* This is the first interface to be added */ - rtable = entry; - } else { - /* One or more interfaces were already added */ - csp_rtable_t * i = rtable; - while (i->next) - i = i->next; - i->next = entry; - } - } - - /* Fill in the data */ - entry->address = address; - entry->netmask = netmask; - entry->interface = ifc; - entry->mac = mac; - - return CSP_ERR_NONE; -} - -#ifdef CSP_DEBUG -void csp_rtable_print(void) { - - for (csp_rtable_t * i = rtable; (i); i = i->next) { - if (i->mac == 255) { - printf("%u/%u %s\r\n", i->address, i->netmask, i->interface->name); - } else { - printf("%u/%u %s %u\r\n", i->address, i->netmask, i->interface->name, i->mac); - } - } - -} -#endif diff --git a/thirdparty/libcsp/src/rtable/csp_rtable_static.c b/thirdparty/libcsp/src/rtable/csp_rtable_static.c deleted file mode 100644 index ea993027..00000000 --- a/thirdparty/libcsp/src/rtable/csp_rtable_static.c +++ /dev/null @@ -1,128 +0,0 @@ -/* -Cubesat Space Protocol - A small network-layer protocol designed for Cubesats -Copyright (C) 2012 GomSpace ApS (http://www.gomspace.com) -Copyright (C) 2012 AAUSAT3 Project (http://aausat3.space.aau.dk) - -This library is free software; you can redistribute it and/or -modify it under the terms of the GNU Lesser General Public -License as published by the Free Software Foundation; either -version 2.1 of the License, or (at your option) any later version. - -This library is distributed in the hope that it will be useful, -but WITHOUT ANY WARRANTY; without even the implied warranty of -MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU -Lesser General Public License for more details. - -You should have received a copy of the GNU Lesser General Public -License along with this library; if not, write to the Free Software -Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA -*/ - -#include -#include -#include - -/* Local typedef for routing table */ -typedef struct __attribute__((__packed__)) csp_rtable_s { - csp_iface_t * interface; - uint8_t mac; -} csp_rtable_t; - -/* Static storage context for routing table */ -static csp_rtable_t routes[CSP_ROUTE_COUNT] = {}; - -/** - * Find entry in static routing table - * This is done by table lookup with fallback to the default route - * The reason why the csp_rtable_t struct is not returned directly - * is that we wish to hide the storage format, mainly because of - * the alternative routing table storage (cidr). - * @param id Node - * @return pointer to found routing entry - */ -static csp_rtable_t * csp_rtable_find(uint8_t id) { - - if (routes[id].interface != NULL) { - return &routes[id]; - } else if (routes[CSP_DEFAULT_ROUTE].interface != NULL) { - return &routes[CSP_DEFAULT_ROUTE]; - } - return NULL; - -} - -csp_iface_t * csp_rtable_find_iface(uint8_t id) { - csp_rtable_t * route = csp_rtable_find(id); - if (route == NULL) - return NULL; - return route->interface; -} - -uint8_t csp_rtable_find_mac(uint8_t id) { - csp_rtable_t * route = csp_rtable_find(id); - if (route == NULL) - return 255; - return route->mac; -} - -void csp_rtable_clear(void) { - memset(routes, 0, sizeof(routes[0]) * CSP_ROUTE_COUNT); -} - -void csp_route_table_load(uint8_t route_table_in[CSP_ROUTE_TABLE_SIZE]) { - memcpy(routes, route_table_in, sizeof(routes[0]) * CSP_ROUTE_COUNT); -} - -void csp_route_table_save(uint8_t route_table_out[CSP_ROUTE_TABLE_SIZE]) { - memcpy(route_table_out, routes, sizeof(routes[0]) * CSP_ROUTE_COUNT); -} - -int csp_rtable_set(uint8_t node, uint8_t mask, csp_iface_t *ifc, uint8_t mac) { - - /* Don't add nothing */ - if (ifc == NULL) - return CSP_ERR_INVAL; - - /** - * Check if the interface has been added. - * - * NOTE: For future implementations, interfaces should call - * csp_route_add_if in its csp_if__init function, instead - * of registering at first route_set, in order to make the interface - * available to network based (CMP) route configuration. - */ - csp_iflist_add(ifc); - - /* Set route */ - if (node <= CSP_DEFAULT_ROUTE) { - routes[node].interface = ifc; - routes[node].mac = mac; - } else { - csp_log_error("Failed to set route: invalid node id %u", node); - return CSP_ERR_INVAL; - } - - return CSP_ERR_NONE; - -} - -void csp_rtable_load(const char * buffer) { -} - -int csp_rtable_check(const char * buffer) { - return -1; -} - -#ifdef CSP_DEBUG -void csp_rtable_print(void) { - int i; - printf("Node Interface Address\r\n"); - for (i = 0; i < CSP_DEFAULT_ROUTE; i++) - if (routes[i].interface != NULL) - printf("%4u %-9s %u\r\n", i, - routes[i].interface->name, - routes[i].mac == CSP_NODE_MAC ? i : routes[i].mac); - printf(" * %-9s %u\r\n", routes[CSP_DEFAULT_ROUTE].interface->name, routes[CSP_DEFAULT_ROUTE].mac); - -} -#endif diff --git a/thirdparty/libcsp/src/transport/CMakeLists.txt b/thirdparty/libcsp/src/transport/CMakeLists.txt deleted file mode 100644 index c509b755..00000000 --- a/thirdparty/libcsp/src/transport/CMakeLists.txt +++ /dev/null @@ -1,4 +0,0 @@ -target_sources(${LIB_CSP_NAME} PRIVATE - csp_rdp.c - csp_udp.c -) diff --git a/thirdparty/libcsp/src/transport/csp_rdp.c b/thirdparty/libcsp/src/transport/csp_rdp.c deleted file mode 100644 index e19968e2..00000000 --- a/thirdparty/libcsp/src/transport/csp_rdp.c +++ /dev/null @@ -1,1102 +0,0 @@ -/* -Cubesat Space Protocol - A small network-layer protocol designed for Cubesats -Copyright (C) 2012 GomSpace ApS (http://www.gomspace.com) -Copyright (C) 2012 AAUSAT3 Project (http://aausat3.space.aau.dk) - -This library is free software; you can redistribute it and/or -modify it under the terms of the GNU Lesser General Public -License as published by the Free Software Foundation; either -version 2.1 of the License, or (at your option) any later version. - -This library is distributed in the hope that it will be useful, -but WITHOUT ANY WARRANTY; without even the implied warranty of -MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU -Lesser General Public License for more details. - -You should have received a copy of the GNU Lesser General Public -License along with this library; if not, write to the Free Software -Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA -*/ - -/* - * This is a implementation of the seq/ack handling taken from the Reliable Datagram Protocol (RDP) - * For more information read RFC 908/1151. The implementation has been extended to include support for - * delayed acknowledgments, to improve performance over half-duplex links. - */ - -#include -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include -#include "../csp_port.h" -#include "../csp_conn.h" -#include "../csp_io.h" -#include "csp_transport.h" - -#ifdef CSP_USE_RDP - -#define RDP_SYN 0x01 -#define RDP_ACK 0x02 -#define RDP_EAK 0x04 -#define RDP_RST 0x08 - -static uint32_t csp_rdp_window_size = 4; -static uint32_t csp_rdp_conn_timeout = 10000; -static uint32_t csp_rdp_packet_timeout = 1000; -static uint32_t csp_rdp_delayed_acks = 1; -static uint32_t csp_rdp_ack_timeout = 1000 / 4; -static uint32_t csp_rdp_ack_delay_count = 4 / 2; - -/* Used for queue calls */ -static CSP_BASE_TYPE pdTrue = 1; - -typedef struct __attribute__((__packed__)) { - /* The timestamp is placed in the padding bytes */ - uint8_t padding[CSP_PADDING_BYTES - 2 * sizeof(uint32_t)]; - uint32_t quarantine; // EACK quarantine period - uint32_t timestamp; // Time the message was sent - uint16_t length; // Length field must be just before CSP ID - csp_id_t id; // CSP id must be just before data - uint8_t data[]; // This just points to the rest of the buffer, without a size indication. -} rdp_packet_t; - -typedef struct __attribute__((__packed__)) { - union __attribute__((__packed__)) { - uint8_t flags; - struct __attribute__((__packed__)) { -#if defined(CSP_BIG_ENDIAN) && !defined(CSP_LITTLE_ENDIAN) - unsigned int res : 4; - unsigned int syn : 1; - unsigned int ack : 1; - unsigned int eak : 1; - unsigned int rst : 1; -#elif defined(CSP_LITTLE_ENDIAN) && !defined(CSP_BIG_ENDIAN) - unsigned int rst : 1; - unsigned int eak : 1; - unsigned int ack : 1; - unsigned int syn : 1; - unsigned int res : 4; -#else - #error "Must define one of CSP_BIG_ENDIAN or CSP_LITTLE_ENDIAN in csp_platform.h" -#endif - }; - }; - uint16_t seq_nr; - uint16_t ack_nr; -} rdp_header_t; - -/** - * RDP Headers: - * The following functions are helper functions that handles the extra RDP - * information that needs to be appended to all data packets. - */ -static rdp_header_t * csp_rdp_header_add(csp_packet_t * packet) { - rdp_header_t * header = (rdp_header_t *) &packet->data[packet->length]; - packet->length += sizeof(rdp_header_t); - memset(header, 0, sizeof(rdp_header_t)); - return header; -} - -static rdp_header_t * csp_rdp_header_remove(csp_packet_t * packet) { - rdp_header_t * header = (rdp_header_t *) &packet->data[packet->length-sizeof(rdp_header_t)]; - packet->length -= sizeof(rdp_header_t); - return header; -} - -static rdp_header_t * csp_rdp_header_ref(csp_packet_t * packet) { - rdp_header_t * header = (rdp_header_t *) &packet->data[packet->length-sizeof(rdp_header_t)]; - return header; -} - -/* Functions for comparing wrapping sequence numbers and timestamps */ - -/* Return 1 if seq is between start and end (both inclusive) */ -static inline int csp_rdp_seq_between(uint16_t seq, uint16_t start, uint16_t end) { - return (uint16_t)(end - start) >= (uint16_t)(seq - start); -} - -/* Return 1 if seq is before cmp */ -static inline int csp_rdp_seq_before(uint16_t seq, uint16_t cmp) { - return (int16_t)(seq - cmp) < 0; -} - -/* Return 1 if seq is after cmp */ -static inline int csp_rdp_seq_after(uint16_t seq, uint16_t cmp) { - return csp_rdp_seq_before(cmp, seq); -} - -/* Return 1 if time is between start and end (both inclusive) */ -static inline int csp_rdp_time_between(uint32_t time, uint32_t start, uint32_t end) { - return (uint32_t)(end - start) >= (uint32_t)(time - start); -} - -/* Return 1 if time is before cmp */ -static inline int csp_rdp_time_before(uint32_t time, uint32_t cmp) { - return (int32_t)(time - cmp) < 0; -} - -/* Return 1 if time is after cmp */ -static inline int csp_rdp_time_after(uint32_t time, uint32_t cmp) { - return csp_rdp_time_before(cmp, time); -} - -/** - * CONTROL MESSAGES - * The following function is used to send empty messages, - * with ACK, SYN or RST flag. - */ -static int csp_rdp_send_cmp(csp_conn_t * conn, csp_packet_t * packet, int flags, int seq_nr, int ack_nr) { - - csp_id_t idout; - - /* Generate message */ - if (!packet) { - packet = csp_buffer_get(20); - if (!packet) - return CSP_ERR_NOMEM; - packet->length = 0; - } - - /* Add RDP header */ - rdp_header_t * header = csp_rdp_header_add(packet); - header->seq_nr = csp_hton16(seq_nr); - header->ack_nr = csp_hton16(ack_nr); - header->ack = (flags & RDP_ACK) ? 1 : 0; - header->eak = (flags & RDP_EAK) ? 1 : 0; - header->syn = (flags & RDP_SYN) ? 1 : 0; - header->rst = (flags & RDP_RST) ? 1 : 0; - - /* Send copy to tx_queue, before sending packet to IF */ - if (flags & RDP_SYN) { - rdp_packet_t * rdp_packet = csp_buffer_clone(packet); - if (rdp_packet == NULL) return CSP_ERR_NOMEM; - rdp_packet->timestamp = csp_get_ms(); - if (csp_queue_enqueue(conn->rdp.tx_queue, &rdp_packet, 0) != CSP_QUEUE_OK) - csp_buffer_free(rdp_packet); - } - - /* Send control messages with high priority */ - idout = conn->idout; - idout.pri = conn->idout.pri < CSP_PRIO_HIGH ? conn->idout.pri : CSP_PRIO_HIGH; - - /* Send packet to IF */ - csp_iface_t * ifout = csp_rtable_find_iface(idout.dst); - if (csp_send_direct(idout, packet, ifout, 0) != CSP_ERR_NONE) { - csp_log_error("INTERFACE ERROR: not possible to send"); - csp_buffer_free(packet); - return CSP_ERR_BUSY; - } - - /* Update last ACK time stamp */ - if (flags & RDP_ACK) { - conn->rdp.rcv_lsa = ack_nr; - conn->rdp.ack_timestamp = csp_get_ms(); - } - - return CSP_ERR_NONE; - -} - -/** - * EXTENDED ACKNOWLEDGEMENTS - * The following function sends an extended ACK packet - */ -static int csp_rdp_send_eack(csp_conn_t * conn) { - - /* Allocate message */ - csp_packet_t * packet_eack = csp_buffer_get(100); - if (packet_eack == NULL) return CSP_ERR_NOMEM; - packet_eack->length = 0; - - /* Loop through RX queue */ - int i, count; - csp_packet_t * packet; - count = csp_queue_size(conn->rdp.rx_queue); - for (i = 0; i < count; i++) { - - if (csp_queue_dequeue_isr(conn->rdp.rx_queue, &packet, &pdTrue) != CSP_QUEUE_OK) { - csp_log_error("Cannot dequeue from rx_queue in queue deliver"); - break; - } - - /* Add seq nr to EACK packet */ - rdp_header_t * header = csp_rdp_header_ref(packet); - packet_eack->data16[packet_eack->length/sizeof(uint16_t)] = csp_hton16(header->seq_nr); - packet_eack->length += sizeof(uint16_t); - csp_log_protocol("Added EACK nr %u", header->seq_nr); - - /* Requeue */ - csp_queue_enqueue_isr(conn->rdp.rx_queue, &packet, &pdTrue); - - } - - return csp_rdp_send_cmp(conn, packet_eack, RDP_ACK | RDP_EAK, conn->rdp.snd_nxt, conn->rdp.rcv_cur); - -} - - -/** - * SYN Packet - * The following function sends a SYN packet - */ -static int csp_rdp_send_syn(csp_conn_t * conn) { - - /* Allocate message */ - csp_packet_t * packet = csp_buffer_get(100); - if (packet == NULL) return CSP_ERR_NOMEM; - - /* Generate contents */ - packet->data32[0] = csp_hton32(csp_rdp_window_size); - packet->data32[1] = csp_hton32(csp_rdp_conn_timeout); - packet->data32[2] = csp_hton32(csp_rdp_packet_timeout); - packet->data32[3] = csp_hton32(csp_rdp_delayed_acks); - packet->data32[4] = csp_hton32(csp_rdp_ack_timeout); - packet->data32[5] = csp_hton32(csp_rdp_ack_delay_count); - packet->length = 6 * sizeof(uint32_t); - - return csp_rdp_send_cmp(conn, packet, RDP_SYN, conn->rdp.snd_iss, 0); - -} - -static inline int csp_rdp_receive_data(csp_conn_t * conn, csp_packet_t * packet) { - - /* Remove RDP header before passing to userspace */ - csp_rdp_header_remove(packet); - - /* Enqueue data */ - if (csp_conn_enqueue_packet(conn, packet) < 0) { - csp_log_warn("Conn RX buffer full"); - return CSP_ERR_NOBUFS; - } - - return CSP_ERR_NONE; - -} - -static inline void csp_rdp_rx_queue_flush(csp_conn_t * conn) { - - /* Loop through RX queue */ - int i, count; - csp_packet_t * packet; - -front: - count = csp_queue_size(conn->rdp.rx_queue); - for (i = 0; i < count; i++) { - - if (csp_queue_dequeue_isr(conn->rdp.rx_queue, &packet, &pdTrue) != CSP_QUEUE_OK) { - csp_log_error("Cannot dequeue from rx_queue in queue deliver"); - break; - } - - rdp_header_t * header = csp_rdp_header_ref(packet); - csp_log_protocol("RX Queue deliver matching Element, seq %u", header->seq_nr); - - /* If the matching packet was found: */ - if (header->seq_nr == (uint16_t)(conn->rdp.rcv_cur + 1)) { - csp_log_protocol("Deliver seq %u", header->seq_nr); - csp_rdp_receive_data(conn, packet); - conn->rdp.rcv_cur++; - /* Loop from first element again */ - goto front; - - /* Otherwise, requeue */ - } else { - csp_queue_enqueue_isr(conn->rdp.rx_queue, &packet, &pdTrue); - } - - } - -} - -static inline bool csp_rdp_seq_in_rx_queue(csp_conn_t * conn, uint16_t seq_nr) { - - /* Loop through RX queue */ - int i, count; - rdp_packet_t * packet; - count = csp_queue_size(conn->rdp.rx_queue); - for (i = 0; i < count; i++) { - - if (csp_queue_dequeue_isr(conn->rdp.rx_queue, &packet, &pdTrue) != CSP_QUEUE_OK) { - csp_log_error("Cannot dequeue from rx_queue in queue exists"); - break; - } - - csp_queue_enqueue_isr(conn->rdp.rx_queue, &packet, &pdTrue); - - rdp_header_t * header = csp_rdp_header_ref((csp_packet_t *) packet); - csp_log_protocol("RX Queue exists matching Element, seq %u", header->seq_nr); - - /* If the matching packet was found, deliver */ - if (header->seq_nr == seq_nr) { - csp_log_protocol("We have a match"); - return true; - } - - } - - return false; - -} - -static inline int csp_rdp_rx_queue_add(csp_conn_t * conn, csp_packet_t * packet, uint16_t seq_nr) { - - if (csp_rdp_seq_in_rx_queue(conn, seq_nr)) - return CSP_QUEUE_ERROR; - return csp_queue_enqueue_isr(conn->rdp.rx_queue, &packet, &pdTrue); - -} - -static void csp_rdp_flush_eack(csp_conn_t * conn, csp_packet_t * eack_packet) { - - /* Loop through TX queue */ - int i, j, count; - rdp_packet_t * packet; - count = csp_queue_size(conn->rdp.tx_queue); - for (i = 0; i < count; i++) { - - if (csp_queue_dequeue(conn->rdp.tx_queue, &packet, 0) != CSP_QUEUE_OK) { - csp_log_error("Cannot dequeue from tx_queue in flush EACK"); - break; - } - - rdp_header_t * header = csp_rdp_header_ref((csp_packet_t *) packet); - csp_log_protocol("EACK compare element, time %u, seq %u", packet->timestamp, csp_ntoh16(header->seq_nr)); - - /* Look for this element in EACKs */ - int match = 0; - for (j = 0; j < (int)((eack_packet->length - sizeof(rdp_header_t)) / sizeof(uint16_t)); j++) { - if (csp_ntoh16(eack_packet->data16[j]) == csp_ntoh16(header->seq_nr)) - match = 1; - - /* Enable this if you want EACK's to trigger retransmission */ - if (csp_ntoh16(eack_packet->data16[j]) > csp_ntoh16(header->seq_nr)) { - uint32_t time_now = csp_get_ms(); - if (csp_rdp_time_after(time_now, packet->quarantine)) { - packet->timestamp = time_now - conn->rdp.packet_timeout - 1; - packet->quarantine = time_now + conn->rdp.packet_timeout / 2; - } - } - } - - if (match == 0) { - /* If not found, put back on tx queue */ - csp_queue_enqueue(conn->rdp.tx_queue, &packet, 0); - } else { - /* Found, free */ - csp_log_protocol("TX Element %u freed", csp_ntoh16(header->seq_nr)); - csp_buffer_free(packet); - } - - } - -} - -static inline bool csp_rdp_should_ack(csp_conn_t * conn) { - - /* If delayed ACKs are not used, always ACK */ - if (!conn->rdp.delayed_acks) { - return true; - } - - /* ACK if time since last ACK is greater than ACK timeout */ - uint32_t time_now = csp_get_ms(); - if (csp_rdp_time_after(time_now, conn->rdp.ack_timestamp + conn->rdp.ack_timeout)) - return true; - - /* ACK if number of unacknowledged packets is greater than delay count */ - if (csp_rdp_seq_after(conn->rdp.rcv_cur, conn->rdp.rcv_lsa + conn->rdp.ack_delay_count)) - return true; - - return false; - -} - -void csp_rdp_flush_all(csp_conn_t * conn) { - - if ((conn == NULL) || conn->rdp.tx_queue == NULL) { - csp_log_error("Null pointer passed to rdp flush all"); - return; - } - - rdp_packet_t * packet; - - /* Empty TX queue */ - while (csp_queue_dequeue_isr(conn->rdp.tx_queue, &packet, &pdTrue) == CSP_QUEUE_OK) { - if (packet != NULL) { - csp_log_protocol("RDP %p: Flush TX Element, time %u, seq %u", conn, packet->timestamp, csp_ntoh16(csp_rdp_header_ref((csp_packet_t *) packet)->seq_nr)); - csp_buffer_free(packet); - } - } - - /* Empty RX queue */ - while (csp_queue_dequeue_isr(conn->rdp.rx_queue, &packet, &pdTrue) == CSP_QUEUE_OK) { - if (packet != NULL) { - csp_log_protocol("RDP %p: Flush RX Element, time %u, seq %u", conn, packet->timestamp, csp_ntoh16(csp_rdp_header_ref((csp_packet_t *) packet)->seq_nr)); - csp_buffer_free(packet); - } - } - -} - - -int csp_rdp_check_ack(csp_conn_t * conn) { - - /* Check all RX queues for spare capacity */ - int prio, avail = 1; - for (prio = 0; prio < CSP_RX_QUEUES; prio++) { - if (CSP_RX_QUEUE_LENGTH - csp_queue_size(conn->rx_queue[prio]) <= 2 * (int32_t)conn->rdp.window_size) { - avail = 0; - break; - } - } - - /* If more space available, only send after ack timeout or immediately if delay_acks is zero */ - if (avail && csp_rdp_should_ack(conn)) - csp_rdp_send_cmp(conn, NULL, RDP_ACK, conn->rdp.snd_nxt, conn->rdp.rcv_cur); - - return CSP_ERR_NONE; - -} - -static inline bool csp_rdp_is_conn_ready_for_tx(csp_conn_t * conn) -{ - // Check Tx window (messages waiting for acks) - if (csp_rdp_seq_after(conn->rdp.snd_nxt, conn->rdp.snd_una + conn->rdp.window_size - 1)) { - return false; - } - return true; -} - -/** - * This function must be called with regular intervals for the - * RDP protocol to work as expected. This takes care of closing - * stale connections and retransmitting traffic. A good place to - * call this function is from the CSP router task. - */ -void csp_rdp_check_timeouts(csp_conn_t * conn) { - - rdp_packet_t * packet; - - /** - * CONNECTION TIMEOUT: - * Check that connection has not timed out inside the network stack - * */ - uint32_t time_now = csp_get_ms(); - if (conn->socket != NULL) { - if (csp_rdp_time_after(time_now, conn->timestamp + conn->rdp.conn_timeout)) { - csp_log_warn("RDP %p: Found a lost connection (now: %u, ts: %u, to: %u), closing now", - conn, time_now, conn->timestamp, conn->rdp.conn_timeout); - csp_close(conn); - return; - } - } - - /** - * CLOSE-WAIT TIMEOUT: - * After waiting a while in CLOSE-WAIT, the connection should be closed. - */ - if (conn->rdp.state == RDP_CLOSE_WAIT) { - if (csp_rdp_time_after(time_now, conn->timestamp + conn->rdp.conn_timeout)) { - csp_log_protocol("RDP %p: CLOSE_WAIT timeout", conn); - csp_close(conn); - } - return; - } - - /** - * MESSAGE TIMEOUT: - * Check each outgoing message for TX timeout - */ - int i, count; - count = csp_queue_size(conn->rdp.tx_queue); - for (i = 0; i < count; i++) { - - if ((csp_queue_dequeue_isr(conn->rdp.tx_queue, &packet, &pdTrue) != CSP_QUEUE_OK) || packet == NULL) { - csp_log_warn("Cannot dequeue from tx_queue in check timeout"); - break; - } - - /* Get header */ - rdp_header_t * header = csp_rdp_header_ref((csp_packet_t *) packet); - - /* If acked, do not retransmit */ - if (csp_rdp_seq_before(csp_ntoh16(header->seq_nr), conn->rdp.snd_una)) { - csp_log_protocol("TX Element Free, time %u, seq %u, una %u", packet->timestamp, csp_ntoh16(header->seq_nr), conn->rdp.snd_una); - csp_buffer_free(packet); - continue; - } - - /* Check timestamp and retransmit if needed */ - if (csp_rdp_time_after(time_now, packet->timestamp + conn->rdp.packet_timeout)) { - csp_log_protocol("TX Element timed out, retransmitting seq %u", csp_ntoh16(header->seq_nr)); - - /* Update to latest outgoing ACK */ - header->ack_nr = csp_hton16(conn->rdp.rcv_cur); - - /* Send copy to tx_queue */ - packet->timestamp = csp_get_ms(); - csp_packet_t * new_packet = csp_buffer_clone(packet); - csp_iface_t * ifout = csp_rtable_find_iface(conn->idout.dst); - if (csp_send_direct(conn->idout, new_packet, ifout, 0) != CSP_ERR_NONE) { - csp_log_warn("Retransmission failed"); - csp_buffer_free(new_packet); - } - - } - - /* Requeue the TX element */ - csp_queue_enqueue_isr(conn->rdp.tx_queue, &packet, &pdTrue); - - } - - /** - * ACK TIMEOUT: - * Check ACK timeouts, if we have unacknowledged segments - */ - if (conn->rdp.delayed_acks) { - csp_rdp_check_ack(conn); - } - - /* Wake user task if connection is open and additional Tx can be done */ - if ((conn->rdp.state == RDP_OPEN) && csp_rdp_is_conn_ready_for_tx(conn)) { - csp_log_protocol("RDP %p: Wake Tx task (check timeouts)", conn); - csp_bin_sem_post(&conn->rdp.tx_wait); - } -} - -void csp_rdp_new_packet(csp_conn_t * conn, csp_packet_t * packet) { - - /* Get RX header and convert to host byte-order */ - rdp_header_t * rx_header = csp_rdp_header_ref(packet); - rx_header->ack_nr = csp_ntoh16(rx_header->ack_nr); - rx_header->seq_nr = csp_ntoh16(rx_header->seq_nr); - - csp_log_protocol("RDP %p: Received in S %u: syn %u, ack %u, eack %u, " - "rst %u, seq_nr %5u, ack_nr %5u, packet_len %u (%u)", - conn, conn->rdp.state, rx_header->syn, rx_header->ack, rx_header->eak, - rx_header->rst, rx_header->seq_nr, rx_header->ack_nr, - packet->length, packet->length - sizeof(rdp_header_t)); - - /* If a RESET was received. */ - if (rx_header->rst) { - - if (rx_header->ack) { - /* Store current ack'ed sequence number */ - conn->rdp.snd_una = rx_header->ack_nr + 1; - } - - if (conn->rdp.state == RDP_CLOSE_WAIT || conn->rdp.state == RDP_CLOSED) { - csp_log_protocol("RDP %p: RST received in CLOSE_WAIT or CLOSED. Now closing connection", conn); - goto discard_close; - } else { - csp_log_protocol("RDP %p: Got RESET in state %u", conn, conn->rdp.state); - - if (rx_header->seq_nr == (uint16_t)(conn->rdp.rcv_cur + 1)) { - csp_log_protocol("RDP %p: RESET in sequence, no more data incoming, reply with RESET", conn); - conn->rdp.state = RDP_CLOSE_WAIT; - conn->timestamp = csp_get_ms(); - csp_rdp_send_cmp(conn, NULL, RDP_ACK | RDP_RST, conn->rdp.snd_nxt, conn->rdp.rcv_cur); - goto discard_close; - } else { - csp_log_protocol("RDP %p: RESET out of sequence, keep connection open", conn); - goto discard_open; - } - } - } - - /* The BIG FAT switch (state-machine) */ - switch(conn->rdp.state) { - - /** - * STATE == CLOSED - */ - case RDP_CLOSED: { - - /* No SYN flag set while in closed. Inform by sending back RST */ - if (!rx_header->syn) { - csp_log_protocol("Not SYN received in CLOSED state. Discarding packet"); - csp_rdp_send_cmp(conn, NULL, RDP_RST, conn->rdp.snd_nxt, conn->rdp.rcv_cur); - goto discard_close; - } - - csp_log_protocol("RDP: SYN-Received"); - - /* Setup TX seq. */ - conn->rdp.snd_iss = (uint16_t)rand(); - conn->rdp.snd_nxt = conn->rdp.snd_iss + 1; - conn->rdp.snd_una = conn->rdp.snd_iss; - - /* Store RX seq. */ - conn->rdp.rcv_cur = rx_header->seq_nr; - conn->rdp.rcv_irs = rx_header->seq_nr; - conn->rdp.rcv_lsa = rx_header->seq_nr; - - /* Store RDP options */ - conn->rdp.window_size = csp_ntoh32(packet->data32[0]); - conn->rdp.conn_timeout = csp_ntoh32(packet->data32[1]); - conn->rdp.packet_timeout = csp_ntoh32(packet->data32[2]); - conn->rdp.delayed_acks = csp_ntoh32(packet->data32[3]); - conn->rdp.ack_timeout = csp_ntoh32(packet->data32[4]); - conn->rdp.ack_delay_count = csp_ntoh32(packet->data32[5]); - csp_log_protocol("RDP: Window Size %u, conn timeout %u, packet timeout %u", - conn->rdp.window_size, conn->rdp.conn_timeout, conn->rdp.packet_timeout); - csp_log_protocol("RDP: Delayed acks: %u, ack timeout %u, ack each %u packet", - conn->rdp.delayed_acks, conn->rdp.ack_timeout, conn->rdp.ack_delay_count); - - /* Connection accepted */ - conn->rdp.state = RDP_SYN_RCVD; - - /* Send SYN/ACK */ - csp_rdp_send_cmp(conn, NULL, RDP_ACK | RDP_SYN, conn->rdp.snd_iss, conn->rdp.rcv_irs); - - goto discard_open; - - } - break; - - /** - * STATE == SYN-SENT - */ - case RDP_SYN_SENT: { - - /* First check SYN/ACK */ - if (rx_header->syn && rx_header->ack) { - - conn->rdp.rcv_cur = rx_header->seq_nr; - conn->rdp.rcv_irs = rx_header->seq_nr; - conn->rdp.rcv_lsa = rx_header->seq_nr - 1; - conn->rdp.snd_una = rx_header->ack_nr + 1; - conn->rdp.ack_timestamp = csp_get_ms(); - conn->rdp.state = RDP_OPEN; - - csp_log_protocol("RDP: NP: Connection OPEN"); - - /* Send ACK */ - csp_rdp_send_cmp(conn, NULL, RDP_ACK, conn->rdp.snd_nxt, conn->rdp.rcv_cur); - - /* Wake TX task */ - csp_log_protocol("RDP %p: Wake Tx task (ack)", conn); - csp_bin_sem_post(&conn->rdp.tx_wait); - - goto discard_open; - } - - /* If there was no SYN in the reply, our SYN message hit an already open connection - * This is handled by sending a RST. - * Normally this would be followed up by a new connection attempt, however - * we don't have a method for signaling this to the user space. - */ - if (rx_header->ack) { - csp_log_error("Half-open connection found, sending RST"); - csp_rdp_send_cmp(conn, NULL, RDP_RST, conn->rdp.snd_nxt, conn->rdp.rcv_cur); - csp_log_protocol("RDP %p: Wake Tx task (rst)", conn); - csp_bin_sem_post(&conn->rdp.tx_wait); - - goto discard_open; - } - - /* Otherwise we have an invalid command, such as a SYN reply to a SYN command, - * indicating simultaneous connections, which is not possible in the way CSP - * reserves some ports for server and some for clients. - */ - csp_log_error("Invalid reply to SYN request"); - goto discard_close; - - } - break; - - /** - * STATE == OPEN - */ - case RDP_SYN_RCVD: - case RDP_OPEN: - { - - /* SYN or !ACK is invalid */ - if (rx_header->syn || !rx_header->ack) { - if (rx_header->seq_nr != conn->rdp.rcv_irs) { - csp_log_error("Invalid SYN or no ACK, resetting!"); - goto discard_close; - } else { - csp_log_protocol("Ignoring duplicate SYN packet!"); - goto discard_open; - } - } - - /* Check sequence number */ - if (!csp_rdp_seq_between(rx_header->seq_nr, conn->rdp.rcv_cur + 1, conn->rdp.rcv_cur + conn->rdp.window_size * 2)) { - csp_log_protocol("Invalid sequence number! %"PRIu16" not between %"PRIu16" and %"PRIu16, - rx_header->seq_nr, conn->rdp.rcv_cur + 1, conn->rdp.rcv_cur + 1 + conn->rdp.window_size * 2); - /* If duplicate SYN received, send another SYN/ACK */ - if (conn->rdp.state == RDP_SYN_RCVD) - csp_rdp_send_cmp(conn, NULL, RDP_ACK | RDP_SYN, conn->rdp.snd_iss, conn->rdp.rcv_irs); - /* If duplicate data packet received, send EACK back */ - if (conn->rdp.state == RDP_OPEN) - csp_rdp_send_eack(conn); - - goto discard_open; - } - - /* Check ACK number */ - if (!csp_rdp_seq_between(rx_header->ack_nr, conn->rdp.snd_una - 1 - (conn->rdp.window_size * 2), conn->rdp.snd_nxt - 1)) { - csp_log_error("Invalid ACK number! %u not between %u and %u", - rx_header->ack_nr, conn->rdp.snd_una - 1 - (conn->rdp.window_size * 2), conn->rdp.snd_nxt - 1); - goto discard_open; - } - - /* Check SYN_RCVD ACK */ - if (conn->rdp.state == RDP_SYN_RCVD) { - if (rx_header->ack_nr != conn->rdp.snd_iss) { - csp_log_error("SYN-RCVD: Wrong ACK number"); - goto discard_close; - } - csp_log_protocol("RDP: NC: Connection OPEN"); - conn->rdp.state = RDP_OPEN; - - /* If a socket is set, this message is the first in a new connection - * so the connection must be queued to the socket. */ - if (conn->socket != NULL) { - - /* Try queueing */ - if (csp_queue_enqueue(conn->socket, &conn, 0) == CSP_QUEUE_FULL) { - csp_log_error("ERROR socket cannot accept more connections"); - goto discard_close; - } - - /* Ensure that this connection will not be posted to this socket again - * and remember that the connection handle has been passed to userspace - * by setting the socket = NULL */ - conn->socket = NULL; - } - - } - - /* Store current ack'ed sequence number */ - conn->rdp.snd_una = rx_header->ack_nr + 1; - - /* We have an EACK */ - if (rx_header->eak) { - if (packet->length > sizeof(rdp_header_t)) - csp_rdp_flush_eack(conn, packet); - goto discard_open; - } - - /* If no data, return here */ - if (packet->length <= sizeof(rdp_header_t)) - goto discard_open; - - /* If message is not in sequence, send EACK and store packet */ - if (rx_header->seq_nr != (uint16_t)(conn->rdp.rcv_cur + 1)) { - if (csp_rdp_rx_queue_add(conn, packet, rx_header->seq_nr) != CSP_QUEUE_OK) { - csp_log_protocol("Duplicate sequence number"); - csp_rdp_check_ack(conn); - goto discard_open; - } - csp_rdp_send_eack(conn); - goto accepted_open; - } - - /* Store sequence number before stripping RDP header */ - uint16_t seq_nr = rx_header->seq_nr; - - /* Receive data */ - if (csp_rdp_receive_data(conn, packet) != CSP_ERR_NONE) - goto discard_open; - - /* Update last received packet */ - conn->rdp.rcv_cur = seq_nr; - - /* Only ACK the message if there is room for a full window in the RX buffer. - * Unacknowledged segments are ACKed by csp_rdp_check_timeouts when the buffer is - * no longer full. */ - csp_rdp_check_ack(conn); - - /* Flush RX queue */ - csp_rdp_rx_queue_flush(conn); - - goto accepted_open; - - } - break; - - case RDP_CLOSE_WAIT: - - /* Ignore SYN or !ACK */ - if (rx_header->syn || !rx_header->ack) { - csp_log_protocol("Invalid SYN or no ACK in CLOSE-WAIT"); - goto discard_open; - } - - /* Check ACK number */ - if (!csp_rdp_seq_between(rx_header->ack_nr, conn->rdp.snd_una - 1 - (conn->rdp.window_size * 2), conn->rdp.snd_nxt - 1)) { - csp_log_error("Invalid ACK number! %u not between %u and %u", - rx_header->ack_nr, conn->rdp.snd_una - 1 - (conn->rdp.window_size * 2), conn->rdp.snd_nxt - 1); - goto discard_open; - } - - /* Store current ack'ed sequence number */ - conn->rdp.snd_una = rx_header->ack_nr + 1; - - /* Send back a reset */ - csp_rdp_send_cmp(conn, NULL, RDP_ACK | RDP_RST, conn->rdp.snd_nxt, conn->rdp.rcv_cur); - - goto discard_open; - - default: - csp_log_error("RDP: ERROR default state!"); - goto discard_close; - } - -discard_close: - /* If user-space has received the connection handle, wake it up, - * by sending a NULL pointer, user-space should close connection */ - if (conn->socket == NULL) { - csp_log_protocol("RDP %p: Waiting for userspace to close", conn); - csp_conn_enqueue_packet(conn, NULL); - } else { - csp_close(conn); - } - -discard_open: - csp_buffer_free(packet); -accepted_open: - return; - -} - -int csp_rdp_connect(csp_conn_t * conn, uint32_t timeout) { - - int retry = 1; - - conn->rdp.window_size = csp_rdp_window_size; - conn->rdp.conn_timeout = csp_rdp_conn_timeout; - conn->rdp.packet_timeout = csp_rdp_packet_timeout; - conn->rdp.delayed_acks = csp_rdp_delayed_acks; - conn->rdp.ack_timeout = csp_rdp_ack_timeout; - conn->rdp.ack_delay_count = csp_rdp_ack_delay_count; - conn->rdp.ack_timestamp = csp_get_ms(); - -retry: - csp_log_protocol("RDP %p: Active connect, conn state %u", conn, conn->rdp.state); - - if (conn->rdp.state == RDP_OPEN) { - csp_log_error("RDP %p: Connection already open", conn); - return CSP_ERR_ALREADY; - } - - /* Randomize ISS */ - conn->rdp.snd_iss = (uint16_t)rand(); - - conn->rdp.snd_nxt = conn->rdp.snd_iss + 1; - conn->rdp.snd_una = conn->rdp.snd_iss; - - csp_log_protocol("RDP %p: AC: Sending SYN", conn); - - /* Ensure semaphore is busy, so router task can release it */ - csp_bin_sem_wait(&conn->rdp.tx_wait, 0); - - /* Send SYN message */ - conn->rdp.state = RDP_SYN_SENT; - if (csp_rdp_send_syn(conn) != CSP_ERR_NONE) - goto error; - - /* Wait for router task to release semaphore */ - csp_log_protocol("RDP %p: AC: Waiting for SYN/ACK reply...", conn); - int result = csp_bin_sem_wait(&conn->rdp.tx_wait, conn->rdp.conn_timeout); - - if (result == CSP_SEMAPHORE_OK) { - if (conn->rdp.state == RDP_OPEN) { - csp_log_protocol("RDP %p: AC: Connection OPEN", conn); - return CSP_ERR_NONE; - } else if(conn->rdp.state == RDP_SYN_SENT) { - if (retry) { - csp_log_warn("RDP %p: Half-open connection detected, RST sent, now retrying", conn); - csp_rdp_flush_all(conn); - retry = 0; - goto retry; - } else { - csp_log_error("RDP %p: Connection stayed half-open, even after RST and retry!", conn); - goto error; - } - } - } else { - csp_log_protocol("RDP %p: AC: Connection Failed", conn); - goto error; - } - -error: - conn->rdp.state = RDP_CLOSE_WAIT; - return CSP_ERR_TIMEDOUT; - -} - -int csp_rdp_send(csp_conn_t * conn, csp_packet_t * packet, uint32_t timeout) { - - if (conn->rdp.state != RDP_OPEN) { - csp_log_error("RDP: ERROR cannot send, connection reset"); - return CSP_ERR_RESET; - } - - while ((conn->rdp.state == RDP_OPEN) && (csp_rdp_is_conn_ready_for_tx(conn) == false)) { - csp_log_protocol("RDP %p: Waiting for window update before sending seq %u", conn, conn->rdp.snd_nxt); - if ((csp_bin_sem_wait(&conn->rdp.tx_wait, conn->rdp.conn_timeout)) != CSP_SEMAPHORE_OK) { - csp_log_error("RDP %p: Timeout during send", conn); - return CSP_ERR_TIMEDOUT; - } - } - - if (conn->rdp.state != RDP_OPEN) { - csp_log_error("RDP: ERROR cannot send, connection reset"); - return CSP_ERR_RESET; - } - - /* Add RDP header */ - rdp_header_t * tx_header = csp_rdp_header_add(packet); - tx_header->ack_nr = csp_hton16(conn->rdp.rcv_cur); - tx_header->seq_nr = csp_hton16(conn->rdp.snd_nxt); - tx_header->ack = 1; - - /* Send copy to tx_queue */ - rdp_packet_t * rdp_packet = csp_buffer_clone(packet); - if (rdp_packet == NULL) { - csp_log_error("Failed to allocate packet buffer"); - return CSP_ERR_NOMEM; - } - - rdp_packet->timestamp = csp_get_ms(); - rdp_packet->quarantine = 0; - if (csp_queue_enqueue(conn->rdp.tx_queue, &rdp_packet, 0) != CSP_QUEUE_OK) { - csp_log_error("No more space in RDP retransmit queue"); - csp_buffer_free(rdp_packet); - return CSP_ERR_NOBUFS; - } - - csp_log_protocol("RDP: Sending in S %u: syn %u, ack %u, eack %u, " - "rst %u, seq_nr %5u, ack_nr %5u, packet_len %u (%u)", - conn->rdp.state, tx_header->syn, tx_header->ack, tx_header->eak, - tx_header->rst, csp_ntoh16(tx_header->seq_nr), csp_ntoh16(tx_header->ack_nr), - packet->length, packet->length - sizeof(rdp_header_t)); - - conn->rdp.snd_nxt++; - return CSP_ERR_NONE; - -} - -int csp_rdp_allocate(csp_conn_t * conn) { - - csp_log_protocol("RDP: Creating RDP queues for conn %p", conn); - - /* Set initial state */ - conn->rdp.state = RDP_CLOSED; - conn->rdp.conn_timeout = csp_rdp_conn_timeout; - conn->rdp.packet_timeout = csp_rdp_packet_timeout; - - /* Create a binary semaphore to wait on for tasks */ - if (csp_bin_sem_create(&conn->rdp.tx_wait) != CSP_SEMAPHORE_OK) { - csp_log_error("Failed to initialize semaphore"); - return CSP_ERR_NOMEM; - } - - /* Create TX queue */ - conn->rdp.tx_queue = csp_queue_create(CSP_RDP_MAX_WINDOW, sizeof(csp_packet_t *)); - if (conn->rdp.tx_queue == NULL) { - csp_log_error("Failed to create TX queue for conn"); - csp_bin_sem_remove(&conn->rdp.tx_wait); - return CSP_ERR_NOMEM; - } - - /* Create RX queue */ - conn->rdp.rx_queue = csp_queue_create(CSP_RDP_MAX_WINDOW * 2, sizeof(csp_packet_t *)); - if (conn->rdp.rx_queue == NULL) { - csp_log_error("Failed to create RX queue for conn"); - csp_bin_sem_remove(&conn->rdp.tx_wait); - csp_queue_remove(conn->rdp.tx_queue); - return CSP_ERR_NOMEM; - } - - return CSP_ERR_NONE; - -} - -/** - * @note This function may only be called from csp_close, and is therefore - * without any checks for null pointers. - */ -int csp_rdp_close(csp_conn_t * conn) { - - if (conn->rdp.state == RDP_CLOSED) - return CSP_ERR_NONE; - - /* If message is open, send reset */ - if (conn->rdp.state != RDP_CLOSE_WAIT) { - conn->rdp.state = RDP_CLOSE_WAIT; - conn->timestamp = csp_get_ms(); - csp_rdp_send_cmp(conn, NULL, RDP_ACK | RDP_RST, conn->rdp.snd_nxt, conn->rdp.rcv_cur); - csp_log_protocol("RDP %p: Close, sent RST", conn); - csp_bin_sem_post(&conn->rdp.tx_wait); // wake up any pendng Tx - return CSP_ERR_AGAIN; - } - - csp_log_protocol("RDP %p: Close in CLOSE_WAIT, now closing", conn); - conn->rdp.state = RDP_CLOSED; - return CSP_ERR_NONE; - -} - -/** - * RDP Set socket options - * Controls important parameters of the RDP protocol. - * These settings will be applied to all new outgoing connections. - * The settings are global, so be sure no other task are conflicting with your settings. - */ -void csp_rdp_set_opt(unsigned int window_size, unsigned int conn_timeout_ms, - unsigned int packet_timeout_ms, unsigned int delayed_acks, - unsigned int ack_timeout, unsigned int ack_delay_count) { - csp_rdp_window_size = window_size; - csp_rdp_conn_timeout = conn_timeout_ms; - csp_rdp_packet_timeout = packet_timeout_ms; - csp_rdp_delayed_acks = delayed_acks; - csp_rdp_ack_timeout = ack_timeout; - csp_rdp_ack_delay_count = ack_delay_count; -} - -void csp_rdp_get_opt(unsigned int * window_size, unsigned int * conn_timeout_ms, - unsigned int * packet_timeout_ms, unsigned int * delayed_acks, - unsigned int * ack_timeout, unsigned int * ack_delay_count) { - - if (window_size) - *window_size = csp_rdp_window_size; - if (conn_timeout_ms) - *conn_timeout_ms = csp_rdp_conn_timeout; - if (packet_timeout_ms) - *packet_timeout_ms = csp_rdp_packet_timeout; - if (delayed_acks) - *delayed_acks = csp_rdp_delayed_acks; - if (ack_timeout) - *ack_timeout = csp_rdp_ack_timeout; - if (ack_delay_count) - *ack_delay_count = csp_rdp_ack_delay_count; -} - -#ifdef CSP_DEBUG -void csp_rdp_conn_print(csp_conn_t * conn) { - - if (conn == NULL) - return; - - printf("\tRDP: State %"PRIu16", rcv %"PRIu16", snd %"PRIu16", win %"PRIu32"\r\n", - conn->rdp.state, conn->rdp.rcv_cur, conn->rdp.snd_una, conn->rdp.window_size); - -} -#endif - -#endif diff --git a/thirdparty/libcsp/src/transport/csp_transport.h b/thirdparty/libcsp/src/transport/csp_transport.h deleted file mode 100644 index 7fcda3dc..00000000 --- a/thirdparty/libcsp/src/transport/csp_transport.h +++ /dev/null @@ -1,46 +0,0 @@ -/* -Cubesat Space Protocol - A small network-layer protocol designed for Cubesats -Copyright (C) 2012 GomSpace ApS (http://www.gomspace.com) -Copyright (C) 2012 AAUSAT3 Project (http://aausat3.space.aau.dk) - -This library is free software; you can redistribute it and/or -modify it under the terms of the GNU Lesser General Public -License as published by the Free Software Foundation; either -version 2.1 of the License, or (at your option) any later version. - -This library is distributed in the hope that it will be useful, -but WITHOUT ANY WARRANTY; without even the implied warranty of -MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU -Lesser General Public License for more details. - -You should have received a copy of the GNU Lesser General Public -License along with this library; if not, write to the Free Software -Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA -*/ - -#ifndef _CSP_TRANSPORT_H_ -#define _CSP_TRANSPORT_H_ - -#ifdef __cplusplus -extern "C" { -#endif - -/** ARRIVING SEGMENT */ -void csp_udp_new_packet(csp_conn_t * conn, csp_packet_t * packet); -void csp_rdp_new_packet(csp_conn_t * conn, csp_packet_t * packet); - -/** RDP: USER REQUESTS */ -int csp_rdp_connect(csp_conn_t * conn, uint32_t timeout); -int csp_rdp_allocate(csp_conn_t * conn); -int csp_rdp_close(csp_conn_t * conn); -void csp_rdp_conn_print(csp_conn_t * conn); -int csp_rdp_send(csp_conn_t * conn, csp_packet_t * packet, uint32_t timeout); -int csp_rdp_check_ack(csp_conn_t * conn); -void csp_rdp_check_timeouts(csp_conn_t * conn); -void csp_rdp_flush_all(csp_conn_t * conn); - -#ifdef __cplusplus -} /* extern "C" */ -#endif - -#endif /* _CSP_TRANSPORT_H_ */ diff --git a/thirdparty/libcsp/src/transport/csp_udp.c b/thirdparty/libcsp/src/transport/csp_udp.c deleted file mode 100644 index 61732703..00000000 --- a/thirdparty/libcsp/src/transport/csp_udp.c +++ /dev/null @@ -1,49 +0,0 @@ -/* -Cubesat Space Protocol - A small network-layer protocol designed for Cubesats -Copyright (C) 2012 GomSpace ApS (http://www.gomspace.com) -Copyright (C) 2012 AAUSAT3 Project (http://aausat3.space.aau.dk) - -This library is free software; you can redistribute it and/or -modify it under the terms of the GNU Lesser General Public -License as published by the Free Software Foundation; either -version 2.1 of the License, or (at your option) any later version. - -This library is distributed in the hope that it will be useful, -but WITHOUT ANY WARRANTY; without even the implied warranty of -MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU -Lesser General Public License for more details. - -You should have received a copy of the GNU Lesser General Public -License along with this library; if not, write to the Free Software -Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA -*/ - -#include -#include -#include "../csp_port.h" -#include "../csp_conn.h" -#include "csp_transport.h" - -void csp_udp_new_packet(csp_conn_t * conn, csp_packet_t * packet) { - - /* Enqueue */ - if (csp_conn_enqueue_packet(conn, packet) < 0) { - csp_log_error("Connection buffer queue full!"); - csp_buffer_free(packet); - return; - } - - /* Try to queue up the new connection pointer */ - if (conn->socket != NULL) { - if (csp_queue_enqueue(conn->socket, &conn, 0) != CSP_QUEUE_OK) { - csp_log_warn("Warning socket connection queue full"); - csp_close(conn); - return; - } - - /* Ensure that this connection will not be posted to this socket again */ - conn->socket = NULL; - } - -} - diff --git a/thirdparty/libcsp/utils/cfpsplit.py b/thirdparty/libcsp/utils/cfpsplit.py deleted file mode 100644 index 9a350e3e..00000000 --- a/thirdparty/libcsp/utils/cfpsplit.py +++ /dev/null @@ -1,52 +0,0 @@ -#!/usr/bin/env python - -# Cubesat Space Protocol - A small network-layer protocol designed for Cubesats -# Copyright (C) 2012 GomSpace ApS (http://www.gomspace.com) -# Copyright (C) 2012 AAUSAT3 Project (http://aausat3.space.aau.dk) -# -# This library is free software; you can redistribute it and/or -# modify it under the terms of the GNU Lesser General Public -# License as published by the Free Software Foundation; either -# version 2.1 of the License, or (at your option) any later version. -# -# This library is distributed in the hope that it will be useful, -# but WITHOUT ANY WARRANTY; without even the implied warranty of -# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU -# Lesser General Public License for more details. -# -# You should have received a copy of the GNU Lesser General Public -# License along with this library; if not, write to the Free Software -# Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA - -# Split CFP header in protocol fields - -import sys - -def usage(): - print("usage: cfpsplit.py HEADER") - -def main(): - try: - hdr = sys.argv[1] - except: - usage() - sys.exit(-1) - - try: - hdrhex = int(hdr, 16) - except: - print("HEADER must be in hexadecimal format") - sys.exit(-1) - - if hdrhex > 0x1fffffff: - print("HEADER is not a valid CFP header") - sys.exit(-1) - - print("Source: {0}".format((hdrhex >> 24) & 0x1f)) - print("Destination: {0}".format((hdrhex >> 19) & 0x1f)) - print("Type: {0}".format("MORE" if ((hdrhex >> 18) & 0x01) else "BEGIN")) - print("Remain: {0}".format((hdrhex >> 10) & 0xff)) - print("Identifier: {0}".format((hdrhex >> 0) & 0x3ff)) - -if __name__ == "__main__": - main() diff --git a/thirdparty/libcsp/utils/cspsplit.py b/thirdparty/libcsp/utils/cspsplit.py deleted file mode 100644 index f4ed942f..00000000 --- a/thirdparty/libcsp/utils/cspsplit.py +++ /dev/null @@ -1,52 +0,0 @@ -#!/usr/bin/env python - -# Cubesat Space Protocol - A small network-layer protocol designed for Cubesats -# Copyright (C) 2012 GomSpace ApS (http://www.gomspace.com) -# Copyright (C) 2012 AAUSAT3 Project (http://aausat3.space.aau.dk) -# -# This library is free software; you can redistribute it and/or -# modify it under the terms of the GNU Lesser General Public -# License as published by the Free Software Foundation; either -# version 2.1 of the License, or (at your option) any later version. -# -# This library is distributed in the hope that it will be useful, -# but WITHOUT ANY WARRANTY; without even the implied warranty of -# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU -# Lesser General Public License for more details. -# -# You should have received a copy of the GNU Lesser General Public -# License along with this library; if not, write to the Free Software -# Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA - -# Split CSP header in protocol fields - -import sys - -def usage(): - print("usage: cspsplit.py HEADER") - -def main(): - try: - hdr = sys.argv[1] - except: - usage() - sys.exit(-1) - - try: - hdrhex = int(hdr, 16) - except: - print("HEADER must be in hexadecimal format") - sys.exit(-1) - - print("Priotity: {0}".format((hdrhex >> 30) & 0x03)) - print("Source: {0}".format((hdrhex >> 25) & 0x1f)) - print("Destination: {0}".format((hdrhex >> 20) & 0x1f)) - print("Destination port: {0}".format((hdrhex >> 14) & 0x3f)) - print("Source port: {0}".format((hdrhex >> 8) & 0x3f)) - print("HMAC: {0}".format("Yes" if ((hdrhex >> 3) & 0x01) else "No")) - print("XTEA: {0}".format("Yes" if ((hdrhex >> 2) & 0x01) else "No")) - print("RDP: {0}".format("Yes" if ((hdrhex >> 1) & 0x01) else "No")) - print("CRC32: {0}".format("Yes" if ((hdrhex >> 0) & 0x01) else "No")) - -if __name__ == "__main__": - main() From 65ec5c35e37b3303b5fe8b3f9cd12a9755d30212 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 25 Aug 2022 20:57:04 +0200 Subject: [PATCH 14/52] another try --- .gitmodules | 3 +++ thirdparty/gomspace-sw | 1 + 2 files changed, 4 insertions(+) create mode 160000 thirdparty/gomspace-sw diff --git a/.gitmodules b/.gitmodules index ac6824b8..50e2e6e1 100644 --- a/.gitmodules +++ b/.gitmodules @@ -22,3 +22,6 @@ [submodule "thirdparty/rapidcsv"] path = thirdparty/rapidcsv url = https://github.com/d99kris/rapidcsv.git +[submodule "thirdparty/gomspace-sw"] + path = thirdparty/gomspace-sw + url = https://egit.irs.uni-stuttgart.de/eive/gomspace-sw.git diff --git a/thirdparty/gomspace-sw b/thirdparty/gomspace-sw new file mode 160000 index 00000000..399c9a3e --- /dev/null +++ b/thirdparty/gomspace-sw @@ -0,0 +1 @@ +Subproject commit 399c9a3eeefcfb2ad589d260f9f5f8b139c74d30 From f264b9781b08275fdc3c85870a07bcf01aaf9b1e Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 25 Aug 2022 20:58:07 +0200 Subject: [PATCH 15/52] bump gomspace-sw submodule --- thirdparty/gomspace-sw | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/thirdparty/gomspace-sw b/thirdparty/gomspace-sw index 399c9a3e..47059ea5 160000 --- a/thirdparty/gomspace-sw +++ b/thirdparty/gomspace-sw @@ -1 +1 @@ -Subproject commit 399c9a3eeefcfb2ad589d260f9f5f8b139c74d30 +Subproject commit 47059ea593dda32e78e59ebd24902ea60db00448 From ad783018bd66866a4de2bb88fbafe9de929fb697 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 25 Aug 2022 20:59:35 +0200 Subject: [PATCH 16/52] add some more compile options --- CMakeLists.txt | 1 + 1 file changed, 1 insertion(+) diff --git a/CMakeLists.txt b/CMakeLists.txt index bc1300d0..5d14f7b3 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -327,6 +327,7 @@ if(CMAKE_CXX_COMPILER_ID STREQUAL "GNU") ) target_compile_options(${OBSW_NAME} PRIVATE ${WARNING_FLAGS}) target_compile_options(${LIB_EIVE_MISSION} PRIVATE ${WARNING_FLAGS}) + target_compile_options(${LIB_DUMMIES} PRIVATE ${WARNING_FLAGS}) endif() set_target_properties(${OBSW_NAME} PROPERTIES OUTPUT_NAME ${OBSW_BIN_NAME}) From 7a1dbf8456740ed47a130b61cfab53a59652f0ed Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 25 Aug 2022 23:22:17 +0200 Subject: [PATCH 17/52] add gom space client code --- CMakeLists.txt | 2 +- bsp_q7s/OBSWConfig.h.in | 1 - bsp_q7s/boardtest/Q7STestTask.cpp | 12 ++++++++++++ thirdparty/gomspace-sw | 2 +- 4 files changed, 14 insertions(+), 3 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 5d14f7b3..ddb9f4b5 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -459,7 +459,7 @@ if(TGT_BSP MATCHES "arm/egse") endif() if(ADD_CSP_LIB) - target_link_libraries(${OBSW_NAME} PRIVATE ${LIB_CSP_NAME}) + target_link_libraries(${OBSW_NAME} PRIVATE ${LIB_CSP_NAME} libp60client) endif() if(EIVE_ADD_ETL_LIB) diff --git a/bsp_q7s/OBSWConfig.h.in b/bsp_q7s/OBSWConfig.h.in index ed321f8c..b9c27840 100644 --- a/bsp_q7s/OBSWConfig.h.in +++ b/bsp_q7s/OBSWConfig.h.in @@ -115,7 +115,6 @@ #cmakedefine LIBGPS_VERSION_MAJOR @LIBGPS_VERSION_MAJOR@ #cmakedefine LIBGPS_VERSION_MINOR @LIBGPS_VERSION_MINOR@ - #ifdef __cplusplus #include "objects/systemObjectList.h" diff --git a/bsp_q7s/boardtest/Q7STestTask.cpp b/bsp_q7s/boardtest/Q7STestTask.cpp index 6cf4e734..0264aba0 100644 --- a/bsp_q7s/boardtest/Q7STestTask.cpp +++ b/bsp_q7s/boardtest/Q7STestTask.cpp @@ -20,6 +20,9 @@ #include "fsfw/timemanager/Stopwatch.h" #include "test/DummyParameter.h" +#include "p60pdu.h" +#include + Q7STestTask::Q7STestTask(object_id_t objectId) : TestTask(objectId) { doTestSdCard = false; doTestScratchApi = false; @@ -35,6 +38,15 @@ ReturnValue_t Q7STestTask::performOneShotAction() { if (doTestScratchApi) { testScratchApi(); } + uint8_t p60pdu_node = 3; + uint8_t hk_mem[P60PDU_HK_SIZE]; + param_index_t p60pdu_hk{}; + p60pdu_hk.physaddr = hk_mem; + if (!p60pdu_get_hk(&p60pdu_hk, p60pdu_node, 1000)) { + printf("Error getting p60pdu hk\n"); + } else { + //param_list(&p60pdu_hk, 1); + } // testJsonLibDirect(); // testDummyParams(); if (doTestProtHandler) { diff --git a/thirdparty/gomspace-sw b/thirdparty/gomspace-sw index 47059ea5..b99ea737 160000 --- a/thirdparty/gomspace-sw +++ b/thirdparty/gomspace-sw @@ -1 +1 @@ -Subproject commit 47059ea593dda32e78e59ebd24902ea60db00448 +Subproject commit b99ea7371f5af591846df267d0a6d72e53f2cd88 From d098ed64036a0c0d58bbc30dc85d7fd584cdd7ae Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Fri, 26 Aug 2022 00:39:06 +0200 Subject: [PATCH 18/52] gomspace lib working --- CMakeLists.txt | 66 ++++++++++--------- bsp_q7s/OBSWConfig.h.in | 4 +- bsp_q7s/boardtest/Q7STestTask.cpp | 24 +++---- bsp_q7s/boardtest/Q7STestTask.h | 1 + mission/devices/GomspaceDeviceHandler.cpp | 7 +- mission/devices/GomspaceDeviceHandler.h | 1 + .../devicedefinitions/GomSpacePackets.h | 24 ++++--- .../devicedefinitions/GomspaceDefinitions.h | 23 +++---- scripts/auto-formatter.sh | 1 + thirdparty/gomspace-sw | 2 +- 10 files changed, 78 insertions(+), 75 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index ddb9f4b5..2a8f1021 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -170,6 +170,9 @@ set(LIB_ETL_TARGET etl::etl) set(LIB_CSP_NAME libcsp) set(LIB_LWGPS_NAME lwgps) set(LIB_ARCSEC wire) +set(LIB_GOMSPACE_CLIENTS gs_clients) +set(LIB_GOMSPACE_CSP gs_csp) + set(THIRD_PARTY_FOLDER thirdparty) set(LIB_CXX_FS -lstdc++fs) set(LIB_CATCH2 Catch2) @@ -190,7 +193,6 @@ set(COMMON_CONFIG_PATH ${COMMON_PATH}/config) set(UNITTEST_CFG_PATH ${UNITTEST_PATH}/testcfg) set(LIB_EIVE_MISSION_PATH mission) -set(LIB_CSP_PATH ${THIRD_PARTY_FOLDER}/libcsp) set(LIB_ETL_PATH ${THIRD_PARTY_FOLDER}/etl) set(LIB_CATCH2_PATH ${THIRD_PARTY_FOLDER}/Catch2) set(LIB_LWGPS_PATH ${THIRD_PARTY_FOLDER}/lwgps) @@ -218,8 +220,8 @@ if(TGT_BSP) set(FSFW_CONFIG_PATH "linux/fsfwconfig") if(NOT BUILD_Q7S_SIMPLE_MODE) set(EIVE_ADD_LINUX_FILES TRUE) - set(ADD_CSP_LIB TRUE) - set(ADD_GOMSPACE_LIB TRUE) + set(ADD_GOMSPACE_CSP TRUE) + set(ADD_GOMSPACE_CLIENTS TRUE) set(FSFW_HAL_ADD_LINUX ON) set(FSFW_HAL_LINUX_ADD_LIBGPIOD ON) set(FSFW_HAL_LINUX_ADD_PERIPHERAL_DRIVERS ON) @@ -301,30 +303,30 @@ set(OBSW_BIN_NAME ${CMAKE_PROJECT_NAME}) if(CMAKE_CXX_COMPILER_ID STREQUAL "GNU") set(WARNING_FLAGS - "-Wall" - "-Wextra" - "-Wimplicit-fallthrough=1" - "-Wno-unused-parameter" - "-Wno-psabi" - "-Wduplicated-cond" # check for duplicate conditions - "-Wduplicated-branches" # check for duplicate branches - "-Wlogical-op" # Search for bitwise operations instead of logical - "-Wnull-dereference" # Search for NULL dereference - "-Wundef" # Warn if undefind marcos are used - "-Wformat=2" # Format string problem detection - "-Wformat-overflow=2" # Formatting issues in printf - "-Wformat-truncation=2" # Formatting issues in printf - "-Wformat-security" # Search for dangerous printf operations - "-Wstrict-overflow=3" # Warn if integer overflows might happen - "-Warray-bounds=2" # Some array bounds violations will be found - "-Wshift-overflow=2" # Search for bit left shift overflows ( #include #include +#include #include #include @@ -18,10 +19,8 @@ #include "bsp_q7s/memory/scratchApi.h" #include "fsfw/tasks/TaskFactory.h" #include "fsfw/timemanager/Stopwatch.h" -#include "test/DummyParameter.h" - #include "p60pdu.h" -#include +#include "test/DummyParameter.h" Q7STestTask::Q7STestTask(object_id_t objectId) : TestTask(objectId) { doTestSdCard = false; @@ -38,15 +37,18 @@ ReturnValue_t Q7STestTask::performOneShotAction() { if (doTestScratchApi) { testScratchApi(); } - uint8_t p60pdu_node = 3; - uint8_t hk_mem[P60PDU_HK_SIZE]; - param_index_t p60pdu_hk{}; - p60pdu_hk.physaddr = hk_mem; - if (!p60pdu_get_hk(&p60pdu_hk, p60pdu_node, 1000)) { - printf("Error getting p60pdu hk\n"); - } else { - //param_list(&p60pdu_hk, 1); + if (DO_TEST_GOMSPACE_API) { + uint8_t p60pdu_node = 3; + uint8_t hk_mem[P60PDU_HK_SIZE]; + param_index_t p60pdu_hk{}; + p60pdu_hk.physaddr = hk_mem; + if (!p60pdu_get_hk(&p60pdu_hk, p60pdu_node, 1000)) { + printf("Error getting p60pdu hk\n"); + } else { + param_list(&p60pdu_hk, 1); + } } + // testJsonLibDirect(); // testDummyParams(); if (doTestProtHandler) { diff --git a/bsp_q7s/boardtest/Q7STestTask.h b/bsp_q7s/boardtest/Q7STestTask.h index 7fadbe6b..739eeb17 100644 --- a/bsp_q7s/boardtest/Q7STestTask.h +++ b/bsp_q7s/boardtest/Q7STestTask.h @@ -16,6 +16,7 @@ class Q7STestTask : public TestTask { private: bool doTestSdCard = false; bool doTestScratchApi = false; + static constexpr bool DO_TEST_GOMSPACE_API = true; bool doTestGpsShm = false; bool doTestGpsSocket = false; bool doTestProtHandler = false; diff --git a/mission/devices/GomspaceDeviceHandler.cpp b/mission/devices/GomspaceDeviceHandler.cpp index 7606159a..d4442df9 100644 --- a/mission/devices/GomspaceDeviceHandler.cpp +++ b/mission/devices/GomspaceDeviceHandler.cpp @@ -230,8 +230,7 @@ ReturnValue_t GomspaceDeviceHandler::generateSetParamCommand(const uint8_t* comm uint16_t payloadlength = sizeof(address) + parameterSize; /* Generate command for CspComIF */ - CspSetParamCommand setParamCmd(querySize, payloadlength, address, - parameterPtr, parameterSize); + CspSetParamCommand setParamCmd(querySize, payloadlength, address, parameterPtr, parameterSize); size_t cspPacketLen = 0; uint8_t* buffer = cspPacket; result = setParamCmd.serialize(&buffer, &cspPacketLen, sizeof(cspPacket), @@ -269,7 +268,7 @@ ReturnValue_t GomspaceDeviceHandler::generateGetParamCommand(const uint8_t* comm } /* Get an check table id to read from */ uint8_t tableId = getParamMessage.getTableId(); - if(not validTableId(tableId)) { + if (not validTableId(tableId)) { sif::error << "GomspaceDeviceHandler: Invalid table id in get parameter" " message" << std::endl; @@ -427,7 +426,7 @@ ReturnValue_t GomspaceDeviceHandler::generateResetWatchdogCmd() { } ReturnValue_t GomspaceDeviceHandler::generateRequestFullTableCmd(uint8_t tableId, - uint16_t hkTableReplySize) { + uint16_t hkTableReplySize) { uint16_t querySize = hkTableReplySize; RequestFullTableCommand requestFullTableCommand(querySize, tableId); diff --git a/mission/devices/GomspaceDeviceHandler.h b/mission/devices/GomspaceDeviceHandler.h index 3bf7a600..82487ba5 100644 --- a/mission/devices/GomspaceDeviceHandler.h +++ b/mission/devices/GomspaceDeviceHandler.h @@ -116,6 +116,7 @@ class GomspaceDeviceHandler : public DeviceHandlerBase { std::array initOutEnb); static bool validTableId(uint8_t id); + private: SetParamMessageUnpacker setParamCacher; /** diff --git a/mission/devices/devicedefinitions/GomSpacePackets.h b/mission/devices/devicedefinitions/GomSpacePackets.h index 6a213b69..650fa763 100644 --- a/mission/devices/devicedefinitions/GomSpacePackets.h +++ b/mission/devices/devicedefinitions/GomSpacePackets.h @@ -8,12 +8,13 @@ #include class CspParamRequestBase : public SerialLinkedListAdapter { -public: - CspParamRequestBase(uint16_t querySize, uint8_t tableId): querySize(querySize), tableId(tableId) { + public: + CspParamRequestBase(uint16_t querySize, uint8_t tableId) + : querySize(querySize), tableId(tableId) { setLinks(); } -protected: + protected: void setLinks() { setStart(&cspPort); cspPort.setNext(&querySize); @@ -104,9 +105,8 @@ class CspPingCommand : public SerialLinkedListAdapter { */ class CspSetParamCommand : public CspParamRequestBase { public: - CspSetParamCommand(uint16_t querySize_, uint16_t payloadlength_, - uint16_t addr_, const uint8_t *parameter_, - uint8_t parameterCount_, uint8_t tableId = 1) + CspSetParamCommand(uint16_t querySize_, uint16_t payloadlength_, uint16_t addr_, + const uint8_t *parameter_, uint8_t parameterCount_, uint8_t tableId = 1) : CspParamRequestBase(querySize_, tableId), addr(addr_), parameter(parameter_, parameterCount_) { @@ -116,8 +116,8 @@ class CspSetParamCommand : public CspParamRequestBase { CspParamRequestBase::action = GOMSPACE::ParamRequestIds::SET; } CspSetParamCommand(const CspSetParamCommand &command) = delete; - private: + private: SerializeElement addr; SerializeElement> parameter; }; @@ -135,17 +135,15 @@ class CspGetParamCommand : public CspParamRequestBase { /* The size of the header of a gomspace CSP packet. */ static const uint8_t GS_HDR_LENGTH = 12; - CspGetParamCommand(uint16_t querySize_, uint8_t tableId_, uint16_t addresslength_, - uint16_t addr_) - : CspParamRequestBase(querySize_, tableId_), - addr(addr_) { + CspGetParamCommand(uint16_t querySize_, uint8_t tableId_, uint16_t addresslength_, uint16_t addr_) + : CspParamRequestBase(querySize_, tableId_), addr(addr_) { total.setNext(&addr); CspParamRequestBase::tableId = tableId_; CspParamRequestBase::payloadlength = addresslength_; } CspGetParamCommand(const CspGetParamCommand &command) = delete; - private: + private: SerializeElement addr; }; @@ -160,7 +158,7 @@ class CspGetParamCommand : public CspParamRequestBase { class RequestFullTableCommand : public CspParamRequestBase { public: RequestFullTableCommand(uint16_t querySize_, uint8_t tableId_) - : CspParamRequestBase(querySize_, tableId_) {} + : CspParamRequestBase(querySize_, tableId_) {} RequestFullTableCommand(const RequestFullTableCommand &command) = delete; diff --git a/mission/devices/devicedefinitions/GomspaceDefinitions.h b/mission/devices/devicedefinitions/GomspaceDefinitions.h index d7a87b16..2f171ce9 100644 --- a/mission/devices/devicedefinitions/GomspaceDefinitions.h +++ b/mission/devices/devicedefinitions/GomspaceDefinitions.h @@ -30,13 +30,13 @@ static const uint8_t P60_PORT_GNDWDT_RESET = 9; * Device commands are derived from the rparam.h of the gomspace lib.. * IDs above 50 are reserved for device specific commands. */ -static const DeviceCommandId_t PING = 1; //!< [EXPORT] : [COMMAND] -static const DeviceCommandId_t NONE = 2; // Set when no command is pending -static const DeviceCommandId_t REBOOT = 4; //!< [EXPORT] : [COMMAND] -static const DeviceCommandId_t GNDWDT_RESET = 9; //!< [EXPORT] : [COMMAND] -static const DeviceCommandId_t PARAM_GET = 0; //!< [EXPORT] : [COMMAND] -static const DeviceCommandId_t PARAM_SET = 255; //!< [EXPORT] : [COMMAND] -static const DeviceCommandId_t REQUEST_HK_TABLE = 16; //!< [EXPORT] : [COMMAND] +static const DeviceCommandId_t PING = 1; //!< [EXPORT] : [COMMAND] +static const DeviceCommandId_t NONE = 2; // Set when no command is pending +static const DeviceCommandId_t REBOOT = 4; //!< [EXPORT] : [COMMAND] +static const DeviceCommandId_t GNDWDT_RESET = 9; //!< [EXPORT] : [COMMAND] +static const DeviceCommandId_t PARAM_GET = 0; //!< [EXPORT] : [COMMAND] +static const DeviceCommandId_t PARAM_SET = 255; //!< [EXPORT] : [COMMAND] +static const DeviceCommandId_t REQUEST_HK_TABLE = 16; //!< [EXPORT] : [COMMAND] static const DeviceCommandId_t REQUEST_CONFIG_TABLE = 17; //!< [EXPORT] : [COMMAND] // Not implemented yet // static const DeviceCommandId_t REQUEST_CALIB_TABLE = 18; //!< [EXPORT] : [COMMAND] @@ -45,7 +45,7 @@ static const DeviceCommandId_t REQUEST_CONFIG_TABLE = 17; //!< [EXPORT] : [COMM static const DeviceCommandId_t PRINT_SWITCH_V_I = 32; static const DeviceCommandId_t PRINT_LATCHUPS = 33; -enum ParamRequestIds: uint8_t { +enum ParamRequestIds : uint8_t { GET = 0x00, REPLY = 0x55, SET = 0xFF, @@ -62,12 +62,7 @@ enum ParamRequestIds: uint8_t { SAVE_TO_STORE = 0x9a }; -enum TableIds: uint8_t { - BOARD_PARAMS = 0, - CONFIG = 1, - CALIBRATION = 2, - HK = 4 -}; +enum TableIds : uint8_t { BOARD_PARAMS = 0, CONFIG = 1, CALIBRATION = 2, HK = 4 }; } // namespace GOMSPACE diff --git a/scripts/auto-formatter.sh b/scripts/auto-formatter.sh index e00a68ae..958ba0ac 100755 --- a/scripts/auto-formatter.sh +++ b/scripts/auto-formatter.sh @@ -23,6 +23,7 @@ if command -v ${cmake_fmt} &> /dev/null; then for dir in ${folder_list[@]}; do find ${dir} ${file_selectors} | xargs ${cmake_fmt} -i done + ${cmake_fmt} -i ./thirdparty/gomspace-sw/CMakeLists.txt else echo "No ${cmake_fmt} tool found, not formatting CMake files" fi diff --git a/thirdparty/gomspace-sw b/thirdparty/gomspace-sw index b99ea737..6b9db5e6 160000 --- a/thirdparty/gomspace-sw +++ b/thirdparty/gomspace-sw @@ -1 +1 @@ -Subproject commit b99ea7371f5af591846df267d0a6d72e53f2cd88 +Subproject commit 6b9db5e60cadcb9bbe1712f0f1b50aede2cbf7be From efb0bce71804c99af8fb74838f30cf0a56d144c7 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Fri, 26 Aug 2022 03:20:44 +0200 Subject: [PATCH 19/52] finally it works --- CMakeLists.txt | 1 + bsp_q7s/boardtest/Q7STestTask.cpp | 1 + bsp_q7s/core/ObjectFactory.cpp | 8 +- linux/csp/CspComIF.cpp | 46 +++--- linux/csp/CspComIF.h | 3 - linux/csp/CspCookie.cpp | 34 ++++- linux/csp/CspCookie.h | 23 ++- mission/devices/GomspaceDeviceHandler.cpp | 133 +++++++----------- mission/devices/GomspaceDeviceHandler.h | 8 +- mission/devices/PDU1Handler.cpp | 3 +- mission/devices/PDU2Handler.cpp | 3 +- .../devicedefinitions/GomspaceDefinitions.h | 13 ++ thirdparty/gomspace-sw | 2 +- 13 files changed, 165 insertions(+), 113 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 2a8f1021..3f159cc4 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -466,6 +466,7 @@ endif() if(ADD_GOMSPACE_CLIENTS) target_link_libraries(${OBSW_NAME} PRIVATE ${LIB_GOMSPACE_CLIENTS}) + target_link_libraries(${LIB_EIVE_MISSION} PRIVATE ${LIB_GOMSPACE_CLIENTS}) endif() if(EIVE_ADD_ETL_LIB) diff --git a/bsp_q7s/boardtest/Q7STestTask.cpp b/bsp_q7s/boardtest/Q7STestTask.cpp index 208e5aed..9c349b59 100644 --- a/bsp_q7s/boardtest/Q7STestTask.cpp +++ b/bsp_q7s/boardtest/Q7STestTask.cpp @@ -3,6 +3,7 @@ #include #include #include +#include #include #include #include diff --git a/bsp_q7s/core/ObjectFactory.cpp b/bsp_q7s/core/ObjectFactory.cpp index 3ac765ff..34586450 100644 --- a/bsp_q7s/core/ObjectFactory.cpp +++ b/bsp_q7s/core/ObjectFactory.cpp @@ -158,10 +158,10 @@ void ObjectFactory::createCommunicationInterfaces(LinuxLibgpioIF** gpioComIF, Ua } void ObjectFactory::createPcduComponents(LinuxLibgpioIF* gpioComIF, PowerSwitchIF** pwrSwitcher) { - CspCookie* p60DockCspCookie = new CspCookie(P60Dock::MAX_REPLY_LENGTH, addresses::P60DOCK); - CspCookie* pdu1CspCookie = new CspCookie(PDU::MAX_REPLY_LENGTH, addresses::PDU1); - CspCookie* pdu2CspCookie = new CspCookie(PDU::MAX_REPLY_LENGTH, addresses::PDU2); - CspCookie* acuCspCookie = new CspCookie(ACU::MAX_REPLY_LENGTH, addresses::ACU); + CspCookie* p60DockCspCookie = new CspCookie(P60Dock::MAX_REPLY_LENGTH, addresses::P60DOCK, 500); + CspCookie* pdu1CspCookie = new CspCookie(PDU::MAX_REPLY_LENGTH, addresses::PDU1, 500); + CspCookie* pdu2CspCookie = new CspCookie(PDU::MAX_REPLY_LENGTH, addresses::PDU2, 500); + CspCookie* acuCspCookie = new CspCookie(ACU::MAX_REPLY_LENGTH, addresses::ACU, 500); auto p60Fdir = new GomspacePowerFdir(objects::P60DOCK_HANDLER); P60DockHandler* p60dockhandler = diff --git a/linux/csp/CspComIF.cpp b/linux/csp/CspComIF.cpp index 9f346405..ee515b9a 100644 --- a/linux/csp/CspComIF.cpp +++ b/linux/csp/CspComIF.cpp @@ -3,6 +3,8 @@ #include #include #include +#include +#include #include "CspCookie.h" @@ -65,44 +67,56 @@ ReturnValue_t CspComIF::initializeInterface(CookieIF* cookie) { /* Insert device information in CSP map */ cspDeviceMap.emplace(cspAddress, vectorBuffer(maxReplyLength)); } - return HasReturnvaluesIF::RETURN_OK; } ReturnValue_t CspComIF::sendMessage(CookieIF* cookie, const uint8_t* sendData, size_t sendLen) { int result; - if (cookie == NULL) { + if (cookie == nullptr) { return HasReturnvaluesIF::RETURN_FAILED; } CspCookie* cspCookie = dynamic_cast(cookie); - if (cspCookie == NULL) { + if (cspCookie == nullptr) { return HasReturnvaluesIF::RETURN_FAILED; } - /* Extract csp port and bytes to query from command buffer */ uint8_t cspPort; uint16_t querySize = 0; - result = getPortAndQuerySize(&sendData, &sendLen, &cspPort, &querySize); - if (result != HasReturnvaluesIF::RETURN_OK) { - return result; + if(cspCookie->getRequest() == CspCookie::SpecialRequestTypes::DEFAULT_COM_IF) { + /* Extract csp port and bytes to query from command buffer */ + result = getPortAndQuerySize(&sendData, &sendLen, &cspPort, &querySize); + if (result != HasReturnvaluesIF::RETURN_OK) { + return result; + } + } else { + cspPort = cspCookie->getCspPort(); + querySize = cspCookie->getReplyLen(); } uint8_t cspAddress = cspCookie->getCspAddress(); switch (cspPort) { - case (Ports::CSP_PING): { + case (GOMSPACE::CspPorts::CSP_PING): { initiatePingRequest(cspAddress, querySize); break; } - case (Ports::CSP_REBOOT): { + case (GOMSPACE::CspPorts::CSP_REBOOT): { csp_reboot(cspAddress); break; } - case (Ports::P60_PORT_GNDWDT_RESET): - case (Ports::P60_PORT_RPARAM): { - /* No CSP fixed port was selected. Send data to the specified port and - * wait for querySize number of bytes */ - result = cspTransfer(cspAddress, cspPort, sendData, sendLen, querySize); - if (result != HasReturnvaluesIF::RETURN_OK) { - return HasReturnvaluesIF::RETURN_FAILED; + case (GOMSPACE::CspPorts::P60_PORT_GNDWDT_RESET_ENUM): + case (GOMSPACE::CspPorts::P60_PORT_RPARAM_ENUM): { + if(cspCookie->getRequest() == CspCookie::SpecialRequestTypes::GET_PDU_HK) { + param_index_t requestStruct{}; + requestStruct.physaddr = cspDeviceMap[cspAddress].data(); + if(!p60pdu_get_hk(&requestStruct, cspAddress, cspCookie->getTimeout())) { + return HasReturnvaluesIF::RETURN_FAILED; + } + } else { + /* No CSP fixed port was selected. Send data to the specified port and + * wait for querySize number of bytes */ + result = cspTransfer(cspAddress, cspPort, sendData, sendLen, querySize); + if (result != HasReturnvaluesIF::RETURN_OK) { + return HasReturnvaluesIF::RETURN_FAILED; + } } replySize = querySize; break; diff --git a/linux/csp/CspComIF.h b/linux/csp/CspComIF.h index d36bbf4f..7a54fa6e 100644 --- a/linux/csp/CspComIF.h +++ b/linux/csp/CspComIF.h @@ -42,8 +42,6 @@ class CspComIF : public DeviceCommunicationIF, public SystemObject { ReturnValue_t cspTransfer(uint8_t cspAddress, uint8_t cspPort, const uint8_t *cmdBuffer, int cmdLen, uint16_t querySize); - enum Ports { CSP_PING = 1, CSP_REBOOT = 4, P60_PORT_RPARAM = 7, P60_PORT_GNDWDT_RESET = 9 }; - typedef uint8_t node_t; using vectorBuffer = std::vector; using VectorBufferMap = std::unordered_map; @@ -59,7 +57,6 @@ class CspComIF : public DeviceCommunicationIF, public SystemObject { /* Interface struct for csp protocol stack */ csp_iface_t csp_if; - char canInterface[5] = "can0"; int bitrate = 1000; diff --git a/linux/csp/CspCookie.cpp b/linux/csp/CspCookie.cpp index 39274908..75d5ba73 100644 --- a/linux/csp/CspCookie.cpp +++ b/linux/csp/CspCookie.cpp @@ -1,10 +1,38 @@ #include "CspCookie.h" -CspCookie::CspCookie(uint16_t maxReplyLength_, uint8_t cspAddress_) - : maxReplyLength(maxReplyLength_), cspAddress(cspAddress_) {} +CspCookie::CspCookie(uint16_t maxReplyLength_, uint8_t cspAddress_, uint32_t timeoutMs) + : maxReplyLength(maxReplyLength_), cspAddress(cspAddress_), timeoutMs(timeoutMs), + reqType(DEFAULT_COM_IF) {} CspCookie::~CspCookie() {} uint16_t CspCookie::getMaxReplyLength() { return maxReplyLength; } -uint8_t CspCookie::getCspAddress() { return cspAddress; } +uint8_t CspCookie::getCspAddress() { + return cspAddress; +} + +CspCookie::SpecialRequestTypes CspCookie::getRequest() const { + return reqType; +} + +void CspCookie::setRequest(SpecialRequestTypes request, size_t replyLen_) { + reqType = request; + replyLen = replyLen_; +} + +uint8_t CspCookie::getCspPort() const { + return cspPort; +} + +uint32_t CspCookie::getTimeout() const { + return timeoutMs; +} + +void CspCookie::setCspPort(uint8_t port) { + cspPort = port; +} + +size_t CspCookie::getReplyLen() const { + return replyLen; +} diff --git a/linux/csp/CspCookie.h b/linux/csp/CspCookie.h index e59f3d35..b0f71d2e 100644 --- a/linux/csp/CspCookie.h +++ b/linux/csp/CspCookie.h @@ -4,6 +4,7 @@ #include #include +#include /** * @brief This is the cookie for devices supporting the CSP (CubeSat Space @@ -12,15 +13,35 @@ */ class CspCookie : public CookieIF { public: - CspCookie(uint16_t maxReplyLength_, uint8_t cspAddress_); + enum SpecialRequestTypes { + DEFAULT_COM_IF, + GET_PDU_HK, + GET_PDU_CONFIG, + GET_ACU_HK, + GET_ACU_CONFIG, + GET_P60DOCK_HK, + GET_P60DOCK_CONFIG + }; + + CspCookie(uint16_t maxReplyLength_, uint8_t cspAddress_, uint32_t timeoutMs); virtual ~CspCookie(); + void setCspPort(uint8_t port); + uint8_t getCspPort() const; uint16_t getMaxReplyLength(); + SpecialRequestTypes getRequest() const; + void setRequest(SpecialRequestTypes request, size_t replyLen); + size_t getReplyLen() const; uint8_t getCspAddress(); + uint32_t getTimeout() const; private: + uint8_t cspPort; uint16_t maxReplyLength; uint8_t cspAddress; + size_t replyLen = 0; + uint32_t timeoutMs; + SpecialRequestTypes reqType; }; #endif /* LINUX_CSP_CSPCOOKIE_H_ */ diff --git a/mission/devices/GomspaceDeviceHandler.cpp b/mission/devices/GomspaceDeviceHandler.cpp index d4442df9..c8659f7a 100644 --- a/mission/devices/GomspaceDeviceHandler.cpp +++ b/mission/devices/GomspaceDeviceHandler.cpp @@ -2,9 +2,13 @@ #include #include +#include +#include +#include #include "devicedefinitions/GomSpacePackets.h" #include "devicedefinitions/powerDefinitions.h" +#include using namespace GOMSPACE; @@ -426,22 +430,29 @@ ReturnValue_t GomspaceDeviceHandler::generateResetWatchdogCmd() { } ReturnValue_t GomspaceDeviceHandler::generateRequestFullTableCmd(uint8_t tableId, - uint16_t hkTableReplySize) { - uint16_t querySize = hkTableReplySize; - RequestFullTableCommand requestFullTableCommand(querySize, tableId); + uint16_t tableReplySize) { + uint16_t querySize = tableReplySize; + if(getObjectId() == objects::PDU1_HANDLER or getObjectId() == objects::PDU2_HANDLER) { + auto* cspCookie = dynamic_cast(comCookie); + cspCookie->setRequest(CspCookie::SpecialRequestTypes::GET_PDU_HK, tableReplySize); + cspCookie->setCspPort(GOMSPACE::CspPorts::P60_PORT_RPARAM_ENUM); + } else { + RequestFullTableCommand requestFullTableCommand(querySize, tableId); - size_t cspPacketLen = 0; - uint8_t* buffer = cspPacket; - ReturnValue_t result = requestFullTableCommand.serialize( - &buffer, &cspPacketLen, sizeof(cspPacket), SerializeIF::Endianness::BIG); - if (result != HasReturnvaluesIF::RETURN_OK) { - sif::error << "GomspaceDeviceHandler::generateRequestFullTableCmd Failed to serialize " - "full table request command " - << std::endl; - return result; + size_t cspPacketLen = 0; + uint8_t* buffer = cspPacket; + ReturnValue_t result = requestFullTableCommand.serialize( + &buffer, &cspPacketLen, sizeof(cspPacket), SerializeIF::Endianness::BIG); + if (result != HasReturnvaluesIF::RETURN_OK) { + sif::error << "GomspaceDeviceHandler::generateRequestFullTableCmd Failed to serialize " + "full table request command " + << std::endl; + return result; + } + rawPacket = cspPacket; + rawPacketLen = cspPacketLen; } - rawPacket = cspPacket; - rawPacketLen = cspPacketLen; + rememberRequestedSize = querySize; rememberCommandId = GOMSPACE::REQUEST_HK_TABLE; return RETURN_OK; @@ -458,7 +469,6 @@ ReturnValue_t GomspaceDeviceHandler::printStatus(DeviceCommandId_t cmd) { ReturnValue_t GomspaceDeviceHandler::parsePduHkTable(PDU::PduCoreHk& coreHk, PDU::PduAuxHk& auxHk, const uint8_t* packet) { - uint16_t dataOffset = 0; PoolReadGuard pg0(&coreHk); PoolReadGuard pg1(&auxHk); if (pg0.getReadResult() != HasReturnvaluesIF::RETURN_OK or @@ -468,92 +478,51 @@ ReturnValue_t GomspaceDeviceHandler::parsePduHkTable(PDU::PduCoreHk& coreHk, PDU } /* Fist 10 bytes contain the gomspace header. Each variable is preceded by the 16-bit table * address. */ - dataOffset += 12; + //dataOffset += 12; for (uint8_t idx = 0; idx < PDU::CHANNELS_LEN; idx++) { - coreHk.currents[idx] = (packet[dataOffset] << 8) | packet[dataOffset + 1]; - dataOffset += 4; + coreHk.currents[idx] = as(packet + (idx * 2)); } for (uint8_t idx = 0; idx < PDU::CHANNELS_LEN; idx++) { - coreHk.voltages[idx] = (packet[dataOffset] << 8) | packet[dataOffset + 1]; - dataOffset += 4; + coreHk.voltages[idx] = as(packet + 0x12 + (idx*2)); } - - auxHk.vcc = *(packet + dataOffset) << 8 | *(packet + dataOffset + 1); - dataOffset += 4; - auxHk.vbat = *(packet + dataOffset) << 8 | *(packet + dataOffset + 1); - dataOffset += 4; - coreHk.temperature = - static_cast(*(packet + dataOffset) << 8 | *(packet + dataOffset + 1)) * 0.1; - dataOffset += 4; + auxHk.vcc.value = as(packet + 0x24); + auxHk.vbat.value = as(packet + 0x26); + coreHk.temperature = as(packet + 0x28) * 0.1; for (uint8_t idx = 0; idx < 3; idx++) { - auxHk.converterEnable[idx] = packet[dataOffset]; - dataOffset += 3; + auxHk.converterEnable[idx] = packet[0x2a + idx]; } for (uint8_t idx = 0; idx < PDU::CHANNELS_LEN; idx++) { - coreHk.outputEnables[idx] = packet[dataOffset]; - dataOffset += 3; + coreHk.outputEnables[idx] = packet[0x2e + idx]; } - - auxHk.bootcause = *(packet + dataOffset) << 24 | *(packet + dataOffset + 1) << 16 | - *(packet + dataOffset + 2) << 8 | *(packet + dataOffset + 3); - dataOffset += 6; - coreHk.bootcount = *(packet + dataOffset) << 24 | *(packet + dataOffset + 1) << 16 | - *(packet + dataOffset + 2) << 8 | *(packet + dataOffset + 3); - dataOffset += 6; - auxHk.uptime = *(packet + dataOffset) << 24 | *(packet + dataOffset + 1) << 16 | - *(packet + dataOffset + 2) << 8 | *(packet + dataOffset + 3); - dataOffset += 6; - auxHk.resetcause = *(packet + dataOffset + 1) << 8 | *(packet + dataOffset); - dataOffset += 4; - coreHk.battMode = *(packet + dataOffset); - /* +10 because here begins the second gomspace csp packet */ - dataOffset += 3 + 10; + auxHk.bootcause = as(packet + 0x38); + coreHk.bootcount = as(packet + 0x3c); + auxHk.uptime = as(packet + 0x40); + auxHk.resetcause = as(packet + 0x44); + coreHk.battMode = *(packet + 0x46); for (uint8_t idx = 0; idx < PDU::CHANNELS_LEN; idx++) { - auxHk.latchups[idx] = *(packet + dataOffset) << 8 | *(packet + dataOffset + 1); - dataOffset += 4; + auxHk.latchups[idx] = as(packet + 0x48 + (idx*2)); } for (uint8_t idx = 0; idx < PDU::DEVICES_NUM; idx++) { - auxHk.deviceTypes[idx] = *(packet + dataOffset); - dataOffset += 3; + auxHk.deviceTypes[idx] = *(packet + 0x5a + idx); } for (uint8_t idx = 0; idx < PDU::DEVICES_NUM; idx++) { - auxHk.devicesStatus[idx] = *(packet + dataOffset); - dataOffset += 3; + auxHk.devicesStatus[idx] = *(packet + 0x62 + idx); } - auxHk.gndWdtReboots = *(packet + dataOffset) << 24 | *(packet + dataOffset + 1) << 16 | - *(packet + dataOffset + 2) << 8 | *(packet + dataOffset + 3); - dataOffset += 6; - auxHk.i2cWdtReboots = *(packet + dataOffset) << 24 | *(packet + dataOffset + 1) << 16 | - *(packet + dataOffset + 2) << 8 | *(packet + dataOffset + 3); - dataOffset += 6; - auxHk.canWdtReboots = *(packet + dataOffset) << 24 | *(packet + dataOffset + 1) << 16 | - *(packet + dataOffset + 2) << 8 | *(packet + dataOffset + 3); - dataOffset += 6; - auxHk.csp1WdtReboots = *(packet + dataOffset) << 24 | *(packet + dataOffset + 1) << 16 | - *(packet + dataOffset + 2) << 8 | *(packet + dataOffset + 3); - dataOffset += 6; - auxHk.csp2WdtReboots = *(packet + dataOffset) << 24 | *(packet + dataOffset + 1) << 16 | - *(packet + dataOffset + 2) << 8 | *(packet + dataOffset + 3); - dataOffset += 6; - auxHk.groundWatchdogSecondsLeft = *(packet + dataOffset) << 24 | - *(packet + dataOffset + 1) << 16 | - *(packet + dataOffset + 2) << 8 | *(packet + dataOffset + 3); - dataOffset += 6; - auxHk.i2cWatchdogSecondsLeft = *(packet + dataOffset) << 24 | *(packet + dataOffset + 1) << 16 | - *(packet + dataOffset + 2) << 8 | *(packet + dataOffset + 3); - dataOffset += 6; - auxHk.canWatchdogSecondsLeft = *(packet + dataOffset) << 24 | *(packet + dataOffset + 1) << 16 | - *(packet + dataOffset + 2) << 8 | *(packet + dataOffset + 3); - dataOffset += 6; - auxHk.csp1WatchdogPingsLeft = *(packet + dataOffset); - dataOffset += 3; - auxHk.csp2WatchdogPingsLeft = *(packet + dataOffset); - + auxHk.gndWdtReboots = as(packet + 0x6c); + auxHk.i2cWdtReboots = as(packet + 0x70); + auxHk.canWdtReboots = as(packet + 0x74); + auxHk.csp1WdtReboots = as(packet + 0x78); + auxHk.csp2WdtReboots = as(packet + 0x78 + 4); + auxHk.groundWatchdogSecondsLeft = as(packet + 0x80); + auxHk.i2cWatchdogSecondsLeft = as(packet + 0x84); + auxHk.canWatchdogSecondsLeft = as(packet + 0x88); + auxHk.csp1WatchdogPingsLeft = *(packet + 0x8c); + auxHk.csp2WatchdogPingsLeft = *(packet + 0x8c + 1); coreHk.setChanged(true); if (not coreHk.isValid()) { coreHk.setValidity(true, true); diff --git a/mission/devices/GomspaceDeviceHandler.h b/mission/devices/GomspaceDeviceHandler.h index 82487ba5..fbacaf07 100644 --- a/mission/devices/GomspaceDeviceHandler.h +++ b/mission/devices/GomspaceDeviceHandler.h @@ -82,7 +82,7 @@ class GomspaceDeviceHandler : public DeviceHandlerBase { * @brief The command to generate a request to receive the full housekeeping table is device * specific. Thus the child has to build this command. */ - virtual ReturnValue_t generateRequestFullTableCmd(uint8_t tableId, uint16_t hkTableSize); + virtual ReturnValue_t generateRequestFullTableCmd(uint8_t tableId, uint16_t tableSize); /** * This command handles printing the HK table to the console. This is useful for debugging @@ -115,6 +115,7 @@ class GomspaceDeviceHandler : public DeviceHandlerBase { LocalDataPoolManager &poolManager, std::array initOutEnb); + template T as(const uint8_t*); static bool validTableId(uint8_t id); private: @@ -160,4 +161,9 @@ class GomspaceDeviceHandler : public DeviceHandlerBase { ReturnValue_t generateResetWatchdogCmd(); }; +template +inline T GomspaceDeviceHandler::as(const uint8_t* ptr) { + return *(reinterpret_cast(ptr)); +} + #endif /* MISSION_DEVICES_GOMSPACEDEVICEHANDLER_H_ */ diff --git a/mission/devices/PDU1Handler.cpp b/mission/devices/PDU1Handler.cpp index 9c2f845c..e2466389 100644 --- a/mission/devices/PDU1Handler.cpp +++ b/mission/devices/PDU1Handler.cpp @@ -2,13 +2,14 @@ #include #include +#include #include "devices/powerSwitcherList.h" PDU1Handler::PDU1Handler(object_id_t objectId, object_id_t comIF, CookieIF *comCookie, FailureIsolationBase *customFdir) : GomspaceDeviceHandler(objectId, comIF, comCookie, customFdir, PDU::MAX_CONFIGTABLE_ADDRESS, - PDU::MAX_HKTABLE_ADDRESS, PDU::HK_TABLE_REPLY_SIZE), + PDU::MAX_HKTABLE_ADDRESS, P60PDU_HK_SIZE), coreHk(this), auxHk(this) {} diff --git a/mission/devices/PDU2Handler.cpp b/mission/devices/PDU2Handler.cpp index 073ccb23..2a1580f3 100644 --- a/mission/devices/PDU2Handler.cpp +++ b/mission/devices/PDU2Handler.cpp @@ -2,13 +2,14 @@ #include #include +#include #include "devices/powerSwitcherList.h" PDU2Handler::PDU2Handler(object_id_t objectId, object_id_t comIF, CookieIF *comCookie, FailureIsolationBase *customFdir) : GomspaceDeviceHandler(objectId, comIF, comCookie, customFdir, PDU::MAX_CONFIGTABLE_ADDRESS, - PDU::MAX_HKTABLE_ADDRESS, PDU::HK_TABLE_REPLY_SIZE), + PDU::MAX_HKTABLE_ADDRESS, P60PDU_HK_SIZE), coreHk(this), auxHk(this) {} diff --git a/mission/devices/devicedefinitions/GomspaceDefinitions.h b/mission/devices/devicedefinitions/GomspaceDefinitions.h index 2f171ce9..ae1a2e83 100644 --- a/mission/devices/devicedefinitions/GomspaceDefinitions.h +++ b/mission/devices/devicedefinitions/GomspaceDefinitions.h @@ -12,6 +12,13 @@ namespace GOMSPACE { +enum CspPorts: uint8_t { + CSP_PING = 1, + CSP_REBOOT = 4, + P60_PORT_RPARAM_ENUM = 7, + P60_PORT_GNDWDT_RESET_ENUM = 9 +}; + enum class Pdu { PDU1, PDU2 }; using ChannelSwitchHook = void (*)(Pdu pdu, uint8_t channel, bool on, void* args); @@ -466,6 +473,12 @@ class PduAuxHk : public StaticLocalDataSet<36> { lp_var_t(sid.objectId, P60System::pool::PDU_WDT_CSP_LEFT2, this); }; +//class HkWrapper { +//public: +// HkWrapper(PduCoreHk& coreHk, PduAuxHk& auxHk) { +// +// } +//}; } // namespace PDU namespace PDU1 { diff --git a/thirdparty/gomspace-sw b/thirdparty/gomspace-sw index 6b9db5e6..0b66e23a 160000 --- a/thirdparty/gomspace-sw +++ b/thirdparty/gomspace-sw @@ -1 +1 @@ -Subproject commit 6b9db5e60cadcb9bbe1712f0f1b50aede2cbf7be +Subproject commit 0b66e23a8900e315f01cfc52088adad10bdabf26 From a7482b657a72bf1dc14a3a247746937a658aa1f1 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Fri, 26 Aug 2022 13:42:58 +0200 Subject: [PATCH 20/52] changed all table parsing to use gom space API --- bsp_q7s/OBSWConfig.h.in | 4 +- bsp_q7s/boardtest/Q7STestTask.h | 2 +- bsp_q7s/core/ObjectFactory.cpp | 2 +- linux/csp/CMakeLists.txt | 2 +- linux/csp/CspComIF.cpp | 33 +++-- mission/CMakeLists.txt | 1 + mission/csp/CMakeLists.txt | 1 + {linux => mission}/csp/CspCookie.cpp | 6 +- {linux => mission}/csp/CspCookie.h | 17 +-- mission/devices/ACUHandler.cpp | 77 ++++------ mission/devices/GomspaceDeviceHandler.cpp | 25 ++-- mission/devices/GomspaceDeviceHandler.h | 3 +- mission/devices/P60DockHandler.cpp | 131 +++++------------- .../devicedefinitions/GomspaceDefinitions.h | 10 ++ thirdparty/gomspace-sw | 2 +- tmtc | 2 +- 16 files changed, 131 insertions(+), 187 deletions(-) create mode 100644 mission/csp/CMakeLists.txt rename {linux => mission}/csp/CspCookie.cpp (77%) rename {linux => mission}/csp/CspCookie.h (71%) diff --git a/bsp_q7s/OBSWConfig.h.in b/bsp_q7s/OBSWConfig.h.in index 05de55fd..b9c27840 100644 --- a/bsp_q7s/OBSWConfig.h.in +++ b/bsp_q7s/OBSWConfig.h.in @@ -58,8 +58,8 @@ #define OBSW_STAR_TRACKER_GROUND_CONFIG 1 #define OBSW_SYRLINKS_SIMULATED 1 -#define OBSW_ADD_TEST_CODE 1 -#define OBSW_ADD_TEST_TASK 1 +#define OBSW_ADD_TEST_CODE 0 +#define OBSW_ADD_TEST_TASK 0 #define OBSW_ADD_TEST_PST 0 // If this is enabled, all other SPI code should be disabled #define OBSW_ADD_SPI_TEST_CODE 0 diff --git a/bsp_q7s/boardtest/Q7STestTask.h b/bsp_q7s/boardtest/Q7STestTask.h index 739eeb17..97e68c66 100644 --- a/bsp_q7s/boardtest/Q7STestTask.h +++ b/bsp_q7s/boardtest/Q7STestTask.h @@ -16,7 +16,7 @@ class Q7STestTask : public TestTask { private: bool doTestSdCard = false; bool doTestScratchApi = false; - static constexpr bool DO_TEST_GOMSPACE_API = true; + static constexpr bool DO_TEST_GOMSPACE_API = false; bool doTestGpsShm = false; bool doTestGpsSocket = false; bool doTestProtHandler = false; diff --git a/bsp_q7s/core/ObjectFactory.cpp b/bsp_q7s/core/ObjectFactory.cpp index 34586450..202b890d 100644 --- a/bsp_q7s/core/ObjectFactory.cpp +++ b/bsp_q7s/core/ObjectFactory.cpp @@ -20,7 +20,7 @@ #include "linux/boardtest/UartTestClass.h" #include "linux/callbacks/gpioCallbacks.h" #include "linux/csp/CspComIF.h" -#include "linux/csp/CspCookie.h" +#include "mission/csp/CspCookie.h" #include "linux/devices/GPSHyperionLinuxController.h" #include "linux/devices/devicedefinitions/PlocMPSoCDefinitions.h" #include "linux/devices/devicedefinitions/StarTrackerDefinitions.h" diff --git a/linux/csp/CMakeLists.txt b/linux/csp/CMakeLists.txt index 71906a69..b58849aa 100644 --- a/linux/csp/CMakeLists.txt +++ b/linux/csp/CMakeLists.txt @@ -1 +1 @@ -target_sources(${OBSW_NAME} PUBLIC CspComIF.cpp CspCookie.cpp) +target_sources(${OBSW_NAME} PUBLIC CspComIF.cpp) diff --git a/linux/csp/CspComIF.cpp b/linux/csp/CspComIF.cpp index ee515b9a..93945c0a 100644 --- a/linux/csp/CspComIF.cpp +++ b/linux/csp/CspComIF.cpp @@ -5,8 +5,12 @@ #include #include #include +#include +#include -#include "CspCookie.h" +#include "mission/csp/CspCookie.h" + +using namespace GOMSPACE; CspComIF::CspComIF(object_id_t objectId) : SystemObject(objectId) {} @@ -82,7 +86,7 @@ ReturnValue_t CspComIF::sendMessage(CookieIF* cookie, const uint8_t* sendData, s uint8_t cspPort; uint16_t querySize = 0; - if(cspCookie->getRequest() == CspCookie::SpecialRequestTypes::DEFAULT_COM_IF) { + if(cspCookie->getRequest() == GOMSPACE::SpecialRequestTypes::DEFAULT_COM_IF) { /* Extract csp port and bytes to query from command buffer */ result = getPortAndQuerySize(&sendData, &sendLen, &cspPort, &querySize); if (result != HasReturnvaluesIF::RETURN_OK) { @@ -94,21 +98,32 @@ ReturnValue_t CspComIF::sendMessage(CookieIF* cookie, const uint8_t* sendData, s } uint8_t cspAddress = cspCookie->getCspAddress(); switch (cspPort) { - case (GOMSPACE::CspPorts::CSP_PING): { + case (CspPorts::CSP_PING): { initiatePingRequest(cspAddress, querySize); break; } - case (GOMSPACE::CspPorts::CSP_REBOOT): { + case (CspPorts::CSP_REBOOT): { csp_reboot(cspAddress); break; } - case (GOMSPACE::CspPorts::P60_PORT_GNDWDT_RESET_ENUM): - case (GOMSPACE::CspPorts::P60_PORT_RPARAM_ENUM): { - if(cspCookie->getRequest() == CspCookie::SpecialRequestTypes::GET_PDU_HK) { + case (CspPorts::P60_PORT_GNDWDT_RESET_ENUM): + case (CspPorts::P60_PORT_RPARAM_ENUM): { + if(cspCookie->getRequest() != SpecialRequestTypes::DEFAULT_COM_IF) { param_index_t requestStruct{}; requestStruct.physaddr = cspDeviceMap[cspAddress].data(); - if(!p60pdu_get_hk(&requestStruct, cspAddress, cspCookie->getTimeout())) { - return HasReturnvaluesIF::RETURN_FAILED; + if(cspCookie->getRequest() == GOMSPACE::SpecialRequestTypes::GET_PDU_HK) { + if(!p60pdu_get_hk(&requestStruct, cspAddress, cspCookie->getTimeout())) { + return HasReturnvaluesIF::RETURN_FAILED; + } + + } else if(cspCookie->getRequest() == GOMSPACE::SpecialRequestTypes::GET_ACU_HK) { + if(!p60acu_get_hk(&requestStruct, cspAddress, cspCookie->getTimeout())) { + return HasReturnvaluesIF::RETURN_FAILED; + } + } else if(cspCookie->getRequest() == GOMSPACE::SpecialRequestTypes::GET_P60DOCK_HK) { + if(!p60dock_get_hk(&requestStruct, cspAddress, cspCookie->getTimeout())) { + return HasReturnvaluesIF::RETURN_FAILED; + } } } else { /* No CSP fixed port was selected. Send data to the specified port and diff --git a/mission/CMakeLists.txt b/mission/CMakeLists.txt index 9c3dad78..73b19b69 100644 --- a/mission/CMakeLists.txt +++ b/mission/CMakeLists.txt @@ -5,3 +5,4 @@ add_subdirectory(utility) add_subdirectory(memory) add_subdirectory(tmtc) add_subdirectory(system) +add_subdirectory(csp) diff --git a/mission/csp/CMakeLists.txt b/mission/csp/CMakeLists.txt new file mode 100644 index 00000000..d5b6f3fe --- /dev/null +++ b/mission/csp/CMakeLists.txt @@ -0,0 +1 @@ +target_sources(${LIB_EIVE_MISSION} PRIVATE CspCookie.cpp) diff --git a/linux/csp/CspCookie.cpp b/mission/csp/CspCookie.cpp similarity index 77% rename from linux/csp/CspCookie.cpp rename to mission/csp/CspCookie.cpp index 75d5ba73..a2265f9e 100644 --- a/linux/csp/CspCookie.cpp +++ b/mission/csp/CspCookie.cpp @@ -2,7 +2,7 @@ CspCookie::CspCookie(uint16_t maxReplyLength_, uint8_t cspAddress_, uint32_t timeoutMs) : maxReplyLength(maxReplyLength_), cspAddress(cspAddress_), timeoutMs(timeoutMs), - reqType(DEFAULT_COM_IF) {} + reqType(GOMSPACE::DEFAULT_COM_IF) {} CspCookie::~CspCookie() {} @@ -12,11 +12,11 @@ uint8_t CspCookie::getCspAddress() { return cspAddress; } -CspCookie::SpecialRequestTypes CspCookie::getRequest() const { +GOMSPACE::SpecialRequestTypes CspCookie::getRequest() const { return reqType; } -void CspCookie::setRequest(SpecialRequestTypes request, size_t replyLen_) { +void CspCookie::setRequest(GOMSPACE::SpecialRequestTypes request, size_t replyLen_) { reqType = request; replyLen = replyLen_; } diff --git a/linux/csp/CspCookie.h b/mission/csp/CspCookie.h similarity index 71% rename from linux/csp/CspCookie.h rename to mission/csp/CspCookie.h index b0f71d2e..71b50e61 100644 --- a/linux/csp/CspCookie.h +++ b/mission/csp/CspCookie.h @@ -2,7 +2,7 @@ #define LINUX_CSP_CSPCOOKIE_H_ #include - +#include "mission/devices/devicedefinitions/GomspaceDefinitions.h" #include #include @@ -13,15 +13,6 @@ */ class CspCookie : public CookieIF { public: - enum SpecialRequestTypes { - DEFAULT_COM_IF, - GET_PDU_HK, - GET_PDU_CONFIG, - GET_ACU_HK, - GET_ACU_CONFIG, - GET_P60DOCK_HK, - GET_P60DOCK_CONFIG - }; CspCookie(uint16_t maxReplyLength_, uint8_t cspAddress_, uint32_t timeoutMs); virtual ~CspCookie(); @@ -29,8 +20,8 @@ class CspCookie : public CookieIF { void setCspPort(uint8_t port); uint8_t getCspPort() const; uint16_t getMaxReplyLength(); - SpecialRequestTypes getRequest() const; - void setRequest(SpecialRequestTypes request, size_t replyLen); + GOMSPACE::SpecialRequestTypes getRequest() const; + void setRequest(GOMSPACE::SpecialRequestTypes request, size_t replyLen); size_t getReplyLen() const; uint8_t getCspAddress(); uint32_t getTimeout() const; @@ -41,7 +32,7 @@ class CspCookie : public CookieIF { uint8_t cspAddress; size_t replyLen = 0; uint32_t timeoutMs; - SpecialRequestTypes reqType; + GOMSPACE::SpecialRequestTypes reqType; }; #endif /* LINUX_CSP_CSPCOOKIE_H_ */ diff --git a/mission/devices/ACUHandler.cpp b/mission/devices/ACUHandler.cpp index 35f8e2dd..3c2572df 100644 --- a/mission/devices/ACUHandler.cpp +++ b/mission/devices/ACUHandler.cpp @@ -1,11 +1,12 @@ #include "ACUHandler.h" - #include "OBSWConfig.h" +#include "p60acu_hk.h" + ACUHandler::ACUHandler(object_id_t objectId, object_id_t comIF, CookieIF *comCookie, FailureIsolationBase *customFdir) : GomspaceDeviceHandler(objectId, comIF, comCookie, customFdir, ACU::MAX_CONFIGTABLE_ADDRESS, - ACU::MAX_HKTABLE_ADDRESS, ACU::HK_TABLE_REPLY_SIZE), + ACU::MAX_HKTABLE_ADDRESS, P60ACU_HK_SIZE), coreHk(this), auxHk(this) {} @@ -13,7 +14,7 @@ ACUHandler::~ACUHandler() {} ReturnValue_t ACUHandler::buildNormalDeviceCommand(DeviceCommandId_t *id) { *id = GOMSPACE::REQUEST_HK_TABLE; - return buildCommandFromCommand(*id, NULL, 0); + return buildCommandFromCommand(*id, nullptr, 0); } void ACUHandler::fillCommandAndReplyMap() { GomspaceDeviceHandler::fillCommandAndReplyMap(); } @@ -48,7 +49,6 @@ LocalPoolDataSetBase *ACUHandler::getDataSetHandle(sid_t sid) { } ReturnValue_t ACUHandler::parseHkTableReply(const uint8_t *packet) { - uint16_t dataOffset = 0; PoolReadGuard pg0(&coreHk); PoolReadGuard pg1(&auxHk); auto res0 = pg0.getReadResult(); @@ -59,80 +59,51 @@ ReturnValue_t ACUHandler::parseHkTableReply(const uint8_t *packet) { if (res1 != RETURN_OK) { return res1; } - dataOffset += 12; for (size_t idx = 0; idx < 6; idx++) { - coreHk.currentInChannels[idx] = (packet[dataOffset] << 8) | packet[dataOffset + 1]; - dataOffset += 4; + coreHk.currentInChannels[idx] = as(packet + (idx*2)); } for (size_t idx = 0; idx < 6; idx++) { - coreHk.voltageInChannels[idx] = (packet[dataOffset] << 8) | packet[dataOffset + 1]; - dataOffset += 4; + coreHk.voltageInChannels[idx] = as(packet + 0xc + (idx*2)); } - coreHk.vcc = *(packet + dataOffset) << 8 | *(packet + dataOffset + 1); - dataOffset += 4; - coreHk.vbat = *(packet + dataOffset) << 8 | *(packet + dataOffset + 1); - dataOffset += 4; + coreHk.vcc = as(packet + 0x1a); + coreHk.vbat = as(packet + 0x18); for (size_t idx = 0; idx < 3; idx++) { - coreHk.temperatures[idx] = - static_cast((packet[dataOffset] << 8) | packet[dataOffset + 1]) * 0.1; - dataOffset += 4; + coreHk.temperatures[idx] = as(packet + 0x1c + (idx*2)) * 0.1; } - coreHk.mpptMode = packet[dataOffset]; - dataOffset += 3; + coreHk.mpptMode = packet[0x22]; for (size_t idx = 0; idx < 6; idx++) { - coreHk.vboostInChannels[idx] = (packet[dataOffset] << 8) | packet[dataOffset + 1]; - dataOffset += 4; + coreHk.vboostInChannels[idx] = as(packet + 0x24 + (idx*2)); } for (size_t idx = 0; idx < 6; idx++) { - coreHk.powerInChannels[idx] = (packet[dataOffset] << 8) | packet[dataOffset + 1]; - dataOffset += 4; + coreHk.powerInChannels[idx] = as(packet + 0x30 + (idx*2)); } for (size_t idx = 0; idx < 3; idx++) { - auxHk.dacEnables[idx] = packet[dataOffset]; - dataOffset += 3; + auxHk.dacEnables[idx] = *(packet + 0x3c + idx); } for (size_t idx = 0; idx < 6; idx++) { - auxHk.dacRawChannelVals[idx] = (packet[dataOffset] << 8) | packet[dataOffset + 1]; - dataOffset += 4; + auxHk.dacRawChannelVals[idx] = as(packet + 0x40 + (idx*2)); } - auxHk.bootCause = *(packet + dataOffset) << 24 | *(packet + dataOffset + 1) << 16 | - *(packet + dataOffset + 2) << 8 | *(packet + dataOffset + 3); - dataOffset += 6; - coreHk.bootcnt = *(packet + dataOffset) << 24 | *(packet + dataOffset + 1) << 16 | - *(packet + dataOffset + 2) << 8 | *(packet + dataOffset + 3); - dataOffset += 6; - coreHk.uptime = *(packet + dataOffset) << 24 | *(packet + dataOffset + 1) << 16 | - *(packet + dataOffset + 2) << 8 | *(packet + dataOffset + 3); - dataOffset += 6; - auxHk.resetCause = *(packet + dataOffset) << 8 | *(packet + dataOffset + 1); - dataOffset += 4; - coreHk.mpptTime = *(packet + dataOffset) << 8 | *(packet + dataOffset + 1); - /* +12 because here starts the second csp packet */ - dataOffset += 2 + 12; - - coreHk.mpptPeriod = *(packet + dataOffset) << 8 | *(packet + dataOffset + 1); - dataOffset += 4; + auxHk.bootCause = as(packet + 0x50); + coreHk.bootcnt = as(packet + 0x54); + coreHk.uptime = as(packet + 0x58); + auxHk.resetCause = as(packet + 0x5c); + coreHk.mpptTime = as(packet + 0x5e); + coreHk.mpptPeriod = as(packet + 0x60); for (size_t idx = 0; idx < 8; idx++) { - auxHk.deviceTypes[idx] = packet[dataOffset]; - dataOffset += 3; + auxHk.deviceTypes[idx] = *(packet + 0x64 + idx); } for (size_t idx = 0; idx < 8; idx++) { - auxHk.devicesStatus[idx] = packet[dataOffset]; - dataOffset += 3; + auxHk.devicesStatus[idx] = *(packet + 0x6c + idx); } - auxHk.wdtCntGnd = *(packet + dataOffset) << 24 | *(packet + dataOffset + 1) << 16 | - *(packet + dataOffset + 2) << 8 | *(packet + dataOffset + 3); - dataOffset += 6; - auxHk.wdtGndLeft = *(packet + dataOffset) << 24 | *(packet + dataOffset + 1) << 16 | - *(packet + dataOffset + 2) << 8 | *(packet + dataOffset + 3); - dataOffset += 6; + auxHk.wdtCntGnd = as(packet + 0x74); + auxHk.wdtGndLeft = as(packet + 0x78); coreHk.setValidity(true, true); auxHk.setValidity(true, true); return RETURN_OK; diff --git a/mission/devices/GomspaceDeviceHandler.cpp b/mission/devices/GomspaceDeviceHandler.cpp index c8659f7a..b5705faf 100644 --- a/mission/devices/GomspaceDeviceHandler.cpp +++ b/mission/devices/GomspaceDeviceHandler.cpp @@ -3,8 +3,6 @@ #include #include #include -#include -#include #include "devicedefinitions/GomSpacePackets.h" #include "devicedefinitions/powerDefinitions.h" @@ -82,14 +80,24 @@ ReturnValue_t GomspaceDeviceHandler::buildCommandFromCommand(DeviceCommandId_t d break; } case (GOMSPACE::REQUEST_HK_TABLE): { - result = generateRequestFullTableCmd(GOMSPACE::TableIds::HK, hkTableReplySize); + auto reqType = SpecialRequestTypes::DEFAULT_COM_IF; + if(getObjectId() == objects::PDU1_HANDLER or getObjectId() == objects::PDU2_HANDLER) { + reqType = SpecialRequestTypes::GET_PDU_HK; + } else if(getObjectId() == objects::ACU_HANDLER) { + reqType = SpecialRequestTypes::GET_ACU_HK; + } else if(getObjectId() == objects::P60DOCK_HANDLER) { + reqType = SpecialRequestTypes::GET_P60DOCK_HK; + } + result = generateRequestFullTableCmd(reqType, GOMSPACE::TableIds::HK, hkTableReplySize); if (result != HasReturnvaluesIF::RETURN_OK) { return result; } break; } case (GOMSPACE::REQUEST_CONFIG_TABLE): { - result = generateRequestFullTableCmd(GOMSPACE::TableIds::CONFIG, configTableReplySize); + + result = generateRequestFullTableCmd(SpecialRequestTypes::DEFAULT_COM_IF, + GOMSPACE::TableIds::CONFIG, configTableReplySize); if (result != HasReturnvaluesIF::RETURN_OK) { return result; } @@ -429,13 +437,14 @@ ReturnValue_t GomspaceDeviceHandler::generateResetWatchdogCmd() { return HasReturnvaluesIF::RETURN_OK; } -ReturnValue_t GomspaceDeviceHandler::generateRequestFullTableCmd(uint8_t tableId, +ReturnValue_t GomspaceDeviceHandler::generateRequestFullTableCmd(SpecialRequestTypes reqType, + uint8_t tableId, uint16_t tableReplySize) { uint16_t querySize = tableReplySize; - if(getObjectId() == objects::PDU1_HANDLER or getObjectId() == objects::PDU2_HANDLER) { + if(reqType != SpecialRequestTypes::DEFAULT_COM_IF) { auto* cspCookie = dynamic_cast(comCookie); - cspCookie->setRequest(CspCookie::SpecialRequestTypes::GET_PDU_HK, tableReplySize); - cspCookie->setCspPort(GOMSPACE::CspPorts::P60_PORT_RPARAM_ENUM); + cspCookie->setRequest(reqType, tableReplySize); + cspCookie->setCspPort(CspPorts::P60_PORT_RPARAM_ENUM); } else { RequestFullTableCommand requestFullTableCommand(querySize, tableId); diff --git a/mission/devices/GomspaceDeviceHandler.h b/mission/devices/GomspaceDeviceHandler.h index fbacaf07..9d90f484 100644 --- a/mission/devices/GomspaceDeviceHandler.h +++ b/mission/devices/GomspaceDeviceHandler.h @@ -1,6 +1,7 @@ #ifndef MISSION_DEVICES_GOMSPACEDEVICEHANDLER_H_ #define MISSION_DEVICES_GOMSPACEDEVICEHANDLER_H_ +#include #include #include "fsfw/devicehandlers/DeviceHandlerBase.h" @@ -82,7 +83,7 @@ class GomspaceDeviceHandler : public DeviceHandlerBase { * @brief The command to generate a request to receive the full housekeeping table is device * specific. Thus the child has to build this command. */ - virtual ReturnValue_t generateRequestFullTableCmd(uint8_t tableId, uint16_t tableSize); + virtual ReturnValue_t generateRequestFullTableCmd(GOMSPACE::SpecialRequestTypes reqType, uint8_t tableId, uint16_t tableSize); /** * This command handles printing the HK table to the console. This is useful for debugging diff --git a/mission/devices/P60DockHandler.cpp b/mission/devices/P60DockHandler.cpp index f059a1d7..b08b57fa 100644 --- a/mission/devices/P60DockHandler.cpp +++ b/mission/devices/P60DockHandler.cpp @@ -3,12 +3,13 @@ #include #include "OBSWConfig.h" +#include "p60dock_hk.h" P60DockHandler::P60DockHandler(object_id_t objectId, object_id_t comIF, CookieIF *comCookie, FailureIsolationBase *customFdir) : GomspaceDeviceHandler(objectId, comIF, comCookie, customFdir, P60Dock::MAX_CONFIGTABLE_ADDRESS, P60Dock::MAX_HKTABLE_ADDRESS, - P60Dock::HK_TABLE_REPLY_SIZE), + P60DOCK_HK_SIZE), coreHk(this), auxHk(this) {} @@ -30,7 +31,6 @@ void P60DockHandler::letChildHandleHkReply(DeviceCommandId_t id, const uint8_t * void P60DockHandler::parseHkTableReply(const uint8_t *packet) { using namespace P60Dock; - uint16_t dataOffset = 0; PoolReadGuard pg0(&coreHk); PoolReadGuard pg1(&auxHk); if (pg0.getReadResult() != HasReturnvaluesIF::RETURN_OK or @@ -43,45 +43,27 @@ void P60DockHandler::parseHkTableReply(const uint8_t *packet) { * Fist 10 bytes contain the gomspace header. Each variable is preceded by the 16-bit table * address. */ - dataOffset += 12; for (uint8_t idx = 0; idx < hk::CHNLS_LEN; idx++) { - coreHk.currents[idx] = *(packet + dataOffset) << 8 | *(packet + dataOffset + 1); - dataOffset += 4; + coreHk.currents[idx] = as(packet + (idx*2)); } for (uint8_t idx = 0; idx < hk::CHNLS_LEN; idx++) { - coreHk.voltages[idx] = *(packet + dataOffset) << 8 | *(packet + dataOffset + 1); - dataOffset += 4; + coreHk.voltages[idx] = as(packet + 0x1a + (idx*2)); } for (uint8_t idx = 0; idx < hk::CHNLS_LEN; idx++) { - coreHk.outputEnables[idx] = *(packet + dataOffset); - dataOffset += 3; + coreHk.outputEnables[idx] = *(packet + 0x34 + idx); } - coreHk.temperature1 = - static_cast(*(packet + dataOffset) << 8 | *(packet + dataOffset + 1)) * 0.1; - dataOffset += 4; - coreHk.temperature2 = - static_cast(*(packet + dataOffset) << 8 | *(packet + dataOffset + 1)) * 0.1; - dataOffset += 4; + coreHk.temperature1 = as(packet + 0x44) * 0.1; + coreHk.temperature2 = as(packet + 0x44 + 2) * 0.1; - auxHk.bootcause = *(packet + dataOffset) << 24 | - - *(packet + dataOffset + 1) << 16 | *(packet + dataOffset + 2) << 8 | - *(packet + dataOffset + 3); - dataOffset += 6; - coreHk.bootCount = *(packet + dataOffset) << 24 | *(packet + dataOffset + 1) << 16 | - *(packet + dataOffset + 2) << 8 | *(packet + dataOffset + 3); + auxHk.bootcause = as(packet + 0x48); + coreHk.bootCount = as(packet + 0x4c); if (firstHk) { triggerEvent(P60_BOOT_COUNT, coreHk.bootCount.value); } - dataOffset += 6; - auxHk.uptime = *(packet + dataOffset) << 24 | *(packet + dataOffset + 1) << 16 | - *(packet + dataOffset + 2) << 8 | *(packet + dataOffset + 3); - dataOffset += 6; - auxHk.resetcause = *(packet + dataOffset) << 8 | *(packet + dataOffset + 1); - - dataOffset += 4; - uint8_t newBattMode = packet[dataOffset]; + auxHk.uptime = as(packet + 0x50); + auxHk.resetcause = as(packet + 0x54); + uint8_t newBattMode = packet[0x56]; if (firstHk) { triggerEvent(BATT_MODE, newBattMode); } else if (newBattMode != coreHk.battMode.value) { @@ -89,83 +71,46 @@ void P60DockHandler::parseHkTableReply(const uint8_t *packet) { } coreHk.battMode = newBattMode; - dataOffset += 3; - auxHk.heaterOn = *(packet + dataOffset); - /* + 13 because here begins a new gomspace csp data field */ - dataOffset += 13; - auxHk.converter5VStatus = *(packet + dataOffset); - dataOffset += 3; + auxHk.heaterOn = *(packet + 0x57); + auxHk.converter5VStatus = *(packet + 0x58); for (uint8_t idx = 0; idx < hk::CHNLS_LEN; idx++) { - auxHk.latchups[idx] = *(packet + dataOffset) << 8 | *(packet + dataOffset + 1); - dataOffset += 4; + auxHk.latchups[idx] = as(packet + 0x5a + (idx*2)); } - auxHk.dockVbatVoltageValue = *(packet + dataOffset) << 8 | *(packet + dataOffset + 1); - dataOffset += 4; - auxHk.dockVccCurrent = *(packet + dataOffset) << 8 | *(packet + dataOffset + 1); - dataOffset += 4; - coreHk.batteryCurrent = *(packet + dataOffset) << 8 | *(packet + dataOffset + 1); - dataOffset += 4; - coreHk.batteryVoltage = *(packet + dataOffset) << 8 | *(packet + dataOffset + 1); - dataOffset += 4; + auxHk.dockVbatVoltageValue = as(packet + 0x74); + auxHk.dockVccCurrent = as(packet + 0x76); + coreHk.batteryCurrent = as(packet + 0x78); + coreHk.batteryVoltage = as(packet + 0x7a); - auxHk.batteryTemperature1 = *(packet + dataOffset) << 8 | *(packet + dataOffset + 1); - dataOffset += 4; - auxHk.batteryTemperature2 = *(packet + dataOffset) << 8 | *(packet + dataOffset + 1); - dataOffset += 4; + auxHk.batteryTemperature1 = as(packet + 0x7c); + auxHk.batteryTemperature2 = as(packet + 0x7c + 2); for (uint8_t idx = 0; idx < NUM_DEVS; idx++) { - auxHk.devicesType[idx] = *(packet + dataOffset); - dataOffset += 3; + auxHk.devicesType[idx] = *(packet + 0x80 + idx); } for (uint8_t idx = 0; idx < NUM_DEVS; idx++) { - auxHk.devicesStatus[idx] = *(packet + dataOffset); - dataOffset += 3; + auxHk.devicesStatus[idx] = *(packet + 0x88 + idx); } - auxHk.dearmStatus = *(packet + dataOffset); - dataOffset += 3; + auxHk.dearmStatus = *(packet + 0x90); - auxHk.wdtCntGnd = *(packet + dataOffset) << 24 | *(packet + dataOffset + 1) << 16 | - *(packet + dataOffset + 2) << 8 | *(packet + dataOffset + 3); - dataOffset += 6; - auxHk.wdtCntI2c = *(packet + dataOffset) << 24 | *(packet + dataOffset + 1) << 16 | - *(packet + dataOffset + 2) << 8 | *(packet + dataOffset + 3); - dataOffset += 6; - auxHk.wdtCntCan = *(packet + dataOffset) << 24 | *(packet + dataOffset + 1) << 16 | - *(packet + dataOffset + 2) << 8 | *(packet + dataOffset + 3); - dataOffset += 6; - auxHk.wdtCntCsp1 = *(packet + dataOffset) << 24 | *(packet + dataOffset + 1) << 16 | - *(packet + dataOffset + 2) << 8 | *(packet + dataOffset + 3); - dataOffset += 6; - auxHk.wdtCntCsp2 = *(packet + dataOffset) << 24 | *(packet + dataOffset + 1) << 16 | - *(packet + dataOffset + 2) << 8 | *(packet + dataOffset + 3); - dataOffset += 6; + auxHk.wdtCntGnd = as(packet + 0x94); + auxHk.wdtCntI2c = as(packet + 0x98); + auxHk.wdtCntCan = as(packet + 0x9c); + auxHk.wdtCntCsp1 = as(packet + 0xa0); + auxHk.wdtCntCsp2 = as(packet + 0xa0 + 4); + auxHk.wdtGndLeft = as(packet + 0xa8); + auxHk.wdtI2cLeft = as(packet + 0xac); + auxHk.wdtCanLeft = as(packet + 0xb0); - auxHk.wdtGndLeft = *(packet + dataOffset) << 24 | *(packet + dataOffset + 1) << 16 | - *(packet + dataOffset + 2) << 8 | *(packet + dataOffset + 3); - dataOffset += 6; - auxHk.wdtI2cLeft = *(packet + dataOffset) << 24 | *(packet + dataOffset + 1) << 16 | - *(packet + dataOffset + 2) << 8 | *(packet + dataOffset + 3); - dataOffset += 6; - auxHk.wdtCanLeft = *(packet + dataOffset) << 24 | *(packet + dataOffset + 1) << 16 | - *(packet + dataOffset + 2) << 8 | *(packet + dataOffset + 3); - /* +16 because here begins a new gomspace csp packet */ - dataOffset += 16; + auxHk.wdtCspLeft1 = *(packet + 0xb4); + auxHk.wdtCspLeft2 = *(packet + 0xb4 + 1); - auxHk.wdtCspLeft1 = *(packet + dataOffset); - dataOffset += 3; - auxHk.wdtCspLeft2 = *(packet + dataOffset); - dataOffset += 3; - - auxHk.batteryChargeCurrent = *(packet + dataOffset) << 8 | *(packet + dataOffset + 1); - dataOffset += 4; - auxHk.batteryDischargeCurrent = *(packet + dataOffset) << 8 | *(packet + dataOffset + 1); - dataOffset += 4; - auxHk.ant6Depl = *(packet + dataOffset); - dataOffset += 3; - auxHk.ar6Depl = *(packet + dataOffset); + auxHk.batteryChargeCurrent = as(packet + 0xb6); + auxHk.batteryDischargeCurrent = as(packet + 0xb8); + auxHk.ant6Depl = *(packet + 0xba); + auxHk.ar6Depl = *(packet + 0xbb); if (firstHk) { firstHk = false; } diff --git a/mission/devices/devicedefinitions/GomspaceDefinitions.h b/mission/devices/devicedefinitions/GomspaceDefinitions.h index ae1a2e83..c4b0e39a 100644 --- a/mission/devices/devicedefinitions/GomspaceDefinitions.h +++ b/mission/devices/devicedefinitions/GomspaceDefinitions.h @@ -12,6 +12,16 @@ namespace GOMSPACE { +enum SpecialRequestTypes { + DEFAULT_COM_IF, + GET_PDU_HK, + GET_PDU_CONFIG, + GET_ACU_HK, + GET_ACU_CONFIG, + GET_P60DOCK_HK, + GET_P60DOCK_CONFIG +}; + enum CspPorts: uint8_t { CSP_PING = 1, CSP_REBOOT = 4, diff --git a/thirdparty/gomspace-sw b/thirdparty/gomspace-sw index 0b66e23a..d24a574f 160000 --- a/thirdparty/gomspace-sw +++ b/thirdparty/gomspace-sw @@ -1 +1 @@ -Subproject commit 0b66e23a8900e315f01cfc52088adad10bdabf26 +Subproject commit d24a574ffd32bd4da8aa69e768180ccae76d6c85 diff --git a/tmtc b/tmtc index d61af604..079a0f94 160000 --- a/tmtc +++ b/tmtc @@ -1 +1 @@ -Subproject commit d61af604fec53b6a0af8d54e7c01792fc9a68790 +Subproject commit 079a0f94727dc3f35578dc412aa01c871ae1ac6a From e804d433aa92974596242857df98c08f9bf933fd Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Fri, 26 Aug 2022 13:46:18 +0200 Subject: [PATCH 21/52] merge conflict --- mission/devices/GomspaceDeviceHandler.cpp | 12 ------------ 1 file changed, 12 deletions(-) diff --git a/mission/devices/GomspaceDeviceHandler.cpp b/mission/devices/GomspaceDeviceHandler.cpp index e17855fe..0046a119 100644 --- a/mission/devices/GomspaceDeviceHandler.cpp +++ b/mission/devices/GomspaceDeviceHandler.cpp @@ -448,7 +448,6 @@ ReturnValue_t GomspaceDeviceHandler::generateRequestFullTableCmd(SpecialRequestT } else { RequestFullTableCommand requestFullTableCommand(querySize, tableId); -<<<<<<< HEAD size_t cspPacketLen = 0; uint8_t* buffer = cspPacket; ReturnValue_t result = requestFullTableCommand.serialize( @@ -461,17 +460,6 @@ ReturnValue_t GomspaceDeviceHandler::generateRequestFullTableCmd(SpecialRequestT } rawPacket = cspPacket; rawPacketLen = cspPacketLen; -======= - size_t cspPacketLen = 0; - uint8_t* buffer = cspPacket; - ReturnValue_t result = requestFullTableCommand.serialize( - &buffer, &cspPacketLen, sizeof(cspPacket), SerializeIF::Endianness::BIG); - if (result != returnvalue::OK) { - sif::error << "GomspaceDeviceHandler::generateRequestFullHkTableCmd Failed to serialize " - "full table request command " - << std::endl; - return result; ->>>>>>> origin/develop } rememberRequestedSize = querySize; From 8c110460a6aae3adc8c271b68d9840c291343bde Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Fri, 26 Aug 2022 14:28:06 +0200 Subject: [PATCH 22/52] added test to retrieve pdu config --- CMakeLists.txt | 1 + bsp_hosted/comIF/ArduinoComIF.cpp | 4 +- bsp_q7s/boardtest/Q7STestTask.cpp | 16 ++++ bsp_q7s/boardtest/Q7STestTask.h | 1 + bsp_q7s/core/ObjectFactory.cpp | 10 +-- bsp_q7s/memory/SdCardManager.h | 15 ++-- dummies/SusDummy.cpp | 4 +- linux/csp/CspComIF.cpp | 35 ++++++-- .../devicedefinitions/PlocMPSoCDefinitions.h | 3 +- linux/devices/startracker/StrHelper.h | 3 +- linux/obc/PdecHandler.h | 4 +- mission/csp/CspCookie.cpp | 28 ++----- mission/csp/CspCookie.h | 7 +- mission/devices/ACUHandler.cpp | 25 +++--- mission/devices/ACUHandler.h | 1 + mission/devices/GomspaceDeviceHandler.cpp | 82 +++++++++---------- mission/devices/GomspaceDeviceHandler.h | 30 +++---- mission/devices/P60DockHandler.cpp | 24 +++--- mission/devices/P60DockHandler.h | 1 + mission/devices/PDU1Handler.cpp | 8 +- mission/devices/PDU1Handler.h | 1 + mission/devices/PDU2Handler.cpp | 8 +- mission/devices/PDU2Handler.h | 1 + .../devicedefinitions/GomspaceDefinitions.h | 43 ++++------ 24 files changed, 185 insertions(+), 170 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 3f159cc4..f85fda0f 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -467,6 +467,7 @@ endif() if(ADD_GOMSPACE_CLIENTS) target_link_libraries(${OBSW_NAME} PRIVATE ${LIB_GOMSPACE_CLIENTS}) target_link_libraries(${LIB_EIVE_MISSION} PRIVATE ${LIB_GOMSPACE_CLIENTS}) + target_link_libraries(${LIB_DUMMIES} PRIVATE ${LIB_GOMSPACE_CLIENTS}) endif() if(EIVE_ADD_ETL_LIB) diff --git a/bsp_hosted/comIF/ArduinoComIF.cpp b/bsp_hosted/comIF/ArduinoComIF.cpp index b669233f..f9206f77 100644 --- a/bsp_hosted/comIF/ArduinoComIF.cpp +++ b/bsp_hosted/comIF/ArduinoComIF.cpp @@ -129,9 +129,7 @@ ArduinoComIF::~ArduinoComIF() { CloseHandle(hCom); #endif } -ReturnValue_t ArduinoComIF::initializeInterface(CookieIF *cookie) { - return returnvalue::OK; -} +ReturnValue_t ArduinoComIF::initializeInterface(CookieIF *cookie) { return returnvalue::OK; } ReturnValue_t ArduinoComIF::sendMessage(CookieIF *cookie, const uint8_t *data, size_t len) { ArduinoCookie *arduinoCookie = dynamic_cast(cookie); diff --git a/bsp_q7s/boardtest/Q7STestTask.cpp b/bsp_q7s/boardtest/Q7STestTask.cpp index 7a5e9617..28c88ad2 100644 --- a/bsp_q7s/boardtest/Q7STestTask.cpp +++ b/bsp_q7s/boardtest/Q7STestTask.cpp @@ -8,6 +8,7 @@ #include #include #include +#include #include #include @@ -50,6 +51,21 @@ ReturnValue_t Q7STestTask::performOneShotAction() { } } + if (DO_TEST_GOMSPACE_GET_CONFIG) { + uint8_t p60pdu_node = 3; + param_index_t requestStruct{}; + requestStruct.table = p60pdu_config; + requestStruct.mem_id = P60PDU_PARAM; + uint8_t hk_mem[P60PDU_PARAM_SIZE]; + requestStruct.count = p60pdu_config_count; + requestStruct.size = P60PDU_PARAM_SIZE; + requestStruct.physaddr = hk_mem; + int result = rparam_get_full_table(&requestStruct, p60pdu_node, P60_PORT_RPARAM, + requestStruct.mem_id, 1000); + param_list(&requestStruct, 1); + return (result == 0); + } + // testJsonLibDirect(); // testDummyParams(); if (doTestProtHandler) { diff --git a/bsp_q7s/boardtest/Q7STestTask.h b/bsp_q7s/boardtest/Q7STestTask.h index 97e68c66..8d39311d 100644 --- a/bsp_q7s/boardtest/Q7STestTask.h +++ b/bsp_q7s/boardtest/Q7STestTask.h @@ -17,6 +17,7 @@ class Q7STestTask : public TestTask { bool doTestSdCard = false; bool doTestScratchApi = false; static constexpr bool DO_TEST_GOMSPACE_API = false; + static constexpr bool DO_TEST_GOMSPACE_GET_CONFIG = true; bool doTestGpsShm = false; bool doTestGpsSocket = false; bool doTestProtHandler = false; diff --git a/bsp_q7s/core/ObjectFactory.cpp b/bsp_q7s/core/ObjectFactory.cpp index 69345830..6627d90d 100644 --- a/bsp_q7s/core/ObjectFactory.cpp +++ b/bsp_q7s/core/ObjectFactory.cpp @@ -20,7 +20,6 @@ #include "linux/boardtest/UartTestClass.h" #include "linux/callbacks/gpioCallbacks.h" #include "linux/csp/CspComIF.h" -#include "mission/csp/CspCookie.h" #include "linux/devices/GPSHyperionLinuxController.h" #include "linux/devices/devicedefinitions/PlocMPSoCDefinitions.h" #include "linux/devices/devicedefinitions/StarTrackerDefinitions.h" @@ -35,6 +34,7 @@ #include "linux/obc/PdecHandler.h" #include "linux/obc/Ptme.h" #include "linux/obc/PtmeConfig.h" +#include "mission/csp/CspCookie.h" #include "mission/system/RwAssembly.h" #include "mission/system/fdir/AcsBoardFdir.h" #include "mission/system/fdir/GomspacePowerFdir.h" @@ -158,10 +158,10 @@ void ObjectFactory::createCommunicationInterfaces(LinuxLibgpioIF** gpioComIF, Ua } void ObjectFactory::createPcduComponents(LinuxLibgpioIF* gpioComIF, PowerSwitchIF** pwrSwitcher) { - CspCookie* p60DockCspCookie = new CspCookie(P60Dock::MAX_REPLY_LENGTH, addresses::P60DOCK, 500); - CspCookie* pdu1CspCookie = new CspCookie(PDU::MAX_REPLY_LENGTH, addresses::PDU1, 500); - CspCookie* pdu2CspCookie = new CspCookie(PDU::MAX_REPLY_LENGTH, addresses::PDU2, 500); - CspCookie* acuCspCookie = new CspCookie(ACU::MAX_REPLY_LENGTH, addresses::ACU, 500); + CspCookie* p60DockCspCookie = new CspCookie(P60Dock::MAX_REPLY_SIZE, addresses::P60DOCK, 500); + CspCookie* pdu1CspCookie = new CspCookie(PDU::MAX_REPLY_SIZE, addresses::PDU1, 500); + CspCookie* pdu2CspCookie = new CspCookie(PDU::MAX_REPLY_SIZE, addresses::PDU2, 500); + CspCookie* acuCspCookie = new CspCookie(ACU::MAX_REPLY_SIZE, addresses::ACU, 500); auto p60Fdir = new GomspacePowerFdir(objects::P60DOCK_HANDLER); P60DockHandler* p60dockhandler = diff --git a/bsp_q7s/memory/SdCardManager.h b/bsp_q7s/memory/SdCardManager.h index 796c7ba2..87ed91d7 100644 --- a/bsp_q7s/memory/SdCardManager.h +++ b/bsp_q7s/memory/SdCardManager.h @@ -46,20 +46,15 @@ class SdCardManager : public SystemObject, public SdCardMountedIF { static constexpr ReturnValue_t OP_ONGOING = returnvalue::makeCode(INTERFACE_ID, 0); static constexpr ReturnValue_t ALREADY_ON = returnvalue::makeCode(INTERFACE_ID, 1); - static constexpr ReturnValue_t ALREADY_MOUNTED = - returnvalue::makeCode(INTERFACE_ID, 2); + static constexpr ReturnValue_t ALREADY_MOUNTED = returnvalue::makeCode(INTERFACE_ID, 2); static constexpr ReturnValue_t ALREADY_OFF = returnvalue::makeCode(INTERFACE_ID, 3); - static constexpr ReturnValue_t STATUS_FILE_NEXISTS = - returnvalue::makeCode(INTERFACE_ID, 10); + static constexpr ReturnValue_t STATUS_FILE_NEXISTS = returnvalue::makeCode(INTERFACE_ID, 10); static constexpr ReturnValue_t STATUS_FILE_FORMAT_INVALID = returnvalue::makeCode(INTERFACE_ID, 11); static constexpr ReturnValue_t MOUNT_ERROR = returnvalue::makeCode(INTERFACE_ID, 12); - static constexpr ReturnValue_t UNMOUNT_ERROR = - returnvalue::makeCode(INTERFACE_ID, 13); - static constexpr ReturnValue_t SYSTEM_CALL_ERROR = - returnvalue::makeCode(INTERFACE_ID, 14); - static constexpr ReturnValue_t POPEN_CALL_ERROR = - returnvalue::makeCode(INTERFACE_ID, 15); + static constexpr ReturnValue_t UNMOUNT_ERROR = returnvalue::makeCode(INTERFACE_ID, 13); + static constexpr ReturnValue_t SYSTEM_CALL_ERROR = returnvalue::makeCode(INTERFACE_ID, 14); + static constexpr ReturnValue_t POPEN_CALL_ERROR = returnvalue::makeCode(INTERFACE_ID, 15); static constexpr uint8_t SUBSYSTEM_ID = SUBSYSTEM_ID::FILE_SYSTEM; diff --git a/dummies/SusDummy.cpp b/dummies/SusDummy.cpp index 2cddd03e..2a2a1bda 100644 --- a/dummies/SusDummy.cpp +++ b/dummies/SusDummy.cpp @@ -33,7 +33,9 @@ ReturnValue_t SusDummy::initialize() { return returnvalue::OK; } -ReturnValue_t SusDummy::handleCommandMessage(CommandMessage* message) { return returnvalue::FAILED; } +ReturnValue_t SusDummy::handleCommandMessage(CommandMessage* message) { + return returnvalue::FAILED; +} void SusDummy::performControlOperation() { iteration++; diff --git a/linux/csp/CspComIF.cpp b/linux/csp/CspComIF.cpp index 1c76c802..18d77cfd 100644 --- a/linux/csp/CspComIF.cpp +++ b/linux/csp/CspComIF.cpp @@ -4,9 +4,11 @@ #include #include #include -#include #include #include +#include +#include +#include #include "mission/csp/CspCookie.h" @@ -86,7 +88,7 @@ ReturnValue_t CspComIF::sendMessage(CookieIF* cookie, const uint8_t* sendData, s uint8_t cspPort; uint16_t querySize = 0; - if(cspCookie->getRequest() == GOMSPACE::SpecialRequestTypes::DEFAULT_COM_IF) { + if (cspCookie->getRequest() == GOMSPACE::SpecialRequestTypes::DEFAULT_COM_IF) { /* Extract csp port and bytes to query from command buffer */ result = getPortAndQuerySize(&sendData, &sendLen, &cspPort, &querySize); if (result != returnvalue::OK) { @@ -96,6 +98,11 @@ ReturnValue_t CspComIF::sendMessage(CookieIF* cookie, const uint8_t* sendData, s cspPort = cspCookie->getCspPort(); querySize = cspCookie->getReplyLen(); } + if (querySize > cspCookie->getMaxReplyLength()) { + sif::error << "Query size " << querySize << " is larger than maximum allowed " + << cspCookie->getMaxReplyLength() << std::endl; + return returnvalue::FAILED; + } uint8_t cspAddress = cspCookie->getCspAddress(); switch (cspPort) { case (CspPorts::CSP_PING): { @@ -108,22 +115,32 @@ ReturnValue_t CspComIF::sendMessage(CookieIF* cookie, const uint8_t* sendData, s } case (CspPorts::P60_PORT_GNDWDT_RESET_ENUM): case (CspPorts::P60_PORT_RPARAM_ENUM): { - if(cspCookie->getRequest() != SpecialRequestTypes::DEFAULT_COM_IF) { + if (cspCookie->getRequest() != SpecialRequestTypes::DEFAULT_COM_IF) { param_index_t requestStruct{}; requestStruct.physaddr = cspDeviceMap[cspAddress].data(); - if(cspCookie->getRequest() == GOMSPACE::SpecialRequestTypes::GET_PDU_HK) { - if(!p60pdu_get_hk(&requestStruct, cspAddress, cspCookie->getTimeout())) { + auto req = cspCookie->getRequest(); + if (req == GOMSPACE::SpecialRequestTypes::GET_PDU_HK) { + if (!p60pdu_get_hk(&requestStruct, cspAddress, cspCookie->getTimeout())) { return returnvalue::FAILED; } - } else if(cspCookie->getRequest() == GOMSPACE::SpecialRequestTypes::GET_ACU_HK) { - if(!p60acu_get_hk(&requestStruct, cspAddress, cspCookie->getTimeout())) { + } else if (req == GOMSPACE::SpecialRequestTypes::GET_ACU_HK) { + if (!p60acu_get_hk(&requestStruct, cspAddress, cspCookie->getTimeout())) { return returnvalue::FAILED; } - } else if(cspCookie->getRequest() == GOMSPACE::SpecialRequestTypes::GET_P60DOCK_HK) { - if(!p60dock_get_hk(&requestStruct, cspAddress, cspCookie->getTimeout())) { + } else if (req == GOMSPACE::SpecialRequestTypes::GET_P60DOCK_HK) { + if (!p60dock_get_hk(&requestStruct, cspAddress, cspCookie->getTimeout())) { return returnvalue::FAILED; } + } else if (req == GOMSPACE::SpecialRequestTypes::GET_PDU_CONFIG) { + requestStruct.table = p60pdu_config; + requestStruct.mem_id = P60PDU_PARAM; + requestStruct.count = p60pdu_config_count; + requestStruct.size = P60PDU_PARAM_SIZE; + int result = rparam_get_full_table(&requestStruct, cspAddress, P60_PORT_RPARAM, + requestStruct.mem_id, cspCookie->getTimeout()); + param_list(&requestStruct, 1); + return (result == 0); } } else { /* No CSP fixed port was selected. Send data to the specified port and diff --git a/linux/devices/devicedefinitions/PlocMPSoCDefinitions.h b/linux/devices/devicedefinitions/PlocMPSoCDefinitions.h index 3423c54c..a5ed0672 100644 --- a/linux/devices/devicedefinitions/PlocMPSoCDefinitions.h +++ b/linux/devices/devicedefinitions/PlocMPSoCDefinitions.h @@ -458,8 +458,7 @@ class TcReplayStart : public TcBase { static const uint8_t ONCE = 1; ReturnValue_t lengthCheck(size_t commandDataLen) { - if (commandDataLen != COMMAND_DATA_LENGTH or - checkPayloadLen() != returnvalue::OK) { + if (commandDataLen != COMMAND_DATA_LENGTH or checkPayloadLen() != returnvalue::OK) { sif::warning << "TcReplayStart: Command has invalid length " << commandDataLen << std::endl; return INVALID_LENGTH; } diff --git a/linux/devices/startracker/StrHelper.h b/linux/devices/startracker/StrHelper.h index 0e6a912e..1bfff60b 100644 --- a/linux/devices/startracker/StrHelper.h +++ b/linux/devices/startracker/StrHelper.h @@ -323,7 +323,8 @@ class StrHelper : public SystemObject, public ExecutableObjectIF { * * @param expectedPosition Value of expected position * - * @return returnvalue::OK if received position matches expected position, otherwise returnvalue::FAILED + * @return returnvalue::OK if received position matches expected position, otherwise + * returnvalue::FAILED */ ReturnValue_t checkReplyPosition(uint32_t expectedPosition); diff --git a/linux/obc/PdecHandler.h b/linux/obc/PdecHandler.h index 2c6a200e..57adfa9f 100644 --- a/linux/obc/PdecHandler.h +++ b/linux/obc/PdecHandler.h @@ -31,9 +31,7 @@ * * @author J. Meier */ -class PdecHandler : public SystemObject, - public ExecutableObjectIF, - public HasActionsIF { +class PdecHandler : public SystemObject, public ExecutableObjectIF, public HasActionsIF { public: /** * @brief Constructor diff --git a/mission/csp/CspCookie.cpp b/mission/csp/CspCookie.cpp index a2265f9e..f6ab1eed 100644 --- a/mission/csp/CspCookie.cpp +++ b/mission/csp/CspCookie.cpp @@ -1,38 +1,28 @@ #include "CspCookie.h" CspCookie::CspCookie(uint16_t maxReplyLength_, uint8_t cspAddress_, uint32_t timeoutMs) - : maxReplyLength(maxReplyLength_), cspAddress(cspAddress_), timeoutMs(timeoutMs), + : maxReplyLength(maxReplyLength_), + cspAddress(cspAddress_), + timeoutMs(timeoutMs), reqType(GOMSPACE::DEFAULT_COM_IF) {} CspCookie::~CspCookie() {} uint16_t CspCookie::getMaxReplyLength() { return maxReplyLength; } -uint8_t CspCookie::getCspAddress() { - return cspAddress; -} +uint8_t CspCookie::getCspAddress() { return cspAddress; } -GOMSPACE::SpecialRequestTypes CspCookie::getRequest() const { - return reqType; -} +GOMSPACE::SpecialRequestTypes CspCookie::getRequest() const { return reqType; } void CspCookie::setRequest(GOMSPACE::SpecialRequestTypes request, size_t replyLen_) { reqType = request; replyLen = replyLen_; } -uint8_t CspCookie::getCspPort() const { - return cspPort; -} +uint8_t CspCookie::getCspPort() const { return cspPort; } -uint32_t CspCookie::getTimeout() const { - return timeoutMs; -} +uint32_t CspCookie::getTimeout() const { return timeoutMs; } -void CspCookie::setCspPort(uint8_t port) { - cspPort = port; -} +void CspCookie::setCspPort(uint8_t port) { cspPort = port; } -size_t CspCookie::getReplyLen() const { - return replyLen; -} +size_t CspCookie::getReplyLen() const { return replyLen; } diff --git a/mission/csp/CspCookie.h b/mission/csp/CspCookie.h index 71b50e61..441eb413 100644 --- a/mission/csp/CspCookie.h +++ b/mission/csp/CspCookie.h @@ -2,9 +2,11 @@ #define LINUX_CSP_CSPCOOKIE_H_ #include -#include "mission/devices/devicedefinitions/GomspaceDefinitions.h" -#include + #include +#include + +#include "mission/devices/devicedefinitions/GomspaceDefinitions.h" /** * @brief This is the cookie for devices supporting the CSP (CubeSat Space @@ -13,7 +15,6 @@ */ class CspCookie : public CookieIF { public: - CspCookie(uint16_t maxReplyLength_, uint8_t cspAddress_, uint32_t timeoutMs); virtual ~CspCookie(); diff --git a/mission/devices/ACUHandler.cpp b/mission/devices/ACUHandler.cpp index ac78f323..28cd9e4b 100644 --- a/mission/devices/ACUHandler.cpp +++ b/mission/devices/ACUHandler.cpp @@ -1,14 +1,17 @@ #include "ACUHandler.h" -#include "OBSWConfig.h" -#include "p60acu_hk.h" +#include "OBSWConfig.h" ACUHandler::ACUHandler(object_id_t objectId, object_id_t comIF, CookieIF *comCookie, FailureIsolationBase *customFdir) - : GomspaceDeviceHandler(objectId, comIF, comCookie, customFdir, ACU::MAX_CONFIGTABLE_ADDRESS, - ACU::MAX_HKTABLE_ADDRESS, P60ACU_HK_SIZE), + : GomspaceDeviceHandler(objectId, comIF, comCookie, cfg, customFdir), coreHk(this), - auxHk(this) {} + auxHk(this) { + cfg.maxConfigTableAddress = ACU::MAX_CONFIGTABLE_ADDRESS; + cfg.maxHkTableAddress = ACU::MAX_HKTABLE_ADDRESS; + cfg.hkTableSize = ACU::HK_TABLE_SIZE; + cfg.cfgTableSize = ACU::CONFIG_TABLE_SIZE; +} ACUHandler::~ACUHandler() {} @@ -60,32 +63,32 @@ ReturnValue_t ACUHandler::parseHkTableReply(const uint8_t *packet) { return res1; } for (size_t idx = 0; idx < 6; idx++) { - coreHk.currentInChannels[idx] = as(packet + (idx*2)); + coreHk.currentInChannels[idx] = as(packet + (idx * 2)); } for (size_t idx = 0; idx < 6; idx++) { - coreHk.voltageInChannels[idx] = as(packet + 0xc + (idx*2)); + coreHk.voltageInChannels[idx] = as(packet + 0xc + (idx * 2)); } coreHk.vcc = as(packet + 0x1a); coreHk.vbat = as(packet + 0x18); for (size_t idx = 0; idx < 3; idx++) { - coreHk.temperatures[idx] = as(packet + 0x1c + (idx*2)) * 0.1; + coreHk.temperatures[idx] = as(packet + 0x1c + (idx * 2)) * 0.1; } coreHk.mpptMode = packet[0x22]; for (size_t idx = 0; idx < 6; idx++) { - coreHk.vboostInChannels[idx] = as(packet + 0x24 + (idx*2)); + coreHk.vboostInChannels[idx] = as(packet + 0x24 + (idx * 2)); } for (size_t idx = 0; idx < 6; idx++) { - coreHk.powerInChannels[idx] = as(packet + 0x30 + (idx*2)); + coreHk.powerInChannels[idx] = as(packet + 0x30 + (idx * 2)); } for (size_t idx = 0; idx < 3; idx++) { auxHk.dacEnables[idx] = *(packet + 0x3c + idx); } for (size_t idx = 0; idx < 6; idx++) { - auxHk.dacRawChannelVals[idx] = as(packet + 0x40 + (idx*2)); + auxHk.dacRawChannelVals[idx] = as(packet + 0x40 + (idx * 2)); } auxHk.bootCause = as(packet + 0x50); diff --git a/mission/devices/ACUHandler.h b/mission/devices/ACUHandler.h index 3154e547..cf5e2517 100644 --- a/mission/devices/ACUHandler.h +++ b/mission/devices/ACUHandler.h @@ -38,6 +38,7 @@ class ACUHandler : public GomspaceDeviceHandler { private: ACU::CoreHk coreHk; ACU::AuxHk auxHk; + TableConfig cfg; bool debugMode = false; /** diff --git a/mission/devices/GomspaceDeviceHandler.cpp b/mission/devices/GomspaceDeviceHandler.cpp index 0046a119..dc9314ae 100644 --- a/mission/devices/GomspaceDeviceHandler.cpp +++ b/mission/devices/GomspaceDeviceHandler.cpp @@ -4,25 +4,29 @@ #include #include +#include + #include "devicedefinitions/GomSpacePackets.h" #include "devicedefinitions/powerDefinitions.h" -#include using namespace GOMSPACE; GomspaceDeviceHandler::GomspaceDeviceHandler(object_id_t objectId, object_id_t comIF, - CookieIF* comCookie, FailureIsolationBase* customFdir, - uint16_t maxConfigTableAddress, - uint16_t maxHkTableAddress, uint16_t hkTableReplySize) - : DeviceHandlerBase(objectId, comIF, comCookie, customFdir), - maxConfigTableAddress(maxConfigTableAddress), - maxHkTableAddress(maxHkTableAddress), - hkTableReplySize(hkTableReplySize) { + CookieIF* comCookie, TableConfig& tableConfig, + FailureIsolationBase* customFdir) + : DeviceHandlerBase(objectId, comIF, comCookie, customFdir), tableCfg(tableConfig) { if (comCookie == nullptr) { sif::error << "GomspaceDeviceHandler::GomspaceDeviceHandler: Invalid com cookie" << std::endl; } } +void GomspaceDeviceHandler::initPduConfigTable() { + tableCfg.maxConfigTableAddress = PDU::MAX_CONFIGTABLE_ADDRESS; + tableCfg.maxHkTableAddress = PDU::MAX_HKTABLE_ADDRESS; + tableCfg.hkTableSize = PDU::HK_TABLE_SIZE; + tableCfg.cfgTableSize = PDU::CONFIG_TABLE_SIZE; +} + GomspaceDeviceHandler::~GomspaceDeviceHandler() {} void GomspaceDeviceHandler::doStartUp() {} @@ -81,23 +85,28 @@ ReturnValue_t GomspaceDeviceHandler::buildCommandFromCommand(DeviceCommandId_t d } case (GOMSPACE::REQUEST_HK_TABLE): { auto reqType = SpecialRequestTypes::DEFAULT_COM_IF; - if(getObjectId() == objects::PDU1_HANDLER or getObjectId() == objects::PDU2_HANDLER) { + if (getObjectId() == objects::PDU1_HANDLER or getObjectId() == objects::PDU2_HANDLER) { reqType = SpecialRequestTypes::GET_PDU_HK; - } else if(getObjectId() == objects::ACU_HANDLER) { + } else if (getObjectId() == objects::ACU_HANDLER) { reqType = SpecialRequestTypes::GET_ACU_HK; - } else if(getObjectId() == objects::P60DOCK_HANDLER) { + } else if (getObjectId() == objects::P60DOCK_HANDLER) { reqType = SpecialRequestTypes::GET_P60DOCK_HK; } - result = generateRequestFullTableCmd(reqType, GOMSPACE::TableIds::HK, hkTableReplySize); + result = generateRequestFullTableCmd(reqType, GOMSPACE::TableIds::HK, tableCfg.hkTableSize); if (result != returnvalue::OK) { return result; } break; } case (GOMSPACE::REQUEST_CONFIG_TABLE): { - - result = generateRequestFullTableCmd(SpecialRequestTypes::DEFAULT_COM_IF, - GOMSPACE::TableIds::CONFIG, configTableReplySize); + auto reqType = SpecialRequestTypes::DEFAULT_COM_IF; + if (getObjectId() == objects::PDU1_HANDLER or getObjectId() == objects::PDU2_HANDLER) { + reqType = SpecialRequestTypes::GET_PDU_CONFIG; + } else { + reqType = SpecialRequestTypes::DEFAULT_COM_IF; + } + result = + generateRequestFullTableCmd(reqType, GOMSPACE::TableIds::CONFIG, tableCfg.cfgTableSize); if (result != returnvalue::OK) { return result; } @@ -230,7 +239,7 @@ ReturnValue_t GomspaceDeviceHandler::generateSetParamCommand(const uint8_t* comm } /* Get and check address */ uint16_t address = setParamCacher.getAddress(); - if (address > maxConfigTableAddress) { + if (address > tableCfg.maxConfigTableAddress) { sif::error << "GomspaceDeviceHandler: Invalid address for set parameter " << "action" << std::endl; return INVALID_ADDRESS; @@ -288,12 +297,12 @@ ReturnValue_t GomspaceDeviceHandler::generateGetParamCommand(const uint8_t* comm } /* Get and check address */ uint16_t address = getParamMessage.getAddress(); - if (address > maxHkTableAddress and tableId == TableIds::HK) { + if (address > tableCfg.maxHkTableAddress and tableId == TableIds::HK) { sif::error << "GomspaceDeviceHandler: Invalid address to get parameter from " << "housekeeping table" << std::endl; return INVALID_ADDRESS; } - if (address > maxConfigTableAddress and tableId == TableIds::CONFIG) { + if (address > tableCfg.maxConfigTableAddress and tableId == TableIds::CONFIG) { sif::error << "GomspaceDeviceHandler: Invalid address to get parameter from " << "configuration table" << std::endl; return INVALID_ADDRESS; @@ -438,30 +447,16 @@ ReturnValue_t GomspaceDeviceHandler::generateResetWatchdogCmd() { } ReturnValue_t GomspaceDeviceHandler::generateRequestFullTableCmd(SpecialRequestTypes reqType, - uint8_t tableId, + uint8_t tableId, uint16_t tableReplySize) { uint16_t querySize = tableReplySize; - if(reqType != SpecialRequestTypes::DEFAULT_COM_IF) { - auto* cspCookie = dynamic_cast(comCookie); - cspCookie->setRequest(reqType, tableReplySize); - cspCookie->setCspPort(CspPorts::P60_PORT_RPARAM_ENUM); - } else { - RequestFullTableCommand requestFullTableCommand(querySize, tableId); - - size_t cspPacketLen = 0; - uint8_t* buffer = cspPacket; - ReturnValue_t result = requestFullTableCommand.serialize( - &buffer, &cspPacketLen, sizeof(cspPacket), SerializeIF::Endianness::BIG); - if (result != HasReturnvaluesIF::RETURN_OK) { - sif::error << "GomspaceDeviceHandler::generateRequestFullTableCmd Failed to serialize " - "full table request command " - << std::endl; - return result; - } - rawPacket = cspPacket; - rawPacketLen = cspPacketLen; + if (reqType == SpecialRequestTypes::DEFAULT_COM_IF) { + sif::warning << "Default communication for table requests not implemented anymore" << std::endl; + return returnvalue::FAILED; } - + auto* cspCookie = dynamic_cast(comCookie); + cspCookie->setRequest(reqType, tableReplySize); + cspCookie->setCspPort(CspPorts::P60_PORT_RPARAM_ENUM); rememberRequestedSize = querySize; rememberCommandId = GOMSPACE::REQUEST_HK_TABLE; return returnvalue::OK; @@ -480,19 +475,18 @@ ReturnValue_t GomspaceDeviceHandler::parsePduHkTable(PDU::PduCoreHk& coreHk, PDU const uint8_t* packet) { PoolReadGuard pg0(&coreHk); PoolReadGuard pg1(&auxHk); - if (pg0.getReadResult() != returnvalue::OK or - pg1.getReadResult() != returnvalue::OK) { + if (pg0.getReadResult() != returnvalue::OK or pg1.getReadResult() != returnvalue::OK) { sif::warning << "Reading PDU1 datasets failed!" << std::endl; return returnvalue::FAILED; } /* Fist 10 bytes contain the gomspace header. Each variable is preceded by the 16-bit table * address. */ - //dataOffset += 12; + // dataOffset += 12; for (uint8_t idx = 0; idx < PDU::CHANNELS_LEN; idx++) { coreHk.currents[idx] = as(packet + (idx * 2)); } for (uint8_t idx = 0; idx < PDU::CHANNELS_LEN; idx++) { - coreHk.voltages[idx] = as(packet + 0x12 + (idx*2)); + coreHk.voltages[idx] = as(packet + 0x12 + (idx * 2)); } auxHk.vcc.value = as(packet + 0x24); auxHk.vbat.value = as(packet + 0x26); @@ -512,7 +506,7 @@ ReturnValue_t GomspaceDeviceHandler::parsePduHkTable(PDU::PduCoreHk& coreHk, PDU coreHk.battMode = *(packet + 0x46); for (uint8_t idx = 0; idx < PDU::CHANNELS_LEN; idx++) { - auxHk.latchups[idx] = as(packet + 0x48 + (idx*2)); + auxHk.latchups[idx] = as(packet + 0x48 + (idx * 2)); } for (uint8_t idx = 0; idx < PDU::DEVICES_NUM; idx++) { diff --git a/mission/devices/GomspaceDeviceHandler.h b/mission/devices/GomspaceDeviceHandler.h index faa3a7ed..51597237 100644 --- a/mission/devices/GomspaceDeviceHandler.h +++ b/mission/devices/GomspaceDeviceHandler.h @@ -8,6 +8,12 @@ #include "mission/devices/devicedefinitions/GomspaceDefinitions.h" #include "returnvalues/classIds.h" +struct TableConfig { + uint16_t maxConfigTableAddress; + uint16_t maxHkTableAddress; + uint16_t hkTableSize; + uint16_t cfgTableSize; +}; /** * @brief This is the device handler class for all gomspace devices. * @@ -39,8 +45,7 @@ class GomspaceDeviceHandler : public DeviceHandlerBase { * device. */ GomspaceDeviceHandler(object_id_t objectId, object_id_t comIF, CookieIF *comCookie, - FailureIsolationBase *customFdir, uint16_t maxConfigTableAddress, - uint16_t maxHkTableAddress, uint16_t hkTableReplySize); + TableConfig &tableConfig, FailureIsolationBase *customFdir); virtual ~GomspaceDeviceHandler(); /** @@ -58,15 +63,9 @@ class GomspaceDeviceHandler : public DeviceHandlerBase { uint8_t rememberCommandId = GOMSPACE::NONE; uint8_t cspPacket[MAX_PACKET_LEN]; - uint16_t maxConfigTableAddress; - uint16_t maxHkTableAddress; - - /** The size of the reply following a full hk table request.*/ - uint16_t hkTableReplySize; - uint16_t configTableReplySize; - LocalPoolDataSetBase *hkTableDataset = nullptr; + void initPduConfigTable(); void doStartUp() override; void doShutDown() override; virtual ReturnValue_t buildNormalDeviceCommand(DeviceCommandId_t *id) override; @@ -83,7 +82,8 @@ class GomspaceDeviceHandler : public DeviceHandlerBase { * @brief The command to generate a request to receive the full housekeeping table is device * specific. Thus the child has to build this command. */ - virtual ReturnValue_t generateRequestFullTableCmd(GOMSPACE::SpecialRequestTypes reqType, uint8_t tableId, uint16_t tableSize); + virtual ReturnValue_t generateRequestFullTableCmd(GOMSPACE::SpecialRequestTypes reqType, + uint8_t tableId, uint16_t tableSize); /** * This command handles printing the HK table to the console. This is useful for debugging @@ -116,11 +116,13 @@ class GomspaceDeviceHandler : public DeviceHandlerBase { LocalDataPoolManager &poolManager, std::array initOutEnb); - template T as(const uint8_t*); + template + T as(const uint8_t *); static bool validTableId(uint8_t id); private: SetParamMessageUnpacker setParamCacher; + TableConfig &tableCfg; /** * @brief Function to generate the command to set a parameter. Command * will be sent to the ComIF over the rawPacket buffer. @@ -162,9 +164,9 @@ class GomspaceDeviceHandler : public DeviceHandlerBase { ReturnValue_t generateResetWatchdogCmd(); }; -template -inline T GomspaceDeviceHandler::as(const uint8_t* ptr) { - return *(reinterpret_cast(ptr)); +template +inline T GomspaceDeviceHandler::as(const uint8_t *ptr) { + return *(reinterpret_cast(ptr)); } #endif /* MISSION_DEVICES_GOMSPACEDEVICEHANDLER_H_ */ diff --git a/mission/devices/P60DockHandler.cpp b/mission/devices/P60DockHandler.cpp index 3b6fb020..d0b4a8f0 100644 --- a/mission/devices/P60DockHandler.cpp +++ b/mission/devices/P60DockHandler.cpp @@ -3,15 +3,17 @@ #include #include "OBSWConfig.h" -#include "p60dock_hk.h" P60DockHandler::P60DockHandler(object_id_t objectId, object_id_t comIF, CookieIF *comCookie, FailureIsolationBase *customFdir) - : GomspaceDeviceHandler(objectId, comIF, comCookie, customFdir, - P60Dock::MAX_CONFIGTABLE_ADDRESS, P60Dock::MAX_HKTABLE_ADDRESS, - P60DOCK_HK_SIZE), + : GomspaceDeviceHandler(objectId, comIF, comCookie, cfg, customFdir), coreHk(this), - auxHk(this) {} + auxHk(this) { + cfg.maxConfigTableAddress = P60Dock::MAX_CONFIGTABLE_ADDRESS; + cfg.maxHkTableAddress = P60Dock::MAX_HKTABLE_ADDRESS; + cfg.hkTableSize = P60Dock::HK_TABLE_SIZE; + cfg.cfgTableSize = P60Dock::CONFIG_TABLE_SIZE; +} P60DockHandler::~P60DockHandler() {} @@ -33,8 +35,7 @@ void P60DockHandler::parseHkTableReply(const uint8_t *packet) { using namespace P60Dock; PoolReadGuard pg0(&coreHk); PoolReadGuard pg1(&auxHk); - if (pg0.getReadResult() != returnvalue::OK or - pg1.getReadResult() != returnvalue::OK) { + if (pg0.getReadResult() != returnvalue::OK or pg1.getReadResult() != returnvalue::OK) { coreHk.setValidity(false, true); auxHk.setValidity(false, true); return; @@ -44,10 +45,10 @@ void P60DockHandler::parseHkTableReply(const uint8_t *packet) { * address. */ for (uint8_t idx = 0; idx < hk::CHNLS_LEN; idx++) { - coreHk.currents[idx] = as(packet + (idx*2)); + coreHk.currents[idx] = as(packet + (idx * 2)); } for (uint8_t idx = 0; idx < hk::CHNLS_LEN; idx++) { - coreHk.voltages[idx] = as(packet + 0x1a + (idx*2)); + coreHk.voltages[idx] = as(packet + 0x1a + (idx * 2)); } for (uint8_t idx = 0; idx < hk::CHNLS_LEN; idx++) { coreHk.outputEnables[idx] = *(packet + 0x34 + idx); @@ -75,7 +76,7 @@ void P60DockHandler::parseHkTableReply(const uint8_t *packet) { auxHk.converter5VStatus = *(packet + 0x58); for (uint8_t idx = 0; idx < hk::CHNLS_LEN; idx++) { - auxHk.latchups[idx] = as(packet + 0x5a + (idx*2)); + auxHk.latchups[idx] = as(packet + 0x5a + (idx * 2)); } auxHk.dockVbatVoltageValue = as(packet + 0x74); @@ -183,8 +184,7 @@ ReturnValue_t P60DockHandler::printStatus(DeviceCommandId_t cmd) { case (GOMSPACE::PRINT_SWITCH_V_I): { PoolReadGuard pg0(&coreHk); PoolReadGuard pg1(&auxHk); - if (pg0.getReadResult() != returnvalue::OK or - pg1.getReadResult() != returnvalue::OK) { + if (pg0.getReadResult() != returnvalue::OK or pg1.getReadResult() != returnvalue::OK) { break; } printHkTableSwitchIV(); diff --git a/mission/devices/P60DockHandler.h b/mission/devices/P60DockHandler.h index 25a9db35..f1061fa1 100644 --- a/mission/devices/P60DockHandler.h +++ b/mission/devices/P60DockHandler.h @@ -52,6 +52,7 @@ class P60DockHandler : public GomspaceDeviceHandler { private: P60Dock::CoreHkSet coreHk; P60Dock::HkTableDataset auxHk; + TableConfig cfg; bool firstHk = true; bool debugMode = false; static constexpr uint8_t MAX_CHANNEL_STR_WIDTH = 16; diff --git a/mission/devices/PDU1Handler.cpp b/mission/devices/PDU1Handler.cpp index 73fae79a..85ce82b1 100644 --- a/mission/devices/PDU1Handler.cpp +++ b/mission/devices/PDU1Handler.cpp @@ -2,16 +2,16 @@ #include #include -#include #include "devices/powerSwitcherList.h" PDU1Handler::PDU1Handler(object_id_t objectId, object_id_t comIF, CookieIF *comCookie, FailureIsolationBase *customFdir) - : GomspaceDeviceHandler(objectId, comIF, comCookie, customFdir, PDU::MAX_CONFIGTABLE_ADDRESS, - PDU::MAX_HKTABLE_ADDRESS, P60PDU_HK_SIZE), + : GomspaceDeviceHandler(objectId, comIF, comCookie, cfg, customFdir), coreHk(this), - auxHk(this) {} + auxHk(this) { + initPduConfigTable(); +} PDU1Handler::~PDU1Handler() {} diff --git a/mission/devices/PDU1Handler.h b/mission/devices/PDU1Handler.h index d900bb15..5b0424ef 100644 --- a/mission/devices/PDU1Handler.h +++ b/mission/devices/PDU1Handler.h @@ -46,6 +46,7 @@ class PDU1Handler : public GomspaceDeviceHandler { /** Dataset for the housekeeping table of the PDU1 */ PDU1::Pdu1CoreHk coreHk; PDU1::Pdu1AuxHk auxHk; + TableConfig cfg; GOMSPACE::ChannelSwitchHook channelSwitchHook = nullptr; void* hookArgs = nullptr; diff --git a/mission/devices/PDU2Handler.cpp b/mission/devices/PDU2Handler.cpp index 671e67dd..54e000fa 100644 --- a/mission/devices/PDU2Handler.cpp +++ b/mission/devices/PDU2Handler.cpp @@ -2,16 +2,16 @@ #include #include -#include #include "devices/powerSwitcherList.h" PDU2Handler::PDU2Handler(object_id_t objectId, object_id_t comIF, CookieIF *comCookie, FailureIsolationBase *customFdir) - : GomspaceDeviceHandler(objectId, comIF, comCookie, customFdir, PDU::MAX_CONFIGTABLE_ADDRESS, - PDU::MAX_HKTABLE_ADDRESS, P60PDU_HK_SIZE), + : GomspaceDeviceHandler(objectId, comIF, comCookie, cfg, customFdir), coreHk(this), - auxHk(this) {} + auxHk(this) { + initPduConfigTable(); +} PDU2Handler::~PDU2Handler() {} diff --git a/mission/devices/PDU2Handler.h b/mission/devices/PDU2Handler.h index e16130fd..3e1be6a2 100644 --- a/mission/devices/PDU2Handler.h +++ b/mission/devices/PDU2Handler.h @@ -45,6 +45,7 @@ class PDU2Handler : public GomspaceDeviceHandler { /** Dataset for the housekeeping table of the PDU2 */ PDU2::Pdu2CoreHk coreHk; PDU2::Pdu2AuxHk auxHk; + TableConfig cfg; GOMSPACE::ChannelSwitchHook channelSwitchHook = nullptr; void* hookArgs = nullptr; diff --git a/mission/devices/devicedefinitions/GomspaceDefinitions.h b/mission/devices/devicedefinitions/GomspaceDefinitions.h index c4b0e39a..7ae9f0a1 100644 --- a/mission/devices/devicedefinitions/GomspaceDefinitions.h +++ b/mission/devices/devicedefinitions/GomspaceDefinitions.h @@ -9,6 +9,12 @@ #include #include "devices/powerSwitcherList.h" +#include "p60acu_hk.h" +#include "p60acu_param.h" +#include "p60dock_hk.h" +#include "p60dock_param.h" +#include "p60pdu_hk.h" +#include "p60pdu_param.h" namespace GOMSPACE { @@ -22,7 +28,7 @@ enum SpecialRequestTypes { GET_P60DOCK_CONFIG }; -enum CspPorts: uint8_t { +enum CspPorts : uint8_t { CSP_PING = 1, CSP_REBOOT = 4, P60_PORT_RPARAM_ENUM = 7, @@ -234,24 +240,16 @@ enum SwitchChannels : uint8_t { GS5V = 12 }; -/** Max reply size reached when requesting full hk table */ -static const uint16_t MAX_REPLY_LENGTH = 407; - static const uint16_t MAX_CONFIGTABLE_ADDRESS = 408; static const uint16_t MAX_HKTABLE_ADDRESS = 187; // Sources: // GomSpace library lib/p60-dock_client/include/gs/p60-dock/param -static const uint16_t HK_TABLE_SIZE = 188; +static const uint16_t HK_TABLE_SIZE = P60DOCK_HK_SIZE; +static const uint16_t CONFIG_TABLE_SIZE = P60DOCK_PARAM_SIZE; +static const size_t MAX_REPLY_SIZE = CONFIG_TABLE_SIZE; static const uint16_t CAL_TABLE = 0xAE; -static const uint16_t CONFIG_TABLE_SIZE = 0x19C; static const uint8_t HK_TABLE_ENTRIES = 100; -/** - * Requesting the full housekeeping table from the P60 dock will generate a reply comprising - * 402 bytes of data. - */ -static const uint16_t HK_TABLE_REPLY_SIZE = 407; - class CoreHkSet : public StaticLocalDataSet<16> { public: CoreHkSet(HasLocalDataPoolIF* owner) @@ -377,12 +375,14 @@ class HkTableDataset : public StaticLocalDataSet<32> { * @brief Constants common for both PDU1 and PDU2. */ namespace PDU { -/** When retrieving full configuration parameter table */ -static const uint16_t MAX_REPLY_LENGTH = 318; + static const uint16_t MAX_CONFIGTABLE_ADDRESS = 316; static const uint16_t MAX_HKTABLE_ADDRESS = 141; /** The size of the csp reply containing the housekeeping table data */ -static const uint16_t HK_TABLE_REPLY_SIZE = 303; +static const uint16_t HK_TABLE_SIZE = P60PDU_HK_SIZE; +static const uint16_t CONFIG_TABLE_SIZE = P60PDU_PARAM_SIZE; +/** When retrieving full configuration parameter table */ +static const uint16_t MAX_REPLY_SIZE = CONFIG_TABLE_SIZE; static const uint8_t HK_TABLE_ENTRIES = 73; static constexpr uint8_t CHANNELS_LEN = 9; @@ -482,13 +482,6 @@ class PduAuxHk : public StaticLocalDataSet<36> { lp_var_t csp1WatchdogPingsLeft = lp_var_t(sid.objectId, P60System::pool::PDU_WDT_CSP_LEFT2, this); }; - -//class HkWrapper { -//public: -// HkWrapper(PduCoreHk& coreHk, PduAuxHk& auxHk) { -// -// } -//}; } // namespace PDU namespace PDU1 { @@ -591,12 +584,12 @@ class Pdu2AuxHk : public ::PDU::PduAuxHk { namespace ACU { -/* When receiving full housekeeping (telemetry) table */ -static const uint16_t MAX_REPLY_LENGTH = 262; static const uint16_t MAX_CONFIGTABLE_ADDRESS = 26; static const uint16_t MAX_HKTABLE_ADDRESS = 120; static const uint8_t HK_TABLE_ENTRIES = 64; -static const uint16_t HK_TABLE_REPLY_SIZE = 262; +static const uint16_t HK_TABLE_SIZE = P60ACU_HK_SIZE; +static const uint16_t CONFIG_TABLE_SIZE = P60ACU_PARAM_SIZE; +static const size_t MAX_REPLY_SIZE = HK_TABLE_SIZE; class CoreHk : public StaticLocalDataSet<14> { public: From e8208a21a40fe30e9a7df489df10eee3c9af39fa Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Sat, 27 Aug 2022 01:02:08 +0200 Subject: [PATCH 23/52] config table retrieval works --- dummies/AcuDummy.cpp | 2 +- dummies/P60DockDummy.cpp | 4 +- dummies/PduDummy.cpp | 2 +- fsfw | 2 +- linux/csp/CspComIF.cpp | 58 ++- linux/csp/CspComIF.h | 11 +- mission/controller/ThermalController.cpp | 12 +- mission/devices/ACUHandler.cpp | 6 +- mission/devices/ACUHandler.h | 4 +- mission/devices/GomspaceDeviceHandler.cpp | 87 ++-- mission/devices/GomspaceDeviceHandler.h | 5 +- mission/devices/P60DockHandler.cpp | 6 +- mission/devices/P60DockHandler.h | 4 +- mission/devices/PDU1Handler.cpp | 4 + mission/devices/PDU1Handler.h | 3 +- mission/devices/PDU2Handler.cpp | 4 + mission/devices/PDU2Handler.h | 3 +- .../devicedefinitions/GomspaceDefinitions.h | 398 ++++++++++-------- tmtc | 2 +- 19 files changed, 366 insertions(+), 251 deletions(-) diff --git a/dummies/AcuDummy.cpp b/dummies/AcuDummy.cpp index b1ad6953..d6ba21d1 100644 --- a/dummies/AcuDummy.cpp +++ b/dummies/AcuDummy.cpp @@ -37,6 +37,6 @@ uint32_t AcuDummy::getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) { return ReturnValue_t AcuDummy::initializeLocalDataPool(localpool::DataPool &localDataPoolMap, LocalDataPoolManager &poolManager) { - localDataPoolMap.emplace(P60System::pool::ACU_TEMPERATURES, new PoolEntry(3)); + localDataPoolMap.emplace(ACU::pool::ACU_TEMPERATURES, new PoolEntry(3)); return returnvalue::OK; } diff --git a/dummies/P60DockDummy.cpp b/dummies/P60DockDummy.cpp index 32df3882..8a3611ca 100644 --- a/dummies/P60DockDummy.cpp +++ b/dummies/P60DockDummy.cpp @@ -40,7 +40,7 @@ uint32_t P60DockDummy::getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) { re ReturnValue_t P60DockDummy::initializeLocalDataPool(localpool::DataPool &localDataPoolMap, LocalDataPoolManager &poolManager) { - localDataPoolMap.emplace(P60System::pool::P60DOCK_TEMPERATURE_1, new PoolEntry({0})); - localDataPoolMap.emplace(P60System::pool::P60DOCK_TEMPERATURE_2, new PoolEntry({0})); + localDataPoolMap.emplace(P60Dock::pool::P60DOCK_TEMPERATURE_1, new PoolEntry({0})); + localDataPoolMap.emplace(P60Dock::pool::P60DOCK_TEMPERATURE_2, new PoolEntry({0})); return returnvalue::OK; } diff --git a/dummies/PduDummy.cpp b/dummies/PduDummy.cpp index a5cc9c33..1c26728c 100644 --- a/dummies/PduDummy.cpp +++ b/dummies/PduDummy.cpp @@ -37,6 +37,6 @@ uint32_t PduDummy::getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) { return ReturnValue_t PduDummy::initializeLocalDataPool(localpool::DataPool &localDataPoolMap, LocalDataPoolManager &poolManager) { - localDataPoolMap.emplace(P60System::pool::PDU_TEMPERATURE, new PoolEntry({0})); + localDataPoolMap.emplace(PDU::pool::PDU_TEMPERATURE, new PoolEntry({0})); return returnvalue::OK; } diff --git a/fsfw b/fsfw index f5866dda..2a75440b 160000 --- a/fsfw +++ b/fsfw @@ -1 +1 @@ -Subproject commit f5866ddacee6cd0f381fb1a69f1d0cf22b5b310a +Subproject commit 2a75440b325b70b7ca90269159440a1f9a4a6e2a diff --git a/linux/csp/CspComIF.cpp b/linux/csp/CspComIF.cpp index 18d77cfd..eb6487ed 100644 --- a/linux/csp/CspComIF.cpp +++ b/linux/csp/CspComIF.cpp @@ -71,7 +71,7 @@ ReturnValue_t CspComIF::initializeInterface(CookieIF* cookie) { uint16_t maxReplyLength = cspCookie->getMaxReplyLength(); if (cspDeviceMap.find(cspAddress) == cspDeviceMap.end()) { /* Insert device information in CSP map */ - cspDeviceMap.emplace(cspAddress, vectorBuffer(maxReplyLength)); + cspDeviceMap.emplace(cspAddress, ReplyInfo(maxReplyLength)); } return returnvalue::OK; } @@ -104,6 +104,10 @@ ReturnValue_t CspComIF::sendMessage(CookieIF* cookie, const uint8_t* sendData, s return returnvalue::FAILED; } uint8_t cspAddress = cspCookie->getCspAddress(); + auto iter = cspDeviceMap.find(cspAddress); + if (iter == cspDeviceMap.end()) { + return returnvalue::FAILED; + } switch (cspPort) { case (CspPorts::CSP_PING): { initiatePingRequest(cspAddress, querySize); @@ -117,7 +121,7 @@ ReturnValue_t CspComIF::sendMessage(CookieIF* cookie, const uint8_t* sendData, s case (CspPorts::P60_PORT_RPARAM_ENUM): { if (cspCookie->getRequest() != SpecialRequestTypes::DEFAULT_COM_IF) { param_index_t requestStruct{}; - requestStruct.physaddr = cspDeviceMap[cspAddress].data(); + requestStruct.physaddr = iter->second.replyBuf.data(); auto req = cspCookie->getRequest(); if (req == GOMSPACE::SpecialRequestTypes::GET_PDU_HK) { if (!p60pdu_get_hk(&requestStruct, cspAddress, cspCookie->getTimeout())) { @@ -139,8 +143,29 @@ ReturnValue_t CspComIF::sendMessage(CookieIF* cookie, const uint8_t* sendData, s requestStruct.size = P60PDU_PARAM_SIZE; int result = rparam_get_full_table(&requestStruct, cspAddress, P60_PORT_RPARAM, requestStruct.mem_id, cspCookie->getTimeout()); - param_list(&requestStruct, 1); - return (result == 0); + if (result != 0) { + return returnvalue::FAILED; + } + } else if (req == GOMSPACE::SpecialRequestTypes::GET_ACU_CONFIG) { + requestStruct.table = p60acu_config; + requestStruct.mem_id = P60ACU_PARAM; + requestStruct.count = p60acu_config_count; + requestStruct.size = P60ACU_PARAM_SIZE; + int result = rparam_get_full_table(&requestStruct, cspAddress, P60_PORT_RPARAM, + requestStruct.mem_id, cspCookie->getTimeout()); + if (result != 0) { + return returnvalue::FAILED; + } + } else if (req == GOMSPACE::SpecialRequestTypes::GET_P60DOCK_CONFIG) { + requestStruct.table = p60dock_config; + requestStruct.mem_id = P60DOCK_PARAM; + requestStruct.count = p60dock_config_count; + requestStruct.size = P60DOCK_PARAM_SIZE; + int result = rparam_get_full_table(&requestStruct, cspAddress, P60_PORT_RPARAM, + requestStruct.mem_id, cspCookie->getTimeout()); + if (result != 0) { + return returnvalue::FAILED; + } } } else { /* No CSP fixed port was selected. Send data to the specified port and @@ -150,7 +175,7 @@ ReturnValue_t CspComIF::sendMessage(CookieIF* cookie, const uint8_t* sendData, s return returnvalue::FAILED; } } - replySize = querySize; + iter->second.replyLen = querySize; break; } default: @@ -176,9 +201,12 @@ ReturnValue_t CspComIF::readReceivedMessage(CookieIF* cookie, uint8_t** buffer, } uint8_t cspAddress = cspCookie->getCspAddress(); - - *buffer = cspDeviceMap[cspAddress].data(); - *size = replySize; + auto iter = cspDeviceMap.find(cspAddress); + if (iter == cspDeviceMap.end()) { + return returnvalue::FAILED; + } + *buffer = iter->second.replyBuf.data(); + *size = iter->second.replyLen; return returnvalue::OK; } @@ -188,13 +216,13 @@ ReturnValue_t CspComIF::cspTransfer(uint8_t cspAddress, uint8_t cspPort, const u uint32_t timeout_ms = 1000; uint16_t bytesRead = 0; int32_t expectedSize = static_cast(querySize); - vectorBufferIter iter = cspDeviceMap.find(cspAddress); + auto iter = cspDeviceMap.find(cspAddress); if (iter == cspDeviceMap.end()) { sif::error << "CSP device with address " << cspAddress << " no found in" << " device map" << std::endl; return returnvalue::FAILED; } - uint8_t* replyBuffer = iter->second.data(); + uint8_t* replyBuffer = iter->second.replyBuf.data(); csp_conn_t* conn = csp_connect(CSP_PRIO_HIGH, cspAddress, cspPort, 0, CSP_O_NONE); @@ -239,7 +267,7 @@ ReturnValue_t CspComIF::cspTransfer(uint8_t cspAddress, uint8_t cspPort, const u csp_close(conn); return returnvalue::FAILED; } - if ((reply->length + bytesRead) > iter->second.size()) { + if ((reply->length + bytesRead) > iter->second.replyBuf.size()) { sif::error << "CspComIF::cspTransfer: Reply buffer to short" << std::endl; csp_buffer_free(reply); csp_close(conn); @@ -286,8 +314,12 @@ void CspComIF::initiatePingRequest(uint8_t cspAddress, uint16_t querySize) { uint32_t replyTime = csp_ping(cspAddress, timeout_ms, querySize, CSP_O_NONE); sif::info << "Ping address: " << cspAddress << ", reply after " << replyTime << " ms" << std::endl; + auto iter = cspDeviceMap.find(cspAddress); + if (iter == cspDeviceMap.end()) { + return; + } /* Store reply time in reply buffer * */ - uint8_t* replyBuffer = cspDeviceMap[cspAddress].data(); + uint8_t* replyBuffer = iter->second.replyBuf.data(); memcpy(replyBuffer, &replyTime, sizeof(replyTime)); - replySize = sizeof(replyTime); + iter->second.replyLen = sizeof(replyTime); } diff --git a/linux/csp/CspComIF.h b/linux/csp/CspComIF.h index 09dc63f8..d2bac4f9 100644 --- a/linux/csp/CspComIF.h +++ b/linux/csp/CspComIF.h @@ -43,15 +43,16 @@ class CspComIF : public DeviceCommunicationIF, public SystemObject { int cmdLen, uint16_t querySize); typedef uint8_t node_t; - using vectorBuffer = std::vector; - using VectorBufferMap = std::unordered_map; - using vectorBufferIter = VectorBufferMap::iterator; + struct ReplyInfo { + ReplyInfo(size_t maxLen) : replyBuf(maxLen){}; + std::vector replyBuf; + size_t replyLen = 0; + }; + using VectorBufferMap = std::unordered_map; /* In this map assigns reply buffers to a CSP device */ VectorBufferMap cspDeviceMap; - uint16_t replySize = 0; - /* This is the CSP address of the OBC. */ node_t cspOwnAddress = 1; diff --git a/mission/controller/ThermalController.cpp b/mission/controller/ThermalController.cpp index 4800b969..52fb40e9 100644 --- a/mission/controller/ThermalController.cpp +++ b/mission/controller/ThermalController.cpp @@ -699,7 +699,7 @@ void ThermalController::copyDevices() { sif::warning << "ThermalController: Failed to commit" << std::endl; } lp_vec_t tempAcu = - lp_vec_t(objects::ACU_HANDLER, P60System::pool::ACU_TEMPERATURES); + lp_vec_t(objects::ACU_HANDLER, ACU::pool::ACU_TEMPERATURES); result = tempAcu.read(); if (result != returnvalue::OK) { sif::warning << "ThermalController: Failed to read ACU temperatures" << std::endl; @@ -715,8 +715,7 @@ void ThermalController::copyDevices() { if (result != returnvalue::OK) { sif::warning << "ThermalController: Failed to commit" << std::endl; } - lp_var_t tempPdu1 = - lp_var_t(objects::PDU1_HANDLER, P60System::pool::PDU_TEMPERATURE); + lp_var_t tempPdu1 = lp_var_t(objects::PDU1_HANDLER, PDU::pool::PDU_TEMPERATURE); result = tempPdu1.read(); if (result != returnvalue::OK) { sif::warning << "ThermalController: Failed to read PDU1 temperature" << std::endl; @@ -730,8 +729,7 @@ void ThermalController::copyDevices() { if (result != returnvalue::OK) { sif::warning << "ThermalController: Failed to commit" << std::endl; } - lp_var_t tempPdu2 = - lp_var_t(objects::PDU2_HANDLER, P60System::pool::PDU_TEMPERATURE); + lp_var_t tempPdu2 = lp_var_t(objects::PDU2_HANDLER, PDU::pool::PDU_TEMPERATURE); result = tempPdu2.read(); if (result != returnvalue::OK) { sif::warning << "ThermalController: Failed to read PDU2 temperature" << std::endl; @@ -746,7 +744,7 @@ void ThermalController::copyDevices() { sif::warning << "ThermalController: Failed to commit" << std::endl; } lp_var_t temp1P60dock = - lp_var_t(objects::P60DOCK_HANDLER, P60System::pool::P60DOCK_TEMPERATURE_1); + lp_var_t(objects::P60DOCK_HANDLER, P60Dock::pool::P60DOCK_TEMPERATURE_1); result = temp1P60dock.read(); if (result != returnvalue::OK) { sif::warning << "ThermalController: Failed to read P60 dock temperature 1" << std::endl; @@ -761,7 +759,7 @@ void ThermalController::copyDevices() { sif::warning << "ThermalController: Failed to commit" << std::endl; } lp_var_t temp2P60dock = - lp_var_t(objects::P60DOCK_HANDLER, P60System::pool::P60DOCK_TEMPERATURE_2); + lp_var_t(objects::P60DOCK_HANDLER, P60Dock::pool::P60DOCK_TEMPERATURE_2); result = temp2P60dock.read(); if (result != returnvalue::OK) { sif::warning << "ThermalController: Failed to read P60 dock temperature 2" << std::endl; diff --git a/mission/devices/ACUHandler.cpp b/mission/devices/ACUHandler.cpp index 28cd9e4b..f229d248 100644 --- a/mission/devices/ACUHandler.cpp +++ b/mission/devices/ACUHandler.cpp @@ -42,6 +42,10 @@ void ACUHandler::letChildHandleHkReply(DeviceCommandId_t id, const uint8_t *pack } } +void ACUHandler::letChildHandleConfigReply(DeviceCommandId_t id, const uint8_t *packet) { + handleDeviceTM(packet, ACU::CONFIG_TABLE_SIZE, id); +} + LocalPoolDataSetBase *ACUHandler::getDataSetHandle(sid_t sid) { if (sid == coreHk.getSid()) { return &coreHk; @@ -114,7 +118,7 @@ ReturnValue_t ACUHandler::parseHkTableReply(const uint8_t *packet) { ReturnValue_t ACUHandler::initializeLocalDataPool(localpool::DataPool &localDataPoolMap, LocalDataPoolManager &poolManager) { - using namespace P60System; + using namespace ACU; localDataPoolMap.emplace(pool::ACU_CURRENT_IN_CHANNELS, new PoolEntry(6)); localDataPoolMap.emplace(pool::ACU_VOLTAGE_IN_CHANNELS, new PoolEntry(6)); diff --git a/mission/devices/ACUHandler.h b/mission/devices/ACUHandler.h index cf5e2517..ef4feab0 100644 --- a/mission/devices/ACUHandler.h +++ b/mission/devices/ACUHandler.h @@ -22,8 +22,8 @@ class ACUHandler : public GomspaceDeviceHandler { LocalDataPoolManager& poolManager) override; protected: - virtual void letChildHandleHkReply(DeviceCommandId_t id, const uint8_t* packet) override; - + void letChildHandleHkReply(DeviceCommandId_t id, const uint8_t* packet) override; + void letChildHandleConfigReply(DeviceCommandId_t id, const uint8_t* packet) override; /** * @brief As soon as the device is in MODE_NORMAL, this function is executed periodically. */ diff --git a/mission/devices/GomspaceDeviceHandler.cpp b/mission/devices/GomspaceDeviceHandler.cpp index dc9314ae..cc4786d0 100644 --- a/mission/devices/GomspaceDeviceHandler.cpp +++ b/mission/devices/GomspaceDeviceHandler.cpp @@ -92,7 +92,8 @@ ReturnValue_t GomspaceDeviceHandler::buildCommandFromCommand(DeviceCommandId_t d } else if (getObjectId() == objects::P60DOCK_HANDLER) { reqType = SpecialRequestTypes::GET_P60DOCK_HK; } - result = generateRequestFullTableCmd(reqType, GOMSPACE::TableIds::HK, tableCfg.hkTableSize); + result = generateRequestFullTableCmd(reqType, GOMSPACE::TableIds::HK, tableCfg.hkTableSize, + deviceCommand); if (result != returnvalue::OK) { return result; } @@ -102,11 +103,13 @@ ReturnValue_t GomspaceDeviceHandler::buildCommandFromCommand(DeviceCommandId_t d auto reqType = SpecialRequestTypes::DEFAULT_COM_IF; if (getObjectId() == objects::PDU1_HANDLER or getObjectId() == objects::PDU2_HANDLER) { reqType = SpecialRequestTypes::GET_PDU_CONFIG; - } else { - reqType = SpecialRequestTypes::DEFAULT_COM_IF; + } else if (getObjectId() == objects::ACU_HANDLER) { + reqType = SpecialRequestTypes::GET_ACU_CONFIG; + } else if (getObjectId() == objects::P60DOCK_HANDLER) { + reqType = SpecialRequestTypes::GET_P60DOCK_CONFIG; } - result = - generateRequestFullTableCmd(reqType, GOMSPACE::TableIds::CONFIG, tableCfg.cfgTableSize); + result = generateRequestFullTableCmd(reqType, GOMSPACE::TableIds::CONFIG, + tableCfg.cfgTableSize, deviceCommand); if (result != returnvalue::OK) { return result; } @@ -124,6 +127,7 @@ void GomspaceDeviceHandler::fillCommandAndReplyMap() { this->insertInCommandAndReplyMap(GOMSPACE::PARAM_SET, 3); this->insertInCommandAndReplyMap(GOMSPACE::PARAM_GET, 3); this->insertInCommandAndReplyMap(GOMSPACE::REQUEST_HK_TABLE, 3); + this->insertInCommandAndReplyMap(GOMSPACE::REQUEST_CONFIG_TABLE, 3); this->insertInCommandMap(GOMSPACE::GNDWDT_RESET); this->insertInCommandMap(GOMSPACE::PRINT_SWITCH_V_I); this->insertInCommandMap(GOMSPACE::PRINT_LATCHUPS); @@ -149,9 +153,16 @@ ReturnValue_t GomspaceDeviceHandler::scanForReply(const uint8_t* start, size_t r rememberCommandId = GOMSPACE::NONE; break; } - case (GOMSPACE::REQUEST_HK_TABLE): { - *foundId = GOMSPACE::REQUEST_HK_TABLE; - *foundLen = rememberRequestedSize + GOMSPACE::GS_HDR_LENGTH; + case (GOMSPACE::REQUEST_HK_TABLE): + case (GOMSPACE::REQUEST_CONFIG_TABLE): { + if (remainingSize < rememberRequestedSize) { + sif::error << "GomspaceDeviceHandler::scanForReply: Table reply, received data smaller " + "than expected " + << rememberRequestedSize << std::endl; + return returnvalue::FAILED; + } + *foundId = rememberCommandId; + *foundLen = rememberRequestedSize; rememberCommandId = GOMSPACE::NONE; break; } @@ -209,6 +220,10 @@ ReturnValue_t GomspaceDeviceHandler::interpretDeviceReply(DeviceCommandId_t id, letChildHandleHkReply(id, packet); break; } + case (GOMSPACE::REQUEST_CONFIG_TABLE): { + letChildHandleConfigReply(id, packet); + break; + } default: break; } @@ -384,38 +399,39 @@ ReturnValue_t GomspaceDeviceHandler::setParamCallback(SetParamMessageUnpacker& u ReturnValue_t GomspaceDeviceHandler::initializePduPool( localpool::DataPool& localDataPoolMap, LocalDataPoolManager& poolManager, std::array initOutEnb) { - localDataPoolMap.emplace(P60System::pool::PDU_CURRENTS, new PoolEntry(9)); - localDataPoolMap.emplace(P60System::pool::PDU_VOLTAGES, new PoolEntry(9)); + using namespace PDU; + localDataPoolMap.emplace(pool::PDU_CURRENTS, new PoolEntry(9)); + localDataPoolMap.emplace(pool::PDU_VOLTAGES, new PoolEntry(9)); - localDataPoolMap.emplace(P60System::pool::PDU_VCC, new PoolEntry({0})); - localDataPoolMap.emplace(P60System::pool::PDU_VBAT, new PoolEntry({0})); - localDataPoolMap.emplace(P60System::pool::PDU_TEMPERATURE, new PoolEntry({0})); - localDataPoolMap.emplace(P60System::pool::PDU_CONV_EN, new PoolEntry(3)); + localDataPoolMap.emplace(pool::PDU_VCC, new PoolEntry({0})); + localDataPoolMap.emplace(pool::PDU_VBAT, new PoolEntry({0})); + localDataPoolMap.emplace(pool::PDU_TEMPERATURE, new PoolEntry({0})); + localDataPoolMap.emplace(pool::PDU_CONV_EN, new PoolEntry(3)); - localDataPoolMap.emplace(P60System::pool::PDU_OUT_ENABLE, + localDataPoolMap.emplace(pool::PDU_OUT_ENABLE, new PoolEntry(initOutEnb.data(), initOutEnb.size())); - localDataPoolMap.emplace(P60System::pool::PDU_BOOTCAUSE, new PoolEntry({0})); - localDataPoolMap.emplace(P60System::pool::PDU_BOOTCNT, new PoolEntry({0})); - localDataPoolMap.emplace(P60System::pool::PDU_UPTIME, new PoolEntry({0})); - localDataPoolMap.emplace(P60System::pool::PDU_RESETCAUSE, new PoolEntry({0})); - localDataPoolMap.emplace(P60System::pool::PDU_BATT_MODE, new PoolEntry({0})); + localDataPoolMap.emplace(pool::PDU_BOOTCAUSE, new PoolEntry({0})); + localDataPoolMap.emplace(pool::PDU_BOOTCNT, new PoolEntry({0})); + localDataPoolMap.emplace(pool::PDU_UPTIME, new PoolEntry({0})); + localDataPoolMap.emplace(pool::PDU_RESETCAUSE, new PoolEntry({0})); + localDataPoolMap.emplace(pool::PDU_BATT_MODE, new PoolEntry({0})); - localDataPoolMap.emplace(P60System::pool::PDU_LATCHUPS, new PoolEntry(9)); + localDataPoolMap.emplace(pool::PDU_LATCHUPS, new PoolEntry(9)); - localDataPoolMap.emplace(P60System::pool::PDU_DEVICES, new PoolEntry(8)); - localDataPoolMap.emplace(P60System::pool::PDU_STATUSES, new PoolEntry(8)); + localDataPoolMap.emplace(pool::PDU_DEVICES, new PoolEntry(8)); + localDataPoolMap.emplace(pool::PDU_STATUSES, new PoolEntry(8)); - localDataPoolMap.emplace(P60System::pool::PDU_WDT_CNT_GND, new PoolEntry({0})); - localDataPoolMap.emplace(P60System::pool::PDU_WDT_CNT_I2C, new PoolEntry({0})); - localDataPoolMap.emplace(P60System::pool::PDU_WDT_CNT_CAN, new PoolEntry({0})); - localDataPoolMap.emplace(P60System::pool::PDU_WDT_CNT_CSP1, new PoolEntry({0})); - localDataPoolMap.emplace(P60System::pool::PDU_WDT_CNT_CSP2, new PoolEntry({0})); - localDataPoolMap.emplace(P60System::pool::PDU_WDT_GND_LEFT, new PoolEntry({0})); - localDataPoolMap.emplace(P60System::pool::PDU_WDT_I2C_LEFT, new PoolEntry({0})); - localDataPoolMap.emplace(P60System::pool::PDU_WDT_CAN_LEFT, new PoolEntry({0})); - localDataPoolMap.emplace(P60System::pool::PDU_WDT_CSP_LEFT1, new PoolEntry({0})); - localDataPoolMap.emplace(P60System::pool::PDU_WDT_CSP_LEFT2, new PoolEntry({0})); + localDataPoolMap.emplace(pool::PDU_WDT_CNT_GND, new PoolEntry({0})); + localDataPoolMap.emplace(pool::PDU_WDT_CNT_I2C, new PoolEntry({0})); + localDataPoolMap.emplace(pool::PDU_WDT_CNT_CAN, new PoolEntry({0})); + localDataPoolMap.emplace(pool::PDU_WDT_CNT_CSP1, new PoolEntry({0})); + localDataPoolMap.emplace(pool::PDU_WDT_CNT_CSP2, new PoolEntry({0})); + localDataPoolMap.emplace(pool::PDU_WDT_GND_LEFT, new PoolEntry({0})); + localDataPoolMap.emplace(pool::PDU_WDT_I2C_LEFT, new PoolEntry({0})); + localDataPoolMap.emplace(pool::PDU_WDT_CAN_LEFT, new PoolEntry({0})); + localDataPoolMap.emplace(pool::PDU_WDT_CSP_LEFT1, new PoolEntry({0})); + localDataPoolMap.emplace(pool::PDU_WDT_CSP_LEFT2, new PoolEntry({0})); return returnvalue::OK; } @@ -448,7 +464,8 @@ ReturnValue_t GomspaceDeviceHandler::generateResetWatchdogCmd() { ReturnValue_t GomspaceDeviceHandler::generateRequestFullTableCmd(SpecialRequestTypes reqType, uint8_t tableId, - uint16_t tableReplySize) { + uint16_t tableReplySize, + DeviceCommandId_t id) { uint16_t querySize = tableReplySize; if (reqType == SpecialRequestTypes::DEFAULT_COM_IF) { sif::warning << "Default communication for table requests not implemented anymore" << std::endl; @@ -458,7 +475,7 @@ ReturnValue_t GomspaceDeviceHandler::generateRequestFullTableCmd(SpecialRequestT cspCookie->setRequest(reqType, tableReplySize); cspCookie->setCspPort(CspPorts::P60_PORT_RPARAM_ENUM); rememberRequestedSize = querySize; - rememberCommandId = GOMSPACE::REQUEST_HK_TABLE; + rememberCommandId = id; return returnvalue::OK; } diff --git a/mission/devices/GomspaceDeviceHandler.h b/mission/devices/GomspaceDeviceHandler.h index 51597237..af847b8a 100644 --- a/mission/devices/GomspaceDeviceHandler.h +++ b/mission/devices/GomspaceDeviceHandler.h @@ -83,7 +83,8 @@ class GomspaceDeviceHandler : public DeviceHandlerBase { * specific. Thus the child has to build this command. */ virtual ReturnValue_t generateRequestFullTableCmd(GOMSPACE::SpecialRequestTypes reqType, - uint8_t tableId, uint16_t tableSize); + uint8_t tableId, uint16_t tableSize, + DeviceCommandId_t id); /** * This command handles printing the HK table to the console. This is useful for debugging @@ -99,7 +100,7 @@ class GomspaceDeviceHandler : public DeviceHandlerBase { * @param packet Pointer to the reply containing the hk table. */ virtual void letChildHandleHkReply(DeviceCommandId_t id, const uint8_t *packet) = 0; - + virtual void letChildHandleConfigReply(DeviceCommandId_t id, const uint8_t *packet) = 0; virtual LocalPoolDataSetBase *getDataSetHandle(sid_t sid) = 0; /** diff --git a/mission/devices/P60DockHandler.cpp b/mission/devices/P60DockHandler.cpp index d0b4a8f0..4d4d4ed2 100644 --- a/mission/devices/P60DockHandler.cpp +++ b/mission/devices/P60DockHandler.cpp @@ -121,7 +121,7 @@ void P60DockHandler::parseHkTableReply(const uint8_t *packet) { ReturnValue_t P60DockHandler::initializeLocalDataPool(localpool::DataPool &localDataPoolMap, LocalDataPoolManager &poolManager) { - using namespace P60System; + using namespace P60Dock; localDataPoolMap.emplace(pool::P60_CURRENTS, &hkCurrents); localDataPoolMap.emplace(pool::P60_VOLTAGES, &hkVoltages); @@ -271,3 +271,7 @@ void P60DockHandler::printHkTableLatchups() { } void P60DockHandler::setDebugMode(bool enable) { this->debugMode = enable; } + +void P60DockHandler::letChildHandleConfigReply(DeviceCommandId_t id, const uint8_t *packet) { + handleDeviceTM(packet, P60Dock::CONFIG_TABLE_SIZE, id); +} diff --git a/mission/devices/P60DockHandler.h b/mission/devices/P60DockHandler.h index f1061fa1..fcad350f 100644 --- a/mission/devices/P60DockHandler.h +++ b/mission/devices/P60DockHandler.h @@ -36,8 +36,8 @@ class P60DockHandler : public GomspaceDeviceHandler { */ virtual ReturnValue_t buildNormalDeviceCommand(DeviceCommandId_t* id) override; - virtual void letChildHandleHkReply(DeviceCommandId_t id, const uint8_t* packet) override; - + void letChildHandleHkReply(DeviceCommandId_t id, const uint8_t* packet) override; + void letChildHandleConfigReply(DeviceCommandId_t id, const uint8_t* packet) override; /** * This command handles printing the HK table to the console. This is useful for debugging * purposes diff --git a/mission/devices/PDU1Handler.cpp b/mission/devices/PDU1Handler.cpp index 85ce82b1..deb747db 100644 --- a/mission/devices/PDU1Handler.cpp +++ b/mission/devices/PDU1Handler.cpp @@ -80,6 +80,10 @@ ReturnValue_t PDU1Handler::setParamCallback(SetParamMessageUnpacker &unpacker, return returnvalue::OK; } +void PDU1Handler::letChildHandleConfigReply(DeviceCommandId_t id, const uint8_t *packet) { + handleDeviceTM(packet, PDU::CONFIG_TABLE_SIZE, id); +} + void PDU1Handler::parseHkTableReply(const uint8_t *packet) { GomspaceDeviceHandler::parsePduHkTable(coreHk, auxHk, packet); } diff --git a/mission/devices/PDU1Handler.h b/mission/devices/PDU1Handler.h index 5b0424ef..262283a6 100644 --- a/mission/devices/PDU1Handler.h +++ b/mission/devices/PDU1Handler.h @@ -35,7 +35,8 @@ class PDU1Handler : public GomspaceDeviceHandler { * @brief In MODE_NORMAL, a command will be built periodically by this function. */ virtual ReturnValue_t buildNormalDeviceCommand(DeviceCommandId_t* id) override; - virtual void letChildHandleHkReply(DeviceCommandId_t id, const uint8_t* packet) override; + void letChildHandleHkReply(DeviceCommandId_t id, const uint8_t* packet) override; + void letChildHandleConfigReply(DeviceCommandId_t id, const uint8_t* packet) override; ReturnValue_t printStatus(DeviceCommandId_t cmd) override; LocalPoolDataSetBase* getDataSetHandle(sid_t sid) override; ReturnValue_t setParamCallback(SetParamMessageUnpacker& unpacker, bool afterExectuion) override; diff --git a/mission/devices/PDU2Handler.cpp b/mission/devices/PDU2Handler.cpp index 54e000fa..65de91a7 100644 --- a/mission/devices/PDU2Handler.cpp +++ b/mission/devices/PDU2Handler.cpp @@ -29,6 +29,10 @@ void PDU2Handler::letChildHandleHkReply(DeviceCommandId_t id, const uint8_t *pac handleDeviceTM(&coreHk, id, true); } +void PDU2Handler::letChildHandleConfigReply(DeviceCommandId_t id, const uint8_t *packet) { + handleDeviceTM(packet, PDU::CONFIG_TABLE_SIZE, id); +} + void PDU2Handler::assignChannelHookFunction(GOMSPACE::ChannelSwitchHook hook, void *args) { this->channelSwitchHook = hook; this->hookArgs = args; diff --git a/mission/devices/PDU2Handler.h b/mission/devices/PDU2Handler.h index 3e1be6a2..40204502 100644 --- a/mission/devices/PDU2Handler.h +++ b/mission/devices/PDU2Handler.h @@ -34,7 +34,8 @@ class PDU2Handler : public GomspaceDeviceHandler { * @brief As soon as the device is in MODE_NORMAL, this function is executed periodically. */ virtual ReturnValue_t buildNormalDeviceCommand(DeviceCommandId_t* id) override; - virtual void letChildHandleHkReply(DeviceCommandId_t id, const uint8_t* packet) override; + void letChildHandleHkReply(DeviceCommandId_t id, const uint8_t* packet) override; + void letChildHandleConfigReply(DeviceCommandId_t id, const uint8_t* packet) override; ReturnValue_t printStatus(DeviceCommandId_t cmd) override; ReturnValue_t setParamCallback(SetParamMessageUnpacker& unpacker, bool afterExecution) override; LocalPoolDataSetBase* getDataSetHandle(sid_t sid) override; diff --git a/mission/devices/devicedefinitions/GomspaceDefinitions.h b/mission/devices/devicedefinitions/GomspaceDefinitions.h index 7ae9f0a1..5447bf42 100644 --- a/mission/devices/devicedefinitions/GomspaceDefinitions.h +++ b/mission/devices/devicedefinitions/GomspaceDefinitions.h @@ -101,9 +101,16 @@ enum class SetIds : uint32_t { P60_CORE = 5, P60_AUX = 6, ACU_CORE = 7, - ACU_AUX = 8 + ACU_AUX = 8, + + PDU_1_CONFIG = 9, + PDU_2_CONFIG = 10 }; +} // namespace P60System + +namespace P60Dock { + namespace pool { enum Ids : lp_id_t { @@ -146,60 +153,8 @@ enum Ids : lp_id_t { P60DOCK_BATT_DISCHARGE_CURRENT, P60DOCK_ANT6_DEPL, P60DOCK_AR6_DEPL, - - // IDs for both PDUs - PDU_CURRENTS, - PDU_VOLTAGES, - PDU_VCC, - PDU_VBAT, - PDU_TEMPERATURE, - PDU_CONV_EN, - PDU_OUT_ENABLE, - PDU_BOOTCAUSE, - PDU_BOOTCNT, - PDU_UPTIME, - PDU_RESETCAUSE, - PDU_BATT_MODE, - PDU_LATCHUPS, - PDU_DEVICES, - PDU_STATUSES, - PDU_WDT_CNT_GND, - PDU_WDT_CNT_I2C, - PDU_WDT_CNT_CAN, - PDU_WDT_CNT_CSP1, - PDU_WDT_CNT_CSP2, - PDU_WDT_GND_LEFT, - PDU_WDT_I2C_LEFT, - PDU_WDT_CAN_LEFT, - PDU_WDT_CSP_LEFT1, - PDU_WDT_CSP_LEFT2, - - /** ACU Ids */ - ACU_CURRENT_IN_CHANNELS, - ACU_VOLTAGE_IN_CHANNELS, - ACU_VCC, - ACU_VBAT, - ACU_TEMPERATURES, - ACU_MPPT_MODE, - ACU_VBOOST_IN_CHANNELS, - ACU_POWER_IN_CHANNELS, - ACU_DAC_ENABLES, - ACU_DAC_RAW_CHANNELS, - ACU_BOOTCAUSE, - ACU_BOOTCNT, - ACU_UPTIME, - ACU_RESET_CAUSE, - ACU_MPPT_TIME, - ACU_MPPT_PERIOD, - ACU_DEVICES, - ACU_DEVICES_STATUS, - ACU_WDT_CNT_GND, - ACU_WDT_GND_LEFT, }; } -} // namespace P60System - -namespace P60Dock { static constexpr uint8_t NUM_DEVS = 8; @@ -260,33 +215,26 @@ class CoreHkSet : public StaticLocalDataSet<16> { /** Measured output currents */ lp_vec_t currents = - lp_vec_t(sid.objectId, P60System::pool::P60_CURRENTS, - this); + lp_vec_t(sid.objectId, pool::P60_CURRENTS, this); /** Measured output voltages */ lp_vec_t voltages = - lp_vec_t(sid.objectId, P60System::pool::P60_VOLTAGES, - this); + lp_vec_t(sid.objectId, pool::P60_VOLTAGES, this); /** Output enable states */ lp_vec_t outputEnables = - lp_vec_t(sid.objectId, - P60System::pool::P60_OUTPUT_ENABLE, this); - lp_var_t bootCount = - lp_var_t(sid.objectId, P60System::pool::P60DOCK_BOOT_CNT, this); - lp_var_t battMode = - lp_var_t(sid.objectId, P60System::pool::P60DOCK_BATT_MODE, this); + lp_vec_t(sid.objectId, pool::P60_OUTPUT_ENABLE, this); + lp_var_t bootCount = lp_var_t(sid.objectId, pool::P60DOCK_BOOT_CNT, this); + lp_var_t battMode = lp_var_t(sid.objectId, pool::P60DOCK_BATT_MODE, this); // Difference between charge and discharge current lp_var_t batteryCurrent = - lp_var_t(sid.objectId, P60System::pool::P60DOCK_BATTERY_CURRENT, this); + lp_var_t(sid.objectId, pool::P60DOCK_BATTERY_CURRENT, this); lp_var_t batteryVoltage = - lp_var_t(sid.objectId, P60System::pool::P60DOCK_BATTERY_VOLTAGE, this); + lp_var_t(sid.objectId, pool::P60DOCK_BATTERY_VOLTAGE, this); - lp_var_t temperature1 = - lp_var_t(sid.objectId, P60System::pool::P60DOCK_TEMPERATURE_1, this); - lp_var_t temperature2 = - lp_var_t(sid.objectId, P60System::pool::P60DOCK_TEMPERATURE_2, this); + lp_var_t temperature1 = lp_var_t(sid.objectId, pool::P60DOCK_TEMPERATURE_1, this); + lp_var_t temperature2 = lp_var_t(sid.objectId, pool::P60DOCK_TEMPERATURE_2, this); }; /** * @brief This class defines a dataset for the hk table of the P60 Dock. @@ -304,70 +252,59 @@ class HkTableDataset : public StaticLocalDataSet<32> { /** Number of detected latchups on each output channel */ lp_vec_t latchups = - lp_vec_t(sid.objectId, P60System::pool::LATCHUPS, - this); + lp_vec_t(sid.objectId, pool::LATCHUPS, this); - lp_var_t bootcause = - lp_var_t(sid.objectId, P60System::pool::P60DOCK_BOOT_CAUSE, this); - lp_var_t uptime = - lp_var_t(sid.objectId, P60System::pool::P60DOCK_UPTIME, this); - lp_var_t resetcause = - lp_var_t(sid.objectId, P60System::pool::P60DOCK_RESETCAUSE, this); + lp_var_t bootcause = lp_var_t(sid.objectId, pool::P60DOCK_BOOT_CAUSE, this); + lp_var_t uptime = lp_var_t(sid.objectId, pool::P60DOCK_UPTIME, this); + lp_var_t resetcause = lp_var_t(sid.objectId, pool::P60DOCK_RESETCAUSE, this); /** Battery heater control only possible on BP4 packs */ - lp_var_t heaterOn = - lp_var_t(sid.objectId, P60System::pool::P60DOCK_HEATER_ON, this); + lp_var_t heaterOn = lp_var_t(sid.objectId, pool::P60DOCK_HEATER_ON, this); lp_var_t converter5VStatus = - lp_var_t(sid.objectId, P60System::pool::P60DOCK_CONV_5V_ENABLE_STATUS, this); + lp_var_t(sid.objectId, pool::P60DOCK_CONV_5V_ENABLE_STATUS, this); lp_var_t dockVbatVoltageValue = - lp_var_t(sid.objectId, P60System::pool::P60DOCK_DOCK_VBAT, this); + lp_var_t(sid.objectId, pool::P60DOCK_DOCK_VBAT, this); lp_var_t dockVccCurrent = - lp_var_t(sid.objectId, P60System::pool::P60DOCK_DOCK_VCC_CURRENT, this); + lp_var_t(sid.objectId, pool::P60DOCK_DOCK_VCC_CURRENT, this); lp_var_t batteryTemperature1 = - lp_var_t(sid.objectId, P60System::pool::P60DOCK_BATTERY_TEMPERATURE_1, this); + lp_var_t(sid.objectId, pool::P60DOCK_BATTERY_TEMPERATURE_1, this); lp_var_t batteryTemperature2 = - lp_var_t(sid.objectId, P60System::pool::P60DOCK_BATTERY_TEMPERATURE_2, this); + lp_var_t(sid.objectId, pool::P60DOCK_BATTERY_TEMPERATURE_2, this); - lp_var_t dearmStatus = - lp_var_t(sid.objectId, P60System::pool::P60DOCK_DEARM_STATUS, this); + lp_var_t dearmStatus = lp_var_t(sid.objectId, pool::P60DOCK_DEARM_STATUS, this); /** Number of reboots due to gnd, i2c, csp watchdog timeout */ - lp_var_t wdtCntGnd = - lp_var_t(sid.objectId, P60System::pool::P60DOCK_WDT_CNT_GND, this); - lp_var_t wdtCntI2c = - lp_var_t(sid.objectId, P60System::pool::P60DOCK_WDT_CNT_I2C, this); - lp_var_t wdtCntCan = - lp_var_t(sid.objectId, P60System::pool::P60DOCK_WDT_CNT_CAN, this); + lp_var_t wdtCntGnd = lp_var_t(sid.objectId, pool::P60DOCK_WDT_CNT_GND, this); + lp_var_t wdtCntI2c = lp_var_t(sid.objectId, pool::P60DOCK_WDT_CNT_I2C, this); + lp_var_t wdtCntCan = lp_var_t(sid.objectId, pool::P60DOCK_WDT_CNT_CAN, this); lp_var_t wdtCntCsp1 = - lp_var_t(sid.objectId, P60System::pool::P60DOCK_WDT_CNT_CSP_1, this); + lp_var_t(sid.objectId, pool::P60DOCK_WDT_CNT_CSP_1, this); lp_var_t wdtCntCsp2 = - lp_var_t(sid.objectId, P60System::pool::P60DOCK_WDT_CNT_CSP_2, this); + lp_var_t(sid.objectId, pool::P60DOCK_WDT_CNT_CSP_2, this); lp_var_t wdtGndLeft = - lp_var_t(sid.objectId, P60System::pool::P60DOCK_WDT_GND_LEFT, this); + lp_var_t(sid.objectId, pool::P60DOCK_WDT_GND_LEFT, this); lp_var_t wdtI2cLeft = - lp_var_t(sid.objectId, P60System::pool::P60DOCK_WDT_I2C_LEFT, this); + lp_var_t(sid.objectId, pool::P60DOCK_WDT_I2C_LEFT, this); lp_var_t wdtCanLeft = - lp_var_t(sid.objectId, P60System::pool::P60DOCK_WDT_CAN_LEFT, this); + lp_var_t(sid.objectId, pool::P60DOCK_WDT_CAN_LEFT, this); lp_var_t wdtCspLeft1 = - lp_var_t(sid.objectId, P60System::pool::P60DOCK_WDT_CSP_LEFT_1, this); + lp_var_t(sid.objectId, pool::P60DOCK_WDT_CSP_LEFT_1, this); lp_var_t wdtCspLeft2 = - lp_var_t(sid.objectId, P60System::pool::P60DOCK_WDT_CSP_LEFT_2, this); + lp_var_t(sid.objectId, pool::P60DOCK_WDT_CSP_LEFT_2, this); lp_var_t batteryChargeCurrent = - lp_var_t(sid.objectId, P60System::pool::P60DOCK_BATT_CHARGE_CURRENT, this); + lp_var_t(sid.objectId, pool::P60DOCK_BATT_CHARGE_CURRENT, this); lp_var_t batteryDischargeCurrent = - lp_var_t(sid.objectId, P60System::pool::P60DOCK_BATT_DISCHARGE_CURRENT, this); - lp_var_t ant6Depl = - lp_var_t(sid.objectId, P60System::pool::P60DOCK_ANT6_DEPL, this); - lp_var_t ar6Depl = - lp_var_t(sid.objectId, P60System::pool::P60DOCK_AR6_DEPL, this); + lp_var_t(sid.objectId, pool::P60DOCK_BATT_DISCHARGE_CURRENT, this); + lp_var_t ant6Depl = lp_var_t(sid.objectId, pool::P60DOCK_ANT6_DEPL, this); + lp_var_t ar6Depl = lp_var_t(sid.objectId, pool::P60DOCK_AR6_DEPL, this); lp_vec_t devicesType = - lp_vec_t(sid.objectId, P60System::pool::DEVICES_TYPE, this); + lp_vec_t(sid.objectId, pool::DEVICES_TYPE, this); lp_vec_t devicesStatus = - lp_vec_t(sid.objectId, P60System::pool::DEVICES_STATUS, this); + lp_vec_t(sid.objectId, pool::DEVICES_STATUS, this); }; } // namespace P60Dock @@ -376,6 +313,66 @@ class HkTableDataset : public StaticLocalDataSet<32> { */ namespace PDU { +namespace pool { +enum Ids { + // IDs for both PDUs + PDU_CURRENTS, + PDU_VOLTAGES, + PDU_VCC, + PDU_VBAT, + PDU_TEMPERATURE, + PDU_CONV_EN, + PDU_OUT_ENABLE, + PDU_BOOTCAUSE, + PDU_BOOTCNT, + PDU_UPTIME, + PDU_RESETCAUSE, + PDU_BATT_MODE, + PDU_LATCHUPS, + PDU_DEVICES, + PDU_STATUSES, + PDU_WDT_CNT_GND, + PDU_WDT_CNT_I2C, + PDU_WDT_CNT_CAN, + PDU_WDT_CNT_CSP1, + PDU_WDT_CNT_CSP2, + PDU_WDT_GND_LEFT, + PDU_WDT_I2C_LEFT, + PDU_WDT_CAN_LEFT, + PDU_WDT_CSP_LEFT1, + PDU_WDT_CSP_LEFT2, + + OUT_ON_CNT, + OUT_OFF_CNT, + INIT_OUT_NORM, + INIT_OUT_SAFE, + INIT_ON_DLY, + INIT_OFF_DLY, + SAFE_OFF_DLY, + CUR_LU_LIM, + CUR_LIM, + CUR_EMA, + OUT_LINK, + OUT_CONV, + OUT_VOLTAGE, + CONV_EN, + CUR_EMA_GAIN, + BATT_HWMAX, + BATT_MAX, + BATT_NORM, + BATT_SAFE, + BATT_CRIT, + WDT_I2C_RST, + WDT_CAN_RST, + WDT_I2C, + WDT_CAN, + WDT_CSP, + WDT_CSP_PING, + WDT_CSP_CHAN, + WDT_CSP_ADDR +}; +} + static const uint16_t MAX_CONFIGTABLE_ADDRESS = 316; static const uint16_t MAX_HKTABLE_ADDRESS = 141; /** The size of the csp reply containing the housekeeping table data */ @@ -395,22 +392,53 @@ class PduCoreHk : public StaticLocalDataSet<9> { PduCoreHk(object_id_t objectId, uint32_t setId) : StaticLocalDataSet(sid_t(objectId, setId)) {} /** Measured output currents */ - lp_vec_t currents = - lp_vec_t(sid.objectId, P60System::pool::PDU_CURRENTS, this); + lp_vec_t currents = lp_vec_t(sid.objectId, pool::PDU_CURRENTS, this); /** Measured output currents */ - lp_vec_t voltages = - lp_vec_t(sid.objectId, P60System::pool::PDU_VOLTAGES, this); + lp_vec_t voltages = lp_vec_t(sid.objectId, pool::PDU_VOLTAGES, this); /** Output switch states */ lp_vec_t outputEnables = - lp_vec_t(sid.objectId, P60System::pool::PDU_OUT_ENABLE, this); + lp_vec_t(sid.objectId, pool::PDU_OUT_ENABLE, this); /** Number of reboots */ - lp_var_t bootcount = - lp_var_t(sid.objectId, P60System::pool::PDU_BOOTCNT, this); + lp_var_t bootcount = lp_var_t(sid.objectId, pool::PDU_BOOTCNT, this); /** Battery mode: 1 = Critical, 2 = Safe, 3 = Normal, 4 = Full */ - lp_var_t battMode = - lp_var_t(sid.objectId, P60System::pool::PDU_BATT_MODE, this); - lp_var_t temperature = - lp_var_t(sid.objectId, P60System::pool::PDU_TEMPERATURE, this); + lp_var_t battMode = lp_var_t(sid.objectId, pool::PDU_BATT_MODE, this); + lp_var_t temperature = lp_var_t(sid.objectId, pool::PDU_TEMPERATURE, this); +}; + +class PduConfig : public StaticLocalDataSet<32> { + public: + PduConfig(HasLocalDataPoolIF* owner, uint32_t setId) : StaticLocalDataSet(owner, setId) {} + + lp_vec_t outOnDelaySecs = + lp_vec_t(sid.objectId, pool::OUT_ON_CNT, this); + lp_vec_t outOffDelaySecs = + lp_vec_t(sid.objectId, pool::OUT_OFF_CNT, this); + lp_vec_t initOutNorm = lp_vec_t(sid.objectId, pool::INIT_OUT_NORM, this); + lp_vec_t initOutSafe = lp_vec_t(sid.objectId, pool::INIT_OUT_SAFE, this); + lp_vec_t initOnDly = lp_vec_t(sid.objectId, pool::INIT_ON_DLY, this); + lp_vec_t initOffDly = lp_vec_t(sid.objectId, pool::INIT_OFF_DLY, this); + lp_vec_t safeOffDly = lp_vec_t(sid.objectId, pool::SAFE_OFF_DLY, this); + lp_vec_t curLuLim = lp_vec_t(sid.objectId, pool::CUR_LU_LIM, this); + lp_vec_t curLim = lp_vec_t(sid.objectId, pool::CUR_LIM, this); + lp_vec_t curEma = lp_vec_t(sid.objectId, pool::CUR_EMA, this); + lp_vec_t outLink = lp_vec_t(sid.objectId, pool::OUT_LINK, this); + lp_vec_t outConv = lp_vec_t(sid.objectId, pool::OUT_CONV, this); + lp_vec_t outVoltage = lp_vec_t(sid.objectId, pool::OUT_VOLTAGE, this); + lp_vec_t convEnable = lp_vec_t(sid.objectId, pool::CONV_EN, this); + lp_var_t curEmaGain = lp_var_t(sid.objectId, pool::CUR_EMA_GAIN, this); + lp_var_t battHwMax = lp_var_t(sid.objectId, pool::BATT_HWMAX, this); + lp_var_t battMax = lp_var_t(sid.objectId, pool::BATT_MAX, this); + lp_var_t battNorm = lp_var_t(sid.objectId, pool::BATT_NORM, this); + lp_var_t battSafe = lp_var_t(sid.objectId, pool::BATT_SAFE, this); + lp_var_t battCrit = lp_var_t(sid.objectId, pool::BATT_CRIT, this); + lp_var_t wdtI2cRst = lp_var_t(sid.objectId, pool::WDT_I2C_RST, this); + lp_var_t wdtCanRst = lp_var_t(sid.objectId, pool::WDT_CAN_RST, this); + lp_var_t wdtI2c = lp_var_t(sid.objectId, pool::WDT_I2C, this); + lp_var_t wdtCan = lp_var_t(sid.objectId, pool::WDT_CAN, this); + lp_vec_t wdtCsp = lp_vec_t(sid.objectId, pool::WDT_CSP, this); + lp_vec_t wdtCspPing = lp_vec_t(sid.objectId, pool::WDT_CSP_PING, this); + lp_vec_t wdtCspChannel = lp_vec_t(sid.objectId, pool::WDT_CSP_CHAN, this); + lp_vec_t wdtCspAddr = lp_vec_t(sid.objectId, pool::WDT_CSP_ADDR, this); }; /** @@ -423,64 +451,56 @@ class PduAuxHk : public StaticLocalDataSet<36> { PduAuxHk(object_id_t objectId, uint32_t setId) : StaticLocalDataSet(sid_t(objectId, setId)) {} /** Measured VCC */ - lp_var_t vcc = lp_var_t(sid.objectId, P60System::pool::PDU_VCC, this); + lp_var_t vcc = lp_var_t(sid.objectId, pool::PDU_VCC, this); /** Measured VBAT */ - lp_var_t vbat = lp_var_t(sid.objectId, P60System::pool::PDU_VBAT, this); + lp_var_t vbat = lp_var_t(sid.objectId, pool::PDU_VBAT, this); /** Output converter enable status */ lp_vec_t converterEnable = - lp_vec_t(sid.objectId, P60System::pool::PDU_CONV_EN, this); + lp_vec_t(sid.objectId, pool::PDU_CONV_EN, this); - lp_var_t bootcause = - lp_var_t(sid.objectId, P60System::pool::PDU_BOOTCAUSE, this); + lp_var_t bootcause = lp_var_t(sid.objectId, pool::PDU_BOOTCAUSE, this); /** Uptime in seconds */ - lp_var_t uptime = lp_var_t(sid.objectId, P60System::pool::PDU_UPTIME, this); - lp_var_t resetcause = - lp_var_t(sid.objectId, P60System::pool::PDU_RESETCAUSE, this); + lp_var_t uptime = lp_var_t(sid.objectId, pool::PDU_UPTIME, this); + lp_var_t resetcause = lp_var_t(sid.objectId, pool::PDU_RESETCAUSE, this); /** Number of detected latchups on each output channel */ - lp_vec_t latchups = - lp_vec_t(sid.objectId, P60System::pool::PDU_LATCHUPS, this); + lp_vec_t latchups = lp_vec_t(sid.objectId, pool::PDU_LATCHUPS, this); /** * There are 8 devices on the PDU. FRAM, ADCs, temperature sensor etc. Each device is * identified by an ID. Refer also to gs-man-nanopower-p60-pdu-200-1.pdf on pages 17 and 18. */ - lp_vec_t deviceTypes = - lp_vec_t(sid.objectId, P60System::pool::PDU_DEVICES, this); + lp_vec_t deviceTypes = lp_vec_t(sid.objectId, pool::PDU_DEVICES, this); /** The status of each device. 0 = None, 1 = Ok, 2 = Error, 3 = Not found */ - lp_vec_t devicesStatus = - lp_vec_t(sid.objectId, P60System::pool::PDU_STATUSES, this); + lp_vec_t devicesStatus = lp_vec_t(sid.objectId, pool::PDU_STATUSES, this); /** Number of reboots triggered by the ground watchdog */ - lp_var_t gndWdtReboots = - lp_var_t(sid.objectId, P60System::pool::PDU_WDT_CNT_GND, this); + lp_var_t gndWdtReboots = lp_var_t(sid.objectId, pool::PDU_WDT_CNT_GND, this); /** Number of reboots triggered through the I2C watchdog. Not relevant for EIVE. */ - lp_var_t i2cWdtReboots = - lp_var_t(sid.objectId, P60System::pool::PDU_WDT_CNT_I2C, this); + lp_var_t i2cWdtReboots = lp_var_t(sid.objectId, pool::PDU_WDT_CNT_I2C, this); /** Number of reboots triggered through the CAN watchdog */ - lp_var_t canWdtReboots = - lp_var_t(sid.objectId, P60System::pool::PDU_WDT_CNT_CAN, this); + lp_var_t canWdtReboots = lp_var_t(sid.objectId, pool::PDU_WDT_CNT_CAN, this); /** Number of reboots triggered through the CSP watchdog */ lp_var_t csp1WdtReboots = - lp_var_t(sid.objectId, P60System::pool::PDU_WDT_CNT_CSP1, this); + lp_var_t(sid.objectId, pool::PDU_WDT_CNT_CSP1, this); lp_var_t csp2WdtReboots = - lp_var_t(sid.objectId, P60System::pool::PDU_WDT_CNT_CSP2, this); + lp_var_t(sid.objectId, pool::PDU_WDT_CNT_CSP2, this); /** Ground watchdog remaining seconds before rebooting */ lp_var_t groundWatchdogSecondsLeft = - lp_var_t(sid.objectId, P60System::pool::PDU_WDT_GND_LEFT, this); + lp_var_t(sid.objectId, pool::PDU_WDT_GND_LEFT, this); /** I2C watchdog remaining seconds before rebooting. Not relevant for EIVE. */ lp_var_t i2cWatchdogSecondsLeft = - lp_var_t(sid.objectId, P60System::pool::PDU_WDT_I2C_LEFT, this); + lp_var_t(sid.objectId, pool::PDU_WDT_I2C_LEFT, this); /** CAN watchdog remaining seconds before rebooting. */ lp_var_t canWatchdogSecondsLeft = - lp_var_t(sid.objectId, P60System::pool::PDU_WDT_CAN_LEFT, this); + lp_var_t(sid.objectId, pool::PDU_WDT_CAN_LEFT, this); /** CSP watchdogs remaining pings before rebooting. */ lp_var_t csp2WatchdogPingsLeft = - lp_var_t(sid.objectId, P60System::pool::PDU_WDT_CSP_LEFT1, this); + lp_var_t(sid.objectId, pool::PDU_WDT_CSP_LEFT1, this); lp_var_t csp1WatchdogPingsLeft = - lp_var_t(sid.objectId, P60System::pool::PDU_WDT_CSP_LEFT2, this); + lp_var_t(sid.objectId, pool::PDU_WDT_CSP_LEFT2, this); }; } // namespace PDU @@ -531,6 +551,12 @@ class Pdu1AuxHk : public ::PDU::PduAuxHk { : PduAuxHk(objectId, static_cast(::P60System::SetIds::PDU_1_AUX)) {} }; +class Pdu1Config : public ::PDU::PduConfig { + public: + Pdu1Config(HasLocalDataPoolIF* owner) + : PduConfig(owner, static_cast(::P60System::SetIds::PDU_1_CONFIG)) {} +}; + } // namespace PDU1 namespace PDU2 { @@ -580,10 +606,42 @@ class Pdu2AuxHk : public ::PDU::PduAuxHk { : PduAuxHk(objectId, static_cast(::P60System::SetIds::PDU_2_AUX)) {} }; +class Pdu2Config : public ::PDU::PduConfig { + public: + Pdu2Config(HasLocalDataPoolIF* owner) + : PduConfig(owner, static_cast(::P60System::SetIds::PDU_2_CONFIG)) {} +}; + } // namespace PDU2 namespace ACU { +namespace pool { +enum Ids : lp_id_t { + /** ACU Ids */ + ACU_CURRENT_IN_CHANNELS, + ACU_VOLTAGE_IN_CHANNELS, + ACU_VCC, + ACU_VBAT, + ACU_TEMPERATURES, + ACU_MPPT_MODE, + ACU_VBOOST_IN_CHANNELS, + ACU_POWER_IN_CHANNELS, + ACU_DAC_ENABLES, + ACU_DAC_RAW_CHANNELS, + ACU_BOOTCAUSE, + ACU_BOOTCNT, + ACU_UPTIME, + ACU_RESET_CAUSE, + ACU_MPPT_TIME, + ACU_MPPT_PERIOD, + ACU_DEVICES, + ACU_DEVICES_STATUS, + ACU_WDT_CNT_GND, + ACU_WDT_GND_LEFT +}; +} + static const uint16_t MAX_CONFIGTABLE_ADDRESS = 26; static const uint16_t MAX_HKTABLE_ADDRESS = 120; static const uint8_t HK_TABLE_ENTRIES = 64; @@ -599,31 +657,27 @@ class CoreHk : public StaticLocalDataSet<14> { CoreHk(object_id_t objectId) : StaticLocalDataSet(sid_t(objectId, static_cast(::P60System::SetIds::ACU_CORE))) {} - lp_var_t mpptMode = - lp_var_t(sid.objectId, P60System::pool::ACU_MPPT_MODE, this); + lp_var_t mpptMode = lp_var_t(sid.objectId, pool::ACU_MPPT_MODE, this); lp_vec_t currentInChannels = - lp_vec_t(sid.objectId, P60System::pool::ACU_CURRENT_IN_CHANNELS, this); + lp_vec_t(sid.objectId, pool::ACU_CURRENT_IN_CHANNELS, this); lp_vec_t voltageInChannels = - lp_vec_t(sid.objectId, P60System::pool::ACU_VOLTAGE_IN_CHANNELS, this); + lp_vec_t(sid.objectId, pool::ACU_VOLTAGE_IN_CHANNELS, this); - lp_var_t vcc = lp_var_t(sid.objectId, P60System::pool::ACU_VCC, this); - lp_var_t vbat = lp_var_t(sid.objectId, P60System::pool::ACU_VBAT, this); + lp_var_t vcc = lp_var_t(sid.objectId, pool::ACU_VCC, this); + lp_var_t vbat = lp_var_t(sid.objectId, pool::ACU_VBAT, this); lp_vec_t vboostInChannels = - lp_vec_t(sid.objectId, P60System::pool::ACU_VBOOST_IN_CHANNELS, this); + lp_vec_t(sid.objectId, pool::ACU_VBOOST_IN_CHANNELS, this); lp_vec_t powerInChannels = - lp_vec_t(sid.objectId, P60System::pool::ACU_POWER_IN_CHANNELS, this); + lp_vec_t(sid.objectId, pool::ACU_POWER_IN_CHANNELS, this); - lp_vec_t temperatures = - lp_vec_t(sid.objectId, P60System::pool::ACU_TEMPERATURES, this); + lp_vec_t temperatures = lp_vec_t(sid.objectId, pool::ACU_TEMPERATURES, this); - lp_var_t bootcnt = lp_var_t(sid.objectId, P60System::pool::ACU_BOOTCNT, this); - lp_var_t uptime = lp_var_t(sid.objectId, P60System::pool::ACU_UPTIME, this); - lp_var_t mpptTime = - lp_var_t(sid.objectId, P60System::pool::ACU_MPPT_TIME, this); - lp_var_t mpptPeriod = - lp_var_t(sid.objectId, P60System::pool::ACU_MPPT_PERIOD, this); + lp_var_t bootcnt = lp_var_t(sid.objectId, pool::ACU_BOOTCNT, this); + lp_var_t uptime = lp_var_t(sid.objectId, pool::ACU_UPTIME, this); + lp_var_t mpptTime = lp_var_t(sid.objectId, pool::ACU_MPPT_TIME, this); + lp_var_t mpptPeriod = lp_var_t(sid.objectId, pool::ACU_MPPT_PERIOD, this); }; /** * @brief This class defines a dataset for the hk table of the ACU. @@ -636,31 +690,25 @@ class AuxHk : public StaticLocalDataSet<12> { AuxHk(object_id_t objectId) : StaticLocalDataSet(sid_t(objectId, static_cast(::P60System::SetIds::ACU_AUX))) {} - lp_vec_t dacEnables = - lp_vec_t(sid.objectId, P60System::pool::ACU_DAC_ENABLES, this); + lp_vec_t dacEnables = lp_vec_t(sid.objectId, pool::ACU_DAC_ENABLES, this); lp_vec_t dacRawChannelVals = - lp_vec_t(sid.objectId, P60System::pool::ACU_DAC_RAW_CHANNELS, this); + lp_vec_t(sid.objectId, pool::ACU_DAC_RAW_CHANNELS, this); - lp_var_t bootCause = - lp_var_t(sid.objectId, P60System::pool::ACU_BOOTCAUSE, this); - lp_var_t resetCause = - lp_var_t(sid.objectId, P60System::pool::ACU_RESET_CAUSE, this); + lp_var_t bootCause = lp_var_t(sid.objectId, pool::ACU_BOOTCAUSE, this); + lp_var_t resetCause = lp_var_t(sid.objectId, pool::ACU_RESET_CAUSE, this); - lp_var_t wdtCntGnd = - lp_var_t(sid.objectId, P60System::pool::ACU_WDT_CNT_GND, this); - lp_var_t wdtGndLeft = - lp_var_t(sid.objectId, P60System::pool::ACU_WDT_GND_LEFT, this); + lp_var_t wdtCntGnd = lp_var_t(sid.objectId, pool::ACU_WDT_CNT_GND, this); + lp_var_t wdtGndLeft = lp_var_t(sid.objectId, pool::ACU_WDT_GND_LEFT, this); /** * There are 8 devices on the PDU. FRAM, ADCs, temperature sensor etc. Each device is * identified by an ID. Refer also to gs-man-nanopower-p60-pdu-200-1.pdf on pages 17 and 18. */ - lp_vec_t deviceTypes = - lp_vec_t(sid.objectId, P60System::pool::ACU_DEVICES, this); + lp_vec_t deviceTypes = lp_vec_t(sid.objectId, pool::ACU_DEVICES, this); /** The status of each device. 0 = None, 1 = Ok, 2 = Error, 3 = Not found */ lp_vec_t devicesStatus = - lp_vec_t(sid.objectId, P60System::pool::ACU_DEVICES_STATUS, this); + lp_vec_t(sid.objectId, pool::ACU_DEVICES_STATUS, this); }; } // namespace ACU diff --git a/tmtc b/tmtc index 079a0f94..efc8e3b4 160000 --- a/tmtc +++ b/tmtc @@ -1 +1 @@ -Subproject commit 079a0f94727dc3f35578dc412aa01c871ae1ac6a +Subproject commit efc8e3b4b2078d070a39efe5338ea89ba56cd248 From 7de26586fbf6a89f172fcb27af699e83f193abe3 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Sat, 27 Aug 2022 15:51:45 +0200 Subject: [PATCH 24/52] need to link gs sw for hosted now --- CMakeLists.txt | 18 +++++------------- thirdparty/gomspace-sw | 2 +- 2 files changed, 6 insertions(+), 14 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index f85fda0f..976bc6ee 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -367,9 +367,7 @@ if(EIVE_ADD_LINUX_FILES) add_subdirectory(${LINUX_PATH}) endif() add_subdirectory(${BSP_PATH}) -if(ADD_GOMSPACE_CSP OR ADD_GOMSPACE_CLIENTS) - add_subdirectory(${LIB_GOMSPACE_PATH}) -endif() +add_subdirectory(${LIB_GOMSPACE_PATH}) add_subdirectory(${COMMON_PATH}) add_subdirectory(${DUMMY_PATH}) @@ -454,21 +452,15 @@ if(TGT_BSP MATCHES "arm/q7s") endif() target_link_libraries(${UNITTEST_NAME} PRIVATE Catch2 ${LIB_EIVE_MISSION} - rapidcsv ${LIB_DUMMIES}) + rapidcsv ${LIB_DUMMIES} ${LIB_GOMSPACE_CLIENTS}) if(TGT_BSP MATCHES "arm/egse") target_link_libraries(${OBSW_NAME} PRIVATE ${LIB_ARCSEC}) endif() -if(ADD_GOMSPACE_CSP) - target_link_libraries(${OBSW_NAME} PRIVATE ${LIB_GOMSPACE_CSP}) -endif() - -if(ADD_GOMSPACE_CLIENTS) - target_link_libraries(${OBSW_NAME} PRIVATE ${LIB_GOMSPACE_CLIENTS}) - target_link_libraries(${LIB_EIVE_MISSION} PRIVATE ${LIB_GOMSPACE_CLIENTS}) - target_link_libraries(${LIB_DUMMIES} PRIVATE ${LIB_GOMSPACE_CLIENTS}) -endif() +target_link_libraries(${OBSW_NAME} PRIVATE ${LIB_GOMSPACE_CLIENTS}) +target_link_libraries(${LIB_EIVE_MISSION} PRIVATE ${LIB_GOMSPACE_CLIENTS}) +target_link_libraries(${LIB_DUMMIES} PRIVATE ${LIB_GOMSPACE_CLIENTS}) if(EIVE_ADD_ETL_LIB) target_link_libraries(${LIB_EIVE_MISSION} PUBLIC ${LIB_ETL_TARGET}) diff --git a/thirdparty/gomspace-sw b/thirdparty/gomspace-sw index d24a574f..15d489b8 160000 --- a/thirdparty/gomspace-sw +++ b/thirdparty/gomspace-sw @@ -1 +1 @@ -Subproject commit d24a574ffd32bd4da8aa69e768180ccae76d6c85 +Subproject commit 15d489b884f46d232ec7d8879e7355d83873f06f From 3b2794735baf4bf310512db25f0cb4c4be1ceb58 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Sat, 27 Aug 2022 15:54:52 +0200 Subject: [PATCH 25/52] disable test --- bsp_q7s/boardtest/Q7STestTask.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/bsp_q7s/boardtest/Q7STestTask.h b/bsp_q7s/boardtest/Q7STestTask.h index 8d39311d..9e39e8b3 100644 --- a/bsp_q7s/boardtest/Q7STestTask.h +++ b/bsp_q7s/boardtest/Q7STestTask.h @@ -17,7 +17,7 @@ class Q7STestTask : public TestTask { bool doTestSdCard = false; bool doTestScratchApi = false; static constexpr bool DO_TEST_GOMSPACE_API = false; - static constexpr bool DO_TEST_GOMSPACE_GET_CONFIG = true; + static constexpr bool DO_TEST_GOMSPACE_GET_CONFIG = false; bool doTestGpsShm = false; bool doTestGpsSocket = false; bool doTestProtHandler = false; From c8c103e5e66d8d21cbd919a70bb47554c199a47c Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Sat, 27 Aug 2022 16:04:55 +0200 Subject: [PATCH 26/52] update changelog --- CHANGELOG.md | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index 9eee73fc..ce393fea 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -13,6 +13,10 @@ list yields a list of all related PRs for each release. # [v1.14.0] - Update for FSFW: `HasReturnvaluesIF` class replaced by namespace `returnvalue` +- Add some GomSpace clients as a submodule dependency. Use this dependency to deserialize the + GomSpace TM tables +- Add API to retrieve GomSpace device parameter tables + PR: https://egit.irs.uni-stuttgart.de/eive/eive-obsw/pulls/287 # [v1.13.0] 24.08.2022 From 108b7620b8f65d2647563b4457e57433b0e663e0 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Sat, 27 Aug 2022 17:18:00 +0200 Subject: [PATCH 27/52] bump tmtc --- tmtc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tmtc b/tmtc index efc8e3b4..602e2cf4 160000 --- a/tmtc +++ b/tmtc @@ -1 +1 @@ -Subproject commit efc8e3b4b2078d070a39efe5338ea89ba56cd248 +Subproject commit 602e2cf4a88ca7a7b4eb89197110d836adec8ff7 From f8ee516d4650bf5fbce56da22e0ff2f9e824de44 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Mon, 29 Aug 2022 13:21:00 +0200 Subject: [PATCH 28/52] bump dependencies --- fsfw | 2 +- tmtc | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/fsfw b/fsfw index 2a75440b..9a590a3f 160000 --- a/fsfw +++ b/fsfw @@ -1 +1 @@ -Subproject commit 2a75440b325b70b7ca90269159440a1f9a4a6e2a +Subproject commit 9a590a3fcd81ed4dd48de503522b6d71f64205d2 diff --git a/tmtc b/tmtc index 602e2cf4..729e9450 160000 --- a/tmtc +++ b/tmtc @@ -1 +1 @@ -Subproject commit 602e2cf4a88ca7a7b4eb89197110d836adec8ff7 +Subproject commit 729e9450139a79a3fa55aec8d109088093ba8410 From f3087182565ea7b0f73dfdf17b18b751dcd64cbf Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Mon, 29 Aug 2022 13:26:13 +0200 Subject: [PATCH 29/52] safety check --- linux/devices/ploc/PlocSupervisorHandler.cpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/linux/devices/ploc/PlocSupervisorHandler.cpp b/linux/devices/ploc/PlocSupervisorHandler.cpp index f683998d..1cff8884 100644 --- a/linux/devices/ploc/PlocSupervisorHandler.cpp +++ b/linux/devices/ploc/PlocSupervisorHandler.cpp @@ -417,6 +417,9 @@ ReturnValue_t PlocSupervisorHandler::buildCommandFromCommand(DeviceCommandId_t d break; } case LOGGING_SET_TOPIC: { + if(commandData == nullptr or commandDataLen == 0) { + return HasActionsIF::INVALID_PARAMETERS; + } uint8_t tpc = *(commandData); RequestLoggingData packet(spParams); result = packet.buildPacket(RequestLoggingData::Sa::SET_LOGGING_TOPIC, tpc); From 3a80c4cb0c4fe015e2f19db990778c4a569400b3 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Mon, 29 Aug 2022 13:32:49 +0200 Subject: [PATCH 30/52] additional nullptr check --- linux/devices/Max31865RtdLowlevelHandler.cpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/linux/devices/Max31865RtdLowlevelHandler.cpp b/linux/devices/Max31865RtdLowlevelHandler.cpp index 4869fe39..f2815f5f 100644 --- a/linux/devices/Max31865RtdLowlevelHandler.cpp +++ b/linux/devices/Max31865RtdLowlevelHandler.cpp @@ -311,6 +311,9 @@ ReturnValue_t Max31865RtdReader::readReceivedMessage(CookieIF* cookie, uint8_t** return returnvalue::FAILED; } auto* rtdCookie = dynamic_cast(cookie); + if(rtdCookie == nullptr) { + return returnvalue::FAILED; + } uint8_t* exchangePtr = rtdCookie->exchangeBuf.data(); size_t serLen = 0; auto result = rtdCookie->db.serialize(&exchangePtr, &serLen, rtdCookie->exchangeBuf.size(), From 04fb2b1261f9f27deaf733ba12c8336836ad4e76 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Mon, 29 Aug 2022 15:44:40 +0200 Subject: [PATCH 31/52] bump tmtc --- tmtc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tmtc b/tmtc index 729e9450..05dd1738 160000 --- a/tmtc +++ b/tmtc @@ -1 +1 @@ -Subproject commit 729e9450139a79a3fa55aec8d109088093ba8410 +Subproject commit 05dd17386070b9f333977f1f2b9f5651f9053b65 From 74b45f30a053da4e261cfc54a76b166ce887acbc Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 30 Aug 2022 15:53:20 +0200 Subject: [PATCH 32/52] move to new DHB TM handler --- CMakeLists.txt | 5 +++-- fsfw | 2 +- linux/devices/GPSHyperionLinuxController.cpp | 2 +- linux/devices/Max31865RtdLowlevelHandler.cpp | 2 +- linux/devices/ploc/PlocSupervisorHandler.cpp | 2 +- linux/devices/startracker/StarTrackerHandler.cpp | 2 +- mission/controller/AcsController.cpp | 2 +- mission/devices/ACUHandler.cpp | 3 ++- mission/devices/GomspaceDeviceHandler.cpp | 6 ++++-- mission/devices/P60DockHandler.cpp | 7 +------ mission/devices/PDU1Handler.cpp | 5 +++-- mission/devices/PDU2Handler.cpp | 9 +++------ 12 files changed, 22 insertions(+), 25 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 976bc6ee..9b8190ec 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -451,8 +451,9 @@ if(TGT_BSP MATCHES "arm/q7s") target_link_libraries(${LIB_EIVE_MISSION} PUBLIC ${LIB_GPS} ${LIB_ARCSEC}) endif() -target_link_libraries(${UNITTEST_NAME} PRIVATE Catch2 ${LIB_EIVE_MISSION} - rapidcsv ${LIB_DUMMIES} ${LIB_GOMSPACE_CLIENTS}) +target_link_libraries( + ${UNITTEST_NAME} PRIVATE Catch2 ${LIB_EIVE_MISSION} rapidcsv ${LIB_DUMMIES} + ${LIB_GOMSPACE_CLIENTS}) if(TGT_BSP MATCHES "arm/egse") target_link_libraries(${OBSW_NAME} PRIVATE ${LIB_ARCSEC}) diff --git a/fsfw b/fsfw index 9a590a3f..21ac8661 160000 --- a/fsfw +++ b/fsfw @@ -1 +1 @@ -Subproject commit 9a590a3fcd81ed4dd48de503522b6d71f64205d2 +Subproject commit 21ac86619e2b1fecf71bd2aa0f09ecbd471c8531 diff --git a/linux/devices/GPSHyperionLinuxController.cpp b/linux/devices/GPSHyperionLinuxController.cpp index 556db7ef..84200baf 100644 --- a/linux/devices/GPSHyperionLinuxController.cpp +++ b/linux/devices/GPSHyperionLinuxController.cpp @@ -87,7 +87,7 @@ ReturnValue_t GPSHyperionLinuxController::initializeLocalDataPool( localDataPoolMap.emplace(GpsHyperion::SATS_IN_USE, new PoolEntry()); localDataPoolMap.emplace(GpsHyperion::SATS_IN_VIEW, new PoolEntry()); localDataPoolMap.emplace(GpsHyperion::FIX_MODE, new PoolEntry()); - poolManager.subscribeForRegularPeriodicPacket({gpsSet.getSid(), 30.0}); + poolManager.subscribeForRegularPeriodicPacket({gpsSet.getSid(), false, 30.0}); return returnvalue::OK; } diff --git a/linux/devices/Max31865RtdLowlevelHandler.cpp b/linux/devices/Max31865RtdLowlevelHandler.cpp index f2815f5f..1be2b1c2 100644 --- a/linux/devices/Max31865RtdLowlevelHandler.cpp +++ b/linux/devices/Max31865RtdLowlevelHandler.cpp @@ -311,7 +311,7 @@ ReturnValue_t Max31865RtdReader::readReceivedMessage(CookieIF* cookie, uint8_t** return returnvalue::FAILED; } auto* rtdCookie = dynamic_cast(cookie); - if(rtdCookie == nullptr) { + if (rtdCookie == nullptr) { return returnvalue::FAILED; } uint8_t* exchangePtr = rtdCookie->exchangeBuf.data(); diff --git a/linux/devices/ploc/PlocSupervisorHandler.cpp b/linux/devices/ploc/PlocSupervisorHandler.cpp index 1cff8884..3846e4a4 100644 --- a/linux/devices/ploc/PlocSupervisorHandler.cpp +++ b/linux/devices/ploc/PlocSupervisorHandler.cpp @@ -417,7 +417,7 @@ ReturnValue_t PlocSupervisorHandler::buildCommandFromCommand(DeviceCommandId_t d break; } case LOGGING_SET_TOPIC: { - if(commandData == nullptr or commandDataLen == 0) { + if (commandData == nullptr or commandDataLen == 0) { return HasActionsIF::INVALID_PARAMETERS; } uint8_t tpc = *(commandData); diff --git a/linux/devices/startracker/StarTrackerHandler.cpp b/linux/devices/startracker/StarTrackerHandler.cpp index 698887b8..47312934 100644 --- a/linux/devices/startracker/StarTrackerHandler.cpp +++ b/linux/devices/startracker/StarTrackerHandler.cpp @@ -1830,7 +1830,7 @@ ReturnValue_t StarTrackerHandler::handleChecksumReply() { } PoolReadGuard rg(&checksumSet); checksumSet.checksum = checksumReply.getChecksum(); - handleDeviceTM(&checksumSet, startracker::CHECKSUM); + handleDeviceTm(util::DataWrapper(checksumSet), startracker::CHECKSUM); #if OBSW_VERBOSE_LEVEL >= 1 && OBSW_DEBUG_STARTRACKER == 1 checksumReply.printChecksum(); #endif /* OBSW_VERBOSE_LEVEL >= 1 && OBSW_DEBUG_STARTRACKER == 1 */ diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp index c6b3207e..09e91e26 100644 --- a/mission/controller/AcsController.cpp +++ b/mission/controller/AcsController.cpp @@ -45,7 +45,7 @@ ReturnValue_t AcsController::initializeLocalDataPool(localpool::DataPool &localD localDataPoolMap.emplace(acsctrl::PoolIds::MGM_3_RM3100_UT, &mgm3PoolVec); localDataPoolMap.emplace(acsctrl::PoolIds::MGM_IMTQ_CAL_NT, &imtqMgmPoolVec); localDataPoolMap.emplace(acsctrl::PoolIds::MGM_IMTQ_CAL_ACT_STATUS, &imtqCalActStatus); - poolManager.subscribeForRegularPeriodicPacket({mgmData.getSid(), 5.0}); + poolManager.subscribeForRegularPeriodicPacket({mgmData.getSid(), false, 5.0}); return returnvalue::OK; } diff --git a/mission/devices/ACUHandler.cpp b/mission/devices/ACUHandler.cpp index f229d248..e86c0801 100644 --- a/mission/devices/ACUHandler.cpp +++ b/mission/devices/ACUHandler.cpp @@ -43,7 +43,8 @@ void ACUHandler::letChildHandleHkReply(DeviceCommandId_t id, const uint8_t *pack } void ACUHandler::letChildHandleConfigReply(DeviceCommandId_t id, const uint8_t *packet) { - handleDeviceTM(packet, ACU::CONFIG_TABLE_SIZE, id); + util::DataWrapper wrapper(packet, ACU::CONFIG_TABLE_SIZE); + handleDeviceTm(wrapper, id); } LocalPoolDataSetBase *ACUHandler::getDataSetHandle(sid_t sid) { diff --git a/mission/devices/GomspaceDeviceHandler.cpp b/mission/devices/GomspaceDeviceHandler.cpp index cc4786d0..9278ed77 100644 --- a/mission/devices/GomspaceDeviceHandler.cpp +++ b/mission/devices/GomspaceDeviceHandler.cpp @@ -177,7 +177,9 @@ ReturnValue_t GomspaceDeviceHandler::interpretDeviceReply(DeviceCommandId_t id, switch (id) { case (GOMSPACE::PING): { SerializeElement replyTime = *packet; - handleDeviceTM(&replyTime, id, true); + util::DataWrapper wrapper; + wrapper.setSerializable(replyTime); + handleDeviceTm(wrapper, id, true); break; } case (GOMSPACE::PARAM_GET): { @@ -204,7 +206,7 @@ ReturnValue_t GomspaceDeviceHandler::interpretDeviceReply(DeviceCommandId_t id, uint16_t address = cspGetParamReply.getAddress(); /* Pack relevant information into a tm packet */ ParamReply paramReply(action, tableId, address, payloadLength, tempPayloadBuffer); - handleDeviceTM(¶mReply, id, true); + handleDeviceTm(util::DataWrapper(paramReply), id, true); break; } case (GOMSPACE::PARAM_SET): { diff --git a/mission/devices/P60DockHandler.cpp b/mission/devices/P60DockHandler.cpp index 4d4d4ed2..574efd81 100644 --- a/mission/devices/P60DockHandler.cpp +++ b/mission/devices/P60DockHandler.cpp @@ -24,11 +24,6 @@ ReturnValue_t P60DockHandler::buildNormalDeviceCommand(DeviceCommandId_t *id) { void P60DockHandler::letChildHandleHkReply(DeviceCommandId_t id, const uint8_t *packet) { parseHkTableReply(packet); - /** - * Hk table will be sent to the commander if hk table request was not triggered by the - * P60DockHandler itself. - */ - handleDeviceTM(&coreHk, id, true); } void P60DockHandler::parseHkTableReply(const uint8_t *packet) { @@ -273,5 +268,5 @@ void P60DockHandler::printHkTableLatchups() { void P60DockHandler::setDebugMode(bool enable) { this->debugMode = enable; } void P60DockHandler::letChildHandleConfigReply(DeviceCommandId_t id, const uint8_t *packet) { - handleDeviceTM(packet, P60Dock::CONFIG_TABLE_SIZE, id); + handleDeviceTm(util::DataWrapper(packet, P60Dock::CONFIG_TABLE_SIZE), id); } diff --git a/mission/devices/PDU1Handler.cpp b/mission/devices/PDU1Handler.cpp index deb747db..e7a077ba 100644 --- a/mission/devices/PDU1Handler.cpp +++ b/mission/devices/PDU1Handler.cpp @@ -22,7 +22,6 @@ ReturnValue_t PDU1Handler::buildNormalDeviceCommand(DeviceCommandId_t *id) { void PDU1Handler::letChildHandleHkReply(DeviceCommandId_t id, const uint8_t *packet) { parseHkTableReply(packet); - handleDeviceTM(&coreHk, id, true); } void PDU1Handler::assignChannelHookFunction(GOMSPACE::ChannelSwitchHook hook, void *args) { @@ -81,7 +80,9 @@ ReturnValue_t PDU1Handler::setParamCallback(SetParamMessageUnpacker &unpacker, } void PDU1Handler::letChildHandleConfigReply(DeviceCommandId_t id, const uint8_t *packet) { - handleDeviceTM(packet, PDU::CONFIG_TABLE_SIZE, id); + util::DataWrapper wrapper; + wrapper.setRawData({packet, PDU::CONFIG_TABLE_SIZE}); + handleDeviceTm(wrapper, id); } void PDU1Handler::parseHkTableReply(const uint8_t *packet) { diff --git a/mission/devices/PDU2Handler.cpp b/mission/devices/PDU2Handler.cpp index 65de91a7..9fb0fffe 100644 --- a/mission/devices/PDU2Handler.cpp +++ b/mission/devices/PDU2Handler.cpp @@ -22,15 +22,12 @@ ReturnValue_t PDU2Handler::buildNormalDeviceCommand(DeviceCommandId_t *id) { void PDU2Handler::letChildHandleHkReply(DeviceCommandId_t id, const uint8_t *packet) { parseHkTableReply(packet); - /** - * Hk table will be sent to the commander if hk table request was not triggered by the - * PDU2Handler itself. - */ - handleDeviceTM(&coreHk, id, true); } void PDU2Handler::letChildHandleConfigReply(DeviceCommandId_t id, const uint8_t *packet) { - handleDeviceTM(packet, PDU::CONFIG_TABLE_SIZE, id); + util::DataWrapper wrapper; + wrapper.setRawData({packet, PDU::CONFIG_TABLE_SIZE}); + handleDeviceTm(wrapper, id); } void PDU2Handler::assignChannelHookFunction(GOMSPACE::ChannelSwitchHook hook, void *args) { From 4649ae2edb63cfeb07120a6b7e1717a4b2f9735b Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 30 Aug 2022 16:06:17 +0200 Subject: [PATCH 33/52] minor tweaks --- fsfw | 2 +- mission/devices/ACUHandler.cpp | 3 +-- 2 files changed, 2 insertions(+), 3 deletions(-) diff --git a/fsfw b/fsfw index 21ac8661..141dcb1f 160000 --- a/fsfw +++ b/fsfw @@ -1 +1 @@ -Subproject commit 21ac86619e2b1fecf71bd2aa0f09ecbd471c8531 +Subproject commit 141dcb1f149b824b5bd6e5d3339ecb712835751e diff --git a/mission/devices/ACUHandler.cpp b/mission/devices/ACUHandler.cpp index e86c0801..727b4ae5 100644 --- a/mission/devices/ACUHandler.cpp +++ b/mission/devices/ACUHandler.cpp @@ -43,8 +43,7 @@ void ACUHandler::letChildHandleHkReply(DeviceCommandId_t id, const uint8_t *pack } void ACUHandler::letChildHandleConfigReply(DeviceCommandId_t id, const uint8_t *packet) { - util::DataWrapper wrapper(packet, ACU::CONFIG_TABLE_SIZE); - handleDeviceTm(wrapper, id); + handleDeviceTm(util::DataWrapper(packet, ACU::CONFIG_TABLE_SIZE), id); } LocalPoolDataSetBase *ACUHandler::getDataSetHandle(sid_t sid) { From 822e8eea4aa04197f43773d8e77d4d8cf87f1e80 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 30 Aug 2022 23:54:05 +0200 Subject: [PATCH 34/52] this is better --- fsfw | 2 +- linux/devices/startracker/StarTrackerHandler.cpp | 2 +- mission/devices/ACUHandler.cpp | 2 +- mission/devices/GomspaceDeviceHandler.cpp | 6 ++---- mission/devices/P60DockHandler.cpp | 2 +- mission/devices/PDU1Handler.cpp | 4 +--- mission/devices/PDU2Handler.cpp | 4 +--- 7 files changed, 8 insertions(+), 14 deletions(-) diff --git a/fsfw b/fsfw index 141dcb1f..726f44ca 160000 --- a/fsfw +++ b/fsfw @@ -1 +1 @@ -Subproject commit 141dcb1f149b824b5bd6e5d3339ecb712835751e +Subproject commit 726f44cafeb8cde21b75df8f061619d51b9d9cfd diff --git a/linux/devices/startracker/StarTrackerHandler.cpp b/linux/devices/startracker/StarTrackerHandler.cpp index 47312934..a4cd432c 100644 --- a/linux/devices/startracker/StarTrackerHandler.cpp +++ b/linux/devices/startracker/StarTrackerHandler.cpp @@ -1830,7 +1830,7 @@ ReturnValue_t StarTrackerHandler::handleChecksumReply() { } PoolReadGuard rg(&checksumSet); checksumSet.checksum = checksumReply.getChecksum(); - handleDeviceTm(util::DataWrapper(checksumSet), startracker::CHECKSUM); + handleDeviceTm(checksumSet, startracker::CHECKSUM); #if OBSW_VERBOSE_LEVEL >= 1 && OBSW_DEBUG_STARTRACKER == 1 checksumReply.printChecksum(); #endif /* OBSW_VERBOSE_LEVEL >= 1 && OBSW_DEBUG_STARTRACKER == 1 */ diff --git a/mission/devices/ACUHandler.cpp b/mission/devices/ACUHandler.cpp index 727b4ae5..2a8ffe8b 100644 --- a/mission/devices/ACUHandler.cpp +++ b/mission/devices/ACUHandler.cpp @@ -43,7 +43,7 @@ void ACUHandler::letChildHandleHkReply(DeviceCommandId_t id, const uint8_t *pack } void ACUHandler::letChildHandleConfigReply(DeviceCommandId_t id, const uint8_t *packet) { - handleDeviceTm(util::DataWrapper(packet, ACU::CONFIG_TABLE_SIZE), id); + handleDeviceTm(packet, ACU::CONFIG_TABLE_SIZE, id); } LocalPoolDataSetBase *ACUHandler::getDataSetHandle(sid_t sid) { diff --git a/mission/devices/GomspaceDeviceHandler.cpp b/mission/devices/GomspaceDeviceHandler.cpp index 9278ed77..5acd209e 100644 --- a/mission/devices/GomspaceDeviceHandler.cpp +++ b/mission/devices/GomspaceDeviceHandler.cpp @@ -177,9 +177,7 @@ ReturnValue_t GomspaceDeviceHandler::interpretDeviceReply(DeviceCommandId_t id, switch (id) { case (GOMSPACE::PING): { SerializeElement replyTime = *packet; - util::DataWrapper wrapper; - wrapper.setSerializable(replyTime); - handleDeviceTm(wrapper, id, true); + handleDeviceTm(replyTime, true); break; } case (GOMSPACE::PARAM_GET): { @@ -206,7 +204,7 @@ ReturnValue_t GomspaceDeviceHandler::interpretDeviceReply(DeviceCommandId_t id, uint16_t address = cspGetParamReply.getAddress(); /* Pack relevant information into a tm packet */ ParamReply paramReply(action, tableId, address, payloadLength, tempPayloadBuffer); - handleDeviceTm(util::DataWrapper(paramReply), id, true); + handleDeviceTm(paramReply, id, true); break; } case (GOMSPACE::PARAM_SET): { diff --git a/mission/devices/P60DockHandler.cpp b/mission/devices/P60DockHandler.cpp index 574efd81..8c768847 100644 --- a/mission/devices/P60DockHandler.cpp +++ b/mission/devices/P60DockHandler.cpp @@ -268,5 +268,5 @@ void P60DockHandler::printHkTableLatchups() { void P60DockHandler::setDebugMode(bool enable) { this->debugMode = enable; } void P60DockHandler::letChildHandleConfigReply(DeviceCommandId_t id, const uint8_t *packet) { - handleDeviceTm(util::DataWrapper(packet, P60Dock::CONFIG_TABLE_SIZE), id); + handleDeviceTm(packet, P60Dock::CONFIG_TABLE_SIZE, id); } diff --git a/mission/devices/PDU1Handler.cpp b/mission/devices/PDU1Handler.cpp index e7a077ba..9a44da5e 100644 --- a/mission/devices/PDU1Handler.cpp +++ b/mission/devices/PDU1Handler.cpp @@ -80,9 +80,7 @@ ReturnValue_t PDU1Handler::setParamCallback(SetParamMessageUnpacker &unpacker, } void PDU1Handler::letChildHandleConfigReply(DeviceCommandId_t id, const uint8_t *packet) { - util::DataWrapper wrapper; - wrapper.setRawData({packet, PDU::CONFIG_TABLE_SIZE}); - handleDeviceTm(wrapper, id); + handleDeviceTm(packet, PDU::CONFIG_TABLE_SIZE, id); } void PDU1Handler::parseHkTableReply(const uint8_t *packet) { diff --git a/mission/devices/PDU2Handler.cpp b/mission/devices/PDU2Handler.cpp index 9fb0fffe..4db6999f 100644 --- a/mission/devices/PDU2Handler.cpp +++ b/mission/devices/PDU2Handler.cpp @@ -25,9 +25,7 @@ void PDU2Handler::letChildHandleHkReply(DeviceCommandId_t id, const uint8_t *pac } void PDU2Handler::letChildHandleConfigReply(DeviceCommandId_t id, const uint8_t *packet) { - util::DataWrapper wrapper; - wrapper.setRawData({packet, PDU::CONFIG_TABLE_SIZE}); - handleDeviceTm(wrapper, id); + handleDeviceTm(packet, PDU::CONFIG_TABLE_SIZE, id); } void PDU2Handler::assignChannelHookFunction(GOMSPACE::ChannelSwitchHook hook, void *args) { From 20df16916ad3c43bd51152484624841a34af68ff Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Wed, 31 Aug 2022 00:03:00 +0200 Subject: [PATCH 35/52] bump fsfw --- fsfw | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/fsfw b/fsfw index 726f44ca..2fa76d36 160000 --- a/fsfw +++ b/fsfw @@ -1 +1 @@ -Subproject commit 726f44cafeb8cde21b75df8f061619d51b9d9cfd +Subproject commit 2fa76d366325372e92a2188f71f143a485e652fc From 7dde1150e8bf355c82ae858eac6cba86d9f7f3a4 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Wed, 31 Aug 2022 22:52:32 +0200 Subject: [PATCH 36/52] gom space commands --- fsfw | 2 +- linux/csp/CspComIF.cpp | 10 +++++ mission/core/GenericFactory.cpp | 2 +- mission/csp/CspCookie.h | 2 +- mission/devices/GomspaceDeviceHandler.cpp | 41 +++++++++++++++++++ .../devicedefinitions/GomspaceDefinitions.h | 15 +++++-- tmtc | 2 +- 7 files changed, 67 insertions(+), 7 deletions(-) diff --git a/fsfw b/fsfw index 2fa76d36..496dac89 160000 --- a/fsfw +++ b/fsfw @@ -1 +1 @@ -Subproject commit 2fa76d366325372e92a2188f71f143a485e652fc +Subproject commit 496dac89e4c094e03578f40d8651485f7f378cb2 diff --git a/linux/csp/CspComIF.cpp b/linux/csp/CspComIF.cpp index eb6487ed..b52bcfa2 100644 --- a/linux/csp/CspComIF.cpp +++ b/linux/csp/CspComIF.cpp @@ -166,6 +166,16 @@ ReturnValue_t CspComIF::sendMessage(CookieIF* cookie, const uint8_t* sendData, s if (result != 0) { return returnvalue::FAILED; } + } else if(req == GOMSPACE::SpecialRequestTypes::SAVE_TABLE) { + if(sendLen < 2) { + return returnvalue::FAILED; + } + const TableInfo* tableInfo = reinterpret_cast(sendData); + int result = gs_rparam_save(cspAddress, cspCookie->getTimeout(), tableInfo->sourceTable, + tableInfo->targetTable); + if (result != 0) { + return returnvalue::FAILED; + } } } else { /* No CSP fixed port was selected. Send data to the specified port and diff --git a/mission/core/GenericFactory.cpp b/mission/core/GenericFactory.cpp index 0a9be3f9..bbd2bb98 100644 --- a/mission/core/GenericFactory.cpp +++ b/mission/core/GenericFactory.cpp @@ -96,7 +96,7 @@ void ObjectFactory::produceGenericObjects(HealthTableIF** healthTable_) { PsbParams(objects::PUS_SERVICE_5_EVENT_REPORTING, apid::EIVE_OBSW, pus::PUS_SERVICE_5), 15, 45); new Service8FunctionManagement(objects::PUS_SERVICE_8_FUNCTION_MGMT, apid::EIVE_OBSW, - pus::PUS_SERVICE_8, 3, 60); + pus::PUS_SERVICE_8, 16, 60); new Service9TimeManagement( PsbParams(objects::PUS_SERVICE_9_TIME_MGMT, apid::EIVE_OBSW, pus::PUS_SERVICE_9)); diff --git a/mission/csp/CspCookie.h b/mission/csp/CspCookie.h index 441eb413..9eb0a639 100644 --- a/mission/csp/CspCookie.h +++ b/mission/csp/CspCookie.h @@ -28,7 +28,7 @@ class CspCookie : public CookieIF { uint32_t getTimeout() const; private: - uint8_t cspPort; + uint8_t cspPort = 0; uint16_t maxReplyLength; uint8_t cspAddress; size_t replyLen = 0; diff --git a/mission/devices/GomspaceDeviceHandler.cpp b/mission/devices/GomspaceDeviceHandler.cpp index 5acd209e..aa4459b4 100644 --- a/mission/devices/GomspaceDeviceHandler.cpp +++ b/mission/devices/GomspaceDeviceHandler.cpp @@ -115,6 +115,45 @@ ReturnValue_t GomspaceDeviceHandler::buildCommandFromCommand(DeviceCommandId_t d } break; } + case (GOMSPACE::SAVE_TABLE_FILE): { + if(commandDataLen > 2) { + return HasActionsIF::INVALID_PARAMETERS; + } + auto* info = reinterpret_cast(cspPacket); + if(commandData[0] != 0 and commandData[0] != 1 and commandData[0] != 2 and commandData[0] != 4) { + return HasActionsIF::INVALID_PARAMETERS; + } + info->sourceTable = commandData[0]; + if (info->sourceTable != 4) { + if(commandDataLen == 2) { + info->targetTable = commandData[1]; + } else { + info->targetTable = info->sourceTable; + } + } else { + // Will be ignored, still set the value which is always used + info->targetTable = 4; + } + rawPacket = cspPacket; + rawPacketLen = sizeof(GOMSPACE::TableInfo); + rememberCommandId = deviceCommand; + break; + } + case (GOMSPACE::SAVE_TABLE_DEFAULT): { + if(commandDataLen != 1) { + return HasActionsIF::INVALID_PARAMETERS; + } + auto* info = reinterpret_cast(cspPacket); + if(commandData[0] != 0 and commandData[0] != 1 and commandData[0] != 2) { + return HasActionsIF::INVALID_PARAMETERS; + } + info->sourceTable = commandData[0]; + info->targetTable = info->sourceTable + 4; + rawPacket = cspPacket; + rawPacketLen = sizeof(GOMSPACE::TableInfo); + rememberCommandId = deviceCommand; + break; + } default: return DeviceHandlerIF::COMMAND_NOT_IMPLEMENTED; } @@ -131,6 +170,8 @@ void GomspaceDeviceHandler::fillCommandAndReplyMap() { this->insertInCommandMap(GOMSPACE::GNDWDT_RESET); this->insertInCommandMap(GOMSPACE::PRINT_SWITCH_V_I); this->insertInCommandMap(GOMSPACE::PRINT_LATCHUPS); + insertInCommandMap(GOMSPACE::SAVE_TABLE_FILE); + insertInCommandMap(GOMSPACE::SAVE_TABLE_DEFAULT); } ReturnValue_t GomspaceDeviceHandler::scanForReply(const uint8_t* start, size_t remainingSize, diff --git a/mission/devices/devicedefinitions/GomspaceDefinitions.h b/mission/devices/devicedefinitions/GomspaceDefinitions.h index 5447bf42..52a66e84 100644 --- a/mission/devices/devicedefinitions/GomspaceDefinitions.h +++ b/mission/devices/devicedefinitions/GomspaceDefinitions.h @@ -18,6 +18,11 @@ namespace GOMSPACE { +struct TableInfo { + uint8_t sourceTable; + uint8_t targetTable; +}; + enum SpecialRequestTypes { DEFAULT_COM_IF, GET_PDU_HK, @@ -25,7 +30,8 @@ enum SpecialRequestTypes { GET_ACU_HK, GET_ACU_CONFIG, GET_P60DOCK_HK, - GET_P60DOCK_CONFIG + GET_P60DOCK_CONFIG, + SAVE_TABLE }; enum CspPorts : uint8_t { @@ -53,14 +59,17 @@ static const uint8_t P60_PORT_GNDWDT_RESET = 9; * Device commands are derived from the rparam.h of the gomspace lib.. * IDs above 50 are reserved for device specific commands. */ +static const DeviceCommandId_t PARAM_GET = 0; //!< [EXPORT] : [COMMAND] static const DeviceCommandId_t PING = 1; //!< [EXPORT] : [COMMAND] static const DeviceCommandId_t NONE = 2; // Set when no command is pending static const DeviceCommandId_t REBOOT = 4; //!< [EXPORT] : [COMMAND] static const DeviceCommandId_t GNDWDT_RESET = 9; //!< [EXPORT] : [COMMAND] -static const DeviceCommandId_t PARAM_GET = 0; //!< [EXPORT] : [COMMAND] -static const DeviceCommandId_t PARAM_SET = 255; //!< [EXPORT] : [COMMAND] static const DeviceCommandId_t REQUEST_HK_TABLE = 16; //!< [EXPORT] : [COMMAND] static const DeviceCommandId_t REQUEST_CONFIG_TABLE = 17; //!< [EXPORT] : [COMMAND] +static const DeviceCommandId_t SAVE_TABLE_FILE = 18; +static const DeviceCommandId_t SAVE_TABLE_DEFAULT = 19; +static const DeviceCommandId_t PARAM_SET = 255; //!< [EXPORT] : [COMMAND] + // Not implemented yet // static const DeviceCommandId_t REQUEST_CALIB_TABLE = 18; //!< [EXPORT] : [COMMAND] //! [EXPORT] : [COMMAND] Print switch states, voltages and currents to the console diff --git a/tmtc b/tmtc index 05dd1738..cc31a756 160000 --- a/tmtc +++ b/tmtc @@ -1 +1 @@ -Subproject commit 05dd17386070b9f333977f1f2b9f5651f9053b65 +Subproject commit cc31a756856623ecc2eec80e155557ac208f10a2 From 3d71cce30b2f317c5449e359928a00f6fba298ae Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 1 Sep 2022 11:04:24 +0200 Subject: [PATCH 37/52] bugfix --- fsfw | 2 +- mission/devices/GomspaceDeviceHandler.cpp | 10 ++++++---- mission/devices/GomspaceDeviceHandler.h | 2 +- 3 files changed, 8 insertions(+), 6 deletions(-) diff --git a/fsfw b/fsfw index 496dac89..cf8fe7ea 160000 --- a/fsfw +++ b/fsfw @@ -1 +1 @@ -Subproject commit 496dac89e4c094e03578f40d8651485f7f378cb2 +Subproject commit cf8fe7ea728bea077b9936bcf0db96845bc6419e diff --git a/mission/devices/GomspaceDeviceHandler.cpp b/mission/devices/GomspaceDeviceHandler.cpp index aa4459b4..ec0001ac 100644 --- a/mission/devices/GomspaceDeviceHandler.cpp +++ b/mission/devices/GomspaceDeviceHandler.cpp @@ -44,6 +44,8 @@ ReturnValue_t GomspaceDeviceHandler::buildTransitionDeviceCommand(DeviceCommandI ReturnValue_t GomspaceDeviceHandler::buildCommandFromCommand(DeviceCommandId_t deviceCommand, const uint8_t* commandData, size_t commandDataLen) { + auto* cspCookie = dynamic_cast(comCookie); + cspCookie->setRequest(SpecialRequestTypes::DEFAULT_COM_IF, 0); ReturnValue_t result = childCommandHook(deviceCommand, commandData, commandDataLen); switch (deviceCommand) { case (GOMSPACE::PING): { @@ -93,7 +95,7 @@ ReturnValue_t GomspaceDeviceHandler::buildCommandFromCommand(DeviceCommandId_t d reqType = SpecialRequestTypes::GET_P60DOCK_HK; } result = generateRequestFullTableCmd(reqType, GOMSPACE::TableIds::HK, tableCfg.hkTableSize, - deviceCommand); + deviceCommand, cspCookie); if (result != returnvalue::OK) { return result; } @@ -109,7 +111,7 @@ ReturnValue_t GomspaceDeviceHandler::buildCommandFromCommand(DeviceCommandId_t d reqType = SpecialRequestTypes::GET_P60DOCK_CONFIG; } result = generateRequestFullTableCmd(reqType, GOMSPACE::TableIds::CONFIG, - tableCfg.cfgTableSize, deviceCommand); + tableCfg.cfgTableSize, deviceCommand, cspCookie); if (result != returnvalue::OK) { return result; } @@ -506,13 +508,13 @@ ReturnValue_t GomspaceDeviceHandler::generateResetWatchdogCmd() { ReturnValue_t GomspaceDeviceHandler::generateRequestFullTableCmd(SpecialRequestTypes reqType, uint8_t tableId, uint16_t tableReplySize, - DeviceCommandId_t id) { + DeviceCommandId_t id, + CspCookie* cspCookie) { uint16_t querySize = tableReplySize; if (reqType == SpecialRequestTypes::DEFAULT_COM_IF) { sif::warning << "Default communication for table requests not implemented anymore" << std::endl; return returnvalue::FAILED; } - auto* cspCookie = dynamic_cast(comCookie); cspCookie->setRequest(reqType, tableReplySize); cspCookie->setCspPort(CspPorts::P60_PORT_RPARAM_ENUM); rememberRequestedSize = querySize; diff --git a/mission/devices/GomspaceDeviceHandler.h b/mission/devices/GomspaceDeviceHandler.h index af847b8a..da4fc136 100644 --- a/mission/devices/GomspaceDeviceHandler.h +++ b/mission/devices/GomspaceDeviceHandler.h @@ -84,7 +84,7 @@ class GomspaceDeviceHandler : public DeviceHandlerBase { */ virtual ReturnValue_t generateRequestFullTableCmd(GOMSPACE::SpecialRequestTypes reqType, uint8_t tableId, uint16_t tableSize, - DeviceCommandId_t id); + DeviceCommandId_t id, CspCookie* cspCookie); /** * This command handles printing the HK table to the console. This is useful for debugging From 27129f3b66971aaac83dd844d9565e54a61e3986 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 1 Sep 2022 11:18:55 +0200 Subject: [PATCH 38/52] increase number of allowed consecutive commands for service 8 --- mission/core/GenericFactory.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/mission/core/GenericFactory.cpp b/mission/core/GenericFactory.cpp index 0a9be3f9..bbd2bb98 100644 --- a/mission/core/GenericFactory.cpp +++ b/mission/core/GenericFactory.cpp @@ -96,7 +96,7 @@ void ObjectFactory::produceGenericObjects(HealthTableIF** healthTable_) { PsbParams(objects::PUS_SERVICE_5_EVENT_REPORTING, apid::EIVE_OBSW, pus::PUS_SERVICE_5), 15, 45); new Service8FunctionManagement(objects::PUS_SERVICE_8_FUNCTION_MGMT, apid::EIVE_OBSW, - pus::PUS_SERVICE_8, 3, 60); + pus::PUS_SERVICE_8, 16, 60); new Service9TimeManagement( PsbParams(objects::PUS_SERVICE_9_TIME_MGMT, apid::EIVE_OBSW, pus::PUS_SERVICE_9)); From 4e92fd442117ab2cb039c65937ac0f02c8a8e262 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 1 Sep 2022 15:23:39 +0200 Subject: [PATCH 39/52] some simplifications --- mission/devices/GomspaceDeviceHandler.cpp | 95 ++++++++++++------- mission/devices/GomspaceDeviceHandler.h | 19 +++- .../devicedefinitions/GomspaceDefinitions.h | 4 +- tmtc | 2 +- 4 files changed, 80 insertions(+), 40 deletions(-) diff --git a/mission/devices/GomspaceDeviceHandler.cpp b/mission/devices/GomspaceDeviceHandler.cpp index ec0001ac..5ae676e4 100644 --- a/mission/devices/GomspaceDeviceHandler.cpp +++ b/mission/devices/GomspaceDeviceHandler.cpp @@ -86,48 +86,41 @@ ReturnValue_t GomspaceDeviceHandler::buildCommandFromCommand(DeviceCommandId_t d break; } case (GOMSPACE::REQUEST_HK_TABLE): { - auto reqType = SpecialRequestTypes::DEFAULT_COM_IF; - if (getObjectId() == objects::PDU1_HANDLER or getObjectId() == objects::PDU2_HANDLER) { - reqType = SpecialRequestTypes::GET_PDU_HK; - } else if (getObjectId() == objects::ACU_HANDLER) { - reqType = SpecialRequestTypes::GET_ACU_HK; - } else if (getObjectId() == objects::P60DOCK_HANDLER) { - reqType = SpecialRequestTypes::GET_P60DOCK_HK; + DeviceType devType; + if(getDevType(devType) != returnvalue::OK) { + return returnvalue::FAILED; } - result = generateRequestFullTableCmd(reqType, GOMSPACE::TableIds::HK, tableCfg.hkTableSize, - deviceCommand, cspCookie); + result = + generateRequestFullHkTableCmd(devType, tableCfg.hkTableSize, deviceCommand, cspCookie); if (result != returnvalue::OK) { return result; } break; } case (GOMSPACE::REQUEST_CONFIG_TABLE): { - auto reqType = SpecialRequestTypes::DEFAULT_COM_IF; - if (getObjectId() == objects::PDU1_HANDLER or getObjectId() == objects::PDU2_HANDLER) { - reqType = SpecialRequestTypes::GET_PDU_CONFIG; - } else if (getObjectId() == objects::ACU_HANDLER) { - reqType = SpecialRequestTypes::GET_ACU_CONFIG; - } else if (getObjectId() == objects::P60DOCK_HANDLER) { - reqType = SpecialRequestTypes::GET_P60DOCK_CONFIG; + DeviceType devType; + if(getDevType(devType) != returnvalue::OK) { + return returnvalue::FAILED; } - result = generateRequestFullTableCmd(reqType, GOMSPACE::TableIds::CONFIG, - tableCfg.cfgTableSize, deviceCommand, cspCookie); + result = generateRequestFullCfgTableCmd(devType, tableCfg.cfgTableSize, + deviceCommand, cspCookie); if (result != returnvalue::OK) { return result; } break; } case (GOMSPACE::SAVE_TABLE_FILE): { - if(commandDataLen > 2) { + if (commandDataLen > 2) { return HasActionsIF::INVALID_PARAMETERS; } auto* info = reinterpret_cast(cspPacket); - if(commandData[0] != 0 and commandData[0] != 1 and commandData[0] != 2 and commandData[0] != 4) { + if (commandData[0] != 0 and commandData[0] != 1 and commandData[0] != 2 and + commandData[0] != 4) { return HasActionsIF::INVALID_PARAMETERS; } info->sourceTable = commandData[0]; if (info->sourceTable != 4) { - if(commandDataLen == 2) { + if (commandDataLen == 2) { info->targetTable = commandData[1]; } else { info->targetTable = info->sourceTable; @@ -142,11 +135,11 @@ ReturnValue_t GomspaceDeviceHandler::buildCommandFromCommand(DeviceCommandId_t d break; } case (GOMSPACE::SAVE_TABLE_DEFAULT): { - if(commandDataLen != 1) { + if (commandDataLen != 1) { return HasActionsIF::INVALID_PARAMETERS; } auto* info = reinterpret_cast(cspPacket); - if(commandData[0] != 0 and commandData[0] != 1 and commandData[0] != 2) { + if (commandData[0] != 0 and commandData[0] != 1 and commandData[0] != 2) { return HasActionsIF::INVALID_PARAMETERS; } info->sourceTable = commandData[0]; @@ -486,6 +479,19 @@ bool GomspaceDeviceHandler::validTableId(uint8_t id) { return false; } +ReturnValue_t GomspaceDeviceHandler::getDevType(GOMSPACE::DeviceType& type) const { + if (getObjectId() == objects::PDU1_HANDLER or getObjectId() == objects::PDU2_HANDLER) { + type = DeviceType::PDU; + } else if (getObjectId() == objects::ACU_HANDLER) { + type = DeviceType::ACU; + } else if (getObjectId() == objects::P60DOCK_HANDLER) { + type = DeviceType::P60DOCK; + } else { + return returnvalue::FAILED; + } + return returnvalue::OK; +} + ReturnValue_t GomspaceDeviceHandler::generateResetWatchdogCmd() { WatchdogResetCommand watchdogResetCommand; size_t cspPacketLen = 0; @@ -505,19 +511,40 @@ ReturnValue_t GomspaceDeviceHandler::generateResetWatchdogCmd() { return returnvalue::OK; } -ReturnValue_t GomspaceDeviceHandler::generateRequestFullTableCmd(SpecialRequestTypes reqType, - uint8_t tableId, - uint16_t tableReplySize, - DeviceCommandId_t id, - CspCookie* cspCookie) { - uint16_t querySize = tableReplySize; - if (reqType == SpecialRequestTypes::DEFAULT_COM_IF) { - sif::warning << "Default communication for table requests not implemented anymore" << std::endl; - return returnvalue::FAILED; +ReturnValue_t GomspaceDeviceHandler::generateRequestFullHkTableCmd(DeviceType dev, + uint16_t tableReplySize, + DeviceCommandId_t id, + CspCookie* cspCookie) { + if (dev == DeviceType::ACU) { + cspCookie->setRequest(SpecialRequestTypes::GET_ACU_HK, tableReplySize); + } else if (dev == DeviceType::P60DOCK) { + cspCookie->setRequest(SpecialRequestTypes::GET_P60DOCK_HK, tableReplySize); + } else if (dev == DeviceType::PDU) { + cspCookie->setRequest(SpecialRequestTypes::GET_PDU_HK, tableReplySize); } - cspCookie->setRequest(reqType, tableReplySize); cspCookie->setCspPort(CspPorts::P60_PORT_RPARAM_ENUM); - rememberRequestedSize = querySize; + rememberRequestedSize = tableReplySize; + rememberCommandId = id; + return returnvalue::OK; +} + +ReturnValue_t GomspaceDeviceHandler::generateRequestFullCfgTableCmd(DeviceType dev, + uint16_t tableReplySize, + DeviceCommandId_t id, + CspCookie* cspCookie) { + if (dev == DeviceType::ACU) { + cspCookie->setRequest(SpecialRequestTypes::GET_ACU_CONFIG, tableReplySize); + } else if (dev == DeviceType::P60DOCK) { + cspCookie->setRequest(SpecialRequestTypes::GET_P60DOCK_CONFIG, tableReplySize); + } else if (dev == DeviceType::PDU) { + cspCookie->setRequest(SpecialRequestTypes::GET_PDU_CONFIG, tableReplySize); + } + cspCookie->setCspPort(CspPorts::P60_PORT_RPARAM_ENUM); + // Unfortunately, this does not work.. + // cspPacket[0] = defaultTable; + // rawPacket = cspPacket; + // rawPacketLen = 1; + rememberRequestedSize = tableReplySize; rememberCommandId = id; return returnvalue::OK; } diff --git a/mission/devices/GomspaceDeviceHandler.h b/mission/devices/GomspaceDeviceHandler.h index da4fc136..528cd2dd 100644 --- a/mission/devices/GomspaceDeviceHandler.h +++ b/mission/devices/GomspaceDeviceHandler.h @@ -82,10 +82,21 @@ class GomspaceDeviceHandler : public DeviceHandlerBase { * @brief The command to generate a request to receive the full housekeeping table is device * specific. Thus the child has to build this command. */ - virtual ReturnValue_t generateRequestFullTableCmd(GOMSPACE::SpecialRequestTypes reqType, - uint8_t tableId, uint16_t tableSize, - DeviceCommandId_t id, CspCookie* cspCookie); - + ReturnValue_t generateRequestFullHkTableCmd(GOMSPACE::DeviceType devType, uint16_t tableSize, + DeviceCommandId_t id, CspCookie *cspCookie); + /** + * Unfortunately, it was not possible to specify the table ID (e.g. request table from + * default store) + * @param devType + * @param tableSize + * @param id + * @param cspCookie + * @return + */ + ReturnValue_t generateRequestFullCfgTableCmd(GOMSPACE::DeviceType devType, + uint16_t tableSize, DeviceCommandId_t id, + CspCookie *cspCookie); + ReturnValue_t getDevType(GOMSPACE::DeviceType& type) const; /** * This command handles printing the HK table to the console. This is useful for debugging * purposes diff --git a/mission/devices/devicedefinitions/GomspaceDefinitions.h b/mission/devices/devicedefinitions/GomspaceDefinitions.h index 52a66e84..4a356aa9 100644 --- a/mission/devices/devicedefinitions/GomspaceDefinitions.h +++ b/mission/devices/devicedefinitions/GomspaceDefinitions.h @@ -23,6 +23,8 @@ struct TableInfo { uint8_t targetTable; }; +enum DeviceType { PDU, ACU, P60DOCK }; + enum SpecialRequestTypes { DEFAULT_COM_IF, GET_PDU_HK, @@ -68,7 +70,7 @@ static const DeviceCommandId_t REQUEST_HK_TABLE = 16; //!< [EXPORT] : [COMM static const DeviceCommandId_t REQUEST_CONFIG_TABLE = 17; //!< [EXPORT] : [COMMAND] static const DeviceCommandId_t SAVE_TABLE_FILE = 18; static const DeviceCommandId_t SAVE_TABLE_DEFAULT = 19; -static const DeviceCommandId_t PARAM_SET = 255; //!< [EXPORT] : [COMMAND] +static const DeviceCommandId_t PARAM_SET = 255; //!< [EXPORT] : [COMMAND] // Not implemented yet // static const DeviceCommandId_t REQUEST_CALIB_TABLE = 18; //!< [EXPORT] : [COMMAND] diff --git a/tmtc b/tmtc index cc31a756..abecbe44 160000 --- a/tmtc +++ b/tmtc @@ -1 +1 @@ -Subproject commit cc31a756856623ecc2eec80e155557ac208f10a2 +Subproject commit abecbe44016313f1b51a29d1358fccb9f541d542 From 87f430fdee6e95ad9d2ed8e8eb9a14bcc710f629 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 1 Sep 2022 16:58:51 +0200 Subject: [PATCH 40/52] some bugfixes for save table feature --- mission/devices/GomspaceDeviceHandler.cpp | 4 ++++ tmtc | 2 +- 2 files changed, 5 insertions(+), 1 deletion(-) diff --git a/mission/devices/GomspaceDeviceHandler.cpp b/mission/devices/GomspaceDeviceHandler.cpp index 5ae676e4..197195e6 100644 --- a/mission/devices/GomspaceDeviceHandler.cpp +++ b/mission/devices/GomspaceDeviceHandler.cpp @@ -131,6 +131,8 @@ ReturnValue_t GomspaceDeviceHandler::buildCommandFromCommand(DeviceCommandId_t d } rawPacket = cspPacket; rawPacketLen = sizeof(GOMSPACE::TableInfo); + cspCookie->setCspPort(CspPorts::P60_PORT_RPARAM_ENUM); + cspCookie->setRequest(SpecialRequestTypes::SAVE_TABLE, 0); rememberCommandId = deviceCommand; break; } @@ -146,6 +148,8 @@ ReturnValue_t GomspaceDeviceHandler::buildCommandFromCommand(DeviceCommandId_t d info->targetTable = info->sourceTable + 4; rawPacket = cspPacket; rawPacketLen = sizeof(GOMSPACE::TableInfo); + cspCookie->setCspPort(CspPorts::P60_PORT_RPARAM_ENUM); + cspCookie->setRequest(SpecialRequestTypes::SAVE_TABLE, 0); rememberCommandId = deviceCommand; break; } diff --git a/tmtc b/tmtc index abecbe44..e162e5c5 160000 --- a/tmtc +++ b/tmtc @@ -1 +1 @@ -Subproject commit abecbe44016313f1b51a29d1358fccb9f541d542 +Subproject commit e162e5c51b0bf08b2018e4169a07eed1e4f7eacb From 6ae0249fdc2367577e00afecb4e660f2e0474987 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 1 Sep 2022 17:31:43 +0200 Subject: [PATCH 41/52] load table command --- linux/csp/CspComIF.cpp | 10 ++++++++++ mission/csp/CspCookie.cpp | 3 ++- mission/devices/GomspaceDeviceHandler.cpp | 15 +++++++++++++++ .../devicedefinitions/GomspaceDefinitions.h | 6 ++++-- tmtc | 2 +- 5 files changed, 32 insertions(+), 4 deletions(-) diff --git a/linux/csp/CspComIF.cpp b/linux/csp/CspComIF.cpp index b52bcfa2..5d8c3fd6 100644 --- a/linux/csp/CspComIF.cpp +++ b/linux/csp/CspComIF.cpp @@ -176,6 +176,16 @@ ReturnValue_t CspComIF::sendMessage(CookieIF* cookie, const uint8_t* sendData, s if (result != 0) { return returnvalue::FAILED; } + } else if(req == GOMSPACE::SpecialRequestTypes::LOAD_TABLE) { + if(sendLen < 2) { + return returnvalue::FAILED; + } + const TableInfo* tableInfo = reinterpret_cast(sendData); + int result = gs_rparam_load(cspAddress, cspCookie->getTimeout(), tableInfo->sourceTable, + tableInfo->targetTable); + if (result != 0) { + return returnvalue::FAILED; + } } } else { /* No CSP fixed port was selected. Send data to the specified port and diff --git a/mission/csp/CspCookie.cpp b/mission/csp/CspCookie.cpp index f6ab1eed..228a8058 100644 --- a/mission/csp/CspCookie.cpp +++ b/mission/csp/CspCookie.cpp @@ -1,10 +1,11 @@ #include "CspCookie.h" +using namespace GOMSPACE; CspCookie::CspCookie(uint16_t maxReplyLength_, uint8_t cspAddress_, uint32_t timeoutMs) : maxReplyLength(maxReplyLength_), cspAddress(cspAddress_), timeoutMs(timeoutMs), - reqType(GOMSPACE::DEFAULT_COM_IF) {} + reqType(SpecialRequestTypes::DEFAULT_COM_IF) {} CspCookie::~CspCookie() {} diff --git a/mission/devices/GomspaceDeviceHandler.cpp b/mission/devices/GomspaceDeviceHandler.cpp index 197195e6..ebfcf2a9 100644 --- a/mission/devices/GomspaceDeviceHandler.cpp +++ b/mission/devices/GomspaceDeviceHandler.cpp @@ -109,6 +109,20 @@ ReturnValue_t GomspaceDeviceHandler::buildCommandFromCommand(DeviceCommandId_t d } break; } + case (GOMSPACE::LOAD_TABLE): { + if (commandDataLen != 2) { + return HasActionsIF::INVALID_PARAMETERS; + } + auto* info = reinterpret_cast(cspPacket); + info->sourceTable = commandData[0]; + info->targetTable = commandData[1]; + rawPacket = cspPacket; + rawPacketLen = sizeof(GOMSPACE::TableInfo); + cspCookie->setCspPort(CspPorts::P60_PORT_RPARAM_ENUM); + cspCookie->setRequest(SpecialRequestTypes::LOAD_TABLE, 0); + rememberCommandId = deviceCommand; + break; + } case (GOMSPACE::SAVE_TABLE_FILE): { if (commandDataLen > 2) { return HasActionsIF::INVALID_PARAMETERS; @@ -171,6 +185,7 @@ void GomspaceDeviceHandler::fillCommandAndReplyMap() { this->insertInCommandMap(GOMSPACE::PRINT_LATCHUPS); insertInCommandMap(GOMSPACE::SAVE_TABLE_FILE); insertInCommandMap(GOMSPACE::SAVE_TABLE_DEFAULT); + insertInCommandMap(GOMSPACE::LOAD_TABLE); } ReturnValue_t GomspaceDeviceHandler::scanForReply(const uint8_t* start, size_t remainingSize, diff --git a/mission/devices/devicedefinitions/GomspaceDefinitions.h b/mission/devices/devicedefinitions/GomspaceDefinitions.h index 4a356aa9..cab71362 100644 --- a/mission/devices/devicedefinitions/GomspaceDefinitions.h +++ b/mission/devices/devicedefinitions/GomspaceDefinitions.h @@ -25,7 +25,7 @@ struct TableInfo { enum DeviceType { PDU, ACU, P60DOCK }; -enum SpecialRequestTypes { +enum class SpecialRequestTypes { DEFAULT_COM_IF, GET_PDU_HK, GET_PDU_CONFIG, @@ -33,7 +33,8 @@ enum SpecialRequestTypes { GET_ACU_CONFIG, GET_P60DOCK_HK, GET_P60DOCK_CONFIG, - SAVE_TABLE + SAVE_TABLE, + LOAD_TABLE }; enum CspPorts : uint8_t { @@ -70,6 +71,7 @@ static const DeviceCommandId_t REQUEST_HK_TABLE = 16; //!< [EXPORT] : [COMM static const DeviceCommandId_t REQUEST_CONFIG_TABLE = 17; //!< [EXPORT] : [COMMAND] static const DeviceCommandId_t SAVE_TABLE_FILE = 18; static const DeviceCommandId_t SAVE_TABLE_DEFAULT = 19; +static const DeviceCommandId_t LOAD_TABLE = 20; static const DeviceCommandId_t PARAM_SET = 255; //!< [EXPORT] : [COMMAND] // Not implemented yet diff --git a/tmtc b/tmtc index e162e5c5..6085c654 160000 --- a/tmtc +++ b/tmtc @@ -1 +1 @@ -Subproject commit e162e5c51b0bf08b2018e4169a07eed1e4f7eacb +Subproject commit 6085c6543c0beb24cc427fae16d1fa2897a2fe5a From 923adbdb1a98ea46fbdd6cb643d61f42dffe1b23 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 1 Sep 2022 17:40:21 +0200 Subject: [PATCH 42/52] bump tmtc --- tmtc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tmtc b/tmtc index 6085c654..f366b1c3 160000 --- a/tmtc +++ b/tmtc @@ -1 +1 @@ -Subproject commit 6085c6543c0beb24cc427fae16d1fa2897a2fe5a +Subproject commit f366b1c3f61f9d19bc0a5829caecdd2ae4285601 From a76816f05536ceb66007ac09eabc6ce787942bf9 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Fri, 2 Sep 2022 13:16:56 +0200 Subject: [PATCH 43/52] update changelog --- CHANGELOG.md | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index ce393fea..30890da2 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -17,6 +17,10 @@ list yields a list of all related PRs for each release. GomSpace TM tables - Add API to retrieve GomSpace device parameter tables PR: https://egit.irs.uni-stuttgart.de/eive/eive-obsw/pulls/287 +- Add API to save and load GomSpace config tables + PR: https://egit.irs.uni-stuttgart.de/eive/eive-obsw/pulls/293 +- Increase number of allowed consescutive action commands from 3 to 16 + PR: https://egit.irs.uni-stuttgart.de/eive/eive-obsw/pulls/294 # [v1.13.0] 24.08.2022 From e381b00a131d1adb2307371138c43a59b3f6d77b Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Fri, 2 Sep 2022 13:19:55 +0200 Subject: [PATCH 44/52] re-point tmtc --- tmtc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tmtc b/tmtc index f366b1c3..603b7e85 160000 --- a/tmtc +++ b/tmtc @@ -1 +1 @@ -Subproject commit f366b1c3f61f9d19bc0a5829caecdd2ae4285601 +Subproject commit 603b7e8574d74ba60692115133cde3cd8b8bd423 From f7196a47916799154525abc8374db7c6fb9972be Mon Sep 17 00:00:00 2001 From: Ulrich Mohr Date: Wed, 14 Sep 2022 12:47:15 +0200 Subject: [PATCH 45/52] updated to new fsfw --- fsfw | 2 +- linux/fsfwconfig/FSFWConfig.h.in | 4 ++++ linux/obc/PtmeConfig.h | 1 + mission/memory/NVMParameterBase.h | 1 + 4 files changed, 7 insertions(+), 1 deletion(-) diff --git a/fsfw b/fsfw index cf8fe7ea..bd594123 160000 --- a/fsfw +++ b/fsfw @@ -1 +1 @@ -Subproject commit cf8fe7ea728bea077b9936bcf0db96845bc6419e +Subproject commit bd594123a2a1579f505071f9c76d1249433c070e diff --git a/linux/fsfwconfig/FSFWConfig.h.in b/linux/fsfwconfig/FSFWConfig.h.in index 072c47c8..25772ce7 100644 --- a/linux/fsfwconfig/FSFWConfig.h.in +++ b/linux/fsfwconfig/FSFWConfig.h.in @@ -4,6 +4,10 @@ #include #include +// It is assumed the user has a subsystem and class ID list in some user header files. +#include "events/subsystemIdRanges.h" +#include "returnvalues/classIds.h" + //! Used to determine whether C++ ostreams are used which can increase //! the binary size significantly. If this is disabled, //! the C stdio functions can be used alternatively diff --git a/linux/obc/PtmeConfig.h b/linux/obc/PtmeConfig.h index 06aff2e2..aa5c663a 100644 --- a/linux/obc/PtmeConfig.h +++ b/linux/obc/PtmeConfig.h @@ -4,6 +4,7 @@ #include "AxiPtmeConfig.h" #include "fsfw/objectmanager/SystemObject.h" #include "fsfw/returnvalues/returnvalue.h" +#include "returnvalues/classIds.h" #include "linux/obc/PtmeConfig.h" /** diff --git a/mission/memory/NVMParameterBase.h b/mission/memory/NVMParameterBase.h index 4f644fcc..dd02fffe 100644 --- a/mission/memory/NVMParameterBase.h +++ b/mission/memory/NVMParameterBase.h @@ -6,6 +6,7 @@ #include #include "fsfw/returnvalues/returnvalue.h" +#include "returnvalues/classIds.h" class NVMParameterBase { public: From ff3bc7da73fbff1e8063428d117344c10cabe0ec Mon Sep 17 00:00:00 2001 From: Ulrich Mohr Date: Wed, 14 Sep 2022 13:18:50 +0200 Subject: [PATCH 46/52] fix for new fsfw --- mission/devices/devicedefinitions/RwDefinitions.h | 1 + 1 file changed, 1 insertion(+) diff --git a/mission/devices/devicedefinitions/RwDefinitions.h b/mission/devices/devicedefinitions/RwDefinitions.h index d6a43622..2923b41b 100644 --- a/mission/devices/devicedefinitions/RwDefinitions.h +++ b/mission/devices/devicedefinitions/RwDefinitions.h @@ -5,6 +5,7 @@ #include #include +#include "events/subsystemIdRanges.h" #include "objects/systemObjectList.h" namespace RwDefinitions { From 4d93368b4c744278f8229f2abc0504ff9b089c9f Mon Sep 17 00:00:00 2001 From: Ulrich Mohr Date: Wed, 14 Sep 2022 13:44:43 +0200 Subject: [PATCH 47/52] fixed for new fsfw --- mission/devices/HeaterHandler.h | 2 ++ mission/devices/IMTQHandler.h | 3 +++ mission/devices/RwHandler.h | 3 +++ mission/devices/SolarArrayDeploymentHandler.h | 3 +++ mission/devices/SusHandler.h | 3 +++ mission/devices/SyrlinksHkHandler.h | 1 + mission/system/SusAssembly.h | 2 ++ mission/system/TcsBoardAssembly.h | 3 +++ 8 files changed, 20 insertions(+) diff --git a/mission/devices/HeaterHandler.h b/mission/devices/HeaterHandler.h index 75badf91..b63f4dfe 100644 --- a/mission/devices/HeaterHandler.h +++ b/mission/devices/HeaterHandler.h @@ -18,6 +18,8 @@ #include #include "devices/heaterSwitcherList.h" +#include "events/subsystemIdRanges.h" +#include "returnvalues/classIds.h" class PowerSwitchIF; class HealthTableIF; diff --git a/mission/devices/IMTQHandler.h b/mission/devices/IMTQHandler.h index 481d554c..5102b2e0 100644 --- a/mission/devices/IMTQHandler.h +++ b/mission/devices/IMTQHandler.h @@ -5,6 +5,9 @@ #include #include +#include "events/subsystemIdRanges.h" +#include "returnvalues/classIds.h" + /** * @brief This is the device handler for the ISIS Magnetorquer iMTQ. * diff --git a/mission/devices/RwHandler.h b/mission/devices/RwHandler.h index 008d5746..f9f66bd5 100644 --- a/mission/devices/RwHandler.h +++ b/mission/devices/RwHandler.h @@ -6,6 +6,9 @@ #include #include +#include "events/subsystemIdRanges.h" +#include "returnvalues/classIds.h" + class GpioIF; /** diff --git a/mission/devices/SolarArrayDeploymentHandler.h b/mission/devices/SolarArrayDeploymentHandler.h index 691bdf77..89be12a6 100644 --- a/mission/devices/SolarArrayDeploymentHandler.h +++ b/mission/devices/SolarArrayDeploymentHandler.h @@ -14,6 +14,9 @@ #include +#include "events/subsystemIdRanges.h" +#include "returnvalues/classIds.h" + /** * @brief This class is used to control the solar array deployment. * diff --git a/mission/devices/SusHandler.h b/mission/devices/SusHandler.h index c1fc08c4..94673c40 100644 --- a/mission/devices/SusHandler.h +++ b/mission/devices/SusHandler.h @@ -7,6 +7,9 @@ #include "fsfw/globalfunctions/PeriodicOperationDivider.h" #include "mission/devices/max1227.h" +#include "events/subsystemIdRanges.h" +#include "returnvalues/classIds.h" + /** * @brief This is the device handler class for the SUS sensor based on the MAX1227 ADC. * diff --git a/mission/devices/SyrlinksHkHandler.h b/mission/devices/SyrlinksHkHandler.h index 44b154f6..a3bf0a67 100644 --- a/mission/devices/SyrlinksHkHandler.h +++ b/mission/devices/SyrlinksHkHandler.h @@ -8,6 +8,7 @@ #include "fsfw/timemanager/Countdown.h" #include "fsfw_hal/linux/gpio/Gpio.h" #include "mission/devices/devicedefinitions/SyrlinksDefinitions.h" +#include "returnvalues/classIds.h" /** * @brief This is the device handler for the syrlinks transceiver. It handles the command diff --git a/mission/system/SusAssembly.h b/mission/system/SusAssembly.h index b9216072..3cce9d86 100644 --- a/mission/system/SusAssembly.h +++ b/mission/system/SusAssembly.h @@ -5,6 +5,8 @@ #include #include "DualLaneAssemblyBase.h" +#include "events/subsystemIdRanges.h" +#include "returnvalues/classIds.h" struct SusAssHelper { public: diff --git a/mission/system/TcsBoardAssembly.h b/mission/system/TcsBoardAssembly.h index 10b451b2..6042abaa 100644 --- a/mission/system/TcsBoardAssembly.h +++ b/mission/system/TcsBoardAssembly.h @@ -5,6 +5,9 @@ #include #include +#include "events/subsystemIdRanges.h" +#include "returnvalues/classIds.h" + struct TcsBoardHelper { TcsBoardHelper(std::array, 16> rtdInfos) : rtdInfos(std::move(rtdInfos)) {} From 63e4448178d48b4047771991757834f2af24eeae Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Wed, 14 Sep 2022 16:47:58 +0200 Subject: [PATCH 48/52] run afmt, bump proj files --- linux/csp/CspComIF.cpp | 12 ++++++------ misc/eclipse/.cproject | 24 +++++++++++------------ mission/devices/GomspaceDeviceHandler.cpp | 8 ++++---- mission/devices/GomspaceDeviceHandler.h | 7 +++---- 4 files changed, 25 insertions(+), 26 deletions(-) diff --git a/linux/csp/CspComIF.cpp b/linux/csp/CspComIF.cpp index 5d8c3fd6..ea508a31 100644 --- a/linux/csp/CspComIF.cpp +++ b/linux/csp/CspComIF.cpp @@ -166,23 +166,23 @@ ReturnValue_t CspComIF::sendMessage(CookieIF* cookie, const uint8_t* sendData, s if (result != 0) { return returnvalue::FAILED; } - } else if(req == GOMSPACE::SpecialRequestTypes::SAVE_TABLE) { - if(sendLen < 2) { + } else if (req == GOMSPACE::SpecialRequestTypes::SAVE_TABLE) { + if (sendLen < 2) { return returnvalue::FAILED; } const TableInfo* tableInfo = reinterpret_cast(sendData); int result = gs_rparam_save(cspAddress, cspCookie->getTimeout(), tableInfo->sourceTable, - tableInfo->targetTable); + tableInfo->targetTable); if (result != 0) { return returnvalue::FAILED; } - } else if(req == GOMSPACE::SpecialRequestTypes::LOAD_TABLE) { - if(sendLen < 2) { + } else if (req == GOMSPACE::SpecialRequestTypes::LOAD_TABLE) { + if (sendLen < 2) { return returnvalue::FAILED; } const TableInfo* tableInfo = reinterpret_cast(sendData); int result = gs_rparam_load(cspAddress, cspCookie->getTimeout(), tableInfo->sourceTable, - tableInfo->targetTable); + tableInfo->targetTable); if (result != 0) { return returnvalue::FAILED; } diff --git a/misc/eclipse/.cproject b/misc/eclipse/.cproject index d30ef1ec..19c18545 100644 --- a/misc/eclipse/.cproject +++ b/misc/eclipse/.cproject @@ -57,7 +57,7 @@ - + @@ -119,7 +119,7 @@ - + @@ -187,7 +187,7 @@ - + @@ -255,7 +255,7 @@ - + @@ -418,7 +418,7 @@ - + @@ -580,7 +580,7 @@ - + @@ -750,7 +750,7 @@ - + @@ -917,7 +917,7 @@ - + @@ -1084,7 +1084,7 @@ - + @@ -1149,7 +1149,7 @@ - + @@ -1317,7 +1317,7 @@ - + @@ -1386,7 +1386,7 @@ - + diff --git a/mission/devices/GomspaceDeviceHandler.cpp b/mission/devices/GomspaceDeviceHandler.cpp index ebfcf2a9..412f2e46 100644 --- a/mission/devices/GomspaceDeviceHandler.cpp +++ b/mission/devices/GomspaceDeviceHandler.cpp @@ -87,7 +87,7 @@ ReturnValue_t GomspaceDeviceHandler::buildCommandFromCommand(DeviceCommandId_t d } case (GOMSPACE::REQUEST_HK_TABLE): { DeviceType devType; - if(getDevType(devType) != returnvalue::OK) { + if (getDevType(devType) != returnvalue::OK) { return returnvalue::FAILED; } result = @@ -99,11 +99,11 @@ ReturnValue_t GomspaceDeviceHandler::buildCommandFromCommand(DeviceCommandId_t d } case (GOMSPACE::REQUEST_CONFIG_TABLE): { DeviceType devType; - if(getDevType(devType) != returnvalue::OK) { + if (getDevType(devType) != returnvalue::OK) { return returnvalue::FAILED; } - result = generateRequestFullCfgTableCmd(devType, tableCfg.cfgTableSize, - deviceCommand, cspCookie); + result = + generateRequestFullCfgTableCmd(devType, tableCfg.cfgTableSize, deviceCommand, cspCookie); if (result != returnvalue::OK) { return result; } diff --git a/mission/devices/GomspaceDeviceHandler.h b/mission/devices/GomspaceDeviceHandler.h index 528cd2dd..af9d3411 100644 --- a/mission/devices/GomspaceDeviceHandler.h +++ b/mission/devices/GomspaceDeviceHandler.h @@ -93,10 +93,9 @@ class GomspaceDeviceHandler : public DeviceHandlerBase { * @param cspCookie * @return */ - ReturnValue_t generateRequestFullCfgTableCmd(GOMSPACE::DeviceType devType, - uint16_t tableSize, DeviceCommandId_t id, - CspCookie *cspCookie); - ReturnValue_t getDevType(GOMSPACE::DeviceType& type) const; + ReturnValue_t generateRequestFullCfgTableCmd(GOMSPACE::DeviceType devType, uint16_t tableSize, + DeviceCommandId_t id, CspCookie *cspCookie); + ReturnValue_t getDevType(GOMSPACE::DeviceType &type) const; /** * This command handles printing the HK table to the console. This is useful for debugging * purposes From 94a70aa2fc9e114d5a98c11511fa173b10e57d80 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 15 Sep 2022 11:36:04 +0200 Subject: [PATCH 49/52] update .cproject file --- misc/eclipse/.cproject | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/misc/eclipse/.cproject b/misc/eclipse/.cproject index 19c18545..9bb5f401 100644 --- a/misc/eclipse/.cproject +++ b/misc/eclipse/.cproject @@ -1256,7 +1256,7 @@ - + @@ -1268,7 +1268,7 @@ - +