rm3100 continued

bsp_hosted/InitMission.cpp

@@ -8,8 +8,8 @@
 #include <fsfw/tasks/FixedTimeslotTaskIF.h>
 #include <fsfw/tasks/PeriodicTaskIF.h>
 #include <fsfw/tasks/TaskFactory.h>
-#include <fsfwconfig/objects/systemObjectList.h>
+#include <objects/systemObjectList.h>
-#include <fsfwconfig/OBSWConfig.h>
+#include <OBSWConfig.h>
 
 #include <iostream>
 

bsp_hosted/fsfwconfig/OBSWConfig.h

@@ -6,6 +6,9 @@
 #ifndef CONFIG_OBSWCONFIG_H_
 #define CONFIG_OBSWCONFIG_H_
 
+#include "returnvalues/classIds.h"
+#include "events/subsystemIdRanges.h"
+
 #define OBSW_ADD_TEST_CODE      0
 
 // These defines should be disabled for mission code but are useful for

bsp_hosted/fsfwconfig/events/subsystemIdRanges.h

@@ -18,6 +18,7 @@ enum: uint8_t {
 	PUS_SERVICE_8,
 	PUS_SERVICE_23,
 	MGM_LIS3MDL,
+	MGM_RM3100,
 
 	DUMMY_DEVICE,
 };

bsp_hosted/fsfwconfig/fsfwconfig.mk

