From 8498a843548bcb3d74b1f42c22557cb361de3bf7 Mon Sep 17 00:00:00 2001
From: Robin Mueller <muellerr@irs.uni-stuttgart.de>
Date: Sat, 19 Feb 2022 16:51:46 +0100
Subject: [PATCH] important bugfix for ADIS handler

---
 mission/devices/GyroADIS1650XHandler.cpp | 41 +++++++++++++-----------
 mission/devices/GyroADIS1650XHandler.h   |  7 ++--
 2 files changed, 27 insertions(+), 21 deletions(-)

diff --git a/mission/devices/GyroADIS1650XHandler.cpp b/mission/devices/GyroADIS1650XHandler.cpp
index 0965e265..e3e418f7 100644
--- a/mission/devices/GyroADIS1650XHandler.cpp
+++ b/mission/devices/GyroADIS1650XHandler.cpp
@@ -20,10 +20,6 @@ GyroADIS1650XHandler::GyroADIS1650XHandler(object_id_t objectId, object_id_t dev
       primaryDataset(this),
       configDataset(this),
       breakCountdown() {
-#if FSFW_HAL_ADIS1650X_GYRO_DEBUG == 1
-  debugDivider = new PeriodicOperationDivider(5);
-#endif
-
 #if OBSW_ADIS1650X_LINUX_COM_IF == 1
   SpiCookie *cookie = dynamic_cast<SpiCookie *>(comCookie);
   if (cookie != nullptr) {
@@ -101,7 +97,7 @@ ReturnValue_t GyroADIS1650XHandler::buildCommandFromCommand(DeviceCommandId_t de
   switch (deviceCommand) {
     case (ADIS1650X::READ_OUT_CONFIG): {
       this->rawPacketLen = ADIS1650X::CONFIG_READOUT_SIZE;
-      uint8_t regList[5];
+      uint8_t regList[5] = {};
       regList[0] = ADIS1650X::DIAG_STAT_REG;
       regList[1] = ADIS1650X::FILTER_CTRL_REG;
       regList[2] = ADIS1650X::MSC_CTRL_REG;
@@ -305,18 +301,18 @@ ReturnValue_t GyroADIS1650XHandler::handleSensorData(const uint8_t *packet) {
         primaryDataset.setValidity(true, true);
       }
 
-#if FSFW_HAL_ADIS1650X_GYRO_DEBUG == 1
-      if (debugDivider->checkAndIncrement()) {
-        sif::info << "GyroADIS1650XHandler: Angular velocities in deg / s" << std::endl;
-        sif::info << "X: " << primaryDataset.angVelocX.value << std::endl;
-        sif::info << "Y: " << primaryDataset.angVelocY.value << std::endl;
-        sif::info << "Z: " << primaryDataset.angVelocZ.value << std::endl;
-        sif::info << "GyroADIS1650XHandler: Accelerations in m / s^2: " << std::endl;
-        sif::info << "X: " << primaryDataset.accelX.value << std::endl;
-        sif::info << "Y: " << primaryDataset.accelY.value << std::endl;
-        sif::info << "Z: " << primaryDataset.accelZ.value << std::endl;
+      if (periodicPrintout) {
+        if (debugDivider.checkAndIncrement()) {
+          sif::info << "GyroADIS1650XHandler: Angular velocities in deg / s" << std::endl;
+          sif::info << "X: " << primaryDataset.angVelocX.value << std::endl;
+          sif::info << "Y: " << primaryDataset.angVelocY.value << std::endl;
+          sif::info << "Z: " << primaryDataset.angVelocZ.value << std::endl;
+          sif::info << "GyroADIS1650XHandler: Accelerations in m / s^2: " << std::endl;
+          sif::info << "X: " << primaryDataset.accelX.value << std::endl;
+          sif::info << "Y: " << primaryDataset.accelY.value << std::endl;
+          sif::info << "Z: " << primaryDataset.accelZ.value << std::endl;
+        }
       }
-#endif
 
       break;
     }
@@ -446,6 +442,9 @@ ReturnValue_t GyroADIS1650XHandler::spiSendCallback(SpiComIF *comIf, SpiCookie *
       }
 
       size_t idx = 0;
+      spi_ioc_transfer *transferStruct = cookie->getTransferStructHandle();
+      uint64_t origTx = transferStruct->tx_buf;
+      uint64_t origRx = transferStruct->rx_buf;
       while (idx < sendLen) {
         // Pull SPI CS low. For now, no support for active high given
         if (gpioId != gpio::NO_GPIO) {
@@ -471,11 +470,12 @@ ReturnValue_t GyroADIS1650XHandler::spiSendCallback(SpiComIF *comIf, SpiCookie *
         if (idx < sendLen) {
           usleep(ADIS1650X::STALL_TIME_MICROSECONDS);
         }
-        spi_ioc_transfer *transferStruct = cookie->getTransferStructHandle();
+
         transferStruct->tx_buf += 2;
         transferStruct->rx_buf += 2;
       }
-
+      transferStruct->tx_buf = origTx;
+      transferStruct->rx_buf = origRx;
       if (gpioId != gpio::NO_GPIO) {
         mutex->unlockMutex();
       }
@@ -486,4 +486,9 @@ ReturnValue_t GyroADIS1650XHandler::spiSendCallback(SpiComIF *comIf, SpiCookie *
 
 void GyroADIS1650XHandler::setToGoToNormalModeImmediately() { goToNormalMode = true; }
 
+void GyroADIS1650XHandler::enablePeriodicPrintouts(bool enable, uint8_t divider) {
+  periodicPrintout = enable;
+  debugDivider.setDivider(divider);
+}
+
 #endif /* OBSW_ADIS1650X_LINUX_COM_IF == 1 */
diff --git a/mission/devices/GyroADIS1650XHandler.h b/mission/devices/GyroADIS1650XHandler.h
index 1db0f3a8..55de1477 100644
--- a/mission/devices/GyroADIS1650XHandler.h
+++ b/mission/devices/GyroADIS1650XHandler.h
@@ -23,6 +23,7 @@ class GyroADIS1650XHandler : public DeviceHandlerBase {
   GyroADIS1650XHandler(object_id_t objectId, object_id_t deviceCommunication, CookieIF *comCookie,
                        ADIS1650X::Type type);
 
+  void enablePeriodicPrintouts(bool enable, uint8_t divider);
   void setToGoToNormalModeImmediately();
 
   // DeviceHandlerBase abstract function implementation
@@ -69,13 +70,13 @@ class GyroADIS1650XHandler : public DeviceHandlerBase {
                                        size_t sendLen, void *args);
 #endif
 
-#if FSFW_HAL_ADIS1650X_GYRO_DEBUG == 1
-  PeriodicOperationDivider *debugDivider;
-#endif
   Countdown breakCountdown;
   void prepareWriteCommand(uint8_t startReg, uint8_t valueOne, uint8_t valueTwo);
 
   ReturnValue_t handleSensorData(const uint8_t *packet);
+
+  bool periodicPrintout = false;
+  PeriodicOperationDivider debugDivider = PeriodicOperationDivider(3);
 };
 
 #endif /* MISSION_DEVICES_GYROADIS16507HANDLER_H_ */