got controls

selfdrive/car/car_helpers.py

@@ -150,6 +150,7 @@ def fingerprint(logcan, sendcan):
     # bail if no cars left or we've been waiting for more than 2s
     failed = (all(len(cc) == 0 for cc in candidate_cars.values()) and frame > frame_fingerprint) or frame > 200
     car_fingerprint = "ALBATROSS"
+    # car_fingerprint = "TOYOTA COROLLA 2017"
     succeeded = car_fingerprint is not None
     done = failed or succeeded
 

selfdrive/car/ocelot/carcontroller.py

@@ -1,8 +1,8 @@
 from cereal import car
 from common.numpy_fast import clip
-from selfdrive.car import apply_toyota_steer_torque_limits
-from selfdrive.car.ocelot.ocelotcan import create_steer_command, create_gas_command, create_brake_cmd
-from selfdrive.car.ocelot.values import SteerLimitParams
+from selfdrive.car import apply_toyota_steer_torque_limits, make_can_msg
+from selfdrive.car.ocelot.ocelotcan import create_toyota_steer_command, create_steer_command, create_gas_command, create_brake_cmd
+from selfdrive.car.ocelot.values import CarControllerParams, STATIC_MSGS
 from opendbc.can.packer import CANPacker
 
 VisualAlert = car.CarControl.HUDControl.VisualAlert
