Merge branch 'master' into ccnc-port

selfdrive/controls/controlsd.py

@@ -185,7 +185,7 @@ class Controls(ControlsExt):
       hudControl.leftLaneDepart = self.sm['driverAssistance'].leftLaneDeparture
       hudControl.rightLaneDepart = self.sm['driverAssistance'].rightLaneDeparture
 
-    if self.sm['selfdriveState'].active:
+    if self.get_lat_active(self.sm):
       CO = self.sm['carOutput']
       if self.CP.steerControlType == car.CarParams.SteerControlType.angle:
         self.steer_limited_by_safety = abs(CC.actuators.steeringAngleDeg - CO.actuatorsOutput.steeringAngleDeg) > \

selfdrive/pandad/pandad.py

@@ -12,6 +12,8 @@ from openpilot.common.params import Params
 from openpilot.system.hardware import HARDWARE
 from openpilot.common.swaglog import cloudlog
 
+from openpilot.sunnypilot.selfdrive.pandad.rivian_long_flasher import flash_rivian_long
+
 
 def get_expected_signature() -> bytes:
   try:
@@ -129,6 +131,9 @@ def main() -> None:
       for serial in panda_serials:
         pandas.append(flash_panda(serial))
 
+      # flash Rivian longitudinal upgrade panda
+      flash_rivian_long(pandas)
+
       # Ensure internal panda is present if expected
       internal_pandas = [panda for panda in pandas if panda.is_internal()]
       if HARDWARE.has_internal_panda() and len(internal_pandas) == 0:

selfdrive/ui/soundd.py

@@ -65,7 +65,7 @@ def check_selfdrive_timeout_alert(sm):
   ss_missing = time.monotonic() - sm.recv_time['selfdriveState']
 
   if ss_missing > SELFDRIVE_STATE_TIMEOUT:
-    if sm['selfdriveState'].enabled and (ss_missing - SELFDRIVE_STATE_TIMEOUT) < 10:
+    if (sm['selfdriveState'].enabled or sm['selfdriveStateSP'].mads.enabled) and (ss_missing - SELFDRIVE_STATE_TIMEOUT) < 10:
       return True
 
   return False
@@ -158,7 +158,7 @@ class Soundd(QuietMode):
     # sounddevice must be imported after forking processes
     import sounddevice as sd
 
-    sm = messaging.SubMaster(['selfdriveState', 'soundPressure'])
+    sm = messaging.SubMaster(['selfdriveState', 'selfdriveStateSP', 'soundPressure'])
 
     with self.get_stream(sd) as stream:
       rk = Ratekeeper(20)

selfdrive/ui/tests/test_soundd.py

@@ -10,8 +10,8 @@ AudibleAlert = car.CarControl.HUDControl.AudibleAlert
 
 class TestSoundd:
   def test_check_selfdrive_timeout_alert(self):
-    sm = SubMaster(['selfdriveState'])
-    pm = PubMaster(['selfdriveState'])
+    sm = SubMaster(['selfdriveState', 'selfdriveStateSP'])
+    pm = PubMaster(['selfdriveState', 'selfdriveStateSP'])
 
     for _ in range(100):
       cs = messaging.new_message('selfdriveState')
@@ -31,5 +31,31 @@ class TestSoundd:
 
     assert check_selfdrive_timeout_alert(sm)
 
+  def test_check_selfdrive_timeout_alert_mads_lateral_only(self):
+    sm = SubMaster(['selfdriveState', 'selfdriveStateSP'])
+    pm = PubMaster(['selfdriveState', 'selfdriveStateSP'])
+
+    for _ in range(100):
+      cs = messaging.new_message('selfdriveState')
+      cs.selfdriveState.enabled = False
+
+      ss_sp = messaging.new_message('selfdriveStateSP')
+      ss_sp.selfdriveStateSP.mads.enabled = True
+
+      pm.send("selfdriveState", cs)
+      pm.send("selfdriveStateSP", ss_sp)
+
+      time.sleep(0.01)
+
+      sm.update(0)
+
+      assert not check_selfdrive_timeout_alert(sm)
+
+    for _ in range(SELFDRIVE_STATE_TIMEOUT * 110):
+      sm.update(0)
+      time.sleep(0.01)
+
+    assert check_selfdrive_timeout_alert(sm)
+
   # TODO: add test with micd for checking that soundd actually outputs sounds
 

