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 <adeebshihadeh@gmail.com>

.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: |

cereal

@@ -1 +1 @@
-Subproject commit d66afca4ac316456711cb80c8e8e2fe91431e1e2
+Subproject commit 0d2ce45fc681f90b33fbcd11e5d80dd294ef751b

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],

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<bool> 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<char> 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"];

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 \
+      if health is None or health.health.hwType == log.HealthData.HwType.unknown:
-         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
+      if (health.health.ignitionLine or health.health.ignitionCan):
-      # Get current power draw somehow
+        # If there is ignition, we integrate the charging rate of the car
-      current_power = 0
+        with self.integration_lock:
-      if get_battery_status() == 'Discharging':
+          self.power_used_uWh = 0
-        # If the battery is discharging, we can use this measurement
+          integration_time_h = (now - self.last_measurement_time) / 3600
-        # On C2: this is low by about 10-15%, probably mostly due to UNO draw not being factored in
+          if integration_time_h < 0:
-        current_power = ((get_battery_voltage() / 1000000) * (get_battery_current() / 1000000))
+            raise ValueError(f"Negative integration time: {integration_time_h}h")
-      elif (health.health.hwType in [log.HealthData.HwType.whitePanda, log.HealthData.HwType.greyPanda]) and (health.health.current > 1):
+          self.car_battery_capacity_uWh += (CAR_CHARGING_RATE_W * 1e6 * integration_time_h)
-        # If white/grey panda, use the integrated current measurements if the measurement is not 0
+          self.last_measurement_time = now
-        # 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
       else:
-        # Do nothing
+        # No ignition, we integrate the offroad power used by the device
-        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
 
-      # Do the integration
+        elif self.next_pulsed_measurement_time is None and not is_uno:
-      self._perform_integration(now, current_power)
+          # 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
+

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()

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