Files
SC-F001/main/power_mgmt.c.old
2025-12-16 12:10:10 -06:00

546 lines
16 KiB
C

/*
* power_mgmt.c
*
* 1 kHz power-management task:
* • Samples all three H-bridge current sensors (DRIVE, AUX, JACK)
* • Samples battery voltage (BAT)
* • Applies EMA filtering on every channel
* • Updates shared volatile globals for the control FSM
* • Handles over-current spike protection
*
* Updated to modern ESP-IDF ADC API (line fitting)
* All variables now defined locally
*
* Created on: Nov 10, 2025
*/
#include <math.h>
#include <stdint.h>
#include <stdbool.h>
#include "driver/rtc_io.h"
#include "esp_log.h"
#include "esp_task_wdt.h"
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "esp_adc/adc_oneshot.h"
#include "esp_adc/adc_cali.h"
#include "esp_adc/adc_cali_scheme.h"
#include "esp_timer.h"
#include "driver/gpio.h"
#include "control_fsm.h"
#include "soc/rtc_io_reg.h"
#include "power_mgmt.h"
#include "storage.h"
#include "rtc.h"
// === GPIO Pin Definitions ===
#define PIN_V_ISENS1 ADC_CHANNEL_0 // GPIO36 / VP
#define PIN_V_ISENS2 ADC_CHANNEL_6 // GPIO34
#define PIN_V_ISENS3 ADC_CHANNEL_7 // GPIO35
#define PIN_V_BATTERY ADC_CHANNEL_3 // GPIO39 / VN
#define PIN_V_SENS_BAT PIN_V_BATTERY
#define PIN_CHG_BULK GPIO_NUM_26
#define AUTOZERO_THRESH 2000.0f // mA
typedef enum {
CHG_T_LOWBAT = 0,
CHG_T_BULK = 1,
CHG_T_STEADY = 2,
} charge_timer_t;
#define N_CHG_TIMERS 3
RTC_DATA_ATTR charge_state_t current_charge_state = CHG_STATE_BULK;
RTC_DATA_ATTR int64_t charge_timers[N_CHG_TIMERS] = {-1};
int64_t now;
charge_state_t get_charging_state() { return current_charge_state; }
void setTimerN(charge_timer_t i, int64_t sec) {
// set the timer for <sec> in the future if it's currently less than now
if (charge_timers[i] < now) {
charge_timers[i] = now + sec;
ESP_LOGI("BAT", "Set timer[%d] +%lld", i, (long long)sec);
}
}
void resetTimerN(charge_timer_t i) {
charge_timers[i] = -1;
}
void resetBatTimers() {
for (uint8_t i=0; i<N_CHG_TIMERS; i++)
resetTimerN(i);
}
bool getTimerN(charge_timer_t i) {
if (charge_timers[i] < 0) return false;
return system_rtc_get_raw_time() > charge_timers[i];
}
#define BULK_CHARGE_S 20 //2*60*60
#define FLOAT_STEADY_S 10 //30*60
#define LOW_DETECT_S 10 //5*60
#define STEADY_MV 13000
#define LOW_MV 12800
void run_charge_fsm() {
now = system_rtc_get_raw_time();
//ESP_LOGI("BAT", "FSM STATE %d", current_charge_state);
if (rtc_is_set()) {
switch(current_charge_state) {
case CHG_STATE_BULK:
// turn off bulk charging and go to float when time is up
if (getTimerN(CHG_T_BULK)) {
ESP_LOGI("BAT", "BULK -> FLOAT");
current_charge_state = CHG_STATE_FLOAT;
}
break;
case CHG_STATE_FLOAT:
if (getTimerN(CHG_T_STEADY)) {
ESP_LOGI("BAT", "FLOAT -> OFF");
current_charge_state = CHG_STATE_OFF;
}
if (get_battery_mV() > STEADY_MV) {
setTimerN(CHG_T_STEADY, FLOAT_STEADY_S);
} else {
resetTimerN(CHG_T_STEADY);
}
// NO break; !! float should also kick into bulk with same triggers
case CHG_STATE_OFF:
// after 5 minutes of low-ish battery go into bulk charge
if (getTimerN(CHG_T_LOWBAT)) {
ESP_LOGI("BAT", " -> BULK");
current_charge_state = CHG_STATE_BULK;
setTimerN(CHG_T_BULK, BULK_CHARGE_S);
}
if (get_battery_mV() < LOW_MV) {
setTimerN(CHG_T_LOWBAT, LOW_DETECT_S);
} else {
resetTimerN(CHG_T_LOWBAT);
}
break;
}
} else {
//ESP_LOGI("BAT", " -> BULK");
current_charge_state = CHG_STATE_BULK;
}
//rtc_gpio_hold_dis(PIN_CHG_BULK);
//rtc_gpio_hold_dis(PIN_CHG_DISABLE);
switch(current_charge_state) {
case CHG_STATE_BULK:
gpio_set_level(PIN_CHG_BULK, 1);
//ESP_LOGI("BAT", "BULK");
break;
case CHG_STATE_FLOAT:
gpio_set_level(PIN_CHG_BULK, 0);
//ESP_LOGI("BAT", "FLOAT");
break;
case CHG_STATE_OFF:
gpio_set_level(PIN_CHG_BULK, 0);
//ESP_LOGI("BAT", "OFF");
break;
}
//rtc_gpio_hold_en(PIN_CHG_BULK);
//rtc_gpio_hold_en(PIN_CHG_DISABLE);
}
typedef struct {
bool enabled; // Auto-zero active for this channel
float threshold_ma; // Max current to consider "zero" (mA)
float learned_offset_mv; // Accumulated zero offset (mV)
bool initialized; // First valid zero established
} autozero_t;
static autozero_t autozero[N_BRIDGES] = {0};
// === E-Fuse (Software Breaker) Configuration ===
static const char* currentLimits_A[N_BRIDGES] = {
[BRIDGE_DRIVE] = "efuse_drive_A", //40000,
[BRIDGE_AUX] = "efuse_aux_A", // 5000,
[BRIDGE_JACK] = "efuse_jack_A" // 10000
};
static const float i2t_thresholds[N_BRIDGES] = { // A^2*s (tunable per bridge if needed)
[BRIDGE_DRIVE] = 6.0f,
[BRIDGE_AUX] = 6.0f,
[BRIDGE_JACK] = 6.0f
};
static const float i_instant[N_BRIDGES] = { // Instant trip multiplier of I_rated
[BRIDGE_DRIVE] = 15.0f,
[BRIDGE_AUX] = 15.0f,
[BRIDGE_JACK] = 15.0f
};
static const float cool_rate[N_BRIDGES] = { // Cooling constant (1/s)
[BRIDGE_DRIVE] = 0.008f,
[BRIDGE_AUX] = 0.008f,
[BRIDGE_JACK] = 0.008f
};
static const int32_t cooldown_ms[N_BRIDGES] = { // Auto-reset delay after trip
[BRIDGE_DRIVE] = 5000,
[BRIDGE_AUX] = 5000,
[BRIDGE_JACK] = 5000
};
static float efuse_heat[N_BRIDGES] = {0};
static uint64_t efuse_trip_time[N_BRIDGES] = {0}; // Timestamp when tripped
static bool efuse_tripped[N_BRIDGES] = {false};
// === ADC Handles ===
static adc_oneshot_unit_handle_t adc1_handle = NULL;
static adc_cali_handle_t adc_cali_handle = NULL;
// === EMA Filter State ===
#define EMA_ALPHA_CURRENT 0.5f
#define EMA_ALPHA_BATTERY 0.05f
static float ema_current[N_BRIDGES] = {0};
static bool ema_init[N_BRIDGES] = {false};
static float ema_battery = 0.0f;
static bool ema_battery_init = false;
// === Shared Volatile Outputs ===
volatile int32_t bridgeCurrents_mA[N_BRIDGES] = {0};
volatile int32_t batteryVoltage_mV = 0;
// === ADC Initialization ===
static esp_err_t adc_init(void) {
if (adc1_handle != NULL) {
return ESP_OK; // Already initialized
}
// ADC1 oneshot mode
adc_oneshot_unit_init_cfg_t init_cfg = {
.unit_id = ADC_UNIT_1,
};
ESP_ERROR_CHECK(adc_oneshot_new_unit(&init_cfg, &adc1_handle));
// Configure all channels
adc_oneshot_chan_cfg_t chan_cfg = {
.atten = ADC_ATTEN_DB_11,
.bitwidth = ADC_BITWIDTH_12,
};
ESP_ERROR_CHECK(adc_oneshot_config_channel(adc1_handle, PIN_V_ISENS1, &chan_cfg));
ESP_ERROR_CHECK(adc_oneshot_config_channel(adc1_handle, PIN_V_ISENS2, &chan_cfg));
ESP_ERROR_CHECK(adc_oneshot_config_channel(adc1_handle, PIN_V_ISENS3, &chan_cfg));
ESP_ERROR_CHECK(adc_oneshot_config_channel(adc1_handle, PIN_V_SENS_BAT, &chan_cfg));
// Line fitting calibration (modern scheme)
adc_cali_line_fitting_config_t cali_cfg = {
.unit_id = ADC_UNIT_1,
.atten = ADC_ATTEN_DB_11,
.bitwidth = ADC_BITWIDTH_12,
};
ESP_ERROR_CHECK(adc_cali_create_scheme_line_fitting(&cali_cfg, &adc_cali_handle));
return ESP_OK;
}
void autozero_enable(bridge_t bridge, bool enable) {
if (bridge >= N_BRIDGES) return;
autozero[bridge].enabled = enable;
if (!enable) {
autozero[bridge].learned_offset_mv = 0.0f;
autozero[bridge].initialized = false;
}
}
void autozero_set_threshold(bridge_t bridge, float threshold_ma) {
if (bridge >= N_BRIDGES) return;
autozero[bridge].threshold_ma = fmaxf(0.0f, threshold_ma);
}
float autozero_get_offset_mv(bridge_t bridge) {
if (bridge >= N_BRIDGES) return 0.0f;
return autozero[bridge].learned_offset_mv;
}
void autozero_reset(bridge_t bridge) {
if (bridge >= N_BRIDGES) return;
autozero[bridge].learned_offset_mv = 0.0f;
autozero[bridge].initialized = false;
}
void autozero_reset_all(void) {
for (uint8_t i = 0; i < N_BRIDGES; i++) {
autozero_reset((bridge_t)i);
}
}
// === Raw Current Reading (mA) ===
static int32_t read_bridge_current_raw(bridge_t bridge) {
int adc_raw = 0;
int voltage_mv = 0;
adc_channel_t pin;
switch(bridge) {
case BRIDGE_DRIVE: pin = PIN_V_ISENS1; break;
case BRIDGE_AUX: pin = PIN_V_ISENS3; break;
case BRIDGE_JACK: pin = PIN_V_ISENS2; break;
default: return -42069; // lol
}
if (adc_oneshot_read(adc1_handle, pin, &adc_raw) != ESP_OK) {
return 0;
}
if (adc_cali_raw_to_voltage(adc_cali_handle, adc_raw, &voltage_mv) != ESP_OK) {
return 0;
}
float current_sense_mv = (float)voltage_mv;
autozero_t *az = &autozero[bridge];
// === AUTO-ZERO LEARNING PHASE ===
if (az->enabled && get_bridge_state(bridge)==0) {
float raw_current_ma = 0.0f;
switch (bridge) {
case BRIDGE_JACK:
case BRIDGE_AUX:
// ACS37042KLHBLT-030B3 is 30A capable and 44 mV/A
raw_current_ma = (current_sense_mv - 1650.0f) * 1000.0f / 44.0f;
break;
case BRIDGE_DRIVE:
// ACS37220LEZATR-100B3 is 100A capable and 13.2 mV/A
raw_current_ma = (current_sense_mv - 1650.0f) * 1000.0f / 13.20f;
break;
}
if (fabsf(raw_current_ma) <= az->threshold_ma) {
// Valid zero sample
if (!az->initialized) {
az->learned_offset_mv = current_sense_mv - 1650.0f;
az->initialized = true;
} else {
// EMA on offset (slow adaptation)
float alpha = 0.1f;
az->learned_offset_mv = alpha * (current_sense_mv - 1650.0f) +
(1.0f - alpha) * az->learned_offset_mv;
}
}
}
// === APPLY AUTO-ZERO OFFSET ===
float corrected_mv = current_sense_mv - az->learned_offset_mv;
int32_t offset_mv = (int32_t)(corrected_mv - 1650.0f);
int32_t current_ma = 0;
switch (bridge) {
case BRIDGE_JACK:
case BRIDGE_AUX:
current_ma = offset_mv * 1000 / 44; // 44 mV/A
break;
case BRIDGE_DRIVE:
current_ma = offset_mv * 10000 / 132; // 13.2 mV/A
break;
}
return current_ma;
}
// === Raw Battery Voltage Reading (mV) ===
static int32_t read_battery_voltage_raw(void)
{
int adc_raw = 0;
int voltage_mv = 0;
if (adc_oneshot_read(adc1_handle, PIN_V_SENS_BAT, &adc_raw) != ESP_OK) {
return 0;
}
if (adc_cali_raw_to_voltage(adc_cali_handle, adc_raw, &voltage_mv) != ESP_OK) {
return 0;
}
// Voltage divider: 150kΩ to 1MΩ → gain = 1.15 → scale = 1150/150
return (int32_t)voltage_mv * 1150 / 150;
}
// === EMA Filter Update ===
static void apply_ema(float *state, bool *init, float alpha, int32_t raw, volatile int32_t *out)
{
if (!*init) {
*state = (float)raw;
*init = true;
} else {
*state = alpha * (float)raw + (1.0f - alpha) * *state;
}
*out = (int32_t)(*state + 0.5f);
}
// === Public Accessors ===
int32_t get_bridge_mA(uint8_t bridge)
{
if (bridge >= N_BRIDGES) return -1;
return (int32_t)bridgeCurrents_mA[bridge];
}
int32_t get_battery_mV(void)
{
return (int32_t)batteryVoltage_mV;
}
// === E-Fuse: Trip Logic (called every cycle) ===
static void efuse_update(uint8_t bridge, float I, float dt, uint64_t now)
{
float I_rated = (float)get_param_i8(currentLimits_A[bridge]);
float I_norm = I / I_rated;
// Instant trip on extreme overcurrent
if (I_norm >= i_instant[bridge]) {
efuse_tripped[bridge] = true;
efuse_trip_time[bridge] = now;
return;
}
// Cooling when below threshold
if (I_norm < 1.1f) {
efuse_heat[bridge] -= efuse_heat[bridge] * cool_rate[bridge] * dt;
efuse_heat[bridge] = fmaxf(0.0f, efuse_heat[bridge]);
efuse_tripped[bridge] = false; // Auto-clear if cooled
return;
}
// Accumulate heat (I²t)
efuse_heat[bridge] += (I_norm * I_norm) * dt;
if (efuse_heat[bridge] >= i2t_thresholds[bridge]) {
efuse_tripped[bridge] = true;
efuse_trip_time[bridge] = now;
}
}
// === E-Fuse: Auto-Reset After Cooldown ===
static void efuse_cooldown_check(uint64_t now)
{
for (uint8_t i = 0; i < N_BRIDGES; i++) {
if (efuse_tripped[i] &&
(now - efuse_trip_time[i]) >= (cooldown_ms[i] * 1000ULL)) {
efuse_heat[i] = 0.0f;
efuse_tripped[i] = false;
}
}
}
// === Public E-Fuse Controls ===
void efuse_reset_all(void)
{
for (uint8_t i = 0; i < N_BRIDGES; i++) {
efuse_heat[i] = 0.0f;
efuse_tripped[i] = false;
}
}
bool efuse_is_tripped(uint8_t bridge)
{
if (bridge >= N_BRIDGES) return false;
return efuse_tripped[bridge];
}
// === Power Management Task ===
void power_mgmt_task(void *param) {
esp_task_wdt_add(NULL);
/*gpio_config_t io_conf = {
.pin_bit_mask = (1ULL << PIN_CHG_DISABLE) | (1ULL << PIN_CHG_BULK),
.mode = GPIO_MODE_OUTPUT,
.pull_up_en = GPIO_PULLUP_DISABLE,
.pull_down_en = GPIO_PULLDOWN_DISABLE,
.intr_type = GPIO_INTR_DISABLE,
};
gpio_config(&io_conf);*/
/*// Enable RTC GPIO domain (required for hold)
rtc_gpio_init(PIN_CHG_DISABLE);
rtc_gpio_init(PIN_CHG_BULK);
// Set as output
rtc_gpio_set_direction(PIN_CHG_DISABLE, RTC_GPIO_MODE_OUTPUT_ONLY);
rtc_gpio_set_direction(PIN_CHG_BULK, RTC_GPIO_MODE_OUTPUT_ONLY);
// Optional: set initial level (will be held)
//rtc_gpio_set_level(PIN_CHG_DISABLE, 1); // e.g., start disabled
//rtc_gpio_set_level(PIN_CHG_BULK, 0);
// **Critical: Enable hold function**
rtc_gpio_hold_en(PIN_CHG_DISABLE);
rtc_gpio_hold_en(PIN_CHG_BULK);*/
ESP_ERROR_CHECK(adc_init());
TickType_t xLastWakeTime = xTaskGetTickCount();
const TickType_t xFrequency = pdMS_TO_TICKS(20);
// Optional: Enable auto-zero with default threshold
autozero_enable(BRIDGE_DRIVE, true);
autozero_enable(BRIDGE_AUX, true);
autozero_enable(BRIDGE_JACK, true);
autozero_set_threshold(BRIDGE_DRIVE, AUTOZERO_THRESH);
autozero_set_threshold(BRIDGE_AUX, AUTOZERO_THRESH);
autozero_set_threshold(BRIDGE_JACK, AUTOZERO_THRESH);
//uint64_t last_wake_time = esp_timer_get_time();
//const uint64_t period = 5000; // 100 us => 10kHz
while (1) {
vTaskDelayUntil(&xLastWakeTime, xFrequency);
uint64_t now_us = esp_timer_get_time();
/*if (now - last_wake_time < period) {
uint32_t delay_us = (period - (now - last_wake_time)) / 1000;
if (delay_us > 0) vTaskDelay(pdMS_TO_TICKS(delay_us));
continue;
}
last_wake_time = now;*/
// Sample currents
for (uint8_t i = 0; i < N_BRIDGES; i++) {
int32_t raw_ma = read_bridge_current_raw((bridge_t)i);
apply_ema(&ema_current[i], &ema_init[i], EMA_ALPHA_CURRENT,
raw_ma, &bridgeCurrents_mA[i]);
// Reset spike timer if under limit
/*if (bridgeCurrents_mA[i] < currentLimits_mA[i]) {
currentSpikeSafeTimes[i] = now + CURRENT_SPIKE_TIME_US;
}*/
// === E-FUSE UPDATE ===
float I = (float)bridgeCurrents_mA[i] / 1000.0f;
float dt = 0.020f; // 20 ms task period
efuse_update(i, I, dt, now_us);
}
/*ESP_LOGI("PWR", "[ %6ld | %6ld | %6ld mA ] { %6ld mV }",
(long)bridgeCurrents_mA[BRIDGE_DRIVE],
(long)bridgeCurrents_mA[BRIDGE_JACK],
(long)bridgeCurrents_mA[BRIDGE_AUX],
(long)batteryVoltage_mV);*/
// Sample battery
int32_t raw_bat = read_battery_voltage_raw();
apply_ema(&ema_battery, &ema_battery_init, EMA_ALPHA_BATTERY,
raw_bat, &batteryVoltage_mV);
//run_charge_fsm();
efuse_cooldown_check(now_us);
esp_task_wdt_reset();
}
}
void start_power() {
xTaskCreate(power_mgmt_task, "PWR", 4096, NULL, 5, NULL);
}
void shutdown_power() {
}