ford: support Edge MK2 yaw counter step of 5

opendbc_repo/opendbc/car/ford/interface.py

@@ -56,6 +56,10 @@ class CarInterface(CarInterfaceBase):
       ret.safetyConfigs[-1].safetyParam |= FordSafetyFlags.LONG_CONTROL.value
       ret.openpilotLongitudinalControl = True
 
+    if ret.flags & FordFlags.ALT_STEER_ANGLE:
+      # Edge MK2 has RCM Yaw_Data_FD1 counter increments of 5 instead of 1
+      ret.safetyConfigs[-1].safetyParam |= FordSafetyFlags.YAW_COUNTER_STEP_5.value
+
     if ret.flags & FordFlags.CANFD:
       ret.safetyConfigs[-1].safetyParam |= FordSafetyFlags.CANFD.value
 

opendbc_repo/opendbc/car/ford/values.py

@@ -42,6 +42,7 @@ class CarControllerParams:
 class FordSafetyFlags(IntFlag):
   LONG_CONTROL = 1
   CANFD = 2
+  YAW_COUNTER_STEP_5 = 4
 
 
 class FordFlags(IntFlag):

opendbc_repo/opendbc/safety/declarations.h

@@ -184,6 +184,7 @@ typedef struct {
   const uint8_t max_counter;         // maximum value of the counter. 0 means that the counter check is skipped
   const bool ignore_quality_flag;    // true if quality flag check is skipped
   const bool ignore_frequency_check; // true if minimum frequency enforcement is skipped
+  const uint8_t counter_step;        // counter increment per message. 0 means 1
 } CanMsgCheck;
 
 typedef struct {

opendbc_repo/opendbc/safety/modes/ford.h

@@ -288,6 +288,20 @@ static safety_config ford_init(uint16_t param) {
     {.msg = {{FORD_Steering_Data_FD1, 0, 8, 10U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}},
   };
 
+  static RxCheck ford_rx_checks_yaw_counter_step_5[] = {
+    {.msg = {{FORD_BrakeSysFeatures, 0, 8, 50U, .max_counter = 15U}, { 0 }, { 0 }}},
+    // FORD_EngVehicleSpThrottle2 has a counter that either randomly skips or by 2, likely ECU bug
+    // Some hybrid models also experience a bug where this checksum mismatches for one or two frames under heavy acceleration with ACC
+    // It has been confirmed that the Bronco Sport's camera only disallows ACC for bad quality flags, not counters or checksums, so we match that
+    {.msg = {{FORD_EngVehicleSpThrottle2, 0, 8, 50U, .ignore_checksum = true, .ignore_counter = true}, { 0 }, { 0 }}},
+    {.msg = {{FORD_Yaw_Data_FD1, 0, 8, 100U, .max_counter = 255U, .counter_step = 5U}, { 0 }, { 0 }}},
+    // These messages have no counter or checksum
+    {.msg = {{FORD_EngBrakeData, 0, 8, 10U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}},
+    {.msg = {{FORD_EngVehicleSpThrottle, 0, 8, 100U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}},
+    {.msg = {{FORD_DesiredTorqBrk, 0, 8, 50U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}},
+    {.msg = {{FORD_Steering_Data_FD1, 0, 8, 10U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}},
+  };
+
   #define FORD_COMMON_TX_MSGS \
     {FORD_Steering_Data_FD1, 0, 8, .check_relay = false}, \
     {FORD_Steering_Data_FD1, 2, 8, .check_relay = false}, \
@@ -315,17 +329,29 @@ static safety_config ford_init(uint16_t param) {
   };
 
   const uint16_t FORD_PARAM_CANFD = 2;
+  const uint16_t FORD_PARAM_YAW_COUNTER_STEP_5 = 4;
   const bool ford_canfd = GET_FLAG(param, FORD_PARAM_CANFD);
+  const bool ford_yaw_counter_step_5 = GET_FLAG(param, FORD_PARAM_YAW_COUNTER_STEP_5);
 
   safety_config ret;
   if (ford_canfd) {
-    ret = BUILD_SAFETY_CFG(ford_rx_checks, FORD_CANFD_STOCK_TX_MSGS);
+    if (ford_yaw_counter_step_5) {
+      ret = BUILD_SAFETY_CFG(ford_rx_checks_yaw_counter_step_5, FORD_CANFD_STOCK_TX_MSGS);
+    } else {
+      ret = BUILD_SAFETY_CFG(ford_rx_checks, FORD_CANFD_STOCK_TX_MSGS);
+    }
 #ifdef ALLOW_DEBUG
     const uint16_t FORD_PARAM_LONGITUDINAL = 1;
     if (GET_FLAG(param, FORD_PARAM_LONGITUDINAL)) {
-      ret = BUILD_SAFETY_CFG(ford_rx_checks, FORD_CANFD_LONG_TX_MSGS);
+      if (ford_yaw_counter_step_5) {
+        ret = BUILD_SAFETY_CFG(ford_rx_checks_yaw_counter_step_5, FORD_CANFD_LONG_TX_MSGS);
+      } else {
+        ret = BUILD_SAFETY_CFG(ford_rx_checks, FORD_CANFD_LONG_TX_MSGS);
+      }
     }
 #endif
+  } else if (ford_yaw_counter_step_5) {
+    ret = BUILD_SAFETY_CFG(ford_rx_checks_yaw_counter_step_5, FORD_LONG_TX_MSGS);
   } else {
     ret = BUILD_SAFETY_CFG(ford_rx_checks, FORD_LONG_TX_MSGS);
   }

opendbc_repo/opendbc/safety/safety.h

@@ -152,7 +152,9 @@ static void update_addr_timestamp(RxCheck addr_list[], int index) {
 
 static void update_counter(RxCheck addr_list[], int index, uint8_t counter) {
   if (index != -1) {
-    uint8_t expected_counter = (addr_list[index].status.last_counter + 1U) % (addr_list[index].msg[addr_list[index].status.index].max_counter + 1U);
+    const CanMsgCheck *msg_check = &addr_list[index].msg[addr_list[index].status.index];
+    const uint8_t counter_step = msg_check->counter_step != 0U ? msg_check->counter_step : 1U;
+    uint8_t expected_counter = (addr_list[index].status.last_counter + counter_step) % (msg_check->max_counter + 1U);
     addr_list[index].status.wrong_counters += (expected_counter == counter) ? -1 : 1;
     addr_list[index].status.wrong_counters = SAFETY_CLAMP(addr_list[index].status.wrong_counters, 0, MAX_WRONG_COUNTERS);
     addr_list[index].status.last_counter = counter;