Update picking of next current allowance to take more than one outstanding allowance into consideration

src/power/PowerScheduler.cpp

@@ -17,19 +17,22 @@ bool PowerScheduler::tryAccquireCurrentAllowance(
     PowerParameters::PowerConsumers consumer, float neededCurrent,
     uint16_t requestedDurationMs) {
   float existingConsumption = getConsumerCurrent(consumer);
-      const bool currentAvailableBelowLimit =
+  const bool currentLimitNotExceeded =
       this->freeLimitCurrentBudget + existingConsumption > 0;
-      const bool currentAvailableBelowMaximum =
+  const bool neededCurrentAvailableBelowMaximum =
       this->freeMaximumCurrentBudget + existingConsumption >= neededCurrent;
-      const bool currentIsInsignificant = neededCurrent < 1;
+  const bool currentIsInsignificant =
+      neededCurrent < PowerParameters::CURRENT_INSIGNIFICANT;
   if (currentIsInsignificant ||
-        (currentAvailableBelowLimit && currentAvailableBelowMaximum)) {
+      (currentLimitNotExceeded && neededCurrentAvailableBelowMaximum)) {
     if (existingConsumption > 0) {
       releaseCurrent(consumer);
     }
     PowerSchedulerMutex lock(powerSchedulerMutex);
     if (lock.isLocked() == false) {
-      ESP_LOGE(TAG, "Failed to Acquire PowerScheduler Mutex during Current Allocation");
+      ESP_LOGE(
+          TAG,
+          "Failed to Acquire PowerScheduler Mutex during Current Allocation");
       return false;
     }
     this->currentAllowances.push_back(PowerScheduler::CurrentAllowance{
@@ -110,14 +113,14 @@ bool PowerScheduler::waitForCurrentAllowance(
         // Exclude existing consumption from the same consumer, as it will be
         // gone if this is granted
         float existingConsumption = getConsumerCurrent(consumer);
-        const bool currentAvailableBelowLimit =
+        const bool currentLimitNotExceeded =
             this->freeLimitCurrentBudget + existingConsumption > 0;
-        const bool currentAvailableBelowMaximum =
+        const bool neededCurrentAvailableBelowMaximum =
             this->freeMaximumCurrentBudget + existingConsumption >=
             neededCurrent;
         const bool currentIsInsignificant = neededCurrent < 1;
         if (currentIsInsignificant ||
-            (currentAvailableBelowLimit && currentAvailableBelowMaximum)) {
+            (currentLimitNotExceeded && neededCurrentAvailableBelowMaximum)) {
           PowerSchedulerMutex lock(powerSchedulerMutex);
           if (lock.isLocked() == false) {
             return false;
@@ -148,7 +151,7 @@ bool PowerScheduler::waitForCurrentAllowance(
     }
     if (notificationStatus == pdFALSE) {
       // We waited long enough...
-      // Remove the task from the list of waiting tasks
+      // Give up and remove the task from the list of waiting tasks
       PowerSchedulerMutex lock(powerSchedulerMutex);
       if (lock.isLocked() == false) {
         return false;
@@ -178,8 +181,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 =
-        getNextExpiringAllowance();
+    PowerScheduler::CurrentAllowance *nextAllowance = getNextAllowance();
     if (nextAllowance != nullptr) {
       xTaskNotify(nextAllowance->taskHandle,
                   PowerScheduler::PowerWakeupReasons::POWER_AVAILABLE,
@@ -237,26 +239,40 @@ PowerScheduler::getCurrentAllowance(TaskHandle_t taskHandle) {
   }
   return nullptr;
 }
-PowerScheduler::CurrentAllowance *
-PowerScheduler::getNextExpiringAllowance(void) {
-  PowerSchedulerMutex lock(powerSchedulerMutex);
+PowerScheduler::CurrentAllowance *PowerScheduler::getNextAllowance(void) {
   TickType_t minTicks = UINT32_MAX;
   CurrentAllowance *nextAllowance = nullptr;
-  if (lock.isLocked() == false) {
-    return nullptr;
-  }
+
+  std::vector<CurrentAllowance> sortedAllowances;
+  sortedAllowances.reserve(currentAllowances.size());
   for (auto &allowance : currentAllowances) {
     if (!(allowance.granted)) {
-      TickType_t ticks =
-          allowance.requestedAt + pdMS_TO_TICKS(allowance.maxSlackTimeMs);
-      if (ticks < minTicks) {
-        minTicks = ticks;
-        nextAllowance = &allowance;
+      sortedAllowances.push_back(allowance);
     }
   }
+  // Sort the not yet granted requests by how much time they have left before
+  // expiring
+  std::sort(sortedAllowances.begin(), sortedAllowances.end(),
+            [](const CurrentAllowance &a, const CurrentAllowance &b) {
+              return a.requestedAt + pdMS_TO_TICKS(a.maxSlackTimeMs) <
+                     b.requestedAt + pdMS_TO_TICKS(b.maxSlackTimeMs);
+            });
+
+  // Get the first one whose power requirement can be met
+  for (CurrentAllowance &allowance : sortedAllowances) {
+    const float existingConsumerConsumption =
+        getConsumerCurrent(allowance.consumer);
+    const bool currentAvailableBelowMaximum =
+        this->freeMaximumCurrentBudget + existingConsumerConsumption >=
+        allowance.neededCurrent;
+    const bool currentInsignificant =
+        allowance.neededCurrent < PowerParameters::CURRENT_INSIGNIFICANT;
+    if (currentInsignificant || currentAvailableBelowMaximum) {
+      return &allowance;
+    }
   }
   // Will be nullptr if no allowance was found
-  return nextAllowance;
+  return nullptr;
 }
 
 PowerScheduler &PowerScheduler::getPowerScheduler(float i_limit_ma,

src/power/PowerScheduler.h

@@ -147,7 +147,7 @@ public:
   CurrentAllowance *getCurrentAllowance(TaskHandle_t taskHandle);
 
   // @brief Retrieve the allowance that will expire next
-  CurrentAllowance *getNextExpiringAllowance(void);
+  CurrentAllowance *getNextAllowance(void);
 
 protected:
   // Current above which there will be no new scheduling