From 68e1208b7f39379abb37ee7e04d82d0a54d7f275 Mon Sep 17 00:00:00 2001
From: Robin Mueller <muellerr@irs.uni-stuttgart.de>
Date: Sat, 19 Feb 2022 17:15:38 +0100
Subject: [PATCH] sus printout runtime configurable

---
 bsp_q7s/core/ObjectFactory.cpp | 14 ++++++++++++++
 linux/devices/SusHandler.cpp   | 29 +++++++++++++++++------------
 linux/devices/SusHandler.h     |  6 ++----
 3 files changed, 33 insertions(+), 16 deletions(-)

diff --git a/bsp_q7s/core/ObjectFactory.cpp b/bsp_q7s/core/ObjectFactory.cpp
index 8ce3b422..d4be4c33 100644
--- a/bsp_q7s/core/ObjectFactory.cpp
+++ b/bsp_q7s/core/ObjectFactory.cpp
@@ -463,6 +463,20 @@ void ObjectFactory::createSunSensorComponents(LinuxLibgpioIF* gpioComIF, SpiComI
   susHandler9->setToGoToNormalMode(true);
   susHandler10->setToGoToNormalMode(true);
   susHandler11->setToGoToNormalMode(true);
+#if OBSW_DEBUG_SUS == 1
+  susHandler0->enablePeriodicPrintout(true, 3);
+  susHandler1->enablePeriodicPrintout(true, 3);
+  susHandler2->enablePeriodicPrintout(true, 3);
+  susHandler3->enablePeriodicPrintout(true, 3);
+  susHandler4->enablePeriodicPrintout(true, 3);
+  susHandler5->enablePeriodicPrintout(true, 3);
+  susHandler6->enablePeriodicPrintout(true, 3);
+  susHandler7->enablePeriodicPrintout(true, 3);
+  susHandler8->enablePeriodicPrintout(true, 3);
+  susHandler9->enablePeriodicPrintout(true, 3);
+  susHandler10->enablePeriodicPrintout(true, 3);
+  susHandler11->enablePeriodicPrintout(true, 3);
+#endif
 #endif
 
 #endif /* OBSW_ADD_SUN_SENSORS == 1 */
diff --git a/linux/devices/SusHandler.cpp b/linux/devices/SusHandler.cpp
index e36460a3..bfd5516b 100644
--- a/linux/devices/SusHandler.cpp
+++ b/linux/devices/SusHandler.cpp
@@ -226,17 +226,22 @@ ReturnValue_t SusHandler::initializeLocalDataPool(localpool::DataPool &localData
 void SusHandler::setToGoToNormalMode(bool enable) { this->goToNormalModeImmediately = enable; }
 
 void SusHandler::printDataset() {
-#if OBSW_VERBOSE_LEVEL >= 1 && OBSW_DEBUG_SUS == 1
-  if (divider.checkAndIncrement()) {
-    sif::info << "SUS " << std::setw(2) << std::dec << static_cast<int>(susIdx) << " ID "
-              << std::hex << "0x" << this->getObjectId() << " [" << std::hex << std::setw(3);
-    sif::info << dataset.ain0 << ",";
-    sif::info << dataset.ain1 << ",";
-    sif::info << dataset.ain2 << ",";
-    sif::info << dataset.ain3 << ",";
-    sif::info << dataset.ain4 << ",";
-    sif::info << dataset.ain5 << "] ";
-    sif::info << "T[C] " << dataset.temperatureCelcius << " C" << std::endl;
+  if(periodicPrintout) {
+    if (divider.checkAndIncrement()) {
+      sif::info << "SUS " << std::setw(2) << std::dec << static_cast<int>(susIdx) << " ID "
+                << std::hex << "0x" << this->getObjectId() << " [" << std::hex << std::setw(3);
+      sif::info << dataset.ain0 << ",";
+      sif::info << dataset.ain1 << ",";
+      sif::info << dataset.ain2 << ",";
+      sif::info << dataset.ain3 << ",";
+      sif::info << dataset.ain4 << ",";
+      sif::info << dataset.ain5 << "] ";
+      sif::info << "T[C] " << dataset.temperatureCelcius << " C" << std::endl;
+    }
   }
-#endif
+}
+
+void SusHandler::enablePeriodicPrintout(bool enable, uint8_t divider) {
+  this->periodicPrintout = enable;
+  this->divider.setDivider(divider);
 }
diff --git a/linux/devices/SusHandler.h b/linux/devices/SusHandler.h
index 4088bd0a..f7826acf 100644
--- a/linux/devices/SusHandler.h
+++ b/linux/devices/SusHandler.h
@@ -32,7 +32,7 @@ class SusHandler : public DeviceHandlerBase {
              LinuxLibgpioIF* gpioComIF, gpioId_t chipSelectId);
   virtual ~SusHandler();
 
-  // virtual ReturnValue_t performOperation(uint8_t counter) override;
+  void enablePeriodicPrintout(bool enable, uint8_t divider);
 
   virtual ReturnValue_t initialize() override;
   void setToGoToNormalMode(bool enable);
@@ -48,7 +48,6 @@ class SusHandler : public DeviceHandlerBase {
   ReturnValue_t scanForReply(const uint8_t* start, size_t remainingSize, DeviceCommandId_t* foundId,
                              size_t* foundLen) override;
   ReturnValue_t interpretDeviceReply(DeviceCommandId_t id, const uint8_t* packet) override;
-  // void setNormalDatapoolEntriesInvalid() override;
   uint32_t getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) override;
   ReturnValue_t initializeLocalDataPool(localpool::DataPool& localDataPoolMap,
                                         LocalDataPoolManager& poolManager) override;
@@ -68,9 +67,8 @@ class SusHandler : public DeviceHandlerBase {
     READ_INT_CLOCKED_CONVERSIONS
   };
 
+  bool periodicPrintout = false;
   PeriodicOperationDivider divider;
-  // LinuxLibgpioIF* gpioComIF = nullptr;
-  // gpioId_t chipSelectId = gpio::NO_GPIO;
   bool goToNormalModeImmediately = false;
   bool commandExecuted = false;