awful solution but works

fsfw

@@ -1 +1 @@
-Subproject commit 0c5c2f6c4f07959a73a083eb3c6e1f3125642477
+Subproject commit 73454c629c042de8efe7aa4fa6dcbf1e184b0961

linux/fsfwconfig/pollingsequence/pollingSequenceFactory.cpp

@@ -430,11 +430,21 @@ ReturnValue_t pst::pstI2c(FixedTimeslotTaskIF *thisSequence) {
   uint32_t length = thisSequence->getPeriodMs();
   static_cast<void>(length);
 #if OBSW_ADD_MGT == 1
-  thisSequence->addSlot(objects::IMTQ_HANDLER, length * 0, DeviceHandlerIF::PERFORM_OPERATION);
+  thisSequence->addSlot(objects::IMTQ_HANDLER, length * 0.2, DeviceHandlerIF::PERFORM_OPERATION);
   thisSequence->addSlot(objects::IMTQ_HANDLER, length * 0.2, DeviceHandlerIF::SEND_WRITE);
-  thisSequence->addSlot(objects::IMTQ_HANDLER, length * 0.4, DeviceHandlerIF::GET_WRITE);
+  thisSequence->addSlot(objects::IMTQ_HANDLER, length * 0.2, DeviceHandlerIF::GET_WRITE);
-  thisSequence->addSlot(objects::IMTQ_HANDLER, length * 0.6, DeviceHandlerIF::SEND_READ);
+  thisSequence->addSlot(objects::IMTQ_HANDLER, length * 0.2, DeviceHandlerIF::SEND_READ);
-  thisSequence->addSlot(objects::IMTQ_HANDLER, length * 0.8, DeviceHandlerIF::GET_READ);
+  thisSequence->addSlot(objects::IMTQ_HANDLER, length * 0.2, DeviceHandlerIF::GET_READ);
+  thisSequence->addSlot(objects::IMTQ_HANDLER, length * 0.2, DeviceHandlerIF::PERFORM_OPERATION);
+  thisSequence->addSlot(objects::IMTQ_HANDLER, length * 0.2, DeviceHandlerIF::SEND_WRITE);
+  thisSequence->addSlot(objects::IMTQ_HANDLER, length * 0.2, DeviceHandlerIF::GET_WRITE);
+  thisSequence->addSlot(objects::IMTQ_HANDLER, length * 0.2, DeviceHandlerIF::SEND_READ);
+  thisSequence->addSlot(objects::IMTQ_HANDLER, length * 0.2, DeviceHandlerIF::GET_READ);
+  thisSequence->addSlot(objects::IMTQ_HANDLER, length * 0.2, DeviceHandlerIF::PERFORM_OPERATION);
+  thisSequence->addSlot(objects::IMTQ_HANDLER, length * 0.2, DeviceHandlerIF::SEND_WRITE);
+  thisSequence->addSlot(objects::IMTQ_HANDLER, length * 0.2, DeviceHandlerIF::GET_WRITE);
+  thisSequence->addSlot(objects::IMTQ_HANDLER, length * 0.2, DeviceHandlerIF::SEND_READ);
+  thisSequence->addSlot(objects::IMTQ_HANDLER, length * 0.2, DeviceHandlerIF::GET_READ);
 #endif
 #if OBSW_ADD_BPX_BATTERY_HANDLER == 1
   thisSequence->addSlot(objects::BPX_BATT_HANDLER, length * 0, DeviceHandlerIF::PERFORM_OPERATION);

mission/controller/AcsController.cpp

@@ -33,11 +33,11 @@ void AcsController::performControlOperation() {
 
   {
       // TODO: Calculate actuator output
-    PoolReadGuard pg(&dipoleSet);
+      //    PoolReadGuard pg(&dipoleSet);
-    MutexGuard mg(torquer::lazyLock());
+      //    MutexGuard mg(torquer::lazyLock());
-    torquer::NEW_ACTUATION_FLAG = true;
+      //    torquer::NEW_ACTUATION_FLAG = true;
       // TODO: Insert correct values here
-    dipoleSet.setDipoles(500, 500, 500, 150);
+      // dipoleSet.setDipoles(500, 500, 500, 150);
   }
 
   {

mission/controller/AcsController.h

@@ -20,7 +20,6 @@ class AcsController : public ExtendedControllerBase {
   enum class InternalState { STARTUP, INITIAL_DELAY, READY };
 
   InternalState internalState = InternalState::STARTUP;
-
   ReturnValue_t handleCommandMessage(CommandMessage* message) override;
   void performControlOperation() override;
 

mission/devices/ImtqHandler.cpp

@@ -27,6 +27,8 @@
 
 #include "mission/devices/torquer.h"
 
+static constexpr bool ACTUATION_WIRETAPPING = false;
+
 ImtqHandler::ImtqHandler(object_id_t objectId, object_id_t comIF, CookieIF* comCookie,
                          power::Switch_t pwrSwitcher)
     : DeviceHandlerBase(objectId, comIF, comCookie),
@@ -46,7 +48,7 @@ ImtqHandler::ImtqHandler(object_id_t objectId, object_id_t comIF, CookieIF* comC
   }
 }
 
-ImtqHandler::~ImtqHandler() {}
+ImtqHandler::~ImtqHandler() = default;
 
 void ImtqHandler::doStartUp() {
   if (goToNormalMode) {
@@ -182,7 +184,7 @@ ReturnValue_t ImtqHandler::buildCommandFromCommand(DeviceCommandId_t deviceComma
       }
       ReturnValue_t result;
       // Commands override anything which was set in the software
-      if (commandBuffer != nullptr) {
+      if (commandData != nullptr) {
         dipoleSet.setValidityBufferGeneration(false);
         result =
             dipoleSet.deSerialize(&commandData, &commandDataLen, SerializeIF::Endianness::NETWORK);
@@ -194,6 +196,11 @@ ReturnValue_t ImtqHandler::buildCommandFromCommand(DeviceCommandId_t deviceComma
         // Read set dipole values from local pool
         PoolReadGuard pg(&dipoleSet);
       }
+      if (ACTUATION_WIRETAPPING) {
+        sif::debug << "Actuating IMTQ with parameters x = " << dipoleSet.xDipole.value
+                   << ", y = " << dipoleSet.yDipole.value << ", z = " << dipoleSet.zDipole.value
+                   << ", duration = " << dipoleSet.currentTorqueDurationMs.value << std::endl;
+      }
       result = buildDipoleActuationCommand();
       if (result != returnvalue::OK) {
         return result;
@@ -312,8 +319,17 @@ ReturnValue_t ImtqHandler::scanForReply(const uint8_t* start, size_t remainingSi
       *foundLen = IMTQ::SIZE_SELF_TEST_RESULTS;
       *foundId = IMTQ::GET_SELF_TEST_RESULT;
       break;
+    case (IMTQ::CC::PAST_AVAILABLE_RESPONSE_BYTES): {
+      sif::warning << "IMTQHandler::scanForReply: Read 0xFF command byte, reading past available "
+                      "bytes. Keep 1 ms delay"
+                      " between I2C send and read"
+                   << std::endl;
+      result = IGNORE_REPLY_DATA;
+      break;
+    }
     default:
-      sif::debug << "IMTQHandler::scanForReply: Reply contains invalid command code" << std::endl;
+      sif::debug << "IMTQHandler::scanForReply: Reply with length " << remainingSize
+                 << "contains invalid command code " << static_cast<int>(*start) << std::endl;
       result = IGNORE_REPLY_DATA;
       break;
   }
@@ -2276,6 +2292,11 @@ void ImtqHandler::checkErrorByte(const uint8_t errorByte, const uint8_t step) {
   }
 }
 
+void ImtqHandler::doSendRead() {
+  TaskFactory::delayTask(1);
+  DeviceHandlerBase::doSendRead();
+}
+
 std::string ImtqHandler::makeStepString(const uint8_t step) {
   std::string stepString("");
   switch (step) {

mission/devices/ImtqHandler.h

@@ -23,6 +23,7 @@ class ImtqHandler : public DeviceHandlerBase {
 
   void setPollingMode(NormalPollingMode pollMode);
 
+  void doSendRead() override;
   /**
    * @brief   Sets mode to MODE_NORMAL. Can be used for debugging.
    */

mission/devices/devicedefinitions/imtqHandlerDefinitions.h

@@ -86,6 +86,7 @@ static const uint8_t GET_COMMANDED_DIPOLE = 0x46;
 static const uint8_t GET_RAW_MTM_MEASUREMENT = 0x42;
 static const uint8_t GET_CAL_MTM_MEASUREMENT = 0x43;
 static const uint8_t GET_SELF_TEST_RESULT = 0x47;
+static const uint8_t PAST_AVAILABLE_RESPONSE_BYTES = 0xff;
 };  // namespace CC
 
 namespace SELF_TEST_AXIS {