Add power modeling and scheduling based on thesis

src/power/Power.cpp

@@ -74,6 +74,16 @@ float Power::getBatteryVoltage() {
 }
 
 int Power::getBatteryChargePercent() {
+  return percentRemaining;
+}
+
+float Power::getBatteryChargeCoulombs() {
+  return coloumbsRemaining;
+}
+
+
+
+int Power::getBatteryVoltageChargePercent() {
   // Get the battery voltage and calculate the charge state based on the
   // discharge curve.
   float batteryVoltage = getBatteryVoltage();
@@ -105,7 +115,69 @@ int Power::getBatteryChargePercent() {
   }
 }
 
-PowerScheduler* Power::powerScheduler = nullptr;
+void Power::updatePowerStateHandler() {
+  float currentCurrent = powerScheduler->getCurrentCurrent();
+  int referenceCurrentMa =
+      PowerParameters::Battery::DISCHARGE_CURVE::REFERENCE_CURRENT_A * 1000;
+
+  // Calculate remaining battery charge in Coulombs based on current and time
+  float coloumbsConsumedSinceLastUpdate =
+      (currentCurrent / 1000) *
+      ((pdTICKS_TO_MS(xTaskGetTickCount() - lastPowerStateUpdate)) / 1000.0);
+
+  // Update coloumbs remaining
+  coloumbsRemaining -= coloumbsConsumedSinceLastUpdate;
+
+  float chargeState;
+
+  // If current flow is close enough to reference, get battery charge state via
+  // voltage curve
+  if ((currentCurrent > (referenceCurrentMa * 0.6)) &&
+      (currentCurrent < (referenceCurrentMa * 1.4))) {
+    // Get battery charge state from voltage curve
+    chargeState = getBatteryVoltageChargePercent();
+  } else {
+    // Calculate battery charge state from Charge consumption
+    float oldChargeState = lastSOC[latestSoCIndex];
+    float chargeState =
+        oldChargeState - ((coloumbsConsumedSinceLastUpdate /
+                           PowerParameters::Battery::CELL_CHARGE_FULL_COLOUMB) *
+                          100);
+  }
+
+  addSoCSample(chargeState);
+
+  // Update percentage remaining based on charge state average
+  float sampleSum = 0;
+  for (int i = 0; i < PowerParameters::Battery::AVERAGING_SAMPLES; i++) {
+    sampleSum += lastSOC[i];
+  }
+  percentRemaining = sampleSum / PowerParameters::Battery::AVERAGING_SAMPLES;
+
+  // Update last update time
+  lastPowerStateUpdate = xTaskGetTickCount();
+
+  // Update the available current (changes based on battery state of charge)
+  powerScheduler->recalculateCurrentBudgets();
+
+  return;
+
+}
+
+float Power::getMax3V3Current() {
+  float u_bat = getBatteryVoltage();
+  float i_bat = PowerParameters::Battery::CELL_CURRENT_1C;
+  float eta = PowerParameters::BUCK_BOOST_EFFICIENCY;
+  constexpr float u_3v3 = 3.3;
+  return (u_bat * i_bat * eta) / u_3v3;
+}
+
+void Power::addSoCSample(float soc) {
+  latestSoCIndex = (latestSoCIndex + 1) % PowerParameters::Battery::AVERAGING_SAMPLES;
+  lastSOC[latestSoCIndex] = soc;
+}
+
+PowerScheduler *Power::powerScheduler = nullptr;
 
 Power::Power() {
   // Initialize the power scheduler

src/power/Power.h

@@ -11,8 +11,6 @@
 #ifndef Power_h
 #define Power_h
 
-#define TOTAL_POWER_MILLIWATTS POWER_BUDGET
-
 enum TaskResumptionReason { POWER_AVAILABLE, TIMEOUT };
 
 class Power {
@@ -55,6 +53,11 @@ public:
   /// @return Battery charge state in percent
   static int getBatteryChargePercent();
 
+  /// @brief Get estimated battery charge state as percentage based on
+  // voltage directly
+  /// @return Battery charge state in percent
+  static int getBatteryVoltageChargePercent();
+
   /// @brief Get estimated battery charge state as coulombs
   /// @return Battery charge state in coulombs
   static float getBatteryChargeCoulombs();
@@ -62,26 +65,35 @@ public:
   /// @brief get available current (after voltage conversion and efficiency
   /// losses, referencing 1C discharge)
   /// @return available current in milliamps
-  static float getAvailableCurrent();
+  static float getMax3V3Current();
 
 protected:
   /// @brief PowerScheduler instance to manage power consumption
   static PowerScheduler *powerScheduler;
 
   /// @brief update Power State
-  static void updatePowerState();
+  static void updatePowerStateHandler();
 
   /*
    * Power State
    */
 
+  /// @brief last time of power state update
+  static TickType_t lastPowerStateUpdate;
+
   /// @brief remaining Charge in coulombs
-  static int coloumbsRemaining;
+  static float coloumbsRemaining;
 
   /// @brief remaining Charge in percent
   static int percentRemaining;
 
-  friend class PowerScheduler;
+  /// @brief Circular array of last calculated values for current state of
+  /// charge
+  static float lastSOC[PowerParameters::Battery::AVERAGING_SAMPLES];
+  static int latestSoCIndex;
+
+  /// @brief Add calculated value to circular array, pushing out oldest value
+  static void addSoCSample(float soc);
 };
 
 extern Power power;

src/power/PowerParameters.h

@@ -41,6 +41,9 @@ namespace PowerParameters {
       static constexpr float VOLTAGE_DIVIDER_FACTOR =
           (VOLTAGE_DIVIDER_R12 + VOLTAGE_DIVIDER_R13) / VOLTAGE_DIVIDER_R13;
     };
+
+    // Configuration
+    static constexpr int AVERAGING_SAMPLES = 10;
   };
 
   // Factors concerning Buck-Boost-Converter

