From bcaf2a36af6ee32b8f8b2bad83ad2178c0254d69 Mon Sep 17 00:00:00 2001 From: robbederks Date: Mon, 17 Aug 2020 11:56:27 +0200 Subject: [PATCH] Car power integrator + power management refactor (#1994) * wip, ready to test * tweaks * fix * fix * fix power monitoring * fix param writing * no forced charging on high voltage * reset capacity on reboot * don't shutdown unless started seen * fix unused var warning * fix linting errors * time is always valid * QCOM gate * Local params * decimate saving * fix linting * rename param * Log car battery capacity * fix put_nonblocking * Added some unit tests * Add test to docker test list * fix precommit * cleanup * run tests in CI * bump cereal Co-authored-by: Adeeb Shihadeh old-commit-hash: 7555379b2b1da2f800e98963ad4436b7fb91cce9 --- .github/workflows/test.yaml | 1 + cereal | 2 +- common/params.py | 1 + selfdrive/boardd/boardd.cc | 47 ++-- selfdrive/thermald/__init__.py | 0 selfdrive/thermald/power_monitoring.py | 193 ++++++++++----- selfdrive/thermald/tests/__init__.py | 0 .../thermald/tests/test_power_monitoring.py | 221 ++++++++++++++++++ selfdrive/thermald/thermald.py | 17 +- 9 files changed, 382 insertions(+), 100 deletions(-) create mode 100644 selfdrive/thermald/__init__.py create mode 100644 selfdrive/thermald/tests/__init__.py create mode 100755 selfdrive/thermald/tests/test_power_monitoring.py diff --git a/.github/workflows/test.yaml b/.github/workflows/test.yaml index f8bbb1bc8d..7d0ae1f456 100644 --- a/.github/workflows/test.yaml +++ b/.github/workflows/test.yaml @@ -146,6 +146,7 @@ jobs: $UNIT_TEST selfdrive/car && \ $UNIT_TEST selfdrive/locationd && \ $UNIT_TEST selfdrive/athena && \ + $UNIT_TEST selfdrive/thermald && \ $UNIT_TEST tools/lib/tests" - name: Upload coverage to Codecov run: | diff --git a/cereal b/cereal index d66afca4ac..0d2ce45fc6 160000 --- a/cereal +++ b/cereal @@ -1 +1 @@ -Subproject commit d66afca4ac316456711cb80c8e8e2fe91431e1e2 +Subproject commit 0d2ce45fc681f90b33fbcd11e5d80dd294ef751b diff --git a/common/params.py b/common/params.py index e106705022..d01b1b4ae9 100755 --- a/common/params.py +++ b/common/params.py @@ -53,6 +53,7 @@ keys = { "AccessToken": [TxType.CLEAR_ON_MANAGER_START], "AthenadPid": [TxType.PERSISTENT], "CalibrationParams": [TxType.PERSISTENT], + "CarBatteryCapacity": [TxType.PERSISTENT], "CarParams": [TxType.CLEAR_ON_MANAGER_START, TxType.CLEAR_ON_PANDA_DISCONNECT], "CarParamsCache": [TxType.CLEAR_ON_MANAGER_START, TxType.CLEAR_ON_PANDA_DISCONNECT], "CarVin": [TxType.CLEAR_ON_MANAGER_START, TxType.CLEAR_ON_PANDA_DISCONNECT], diff --git a/selfdrive/boardd/boardd.cc b/selfdrive/boardd/boardd.cc index e1b275dd3e..1c83abf292 100644 --- a/selfdrive/boardd/boardd.cc +++ b/selfdrive/boardd/boardd.cc @@ -36,13 +36,6 @@ #define CUTOFF_IL 200 #define SATURATE_IL 1600 #define NIBBLE_TO_HEX(n) ((n) < 10 ? (n) + '0' : ((n) - 10) + 'a') -#define VOLTAGE_K 0.091 // LPF gain for 5s tau (dt/tau / (dt/tau + 1)) - -#ifdef QCOM -const uint32_t NO_IGNITION_CNT_MAX = 2 * 60 * 60 * 30; // turn off charge after 30 hrs -const float VBATT_START_CHARGING = 11.5; -const float VBATT_PAUSE_CHARGING = 11.0; -#endif Panda * panda = NULL; std::atomic safety_setter_thread_running(false); @@ -279,7 +272,6 @@ void can_health_thread() { uint32_t no_ignition_cnt = 0; bool ignition_last = false; - float voltage_f = 12.5; // filtered voltage // Broadcast empty health message when panda is not yet connected while (!panda){ @@ -306,8 +298,6 @@ void can_health_thread() { health.ignition_line = 1; } - voltage_f = VOLTAGE_K * (health.voltage / 1000.0) + (1.0 - VOLTAGE_K) * voltage_f; // LPF - // Make sure CAN buses are live: safety_setter_thread does not work if Panda CAN are silent and there is only one other CAN node if (health.safety_model == (uint8_t)(cereal::CarParams::SafetyModel::SILENT)) { panda->set_safety_model(cereal::CarParams::SafetyModel::NO_OUTPUT); @@ -321,24 +311,6 @@ void can_health_thread() { no_ignition_cnt += 1; } -#ifdef QCOM - bool cdp_mode = health.usb_power_mode == (uint8_t)(cereal::HealthData::UsbPowerMode::CDP); - bool no_ignition_exp = no_ignition_cnt > NO_IGNITION_CNT_MAX; - if ((no_ignition_exp || (voltage_f < VBATT_PAUSE_CHARGING)) && cdp_mode && !ignition) { - std::vector disable_power_down = read_db_bytes("DisablePowerDown"); - if (disable_power_down.size() != 1 || disable_power_down[0] != '1') { - LOGW("TURN OFF CHARGING!\n"); - panda->set_usb_power_mode(cereal::HealthData::UsbPowerMode::CLIENT); - LOGW("POWER DOWN DEVICE\n"); - system("service call power 17 i32 0 i32 1"); - } - } - if (!no_ignition_exp && (voltage_f > VBATT_START_CHARGING) && !cdp_mode) { - LOGW("TURN ON CHARGING!\n"); - panda->set_usb_power_mode(cereal::HealthData::UsbPowerMode::CDP); - } -#endif - #ifndef __x86_64__ bool power_save_desired = !ignition; if (health.power_save_enabled != power_save_desired){ @@ -427,6 +399,9 @@ void hardware_control_thread() { uint16_t prev_fan_speed = 999; uint16_t ir_pwr = 0; uint16_t prev_ir_pwr = 999; +#ifdef QCOM + bool prev_charging_disabled = false; +#endif unsigned int cnt = 0; while (!do_exit && panda->connected) { @@ -434,11 +409,27 @@ void hardware_control_thread() { sm.update(1000); // TODO: what happens if EINTR is sent while in sm.update? if (sm.updated("thermal")){ + // Fan speed uint16_t fan_speed = sm["thermal"].getThermal().getFanSpeed(); if (fan_speed != prev_fan_speed || cnt % 100 == 0){ panda->set_fan_speed(fan_speed); prev_fan_speed = fan_speed; } + +#ifdef QCOM + // Charging mode + bool charging_disabled = sm["thermal"].getThermal().getChargingDisabled(); + if (charging_disabled != prev_charging_disabled){ + if (charging_disabled){ + panda->set_usb_power_mode(cereal::HealthData::UsbPowerMode::CLIENT); + LOGW("TURN OFF CHARGING!\n"); + } else { + panda->set_usb_power_mode(cereal::HealthData::UsbPowerMode::CDP); + LOGW("TURN ON CHARGING!\n"); + } + prev_charging_disabled = charging_disabled; + } +#endif } if (sm.updated("frontFrame")){ auto event = sm["frontFrame"]; diff --git a/selfdrive/thermald/__init__.py b/selfdrive/thermald/__init__.py new file mode 100644 index 0000000000..e69de29bb2 diff --git a/selfdrive/thermald/power_monitoring.py b/selfdrive/thermald/power_monitoring.py index 6bf26ee0ea..cd4bb7d7bc 100644 --- a/selfdrive/thermald/power_monitoring.py +++ b/selfdrive/thermald/power_monitoring.py @@ -1,4 +1,3 @@ -import datetime import random import threading import time @@ -6,10 +5,19 @@ from statistics import mean from cereal import log from common.realtime import sec_since_boot +from common.params import Params, put_nonblocking from selfdrive.swaglog import cloudlog PANDA_OUTPUT_VOLTAGE = 5.28 +CAR_VOLTAGE_LOW_PASS_K = 0.091 # LPF gain for 5s tau (dt/tau / (dt/tau + 1)) +# A C2 uses about 1W while idling, and 30h seens like a good shutoff for most cars +# While driving, a battery charges completely in about 30-60 minutes +CAR_BATTERY_CAPACITY_uWh = 30e6 +CAR_CHARGING_RATE_W = 45 + +VBATT_PAUSE_CHARGING = 11.0 +MAX_TIME_OFFROAD_S = 30*3600 # Parameters def get_battery_capacity(): @@ -36,7 +44,7 @@ def get_usb_present(): def get_battery_charging(): # This does correspond with actually charging - return _read_param("/sys/class/power_supply/battery/charge_type", lambda x: x.strip() != "N/A", False) + return _read_param("/sys/class/power_supply/battery/charge_type", lambda x: x.strip() != "N/A", True) def set_battery_charging(on): @@ -60,91 +68,117 @@ def panda_current_to_actual_current(panda_current): class PowerMonitoring: def __init__(self): + self.params = Params() self.last_measurement_time = None # Used for integration delta + self.last_save_time = 0 # Used for saving current value in a param self.power_used_uWh = 0 # Integrated power usage in uWh since going into offroad self.next_pulsed_measurement_time = None + self.car_voltage_mV = 12e3 # Low-passed version of health voltage self.integration_lock = threading.Lock() + car_battery_capacity_uWh = self.params.get("CarBatteryCapacity") + if car_battery_capacity_uWh is None: + car_battery_capacity_uWh = 0 + + # Reset capacity if it's low + self.car_battery_capacity_uWh = max((CAR_BATTERY_CAPACITY_uWh / 10), int(car_battery_capacity_uWh)) + + # Calculation tick def calculate(self, health): try: now = sec_since_boot() - # Check that time is valid - if datetime.datetime.fromtimestamp(now).year < 2019: - return - - # Only integrate when there is no ignition # If health is None, we're probably not in a car, so we don't care - if health is None or (health.health.ignitionLine or health.health.ignitionCan) or \ - health.health.hwType == log.HealthData.HwType.unknown: + if health is None or health.health.hwType == log.HealthData.HwType.unknown: with self.integration_lock: self.last_measurement_time = None self.next_pulsed_measurement_time = None self.power_used_uWh = 0 return + # Low-pass battery voltage + self.car_voltage_mV = ((health.health.voltage * CAR_VOLTAGE_LOW_PASS_K) + (self.car_voltage_mV * (1 - CAR_VOLTAGE_LOW_PASS_K))) + + # Cap the car battery power and save it in a param every 10-ish seconds + self.car_battery_capacity_uWh = max(self.car_battery_capacity_uWh, 0) + self.car_battery_capacity_uWh = min(self.car_battery_capacity_uWh, CAR_BATTERY_CAPACITY_uWh) + if now - self.last_save_time >= 10: + put_nonblocking("CarBatteryCapacity", str(int(self.car_battery_capacity_uWh))) + self.last_save_time = now + # First measurement, set integration time with self.integration_lock: if self.last_measurement_time is None: self.last_measurement_time = now return - is_uno = health.health.hwType == log.HealthData.HwType.uno - # Get current power draw somehow - current_power = 0 - if get_battery_status() == 'Discharging': - # If the battery is discharging, we can use this measurement - # On C2: this is low by about 10-15%, probably mostly due to UNO draw not being factored in - current_power = ((get_battery_voltage() / 1000000) * (get_battery_current() / 1000000)) - elif (health.health.hwType in [log.HealthData.HwType.whitePanda, log.HealthData.HwType.greyPanda]) and (health.health.current > 1): - # If white/grey panda, use the integrated current measurements if the measurement is not 0 - # If the measurement is 0, the current is 400mA or greater, and out of the measurement range of the panda - # This seems to be accurate to about 5% - current_power = (PANDA_OUTPUT_VOLTAGE * panda_current_to_actual_current(health.health.current)) - elif (self.next_pulsed_measurement_time is not None) and (self.next_pulsed_measurement_time <= now): - # TODO: Figure out why this is off by a factor of 3/4??? - FUDGE_FACTOR = 1.33 - - # Turn off charging for about 10 sec in a thread that does not get killed on SIGINT, and perform measurement here to avoid blocking thermal - def perform_pulse_measurement(now): - try: - set_battery_charging(False) - time.sleep(5) - - # Measure for a few sec to get a good average - voltages = [] - currents = [] - for _ in range(6): - voltages.append(get_battery_voltage()) - currents.append(get_battery_current()) - time.sleep(1) - current_power = ((mean(voltages) / 1000000) * (mean(currents) / 1000000)) - - self._perform_integration(now, current_power * FUDGE_FACTOR) - - # Enable charging again - set_battery_charging(True) - except Exception: - cloudlog.exception("Pulsed power measurement failed") - - # Start pulsed measurement and return - threading.Thread(target=perform_pulse_measurement, args=(now,)).start() - self.next_pulsed_measurement_time = None - return - - elif self.next_pulsed_measurement_time is None and not is_uno: - # On a charging EON with black panda, or drawing more than 400mA out of a white/grey one - # Only way to get the power draw is to turn off charging for a few sec and check what the discharging rate is - # We shouldn't do this very often, so make sure it has been some long-ish random time interval - self.next_pulsed_measurement_time = now + random.randint(120, 180) - return + if (health.health.ignitionLine or health.health.ignitionCan): + # If there is ignition, we integrate the charging rate of the car + with self.integration_lock: + self.power_used_uWh = 0 + integration_time_h = (now - self.last_measurement_time) / 3600 + if integration_time_h < 0: + raise ValueError(f"Negative integration time: {integration_time_h}h") + self.car_battery_capacity_uWh += (CAR_CHARGING_RATE_W * 1e6 * integration_time_h) + self.last_measurement_time = now else: - # Do nothing - return + # No ignition, we integrate the offroad power used by the device + is_uno = health.health.hwType == log.HealthData.HwType.uno + # Get current power draw somehow + current_power = 0 + if get_battery_status() == 'Discharging': + # If the battery is discharging, we can use this measurement + # On C2: this is low by about 10-15%, probably mostly due to UNO draw not being factored in + current_power = ((get_battery_voltage() / 1000000) * (get_battery_current() / 1000000)) + elif (health.health.hwType in [log.HealthData.HwType.whitePanda, log.HealthData.HwType.greyPanda]) and (health.health.current > 1): + # If white/grey panda, use the integrated current measurements if the measurement is not 0 + # If the measurement is 0, the current is 400mA or greater, and out of the measurement range of the panda + # This seems to be accurate to about 5% + current_power = (PANDA_OUTPUT_VOLTAGE * panda_current_to_actual_current(health.health.current)) + elif (self.next_pulsed_measurement_time is not None) and (self.next_pulsed_measurement_time <= now): + # TODO: Figure out why this is off by a factor of 3/4??? + FUDGE_FACTOR = 1.33 - # Do the integration - self._perform_integration(now, current_power) + # Turn off charging for about 10 sec in a thread that does not get killed on SIGINT, and perform measurement here to avoid blocking thermal + def perform_pulse_measurement(now): + try: + set_battery_charging(False) + time.sleep(5) + + # Measure for a few sec to get a good average + voltages = [] + currents = [] + for _ in range(6): + voltages.append(get_battery_voltage()) + currents.append(get_battery_current()) + time.sleep(1) + current_power = ((mean(voltages) / 1000000) * (mean(currents) / 1000000)) + + self._perform_integration(now, current_power * FUDGE_FACTOR) + + # Enable charging again + set_battery_charging(True) + except Exception: + cloudlog.exception("Pulsed power measurement failed") + + # Start pulsed measurement and return + threading.Thread(target=perform_pulse_measurement, args=(now,)).start() + self.next_pulsed_measurement_time = None + return + + elif self.next_pulsed_measurement_time is None and not is_uno: + # On a charging EON with black panda, or drawing more than 400mA out of a white/grey one + # Only way to get the power draw is to turn off charging for a few sec and check what the discharging rate is + # We shouldn't do this very often, so make sure it has been some long-ish random time interval + self.next_pulsed_measurement_time = now + random.randint(120, 180) + return + else: + # Do nothing + return + + # Do the integration + self._perform_integration(now, current_power) except Exception: cloudlog.exception("Power monitoring calculation failed") @@ -157,6 +191,7 @@ class PowerMonitoring: if power_used < 0: raise ValueError(f"Negative power used! Integration time: {integration_time_h} h Current Power: {power_used} uWh") self.power_used_uWh += power_used + self.car_battery_capacity_uWh -= power_used self.last_measurement_time = t except Exception: cloudlog.exception("Integration failed") @@ -164,3 +199,37 @@ class PowerMonitoring: # Get the power usage def get_power_used(self): return int(self.power_used_uWh) + + def get_car_battery_capacity(self): + return int(self.car_battery_capacity_uWh) + + # See if we need to disable charging + def should_disable_charging(self, health, offroad_timestamp): + if health is None or offroad_timestamp is None: + return False + + now = sec_since_boot() + disable_charging = False + disable_charging |= (now - offroad_timestamp) > MAX_TIME_OFFROAD_S + disable_charging |= (self.car_voltage_mV < (VBATT_PAUSE_CHARGING * 1e3)) + disable_charging |= (self.car_battery_capacity_uWh <= 0) + disable_charging &= (not health.health.ignitionLine and not health.health.ignitionCan) + disable_charging &= (self.params.get("DisablePowerDown") != b"1") + return disable_charging + + # See if we need to shutdown + def should_shutdown(self, health, offroad_timestamp, started_seen, LEON): + if health is None or offroad_timestamp is None: + return False + + now = sec_since_boot() + panda_charging = (health.health.usbPowerMode != log.HealthData.UsbPowerMode.client) + BATT_PERC_OFF = 10 if LEON else 3 + + should_shutdown = False + # Wait until we have shut down charging before powering down + should_shutdown |= (not panda_charging and self.should_disable_charging(health, offroad_timestamp)) + should_shutdown |= ((get_battery_capacity() < BATT_PERC_OFF) and (not get_battery_charging()) and ((now - offroad_timestamp) > 60)) + should_shutdown &= started_seen + return should_shutdown + diff --git a/selfdrive/thermald/tests/__init__.py b/selfdrive/thermald/tests/__init__.py new file mode 100644 index 0000000000..e69de29bb2 diff --git a/selfdrive/thermald/tests/test_power_monitoring.py b/selfdrive/thermald/tests/test_power_monitoring.py new file mode 100755 index 0000000000..af46e447e5 --- /dev/null +++ b/selfdrive/thermald/tests/test_power_monitoring.py @@ -0,0 +1,221 @@ +#!/usr/bin/env python3 +import unittest +from unittest.mock import patch +from parameterized import parameterized + +from cereal import log +import cereal.messaging as messaging +from common.params import Params +params = Params() + +# Create fake time +ssb = 0 +def mock_sec_since_boot(): + global ssb + ssb += 1 + return ssb + +with patch("common.realtime.sec_since_boot", new=mock_sec_since_boot): + with patch("common.params.put_nonblocking", new=params.put): + from selfdrive.thermald.power_monitoring import PowerMonitoring, CAR_BATTERY_CAPACITY_uWh, \ + PANDA_OUTPUT_VOLTAGE, CAR_CHARGING_RATE_W, \ + VBATT_PAUSE_CHARGING + +def actual_current_to_panda_current(actual_current): + return max(int(((3.3 - (actual_current * 8.25)) * 4096) / 3.3), 0) + +TEST_DURATION_S = 50 +ALL_PANDA_TYPES = [(hw_type,) for hw_type in [log.HealthData.HwType.whitePanda, + log.HealthData.HwType.greyPanda, + log.HealthData.HwType.blackPanda, + log.HealthData.HwType.uno]] + +def pm_patch(name, value, constant=False): + if constant: + return patch(f"selfdrive.thermald.power_monitoring.{name}", value) + return patch(f"selfdrive.thermald.power_monitoring.{name}", return_value=value) + +class TestPowerMonitoring(unittest.TestCase): + def setUp(self): + # Clear stored capacity before each test + params.delete("CarBatteryCapacity") + params.delete("DisablePowerDown") + + def mock_health(self, ignition, hw_type, car_voltage=12, current=0): + health = messaging.new_message('health') + health.health.hwType = hw_type + health.health.voltage = car_voltage * 1e3 + health.health.current = actual_current_to_panda_current(current) + health.health.ignitionLine = ignition + health.health.ignitionCan = False + return health + + # Test to see that it doesn't do anything when health is None + def test_health_present(self): + pm = PowerMonitoring() + for _ in range(10): + pm.calculate(None) + self.assertEqual(pm.get_power_used(), 0) + self.assertEqual(pm.get_car_battery_capacity(), (CAR_BATTERY_CAPACITY_uWh / 10)) + + # Test to see that it doesn't integrate offroad when ignition is True + @parameterized.expand(ALL_PANDA_TYPES) + def test_offroad_ignition(self, hw_type): + pm = PowerMonitoring() + for _ in range(10): + pm.calculate(self.mock_health(True, hw_type)) + self.assertEqual(pm.get_power_used(), 0) + + # Test to see that it integrates with white/grey panda while charging + @parameterized.expand([(log.HealthData.HwType.whitePanda,), (log.HealthData.HwType.greyPanda,)]) + def test_offroad_integration_white(self, hw_type): + with pm_patch("get_battery_voltage", 4e6), pm_patch("get_battery_current", 1e5), pm_patch("get_battery_status", "Charging"): + pm = PowerMonitoring() + for _ in range(TEST_DURATION_S + 1): + pm.calculate(self.mock_health(False, hw_type, current=0.1)) + expected_power_usage = ((TEST_DURATION_S/3600) * (0.1 * PANDA_OUTPUT_VOLTAGE) * 1e6) + self.assertLess(abs(pm.get_power_used() - expected_power_usage), 10) + + # Test to see that it integrates with discharging battery + @parameterized.expand(ALL_PANDA_TYPES) + def test_offroad_integration_discharging(self, hw_type): + BATT_VOLTAGE = 4 + BATT_CURRENT = 1 + with pm_patch("get_battery_voltage", BATT_VOLTAGE * 1e6), pm_patch("get_battery_current", BATT_CURRENT * 1e6), \ + pm_patch("get_battery_status", "Discharging"): + pm = PowerMonitoring() + for _ in range(TEST_DURATION_S + 1): + pm.calculate(self.mock_health(False, hw_type)) + expected_power_usage = ((TEST_DURATION_S/3600) * (BATT_VOLTAGE * BATT_CURRENT) * 1e6) + self.assertLess(abs(pm.get_power_used() - expected_power_usage), 10) + + # Test to check positive integration of car_battery_capacity + @parameterized.expand(ALL_PANDA_TYPES) + def test_car_battery_integration_onroad(self, hw_type): + BATT_VOLTAGE = 4 + BATT_CURRENT = 1 + with pm_patch("get_battery_voltage", BATT_VOLTAGE * 1e6), pm_patch("get_battery_current", BATT_CURRENT * 1e6), \ + pm_patch("get_battery_status", "Discharging"): + pm = PowerMonitoring() + pm.car_battery_capacity_uWh = 0 + for _ in range(TEST_DURATION_S + 1): + pm.calculate(self.mock_health(True, hw_type)) + expected_capacity = ((TEST_DURATION_S/3600) * CAR_CHARGING_RATE_W * 1e6) + self.assertLess(abs(pm.get_car_battery_capacity() - expected_capacity), 10) + + # Test to check positive integration upper limit + @parameterized.expand(ALL_PANDA_TYPES) + def test_car_battery_integration_upper_limit(self, hw_type): + BATT_VOLTAGE = 4 + BATT_CURRENT = 1 + with pm_patch("get_battery_voltage", BATT_VOLTAGE * 1e6), pm_patch("get_battery_current", BATT_CURRENT * 1e6), \ + pm_patch("get_battery_status", "Discharging"): + pm = PowerMonitoring() + pm.car_battery_capacity_uWh = CAR_BATTERY_CAPACITY_uWh - 1000 + for _ in range(TEST_DURATION_S + 1): + pm.calculate(self.mock_health(True, hw_type)) + estimated_capacity = CAR_BATTERY_CAPACITY_uWh + (CAR_CHARGING_RATE_W / 3600 * 1e6) + self.assertLess(abs(pm.get_car_battery_capacity() - estimated_capacity), 10) + + # Test to check negative integration of car_battery_capacity + @parameterized.expand(ALL_PANDA_TYPES) + def test_car_battery_integration_offroad(self, hw_type): + BATT_VOLTAGE = 4 + BATT_CURRENT = 1 + with pm_patch("get_battery_voltage", BATT_VOLTAGE * 1e6), pm_patch("get_battery_current", BATT_CURRENT * 1e6), \ + pm_patch("get_battery_status", "Discharging"): + pm = PowerMonitoring() + pm.car_battery_capacity_uWh = CAR_BATTERY_CAPACITY_uWh + for _ in range(TEST_DURATION_S + 1): + pm.calculate(self.mock_health(False, hw_type)) + expected_capacity = CAR_BATTERY_CAPACITY_uWh - ((TEST_DURATION_S/3600) * (BATT_VOLTAGE * BATT_CURRENT) * 1e6) + self.assertLess(abs(pm.get_car_battery_capacity() - expected_capacity), 10) + + # Test to check negative integration lower limit + @parameterized.expand(ALL_PANDA_TYPES) + def test_car_battery_integration_lower_limit(self, hw_type): + BATT_VOLTAGE = 4 + BATT_CURRENT = 1 + with pm_patch("get_battery_voltage", BATT_VOLTAGE * 1e6), pm_patch("get_battery_current", BATT_CURRENT * 1e6), \ + pm_patch("get_battery_status", "Discharging"): + pm = PowerMonitoring() + pm.car_battery_capacity_uWh = 1000 + for _ in range(TEST_DURATION_S + 1): + pm.calculate(self.mock_health(False, hw_type)) + estimated_capacity = 0 - ((1/3600) * (BATT_VOLTAGE * BATT_CURRENT) * 1e6) + self.assertLess(abs(pm.get_car_battery_capacity() - estimated_capacity), 10) + + # Test to check policy of stopping charging after MAX_TIME_OFFROAD_S + @parameterized.expand(ALL_PANDA_TYPES) + def test_max_time_offroad(self, hw_type): + global ssb + BATT_VOLTAGE = 4 + BATT_CURRENT = 0 # To stop shutting down for other reasons + MOCKED_MAX_OFFROAD_TIME = 3600 + with pm_patch("get_battery_voltage", BATT_VOLTAGE * 1e6), pm_patch("get_battery_current", BATT_CURRENT * 1e6), \ + pm_patch("get_battery_status", "Discharging"), pm_patch("MAX_TIME_OFFROAD_S", MOCKED_MAX_OFFROAD_TIME, constant=True): + pm = PowerMonitoring() + pm.car_battery_capacity_uWh = CAR_BATTERY_CAPACITY_uWh + start_time = ssb + health = self.mock_health(False, hw_type) + while ssb <= start_time + MOCKED_MAX_OFFROAD_TIME: + pm.calculate(health) + if (ssb - start_time) % 1000 == 0 and ssb < start_time + MOCKED_MAX_OFFROAD_TIME: + self.assertFalse(pm.should_disable_charging(health, start_time)) + self.assertTrue(pm.should_disable_charging(health, start_time)) + + # Test to check policy of stopping charging when the car voltage is too low + @parameterized.expand(ALL_PANDA_TYPES) + def test_car_voltage(self, hw_type): + global ssb + BATT_VOLTAGE = 4 + BATT_CURRENT = 0 # To stop shutting down for other reasons + TEST_TIME = 100 + with pm_patch("get_battery_voltage", BATT_VOLTAGE * 1e6), pm_patch("get_battery_current", BATT_CURRENT * 1e6), \ + pm_patch("get_battery_status", "Discharging"): + pm = PowerMonitoring() + pm.car_battery_capacity_uWh = CAR_BATTERY_CAPACITY_uWh + health = self.mock_health(False, hw_type, car_voltage=(VBATT_PAUSE_CHARGING - 1)) + for i in range(TEST_TIME): + pm.calculate(health) + if i % 10 == 0: + self.assertEqual(pm.should_disable_charging(health, ssb), (pm.car_voltage_mV < VBATT_PAUSE_CHARGING*1e3)) + self.assertTrue(pm.should_disable_charging(health, ssb)) + + # Test to check policy of not stopping charging when DisablePowerDown is set + def test_disable_power_down(self): + global ssb + BATT_VOLTAGE = 4 + BATT_CURRENT = 0 # To stop shutting down for other reasons + TEST_TIME = 100 + params.put("DisablePowerDown", b"1") + with pm_patch("get_battery_voltage", BATT_VOLTAGE * 1e6), pm_patch("get_battery_current", BATT_CURRENT * 1e6), \ + pm_patch("get_battery_status", "Discharging"): + pm = PowerMonitoring() + pm.car_battery_capacity_uWh = CAR_BATTERY_CAPACITY_uWh + health = self.mock_health(False, log.HealthData.HwType.uno, car_voltage=(VBATT_PAUSE_CHARGING - 1)) + for i in range(TEST_TIME): + pm.calculate(health) + if i % 10 == 0: + self.assertFalse(pm.should_disable_charging(health, ssb)) + self.assertFalse(pm.should_disable_charging(health, ssb)) + + # Test to check policy of not stopping charging when ignition + def test_ignition(self): + global ssb + BATT_VOLTAGE = 4 + BATT_CURRENT = 0 # To stop shutting down for other reasons + TEST_TIME = 100 + with pm_patch("get_battery_voltage", BATT_VOLTAGE * 1e6), pm_patch("get_battery_current", BATT_CURRENT * 1e6), \ + pm_patch("get_battery_status", "Discharging"): + pm = PowerMonitoring() + pm.car_battery_capacity_uWh = CAR_BATTERY_CAPACITY_uWh + health = self.mock_health(True, log.HealthData.HwType.uno, car_voltage=(VBATT_PAUSE_CHARGING - 1)) + for i in range(TEST_TIME): + pm.calculate(health) + if i % 10 == 0: + self.assertFalse(pm.should_disable_charging(health, ssb)) + self.assertFalse(pm.should_disable_charging(health, ssb)) + +if __name__ == "__main__": + unittest.main() diff --git a/selfdrive/thermald/thermald.py b/selfdrive/thermald/thermald.py index 093b832da0..1a856e0494 100755 --- a/selfdrive/thermald/thermald.py +++ b/selfdrive/thermald/thermald.py @@ -144,9 +144,6 @@ def handle_fan_uno(max_cpu_temp, bat_temp, fan_speed, ignition): def thermald_thread(): - # prevent LEECO from undervoltage - BATT_PERC_OFF = 10 if LEON else 3 - health_timeout = int(1000 * 2.5 * DT_TRML) # 2.5x the expected health frequency # now loop @@ -402,15 +399,17 @@ def thermald_thread(): off_ts = sec_since_boot() os.system('echo powersave > /sys/class/devfreq/soc:qcom,cpubw/governor') - # shutdown if the battery gets lower than 3%, it's discharging, we aren't running for - # more than a minute but we were running - if msg.thermal.batteryPercent < BATT_PERC_OFF and msg.thermal.batteryStatus == "Discharging" and \ - started_seen and (sec_since_boot() - off_ts) > 60: - os.system('LD_LIBRARY_PATH="" svc power shutdown') - # Offroad power monitoring pm.calculate(health) msg.thermal.offroadPowerUsage = pm.get_power_used() + msg.thermal.carBatteryCapacity = pm.get_car_battery_capacity() + + # Check if we need to disable charging (handled by boardd) + msg.thermal.chargingDisabled = pm.should_disable_charging(health, off_ts) + + # Check if we need to shut down + if pm.should_shutdown(health, off_ts, started_seen, LEON): + os.system('LD_LIBRARY_PATH="" svc power shutdown') msg.thermal.chargingError = current_filter.x > 0. and msg.thermal.batteryPercent < 90 # if current is positive, then battery is being discharged msg.thermal.started = started_ts is not None