Toyota SAS and mild tuning

RetroPilot · Sep 22, 2023 · e2ce83c · e2ce83c
1 parent b3b40da
commit e2ce83c
Show file tree

Hide file tree

Showing 4 changed files with 20 additions and 11 deletions.
diff --git a/selfdrive/car/ocelot/carstate.py b/selfdrive/car/ocelot/carstate.py
@@ -21,6 +21,8 @@ def __init__(self, CP):
     self.oldSpeedDn = False
     self.setSpeed = 10
     self.buttonStates = BUTTON_STATES.copy()
+    self.angle_last = 0
+    self.frame = 0
 
   def update(self, cp, cp_body):
     ret = car.CarState.new_message()
@@ -51,9 +53,12 @@ def update(self, cp, cp_body):
 
     # Toyota SAS
     # Do we need an angle sensor for demo?
-    ret.steeringAngleDeg = cp.vl["TOYOTA_STEERING_ANGLE_SENSOR1"]['TOYOTA_STEER_ANGLE'] + cp.vl["TOYOTA_STEERING_ANGLE_SENSOR1"]['TOYOTA_STEER_FRACTION']
-    ret.steeringRateDeg = cp.vl["TOYOTA_STEERING_ANGLE_SENSOR1"]['TOYOTA_STEER_RATE']
-
+    ret.steeringAngleDeg = -(cp.vl["TOYOTA_STEERING_ANGLE_SENSOR1"]['TOYOTA_STEER_ANGLE'] + cp.vl["TOYOTA_STEERING_ANGLE_SENSOR1"]['TOYOTA_STEER_FRACTION'])
+    ret.steeringRateDeg = (ret.steeringAngleDeg - self.angle_last) * 5
+    self.frame += 1
+    if self.frame == 5:
+      self.angle_last = cp.vl["TOYOTA_STEERING_ANGLE_SENSOR1"]['TOYOTA_STEER_ANGLE'] + cp.vl["TOYOTA_STEERING_ANGLE_SENSOR1"]['TOYOTA_STEER_FRACTION']
+      self.frame = 0
 
     # Steering information from smart standin ECU
     raw_torque = 0
@@ -122,13 +127,17 @@ def get_can_parser(CP):
       # ("PED_GAS2", "PEDAL_GAS_SENSOR", 0),
       ("ENABLE", "CRUISE_STATE", 0),
       ("SPEED", "CRUISE_STATE", 0),
+      ("TOYOTA_STEER_ANGLE", "TOYOTA_STEERING_ANGLE_SENSOR1", 0),
+      ("TOYOTA_STEER_FRACTION", "TOYOTA_STEERING_ANGLE_SENSOR1", 0),
+      ("TOYOTA_STEER_RATE", "TOYOTA_STEERING_ANGLE_SENSOR1", 0),
     ]
 
     checks = [
       #("OCELOT_BRAKE_STATUS", 80),
       #("PEDAL_GAS_SENSOR", 50),
       ("OCELOT_STEERING_STATUS", 80),
-      ("CRUISE_STATE", 80)
+      ("CRUISE_STATE", 80),
+      ("TOYOTA_STEERING_ANGLE_SENSOR1", 80),
     ]
 
     return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 2)

diff --git a/selfdrive/car/ocelot/interface.py b/selfdrive/car/ocelot/interface.py
@@ -34,12 +34,12 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=[]):  # py
     ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]]
     stop_and_go = True
     ret.safetyParam = 100
-    ret.wheelbase = 2.36
-    ret.steerRatio = 21
+    ret.wheelbase = 2.7
+    ret.steerRatio = 18.27
     tire_stiffness_factor = 0.444
     ret.mass = 810 + STD_CARGO_KG
-    ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.3], [0.05]]
-    ret.lateralTuning.pid.kf = 0.00007   # full torque for 20 deg at 80mph means 0.00007818594
+    ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.02], [0.05]]
+    ret.lateralTuning.pid.kf = 0.00003   # full torque for 20 deg at 80mph means 0.00007818594
 
 
     ret.steerRateCost = 1.

diff --git a/selfdrive/car/ocelot/ocelotcan.py b/selfdrive/car/ocelot/ocelotcan.py
@@ -38,4 +38,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", 2, values)
diff --git a/selfdrive/car/ocelot/values.py b/selfdrive/car/ocelot/values.py
@@ -7,8 +7,8 @@
 # Steer torque limits
 class SteerLimitParams:
   STEER_MAX = 1024 #1500
-  STEER_DELTA_UP = 128 #25       # 1.5s time to peak torque
-  STEER_DELTA_DOWN = 128 #25     # always lower than 45 otherwise the Rav4 faults (Prius seems ok with 50)
+  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 CAR: