From 7522eac9ebfdd01f7aa01e511eacd056f8155cf1 Mon Sep 17 00:00:00 2001
From: Robin Mueller <muellerr@irs.uni-stuttgart.de>
Date: Sun, 2 Apr 2023 23:43:41 +0200
Subject: [PATCH] done

---
 linux/acs/ImtqPollingTask.cpp | 20 ++++++++++++++++++++
 linux/acs/ImtqPollingTask.h   |  7 +------
 mission/acs/ImtqHandler.cpp   |  2 ++
 mission/acs/imtqHelpers.cpp   | 21 +++++++++++++++++++++
 mission/acs/imtqHelpers.h     | 12 +++++++++++-
 5 files changed, 55 insertions(+), 7 deletions(-)

diff --git a/linux/acs/ImtqPollingTask.cpp b/linux/acs/ImtqPollingTask.cpp
index cfa4e337..dc2af7dd 100644
--- a/linux/acs/ImtqPollingTask.cpp
+++ b/linux/acs/ImtqPollingTask.cpp
@@ -52,6 +52,26 @@ void ImtqPollingTask::handleMeasureStep() {
   ImtqRepliesDefault replies(replyBuf.data());
   // If some startup handling is added later, set configured after it was done once.
   if (performStartup) {
+    // Set integration time for the MGM.
+    cmdBuf[0] = imtq::CC::SET_PARAM;
+    size_t dummy = 0;
+    SerializeAdapter::serialize(&imtq::param::INTEGRATION_TIME_SELECT, cmdBuf.data() + 1, &dummy,
+                                cmdBuf.size(), SerializeIF::Endianness::LITTLE);
+    cmdBuf[3] = currentRequest.integrationTimeSel;
+    ReturnValue_t result = performI2cFullRequest(replyBuf.data(), 5);
+    if (result != returnvalue::OK) {
+      comStatus = imtq::STARTUP_CFG_ERROR;
+    }
+    if (replyBuf[0] != imtq::CC::SET_PARAM) {
+      sif::error << "ImtqPollingTask: First byte of reply not equal to sent CC" << std::endl;
+      comStatus = imtq::STARTUP_CFG_ERROR;
+    }
+    if (replyBuf[4] != currentRequest.integrationTimeSel) {
+      sif::error << "ImtqPollingTask: Integration time confiuration failed" << std::endl;
+      comStatus = imtq::STARTUP_CFG_ERROR;
+    }
+    currentIntegrationTimeMs =
+        imtq::integrationTimeFromSelectValue(currentRequest.integrationTimeSel);
     performStartup = false;
   }
   replies.setConfigured();
diff --git a/linux/acs/ImtqPollingTask.h b/linux/acs/ImtqPollingTask.h
index f4190658..378a8e38 100644
--- a/linux/acs/ImtqPollingTask.h
+++ b/linux/acs/ImtqPollingTask.h
@@ -36,16 +36,11 @@ class ImtqPollingTask : public SystemObject,
   address_t i2cAddr = 0;
   uint32_t currentIntegrationTimeMs = 10;
   // Required in addition to integration time, otherwise old data might be read.
-  static constexpr uint32_t MGM_READ_BUFFER_TIME_MS = 6;
+  static constexpr uint32_t MGM_READ_BUFFER_TIME_MS = 4;
   bool ignoreNextActuateRequest = false;
   bool performStartup = false;
 
   imtq::Request currentRequest{};
-  // int16_t dipoles[3] = {};
-  // uint16_t torqueDuration = 0;
-  // imtq::RequestType currentRequest = imtq::RequestType::MEASURE_NO_ACTUATION;
-  // imtq::SpecialRequest specialRequest = imtq::SpecialRequest::NONE;
-  // acs::SimpleSensorMode mode = acs::SimpleSensorMode::OFF;
 
   std::array<uint8_t, 32> cmdBuf;
   std::array<uint8_t, 524> replyBuf;
diff --git a/mission/acs/ImtqHandler.cpp b/mission/acs/ImtqHandler.cpp
index 1168ed33..e38af787 100644
--- a/mission/acs/ImtqHandler.cpp
+++ b/mission/acs/ImtqHandler.cpp
@@ -240,6 +240,8 @@ ReturnValue_t ImtqHandler::buildCommandFromCommand(DeviceCommandId_t deviceComma
     case (imtq::cmdIds::REQUEST): {
       request.requestType = imtq::RequestType::MEASURE_NO_ACTUATION;
       request.specialRequest = imtq::SpecialRequest::NONE;
+      // 6 ms integration time instead of 10 ms.
+      request.integrationTimeSel = 2;
       expectedReply = imtq::cmdIds::REPLY_NO_TORQUE;
       rawPacket = reinterpret_cast<uint8_t*>(&request);
       rawPacketLen = sizeof(imtq::Request);
diff --git a/mission/acs/imtqHelpers.cpp b/mission/acs/imtqHelpers.cpp
index 758cfe23..a222eb76 100644
--- a/mission/acs/imtqHelpers.cpp
+++ b/mission/acs/imtqHelpers.cpp
@@ -1,5 +1,26 @@
 #include "imtqHelpers.h"
 
+uint16_t imtq::integrationTimeFromSelectValue(uint8_t value) {
+  switch (value) {
+    case (0):
+      return 2;
+    case (1):
+      return 3;
+    case (2):
+      return 6;
+    case (3):
+      return 10;
+    case (4):
+      return 20;
+    case (5):
+      return 40;
+    case (6):
+      return 80;
+    default:
+      return 10;
+  }
+}
+
 size_t imtq::getReplySize(CC::CC cc, size_t* optSecondSize) {
   switch (cc) {
     // Software reset is a bit special and can also cause a I2C NAK because
diff --git a/mission/acs/imtqHelpers.h b/mission/acs/imtqHelpers.h
index cd39c907..38530d3c 100644
--- a/mission/acs/imtqHelpers.h
+++ b/mission/acs/imtqHelpers.h
@@ -14,6 +14,8 @@ class ImtqHandler;
 
 namespace imtq {
 
+uint16_t integrationTimeFromSelectValue(uint8_t value);
+
 enum class RequestType : uint8_t { MEASURE_NO_ACTUATION, ACTUATE, DO_NOTHING };
 
 enum class SpecialRequest : uint8_t {
@@ -59,9 +61,10 @@ static const ReturnValue_t CC_UNAVAILABLE = MAKE_RETURN_CODE(5);
 static const ReturnValue_t INTERNAL_PROCESSING_ERROR = MAKE_RETURN_CODE(6);
 static const ReturnValue_t REJECTED_WITHOUT_REASON = MAKE_RETURN_CODE(7);
 static const ReturnValue_t CMD_ERR_UNKNOWN = MAKE_RETURN_CODE(8);
+static constexpr ReturnValue_t STARTUP_CFG_ERROR = MAKE_RETURN_CODE(9);
 //! [EXPORT] : [COMMENT] The status reply to a self test command was received but no self test
 //! command has been sent. This should normally never happen.
-static const ReturnValue_t UNEXPECTED_SELF_TEST_REPLY = MAKE_RETURN_CODE(0xA7);
+static const ReturnValue_t UNEXPECTED_SELF_TEST_REPLY = MAKE_RETURN_CODE(10);
 
 namespace cmdIds {
 
@@ -164,6 +167,13 @@ enum CC : uint8_t {
 
 }  // namespace CC
 
+namespace param {
+
+static constexpr uint16_t SEL_MTM = 0x2002;
+static constexpr uint16_t INTEGRATION_TIME_SELECT = 0x2003;
+
+}  // namespace param
+
 size_t getReplySize(CC::CC cc, size_t* optSecondSize = nullptr);
 
 namespace mode {