src/power/PowerScheduler.cpp

@@ -11,21 +11,27 @@
  */
 
 #include "PowerScheduler.h"
+#include "Power.h"
 
 bool PowerScheduler::tryAccquireCurrentAllowance(
     PowerParameters::PowerConsumers consumer, uint16_t neededCurrent,
     uint16_t requestedDurationMs) {
   portENTER_CRITICAL(&mux);
-  if (this->freeLimitCurrentBudget > 0 &&
+  float existingConsumption = getConsumerCurrent(consumer);
-      this->freeMaximumCurrentBudget >= neededCurrent) {
+  if ((this->freeLimitCurrentBudget + existingConsumption) > 0 &&
-    this->currentAllowances.push_back(
+      (this->freeMaximumCurrentBudget + existingConsumption) >= neededCurrent) {
-        {.consumer = consumer,
+    if (existingConsumption > 0) {
+      releaseCurrent(consumer);
+    }
+    this->currentAllowances.push_back(PowerScheduler::CurrentAllowance{
+        .consumer = consumer,
         .maxSlackTimeMs = 0,
         .requestedDurationMs = requestedDurationMs,
         .taskHandle = xTaskGetCurrentTaskHandle(),
         .neededCurrent = neededCurrent,
         .requestedAt = xTaskGetTickCount(),
-         .granted = false});
+        .grantedAt = xTaskGetTickCount(),
+        .granted = true});
     this->recalculateCurrentBudgets();
     portEXIT_CRITICAL(&mux);
     return true;
@@ -79,8 +85,24 @@ bool PowerScheduler::waitForCurrentAllowance(
           PowerScheduler::PowerWakeupReasons::POWER_AVAILABLE) {
         // We were woken up because new power is available, check if it is
         // enough
-        if (this->freeLimitCurrentBudget > 0 &&
+        float existingConsumption = getConsumerCurrent(consumer);
-            this->freeMaximumCurrentBudget >= neededCurrent) {
+        if ((this->freeLimitCurrentBudget + existingConsumption) > 0 &&
+            (this->freeMaximumCurrentBudget + existingConsumption) >=
+                neededCurrent) {
+          // TODO Check if there is a currently active allowance for this
+          // consumer and if so, replace it with the new one
+          if (existingConsumption > 0) {
+            releaseCurrent(consumer);
+          }
+          this->currentAllowances.push_back(
+              {.consumer = consumer,
+               .maxSlackTimeMs = 0,
+               .requestedDurationMs = requestedDurationMs,
+               .taskHandle = xTaskGetCurrentTaskHandle(),
+               .neededCurrent = neededCurrent,
+               .requestedAt = initialTickCount,
+               .grantedAt = xTaskGetTickCount(),
+               .granted = true});
           return true;
         } else {
           // Still not enough power available for us. Wait the remaining ticks.
@@ -95,8 +117,7 @@ bool PowerScheduler::waitForCurrentAllowance(
       // Remove the task from the list of waiting tasks
       for (auto it = currentAllowances.begin(); it != currentAllowances.end();
            ++it) {
-        if (it->consumer == consumer &&
+        if (it->consumer == consumer && it->requestedAt == initialTickCount) {
-            it->requestedAt == initialTickCount) {
           currentAllowances.erase(it);
           break;
         }
@@ -113,7 +134,7 @@ void PowerScheduler::checkWaitingTasks(void) {
   // If there are requested allowances, try to grant the one expiring next
   if (this->currentAllowances.size() > 0) {
 
-    PowerScheduler::CurrentAllowance* nextAllowance =
+    PowerScheduler::CurrentAllowance *nextAllowance =
         getNextExpiringAllowance();
     if (nextAllowance != nullptr) {
       xTaskNotify(nextAllowance->taskHandle,
@@ -124,18 +145,20 @@ void PowerScheduler::checkWaitingTasks(void) {
 }
 
 void PowerScheduler::recalculateCurrentBudgets(void) {
-  // TODO: replace with actual current modeling
+  // Get the respective maximums and subtract currently flowing currents
-  this->freeLimitCurrentBudget = this->limitCurrent;
+  float tempFreeLimitCurrentBudget = Power::getMax3V3Current();
-  this->freeMaximumCurrentBudget = this->maximumCurrent * 2;
+  float tempFreeMaximumCurrentBudget = Power::getMax3V3Current() * 2;
   for (auto &allowance : currentAllowances) {
     if (allowance.granted) {
-      this->freeLimitCurrentBudget -= allowance.neededCurrent;
+      tempFreeLimitCurrentBudget -= allowance.neededCurrent;
-      this->freeMaximumCurrentBudget -= allowance.neededCurrent;
+      tempFreeMaximumCurrentBudget -= allowance.neededCurrent;
     }
   }
+  this->freeLimitCurrentBudget = tempFreeLimitCurrentBudget;
+  this->freeMaximumCurrentBudget = tempFreeMaximumCurrentBudget;
 }
 
-PowerScheduler::CurrentAllowance*
+PowerScheduler::CurrentAllowance *
 PowerScheduler::getCurrentAllowance(PowerParameters::PowerConsumers consumer) {
   for (auto &allowance : currentAllowances) {
     if (allowance.consumer == consumer) {
@@ -144,7 +167,7 @@ PowerScheduler::getCurrentAllowance(PowerParameters::PowerConsumers consumer) {
   }
   return nullptr;
 }
-PowerScheduler::CurrentAllowance*
+PowerScheduler::CurrentAllowance *
 PowerScheduler::getCurrentAllowance(TaskHandle_t taskHandle) {
   for (auto &allowance : currentAllowances) {
     if (allowance.taskHandle == taskHandle) {
@@ -153,7 +176,7 @@ PowerScheduler::getCurrentAllowance(TaskHandle_t taskHandle) {
   }
   return nullptr;
 }
-PowerScheduler::CurrentAllowance*
+PowerScheduler::CurrentAllowance *
 PowerScheduler::getNextExpiringAllowance(void) {
   TickType_t minTicks = UINT32_MAX;
   CurrentAllowance *nextAllowance = nullptr;
@@ -171,7 +194,7 @@ PowerScheduler::getNextExpiringAllowance(void) {
   return nextAllowance;
 }
 
-PowerScheduler& PowerScheduler::getPowerScheduler(float i_limit_ma,
+PowerScheduler &PowerScheduler::getPowerScheduler(float i_limit_ma,
                                                   float i_max_ma) {
   if (powerSchedulerInstance == nullptr) {
     // Double check locking
@@ -185,6 +208,35 @@ PowerScheduler& PowerScheduler::getPowerScheduler(float i_limit_ma,
   return *powerSchedulerInstance;
 }
 
+float PowerScheduler::getCurrentCurrent(void) {
+  float currentSum = 0;
+  for (auto &allowance : currentAllowances) {
+    if (allowance.granted) {
+      currentSum += allowance.neededCurrent;
+    }
+  }
+  return currentSum;
+}
+
+float PowerScheduler::getFreeCurrentBudget(void) {
+  return this->freeLimitCurrentBudget;
+}
+
+float PowerScheduler::getFreeHardMaxCurrent(void) {
+  return this->freeMaximumCurrentBudget;
+}
+
+float PowerScheduler::getConsumerCurrent(
+    PowerParameters::PowerConsumers consumer) {
+  float currentSum = 0;
+  for (auto &allowance : currentAllowances) {
+    if (allowance.consumer == consumer && allowance.granted) {
+      currentSum += allowance.neededCurrent;
+    }
+  }
+  return currentSum;
+}
+
 PowerScheduler::PowerScheduler(float i_limit_ma, float i_max_ma) {
   this->limitCurrent = i_limit_ma;
   this->maximumCurrent = i_max_ma;

src/power/PowerScheduler.h

@@ -24,17 +24,18 @@ private:
   static constexpr uint16_t DEFAULT_SLACK_TIME_MS = 100;
   PowerScheduler(float i_limit_ma, float i_max_ma);
 
-  public:
+public:
   ~PowerScheduler();
   /// @brief Initialize the singleton instance of the power manager
   /// @return reference to the power manager
-  static PowerScheduler& getPowerScheduler(float i_limit_ma=0, float i_max_ma=0);
+  static PowerScheduler &getPowerScheduler(float i_limit_ma = 0,
+                                           float i_max_ma = 0);
   /// @brief Get the current free current budget (to C1 discharge)
   /// @return the amount of power that is currently available (in mA)
-  uint16_t getFreeCurrentBudget(void);
+  float getFreeCurrentBudget(void);
   /// @brief Get the current hard maximum free current (to C2 discharge)
   /// @return the maximum amount of power that can be allocated (in mA)
-  uint16_t getFreeHardMaxCurrent(void);
+  float getFreeHardMaxCurrent(void);
 
   /// @brief Request an allowance of a certain number of milliamperes from the
   /// power scheduler without waiting for it (meaning it will not be scheduled
@@ -66,7 +67,7 @@ private:
 
   //// @brief Get currently granted power
   /// @return the amount of power that is currently allocated (in mA)
-  uint16_t getCurrentCurrent(void);
+  float getCurrentCurrent(void);
 
   /// @brief Power consumer data structure
   struct CurrentAllowance {
@@ -76,7 +77,7 @@ private:
     TaskHandle_t taskHandle;
     uint16_t neededCurrent;
     TickType_t requestedAt;
-    TickType_t startedAt;
+    TickType_t grantedAt;
     bool granted;
   };
 
@@ -85,6 +86,20 @@ private:
     POWER_AVAILABLE = 1,
     POWER_EXPIRED = 2,
   };
+  // @brief Responsible for recalculating the current budgets
+  void recalculateCurrentBudgets(void);
+
+  // @brief Get current consumption of a consumer
+  float getConsumerCurrent(PowerParameters::PowerConsumers consumer);
+
+  // @brief Retrieve the current allowance for a given consumer
+  CurrentAllowance *
+  getCurrentAllowance(PowerParameters::PowerConsumers consumer);
+  // @brief Retrieve the current allowance for a given task
+  CurrentAllowance *getCurrentAllowance(TaskHandle_t taskHandle);
+
+  // @brief Retrieve the allowance that will expire next
+  CurrentAllowance *getNextExpiringAllowance(void);
 
 protected:
   // Current above which there will be no new scheduling
@@ -100,25 +115,12 @@ protected:
   // @brief Responsible for selecting the next task to be granted power
   void checkWaitingTasks(void);
 
-  // @brief Responsible for recalculating the current budgets
-  void recalculateCurrentBudgets(void);
-  // @brief Retrieve the current allowance for a given consumer
-  CurrentAllowance *
-  getCurrentAllowance(PowerParameters::PowerConsumers consumer);
-  // @brief Retrieve the current allowance for a given task
-  CurrentAllowance *getCurrentAllowance(TaskHandle_t taskHandle);
-
-  // @brief Retrieve the allowance that will expire next
-  CurrentAllowance *getNextExpiringAllowance(void);
-
   // @brief Mutex to protect the power scheduler from concurrent access
   static portMUX_TYPE mux;
 
   std::vector<PowerScheduler::CurrentAllowance> currentAllowances;
 };
 
-
-
 static PowerScheduler *powerSchedulerInstance;
 
 #endif // PowerScheduler_h