Fixes

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();

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 =
+  uint16_t batteryAdcValue = analogRead(PowerParameters::PinConfig::BAT_ADC);
-      analogRead(PowerParameters::PinConfig::BAT_ADC) *
-      (PowerParameters::Battery::BAT_ADC::VOLTAGE_DIVIDER_FACTOR);
   // 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() {
+Power::Power() {}
-  // Initialize the power scheduler
-  powerScheduler = &PowerScheduler::getPowerScheduler();
-  // Initialize the mutex
-  mux = portMUX_INITIALIZER_UNLOCKED;
-}

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;

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_1C_MA = CELL_CAPACITY_MAH;
-    static constexpr float CELL_CURRENT_2C = CELL_CAPACITY_MAH * 2;
+    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;

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<CurrentAllowance>();
-  mux = portMUX_INITIALIZER_UNLOCKED;
 }
 
-portMUX_TYPE PowerScheduler::mux = portMUX_INITIALIZER_UNLOCKED;
-
 PowerScheduler::~PowerScheduler() {}

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<PowerScheduler::CurrentAllowance> currentAllowances;
 };