sunnypilot/selfdrive/pandad/rivian_long_flasher.py

@@ -0,0 +1,96 @@
+#!/usr/bin/env python3
+"""
+Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
+
+This file is part of sunnypilot and is licensed under the MIT License.
+See the LICENSE.md file in the root directory for more details.
+"""
+import os
+from itertools import accumulate
+
+from cereal import car, messaging
+from panda import Panda
+from openpilot.common.params import Params
+from openpilot.common.swaglog import cloudlog
+
+FW_PATH = os.path.join(os.path.dirname(os.path.abspath(__file__)), "rivian_long_fw.bin.signed")
+SECTOR_SIZES = [0x4000] * 4 + [0x10000] + [0x20000] * 11
+
+
+def _is_rivian() -> bool:
+  params = Params()
+
+  # check fixed fingerprint
+  if bundle := params.get("CarPlatformBundle"):
+    if bundle.get("brand") == "rivian":
+      return True
+
+  # check cached fingerprint
+  CP_bytes = params.get("CarParamsPersistent")
+  if CP_bytes is not None:
+    CP = messaging.log_from_bytes(CP_bytes, car.CarParams)
+    if CP.brand == "rivian":
+      return True
+
+  return False
+
+
+def _flash_static(handle, code):
+  assert Panda.flasher_present(handle)
+  last_sector = next((i + 1 for i, v in enumerate(accumulate(SECTOR_SIZES[1:])) if v > len(code)), -1)
+  assert 1 <= last_sector < 7, "Invalid firmware size"
+
+  handle.controlWrite(Panda.REQUEST_IN, 0xb1, 0, 0, b'')
+  for i in range(1, last_sector + 1):
+    handle.controlWrite(Panda.REQUEST_IN, 0xb2, i, 0, b'')
+  for i in range(0, len(code), 0x10):
+    handle.bulkWrite(2, code[i:i + 0x10])
+  try:
+    handle.controlWrite(Panda.REQUEST_IN, 0xd8, 0, 0, b'', expect_disconnect=True)
+  except Exception:
+    pass
+
+
+def _flash_panda(panda: Panda) -> None:
+  expected_sig = Panda.get_signature_from_firmware(FW_PATH)
+  if not panda.bootstub and panda.get_signature() == expected_sig:
+    cloudlog.info(f"F4 panda {panda.get_usb_serial()} already up to date")
+    return
+
+  cloudlog.info(f"Flashing F4 panda {panda.get_usb_serial()}")
+  with open(FW_PATH, "rb") as f:
+    code = f.read()
+
+  if not panda.bootstub:
+    # enter bootstub directly, panda.reset() rejects deprecated hw types
+    try:
+      panda._handle.controlWrite(Panda.REQUEST_IN, 0xd1, 1, 0, b'', timeout=15000, expect_disconnect=True)
+    except Exception:
+      pass
+    panda.close()
+    panda.reconnect()
+
+  _flash_static(panda._handle, code)
+  panda.reconnect()
+
+
+def flash_rivian_long(pandas: list[Panda]) -> None:
+  if not os.path.isfile(FW_PATH):
+    cloudlog.error(f"Rivian longitudinal upgrade firmware not found at {FW_PATH}")
+    return
+
+  if not _is_rivian():
+    cloudlog.info("Not a Rivian, skipping longitudinal upgrade...")
+    return
+
+  for panda in pandas:
+    # only flash external black pandas (HW_TYPE_BLACK = 0x03)
+    if panda.get_type() == b'\x03' and not panda.is_internal():
+      try:
+        _flash_panda(panda)
+      except Exception:
+        cloudlog.exception(f"Failed to flash F4 panda {panda.get_usb_serial()}")
+
+
+if __name__ == '__main__':
+  flash_rivian_long([Panda(s) for s in Panda.list()])