New mazdas with TI2 support

common/realtime.py

@@ -94,3 +94,88 @@ def monitor_time(self) -> bool:
     self._frame += 1
     self._remaining = remaining
     return lagged
+
+class DurationTimer:
+  def __init__(self, duration=0, step=DT_CTRL) -> None:
+    self.step = step
+    self.duration = duration
+    self.was_reset = False
+    self.timer = 0
+    self.min = float("-inf") # type: float
+    self.max = float("inf") # type: float
+
+  def tick_obj(self) -> None:
+    self.timer += self.step
+    # reset on overflow
+    self.reset() if (self.timer == (self.max or self.min)) else None
+
+  def reset(self) -> None:
+    """Resets this objects timer"""
+    self.timer = 0
+    self.was_reset = True
+
+  def active(self) -> bool:
+    """Returns true if time since last reset is less than duration"""
+    return bool(round(self.timer,2) < self.duration)
+
+  def adjust(self, duration) -> None:
+    """Adjusts the duration of the timer"""
+    self.duration = duration
+
+  def once_after_reset(self) -> bool: 
+    """Returns true only one time after calling reset()"""
+    ret = self.was_reset
+    self.was_reset = False
+    return ret
+
+  @staticmethod
+  def interval_obj(rate, frame) -> bool:
+    if frame % rate == 0: # Highlighting shows "frame" in white
+      return True
+    return False
+
+class ModelTimer(DurationTimer):
+  frame = 0 # type: int
+  objects = [] # type: List[DurationTimer]
+  def __init__(self, duration=0) -> None:
+    self.step = DT_MDL
+    super().__init__(duration, self.step)
+    self.__class__.objects.append(self)
+
+  @classmethod
+  def tick(cls) -> None:
+    cls.frame += 1
+    for obj in cls.objects:
+      ModelTimer.tick_obj(obj)
+
+  @classmethod
+  def reset_all(cls) -> None:
+    for obj in cls.objects:
+      obj.reset()
+
+  @classmethod
+  def interval(cls, rate) -> bool:
+    return ModelTimer.interval_obj(rate, cls.frame)
+
+class ControlsTimer(DurationTimer):
+  frame = 0
+  objects = [] # type: List[DurationTimer]
+  def __init__(self, duration=0) -> None:
+    self.step = DT_CTRL
+    super().__init__(duration=duration, step=self.step)
+    self.__class__.objects.append(self)
+
+  @classmethod
+  def tick(cls) -> None:
+    cls.frame += 1
+    for obj in cls.objects:
+      ControlsTimer.tick_obj(obj)
+
+  @classmethod
+  def reset_all(cls) -> None:
+    for obj in cls.objects:
+      obj.reset()
+
+  @classmethod
+  def interval(cls, rate) -> bool:
+    return ControlsTimer.interval_obj(rate, cls.frame)

selfdrive/car/interfaces.py

@@ -136,7 +136,7 @@ def get_steer_feedforward_function(self):
     return self.get_steer_feedforward_default
 
   @staticmethod
