mirror of
https://gitlab.dit.htwk-leipzig.de/phillip.kuehne/dezibot.git
synced 2025-06-02 01:41:47 +02:00
363 lines
14 KiB
C++
363 lines
14 KiB
C++
/**
|
|
* @file Power.h
|
|
* @author Phillip Kühne
|
|
* @brief This component provides utilities for keeping track of power usage
|
|
* consumption.
|
|
* @version 0.1
|
|
* @date 2024-11-23
|
|
*/
|
|
|
|
#include "Power.h"
|
|
|
|
SemaphoreHandle_t Power::powerMutex = NULL;
|
|
|
|
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");
|
|
|
|
// 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(
|
|
PowerParameters::Battery::CELL_CURRENT_1C_MA,
|
|
PowerParameters::Battery::CELL_CURRENT_2C_MA);
|
|
Power::initPowerState();
|
|
Power::recalculateCurrentBudgets();
|
|
TaskHandle_t xHandle = NULL;
|
|
xTaskCreate(vTaskUpdatePowerState, "vTaskPowerStateUpdate", 4096, NULL,
|
|
tskIDLE_PRIORITY, &xHandle);
|
|
configASSERT(xHandle);
|
|
|
|
if (!(powerScheduler->tryAccquireCurrentAllowance(
|
|
PowerParameters::PowerConsumers::ESP,
|
|
PowerParameters::CurrentConsumptions::CURRENT_ESP_AVG))) {
|
|
ESP_LOGE(TAG, "Could not get power for ESP");
|
|
Serial.println("Could not get power for ESP");
|
|
}
|
|
}
|
|
}
|
|
|
|
float Power::getFreeLimitCurrentBudget() {
|
|
return powerScheduler->getFreeLimitCurrentBudget();
|
|
}
|
|
|
|
float Power::getFreeMaximumCurrentBudget() {
|
|
return powerScheduler->getFreeMaximumCurrentBudget();
|
|
}
|
|
|
|
bool Power::tryAccquireCurrentAllowance(
|
|
PowerParameters::PowerConsumers consumer, uint16_t neededCurrent,
|
|
uint16_t requestedDurationMs) {
|
|
return powerScheduler->tryAccquireCurrentAllowance(consumer, neededCurrent,
|
|
requestedDurationMs);
|
|
}
|
|
|
|
void Power::releaseCurrent(PowerParameters::PowerConsumers consumer) {
|
|
powerScheduler->releaseCurrent(consumer);
|
|
}
|
|
|
|
bool Power::waitForCurrentAllowance(PowerParameters::PowerConsumers consumer,
|
|
uint16_t neededCurrent,
|
|
uint16_t maxSlackTimeMs,
|
|
uint16_t requestedDurationMs) {
|
|
return powerScheduler->waitForCurrentAllowance(
|
|
consumer, neededCurrent, maxSlackTimeMs, requestedDurationMs);
|
|
}
|
|
|
|
void Power::beginPermanentDeepSleep(void) {
|
|
return powerScheduler->beginPermanentDeepSleep();
|
|
}
|
|
|
|
float Power::getCurrentCurrent(void) {
|
|
return powerScheduler->getCurrentCurrent();
|
|
}
|
|
|
|
float Power::getBatteryCurrent(void) {
|
|
const float i_3v3 = getCurrentCurrent();
|
|
const float u_3v3 = 3.3;
|
|
const float u_bat = getBatteryVoltage();
|
|
const float eta = PowerParameters::BUCK_BOOST_EFFICIENCY;
|
|
return (u_3v3 * i_3v3) / (u_bat * eta);
|
|
}
|
|
|
|
void Power::recalculateCurrentBudgets(void) {
|
|
return powerScheduler->recalculateCurrentBudgets();
|
|
}
|
|
|
|
float Power::getConsumerCurrent(PowerParameters::PowerConsumers consumer) {
|
|
return powerScheduler->getConsumerCurrent(consumer);
|
|
}
|
|
|
|
float Power::getBatteryVoltage() {
|
|
// Get the battery voltage from the ADC and convert it to a voltage
|
|
// using the voltage divider.
|
|
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);
|
|
// Disable voltage divider
|
|
digitalWrite(PowerParameters::PinConfig::BAT_ADC_EN, LOW);
|
|
// Convert ADC value to voltage
|
|
float batteryVoltage =
|
|
(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;
|
|
}
|
|
|
|
float Power::getBatteryChargePercent() { return percentRemaining; }
|
|
|
|
float Power::getBatteryChargeCoulombs() { return coloumbsRemaining; }
|
|
|
|
float Power::getBatteryVoltageChargePercent() {
|
|
// Directly get the battery voltage and calculate the charge state based on
|
|
// the discharge curve.
|
|
float batteryVoltage = getBatteryVoltage();
|
|
float chargeState = 0;
|
|
// Clamp edge cases
|
|
if (batteryVoltage >=
|
|
PowerParameters::Battery::DISCHARGE_CURVE::VOLTAGES[0]) {
|
|
return PowerParameters::Battery::DISCHARGE_CURVE::CHARGE_STATES[0];
|
|
}
|
|
if (batteryVoltage <=
|
|
PowerParameters::Battery::DISCHARGE_CURVE::VOLTAGES
|
|
[PowerParameters::Battery::DISCHARGE_CURVE::NUM_POINTS - 1]) {
|
|
return PowerParameters::Battery::DISCHARGE_CURVE::CHARGE_STATES
|
|
[PowerParameters::Battery::DISCHARGE_CURVE::NUM_POINTS - 1];
|
|
}
|
|
float p1_x, p1_y, p2_x, p2_y;
|
|
for (int i = 0; i < PowerParameters::Battery::DISCHARGE_CURVE::NUM_POINTS;
|
|
i++) {
|
|
if (batteryVoltage >=
|
|
PowerParameters::Battery::DISCHARGE_CURVE::VOLTAGES[i]) {
|
|
p1_y = PowerParameters::Battery::DISCHARGE_CURVE::CHARGE_STATES[i];
|
|
p1_x = PowerParameters::Battery::DISCHARGE_CURVE::VOLTAGES[i];
|
|
p2_y = PowerParameters::Battery::DISCHARGE_CURVE::CHARGE_STATES[i + 1];
|
|
p2_x = PowerParameters::Battery::DISCHARGE_CURVE::VOLTAGES[i + 1];
|
|
chargeState =
|
|
((p2_y - p1_y) / (p2_x - p1_x)) * (batteryVoltage - p1_x) + p1_y;
|
|
return chargeState;
|
|
}
|
|
}
|
|
}
|
|
|
|
void Power::updatePowerStateHandler() {
|
|
|
|
// Update supply and charge state flags
|
|
Power::busPowered = !digitalRead(PowerParameters::PinConfig::VUSB_SENS);
|
|
Power::chargingState = digitalRead(PowerParameters::PinConfig::BAT_CHG_STAT);
|
|
|
|
ESP_LOGD(TAG, "Bus Powered: %d, Charging: %d", busPowered, chargingState);
|
|
|
|
|
|
float currentCurrent = powerScheduler->getCurrentCurrent();
|
|
int referenceCurrentMa =
|
|
PowerParameters::Battery::DISCHARGE_CURVE::REFERENCE_CURRENT_A * 1000;
|
|
|
|
float coloumbsConsumedSinceLastUpdate;
|
|
|
|
// Calculate remaining battery charge in Coulombs based on current and time
|
|
if (!Power::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 (!Power::busPowered) {
|
|
if ((currentCurrent > (referenceCurrentMa * 0.6)) &&
|
|
(currentCurrent < (referenceCurrentMa * 1.4))) {
|
|
// Get battery charge state from voltage curve
|
|
chargeState = getBatteryVoltageChargePercent();
|
|
ESP_LOGD(TAG, "Charge state from voltage: %f", chargeState);
|
|
} else {
|
|
// Estimate battery charge state from charge consumption
|
|
float oldChargeState = lastSOC[latestSoCIndex];
|
|
chargeState = oldChargeState -
|
|
((coloumbsConsumedSinceLastUpdate /
|
|
PowerParameters::Battery::CELL_CHARGE_FULL_COLOUMB) *
|
|
100);
|
|
ESP_LOGD(TAG, "Charge state from current: %f", chargeState);
|
|
}
|
|
} else {
|
|
// If we are charging, we can't estimate the charge state based on current
|
|
// consumption
|
|
chargeState = getBatteryVoltageChargePercent();
|
|
ESP_LOGD(TAG, "Charge state from voltage (USB powered): %f", chargeState);
|
|
}
|
|
|
|
addSoCSample(chargeState);
|
|
|
|
for (int i = 0; i < PowerParameters::Battery::AVERAGING_SAMPLES; i++) {
|
|
ESP_LOGD(TAG, "SoC[%d]: %f", i, lastSOC[i]);
|
|
}
|
|
|
|
// Update percentage remaining based on charge state average
|
|
float sampleSum = 0;
|
|
for (int i = 0; i < PowerParameters::Battery::AVERAGING_SAMPLES; i++) {
|
|
sampleSum += lastSOC[i];
|
|
}
|
|
Power::percentRemaining = sampleSum / PowerParameters::Battery::AVERAGING_SAMPLES;
|
|
|
|
// Update last update time
|
|
Power::lastPowerStateUpdate = xTaskGetTickCount();
|
|
|
|
// Update the available current (changes based on battery state of charge)
|
|
Power::powerScheduler->recalculateCurrentBudgets();
|
|
ESP_LOGD(TAG, "Current: %f mA, Charge: %f Coulombs, %f %%", currentCurrent,
|
|
coloumbsRemaining, percentRemaining);
|
|
|
|
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;
|
|
constexpr float u_3v3 = 3.3;
|
|
return (u_bat * i_bat * eta) / u_3v3;
|
|
}
|
|
|
|
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;
|
|
}
|
|
|
|
void Power::initPowerState(void) {
|
|
// Initialize the power state
|
|
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;
|
|
// Set up flags and pins for them
|
|
pinMode(PowerParameters::PinConfig::VUSB_SENS, INPUT_PULLUP);
|
|
pinMode(PowerParameters::PinConfig::BAT_CHG_STAT, INPUT_PULLUP);
|
|
busPowered = digitalRead(PowerParameters::PinConfig::VUSB_SENS);
|
|
chargingState = digitalRead(PowerParameters::PinConfig::BAT_CHG_STAT);
|
|
}
|
|
|
|
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 (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("=============================================\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;
|
|
float Power::coloumbsRemaining =
|
|
PowerParameters::Battery::CELL_CHARGE_FULL_COLOUMB;
|
|
float Power::percentRemaining = 100.0;
|
|
PowerScheduler *Power::powerScheduler = nullptr;
|
|
|
|
bool Power::busPowered = false;
|
|
|
|
bool Power::chargingState = false;
|
|
|
|
Power::Power() {}
|
|
|
|
Power::~Power() {} |