@@ -63,8 +63,8 @@ def update(self, enabled, CS, frame, actuators, pcm_cancel_cmd, hud_alert,
     apply_accel = clip(apply_accel * ACCEL_SCALE, ACCEL_MIN, ACCEL_MAX)
 
     # steer torque
-    new_steer = int(round(actuators.steer * SteerLimitParams.STEER_MAX))
-    apply_steer = apply_toyota_steer_torque_limits(new_steer, self.last_steer, CS.out.steeringTorqueEps, SteerLimitParams)
+    new_steer = int(round(actuators.steer * CarControllerParams.STEER_MAX))
+    apply_steer = apply_toyota_steer_torque_limits(new_steer, self.last_steer, CS.out.steeringTorqueEps, CarControllerParams)
     self.steer_rate_limited = new_steer != apply_steer
 
     # # only cut torque when steer state is a known fault
@@ -96,7 +96,8 @@ def update(self, enabled, CS, frame, actuators, pcm_cancel_cmd, hud_alert,
     # 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, frame))
+    # can_sends.append(create_steer_command(self.packer, apply_steer, apply_steer_req, frame))
+    can_sends.append(create_toyota_steer_command(self.packer, apply_steer, apply_steer_req, frame))
 
     if (frame % 2 == 0):
       # send exactly zero if apply_gas is zero. Interceptor will send the max between read value and apply_gas.
@@ -117,8 +118,8 @@ def update(self, enabled, CS, frame, actuators, pcm_cancel_cmd, hud_alert,
 
     # #*** static msgs ***
 
-    # for (addr, ecu, cars, bus, fr_step, vl) in STATIC_MSGS:
-    #   if frame % fr_step == 0 and ecu in self.fake_ecus and CS.CP.carFingerprint in cars:
-    #     can_sends.append(make_can_msg(addr, vl, bus))
+    for (addr, ecu, cars, bus, fr_step, vl) in STATIC_MSGS:
+      if frame % fr_step == 0 and CS.CP.carFingerprint in cars:
+        can_sends.append(make_can_msg(addr, vl, bus))
 
     return can_sends

selfdrive/car/ocelot/ocelotcan.py

@@ -1,20 +1,53 @@
-# def create_steer_command(packer, steer, mode, raw_cnt):
-#   """Creates a CAN message for the Seb Smith EPAS Steer Command."""
+def create_steer_command(packer, steer, mode, raw_cnt):
+  """Creates a CAN message for the Seb Smith EPAS Steer Command."""
 
-#   values = {
-#     "STEER_MODE": mode,
-#     "REQUESTED_STEER_TORQUE": steer,
-#     "COUNTER": raw_cnt,
-#   }
-#   return packer.make_can_msg("OCELOT_STEERING_COMMAND", 2, values)
+  values = {
+    "STEER_MODE": mode,
+    "REQUESTED_STEER_TORQUE": steer,
+    "COUNTER": raw_cnt,
+  }
+  return packer.make_can_msg("OCELOT_STEERING_COMMAND", 0, values)
+
+def toyota_checksum(addr, dat, len):
+  cksum = 0
+  for i in dat:
+    cksum += i
+  addr1 = (addr & 0xFF)
+  addr2 = (addr >> 8)
+  cksum += addr1 + addr2 + len
+  cksum = cksum & 0xFF
+  print(''.join('{:02x}'.format(cksum)))
+  return cksum
+
+def create_toyota_steer_command(packer, steer, steer_req, raw_cnt):
+  """Creates a CAN message for the Toyota Steer Command."""
+
+  values = {
+    "STEER_REQUEST": steer_req,
+    "STEER_TORQUE_CMD": steer,
+    "TOYOTA_COUNTER": raw_cnt,
+    "SET_ME_1": 1,
+  }
+
+  # must manually create the can structure again for checksum
+  dat1 = (values["STEER_REQUEST"] & 1) | ((values["TOYOTA_COUNTER"] << 1) & 0x3F) | (1 << 7)
+  dat2 = (values["STEER_TORQUE_CMD"] >> 8) & 0xFF
+  dat3 = (values["STEER_TORQUE_CMD"]) & 0xFF
+  dat = [dat1,dat2,dat3]
+  for i in dat:
+    print(''.join('{:02x}'.format(i)))
+
+  values["TOYOTA_CHECKSUM"] = toyota_checksum(0x2E4, dat, 5)
+
+  return packer.make_can_msg("TOYOTA_STEERING_LKA", 0, values)
 
 def create_steer_command(packer, steer, enabled, raw_cnt):
   values = {
     "REQUESTED_STEER_TORQUE": steer if enabled else 0.,
     "STEER_MODE": 1 if enabled else 0,
     "COUNTER": raw_cnt,
   }
-  return packer.make_can_msg("OCELOT_STEERING_COMMAND", 2, values)
+  return packer.make_can_msg("OCELOT_STEERING_COMMAND", 0, values)
 
 def create_gas_command(packer, gas_amount, idx):
   # Common gas pedal msg generator
@@ -29,7 +62,7 @@ def create_gas_command(packer, gas_amount, idx):
     values["GAS_COMMAND"] = gas_amount * 2500
     values["GAS_COMMAND2"] = gas_amount * 1250
 
-  return packer.make_can_msg("PEDAL_GAS_COMMAND", 2, values)
+  return packer.make_can_msg("PEDAL_GAS_COMMAND", 0, values)
 
 def create_brake_cmd(packer, enabled, brake, raw_cnt):
   values = {
@@ -38,4 +71,4 @@ def create_brake_cmd(packer, enabled, brake, raw_cnt):
     "BRAKE_MODE": 2 if enabled else 0,
     "COUNTER" : raw_cnt,
   }
-  return packer.make_can_msg("OCELOT_BRAKE_COMMAND", 2, values)
+  return packer.make_can_msg("OCELOT_BRAKE_COMMAND", 0, values)

selfdrive/car/ocelot/values.py

@@ -4,12 +4,18 @@
 from cereal import car
 Ecu = car.CarParams.Ecu
 
-# Steer torque limits
-class SteerLimitParams:
-  STEER_MAX = 1024 #1500
-  STEER_DELTA_UP = 64 #25       # 1.5s time to peak torque
-  STEER_DELTA_DOWN = 64 #25     # always lower than 45 otherwise the Rav4 faults (Prius seems ok with 50)
-  STEER_ERROR_MAX = 1024 #1500     # max delta between torque cmd and torque motor
+# # Steer torque limits
+# class SteerLimitParams:
+#   STEER_MAX = 1024 #1500
+#   STEER_DELTA_UP = 64 #25       # 1.5s time to peak torque
+#   STEER_DELTA_DOWN = 64 #25     # always lower than 45 otherwise the Rav4 faults (Prius seems ok with 50)
+#   STEER_ERROR_MAX = 1024 #1500     # max delta between torque cmd and torque motor
+
+class CarControllerParams:
+  STEER_MAX = 1500
+  STEER_DELTA_UP = 5    #10    # 1.5s time to peak torque
+  STEER_DELTA_DOWN = 10 #25    # always lower than 45 otherwise the Rav4 faults (Prius seems ok with 50)
+  STEER_ERROR_MAX = 1500     # max delta between torque cmd and torque motor
 
 class CAR:
   SMART_ROADSTER_COUPE = "SMART ROADSTER COUPE 2003-2006"
@@ -24,6 +30,19 @@ class CAR:
   "gapAdjustCruise": False
 }
 
+# addr: (ecu, cars, bus, 1/freq*100, vl)
+STATIC_MSGS = [
+  # (0x128, Ecu.dsu, (CAR.ALBATROSS), 1,   3, b'\xf4\x01\x90\x83\x00\x37'),
+  # (0x141, Ecu.dsu, (CAR.ALBATROSS), 1,   2, b'\x00\x00\x00\x46'),
+  # (0x160, Ecu.dsu, (CAR.ALBATROSS), 1,   7, b'\x00\x00\x08\x12\x01\x31\x9c\x51'),
+  # (0x161, Ecu.dsu, (CAR.ALBATROSS), 1,   7, b'\x00\x1e\x00\x00\x00\x80\x07'),
+  (0x283, Ecu.dsu, (CAR.ALBATROSS), 0,   3, b'\x00\x00\x00\x00\x00\x00\x8c'),
+  (0x344, Ecu.dsu, (CAR.ALBATROSS), 0,   5, b'\x00\x00\x01\x00\x00\x00\x00\x50'),
+  (0x365, Ecu.dsu, (CAR.ALBATROSS), 0,  20, b'\x00\x00\x00\x80\xfc\x00\x08'),
+  (0x366, Ecu.dsu, (CAR.ALBATROSS), 0,  20, b'\x00\x72\x07\xff\x09\xfe\x00'),
+  (0x4CB, Ecu.dsu, (CAR.ALBATROSS), 0, 100, b'\x0c\x00\x00\x00\x00\x00\x00\x00'),
+]
+
 FINGERPRINTS = {
   CAR.SMART_ROADSTER_COUPE: [{
      36: 8, 37: 8, 170: 8, 180: 1, 186: 4, 253:8, 254:8, 255: 8, 426: 6, 452: 8, 464: 8, 466: 8, 467: 8, 512: 6, 513:6, 547: 8, 548: 8, 552: 4, 608: 8, 610: 5, 643: 7, 705: 8, 740: 5, 800: 8, 835: 8, 836: 8, 849: 4, 869: 7, 870: 7, 871: 2, 896: 8, 897: 8, 900: 6, 902: 6, 905: 8, 911: 8, 916: 2, 921: 8, 933: 8, 944: 8, 945: 8, 951: 8, 955: 4, 956: 8, 979: 2, 998: 5, 999: 7, 1000: 8, 1001: 8, 1017: 8, 1041: 8, 1042: 8, 1043: 8, 1044: 8, 1056: 8, 1059: 1, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 1196: 8, 1222: 8, 1224:8, 1227: 8, 1235: 8, 1279: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1561: 8, 1562: 8, 1568: 8, 1569: 8, 1570: 8, 1571: 8, 1572: 8, 1592: 8, 1596: 8, 1597: 8, 1600: 8, 1664: 8, 1779: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8, 2016: 8, 2017: 8, 2018: 8, 2019: 8, 2020: 8, 2021: 8, 2022: 8, 2023: 8, 2024: 8

tools/carcontrols/debug_controls.py

@@ -44,10 +44,10 @@ def steer_thread():
     actuators = car.CarControl.Actuators.new_message()
 
     if joystick is not None:
-      axis_3 = clip(-joystick.testJoystick.axes[3] * 1.05, -1., 1.)          # -1 to 1
+      axis_3 = clip(-joystick.testJoystick.axes[1] * 1.05, -1., 1.)          # -1 to 1
       actuators.steer = axis_3
       actuators.steeringAngleDeg = axis_3 * 43.   # deg
-      axis_1 = clip(-joystick.testJoystick.axes[1] * 1.05, -1., 1.)          # -1 to 1
+      axis_1 = clip(-joystick.testJoystick.axes[3] * 1.05, -1., 1.)          # -1 to 1
       actuators.gas = max(axis_1, 0.)
       actuators.brake = max(-axis_1, 0.)