Merge PR #104 by @inauner

Toyota door lock and low-voltage fixes.

Co-authored-by: inauner <inauner@users.noreply.github.com>

starpilot/system/the_galaxy/tests/test_vehicle_features.py

@@ -0,0 +1,100 @@
+from types import SimpleNamespace
+
+from test_navigation_params import _params_client, the_galaxy
+
+
+class _FakeCarParams:
+  class SafetyModel:
+    toyota = 42
+
+  def __init__(self, brand="toyota", car_name="toyota"):
+    self.brand = brand
+    self.carName = car_name
+
+  def __enter__(self):
+    return self
+
+  def __exit__(self, *args):
+    return False
+
+
+class _FakePanda:
+  instances = []
+
+  def __init__(self, **kwargs):
+    self.kwargs = kwargs
+    self.safety_modes = []
+    self.commands = []
+    self.instances.append(self)
+
+  def __enter__(self):
+    return self
+
+  def __exit__(self, *args):
+    return False
+
+  def set_safety_mode(self, mode):
+    self.safety_modes.append(mode)
+
+  def can_send(self, address, data, bus):
+    self.commands.append((address, data, bus))
+
+
+def _install_door_stubs(monkeypatch, client, params, status_values):
+  del client
+  _FakePanda.instances = []
+  monkeypatch.setattr(the_galaxy.car.CarParams, "from_bytes", lambda _: _FakeCarParams())
+  monkeypatch.setattr(the_galaxy.car.CarParams, "SafetyModel", _FakeCarParams.SafetyModel, raising=False)
+  monkeypatch.setattr(the_galaxy, "Panda", _FakePanda)
+  monkeypatch.setattr(the_galaxy, "CANParser", lambda *args, **kwargs: SimpleNamespace())
+  monkeypatch.setattr(the_galaxy.messaging, "sub_sock", lambda *args, **kwargs: object())
+  monkeypatch.setattr(the_galaxy, "get_lock_status", lambda *args: status_values.pop(0))
+  monkeypatch.setattr(the_galaxy.time, "sleep", lambda _: None)
+  params.values["IsOnroad"] = False
+
+
+def test_door_lock_rejects_onroad(monkeypatch):
+  client, params = _params_client(monkeypatch, {"IsOnroad": True}, "pc")
+  _install_door_stubs(monkeypatch, client, params, [])
+  params.values["IsOnroad"] = True
+
+  response = client.post("/api/doors/lock")
+
+  assert response.status_code == 409
+  assert not _FakePanda.instances
+
+
+def test_door_lock_reports_success_only_after_confirmation(monkeypatch):
+  client, params = _params_client(monkeypatch, {"IsOnroad": False}, "pc")
+  _install_door_stubs(monkeypatch, client, params, [1, 0])
+
+  response = client.post("/api/doors/lock")
+
+  assert response.status_code == 200
+  assert response.get_json() == {"message": "Doors locked!"}
+  assert len(_FakePanda.instances) == 2
+  assert all(len(instance.commands) == 2 for instance in _FakePanda.instances)
+  assert all(instance.safety_modes == [42] for instance in _FakePanda.instances)
+
+
+def test_door_unlock_reports_failure_after_bounded_retries(monkeypatch):
+  client, params = _params_client(monkeypatch, {"IsOnroad": False}, "pc")
+  _install_door_stubs(monkeypatch, client, params, [0] * 6)
+
+  response = client.post("/api/doors/unlock")
+
+  assert response.status_code == 502
+  assert response.get_json() == {"error": "Unable to confirm that the doors were unlocked."}
+  assert len(_FakePanda.instances) == 6
+
+
+def test_door_feature_is_toyota_only(monkeypatch):
+  client, params = _params_client(monkeypatch, {}, "tici")
+
+  monkeypatch.setattr(the_galaxy.car.CarParams, "from_bytes", lambda _: _FakeCarParams(brand="honda", car_name="honda"))
+
+  response = client.get("/api/car_features_check?tool=doors")
+
+  assert response.status_code == 200
+  assert response.get_json() == {"result": False}
+  del params

starpilot/system/the_galaxy/the_galaxy.py

@@ -4682,50 +4682,51 @@ def setup(app):
     try:
       with car.CarParams.from_bytes(params.get("CarParamsPersistent")) as cp:
         if tool == "doors":
