ford - reset steering angle state when driver stops overriding steering

board/safety.h

@@ -72,6 +72,9 @@ bool gas_pressed = false;
 bool gas_pressed_prev = false;
 bool brake_pressed = false;
 bool brake_pressed_prev = false;
+bool steering_pressed = false;
+bool steering_pressed_prev = false;
+uint8_t steering_pressed_cnt = 0;
 bool regen_braking = false;
 bool regen_braking_prev = false;
 bool cruise_engaged_prev = false;
@@ -394,6 +397,8 @@ int set_safety_hooks(uint16_t mode, uint16_t param) {
   gas_pressed_prev = false;
   brake_pressed = false;
   brake_pressed_prev = false;
+  steering_pressed = false;
+  steering_pressed_prev = false;
   regen_braking = false;
   regen_braking_prev = false;
   cruise_engaged_prev = false;
@@ -408,6 +413,7 @@ int set_safety_hooks(uint16_t mode, uint16_t param) {
   ts_steer_req_mismatch_last = 0;
   valid_steer_req_count = 0;
   invalid_steer_req_count = 0;
+  steering_pressed_cnt = 0;
 
   // reset samples
   reset_sample(&vehicle_speed);
@@ -478,6 +484,19 @@ void update_sample(struct sample_t *sample, int sample_new) {
   }
 }
 
+// Applies filtering on steering pressed for noisy driver torque signals.
+void update_steering_pressed(bool steer_pressed, uint8_t steering_pressed_min_count) {
+  if (steer_pressed) {
+    steering_pressed_cnt += 1;
+  } else {
+    steering_pressed_cnt = 0;
+  }
+  if (steering_pressed_cnt > steering_pressed_min_count * 2) {
+    steering_pressed_cnt = steering_pressed_min_count * 2;
+  }
+  steering_pressed = steering_pressed_cnt > steering_pressed_min_count;
+}
+
 static bool max_limit_check(int val, const int MAX_VAL, const int MIN_VAL) {
   return (val > MAX_VAL) || (val < MIN_VAL);
 }
@@ -694,6 +713,14 @@ bool steer_angle_cmd_checks(int desired_angle, bool steer_control_enabled, const
     int delta_angle_up = (interpolate(limits.angle_rate_up_lookup, (vehicle_speed.min / VEHICLE_SPEED_FACTOR) - 1.) * limits.angle_deg_to_can) + 1.;
     int delta_angle_down = (interpolate(limits.angle_rate_down_lookup, (vehicle_speed.min / VEHICLE_SPEED_FACTOR) - 1.) * limits.angle_deg_to_can) + 1.;
 
+    // When steering is no longer pressed reset the last desired angle to the most recent measured
+    // angle. This allows for steering commands to continue from the position of the wheel when the
+    // driver steering speed is greater than the angle rate limit.
+    if (!steering_pressed && steering_pressed_prev) {
+      desired_angle_last = angle_meas.values[0];
+    }
+    steering_pressed_prev = steering_pressed;
+
     // allow down limits at zero since small floats will be rounded to 0
     int highest_desired_angle = desired_angle_last + ((desired_angle_last > 0) ? delta_angle_up : delta_angle_down);
     int lowest_desired_angle = desired_angle_last - ((desired_angle_last >= 0) ? delta_angle_down : delta_angle_up);

board/safety/safety_ford.h

@@ -9,6 +9,7 @@
 #define FORD_BrakeSysFeatures      0x415   // RX from ABS, for vehicle speed
 #define FORD_EngVehicleSpThrottle2 0x202   // RX from PCM, for second vehicle speed
 #define FORD_Yaw_Data_FD1          0x91    // RX from RCM, for yaw rate
+#define FORD_EPAS_INFO             0x82    // RX from EPAS, for steering torque
 #define FORD_Steering_Data_FD1     0x083   // TX by OP, various driver switches and LKAS/CC buttons
 #define FORD_ACCDATA               0x186   // TX by OP, ACC controls
 #define FORD_ACCDATA_3             0x18A   // TX by OP, ACC/TJA user interface
@@ -98,6 +99,8 @@ static bool ford_longitudinal = false;
 #define FORD_CANFD_INACTIVE_CURVATURE_RATE 1024U
 
 #define FORD_MAX_SPEED_DELTA 2.0  // m/s
+#define FORD_DRIVER_STEERING_TORQUE_MIN 112 // -1 nm
+#define FORD_DRIVER_STEERING_TORQUE_MAX 144 //  1 nm
 
 static bool ford_lkas_msg_check(int addr) {
   return (addr == FORD_ACCDATA_3)
@@ -182,6 +185,13 @@ static void ford_rx_hook(const CANPacket_t *to_push) {
       pcm_cruise_check(cruise_engaged);
     }
 
+    // Update driver steering state
+    if (addr == FORD_EPAS_INFO) {
+      uint8_t torque_driver = GET_BYTE(to_push, 1);
+      bool steer_pressed = torque_driver < FORD_DRIVER_STEERING_TORQUE_MIN || torque_driver > FORD_DRIVER_STEERING_TORQUE_MAX;
+      update_steering_pressed(steer_pressed, 5);
+    }
+
     // If steering controls messages are received on the destination bus, it's an indication
     // that the relay might be malfunctioning.
     bool stock_ecu_detected = ford_lkas_msg_check(addr);

board/safety_declarations.h

@@ -173,6 +173,7 @@ bool safety_tx_hook(CANPacket_t *to_send);
 uint32_t get_ts_elapsed(uint32_t ts, uint32_t ts_last);
 int to_signed(int d, int bits);
 void update_sample(struct sample_t *sample, int sample_new);
+void update_steering_pressed(bool steer_pressed, uint8_t steering_pressed_min_count);
 bool get_longitudinal_allowed(void);
 int ROUND(float val);
 void gen_crc_lookup_table_8(uint8_t poly, uint8_t crc_lut[]);