diff --git a/cereal/car.capnp b/cereal/car.capnp index cd3f35a621113c..7e6ded8cd33d10 100644 --- a/cereal/car.capnp +++ b/cereal/car.capnp @@ -548,6 +548,8 @@ struct CarParams { wheelSpeedFactor @63 :Float32; # Multiplier on wheels speeds to computer actual speeds + secOCKey @74 :Data; + struct SafetyConfig { safetyModel @0 :SafetyModel; safetyParam @3 :UInt16; diff --git a/common/params.cc b/common/params.cc index eaa71189913b32..7a880d31f62a43 100644 --- a/common/params.cc +++ b/common/params.cc @@ -189,6 +189,7 @@ std::unordered_map keys = { {"RecordFrontLock", PERSISTENT}, // for the internal fleet {"ReplayControlsState", CLEAR_ON_MANAGER_START | CLEAR_ON_ONROAD_TRANSITION}, {"RouteCount", PERSISTENT}, + {"SecOCKey", PERSISTENT}, {"SnoozeUpdate", CLEAR_ON_MANAGER_START | CLEAR_ON_OFFROAD_TRANSITION}, {"SshEnabled", PERSISTENT}, {"TermsVersion", PERSISTENT}, diff --git a/selfdrive/car/card.py b/selfdrive/car/card.py index 6f63a54c32136f..31b9f8f2863c91 100755 --- a/selfdrive/car/card.py +++ b/selfdrive/car/card.py @@ -174,7 +174,8 @@ def controls_update(self, CS: car.CarState, CC: car.CarControl): # send car controls over can now_nanos = self.can_log_mono_time if REPLAY else int(time.monotonic() * 1e9) self.last_actuators_output, can_sends = self.CI.apply(CC, now_nanos, self.frogpilot_toggles) - self.pm.send('sendcan', can_list_to_can_capnp(can_sends, msgtype='sendcan', valid=CS.canValid)) + result = can_list_to_can_capnp(can_sends, msgtype='sendcan', valid=CS.canValid) + self.pm.send('sendcan', result) self.CC_prev = CC diff --git a/selfdrive/car/secoc.py b/selfdrive/car/secoc.py index 971ea36a192a90..82279d64b1685c 100644 --- a/selfdrive/car/secoc.py +++ b/selfdrive/car/secoc.py @@ -6,7 +6,7 @@ def add_mac(key, trip_cnt, reset_cnt, msg_cnt, msg): # TODO: clean up conversion to and from hex - addr, payload, bus = msg + addr, t, payload, bus = msg reset_flag = reset_cnt & 0b11 msg_cnt_flag = msg_cnt & 0b11 payload = payload[:4] @@ -30,7 +30,7 @@ def add_mac(key, trip_cnt, reset_cnt, msg_cnt, msg): msg = payload.hex() + msg_cnt_rst_flag + mac payload = bytes.fromhex(msg) - return (addr, payload, bus) + return (addr, t, payload, bus) def build_sync_mac(key, trip_cnt, reset_cnt, id_=0xf): id_ = struct.pack('>H', id_) # 16 diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py index a34bb73d29bc19..d3e3685a711b89 100644 --- a/selfdrive/car/toyota/carcontroller.py +++ b/selfdrive/car/toyota/carcontroller.py @@ -111,7 +111,7 @@ def update(self, CC, CS, now_nanos, frogpilot_toggles): self.secoc_lta_message_counter = 0 self.secoc_prev_reset_counter = CS.secoc_synchronization['RESET_CNT'] - expected_mac = build_sync_mac(self.secoc_key, int(CS.secoc_synchronization['TRIP_CNT']), int(CS.secoc_synchronization['RESET_CNT'])) + expected_mac = build_sync_mac(self.CP.secOCKey, int(CS.secoc_synchronization['TRIP_CNT']), int(CS.secoc_synchronization['RESET_CNT'])) if int(CS.secoc_synchronization['AUTHENTICATOR']) != expected_mac and self.secoc_mismatch_counter < 100: self.secoc_mismatch_counter += 1 @@ -151,7 +151,7 @@ def update(self, CC, CS, now_nanos, frogpilot_toggles): steer_command = toyotacan.create_steer_command(self.packer, apply_steer, apply_steer_req) if self.CP.flags & ToyotaFlags.SECOC.value: # TODO: check if this slow and needs to be done by the CANPacker - steer_command = add_mac(self.secoc_key, + steer_command = add_mac(self.CP.secOCKey, int(CS.secoc_synchronization['TRIP_CNT']), int(CS.secoc_synchronization['RESET_CNT']), self.secoc_lka_message_counter, @@ -174,7 +174,7 @@ def update(self, CC, CS, now_nanos, frogpilot_toggles): if self.CP.flags & ToyotaFlags.SECOC.value: lta_steer_2 = toyotacan.create_lta_steer_command_2(self.packer, self.frame // 2) - lta_steer_2 = add_mac(self.secoc_key, + lta_steer_2 = add_mac(self.CP.secOCKey, int(CS.secoc_synchronization['TRIP_CNT']), int(CS.secoc_synchronization['RESET_CNT']), self.secoc_lta_message_counter, diff --git a/selfdrive/car/toyota/carstate.py b/selfdrive/car/toyota/carstate.py index fafd17afcca883..7891b25412d097 100644 --- a/selfdrive/car/toyota/carstate.py +++ b/selfdrive/car/toyota/carstate.py @@ -45,7 +45,6 @@ class CarState(CarStateBase): def __init__(self, CP): super().__init__(CP) can_define = CANDefine(DBC[CP.carFingerprint]["pt"]) - self.shifter_values = can_define.dv["GEAR_PACKET"]["GEAR"] self.eps_torque_scale = EPS_SCALE[CP.carFingerprint] / 100. self.cluster_speed_hyst_gap = CV.KPH_TO_MS / 2. self.cluster_min_speed = CV.KPH_TO_MS / 2. @@ -237,8 +236,12 @@ def update(self, cp, cp_cam, CC, frogpilot_toggles): fp_ret.dashboardSpeedLimit = calculate_speed_limit(cp_cam, frogpilot_toggles) - fp_ret.ecoGear = cp.vl["GEAR_PACKET"]['ECON_ON'] == 1 - fp_ret.sportGear = cp.vl["GEAR_PACKET"]['SPORT_ON_2' if self.CP.flags & ToyotaFlags.NO_DSU else 'SPORT_ON'] == 1 + if self.CP.flags & ToyotaFlags.SECOC.value: + fp_ret.ecoGear = False + fp_ret.sportGear = False + else: + fp_ret.ecoGear = cp.vl["GEAR_PACKET"]['ECON_ON'] == 1 + fp_ret.sportGear = cp.vl["GEAR_PACKET"]['SPORT_ON_2' if self.CP.flags & ToyotaFlags.NO_DSU else 'SPORT_ON'] == 1 if self.CP.carFingerprint != CAR.TOYOTA_PRIUS_V: self.lkas_previously_enabled = self.lkas_enabled diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py index 3ac81cab7e4d7b..b101d46d92579a 100644 --- a/selfdrive/car/toyota/interface.py +++ b/selfdrive/car/toyota/interface.py @@ -1,6 +1,7 @@ from cereal import car, custom from panda import Panda from panda.python import uds +from openpilot.common.params import Params from openpilot.selfdrive.car.toyota.values import Ecu, CAR, DBC, ToyotaFlags, CarControllerParams, TSS2_CAR, RADAR_ACC_CAR, NO_DSU_CAR, \ MIN_ACC_SPEED, EPS_SCALE, UNSUPPORTED_DSU_CAR, NO_STOP_TIMER_CAR, ANGLE_CONTROL_CAR, STOP_AND_GO_CAR from openpilot.selfdrive.car import create_button_events, get_safety_config @@ -29,7 +30,7 @@ def _get_params(ret, candidate, fingerprint, car_fw, disable_openpilot_long, exp ret.safetyConfigs[0].safetyParam |= Panda.FLAG_TOYOTA_ALT_BRAKE if ret.flags & ToyotaFlags.SECOC.value: - ret.secOcRequired = True + # ret.secOcRequired = True ret.safetyConfigs[0].safetyParam |= Panda.FLAG_TOYOTA_SECOC if candidate in ANGLE_CONTROL_CAR: @@ -147,6 +148,16 @@ def _get_params(ret, candidate, fingerprint, car_fw, disable_openpilot_long, exp # to a negative value, so it won't matter. ret.minEnableSpeed = -1. if (candidate in STOP_AND_GO_CAR or ret.enableGasInterceptor) else MIN_ACC_SPEED + # Read SecOC key from param + if ret.flags & ToyotaFlags.SECOC.value: + key = Params().get("SecOCKey", encoding='utf8') + + # TODO: show warning, and handle setting key in CI + if key is None: + key = "0" * 32 + + ret.secOCKey = bytes.fromhex(key.strip()) + tune = ret.longitudinalTuning if candidate in TSS2_CAR: tune.kpV = [0.0]