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.

[incognitojam]

Just created a separate PR for the "try Panda bus for FPv2 queries" stuff ^^

[incognitojam]

Added some signals for yawRate, brake (although it's brake torque? not pedal pct) and I think I've found a better signal for steeringPressed, too

[EasternPA]

Following with great excitement. Any reason why the suggested documentation update in the first post indicates that Automatics with Stop and Go will disengage at 10 MPH? What am I missing?

[incognitojam]

Following with great excitement. Any reason why the suggested documentation update in the first post indicates that Automatics with Stop and Go will disengage at 10 MPH? What am I missing?

I think I might've made a mistake with that actually, was just from memory, it doesn't make sense actually!

[EasternPA]

Following with great excitement. Any reason why the suggested documentation update in the first post indicates that Automatics with Stop and Go will disengage at 10 MPH? What am I missing?

I think I might've made a mistake with that actually, was just from memory, it doesn't make sense actually!

Thank you for making that update so quickly. I have about 8 months before I must pick up my next OP ride, and I'm not sure what to get next. You're doing great work here, keep it up!

[scripty29]

Hey @incognitojam i have been following you for a little while. I have a 2019 F150 with ACC stop and go and LKA. Any chance your code will work with my truck and or do you need additional testers? Also willing to learn how to tune etc.. but I don’t have a lot of coding experience. Cheers on the work you have done so far!

[incognitojam]

I have a 2019 F150 with ACC stop and go and LKA. Any chance your code will work with my truck

Hi @scripty29 , I'm not sure it will work on your F150... Do you know if Ford offered "traffic jam assist" or lane centering option from the factory? You might not have a compatible power steering module

You'd be able to check if you get an OBD adapter and the Forscan software (or another scan tool if you have access to that) Look at the PSCM configuration and see what options there are. If you want you could share the As-Built Data and the firmware if you can see that too

[scripty29]

I have Forscan and can check this afternoon! I will report back with my findings 🤞

[incognitojam]

Just rebased this onto the latest master, using the new FW query stuff so that there isn't a hack for choosing the panda bus anymore

[adeebshihadeh]

I'd like to get this merged as it seems very similar to the current F150 platform. Here's some quick comments after going through the PR - I'll do a thorough review sometime this week.

Let's start by whittling this down to a subset of this PR that can be merged right away:

• fordcan changes to build the new messages
• carState
• carController
• new car models + fusion removal
• possibly the FW query
• re-enable dashcamOnly

Feel free to do that in this PR or break out into a new one.

[adeebshihadeh]

CAN fingerprint detection can be unreliable, switch to checking ECU fw versions if possible

[incognitojam]

Is there an example of how to do this somewhere else?

[incognitojam]

actually, I found how to do it by reading car_fw, but I guess you can only use this after the FPv2 stuff is added

[incognitojam]

From memory, the transmission ECU never appeared in the car_fw... Only saw the eps, esp, radar, camera and engine responses. I tried looking at 360a's routes on useradmin but for some reason I can't see the car params?

EDIT: 0x7E1 should be the TCM. I was also thinking we could add the blindspot modules as well, which are 0x7C4 (SODL) and 0x7C6 (SODR), but maybe it's not possible to speak to those from this bus...

[adeebshihadeh]

doesn't work on your car?

[incognitojam]

I'll have to test this again... I can't remember what the problem was

[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.