Improve acc stock and go by removing the 3 seconds limit

Signed-off-by: Jafar Al-Gharaibeh <to.jafar@gmail.com>

selfdrive/car/mazda/carcontroller.py

@@ -1,5 +1,5 @@
 from selfdrive.car.mazda import mazdacan
-from selfdrive.car.mazda.values import SteerLimitParams
+from selfdrive.car.mazda.values import SteerLimitParams, Buttons
 from opendbc.can.packer import CANPacker
 from selfdrive.car import apply_std_steer_torque_limits
 
@@ -9,7 +9,8 @@ class CarController():
     self.packer = CANPacker(dbc_name)
     self.steer_rate_limited = False
 
-  def update(self, enabled, CS, frame, actuators):
+  def update(self, enabled, CS, frame, actuators, pcm_cancel_cmd, visual_alert,
+             left_lane, right_lane, left_lane_depart, right_lane_depart):
     """ Controls thread """
 
     can_sends = []
@@ -22,11 +23,18 @@ class CarController():
       apply_steer = apply_std_steer_torque_limits(new_steer, self.apply_steer_last,
                                                   CS.out.steeringTorque, SteerLimitParams)
       self.steer_rate_limited = new_steer != apply_steer
+
+      if CS.out.standstill and frame % 50 == 0:
+        # Mazda Stop and Go requires a RES button (or gas) press if the car stops more than 3 seconds
+        # Send Resume button at 2hz if we're engaged at standstill to support full stop and go!
+        # TODO: improve the resume trigger logic by looking at actual radar data
+        can_sends.append(mazdacan.create_cancel_acc(self.packer, CS.CP.carFingerprint, Buttons.RESUME))
     else:
       apply_steer = 0
-      self.steer_rate_limited = False
-      if CS.acc_active and frame % 10 == 0:
-        can_sends.append(mazdacan.create_cancel_acc(self.packer, CS.CP.carFingerprint))
+      if CS.out.cruiseState.enabled and frame % 10 == 0:
+        # Cancel Stock ACC if it's engaged with OP disengaged
+        # Match stock message rate which is sent at 10hz
+        can_sends.append(mazdacan.create_cancel_acc(self.packer, CS.CP.carFingerprint, Buttons.CANCEL))
 
 
     self.apply_steer_last = apply_steer

selfdrive/car/mazda/carstate.py

@@ -75,12 +75,11 @@ class CarState(CarStateBase):
                 cp.vl["CRZ_BTNS"]['SET_M']]):
       self.cruise_speed = ret.vEgoRaw
 
-    ret.cruiseState.available = cp.vl["CRZ_CTRL"]['CRZ_ACTIVE'] == 1
-    self.acc_active = ret.cruiseState.available
-    ret.cruiseState.enabled = self.acc_active
+    ret.cruiseState.available = True
+    ret.cruiseState.enabled =  cp.vl["CRZ_CTRL"]['CRZ_ACTIVE'] == 1
     ret.cruiseState.speed = self.cruise_speed
 
-    if self.acc_active:
+    if ret.cruiseState.enabled:
       if not self.lkas_on:
         if not self.acc_active_last:
           self.low_speed_lockout = True
@@ -92,7 +91,7 @@ class CarState(CarStateBase):
 
       ret.steerWarning = handsoff
 
-    self.acc_active_last = self.acc_active
+    self.acc_active_last = ret.cruiseState.enabled
 
     self.cam_lkas = cp_cam.vl["CAM_LKAS"]
     ret.steerError = cp_cam.vl["CAM_LKAS"]['ERR_BIT_1'] == 1
@@ -170,14 +169,14 @@ class CarState(CarStateBase):
     if CP.carFingerprint == CAR.CX5:
       signals += [
         # sig_name, sig_address, default
-        ("LKAS_REQUEST",     "CAM_LKAS", 2048),
+        ("LKAS_REQUEST",     "CAM_LKAS", 0),
         ("CTR",              "CAM_LKAS", 0),
         ("ERR_BIT_1",        "CAM_LKAS", 0),
         ("LINE_NOT_VISIBLE", "CAM_LKAS", 0),
         ("LDW",              "CAM_LKAS", 0),
         ("BIT_1",            "CAM_LKAS", 1),
         ("ERR_BIT_2",        "CAM_LKAS", 0),
-        ("STEERING_ANGLE",   "CAM_LKAS", 2048),
+        ("STEERING_ANGLE",   "CAM_LKAS", 0),
         ("ANGLE_ENABLED",    "CAM_LKAS", 0),
         ("CHKSUM",           "CAM_LKAS", 0),
       ]

