rad sensor change wip

mission/devices/SusHandler.cpp

@@ -0,0 +1,165 @@
+#include <fsfw/datapool/PoolReadGuard.h>
+#include <mission/devices/SusHandler.h>
+#include <OBSWConfig.h>
+
+SusHandler::SusHandler(object_id_t objectId, object_id_t comIF,
+		CookieIF * comCookie) :
+		DeviceHandlerBase(objectId, comIF, comCookie), dataset(
+				this) {
+	if (comCookie == NULL) {
+		sif::error << "SusHandler: Invalid com cookie" << std::endl;
+	}
+}
+
+SusHandler::~SusHandler() {
+}
+
+
+void SusHandler::doStartUp(){
+    if (internalState == InternalState::CONFIGURED) {
+#if OBSW_SWITCH_TO_NORMAL_MODE_AFTER_STARTUP == 1
+        setMode(MODE_NORMAL);
+#else
+        setMode(_MODE_TO_ON);
+#endif
+    }
+}
+
+void SusHandler::doShutDown(){
+    setMode(_MODE_POWER_DOWN);
+}
+
+ReturnValue_t SusHandler::buildNormalDeviceCommand(
+		DeviceCommandId_t * id) {
+
+    switch (communicationStep) {
+    case CommunicationStep::START_CONVERSION: {
+        *id = SUS::START_CONVERSION;
+        communicationStep = CommunicationStep::READ_CONVERSIONS;
+        break;
+    }
+    case CommunicationStep::READ_CONVERSIONS: {
+        *id = SUS::READ_CONVERSIONS;
+//        communicationStep = CommunicationStep::START_CONVERSION;
+        communicationStep = CommunicationStep::READ_CONVERSIONS;
+        break;
+    }
+    default: {
+        sif::debug << "SusHandler::buildNormalDeviceCommand: Unknwon communication "
+                << "step" << std::endl;
+        return HasReturnvaluesIF::RETURN_OK;
+    }
+    }
+	return buildCommandFromCommand(*id, nullptr, 0);
+}
+
+ReturnValue_t SusHandler::buildTransitionDeviceCommand(
+		DeviceCommandId_t * id){
+    if (internalState == InternalState::SETUP) {
+        *id = SUS::WRITE_SETUP;
+    }
+    else {
+        return HasReturnvaluesIF::RETURN_OK;
+    }
+	return buildCommandFromCommand(*id, nullptr, 0);
+}
+
+ReturnValue_t SusHandler::buildCommandFromCommand(
+		DeviceCommandId_t deviceCommand, const uint8_t * commandData,
+		size_t commandDataLen) {
+	switch(deviceCommand) {
+	case(SUS::WRITE_SETUP): {
+		cmdBuffer[0] = SUS::SETUP_DEFINITION;
+		cmdBuffer[1] = SUS::UNIPOLAR_CONFIG;
+		rawPacket = cmdBuffer;
+		rawPacketLen = 2;
+		internalState = InternalState::CONFIGURED;
+		return RETURN_OK;
+	}
+	case(SUS::START_CONVERSION): {
+	    cmdBuffer[0] = SUS::CONVERSION_DEFINITION;
+ 		rawPacket = cmdBuffer;
+		rawPacketLen = 1;
+		return RETURN_OK;
+	}
+	case(SUS::READ_CONVERSIONS): {
+	    cmdBuffer[0] = SUS::DUMMY_BYTE;
+	    cmdBuffer[1] = SUS::DUMMY_BYTE;
+	    cmdBuffer[2] = SUS::DUMMY_BYTE;
+	    cmdBuffer[3] = SUS::DUMMY_BYTE;
+	    cmdBuffer[4] = SUS::DUMMY_BYTE;
+	    cmdBuffer[5] = SUS::DUMMY_BYTE;
+	    cmdBuffer[6] = SUS::DUMMY_BYTE;
+	    cmdBuffer[7] = SUS::DUMMY_BYTE;
+	    cmdBuffer[8] = SUS::DUMMY_BYTE;
+        rawPacket = cmdBuffer;
+        rawPacketLen = SUS::READ_SIZE;
+	    return RETURN_OK;
+	}
+	default:
+		return DeviceHandlerIF::COMMAND_NOT_IMPLEMENTED;
+	}
+	return HasReturnvaluesIF::RETURN_FAILED;
+}
+
+void SusHandler::fillCommandAndReplyMap() {
+    this->insertInCommandMap(SUS::WRITE_SETUP);
+    this->insertInCommandMap(SUS::START_CONVERSION);
+    this->insertInCommandAndReplyMap(SUS::READ_CONVERSIONS, 1, &dataset,
+            SUS::READ_SIZE);
+}
+
+ReturnValue_t SusHandler::scanForReply(const uint8_t *start,
+        size_t remainingSize, DeviceCommandId_t *foundId, size_t *foundLen) {
+    *foundId = this->getPendingCommand();
+    *foundLen = remainingSize;
+    return HasReturnvaluesIF::RETURN_OK;
+}
+
+ReturnValue_t SusHandler::interpretDeviceReply(DeviceCommandId_t id,
+        const uint8_t *packet) {
+	switch (id) {
+	case SUS::READ_CONVERSIONS: {
+	    PoolReadGuard readSet(&dataset);
+	    dataset.temperatureCelcius = (*(packet) << 8 | *(packet + 1)) * 0.125;
+	    dataset.diffScanChannel0_1 = (*(packet + 2) << 8 | *(packet + 3));
+	    dataset.diffScanChannel2_3 = (*(packet + 2) << 8 | *(packet + 3));
+	    dataset.diffScanChannel4_5 = (*(packet + 2) << 8 | *(packet + 3));
+#if OBSW_VERBOSE_LEVEL >= 1 && DEBUG_SUS
+        sif::info << "SUS with object id " << std::hex << this->getObjectId() << ", temperature: "
+                << dataset.temperatureCelcius << " °C" << std::endl;
+        sif::info << "SUS with object id " << std::hex << this->getObjectId() << ", channel 0/1: "
+                << dataset.diffScanChannel0_1 << std::endl;
+        sif::info << "SUS with object id " << std::hex << this->getObjectId() << ", channel 2/3: "
+                << dataset.diffScanChannel2_3 << std::endl;
+        sif::info << "SUS with object id " << std::hex << this->getObjectId() << ", channel 4/5: "
+                << dataset.diffScanChannel4_5 << std::endl;
+#endif
+		break;
+	}
+	default: {
+	    sif::debug << "SusHandler::interpretDeviceReply: Unknown reply id" << std::endl;
+		return DeviceHandlerIF::UNKNOWN_DEVICE_REPLY;
+	}
+
+	}
+	return HasReturnvaluesIF::RETURN_OK;
+}
+
+void SusHandler::setNormalDatapoolEntriesInvalid(){
+
+}
+
+uint32_t SusHandler::getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo){
+	return 5000;
+}
+
+ReturnValue_t SusHandler::initializeLocalDataPool(localpool::DataPool& localDataPoolMap,
+        LocalDataPoolManager& poolManager) {
+    localDataPoolMap.emplace(SUS::TEMPERATURE_C, new PoolEntry<float>( { 0.0 }));
+    localDataPoolMap.emplace(SUS::DIFF_SCAN_CHANNEL_0_1, new PoolEntry<uint16_t>( { 0 }));
+    localDataPoolMap.emplace(SUS::DIFF_SCAN_CHANNEL_2_3, new PoolEntry<uint16_t>( { 0 }));
+    localDataPoolMap.emplace(SUS::DIFF_SCAN_CHANNEL_4_5, new PoolEntry<uint16_t>( { 0 }));
+    return HasReturnvaluesIF::RETURN_OK;
+}
+