diff --git a/src/Dezibot.cpp b/src/Dezibot.cpp index 2282813..d03f3bc 100644 --- a/src/Dezibot.cpp +++ b/src/Dezibot.cpp @@ -9,7 +9,9 @@ Dezibot::Dezibot() : multiColorLight() {}; 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 53e7ddc..638484c 100644 --- a/src/power/Power.cpp +++ b/src/power/Power.cpp @@ -8,13 +8,31 @@ */ #include "Power.h" -static portMUX_TYPE mux; + +void vTaskUpdatePowerState(void *pvParameters) { + for (;;) { + ESP_LOGV(TAG, "Updating Power State..."); + Power::updatePowerStateHandler(); + vTaskDelay(pdMS_TO_TICKS(10)); + } +} void Power::begin() { // Check if another instance of us already initialized the power scheduler, // if not, we will do it. + ESP_LOGI(TAG, "Initializing Power Management"); + if (powerScheduler == nullptr) { - powerScheduler = &PowerScheduler::getPowerScheduler(); + ESP_LOGI(TAG, "Creating Power Scheduler"); + powerScheduler = &PowerScheduler::getPowerScheduler( + PowerParameters::Battery::CELL_CURRENT_1C_MA, + PowerParameters::Battery::CELL_CURRENT_2C_MA); + Power::recalculateCurrentBudgets(); + Power::updatePowerStateHandler(); + TaskHandle_t xHandle = NULL; + xTaskCreate(vTaskUpdatePowerState, "vTaskPowerStateUpdate", 4096, NULL, + tskIDLE_PRIORITY, &xHandle); + configASSERT(xHandle); if (!(powerScheduler->tryAccquireCurrentAllowance( PowerParameters::PowerConsumers::ESP, @@ -71,20 +89,22 @@ float Power::getConsumerCurrent(PowerParameters::PowerConsumers consumer) { float Power::getBatteryVoltage() { // Get the battery voltage from the ADC and convert it to a voltage // using the voltage divider. - portENTER_CRITICAL(&mux); + pinMode(PowerParameters::PinConfig::BAT_ADC_EN, OUTPUT); + pinMode(PowerParameters::PinConfig::BAT_ADC, INPUT); // Enable voltage divider digitalWrite(PowerParameters::PinConfig::BAT_ADC_EN, HIGH); + // Allow voltage to stabilize + delayMicroseconds(10); // Returns value between 0 and 4095 mapping to between 0 and 3.3V - uint16_t batteryAdcValue = - analogRead(PowerParameters::PinConfig::BAT_ADC) * - (PowerParameters::Battery::BAT_ADC::VOLTAGE_DIVIDER_FACTOR); + uint16_t batteryAdcValue = analogRead(PowerParameters::PinConfig::BAT_ADC); // Disable voltage divider digitalWrite(PowerParameters::PinConfig::BAT_ADC_EN, LOW); - portEXIT_CRITICAL(&mux); // Convert ADC value to voltage float batteryVoltage = - (batteryAdcValue * 3.3 / 4095) * + (batteryAdcValue / 4095.0 * 3.3) * PowerParameters::Battery::BAT_ADC::VOLTAGE_DIVIDER_FACTOR; + ESP_LOGD(TAG, "Battery ADC value: %d, Calculated voltage: %.2f", + batteryAdcValue, batteryVoltage); return batteryVoltage; } @@ -168,13 +188,14 @@ void Power::updatePowerStateHandler() { // Update the available current (changes based on battery state of charge) powerScheduler->recalculateCurrentBudgets(); - + ESP_LOGV(TAG, "Current: %f mA, Charge: %f Coulombs, %d %%", currentCurrent, + coloumbsRemaining, percentRemaining); return; } float Power::getMax3V3Current() { float u_bat = getBatteryVoltage(); - float i_bat = PowerParameters::Battery::CELL_CURRENT_1C; + float i_bat = PowerParameters::Battery::CELL_CURRENT_1C_MA; float eta = PowerParameters::BUCK_BOOST_EFFICIENCY; constexpr float u_3v3 = 3.3; return (u_bat * i_bat * eta) / u_3v3; @@ -186,11 +207,19 @@ void Power::addSoCSample(float soc) { lastSOC[latestSoCIndex] = soc; } +void Power::initPowerState(void) { + // Initialize the power state + lastPowerStateUpdate = xTaskGetTickCount(); + // TODO: Get initial battery charge state based on voltage, set coloumbs based + // on that +} + +int Power::latestSoCIndex = 0; +float Power::lastSOC[PowerParameters::Battery::AVERAGING_SAMPLES] = {0}; +TickType_t Power::lastPowerStateUpdate = 0; +float Power::coloumbsRemaining = + PowerParameters::Battery::CELL_CHARGE_FULL_COLOUMB; +int Power::percentRemaining = 100.0; PowerScheduler *Power::powerScheduler = nullptr; -Power::Power() { - // Initialize the power scheduler - powerScheduler = &PowerScheduler::getPowerScheduler(); - // Initialize the mutex - mux = portMUX_INITIALIZER_UNLOCKED; -} \ No newline at end of file +Power::Power() {} \ No newline at end of file diff --git a/src/power/Power.h b/src/power/Power.h index 11685b7..daebe8d 100644 --- a/src/power/Power.h +++ b/src/power/Power.h @@ -88,12 +88,14 @@ public: /// @return available current in milliamps static float getMax3V3Current(); + /// @brief update Power State + /// @note needs to be public for task creation + static void updatePowerStateHandler(); + protected: /// @brief PowerScheduler instance to manage power consumption static PowerScheduler *powerScheduler; - /// @brief update Power State - static void updatePowerStateHandler(); /* * Power State @@ -115,6 +117,9 @@ protected: /// @brief Add calculated value to circular array, pushing out oldest value static void addSoCSample(float soc); + + /// @brief initialize the power state + static void initPowerState(void); }; extern Power power; diff --git a/src/power/PowerParameters.h b/src/power/PowerParameters.h index 0cd3eb8..6da3bd9 100644 --- a/src/power/PowerParameters.h +++ b/src/power/PowerParameters.h @@ -33,8 +33,8 @@ namespace PowerParameters { static constexpr float CELL_CHARGE_FULL_COLOUMB = CELL_CAPACITY_MAH * 3.6; static constexpr float CELL_ENERGY_FULL_JOULES = CELL_CAPACITY_MAH * CELL_VOLTAGE_NOMINAL * 3.6; - static constexpr float CELL_CURRENT_1C = CELL_CAPACITY_MAH; - static constexpr float CELL_CURRENT_2C = CELL_CAPACITY_MAH * 2; + static constexpr float CELL_CURRENT_1C_MA = CELL_CAPACITY_MAH; + static constexpr float CELL_CURRENT_2C_MA = CELL_CAPACITY_MAH * 2; struct BAT_ADC { static constexpr float VOLTAGE_DIVIDER_R12 = 27e3; static constexpr float VOLTAGE_DIVIDER_R13 = 10e3; diff --git a/src/power/PowerScheduler.cpp b/src/power/PowerScheduler.cpp index 5d59e77..e2acd1c 100644 --- a/src/power/PowerScheduler.cpp +++ b/src/power/PowerScheduler.cpp @@ -14,9 +14,8 @@ #include "Power.h" bool PowerScheduler::tryAccquireCurrentAllowance( - PowerParameters::PowerConsumers consumer, uint16_t neededCurrent, + PowerParameters::PowerConsumers consumer, float neededCurrent, uint16_t requestedDurationMs) { - portENTER_CRITICAL(&mux); float existingConsumption = getConsumerCurrent(consumer); const bool currentAvailableBelowLimit = this->freeLimitCurrentBudget + existingConsumption > 0; @@ -38,15 +37,19 @@ bool PowerScheduler::tryAccquireCurrentAllowance( .grantedAt = xTaskGetTickCount(), .granted = true}); this->recalculateCurrentBudgets(); - portEXIT_CRITICAL(&mux); return true; } else { + ESP_LOGI(TAG, + "Task %p denied %f mA of power;" + "currently allocated: %f mA;" + "total available (2C): %f mA", + xTaskGetCurrentTaskHandle(), neededCurrent, getCurrentCurrent(), + maximumCurrent); return false; } } void PowerScheduler::releaseCurrent(PowerParameters::PowerConsumers consumer) { - portENTER_CRITICAL(&mux); for (auto it = currentAllowances.begin(); it != currentAllowances.end(); ++it) { if (it->consumer == consumer) { @@ -55,13 +58,12 @@ void PowerScheduler::releaseCurrent(PowerParameters::PowerConsumers consumer) { } } recalculateCurrentBudgets(); - portEXIT_CRITICAL(&mux); // Check if there are tasks waiting for power checkWaitingTasks(); } bool PowerScheduler::waitForCurrentAllowance( - PowerParameters::PowerConsumers consumer, uint16_t neededCurrent, + PowerParameters::PowerConsumers consumer, float neededCurrent, uint16_t maxSlackTimeMs, uint16_t requestedDurationMs) { if (tryAccquireCurrentAllowance(consumer, neededCurrent, requestedDurationMs)) { @@ -98,7 +100,7 @@ bool PowerScheduler::waitForCurrentAllowance( const bool currentAvailableBelowMaximum = this->freeMaximumCurrentBudget + existingConsumption >= neededCurrent; - const bool currentIsInsignificant = neededCurrent < 0.1; + const bool currentIsInsignificant = neededCurrent < 1; if (currentIsInsignificant || (currentAvailableBelowLimit && currentAvailableBelowMaximum)) { // TODO Check if there is a currently active allowance for this @@ -137,6 +139,12 @@ bool PowerScheduler::waitForCurrentAllowance( break; } } + ESP_LOGI(TAG, + "Task %p timed out waiting for %f mA of power;" + "currently allocated: %f mA;" + "total available (2C): %f mA", + xTaskGetCurrentTaskHandle(), neededCurrent, getCurrentCurrent(), + maximumCurrent); return false; } else { // Should be impossible to reach @@ -161,7 +169,9 @@ 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; for (auto &allowance : currentAllowances) { if (allowance.granted) { @@ -171,6 +181,8 @@ void PowerScheduler::recalculateCurrentBudgets(void) { } this->freeLimitCurrentBudget = tempFreeLimitCurrentBudget; this->freeMaximumCurrentBudget = tempFreeMaximumCurrentBudget; + ESP_LOGV(TAG, "Current budgets recalculated: %f mA, %f mA", + this->freeLimitCurrentBudget, this->freeMaximumCurrentBudget); } PowerScheduler::CurrentAllowance * @@ -214,11 +226,9 @@ PowerScheduler &PowerScheduler::getPowerScheduler(float i_limit_ma, if (powerSchedulerInstance == nullptr) { // Double check locking // https://www.aristeia.com/Papers/DDJ%5FJul%5FAug%5F2004%5Frevised.pdf - taskENTER_CRITICAL(&mux); if (powerSchedulerInstance == nullptr) { powerSchedulerInstance = new PowerScheduler(i_limit_ma, i_max_ma); } - taskEXIT_CRITICAL(&mux); } return *powerSchedulerInstance; } @@ -256,9 +266,6 @@ PowerScheduler::PowerScheduler(float i_limit_ma, float i_max_ma) { this->limitCurrent = i_limit_ma; this->maximumCurrent = i_max_ma; this->currentAllowances = std::vector(); - mux = portMUX_INITIALIZER_UNLOCKED; } -portMUX_TYPE PowerScheduler::mux = portMUX_INITIALIZER_UNLOCKED; - PowerScheduler::~PowerScheduler() {} \ No newline at end of file diff --git a/src/power/PowerScheduler.h b/src/power/PowerScheduler.h index 636fbb1..f3fad60 100644 --- a/src/power/PowerScheduler.h +++ b/src/power/PowerScheduler.h @@ -49,7 +49,7 @@ public: /// 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, - uint16_t neededcurrent, + float neededcurrent, uint16_t requestedDurationMs = 0); /// @brief "Return" the current currently allocated to a consumer /// @param consumer the active consumer to release the current for @@ -64,7 +64,7 @@ public: /// available /// @return whether the power could be successfully allocatedy bool waitForCurrentAllowance(PowerParameters::PowerConsumers consumer, - uint16_t neededCurrent, + float neededCurrent, uint16_t maxSlackTimeMs = DEFAULT_SLACK_TIME_MS, uint16_t requestedDurationMs = 0); /// @brief Put the ESP32 into deep sleep mode, without a method to wake up @@ -81,7 +81,7 @@ public: uint16_t maxSlackTimeMs; uint16_t requestedDurationMs; TaskHandle_t taskHandle; - uint16_t neededCurrent; + float neededCurrent; TickType_t requestedAt; TickType_t grantedAt; bool granted; @@ -122,7 +122,6 @@ protected: void checkWaitingTasks(void); // @brief Mutex to protect the power scheduler from concurrent access - static portMUX_TYPE mux; std::vector currentAllowances; };