implement for ford

2026-09-18 12:33:43 +08:00 · 2024-09-16 15:46:31 -04:00
parent 45739d199c
commit b10fe24d57
5 changed files with 77 additions and 1 deletions
@@ -0,0 +1,28 @@
+from cereal import car
+from openpilot.selfdrive.car.ford.values import CarControllerParams
+from openpilot.sunnypilot.controls.lib.custom_stock_longitudinal_controller.controllers import CustomStockLongitudinalControllerBase, \
+  SendCan
+from openpilot.sunnypilot.selfdrive.car.ford.custom_stock_longitudinal_controller import fordcan
+
+ButtonType = car.CarState.ButtonEvent.Type
+# TODO-SP: Handle this better in the base class
+ACCEL_BUTTON = 1
+DECEL_BUTTON = 2
+
+
+class CustomStockLongitudinalController(CustomStockLongitudinalControllerBase):
+  def __init__(self, car, car_controller, car_state, CP):
+    super().__init__(car, car_controller, car_state, CP)
+    self.accel_button = ACCEL_BUTTON
+    self.decel_button = DECEL_BUTTON
+    self.cruise_buttons = {ButtonType.decelCruise: 0, ButtonType.accelCruise: 0, ButtonType.gapAdjustCruise: 0,
+                           ButtonType.resumeCruise: 0, ButtonType.setCruise: 0}
+
+  def create_mock_button_messages(self) -> list[SendCan]:
+    can_sends = []
+
+    if self.cruise_button is not None and (self.car_controller.frame % CarControllerParams.BUTTONS_STEP) == 0:
+      can_sends.append(fordcan.create_button_msg(self.car_controller.packer, self.car_controller.CAN.camera, self.car_state.buttons_stock_values, buttons=self.cruise_button))
+      can_sends.append(fordcan.create_button_msg(self.car_controller.packer, self.car_controller.CAN.main, self.car_state.buttons_stock_values, buttons=self.cruise_button))
+
+    return can_sends
@@ -0,0 +1,48 @@
+def create_button_msg(packer, bus: int, stock_values: dict, buttons=0):
+  """
+  Creates a CAN message for the Ford SCCM buttons/switches.
+
+  Includes cruise control buttons, turn lights and more.
+
+  Frequency is 10Hz.
+  """
+
+  values = {s: stock_values[s] for s in [
+    "HeadLghtHiFlash_D_Stat",  # SCCM Passthrough the remaining buttons
+    "TurnLghtSwtch_D_Stat",    # SCCM Turn signal switch
+    "WiprFront_D_Stat",
+    "LghtAmb_D_Sns",
+    "AccButtnGapDecPress",
+    "AccButtnGapIncPress",
+    "AslButtnOnOffCnclPress",
+    "AslButtnOnOffPress",
+    "LaSwtchPos_D_Stat",
+    "CcAslButtnCnclResPress",
+    "CcAslButtnDeny_B_Actl",
+    "CcAslButtnIndxDecPress",
+    "CcAslButtnIndxIncPress",
+    "CcAslButtnOffCnclPress",
+    "CcAslButtnOnOffCncl",
+    "CcAslButtnOnPress",
+    "CcAslButtnResDecPress",
+    "CcAslButtnResIncPress",
+    "CcAslButtnSetDecPress",
+    "CcAslButtnSetIncPress",
+    "CcAslButtnSetPress",
+    "CcButtnOffPress",
+    "CcButtnOnOffCnclPress",
+    "CcButtnOnOffPress",
+    "CcButtnOnPress",
+    "HeadLghtHiFlash_D_Actl",
+    "HeadLghtHiOn_B_StatAhb",
+    "AhbStat_B_Dsply",
+    "AccButtnGapTogglePress",
+    "WiprFrontSwtch_D_Stat",
+    "HeadLghtHiCtrl_D_RqAhb",
+  ]}
+
+  values.update({
+    "CcAslButtnSetIncPress": 1 if buttons == 1 else 0,      # CC increase button
+    "CcAslButtnSetDecPress": 1 if buttons == 2 else 0,      # CC decrease button
+  })
+  return packer.make_can_msg("Steering_Data_FD1", bus, values)