diff --git a/src/power/PowerScheduler.cpp b/src/power/PowerScheduler.cpp index 396e84b..beb3d5d 100644 --- a/src/power/PowerScheduler.cpp +++ b/src/power/PowerScheduler.cpp @@ -16,20 +16,23 @@ bool PowerScheduler::tryAccquireCurrentAllowance( PowerParameters::PowerConsumers consumer, float neededCurrent, uint16_t requestedDurationMs) { - float existingConsumption = getConsumerCurrent(consumer); - const bool currentAvailableBelowLimit = + float existingConsumption = getConsumerCurrent(consumer); + 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)) { + const bool currentIsInsignificant = + neededCurrent < PowerParameters::CURRENT_INSIGNIFICANT; + if (currentIsInsignificant || + (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,33 +239,47 @@ 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 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, float i_max_ma) { if (powerSchedulerInstance == nullptr) { - powerSchedulerInstance = new PowerScheduler(i_limit_ma, i_max_ma); - } + powerSchedulerInstance = new PowerScheduler(i_limit_ma, i_max_ma); + } return *powerSchedulerInstance; } diff --git a/src/power/PowerScheduler.h b/src/power/PowerScheduler.h index aa7e66f..92127b9 100644 --- a/src/power/PowerScheduler.h +++ b/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