Ford: Improve Long Controls

[coffee-cake-isaac]

Description [x] Current CAN-FD Ford Platforms experience a massive amount of brake spamming when approaching red lights/stop signs/stopped cars.

A few of us have deduced that this was due to the longitudinal tuning as well as the signals being off-base from what BlueCruise was doing.

Verification [x] Through the efforts of several community members, we've found that if we don't actuate braking til true INACTIVE_GAS is used (-5), as well as some longitudinal tuning, we can effectively eliminate the brake pumping/stuttering that can be seen in the current iteration of the Ford long control

We have been testing this in our forks for over a month and we have all experienced the same thing - improved long control when coming to a stop.

[adeebshihadeh]

We'll try this on our Bronco. Can you also make the tuning apply to it instead of gating on CAN FD?

[coffee-cake-isaac]

Yeah, I'll get all these comments addressed shortly.

[coffee-cake-isaac]

@adeebshihadeh So, I think this is okay now? Should cover all Ford vehicles. Process Replay is failing, but I believe that might be intentional since the long tuning changes affect how the existing route would be handled?

[adeebshihadeh]

Yes, that's intentional. We'll try it out and merge if it's good.

[sshane]

Can you provide a stock route before and after these changes, maybe stopping for a lead car at a traffic light/slowing for some leads in general?

[coffee-cake-isaac]

@sshane I can grab them tomorrow.

One of the biggest reasons we did this to begin with is because the model more often than not would go slightly above 0 and slightly below 0 and so the brake would be pumping constantly.

