From c486bb2cf38b757adf1b91ec19b2b35d4ccb5e98 Mon Sep 17 00:00:00 2001
From: meggert <eggertm@irs.uni-stuttgart.de>
Date: Wed, 27 Sep 2023 14:46:45 +0200
Subject: [PATCH] pwr ctrl fdir

---
 mission/controller/PowerController.cpp | 21 ++++++++++++++++++++-
 mission/controller/PowerController.h   |  5 +++++
 mission/power/defs.h                   |  6 ++++++
 3 files changed, 31 insertions(+), 1 deletion(-)

diff --git a/mission/controller/PowerController.cpp b/mission/controller/PowerController.cpp
index 1e45a902..12ed5216 100644
--- a/mission/controller/PowerController.cpp
+++ b/mission/controller/PowerController.cpp
@@ -54,6 +54,12 @@ ReturnValue_t PowerController::getParameter(uint8_t domainId, uint8_t parameterI
         case 0x3:
           parameterWrapper->set(maxAllowedTimeDiff);
           break;
+        case 0x4:
+          parameterWrapper->set(payloadLimit);
+          break;
+        case 0x5:
+          parameterWrapper->set(higherModesLimit);
+          break;
         default:
           return INVALID_IDENTIFIER_ID;
       }
@@ -213,7 +219,20 @@ void PowerController::calculateStateOfCharge() {
 }
 
 void PowerController::watchStateOfCharge() {
-  // ToDo: FDIR
+  if (pwrCtrlCoreHk.coulombCounterCharge.isValid()) {
+    if (not pwrLvlLowFlag and pwrCtrlCoreHk.coulombCounterCharge.value < payloadLimit) {
+      triggerEvent(power::POWER_LEVEL_LOW);
+      pwrLvlLowFlag = true;
+    } else {
+      pwrLvlLowFlag = false;
+    }
+    if (not pwrLvlCriticalFlag and pwrCtrlCoreHk.coulombCounterCharge.value < higherModesLimit) {
+      triggerEvent(power::POWER_LEVEL_CRITICAL);
+      pwrLvlCriticalFlag = true;
+    } else {
+      pwrLvlCriticalFlag = false;
+    }
+  }
 }
 
 ReturnValue_t PowerController::calculateOpenCircuitVoltageCharge() {
diff --git a/mission/controller/PowerController.h b/mission/controller/PowerController.h
index fbd2709b..e7031ad8 100644
--- a/mission/controller/PowerController.h
+++ b/mission/controller/PowerController.h
@@ -57,6 +57,8 @@ class PowerController : public ExtendedControllerBase, public ReceivesParameterM
   float batteryMaximumCapacity = 2.6 * 2;                 // [Ah]
   float coulombCounterVoltageUpperThreshold = 16.2e3;     // [mV]
   double maxAllowedTimeDiff = 0.5;                        // [s]
+  float payloadLimit = 0.75;                              // [%]
+  float higherModesLimit = 0.6;                           // [%]
 
   // OCV Look-up-Table {[Ah],[V]}
   static constexpr uint8_t LOOK_UP_TABLE_MAX_IDX = 99;
@@ -111,6 +113,9 @@ class PowerController : public ExtendedControllerBase, public ReceivesParameterM
   static constexpr int16_t INVALID_TOTAL_BATTERY_CURRENT = 0;
   static constexpr float INVALID_SOC = -1;
 
+  bool pwrLvlLowFlag = false;
+  bool pwrLvlCriticalFlag = false;
+
   // HK Datasets for Calculation
   BpxBatteryHk bpxBatteryHk = BpxBatteryHk(objects::BPX_BATT_HANDLER);
   P60Dock::CoreHkSet p60CoreHk = P60Dock::CoreHkSet(objects::P60DOCK_HANDLER);
diff --git a/mission/power/defs.h b/mission/power/defs.h
index 375aaf6e..22a8af53 100644
--- a/mission/power/defs.h
+++ b/mission/power/defs.h
@@ -55,6 +55,12 @@ static constexpr Event DATASET_READ_FAILED = event::makeEvent(SUBSYSTEM_ID, 4, s
 static constexpr Event VOLTAGE_OUT_OF_BOUNDS = event::makeEvent(SUBSYSTEM_ID, 5, severity::HIGH);
 //! [EXPORT] : [COMMENT] Time difference for Coulomb Counter was too large.
 static constexpr Event TIMEDELTA_OUT_OF_BOUNDS = event::makeEvent(SUBSYSTEM_ID, 6, severity::LOW);
+//! [EXPORT] : [COMMENT] The State of Charge is below the limit for payload use. Setting Payload to
+//! faulty.
+static constexpr Event POWER_LEVEL_LOW = event::makeEvent(SUBSYSTEM_ID, 7, severity::HIGH);
+//! [EXPORT] : [COMMENT] The State of Charge is below the limit for higher modes. Setting Reaction
+//! Wheels to faulty.
+static constexpr Event POWER_LEVEL_CRITICAL = event::makeEvent(SUBSYSTEM_ID, 8, severity::HIGH);
 
 enum class States { IDLE, SWITCHING_POWER, CHECKING_POWER, MODE_COMMANDING };
 enum class OpCodes { NONE, TO_OFF_DONE, TO_NOT_OFF_DONE, TIMEOUT_OCCURED };