Ford: set steeringRateLimited when exceeding angle limits

Author: incognitojam

selfdrive/car/ford/carcontroller.py

@@ -6,61 +6,78 @@
 from selfdrive.car.ford.values import CarControllerParams
 from opendbc.can.packer import CANPacker
 
+DEBUG_RAMP_TYPE = False
+
 VisualAlert = car.CarControl.HUDControl.VisualAlert
 
+
+def apply_steer_angle_limits(apply_steer, apply_steer_last, vEgo, LIMITS):
+  steer_up = apply_steer * apply_steer_last > 0. and abs(apply_steer) > abs(apply_steer_last)
+  rate_limit = LIMITS.STEER_RATE_LIMIT_UP if steer_up else LIMITS.STEER_RATE_LIMIT_DOWN
+  max_angle_diff = interp(vEgo, rate_limit.speed_points, rate_limit.max_angle_diff_points)
+  return clip(apply_steer, (apply_steer_last - max_angle_diff), (apply_steer_last + max_angle_diff))
+
+
 class CarController():
   def __init__(self, dbc_name, CP, VM):
     self.CP = CP
     self.vehicle_model = VM
     self.packer = CANPacker(dbc_name)
     self.params = CarControllerParams
 
-    self.enabled_last = False
+    self.apply_steer_last = 0
+    self.steer_rate_limited = False
     self.main_on_last = False
-    self.generic_toggle_last = False
+    self.enabled_last = False
     self.steer_alert_last = False
-    self.angle_last = 0
+    self.generic_toggle_last = False
+
+    self.ramp_type = 0
 
   def update(self, enabled, CS, frame, actuators, cruise_cancel, visual_alert):
     can_sends = []
-
     main_on = CS.out.cruiseState.available
     steer_alert = visual_alert in [VisualAlert.steerRequired, VisualAlert.ldw]
 
-    if cruise_cancel:
-      can_sends.append(fordcan.spam_cancel_button(self.packer))
+    apply_steer = 0
+    self.steer_rate_limited = False
 
-    if (frame % self.params.LKAS_STEER_STEP) == 0:
-      angle = actuators.steeringAngleDeg
+    if enabled:
+      # calculate steer angle, apply angle rate limits
+      apply_steer = apply_steer_angle_limits(actuators.steeringAngleDeg, self.apply_steer_last, CS.out.vEgo, CarControllerParams)
+      self.steer_rate_limited = actuators.steeringAngleDeg != apply_steer
 
-      # angle rate limit based on speed
-      steer_up = angle * self.angle_last > 0. and abs(angle) > abs(self.angle_last)
-      rate_limit = self.params.STEER_RATE_LIMIT_UP if steer_up else self.params.STEER_RATE_LIMIT_DOWN
-      max_angle_diff = interp(CS.out.vEgo, rate_limit.speed_points, rate_limit.max_angle_diff_points)
-      angle = clip(angle, (self.angle_last - max_angle_diff), (self.angle_last + max_angle_diff))
+    if cruise_cancel:
+      # cancel stock ACC
+      can_sends.append(fordcan.spam_cancel_button(self.packer))
 
-      if enabled:
-        lca_rq = 1
-      else:
-        lca_rq = 0
+    if CS.out.genericToggle and not self.generic_toggle_last:
+      # ramp type experiment
+      if DEBUG_RAMP_TYPE:
+        # TODO: stock varies this with distance from center
+        self.ramp_type += 1
+        if self.ramp_type > 3:
+          self.ramp_type = 0
 
-      angle_rad = angle * CV.DEG_TO_RAD
+    if (frame % self.params.LKAS_STEER_STEP) == 0:
+      lca_rq = 1 if enabled else 0
+      apply_steer_rad = apply_steer * CV.DEG_TO_RAD
       curvature = self.vehicle_model.calc_curvature(math.radians(actuators.steeringAngleDeg), CS.out.vEgo, 0.0)
-      ramp_type = 3
 
-      can_sends.append(fordcan.create_steer_command(self.packer, angle, curvature))
-      can_sends.append(fordcan.create_steer2_command(self.packer, lca_rq, ramp_type, angle_rad, curvature))
+      self.apply_steer_last = apply_steer
 
-      self.angle_last = angle
-      self.generic_toggle_last = CS.out.genericToggle
+      # send steering commands
+      can_sends.append(fordcan.create_steer_command(self.packer, apply_steer, curvature))
+      can_sends.append(fordcan.create_steer2_command(self.packer, lca_rq, self.ramp_type, apply_steer_rad, curvature))
 
-    # TODO: do we also have to send if CS.ipma_data changed?
+    # TODO: do we need to send when values change? is 1hz acceptable?
     if (frame % self.params.LKAS_UI_STEP) == 0 or (self.main_on_last != main_on) or (self.enabled_last != enabled) or \
        (self.steer_alert_last != steer_alert):
       can_sends.append(fordcan.create_lkas_ui_command(self.packer, main_on, enabled, steer_alert, CS.ipma_data))
 
     self.main_on_last = main_on
     self.enabled_last = enabled
     self.steer_alert_last = steer_alert
+    self.generic_toggle_last = CS.out.genericToggle
 
     return actuators, can_sends

selfdrive/car/ford/interface.py

@@ -65,8 +65,10 @@ def update(self, c, can_strings):
 
     ret.canValid = self.cp.can_valid and self.cp_cam.can_valid
 
-    # TODO: fixup alerts for LCA
+    # speeds
+    ret.steeringRateLimited = self.CC.steer_rate_limited if self.CC is not None else False
 
+    # events
     events = self.create_common_events(ret)
     if self.CS.park_brake:
       events.add(EventName.parkBrake)