Ford: Initial support for LCA vehicles

[incognitojam]

This PR adds support for the Focus Mk4 (EU) and Escape Mk4 using the Lane Centering function.

I am using a manual-transmission Ford Focus 2018 with stock CC and LKA (retrofitted ACC - radar seems to be factory fitted in the UK, perhaps for pre-collision assist, but ACC can be enabled using Forscan).

Thanks to Rikka, Desta, 360a and everyone else in the #ford channel for providing code, ideas and help with this. Thanks to @jyoung8607 for review and help getting this to work!

Checklist

• added to docs/CARS.md
• test route added to routes.py
• route with openpilot:
• route with stock system:
• car harness used (if comma doesn't sell it, put N/A): I think it is the same camera connector as the Mazda harness, but a different pinout (Molex WM10326-ND)

Related PRs:

• panda: Ford safety for LCA vehicles panda#812
• opendbc:
  • Ford: update common dbc opendbc#476
  • Ford: add OP-style transmission values opendbc#590

Documentation

Steering commands

We can command the Power Steering Control Module (PSCM) to steer the car using the LateralMotionControl message, from Ford's Lane Centering Assist (LCA) / Traffic Jam Assist (TJA) feature. This gives us curvature-based control.

All cars which have LCA/TJA as an option should have a compatible PSCM fitted, and vehicles owners who didn't buy the optio from Ford should be able to enable it by changing the LCA/TJA enable bit in the PSCM As-Built Data.

From short testing so far, the Lane_Assist_Data1 message must be sent alongside the LCA message for the PSCM to behave. This command is used by the stock system for the "Lane Keep Aid/Assist" (LKA) feature, which corrects the steering wheel when the car drifts out of the lane, but does not keep the car centered. This was the old method of commanding steering, but was subject to the 10s lockout on almost all vehicles.

Initially it was believed that LateralMotionControl2 was used for LCA, as it has some interesting signals and checksums and counters, but then we found that we could use LateralMotionControl. As of yet, we don't know what LateralMotionControl2 is used for. It is not sent on the vehicles we have tested so far. Perhaps it is used for Blue Cruise.

We can steer down to 0mph (it is not linked to the cruise control state), although the cruise control min engage speed and disengage speed varies depending on transmission and stop-and-go.

Method	Min Steer Speed	Lockout
LKA	35mph	Yes, 200-300ms every 10 seconds
LCA	0mph	No

Transmission	Stop and Go	Min Engage Speed	Disengage Speed
Manual	-	20mph	20mph
Automatic	No	20mph	12mph
Automatic	Yes	0mph	-

Curvature Control

LCA is a "curvature" based control method. The Image Processing Module A (IPMA) in the vehicle writes a message describing the vehicle's current position in a lane and the lane's properties (e.g. curvature) to the CAN bus, and then the PSCM integrates this information with other data (e.g. vehicle yaw) to actuate the steering wheel to the correct position.

To achieve Lateral Control with openpilot, we can take some values from the lateral plan to estimate the lane curvature and path offset. Doing this gives adequate results, even without writing path angle and curvature rate. However, I think lane angle and curvature rate might be able to get us even better control out of the PSCM.

The signals for ramp speed and precision may be worth experimenting with when considering tuning/rate limits.

Longitudinal Control

Although not approached in this PR, I believe we will be able to implement longitudinal control quite easily in the future. As the Cruise Control Module (C-CM, the radar) is on a private bus with the IPMA, it is the IPMA which is responsible for calculating and sending ACC commands. We can also intercept these with the same harness. We may even be able to provide ACC for vehicles without a radar, with a few coding changes.

[incognitojam]

I don't have stock LCA in my Ford since it's a manual-transmission and I haven't found a workaround to make it work using the As-Built Data yet (the LCA and TJA features are set with the same bit flag, which makes the IPMA fault when I enable it).

My IPMA does output the LateralMotionControl messages, but does not set the mode signal, so the PSCM does not act on it. It only sends LKA actions. I could record a route so that you could still compare this data to OP but I would be steering 🤦‍♂️.

Alternatively, another community member has an Escape with LCA (who provided me with a route so that I could see the CAN data). Perhaps we could port the Escape too as both a stock LCA and OP route could be provided and compared.

[incognitojam]

I've just rebased this branch on the latest master commit. I will continue to look at tuning when I next have free time, just it is hard to record a "drive" because the tuning is so poor at the moment I cannot let it steer for long periods, I don't know if there would be much value in sharing more drives yet. Guidances for next steps would be appreciated!

[adeebshihadeh]

Guidances for next steps would be appreciated!

What kind of guidance are you looking for exactly? Tuning, getting this merged?

[incognitojam]

I should have been more specific 😅 I'm just racking my brains trying to figure out why this isn't steering as expected. Got some help in the #tuning channel though, and I'm going to experiment to see if the car is actually torque based and not angle based... Will be testing on another branch later!

I thought it would have been angle based because the LKA method (and I've been told the park assist method too) are angle based. It would be odd that the leaked DBC would be wrong about this... But the plotjuggler chart seems to show that the desired steering angle/sendcan angle is almost the derivative of the actual steering angle.

[incognitojam]

Currently I'm seeing the steerRatio increasing constantly over the length of a drive... OP steers quite strongly when only small adjustments to the steering angle are required. Will try adjusting steerRateCost and disabling the params learner for steer ratio.

[incognitojam]

Pinning the steerRatio to 1 seems to give better results with less ping pong. Current theory is that the Ford LCA command accepts the wheels steering angle rather than the steering wheel angle.

[adeebshihadeh]

Nice job @incognitojam! Seems we'll be able to support quite a few existing Fords as well as the new Blue Cruise platform with this port. LateralMotionControl2 is present on the F-150 with Blue Cruise, but we never saw it steer due to the faults.

Here's the next steps:

• break out FW query into a new PR
• finish & merge the matching panda safety
• good quality steering with the curvature message

We can also review and merge the matching panda safety #812 once it's ready, though it'll stay behind the debug panda flag until we get a Ford again and verify it. We'll be ready to invest more time into this once the whole port is working well on a car from the factory.