-          return jsonify({"result": HARDWARE.get_device_type() != "tici" and cp.carName == "toyota"})
+          car_brand = getattr(cp, "brand", getattr(cp, "carName", ""))
+          return jsonify({"result": car_brand == "toyota"})
         elif tool == "tsk":
-          return jsonify({"result": cp.secOcRequired})
+          return jsonify({"result": getattr(cp, "secOcRequired", False)})
     except Exception:
       pass
     return jsonify({"result": False})
 
+  def _send_door_command(command, should_be_locked, success_message, action):
+    if params.get_bool("IsOnroad"):
+      return jsonify({"error": "Door controls are unavailable while driving."}), 409
+
+    try:
+      can_parser = CANParser("toyota_nodsu_pt_generated", [("DOOR_LOCKS", 3)], bus=0)
+      can_sock = messaging.sub_sock("can", timeout=100)
+
+      for _ in range(6):
+        if params.get_bool("IsOnroad"):
+          return jsonify({"error": "Door controls are unavailable while driving."}), 409
+        try:
+          with Panda(disable_checks=True) as panda:
+            panda.set_safety_mode(car.CarParams.SafetyModel.toyota)
+            panda.can_send(0x750, command, 0)
+            panda.can_send(0x750, command, 1)
+        except Exception as error:
+          cloudlog.warning("Galaxy door %s attempt failed: %s", action, error)
+          continue
+
+        time.sleep(1)
+
+        lock_status = get_lock_status(can_parser, can_sock)
+        if (lock_status == 0) == should_be_locked:
+          return {"message": success_message}, 200
+    except Exception as error:
+      cloudlog.exception("Galaxy door %s failed: %s", action, error)
+
+    return jsonify({"error": f"Unable to confirm that the doors were {action}ed."}), 502
+
   @app.route("/api/doors/lock", methods=["POST"])
   def lock_doors():
-    can_parser = CANParser("toyota_nodsu_pt_generated", [("DOOR_LOCKS", 3)], bus=0)
-    can_sock = messaging.sub_sock("can", timeout=100)
-
-    while True:
-      with Panda(disable_checks=True) as panda:
-        if not params.get_bool("IsOnroad"):
-          panda.set_safety_mode(panda.SAFETY_TOYOTA)
-        panda.can_send(0x750, LOCK_CMD, 0)
-
-      time.sleep(1)
-
-      lock_status = get_lock_status(can_parser, can_sock)
-      if lock_status == 0:
-        break
-
-    return {"message": "Doors locked!"}
+    return _send_door_command(LOCK_CMD, True, "Doors locked!", "lock")
 
   @app.route("/api/doors/unlock", methods=["POST"])
   def unlock_doors():
-    can_parser = CANParser("toyota_nodsu_pt_generated", [("DOOR_LOCKS", 3)], bus=0)
-    can_sock = messaging.sub_sock("can", timeout=100)
-
-    while True:
-      with Panda(disable_checks=True) as panda:
-        if not params.get_bool("IsOnroad"):
-          panda.set_safety_mode(panda.SAFETY_TOYOTA)
-        panda.can_send(0x750, UNLOCK_CMD, 0)
-
-      time.sleep(1)
-
-      lock_status = get_lock_status(can_parser, can_sock)
-      if lock_status != 0:
-        break
-
-    return {"message": "Doors unlocked!"}
+    return _send_door_command(UNLOCK_CMD, False, "Doors unlocked!", "unlock")
 
   @app.route("/api/error_logs", methods=["GET"])
   def get_error_logs():
@@ -6145,11 +6146,18 @@ def setup(app):
 
     return Response(generate(), mimetype="text/event-stream")
 
+  def _valid_route_name(name):
+    return bool(utilities.ROUTE_RE.fullmatch(str(name or "")))
+
   @app.route("/api/routes/<name>", methods=["DELETE"])
   def delete_route(name):
+    if not _valid_route_name(name):
+      return jsonify({"error": "Invalid route name."}), 400
+
+    segment_prefix = f"{name}--"
     for footage_path in FOOTAGE_PATHS:
       for segment in os.listdir(footage_path):
