TSK Fixes Sample

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;

common/params.cc

@@ -189,6 +189,7 @@ std::unordered_map<std::string, uint32_t> 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},

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
 

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

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,

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

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]