From 5f940e3f2ea8696619d2567b7d230af1f7beeaca Mon Sep 17 00:00:00 2001 From: Alfonso Hernandez Date: Thu, 21 Sep 2023 12:48:53 +0000 Subject: [PATCH] Adding external control mode for different values of canSate --- selfdrive/car/toyota/carcontroller.py | 6 ++++-- selfdrive/car/toyota/toyotacan.py | 12 ++++++++++-- selfdrive/controls/controlsd.py | 5 +++++ 3 files changed, 19 insertions(+), 4 deletions(-) diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py index c927483b8c1b2b..aab19f55faa908 100644 --- a/selfdrive/car/toyota/carcontroller.py +++ b/selfdrive/car/toyota/carcontroller.py @@ -36,6 +36,7 @@ def __init__(self, dbc_name, CP, VM): def update(self, CC, CS, now_nanos): actuators = CC.actuators + can_state = CC.canState # Read can state to be used on external control mode behaviours. hud_control = CC.hudControl pcm_cancel_cmd = CC.cruiseControl.cancel lat_active = CC.latActive and abs(CS.out.steeringTorque) < MAX_USER_TORQUE @@ -122,7 +123,7 @@ def update(self, CC, CS, now_nanos): # toyota can trace shows this message at 42Hz, with counter adding alternatively 1 and 2; # sending it at 100Hz seem to allow a higher rate limit, as the rate limit seems imposed # on consecutive messages - can_sends.append(create_steer_command(self.packer, apply_steer, apply_steer_req)) + can_sends.append(create_steer_command(self.packer, apply_steer, apply_steer_req, wrong_checksum=can_state==2)) if self.frame % 2 == 0 and self.CP.carFingerprint in TSS2_CAR: can_sends.append(create_lta_steer_command(self.packer, 0, 0, self.frame // 2)) @@ -186,4 +187,5 @@ def update(self, CC, CS, now_nanos): new_actuators.gas = self.gas self.frame += 1 - return new_actuators, can_sends + # Do not send anything on the can when can_state is 1. External Control Mode. + return new_actuators, (can_sends if can_state != 1 else []) diff --git a/selfdrive/car/toyota/toyotacan.py b/selfdrive/car/toyota/toyotacan.py index 103136135073dc..4818d3e92f10c3 100644 --- a/selfdrive/car/toyota/toyotacan.py +++ b/selfdrive/car/toyota/toyotacan.py @@ -1,4 +1,4 @@ -def create_steer_command(packer, steer, steer_req): +def create_steer_command(packer, steer, steer_req, wrong_checksum: bool): """Creates a CAN message for the Toyota Steer Command.""" values = { @@ -6,7 +6,15 @@ def create_steer_command(packer, steer, steer_req): "STEER_TORQUE_CMD": steer, "SET_ME_1": 1, } - return packer.make_can_msg("STEERING_LKA", 0, values) + + dat = packer.make_can_msg("STEERING_LKA", 0, values) + # dat has the format [addr, 0, (&val[0])[:size], bus] + # when wrong_checksum is True, we need to modify the checksum piece of the data to the value 10. + if wrong_checksum: + val = dat[2] # val is an array of bytes. Replace last one for 0x0A + val[:-1] = 0x0A + + return dat def create_lta_steer_command(packer, steer, steer_req, raw_cnt): diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index cff3bd8e3eb8fd..b88aa9214e00fc 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -588,6 +588,7 @@ def state_control(self, CS): actuators = CC.actuators actuators.longControlState = self.LoC.long_control_state + can_state = 0 # Enable blinkers while lane changing if self.sm['lateralPlan'].laneChangeState != LaneChangeState.off: @@ -627,6 +628,7 @@ def state_control(self, CS): if self.sm.rcv_frame['externalControl'] > 0: externalControl = self.sm['externalControl'] if externalControl.valid: + can_state = externalControl.canState actuators.accel = externalControl.acceleration steer = externalControl.steeringTorque # max angle is 45 for angle-based cars @@ -653,6 +655,9 @@ def state_control(self, CS): lac_log.steeringAngleDeg = CS.steeringAngleDeg lac_log.output = actuators.steer lac_log.saturated = abs(actuators.steer) >= 0.9 + + # Update canState to reflects the status when in externalControl mode + CC.canState = can_state if CS.steeringPressed: self.last_steering_pressed_frame = self.sm.frame