From fea0b2bfe9ba85c429b40643a1cda3675cb9a824 Mon Sep 17 00:00:00 2001
From: Robin Mueller <muellerr@irs.uni-stuttgart.de>
Date: Wed, 2 Feb 2022 17:53:07 +0100
Subject: [PATCH] added debug printouts

---
 mission/devices/BpxBatteryHandler.cpp | 24 +++++++++++++++++++++++-
 1 file changed, 23 insertions(+), 1 deletion(-)

diff --git a/mission/devices/BpxBatteryHandler.cpp b/mission/devices/BpxBatteryHandler.cpp
index a1069cfb..a6a9fe9f 100644
--- a/mission/devices/BpxBatteryHandler.cpp
+++ b/mission/devices/BpxBatteryHandler.cpp
@@ -1,4 +1,7 @@
 #include "BpxBatteryHandler.h"
+#include "OBSWConfig.h"
+
+#include <fsfw/datapool/PoolReadGuard.h>
 
 BpxBatteryHandler::BpxBatteryHandler(object_id_t objectId, object_id_t comIF, CookieIF* comCookie)
     : DeviceHandlerBase(objectId, comIF, comCookie), hkSet(this), cfgSet(this) {}
@@ -30,6 +33,10 @@ ReturnValue_t BpxBatteryHandler::buildNormalDeviceCommand(DeviceCommandId_t* id)
 }
 
 ReturnValue_t BpxBatteryHandler::buildTransitionDeviceCommand(DeviceCommandId_t* id) {
+  if(state == States::CHECK_COM) {
+    *id = BpxBattery::PING;
+    return buildCommandFromCommand(*id, nullptr, 0);
+  }
   return HasReturnvaluesIF::RETURN_OK;
 }
 
@@ -47,7 +54,7 @@ ReturnValue_t BpxBatteryHandler::buildCommandFromCommand(DeviceCommandId_t devic
                                                          size_t commandDataLen) {
   switch (deviceCommand) {
     case (BpxBattery::PING): {
-      if (commandDataLen == 1) {
+      if (commandDataLen == 1 and commandData != nullptr) {
         sentPingByte = commandData[0];
       } else {
         sentPingByte = BpxBattery::DEFAULT_PING_SENT_BYTE;
@@ -172,10 +179,24 @@ ReturnValue_t BpxBatteryHandler::scanForReply(const uint8_t* start, size_t remai
 ReturnValue_t BpxBatteryHandler::interpretDeviceReply(DeviceCommandId_t id, const uint8_t* packet) {
   switch (id) {
     case (BpxBattery::GET_HK): {
+      PoolReadGuard rg(&hkSet);
       ReturnValue_t result = hkSet.parseRawHk(packet, 21);
       if (result != HasReturnvaluesIF::RETURN_OK) {
         return result;
       }
+#if OBSW_DEBUG_BPX_BATT == 1
+     sif::info << "BPX Battery HK output:" << std::endl;
+     sif::info << "Charge current [mA]: " << hkSet.chargeCurrent << std::endl;
+     sif::info << "Discharge current [mA]; " << hkSet.dischargeCurrent << std::endl;
+     sif::info << "Heater current [mA]: " << hkSet.heaterCurrent << std::endl;
+     sif::info << "Battery voltage [mV]: " << hkSet.battVoltage << std::endl;
+     sif::info << "Battery Temperature 1 [C]: " << hkSet.battTemp1 << std::endl;
+     sif::info << "Battery Temperature 2 [C]: " << hkSet.battTemp2 << std::endl;
+     sif::info << "Battery Temperature 3 [C]: " << hkSet.battTemp3 << std::endl;
+     sif::info << "Battery Temperature 4 [C]: " << hkSet.battTemp4 << std::endl;
+     sif::info << "Battery Reboot Counter: " << hkSet.rebootCounter << std::endl;
+     sif::info << "Battery Boot Cause: " << hkSet.bootcause << std::endl;
+#endif
       break;
     }
     case (BpxBattery::PING): {
@@ -197,6 +218,7 @@ ReturnValue_t BpxBatteryHandler::interpretDeviceReply(DeviceCommandId_t id, cons
       break;
     }
     case (BpxBattery::CONFIG_GET): {
+      PoolReadGuard rg(&cfgSet);
       ReturnValue_t result = cfgSet.parseRawHk(packet, 3);
       if (result != HasReturnvaluesIF::RETURN_OK) {
         return result;