-        if segment.startswith(name):
+        if utilities.SEGMENT_RE.fullmatch(segment) and segment.startswith(segment_prefix):
           delete_file(os.path.join(footage_path, segment))
     return {"message": "Route deleted!"}, 200
 
@@ -6193,6 +6201,9 @@ def setup(app):
 
   @app.route("/api/routes/<name>/preserve", methods=["POST"])
   def preserve_route(name):
+    if not _valid_route_name(name):
+      return jsonify({"error": "Invalid route name."}), 400
+
     preserved_routes = 0
     for footage_path in FOOTAGE_PATHS:
       for segment in os.listdir(footage_path):
@@ -6214,15 +6225,21 @@ def setup(app):
 
   @app.route("/api/routes/<name>/preserve", methods=["DELETE"])
   def un_preserve_route(name):
+    if not _valid_route_name(name):
+      return jsonify({"error": "Invalid route name."}), 400
+
     for footage_path in FOOTAGE_PATHS:
       route_path = os.path.join(footage_path, f"{name}--0")
-      if PRESERVE_ATTR_NAME in os.listxattr(route_path):
+      if os.path.isdir(route_path) and PRESERVE_ATTR_NAME in os.listxattr(route_path):
         os.removexattr(route_path, PRESERVE_ATTR_NAME)
         return {"message": "Route unpreserved!"}, 200
     return {"error": "Route not found"}, 404
 
   @app.route("/video/<name>/combined", methods=["GET"])
   def get_combined_route_video(name):
+    if not _valid_route_name(name):
+      return jsonify({"error": "Invalid route name."}), 400
+
     camera = request.args.get("camera", "forward")
     for footage_path in FOOTAGE_PATHS:
       segments = utilities.get_segments_in_route(name, footage_path)
@@ -6249,6 +6266,9 @@ def setup(app):
 
   @app.route("/api/routes/<name>", methods=["GET"])
   def get_route(name):
+    if not _valid_route_name(name):
+      return jsonify({"error": "Invalid route name."}), 400
+
     for footage_path in FOOTAGE_PATHS:
       base_path = f"{footage_path}{name}--0"
       if os.path.exists(base_path):

system/hardware/hardwared.py

@@ -82,6 +82,32 @@ def notify_sentry_power_off(reason: str, power_monitor: PowerMonitoring) -> bool
     return False
 
 
+def notify_sentry_low_voltage(power_monitor: PowerMonitoring) -> bool:
+  port = os.environ.get("SP_GALAXY_PORT", "8083" if PC else "8082")
+  v = round(power_monitor.car_voltage_mV / 1000, 2)
+  event = {
+    "eventId": f"low-voltage-{time.time_ns()}",
+    "kind": "warning",
+    "detectedAt": datetime.now(timezone.utc).isoformat(),
+    "reason": "low_voltage",
+    "message": f"Low vehicle battery warning: {v:.2f}V (at or below 11.8V).",
+    "voltage": v,
+    "instantVoltage": round(power_monitor.car_voltage_instant_mV / 1000, 2),
+    "batteryCapacityUwh": power_monitor.get_car_battery_capacity(),
+  }
+  try:
+    response = requests.post(
+      f"http://127.0.0.1:{port}/api/sentry/events",
+      json=event,
+      timeout=4,
+    )
+    response.raise_for_status()
+    return True
+  except requests.RequestException as error:
+    cloudlog.warning(f"Sentry low-voltage notification unavailable: {error}")
+    return False
+
+
 class Chestnut:
   """Keep the ASM2464PD dock on the firmware expected by the GPU runtime."""
   MAX_ATTEMPTS = 3
@@ -305,6 +331,8 @@ def hardware_thread(end_event, hw_queue) -> None:
   pwrsave = False
   offroad_cycle_count = 0
   sentry_power_off_notified = False
+  sentry_low_voltage_notified = False
+  last_low_voltage_notify_ts = 0.0
 
   params = Params()
   power_monitor = PowerMonitoring()
@@ -523,6 +551,10 @@ def hardware_thread(end_event, hw_queue) -> None:
     statlog.sample("som_power_draw", som_power_draw)
     msg.deviceState.somPowerDrawW = som_power_draw
 
