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.
@@ -59,17 +63,26 @@ def create_steering_control(packer, car_fingerprint, frame, apply_steer, lkas):
   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 = {