Mazda: send dash alerts (#22730)

* Mazda: send dash alerts

* no actuators

* ldw

* send alerts

* test

* use cam parser

* add generic toggle

* wip

* revert some stuff

* try that

* steer required works

* cleanup

* bump panda

* little more cleanup

* no lane lines

Co-authored-by: Comma Device <device@comma.ai>

panda

@@ -1 +1 @@
-Subproject commit 0d4e98f6056a69da8933c8a690a803bc96e0e528
+Subproject commit da4efd21c49fcdfaddca7641bfbaff7dd88b763f

selfdrive/car/mazda/carcontroller.py

@@ -1,8 +1,11 @@
+from cereal import car
+from opendbc.can.packer import CANPacker
 from selfdrive.car.mazda import mazdacan
 from selfdrive.car.mazda.values import CarControllerParams, Buttons
-from opendbc.can.packer import CANPacker
 from selfdrive.car import apply_std_steer_torque_limits
 
+VisualAlert = car.CarControl.HUDControl.VisualAlert
+
 class CarController():
   def __init__(self, dbc_name, CP, VM):
     self.apply_steer_last = 0
@@ -10,16 +13,13 @@ class CarController():
     self.steer_rate_limited = False
     self.brake_counter = 0
 
-  def update(self, enabled, CS, frame, actuators):
+  def update(self, c, CS, frame):
-    """ Controls thread """
-
     can_sends = []
+    apply_steer = 0
 
-    ### STEER ###
+    if c.enabled:
-
-    if enabled:
       # calculate steer and also set limits due to driver torque
-      new_steer = int(round(actuators.steer * CarControllerParams.STEER_MAX))
+      new_steer = int(round(c.actuators.steer * CarControllerParams.STEER_MAX))
       apply_steer = apply_std_steer_torque_limits(new_steer, self.apply_steer_last,
                                                   CS.out.steeringTorque, CarControllerParams)
       self.steer_rate_limited = new_steer != apply_steer
@@ -30,7 +30,6 @@ class CarController():
         # TODO: improve the resume trigger logic by looking at actual radar data
         can_sends.append(mazdacan.create_button_cmd(self.packer, CS.CP.carFingerprint, Buttons.RESUME))
     else:
-      apply_steer = 0
       self.steer_rate_limited = False
       if CS.out.cruiseState.enabled:
         # if brake is pressed, let us wait >20ms before trying to disable crz to avoid
@@ -46,6 +45,13 @@ class CarController():
 
     self.apply_steer_last = apply_steer
 
+    # send HUD alerts
+    if frame % 50 == 0:
+      ldw = c.hudControl.visualAlert == VisualAlert.ldw
+      steer_required = c.hudControl.visualAlert == VisualAlert.steerRequired
+      can_sends.append(mazdacan.create_alert_command(self.packer, CS.cam_laneinfo, ldw, steer_required))
+
+    # send steering command
     can_sends.append(mazdacan.create_steering_control(self.packer, CS.CP.carFingerprint,
                                                       frame, apply_steer, CS.cam_lkas))
     return can_sends

selfdrive/car/mazda/carstate.py

@@ -33,6 +33,7 @@ class CarState(CarStateBase):
     can_gear = int(cp.vl["GEAR"]["GEAR"])
     ret.gearShifter = self.parse_gear_shifter(self.shifter_values.get(can_gear, None))
 
+    ret.genericToggle = bool(cp.vl["BLINK_INFO"]["HIGH_BEAMS"])
     ret.leftBlindspot = cp.vl["BSM"]["LEFT_BS1"] == 1
     ret.rightBlindspot = cp.vl["BSM"]["RIGHT_BS1"] == 1
     ret.leftBlinker, ret.rightBlinker = self.update_blinker_from_lamp(40, cp.vl["BLINK_INFO"]["LEFT_BLINK"] == 1,
@@ -82,6 +83,7 @@ class CarState(CarStateBase):
     self.acc_active_last = ret.cruiseState.enabled
 
     self.cam_lkas = cp_cam.vl["CAM_LKAS"]
+    self.cam_laneinfo = cp_cam.vl["CAM_LANEINFO"]
     ret.steerError = cp_cam.vl["CAM_LKAS"]["ERR_BIT_1"] == 1
 
     return ret
@@ -93,6 +95,7 @@ class CarState(CarStateBase):
       # sig_name, sig_address, default
       ("LEFT_BLINK", "BLINK_INFO", 0),
       ("RIGHT_BLINK", "BLINK_INFO", 0),
+      ("HIGH_BEAMS", "BLINK_INFO", 0),
       ("STEER_ANGLE", "STEER", 0),
       ("STEER_ANGLE_RATE", "STEER_RATE", 0),
       ("STEER_TORQUE_SENSOR", "STEER_TORQUE", 0),
@@ -166,16 +169,26 @@ class CarState(CarStateBase):
         ("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", 0),
         ("ANGLE_ENABLED", "CAM_LKAS", 0),
         ("CHKSUM", "CAM_LKAS", 0),
+
+        ("LINE_VISIBLE", "CAM_LANEINFO", 0),
+        ("LINE_NOT_VISIBLE", "CAM_LANEINFO", 1),
+        ("LANE_LINES", "CAM_LANEINFO", 0),
+        ("BIT1", "CAM_LANEINFO", 0),
+        ("BIT2", "CAM_LANEINFO", 0),
+        ("BIT3", "CAM_LANEINFO", 0),
+        ("NO_ERR_BIT", "CAM_LANEINFO", 1),
+        ("S1", "CAM_LANEINFO", 0),
+        ("S1_HBEAM", "CAM_LANEINFO", 0),
       ]
 
       checks += [
         # sig_address, frequency
+        ("CAM_LANEINFO", 2),
         ("CAM_LKAS", 16),
       ]
 

selfdrive/car/mazda/interface.py

@@ -95,6 +95,6 @@ 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, self.CS, self.frame)
     self.frame += 1
     return can_sends

