diff --git a/panda b/panda index 6a6cd44519..447b587e97 160000 --- a/panda +++ b/panda @@ -1 +1 @@ -Subproject commit 6a6cd445195c4d897d1b39b0fe1b1783304804e8 +Subproject commit 447b587e9787988bafa84483290f709a5f834aee diff --git a/sunnypilot/selfdrive/car/ford/__init__.py b/sunnypilot/selfdrive/car/ford/__init__.py new file mode 100644 index 0000000000..e69de29bb2 diff --git a/sunnypilot/selfdrive/car/ford/custom_stock_longitudinal_controller/__init__.py b/sunnypilot/selfdrive/car/ford/custom_stock_longitudinal_controller/__init__.py new file mode 100644 index 0000000000..e69de29bb2 diff --git a/sunnypilot/selfdrive/car/ford/custom_stock_longitudinal_controller/controller.py b/sunnypilot/selfdrive/car/ford/custom_stock_longitudinal_controller/controller.py new file mode 100644 index 0000000000..6cb6643ee3 --- /dev/null +++ b/sunnypilot/selfdrive/car/ford/custom_stock_longitudinal_controller/controller.py @@ -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 diff --git a/sunnypilot/selfdrive/car/ford/custom_stock_longitudinal_controller/fordcan.py b/sunnypilot/selfdrive/car/ford/custom_stock_longitudinal_controller/fordcan.py new file mode 100644 index 0000000000..9a4815ad2c --- /dev/null +++ b/sunnypilot/selfdrive/car/ford/custom_stock_longitudinal_controller/fordcan.py @@ -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)