From 264e37c983fe393afcc02631ad9b40078b171c9f Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Phillip=20K=C3=BChne?= Date: Fri, 14 Feb 2025 12:00:31 +0100 Subject: [PATCH] Add RAII-wrapped Mutexes for access to state-tracking variables --- src/Dezibot.cpp | 1 - src/power/Power.cpp | 148 +++++++++++++++++++++++++++-------- src/power/Power.h | 74 ++++++++++++++++-- src/power/PowerScheduler.cpp | 75 ++++++++++++++---- src/power/PowerScheduler.h | 48 +++++++++++- 5 files changed, 287 insertions(+), 59 deletions(-) diff --git a/src/Dezibot.cpp b/src/Dezibot.cpp index d03f3bc..2576f9f 100644 --- a/src/Dezibot.cpp +++ b/src/Dezibot.cpp @@ -11,7 +11,6 @@ void Dezibot::begin(void) { ESP_LOGI("Dezibot", "Initializing Dezibot"); power.begin(); - delay(10); Wire.begin(SDA_PIN, SCL_PIN); infraredLight.begin(); lightDetection.begin(); diff --git a/src/power/Power.cpp b/src/power/Power.cpp index 3525453..45bd6ed 100644 --- a/src/power/Power.cpp +++ b/src/power/Power.cpp @@ -9,6 +9,8 @@ #include "Power.h" +SemaphoreHandle_t Power::powerMutex = NULL; + void vTaskUpdatePowerState(void *pvParameters) { for (;;) { ESP_LOGV(TAG, "Updating Power State..."); @@ -22,6 +24,15 @@ void Power::begin() { // if not, we will do it. ESP_LOGI(TAG, "Initializing Power Management"); + // Create mutex if it doesn't exist + if (powerMutex == NULL) { + powerMutex = xSemaphoreCreateMutex(); + if (powerMutex == NULL) { + ESP_LOGE(TAG, "Failed to create power mutex"); + Serial.println("Failed to create power mutex"); + } + } + if (powerScheduler == nullptr) { ESP_LOGI(TAG, "Creating Power Scheduler"); powerScheduler = &PowerScheduler::getPowerScheduler( @@ -149,29 +160,38 @@ void Power::updatePowerStateHandler() { int referenceCurrentMa = PowerParameters::Battery::DISCHARGE_CURVE::REFERENCE_CURRENT_A * 1000; + float coloumbsConsumedSinceLastUpdate; + // 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; + if (!busPowered) { + 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(); + if (!busPowered) { + if ((currentCurrent > (referenceCurrentMa * 0.6)) && + (currentCurrent < (referenceCurrentMa * 1.4))) { + // Get battery charge state from voltage curve + chargeState = getBatteryVoltageChargePercent(); + } else { + // Estimate battery charge state from charge consumption + float oldChargeState = lastSOC[latestSoCIndex]; + chargeState = oldChargeState - + ((coloumbsConsumedSinceLastUpdate / + PowerParameters::Battery::CELL_CHARGE_FULL_COLOUMB) * + 100); + } } else { - // Estimate battery charge state from charge consumption - float oldChargeState = lastSOC[latestSoCIndex]; - chargeState = - oldChargeState - ((coloumbsConsumedSinceLastUpdate / - PowerParameters::Battery::CELL_CHARGE_FULL_COLOUMB) * - 100); + // If we are charging, we can't estimate the charge state based on current + // consumption + chargeState = getBatteryVoltageChargePercent(); } addSoCSample(chargeState); @@ -190,10 +210,16 @@ void Power::updatePowerStateHandler() { powerScheduler->recalculateCurrentBudgets(); ESP_LOGV(TAG, "Current: %f mA, Charge: %f Coulombs, %d %%", currentCurrent, coloumbsRemaining, percentRemaining); + + // Update supply and charge state flags + busPowered = digitalRead(PowerParameters::PinConfig::VUSB_SENS); + chargingState = digitalRead(PowerParameters::PinConfig::BAT_CHG_STAT); + return; } float Power::getMax3V3Current() { + // Conversion from Thesis float u_bat = getBatteryVoltage(); float i_bat = PowerParameters::Battery::CELL_CURRENT_1C_MA; float eta = PowerParameters::BUCK_BOOST_EFFICIENCY; @@ -202,6 +228,11 @@ float Power::getMax3V3Current() { } void Power::addSoCSample(float soc) { + PowerMutex lock(powerMutex); + if (!lock.isLocked()) { + ESP_LOGE(TAG, "Could not take power to add SoC sample"); + return; + } latestSoCIndex = (latestSoCIndex + 1) % PowerParameters::Battery::AVERAGING_SAMPLES; lastSOC[latestSoCIndex] = soc; @@ -221,6 +252,11 @@ void Power::initPowerState(void) { constexpr float fullColoumbs = PowerParameters::Battery::CELL_CHARGE_FULL_COLOUMB; percentRemaining = initialChargePercentages; + // Set up flags and pins for them + pinMode(PowerParameters::PinConfig::VUSB_SENS, INPUT); + pinMode(PowerParameters::PinConfig::BAT_CHG_STAT, INPUT_PULLUP); + busPowered = digitalRead(PowerParameters::PinConfig::VUSB_SENS); + chargingState = digitalRead(PowerParameters::PinConfig::BAT_CHG_STAT); } void Power::dumpPowerStatistics() { @@ -228,31 +264,69 @@ void Power::dumpPowerStatistics() { Serial.printf("Current: %f mA\r\n", Power::getCurrentCurrent()); Serial.printf("Battery Voltage: %f V\r\n", Power::getBatteryVoltage()); Serial.printf("Battery Charge: %f %%\r\n", Power::getBatteryChargePercent()); - Serial.printf("Battery Charge: %f Coulombs\r\n", Power::getBatteryChargeCoulombs()); - Serial.printf("Max 3.3V Current in this state: %f mA\r\n", Power::getMax3V3Current()); + Serial.printf("Battery Charge: %f Coulombs\r\n", + Power::getBatteryChargeCoulombs()); + Serial.printf("Max 3.3V Current in this state (1C, 2C): %f mA, %f mA \r\n", + Power::getMax3V3Current(), Power::getMax3V3Current() * 2); Serial.printf("=========================================\r\n"); } void Power::dumpConsumerStatistics() { Serial.printf("======== Dezibot Consumer Statistics ========\r\n"); - Serial.printf("ESP: %f mA\r\n", Power::getConsumerCurrent(PowerParameters::PowerConsumers::ESP)); - Serial.printf("WIFI: %f mA\r\n", Power::getConsumerCurrent(PowerParameters::PowerConsumers::WIFI)); - Serial.printf("LED_RGB_TOP_LEFT: %f mA\r\n", Power::getConsumerCurrent(PowerParameters::PowerConsumers::LED_RGB_TOP_LEFT)); - Serial.printf("LED_RGB_TOP_RIGHT: %f mA\r\n", Power::getConsumerCurrent(PowerParameters::PowerConsumers::LED_RGB_TOP_RIGHT)); - Serial.printf("LED_RGB_BOTTOM: %f mA\r\n", Power::getConsumerCurrent(PowerParameters::PowerConsumers::LED_RGB_BOTTOM)); - Serial.printf("RGBW_SENSOR: %f mA\r\n", Power::getConsumerCurrent(PowerParameters::PowerConsumers::RGBW_SENSOR)); - Serial.printf("LED_IR_BOTTOM: %f mA\r\n", Power::getConsumerCurrent(PowerParameters::PowerConsumers::LED_IR_BOTTOM)); - Serial.printf("LED_IR_FRONT: %f mA\r\n", Power::getConsumerCurrent(PowerParameters::PowerConsumers::LED_IR_FRONT)); - Serial.printf("PT_IR: %f mA\r\n", Power::getConsumerCurrent(PowerParameters::PowerConsumers::PT_IR)); - Serial.printf("PT_DL: %f mA\r\n", Power::getConsumerCurrent(PowerParameters::PowerConsumers::PT_DL)); - Serial.printf("LED_UV: %f mA\r\n", Power::getConsumerCurrent(PowerParameters::PowerConsumers::LED_UV)); - Serial.printf("DISPLAY_OLED: %f mA\r\n", Power::getConsumerCurrent(PowerParameters::PowerConsumers::DISPLAY_OLED)); - Serial.printf("MOTOR_LEFT: %f mA\r\n", Power::getConsumerCurrent(PowerParameters::PowerConsumers::MOTOR_LEFT)); - Serial.printf("MOTOR_RIGHT: %f mA\r\n", Power::getConsumerCurrent(PowerParameters::PowerConsumers::MOTOR_RIGHT)); - Serial.printf("IMU: %f mA\r\n", Power::getConsumerCurrent(PowerParameters::PowerConsumers::IMU)); + Serial.printf("ESP: %f mA\r\n", Power::getConsumerCurrent( + PowerParameters::PowerConsumers::ESP)); + Serial.printf("WIFI: %f mA\r\n", Power::getConsumerCurrent( + PowerParameters::PowerConsumers::WIFI)); + Serial.printf("LED_RGB_TOP_LEFT: %f mA\r\n", + Power::getConsumerCurrent( + PowerParameters::PowerConsumers::LED_RGB_TOP_LEFT)); + Serial.printf("LED_RGB_TOP_RIGHT: %f mA\r\n", + Power::getConsumerCurrent( + PowerParameters::PowerConsumers::LED_RGB_TOP_RIGHT)); + Serial.printf("LED_RGB_BOTTOM: %f mA\r\n", + Power::getConsumerCurrent( + PowerParameters::PowerConsumers::LED_RGB_BOTTOM)); + Serial.printf( + "RGBW_SENSOR: %f mA\r\n", + Power::getConsumerCurrent(PowerParameters::PowerConsumers::RGBW_SENSOR)); + Serial.printf("LED_IR_BOTTOM: %f mA\r\n", + Power::getConsumerCurrent( + PowerParameters::PowerConsumers::LED_IR_BOTTOM)); + Serial.printf( + "LED_IR_FRONT: %f mA\r\n", + Power::getConsumerCurrent(PowerParameters::PowerConsumers::LED_IR_FRONT)); + Serial.printf( + "PT_IR: %f mA\r\n", + Power::getConsumerCurrent(PowerParameters::PowerConsumers::PT_IR)); + Serial.printf( + "PT_DL: %f mA\r\n", + Power::getConsumerCurrent(PowerParameters::PowerConsumers::PT_DL)); + Serial.printf( + "LED_UV: %f mA\r\n", + Power::getConsumerCurrent(PowerParameters::PowerConsumers::LED_UV)); + Serial.printf( + "DISPLAY_OLED: %f mA\r\n", + Power::getConsumerCurrent(PowerParameters::PowerConsumers::DISPLAY_OLED)); + Serial.printf( + "MOTOR_LEFT: %f mA\r\n", + Power::getConsumerCurrent(PowerParameters::PowerConsumers::MOTOR_LEFT)); + Serial.printf( + "MOTOR_RIGHT: %f mA\r\n", + Power::getConsumerCurrent(PowerParameters::PowerConsumers::MOTOR_RIGHT)); + Serial.printf("IMU: %f mA\r\n", Power::getConsumerCurrent( + PowerParameters::PowerConsumers::IMU)); Serial.printf("=============================================\r\n"); } +bool Power::isUSBPowered() { return busPowered; } + +bool Power::isBatteryPowered() { return !busPowered; } + +bool Power::isBatteryCharging() { return chargingState && busPowered; } + +bool Power::isBatteryDischarging() { return !chargingState && !busPowered; } + +bool Power::isBatteryFullyCharged() { return !chargingState && busPowered; } int Power::latestSoCIndex = 0; float Power::lastSOC[PowerParameters::Battery::AVERAGING_SAMPLES] = {0}; TickType_t Power::lastPowerStateUpdate = 0; @@ -261,4 +335,10 @@ float Power::coloumbsRemaining = float Power::percentRemaining = 100.0; PowerScheduler *Power::powerScheduler = nullptr; -Power::Power() {} \ No newline at end of file +bool Power::busPowered = false; + +bool Power::chargingState = false; + +Power::Power() {} + +Power::~Power() {} \ No newline at end of file diff --git a/src/power/Power.h b/src/power/Power.h index 90b3e2e..343d078 100644 --- a/src/power/Power.h +++ b/src/power/Power.h @@ -8,6 +8,12 @@ */ #include "PowerScheduler.h" +#include "esp_adc/adc_cali.h" +#include "esp_adc/adc_cali_scheme.h" +#include "driver/gpio.h" +#include "driver/adc.h" +#include "esp_adc/adc_oneshot.h" +#include "freertos/semphr.h" #ifndef Power_h #define Power_h @@ -17,6 +23,33 @@ enum TaskResumptionReason { POWER_AVAILABLE, TIMEOUT }; class Power { +private: + static SemaphoreHandle_t powerMutex; + static constexpr uint16_t MUTEX_TIMEOUT_MS = 1; + + // RAII for mutex + class PowerMutex { + private: + SemaphoreHandle_t &mutex; + bool locked; + + public: + PowerMutex(SemaphoreHandle_t &mutex) : mutex(mutex), locked(false) { + locked = + (xSemaphoreTake(mutex, pdMS_TO_TICKS(MUTEX_TIMEOUT_MS)) == pdTRUE); + if (!locked) { + ESP_LOGW(TAG, "Could not take power mutex"); + } + } + + ~PowerMutex() { + if (locked) { + xSemaphoreGive(mutex); + } + } + bool isLocked() { return locked; } + }; + protected: /// @brief PowerScheduler instance to manage power consumption static PowerScheduler *powerScheduler; @@ -34,6 +67,10 @@ protected: /// @brief remaining Charge in percent static float percentRemaining; + static bool busPowered; + + static bool chargingState; + /// @brief Circular array of last calculated values for current state of /// charge static float lastSOC[PowerParameters::Battery::AVERAGING_SAMPLES]; @@ -48,7 +85,8 @@ protected: public: static void begin(void); Power(); - /// @brief Get the current free current budget (to C1 discharge) + ~Power(); + /// @brief Get the current free current budget (to C1 discharge) /// @return the amount of power that is currently available (in mA) static float getFreeLimitCurrentBudget(void); /// @brief Get the current hard maximum free current (to C2 discharge) @@ -63,8 +101,8 @@ public: /// @return whether the current could be successfully allocated static bool tryAccquireCurrentAllowance(PowerParameters::PowerConsumers consumer, - uint16_t neededcurrent, - uint16_t requestedDurationMs = 0); + uint16_t neededcurrent, + uint16_t requestedDurationMs = 0); /// @brief "Return" the current currently allocated to a consumer /// @param consumer the active consumer to release the current for static void releaseCurrent(PowerParameters::PowerConsumers consumer); @@ -77,9 +115,9 @@ public: /// available /// @return whether the power could be successfully allocatedy static bool waitForCurrentAllowance(PowerParameters::PowerConsumers consumer, - uint16_t neededCurrent, - uint16_t maxSlackTimeMs, - uint16_t requestedDurationMs); + uint16_t neededCurrent, + uint16_t maxSlackTimeMs, + uint16_t requestedDurationMs); /// @brief Put the ESP32 into deep sleep mode, without a method to wake up /// again. Basically this is a shutdown. static void beginPermanentDeepSleep(void); @@ -88,7 +126,8 @@ public: /// @return the amount of power that is currently allocated (in mA) static float getCurrentCurrent(void); - // @brief Responsible for recalculating the current budgets + /// @brief Responsible for recalculating the current budgets + /// @note these change based on the current state of charge static void recalculateCurrentBudgets(void); // @brief Get current consumption of a consumer @@ -125,6 +164,27 @@ public: /// @brief dump consumer statistics to serial static void dumpConsumerStatistics(); + + /// @brief get wether power is supplied via USB + /// @return true if power is supplied via USB + static bool isUSBPowered(); + + /// @brief get wether power is supplied via battery + /// @return true if power is supplied via battery + static bool isBatteryPowered(); + + /// @brief get wether the battery is currently charging + /// @return true if the battery is charging + static bool isBatteryCharging(); + + /// @brief get wether the battery is currently discharging + /// @return true if the battery is discharging + static bool isBatteryDischarging(); + + /// @brief get wether the battery is currently fully charged + /// @return true if the battery is fully charged + static bool isBatteryFullyCharged(); + }; extern Power power; diff --git a/src/power/PowerScheduler.cpp b/src/power/PowerScheduler.cpp index d0fb26e..c82bd24 100644 --- a/src/power/PowerScheduler.cpp +++ b/src/power/PowerScheduler.cpp @@ -16,17 +16,22 @@ bool PowerScheduler::tryAccquireCurrentAllowance( PowerParameters::PowerConsumers consumer, float neededCurrent, uint16_t requestedDurationMs) { - float existingConsumption = getConsumerCurrent(consumer); - const bool currentAvailableBelowLimit = + float existingConsumption = getConsumerCurrent(consumer); + const bool currentAvailableBelowLimit = this->freeLimitCurrentBudget + existingConsumption > 0; - const bool currentAvailableBelowMaximum = + const bool currentAvailableBelowMaximum = this->freeMaximumCurrentBudget + existingConsumption >= neededCurrent; - const bool currentIsInsignificant = neededCurrent < 1; - if (currentIsInsignificant || - (currentAvailableBelowLimit && currentAvailableBelowMaximum)) { + const bool currentIsInsignificant = neededCurrent < 1; + if (currentIsInsignificant || + (currentAvailableBelowLimit && currentAvailableBelowMaximum)) { if (existingConsumption > 0) { releaseCurrent(consumer); } + PowerSchedulerMutex lock(powerSchedulerMutex); + if (lock.isLocked() == false) { + ESP_LOGE(TAG, "Failed to Acquire PowerScheduler Mutex during Current Allocation"); + return false; + } this->currentAllowances.push_back(PowerScheduler::CurrentAllowance{ .consumer = consumer, .maxSlackTimeMs = 0, @@ -37,6 +42,7 @@ bool PowerScheduler::tryAccquireCurrentAllowance( .grantedAt = xTaskGetTickCount(), .granted = true}); this->recalculateCurrentBudgets(); + lock.unlock(); return true; } else { ESP_LOGI(TAG, @@ -50,6 +56,10 @@ bool PowerScheduler::tryAccquireCurrentAllowance( } void PowerScheduler::releaseCurrent(PowerParameters::PowerConsumers consumer) { + PowerSchedulerMutex lock(powerSchedulerMutex); + if (lock.isLocked() == false) { + return; + } for (auto it = currentAllowances.begin(); it != currentAllowances.end(); ++it) { if (it->consumer == consumer) { @@ -57,6 +67,7 @@ void PowerScheduler::releaseCurrent(PowerParameters::PowerConsumers consumer) { break; } } + lock.unlock(); recalculateCurrentBudgets(); // Check if there are tasks waiting for power checkWaitingTasks(); @@ -81,8 +92,12 @@ bool PowerScheduler::waitForCurrentAllowance( .neededCurrent = neededCurrent, .requestedAt = initialTickCount, .granted = false}; - + PowerSchedulerMutex lock(powerSchedulerMutex); + if (lock.isLocked() == false) { + return false; + } this->currentAllowances.push_back(newAllowance); + lock.unlock(); uint32_t notificationValue; BaseType_t notificationStatus = xTaskNotifyWait( 0, 0, ¬ificationValue, pdMS_TO_TICKS(maxSlackTimeMs)); @@ -103,8 +118,10 @@ bool PowerScheduler::waitForCurrentAllowance( const bool currentIsInsignificant = neededCurrent < 1; if (currentIsInsignificant || (currentAvailableBelowLimit && currentAvailableBelowMaximum)) { - // TODO Check if there is a currently active allowance for this - // consumer and if so, replace it with the new one + PowerSchedulerMutex lock(powerSchedulerMutex); + if (lock.isLocked() == false) { + return false; + } if (existingConsumption > 0) { releaseCurrent(consumer); } @@ -132,6 +149,10 @@ bool PowerScheduler::waitForCurrentAllowance( if (notificationStatus == pdFALSE) { // We waited long enough... // Remove the task from the list of waiting tasks + PowerSchedulerMutex lock(powerSchedulerMutex); + if (lock.isLocked() == false) { + return false; + } for (auto it = currentAllowances.begin(); it != currentAllowances.end(); ++it) { if (it->consumer == consumer && it->requestedAt == initialTickCount) { @@ -168,11 +189,16 @@ void PowerScheduler::checkWaitingTasks(void) { } void PowerScheduler::recalculateCurrentBudgets(void) { + // Get the respective maximums and subtract currently flowing currents ESP_LOGI(TAG, "Recalculating current budgets..."); float tempFreeLimitCurrentBudget = Power::getMax3V3Current(); ESP_LOGI(TAG, "Got max 3V3 current: %.2f", tempFreeLimitCurrentBudget); float tempFreeMaximumCurrentBudget = Power::getMax3V3Current() * 2; + PowerSchedulerMutex lock(powerSchedulerMutex); + if (lock.isLocked() == false) { + return; + } for (auto &allowance : currentAllowances) { if (allowance.granted) { tempFreeLimitCurrentBudget -= allowance.neededCurrent; @@ -187,6 +213,10 @@ void PowerScheduler::recalculateCurrentBudgets(void) { PowerScheduler::CurrentAllowance * PowerScheduler::getCurrentAllowance(PowerParameters::PowerConsumers consumer) { + PowerSchedulerMutex lock(powerSchedulerMutex); + if (lock.isLocked() == false) { + return nullptr; + } for (auto &allowance : currentAllowances) { if (allowance.consumer == consumer) { return &allowance; @@ -196,6 +226,10 @@ PowerScheduler::getCurrentAllowance(PowerParameters::PowerConsumers consumer) { } PowerScheduler::CurrentAllowance * PowerScheduler::getCurrentAllowance(TaskHandle_t taskHandle) { + PowerSchedulerMutex lock(powerSchedulerMutex); + if (lock.isLocked() == false) { + return nullptr; + } for (auto &allowance : currentAllowances) { if (allowance.taskHandle == taskHandle) { return &allowance; @@ -205,8 +239,12 @@ PowerScheduler::getCurrentAllowance(TaskHandle_t taskHandle) { } PowerScheduler::CurrentAllowance * PowerScheduler::getNextExpiringAllowance(void) { + PowerSchedulerMutex lock(powerSchedulerMutex); TickType_t minTicks = UINT32_MAX; CurrentAllowance *nextAllowance = nullptr; + if (lock.isLocked() == false) { + return nullptr; + } for (auto &allowance : currentAllowances) { if (!(allowance.granted)) { TickType_t ticks = @@ -224,17 +262,17 @@ PowerScheduler::getNextExpiringAllowance(void) { PowerScheduler &PowerScheduler::getPowerScheduler(float i_limit_ma, float i_max_ma) { if (powerSchedulerInstance == nullptr) { - // Double check locking - // https://www.aristeia.com/Papers/DDJ%5FJul%5FAug%5F2004%5Frevised.pdf - if (powerSchedulerInstance == nullptr) { powerSchedulerInstance = new PowerScheduler(i_limit_ma, i_max_ma); } - } return *powerSchedulerInstance; } float PowerScheduler::getCurrentCurrent(void) { float currentSum = 0; + PowerSchedulerMutex lock(powerSchedulerMutex); + if (lock.isLocked() == false) { + return false; + } for (auto &allowance : currentAllowances) { if (allowance.granted) { currentSum += allowance.neededCurrent; @@ -253,6 +291,10 @@ float PowerScheduler::getFreeMaximumCurrentBudget(void) { float PowerScheduler::getConsumerCurrent( PowerParameters::PowerConsumers consumer) { + PowerSchedulerMutex lock(powerSchedulerMutex); + if (lock.isLocked() == false) { + return false; + } float currentSum = 0; for (auto &allowance : currentAllowances) { if (allowance.consumer == consumer && allowance.granted) { @@ -266,6 +308,11 @@ PowerScheduler::PowerScheduler(float i_limit_ma, float i_max_ma) { this->limitCurrent = i_limit_ma; this->maximumCurrent = i_max_ma; this->currentAllowances = std::vector(); + this->powerSchedulerMutex = xSemaphoreCreateMutex(); } -PowerScheduler::~PowerScheduler() {} \ No newline at end of file +PowerScheduler::~PowerScheduler() { + if (powerSchedulerMutex != NULL) { + vSemaphoreDelete(powerSchedulerMutex); + } +} \ No newline at end of file diff --git a/src/power/PowerScheduler.h b/src/power/PowerScheduler.h index f3fad60..c32e6a7 100644 --- a/src/power/PowerScheduler.h +++ b/src/power/PowerScheduler.h @@ -24,8 +24,50 @@ class PowerScheduler { private: static constexpr uint16_t DEFAULT_SLACK_TIME_MS = 100; + static constexpr uint16_t MUTEX_TIMEOUT_MS = 10; + SemaphoreHandle_t powerSchedulerMutex; PowerScheduler(float i_limit_ma, float i_max_ma); + // RAII for mutex + class PowerSchedulerMutex { + private: + SemaphoreHandle_t &mutex; + bool locked; + + public: + PowerSchedulerMutex(SemaphoreHandle_t &mutex) : mutex(mutex), locked(false) { + locked = + (xSemaphoreTake(mutex, pdMS_TO_TICKS(MUTEX_TIMEOUT_MS)) == pdTRUE); + if (!locked) { + ESP_LOGW(TAG, "Could not take powerScheduler mutex"); + } + } + + void unlock() { + if (locked) { + xSemaphoreGive(mutex); + locked = false; + } + } + + void lock() { + if (!locked) { + locked = (xSemaphoreTake(mutex, pdMS_TO_TICKS(MUTEX_TIMEOUT_MS)) == + pdTRUE); + if (!locked) { + ESP_LOGW(TAG, "Could not take power mutex"); + } + } + } + + ~PowerSchedulerMutex() { + if (locked) { + xSemaphoreGive(mutex); + } + } + bool isLocked() { return locked; } + }; + public: ~PowerScheduler(); /// @brief Initialize the singleton instance of the power manager @@ -45,9 +87,9 @@ public: /// @param neededCurrent the amount of current we want to be accounted for (in /// mA) /// @return whether the current could be successfully allocated - /// @note This takes existing power consumption by the same consumer into account, - /// so requesting the same or less power will always succeed. Also, small amounts - /// of power (below 1 mA) will always be granted. + /// @note This takes existing power consumption by the same consumer into + /// account, so requesting the same or less power will always succeed. Also, + /// small amounts of power (below 1 mA) will always be granted. bool tryAccquireCurrentAllowance(PowerParameters::PowerConsumers consumer, float neededcurrent, uint16_t requestedDurationMs = 0);