-  def torque_from_lateral_accel_linear(lateral_accel_value, torque_params, lateral_accel_error, lateral_accel_deadzone, vego, friction_compensation):
+  def torque_from_lateral_accel_linear(lateral_accel_value, torque_params, lateral_accel_error, lateral_accel_deadzone, steering_angle, vego, friction_compensation):
     # The default is a linear relationship between torque and lateral acceleration (accounting for road roll and steering friction)
     friction_interp = interp(
       apply_center_deadzone(lateral_accel_error, lateral_accel_deadzone),
@@ -146,8 +146,21 @@ def torque_from_lateral_accel_linear(lateral_accel_value, torque_params, lateral
     friction = friction_interp if friction_compensation else 0.0
     return (lateral_accel_value / torque_params.latAccelFactor) + friction
 
-  def torque_from_lateral_accel(self) -> TorqueFromLateralAccelCallbackType:
-    return self.torque_from_lateral_accel_linear
+  @staticmethod
+  def torque_from_lateral_accel_non_linear(lateral_accel_value, torque_params, lateral_accel_error, lateral_accel_deadzone, steering_angle, vego, friction_compensation):
+    steering_angle = abs(steering_angle)
+    lat_factor = torque_params.latAccelFactor * ((steering_angle * torque_params.latAngleFactor) + 1)
+    friction_interp = interp(
+      apply_center_deadzone(lateral_accel_error, lateral_accel_deadzone),
+      [-FRICTION_THRESHOLD, FRICTION_THRESHOLD],
+      [-torque_params.friction, torque_params.friction]
+    )
+    friction = friction_interp if friction_compensation else 0.0
+    return (lateral_accel_value / lat_factor) + friction
+
+  def torque_from_lateral_accel(self, linear) -> TorqueFromLateralAccelCallbackType:
+    return self.torque_from_lateral_accel_linear if linear else self.torque_from_lateral_accel_non_linear
+
 
   # returns a set of default params to avoid repetition in car specific params
   @staticmethod
@@ -200,7 +213,7 @@ def configure_lqr_tune(tune):
     tune.lqr.dcGain = 0.002237852961363602
 
   @staticmethod
-  def configure_torque_tune(candidate, tune, steering_angle_deadzone_deg=0.0, use_steering_angle=True):
+  def configure_torque_tune(candidate, tune, steering_angle_deadzone_deg=0.0, use_steering_angle=True, linear = True):
     params = get_torque_params(candidate)
 
     tune.init('torque')
@@ -212,14 +225,15 @@ def configure_torque_tune(candidate, tune, steering_angle_deadzone_deg=0.0, use_
     tune.torque.latAccelFactor = params['LAT_ACCEL_FACTOR']
     tune.torque.latAccelOffset = 0.0
     tune.torque.steeringAngleDeadzoneDeg = steering_angle_deadzone_deg
+    tune.torque.linear = linear
 
   @staticmethod
-  def configure_dp_tune(candidate, tune, steering_angle_deadzone_deg=0.0, use_steering_angle=True):
+  def configure_dp_tune(candidate, tune, steering_angle_deadzone_deg=0.0, use_steering_angle=True, linear = True):
     params = Params()
     if params.get_bool('dp_lateral_lqr'):
       CarInterfaceBase.configure_lqr_tune(tune)
     elif params.get_bool('dp_lateral_torque'):
-      CarInterfaceBase.configure_torque_tune(candidate, tune, steering_angle_deadzone_deg, use_steering_angle)
+      CarInterfaceBase.configure_torque_tune(candidate, tune, steering_angle_deadzone_deg, use_steering_angle, linear)
 
   @abstractmethod
   def _update(self, c: car.CarControl) -> car.CarState:

selfdrive/car/mazda/carcontroller.py

@@ -2,9 +2,11 @@
 from opendbc.can.packer import CANPacker
 from selfdrive.car import apply_std_steer_torque_limits
 from selfdrive.car.mazda import mazdacan
-from selfdrive.car.mazda.values import CarControllerParams, Buttons
+from selfdrive.car.mazda.values import CarControllerParams, Buttons, GEN1
+from common.realtime import ControlsTimer as Timer
 
 VisualAlert = car.CarControl.HUDControl.VisualAlert
+LongCtrlState = car.CarControl.Actuators.LongControlState
 
 
 class CarController:
@@ -14,52 +16,82 @@ def __init__(self, dbc_name, CP, VM):
     self.packer = CANPacker(dbc_name)
     self.brake_counter = 0
     self.frame = 0
+    self.params = CarControllerParams(CP)
+    self.hold_timer = Timer(6.0)
+    self.hold_delay = Timer(0.5) # delay before we start holding as to not hit the brakes too hard
+    self.resume_timer = Timer(0.5)
 
   def update(self, CC, CS, now_nanos):
     can_sends = []
 
     apply_steer = 0
-
+    
     if CC.latActive:
       # calculate steer and also set limits due to driver torque
-      new_steer = int(round(CC.actuators.steer * CarControllerParams.STEER_MAX))
+      new_steer = int(round(CC.actuators.steer * self.params.STEER_MAX))
       apply_steer = apply_std_steer_torque_limits(new_steer, self.apply_steer_last,
-                                                  CS.out.steeringTorque, CarControllerParams)
-
-    if CC.cruiseControl.cancel:
-      # If brake is pressed, let us wait >70ms before trying to disable crz to avoid
-      # a race condition with the stock system, where the second cancel from openpilot
-      # will disable the crz 'main on'. crz ctrl msg runs at 50hz. 70ms allows us to
-      # read 3 messages and most likely sync state before we attempt cancel.
-      self.brake_counter = self.brake_counter + 1
-      if self.frame % 10 == 0 and not (CS.out.brakePressed and self.brake_counter < 7):
-        # Cancel Stock ACC if it's enabled while OP is disengaged
-        # Send at a rate of 10hz until we sync with stock ACC state
-        can_sends.append(mazdacan.create_button_cmd(self.packer, self.CP.carFingerprint, CS.crz_btns_counter, Buttons.CANCEL))
-    else:
-      self.brake_counter = 0
-      if CC.cruiseControl.resume and self.frame % 5 == 0:
-        # Mazda Stop and Go requires a RES button (or gas) press if the car stops more than 3 seconds
-        # Send Resume button when planner wants car to move
-        can_sends.append(mazdacan.create_button_cmd(self.packer, self.CP.carFingerprint, CS.crz_btns_counter, Buttons.RESUME))
-
+                                                  CS.out.steeringTorque, self.params)
     self.apply_steer_last = apply_steer
+
+    if self.CP.carFingerprint in GEN1:
+      if CC.cruiseControl.cancel:
+        # If brake is pressed, let us wait >70ms before trying to disable crz to avoid
+        # a race condition with the stock system, where the second cancel from openpilot
+        # will disable the crz 'main on'. crz ctrl msg runs at 50hz. 70ms allows us to
+        # read 3 messages and most likely sync state before we attempt cancel.
+        self.brake_counter = self.brake_counter + 1
+        if self.frame % 10 == 0 and not (CS.out.brakePressed and self.brake_counter < 7):
+          # Cancel Stock ACC if it's enabled while OP is disengaged
+          # Send at a rate of 10hz until we sync with stock ACC state
+          can_sends.append(mazdacan.create_button_cmd(self.packer, self.CP.carFingerprint, CS.crz_btns_counter, Buttons.CANCEL))
+      else:
+        self.brake_counter = 0
+        if CC.cruiseControl.resume and self.frame % 5 == 0:
+          # Mazda Stop and Go requires a RES button (or gas) press if the car stops more than 3 seconds
+          # Send Resume button when planner wants car to move
+          can_sends.append(mazdacan.create_button_cmd(self.packer, self.CP.carFingerprint, CS.crz_btns_counter, Buttons.RESUME))
 
-    # send HUD alerts
-    if self.frame % 50 == 0:
-      ldw = CC.hudControl.visualAlert == VisualAlert.ldw
-      steer_required = CC.hudControl.visualAlert == VisualAlert.steerRequired
-      # TODO: find a way to silence audible warnings so we can add more hud alerts
-      steer_required = steer_required and CS.lkas_allowed_speed
-      can_sends.append(mazdacan.create_alert_command(self.packer, CS.cam_laneinfo, ldw, steer_required))
-
+      # send HUD alerts
+      if self.frame % 50 == 0:
+        ldw = CC.hudControl.visualAlert == VisualAlert.ldw
+        steer_required = CC.hudControl.visualAlert == VisualAlert.steerRequired
+        # TODO: find a way to silence audible warnings so we can add more hud alerts
+        steer_required = steer_required and CS.lkas_allowed_speed
+        can_sends.append(mazdacan.create_alert_command(self.packer, CS.cam_laneinfo, ldw, steer_required))
+
+    else: # gen2
+      resume = False
+      hold = False
+      if Timer.interval(2): # send ACC command at 50hz 
+        """
+        Without this hold/resum logic, the car will only stop momentarily.
+        It will then start creeping forward again. This logic allows the car to
+        apply the electric brake to hold the car. The hold delay also fixes a
+        bug with the stock ACC where it sometimes will apply the brakes too early 
+        when coming to a stop. 
+        """
+        if CS.out.standstill: # if we're stopped
+          if not self.hold_delay.active(): # and we have been stopped for more than hold_delay duration. This prevents a hard brake if we aren't fully stopped.
+            if (CC.cruiseControl.resume or CC.cruiseControl.override or (CC.actuators.longControlState == LongCtrlState.starting)): # and we want to resume
+              self.resume_timer.reset() # reset the resume timer so its active
+            else: # otherwise we're holding
+              hold = self.hold_timer.active() # hold for 6s. This allows the electric brake to hold the car.
+
+        else: # if we're moving
+          self.hold_timer.reset() # reset the hold timer so its active when we stop
+          self.hold_delay.reset() # reset the hold delay
+
+        resume = self.resume_timer.active() # stay on for 0.5s to release the brake. This allows the car to move.
+        can_sends.append(mazdacan.create_acc_cmd(self, self.packer, CS, CC, hold, resume))
+
     # send steering command
     can_sends.append(mazdacan.create_steering_control(self.packer, self.CP.carFingerprint,
                                                       self.frame, apply_steer, CS.cam_lkas))
 
     new_actuators = CC.actuators.copy()
-    new_actuators.steer = apply_steer / CarControllerParams.STEER_MAX
+    new_actuators.steer = apply_steer / self.params.STEER_MAX
     new_actuators.steerOutputCan = apply_steer
 
     self.frame += 1
+    Timer.tick()
     return new_actuators, can_sends