MADS: Ford support (#13)

board/safety/safety_ford.h

@@ -237,7 +237,7 @@ static int ford_rx_hook(CANPacket_t *to_push) {
       // Signal: Veh_V_ActlEng
       float filtered_pcm_speed = ((GET_BYTE(to_push, 6) << 8) | GET_BYTE(to_push, 7)) * 0.01 / 3.6;
       if (ABS(filtered_pcm_speed - ((float)vehicle_speed.values[0] / VEHICLE_SPEED_FACTOR)) > FORD_MAX_SPEED_DELTA) {
-        controls_allowed = 0;
+        controls_allowed_long = 0;
       }
     }
 
@@ -266,6 +266,17 @@ static int ford_rx_hook(CANPacket_t *to_push) {
       unsigned int cruise_state = GET_BYTE(to_push, 1) & 0x07U;
       bool cruise_engaged = (cruise_state == 4U) || (cruise_state == 5U);
       pcm_cruise_check(cruise_engaged);
+
+      acc_main_on = (cruise_state == 3U) || (cruise_state == 4U) || (cruise_state == 5U);
+      if (acc_main_on && mads_enabled) {
+        controls_allowed = 1;
+      }
+      if (!acc_main_on && acc_main_on_prev) {
+        disengageFromBrakes = false;
+        controls_allowed = 0;
+        controls_allowed_long = 0;
+      }
+      acc_main_on_prev = acc_main_on;
     }
 
     // If steering controls messages are received on the destination bus, it's an indication
@@ -321,8 +332,8 @@ static int ford_tx_hook(CANPacket_t *to_send) {
     // Violation if resume button is pressed while controls not allowed, or
     // if cancel button is pressed when cruise isn't engaged.
     bool violation = false;
-    violation |= (GET_BIT(to_send, 8U) == 1U) && !cruise_engaged_prev;   // Signal: CcAslButtnCnclPress (cancel)
-    violation |= (GET_BIT(to_send, 25U) == 1U) && !controls_allowed;     // Signal: CcAsllButtnResPress (resume)
+    violation |= (GET_BIT(to_send, 8U) == 1U) && !cruise_engaged_prev;                              // Signal: CcAslButtnCnclPress (cancel)
+    violation |= (GET_BIT(to_send, 25U) == 1U) && !(controls_allowed && controls_allowed_long);     // Signal: CcAsllButtnResPress (resume)
 
     if (violation) {
       tx = 0;