Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Ford: Initial support for LCA vehicles #23331

Merged
merged 9 commits into from
Apr 14, 2022

Conversation

incognitojam
Copy link
Contributor

@incognitojam incognitojam commented Dec 28, 2021

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:

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.

Screenshot from 2022-03-13 13-59-10

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
Copy link
Contributor Author

incognitojam commented Dec 28, 2021

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.

@pd0wm pd0wm added the car port label Dec 29, 2021
@incognitojam incognitojam changed the title Ford support for LCA vehicles Ford: Support for LCA vehicles Dec 29, 2021
@incognitojam incognitojam force-pushed the ford-port branch 5 times, most recently from 1d4009d to 95dae6c Compare January 4, 2022 20:12
@incognitojam incognitojam force-pushed the ford-port branch 2 times, most recently from f8c863c to 979692e Compare January 11, 2022 17:06
@incognitojam
Copy link
Contributor Author

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!

@incognitojam incognitojam force-pushed the ford-port branch 2 times, most recently from cd4c00d to 0e06b79 Compare January 11, 2022 19:54
@adeebshihadeh
Copy link
Contributor

Guidances for next steps would be appreciated!

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

@incognitojam
Copy link
Contributor Author

incognitojam commented Jan 12, 2022

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!

image

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 incognitojam force-pushed the ford-port branch 10 times, most recently from f4fd9de to e227d34 Compare January 15, 2022 20:42
@incognitojam
Copy link
Contributor Author

Screenshot from 2022-01-15 21-36-30
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
Copy link
Contributor Author

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 incognitojam force-pushed the ford-port branch 5 times, most recently from edfb218 to 006c5ff Compare April 14, 2022 12:03
@adeebshihadeh adeebshihadeh marked this pull request as ready for review April 14, 2022 20:37
@incognitojam incognitojam force-pushed the ford-port branch 3 times, most recently from 60c58ef to f77cb0e Compare April 14, 2022 20:44
@adeebshihadeh
Copy link
Contributor

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.

@adeebshihadeh adeebshihadeh merged commit acd455e into commaai:master Apr 14, 2022
@incognitojam incognitojam changed the title Ford: Support for LCA vehicles Ford: Initial support for LCA vehicles Apr 14, 2022
incognitojam added a commit to incognitojam/openpilot that referenced this pull request Jul 21, 2022
* Ford: add Focus Mk4

Also removes support for the Ford Fusion.

* Ford: LKAS/LCA steering and UI CAN commands

* Ford: implement CarController w/ steering and lanes ui

* Ford: FPv2 firmware request

* Ford: Add FW for 2018 Ford Focus

* Ford: add Escape Mk4

* bump panda

* cleanup

* add that back

Co-authored-by: Adeeb Shihadeh <[email protected]>
incognitojam added a commit to incognitojam/openpilot that referenced this pull request Jul 24, 2022
* Ford: add Focus Mk4

Also removes support for the Ford Fusion.

* Ford: LKAS/LCA steering and UI CAN commands

* Ford: implement CarController w/ steering and lanes ui

* Ford: FPv2 firmware request

* Ford: Add FW for 2018 Ford Focus

* Ford: add Escape Mk4

* bump panda

* cleanup

* add that back

Co-authored-by: Adeeb Shihadeh <[email protected]>
incognitojam added a commit to incognitojam/openpilot that referenced this pull request Jul 24, 2022
* Ford: add Focus Mk4

Also removes support for the Ford Fusion.

* Ford: LKAS/LCA steering and UI CAN commands

* Ford: implement CarController w/ steering and lanes ui

* Ford: FPv2 firmware request

* Ford: Add FW for 2018 Ford Focus

* Ford: add Escape Mk4

* bump panda

* cleanup

* add that back

Co-authored-by: Adeeb Shihadeh <[email protected]>
incognitojam added a commit to incognitojam/openpilot that referenced this pull request Jul 24, 2022
* Ford: add Focus Mk4

Also removes support for the Ford Fusion.

* Ford: LKAS/LCA steering and UI CAN commands

* Ford: implement CarController w/ steering and lanes ui

* Ford: FPv2 firmware request

* Ford: Add FW for 2018 Ford Focus

* Ford: add Escape Mk4

* bump panda

* cleanup

* add that back

Co-authored-by: Adeeb Shihadeh <[email protected]>
@incognitojam incognitojam deleted the ford-port branch October 12, 2022 22:38
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Projects
None yet
Development

Successfully merging this pull request may close these issues.

7 participants