We can very audibly hear it in the F150s (I can't speak for Mach Es or any other Q4 vehicles... or Q3 vehicles for that matter). But I'll grab the routes tomorrow.

[coffee-cake-isaac]

Actually, I have the routes now:

112e4d6e0cad05e1|2023-12-14--17-00-04 - This is with the actuation changes. b9a91fc95c5ba5c7|2023-01-26--15-06-14 - This is a stock BlueCruise route.

Both should have all logs uploaded but let me know if something isn't right.

[sshane]

Yassine tried this on our Bronco, it undershot around the time we unset the braking bit. You might want to implement some hysteresis around 0 m/s^2 instead. What does the stock system do?

[sshane]

Checked your master openpilot route (112e4d6e0cad05e1|2023-12-14--17-00-04), the problem your PR is supposed to address is likely caused by something else. Here we don't touch any of the brake bits:

Have you tried the recent Ford long PR which sets a few extra signals that the stock system uses? That may help: https://github.com/commaai/openpilot/pull/28948

[coffee-cake-isaac]

After talking with Shane, I'm going to do some testing today to get some more routes.

There's a chance that just the tune is necessary but we'll need another route or two to be certain.

[sheaduncan]

route	fork	Operating Condition	notes
09136c309ba9461d|2023-12-15--11-54-12--0	tune + actuate_brake = -5	e2e
09136c309ba9461d|2023-12-15--11-43-23--0	tune + actuate_brake = -5	OP Long
09136c309ba9461d|2023-12-15--10-13-25--0	tune only	e2e
09136c309ba9461d|2023-12-15--09-59-22--0	tune only	OP Long	collision warning
09136c309ba9461d|2023-12-15--09-52-20--0	master as-is	e2e
09136c309ba9461d|2023-12-15--09-48-10--0	master as-is	OP Long	collision warning
09136c309ba9461d|2023-12-15--09-34-42--0	master as-is	Ford Stock Lat & ACC

[coffee-cake-isaac]

Some of the Ford folks and I have mashed our heads together and come up with a different solution that is yielding nice results.

Genie was the primary person who helped build the logic and the code, however Ajz, Shea and myself have all been working towards a solution. Here's what we've found.

AccVeh_V_Trg

• Observations:

  1. Seeing that the value for AccVeh_V_Trg is hardcoded, we looked in the CAN messages using PlotJuggler and noticed that while:
  2. ACC is enabled the value of AccVeh_V_Trg is the close to the value of one of the /longitudinalPlan/speeds, so we just picked speed[0] since they are all pretty much the same.
  3. ACC is disabled - sets the value to target CC - this should not be very important, because OP is not in control when ACC is disabled.

• Implementation:

  1. /carControl/actuators/speed is always 0 - since it's available and 0, we populate it with the speeds[0] value in carcontroller.py read and convert mph to kph and send it to create_acc_msg

AccPrpl_A_Pred

• Observations:

  1. During signal analysis in PlotJuggler I noticed that AccPrpl_A_Pred follows AccPrpl_A_Rq, which is what in already upstream BUT there are some differences:
• 2. AccPrpl_A_Rq is -5, AccPrpl_A_Pred will not be set to -5, but follows its path. there is no AccPrpl_A_Pred > 0 when AccPrpl_A_Rq is -5

• Implementation:

1. Keeping the observations in mind and since we don't have two signals, OP implements the "single pedal" functionality for all the cars, we decided to use accel as base for AccPrpl_A_Pred since it doesn't go to -5 once it reaches -0.5 as gas does, but also limit AccPrpl_A_Pred to 0 when AccPrpl_A_Rq is 0.

• Limitation:

1. Maybe we should use actutors.accel instead of accel, accel is clipped to be between -2 and 2 before sent to fordcan.

AccBrkPrchg_B_Rq and AccBrkDecel_B_Rq

• Observations:

  1. Seeing that sometimes the actuators flip/flop between activate/inactive in consecutive frames, I decided to add a hysteresis, for now. It’s really small, around -0.05 around 0.

• Implementation:

  1. Move the actuation decision to the car controller since I can use self. to save the previous accel and use the hysteresis.

• Limitation:

  1. Based on observations the AccBrkPrchg_B_Rq most of the time enables around -0.25 to -0.3 and AccBrkDecel_B_Rq a bit later. We do see Precharge without braking but we almost never or very rarely see braking without precharge. We're a bit concerned that we currently activate the AccBrkPrchg_B_Rq and AccBrkDecel_B_Rq while we are at cruising speed and the accel drops below 0 - this adds, maybe unnecessary, wear and tear on the braking system. Maybe we need to build some smart planning to slow down the detection mechanism. We're not mechanics, hence I'm not sure this is a valid concern or not.

[blue-genie]

I must be missing something but https://github.com/commaai/panda/blob/master/board/safety/safety_ford.h#L287 might be too restrictive for a _Pred signal. I would be OK for _Rq signal for clipping to acceptable limits, but for _Pred you want to be as accurate as possible. You can also see in the last screenshot form PJ that _Rq drops from -0.5 to -5, but _Pred keeps going in the negative realm, even past -2 m/s^2 - which is the lower clipping value for accel - but I think that's OK.

[github-actions[bot]]

This PR has had no activity for 30 days. It will be automatically closed in 7 days if there is no activity.

[sshane]

Was there anything simple you found that improved things? This PR includes changes unrelated to the title

[blue-genie]

@sshane I don't think we found anything "simple", @coffee-cake-isaac still needs to update the the PR, but because the of the nosy accel, we built a better hysteresis and then applied a interpolation with convergence - to create a better brake experience, this improved my long control, and a few other F-150 users as well.

For ICE I described in the Ford discord as well but the idea is to:

• enable pre-charger at -0.25 and brake actuator at -0.3
• disable pre-charger and brake actuator at -0.05

then we do some interpolation on the brake signal, to start bringing it back to 0, since we enable the brake at -0.3, we use some interpolation like [-3.5, -1.5, -0.3 ] to [-3.5, -1.5, 0], this resets the brake signal from -0.3 to 0, converging at -1.5, then from that point the brake signal is sent as it comes in

@coffee-cake-isaac has different values for EV

benefits:

1. we eliminate the brake/pre-charger flickering - since accel is noisy
2. we don't get a harsh brake from the beginning, instead of sending directly -0.3, we send 0 brake and we have a smoother brake.

it's not perfect but it's tolerable.

This is a plot with the logic in action, you can see how noisy the actuators/accel is.

This is my entire drive, you can see that there are no unneeded pre-charger or brake actuators enabling:

our changes will be added soon, but it's not extremely something simple.

[coffee-cake-isaac]

I think @blue-genie hit the nail on the head.

All of these changes are required for a usable long experience. Prior to this, we were unable to turn on OP Long due to issues of brake spamming and just general jerkiness.

We've learned a lot since we've opened the PR and this now needs to actually be slightly expanded.

For EVs (at least the F150 Lightning, but I'd imagine the Mach E as well), the vehicles basically go into freeroll mode. When enabling long controls (both on stock and on OP long), one pedal mode is disabled. And because of that, and the lack of engine, there is no engine braking at all. The entirety of the action is controlled via the ACC message.

As a result, we have quite a few differences between the EVs and the ICE models. I was waiting til we got this either closed/merged to further the breakout even more as you guys mentioned you like having more atomic-like PRs so you can revert if necessary without undoing a lot of other changes.

The changes for EVs that I've noticed is the removal for precharging. On EVs at least, I noticed no difference when precharging and when not.

Additionally, I've written a simpler hysteresis function to determine if we need to actuate the brakes. If you think it belongs in this PR, then I think that's fine. But I think for the masses, this is currently the path of least resistance.

@sshane

[github-actions[bot]]

This PR has had no activity for 30 days. It will be automatically closed in 7 days if there is no activity.

[MikeDoyal]

I've had some time to drive my Edge (Q3 harness, ICE, non CAN-FD) on the same changes @blue-genie and @coffee-cake-isaac describe above. It has allowed me to enable longitudinal control full time. Prior to the changes, I was experiencing the same brake spamming the other CAN-FD drivers were seeing. It prevented me from running any long control or e2e due to a very uncomfortable experience driving in the vehicle as well as the people behind me. Below you can see similar results, it prevented unnecessary pre-charge or brake actuation

[github-actions[bot]]

It looks like you didn't use one of the Pull Request templates. Please check the contributing docs. Also make sure that you didn't modify any of the checkboxes or headings within the template.

[coffee-cake-isaac]

@adeebshihadeh @sshane We've updated our logic with a more robust hysteresis. This is working well on our cars. What're your thoughts about this? I think Ford is a bit unique with their precharge, if I'm not mistaken.

We're trying to approach this from both a software and hardware view point. We don't want to be filling the brake lines constantly if it's unnecessary (trying to prevent additional wear/tear).

[crashnburn0]

I would like to confirm that all code from this PR has been added on my fork and is fully operational on a 2023 Ford Maverick Hybrid XLT Lux. The comma 3x is running SP dev c3 which was synced 3 days ago. Before the calibration changes, while following vehicles down the famous I-10 bridge at Lake Charles the hybrid would surge the brakes and then restart the engine and accelerate, followed by slamming on the brakes, and then repeat. It never seemed to follow down hills well at all. AFTER the code changes I drove for 30 minutes and the experimental long control worked flawless driving down 4 different tall bridges. I would say that it is now 98% drive quality of the factory ford acc experience, which is absolutely amazing! I have a much longer trip tomorrow and I will be monitoring and reporting any issues or experiences I have with the new code.

For anyone working on trying this, please make sure you add line 3 to carcontroller.py

from openpilot.common.conversions import Conversions as CV

I didn't see anything saying that this line should be added (green highlights with +) but Isaac confirmed that it has to be there or it won’t work.

[Camry2731]

I wanted to chime in here as well to say that I've been testing these changes as well and it works very well. It allows me to actually use experimental mode without the issues I was having before

[ncoop23]

I would like to confirm that all code from this PR has been added on my fork and is fully operational on a 2023 Ford Maverick Hybrid XLT Lux. The comma 3x is running SP dev c3 which was synced 3 days ago. Before the calibration changes, while following vehicles down the famous I-10 bridge at Lake Charles the hybrid would surge the brakes and then restart the engine and accelerate, followed by slamming on the brakes, and then repeat. It never seemed to follow down hills well at all. AFTER the code changes I drove for 30 minutes and the experimental long control worked flawless driving down 4 different tall bridges. I would say that it is now 98% drive quality of the factory ford acc experience, which is absolutely amazing! I have a much longer trip tomorrow and I will be monitoring and reporting any issues or experiences I have with the new code.

For anyone working on trying this, please make sure you add line 3 to carcontroller.py

from openpilot.common.conversions import Conversions as CV

I didn't see anything saying that this line should be added (green highlights with +) but Isaac confirmed that it has to be there or it won’t work.

I have a Maverick Hybrid Lariat Lux also and am game to test. Exp has been unusable for me.

[coffee-cake-isaac]

Just trying to bump this again. What can we do to help move this along? Do you need more data?

[ri06667]

Please fix, ruining my life

[incognitojam]

I think it would be better to close this PR and try to make small changes which you can demonstrate, with data, improve long control. There are also conflicts on this branch after other changes were made...

For example, sending a real value for AccVeh_V_Trg target speed. You might be able to demonstrate with plots that the car follows the desired acceleration/speed more closely when sending that signal.

Secondly, the longitudinal tuning in ford/interface.py that you have tested for CAN FD cars can be its own PR. You should make sure to only set it for the tested cars.

[coffee-cake-isaac]

Moving this to draft. Initial impressions with 20hz model were very promising and may not need this. Will revisit with new PJ info soon