max accel tweaks

Author: ErichMoraga

selfdrive/car/toyota/carcontroller.py

@@ -6,6 +6,7 @@
                                            create_fcw_command
 from selfdrive.car.toyota.values import Ecu, CAR, STATIC_MSGS, SteerLimitParams
 from opendbc.can.packer import CANPacker
+from selfdrive.config import Conversions as CV
 
 VisualAlert = car.CarControl.HUDControl.VisualAlert
 
@@ -65,8 +66,22 @@ def update(self, enabled, CS, frame, actuators, pcm_cancel_cmd, hud_alert,
     else:
       apply_accel = actuators.gas - actuators.brake
 
+    """
+    0 - 13mph: max accel of 1.5
+    13-50 mph - 1.5m/s2 - 0.5 m/s2 dropping linearly with speed
+    50 mph + = 0.5
+    """
+    curr_speed_mph = CS.out.vEgo * CV.MS_TO_MPH
+    new_accel_max = ACCEL_MAX
+    if curr_speed_mph > 13:
+      if curr_speed_mph >= 50:
+        new_accel_max = 0.5
+      else:
+        # variable max_aceel between 10 mph and 50 mph
+        new_accel_max = ACCEL_MAX - (((curr_speed_mph - 13.0)/ 37))
+
     apply_accel, self.accel_steady = accel_hysteresis(apply_accel, self.accel_steady, enabled)
-    apply_accel = clip(apply_accel * ACCEL_SCALE, ACCEL_MIN, ACCEL_MAX)
+    apply_accel = clip(apply_accel * ACCEL_SCALE, ACCEL_MIN, new_accel_max)
 
     # steer torque
     new_steer = int(round(actuators.steer * SteerLimitParams.STEER_MAX))