Add power state initialisation

src/power/Power.cpp

@@ -27,8 +27,8 @@ void Power::begin() {
     powerScheduler = &PowerScheduler::getPowerScheduler(
         PowerParameters::Battery::CELL_CURRENT_1C_MA,
         PowerParameters::Battery::CELL_CURRENT_2C_MA);
+    Power::initPowerState();
     Power::recalculateCurrentBudgets();
-    Power::updatePowerStateHandler();
     TaskHandle_t xHandle = NULL;
     xTaskCreate(vTaskUpdatePowerState, "vTaskPowerStateUpdate", 4096, NULL,
                 tskIDLE_PRIORITY, &xHandle);
@@ -108,11 +108,11 @@ float Power::getBatteryVoltage() {
   return batteryVoltage;
 }
 
-int Power::getBatteryChargePercent() { return percentRemaining; }
+float Power::getBatteryChargePercent() { return percentRemaining; }
 
 float Power::getBatteryChargeCoulombs() { return coloumbsRemaining; }
 
-int Power::getBatteryVoltageChargePercent() {
+float Power::getBatteryVoltageChargePercent() {
   // Directly get the battery voltage and calculate the charge state based on
   // the discharge curve.
   float batteryVoltage = getBatteryVoltage();
@@ -166,9 +166,9 @@ void Power::updatePowerStateHandler() {
     // Get battery charge state from voltage curve
     chargeState = getBatteryVoltageChargePercent();
   } else {
-    // Calculate battery charge state from Charge consumption
+    // Estimate battery charge state from charge consumption
     float oldChargeState = lastSOC[latestSoCIndex];
-    float chargeState =
+    chargeState =
         oldChargeState - ((coloumbsConsumedSinceLastUpdate /
                            PowerParameters::Battery::CELL_CHARGE_FULL_COLOUMB) *
                           100);
@@ -212,6 +212,45 @@ void Power::initPowerState(void) {
   lastPowerStateUpdate = xTaskGetTickCount();
   // TODO: Get initial battery charge state based on voltage, set coloumbs based
   // on that
+  const float initialChargePercentages = getBatteryVoltageChargePercent();
+  for (int i = 0; i < PowerParameters::Battery::AVERAGING_SAMPLES; i++) {
+    lastSOC[i] = initialChargePercentages;
+  }
+  coloumbsRemaining = initialChargePercentages / 100 *
+                      PowerParameters::Battery::CELL_CHARGE_FULL_COLOUMB;
+  constexpr float fullColoumbs =
+      PowerParameters::Battery::CELL_CHARGE_FULL_COLOUMB;
+  percentRemaining = initialChargePercentages;
+}
+
+void Power::dumpPowerStatistics() {
+  Serial.printf("======== Dezibot Power Statistics ========\r\n");
+  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("=========================================\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("=============================================\r\n");
 }
 
 int Power::latestSoCIndex = 0;
@@ -219,7 +258,7 @@ 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;
+float Power::percentRemaining = 100.0;
 PowerScheduler *Power::powerScheduler = nullptr;
 
 Power::Power() {}

src/power/Power.h

@@ -17,6 +17,34 @@ enum TaskResumptionReason { POWER_AVAILABLE, TIMEOUT };
 
 class Power {
 
+protected:
+  /// @brief PowerScheduler instance to manage power consumption
+  static PowerScheduler *powerScheduler;
+
+  /*
+   * Power State
+   */
+
+  /// @brief last time of power state update
+  static TickType_t lastPowerStateUpdate;
+
+  /// @brief remaining Charge in coulombs
+  static float coloumbsRemaining;
+
+  /// @brief remaining Charge in percent
+  static float percentRemaining;
+
+  /// @brief Circular array of last calculated values for current state of
+  /// charge
+  static float lastSOC[PowerParameters::Battery::AVERAGING_SAMPLES];
+  static int latestSoCIndex;
+
+  /// @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);
+
 public:
   static void begin(void);
   Power();
@@ -30,10 +58,11 @@ public:
   /// @brief Request an allowance of a certain number of milliamperes from the
   /// power scheduler without waiting for it (meaning it will not be scheduled
   /// for future allocation). Only one can be active per consumer.
-  /// @param neededCurrent the amount of current we want to be accounted for (in
+  /// @param neededCurrent the amount of current we want to be accounted for
-  /// mA)
+  /// (in mA)
   /// @return whether the current could be successfully allocated
-  static bool tryAccquireCurrentAllowance(PowerParameters::PowerConsumers consumer,
+  static bool
+  tryAccquireCurrentAllowance(PowerParameters::PowerConsumers consumer,
                                    uint16_t neededcurrent,
                                    uint16_t requestedDurationMs = 0);
   /// @brief "Return" the current currently allocated to a consumer
@@ -65,19 +94,18 @@ public:
   // @brief Get current consumption of a consumer
   static float getConsumerCurrent(PowerParameters::PowerConsumers consumer);
 
-
   /// @brief Get battery voltage measurement.
   /// @return Battery Terminal Voltage in Volts
   static float getBatteryVoltage();
 
   /// @brief Get estimated battery charge state as percentage
   /// @return Battery charge state in percent
-  static int getBatteryChargePercent();
+  static float getBatteryChargePercent();
 
   /// @brief Get estimated battery charge state as percentage based on
   // voltage directly
   /// @return Battery charge state in percent
-  static int getBatteryVoltageChargePercent();
+  static float getBatteryVoltageChargePercent();
 
   /// @brief Get estimated battery charge state as coulombs
   /// @return Battery charge state in coulombs
@@ -92,34 +120,11 @@ public:
   /// @note needs to be public for task creation
   static void updatePowerStateHandler();
 
-protected:
+  /// @brief dump power statistics to serial
-  /// @brief PowerScheduler instance to manage power consumption
+  static void dumpPowerStatistics();
-  static PowerScheduler *powerScheduler;
 
-
+  /// @brief dump consumer statistics to serial
-  /*
+  static void dumpConsumerStatistics();
-   * Power State
-   */
-
-  /// @brief last time of power state update
-  static TickType_t lastPowerStateUpdate;
-
-  /// @brief remaining Charge in coulombs
-  static float coloumbsRemaining;
-
-  /// @brief remaining Charge in percent
-  static int percentRemaining;
-
-  /// @brief Circular array of last calculated values for current state of
-  /// charge
-  static float lastSOC[PowerParameters::Battery::AVERAGING_SAMPLES];
-  static int latestSoCIndex;
-
-  /// @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;