@@ -5,11 +5,3 @@ CXXSRC += $(wildcard $(CURRENTPATH)/pollingsequence/*.cpp)
 CXXSRC += $(wildcard $(CURRENTPATH)/events/*.cpp)
 
 INCLUDES += $(CURRENTPATH)
-INCLUDES += $(CURRENTPATH)/objects
-INCLUDES += $(CURRENTPATH)/ipc
-INCLUDES += $(CURRENTPATH)/pollingsequence
-INCLUDES += $(CURRENTPATH)/returnvalues
-INCLUDES += $(CURRENTPATH)/tmtc
-INCLUDES += $(CURRENTPATH)/events
-INCLUDES += $(CURRENTPATH)/devices
-INCLUDES += $(CURRENTPATH)/cdatapool

bsp_hosted/fsfwconfig/returnvalues/classIds.h

@@ -11,7 +11,8 @@
 namespace CLASS_ID {
 enum {
 	MISSION_CLASS_ID_START = FW_CLASS_ID_COUNT,
-	MGM_LIS3MDL
+	MGM_LIS3MDL,
+	MGM_RM3100
 };
 }
 

bsp_hosted/main.cpp

@@ -1,5 +1,6 @@
-#include <bsp_hosted/fsfwconfig/OBSWVersion.h>
+#include "InitMission.h"
-#include <bsp_linux/InitMission.h>
+
+#include <OBSWVersion.h>
 #include <fsfw/tasks/TaskFactory.h>
 #include <iostream>
 

fsfwconfig/OBSWConfig.h

@@ -6,6 +6,9 @@
 #ifndef FSFWCONFIG_OBSWCONFIG_H_
 #define FSFWCONFIG_OBSWCONFIG_H_
 
+#include "returnvalues/classIds.h"
+#include "events/subsystemIdRanges.h"
+
 #define OBSW_ADD_TEST_CODE      0
 
 // These defines should be disabled for mission code but are useful for

fsfwconfig/events/subsystemIdRanges.h

@@ -17,7 +17,8 @@ enum: uint8_t {
 	PUS_SERVICE_6,
 	PUS_SERVICE_8,
 	PUS_SERVICE_23,
-	MGM_LIS3MDL
+	MGM_LIS3MDL,
+	MGM_RM3100
 };
 }
 

fsfwconfig/returnvalues/classIds.h

@@ -11,7 +11,8 @@
 namespace CLASS_ID {
 enum {
 	MISSION_CLASS_ID_START = FW_CLASS_ID_COUNT,
-	MGM_LIS3MDL
+	MGM_LIS3MDL,
+	MGM_RM3100
 };
 }
 

mission/devices/MGMHandlerLIS3MDL.cpp

@@ -39,7 +39,7 @@ void MGMHandlerLIS3MDL::doStartUp() {
 		// Set up cached registers which will be used to configure the MGM.
 		if(commandExecuted) {
 			commandExecuted = false;
-			setMode(MODE_NORMAL);
+			setMode(_MODE_TO_ON);
 		}
 		break;
 	}

mission/devices/MGMHandlerRM3100.cpp

@@ -1,4 +1,5 @@
 #include "MGMHandlerRM3100.h"
+
 #include <fsfw/devicehandlers/DeviceHandlerMessage.h>
 #include <fsfw/objectmanager/SystemObjectIF.h>
 #include <fsfw/returnvalues/HasReturnvaluesIF.h>
@@ -16,61 +17,115 @@ void MGMHandlerRM3100::doStartUp() {
 		internalState = STATE_CONFIGURE_CMM;
 	}
 
-	switch(internalState) {
+	if(internalState == STATE_CONFIGURE_CMM) {
-	case(STATE_CONFIGURE_CMM): {
+		internalState = STATE_READ_CMM;
-		break;
 	}
-	case(STATE_READ_CMM): {
+	else if(internalState == STATE_READ_CMM) {
-		break;
+		if(commandExecuted) {
+			internalState = STATE_CONFIGURE_TMRC;
 		}
-	case(STATE_CONFIGURE_TMRC): {
-		break;
 	}
-	case(STATE_READ_TMRC): {
+
-		break;
+	if(internalState == STATE_CONFIGURE_TMRC) {
+		internalState = STATE_READ_TMRC;
+	}
+	else if(internalState == STATE_READ_TMRC) {
+		if(commandExecuted) {
+			internalState = STATE_NORMAL;
+			setMode(_MODE_TO_ON);
 		}
 	}
 }
 
 void MGMHandlerRM3100::doShutDown() {
+	setMode(_MODE_POWER_DOWN);
 }
 
 ReturnValue_t MGMHandlerRM3100::buildTransitionDeviceCommand(
 		DeviceCommandId_t *id) {
 	if(internalState == STATE_CONFIGURE_CMM) {
 		*id = RM3100::CONFIGURE_CMM;
-		commandBuffer[0] = RM3100::CMM_REGISTER;
-		commandBuffer[1] = RM3100::CMM_VALUE;
-		this->rawPacket = commandBuffer;
-		this->rawPacketLen = 2;
 	}
 
 	if(internalState == STATE_READ_CMM) {
 		*id = RM3100::READ_CMM;
-		commandBuffer[0] = RM3100::CMM_REGISTER | RM3100::READ_MASK;
-		commandBuffer[1] = 0;
-		this->rawPacket = commandBuffer;
-		this->rawPacketLen = 2;
-	}
-	return RETURN_OK;
 	}
 
+	if(internalState == STATE_CONFIGURE_TMRC) {
+		*id = RM3100::CONFIGURE_TMRC;
+	}
 
-ReturnValue_t MGMHandlerRM3100::buildNormalDeviceCommand(
+	if(internalState == STATE_READ_TMRC) {
-		DeviceCommandId_t *id) {
+		*id = RM3100::READ_TMRC;
-	return RETURN_OK;
+	}
+
+	return buildCommandFromCommand(*id, nullptr, 0);
 }
 
 ReturnValue_t MGMHandlerRM3100::buildCommandFromCommand(
 		DeviceCommandId_t deviceCommand, const uint8_t *commandData,
 		size_t commandDataLen) {
+	switch(deviceCommand) {
+	case(RM3100::CONFIGURE_CMM): {
+		commandBuffer[0] = RM3100::CMM_REGISTER;
+		commandBuffer[1] = RM3100::CMM_VALUE;
+		rawPacket = commandBuffer;
+		rawPacketLen = 2;
+		break;
+	}
+	case(RM3100::READ_CMM): {
+		commandBuffer[0] = RM3100::CMM_REGISTER | RM3100::READ_MASK;
+		commandBuffer[1] = 0;
+		rawPacket = commandBuffer;
+		rawPacketLen = 2;
+		break;
+	}
+	case(RM3100::CONFIGURE_TMRC): {
+		return handleTmrcConfigCommand(deviceCommand, commandData,
+				commandDataLen);
+	}
+	case(RM3100::READ_TMRC): {
+		commandBuffer[0] = RM3100::TMRC_REGISTER | RM3100::READ_MASK;
+		commandBuffer[1] = 0;
+		rawPacket = commandBuffer;
+		rawPacketLen = 2;
+		break;
+	}
+	case(RM3100::CONFIGURE_CYCLE_COUNT): {
+		return handleCycleCountConfigCommand(deviceCommand, commandData,
+				commandDataLen);
+	}
+	case(RM3100::READ_CYCLE_COUNT): {
+		commandBuffer[0] = RM3100::CYCLE_COUNT_START_REGISTER |
+				RM3100::READ_MASK;
+		std::memset(commandBuffer + 1, 0, 6);
+		rawPacket = commandBuffer;
+		rawPacketLen = 7;
+		break;
+	}
+	case(RM3100::READ_DATA): {
+		commandBuffer[0] = RM3100::MEASUREMENT_REG_START | RM3100::READ_MASK;
+		std::memset(commandBuffer + 1, 0,  9);
+		rawPacketLen = 10;
+		break;
+	}
+	default:
+		return DeviceHandlerIF::COMMAND_NOT_IMPLEMENTED;
+	}
 	return RETURN_OK;
 }
 
+ReturnValue_t MGMHandlerRM3100::buildNormalDeviceCommand(
+		DeviceCommandId_t *id) {
+	*id = RM3100::READ_DATA;
+	return buildCommandFromCommand(*id, nullptr, 0);
+}
+
 ReturnValue_t MGMHandlerRM3100::scanForReply(const uint8_t *start,
         size_t len, DeviceCommandId_t *foundId,
 		size_t *foundLen) {
 
+	// SPI, ID will always be the one of the last sent command.
 	*foundId = this->getPendingCommand();
 	*foundLen = this->rawPacketLen;
 
@@ -85,27 +140,115 @@ ReturnValue_t MGMHandlerRM3100::scanForReply(const uint8_t *start,
 
 ReturnValue_t MGMHandlerRM3100::interpretDeviceReply(
 		DeviceCommandId_t id, const uint8_t *packet) {
-	if(id == RM3100::CONFIGURE_CMM or id == RM3100::CONFIGURE_TMRC) {
+	if(id == RM3100::READ_CMM) {
-		commandExecuted = true;
-	}
-	else if(id == RM3100::READ_CMM) {
 		if(packet[1] == cmmRegValue) {
 			commandExecuted = true;
 		}
+		else {
+			// Attempt reconfiguration.
+			internalState = STATE_CONFIGURE_CMM;
+			return DeviceHandlerIF::DEVICE_REPLY_INVALID;
+		}
 	}
 	else if(id == RM3100::READ_TMRC) {
 		if(packet[1] == tmrcRegValue) {
 			commandExecuted = true;
+			// Reading TMRC was commanded.
+			if(mode == MODE_NORMAL) {
+
+			}
 		}
 		else {
 			// Attempt reconfiguration.
 			internalState = STATE_CONFIGURE_TMRC;
-			return HasReturnvaluesIF::RETURN_FAILED;
+			return DeviceHandlerIF::DEVICE_REPLY_INVALID;
 		}
 	}
+	else if(id == RM3100::READ_DATA) {
+		// analyze data here.
+		// Field strengths in micro Tesla
+		int32_t fieldStrengthX = (packet[1] << 16 | packet[2] << 8 | packet[3])
+				* scaleFactorX;
+		int32_t fieldStrengthY = (packet[4] << 16 | packet[5] << 8 | packet[6])
+				* scaleFactorY;
+		int32_t fieldStrengthZ = (packet[7] << 16 | packet[8] << 8 | packet[9])
+				* scaleFactorZ;
+	}
 	else {
 		return DeviceHandlerIF::UNKNOWN_DEVICE_REPLY;
 	}
 
 	return RETURN_OK;
 }
+
+ReturnValue_t MGMHandlerRM3100::handleCycleCountConfigCommand(
+		DeviceCommandId_t deviceCommand, const uint8_t *commandData,
+		size_t commandDataLen) {
+	if(commandData == nullptr) {
+		return DeviceHandlerIF::INVALID_COMMAND_PARAMETER;
+	}
+
+	// Set cycle count
+	if(commandDataLen == 2) {
+		handleCycleCommand(true, commandData, commandDataLen);
+	}
+	else if(commandDataLen == 6) {
+		handleCycleCommand(false, commandData, commandDataLen);
+	}
+	else {
+		return DeviceHandlerIF::INVALID_COMMAND_PARAMETER;
+	}
+
+	commandBuffer[0] = RM3100::CYCLE_COUNT_VALUE;
+	std::memcpy(commandBuffer + 1, &cycleCountRegValueX, 2);
+	std::memcpy(commandBuffer + 3, &cycleCountRegValueY, 2);
+	std::memcpy(commandBuffer + 5, &cycleCountRegValueZ, 2);
+	rawPacketLen = 7;
+	rawPacket = commandBuffer;
+	return HasReturnvaluesIF::RETURN_OK;
+}
+
+ReturnValue_t MGMHandlerRM3100::handleCycleCommand(bool oneCycleValue,
+		const uint8_t *commandData, size_t commandDataLen) {
+	RM3100::CycleCountCommand command(oneCycleValue);
+	ReturnValue_t result = command.deSerialize(&commandData, &commandDataLen,
+			SerializeIF::Endianness::BIG);
+	if(result != HasReturnvaluesIF::RETURN_OK) {
+		return result;
+	}
+
+	// Data sheet p.30
+	// "while noise limits the useful upper range to ~400 cycle counts."
+	if(command.cycleCountX > 450 ) {
+		return DeviceHandlerIF::INVALID_COMMAND_PARAMETER;
+	}
+
+	if(not oneCycleValue and
+			(command.cycleCountY > 450 or command.cycleCountZ > 450)) {
+		return DeviceHandlerIF::INVALID_COMMAND_PARAMETER;
+	}
+
+	cycleCountRegValueX = command.cycleCountX;
+	cycleCountRegValueY = command.cycleCountY;
+	cycleCountRegValueZ = command.cycleCountZ;
+	return HasReturnvaluesIF::RETURN_OK;
+}
+
+ReturnValue_t MGMHandlerRM3100::handleTmrcConfigCommand(
+		DeviceCommandId_t deviceCommand, const uint8_t *commandData,
+		size_t commandDataLen) {
+	if(commandData == nullptr) {
+		return DeviceHandlerIF::INVALID_COMMAND_PARAMETER;
+	}
+
+	if(commandDataLen != 1) {
+		return DeviceHandlerIF::INVALID_COMMAND_PARAMETER;
+	}
+
+	commandBuffer[0] = RM3100::TMRC_REGISTER;
+	commandBuffer[1] = commandData[1];
+	rawPacketLen = 2;
+	rawPacket = commandBuffer;
+	return HasReturnvaluesIF::RETURN_OK;
+}
+

mission/devices/MGMHandlerRM3100.h

@@ -4,8 +4,12 @@
 #include "devicedefinitions/MGMHandlerRM3100Definitions.h"
 #include <fsfw/devicehandlers/DeviceHandlerBase.h>
 
+#include <OBSWConfig.h>
+
 class MGMHandlerRM3100: public DeviceHandlerBase {
 public:
+	static const uint8_t INTERFACE_ID = CLASS_ID::MGM_RM3100;
+	static const uint8_t SUBSYSTEM_ID = SUBSYSTEM_ID::MGM_RM3100;
 
 	MGMHandlerRM3100(object_id_t objectId, object_id_t deviceCommunication,
 	        CookieIF* comCookie);
@@ -45,11 +49,25 @@ private:
 	InternalState internalState = InternalState::STATE_NONE;
 	bool commandExecuted = false;
 
-	uint8_t commandBuffer[9];
+	uint8_t commandBuffer[10];
 	uint8_t commandBufferLen = 0;
 
 	uint8_t cmmRegValue = RM3100::CMM_VALUE;
 	uint8_t tmrcRegValue = RM3100::TMRC_DEFAULT_VALUE;
+	uint16_t cycleCountRegValueX = RM3100::CYCLE_COUNT_VALUE;
+	uint16_t cycleCountRegValueY = RM3100::CYCLE_COUNT_VALUE;
+	uint16_t cycleCountRegValueZ = RM3100::CYCLE_COUNT_VALUE;
+	float scaleFactorX = 1 / RM3100::DEFAULT_GAIN;
+	float scaleFactorY = 1 / RM3100::DEFAULT_GAIN;
+	float scaleFactorZ = 1 / RM3100::DEFAULT_GAIN;
+
+	ReturnValue_t handleCycleCountConfigCommand(DeviceCommandId_t deviceCommand,
+			const uint8_t *commandData,size_t commandDataLen);
+	ReturnValue_t handleCycleCommand(bool oneCycleValue,
+			const uint8_t *commandData, size_t commandDataLen);
+
+	ReturnValue_t handleTmrcConfigCommand(DeviceCommandId_t deviceCommand,
+			const uint8_t *commandData,size_t commandDataLen);
 };
 
 #endif /* MISSION_DEVICEHANDLING_MGMRM3100HANDLER_H_ */

mission/devices/devicedefinitions/MGMHandlerRM3100Definitions.h

@@ -1,7 +1,10 @@
 #ifndef MISSION_DEVICES_DEVICEDEFINITIONS_MGMHANDLERRM3100DEFINITIONS_H_
 #define MISSION_DEVICES_DEVICEDEFINITIONS_MGMHANDLERRM3100DEFINITIONS_H_
 
+#include <fsfw/datapoollocal/StaticLocalDataSet.h>
+#include <fsfw/datapoollocal/LocalPoolVariable.h>
 #include <fsfw/devicehandlers/DeviceHandlerIF.h>
+#include <fsfw/serialize/SerialLinkedListAdapter.h>
 #include <cstdint>
 
 namespace RM3100 {
@@ -24,9 +27,11 @@ static constexpr uint8_t CMM_VALUE = SET_CMM_CMZ | SET_CMM_CMY | SET_CMM_CMX |
 /*----------------------------------------------------------------------------*/
 /* Cycle count register
 /*----------------------------------------------------------------------------*/
+// Default value (200)
 static constexpr uint8_t CYCLE_COUNT_VALUE = 0xC8;
 
-static constexpr uint8_t GAIN = CYCLE_COUNT_VALUE / 100 * 38;
+static constexpr float DEFAULT_GAIN = static_cast<float>(CYCLE_COUNT_VALUE) /
+		100 * 38;
 static constexpr uint8_t CYCLE_COUNT_START_REGISTER = 0x04;
 
 /*----------------------------------------------------------------------------*/
@@ -48,13 +53,74 @@ static constexpr uint8_t REVID_REGISTER = 0x36;
 // Range in Microtesla. 1 T equals 10000 Gauss (for comparison with LIS3 MGM)
 static constexpr uint16_t RANGE = 800;
 
-static constexpr DeviceCommandId_t CONFIGURE_CMM = 3;
+static constexpr DeviceCommandId_t READ_DATA = 0;
-static constexpr DeviceCommandId_t READ_CMM = 4;
 
-//static constexpr DeviceCommandId_t CONFIGURE_CYC
+static constexpr DeviceCommandId_t CONFIGURE_CMM = 1;
+static constexpr DeviceCommandId_t READ_CMM = 2;
 
-static constexpr DeviceCommandId_t CONFIGURE_TMRC = 5;
+static constexpr DeviceCommandId_t CONFIGURE_TMRC = 3;
-static constexpr DeviceCommandId_t READ_TMRC = 6;
+static constexpr DeviceCommandId_t READ_TMRC = 4;
+
+static constexpr DeviceCommandId_t CONFIGURE_CYCLE_COUNT = 5;
+static constexpr DeviceCommandId_t READ_CYCLE_COUNT = 6;
+
+class CycleCountCommand: public SerialLinkedListAdapter<SerializeIF> {
+public:
+	CycleCountCommand(bool oneCycleCount = true): oneCycleCount(oneCycleCount) {
+		setLinks(oneCycleCount);
+	}
+
+	ReturnValue_t deSerialize(const uint8_t** buffer, size_t* size,
+			Endianness streamEndianness) override {
+		ReturnValue_t result = SerialLinkedListAdapter::deSerialize(buffer,
+				size, streamEndianness);
+		if(oneCycleCount) {
+			cycleCountY = cycleCountX;
+			cycleCountZ = cycleCountX;
+		}
+		return result;
+	}
+
+	SerializeElement<uint16_t> cycleCountX;
+	SerializeElement<uint16_t> cycleCountY;
+	SerializeElement<uint16_t> cycleCountZ;
+
+private:
+	void setLinks(bool oneCycleCount) {
+		setStart(&cycleCountX);
+		if(not oneCycleCount) {
+			cycleCountX.setNext(&cycleCountY);
+			cycleCountY.setNext(&cycleCountZ);
+		}
+	}
+
+	bool oneCycleCount;
+};
+
+static constexpr uint32_t MGM_DATASET_ID = READ_DATA;
+
+enum MgmPoolIds: lp_id_t {
+    FIELD_STRENGTH_X,
+    FIELD_STRENGTH_Y,
+    FIELD_STRENGTH_Z,
+};
+
+class Rm3100PrimaryDataset: public StaticLocalDataSet<3 * sizeof(float)> {
+public:
+	Rm3100PrimaryDataset(HasLocalDataPoolIF* hkOwner):
+        StaticLocalDataSet(hkOwner, MGM_DATASET_ID) {}
+
+	Rm3100PrimaryDataset(object_id_t mgmId):
+        StaticLocalDataSet(sid_t(mgmId, MGM_DATASET_ID)) {}
+
+	// Field strengths in micro Tesla.
+    lp_var_t<float> fieldStrengthX = lp_var_t<float>(sid.objectId,
+            FIELD_STRENGTH_X, this);
+    lp_var_t<float> fieldStrengthY = lp_var_t<float>(sid.objectId,
+            FIELD_STRENGTH_Y, this);
+    lp_var_t<float> fieldStrengthZ = lp_var_t<float>(sid.objectId,
+            FIELD_STRENGTH_Z, this);
+};
 
 }