wip

selfdrive/car/ford/carstate.py

@@ -1,10 +1,10 @@
 from cereal import car
-from openpilot.common.conversions import Conversions as CV
 from opendbc.can.can_define import CANDefine
 from opendbc.can.parser import CANParser
-from openpilot.selfdrive.car.interfaces import CarStateBase
+from openpilot.common.conversions import Conversions as CV
 from openpilot.selfdrive.car.ford.fordcan import CanBus
-from openpilot.selfdrive.car.ford.values import CANFD_CAR, CarControllerParams, DBC
+from openpilot.selfdrive.car.ford.values import DBC, CarControllerParams, FordFlags
+from openpilot.selfdrive.car.interfaces import CarStateBase
 
 GearShifter = car.CarState.GearShifter
 TransmissionType = car.CarParams.TransmissionType
@@ -18,17 +18,13 @@ class CarState(CarStateBase):
       self.shifter_values = can_define.dv["Gear_Shift_by_Wire_FD1"]["TrnRng_D_RqGsm"]
 
     self.vehicle_sensors_valid = False
-    self.unsupported_platform = False
+
+    self.prev_distance_button = 0
+    self.distance_button = 0
 
   def update(self, cp, cp_cam, frogpilot_variables):
     ret = car.CarState.new_message()
 
-    # Ford Q3 hybrid variants experience a bug where a message from the PCM sends invalid checksums,
-    # this must be root-caused before enabling support. Ford Q4 hybrids do not have this problem.
-    # TrnAin_Tq_Actl and its quality flag are only set on ICE platform variants
-    self.unsupported_platform = (cp.vl["VehicleOperatingModes"]["TrnAinTq_D_Qf"] == 0 and
-                                 self.CP.carFingerprint not in CANFD_CAR)
-
     # Occasionally on startup, the ABS module recalibrates the steering pinion offset, so we need to block engagement
     # The vehicle usually recovers out of this state within a minute of normal driving
     self.vehicle_sensors_valid = cp.vl["SteeringPinion_Data"]["StePinCompAnEst_D_Qf"] == 3
@@ -56,7 +52,7 @@ class CarState(CarStateBase):
     ret.steerFaultPermanent = cp.vl["EPAS_INFO"]["EPAS_Failure"] in (2, 3)
     ret.espDisabled = cp.vl["Cluster_Info1_FD1"]["DrvSlipCtlMde_D_Rq"] != 0  # 0 is default mode
 
-    if self.CP.carFingerprint in CANFD_CAR:
+    if self.CP.flags & FordFlags.CANFD:
       # this signal is always 0 on non-CAN FD cars
       ret.steerFaultTemporary |= cp.vl["Lane_Assist_Data3_FD1"]["LatCtlSte_D_Stat"] not in (1, 2, 3)
 
@@ -90,6 +86,8 @@ class CarState(CarStateBase):
     ret.rightBlinker = cp.vl["Steering_Data_FD1"]["TurnLghtSwtch_D_Stat"] == 2
     # TODO: block this going to the camera otherwise it will enable stock TJA
     ret.genericToggle = bool(cp.vl["Steering_Data_FD1"]["TjaButtnOnOffPress"])
+    self.prev_distance_button = self.distance_button
+    self.distance_button = cp.vl["Steering_Data_FD1"]["AccButtnGapTogglePress"]
 
     # lock info
     ret.doorOpen = any([cp.vl["BodyInfo_3_FD1"]["DrStatDrv_B_Actl"], cp.vl["BodyInfo_3_FD1"]["DrStatPsngr_B_Actl"],
@@ -98,7 +96,7 @@ class CarState(CarStateBase):
 
     # blindspot sensors
     if self.CP.enableBsm:
-      cp_bsm = cp_cam if self.CP.carFingerprint in CANFD_CAR else cp
+      cp_bsm = cp_cam if self.CP.flags & FordFlags.CANFD else cp
       ret.leftBlindspot = cp_bsm.vl["Side_Detect_L_Stat"]["SodDetctLeft_D_Stat"] != 0
       ret.rightBlindspot = cp_bsm.vl["Side_Detect_R_Stat"]["SodDetctRight_D_Stat"] != 0
 
@@ -108,6 +106,9 @@ class CarState(CarStateBase):
     self.acc_tja_status_stock_values = cp_cam.vl["ACCDATA_3"]
     self.lkas_status_stock_values = cp_cam.vl["IPMA_Data"]
 
+    self.lkas_previously_enabled = self.lkas_enabled
+    self.lkas_enabled = bool(cp.vl["Steering_Data_FD1"]["TjaButtnOnOffPress"])
+
     return ret
 
   @staticmethod
@@ -129,7 +130,7 @@ class CarState(CarStateBase):
       ("RCMStatusMessage2_FD1", 10),
     ]
 
-    if CP.carFingerprint in CANFD_CAR:
+    if CP.flags & FordFlags.CANFD:
       messages += [
         ("Lane_Assist_Data3_FD1", 33),
       ]
@@ -144,7 +145,7 @@ class CarState(CarStateBase):
         ("BCM_Lamp_Stat_FD1", 1),
       ]
 
-    if CP.enableBsm and CP.carFingerprint not in CANFD_CAR:
+    if CP.enableBsm and not (CP.flags & FordFlags.CANFD):
       messages += [
         ("Side_Detect_L_Stat", 5),
         ("Side_Detect_R_Stat", 5),
@@ -162,7 +163,7 @@ class CarState(CarStateBase):
       ("IPMA_Data", 1),
     ]
 
-    if CP.enableBsm and CP.carFingerprint in CANFD_CAR:
+    if CP.enableBsm and CP.flags & FordFlags.CANFD:
       messages += [
         ("Side_Detect_L_Stat", 5),
         ("Side_Detect_R_Stat", 5),