selfdrive/car/mazda/mazdacan.py

@@ -1,5 +1,8 @@
+import copy
+
 from selfdrive.car.mazda.values import GEN1, Buttons
 
+
 def create_steering_control(packer, car_fingerprint, frame, apply_steer, lkas):
 
   tmp = apply_steer + 2048
@@ -7,10 +10,11 @@ def create_steering_control(packer, car_fingerprint, frame, apply_steer, lkas):
   lo = tmp & 0xFF
   hi = tmp >> 8
 
+  # copy values from camera
   b1 = int(lkas["BIT_1"])
-  ldw = int(lkas["LDW"])
   er1 = int(lkas["ERR_BIT_1"])
   lnv = 0
+  ldw = 0
   er2 = int(lkas["ERR_BIT_2"])
 
   # Some older models do have these, newer models don't.
@@ -28,7 +32,7 @@ def create_steering_control(packer, car_fingerprint, frame, apply_steer, lkas):
   # bytes:     [    1  ] [ 2 ] [             3               ]  [           4         ]
   csum = 249 - ctr - hi - lo - (lnv << 3) - er1 - (ldw << 7) - ( er2 << 4) - (b1 << 5)
 
-  #bytes       [ 5 ] [ 6 ] [    7   ]
+  # bytes      [ 5 ] [ 6 ] [    7   ]
   csum = csum - ahi - amd - alo - b2
 
   if ahi == 1:
@@ -44,63 +48,72 @@ def create_steering_control(packer, car_fingerprint, frame, apply_steer, lkas):
 
   if car_fingerprint in GEN1:
     values = {
-      "LKAS_REQUEST"     : apply_steer,
+      "LKAS_REQUEST": apply_steer,
-      "CTR"              : ctr,
+      "CTR": ctr,
-      "ERR_BIT_1"        : er1,
+      "ERR_BIT_1": er1,
       "LINE_NOT_VISIBLE" : lnv,
-      "LDW"              : ldw,
+      "LDW": ldw,
-      "BIT_1"            : b1,
+      "BIT_1": b1,
-      "ERR_BIT_2"        : er2,
+      "ERR_BIT_2": er2,
-      "STEERING_ANGLE"   : steering_angle,
+      "STEERING_ANGLE": steering_angle,
-      "ANGLE_ENABLED"    : b2,
+      "ANGLE_ENABLED": b2,
-      "CHKSUM"           : csum
+      "CHKSUM": csum
     }
 
   return packer.make_can_msg("CAM_LKAS", 0, values)
 
 
+def create_alert_command(packer, cam_msg: dict, ldw: bool, steer_required: bool):
+  values = copy.copy(cam_msg)
+  values.update({
+    # TODO: what's the difference between all these? do we need to send all?
+    "HANDS_WARN_3_BITS": 0b111 if steer_required else 0,
+    "HANDS_ON_STEER_WARN": steer_required,
+    "HANDS_ON_STEER_WARN_2": steer_required,
+
+    # TODO: right lane works, left doesn't
+    # TODO: need to do something about L/R
+    "LDW_WARN_LL": 0,
+    "LDW_WARN_RL": 0,
+  })
+  return packer.make_can_msg("CAM_LANEINFO", 0, values)
+
+
 def create_button_cmd(packer, car_fingerprint, button):
 
-  if button == Buttons.CANCEL:
+  can = int(button == Buttons.CANCEL)
-    can = 1
+  res = int(button == Buttons.RESUME)
-    res = 0
-  elif button == Buttons.RESUME:
-    can = 0
-    res = 1
-  else:
-    can = 0
-    res = 0
 
   if car_fingerprint in GEN1:
     values = {
-      "CAN_OFF"           : can,
+      "CAN_OFF": can,
-      "CAN_OFF_INV"       : (can + 1) % 2,
+      "CAN_OFF_INV": (can + 1) % 2,
 
-      "SET_P"             : 0,
+      "SET_P": 0,
-      "SET_P_INV"         : 1,
+      "SET_P_INV": 1,
 
-      "RES"               : res,
+      "RES": res,
-      "RES_INV"           : (res + 1) % 2,
+      "RES_INV": (res + 1) % 2,
 
-      "SET_M"             : 0,
+      "SET_M": 0,
-      "SET_M_INV"         : 1,
+      "SET_M_INV": 1,
 
-      "DISTANCE_LESS"     : 0,
+      "DISTANCE_LESS": 0,
-      "DISTANCE_LESS_INV" : 1,
+      "DISTANCE_LESS_INV": 1,
 
-      "DISTANCE_MORE"     : 0,
+      "DISTANCE_MORE": 0,
-      "DISTANCE_MORE_INV" : 1,
+      "DISTANCE_MORE_INV": 1,
 
-      "MODE_X"            : 0,
+      "MODE_X": 0,
-      "MODE_X_INV"        : 1,
+      "MODE_X_INV": 1,
 
-      "MODE_Y"            : 0,
+      "MODE_Y": 0,
-      "MODE_Y_INV"        : 1,
+      "MODE_Y_INV": 1,
 
-      "BIT1"              : 1,
+      "BIT1": 1,
-      "BIT2"              : 1,
+      "BIT2": 1,
-      "BIT3"              : 1,
+      "BIT3": 1,
-      "CTR"               : 0
+      "CTR": 0
     }
 
     return packer.make_can_msg("CRZ_BTNS", 0, values)