Rivian: cancel command (#1910)

* add candidate cancel command and test

* fixes

* this was correct

* try spacing out the messages

* try adas status to acm to cancel

* try a bunch of stuff

* send the entire range

* try faulted

* try two faults

* try faster

* 50hz

* Revert "50hz"

This reverts commit babcbdf.

* no fault on dash

* 50hz

* see if "unavailable" also works

* try sending just 1

* 100 Hz

* Revert "100 Hz"

This reverts commit 29ae625.

* Revert "try sending just 1"

This reverts commit 64908eb.

* revert other stuff - forgot counter!

* Revert "revert other stuff - forgot counter!"

This reverts commit 5ce09b7.

* try counter

* final thing

* 100 hz

* Revert "100 hz" (faults)

This reverts commit bea3977.

* nvm we can't do this since the state management to know when openpilot stops sending cancel is too complex

* Revert "nvm we can't do this since the state management to know when openpilot stops sending cancel is too complex"

This reverts commit 19fb385.

* forward through openpilot

* try these two

* let's also try this

* are we doing this right?

* temp fault?

* ?

* try oscillating?!

* that works?!

* maybe ACM needs to see VDM status rise before VDM can request cancel with a falling edge

* try to cancel immediately (1 frame of available)

* no counter

* cmts

* 3 frames

* comments

* clean up safety

* clean up

* cc clean up

* typo

* comments

* clean this up too

* and this too

Author: sshane

opendbc/car/rivian/carcontroller.py

@@ -1,7 +1,7 @@
 from opendbc.can.packer import CANPacker
 from opendbc.car import Bus, apply_driver_steer_torque_limits
 from opendbc.car.interfaces import CarControllerBase
-from opendbc.car.rivian.riviancan import create_lka_steering, create_longitudinal, create_wheel_touch
+from opendbc.car.rivian.riviancan import create_lka_steering, create_longitudinal, create_wheel_touch, create_adas_status
 from opendbc.car.rivian.values import CarControllerParams
 
 
@@ -11,6 +11,8 @@ def __init__(self, dbc_names, CP):
     self.apply_torque_last = 0
     self.packer = CANPacker(dbc_names[Bus.pt])
 
+    self.cancel_frames = 0
+
   def update(self, CC, CS, now_nanos):
     actuators = CC.actuators
     can_sends = []
@@ -31,6 +33,17 @@ def update(self, CC, CS, now_nanos):
     # Longitudinal control
     if self.CP.openpilotLongitudinalControl:
       can_sends.append(create_longitudinal(self.packer, self.frame % 15, actuators.accel, CC.enabled))
+    else:
+      interface_status = None
+      if CC.cruiseControl.cancel:
+        # if there is a noEntry, we need to send a status of "available" before the ACM will accept "unavailable"
+        # send "available" right away as the VDM itself takes a few frames to acknowledge
+        interface_status = 1 if self.cancel_frames < 5 else 0
+        self.cancel_frames += 1
+      else:
+        self.cancel_frames = 0
+
+      can_sends.append(create_adas_status(self.packer, CS.vdm_adas_status, interface_status))
 
     new_actuators = actuators.as_builder()
     new_actuators.torque = apply_torque / CarControllerParams.STEER_MAX

opendbc/car/rivian/carstate.py

@@ -13,6 +13,7 @@ def __init__(self, CP):
 
     self.acm_lka_hba_cmd = None
     self.sccm_wheel_touch = None
+    self.vdm_adas_status = None
 
   def update(self, can_parsers) -> structs.CarState:
     cp = can_parsers[Bus.pt]
@@ -86,6 +87,7 @@ def update(self, can_parsers) -> structs.CarState:
     # Messages needed by carcontroller
     self.acm_lka_hba_cmd = copy.copy(cp_cam.vl["ACM_lkaHbaCmd"])
     self.sccm_wheel_touch = copy.copy(cp.vl["SCCM_WheelTouch"])
+    self.vdm_adas_status = copy.copy(cp.vl["VDM_AdasSts"])
 
     return ret
 

opendbc/car/rivian/riviancan.py

@@ -83,3 +83,26 @@ def create_longitudinal(packer, frame, accel, enabled):
   data = packer.make_can_msg("ACM_longitudinalRequest", 0, values)[1]
   values["ACM_longitudinalRequest_Checksum"] = checksum(data[1:], 0x1D, 0x12)
   return packer.make_can_msg("ACM_longitudinalRequest", 0, values)
+
+
+def create_adas_status(packer, vdm_adas_status, interface_status):
+  values = {s: vdm_adas_status[s] for s in (
+    "VDM_AdasStatus_Checksum",
+    "VDM_AdasStatus_Counter",
+    "VDM_AdasDecelLimit",
+    "VDM_AdasDriverAccelPriorityStatus",
+    "VDM_AdasFaultStatus",
+    "VDM_AdasAccelLimit",
+    "VDM_AdasDriverModeStatus",
+    "VDM_AdasAccelRequest",
+    "VDM_AdasInterfaceStatus",
+    "VDM_AdasAccelRequestAcknowledged",
+    "VDM_AdasVehicleHoldStatus",
+  )}
+
+  if interface_status is not None:
+    values["VDM_AdasInterfaceStatus"] = interface_status
+
+  data = packer.make_can_msg("VDM_AdasSts", 2, values)[1]
+  values["VDM_AdasStatus_Checksum"] = checksum(data[1:], 0x1D, 0xD1)
+  return packer.make_can_msg("VDM_AdasSts", 2, values)

opendbc/dbc/rivian_primary_actuator.dbc

@@ -185,7 +185,7 @@ BO_ 354 VDM_AdasSts: 8 VDM
  SG_ VDM_AdasStatus_Checksum : 7|8@0+ (1,0) [0|0] "" ACM
  SG_ VDM_AdasStatus_Counter : 11|4@0+ (1,0) [0|0] "" ACM
  SG_ VDM_AdasDecelLimit : 17|10@0+ (0.01,0) [0|10.23] "m/s^2" ACM
- SG_ VDM_AdasDriverAccelPriorityStatu : 19|2@0+ (1,0) [0|3] "" ACM
+ SG_ VDM_AdasDriverAccelPriorityStatus : 19|2@0+ (1,0) [0|3] "" ACM
  SG_ VDM_AdasFaultStatus : 23|4@0+ (1,0) [0|15] "" ACM
  SG_ VDM_AdasAccelLimit : 33|10@0+ (0.01,0) [0|10.23] "m/s^2" ACM
  SG_ VDM_AdasDriverModeStatus : 36|3@0+ (1,0) [0|7] "" ACM
@@ -874,7 +874,7 @@ VAL_ 352 ACM_VehicleHoldRequired 0 "ACM_VEHICLEHOLDREQ_NO_REQUEST" 1 "ACM_VEHICL
 VAL_ 352 ACM_PrndRequired 0 "ACM_PRNDREQ_PARK" 1 "ACM_PRNDREQ_REVERSE" 2 "ACM_PRNDREQ_NEUTRAL" 3 "ACM_PRNDREQ_DRIVE" 4 "ACM_PRNDREQ_NOT_USED";
 VAL_ 352 ACM_longInterfaceEnable 0 "ACM_LONGIFEN_INIT" 1 "ACM_LONGIFEN_LONGITUDINAL_INTERFACE_ENABLE" 2 "ACM_LONGIFEN_LONGITUDINAL_INTERFACE_DISABLE" 3 "ACM_LONGIFEN_SNA";
 VAL_ 352 ACM_AccelerationRequestType 0 "ACM_ACCELREQTYPE_INIT" 1 "ACM_ACCELREQTYPE_ACCEL_NEGATIVE" 2 "ACM_ACCELREQTYPE_ACCEL_POSITIVE" 3 "ACM_ACCELREQTYPE_SNA";
-VAL_ 354 VDM_AdasDriverAccelPriorityStatu 0 "VDM_AdasDriverAccelPriorityStatus_Driver" 1 "VDM_AdasDriverAccelPriorityStatus_Adas";
+VAL_ 354 VDM_AdasDriverAccelPriorityStatus 0 "VDM_AdasDriverAccelPriorityStatus_Driver" 1 "VDM_AdasDriverAccelPriorityStatus_Adas";
 VAL_ 354 VDM_AdasFaultStatus 0 "VDM_AdasFlaultStatus_No_Fault" 1 "VDM_AdasFaultStatus_Brk_Intv" 2 "VDM_AdasFlaultStatus_Cntr_Fault" 3 "VDM_AdasFlaultStatus_Imps_Cmd" 15 "VDM_AdasFlaultStatus_Sna";
 VAL_ 354 VDM_AdasDriverModeStatus 0 "VDM_AdasDriverModeStatus_Human" 1 "VDM_AdasDriverModeStatus_Adas" 2 "VDM_AdasDriverModeStatus_Reserved" 3 "VDM_AdasDriverModeStatus_Sna";
 VAL_ 354 VDM_AdasInterfaceStatus 0 "VDM_AdasInterfaceStatus_Unavailable" 1 "VDM_AdasInterfaceStatus_Available" 2 "VDM_AdasInterfaceStatus_Enabled" 3 "VDM_AdasInterfaceStatus_Faulted";

opendbc/safety/safety/safety_rivian.h

@@ -100,6 +100,11 @@ static int rivian_fwd_hook(int bus, int addr) {
       block_msg = true;
     }
 
+    // VDM_AdasSts: for canceling stock ACC
+    if ((addr == 0x162) && !rivian_longitudinal) {
+      block_msg = true;
+    }
+
     if (!block_msg) {
       bus_fwd = 2;
     }
@@ -125,8 +130,8 @@ static int rivian_fwd_hook(int bus, int addr) {
 }
 
 static safety_config rivian_init(uint16_t param) {
-  // 0x120 = ACM_lkaHbaCmd, 0x321 = SCCM_WheelTouch
-  static const CanMsg RIVIAN_TX_MSGS[] = {{0x120, 0, 8}, {0x321, 2, 7}};
+  // 0x120 = ACM_lkaHbaCmd, 0x321 = SCCM_WheelTouch, 0x162 = VDM_AdasSts
+  static const CanMsg RIVIAN_TX_MSGS[] = {{0x120, 0, 8}, {0x321, 2, 7}, {0x162, 2, 8}};
   // 0x160 = ACM_longitudinalRequest
   static const CanMsg RIVIAN_LONG_TX_MSGS[] = {{0x120, 0, 8}, {0x321, 2, 7}, {0x160, 0, 5}};
 

opendbc/safety/tests/test_rivian.py

@@ -10,10 +10,10 @@
 
 class TestRivianSafetyBase(common.PandaCarSafetyTest, common.DriverTorqueSteeringSafetyTest, common.LongitudinalAccelSafetyTest):
 
-  TX_MSGS = [[0x120, 0], [0x321, 2]]
+  TX_MSGS = [[0x120, 0], [0x321, 2], [0x162, 2]]
   STANDSTILL_THRESHOLD = 0
   RELAY_MALFUNCTION_ADDRS = {0: (0x120,)}
-  FWD_BLACKLISTED_ADDRS = {0: [0x321], 2: [0x120]}
+  FWD_BLACKLISTED_ADDRS = {0: [0x321, 0x162], 2: [0x120]}
   FWD_BUS_LOOKUP = {0: 2, 2: 0}
 
   MAX_TORQUE = 250
@@ -64,7 +64,7 @@ def _accel_msg(self, accel: float):
     return self.packer.make_can_msg_panda("ACM_longitudinalRequest", 0, values)
 
   def test_wheel_touch(self):
-    self.safety.set_controls_allowed(True)
+    # For hiding hold wheel alert on engage
     for controls_allowed in (True, False):
       self.safety.set_controls_allowed(controls_allowed)
       values = {
@@ -85,6 +85,14 @@ def setUp(self):
     self.safety.set_safety_hooks(CarParams.SafetyModel.rivian, 0)
     self.safety.init_tests()
 
+  def test_adas_status(self):
+    # For canceling stock ACC
+    for controls_allowed in (True, False):
+      self.safety.set_controls_allowed(controls_allowed)
+      for interface_status in range(4):
+        values = {"VDM_AdasInterfaceStatus": interface_status}
+        self.assertTrue(self._tx(self.packer.make_can_msg_panda("VDM_AdasSts", 2, values)))
+
 
 class TestRivianLongitudinalSafety(TestRivianSafetyBase):