selfdrive/car/mazda/interface.py

@@ -1,7 +1,7 @@
 #!/usr/bin/env python3
 from cereal import car
 from selfdrive.config import Conversions as CV
-from selfdrive.car.mazda.values import CAR,  FINGERPRINTS, ECU_FINGERPRINT, Ecu
+from selfdrive.car.mazda.values import CAR, LKAS_LIMITS, FINGERPRINTS, ECU_FINGERPRINT, Ecu
 from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness, gen_empty_fingerprint, is_ecu_disconnected
 from selfdrive.car.interfaces import CarInterfaceBase
 
@@ -40,8 +40,8 @@ class CarInterface(CarInterfaceBase):
 
       ret.lateralTuning.pid.kf = 0.00006
 
-      # No steer below 45kph
-      ret.minSteerSpeed = 45 * CV.KPH_TO_MS
+      # No steer below disable speed
+      ret.minSteerSpeed = LKAS_LIMITS.DISABLE_SPEED * CV.KPH_TO_MS
 
 
     ret.centerToFront = ret.wheelbase * 0.41
@@ -86,6 +86,9 @@ class CarInterface(CarInterfaceBase):
     return self.CS.out
 
   def apply(self, c):
-    can_sends = self.CC.update(c.enabled, self.CS, self.frame, c.actuators)
+    can_sends = self.CC.update(c.enabled, self.CS, self.frame, c.actuators,
+                               c.cruiseControl.cancel, c.hudControl.visualAlert,
+                               c.hudControl.leftLaneVisible, c.hudControl.rightLaneVisible,
+                               c.hudControl.leftLaneDepart, c.hudControl.rightLaneDepart)
     self.frame += 1
     return can_sends

selfdrive/car/mazda/mazdacan.py

@@ -1,4 +1,4 @@
-from selfdrive.car.mazda.values import CAR
+from selfdrive.car.mazda.values import CAR, Buttons
 
 def create_steering_control(packer, car_fingerprint, frame, apply_steer, lkas):
 
@@ -57,17 +57,28 @@ def create_steering_control(packer, car_fingerprint, frame, apply_steer, lkas):
   return packer.make_can_msg("CAM_LKAS", 0, values)
 
 
-def create_cancel_acc(packer, car_fingerprint):
+def create_cancel_acc(packer, car_fingerprint, button):
+
+  if button == Buttons.CANCEL:
+    can = 1
+    res = 0
+  elif button == Buttons.RESUME:
+    can = 0
+    res = 1
+  else:
+    can = 0
+    res = 0
+
   if car_fingerprint == CAR.CX5:
     values = {
-      "CAN_OFF"           : 1,
-      "CAN_OFF_INV"       : 0,
+      "CAN_OFF"           : can,
+      "CAN_OFF_INV"       : (can + 1) % 2,
 
       "SET_P"             : 0,
       "SET_P_INV"         : 1,
 
-      "RES"               : 0,
-      "RES_INV"           : 1,
+      "RES"               : res,
+      "RES_INV"           : (res + 1) % 2,
 
       "SET_M"             : 0,
       "SET_M_INV"         : 1,

selfdrive/car/mazda/values.py

@@ -22,6 +22,13 @@ class LKAS_LIMITS:
   DISABLE_SPEED   = 45  #kph
   ENABLE_SPEED    = 52  #kph
 
+class Buttons:
+  NONE = 0
+  SET_PLUS = 1
+  SET_MINUS = 2
+  RESUME = 3
+  CANCEL = 4
+
 FINGERPRINTS = {
   CAR.CX5: [
     # CX-5 2017 GT