/* * 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 #include #include #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 "i2c.h" #include "sensors.h" #include "soc/rtc_io_reg.h" #include "power_mgmt.h" #include "storage.h" #include "rtc.h" #define TAG "POWER" // === 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 // map from relay number to bridge /*bridge_t bridge_map[] = { -1, BRIDGE_AUX, BRIDGE_AUX, BRIDGE_AUX, BRIDGE_JACK, BRIDGE_JACK, BRIDGE_DRIVE, BRIDGE_DRIVE };*/ // update time #define UPDATE_MS 20 #define UPDATE_S 0.02f extern int64_t fsm_now; // us // E-fuse data typedef struct { int64_t az_enable_time; // Timestamp to enable autozeroing at (negative to disable) float az_offset; // Accumulated zero offset bool az_initialized; // First valid zero established float raw_current; bool ema_init; float ema_current; float current; // with all the corrections applied float current_spike; float heat; efuse_trip_t tripped; int64_t trip_time; int64_t on_us; int64_t off_us; } isens_channel_t; static isens_channel_t isens[N_BRIDGES] = {0}; /**** DRIVE RELAYS ****/ bool relay_states[8] = {false}; //int64_t bridge_transitions_on[NUM_BRIDGES] = {-1}; // last time relay turned on (used to ignore inrush) //int64_t bridge_transitions_off[NUM_BRIDGES] = {-1}; // last time relay turned off (used to enable autozero) relay_port_t last_relay_state; // actually write relay states, taking note of transitions, and debouncing transitions to on. #define BRIDGE_TRANSITION_LOGIC(BRIDGE_NAME) \ if (relay_state.bridges.BRIDGE_NAME == last_relay_state.bridges.BRIDGE_NAME) { \ /* no change; no need to do anything */ \ if(false) if (BRIDGE_##BRIDGE_NAME == BRIDGE_JACK) ESP_LOGI(TAG, "NO CHANGE"); \ } \ else if (last_relay_state.bridges.BRIDGE_NAME != BRIDGE_OFF && relay_state.bridges.BRIDGE_NAME == BRIDGE_OFF) { \ isens[BRIDGE_##BRIDGE_NAME].off_us = fsm_now; \ if(false) if (BRIDGE_##BRIDGE_NAME == BRIDGE_JACK) ESP_LOGI(TAG, "ON -> OFF"); \ } \ else if (last_relay_state.bridges.BRIDGE_NAME == BRIDGE_OFF && relay_state.bridges.BRIDGE_NAME != BRIDGE_OFF) { \ if (fsm_now > isens[BRIDGE_##BRIDGE_NAME].off_us + 2*get_param_value_t(PARAM_EFUSE_INRUSH_US).u32) { \ isens[BRIDGE_##BRIDGE_NAME].on_us = fsm_now; \ if(false) if (BRIDGE_##BRIDGE_NAME == BRIDGE_JACK) ESP_LOGI(TAG, "OFF -> ON"); \ } else { \ relay_state.bridges.BRIDGE_NAME = BRIDGE_OFF; \ if(false) if (BRIDGE_##BRIDGE_NAME == BRIDGE_JACK) ESP_LOGI(TAG, "NOT YET; -> OFF"); \ } \ } \ else { \ if(false) if (BRIDGE_##BRIDGE_NAME == BRIDGE_JACK) ESP_LOGE(TAG, "TOO FAST OF TRANSITION"); \ isens[BRIDGE_##BRIDGE_NAME].off_us = fsm_now; \ relay_state.bridges.BRIDGE_NAME = BRIDGE_OFF; \ } esp_err_t drive_relays(relay_port_t relay_state) { // Four types of transitions. // Not a transition: this does nothing // Anything -> off: always allowed. Record the transition time // off -> anything: has debouncing; set & record transition if fsm_now > bridge_transitions_off + debounce, otherwise keep bridge off. // fwd/rev/on -> fwd/rev/on: not allowed. Actually go to 0. Record the transition time. BRIDGE_TRANSITION_LOGIC(DRIVE) BRIDGE_TRANSITION_LOGIC(JACK) BRIDGE_TRANSITION_LOGIC(AUX) relay_state.bridges.SENSORS = 1; if (!get_is_safe()) relay_state.bridges.DRIVE = 0; last_relay_state = relay_state; //ESP_LOGI(TAG, "RELAY STATE: %x", state); return i2c_set_relays(relay_state); } /**** CURRENT / VOLTAGE MONITORING ****/ // === ADC Handles === static adc_oneshot_unit_handle_t adc1_handle = NULL; static adc_cali_handle_t adc_cali_handle = NULL; static float ema_battery = 0.0f; static bool ema_battery_init = false; esp_err_t adc_init() { // 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_12, .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_12, .bitwidth = ADC_BITWIDTH_12, }; ESP_ERROR_CHECK(adc_cali_create_scheme_line_fitting(&cali_cfg, &adc_cali_handle)); return ESP_OK; } esp_err_t adc_post(void) { // Read all 4 channels twice with a short delay; flag if frozen or wildly out of range const adc_channel_t channels[] = { PIN_V_ISENS1, PIN_V_ISENS2, PIN_V_ISENS3, PIN_V_SENS_BAT }; const char *names[] = { "ISENS1", "ISENS2", "ISENS3", "BATTERY" }; int first[4], second[4]; for (int i = 0; i < 4; i++) { if (adc_oneshot_read(adc1_handle, channels[i], &first[i]) != ESP_OK) { ESP_LOGE(TAG, "POST: ADC read failed on %s", names[i]); return ESP_FAIL; } } vTaskDelay(pdMS_TO_TICKS(5)); for (int i = 0; i < 4; i++) { if (adc_oneshot_read(adc1_handle, channels[i], &second[i]) != ESP_OK) { ESP_LOGE(TAG, "POST: ADC read failed on %s (2nd)", names[i]); return ESP_FAIL; } } // Check for frozen ADC (identical readings on noise-bearing current sense channels) for (int i = 0; i < 3; i++) { // only current sense, not battery (battery can be stable) if (first[i] == second[i] && first[i] != 0) { ESP_LOGW(TAG, "POST: ADC %s may be frozen (both reads = %d)", names[i], first[i]); // Warning only — a truly stuck ADC will trip efuse protections anyway } } ESP_LOGI(TAG, "POST: ADC OK (BAT=%d/%d, I1=%d/%d, I2=%d/%d, I3=%d/%d)", first[3], second[3], first[0], second[0], first[1], second[1], first[2], second[2]); return ESP_OK; } float get_raw_battery_voltage(void) { int adc_raw = 0; int voltage_mv = 0; if (adc_oneshot_read(adc1_handle, PIN_V_SENS_BAT, &adc_raw) != ESP_OK) { return NAN; } if (adc_cali_raw_to_voltage(adc_cali_handle, adc_raw, &voltage_mv) != ESP_OK) { return NAN; } // Voltage divider: 150kohm to 1Mohm -> gain = 1.15 -> scale = 1150/150 return voltage_mv * get_param_value_t(PARAM_V_SENS_K).f32 + get_param_value_t(PARAM_V_SENS_OFFSET).f32; // same as / 1000.0 * 1150.0 / 150.0; } esp_err_t process_battery_voltage(void) { float raw = get_raw_battery_voltage(); if (!ema_battery_init) { ema_battery = (float)raw; ema_battery_init = true; } else { float alpha = get_param_value_t(PARAM_ADC_ALPHA_BATTERY).f32; if (isnan(raw)) { //ESP_LOGI(TAG, "RAW BATTERY IS NAN"); } else { if (isnan(ema_battery) || isnan(alpha)) { ema_battery = raw; } else { ema_battery = alpha * (float)raw + (1.0f - alpha) * ema_battery; } } } return ESP_OK; } void disable_autozero(bridge_t bridge) { // enable autozeroing for this bridge 1 second from now isens[bridge].az_enable_time = fsm_now+1000000; //ESP_LOGI(TAG, "KILLING BRIDGE %d; %lld -> %lld", bridge, (long long int) now, (long long int) isens[bridge].az_enable_time); } bool get_bridge_overcurrent(bridge_t bridge, float threshold) { if (bridge < 0 || bridge>=NUM_BRIDGES) return true; // I GUESS? if (fsm_now < isens[bridge].on_us + get_param_value_t(PARAM_EFUSE_INRUSH_US).u32) return false; if (isens[bridge].raw_current < threshold) return false; return true; } bool get_bridge_spike(bridge_t bridge, float threshold) { if (bridge < 0 || bridge>=NUM_BRIDGES) return true; // I GUESS? if (fsm_now < isens[bridge].on_us + get_param_value_t(PARAM_EFUSE_INRUSH_US).u32) return false; if (isens[bridge].current_spike < threshold) return false; return true; } esp_err_t process_bridge_current(bridge_t bridge) { if (bridge < 0 || bridge >= NUM_BRIDGES) return ESP_ERR_INVALID_ARG; int adc_raw = 0; int voltage_mv = 0; isens_channel_t *channel = &isens[bridge]; adc_channel_t pin; switch(bridge) { case BRIDGE_DRIVE: pin = PIN_V_ISENS1; break; case BRIDGE_JACK: pin = PIN_V_ISENS2; break; case BRIDGE_AUX: pin = PIN_V_ISENS3; break; default: return ESP_ERR_INVALID_ARG; } 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 last_current = channel->raw_current; channel->raw_current = NAN; switch (bridge) { case BRIDGE_JACK: case BRIDGE_AUX: // ACS37042KLHBLT-030B3 is 30A capable and 44 mV/A channel->raw_current = (voltage_mv - 1650.0f) / 44.0f; break; case BRIDGE_DRIVE: // ACS37220LEZATR-100B3 is 100A capable and 13.2 mV/A channel->raw_current = -(voltage_mv - 1650.0f) / 13.2f; break; default: break; } if (!channel->ema_init) { channel->ema_current = channel->raw_current; channel->ema_init = true; } else { float alpha = get_param_value_t(PARAM_ADC_ALPHA_ISENS).f32; if (isnan(channel->raw_current)) { //ESP_LOGI(TAG, "RAW BATTERY IS NAN"); channel->ema_current = NAN; } else { if (isnan(ema_battery) || isnan(alpha)) { channel->ema_current = channel->raw_current; } else { channel->ema_current = alpha * channel->raw_current + (1.0f - alpha) * channel->ema_current; } } } // === AUTO-ZERO LEARNING PHASE === if (fsm_now > channel->az_enable_time) { //ESP_LOGI(TAG, "AZING %d", bridge); float db = get_param_value_t(PARAM_ADC_DB_IAZ).f32; if (isnan(db) || fabsf(channel->ema_current) <= db) { // Valid zero sample if (!channel->az_initialized) { channel->az_offset = channel->ema_current; channel->az_initialized = true; } else { float alpha = get_param_value_t(PARAM_ADC_ALPHA_IAZ).f32; if (isnan(channel->raw_current)) { //ESP_LOGI(TAG, "RAW BATTERY IS NAN"); } else { if (isnan(ema_battery) || isnan(alpha)) { channel->az_offset = channel->ema_current; } else { channel->az_offset = alpha * channel->ema_current + (1.0f - alpha) * channel->az_offset; } } } } } // Apply the offset channel->current = channel->raw_current - channel->az_offset; channel->raw_current = channel->raw_current - channel->az_offset; channel->current_spike = channel->raw_current - last_current; // PARAMETERS FOR E-FUSING ALGORITHM // PARAM_EFUSE_KINST : ratio of nominal current that should cause an immediate shutdown // PARAM_EFUSE_TCOOL : cooldown timer from trip (in microseconds) // PARAM_EFUSE_TAUCOOL : speed of cooldown for heating (units are 1/s; bigger = faster cooldown) // Monitor E-fusing float I_nominal = NAN; switch(bridge) { case BRIDGE_DRIVE: I_nominal = get_param_value_t(PARAM_EFUSE_INOM_1).f32; break; case BRIDGE_JACK: I_nominal = get_param_value_t(PARAM_EFUSE_INOM_2).f32; break; case BRIDGE_AUX: I_nominal = get_param_value_t(PARAM_EFUSE_INOM_3).f32; break; default: break; } // Normalize the current as a fraction of rated current float I_norm = fabsf(channel->current / I_nominal); // Instant trip on extreme overcurrent if (fsm_now > channel->on_us + get_param_value_t(PARAM_EFUSE_INRUSH_US).u32 && I_norm >= get_param_value_t(PARAM_EFUSE_KINST).f32) { // Check if overcurrent has persisted long enough channel->tripped = true; channel->trip_time = fsm_now; //ESP_LOGI(TAG, "FUSE TRIP: Inom: %+.5f HEAT:%+2.5f", I_norm, channel->heat); return ESP_OK; // no more processing, if we're over, we're over // Still in overcurrent but within inrush tolerance window - don't trip yet } // Accumulate heat channel->heat += (I_norm * I_norm) * UPDATE_S; // Only do cooling when below threshold if (I_norm < 1.0f) { // if we are hot we radiate more heat // (I^2/I^2*t) * (1/t) * t = I^2/I^2*t channel->heat -= channel->heat * get_param_value_t(PARAM_EFUSE_TAUCOOL).f32 * UPDATE_S; channel->heat = fmaxf(0.0f, channel->heat); // keep it from going negative // channel.tripped = false; // Auto-clear if cooled (WTF why this is insane) } // If built-up heat exceeds the time limit, trip // Recall units of heat are (current_actual^2/current_nominal^2)*time // Ergo, heat is measured in seconds if (channel->heat > get_param_value_t(PARAM_EFUSE_HEAT_THRESH).f32) { channel->tripped = true; channel->trip_time = fsm_now; // If we're not overheated // And enough time has passed // Go ahead and reset the e-fuse } else if (channel->tripped && (fsm_now - channel->trip_time) > get_param_value_t(PARAM_EFUSE_TCOOL).u32) { channel->tripped = false; // channel.heat = 0.0f // I think we should wait for the e-fuse to catch up } //if (bridge == BRIDGE_JACK) ESP_LOGI(TAG, "TIME: %lld", (long long) fsm_now); //if (bridge == BRIDGE_JACK) ESP_LOGI(TAG, "FUSE: trip [%d] %lld, raw_a: %+.4f cur: %+.4f Inorm: %+.5f HEAT:%+2.5f", channel->tripped, channel->trip_time, channel->raw_current, channel->current, I_norm, channel->heat); return ESP_OK; } // === Public Accessors === float get_bridge_A(bridge_t bridge) { if (bridge >= N_BRIDGES) return NAN; return isens[bridge].current; } float get_bridge_raw_A(bridge_t bridge) { if (bridge >= N_BRIDGES) return NAN; return isens[bridge].raw_current; } float efuse_get_heat(bridge_t bridge) { if (bridge >= N_BRIDGES) return NAN; return isens[bridge].heat; } float get_battery_V(void) { if (ema_battery_init) return ema_battery; return get_raw_battery_voltage(); } efuse_trip_t efuse_get(bridge_t bridge) { if (bridge >= N_BRIDGES) return false; return isens[bridge].tripped; } void efuse_set(bridge_t bridge, efuse_trip_t state) { if (bridge >= N_BRIDGES) return; isens[bridge].tripped = state; isens[bridge].trip_time = fsm_now; }