From 6ec4d22a14f8f1bee214942aeb7334542a998ab3 Mon Sep 17 00:00:00 2001
From: "Jakob.Meier" <meierj@irs.uni-stuttgart.de>
Date: Fri, 19 Mar 2021 15:33:43 +0100
Subject: [PATCH] IMTQ dipole actuation command, wip

---
 mission/devices/IMTQHandler.cpp               |  9 ++++-
 .../IMTQHandlerDefinitions.h                  | 35 ++++++++++++++-----
 2 files changed, 34 insertions(+), 10 deletions(-)

diff --git a/mission/devices/IMTQHandler.cpp b/mission/devices/IMTQHandler.cpp
index 7ebeff05..d6de4ab2 100644
--- a/mission/devices/IMTQHandler.cpp
+++ b/mission/devices/IMTQHandler.cpp
@@ -47,9 +47,16 @@ ReturnValue_t IMTQHandler::buildCommandFromCommand(
 		return RETURN_OK;
 	}
 	case(IMTQ::START_ACTUATION_DIPOLE): {
+	    /* IMTQ expects low byte first */
 	    commandBuffer[0] = IMTQ::CC::START_ACTUATION_DIPOLE;
 	    commandBuffer[1] = *(commandData + 1);
-	    commandBuffer[2] = *(commandData + 2);
+	    commandBuffer[2] = *(commandData);
+	    commandBuffer[3] = *(commandData + 3);
+	    commandBuffer[4] = *(commandData + 2);
+	    commandBuffer[5] = *(commandData + 5);
+	    commandBuffer[6] = *(commandData + 4);
+	    commandBuffer[7] = *(commandData + 7);
+	    commandBuffer[8] = *(commandData + 6);
 		rawPacket = commandBuffer;
 		return RETURN_OK;
 	}
diff --git a/mission/devices/devicedefinitions/IMTQHandlerDefinitions.h b/mission/devices/devicedefinitions/IMTQHandlerDefinitions.h
index 34e14dc6..a153e4e2 100644
--- a/mission/devices/devicedefinitions/IMTQHandlerDefinitions.h
+++ b/mission/devices/devicedefinitions/IMTQHandlerDefinitions.h
@@ -84,6 +84,9 @@ public:
 /**
  * @brief   This class can be used to ease the generation of an action message commanding the
  *          IMTQHandler to configure the magnettorquer with the desired dipoles.
+ *
+ * @details Deserialize the packet, write the deserialized data to the ipc store and store the
+ *          the ipc store address in the action message.
  */
 class CommandDipolePacket : public SerialLinkedListAdapter<SerializeIF> {
 public:
@@ -93,16 +96,30 @@ public:
     }
 
 private:
-    CommandDipolePacket(uint16_t xDipole, uint16_t yDipole, uint16_t zDipole);
-    void setLinks() {
-        setStart(&cspPort);
-        cspPort.setNext(&querySize);
-        querySize.setNext(&magic);
+
+    /**
+     * @brief   Constructor
+     *
+     * @param xDipole   The dipole of the x coil in 10^-4*Am^2
+     * @param yDipole   The dipole of the y coil in 10^-4*Am^2
+     * @param zDipole   The dipole of the z coil in 10^-4*Am^2
+     * @param duration  The duration in milliseconds the dipole will be generated by the coils.
+     *                  When set to 0, the dipole will be generated until a new dipole actuation
+     *                  command is sent.
+     */
+    CommandDipolePacket(uint16_t xDipole, uint16_t yDipole, uint16_t zDipole) :
+            xDipole(xDipole), yDipole(yDipole), zDipole(zDipole), duration(duration) {
     }
-    SerializeElement<uint8_t> cspPort = GOMSPACE::P60_PORT_GNDWDT_RESET;
-    SerializeElement<uint16_t> querySize = 1;
-    /* Sending 0x78 to port 9 of a gomspace device resets the ground watchdog */
-    SerializeElement<uint8_t> magic = 0x78;
+    void setLinks() {
+        setStart(&xDipole);
+        xDipole.setNext(&yDipole);
+        yDipole.setNext(&zDipole);
+        zDipole.setNext(&duration);
+    }
+    SerializeElement<uint16_t> xDipole;
+    SerializeElement<uint16_t> yDipole;
+    SerializeElement<uint16_t> zDipole;
+    SerializeElement<uint16_t> duration;
 };
 }