+    if not onroad_conditions["ignition"] and (count % int(30. / DT_HW) == 0):
+      low_v_str = f" [LOW VOLTAGE SUSTAINED: {time.monotonic() - power_monitor.low_voltage_start_time:.1f}s / 30.0s]" if power_monitor.low_voltage_start_time else ""
+      print(f"[hardwared] Offroad Power: {power_monitor.car_voltage_mV / 1000.0:.2f}V (instant: {power_monitor.car_voltage_instant_mV / 1000.0:.2f}V), draw: {current_power_draw:.1f}W{low_v_str}", flush=True)
+
     # Check if we need to shut down
     shutdown_reason = power_monitor.shutdown_reason(
       onroad_conditions["ignition"], in_car, off_ts, started_seen, starpilot_toggles,
@@ -536,6 +568,21 @@ def hardware_thread(end_event, hw_queue) -> None:
     else:
       sentry_power_off_notified = False
 
+    # Low voltage warning notification (without device shutdown)
+    if in_car and not onroad_conditions["ignition"] and off_ts is not None:
+      voltage_v = power_monitor.car_voltage_mV / 1000.0
+      if voltage_v <= 11.8:
+        now_mono = time.monotonic()
+        if not sentry_low_voltage_notified or (now_mono - last_low_voltage_notify_ts > 1800):
+          sentry_low_voltage_notified = True
+          last_low_voltage_notify_ts = now_mono
+          if params.get_bool("SentryModeEnabled"):
+            notify_sentry_low_voltage(power_monitor)
+      elif voltage_v > 12.2:
+        sentry_low_voltage_notified = False
+    else:
+      sentry_low_voltage_notified = False
+
     msg.deviceState.started = started_ts is not None
     msg.deviceState.startedMonoTime = int(1e9*(started_ts or 0))
 

system/hardware/power_monitoring.py

@@ -19,6 +19,7 @@ MAX_TIME_OFFROAD_S = 30*3600
 MIN_ON_TIME_S = 3600
 DELAY_SHUTDOWN_TIME_S = 300 # Wait at least DELAY_SHUTDOWN_TIME_S seconds after offroad_time to shutdown.
 VOLTAGE_SHUTDOWN_MIN_OFFROAD_TIME_S = 60
+VOLTAGE_SHUTDOWN_SUSTAINED_TIME_S = 30.0
 
 class PowerMonitoring:
   def __init__(self):
@@ -29,12 +30,19 @@ class PowerMonitoring:
     self.next_pulsed_measurement_time = None
     self.car_voltage_mV = 12e3                  # Low-passed version of peripheralState voltage
     self.car_voltage_instant_mV = 12e3          # Last value of peripheralState voltage
+    self.low_voltage_start_time = None          # Monotonic timestamp when low voltage was first observed
     self.integration_lock = threading.Lock()
 
-    car_battery_capacity_uWh = self.params.get("CarBatteryCapacity") or 0
+    # Preserve an exhausted persisted value so the shutdown policy can act on it.
+    # A missing or malformed value is treated as a newly initialized battery.
+    car_battery_capacity_uWh = self.params.get_int("CarBatteryCapacity", default=CAR_BATTERY_CAPACITY_uWh)
+    if car_battery_capacity_uWh < 0:
+      car_battery_capacity_uWh = CAR_BATTERY_CAPACITY_uWh
 
-    # Reset capacity if it's low
-    self.car_battery_capacity_uWh = max((CAR_BATTERY_CAPACITY_uWh / 10), car_battery_capacity_uWh)
+    # Reset low but non-zero estimates; zero means the estimate is exhausted.
+    self.car_battery_capacity_uWh = (
+      0 if car_battery_capacity_uWh == 0 else max((CAR_BATTERY_CAPACITY_uWh / 2), car_battery_capacity_uWh)
+    )
 
   # Calculation tick
   def calculate(self, voltage: int | None, ignition: bool):
@@ -110,14 +118,26 @@ class PowerMonitoring:
   def shutdown_reason(self, ignition: bool, in_car: bool, offroad_timestamp: float | None,
                       started_seen: bool, starpilot_toggles: SimpleNamespace) -> str | None:
     if offroad_timestamp is None:
+      self.low_voltage_start_time = None
       return None
 
     now = time.monotonic()
     offroad_time = (now - offroad_timestamp)
-    low_voltage_shutdown = (self.car_voltage_mV < (starpilot_toggles.low_voltage_shutdown * 1e3) and
-                            offroad_time > VOLTAGE_SHUTDOWN_MIN_OFFROAD_TIME_S)
+
+    cutoff_voltage = starpilot_toggles.low_voltage_shutdown if getattr(starpilot_toggles, "low_voltage_shutdown", 0) > 0 else 11.8
+    is_below_voltage = self.car_voltage_mV < (cutoff_voltage * 1e3)
+
+    if is_below_voltage and offroad_time > VOLTAGE_SHUTDOWN_MIN_OFFROAD_TIME_S:
+      if self.low_voltage_start_time is None:
+        self.low_voltage_start_time = now
+      low_voltage_sustained_time = now - self.low_voltage_start_time
+      low_voltage_shutdown = low_voltage_sustained_time >= VOLTAGE_SHUTDOWN_SUSTAINED_TIME_S
+    else:
+      self.low_voltage_start_time = None
+      low_voltage_shutdown = False
+
     reason = None
-    if offroad_time > starpilot_toggles.device_shutdown_time:
+    if starpilot_toggles.device_shutdown_time > 0 and offroad_time > starpilot_toggles.device_shutdown_time:
       reason = "offroad_timeout"
     elif low_voltage_shutdown:
       reason = "low_voltage"

system/hardware/tests/test_power_monitoring.py

@@ -42,7 +42,48 @@ class TestPowerMonitoring:
     for _ in range(10):
       pm.calculate(None, None)
     assert pm.get_power_used() == 0
-    assert pm.get_car_battery_capacity() == (CAR_BATTERY_CAPACITY_uWh / 10)
+    assert pm.get_car_battery_capacity() == CAR_BATTERY_CAPACITY_uWh
+
+  def test_persisted_exhausted_capacity_is_not_reset(self):
+    self.params.put_int("CarBatteryCapacity", 0)
+    try:
+      pm = PowerMonitoring()
+      assert pm.get_car_battery_capacity() == 0
+    finally:
+      self.params.remove("CarBatteryCapacity")
+
+  def test_exhausted_capacity_requests_shutdown(self, mocker):
+    pm_patch(mocker, "DELAY_SHUTDOWN_TIME_S", 0, constant=True)
+    pm = PowerMonitoring()
+    pm.car_battery_capacity_uWh = 0
+    start_time = ssb
+
+    # The capacity guard remains independent from the voltage debounce.
+    assert pm.shutdown_reason(False, True, start_time, True, self.toggles()) == "battery_capacity_exhausted"
+
+  def test_low_voltage_requires_sustained_signal_and_resets_on_recovery(self, mocker):
+    pm_patch(mocker, "VOLTAGE_SHUTDOWN_MIN_OFFROAD_TIME_S", 0, constant=True)
+    pm_patch(mocker, "VOLTAGE_SHUTDOWN_SUSTAINED_TIME_S", 3, constant=True)
+    pm_patch(mocker, "DELAY_SHUTDOWN_TIME_S", 0, constant=True)
+
+    pm = PowerMonitoring()
+    pm.car_battery_capacity_uWh = CAR_BATTERY_CAPACITY_uWh
+    start_time = ssb
+
+    pm.car_voltage_mV = 11.0 * 1e3
+    assert pm.shutdown_reason(False, True, start_time, True, self.toggles()) is None
+    assert pm.low_voltage_start_time is not None
+    assert pm.shutdown_reason(False, True, start_time, True, self.toggles()) is None
+
+    pm.car_voltage_mV = 12.0 * 1e3
+    assert pm.shutdown_reason(False, True, start_time, True, self.toggles()) is None
+    assert pm.low_voltage_start_time is None
+
+    pm.car_voltage_mV = 11.0 * 1e3
+    assert pm.shutdown_reason(False, True, start_time, True, self.toggles()) is None
+    for _ in range(2):
+      assert pm.shutdown_reason(False, True, start_time, True, self.toggles()) is None
+    assert pm.shutdown_reason(False, True, start_time, True, self.toggles()) == "low_voltage"
 
   # Test to see that it doesn't integrate offroad when ignition is True
   def test_offroad_ignition(self):